diff --git a/.clang-format b/.clang-format
index 7e45a49..fcdaf0d 100644
--- a/.clang-format
+++ b/.clang-format
@@ -11,7 +11,7 @@
     - Regex:     '^<(cxx|Halide|HalidBuffer|lzma|node|v8|osqp|oscqp\+\+|snappy|snappy-sinksource).h>$'
       Priority:  3
     # Force some more third-party headers to be appropriately categorized.
-    - Regex:     '^(<|")(absl|ceres|ctre|Eigen|external|FRC_NetworkCommunication|foxglove|flatbuffers|glib-2.0|glog|gflags|gmock|gtest|google|libusb-1.0|lz4|opencv2|openssl|rawrtcc|sanitizer|nlohmann|third_party|wpi)/.*(>|")$'
+    - Regex:     '^(<|")(absl|ceres|ctre|Eigen|external|FRC_NetworkCommunication|foxglove|flatbuffers|glib-2.0|glog|gflags|gmock|gtest|google|libusb-1.0|lz4|opencv2|openssl|rawrtcc|sanitizer|nlohmann|third_party|wpi|tl)/.*(>|")$'
       Priority:  3
     # The default regexes. For some reason, if we don't specify them here, then
     # clang-format will overwrite them with the custom ones we specify above.
diff --git a/BUILD b/BUILD
index b38e376..a629815 100644
--- a/BUILD
+++ b/BUILD
@@ -11,6 +11,8 @@
     "tsconfig.json",
     "tsconfig.node.json",
     "rollup.config.js",
+    # Expose .clang-format so that the static flatbuffer codegen can format its files nicely.
+    ".clang-format",
 ])
 
 # The root repo tsconfig
diff --git a/WORKSPACE b/WORKSPACE
index 9b19ff5..3b2eb57 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -479,9 +479,9 @@
 http_archive(
     name = "allwpilib_ni_libraries",
     build_file = "@//debian:ni-libraries.BUILD",
-    sha256 = "b325099ed896d1325c9cd998f07053b4e60e28a65d991f86f28203bf6728eccd",
-    strip_prefix = "ni-libraries-2024.1.1",
-    url = "https://github.com/wpilibsuite/ni-libraries/archive/refs/tags/v2024.1.1.zip",
+    sha256 = "86458884701c817351b2ec651a2f13591258da54b4f54f05d8f1ce04eb214ba5",
+    strip_prefix = "ni-libraries-2024.2.1",
+    url = "https://github.com/wpilibsuite/ni-libraries/archive/refs/tags/v2024.2.1.zip",
 )
 
 # For protobuf. Don't use these.
@@ -703,9 +703,9 @@
     deps = ["@//third_party/allwpilib/wpimath"],
 )
 """,
-    sha256 = "8b7a64cf71d83f55492a4896d05e7cb3c75c64357b58059e529f59eab7c54ca4",
+    sha256 = "1b9295ac868d619d0263f285b372a22297798e45c8fdeb83ca99154a097c5c98",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.0.0-beta-5/api-cpp-24.0.0-beta-5-headers.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.1.0/api-cpp-24.1.0-headers.zip",
     ],
 )
 
@@ -727,9 +727,9 @@
     target_compatible_with = ['@//tools/platforms/hardware:roborio'],
 )
 """,
-    sha256 = "cdf64c40418fb4ddedcaca70d2c847f2412c1e1242b5b53ec79272c354ab7084",
+    sha256 = "51f4921f8995e3e80ba347a90f6fa5c0cb7d0e438923a1a736554f263e2d5b8e",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.0.0-beta-5/api-cpp-24.0.0-beta-5-linuxathena.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.1.0/api-cpp-24.1.0-linuxathena.zip",
     ],
 )
 
@@ -742,9 +742,9 @@
     hdrs = glob(['ctre/**/*.h', 'ctre/**/*.hpp']),
 )
 """,
-    sha256 = "c5a9da5b4e3d3d886971a88b97d2bcb8dc41ba61dd9a7af65ab80b8c3eb3c296",
+    sha256 = "72be3e50205e2546736361e3aba9c0de5d5318b263c195600f9b558c3d92cb9e",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.0.0-beta-5/tools-24.0.0-beta-5-headers.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.1.0/tools-24.1.0-headers.zip",
     ],
 )
 
@@ -766,9 +766,9 @@
     target_compatible_with = ['@//tools/platforms/hardware:roborio'],
 )
 """,
-    sha256 = "a119d3a1346e3427293237b9780f5063263e5b2803d682a919f6d37ae03837a1",
+    sha256 = "76d30cb8c0988eb6accab10e77ceae492782edb64c06f7df628b8c161cf0220a",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.0.0-beta-5/tools-24.0.0-beta-5-linuxathena.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.1.0/tools-24.1.0-linuxathena.zip",
     ],
 )
 
@@ -781,9 +781,9 @@
     hdrs = glob(['ctre/phoenix/**/*.h']),
 )
 """,
-    sha256 = "58e32fa7f08aff009703233ba8cd5379c9f7ec82ba2dc41ff94364573141e462",
+    sha256 = "d2790e2495a59a3b14ab4b0fd03c2e25292d6874168f5642165acb0d1bab2f99",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.32.0-beta-4/api-cpp-5.32.0-beta-4-headers.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.33.0/api-cpp-5.33.0-headers.zip",
     ],
 )
 
@@ -805,9 +805,9 @@
     target_compatible_with = ['@//tools/platforms/hardware:roborio'],
 )
 """,
-    sha256 = "8fd0c6c147dfaf71b309c6293c2a69ea0b125c694943aab9cb16acfbd2764b4d",
+    sha256 = "e67348b0db1e61797700b5c4e50b38b745f91899665b740b09fde879e4d8ad7a",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.32.0-beta-4/api-cpp-5.32.0-beta-4-linuxathena.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.33.0/api-cpp-5.33.0-linuxathena.zip",
     ],
 )
 
@@ -820,9 +820,9 @@
     hdrs = glob(['ctre/phoenix/**/*.h']),
 )
 """,
-    sha256 = "6addc983287221f6af94ea96ef30c4767fa9d02eced0333156deed6bf6f4888e",
+    sha256 = "56cb8272d623721560eea360730d2508f4f365e2ac1060aec5fbd546cffc0771",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.32.0-beta-4/cci-5.32.0-beta-4-headers.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.33.0/cci-5.33.0-headers.zip",
     ],
 )
 
@@ -844,9 +844,9 @@
     target_compatible_with = ['@//tools/platforms/hardware:roborio'],
 )
 """,
-    sha256 = "072cefb9caf71340e01b4847be493864efce1820cf38fdbfff8bf2122547cf52",
+    sha256 = "7bd8e9950c3e62a3c6ce967e3491c438ba08effde51d434f85b756fa004fbec9",
     urls = [
-        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.32.0-beta-4/cci-5.32.0-beta-4-linuxathena.zip",
+        "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.33.0/cci-5.33.0-linuxathena.zip",
     ],
 )
 
@@ -1643,3 +1643,29 @@
 load("@hedron_compile_commands//:workspace_setup.bzl", "hedron_compile_commands_setup")
 
 hedron_compile_commands_setup()
+
+http_archive(
+    name = "calibrate_multi_cameras_data",
+    build_file_content = """
+filegroup(
+    name = "calibrate_multi_cameras_data",
+    srcs = glob(["**"]),
+    visibility = ["//visibility:public"],
+)""",
+    sha256 = "b106b3b975d3cf3ad3fcd5e4be7409f6095e1d531346a90c4ad6bdb7da1d08a5",
+    url = "https://software.frc971.org/Build-Dependencies/2023_calibrate_multi_cameras_data.tar.gz",
+)
+
+http_archive(
+    name = "com_github_tartanllama_expected",
+    build_file_content = """
+cc_library(
+  name = "com_github_tartanllama_expected",
+  srcs = ["include/tl/expected.hpp"],
+  includes = ["include"],
+  visibility = ["//visibility:public"],
+)""",
+    sha256 = "1db357f46dd2b24447156aaf970c4c40a793ef12a8a9c2ad9e096d9801368df6",
+    strip_prefix = "expected-1.1.0",
+    url = "https://github.com/TartanLlama/expected/archive/refs/tags/v1.1.0.tar.gz",
+)
diff --git a/aos/BUILD b/aos/BUILD
index acef762..4d041ca 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -1,4 +1,5 @@
 load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library", "flatbuffer_rust_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 load("@rules_rust//rust:defs.bzl", "rust_library", "rust_test")
 load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
@@ -233,10 +234,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "configuration_fbs",
     srcs = ["configuration.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -364,7 +364,7 @@
 flatbuffer_cc_library(
     name = "json_to_flatbuffer_fbs",
     srcs = ["json_to_flatbuffer.fbs"],
-    gen_reflections = 1,
+    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//aos:__subpackages__"],
 )
@@ -418,6 +418,7 @@
         ":flatbuffer_utils",
         ":flatbuffers",
         ":json_tokenizer",
+        "//aos/flatbuffers:builder",
         "//aos/util:file",
         "@com_github_google_flatbuffers//:flatbuffers",
         "@com_github_google_glog//:glog",
diff --git a/aos/actions/BUILD b/aos/actions/BUILD
index a76dc3f..5667c12 100644
--- a/aos/actions/BUILD
+++ b/aos/actions/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 
 package(default_visibility = ["//visibility:public"])
@@ -23,24 +23,21 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "actions_fbs",
     srcs = ["actions.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "test_action_fbs",
     srcs = ["test_action.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "test_action2_fbs",
     srcs = ["test_action2.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/aos/events/BUILD b/aos/events/BUILD
index c398e7b..9910fc6 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -1,5 +1,6 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_rust_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_rust_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 load("//tools/build_rules:autocxx.bzl", "autocxx_library")
@@ -7,23 +8,20 @@
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "test_message_fbs",
     srcs = ["test_message.fbs"],
-    gen_reflections = 1,
-    target_compatible_with = ["@platforms//os:linux"],
 )
 
 cc_static_flatbuffer(
     name = "test_message_schema",
     function = "aos::TestMessageSchema",
-    target = ":test_message_fbs_reflection_out",
+    target = ":test_message_fbs_fbs_reflection_out",
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "event_loop_fbs",
     srcs = ["event_loop.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     deps = ["//aos:configuration_fbs"],
 )
@@ -34,11 +32,9 @@
     target = ":event_loop_fbs_reflection_out",
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "ping_fbs",
     srcs = ["ping.fbs"],
-    gen_reflections = 1,
-    target_compatible_with = ["@platforms//os:linux"],
 )
 
 flatbuffer_ts_library(
@@ -65,11 +61,9 @@
     }),
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "pong_fbs",
     srcs = ["pong.fbs"],
-    gen_reflections = 1,
-    target_compatible_with = ["@platforms//os:linux"],
 )
 
 cc_library(
@@ -132,6 +126,7 @@
         "//aos:ftrace",
         "//aos:realtime",
         "//aos:uuid",
+        "//aos/flatbuffers:builder",
         "//aos/ipc_lib:data_alignment",
         "//aos/logging",
         "//aos/time",
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index f57053f..1ff0fb7 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -20,6 +20,7 @@
 #include "aos/events/event_loop_generated.h"
 #include "aos/events/timing_statistics.h"
 #include "aos/flatbuffers.h"
+#include "aos/flatbuffers/builder.h"
 #include "aos/ftrace.h"
 #include "aos/ipc_lib/data_alignment.h"
 #include "aos/json_to_flatbuffer.h"
@@ -183,9 +184,21 @@
   // Returns the associated flatbuffers-style allocator. This must be
   // deallocated before the message is sent.
   ChannelPreallocatedAllocator *fbb_allocator() {
-    fbb_allocator_ = ChannelPreallocatedAllocator(
-        reinterpret_cast<uint8_t *>(data()), size(), channel());
-    return &fbb_allocator_;
+    CHECK(!static_allocator_.has_value())
+        << ": May not mix-and-match static and raw flatbuffer builders.";
+    if (fbb_allocator_.has_value()) {
+      CHECK(!fbb_allocator_.value().allocated())
+          << ": May not have multiple active allocators on a single sender.";
+    }
+    return &fbb_allocator_.emplace(reinterpret_cast<uint8_t *>(data()), size(),
+                                   channel());
+  }
+
+  fbs::SpanAllocator *static_allocator() {
+    CHECK(!fbb_allocator_.has_value())
+        << ": May not mix-and-match static and raw flatbuffer builders.";
+    return &static_allocator_.emplace(
+        std::span<uint8_t>{reinterpret_cast<uint8_t *>(data()), size()});
   }
 
   // Index of the buffer which is currently exposed by data() and the various
@@ -228,7 +241,11 @@
   internal::RawSenderTiming timing_;
   Ftrace ftrace_;
 
-  ChannelPreallocatedAllocator fbb_allocator_{nullptr, 0, nullptr};
+  // Depending on which API is being used, we will populate either
+  // fbb_allocator_ (for use with FlatBufferBuilders) or the SpanAllocator (for
+  // use with the static flatbuffer API).
+  std::optional<ChannelPreallocatedAllocator> fbb_allocator_;
+  std::optional<fbs::SpanAllocator> static_allocator_;
 };
 
 // Needed for compatibility with glog
@@ -325,11 +342,56 @@
 };
 
 // Sends messages to a channel.
+// The type T used with the Sender may either be a raw flatbuffer type (e.g.,
+// aos::examples::Ping) or the static flatbuffer type (e.g.
+// aos::examples::PingStatic). The Builder type that you use must correspond
+// with the flatbuffer type being used.
 template <typename T>
 class Sender {
  public:
   Sender() {}
 
+  // Represents a single message that is about to be sent on the channel.
+  // Uses the static flatbuffer API rather than the FlatBufferBuilder paradigm.
+  //
+  // Typical usage pattern is:
+  //
+  // Sender<PingStatic>::Builder builder = sender.MakeStaticBuilder()
+  // builder.get()->set_value(971);
+  // builder.CheckOk(builder.Send());
+  class StaticBuilder {
+   public:
+    StaticBuilder(RawSender *sender, fbs::SpanAllocator *allocator)
+        : builder_(allocator), sender_(CHECK_NOTNULL(sender)) {}
+    StaticBuilder(const StaticBuilder &) = delete;
+    StaticBuilder(StaticBuilder &&) = default;
+
+    StaticBuilder &operator=(const StaticBuilder &) = delete;
+    StaticBuilder &operator=(StaticBuilder &&) = default;
+
+    fbs::Builder<T> *builder() {
+      DCHECK(builder_.has_value());
+      return &builder_.value();
+    }
+
+    T *get() { return builder()->get(); }
+    T &operator*() { return *get(); }
+    T *operator->() { return get(); }
+
+    RawSender::Error Send() {
+      const auto err = sender_->Send(builder_.value().buffer().size());
+      builder_.reset();
+      return err;
+    }
+
+    // Equivalent to RawSender::CheckOk
+    void CheckOk(const RawSender::Error err) { sender_->CheckOk(err); };
+
+   private:
+    std::optional<fbs::Builder<T>> builder_;
+    RawSender *sender_;
+  };
+
   // Represents a single message about to be sent to the queue.
   // The lifecycle goes:
   //
@@ -399,6 +461,9 @@
   // assigning a default-constructed Builder to it) before calling this method
   // again to overwrite the value in the variable.
   Builder MakeBuilder();
+  StaticBuilder MakeStaticBuilder() {
+    return StaticBuilder(sender_.get(), sender_->static_allocator());
+  }
 
   // Sends a prebuilt flatbuffer.
   // This will copy the data out of the provided flatbuffer, and so does not
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index c861284..5ec494f 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -9,6 +9,7 @@
 #include "gtest/gtest.h"
 
 #include "aos/events/test_message_generated.h"
+#include "aos/events/test_message_static.h"
 #include "aos/flatbuffer_merge.h"
 #include "aos/logging/log_message_generated.h"
 #include "aos/logging/logging.h"
@@ -101,6 +102,38 @@
   EXPECT_TRUE(happened);
 }
 
+// Tests that watcher can receive messages from a static sender.
+// This confirms that the "static" flatbuffer API works with the EventLoop
+// senders.
+TEST_P(AbstractEventLoopTest, BasicStatic) {
+  auto loop1 = Make();
+  auto loop2 = MakePrimary();
+
+  aos::Sender<TestMessageStatic> sender =
+      loop1->MakeSender<TestMessageStatic>("/test");
+
+  bool happened = false;
+
+  loop2->OnRun([&]() {
+    happened = true;
+
+    aos::Sender<TestMessageStatic>::StaticBuilder msg =
+        sender.MakeStaticBuilder();
+    msg->set_value(200);
+    CHECK(msg.builder()->Verify());
+    msg.CheckOk(msg.Send());
+  });
+
+  loop2->MakeWatcher("/test", [&](const TestMessage &message) {
+    EXPECT_EQ(message.value(), 200);
+    this->Exit();
+  });
+
+  EXPECT_FALSE(happened);
+  Run();
+  EXPECT_TRUE(happened);
+}
+
 // Tests that watcher can receive messages from a sender, sent via SendDetached.
 TEST_P(AbstractEventLoopTest, BasicSendDetached) {
   auto loop1 = Make();
@@ -3445,7 +3478,7 @@
     builder.MakeBuilder<TestMessage>().Finish();
     // But not a second one.
     EXPECT_DEATH(sender.MakeBuilder().MakeBuilder<TestMessage>().Finish(),
-                 "May not overwrite in-use allocator");
+                 "May not have multiple active allocators");
   }
 
   FlatbufferDetachedBuffer<TestMessage> detached =
@@ -3457,7 +3490,7 @@
   {
     // This is the second one, after the detached one, so it should fail.
     EXPECT_DEATH(sender.MakeBuilder().MakeBuilder<TestMessage>().Finish(),
-                 "May not overwrite in-use allocator");
+                 "May not have multiple active allocators");
   }
 
   // Clear the detached one, and then we should be able to create another.
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index 132da04..cc8e5c3 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -427,6 +427,7 @@
 template <typename T>
 RawSender::Error Sender<T>::Send(
     const NonSizePrefixedFlatbuffer<T> &flatbuffer) {
+  CHECK(valid()) << ": Sender must be initialized before sending.";
   return sender_->Send(flatbuffer.span().data(), flatbuffer.span().size());
 }
 
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index a9c8497..1743f88 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -1,22 +1,20 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "logger_fbs",
     srcs = ["logger.fbs"],
-    gen_reflections = True,
-    includes = [
-        "//aos:configuration_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
+    deps = [
+        "//aos:configuration_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "replay_timing_fbs",
     srcs = ["replay_timing.fbs"],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
@@ -27,10 +25,9 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "log_replayer_stats_fbs",
     srcs = ["log_replayer_stats.fbs"],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
         ":replay_config_fbs",
@@ -44,10 +41,9 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "replay_config_fbs",
     srcs = ["log_replayer_config.fbs"],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
     deps = [
@@ -958,10 +954,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "test_message_fbs",
     srcs = ["test_message.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
@@ -992,3 +987,16 @@
         "@com_github_google_glog//:glog",
     ],
 )
+
+cc_binary(
+    name = "ssd_profiler",
+    srcs = [
+        "ssd_profiler.cc",
+    ],
+    deps = [
+        "//aos:init",
+        "//aos/containers:resizeable_buffer",
+        "//aos/time",
+        "@com_github_google_glog//:glog",
+    ],
+)
diff --git a/y2023/ssd_profiler.cc b/aos/events/logging/ssd_profiler.cc
similarity index 100%
rename from y2023/ssd_profiler.cc
rename to aos/events/logging/ssd_profiler.cc
diff --git a/aos/events/ping_lib.cc b/aos/events/ping_lib.cc
index 56dde2f..cd36378 100644
--- a/aos/events/ping_lib.cc
+++ b/aos/events/ping_lib.cc
@@ -3,8 +3,8 @@
 #include "gflags/gflags.h"
 #include "glog/logging.h"
 
-#include "aos/events/ping_generated.h"
-#include "aos/events/pong_generated.h"
+#include "aos/events/ping_static.h"
+#include "aos/events/pong_static.h"
 #include "aos/json_to_flatbuffer.h"
 
 DEFINE_int32(sleep_us, 10000, "Time to sleep between pings");
@@ -15,7 +15,7 @@
 
 Ping::Ping(EventLoop *event_loop, std::string_view channel_name)
     : event_loop_(event_loop),
-      sender_(event_loop_->MakeSender<examples::Ping>(channel_name)) {
+      sender_(event_loop_->MakeSender<examples::PingStatic>(channel_name)) {
   timer_handle_ = event_loop_->AddTimer([this]() { SendPing(); });
   timer_handle_->set_name("ping");
 
@@ -38,12 +38,12 @@
                  << FLAGS_sleep_us << "us.";
   }
   ++count_;
-  aos::Sender<examples::Ping>::Builder builder = sender_.MakeBuilder();
-  examples::Ping::Builder ping_builder = builder.MakeBuilder<examples::Ping>();
-  ping_builder.add_value(count_);
-  ping_builder.add_send_time(
+  aos::Sender<examples::PingStatic>::StaticBuilder builder =
+      sender_.MakeStaticBuilder();
+  builder->set_value(count_);
+  builder->set_send_time(
       event_loop_->monotonic_now().time_since_epoch().count());
-  builder.CheckOk(builder.Send(ping_builder.Finish()));
+  builder.CheckOk(builder.Send());
   VLOG(2) << "Sending ping";
 }
 
diff --git a/aos/events/ping_lib.h b/aos/events/ping_lib.h
index fec6c45..aa40d6b 100644
--- a/aos/events/ping_lib.h
+++ b/aos/events/ping_lib.h
@@ -5,8 +5,8 @@
 #include <string_view>
 
 #include "aos/events/event_loop.h"
-#include "aos/events/ping_generated.h"
-#include "aos/events/pong_generated.h"
+#include "aos/events/ping_static.h"
+#include "aos/events/pong_static.h"
 
 namespace aos {
 
@@ -25,7 +25,7 @@
   void HandlePong(const examples::Pong &pong);
 
   aos::EventLoop *event_loop_;
-  aos::Sender<examples::Ping> sender_;
+  aos::Sender<examples::PingStatic> sender_;
   // Timer handle which sends the Ping message.
   aos::TimerHandler *timer_handle_;
   // Number of pings sent.
diff --git a/aos/events/pong_lib.cc b/aos/events/pong_lib.cc
index e8bf171..8fc3f62 100644
--- a/aos/events/pong_lib.cc
+++ b/aos/events/pong_lib.cc
@@ -3,8 +3,8 @@
 #include "glog/logging.h"
 
 #include "aos/events/event_loop.h"
-#include "aos/events/ping_generated.h"
-#include "aos/events/pong_generated.h"
+#include "aos/events/ping_static.h"
+#include "aos/events/pong_static.h"
 
 DEFINE_bool(fetch, false, "Poll & fetch messages instead of using a watcher.");
 DEFINE_uint32(fetch_period_ms, 10, "Frequency at which to fetch.");
@@ -14,7 +14,7 @@
 Pong::Pong(EventLoop *event_loop)
     : event_loop_(event_loop),
       fetcher_(event_loop_->MakeFetcher<examples::Ping>("/test")),
-      sender_(event_loop_->MakeSender<examples::Pong>("/test")) {
+      sender_(event_loop_->MakeSender<examples::PongStatic>("/test")) {
   if (FLAGS_fetch) {
     event_loop_
         ->AddPhasedLoop(
@@ -40,11 +40,11 @@
   }
   last_value_ = ping.value();
   last_send_time_ = ping.send_time();
-  aos::Sender<examples::Pong>::Builder builder = sender_.MakeBuilder();
-  examples::Pong::Builder pong_builder = builder.MakeBuilder<examples::Pong>();
-  pong_builder.add_value(ping.value());
-  pong_builder.add_initial_send_time(ping.send_time());
-  builder.CheckOk(builder.Send(pong_builder.Finish()));
+  aos::Sender<examples::PongStatic>::StaticBuilder builder =
+      sender_.MakeStaticBuilder();
+  builder->set_value(ping.value());
+  builder->set_initial_send_time(ping.send_time());
+  builder.CheckOk(builder.Send());
 }
 
 }  // namespace aos
diff --git a/aos/events/pong_lib.h b/aos/events/pong_lib.h
index a12dad0..17e5d35 100644
--- a/aos/events/pong_lib.h
+++ b/aos/events/pong_lib.h
@@ -2,8 +2,8 @@
 #define AOS_EVENTS_PONG_LIB_H_
 
 #include "aos/events/event_loop.h"
-#include "aos/events/ping_generated.h"
-#include "aos/events/pong_generated.h"
+#include "aos/events/ping_static.h"
+#include "aos/events/pong_static.h"
 
 namespace aos {
 
@@ -18,7 +18,7 @@
   void HandlePing(const examples::Ping &ping);
   EventLoop *event_loop_;
   aos::Fetcher<examples::Ping> fetcher_;
-  aos::Sender<examples::Pong> sender_;
+  aos::Sender<examples::PongStatic> sender_;
   int32_t last_value_ = 0;
   int32_t last_send_time_ = 0;
 
diff --git a/aos/events/shm_event_loop_test.cc b/aos/events/shm_event_loop_test.cc
index 2382f07..2134687 100644
--- a/aos/events/shm_event_loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -389,7 +389,9 @@
     SCOPED_TRACE(std::to_string(i));
     EXPECT_DEATH(
         {
-          ++static_cast<char *>(sender->data())[-1 - i];
+          // Can't use `data()[-1 -i]` here because that's undefined behaviour.
+          // We do manual pointer arithmetic to work around that.
+          ++(*(static_cast<char *>(sender->data()) - 1 - i));
           sender->CheckOk(sender->Send(0));
         },
         "Somebody wrote outside the buffer of their message");
diff --git a/aos/flatbuffer_utils.cc b/aos/flatbuffer_utils.cc
index 3429fe4..5abe676 100644
--- a/aos/flatbuffer_utils.cc
+++ b/aos/flatbuffer_utils.cc
@@ -19,6 +19,26 @@
   LOG(FATAL) << "Unimplemented";
 }
 
+bool FlatbufferType::IsTable() const {
+  if (type_table_) {
+    return type_table_->st == flatbuffers::ST_TABLE;
+  }
+  if (object_) {
+    return !object_->is_struct();
+  }
+  LOG(FATAL) << "Unimplemented";
+}
+
+bool FlatbufferType::IsStruct() const {
+  if (type_table_) {
+    return type_table_->st == flatbuffers::ST_STRUCT;
+  }
+  if (object_) {
+    return object_->is_struct();
+  }
+  LOG(FATAL) << "Unimplemented";
+}
+
 bool FlatbufferType::IsEnum() const {
   if (type_table_) {
     return type_table_->st == flatbuffers::ST_ENUM;
@@ -256,10 +276,64 @@
 
 }  // namespace
 
+size_t FlatbufferType::InlineSize() const {
+  DCHECK(IsSequence());
+  if (type_table_) {
+    return flatbuffers::InlineSize(flatbuffers::ElementaryType::ET_SEQUENCE,
+                                   type_table_);
+  }
+  if (object_) {
+    return object_->is_struct() ? object_->bytesize() : /*offset size*/ 4u;
+  }
+  if (enum_) {
+    return BaseTypeInlineSize(enum_->underlying_type()->base_type());
+  }
+  LOG(FATAL) << "Unimplemented";
+}
+
+// Returns the required alignment for this type.
+size_t FlatbufferType::Alignment() const {
+  if (type_table_) {
+    // Attempt to derive alignment as max alignment of the members.
+    size_t alignment = 1u;
+    for (size_t field_index = 0;
+         field_index < static_cast<size_t>(NumberFields()); ++field_index) {
+      alignment = std::max(alignment, FieldInlineAlignment(field_index));
+    }
+    return alignment;
+  }
+  if (object_) {
+    return object_->minalign();
+  }
+  // We don't do a great job of supporting unions in general, and as of this
+  // writing did not try to look up what the alignment rules for unions were.
+  LOG(FATAL) << "Unimplemented";
+}
+
+size_t FlatbufferType::FieldInlineAlignment(size_t field_index) const {
+  if (FieldIsSequence(field_index) && FieldType(field_index).IsStruct()) {
+    return FieldType(field_index).Alignment();
+  }
+  return FieldInlineSize(field_index);
+}
+
+size_t FlatbufferType::StructFieldOffset(int index) const {
+  DCHECK(IsStruct());
+  if (type_table_) {
+    return type_table_->values[index];
+  }
+  if (object_) {
+    return ReflectionObjectField(index)->offset();
+  }
+  LOG(FATAL) << "Unimplemented";
+}
+
 size_t FlatbufferType::FieldInlineSize(int index) const {
   DCHECK(IsSequence());
   if (type_table_) {
-    return flatbuffers::InlineSize(FieldElementaryType(index), type_table_);
+    return flatbuffers::InlineSize(
+        FieldElementaryType(index),
+        FieldIsSequence(index) ? FieldType(index).type_table_ : nullptr);
   }
   if (object_ || enum_) {
     const reflection::Type *const type = ReflectionType(index);
diff --git a/aos/flatbuffer_utils.h b/aos/flatbuffer_utils.h
index 0287b6d..aa22e68 100644
--- a/aos/flatbuffer_utils.h
+++ b/aos/flatbuffer_utils.h
@@ -43,6 +43,12 @@
   // Returns whether this type is an enum.
   bool IsEnum() const;
 
+  // Returns whether this type is a struct.
+  bool IsStruct() const;
+
+  // Returns whether this type is a table.
+  bool IsTable() const;
+
   // Returns whether the given field is a sequence (table, struct, or union).
   //
   // Only valid for sequences (tables, structs, or unions).
@@ -84,6 +90,7 @@
   //
   // Only valid for sequences (tables, structs, or unions).
   size_t FieldInlineSize(int index) const;
+  size_t InlineSize() const;
 
   // Returns the total number of fields.
   //
@@ -95,6 +102,16 @@
   // Only valid for sequences (tables, structs, or unions).
   FlatbufferType FieldType(int index) const;
 
+  // Returns the offset of the specified field within the struct.
+  //
+  // Only valid for structs.
+  size_t StructFieldOffset(int index) const;
+
+  // Returns the required alignment for this type.
+  size_t Alignment() const;
+  // The required alignment of the inline data for the specified field.
+  size_t FieldInlineAlignment(size_t field_index) const;
+
  private:
   explicit FlatbufferType(const reflection::Schema *schema,
                           const reflection::Object *object)
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index ed9c8ac..a299fe9 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -529,6 +529,15 @@
   std::shared_ptr<absl::Span<uint8_t>> span_;
 };
 
+// The regular flatbuffer API makes it surprisingly irritating to unpack an
+// Object into an ObjectT without a bunch of extra lines.
+template <typename T>
+T::NativeTableType UnpackFlatbuffer(const T *fbs) {
+  typename T::NativeTableType object;
+  fbs->UnPackTo(&object);
+  return object;
+}
+
 }  // namespace aos
 
 #endif  // AOS_FLATBUFFERS_H_
diff --git a/aos/flatbuffers/BUILD b/aos/flatbuffers/BUILD
new file mode 100644
index 0000000..08d548f
--- /dev/null
+++ b/aos/flatbuffers/BUILD
@@ -0,0 +1,166 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
+cc_library(
+    name = "static_flatbuffers",
+    srcs = ["static_flatbuffers.cc"],
+    hdrs = ["static_flatbuffers.h"],
+    deps = [
+        ":static_table",
+        ":static_vector",
+        "//aos:json_to_flatbuffer",
+        "@com_github_google_flatbuffers//:flatbuffers",
+        "@com_github_google_glog//:glog",
+        "@com_google_absl//absl/strings",
+        "@com_google_absl//absl/strings:str_format",
+    ],
+)
+
+cc_library(
+    name = "base",
+    srcs = ["base.cc"],
+    hdrs = ["base.h"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "@com_github_google_flatbuffers//:flatbuffers",
+        "@com_github_google_glog//:glog",
+    ],
+)
+
+cc_test(
+    name = "base_test",
+    srcs = ["base_test.cc"],
+    deps = [
+        ":base",
+        "//aos/testing:googletest",
+    ],
+)
+
+cc_library(
+    name = "static_table",
+    hdrs = ["static_table.h"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":base",
+        "@com_github_google_flatbuffers//:flatbuffers",
+        "@com_github_google_glog//:glog",
+    ],
+)
+
+cc_library(
+    name = "static_vector",
+    hdrs = ["static_vector.h"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/containers:inlined_vector",
+        "//aos/containers:sized_array",
+        "@com_github_google_flatbuffers//:flatbuffers",
+        "@com_github_google_glog//:glog",
+    ],
+)
+
+cc_library(
+    name = "builder",
+    hdrs = ["builder.h"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":static_table",
+        "//aos:flatbuffers",
+    ],
+)
+
+cc_static_flatbuffer(
+    name = "test_schema",
+    function = "aos::fbs::testing::TestTableSchema",
+    target = ":test_fbs_fbs_reflection_out",
+)
+
+cc_static_flatbuffer(
+    name = "interesting_schemas",
+    function = "aos::fbs::testing::UnsupportedSchema",
+    target = ":interesting_schemas_fbs_reflection_out",
+)
+
+flatbuffer_cc_library(
+    name = "interesting_schemas_fbs",
+    srcs = ["interesting_schemas.fbs"],
+    gen_reflections = True,
+)
+
+cc_test(
+    name = "static_flatbuffers_test",
+    srcs = ["static_flatbuffers_test.cc"],
+    data = ["//aos/flatbuffers/test_dir:test_data"],
+    deps = [
+        ":builder",
+        ":interesting_schemas",
+        ":static_flatbuffers",
+        ":test_fbs",
+        ":test_schema",
+        "//aos:flatbuffers",
+        "//aos:json_to_flatbuffer",
+        "//aos/flatbuffers/test_dir:type_coverage_fbs",
+        "//aos/testing:googletest",
+        "//aos/testing:path",
+        "@com_github_google_flatbuffers//src:flatc_library",
+    ],
+)
+
+cc_test(
+    name = "static_flatbuffers_fuzz_test",
+    # This does a combinatoric sweep over a variety of things, and can end up taking a while.
+    timeout = "eternal",
+    srcs = ["static_flatbuffers_fuzz_test.cc"],
+    data = ["//aos/flatbuffers/test_dir:test_data"],
+    deps = [
+        ":builder",
+        ":static_flatbuffers",
+        ":test_fbs",
+        "//aos:flatbuffers",
+        "//aos:json_to_flatbuffer",
+        "//aos/flatbuffers/test_dir:type_coverage_fbs",
+        "//aos/testing:googletest",
+        "//aos/testing:path",
+    ],
+)
+
+cc_binary(
+    name = "generate",
+    srcs = ["generate.cc"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":static_flatbuffers",
+        "//aos:flatbuffers",
+        "//aos:init",
+        "//aos:json_to_flatbuffer",
+        "//aos/util:file",
+        "@com_github_google_flatbuffers//:flatbuffers",
+    ],
+)
+
+sh_binary(
+    name = "generate_wrapper",
+    srcs = ["generate.sh"],
+    data = [
+        ":generate",
+        "//:.clang-format",
+        "@llvm_k8//:bin",
+    ],
+    visibility = ["//visibility:public"],
+    deps = ["@bazel_tools//tools/bash/runfiles"],
+)
+
+static_flatbuffer(
+    name = "test_fbs",
+    srcs = ["test.fbs"],
+    deps = ["//aos/flatbuffers/test_dir:include_fbs"],
+)
+
+filegroup(
+    name = "test_static_file",
+    srcs = ["test_static.h"],
+    visibility = [":__subpackages__"],
+)
diff --git a/aos/flatbuffers/base.cc b/aos/flatbuffers/base.cc
new file mode 100644
index 0000000..697f837
--- /dev/null
+++ b/aos/flatbuffers/base.cc
@@ -0,0 +1,212 @@
+#include "aos/flatbuffers/base.h"
+namespace aos::fbs {
+
+namespace {
+void *DereferenceOffset(uoffset_t *offset) {
+  return reinterpret_cast<uint8_t *>(offset) + *offset;
+}
+}  // namespace
+
+bool ResizeableObject::InsertBytes(void *insertion_point, size_t bytes,
+                                   SetZero set_zero) {
+  // See comments on InsertBytes() declaration and in FixObjects()
+  // implementation below.
+  CHECK_LT(buffer_.data(), reinterpret_cast<const uint8_t *>(insertion_point))
+      << ": Insertion may not be prior to the start of the buffer.";
+  // Check that we started off with a properly aligned size.
+  // Doing this CHECK earlier is tricky because if done in the constructor then
+  // it executes prior to the Alignment() implementation being available.
+  CHECK_EQ(0u, buffer_.size() % Alignment());
+  // Note that we will round up the size to the current alignment, so that we
+  // ultimately end up only adjusting the buffer size by a multiple of its
+  // alignment, to avoid having to do any more complicated bookkeeping.
+  const size_t aligned_bytes = PaddedSize(bytes, Alignment());
+  if (parent_ != nullptr) {
+    return parent_->InsertBytes(insertion_point, aligned_bytes, set_zero);
+  } else {
+    std::optional<std::span<uint8_t>> new_buffer =
+        CHECK_NOTNULL(allocator_)
+            ->InsertBytes(insertion_point, aligned_bytes, Alignment(),
+                          set_zero);
+    if (!new_buffer.has_value()) {
+      return false;
+    }
+    UpdateBuffer(new_buffer.value(),
+                 new_buffer.value().data() +
+                     (reinterpret_cast<const uint8_t *>(insertion_point) -
+                      buffer_.data()),
+                 aligned_bytes);
+    return true;
+  }
+}
+
+void ResizeableObject::UpdateBuffer(std::span<uint8_t> new_buffer,
+                                    void *modification_point,
+                                    ssize_t bytes_inserted) {
+  buffer_ = new_buffer;
+  FixObjects(modification_point, bytes_inserted);
+  ObserveBufferModification();
+}
+
+std::span<uint8_t> ResizeableObject::BufferForObject(
+    size_t absolute_offset, size_t size, size_t terminal_alignment) {
+  const size_t padded_size = PaddedSize(size, terminal_alignment);
+  std::span<uint8_t> padded_buffer =
+      internal::GetSubSpan(buffer_, absolute_offset, padded_size);
+  std::span<uint8_t> object_buffer =
+      internal::GetSubSpan(padded_buffer, 0, size);
+  std::span<uint8_t> padding = internal::GetSubSpan(padded_buffer, size);
+  internal::ClearSpan(padding);
+  return object_buffer;
+}
+
+void ResizeableObject::FixObjects(void *modification_point,
+                                  ssize_t bytes_inserted) {
+  CHECK_EQ(bytes_inserted % Alignment(), 0u)
+      << ": We only support inserting N * Alignment() bytes at a time. This "
+         "may change in the future.";
+  for (size_t index = 0; index < NumberOfSubObjects(); ++index) {
+    SubObject object = GetSubObject(index);
+    const void *const absolute_offset =
+        PointerForAbsoluteOffset(*object.absolute_offset);
+    if (absolute_offset >= modification_point &&
+        object.inline_entry < modification_point) {
+      if (*object.inline_entry != 0) {
+        CHECK_EQ(static_cast<const void *>(
+                     static_cast<const uint8_t *>(absolute_offset) +
+                     CHECK_NOTNULL(object.object)->AbsoluteOffsetOffset()),
+                 DereferenceOffset(object.inline_entry));
+        *object.inline_entry += bytes_inserted;
+        CHECK_GE(DereferenceOffset(object.inline_entry), modification_point)
+            << ": Encountered offset which points to a now-deleted section "
+               "of memory. The offset should have been null'd out prior to "
+               "deleting the memory.";
+      } else {
+        CHECK_EQ(nullptr, object.object);
+      }
+      *object.absolute_offset += bytes_inserted;
+    }
+    // We only need to update the object's buffer if it currently exists.
+    if (object.object != nullptr) {
+      std::span<uint8_t> subbuffer = BufferForObject(
+          *object.absolute_offset, object.object->buffer_.size(),
+          object.object->Alignment());
+      // By convention (enforced in InsertBytes()), the modification_point shall
+      // not be at the start of the subobjects data buffer; it may be the byte
+      // just past the end of the buffer. This makes it so that is unambiguous
+      // which subobject(s) should get the extra space when a buffer size
+      // increase is requested on the edge of a buffer.
+      if (subbuffer.data() < modification_point &&
+          (subbuffer.data() + subbuffer.size()) >= modification_point) {
+        subbuffer = {subbuffer.data(), subbuffer.size() + bytes_inserted};
+      }
+      object.object->UpdateBuffer(subbuffer, modification_point,
+                                  bytes_inserted);
+    }
+  }
+}
+
+std::optional<std::span<uint8_t>> VectorAllocator::Allocate(
+    size_t size, size_t /*alignment*/, SetZero set_zero) {
+  CHECK(buffer_.empty()) << ": Must deallocate before calling Allocate().";
+  buffer_.resize(size);
+  if (set_zero == SetZero::kYes) {
+    memset(buffer_.data(), 0, buffer_.size());
+  }
+  return std::span<uint8_t>{buffer_.data(), buffer_.size()};
+}
+
+std::optional<std::span<uint8_t>> VectorAllocator::InsertBytes(
+    void *insertion_point, size_t bytes, size_t /*alignment*/, SetZero) {
+  const ssize_t insertion_index =
+      reinterpret_cast<uint8_t *>(insertion_point) - buffer_.data();
+  CHECK_LE(0, insertion_index);
+  CHECK_LE(insertion_index, static_cast<ssize_t>(buffer_.size()));
+  buffer_.insert(buffer_.begin() + insertion_index, bytes, 0);
+  return std::span<uint8_t>{buffer_.data(), buffer_.size()};
+}
+
+std::span<uint8_t> VectorAllocator::RemoveBytes(
+    std::span<uint8_t> remove_bytes) {
+  const ssize_t removal_index = remove_bytes.data() - buffer_.data();
+  CHECK_LE(0, removal_index);
+  CHECK_LE(removal_index, static_cast<ssize_t>(buffer_.size()));
+  CHECK_LE(removal_index + remove_bytes.size(), buffer_.size());
+  buffer_.erase(buffer_.begin() + removal_index,
+                buffer_.begin() + removal_index + remove_bytes.size());
+  return {buffer_.data(), buffer_.size()};
+}
+
+std::optional<std::span<uint8_t>> SpanAllocator::Allocate(size_t size,
+                                                          size_t /*alignment*/,
+                                                          SetZero set_zero) {
+  CHECK(!allocated_);
+  if (size > buffer_.size()) {
+    return std::nullopt;
+  }
+  if (set_zero == SetZero::kYes) {
+    memset(buffer_.data(), 0, buffer_.size());
+  }
+  allocated_size_ = size;
+  allocated_ = true;
+  return internal::GetSubSpan(buffer_, buffer_.size() - size);
+}
+
+std::optional<std::span<uint8_t>> SpanAllocator::InsertBytes(
+    void *insertion_point, size_t bytes, size_t /*alignment*/,
+    SetZero set_zero) {
+  uint8_t *insertion_point_typed = reinterpret_cast<uint8_t *>(insertion_point);
+  const ssize_t insertion_index = insertion_point_typed - buffer_.data();
+  CHECK_LE(0, insertion_index);
+  CHECK_LE(insertion_index, static_cast<ssize_t>(buffer_.size()));
+  const size_t new_size = allocated_size_ + bytes;
+  if (new_size > buffer_.size()) {
+    VLOG(1) << ": Insufficient space to grow by " << bytes << " bytes.";
+    return std::nullopt;
+  }
+  const size_t old_start_index = buffer_.size() - allocated_size_;
+  const size_t new_start_index = buffer_.size() - new_size;
+  memmove(buffer_.data() + new_start_index, buffer_.data() + old_start_index,
+          insertion_index - old_start_index);
+  if (set_zero == SetZero::kYes) {
+    memset(insertion_point_typed - bytes, 0, bytes);
+  }
+  allocated_size_ = new_size;
+  return internal::GetSubSpan(buffer_, buffer_.size() - allocated_size_);
+}
+
+std::span<uint8_t> SpanAllocator::RemoveBytes(std::span<uint8_t> remove_bytes) {
+  const ssize_t removal_index = remove_bytes.data() - buffer_.data();
+  const size_t old_start_index = buffer_.size() - allocated_size_;
+  CHECK_LE(static_cast<ssize_t>(old_start_index), removal_index);
+  CHECK_LE(removal_index, static_cast<ssize_t>(buffer_.size()));
+  CHECK_LE(removal_index + remove_bytes.size(), buffer_.size());
+  uint8_t *old_buffer_start = buffer_.data() + old_start_index;
+  memmove(old_buffer_start + remove_bytes.size(), old_buffer_start,
+          removal_index - old_start_index);
+  allocated_size_ -= remove_bytes.size();
+  return internal::GetSubSpan(buffer_, buffer_.size() - allocated_size_);
+}
+
+void SpanAllocator::Deallocate(std::span<uint8_t>) {
+  CHECK(allocated_) << ": Called Deallocate() without a prior allocation.";
+  allocated_ = false;
+}
+
+namespace internal {
+std::ostream &DebugBytes(std::span<const uint8_t> span, std::ostream &os) {
+  constexpr size_t kRowSize = 8u;
+  for (size_t index = 0; index < span.size(); index += kRowSize) {
+    os << std::hex << std::setw(4) << std::setfill('0') << std::uppercase
+       << index << ": ";
+    for (size_t subindex = 0;
+         subindex < kRowSize && (index + subindex) < span.size(); ++subindex) {
+      os << std::setw(2) << static_cast<int>(span[index + subindex]) << " ";
+    }
+    os << "\n";
+  }
+  os << std::resetiosflags(std::ios_base::basefield | std::ios_base::uppercase);
+  return os;
+}
+}  // namespace internal
+}  // namespace aos::fbs
diff --git a/aos/flatbuffers/base.h b/aos/flatbuffers/base.h
new file mode 100644
index 0000000..87e07c5
--- /dev/null
+++ b/aos/flatbuffers/base.h
@@ -0,0 +1,312 @@
+#ifndef AOS_FLATBUFFERS_BASE_H_
+#define AOS_FLATBUFFERS_BASE_H_
+#include <iomanip>
+#include <memory>
+#include <optional>
+#include <span>
+
+#include "flatbuffers/base.h"
+#include "glog/logging.h"
+namespace aos::fbs {
+using ::flatbuffers::soffset_t;
+using ::flatbuffers::uoffset_t;
+using ::flatbuffers::voffset_t;
+
+// Returns the smallest multiple of alignment that is greater than or equal to
+// size.
+constexpr size_t PaddedSize(size_t size, size_t alignment) {
+  // We can be clever with bitwise operations by assuming that aligment is a
+  // power of two. Or we can just be clearer about what we mean and eat a few
+  // integer divides.
+  return (((size - 1) / alignment) + 1) * alignment;
+}
+
+// Used as a parameter to methods where we are messing with memory and may or
+// may not want to clear it to zeroes.
+enum class SetZero { kYes, kNo };
+
+class Allocator;
+
+// Parent type of any object that may need to dynamically change size at
+// runtime. Used by the static table and vector types to request additional
+// blocks of memory when needed.
+//
+// The way that this works is that every ResizeableObject has some number of
+// children that are themselves ResizeableObject's and whose memory is entirely
+// contained within their parent's memory. A ResizeableObject without a parent
+// instead has an Allocator that it can use to allocate additional blocks
+// of memory. Whenever a child needs to grow in size, it will make a call to
+// InsertBytes() on its parent, which will percolate up until InsertBytes() gets
+// called on the root allocator. If the insert succeeds, then every single child
+// through the entire tree will get notified (this is because the allocator may
+// have shifted the entire memory buffer, so any pointers may need to be
+// updated). Child types will provide implementations of the GetObjects() method
+// to both allow tree traversal as well as to allow the various internal offsets
+// to be updated appropriately.
+class ResizeableObject {
+ public:
+  // Returns the underlying memory buffer into which the flatbuffer will be
+  // serialized.
+  std::span<uint8_t> buffer() { return buffer_; }
+  std::span<const uint8_t> buffer() const { return buffer_; }
+
+  // Updates the underlying memory buffer to new_buffer, with an indication of
+  // where bytes were inserted/removed from the buffer. It is assumed that
+  // new_buffer already has the state of the serialized flatbuffer
+  // copied into it.
+  // * When bytes have been inserted, modification_point will point to the first
+  //   of the inserted bytes in new_buffer and bytes_inserted will be the number
+  //   of new bytes.
+  // * Buffer shrinkage is not currently supported.
+  // * When bytes_inserted is zero, modification_point is ignored.
+  void UpdateBuffer(std::span<uint8_t> new_buffer, void *modification_point,
+                    ssize_t bytes_inserted);
+
+ protected:
+  // Data associated with a sub-object of this object.
+  struct SubObject {
+    // A direct pointer to the inline entry in the flatbuffer table data. The
+    // pointer must be valid, but the entry itself may be zero if the object is
+    // not actually populated.
+    // If *inline_entry is non-zero, this will get updated if any new memory got
+    // added/removed in-between inline_entry and the actual data pointed to be
+    // inline_entry.
+    uoffset_t *inline_entry;
+    // The actual child object. Should be nullptr if *inline_entry is zero; must
+    // be valid if *inline_entry is non-zero.
+    ResizeableObject *object;
+    // The nominal offset from buffer_.data() to object->buffer_.data().
+    // Must be provided, and must always be valid, even if *inline_entry is
+    // zero.
+    // I.e., the following holds when object is not nullptr:
+    // SubObject object = parent.GetSubObject(index);
+    // CHECK_EQ(parent.buffer()->data() + *object.absolute_offset,
+    // object.object->buffer().data());
+    size_t *absolute_offset;
+  };
+
+  ResizeableObject(std::span<uint8_t> buffer, ResizeableObject *parent)
+      : buffer_(buffer), parent_(parent) {}
+  ResizeableObject(std::span<uint8_t> buffer, Allocator *allocator)
+      : buffer_(buffer), allocator_(allocator) {}
+  ResizeableObject(std::span<uint8_t> buffer,
+                   std::unique_ptr<Allocator> allocator)
+      : buffer_(buffer),
+        owned_allocator_(std::move(allocator)),
+        allocator_(owned_allocator_.get()) {}
+  ResizeableObject(const ResizeableObject &) = delete;
+  ResizeableObject &operator=(const ResizeableObject &) = delete;
+  // Users do not end up using the move constructor; however, it is needed to
+  // handle the fact that a ResizeableObject may be a member of an std::vector
+  // in the various generated types.
+  ResizeableObject(ResizeableObject &&other)
+      : buffer_(other.buffer_),
+        owned_allocator_(std::move(other.owned_allocator_)),
+        allocator_(owned_allocator_.get()) {
+    other.buffer_ = {};
+    other.allocator_ = nullptr;
+  }
+  // Required alignment of this object.
+  virtual size_t Alignment() const = 0;
+  // Offset from the start of buffer() to the actual start of the object in
+  // question (this is important for vectors, where the vector itself cannot
+  // have internal padding, and so the start of the vector may be offset from
+  // the start of the buffer to handle alignment).
+  virtual size_t AbsoluteOffsetOffset() const = 0;
+  // Causes bytes bytes to be inserted between insertion_point - 1 and
+  // insertion_point.
+  // If requested, the new bytes will be cleared to zero; otherwise they will be
+  // left uninitialized.
+  // The insertion_point may not be equal to this->buffer_.data(); it may be a
+  // pointer just past the end of the buffer. This is to ease the
+  // implementation, and is merely a requirement that any buffer growth occur
+  // only on the inside or past the end of the vector, and not prior to the
+  // start of the vector.
+  // Returns true on success, false on failure (e.g., if the allocator has no
+  // memory available).
+  bool InsertBytes(void *insertion_point, size_t bytes, SetZero set_zero);
+  // Called *after* the internal buffer_ has been swapped out and *after* the
+  // object tree has been traversed and fixed.
+  virtual void ObserveBufferModification() {}
+
+  // Returns the index'th sub object of this object.
+  // index must be less than NumberOfSubObjects().
+  // This will include objects which are not currently populated but which may
+  // be populated in the future (so that we can track what the necessary offsets
+  // are when we do populate it).
+  virtual SubObject GetSubObject(size_t index) = 0;
+  // Number of sub-objects of this object. May be zero.
+  virtual size_t NumberOfSubObjects() const = 0;
+
+  // Treating the supplied absolute_offset as an offset into the internal memory
+  // buffer, return the pointer to the underlying memory.
+  const void *PointerForAbsoluteOffset(const size_t absolute_offset) {
+    return buffer_.data() + absolute_offset;
+  }
+  // Returns a span at the requested offset into the buffer. terminal_alignment
+  // does not align the start of the buffer; instead, it ensures that the memory
+  // from absolute_offset + size until the next multiple of terminal_alignment
+  // is set to all zeroes.
+  std::span<uint8_t> BufferForObject(size_t absolute_offset, size_t size,
+                                     size_t terminal_alignment);
+  // When memory has been inserted/removed, this iterates over the sub-objects
+  // and notifies/adjusts them appropriately.
+  // This will be called after buffer_ has been updated, and:
+  // * For insertion, modification_point will point into the new buffer_ to the
+  //   first of the newly inserted bytes.
+  // * Removal is not entirely implemented yet, but for removal,
+  //   modification_point should point to the first byte after the removed
+  //   chunk.
+  void FixObjects(void *modification_point, ssize_t bytes_inserted);
+
+  Allocator *allocator() { return allocator_; }
+
+  std::span<uint8_t> buffer_;
+
+ private:
+  ResizeableObject *parent_ = nullptr;
+  std::unique_ptr<Allocator> owned_allocator_;
+  Allocator *allocator_ = nullptr;
+};
+
+// Interface to represent a memory allocator for use with ResizeableObject.
+class Allocator {
+ public:
+  virtual ~Allocator() {}
+  // Allocates memory of the requested size and alignment. alignment is not
+  // guaranteed.
+  // On failure to allocate the requested size, returns nullopt;
+  // Never returns a partial span.
+  // The span will be initialized to zero upon request.
+  // Once Allocate() has been called once, it may not be called again until
+  // Deallocate() has been called. In order to adjust the size of the buffer,
+  // call InsertBytes() and RemoveBytes().
+  [[nodiscard]] virtual std::optional<std::span<uint8_t>> Allocate(
+      size_t size, size_t alignment_hint, SetZero set_zero) = 0;
+  // Identical to Allocate(), but dies on failure.
+  [[nodiscard]] std::span<uint8_t> AllocateOrDie(size_t size,
+                                                 size_t alignment_hint,
+                                                 SetZero set_zero) {
+    std::optional<std::span<uint8_t>> span =
+        Allocate(size, alignment_hint, set_zero);
+    CHECK(span.has_value()) << ": Failed to allocate " << size << " bytes.";
+    CHECK_EQ(size, span.value().size())
+        << ": Failed to allocate " << size << " bytes.";
+    return span.value();
+  }
+  // Increases the size of the buffer by inserting bytes bytes immediately
+  // before insertion_point.
+  // alignment_hint specifies the alignment of the entire buffer, not of the
+  // inserted bytes.
+  // The returned span may or may not overlap with the original buffer in
+  // memory.
+  // The inserted bytes will be set to zero or uninitialized depending on the
+  // value of SetZero.
+  // insertion_point must be in (or 1 past the end of) the buffer.
+  // Returns nullopt on a failure to allocate the requested bytes.
+  [[nodiscard]] virtual std::optional<std::span<uint8_t>> InsertBytes(
+      void *insertion_point, size_t bytes, size_t alignment_hint,
+      SetZero set_zero) = 0;
+  // Removes the requested span of bytes from the buffer, returning the new
+  // buffer.
+  [[nodiscard]] virtual std::span<uint8_t> RemoveBytes(
+      std::span<uint8_t> remove_bytes) = 0;
+  // Deallocates the currently allocated buffer. The provided buffer must match
+  // the latest version of the buffer.
+  // If Allocate() has been called, Deallocate() must be called prior to
+  // destroying the Allocator.
+  virtual void Deallocate(std::span<uint8_t> buffer) = 0;
+};
+
+// Allocator that uses an std::vector to allow arbitrary-sized allocations.
+// Does not provide any alignment guarantees.
+class VectorAllocator : public Allocator {
+ public:
+  VectorAllocator() {}
+  ~VectorAllocator() {
+    CHECK(buffer_.empty())
+        << ": Must deallocate before destroying the VectorAllocator.";
+  }
+  std::optional<std::span<uint8_t>> Allocate(size_t size, size_t /*alignment*/,
+                                             SetZero set_zero) override;
+  std::optional<std::span<uint8_t>> InsertBytes(void *insertion_point,
+                                                size_t bytes,
+                                                size_t /*alignment*/,
+                                                SetZero /*set_zero*/) override;
+  std::span<uint8_t> RemoveBytes(std::span<uint8_t> remove_bytes) override;
+
+  void Deallocate(std::span<uint8_t>) override {
+    CHECK(!buffer_.empty())
+        << ": Called Deallocate() without a prior allocation.";
+    buffer_.resize(0);
+  }
+
+ private:
+  std::vector<uint8_t> buffer_;
+};
+
+// Allocator that allocates all of its memory within a provided span. To match
+// the behavior of the FlatBufferBuilder, it will start its allocations at the
+// end of the provided span.
+//
+// Attempts to allocate more memory than is present in the provided buffer will
+// fail.
+class SpanAllocator : public Allocator {
+ public:
+  SpanAllocator(std::span<uint8_t> buffer) : buffer_(buffer) {}
+  ~SpanAllocator() {
+    CHECK(!allocated_)
+        << ": Must deallocate before destroying the SpanAllocator.";
+  }
+
+  std::optional<std::span<uint8_t>> Allocate(size_t size, size_t /*alignment*/,
+                                             SetZero set_zero) override;
+
+  std::optional<std::span<uint8_t>> InsertBytes(void *insertion_point,
+                                                size_t bytes,
+                                                size_t /*alignment*/,
+                                                SetZero set_zero) override;
+
+  std::span<uint8_t> RemoveBytes(std::span<uint8_t> remove_bytes) override;
+
+  void Deallocate(std::span<uint8_t>) override;
+
+ private:
+  std::span<uint8_t> buffer_;
+  bool allocated_ = false;
+  size_t allocated_size_ = 0;
+};
+
+namespace internal {
+std::ostream &DebugBytes(std::span<const uint8_t> span, std::ostream &os);
+inline void ClearSpan(std::span<uint8_t> span) {
+  memset(span.data(), 0, span.size());
+}
+// std::span::subspan does not do bounds checking.
+template <typename T>
+inline std::span<T> GetSubSpan(std::span<T> span, size_t offset,
+                               size_t count = std::dynamic_extent) {
+  if (count != std::dynamic_extent) {
+    CHECK_LE(offset + count, span.size());
+  }
+  return span.subspan(offset, count);
+}
+// Normal users should never move any of the special flatbuffer types that we
+// provide. However, they do need to be moveable in order to support the use of
+// resizeable vectors. As such, we make all the move constructors private and
+// friend the TableMover struct. The TableMover struct is then used in places
+// that need to have moveable objects. It should never be used by a user.
+template <typename T>
+struct TableMover {
+  TableMover(std::span<uint8_t> buffer, ResizeableObject *parent)
+      : t(buffer, parent) {}
+  TableMover(std::span<uint8_t> buffer, Allocator *allocator)
+      : t(buffer, allocator) {}
+  TableMover(std::span<uint8_t> buffer, ::std::unique_ptr<Allocator> allocator)
+      : t(buffer, ::std::move(allocator)) {}
+  TableMover(TableMover &&) = default;
+  T t;
+};
+}  // namespace internal
+}  // namespace aos::fbs
+#endif  // AOS_FLATBUFFERS_BASE_H_
diff --git a/aos/flatbuffers/base_test.cc b/aos/flatbuffers/base_test.cc
new file mode 100644
index 0000000..522882d
--- /dev/null
+++ b/aos/flatbuffers/base_test.cc
@@ -0,0 +1,254 @@
+#include "aos/flatbuffers/base.h"
+
+#include "gtest/gtest.h"
+
+namespace aos::fbs::testing {
+// Tests that PaddedSize() behaves as expected.
+TEST(BaseTest, PaddedSize) {
+  EXPECT_EQ(0, PaddedSize(0, 4));
+  EXPECT_EQ(4, PaddedSize(4, 4));
+  EXPECT_EQ(8, PaddedSize(5, 4));
+  EXPECT_EQ(8, PaddedSize(6, 4));
+  EXPECT_EQ(8, PaddedSize(7, 4));
+}
+
+inline constexpr size_t kDefaultSize = 16;
+template <typename T>
+class AllocatorTest : public ::testing::Test {
+ protected:
+  AllocatorTest() : allocator_(std::make_unique<T>()) {}
+  std::vector<uint8_t> buffer_;
+  // unique_ptr so that we can destroy the allocator at will.
+  std::unique_ptr<T> allocator_;
+};
+
+template <>
+AllocatorTest<SpanAllocator>::AllocatorTest()
+    : buffer_(kDefaultSize),
+      allocator_(std::make_unique<SpanAllocator>(
+          std::span<uint8_t>{buffer_.data(), buffer_.size()})) {}
+
+using AllocatorTypes = ::testing::Types<SpanAllocator, VectorAllocator>;
+TYPED_TEST_SUITE(AllocatorTest, AllocatorTypes);
+
+// Tests that we can create and not use a VectorAllocator.
+TYPED_TEST(AllocatorTest, UnusedAllocator) {}
+
+// Tests that a simple allocate works.
+TYPED_TEST(AllocatorTest, BasicAllocate) {
+  std::span<uint8_t> span =
+      this->allocator_->Allocate(kDefaultSize, 4, SetZero::kYes).value();
+  ASSERT_EQ(kDefaultSize, span.size());
+  // We set SetZero::kYes; it should be zero-initialized.
+  EXPECT_EQ(kDefaultSize, std::count(span.begin(), span.end(), 0));
+  this->allocator_->Deallocate(span);
+}
+
+// Tests that we can insert bytes into an arbitrary spot in the buffer.
+TYPED_TEST(AllocatorTest, InsertBytes) {
+  const size_t half_size = kDefaultSize / 2;
+  std::span<uint8_t> span =
+      this->allocator_->Allocate(half_size, 4, SetZero::kYes).value();
+  ASSERT_EQ(half_size, span.size());
+  // Set the span with some sentinel values so that we can detect that the
+  // insertion occurred correctly.
+  for (size_t ii = 0; ii < span.size(); ++ii) {
+    span[ii] = ii + 1;
+  }
+
+  // Insert new bytes such that one old byte will still be at the start.
+  span = this->allocator_
+             ->InsertBytes(span.data() + 1u, half_size, 0, SetZero::kYes)
+             .value();
+  ASSERT_EQ(kDefaultSize, span.size());
+  size_t index = 0;
+  EXPECT_EQ(1u, span[index]);
+  index++;
+  for (; index < half_size + 1u; ++index) {
+    EXPECT_EQ(0u, span[index]);
+  }
+  for (; index < span.size(); ++index) {
+    EXPECT_EQ(index - half_size + 1, span[index]);
+  }
+  this->allocator_->Deallocate(span);
+}
+
+// Tests that we can remove bytes from an arbitrary spot in the buffer.
+TYPED_TEST(AllocatorTest, RemoveBytes) {
+  const size_t half_size = kDefaultSize / 2;
+  std::span<uint8_t> span =
+      this->allocator_->Allocate(kDefaultSize, 4, SetZero::kYes).value();
+  ASSERT_EQ(kDefaultSize, span.size());
+  // Set the span with some sentinel values so that we can detect that the
+  // removal occurred correctly.
+  for (size_t ii = 0; ii < span.size(); ++ii) {
+    span[ii] = ii + 1;
+  }
+
+  // Remove bytes such that one old byte will remain at the start, and a chunk
+  // of 8 bytes will be cut out after that..
+  span = this->allocator_->RemoveBytes(span.subspan(1, half_size));
+  ASSERT_EQ(half_size, span.size());
+  size_t index = 0;
+  EXPECT_EQ(1u, span[index]);
+  index++;
+  for (; index < span.size(); ++index) {
+    EXPECT_EQ(index + half_size + 1, span[index]);
+  }
+  this->allocator_->Deallocate(span);
+}
+
+// Tests that if we fail to deallocate that we fail during destruction.
+TYPED_TEST(AllocatorTest, NoDeallocate) {
+  EXPECT_DEATH(
+      {
+        EXPECT_EQ(
+            4, this->allocator_->Allocate(4, 4, SetZero::kYes).value().size());
+        this->allocator_.reset();
+      },
+      "Must deallocate");
+}
+
+// Tests that if we never allocate that we cannot deallocate.
+TYPED_TEST(AllocatorTest, NoAllocateThenDeallocate) {
+  EXPECT_DEATH(this->allocator_->Deallocate(std::span<uint8_t>()),
+               "prior allocation");
+}
+
+// Tests that if we attempt to allocate more than the backing span allows that
+// we correctly return an empty span.
+TEST(SpanAllocatorTest, OverAllocate) {
+  std::vector<uint8_t> buffer(kDefaultSize);
+  SpanAllocator allocator({buffer.data(), buffer.size()});
+  EXPECT_FALSE(
+      allocator.Allocate(kDefaultSize + 1u, 0, SetZero::kYes).has_value());
+}
+
+// Tests that if we attempt to insert more than the backing span allows that
+// we correctly return an empty span.
+TEST(SpanAllocatorTest, OverInsert) {
+  std::vector<uint8_t> buffer(kDefaultSize);
+  SpanAllocator allocator({buffer.data(), buffer.size()});
+  std::span<uint8_t> span =
+      allocator.Allocate(kDefaultSize, 0, SetZero::kYes).value();
+  EXPECT_EQ(kDefaultSize, span.size());
+  EXPECT_FALSE(
+      allocator.InsertBytes(span.data(), 1u, 0, SetZero::kYes).has_value());
+  allocator.Deallocate(span);
+}
+
+// Because we really aren't meant to instantiate ResizeableObject's directly (if
+// nothing else it has virtual member functions), define a testing
+// implementation.
+
+class TestResizeableObject : public ResizeableObject {
+ public:
+  TestResizeableObject(std::span<uint8_t> buffer, ResizeableObject *parent)
+      : ResizeableObject(buffer, parent) {}
+  TestResizeableObject(std::span<uint8_t> buffer, Allocator *allocator)
+      : ResizeableObject(buffer, allocator) {}
+  virtual ~TestResizeableObject() {}
+  using ResizeableObject::SubObject;
+  bool InsertBytes(void *insertion_point, size_t bytes) {
+    return ResizeableObject::InsertBytes(insertion_point, bytes, SetZero::kYes);
+  }
+  TestResizeableObject(TestResizeableObject &&) = default;
+
+  struct TestObject {
+    uoffset_t inline_entry_offset;
+    std::unique_ptr<TestResizeableObject> object;
+    size_t absolute_offset;
+  };
+
+  // Adds a new object of the requested size.
+  void AddEntry(uoffset_t inline_entry_offset, size_t absolute_offset,
+                size_t buffer_size, bool set_object) {
+    *reinterpret_cast<uoffset_t *>(buffer_.data() + inline_entry_offset) =
+        set_object ? (absolute_offset - inline_entry_offset) : 0;
+    objects_.emplace_back(
+        TestObject{inline_entry_offset, nullptr, absolute_offset});
+    if (set_object) {
+      objects_.back().object = std::make_unique<TestResizeableObject>(
+          buffer().subspan(absolute_offset, buffer_size), this);
+    }
+  }
+
+  size_t NumberOfSubObjects() const override { return objects_.size(); }
+  SubObject GetSubObject(size_t index) override {
+    TestObject &subobject = objects_.at(index);
+    return {reinterpret_cast<uoffset_t *>(buffer_.data() +
+                                          subobject.inline_entry_offset),
+            subobject.object.get(), &subobject.absolute_offset};
+  }
+
+  TestObject &GetObject(size_t index) { return objects_.at(index); }
+
+  size_t Alignment() const override { return 64; }
+  size_t AbsoluteOffsetOffset() const override { return 0; }
+
+ private:
+  std::vector<TestObject> objects_;
+};
+
+class ResizeableObjectTest : public ::testing::Test {
+ protected:
+  static constexpr size_t kInitialSize = 128;
+  ResizeableObjectTest()
+      : object_(allocator_.Allocate(kInitialSize, 4, SetZero::kYes).value(),
+                &allocator_) {}
+  ~ResizeableObjectTest() { allocator_.Deallocate(object_.buffer()); }
+  VectorAllocator allocator_;
+  TestResizeableObject object_;
+};
+
+// Tests that if we created an object and then do nothing with it that nothing
+// untoward happens.
+TEST_F(ResizeableObjectTest, DoNothing) {}
+
+// Test that when we move the ResizeableObject we clear the reference to the old
+// buffer.
+TEST_F(ResizeableObjectTest, Move) {
+  TestResizeableObject target_object = std::move(object_);
+  ASSERT_EQ(0u, object_.buffer().size());
+  ASSERT_EQ(kInitialSize, target_object.buffer().size());
+}
+
+// Tests the pathways for resizing a nested ResizeableObject works.
+TEST_F(ResizeableObjectTest, ResizeNested) {
+  constexpr size_t kAbsoluteOffset = 64;
+  object_.AddEntry(4, kAbsoluteOffset, 64, true);
+  TestResizeableObject *subobject = object_.GetObject(0).object.get();
+  object_.AddEntry(0, kAbsoluteOffset, 64, false);
+  EXPECT_EQ(60, *object_.GetSubObject(0).inline_entry);
+  EXPECT_EQ(0, *object_.GetSubObject(1).inline_entry);
+  EXPECT_EQ(64, object_.GetObject(0).object->buffer().data() -
+                    object_.buffer().data());
+
+  constexpr size_t kInsertBytes = 5;
+  // The insert should succeed.
+  ASSERT_TRUE(
+      subobject->InsertBytes(subobject->buffer().data() + 1u, kInsertBytes));
+  // We should now observe the size of the buffers increasing, but the start
+  // _not_ moving.
+  // We should've rounded the insert up to the alignment we areusing (64 bytes).
+  EXPECT_EQ(kInitialSize + 64, object_.buffer().size());
+  EXPECT_EQ(128, subobject->buffer().size());
+  EXPECT_EQ(60, *object_.GetSubObject(0).inline_entry);
+  EXPECT_EQ(0, *object_.GetSubObject(1).inline_entry);
+  EXPECT_EQ(kAbsoluteOffset, object_.GetObject(0).absolute_offset);
+  EXPECT_EQ(kAbsoluteOffset, object_.GetObject(1).absolute_offset);
+
+  // And next we insert before the subobjects, so that we can see their offsets
+  // shift. The insert should succeed.
+  ASSERT_TRUE(object_.InsertBytes(subobject->buffer().data(), kInsertBytes));
+  EXPECT_EQ(kInitialSize + 2 * 64, object_.buffer().size());
+  EXPECT_EQ(128, subobject->buffer().size());
+  EXPECT_EQ(60 + 64, *object_.GetSubObject(0).inline_entry);
+  // The unpopulated object's inline entry should not have changed since
+  // it was zero.
+  EXPECT_EQ(0, *object_.GetSubObject(1).inline_entry);
+  EXPECT_EQ(kAbsoluteOffset + 64, object_.GetObject(0).absolute_offset);
+  EXPECT_EQ(kAbsoluteOffset + 64, object_.GetObject(1).absolute_offset);
+}
+
+}  // namespace aos::fbs::testing
diff --git a/aos/flatbuffers/builder.h b/aos/flatbuffers/builder.h
new file mode 100644
index 0000000..db89d10
--- /dev/null
+++ b/aos/flatbuffers/builder.h
@@ -0,0 +1,142 @@
+#ifndef AOS_FLATBUFFERS_BUILDER_H_
+#define AOS_FLATBUFFERS_BUILDER_H_
+#include "aos/flatbuffers.h"
+#include "aos/flatbuffers/static_table.h"
+namespace aos::fbs {
+
+// Builder class to handle the memory for a static flatbuffer object. This
+// fulfills a similar role to the FlatBufferBuilder type in the traditional API.
+// Typical usage:
+//  aos::fbs::VectorAllocator allocator;
+//  Builder<TestTableStatic> builder(&allocator);
+//  TestTableStatic *object = builder.get();
+//  object->set_scalar(123);
+//
+// At all points you will have a valid and complete flatbuffer, so you never
+// need to call Finish() or anything. You can just directly use the flatbuffer
+// as if it is a real flatbuffer.
+template <typename T>
+class Builder final : public ResizeableObject {
+ public:
+  static constexpr size_t kBufferSize = T::kUnalignedBufferSize;
+  // Note on memory initialization: We zero-initialize all the memory that we
+  // create at the start. While this can be overkill, it is simpler to manage
+  // the alternatives, and we don't currently have a clear performance need for
+  // doing this more piecemeal. Note that the memory that this zero-initializes
+  // falls into three categories:
+  // 1. Padding that we need to zero-initialize (arguably we could get away
+  //    without initializing our padding, but leaving uninitialized memory
+  //    floating around generally isn't great, especially since we generally
+  //    want to be able to compress flatbuffer messages efficiently).
+  // 2. Memory that will end up getting used.
+  // 3. Memory corresponding to sub-tables/vectors that may or may not end up
+  //    getting used; if it is not used, we want it to get zero-initialized
+  //    since it will effectively be padding. If it is used, then the
+  //    zero-initialization is redundant.
+  // For messages with large byte buffers (e.g., for camera messages), we
+  // typically expect that the user will end up dynamically resizing the buffer
+  // rather than having the length statically set. In those cases, the user
+  // still has the ability to select whether or not the new memory gets
+  // zero-initialized.
+  Builder(Allocator *allocator)
+      : ResizeableObject(
+            allocator->AllocateOrDie(kBufferSize, T::kAlign, SetZero::kYes),
+            allocator),
+        flatbuffer_start_(BufferStart(buffer_)),
+        flatbuffer_(internal::GetSubSpan(buffer_, flatbuffer_start_, T::kSize),
+                    this) {
+    SetPrefix();
+  }
+  Builder(std::unique_ptr<Allocator> allocator =
+              std::make_unique<VectorAllocator>())
+      : ResizeableObject(
+            allocator->AllocateOrDie(kBufferSize, T::kAlign, SetZero::kYes),
+            std::move(allocator)),
+        flatbuffer_start_(BufferStart(buffer_)),
+        flatbuffer_(internal::GetSubSpan(buffer_, flatbuffer_start_, T::kSize),
+                    this) {
+    SetPrefix();
+  }
+  Builder(Builder &&other)
+      : ResizeableObject(std::move(other)),
+        flatbuffer_(std::move(other.flatbuffer_)) {
+    flatbuffer_start_ = other.flatbuffer_start_;
+    other.flatbuffer_start_ = 0;
+  }
+
+  ~Builder() {
+    if (allocator() != nullptr) {
+      allocator()->Deallocate(buffer_);
+    }
+  }
+
+  // Returns an object containing the current raw flatbuffer type. Note that if
+  // the allocator type allows changes to the structure/amount of allocated
+  // memory, the underlying buffer will not be stable and so the returned
+  // FlatbufferSpan may be invalidated by mutations to the flatbuffer.
+  FlatbufferSpan<typename T::Flatbuffer> AsFlatbufferSpan() {
+    return {buffer()};
+  }
+
+  // Returns true if the flatbuffer is validly constructed. Should always return
+  // true (barring some sort of memory corruption). Exposed for convenience.
+  bool Verify() { return AsFlatbufferSpan().Verify(); }
+
+  // Returns the actual object for you to operate on and construct the
+  // flatbuffer. Unlike AsFlatbufferSpan(), this will be stable.
+  T *get() { return &flatbuffer_.t; }
+  T &operator*() { return *get(); }
+  T *operator->() { return get(); }
+
+ private:
+  size_t Alignment() const override { return flatbuffer_.t.Alignment(); }
+  size_t AbsoluteOffsetOffset() const override { return 0; }
+  size_t NumberOfSubObjects() const override { return 1; }
+  void SetPrefix() {
+    // We can't do much if the provided buffer isn't at least 4-byte aligned,
+    // because we are required to put the root table offset at the start of the
+    // buffer.
+    CHECK_EQ(reinterpret_cast<size_t>(buffer_.data()) % alignof(uoffset_t), 0u);
+    *reinterpret_cast<uoffset_t *>(buffer_.data()) = flatbuffer_start_;
+  }
+  // Because the allocator API doesn't provide a way for us to request a
+  // strictly aligned buffer, manually align the start of the actual flatbuffer
+  // data if needed.
+  static size_t BufferStart(std::span<uint8_t> buffer) {
+    return aos::fbs::PaddedSize(
+               reinterpret_cast<size_t>(buffer.data()) + sizeof(uoffset_t),
+               T::kAlign) -
+           reinterpret_cast<size_t>(buffer.data());
+  }
+
+  // Some allocators don't do a great job of supporting arbitrary alignments; if
+  // the alignment of the buffer changes, we need to reshuffle everything to
+  // continue guaranteeing alignment.
+  void ObserveBufferModification() override {
+    const size_t new_start = BufferStart(buffer_);
+    if (new_start != flatbuffer_start_) {
+      const size_t used_size = flatbuffer_.t.buffer().size();
+      CHECK_LT(flatbuffer_start_ + used_size, buffer_.size());
+      CHECK_LT(new_start + used_size, buffer_.size());
+      memmove(buffer_.data() + new_start, buffer_.data() + flatbuffer_start_,
+              used_size);
+      flatbuffer_.t.UpdateBuffer(
+          internal::GetSubSpan(buffer_, new_start, used_size),
+          buffer_.data() + new_start, 0);
+      flatbuffer_start_ = new_start;
+      SetPrefix();
+    }
+  }
+  using ResizeableObject::SubObject;
+  SubObject GetSubObject(size_t index) override {
+    CHECK_EQ(0u, index);
+    return {reinterpret_cast<uoffset_t *>(buffer_.data()), &flatbuffer_.t,
+            &flatbuffer_start_};
+  }
+  // Offset from the start of the buffer to the actual start of the flatbuffer
+  // (identical to the root offset of the flatbuffer).
+  size_t flatbuffer_start_;
+  internal::TableMover<T> flatbuffer_;
+};
+}  // namespace aos::fbs
+#endif  // AOS_FLATBUFFERS_BUILDER_H_
diff --git a/aos/flatbuffers/generate.bzl b/aos/flatbuffers/generate.bzl
new file mode 100644
index 0000000..288b281
--- /dev/null
+++ b/aos/flatbuffers/generate.bzl
@@ -0,0 +1,58 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@aspect_bazel_lib//lib:run_binary.bzl", "run_binary")
+
+def static_flatbuffer(name, visibility = None, deps = [], srcs = [], **kwargs):
+    """Generates the code for the static C++ flatbuffer API for the specified fbs file.
+
+    Generates a cc_library of name name that can be depended on by C++ code and other
+    static_flatbuffer rules.
+
+    The cc_library will consist of a single file suffixed with _static.h and prefixed
+    with the name of the flatbuffer file itself (i.e., if you have a src of foo.fbs, then
+    the resulting header will be foo_static.h).
+
+    Args:
+      name: Target name.
+      srcs: List of fbs files to codegen.
+      visibility: Desired rule visibility.
+      deps: List of static_flatbuffer dependencies of this rule.
+    """
+    fbs_suffix = "_fbs"
+
+    flatbuffer_cc_library(
+        name = name + fbs_suffix,
+        srcs = srcs,
+        deps = [dep + fbs_suffix for dep in deps],
+        gen_reflections = True,
+        visibility = visibility,
+        **kwargs
+    )
+
+    # Until we make this a proper rule with providers or the such, we just manage headers
+    # by having a strong convention where the header will be a function of the fbs name
+    # rather than a function of the rule name.
+    header_names = [file.removesuffix(".fbs") + "_static.h" for file in srcs]
+    reflection_out = name + fbs_suffix + "_reflection_out"
+
+    run_binary(
+        name = name + "_gen",
+        tool = "@org_frc971//aos/flatbuffers:generate_wrapper",
+        srcs = [reflection_out],
+        outs = header_names,
+        env = {
+            "BFBS_FILES": "$(execpaths %s)" % (reflection_out,),
+            "BASE_FILES": " ".join(srcs),
+            "OUT_FILES": " ".join(["$(execpath %s)" % (name,) for name in header_names]),
+        },
+    )
+    native.cc_library(
+        name = name,
+        hdrs = header_names,
+        deps = ["@org_frc971//aos/flatbuffers:static_table", "@org_frc971//aos/flatbuffers:static_vector", name + fbs_suffix] + deps,
+        visibility = visibility,
+    )
+    native.alias(
+        name = name + "_reflection_out",
+        actual = name + fbs_suffix + "_reflection_out",
+        visibility = visibility,
+    )
diff --git a/aos/flatbuffers/generate.cc b/aos/flatbuffers/generate.cc
new file mode 100644
index 0000000..030de35
--- /dev/null
+++ b/aos/flatbuffers/generate.cc
@@ -0,0 +1,28 @@
+#include "flatbuffers/reflection_generated.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/flatbuffers/static_flatbuffers.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+
+DEFINE_string(reflection_bfbs, "", "Path to the .bfbs reflection file.");
+DEFINE_string(output_file, "", "Path to the output header to write.");
+DEFINE_string(base_file_name, "",
+              "Name of the base file to generate code for as used by the "
+              "reflection::Schema object.");
+
+namespace aos::fbs {
+int Main() {
+  aos::FlatbufferVector<reflection::Schema> schema =
+      aos::FileToFlatbuffer<reflection::Schema>(FLAGS_reflection_bfbs);
+  aos::util::WriteStringToFileOrDie(
+      FLAGS_output_file,
+      GenerateCodeForRootTableFile(&schema.message(), FLAGS_base_file_name));
+  return EXIT_SUCCESS;
+}
+}  // namespace aos::fbs
+int main(int argc, char *argv[]) {
+  aos::InitGoogle(&argc, &argv);
+  return aos::fbs::Main();
+}
diff --git a/aos/flatbuffers/generate.sh b/aos/flatbuffers/generate.sh
new file mode 100755
index 0000000..09bb4dc
--- /dev/null
+++ b/aos/flatbuffers/generate.sh
@@ -0,0 +1,30 @@
+#!/bin/bash
+
+# Wrapper script to handle codegen for the static flatbuffer API. The actual
+# work is done in calling the generate C++ script, but we also clang-format
+# the output headers so that they are not completely unintelligible.
+
+# --- begin runfiles.bash initialization v2 ---
+# Copy-pasted from the Bazel Bash runfiles library v2.
+set -uo pipefail; f=bazel_tools/tools/bash/runfiles/runfiles.bash
+source "${RUNFILES_DIR:-/dev/null}/$f" 2>/dev/null || \
+  source "$(grep -sm1 "^$f " "${RUNFILES_MANIFEST_FILE:-/dev/null}" | cut -f2- -d' ')" 2>/dev/null || \
+  source "$0.runfiles/$f" 2>/dev/null || \
+  source "$(grep -sm1 "^$f " "$0.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+  source "$(grep -sm1 "^$f " "$0.exe.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+  { echo>&2 "ERROR: cannot find $f"; exit 1; }; f=; set -e
+# --- end runfiles.bash initialization v2 ---
+
+INPUTS=($BFBS_FILES)
+SCHEMA_FILES=($BASE_FILES)
+OUTPUTS=($OUT_FILES)
+
+LEN=${#INPUTS[@]}
+
+for ((i = 0; i < $LEN; i++)); do
+  $(rlocation org_frc971/aos/flatbuffers/generate) \
+    --reflection_bfbs "${INPUTS[i]}" \
+    --output_file "${OUTPUTS[i]}" \
+    --base_file_name "$(basename ${SCHEMA_FILES[i]})"
+  $(rlocation llvm_k8/bin/clang-format) --style=file:"$(rlocation org_frc971/.clang-format)" -i "${OUTPUTS[i]}"
+done
diff --git a/aos/flatbuffers/interesting_schemas.fbs b/aos/flatbuffers/interesting_schemas.fbs
new file mode 100644
index 0000000..99c64b6
--- /dev/null
+++ b/aos/flatbuffers/interesting_schemas.fbs
@@ -0,0 +1,44 @@
+namespace aos.fbs.testing;
+
+attribute "static_length";
+
+table FirstUnion {
+  foo:int (id: 0);
+}
+
+table SecondUnion {
+  bar:double (id: 0);
+}
+
+union UnionType { FirstUnion, SecondUnion }
+
+table TableWithUnion {
+  union_field:UnionType (id: 1);
+}
+
+table MissingVectorLength {
+  vector_no_length:[int] (id: 0);
+}
+
+table NonIntegerVectorLength {
+  vector_badlength:[int] (id: 0, static_length: "abc");
+}
+
+table NegativeVectorLength {
+  vector_badlength:[int] (id: 0, static_length: -1);
+}
+
+table ZeroVectorLength {
+  vector_badlength:[int] (id: 0, static_length: 0);
+}
+
+table MissingStringLength {
+  string_no_length:string (id: 0);
+}
+
+table MissingSubStringLength {
+  substring_no_length:[string] (id: 0, static_length: 3);
+}
+
+// Arbitrary.
+root_type TableWithUnion;
diff --git a/aos/flatbuffers/static_flatbuffers.cc b/aos/flatbuffers/static_flatbuffers.cc
new file mode 100644
index 0000000..8d8942c
--- /dev/null
+++ b/aos/flatbuffers/static_flatbuffers.cc
@@ -0,0 +1,832 @@
+#include "aos/flatbuffers/static_flatbuffers.h"
+
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_format.h"
+#include "absl/strings/str_join.h"
+#include "absl/strings/str_replace.h"
+#include "glog/logging.h"
+
+#include "aos/flatbuffers/static_table.h"
+#include "aos/json_to_flatbuffer.h"
+namespace aos::fbs {
+namespace {
+// Represents a given field within a type with all of the data that we actually
+// care about.
+struct FieldData {
+  // Field name.
+  std::string name;
+  // Whether it is an inline data type (scalar/struct vs vector/table/string).
+  bool is_inline = true;
+  // Whether this is a struct or not.
+  bool is_struct = false;
+  // Full C++ type of this field.
+  std::string full_type = "";
+  // Full flatbuffer type for this field.
+  // Only specified for Tables.
+  std::optional<std::string> fbs_type = std::nullopt;
+  // Size of this field in the inline field data (i.e., size of the field for
+  // is_inline fields; 4 bytes for the offset for vectors/tables/strings).
+  size_t inline_size = 0u;
+  // Alignment of the inline data.
+  size_t inline_alignment = 0u;
+  // vtable offset of the field.
+  size_t vtable_offset = 0u;
+};
+
+const reflection::Object *GetObject(const reflection::Schema *schema,
+                                    const int index) {
+  return (index == -1) ? schema->root_table() : schema->objects()->Get(index);
+}
+
+// Returns the flatbuffer field attribute with the specified name, if available.
+std::optional<std::string_view> GetAttribute(const reflection::Field *field,
+                                             std::string_view attribute) {
+  if (!field->has_attributes()) {
+    return std::nullopt;
+  }
+  const reflection::KeyValue *kv =
+      field->attributes()->LookupByKey(attribute.data());
+  if (kv == nullptr) {
+    return std::nullopt;
+  }
+  return kv->value()->string_view();
+}
+
+// Returns the implied value of an attribute that specifies a length (i.e., 0 if
+// the attribute is not specified; the integer value otherwise).
+int64_t GetLengthAttributeOrZero(const reflection::Field *field,
+                                 std::string_view attribute) {
+  std::optional<std::string_view> str = GetAttribute(field, attribute);
+  if (!str.has_value()) {
+    return 0;
+  }
+  int64_t value;
+  CHECK(absl::SimpleAtoi(str.value(), &value))
+      << ": Field " << field->name()->string_view()
+      << " must specify a positive integer for the " << attribute
+      << " attribute. Got \"" << str.value() << "\".";
+  CHECK_LE(0, value) << ": Field " << field->name()->string_view()
+                     << " must have a non-negative " << attribute << ".";
+  return value;
+}
+
+const std::string ScalarCppType(const reflection::BaseType type) {
+  switch (type) {
+    case reflection::BaseType::Bool:
+      return "bool";
+    case reflection::BaseType::Byte:
+      return "int8_t";
+    case reflection::BaseType::UByte:
+      return "uint8_t";
+    case reflection::BaseType::Short:
+      return "int16_t";
+    case reflection::BaseType::UShort:
+      return "uint16_t";
+    case reflection::BaseType::Int:
+      return "int32_t";
+    case reflection::BaseType::UInt:
+      return "uint32_t";
+    case reflection::BaseType::Long:
+      return "int64_t";
+    case reflection::BaseType::ULong:
+      return "uint64_t";
+    case reflection::BaseType::Float:
+      return "float";
+    case reflection::BaseType::Double:
+      return "double";
+    case reflection::BaseType::UType:
+    case reflection::BaseType::String:
+    case reflection::BaseType::Vector:
+    case reflection::BaseType::Obj:
+    case reflection::BaseType::None:
+    case reflection::BaseType::Union:
+    case reflection::BaseType::Array:
+    case reflection::BaseType::MaxBaseType:
+      LOG(FATAL) << ": Type " << reflection::EnumNameBaseType(type)
+                 << " not a scalar.";
+  }
+  LOG(FATAL) << "Unreachable";
+}
+
+const std::string FlatbufferNameToCppName(const std::string_view input) {
+  return absl::StrReplaceAll(input, {{".", "::"}});
+}
+
+const std::string AosNameForRawFlatbuffer(const std::string_view base_name) {
+  return absl::StrCat(base_name, "Static");
+}
+
+const std::string IncludePathForFbs(
+    std::string_view fbs_file, std::string_view include_suffix = "static") {
+  fbs_file.remove_suffix(4);
+  return absl::StrCat(fbs_file, "_", include_suffix, ".h");
+}
+
+std::string ScalarOrEnumType(const reflection::Schema *schema,
+                             const reflection::BaseType type, int index) {
+  return (index < 0) ? ScalarCppType(type)
+                     : FlatbufferNameToCppName(
+                           schema->enums()->Get(index)->name()->string_view());
+}
+
+void PopulateTypeData(const reflection::Schema *schema,
+                      const reflection::Field *field_fbs, FieldData *field) {
+  VLOG(1) << aos::FlatbufferToJson(field_fbs);
+  const reflection::Type *type = field_fbs->type();
+  field->inline_size = type->base_size();
+  field->inline_alignment = type->base_size();
+  switch (type->base_type()) {
+    case reflection::BaseType::Bool:
+    case reflection::BaseType::Byte:
+    case reflection::BaseType::UByte:
+    case reflection::BaseType::Short:
+    case reflection::BaseType::UShort:
+    case reflection::BaseType::Int:
+    case reflection::BaseType::UInt:
+    case reflection::BaseType::Long:
+    case reflection::BaseType::ULong:
+    case reflection::BaseType::Float:
+    case reflection::BaseType::Double:
+      // We have a scalar field, so things are relatively
+      // straightforwards.
+      field->is_inline = true;
+      field->is_struct = false;
+      field->full_type =
+          ScalarOrEnumType(schema, type->base_type(), type->index());
+      return;
+    case reflection::BaseType::String: {
+      field->is_inline = false;
+      field->is_struct = false;
+      field->full_type =
+          absl::StrFormat("::aos::fbs::String<%d>",
+                          GetLengthAttributeOrZero(field_fbs, "static_length"));
+      return;
+    }
+    case reflection::BaseType::Vector: {
+      // We need to extract the name of the elements of the vector.
+      std::string element_type;
+      bool elements_are_inline = true;
+      if (type->base_type() == reflection::BaseType::Vector) {
+        switch (type->element()) {
+          case reflection::BaseType::Obj: {
+            const reflection::Object *element_object =
+                GetObject(schema, type->index());
+            element_type =
+                FlatbufferNameToCppName(element_object->name()->string_view());
+            elements_are_inline = element_object->is_struct();
+            if (!element_object->is_struct()) {
+              element_type = AosNameForRawFlatbuffer(element_type);
+              field->fbs_type = element_object->name()->string_view();
+            }
+            break;
+          }
+          case reflection::BaseType::String:
+            element_type =
+                absl::StrFormat("::aos::fbs::String<%d>",
+                                GetLengthAttributeOrZero(
+                                    field_fbs, "static_vector_string_length"));
+            elements_are_inline = false;
+            break;
+          case reflection::BaseType::Vector:
+            LOG(FATAL) << "Vectors of vectors do not exist in flatbuffers.";
+          default:
+            element_type =
+                ScalarOrEnumType(schema, type->element(), type->index());
+        };
+      }
+      field->is_inline = false;
+      field->is_struct = false;
+      field->full_type =
+          absl::StrFormat("::aos::fbs::Vector<%s, %d, %s, %s>", element_type,
+                          GetLengthAttributeOrZero(field_fbs, "static_length"),
+                          elements_are_inline ? "true" : "false",
+                          GetAttribute(field_fbs, "force_align").value_or("0"));
+      return;
+    }
+    case reflection::BaseType::Obj: {
+      const reflection::Object *object = GetObject(schema, type->index());
+      field->is_inline = object->is_struct();
+      field->is_struct = object->is_struct();
+      const std::string flatbuffer_name =
+          FlatbufferNameToCppName(object->name()->string_view());
+      if (field->is_inline) {
+        field->full_type = flatbuffer_name;
+        field->inline_size = object->bytesize();
+        field->inline_alignment = object->minalign();
+      } else {
+        field->fbs_type = object->name()->string_view();
+        field->full_type = AosNameForRawFlatbuffer(flatbuffer_name);
+      }
+      return;
+    }
+    case reflection::BaseType::None:
+    case reflection::BaseType::UType:
+    case reflection::BaseType::Union:
+    case reflection::BaseType::Array:
+    case reflection::BaseType::MaxBaseType:
+      LOG(FATAL) << ": Type " << reflection::EnumNameBaseType(type->base_type())
+                 << " not supported currently.";
+  };
+}
+
+std::string MakeMoveConstructor(std::string_view type_name) {
+  return absl::StrFormat(R"code(
+  // We need to provide a MoveConstructor to allow this table to be
+  // used inside of vectors, but we do not want it readily available to
+  // users. See TableMover for more details.
+  %s(%s &&) = default;
+  friend struct ::aos::fbs::internal::TableMover<%s>;
+  )code",
+                         type_name, type_name, type_name);
+}
+
+std::string MakeConstructor(std::string_view type_name) {
+  const std::string constructor_body =
+      R"code(
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+)code";
+  return absl::StrFormat(R"code(
+  // Constructors for creating a flatbuffer object.
+  // Users should typically use the Builder class to create these objects,
+  // in order to allow it to populate the root table offset.
+
+  // The buffer provided to these constructors should be aligned to kAlign
+  // and kSize in length.
+  // The parent/allocator may not be nullptr.
+  %s(std::span<uint8_t> buffer, ::aos::fbs::ResizeableObject *parent) : Table(buffer, parent) {
+    %s
+  }
+  %s(std::span<uint8_t> buffer, ::aos::fbs::Allocator *allocator) : Table(buffer, allocator) {
+    %s
+  }
+  %s(std::span<uint8_t> buffer, ::std::unique_ptr<::aos::fbs::Allocator> allocator) : Table(buffer, ::std::move(allocator)) {
+    %s
+  }
+)code",
+                         type_name, constructor_body, type_name,
+                         constructor_body, type_name, constructor_body);
+}
+
+std::string MemberName(const FieldData &data) {
+  return absl::StrCat(data.name, "_");
+}
+
+std::string ObjectAbsoluteOffsetName(const FieldData &data) {
+  return absl::StrCat("object_absolute_offset_", data.name);
+}
+
+std::string InlineAbsoluteOffsetName(const FieldData &data) {
+  return absl::StrCat("kInlineAbsoluteOffset_", data.name);
+}
+
+// Generate the clear_* method for the requested field.
+std::string MakeClearer(const FieldData &field) {
+  std::string logical_clearer;
+  if (!field.is_inline) {
+    logical_clearer = MemberName(field) + ".reset();";
+  }
+  return absl::StrFormat(R"code(
+  // Clears the %s field. This will cause has_%s() to return false.
+  void clear_%s() {
+    %s
+    ClearField(%s, %d, %d);
+  }
+  )code",
+                         field.name, field.name, field.name, logical_clearer,
+                         InlineAbsoluteOffsetName(field), field.inline_size,
+                         field.vtable_offset);
+}
+
+// Generate the has_* method for the requested field.
+std::string MakeHaser(const FieldData &field) {
+  return absl::StrFormat(R"code(
+  // Returns true if the %s field is set and can be accessed.
+  bool has_%s() const {
+    return AsFlatbuffer().has_%s();
+  }
+  )code",
+                         field.name, field.name, field.name);
+}
+
+// Generates the accessors for fields which are stored inline in the flatbuffer
+// table (scalars, structs, and enums) .
+std::string MakeInlineAccessors(const FieldData &field,
+                                const size_t inline_absolute_offset) {
+  CHECK_EQ(inline_absolute_offset % field.inline_alignment, 0u)
+      << ": Unaligned field " << field.name << " on " << field.full_type
+      << " with inline offset of " << inline_absolute_offset
+      << " and alignment of " << field.inline_alignment;
+  const std::string setter =
+      absl::StrFormat(R"code(
+  // Sets the %s field, causing it to be populated if it is not already.
+  // This will populate the field even if the specified value is the default.
+  void set_%s(const %s &value) {
+    SetField<%s>(%s, %d, value);
+  }
+  )code",
+                      field.name, field.name, field.full_type, field.full_type,
+                      InlineAbsoluteOffsetName(field), field.vtable_offset);
+  const std::string getters = absl::StrFormat(
+      R"code(
+  // Returns the value of %s if set; nullopt otherwise.
+  std::optional<%s> %s() const {
+    return has_%s() ? std::make_optional(Get<%s>(%s)) : std::nullopt;;
+  }
+  // Returns a pointer to modify the %s field.
+  // The pointer may be invalidated by mutations/movements of the underlying buffer.
+  // Returns nullptr if the field is not set.
+  %s* mutable_%s() {
+    return has_%s() ? MutableGet<%s>(%s) : nullptr;
+  }
+  )code",
+      field.name, field.full_type, field.name, field.name, field.full_type,
+      InlineAbsoluteOffsetName(field), field.name, field.full_type, field.name,
+      field.name, field.full_type, InlineAbsoluteOffsetName(field));
+  const std::string clearer = MakeClearer(field);
+  return setter + getters + clearer + MakeHaser(field);
+}
+
+// Generates the accessors for fields which are not inline fields and have an
+// offset to the actual field content stored inline in the flatbuffer table.
+std::string MakeOffsetDataAccessors(const FieldData &field) {
+  const std::string setter = absl::StrFormat(
+      R"code(
+  // Creates an empty object for the %s field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  %s* add_%s() {
+    CHECK(!%s.has_value());
+    constexpr size_t kVtableIndex = %d;
+    // Construct the *Static object that we will use for managing this subtable.
+    %s.emplace(BufferForObject(%s, %s::kSize, kAlign), this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(%s, kVtableIndex, %s + %s::kOffset - %s);
+    return &%s.value().t;
+  }
+  )code",
+      field.name, field.full_type, field.name, MemberName(field),
+      field.vtable_offset, MemberName(field), ObjectAbsoluteOffsetName(field),
+      field.full_type, InlineAbsoluteOffsetName(field),
+      ObjectAbsoluteOffsetName(field), field.full_type,
+      InlineAbsoluteOffsetName(field), MemberName(field));
+  const std::string getters = absl::StrFormat(
+      R"code(
+  // Returns a pointer to the %s field, if set. nullptr otherwise.
+  const %s* %s() const {
+    return %s.has_value() ? &%s.value().t : nullptr;
+  }
+  %s* mutable_%s() {
+    return %s.has_value() ? &%s.value().t : nullptr;
+  }
+  )code",
+      field.name, field.full_type, field.name, MemberName(field),
+      MemberName(field), field.full_type, field.name, MemberName(field),
+      MemberName(field));
+  return setter + getters + MakeClearer(field) + MakeHaser(field);
+}
+
+std::string MakeAccessors(const FieldData &field,
+                          size_t inline_absolute_offset) {
+  return field.is_inline ? MakeInlineAccessors(field, inline_absolute_offset)
+                         : MakeOffsetDataAccessors(field);
+}
+
+std::string MakeMembers(const FieldData &field,
+                        std::string_view offset_data_absolute_offset,
+                        size_t inline_absolute_offset) {
+  if (field.is_inline) {
+    return absl::StrFormat(
+        R"code(
+    // Offset from the start of the buffer to the inline data for the %s field.
+    static constexpr size_t %s = %d;
+    )code",
+        field.name, InlineAbsoluteOffsetName(field), inline_absolute_offset);
+  } else {
+    return absl::StrFormat(
+        R"code(
+    // Members relating to the %s field.
+    //
+    // *Static object used for managing this subtable. Will be nullopt
+    // when the field is not populated.
+    // We use the TableMover to be able to make this object moveable.
+    std::optional<::aos::fbs::internal::TableMover<%s>> %s;
+    // Offset from the start of the buffer to the start of the actual
+    // data for this field. Will be updated even when the table is not
+    // populated, so that we know where to construct it when requested.
+    size_t %s = %s;
+    // Offset from the start of the buffer to the offset in the inline data for
+    // this field.
+    static constexpr size_t %s = %d;
+    )code",
+        field.name, field.full_type, MemberName(field),
+        ObjectAbsoluteOffsetName(field), offset_data_absolute_offset,
+        InlineAbsoluteOffsetName(field), inline_absolute_offset);
+  }
+}
+
+std::string MakeFullClearer(const std::vector<FieldData> &fields) {
+  std::vector<std::string> clearers;
+  for (const FieldData &field : fields) {
+    clearers.emplace_back(absl::StrFormat("clear_%s();", field.name));
+  }
+  return absl::StrFormat(R"code(
+  // Clears every field of the table, removing any existing state.
+  void Clear() { %s })code",
+                         absl::StrJoin(clearers, "\n"));
+}
+
+std::string MakeCopier(const std::vector<FieldData> &fields) {
+  std::vector<std::string> copiers;
+  for (const FieldData &field : fields) {
+    if (field.is_struct) {
+      copiers.emplace_back(absl::StrFormat(R"code(
+      if (other->has_%s()) {
+        set_%s(*other->%s());
+      }
+      )code",
+                                           field.name, field.name, field.name));
+    } else if (field.is_inline) {
+      copiers.emplace_back(absl::StrFormat(R"code(
+      if (other->has_%s()) {
+        set_%s(other->%s());
+      }
+      )code",
+                                           field.name, field.name, field.name));
+    } else {
+      copiers.emplace_back(absl::StrFormat(R"code(
+      if (other->has_%s()) {
+        if (!CHECK_NOTNULL(add_%s())->FromFlatbuffer(other->%s())) {
+          // Fail if we were unable to copy (e.g., if we tried to copy in a long
+          // vector and do not have the space for it).
+          return false;
+        }
+      }
+      )code",
+                                           field.name, field.name, field.name));
+    }
+  }
+  return absl::StrFormat(
+      R"code(
+  // Copies the contents of the provided flatbuffer into this flatbuffer,
+  // returning true on success.
+  // This is a deep copy, and will call FromFlatbuffer on any constituent
+  // objects.
+  [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+    Clear();
+    %s
+    return true;
+  }
+)code",
+      absl::StrJoin(copiers, "\n"));
+}
+
+std::string MakeSubObjectList(const std::vector<FieldData> &fields) {
+  size_t num_object_fields = 0;
+  std::vector<std::string> object_offsets;
+  std::vector<std::string> objects;
+  std::vector<std::string> inline_offsets;
+  for (const FieldData &field : fields) {
+    if (!field.is_inline) {
+      ++num_object_fields;
+      object_offsets.push_back(
+          absl::StrFormat("&%s", ObjectAbsoluteOffsetName(field)));
+      objects.push_back(absl::StrFormat("&%s->t", MemberName(field)));
+      inline_offsets.push_back(InlineAbsoluteOffsetName(field));
+    }
+  }
+  if (num_object_fields == 0) {
+    return R"code(
+  // This object has no non-inline subobjects, so we don't have to do anything special.
+  size_t NumberOfSubObjects() const final { return 0; }
+  using ::aos::fbs::ResizeableObject::SubObject;
+  SubObject GetSubObject(size_t) final { LOG(FATAL) << "No subobjects."; }
+  )code";
+  }
+  return absl::StrFormat(R"code(
+  size_t NumberOfSubObjects() const final { return %d; }
+  using ::aos::fbs::ResizeableObject::SubObject;
+  SubObject GetSubObject(size_t index) final {
+    SubObject object;
+    // Note: The below arrays are local variables rather than class members to
+    // avoid having to deal with what happens to them if the object is moved.
+
+    // Array of the members that we use for tracking where the buffers for
+    // each subobject belong.
+    // Pointers because these may need to be modified when memory is
+    // inserted into the buffer.
+    const std::array<size_t*, %d> subobject_object_offsets{%s};
+    // Actual subobjects; note that the pointers will be invalid when the
+    // field is not populated.
+    const std::array<::aos::fbs::ResizeableObject*, %d> subobject_objects{%s};
+    // Absolute offsets from the start of the buffer to where the inline
+    // entry is for each table. These offsets do not need to change at
+    // runtime (because memory is never inserted into the start of
+    // a given table), but the offsets pointed to by these offsets
+    // may need to be updated.
+    const std::array<size_t, %d> subobject_inline_offsets{%s};
+    object.inline_entry = MutableGet<::flatbuffers::uoffset_t>(subobject_inline_offsets[index]);
+    object.object = (*object.inline_entry == 0) ? nullptr : subobject_objects[index];
+    object.absolute_offset = subobject_object_offsets[index];
+    return object;
+  }
+  )code",
+                         num_object_fields, num_object_fields,
+                         absl::StrJoin(object_offsets, ", "), num_object_fields,
+                         absl::StrJoin(objects, ", "), num_object_fields,
+                         absl::StrJoin(inline_offsets, ", "));
+}
+
+std::string AlignCppString(const std::string_view expression,
+                           const std::string_view alignment) {
+  return absl::StrFormat("::aos::fbs::PaddedSize(%s, %s)", expression,
+                         alignment);
+}
+
+std::string MakeInclude(std::string_view path, bool system = false) {
+  return absl::StrFormat("#include %s%s%s\n", system ? "<" : "\"", path,
+                         system ? ">" : "\"");
+}
+
+}  // namespace
+GeneratedObject GenerateCodeForObject(const reflection::Schema *schema,
+                                      int object_index) {
+  return GenerateCodeForObject(schema, GetObject(schema, object_index));
+}
+GeneratedObject GenerateCodeForObject(const reflection::Schema *schema,
+                                      const reflection::Object *object) {
+  std::vector<FieldData> fields;
+  for (const reflection::Field *field_fbs : *object->fields()) {
+    if (field_fbs->deprecated()) {
+      // Don't codegen anything for deprecated fields.
+      continue;
+    }
+    FieldData field{.name = field_fbs->name()->str(),
+                    .vtable_offset = field_fbs->offset()};
+    PopulateTypeData(schema, field_fbs, &field);
+    fields.push_back(field);
+  }
+  const size_t nominal_min_align = object->minalign();
+  std::string out_of_line_member_size = "";
+  // inline_absolute_offset tracks the current position of the inline table
+  // contents so that we can assign static offsets to each field.
+  size_t inline_absolute_offset = sizeof(soffset_t);
+  // offset_data_relative_offset tracks the current size of the various
+  // sub-tables/vectors/strings that get stored at the end of the buffer.
+  // For simplicity, the offset data will start at a fully aligned offset
+  // (which may be larger than the soffset_t at the start of the table).
+  // Note that this is a string because it's irritating to actually pipe the
+  // numbers for size/alignment up here, so we just accumulate them here and
+  // then write the expression directly into the C++.
+  std::string offset_data_relative_offset = "0";
+  const std::string offset_data_start_expression =
+      "::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign)";
+  std::string accessors;
+  std::string members;
+  std::set<std::string> includes = {
+      MakeInclude("optional", true),
+      MakeInclude("aos/flatbuffers/static_table.h"),
+      MakeInclude("aos/flatbuffers/static_vector.h")};
+  for (const reflection::SchemaFile *file : *schema->fbs_files()) {
+    includes.insert(
+        MakeInclude(IncludePathForFbs(file->filename()->string_view())));
+    includes.insert(MakeInclude(
+        IncludePathForFbs(file->filename()->string_view(), "generated")));
+    for (const flatbuffers::String *included : *file->included_filenames()) {
+      includes.insert(MakeInclude(IncludePathForFbs(included->string_view())));
+    }
+  }
+  std::vector<std::string> alignments;
+  std::set<std::string> subobject_names;
+  for (const FieldData &field : fields) {
+    inline_absolute_offset =
+        PaddedSize(inline_absolute_offset, field.inline_alignment);
+    if (!field.is_inline) {
+      // All sub-fields will get aligned to the parent alignment. This makes
+      // some book-keeping a bit easier, at the expense of some gratuitous
+      // padding.
+      offset_data_relative_offset =
+          AlignCppString(offset_data_relative_offset, "kAlign");
+      alignments.push_back(field.full_type + "::kAlign");
+    } else {
+      alignments.push_back(std::to_string(field.inline_alignment));
+    }
+    const std::string offset_data_absolute_offset =
+        offset_data_start_expression + " + " + offset_data_relative_offset;
+    accessors += MakeAccessors(field, inline_absolute_offset);
+    members +=
+        MakeMembers(field, offset_data_absolute_offset, inline_absolute_offset);
+
+    inline_absolute_offset += field.inline_size;
+    if (!field.is_inline) {
+      offset_data_relative_offset +=
+          absl::StrFormat(" + %s::kSize", field.full_type);
+    }
+    if (field.fbs_type.has_value()) {
+      // Is this not getting populate for the root schema?
+      subobject_names.insert(field.fbs_type.value());
+    }
+  }
+
+  const std::string alignment = absl::StrCat(
+      "static constexpr size_t kAlign = std::max<size_t>({kMinAlign, ",
+      absl::StrJoin(alignments, ", "), "});\n");
+  const std::string size =
+      absl::StrCat("static constexpr size_t kSize = ",
+                   AlignCppString(offset_data_start_expression + " + " +
+                                      offset_data_relative_offset,
+                                  "kAlign"),
+                   ";");
+  const size_t inline_data_size = inline_absolute_offset;
+  const std::string constants = absl::StrFormat(
+      R"code(
+  // Space taken up by the inline portion of the flatbuffer table data, in bytes.
+  static constexpr size_t kInlineDataSize = %d;
+  // Space taken up by the vtable for this object, in bytes.
+  static constexpr size_t kVtableSize = sizeof(::flatbuffers::voffset_t) * (2 + %d);
+  // Offset from the start of the internal memory buffer to the start of the vtable.
+  static constexpr size_t kVtableStart = ::aos::fbs::PaddedSize(kInlineDataSize, alignof(::flatbuffers::voffset_t));
+  // Required alignment of this object. The buffer that this object gets constructed
+  // into must be aligned to this value.
+  %s
+  // Nominal size of this object, in bytes. The object may grow beyond this size,
+  // but will always start at this size and so the initial buffer must match
+  // this size.
+  %s
+  static_assert(%d <= kAlign, "Flatbuffer schema minalign should not exceed our required alignment.");
+  // Offset from the start of the memory buffer to the start of any out-of-line data (subtables,
+  // vectors, strings).
+  static constexpr size_t kOffsetDataStart = %s;
+  // Size required for a buffer that includes a root table offset at the start.
+  static constexpr size_t kRootSize = ::aos::fbs::PaddedSize(kSize + sizeof(::flatbuffers::uoffset_t), kAlign);
+  // Minimum size required to build this flatbuffer in an entirely unaligned buffer
+  // (including the root table offset). Made to be a multiple of kAlign for convenience.
+  static constexpr size_t kUnalignedBufferSize = kRootSize + kAlign;
+  // Offset at which the table vtable offset occurs. This is only needed for vectors.
+  static constexpr size_t kOffset = 0;
+  // Various overrides to support the Table parent class.
+  size_t FixedVtableOffset() const final { return kVtableStart; }
+  size_t VtableSize() const final { return kVtableSize; }
+  size_t InlineTableSize() const final { return kInlineDataSize; }
+  size_t OffsetDataStart() const final { return kOffsetDataStart; }
+  size_t Alignment() const final { return kAlign; }
+  // Exposes the name of the flatbuffer type to allow interchangeable use
+  // of the Flatbuffer and FlatbufferStatic types in various AOS methods.
+  static const char *GetFullyQualifiedName() { return Flatbuffer::GetFullyQualifiedName(); }
+)code",
+      inline_data_size, object->fields()->size(), alignment, size,
+      nominal_min_align, offset_data_start_expression);
+  const std::string_view fbs_type_name = object->name()->string_view();
+  const std::string type_namespace = FlatbufferNameToCppName(
+      fbs_type_name.substr(0, fbs_type_name.find_last_of(".")));
+  const std::string type_name = AosNameForRawFlatbuffer(
+      fbs_type_name.substr(fbs_type_name.find_last_of(".") + 1));
+  const std::string object_code = absl::StrFormat(
+      R"code(
+namespace %s {
+class %s : public ::aos::fbs::Table {
+  public:
+  // The underlying "raw" flatbuffer type for this type.
+  typedef %s Flatbuffer;
+  // Returns this object as a flatbuffer type. This reference may not be valid
+  // following mutations to the underlying flatbuffer, due to how memory may get
+  // may get moved around.
+  const Flatbuffer &AsFlatbuffer() const { return *GetFlatbuffer<Flatbuffer>(); }
+%s
+%s
+  virtual ~%s() {}
+%s
+%s
+%s
+  private:
+%s
+%s
+%s
+};
+}
+  )code",
+      type_namespace, type_name, FlatbufferNameToCppName(fbs_type_name),
+      constants, MakeConstructor(type_name), type_name, accessors,
+      MakeFullClearer(fields), MakeCopier(fields),
+      MakeMoveConstructor(type_name), members, MakeSubObjectList(fields));
+
+  GeneratedObject result;
+  result.name = fbs_type_name;
+  result.include_declarations = includes;
+  result.code = object_code;
+  result.subobjects = subobject_names;
+  return result;
+}
+
+namespace {
+
+// Generated C++ code for an entire fbs file.
+// This includes all of the actual C++ code that will be written to a file (call
+// GenerateCode() to actually get the desired contents of the file).
+struct GeneratedCode {
+  // Prefix (for include guards).
+  std::string contents_prefix;
+  // Full set of required #include declarations.
+  std::set<std::string> include_declarations;
+  // Ordered list of objects (order is necessary to ensure that any dependencies
+  // between objects are managed correctly).
+  std::vector<GeneratedObject> objects;
+  // Suffix (for include guards).
+  std::string contents_suffix;
+
+  // Combine the above things into the string that actually needs to be written
+  // to a file.
+  std::string GenerateCode() const;
+  // Combines the code for multiple objects into one.
+  static GeneratedCode MergeCode(const std::vector<GeneratedObject> &objects);
+};
+
+std::string GeneratedCode::GenerateCode() const {
+  std::string result =
+      contents_prefix + absl::StrJoin(include_declarations, "");
+  for (const auto &object : objects) {
+    result += object.code;
+  }
+  result += contents_suffix;
+  return result;
+}
+
+GeneratedCode GeneratedCode::MergeCode(
+    const std::vector<GeneratedObject> &objects) {
+  GeneratedCode result;
+  // TODO(james): Should we use #ifdef include guards instead?
+  result.contents_prefix =
+      "#pragma once\n// This is a generated file. Do not modify.\n";
+  // We need to get the ordering of objects correct in order to ensure that
+  // depended-on objects appear before their dependees.
+  // In order to do this, we:
+  // 1) Assume that any objects not in the provided vector must exist in
+  //    #includes and so can be ignored.
+  // 2) Create a list of all the objects we have been provided but which we have
+  //    not yet added to the results vector.
+  // 3) Until said list is empty, we iterate over it and find any object(s)
+  //    which have no dependencies in the list itself, and add them to the
+  //    result.
+  // We aren't going to worry about efficient graph traversal here or anything.
+  // We also don't currently attempt to support circular dependencies.
+  std::map<std::string_view, const GeneratedObject *> remaining_objects;
+  for (const auto &object : objects) {
+    remaining_objects[object.name] = &object;
+  }
+  while (!remaining_objects.empty()) {
+    std::string_view to_remove;
+    for (const auto &pair : remaining_objects) {
+      bool has_dependencies = false;
+      for (const std::string_view subobject : pair.second->subobjects) {
+        if (remaining_objects.contains(subobject)) {
+          has_dependencies = true;
+        }
+      }
+      if (has_dependencies) {
+        continue;
+      }
+      to_remove = pair.first;
+      result.objects.push_back(*pair.second);
+      result.include_declarations.insert(
+          pair.second->include_declarations.begin(),
+          pair.second->include_declarations.end());
+      break;
+    }
+    // In order to support circular dependencies, two main things have to
+    // change:
+    // 1. We have to dynamically allow depopulating table members (rather than
+    //    just supporting dynamically lengthed vectors).
+    // 2. Some of the codegen needs to be tweaked so that we can have the
+    // generated
+    //    C++ classes depend on one another.
+    CHECK(!to_remove.empty())
+        << ": Circular dependencies in flatbuffers schemas are not supported.";
+    CHECK_EQ(1u, remaining_objects.erase(to_remove))
+        << ": Failed to remove " << to_remove;
+  }
+  return result;
+}
+}  // namespace
+
+std::string GenerateCodeForRootTableFile(const reflection::Schema *schema,
+                                         std::string_view file_hint) {
+  const reflection::Object *root_object = GetObject(schema, -1);
+  const std::string_view root_file =
+      (root_object == nullptr) ? file_hint
+                               : root_object->declaration_file()->string_view();
+  std::vector<GeneratedObject> objects;
+  if (root_object != nullptr) {
+    objects.push_back(GenerateCodeForObject(schema, root_object));
+  }
+  for (const reflection::Object *object : *schema->objects()) {
+    if (object->is_struct()) {
+      continue;
+    }
+    if (object->declaration_file()->string_view() == root_file) {
+      objects.push_back(GenerateCodeForObject(schema, object));
+    }
+  }
+  return GeneratedCode::MergeCode(objects).GenerateCode();
+}
+}  // namespace aos::fbs
diff --git a/aos/flatbuffers/static_flatbuffers.h b/aos/flatbuffers/static_flatbuffers.h
new file mode 100644
index 0000000..6a5f800
--- /dev/null
+++ b/aos/flatbuffers/static_flatbuffers.h
@@ -0,0 +1,47 @@
+#ifndef AOS_FLATBUFFERS_STATIC_FLATBUFFERS_H_
+#define AOS_FLATBUFFERS_STATIC_FLATBUFFERS_H_
+#include <map>
+#include <set>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include "flatbuffers/reflection_generated.h"
+namespace aos::fbs {
+
+// Raw C++ code needed to represent a single flatbuffer table.
+// The various strings in this struct represent the actual C++ code that will be
+// used for this object; it is split up into pieces in order to allow us to
+// combine multiple flatbuffer tables into a single generated file (namely,
+// pulling the include declarations out to the top and including a set of
+// dependencies so that we can order the code correctly).
+// Primarily exposed here to allow for testing of intermediate functions.
+struct GeneratedObject {
+  // Fully qualified name of the object, in flatbuffer schema rules (e.g.
+  // aos.examples.Ping).
+  std::string name;
+  // All #include statements required for this object.
+  std::set<std::string> include_declarations;
+  // Fully qualified names of all sub-objects, in flatbuffer schema rules (e.g.
+  // aos.examples.Ping). Used to manage ordering of codegen.
+  std::set<std::string> subobjects;
+  // Actual code specific to this object.
+  std::string code;
+};
+
+// Produces generated code for all flatbuffer tables in the file corresponding
+// to the provided Schema object.
+// file_hint is the name of the file that we should be generating code for; this
+// is used if there is no root table specified for the fbs file so that we can
+// infer which objects to generate code for.
+std::string GenerateCodeForRootTableFile(const reflection::Schema *schema,
+                                         std::string_view file_hint);
+
+// Helper functions to generate the code for individual objects; primarily
+// exposed for testing.
+GeneratedObject GenerateCodeForObject(const reflection::Schema *schema,
+                                      int object_index);
+GeneratedObject GenerateCodeForObject(const reflection::Schema *schema,
+                                      const reflection::Object *object);
+}  // namespace aos::fbs
+#endif  // AOS_FLATBUFFERS_STATIC_FLATBUFFERS_H_
diff --git a/aos/flatbuffers/static_flatbuffers_fuzz_test.cc b/aos/flatbuffers/static_flatbuffers_fuzz_test.cc
new file mode 100644
index 0000000..fb1b3e1
--- /dev/null
+++ b/aos/flatbuffers/static_flatbuffers_fuzz_test.cc
@@ -0,0 +1,212 @@
+#include "absl/strings/str_format.h"
+#include "absl/strings/str_join.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/flatbuffers/builder.h"
+#include "aos/flatbuffers/static_flatbuffers.h"
+#include "aos/flatbuffers/test_dir/type_coverage_static.h"
+#include "aos/flatbuffers/test_static.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/testing/path.h"
+#include "aos/testing/tmpdir.h"
+#include "aos/util/file.h"
+
+namespace aos::fbs::testing {
+
+class StaticFlatbuffersFuzzTest : public ::testing::Test {
+ protected:
+  template <typename T>
+  void VerifyJson(const std::string_view data) {
+    Builder<T> json_builder = aos::JsonToStaticFlatbuffer<T>(data);
+
+    EXPECT_EQ(data, aos::FlatbufferToJson(json_builder.AsFlatbufferSpan(),
+                                          {.multi_line = true}));
+  }
+};
+
+namespace {
+void Combine(const std::span<const std::vector<std::string_view>> &strings,
+             std::function<void(const std::vector<std::string_view> &)> handler,
+             const std::vector<std::string_view> &current_combination) {
+  if (strings.empty()) {
+    handler(current_combination);
+    return;
+  }
+  for (const std::string_view &str : strings.front()) {
+    std::vector<std::string_view> combination = current_combination;
+    combination.push_back(str);
+    Combine(strings.subspan(1), handler, combination);
+  }
+}
+void Combine(
+    const std::vector<std::vector<std::string_view>> &strings,
+    std::function<void(const std::vector<std::string_view> &)> handler) {
+  Combine(std::span<const std::vector<std::string_view>>{strings.data(),
+                                                         strings.size()},
+          handler, {});
+}
+}  // namespace
+
+// Iterate over lots of variations of different flatbuffers to try to see if we
+// can exercise weird corner-cases.
+TEST_F(StaticFlatbuffersFuzzTest, JsonFuzzing) {
+  std::vector<std::vector<std::string_view>> stanzas{
+      {"", "\"scalar\": 1323"},
+      {"", "\"vector_of_scalars\": [\n  \n ]",
+       "\"vector_of_scalars\": [\n  123\n ]",
+       "\"vector_of_scalars\": [\n  123,\n  456\n ]"},
+      {"", "\"string\": \"\"", "\"string\": \"abcdef\"",
+       "\"string\": \"abcdefghijklmnopqrstuvwxyz\""},
+      {
+          "",
+          "\"vector_of_strings\": [\n  \n ]",
+          "\"vector_of_strings\": [\n  \"\",\n  \"abcdef\"\n ]",
+          "\"vector_of_strings\": [\n  \"\",\n  \"abcdef\",\n  "
+          "\"abcdefghijklmnopqrstuvwxyz\"\n ]",
+          "\"vector_of_strings\": [\n  \"\",\n  \"abcdef\",\n  \"971\",\n  "
+          "\"abcdefghijklmnopqrstuvwxyz\"\n ]",
+          "\"vector_of_strings\": [\n  \"\",\n  \"abcdef\",\n  "
+          "\"abcdefghijklmnopqrstuvwxyz\",\n  \"971\",\n  \"123\"\n ]",
+          "\"vector_of_strings\": [\n  \"\",\n  \"abcdef\",\n  \"xyz\",\n  "
+          "\"971\",\n  \"123\"\n ]",
+      },
+      {
+          "",
+          "\"substruct\": {\n  \"x\": 971.0,\n  \"y\": 123.0\n }",
+      },
+      {
+          "",
+          "\"subtable\": {\n\n }",
+          "\"subtable\": {\n  \"baz\": 1.23\n }",
+          "\"subtable\": {\n  \"foo\": 123,\n  \"baz\": 1.23\n }",
+      },
+      {
+          "",
+          "\"vector_aligned\": [\n  \n ]",
+          "\"vector_aligned\": [\n  678\n ]",
+          "\"vector_aligned\": [\n  678,\n  456\n ]",
+          "\"vector_aligned\": [\n  7,\n  6,\n  5,\n  4,\n  3,\n  2,\n  1,\n  "
+          "0\n ]",
+      },
+      {
+          "",
+          "\"vector_of_structs\": [\n  \n ]",
+          R"json("vector_of_structs": [
+  {
+   "x": 1.0,
+   "y": 2.0
+  }
+ ])json",
+          R"json("vector_of_structs": [
+  {
+   "x": 1.0,
+   "y": 2.0
+  },
+  {
+   "x": 3.0,
+   "y": 4.0
+  },
+  {
+   "x": 5.0,
+   "y": 6.0
+  }
+ ])json",
+          R"json("vector_of_structs": [
+  {
+   "x": 1.0,
+   "y": 2.0
+  },
+  {
+   "x": 3.0,
+   "y": 4.0
+  },
+  {
+   "x": 5.0,
+   "y": 6.0
+  },
+  {
+   "x": 7.0,
+   "y": 8.0
+  },
+  {
+   "x": 9.0,
+   "y": 10.0
+  }
+ ])json",
+      },
+      {
+          "",
+          "\"vector_of_tables\": [\n  \n ]",
+          R"json("vector_of_tables": [
+  {
+
+  }
+ ])json",
+          R"json("vector_of_tables": [
+  {
+   "foo": 1
+  }
+ ])json",
+          R"json("vector_of_tables": [
+  {
+   "foo": 1
+  },
+  {
+   "foo": 2
+  },
+  {
+   "foo": 3
+  },
+  {
+   "foo": 4
+  },
+  {
+   "foo": 5
+  },
+  {
+   "foo": 6
+  }
+ ])json",
+      },
+      {
+          "",
+          "\"included_table\": {\n\n }",
+          "\"included_table\": {\n  \"foo\": \"A\"\n }",
+      },
+      {
+          "",
+          "\"unspecified_length_vector\": [\n  \n ]",
+          "\"unspecified_length_vector\": [\n  123\n ]",
+          "\"unspecified_length_vector\": [\n  123,\n  100\n ]",
+      },
+      {
+          "",
+          "\"unspecified_length_string\": \"\"",
+          "\"unspecified_length_string\": \"Hello, World!\"",
+      },
+      {
+          "",
+          "\"unspecified_length_vector_of_strings\": [\n  \n ]",
+          "\"unspecified_length_vector_of_strings\": [\n  \"\"\n ]",
+          "\"unspecified_length_vector_of_strings\": [\n  \"Goodbye, \",\n  "
+          "\"World!\"\n ]",
+      },
+  };
+  Combine(stanzas, [this](const std::vector<std::string_view> &strings) {
+    std::vector<std::string_view> no_empty_strings;
+    for (const std::string_view &str : strings) {
+      if (!str.empty()) {
+        no_empty_strings.push_back(str);
+      }
+    }
+    if (no_empty_strings.empty()) {
+      VerifyJson<TestTableStatic>("{\n\n}");
+    } else {
+      VerifyJson<TestTableStatic>(
+          "{\n " + absl::StrJoin(no_empty_strings, ",\n ") + "\n}");
+    }
+  });
+}
+}  // namespace aos::fbs::testing
diff --git a/aos/flatbuffers/static_flatbuffers_test.cc b/aos/flatbuffers/static_flatbuffers_test.cc
new file mode 100644
index 0000000..2acc3c8
--- /dev/null
+++ b/aos/flatbuffers/static_flatbuffers_test.cc
@@ -0,0 +1,990 @@
+#include "aos/flatbuffers/static_flatbuffers.h"
+
+#include "absl/strings/str_format.h"
+#include "absl/strings/str_join.h"
+#include "external/com_github_google_flatbuffers/src/annotated_binary_text_gen.h"
+#include "external/com_github_google_flatbuffers/src/binary_annotator.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/flatbuffers/builder.h"
+#include "aos/flatbuffers/interesting_schemas.h"
+#include "aos/flatbuffers/test_dir/type_coverage_static.h"
+#include "aos/flatbuffers/test_schema.h"
+#include "aos/flatbuffers/test_static.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/testing/path.h"
+#include "aos/testing/tmpdir.h"
+#include "aos/util/file.h"
+
+namespace aos::fbs::testing {
+
+namespace {
+// Uses the binary schema to annotate a provided flatbuffer.  Returns the
+// annotated flatbuffer.
+std::string AnnotateBinaries(
+    const aos::NonSizePrefixedFlatbuffer<reflection::Schema> &schema,
+    flatbuffers::span<uint8_t> binary_data) {
+  flatbuffers::BinaryAnnotator binary_annotator(
+      schema.span().data(), schema.span().size(), binary_data.data(),
+      binary_data.size());
+
+  auto annotations = binary_annotator.Annotate();
+  const std::string schema_filename =
+      aos::testing::TestTmpDir() + "/schema.bfbs";
+
+  aos::WriteFlatbufferToFile(schema_filename, schema);
+
+  flatbuffers::AnnotatedBinaryTextGenerator text_generator(
+      flatbuffers::AnnotatedBinaryTextGenerator::Options{}, annotations,
+      binary_data.data(), binary_data.size());
+
+  text_generator.Generate(aos::testing::TestTmpDir() + "/foo.bfbs",
+                          schema_filename);
+
+  return aos::util::ReadFileToStringOrDie(aos::testing::TestTmpDir() +
+                                          "/foo.afb");
+}
+const reflection::Object *GetObjectByName(const reflection::Schema *schema,
+                                          std::string_view name) {
+  for (const reflection::Object *object : *schema->objects()) {
+    if (object->name()->string_view() == name) {
+      return object;
+    }
+  }
+  return nullptr;
+}
+
+// Accesses all the values in the supplied span. Used to ensure that memory
+// sanitizers can observe uninitialized memory.
+void TestMemory(std::span<uint8_t> memory) {
+  std::stringstream str;
+  internal::DebugBytes(memory, str);
+  EXPECT_LT(0u, str.view().size());
+}
+}  // namespace
+
+class StaticFlatbuffersTest : public ::testing::Test {
+ protected:
+  template <typename T>
+  void VerifyJson(const std::string_view data) {
+    Builder<T> json_builder = aos::JsonToStaticFlatbuffer<T>(data);
+
+    EXPECT_EQ(data, aos::FlatbufferToJson(json_builder.AsFlatbufferSpan(),
+                                          {.multi_line = true}));
+  }
+  aos::FlatbufferSpan<reflection::Schema> test_schema_{TestTableSchema()};
+  aos::FlatbufferSpan<reflection::Schema> interesting_schemas_{
+      UnsupportedSchema()};
+};
+
+// Test that compiles the same code that is used by an example in
+// //aos/documentation/aos/docs/flatbuffers.md.
+TEST_F(StaticFlatbuffersTest, DocumentationExample) {
+  aos::fbs::VectorAllocator allocator;
+  Builder<TestTableStatic> builder(&allocator);
+  TestTableStatic *object = builder.get();
+  object->set_scalar(123);
+  {
+    auto vector = object->add_vector_of_scalars();
+    CHECK(vector->emplace_back(4));
+    CHECK(vector->emplace_back(5));
+  }
+  {
+    auto string = object->add_string();
+    string->SetString("Hello, World!");
+  }
+  {
+    auto vector_of_strings = object->add_vector_of_strings();
+    auto sub_string = CHECK_NOTNULL(vector_of_strings->emplace_back());
+    CHECK(sub_string->emplace_back('D'));
+  }
+  { object->set_substruct({971, 254}); }
+  {
+    auto subtable = object->add_subtable();
+    subtable->set_foo(1234);
+  }
+  {
+    auto vector = object->add_vector_of_structs();
+    CHECK(vector->emplace_back({48, 67}));
+    CHECK(vector->emplace_back({118, 148}));
+    CHECK(vector->emplace_back({971, 973}));
+    // Max vector size is three; this should fail.
+    CHECK(!vector->emplace_back({1114, 2056}));
+  }
+  {
+    auto vector = object->add_vector_of_tables();
+    auto subobject = vector->emplace_back();
+    subobject->set_foo(222);
+  }
+  {
+    auto subtable = object->add_included_table();
+    subtable->set_foo(included::TestEnum::B);
+  }
+  ASSERT_TRUE(builder.AsFlatbufferSpan().Verify());
+  LOG(INFO) << aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                                     {.multi_line = true});
+  LOG(INFO) << AnnotateBinaries(test_schema_, builder.buffer());
+}
+
+// Test that compiles the same code that is used by an example in
+// //aos/documentation/aos/docs/flatbuffers.md showing how to convert a
+// Populate*() method for adding a subtable to a flatbuffer.
+namespace {
+flatbuffers::Offset<SubTable> PopulateOld(flatbuffers::FlatBufferBuilder *fbb) {
+  SubTable::Builder builder(*fbb);
+  builder.add_foo(1234);
+  return builder.Finish();
+}
+void PopulateStatic(SubTableStatic *subtable) { subtable->set_foo(1234); }
+}  // namespace
+TEST_F(StaticFlatbuffersTest, PopulateMethodConversionExample) {
+  // Using a FlatBufferBuilder:
+  flatbuffers::FlatBufferBuilder fbb;
+  // Note: the PopulateOld() *must* be called prior to creating the builder.
+  const flatbuffers::Offset<SubTable> subtable_offset = PopulateOld(&fbb);
+  TestTable::Builder testtable_builder(fbb);
+  testtable_builder.add_subtable(subtable_offset);
+  fbb.Finish(testtable_builder.Finish());
+  aos::FlatbufferDetachedBuffer<TestTable> fbb_finished = fbb.Release();
+
+  // Using the static flatbuffer API.
+  aos::fbs::VectorAllocator allocator;
+  Builder<TestTableStatic> static_builder(&allocator);
+  PopulateStatic(CHECK_NOTNULL(static_builder.get()->add_subtable()));
+
+  // And confirm that they both contain the expected flatbuffer:
+  const std::string expected = R"json({ "subtable": { "foo": 1234 } })json";
+  EXPECT_EQ(expected, aos::FlatbufferToJson(fbb_finished));
+  EXPECT_EQ(expected, aos::FlatbufferToJson(static_builder.AsFlatbufferSpan()));
+}
+
+TEST_F(StaticFlatbuffersTest, UnsupportedSchema) {
+  const reflection::Schema *schema = &interesting_schemas_.message();
+  EXPECT_DEATH(
+      GenerateCodeForObject(
+          schema, GetObjectByName(schema, "aos.fbs.testing.TableWithUnion")),
+      "Union not supported");
+  GenerateCodeForObject(
+      schema, GetObjectByName(schema, "aos.fbs.testing.MissingVectorLength"));
+  EXPECT_DEATH(
+      GenerateCodeForObject(
+          schema,
+          GetObjectByName(schema, "aos.fbs.testing.NonIntegerVectorLength")),
+      "vector_badlength must specify a positive integer for the "
+      "static_length attribute.");
+  EXPECT_DEATH(GenerateCodeForObject(
+                   schema, GetObjectByName(
+                               schema, "aos.fbs.testing.NegativeVectorLength")),
+               "Field vector_badlength must have a non-negative "
+               "static_length.");
+  GenerateCodeForObject(
+      schema, GetObjectByName(schema, "aos.fbs.testing.ZeroVectorLength"));
+  GenerateCodeForObject(
+      schema, GetObjectByName(schema, "aos.fbs.testing.MissingStringLength"));
+  GenerateCodeForObject(
+      schema,
+      GetObjectByName(schema, "aos.fbs.testing.MissingSubStringLength"));
+}
+
+// Tests that we can go through and manually build up a big flatbuffer and that
+// it stays valid at all points.
+TEST_F(StaticFlatbuffersTest, ManuallyConstructFlatbuffer) {
+  {
+    aos::fbs::VectorAllocator allocator;
+    Builder<SubTableStatic> builder(&allocator);
+    SubTableStatic *object = builder.get();
+    if (!builder.AsFlatbufferSpan().Verify()) {
+      LOG(ERROR) << object->SerializationDebugString() << "\nRoot table offset "
+                 << *reinterpret_cast<const uoffset_t *>(
+                        builder.buffer().data())
+                 << "\nraw bytes\n";
+      aos::fbs::internal::DebugBytes(builder.buffer(), std::cerr);
+      FAIL();
+      return;
+    }
+    EXPECT_EQ("{  }", aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+    object->set_foo(123);
+    object->set_baz(971);
+    CHECK(builder.AsFlatbufferSpan().Verify());
+    EXPECT_EQ(123, object->AsFlatbuffer().foo());
+    EXPECT_EQ(971, object->AsFlatbuffer().baz());
+    EXPECT_EQ(R"json({ "foo": 123, "baz": 971.0 })json",
+              aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+    TestMemory(builder.buffer());
+  }
+  {
+    // aos::FixedAllocator allocator(TestTableStatic::kUnalignedBufferSize);
+    aos::fbs::VectorAllocator allocator;
+    Builder<TestTableStatic> builder(&allocator);
+    TestTableStatic *object = builder.get();
+    const aos::fbs::testing::TestTable &fbs = object->AsFlatbuffer();
+    VLOG(1) << object->SerializationDebugString();
+    CHECK(builder.AsFlatbufferSpan().Verify());
+    EXPECT_EQ("{  }", aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+    {
+      ASSERT_FALSE(object->has_scalar());
+      object->set_scalar(123);
+      EXPECT_TRUE(fbs.has_scalar());
+      EXPECT_EQ(123, fbs.scalar());
+    }
+    EXPECT_EQ(R"json({ "scalar": 123 })json",
+              aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+    {
+      ASSERT_FALSE(object->has_vector_of_scalars());
+      auto vector = object->add_vector_of_scalars();
+      ASSERT_TRUE(vector->emplace_back(4));
+      ASSERT_TRUE(vector->emplace_back(5));
+      ASSERT_TRUE(object->has_vector_of_scalars());
+      ASSERT_TRUE(fbs.has_vector_of_scalars());
+      VLOG(1) << vector->SerializationDebugString();
+      EXPECT_TRUE(fbs.has_vector_of_scalars());
+      EXPECT_EQ(2u, fbs.vector_of_scalars()->size());
+      EXPECT_EQ(4, fbs.vector_of_scalars()->Get(0));
+      EXPECT_EQ(5, fbs.vector_of_scalars()->Get(1));
+    }
+    EXPECT_EQ(R"json({ "scalar": 123, "vector_of_scalars": [ 4, 5 ] })json",
+              aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+    {
+      EXPECT_FALSE(object->has_string());
+      auto string = object->add_string();
+      EXPECT_TRUE(object->has_string());
+      string->SetString("Hello, World!");
+      EXPECT_EQ(13u, object->string()->size());
+      ASSERT_TRUE(fbs.has_string());
+      ASSERT_EQ(13u, fbs.string()->size());
+      EXPECT_EQ("Hello, World!", fbs.string()->string_view());
+      // Check that we null-terminated correctly.
+      EXPECT_EQ(13u, strnlen(fbs.string()->c_str(), 20));
+    }
+    EXPECT_EQ(
+        R"json({ "scalar": 123, "vector_of_scalars": [ 4, 5 ], "string": "Hello, World!" })json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+    {
+      EXPECT_FALSE(object->has_vector_of_strings());
+      auto vector_of_strings = object->add_vector_of_strings();
+      EXPECT_TRUE(object->has_vector_of_strings());
+      auto sub_string = CHECK_NOTNULL(vector_of_strings->emplace_back());
+      ASSERT_TRUE(sub_string->emplace_back('D'));
+      EXPECT_TRUE(fbs.has_vector_of_strings());
+      ASSERT_EQ(1u, fbs.vector_of_strings()->size());
+      ASSERT_EQ(1u, fbs.vector_of_strings()->Get(0)->size());
+      EXPECT_EQ('D', fbs.vector_of_strings()->Get(0)->Get(0));
+    }
+    EXPECT_EQ(
+        R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ]
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    {
+      EXPECT_FALSE(object->has_substruct());
+      object->set_substruct({971, 254});
+      EXPECT_TRUE(object->has_substruct());
+      EXPECT_TRUE(fbs.has_substruct());
+      EXPECT_EQ(971, fbs.substruct()->x());
+      EXPECT_EQ(254, fbs.substruct()->y());
+    }
+    EXPECT_EQ(
+        R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ }
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    {
+      auto subtable = object->add_subtable();
+      subtable->set_foo(1234);
+      EXPECT_TRUE(fbs.has_subtable());
+      EXPECT_EQ(1234, fbs.subtable()->foo());
+      EXPECT_FALSE(fbs.subtable()->has_baz());
+    }
+    EXPECT_EQ(
+        R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ },
+ "subtable": {
+  "foo": 1234
+ }
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    {
+      auto vector = object->add_vector_of_structs();
+      ASSERT_TRUE(vector->emplace_back({48, 67}));
+      ASSERT_TRUE(vector->emplace_back({118, 148}));
+      ASSERT_TRUE(vector->emplace_back({971, 973}));
+      ASSERT_FALSE(vector->emplace_back({1114, 2056}));
+      EXPECT_TRUE(fbs.has_vector_of_structs());
+      EXPECT_EQ(3u, fbs.vector_of_structs()->size());
+      EXPECT_EQ(48, fbs.vector_of_structs()->Get(0)->x());
+      EXPECT_EQ(67, fbs.vector_of_structs()->Get(0)->y());
+      EXPECT_EQ(118, fbs.vector_of_structs()->Get(1)->x());
+      EXPECT_EQ(object->vector_of_structs()->at(1).x(),
+                fbs.vector_of_structs()->Get(1)->x());
+      EXPECT_EQ((*object->vector_of_structs())[1].x(),
+                fbs.vector_of_structs()->Get(1)->x());
+      EXPECT_EQ(148, fbs.vector_of_structs()->Get(1)->y());
+      EXPECT_EQ(971, fbs.vector_of_structs()->Get(2)->x());
+      EXPECT_EQ(973, fbs.vector_of_structs()->Get(2)->y());
+    }
+    EXPECT_EQ(
+        R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ },
+ "subtable": {
+  "foo": 1234
+ },
+ "vector_of_structs": [
+  {
+   "x": 48.0,
+   "y": 67.0
+  },
+  {
+   "x": 118.0,
+   "y": 148.0
+  },
+  {
+   "x": 971.0,
+   "y": 973.0
+  }
+ ]
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    {
+      EXPECT_FALSE(object->has_vector_of_tables());
+      auto vector = object->add_vector_of_tables();
+      EXPECT_TRUE(object->has_vector_of_tables());
+      auto subobject = vector->emplace_back();
+      subobject->set_foo(222);
+      EXPECT_TRUE(fbs.has_vector_of_tables());
+      EXPECT_EQ(1u, fbs.vector_of_tables()->size());
+      EXPECT_EQ(222, fbs.vector_of_tables()->Get(0)->foo());
+      EXPECT_EQ(object->vector_of_tables()->at(0).foo(),
+                fbs.vector_of_tables()->Get(0)->foo());
+    }
+    EXPECT_EQ(
+        R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ },
+ "subtable": {
+  "foo": 1234
+ },
+ "vector_of_structs": [
+  {
+   "x": 48.0,
+   "y": 67.0
+  },
+  {
+   "x": 118.0,
+   "y": 148.0
+  },
+  {
+   "x": 971.0,
+   "y": 973.0
+  }
+ ],
+ "vector_of_tables": [
+  {
+   "foo": 222
+  }
+ ]
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    {
+      EXPECT_FALSE(object->has_included_table());
+      auto subtable = object->add_included_table();
+      EXPECT_TRUE(object->has_included_table());
+      subtable->set_foo(included::TestEnum::B);
+      ASSERT_TRUE(fbs.has_included_table());
+      ASSERT_TRUE(fbs.included_table()->has_foo());
+      EXPECT_EQ(included::TestEnum::B, fbs.included_table()->foo());
+    }
+    EXPECT_EQ(
+        R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ },
+ "subtable": {
+  "foo": 1234
+ },
+ "vector_of_structs": [
+  {
+   "x": 48.0,
+   "y": 67.0
+  },
+  {
+   "x": 118.0,
+   "y": 148.0
+  },
+  {
+   "x": 971.0,
+   "y": 973.0
+  }
+ ],
+ "vector_of_tables": [
+  {
+   "foo": 222
+  }
+ ],
+ "included_table": {
+  "foo": "B"
+ }
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    {
+      auto aligned_vector = object->add_vector_aligned();
+      ASSERT_EQ(64,
+                std::remove_reference<decltype(*aligned_vector)>::type::kAlign);
+      ASSERT_EQ(64, TestTableStatic::kAlign);
+      ASSERT_TRUE(aligned_vector->emplace_back(444));
+      EXPECT_TRUE(fbs.has_vector_aligned());
+      EXPECT_EQ(1u, fbs.vector_aligned()->size());
+      EXPECT_EQ(0u,
+                reinterpret_cast<size_t>(fbs.vector_aligned()->data()) % 64);
+      EXPECT_EQ(444, fbs.vector_aligned()->Get(0));
+    }
+    VLOG(1) << object->SerializationDebugString();
+    CHECK(builder.AsFlatbufferSpan().Verify());
+    const std::string expected_contents =
+        R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ },
+ "subtable": {
+  "foo": 1234
+ },
+ "vector_aligned": [
+  444
+ ],
+ "vector_of_structs": [
+  {
+   "x": 48.0,
+   "y": 67.0
+  },
+  {
+   "x": 118.0,
+   "y": 148.0
+  },
+  {
+   "x": 971.0,
+   "y": 973.0
+  }
+ ],
+ "vector_of_tables": [
+  {
+   "foo": 222
+  }
+ ],
+ "included_table": {
+  "foo": "B"
+ }
+})json";
+    EXPECT_EQ(expected_contents,
+              aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                                    {.multi_line = true}));
+    VLOG(1) << AnnotateBinaries(test_schema_, builder.buffer());
+    VerifyJson<TestTableStatic>(expected_contents);
+    {
+      auto aligned_vector = object->mutable_vector_aligned();
+      ASSERT_TRUE(aligned_vector->reserve(100));
+      EXPECT_EQ(100, aligned_vector->capacity());
+      ASSERT_TRUE(builder.AsFlatbufferSpan().Verify())
+          << aligned_vector->SerializationDebugString();
+      EXPECT_EQ(expected_contents,
+                aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                                      {.multi_line = true}));
+      std::vector<int> scalars;
+      scalars.push_back(aligned_vector->at(0));
+      while (aligned_vector->size() < 100u) {
+        scalars.push_back(aligned_vector->size());
+        CHECK(aligned_vector->emplace_back(aligned_vector->size()));
+      }
+      VLOG(1) << aligned_vector->SerializationDebugString();
+      VLOG(1) << AnnotateBinaries(test_schema_, builder.buffer());
+      EXPECT_EQ(absl::StrFormat(
+                    R"json({
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ },
+ "subtable": {
+  "foo": 1234
+ },
+ "vector_aligned": [
+  %s
+ ],
+ "vector_of_structs": [
+  {
+   "x": 48.0,
+   "y": 67.0
+  },
+  {
+   "x": 118.0,
+   "y": 148.0
+  },
+  {
+   "x": 971.0,
+   "y": 973.0
+  }
+ ],
+ "vector_of_tables": [
+  {
+   "foo": 222
+  }
+ ],
+ "included_table": {
+  "foo": "B"
+ }
+})json",
+                    absl::StrJoin(scalars, ",\n  ")),
+                aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                                      {.multi_line = true}));
+    }
+
+    {
+      auto unspecified_vector = object->add_unspecified_length_vector();
+      ASSERT_NE(nullptr, unspecified_vector);
+      ASSERT_EQ(0, unspecified_vector->capacity());
+      ASSERT_FALSE(unspecified_vector->emplace_back(0));
+      ASSERT_TRUE(unspecified_vector->reserve(2));
+      ASSERT_TRUE(unspecified_vector->emplace_back(1));
+      ASSERT_TRUE(unspecified_vector->emplace_back(2));
+      ASSERT_FALSE(unspecified_vector->emplace_back(3));
+      ASSERT_TRUE(builder.AsFlatbufferSpan().Verify());
+    }
+    TestMemory(builder.buffer());
+  }
+}
+
+// Tests that field clearing (and subsequent resetting) works properly.
+TEST_F(StaticFlatbuffersTest, ClearFields) {
+  aos::fbs::VectorAllocator allocator;
+  Builder<TestTableStatic> builder(&allocator);
+  TestTableStatic *object = builder.get();
+  // For each field, we will confirm the following:
+  // * Clearing a non-existent field causes no issues.
+  // * We can set a field, clear it, and have it not be present.
+  // * We can set the field again afterwards.
+  {
+    object->clear_scalar();
+    ASSERT_TRUE(builder.Verify());
+    object->set_scalar(123);
+    EXPECT_EQ(123, object->AsFlatbuffer().scalar());
+    object->clear_scalar();
+    ASSERT_TRUE(builder.Verify());
+    EXPECT_FALSE(object->has_scalar());
+    object->set_scalar(456);
+    EXPECT_EQ(456, object->AsFlatbuffer().scalar());
+  }
+  {
+    object->clear_vector_of_scalars();
+    ASSERT_TRUE(builder.Verify());
+    EXPECT_FALSE(object->has_vector_of_scalars());
+    auto vector = object->add_vector_of_scalars();
+    ASSERT_TRUE(vector->emplace_back(4));
+    ASSERT_TRUE(vector->emplace_back(5));
+    ASSERT_TRUE(vector->emplace_back(6));
+    // Deliberately force a resize of the vector to ensure that we can exercise
+    // what happens if we clear a non-standard size field.
+    ASSERT_FALSE(vector->emplace_back(7));
+    ASSERT_TRUE(vector->reserve(4));
+    ASSERT_TRUE(vector->emplace_back(7));
+    EXPECT_EQ(
+        R"json({
+ "scalar": 456,
+ "vector_of_scalars": [
+  4,
+  5,
+  6,
+  7
+ ]
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    ASSERT_TRUE(builder.Verify());
+    object->clear_vector_of_scalars();
+    ASSERT_TRUE(builder.Verify());
+    ASSERT_FALSE(object->has_vector_of_scalars())
+        << aos::FlatbufferToJson(builder.AsFlatbufferSpan());
+    vector = CHECK_NOTNULL(object->add_vector_of_scalars());
+    ASSERT_TRUE(builder.Verify());
+    EXPECT_EQ(0u, object->AsFlatbuffer().vector_of_scalars()->size());
+    ASSERT_TRUE(vector->emplace_back(9));
+    ASSERT_TRUE(vector->emplace_back(7));
+    ASSERT_TRUE(vector->emplace_back(1));
+    // This vector has no knowledge of the past resizing; it should fail to add
+    // an extra number.
+    ASSERT_FALSE(vector->emplace_back(7));
+  }
+  {
+    object->clear_substruct();
+    ASSERT_TRUE(builder.Verify());
+    EXPECT_FALSE(object->has_substruct());
+    object->set_substruct(SubStruct{2, 3});
+    EXPECT_EQ(
+        R"json({
+ "scalar": 456,
+ "vector_of_scalars": [
+  9,
+  7,
+  1
+ ],
+ "substruct": {
+  "x": 2.0,
+  "y": 3.0
+ }
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    object->clear_substruct();
+    ASSERT_TRUE(builder.Verify());
+    EXPECT_FALSE(object->has_substruct());
+    object->set_substruct(SubStruct{4, 5});
+    EXPECT_EQ(
+        R"json({
+ "scalar": 456,
+ "vector_of_scalars": [
+  9,
+  7,
+  1
+ ],
+ "substruct": {
+  "x": 4.0,
+  "y": 5.0
+ }
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+  }
+  {
+    object->clear_subtable();
+    ASSERT_TRUE(builder.Verify());
+    EXPECT_FALSE(object->has_subtable());
+    auto subtable = CHECK_NOTNULL(object->add_subtable());
+    subtable->set_baz(9.71);
+    EXPECT_EQ(
+        R"json({
+ "scalar": 456,
+ "vector_of_scalars": [
+  9,
+  7,
+  1
+ ],
+ "substruct": {
+  "x": 4.0,
+  "y": 5.0
+ },
+ "subtable": {
+  "baz": 9.71
+ }
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+    object->clear_subtable();
+    ASSERT_TRUE(builder.Verify());
+    EXPECT_FALSE(object->has_subtable());
+    subtable = CHECK_NOTNULL(object->add_subtable());
+    subtable->set_baz(16.78);
+    EXPECT_EQ(
+        R"json({
+ "scalar": 456,
+ "vector_of_scalars": [
+  9,
+  7,
+  1
+ ],
+ "substruct": {
+  "x": 4.0,
+  "y": 5.0
+ },
+ "subtable": {
+  "baz": 16.780001
+ }
+})json",
+        aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                              {.multi_line = true}));
+  }
+  TestMemory(builder.buffer());
+}
+
+// Try to cover ~all supported scalar/flatbuffer types using JSON convenience
+// functions.
+TEST_F(StaticFlatbuffersTest, FlatbufferTypeCoverage) {
+  VerifyJson<frc971::testing::ConfigurationStatic>("{\n\n}");
+  std::string populated_config =
+      aos::util::ReadFileToStringOrDie(aos::testing::ArtifactPath(
+          "aos/flatbuffers/test_dir/type_coverage.json"));
+  // Get rid of a pesky new line.
+  populated_config = populated_config.substr(0, populated_config.size() - 1);
+  VerifyJson<frc971::testing::ConfigurationStatic>(populated_config);
+
+  // And now play around with mutating the buffer.
+  Builder<frc971::testing::ConfigurationStatic> builder =
+      aos::JsonToStaticFlatbuffer<frc971::testing::ConfigurationStatic>(
+          populated_config);
+  ASSERT_TRUE(builder.Verify());
+  builder.get()->clear_foo_float();
+  ASSERT_TRUE(builder.Verify());
+  ASSERT_FALSE(builder.get()->AsFlatbuffer().has_foo_float());
+  builder.get()->set_foo_float(1.111);
+  ASSERT_TRUE(builder.Verify());
+  ASSERT_FLOAT_EQ(1.111, builder.get()->AsFlatbuffer().foo_float());
+  TestMemory(builder.buffer());
+}
+
+TEST_F(StaticFlatbuffersTest, MinimallyAlignedTable) {
+  VerifyJson<MinimallyAlignedTableStatic>("{\n \"field\": 123\n}");
+  static_assert(4u == alignof(uoffset_t),
+                "The alignment of a uoffset_t is expected to be 4.");
+  ASSERT_EQ(alignof(uoffset_t), MinimallyAlignedTableStatic::kAlign)
+      << "No table should have an alignment of less than the alignment of the "
+         "table's root offset.";
+}
+
+// Confirm that we can use the SpanAllocator with a span that provides exactly
+// the required buffer size.
+TEST_F(StaticFlatbuffersTest, ExactSizeSpanAllocator) {
+  uint8_t buffer[Builder<TestTableStatic>::kBufferSize];
+  aos::fbs::SpanAllocator allocator({buffer, sizeof(buffer)});
+  Builder<TestTableStatic> builder(&allocator);
+  TestTableStatic *object = builder.get();
+  object->set_scalar(123);
+  {
+    auto vector = object->add_vector_of_scalars();
+    ASSERT_TRUE(vector->emplace_back(4));
+    ASSERT_TRUE(vector->emplace_back(5));
+  }
+  {
+    auto string = object->add_string();
+    string->SetString("Hello, World!");
+  }
+  {
+    auto vector_of_strings = object->add_vector_of_strings();
+    auto sub_string = CHECK_NOTNULL(vector_of_strings->emplace_back());
+    ASSERT_TRUE(sub_string->emplace_back('D'));
+  }
+  { object->set_substruct({971, 254}); }
+  {
+    auto subtable = object->add_subtable();
+    subtable->set_foo(1234);
+  }
+  {
+    auto vector = object->add_vector_of_structs();
+    ASSERT_TRUE(vector->emplace_back({48, 67}));
+    ASSERT_TRUE(vector->emplace_back({118, 148}));
+    ASSERT_TRUE(vector->emplace_back({971, 973}));
+    // Max vector size is three; this should fail.
+    ASSERT_FALSE(vector->emplace_back({1114, 2056}));
+    // We don't have any extra space available.
+    ASSERT_FALSE(vector->reserve(4));
+    ASSERT_FALSE(vector->emplace_back({1114, 2056}));
+  }
+  {
+    auto vector = object->add_vector_of_tables();
+    auto subobject = vector->emplace_back();
+    subobject->set_foo(222);
+  }
+  {
+    auto subtable = object->add_included_table();
+    subtable->set_foo(included::TestEnum::B);
+  }
+  ASSERT_TRUE(builder.AsFlatbufferSpan().Verify());
+  VLOG(1) << aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                                   {.multi_line = true});
+  VLOG(1) << AnnotateBinaries(test_schema_, builder.buffer());
+  TestMemory(builder.buffer());
+}
+
+// Test that when we provide too small of a span to the Builder that it
+// correctly fails.
+TEST_F(StaticFlatbuffersTest, TooSmallSpanAllocator) {
+  std::vector<uint8_t> buffer;
+  buffer.resize(10, 0);
+  aos::fbs::SpanAllocator allocator({buffer.data(), buffer.size()});
+  EXPECT_DEATH(Builder<TestTableStatic>{&allocator}, "Failed to allocate");
+}
+
+// Verify that if we create a span with extra headroom that that lets us
+// dynamically alter the size of vectors in the flatbuffers.
+TEST_F(StaticFlatbuffersTest, ExtraLargeSpanAllocator) {
+  uint8_t buffer[Builder<TestTableStatic>::kBufferSize + 10000];
+  aos::fbs::SpanAllocator allocator({buffer, sizeof(buffer)});
+  Builder<TestTableStatic> builder(&allocator);
+  TestTableStatic *object = builder.get();
+  {
+    auto vector = object->add_unspecified_length_vector();
+    // Confirm that the vector does indeed start out at zero length.
+    ASSERT_FALSE(vector->emplace_back(4));
+    ASSERT_TRUE(vector->reserve(9000));
+    vector->resize(256);
+    for (size_t index = 0; index < 256; ++index) {
+      vector->at(index) = static_cast<uint8_t>(index);
+    }
+  }
+  ASSERT_EQ(256, object->AsFlatbuffer().unspecified_length_vector()->size());
+  size_t expected = 0;
+  for (const uint8_t value :
+       *object->AsFlatbuffer().unspecified_length_vector()) {
+    EXPECT_EQ(expected++, value);
+  }
+  expected = 0;
+  for (const uint8_t value : *object->unspecified_length_vector()) {
+    EXPECT_EQ(expected++, value);
+  }
+  TestMemory(builder.buffer());
+}
+
+// Tests that the iterators on the Vector type work.
+TEST_F(StaticFlatbuffersTest, IteratorTest) {
+  Builder<TestTableStatic> builder(std::make_unique<VectorAllocator>());
+  {
+    auto vector = builder->add_unspecified_length_vector();
+    ASSERT_TRUE(vector->reserve(9000));
+    vector->resize(256);
+    uint8_t set_value = 0;
+    for (uint8_t &destination : *vector) {
+      destination = set_value;
+      ++set_value;
+    }
+    uint8_t expected = 0;
+    for (const uint8_t value : *builder->unspecified_length_vector()) {
+      EXPECT_EQ(expected, value);
+      ++expected;
+    }
+    // Exercise some of the random access iterator functionality to ensure that
+    // we have it implemented.
+    auto begin_it = vector->begin();
+    EXPECT_EQ(begin_it + 256, vector->end());
+    EXPECT_EQ(7, *(begin_it + 7));
+    EXPECT_EQ(255, *(vector->end() - 1));
+    EXPECT_EQ(256, vector->end() - vector->begin());
+    EXPECT_EQ(-256, vector->begin() - vector->end());
+    static_assert(std::random_access_iterator<decltype(vector->begin())>,
+                  "The vector iterator does not meet the requirements of a "
+                  "random access iterator.");
+  }
+  {
+    auto vector = builder->add_vector_of_structs();
+    vector->resize(3);
+    double set_value = 0;
+    for (SubStruct &destination : *vector) {
+      destination.mutate_x(set_value);
+      destination.mutate_y(-set_value);
+      set_value += 1.0;
+    }
+    double expected = 0;
+    for (const SubStruct &value : *builder->vector_of_structs()) {
+      EXPECT_EQ(expected, value.x());
+      EXPECT_EQ(-expected, value.y());
+      expected += 1.0;
+    }
+    static_assert(std::random_access_iterator<decltype(vector->begin())>,
+                  "The vector iterator does not meet the requirements of a "
+                  "random access iterator.");
+  }
+  {
+    auto vector = builder->add_vector_of_tables();
+    vector->resize(3);
+    int set_value = 0;
+    for (SubTableStatic &destination : *vector) {
+      destination.set_foo(set_value);
+      set_value += 1;
+    }
+    int expected = 0;
+    for (const SubTableStatic &value : *builder->vector_of_tables()) {
+      EXPECT_EQ(expected, value.foo());
+      EXPECT_FALSE(value.has_baz());
+      expected += 1;
+    }
+    static_assert(std::random_access_iterator<decltype(vector->begin())>,
+                  "The vector iterator does not meet the requirements of a "
+                  "random access iterator.");
+  }
+}
+}  // namespace aos::fbs::testing
diff --git a/aos/flatbuffers/static_table.h b/aos/flatbuffers/static_table.h
new file mode 100644
index 0000000..e9619a7
--- /dev/null
+++ b/aos/flatbuffers/static_table.h
@@ -0,0 +1,119 @@
+#ifndef AOS_FLATBUFFERS_STATIC_TABLE_H_
+#define AOS_FLATBUFFERS_STATIC_TABLE_H_
+#include <algorithm>
+#include <span>
+
+#include "flatbuffers/base.h"
+#include "glog/logging.h"
+
+#include "aos/flatbuffers/base.h"
+namespace aos::fbs {
+
+// This Table object is used as the parent class to the generated code for every
+// flatbuffer table that we generate code for.
+// This object primarily serves to provide some useful common methods for
+// mutating the flatbuffer memory.
+//
+// Every table will be aligned to the greatest alignment of all of its members
+// and its size will be equal to a multiple of the alignment. Each table shall
+// have the following layout: [vtable offset; inline data with padding; vtable;
+// padding; table/vector data with padding]
+class Table : public ResizeableObject {
+ public:
+  // Prints out a debug string of the raw flatbuffer memory. Does not currently
+  // do anything intelligent ot traverse down into the subobjects of the
+  // flatbuffer (if you want that, then use the flatbuffer binary
+  // annotator---this code mostly exists for debugging the static flatbuffers
+  // implementation itself).
+  std::string SerializationDebugString() const {
+    std::stringstream str;
+    str << "Size: " << buffer_.size() << " alignment: " << Alignment() << "\n";
+    str << "Observed Vtable offset " << Get<soffset_t>(0) << "\n";
+    str << "Inline Size " << InlineTableSize() << " Inline Bytes:\n";
+    internal::DebugBytes(internal::GetSubSpan(buffer_, 4, InlineTableSize()),
+                         str);
+    str << "Vtable offset " << FixedVtableOffset() << " Vtable size "
+        << VtableSize() << " Vtable contents:\n";
+    internal::DebugBytes(
+        internal::GetSubSpan(buffer_, FixedVtableOffset(), VtableSize()), str);
+    str << "Offset data offset " << OffsetDataStart() << "\n";
+    // Actual contents can be big; don't print them out until we run into a
+    // situation where we need to debug that.
+    return str.str();
+  }
+
+ protected:
+  static constexpr size_t kMinAlign = alignof(uoffset_t);
+
+  Table(std::span<uint8_t> buffer, ResizeableObject *parent)
+      : ResizeableObject(buffer, parent) {}
+  Table(std::span<uint8_t> buffer, Allocator *allocator)
+      : ResizeableObject(buffer, allocator) {}
+  Table(std::span<uint8_t> buffer, ::std::unique_ptr<Allocator> allocator)
+      : ResizeableObject(buffer, ::std::move(allocator)) {}
+  Table(Table &&) = default;
+  virtual ~Table() {}
+  virtual size_t FixedVtableOffset() const = 0;
+  virtual size_t VtableSize() const = 0;
+  virtual size_t InlineTableSize() const = 0;
+  virtual size_t OffsetDataStart() const = 0;
+  size_t AbsoluteOffsetOffset() const override { return 0; }
+  void PopulateVtable() {
+    // Zero out everything up to the start of the sub-messages/tables, which are
+    // responsible for doing their own memory initialization.
+    internal::ClearSpan(internal::GetSubSpan(buffer_, 0, OffsetDataStart()));
+    // Set the offset to the start of the vtable (points backwards, hence the
+    // sign inversion).
+    Set<soffset_t>(0, -FixedVtableOffset());
+    // First element of the vtable is the size of the table.
+    Set<voffset_t>(FixedVtableOffset(), VtableSize());
+    // Second element of the vtable is the size of the inlined data (not really
+    // used by anything...).
+    Set<voffset_t>(FixedVtableOffset() + sizeof(voffset_t), InlineTableSize());
+  }
+
+  template <typename T>
+  void SetField(size_t absolute_offset, size_t vtable_offset, const T &value) {
+    Set<T>(absolute_offset, value);
+    CHECK_EQ(0u, (absolute_offset + reinterpret_cast<size_t>(buffer_.data())) %
+                     alignof(T));
+    Set<voffset_t>(FixedVtableOffset() + vtable_offset, absolute_offset);
+  }
+
+  void ClearField(size_t absolute_offset, size_t inline_size,
+                  size_t vtable_offset) {
+    // TODO: Remove/account for any excess allocated memory.
+    internal::ClearSpan(
+        internal::GetSubSpan(buffer_, absolute_offset, inline_size));
+    Set<voffset_t>(FixedVtableOffset() + vtable_offset, 0);
+  }
+
+  template <typename T>
+  const T &Get(size_t absolute_offset) const {
+    return *reinterpret_cast<const T *>(buffer_.data() + absolute_offset);
+  }
+
+  template <typename T>
+  T *MutableGet(size_t absolute_offset) {
+    return reinterpret_cast<T *>(buffer_.data() + absolute_offset);
+  }
+
+  template <typename T>
+  T *GetMutableFlatbuffer() {
+    return reinterpret_cast<T *>(buffer_.data());
+  }
+
+  template <typename T>
+  const T *GetFlatbuffer() const {
+    return reinterpret_cast<const T *>(buffer_.data());
+  }
+
+ private:
+  template <typename T>
+  void Set(size_t absolute_offset, const T &value) {
+    *reinterpret_cast<T *>(buffer_.data() + absolute_offset) = value;
+  }
+};
+
+}  // namespace aos::fbs
+#endif  // AOS_FLATBUFFERS_STATIC_TABLE_H_
diff --git a/aos/flatbuffers/static_vector.h b/aos/flatbuffers/static_vector.h
new file mode 100644
index 0000000..7349a8d
--- /dev/null
+++ b/aos/flatbuffers/static_vector.h
@@ -0,0 +1,785 @@
+#ifndef AOS_FLATBUFFERS_STATIC_VECTOR_H_
+#define AOS_FLATBUFFERS_STATIC_VECTOR_H_
+#include <span>
+
+#include "flatbuffers/base.h"
+#include "flatbuffers/vector.h"
+#include "glog/logging.h"
+
+#include "aos/containers/inlined_vector.h"
+#include "aos/containers/sized_array.h"
+#include "aos/flatbuffers/base.h"
+
+namespace aos::fbs {
+
+namespace internal {
+// Helper class for managing how we specialize the Vector object for different
+// contained types.
+// Users of the Vector class should never need to care about this.
+// Template arguments:
+//   T: The type that the vector stores.
+//   kInline: Whether the type in question is stored inline or not.
+//   Enable: Used for SFINAE around struct values; can be ignored.
+// The struct provides the following types:
+//   Type: The type of the data that will be stored inline in the vector.
+//   ObjectType: The type of the actual data (only used for non-inline objects).
+//   FlatbufferType: The type used by flatbuffers::Vector to store this type.
+//   ConstFlatbufferType: The type used by a const flatbuffers::Vector to store
+//     this type.
+//   kDataAlign: Alignment required by the stored type.
+//   kDataSize: Nominal size required by each non-inline data member. This is
+//     what will be initially allocated; once created, individual members may
+//     grow to accommodate dynamically lengthed vectors.
+template <typename T, bool kInline, class Enable = void>
+struct InlineWrapper;
+}  // namespace internal
+
+// This Vector class provides a mutable, resizeable, flatbuffer vector.
+//
+// Upon creation, the Vector will start with enough space allocated for
+// kStaticLength elements, and must be provided with a memory buffer that
+// is large enough to serialize all the kStaticLength members (kStaticLength may
+// be zero).
+//
+// Once created, the Vector may be grown using calls to reserve().
+// This will result in the Vector attempting to allocate memory via its
+// parent object; such calls may fail if there is no space available in the
+// allocator.
+//
+// Note that if you are using the Vector class in a realtime context (and thus
+// must avoid dynamic memory allocations) you must only be using a Vector of
+// inline data (i.e., scalars, enums, or structs). Flatbuffer tables and strings
+// require overhead to manage and so require some form of dynamic memory
+// allocation. If we discover a strong use-case for such things, then we may
+// provide some interface that allows managing said metadata on the stack or
+// in another realtime-safe manner.
+//
+// Template arguments:
+//   T: Type contained by the vector; either a scalar/struct/enum type or a
+//      static flatbuffer type of some sort (a String or an implementation of
+//      aos::fbs::Table).
+//   kStaticLength: Number of elements to statically allocate memory for.
+//      May be zero.
+//   kInline: Whether the type T will be stored inline in the vector.
+//   kForceAlign: Alignment to force for the start of the vector (e.g., for
+//     byte arrays it may be desirable to have the entire array aligned).
+//   kNullTerminate: Whether to reserve an extra byte past the end of
+//     the inline data for null termination. Not included in kStaticLength,
+//     so if e.g. you want to store the string "abc" then kStaticLength can
+//     be 3 and kNullTerminate can be true and the vector data will take
+//     up 4 bytes of memory.
+//
+// Vector buffer memory layout:
+// * Requirements:
+//   * Minimum alignment of 4 bytes (for element count).
+//   * The start of the vector data must be aligned to either
+//     alignof(InlineType) or a user-specified number.
+//   * The element count for the vector must immediately precede the vector
+//     data (and so may itself not be aligned to alignof(InlineType)).
+//   * For non-inlined types, the individual types must be aligned to
+//     their own alignment.
+// * In order to accommodate this, the vector buffer as a whole must
+//   generally be aligned to the greatest of the above alignments. There
+//   are two reasonable ways one could do this:
+//   * Require that the 4th byte of the buffer provided by aligned to
+//     the maximum alignment of its contents.
+//   * Require that the buffer itself by aligned, and provide padding
+//     ourselves. The Vector would then have to expose its own offset
+//     because it would not start at the start of the buffer.
+//   The former requires that the wrapping code understand the internals
+//   of how vectors work; the latter generates extra padding and adds
+//   extra logic around handling non-zero offsets.
+//   To maintain general simplicity, we will use the second condition and eat
+//   the cost of the potential extra few bytes of padding.
+// * The layout of the buffer will thus be:
+//   [padding; element_count; inline_data; padding; offset_data]
+//   The first padding will be of size max(0, kAlign - 4).
+//   The element_count is of size 4.
+//   The inline_data is of size sizeof(InlineType) * kStaticLength.
+//   The second padding is of size
+//       (kAlign - ((sizeof(InlineType) * kStaticLength) % kAlign)).
+//   The remaining data is only present if kInline is false.
+//   The offset data is of size T::kSize * kStaticLength. T::kSize % T::kAlign
+//   must be zero.
+//   Note that no padding is required on the end because T::kAlign will always
+//   end up being equal to the alignment (this can only be violated if
+//   kForceAlign is used, but we do not allow that).
+//   The Vector class leaves any padding uninitialized. Until and unless we
+//   determine that it is a performance issue, it is the responsibility of the
+//   parent of this object to zero-initialize the memory.
+template <typename T, size_t kStaticLength, bool kInline,
+          size_t kForceAlign = 0, bool kNullTerminate = false>
+class Vector : public ResizeableObject {
+  template <typename VectorType, typename ValueType>
+  class generic_iterator {
+   public:
+    using iterator_category = std::random_access_iterator_tag;
+    using value_type = ValueType;
+    using difference_type = std::ptrdiff_t;
+    using pointer = value_type *;
+    using reference = value_type &;
+
+    explicit generic_iterator(VectorType *vector, size_t index)
+        : vector_(vector), index_(index) {}
+    generic_iterator(const generic_iterator &) = default;
+    generic_iterator() : vector_(nullptr), index_(0) {}
+    generic_iterator &operator=(const generic_iterator &) = default;
+
+    generic_iterator &operator++() {
+      ++index_;
+      return *this;
+    }
+    generic_iterator operator++(int) {
+      generic_iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    generic_iterator &operator--() {
+      --index_;
+      return *this;
+    }
+    generic_iterator operator--(int) {
+      generic_iterator retval = *this;
+      --(*this);
+      return retval;
+    }
+    bool operator==(const generic_iterator &other) const {
+      CHECK_EQ(other.vector_, vector_);
+      return index_ == other.index_;
+    }
+    std::strong_ordering operator<=>(const generic_iterator &other) const {
+      CHECK_EQ(other.vector_, vector_);
+      return index_ <=> other.index_;
+    }
+    reference operator*() const { return vector_->at(index_); }
+    difference_type operator-(const generic_iterator &other) const {
+      CHECK_EQ(other.vector_, vector_);
+      return index_ - other.index_;
+    }
+    generic_iterator operator-(difference_type decrement) const {
+      return generic_iterator(vector_, index_ - decrement);
+    }
+    friend generic_iterator operator-(difference_type decrement,
+                                      const generic_iterator &rhs) {
+      return rhs - decrement;
+    }
+    generic_iterator operator+(difference_type increment) const {
+      return generic_iterator(vector_, index_ + increment);
+    }
+    friend generic_iterator operator+(difference_type increment,
+                                      const generic_iterator &rhs) {
+      return rhs + increment;
+    }
+    generic_iterator &operator+=(difference_type increment) {
+      index_ += increment;
+      return *this;
+    }
+    generic_iterator &operator-=(difference_type increment) {
+      index_ -= increment;
+      return *this;
+    }
+    reference operator[](difference_type index) const {
+      return *(*this + index);
+    }
+
+   private:
+    VectorType *vector_;
+    size_t index_;
+  };
+
+ public:
+  using iterator = generic_iterator<Vector, T>;
+  using const_iterator = generic_iterator<const Vector, const T>;
+
+  static_assert(kInline || !kNullTerminate,
+                "It does not make sense to null-terminate vectors of objects.");
+  // Type stored inline in the serialized vector (offsets for tables/strings; T
+  // otherwise).
+  using InlineType = typename internal::InlineWrapper<T, kInline>::Type;
+  // OUt-of-line type for out-of-line T.
+  using ObjectType = typename internal::InlineWrapper<T, kInline>::ObjectType;
+  // Type used as the template parameter to flatbuffers::Vector<>.
+  using FlatbufferType =
+      typename internal::InlineWrapper<T, kInline>::FlatbufferType;
+  using ConstFlatbufferType =
+      typename internal::InlineWrapper<T, kInline>::ConstFlatbufferType;
+  // flatbuffers::Vector type that corresponds to this Vector.
+  typedef flatbuffers::Vector<FlatbufferType> Flatbuffer;
+  typedef const flatbuffers::Vector<ConstFlatbufferType> ConstFlatbuffer;
+  // Alignment of the inline data.
+  static constexpr size_t kInlineAlign =
+      std::max(kForceAlign, alignof(InlineType));
+  // Type used for serializing the length of the vector.
+  typedef uint32_t LengthType;
+  // Overall alignment of this type, and required alignment of the buffer that
+  // must be provided to the Vector.
+  static constexpr size_t kAlign =
+      std::max({alignof(LengthType), kInlineAlign,
+                internal::InlineWrapper<T, kInline>::kDataAlign});
+  // Padding inserted prior to the length element of the vector (to manage
+  // alignment of the data properly; see class comment)
+  static constexpr size_t kPadding1 =
+      std::max<size_t>(0, kAlign - sizeof(LengthType));
+  // Size of the vector length field.
+  static constexpr size_t kLengthSize = sizeof(LengthType);
+  // Size of all the inline vector data, including null termination (prior to
+  // any dynamic increases in size).
+  static constexpr size_t kInlineSize =
+      sizeof(InlineType) * (kStaticLength + (kNullTerminate ? 1 : 0));
+  // Per-element size of any out-of-line data.
+  static constexpr size_t kDataElementSize =
+      internal::InlineWrapper<T, kInline>::kDataSize;
+  // Padding between the inline data and any out-of-line data, to manage
+  // mismatches in alignment between the two.
+  static constexpr size_t kPadding2 = kAlign - (kInlineSize % kAlign);
+  // Total statically allocated space for any out-of-line data ("offset data")
+  // (prior to any dynamic increases in size).
+  static constexpr size_t kOffsetOffsetDataSize =
+      kInline ? 0 : (kStaticLength * kDataElementSize);
+  // Total nominal size of the Vector.
+  static constexpr size_t kSize =
+      kPadding1 + kLengthSize + kInlineSize + kPadding2 + kOffsetOffsetDataSize;
+  // Offset from the start of the provided buffer to where the actual start of
+  // the vector is.
+  static constexpr size_t kOffset = kPadding1;
+  // Constructors; the provided buffer must be aligned to kAlign and be kSize in
+  // length. parent must be non-null.
+  Vector(std::span<uint8_t> buffer, ResizeableObject *parent)
+      : ResizeableObject(buffer, parent) {
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    CHECK_EQ(kSize, buffer.size());
+    SetLength(0u);
+    if (!kInline) {
+      // Initialize the offsets for any sub-tables. These are used to track
+      // where each table will get serialized in memory as memory gets
+      // resized/moved around.
+      for (size_t index = 0; index < kStaticLength; ++index) {
+        object_absolute_offsets_.emplace_back(kPadding1 + kLengthSize +
+                                              kInlineSize + kPadding2 +
+                                              index * kDataElementSize);
+      }
+    }
+  }
+  Vector(const Vector &) = delete;
+  Vector &operator=(const Vector &) = delete;
+  virtual ~Vector() {}
+  // Current allocated length of this vector.
+  // Does not include null termination.
+  size_t capacity() const { return allocated_length_; }
+  // Current length of the vector.
+  // Does not include null termination.
+  size_t size() const { return length_; }
+
+  // Appends an element to the Vector. Used when kInline is false. Returns
+  // nullptr if the append failed due to insufficient capacity. If you need to
+  // increase the capacity() of the vector, call reserve().
+  [[nodiscard]] T *emplace_back();
+  // Appends an element to the Vector. Used when kInline is true. Returns false
+  // if there is insufficient capacity for a new element.
+  [[nodiscard]] bool emplace_back(T element) {
+    static_assert(kInline);
+    return AddInlineElement(element);
+  }
+
+  // Adjusts the allocated size of the vector (does not affect the actual
+  // current length as returned by size()). Returns true on success, and false
+  // if the allocation failed for some reason.
+  // Note that reductions in size will not currently result in the allocated
+  // size actually changing.
+  [[nodiscard]] bool reserve(size_t new_length) {
+    if (new_length > allocated_length_) {
+      const size_t new_elements = new_length - allocated_length_;
+      // First, we must add space for our new inline elements.
+      if (!InsertBytes(
+              inline_data() + allocated_length_ + (kNullTerminate ? 1 : 0),
+              new_elements * sizeof(InlineType), SetZero::kYes)) {
+        return false;
+      }
+      if (!kInline) {
+        // For non-inline objects, create the space required for all the new
+        // object data.
+        const size_t insertion_point = buffer_.size();
+        if (!InsertBytes(buffer_.data() + insertion_point,
+                         new_elements * kDataElementSize, SetZero::kYes)) {
+          return false;
+        }
+        for (size_t index = 0; index < new_elements; ++index) {
+          // Note that the already-allocated data may be arbitrarily-sized, so
+          // we cannot use the same static calculation that we do in the
+          // constructor.
+          object_absolute_offsets_.emplace_back(insertion_point +
+                                                index * kDataElementSize);
+        }
+        objects_.reserve(new_length);
+      }
+      allocated_length_ = new_length;
+    }
+    return true;
+  }
+
+  // Accessors for using the Vector as a flatbuffers::Vector.
+  // Note that these pointers will be unstable if any memory allocations occur
+  // that cause memory to get shifted around.
+  ConstFlatbuffer *AsFlatbufferVector() const {
+    return reinterpret_cast<const Flatbuffer *>(vector_buffer().data());
+  }
+
+  // Copies the contents of the provided vector into this; returns false on
+  // failure (e.g., if the provided vector is too long for the amount of space
+  // we can allocate through reserve()).
+  // This is a deep copy, and will call FromFlatbuffer on any constituent
+  // objects.
+  [[nodiscard]] bool FromFlatbuffer(ConstFlatbuffer *vector);
+
+  // Returns the element at the provided index. index must be less than size().
+  const T &at(size_t index) const {
+    CHECK_LT(index, length_);
+    return unsafe_at(index);
+  }
+
+  // Same as at(), except that bounds checks are only performed in non-optimized
+  // builds.
+  // TODO(james): The GetInlineElement() call itself does some bounds-checking;
+  // consider down-grading that.
+  const T &unsafe_at(size_t index) const {
+    DCHECK_LT(index, length_);
+    if (kInline) {
+      // This reinterpret_cast is extremely wrong if T != InlineType (this is
+      // fine because we only do this if kInline is true).
+      // TODO(james): Get the templating improved so that we can get away with
+      // specializing at() instead of using if statements. Resolving this will
+      // also allow deduplicating the Resize() calls.
+      // This specialization is difficult because you cannot partially
+      // specialize a templated class method (online things seem to suggest e.g.
+      // using a struct as the template parameter rather than having separate
+      // parameters).
+      return reinterpret_cast<const T &>(GetInlineElement(index));
+    } else {
+      return objects_[index].t;
+    }
+  }
+
+  // Returns a mutable pointer to the element at the provided index. index must
+  // be less than size().
+  T &at(size_t index) {
+    CHECK_LT(index, length_);
+    return unsafe_at(index);
+  }
+
+  // Same as at(), except that bounds checks are only performed in non-optimized
+  // builds.
+  // TODO(james): The GetInlineElement() call itself does some bounds-checking;
+  // consider down-grading that.
+  T &unsafe_at(size_t index) {
+    DCHECK_LT(index, length_);
+    if (kInline) {
+      // This reinterpret_cast is extremely wrong if T != InlineType (this is
+      // fine because we only do this if kInline is true).
+      // TODO(james): Get the templating improved so that we can get away with
+      // specializing at() instead of using if statements. Resolving this will
+      // also allow deduplicating the Resize() calls.
+      // This specialization is difficult because you cannot partially
+      // specialize a templated class method (online things seem to suggest e.g.
+      // using a struct as the template parameter rather than having separate
+      // parameters).
+      return reinterpret_cast<T &>(GetInlineElement(index));
+    } else {
+      return objects_[index].t;
+    }
+  }
+
+  const T &operator[](size_t index) const { return at(index); }
+  T &operator[](size_t index) { return at(index); }
+
+  // Resizes the vector to the requested size.
+  // size must be less than or equal to the current capacity() of the vector.
+  // Does not allocate additional memory (call reserve() to allocate additional
+  // memory).
+  // Zero-initializes all inline element; initializes all subtable/string
+  // elements to extant but empty objects.
+  void resize(size_t size);
+
+  // Resizes an inline vector to the requested size.
+  // When changing the size of the vector, the removed/inserted elements will be
+  // set to zero if requested. Otherwise, they will be left uninitialized.
+  void resize_inline(size_t size, SetZero set_zero) {
+    CHECK_LE(size, allocated_length_);
+    static_assert(
+        kInline,
+        "Vector::resize_inline() only works for inline vector types (scalars, "
+        "enums, structs).");
+    if (size == length_) {
+      return;
+    }
+    if (set_zero == SetZero::kYes) {
+      memset(
+          reinterpret_cast<void *>(inline_data() + std::min(size, length_)), 0,
+          std::abs(static_cast<ssize_t>(length_) - static_cast<ssize_t>(size)) *
+              sizeof(InlineType));
+    }
+    length_ = size;
+    SetLength(length_);
+  }
+  // Resizes a vector of offsets to the requested size.
+  // If the size is increased, the new elements will be initialized
+  // to empty but extant objects for non-inlined types (so, zero-length
+  // vectors/strings; objects that exist but have no fields populated).
+  // Note that this is always equivalent to resize().
+  void resize_not_inline(size_t size) {
+    CHECK_LE(size, allocated_length_);
+    static_assert(!kInline,
+                  "Vector::resize_not_inline() only works for offset vector "
+                  "types (objects, strings).");
+    if (size == length_) {
+      return;
+    } else if (length_ > size) {
+      // TODO: Remove any excess allocated memory.
+      length_ = size;
+      SetLength(length_);
+      return;
+    } else {
+      while (length_ < size) {
+        CHECK_NOTNULL(emplace_back());
+      }
+    }
+  }
+
+  // Accessors directly to the inline data of a vector.
+  const T *data() const {
+    static_assert(kInline,
+                  "If you have a use-case for directly accessing the "
+                  "flatbuffer data pointer for vectors of "
+                  "objects/strings, please start a discussion.");
+    return inline_data();
+  }
+
+  T *data() {
+    static_assert(kInline,
+                  "If you have a use-case for directly accessing the "
+                  "flatbuffer data pointer for vectors of "
+                  "objects/strings, please start a discussion.");
+    return inline_data();
+  }
+
+  // Iterators to allow easy use with standard C++ features.
+  iterator begin() { return iterator(this, 0); }
+  iterator end() { return iterator(this, size()); }
+  const_iterator begin() const { return const_iterator(this, 0); }
+  const_iterator end() const { return const_iterator(this, size()); }
+
+  std::string SerializationDebugString() const {
+    std::stringstream str;
+    str << "Raw Size: " << kSize << " alignment: " << kAlign
+        << " allocated length: " << allocated_length_ << " inline alignment "
+        << kInlineAlign << " kPadding1 " << kPadding1 << "\n";
+    str << "Observed length " << GetLength() << " (expected " << length_
+        << ")\n";
+    str << "Inline Size " << kInlineSize << " Inline bytes/value:\n";
+    // TODO(james): Get pretty-printing for structs so we can provide better
+    // debug.
+    internal::DebugBytes(
+        internal::GetSubSpan(vector_buffer(), kLengthSize,
+                             sizeof(InlineType) * allocated_length_),
+        str);
+    str << "kPadding2 " << kPadding2 << " offset data size "
+        << kOffsetOffsetDataSize << "\n";
+    return str.str();
+  }
+
+ protected:
+  friend struct internal::TableMover<
+      Vector<T, kStaticLength, kInline, kForceAlign, kNullTerminate>>;
+  // protected so that the String class can access the move constructor.
+  Vector(Vector &&) = default;
+
+ private:
+  // See kAlign and kOffset.
+  size_t Alignment() const final { return kAlign; }
+  size_t AbsoluteOffsetOffset() const override { return kOffset; }
+  // Returns a buffer that starts at the start of the vector itself (past any
+  // padding).
+  std::span<uint8_t> vector_buffer() {
+    return internal::GetSubSpan(buffer(), kPadding1);
+  }
+  std::span<const uint8_t> vector_buffer() const {
+    return internal::GetSubSpan(buffer(), kPadding1);
+  }
+
+  bool AddInlineElement(InlineType e) {
+    if (length_ == allocated_length_) {
+      return false;
+    }
+    SetInlineElement(length_, e);
+    ++length_;
+    SetLength(length_);
+    return true;
+  }
+
+  void SetInlineElement(size_t index, InlineType value) {
+    CHECK_LT(index, allocated_length_);
+    inline_data()[index] = value;
+  }
+
+  InlineType &GetInlineElement(size_t index) {
+    CHECK_LT(index, allocated_length_);
+    return inline_data()[index];
+  }
+
+  const InlineType &GetInlineElement(size_t index) const {
+    CHECK_LT(index, allocated_length_);
+    return inline_data()[index];
+  }
+
+  // Returns a pointer to the start of the inline data itself.
+  InlineType *inline_data() {
+    return reinterpret_cast<InlineType *>(vector_buffer().data() + kLengthSize);
+  }
+  const InlineType *inline_data() const {
+    return reinterpret_cast<const InlineType *>(vector_buffer().data() +
+                                                kLengthSize);
+  }
+
+  // Updates the length of the vector to match the provided length. Does not set
+  // the length_ member.
+  void SetLength(LengthType length) {
+    *reinterpret_cast<LengthType *>(vector_buffer().data()) = length;
+    if (kNullTerminate) {
+      memset(reinterpret_cast<void *>(inline_data() + length), 0,
+             sizeof(InlineType));
+    }
+  }
+  LengthType GetLength() const {
+    return *reinterpret_cast<const LengthType *>(vector_buffer().data());
+  }
+
+  // Overrides to allow ResizeableObject to manage memory adjustments.
+  size_t NumberOfSubObjects() const final {
+    return kInline ? 0 : allocated_length_;
+  }
+  using ResizeableObject::SubObject;
+  SubObject GetSubObject(size_t index) final {
+    return SubObject{
+        reinterpret_cast<uoffset_t *>(&GetInlineElement(index)),
+        // In order to let this compile regardless of whether type T is an
+        // object type or not, we just use a reinterpret_cast.
+        (index < length_)
+            ? reinterpret_cast<ResizeableObject *>(&objects_[index].t)
+            : nullptr,
+        &object_absolute_offsets_[index]};
+  }
+  // Implementation that handles copying from a flatbuffers::Vector of an inline
+  // data type.
+  [[nodiscard]] bool FromInlineFlatbuffer(ConstFlatbuffer *vector) {
+    if (!reserve(CHECK_NOTNULL(vector)->size())) {
+      return false;
+    }
+
+    // We will be overwriting the whole vector very shortly; there is no need to
+    // clear the buffer to zero.
+    resize_inline(vector->size(), SetZero::kNo);
+
+    memcpy(inline_data(), vector->Data(), size() * sizeof(InlineType));
+    return true;
+  }
+
+  // Implementation that handles copying from a flatbuffers::Vector of a
+  // not-inline data type.
+  [[nodiscard]] bool FromNotInlineFlatbuffer(const Flatbuffer *vector) {
+    if (!reserve(vector->size())) {
+      return false;
+    }
+    // "Clear" the vector.
+    resize_not_inline(0);
+
+    for (const typename T::Flatbuffer *entry : *vector) {
+      if (!CHECK_NOTNULL(emplace_back())->FromFlatbuffer(entry)) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  // In order to allow for easy partial template specialization, we use a
+  // non-member class to call FromInline/FromNotInlineFlatbuffer and
+  // resize_inline/resize_not_inline. There are not actually any great ways to
+  // do this with just our own class member functions, so instead we make these
+  // methods members of a friend of the Vector class; we then partially
+  // specialize the entire InlineWrapper class and use it to isolate anything
+  // that needs to have a common user interface while still having separate
+  // actual logic.
+  template <typename T_, bool kInline_, class Enable_>
+  friend struct internal::InlineWrapper;
+
+  // Note: The objects here really want to be owned by this object (as opposed
+  // to e.g. returning a stack-allocated object from the emplace_back() methods
+  // that the user then owns). There are two main challenges with have the user
+  // own the object on question:
+  // 1. We can't have >1 reference floating around, or else one object's state
+  //    can become out of date. This forces us to do ref-counting and could
+  //    make certain types of code obnoxious to write.
+  // 2. Once the user-created object goes out of scope, we lose all of its
+  //    internal state. In _theory_ it should be possible to reconstruct most
+  //    of the relevant state by examining the contents of the buffer, but
+  //    doing so would be cumbersome.
+  aos::InlinedVector<internal::TableMover<ObjectType>,
+                     kInline ? 0 : kStaticLength>
+      objects_;
+  aos::InlinedVector<size_t, kInline ? 0 : kStaticLength>
+      object_absolute_offsets_;
+  // Current actual length of the vector.
+  size_t length_ = 0;
+  // Current length that we have allocated space available for.
+  size_t allocated_length_ = kStaticLength;
+};
+
+template <typename T, size_t kStaticLength, bool kInline, size_t kForceAlign,
+          bool kNullTerminate>
+T *Vector<T, kStaticLength, kInline, kForceAlign,
+          kNullTerminate>::emplace_back() {
+  static_assert(!kInline);
+  if (length_ >= allocated_length_) {
+    return nullptr;
+  }
+  const size_t object_start = object_absolute_offsets_[length_];
+  std::span<uint8_t> object_buffer =
+      internal::GetSubSpan(buffer(), object_start, T::kSize);
+  objects_.emplace_back(object_buffer, this);
+  const uoffset_t offset =
+      object_start - (reinterpret_cast<size_t>(&GetInlineElement(length_)) -
+                      reinterpret_cast<size_t>(buffer().data()));
+  CHECK(AddInlineElement(offset));
+  return &objects_[objects_.size() - 1].t;
+}
+
+// The String class is a special version of the Vector that is always
+// null-terminated, always contains 1-byte character elements, and which has a
+// few extra methods for convenient string access.
+template <size_t kStaticLength>
+class String : public Vector<char, kStaticLength, true, 0, true> {
+ public:
+  typedef Vector<char, kStaticLength, true, 0, true> VectorType;
+  typedef flatbuffers::String Flatbuffer;
+  String(std::span<uint8_t> buffer, ResizeableObject *parent)
+      : VectorType(buffer, parent) {}
+  virtual ~String() {}
+  void SetString(std::string_view string) {
+    CHECK_LT(string.size(), VectorType::capacity());
+    VectorType::resize_inline(string.size(), SetZero::kNo);
+    memcpy(VectorType::data(), string.data(), string.size());
+  }
+  std::string_view string_view() const {
+    return std::string_view(VectorType::data(), VectorType::size());
+  }
+  std::string str() const {
+    return std::string(VectorType::data(), VectorType::size());
+  }
+  const char *c_str() const { return VectorType::data(); }
+
+ private:
+  friend struct internal::TableMover<String<kStaticLength>>;
+  String(String &&) = default;
+};
+
+namespace internal {
+// Specialization for all non-inline vector types. All of these types will just
+// use offsets for their inline data and have appropriate member types/constants
+// for the remaining fields.
+template <typename T>
+struct InlineWrapper<T, false, void> {
+  typedef uoffset_t Type;
+  typedef T ObjectType;
+  typedef flatbuffers::Offset<typename T::Flatbuffer> FlatbufferType;
+  typedef flatbuffers::Offset<typename T::Flatbuffer> ConstFlatbufferType;
+  static_assert((T::kSize % T::kAlign) == 0);
+  static constexpr size_t kDataAlign = T::kAlign;
+  static constexpr size_t kDataSize = T::kSize;
+  template <typename StaticVector>
+  static bool FromFlatbuffer(
+      StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+    return to->FromNotInlineFlatbuffer(from);
+  }
+  template <typename StaticVector>
+  static void ResizeVector(StaticVector *target, size_t size) {
+    target->resize_not_inline(size);
+  }
+};
+// Specialization for "normal" scalar inline data (ints, floats, doubles,
+// enums).
+template <typename T>
+struct InlineWrapper<T, true,
+                     typename std::enable_if_t<!std::is_class<T>::value>> {
+  typedef T Type;
+  typedef T ObjectType;
+  typedef T FlatbufferType;
+  typedef T ConstFlatbufferType;
+  static constexpr size_t kDataAlign = alignof(T);
+  static constexpr size_t kDataSize = sizeof(T);
+  template <typename StaticVector>
+  static bool FromFlatbuffer(
+      StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+    return to->FromInlineFlatbuffer(from);
+  }
+  template <typename StaticVector>
+  static void ResizeVector(StaticVector *target, size_t size) {
+    target->resize_inline(size, SetZero::kYes);
+  }
+};
+// Specialization for booleans, given that flatbuffers uses uint8_t's for bools.
+template <>
+struct InlineWrapper<bool, true, void> {
+  typedef uint8_t Type;
+  typedef uint8_t ObjectType;
+  typedef uint8_t FlatbufferType;
+  typedef uint8_t ConstFlatbufferType;
+  static constexpr size_t kDataAlign = 1u;
+  static constexpr size_t kDataSize = 1u;
+  template <typename StaticVector>
+  static bool FromFlatbuffer(
+      StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+    return to->FromInlineFlatbuffer(from);
+  }
+  template <typename StaticVector>
+  static void ResizeVector(StaticVector *target, size_t size) {
+    target->resize_inline(size, SetZero::kYes);
+  }
+};
+// Specialization for flatbuffer structs.
+// The flatbuffers codegen uses struct pointers rather than references or the
+// such, so it needs to be treated special.
+template <typename T>
+struct InlineWrapper<T, true,
+                     typename std::enable_if_t<std::is_class<T>::value>> {
+  typedef T Type;
+  typedef T ObjectType;
+  typedef T *FlatbufferType;
+  typedef const T *ConstFlatbufferType;
+  static constexpr size_t kDataAlign = alignof(T);
+  static constexpr size_t kDataSize = sizeof(T);
+  template <typename StaticVector>
+  static bool FromFlatbuffer(
+      StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+    return to->FromInlineFlatbuffer(from);
+  }
+  template <typename StaticVector>
+  static void ResizeVector(StaticVector *target, size_t size) {
+    target->resize_inline(size, SetZero::kYes);
+  }
+};
+}  // namespace internal
+   //
+template <typename T, size_t kStaticLength, bool kInline, size_t kForceAlign,
+          bool kNullTerminate>
+bool Vector<T, kStaticLength, kInline, kForceAlign,
+            kNullTerminate>::FromFlatbuffer(ConstFlatbuffer *vector) {
+  return internal::InlineWrapper<T, kInline>::FromFlatbuffer(this, vector);
+}
+
+template <typename T, size_t kStaticLength, bool kInline, size_t kForceAlign,
+          bool kNullTerminate>
+void Vector<T, kStaticLength, kInline, kForceAlign, kNullTerminate>::resize(
+    size_t size) {
+  internal::InlineWrapper<T, kInline>::ResizeVector(this, size);
+}
+
+}  // namespace aos::fbs
+#endif  // AOS_FLATBUFFERS_STATIC_VECTOR_H_
diff --git a/aos/flatbuffers/test.fbs b/aos/flatbuffers/test.fbs
new file mode 100644
index 0000000..97b0fef
--- /dev/null
+++ b/aos/flatbuffers/test.fbs
@@ -0,0 +1,44 @@
+include "aos/flatbuffers/test_dir/include.fbs";
+
+// TODO: test example with multiple namespaces per file.
+namespace aos.fbs.testing;
+
+struct SubStruct {
+  x:double;
+  y:double;
+}
+
+table MinimallyAlignedTable {
+  field:ubyte (id: 0);
+}
+
+table SubTable {
+ foo:short (id: 0);
+ bar:short (id: 1, deprecated);
+ baz:float (id: 2);
+}
+
+attribute "static_length";
+attribute "static_vector_string_length";
+
+table TestTable {
+  scalar:int (id: 0);
+  vector_of_scalars:[int] (id: 1, static_length: 3);
+  string:string (id: 2, static_length: 20);
+  vector_of_strings:[string] (id: 3, static_length: 3, static_vector_string_length: 10);
+  substruct:SubStruct (id: 4);
+  subtable:SubTable (id: 5);
+  // The force-aligned vector is deliberately put in the middle of the table
+  // both by ID and alphabetically (both of these can affect the order in which
+  // certain things are evaluated, and during development there were some issues
+  // with this).
+  vector_aligned:[int] (id: 6, force_align: 64, static_length: 3);
+  vector_of_structs:[SubStruct] (id: 7, static_length: 3);
+  vector_of_tables:[SubTable] (id: 8, static_length: 3);
+  included_table:aos.fbs.testing.included.IncludedTable (id: 9);
+  unspecified_length_vector:[ubyte] (id: 10);
+  unspecified_length_string:string (id: 11);
+  unspecified_length_vector_of_strings:[string] (id: 12);
+}
+
+root_type TestTable;
diff --git a/aos/flatbuffers/test_dir/BUILD b/aos/flatbuffers/test_dir/BUILD
new file mode 100644
index 0000000..76f5fb4
--- /dev/null
+++ b/aos/flatbuffers/test_dir/BUILD
@@ -0,0 +1,39 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
+static_flatbuffer(
+    name = "include_fbs",
+    srcs = ["include.fbs"],
+    visibility = ["//visibility:public"],
+)
+
+static_flatbuffer(
+    name = "type_coverage_fbs",
+    srcs = ["type_coverage.fbs"],
+    visibility = ["//visibility:public"],
+)
+
+filegroup(
+    name = "test_data",
+    srcs = ["type_coverage.json"],
+    visibility = ["//visibility:public"],
+)
+
+sh_binary(
+    name = "copy_test_static_header",
+    srcs = ["copy_test_static_header.sh"],
+    args = ["$(location //aos/flatbuffers:test_static_file)"],
+    data = ["//aos/flatbuffers:test_static_file"],
+)
+
+sh_test(
+    name = "compare_generated_files",
+    srcs = ["compare_generated_files.sh"],
+    args = [
+        "$(location //aos/flatbuffers:test_static_file)",
+        "$(location sample_test_static.h)",
+    ],
+    data = [
+        "//aos/flatbuffers:test_static_file",
+        "@org_frc971//aos/flatbuffers/test_dir:sample_test_static.h",
+    ],
+)
diff --git a/aos/flatbuffers/test_dir/compare_generated_files.sh b/aos/flatbuffers/test_dir/compare_generated_files.sh
new file mode 100755
index 0000000..6b7d584
--- /dev/null
+++ b/aos/flatbuffers/test_dir/compare_generated_files.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+# This test confirms that the sample_test_static.h is kept up to date.
+# This primarily serves to allow people to easily see what changes to
+# generated code will looks like when doing code reviews. The checked-in
+# file can be regenerated by running
+# $ bazel run @org_frc971//aos/flatbuffers/test_dir:copy_test_static_header
+diff $1 $2
diff --git a/aos/flatbuffers/test_dir/copy_test_static_header.sh b/aos/flatbuffers/test_dir/copy_test_static_header.sh
new file mode 100755
index 0000000..11e5b8e
--- /dev/null
+++ b/aos/flatbuffers/test_dir/copy_test_static_header.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+OUTPUT=${BUILD_WORKSPACE_DIRECTORY}/aos/flatbuffers/test_dir/sample_test_static.h
+cp $1 ${OUTPUT}
+chmod 644 ${OUTPUT}
diff --git a/aos/flatbuffers/test_dir/include.fbs b/aos/flatbuffers/test_dir/include.fbs
new file mode 100644
index 0000000..df34259
--- /dev/null
+++ b/aos/flatbuffers/test_dir/include.fbs
@@ -0,0 +1,10 @@
+namespace aos.fbs.testing.included;
+enum TestEnum : ubyte {
+  A = 0,
+  B = 1,
+}
+
+table IncludedTable {
+ foo:TestEnum (id: 0);
+}
+
diff --git a/aos/flatbuffers/test_dir/sample_test_static.h b/aos/flatbuffers/test_dir/sample_test_static.h
new file mode 100644
index 0000000..a6362d7
--- /dev/null
+++ b/aos/flatbuffers/test_dir/sample_test_static.h
@@ -0,0 +1,1628 @@
+#pragma once
+// This is a generated file. Do not modify.
+#include <optional>
+
+#include "aos/flatbuffers/static_table.h"
+#include "aos/flatbuffers/static_vector.h"
+#include "aos/flatbuffers/test_dir/include_generated.h"
+#include "aos/flatbuffers/test_dir/include_static.h"
+#include "aos/flatbuffers/test_generated.h"
+#include "aos/flatbuffers/test_static.h"
+
+namespace aos::fbs::testing {
+class MinimallyAlignedTableStatic : public ::aos::fbs::Table {
+ public:
+  // The underlying "raw" flatbuffer type for this type.
+  typedef aos::fbs::testing::MinimallyAlignedTable Flatbuffer;
+  // Returns this object as a flatbuffer type. This reference may not be valid
+  // following mutations to the underlying flatbuffer, due to how memory may get
+  // may get moved around.
+  const Flatbuffer &AsFlatbuffer() const {
+    return *GetFlatbuffer<Flatbuffer>();
+  }
+
+  // Space taken up by the inline portion of the flatbuffer table data, in
+  // bytes.
+  static constexpr size_t kInlineDataSize = 5;
+  // Space taken up by the vtable for this object, in bytes.
+  static constexpr size_t kVtableSize =
+      sizeof(::flatbuffers::voffset_t) * (2 + 1);
+  // Offset from the start of the internal memory buffer to the start of the
+  // vtable.
+  static constexpr size_t kVtableStart = ::aos::fbs::PaddedSize(
+      kInlineDataSize, alignof(::flatbuffers::voffset_t));
+  // Required alignment of this object. The buffer that this object gets
+  // constructed into must be aligned to this value.
+  static constexpr size_t kAlign = std::max<size_t>({kMinAlign, 1});
+
+  // Nominal size of this object, in bytes. The object may grow beyond this
+  // size, but will always start at this size and so the initial buffer must
+  // match this size.
+  static constexpr size_t kSize = ::aos::fbs::PaddedSize(
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) + 0, kAlign);
+  static_assert(
+      1 <= kAlign,
+      "Flatbuffer schema minalign should not exceed our required alignment.");
+  // Offset from the start of the memory buffer to the start of any out-of-line
+  // data (subtables, vectors, strings).
+  static constexpr size_t kOffsetDataStart =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign);
+  // Size required for a buffer that includes a root table offset at the start.
+  static constexpr size_t kRootSize =
+      ::aos::fbs::PaddedSize(kSize + sizeof(::flatbuffers::uoffset_t), kAlign);
+  // Minimum size required to build this flatbuffer in an entirely unaligned
+  // buffer (including the root table offset). Made to be a multiple of kAlign
+  // for convenience.
+  static constexpr size_t kUnalignedBufferSize = kRootSize + kAlign;
+  // Offset at which the table vtable offset occurs. This is only needed for
+  // vectors.
+  static constexpr size_t kOffset = 0;
+  // Various overrides to support the Table parent class.
+  size_t FixedVtableOffset() const final { return kVtableStart; }
+  size_t VtableSize() const final { return kVtableSize; }
+  size_t InlineTableSize() const final { return kInlineDataSize; }
+  size_t OffsetDataStart() const final { return kOffsetDataStart; }
+  size_t Alignment() const final { return kAlign; }
+  // Exposes the name of the flatbuffer type to allow interchangeable use
+  // of the Flatbuffer and FlatbufferStatic types in various AOS methods.
+  static const char *GetFullyQualifiedName() {
+    return Flatbuffer::GetFullyQualifiedName();
+  }
+
+  // Constructors for creating a flatbuffer object.
+  // Users should typically use the Builder class to create these objects,
+  // in order to allow it to populate the root table offset.
+
+  // The buffer provided to these constructors should be aligned to kAlign
+  // and kSize in length.
+  // The parent/allocator may not be nullptr.
+  MinimallyAlignedTableStatic(std::span<uint8_t> buffer,
+                              ::aos::fbs::ResizeableObject *parent)
+      : Table(buffer, parent) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+  MinimallyAlignedTableStatic(std::span<uint8_t> buffer,
+                              ::aos::fbs::Allocator *allocator)
+      : Table(buffer, allocator) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+  MinimallyAlignedTableStatic(
+      std::span<uint8_t> buffer,
+      ::std::unique_ptr<::aos::fbs::Allocator> allocator)
+      : Table(buffer, ::std::move(allocator)) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+
+  virtual ~MinimallyAlignedTableStatic() {}
+
+  // Sets the field field, causing it to be populated if it is not already.
+  // This will populate the field even if the specified value is the default.
+  void set_field(const uint8_t &value) {
+    SetField<uint8_t>(kInlineAbsoluteOffset_field, 4, value);
+  }
+
+  // Returns the value of field if set; nullopt otherwise.
+  std::optional<uint8_t> field() const {
+    return has_field()
+               ? std::make_optional(Get<uint8_t>(kInlineAbsoluteOffset_field))
+               : std::nullopt;
+    ;
+  }
+  // Returns a pointer to modify the field field.
+  // The pointer may be invalidated by mutations/movements of the underlying
+  // buffer. Returns nullptr if the field is not set.
+  uint8_t *mutable_field() {
+    return has_field() ? MutableGet<uint8_t>(kInlineAbsoluteOffset_field)
+                       : nullptr;
+  }
+
+  // Clears the field field. This will cause has_field() to return false.
+  void clear_field() { ClearField(kInlineAbsoluteOffset_field, 1, 4); }
+
+  // Returns true if the field field is set and can be accessed.
+  bool has_field() const { return AsFlatbuffer().has_field(); }
+
+  // Clears every field of the table, removing any existing state.
+  void Clear() { clear_field(); }
+
+  // Copies the contents of the provided flatbuffer into this flatbuffer,
+  // returning true on success.
+  // This is a deep copy, and will call FromFlatbuffer on any constituent
+  // objects.
+  [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+    Clear();
+
+    if (other->has_field()) {
+      set_field(other->field());
+    }
+
+    return true;
+  }
+
+ private:
+  // We need to provide a MoveConstructor to allow this table to be
+  // used inside of vectors, but we do not want it readily available to
+  // users. See TableMover for more details.
+  MinimallyAlignedTableStatic(MinimallyAlignedTableStatic &&) = default;
+  friend struct ::aos::fbs::internal::TableMover<MinimallyAlignedTableStatic>;
+
+  // Offset from the start of the buffer to the inline data for the field field.
+  static constexpr size_t kInlineAbsoluteOffset_field = 4;
+
+  // This object has no non-inline subobjects, so we don't have to do anything
+  // special.
+  size_t NumberOfSubObjects() const final { return 0; }
+  using ::aos::fbs::ResizeableObject::SubObject;
+  SubObject GetSubObject(size_t) final { LOG(FATAL) << "No subobjects."; }
+};
+}  // namespace aos::fbs::testing
+
+namespace aos::fbs::testing {
+class SubTableStatic : public ::aos::fbs::Table {
+ public:
+  // The underlying "raw" flatbuffer type for this type.
+  typedef aos::fbs::testing::SubTable Flatbuffer;
+  // Returns this object as a flatbuffer type. This reference may not be valid
+  // following mutations to the underlying flatbuffer, due to how memory may get
+  // may get moved around.
+  const Flatbuffer &AsFlatbuffer() const {
+    return *GetFlatbuffer<Flatbuffer>();
+  }
+
+  // Space taken up by the inline portion of the flatbuffer table data, in
+  // bytes.
+  static constexpr size_t kInlineDataSize = 10;
+  // Space taken up by the vtable for this object, in bytes.
+  static constexpr size_t kVtableSize =
+      sizeof(::flatbuffers::voffset_t) * (2 + 3);
+  // Offset from the start of the internal memory buffer to the start of the
+  // vtable.
+  static constexpr size_t kVtableStart = ::aos::fbs::PaddedSize(
+      kInlineDataSize, alignof(::flatbuffers::voffset_t));
+  // Required alignment of this object. The buffer that this object gets
+  // constructed into must be aligned to this value.
+  static constexpr size_t kAlign = std::max<size_t>({kMinAlign, 4, 2});
+
+  // Nominal size of this object, in bytes. The object may grow beyond this
+  // size, but will always start at this size and so the initial buffer must
+  // match this size.
+  static constexpr size_t kSize = ::aos::fbs::PaddedSize(
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) + 0, kAlign);
+  static_assert(
+      1 <= kAlign,
+      "Flatbuffer schema minalign should not exceed our required alignment.");
+  // Offset from the start of the memory buffer to the start of any out-of-line
+  // data (subtables, vectors, strings).
+  static constexpr size_t kOffsetDataStart =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign);
+  // Size required for a buffer that includes a root table offset at the start.
+  static constexpr size_t kRootSize =
+      ::aos::fbs::PaddedSize(kSize + sizeof(::flatbuffers::uoffset_t), kAlign);
+  // Minimum size required to build this flatbuffer in an entirely unaligned
+  // buffer (including the root table offset). Made to be a multiple of kAlign
+  // for convenience.
+  static constexpr size_t kUnalignedBufferSize = kRootSize + kAlign;
+  // Offset at which the table vtable offset occurs. This is only needed for
+  // vectors.
+  static constexpr size_t kOffset = 0;
+  // Various overrides to support the Table parent class.
+  size_t FixedVtableOffset() const final { return kVtableStart; }
+  size_t VtableSize() const final { return kVtableSize; }
+  size_t InlineTableSize() const final { return kInlineDataSize; }
+  size_t OffsetDataStart() const final { return kOffsetDataStart; }
+  size_t Alignment() const final { return kAlign; }
+  // Exposes the name of the flatbuffer type to allow interchangeable use
+  // of the Flatbuffer and FlatbufferStatic types in various AOS methods.
+  static const char *GetFullyQualifiedName() {
+    return Flatbuffer::GetFullyQualifiedName();
+  }
+
+  // Constructors for creating a flatbuffer object.
+  // Users should typically use the Builder class to create these objects,
+  // in order to allow it to populate the root table offset.
+
+  // The buffer provided to these constructors should be aligned to kAlign
+  // and kSize in length.
+  // The parent/allocator may not be nullptr.
+  SubTableStatic(std::span<uint8_t> buffer,
+                 ::aos::fbs::ResizeableObject *parent)
+      : Table(buffer, parent) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+  SubTableStatic(std::span<uint8_t> buffer, ::aos::fbs::Allocator *allocator)
+      : Table(buffer, allocator) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+  SubTableStatic(std::span<uint8_t> buffer,
+                 ::std::unique_ptr<::aos::fbs::Allocator> allocator)
+      : Table(buffer, ::std::move(allocator)) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+
+  virtual ~SubTableStatic() {}
+
+  // Sets the baz field, causing it to be populated if it is not already.
+  // This will populate the field even if the specified value is the default.
+  void set_baz(const float &value) {
+    SetField<float>(kInlineAbsoluteOffset_baz, 8, value);
+  }
+
+  // Returns the value of baz if set; nullopt otherwise.
+  std::optional<float> baz() const {
+    return has_baz() ? std::make_optional(Get<float>(kInlineAbsoluteOffset_baz))
+                     : std::nullopt;
+    ;
+  }
+  // Returns a pointer to modify the baz field.
+  // The pointer may be invalidated by mutations/movements of the underlying
+  // buffer. Returns nullptr if the field is not set.
+  float *mutable_baz() {
+    return has_baz() ? MutableGet<float>(kInlineAbsoluteOffset_baz) : nullptr;
+  }
+
+  // Clears the baz field. This will cause has_baz() to return false.
+  void clear_baz() { ClearField(kInlineAbsoluteOffset_baz, 4, 8); }
+
+  // Returns true if the baz field is set and can be accessed.
+  bool has_baz() const { return AsFlatbuffer().has_baz(); }
+
+  // Sets the foo field, causing it to be populated if it is not already.
+  // This will populate the field even if the specified value is the default.
+  void set_foo(const int16_t &value) {
+    SetField<int16_t>(kInlineAbsoluteOffset_foo, 4, value);
+  }
+
+  // Returns the value of foo if set; nullopt otherwise.
+  std::optional<int16_t> foo() const {
+    return has_foo()
+               ? std::make_optional(Get<int16_t>(kInlineAbsoluteOffset_foo))
+               : std::nullopt;
+    ;
+  }
+  // Returns a pointer to modify the foo field.
+  // The pointer may be invalidated by mutations/movements of the underlying
+  // buffer. Returns nullptr if the field is not set.
+  int16_t *mutable_foo() {
+    return has_foo() ? MutableGet<int16_t>(kInlineAbsoluteOffset_foo) : nullptr;
+  }
+
+  // Clears the foo field. This will cause has_foo() to return false.
+  void clear_foo() { ClearField(kInlineAbsoluteOffset_foo, 2, 4); }
+
+  // Returns true if the foo field is set and can be accessed.
+  bool has_foo() const { return AsFlatbuffer().has_foo(); }
+
+  // Clears every field of the table, removing any existing state.
+  void Clear() {
+    clear_baz();
+    clear_foo();
+  }
+
+  // Copies the contents of the provided flatbuffer into this flatbuffer,
+  // returning true on success.
+  // This is a deep copy, and will call FromFlatbuffer on any constituent
+  // objects.
+  [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+    Clear();
+
+    if (other->has_baz()) {
+      set_baz(other->baz());
+    }
+
+    if (other->has_foo()) {
+      set_foo(other->foo());
+    }
+
+    return true;
+  }
+
+ private:
+  // We need to provide a MoveConstructor to allow this table to be
+  // used inside of vectors, but we do not want it readily available to
+  // users. See TableMover for more details.
+  SubTableStatic(SubTableStatic &&) = default;
+  friend struct ::aos::fbs::internal::TableMover<SubTableStatic>;
+
+  // Offset from the start of the buffer to the inline data for the baz field.
+  static constexpr size_t kInlineAbsoluteOffset_baz = 4;
+
+  // Offset from the start of the buffer to the inline data for the foo field.
+  static constexpr size_t kInlineAbsoluteOffset_foo = 8;
+
+  // This object has no non-inline subobjects, so we don't have to do anything
+  // special.
+  size_t NumberOfSubObjects() const final { return 0; }
+  using ::aos::fbs::ResizeableObject::SubObject;
+  SubObject GetSubObject(size_t) final { LOG(FATAL) << "No subobjects."; }
+};
+}  // namespace aos::fbs::testing
+
+namespace aos::fbs::testing {
+class TestTableStatic : public ::aos::fbs::Table {
+ public:
+  // The underlying "raw" flatbuffer type for this type.
+  typedef aos::fbs::testing::TestTable Flatbuffer;
+  // Returns this object as a flatbuffer type. This reference may not be valid
+  // following mutations to the underlying flatbuffer, due to how memory may get
+  // may get moved around.
+  const Flatbuffer &AsFlatbuffer() const {
+    return *GetFlatbuffer<Flatbuffer>();
+  }
+
+  // Space taken up by the inline portion of the flatbuffer table data, in
+  // bytes.
+  static constexpr size_t kInlineDataSize = 68;
+  // Space taken up by the vtable for this object, in bytes.
+  static constexpr size_t kVtableSize =
+      sizeof(::flatbuffers::voffset_t) * (2 + 13);
+  // Offset from the start of the internal memory buffer to the start of the
+  // vtable.
+  static constexpr size_t kVtableStart = ::aos::fbs::PaddedSize(
+      kInlineDataSize, alignof(::flatbuffers::voffset_t));
+  // Required alignment of this object. The buffer that this object gets
+  // constructed into must be aligned to this value.
+  static constexpr size_t kAlign = std::max<size_t>(
+      {kMinAlign, aos::fbs::testing::included::IncludedTableStatic::kAlign, 4,
+       ::aos::fbs::String<20>::kAlign, 8,
+       aos::fbs::testing::SubTableStatic::kAlign, ::aos::fbs::String<0>::kAlign,
+       ::aos::fbs::Vector<uint8_t, 0, true, 0>::kAlign,
+       ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0>::kAlign,
+       ::aos::fbs::Vector<int32_t, 3, true, 64>::kAlign,
+       ::aos::fbs::Vector<int32_t, 3, true, 0>::kAlign,
+       ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0>::kAlign,
+       ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true, 0>::kAlign,
+       ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3, false,
+                          0>::kAlign});
+
+  // Nominal size of this object, in bytes. The object may grow beyond this
+  // size, but will always start at this size and so the initial buffer must
+  // match this size.
+  static constexpr size_t kSize = ::aos::fbs::PaddedSize(
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(
+                      ::aos::fbs::PaddedSize(
+                          ::aos::fbs::PaddedSize(
+                              ::aos::fbs::PaddedSize(
+                                  ::aos::fbs::PaddedSize(
+                                      ::aos::fbs::PaddedSize(
+                                          ::aos::fbs::PaddedSize(
+                                              ::aos::fbs::PaddedSize(
+                                                  ::aos::fbs::PaddedSize(
+                                                      0, kAlign) +
+                                                      aos::fbs::testing::included::
+                                                          IncludedTableStatic::
+                                                              kSize,
+                                                  kAlign) +
+                                                  ::aos::fbs::String<20>::kSize,
+                                              kAlign) +
+                                              aos::fbs::testing::
+                                                  SubTableStatic::kSize,
+                                          kAlign) +
+                                          ::aos::fbs::String<0>::kSize,
+                                      kAlign) +
+                                      ::aos::fbs::Vector<uint8_t, 0, true,
+                                                         0>::kSize,
+                                  kAlign) +
+                                  ::aos::fbs::Vector<::aos::fbs::String<0>, 0,
+                                                     false, 0>::kSize,
+                              kAlign) +
+                              ::aos::fbs::Vector<int32_t, 3, true, 64>::kSize,
+                          kAlign) +
+                          ::aos::fbs::Vector<int32_t, 3, true, 0>::kSize,
+                      kAlign) +
+                      ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false,
+                                         0>::kSize,
+                  kAlign) +
+                  ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true,
+                                     0>::kSize,
+              kAlign) +
+          ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3, false,
+                             0>::kSize,
+      kAlign);
+  static_assert(
+      1 <= kAlign,
+      "Flatbuffer schema minalign should not exceed our required alignment.");
+  // Offset from the start of the memory buffer to the start of any out-of-line
+  // data (subtables, vectors, strings).
+  static constexpr size_t kOffsetDataStart =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign);
+  // Size required for a buffer that includes a root table offset at the start.
+  static constexpr size_t kRootSize =
+      ::aos::fbs::PaddedSize(kSize + sizeof(::flatbuffers::uoffset_t), kAlign);
+  // Minimum size required to build this flatbuffer in an entirely unaligned
+  // buffer (including the root table offset). Made to be a multiple of kAlign
+  // for convenience.
+  static constexpr size_t kUnalignedBufferSize = kRootSize + kAlign;
+  // Offset at which the table vtable offset occurs. This is only needed for
+  // vectors.
+  static constexpr size_t kOffset = 0;
+  // Various overrides to support the Table parent class.
+  size_t FixedVtableOffset() const final { return kVtableStart; }
+  size_t VtableSize() const final { return kVtableSize; }
+  size_t InlineTableSize() const final { return kInlineDataSize; }
+  size_t OffsetDataStart() const final { return kOffsetDataStart; }
+  size_t Alignment() const final { return kAlign; }
+  // Exposes the name of the flatbuffer type to allow interchangeable use
+  // of the Flatbuffer and FlatbufferStatic types in various AOS methods.
+  static const char *GetFullyQualifiedName() {
+    return Flatbuffer::GetFullyQualifiedName();
+  }
+
+  // Constructors for creating a flatbuffer object.
+  // Users should typically use the Builder class to create these objects,
+  // in order to allow it to populate the root table offset.
+
+  // The buffer provided to these constructors should be aligned to kAlign
+  // and kSize in length.
+  // The parent/allocator may not be nullptr.
+  TestTableStatic(std::span<uint8_t> buffer,
+                  ::aos::fbs::ResizeableObject *parent)
+      : Table(buffer, parent) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+  TestTableStatic(std::span<uint8_t> buffer, ::aos::fbs::Allocator *allocator)
+      : Table(buffer, allocator) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+  TestTableStatic(std::span<uint8_t> buffer,
+                  ::std::unique_ptr<::aos::fbs::Allocator> allocator)
+      : Table(buffer, ::std::move(allocator)) {
+    CHECK_EQ(buffer.size(), kSize);
+    CHECK_EQ(0u, reinterpret_cast<size_t>(buffer.data()) % kAlign);
+    PopulateVtable();
+  }
+
+  virtual ~TestTableStatic() {}
+
+  // Creates an empty object for the included_table field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  aos::fbs::testing::included::IncludedTableStatic *add_included_table() {
+    CHECK(!included_table_.has_value());
+    constexpr size_t kVtableIndex = 22;
+    // Construct the *Static object that we will use for managing this subtable.
+    included_table_.emplace(
+        BufferForObject(object_absolute_offset_included_table,
+                        aos::fbs::testing::included::IncludedTableStatic::kSize,
+                        kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_included_table, kVtableIndex,
+        object_absolute_offset_included_table +
+            aos::fbs::testing::included::IncludedTableStatic::kOffset -
+            kInlineAbsoluteOffset_included_table);
+    return &included_table_.value().t;
+  }
+
+  // Returns a pointer to the included_table field, if set. nullptr otherwise.
+  const aos::fbs::testing::included::IncludedTableStatic *included_table()
+      const {
+    return included_table_.has_value() ? &included_table_.value().t : nullptr;
+  }
+  aos::fbs::testing::included::IncludedTableStatic *mutable_included_table() {
+    return included_table_.has_value() ? &included_table_.value().t : nullptr;
+  }
+
+  // Clears the included_table field. This will cause has_included_table() to
+  // return false.
+  void clear_included_table() {
+    included_table_.reset();
+    ClearField(kInlineAbsoluteOffset_included_table, 4, 22);
+  }
+
+  // Returns true if the included_table field is set and can be accessed.
+  bool has_included_table() const {
+    return AsFlatbuffer().has_included_table();
+  }
+
+  // Sets the scalar field, causing it to be populated if it is not already.
+  // This will populate the field even if the specified value is the default.
+  void set_scalar(const int32_t &value) {
+    SetField<int32_t>(kInlineAbsoluteOffset_scalar, 4, value);
+  }
+
+  // Returns the value of scalar if set; nullopt otherwise.
+  std::optional<int32_t> scalar() const {
+    return has_scalar()
+               ? std::make_optional(Get<int32_t>(kInlineAbsoluteOffset_scalar))
+               : std::nullopt;
+    ;
+  }
+  // Returns a pointer to modify the scalar field.
+  // The pointer may be invalidated by mutations/movements of the underlying
+  // buffer. Returns nullptr if the field is not set.
+  int32_t *mutable_scalar() {
+    return has_scalar() ? MutableGet<int32_t>(kInlineAbsoluteOffset_scalar)
+                        : nullptr;
+  }
+
+  // Clears the scalar field. This will cause has_scalar() to return false.
+  void clear_scalar() { ClearField(kInlineAbsoluteOffset_scalar, 4, 4); }
+
+  // Returns true if the scalar field is set and can be accessed.
+  bool has_scalar() const { return AsFlatbuffer().has_scalar(); }
+
+  // Creates an empty object for the string field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  ::aos::fbs::String<20> *add_string() {
+    CHECK(!string_.has_value());
+    constexpr size_t kVtableIndex = 8;
+    // Construct the *Static object that we will use for managing this subtable.
+    string_.emplace(BufferForObject(object_absolute_offset_string,
+                                    ::aos::fbs::String<20>::kSize, kAlign),
+                    this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_string, kVtableIndex,
+        object_absolute_offset_string + ::aos::fbs::String<20>::kOffset -
+            kInlineAbsoluteOffset_string);
+    return &string_.value().t;
+  }
+
+  // Returns a pointer to the string field, if set. nullptr otherwise.
+  const ::aos::fbs::String<20> *string() const {
+    return string_.has_value() ? &string_.value().t : nullptr;
+  }
+  ::aos::fbs::String<20> *mutable_string() {
+    return string_.has_value() ? &string_.value().t : nullptr;
+  }
+
+  // Clears the string field. This will cause has_string() to return false.
+  void clear_string() {
+    string_.reset();
+    ClearField(kInlineAbsoluteOffset_string, 4, 8);
+  }
+
+  // Returns true if the string field is set and can be accessed.
+  bool has_string() const { return AsFlatbuffer().has_string(); }
+
+  // Sets the substruct field, causing it to be populated if it is not already.
+  // This will populate the field even if the specified value is the default.
+  void set_substruct(const aos::fbs::testing::SubStruct &value) {
+    SetField<aos::fbs::testing::SubStruct>(kInlineAbsoluteOffset_substruct, 12,
+                                           value);
+  }
+
+  // Returns the value of substruct if set; nullopt otherwise.
+  std::optional<aos::fbs::testing::SubStruct> substruct() const {
+    return has_substruct()
+               ? std::make_optional(Get<aos::fbs::testing::SubStruct>(
+                     kInlineAbsoluteOffset_substruct))
+               : std::nullopt;
+    ;
+  }
+  // Returns a pointer to modify the substruct field.
+  // The pointer may be invalidated by mutations/movements of the underlying
+  // buffer. Returns nullptr if the field is not set.
+  aos::fbs::testing::SubStruct *mutable_substruct() {
+    return has_substruct() ? MutableGet<aos::fbs::testing::SubStruct>(
+                                 kInlineAbsoluteOffset_substruct)
+                           : nullptr;
+  }
+
+  // Clears the substruct field. This will cause has_substruct() to return
+  // false.
+  void clear_substruct() {
+    ClearField(kInlineAbsoluteOffset_substruct, 16, 12);
+  }
+
+  // Returns true if the substruct field is set and can be accessed.
+  bool has_substruct() const { return AsFlatbuffer().has_substruct(); }
+
+  // Creates an empty object for the subtable field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  aos::fbs::testing::SubTableStatic *add_subtable() {
+    CHECK(!subtable_.has_value());
+    constexpr size_t kVtableIndex = 14;
+    // Construct the *Static object that we will use for managing this subtable.
+    subtable_.emplace(
+        BufferForObject(object_absolute_offset_subtable,
+                        aos::fbs::testing::SubTableStatic::kSize, kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_subtable, kVtableIndex,
+        object_absolute_offset_subtable +
+            aos::fbs::testing::SubTableStatic::kOffset -
+            kInlineAbsoluteOffset_subtable);
+    return &subtable_.value().t;
+  }
+
+  // Returns a pointer to the subtable field, if set. nullptr otherwise.
+  const aos::fbs::testing::SubTableStatic *subtable() const {
+    return subtable_.has_value() ? &subtable_.value().t : nullptr;
+  }
+  aos::fbs::testing::SubTableStatic *mutable_subtable() {
+    return subtable_.has_value() ? &subtable_.value().t : nullptr;
+  }
+
+  // Clears the subtable field. This will cause has_subtable() to return false.
+  void clear_subtable() {
+    subtable_.reset();
+    ClearField(kInlineAbsoluteOffset_subtable, 4, 14);
+  }
+
+  // Returns true if the subtable field is set and can be accessed.
+  bool has_subtable() const { return AsFlatbuffer().has_subtable(); }
+
+  // Creates an empty object for the unspecified_length_string field, which you
+  // can then populate/modify as desired. The field must not be populated yet.
+  ::aos::fbs::String<0> *add_unspecified_length_string() {
+    CHECK(!unspecified_length_string_.has_value());
+    constexpr size_t kVtableIndex = 26;
+    // Construct the *Static object that we will use for managing this subtable.
+    unspecified_length_string_.emplace(
+        BufferForObject(object_absolute_offset_unspecified_length_string,
+                        ::aos::fbs::String<0>::kSize, kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_unspecified_length_string, kVtableIndex,
+        object_absolute_offset_unspecified_length_string +
+            ::aos::fbs::String<0>::kOffset -
+            kInlineAbsoluteOffset_unspecified_length_string);
+    return &unspecified_length_string_.value().t;
+  }
+
+  // Returns a pointer to the unspecified_length_string field, if set. nullptr
+  // otherwise.
+  const ::aos::fbs::String<0> *unspecified_length_string() const {
+    return unspecified_length_string_.has_value()
+               ? &unspecified_length_string_.value().t
+               : nullptr;
+  }
+  ::aos::fbs::String<0> *mutable_unspecified_length_string() {
+    return unspecified_length_string_.has_value()
+               ? &unspecified_length_string_.value().t
+               : nullptr;
+  }
+
+  // Clears the unspecified_length_string field. This will cause
+  // has_unspecified_length_string() to return false.
+  void clear_unspecified_length_string() {
+    unspecified_length_string_.reset();
+    ClearField(kInlineAbsoluteOffset_unspecified_length_string, 4, 26);
+  }
+
+  // Returns true if the unspecified_length_string field is set and can be
+  // accessed.
+  bool has_unspecified_length_string() const {
+    return AsFlatbuffer().has_unspecified_length_string();
+  }
+
+  // Creates an empty object for the unspecified_length_vector field, which you
+  // can then populate/modify as desired. The field must not be populated yet.
+  ::aos::fbs::Vector<uint8_t, 0, true, 0> *add_unspecified_length_vector() {
+    CHECK(!unspecified_length_vector_.has_value());
+    constexpr size_t kVtableIndex = 24;
+    // Construct the *Static object that we will use for managing this subtable.
+    unspecified_length_vector_.emplace(
+        BufferForObject(object_absolute_offset_unspecified_length_vector,
+                        ::aos::fbs::Vector<uint8_t, 0, true, 0>::kSize, kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_unspecified_length_vector, kVtableIndex,
+        object_absolute_offset_unspecified_length_vector +
+            ::aos::fbs::Vector<uint8_t, 0, true, 0>::kOffset -
+            kInlineAbsoluteOffset_unspecified_length_vector);
+    return &unspecified_length_vector_.value().t;
+  }
+
+  // Returns a pointer to the unspecified_length_vector field, if set. nullptr
+  // otherwise.
+  const ::aos::fbs::Vector<uint8_t, 0, true, 0> *unspecified_length_vector()
+      const {
+    return unspecified_length_vector_.has_value()
+               ? &unspecified_length_vector_.value().t
+               : nullptr;
+  }
+  ::aos::fbs::Vector<uint8_t, 0, true, 0> *mutable_unspecified_length_vector() {
+    return unspecified_length_vector_.has_value()
+               ? &unspecified_length_vector_.value().t
+               : nullptr;
+  }
+
+  // Clears the unspecified_length_vector field. This will cause
+  // has_unspecified_length_vector() to return false.
+  void clear_unspecified_length_vector() {
+    unspecified_length_vector_.reset();
+    ClearField(kInlineAbsoluteOffset_unspecified_length_vector, 4, 24);
+  }
+
+  // Returns true if the unspecified_length_vector field is set and can be
+  // accessed.
+  bool has_unspecified_length_vector() const {
+    return AsFlatbuffer().has_unspecified_length_vector();
+  }
+
+  // Creates an empty object for the unspecified_length_vector_of_strings field,
+  // which you can then populate/modify as desired. The field must not be
+  // populated yet.
+  ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0> *
+  add_unspecified_length_vector_of_strings() {
+    CHECK(!unspecified_length_vector_of_strings_.has_value());
+    constexpr size_t kVtableIndex = 28;
+    // Construct the *Static object that we will use for managing this subtable.
+    unspecified_length_vector_of_strings_.emplace(
+        BufferForObject(
+            object_absolute_offset_unspecified_length_vector_of_strings,
+            ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0>::kSize,
+            kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_unspecified_length_vector_of_strings,
+        kVtableIndex,
+        object_absolute_offset_unspecified_length_vector_of_strings +
+            ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0>::kOffset -
+            kInlineAbsoluteOffset_unspecified_length_vector_of_strings);
+    return &unspecified_length_vector_of_strings_.value().t;
+  }
+
+  // Returns a pointer to the unspecified_length_vector_of_strings field, if
+  // set. nullptr otherwise.
+  const ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0> *
+  unspecified_length_vector_of_strings() const {
+    return unspecified_length_vector_of_strings_.has_value()
+               ? &unspecified_length_vector_of_strings_.value().t
+               : nullptr;
+  }
+  ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0> *
+  mutable_unspecified_length_vector_of_strings() {
+    return unspecified_length_vector_of_strings_.has_value()
+               ? &unspecified_length_vector_of_strings_.value().t
+               : nullptr;
+  }
+
+  // Clears the unspecified_length_vector_of_strings field. This will cause
+  // has_unspecified_length_vector_of_strings() to return false.
+  void clear_unspecified_length_vector_of_strings() {
+    unspecified_length_vector_of_strings_.reset();
+    ClearField(kInlineAbsoluteOffset_unspecified_length_vector_of_strings, 4,
+               28);
+  }
+
+  // Returns true if the unspecified_length_vector_of_strings field is set and
+  // can be accessed.
+  bool has_unspecified_length_vector_of_strings() const {
+    return AsFlatbuffer().has_unspecified_length_vector_of_strings();
+  }
+
+  // Creates an empty object for the vector_aligned field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  ::aos::fbs::Vector<int32_t, 3, true, 64> *add_vector_aligned() {
+    CHECK(!vector_aligned_.has_value());
+    constexpr size_t kVtableIndex = 16;
+    // Construct the *Static object that we will use for managing this subtable.
+    vector_aligned_.emplace(
+        BufferForObject(object_absolute_offset_vector_aligned,
+                        ::aos::fbs::Vector<int32_t, 3, true, 64>::kSize,
+                        kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_vector_aligned, kVtableIndex,
+        object_absolute_offset_vector_aligned +
+            ::aos::fbs::Vector<int32_t, 3, true, 64>::kOffset -
+            kInlineAbsoluteOffset_vector_aligned);
+    return &vector_aligned_.value().t;
+  }
+
+  // Returns a pointer to the vector_aligned field, if set. nullptr otherwise.
+  const ::aos::fbs::Vector<int32_t, 3, true, 64> *vector_aligned() const {
+    return vector_aligned_.has_value() ? &vector_aligned_.value().t : nullptr;
+  }
+  ::aos::fbs::Vector<int32_t, 3, true, 64> *mutable_vector_aligned() {
+    return vector_aligned_.has_value() ? &vector_aligned_.value().t : nullptr;
+  }
+
+  // Clears the vector_aligned field. This will cause has_vector_aligned() to
+  // return false.
+  void clear_vector_aligned() {
+    vector_aligned_.reset();
+    ClearField(kInlineAbsoluteOffset_vector_aligned, 4, 16);
+  }
+
+  // Returns true if the vector_aligned field is set and can be accessed.
+  bool has_vector_aligned() const {
+    return AsFlatbuffer().has_vector_aligned();
+  }
+
+  // Creates an empty object for the vector_of_scalars field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  ::aos::fbs::Vector<int32_t, 3, true, 0> *add_vector_of_scalars() {
+    CHECK(!vector_of_scalars_.has_value());
+    constexpr size_t kVtableIndex = 6;
+    // Construct the *Static object that we will use for managing this subtable.
+    vector_of_scalars_.emplace(
+        BufferForObject(object_absolute_offset_vector_of_scalars,
+                        ::aos::fbs::Vector<int32_t, 3, true, 0>::kSize, kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_vector_of_scalars, kVtableIndex,
+        object_absolute_offset_vector_of_scalars +
+            ::aos::fbs::Vector<int32_t, 3, true, 0>::kOffset -
+            kInlineAbsoluteOffset_vector_of_scalars);
+    return &vector_of_scalars_.value().t;
+  }
+
+  // Returns a pointer to the vector_of_scalars field, if set. nullptr
+  // otherwise.
+  const ::aos::fbs::Vector<int32_t, 3, true, 0> *vector_of_scalars() const {
+    return vector_of_scalars_.has_value() ? &vector_of_scalars_.value().t
+                                          : nullptr;
+  }
+  ::aos::fbs::Vector<int32_t, 3, true, 0> *mutable_vector_of_scalars() {
+    return vector_of_scalars_.has_value() ? &vector_of_scalars_.value().t
+                                          : nullptr;
+  }
+
+  // Clears the vector_of_scalars field. This will cause has_vector_of_scalars()
+  // to return false.
+  void clear_vector_of_scalars() {
+    vector_of_scalars_.reset();
+    ClearField(kInlineAbsoluteOffset_vector_of_scalars, 4, 6);
+  }
+
+  // Returns true if the vector_of_scalars field is set and can be accessed.
+  bool has_vector_of_scalars() const {
+    return AsFlatbuffer().has_vector_of_scalars();
+  }
+
+  // Creates an empty object for the vector_of_strings field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0> *
+  add_vector_of_strings() {
+    CHECK(!vector_of_strings_.has_value());
+    constexpr size_t kVtableIndex = 10;
+    // Construct the *Static object that we will use for managing this subtable.
+    vector_of_strings_.emplace(
+        BufferForObject(
+            object_absolute_offset_vector_of_strings,
+            ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0>::kSize,
+            kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_vector_of_strings, kVtableIndex,
+        object_absolute_offset_vector_of_strings +
+            ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0>::kOffset -
+            kInlineAbsoluteOffset_vector_of_strings);
+    return &vector_of_strings_.value().t;
+  }
+
+  // Returns a pointer to the vector_of_strings field, if set. nullptr
+  // otherwise.
+  const ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0> *
+  vector_of_strings() const {
+    return vector_of_strings_.has_value() ? &vector_of_strings_.value().t
+                                          : nullptr;
+  }
+  ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0> *
+  mutable_vector_of_strings() {
+    return vector_of_strings_.has_value() ? &vector_of_strings_.value().t
+                                          : nullptr;
+  }
+
+  // Clears the vector_of_strings field. This will cause has_vector_of_strings()
+  // to return false.
+  void clear_vector_of_strings() {
+    vector_of_strings_.reset();
+    ClearField(kInlineAbsoluteOffset_vector_of_strings, 4, 10);
+  }
+
+  // Returns true if the vector_of_strings field is set and can be accessed.
+  bool has_vector_of_strings() const {
+    return AsFlatbuffer().has_vector_of_strings();
+  }
+
+  // Creates an empty object for the vector_of_structs field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true, 0> *
+  add_vector_of_structs() {
+    CHECK(!vector_of_structs_.has_value());
+    constexpr size_t kVtableIndex = 18;
+    // Construct the *Static object that we will use for managing this subtable.
+    vector_of_structs_.emplace(
+        BufferForObject(
+            object_absolute_offset_vector_of_structs,
+            ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true, 0>::kSize,
+            kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_vector_of_structs, kVtableIndex,
+        object_absolute_offset_vector_of_structs +
+            ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true,
+                               0>::kOffset -
+            kInlineAbsoluteOffset_vector_of_structs);
+    return &vector_of_structs_.value().t;
+  }
+
+  // Returns a pointer to the vector_of_structs field, if set. nullptr
+  // otherwise.
+  const ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true, 0> *
+  vector_of_structs() const {
+    return vector_of_structs_.has_value() ? &vector_of_structs_.value().t
+                                          : nullptr;
+  }
+  ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true, 0> *
+  mutable_vector_of_structs() {
+    return vector_of_structs_.has_value() ? &vector_of_structs_.value().t
+                                          : nullptr;
+  }
+
+  // Clears the vector_of_structs field. This will cause has_vector_of_structs()
+  // to return false.
+  void clear_vector_of_structs() {
+    vector_of_structs_.reset();
+    ClearField(kInlineAbsoluteOffset_vector_of_structs, 4, 18);
+  }
+
+  // Returns true if the vector_of_structs field is set and can be accessed.
+  bool has_vector_of_structs() const {
+    return AsFlatbuffer().has_vector_of_structs();
+  }
+
+  // Creates an empty object for the vector_of_tables field, which you can
+  // then populate/modify as desired.
+  // The field must not be populated yet.
+  ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3, false, 0> *
+  add_vector_of_tables() {
+    CHECK(!vector_of_tables_.has_value());
+    constexpr size_t kVtableIndex = 20;
+    // Construct the *Static object that we will use for managing this subtable.
+    vector_of_tables_.emplace(
+        BufferForObject(object_absolute_offset_vector_of_tables,
+                        ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3,
+                                           false, 0>::kSize,
+                        kAlign),
+        this);
+    // Actually set the appropriate fields in the flatbuffer memory itself.
+    SetField<::flatbuffers::uoffset_t>(
+        kInlineAbsoluteOffset_vector_of_tables, kVtableIndex,
+        object_absolute_offset_vector_of_tables +
+            ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3, false,
+                               0>::kOffset -
+            kInlineAbsoluteOffset_vector_of_tables);
+    return &vector_of_tables_.value().t;
+  }
+
+  // Returns a pointer to the vector_of_tables field, if set. nullptr otherwise.
+  const ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3, false, 0> *
+  vector_of_tables() const {
+    return vector_of_tables_.has_value() ? &vector_of_tables_.value().t
+                                         : nullptr;
+  }
+  ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3, false, 0> *
+  mutable_vector_of_tables() {
+    return vector_of_tables_.has_value() ? &vector_of_tables_.value().t
+                                         : nullptr;
+  }
+
+  // Clears the vector_of_tables field. This will cause has_vector_of_tables()
+  // to return false.
+  void clear_vector_of_tables() {
+    vector_of_tables_.reset();
+    ClearField(kInlineAbsoluteOffset_vector_of_tables, 4, 20);
+  }
+
+  // Returns true if the vector_of_tables field is set and can be accessed.
+  bool has_vector_of_tables() const {
+    return AsFlatbuffer().has_vector_of_tables();
+  }
+
+  // Clears every field of the table, removing any existing state.
+  void Clear() {
+    clear_included_table();
+    clear_scalar();
+    clear_string();
+    clear_substruct();
+    clear_subtable();
+    clear_unspecified_length_string();
+    clear_unspecified_length_vector();
+    clear_unspecified_length_vector_of_strings();
+    clear_vector_aligned();
+    clear_vector_of_scalars();
+    clear_vector_of_strings();
+    clear_vector_of_structs();
+    clear_vector_of_tables();
+  }
+
+  // Copies the contents of the provided flatbuffer into this flatbuffer,
+  // returning true on success.
+  // This is a deep copy, and will call FromFlatbuffer on any constituent
+  // objects.
+  [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+    Clear();
+
+    if (other->has_included_table()) {
+      if (!CHECK_NOTNULL(add_included_table())
+               ->FromFlatbuffer(other->included_table())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_scalar()) {
+      set_scalar(other->scalar());
+    }
+
+    if (other->has_string()) {
+      if (!CHECK_NOTNULL(add_string())->FromFlatbuffer(other->string())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_substruct()) {
+      set_substruct(*other->substruct());
+    }
+
+    if (other->has_subtable()) {
+      if (!CHECK_NOTNULL(add_subtable())->FromFlatbuffer(other->subtable())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_unspecified_length_string()) {
+      if (!CHECK_NOTNULL(add_unspecified_length_string())
+               ->FromFlatbuffer(other->unspecified_length_string())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_unspecified_length_vector()) {
+      if (!CHECK_NOTNULL(add_unspecified_length_vector())
+               ->FromFlatbuffer(other->unspecified_length_vector())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_unspecified_length_vector_of_strings()) {
+      if (!CHECK_NOTNULL(add_unspecified_length_vector_of_strings())
+               ->FromFlatbuffer(
+                   other->unspecified_length_vector_of_strings())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_vector_aligned()) {
+      if (!CHECK_NOTNULL(add_vector_aligned())
+               ->FromFlatbuffer(other->vector_aligned())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_vector_of_scalars()) {
+      if (!CHECK_NOTNULL(add_vector_of_scalars())
+               ->FromFlatbuffer(other->vector_of_scalars())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_vector_of_strings()) {
+      if (!CHECK_NOTNULL(add_vector_of_strings())
+               ->FromFlatbuffer(other->vector_of_strings())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_vector_of_structs()) {
+      if (!CHECK_NOTNULL(add_vector_of_structs())
+               ->FromFlatbuffer(other->vector_of_structs())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    if (other->has_vector_of_tables()) {
+      if (!CHECK_NOTNULL(add_vector_of_tables())
+               ->FromFlatbuffer(other->vector_of_tables())) {
+        // Fail if we were unable to copy (e.g., if we tried to copy in a long
+        // vector and do not have the space for it).
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+ private:
+  // We need to provide a MoveConstructor to allow this table to be
+  // used inside of vectors, but we do not want it readily available to
+  // users. See TableMover for more details.
+  TestTableStatic(TestTableStatic &&) = default;
+  friend struct ::aos::fbs::internal::TableMover<TestTableStatic>;
+
+  // Members relating to the included_table field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<
+      aos::fbs::testing::included::IncludedTableStatic>>
+      included_table_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_included_table =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(0, kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_included_table = 4;
+
+  // Offset from the start of the buffer to the inline data for the scalar
+  // field.
+  static constexpr size_t kInlineAbsoluteOffset_scalar = 8;
+
+  // Members relating to the string field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<::aos::fbs::String<20>>>
+      string_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_string =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(0, kAlign) +
+              aos::fbs::testing::included::IncludedTableStatic::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_string = 12;
+
+  // Offset from the start of the buffer to the inline data for the substruct
+  // field.
+  static constexpr size_t kInlineAbsoluteOffset_substruct = 16;
+
+  // Members relating to the subtable field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<
+      ::aos::fbs::internal::TableMover<aos::fbs::testing::SubTableStatic>>
+      subtable_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_subtable =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(0, kAlign) +
+                  aos::fbs::testing::included::IncludedTableStatic::kSize,
+              kAlign) +
+              ::aos::fbs::String<20>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_subtable = 32;
+
+  // Members relating to the unspecified_length_string field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<::aos::fbs::String<0>>>
+      unspecified_length_string_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_unspecified_length_string =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(0, kAlign) +
+                      aos::fbs::testing::included::IncludedTableStatic::kSize,
+                  kAlign) +
+                  ::aos::fbs::String<20>::kSize,
+              kAlign) +
+              aos::fbs::testing::SubTableStatic::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_unspecified_length_string = 36;
+
+  // Members relating to the unspecified_length_vector field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<
+      ::aos::fbs::internal::TableMover<::aos::fbs::Vector<uint8_t, 0, true, 0>>>
+      unspecified_length_vector_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_unspecified_length_vector =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(::aos::fbs::PaddedSize(0, kAlign) +
+                                             aos::fbs::testing::included::
+                                                 IncludedTableStatic::kSize,
+                                         kAlign) +
+                      ::aos::fbs::String<20>::kSize,
+                  kAlign) +
+                  aos::fbs::testing::SubTableStatic::kSize,
+              kAlign) +
+              ::aos::fbs::String<0>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_unspecified_length_vector = 40;
+
+  // Members relating to the unspecified_length_vector_of_strings field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<
+      ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0>>>
+      unspecified_length_vector_of_strings_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_unspecified_length_vector_of_strings =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(
+                      ::aos::fbs::PaddedSize(::aos::fbs::PaddedSize(0, kAlign) +
+                                                 aos::fbs::testing::included::
+                                                     IncludedTableStatic::kSize,
+                                             kAlign) +
+                          ::aos::fbs::String<20>::kSize,
+                      kAlign) +
+                      aos::fbs::testing::SubTableStatic::kSize,
+                  kAlign) +
+                  ::aos::fbs::String<0>::kSize,
+              kAlign) +
+              ::aos::fbs::Vector<uint8_t, 0, true, 0>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t
+      kInlineAbsoluteOffset_unspecified_length_vector_of_strings = 44;
+
+  // Members relating to the vector_aligned field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<
+      ::aos::fbs::Vector<int32_t, 3, true, 64>>>
+      vector_aligned_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_vector_aligned =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(
+                      ::aos::fbs::PaddedSize(
+                          ::aos::fbs::PaddedSize(
+                              ::aos::fbs::PaddedSize(0, kAlign) +
+                                  aos::fbs::testing::included::
+                                      IncludedTableStatic::kSize,
+                              kAlign) +
+                              ::aos::fbs::String<20>::kSize,
+                          kAlign) +
+                          aos::fbs::testing::SubTableStatic::kSize,
+                      kAlign) +
+                      ::aos::fbs::String<0>::kSize,
+                  kAlign) +
+                  ::aos::fbs::Vector<uint8_t, 0, true, 0>::kSize,
+              kAlign) +
+              ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_vector_aligned = 48;
+
+  // Members relating to the vector_of_scalars field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<
+      ::aos::fbs::internal::TableMover<::aos::fbs::Vector<int32_t, 3, true, 0>>>
+      vector_of_scalars_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_vector_of_scalars =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(
+                      ::aos::fbs::PaddedSize(
+                          ::aos::fbs::PaddedSize(
+                              ::aos::fbs::PaddedSize(
+                                  ::aos::fbs::PaddedSize(0, kAlign) +
+                                      aos::fbs::testing::included::
+                                          IncludedTableStatic::kSize,
+                                  kAlign) +
+                                  ::aos::fbs::String<20>::kSize,
+                              kAlign) +
+                              aos::fbs::testing::SubTableStatic::kSize,
+                          kAlign) +
+                          ::aos::fbs::String<0>::kSize,
+                      kAlign) +
+                      ::aos::fbs::Vector<uint8_t, 0, true, 0>::kSize,
+                  kAlign) +
+                  ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false, 0>::kSize,
+              kAlign) +
+              ::aos::fbs::Vector<int32_t, 3, true, 64>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_vector_of_scalars = 52;
+
+  // Members relating to the vector_of_strings field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<
+      ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0>>>
+      vector_of_strings_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_vector_of_strings =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(
+                      ::aos::fbs::PaddedSize(
+                          ::aos::fbs::PaddedSize(
+                              ::aos::fbs::PaddedSize(
+                                  ::aos::fbs::PaddedSize(
+                                      ::aos::fbs::PaddedSize(0, kAlign) +
+                                          aos::fbs::testing::included::
+                                              IncludedTableStatic::kSize,
+                                      kAlign) +
+                                      ::aos::fbs::String<20>::kSize,
+                                  kAlign) +
+                                  aos::fbs::testing::SubTableStatic::kSize,
+                              kAlign) +
+                              ::aos::fbs::String<0>::kSize,
+                          kAlign) +
+                          ::aos::fbs::Vector<uint8_t, 0, true, 0>::kSize,
+                      kAlign) +
+                      ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false,
+                                         0>::kSize,
+                  kAlign) +
+                  ::aos::fbs::Vector<int32_t, 3, true, 64>::kSize,
+              kAlign) +
+              ::aos::fbs::Vector<int32_t, 3, true, 0>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_vector_of_strings = 56;
+
+  // Members relating to the vector_of_structs field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<
+      ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true, 0>>>
+      vector_of_structs_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_vector_of_structs =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(
+                      ::aos::fbs::PaddedSize(
+                          ::aos::fbs::PaddedSize(
+                              ::aos::fbs::PaddedSize(
+                                  ::aos::fbs::PaddedSize(
+                                      ::aos::fbs::PaddedSize(
+                                          ::aos::fbs::PaddedSize(0, kAlign) +
+                                              aos::fbs::testing::included::
+                                                  IncludedTableStatic::kSize,
+                                          kAlign) +
+                                          ::aos::fbs::String<20>::kSize,
+                                      kAlign) +
+                                      aos::fbs::testing::SubTableStatic::kSize,
+                                  kAlign) +
+                                  ::aos::fbs::String<0>::kSize,
+                              kAlign) +
+                              ::aos::fbs::Vector<uint8_t, 0, true, 0>::kSize,
+                          kAlign) +
+                          ::aos::fbs::Vector<::aos::fbs::String<0>, 0, false,
+                                             0>::kSize,
+                      kAlign) +
+                      ::aos::fbs::Vector<int32_t, 3, true, 64>::kSize,
+                  kAlign) +
+                  ::aos::fbs::Vector<int32_t, 3, true, 0>::kSize,
+              kAlign) +
+              ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false, 0>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_vector_of_structs = 60;
+
+  // Members relating to the vector_of_tables field.
+  //
+  // *Static object used for managing this subtable. Will be nullopt
+  // when the field is not populated.
+  // We use the TableMover to be able to make this object moveable.
+  std::optional<::aos::fbs::internal::TableMover<
+      ::aos::fbs::Vector<aos::fbs::testing::SubTableStatic, 3, false, 0>>>
+      vector_of_tables_;
+  // Offset from the start of the buffer to the start of the actual
+  // data for this field. Will be updated even when the table is not
+  // populated, so that we know where to construct it when requested.
+  size_t object_absolute_offset_vector_of_tables =
+      ::aos::fbs::PaddedSize(kVtableStart + kVtableSize, kAlign) +
+      ::aos::fbs::PaddedSize(
+          ::aos::fbs::PaddedSize(
+              ::aos::fbs::PaddedSize(
+                  ::aos::fbs::PaddedSize(
+                      ::aos::fbs::PaddedSize(
+                          ::aos::fbs::PaddedSize(
+                              ::aos::fbs::PaddedSize(
+                                  ::aos::fbs::PaddedSize(
+                                      ::aos::fbs::PaddedSize(
+                                          ::aos::fbs::PaddedSize(
+                                              ::aos::fbs::PaddedSize(0,
+                                                                     kAlign) +
+                                                  aos::fbs::testing::included::
+                                                      IncludedTableStatic::
+                                                          kSize,
+                                              kAlign) +
+                                              ::aos::fbs::String<20>::kSize,
+                                          kAlign) +
+                                          aos::fbs::testing::SubTableStatic::
+                                              kSize,
+                                      kAlign) +
+                                      ::aos::fbs::String<0>::kSize,
+                                  kAlign) +
+                                  ::aos::fbs::Vector<uint8_t, 0, true,
+                                                     0>::kSize,
+                              kAlign) +
+                              ::aos::fbs::Vector<::aos::fbs::String<0>, 0,
+                                                 false, 0>::kSize,
+                          kAlign) +
+                          ::aos::fbs::Vector<int32_t, 3, true, 64>::kSize,
+                      kAlign) +
+                      ::aos::fbs::Vector<int32_t, 3, true, 0>::kSize,
+                  kAlign) +
+                  ::aos::fbs::Vector<::aos::fbs::String<10>, 3, false,
+                                     0>::kSize,
+              kAlign) +
+              ::aos::fbs::Vector<aos::fbs::testing::SubStruct, 3, true,
+                                 0>::kSize,
+          kAlign);
+  // Offset from the start of the buffer to the offset in the inline data for
+  // this field.
+  static constexpr size_t kInlineAbsoluteOffset_vector_of_tables = 64;
+
+  size_t NumberOfSubObjects() const final { return 11; }
+  using ::aos::fbs::ResizeableObject::SubObject;
+  SubObject GetSubObject(size_t index) final {
+    SubObject object;
+    // Note: The below arrays are local variables rather than class members to
+    // avoid having to deal with what happens to them if the object is moved.
+
+    // Array of the members that we use for tracking where the buffers for
+    // each subobject belong.
+    // Pointers because these may need to be modified when memory is
+    // inserted into the buffer.
+    const std::array<size_t *, 11> subobject_object_offsets{
+        &object_absolute_offset_included_table,
+        &object_absolute_offset_string,
+        &object_absolute_offset_subtable,
+        &object_absolute_offset_unspecified_length_string,
+        &object_absolute_offset_unspecified_length_vector,
+        &object_absolute_offset_unspecified_length_vector_of_strings,
+        &object_absolute_offset_vector_aligned,
+        &object_absolute_offset_vector_of_scalars,
+        &object_absolute_offset_vector_of_strings,
+        &object_absolute_offset_vector_of_structs,
+        &object_absolute_offset_vector_of_tables};
+    // Actual subobjects; note that the pointers will be invalid when the
+    // field is not populated.
+    const std::array<::aos::fbs::ResizeableObject *, 11> subobject_objects{
+        &included_table_->t,
+        &string_->t,
+        &subtable_->t,
+        &unspecified_length_string_->t,
+        &unspecified_length_vector_->t,
+        &unspecified_length_vector_of_strings_->t,
+        &vector_aligned_->t,
+        &vector_of_scalars_->t,
+        &vector_of_strings_->t,
+        &vector_of_structs_->t,
+        &vector_of_tables_->t};
+    // Absolute offsets from the start of the buffer to where the inline
+    // entry is for each table. These offsets do not need to change at
+    // runtime (because memory is never inserted into the start of
+    // a given table), but the offsets pointed to by these offsets
+    // may need to be updated.
+    const std::array<size_t, 11> subobject_inline_offsets{
+        kInlineAbsoluteOffset_included_table,
+        kInlineAbsoluteOffset_string,
+        kInlineAbsoluteOffset_subtable,
+        kInlineAbsoluteOffset_unspecified_length_string,
+        kInlineAbsoluteOffset_unspecified_length_vector,
+        kInlineAbsoluteOffset_unspecified_length_vector_of_strings,
+        kInlineAbsoluteOffset_vector_aligned,
+        kInlineAbsoluteOffset_vector_of_scalars,
+        kInlineAbsoluteOffset_vector_of_strings,
+        kInlineAbsoluteOffset_vector_of_structs,
+        kInlineAbsoluteOffset_vector_of_tables};
+    object.inline_entry =
+        MutableGet<::flatbuffers::uoffset_t>(subobject_inline_offsets[index]);
+    object.object =
+        (*object.inline_entry == 0) ? nullptr : subobject_objects[index];
+    object.absolute_offset = subobject_object_offsets[index];
+    return object;
+  }
+};
+}  // namespace aos::fbs::testing
diff --git a/aos/flatbuffers/test_dir/type_coverage.fbs b/aos/flatbuffers/test_dir/type_coverage.fbs
new file mode 100644
index 0000000..d364fd5
--- /dev/null
+++ b/aos/flatbuffers/test_dir/type_coverage.fbs
@@ -0,0 +1,152 @@
+// This fbs file attempts to cover a wide variety of flatbuffer scenarios; it
+// is mostly just copied from json_to_flatbuffer.fbs, which itself steals from
+// other sources.
+
+// Use a namespace that has no overlap with the aos::fbs namespace of the underlying code.
+namespace frc971.testing;
+
+enum BaseType : byte {
+    None,
+    UType,
+    Bool,
+    Byte,
+    UByte,
+    Short,
+    UShort,
+    Int,
+    UInt,
+    Long,
+    ULong,
+    Float,
+    Double,
+    String,
+    Vector,
+    Obj,     // Used for tables & structs.
+    Union,
+    Array
+}
+
+enum NonConsecutive : int {
+  Zero = 0,
+  Big = 10000000,
+}
+
+table Location {
+  name:string (id: 0);
+  type:string (id: 1);
+  frequency:int (id: 2);
+  max_size:int (id: 3);
+}
+
+table Map {
+  match:Location (id: 0);
+  rename:Location (id: 1);
+}
+
+table Application {
+  name:string (id: 0);
+  priority:int (id: 1);
+  maps:[Map] (id: 2);
+  long_thingy:uint64 (id: 3);
+}
+
+table VectorOfStrings {
+  str:[string] (id: 0);
+}
+
+table VectorOfVectorOfString {
+  v:[VectorOfStrings] (id: 0);
+}
+
+struct FooStructNested {
+  foo_byte:byte;
+}
+
+struct FooStruct {
+  foo_byte:byte;
+  nested_struct:FooStructNested;
+}
+
+struct ScalarSweepStruct {
+  foo_float:float;
+  foo_double:double;
+  foo_int32:int32;
+  foo_uint32:uint32;
+  foo_int64:int64;
+  foo_uint64:uint64;
+}
+
+struct StructEnum {
+  foo_enum:BaseType;
+}
+
+table Configuration {
+  locations:[Location] (id: 0);
+  maps:[Map] (id: 1);
+  apps:[Application] (id: 2);
+  imports:[string] (id: 3);
+
+  // 8 bit: byte ubyte bool
+  // 16 bit: short ushort
+  // 32 bit: int uint float
+  // 64 bit: long ulong double
+
+  // Simple values.
+  foo_byte:byte (id: 4);
+  foo_ubyte:ubyte (id: 5);
+  foo_bool:bool (id: 6);
+
+  foo_short:short (id: 7);
+  foo_ushort:ushort (id: 8);
+
+  foo_int:int (id: 9);
+  foo_uint:uint (id: 10);
+
+  foo_long:long (id: 11);
+  foo_ulong:ulong (id: 12);
+
+  foo_float:float (id: 13);
+  foo_double:double (id: 14);
+
+  foo_string:string (id: 15);
+
+  foo_enum:BaseType (id: 16);
+  foo_enum_default:BaseType = None (id: 17);
+
+  // Test vectors now.
+  vector_foo_byte:[byte] (id: 18);
+  vector_foo_ubyte:[ubyte] (id: 19);
+  vector_foo_bool:[bool] (id: 20);
+
+  vector_foo_short:[short] (id: 21);
+  vector_foo_ushort:[ushort] (id: 22);
+
+  vector_foo_int:[int] (id: 23);
+  vector_foo_uint:[uint] (id: 24);
+
+  vector_foo_long:[long] (id: 25);
+  vector_foo_ulong:[ulong] (id: 26);
+
+  vector_foo_float:[float] (id: 27);
+  vector_foo_double:[double] (id: 28);
+
+  vector_foo_string:[string] (id: 29);
+
+  vector_foo_enum:[BaseType] (id: 30);
+
+  // And a simple nested application.
+  single_application:Application (id: 31);
+
+  vov:VectorOfVectorOfString (id: 32);
+
+  foo_struct:FooStruct (id: 33);
+  vector_foo_struct:[FooStruct] (id: 34);
+  foo_struct_enum:StructEnum (id: 35);
+  foo_struct_scalars:ScalarSweepStruct (id: 36);
+  vector_foo_struct_scalars:[ScalarSweepStruct] (id: 37);
+
+  foo_enum_nonconsecutive:NonConsecutive (id: 38);
+  foo_enum_nonconsecutive_default:NonConsecutive = Big (id: 39);
+}
+
+root_type Configuration;
diff --git a/aos/flatbuffers/test_dir/type_coverage.json b/aos/flatbuffers/test_dir/type_coverage.json
new file mode 100644
index 0000000..6d2bb16
--- /dev/null
+++ b/aos/flatbuffers/test_dir/type_coverage.json
@@ -0,0 +1,216 @@
+{
+ "locations": [
+  {
+   "name": "foobar",
+   "type": "FooBar",
+   "max_size": 123
+  },
+  {
+   "name": "ten",
+   "type": "eleven",
+   "frequency": 12,
+   "max_size": 13
+  }
+ ],
+ "maps": [
+  {
+   "match": {
+    "type": ""
+   },
+   "rename": {
+    "type": "ABC"
+   }
+  }
+ ],
+ "apps": [
+  {
+   "name": "APPLICATION NAME",
+   "priority": 1,
+   "maps": [
+    {
+     "match": {
+      "type": "123"
+     },
+     "rename": {
+      "type": "456"
+     }
+    },
+    {
+
+    },
+    {
+     "match": {
+      "name": "foo",
+      "type": "Abc"
+     },
+     "rename": {
+      "name": "Def",
+      "type": "bar"
+     }
+    }
+   ]
+  }
+ ],
+ "imports": [
+  "file_one",
+  "file_twoooooooooooooooooooooooooooooooooooooooooooooooooooo",
+  "",
+  "",
+  "",
+  "file_three"
+ ],
+ "foo_byte": -123,
+ "foo_ubyte": 200,
+ "foo_bool": true,
+ "foo_short": -200,
+ "foo_ushort": 40000,
+ "foo_int": -40000,
+ "foo_uint": 3000000000,
+ "foo_long": -3000000000,
+ "foo_ulong": 9223372036854775809,
+ "foo_float": 1.23,
+ "foo_double": 1.45,
+ "foo_string": "Hello, Wooooooooooooooooooooooooooooooooooooooooooooorld!",
+ "foo_enum": "Byte",
+ "foo_enum_default": "UByte",
+ "vector_foo_byte": [
+  -1,
+  -2,
+  -3,
+  -4
+ ],
+ "vector_foo_ubyte": [
+  1,
+  2,
+  3,
+  4
+ ],
+ "vector_foo_bool": [
+  true,
+  false,
+  false,
+  true
+ ],
+ "vector_foo_short": [
+  123,
+  456,
+  789,
+  -123
+ ],
+ "vector_foo_ushort": [
+  123,
+  456,
+  789,
+  1023
+ ],
+ "vector_foo_int": [
+  -10,
+  -20,
+  -30,
+  -40
+ ],
+ "vector_foo_uint": [
+  10,
+  20,
+  30,
+  40
+ ],
+ "vector_foo_long": [
+  1,
+  1,
+  2,
+  3,
+  5,
+  8,
+  13
+ ],
+ "vector_foo_ulong": [
+  21,
+  34,
+  55
+ ],
+ "vector_foo_float": [
+  21.0,
+  3.4,
+  5.5
+ ],
+ "vector_foo_double": [
+  21.1,
+  3.5,
+  5.6
+ ],
+ "vector_foo_string": [
+  "Hello",
+  ", ",
+  "World",
+  "!",
+  ""
+ ],
+ "vector_foo_enum": [
+  "None",
+  "Byte"
+ ],
+ "single_application": {
+
+ },
+ "vov": {
+  "v": [
+   {
+    "str": [
+     "one",
+     "two",
+     "three",
+     "four"
+    ]
+   },
+   {
+
+   }
+  ]
+ },
+ "foo_struct": {
+  "foo_byte": 123,
+  "nested_struct": {
+   "foo_byte": -20
+  }
+ },
+ "vector_foo_struct": [
+  {
+   "foo_byte": 123,
+   "nested_struct": {
+    "foo_byte": -20
+   }
+  }
+ ],
+ "foo_struct_enum": {
+  "foo_enum": "Float"
+ },
+ "foo_struct_scalars": {
+  "foo_float": 1.23,
+  "foo_double": 4.56,
+  "foo_int32": -789,
+  "foo_uint32": 789,
+  "foo_int64": -5000000000,
+  "foo_uint64": 5000000000
+ },
+ "vector_foo_struct_scalars": [
+  {
+   "foo_float": 1.0,
+   "foo_double": 2.0,
+   "foo_int32": 3,
+   "foo_uint32": 4,
+   "foo_int64": 5,
+   "foo_uint64": 6
+  },
+  {
+   "foo_float": 7.0,
+   "foo_double": 8.0,
+   "foo_int32": 9,
+   "foo_uint32": 10,
+   "foo_int64": 11,
+   "foo_uint64": 12
+  }
+ ],
+ "foo_enum_nonconsecutive": "Big",
+ "foo_enum_nonconsecutive_default": "Zero"
+}
diff --git a/aos/flatbuffers_test.cc b/aos/flatbuffers_test.cc
index dbbe817..333d5d7 100644
--- a/aos/flatbuffers_test.cc
+++ b/aos/flatbuffers_test.cc
@@ -23,6 +23,16 @@
   EXPECT_FALSE(empty.Verify());
 }
 
+// Test that the UnpackFlatbuffer builds & works.
+TEST(FlatbufferTest, UnpackFlatbuffer) {
+  const FlatbufferDetachedBuffer<Location> fb =
+      JsonToFlatbuffer<Location>("{\"name\": \"abc\", \"frequency\": 971}");
+
+  LocationT object = UnpackFlatbuffer(&fb.message());
+  EXPECT_EQ("abc", object.name);
+  EXPECT_EQ(971, object.frequency);
+}
+
 // Tests the ability to map a flatbuffer on disk to memory
 TEST(FlatbufferMMapTest, Verify) {
   FlatbufferDetachedBuffer<Configuration> fb =
diff --git a/aos/json_to_flatbuffer.cc b/aos/json_to_flatbuffer.cc
index 023deea..3f049d7 100644
--- a/aos/json_to_flatbuffer.cc
+++ b/aos/json_to_flatbuffer.cc
@@ -23,10 +23,10 @@
 // Class to hold one of the 3 json types for an array.
 struct Element {
   // The type.
-  enum class ElementType { INT, DOUBLE, OFFSET };
+  enum class ElementType { INT, DOUBLE, OFFSET, STRUCT };
 
   // Constructs an Element holding an integer.
-  Element(int64_t new_int_element)
+  Element(absl::int128 new_int_element)
       : int_element(new_int_element), type(ElementType::INT) {}
   // Constructs an Element holding an double.
   Element(double new_double_element)
@@ -34,13 +34,24 @@
   // Constructs an Element holding an Offset.
   Element(flatbuffers::Offset<flatbuffers::String> new_offset_element)
       : offset_element(new_offset_element), type(ElementType::OFFSET) {}
+  // Constructs an Element holding a struct.
+  Element(std::vector<uint8_t> struct_data)
+      : /*initialize the union member to keep the compiler happy*/ int_element(
+            0),
+        struct_data(std::move(struct_data)),
+        type(ElementType::STRUCT) {}
 
   // Union for the various datatypes.
   union {
-    int64_t int_element;
+    absl::int128 int_element;
     double double_element;
     flatbuffers::Offset<flatbuffers::String> offset_element;
   };
+  // Because we can't know the maximum size of any potential structs at
+  // compile-time, we will use a vector to store the vector data inline.
+  // If you were to do a reinterpret_cast<StructType*>(struct_data.data()) then
+  // you would have an instance of the struct in question.
+  std::vector<uint8_t> struct_data;
 
   // And an enum signaling which one is in use.
   ElementType type;
@@ -48,13 +59,15 @@
 
 // Structure to represent a field element.
 struct FieldElement {
-  FieldElement(int new_field_index, int64_t int_element)
+  FieldElement(int new_field_index, absl::int128 int_element)
       : element(int_element), field_index(new_field_index) {}
   FieldElement(int new_field_index, double double_element)
       : element(double_element), field_index(new_field_index) {}
   FieldElement(int new_field_index,
                flatbuffers::Offset<flatbuffers::String> offset_element)
       : element(offset_element), field_index(new_field_index) {}
+  FieldElement(int new_field_index, const Element &element)
+      : element(element), field_index(new_field_index) {}
 
   // Data to write.
   Element element;
@@ -68,29 +81,179 @@
 bool AddSingleElement(FlatbufferType type, const FieldElement &field_element,
                       ::std::vector<bool> *fields_in_use,
                       flatbuffers::FlatBufferBuilder *fbb);
-bool AddSingleElement(FlatbufferType type, int field_index, int64_t int_value,
+bool AddSingleElement(FlatbufferType type, int field_index,
+                      absl::int128 int_value,
                       flatbuffers::FlatBufferBuilder *fbb);
 bool AddSingleElement(FlatbufferType type, int field_index, double double_value,
                       flatbuffers::FlatBufferBuilder *fbb);
 bool AddSingleElement(FlatbufferType type, int field_index,
                       flatbuffers::Offset<flatbuffers::String> offset_element,
                       flatbuffers::FlatBufferBuilder *fbb);
+bool AddSingleElement(FlatbufferType type, int field_index,
+                      const std::vector<uint8_t> &struct_data,
+                      flatbuffers::FlatBufferBuilder *fbb);
+
+template <typename T, typename U>
+void SetMemory(U value, uint8_t *destination) {
+  // destination may be poorly aligned. As such, we should not simply do
+  // *reinterpret_cast<T*>(destination) = value directly.
+  const T casted = static_cast<T>(value);
+  memcpy(destination, &casted, sizeof(T));
+}
+
+bool SetStructElement(FlatbufferType type, int field_index, absl::int128 value,
+                      uint8_t *destination) {
+  const flatbuffers::ElementaryType elementary_type =
+      type.FieldElementaryType(field_index);
+  switch (elementary_type) {
+    case flatbuffers::ET_CHAR:
+      SetMemory<int8_t>(value, destination);
+      break;
+    case flatbuffers::ET_UCHAR:
+      SetMemory<uint8_t>(value, destination);
+      break;
+    case flatbuffers::ET_SHORT:
+      SetMemory<int16_t>(value, destination);
+      break;
+    case flatbuffers::ET_USHORT:
+      SetMemory<uint16_t>(value, destination);
+      break;
+    case flatbuffers::ET_INT:
+      SetMemory<int32_t>(value, destination);
+      break;
+    case flatbuffers::ET_UINT:
+      SetMemory<uint32_t>(value, destination);
+      break;
+    case flatbuffers::ET_LONG:
+      SetMemory<int64_t>(value, destination);
+      break;
+    case flatbuffers::ET_ULONG:
+      SetMemory<uint64_t>(value, destination);
+      break;
+    case flatbuffers::ET_BOOL:
+      SetMemory<bool>(value, destination);
+      break;
+    case flatbuffers::ET_FLOAT:
+      SetMemory<float>(value, destination);
+      break;
+    case flatbuffers::ET_DOUBLE:
+      SetMemory<double>(value, destination);
+      break;
+    case flatbuffers::ET_STRING:
+    case flatbuffers::ET_UTYPE:
+    case flatbuffers::ET_SEQUENCE: {
+      const std::string_view name = type.FieldName(field_index);
+      fprintf(stderr,
+              "Mismatched type for field '%.*s'. Got: integer, expected %s\n",
+              static_cast<int>(name.size()), name.data(),
+              ElementaryTypeName(elementary_type));
+      return false;
+    }
+  }
+  return true;
+}
+
+bool SetStructElement(FlatbufferType type, int field_index, double value,
+                      uint8_t *destination) {
+  const flatbuffers::ElementaryType elementary_type =
+      type.FieldElementaryType(field_index);
+  switch (elementary_type) {
+    case flatbuffers::ET_FLOAT:
+      SetMemory<float>(value, destination);
+      break;
+    case flatbuffers::ET_DOUBLE:
+      SetMemory<double>(value, destination);
+      break;
+    case flatbuffers::ET_CHAR:
+    case flatbuffers::ET_UCHAR:
+    case flatbuffers::ET_SHORT:
+    case flatbuffers::ET_USHORT:
+    case flatbuffers::ET_INT:
+    case flatbuffers::ET_UINT:
+    case flatbuffers::ET_LONG:
+    case flatbuffers::ET_ULONG:
+    case flatbuffers::ET_BOOL:
+    case flatbuffers::ET_STRING:
+    case flatbuffers::ET_UTYPE:
+    case flatbuffers::ET_SEQUENCE: {
+      const std::string_view name = type.FieldName(field_index);
+      fprintf(stderr,
+              "Mismatched type for field '%.*s'. Got: integer, expected %s\n",
+              static_cast<int>(name.size()), name.data(),
+              ElementaryTypeName(elementary_type));
+      return false;
+    }
+  }
+  return true;
+}
 
 // Writes an array of FieldElement (with the definition in "type") to the
 // builder.  Returns the offset of the resulting table.
-flatbuffers::uoffset_t WriteTable(FlatbufferType type,
-                                  const ::std::vector<FieldElement> &elements,
-                                  flatbuffers::FlatBufferBuilder *fbb) {
-  // End of a nested struct!  Add it.
-  const flatbuffers::uoffset_t start = fbb->StartTable();
+std::optional<Element> WriteObject(FlatbufferType type,
+                                   const ::std::vector<FieldElement> &elements,
+                                   flatbuffers::FlatBufferBuilder *fbb) {
+  // End of a nested object!  Add it.
+  if (type.IsTable()) {
+    const flatbuffers::uoffset_t start = fbb->StartTable();
 
-  ::std::vector<bool> fields_in_use(type.NumberFields(), false);
+    ::std::vector<bool> fields_in_use(type.NumberFields(), false);
 
-  for (const FieldElement &field_element : elements) {
-    AddSingleElement(type, field_element, &fields_in_use, fbb);
+    for (const FieldElement &field_element : elements) {
+      AddSingleElement(type, field_element, &fields_in_use, fbb);
+    }
+
+    return Element{
+        flatbuffers::Offset<flatbuffers::String>{fbb->EndTable(start)}};
+  } else if (type.IsStruct()) {
+    // In order to write an inline struct, we need to fill out each field at the
+    // correct position inline in memory. In order to do this, we retrieve the
+    // offset/size of each field, and directly populate that memory with the
+    // relevant value.
+    std::vector<uint8_t> buffer(type.InlineSize(), 0);
+    for (size_t field_index = 0;
+         field_index < static_cast<size_t>(type.NumberFields());
+         ++field_index) {
+      auto it = std::find_if(elements.begin(), elements.end(),
+                             [field_index](const FieldElement &field) {
+                               return field.field_index ==
+                                      static_cast<int>(field_index);
+                             });
+      if (it == elements.end()) {
+        fprintf(stderr,
+                "All fields must be specified for struct types (field %s "
+                "missing).\n",
+                type.FieldName(field_index).data());
+        return std::nullopt;
+      }
+
+      uint8_t *field_data = buffer.data() + type.StructFieldOffset(field_index);
+      const size_t field_size = type.FieldInlineSize(field_index);
+      switch (it->element.type) {
+        case Element::ElementType::INT:
+          if (!SetStructElement(type, field_index, it->element.int_element,
+                                field_data)) {
+            return std::nullopt;
+          }
+          break;
+        case Element::ElementType::DOUBLE:
+          if (!SetStructElement(type, field_index, it->element.double_element,
+                                field_data)) {
+            return std::nullopt;
+          }
+          break;
+        case Element::ElementType::STRUCT:
+          CHECK_EQ(field_size, it->element.struct_data.size());
+          memcpy(field_data, it->element.struct_data.data(), field_size);
+          break;
+        case Element::ElementType::OFFSET:
+          LOG(FATAL)
+              << "This should be unreachable; structs cannot contain offsets.";
+          break;
+      }
+    }
+    return Element{buffer};
   }
-
-  return fbb->EndTable(start);
+  LOG(FATAL) << "Unimplemented.";
 }
 
 // Class to parse JSON into a flatbuffer.
@@ -135,7 +298,7 @@
 
   // Adds *_value for the provided field.  If we are in a vector, queues the
   // data up in vector_elements.  Returns true on success.
-  bool AddElement(int field_index, int64_t int_value);
+  bool AddElement(int field_index, absl::int128 int_value);
   bool AddElement(int field_index, double double_value);
   bool AddElement(int field_index, const ::std::string &data);
 
@@ -144,12 +307,13 @@
 
   // Pushes an element as part of a vector.  Returns true on success.
   bool PushElement(flatbuffers::ElementaryType elementary_type,
-                   int64_t int_value);
+                   absl::int128 int_value);
   bool PushElement(flatbuffers::ElementaryType elementary_type,
                    double double_value);
   bool PushElement(flatbuffers::ElementaryType elementary_type,
                    flatbuffers::Offset<flatbuffers::String> offset_value);
-
+  bool PushElement(const FlatbufferType &type,
+                   const std::vector<uint8_t> &struct_data);
   flatbuffers::FlatBufferBuilder *fbb_;
 
   // This holds the state information that is needed as you recurse into
@@ -170,8 +334,8 @@
     // For scalar types (not strings, and not nested tables), the vector ends
     // up being implemented as a start and end, and a block of data.  So we
     // can't just push offsets in as we go.  We either need to reproduce the
-    // logic inside flatbuffers, or build up vectors of the data.  Vectors will
-    // be a bit of extra stack space, but whatever.
+    // logic inside flatbuffers, or build up vectors of the data.  Vectors
+    // will be a bit of extra stack space, but whatever.
     //
     // Strings and nested structures are vectors of offsets.
     // into the vector. Once you get to the end, you build up a vector and
@@ -201,6 +365,7 @@
         }
         break;
       case Tokenizer::TokenType::kError:
+        fprintf(stderr, "Encountered an error in the tokenizer\n");
         return false;
         break;
 
@@ -238,17 +403,23 @@
           fprintf(stderr, "Empty stack\n");
           return false;
         } else {
-          // End of a nested struct!  Add it.
-          const flatbuffers::uoffset_t end =
-              WriteTable(stack_.back().type, stack_.back().elements, fbb_);
+          // End of a nested object!  Add it.
+          std::optional<Element> object =
+              WriteObject(stack_.back().type, stack_.back().elements, fbb_);
+          if (!object.has_value()) {
+            return false;
+          }
 
           // We now want to talk about the parent structure.  Pop the child.
           stack_.pop_back();
 
           if (stack_.size() == 0) {
+            CHECK_EQ(static_cast<int>(object->type),
+                     static_cast<int>(Element::ElementType::OFFSET))
+                << ": JSON parsing only supports parsing flatbuffer tables.";
             // Instead of queueing it up in the stack, return it through the
             // passed in variable.
-            *table_end = end;
+            *table_end = object->offset_element.o;
           } else {
             // And now we can add it.
             const int field_index = stack_.back().field_index;
@@ -256,10 +427,10 @@
             // Do the right thing if we are in a vector.
             if (in_vector()) {
               stack_.back().vector_elements.emplace_back(
-                  flatbuffers::Offset<flatbuffers::String>(end));
+                  std::move(object.value()));
             } else {
-              stack_.back().elements.emplace_back(
-                  field_index, flatbuffers::Offset<flatbuffers::String>(end));
+              stack_.back().elements.emplace_back(field_index,
+                                                  std::move(object.value()));
             }
           }
         }
@@ -267,11 +438,13 @@
 
       case Tokenizer::TokenType::kStartArray:  // [
         if (stack_.size() == 0) {
-          // We don't support an array of structs at the root level.
+          fprintf(stderr,
+                  "We don't support an array of structs at the root level.\n");
           return false;
         }
         // Sanity check that we aren't trying to make a vector of vectors.
         if (in_vector()) {
+          fprintf(stderr, "We don't support vectors of vectors.\n");
           return false;
         }
         set_in_vector(true);
@@ -279,6 +452,7 @@
         break;
       case Tokenizer::TokenType::kEndArray: {  // ]
         if (!in_vector()) {
+          fprintf(stderr, "Encountered ']' with no prior '['.\n");
           return false;
         }
 
@@ -294,7 +468,7 @@
       case Tokenizer::TokenType::kNumberValue: {
         bool is_int = true;
         double double_value;
-        long long int_value;
+        absl::int128 int_value;
         if (token == Tokenizer::TokenType::kTrueValue) {
           int_value = 1;
         } else if (token == Tokenizer::TokenType::kFalseValue) {
@@ -313,7 +487,7 @@
 
         if (is_int) {
           // No need to get too stressed about bool vs int.  Convert them all.
-          int64_t val = int_value;
+          absl::int128 val = int_value;
           if (!AddElement(field_index, val)) return false;
         } else {
           if (!AddElement(field_index, double_value)) return false;
@@ -342,9 +516,13 @@
   return false;
 }
 
-bool JsonParser::AddElement(int field_index, int64_t int_value) {
+bool JsonParser::AddElement(int field_index, absl::int128 int_value) {
   if (stack_.back().type.FieldIsRepeating(field_index) != in_vector()) {
-    fprintf(stderr, "Type and json disagree on if we are in a vector or not\n");
+    fprintf(stderr,
+            "Type and json disagree on if we are in a vector or not (JSON "
+            "believes that we are%s in a vector for field '%s').\n",
+            in_vector() ? "" : " not",
+            stack_.back().type.FieldName(field_index).data());
     return false;
   }
 
@@ -358,7 +536,11 @@
 
 bool JsonParser::AddElement(int field_index, double double_value) {
   if (stack_.back().type.FieldIsRepeating(field_index) != in_vector()) {
-    fprintf(stderr, "Type and json disagree on if we are in a vector or not\n");
+    fprintf(stderr,
+            "Type and json disagree on if we are in a vector or not (JSON "
+            "believes that we are%s in a vector for field '%s').\n",
+            in_vector() ? "" : " not",
+            stack_.back().type.FieldName(field_index).data());
     return false;
   }
 
@@ -372,7 +554,11 @@
 
 bool JsonParser::AddElement(int field_index, const ::std::string &data) {
   if (stack_.back().type.FieldIsRepeating(field_index) != in_vector()) {
-    fprintf(stderr, "Type and json disagree on if we are in a vector or not\n");
+    fprintf(stderr,
+            "Type and json disagree on if we are in a vector or not (JSON "
+            "believes that we are%s in a vector for field '%s').\n",
+            in_vector() ? "" : " not",
+            stack_.back().type.FieldName(field_index).data());
     return false;
   }
 
@@ -393,7 +579,7 @@
         const FlatbufferType enum_type = type.FieldType(field_index);
         CHECK(enum_type.IsEnum());
 
-        const std::optional<int64_t> int_value = enum_type.EnumValue(data);
+        const std::optional<absl::int128> int_value = enum_type.EnumValue(data);
 
         if (!int_value) {
           const std::string_view name = type.FieldName(field_index);
@@ -448,11 +634,15 @@
     case Element::ElementType::OFFSET:
       return AddSingleElement(type, field_element.field_index,
                               field_element.element.offset_element, fbb);
+    case Element::ElementType::STRUCT:
+      return AddSingleElement(type, field_element.field_index,
+                              field_element.element.struct_data, fbb);
   }
   return false;
 }
 
-bool AddSingleElement(FlatbufferType type, int field_index, int64_t int_value,
+bool AddSingleElement(FlatbufferType type, int field_index,
+                      absl::int128 int_value,
                       flatbuffers::FlatBufferBuilder *fbb
 
 ) {
@@ -463,37 +653,39 @@
       type.FieldElementaryType(field_index);
   switch (elementary_type) {
     case flatbuffers::ET_BOOL:
-      fbb->AddElement<bool>(field_offset, int_value);
+      fbb->AddElement<bool>(field_offset, static_cast<bool>(int_value));
       return true;
     case flatbuffers::ET_CHAR:
-      fbb->AddElement<int8_t>(field_offset, int_value);
+      fbb->AddElement<int8_t>(field_offset, static_cast<int8_t>(int_value));
       return true;
     case flatbuffers::ET_UCHAR:
-      fbb->AddElement<uint8_t>(field_offset, int_value);
+      fbb->AddElement<uint8_t>(field_offset, static_cast<uint8_t>(int_value));
       return true;
     case flatbuffers::ET_SHORT:
-      fbb->AddElement<int16_t>(field_offset, int_value);
+      fbb->AddElement<int16_t>(field_offset, static_cast<int16_t>(int_value));
       return true;
     case flatbuffers::ET_USHORT:
-      fbb->AddElement<uint16_t>(field_offset, int_value);
+      fbb->AddElement<uint16_t>(field_offset, static_cast<uint16_t>(int_value));
       return true;
     case flatbuffers::ET_INT:
-      fbb->AddElement<int32_t>(field_offset, int_value);
+      fbb->AddElement<int32_t>(field_offset, static_cast<int32_t>(int_value));
       return true;
     case flatbuffers::ET_UINT:
-      fbb->AddElement<uint32_t>(field_offset, int_value);
+      fbb->AddElement<uint32_t>(field_offset, static_cast<uint32_t>(int_value));
       return true;
     case flatbuffers::ET_LONG:
-      fbb->AddElement<int64_t>(field_offset, int_value);
+      fbb->AddElement<int64_t>(field_offset, static_cast<int64_t>(int_value));
       return true;
     case flatbuffers::ET_ULONG:
-      fbb->AddElement<uint64_t>(field_offset, int_value);
+      fbb->AddElement<uint64_t>(field_offset, static_cast<uint64_t>(int_value));
       return true;
+      // The floating point cases occur when someone specifies an integer in the
+      // JSON for a double field.
     case flatbuffers::ET_FLOAT:
-      fbb->AddElement<float>(field_offset, int_value);
+      fbb->AddElement<float>(field_offset, static_cast<float>(int_value));
       return true;
     case flatbuffers::ET_DOUBLE:
-      fbb->AddElement<double>(field_offset, int_value);
+      fbb->AddElement<double>(field_offset, static_cast<double>(int_value));
       return true;
     case flatbuffers::ET_STRING:
     case flatbuffers::ET_UTYPE:
@@ -545,6 +737,7 @@
   }
   return false;
 }
+
 bool AddSingleElement(FlatbufferType type, int field_index,
                       flatbuffers::Offset<flatbuffers::String> offset_element,
                       flatbuffers::FlatBufferBuilder *fbb) {
@@ -587,11 +780,27 @@
   return false;
 }
 
+bool AddSingleElement(FlatbufferType type, int field_index,
+                      const std::vector<uint8_t> &data,
+                      flatbuffers::FlatBufferBuilder *fbb) {
+  // Structs are always inline.
+  // We have to do somewhat manual serialization to get the struct into place,
+  // since the regular FlatBufferBuilder assumes that you will know the type of
+  // the struct that you are constructing at compile time.
+  fbb->Align(type.FieldType(field_index).Alignment());
+  fbb->PushBytes(data.data(), data.size());
+  fbb->AddStructOffset(flatbuffers::FieldIndexToOffset(
+                           static_cast<flatbuffers::voffset_t>(field_index)),
+                       fbb->GetSize());
+  return true;
+}
+
 bool JsonParser::FinishVector(int field_index) {
   // Vectors have a start (unfortunately which needs to know the size)
   const size_t inline_size = stack_.back().type.FieldInlineSize(field_index);
+  const size_t alignment = stack_.back().type.FieldInlineAlignment(field_index);
   fbb_->StartVector(stack_.back().vector_elements.size(), inline_size,
-                    /*align=*/inline_size);
+                    /*align=*/alignment);
 
   const flatbuffers::ElementaryType elementary_type =
       stack_.back().type.FieldElementaryType(field_index);
@@ -609,6 +818,11 @@
       case Element::ElementType::OFFSET:
         if (!PushElement(elementary_type, element.offset_element)) return false;
         break;
+      case Element::ElementType::STRUCT:
+        if (!PushElement(stack_.back().type.FieldType(field_index),
+                         element.struct_data))
+          return false;
+        break;
     }
   }
 
@@ -621,40 +835,40 @@
 }
 
 bool JsonParser::PushElement(flatbuffers::ElementaryType elementary_type,
-                             int64_t int_value) {
+                             absl::int128 int_value) {
   switch (elementary_type) {
     case flatbuffers::ET_BOOL:
-      fbb_->PushElement<bool>(int_value);
+      fbb_->PushElement<bool>(static_cast<bool>(int_value));
       return true;
     case flatbuffers::ET_CHAR:
-      fbb_->PushElement<int8_t>(int_value);
+      fbb_->PushElement<int8_t>(static_cast<int8_t>(int_value));
       return true;
     case flatbuffers::ET_UCHAR:
-      fbb_->PushElement<uint8_t>(int_value);
+      fbb_->PushElement<uint8_t>(static_cast<uint8_t>(int_value));
       return true;
     case flatbuffers::ET_SHORT:
-      fbb_->PushElement<int16_t>(int_value);
+      fbb_->PushElement<int16_t>(static_cast<int16_t>(int_value));
       return true;
     case flatbuffers::ET_USHORT:
-      fbb_->PushElement<uint16_t>(int_value);
+      fbb_->PushElement<uint16_t>(static_cast<uint16_t>(int_value));
       return true;
     case flatbuffers::ET_INT:
-      fbb_->PushElement<int32_t>(int_value);
+      fbb_->PushElement<int32_t>(static_cast<int32_t>(int_value));
       return true;
     case flatbuffers::ET_UINT:
-      fbb_->PushElement<uint32_t>(int_value);
+      fbb_->PushElement<uint32_t>(static_cast<uint32_t>(int_value));
       return true;
     case flatbuffers::ET_LONG:
-      fbb_->PushElement<int64_t>(int_value);
+      fbb_->PushElement<int64_t>(static_cast<int64_t>(int_value));
       return true;
     case flatbuffers::ET_ULONG:
-      fbb_->PushElement<uint64_t>(int_value);
+      fbb_->PushElement<uint64_t>(static_cast<uint64_t>(int_value));
       return true;
     case flatbuffers::ET_FLOAT:
-      fbb_->PushElement<float>(int_value);
+      fbb_->PushElement<float>(static_cast<float>(int_value));
       return true;
     case flatbuffers::ET_DOUBLE:
-      fbb_->PushElement<double>(int_value);
+      fbb_->PushElement<double>(static_cast<double>(int_value));
       return true;
     case flatbuffers::ET_STRING:
     case flatbuffers::ET_UTYPE:
@@ -698,6 +912,17 @@
   return false;
 }
 
+bool JsonParser::PushElement(const FlatbufferType &type,
+                             const std::vector<uint8_t> &struct_data) {
+  // To add a struct to a vector, we just need to get the relevant bytes pushed
+  // straight into the builder. The FlatBufferBuilder normally expects that you
+  // will know the type of your struct at compile-time, so doesn't have a
+  // first-class way to do this.
+  fbb_->Align(type.Alignment());
+  fbb_->PushBytes(struct_data.data(), struct_data.size());
+  return true;
+}
+
 bool JsonParser::PushElement(
     flatbuffers::ElementaryType elementary_type,
     flatbuffers::Offset<flatbuffers::String> offset_value) {
diff --git a/aos/json_to_flatbuffer.fbs b/aos/json_to_flatbuffer.fbs
index 7180322..e66ec4a 100644
--- a/aos/json_to_flatbuffer.fbs
+++ b/aos/json_to_flatbuffer.fbs
@@ -62,6 +62,15 @@
   nested_struct:FooStructNested;
 }
 
+struct ScalarSweepStruct {
+  foo_float:float;
+  foo_double:double;
+  foo_int32:int32;
+  foo_uint32:uint32;
+  foo_int64:int64;
+  foo_uint64:uint64;
+}
+
 struct StructEnum {
   foo_enum:BaseType;
 }
@@ -131,9 +140,11 @@
   foo_struct:FooStruct (id: 34);
   vector_foo_struct:[FooStruct] (id: 35);
   foo_struct_enum:StructEnum (id: 36);
+  foo_struct_scalars:ScalarSweepStruct (id: 37);
+  vector_foo_struct_scalars:[ScalarSweepStruct] (id: 38);
 
-  foo_enum_nonconsecutive:NonConsecutive (id: 37);
-  foo_enum_nonconsecutive_default:NonConsecutive = Big (id: 38);
+  foo_enum_nonconsecutive:NonConsecutive (id: 39);
+  foo_enum_nonconsecutive_default:NonConsecutive = Big (id: 40);
 }
 
 root_type Configuration;
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index b664036..6ef3544 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -12,6 +12,7 @@
 #include "aos/fast_string_builder.h"
 #include "aos/flatbuffer_utils.h"
 #include "aos/flatbuffers.h"
+#include "aos/flatbuffers/builder.h"
 #include "aos/util/file.h"
 
 namespace aos {
@@ -38,6 +39,15 @@
       JsonToFlatbuffer(data, FlatbufferType(T::MiniReflectTypeTable()), fbb).o);
 }
 
+template <typename T>
+inline fbs::Builder<T> JsonToStaticFlatbuffer(const std::string_view data) {
+  const aos::FlatbufferDetachedBuffer<typename T::Flatbuffer> fbs =
+      JsonToFlatbuffer<typename T::Flatbuffer>(data);
+  fbs::Builder<T> builder(std::make_unique<aos::fbs::VectorAllocator>());
+  CHECK(builder.get()->FromFlatbuffer(&fbs.message()));
+  return builder;
+}
+
 struct JsonOptions {
   // controls if the Json is written out on multiple lines or one.
   bool multi_line = false;
@@ -53,7 +63,7 @@
 // Converts a flatbuffer into a Json string.
 // The methods below are generally more useful than TableFlatbufferToJson.
 ::std::string TableFlatbufferToJson(const flatbuffers::Table *t,
-                                    const ::flatbuffers::TypeTable *typetable,
+                                    const flatbuffers::TypeTable *typetable,
                                     JsonOptions json_options = {});
 
 // Converts a Flatbuffer<T> holding a flatbuffer to JSON.
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index a145364..4900711 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -87,6 +87,62 @@
   EXPECT_TRUE(JsonAndBack("{ \"foo_enum_nonconsecutive\": \"Big\" }"));
 }
 
+TEST_F(JsonToFlatbufferTest, Structs) {
+  EXPECT_TRUE(
+      JsonAndBack("{ \"foo_struct\": { \"foo_byte\": 1, \"nested_struct\": { "
+                  "\"foo_byte\": 2 } } }"));
+  EXPECT_TRUE(JsonAndBack(
+      "{ \"foo_struct_scalars\": { \"foo_float\": 1.234, \"foo_double\": "
+      "4.567, \"foo_int32\": -971, \"foo_uint32\": 4294967294, \"foo_int64\": "
+      "-1030, \"foo_uint64\": 18446744073709551614 } }"));
+  // Confirm that we parse integers into floating point fields correctly.
+  EXPECT_TRUE(JsonAndBack(
+      "{ \"foo_struct_scalars\": { \"foo_float\": 1, \"foo_double\": "
+      "2, \"foo_int32\": 3, \"foo_uint32\": 4, \"foo_int64\": "
+      "5, \"foo_uint64\": 6 } }",
+      "{ \"foo_struct_scalars\": { \"foo_float\": 1.0, \"foo_double\": "
+      "2.0, \"foo_int32\": 3, \"foo_uint32\": 4, \"foo_int64\": "
+      "5, \"foo_uint64\": 6 } }"));
+  EXPECT_TRUE(JsonAndBack(
+      "{ \"vector_foo_struct_scalars\": [ { \"foo_float\": 1.234, "
+      "\"foo_double\": 4.567, \"foo_int32\": -971, \"foo_uint32\": 4294967294, "
+      "\"foo_int64\": -1030, \"foo_uint64\": 18446744073709551614 }, { "
+      "\"foo_float\": 2.0, \"foo_double\": 4.1, \"foo_int32\": 10, "
+      "\"foo_uint32\": 13, \"foo_int64\": 15, \"foo_uint64\": 18 } ] }"));
+  EXPECT_TRUE(
+      JsonAndBack("{ \"foo_struct_enum\": { \"foo_enum\": \"UByte\" } }"));
+  EXPECT_TRUE(
+      JsonAndBack("{ \"vector_foo_struct\": [ { \"foo_byte\": 1, "
+                  "\"nested_struct\": { \"foo_byte\": 2 } } ] }"));
+  EXPECT_TRUE(JsonAndBack(
+      "{ \"vector_foo_struct\": [ { \"foo_byte\": 1, \"nested_struct\": { "
+      "\"foo_byte\": 2 } }, { \"foo_byte\": 3, \"nested_struct\": { "
+      "\"foo_byte\": 4 } }, { \"foo_byte\": 5, \"nested_struct\": { "
+      "\"foo_byte\": 6 } } ] }"));
+}
+
+// Confirm that we correctly die when input JSON is missing fields inside of a
+// struct.
+TEST_F(JsonToFlatbufferTest, StructMissingField) {
+  ::testing::internal::CaptureStderr();
+  EXPECT_FALSE(
+      JsonAndBack("{ \"foo_struct\": { \"nested_struct\": { "
+                  "\"foo_byte\": 2 } } }"));
+  EXPECT_FALSE(JsonAndBack(
+      "{ \"foo_struct\": { \"foo_byte\": 1, \"nested_struct\": {  } } }"));
+  EXPECT_FALSE(JsonAndBack("{ \"foo_struct\": { \"foo_byte\": 1 } }"));
+  std::string output = ::testing::internal::GetCapturedStderr();
+  EXPECT_EQ(
+      R"output(All fields must be specified for struct types (field foo_byte missing).
+All fields must be specified for struct types (field foo_byte missing).
+All fields must be specified for struct types (field foo_byte missing).
+All fields must be specified for struct types (field foo_byte missing).
+All fields must be specified for struct types (field nested_struct missing).
+All fields must be specified for struct types (field nested_struct missing).
+)output",
+      output);
+}
+
 // Tests that Inf is handled correctly
 TEST_F(JsonToFlatbufferTest, Inf) {
   EXPECT_TRUE(JsonAndBack("{ \"foo_float\": inf }"));
@@ -209,7 +265,7 @@
 }
 
 // Test nested messages, and arrays of nested messages.
-TEST_F(JsonToFlatbufferTest, NestedStruct) {
+TEST_F(JsonToFlatbufferTest, NestedTable) {
   EXPECT_TRUE(
       JsonAndBack("{ \"single_application\": { \"name\": \"woot\" } }"));
 
diff --git a/aos/json_tokenizer.cc b/aos/json_tokenizer.cc
index eab7fcc..d277c1e 100644
--- a/aos/json_tokenizer.cc
+++ b/aos/json_tokenizer.cc
@@ -531,14 +531,9 @@
   return TokenType::kError;
 }
 
-bool Tokenizer::FieldAsInt(long long *value) {
+bool Tokenizer::FieldAsInt(absl::int128 *value) {
   const char *pos = field_value().c_str();
-  errno = 0;
-  *value = strtoll(field_value().c_str(), const_cast<char **>(&pos), 10);
-  if (pos != field_value().c_str() + field_value().size() || errno != 0) {
-    return false;
-  }
-  return true;
+  return absl::SimpleAtoi(pos, value);
 }
 
 bool Tokenizer::FieldAsDouble(double *value) {
diff --git a/aos/json_tokenizer.h b/aos/json_tokenizer.h
index 892c575..ab928f4 100644
--- a/aos/json_tokenizer.h
+++ b/aos/json_tokenizer.h
@@ -5,6 +5,7 @@
 #include <string_view>
 #include <vector>
 
+#include "absl/strings/numbers.h"
 #include "flatbuffers/util.h"
 
 namespace aos {
@@ -41,7 +42,7 @@
 
   // Parses the current field value as a long long.  Returns false if it failed
   // to parse.
-  bool FieldAsInt(long long *value);
+  bool FieldAsInt(absl::int128 *value);
   // Parses the current field value as a double.  Returns false if it failed
   // to parse.
   bool FieldAsDouble(double *value);
diff --git a/aos/logging/BUILD b/aos/logging/BUILD
index e179d6b..9bd4807 100644
--- a/aos/logging/BUILD
+++ b/aos/logging/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
 
 # The primary client logging interface.
@@ -52,10 +52,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "log_message_fbs",
     srcs = ["log_message.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -67,12 +66,11 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "dynamic_log_command_fbs",
     srcs = [
         "dynamic_log_command.fbs",
     ],
-    gen_reflections = 1,
     visibility = ["//visibility:public"],
 )
 
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 7a8ceb5..cd31731 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -1,3 +1,4 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos/seasocks:gen_embedded.bzl", "gen_embedded")
 load("//aos:config.bzl", "aos_config")
 load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
@@ -9,10 +10,9 @@
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "remote_message_fbs",
     srcs = ["remote_message.fbs"],
-    gen_reflections = 1,
 )
 
 cc_static_flatbuffer(
@@ -21,14 +21,13 @@
     target = ":remote_message_fbs_reflection_out",
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "connect_fbs",
     srcs = ["connect.fbs"],
-    gen_reflections = 1,
-    includes = [
-        "//aos:configuration_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos:configuration_fbs",
+    ],
 )
 
 flatbuffer_ts_library(
@@ -40,21 +39,19 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "remote_data_fbs",
     srcs = ["remote_data.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "timestamp_fbs",
     srcs = ["timestamp.fbs"],
-    gen_reflections = 1,
-    includes = [
-        "//aos:configuration_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos:configuration_fbs",
+    ],
 )
 
 cc_static_flatbuffer(
@@ -64,10 +61,9 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "message_bridge_client_fbs",
     srcs = ["message_bridge_client.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
         ":message_bridge_server_fbs",
@@ -90,10 +86,9 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "sctp_config_fbs",
     srcs = ["sctp_config.fbs"],
-    gen_reflections = 1,
 )
 
 cc_static_flatbuffer(
@@ -102,10 +97,9 @@
     target = ":sctp_config_fbs_reflection_out",
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "sctp_config_request_fbs",
     srcs = ["sctp_config_request.fbs"],
-    gen_reflections = 1,
 )
 
 cc_static_flatbuffer(
@@ -115,10 +109,9 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "message_bridge_server_fbs",
     srcs = ["message_bridge_server.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
         "//aos:configuration_fbs",
@@ -624,11 +617,11 @@
     name = "web_proxy_fbs",
     srcs = ["web_proxy.fbs"],
     gen_reflections = True,
-    includes = [
-        ":connect_fbs_includes",
-        "//aos:configuration_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":connect_fbs_fbs",
+        "//aos:configuration_fbs_fbs",
+    ],
 )
 
 flatbuffer_ts_library(
diff --git a/aos/network/message_bridge_server_status.cc b/aos/network/message_bridge_server_status.cc
index 22270d9..f540b1e 100644
--- a/aos/network/message_bridge_server_status.cc
+++ b/aos/network/message_bridge_server_status.cc
@@ -126,6 +126,11 @@
         configuration::GetChannel(event_loop_->configuration(), "/aos",
                                   Timestamp::GetFullyQualifiedName(),
                                   event_loop_->name(), destination_node);
+    CHECK(other_timestamp_channel)
+        << "Failed to find other timestamp channel \"/aos\" type "
+        << Timestamp::GetFullyQualifiedName() << " for destination node "
+        << destination_node->name()->string_view() << " from node "
+        << event_loop_->node()->name()->string_view();
 
     ServerConnection *const server_connection =
         FindServerConnection(destination_node->name()->string_view());
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index 1873166..a7d7e02 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 
 exports_files(["roborio_irq_config.json"])
@@ -183,27 +183,24 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "starter_fbs",
     srcs = ["starter.fbs"],
-    gen_reflections = True,
-    includes = ["//aos/util:process_info_fbs_includes"],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
+    deps = ["//aos/util:process_info_fbs"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "kthread_fbs",
     srcs = ["kthread.fbs"],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "starter_rpc_fbs",
     srcs = ["starter_rpc.fbs"],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 361616c..6ba9543 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -23,6 +23,26 @@
 
 namespace chrono = ::std::chrono;
 
+namespace {
+
+void PrintToStream(std::ostream &stream, chrono::nanoseconds duration) {
+  chrono::seconds seconds = chrono::duration_cast<chrono::seconds>(duration);
+  if (duration < chrono::nanoseconds(0)) {
+    stream << "-" << -seconds.count() << "." << std::setfill('0')
+           << std::setw(9)
+           << chrono::duration_cast<chrono::nanoseconds>(seconds - duration)
+                  .count()
+           << "sec";
+  } else {
+    stream << seconds.count() << "." << std::setfill('0') << std::setw(9)
+           << chrono::duration_cast<chrono::nanoseconds>(duration - seconds)
+                  .count()
+           << "sec";
+  }
+}
+
+}  // namespace
+
 #ifdef __linux__
 
 namespace std {
@@ -57,27 +77,7 @@
 
 std::ostream &operator<<(std::ostream &stream,
                          const aos::monotonic_clock::time_point &now) {
-  if (now < monotonic_clock::epoch()) {
-    std::chrono::seconds seconds =
-        std::chrono::duration_cast<std::chrono::seconds>(
-            now.time_since_epoch());
-
-    stream << "-" << -seconds.count() << "." << std::setfill('0')
-           << std::setw(9)
-           << std::chrono::duration_cast<std::chrono::nanoseconds>(
-                  seconds - now.time_since_epoch())
-                  .count()
-           << "sec";
-  } else {
-    std::chrono::seconds seconds =
-        std::chrono::duration_cast<std::chrono::seconds>(
-            now.time_since_epoch());
-    stream << seconds.count() << "." << std::setfill('0') << std::setw(9)
-           << std::chrono::duration_cast<std::chrono::nanoseconds>(
-                  now.time_since_epoch() - seconds)
-                  .count()
-           << "sec";
-  }
+  PrintToStream(stream, now.time_since_epoch());
   return stream;
 }
 
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 8450298..2caea22 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
 load("config_validator_macro.bzl", "config_validator_test")
 
@@ -79,10 +79,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "clock_timepoints_fbs",
     srcs = ["clock_timepoints.fbs"],
-    gen_reflections = True,
     deps = ["//aos:configuration_fbs"],
 )
 
@@ -368,10 +367,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "process_info_fbs",
     srcs = ["process_info.fbs"],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -536,7 +534,7 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "config_validator_config_fbs",
     srcs = ["config_validator_config.fbs"],
     target_compatible_with = ["@platforms//os:linux"],
diff --git a/aos/util/simulation_logger.cc b/aos/util/simulation_logger.cc
index 513fa65..32ff706 100644
--- a/aos/util/simulation_logger.cc
+++ b/aos/util/simulation_logger.cc
@@ -15,7 +15,15 @@
     event_loop_->SkipTimingReport();
   }
   event_loop_->SkipAosLog();
-  event_loop_->OnRun([this]() { logger_->StartLogging(std::move(namer_)); });
+
+  // TODO (James, Maxwell) This shouldn't be necessary to have a delay here.
+  // We'd like to have the logger start as soon as the event loop starts. The
+  // logger must be started after (not on) `factory->send_delay()` amount of
+  // time. Keep this simple, use two of those delays.
+  TimerHandler *status_timer = event_loop_->AddTimer(
+      [this]() { logger_->StartLogging(std::move(namer_)); });
+  status_timer->Schedule(
+      monotonic_clock::time_point(factory->send_delay() * 2));
 }
 
 std::vector<std::unique_ptr<LoggerState>> MakeLoggersForNodes(
diff --git a/documentation/aos/docs/flatbuffers.md b/documentation/aos/docs/flatbuffers.md
index 438b882..132348d 100644
--- a/documentation/aos/docs/flatbuffers.md
+++ b/documentation/aos/docs/flatbuffers.md
@@ -1 +1,547 @@
 # FlatBuffers
+
+This document covers the "static flatbuffers API".
+
+This API is a custom C++ API for serializing flatbuffers developed for AOS. The
+serialized flatbuffers are fully compatible with the existing flatbuffers
+specification.
+
+## Design
+
+The overall goal of the static flatbuffers API is to make it so that a user can
+construct flatbuffers against a fixed-size memory buffer while being able to
+readily mutate any part of the flatbuffer object at any point during
+construction (rather than being forced to construct things from the bottom up).
+
+In particular:
+
+* The API should be able to both construct flatbuffers against fixed-size memory
+  buffers (for use in realtime code) as well as against variable-size buffers
+  (for easy offline flatbuffer manipulation).
+* We want to be able to select vector sizes at runtime (including in realtime
+  code) so that we can support using e.g. the same camera message schema (which
+  would generally contain a byte array of data) for multiple different
+  resolutions of an image.
+* The API should require minimal modifications to existing .fbs files (in fact,
+  it is usable without any modifications).
+* We want to be able to provide an option for deriving strict upper bounds for
+  AOS channel `max_size`'s (this is not currently fully implemented). This does
+  require specifying maximum vector sizes (and adhering to them) in the message
+  schemas.
+* There should be low performance impacts when using the API normally (as
+  compared to trying to use the `FlatBufferBuilder` API).
+* The API should be difficult to accidentally use incorrectly (e.g., the
+  existing flatbuffers API requires that you not build multiple tables at once,
+  and enforces this with runtime debug assertions; this is both hard to develop
+  and prone to memory corruption in situations where someone never runs debug
+  builds of their code).
+
+In order to accomplish this, we provide a codegen'd interface in place of the
+regular flatbuffer API. It provides the following objects/interfaces to work with:
+
+* For each table, a codegen'd class which inherits from the `aos::fbs::Table`
+  object. This provides accessors to let you set, get, clear, and mutate table
+  members at any time. The table objects take in an aligned `std::span` into
+  which they construct the flatbuffer. This class will be named `FooStatic`
+  for a given flatbuffer type `Foo`.
+* For flatbuffer vector/strings, `Vector` and `String` objects are provided
+  (the `Vector` object will generally be created by calling `add_*` on the
+  appropriate member of a table). These generally operate similarly in concept
+  to the table objects, allowing you to add/remove/modify elements at will.
+  `Vector`s and `String`s can have a nominal maximum length specified in order
+  to have the memory for those elements statically allocated, while also having
+  the ability to dynamically increase the size of the vectors.
+* In order to allow the construction of a flatbuffer table, a
+  templated `aos::fbs::Builder` object is provided which can
+  take an allocator and then provide the relevant table class to the user.
+* We provide an `Allocator` class and various implementations (e.g., a
+  `VectorAllocator` backed by an `std::vector`) for managing the memory into
+  which the `Builder` will serialize the flatbuffer.
+* A new `MakeStaticBuilder` method is provided on the `aos::Sender` class which
+  constructs an `aos::fbs::Builder` to allow you to construct a message to be
+  sent on an AOS channel.
+* Existing `flatbuffer_cc_library` bazel targets get turned into `static_flatbuffer`
+  targets of the same name. Libraries do not need to change how they depend on
+  the bazel target, although using the new API in your code will require
+  importing a different header and using a different class than before.
+
+### Alignment
+
+Significant effort must be made to ensure that all objects are correctly
+aligned. This includes the `force_align` attribute which can be added to vectors
+to, e.g., allow you to over-align byte vectors that may need to be used to
+store aligned data. The current alignment approach is relatively conservative,
+which may result in excessive padding (all padding in the serialized flatbuffers
+should get cleared to zero, hopefully allowing compression algorithms to handle
+the extra bytes reasonably efficiently).
+
+As a user, you should never need to do anything else to get correct alignment.
+Further discussion in this section is mostly relevant for those
+modifying/reviewing the internals.
+
+Internally, every `Vector` and `Table` type tracks its required alignment using
+a `kAlign` constant. This constant is set as the maximum alignment of any
+members of the object (this will always be a minimum of `4` because every
+flatbuffer table includes a 4-byte vtable offset and every flatbuffer vector
+includes a 4-byte length). The buffers provided to the constructors of these
+objects must be aligned, and the `kSize` constant that the objects provide will
+always be a multiple of the alignment. Additional discussion of the detailed
+layout of memory inside of the `Vector` and `Table` types can be found in the
+comments on the respective class declarations.
+
+In order to handle alignment correctly in our `Builder` and `Allocator` classes,
+we end up forcing the `Builder` to be able to accept semi-arbitrarily aligned
+buffers in order to ease the `Allocator` implementation (e.g., the
+`VectorAllocator` uses a `std::vector` internally which does not necessarily
+align its memory). The `Builder` then adds padding as needed and passes an
+appropriately aligned buffer down to the `Table` class.
+
+## Basic API Examples
+
+This example will walk through what the API for the following set of tables
+looks like (see `//aos/flatbuffers:test.fbs`:
+
+```cpp
+// Note: in the actual sample code, these are two separate files where one is
+// included in the other.
+namespace aos.fbs.testing.included;
+enum TestEnum : ubyte {
+  A = 0,
+  B = 1,
+}
+
+table IncludedTable {
+ foo:TestEnum (id: 0);
+}
+
+namespace aos.fbs.testing;
+
+struct SubStruct {
+  x:double;
+  y:double;
+}
+
+table SubTable {
+ foo:short (id: 0);
+ bar:short (id: 1, deprecated);
+ baz:float (id: 2);
+}
+
+attribute "static_length";
+attribute "static_vector_string_length";
+
+table TestTable {
+  scalar:int (id: 0);
+  vector_of_scalars:[int] (id: 1, static_length: 3);
+  string:string (id: 2, static_length: 20);
+  vector_of_strings:[string] (id: 3, static_length: 3, static_vector_string_length: 10);
+  substruct:SubStruct (id: 4);
+  subtable:SubTable (id: 5);
+  // The force-aligned vector is deliberately put in the middle of the table
+  // both by ID and alphabetically (both of these can affect the order in which
+  // certain things are evaluated, and during development there were some issues
+  // with this).
+  vector_aligned:[int] (id: 6, force_align: 64, static_length: 3);
+  vector_of_structs:[SubStruct] (id: 7, static_length: 3);
+  vector_of_tables:[SubTable] (id: 8, static_length: 3);
+  included_table:aos.fbs.testing.included.IncludedTable (id: 9);
+  unspecified_length_vector:[ubyte] (id: 10);
+  unspecified_length_string:string (id: 11);
+  unspecified_length_vector_of_strings:[string] (id: 12);
+}
+
+root_type TestTable;
+```
+
+All created types have an `AsFlatbuffer()` method which allows you to access the
+type using the regular generated flatbuffer API and a `FromFlatbuffer()` method
+which attempts to copy the specified flatbuffer into the current object.
+
+### Sample Usage
+
+The below example constructs a table of the above example `TestTable`:
+
+```cpp
+aos::FixedAllocator allocator(TestTableStatic::kUnalignedBufferSize);
+Builder<TestTableStatic> builder(&allocator);
+TestTableStatic *object = builder.get();
+object->set_scalar(123);
+{
+  auto vector = object->add_vector_of_scalars();
+  CHECK(vector->emplace_back(4));
+  CHECK(vector->emplace_back(5));
+}
+{
+  auto string = object->add_string();
+  string->SetString("Hello, World!");
+}
+{
+  auto vector_of_strings = object->add_vector_of_strings();
+  auto sub_string = CHECK_NOTNULL(vector_of_strings->emplace_back());
+  CHECK(sub_string->emplace_back('D'));
+}
+{
+  object->set_substruct({971, 254});
+}
+{
+  auto subtable = object->add_subtable();
+  subtable->set_foo(1234);
+}
+{
+  auto vector = object->add_vector_of_structs();
+  CHECK(vector->emplace_back({48, 67}));
+  CHECK(vector->emplace_back({118, 148}));
+  CHECK(vector->emplace_back({971, 973}));
+  // Max vector size is three; this should fail.
+  CHECK(!vector->emplace_back({1114, 2056}));
+}
+{
+  auto vector = object->add_vector_of_tables();
+  auto subobject = vector->emplace_back();
+  subobject->set_foo(222);
+}
+{
+  auto subtable = object->add_included_table();
+  subtable->set_foo(included::TestEnum::B);
+}
+LOG(INFO) <<
+    aos::FlatbufferToJson(builder.AsFlatbufferSpan(),
+                          {.multi_line = true});
+```
+
+This will then output:
+
+```json
+{
+ "scalar": 123,
+ "vector_of_scalars": [
+  4,
+  5
+ ],
+ "string": "Hello, World!",
+ "vector_of_strings": [
+  "D"
+ ],
+ "substruct": {
+  "x": 971.0,
+  "y": 254.0
+ },
+ "subtable": {
+  "foo": 1234
+ },
+ "vector_of_structs": [
+  {
+   "x": 48.0,
+   "y": 67.0
+  },
+  {
+   "x": 118.0,
+   "y": 148.0
+  },
+  {
+   "x": 971.0,
+   "y": 973.0
+  }
+ ],
+ "vector_of_tables": [
+  {
+   "foo": 222
+  }
+ ],
+ "included_table": {
+  "foo": "B"
+ }
+}
+```
+
+### Converting `Populate*()` methods
+
+With existing flatbuffer code it is common to have
+`flatbuffers::Offset<> Populate*(FlatBufferBuilder*)` methods for populating
+subtables of a message. When converting these to the static API, you can
+keep the same patterns (although you have more flexibility available if you
+choose), but modify the `Populate` call slightly:
+
+```cpp
+namespace {
+flatbuffers::Offset<SubTable> PopulateOld(flatbuffers::FlatBufferBuilder *fbb) {
+  SubTable::Builder builder(*fbb);
+  builder.add_foo(1234);
+  return builder.Finish();
+}
+void PopulateStatic(SubTableStatic *subtable) { subtable->set_foo(1234); }
+}  // namespace
+TEST_F(StaticFlatbuffersTest, PopulateMethodConversionExample) {
+  // Using a FlatBufferBuilder:
+  flatbuffers::FlatBufferBuilder fbb;
+  // Note: the PopulateOld() *must* be called prior to creating the builder.
+  const flatbuffers::Offset<SubTable> subtable_offset = PopulateOld(&fbb);
+  TestTable::Builder testtable_builder(fbb);
+  testtable_builder.add_subtable(subtable_offset);
+  fbb.Finish(testtable_builder.Finish());
+  aos::FlatbufferDetachedBuffer<TestTable> fbb_finished = fbb.Release();
+
+  // Using the static flatbuffer API.
+  aos::fbs::VectorAllocator allocator;
+  Builder<TestTableStatic> static_builder(&allocator);
+  PopulateStatic(CHECK_NOTNULL(static_builder.get()->add_subtable()));
+
+  // And confirm that they both contain the expected flatbuffer:
+  const std::string expected = R"json({ "subtable": { "foo": 1234 }})json";
+  EXPECT_EQ(expected, aos::FlatbufferToJson(fbb_finished));
+  EXPECT_EQ(expected, aos::FlatbufferToJson(static_builder.AsFlatbufferSpan()));
+}
+```
+
+### Scalar Fields
+
+Scalar fields have an API which is reasonably close to that of the base
+flatbuffer builder API. Because space for the scalar fields (as with everything)
+is pre-allocated, these accessors may be called at any time.
+
+For an `int` field named `scalar`, we will have the following methods. Note that
+prior to any `set_*` method being called, the value will not be populated and so
+`has_*` methods will return false and accessors will return `nullopt/nullptr`:
+
+```cpp
+// Populates the value and sets it to the requested value. Calling set_scalar()
+// will cause has_scalar() to return true.
+void set_scalar(const int32_t &value);
+
+// Returns the value of scalar, if populated. Otherwise, returns nullopt.
+std::optional<int32_t> scalar() const;
+
+// Returns a pointer to the scalar, if populated. Otherwise, returns nullptr.
+// Note that because of the nature of this API we _could_ support always
+// returning a valid pointer, but then it would be relatively easy for a user
+// to modify the value of a field without ever causing it to become "populated."
+int32_t *mutable_scalar();
+
+// Clears the field. Does not invalidate pointers returned by
+// `mutable_scalar()`, although it will set the value of the field to zero.
+void clear_scalar();
+
+// Returns true if the scalar field is populated.
+bool has_scalar() const;
+```
+
+### Enum fields
+
+Enum fields operate identically to scalar fields, except that the type in
+question is the flatbuffer enum type rather than a C++ scalar of some sort.
+
+### Struct fields
+
+Struct fields operate identically to scalar fields, except that the type in
+question is the flatbuffer C-struct type rather than a scalar.
+
+*Note*: This is different than how the raw flatbuffer API handles structs.
+Regular flatbuffers actually pass around pointers to the structs rather than
+references.
+
+### Table fields
+
+For fields of a table which are themselves tables, the accessors will return a
+pointer to an object that can be used to access/populate the subtable in
+question. The accessors are generally similar to those used by the scalar
+fields.
+
+The accessors that will be generated for a field named `subtable` of type
+`SubTable` are below:
+
+```cpp
+// Creates a SubTable at the subtable member.
+// Will die if the field is already populated (this aspect of the API is
+// subject to change if we discover that people like to be able to call
+// add_* multiple times).
+aos::fbs::testing::SubTableStatic *add_subtable();
+
+// The following will return pointers to the subtable member, or nullptr
+// if it is not populated.
+const aos::fbs::testing::SubTableStatic *subtable() const:
+aos::fbs::testing::SubTableStatic *mutable_subtable();
+
+// Depopulates the subtable member.
+void clear_subtable();
+
+// Returns true if the subtable has been populated. This does not
+// mean that there is necessarily anything interesting *in* the table,
+// just that it exists and can be modified.
+bool has_subtable() const;
+```
+
+### Vectors
+
+A vector may contain any other type, except for other vectors (with the
+exception of strings---vectors of strings are permitted). The APIs for
+inline types versus not-inline types (name improvements are welcome...
+maybe "object"?) are slightly different because of differences in how
+the underlying serialization works.
+
+As already mentioned, each vector will have a "static" size, which is specified
+in the flatbuffer schema by the `static_length` attribute (in order to use
+this attribute you must have an `attribute "static_length";` line
+somewhere in your `.fbs` file). This represents the number of elements that will
+have space pre-allocated in the vector. Changing this number does not cause any issues
+with backwards compatibility because the underlying flatbuffer representation
+permits arbitrary (up to 2^32) length vectors. This is necessary for
+choosing how much space to allocate when constructing the flatbuffer.
+The maximum size of a vector may be accessed at runtime using the `capacity()`
+accessor on the `aos::fbs::Vector` type.
+
+*Note*: You may not use dynamically sized vectors of strings or tables in
+realtime code, as allocating space for each additional string/table member
+requires overhead which cannot be conveniently allocated anywhere except the
+heap. The primary use-case for dynamically sized vectors in realtime code
+is for vectors of scalars; if this changes, we can try to add options to
+support this. Dynamically sized vectors of tables/strings are supported in
+non-realtime code.
+
+If you wish to increase the alignment of a vector beyond the base alignment, you
+can use the `force_align` attribute , as seen below:
+
+```
+vector_aligned:[int] (id: 6, force_align: 64, static_length: 3);
+```
+
+If you do this, the first element of the vector will be aligned to the requested
+alignment.
+
+The `aos::fbs::Vector` API is designed to mirror the `std::vector` API, with
+some changes to accommodate better error-handling. Common accessors:
+
+* `capacity()`: Maximum number of elements that this vector can accommodate.
+* `size()`: Current number of elements populated in this vector.
+* `T *emplace_back()`: Adds a not-inline (string or table) type to the vector and returns
+  the added object. If there is no more space, returns `nullptr` (call
+  `reserve()` to attempt to allocate more space).
+* `bool emplace_back(T)`: Adds an inline (scalar, enum, or struct) type to the vector and
+  returns true on success. Returns false if there is no more space in the
+  vector (call `reserve()` to attempt to allocate more space).
+* `AsFlatbufferVector()`, `AsMutableFlatbufferVector()`: Returns a
+  `flatbuffer::Vector` of the appropriate type pointing to the vector
+  that we are constructing.
+* `T &at(size_t index)`/`T& operator[](size_t index)`: Returns the
+  object at the requested index. Dies if `index >= size()` (unlike
+  `std::vector`, `operator[]` does do bounds checking. Use `unsafe_at()` if you
+  want to avoid the performance overhead of bounds checking).
+* `resize_inline(size_t size, SetZero set_zero)`/`resize(size_t size)`:
+  Resizes the vector to the requested size (dies if the vector cannot
+  accommodate the requested size). For inline types,
+  you may optionally leave any newly inserted elements uninitialized.
+  For not-inline types, will default construct new elements.
+* `T* data()`: Returns a pointer to the first element of the vector. Only valid
+  for inline data types.
+* `bool reserve(size_t new_length)`: Used to dynamically change the amount of
+  space allocated for the vector; returns false on failure (e.g., if you are in
+  a fixed-size allocator that does not support increasing the size past a
+  certain point).
+* `bool FromFlatbuffer(const flatbuffers::Vector<>*)`: Attempts to copy an
+  existing vector into this `Vector`. This may attempt to call `reserve()`
+  if the new vector is longer than `capacity()`. If the copy fails for
+  any reason, returns `false`.
+
+#### Managing Resizing of Vectors
+
+When dealing with sizes of vectors, there are two separate "lengths" that are
+relevant at any given time:
+
+1. The `capacity`/allocated length of the vector. This is the length for which
+   there is currently space allocated in the flatbuffer array and in the
+   `Vector` object itself. Upon initialization, this will be equal to the
+   `static_length` for the vector. This can only be changed by calling
+   `reserve()` or indirectly in `FromFlatbuffer()` (which calls `reserve()`).
+2. The `size`/current actual length of the vector. This is the number of
+   elements that are currently actually populated in the vector. The current
+   `size` of the vector cannot exceed the `capacity`. This will be modified by
+   calls to `emplace_back()`/`Resize*()` (and indirectly by `FromFlatbuffer()`).
+
+Because `emplace_back()` and `Resize*()` do not call `reserve()` themselves, they will
+return `false` if the capacity of the vector does not currently allow for the
+element to be added; when `emplace_back()` returns false, you may call `reserve()` to
+attempt to allocate the requisite space; if the allocation itself fails (e.g.,
+if you are allocating against a fixed size buffer and do not have sufficient
+space for the requested allocation). This means that the user will be forced to
+explicitly request changes to the memory layout and allocation of the flatbuffer
+rather than being able to hide it inside of calls to `emplace_back()` or the such.
+
+### Strings
+
+Strings are a special case of vectors. The key differences are:
+
+* They are always null-terminated (this is enforced by the API
+  itself).
+* Because they are null-terminated, the actual vector length will
+  be `static_length + 1`. Users should not attempt to
+  access this extra character.
+* The vector elements are of type `char`.
+* Strings are supposed to be UTF-8. This C++ API does not enforce
+  this constraint, but if you want a vector of bytes, then use
+  a vector of bytes (`[ubyte]`) instead.
+* For the special-case where you have a vector of strings, you may specify
+  the static length of the strings inside of the vector using the
+  `static_vector_string_length` attribute.
+
+
+## Use With EventLoop Senders
+
+In order to use the static API with `aos::Sender`s you need to:
+
+1. Change the `aos::Sender` to be templated on the `MessageStatic` instead of
+   the `Message` type.
+2. Use the `MakeStaticBuilder` method instead of the `MakeBuilder` method.
+3. Alter the actual flatbuffer API calls to use the new object.
+4. In the `Send()` call, remove the `builder.Finish()` as it is no longer necessary.
+
+### Simple Conversion Example
+
+This is an extremely simple example of converting a sender to the new API, taken
+from the `event_loop_param_test.cc`:
+
+First, the bazel targets must be updated to generate the new code:
+
+```python
+# This load must be added so that the static_flatbuffer rule is available.
+load("@org_frc971//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
+# Remove the prior `flatbuffer_cc_library` and replace it with something like
+# this.
+# This target had no dependencies, but any deps will also need to be
+# upgraded to static_flatbuffer rules.
+static_flatbuffer(
+    name = "test_message_fbs",
+    src = "test_message.fbs",
+)
+```
+
+Before:
+```cpp
+  aos::Sender<TestMessage> sender = loop1->MakeSender<TestMessage>("/test");
+
+  loop->OnRun([&]() {
+    aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+    TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+    builder.add_value(200);
+    msg.CheckOk(msg.Send(builder.Finish()));
+  });
+```
+
+After:
+```cpp
+  aos::Sender<TestMessageStatic> sender =
+      loop1->MakeSender<TestMessageStatic>("/test");
+
+  loop->OnRun([&]() {
+    aos::Sender<TestMessageStatic>::StaticBuilder msg =
+        sender.MakeStaticBuilder();
+    msg->set_value(200);
+    msg.CheckOk(msg.Send());
+  });
+```
+
+## Future Improvements
+
+### Suggested API Additions/Improvements
+
+* A  `add_or_get_subtable` generated method that avoids the need for the user to
+  check `has_subtable()` before calling `add_subtable()`.
diff --git a/documentation/aos/examples/BUILD b/documentation/aos/examples/BUILD
index f224409..bf3e2eb 100644
--- a/documentation/aos/examples/BUILD
+++ b/documentation/aos/examples/BUILD
@@ -1,9 +1,8 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "sensor_data_fbs",
     srcs = ["sensor_data.fbs"],
-    gen_reflections = True,
 )
 
 cc_library(
diff --git a/frc971/BUILD b/frc971/BUILD
index 4287462..dd1b367 100644
--- a/frc971/BUILD
+++ b/frc971/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
@@ -17,6 +17,10 @@
         "constants.h",
     ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/zeroing:constants_fbs",
+    ],
 )
 
 py_library(
@@ -26,12 +30,11 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "can_configuration_fbs",
     srcs = [
         ":can_configuration.fbs",
     ],
-    gen_reflections = 1,
     visibility = ["//visibility:public"],
 )
 
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 9b9ab86..93f50f4 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -1,5 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 load("//aos:config.bzl", "aos_config")
 
@@ -112,12 +112,11 @@
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "plot_data_fbs",
     srcs = [
         "plot_data.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/frc971/autonomous/BUILD b/frc971/autonomous/BUILD
index 58dce60..0dd86ca 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -1,20 +1,18 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "auto_fbs",
     srcs = ["auto.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "auto_mode_fbs",
     srcs = ["auto_mode.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -45,6 +43,20 @@
     ],
 )
 
+cc_library(
+    name = "user_button_localized_autonomous_actor",
+    srcs = [
+        "user_button_localized_autonomous_actor.cc",
+    ],
+    hdrs = [
+        "user_button_localized_autonomous_actor.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":base_autonomous_actor",
+    ],
+)
+
 aos_config(
     name = "aos_config",
     src = "autonomous_config.json",
diff --git a/frc971/autonomous/user_button_localized_autonomous_actor.cc b/frc971/autonomous/user_button_localized_autonomous_actor.cc
new file mode 100644
index 0000000..5358a28
--- /dev/null
+++ b/frc971/autonomous/user_button_localized_autonomous_actor.cc
@@ -0,0 +1,113 @@
+#include "frc971/autonomous/user_button_localized_autonomous_actor.h"
+
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+namespace this_thread = ::std::this_thread;
+namespace drivetrain = frc971::control_loops::drivetrain;
+
+namespace frc971 {
+namespace autonomous {
+
+UserButtonLocalizedAutonomousActor::UserButtonLocalizedAutonomousActor(
+    ::aos::EventLoop *event_loop,
+    const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
+    : BaseAutonomousActor(event_loop, dt_config),
+      robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
+      joystick_state_fetcher_(
+          event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+  drivetrain_status_fetcher_.Fetch();
+  replan_timer_ = event_loop->AddTimer([this]() { DoReplan(); });
+
+  event_loop->OnRun([this, event_loop]() {
+    replan_timer_->Schedule(event_loop->monotonic_now());
+    button_poll_->Schedule(event_loop->monotonic_now(),
+                           chrono::milliseconds(50));
+  });
+
+  button_poll_ = event_loop->AddTimer([this]() {
+    const aos::monotonic_clock::time_point now =
+        this->event_loop()->context().monotonic_event_time;
+    if (robot_state_fetcher_.Fetch()) {
+      if (robot_state_fetcher_->user_button()) {
+        user_indicated_safe_to_reset_ = true;
+        MaybeSendStartingPosition();
+      }
+    }
+    if (joystick_state_fetcher_.Fetch()) {
+      if (joystick_state_fetcher_->has_alliance() &&
+          (joystick_state_fetcher_->alliance() != alliance_)) {
+        alliance_ = joystick_state_fetcher_->alliance();
+        is_planned_ = false;
+        // Only kick the planning out by 2 seconds. If we end up enabled in
+        // that second, then we will kick it out further based on the code
+        // below.
+        replan_timer_->Schedule(now + std::chrono::seconds(2));
+      }
+      if (joystick_state_fetcher_->enabled()) {
+        if (!is_planned_) {
+          // Only replan once we've been disabled for 5 seconds.
+          replan_timer_->Schedule(now + std::chrono::seconds(5));
+        }
+      }
+    }
+  });
+}
+
+void UserButtonLocalizedAutonomousActor::DoReplan() {
+  if (!drivetrain_status_fetcher_.Fetch()) {
+    replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
+    AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
+    return;
+  }
+
+  if (alliance_ == aos::Alliance::kInvalid) {
+    return;
+  }
+
+  sent_starting_position_ = false;
+
+  Replan();
+}
+
+void UserButtonLocalizedAutonomousActor::MaybeSendStartingPosition() {
+  if (is_planned_ && user_indicated_safe_to_reset_ &&
+      !sent_starting_position_) {
+    CHECK(starting_position_);
+    SendStartingPosition(starting_position_.value());
+  }
+}
+
+void UserButtonLocalizedAutonomousActor::DoReset() {
+  InitializeEncoders();
+  ResetDrivetrain();
+
+  joystick_state_fetcher_.Fetch();
+  CHECK(joystick_state_fetcher_.get() != nullptr)
+      << "Expect at least one JoystickState message before running auto...";
+  alliance_ = joystick_state_fetcher_->alliance();
+
+  Reset();
+}
+
+bool UserButtonLocalizedAutonomousActor::RunAction(
+    const ::frc971::autonomous::AutonomousActionParams *params) {
+  DoReset();
+
+  AOS_LOG(INFO, "Params are %d\n", params->mode());
+
+  if (!user_indicated_safe_to_reset_) {
+    AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+    CHECK(starting_position_);
+    SendStartingPosition(starting_position_.value());
+  }
+  // Clear this so that we don't accidentally resend things as soon as we
+  // replan later.
+  user_indicated_safe_to_reset_ = false;
+  is_planned_ = false;
+  starting_position_.reset();
+
+  return Run(params);
+}
+
+}  // namespace autonomous
+}  // namespace frc971
diff --git a/frc971/autonomous/user_button_localized_autonomous_actor.h b/frc971/autonomous/user_button_localized_autonomous_actor.h
new file mode 100644
index 0000000..07d083a
--- /dev/null
+++ b/frc971/autonomous/user_button_localized_autonomous_actor.h
@@ -0,0 +1,60 @@
+#ifndef FRC971_AUTONOMOUS_EXTENDED_AUTONOMOUS_ACTOR_H_
+#define FRC971_AUTONOMOUS_EXTENDED_AUTONOMOUS_ACTOR_H_
+
+#include <memory>
+
+#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
+#include "aos/events/shm_event_loop.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
+
+namespace frc971 {
+namespace autonomous {
+
+class UserButtonLocalizedAutonomousActor : public BaseAutonomousActor {
+ public:
+  explicit UserButtonLocalizedAutonomousActor(
+      ::aos::EventLoop *event_loop,
+      const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+
+  bool RunAction(
+      const ::frc971::autonomous::AutonomousActionParams *params) override;
+
+ protected:
+  virtual bool Run(
+      const ::frc971::autonomous::AutonomousActionParams *params) = 0;
+  virtual void SendStartingPosition(const Eigen::Vector3d &start) = 0;
+  virtual void Replan() = 0;
+  virtual void Reset() = 0;
+
+  void MaybeSendStartingPosition();
+
+  ::aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+  ::aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+
+  aos::Alliance alliance_ = aos::Alliance::kInvalid;
+  bool is_planned_ = false;
+  bool sent_starting_position_ = false;
+
+  std::optional<Eigen::Vector3d> starting_position_;
+
+ private:
+  void DoReplan();
+  void DoReset();
+
+  aos::TimerHandler *replan_timer_;
+  aos::TimerHandler *button_poll_;
+
+  bool user_indicated_safe_to_reset_ = false;
+};
+
+}  // namespace autonomous
+}  // namespace frc971
+
+#endif  // FRC971_AUTONOMOUS_EXTENDED_AUTONOMOUS_ACTOR_H_
diff --git a/frc971/can_configuration.fbs b/frc971/can_configuration.fbs
index 4ce70ab..c85b0e8 100644
--- a/frc971/can_configuration.fbs
+++ b/frc971/can_configuration.fbs
@@ -3,7 +3,7 @@
 // Message which triggers wpilib_interface to print out the current
 // configuration, and optionally re-apply it.
 table CANConfiguration {
-  // If true, re-apply the configs to see if that fixes the falcon.
+  // If true, re-apply the configs to see if that fixes the TalonFX Motor.
   reapply:bool = false (id: 0);
 }
 
diff --git a/frc971/can_logger/BUILD b/frc971/can_logger/BUILD
index a18a379..f568aa9 100644
--- a/frc971/can_logger/BUILD
+++ b/frc971/can_logger/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 cc_binary(
     name = "can_logger",
@@ -29,12 +29,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "can_logging_fbs",
     srcs = [
         "can_logging.fbs",
     ],
-    gen_reflections = 1,
     visibility = ["//visibility:public"],
 )
 
diff --git a/frc971/codelab/BUILD b/frc971/codelab/BUILD
index 4e1da3d..5e0c1ea 100644
--- a/frc971/codelab/BUILD
+++ b/frc971/codelab/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 
 package(default_visibility = ["//visibility:public"])
@@ -37,39 +37,35 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "basic_goal_fbs",
     srcs = [
         "basic_goal.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "basic_output_fbs",
     srcs = [
         "basic_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "basic_status_fbs",
     srcs = [
         "basic_status.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "basic_position_fbs",
     srcs = [
         "basic_position.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/frc971/constants.h b/frc971/constants.h
index cf53287..0367937 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -3,142 +3,34 @@
 
 #include <cstddef>
 
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/zeroing/constants_generated.h"
+
 namespace frc971 {
 namespace constants {
 
-struct HallEffectZeroingConstants {
-  // The absolute position of the lower edge of the hall effect sensor.
-  double lower_hall_position;
-  // The absolute position of the upper edge of the hall effect sensor.
-  double upper_hall_position;
-  // The difference in scaled units between two hall effect edges.  This is the
-  // number of units/cycle.
-  double index_difference;
-  // Number of cycles we need to see the hall effect high.
-  size_t hall_trigger_zeroing_length;
-  // Direction the system must be moving in order to zero. True is positive,
-  // False is negative direction.
-  bool zeroing_move_direction;
-};
+typedef frc971::zeroing::HallEffectZeroingConstantsT HallEffectZeroingConstants;
 
-struct PotAndIndexPulseZeroingConstants {
-  // The number of samples in the moving average filter.
-  size_t average_filter_size;
-  // The difference in scaled units between two index pulses.
-  double index_difference;
-  // The absolute position in scaled units of one of the index pulses.
-  double measured_index_position;
-  // Value between 0 and .5 which determines a fraction of the index_diff
-  // you want to use.
-  double allowable_encoder_error;
-};
+typedef frc971::zeroing::PotAndIndexPulseZeroingConstantsT
+    PotAndIndexPulseZeroingConstants;
 
-struct EncoderPlusIndexZeroingConstants {
-  // The amount of index pulses in the joint's range of motion.
-  int index_pulse_count;
-  // The difference in scaled units between two index pulses.
-  double index_difference;
-  // The absolute position in scaled units of one of the index pulses.
-  double measured_index_position;
-  // The index pulse that is known, going from lowest in the range of motion to
-  // highest (Starting at 0).
-  int known_index_pulse;
-  // Value between 0 and 0.5 which determines a fraction of the index_diff
-  // you want to use. If an index pulse deviates by more than this amount from
-  // where we expect to see one then we flag an error.
-  double allowable_encoder_error;
-};
+typedef frc971::zeroing::EncoderPlusIndexZeroingConstantsT
+    EncoderPlusIndexZeroingConstants;
 
-struct PotAndAbsoluteEncoderZeroingConstants {
-  // The number of samples in the moving average filter.
-  size_t average_filter_size;
-  // The distance that the absolute encoder needs to complete a full rotation.
-  double one_revolution_distance;
-  // Measured absolute position of the encoder when at zero.
-  double measured_absolute_position;
+typedef frc971::zeroing::PotAndAbsoluteEncoderZeroingConstantsT
+    PotAndAbsoluteEncoderZeroingConstants;
 
-  // Treshold for deciding if we are moving. moving_buffer_size samples need to
-  // be within this distance of each other before we use the middle one to zero.
-  double zeroing_threshold;
-  // Buffer size for deciding if we are moving.
-  size_t moving_buffer_size;
+typedef frc971::zeroing::RelativeEncoderZeroingConstantsT
+    RelativeEncoderZeroingConstants;
 
-  // Value between 0 and 1 indicating what fraction of one_revolution_distance
-  // it is acceptable for the offset to move.
-  double allowable_encoder_error;
-};
+typedef frc971::zeroing::ContinuousAbsoluteEncoderZeroingConstantsT
+    ContinuousAbsoluteEncoderZeroingConstants;
 
-struct RelativeEncoderZeroingConstants {};
+typedef frc971::zeroing::AbsoluteEncoderZeroingConstantsT
+    AbsoluteEncoderZeroingConstants;
 
-struct ContinuousAbsoluteEncoderZeroingConstants {
-  // The number of samples in the moving average filter.
-  size_t average_filter_size;
-  // The distance that the absolute encoder needs to complete a full rotation.
-  // It is presumed that this will always be 2 * pi for any subsystem using this
-  // class, unless you have a continuous system that for some reason doesn't
-  // have a logical period of 1 revolution in radians.
-  double one_revolution_distance;
-  // Measured absolute position of the encoder when at zero.
-  double measured_absolute_position;
-
-  // Threshold for deciding if we are moving. moving_buffer_size samples need to
-  // be within this distance of each other before we use the middle one to zero.
-  double zeroing_threshold;
-  // Buffer size for deciding if we are moving.
-  size_t moving_buffer_size;
-
-  // Value between 0 and 1 indicating what fraction of a revolution
-  // it is acceptable for the offset to move.
-  double allowable_encoder_error;
-};
-
-struct AbsoluteEncoderZeroingConstants {
-  // The number of samples in the moving average filter.
-  size_t average_filter_size;
-  // The distance that the absolute encoder needs to complete a full rotation.
-  double one_revolution_distance;
-  // Measured absolute position of the encoder when at zero.
-  double measured_absolute_position;
-  // Position of the middle of the range of motion in output coordinates.
-  double middle_position;
-
-  // Threshold for deciding if we are moving. moving_buffer_size samples need to
-  // be within this distance of each other before we use the middle one to zero.
-  double zeroing_threshold;
-  // Buffer size for deciding if we are moving.
-  size_t moving_buffer_size;
-
-  // Value between 0 and 1 indicating what fraction of one_revolution_distance
-  // it is acceptable for the offset to move.
-  double allowable_encoder_error;
-};
-
-struct AbsoluteAndAbsoluteEncoderZeroingConstants {
-  // The number of samples in the moving average filter.
-  size_t average_filter_size;
-  // The distance that the absolute encoder needs to complete a full rotation.
-  double one_revolution_distance;
-  // Measured absolute position of the encoder when at zero.
-  double measured_absolute_position;
-
-  // The distance that the single turn absolute encoder needs to complete a full
-  // rotation.
-  double single_turn_one_revolution_distance;
-  // Measured absolute position of the single turn encoder when at zero.
-  double single_turn_measured_absolute_position;
-  // Position of the middle of the range of motion in output coordinates.
-  double single_turn_middle_position;
-
-  // Threshold for deciding if we are moving. moving_buffer_size samples need to
-  // be within this distance of each other before we use the middle one to zero.
-  double zeroing_threshold;
-  // Buffer size for deciding if we are moving.
-  size_t moving_buffer_size;
-
-  // Value between 0 and 1 indicating what fraction of one_revolution_distance
-  // it is acceptable for the offset to move.
-  double allowable_encoder_error;
-};
+typedef frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingConstantsT
+    AbsoluteAndAbsoluteEncoderZeroingConstants;
 
 // Defines a range of motion for a subsystem.
 // These are all absolute positions in scaled units.
@@ -152,6 +44,13 @@
   constexpr double middle_soft() const { return (lower + upper) / 2.0; }
 
   constexpr double range() const { return upper_hard - lower_hard; }
+
+  static Range FromFlatbuffer(const frc971::Range *range) {
+    return {.lower_hard = range->lower_hard(),
+            .upper_hard = range->upper_hard(),
+            .lower = range->lower(),
+            .upper = range->upper()};
+  }
 };
 
 }  // namespace constants
diff --git a/frc971/constants/testdata/BUILD b/frc971/constants/testdata/BUILD
index a20264c..fee7b86 100644
--- a/frc971/constants/testdata/BUILD
+++ b/frc971/constants/testdata/BUILD
@@ -1,26 +1,24 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 exports_files([
     "test_constants.json",
     "syntax_error.json",
 ])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "constants_list_fbs",
     srcs = ["constants_list.fbs"],
-    gen_reflections = 1,
-    includes = [
-        ":constants_data_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
+    deps = [
+        ":constants_data_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "constants_data_fbs",
     srcs = ["constants_data.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index c03a964..53d6b27 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,5 +1,5 @@
 load("//aos:config.bzl", "aos_config")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 package(default_visibility = ["//visibility:public"])
@@ -192,7 +192,7 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "control_loops_fbs",
     srcs = [
         "control_loops.fbs",
@@ -200,16 +200,16 @@
 )
 
 flatbuffer_ts_library(
-    name = "can_falcon_ts_fbs",
+    name = "can_talonfx_ts_fbs",
     srcs = [
-        "can_falcon.fbs",
+        "can_talonfx.fbs",
     ],
 )
 
-flatbuffer_cc_library(
-    name = "can_falcon_fbs",
+static_flatbuffer(
+    name = "can_talonfx_fbs",
     srcs = [
-        "can_falcon.fbs",
+        "can_talonfx.fbs",
     ],
 )
 
@@ -395,15 +395,15 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "profiled_subsystem_fbs",
     srcs = [
         "profiled_subsystem.fbs",
     ],
-    includes = [
-        ":control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":control_loops_fbs",
+    ],
 )
 
 cc_library(
@@ -575,76 +575,70 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_fbs",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        ":control_loops_fbs_includes",
-        ":profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":control_loops_fbs",
+        ":profiled_subsystem_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_fbs",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test_subsystem_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_position_fbs",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        ":control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "static_zeroing_single_dof_profiled_subsystem_test_absolute_position_fbs",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test_absolute_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        ":control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_encoder_status_fbs",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_encoder_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        ":control_loops_fbs_includes",
-        ":profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":control_loops_fbs",
+        ":profiled_subsystem_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "static_zeroing_single_dof_profiled_subsystem_test_absolute_encoder_status_fbs",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test_absolute_encoder_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        ":control_loops_fbs_includes",
-        ":profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":control_loops_fbs",
+        ":profiled_subsystem_fbs",
+    ],
 )
 
 cc_test(
diff --git a/frc971/control_loops/can_falcon.fbs b/frc971/control_loops/can_falcon.fbs
deleted file mode 100644
index 2adbae0..0000000
--- a/frc971/control_loops/can_falcon.fbs
+++ /dev/null
@@ -1,26 +0,0 @@
-namespace frc971.control_loops;
-
-table CANFalcon {
-  // The CAN id of the falcon
-  id:int (id: 0);
-
-  // In Amps
-  supply_current:float (id: 1);
-
-  // In Amps
-  // Stator current where positive current means torque is applied in
-  // the motor's forward direction as determined by its Inverted setting.
-  torque_current:float (id: 2);
-
-  // In Volts
-  supply_voltage:float (id: 3);
-
-  // In degrees Celsius
-  device_temp:float (id: 4);
-
-  // In meters traveled on the drivetrain
-  position:float (id: 5);
-
-  // Nominal range is -1 to 1, but can be -2 to +2
-  duty_cycle: float (id: 6);
-}
diff --git a/frc971/control_loops/can_talonfx.fbs b/frc971/control_loops/can_talonfx.fbs
new file mode 100644
index 0000000..81efbb1
--- /dev/null
+++ b/frc971/control_loops/can_talonfx.fbs
@@ -0,0 +1,26 @@
+namespace frc971.control_loops;
+
+table CANTalonFX {
+  // The CAN id of the talonfx motor
+  id:int (id: 0);
+
+  // In Amps
+  supply_current:float (id: 1);
+
+  // In Amps
+  // Stator current where positive current means torque is applied in
+  // the motor's forward direction as determined by its Inverted setting.
+  torque_current:float (id: 2);
+
+  // In Volts
+  supply_voltage:float (id: 3);
+
+  // In degrees Celsius
+  device_temp:float (id: 4);
+
+  // In meters traveled on the drivetrain
+  position:float (id: 5);
+
+  // Nominal range is -1 to 1, but can be -2 to +2
+  duty_cycle: float (id: 6);
+}
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index da2dc05..db2695e 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -256,6 +256,13 @@
   max_acceleration:float (id: 1);
 }
 
+table Range {
+  lower_hard:double (id: 0);
+  upper_hard:double (id: 1);
+  lower:double (id: 2);
+  upper:double (id: 3);
+}
+
 enum ConstraintType : byte {
   CONSTRAINT_TYPE_UNDEFINED,
   LONGITUDINAL_ACCELERATION,
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index e631799..7a25bcd 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,5 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 load("//aos:config.bzl", "aos_config")
 load("//tools/build_rules:select.bzl", "cpu_select")
@@ -7,54 +7,47 @@
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_can_position_fbs",
     srcs = ["drivetrain_can_position.fbs"],
-    gen_reflections = 1,
-    deps = ["//frc971/control_loops:can_falcon_fbs"],
+    deps = ["//frc971/control_loops:can_talonfx_fbs"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "spline_goal_fbs",
     srcs = ["spline_goal.fbs"],
-    gen_reflections = 1,
-    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+    deps = ["//frc971/control_loops:control_loops_fbs"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_goal_fbs",
     srcs = ["drivetrain_goal.fbs"],
-    gen_reflections = 1,
-    includes = [
-        ":spline_goal_fbs_includes",
-        "//frc971/control_loops:control_loops_fbs_includes",
+    deps = [
+        ":spline_goal_fbs",
+        "//frc971/control_loops:control_loops_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_output_fbs",
     srcs = ["drivetrain_output.fbs"],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_position_fbs",
     srcs = ["drivetrain_position.fbs"],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_status_fbs",
     srcs = ["drivetrain_status.fbs"],
-    gen_reflections = 1,
-    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+    deps = ["//frc971/control_loops:control_loops_fbs"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "trajectory_fbs",
     srcs = ["trajectory.fbs"],
-    gen_reflections = 1,
-    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+    deps = ["//frc971/control_loops:control_loops_fbs"],
 )
 
 cc_static_flatbuffer(
@@ -81,7 +74,7 @@
     name = "drivetrain_can_position_ts_fbs",
     srcs = ["drivetrain_can_position.fbs"],
     target_compatible_with = ["@platforms//os:linux"],
-    deps = ["//frc971/control_loops:can_falcon_ts_fbs"],
+    deps = ["//frc971/control_loops:can_talonfx_ts_fbs"],
 )
 
 genrule(
@@ -112,33 +105,29 @@
     cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_goal_float_fbs",
     srcs = ["drivetrain_goal_float.fbs"],
-    gen_reflections = 1,
-    includes = [
-        ":spline_goal_fbs_includes",
-        "//frc971/control_loops:control_loops_fbs_includes",
+    deps = [
+        ":spline_goal_fbs",
+        "//frc971/control_loops:control_loops_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_output_float_fbs",
     srcs = ["drivetrain_output_float.fbs"],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_position_float_fbs",
     srcs = ["drivetrain_position_float.fbs"],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_status_float_fbs",
     srcs = ["drivetrain_status_float.fbs"],
-    gen_reflections = 1,
-    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+    deps = ["//frc971/control_loops:control_loops_fbs"],
 )
 
 aos_config(
@@ -228,10 +217,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "localizer_fbs",
     srcs = ["localizer.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
index 192f23d..b451ea9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -1,9 +1,9 @@
-include "frc971/control_loops/can_falcon.fbs";
+include "frc971/control_loops/can_talonfx.fbs";
 namespace frc971.control_loops.drivetrain;
 
 // CAN readings from the CAN sensor reader loop
 table CANPosition {
-  falcons: [CANFalcon] (id: 0);
+  talonfxs: [CANTalonFX] (id: 0);
 
   // The timestamp of the measurement on the canivore clock in nanoseconds
   // This will have less jitter than the
diff --git a/frc971/control_loops/drivetrain/localization/BUILD b/frc971/control_loops/drivetrain/localization/BUILD
index 06c8d57..7b86f09 100644
--- a/frc971/control_loops/drivetrain/localization/BUILD
+++ b/frc971/control_loops/drivetrain/localization/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 cc_library(
@@ -49,12 +49,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "localizer_output_fbs",
     srcs = [
         "localizer_output.fbs",
     ],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/frc971/control_loops/drivetrain/swerve/BUILD b/frc971/control_loops/drivetrain/swerve/BUILD
index 06ba36a..5b94fa8 100644
--- a/frc971/control_loops/drivetrain/swerve/BUILD
+++ b/frc971/control_loops/drivetrain/swerve/BUILD
@@ -1,15 +1,13 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "swerve_drivetrain_output_fbs",
     srcs = ["swerve_drivetrain_output.fbs"],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "swerve_drivetrain_position_fbs",
     srcs = ["swerve_drivetrain_position.fbs"],
-    gen_reflections = 1,
 )
diff --git a/frc971/control_loops/flywheel/BUILD b/frc971/control_loops/flywheel/BUILD
new file mode 100644
index 0000000..f077682
--- /dev/null
+++ b/frc971/control_loops/flywheel/BUILD
@@ -0,0 +1,111 @@
+load("//aos:config.bzl", "aos_config")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+static_flatbuffer(
+    name = "flywheel_controller_status_fbs",
+    srcs = [
+        "flywheel_controller_status.fbs",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+    name = "flywheel_controller_status_ts_fbs",
+    srcs = [
+        "flywheel_controller_status.fbs",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+cc_library(
+    name = "flywheel_test_plant",
+    hdrs = [
+        "flywheel_test_plant.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":flywheel_controller",
+    ],
+)
+
+cc_library(
+    name = "flywheel_controller",
+    srcs = [
+        "flywheel_controller.cc",
+    ],
+    hdrs = [
+        "flywheel_controller.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":flywheel_controller_status_fbs",
+        "//frc971/control_loops:control_loop",
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:profiled_subsystem",
+    ],
+)
+
+cc_test(
+    name = "flywheel_controller_test",
+    srcs = ["flywheel_controller_test.cc"],
+    data = [
+        ":flywheel_controller_test_config",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":flywheel_controller",
+        ":flywheel_controller_test_plants",
+        ":flywheel_test_plant",
+        "//aos/testing:googletest",
+        "//frc971/control_loops:control_loop_test",
+    ],
+)
+
+aos_config(
+    name = "flywheel_controller_test_config",
+    src = "flywheel_controller_test_config_source.json",
+    flatbuffers = [
+        "//frc971/input:joystick_state_fbs",
+        "//frc971/input:robot_state_fbs",
+        "//aos/logging:log_message_fbs",
+        "//aos/events:event_loop_fbs",
+        ":flywheel_controller_status_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+)
+
+genrule(
+    name = "genrule_flywheel_controller_test",
+    outs = [
+        "flywheel_controller_test_plant.h",
+        "flywheel_controller_test_plant.cc",
+        "integral_flywheel_controller_test_plant.h",
+        "integral_flywheel_controller_test_plant.cc",
+    ],
+    cmd = "$(location //frc971/control_loops/python:flywheel_controller_test) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//frc971/control_loops/python:flywheel_controller_test",
+    ],
+)
+
+cc_library(
+    name = "flywheel_controller_test_plants",
+    srcs = [
+        "flywheel_controller_test_plant.cc",
+        "integral_flywheel_controller_test_plant.cc",
+    ],
+    hdrs = [
+        "flywheel_controller_test_plant.h",
+        "integral_flywheel_controller_test_plant.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
diff --git a/frc971/control_loops/flywheel/flywheel_controller.cc b/frc971/control_loops/flywheel/flywheel_controller.cc
new file mode 100644
index 0000000..dc82147
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller.cc
@@ -0,0 +1,178 @@
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
+
+#include <chrono>
+
+#include "aos/logging/logging.h"
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+// Class to current limit battery current for a flywheel controller.
+class CurrentLimitedStateFeedbackController
+    : public StateFeedbackLoop<3, 1, 1, double,
+                               StateFeedbackHybridPlant<3, 1, 1>,
+                               HybridKalman<3, 1, 1>> {
+ public:
+  // Builds a CurrentLimitedStateFeedbackController given the coefficients, bemf
+  // coefficient (units of radians/sec / volt), and motor resistance in ohms.
+  CurrentLimitedStateFeedbackController(
+      StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                        HybridKalman<3, 1, 1>> &&other,
+      double bemf, double resistance)
+      : StateFeedbackLoop(std::move(other)),
+        bemf_(bemf),
+        resistance_(resistance) {}
+
+  double resistance() const { return resistance_; }
+  double bemf() const { return bemf_; }
+
+  void CapU() override {
+    const double bemf_voltage = X_hat(1) / bemf_;
+    // Solve the system of equations:
+    //
+    //   motor_current = (u - bemf_voltage) / resistance
+    //   battery_current = ((u - bemf_voltage) / resistance) * u / 12.0
+    //   0.0 = u * u - u * bemf_voltage - max_current * 12.0 * resistance
+    //
+    // And we have a quadratic!
+    const double a = 1;
+    const double b = -bemf_voltage;
+    const double c_positive = -70.0 * 12.0 * resistance_;
+    const double c_negative = -25.0 * 12.0 * resistance_;
+
+    // Root is always positive.
+    const double root_positive = std::sqrt(b * b - 4.0 * a * c_positive);
+    const double root_negative = std::sqrt(b * b - 4.0 * a * c_negative);
+    const double upper_limit = (-b + root_positive) / (2.0 * a);
+    const double lower_limit = (-b - root_negative) / (2.0 * a);
+
+    // Limit to the battery voltage and the current limit voltage.
+    mutable_U(0, 0) = std::clamp(U(0, 0), lower_limit, upper_limit);
+
+    double lower_clamp = (std::abs(R(1)) > 50.0) ? 1.0 : 0.0;
+    double upper_clamp = 12.0;
+
+    if (R(1) < 0.0) {
+      // If R(1) is negative, swap and invert.
+      std::swap(lower_clamp, upper_clamp);
+      lower_clamp *= -1.0;
+      upper_clamp *= -1.0;
+    }
+
+    mutable_U(0, 0) = std::clamp(U(0, 0), lower_clamp, upper_clamp);
+  }
+
+ private:
+  double bemf_ = 0.0;
+  double resistance_ = 0.0;
+};
+
+FlywheelController::FlywheelController(
+    StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                      HybridKalman<3, 1, 1>> &&loop,
+    double bemf, double resistance)
+    : loop_(new CurrentLimitedStateFeedbackController(std::move(loop), bemf,
+                                                      resistance)) {
+  history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
+      0, ::aos::monotonic_clock::epoch()));
+  Y_.setZero();
+}
+
+void FlywheelController::set_goal(double angular_velocity_goal) {
+  loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
+  last_goal_ = angular_velocity_goal;
+}
+
+void FlywheelController::set_position(
+    double current_position,
+    const aos::monotonic_clock::time_point position_timestamp) {
+  // Project time forwards.
+  const int newest_history_position =
+      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+  if (!first_) {
+    loop_->UpdateObserver(
+        loop_->U(),
+        position_timestamp - std::get<1>(history_[newest_history_position]));
+  } else {
+    first_ = false;
+  }
+
+  // Update position in the model.
+  Y_ << current_position;
+
+  // Add the position to the history.
+  history_[history_position_] =
+      std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
+                                                            position_timestamp);
+  history_position_ = (history_position_ + 1) % kHistoryLength;
+
+  loop_->Correct(Y_);
+}
+
+double FlywheelController::voltage() const { return loop_->U(0, 0); }
+
+double FlywheelController::current() const {
+  return ((voltage() - (velocity() / loop_->bemf())) / (loop_->resistance())) *
+         voltage() / 12.0;
+}
+
+void FlywheelController::Update(bool disabled) {
+  loop_->mutable_R() = loop_->next_R();
+
+  if (std::abs(loop_->R(1, 0)) < 1.0) {
+    // Kill power at low angular velocities.
+    disabled = true;
+  }
+
+  loop_->UpdateController(disabled);
+}
+
+flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
+    flatbuffers::FlatBufferBuilder *fbb) {
+  // Compute the oldest point in the history.
+  const int oldest_history_position = history_position_;
+  const int newest_history_position =
+      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+  const int second_newest_history_position =
+      ((newest_history_position == 0) ? kHistoryLength
+                                      : newest_history_position) -
+      1;
+
+  const double total_loop_time = ::aos::time::DurationInSeconds(
+      std::get<1>(history_[newest_history_position]) -
+      std::get<1>(history_[oldest_history_position]));
+
+  const double distance_traveled =
+      std::get<0>(history_[newest_history_position]) -
+      std::get<0>(history_[oldest_history_position]);
+
+  const double last_loop_time = ::aos::time::DurationInSeconds(
+      std::get<1>(history_[newest_history_position]) -
+      std::get<1>(history_[second_newest_history_position]));
+
+  const double last_distance_traveled =
+      std::get<0>(history_[newest_history_position]) -
+      std::get<0>(history_[second_newest_history_position]);
+
+  // Compute the distance moved over that time period.
+  avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
+
+  FlywheelControllerStatusBuilder builder(*fbb);
+
+  builder.add_avg_angular_velocity(avg_angular_velocity_);
+  builder.add_dt_angular_velocity(last_distance_traveled / last_loop_time);
+  builder.add_angular_velocity(loop_->X_hat(1, 0));
+  builder.add_voltage_error(loop_->X_hat(2, 0));
+  builder.add_commanded_current(current());
+  builder.add_angular_velocity_goal(last_goal_);
+  return builder.Finish();
+}
+
+FlywheelController::~FlywheelController() {}
+
+double FlywheelController::velocity() const { return loop_->X_hat(1, 0); }
+
+}  // namespace flywheel
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/flywheel/flywheel_controller.h b/frc971/control_loops/flywheel/flywheel_controller.h
new file mode 100644
index 0000000..db8baeb
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller.h
@@ -0,0 +1,80 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+
+#include <memory>
+
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/flywheel/flywheel_controller_status_generated.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+class CurrentLimitedStateFeedbackController;
+
+// Handles the velocity control of each flywheel.
+class FlywheelController {
+ public:
+  FlywheelController(
+      StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                        HybridKalman<3, 1, 1>> &&loop,
+      double bemf, double resistance);
+
+  ~FlywheelController();
+
+  // Sets the velocity goal in radians/sec
+  void set_goal(double angular_velocity_goal);
+  double goal() const { return last_goal_; }
+  // Sets the current encoder position in radians
+  void set_position(double current_position,
+                    const aos::monotonic_clock::time_point position_timestamp);
+
+  // Populates the status structure.
+  flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
+      flatbuffers::FlatBufferBuilder *fbb);
+
+  // Returns the control loop calculated voltage.
+  double voltage() const;
+
+  // Returns the expected battery current for the last U.
+  double current() const;
+
+  // Returns the instantaneous velocity.
+  double velocity() const;
+
+  // Executes the control loop for a cycle.
+  void Update(bool disabled);
+
+  double avg_angular_velocity() { return avg_angular_velocity_; }
+
+ private:
+  // The current sensor measurement.
+  Eigen::Matrix<double, 1, 1> Y_;
+  // The control loop.
+  ::std::unique_ptr<CurrentLimitedStateFeedbackController> loop_;
+
+  // History array for calculating a filtered angular velocity.
+  static constexpr int kHistoryLength = 10;
+  ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
+               kHistoryLength>
+      history_;
+  ptrdiff_t history_position_ = 0;
+
+  // Average velocity logging.
+  double avg_angular_velocity_ = 0;
+
+  double last_goal_ = 0;
+
+  bool first_ = true;
+
+  DISALLOW_COPY_AND_ASSIGN(FlywheelController);
+};
+
+}  // namespace flywheel
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
diff --git a/frc971/control_loops/flywheel/flywheel_controller_status.fbs b/frc971/control_loops/flywheel/flywheel_controller_status.fbs
new file mode 100644
index 0000000..5e079e3
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller_status.fbs
@@ -0,0 +1,24 @@
+namespace frc971.control_loops.flywheel;
+
+table FlywheelControllerStatus {
+  // The current average velocity in radians/second
+  avg_angular_velocity:double (id: 0);
+
+  // The current instantaneous filtered velocity in radians/second.
+  angular_velocity:double (id: 1);
+
+  // The target speed selected by the lookup table or from manual override
+  // Can be compared to velocity to determine if ready.
+  angular_velocity_goal:double (id: 2);
+
+  // Voltage error.
+  voltage_error:double (id: 3);
+
+  // The commanded battery current.
+  commanded_current:double (id: 4);
+
+  // The angular velocity of the flywheel computed using delta x / delta t
+  dt_angular_velocity:double (id: 5);
+}
+
+root_type FlywheelControllerStatus;
diff --git a/frc971/control_loops/flywheel/flywheel_controller_test.cc b/frc971/control_loops/flywheel/flywheel_controller_test.cc
new file mode 100644
index 0000000..26aabc9
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller_test.cc
@@ -0,0 +1,103 @@
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+#include "aos/configuration.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/flywheel/flywheel_controller_test_plant.h"
+#include "frc971/control_loops/flywheel/flywheel_test_plant.h"
+#include "frc971/control_loops/flywheel/integral_flywheel_controller_test_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+namespace testing {
+class FlywheelTest : public ::frc971::testing::ControlLoopTest {
+ public:
+  FlywheelTest()
+      : ::frc971::testing::ControlLoopTest(
+            aos::configuration::ReadConfig(
+                "frc971/control_loops/flywheel/"
+                "flywheel_controller_test_config.json"),
+            std::chrono::microseconds(5050)),
+        test_event_loop_(MakeEventLoop("test")),
+        flywheel_plant_(
+            new FlywheelPlant(MakeFlywheelTestPlant(), kBemf, kResistance)),
+        flywheel_controller_(MakeIntegralFlywheelTestLoop(), kBemf,
+                             kResistance),
+        flywheel_controller_sender_(
+            test_event_loop_->MakeSender<FlywheelControllerStatus>("/loop")) {
+    phased_loop_handle_ =
+        test_event_loop_->AddPhasedLoop([this](int) { Simulate(); }, dt());
+  }
+
+  void Simulate() {
+    const aos::monotonic_clock::time_point timestamp =
+        test_event_loop_->context().monotonic_event_time;
+    ::Eigen::Matrix<double, 1, 1> flywheel_U;
+    flywheel_U << flywheel_voltage_ + flywheel_plant_->voltage_offset();
+
+    // Confirm that we aren't drawing too much current.  2 motors -> twice the
+    // lumped current since our model can't tell them apart.
+    CHECK_NEAR(flywheel_plant_->battery_current(flywheel_U), 0.0, 200.0);
+
+    flywheel_plant_->Update(flywheel_U);
+
+    flywheel_controller_.set_position(flywheel_plant_->Y(0, 0), timestamp);
+
+    flywheel_controller_.set_goal(goal_);
+
+    flywheel_controller_.Update(false);
+    aos::FlatbufferFixedAllocatorArray<FlywheelControllerStatus, 512>
+        flywheel_status_buffer;
+
+    flywheel_status_buffer.Finish(
+        flywheel_controller_.SetStatus(flywheel_status_buffer.fbb()));
+
+    flywheel_voltage_ = flywheel_controller_.voltage();
+
+    last_angular_velocity_ =
+        flywheel_status_buffer.message().angular_velocity();
+  }
+
+  void VerifyNearGoal() { EXPECT_NEAR(last_angular_velocity_, goal_, 0.1); }
+
+  void set_goal(double goal) { goal_ = goal; }
+
+ private:
+  ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+  ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+  std::unique_ptr<FlywheelPlant> flywheel_plant_;
+  FlywheelController flywheel_controller_;
+
+  aos::Sender<FlywheelControllerStatus> flywheel_controller_sender_;
+
+  double last_angular_velocity_ = 0.0;
+
+  double flywheel_voltage_ = 0.0;
+  double goal_ = 0.0;
+};
+
+TEST_F(FlywheelTest, DoNothing) {
+  set_goal(0);
+  RunFor(std::chrono::seconds(2));
+  VerifyNearGoal();
+}
+
+TEST_F(FlywheelTest, PositiveTest) {
+  set_goal(700.0);
+  RunFor(std::chrono::seconds(4));
+  VerifyNearGoal();
+}
+
+TEST_F(FlywheelTest, NegativeTest) {
+  set_goal(-700.0);
+  RunFor(std::chrono::seconds(8));
+  VerifyNearGoal();
+}
+}  // namespace testing
+}  // namespace flywheel
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/flywheel/flywheel_controller_test_config_source.json b/frc971/control_loops/flywheel/flywheel_controller_test_config_source.json
new file mode 100644
index 0000000..1a9b727
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller_test_config_source.json
@@ -0,0 +1,27 @@
+{
+  "channels": [
+    {
+      "name": "/aos",
+      "type": "aos.JoystickState"
+    },
+    {
+      "name": "/aos",
+      "type": "aos.logging.LogMessageFbs",
+      "frequency": 400
+    },
+    {
+      "name": "/aos",
+      "type": "aos.RobotState",
+      "frequency": 250
+    },
+    {
+      "name": "/aos",
+      "type": "aos.timing.Report"
+    },
+    {
+      "name": "/loop",
+      "type": "frc971.control_loops.flywheel.FlywheelControllerStatus",
+      "frequency": 200
+    }
+  ]
+}
diff --git a/frc971/control_loops/flywheel/flywheel_test_plant.h b/frc971/control_loops/flywheel/flywheel_test_plant.h
new file mode 100644
index 0000000..01e347d
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_test_plant.h
@@ -0,0 +1,47 @@
+#ifndef FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
+#define FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
+
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+  explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other, double bemf,
+                         double resistance)
+      : StateFeedbackPlant<2, 1, 1>(::std::move(other)),
+        bemf_(bemf),
+        resistance_(resistance) {}
+
+  void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
+    EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+    EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+  }
+
+  double motor_current(const Eigen::Matrix<double, 1, 1> U) const {
+    return (U(0) - X(1) / bemf_) / resistance_;
+  }
+
+  double battery_current(const Eigen::Matrix<double, 1, 1> U) const {
+    return motor_current(U) * U(0) / 12.0;
+  }
+
+  double voltage_offset() const { return voltage_offset_; }
+  void set_voltage_offset(double voltage_offset) {
+    voltage_offset_ = voltage_offset;
+  }
+
+ private:
+  double voltage_offset_ = 0.0;
+
+  double bemf_;
+  double resistance_;
+};
+
+}  // namespace flywheel
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 0066b93..7f458b5 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -202,7 +202,7 @@
     data = glob([
         "field_images/*.png",
         "field_images/*.svg",
-    ]) + ["//third_party/y2023/field:pictures"],
+    ]) + ["//third_party/y2023/field:pictures"] + ["//third_party/y2024/field:pictures"],
     legacy_create_init = False,
     target_compatible_with = ["@platforms//cpu:x86_64"],
     visibility = ["//visibility:public"],
@@ -247,6 +247,21 @@
 )
 
 py_binary(
+    name = "flywheel",
+    srcs = [
+        "flywheel.py",
+    ],
+    legacy_create_init = False,
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:controls",
+        "@pip//matplotlib",
+        "@pip//pygobject",
+    ],
+)
+
+py_binary(
     name = "static_zeroing_single_dof_profiled_subsystem_test",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test.py",
@@ -261,3 +276,19 @@
         "@pip//python_gflags",
     ],
 )
+
+py_binary(
+    name = "flywheel_controller_test",
+    srcs = [
+        "flywheel_controller_test.py",
+    ],
+    legacy_create_init = False,
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":controls",
+        ":flywheel",
+        ":python_init",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index 7bc45db..e4ff6b7 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -37,6 +37,8 @@
 Robot2021 = Robot2020
 Robot2022 = RobotType(width=0.8763, length=0.96647)
 Robot2023 = RobotType(width=0.6061, length=0.77581)
+#TODO (Nathan): Update 2024 robot dimensions when CAD is done
+Robot2024 = RobotType(width=0.9017, length=0.9525)  # 35.5 in x 37.5 in
 
 FIELDS = {
     "2019 Field":
@@ -127,9 +129,17 @@
               length=8.10895,
               robot=Robot2023,
               field_id="//third_party/y2023/field/2023.png"),
+    "2024 Field":
+    FieldType("2024 Field",
+              tags=[],
+              year=2024,
+              width=16.54175,
+              length=8.21055,
+              robot=Robot2024,
+              field_id="//third_party/y2024/field/2024.png"),
 }
 
-FIELD = FIELDS["2023 Field"]
+FIELD = FIELDS["2024 Field"]
 
 
 def get_json_folder(field):
@@ -139,6 +149,7 @@
         return "y2022/actors/splines"
     elif field.year == 2023:
         return "y2023/autonomous/splines"
+    #TODO: Update 2024 spline jsons
     else:
         return "frc971/control_loops/python/spline_jsons"
 
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 7e54bdf..e179193 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -743,3 +743,33 @@
         # Diameter of 1.9", weight of: 100 grams
         # TODO(austin): Get a number from Scott Westbrook for the mass
         self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)
+
+
+class KrakenFOC(object):
+    """Class representing the WCP Kraken X60 motor using
+    Field Oriented Controls (FOC) communication.
+
+    All numbers based on data from
+    https://wcproducts.com/products/kraken.
+    """
+
+    def __init__(self):
+        # Stall Torque in N m
+        self.stall_torque = 9.37
+        # Stall Current in Amps
+        self.stall_current = 483.0
+        # Free Speed in rad / sec
+        self.free_speed = 5800.0 / 60.0 * 2.0 * numpy.pi
+        # Free Current in Amps
+        self.free_current = 2.0
+        # Resistance of the motor, divided by 2 to account for the 2 motors
+        self.resistance = 12.0 / self.stall_current
+        # Motor velocity constant
+        self.Kv = (self.free_speed /
+                   (12.0 - self.resistance * self.free_current))
+        # Torque constant
+        self.Kt = self.stall_torque / self.stall_current
+        # Motor inertia in kg m^2
+        # Diameter of 1.9", weight of: 100 grams
+        # TODO(Filip): Update motor inertia for Kraken, currently using Falcon motor inertia
+        self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 2fed5f0..5ff4404 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -566,6 +566,8 @@
     # Distance in meters to call 1/2 field.
     kSprintDistance = 8.0
 
+    kMaxBreakerCurrent = 220
+
     vbat = 12.6
     # Measured resistance of the battery, pd board, and breakers.
     Rw = 0.023
@@ -607,20 +609,50 @@
 
         # Max current we could push through the motors is what we would get if
         # we short the battery through the battery resistance into the motor.
-        max_motor_current = (vbat - (bemf_left + bemf_right) / 2.0) / (
-            Rw + drivetrain.resistance / 2.0)
+        bemf = (bemf_left + bemf_right) / 2.0
+        max_motor_current = (vbat - bemf) / (Rw + drivetrain.resistance / 2.0)
 
         max_motor_currents.append(max_motor_current /
                                   (drivetrain_params.num_motors * 2))
 
         # From this current, we can compute the voltage we can apply.
         # This is either the traction limit or the current limit.
-        max_voltage_left = bemf_left + min(
-            max_motor_current / 2,
-            left_traction_current) * drivetrain.resistance
-        max_voltage_right = bemf_right + min(
-            max_motor_current / 2,
-            right_traction_current) * drivetrain.resistance
+        max_current_request_left = min(max_motor_current / 2,
+                                       left_traction_current)
+        max_current_request_right = min(max_motor_current / 2,
+                                        right_traction_current)
+        max_voltage_left = (bemf_left +
+                            max_current_request_left * drivetrain.resistance)
+        max_voltage_right = (bemf_right +
+                             max_current_request_right * drivetrain.resistance)
+
+        # Now, make sure we don't pull more power out of the battery than the
+        # breakers will let us pull.  Do this by comparing the max power we can
+        # pull out of the battery with the requested power.
+        #
+        # TODO(austin): This all assumes the robot is symetric...
+        max_battery_wattage = kMaxBreakerCurrent * (vbat -
+                                                    kMaxBreakerCurrent * Rw)
+        if (max_current_request_left * max_voltage_left +
+                max_current_request_right * max_voltage_right >
+                max_battery_wattage):
+            # Now solve the quadratic equation to figure out what the overall
+            # motor current can be which puts us at the max battery wattage.
+            max_motor_current = (
+                -bemf + math.sqrt(bemf * bemf + 4 * drivetrain.resistance /
+                                  2.0 * max_battery_wattage)) / (
+                                      2.0 * drivetrain.resistance / 2.0)
+            # Clip each side's currents to 1/2 of the max motor current since
+            # we know we are limited.
+            max_current_request_left = min(max_motor_current / 2.0,
+                                           max_current_request_left)
+            max_current_request_right = min(max_motor_current / 2.0,
+                                            max_current_request_right)
+            # And then update the voltages.
+            max_voltage_left = (
+                bemf_left + max_current_request_left * drivetrain.resistance)
+            max_voltage_right = (
+                bemf_right + max_current_request_right * drivetrain.resistance)
 
         simulated_left_position.append(drivetrain.X[0, 0])
         simulated_left_velocity.append(drivetrain.X[1, 0])
diff --git a/frc971/control_loops/python/flywheel.py b/frc971/control_loops/python/flywheel.py
new file mode 100644
index 0000000..1280b36
--- /dev/null
+++ b/frc971/control_loops/python/flywheel.py
@@ -0,0 +1,329 @@
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+
+import matplotlib
+import matplotlib.pyplot as plt
+
+import glog
+
+
+class FlywheelParams(object):
+
+    def __init__(self,
+                 name,
+                 motor,
+                 G,
+                 J,
+                 q_pos,
+                 q_vel,
+                 q_voltage,
+                 r_pos,
+                 controller_poles,
+                 dt=0.00505):
+        self.name = name
+        self.motor = motor
+        self.G = G
+        self.J = J
+        self.q_pos = q_pos
+        self.q_vel = q_vel
+        self.q_voltage = q_voltage
+        self.r_pos = r_pos
+        self.dt = dt
+        self.controller_poles = controller_poles
+
+
+class VelocityFlywheel(control_loop.HybridControlLoop):
+
+    def __init__(self, params, name="Flywheel"):
+        super(VelocityFlywheel, self).__init__(name=name)
+        self.params = params
+        # Set Motor
+        self.motor = self.params.motor
+        # Moment of inertia of the flywheel wheel in kg m^2
+        self.J = self.params.J
+        # Gear ratio
+        self.G = self.params.G
+        # Control loop time step
+        self.dt = self.params.dt
+
+        # State feedback matrices
+        # [angular velocity]
+        self.A_continuous = numpy.matrix([[
+            -self.motor.Kt / self.motor.Kv /
+            (self.J * self.G * self.G * self.motor.resistance)
+        ]])
+        self.B_continuous = numpy.matrix(
+            [[self.motor.Kt / (self.J * self.G * self.motor.resistance)]])
+        self.C = numpy.matrix([[1]])
+        self.D = numpy.matrix([[0]])
+
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
+
+        self.PlaceControllerPoles(self.params.controller_poles)
+
+        # Generated controller not used.
+        self.PlaceObserverPoles([0.3])
+
+        self.U_max = numpy.matrix([[12.0]])
+        self.U_min = numpy.matrix([[-12.0]])
+
+        qff_vel = 8.0
+        self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+        self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+        glog.debug('K: %s', str(self.K))
+        glog.debug('Poles: %s',
+                   str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+
+class Flywheel(VelocityFlywheel):
+
+    def __init__(self, params, name="Flywheel"):
+        super(Flywheel, self).__init__(params, name=name)
+
+        self.A_continuous_unaugmented = self.A_continuous
+        self.B_continuous_unaugmented = self.B_continuous
+
+        self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+        self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+        self.A_continuous[0, 1] = 1
+
+        self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+        self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+        # State feedback matrices
+        # [position, angular velocity]
+        self.C = numpy.matrix([[1, 0]])
+        self.D = numpy.matrix([[0]])
+
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
+
+        rpl = 0.45
+        ipl = 0.07
+        self.PlaceObserverPoles([rpl + 1j * ipl, rpl - 1j * ipl])
+
+        self.K_unaugmented = self.K
+        self.K = numpy.matrix(numpy.zeros((1, 2)))
+        self.K[0, 1:2] = self.K_unaugmented
+        self.Kff_unaugmented = self.Kff
+        self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+        self.Kff[0, 1:2] = self.Kff_unaugmented
+
+        self.InitializeState()
+
+
+class IntegralFlywheel(Flywheel):
+
+    def __init__(self, params, name="IntegralFlywheel"):
+        super(IntegralFlywheel, self).__init__(params, name=name)
+
+        self.A_continuous_unaugmented = self.A_continuous
+        self.B_continuous_unaugmented = self.B_continuous
+
+        self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+        self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+        self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+        self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+        self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+        # states
+        # [position, velocity, voltage_error]
+        self.C_unaugmented = self.C
+        self.C = numpy.matrix(numpy.zeros((1, 3)))
+        self.C[0:1, 0:2] = self.C_unaugmented
+
+        glog.debug('A_continuous %s' % str(self.A_continuous))
+        glog.debug('B_continuous %s' % str(self.B_continuous))
+        glog.debug('C %s' % str(self.C))
+
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
+
+        glog.debug('A %s' % str(self.A))
+        glog.debug('B %s' % str(self.B))
+
+        q_pos = self.params.q_pos
+        q_vel = self.params.q_vel
+        q_voltage = self.params.q_voltage
+        self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+                                          [0.0, (q_vel**2.0), 0.0],
+                                          [0.0, 0.0, (q_voltage**2.0)]])
+
+        r_pos = self.params.r_pos
+        self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
+
+        _, _, self.Q, self.R = controls.kalmd(A_continuous=self.A_continuous,
+                                              B_continuous=self.B_continuous,
+                                              Q_continuous=self.Q_continuous,
+                                              R_continuous=self.R_continuous,
+                                              dt=self.dt)
+
+        glog.debug('Q_discrete %s' % (str(self.Q)))
+        glog.debug('R_discrete %s' % (str(self.R)))
+
+        self.KalmanGain, self.P_steady_state = controls.kalman(A=self.A,
+                                                               B=self.B,
+                                                               C=self.C,
+                                                               Q=self.Q,
+                                                               R=self.R)
+        self.L = self.A * self.KalmanGain
+
+        self.K_unaugmented = self.K
+        self.K = numpy.matrix(numpy.zeros((1, 3)))
+        self.K[0, 0:2] = self.K_unaugmented
+        self.K[0, 2] = 1
+        self.Kff_unaugmented = self.Kff
+        self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+        self.Kff[0, 0:2] = self.Kff_unaugmented
+
+        self.InitializeState()
+
+
+def PlotSpinup(params, goal, iterations=400):
+    """Runs the flywheel plant with an initial condition and goal.
+
+    Args:
+        flywheel: Flywheel object to use.
+        goal: goal state.
+        iterations: Number of timesteps to run the model for.
+        controller_flywheel: Flywheel object to get K from, or None if we should
+             use flywheel.
+        observer_flywheel: Flywheel object to use for the observer, or None if we
+            should use the actual state.
+    """
+
+    # Various lists for graphing things.
+    t = []
+    x = []
+    v = []
+    a = []
+    x_hat = []
+    u = []
+    offset = []
+
+    flywheel = Flywheel(params, params.name)
+    controller_flywheel = IntegralFlywheel(params, params.name)
+    observer_flywheel = IntegralFlywheel(params, params.name)
+    vbat = 12.0
+
+    if t:
+        initial_t = t[-1] + flywheel.dt
+    else:
+        initial_t = 0
+
+    for i in range(iterations):
+        X_hat = flywheel.X
+
+        if observer_flywheel is not None:
+            X_hat = observer_flywheel.X_hat
+            x_hat.append(observer_flywheel.X_hat[1, 0])
+
+        ff_U = controller_flywheel.Kff * (goal - observer_flywheel.A * goal)
+
+        U = controller_flywheel.K * (goal - X_hat) + ff_U
+        U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+        x.append(flywheel.X[0, 0])
+
+        if v:
+            last_v = v[-1]
+        else:
+            last_v = 0
+
+        v.append(flywheel.X[1, 0])
+        a.append((v[-1] - last_v) / flywheel.dt)
+
+        if observer_flywheel is not None:
+            observer_flywheel.Y = flywheel.Y
+            observer_flywheel.CorrectHybridObserver(U)
+            offset.append(observer_flywheel.X_hat[2, 0])
+
+        applied_U = U.copy()
+        if i > 200:
+            applied_U += 2
+        flywheel.Update(applied_U)
+
+        if observer_flywheel is not None:
+            observer_flywheel.PredictHybridObserver(U, flywheel.dt)
+
+        t.append(initial_t + i * flywheel.dt)
+        u.append(U[0, 0])
+
+    matplotlib.use("GTK3Agg")
+
+    plt.subplot(3, 1, 1)
+    plt.plot(t, v, label='x')
+    plt.plot(t, x_hat, label='x_hat')
+    plt.legend()
+
+    plt.subplot(3, 1, 2)
+    plt.plot(t, u, label='u')
+    plt.plot(t, offset, label='voltage_offset')
+    plt.legend()
+
+    plt.subplot(3, 1, 3)
+    plt.plot(t, a, label='a')
+    plt.legend()
+
+    plt.show()
+
+
+def WriteFlywheel(params, plant_files, controller_files, namespace):
+    """Writes out the constants for a flywheel to a file.
+
+    Args:
+      params: list of Flywheel Params, the
+        parameters defining the system.
+      plant_files: list of strings, the cc and h files for the plant.
+      controller_files: list of strings, the cc and h files for the integral
+        controller.
+      namespaces: list of strings, the namespace list to use.
+    """
+    # Write the generated constants out to a file.
+    flywheels = []
+    integral_flywheels = []
+
+    if type(params) is list:
+        name = params[0].name
+        for index, param in enumerate(params):
+            flywheels.append(Flywheel(param, name=param.name + str(index)))
+            integral_flywheels.append(
+                IntegralFlywheel(param,
+                                 name='Integral' + param.name + str(index)))
+    else:
+        name = params.name
+        flywheels.append(Flywheel(params, params.name))
+        integral_flywheels.append(
+            IntegralFlywheel(params, name='Integral' + params.name))
+
+    loop_writer = control_loop.ControlLoopWriter(name,
+                                                 flywheels,
+                                                 namespaces=namespace)
+    loop_writer.AddConstant(
+        control_loop.Constant('kOutputRatio', '%f', flywheels[0].G))
+    loop_writer.AddConstant(
+        control_loop.Constant('kFreeSpeed', '%f',
+                              flywheels[0].motor.free_speed))
+    loop_writer.AddConstant(
+        control_loop.Constant('kBemf',
+                              '%f',
+                              flywheels[0].motor.Kv * flywheels[0].G,
+                              comment="// Radians/sec / volt"))
+    loop_writer.AddConstant(
+        control_loop.Constant('kResistance',
+                              '%f',
+                              flywheels[0].motor.resistance,
+                              comment="// Ohms"))
+    loop_writer.Write(plant_files[0], plant_files[1])
+
+    integral_loop_writer = control_loop.ControlLoopWriter(
+        'Integral' + name,
+        integral_flywheels,
+        namespaces=namespace,
+        plant_type='StateFeedbackHybridPlant',
+        observer_type='HybridKalman')
+    integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/frc971/control_loops/python/flywheel_controller_test.py b/frc971/control_loops/python/flywheel_controller_test.py
new file mode 100644
index 0000000..4f4d929
--- /dev/null
+++ b/frc971/control_loops/python/flywheel_controller_test.py
@@ -0,0 +1,48 @@
+#!/usr/bin/python3
+
+# Generates a test flywheel for flywheel_controller_test
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import flywheel
+
+import numpy
+import sys
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+    pass
+
+kFlywheel = flywheel.FlywheelParams(name='FlywheelTest',
+                                    motor=control_loop.Falcon(),
+                                    G=(60.0 / 48.0),
+                                    J=0.0035,
+                                    q_pos=0.01,
+                                    q_vel=10.0,
+                                    q_voltage=4.0,
+                                    r_pos=0.01,
+                                    controller_poles=[.95])
+
+
+def main(argv):
+    if FLAGS.plot:
+        R = numpy.matrix([[0.0], [500.0], [0.0]])
+        flywheel.PlotSpinup(params=kFlywheel, goal=R, iterations=400)
+        return 0
+
+    # Write the generated constants out to a file.
+    if len(argv) != 5:
+        glog.fatal('Expected .h file name and .cc file name')
+    else:
+        namespaces = ['frc971', 'control_loops', 'flywheel']
+        flywheel.WriteFlywheel(kFlywheel, argv[1:3], argv[3:5], namespaces)
+
+
+if __name__ == '__main__':
+    argv = FLAGS(sys.argv)
+    glog.init()
+    sys.exit(main(argv))
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 65135d2..5c94bd5 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -6,12 +6,6 @@
 namespace frc971 {
 namespace control_loops {
 
-// TODO(austin): Use ProfileParametersT...
-struct ProfileParametersStruct {
-  float max_velocity;
-  float max_acceleration;
-};
-
 template <typename ZeroingEstimator>
 struct StaticZeroingSingleDOFProfiledSubsystemParams {
   // Maximum voltage while the subsystem is zeroing
@@ -21,16 +15,16 @@
   double operating_voltage;
 
   // Maximum velocity (units/s) and acceleration while State::ZEROING
-  ProfileParametersStruct zeroing_profile_params;
+  ProfileParametersT zeroing_profile_params;
 
   // Maximum velocity (units/s) and acceleration while State::RUNNING if max
   // velocity or acceleration in goal profile_params is 0
-  ProfileParametersStruct default_profile_params;
+  ProfileParametersT default_profile_params;
 
   // Maximum range of the subsystem in meters
   ::frc971::constants::Range range;
 
-  // Zeroing constants for PotAndABsoluteEncoder estimator
+  // Zeroing constants for the estimator
   typename ZeroingEstimator::ZeroingConstants zeroing_constants;
 
   // Function that makes the integral loop for the subsystem
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index ace528e..1fd9049 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -83,25 +83,26 @@
 const frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
     TestIntakeSystemValues<
         frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
-        kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
+        {}, kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
 
 template <>
 const frc971::zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
     TestIntakeSystemValues<
         frc971::zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
-        kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20, 1.9};
+        {}, kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20,
+        1.9};
 
 template <typename ZeroingEstimator>
 const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
 TestIntakeSystemValues<ZeroingEstimator>::make_params() {
-  StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator> params(
-      {kZeroingVoltage,
-       kOperatingVoltage,
-       {0.1, 1.0},
-       {0.3, 5.0},
-       kRange,
-       TestIntakeSystemValues::kZeroing,
-       &MakeIntegralTestIntakeSystemLoop});
+  StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator> params{
+      kZeroingVoltage,
+      kOperatingVoltage,
+      {{}, 0.1, 1.0},
+      {{}, 0.3, 5.0},
+      kRange,
+      TestIntakeSystemValues::kZeroing,
+      &MakeIntegralTestIntakeSystemLoop};
   return params;
 }
 
diff --git a/frc971/imu_reader/BUILD b/frc971/imu_reader/BUILD
index 4825900..cf2df44 100644
--- a/frc971/imu_reader/BUILD
+++ b/frc971/imu_reader/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 cc_library(
@@ -22,7 +22,7 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "imu_failures_fbs",
     srcs = [
         "imu_failures.fbs",
diff --git a/frc971/input/BUILD b/frc971/input/BUILD
index 8515153..329dfce 100644
--- a/frc971/input/BUILD
+++ b/frc971/input/BUILD
@@ -1,5 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 package(default_visibility = ["//visibility:public"])
 
@@ -90,17 +90,15 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "robot_state_fbs",
     srcs = ["robot_state.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "joystick_state_fbs",
     srcs = ["joystick_state.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/frc971/math/BUILD b/frc971/math/BUILD
new file mode 100644
index 0000000..54df5bd
--- /dev/null
+++ b/frc971/math/BUILD
@@ -0,0 +1,29 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
+static_flatbuffer(
+    name = "matrix_fbs",
+    srcs = ["matrix.fbs"],
+    visibility = ["//visibility:public"],
+)
+
+cc_library(
+    name = "flatbuffers_matrix",
+    hdrs = ["flatbuffers_matrix.h"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":matrix_fbs",
+        "//aos:json_to_flatbuffer",
+        "@com_github_google_glog//:glog",
+        "@com_github_tartanllama_expected",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
+
+cc_test(
+    name = "flatbuffers_matrix_test",
+    srcs = ["flatbuffers_matrix_test.cc"],
+    deps = [
+        ":flatbuffers_matrix",
+        "//aos/testing:googletest",
+    ],
+)
diff --git a/frc971/math/flatbuffers_matrix.h b/frc971/math/flatbuffers_matrix.h
new file mode 100644
index 0000000..57013c9
--- /dev/null
+++ b/frc971/math/flatbuffers_matrix.h
@@ -0,0 +1,133 @@
+#ifndef FRC971_MATH_FLATBUFFERS_MATRIX_H_
+#define FRC971_MATH_FLATBUFFERS_MATRIX_H_
+// This library provides utilities for converting between a frc971.fbs.Matrix
+// flatbuffer type and an Eigen::Matrix.
+// The interesting methods are ToEigen(), ToEigenOrDie(), and FromEigen().
+#include "glog/logging.h"
+#include "tl/expected.hpp"
+#include <Eigen/Core>
+
+#include "frc971/math/matrix_static.h"
+
+namespace frc971 {
+inline constexpr Eigen::StorageOptions ToEigenStorageOrder(
+    fbs::StorageOrder storage_order, int Rows, int Cols) {
+  // Eigen only implements one *Major version of the Matrix class for vectors.
+  // See https://eigen.tuxfamily.org/bz/show_bug.cgi?id=416
+  if (Rows == 1) {
+    return Eigen::RowMajor;
+  }
+  if (Cols == 1) {
+    return Eigen::ColMajor;
+  }
+  return storage_order == fbs::StorageOrder::ColMajor ? Eigen::ColMajor
+                                                      : Eigen::RowMajor;
+}
+
+template <int Rows, int Cols, fbs::StorageOrder StorageOrder>
+struct EigenMatrix {
+  typedef Eigen::Matrix<double, Rows, Cols,
+                        ToEigenStorageOrder(StorageOrder, Rows, Cols)>
+      type;
+};
+
+inline std::ostream &operator<<(std::ostream &os, fbs::MatrixField field) {
+  os << fbs::EnumNameMatrixField(field);
+  return os;
+}
+
+inline std::ostream &operator<<(std::ostream &os, fbs::FieldError error) {
+  os << fbs::EnumNameFieldError(error);
+  return os;
+}
+
+struct ConversionFailure {
+  fbs::MatrixField field;
+  fbs::FieldError error;
+  bool operator==(const ConversionFailure &) const = default;
+};
+
+inline std::ostream &operator<<(std::ostream &os, ConversionFailure failure) {
+  os << "(" << failure.field << ", " << failure.error << ")";
+  return os;
+}
+
+template <int Rows, int Cols,
+          fbs::StorageOrder StorageOrder = fbs::StorageOrder::ColMajor>
+tl::expected<typename EigenMatrix<Rows, Cols, StorageOrder>::type,
+             ConversionFailure>
+ToEigen(const fbs::Matrix &matrix) {
+  if (!matrix.has_rows()) {
+    return tl::unexpected(
+        ConversionFailure{fbs::MatrixField::kRows, fbs::FieldError::kMissing});
+  }
+  if (!matrix.has_cols()) {
+    return tl::unexpected(
+        ConversionFailure{fbs::MatrixField::kCols, fbs::FieldError::kMissing});
+  }
+  if (!matrix.has_data()) {
+    return tl::unexpected(
+        ConversionFailure{fbs::MatrixField::kData, fbs::FieldError::kMissing});
+  }
+  if (matrix.rows() != Rows) {
+    return tl::unexpected(ConversionFailure{
+        fbs::MatrixField::kRows, fbs::FieldError::kInconsistentWithTemplate});
+  }
+  if (matrix.cols() != Cols) {
+    return tl::unexpected(ConversionFailure{
+        fbs::MatrixField::kCols, fbs::FieldError::kInconsistentWithTemplate});
+  }
+  if (matrix.storage_order() != StorageOrder) {
+    return tl::unexpected(
+        ConversionFailure{fbs::MatrixField::kStorageOrder,
+                          fbs::FieldError::kInconsistentWithTemplate});
+  }
+  if (matrix.data()->size() != Rows * Cols) {
+    return tl::unexpected(ConversionFailure{
+        fbs::MatrixField::kData, fbs::FieldError::kInconsistentWithTemplate});
+  }
+  return typename EigenMatrix<Rows, Cols, StorageOrder>::type(
+      matrix.data()->data());
+}
+
+template <int Rows, int Cols,
+          fbs::StorageOrder StorageOrder = fbs::StorageOrder::ColMajor>
+typename EigenMatrix<Rows, Cols, StorageOrder>::type ToEigenOrDie(
+    const fbs::Matrix &matrix) {
+  auto result = ToEigen<Rows, Cols, StorageOrder>(matrix);
+  if (!result.has_value()) {
+    LOG(FATAL) << "Failed to convert to matrix with error " << result.error()
+               << ".";
+  }
+  return result.value();
+}
+
+template <int Rows, int Cols,
+          fbs::StorageOrder StorageOrder = fbs::StorageOrder::ColMajor>
+bool FromEigen(
+    const typename EigenMatrix<Rows, Cols, StorageOrder>::type &matrix,
+    fbs::MatrixStatic *flatbuffer) {
+  constexpr size_t kSize = Rows * Cols;
+  auto data = flatbuffer->add_data();
+  if (!data->reserve(kSize)) {
+    return false;
+  }
+  // TODO(james): Use From*() methods once they get upstreamed...
+  data->resize(kSize);
+  std::copy(matrix.data(), matrix.data() + kSize, data->data());
+  flatbuffer->set_rows(Rows);
+  flatbuffer->set_cols(Cols);
+  flatbuffer->set_storage_order(StorageOrder);
+  return true;
+}
+
+template <typename T>
+bool FromEigen(const T &matrix, fbs::MatrixStatic *flatbuffer) {
+  return FromEigen<T::RowsAtCompileTime, T::ColsAtCompileTime,
+                   (T::IsRowMajor ? fbs::StorageOrder::RowMajor
+                                  : fbs::StorageOrder::ColMajor)>(matrix,
+                                                                  flatbuffer);
+}
+
+}  // namespace frc971
+#endif  // FRC971_MATH_FLATBUFFERS_MATRIX_H_
diff --git a/frc971/math/flatbuffers_matrix_test.cc b/frc971/math/flatbuffers_matrix_test.cc
new file mode 100644
index 0000000..2807309
--- /dev/null
+++ b/frc971/math/flatbuffers_matrix_test.cc
@@ -0,0 +1,90 @@
+#include "frc971/math/flatbuffers_matrix.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/json_to_flatbuffer.h"
+
+namespace frc971::testing {
+
+class FlatbuffersMatrixTest : public ::testing::Test {
+ protected:
+  template <int Rows, int Cols,
+            fbs::StorageOrder StorageOrder = fbs::StorageOrder::ColMajor>
+  tl::expected<typename EigenMatrix<Rows, Cols, StorageOrder>::type,
+               ConversionFailure>
+  ToEigen(std::string_view json) {
+    return frc971::ToEigen<Rows, Cols, StorageOrder>(
+        aos::FlatbufferDetachedBuffer<fbs::Matrix>(
+            aos::JsonToFlatbuffer<fbs::Matrix>(json))
+            .message());
+  }
+};
+
+TEST_F(FlatbuffersMatrixTest, ReadWriteMatrix) {
+  const Eigen::Matrix<double, 3, 4> expected{
+      {0, 1, 2, 3}, {4, 5, 6, 7}, {8, 9, 10, 11}};
+  aos::fbs::Builder<fbs::MatrixStatic> builder;
+  ASSERT_TRUE(FromEigen(expected, builder.get()));
+  EXPECT_EQ(
+      "{ \"rows\": 3, \"cols\": 4, \"storage_order\": \"ColMajor\", \"data\": "
+      "[ 0.0, 4.0, 8.0, 1.0, 5.0, 9.0, 2.0, 6.0, 10.0, 3.0, 7.0, 11.0 ] }",
+      aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+
+  const Eigen::Matrix<double, 3, 4> result =
+      ToEigenOrDie<3, 4>(builder->AsFlatbuffer());
+  EXPECT_EQ(expected, result);
+}
+
+TEST_F(FlatbuffersMatrixTest, ReadWriteMatrixRowMajor) {
+  const Eigen::Matrix<double, 3, 4, Eigen::StorageOptions::RowMajor> expected{
+      {0, 1, 2, 3}, {4, 5, 6, 7}, {8, 9, 10, 11}};
+  aos::fbs::Builder<fbs::MatrixStatic> builder;
+  ASSERT_TRUE(FromEigen(expected, builder.get()));
+  EXPECT_EQ(
+      "{ \"rows\": 3, \"cols\": 4, \"storage_order\": \"RowMajor\", \"data\": "
+      "[ 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0 ] }",
+      aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+
+  const Eigen::Matrix<double, 3, 4, Eigen::StorageOptions::RowMajor> result =
+      ToEigenOrDie<3, 4, fbs::StorageOrder::RowMajor>(builder->AsFlatbuffer());
+  EXPECT_EQ(expected, result);
+}
+
+class FlatbuffersMatrixParamTest
+    : public FlatbuffersMatrixTest,
+      public ::testing::WithParamInterface<
+          std::tuple<std::string, ConversionFailure>> {};
+TEST_P(FlatbuffersMatrixParamTest, ConversionFailures) {
+  auto result = this->ToEigen<3, 4>(std::get<0>(GetParam()));
+  EXPECT_FALSE(result.has_value());
+  EXPECT_EQ(std::get<1>(GetParam()), result.error());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    ConversionFailureTests, FlatbuffersMatrixParamTest,
+    ::testing::Values(
+        std::make_tuple("{}", ConversionFailure{fbs::MatrixField::kRows,
+                                                fbs::FieldError::kMissing}),
+        std::make_tuple(R"json({"rows": 3})json",
+                        ConversionFailure{fbs::MatrixField::kCols,
+                                          fbs::FieldError::kMissing}),
+        std::make_tuple(R"json({"rows": 3, "cols": 4})json",
+                        ConversionFailure{fbs::MatrixField::kData,
+                                          fbs::FieldError::kMissing}),
+        std::make_tuple(
+            R"json({"rows": 1, "cols": 4, "data": [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]})json",
+            ConversionFailure{fbs::MatrixField::kRows,
+                              fbs::FieldError::kInconsistentWithTemplate}),
+        std::make_tuple(
+            R"json({"rows": 3, "cols": 7, "data": [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]})json",
+            ConversionFailure{fbs::MatrixField::kCols,
+                              fbs::FieldError::kInconsistentWithTemplate}),
+        std::make_tuple(R"json({"rows": 3, "cols": 4, "data": []})json",
+                        ConversionFailure{
+                            fbs::MatrixField::kData,
+                            fbs::FieldError::kInconsistentWithTemplate}),
+        std::make_tuple(
+            R"json({"rows": 3, "cols": 4, "storage_order": "RowMajor", "data": [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]})json",
+            ConversionFailure{fbs::MatrixField::kStorageOrder,
+                              fbs::FieldError::kInconsistentWithTemplate})));
+}  // namespace frc971::testing
diff --git a/frc971/math/matrix.fbs b/frc971/math/matrix.fbs
new file mode 100644
index 0000000..8bbd03a
--- /dev/null
+++ b/frc971/math/matrix.fbs
@@ -0,0 +1,39 @@
+namespace frc971.fbs;
+
+enum StorageOrder : ubyte {
+  // Column-major; i.e., for a matrix
+  // [1 2]
+  // [3 4]
+  // The memory layout will be 1 3 2 4.
+  ColMajor = 0,
+  // Row-major; i.e., for a matrix
+  // [1 2]
+  // [3 4]
+  // The memory layout will be 1 2 3 4.
+  RowMajor = 1,
+}
+
+// Represents a dynamically-sized 2-D matrix that is either row-major or column-major.
+table Matrix {
+  // rows and cols must both be greater than zero.
+  rows:uint (id: 0);
+  cols:uint (id: 1);
+  storage_order:StorageOrder = ColMajor (id: 2);
+  // data must be present and must have a length of rows * cols.
+  data:[double] (id: 3);
+}
+
+// The below enums are used in C++ code for communicating errors in parsing
+// the matrix; they are mostly only defined in the fbs file so that we get
+// pre-generated functions for converting the enum values to strings.
+enum MatrixField : ubyte {
+  kRows = 0,
+  kCols,
+  kStorageOrder,
+  kData,
+}
+
+enum FieldError : ubyte {
+  kInconsistentWithTemplate = 0,
+  kMissing,
+}
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
index 0bff39b..016bd81 100644
--- a/frc971/orin/argus_camera.cc
+++ b/frc971/orin/argus_camera.cc
@@ -276,6 +276,8 @@
       CHECK(ret == 0) << ": NvBufSurfaceFromFd failed";
 
       ret = NvBufSurfaceMapEglImage(surf_[i], 0);
+      // This check typically fails from having X forwarding enabled.
+      // Always call argus_camera without X forwarding.
       CHECK(ret == 0) << ": NvBufSurfaceMapEglImage failed";
 
       egl_images_[i] = surf_[i]->surfaceList[0].mappedAddr.eglImage;
diff --git a/frc971/queues/BUILD b/frc971/queues/BUILD
index 17b610c..8d7563b 100644
--- a/frc971/queues/BUILD
+++ b/frc971/queues/BUILD
@@ -1,21 +1,19 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "gyro_fbs",
     srcs = [
         "gyro.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "gyro_uid_fbs",
     srcs = [
         "gyro_uid.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 84f3be7..3c94b35 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -1,11 +1,11 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_py_library")
 load("//aos:config.bzl", "aos_config")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "vision_fbs",
     srcs = ["vision.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -17,10 +17,9 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "calibration_fbs",
     srcs = ["calibration.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -146,10 +145,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "target_map_fbs",
     srcs = ["target_map.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 33a3cd1..2cf2109 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -1,16 +1,15 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 load("//aos:config.bzl", "aos_config")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "logging_fbs",
     srcs = [
         "logging.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
@@ -126,12 +125,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "loop_output_handler_test_fbs",
     srcs = [
         "loop_output_handler_test.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
@@ -229,12 +227,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "pdp_values_fbs",
     srcs = [
         "pdp_values.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
@@ -268,25 +265,23 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "imu_fbs",
     srcs = [
         "imu.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "imu_batch_fbs",
     srcs = [
         "imu_batch.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        ":imu_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":imu_fbs",
+    ],
 )
 
 flatbuffer_ts_library(
@@ -401,6 +396,7 @@
         "//aos/time",
         "//aos/util:phased_loop",
         "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
         "//third_party:wpilib",
     ],
 )
@@ -496,12 +492,12 @@
 )
 
 cc_library(
-    name = "falcon",
+    name = "talonfx",
     srcs = [
-        "falcon.cc",
+        "talonfx.cc",
     ],
     hdrs = [
-        "falcon.h",
+        "talonfx.h",
     ],
     target_compatible_with = ["//tools/platforms/hardware:roborio"],
     deps = [
@@ -518,13 +514,46 @@
 )
 
 cc_library(
+    name = "can_drivetrain_writer",
+    srcs = [
+        "can_drivetrain_writer.cc",
+    ],
+    hdrs = [
+        "can_drivetrain_writer.h",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:roborio"],
+    deps = [
+        ":loop_output_handler",
+        ":talonfx",
+        "//frc971:can_configuration_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+    ],
+)
+
+cc_library(
+    name = "generic_can_writer",
+    srcs = [
+        "generic_can_writer.cc",
+    ],
+    hdrs = [
+        "generic_can_writer.h",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:roborio"],
+    deps = [
+        ":loop_output_handler",
+        ":talonfx",
+        "//frc971:can_configuration_fbs",
+    ],
+)
+
+cc_library(
     name = "can_sensor_reader",
     srcs = ["can_sensor_reader.cc"],
     hdrs = ["can_sensor_reader.h"],
     target_compatible_with = ["//tools/platforms/hardware:roborio"],
     visibility = ["//visibility:public"],
     deps = [
-        ":falcon",
+        ":talonfx",
         "//aos:realtime",
         "//aos/containers:sized_array",
         "//aos/events:event_loop",
diff --git a/frc971/wpilib/can_drivetrain_writer.cc b/frc971/wpilib/can_drivetrain_writer.cc
new file mode 100644
index 0000000..ae82812
--- /dev/null
+++ b/frc971/wpilib/can_drivetrain_writer.cc
@@ -0,0 +1,66 @@
+#include "frc971/wpilib/can_drivetrain_writer.h"
+
+using frc971::wpilib::CANDrivetrainWriter;
+
+CANDrivetrainWriter::CANDrivetrainWriter(::aos::EventLoop *event_loop)
+    : ::frc971::wpilib::LoopOutputHandler<
+          ::frc971::control_loops::drivetrain::Output>(event_loop,
+                                                       "/drivetrain") {
+  event_loop->SetRuntimeRealtimePriority(kDrivetrainWriterPriority);
+
+  event_loop->OnRun([this]() { WriteConfigs(); });
+}
+
+void CANDrivetrainWriter::set_talonfxs(
+    std::vector<std::shared_ptr<TalonFX>> right_talonfxs,
+    std::vector<std::shared_ptr<TalonFX>> left_talonfxs) {
+  right_talonfxs_ = std::move(right_talonfxs);
+  left_talonfxs_ = std::move(left_talonfxs);
+}
+
+void CANDrivetrainWriter::HandleCANConfiguration(
+    const CANConfiguration &configuration) {
+  for (auto talonfx : right_talonfxs_) {
+    talonfx->PrintConfigs();
+  }
+
+  for (auto talonfx : left_talonfxs_) {
+    talonfx->PrintConfigs();
+  }
+
+  if (configuration.reapply()) {
+    WriteConfigs();
+  }
+}
+
+void CANDrivetrainWriter::WriteConfigs() {
+  for (auto talonfx : right_talonfxs_) {
+    talonfx->WriteConfigs();
+  }
+
+  for (auto talonfx : left_talonfxs_) {
+    talonfx->WriteConfigs();
+  }
+}
+
+void CANDrivetrainWriter::Write(
+    const ::frc971::control_loops::drivetrain::Output &output) {
+  for (auto talonfx : right_talonfxs_) {
+    talonfx->WriteVoltage(output.right_voltage());
+  }
+
+  for (auto talonfx : left_talonfxs_) {
+    talonfx->WriteVoltage(output.left_voltage());
+  }
+}
+
+void CANDrivetrainWriter::Stop() {
+  AOS_LOG(WARNING, "Drivetrain CAN output too old.\n");
+  for (auto talonfx : right_talonfxs_) {
+    talonfx->WriteVoltage(0);
+  }
+
+  for (auto talonfx : left_talonfxs_) {
+    talonfx->WriteVoltage(0);
+  }
+}
diff --git a/frc971/wpilib/can_drivetrain_writer.h b/frc971/wpilib/can_drivetrain_writer.h
new file mode 100644
index 0000000..1c0ba95
--- /dev/null
+++ b/frc971/wpilib/can_drivetrain_writer.h
@@ -0,0 +1,34 @@
+#include "frc971/can_configuration_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/talonfx.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class CANDrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+                                ::frc971::control_loops::drivetrain::Output> {
+ public:
+  CANDrivetrainWriter(::aos::EventLoop *event_loop);
+
+  void set_talonfxs(std::vector<std::shared_ptr<TalonFX>> right_talonfxs,
+                    std::vector<std::shared_ptr<TalonFX>> left_talonfxs);
+
+  void HandleCANConfiguration(const CANConfiguration &configuration);
+
+  static constexpr int kDrivetrainWriterPriority = 35;
+
+ private:
+  void WriteConfigs();
+
+  void Write(
+      const ::frc971::control_loops::drivetrain::Output &output) override;
+
+  void Stop() override;
+
+  std::vector<std::shared_ptr<TalonFX>> right_talonfxs_;
+  std::vector<std::shared_ptr<TalonFX>> left_talonfxs_;
+};
+
+}  // namespace wpilib
+}  // namespace frc971
diff --git a/frc971/wpilib/can_sensor_reader.cc b/frc971/wpilib/can_sensor_reader.cc
index ad244fb..54be05d 100644
--- a/frc971/wpilib/can_sensor_reader.cc
+++ b/frc971/wpilib/can_sensor_reader.cc
@@ -6,11 +6,11 @@
 CANSensorReader::CANSensorReader(
     aos::EventLoop *event_loop,
     std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry,
-    std::vector<std::shared_ptr<Falcon>> falcons,
+    std::vector<std::shared_ptr<TalonFX>> talonfxs,
     std::function<void(ctre::phoenix::StatusCode status)> flatbuffer_callback)
     : event_loop_(event_loop),
       signals_(signals_registry.begin(), signals_registry.end()),
-      falcons_(falcons),
+      talonfxs_(talonfxs),
       flatbuffer_callback_(flatbuffer_callback) {
   event_loop->SetRuntimeRealtimePriority(40);
 
@@ -32,7 +32,7 @@
       ctre::phoenix6::BaseStatusSignal::WaitForAll(20_ms, signals_);
 
   if (!status.IsOK()) {
-    AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
+    AOS_LOG(ERROR, "Failed to read signals from talonfx motors: %s: %s",
             status.GetName(), status.GetDescription());
   }
 
diff --git a/frc971/wpilib/can_sensor_reader.h b/frc971/wpilib/can_sensor_reader.h
index 764a041..8ef5fd1 100644
--- a/frc971/wpilib/can_sensor_reader.h
+++ b/frc971/wpilib/can_sensor_reader.h
@@ -7,7 +7,7 @@
 #include "aos/events/event_loop.h"
 #include "aos/events/shm_event_loop.h"
 #include "aos/realtime.h"
-#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/talonfx.h"
 
 namespace frc971 {
 namespace wpilib {
@@ -16,7 +16,7 @@
   CANSensorReader(
       aos::EventLoop *event_loop,
       std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry,
-      std::vector<std::shared_ptr<Falcon>> falcons,
+      std::vector<std::shared_ptr<TalonFX>> talonfxs,
       std::function<void(ctre::phoenix::StatusCode status)>
           flatbuffer_callback);
 
@@ -27,9 +27,9 @@
 
   const std::vector<ctre::phoenix6::BaseStatusSignal *> signals_;
 
-  // This is a vector of falcons becuase we don't need to care
-  // about falcons individually.
-  std::vector<std::shared_ptr<Falcon>> falcons_;
+  // This is a vector of talonfxs becuase we don't need to care
+  // about talonfxs individually.
+  std::vector<std::shared_ptr<TalonFX>> talonfxs_;
 
   // Pointer to the timer handler used to modify the wakeup.
   ::aos::TimerHandler *timer_handler_;
diff --git a/frc971/wpilib/falcon.cc b/frc971/wpilib/falcon.cc
deleted file mode 100644
index deeb9f8..0000000
--- a/frc971/wpilib/falcon.cc
+++ /dev/null
@@ -1,138 +0,0 @@
-#include "frc971/wpilib/falcon.h"
-
-using frc971::wpilib::Falcon;
-using frc971::wpilib::kMaxBringupPower;
-
-Falcon::Falcon(int device_id, bool inverted, std::string canbus,
-               std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
-               double stator_current_limit, double supply_current_limit)
-    : talon_(device_id, canbus),
-      device_id_(device_id),
-      inverted_(inverted),
-      device_temp_(talon_.GetDeviceTemp()),
-      supply_voltage_(talon_.GetSupplyVoltage()),
-      supply_current_(talon_.GetSupplyCurrent()),
-      torque_current_(talon_.GetTorqueCurrent()),
-      position_(talon_.GetPosition()),
-      duty_cycle_(talon_.GetDutyCycle()),
-      stator_current_limit_(stator_current_limit),
-      supply_current_limit_(supply_current_limit) {
-  // device temp is not timesynced so don't add it to the list of signals
-  device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
-
-  CHECK_NOTNULL(signals);
-
-  supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
-  signals->push_back(&supply_voltage_);
-
-  supply_current_.SetUpdateFrequency(kCANUpdateFreqHz);
-  signals->push_back(&supply_current_);
-
-  torque_current_.SetUpdateFrequency(kCANUpdateFreqHz);
-  signals->push_back(&torque_current_);
-
-  position_.SetUpdateFrequency(kCANUpdateFreqHz);
-  signals->push_back(&position_);
-
-  duty_cycle_.SetUpdateFrequency(kCANUpdateFreqHz);
-  signals->push_back(&duty_cycle_);
-}
-
-Falcon::Falcon(FalconParams params, std::string canbus,
-               std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
-               double stator_current_limit, double supply_current_limit)
-    : Falcon(params.device_id, params.inverted, canbus, signals,
-             stator_current_limit, supply_current_limit) {}
-
-void Falcon::PrintConfigs() {
-  ctre::phoenix6::configs::TalonFXConfiguration configuration;
-  ctre::phoenix::StatusCode status =
-      talon_.GetConfigurator().Refresh(configuration);
-  if (!status.IsOK()) {
-    AOS_LOG(ERROR, "Failed to get falcon configuration: %s: %s",
-            status.GetName(), status.GetDescription());
-  }
-  AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
-}
-
-void Falcon::WriteConfigs() {
-  ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
-  current_limits.StatorCurrentLimit = stator_current_limit_;
-  current_limits.StatorCurrentLimitEnable = true;
-  current_limits.SupplyCurrentLimit = supply_current_limit_;
-  current_limits.SupplyCurrentLimitEnable = true;
-
-  ctre::phoenix6::configs::MotorOutputConfigs output_configs;
-  output_configs.NeutralMode = ctre::phoenix6::signals::NeutralModeValue::Brake;
-  output_configs.DutyCycleNeutralDeadband = 0;
-
-  output_configs.Inverted = inverted_;
-
-  ctre::phoenix6::configs::TalonFXConfiguration configuration;
-  configuration.CurrentLimits = current_limits;
-  configuration.MotorOutput = output_configs;
-
-  ctre::phoenix::StatusCode status =
-      talon_.GetConfigurator().Apply(configuration);
-  if (!status.IsOK()) {
-    AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
-            status.GetName(), status.GetDescription());
-  }
-
-  PrintConfigs();
-}
-
-ctre::phoenix::StatusCode Falcon::WriteCurrent(double current,
-                                               double max_voltage) {
-  ctre::phoenix6::controls::TorqueCurrentFOC control(
-      static_cast<units::current::ampere_t>(current));
-  // Using 0_Hz here makes it a one-shot update.
-  control.UpdateFreqHz = 0_Hz;
-  control.MaxAbsDutyCycle = SafeSpeed(max_voltage);
-  ctre::phoenix::StatusCode status = talon()->SetControl(control);
-  if (!status.IsOK()) {
-    AOS_LOG(ERROR, "Failed to write control to falcon %d: %s: %s", device_id(),
-            status.GetName(), status.GetDescription());
-  }
-
-  return status;
-}
-
-ctre::phoenix::StatusCode Falcon::WriteVoltage(double voltage) {
-  ctre::phoenix6::controls::DutyCycleOut control(SafeSpeed(voltage));
-
-  // Using 0_Hz here makes it a one-shot update.
-  control.UpdateFreqHz = 0_Hz;
-  control.EnableFOC = true;
-
-  ctre::phoenix::StatusCode status = talon()->SetControl(control);
-  if (!status.IsOK()) {
-    AOS_LOG(ERROR, "Failed to write control to falcon %d: %s: %s", device_id(),
-            status.GetName(), status.GetDescription());
-  }
-
-  return status;
-}
-
-void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
-                               double gear_ratio) {
-  control_loops::CANFalcon::Builder builder(*fbb);
-  builder.add_id(device_id_);
-  builder.add_device_temp(device_temp());
-  builder.add_supply_voltage(supply_voltage());
-  builder.add_supply_current(supply_current());
-  builder.add_torque_current(torque_current());
-  builder.add_duty_cycle(duty_cycle());
-  builder.add_position(position() * gear_ratio);
-
-  last_position_offset_ = builder.Finish();
-}
-
-std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
-Falcon::TakeOffset() {
-  auto option_offset = last_position_offset_;
-
-  last_position_offset_.reset();
-
-  return option_offset;
-}
diff --git a/frc971/wpilib/falcon.h b/frc971/wpilib/falcon.h
deleted file mode 100644
index 804c7a0..0000000
--- a/frc971/wpilib/falcon.h
+++ /dev/null
@@ -1,109 +0,0 @@
-#ifndef FRC971_WPILIB_FALCON_H_
-#define FRC971_WPILIB_FALCON_H_
-
-#include <chrono>
-#include <cinttypes>
-#include <vector>
-
-#include "ctre/phoenix6/TalonFX.hpp"
-#include "glog/logging.h"
-
-#include "aos/commonmath.h"
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
-
-namespace control_loops = ::frc971::control_loops;
-
-namespace frc971 {
-namespace wpilib {
-
-struct FalconParams {
-  int device_id;
-  bool inverted;
-};
-
-static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
-static constexpr double kMaxBringupPower = 12.0;
-
-// Gets info from and writes to falcon motors using the TalonFX controller.
-class Falcon {
- public:
-  Falcon(int device_id, bool inverted, std::string canbus,
-         std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
-         double stator_current_limit, double supply_current_limit);
-
-  Falcon(FalconParams params, std::string canbus,
-         std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
-         double stator_current_limit, double supply_current_limit);
-
-  void PrintConfigs();
-
-  void WriteConfigs();
-  ctre::phoenix::StatusCode WriteCurrent(double current, double max_voltage);
-
-  ctre::phoenix::StatusCode WriteVoltage(double voltage);
-
-  ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
-
-  // The position of the Falcon output shaft is multiplied by gear_ratio
-  void SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
-                         double gear_ratio);
-
-  std::optional<flatbuffers::Offset<control_loops::CANFalcon>> TakeOffset();
-
-  int device_id() const { return device_id_; }
-  float device_temp() const { return device_temp_.GetValue().value(); }
-  float supply_voltage() const { return supply_voltage_.GetValue().value(); }
-  float supply_current() const { return supply_current_.GetValue().value(); }
-  float torque_current() const { return torque_current_.GetValue().value(); }
-  float duty_cycle() const { return duty_cycle_.GetValue().value(); }
-  float position() const {
-    return static_cast<units::angle::radian_t>(position_.GetValue()).value();
-  }
-
-  // returns the monotonic timestamp of the latest timesynced reading in the
-  // timebase of the the syncronized CAN bus clock.
-  int64_t GetTimestamp() {
-    std::chrono::nanoseconds latest_timestamp =
-        torque_current_.GetTimestamp().GetTime();
-
-    return latest_timestamp.count();
-  }
-
-  void RefreshNontimesyncedSignals() { device_temp_.Refresh(); };
-
-  void set_stator_current_limit(double stator_current_limit) {
-    stator_current_limit_ = stator_current_limit;
-  }
-
-  void set_supply_current_limit(double supply_current_limit) {
-    supply_current_limit_ = supply_current_limit;
-  }
-
-  static double SafeSpeed(double voltage) {
-    return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
-  }
-
- private:
-  ctre::phoenix6::hardware::TalonFX talon_;
-  int device_id_;
-
-  ctre::phoenix6::signals::InvertedValue inverted_;
-
-  ctre::phoenix6::StatusSignal<units::temperature::celsius_t> device_temp_;
-  ctre::phoenix6::StatusSignal<units::voltage::volt_t> supply_voltage_;
-  ctre::phoenix6::StatusSignal<units::current::ampere_t> supply_current_,
-      torque_current_;
-  ctre::phoenix6::StatusSignal<units::angle::turn_t> position_;
-  ctre::phoenix6::StatusSignal<units::dimensionless::scalar_t> duty_cycle_;
-
-  double stator_current_limit_;
-  double supply_current_limit_;
-
-  std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
-      last_position_offset_;
-};
-}  // namespace wpilib
-}  // namespace frc971
-#endif  // FRC971_WPILIB_FALCON_H_
diff --git a/frc971/wpilib/generic_can_writer.cc b/frc971/wpilib/generic_can_writer.cc
new file mode 100644
index 0000000..8658180
--- /dev/null
+++ b/frc971/wpilib/generic_can_writer.cc
@@ -0,0 +1 @@
+#include "frc971/wpilib/generic_can_writer.h"
diff --git a/frc971/wpilib/generic_can_writer.h b/frc971/wpilib/generic_can_writer.h
new file mode 100644
index 0000000..7555c0a
--- /dev/null
+++ b/frc971/wpilib/generic_can_writer.h
@@ -0,0 +1,74 @@
+#include <map>
+#include <string_view>
+
+#include "frc971/can_configuration_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/talonfx.h"
+
+namespace frc971 {
+namespace wpilib {
+
+/// This class uses a callback whenever it writes so that the caller can use any
+/// flatbuffer to write to the talonfx motor.
+template <typename T>
+class GenericCANWriter : public LoopOutputHandler<T> {
+ public:
+  GenericCANWriter(
+      ::aos::EventLoop *event_loop,
+      std::function<
+          void(const T &output,
+               std::map<std::string, std::shared_ptr<TalonFX>> talonfx_map)>
+          write_callback)
+      : LoopOutputHandler<T>(event_loop, "/superstructure"),
+        write_callback_(write_callback) {
+    event_loop->SetRuntimeRealtimePriority(kGenericCANWriterPriority);
+
+    event_loop->OnRun([this]() { WriteConfigs(); });
+  }
+
+  void HandleCANConfiguration(const CANConfiguration &configuration) {
+    for (auto &[_, talonfx] : talonfx_map_) {
+      talonfx->PrintConfigs();
+    }
+
+    if (configuration.reapply()) {
+      WriteConfigs();
+    }
+  }
+
+  void add_talonfx(std::string_view name, std::shared_ptr<TalonFX> talonfx) {
+    talonfx_map_.insert({name, std::move(talonfx)});
+  }
+
+  static constexpr int kGenericCANWriterPriority = 35;
+
+ private:
+  void WriteConfigs() {
+    for (auto &[_, talonfx] : talonfx_map_) {
+      talonfx->WriteConfigs();
+    }
+  }
+
+  void Write(const T &output) override {
+    write_callback_(output, talonfx_map_);
+  }
+
+  void Stop() override {
+    AOS_LOG(WARNING, "Generic CAN output too old.\n");
+    for (auto &[_, talonfx] : talonfx_map_) {
+      talonfx->WriteVoltage(0);
+    }
+  }
+
+  // Maps each name to a talonfx to let the caller retreive them when
+  // writing
+  std::map<std::string_view, std::shared_ptr<TalonFX>> talonfx_map_;
+
+  std::function<void(
+      const T &output,
+      std::map<std::string, std::shared_ptr<TalonFX>> talonfx_map)>
+      write_callback_;
+};
+
+}  // namespace wpilib
+}  // namespace frc971
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index bbcc3b5..1486042 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -84,6 +84,29 @@
   return monotonic_clock::epoch() + (new_fpga_time + *fpga_offset);
 }
 
+void SensorReader::SendDrivetrainPosition(
+    aos::Sender<control_loops::drivetrain::PositionStatic>::StaticBuilder
+        builder,
+    std::function<double(double input)> velocity_translate,
+    std::function<double(double input)> encoder_to_meters, bool left_inverted,
+    bool right_inverted) {
+  builder->set_left_encoder(
+      (left_inverted ? -1.0 : 1.0) *
+      encoder_to_meters(drivetrain_left_encoder_->GetRaw()));
+  builder->set_left_speed(
+      (left_inverted ? -1.0 : 1.0) *
+      velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+
+  builder->set_right_encoder(
+      (right_inverted ? -1.0 : 1.0) *
+      encoder_to_meters(drivetrain_right_encoder_->GetRaw()));
+  builder->set_right_speed(
+      (right_inverted ? -1.0 : 1.0) *
+      velocity_translate(drivetrain_right_encoder_->GetPeriod()));
+
+  builder.CheckOk(builder.Send());
+}
+
 void SensorReader::DoStart() {
   Start();
   if (dma_synchronizer_) {
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 59434a8..bf8c96a 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -8,7 +8,8 @@
 #include "aos/events/shm_event_loop.h"
 #include "aos/stl_mutex/stl_mutex.h"
 #include "aos/time/time.h"
-#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/control_loops_static.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_static.h"
 #include "frc971/input/robot_state_generated.h"
 #include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
 #include "frc971/wpilib/ahal/DigitalInput.h"
@@ -64,149 +65,149 @@
   // Copies a DMAEncoder to a IndexPosition with the correct unit and direction
   // changes.
   void CopyPosition(const ::frc971::wpilib::DMAEncoder &encoder,
-                    ::frc971::IndexPositionT *position,
+                    ::frc971::IndexPositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->encoder =
-        multiplier * encoder_translate(encoder.polled_encoder_value(),
-                                       encoder_counts_per_revolution,
-                                       encoder_ratio);
-    position->latched_encoder =
+    position->set_encoder(multiplier *
+                          encoder_translate(encoder.polled_encoder_value(),
+                                            encoder_counts_per_revolution,
+                                            encoder_ratio));
+    position->set_latched_encoder(
         multiplier * encoder_translate(encoder.last_encoder_value(),
                                        encoder_counts_per_revolution,
-                                       encoder_ratio);
-    position->index_pulses = encoder.index_posedge_count();
+                                       encoder_ratio));
+    position->set_index_pulses(encoder.index_posedge_count());
   }
 
   // Copies a AbsoluteEncoderAndPotentiometer to a PotAndAbsolutePosition with
   // the correct unit and direction changes.
   void CopyPosition(
       const ::frc971::wpilib::AbsoluteEncoderAndPotentiometer &encoder,
-      ::frc971::PotAndAbsolutePositionT *position,
+      ::frc971::PotAndAbsolutePositionStatic *position,
       double encoder_counts_per_revolution, double encoder_ratio,
       ::std::function<double(double)> potentiometer_translate, bool reverse,
       double pot_offset) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->pot = multiplier * potentiometer_translate(
-                                     encoder.ReadPotentiometerVoltage()) +
-                    pot_offset;
-    position->encoder =
-        multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
-                                       encoder_counts_per_revolution,
-                                       encoder_ratio);
+    position->set_pot(multiplier * potentiometer_translate(
+                                       encoder.ReadPotentiometerVoltage()) +
+                      pot_offset);
+    position->set_encoder(multiplier *
+                          encoder_translate(encoder.ReadRelativeEncoder(),
+                                            encoder_counts_per_revolution,
+                                            encoder_ratio));
 
-    position->absolute_encoder =
-        (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
-                 : encoder.ReadAbsoluteEncoder()) *
-        encoder_ratio * (2.0 * M_PI);
+    position->set_absolute_encoder((reverse
+                                        ? (1.0 - encoder.ReadAbsoluteEncoder())
+                                        : encoder.ReadAbsoluteEncoder()) *
+                                   encoder_ratio * (2.0 * M_PI));
   }
 
   // Copies an AbsoluteEncoderAndPotentiometer to an AbsoluteAndAbsolutePosition
   // with the correct unit and direction changes.
   void CopyPosition(const ::frc971::wpilib::AbsoluteAndAbsoluteEncoder &encoder,
-                    ::frc971::AbsoluteAndAbsolutePositionT *position,
+                    ::frc971::AbsoluteAndAbsolutePositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     double single_turn_encoder_ratio, bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->encoder =
-        multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
-                                       encoder_counts_per_revolution,
-                                       encoder_ratio);
+    position->set_encoder(multiplier *
+                          encoder_translate(encoder.ReadRelativeEncoder(),
+                                            encoder_counts_per_revolution,
+                                            encoder_ratio));
 
-    position->absolute_encoder =
-        (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
-                 : encoder.ReadAbsoluteEncoder()) *
-        encoder_ratio * (2.0 * M_PI);
+    position->set_absolute_encoder((reverse
+                                        ? (1.0 - encoder.ReadAbsoluteEncoder())
+                                        : encoder.ReadAbsoluteEncoder()) *
+                                   encoder_ratio * (2.0 * M_PI));
 
-    position->single_turn_absolute_encoder =
+    position->set_single_turn_absolute_encoder(
         (reverse ? (1.0 - encoder.ReadSingleTurnAbsoluteEncoder())
                  : encoder.ReadSingleTurnAbsoluteEncoder()) *
-        single_turn_encoder_ratio * (2.0 * M_PI);
+        single_turn_encoder_ratio * (2.0 * M_PI));
   }
 
   // Copies a DMAEdgeCounter to a HallEffectAndPosition with the correct unit
   // and direction changes.
   void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
-                    ::frc971::HallEffectAndPositionT *position,
+                    ::frc971::HallEffectAndPositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->encoder =
-        multiplier * encoder_translate(counter.polled_encoder(),
-                                       encoder_counts_per_revolution,
-                                       encoder_ratio);
-    position->current = !counter.polled_value();
-    position->posedge_count = counter.negative_count();
-    position->negedge_count = counter.positive_count();
-    position->posedge_value =
+    position->set_encoder(multiplier *
+                          encoder_translate(counter.polled_encoder(),
+                                            encoder_counts_per_revolution,
+                                            encoder_ratio));
+    position->set_current(!counter.polled_value());
+    position->set_posedge_count(counter.negative_count());
+    position->set_negedge_count(counter.positive_count());
+    position->set_posedge_value(
         multiplier * encoder_translate(counter.last_negative_encoder_value(),
                                        encoder_counts_per_revolution,
-                                       encoder_ratio);
-    position->negedge_value =
+                                       encoder_ratio));
+    position->set_negedge_value(
         multiplier * encoder_translate(counter.last_positive_encoder_value(),
                                        encoder_counts_per_revolution,
-                                       encoder_ratio);
+                                       encoder_ratio));
   }
 
   // Copies a Absolute Encoder with the correct unit
   // and direction changes.
   void CopyPosition(const ::frc971::wpilib::AbsoluteEncoder &encoder,
-                    ::frc971::AbsolutePositionT *position,
+                    ::frc971::AbsolutePositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->encoder =
-        multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
-                                       encoder_counts_per_revolution,
-                                       encoder_ratio);
+    position->set_encoder(multiplier *
+                          encoder_translate(encoder.ReadRelativeEncoder(),
+                                            encoder_counts_per_revolution,
+                                            encoder_ratio));
 
-    position->absolute_encoder =
-        (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
-                 : encoder.ReadAbsoluteEncoder()) *
-        encoder_ratio * (2.0 * M_PI);
+    position->set_absolute_encoder((reverse
+                                        ? (1.0 - encoder.ReadAbsoluteEncoder())
+                                        : encoder.ReadAbsoluteEncoder()) *
+                                   encoder_ratio * (2.0 * M_PI));
   }
 
   void CopyPosition(const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
-                    ::frc971::PotAndIndexPositionT *position,
+                    ::frc971::PotAndIndexPositionStatic *position,
                     ::std::function<double(int32_t)> encoder_translate,
                     ::std::function<double(double)> potentiometer_translate,
                     bool reverse, double pot_offset) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->encoder =
-        multiplier * encoder_translate(encoder.polled_encoder_value());
-    position->pot = multiplier * potentiometer_translate(
-                                     encoder.polled_potentiometer_voltage()) +
-                    pot_offset;
-    position->latched_encoder =
-        multiplier * encoder_translate(encoder.last_encoder_value());
-    position->latched_pot =
+    position->set_encoder(multiplier *
+                          encoder_translate(encoder.polled_encoder_value()));
+    position->set_pot(multiplier * potentiometer_translate(
+                                       encoder.polled_potentiometer_voltage()) +
+                      pot_offset);
+    position->set_latched_encoder(
+        multiplier * encoder_translate(encoder.last_encoder_value()));
+    position->set_latched_pot(
         multiplier *
             potentiometer_translate(encoder.last_potentiometer_voltage()) +
-        pot_offset;
-    position->index_pulses = encoder.index_posedge_count();
+        pot_offset);
+    position->set_index_pulses(encoder.index_posedge_count());
   }
 
   // Copies a relative digital encoder.
   void CopyPosition(const ::frc::Encoder &encoder,
-                    ::frc971::RelativePositionT *position,
+                    ::frc971::RelativePositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->encoder =
-        multiplier * encoder_translate(encoder.GetRaw(),
-                                       encoder_counts_per_revolution,
-                                       encoder_ratio);
+    position->set_encoder(multiplier *
+                          encoder_translate(encoder.GetRaw(),
+                                            encoder_counts_per_revolution,
+                                            encoder_ratio));
   }
 
   // Copies a potentiometer
   void CopyPosition(const ::frc::AnalogInput &input,
-                    ::frc971::RelativePositionT *position,
+                    ::frc971::RelativePositionStatic *position,
                     ::std::function<double(double)> potentiometer_translate,
                     bool reverse, double pot_offset) {
     const double multiplier = reverse ? -1.0 : 1.0;
-    position->encoder =
-        multiplier * potentiometer_translate(input.GetVoltage()) + pot_offset;
+    position->set_encoder(
+        multiplier * potentiometer_translate(input.GetVoltage()) + pot_offset);
   }
 
   double encoder_translate(int32_t value, double counts_per_revolution,
@@ -215,6 +216,13 @@
            (2.0 * M_PI);
   }
 
+  void SendDrivetrainPosition(
+      aos::Sender<control_loops::drivetrain::PositionStatic>::StaticBuilder
+          builder,
+      std::function<double(double input)> velocity_translate,
+      std::function<double(double input)> encoder_to_meters, bool left_inverted,
+      bool right_inverted);
+
   ::aos::EventLoop *event_loop_;
   ::aos::Sender<::aos::RobotState> robot_state_sender_;
 
diff --git a/frc971/wpilib/swerve/BUILD b/frc971/wpilib/swerve/BUILD
index 9f559f6..4493781 100644
--- a/frc971/wpilib/swerve/BUILD
+++ b/frc971/wpilib/swerve/BUILD
@@ -13,8 +13,8 @@
         "//aos/logging",
         "//frc971:can_configuration_fbs",
         "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
-        "//frc971/wpilib:falcon",
         "//frc971/wpilib:loop_output_handler",
+        "//frc971/wpilib:talonfx",
         "//third_party:phoenix6",
     ],
 )
@@ -25,6 +25,6 @@
         "swerve_module.h",
     ],
     deps = [
-        "//frc971/wpilib:falcon",
+        "//frc971/wpilib:talonfx",
     ],
 )
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
index bfc5d73..e4b4ee9 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
@@ -14,10 +14,10 @@
   event_loop->OnRun([this]() { WriteConfigs(); });
 }
 
-void DrivetrainWriter::set_falcons(std::shared_ptr<SwerveModule> front_left,
-                                   std::shared_ptr<SwerveModule> front_right,
-                                   std::shared_ptr<SwerveModule> back_left,
-                                   std::shared_ptr<SwerveModule> back_right) {
+void DrivetrainWriter::set_talonfxs(std::shared_ptr<SwerveModule> front_left,
+                                    std::shared_ptr<SwerveModule> front_right,
+                                    std::shared_ptr<SwerveModule> back_left,
+                                    std::shared_ptr<SwerveModule> back_right) {
   front_left_ = std::move(front_left);
   front_right_ = std::move(front_right);
   back_left_ = std::move(back_left);
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.h b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
index 4bd6639..d02d4dc 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.h
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
@@ -5,9 +5,9 @@
 
 #include "frc971/can_configuration_generated.h"
 #include "frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output_generated.h"
-#include "frc971/wpilib/falcon.h"
 #include "frc971/wpilib/loop_output_handler.h"
 #include "frc971/wpilib/swerve/swerve_module.h"
+#include "frc971/wpilib/talonfx.h"
 
 namespace frc971 {
 namespace wpilib {
@@ -22,10 +22,10 @@
   DrivetrainWriter(::aos::EventLoop *event_loop, int drivetrain_writer_priority,
                    double max_voltage);
 
-  void set_falcons(std::shared_ptr<SwerveModule> front_left,
-                   std::shared_ptr<SwerveModule> front_right,
-                   std::shared_ptr<SwerveModule> back_left,
-                   std::shared_ptr<SwerveModule> back_right);
+  void set_talonfxs(std::shared_ptr<SwerveModule> front_left,
+                    std::shared_ptr<SwerveModule> front_right,
+                    std::shared_ptr<SwerveModule> back_left,
+                    std::shared_ptr<SwerveModule> back_right);
 
   void HandleCANConfiguration(const CANConfiguration &configuration);
 
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index f449afa..d65547a 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -1,23 +1,23 @@
 #ifndef FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
 #define FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
 
-#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/talonfx.h"
 
 namespace frc971 {
 namespace wpilib {
 namespace swerve {
 
 struct SwerveModule {
-  SwerveModule(FalconParams rotation_params, FalconParams translation_params,
+  SwerveModule(TalonFXParams rotation_params, TalonFXParams translation_params,
                std::string canbus,
                std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
                double stator_current_limit, double supply_current_limit)
-      : rotation(std::make_shared<Falcon>(rotation_params, canbus, signals,
-                                          stator_current_limit,
-                                          supply_current_limit)),
-        translation(std::make_shared<Falcon>(translation_params, canbus,
-                                             signals, stator_current_limit,
-                                             supply_current_limit)) {}
+      : rotation(std::make_shared<TalonFX>(rotation_params, canbus, signals,
+                                           stator_current_limit,
+                                           supply_current_limit)),
+        translation(std::make_shared<TalonFX>(translation_params, canbus,
+                                              signals, stator_current_limit,
+                                              supply_current_limit)) {}
 
   void WriteModule(
       const frc971::control_loops::drivetrain::swerve::SwerveModuleOutput
@@ -35,8 +35,8 @@
     translation->WriteCurrent(translation_current, max_voltage);
   }
 
-  std::shared_ptr<Falcon> rotation;
-  std::shared_ptr<Falcon> translation;
+  std::shared_ptr<TalonFX> rotation;
+  std::shared_ptr<TalonFX> translation;
 };
 
 }  // namespace swerve
diff --git a/frc971/wpilib/talonfx.cc b/frc971/wpilib/talonfx.cc
new file mode 100644
index 0000000..5e77618
--- /dev/null
+++ b/frc971/wpilib/talonfx.cc
@@ -0,0 +1,125 @@
+#include "frc971/wpilib/talonfx.h"
+
+using frc971::wpilib::kMaxBringupPower;
+using frc971::wpilib::TalonFX;
+
+TalonFX::TalonFX(int device_id, bool inverted, std::string canbus,
+                 std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+                 double stator_current_limit, double supply_current_limit)
+    : talon_(device_id, canbus),
+      device_id_(device_id),
+      inverted_(inverted),
+      device_temp_(talon_.GetDeviceTemp()),
+      supply_voltage_(talon_.GetSupplyVoltage()),
+      supply_current_(talon_.GetSupplyCurrent()),
+      torque_current_(talon_.GetTorqueCurrent()),
+      position_(talon_.GetPosition()),
+      duty_cycle_(talon_.GetDutyCycle()),
+      stator_current_limit_(stator_current_limit),
+      supply_current_limit_(supply_current_limit) {
+  // device temp is not timesynced so don't add it to the list of signals
+  device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
+
+  CHECK_NOTNULL(signals);
+
+  supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
+  signals->push_back(&supply_voltage_);
+
+  supply_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+  signals->push_back(&supply_current_);
+
+  torque_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+  signals->push_back(&torque_current_);
+
+  position_.SetUpdateFrequency(kCANUpdateFreqHz);
+  signals->push_back(&position_);
+
+  duty_cycle_.SetUpdateFrequency(kCANUpdateFreqHz);
+  signals->push_back(&duty_cycle_);
+}
+
+TalonFX::TalonFX(TalonFXParams params, std::string canbus,
+                 std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+                 double stator_current_limit, double supply_current_limit)
+    : TalonFX(params.device_id, params.inverted, canbus, signals,
+              stator_current_limit, supply_current_limit) {}
+
+void TalonFX::PrintConfigs() {
+  ctre::phoenix6::configs::TalonFXConfiguration configuration;
+  ctre::phoenix::StatusCode status =
+      talon_.GetConfigurator().Refresh(configuration);
+  if (!status.IsOK()) {
+    AOS_LOG(ERROR, "Failed to get talonfx motor configuration: %s: %s",
+            status.GetName(), status.GetDescription());
+  }
+  AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
+}
+
+void TalonFX::WriteConfigs() {
+  ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+  current_limits.StatorCurrentLimit = stator_current_limit_;
+  current_limits.StatorCurrentLimitEnable = true;
+  current_limits.SupplyCurrentLimit = supply_current_limit_;
+  current_limits.SupplyCurrentLimitEnable = true;
+
+  ctre::phoenix6::configs::MotorOutputConfigs output_configs;
+  output_configs.NeutralMode = ctre::phoenix6::signals::NeutralModeValue::Brake;
+  output_configs.DutyCycleNeutralDeadband = 0;
+
+  output_configs.Inverted = inverted_;
+
+  ctre::phoenix6::configs::TalonFXConfiguration configuration;
+  configuration.CurrentLimits = current_limits;
+  configuration.MotorOutput = output_configs;
+
+  ctre::phoenix::StatusCode status =
+      talon_.GetConfigurator().Apply(configuration);
+  if (!status.IsOK()) {
+    AOS_LOG(ERROR, "Failed to set talonfx motor configuration: %s: %s",
+            status.GetName(), status.GetDescription());
+  }
+
+  PrintConfigs();
+}
+
+ctre::phoenix::StatusCode TalonFX::WriteCurrent(double current,
+                                                double max_voltage) {
+  ctre::phoenix6::controls::TorqueCurrentFOC control(
+      static_cast<units::current::ampere_t>(current));
+  // Using 0_Hz here makes it a one-shot update.
+  control.UpdateFreqHz = 0_Hz;
+  control.MaxAbsDutyCycle = SafeSpeed(max_voltage);
+  ctre::phoenix::StatusCode status = talon()->SetControl(control);
+  if (!status.IsOK()) {
+    AOS_LOG(ERROR, "Failed to write control to talonfx motor %d: %s: %s",
+            device_id(), status.GetName(), status.GetDescription());
+  }
+
+  return status;
+}
+
+ctre::phoenix::StatusCode TalonFX::WriteVoltage(double voltage) {
+  ctre::phoenix6::controls::DutyCycleOut control(SafeSpeed(voltage));
+
+  // Using 0_Hz here makes it a one-shot update.
+  control.UpdateFreqHz = 0_Hz;
+  control.EnableFOC = true;
+
+  ctre::phoenix::StatusCode status = talon()->SetControl(control);
+  if (!status.IsOK()) {
+    AOS_LOG(ERROR, "Failed to write control to talonfx motor %d: %s: %s",
+            device_id(), status.GetName(), status.GetDescription());
+  }
+
+  return status;
+}
+void TalonFX::SerializePosition(control_loops::CANTalonFXStatic *can_talonfx,
+                                double gear_ratio) {
+  can_talonfx->set_id(device_id_);
+  can_talonfx->set_device_temp(device_temp());
+  can_talonfx->set_supply_voltage(supply_voltage());
+  can_talonfx->set_supply_current(supply_current());
+  can_talonfx->set_torque_current(torque_current());
+  can_talonfx->set_duty_cycle(duty_cycle());
+  can_talonfx->set_position(position() * gear_ratio);
+}
diff --git a/frc971/wpilib/talonfx.h b/frc971/wpilib/talonfx.h
new file mode 100644
index 0000000..c25fa81
--- /dev/null
+++ b/frc971/wpilib/talonfx.h
@@ -0,0 +1,105 @@
+#ifndef FRC971_WPILIB_TALONFX_MOTOR_H_
+#define FRC971_WPILIB_TALONFX_MOTOR_H_
+
+#include <chrono>
+#include <cinttypes>
+#include <vector>
+
+#include "ctre/phoenix6/TalonFX.hpp"
+#include "glog/logging.h"
+
+#include "aos/commonmath.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/can_talonfx_static.h"
+
+namespace control_loops = ::frc971::control_loops;
+
+namespace frc971 {
+namespace wpilib {
+
+struct TalonFXParams {
+  int device_id;
+  bool inverted;
+};
+
+static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+static constexpr double kMaxBringupPower = 12.0;
+
+// Class which represents a motor controlled by a TalonFX motor controller over
+// CAN.
+class TalonFX {
+ public:
+  TalonFX(int device_id, bool inverted, std::string canbus,
+          std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+          double stator_current_limit, double supply_current_limit);
+
+  TalonFX(TalonFXParams params, std::string canbus,
+          std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+          double stator_current_limit, double supply_current_limit);
+
+  void PrintConfigs();
+
+  void WriteConfigs();
+  ctre::phoenix::StatusCode WriteCurrent(double current, double max_voltage);
+
+  ctre::phoenix::StatusCode WriteVoltage(double voltage);
+
+  ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
+
+  // The position of the TalonFX output shaft is multiplied by gear_ratio
+  void SerializePosition(control_loops::CANTalonFXStatic *can_falcon,
+                         double gear_ratio);
+
+  int device_id() const { return device_id_; }
+  float device_temp() const { return device_temp_.GetValue().value(); }
+  float supply_voltage() const { return supply_voltage_.GetValue().value(); }
+  float supply_current() const { return supply_current_.GetValue().value(); }
+  float torque_current() const { return torque_current_.GetValue().value(); }
+  float duty_cycle() const { return duty_cycle_.GetValue().value(); }
+  float position() const {
+    return static_cast<units::angle::radian_t>(position_.GetValue()).value();
+  }
+
+  // returns the monotonic timestamp of the latest timesynced reading in the
+  // timebase of the the syncronized CAN bus clock.
+  int64_t GetTimestamp() {
+    std::chrono::nanoseconds latest_timestamp =
+        torque_current_.GetTimestamp().GetTime();
+
+    return latest_timestamp.count();
+  }
+
+  void RefreshNontimesyncedSignals() { device_temp_.Refresh(); };
+
+  void set_stator_current_limit(double stator_current_limit) {
+    stator_current_limit_ = stator_current_limit;
+  }
+
+  void set_supply_current_limit(double supply_current_limit) {
+    supply_current_limit_ = supply_current_limit;
+  }
+
+  static double SafeSpeed(double voltage) {
+    return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+  }
+
+ private:
+  ctre::phoenix6::hardware::TalonFX talon_;
+  int device_id_;
+
+  ctre::phoenix6::signals::InvertedValue inverted_;
+
+  ctre::phoenix6::StatusSignal<units::temperature::celsius_t> device_temp_;
+  ctre::phoenix6::StatusSignal<units::voltage::volt_t> supply_voltage_;
+  ctre::phoenix6::StatusSignal<units::current::ampere_t> supply_current_,
+      torque_current_;
+  ctre::phoenix6::StatusSignal<units::angle::turn_t> position_;
+  ctre::phoenix6::StatusSignal<units::dimensionless::scalar_t> duty_cycle_;
+
+  double stator_current_limit_;
+  double supply_current_limit_;
+};
+}  // namespace wpilib
+}  // namespace frc971
+#endif  // FRC971_WPILIB_TALONFX_MOTOR_H_
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index 8ca7f67..81ed998 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -1,5 +1,12 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
 package(default_visibility = ["//visibility:public"])
 
+static_flatbuffer(
+    name = "constants_fbs",
+    srcs = ["constants.fbs"],
+)
+
 cc_library(
     name = "averager",
     hdrs = [
diff --git a/frc971/zeroing/absolute_and_absolute_encoder_test.cc b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
index b28f637..5dac38b 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder_test.cc
+++ b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
@@ -42,6 +42,7 @@
       0.4 * single_turn_distance_per_revolution;
 
   AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+      {},
       kSampleSize,
       distance_per_revolution,
       measured_absolute_position,
@@ -89,6 +90,7 @@
       0.4 * single_turn_distance_per_revolution;
 
   AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+      {},
       kSampleSize,
       distance_per_revolution,
       measured_absolute_position,
@@ -144,6 +146,7 @@
       0.4 * single_turn_distance_per_revolution;
 
   AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+      {},
       kSampleSize,
       distance_per_revolution,
       measured_absolute_position,
@@ -175,9 +178,16 @@
 // Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
 TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
        TestAbsoluteAndAbsoluteEncoderZeroingWithNaN) {
-  AbsoluteAndAbsoluteEncoderZeroingConstants constants{
-      kSampleSize,        1, 0.3, 1, 0.3, 2.5, 0.1, kMovingBufferSize,
-      kIndexErrorFraction};
+  AbsoluteAndAbsoluteEncoderZeroingConstants constants{{},
+                                                       kSampleSize,
+                                                       1,
+                                                       0.3,
+                                                       1,
+                                                       0.3,
+                                                       2.5,
+                                                       0.1,
+                                                       kMovingBufferSize,
+                                                       kIndexErrorFraction};
 
   AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
 
@@ -215,6 +225,7 @@
       0.4 * single_turn_distance_per_revolution;
 
   AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+      {},
       kSampleSize,
       distance_per_revolution,
       measured_absolute_position,
@@ -276,6 +287,7 @@
       0.4 * single_turn_distance_per_revolution;
 
   AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+      {},
       kSampleSize,
       distance_per_revolution,
       measured_absolute_position,
diff --git a/frc971/zeroing/absolute_encoder_test.cc b/frc971/zeroing/absolute_encoder_test.cc
index fdc1fa0..6e78faa 100644
--- a/frc971/zeroing/absolute_encoder_test.cc
+++ b/frc971/zeroing/absolute_encoder_test.cc
@@ -31,10 +31,14 @@
   const double start_pos = 2.1;
   double measured_absolute_position = 0.3 * index_diff;
 
-  AbsoluteEncoderZeroingConstants constants{
-      kSampleSize,        index_diff, measured_absolute_position,
-      kMiddlePosition,    0.1,        kMovingBufferSize,
-      kIndexErrorFraction};
+  AbsoluteEncoderZeroingConstants constants{{},
+                                            kSampleSize,
+                                            index_diff,
+                                            measured_absolute_position,
+                                            kMiddlePosition,
+                                            0.1,
+                                            kMovingBufferSize,
+                                            kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -61,10 +65,14 @@
   double measured_absolute_position = 0.3 * index_diff;
   const double kMiddlePosition = 2.5;
 
-  AbsoluteEncoderZeroingConstants constants{
-      kSampleSize,        index_diff, measured_absolute_position,
-      kMiddlePosition,    0.1,        kMovingBufferSize,
-      kIndexErrorFraction};
+  AbsoluteEncoderZeroingConstants constants{{},
+                                            kSampleSize,
+                                            index_diff,
+                                            measured_absolute_position,
+                                            kMiddlePosition,
+                                            0.1,
+                                            kMovingBufferSize,
+                                            kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -100,10 +108,14 @@
   double measured_absolute_position = 0.3 * index_diff;
   const double kMiddlePosition = 2.5;
 
-  AbsoluteEncoderZeroingConstants constants{
-      kSampleSize,        index_diff, measured_absolute_position,
-      kMiddlePosition,    0.1,        kMovingBufferSize,
-      kIndexErrorFraction};
+  AbsoluteEncoderZeroingConstants constants{{},
+                                            kSampleSize,
+                                            index_diff,
+                                            measured_absolute_position,
+                                            kMiddlePosition,
+                                            0.1,
+                                            kMovingBufferSize,
+                                            kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -122,8 +134,14 @@
 
 // Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
 TEST_F(AbsoluteEncoderZeroingTest, TestAbsoluteEncoderZeroingWithNaN) {
-  AbsoluteEncoderZeroingConstants constants{
-      kSampleSize, 1, 0.3, 1.0, 0.1, kMovingBufferSize, kIndexErrorFraction};
+  AbsoluteEncoderZeroingConstants constants{{},
+                                            kSampleSize,
+                                            1,
+                                            0.3,
+                                            1.0,
+                                            0.1,
+                                            kMovingBufferSize,
+                                            kIndexErrorFraction};
 
   AbsoluteEncoderZeroingEstimator estimator(constants);
 
diff --git a/frc971/zeroing/constants.fbs b/frc971/zeroing/constants.fbs
new file mode 100644
index 0000000..63e9136
--- /dev/null
+++ b/frc971/zeroing/constants.fbs
@@ -0,0 +1,135 @@
+namespace frc971.zeroing;
+
+table HallEffectZeroingConstants {
+  // The absolute position of the lower edge of the hall effect sensor.
+  lower_hall_position:double (id: 0);
+  // The absolute position of the upper edge of the hall effect sensor.
+  upper_hall_position:double (id: 1);
+  // The difference in scaled units between two hall effect edges.  This is the
+  // number of units/cycle.
+  index_difference:double (id: 2);
+  // Number of cycles we need to see the hall effect high.
+  hall_trigger_zeroing_length:uint64 (id: 3);
+  // Direction the system must be moving in order to zero. True is positive,
+  // False is negative direction.
+  zeroing_move_direction:bool (id: 4);
+}
+
+table PotAndIndexPulseZeroingConstants {
+  // The number of samples in the moving average filter.
+  average_filter_size:uint64 (id: 0);
+  // The difference in scaled units between two index pulses.
+  index_difference:double (id: 1);
+  // The absolute position in scaled units of one of the index pulses.
+  measured_index_position:double (id: 2);
+  // Value between 0 and .5 which determines a fraction of the index_diff
+  // you want to use.
+  allowable_encoder_error:double (id: 3);
+}
+
+table EncoderPlusIndexZeroingConstants {
+  // The amount of index pulses in the joint's range of motion.
+  index_pulse_count:int (id: 0);
+  // The difference in scaled units between two index pulses.
+  index_difference:double (id: 1);
+  // The absolute position in scaled units of one of the index pulses.
+  measured_index_position:double (id: 2);
+  // The index pulse that is known, going from lowest in the range of motion to
+  // highest (Starting at 0).
+  known_index_pulse:int (id: 3);
+  // Value between 0 and 0.5 which determines a fraction of the index_diff
+  // you want to use. If an index pulse deviates by more than this amount from
+  // where we expect to see one then we flag an error.
+  allowable_encoder_error:double (id: 4);
+}
+
+table PotAndAbsoluteEncoderZeroingConstants {
+  // The number of samples in the moving average filter.
+  average_filter_size:uint64 (id: 0);
+  // The distance that the absolute encoder needs to complete a full rotation.
+  one_revolution_distance:double (id: 1);
+  // Measured absolute position of the encoder when at zero.
+  measured_absolute_position:double (id: 2);
+
+  // Treshold for deciding if we are moving. moving_buffer_size samples need to
+  // be within this distance of each other before we use the middle one to zero.
+  zeroing_threshold:double (id: 3);
+  // Buffer size for deciding if we are moving.
+  moving_buffer_size:uint64 (id: 4);
+
+  // Value between 0 and 1 indicating what fraction of one_revolution_distance
+  // it is acceptable for the offset to move.
+  allowable_encoder_error:double (id: 5);
+}
+
+table RelativeEncoderZeroingConstants {}
+
+table ContinuousAbsoluteEncoderZeroingConstants {
+  // The number of samples in the moving average filter.
+  average_filter_size:uint64 (id: 0);
+  // The distance that the absolute encoder needs to complete a full rotation.
+  // It is presumed that this will always be 2 * pi for any subsystem using this
+  // class, unless you have a continuous system that for some reason doesn't
+  // have a logical period of 1 revolution in radians.
+  one_revolution_distance:double (id: 1);
+  // Measured absolute position of the encoder when at zero.
+  measured_absolute_position:double (id: 2);
+
+  // Threshold for deciding if we are moving. moving_buffer_size samples need to
+  // be within this distance of each other before we use the middle one to zero.
+  zeroing_threshold:double (id: 3);
+  // Buffer size for deciding if we are moving.
+  moving_buffer_size:uint64 (id: 4);
+
+  // Value between 0 and 1 indicating what fraction of a revolution
+  // it is acceptable for the offset to move.
+  allowable_encoder_error:double (id: 5);
+}
+
+table AbsoluteEncoderZeroingConstants {
+  // The number of samples in the moving average filter.
+  average_filter_size:uint64 (id: 0);
+  // The distance that the absolute encoder needs to complete a full rotation.
+  one_revolution_distance:double (id: 1);
+  // Measured absolute position of the encoder when at zero.
+  measured_absolute_position:double (id: 2);
+  // Position of the middle of the range of motion in output coordinates.
+  middle_position:double (id: 3);
+
+  // Threshold for deciding if we are moving. moving_buffer_size samples need to
+  // be within this distance of each other before we use the middle one to zero.
+  zeroing_threshold:double (id: 4);
+  // Buffer size for deciding if we are moving.
+  moving_buffer_size:uint64 (id: 5);
+
+  // Value between 0 and 1 indicating what fraction of one_revolution_distance
+  // it is acceptable for the offset to move.
+  allowable_encoder_error:double (id: 6);
+}
+
+table AbsoluteAndAbsoluteEncoderZeroingConstants {
+  // The number of samples in the moving average filter.
+  average_filter_size:uint64 (id: 0);
+  // The distance that the absolute encoder needs to complete a full rotation.
+  one_revolution_distance:double (id: 1);
+  // Measured absolute position of the encoder when at zero.
+  measured_absolute_position:double (id: 2);
+
+  // The distance that the single turn absolute encoder needs to complete a full
+  // rotation.
+  single_turn_one_revolution_distance:double (id: 3);
+  // Measured absolute position of the single turn encoder when at zero.
+  single_turn_measured_absolute_position:double (id: 4);
+  // Position of the middle of the range of motion in output coordinates.
+  single_turn_middle_position:double (id: 5);
+
+  // Threshold for deciding if we are moving. moving_buffer_size samples need to
+  // be within this distance of each other before we use the middle one to zero.
+  zeroing_threshold:double (id: 6);
+  // Buffer size for deciding if we are moving.
+  moving_buffer_size:uint64 (id: 7);
+
+  // Value between 0 and 1 indicating what fraction of one_revolution_distance
+  // it is acceptable for the offset to move.
+  allowable_encoder_error:double (id: 8);
+}
diff --git a/frc971/zeroing/continuous_absolute_encoder_test.cc b/frc971/zeroing/continuous_absolute_encoder_test.cc
index 3869393..75d6de0 100644
--- a/frc971/zeroing/continuous_absolute_encoder_test.cc
+++ b/frc971/zeroing/continuous_absolute_encoder_test.cc
@@ -34,8 +34,8 @@
   double measured_absolute_position = 0.3 * index_diff;
 
   ContinuousAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, index_diff,        measured_absolute_position,
-      0.1,         kMovingBufferSize, kIndexErrorFraction};
+      {},  kSampleSize,       index_diff,         measured_absolute_position,
+      0.1, kMovingBufferSize, kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -65,8 +65,8 @@
   double measured_absolute_position = 0.3 * index_diff;
 
   ContinuousAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, index_diff,        measured_absolute_position,
-      0.1,         kMovingBufferSize, kIndexErrorFraction};
+      {},  kSampleSize,       index_diff,         measured_absolute_position,
+      0.1, kMovingBufferSize, kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -103,8 +103,8 @@
   double measured_absolute_position = 0.3 * index_diff;
 
   ContinuousAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, index_diff,        measured_absolute_position,
-      0.1,         kMovingBufferSize, kIndexErrorFraction};
+      {},  kSampleSize,       index_diff,         measured_absolute_position,
+      0.1, kMovingBufferSize, kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -143,8 +143,8 @@
   double measured_absolute_position = 0.3 * index_diff;
 
   ContinuousAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, index_diff,        measured_absolute_position,
-      0.1,         kMovingBufferSize, kIndexErrorFraction};
+      {},  kSampleSize,       index_diff,         measured_absolute_position,
+      0.1, kMovingBufferSize, kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -165,7 +165,7 @@
 TEST_F(ContinuousAbsoluteEncoderZeroingTest,
        TestContinuousAbsoluteEncoderZeroingWithNaN) {
   ContinuousAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
+      {}, kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
 
   ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
 
diff --git a/frc971/zeroing/pot_and_absolute_encoder_test.cc b/frc971/zeroing/pot_and_absolute_encoder_test.cc
index 5c05cc1..4cd3821 100644
--- a/frc971/zeroing/pot_and_absolute_encoder_test.cc
+++ b/frc971/zeroing/pot_and_absolute_encoder_test.cc
@@ -33,8 +33,8 @@
   double measured_absolute_position = 0.3 * index_diff;
 
   PotAndAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, index_diff,        measured_absolute_position,
-      0.1,         kMovingBufferSize, kIndexErrorFraction};
+      {},  kSampleSize,       index_diff,         measured_absolute_position,
+      0.1, kMovingBufferSize, kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -62,8 +62,8 @@
   double measured_absolute_position = 0.3 * index_diff;
 
   PotAndAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, index_diff,        measured_absolute_position,
-      0.1,         kMovingBufferSize, kIndexErrorFraction};
+      {},  kSampleSize,       index_diff,         measured_absolute_position,
+      0.1, kMovingBufferSize, kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -99,8 +99,8 @@
   double measured_absolute_position = 0.3 * index_diff;
 
   PotAndAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, index_diff,        measured_absolute_position,
-      0.1,         kMovingBufferSize, kIndexErrorFraction};
+      {},  kSampleSize,       index_diff,         measured_absolute_position,
+      0.1, kMovingBufferSize, kIndexErrorFraction};
 
   sim.Initialize(start_pos, index_diff / 3.0, 0.0,
                  constants.measured_absolute_position);
@@ -121,7 +121,7 @@
 TEST_F(PotAndAbsoluteEncoderZeroingTest,
        TestPotAndAbsoluteEncoderZeroingWithNaN) {
   PotAndAbsoluteEncoderZeroingConstants constants{
-      kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
+      {}, kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
 
   PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
 
diff --git a/frc971/zeroing/pot_and_index_test.cc b/frc971/zeroing/pot_and_index_test.cc
index b2b00ce..f2e464f 100644
--- a/frc971/zeroing/pot_and_index_test.cc
+++ b/frc971/zeroing/pot_and_index_test.cc
@@ -27,7 +27,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.6 * index_diff, index_diff / 3.0);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
 
   // The zeroing code is supposed to perform some filtering on the difference
   // between the potentiometer value and the encoder value. We assume that 300
@@ -51,7 +51,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(position, index_diff / 3.0);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
 
   // Make sure that the zeroing code does not consider itself zeroed until we
   // collect a good amount of samples. In this case we're waiting until the
@@ -70,7 +70,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.6, index_diff / 3.0);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
 
   // The zeroing code is supposed to perform some filtering on the difference
   // between the potentiometer value and the encoder value. We assume that 300
@@ -104,7 +104,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.5 * index_diff, index_diff / 3.0);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
 
   // The zeroing code is supposed to perform some filtering on the difference
   // between the potentiometer value and the encoder value. We assume that 300
@@ -138,7 +138,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.5 * index_diff, index_diff / 3.0);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
 
   for (unsigned int i = 0; i < kSampleSize / 2; i++) {
     MoveTo(&sim, &estimator, 3.5 * index_diff);
@@ -158,7 +158,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.1 * index_diff, index_diff / 3.0);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
 
   MoveTo(&sim, &estimator, 3.1 * index_diff);
 
@@ -174,7 +174,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.1 * index_diff, index_diff / 3.0);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
 
   // Make sure to fill up the averaging filter with samples.
   for (unsigned int i = 0; i < kSampleSize; i++) {
@@ -209,7 +209,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.3 * index_diff, index_diff / 3.0, known_index_pos);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
 
   // Make sure to fill up the averaging filter with samples.
   for (unsigned int i = 0; i < kSampleSize; i++) {
@@ -234,7 +234,7 @@
 TEST_F(PotAndIndexZeroingTest, BasicErrorAPITest) {
   const double index_diff = 1.0;
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+      {}, kSampleSize, index_diff, 0.0, kIndexErrorFraction});
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
 
@@ -262,7 +262,7 @@
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
   PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
-      sample_size, index_diff, known_index_pos, kIndexErrorFraction});
+      {}, sample_size, index_diff, known_index_pos, kIndexErrorFraction});
 
   for (size_t i = 0; i < sample_size; i++) {
     MoveTo(&sim, &estimator, 13 * index_diff);
diff --git a/third_party/allwpilib/.clang-format b/third_party/allwpilib/.clang-format
index f69bef0..111b4a8 100644
--- a/third_party/allwpilib/.clang-format
+++ b/third_party/allwpilib/.clang-format
@@ -145,6 +145,8 @@
 LineEnding:      DeriveLF
 MacroBlockBegin: ''
 MacroBlockEnd:   ''
+Macros:
+  - 'HAL_ENUM(name)=enum name'
 MaxEmptyLinesToKeep: 1
 NamespaceIndentation: None
 ObjCBinPackProtocolList: Never
diff --git a/third_party/allwpilib/.clang-tidy b/third_party/allwpilib/.clang-tidy
index 6f1ae3a..e8fe57b 100644
--- a/third_party/allwpilib/.clang-tidy
+++ b/third_party/allwpilib/.clang-tidy
@@ -49,7 +49,6 @@
   google-build-namespaces,
   google-explicit-constructor,
   google-global-names-in-headers,
-  google-readability-avoid-underscore-in-googletest-name,
   google-readability-casting,
   google-runtime-operator,
   misc-definitions-in-headers,
diff --git a/third_party/allwpilib/.gersemirc b/third_party/allwpilib/.gersemirc
new file mode 100644
index 0000000..19ed2c2
--- /dev/null
+++ b/third_party/allwpilib/.gersemirc
@@ -0,0 +1,6 @@
+color: false
+definitions: []
+line_length: 100
+list_expansion: favour-inlining
+quiet: false
+unsafe: false
diff --git a/third_party/allwpilib/.gitattributes b/third_party/allwpilib/.gitattributes
index ae2ea0e..483e5c1 100644
--- a/third_party/allwpilib/.gitattributes
+++ b/third_party/allwpilib/.gitattributes
@@ -3,3 +3,8 @@
 *.json text eol=lf
 *.md text eol=lf
 *.xml text eol=lf
+
+# Generated files
+hal/src/generated/** linguist-generated
+ntcore/src/generated/** linguist-generated
+wpimath/src/generated/** linguist-generated
diff --git a/third_party/allwpilib/.github/workflows/cmake.yml b/third_party/allwpilib/.github/workflows/cmake.yml
index 963dc75..a101b4a 100644
--- a/third_party/allwpilib/.github/workflows/cmake.yml
+++ b/third_party/allwpilib/.github/workflows/cmake.yml
@@ -32,7 +32,7 @@
 
       - name: Install QuickBuffers (Linux)
         if: runner.os == 'Linux'
-        run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.2/protoc-gen-quickbuf_1.3.2_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.2_amd64.deb
+        run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf_1.3.3_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.3_amd64.deb
 
       - name: Install opencv (macOS)
         run: brew install opencv protobuf@3 ninja
@@ -50,7 +50,7 @@
       - name: Install jinja
         run: python -m pip install jinja2
 
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
 
       - name: configure
         run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache ${{ matrix.flags }}
@@ -82,7 +82,7 @@
       - name: Install jinja
         run: python -m pip install jinja2
 
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
 
       - name: Run vcpkg
         uses: lukka/run-vcpkg@v11.1
diff --git a/third_party/allwpilib/.github/workflows/comment-command.yml b/third_party/allwpilib/.github/workflows/comment-command.yml
index 77ebae9..7dd0ad6 100644
--- a/third_party/allwpilib/.github/workflows/comment-command.yml
+++ b/third_party/allwpilib/.github/workflows/comment-command.yml
@@ -19,12 +19,12 @@
               comment_id: context.payload.comment.id,
               content: "rocket",
             });
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
+          fetch-depth: 0
           token: ${{ secrets.COMMENT_COMMAND_PAT_TOKEN }}
       - name: Fetch all history and metadata
         run: |
-          git fetch --prune --unshallow
           git checkout -b pr
           git branch -f main origin/main
       - name: Checkout PR
diff --git a/third_party/allwpilib/.github/workflows/documentation.yml b/third_party/allwpilib/.github/workflows/documentation.yml
index 83d74da..b618ff8 100644
--- a/third_party/allwpilib/.github/workflows/documentation.yml
+++ b/third_party/allwpilib/.github/workflows/documentation.yml
@@ -16,7 +16,7 @@
     if: github.repository_owner == 'wpilibsuite' && (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
     concurrency: ci-docs-publish
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
           persist-credentials: false
diff --git a/third_party/allwpilib/.github/workflows/fix_compile_commands.py b/third_party/allwpilib/.github/workflows/fix_compile_commands.py
new file mode 100755
index 0000000..8a52830
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/fix_compile_commands.py
@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+import argparse
+import json
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Fix compile_commands.json generated by Gradle"
+    )
+    parser.add_argument("filename", help="compile_commands.json location")
+    cmd_args = parser.parse_args()
+
+    # Read JSON
+    with open(cmd_args.filename) as f:
+        data = json.load(f)
+
+    for obj in data:
+        out_args = []
+
+        # Filter out -isystem flags that cause false positives
+        iter_args = iter(obj["arguments"])
+        for arg in iter_args:
+            if arg == "-isystem":
+                next_arg = next(iter_args)
+
+                # /usr/lib/gcc/x86_64-pc-linux-gnu/13.2.1/include/xmmintrin.h:54:1:
+                # error: conflicting types for '_mm_prefetch' [clang-diagnostic-error]
+                if not next_arg.startswith("/usr/lib/gcc/"):
+                    out_args += ["-isystem", next_arg]
+            else:
+                out_args.append(arg)
+
+        obj["arguments"] = out_args
+
+    # Write JSON
+    with open(cmd_args.filename, "w") as f:
+        json.dump(data, f, indent=2)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml b/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml
index d678bc4..9c7b7a2 100644
--- a/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml
+++ b/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml
@@ -10,5 +10,5 @@
     name: "Validation"
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - uses: gradle/wrapper-validation-action@v1
diff --git a/third_party/allwpilib/.github/workflows/gradle.yml b/third_party/allwpilib/.github/workflows/gradle.yml
index 3af54fb..7ed26ed 100644
--- a/third_party/allwpilib/.github/workflows/gradle.yml
+++ b/third_party/allwpilib/.github/workflows/gradle.yml
@@ -37,7 +37,7 @@
           large-packages: false
           docker-images: false
           swap-storage: false
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
       - name: Set release environment variable
@@ -54,14 +54,14 @@
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
       - name: Check free disk space
         run: df .
-      - uses: actions/upload-artifact@v3
+      - uses: actions/upload-artifact@v4
         with:
           name: ${{ matrix.artifact-name }}
           path: build/allOutputs
 
   build-host:
     env:
-      MACOSX_DEPLOYMENT_TARGET: 11
+      MACOSX_DEPLOYMENT_TARGET: 12
     strategy:
       fail-fast: false
       matrix:
@@ -109,7 +109,7 @@
     name: "Build - ${{ matrix.artifact-name }}"
     runs-on: ${{ matrix.os }}
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
       - uses: actions/setup-java@v3
@@ -159,7 +159,7 @@
       - name: Check disk free space (Windows)
         run: wmic logicaldisk get caption, freespace
         if: matrix.os == 'windows-2022'
-      - uses: actions/upload-artifact@v3
+      - uses: actions/upload-artifact@v4
         with:
           name: ${{ matrix.artifact-name }}
           path: ${{ matrix.build-dir }}/${{ matrix.outputs }}
@@ -168,7 +168,7 @@
     name: "Build - Documentation"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
       - uses: actions/setup-java@v3
@@ -183,7 +183,7 @@
         env:
           ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
-      - uses: actions/upload-artifact@v3
+      - uses: actions/upload-artifact@v4
         with:
           name: Documentation
           path: docs/build/outputs
@@ -206,13 +206,13 @@
           large-packages: false
           docker-images: false
           swap-storage: false
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         if: |
           github.repository_owner == 'wpilibsuite' &&
           (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
         with:
           repository: wpilibsuite/build-tools
-      - uses: actions/download-artifact@v3
+      - uses: actions/download-artifact@v4
         if: |
           github.repository_owner == 'wpilibsuite' &&
           (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
@@ -255,7 +255,7 @@
           RUN_AZURE_ARTIFACTORY_RELEASE: "TRUE"
           ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
-      - uses: actions/upload-artifact@v3
+      - uses: actions/upload-artifact@v4
         if: |
           github.repository_owner == 'wpilibsuite' &&
           (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
diff --git a/third_party/allwpilib/.github/workflows/lint-format.yml b/third_party/allwpilib/.github/workflows/lint-format.yml
index fe20537..53de173 100644
--- a/third_party/allwpilib/.github/workflows/lint-format.yml
+++ b/third_party/allwpilib/.github/workflows/lint-format.yml
@@ -15,11 +15,11 @@
     name: "wpiformat"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
       - name: Fetch all history and metadata
         run: |
-          git config --global --add safe.directory /__w/allwpilib/allwpilib
-          git fetch --prune --unshallow
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.8
@@ -27,7 +27,7 @@
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat
+        run: pip3 install wpiformat==2023.36
       - name: Run
         run: wpiformat
       - name: Check output
@@ -35,7 +35,7 @@
       - name: Generate diff
         run: git diff HEAD > wpiformat-fixes.patch
         if: ${{ failure() }}
-      - uses: actions/upload-artifact@v3
+      - uses: actions/upload-artifact@v4
         with:
           name: wpiformat fixes
           path: wpiformat-fixes.patch
@@ -53,11 +53,12 @@
     runs-on: ubuntu-22.04
     container: wpilib/roborio-cross-ubuntu:2023-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
       - name: Fetch all history and metadata
         run: |
           git config --global --add safe.directory /__w/allwpilib/allwpilib
-          git fetch --prune --unshallow
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.8
@@ -67,21 +68,27 @@
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Create compile_commands.json
-        run: ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio
+        run: |
+          ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio
+          ./.github/workflows/fix_compile_commands.py build/TargetedCompileCommands/linuxx86-64release/compile_commands.json
+          ./.github/workflows/fix_compile_commands.py build/TargetedCompileCommands/linuxx86-64debug/compile_commands.json
       - name: List changed files
         run: wpiformat -list-changed-files
-      - name: Run clang-tidy
-        run: wpiformat -no-format -tidy-changed -compile-commands=build/compile_commands/linuxx86-64 -vv
+      - name: Run clang-tidy release
+        run: wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64release -vv
+      - name: Run clang-tidy debug
+        run: wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64debug -vv
   javaformat:
     name: "Java format"
     runs-on: ubuntu-22.04
     container: wpilib/ubuntu-base:22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
       - name: Fetch all history and metadata
         run: |
           git config --global --add safe.directory /__w/allwpilib/allwpilib
-          git fetch --prune --unshallow
           git checkout -b pr
           git branch -f main origin/main
       - name: Run Java format
@@ -95,7 +102,7 @@
     name: "Documentation"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
       - uses: actions/setup-java@v3
diff --git a/third_party/allwpilib/.github/workflows/pregenerate.yml b/third_party/allwpilib/.github/workflows/pregenerate.yml
new file mode 100644
index 0000000..fb85149
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/pregenerate.yml
@@ -0,0 +1,46 @@
+name: Check Pregenerated Files
+
+on:
+  pull_request:
+  push:
+    branches-ignore:
+      - main
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  update:
+    name: "Update"
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+      - name: Set up Python 3.9
+        uses: actions/setup-python@v4
+        with:
+          python-version: 3.9
+      - name: Install jinja
+        run: python -m pip install jinja2
+      - name: Install protobuf dependencies
+        run: sudo apt-get update && sudo apt-get install -y protobuf-compiler && wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe && chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe
+      - name: Run hal
+        run: ./hal/generate_usage_reporting.py
+      - name: Run ntcore
+        run: ./ntcore/generate_topics.py
+      - name: Run wpimath
+        run: ./wpimath/generate_numbers.py && ./wpimath/generate_quickbuf.py protoc protoc-gen-quickbuf-1.3.3-linux-x86_64.exe
+      - name: Add untracked files to index so they count as changes
+        run: git add -A
+      - name: Check output
+        run: git --no-pager diff --exit-code HEAD
+      - name: Generate diff
+        run: git diff HEAD > pregenerated-files-fixes.patch
+        if: ${{ failure() }}
+      - uses: actions/upload-artifact@v4
+        with:
+          name: pregenerated-files-fixes
+          path: pregenerated-files-fixes.patch
+        if: ${{ failure() }}
diff --git a/third_party/allwpilib/.github/workflows/sanitizers.yml b/third_party/allwpilib/.github/workflows/sanitizers.yml
index c0b6ac3..cf4d108 100644
--- a/third_party/allwpilib/.github/workflows/sanitizers.yml
+++ b/third_party/allwpilib/.github/workflows/sanitizers.yml
@@ -33,7 +33,7 @@
 
       - name: Install QuickBuffers
         if: runner.os == 'Linux'
-        run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.2/protoc-gen-quickbuf_1.3.2_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.2_amd64.deb
+        run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf_1.3.3_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.3_amd64.deb
 
       - name: Run sccache-cache
         uses: mozilla-actions/sccache-action@v0.0.3
@@ -41,7 +41,7 @@
       - name: Install jinja
         run: python -m pip install jinja2
 
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
 
       - name: configure
         run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} ..
diff --git a/third_party/allwpilib/.github/workflows/upstream-utils.yml b/third_party/allwpilib/.github/workflows/upstream-utils.yml
index 0e4a46f..afaffd5 100644
--- a/third_party/allwpilib/.github/workflows/upstream-utils.yml
+++ b/third_party/allwpilib/.github/workflows/upstream-utils.yml
@@ -15,10 +15,11 @@
     name: "Update"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
       - name: Fetch all history and metadata
         run: |
-          git fetch --prune --unshallow
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.9
diff --git a/third_party/allwpilib/.styleguide b/third_party/allwpilib/.styleguide
index 1ba4e1d..f67c100 100644
--- a/third_party/allwpilib/.styleguide
+++ b/third_party/allwpilib/.styleguide
@@ -10,6 +10,7 @@
 }
 
 modifiableFileExclude {
+  cmake/toolchains/
   \.patch$
   gradlew
 }
diff --git a/third_party/allwpilib/CMakeLists.txt b/third_party/allwpilib/CMakeLists.txt
index f994bf1..79df359 100644
--- a/third_party/allwpilib/CMakeLists.txt
+++ b/third_party/allwpilib/CMakeLists.txt
@@ -1,12 +1,15 @@
 # Disable in-source builds to prevent source tree corruption.
 if(" ${CMAKE_CURRENT_SOURCE_DIR}" STREQUAL " ${CMAKE_CURRENT_BINARY_DIR}")
-  message(FATAL_ERROR "
+    message(
+        FATAL_ERROR
+        "
 FATAL: In-source builds are not allowed.
        You should create a separate directory for build files.
-")
+"
+    )
 endif()
 
-if ("${CMAKE_HOST_SYSTEM_NAME}" STREQUAL "Windows")
+if("${CMAKE_HOST_SYSTEM_NAME}" STREQUAL "Windows")
     set(CMAKE_SYSTEM_VERSION 10.0.18362.0 CACHE STRING INTERNAL FORCE)
     set(CMAKE_VS_WINDOWS_TARGET_PLATFORM_VERSION 10.0.18362.0 CACHE STRING INTERNAL FORCE)
 endif()
@@ -16,7 +19,7 @@
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules")
 
 # Make timestamps of extracted files from FetchContent the time of extraction
-if (POLICY CMP0135)
+if(POLICY CMP0135)
     cmake_policy(SET CMP0135 NEW)
 endif()
 
@@ -24,7 +27,7 @@
 
 set(WPILIB_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR})
 
-INCLUDE(CPack)
+include(CPack)
 
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
 set_property(GLOBAL PROPERTY USE_FOLDERS ON)
@@ -35,31 +38,32 @@
 set(CMAKE_JAVA_TARGET_OUTPUT_DIR ${WPILIB_BINARY_DIR}/jar)
 
 # use, i.e. don't skip the full RPATH for the build tree
-SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+set(CMAKE_SKIP_BUILD_RPATH FALSE)
 
 # when building, don't use the install RPATH already
 # (but later on when installing)
-SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
 
-SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
+set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
 
 # add the automatically determined parts of the RPATH
 # which point to directories outside the build tree to the install RPATH
-SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # the RPATH to be used when installing, but only if it's not a system directory
-LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
-IF("${isSystemDir}" STREQUAL "-1")
-   SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
-ENDIF("${isSystemDir}" STREQUAL "-1")
+list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
+if("${isSystemDir}" STREQUAL "-1")
+    set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
+endif()
 
 # Options for building certain parts of the repo. Everything is built by default.
 option(BUILD_SHARED_LIBS "Build with shared libs (needed for JNI)" ON)
 option(WITH_JAVA "Include Java and JNI in the build" ON)
-option(WITH_JAVA_SOURCE "Build Java source jars" ON)
+option(WITH_JAVA_SOURCE "Build Java source jars" ${WITH_JAVA})
 option(WITH_CSCORE "Build cscore (needs OpenCV)" ON)
 option(WITH_NTCORE "Build ntcore" ON)
 option(WITH_WPIMATH "Build wpimath" ON)
+option(WITH_WPIUNITS "Build wpiunits" ON)
 option(WITH_WPILIB "Build hal, wpilibc/j, and myRobot (needs OpenCV)" ON)
 option(WITH_EXAMPLES "Build examples" OFF)
 option(WITH_TESTS "Build unit tests (requires internet connection)" ON)
@@ -75,135 +79,154 @@
 option(USE_SYSTEM_LIBUV "Use system libuv" OFF)
 option(USE_SYSTEM_EIGEN "Use system eigen" OFF)
 
-# Options for installation.
-option(WITH_FLAT_INSTALL "Use a flat install directory" OFF)
-
 # Options for location of OpenCV Java.
 set(OPENCV_JAVA_INSTALL_DIR "" CACHE PATH "Location to search for the OpenCV jar file")
 
+# Options for compilation flags.
+option(NO_WERROR "Disable -Werror flag during compilation" OFF)
+
 # Set default build type to release with debug info (i.e. release mode optimizations
 # are performed, but debug info still exists).
-if (NOT CMAKE_BUILD_TYPE)
-    set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "" FORCE)
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "" FORCE)
 endif()
 
-if (WITH_JAVA AND NOT BUILD_SHARED_LIBS)
-    message(FATAL_ERROR "
+if(WITH_JAVA AND NOT BUILD_SHARED_LIBS)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build static libs with Java enabled.
        Static libs requires both BUILD_SHARED_LIBS=OFF and
        WITH_JAVA=OFF
-")
+"
+    )
 endif()
 
-if (WITH_SIMULATION_MODULES AND NOT BUILD_SHARED_LIBS)
-    message(FATAL_ERROR "
+if(WITH_SIMULATION_MODULES AND NOT BUILD_SHARED_LIBS)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build static libs with simulation modules enabled.
        Static libs requires both BUILD_SHARED_LIBS=OFF and
        WITH_SIMULATION_MODULES=OFF
-")
+"
+    )
 endif()
 
-if (NOT WITH_JAVA OR NOT WITH_CSCORE)
+if(NOT WITH_JAVA OR NOT WITH_CSCORE)
     if(NOT "${OPENCV_JAVA_INSTALL_DIR}" STREQUAL "")
-        message(WARNING "
+        message(
+            WARNING
+            "
 WARNING: OpenCV Java dir set but java is not enabled!
 It will be ignored.
-")
+"
+        )
     endif()
 endif()
 
-if (NOT WITH_WPILIB AND WITH_SIMULATION_MODULES)
-    message(FATAL_ERROR "
+if(NOT WITH_WPILIB AND WITH_SIMULATION_MODULES)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build simulation modules with wpilib disabled.
        Enable wpilib by setting WITH_WPILIB=ON
-")
+"
+    )
 endif()
 
-if (NOT WITH_NTCORE AND WITH_CSCORE)
-    message(FATAL_ERROR "
+if(NOT WITH_NTCORE AND WITH_CSCORE)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build cameraserver without ntcore.
        Enable ntcore by setting WITH_NTCORE=ON
-")
+"
+    )
 endif()
 
-if (NOT WITH_NTCORE AND WITH_GUI)
-    message(FATAL_ERROR "
+if(NOT WITH_NTCORE AND WITH_GUI)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build GUI modules without ntcore.
        Enable ntcore by setting WITH_NTCORE=ON
-")
+"
+    )
 endif()
 
-if (NOT WITH_NTCORE AND WITH_SIMULATION_MODULES)
-    message(FATAL_ERROR "
+if(NOT WITH_NTCORE AND WITH_SIMULATION_MODULES)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build simulation modules without ntcore.
        Enable ntcore by setting WITH_NTCORE=ON
-")
+"
+    )
 endif()
 
-if (NOT WITH_NTCORE AND WITH_WPILIB)
-    message(FATAL_ERROR "
+if(NOT WITH_NTCORE AND WITH_WPILIB)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build wpilib without ntcore.
        Enable ntcore by setting WITH_NTCORE=ON
-")
+"
+    )
 endif()
 
-if (NOT WITH_WPIMATH AND WITH_WPILIB)
-    message(FATAL_ERROR "
+if(NOT WITH_WPIMATH AND WITH_WPILIB)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build wpilib without wpimath.
        Enable wpimath by setting WITH_WPIMATH=ON
-")
+"
+    )
 endif()
 
-set( wpilib_dest "")
-set( include_dest include )
-set( java_lib_dest java )
-set( jni_lib_dest jni )
-
-if (WITH_FLAT_INSTALL)
-    set (wpilib_config_dir ${wpilib_dest})
-else()
-    set (wpilib_config_dir share/wpilib)
+if(NOT WITH_WPIUNITS AND WITH_WPIMATH AND WITH_JAVA)
+    message(
+        FATAL_ERROR
+        "
+FATAL: Cannot build Java wpimath without wpiunits.
+       Enable wpiunits by setting WITH_WPIUNITS=ON or disable the Java build by setting WITH_JAVA=OFF
+"
+    )
 endif()
 
-if (USE_SYSTEM_LIBUV)
-set (LIBUV_SYSTEM_REPLACE "
+set(include_dest include)
+set(java_lib_dest java)
+set(jni_lib_dest jni)
+
+if(USE_SYSTEM_LIBUV)
+    set(LIBUV_SYSTEM_REPLACE
+        "
 find_dependency(libuv CONFIG)
-")
+"
+    )
 endif()
 
-if (USE_SYSTEM_EIGEN)
-set (EIGEN_SYSTEM_REPLACE "find_package(Eigen3 CONFIG)")
+if(USE_SYSTEM_EIGEN)
+    set(EIGEN_SYSTEM_REPLACE "find_package(Eigen3 CONFIG)")
 endif()
 
 find_package(LIBSSH 0.7.1)
 
 find_package(Protobuf REQUIRED)
-find_program(Quickbuf_EXECUTABLE
-    NAMES protoc-gen-quickbuf
-    DOC "The Quickbuf protoc plugin"
-)
 
-if (WITH_FLAT_INSTALL)
-set(WPIUTIL_DEP_REPLACE "include($\{SELF_DIR\}/wpiutil-config.cmake)")
-set(WPINET_DEP_REPLACE "include($\{SELF_DIR\}/wpinet-config.cmake)")
-set(NTCORE_DEP_REPLACE "include($\{SELF_DIR\}/ntcore-config.cmake)")
-set(CSCORE_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cscore-config.cmake)")
-set(CAMERASERVER_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cameraserver-config.cmake)")
-set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)")
-set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)")
-set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)")
-set(WPILIBNEWCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibNewcommands-config.cmake)")
-else()
-set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
-set(WPINET_DEP_REPLACE "find_dependency(wpinet)")
-set(NTCORE_DEP_REPLACE "find_dependency(ntcore)")
-set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)")
+set(APRILTAG_DEP_REPLACE "find_dependency(apriltag)")
 set(CAMERASERVER_DEP_REPLACE_IMPL "find_dependency(cameraserver)")
+set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)")
 set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)")
-set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
+set(NTCORE_DEP_REPLACE "find_dependency(ntcore)")
 set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)")
+set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)")
 set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)")
-endif()
+set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
+set(WPINET_DEP_REPLACE "find_dependency(wpinet)")
+set(WPIUNITS_DEP_REPLACE "find_dependency(wpiunits)")
+set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
 
 set(FILENAME_DEP_REPLACE "get_filename_component(SELF_DIR \"$\{CMAKE_CURRENT_LIST_FILE\}\" PATH)")
 set(SELF_DIR "$\{SELF_DIR\}")
@@ -221,7 +244,15 @@
         list(APPEND CMAKE_CONFIGURATION_TYPES Ubsan)
     endif()
 else()
-    set(allowedBuildTypes Asan Tsan Ubsan Debug Release RelWithDebInfo MinSizeRel)
+    set(allowedBuildTypes
+        Asan
+        Tsan
+        Ubsan
+        Debug
+        Release
+        RelWithDebInfo
+        MinSizeRel
+    )
     set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "${allowedBuildTypes}")
 
     if(CMAKE_BUILD_TYPE AND NOT CMAKE_BUILD_TYPE IN_LIST allowedBuildTypes)
@@ -230,54 +261,90 @@
 endif()
 
 set(CMAKE_C_FLAGS_ASAN
-    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C compiler for Asan build type or configuration." FORCE)
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C compiler for Asan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_CXX_FLAGS_ASAN
-    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C++ compiler for Asan build type or configuration." FORCE)
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C++ compiler for Asan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_EXE_LINKER_FLAGS_ASAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address" CACHE STRING
-    "Linker flags to be used to create executables for Asan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address"
+    CACHE STRING
+    "Linker flags to be used to create executables for Asan build type."
+    FORCE
+)
 
 set(CMAKE_SHARED_LINKER_FLAGS_ASAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address" CACHE STRING
-    "Linker lags to be used to create shared libraries for Asan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address"
+    CACHE STRING
+    "Linker lags to be used to create shared libraries for Asan build type."
+    FORCE
+)
 
 set(CMAKE_C_FLAGS_TSAN
-    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C compiler for Tsan build type or configuration." FORCE)
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C compiler for Tsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_CXX_FLAGS_TSAN
-    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C++ compiler for Tsan build type or configuration." FORCE)
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C++ compiler for Tsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_EXE_LINKER_FLAGS_TSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread" CACHE STRING
-    "Linker flags to be used to create executables for Tsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread"
+    CACHE STRING
+    "Linker flags to be used to create executables for Tsan build type."
+    FORCE
+)
 
 set(CMAKE_SHARED_LINKER_FLAGS_TSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread" CACHE STRING
-    "Linker lags to be used to create shared libraries for Tsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread"
+    CACHE STRING
+    "Linker lags to be used to create shared libraries for Tsan build type."
+    FORCE
+)
 
 set(CMAKE_C_FLAGS_UBSAN
-    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C compiler for Ubsan build type or configuration." FORCE)
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C compiler for Ubsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_CXX_FLAGS_UBSAN
-    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C++ compiler for Ubsan build type or configuration." FORCE)
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C++ compiler for Ubsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_EXE_LINKER_FLAGS_UBSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all" CACHE STRING
-    "Linker flags to be used to create executables for Ubsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all"
+    CACHE STRING
+    "Linker flags to be used to create executables for Ubsan build type."
+    FORCE
+)
 
 set(CMAKE_SHARED_LINKER_FLAGS_UBSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined" CACHE STRING
-    "Linker lags to be used to create shared libraries for Ubsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined"
+    CACHE STRING
+    "Linker lags to be used to create shared libraries for Ubsan build type."
+    FORCE
+)
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     enable_testing()
     add_subdirectory(googletest)
     include(GoogleTest)
@@ -285,41 +352,49 @@
 
 add_subdirectory(wpiutil)
 
-if (WITH_NTCORE)
+if(WITH_NTCORE)
     add_subdirectory(wpinet)
     add_subdirectory(ntcore)
 endif()
 
-if (WITH_WPIMATH)
+if(WITH_WPIMATH)
+    if(WITH_JAVA)
+        add_subdirectory(wpiunits)
+    endif()
     add_subdirectory(wpimath)
 endif()
 
-if (WITH_GUI)
+if(WITH_WPIUNITS AND NOT WITH_WPIMATH)
+    # In case of building wpiunits standalone
+    add_subdirectory(wpiunits)
+endif()
+
+if(WITH_GUI)
     add_subdirectory(fieldImages)
     add_subdirectory(imgui)
     add_subdirectory(wpigui)
     add_subdirectory(glass)
     add_subdirectory(outlineviewer)
     add_subdirectory(sysid)
-    if (LIBSSH_FOUND)
+    if(LIBSSH_FOUND)
         add_subdirectory(roborioteamnumbersetter)
         add_subdirectory(datalogtool)
     endif()
 endif()
 
-if (WITH_WPILIB OR WITH_SIMULATION_MODULES)
+if(WITH_WPILIB OR WITH_SIMULATION_MODULES)
     set(HAL_DEP_REPLACE ${HAL_DEP_REPLACE_IMPL})
     add_subdirectory(hal)
 endif()
 
-if (WITH_CSCORE)
+if(WITH_CSCORE)
     set(CSCORE_DEP_REPLACE ${CSCORE_DEP_REPLACE_IMPL})
     set(CAMERASERVER_DEP_REPLACE ${CAMERASERVER_DEP_REPLACE_IMPL})
     add_subdirectory(cscore)
     add_subdirectory(cameraserver)
 endif()
 
-if (WITH_WPILIB)
+if(WITH_WPILIB)
     set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
     add_subdirectory(apriltag)
     add_subdirectory(wpilibj)
@@ -327,15 +402,15 @@
     add_subdirectory(wpilibNewCommands)
     add_subdirectory(romiVendordep)
     add_subdirectory(xrpVendordep)
-    if (WITH_EXAMPLES)
+    if(WITH_EXAMPLES)
         add_subdirectory(wpilibcExamples)
     endif()
     add_subdirectory(myRobot)
 endif()
 
-if (WITH_SIMULATION_MODULES AND NOT WITH_EXTERNAL_HAL)
+if(WITH_SIMULATION_MODULES AND NOT WITH_EXTERNAL_HAL)
     add_subdirectory(simulation)
 endif()
 
-configure_file(wpilib-config.cmake.in ${WPILIB_BINARY_DIR}/wpilib-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/wpilib-config.cmake DESTINATION ${wpilib_config_dir})
+configure_file(wpilib-config.cmake.in ${WPILIB_BINARY_DIR}/wpilib-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/wpilib-config.cmake DESTINATION share/wpilib)
diff --git a/third_party/allwpilib/LICENSE.md b/third_party/allwpilib/LICENSE.md
index 43b62ec..645e542 100644
--- a/third_party/allwpilib/LICENSE.md
+++ b/third_party/allwpilib/LICENSE.md
@@ -1,4 +1,4 @@
-Copyright (c) 2009-2023 FIRST and other WPILib contributors
+Copyright (c) 2009-2024 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
diff --git a/third_party/allwpilib/MAINTAINERS.md b/third_party/allwpilib/MAINTAINERS.md
index d4c3490..483130c 100644
--- a/third_party/allwpilib/MAINTAINERS.md
+++ b/third_party/allwpilib/MAINTAINERS.md
@@ -1,5 +1,5 @@
 ## Publishing Third Party Dependencies
-Currently the 3rd party deps are imgui, opencv, and google test
+Currently the 3rd party deps are imgui, opencv, google test, libssh, and apriltaglib
 
 For publishing these dependencies, the version needs to be manually updated in the publish.gradle file of their respective repository.
 Then, in the azure build for the dependency you want to build for, manually start a pipeline build (As of current, this is the `Run Pipeline` button).
@@ -24,4 +24,4 @@
 Before publishing, make sure to update the version in build.gradle. Publishing must happen locally, using the command `./gradlew publishPlugin`. This does require your API key for publishing to be set.
 
 ## Building the installer
-Update the GradleRIO version in gradle.properties, and in the scripts folder in vscode, update the vscode extension. Then push, it will build the installer on azure.
+Update the GradleRIO version in gradle.properties, and in the scripts folder in vscode, update the vscode extension. To publish a release build, upload a new tag, and a release will automatically be built and published to artifactory and cloudflare.
diff --git a/third_party/allwpilib/MavenArtifacts.md b/third_party/allwpilib/MavenArtifacts.md
index 655f9c5..9870028 100644
--- a/third_party/allwpilib/MavenArtifacts.md
+++ b/third_party/allwpilib/MavenArtifacts.md
@@ -145,6 +145,11 @@
   * wpinet
   * wpiutil
 
+* wpiunits
+
+* apriltag
+  * wpiutil
+  * wpimath
 
 ### Third Party Artifacts
 
@@ -152,6 +157,7 @@
 
 All artifacts are based at `edu.wpi.first.thirdparty.frcYEAR` in the repository.
 
+* apriltaglib
 * googletest
 * imgui
 * opencv
diff --git a/third_party/allwpilib/README-CMAKE.md b/third_party/allwpilib/README-CMAKE.md
index 776f808..0d6f0b9 100644
--- a/third_party/allwpilib/README-CMAKE.md
+++ b/third_party/allwpilib/README-CMAKE.md
@@ -12,6 +12,7 @@
 * halsim
 * wpigui
 * wpimath
+* wpiunits
 * wpilibNewCommands
 
 By default, all libraries except for the HAL and WPILib get built with a default CMake setup. The libraries are built as shared libraries, and include the JNI libraries as well as building the Java JARs.
@@ -44,6 +45,8 @@
   * This option will cause ntcore to be built. Turning this off will implicitly disable wpinet and wpilib as well, irrespective of their specific options.
 * `WITH_WPIMATH` (ON Default)
   * This option will build the wpimath library. This option must be on to build wpilib.
+* `WITH_WPIUNITS` (ON Default)
+  * This option will build the wpiunits library. This option must be on to build the Java wpimath library and requires `WITH_JAVA` to also be on.
 * `WITH_WPILIB` (ON Default)
   * This option will build the hal and wpilibc/j during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO.
 * `WITH_EXAMPLES` (ON Default)
@@ -60,6 +63,8 @@
   * TODO
 * `OPENCV_JAVA_INSTALL_DIR`
   * Set this option to the location of the archive of the OpenCV Java bindings (it should be called opencv-xxx.jar, with the x'es being version numbers). NOTE: set it to the LOCATION of the file, not the file itself!
+* `NO_WERROR` (OFF Default)
+  * This option will disable the `-Werror` compilation flag for non-MSVC builds.
 
 ## Build Setup
 
diff --git a/third_party/allwpilib/apriltag/CMakeLists.txt b/third_party/allwpilib/apriltag/CMakeLists.txt
index 9d97363..9ec2a69 100644
--- a/third_party/allwpilib/apriltag/CMakeLists.txt
+++ b/third_party/allwpilib/apriltag/CMakeLists.txt
@@ -4,121 +4,146 @@
 include(GenResources)
 include(FetchContent)
 
-FetchContent_Declare(
+fetchcontent_declare(
     apriltaglib
-    GIT_REPOSITORY  https://github.com/wpilibsuite/apriltag.git
-    GIT_TAG         64be6ab26abf5e995321997fd0752c609a7e30f4
+    GIT_REPOSITORY https://github.com/wpilibsuite/apriltag.git
+    GIT_TAG 64be6ab26abf5e995321997fd0752c609a7e30f4
 )
 
 # Don't use apriltag's CMakeLists.txt due to conflicting naming and JNI
-FetchContent_GetProperties(apriltaglib)
+fetchcontent_getproperties(apriltaglib)
 if(NOT apriltaglib_POPULATED)
-    FetchContent_Populate(apriltaglib)
+    fetchcontent_populate(apriltaglib)
 endif()
 
 aux_source_directory(${apriltaglib_SOURCE_DIR}/common APRILTAGLIB_COMMON_SRC)
 file(GLOB TAG_FILES ${apriltaglib_SOURCE_DIR}/tag*.c)
-set(APRILTAGLIB_SRCS ${apriltaglib_SOURCE_DIR}/apriltag.c ${apriltaglib_SOURCE_DIR}/apriltag_pose.c ${apriltaglib_SOURCE_DIR}/apriltag_quad_thresh.c)
+set(APRILTAGLIB_SRCS
+    ${apriltaglib_SOURCE_DIR}/apriltag.c
+    ${apriltaglib_SOURCE_DIR}/apriltag_pose.c
+    ${apriltaglib_SOURCE_DIR}/apriltag_quad_thresh.c
+)
 
 file(GLOB apriltag_jni_src src/main/native/cpp/jni/AprilTagJNI.cpp)
 
-if (WITH_JAVA)
-  find_package(Java REQUIRED)
-  find_package(JNI REQUIRED)
-  include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    find_package(JNI REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  set(CMAKE_JNI_TARGET true)
+    set(CMAKE_JNI_TARGET true)
 
-  file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
-  file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
-  find_file(OPENCV_JAR_FILE
-    NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
-    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/share/java
-    NO_DEFAULT_PATH)
+    file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+    file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+    find_file(
+        OPENCV_JAR_FILE
+        NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
+        PATHS
+            ${OPENCV_JAVA_INSTALL_DIR}
+            ${OpenCV_INSTALL_PATH}/bin
+            ${OpenCV_INSTALL_PATH}/share/java
+        NO_DEFAULT_PATH
+    )
 
-  set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${EJML_JARS} ${JACKSON_JARS})
+    set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${EJML_JARS} ${JACKSON_JARS})
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-  file(GLOB_RECURSE JAVA_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} src/main/native/resources/*.json)
-  add_jar(apriltag_jar
-    SOURCES ${JAVA_SOURCES}
-    RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES}
-    INCLUDE_JARS wpimath_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE}
-    OUTPUT_NAME apriltag
-    GENERATE_NATIVE_HEADERS apriltag_jni_headers)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+    file(
+        GLOB_RECURSE JAVA_RESOURCES
+        RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
+        src/main/native/resources/*.json
+    )
+    add_jar(
+        apriltag_jar
+        SOURCES ${JAVA_SOURCES}
+        RESOURCES
+        NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES}
+        INCLUDE_JARS wpimath_jar wpiunits_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE}
+        OUTPUT_NAME apriltag
+        GENERATE_NATIVE_HEADERS apriltag_jni_headers
+    )
 
-  get_property(APRILTAG_JAR_FILE TARGET apriltag_jar PROPERTY JAR_FILE)
-  install(FILES ${APRILTAG_JAR_FILE} DESTINATION "${java_lib_dest}")
+    install_jar(apriltag_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS apriltag_jar FILE apriltag_jar.cmake DESTINATION share/apriltag)
 
-  set_property(TARGET apriltag_jar PROPERTY FOLDER "java")
+    add_library(apriltagjni ${apriltag_jni_src})
+    wpilib_target_warnings(apriltagjni)
+    target_link_libraries(apriltagjni PUBLIC apriltag)
 
-  add_library(apriltagjni ${apriltag_jni_src})
-  wpilib_target_warnings(apriltagjni)
-  target_link_libraries(apriltagjni PUBLIC apriltag)
+    set_property(TARGET apriltagjni PROPERTY FOLDER "libraries")
 
-  set_property(TARGET apriltagjni PROPERTY FOLDER "libraries")
+    target_link_libraries(apriltagjni PRIVATE apriltag_jni_headers)
+    add_dependencies(apriltagjni apriltag_jar)
 
-  target_link_libraries(apriltagjni PRIVATE apriltag_jni_headers)
-  add_dependencies(apriltagjni apriltag_jar)
-
-  install(TARGETS apriltagjni EXPORT apriltagjni)
-
+    install(TARGETS apriltagjni EXPORT apriltagjni)
 endif()
 
-if (WITH_JAVA_SOURCE)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  file(GLOB APRILTAG_SOURCES src/main/java/edu/wpi/first/apriltag/*.java)
-  add_jar(apriltag_src_jar
-  RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${APRILTAG_SOURCES}
-  NAMESPACE "edu/wpi/first/apriltag/jni" src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
-  OUTPUT_NAME apriltag-sources)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB APRILTAG_SOURCES src/main/java/edu/wpi/first/apriltag/*.java)
+    add_jar(
+        apriltag_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/apriltag" ${APRILTAG_SOURCES}
+        NAMESPACE
+            "edu/wpi/first/apriltag/jni"
+            src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
+        OUTPUT_NAME apriltag-sources
+    )
 
-  get_property(APRILTAG_SRC_JAR_FILE TARGET apriltag_src_jar PROPERTY JAR_FILE)
-  install(FILES ${APRILTAG_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+    get_property(APRILTAG_SRC_JAR_FILE TARGET apriltag_src_jar PROPERTY JAR_FILE)
+    install(FILES ${APRILTAG_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-  set_property(TARGET apriltag_src_jar PROPERTY FOLDER "java")
+    set_property(TARGET apriltag_src_jar PROPERTY FOLDER "java")
 endif()
 
 generate_resources(src/main/native/resources/edu/wpi/first/apriltag generated/main/cpp APRILTAG frc apriltag_resources_src)
 
 file(GLOB apriltag_native_src src/main/native/cpp/*.cpp)
 
-add_library(apriltag ${apriltag_native_src} ${apriltag_resources_src} ${APRILTAGLIB_SRCS} ${APRILTAGLIB_COMMON_SRC} ${TAG_FILES})
+add_library(
+    apriltag
+    ${apriltag_native_src}
+    ${apriltag_resources_src}
+    ${APRILTAGLIB_SRCS}
+    ${APRILTAGLIB_COMMON_SRC}
+    ${TAG_FILES}
+)
 set_target_properties(apriltag PROPERTIES DEBUG_POSTFIX "d")
 
 set_property(TARGET apriltag PROPERTY FOLDER "libraries")
 target_compile_features(apriltag PUBLIC cxx_std_20)
 wpilib_target_warnings(apriltag)
 # disable warnings that apriltaglib can't handle
-if (MSVC)
-  target_compile_options(apriltag PRIVATE /wd4018 /wd4005 /wd4996)
+if(MSVC)
+    target_compile_options(apriltag PRIVATE /wd4018 /wd4005 /wd4996)
 else()
-  target_compile_options(apriltag PRIVATE -Wno-sign-compare -Wno-gnu-zero-variadic-macro-arguments -Wno-type-limits)
+    target_compile_options(
+        apriltag
+        PRIVATE -Wno-sign-compare -Wno-gnu-zero-variadic-macro-arguments -Wno-type-limits
+    )
 endif()
 
 target_link_libraries(apriltag wpimath)
 
-target_include_directories(apriltag PUBLIC
-  $<BUILD_INTERFACE:${apriltaglib_SOURCE_DIR}>
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-  $<INSTALL_INTERFACE:${include_dest}/apriltag>)
+target_include_directories(
+    apriltag
+    PUBLIC
+        $<BUILD_INTERFACE:${apriltaglib_SOURCE_DIR}>
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/apriltag>
+)
 
 install(TARGETS apriltag EXPORT apriltag)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag")
 
-if (WITH_FLAT_INSTALL)
-    set (apriltag_config_dir ${wpilib_dest})
-else()
-    set (apriltag_config_dir share/apriltag)
-endif()
+configure_file(apriltag-config.cmake.in ${WPILIB_BINARY_DIR}/apriltag-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION share/apriltag)
+install(EXPORT apriltag DESTINATION share/apriltag)
 
-configure_file(apriltag-config.cmake.in ${WPILIB_BINARY_DIR}/apriltag-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION ${apriltag_config_dir})
-install(EXPORT apriltag DESTINATION ${apriltag_config_dir})
-
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(apriltag src/test/native/cpp)
     target_include_directories(apriltag_test PRIVATE src/test/native/include)
     target_link_libraries(apriltag_test apriltag gmock_main)
diff --git a/third_party/allwpilib/apriltag/README.md b/third_party/allwpilib/apriltag/README.md
new file mode 100644
index 0000000..5bf1d67
--- /dev/null
+++ b/third_party/allwpilib/apriltag/README.md
@@ -0,0 +1,28 @@
+# AprilTag
+
+## Adding new field to AprilTagFields
+
+### Adding field JSON
+
+1. Add a field layout CSV file to `src/main/native/resources/edu/wpi/first/apriltag`
+    1. See docstring in `convert_apriltag_layouts.py` for more
+2. Run `convert_apriltag_layouts.py` in the same directory as this readme to generate the JSON
+3. That script overwrites all generated JSONs, so undo undesired changes if necessary
+4. Update the field dimensions at the bottom of the JSON
+    1. Length should be in meters from alliance wall to alliance wall
+    2. Width should be in meters from inside guardrail plastic to plastic
+
+### Java updates
+
+1. Update `src/main/java/edu/wpi/first/apriltag/AprilTagFields.java`
+    1. Add enum value for new field to `AprilTagFields`
+    2. Update `AprilTagFields.kDefaultField` if necessary
+
+### C++ updates
+
+1. Update `src/main/native/include/frc/apriltag/AprilTagFields.h`
+    1. Add enum value for new field to `AprilTagFields`
+    2. Update `AprilTagFields::kDefaultField` if necessary
+2. Update `src/main/native/cpp/AprilTagFields.cpp`
+    1. Add resource getter prototype like `std::string_view GetResource_2024_crescendo_json()`
+    2. Add case for new field to switch in `LoadAprilTagLayoutField()`
diff --git a/third_party/allwpilib/apriltag/apriltag-config.cmake.in b/third_party/allwpilib/apriltag/apriltag-config.cmake.in
index 7477a0e..9cb6093 100644
--- a/third_party/allwpilib/apriltag/apriltag-config.cmake.in
+++ b/third_party/allwpilib/apriltag/apriltag-config.cmake.in
@@ -5,3 +5,6 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/apriltag.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/apriltag_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/apriltag/convert_apriltag_layouts.py b/third_party/allwpilib/apriltag/convert_apriltag_layouts.py
new file mode 100755
index 0000000..2c1d60e
--- /dev/null
+++ b/third_party/allwpilib/apriltag/convert_apriltag_layouts.py
@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+
+"""
+This script converts all AprilTag field layout CSV files in
+src/main/native/resources/edu/wpi/first/apriltag to the JSON format
+AprilTagFields expects.
+
+The input CSV has the following format:
+
+* Columns: ID, X, Y, Z, Rotation
+* ID is a positive integer
+* X, Y, and Z are decimal inches
+* Rotation is yaw in degrees
+
+The values come from a table in the layout marking diagram (e.g.,
+https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf).
+"""
+
+import csv
+import json
+import os
+
+from wpimath import geometry, units
+import numpy as np
+
+
+def main():
+    # Find AprilTag field layout CSVs
+    filenames = [
+        os.path.join(dp, f)
+        for dp, dn, fn in os.walk("src/main/native/resources/edu/wpi/first/apriltag")
+        for f in fn
+        if f.endswith(".csv")
+    ]
+
+    for filename in filenames:
+        json_data = {"tags": [], "field": {"length": 0.0, "width": 0.0}}
+
+        # Read CSV and fill in JSON data
+        with open(filename, newline="") as csvfile:
+            reader = csv.reader(csvfile, delimiter=",")
+
+            # Skip header
+            next(reader)
+
+            for row in reader:
+                # Unpack row elements
+                id = int(row[0])
+                x = float(row[1])
+                y = float(row[2])
+                z = float(row[3])
+                rotation = float(row[4])
+
+                # Turn yaw into quaternion
+                q = geometry.Rotation3d(
+                    units.radians(0.0),
+                    units.radians(0.0),
+                    units.degreesToRadians(rotation),
+                ).getQuaternion()
+
+                json_data["tags"].append(
+                    {
+                        "ID": id,
+                        "pose": {
+                            "translation": {
+                                "x": units.inchesToMeters(x),
+                                "y": units.inchesToMeters(y),
+                                "z": units.inchesToMeters(z),
+                            },
+                            "rotation": {
+                                "quaternion": {
+                                    "W": q.W(),
+                                    "X": q.X(),
+                                    "Y": q.Y(),
+                                    "Z": q.Z(),
+                                }
+                            },
+                        },
+                    }
+                )
+
+        # Write JSON
+        with open(filename.replace(".csv", ".json"), "w") as f:
+            json.dump(json_data, f, indent=2)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java
index 8e455f6..9ee0207 100644
--- a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java
+++ b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java
@@ -6,17 +6,28 @@
 
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.apriltag.jni.AprilTagJNI;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.util.RawFrame;
 import java.util.Objects;
 
+/** Represents an AprilTag's metadata. */
 @SuppressWarnings("MemberName")
 public class AprilTag {
+  /** The tag's ID. */
   @JsonProperty(value = "ID")
   public int ID;
 
+  /** The tag's pose. */
   @JsonProperty(value = "pose")
   public Pose3d pose;
 
+  /**
+   * Constructs an AprilTag.
+   *
+   * @param ID The tag's ID.
+   * @param pose The tag's pose.
+   */
   @SuppressWarnings("ParameterName")
   @JsonCreator
   public AprilTag(
@@ -44,4 +55,28 @@
   public String toString() {
     return "AprilTag(ID: " + ID + ", pose: " + pose + ")";
   }
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 16h5 passed in.
+   *
+   * @param id id
+   * @return A RawFrame containing the AprilTag image
+   */
+  public static RawFrame generate16h5AprilTagImage(int id) {
+    RawFrame frame = new RawFrame();
+    AprilTagJNI.generate16h5AprilTagImage(frame, frame.getNativeObj(), id);
+    return frame;
+  }
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 36h11 passed in.
+   *
+   * @param id id
+   * @return A RawFrame containing the AprilTag image
+   */
+  public static RawFrame generate36h11AprilTagImage(int id) {
+    RawFrame frame = new RawFrame();
+    AprilTagJNI.generate36h11AprilTagImage(frame, frame.getNativeObj(), id);
+    return frame;
+  }
 }
diff --git a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java
index 9270cc9..954cb86 100644
--- a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java
+++ b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java
@@ -72,7 +72,7 @@
    * @return Homography matrix
    */
   public Matrix<N3, N3> getHomographyMatrix() {
-    return new MatBuilder<>(Nat.N3(), Nat.N3()).fill(m_homography);
+    return MatBuilder.fill(Nat.N3(), Nat.N3(), m_homography);
   }
 
   /**
@@ -95,7 +95,7 @@
 
   /**
    * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
-   * around the tag.
+   * around the tag. Index 0 is the bottom left corner.
    *
    * @param ndx Corner index (range is 0-3, inclusive)
    * @return Corner point X coordinate
@@ -106,7 +106,7 @@
 
   /**
    * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
-   * around the tag.
+   * around the tag. Index 0 is the bottom left corner.
    *
    * @param ndx Corner index (range is 0-3, inclusive)
    * @return Corner point Y coordinate
@@ -117,7 +117,8 @@
 
   /**
    * Gets the corners of the tag in image pixel coordinates. These always wrap counter-clock wise
-   * around the tag.
+   * around the tag. The first set of corner coordinates are the coordinates for the bottom left
+   * corner.
    *
    * @return Corner point array (X and Y for each corner in order)
    */
diff --git a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java
index b8e9cb0..a0774da 100644
--- a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java
+++ b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java
@@ -57,8 +57,21 @@
      */
     public boolean debug;
 
+    /** Default constructor. */
     public Config() {}
 
+    /**
+     * Constructs a detector configuration.
+     *
+     * @param numThreads How many threads should be used for computation.
+     * @param quadDecimate Quad decimation.
+     * @param quadSigma What Gaussian blur should be applied to the segmented image (used for quad
+     *     detection).
+     * @param refineEdges When true, the edges of the each quad are adjusted to "snap to" strong
+     *     gradients nearby.
+     * @param decodeSharpening How much sharpening should be done to decoded images.
+     * @param debug Debug mode.
+     */
     Config(
         int numThreads,
         float quadDecimate,
@@ -139,8 +152,21 @@
      */
     public boolean deglitch;
 
+    /** Default constructor. */
     public QuadThresholdParameters() {}
 
+    /**
+     * Constructs quad threshold parameters.
+     *
+     * @param minClusterPixels Threshold used to reject quads containing too few pixels.
+     * @param maxNumMaxima How many corner candidates to consider when segmenting a group of pixels
+     *     into a quad.
+     * @param criticalAngle Critical angle, in radians.
+     * @param maxLineFitMSE When fitting lines to the contours, the maximum mean squared error
+     *     allowed.
+     * @param minWhiteBlackDiff Minimum brightness offset.
+     * @param deglitch Whether the thresholded image be should be deglitched.
+     */
     QuadThresholdParameters(
         int minClusterPixels,
         int maxNumMaxima,
@@ -182,6 +208,7 @@
     }
   }
 
+  /** Constructs an AprilTagDetector. */
   public AprilTagDetector() {
     m_native = AprilTagJNI.createDetector();
   }
@@ -244,7 +271,7 @@
    * Adds a family of tags to be detected.
    *
    * @param fam Family name, e.g. "tag16h5"
-   * @param bitsCorrected maximum number of bits to correct
+   * @param bitsCorrected Maximum number of bits to correct
    * @throws IllegalArgumentException if family name not recognized
    */
   public void addFamily(String fam, int bitsCorrected) {
@@ -270,6 +297,8 @@
   /**
    * Detect tags from an 8-bit image.
    *
+   * <p>The image must be grayscale.
+   *
    * @param img 8-bit OpenCV Mat image
    * @return Results (array of AprilTagDetection)
    */
diff --git a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java
index ce2336c..5d0e424 100644
--- a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java
+++ b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java
@@ -44,8 +44,11 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class AprilTagFieldLayout {
+  /** Common origin positions for the AprilTag coordinate system. */
   public enum OriginPosition {
+    /** Blue alliance wall, right side. */
     kBlueAllianceWallRightSide,
+    /** Red alliance wall, right side. */
     kRedAllianceWallRightSide,
   }
 
@@ -143,7 +146,7 @@
    * @param origin The predefined origin
    */
   @JsonIgnore
-  public void setOrigin(OriginPosition origin) {
+  public final void setOrigin(OriginPosition origin) {
     switch (origin) {
       case kBlueAllianceWallRightSide:
         setOrigin(new Pose3d());
@@ -168,7 +171,7 @@
    * @param origin The new origin for tag transformations
    */
   @JsonIgnore
-  public void setOrigin(Pose3d origin) {
+  public final void setOrigin(Pose3d origin) {
     m_origin = origin;
   }
 
diff --git a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java
index dda8d34..653151e 100644
--- a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java
+++ b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java
@@ -7,15 +7,22 @@
 import java.io.IOException;
 import java.io.UncheckedIOException;
 
+/** Loadable AprilTag field layouts. */
 public enum AprilTagFields {
+  /** 2022 Rapid React. */
   k2022RapidReact("2022-rapidreact.json"),
-  k2023ChargedUp("2023-chargedup.json");
+  /** 2023 Charged Up. */
+  k2023ChargedUp("2023-chargedup.json"),
+  /** 2024 Crescendo. */
+  k2024Crescendo("2024-crescendo.json");
 
+  /** Base resource directory. */
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2023ChargedUp;
+  public static final AprilTagFields kDefaultField = k2024Crescendo;
 
+  /** Resource filename. */
   public final String m_resourceFile;
 
   AprilTagFields(String resourceFile) {
diff --git a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java
index 2b7f68a..182f394 100644
--- a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java
+++ b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java
@@ -29,10 +29,19 @@
       this.cy = cy;
     }
 
+    /** Tag size, in meters. */
     public double tagSize;
+
+    /** Camera horizontal focal length, in pixels. */
     public double fx;
+
+    /** Camera vertical focal length, in pixels. */
     public double fy;
+
+    /** Camera horizontal focal center, in pixels. */
     public double cx;
+
+    /** Camera vertical focal center, in pixels. */
     public double cy;
 
     @Override
diff --git a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
index 1a7c692..6aabbf9 100644
--- a/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
+++ b/third_party/allwpilib/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
@@ -8,25 +8,41 @@
 import edu.wpi.first.apriltag.AprilTagDetector;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.util.RawFrame;
 import edu.wpi.first.util.RuntimeLoader;
 import java.io.IOException;
 import java.util.concurrent.atomic.AtomicBoolean;
 
+/** AprilTag JNI. */
 public class AprilTagJNI {
   static boolean libraryLoaded = false;
 
   static RuntimeLoader<AprilTagJNI> loader = null;
 
+  /** Sets whether JNI should be loaded in the static block. */
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
     public static boolean getExtractOnStaticLoad() {
       return extractOnStaticLoad.get();
     }
 
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
     public static void setExtractOnStaticLoad(boolean load) {
       extractOnStaticLoad.set(load);
     }
+
+    /** Utility class. */
+    private Helper() {}
   }
 
   static {
@@ -44,31 +60,119 @@
     }
   }
 
+  /**
+   * Constructs an AprilTag detector engine.
+   *
+   * @return The detector engine handle
+   */
   public static native long createDetector();
 
+  /**
+   * Destroys an AprilTag detector engine.
+   *
+   * @param det The detector engine handle
+   */
   public static native void destroyDetector(long det);
 
+  /**
+   * Sets the detector engine configuration.
+   *
+   * @param det The detector engine handle
+   * @param config A configuration
+   */
   public static native void setDetectorConfig(long det, AprilTagDetector.Config config);
 
+  /**
+   * Gets the detector engine configuration.
+   *
+   * @param det The detector engine handle
+   * @return The configuration
+   */
   public static native AprilTagDetector.Config getDetectorConfig(long det);
 
+  /**
+   * Sets the detector engine quad threshold parameters.
+   *
+   * @param det The detector engine handle
+   * @param params Quad threshold parameters
+   */
   public static native void setDetectorQTP(
       long det, AprilTagDetector.QuadThresholdParameters params);
 
+  /**
+   * Gets the detector engine quad threshold parameters.
+   *
+   * @param det The detector engine handle
+   * @return Quad threshold parameters
+   */
   public static native AprilTagDetector.QuadThresholdParameters getDetectorQTP(long det);
 
+  /**
+   * Adds a family of tags to be detected by the detector engine.
+   *
+   * @param det The detector engine handle
+   * @param fam Family name, e.g. "tag16h5"
+   * @param bitsCorrected Maximum number of bits to correct
+   * @return False if family can't be found
+   */
   public static native boolean addFamily(long det, String fam, int bitsCorrected);
 
+  /**
+   * Removes a family of tags from the detector.
+   *
+   * @param det The detector engine handle
+   * @param fam Family name, e.g. "tag16h5"
+   */
   public static native void removeFamily(long det, String fam);
 
+  /**
+   * Unregister all families.
+   *
+   * @param det The detector engine handle
+   */
   public static native void clearFamilies(long det);
 
+  /**
+   * Detect tags from an 8-bit image.
+   *
+   * @param det The detector engine handle
+   * @param width The width of the image
+   * @param height The height of the image
+   * @param stride The number of bytes between image rows (often the same as width)
+   * @param bufAddr The address of the image buffer
+   * @return The results (array of AprilTagDetection)
+   */
   public static native AprilTagDetection[] detect(
       long det, int width, int height, int stride, long bufAddr);
 
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param tagSize The tag size, in meters
+   * @param fx The camera horizontal focal length, in pixels
+   * @param fy The camera vertical focal length, in pixels
+   * @param cx The camera horizontal focal center, in pixels
+   * @param cy The camera vertical focal center, in pixels
+   * @return Pose estimate
+   */
   public static native Transform3d estimatePoseHomography(
       double[] homography, double tagSize, double fx, double fy, double cx, double cy);
 
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
+   * the object-space error of each.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param tagSize The tag size, in meters
+   * @param fx The camera horizontal focal length, in pixels
+   * @param fy The camera vertical focal length, in pixels
+   * @param cx The camera horizontal focal center, in pixels
+   * @param cy The camera vertical focal center, in pixels
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
   public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration(
       double[] homography,
       double[] corners,
@@ -79,6 +183,20 @@
       double cy,
       int nIters);
 
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
+   * object-space error.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param tagSize The tag size, in meters
+   * @param fx The camera horizontal focal length, in pixels
+   * @param fy The camera vertical focal length, in pixels
+   * @param cx The camera horizontal focal center, in pixels
+   * @param cy The camera vertical focal center, in pixels
+   * @return Pose estimate
+   */
   public static native Transform3d estimatePose(
       double[] homography,
       double[] corners,
@@ -87,4 +205,25 @@
       double fy,
       double cx,
       double cy);
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 16h5 passed in.
+   *
+   * @param frameObj generated frame (output parameter).
+   * @param frame raw frame handle
+   * @param id id
+   */
+  public static native void generate16h5AprilTagImage(RawFrame frameObj, long frame, int id);
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 36h11 passed in.
+   *
+   * @param frameObj generated frame (output parameter).
+   * @param frame raw frame handle
+   * @param id id
+   */
+  public static native void generate36h11AprilTagImage(RawFrame frameObj, long frame, int id);
+
+  /** Utility class. */
+  private AprilTagJNI() {}
 }
diff --git a/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTag.cpp b/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTag.cpp
index 3805573..99ac81b 100644
--- a/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTag.cpp
+++ b/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTag.cpp
@@ -4,10 +4,53 @@
 
 #include "frc/apriltag/AprilTag.h"
 
+#include <cstring>
+
 #include <wpi/json.h>
 
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+#include "tag16h5.h"
+#include "tag36h11.h"
+
 using namespace frc;
 
+static bool FamilyToImage(wpi::RawFrame* frame, apriltag_family_t* family,
+                          int id) {
+  image_u8_t* image = apriltag_to_image(family, id);
+  size_t totalDataSize = image->height * image->stride;
+  bool rv = frame->Reserve(totalDataSize);
+  std::memcpy(frame->data, image->buf, totalDataSize);
+  frame->size = totalDataSize;
+  frame->width = image->width;
+  frame->height = image->height;
+  frame->stride = image->stride;
+  frame->pixelFormat = WPI_PIXFMT_GRAY;
+  image_u8_destroy(image);
+  return rv;
+}
+
+bool AprilTag::Generate36h11AprilTagImage(wpi::RawFrame* frame, int id) {
+  apriltag_family_t* tagFamily = tag36h11_create();
+  bool rv = FamilyToImage(frame, tagFamily, id);
+  tag36h11_destroy(tagFamily);
+  return rv;
+}
+
+bool AprilTag::Generate16h5AprilTagImage(wpi::RawFrame* frame, int id) {
+  apriltag_family_t* tagFamily = tag16h5_create();
+  bool rv = FamilyToImage(frame, tagFamily, id);
+  tag16h5_destroy(tagFamily);
+  return rv;
+}
+
 void frc::to_json(wpi::json& json, const AprilTag& apriltag) {
   json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}};
 }
diff --git a/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
index fd570ac..59b30ec 100644
--- a/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
+++ b/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
@@ -22,7 +22,7 @@
     throw std::runtime_error(fmt::format("Cannot open file: {}", path));
   }
 
-  wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+  wpi::json json = wpi::json::parse(fileBuffer->GetCharBuffer());
 
   for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
     m_apriltags[tag.ID] = tag;
@@ -49,6 +49,15 @@
   return m_fieldWidth;
 }
 
+std::vector<AprilTag> AprilTagFieldLayout::GetTags() const {
+  std::vector<AprilTag> tags;
+  tags.reserve(m_apriltags.size());
+  for (const auto& tag : m_apriltags) {
+    tags.emplace_back(tag.second);
+  }
+  return tags;
+}
+
 void AprilTagFieldLayout::SetOrigin(OriginPosition origin) {
   switch (origin) {
     case OriginPosition::kBlueAllianceWallRightSide:
diff --git a/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFields.cpp b/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFields.cpp
index 2f92937..be512ca 100644
--- a/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFields.cpp
+++ b/third_party/allwpilib/apriltag/src/main/native/cpp/AprilTagFields.cpp
@@ -11,6 +11,7 @@
 // C++ generated from resource files
 std::string_view GetResource_2022_rapidreact_json();
 std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2024_crescendo_json();
 
 AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
   std::string_view fieldString;
@@ -21,6 +22,9 @@
     case AprilTagField::k2023ChargedUp:
       fieldString = GetResource_2023_chargedup_json();
       break;
+    case AprilTagField::k2024Crescendo:
+      fieldString = GetResource_2024_crescendo_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }
diff --git a/third_party/allwpilib/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp b/third_party/allwpilib/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp
index c957b2d..b68b131 100644
--- a/third_party/allwpilib/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp
+++ b/third_party/allwpilib/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp
@@ -2,12 +2,17 @@
 // 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 <jni.h>
+
 #include <cstdio>
 #include <cstring>
 
+#define WPI_RAWFRAME_JNI
+#include <wpi/RawFrame.h>
 #include <wpi/jni_util.h>
 
 #include "edu_wpi_first_apriltag_jni_AprilTagJNI.h"
+#include "frc/apriltag/AprilTag.h"
 #include "frc/apriltag/AprilTagDetector.h"
 #include "frc/apriltag/AprilTagPoseEstimator.h"
 
@@ -24,6 +29,7 @@
 static JClass rotation3dCls;
 static JClass transform3dCls;
 static JClass translation3dCls;
+static JClass rawFrameCls;
 static JException illegalArgEx;
 static JException nullPointerEx;
 
@@ -36,7 +42,8 @@
     {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls},
     {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls},
     {"edu/wpi/first/math/geometry/Transform3d", &transform3dCls},
-    {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}};
+    {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls},
+    {"edu/wpi/first/util/RawFrame", &rawFrameCls}};
 
 static const JExceptionInit exceptions[] = {
     {"java/lang/IllegalArgumentException", &illegalArgEx},
@@ -587,4 +594,41 @@
   return MakeJObject(env, estimator.Estimate(harr, carr));
 }
 
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    generate16h5AprilTagImage
+ * Signature: (Ljava/lang/Object;JI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate16h5AprilTagImage
+  (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id)
+{
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  if (!frame) {
+    nullPointerEx.Throw(env, "frame is null");
+    return;
+  }
+  bool newData = AprilTag::Generate16h5AprilTagImage(frame, id);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData);
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    generate36h11AprilTagImage
+ * Signature: (Ljava/lang/Object;JI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate36h11AprilTagImage
+  (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id)
+{
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  if (!frame) {
+    nullPointerEx.Throw(env, "frame is null");
+    return;
+  }
+  // function might reallocate
+  bool newData = AprilTag::Generate36h11AprilTagImage(frame, id);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData);
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTag.h b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTag.h
index ba24530..a9ef78c 100644
--- a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTag.h
+++ b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTag.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <wpi/RawFrame.h>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -11,15 +12,20 @@
 
 namespace frc {
 
+/**
+ * Represents an AprilTag's metadata.
+ */
 struct WPILIB_DLLEXPORT AprilTag {
+  /// The tag's ID.
   int ID;
 
+  /// The tag's pose.
   Pose3d pose;
 
-  /**
-   * Checks equality between this AprilTag and another object.
-   */
   bool operator==(const AprilTag&) const = default;
+
+  static bool Generate36h11AprilTagImage(wpi::RawFrame* frame, int id);
+  static bool Generate16h5AprilTagImage(wpi::RawFrame* frame, int id);
 };
 
 WPILIB_DLLEXPORT
diff --git a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h
index babcbd8..74f8c64 100644
--- a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h
+++ b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h
@@ -93,7 +93,7 @@
 
   /**
    * Gets a corner of the tag in image pixel coordinates. These always
-   * wrap counter-clock wise around the tag.
+   * wrap counter-clock wise around the tag. Index 0 is the bottom left corner.
    *
    * @param ndx Corner index (range is 0-3, inclusive)
    * @return Corner point
@@ -104,7 +104,8 @@
 
   /**
    * Gets the corners of the tag in image pixel coordinates. These always
-   * wrap counter-clock wise around the tag.
+   * wrap counter-clock wise around the tag. The first set of corner coordinates
+   * are the coordinates for the bottom left corner.
    *
    * @param cornersBuf Corner point array (X and Y for each corner in order)
    * @return Corner point array (copy of cornersBuf span)
diff --git a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h
index 8a78d1b..59dedf1 100644
--- a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h
+++ b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h
@@ -205,7 +205,7 @@
    * Adds a family of tags to be detected.
    *
    * @param fam Family name, e.g. "tag16h5"
-   * @param bitsCorrected
+   * @param bitsCorrected Maximum number of bits to correct
    * @return False if family can't be found
    */
   bool AddFamily(std::string_view fam, int bitsCorrected = 2);
@@ -226,6 +226,7 @@
 
   /**
    * Detect tags from an 8-bit image.
+   * The image must be grayscale.
    *
    * @param width width of the image
    * @param height height of the image
@@ -237,6 +238,7 @@
 
   /**
    * Detect tags from an 8-bit image.
+   * The image must be grayscale.
    *
    * @param width width of the image
    * @param height height of the image
diff --git a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
index ddc1fed..1f5397b 100644
--- a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
+++ b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
@@ -38,8 +38,13 @@
  * towards the opposing alliance). */
 class WPILIB_DLLEXPORT AprilTagFieldLayout {
  public:
+  /**
+   * Common origin positions for the AprilTag coordinate system.
+   */
   enum class OriginPosition {
+    /// Blue alliance wall, right side.
     kBlueAllianceWallRightSide,
+    /// Red alliance wall, right side.
     kRedAllianceWallRightSide,
   };
 
@@ -75,6 +80,12 @@
   units::meter_t GetFieldWidth() const;
 
   /**
+   * Returns a vector of all the april tags used in this layout.
+   * @return list of tags
+   */
+  std::vector<AprilTag> GetTags() const;
+
+  /**
    * Sets the origin based on a predefined enumeration of coordinate frame
    * origins. The origins are calculated from the field dimensions.
    *
diff --git a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h
index 4bab299..933cf28 100644
--- a/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h
+++ b/third_party/allwpilib/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h
@@ -12,9 +12,16 @@
 
 namespace frc {
 
+/**
+ * Loadable AprilTag field layouts.
+ */
 enum class AprilTagField {
+  /// 2022 Rapid React.
   k2022RapidReact,
+  /// 2023 Charged Up.
   k2023ChargedUp,
+  /// 2024 Crescendo.
+  k2024Crescendo,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users
diff --git a/third_party/allwpilib/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv b/third_party/allwpilib/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv
new file mode 100644
index 0000000..cf570ce
--- /dev/null
+++ b/third_party/allwpilib/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv
@@ -0,0 +1,17 @@
+ID,X,Y,Z,Rotation
+1,593.68,9.68,53.38,120
+2,637.21,34.79,53.38,120
+3,652.73,196.17,57.13,180
+4,652.73,218.42,57.13,180
+5,578.77,323.00,53.38,270
+6,72.5,323.00,53.38,270
+7,-1.50,218.42,57.13,0
+8,-1.50,196.17,57.13,0
+9,14.02,34.79,53.38,60
+10,57.54,9.68,53.38,60
+11,468.69,146.19,52.00,300
+12,468.69,177.10,52.00,60
+13,441.74,161.62,52.00,180
+14,209.48,161.62,52.00,0
+15,182.73,177.10,52.00,120
+16,182.73,146.19,52.00,240
diff --git a/third_party/allwpilib/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json b/third_party/allwpilib/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json
new file mode 100644
index 0000000..0da1bac
--- /dev/null
+++ b/third_party/allwpilib/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json
@@ -0,0 +1,296 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 15.079471999999997,
+          "y": 0.24587199999999998,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 16.185134,
+          "y": 0.883666,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 16.579342,
+          "y": 4.982717999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 16.579342,
+          "y": 5.547867999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 14.700757999999999,
+          "y": 8.2042,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 1.8415,
+          "y": 8.2042,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": -0.038099999999999995,
+          "y": 5.547867999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": -0.038099999999999995,
+          "y": 4.982717999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 0.356108,
+          "y": 0.883666,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 1.4615159999999998,
+          "y": 0.24587199999999998,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 11.904726,
+          "y": 3.7132259999999997,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.904726,
+          "y": 4.49834,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 11.220196,
+          "y": 4.105148,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 5.320792,
+          "y": 4.105148,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 4.641342,
+          "y": 4.49834,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 4.641342,
+          "y": 3.7132259999999997,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.451,
+    "width": 8.211
+  }
+}
diff --git a/third_party/allwpilib/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java b/third_party/allwpilib/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java
new file mode 100644
index 0000000..3f3a9e6
--- /dev/null
+++ b/third_party/allwpilib/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java
@@ -0,0 +1,34 @@
+// 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.
+
+package edu.wpi.first.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.util.PixelFormat;
+import org.junit.jupiter.api.Test;
+
+class AprilTagGenerationTest {
+  @Test
+  void test36h11() {
+    var frame = AprilTag.generate36h11AprilTagImage(1);
+    assertEquals(PixelFormat.kGray, frame.getPixelFormat());
+    assertEquals(10, frame.getWidth());
+    assertEquals(10, frame.getHeight());
+    int stride = frame.getStride();
+    assertEquals(stride * 10, frame.getSize());
+    // check the diagonal values
+    var data = frame.getData();
+    assertEquals(-1, data.get(stride * 0 + 0)); // outer border is white
+    assertEquals(0, data.get(stride * 1 + 1)); // inner border is black
+    assertEquals(-1, data.get(stride * 2 + 2));
+    assertEquals(-1, data.get(stride * 3 + 3));
+    assertEquals(-1, data.get(stride * 4 + 4));
+    assertEquals(0, data.get(stride * 5 + 5));
+    assertEquals(0, data.get(stride * 6 + 6));
+    assertEquals(-1, data.get(stride * 7 + 7));
+    assertEquals(0, data.get(stride * 8 + 8)); // inner border
+    assertEquals(-1, data.get(stride * 9 + 9)); // outer border
+  }
+}
diff --git a/third_party/allwpilib/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java b/third_party/allwpilib/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java
index 901c72b..d5deb57 100644
--- a/third_party/allwpilib/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java
+++ b/third_party/allwpilib/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java
@@ -12,7 +12,6 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.util.Units;
-import java.io.IOException;
 import java.util.Optional;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.Test;
@@ -28,7 +27,7 @@
   }
 
   @Test
-  void test2022RapidReact() throws IOException {
+  void test2022RapidReact() {
     AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField();
 
     // Blue Hangar Truss - Hub
diff --git a/third_party/allwpilib/build.gradle b/third_party/allwpilib/build.gradle
index b1c3ca2..90c08c8 100644
--- a/third_party/allwpilib/build.gradle
+++ b/third_party/allwpilib/build.gradle
@@ -23,7 +23,7 @@
     id 'net.ltgt.errorprone' version '3.1.0' apply false
     id 'com.github.johnrengelman.shadow' version '8.1.1' apply false
     id 'com.diffplug.spotless' version '6.20.0' apply false
-    id 'com.github.spotbugs' version '5.1.3' apply false
+    id 'com.github.spotbugs' version '6.0.2' apply false
     id 'com.google.protobuf' version '0.9.3' apply false
 }
 
@@ -50,6 +50,9 @@
     publishAlways()
 }
 
+import com.github.spotbugs.snom.Effort
+ext.spotbugsEffort = Effort.MAX
+
 ext.licenseFile = files("$rootDir/LICENSE.md", "$rootDir/ThirdPartyNotices.txt")
 
 if (project.hasProperty("publishVersion")) {
@@ -171,5 +174,5 @@
 }
 
 wrapper {
-    gradleVersion = '8.4'
+    gradleVersion = '8.5'
 }
diff --git a/third_party/allwpilib/buildSrc/build.gradle b/third_party/allwpilib/buildSrc/build.gradle
index 2c52479..ed2748e 100644
--- a/third_party/allwpilib/buildSrc/build.gradle
+++ b/third_party/allwpilib/buildSrc/build.gradle
@@ -9,5 +9,5 @@
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2024.3.1"
+    implementation "edu.wpi.first:native-utils:2024.5.2"
 }
diff --git a/third_party/allwpilib/cameraserver/CMakeLists.txt b/third_party/allwpilib/cameraserver/CMakeLists.txt
index 177e009..6290049 100644
--- a/third_party/allwpilib/cameraserver/CMakeLists.txt
+++ b/third_party/allwpilib/cameraserver/CMakeLists.txt
@@ -3,10 +3,10 @@
 include(CompileWarnings)
 include(AddTest)
 
-find_package( OpenCV REQUIRED )
+find_package(OpenCV REQUIRED)
 
 # Java bindings
-if (WITH_JAVA)
+if(WITH_JAVA)
     find_package(Java REQUIRED)
     include(UseJava)
     set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
@@ -15,28 +15,42 @@
 
     set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
 
-    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
+    find_file(
+        OPENCV_JAR_FILE
+        NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
+        PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin
+        NO_DEFAULT_PATH
+    )
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
 
-    add_jar(cameraserver_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar cscore_jar ntcore_jar ${OPENCV_JAR_FILE} OUTPUT_NAME cameraserver)
+    add_jar(
+        cameraserver_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS wpiutil_jar cscore_jar ntcore_jar ${OPENCV_JAR_FILE}
+        OUTPUT_NAME cameraserver
+    )
 
-    get_property(CAMERASERVER_JAR_FILE TARGET cameraserver_jar PROPERTY JAR_FILE)
-    install(FILES ${CAMERASERVER_JAR_FILE} DESTINATION "${java_lib_dest}")
-
-    set_property(TARGET cameraserver_jar PROPERTY FOLDER "java")
-
+    install_jar(cameraserver_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(
+        TARGETS cameraserver_jar
+        FILE cameraserver_jar.cmake
+        DESTINATION share/cameraserver
+    )
 endif()
 
-if (WITH_JAVA_SOURCE)
+if(WITH_JAVA_SOURCE)
     find_package(Java REQUIRED)
     include(UseJava)
     file(GLOB CAMERASERVER_SOURCES src/main/java/edu/wpi/first/cameraserver/*.java)
     file(GLOB VISION_SOURCES src/main/java/edu/wpi/first/vision/*.java)
-    add_jar(cameraserver_src_jar
-    RESOURCES NAMESPACE "edu/wpi/first/cameraserver" ${CAMERASERVER_SOURCES}
-    NAMESPACE "edu/wpi/first/vision" ${VISION_SOURCES}
-    OUTPUT_NAME cameraserver-sources)
+    add_jar(
+        cameraserver_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/cameraserver" ${CAMERASERVER_SOURCES}
+        NAMESPACE "edu/wpi/first/vision" ${VISION_SOURCES}
+        OUTPUT_NAME cameraserver-sources
+    )
 
     get_property(CAMERASERVER_SRC_JAR_FILE TARGET cameraserver_src_jar PROPERTY JAR_FILE)
     install(FILES ${CAMERASERVER_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
@@ -44,13 +58,15 @@
     set_property(TARGET cameraserver_src_jar PROPERTY FOLDER "java")
 endif()
 
-file(GLOB_RECURSE
-    cameraserver_native_src src/main/native/cpp/*.cpp)
+file(GLOB_RECURSE cameraserver_native_src src/main/native/cpp/*.cpp)
 add_library(cameraserver ${cameraserver_native_src})
 set_target_properties(cameraserver PROPERTIES DEBUG_POSTFIX "d")
-target_include_directories(cameraserver PUBLIC
-                $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/cameraserver>)
+target_include_directories(
+    cameraserver
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/cameraserver>
+)
 wpilib_target_warnings(cameraserver)
 target_link_libraries(cameraserver PUBLIC ntcore cscore wpiutil ${OpenCV_LIBS})
 
@@ -59,15 +75,9 @@
 install(TARGETS cameraserver EXPORT cameraserver)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cameraserver")
 
-if (WITH_FLAT_INSTALL)
-    set (cameraserver_config_dir ${wpilib_dest})
-else()
-    set (cameraserver_config_dir share/cameraserver)
-endif()
-
-configure_file(cameraserver-config.cmake.in ${WPILIB_BINARY_DIR}/cameraserver-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/cameraserver-config.cmake DESTINATION ${cameraserver_config_dir})
-install(EXPORT cameraserver DESTINATION ${cameraserver_config_dir})
+configure_file(cameraserver-config.cmake.in ${WPILIB_BINARY_DIR}/cameraserver-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/cameraserver-config.cmake DESTINATION share/cameraserver)
+install(EXPORT cameraserver DESTINATION share/cameraserver)
 
 file(GLOB multiCameraServer_src multiCameraServer/src/main/native/cpp/*.cpp)
 add_executable(multiCameraServer ${multiCameraServer_src})
@@ -76,7 +86,7 @@
 
 set_property(TARGET multiCameraServer PROPERTY FOLDER "examples")
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(cameraserver src/test/native/cpp)
     target_link_libraries(cameraserver_test cameraserver gtest)
 endif()
diff --git a/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in b/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in
index 94d6813..46d522d 100644
--- a/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in
+++ b/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in
@@ -7,3 +7,6 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/cameraserver.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/cameraserver_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
index 1f79182..f69a6fe 100644
--- a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
+++ b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
@@ -98,7 +98,7 @@
     // parse file
     JsonElement top;
     try {
-      top = new JsonParser().parse(Files.newBufferedReader(Paths.get(configFile)));
+      top = JsonParser.parseReader(Files.newBufferedReader(Paths.get(configFile)));
     } catch (IOException ex) {
       System.err.println("could not open '" + configFile + "': " + ex);
       return false;
diff --git a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp
index 279eeba..706e2a2 100644
--- a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp
+++ b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp
@@ -105,7 +105,7 @@
   // parse file
   wpi::json j;
   try {
-    j = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+    j = wpi::json::parse(fileBuffer->GetCharBuffer());
   } catch (const wpi::json::parse_error& e) {
     fmt::print(stderr, "config error in '{}': byte {}: {}\n", configFile,
                e.byte, e.what());
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
index d4a1dd6..12dfbf6 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.cscore.VideoException;
 import edu.wpi.first.cscore.VideoListener;
 import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.cscore.VideoMode.PixelFormat;
 import edu.wpi.first.cscore.VideoSink;
 import edu.wpi.first.cscore.VideoSource;
 import edu.wpi.first.networktables.BooleanEntry;
@@ -27,6 +26,7 @@
 import edu.wpi.first.networktables.StringArrayTopic;
 import edu.wpi.first.networktables.StringEntry;
 import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.util.PixelFormat;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.HashMap;
@@ -39,6 +39,7 @@
  * NetworkTables.
  */
 public final class CameraServer {
+  /** CameraServer base port. */
   public static final int kBasePort = 1181;
 
   private static final String kPublishName = "/CameraPublisher";
@@ -114,7 +115,7 @@
     }
 
     @Override
-    public void close() throws Exception {
+    public void close() {
       if (m_booleanValueEntry != null) {
         m_booleanValueEntry.close();
       }
@@ -686,7 +687,7 @@
    */
   public static MjpegServer addSwitchedCamera(String name) {
     // create a dummy CvSource
-    CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, 160, 120, 30);
+    CvSource source = new CvSource(name, PixelFormat.kMJPEG, 160, 120, 30);
     MjpegServer server = startAutomaticCapture(source);
     synchronized (CameraServer.class) {
       m_fixedSources.put(server.getHandle(), source.getHandle());
@@ -749,6 +750,34 @@
    * Get OpenCV access to the specified camera. This allows you to get images from the camera for
    * image processing on the roboRIO.
    *
+   * @param camera Camera (e.g. as returned by startAutomaticCapture).
+   * @param pixelFormat Desired pixelFormat of the camera
+   * @return OpenCV sink for the specified camera
+   */
+  public static CvSink getVideo(VideoSource camera, PixelFormat pixelFormat) {
+    String name = "opencv_" + camera.getName();
+
+    synchronized (CameraServer.class) {
+      VideoSink sink = m_sinks.get(name);
+      if (sink != null) {
+        VideoSink.Kind kind = sink.getKind();
+        if (kind != VideoSink.Kind.kCv) {
+          throw new VideoException("expected OpenCV sink, but got " + kind);
+        }
+        return (CvSink) sink;
+      }
+    }
+
+    CvSink newsink = new CvSink(name, pixelFormat);
+    newsink.setSource(camera);
+    addServer(newsink);
+    return newsink;
+  }
+
+  /**
+   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
+   * image processing on the roboRIO.
+   *
    * @param name Camera name
    * @return OpenCV sink for the specified camera
    */
@@ -773,7 +802,7 @@
    * @return OpenCV source for the MJPEG stream
    */
   public static CvSource putVideo(String name, int width, int height) {
-    CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30);
+    CvSource source = new CvSource(name, PixelFormat.kMJPEG, width, height, 30);
     startAutomaticCapture(source);
     return source;
   }
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java
index 4726de1..dfa570f 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.cameraserver;
 
+/** CameraServer shared functions. */
 public interface CameraServerShared {
   /**
    * get the main thread id func.
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java
index 3d9e119..b129d55 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.cameraserver;
 
+/** Storage for CameraServerShared instance. */
 public final class CameraServerSharedStore {
   private static CameraServerShared cameraServerShared;
 
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java
index ab7072f..b9459cb 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java
@@ -14,6 +14,7 @@
  * code. The easiest way to use this is to run it in a {@link VisionThread} and use the listener to
  * take snapshots of the pipeline's outputs.
  *
+ * @param <P> Vision pipeline type.
  * @see VisionPipeline
  * @see VisionThread
  * @see <a href="package-summary.html">vision</a>
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java
index 6f1a1e3..93d4e83 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java
@@ -21,6 +21,7 @@
    *
    * @param visionRunner the runner for a vision pipeline
    */
+  @SuppressWarnings("this-escape")
   public VisionThread(VisionRunner<?> visionRunner) {
     super(visionRunner::runForever, "WPILib Vision Thread");
     setDaemon(true);
diff --git a/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp b/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
index d6014fa..97dba02 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
+++ b/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
@@ -622,6 +622,33 @@
   return newsink;
 }
 
+cs::CvSink CameraServer::GetVideo(const cs::VideoSource& camera,
+                                  cs::VideoMode::PixelFormat pixelFormat) {
+  auto& inst = ::GetInstance();
+  wpi::SmallString<64> name{"opencv_"};
+  name += camera.GetName();
+
+  {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sinks.find(name);
+    if (it != inst.m_sinks.end()) {
+      auto kind = it->second.GetKind();
+      if (kind != cs::VideoSink::kCv) {
+        auto csShared = GetCameraServerShared();
+        csShared->SetCameraServerError("expected OpenCV sink, but got {}",
+                                       static_cast<int>(kind));
+        return cs::CvSink{};
+      }
+      return *static_cast<cs::CvSink*>(&it->second);
+    }
+  }
+
+  cs::CvSink newsink{name.str(), pixelFormat};
+  newsink.SetSource(camera);
+  AddServer(newsink);
+  return newsink;
+}
+
 cs::CvSink CameraServer::GetVideo(std::string_view name) {
   auto& inst = ::GetInstance();
   cs::VideoSource source;
@@ -638,6 +665,23 @@
   return GetVideo(source);
 }
 
+cs::CvSink CameraServer::GetVideo(std::string_view name,
+                                  cs::VideoMode::PixelFormat pixelFormat) {
+  auto& inst = ::GetInstance();
+  cs::VideoSource source;
+  {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sources.find(name);
+    if (it == inst.m_sources.end()) {
+      auto csShared = GetCameraServerShared();
+      csShared->SetCameraServerError("could not find camera {}", name);
+      return cs::CvSink{};
+    }
+    source = it->second;
+  }
+  return GetVideo(source, pixelFormat);
+}
+
 cs::CvSource CameraServer::PutVideo(std::string_view name, int width,
                                     int height) {
   ::GetInstance();
diff --git a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h
index 29e16d7..0960612 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h
+++ b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h
@@ -22,10 +22,8 @@
  */
 class CameraServer {
  public:
+  /// CameraServer base port.
   static constexpr uint16_t kBasePort = 1181;
-  static constexpr int kSize640x480 = 0;
-  static constexpr int kSize320x240 = 1;
-  static constexpr int kSize160x120 = 2;
 
   /**
    * Start automatically capturing images to send to the dashboard.
@@ -195,11 +193,33 @@
    * Get OpenCV access to the specified camera.  This allows you to get
    * images from the camera for image processing on the roboRIO.
    *
+   * @param camera Camera (e.g. as returned by startAutomaticCapture).
+   * @param pixelFormat The desired pixelFormat of captured frames from the
+   * camera
+   */
+  static cs::CvSink GetVideo(const cs::VideoSource& camera,
+                             cs::VideoMode::PixelFormat pixelFormat);
+
+  /**
+   * Get OpenCV access to the specified camera.  This allows you to get
+   * images from the camera for image processing on the roboRIO.
+   *
    * @param name Camera name
    */
   static cs::CvSink GetVideo(std::string_view name);
 
   /**
+   * Get OpenCV access to the specified camera.  This allows you to get
+   * images from the camera for image processing on the roboRIO.
+   *
+   * @param name Camera name
+   * @param pixelFormat The desired pixelFormat of captured frames from the
+   * camera
+   */
+  static cs::CvSink GetVideo(std::string_view name,
+                             cs::VideoMode::PixelFormat pixelFormat);
+
+  /**
    * Create a MJPEG stream with OpenCV input. This can be called to pass custom
    * annotated images to the dashboard.
    *
diff --git a/third_party/allwpilib/cmake/modules/AddTest.cmake b/third_party/allwpilib/cmake/modules/AddTest.cmake
index eef4113..4896c4a 100644
--- a/third_party/allwpilib/cmake/modules/AddTest.cmake
+++ b/third_party/allwpilib/cmake/modules/AddTest.cmake
@@ -4,10 +4,10 @@
     file(GLOB_RECURSE test_src ${srcdir}/*.cpp)
     add_executable(${name}_test ${test_src})
     wpilib_target_warnings(${name}_test)
-    if (BUILD_SHARED_LIBS)
+    if(BUILD_SHARED_LIBS)
         target_compile_definitions(${name}_test PRIVATE -DGTEST_LINKED_AS_SHARED_LIBRARY)
     endif()
-    if (MSVC)
+    if(MSVC)
         target_compile_options(${name}_test PRIVATE /wd4101 /wd4251)
     endif()
     add_test(NAME ${name} COMMAND ${name}_test)
diff --git a/third_party/allwpilib/cmake/modules/CompileWarnings.cmake b/third_party/allwpilib/cmake/modules/CompileWarnings.cmake
index 8f5d54e..5de1032 100644
--- a/third_party/allwpilib/cmake/modules/CompileWarnings.cmake
+++ b/third_party/allwpilib/cmake/modules/CompileWarnings.cmake
@@ -1,28 +1,56 @@
 macro(wpilib_target_warnings target)
     if(NOT MSVC)
-        target_compile_options(${target} PRIVATE -Wall -pedantic -Wextra -Werror -Wno-unused-parameter ${WPILIB_TARGET_WARNINGS})
+        set(WARNING_FLAGS
+            -Wall
+            -pedantic
+            -Wextra
+            -Wno-unused-parameter
+            ${WPILIB_TARGET_WARNINGS}
+        )
+        if(NOT NO_WERROR)
+            set(WARNING_FLAGS ${WARNING_FLAGS} -Werror)
+        endif()
+
+        target_compile_options(${target} PRIVATE ${WARNING_FLAGS})
     else()
-        target_compile_options(${target} PRIVATE /wd4146 /wd4244 /wd4251 /wd4267 /WX /D_CRT_SECURE_NO_WARNINGS ${WPILIB_TARGET_WARNINGS})
+        target_compile_options(
+            ${target}
+            PRIVATE
+                /wd4146
+                /wd4244
+                /wd4251
+                /wd4267
+                /WX
+                /D_CRT_SECURE_NO_WARNINGS
+                ${WPILIB_TARGET_WARNINGS}
+        )
     endif()
 
     # Suppress C++-specific OpenCV warning; C compiler rejects it with an error
     # https://github.com/opencv/opencv/issues/20269
     if(UNIX AND NOT APPLE)
-        target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Wno-deprecated-enum-enum-conversion>)
+        target_compile_options(
+            ${target}
+            PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Wno-deprecated-enum-enum-conversion>
+        )
     elseif(UNIX AND APPLE)
-        target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Wno-deprecated-anon-enum-enum-conversion>)
+        target_compile_options(
+            ${target}
+            PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Wno-deprecated-anon-enum-enum-conversion>
+        )
     endif()
 
     # Suppress warning "enumeration types with a fixed underlying type are a
     # Clang extension"
     if(APPLE)
-      target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:C>:-Wno-fixed-enum-extension>)
+        target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:C>:-Wno-fixed-enum-extension>)
     endif()
 
     # Compress debug info with GCC
-    if ((${CMAKE_BUILD_TYPE} STREQUAL "Debug" OR
-         ${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo") AND
-        ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
-      target_compile_options(${target} PRIVATE -gz=zlib)
+    if(
+        (${CMAKE_BUILD_TYPE} STREQUAL "Debug" OR ${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo")
+        AND ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU"
+    )
+        target_compile_options(${target} PRIVATE -gz=zlib)
     endif()
 endmacro()
diff --git a/third_party/allwpilib/cmake/modules/DownloadAndCheck.cmake b/third_party/allwpilib/cmake/modules/DownloadAndCheck.cmake
index 59c8799..a1807d5 100644
--- a/third_party/allwpilib/cmake/modules/DownloadAndCheck.cmake
+++ b/third_party/allwpilib/cmake/modules/DownloadAndCheck.cmake
@@ -1,11 +1,11 @@
 macro(download_and_check source destination)
-  file(DOWNLOAD ${source} ${destination} STATUS download_status)
-  list(GET download_status 0 status_code)
-  list(GET download_status 1 status_message)
+    file(DOWNLOAD ${source} ${destination} STATUS download_status)
+    list(GET download_status 0 status_code)
+    list(GET download_status 1 status_message)
 
-  if(${status_code} EQUAL 0)
-    message(VERBOSE "Download of \"${source}\" successful.")
-  else()
-    message(FATAL_ERROR "Download of \"${source}\" failed: ${status_message}")
-  endif()
+    if(${status_code} EQUAL 0)
+        message(VERBOSE "Download of \"${source}\" successful.")
+    else()
+        message(FATAL_ERROR "Download of \"${source}\" failed: ${status_message}")
+    endif()
 endmacro()
diff --git a/third_party/allwpilib/cmake/modules/FindLIBSSH.cmake b/third_party/allwpilib/cmake/modules/FindLIBSSH.cmake
index ba2d778..6578aa9 100644
--- a/third_party/allwpilib/cmake/modules/FindLIBSSH.cmake
+++ b/third_party/allwpilib/cmake/modules/FindLIBSSH.cmake
@@ -38,30 +38,28 @@
     # in cache already
     set(LIBSSH_FOUND TRUE)
 else()
-    find_path(LIBSSH_INCLUDE_DIR
-        NAMES
-        libssh/libssh.h
+    find_path(
+        LIBSSH_INCLUDE_DIR
+        NAMES libssh/libssh.h
         PATHS
-        /usr/include
-        /usr/local/include
-        /opt/local/include
-        /sw/include
-        ${CMAKE_INCLUDE_PATH}
-        ${CMAKE_INSTALL_PREFIX}/include
+            /usr/include
+            /usr/local/include
+            /opt/local/include
+            /sw/include
+            ${CMAKE_INCLUDE_PATH}
+            ${CMAKE_INSTALL_PREFIX}/include
     )
 
-    find_library(LIBSSH_LIBRARY
-        NAMES
-        ssh.so
-        libssh.so
-        libssh.dylib
+    find_library(
+        LIBSSH_LIBRARY
+        NAMES ssh.so libssh.so libssh.dylib
         PATHS
-        /usr/lib
-        /usr/local/lib
-        /opt/local/lib
-        /sw/lib
-        ${CMAKE_LIBRARY_PATH}
-        ${CMAKE_INSTALL_PREFIX}/lib
+            /usr/lib
+            /usr/local/lib
+            /opt/local/lib
+            /sw/lib
+            ${CMAKE_LIBRARY_PATH}
+            ${CMAKE_INSTALL_PREFIX}/lib
     )
 
     if(LIBSSH_INCLUDE_DIR AND LIBSSH_LIBRARY)
@@ -71,34 +69,60 @@
         else()
             set(LIBSSH_HEADER_PATH ${LIBSSH_INCLUDE_DIR}/libssh/libssh.h)
         endif()
-        file(STRINGS ${LIBSSH_HEADER_PATH} LIBSSH_VERSION_MAJOR
-            REGEX "#define[ ]+LIBSSH_VERSION_MAJOR[ ]+[0-9]+")
+        file(
+            STRINGS
+            ${LIBSSH_HEADER_PATH}
+            LIBSSH_VERSION_MAJOR
+            REGEX "#define[ ]+LIBSSH_VERSION_MAJOR[ ]+[0-9]+"
+        )
         if(NOT LIBSSH_VERSION_MAJOR)
-            message(STATUS "LIBSSH_VERSION_MAJOR not found, assuming libssh is too old and cannot be used!")
+            message(
+                STATUS
+                "LIBSSH_VERSION_MAJOR not found, assuming libssh is too old and cannot be used!"
+            )
             set(LIBSSH_INCLUDE_DIR "LIBSSH_INCLUDE_DIR-NOTFOUND")
             set(LIBSSH_LIBRARY "LIBSSH_LIBRARY-NOTFOUND")
         else()
             string(REGEX MATCH "[0-9]+" LIBSSH_VERSION_MAJOR ${LIBSSH_VERSION_MAJOR})
-            file(STRINGS ${LIBSSH_HEADER_PATH} LIBSSH_VERSION_MINOR
-                REGEX "#define[ ]+LIBSSH_VERSION_MINOR[ ]+[0-9]+")
+            file(
+                STRINGS
+                ${LIBSSH_HEADER_PATH}
+                LIBSSH_VERSION_MINOR
+                REGEX "#define[ ]+LIBSSH_VERSION_MINOR[ ]+[0-9]+"
+            )
             string(REGEX MATCH "[0-9]+" LIBSSH_VERSION_MINOR ${LIBSSH_VERSION_MINOR})
-            file(STRINGS ${LIBSSH_HEADER_PATH} LIBSSH_VERSION_PATCH
-                REGEX "#define[ ]+LIBSSH_VERSION_MICRO[ ]+[0-9]+")
+            file(
+                STRINGS
+                ${LIBSSH_HEADER_PATH}
+                LIBSSH_VERSION_PATCH
+                REGEX "#define[ ]+LIBSSH_VERSION_MICRO[ ]+[0-9]+"
+            )
             string(REGEX MATCH "[0-9]+" LIBSSH_VERSION_PATCH ${LIBSSH_VERSION_PATCH})
 
-            set(LIBSSH_VERSION ${LIBSSH_VERSION_MAJOR}.${LIBSSH_VERSION_MINOR}.${LIBSSH_VERSION_PATCH})
+            set(LIBSSH_VERSION
+                ${LIBSSH_VERSION_MAJOR}.${LIBSSH_VERSION_MINOR}.${LIBSSH_VERSION_PATCH}
+            )
 
             if(LIBSSH_VERSION VERSION_LESS 0.8.0)
                 # libssh_threads also needs to be linked for these versions
-                string(REPLACE "libssh.so" "libssh_threads.so"
+                string(
+                    REPLACE
+                    "libssh.so"
+                    "libssh_threads.so"
                     LIBSSH_THREADS_LIBRARY
                     ${LIBSSH_LIBRARY}
                 )
-                string(REPLACE "libssh.dylib" "libssh_threads.dylib"
+                string(
+                    REPLACE
+                    "libssh.dylib"
+                    "libssh_threads.dylib"
                     LIBSSH_THREADS_LIBRARY
                     ${LIBSSH_THREADS_LIBRARY}
                 )
-                string(REPLACE "ssh.so" "ssh_threads.so"
+                string(
+                    REPLACE
+                    "ssh.so"
+                    "ssh_threads.so"
                     LIBSSH_THREADS_LIBRARY
                     ${LIBSSH_THREADS_LIBRARY}
                 )
@@ -110,7 +134,10 @@
     set(LIBSSH_LIBRARIES ${LIBSSH_LIBRARY} ${LIBSSH_THREADS_LIBRARY})
     mark_as_advanced(LIBSSH_INCLUDE_DIRS LIBSSH_LIBRARIES)
 
-    find_package_handle_standard_args(LIBSSH FOUND_VAR LIBSSH_FOUND
+    find_package_handle_standard_args(
+        LIBSSH
+        FOUND_VAR LIBSSH_FOUND
         REQUIRED_VARS LIBSSH_INCLUDE_DIRS LIBSSH_LIBRARIES
-        VERSION_VAR LIBSSH_VERSION)
+        VERSION_VAR LIBSSH_VERSION
+    )
 endif()
diff --git a/third_party/allwpilib/cmake/modules/GenResources.cmake b/third_party/allwpilib/cmake/modules/GenResources.cmake
index d80a364..c7d9f92 100644
--- a/third_party/allwpilib/cmake/modules/GenResources.cmake
+++ b/third_party/allwpilib/cmake/modules/GenResources.cmake
@@ -1,26 +1,30 @@
 set(scripts_dir "${CMAKE_CURRENT_LIST_DIR}/../scripts")
-macro(generate_resources inputDir outputDir prefix namespace outputFiles)
-  file(GLOB inputFiles ${inputDir}/*)
-  set(${outputFiles})
-  foreach(input ${inputFiles})
-    get_filename_component(inputBase ${input} NAME)
-    if("${inputBase}" MATCHES "^\\.")
-      continue()
-    endif()
-    set(output "${outputDir}/${inputBase}.cpp")
-    list(APPEND ${outputFiles} "${output}")
+macro(
+    generate_resources
+    inputDir
+    outputDir
+    prefix
+    namespace
+    outputFiles
+)
+    file(GLOB inputFiles ${inputDir}/*)
+    set(${outputFiles})
+    foreach(input ${inputFiles})
+        get_filename_component(inputBase ${input} NAME)
+        if("${inputBase}" MATCHES "^\\.")
+            continue()
+        endif()
+        set(output "${outputDir}/${inputBase}.cpp")
+        list(APPEND ${outputFiles} "${output}")
 
-    add_custom_command(
-      OUTPUT ${output}
-      COMMAND ${CMAKE_COMMAND}
-        "-Dinput=${input}"
-        "-Doutput=${output}"
-        "-Dprefix=${prefix}"
-        "-Dnamespace=${namespace}"
-        -P "${scripts_dir}/GenResource.cmake"
-      MAIN_DEPENDENCY ${input}
-      DEPENDS ${scripts_dir}/GenResource.cmake
-      VERBATIM
-    )
-  endforeach()
+        add_custom_command(
+            OUTPUT ${output}
+            COMMAND
+                ${CMAKE_COMMAND} "-Dinput=${input}" "-Doutput=${output}" "-Dprefix=${prefix}"
+                "-Dnamespace=${namespace}" -P "${scripts_dir}/GenResource.cmake"
+            MAIN_DEPENDENCY ${input}
+            DEPENDS ${scripts_dir}/GenResource.cmake
+            VERBATIM
+        )
+    endforeach()
 endmacro()
diff --git a/third_party/allwpilib/cmake/modules/LinkMacOSGUI.cmake b/third_party/allwpilib/cmake/modules/LinkMacOSGUI.cmake
index f76554e..6bfe540 100644
--- a/third_party/allwpilib/cmake/modules/LinkMacOSGUI.cmake
+++ b/third_party/allwpilib/cmake/modules/LinkMacOSGUI.cmake
@@ -1,5 +1,8 @@
 macro(wpilib_link_macos_gui target)
-    if (APPLE)
-        set_target_properties(${target} PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore")
+    if(APPLE)
+        set_target_properties(
+            ${target}
+            PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore"
+        )
     endif()
 endmacro()
diff --git a/third_party/allwpilib/cmake/modules/SubDirList.cmake b/third_party/allwpilib/cmake/modules/SubDirList.cmake
index 0776098..409eb18 100644
--- a/third_party/allwpilib/cmake/modules/SubDirList.cmake
+++ b/third_party/allwpilib/cmake/modules/SubDirList.cmake
@@ -1,17 +1,17 @@
 macro(subdir_list result curdir)
-  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
-  set(dirlist "")
-  foreach(child ${children})
-    if(IS_DIRECTORY ${curdir}/${child})
-      list(APPEND dirlist ${child})
-    endif()
-  endforeach()
-  set(${result} ${dirlist})
+    file(GLOB children RELATIVE ${curdir} ${curdir}/*)
+    set(dirlist "")
+    foreach(child ${children})
+        if(IS_DIRECTORY ${curdir}/${child})
+            list(APPEND dirlist ${child})
+        endif()
+    endforeach()
+    set(${result} ${dirlist})
 endmacro()
 
 macro(add_all_subdirectories curdir)
-  subdir_list(_SUBPROJECTS ${curdir})
-  foreach(dir ${_SUBPROJECTS})
-    add_subdirectory(${dir})
-  endforeach()
+    subdir_list(_SUBPROJECTS ${curdir})
+    foreach(dir ${_SUBPROJECTS})
+        add_subdirectory(${dir})
+    endforeach()
 endmacro()
diff --git a/third_party/allwpilib/cmake/scripts/GenResource.cmake b/third_party/allwpilib/cmake/scripts/GenResource.cmake
index 3c6e251..ad9ca91 100644
--- a/third_party/allwpilib/cmake/scripts/GenResource.cmake
+++ b/third_party/allwpilib/cmake/scripts/GenResource.cmake
@@ -1,23 +1,35 @@
 # Parameters: input output prefix namespace
-FILE(READ ${input} fileHex HEX)
-STRING(LENGTH "${fileHex}" fileHexSize)
-MATH(EXPR fileSize "${fileHexSize} / 2")
+file(READ ${input} fileHex HEX)
+string(LENGTH "${fileHex}" fileHexSize)
+math(EXPR fileSize "${fileHexSize} / 2")
 
-GET_FILENAME_COMPONENT(inputBase ${input} NAME)
-STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" funcName "${inputBase}")
-SET(funcName "GetResource_${funcName}")
+get_filename_component(inputBase ${input} NAME)
+string(REGEX REPLACE "[^a-zA-Z0-9]" "_" funcName "${inputBase}")
+set(funcName "GetResource_${funcName}")
 
-FILE(WRITE "${output}" "#include <stddef.h>\n#include <string_view>\nextern \"C\" {\nstatic const unsigned char contents[] = {")
+file(
+    WRITE
+    "${output}"
+    "#include <stddef.h>\n#include <string_view>\nextern \"C\" {\nstatic const unsigned char contents[] = {"
+)
 
-STRING(REGEX MATCHALL ".." outputData "${fileHex}")
-STRING(REGEX REPLACE ";" ", 0x" outputData "${outputData}")
-FILE(APPEND "${output}" " 0x${outputData} };\n")
-FILE(APPEND "${output}" "const unsigned char* ${prefix}${funcName}(size_t* len) {\n  *len = ${fileSize};\n  return contents;\n}\n}\n")
+string(REGEX MATCHALL ".." outputData "${fileHex}")
+string(REGEX REPLACE ";" ", 0x" outputData "${outputData}")
+file(APPEND "${output}" " 0x${outputData} };\n")
+file(
+    APPEND
+    "${output}"
+    "const unsigned char* ${prefix}${funcName}(size_t* len) {\n  *len = ${fileSize};\n  return contents;\n}\n}\n"
+)
 
-IF(NOT namespace STREQUAL "")
-  FILE(APPEND "${output}" "namespace ${namespace} {\n")
-ENDIF()
-FILE(APPEND "${output}" "std::string_view ${funcName}() {\n  return std::string_view(reinterpret_cast<const char*>(contents), ${fileSize});\n}\n")
-IF(NOT namespace STREQUAL "")
-  FILE(APPEND "${output}" "}\n")
-ENDIF()
+if(NOT namespace STREQUAL "")
+    file(APPEND "${output}" "namespace ${namespace} {\n")
+endif()
+file(
+    APPEND
+    "${output}"
+    "std::string_view ${funcName}() {\n  return std::string_view(reinterpret_cast<const char*>(contents), ${fileSize});\n}\n"
+)
+if(NOT namespace STREQUAL "")
+    file(APPEND "${output}" "}\n")
+endif()
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/LifetimeWrappers.h b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/LifetimeWrappers.h
index 5731200..e196d84 100644
--- a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/LifetimeWrappers.h
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/LifetimeWrappers.h
@@ -190,12 +190,12 @@
   HAL_RelayHandle handle = 0;
 };
 
-#define ASSERT_LAST_ERROR_STATUS(status, x)                          \
-  do {                                                               \
-    ASSERT_EQ(status, HAL_USE_LAST_ERROR);                           \
-    const char* lastErrorMessageInMacro = HAL_GetLastError(&status); \
-    static_cast<void>(lastErrorMessageInMacro);                      \
-    ASSERT_EQ(status, x);                                            \
+#define ASSERT_LAST_ERROR_STATUS(status, x)                \
+  do {                                                     \
+    ASSERT_EQ(status, HAL_USE_LAST_ERROR);                 \
+    [[maybe_unused]] const char* lastErrorMessageInMacro = \
+        HAL_GetLastError(&status);                         \
+    ASSERT_EQ(status, x);                                  \
   } while (0)
 
 }  // namespace hlt
diff --git a/third_party/allwpilib/cscore/CMakeLists.txt b/third_party/allwpilib/cscore/CMakeLists.txt
index a6a89b3..7891486 100644
--- a/third_party/allwpilib/cscore/CMakeLists.txt
+++ b/third_party/allwpilib/cscore/CMakeLists.txt
@@ -5,10 +5,9 @@
 include(AddTest)
 include(LinkMacOSGUI)
 
-find_package( OpenCV REQUIRED )
+find_package(OpenCV REQUIRED)
 
-file(GLOB
-    cscore_native_src src/main/native/cpp/*.cpp)
+file(GLOB cscore_native_src src/main/native/cpp/*.cpp)
 file(GLOB cscore_linux_src src/main/native/linux/*.cpp)
 file(GLOB cscore_osx_src src/main/native/osx/*.cpp)
 file(GLOB cscore_osx_objc_src src/main/native/objcpp/*.mm)
@@ -18,10 +17,15 @@
 set_target_properties(cscore PROPERTIES DEBUG_POSTFIX "d")
 
 if(NOT MSVC)
-    if (APPLE)
+    if(APPLE)
         target_sources(cscore PRIVATE ${cscore_osx_src} ${cscore_osx_objc_src})
         target_compile_options(cscore PRIVATE "-fobjc-arc")
-        set_target_properties(cscore PROPERTIES LINK_FLAGS "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo")
+        set_target_properties(
+            cscore
+            PROPERTIES
+                LINK_FLAGS
+                    "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo"
+        )
     else()
         target_sources(cscore PRIVATE ${cscore_linux_src})
     endif()
@@ -31,9 +35,12 @@
     target_compile_definitions(cscore PRIVATE -D_CRT_SECURE_NO_WARNINGS)
 endif()
 
-target_include_directories(cscore PUBLIC
-                $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/cscore>)
+target_include_directories(
+    cscore
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/cscore>
+)
 target_include_directories(cscore PRIVATE src/main/native/cpp)
 wpilib_target_warnings(cscore)
 target_link_libraries(cscore PUBLIC wpinet wpiutil ${OpenCV_LIBS})
@@ -43,15 +50,9 @@
 install(TARGETS cscore EXPORT cscore)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cscore")
 
-if (WITH_FLAT_INSTALL)
-    set (cscore_config_dir ${wpilib_dest})
-else()
-    set (cscore_config_dir share/cscore)
-endif()
-
-configure_file(cscore-config.cmake.in ${WPILIB_BINARY_DIR}/cscore-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/cscore-config.cmake DESTINATION ${cscore_config_dir})
-install(EXPORT cscore DESTINATION ${cscore_config_dir})
+configure_file(cscore-config.cmake.in ${WPILIB_BINARY_DIR}/cscore-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/cscore-config.cmake DESTINATION share/cscore)
+install(EXPORT cscore DESTINATION share/cscore)
 
 subdir_list(cscore_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
 foreach(example ${cscore_examples})
@@ -68,7 +69,7 @@
         add_executable(cscore_${example} ${cscore_example_src})
         wpilib_target_warnings(cscore_${example})
 
-        if (${example} STREQUAL "usbviewer")
+        if(${example} STREQUAL "usbviewer")
             wpilib_link_macos_gui(cscore_${example})
         endif()
 
@@ -78,7 +79,7 @@
 endforeach()
 
 # Java bindings
-if (WITH_JAVA)
+if(WITH_JAVA)
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
@@ -90,36 +91,68 @@
         set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
     endif()
 
-    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/share/java NO_DEFAULT_PATH)
-    find_file(OPENCV_JNI_FILE NAMES libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.so
-                                    libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dylib
-                                    opencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll
-                                    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/bin/Release ${OpenCV_INSTALL_PATH}/bin/Debug ${OpenCV_INSTALL_PATH}/lib ${OpenCV_INSTALL_PATH}/lib/jni NO_DEFAULT_PATH)
+    find_file(
+        OPENCV_JAR_FILE
+        NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
+        PATHS
+            ${OPENCV_JAVA_INSTALL_DIR}
+            ${OpenCV_INSTALL_PATH}/bin
+            ${OpenCV_INSTALL_PATH}/share/java
+        NO_DEFAULT_PATH
+    )
+    find_file(
+        OPENCV_JNI_FILE
+        NAMES
+            libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.so
+            libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dylib
+            opencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll
+        PATHS
+            ${OPENCV_JAVA_INSTALL_DIR}
+            ${OpenCV_INSTALL_PATH}/bin
+            ${OpenCV_INSTALL_PATH}/bin/Release
+            ${OpenCV_INSTALL_PATH}/bin/Debug
+            ${OpenCV_INSTALL_PATH}/lib
+            ${OpenCV_INSTALL_PATH}/lib/jni
+        NO_DEFAULT_PATH
+    )
 
-    file(GLOB
-        cscore_jni_src src/main/native/cpp/jni/CameraServerJNI.cpp)
+    file(GLOB cscore_jni_src src/main/native/cpp/jni/CameraServerJNI.cpp)
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
     set(CMAKE_JNI_TARGET true)
 
-    add_jar(cscore_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar ${OPENCV_JAR_FILE} OUTPUT_NAME cscore GENERATE_NATIVE_HEADERS cscore_jni_headers)
+    add_jar(
+        cscore_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS wpiutil_jar ${OPENCV_JAR_FILE}
+        OUTPUT_NAME cscore
+        GENERATE_NATIVE_HEADERS cscore_jni_headers
+    )
 
-    get_property(CSCORE_JAR_FILE TARGET cscore_jar PROPERTY JAR_FILE)
-    install(FILES ${CSCORE_JAR_FILE} DESTINATION "${java_lib_dest}")
+    install_jar(cscore_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS cscore_jar FILE cscore_jar.cmake DESTINATION share/cscore)
     install(FILES ${OPENCV_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-    if (MSVC)
+    if(MSVC)
         install(FILES ${OPENCV_JNI_FILE} DESTINATION "${jni_lib_dest}")
 
         foreach(cvFile ${OpenCV_LIBS})
-            find_file(${cvFile}Loc NAMES ${cvFile}${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll
-                              PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/bin/Release ${OpenCV_INSTALL_PATH}/bin/Debug ${OpenCV_INSTALL_PATH}/lib NO_DEFAULT_PATH)
+            find_file(
+                ${cvFile}Loc
+                NAMES
+                    ${cvFile}${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll
+                PATHS
+                    ${OPENCV_JAVA_INSTALL_DIR}
+                    ${OpenCV_INSTALL_PATH}/bin
+                    ${OpenCV_INSTALL_PATH}/bin/Release
+                    ${OpenCV_INSTALL_PATH}/bin/Debug
+                    ${OpenCV_INSTALL_PATH}/lib
+                NO_DEFAULT_PATH
+            )
             install(FILES ${${cvFile}Loc} DESTINATION "${jni_lib_dest}")
         endforeach()
     endif()
 
-    set_property(TARGET cscore_jar PROPERTY FOLDER "java")
-
     add_library(cscorejni ${cscore_jni_src})
     wpilib_target_warnings(cscorejni)
     target_link_libraries(cscorejni PUBLIC cscore wpiutil ${OpenCV_LIBS})
@@ -130,18 +163,20 @@
     add_dependencies(cscorejni cscore_jar)
 
     install(TARGETS cscorejni EXPORT cscorejni)
-
 endif()
 
-if (WITH_JAVA_SOURCE)
+if(WITH_JAVA_SOURCE)
     find_package(Java REQUIRED)
     include(UseJava)
     file(GLOB CSCORE_SOURCES src/main/java/edu/wpi/first/cscore/*.java)
     file(GLOB CSCORE_RAW_SOURCES src/main/java/edu/wpi/first/cscore/raw/*.java)
-    add_jar(cscore_src_jar
-    RESOURCES NAMESPACE "edu/wpi/first/cscore" ${CSCORE_SOURCES}
-    NAMESPACE "edu/wpi/first/cscore/raw" ${CSCORE_RAW_SOURCES}
-    OUTPUT_NAME cscore-sources)
+    add_jar(
+        cscore_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/cscore" ${CSCORE_SOURCES}
+        NAMESPACE "edu/wpi/first/cscore/raw" ${CSCORE_RAW_SOURCES}
+        OUTPUT_NAME cscore-sources
+    )
 
     get_property(CSCORE_SRC_JAR_FILE TARGET cscore_src_jar PROPERTY JAR_FILE)
     install(FILES ${CSCORE_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
@@ -149,7 +184,7 @@
     set_property(TARGET cscore_src_jar PROPERTY FOLDER "java")
 endif()
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(cscore src/test/native/cpp)
     target_link_libraries(cscore_test cscore gmock)
 endif()
diff --git a/third_party/allwpilib/cscore/cscore-config.cmake.in b/third_party/allwpilib/cscore/cscore-config.cmake.in
index 33e0af3..da5a71f 100644
--- a/third_party/allwpilib/cscore/cscore-config.cmake.in
+++ b/third_party/allwpilib/cscore/cscore-config.cmake.in
@@ -5,3 +5,6 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/cscore.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/cscore_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java
index 6619522..fdbfe61 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java
@@ -9,21 +9,36 @@
 import java.util.concurrent.atomic.AtomicBoolean;
 import org.opencv.core.Core;
 
+/** CameraServer CV JNI. */
 public class CameraServerCvJNI {
   static boolean libraryLoaded = false;
 
   static RuntimeLoader<Core> loader = null;
 
+  /** Sets whether JNI should be loaded in the static block. */
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
     public static boolean getExtractOnStaticLoad() {
       return extractOnStaticLoad.get();
     }
 
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
     public static void setExtractOnStaticLoad(boolean load) {
       extractOnStaticLoad.set(load);
     }
+
+    /** Utility class. */
+    private Helper() {}
   }
 
   static {
@@ -64,7 +79,7 @@
 
   public static native void putSourceFrame(int source, long imageNativeObj);
 
-  public static native int createCvSink(String name);
+  public static native int createCvSink(String name, int pixelFormat);
 
   // public static native int createCvSinkCallback(String name,
   //                            void (*processFrame)(long time));
@@ -72,4 +87,7 @@
   public static native long grabSinkFrame(int sink, long imageNativeObj);
 
   public static native long grabSinkFrameTimeout(int sink, long imageNativeObj, double timeout);
+
+  /** Utility class. */
+  private CameraServerCvJNI() {}
 }
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java
index 148be68..0d534b0 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java
@@ -4,28 +4,43 @@
 
 package edu.wpi.first.cscore;
 
-import edu.wpi.first.cscore.raw.RawFrame;
+import edu.wpi.first.util.RawFrame;
 import edu.wpi.first.util.RuntimeLoader;
 import java.io.IOException;
 import java.nio.ByteBuffer;
 import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.function.Consumer;
 
+/** CameraServer JNI. */
 public class CameraServerJNI {
   static boolean libraryLoaded = false;
 
   static RuntimeLoader<CameraServerJNI> loader = null;
 
+  /** Sets whether JNI should be loaded in the static block. */
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
     public static boolean getExtractOnStaticLoad() {
       return extractOnStaticLoad.get();
     }
 
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
     public static void setExtractOnStaticLoad(boolean load) {
       extractOnStaticLoad.set(load);
     }
+
+    /** Utility class. */
+    private Helper() {}
   }
 
   static {
@@ -182,21 +197,13 @@
   //
   // Image Source Functions
   //
+  public static native void putRawSourceFrame(int source, long frame);
+
   public static native void putRawSourceFrameBB(
-      int source, ByteBuffer data, int width, int height, int pixelFormat, int totalData);
+      int source, ByteBuffer data, int size, int width, int height, int stride, int pixelFormat);
 
-  public static native void putRawSourceFrame(
-      int source, long data, int width, int height, int pixelFormat, int totalData);
-
-  public static void putRawSourceFrame(int source, RawFrame raw) {
-    putRawSourceFrame(
-        source,
-        raw.getDataPtr(),
-        raw.getWidth(),
-        raw.getHeight(),
-        raw.getPixelFormat(),
-        raw.getTotalData());
-  }
+  public static native void putRawSourceFrameData(
+      int source, long data, int size, int width, int height, int stride, int pixelFormat);
 
   public static native void notifySourceError(int source, String msg);
 
@@ -263,47 +270,10 @@
   //
   public static native void setSinkDescription(int sink, String description);
 
-  private static native long grabRawSinkFrameImpl(
-      int sink,
-      RawFrame rawFrame,
-      long rawFramePtr,
-      ByteBuffer byteBuffer,
-      int width,
-      int height,
-      int pixelFormat);
+  public static native long grabRawSinkFrame(int sink, RawFrame frame, long nativeObj);
 
-  private static native long grabRawSinkFrameTimeoutImpl(
-      int sink,
-      RawFrame rawFrame,
-      long rawFramePtr,
-      ByteBuffer byteBuffer,
-      int width,
-      int height,
-      int pixelFormat,
-      double timeout);
-
-  public static long grabSinkFrame(int sink, RawFrame rawFrame) {
-    return grabRawSinkFrameImpl(
-        sink,
-        rawFrame,
-        rawFrame.getFramePtr(),
-        rawFrame.getDataByteBuffer(),
-        rawFrame.getWidth(),
-        rawFrame.getHeight(),
-        rawFrame.getPixelFormat());
-  }
-
-  public static long grabSinkFrameTimeout(int sink, RawFrame rawFrame, double timeout) {
-    return grabRawSinkFrameTimeoutImpl(
-        sink,
-        rawFrame,
-        rawFrame.getFramePtr(),
-        rawFrame.getDataByteBuffer(),
-        rawFrame.getWidth(),
-        rawFrame.getHeight(),
-        rawFrame.getPixelFormat(),
-        timeout);
-  }
+  public static native long grabRawSinkFrameTimeout(
+      int sink, RawFrame frame, long nativeObj, double timeout);
 
   public static native String getSinkError(int sink);
 
@@ -334,7 +304,9 @@
   // Telemetry Functions
   //
   public enum TelemetryKind {
+    /** kSourceBytesReceived. */
     kSourceBytesReceived(1),
+    /** kSourceFramesReceived. */
     kSourceFramesReceived(2);
 
     private final int value;
@@ -387,13 +359,12 @@
 
   public static native String[] getNetworkInterfaces();
 
-  public static native long allocateRawFrame();
-
-  public static native void freeRawFrame(long frame);
-
   public static native void runMainRunLoop();
 
   public static native int runMainRunLoopTimeout(double timeoutSeconds);
 
   public static native void stopMainRunLoop();
+
+  /** Utility class. */
+  private CameraServerJNI() {}
 }
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java
index 88ca8b1..af390e5 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.cscore;
 
+import edu.wpi.first.util.PixelFormat;
 import org.opencv.core.Mat;
 
 /**
@@ -16,9 +17,20 @@
    * get each new image.
    *
    * @param name Source name (arbitrary unique identifier)
+   * @param pixelFormat Source pixel format
+   */
+  public CvSink(String name, PixelFormat pixelFormat) {
+    super(CameraServerCvJNI.createCvSink(name, pixelFormat.getValue()));
+  }
+
+  /**
+   * Create a sink for accepting OpenCV images. WaitForFrame() must be called on the created sink to
+   * get each new image. Defaults to kBGR for pixelFormat
+   *
+   * @param name Source name (arbitrary unique identifier)
    */
   public CvSink(String name) {
-    super(CameraServerCvJNI.createCvSink(name));
+    this(name, PixelFormat.kBGR);
   }
 
   /// Create a sink for accepting OpenCV images in a separate thread.
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSource.java
index 3934a09..4c29ffc 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSource.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSource.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.cscore;
 
+import edu.wpi.first.util.PixelFormat;
 import org.opencv.core.Mat;
 
 /**
@@ -32,7 +33,7 @@
    * @param height height
    * @param fps fps
    */
-  public CvSource(String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
+  public CvSource(String name, PixelFormat pixelFormat, int width, int height, int fps) {
     super(CameraServerCvJNI.createCvSource(name, pixelFormat.getValue(), width, height, fps));
   }
 
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/HttpCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/HttpCamera.java
index 8c72350..e74b4ed 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/HttpCamera.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/HttpCamera.java
@@ -7,9 +7,13 @@
 /** A source that represents a MJPEG-over-HTTP (IP) camera. */
 public class HttpCamera extends VideoCamera {
   public enum HttpCameraKind {
+    /** Unknown camera kind. */
     kUnknown(0),
+    /** MJPG Streamer camera. */
     kMJPGStreamer(1),
+    /** CS Core camera. */
     kCSCore(2),
+    /** Axis camera. */
     kAxis(3);
 
     private final int value;
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoCamera.java
index 23cf7d6..b6b8a75 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoCamera.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoCamera.java
@@ -12,6 +12,9 @@
     public static final int kFixedOutdoor2 = 5000;
     public static final int kFixedFluorescent1 = 5100;
     public static final int kFixedFlourescent2 = 5200;
+
+    /** Default constructor. */
+    public WhiteBalance() {}
   }
 
   protected VideoCamera(int handle) {
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoEvent.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoEvent.java
index 0c60d6d..55ba6e9 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoEvent.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoEvent.java
@@ -8,26 +8,47 @@
 @SuppressWarnings("MemberName")
 public class VideoEvent {
   public enum Kind {
+    /** Unknown video event. */
     kUnknown(0x0000),
+    /** Source Created event. */
     kSourceCreated(0x0001),
+    /** Source Destroyed event. */
     kSourceDestroyed(0x0002),
+    /** Source Connected event. */
     kSourceConnected(0x0004),
+    /** Source Disconnected event. */
     kSourceDisconnected(0x0008),
+    /** Source Video Modes Updated event. */
     kSourceVideoModesUpdated(0x0010),
+    /** Source VideoMode Changed event. */
     kSourceVideoModeChanged(0x0020),
+    /** Source Property Created event. */
     kSourcePropertyCreated(0x0040),
+    /** Source Property Value Updated event. */
     kSourcePropertyValueUpdated(0x0080),
+    /** Source Property Choices Updated event. */
     kSourcePropertyChoicesUpdated(0x0100),
+    /** Sink Source Changed event. */
     kSinkSourceChanged(0x0200),
+    /** Sink Created event. */
     kSinkCreated(0x0400),
+    /** Sink Destroyed event. */
     kSinkDestroyed(0x0800),
+    /** Sink Enabled event. */
     kSinkEnabled(0x1000),
+    /** Sink Disabled event. */
     kSinkDisabled(0x2000),
+    /** Network Interfaces Changed event. */
     kNetworkInterfacesChanged(0x4000),
+    /** Telemetry Updated event. */
     kTelemetryUpdated(0x8000),
+    /** Sink Property Created event. */
     kSinkPropertyCreated(0x10000),
+    /** Sink Property Value Updated event. */
     kSinkPropertyValueUpdated(0x20000),
+    /** Sink Property Choices Updated event. */
     kSinkPropertyChoicesUpdated(0x40000),
+    /** Usb Cameras Changed event. */
     kUsbCamerasChanged(0x80000);
 
     private final int value;
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoListener.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoListener.java
index 652d12d..c95e545 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoListener.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoListener.java
@@ -102,7 +102,7 @@
                       listener.accept(event);
                     } catch (Throwable throwable) {
                       System.err.println(
-                          "Unhandled exception during listener callback: " + throwable.toString());
+                          "Unhandled exception during listener callback: " + throwable);
                       throwable.printStackTrace();
                     }
                   }
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java
index b818c7e..712f695 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java
@@ -4,38 +4,12 @@
 
 package edu.wpi.first.cscore;
 
+import edu.wpi.first.util.PixelFormat;
 import java.util.Objects;
 
 /** Video mode. */
 @SuppressWarnings("MemberName")
 public class VideoMode {
-  public enum PixelFormat {
-    kUnknown(0),
-    kMJPEG(1),
-    kYUYV(2),
-    kRGB565(3),
-    kBGR(4),
-    kGray(5),
-    kY16(6),
-    kUYVY(7);
-
-    private final int value;
-
-    PixelFormat(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  private static final PixelFormat[] m_pixelFormatValues = PixelFormat.values();
-
-  public static PixelFormat getPixelFormatFromInt(int pixelFormat) {
-    return m_pixelFormatValues[pixelFormat];
-  }
-
   /**
    * Create a new video mode.
    *
@@ -45,7 +19,7 @@
    * @param fps The camera's frames per second.
    */
   public VideoMode(int pixelFormat, int width, int height, int fps) {
-    this.pixelFormat = getPixelFormatFromInt(pixelFormat);
+    this.pixelFormat = PixelFormat.getFromInt(pixelFormat);
     this.width = width;
     this.height = height;
     this.fps = fps;
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoProperty.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoProperty.java
index 8179ba3..da436f3 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoProperty.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoProperty.java
@@ -6,11 +6,17 @@
 
 /** A source or sink property. */
 public class VideoProperty {
+  /** VideoProperty property types. */
   public enum Kind {
+    /** No specific property. */
     kNone(0),
+    /** Boolean property. */
     kBoolean(1),
+    /** Integer property. */
     kInteger(2),
+    /** String property. */
     kString(4),
+    /** Enum property. */
     kEnum(8);
 
     private final int value;
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSink.java
index 8b07f46..4540618 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSink.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSink.java
@@ -9,10 +9,15 @@
  * (e.g. from a stereo or depth camera); these are called channels.
  */
 public class VideoSink implements AutoCloseable {
+  /** Video sink types. */
   public enum Kind {
+    /** Unknown video sink type. */
     kUnknown(0),
+    /** MJPEG video sink. */
     kMjpeg(2),
+    /** CV video sink. */
     kCv(4),
+    /** Raw video sink. */
     kRaw(8);
 
     private final int value;
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSource.java
index 0ae5add..cee5eaa 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSource.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSource.java
@@ -4,16 +4,23 @@
 
 package edu.wpi.first.cscore;
 
+import edu.wpi.first.util.PixelFormat;
+
 /**
  * A source for video that provides a sequence of frames. Each frame may consist of multiple images
  * (e.g. from a stereo or depth camera); these are called channels.
  */
 public class VideoSource implements AutoCloseable {
   public enum Kind {
+    /** Unknown video source. */
     kUnknown(0),
+    /** USB video source. */
     kUsb(1),
+    /** HTTP video source. */
     kHttp(2),
+    /** CV video source. */
     kCv(4),
+    /** Raw video source. */
     kRaw(8);
 
     private final int value;
@@ -235,7 +242,7 @@
    * @param fps desired FPS
    * @return True if set successfully
    */
-  public boolean setVideoMode(VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
+  public boolean setVideoMode(PixelFormat pixelFormat, int width, int height, int fps) {
     return CameraServerJNI.setSourceVideoMode(m_handle, pixelFormat.getValue(), width, height, fps);
   }
 
@@ -245,7 +252,7 @@
    * @param pixelFormat desired pixel format
    * @return True if set successfully
    */
-  public boolean setPixelFormat(VideoMode.PixelFormat pixelFormat) {
+  public boolean setPixelFormat(PixelFormat pixelFormat) {
     return CameraServerJNI.setSourcePixelFormat(m_handle, pixelFormat.getValue());
   }
 
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawFrame.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawFrame.java
deleted file mode 100644
index 5f62481..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawFrame.java
+++ /dev/null
@@ -1,156 +0,0 @@
-// 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.
-
-package edu.wpi.first.cscore.raw;
-
-import edu.wpi.first.cscore.CameraServerJNI;
-import java.nio.ByteBuffer;
-
-/**
- * Class for storing raw frame data between image read call.
- *
- * <p>Data is reused for each frame read, rather then reallocating every frame.
- */
-public class RawFrame implements AutoCloseable {
-  private final long m_framePtr;
-  private ByteBuffer m_dataByteBuffer;
-  private long m_dataPtr;
-  private int m_totalData;
-  private int m_width;
-  private int m_height;
-  private int m_pixelFormat;
-
-  /** Construct a new RawFrame. */
-  public RawFrame() {
-    m_framePtr = CameraServerJNI.allocateRawFrame();
-  }
-
-  /**
-   * Close the RawFrame, releasing native resources. Any images currently using the data will be
-   * invalidated.
-   */
-  @Override
-  public void close() {
-    CameraServerJNI.freeRawFrame(m_framePtr);
-  }
-
-  /**
-   * Called from JNI to set data in class.
-   *
-   * @param dataByteBuffer A ByteBuffer pointing to the frame data.
-   * @param dataPtr A long (a char* in native code) pointing to the frame data.
-   * @param totalData The total length of the data stored in the frame.
-   * @param width The width of the frame.
-   * @param height The height of the frame.
-   * @param pixelFormat The PixelFormat of the frame.
-   */
-  public void setData(
-      ByteBuffer dataByteBuffer,
-      long dataPtr,
-      int totalData,
-      int width,
-      int height,
-      int pixelFormat) {
-    m_dataByteBuffer = dataByteBuffer;
-    m_dataPtr = dataPtr;
-    m_totalData = totalData;
-    m_width = width;
-    m_height = height;
-    m_pixelFormat = pixelFormat;
-  }
-
-  /**
-   * Get the pointer to native representation of this frame.
-   *
-   * @return The pointer to native representation of this frame.
-   */
-  public long getFramePtr() {
-    return m_framePtr;
-  }
-
-  /**
-   * Get a ByteBuffer pointing to the frame data. This ByteBuffer is backed by the frame directly.
-   * Its lifetime is controlled by the frame. If a new frame gets read, it will overwrite the
-   * current one.
-   *
-   * @return A ByteBuffer pointing to the frame data.
-   */
-  public ByteBuffer getDataByteBuffer() {
-    return m_dataByteBuffer;
-  }
-
-  /**
-   * Get a long (is a char* in native code) pointing to the frame data. This pointer is backed by
-   * the frame directly. Its lifetime is controlled by the frame. If a new frame gets read, it will
-   * overwrite the current one.
-   *
-   * @return A long pointing to the frame data.
-   */
-  public long getDataPtr() {
-    return m_dataPtr;
-  }
-
-  /**
-   * Get the total length of the data stored in the frame.
-   *
-   * @return The total length of the data stored in the frame.
-   */
-  public int getTotalData() {
-    return m_totalData;
-  }
-
-  /**
-   * Get the width of the frame.
-   *
-   * @return The width of the frame.
-   */
-  public int getWidth() {
-    return m_width;
-  }
-
-  /**
-   * Set the width of the frame.
-   *
-   * @param width The width of the frame.
-   */
-  public void setWidth(int width) {
-    this.m_width = width;
-  }
-
-  /**
-   * Get the height of the frame.
-   *
-   * @return The height of the frame.
-   */
-  public int getHeight() {
-    return m_height;
-  }
-
-  /**
-   * Set the height of the frame.
-   *
-   * @param height The height of the frame.
-   */
-  public void setHeight(int height) {
-    this.m_height = height;
-  }
-
-  /**
-   * Get the PixelFormat of the frame.
-   *
-   * @return The PixelFormat of the frame.
-   */
-  public int getPixelFormat() {
-    return m_pixelFormat;
-  }
-
-  /**
-   * Set the PixelFormat of the frame.
-   *
-   * @param pixelFormat The PixelFormat of the frame.
-   */
-  public void setPixelFormat(int pixelFormat) {
-    this.m_pixelFormat = pixelFormat;
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java
index ae5b0ef..babb0fd 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.cscore.CameraServerJNI;
 import edu.wpi.first.cscore.ImageSink;
+import edu.wpi.first.util.RawFrame;
 
 /**
  * A sink for user code to accept video frames as raw bytes.
@@ -32,7 +33,7 @@
    * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
    *     is in the same time base as wpi::Now(), and is in 1 us increments.
    */
-  protected long grabFrame(RawFrame frame) {
+  public long grabFrame(RawFrame frame) {
     return grabFrame(frame, 0.225);
   }
 
@@ -45,8 +46,8 @@
    * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
    *     is in the same time base as wpi::Now(), and is in 1 us increments.
    */
-  protected long grabFrame(RawFrame frame, double timeout) {
-    return CameraServerJNI.grabSinkFrameTimeout(m_handle, frame, timeout);
+  public long grabFrame(RawFrame frame, double timeout) {
+    return CameraServerJNI.grabRawSinkFrameTimeout(m_handle, frame, frame.getNativeObj(), timeout);
   }
 
   /**
@@ -57,7 +58,7 @@
    * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
    *     is in the same time base as wpi::Now(), and is in 1 us increments.
    */
-  protected long grabFrameNoTimeout(RawFrame frame) {
-    return CameraServerJNI.grabSinkFrame(m_handle, frame);
+  public long grabFrameNoTimeout(RawFrame frame) {
+    return CameraServerJNI.grabRawSinkFrame(m_handle, frame, frame.getNativeObj());
   }
 }
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java
index f1be050..1c8bc23 100644
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java
@@ -7,6 +7,9 @@
 import edu.wpi.first.cscore.CameraServerJNI;
 import edu.wpi.first.cscore.ImageSource;
 import edu.wpi.first.cscore.VideoMode;
+import edu.wpi.first.util.PixelFormat;
+import edu.wpi.first.util.RawFrame;
+import java.nio.ByteBuffer;
 
 /**
  * A source for user code to provide video frames as raw bytes.
@@ -35,7 +38,7 @@
    * @param height height
    * @param fps fps
    */
-  public RawSource(String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
+  public RawSource(String name, PixelFormat pixelFormat, int width, int height, int fps) {
     super(CameraServerJNI.createRawSource(name, pixelFormat.getValue(), width, height, fps));
   }
 
@@ -44,35 +47,41 @@
    *
    * @param image raw frame image
    */
-  protected void putFrame(RawFrame image) {
-    CameraServerJNI.putRawSourceFrame(m_handle, image);
+  public void putFrame(RawFrame image) {
+    CameraServerJNI.putRawSourceFrame(m_handle, image.getNativeObj());
   }
 
   /**
    * Put a raw image and notify sinks.
    *
-   * @param data raw frame data pointer
+   * @param data raw frame native data pointer
+   * @param size total size in bytes
    * @param width frame width
    * @param height frame height
+   * @param stride size of each row in bytes
    * @param pixelFormat pixel format
-   * @param totalData length of data in total
-   */
-  protected void putFrame(long data, int width, int height, int pixelFormat, int totalData) {
-    CameraServerJNI.putRawSourceFrame(m_handle, data, width, height, pixelFormat, totalData);
-  }
-
-  /**
-   * Put a raw image and notify sinks.
-   *
-   * @param data raw frame data pointer
-   * @param width frame width
-   * @param height frame height
-   * @param pixelFormat pixel format
-   * @param totalData length of data in total
    */
   protected void putFrame(
-      long data, int width, int height, VideoMode.PixelFormat pixelFormat, int totalData) {
-    CameraServerJNI.putRawSourceFrame(
-        m_handle, data, width, height, pixelFormat.getValue(), totalData);
+      long data, int size, int width, int height, int stride, PixelFormat pixelFormat) {
+    CameraServerJNI.putRawSourceFrameData(
+        m_handle, data, size, width, height, stride, pixelFormat.getValue());
+  }
+
+  /**
+   * Put a raw image and notify sinks.
+   *
+   * @param data raw frame native ByteBuffer
+   * @param width frame width
+   * @param height frame height
+   * @param stride size of each row in bytes
+   * @param pixelFormat pixel format
+   */
+  public void putFrame(
+      ByteBuffer data, int width, int height, int stride, PixelFormat pixelFormat) {
+    if (!data.isDirect()) {
+      throw new UnsupportedOperationException("ByteBuffer must be direct");
+    }
+    CameraServerJNI.putRawSourceFrameBB(
+        m_handle, data, data.limit(), width, height, stride, pixelFormat.getValue());
   }
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp
index 942fdaf..a5447cc 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp
@@ -19,16 +19,18 @@
 using namespace cs;
 
 CvSinkImpl::CvSinkImpl(std::string_view name, wpi::Logger& logger,
-                       Notifier& notifier, Telemetry& telemetry)
-    : SinkImpl{name, logger, notifier, telemetry} {
+                       Notifier& notifier, Telemetry& telemetry,
+                       VideoMode::PixelFormat pixelFormat)
+    : SinkImpl{name, logger, notifier, telemetry}, m_pixelFormat{pixelFormat} {
   m_active = true;
   // m_thread = std::thread(&CvSinkImpl::ThreadMain, this);
 }
 
 CvSinkImpl::CvSinkImpl(std::string_view name, wpi::Logger& logger,
                        Notifier& notifier, Telemetry& telemetry,
+                       VideoMode::PixelFormat pixelFormat,
                        std::function<void(uint64_t time)> processFrame)
-    : SinkImpl{name, logger, notifier, telemetry} {}
+    : SinkImpl{name, logger, notifier, telemetry}, m_pixelFormat{pixelFormat} {}
 
 CvSinkImpl::~CvSinkImpl() {
   Stop();
@@ -65,7 +67,7 @@
     return 0;  // signal error
   }
 
-  if (!frame.GetCv(image)) {
+  if (!frame.GetCv(image, m_pixelFormat)) {
     // Shouldn't happen, but just in case...
     std::this_thread::sleep_for(std::chrono::milliseconds(20));
     return 0;
@@ -91,7 +93,7 @@
     return 0;  // signal error
   }
 
-  if (!frame.GetCv(image)) {
+  if (!frame.GetCv(image, m_pixelFormat)) {
     // Shouldn't happen, but just in case...
     std::this_thread::sleep_for(std::chrono::milliseconds(20));
     return 0;
@@ -127,20 +129,23 @@
 
 namespace cs {
 
-CS_Sink CreateCvSink(std::string_view name, CS_Status* status) {
+CS_Sink CreateCvSink(std::string_view name, VideoMode::PixelFormat pixelFormat,
+                     CS_Status* status) {
   auto& inst = Instance::GetInstance();
   return inst.CreateSink(
       CS_SINK_CV, std::make_shared<CvSinkImpl>(name, inst.logger, inst.notifier,
-                                               inst.telemetry));
+                                               inst.telemetry, pixelFormat));
 }
 
 CS_Sink CreateCvSinkCallback(std::string_view name,
+                             VideoMode::PixelFormat pixelFormat,
                              std::function<void(uint64_t time)> processFrame,
                              CS_Status* status) {
   auto& inst = Instance::GetInstance();
   return inst.CreateSink(
-      CS_SINK_CV, std::make_shared<CvSinkImpl>(name, inst.logger, inst.notifier,
-                                               inst.telemetry, processFrame));
+      CS_SINK_CV,
+      std::make_shared<CvSinkImpl>(name, inst.logger, inst.notifier,
+                                   inst.telemetry, pixelFormat, processFrame));
 }
 
 static constexpr unsigned SinkMask = CS_SINK_CV | CS_SINK_RAW;
@@ -206,15 +211,19 @@
 
 extern "C" {
 
-CS_Sink CS_CreateCvSink(const char* name, CS_Status* status) {
-  return cs::CreateCvSink(name, status);
+CS_Sink CS_CreateCvSink(const char* name, enum WPI_PixelFormat pixelFormat,
+                        CS_Status* status) {
+  return cs::CreateCvSink(
+      name, static_cast<VideoMode::PixelFormat>(pixelFormat), status);
 }
 
-CS_Sink CS_CreateCvSinkCallback(const char* name, void* data,
+CS_Sink CS_CreateCvSinkCallback(const char* name,
+                                enum WPI_PixelFormat pixelFormat, void* data,
                                 void (*processFrame)(void* data, uint64_t time),
                                 CS_Status* status) {
   return cs::CreateCvSinkCallback(
-      name, [=](uint64_t time) { processFrame(data, time); }, status);
+      name, static_cast<VideoMode::PixelFormat>(pixelFormat),
+      [=](uint64_t time) { processFrame(data, time); }, status);
 }
 
 void CS_SetSinkDescription(CS_Sink sink, const char* description,
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h
index ad63a20..da9392f 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h
@@ -25,9 +25,9 @@
 class CvSinkImpl : public SinkImpl {
  public:
   CvSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
-             Telemetry& telemetry);
+             Telemetry& telemetry, VideoMode::PixelFormat pixelFormat);
   CvSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
-             Telemetry& telemetry,
+             Telemetry& telemetry, VideoMode::PixelFormat pixelFormat,
              std::function<void(uint64_t time)> processFrame);
   ~CvSinkImpl() override;
 
@@ -42,6 +42,7 @@
   std::atomic_bool m_active;  // set to false to terminate threads
   std::thread m_thread;
   std::function<void(uint64_t time)> m_processFrame;
+  VideoMode::PixelFormat m_pixelFormat;
 };
 
 }  // namespace cs
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp
index e6f8e7c..1a30393 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp
@@ -709,8 +709,9 @@
   return ConvertImpl(cur, pixelFormat, requiredJpegQuality, defaultJpegQuality);
 }
 
-bool Frame::GetCv(cv::Mat& image, int width, int height) {
-  Image* rawImage = GetImage(width, height, VideoMode::kBGR);
+bool Frame::GetCv(cv::Mat& image, int width, int height,
+                  VideoMode::PixelFormat pixelFormat) {
+  Image* rawImage = GetImage(width, height, pixelFormat);
   if (!rawImage) {
     return false;
   }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h
index d5f5373..6132463 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h
@@ -219,10 +219,11 @@
                         defaultQuality);
   }
 
-  bool GetCv(cv::Mat& image) {
-    return GetCv(image, GetOriginalWidth(), GetOriginalHeight());
+  bool GetCv(cv::Mat& image, VideoMode::PixelFormat pixelFormat) {
+    return GetCv(image, GetOriginalWidth(), GetOriginalHeight(), pixelFormat);
   }
-  bool GetCv(cv::Mat& image, int width, int height);
+  bool GetCv(cv::Mat& image, int width, int height,
+             VideoMode::PixelFormat pixelFormat);
 
  private:
   Image* ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Image.h b/third_party/allwpilib/cscore/src/main/native/cpp/Image.h
index 83cbe89..b66a7f1 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Image.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Image.h
@@ -72,6 +72,23 @@
     return cv::Mat{height, width, type, m_data.data()};
   }
 
+  int GetStride() const {
+    switch (pixelFormat) {
+      case VideoMode::kYUYV:
+      case VideoMode::kRGB565:
+      case VideoMode::kY16:
+      case VideoMode::kUYVY:
+        return 2 * width;
+      case VideoMode::kBGR:
+        return 3 * width;
+      case VideoMode::kGray:
+        return width;
+      case VideoMode::kMJPEG:
+      default:
+        return 0;
+    }
+  }
+
   cv::_InputArray AsInputArray() { return cv::_InputArray{m_data}; }
 
   bool Is(int width_, int height_) {
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp
index 31c57d2..7711641 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp
@@ -40,7 +40,7 @@
   }
 }
 
-uint64_t RawSinkImpl::GrabFrame(CS_RawFrame& image) {
+uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image) {
   SetEnabled(true);
 
   auto source = GetSource();
@@ -60,7 +60,7 @@
   return GrabFrameImpl(image, frame);
 }
 
-uint64_t RawSinkImpl::GrabFrame(CS_RawFrame& image, double timeout) {
+uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image, double timeout) {
   SetEnabled(true);
 
   auto source = GetSource();
@@ -80,11 +80,11 @@
   return GrabFrameImpl(image, frame);
 }
 
-uint64_t RawSinkImpl::GrabFrameImpl(CS_RawFrame& rawFrame,
+uint64_t RawSinkImpl::GrabFrameImpl(WPI_RawFrame& rawFrame,
                                     Frame& incomingFrame) {
   Image* newImage = nullptr;
 
-  if (rawFrame.pixelFormat == CS_PixelFormat::CS_PIXFMT_UNKNOWN) {
+  if (rawFrame.pixelFormat == WPI_PixelFormat::WPI_PIXFMT_UNKNOWN) {
     // Always get incoming image directly on unknown
     newImage = incomingFrame.GetExistingImage(0);
   } else {
@@ -106,13 +106,13 @@
     return 0;
   }
 
-  CS_AllocateRawFrameData(&rawFrame, newImage->size());
+  WPI_AllocateRawFrameData(&rawFrame, newImage->size());
   rawFrame.height = newImage->height;
   rawFrame.width = newImage->width;
+  rawFrame.stride = newImage->GetStride();
   rawFrame.pixelFormat = newImage->pixelFormat;
-  rawFrame.totalData = newImage->size();
-  std::copy(newImage->data(), newImage->data() + rawFrame.totalData,
-            rawFrame.data);
+  rawFrame.size = newImage->size();
+  std::copy(newImage->data(), newImage->data() + rawFrame.size, rawFrame.data);
 
   return incomingFrame.GetTime();
 }
@@ -159,7 +159,7 @@
                                           inst.telemetry, processFrame));
 }
 
-uint64_t GrabSinkFrame(CS_Sink sink, CS_RawFrame& image, CS_Status* status) {
+uint64_t GrabSinkFrame(CS_Sink sink, WPI_RawFrame& image, CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data || data->kind != CS_SINK_RAW) {
     *status = CS_INVALID_HANDLE;
@@ -168,7 +168,7 @@
   return static_cast<RawSinkImpl&>(*data->sink).GrabFrame(image);
 }
 
-uint64_t GrabSinkFrameTimeout(CS_Sink sink, CS_RawFrame& image, double timeout,
+uint64_t GrabSinkFrameTimeout(CS_Sink sink, WPI_RawFrame& image, double timeout,
                               CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data || data->kind != CS_SINK_RAW) {
@@ -192,12 +192,12 @@
       name, [=](uint64_t time) { processFrame(data, time); }, status);
 }
 
-uint64_t CS_GrabRawSinkFrame(CS_Sink sink, struct CS_RawFrame* image,
+uint64_t CS_GrabRawSinkFrame(CS_Sink sink, struct WPI_RawFrame* image,
                              CS_Status* status) {
   return cs::GrabSinkFrame(sink, *image, status);
 }
 
-uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct CS_RawFrame* image,
+uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct WPI_RawFrame* image,
                                     double timeout, CS_Status* status) {
   return cs::GrabSinkFrameTimeout(sink, *image, timeout, status);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h
index 6e8032a..ac71976 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h
@@ -32,13 +32,13 @@
 
   void Stop();
 
-  uint64_t GrabFrame(CS_RawFrame& frame);
-  uint64_t GrabFrame(CS_RawFrame& frame, double timeout);
+  uint64_t GrabFrame(WPI_RawFrame& frame);
+  uint64_t GrabFrame(WPI_RawFrame& frame, double timeout);
 
  private:
   void ThreadMain();
 
-  uint64_t GrabFrameImpl(CS_RawFrame& rawFrame, Frame& incomingFrame);
+  uint64_t GrabFrameImpl(WPI_RawFrame& rawFrame, Frame& incomingFrame);
 
   std::atomic_bool m_active;  // set to false to terminate threads
   std::thread m_thread;
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp
index bad26a0..0d53f89 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp
@@ -21,7 +21,7 @@
 
 RawSourceImpl::~RawSourceImpl() = default;
 
-void RawSourceImpl::PutFrame(const CS_RawFrame& image) {
+void RawSourceImpl::PutFrame(const WPI_RawFrame& image) {
   int type;
   switch (image.pixelFormat) {
     case VideoMode::kYUYV:
@@ -39,10 +39,11 @@
       type = CV_8UC1;
       break;
   }
-  cv::Mat finalImage{image.height, image.width, type, image.data};
+  cv::Mat finalImage{image.height, image.width, type, image.data,
+                     static_cast<size_t>(image.stride)};
   std::unique_ptr<Image> dest =
       AllocImage(static_cast<VideoMode::PixelFormat>(image.pixelFormat),
-                 image.width, image.height, image.totalData);
+                 image.width, image.height, image.size);
   finalImage.copyTo(dest->AsMat());
 
   SourceImpl::PutFrame(std::move(dest), wpi::Now());
@@ -57,7 +58,7 @@
                                               inst.telemetry, mode));
 }
 
-void PutSourceFrame(CS_Source source, const CS_RawFrame& image,
+void PutSourceFrame(CS_Source source, const WPI_RawFrame& image,
                     CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || data->kind != CS_SOURCE_RAW) {
@@ -75,7 +76,7 @@
                              status);
 }
 
-void CS_PutRawSourceFrame(CS_Source source, const struct CS_RawFrame* image,
+void CS_PutRawSourceFrame(CS_Source source, const struct WPI_RawFrame* image,
                           CS_Status* status) {
   return cs::PutSourceFrame(source, *image, status);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h
index 5887ed4..44a20b9 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h
@@ -25,7 +25,7 @@
   ~RawSourceImpl() override;
 
   // Raw-specific functions
-  void PutFrame(const CS_RawFrame& image);
+  void PutFrame(const WPI_RawFrame& image);
 
  private:
   std::atomic_bool m_connected{true};
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp
index 3fbbfcd..45583ac 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp
@@ -181,7 +181,7 @@
 }
 
 CS_Bool CS_SetSourceVideoModeDiscrete(CS_Source source,
-                                      enum CS_PixelFormat pixelFormat,
+                                      enum WPI_PixelFormat pixelFormat,
                                       int width, int height, int fps,
                                       CS_Status* status) {
   return cs::SetSourceVideoMode(
@@ -193,7 +193,7 @@
 }
 
 CS_Bool CS_SetSourcePixelFormat(CS_Source source,
-                                enum CS_PixelFormat pixelFormat,
+                                enum WPI_PixelFormat pixelFormat,
                                 CS_Status* status) {
   return cs::SetSourcePixelFormat(
       source,
@@ -541,25 +541,4 @@
   std::free(interfaces);
 }
 
-void CS_AllocateRawFrameData(CS_RawFrame* frame, int requestedSize) {
-  if (frame->dataLength >= requestedSize) {
-    return;
-  }
-  if (frame->data) {
-    frame->data =
-        static_cast<char*>(wpi::safe_realloc(frame->data, requestedSize));
-  } else {
-    frame->data = static_cast<char*>(wpi::safe_malloc(requestedSize));
-  }
-  frame->dataLength = requestedSize;
-}
-
-void CS_FreeRawFrameData(CS_RawFrame* frame) {
-  if (frame->data) {
-    std::free(frame->data);
-    frame->data = nullptr;
-    frame->dataLength = 0;
-  }
-}
-
 }  // extern "C"
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
index ddd0ff5..d7a9b45 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
@@ -7,10 +7,12 @@
 
 #include <fmt/format.h>
 #include <opencv2/core/core.hpp>
+
+#define WPI_RAWFRAME_JNI
+#include <wpi/RawFrame.h>
 #include <wpi/SmallString.h>
 #include <wpi/jni_util.h>
 
-#include "cscore_cpp.h"
 #include "cscore_cv.h"
 #include "cscore_raw.h"
 #include "cscore_runloop.h"
@@ -44,7 +46,7 @@
     {"edu/wpi/first/cscore/UsbCameraInfo", &usbCameraInfoCls},
     {"edu/wpi/first/cscore/VideoMode", &videoModeCls},
     {"edu/wpi/first/cscore/VideoEvent", &videoEventCls},
-    {"edu/wpi/first/cscore/raw/RawFrame", &rawFrameCls}};
+    {"edu/wpi/first/util/RawFrame", &rawFrameCls}};
 
 static const JExceptionInit exceptions[] = {
     {"edu/wpi/first/cscore/VideoException", &videoEx},
@@ -1221,48 +1223,78 @@
   }
 }
 
-// int width, int height, int pixelFormat, int totalData
-
 /*
  * Class:     edu_wpi_first_cscore_CameraServerJNI
- * Method:    putRawSourceFrameBB
- * Signature: (ILjava/lang/Object;IIII)V
+ * Method:    putRawSourceFrame
+ * Signature: (IJ)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameBB
-  (JNIEnv* env, jclass, jint source, jobject byteBuffer, jint width,
-   jint height, jint pixelFormat, jint totalData)
+Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrame
+  (JNIEnv* env, jclass, jint source, jlong framePtr)
 {
-  CS_RawFrame rawFrame;
-  rawFrame.data =
-      reinterpret_cast<char*>(env->GetDirectBufferAddress(byteBuffer));
-  rawFrame.totalData = totalData;
-  rawFrame.pixelFormat = pixelFormat;
-  rawFrame.width = width;
-  rawFrame.height = height;
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  if (!frame) {
+    nullPointerEx.Throw(env, "frame is null");
+    return;
+  }
   CS_Status status = 0;
-  cs::PutSourceFrame(source, rawFrame, &status);
+  cs::PutSourceFrame(source, *frame, &status);
   CheckStatus(env, status);
 }
 
 /*
  * Class:     edu_wpi_first_cscore_CameraServerJNI
- * Method:    putRawSourceFrame
- * Signature: (IJIIII)V
+ * Method:    putRawSourceFrameBB
+ * Signature: (ILjava/lang/Object;IIIII)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrame
-  (JNIEnv* env, jclass, jint source, jlong ptr, jint width, jint height,
-   jint pixelFormat, jint totalData)
+Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameBB
+  (JNIEnv* env, jclass, jint source, jobject data, jint size, jint width,
+   jint height, jint stride, jint pixelFormat)
 {
-  CS_RawFrame rawFrame;
-  rawFrame.data = reinterpret_cast<char*>(static_cast<intptr_t>(ptr));
-  rawFrame.totalData = totalData;
-  rawFrame.pixelFormat = pixelFormat;
-  rawFrame.width = width;
-  rawFrame.height = height;
+  WPI_RawFrame frame;  // use WPI_Frame because we don't want the destructor
+  frame.data = static_cast<uint8_t*>(env->GetDirectBufferAddress(data));
+  if (!frame.data) {
+    nullPointerEx.Throw(env, "data is null");
+    return;
+  }
+  frame.freeFunc = nullptr;
+  frame.freeCbData = nullptr;
+  frame.size = size;
+  frame.width = width;
+  frame.height = height;
+  frame.stride = stride;
+  frame.pixelFormat = pixelFormat;
   CS_Status status = 0;
-  cs::PutSourceFrame(source, rawFrame, &status);
+  cs::PutSourceFrame(source, frame, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    putRawSourceFrameData
+ * Signature: (IJIIIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameData
+  (JNIEnv* env, jclass, jint source, jlong data, jint size, jint width,
+   jint height, jint stride, jint pixelFormat)
+{
+  WPI_RawFrame frame;  // use WPI_Frame because we don't want the destructor
+  frame.data = reinterpret_cast<uint8_t*>(data);
+  if (!frame.data) {
+    nullPointerEx.Throw(env, "data is null");
+    return;
+  }
+  frame.freeFunc = nullptr;
+  frame.freeCbData = nullptr;
+  frame.size = size;
+  frame.width = width;
+  frame.height = height;
+  frame.stride = stride;
+  frame.pixelFormat = pixelFormat;
+  CS_Status status = 0;
+  cs::PutSourceFrame(source, frame, &status);
   CheckStatus(env, status);
 }
 
@@ -1392,18 +1424,20 @@
 /*
  * Class:     edu_wpi_first_cscore_CameraServerCvJNI
  * Method:    createCvSink
- * Signature: (Ljava/lang/String;)I
+ * Signature: (Ljava/lang/String;I)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_cscore_CameraServerCvJNI_createCvSink
-  (JNIEnv* env, jclass, jstring name)
+  (JNIEnv* env, jclass, jstring name, jint pixelFormat)
 {
   if (!name) {
     nullPointerEx.Throw(env, "name cannot be null");
     return 0;
   }
   CS_Status status = 0;
-  auto val = cs::CreateCvSink(JStringRef{env, name}.str(), &status);
+  auto val = cs::CreateCvSink(
+      JStringRef{env, name}.str(),
+      static_cast<cs::VideoMode::PixelFormat>(pixelFormat), &status);
   CheckStatus(env, status);
   return val;
 }
@@ -1721,72 +1755,47 @@
   }
 }
 
-static void SetRawFrameData(JNIEnv* env, jobject rawFrameObj,
-                            jobject byteBuffer, bool didChangeDataPtr,
-                            const CS_RawFrame& frame) {
-  static jmethodID setMethod =
-      env->GetMethodID(rawFrameCls, "setData", "(Ljava/nio/ByteBuffer;JIIII)V");
-  jlong framePtr = static_cast<jlong>(reinterpret_cast<intptr_t>(frame.data));
-
-  if (didChangeDataPtr) {
-    byteBuffer = env->NewDirectByteBuffer(frame.data, frame.dataLength);
-  }
-
-  env->CallVoidMethod(
-      rawFrameObj, setMethod, byteBuffer, framePtr,
-      static_cast<jint>(frame.totalData), static_cast<jint>(frame.width),
-      static_cast<jint>(frame.height), static_cast<jint>(frame.pixelFormat));
-}
-
 /*
  * Class:     edu_wpi_first_cscore_CameraServerJNI
- * Method:    grabRawSinkFrameImpl
- * Signature: (ILjava/lang/Object;JLjava/lang/Object;III)J
+ * Method:    grabRawSinkFrame
+ * Signature: (ILjava/lang/Object;J)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameImpl
-  (JNIEnv* env, jclass, jint sink, jobject rawFrameObj, jlong rawFramePtr,
-   jobject byteBuffer, jint width, jint height, jint pixelFormat)
+Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrame
+  (JNIEnv* env, jclass, jint sink, jobject frameObj, jlong framePtr)
 {
-  CS_RawFrame* ptr =
-      reinterpret_cast<CS_RawFrame*>(static_cast<intptr_t>(rawFramePtr));
-  auto origDataPtr = ptr->data;
-  ptr->width = width;
-  ptr->height = height;
-  ptr->pixelFormat = pixelFormat;
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  auto origData = frame->data;
   CS_Status status = 0;
-  auto rv = cs::GrabSinkFrame(static_cast<CS_Sink>(sink), *ptr, &status);
+  auto rv = cs::GrabSinkFrame(static_cast<CS_Sink>(sink), *frame, &status);
   if (!CheckStatus(env, status)) {
     return 0;
   }
-  SetRawFrameData(env, rawFrameObj, byteBuffer, origDataPtr != ptr->data, *ptr);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame,
+                    origData != frame->data);
   return rv;
 }
 
 /*
  * Class:     edu_wpi_first_cscore_CameraServerJNI
- * Method:    grabRawSinkFrameTimeoutImpl
- * Signature: (ILjava/lang/Object;JLjava/lang/Object;IIID)J
+ * Method:    grabRawSinkFrameTimeout
+ * Signature: (ILjava/lang/Object;JD)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameTimeoutImpl
-  (JNIEnv* env, jclass, jint sink, jobject rawFrameObj, jlong rawFramePtr,
-   jobject byteBuffer, jint width, jint height, jint pixelFormat,
+Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameTimeout
+  (JNIEnv* env, jclass, jint sink, jobject frameObj, jlong framePtr,
    jdouble timeout)
 {
-  CS_RawFrame* ptr =
-      reinterpret_cast<CS_RawFrame*>(static_cast<intptr_t>(rawFramePtr));
-  auto origDataPtr = ptr->data;
-  ptr->width = width;
-  ptr->height = height;
-  ptr->pixelFormat = pixelFormat;
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  auto origData = frame->data;
   CS_Status status = 0;
-  auto rv = cs::GrabSinkFrameTimeout(static_cast<CS_Sink>(sink), *ptr, timeout,
-                                     &status);
+  auto rv = cs::GrabSinkFrameTimeout(static_cast<CS_Sink>(sink), *frame,
+                                     timeout, &status);
   if (!CheckStatus(env, status)) {
     return 0;
   }
-  SetRawFrameData(env, rawFrameObj, byteBuffer, origDataPtr != ptr->data, *ptr);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame,
+                    origData != frame->data);
   return rv;
 }
 
@@ -2201,34 +2210,6 @@
 
 /*
  * Class:     edu_wpi_first_cscore_CameraServerJNI
- * Method:    allocateRawFrame
- * Signature: ()J
- */
-JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_cscore_CameraServerJNI_allocateRawFrame
-  (JNIEnv*, jclass)
-{
-  cs::RawFrame* rawFrame = new cs::RawFrame{};
-  intptr_t rawFrameIntPtr = reinterpret_cast<intptr_t>(rawFrame);
-  return static_cast<jlong>(rawFrameIntPtr);
-}
-
-/*
- * Class:     edu_wpi_first_cscore_CameraServerJNI
- * Method:    freeRawFrame
- * Signature: (J)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_cscore_CameraServerJNI_freeRawFrame
-  (JNIEnv*, jclass, jlong rawFrame)
-{
-  cs::RawFrame* ptr =
-      reinterpret_cast<cs::RawFrame*>(static_cast<intptr_t>(rawFrame));
-  delete ptr;
-}
-
-/*
- * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    runMainRunLoop
  * Signature: ()V
  */
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h
index 3d30a0e..277d0ff 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h
@@ -7,6 +7,8 @@
 
 #include <stdint.h>
 
+#include <wpi/RawFrame.h>
+
 #ifdef __cplusplus
 #include <cstddef>
 #else
@@ -85,20 +87,6 @@
 };
 
 /**
- * Pixel formats
- */
-enum CS_PixelFormat {
-  CS_PIXFMT_UNKNOWN = 0,
-  CS_PIXFMT_MJPEG,
-  CS_PIXFMT_YUYV,
-  CS_PIXFMT_RGB565,
-  CS_PIXFMT_BGR,
-  CS_PIXFMT_GRAY,
-  CS_PIXFMT_Y16,
-  CS_PIXFMT_UYVY
-};
-
-/**
  * Video mode
  */
 typedef struct CS_VideoMode {
@@ -302,11 +290,11 @@
 CS_Bool CS_SetSourceVideoMode(CS_Source source, const CS_VideoMode* mode,
                               CS_Status* status);
 CS_Bool CS_SetSourceVideoModeDiscrete(CS_Source source,
-                                      enum CS_PixelFormat pixelFormat,
+                                      enum WPI_PixelFormat pixelFormat,
                                       int width, int height, int fps,
                                       CS_Status* status);
 CS_Bool CS_SetSourcePixelFormat(CS_Source source,
-                                enum CS_PixelFormat pixelFormat,
+                                enum WPI_PixelFormat pixelFormat,
                                 CS_Status* status);
 CS_Bool CS_SetSourceResolution(CS_Source source, int width, int height,
                                CS_Status* status);
@@ -382,8 +370,10 @@
  */
 CS_Sink CS_CreateMjpegServer(const char* name, const char* listenAddress,
                              int port, CS_Status* status);
-CS_Sink CS_CreateCvSink(const char* name, CS_Status* status);
-CS_Sink CS_CreateCvSinkCallback(const char* name, void* data,
+CS_Sink CS_CreateCvSink(const char* name, enum WPI_PixelFormat pixelFormat,
+                        CS_Status* status);
+CS_Sink CS_CreateCvSinkCallback(const char* name,
+                                enum WPI_PixelFormat pixelFormat, void* data,
                                 void (*processFrame)(void* data, uint64_t time),
                                 CS_Status* status);
 /** @} */
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h
index 72c37f8..1d15039 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h
@@ -13,6 +13,7 @@
 #include <string_view>
 #include <vector>
 
+#include <wpi/RawFrame.h>
 #include <wpi/SmallVector.h>
 #include <wpi/json_fwd.h>
 
@@ -60,14 +61,14 @@
  */
 struct VideoMode : public CS_VideoMode {
   enum PixelFormat {
-    kUnknown = CS_PIXFMT_UNKNOWN,
-    kMJPEG = CS_PIXFMT_MJPEG,
-    kYUYV = CS_PIXFMT_YUYV,
-    kRGB565 = CS_PIXFMT_RGB565,
-    kBGR = CS_PIXFMT_BGR,
-    kGray = CS_PIXFMT_GRAY,
-    kY16 = CS_PIXFMT_Y16,
-    kUYVY = CS_PIXFMT_UYVY
+    kUnknown = WPI_PIXFMT_UNKNOWN,
+    kMJPEG = WPI_PIXFMT_MJPEG,
+    kYUYV = WPI_PIXFMT_YUYV,
+    kRGB565 = WPI_PIXFMT_RGB565,
+    kBGR = WPI_PIXFMT_BGR,
+    kGray = WPI_PIXFMT_GRAY,
+    kY16 = WPI_PIXFMT_Y16,
+    kUYVY = WPI_PIXFMT_UYVY
   };
   VideoMode() {
     pixelFormat = 0;
@@ -316,8 +317,10 @@
  */
 CS_Sink CreateMjpegServer(std::string_view name, std::string_view listenAddress,
                           int port, CS_Status* status);
-CS_Sink CreateCvSink(std::string_view name, CS_Status* status);
+CS_Sink CreateCvSink(std::string_view name, VideoMode::PixelFormat pixelFormat,
+                     CS_Status* status);
 CS_Sink CreateCvSinkCallback(std::string_view name,
+                             VideoMode::PixelFormat pixelFormat,
                              std::function<void(uint64_t time)> processFrame,
                              CS_Status* status);
 
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h
index 66cb23e..6fdfdfb 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h
@@ -127,8 +127,10 @@
    * image.
    *
    * @param name Source name (arbitrary unique identifier)
+   * @param pixelFormat Source pixel format
    */
-  explicit CvSink(std::string_view name);
+  explicit CvSink(std::string_view name, VideoMode::PixelFormat pixelFormat =
+                                             VideoMode::PixelFormat::kBGR);
 
   /**
    * Create a sink for accepting OpenCV images in a separate thread.
@@ -141,9 +143,10 @@
    *        time=0 if an error occurred.  processFrame should call GetImage()
    *        or GetError() as needed, but should not call (except in very
    *        unusual circumstances) WaitForImage().
+   * @param pixelFormat Source pixel format
    */
-  CvSink(std::string_view name,
-         std::function<void(uint64_t time)> processFrame);
+  CvSink(std::string_view name, std::function<void(uint64_t time)> processFrame,
+         VideoMode::PixelFormat pixelFormat = VideoMode::PixelFormat::kBGR);
 
   /**
    * Wait for the next frame and get the image.
@@ -184,13 +187,15 @@
   PutSourceFrame(m_handle, image, &m_status);
 }
 
-inline CvSink::CvSink(std::string_view name) {
-  m_handle = CreateCvSink(name, &m_status);
+inline CvSink::CvSink(std::string_view name,
+                      VideoMode::PixelFormat pixelFormat) {
+  m_handle = CreateCvSink(name, pixelFormat, &m_status);
 }
 
 inline CvSink::CvSink(std::string_view name,
-                      std::function<void(uint64_t time)> processFrame) {
-  m_handle = CreateCvSinkCallback(name, processFrame, &m_status);
+                      std::function<void(uint64_t time)> processFrame,
+                      VideoMode::PixelFormat pixelFormat) {
+  m_handle = CreateCvSinkCallback(name, pixelFormat, processFrame, &m_status);
 }
 
 inline uint64_t CvSink::GrabFrame(cv::Mat& image, double timeout) const {
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h
index f13a6fd..6fca373 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h
@@ -44,10 +44,15 @@
 
  public:
   enum Kind {
+    /// No specific property.
     kNone = CS_PROP_NONE,
+    /// Boolean property.
     kBoolean = CS_PROP_BOOLEAN,
+    /// Integer property.
     kInteger = CS_PROP_INTEGER,
+    /// String property.
     kString = CS_PROP_STRING,
+    /// Enum property.
     kEnum = CS_PROP_ENUM
   };
 
@@ -100,9 +105,13 @@
 
  public:
   enum Kind {
+    /// Unknown video source.
     kUnknown = CS_SOURCE_UNKNOWN,
+    /// USB video source.
     kUsb = CS_SOURCE_USB,
+    /// HTTP video source.
     kHttp = CS_SOURCE_HTTP,
+    /// CV video source.
     kCv = CS_SOURCE_CV
   };
 
@@ -471,9 +480,13 @@
 class HttpCamera : public VideoCamera {
  public:
   enum HttpCameraKind {
+    /// Unknown camera kind.
     kUnknown = CS_HTTP_UNKNOWN,
+    /// MJPG Streamer camera.
     kMJPGStreamer = CS_HTTP_MJPGSTREAMER,
+    /// CS Core camera.
     kCSCore = CS_HTTP_CSCORE,
+    /// Axis camera.
     kAxis = CS_HTTP_AXIS
   };
 
@@ -716,8 +729,11 @@
 
  public:
   enum Kind {
+    /// Unknown sink type.
     kUnknown = CS_SINK_UNKNOWN,
+    /// MJPEG video sink.
     kMjpeg = CS_SINK_MJPEG,
+    /// CV video sink.
     kCv = CS_SINK_CV
   };
 
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h
index 21f4009..6d49612 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h
@@ -9,36 +9,23 @@
 
 #include "cscore_c.h"
 
+// NOLINTBEGIN
 #ifdef __cplusplus
 #include "cscore_oo.h"
 #endif
 
-/**
- * Raw Frame
- */
-typedef struct CS_RawFrame {  // NOLINT
-  char* data;
-  int dataLength;
-  int pixelFormat;
-  int width;
-  int height;
-  int totalData;
-} CS_RawFrame;
-
 #ifdef __cplusplus
 extern "C" {
 #endif
+// NOLINTEND
 
 /**
  * @defgroup cscore_raw_cfunc Raw Image Functions
  * @{
  */
-void CS_AllocateRawFrameData(CS_RawFrame* frame, int requestedSize);
-void CS_FreeRawFrameData(CS_RawFrame* frame);
-
-uint64_t CS_GrabRawSinkFrame(CS_Sink sink, struct CS_RawFrame* rawImage,
+uint64_t CS_GrabRawSinkFrame(CS_Sink sink, struct WPI_RawFrame* rawImage,
                              CS_Status* status);
-uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct CS_RawFrame* rawImage,
+uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct WPI_RawFrame* rawImage,
                                     double timeout, CS_Status* status);
 
 CS_Sink CS_CreateRawSink(const char* name, CS_Status* status);
@@ -48,7 +35,7 @@
                                                       uint64_t time),
                                  CS_Status* status);
 
-void CS_PutRawSourceFrame(CS_Source source, const struct CS_RawFrame* image,
+void CS_PutRawSourceFrame(CS_Source source, const struct WPI_RawFrame* image,
                           CS_Status* status);
 
 CS_Source CS_CreateRawSource(const char* name, const CS_VideoMode* mode,
@@ -62,19 +49,6 @@
 #ifdef __cplusplus
 namespace cs {
 
-struct RawFrame : public CS_RawFrame {
-  RawFrame() {
-    data = nullptr;
-    dataLength = 0;
-    pixelFormat = CS_PIXFMT_UNKNOWN;
-    width = 0;
-    height = 0;
-    totalData = 0;
-  }
-
-  ~RawFrame() { CS_FreeRawFrameData(this); }
-};
-
 /**
  * @defgroup cscore_raw_func Raw Image Functions
  * @{
@@ -88,10 +62,10 @@
                               std::function<void(uint64_t time)> processFrame,
                               CS_Status* status);
 
-void PutSourceFrame(CS_Source source, const CS_RawFrame& image,
+void PutSourceFrame(CS_Source source, const WPI_RawFrame& image,
                     CS_Status* status);
-uint64_t GrabSinkFrame(CS_Sink sink, CS_RawFrame& image, CS_Status* status);
-uint64_t GrabSinkFrameTimeout(CS_Sink sink, CS_RawFrame& image, double timeout,
+uint64_t GrabSinkFrame(CS_Sink sink, WPI_RawFrame& image, CS_Status* status);
+uint64_t GrabSinkFrameTimeout(CS_Sink sink, WPI_RawFrame& image, double timeout,
                               CS_Status* status);
 
 /**
@@ -129,7 +103,7 @@
    *
    * @param image raw frame image
    */
-  void PutFrame(RawFrame& image);
+  void PutFrame(wpi::RawFrame& image);
 };
 
 /**
@@ -177,7 +151,7 @@
    *         and is in 1 us increments.
    */
   [[nodiscard]]
-  uint64_t GrabFrame(RawFrame& image, double timeout = 0.225) const;
+  uint64_t GrabFrame(wpi::RawFrame& image, double timeout = 0.225) const;
 
   /**
    * Wait for the next frame and get the image.  May block forever.
@@ -188,7 +162,7 @@
    *         and is in 1 us increments.
    */
   [[nodiscard]]
-  uint64_t GrabFrameNoTimeout(RawFrame& image) const;
+  uint64_t GrabFrameNoTimeout(wpi::RawFrame& image) const;
 };
 
 inline RawSource::RawSource(std::string_view name, const VideoMode& mode) {
@@ -202,7 +176,7 @@
       CreateRawSource(name, VideoMode{format, width, height, fps}, &m_status);
 }
 
-inline void RawSource::PutFrame(RawFrame& image) {
+inline void RawSource::PutFrame(wpi::RawFrame& image) {
   m_status = 0;
   PutSourceFrame(m_handle, image, &m_status);
 }
@@ -216,12 +190,12 @@
   m_handle = CreateRawSinkCallback(name, processFrame, &m_status);
 }
 
-inline uint64_t RawSink::GrabFrame(RawFrame& image, double timeout) const {
+inline uint64_t RawSink::GrabFrame(wpi::RawFrame& image, double timeout) const {
   m_status = 0;
   return GrabSinkFrameTimeout(m_handle, image, timeout, &m_status);
 }
 
-inline uint64_t RawSink::GrabFrameNoTimeout(RawFrame& image) const {
+inline uint64_t RawSink::GrabFrameNoTimeout(wpi::RawFrame& image) const {
   m_status = 0;
   return GrabSinkFrame(m_handle, image, &m_status);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h
index 11f42d6..7b687f8 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h
@@ -62,7 +62,7 @@
   void PutFrame(cv::Mat& image);
 
  private:
-  RawFrame rawFrame;
+  wpi::RawFrame rawFrame;
 };
 
 /**
@@ -151,7 +151,7 @@
   uint64_t GrabFrameNoTimeoutDirect(cv::Mat& image);
 
  private:
-  RawFrame rawFrame;
+  wpi::RawFrame rawFrame;
 };
 
 inline RawCvSource::RawCvSource(std::string_view name, const VideoMode& mode)
@@ -168,7 +168,8 @@
   rawFrame.width = image.cols;
   rawFrame.height = image.rows;
   rawFrame.totalData = image.total() * image.channels();
-  rawFrame.pixelFormat = image.channels() == 3 ? CS_PIXFMT_BGR : CS_PIXFMT_GRAY;
+  rawFrame.pixelFormat =
+      image.channels() == 3 ? WPI_PIXFMT_BGR : WPI_PIXFMT_GRAY;
   PutSourceFrame(m_handle, rawFrame, &m_status);
 }
 
@@ -201,7 +202,7 @@
 inline uint64_t RawCvSink::GrabFrameDirect(cv::Mat& image, double timeout) {
   rawFrame.height = 0;
   rawFrame.width = 0;
-  rawFrame.pixelFormat = CS_PixelFormat::CS_PIXFMT_BGR;
+  rawFrame.pixelFormat = WPI_PixelFormat::WPI_PIXFMT_BGR;
   m_status = RawSink::GrabFrame(rawFrame, timeout);
   if (m_status <= 0) {
     return m_status;
@@ -213,7 +214,7 @@
 inline uint64_t RawCvSink::GrabFrameNoTimeoutDirect(cv::Mat& image) {
   rawFrame.height = 0;
   rawFrame.width = 0;
-  rawFrame.pixelFormat = CS_PixelFormat::CS_PIXFMT_BGR;
+  rawFrame.pixelFormat = WPI_PixelFormat::WPI_PIXFMT_BGR;
   m_status = RawSink::GrabFrameNoTimeout(rawFrame);
   if (m_status <= 0) {
     return m_status;
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp b/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp
index 1a0ac8b..9771cfa 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp
@@ -89,6 +89,7 @@
   std::memset(&addr, 0, sizeof(addr));
   addr.nl_family = AF_NETLINK;
   addr.nl_groups = RTMGRP_LINK | RTMGRP_IPV4_IFADDR;
+  // NOLINTNEXTLINE(modernize-avoid-bind)
   if (bind(sd, reinterpret_cast<struct sockaddr*>(&addr), sizeof(addr)) < 0) {
     ERROR("NetworkListener: could not create socket: {}", std::strerror(errno));
     ::close(sd);
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp b/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp
index 716edd9..35634fa 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp
@@ -99,8 +99,9 @@
 
 WindowsMessagePump::~WindowsMessagePump() {
   auto res = SendMessageA(hwnd, WM_CLOSE, NULL, NULL);
-  if (m_mainThread.joinable())
+  if (m_mainThread.joinable()) {
     m_mainThread.join();
+  }
 }
 
 void WindowsMessagePump::ThreadMain(HANDLE eventHandle) {
diff --git a/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/VideoModeTest.java b/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/VideoModeTest.java
index 738888d..6108c5c 100644
--- a/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/VideoModeTest.java
+++ b/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/VideoModeTest.java
@@ -7,13 +7,14 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import edu.wpi.first.util.PixelFormat;
 import org.junit.jupiter.api.Test;
 
 class VideoModeTest {
   @Test
   void equalityTest() {
-    VideoMode a = new VideoMode(VideoMode.PixelFormat.kMJPEG, 1920, 1080, 30);
-    VideoMode b = new VideoMode(VideoMode.PixelFormat.kMJPEG, 1920, 1080, 30);
+    VideoMode a = new VideoMode(PixelFormat.kMJPEG, 1920, 1080, 30);
+    VideoMode b = new VideoMode(PixelFormat.kMJPEG, 1920, 1080, 30);
 
     assertEquals(a, b);
     assertNotEquals(a, null);
diff --git a/third_party/allwpilib/datalogtool/CMakeLists.txt b/third_party/allwpilib/datalogtool/CMakeLists.txt
index 72a05c6..00b760e 100644
--- a/third_party/allwpilib/datalogtool/CMakeLists.txt
+++ b/third_party/allwpilib/datalogtool/CMakeLists.txt
@@ -9,7 +9,7 @@
 
 file(GLOB datalogtool_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
 
-if (WIN32)
+if(WIN32)
     set(datalogtool_rc src/main/native/win/datalogtool.rc)
 elseif(APPLE)
     set(MACOSX_BUNDLE_ICON_FILE datalogtool.icns)
@@ -17,12 +17,18 @@
     set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
 endif()
 
-add_executable(datalogtool ${datalogtool_src} ${datalogtool_resources_src} ${datalogtool_rc} ${APP_ICON_MACOSX})
+add_executable(
+    datalogtool
+    ${datalogtool_src}
+    ${datalogtool_resources_src}
+    ${datalogtool_rc}
+    ${APP_ICON_MACOSX}
+)
 wpilib_link_macos_gui(datalogtool)
 target_link_libraries(datalogtool libglass ${LIBSSH_LIBRARIES})
 target_include_directories(datalogtool SYSTEM PRIVATE ${LIBSSH_INCLUDE_DIRS})
 
-if (WIN32)
+if(WIN32)
     set_target_properties(datalogtool PROPERTIES WIN32_EXECUTABLE YES)
 elseif(APPLE)
     set_target_properties(datalogtool PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "datalogTool")
diff --git a/third_party/allwpilib/datalogtool/src/main/generate/WPILibVersion.cpp.in b/third_party/allwpilib/datalogtool/src/main/generate/WPILibVersion.cpp.in
index b0a4490..cfe2441 100644
--- a/third_party/allwpilib/datalogtool/src/main/generate/WPILibVersion.cpp.in
+++ b/third_party/allwpilib/datalogtool/src/main/generate/WPILibVersion.cpp.in
@@ -1,4 +1,4 @@
-/*
+/**
  * Autogenerated file! Do not manually edit this file. This version is regenerated
  * any time the publish task is run, or when this file is deleted.
  */
diff --git a/third_party/allwpilib/datalogtool/src/main/native/cpp/DataLogThread.cpp b/third_party/allwpilib/datalogtool/src/main/native/cpp/DataLogThread.cpp
deleted file mode 100644
index 90c8b19..0000000
--- a/third_party/allwpilib/datalogtool/src/main/native/cpp/DataLogThread.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-// 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 "DataLogThread.h"
-
-#include <fmt/format.h>
-
-DataLogThread::~DataLogThread() {
-  if (m_thread.joinable()) {
-    m_active = false;
-    m_thread.join();
-  }
-}
-
-void DataLogThread::ReadMain() {
-  for (auto record : m_reader) {
-    if (!m_active) {
-      break;
-    }
-    ++m_numRecords;
-    if (record.IsStart()) {
-      wpi::log::StartRecordData data;
-      if (record.GetStartData(&data)) {
-        std::scoped_lock lock{m_mutex};
-        if (m_entries.find(data.entry) != m_entries.end()) {
-          fmt::print("...DUPLICATE entry ID, overriding\n");
-        }
-        m_entries[data.entry] = data;
-        m_entryNames.emplace(data.name, data);
-        sigEntryAdded(data);
-      } else {
-        fmt::print("Start(INVALID)\n");
-      }
-    } else if (record.IsFinish()) {
-      int entry;
-      if (record.GetFinishEntry(&entry)) {
-        std::scoped_lock lock{m_mutex};
-        auto it = m_entries.find(entry);
-        if (it == m_entries.end()) {
-          fmt::print("...ID not found\n");
-        } else {
-          m_entries.erase(it);
-        }
-      } else {
-        fmt::print("Finish(INVALID)\n");
-      }
-    } else if (record.IsSetMetadata()) {
-      wpi::log::MetadataRecordData data;
-      if (record.GetSetMetadataData(&data)) {
-        std::scoped_lock lock{m_mutex};
-        auto it = m_entries.find(data.entry);
-        if (it == m_entries.end()) {
-          fmt::print("...ID not found\n");
-        } else {
-          it->second.metadata = data.metadata;
-          auto nameIt = m_entryNames.find(it->second.name);
-          if (nameIt != m_entryNames.end()) {
-            nameIt->second.metadata = data.metadata;
-          }
-        }
-      } else {
-        fmt::print("SetMetadata(INVALID)\n");
-      }
-    } else if (record.IsControl()) {
-      fmt::print("Unrecognized control record\n");
-    }
-  }
-
-  sigDone();
-  m_done = true;
-}
diff --git a/third_party/allwpilib/datalogtool/src/main/native/cpp/DataLogThread.h b/third_party/allwpilib/datalogtool/src/main/native/cpp/DataLogThread.h
deleted file mode 100644
index 267aa1f..0000000
--- a/third_party/allwpilib/datalogtool/src/main/native/cpp/DataLogThread.h
+++ /dev/null
@@ -1,71 +0,0 @@
-// 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 <atomic>
-#include <functional>
-#include <map>
-#include <string>
-#include <string_view>
-#include <thread>
-#include <utility>
-
-#include <wpi/DataLogReader.h>
-#include <wpi/DenseMap.h>
-#include <wpi/Signal.h>
-#include <wpi/mutex.h>
-
-class DataLogThread {
- public:
-  explicit DataLogThread(wpi::log::DataLogReader reader)
-      : m_reader{std::move(reader)}, m_thread{[=, this] { ReadMain(); }} {}
-  ~DataLogThread();
-
-  bool IsDone() const { return m_done; }
-  std::string_view GetBufferIdentifier() const {
-    return m_reader.GetBufferIdentifier();
-  }
-  unsigned int GetNumRecords() const { return m_numRecords; }
-  unsigned int GetNumEntries() const {
-    std::scoped_lock lock{m_mutex};
-    return m_entryNames.size();
-  }
-
-  // Passes wpi::log::StartRecordData to func
-  template <typename T>
-  void ForEachEntryName(T&& func) {
-    std::scoped_lock lock{m_mutex};
-    for (auto&& kv : m_entryNames) {
-      func(kv.second);
-    }
-  }
-
-  wpi::log::StartRecordData GetEntry(std::string_view name) const {
-    std::scoped_lock lock{m_mutex};
-    auto it = m_entryNames.find(name);
-    if (it == m_entryNames.end()) {
-      return {};
-    }
-    return it->second;
-  }
-
-  const wpi::log::DataLogReader& GetReader() const { return m_reader; }
-
-  // note: these are called on separate thread
-  wpi::sig::Signal_mt<const wpi::log::StartRecordData&> sigEntryAdded;
-  wpi::sig::Signal_mt<> sigDone;
-
- private:
-  void ReadMain();
-
-  wpi::log::DataLogReader m_reader;
-  mutable wpi::mutex m_mutex;
-  std::atomic_bool m_active{true};
-  std::atomic_bool m_done{false};
-  std::atomic<unsigned int> m_numRecords{0};
-  std::map<std::string, wpi::log::StartRecordData, std::less<>> m_entryNames;
-  wpi::DenseMap<int, wpi::log::StartRecordData> m_entries;
-  std::thread m_thread;
-};
diff --git a/third_party/allwpilib/datalogtool/src/main/native/cpp/Downloader.cpp b/third_party/allwpilib/datalogtool/src/main/native/cpp/Downloader.cpp
index 9629e3b..0a8b90d 100644
--- a/third_party/allwpilib/datalogtool/src/main/native/cpp/Downloader.cpp
+++ b/third_party/allwpilib/datalogtool/src/main/native/cpp/Downloader.cpp
@@ -27,7 +27,7 @@
 
 Downloader::Downloader(glass::Storage& storage)
     : m_serverTeam{storage.GetString("serverTeam")},
-      m_remoteDir{storage.GetString("remoteDir", "/home/lvuser")},
+      m_remoteDir{storage.GetString("remoteDir", "/home/lvuser/logs")},
       m_username{storage.GetString("username", "lvuser")},
       m_localDir{storage.GetString("localDir")},
       m_deleteAfter{storage.GetBool("deleteAfter", true)},
diff --git a/third_party/allwpilib/datalogtool/src/main/native/cpp/Exporter.cpp b/third_party/allwpilib/datalogtool/src/main/native/cpp/Exporter.cpp
index 9201c8f..3c0ade1 100644
--- a/third_party/allwpilib/datalogtool/src/main/native/cpp/Exporter.cpp
+++ b/third_party/allwpilib/datalogtool/src/main/native/cpp/Exporter.cpp
@@ -17,6 +17,7 @@
 #include <fmt/chrono.h>
 #include <fmt/format.h>
 #include <glass/Storage.h>
+#include <glass/support/DataLogReaderThread.h>
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <imgui_stdlib.h>
@@ -32,11 +33,10 @@
 #include <wpi/raw_ostream.h>
 
 #include "App.h"
-#include "DataLogThread.h"
 
 namespace {
 struct InputFile {
-  explicit InputFile(std::unique_ptr<DataLogThread> datalog);
+  explicit InputFile(std::unique_ptr<glass::DataLogReaderThread> datalog);
 
   InputFile(std::string_view filename, std::string_view status)
       : filename{filename},
@@ -47,7 +47,7 @@
 
   std::string filename;
   std::string stem;
-  std::unique_ptr<DataLogThread> datalog;
+  std::unique_ptr<glass::DataLogReaderThread> datalog;
   std::string status;
   bool highlight = false;
 };
@@ -135,7 +135,7 @@
   }
 }
 
-InputFile::InputFile(std::unique_ptr<DataLogThread> datalog_)
+InputFile::InputFile(std::unique_ptr<glass::DataLogReaderThread> datalog_)
     : filename{datalog_->GetBufferIdentifier()},
       stem{fs::path{filename}.stem().string()},
       datalog{std::move(datalog_)} {
@@ -192,7 +192,7 @@
   }
 
   return std::make_unique<InputFile>(
-      std::make_unique<DataLogThread>(std::move(reader)));
+      std::make_unique<glass::DataLogReaderThread>(std::move(reader)));
 }
 
 void DisplayInputFiles() {
@@ -284,9 +284,10 @@
     if (ImGui::IsItemHovered()) {
       ImGui::BeginTooltip();
       for (auto inputFile : entry.inputFiles) {
-        ImGui::Text(
-            "%s: %s", inputFile->stem.c_str(),
-            std::string{inputFile->datalog->GetEntry(entry.name).type}.c_str());
+        if (auto info = inputFile->datalog->GetEntry(entry.name)) {
+          ImGui::Text("%s: %s", inputFile->stem.c_str(),
+                      std::string{info->type}.c_str());
+        }
       }
       ImGui::EndTooltip();
     }
@@ -300,10 +301,10 @@
     if (ImGui::IsItemHovered()) {
       ImGui::BeginTooltip();
       for (auto inputFile : entry.inputFiles) {
-        ImGui::Text(
-            "%s: %s", inputFile->stem.c_str(),
-            std::string{inputFile->datalog->GetEntry(entry.name).metadata}
-                .c_str());
+        if (auto info = inputFile->datalog->GetEntry(entry.name)) {
+          ImGui::Text("%s: %s", inputFile->stem.c_str(),
+                      std::string{info->metadata}.c_str());
+        }
       }
       ImGui::EndTooltip();
     }
diff --git a/third_party/allwpilib/docs/build.gradle b/third_party/allwpilib/docs/build.gradle
index 1a1e3b1..1d6e65a 100644
--- a/third_party/allwpilib/docs/build.gradle
+++ b/third_party/allwpilib/docs/build.gradle
@@ -144,6 +144,10 @@
 
     exclude '*.pb.h'
 
+    // Save space by excluding protobuf and eigen
+    exclude 'Eigen/**'
+    exclude 'google/protobuf/**'
+
     aliases 'effects=\\par <i>Effects:</i>^^',
             'notes=\\par <i>Notes:</i>^^',
             'requires=\\par <i>Requires:</i>^^',
@@ -204,28 +208,25 @@
     }
 }
 
-ext {
-    sharedCvConfigs = [:]
-    staticCvConfigs = [:]
-    useJava = true
-    useCpp = false
-    skipDev = true
-    useDocumentation = true
-}
-
-apply from: "${rootDir}/shared/opencv.gradle"
-
 task generateJavaDocs(type: Javadoc) {
-    classpath += project(":wpimath").sourceSets.main.compileClasspath
+    classpath += project(":wpilibj").sourceSets.main.compileClasspath
     options.links("https://docs.oracle.com/en/java/javase/17/docs/api/")
+    options.links("https://docs.opencv.org/4.x/javadoc/")
     options.addStringOption("tag", "pre:a:Pre-Condition")
+    options.addBooleanOption("Xdoclint/package:-edu.wpi.first.math.proto," +
+            "-edu.wpi.first.math.controller.proto," +
+            "-edu.wpi.first.math.controller.struct," +
+            "-edu.wpi.first.math.geometry.proto," +
+            "-edu.wpi.first.math.geometry.struct," +
+            "-edu.wpi.first.math.kinematics.proto," +
+            "-edu.wpi.first.math.kinematics.struct," +
+            "-edu.wpi.first.math.system.plant.proto," +
+            "-edu.wpi.first.math.system.plant.struct," +
+            "-edu.wpi.first.math.trajectory.proto", true)
     options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true)
     options.addBooleanOption('html5', true)
     options.linkSource(true)
-    dependsOn project(':hal').generateUsageReporting
-    dependsOn project(':ntcore').ntcoreGenerateJavaTypes
     dependsOn project(':wpilibj').generateJavaVersion
-    dependsOn project(':wpimath').generateNat
     source project(':apriltag').sourceSets.main.java
     source project(':cameraserver').sourceSets.main.java
     source project(':cscore').sourceSets.main.java
diff --git a/third_party/allwpilib/fieldImages/CMakeLists.txt b/third_party/allwpilib/fieldImages/CMakeLists.txt
index 3532790..45fe49c 100644
--- a/third_party/allwpilib/fieldImages/CMakeLists.txt
+++ b/third_party/allwpilib/fieldImages/CMakeLists.txt
@@ -3,7 +3,7 @@
 include(CompileWarnings)
 include(GenResources)
 
-if (WITH_JAVA)
+if(WITH_JAVA)
     find_package(Java REQUIRED)
     include(UseJava)
 
@@ -11,30 +11,42 @@
     set(CMAKE_JAVA_INCLUDE_PATH fieldImages.jar ${JACKSON_JARS})
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-    file(GLOB_RECURSE JAVA_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} src/main/native/resources/*.json src/main/native/resources/*.png src/main/native/resources/*.jpg)
-    add_jar(field_images_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/fields" ${JAVA_RESOURCES} OUTPUT_NAME fieldImages)
+    file(
+        GLOB_RECURSE JAVA_RESOURCES
+        RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
+        src/main/native/resources/*.json
+        src/main/native/resources/*.png
+        src/main/native/resources/*.jpg
+    )
+    add_jar(
+        field_images_jar
+        SOURCES ${JAVA_SOURCES}
+        RESOURCES
+        NAMESPACE "edu/wpi/first/fields" ${JAVA_RESOURCES}
+        OUTPUT_NAME fieldImages
+    )
 
     get_property(FIELD_IMAGES_JAR_FILE TARGET field_images_jar PROPERTY JAR_FILE)
     install(FILES ${FIELD_IMAGES_JAR_FILE} DESTINATION "${java_lib_dest}")
 
     set_property(TARGET field_images_jar PROPERTY FOLDER "java")
-
 endif()
 
-
 generate_resources(src/main/native/resources/edu/wpi/first/fields generated/main/cpp FIELDS fields field_images_resources_src)
 
-
 add_library(fieldImages src/main/native/cpp/fields.cpp ${field_images_resources_src})
 set_target_properties(fieldImages PROPERTIES DEBUG_POSTFIX "d")
 
 set_property(TARGET fieldImages PROPERTY FOLDER "libraries")
 target_compile_features(fieldImages PUBLIC cxx_std_20)
-if (MSVC)
+if(MSVC)
     target_compile_options(fieldImages PUBLIC /bigobj)
 endif()
 wpilib_target_warnings(fieldImages)
 
-target_include_directories(fieldImages PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/fields>)
+target_include_directories(
+    fieldImages
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/fields>
+)
diff --git a/third_party/allwpilib/fieldImages/publish.gradle b/third_party/allwpilib/fieldImages/publish.gradle
index 33eb5ae..688376e 100644
--- a/third_party/allwpilib/fieldImages/publish.gradle
+++ b/third_party/allwpilib/fieldImages/publish.gradle
@@ -2,7 +2,7 @@
 
 def baseArtifactId = project.nativeName
 def artifactGroupId = project.groupId
-def cppZipBaseName = "_GROUP_edu_wpi_first_fieldIimages_ID_${baseArtifactId}-cpp_CLS"
+def cppZipBaseName = "_GROUP_edu_wpi_first_fieldImages_ID_${baseArtifactId}-cpp_CLS"
 
 def outputsFolder = file("$project.buildDir/outputs")
 
diff --git a/third_party/allwpilib/fieldImages/src/main/java/edu/wpi/fields/Fields.java b/third_party/allwpilib/fieldImages/src/main/java/edu/wpi/fields/Fields.java
index 119415e..a92cfb0 100644
--- a/third_party/allwpilib/fieldImages/src/main/java/edu/wpi/fields/Fields.java
+++ b/third_party/allwpilib/fieldImages/src/main/java/edu/wpi/fields/Fields.java
@@ -15,12 +15,13 @@
   k2021GalacticSearchB("2021-galacticsearchb.json"),
   k2021Slalom("2021-slalompath.json"),
   k2022RapidReact("2022-rapidreact.json"),
-  k2023ChargedUp("2023-chargedup.json");
+  k2023ChargedUp("2023-chargedup.json"),
+  k2024Crescendo("2024-crescendo.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/fields/";
 
   /** Alias to the current game. */
-  public static final Fields kDefaultField = k2023ChargedUp;
+  public static final Fields kDefaultField = k2024Crescendo;
 
   public final String m_resourceFile;
 
diff --git a/third_party/allwpilib/fieldImages/src/main/native/cpp/fields.cpp b/third_party/allwpilib/fieldImages/src/main/native/cpp/fields.cpp
index 4f79e6b..6fd6af0 100644
--- a/third_party/allwpilib/fieldImages/src/main/native/cpp/fields.cpp
+++ b/third_party/allwpilib/fieldImages/src/main/native/cpp/fields.cpp
@@ -15,10 +15,13 @@
 #include "fields/2021-slalom.h"
 #include "fields/2022-rapidreact.h"
 #include "fields/2023-chargedup.h"
+#include "fields/2024-crescendo.h"
 
 using namespace fields;
 
 static const Field kFields[] = {
+    {"2024 Crescendo", GetResource_2024_crescendo_json,
+     GetResource_2024_field_png},
     {"2023 Charged Up", GetResource_2023_chargedup_json,
      GetResource_2023_field_png},
     {"2022 Rapid React", GetResource_2022_rapidreact_json,
diff --git a/third_party/allwpilib/fieldImages/src/main/native/include/fields/2024-crescendo.h b/third_party/allwpilib/fieldImages/src/main/native/include/fields/2024-crescendo.h
new file mode 100644
index 0000000..11947ef
--- /dev/null
+++ b/third_party/allwpilib/fieldImages/src/main/native/include/fields/2024-crescendo.h
@@ -0,0 +1,12 @@
+// 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 <string_view>
+
+namespace fields {
+std::string_view GetResource_2024_crescendo_json();
+std::string_view GetResource_2024_field_png();
+}  // namespace fields
diff --git a/third_party/allwpilib/fieldImages/src/main/native/resources/edu/wpi/first/fields/2024-crescendo.json b/third_party/allwpilib/fieldImages/src/main/native/resources/edu/wpi/first/fields/2024-crescendo.json
new file mode 100644
index 0000000..9a1cbf3
--- /dev/null
+++ b/third_party/allwpilib/fieldImages/src/main/native/resources/edu/wpi/first/fields/2024-crescendo.json
@@ -0,0 +1,19 @@
+{
+  "game": "Crescendo",
+  "field-image": "2024-field.png",
+  "field-corners": {
+    "top-left": [
+      113,
+      25
+    ],
+    "bottom-right": [
+      1008,
+      522
+    ]
+  },
+  "field-size": [
+    54.27083,
+    26.9375
+  ],
+  "field-unit": "foot"
+}
diff --git a/third_party/allwpilib/fieldImages/src/main/native/resources/edu/wpi/first/fields/2024-field.png b/third_party/allwpilib/fieldImages/src/main/native/resources/edu/wpi/first/fields/2024-field.png
new file mode 100644
index 0000000..c642a7f
--- /dev/null
+++ b/third_party/allwpilib/fieldImages/src/main/native/resources/edu/wpi/first/fields/2024-field.png
Binary files differ
diff --git a/third_party/allwpilib/glass/CMakeLists.txt b/third_party/allwpilib/glass/CMakeLists.txt
index b949fc0..36f94f5 100644
--- a/third_party/allwpilib/glass/CMakeLists.txt
+++ b/third_party/allwpilib/glass/CMakeLists.txt
@@ -18,9 +18,12 @@
 wpilib_target_warnings(libglass)
 target_link_libraries(libglass PUBLIC wpigui wpimath wpiutil fieldImages)
 
-target_include_directories(libglass PUBLIC
-                           $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/lib/native/include>
-                           $<INSTALL_INTERFACE:${include_dest}/glass>)
+target_include_directories(
+    libglass
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/lib/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/glass>
+)
 
 install(TARGETS libglass EXPORT libglass)
 install(DIRECTORY src/lib/native/include/ DESTINATION "${include_dest}/glass")
@@ -39,9 +42,12 @@
 wpilib_target_warnings(libglassnt)
 target_link_libraries(libglassnt PUBLIC ntcore libglass)
 
-target_include_directories(libglassnt PUBLIC
-                           $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/libnt/native/include>
-                           $<INSTALL_INTERFACE:${include_dest}/glass>)
+target_include_directories(
+    libglassnt
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/libnt/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/glass>
+)
 
 install(TARGETS libglassnt EXPORT libglassnt)
 install(DIRECTORY src/libnt/native/include/ DESTINATION "${include_dest}/glass")
@@ -55,7 +61,7 @@
 
 file(GLOB glass_src src/app/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
 
-if (WIN32)
+if(WIN32)
     set(glass_rc src/app/native/win/glass.rc)
 elseif(APPLE)
     set(MACOSX_BUNDLE_ICON_FILE glass.icns)
@@ -68,18 +74,8 @@
 wpilib_target_warnings(glass)
 target_link_libraries(glass libglassnt libglass)
 
-if (WIN32)
+if(WIN32)
     set_target_properties(glass PROPERTIES WIN32_EXECUTABLE YES)
 elseif(APPLE)
     set_target_properties(glass PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "Glass")
 endif()
-
-#if (MSVC OR FLAT_INSTALL_WPILIB)
-#    set (wpigui_config_dir ${wpilib_dest})
-#else()
-#    set (wpigui_config_dir share/wpigui)
-#endif()
-
-#configure_file(wpigui-config.cmake.in ${CMAKE_BINARY_DIR}/wpigui-config.cmake )
-#install(FILES ${CMAKE_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir})
-#install(EXPORT wpigui DESTINATION ${wpigui_config_dir})
diff --git a/third_party/allwpilib/glass/src/app/generate/WPILibVersion.cpp.in b/third_party/allwpilib/glass/src/app/generate/WPILibVersion.cpp.in
index b0a4490..cfe2441 100644
--- a/third_party/allwpilib/glass/src/app/generate/WPILibVersion.cpp.in
+++ b/third_party/allwpilib/glass/src/app/generate/WPILibVersion.cpp.in
@@ -1,4 +1,4 @@
-/*
+/**
  * Autogenerated file! Do not manually edit this file. This version is regenerated
  * any time the publish task is run, or when this file is deleted.
  */
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/Context.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/Context.cpp
index e09a86e..5adcd94 100644
--- a/third_party/allwpilib/glass/src/lib/native/cpp/Context.cpp
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/Context.cpp
@@ -137,9 +137,8 @@
     return false;
   } else {
     try {
-      return JsonToWindow(
-          wpi::json::parse(fileBuffer->begin(), fileBuffer->end()),
-          filename.c_str());
+      return JsonToWindow(wpi::json::parse(fileBuffer->GetCharBuffer()),
+                          filename.c_str());
     } catch (wpi::json::parse_error& e) {
       ImGui::LogText("Error loading %s: %s", filename.c_str(), e.what());
       return false;
@@ -164,9 +163,8 @@
       createdStorage = true;
     }
     try {
-      storage->FromJson(
-          wpi::json::parse(fileBuffer->begin(), fileBuffer->end()),
-          filename.c_str());
+      storage->FromJson(wpi::json::parse(fileBuffer->GetCharBuffer()),
+                        filename.c_str());
     } catch (wpi::json::parse_error& e) {
       ImGui::LogText("Error loading %s: %s", filename.c_str(), e.what());
       if (createdStorage) {
@@ -180,8 +178,6 @@
 
 static bool LoadStorageImpl(Context* ctx, std::string_view dir,
                             std::string_view name) {
-  WorkspaceResetImpl();
-
   bool rv = true;
   for (auto&& root : ctx->storageRoots) {
     std::string filename;
@@ -423,6 +419,7 @@
 bool glass::LoadStorage(std::string_view dir) {
   SaveStorage();
   SetStorageDir(dir);
+  WorkspaceResetImpl();
   LoadWindowStorageImpl((fs::path{gContext->storageLoadDir} /
                          fmt::format("{}-window.json", gContext->storageName))
                             .string());
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/PIDController.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/PIDController.cpp
index 3c83b11..8aa5e1f 100644
--- a/third_party/allwpilib/glass/src/lib/native/cpp/other/PIDController.cpp
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/PIDController.cpp
@@ -29,6 +29,16 @@
         callback(*v);
       }
     };
+    // Workaround to allow for the input of inf, -inf, and nan
+    auto createTuningParameterNoFilter =
+        [flag](const char* name, double* v,
+               std::function<void(double)> callback) {
+          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+          if (ImGui::InputScalar(name, ImGuiDataType_Double, v, NULL, NULL,
+                                 "%.3f", flag)) {
+            callback(*v);
+          }
+        };
 
     if (auto p = m->GetPData()) {
       double value = p->GetValue();
@@ -47,6 +57,11 @@
       createTuningParameter("Setpoint", &value,
                             [=](auto v) { m->SetSetpoint(v); });
     }
+    if (auto s = m->GetIZoneData()) {
+      double value = s->GetValue();
+      createTuningParameterNoFilter("IZone", &value,
+                                    [=](auto v) { m->SetIZone(v); });
+    }
   } else {
     ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
     ImGui::Text("Unknown PID Controller");
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp
new file mode 100644
index 0000000..8113c8c
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp
@@ -0,0 +1,69 @@
+// 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 "glass/other/ProfiledPIDController.h"
+
+#include <string>
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayProfiledPIDController(ProfiledPIDControllerModel* m) {
+  if (auto name = m->GetName()) {
+    ImGui::Text("%s", name);
+    ImGui::Separator();
+  }
+
+  if (m->Exists()) {
+    auto flag = m->IsReadOnly() ? ImGuiInputTextFlags_ReadOnly
+                                : ImGuiInputTextFlags_None;
+    auto createTuningParameter = [flag](const char* name, double* v,
+                                        std::function<void(double)> callback) {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      if (ImGui::InputDouble(name, v, 0.0, 0.0, "%.3f", flag)) {
+        callback(*v);
+      }
+    };
+    // Workaround to allow for the input of inf, -inf, and nan
+    auto createTuningParameterNoFilter =
+        [flag](const char* name, double* v,
+               std::function<void(double)> callback) {
+          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+          if (ImGui::InputScalar(name, ImGuiDataType_Double, v, NULL, NULL,
+                                 "%.3f", flag)) {
+            callback(*v);
+          }
+        };
+
+    if (auto p = m->GetPData()) {
+      double value = p->GetValue();
+      createTuningParameter("P", &value, [=](auto v) { m->SetP(v); });
+    }
+    if (auto i = m->GetIData()) {
+      double value = i->GetValue();
+      createTuningParameter("I", &value, [=](auto v) { m->SetI(v); });
+    }
+    if (auto d = m->GetDData()) {
+      double value = d->GetValue();
+      createTuningParameter("D", &value, [=](auto v) { m->SetD(v); });
+    }
+    if (auto s = m->GetGoalData()) {
+      double value = s->GetValue();
+      createTuningParameter("Goal", &value, [=](auto v) { m->SetGoal(v); });
+    }
+    if (auto s = m->GetIZoneData()) {
+      double value = s->GetValue();
+      createTuningParameterNoFilter("IZone", &value,
+                                    [=](auto v) { m->SetIZone(v); });
+    }
+  } else {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown PID Controller");
+    ImGui::PopStyleColor();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/support/DataLogReaderThread.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/support/DataLogReaderThread.cpp
new file mode 100644
index 0000000..9d90bb3
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/support/DataLogReaderThread.cpp
@@ -0,0 +1,124 @@
+// 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 "glass/support/DataLogReaderThread.h"
+
+#include <utility>
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+DataLogReaderThread::~DataLogReaderThread() {
+  if (m_thread.joinable()) {
+    m_active = false;
+    m_thread.join();
+  }
+}
+
+void DataLogReaderThread::ReadMain() {
+  wpi::SmallDenseMap<
+      int, std::pair<DataLogReaderEntry*, std::span<const uint8_t>>, 8>
+      schemaEntries;
+
+  for (auto recordIt = m_reader.begin(), recordEnd = m_reader.end();
+       recordIt != recordEnd; ++recordIt) {
+    auto& record = *recordIt;
+    if (!m_active) {
+      break;
+    }
+    ++m_numRecords;
+    if (record.IsStart()) {
+      DataLogReaderEntry data;
+      if (record.GetStartData(&data)) {
+        std::scoped_lock lock{m_mutex};
+        auto& entryPtr = m_entriesById[data.entry];
+        if (entryPtr) {
+          fmt::print("...DUPLICATE entry ID, overriding\n");
+        }
+        auto [it, isNew] = m_entriesByName.emplace(data.name, data);
+        if (isNew) {
+          it->second.ranges.emplace_back(recordIt, recordEnd);
+        }
+        entryPtr = &it->second;
+        if (data.type == "structschema" ||
+            data.type == "proto:FileDescriptorProto") {
+          schemaEntries.try_emplace(data.entry, entryPtr,
+                                    std::span<const uint8_t>{});
+        }
+        sigEntryAdded(data);
+      } else {
+        fmt::print("Start(INVALID)\n");
+      }
+    } else if (record.IsFinish()) {
+      int entry;
+      if (record.GetFinishEntry(&entry)) {
+        std::scoped_lock lock{m_mutex};
+        auto it = m_entriesById.find(entry);
+        if (it == m_entriesById.end()) {
+          fmt::print("...ID not found\n");
+        } else {
+          it->second->ranges.back().m_end = recordIt;
+          m_entriesById.erase(it);
+        }
+      } else {
+        fmt::print("Finish(INVALID)\n");
+      }
+    } else if (record.IsSetMetadata()) {
+      wpi::log::MetadataRecordData data;
+      if (record.GetSetMetadataData(&data)) {
+        std::scoped_lock lock{m_mutex};
+        auto it = m_entriesById.find(data.entry);
+        if (it == m_entriesById.end()) {
+          fmt::print("...ID not found\n");
+        } else {
+          it->second->metadata = data.metadata;
+        }
+      } else {
+        fmt::print("SetMetadata(INVALID)\n");
+      }
+    } else if (record.IsControl()) {
+      fmt::print("Unrecognized control record\n");
+    } else {
+      auto it = schemaEntries.find(record.GetEntry());
+      if (it != schemaEntries.end()) {
+        it->second.second = record.GetRaw();
+      }
+    }
+  }
+
+  // build schema databases
+  for (auto&& schemaPair : schemaEntries) {
+    auto name = schemaPair.second.first->name;
+    auto data = schemaPair.second.second;
+    if (data.empty()) {
+      continue;
+    }
+    if (wpi::starts_with(name, "NT:")) {
+      name = wpi::drop_front(name, 3);
+    }
+    if (wpi::starts_with(name, "/.schema/struct:")) {
+      auto typeStr = wpi::drop_front(name, 16);
+      std::string_view schema{reinterpret_cast<const char*>(data.data()),
+                              data.size()};
+      std::string err;
+      auto desc = m_structDb.Add(typeStr, schema, &err);
+      if (!desc) {
+        fmt::print("could not decode struct '{}' schema '{}': {}\n", name,
+                   schema, err);
+      }
+    } else if (wpi::starts_with(name, "/.schema/proto:")) {
+      // protobuf descriptor handling
+      auto filename = wpi::drop_front(name, 15);
+      if (!m_protoDb.Add(filename, data)) {
+        fmt::print("could not decode protobuf '{}' filename '{}'\n", name,
+                   filename);
+      }
+    }
+  }
+
+  sigDone();
+  m_done = true;
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/PIDController.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/PIDController.h
index ab0dcb3..8c11c59 100644
--- a/third_party/allwpilib/glass/src/lib/native/include/glass/other/PIDController.h
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/PIDController.h
@@ -16,11 +16,13 @@
   virtual DataSource* GetIData() = 0;
   virtual DataSource* GetDData() = 0;
   virtual DataSource* GetSetpointData() = 0;
+  virtual DataSource* GetIZoneData() = 0;
 
   virtual void SetP(double value) = 0;
   virtual void SetI(double value) = 0;
   virtual void SetD(double value) = 0;
   virtual void SetSetpoint(double value) = 0;
+  virtual void SetIZone(double value) = 0;
 };
 void DisplayPIDController(PIDControllerModel* m);
 }  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/ProfiledPIDController.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/ProfiledPIDController.h
new file mode 100644
index 0000000..bebf3ab
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/ProfiledPIDController.h
@@ -0,0 +1,28 @@
+// 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 "glass/Model.h"
+
+namespace glass {
+class DataSource;
+class ProfiledPIDControllerModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+
+  virtual DataSource* GetPData() = 0;
+  virtual DataSource* GetIData() = 0;
+  virtual DataSource* GetDData() = 0;
+  virtual DataSource* GetGoalData() = 0;
+  virtual DataSource* GetIZoneData() = 0;
+
+  virtual void SetP(double value) = 0;
+  virtual void SetI(double value) = 0;
+  virtual void SetD(double value) = 0;
+  virtual void SetGoal(double value) = 0;
+  virtual void SetIZone(double value) = 0;
+};
+void DisplayProfiledPIDController(ProfiledPIDControllerModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/DataLogReaderThread.h b/third_party/allwpilib/glass/src/lib/native/include/glass/support/DataLogReaderThread.h
new file mode 100644
index 0000000..bcbc962
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/DataLogReaderThread.h
@@ -0,0 +1,101 @@
+// 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 <atomic>
+#include <functional>
+#include <map>
+#include <string>
+#include <string_view>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <wpi/DataLogReader.h>
+#include <wpi/DenseMap.h>
+#include <wpi/Signal.h>
+#include <wpi/mutex.h>
+#include <wpi/protobuf/ProtobufMessageDatabase.h>
+#include <wpi/struct/DynamicStruct.h>
+
+namespace glass {
+
+class DataLogReaderRange {
+ public:
+  DataLogReaderRange(wpi::log::DataLogReader::iterator begin,
+                     wpi::log::DataLogReader::iterator end)
+      : m_begin{begin}, m_end{end} {}
+
+  wpi::log::DataLogReader::iterator begin() const { return m_begin; }
+  wpi::log::DataLogReader::iterator end() const { return m_end; }
+
+  wpi::log::DataLogReader::iterator m_begin;
+  wpi::log::DataLogReader::iterator m_end;
+};
+
+class DataLogReaderEntry : public wpi::log::StartRecordData {
+ public:
+  std::vector<DataLogReaderRange> ranges;  // ranges where this entry is valid
+};
+
+class DataLogReaderThread {
+ public:
+  explicit DataLogReaderThread(wpi::log::DataLogReader reader)
+      : m_reader{std::move(reader)}, m_thread{[this] { ReadMain(); }} {}
+  ~DataLogReaderThread();
+
+  bool IsDone() const { return m_done; }
+  std::string_view GetBufferIdentifier() const {
+    return m_reader.GetBufferIdentifier();
+  }
+  unsigned int GetNumRecords() const { return m_numRecords; }
+  unsigned int GetNumEntries() const {
+    std::scoped_lock lock{m_mutex};
+    return m_entriesByName.size();
+  }
+
+  // Passes Entry& to func
+  template <typename T>
+  void ForEachEntryName(T&& func) {
+    std::scoped_lock lock{m_mutex};
+    for (auto&& kv : m_entriesByName) {
+      func(kv.second);
+    }
+  }
+
+  const DataLogReaderEntry* GetEntry(std::string_view name) const {
+    std::scoped_lock lock{m_mutex};
+    auto it = m_entriesByName.find(name);
+    if (it == m_entriesByName.end()) {
+      return nullptr;
+    }
+    return &it->second;
+  }
+
+  wpi::StructDescriptorDatabase& GetStructDatabase() { return m_structDb; }
+  wpi::ProtobufMessageDatabase& GetProtobufDatabase() { return m_protoDb; }
+
+  const wpi::log::DataLogReader& GetReader() const { return m_reader; }
+
+  // note: these are called on separate thread
+  wpi::sig::Signal_mt<const DataLogReaderEntry&> sigEntryAdded;
+  wpi::sig::Signal_mt<> sigDone;
+
+ private:
+  void ReadMain();
+
+  wpi::log::DataLogReader m_reader;
+  mutable wpi::mutex m_mutex;
+  std::atomic_bool m_active{true};
+  std::atomic_bool m_done{false};
+  std::atomic<unsigned int> m_numRecords{0};
+  std::map<std::string, DataLogReaderEntry, std::less<>> m_entriesByName;
+  wpi::DenseMap<int, DataLogReaderEntry*> m_entriesById;
+  wpi::StructDescriptorDatabase m_structDb;
+  wpi::ProtobufMessageDatabase m_protoDb;
+  std::thread m_thread;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTPIDController.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTPIDController.cpp
index 1dde27d..9796f6d 100644
--- a/third_party/allwpilib/glass/src/libnt/native/cpp/NTPIDController.cpp
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTPIDController.cpp
@@ -23,10 +23,12 @@
       m_d{inst.GetDoubleTopic(fmt::format("{}/d", path)).GetEntry(0)},
       m_setpoint{
           inst.GetDoubleTopic(fmt::format("{}/setpoint", path)).GetEntry(0)},
+      m_iZone{inst.GetDoubleTopic(fmt::format("{}/izone", path)).GetEntry(0)},
       m_pData{fmt::format("NTPIDCtrlP:{}", path)},
       m_iData{fmt::format("NTPIDCtrlI:{}", path)},
       m_dData{fmt::format("NTPIDCtrlD:{}", path)},
       m_setpointData{fmt::format("NTPIDCtrlStpt:{}", path)},
+      m_iZoneData{fmt::format("NTPIDCtrlIZone:{}", path)},
       m_nameValue{wpi::rsplit(path, '/').second} {}
 
 void NTPIDControllerModel::SetP(double value) {
@@ -44,6 +46,9 @@
 void NTPIDControllerModel::SetSetpoint(double value) {
   m_setpoint.Set(value);
 }
+void NTPIDControllerModel::SetIZone(double value) {
+  m_iZone.Set(value);
+}
 
 void NTPIDControllerModel::Update() {
   for (auto&& v : m_name.ReadQueue()) {
@@ -61,6 +66,9 @@
   for (auto&& v : m_setpoint.ReadQueue()) {
     m_setpointData.SetValue(v.value, v.time);
   }
+  for (auto&& v : m_iZone.ReadQueue()) {
+    m_iZoneData.SetValue(v.value, v.time);
+  }
   for (auto&& v : m_controllable.ReadQueue()) {
     m_controllableValue = v.value;
   }
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp
new file mode 100644
index 0000000..bae069b
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp
@@ -0,0 +1,80 @@
+// 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 "glass/networktables/NTProfiledPIDController.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTProfiledPIDControllerModel::NTProfiledPIDControllerModel(
+    std::string_view path)
+    : NTProfiledPIDControllerModel(nt::NetworkTableInstance::GetDefault(),
+                                   path) {}
+
+NTProfiledPIDControllerModel::NTProfiledPIDControllerModel(
+    nt::NetworkTableInstance inst, std::string_view path)
+    : m_inst{inst},
+      m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
+      m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
+                         .Subscribe(false)},
+      m_p{inst.GetDoubleTopic(fmt::format("{}/p", path)).GetEntry(0)},
+      m_i{inst.GetDoubleTopic(fmt::format("{}/i", path)).GetEntry(0)},
+      m_d{inst.GetDoubleTopic(fmt::format("{}/d", path)).GetEntry(0)},
+      m_goal{inst.GetDoubleTopic(fmt::format("{}/goal", path)).GetEntry(0)},
+      m_iZone{inst.GetDoubleTopic(fmt::format("{}/izone", path)).GetEntry(0)},
+      m_pData{fmt::format("NTPIDCtrlP:{}", path)},
+      m_iData{fmt::format("NTPIDCtrlI:{}", path)},
+      m_dData{fmt::format("NTPIDCtrlD:{}", path)},
+      m_goalData{fmt::format("NTPIDCtrlGoal:{}", path)},
+      m_iZoneData{fmt::format("NTPIDCtrlIZone:{}", path)},
+      m_nameValue{wpi::rsplit(path, '/').second} {}
+
+void NTProfiledPIDControllerModel::SetP(double value) {
+  m_p.Set(value);
+}
+
+void NTProfiledPIDControllerModel::SetI(double value) {
+  m_i.Set(value);
+}
+
+void NTProfiledPIDControllerModel::SetD(double value) {
+  m_d.Set(value);
+}
+
+void NTProfiledPIDControllerModel::SetGoal(double value) {
+  m_goal.Set(value);
+}
+void NTProfiledPIDControllerModel::SetIZone(double value) {
+  m_iZone.Set(value);
+}
+
+void NTProfiledPIDControllerModel::Update() {
+  for (auto&& v : m_name.ReadQueue()) {
+    m_nameValue = std::move(v.value);
+  }
+  for (auto&& v : m_p.ReadQueue()) {
+    m_pData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_i.ReadQueue()) {
+    m_iData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_d.ReadQueue()) {
+    m_dData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_goal.ReadQueue()) {
+    m_goalData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_iZone.ReadQueue()) {
+    m_iZoneData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_controllable.ReadQueue()) {
+    m_controllableValue = v.value;
+  }
+}
+
+bool NTProfiledPIDControllerModel::Exists() {
+  return m_goal.Exists();
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTStringChooser.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTStringChooser.cpp
index b892a2c..26074fc 100644
--- a/third_party/allwpilib/glass/src/libnt/native/cpp/NTStringChooser.cpp
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTStringChooser.cpp
@@ -5,6 +5,7 @@
 #include "glass/networktables/NTStringChooser.h"
 
 #include <fmt/format.h>
+#include <wpi/json.h>
 
 using namespace glass;
 
@@ -16,17 +17,17 @@
     : m_inst{inst},
       m_default{
           m_inst.GetStringTopic(fmt::format("{}/default", path)).Subscribe("")},
-      m_selected{
-          m_inst.GetStringTopic(fmt::format("{}/selected", path)).GetEntry("")},
+      m_selected{m_inst.GetStringTopic(fmt::format("{}/selected", path))
+                     .Subscribe("")},
+      m_selectedPub{m_inst.GetStringTopic(fmt::format("{}/selected", path))
+                        .PublishEx("string", {{"retained", true}})},
       m_active{
           m_inst.GetStringTopic(fmt::format("{}/active", path)).Subscribe("")},
       m_options{m_inst.GetStringArrayTopic(fmt::format("{}/options", path))
-                    .Subscribe({})} {
-  m_selected.GetTopic().SetRetained(true);
-}
+                    .Subscribe({})} {}
 
 void NTStringChooserModel::SetSelected(std::string_view val) {
-  m_selected.Set(val);
+  m_selectedPub.Set(val);
 }
 
 void NTStringChooserModel::Update() {
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp
index 57469a2..71ff843 100644
--- a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp
@@ -879,7 +879,10 @@
           } else if (desc->IsValid()) {
             // loop over all entries with this type and update
             for (auto&& entryPair : m_entries) {
-              auto ts = entryPair.second->info.type_str;
+              if (!entryPair.second) {
+                continue;
+              }
+              std::string_view ts = entryPair.second->info.type_str;
               if (!wpi::starts_with(ts, "struct:")) {
                 continue;
               }
@@ -901,7 +904,10 @@
           } else {
             // loop over all protobuf entries and update (conservatively)
             for (auto&& entryPair : m_entries) {
-              auto& ts = entryPair.second->info.type_str;
+              if (!entryPair.second) {
+                continue;
+              }
+              std::string_view ts = entryPair.second->info.type_str;
               if (wpi::starts_with(ts, "proto:")) {
                 entryPair.second->UpdateFromValue(*this);
               }
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp
index 413899e..61bac02 100644
--- a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp
@@ -83,6 +83,18 @@
       // FIXME: enabled?
       // data is the last item, so is guaranteed to be null-terminated
       ImGui::MenuItem(path.back().data(), nullptr, &visible, true);
+      // Add type label to smartdashboard sendables
+      if (wpi::starts_with(entry->name, "/SmartDashboard/")) {
+        auto typeEntry = m_typeCache.FindValue(entry->name);
+        if (typeEntry) {
+          ImGui::SameLine();
+          ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+          ImGui::Text("%s", typeEntry->stringVal.c_str());
+          ImGui::PopStyleColor();
+          ImGui::SameLine();
+          ImGui::Dummy(ImVec2(10.0f, 0.0f));
+        }
+      }
       if (!wasVisible && visible) {
         Show(entry.get(), entry->window);
       } else if (wasVisible && !visible && entry->window) {
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/StandardNetworkTables.cpp
index ef61025..9031873 100644
--- a/third_party/allwpilib/glass/src/libnt/native/cpp/StandardNetworkTables.cpp
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/StandardNetworkTables.cpp
@@ -14,6 +14,7 @@
 #include "glass/networktables/NTMechanism2D.h"
 #include "glass/networktables/NTMotorController.h"
 #include "glass/networktables/NTPIDController.h"
+#include "glass/networktables/NTProfiledPIDController.h"
 #include "glass/networktables/NTStringChooser.h"
 #include "glass/networktables/NTSubsystem.h"
 #include "glass/networktables/NetworkTablesProvider.h"
@@ -142,6 +143,18 @@
         });
       });
   provider.Register(
+      NTProfiledPIDControllerModel::kType,
+      [](nt::NetworkTableInstance inst, const char* path) {
+        return std::make_unique<NTProfiledPIDControllerModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char* path) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView([=] {
+          DisplayProfiledPIDController(
+              static_cast<NTProfiledPIDControllerModel*>(model));
+        });
+      });
+  provider.Register(
       NTMotorControllerModel::kType,
       [](nt::NetworkTableInstance inst, const char* path) {
         return std::make_unique<NTMotorControllerModel>(inst, path);
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTPIDController.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTPIDController.h
index f901f72..d19dcea 100644
--- a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTPIDController.h
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTPIDController.h
@@ -29,11 +29,13 @@
   DataSource* GetIData() override { return &m_iData; }
   DataSource* GetDData() override { return &m_dData; }
   DataSource* GetSetpointData() override { return &m_setpointData; }
+  DataSource* GetIZoneData() override { return &m_iZoneData; }
 
   void SetP(double value) override;
   void SetI(double value) override;
   void SetD(double value) override;
   void SetSetpoint(double value) override;
+  void SetIZone(double value) override;
 
   void Update() override;
   bool Exists() override;
@@ -47,11 +49,13 @@
   nt::DoubleEntry m_i;
   nt::DoubleEntry m_d;
   nt::DoubleEntry m_setpoint;
+  nt::DoubleEntry m_iZone;
 
   DataSource m_pData;
   DataSource m_iData;
   DataSource m_dData;
   DataSource m_setpointData;
+  DataSource m_iZoneData;
 
   std::string m_nameValue;
   bool m_controllableValue = false;
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h
new file mode 100644
index 0000000..e968f79
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h
@@ -0,0 +1,64 @@
+// 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 <string>
+#include <string_view>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StringTopic.h>
+
+#include "glass/DataSource.h"
+#include "glass/other/ProfiledPIDController.h"
+
+namespace glass {
+class NTProfiledPIDControllerModel : public ProfiledPIDControllerModel {
+ public:
+  static constexpr const char* kType = "ProfiledPIDController";
+
+  explicit NTProfiledPIDControllerModel(std::string_view path);
+  NTProfiledPIDControllerModel(nt::NetworkTableInstance inst,
+                               std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+
+  DataSource* GetPData() override { return &m_pData; }
+  DataSource* GetIData() override { return &m_iData; }
+  DataSource* GetDData() override { return &m_dData; }
+  DataSource* GetGoalData() override { return &m_goalData; }
+  DataSource* GetIZoneData() override { return &m_iZoneData; }
+
+  void SetP(double value) override;
+  void SetI(double value) override;
+  void SetD(double value) override;
+  void SetGoal(double value) override;
+  void SetIZone(double value) override;
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return !m_controllableValue; }
+
+ private:
+  nt::NetworkTableInstance m_inst;
+  nt::StringSubscriber m_name;
+  nt::BooleanSubscriber m_controllable;
+  nt::DoubleEntry m_p;
+  nt::DoubleEntry m_i;
+  nt::DoubleEntry m_d;
+  nt::DoubleEntry m_goal;
+  nt::DoubleEntry m_iZone;
+
+  DataSource m_pData;
+  DataSource m_iData;
+  DataSource m_dData;
+  DataSource m_goalData;
+  DataSource m_iZoneData;
+
+  std::string m_nameValue;
+  bool m_controllableValue = false;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTStringChooser.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTStringChooser.h
index d770a74..49c4802 100644
--- a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTStringChooser.h
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTStringChooser.h
@@ -39,7 +39,8 @@
  private:
   nt::NetworkTableInstance m_inst;
   nt::StringSubscriber m_default;
-  nt::StringEntry m_selected;
+  nt::StringSubscriber m_selected;
+  nt::StringPublisher m_selectedPub;
   nt::StringSubscriber m_active;
   nt::StringArraySubscriber m_options;
 
diff --git a/third_party/allwpilib/googletest/CMakeLists.txt b/third_party/allwpilib/googletest/CMakeLists.txt
index e53fca1..9c09259 100644
--- a/third_party/allwpilib/googletest/CMakeLists.txt
+++ b/third_party/allwpilib/googletest/CMakeLists.txt
@@ -1,14 +1,15 @@
 include(FetchContent)
 
-FetchContent_Declare(
+fetchcontent_declare(
     googletest
-    GIT_REPOSITORY  https://github.com/google/googletest.git
-    GIT_TAG         58d77fa8070e8cec2dc1ed015d66b454c8d78850 # 1.12.1
+    GIT_REPOSITORY https://github.com/google/googletest.git
+    GIT_TAG
+        58d77fa8070e8cec2dc1ed015d66b454c8d78850 # 1.12.1
 )
 
-FetchContent_GetProperties(googletest)
+fetchcontent_getproperties(googletest)
 if(NOT googletest_POPULATED)
-    FetchContent_Populate(googletest)
+    fetchcontent_populate(googletest)
 
     # Prevent overriding the parent project's compiler/linker
     # settings on Windows
diff --git a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar
index 7f93135..d64cd49 100644
--- a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar
+++ b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar
Binary files differ
diff --git a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties
index 3fa8f86..1af9e09 100644
--- a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties
+++ b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties
@@ -1,6 +1,6 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip
 networkTimeout=10000
 validateDistributionUrl=true
 zipStoreBase=GRADLE_USER_HOME
diff --git a/third_party/allwpilib/hal/CMakeLists.txt b/third_party/allwpilib/hal/CMakeLists.txt
index 603190b..3333e9b 100644
--- a/third_party/allwpilib/hal/CMakeLists.txt
+++ b/third_party/allwpilib/hal/CMakeLists.txt
@@ -3,33 +3,18 @@
 include(CompileWarnings)
 include(AddTest)
 
-file(STRINGS src/generate/Instances.txt RAW_INSTANCES)
-file(STRINGS src/generate/ResourceType.txt RAW_RESOURCE_TYPES)
-
-SET(usage_reporting_types_cpp "")
-SET(usage_reporting_instances_cpp "")
-SET(usage_reporting_types "")
-SET(usage_reporting_instances "")
-
-foreach(ITEM ${RAW_INSTANCES})
-    list(APPEND usage_reporting_instances_cpp "    ${ITEM},")
-    list(APPEND usage_reporting_instances "\n    public static final int ${ITEM};")
-endforeach()
-
-foreach(ITEM ${RAW_RESOURCE_TYPES})
-    list(APPEND usage_reporting_types_cpp "    ${ITEM},")
-    list(APPEND usage_reporting_types "\n    public static final int ${ITEM};")
-endforeach()
-
-string(REPLACE ";" "\n" usage_reporting_types_cpp "${usage_reporting_types_cpp}")
-string(REPLACE ";" "\n" usage_reporting_instances_cpp "${usage_reporting_instances_cpp}")
-
-file(GLOB
-    hal_shared_native_src src/main/native/cpp/*.cpp
-    hal_shared_native_src src/main/native/cpp/cpp/*.cpp
-    hal_shared_native_src src/main/native/cpp/handles/*.cpp
-    hal_sim_native_src src/main/native/sim/*.cpp
-    hal_sim_native_src src/main/native/sim/mockdata/*.cpp)
+file(
+    GLOB hal_shared_native_src
+    src/main/native/cpp/*.cpp
+    hal_shared_native_src
+    src/main/native/cpp/cpp/*.cpp
+    hal_shared_native_src
+    src/main/native/cpp/handles/*.cpp
+    hal_sim_native_src
+    src/main/native/sim/*.cpp
+    hal_sim_native_src
+    src/main/native/sim/mockdata/*.cpp
+)
 add_library(hal ${hal_shared_native_src})
 wpilib_target_warnings(hal)
 set_target_properties(hal PROPERTIES DEBUG_POSTFIX "d")
@@ -40,37 +25,35 @@
     target_sources(hal PRIVATE ${hal_sim_native_src})
 endif()
 
-configure_file(src/generate/FRCUsageReporting.h.in gen/hal/FRCUsageReporting.h)
-
 set_target_properties(hal PROPERTIES OUTPUT_NAME "wpiHal")
 
-target_include_directories(hal PUBLIC
-                $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/hal>)
+target_include_directories(
+    hal
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/hal>
+)
 
-target_include_directories(hal PUBLIC
-                $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/gen>
-                            $<INSTALL_INTERFACE:${include_dest}/hal>)
+target_include_directories(
+    hal
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/generated/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/hal>
+)
 target_link_libraries(hal PUBLIC wpiutil)
 
 set_property(TARGET hal PROPERTY FOLDER "libraries")
 
 install(TARGETS hal EXPORT hal)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/hal")
-install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/gen/ DESTINATION "${include_dest}/hal")
+install(DIRECTORY src/generated/main/native/include/ DESTINATION "${include_dest}/hal")
 
-if (WITH_FLAT_INSTALL)
-    set (hal_config_dir ${wpilib_dest})
-else()
-    set (hal_config_dir share/hal)
-endif()
-
-configure_file(hal-config.cmake.in ${WPILIB_BINARY_DIR}/hal-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/hal-config.cmake DESTINATION ${hal_config_dir})
-install(EXPORT hal DESTINATION ${hal_config_dir})
+configure_file(hal-config.cmake.in ${WPILIB_BINARY_DIR}/hal-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/hal-config.cmake DESTINATION share/hal)
+install(EXPORT hal DESTINATION share/hal)
 
 # Java bindings
-if (WITH_JAVA)
+if(WITH_JAVA)
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
@@ -80,15 +63,19 @@
 
     file(GLOB_RECURSE hal_shared_jni_src src/main/native/cpp/jni/*.cpp)
 
-    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java src/generated/main/java/*.java)
     set(CMAKE_JNI_TARGET true)
 
-    add_jar(hal_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/FRCNetComm.java INCLUDE_JARS wpiutil_jar OUTPUT_NAME wpiHal GENERATE_NATIVE_HEADERS hal_jni_headers)
+    add_jar(
+        hal_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS wpiutil_jar
+        OUTPUT_NAME wpiHal
+        GENERATE_NATIVE_HEADERS hal_jni_headers
+    )
 
-    get_property(HAL_JAR_FILE TARGET hal_jar PROPERTY JAR_FILE)
-    install(FILES ${HAL_JAR_FILE} DESTINATION "${java_lib_dest}")
-
-    set_property(TARGET hal_jar PROPERTY FOLDER "java")
+    install_jar(hal_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS hal_jar FILE hal_jar.cmake DESTINATION share/hal)
 
     add_library(haljni ${hal_shared_jni_src})
 
@@ -107,23 +94,27 @@
     add_dependencies(haljni hal_jar)
 
     install(TARGETS haljni EXPORT haljni)
-
 endif()
 
-if (WITH_JAVA_SOURCE)
+if(WITH_JAVA_SOURCE)
     find_package(Java REQUIRED)
     include(UseJava)
-    file(GLOB HAL_SOURCES src/main/java/edu/wpi/first/hal/*.java ${CMAKE_CURRENT_BINARY_DIR}/FRCNetComm.java)
+    file(GLOB HAL_SOURCES src/main/java/edu/wpi/first/hal/*.java src/generated/main/java/*.java)
     file(GLOB HAL_CAN_SOURCES src/main/java/edu/wpi/first/hal/can/*.java)
     file(GLOB HAL_SIMULATION_SOURCES src/main/java/edu/wpi/first/hal/simulation/*.java)
     file(GLOB HAL_UTIL_SOURCES src/main/java/edu/wpi/first/hal/util/*.java)
-    add_jar(hal_src_jar
-    RESOURCES NAMESPACE "edu/wpi/first/hal" ${HAL_SOURCES}
-    NAMESPACE "edu/wpi/first/hal/can" ${HAL_CAN_SOURCES}
-    NAMESPACE "edu/wpi/first/hal/communication" src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
-    NAMESPACE "edu/wpi/first/hal/simulation" ${HAL_SIMULATION_SOURCES}
-    NAMESPACE "edu/wpi/first/hal/util" ${HAL_UTIL_SOURCES}
-    OUTPUT_NAME wpiHal-sources)
+    add_jar(
+        hal_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/hal" ${HAL_SOURCES}
+        NAMESPACE "edu/wpi/first/hal/can" ${HAL_CAN_SOURCES}
+        NAMESPACE
+            "edu/wpi/first/hal/communication"
+            src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
+        NAMESPACE "edu/wpi/first/hal/simulation" ${HAL_SIMULATION_SOURCES}
+        NAMESPACE "edu/wpi/first/hal/util" ${HAL_UTIL_SOURCES}
+        OUTPUT_NAME wpiHal-sources
+    )
 
     get_property(HAL_SRC_JAR_FILE TARGET hal_src_jar PROPERTY JAR_FILE)
     install(FILES ${HAL_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
@@ -131,7 +122,7 @@
     set_property(TARGET hal_src_jar PROPERTY FOLDER "java")
 endif()
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(hal src/test/native/cpp)
     target_link_libraries(hal_test hal gtest)
 endif()
diff --git a/third_party/allwpilib/hal/build.gradle b/third_party/allwpilib/hal/build.gradle
index 03d07a0..8abdfdc 100644
--- a/third_party/allwpilib/hal/build.gradle
+++ b/third_party/allwpilib/hal/build.gradle
@@ -1,60 +1,9 @@
-def javaResourceFile = file("$buildDir/generated/java/edu/wpi/first/hal/FRCNetComm.java")
-def cppResourceFile = file("$buildDir/generated/headers/hal/FRCUsageReporting.h")
-
-def generateUsageReporting = tasks.register('generateUsageReporting') {
-    def javaBase = file('src/generate/FRCNetComm.java.in')
-    def cppBase = file('src/generate/FRCUsageReporting.h.in')
-
-    def instances = file('src/generate/Instances.txt')
-    def resourceType = file('src/generate/ResourceType.txt')
-
-    inputs.file(javaBase)
-    inputs.file(cppBase)
-    inputs.file(instances)
-    inputs.file(resourceType)
-
-    outputs.file(javaResourceFile)
-    outputs.file(cppResourceFile)
-
-    doLast {
-        def instanceTextJava = instances as String[]
-        def instanceTextCpp = instances as String[]
-        def resourceTextJava = resourceType as String[]
-        def resourceTextCpp = resourceType as String[]
-
-        instanceTextJava = instanceTextJava.collect {
-            "    public static final int ${it};"
-        }.join('\n')
-
-        instanceTextCpp = instanceTextCpp.collect {
-            "    ${it},"
-        }.join('\n')
-
-        resourceTextJava = resourceTextJava.collect {
-            "    public static final int ${it};"
-        }.join('\n')
-
-        resourceTextCpp = resourceTextCpp.collect {
-            "    ${it},"
-        }.join('\n')
-
-        javaResourceFile.text = javaBase.text.replace('${usage_reporting_types}', resourceTextJava).replace('${usage_reporting_instances}', instanceTextJava)
-        cppResourceFile.text = cppBase.text.replace('${usage_reporting_types_cpp}', resourceTextCpp).replace('${usage_reporting_instances_cpp}', instanceTextCpp)
-    }
-}
-
 ext {
     addHalDependency = { binary, shared->
-        binary.tasks.withType(AbstractNativeSourceCompileTask) {
-            it.dependsOn generateUsageReporting
-        }
         binary.lib project: ':hal', library: 'hal', linkage: shared
     }
 
     addHalJniDependency = { binary->
-        binary.tasks.withType(AbstractNativeSourceCompileTask) {
-            it.dependsOn generateUsageReporting
-        }
         binary.lib project: ':hal', library: 'halJNIShared', linkage: 'shared'
     }
 
@@ -62,16 +11,8 @@
     setBaseName = 'wpiHal'
     devMain = 'edu.wpi.first.hal.DevMain'
     niLibraries = true
-    generatedHeaders = "$buildDir/generated/headers"
-    jniSplitSetup = {
-        it.tasks.withType(AbstractNativeSourceCompileTask) {
-            it.dependsOn generateUsageReporting
-        }
-    }
+    generatedHeaders = "src/generated/main/native/include"
     splitSetup = {
-        it.tasks.withType(AbstractNativeSourceCompileTask) {
-            it.dependsOn generateUsageReporting
-        }
         if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
             it.sources {
                 athenaCpp(CppSourceSet) {
@@ -109,11 +50,7 @@
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
-sourceSets.main.java.srcDir "${buildDir}/generated/java/"
-
-compileJava {
-    dependsOn generateUsageReporting
-}
+sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
 
 cppSourcesZip {
     from('src/main/native/athena') {
@@ -126,7 +63,6 @@
 }
 
 cppHeadersZip {
-    dependsOn generateUsageReporting
     from(generatedHeaders) {
         into '/'
     }
@@ -160,9 +96,6 @@
 model {
     binaries {
         all {
-            it.tasks.withType(AbstractNativeSourceCompileTask) {
-                it.dependsOn generateUsageReporting
-            }
             if (!(it instanceof NativeBinarySpec)) return
                 if (it.component.name != 'hal' && it.component.name != 'halBase') return
                 if (it.targetPlatform.name != nativeUtils.wpi.platforms.roborio) return
diff --git a/third_party/allwpilib/hal/generate_usage_reporting.py b/third_party/allwpilib/hal/generate_usage_reporting.py
new file mode 100755
index 0000000..c355ea2
--- /dev/null
+++ b/third_party/allwpilib/hal/generate_usage_reporting.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+# 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.
+import pathlib
+
+
+def main():
+    # Gets the folder this script is in (the hal/ directory)
+    HAL_ROOT = pathlib.Path(__file__).parent
+    java_package = "edu/wpi/first/hal"
+    (HAL_ROOT/"src/generated/main/native/include/hal").mkdir(parents=True, exist_ok=True)
+    (HAL_ROOT/f"src/generated/main/java/{java_package}").mkdir(parents=True, exist_ok=True)
+    usage_reporting_types_cpp = [] 
+    usage_reporting_instances_cpp = []
+    usage_reporting_types = []
+    usage_reporting_instances = []
+    with open(HAL_ROOT/"src/generate/Instances.txt") as instances:
+        for instance in instances:
+            usage_reporting_instances_cpp.append(f"    {instance.strip()},")
+            usage_reporting_instances.append(
+                f"    /** {instance.strip()}. */\n"
+                f"    public static final int {instance.strip()};")
+
+    with open(HAL_ROOT/"src/generate/ResourceType.txt") as resource_types:
+        for resource_type in resource_types:
+            usage_reporting_types_cpp.append(f"    {resource_type.strip()},")
+            usage_reporting_types.append(
+                f"    /** {resource_type.strip()}. */\n"
+                f"    public static final int {resource_type.strip()};")
+
+    with open(HAL_ROOT/"src/generate/FRCNetComm.java.in") as java_usage_reporting:
+        contents = (java_usage_reporting.read()
+            .replace(r"${usage_reporting_types}", "\n".join(usage_reporting_types))
+            .replace(r"${usage_reporting_instances}", "\n".join(usage_reporting_instances)))
+        
+        with open(HAL_ROOT/f"src/generated/main/java/{java_package}/FRCNetComm.java", "w") as java_out:
+            java_out.write(contents)
+
+    with open(HAL_ROOT/"src/generate/FRCUsageReporting.h.in") as cpp_usage_reporting:
+        contents = (cpp_usage_reporting.read()
+            .replace(r"${usage_reporting_types_cpp}", "\n".join(usage_reporting_types_cpp))
+            .replace(r"${usage_reporting_instances_cpp}", "\n".join(usage_reporting_instances_cpp)))
+
+        with open(HAL_ROOT/"src/generated/main/native/include/hal/FRCUsageReporting.h", "w") as cpp_out:
+            cpp_out.write(contents)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/third_party/allwpilib/hal/hal-config.cmake.in b/third_party/allwpilib/hal/hal-config.cmake.in
index 49eac3d..7429597 100644
--- a/third_party/allwpilib/hal/hal-config.cmake.in
+++ b/third_party/allwpilib/hal/hal-config.cmake.in
@@ -4,3 +4,6 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/hal.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/hal_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in b/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in
index 703e034..e58a578 100644
--- a/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in
+++ b/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in
@@ -1,13 +1,15 @@
-/*
- * Autogenerated file! Do not manually edit this file.
- */
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./hal/generate_usage_reporting.py. DO NOT MODIFY
 
 package edu.wpi.first.hal;
 
 /**
  * JNI wrapper for library <b>FRC_NetworkCommunication</b><br>.
  */
-public class FRCNetComm {
+public final class FRCNetComm {
   /**
    * Resource type from UsageReporting.
    */
@@ -29,4 +31,7 @@
 
 ${usage_reporting_instances}
   }
+
+  /** Utility class. */
+  private FRCNetComm() {}
 }
diff --git a/third_party/allwpilib/hal/src/generate/FRCUsageReporting.h.in b/third_party/allwpilib/hal/src/generate/FRCUsageReporting.h.in
index 34330c8..03a78f0 100644
--- a/third_party/allwpilib/hal/src/generate/FRCUsageReporting.h.in
+++ b/third_party/allwpilib/hal/src/generate/FRCUsageReporting.h.in
@@ -1,3 +1,9 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./hal/generate_usage_reporting.py. DO NOT MODIFY
+
 #pragma once
 
 #include <stdint.h>
diff --git a/third_party/allwpilib/hal/src/generate/Instances.txt b/third_party/allwpilib/hal/src/generate/Instances.txt
index bc30057..fc27682 100644
--- a/third_party/allwpilib/hal/src/generate/Instances.txt
+++ b/third_party/allwpilib/hal/src/generate/Instances.txt
@@ -12,6 +12,7 @@
 kFramework_Timed = 4
 kFramework_ROS = 5
 kFramework_RobotBuilder = 6
+kFramework_AdvantageKit = 7
 kRobotDrive_ArcadeStandard = 1
 kRobotDrive_ArcadeButtonSpin = 2
 kRobotDrive_ArcadeRatioCurve = 3
@@ -44,6 +45,7 @@
 kCommand_Scheduler = 1
 kCommand2_Scheduler = 2
 kSmartDashboard_Instance = 1
+kSmartDashboard_LiveWindow = 2
 kKinematics_DifferentialDrive = 1
 kKinematics_MecanumDrive = 2
 kKinematics_SwerveDrive = 3
diff --git a/third_party/allwpilib/hal/src/generate/ResourceType.txt b/third_party/allwpilib/hal/src/generate/ResourceType.txt
index 943d8b0..13c42c0 100644
--- a/third_party/allwpilib/hal/src/generate/ResourceType.txt
+++ b/third_party/allwpilib/hal/src/generate/ResourceType.txt
@@ -100,3 +100,17 @@
 kResourceType_CTRE_future12 = 99
 kResourceType_CTRE_future13 = 100
 kResourceType_CTRE_future14 = 101
+kResourceType_ExponentialProfile = 102
+kResourceType_PS4Controller = 103
+kResourceType_PhotonCamera = 104
+kResourceType_PhotonPoseEstimator = 105
+kResourceType_PathPlannerPath = 106
+kResourceType_PathPlannerAuto = 107
+kResourceType_PathFindingCommand = 108
+kResourceType_Redux_future1 = 109
+kResourceType_Redux_future2 = 110
+kResourceType_Redux_future3 = 111
+kResourceType_Redux_future4 = 112
+kResourceType_Redux_future5 = 113
+kResourceType_RevSparkFlexCAN = 114
+kResourceType_RevSparkFlexPWM = 115
diff --git a/third_party/allwpilib/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java b/third_party/allwpilib/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java
new file mode 100644
index 0000000..0814a3d
--- /dev/null
+++ b/third_party/allwpilib/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java
@@ -0,0 +1,375 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./hal/generate_usage_reporting.py. DO NOT MODIFY
+
+package edu.wpi.first.hal;
+
+/**
+ * JNI wrapper for library <b>FRC_NetworkCommunication</b><br>.
+ */
+public final class FRCNetComm {
+  /**
+   * Resource type from UsageReporting.
+   */
+  @SuppressWarnings("TypeName")
+  public static final class tResourceType {
+    private tResourceType() {
+    }
+
+    /** kResourceType_Controller = 0. */
+    public static final int kResourceType_Controller = 0;
+    /** kResourceType_Module = 1. */
+    public static final int kResourceType_Module = 1;
+    /** kResourceType_Language = 2. */
+    public static final int kResourceType_Language = 2;
+    /** kResourceType_CANPlugin = 3. */
+    public static final int kResourceType_CANPlugin = 3;
+    /** kResourceType_Accelerometer = 4. */
+    public static final int kResourceType_Accelerometer = 4;
+    /** kResourceType_ADXL345 = 5. */
+    public static final int kResourceType_ADXL345 = 5;
+    /** kResourceType_AnalogChannel = 6. */
+    public static final int kResourceType_AnalogChannel = 6;
+    /** kResourceType_AnalogTrigger = 7. */
+    public static final int kResourceType_AnalogTrigger = 7;
+    /** kResourceType_AnalogTriggerOutput = 8. */
+    public static final int kResourceType_AnalogTriggerOutput = 8;
+    /** kResourceType_CANJaguar = 9. */
+    public static final int kResourceType_CANJaguar = 9;
+    /** kResourceType_Compressor = 10. */
+    public static final int kResourceType_Compressor = 10;
+    /** kResourceType_Counter = 11. */
+    public static final int kResourceType_Counter = 11;
+    /** kResourceType_Dashboard = 12. */
+    public static final int kResourceType_Dashboard = 12;
+    /** kResourceType_DigitalInput = 13. */
+    public static final int kResourceType_DigitalInput = 13;
+    /** kResourceType_DigitalOutput = 14. */
+    public static final int kResourceType_DigitalOutput = 14;
+    /** kResourceType_DriverStationCIO = 15. */
+    public static final int kResourceType_DriverStationCIO = 15;
+    /** kResourceType_DriverStationEIO = 16. */
+    public static final int kResourceType_DriverStationEIO = 16;
+    /** kResourceType_DriverStationLCD = 17. */
+    public static final int kResourceType_DriverStationLCD = 17;
+    /** kResourceType_Encoder = 18. */
+    public static final int kResourceType_Encoder = 18;
+    /** kResourceType_GearTooth = 19. */
+    public static final int kResourceType_GearTooth = 19;
+    /** kResourceType_Gyro = 20. */
+    public static final int kResourceType_Gyro = 20;
+    /** kResourceType_I2C = 21. */
+    public static final int kResourceType_I2C = 21;
+    /** kResourceType_Framework = 22. */
+    public static final int kResourceType_Framework = 22;
+    /** kResourceType_Jaguar = 23. */
+    public static final int kResourceType_Jaguar = 23;
+    /** kResourceType_Joystick = 24. */
+    public static final int kResourceType_Joystick = 24;
+    /** kResourceType_Kinect = 25. */
+    public static final int kResourceType_Kinect = 25;
+    /** kResourceType_KinectStick = 26. */
+    public static final int kResourceType_KinectStick = 26;
+    /** kResourceType_PIDController = 27. */
+    public static final int kResourceType_PIDController = 27;
+    /** kResourceType_Preferences = 28. */
+    public static final int kResourceType_Preferences = 28;
+    /** kResourceType_PWM = 29. */
+    public static final int kResourceType_PWM = 29;
+    /** kResourceType_Relay = 30. */
+    public static final int kResourceType_Relay = 30;
+    /** kResourceType_RobotDrive = 31. */
+    public static final int kResourceType_RobotDrive = 31;
+    /** kResourceType_SerialPort = 32. */
+    public static final int kResourceType_SerialPort = 32;
+    /** kResourceType_Servo = 33. */
+    public static final int kResourceType_Servo = 33;
+    /** kResourceType_Solenoid = 34. */
+    public static final int kResourceType_Solenoid = 34;
+    /** kResourceType_SPI = 35. */
+    public static final int kResourceType_SPI = 35;
+    /** kResourceType_Task = 36. */
+    public static final int kResourceType_Task = 36;
+    /** kResourceType_Ultrasonic = 37. */
+    public static final int kResourceType_Ultrasonic = 37;
+    /** kResourceType_Victor = 38. */
+    public static final int kResourceType_Victor = 38;
+    /** kResourceType_Button = 39. */
+    public static final int kResourceType_Button = 39;
+    /** kResourceType_Command = 40. */
+    public static final int kResourceType_Command = 40;
+    /** kResourceType_AxisCamera = 41. */
+    public static final int kResourceType_AxisCamera = 41;
+    /** kResourceType_PCVideoServer = 42. */
+    public static final int kResourceType_PCVideoServer = 42;
+    /** kResourceType_SmartDashboard = 43. */
+    public static final int kResourceType_SmartDashboard = 43;
+    /** kResourceType_Talon = 44. */
+    public static final int kResourceType_Talon = 44;
+    /** kResourceType_HiTechnicColorSensor = 45. */
+    public static final int kResourceType_HiTechnicColorSensor = 45;
+    /** kResourceType_HiTechnicAccel = 46. */
+    public static final int kResourceType_HiTechnicAccel = 46;
+    /** kResourceType_HiTechnicCompass = 47. */
+    public static final int kResourceType_HiTechnicCompass = 47;
+    /** kResourceType_SRF08 = 48. */
+    public static final int kResourceType_SRF08 = 48;
+    /** kResourceType_AnalogOutput = 49. */
+    public static final int kResourceType_AnalogOutput = 49;
+    /** kResourceType_VictorSP = 50. */
+    public static final int kResourceType_VictorSP = 50;
+    /** kResourceType_PWMTalonSRX = 51. */
+    public static final int kResourceType_PWMTalonSRX = 51;
+    /** kResourceType_CANTalonSRX = 52. */
+    public static final int kResourceType_CANTalonSRX = 52;
+    /** kResourceType_ADXL362 = 53. */
+    public static final int kResourceType_ADXL362 = 53;
+    /** kResourceType_ADXRS450 = 54. */
+    public static final int kResourceType_ADXRS450 = 54;
+    /** kResourceType_RevSPARK = 55. */
+    public static final int kResourceType_RevSPARK = 55;
+    /** kResourceType_MindsensorsSD540 = 56. */
+    public static final int kResourceType_MindsensorsSD540 = 56;
+    /** kResourceType_DigitalGlitchFilter = 57. */
+    public static final int kResourceType_DigitalGlitchFilter = 57;
+    /** kResourceType_ADIS16448 = 58. */
+    public static final int kResourceType_ADIS16448 = 58;
+    /** kResourceType_PDP = 59. */
+    public static final int kResourceType_PDP = 59;
+    /** kResourceType_PCM = 60. */
+    public static final int kResourceType_PCM = 60;
+    /** kResourceType_PigeonIMU = 61. */
+    public static final int kResourceType_PigeonIMU = 61;
+    /** kResourceType_NidecBrushless = 62. */
+    public static final int kResourceType_NidecBrushless = 62;
+    /** kResourceType_CANifier = 63. */
+    public static final int kResourceType_CANifier = 63;
+    /** kResourceType_TalonFX = 64. */
+    public static final int kResourceType_TalonFX = 64;
+    /** kResourceType_CTRE_future1 = 65. */
+    public static final int kResourceType_CTRE_future1 = 65;
+    /** kResourceType_CTRE_future2 = 66. */
+    public static final int kResourceType_CTRE_future2 = 66;
+    /** kResourceType_CTRE_future3 = 67. */
+    public static final int kResourceType_CTRE_future3 = 67;
+    /** kResourceType_CTRE_future4 = 68. */
+    public static final int kResourceType_CTRE_future4 = 68;
+    /** kResourceType_CTRE_future5 = 69. */
+    public static final int kResourceType_CTRE_future5 = 69;
+    /** kResourceType_CTRE_future6 = 70. */
+    public static final int kResourceType_CTRE_future6 = 70;
+    /** kResourceType_LinearFilter = 71. */
+    public static final int kResourceType_LinearFilter = 71;
+    /** kResourceType_XboxController = 72. */
+    public static final int kResourceType_XboxController = 72;
+    /** kResourceType_UsbCamera = 73. */
+    public static final int kResourceType_UsbCamera = 73;
+    /** kResourceType_NavX = 74. */
+    public static final int kResourceType_NavX = 74;
+    /** kResourceType_Pixy = 75. */
+    public static final int kResourceType_Pixy = 75;
+    /** kResourceType_Pixy2 = 76. */
+    public static final int kResourceType_Pixy2 = 76;
+    /** kResourceType_ScanseSweep = 77. */
+    public static final int kResourceType_ScanseSweep = 77;
+    /** kResourceType_Shuffleboard = 78. */
+    public static final int kResourceType_Shuffleboard = 78;
+    /** kResourceType_CAN = 79. */
+    public static final int kResourceType_CAN = 79;
+    /** kResourceType_DigilentDMC60 = 80. */
+    public static final int kResourceType_DigilentDMC60 = 80;
+    /** kResourceType_PWMVictorSPX = 81. */
+    public static final int kResourceType_PWMVictorSPX = 81;
+    /** kResourceType_RevSparkMaxPWM = 82. */
+    public static final int kResourceType_RevSparkMaxPWM = 82;
+    /** kResourceType_RevSparkMaxCAN = 83. */
+    public static final int kResourceType_RevSparkMaxCAN = 83;
+    /** kResourceType_ADIS16470 = 84. */
+    public static final int kResourceType_ADIS16470 = 84;
+    /** kResourceType_PIDController2 = 85. */
+    public static final int kResourceType_PIDController2 = 85;
+    /** kResourceType_ProfiledPIDController = 86. */
+    public static final int kResourceType_ProfiledPIDController = 86;
+    /** kResourceType_Kinematics = 87. */
+    public static final int kResourceType_Kinematics = 87;
+    /** kResourceType_Odometry = 88. */
+    public static final int kResourceType_Odometry = 88;
+    /** kResourceType_Units = 89. */
+    public static final int kResourceType_Units = 89;
+    /** kResourceType_TrapezoidProfile = 90. */
+    public static final int kResourceType_TrapezoidProfile = 90;
+    /** kResourceType_DutyCycle = 91. */
+    public static final int kResourceType_DutyCycle = 91;
+    /** kResourceType_AddressableLEDs = 92. */
+    public static final int kResourceType_AddressableLEDs = 92;
+    /** kResourceType_FusionVenom = 93. */
+    public static final int kResourceType_FusionVenom = 93;
+    /** kResourceType_CTRE_future7 = 94. */
+    public static final int kResourceType_CTRE_future7 = 94;
+    /** kResourceType_CTRE_future8 = 95. */
+    public static final int kResourceType_CTRE_future8 = 95;
+    /** kResourceType_CTRE_future9 = 96. */
+    public static final int kResourceType_CTRE_future9 = 96;
+    /** kResourceType_CTRE_future10 = 97. */
+    public static final int kResourceType_CTRE_future10 = 97;
+    /** kResourceType_CTRE_future11 = 98. */
+    public static final int kResourceType_CTRE_future11 = 98;
+    /** kResourceType_CTRE_future12 = 99. */
+    public static final int kResourceType_CTRE_future12 = 99;
+    /** kResourceType_CTRE_future13 = 100. */
+    public static final int kResourceType_CTRE_future13 = 100;
+    /** kResourceType_CTRE_future14 = 101. */
+    public static final int kResourceType_CTRE_future14 = 101;
+    /** kResourceType_ExponentialProfile = 102. */
+    public static final int kResourceType_ExponentialProfile = 102;
+    /** kResourceType_PS4Controller = 103. */
+    public static final int kResourceType_PS4Controller = 103;
+    /** kResourceType_PhotonCamera = 104. */
+    public static final int kResourceType_PhotonCamera = 104;
+    /** kResourceType_PhotonPoseEstimator = 105. */
+    public static final int kResourceType_PhotonPoseEstimator = 105;
+    /** kResourceType_PathPlannerPath = 106. */
+    public static final int kResourceType_PathPlannerPath = 106;
+    /** kResourceType_PathPlannerAuto = 107. */
+    public static final int kResourceType_PathPlannerAuto = 107;
+    /** kResourceType_PathFindingCommand = 108. */
+    public static final int kResourceType_PathFindingCommand = 108;
+    /** kResourceType_Redux_future1 = 109. */
+    public static final int kResourceType_Redux_future1 = 109;
+    /** kResourceType_Redux_future2 = 110. */
+    public static final int kResourceType_Redux_future2 = 110;
+    /** kResourceType_Redux_future3 = 111. */
+    public static final int kResourceType_Redux_future3 = 111;
+    /** kResourceType_Redux_future4 = 112. */
+    public static final int kResourceType_Redux_future4 = 112;
+    /** kResourceType_Redux_future5 = 113. */
+    public static final int kResourceType_Redux_future5 = 113;
+    /** kResourceType_RevSparkFlexCAN = 114. */
+    public static final int kResourceType_RevSparkFlexCAN = 114;
+    /** kResourceType_RevSparkFlexPWM = 115. */
+    public static final int kResourceType_RevSparkFlexPWM = 115;
+  }
+
+  /**
+   * Instances from UsageReporting.
+   */
+  @SuppressWarnings("TypeName")
+  public static final class tInstances {
+    private tInstances() {
+    }
+
+    /** kLanguage_LabVIEW = 1. */
+    public static final int kLanguage_LabVIEW = 1;
+    /** kLanguage_CPlusPlus = 2. */
+    public static final int kLanguage_CPlusPlus = 2;
+    /** kLanguage_Java = 3. */
+    public static final int kLanguage_Java = 3;
+    /** kLanguage_Python = 4. */
+    public static final int kLanguage_Python = 4;
+    /** kLanguage_DotNet = 5. */
+    public static final int kLanguage_DotNet = 5;
+    /** kLanguage_Kotlin = 6. */
+    public static final int kLanguage_Kotlin = 6;
+    /** kCANPlugin_BlackJagBridge = 1. */
+    public static final int kCANPlugin_BlackJagBridge = 1;
+    /** kCANPlugin_2CAN = 2. */
+    public static final int kCANPlugin_2CAN = 2;
+    /** kFramework_Iterative = 1. */
+    public static final int kFramework_Iterative = 1;
+    /** kFramework_Simple = 2. */
+    public static final int kFramework_Simple = 2;
+    /** kFramework_CommandControl = 3. */
+    public static final int kFramework_CommandControl = 3;
+    /** kFramework_Timed = 4. */
+    public static final int kFramework_Timed = 4;
+    /** kFramework_ROS = 5. */
+    public static final int kFramework_ROS = 5;
+    /** kFramework_RobotBuilder = 6. */
+    public static final int kFramework_RobotBuilder = 6;
+    /** kFramework_AdvantageKit = 7. */
+    public static final int kFramework_AdvantageKit = 7;
+    /** kRobotDrive_ArcadeStandard = 1. */
+    public static final int kRobotDrive_ArcadeStandard = 1;
+    /** kRobotDrive_ArcadeButtonSpin = 2. */
+    public static final int kRobotDrive_ArcadeButtonSpin = 2;
+    /** kRobotDrive_ArcadeRatioCurve = 3. */
+    public static final int kRobotDrive_ArcadeRatioCurve = 3;
+    /** kRobotDrive_Tank = 4. */
+    public static final int kRobotDrive_Tank = 4;
+    /** kRobotDrive_MecanumPolar = 5. */
+    public static final int kRobotDrive_MecanumPolar = 5;
+    /** kRobotDrive_MecanumCartesian = 6. */
+    public static final int kRobotDrive_MecanumCartesian = 6;
+    /** kRobotDrive2_DifferentialArcade = 7. */
+    public static final int kRobotDrive2_DifferentialArcade = 7;
+    /** kRobotDrive2_DifferentialTank = 8. */
+    public static final int kRobotDrive2_DifferentialTank = 8;
+    /** kRobotDrive2_DifferentialCurvature = 9. */
+    public static final int kRobotDrive2_DifferentialCurvature = 9;
+    /** kRobotDrive2_MecanumCartesian = 10. */
+    public static final int kRobotDrive2_MecanumCartesian = 10;
+    /** kRobotDrive2_MecanumPolar = 11. */
+    public static final int kRobotDrive2_MecanumPolar = 11;
+    /** kRobotDrive2_KilloughCartesian = 12. */
+    public static final int kRobotDrive2_KilloughCartesian = 12;
+    /** kRobotDrive2_KilloughPolar = 13. */
+    public static final int kRobotDrive2_KilloughPolar = 13;
+    /** kDriverStationCIO_Analog = 1. */
+    public static final int kDriverStationCIO_Analog = 1;
+    /** kDriverStationCIO_DigitalIn = 2. */
+    public static final int kDriverStationCIO_DigitalIn = 2;
+    /** kDriverStationCIO_DigitalOut = 3. */
+    public static final int kDriverStationCIO_DigitalOut = 3;
+    /** kDriverStationEIO_Acceleration = 1. */
+    public static final int kDriverStationEIO_Acceleration = 1;
+    /** kDriverStationEIO_AnalogIn = 2. */
+    public static final int kDriverStationEIO_AnalogIn = 2;
+    /** kDriverStationEIO_AnalogOut = 3. */
+    public static final int kDriverStationEIO_AnalogOut = 3;
+    /** kDriverStationEIO_Button = 4. */
+    public static final int kDriverStationEIO_Button = 4;
+    /** kDriverStationEIO_LED = 5. */
+    public static final int kDriverStationEIO_LED = 5;
+    /** kDriverStationEIO_DigitalIn = 6. */
+    public static final int kDriverStationEIO_DigitalIn = 6;
+    /** kDriverStationEIO_DigitalOut = 7. */
+    public static final int kDriverStationEIO_DigitalOut = 7;
+    /** kDriverStationEIO_FixedDigitalOut = 8. */
+    public static final int kDriverStationEIO_FixedDigitalOut = 8;
+    /** kDriverStationEIO_PWM = 9. */
+    public static final int kDriverStationEIO_PWM = 9;
+    /** kDriverStationEIO_Encoder = 10. */
+    public static final int kDriverStationEIO_Encoder = 10;
+    /** kDriverStationEIO_TouchSlider = 11. */
+    public static final int kDriverStationEIO_TouchSlider = 11;
+    /** kADXL345_SPI = 1. */
+    public static final int kADXL345_SPI = 1;
+    /** kADXL345_I2C = 2. */
+    public static final int kADXL345_I2C = 2;
+    /** kCommand_Scheduler = 1. */
+    public static final int kCommand_Scheduler = 1;
+    /** kCommand2_Scheduler = 2. */
+    public static final int kCommand2_Scheduler = 2;
+    /** kSmartDashboard_Instance = 1. */
+    public static final int kSmartDashboard_Instance = 1;
+    /** kSmartDashboard_LiveWindow = 2. */
+    public static final int kSmartDashboard_LiveWindow = 2;
+    /** kKinematics_DifferentialDrive = 1. */
+    public static final int kKinematics_DifferentialDrive = 1;
+    /** kKinematics_MecanumDrive = 2. */
+    public static final int kKinematics_MecanumDrive = 2;
+    /** kKinematics_SwerveDrive = 3. */
+    public static final int kKinematics_SwerveDrive = 3;
+    /** kOdometry_DifferentialDrive = 1. */
+    public static final int kOdometry_DifferentialDrive = 1;
+    /** kOdometry_MecanumDrive = 2. */
+    public static final int kOdometry_MecanumDrive = 2;
+    /** kOdometry_SwerveDrive = 3. */
+    public static final int kOdometry_SwerveDrive = 3;
+  }
+
+  /** Utility class. */
+  private FRCNetComm() {}
+}
diff --git a/third_party/allwpilib/hal/src/generated/main/native/include/hal/FRCUsageReporting.h b/third_party/allwpilib/hal/src/generated/main/native/include/hal/FRCUsageReporting.h
new file mode 100644
index 0000000..024ff13
--- /dev/null
+++ b/third_party/allwpilib/hal/src/generated/main/native/include/hal/FRCUsageReporting.h
@@ -0,0 +1,228 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./hal/generate_usage_reporting.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// ifdef's definition is to allow for default parameters in C++.
+#ifdef __cplusplus
+/**
+ * Reports a hardware usage to the HAL.
+ *
+ * @param resource       the used resource
+ * @param instanceNumber the instance of the resource
+ * @param context        a user specified context index
+ * @param feature        a user specified feature string
+ * @return               the index of the added value in NetComm
+ */
+int64_t HAL_Report(int32_t resource, int32_t instanceNumber,
+                   int32_t context = 0, const char* feature = nullptr);
+#else
+
+/**
+ * Reports a hardware usage to the HAL.
+ *
+ * @param resource       the used resource
+ * @param instanceNumber the instance of the resource
+ * @param context        a user specified context index
+ * @param feature        a user specified feature string
+ * @return               the index of the added value in NetComm
+ */
+int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
+                   const char* feature);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+/*
+ * Autogenerated file! Do not manually edit this file.
+ */
+
+#ifdef __cplusplus
+namespace HALUsageReporting {
+  enum tResourceType : int32_t {
+    kResourceType_Controller = 0,
+    kResourceType_Module = 1,
+    kResourceType_Language = 2,
+    kResourceType_CANPlugin = 3,
+    kResourceType_Accelerometer = 4,
+    kResourceType_ADXL345 = 5,
+    kResourceType_AnalogChannel = 6,
+    kResourceType_AnalogTrigger = 7,
+    kResourceType_AnalogTriggerOutput = 8,
+    kResourceType_CANJaguar = 9,
+    kResourceType_Compressor = 10,
+    kResourceType_Counter = 11,
+    kResourceType_Dashboard = 12,
+    kResourceType_DigitalInput = 13,
+    kResourceType_DigitalOutput = 14,
+    kResourceType_DriverStationCIO = 15,
+    kResourceType_DriverStationEIO = 16,
+    kResourceType_DriverStationLCD = 17,
+    kResourceType_Encoder = 18,
+    kResourceType_GearTooth = 19,
+    kResourceType_Gyro = 20,
+    kResourceType_I2C = 21,
+    kResourceType_Framework = 22,
+    kResourceType_Jaguar = 23,
+    kResourceType_Joystick = 24,
+    kResourceType_Kinect = 25,
+    kResourceType_KinectStick = 26,
+    kResourceType_PIDController = 27,
+    kResourceType_Preferences = 28,
+    kResourceType_PWM = 29,
+    kResourceType_Relay = 30,
+    kResourceType_RobotDrive = 31,
+    kResourceType_SerialPort = 32,
+    kResourceType_Servo = 33,
+    kResourceType_Solenoid = 34,
+    kResourceType_SPI = 35,
+    kResourceType_Task = 36,
+    kResourceType_Ultrasonic = 37,
+    kResourceType_Victor = 38,
+    kResourceType_Button = 39,
+    kResourceType_Command = 40,
+    kResourceType_AxisCamera = 41,
+    kResourceType_PCVideoServer = 42,
+    kResourceType_SmartDashboard = 43,
+    kResourceType_Talon = 44,
+    kResourceType_HiTechnicColorSensor = 45,
+    kResourceType_HiTechnicAccel = 46,
+    kResourceType_HiTechnicCompass = 47,
+    kResourceType_SRF08 = 48,
+    kResourceType_AnalogOutput = 49,
+    kResourceType_VictorSP = 50,
+    kResourceType_PWMTalonSRX = 51,
+    kResourceType_CANTalonSRX = 52,
+    kResourceType_ADXL362 = 53,
+    kResourceType_ADXRS450 = 54,
+    kResourceType_RevSPARK = 55,
+    kResourceType_MindsensorsSD540 = 56,
+    kResourceType_DigitalGlitchFilter = 57,
+    kResourceType_ADIS16448 = 58,
+    kResourceType_PDP = 59,
+    kResourceType_PCM = 60,
+    kResourceType_PigeonIMU = 61,
+    kResourceType_NidecBrushless = 62,
+    kResourceType_CANifier = 63,
+    kResourceType_TalonFX = 64,
+    kResourceType_CTRE_future1 = 65,
+    kResourceType_CTRE_future2 = 66,
+    kResourceType_CTRE_future3 = 67,
+    kResourceType_CTRE_future4 = 68,
+    kResourceType_CTRE_future5 = 69,
+    kResourceType_CTRE_future6 = 70,
+    kResourceType_LinearFilter = 71,
+    kResourceType_XboxController = 72,
+    kResourceType_UsbCamera = 73,
+    kResourceType_NavX = 74,
+    kResourceType_Pixy = 75,
+    kResourceType_Pixy2 = 76,
+    kResourceType_ScanseSweep = 77,
+    kResourceType_Shuffleboard = 78,
+    kResourceType_CAN = 79,
+    kResourceType_DigilentDMC60 = 80,
+    kResourceType_PWMVictorSPX = 81,
+    kResourceType_RevSparkMaxPWM = 82,
+    kResourceType_RevSparkMaxCAN = 83,
+    kResourceType_ADIS16470 = 84,
+    kResourceType_PIDController2 = 85,
+    kResourceType_ProfiledPIDController = 86,
+    kResourceType_Kinematics = 87,
+    kResourceType_Odometry = 88,
+    kResourceType_Units = 89,
+    kResourceType_TrapezoidProfile = 90,
+    kResourceType_DutyCycle = 91,
+    kResourceType_AddressableLEDs = 92,
+    kResourceType_FusionVenom = 93,
+    kResourceType_CTRE_future7 = 94,
+    kResourceType_CTRE_future8 = 95,
+    kResourceType_CTRE_future9 = 96,
+    kResourceType_CTRE_future10 = 97,
+    kResourceType_CTRE_future11 = 98,
+    kResourceType_CTRE_future12 = 99,
+    kResourceType_CTRE_future13 = 100,
+    kResourceType_CTRE_future14 = 101,
+    kResourceType_ExponentialProfile = 102,
+    kResourceType_PS4Controller = 103,
+    kResourceType_PhotonCamera = 104,
+    kResourceType_PhotonPoseEstimator = 105,
+    kResourceType_PathPlannerPath = 106,
+    kResourceType_PathPlannerAuto = 107,
+    kResourceType_PathFindingCommand = 108,
+    kResourceType_Redux_future1 = 109,
+    kResourceType_Redux_future2 = 110,
+    kResourceType_Redux_future3 = 111,
+    kResourceType_Redux_future4 = 112,
+    kResourceType_Redux_future5 = 113,
+    kResourceType_RevSparkFlexCAN = 114,
+    kResourceType_RevSparkFlexPWM = 115,
+  };
+  enum tInstances : int32_t {
+    kLanguage_LabVIEW = 1,
+    kLanguage_CPlusPlus = 2,
+    kLanguage_Java = 3,
+    kLanguage_Python = 4,
+    kLanguage_DotNet = 5,
+    kLanguage_Kotlin = 6,
+    kCANPlugin_BlackJagBridge = 1,
+    kCANPlugin_2CAN = 2,
+    kFramework_Iterative = 1,
+    kFramework_Simple = 2,
+    kFramework_CommandControl = 3,
+    kFramework_Timed = 4,
+    kFramework_ROS = 5,
+    kFramework_RobotBuilder = 6,
+    kFramework_AdvantageKit = 7,
+    kRobotDrive_ArcadeStandard = 1,
+    kRobotDrive_ArcadeButtonSpin = 2,
+    kRobotDrive_ArcadeRatioCurve = 3,
+    kRobotDrive_Tank = 4,
+    kRobotDrive_MecanumPolar = 5,
+    kRobotDrive_MecanumCartesian = 6,
+    kRobotDrive2_DifferentialArcade = 7,
+    kRobotDrive2_DifferentialTank = 8,
+    kRobotDrive2_DifferentialCurvature = 9,
+    kRobotDrive2_MecanumCartesian = 10,
+    kRobotDrive2_MecanumPolar = 11,
+    kRobotDrive2_KilloughCartesian = 12,
+    kRobotDrive2_KilloughPolar = 13,
+    kDriverStationCIO_Analog = 1,
+    kDriverStationCIO_DigitalIn = 2,
+    kDriverStationCIO_DigitalOut = 3,
+    kDriverStationEIO_Acceleration = 1,
+    kDriverStationEIO_AnalogIn = 2,
+    kDriverStationEIO_AnalogOut = 3,
+    kDriverStationEIO_Button = 4,
+    kDriverStationEIO_LED = 5,
+    kDriverStationEIO_DigitalIn = 6,
+    kDriverStationEIO_DigitalOut = 7,
+    kDriverStationEIO_FixedDigitalOut = 8,
+    kDriverStationEIO_PWM = 9,
+    kDriverStationEIO_Encoder = 10,
+    kDriverStationEIO_TouchSlider = 11,
+    kADXL345_SPI = 1,
+    kADXL345_I2C = 2,
+    kCommand_Scheduler = 1,
+    kCommand2_Scheduler = 2,
+    kSmartDashboard_Instance = 1,
+    kSmartDashboard_LiveWindow = 2,
+    kKinematics_DifferentialDrive = 1,
+    kKinematics_MecanumDrive = 2,
+    kKinematics_SwerveDrive = 3,
+    kOdometry_DifferentialDrive = 1,
+    kOdometry_MecanumDrive = 2,
+    kOdometry_SwerveDrive = 3,
+  };
+}
+#endif
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java
index 5f34924..fc1bbb5 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java
@@ -59,4 +59,7 @@
    * @return the Z acceleration
    */
   public static native double getAccelerometerZ();
+
+  /** Utility class. */
+  private AccelerometerJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java
index f416ff8..569c94b 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java
@@ -13,6 +13,9 @@
   /** The number of sample value was accumulated over. */
   public long count;
 
+  /** Constructs an AccumulatorResult. */
+  public AccumulatorResult() {}
+
   /**
    * Set the value and count.
    *
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
index b2695a3..a7a3552 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
@@ -97,4 +97,7 @@
    * @see "HAL_StopAddressableLEDOutput"
    */
   public static native void stop(int handle);
+
+  /** Utility class. */
+  private AddressableLEDJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java
index 56c2143..6b1fd84 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java
@@ -4,12 +4,20 @@
 
 package edu.wpi.first.hal;
 
+/** Alliance station ID. */
 public enum AllianceStationID {
+  /** Unknown. */
   Unknown,
+  /** Red 1. */
   Red1,
+  /** Red 2. */
   Red2,
+  /** Red 3. */
   Red3,
+  /** Blue 1. */
   Blue1,
+  /** Blue 2. */
   Blue2,
+  /** Blue 3. */
   Blue3
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java
index cbc3d4f..5d483f9 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java
@@ -126,4 +126,7 @@
    * @see "HAL_GetAnalogGyroCenter"
    */
   public static native int getAnalogGyroCenter(int handle);
+
+  /** Utility class. */
+  private AnalogGyroJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
index bf8b27f..2884b8c 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
@@ -84,6 +84,16 @@
    */
   public static native boolean checkAnalogInputChannel(int channel);
 
+  /**
+   * Checks that the analog output channel number is valid.
+   *
+   * <p>Verifies that the analog channel number is one of the legal channel numbers. Channel numbers
+   * are 0-based.
+   *
+   * @param channel The analog output channel number.
+   * @return Analog channel is valid
+   * @see "HAL_CheckAnalogOutputChannel"
+   */
   public static native boolean checkAnalogOutputChannel(int channel);
 
   /**
@@ -95,8 +105,22 @@
    */
   public static native void setAnalogInputSimDevice(int handle, int device);
 
+  /**
+   * Sets an analog output value.
+   *
+   * @param portHandle the analog output handle
+   * @param voltage the voltage (0-5v) to output
+   * @see "HAL_SetAnalogOutput"
+   */
   public static native void setAnalogOutput(int portHandle, double voltage);
 
+  /**
+   * Gets the current analog output value.
+   *
+   * @param portHandle the analog output handle
+   * @return the current output voltage (0-5v)
+   * @see "HAL_GetAnalogOutput"
+   */
   public static native double getAnalogOutput(int portHandle);
 
   /**
@@ -497,4 +521,7 @@
    * @see "HAL_GetAnalogTriggerFPGAIndex"
    */
   public static native int getAnalogTriggerFPGAIndex(int analogTriggerHandle);
+
+  /** Utility class. */
+  private AnalogJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
index b42def4..90b1688 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
@@ -177,4 +177,7 @@
    */
   public static native boolean readCANPacketTimeout(
       int handle, int apiId, int timeoutMs, CANData data);
+
+  /** Utility class. */
+  private CANAPIJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java
new file mode 100644
index 0000000..5a8a3e6
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java
@@ -0,0 +1,117 @@
+// 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.
+
+package edu.wpi.first.hal;
+
+/**
+ * CAN API Types.
+ *
+ * <p>This class defines enums for CAN device types and manufacturer IDs as specified in the WPILib
+ * documentation: https://docs.wpilib.org/en/stable/docs/software/can-devices/can-addressing.html
+ */
+@SuppressWarnings("PMD.MissingStaticMethodInNonInstantiatableClass")
+public final class CANAPITypes {
+  /**
+   * FRC CAN device type.
+   *
+   * <p>This enum represents different types of CAN devices. Teams are encouraged to use the
+   * kMiscellaneous for custom or miscellaneous devices.
+   *
+   * @see <a href=
+   *     "https://docs.wpilib.org/en/stable/docs/software/can-devices/can-addressing.html">CAN
+   *     Device Types</a>
+   */
+  public enum CANDeviceType {
+    /** Broadcast. */
+    kBroadcast(0),
+    /** Robot controller. */
+    kRobotController(1),
+    /** Motor controller. */
+    kMotorController(2),
+    /** Relay controller. */
+    kRelayController(3),
+    /** Gyro sensor. */
+    kGyroSensor(4),
+    /** Accelerometer. */
+    kAccelerometer(5),
+    /** Ultrasonic sensor. */
+    kUltrasonicSensor(6),
+    /** Gear tooth sensor. */
+    kGearToothSensor(7),
+    /** Power distribution. */
+    kPowerDistribution(8),
+    /** Pneumatics. */
+    kPneumatics(9),
+    /** Miscellaneous. */
+    kMiscellaneous(10),
+    /** IO breakout. */
+    kIOBreakout(11),
+    /** Firmware update. */
+    kFirmwareUpdate(31);
+
+    @SuppressWarnings("PMD.MemberName")
+    public final int id;
+
+    CANDeviceType(int id) {
+      this.id = id;
+    }
+  }
+
+  /**
+   * FRC CAN manufacturer ID.
+   *
+   * <p>This enum represents different manufacturer IDs for CAN devices. Teams are encouraged to use
+   * the kTeamUse manufacturer ID for custom or team-specific devices.
+   *
+   * @see <a href=
+   *     "https://docs.wpilib.org/en/stable/docs/software/can-devices/can-addressing.html">CAN
+   *     Manufacturer IDs</a>
+   */
+  public enum CANManufacturer {
+    /** Broadcast. */
+    kBroadcast(0),
+    /** National Instruments. */
+    kNI(1),
+    /** Luminary Micro. */
+    kLM(2),
+    /** DEKA Research and Development Corp. */
+    kDEKA(3),
+    /** Cross the Road Electronics. */
+    kCTRE(4),
+    /** REV Robotics. */
+    kREV(5),
+    /** Grapple. */
+    kGrapple(6),
+    /** MindSensors. */
+    kMS(7),
+    /** Team use. */
+    kTeamUse(8),
+    /** Kauai Labs. */
+    kKauaiLabs(9),
+    /** Copperforge. */
+    kCopperforge(10),
+    /** Playing With Fusion. */
+    kPWF(11),
+    /** Studica. */
+    kStudica(12),
+    /** TheThriftyBot. */
+    kTheThriftyBot(13),
+    /** Redux Robotics. */
+    kReduxRobotics(14),
+    /** AndyMark. */
+    kAndyMark(15),
+    /** Vivid-Hosting. */
+    kVividHosting(16);
+
+    @SuppressWarnings("PMD.MemberName")
+    public final int id;
+
+    CANManufacturer(int id) {
+      this.id = id;
+    }
+  }
+
+  /** Utility class. */
+  private CANAPITypes() {}
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java
index 0e29568..ffd2f39 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java
@@ -15,6 +15,9 @@
   /** CAN frame timestamp in milliseconds. */
   public long timestamp;
 
+  /** Default constructor. */
+  public CANData() {}
+
   /**
    * API used from JNI to set the data.
    *
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANStreamMessage.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANStreamMessage.java
index bdb2112..5706aa0 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANStreamMessage.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANStreamMessage.java
@@ -17,6 +17,9 @@
   @SuppressWarnings("MemberName")
   public int messageID;
 
+  /** Default constructor. */
+  public CANStreamMessage() {}
+
   /**
    * API used from JNI to set the data.
    *
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CTREPCMJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CTREPCMJNI.java
index b657da5..0abe084 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CTREPCMJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CTREPCMJNI.java
@@ -221,4 +221,7 @@
    * @see "HAL_SetCTREPCMOneShotDuration"
    */
   public static native void setOneShotDuration(int handle, int index, int durMs);
+
+  /** Utility class. */
+  private CTREPCMJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java
index 4843449..2ccef74 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java
@@ -17,4 +17,7 @@
    * @see "HAL_GetSystemClockTicksPerMicrosecond"
    */
   public static native int getSystemClockTicksPerMicrosecond();
+
+  /** Utility class. */
+  private ConstantsJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
index 7dff0f4..31f8a7d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
@@ -13,6 +13,9 @@
   private boolean m_fmsAttached;
   private boolean m_dsAttached;
 
+  /** Default constructor. */
+  public ControlWord() {}
+
   void update(
       boolean enabled,
       boolean autonomous,
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java
index ebc4cff..6823185 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java
@@ -279,4 +279,7 @@
    * @see "HAL_SetCounterReverseDirection"
    */
   public static native void setCounterReverseDirection(int counterHandle, boolean reverseDirection);
+
+  /** Utility class. */
+  private CounterJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java
index 3330f4a..4d597cc 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java
@@ -178,4 +178,7 @@
    * @see "HAL_SetDigitalPWMOutputChannel"
    */
   public static native void setDigitalPWMOutputChannel(int pwmGenerator, int channel);
+
+  /** Utility class. */
+  DIOJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java
index 00000d2..77eebf0 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java
@@ -245,4 +245,7 @@
    * @return The DMA sample
    */
   public static native DMAJNISample.BaseStore getSensorReadData(int handle);
+
+  /** Utility class. */
+  private DMAJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java
index 22f21c8..df66c55 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java
@@ -26,6 +26,9 @@
   private long m_timeStamp;
   private Map<Integer, BaseStore> m_propertyMap = new HashMap<>();
 
+  /** Default constructor. */
+  public DMAJNISample() {}
+
   public int update(int dmaHandle, double timeoutSeconds) {
     m_timeStamp = DMAJNI.readDMA(dmaHandle, timeoutSeconds, m_dataBuffer, m_storage);
     return m_storage[25];
@@ -131,8 +134,7 @@
     }
 
     // + 2 Hack, but needed to not have to call into JNI
-    int value = readValue(data.m_valueType + 2, data.m_index);
-    return value;
+    return readValue(data.m_valueType + 2, data.m_index);
   }
 
   public void getAnalogAccumulator(int analogInputHandle, AccumulatorResult result) {
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java
index 9a16984..ececa13 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java
@@ -59,4 +59,7 @@
    * @see "HAL_GetFilterPeriod"
    */
   public static native int getFilterPeriod(int filterIndex);
+
+  /** Utility class. */
+  private DigitalGlitchFilterJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java
index 4a46b52..a7ec9f5 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java
@@ -372,5 +372,6 @@
    */
   public static native boolean getOutputsActive();
 
+  /** Utility class. */
   private DriverStationJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
index 1e7dfa0..6920b29 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
@@ -78,4 +78,7 @@
    * @see "HAL_GetDutyCycleFPGAIndex"
    */
   public static native int getFPGAIndex(int handle);
+
+  /** Utility class. */
+  private DutyCycleJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java
index d219db0..59503f8 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java
@@ -275,4 +275,7 @@
    * @see "HAL_GetEncoderEncodingType"
    */
   public static native int getEncoderEncodingType(int encoderHandle);
+
+  /** Utility class. */
+  private EncoderJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java
index 11d9b4d..0cadf39 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java
@@ -90,6 +90,10 @@
   /**
    * Gets the state of the "USER" button on the roboRIO.
    *
+   * <p>Warning: the User Button is used to stop user programs from automatically loading if it is
+   * held for more then 5 seconds. Because of this, it's not recommended to be used by teams for any
+   * other purpose.
+   *
    * @return true if the button is currently pressed down
    * @see "HAL_GetFPGAButton"
    */
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java
index 90e8ddb..1b729af 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java
@@ -135,4 +135,7 @@
    * @see "HAL_CloseI2C"
    */
   public static native void i2CClose(int port);
+
+  /** Utility class. */
+  private I2CJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
index c605435..ec60b83 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
@@ -113,4 +113,7 @@
    * @see "HAL_ReleaseWaitingInterrupt"
    */
   public static native void releaseWaitingInterrupt(int interruptHandle);
+
+  /** Utility class. */
+  private InterruptJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java
index 6826cd8..49bcb4a 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java
@@ -13,16 +13,30 @@
   static boolean libraryLoaded = false;
   static RuntimeLoader<JNIWrapper> loader = null;
 
+  /** Sets whether JNI should be loaded in the static block. */
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
     public static boolean getExtractOnStaticLoad() {
       return extractOnStaticLoad.get();
     }
 
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
     public static void setExtractOnStaticLoad(boolean load) {
       extractOnStaticLoad.set(load);
     }
+
+    /** Utility class. */
+    private Helper() {}
   }
 
   static {
@@ -57,4 +71,7 @@
   }
 
   public static void suppressUnused(Object object) {}
+
+  /** Utility class. */
+  public JNIWrapper() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/LEDJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/LEDJNI.java
new file mode 100644
index 0000000..397328e
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/LEDJNI.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.hal;
+
+public class LEDJNI extends JNIWrapper {
+  public static final int RADIO_LED_STATE_OFF = 0;
+  public static final int RADIO_LED_STATE_GREEN = 1;
+  public static final int RADIO_LED_STATE_RED = 2;
+  public static final int RADIO_LED_STATE_ORANGE = 3;
+
+  /**
+   * Set the state of the "Radio" LED.
+   *
+   * @param state The state to set the LED to.
+   * @see "HAL_SetRadioLEDState"
+   */
+  public static native void setRadioLEDState(int state);
+
+  /**
+   * Get the state of the "Radio" LED.
+   *
+   * @return The state of the LED.
+   * @see "HAL_GetRadioLEDState"
+   */
+  public static native int getRadioLEDState();
+
+  /** Utility class. */
+  private LEDJNI() {}
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java
index 699ecc5..d9e9796 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java
@@ -22,6 +22,9 @@
   /** Stores the match type. */
   public int matchType;
 
+  /** Default constructor. */
+  public MatchInfoData() {}
+
   /**
    * Called from JNI to set the structure data.
    *
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
index 5620f03..04784bc 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
@@ -101,4 +101,7 @@
    * @return the FPGA time the notifier returned
    */
   public static native long waitForNotifierAlarm(int notifierHandle);
+
+  /** Utility class. */
+  private NotifierJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java
index dc2abc1..3f0e3d4 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java
@@ -5,12 +5,41 @@
 package edu.wpi.first.hal;
 
 public class PWMJNI extends DIOJNI {
+  /**
+   * Initializes a PWM port.
+   *
+   * @param halPortHandle the port to initialize
+   * @return the created pwm handle
+   */
   public static native int initializePWMPort(int halPortHandle);
 
+  /**
+   * Checks if a pwm channel is valid.
+   *
+   * @param channel the channel to check
+   * @return true if the channel is valid, otherwise false
+   */
   public static native boolean checkPWMChannel(int channel);
 
+  /**
+   * Frees a PWM port.
+   *
+   * @param pwmPortHandle the pwm handle
+   */
   public static native void freePWMPort(int pwmPortHandle);
 
+  /**
+   * Sets the configuration settings for the PWM channel.
+   *
+   * <p>All values are in microseconds.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @param maxPwm the maximum PWM value
+   * @param deadbandMaxPwm the high range of the center deadband
+   * @param centerPwm the center PWM value
+   * @param deadbandMinPwm the low range of the center deadband
+   * @param minPwm the minimum PWM value
+   */
   public static native void setPWMConfigMicroseconds(
       int pwmPortHandle,
       int maxPwm,
@@ -19,29 +48,122 @@
       int deadbandMinPwm,
       int minPwm);
 
+  /**
+   * Gets the pwm configuration settings for the PWM channel.
+   *
+   * <p>Values are in microseconds.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @return the pwm configuration settings
+   */
   public static native PWMConfigDataResult getPWMConfigMicroseconds(int pwmPortHandle);
 
+  /**
+   * Sets if the FPGA should output the center value if the input value is within the deadband.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @param eliminateDeadband true to eliminate deadband, otherwise false
+   */
   public static native void setPWMEliminateDeadband(int pwmPortHandle, boolean eliminateDeadband);
 
+  /**
+   * Gets the current eliminate deadband value.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @return true if set, otherwise false
+   */
   public static native boolean getPWMEliminateDeadband(int pwmPortHandle);
 
+  /**
+   * Sets a PWM channel to the desired pulse width in microseconds.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @param microsecondPulseTime the PWM value to set
+   */
   public static native void setPulseTimeMicroseconds(int pwmPortHandle, int microsecondPulseTime);
 
+  /**
+   * Sets a PWM channel to the desired scaled value.
+   *
+   * <p>The values range from -1 to 1 and the period is controlled by the PWM Period and MinHigh
+   * registers.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @param speed the scaled PWM value to set
+   */
   public static native void setPWMSpeed(int pwmPortHandle, double speed);
 
+  /**
+   * Sets a PWM channel to the desired position value.
+   *
+   * <p>The values range from 0 to 1 and the period is controlled by the PWM Period and MinHigh
+   * registers.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @param position the positional PWM value to set
+   */
   public static native void setPWMPosition(int pwmPortHandle, double position);
 
+  /**
+   * Gets the current microsecond pulse time from a PWM channel.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @return the current PWM microsecond pulse time
+   */
   public static native int getPulseTimeMicroseconds(int pwmPortHandle);
 
+  /**
+   * Gets a scaled value from a PWM channel.
+   *
+   * <p>The values range from -1 to 1.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @return the current speed PWM value
+   */
   public static native double getPWMSpeed(int pwmPortHandle);
 
+  /**
+   * Gets a position value from a PWM channel.
+   *
+   * <p>The values range from 0 to 1.
+   *
+   * @param pwmPortHandle the PWM handle
+   * @return the current positional PWM value
+   */
   public static native double getPWMPosition(int pwmPortHandle);
 
+  /**
+   * Sets a PWM channel to be disabled.
+   *
+   * <p>The channel is disabled until the next time it is set. Note this is different from just
+   * setting a 0 speed, as this will actively stop all signaling on the channel.
+   *
+   * @param pwmPortHandle the PWM handle.
+   */
   public static native void setPWMDisabled(int pwmPortHandle);
 
+  /**
+   * Forces a PWM signal to go to 0 temporarily.
+   *
+   * @param pwmPortHandle the PWM handle.
+   */
   public static native void latchPWMZero(int pwmPortHandle);
 
+  /**
+   * Sets the PWM output to be a continuous high signal while enabled.
+   *
+   * @param pwmPortHandle the PWM handle.
+   */
   public static native void setAlwaysHighMode(int pwmPortHandle);
 
+  /**
+   * Sets how how often the PWM signal is squelched, thus scaling the period.
+   *
+   * @param pwmPortHandle the PWM handle.
+   * @param squelchMask the 2-bit mask of outputs to squelch
+   */
   public static native void setPWMPeriodScale(int pwmPortHandle, int squelchMask);
+
+  /** Utility class. */
+  private PWMJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java
index 20f9b42..b47697b 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java
@@ -185,4 +185,7 @@
    * @see "HAL_GetNumREVPHChannels"
    */
   public static native int getNumREVPHChannels();
+
+  /** Utility class. */
+  private PortsJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java
index aa2cac5..ffc2793 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java
@@ -61,6 +61,69 @@
   public final boolean HardwareFault;
 
   /**
+   * Gets whether there is a breaker fault at the specified channel.
+   *
+   * @param channel Channel to check for faults.
+   * @return True if there is a breaker fault at the channel, otherwise false.
+   * @throws IndexOutOfBoundsException if the provided channel is outside of the range supported by
+   *     the hardware.
+   */
+  public final boolean getBreakerFault(int channel) {
+    switch (channel) {
+      case 0:
+        return Channel0BreakerFault;
+      case 1:
+        return Channel1BreakerFault;
+      case 2:
+        return Channel2BreakerFault;
+      case 3:
+        return Channel3BreakerFault;
+      case 4:
+        return Channel4BreakerFault;
+      case 5:
+        return Channel5BreakerFault;
+      case 6:
+        return Channel6BreakerFault;
+      case 7:
+        return Channel7BreakerFault;
+      case 8:
+        return Channel8BreakerFault;
+      case 9:
+        return Channel9BreakerFault;
+      case 10:
+        return Channel10BreakerFault;
+      case 11:
+        return Channel11BreakerFault;
+      case 12:
+        return Channel12BreakerFault;
+      case 13:
+        return Channel13BreakerFault;
+      case 14:
+        return Channel14BreakerFault;
+      case 15:
+        return Channel15BreakerFault;
+      case 16:
+        return Channel16BreakerFault;
+      case 17:
+        return Channel17BreakerFault;
+      case 18:
+        return Channel18BreakerFault;
+      case 19:
+        return Channel19BreakerFault;
+      case 20:
+        return Channel20BreakerFault;
+      case 21:
+        return Channel21BreakerFault;
+      case 22:
+        return Channel22BreakerFault;
+      case 23:
+        return Channel23BreakerFault;
+      default:
+        throw new IndexOutOfBoundsException("Power distribution fault channel out of bounds!");
+    }
+  }
+
+  /**
    * Constructs from a bitfield.
    *
    * @param faults faults
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java
index bc7060d..93f9a3d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java
@@ -282,4 +282,7 @@
    * @see "HAL_GetPowerDistributionVersion"
    */
   public static native PowerDistributionVersion getVersion(int handle);
+
+  /** Utility class. */
+  private PowerDistributionJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java
index f60f8df..060fbf5 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java
@@ -63,6 +63,69 @@
   public final boolean HasReset;
 
   /**
+   * Gets whether there is a sticky breaker fault at the specified channel.
+   *
+   * @param channel Channel to check for sticky faults.
+   * @return True if there is a sticky breaker fault at the channel, otherwise false.
+   * @throws IndexOutOfBoundsException if the provided channel is outside of the range supported by
+   *     the hardware.
+   */
+  public final boolean getBreakerFault(int channel) {
+    switch (channel) {
+      case 0:
+        return Channel0BreakerFault;
+      case 1:
+        return Channel1BreakerFault;
+      case 2:
+        return Channel2BreakerFault;
+      case 3:
+        return Channel3BreakerFault;
+      case 4:
+        return Channel4BreakerFault;
+      case 5:
+        return Channel5BreakerFault;
+      case 6:
+        return Channel6BreakerFault;
+      case 7:
+        return Channel7BreakerFault;
+      case 8:
+        return Channel8BreakerFault;
+      case 9:
+        return Channel9BreakerFault;
+      case 10:
+        return Channel10BreakerFault;
+      case 11:
+        return Channel11BreakerFault;
+      case 12:
+        return Channel12BreakerFault;
+      case 13:
+        return Channel13BreakerFault;
+      case 14:
+        return Channel14BreakerFault;
+      case 15:
+        return Channel15BreakerFault;
+      case 16:
+        return Channel16BreakerFault;
+      case 17:
+        return Channel17BreakerFault;
+      case 18:
+        return Channel18BreakerFault;
+      case 19:
+        return Channel19BreakerFault;
+      case 20:
+        return Channel20BreakerFault;
+      case 21:
+        return Channel21BreakerFault;
+      case 22:
+        return Channel22BreakerFault;
+      case 23:
+        return Channel23BreakerFault;
+      default:
+        throw new IndexOutOfBoundsException("Power distribution fault channel out of bounds!");
+    }
+  }
+
+  /**
    * Constructs from a bitfield.
    *
    * @param faults faults
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java
index fdd1233..bfb5ff2 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java
@@ -4,18 +4,25 @@
 
 package edu.wpi.first.hal;
 
+/** Power distribution version. */
 @SuppressWarnings("MemberName")
 public class PowerDistributionVersion {
+  /** Firmware major version number. */
   public final int firmwareMajor;
 
+  /** Firmware minor version number. */
   public final int firmwareMinor;
 
+  /** Firmware fix version number. */
   public final int firmwareFix;
 
+  /** Hardware minor version number. */
   public final int hardwareMinor;
 
+  /** Hardware major version number. */
   public final int hardwareMajor;
 
+  /** Unique ID. */
   public final int uniqueId;
 
   /**
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java
index 8dd9548..900b423 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java
@@ -167,4 +167,7 @@
    * @return current CPU temperature in degrees Celsius
    */
   public static native double getCPUTemp();
+
+  /** Utility class. */
+  private PowerJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHFaults.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHFaults.java
index 23f0882..931f8b7 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHFaults.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHFaults.java
@@ -51,6 +51,53 @@
   public final boolean HardwareFault;
 
   /**
+   * Gets whether there is a fault at the specified channel.
+   *
+   * @param channel Channel to check for faults.
+   * @return True if a a fault exists at the channel, otherwise false.
+   * @throws IndexOutOfBoundsException if the provided channel is outside of the range supported by
+   *     the hardware.
+   */
+  public final boolean getChannelFault(int channel) {
+    switch (channel) {
+      case 0:
+        return Channel0Fault;
+      case 1:
+        return Channel1Fault;
+      case 2:
+        return Channel2Fault;
+      case 3:
+        return Channel3Fault;
+      case 4:
+        return Channel4Fault;
+      case 5:
+        return Channel5Fault;
+      case 6:
+        return Channel6Fault;
+      case 7:
+        return Channel7Fault;
+      case 8:
+        return Channel8Fault;
+      case 9:
+        return Channel9Fault;
+      case 10:
+        return Channel10Fault;
+      case 11:
+        return Channel11Fault;
+      case 12:
+        return Channel12Fault;
+      case 13:
+        return Channel13Fault;
+      case 14:
+        return Channel14Fault;
+      case 15:
+        return Channel15Fault;
+      default:
+        throw new IndexOutOfBoundsException("Pneumatics fault channel out of bounds!");
+    }
+  }
+
+  /**
    * Called from HAL to construct.
    *
    * @param faults the fault bitfields
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java
index b1935b2..1180b9b 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java
@@ -286,4 +286,7 @@
    * @see "HAL_GetREVPHVersion"
    */
   public static native REVPHVersion getVersion(int handle);
+
+  /** Utility class. */
+  private REVPHJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java
index 10a39b4..abe146e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java
@@ -57,4 +57,7 @@
    * @see "HAL_GetRelay"
    */
   public static native boolean getRelay(int relayPortHandle);
+
+  /** Utility class. */
+  private RelayJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
index 82b9fed..18a0a8e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
@@ -302,4 +302,7 @@
    */
   public static native void spiConfigureAutoStall(
       int port, int csToSclkTicks, int stallTicks, int pow2BytesPerRead);
+
+  /** Utility class. */
+  private SPIJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java
index e156f3a..052218f 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java
@@ -205,4 +205,7 @@
    * @see "HAL_CloseSerial"
    */
   public static native void serialClose(int handle);
+
+  /** Utility class. */
+  private SerialPortJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java
index 21ac69a..4c57d05 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java
@@ -16,9 +16,13 @@
  * edu.wpi.first.wpilibj.ADXRS450_Gyro} for an example implementation.
  */
 public class SimDevice implements AutoCloseable {
+  /** Sim device direction. */
   public enum Direction {
+    /** Input direction for simulation devices. */
     kInput(SimDeviceJNI.kInput),
+    /** Output direction for simulation devices. */
     kOutput(SimDeviceJNI.kOutput),
+    /** Bidirectional direction for simulation devices. */
     kBidir(SimDeviceJNI.kBidir);
 
     public final int m_value;
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java
index 4352a2d..fdf09b4 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java
@@ -286,4 +286,7 @@
    * @param handle simulated value handle
    */
   public static native void resetSimValue(int handle);
+
+  /** Utility class. */
+  private SimDeviceJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java
index 2ccc808..2183ae3 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java
@@ -37,4 +37,7 @@
    * @see "HAL_SetCurrentThreadPriority"
    */
   public static native boolean setCurrentThreadPriority(boolean realTime, int priority);
+
+  /** Utility class. */
+  private ThreadsJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java
index e0734dd..f137c73 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java
@@ -9,6 +9,11 @@
 import java.nio.ByteBuffer;
 import java.nio.IntBuffer;
 
+/**
+ * CAN API HAL JNI Functions.
+ *
+ * @see "hal/CAN.h"
+ */
 @SuppressWarnings("MethodName")
 public class CANJNI extends JNIWrapper {
   public static final int CAN_SEND_PERIOD_NO_REPEAT = 0;
@@ -18,18 +23,72 @@
   public static final int CAN_IS_FRAME_REMOTE = 0x80000000;
   public static final int CAN_IS_FRAME_11BIT = 0x40000000;
 
+  /** Default constructor. */
+  public CANJNI() {}
+
+  /**
+   * Sends a CAN message.
+   *
+   * @param messageID The ID of the CAN message.
+   * @param data The data bytes to be sent.
+   * @param periodMs The period in milliseconds at which to send the message, use {@link
+   *     #CAN_SEND_PERIOD_NO_REPEAT} for a single send.
+   */
   public static native void FRCNetCommCANSessionMuxSendMessage(
       int messageID, byte[] data, int periodMs);
 
+  /**
+   * Receives a CAN message.
+   *
+   * @param messageID store for the received message ID (output parameter).
+   * @param messageIDMask the message ID mask to look for
+   * @param timeStamp the packet received timestamp (based off of CLOCK_MONOTONIC) (output
+   *     parameter).
+   * @return The data bytes of the received message.
+   */
   public static native byte[] FRCNetCommCANSessionMuxReceiveMessage(
       IntBuffer messageID, int messageIDMask, ByteBuffer timeStamp);
 
+  /**
+   * Retrieves the current status of the CAN bus.
+   *
+   * @param status The CANStatus object to hold the retrieved status.
+   */
   public static native void getCANStatus(CANStatus status);
 
+  /**
+   * Opens a new CAN stream session for receiving CAN messages with specified filters.
+   *
+   * @param messageID The CAN messageID to match against. The bits of the messageID are bitwise
+   *     ANDed with the messageIDMask.
+   * @param messageIDMask The CAN messageIDMask is a bit-wise mask of bits in the messageID to match
+   *     against. This allows matching against multiple frames. For example, providing an messageID
+   *     of 0x2050001 and a mask of 0x1FFF003F would match all REV motor controller frames for a
+   *     device with CAN ID 1. Providing a mask of 0x1FFFFFFF means that only the exact messageID
+   *     will be matched. Providing a mask of 0 would match any frame of any type.
+   * @param maxMessages The maximum number of messages that can be buffered in the session.
+   * @return The handle to the opened CAN stream session.
+   */
   public static native int openCANStreamSession(int messageID, int messageIDMask, int maxMessages);
 
+  /**
+   * Closes a CAN stream session.
+   *
+   * @param sessionHandle The handle of the CAN stream session to be closed.
+   */
   public static native void closeCANStreamSession(int sessionHandle);
 
+  /**
+   * Reads messages from a CAN stream session.
+   *
+   * @param sessionHandle The handle of the CAN stream session.
+   * @param messages An array to hold the CANStreamMessage objects (output parameter).
+   * @param messagesToRead The number of messages to read from the session.
+   * @return The number of messages read into the buffer
+   * @throws CANStreamOverflowException If the number of messages to read exceeds the capacity of
+   *     the provided messages array.
+   */
   public static native int readCANStreamSession(
-      int sessionHandle, CANStreamMessage[] messages, int messagesToRead);
+      int sessionHandle, CANStreamMessage[] messages, int messagesToRead)
+      throws CANStreamOverflowException;
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java
index 8ec89f7..1966a17 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java
@@ -22,6 +22,9 @@
   /** The CAN Bus transmit error count. */
   public int transmitErrorCount;
 
+  /** Default constructor. */
+  public CANStatus() {}
+
   /**
    * Set CAN bus status.
    *
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStreamOverflowException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStreamOverflowException.java
new file mode 100644
index 0000000..f5e2f21
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStreamOverflowException.java
@@ -0,0 +1,38 @@
+// 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.
+
+package edu.wpi.first.hal.can;
+
+import edu.wpi.first.hal.CANStreamMessage;
+import java.io.IOException;
+
+public class CANStreamOverflowException extends IOException {
+  /** The messages. */
+  private final CANStreamMessage[] m_messages;
+
+  /** The length of messages read. */
+  private final int m_messagesRead;
+
+  /**
+   * Constructs a new CANStreamOverflowException.
+   *
+   * @param messages The messages
+   * @param messagesRead The length of messages read
+   */
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public CANStreamOverflowException(CANStreamMessage[] messages, int messagesRead) {
+    super("A CAN Stream has overflowed. Data will be missed");
+    this.m_messages = messages;
+    this.m_messagesRead = messagesRead;
+  }
+
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public CANStreamMessage[] getMessages() {
+    return m_messages;
+  }
+
+  public int getMessagesRead() {
+    return m_messagesRead;
+  }
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
index ad17a3e..842398f53 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
@@ -12,4 +12,7 @@
   public static final int kRIOStatusOperationTimedOut = -52007;
   public static final int kRIOStatusFeatureNotSupported = kRioStatusOffset - 193;
   public static final int kRIOStatusResourceNotInitialized = -52010;
+
+  /** Default constructor. */
+  public NIRioStatus() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
index 5ea3726..26975c0 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
@@ -6,7 +6,16 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for accelerometer data. */
 public class AccelerometerDataJNI extends JNIWrapper {
+  /**
+   * Register a callback to be run when this accelerometer activates.
+   *
+   * @param index the index
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
   public static native int registerActiveCallback(
       int index, NotifyCallback callback, boolean initialNotify);
 
@@ -53,4 +62,7 @@
   public static native void setZ(int index, double z);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private AccelerometerDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
index 8ba0e94..b146535 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for addressable LED data. */
 public class AddressableLEDDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -54,4 +55,7 @@
   public static native void resetData(int index);
 
   public static native int findForChannel(int channel);
+
+  /** Utility class. */
+  private AddressableLEDDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
index a6acc9a..b369dcd 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for analog gyro data. */
 public class AnalogGyroDataJNI extends JNIWrapper {
   public static native int registerAngleCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -35,4 +36,7 @@
   public static native void setInitialized(int index, boolean initialized);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private AnalogGyroDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
index 4229c09..b3dde0e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for analog input data. */
 public class AnalogInDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -89,4 +90,7 @@
   public static native void setAccumulatorDeadband(int index, int AccumulatorDeadband);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private AnalogInDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
index 6af16a0..f8e4eec 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for analog output data. */
 public class AnalogOutDataJNI extends JNIWrapper {
   public static native int registerVoltageCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -26,4 +27,7 @@
   public static native void setInitialized(int index, boolean initialized);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private AnalogOutDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
index 67b65b5..681e5bd 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for analog trigger data. */
 public class AnalogTriggerDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -37,4 +38,7 @@
   public static native void resetData(int index);
 
   public static native int findForChannel(int channel);
+
+  /** Utility class. */
+  private AnalogTriggerDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
index e93a921..7c66563 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
@@ -4,6 +4,14 @@
 
 package edu.wpi.first.hal.simulation;
 
+/** Interface for simulation buffer callbacks. */
 public interface BufferCallback {
+  /**
+   * Simulation buffer callback function.
+   *
+   * @param name Buffer name.
+   * @param buffer Buffer.
+   * @param count Buffer size.
+   */
   void callback(String name, byte[] buffer, int count);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/CTREPCMDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/CTREPCMDataJNI.java
index 6d94438..265edd0 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/CTREPCMDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/CTREPCMDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for CTRE PCM data. */
 public class CTREPCMDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -68,4 +69,7 @@
       int index, int channel, NotifyCallback callback, boolean initialNotify);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private CTREPCMDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
index 0c59415..dbbab99 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for DIO data. */
 public class DIODataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -53,4 +54,7 @@
   public static native void setFilterIndex(int index, int filterIndex);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private DIODataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
index addcffc..e72a233 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for digital PWM data. */
 public class DigitalPWMDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -37,4 +38,7 @@
   public static native void resetData(int index);
 
   public static native int findForChannel(int channel);
+
+  /** Utility class. */
+  private DigitalPWMDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
index 196f017..e310b14 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for Driver Station data. */
 public class DriverStationDataJNI extends JNIWrapper {
   public static native int registerEnabledCallback(NotifyCallback callback, boolean initialNotify);
 
@@ -134,4 +135,7 @@
   public static native void setReplayNumber(int replayNumber);
 
   public static native void resetData();
+
+  /** Utility class. */
+  private DriverStationDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
index 8f244ca..e2ed873 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for duty cycle data. */
 public class DutyCycleDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -37,4 +38,7 @@
   public static native void resetData(int index);
 
   public static native int findForChannel(int channel);
+
+  /** Utility class. */
+  private DutyCycleDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
index e48607a..d853d00 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for encoder data. */
 public class EncoderDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -99,4 +100,7 @@
   public static native void resetData(int index);
 
   public static native int findForChannel(int channel);
+
+  /** Utility class. */
+  private EncoderDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
index ebe15eb..f74d63e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for I2C data. */
 public class I2CDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -25,4 +26,7 @@
   public static native void cancelWriteCallback(int index, int uid);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private I2CDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
index 9c7b0a2..9323d1a 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
@@ -6,8 +6,12 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for notifier data. */
 public class NotifierDataJNI extends JNIWrapper {
   public static native long getNextTimeout();
 
   public static native int getNumNotifiers();
+
+  /** Utility class. */
+  private NotifierDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
index d66256b..10f8b66 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for PWM data. */
 public class PWMDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -62,4 +63,7 @@
   public static native void setZeroLatch(int index, boolean zeroLatch);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private PWMDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PowerDistributionDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PowerDistributionDataJNI.java
index c17da13..80a7e46 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PowerDistributionDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PowerDistributionDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for power distribution data. */
 public class PowerDistributionDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -44,4 +45,7 @@
   public static native void setCurrent(int index, int channel, double current);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private PowerDistributionDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java
index 96b9701..9609f3d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for REV PH data. */
 public class REVPHDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -68,4 +69,7 @@
       int index, int channel, NotifyCallback callback, boolean initialNotify);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private REVPHDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
index 3b165e2..7343715 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for relay data. */
 public class RelayDataJNI extends JNIWrapper {
   public static native int registerInitializedForwardCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -44,4 +45,7 @@
   public static native void setReverse(int index, boolean reverse);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private RelayDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
index 7f51331..83d599d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for roboRIO data. */
 public class RoboRioDataJNI extends JNIWrapper {
   public static native int registerFPGAButtonCallback(
       NotifyCallback callback, boolean initialNotify);
@@ -176,5 +177,17 @@
 
   public static native void setComments(String comments);
 
+  public static native int registerRadioLEDStateCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelRadioLEDStateCallback(int uid);
+
+  public static native int getRadioLEDState();
+
+  public static native void setRadioLEDState(int state);
+
   public static native void resetData();
+
+  /** Utility class. */
+  private RoboRioDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
index 9ceaad8..b38fb55 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for SPI accelerometer data. */
 public class SPIAccelerometerDataJNI extends JNIWrapper {
   public static native int registerActiveCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -53,4 +54,7 @@
   public static native void setZ(int index, double z);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private SPIAccelerometerDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
index 11b777e..fa8df89 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for SPI data. */
 public class SPIDataJNI extends JNIWrapper {
   public static native int registerInitializedCallback(
       int index, NotifyCallback callback, boolean initialNotify);
@@ -30,4 +31,7 @@
   public static native void cancelReadAutoReceiveBufferCallback(int index, int uid);
 
   public static native void resetData(int index);
+
+  /** Utility class. */
+  private SPIDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
index 092b955..b4abb33 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.hal.HALValue;
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for sim device data. */
 public class SimDeviceDataJNI extends JNIWrapper {
   public static native void setSimDeviceEnabled(String prefix, boolean enabled);
 
@@ -106,4 +107,7 @@
   public static native double[] getSimValueEnumDoubleValues(int handle);
 
   public static native void resetSimDeviceData();
+
+  /** Utility class. */
+  private SimDeviceDataJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
index 3dfeb06..298929c 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.JNIWrapper;
 
+/** JNI for simulator. */
 public class SimulatorJNI extends JNIWrapper {
   public static native void setRuntimeType(int type);
 
@@ -28,4 +29,7 @@
   public static native void stepTimingAsync(long delta);
 
   public static native void resetHandles();
+
+  /** Utility class. */
+  private SimulatorJNI() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
index 7be73b4..6c188d3 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
@@ -6,6 +6,7 @@
 
 /** Exception for bad status codes from the chip object. */
 public final class UncleanStatusException extends IllegalStateException {
+  /** The status code from the chip object. */
   private final int m_statusCode;
 
   /**
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp b/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp
index 70f09e0..40332ef 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp
@@ -281,4 +281,19 @@
     }
   }
 }
+
+uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
+                            int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  uint32_t messageId = CreateCANId(can.get(), apiId);
+
+  uint32_t session = 0;
+  HAL_CAN_OpenStreamSession(&session, messageId, 0x1FFFFFFF, depth, status);
+  return session;
+}
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.cpp b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.cpp
index 1a25a64..21896ec 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.cpp
@@ -10,6 +10,7 @@
 #include "HALInitializer.h"
 #include "HALInternal.h"
 #include "PortsInternal.h"
+#include "hal/CAN.h"
 #include "hal/CANAPI.h"
 #include "hal/Errors.h"
 #include "hal/handles/IndexedHandleResource.h"
@@ -107,6 +108,8 @@
 struct PDP {
   HAL_CANHandle canHandle;
   std::string previousAllocation;
+  bool streamHandleAllocated{false};
+  uint32_t streamSessionHandles[3];
 };
 }  // namespace
 
@@ -523,4 +526,241 @@
   HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status);
 }
 
+uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
+                            int32_t* status);
+
+void HAL_StartPDPStream(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (pdp->streamHandleAllocated) {
+    *status = RESOURCE_IS_ALLOCATED;
+    return;
+  }
+
+  pdp->streamSessionHandles[0] =
+      HAL_StartCANStream(pdp->canHandle, Status1, 50, status);
+  if (*status != 0) {
+    return;
+  }
+  pdp->streamSessionHandles[1] =
+      HAL_StartCANStream(pdp->canHandle, Status2, 50, status);
+  if (*status != 0) {
+    HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
+    return;
+  }
+  pdp->streamSessionHandles[2] =
+      HAL_StartCANStream(pdp->canHandle, Status3, 50, status);
+  if (*status != 0) {
+    HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
+    HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]);
+    return;
+  }
+  pdp->streamHandleAllocated = true;
+}
+
+HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
+                                                       int32_t* count,
+                                                       int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return nullptr;
+  }
+
+  if (!pdp->streamHandleAllocated) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return nullptr;
+  }
+
+  *count = 0;
+  // 3 streams, 6 channels per stream, 50 depth per stream
+  HAL_PowerDistributionChannelData* retData =
+      new HAL_PowerDistributionChannelData[3 * 6 * 50];
+
+  HAL_CANStreamMessage messages[50];
+  uint32_t messagesRead = 0;
+  HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[0], messages, 50,
+                            &messagesRead, status);
+  if (*status < 0) {
+    goto Exit;
+  }
+
+  for (uint32_t i = 0; i < messagesRead; i++) {
+    PdpStatus1 pdpStatus;
+    std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
+    uint32_t timestamp = messages[i].timeStamp;
+
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
+         pdpStatus.bits.chan1_l2) *
+        0.125;
+    retData[*count].channel = 1;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
+         pdpStatus.bits.chan2_l4) *
+        0.125;
+    retData[*count].channel = 2;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
+         pdpStatus.bits.chan3_l6) *
+        0.125;
+    retData[*count].channel = 3;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
+         pdpStatus.bits.chan4_l8) *
+        0.125;
+    retData[*count].channel = 4;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
+         pdpStatus.bits.chan5_l2) *
+        0.125;
+    retData[*count].channel = 5;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
+         pdpStatus.bits.chan6_l4) *
+        0.125;
+    retData[*count].channel = 6;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+  }
+
+  messagesRead = 0;
+  HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[1], messages, 50,
+                            &messagesRead, status);
+  if (*status < 0) {
+    goto Exit;
+  }
+
+  for (uint32_t i = 0; i < messagesRead; i++) {
+    PdpStatus2 pdpStatus;
+    std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
+    uint32_t timestamp = messages[i].timeStamp;
+
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
+         pdpStatus.bits.chan7_l2) *
+        0.125;
+    retData[*count].channel = 7;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
+         pdpStatus.bits.chan8_l4) *
+        0.125;
+    retData[*count].channel = 8;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
+         pdpStatus.bits.chan9_l6) *
+        0.125;
+    retData[*count].channel = 9;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
+         pdpStatus.bits.chan10_l8) *
+        0.125;
+    retData[*count].channel = 10;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
+         pdpStatus.bits.chan11_l2) *
+        0.125;
+    retData[*count].channel = 11;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
+         pdpStatus.bits.chan12_l4) *
+        0.125;
+    retData[*count].channel = 12;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+  }
+
+  messagesRead = 0;
+  HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[2], messages, 50,
+                            &messagesRead, status);
+  if (*status < 0) {
+    goto Exit;
+  }
+
+  for (uint32_t i = 0; i < messagesRead; i++) {
+    PdpStatus3 pdpStatus;
+    std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
+    uint32_t timestamp = messages[i].timeStamp;
+
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
+         pdpStatus.bits.chan13_l2) *
+        0.125;
+    retData[*count].channel = 13;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
+         pdpStatus.bits.chan14_l4) *
+        0.125;
+    retData[*count].channel = 14;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
+         pdpStatus.bits.chan15_l6) *
+        0.125;
+    retData[*count].channel = 15;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        ((static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
+         pdpStatus.bits.chan16_l8) *
+        0.125;
+    retData[*count].channel = 16;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+  }
+
+Exit:
+  if (*status < 0) {
+    delete[] retData;
+    retData = nullptr;
+  }
+  return retData;
+}
+
+void HAL_StopPDPStream(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (!pdp->streamHandleAllocated) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return;
+  }
+
+  HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
+  HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]);
+  HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[2]);
+
+  pdp->streamHandleAllocated = false;
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.h b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.h
index dd4b298..99606e0 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.h
@@ -6,6 +6,7 @@
 
 #include <stdint.h>
 
+#include "hal/PowerDistribution.h"
 #include "hal/Types.h"
 
 /**
@@ -130,6 +131,15 @@
  * @param handle the module handle
  */
 void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status);
+
+void HAL_StartPDPStream(HAL_PDPHandle handle, int32_t* status);
+
+HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
+                                                       int32_t* count,
+                                                       int32_t* status);
+
+void HAL_StopPDPStream(HAL_PDPHandle handle, int32_t* status);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif
diff --git a/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp b/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp
index 12739c8..c30fbb9 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp
@@ -181,9 +181,10 @@
 namespace {
 struct TcpCache {
   TcpCache() { std::memset(this, 0, sizeof(*this)); }
-  void Update(uint32_t mask);
+  bool Update(uint32_t mask);
   void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); }
 
+  bool hasReadMatchInfo = false;
   HAL_MatchInfo matchInfo;
   HAL_JoystickDescriptor descriptors[HAL_kMaxJoysticks];
 };
@@ -199,15 +200,25 @@
                                            kTcpRecvMask_MatchInfo |
                                            kTcpRecvMask_GameSpecific;
 
-void TcpCache::Update(uint32_t mask) {
+bool TcpCache::Update(uint32_t mask) {
+  bool failedToReadInfo = false;
   if ((mask & combinedMatchInfoMask) != 0) {
-    HAL_GetMatchInfoInternal(&matchInfo);
+    int status = HAL_GetMatchInfoInternal(&matchInfo);
+    if (status != 0) {
+      failedToReadInfo = true;
+      if (!hasReadMatchInfo) {
+        std::memset(&matchInfo, 0, sizeof(matchInfo));
+      }
+    } else {
+      hasReadMatchInfo = true;
+    }
   }
   for (int i = 0; i < HAL_kMaxJoysticks; i++) {
     if ((mask & (1 << i)) != 0) {
       HAL_GetJoystickDescriptorInternal(i, &descriptors[i]);
     }
   }
+  return failedToReadInfo;
 }
 
 namespace hal::init {
@@ -532,13 +543,21 @@
     if (controlWord.dsAttached) {
       newestControlWord = currentRead->controlWord;
     } else {
+      // Zero out the control word. When the DS has never been connected
+      // this returns garbage. And there is no way we can detect that.
+      std::memset(&controlWord, 0, sizeof(controlWord));
       newestControlWord = controlWord;
     }
   }
 
   uint32_t mask = tcpMask.exchange(0);
   if (mask != 0) {
-    tcpCache.Update(mask);
+    bool failedToReadMatchInfo = tcpCache.Update(mask);
+    if (failedToReadMatchInfo) {
+      // If we failed to read match info
+      // we want to try again next iteration
+      tcpMask.fetch_or(combinedMatchInfoMask);
+    }
     std::scoped_lock tcpLock(tcpCacheMutex);
     tcpCache.CloneTo(&tcpCurrent);
   }
diff --git a/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp b/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp
index 2eeaeb8..948445e 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp
@@ -4,6 +4,7 @@
 
 #include "hal/HAL.h"
 
+#include <dlfcn.h>
 #include <signal.h>  // linux for kill
 #include <sys/prctl.h>
 #include <unistd.h>
@@ -81,6 +82,7 @@
   InitializeFRCDriverStation();
   InitializeI2C();
   InitializeInterrupts();
+  InitializeLEDs();
   InitializeMain();
   InitializeNotifier();
   InitializeCTREPDP();
@@ -496,6 +498,35 @@
   return true;
 }
 
+static void SetupNowRio(void) {
+  nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
+      nLoadOut::getTargetClass();
+
+  int32_t status = 0;
+
+  Dl_info info;
+  status = dladdr(reinterpret_cast<void*>(tHMB::create), &info);
+  if (status == 0) {
+    fmt::print(stderr, "Failed to call dladdr on chipobject {}\n", dlerror());
+    return;
+  }
+
+  void* chipObjectLibrary = dlopen(info.dli_fname, RTLD_LAZY);
+  if (chipObjectLibrary == nullptr) {
+    fmt::print(stderr, "Failed to call dlopen on chipobject {}\n", dlerror());
+    return;
+  }
+
+  std::unique_ptr<tHMB> hmb;
+  hmb.reset(tHMB::create(&status));
+  if (hmb == nullptr) {
+    fmt::print(stderr, "Failed to open HMB on chipobject {}\n", status);
+    dlclose(chipObjectLibrary);
+    return;
+  }
+  wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb));
+}
+
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
   static std::atomic_bool initialized{false};
   static wpi::mutex initializeMutex;
@@ -536,14 +567,13 @@
     setNewDataSem(nullptr);
   });
 
-  // Setup WPI_Now to use FPGA timestamp
-  // this also sets nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass
-  wpi::impl::SetupNowRio();
+  SetupNowRio();
 
   int32_t status = 0;
 
   HAL_InitializeHMB(&status);
   if (status != 0) {
+    fmt::print(stderr, "Failed to open HAL HMB, status code {}\n", status);
     return false;
   }
   hmbBuffer = HAL_GetHMBBuffer();
diff --git a/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h b/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h
index 54e1a92..180b881 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h
@@ -39,6 +39,7 @@
 extern void InitializeHAL();
 extern void InitializeI2C();
 extern void InitializeInterrupts();
+extern void InitializeLEDs();
 extern void InitializeMain();
 extern void InitializeNotifier();
 extern void InitializeCTREPDP();
diff --git a/third_party/allwpilib/hal/src/main/native/athena/LEDs.cpp b/third_party/allwpilib/hal/src/main/native/athena/LEDs.cpp
new file mode 100644
index 0000000..8fa603e
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/LEDs.cpp
@@ -0,0 +1,89 @@
+// 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 "hal/LEDs.h"
+
+#include <unistd.h>
+
+#include <fstream>
+
+#include <wpi/fs.h>
+
+#include "hal/Errors.h"
+
+namespace hal::init {
+
+void InitializeLEDs() {
+  int32_t status = 0;
+  HAL_SetRadioLEDState(HAL_RadioLED_kOff, &status);
+}
+}  // namespace hal::init
+
+static const fs::path radioLEDGreenFilePath =
+    "/sys/class/leds/nilrt:wifi:primary/brightness";
+static const fs::path radioLEDRedFilePath =
+    "/sys/class/leds/nilrt:wifi:secondary/brightness";
+
+static const char* onStr = "1";
+static const char* offStr = "0";
+
+extern "C" {
+void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
+  std::error_code ec;
+  fs::file_t greenFile = fs::OpenFileForWrite(radioLEDGreenFilePath, ec,
+                                              fs::CD_OpenExisting, fs::OF_Text);
+  if (ec) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+  fs::file_t redFile = fs::OpenFileForWrite(radioLEDRedFilePath, ec,
+                                            fs::CD_OpenExisting, fs::OF_Text);
+  if (ec) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+
+  write(greenFile, state & HAL_RadioLED_kGreen ? onStr : offStr, 1);
+  write(redFile, state & HAL_RadioLED_kRed ? onStr : offStr, 1);
+
+  fs::CloseFile(greenFile);
+  fs::CloseFile(redFile);
+}
+
+bool ReadStateFromFile(fs::path path, int32_t* status) {
+  std::error_code ec;
+  fs::file_t file = fs::OpenFileForRead(path, ec, fs::OF_Text);
+  if (ec) {
+    *status = INCOMPATIBLE_STATE;
+    return false;
+  }
+  // We only need to read one byte because the file won't have leading zeros.
+  char buf[1]{};
+  size_t count = read(file, buf, 1);
+  if (count == 0) {
+    *status = INCOMPATIBLE_STATE;
+    return false;
+  }
+  // If the brightness is not zero, the LED is on.
+  return buf[0] != '0';
+}
+
+HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status) {
+  bool green = ReadStateFromFile(radioLEDGreenFilePath, status);
+  bool red = ReadStateFromFile(radioLEDRedFilePath, status);
+  if (*status == 0) {
+    if (green && red) {
+      return HAL_RadioLED_kOrange;
+    } else if (green) {
+      return HAL_RadioLED_kGreen;
+    } else if (red) {
+      return HAL_RadioLED_kRed;
+    } else {
+      return HAL_RadioLED_kOff;
+    }
+  } else {
+    return HAL_RadioLED_kOff;
+  }
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PowerDistribution.cpp b/third_party/allwpilib/hal/src/main/native/athena/PowerDistribution.cpp
index 0d2b963..580aa90 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/PowerDistribution.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/PowerDistribution.cpp
@@ -273,4 +273,36 @@
   }
 }
 
+void HAL_StartPowerDistributionStream(HAL_PowerDistributionHandle handle,
+                                      int32_t* status) {
+  if (IsCtre(handle)) {
+    HAL_StartPDPStream(handle, status);
+  } else {
+    HAL_StartREVPDHStream(handle, status);
+  }
+}
+
+HAL_PowerDistributionChannelData* HAL_GetPowerDistributionStreamData(
+    HAL_PowerDistributionHandle handle, int32_t* count, int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPStreamData(handle, count, status);
+  } else {
+    return HAL_GetREVPDHStreamData(handle, count, status);
+  }
+}
+
+void HAL_FreePowerDistributionStreamData(HAL_PowerDistributionChannelData* data,
+                                         int32_t count) {
+  delete[] data;
+}
+
+void HAL_StopPowerDistributionStream(HAL_PowerDistributionHandle handle,
+                                     int32_t* status) {
+  if (IsCtre(handle)) {
+    HAL_StopPDPStream(handle, status);
+  } else {
+    HAL_StopREVPDHStream(handle, status);
+  }
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/REVPDH.cpp b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.cpp
index 10c019d..3398404 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/REVPDH.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.cpp
@@ -4,6 +4,7 @@
 
 #include "REVPDH.h"
 
+#include <hal/CAN.h>
 #include <hal/CANAPI.h>
 #include <hal/CANAPITypes.h>
 #include <hal/Errors.h>
@@ -37,6 +38,8 @@
   HAL_CANHandle hcan;
   std::string previousAllocation;
   HAL_PowerDistributionVersion versionInfo;
+  bool streamHandleAllocated{false};
+  uint32_t streamSessionHandles[4];
 };
 
 }  // namespace
@@ -636,4 +639,271 @@
                      PDH_CLEAR_FAULTS_FRAME_API, status);
 }
 
+uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
+                            int32_t* status);
+
+void HAL_StartREVPDHStream(HAL_REVPDHHandle handle, int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (hpdh->streamHandleAllocated) {
+    *status = RESOURCE_IS_ALLOCATED;
+    return;
+  }
+
+  hpdh->streamSessionHandles[0] =
+      HAL_StartCANStream(hpdh->hcan, PDH_STATUS_0_FRAME_API, 50, status);
+  if (*status != 0) {
+    return;
+  }
+  hpdh->streamSessionHandles[1] =
+      HAL_StartCANStream(hpdh->hcan, PDH_STATUS_1_FRAME_API, 50, status);
+  if (*status != 0) {
+    HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
+    return;
+  }
+  hpdh->streamSessionHandles[2] =
+      HAL_StartCANStream(hpdh->hcan, PDH_STATUS_2_FRAME_API, 50, status);
+  if (*status != 0) {
+    HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
+    HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]);
+    return;
+  }
+  hpdh->streamSessionHandles[3] =
+      HAL_StartCANStream(hpdh->hcan, PDH_STATUS_3_FRAME_API, 50, status);
+  if (*status != 0) {
+    HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
+    HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]);
+    HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[3]);
+    return;
+  }
+  hpdh->streamHandleAllocated = true;
+}
+
+HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
+    HAL_REVPDHHandle handle, int32_t* count, int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return nullptr;
+  }
+
+  if (!hpdh->streamHandleAllocated) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return nullptr;
+  }
+
+  *count = 0;
+  // 4 streams, 6 channels per stream, 50 depth per stream
+  HAL_PowerDistributionChannelData* retData =
+      new HAL_PowerDistributionChannelData[4 * 6 * 50];
+
+  HAL_CANStreamMessage messages[50];
+  uint32_t messagesRead = 0;
+  HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[0], messages, 50,
+                            &messagesRead, status);
+  if (*status < 0) {
+    goto Exit;
+  }
+
+  for (uint32_t i = 0; i < messagesRead; i++) {
+    PDH_status_0_t statusFrame0;
+    PDH_status_0_unpack(&statusFrame0, messages[i].data, PDH_STATUS_0_LENGTH);
+    uint32_t timestamp = messages[i].timeStamp;
+
+    retData[*count].current =
+        PDH_status_0_channel_0_current_decode(statusFrame0.channel_0_current);
+    retData[*count].channel = 1;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_0_channel_1_current_decode(statusFrame0.channel_1_current);
+    retData[*count].channel = 2;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_0_channel_2_current_decode(statusFrame0.channel_2_current);
+    retData[*count].channel = 3;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_0_channel_3_current_decode(statusFrame0.channel_3_current);
+    retData[*count].channel = 4;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_0_channel_4_current_decode(statusFrame0.channel_4_current);
+    retData[*count].channel = 5;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_0_channel_5_current_decode(statusFrame0.channel_5_current);
+    retData[*count].channel = 6;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+  }
+
+  messagesRead = 0;
+  HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[1], messages, 50,
+                            &messagesRead, status);
+  if (*status < 0) {
+    goto Exit;
+  }
+
+  for (uint32_t i = 0; i < messagesRead; i++) {
+    PDH_status_1_t statusFrame1;
+    PDH_status_1_unpack(&statusFrame1, messages[i].data, PDH_STATUS_1_LENGTH);
+    uint32_t timestamp = messages[i].timeStamp;
+
+    retData[*count].current =
+        PDH_status_1_channel_6_current_decode(statusFrame1.channel_6_current);
+    retData[*count].channel = 7;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_1_channel_7_current_decode(statusFrame1.channel_7_current);
+    retData[*count].channel = 8;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_1_channel_8_current_decode(statusFrame1.channel_8_current);
+    retData[*count].channel = 9;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_1_channel_9_current_decode(statusFrame1.channel_9_current);
+    retData[*count].channel = 10;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_1_channel_10_current_decode(statusFrame1.channel_10_current);
+    retData[*count].channel = 11;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_1_channel_11_current_decode(statusFrame1.channel_11_current);
+    retData[*count].channel = 12;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+  }
+
+  messagesRead = 0;
+  HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[2], messages, 50,
+                            &messagesRead, status);
+  if (*status < 0) {
+    goto Exit;
+  }
+
+  for (uint32_t i = 0; i < messagesRead; i++) {
+    PDH_status_2_t statusFrame2;
+    PDH_status_2_unpack(&statusFrame2, messages[i].data, PDH_STATUS_2_LENGTH);
+    uint32_t timestamp = messages[i].timeStamp;
+
+    retData[*count].current =
+        PDH_status_2_channel_12_current_decode(statusFrame2.channel_12_current);
+    retData[*count].channel = 13;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_2_channel_13_current_decode(statusFrame2.channel_13_current);
+    retData[*count].channel = 14;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_2_channel_14_current_decode(statusFrame2.channel_14_current);
+    retData[*count].channel = 15;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_2_channel_15_current_decode(statusFrame2.channel_15_current);
+    retData[*count].channel = 16;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_2_channel_16_current_decode(statusFrame2.channel_16_current);
+    retData[*count].channel = 17;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_2_channel_17_current_decode(statusFrame2.channel_17_current);
+    retData[*count].channel = 18;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+  }
+
+  messagesRead = 0;
+  HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[3], messages, 50,
+                            &messagesRead, status);
+  if (*status < 0) {
+    goto Exit;
+  }
+
+  for (uint32_t i = 0; i < messagesRead; i++) {
+    PDH_status_3_t statusFrame3;
+    PDH_status_3_unpack(&statusFrame3, messages[i].data, PDH_STATUS_3_LENGTH);
+    uint32_t timestamp = messages[i].timeStamp;
+
+    retData[*count].current =
+        PDH_status_3_channel_18_current_decode(statusFrame3.channel_18_current);
+    retData[*count].channel = 19;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_3_channel_19_current_decode(statusFrame3.channel_19_current);
+    retData[*count].channel = 20;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_3_channel_20_current_decode(statusFrame3.channel_20_current);
+    retData[*count].channel = 21;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_3_channel_21_current_decode(statusFrame3.channel_21_current);
+    retData[*count].channel = 22;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_3_channel_22_current_decode(statusFrame3.channel_22_current);
+    retData[*count].channel = 23;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+    retData[*count].current =
+        PDH_status_3_channel_23_current_decode(statusFrame3.channel_23_current);
+    retData[*count].channel = 24;
+    retData[*count].timestamp = timestamp;
+    (*count)++;
+  }
+
+Exit:
+  if (*status < 0) {
+    delete[] retData;
+    retData = nullptr;
+  }
+  return retData;
+}
+
+void HAL_StopREVPDHStream(HAL_REVPDHHandle handle, int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (!hpdh->streamHandleAllocated) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return;
+  }
+
+  HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
+  HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]);
+  HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[2]);
+  HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[3]);
+
+  hpdh->streamHandleAllocated = false;
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/REVPDH.h b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.h
index d0b10f2..0046455 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/REVPDH.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.h
@@ -158,6 +158,13 @@
  */
 void HAL_ClearREVPDHStickyFaults(HAL_REVPDHHandle handle, int32_t* status);
 
+void HAL_StartREVPDHStream(HAL_REVPDHHandle handle, int32_t* status);
+
+HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
+    HAL_REVPDHHandle handle, int32_t* count, int32_t* status);
+
+void HAL_StopREVPDHStream(HAL_REVPDHHandle handle, int32_t* status);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp
index 8559394..b57b706 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp
@@ -30,6 +30,7 @@
 DEFINE_CAPI(double, BrownoutVoltage, 6.75)
 DEFINE_CAPI(double, CPUTemp, 45.0)
 DEFINE_CAPI(int32_t, TeamNumber, 0)
+DEFINE_CAPI(HAL_RadioLEDState, RadioLEDState, HAL_RadioLED_kOff);
 
 int32_t HALSIM_RegisterRoboRioSerialNumberCallback(
     HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) {
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp
index 9375a06..40838d3 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp
@@ -135,6 +135,17 @@
   (JNIEnv* env, jclass, jint sessionHandle, jobjectArray messages,
    jint messagesToRead)
 {
+  if (messages == nullptr) {
+    ThrowNullPointerException(env, "messages cannot be null");
+    return 0;
+  }
+
+  jsize messagesArrayLen = env->GetArrayLength(messages);
+
+  if (messagesArrayLen < messagesToRead) {
+    messagesToRead = messagesArrayLen;
+  }
+
   uint32_t handle = static_cast<uint32_t>(sessionHandle);
   uint32_t messagesRead = 0;
 
@@ -151,7 +162,8 @@
     return 0;
   }
 
-  if (!CheckStatus(env, status)) {
+  if (status != HAL_ERR_CANSessionMux_SessionOverrun &&
+      !CheckStatus(env, status)) {
     return 0;
   }
 
@@ -160,8 +172,15 @@
     JLocal<jobject> elem{
         env, static_cast<jstring>(env->GetObjectArrayElement(messages, i))};
     if (!elem) {
-      // TODO decide if should throw
-      continue;
+      // If element doesn't exist, construct it in place. If that fails, we are
+      // OOM, just return
+      elem = JLocal<jobject>{env, CreateCANStreamMessage(env)};
+      if (elem) {
+        std::printf("Allocated and set object\n");
+        env->SetObjectArrayElement(messages, i, elem);
+      } else {
+        return 0;
+      }
     }
     JLocal<jbyteArray> toSetArray{
         env, SetCANStreamObject(env, elem, msg->dataSize, msg->messageID,
@@ -174,6 +193,12 @@
                             reinterpret_cast<jbyte*>(msg->data));
   }
 
+  if (status == HAL_ERR_CANSessionMux_SessionOverrun) {
+    ThrowCANStreamOverflowException(env, messages,
+                                    static_cast<jint>(messagesRead));
+    return 0;
+  }
+
   return static_cast<jint>(messagesRead);
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp
index 6316a8c..1915108 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp
@@ -46,6 +46,7 @@
 static JException canMessageNotAllowedExCls;
 static JException canNotInitializedExCls;
 static JException uncleanStatusExCls;
+static JException nullPointerEx;
 static JClass powerDistributionVersionCls;
 static JClass pwmConfigDataResultCls;
 static JClass canStatusCls;
@@ -56,6 +57,7 @@
 static JClass halValueCls;
 static JClass baseStoreCls;
 static JClass revPHVersionCls;
+static JClass canStreamOverflowExCls;
 
 static const JClassInit classes[] = {
     {"edu/wpi/first/hal/PowerDistributionVersion",
@@ -68,7 +70,9 @@
     {"edu/wpi/first/hal/CANStreamMessage", &canStreamMessageCls},
     {"edu/wpi/first/hal/HALValue", &halValueCls},
     {"edu/wpi/first/hal/DMAJNISample$BaseStore", &baseStoreCls},
-    {"edu/wpi/first/hal/REVPHVersion", &revPHVersionCls}};
+    {"edu/wpi/first/hal/REVPHVersion", &revPHVersionCls},
+    {"edu/wpi/first/hal/can/CANStreamOverflowException",
+     &canStreamOverflowExCls}};
 
 static const JExceptionInit exceptions[] = {
     {"java/lang/IllegalArgumentException", &illegalArgExCls},
@@ -82,7 +86,8 @@
      &canMessageNotAllowedExCls},
     {"edu/wpi/first/hal/can/CANNotInitializedException",
      &canNotInitializedExCls},
-    {"edu/wpi/first/hal/util/UncleanStatusException", &uncleanStatusExCls}};
+    {"edu/wpi/first/hal/util/UncleanStatusException", &uncleanStatusExCls},
+    {"java/lang/NullPointerException", &nullPointerEx}};
 
 namespace hal {
 
@@ -209,6 +214,20 @@
   }
 }
 
+void ThrowNullPointerException(JNIEnv* env, std::string_view msg) {
+  nullPointerEx.Throw(env, msg);
+}
+
+void ThrowCANStreamOverflowException(JNIEnv* env, jobjectArray messages,
+                                     jint length) {
+  static jmethodID constructor =
+      env->GetMethodID(canStreamOverflowExCls, "<init>",
+                       "([Ledu/wpi/first/hal/CANStreamMessage;I)V");
+  jobject exception =
+      env->NewObject(canStreamOverflowExCls, constructor, messages, length);
+  env->Throw(static_cast<jthrowable>(exception));
+}
+
 void ThrowIllegalArgumentException(JNIEnv* env, std::string_view msg) {
   illegalArgExCls.Throw(env, msg);
 }
@@ -365,6 +384,12 @@
       static_cast<jint>(hardwareMajor), static_cast<jint>(uniqueId));
 }
 
+jobject CreateCANStreamMessage(JNIEnv* env) {
+  static jmethodID constructor =
+      env->GetMethodID(canStreamMessageCls, "<init>", "()V");
+  return env->NewObject(canStreamMessageCls, constructor);
+}
+
 JavaVM* GetJVM() {
   return jvm;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h
index 9c9487c..b64238b 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h
@@ -51,6 +51,9 @@
   return status == 0;
 }
 
+void ThrowNullPointerException(JNIEnv* env, std::string_view msg);
+void ThrowCANStreamOverflowException(JNIEnv* env, jobjectArray messages,
+                                     jint length);
 void ThrowIllegalArgumentException(JNIEnv* env, std::string_view msg);
 void ThrowBoundaryException(JNIEnv* env, double value, double lower,
                             double upper);
@@ -93,6 +96,8 @@
                                        uint32_t hardwareMajor,
                                        uint32_t uniqueId);
 
+jobject CreateCANStreamMessage(JNIEnv* env);
+
 JavaVM* GetJVM();
 
 }  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/LEDJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/LEDJNI.cpp
new file mode 100644
index 0000000..62aeaff
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/LEDJNI.cpp
@@ -0,0 +1,51 @@
+// 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/core.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_LEDJNI.h"
+#include "hal/LEDs.h"
+
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_OFF ==
+              HAL_RadioLEDState::HAL_RadioLED_kOff);
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_GREEN ==
+              HAL_RadioLEDState::HAL_RadioLED_kGreen);
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_RED ==
+              HAL_RadioLEDState::HAL_RadioLED_kRed);
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_ORANGE ==
+              HAL_RadioLEDState::HAL_RadioLED_kOrange);
+
+using namespace hal;
+
+extern "C" {
+/*
+ * Class:     edu_wpi_first_hal_LEDJNI
+ * Method:    setRadioLEDState
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_LEDJNI_setRadioLEDState
+  (JNIEnv* env, jclass, jint state)
+{
+  int32_t status = 0;
+  HAL_SetRadioLEDState(static_cast<HAL_RadioLEDState>(state), &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_LEDJNI
+ * Method:    getRadioLEDState
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_LEDJNI_getRadioLEDState
+  (JNIEnv* env, jclass)
+{
+  int32_t status = 0;
+  auto retVal = HAL_GetRadioLEDState(&status);
+  CheckStatus(env, status);
+  return retVal;
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
index 9555f59..c9a2aca 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
@@ -985,6 +985,57 @@
 
 /*
  * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerRadioLEDStateCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerRadioLEDStateCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioRadioLEDStateCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelRadioLEDStateCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelRadioLEDStateCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioRadioLEDStateCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getRadioLEDState
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getRadioLEDState
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioRadioLEDState();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setRadioLEDState
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setRadioLEDState
+  (JNIEnv*, jclass, jint value)
+{
+  HALSIM_SetRoboRioRadioLEDState(static_cast<HAL_RadioLEDState>(value));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
  * Method:    resetData
  * Signature: ()V
  */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/.clang-tidy b/third_party/allwpilib/hal/src/main/native/include/hal/.clang-tidy
index 4f4edda..2631285 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/.clang-tidy
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/.clang-tidy
@@ -36,7 +36,6 @@
   bugprone-unhandled-self-assignment,
   bugprone-unused-raii,
   bugprone-virtual-near-miss,
-  cert-dcl58-cpp,
   cert-err52-cpp,
   cert-err60-cpp,
   cert-mem57-cpp,
@@ -54,7 +53,6 @@
   google-readability-avoid-underscore-in-googletest-name,
   google-readability-casting,
   google-runtime-operator,
-  llvm-twine-local,
   misc-definitions-in-headers,
   misc-misplaced-const,
   misc-new-delete-overloads,
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h b/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h
index 9131e83..ab48ca5 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h
@@ -12,7 +12,6 @@
  * @{
  */
 
-// clang-format off
 /**
  * The acceptable accelerometer ranges.
  */
@@ -21,7 +20,6 @@
   HAL_AccelerometerRange_k4G = 1,
   HAL_AccelerometerRange_k8G = 2,
 };
-// clang-format on
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h
index f4622e3..e678b68 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h
@@ -14,7 +14,6 @@
  * @{
  */
 
-// clang-format off
 /**
  * The type of analog trigger to trigger on.
  */
@@ -24,7 +23,6 @@
   HAL_Trigger_kRisingPulse = 2,
   HAL_Trigger_kFallingPulse = 3
 };
-// clang-format on
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h
index decaf86..247732c 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h
@@ -14,25 +14,37 @@
  * @{
  */
 
-// clang-format off
 /**
  * The CAN device type.
  *
  * Teams should use HAL_CAN_Dev_kMiscellaneous
  */
 HAL_ENUM(HAL_CANDeviceType) {
+  /// Broadcast.
   HAL_CAN_Dev_kBroadcast = 0,
+  /// Robot controller.
   HAL_CAN_Dev_kRobotController = 1,
+  /// Motor controller.
   HAL_CAN_Dev_kMotorController = 2,
+  /// Relay controller.
   HAL_CAN_Dev_kRelayController = 3,
+  /// Gyro sensor.
   HAL_CAN_Dev_kGyroSensor = 4,
+  /// Accelerometer.
   HAL_CAN_Dev_kAccelerometer = 5,
+  /// Ultrasonic sensor.
   HAL_CAN_Dev_kUltrasonicSensor = 6,
+  /// Gear tooth sensor.
   HAL_CAN_Dev_kGearToothSensor = 7,
+  /// Power distribution.
   HAL_CAN_Dev_kPowerDistribution = 8,
+  /// Pneumatics.
   HAL_CAN_Dev_kPneumatics = 9,
+  /// Miscellaneous.
   HAL_CAN_Dev_kMiscellaneous = 10,
+  /// IO breakout.
   HAL_CAN_Dev_kIOBreakout = 11,
+  /// Firmware update.
   HAL_CAN_Dev_kFirmwareUpdate = 31
 };
 
@@ -42,23 +54,39 @@
  * Teams should use HAL_CAN_Man_kTeamUse.
  */
 HAL_ENUM(HAL_CANManufacturer) {
+  /// Broadcast.
   HAL_CAN_Man_kBroadcast = 0,
+  /// National Instruments.
   HAL_CAN_Man_kNI = 1,
+  /// Luminary Micro.
   HAL_CAN_Man_kLM = 2,
+  /// DEKA Research and Development Corp.
   HAL_CAN_Man_kDEKA = 3,
+  /// Cross the Road Electronics.
   HAL_CAN_Man_kCTRE = 4,
+  /// REV robotics.
   HAL_CAN_Man_kREV = 5,
+  /// Grapple.
   HAL_CAN_Man_kGrapple = 6,
+  /// MindSensors.
   HAL_CAN_Man_kMS = 7,
+  /// Team use.
   HAL_CAN_Man_kTeamUse = 8,
+  /// Kauai Labs.
   HAL_CAN_Man_kKauaiLabs = 9,
+  /// Copperforge.
   HAL_CAN_Man_kCopperforge = 10,
+  /// Playing With Fusion.
   HAL_CAN_Man_kPWF = 11,
+  /// Studica.
   HAL_CAN_Man_kStudica = 12,
+  /// TheThriftyBot.
   HAL_CAN_Man_kTheThriftyBot = 13,
+  /// Redux Robotics.
   HAL_CAN_Man_kReduxRobotics = 14,
+  /// AndyMark.
   HAL_CAN_Man_kAndyMark = 15,
+  /// Vivid-Hosting.
   HAL_CAN_Man_kVividHosting = 16
 };
-// clang-format on
 /** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h b/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h
index 0338cda..9c9d3af 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h
@@ -15,7 +15,6 @@
  * @{
  */
 
-// clang-format off
 /**
  * The counter mode.
  */
@@ -25,7 +24,6 @@
   HAL_Counter_kPulseLength = 2,
   HAL_Counter_kExternalDirection = 3
 };
-// clang-format on
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h b/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h
index b735b6d..2d570a4 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h
@@ -13,7 +13,6 @@
  * @{
  */
 
-// clang-format off
 /**
  * The DMA Read Status.
  */
@@ -22,7 +21,6 @@
   HAL_DMA_TIMEOUT = 2,
   HAL_DMA_ERROR = 3,
 };
-// clang-format on
 
 /**
  * Buffer for containing all DMA data for a specific sample.
@@ -140,6 +138,8 @@
 /**
  * Adds timer data for an counter to be collected by DMA.
  *
+ * This can only be called if DMA is not started.
+ *
  * @param[in] handle the dma handle
  * @param[in] counterHandle the counter to add
  * @param[out] status Error status variable. 0 on success.
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h
index 5edacf5..e079074 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h
@@ -6,6 +6,8 @@
 
 #include <stdint.h>
 
+#include <stddef.h>  //NOLINT
+
 #include <wpi/Synchronization.h>
 
 #include "hal/DriverStationTypes.h"
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h
index 5917c51..8282024 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h
@@ -40,7 +40,6 @@
 };
 typedef struct HAL_ControlWord HAL_ControlWord;
 
-// clang-format off
 HAL_ENUM(HAL_AllianceStationID) {
   HAL_AllianceStationID_kUnknown = 0,
   HAL_AllianceStationID_kRed1,
@@ -57,7 +56,6 @@
   HAL_kMatchType_qualification,
   HAL_kMatchType_elimination,
 };
-// clang-format on
 
 /* The maximum number of axes that will be stored in a single HALJoystickAxes
  * struct. This is used for allocating buffers, not bounds checking, since
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h b/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h
index c6e16a6..451f1aa 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h
@@ -15,7 +15,6 @@
  * @{
  */
 
-// clang-format off
 /**
  * The type of index pulse for the encoder.
  */
@@ -34,7 +33,6 @@
   HAL_Encoder_k2X,
   HAL_Encoder_k4X
 };
-// clang-format on
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h b/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h
index 0e1294b..d7c3c02 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h
@@ -24,6 +24,7 @@
 #include "hal/HALBase.h"
 #include "hal/I2C.h"
 #include "hal/Interrupts.h"
+#include "hal/LEDs.h"
 #include "hal/Main.h"
 #include "hal/Notifier.h"
 #include "hal/PWM.h"
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h b/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h
index fe8d1bf..3e9ddc6 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h
@@ -22,9 +22,11 @@
  * @{
  */
 
-// clang-format off
-HAL_ENUM(HAL_RuntimeType) { HAL_Runtime_RoboRIO, HAL_Runtime_RoboRIO2, HAL_Runtime_Simulation };
-// clang-format on
+HAL_ENUM(HAL_RuntimeType) {
+  HAL_Runtime_RoboRIO,
+  HAL_Runtime_RoboRIO2,
+  HAL_Runtime_Simulation
+};
 
 #ifdef __cplusplus
 extern "C" {
@@ -108,6 +110,10 @@
 /**
  * Gets the state of the "USER" button on the roboRIO.
  *
+ * @warning the User Button is used to stop user programs from automatically
+ * loading if it is held for more then 5 seconds. Because of this, it's not
+ * recommended to be used by teams for any other purpose.
+ *
  * @param[out] status the error code, or 0 for success
  * @return true if the button is currently pressed down
  */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h
index 6aac0fb..8c658ee 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h
@@ -14,9 +14,11 @@
  * @{
  */
 
-// clang-format off
-HAL_ENUM(HAL_I2CPort) { HAL_I2C_kInvalid = -1, HAL_I2C_kOnboard, HAL_I2C_kMXP };
-// clang-format on
+HAL_ENUM(HAL_I2CPort) {
+  HAL_I2C_kInvalid = -1,
+  HAL_I2C_kOnboard,
+  HAL_I2C_kMXP
+};
 
 #ifdef __cplusplus
 namespace hal {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/LEDs.h b/third_party/allwpilib/hal/src/main/native/include/hal/LEDs.h
new file mode 100644
index 0000000..d7ee716
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/LEDs.h
@@ -0,0 +1,34 @@
+// 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 "hal/Types.h"
+
+HAL_ENUM(HAL_RadioLEDState) {
+  HAL_RadioLED_kOff = 0,
+  HAL_RadioLED_kGreen = 1,
+  HAL_RadioLED_kRed = 2,
+  HAL_RadioLED_kOrange = 3
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/**
+ * Set the state of the "Radio" LED.
+ * @param state The state to set the LED to.
+ * @param[out] status the error code, or 0 for success
+ */
+void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status);
+
+/**
+ * Get the state of the "Radio" LED.
+ *
+ * @param[out] status the error code, or 0 for success
+ * @return The state of the LED.
+ */
+HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status);
+#ifdef __cplusplus
+}  // extern "C"
+#endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/PowerDistribution.h b/third_party/allwpilib/hal/src/main/native/include/hal/PowerDistribution.h
index 47543bb..c2dc3cd 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/PowerDistribution.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/PowerDistribution.h
@@ -15,7 +15,6 @@
  * @{
  */
 
-// clang-format off
 /**
  * The types of power distribution devices.
  */
@@ -24,7 +23,6 @@
   HAL_PowerDistributionType_kCTRE = 1,
   HAL_PowerDistributionType_kRev = 2,
 };
-// clang-format on
 
 #define HAL_DEFAULT_POWER_DISTRIBUTION_MODULE -1
 
@@ -219,12 +217,21 @@
 HAL_Bool HAL_GetPowerDistributionSwitchableChannel(
     HAL_PowerDistributionHandle handle, int32_t* status);
 
+/**
+ * Power distribution version.
+ */
 struct HAL_PowerDistributionVersion {
+  /// Firmware major version number.
   uint32_t firmwareMajor;
+  /// Firmware minor version number.
   uint32_t firmwareMinor;
+  /// Firmware fix version number.
   uint32_t firmwareFix;
+  /// Hardware minor version number.
   uint32_t hardwareMinor;
+  /// Hardware major version number.
   uint32_t hardwareMajor;
+  /// Unique ID.
   uint32_t uniqueId;
 };
 
@@ -324,6 +331,24 @@
     HAL_PowerDistributionHandle handle,
     HAL_PowerDistributionStickyFaults* stickyFaults, int32_t* status);
 
+void HAL_StartPowerDistributionStream(HAL_PowerDistributionHandle handle,
+                                      int32_t* status);
+
+typedef struct HAL_PowerDistributionChannelData {
+  float current;
+  int32_t channel;
+  uint32_t timestamp;
+} HAL_PowerDistributionChannelData;
+
+HAL_PowerDistributionChannelData* HAL_GetPowerDistributionStreamData(
+    HAL_PowerDistributionHandle handle, int32_t* count, int32_t* status);
+
+void HAL_FreePowerDistributionStreamData(HAL_PowerDistributionChannelData* data,
+                                         int32_t count);
+
+void HAL_StopPowerDistributionStream(HAL_PowerDistributionHandle handle,
+                                     int32_t* status);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/REVPH.h b/third_party/allwpilib/hal/src/main/native/include/hal/REVPH.h
index 4476675..754c0f2 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/REVPH.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/REVPH.h
@@ -17,11 +17,11 @@
 /**
  * The compressor configuration type
  */
-HAL_ENUM(HAL_REVPHCompressorConfigType){
-    HAL_REVPHCompressorConfigType_kDisabled = 0,
-    HAL_REVPHCompressorConfigType_kDigital = 1,
-    HAL_REVPHCompressorConfigType_kAnalog = 2,
-    HAL_REVPHCompressorConfigType_kHybrid = 3,
+HAL_ENUM(HAL_REVPHCompressorConfigType) {
+  HAL_REVPHCompressorConfigType_kDisabled = 0,
+  HAL_REVPHCompressorConfigType_kDigital = 1,
+  HAL_REVPHCompressorConfigType_kAnalog = 2,
+  HAL_REVPHCompressorConfigType_kHybrid = 3,
 };
 
 /**
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h
index 34b5d61..d521665 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h
@@ -14,7 +14,6 @@
  * @{
  */
 
-// clang-format off
 HAL_ENUM(HAL_SPIPort) {
   HAL_SPI_kInvalid = -1,
   HAL_SPI_kOnboardCS0,
@@ -23,16 +22,13 @@
   HAL_SPI_kOnboardCS3,
   HAL_SPI_kMXP
 };
-// clang-format on
 
-// clang-format off
 HAL_ENUM(HAL_SPIMode) {
   HAL_SPI_kMode0 = 0,
   HAL_SPI_kMode1 = 1,
   HAL_SPI_kMode2 = 2,
   HAL_SPI_kMode3 = 3,
 };
-// clang-format on
 
 #ifdef __cplusplus
 namespace hal {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h b/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h
index f4a357e..c6b80c7 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h
@@ -14,14 +14,12 @@
  * @{
  */
 
-// clang-format off
 HAL_ENUM(HAL_SerialPort) {
   HAL_SerialPort_Onboard = 0,
   HAL_SerialPort_MXP = 1,
   HAL_SerialPort_USB1 = 2,
   HAL_SerialPort_USB2 = 3
 };
-// clang-format on
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h b/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h
index 0e75cea..e5e3324 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h
@@ -30,13 +30,11 @@
 /**
  * Direction of a simulated value (from the perspective of user code).
  */
-// clang-format off
 HAL_ENUM(HAL_SimValueDirection) {
-  HAL_SimValueInput = 0,  /**< input to user code from the simulator */
-  HAL_SimValueOutput,     /**< output from user code to the simulator */
-  HAL_SimValueBidir       /**< bidirectional between user code and simulator */
+  HAL_SimValueInput = 0, /**< input to user code from the simulator */
+  HAL_SimValueOutput,    /**< output from user code to the simulator */
+  HAL_SimValueBidir      /**< bidirectional between user code and simulator */
 };
-// clang-format on
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h
index d191fcb..83d68ea 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h
@@ -6,6 +6,7 @@
 
 #include <cstddef>
 
+#include "hal/LEDs.h"
 #include "hal/Types.h"
 #include "hal/simulation/NotifyListener.h"
 
@@ -145,9 +146,6 @@
 size_t HALSIM_GetRoboRioComments(char* buffer, size_t size);
 void HALSIM_SetRoboRioComments(const char* comments, size_t size);
 
-void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-
 int32_t HALSIM_RegisterRoboRioCPUTempCallback(HAL_NotifyCallback callback,
                                               void* param,
                                               HAL_Bool initialNotify);
@@ -155,6 +153,15 @@
 double HALSIM_GetRoboRioCPUTemp(void);
 void HALSIM_SetRoboRioCPUTemp(double cpuTemp);
 
+int32_t HALSIM_RegisterRoboRioRadioLEDStateCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioRadioLEDStateCallback(int32_t uid);
+HAL_RadioLEDState HALSIM_GetRoboRioRadioLEDState(void);
+void HALSIM_SetRoboRioRadioLEDState(HAL_RadioLEDState state);
+
+void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
 #ifdef __cplusplus
 }  // extern "C"
 #endif
diff --git a/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp b/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp
index ce8b6dd..c7b632c 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp
@@ -367,12 +367,12 @@
 #ifdef _WIN32
   TIMECAPS tc;
   if (timeGetDevCaps(&tc, sizeof(tc)) == TIMERR_NOERROR) {
-    UINT target = min(1, tc.wPeriodMin);
+    UINT target = (std::min)(static_cast<UINT>(1), tc.wPeriodMin);
     timeBeginPeriod(target);
     std::atexit([]() {
       TIMECAPS tc;
       if (timeGetDevCaps(&tc, sizeof(tc)) == TIMERR_NOERROR) {
-        UINT target = min(1, tc.wPeriodMin);
+        UINT target = (std::min)(static_cast<UINT>(1), tc.wPeriodMin);
         timeEndPeriod(target);
       }
     });
diff --git a/third_party/allwpilib/hal/src/main/native/sim/LEDs.cpp b/third_party/allwpilib/hal/src/main/native/sim/LEDs.cpp
new file mode 100644
index 0000000..e7d45aa
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/LEDs.cpp
@@ -0,0 +1,21 @@
+// 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 "hal/LEDs.h"
+
+#include "hal/simulation/RoboRioData.h"
+
+namespace hal::init {
+void InitializeLEDs() {}
+}  // namespace hal::init
+
+extern "C" {
+
+void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
+  HALSIM_SetRoboRioRadioLEDState(state);
+}
+HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status) {
+  return HALSIM_GetRoboRioRadioLEDState();
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp
index 9a9a1a1..8ccc019 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp
@@ -136,6 +136,7 @@
 DEFINE_CAPI(double, BrownoutVoltage, brownoutVoltage)
 DEFINE_CAPI(double, CPUTemp, cpuTemp)
 DEFINE_CAPI(int32_t, TeamNumber, teamNumber)
+DEFINE_CAPI(HAL_RadioLEDState, RadioLEDState, radioLedState)
 
 int32_t HALSIM_RegisterRoboRioSerialNumberCallback(
     HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) {
@@ -192,5 +193,6 @@
   REGISTER(userFaults3V3);
   REGISTER(brownoutVoltage);
   REGISTER(cpuTemp);
+  REGISTER(radioLedState);
 }
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
index aef61be..413c38e 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
@@ -32,10 +32,15 @@
   HAL_SIMDATAVALUE_DEFINE_NAME(BrownoutVoltage)
   HAL_SIMDATAVALUE_DEFINE_NAME(CPUTemp)
   HAL_SIMDATAVALUE_DEFINE_NAME(TeamNumber)
+  HAL_SIMDATAVALUE_DEFINE_NAME(RadioLEDState)
 
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(SerialNumber)
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(Comments);
 
+  static inline HAL_Value MakeRadioLEDStateValue(HAL_RadioLEDState value) {
+    return HAL_MakeEnum(value);
+  }
+
  public:
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFPGAButtonName> fpgaButton{false};
   SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{12.0};
@@ -61,6 +66,8 @@
       6.75};
   SimDataValue<double, HAL_MakeDouble, GetCPUTempName> cpuTemp{45.0};
   SimDataValue<int32_t, HAL_MakeInt, GetTeamNumberName> teamNumber{0};
+  SimDataValue<HAL_RadioLEDState, MakeRadioLEDStateValue, GetRadioLEDStateName>
+      radioLedState{HAL_RadioLED_kOff};
 
   int32_t RegisterSerialNumberCallback(HAL_RoboRioStringCallback callback,
                                        void* param, HAL_Bool initialNotify);
diff --git a/third_party/allwpilib/imgui/CMakeLists.txt b/third_party/allwpilib/imgui/CMakeLists.txt
index 6ba8455..f267a2c 100644
--- a/third_party/allwpilib/imgui/CMakeLists.txt
+++ b/third_party/allwpilib/imgui/CMakeLists.txt
@@ -1,49 +1,44 @@
-INCLUDE(FetchContent)
+include(FetchContent)
 
-FetchContent_Declare(
+fetchcontent_declare(
     glfw3
-    GIT_REPOSITORY  https://github.com/glfw/glfw.git
-    GIT_TAG         6b57e08bb0078c9834889eab871bac2368198c15
+    GIT_REPOSITORY https://github.com/glfw/glfw.git
+    GIT_TAG 6b57e08bb0078c9834889eab871bac2368198c15
 )
-FetchContent_Declare(
+fetchcontent_declare(
     gl3w
-    GIT_REPOSITORY  https://github.com/skaslev/gl3w
-    GIT_TAG         5f8d7fd191ba22ff2b60c1106d7135bb9a335533
+    GIT_REPOSITORY https://github.com/skaslev/gl3w
+    GIT_TAG 5f8d7fd191ba22ff2b60c1106d7135bb9a335533
 )
-FetchContent_Declare(
+fetchcontent_declare(
     imgui
-    GIT_REPOSITORY  https://github.com/ocornut/imgui.git
-                    # docking branch
-    GIT_TAG         64b1e448d20c9be9275af731c34b4c7bf14a8e95
+    GIT_REPOSITORY https://github.com/ocornut/imgui.git
+    # docking branch
+    GIT_TAG 64b1e448d20c9be9275af731c34b4c7bf14a8e95
 )
-FetchContent_Declare(
+fetchcontent_declare(
     implot
-    GIT_REPOSITORY  https://github.com/epezent/implot.git
-                    # v0.16
-    GIT_TAG         18c72431f8265e2b0b5378a3a73d8a883b2175ff
+    GIT_REPOSITORY https://github.com/epezent/implot.git
+    # v0.16
+    GIT_TAG 18c72431f8265e2b0b5378a3a73d8a883b2175ff
 )
-FetchContent_Declare(
+fetchcontent_declare(
     fonts
-    URL         https://github.com/wpilibsuite/thirdparty-fonts/releases/download/v0.2/fonts.zip
-    URL_HASH    SHA256=cedf365657fab0770e11f72d49e4f0f889f564d2e635a4d214029d0ab6bcd324
+    URL https://github.com/wpilibsuite/thirdparty-fonts/releases/download/v0.2/fonts.zip
+    URL_HASH SHA256=cedf365657fab0770e11f72d49e4f0f889f564d2e635a4d214029d0ab6bcd324
 )
-FetchContent_Declare(
+fetchcontent_declare(
     stb
-    GIT_REPOSITORY  https://github.com/nothings/stb.git
-    GIT_TAG         c9064e317699d2e495f36ba4f9ac037e88ee371a
+    GIT_REPOSITORY https://github.com/nothings/stb.git
+    GIT_TAG c9064e317699d2e495f36ba4f9ac037e88ee371a
 )
 
-FetchContent_MakeAvailable(
-    imgui
-    implot
-    fonts
-    stb
-)
+fetchcontent_makeavailable(imgui implot fonts stb)
 
 # Add glfw directly to our build.
-FetchContent_GetProperties(glfw3)
+fetchcontent_getproperties(glfw3)
 if(NOT glfw3_POPULATED)
-    FetchContent_Populate(glfw3)
+    fetchcontent_populate(glfw3)
     set(SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS})
     set(BUILD_SHARED_LIBS OFF)
     set(GLFW_INSTALL OFF)
@@ -53,9 +48,9 @@
 endif()
 
 # Don't use gl3w CMakeLists.txt due to https://github.com/skaslev/gl3w/issues/66
-FetchContent_GetProperties(gl3w)
+fetchcontent_getproperties(gl3w)
 if(NOT gl3w_POPULATED)
-    FetchContent_Populate(gl3w)
+    fetchcontent_populate(gl3w)
 endif()
 if(NOT EXISTS "${gl3w_BINARY_DIR}/src/gl3w.c")
     find_package(Python COMPONENTS Interpreter Development REQUIRED)
@@ -76,17 +71,18 @@
     ${imgui_SOURCE_DIR}/backends/imgui_impl_opengl3.cpp
     ${gl3w_BINARY_DIR}/src/gl3w.c
     ${fonts_sources}
-    src/stb_image.cpp)
-if (MSVC)
-  add_library(imgui STATIC ${imgui_all_sources})
+    src/stb_image.cpp
+)
+if(MSVC)
+    add_library(imgui STATIC ${imgui_all_sources})
 else()
-  add_library(imgui ${imgui_all_sources})
+    add_library(imgui ${imgui_all_sources})
 endif()
 target_compile_definitions(imgui PUBLIC IMGUI_IMPL_OPENGL_LOADER_GL3W)
-if (MSVC)
+if(MSVC)
     target_sources(imgui PRIVATE ${imgui_SOURCE_DIR}/backends/imgui_impl_dx11.cpp)
 else()
-    if (APPLE)
+    if(APPLE)
         target_compile_options(imgui PRIVATE -fobjc-arc)
         set_target_properties(imgui PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore")
         target_sources(imgui PRIVATE ${imgui_SOURCE_DIR}/backends/imgui_impl_metal.mm)
@@ -95,17 +91,17 @@
     endif()
 endif()
 target_link_libraries(imgui PUBLIC glfw)
-target_include_directories(imgui
+target_include_directories(
+    imgui
     PUBLIC
-    "$<BUILD_INTERFACE:${imgui_SOURCE_DIR}>"
-    "$<BUILD_INTERFACE:${imgui_SOURCE_DIR}/misc/cpp>"
-    "$<BUILD_INTERFACE:${implot_SOURCE_DIR}>"
-    "$<BUILD_INTERFACE:${imgui_SOURCE_DIR}/backends>"
-    "$<BUILD_INTERFACE:${gl3w_BINARY_DIR}/include>"
-    "$<BUILD_INTERFACE:${fonts_SOURCE_DIR}/include>"
-    "$<BUILD_INTERFACE:${stb_SOURCE_DIR}>"
-    PRIVATE
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>"
+        "$<BUILD_INTERFACE:${imgui_SOURCE_DIR}>"
+        "$<BUILD_INTERFACE:${imgui_SOURCE_DIR}/misc/cpp>"
+        "$<BUILD_INTERFACE:${implot_SOURCE_DIR}>"
+        "$<BUILD_INTERFACE:${imgui_SOURCE_DIR}/backends>"
+        "$<BUILD_INTERFACE:${gl3w_BINARY_DIR}/include>"
+        "$<BUILD_INTERFACE:${fonts_SOURCE_DIR}/include>"
+        "$<BUILD_INTERFACE:${stb_SOURCE_DIR}>"
+    PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>"
 )
 
 set_property(TARGET imgui PROPERTY POSITION_INDEPENDENT_CODE ON)
diff --git a/third_party/allwpilib/msvcruntime/build.gradle b/third_party/allwpilib/msvcruntime/build.gradle
index 7e268e8..2bbcd13 100644
--- a/third_party/allwpilib/msvcruntime/build.gradle
+++ b/third_party/allwpilib/msvcruntime/build.gradle
@@ -36,8 +36,7 @@
         return
     }
 
-    // TODO Remove VersionNumber
-    def expectedVersion = VersionNumber.parse(defaultRedistFile.text.trim())
+    def expectedVersion = defaultRedistFile.text.trim()
 
     def runtimeLocation = file("$vsDirectory\\VC\\Redist\\MSVC\\$expectedVersion")
 
diff --git a/third_party/allwpilib/myRobot/build.gradle b/third_party/allwpilib/myRobot/build.gradle
index 17407cd..a513d26 100644
--- a/third_party/allwpilib/myRobot/build.gradle
+++ b/third_party/allwpilib/myRobot/build.gradle
@@ -130,7 +130,7 @@
                 myRobotJava(JavaArtifact) {
                     jarTask = shadowJar
                     postdeploy << { ctx ->
-                        ctx.execute("echo '/usr/local/frc/JRE/bin/java -XX:+UseG1GC -XX:MaxGCPauseMillis=1 -XX:GCTimeRatio=1 -Djava.library.path=/usr/local/frc/third-party/lib -Djava.lang.invoke.stringConcat=BC_SB -jar /home/lvuser/myRobot-all.jar' > /home/lvuser/robotCommand")
+                        ctx.execute("echo '/usr/local/frc/JRE/bin/java -XX:+UseSerialGC -Djava.library.path=/usr/local/frc/third-party/lib -Djava.lang.invoke.stringConcat=BC_SB -jar /home/lvuser/myRobot-all.jar' > /home/lvuser/robotCommand")
                         ctx.execute("chmod +x /home/lvuser/robotCommand; chown lvuser /home/lvuser/robotCommand")
                     }
                 }
diff --git a/third_party/allwpilib/ntcore/.clang-tidy b/third_party/allwpilib/ntcore/.clang-tidy
new file mode 100644
index 0000000..41c15bb
--- /dev/null
+++ b/third_party/allwpilib/ntcore/.clang-tidy
@@ -0,0 +1,69 @@
+Checks:
+  'bugprone-assert-side-effect,
+  bugprone-bool-pointer-implicit-conversion,
+  bugprone-copy-constructor-init,
+  bugprone-dangling-handle,
+  bugprone-dynamic-static-initializers,
+  bugprone-forwarding-reference-overload,
+  bugprone-inaccurate-erase,
+  bugprone-incorrect-roundings,
+  bugprone-integer-division,
+  bugprone-lambda-function-name,
+  bugprone-misplaced-operator-in-strlen-in-alloc,
+  bugprone-misplaced-widening-cast,
+  bugprone-move-forwarding-reference,
+  bugprone-multiple-statement-macro,
+  bugprone-parent-virtual-call,
+  bugprone-posix-return,
+  bugprone-sizeof-container,
+  bugprone-sizeof-expression,
+  bugprone-spuriously-wake-up-functions,
+  bugprone-string-constructor,
+  bugprone-string-integer-assignment,
+  bugprone-string-literal-with-embedded-nul,
+  bugprone-suspicious-enum-usage,
+  bugprone-suspicious-include,
+  bugprone-suspicious-memset-usage,
+  bugprone-suspicious-missing-comma,
+  bugprone-suspicious-semicolon,
+  bugprone-suspicious-string-compare,
+  bugprone-throw-keyword-missing,
+  bugprone-too-small-loop-variable,
+  bugprone-undefined-memory-manipulation,
+  bugprone-undelegated-constructor,
+  bugprone-unhandled-self-assignment,
+  bugprone-unused-raii,
+  bugprone-virtual-near-miss,
+  cert-err52-cpp,
+  cert-err60-cpp,
+  cert-mem57-cpp,
+  cert-oop57-cpp,
+  cert-oop58-cpp,
+  clang-diagnostic-*,
+  -clang-diagnostic-deprecated-declarations,
+  -clang-diagnostic-#warnings,
+  -clang-diagnostic-pedantic,
+  clang-analyzer-*,
+  cppcoreguidelines-slicing,
+  google-build-namespaces,
+  google-explicit-constructor,
+  google-global-names-in-headers,
+  google-readability-avoid-underscore-in-googletest-name,
+  google-readability-casting,
+  google-runtime-operator,
+  misc-definitions-in-headers,
+  misc-misplaced-const,
+  misc-new-delete-overloads,
+  misc-non-copyable-objects,
+  modernize-avoid-bind,
+  modernize-concat-nested-namespaces,
+  modernize-make-shared,
+  modernize-make-unique,
+  modernize-pass-by-value,
+  modernize-use-default-member-init,
+  modernize-use-noexcept,
+  modernize-use-nullptr,
+  modernize-use-override,
+  modernize-use-using,
+  readability-braces-around-statements'
+FormatStyle: file
diff --git a/third_party/allwpilib/ntcore/.styleguide b/third_party/allwpilib/ntcore/.styleguide
index 1808bb5..13634e4 100644
--- a/third_party/allwpilib/ntcore/.styleguide
+++ b/third_party/allwpilib/ntcore/.styleguide
@@ -14,6 +14,8 @@
 
 generatedFileExclude {
   ntcore/doc/
+  ntcore/src/generated
+  .*\.jinja
 }
 
 repoRootNameOverride {
diff --git a/third_party/allwpilib/ntcore/CMakeLists.txt b/third_party/allwpilib/ntcore/CMakeLists.txt
index 5216eda..2b9d326 100644
--- a/third_party/allwpilib/ntcore/CMakeLists.txt
+++ b/third_party/allwpilib/ntcore/CMakeLists.txt
@@ -3,30 +3,25 @@
 include(CompileWarnings)
 include(AddTest)
 
-execute_process(COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/generate_topics.py ${WPILIB_BINARY_DIR}/ntcore RESULT_VARIABLE generateResult)
-if(NOT (generateResult EQUAL "0"))
-  # Try python
-  execute_process(COMMAND python ${CMAKE_CURRENT_SOURCE_DIR}/generate_topics.py ${WPILIB_BINARY_DIR}/ntcore RESULT_VARIABLE generateResult)
-  if(NOT (generateResult EQUAL "0"))
-    message(FATAL_ERROR "python and python3 generate_topics.py failed")
-  endif()
-endif()
-
-file(GLOB ntcore_native_src
+file(
+    GLOB ntcore_native_src
     src/main/native/cpp/*.cpp
-    ${WPILIB_BINARY_DIR}/ntcore/generated/main/native/cpp/*.cpp
+    src/generated/main/native/cpp/*.cpp
     src/main/native/cpp/net/*.cpp
     src/main/native/cpp/net3/*.cpp
     src/main/native/cpp/networktables/*.cpp
-    src/main/native/cpp/tables/*.cpp)
+    src/main/native/cpp/tables/*.cpp
+)
 add_library(ntcore ${ntcore_native_src})
 set_target_properties(ntcore PROPERTIES DEBUG_POSTFIX "d")
-target_include_directories(ntcore
-                PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/cpp
-                PUBLIC
-                $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                $<BUILD_INTERFACE:${WPILIB_BINARY_DIR}/ntcore/generated/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/ntcore>)
+target_include_directories(
+    ntcore
+    PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/cpp
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/generated/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/ntcore>
+)
 wpilib_target_warnings(ntcore)
 target_compile_features(ntcore PUBLIC cxx_std_20)
 target_link_libraries(ntcore PUBLIC wpinet wpiutil)
@@ -35,43 +30,38 @@
 
 install(TARGETS ntcore EXPORT ntcore)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/ntcore")
-install(DIRECTORY ${WPILIB_BINARY_DIR}/ntcore/generated/main/native/include/ DESTINATION "${include_dest}/ntcore")
+install(DIRECTORY src/generated/main/native/include/ DESTINATION "${include_dest}/ntcore")
 
-if (WITH_FLAT_INSTALL)
-    set (ntcore_config_dir ${wpilib_dest})
-else()
-    set (ntcore_config_dir share/ntcore)
-endif()
-
-configure_file(ntcore-config.cmake.in ${WPILIB_BINARY_DIR}/ntcore-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/ntcore-config.cmake DESTINATION ${ntcore_config_dir})
-install(EXPORT ntcore DESTINATION ${ntcore_config_dir})
+configure_file(ntcore-config.cmake.in ${WPILIB_BINARY_DIR}/ntcore-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/ntcore-config.cmake DESTINATION share/ntcore)
+install(EXPORT ntcore DESTINATION share/ntcore)
 
 # Java bindings
-if (WITH_JAVA)
+if(WITH_JAVA)
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
     set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-    file(GLOB QUICKBUF_JAR
-        ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar)
+    file(GLOB QUICKBUF_JAR ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar)
 
     set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${QUICKBUF_JAR})
 
-    file(GLOB ntcore_jni_src
-        src/main/native/cpp/jni/*.cpp
-        ${WPILIB_BINARY_DIR}/ntcore/generated/main/native/cpp/jni/*.cpp)
+    file(GLOB ntcore_jni_src src/main/native/cpp/jni/*.cpp src/generated/main/native/cpp/jni/*.cpp)
 
-    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${WPILIB_BINARY_DIR}/ntcore/generated/*.java)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java src/generated/main/java/*.java)
     set(CMAKE_JNI_TARGET true)
 
-    add_jar(ntcore_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar OUTPUT_NAME ntcore GENERATE_NATIVE_HEADERS ntcore_jni_headers)
+    add_jar(
+        ntcore_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS wpiutil_jar
+        OUTPUT_NAME ntcore
+        GENERATE_NATIVE_HEADERS ntcore_jni_headers
+    )
 
-    get_property(NTCORE_JAR_FILE TARGET ntcore_jar PROPERTY JAR_FILE)
-    install(FILES ${NTCORE_JAR_FILE} DESTINATION "${java_lib_dest}")
-
-    set_property(TARGET ntcore_jar PROPERTY FOLDER "java")
+    install_jar(ntcore_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS ntcore_jar FILE ntcore_jar.cmake DESTINATION share/ntcore)
 
     add_library(ntcorejni ${ntcore_jni_src})
     wpilib_target_warnings(ntcorejni)
@@ -79,7 +69,7 @@
 
     set_property(TARGET ntcorejni PROPERTY FOLDER "libraries")
 
-    if (MSVC)
+    if(MSVC)
         install(TARGETS ntcorejni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
     endif()
 
@@ -87,16 +77,22 @@
     add_dependencies(ntcorejni ntcore_jar)
 
     install(TARGETS ntcorejni EXPORT ntcorejni)
-
 endif()
 
-if (WITH_JAVA_SOURCE)
+if(WITH_JAVA_SOURCE)
     find_package(Java REQUIRED)
     include(UseJava)
-    file(GLOB NTCORE_SOURCES src/main/java/edu/wpi/first/networktables/*.java ${WPILIB_BINARY_DIR}/ntcore/generated/*.java)
-    add_jar(ntcore_src_jar
-    RESOURCES NAMESPACE "edu/wpi/first/networktables" ${NTCORE_SOURCES}
-    OUTPUT_NAME ntcore-sources)
+    file(
+        GLOB NTCORE_SOURCES
+        src/main/java/edu/wpi/first/networktables/*.java
+        src/generated/main/java/*.java
+    )
+    add_jar(
+        ntcore_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/networktables" ${NTCORE_SOURCES}
+        OUTPUT_NAME ntcore-sources
+    )
 
     get_property(NTCORE_SRC_JAR_FILE TARGET ntcore_src_jar PROPERTY JAR_FILE)
     install(FILES ${NTCORE_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
@@ -108,7 +104,7 @@
 wpilib_target_warnings(ntcoredev)
 target_link_libraries(ntcoredev ntcore)
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(ntcore src/test/native/cpp)
     target_include_directories(ntcore_test PRIVATE src/main/native/cpp)
     target_link_libraries(ntcore_test ntcore gmock_main wpiutil_testlib)
diff --git a/third_party/allwpilib/ntcore/build.gradle b/third_party/allwpilib/ntcore/build.gradle
index c5464cc..510e5c1 100644
--- a/third_party/allwpilib/ntcore/build.gradle
+++ b/third_party/allwpilib/ntcore/build.gradle
@@ -1,290 +1,24 @@
-import groovy.json.JsonSlurper;
-import com.hubspot.jinjava.Jinjava;
-import com.hubspot.jinjava.JinjavaConfig;
-
-def ntcoreTypesInputFile = file("src/generate/types.json")
-def ntcoreJavaTypesInputDir = file("src/generate/java")
-def ntcoreJavaTypesOutputDir = file("$buildDir/generated/main/java/edu/wpi/first/networktables")
-
-task ntcoreGenerateJavaTypes() {
-    description = "Generates ntcore Java type classes"
-    group = "WPILib"
-
-    inputs.file ntcoreTypesInputFile
-    inputs.dir ntcoreJavaTypesInputDir
-    outputs.dir ntcoreJavaTypesOutputDir
-
-    doLast {
-        def jsonSlurper = new JsonSlurper()
-        def jsonTypes = jsonSlurper.parse(ntcoreTypesInputFile)
-
-        ntcoreJavaTypesOutputDir.deleteDir()
-        ntcoreJavaTypesOutputDir.mkdirs()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        ntcoreJavaTypesInputDir.listFiles().each { File file ->
-            def template = file.text
-            def outfn = file.name.substring(0, file.name.length() - 6)
-            if (file.name.startsWith("NetworkTable") || file.name.startsWith("Generic")) {
-                def replacements = new HashMap<String,?>()
-                replacements.put("types", jsonTypes)
-                def output = jinjava.render(template, replacements)
-                new File(ntcoreJavaTypesOutputDir, outfn).write(output)
-            } else {
-                jsonTypes.each { Map<String,?> replacements ->
-                    def output = jinjava.render(template, replacements)
-                    def typename = replacements.get("TypeName")
-                    File outfile
-                    if (outfn == "Timestamped.java") {
-                        outfile = new File(ntcoreJavaTypesOutputDir, "Timestamped${typename}.java")
-                    } else {
-                        outfile = new File(ntcoreJavaTypesOutputDir, "${typename}${outfn}")
-                    }
-                    outfile.write(output)
-                }
-            }
-        }
-    }
-}
-
-def ntcoreCppTypesInputDir = file("src/generate/include/networktables")
-def ntcoreCppTypesOutputDir = file("$buildDir/generated/main/native/include/networktables")
-
-task ntcoreGenerateCppTypes() {
-    description = "Generates ntcore C++ type classes"
-    group = "WPILib"
-
-    inputs.file ntcoreTypesInputFile
-    inputs.dir ntcoreCppTypesInputDir
-    outputs.dir ntcoreCppTypesOutputDir
-
-    doLast {
-        def jsonSlurper = new JsonSlurper()
-        def jsonTypes = jsonSlurper.parse(ntcoreTypesInputFile)
-
-        ntcoreCppTypesOutputDir.deleteDir()
-        ntcoreCppTypesOutputDir.mkdirs()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        ntcoreCppTypesInputDir.listFiles().each { File file ->
-            def template = file.text
-            def outfn = file.name.substring(0, file.name.length() - 6)
-            jsonTypes.each { Map<String,?> replacements ->
-                def output = jinjava.render(template, replacements)
-                def typename = replacements.get("TypeName")
-                def outfile = new File(ntcoreCppTypesOutputDir, "${typename}${outfn}")
-                outfile.write(output)
-            }
-        }
-    }
-}
-
-def ntcoreCppHandleSourceInputFile = file("src/generate/cpp/ntcore_cpp_types.cpp.jinja")
-def ntcoreCppHandleSourceOutputFile = file("$buildDir/generated/main/native/cpp/ntcore_cpp_types.cpp")
-
-task ntcoreGenerateCppHandleSource() {
-    description = "Generates ntcore C++ handle source"
-    group = "WPILib"
-
-    inputs.files([
-        ntcoreTypesInputFile,
-        ntcoreCppHandleSourceInputFile
-    ])
-    outputs.file ntcoreCppHandleSourceOutputFile
-
-    doLast {
-        def jsonSlurper = new JsonSlurper()
-        def jsonTypes = jsonSlurper.parse(ntcoreTypesInputFile)
-
-        ntcoreCppHandleSourceOutputFile.delete()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        def template = ntcoreCppHandleSourceInputFile.text
-        def replacements = new HashMap<String,?>()
-        replacements.put("types", jsonTypes)
-        def output = jinjava.render(template, replacements)
-        ntcoreCppHandleSourceOutputFile.write(output)
-    }
-}
-
-def ntcoreCppHandleHeaderInputFile = file("src/generate/include/ntcore_cpp_types.h.jinja")
-def ntcoreCppHandleHeaderOutputFile = file("$buildDir/generated/main/native/include/ntcore_cpp_types.h")
-
-task ntcoreGenerateCppHandleHeader() {
-    description = "Generates ntcore C++ handle header"
-    group = "WPILib"
-
-    inputs.files([
-        ntcoreTypesInputFile,
-        ntcoreCppHandleHeaderInputFile
-    ])
-    outputs.file ntcoreCppHandleHeaderOutputFile
-
-    doLast {
-        def jsonSlurper = new JsonSlurper()
-        def jsonTypes = jsonSlurper.parse(ntcoreTypesInputFile)
-
-        ntcoreCppHandleHeaderOutputFile.delete()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        def template = ntcoreCppHandleHeaderInputFile.text
-        def replacements = new HashMap<String,?>()
-        replacements.put("types", jsonTypes)
-        def output = jinjava.render(template, replacements)
-        ntcoreCppHandleHeaderOutputFile.write(output)
-    }
-}
-
-def ntcoreCHandleSourceInputFile = file("src/generate/cpp/ntcore_c_types.cpp.jinja")
-def ntcoreCHandleSourceOutputFile = file("$buildDir/generated/main/native/cpp/ntcore_c_types.cpp")
-
-task ntcoreGenerateCHandleSource() {
-    description = "Generates ntcore C handle source"
-    group = "WPILib"
-
-    inputs.files([
-        ntcoreTypesInputFile,
-        ntcoreCHandleSourceInputFile
-    ])
-    outputs.file ntcoreCHandleSourceOutputFile
-
-    doLast {
-        def jsonSlurper = new JsonSlurper()
-        def jsonTypes = jsonSlurper.parse(ntcoreTypesInputFile)
-
-        ntcoreCHandleSourceOutputFile.delete()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        def template = ntcoreCHandleSourceInputFile.text
-        def replacements = new HashMap<String,?>()
-        replacements.put("types", jsonTypes)
-        def output = jinjava.render(template, replacements)
-        ntcoreCHandleSourceOutputFile.write(output)
-    }
-}
-
-def ntcoreCHandleHeaderInputFile = file("src/generate/include/ntcore_c_types.h.jinja")
-def ntcoreCHandleHeaderOutputFile = file("$buildDir/generated/main/native/include/ntcore_c_types.h")
-
-task ntcoreGenerateCHandleHeader() {
-    description = "Generates ntcore C handle header"
-    group = "WPILib"
-
-    inputs.files([
-        ntcoreTypesInputFile,
-        ntcoreCHandleHeaderInputFile
-    ])
-    outputs.file ntcoreCHandleHeaderOutputFile
-
-    doLast {
-        def jsonSlurper = new JsonSlurper()
-        def jsonTypes = jsonSlurper.parse(ntcoreTypesInputFile)
-
-        ntcoreCHandleHeaderOutputFile.delete()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        def template = ntcoreCHandleHeaderInputFile.text
-        def replacements = new HashMap<String,?>()
-        replacements.put("types", jsonTypes)
-        def output = jinjava.render(template, replacements)
-        ntcoreCHandleHeaderOutputFile.write(output)
-    }
-}
-
-def ntcoreJniSourceInputFile = file("src/generate/cpp/jni/types_jni.cpp.jinja")
-def ntcoreJniSourceOutputFile = file("$buildDir/generated/main/native/cpp/jni/types_jni.cpp")
-
-task ntcoreGenerateJniSource() {
-    description = "Generates ntcore JNI types source"
-    group = "WPILib"
-
-    inputs.files([
-        ntcoreTypesInputFile,
-        ntcoreJniSourceInputFile
-    ])
-    outputs.file ntcoreJniSourceOutputFile
-
-    doLast {
-        def jsonSlurper = new JsonSlurper()
-        def jsonTypes = jsonSlurper.parse(ntcoreTypesInputFile)
-
-        ntcoreJniSourceOutputFile.delete()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        def template = ntcoreJniSourceInputFile.text
-        def replacements = new HashMap<String,?>()
-        replacements.put("types", jsonTypes)
-        def output = jinjava.render(template, replacements)
-        ntcoreJniSourceOutputFile.write(output)
-    }
-}
-
 ext {
     addNtcoreDependency = { binary, shared->
-        binary.tasks.withType(AbstractNativeSourceCompileTask) {
-            it.dependsOn ntcoreGenerateCppTypes
-            it.dependsOn ntcoreGenerateCppHandleHeader
-            it.dependsOn ntcoreGenerateCHandleHeader
-        }
         binary.lib project: ':ntcore', library: 'ntcore', linkage: shared
     }
 
     addNtcoreJniDependency = { binary->
-        binary.tasks.withType(AbstractNativeSourceCompileTask) {
-            it.dependsOn ntcoreGenerateCppTypes
-            it.dependsOn ntcoreGenerateCppHandleHeader
-            it.dependsOn ntcoreGenerateCHandleHeader
-        }
         binary.lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
     }
 
     nativeName = 'ntcore'
     devMain = 'edu.wpi.first.ntcore.DevMain'
-    generatedSources = "$buildDir/generated/main/native/cpp"
-    generatedHeaders = "$buildDir/generated/main/native/include"
+    generatedSources = "$projectDir/src/generated/main/native/cpp"
+    generatedHeaders = "$projectDir/src/generated/main/native/include"
     jniSplitSetup = {
-        it.tasks.withType(CppCompile) {
-            it.dependsOn ntcoreGenerateCppTypes
-            it.dependsOn ntcoreGenerateCppHandleSource
-            it.dependsOn ntcoreGenerateCppHandleHeader
-            it.dependsOn ntcoreGenerateCHandleSource
-            it.dependsOn ntcoreGenerateCHandleHeader
-            it.dependsOn ntcoreGenerateJniSource
-        }
     }
     splitSetup = {
         it.tasks.withType(CppCompile) {
-            it.dependsOn ntcoreGenerateCppTypes
-            it.dependsOn ntcoreGenerateCppHandleSource
-            it.dependsOn ntcoreGenerateCppHandleHeader
-            it.dependsOn ntcoreGenerateCHandleSource
-            it.dependsOn ntcoreGenerateCHandleHeader
-            it.dependsOn ntcoreGenerateJniSource
             it.includes 'src/main/native/cpp'
         }
     }
     exeSplitSetup = {
-        it.tasks.withType(CppCompile) {
-            it.dependsOn ntcoreGenerateCppTypes
-            it.dependsOn ntcoreGenerateCppHandleSource
-            it.dependsOn ntcoreGenerateCppHandleHeader
-            it.dependsOn ntcoreGenerateCHandleSource
-            it.dependsOn ntcoreGenerateCHandleHeader
-        }
     }
 }
 
@@ -308,13 +42,9 @@
     }
 }
 
-sourceSets.main.java.srcDir "${buildDir}/generated/main/java"
-compileJava.dependsOn ntcoreGenerateJavaTypes
+sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
 
 cppHeadersZip {
-    dependsOn ntcoreGenerateCppTypes
-    dependsOn ntcoreGenerateCppHandleHeader
-    dependsOn ntcoreGenerateCHandleHeader
     from(generatedHeaders) {
         into '/'
     }
diff --git a/third_party/allwpilib/ntcore/doc/networktables4.adoc b/third_party/allwpilib/ntcore/doc/networktables4.adoc
index da157e5..29e74a1 100644
--- a/third_party/allwpilib/ntcore/doc/networktables4.adoc
+++ b/third_party/allwpilib/ntcore/doc/networktables4.adoc
@@ -23,7 +23,7 @@
 * Recommend that timestamp synchronization occur immediately following connection establishment and prior to any other control messages
 * Recommend text and binary combining into a single WebSockets frame be limited to the network MTU (unless necessary to transport the message)
 * Recommend WebSockets fragmentation be used on large frames to enable rapid handling of PING messages
-* Add an option for topics to be marked transient (in which case no last value is retained by the server or sent to clients on initial subscription)
+* Add a (default true) option for topics to be marked cached (in which case the last value is retained by the server and sent to clients on initial subscription)
 * Recommend clients subscribe to the `$sub$<topic>` meta-topic for each topic published by the client, and use this information to control what value updates are sent over the network to the server
 
 Version 4.1 uses a different WebSockets subprotocol string than version 4.0, so it is easy for both clients and servers to simultaneously support both versions 4.0 and 4.1. Due to WebSockets implementation bugs in version 4.0, version 4.1 implementations must not send WebSockets PING messages on version 4.0 connections.
@@ -100,7 +100,7 @@
 [[reconnection]]
 === Caching and Reconnection Handling
 
-Servers shall keep a retained value for each topic for the purposes of <<msg-subscribe>> requests; the retained value shall be the value in the largest timestamp (greater-than or equal-to) message received for that topic.  This retained value is deleted if the topic is deleted (e.g. there are no more publishers).
+Servers shall keep a retained value for each cached topic for the purposes of <<msg-subscribe>> requests; the retained value shall be the value in the largest timestamp (greater-than or equal-to) message received for that topic.  This retained value is deleted if the topic is deleted (e.g. there are no more publishers).
 
 Clients may similarly keep a retained value for each topic for ease of use by user code.  If this is done, this retained value shall be updated by both locally published values and received messages for that topic with greater-than/equal-to timestamps, and the retained value shall be deleted when a <<msg-unannounce>> is received.
 
@@ -419,6 +419,7 @@
 |Property|Type|Description|Notes
 |`persistent`|boolean|Persistent Flag|If true, the last set value will be periodically saved to persistent storage on the server and be restored during server startup.  Topics with this property set to true will not be deleted by the server when the last publisher stops publishing.
 |`retained`|boolean|Retained Flag|Topics with this property set to true will not be deleted by the server when the last publisher stops publishing.
+|`cached`|boolean|Cached Flag|If false, the server and clients will not store the value of the topic.  This means that only value updates will be available for the topic.
 |===
 
 [[sub-options]]
@@ -619,7 +620,7 @@
 [[msg-subscribe]]
 ==== Subscribe Message (`subscribe`)
 
-Sent from a client to the server to indicate the client wants to subscribe to value changes for the specified topics / groups of topics.  The server shall send MessagePack messages containing the current values for any existing topics upon receipt, and continue sending MessagePack messages for future value changes.  If a topic does not yet exist, no message is sent until it is created (via a publish), at which point a <<msg-announce>> will be sent and MessagePack messages will automatically follow as they are published.
+Sent from a client to the server to indicate the client wants to subscribe to value changes for the specified topics / groups of topics.  The server shall send MessagePack messages containing the current values for any existing cached topics upon receipt, and continue sending MessagePack messages for future value changes.  If a topic does not yet exist, no message is sent until it is created (via a publish), at which point a <<msg-announce>> will be sent and MessagePack messages will automatically follow as they are published.
 
 Subscriptions may overlap; only one MessagePack message is sent per value change regardless of the number of subscriptions.  Sending a `subscribe` message with the same subscription UID as a previous `subscribe` message results in updating the subscription (replacing the array of identifiers and updating any specified options).
 
diff --git a/third_party/allwpilib/ntcore/generate_topics.py b/third_party/allwpilib/ntcore/generate_topics.py
old mode 100644
new mode 100755
index ece7df2..5afd61a
--- a/third_party/allwpilib/ntcore/generate_topics.py
+++ b/third_party/allwpilib/ntcore/generate_topics.py
@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 import glob
 import os
 import sys
@@ -17,23 +19,22 @@
                 return
 
     # File either doesn't exist or has different contents
-    with open(outpathname, "w") as f:
+    with open(outpathname, "w", newline="\n") as f:
         f.write(contents)
 
 
 def main():
     dirname, _ = os.path.split(os.path.abspath(__file__))
-    cmake_binary_dir = sys.argv[1]
 
     with open(f"{dirname}/src/generate/types.json") as f:
         types = json.load(f)
 
     # Java files
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate/java"), autoescape=False
+        loader=FileSystemLoader(f"{dirname}/src/generate/main/java"), autoescape=False
     )
-    rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/networktables"
-    for fn in glob.glob(f"{dirname}/src/generate/java/*.jinja"):
+    rootPath = f"{dirname}/src/generated/main/java/edu/wpi/first/networktables"
+    for fn in glob.glob(f"{dirname}/src/generate/main/java/*.jinja"):
         template = env.get_template(os.path.basename(fn))
         outfn = os.path.basename(fn)[:-6]  # drop ".jinja"
         if os.path.basename(fn).startswith("NetworkTable") or os.path.basename(
@@ -52,11 +53,15 @@
 
     # C++ classes
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate/include/networktables"),
+        loader=FileSystemLoader(
+            f"{dirname}/src/generate/main/native/include/networktables"
+        ),
         autoescape=False,
     )
-    rootPath = f"{cmake_binary_dir}/generated/main/native/include/networktables"
-    for fn in glob.glob(f"{dirname}/src/generate/include/networktables/*.jinja"):
+    rootPath = f"{dirname}/src/generated/main/native/include/networktables"
+    for fn in glob.glob(
+        f"{dirname}/src/generate/main/native/include/networktables/*.jinja"
+    ):
         template = env.get_template(os.path.basename(fn))
         outfn = os.path.basename(fn)[:-6]  # drop ".jinja"
         for replacements in types:
@@ -66,55 +71,56 @@
 
     # C++ handle API (header)
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate/include"), autoescape=False
+        loader=FileSystemLoader(f"{dirname}/src/generate/main/native/include"),
+        autoescape=False,
     )
     template = env.get_template("ntcore_cpp_types.h.jinja")
     output = template.render(types=types)
     Output(
-        f"{cmake_binary_dir}/generated/main/native/include",
+        f"{dirname}/src/generated/main/native/include",
         "ntcore_cpp_types.h",
         output,
     )
 
     # C++ handle API (source)
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate/cpp"), autoescape=False
+        loader=FileSystemLoader(f"{dirname}/src/generate/main/native/cpp"),
+        autoescape=False,
     )
     template = env.get_template("ntcore_cpp_types.cpp.jinja")
     output = template.render(types=types)
-    Output(
-        f"{cmake_binary_dir}/generated/main/native/cpp", "ntcore_cpp_types.cpp", output
-    )
+    Output(f"{dirname}/src/generated/main/native/cpp", "ntcore_cpp_types.cpp", output)
 
     # C handle API (header)
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate/include"), autoescape=False
+        loader=FileSystemLoader(f"{dirname}/src/generate/main/native/include"),
+        autoescape=False,
     )
     template = env.get_template("ntcore_c_types.h.jinja")
     output = template.render(types=types)
     Output(
-        f"{cmake_binary_dir}/generated/main/native/include",
+        f"{dirname}/src/generated/main/native/include",
         "ntcore_c_types.h",
         output,
     )
 
     # C handle API (source)
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate/cpp"), autoescape=False
+        loader=FileSystemLoader(f"{dirname}/src/generate/main/native/cpp"),
+        autoescape=False,
     )
     template = env.get_template("ntcore_c_types.cpp.jinja")
     output = template.render(types=types)
-    Output(
-        f"{cmake_binary_dir}/generated/main/native/cpp", "ntcore_c_types.cpp", output
-    )
+    Output(f"{dirname}/src/generated/main/native/cpp", "ntcore_c_types.cpp", output)
 
     # JNI
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate/cpp/jni"), autoescape=False
+        loader=FileSystemLoader(f"{dirname}/src/generate/main/native/cpp/jni"),
+        autoescape=False,
     )
     template = env.get_template("types_jni.cpp.jinja")
     output = template.render(types=types)
-    Output(f"{cmake_binary_dir}/generated/main/native/cpp/jni", "types_jni.cpp", output)
+    Output(f"{dirname}/src/generated/main/native/cpp/jni", "types_jni.cpp", output)
 
 
 if __name__ == "__main__":
diff --git a/third_party/allwpilib/ntcore/ntcore-config.cmake.in b/third_party/allwpilib/ntcore/ntcore-config.cmake.in
index 0a85f8b..9642d63 100644
--- a/third_party/allwpilib/ntcore/ntcore-config.cmake.in
+++ b/third_party/allwpilib/ntcore/ntcore-config.cmake.in
@@ -5,3 +5,6 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/ntcore.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/ntcore_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp b/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp
index 6e43fdb..d724bbe 100644
--- a/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp
@@ -24,7 +24,7 @@
 void stress();
 
 int main(int argc, char* argv[]) {
-  wpi::impl::SetupNowRio();
+  wpi::impl::SetupNowDefaultOnRio();
 
   if (argc == 2 && std::string_view{argv[1]} == "bench") {
     bench();
diff --git a/third_party/allwpilib/ntcore/src/generate/cpp/jni/types_jni.cpp.jinja b/third_party/allwpilib/ntcore/src/generate/cpp/jni/types_jni.cpp.jinja
deleted file mode 100644
index 29cf570..0000000
--- a/third_party/allwpilib/ntcore/src/generate/cpp/jni/types_jni.cpp.jinja
+++ /dev/null
@@ -1,369 +0,0 @@
-// 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 <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "edu_wpi_first_networktables_NetworkTablesJNI.h"
-#include "ntcore.h"
-
-using namespace wpi::java;
-
-//
-// Globals and load/unload
-//
-{% for t in types %}
-static JClass timestamped{{ t.TypeName }}Cls;
-{%- endfor %}
-{%- for t in types %}
-{%- if t.jni.ToJavaArray == "MakeJObjectArray" %}
-static JClass {{ t.jni.jtype }}Cls;
-{%- endif %}
-{%- endfor %}
-static JException illegalArgEx;
-static JException indexOobEx;
-static JException nullPointerEx;
-
-static const JClassInit classes[] = {
-{%- for t in types %}
-    {"edu/wpi/first/networktables/Timestamped{{ t.TypeName }}", &timestamped{{ t.TypeName }}Cls},
-{%- endfor %}
-{%- for t in types %}
-{%- if t.jni.ToJavaArray == "MakeJObjectArray" %}
-    {"{{ t.jni.jtypestr }}", &{{ t.jni.jtype }}Cls},
-{%- endif %}
-{%- endfor %}
-};
-
-static const JExceptionInit exceptions[] = {
-    {"java/lang/IllegalArgumentException", &illegalArgEx},
-    {"java/lang/IndexOutOfBoundsException", &indexOobEx},
-    {"java/lang/NullPointerException", &nullPointerEx},
-};
-
-namespace nt {
-
-bool JNI_LoadTypes(JNIEnv* env) {
-  // Cache references to classes
-  for (auto& c : classes) {
-    *c.cls = JClass(env, c.name);
-    if (!*c.cls) {
-      return false;
-    }
-  }
-
-  for (auto& c : exceptions) {
-    *c.cls = JException(env, c.name);
-    if (!*c.cls) {
-      return false;
-    }
-  }
-
-  return true;
-}
-
-void JNI_UnloadTypes(JNIEnv* env) {
-  // Delete global references
-  for (auto& c : classes) {
-    c.cls->free(env);
-  }
-  for (auto& c : exceptions) {
-    c.cls->free(env);
-  }
-}
-
-}  // namespace nt
-
-static std::vector<int> FromJavaBooleanArray(JNIEnv* env, jbooleanArray jarr) {
-  CriticalJSpan<const jboolean> ref{env, jarr};
-  if (!ref) {
-    return {};
-  }
-  std::span<const jboolean> elements{ref};
-  size_t len = elements.size();
-  std::vector<int> arr;
-  arr.reserve(len);
-  for (size_t i = 0; i < len; ++i) {
-    arr.push_back(elements[i]);
-  }
-  return arr;
-}
-
-static std::vector<std::string> FromJavaStringArray(JNIEnv* env, jobjectArray jarr) {
-  size_t len = env->GetArrayLength(jarr);
-  std::vector<std::string> arr;
-  arr.reserve(len);
-  for (size_t i = 0; i < len; ++i) {
-    JLocal<jstring> elem{
-        env, static_cast<jstring>(env->GetObjectArrayElement(jarr, i))};
-    if (!elem) {
-      return {};
-    }
-    arr.emplace_back(JStringRef{env, elem}.str());
-  }
-  return arr;
-}
-
-{% for t in types %}
-static jobject MakeJObject(JNIEnv* env, nt::Timestamped{{ t.TypeName }} value) {
-  static jmethodID constructor = env->GetMethodID(
-      timestamped{{ t.TypeName }}Cls, "<init>", "(JJ{{ t.jni.jtypestr }})V");
-{%- if t.jni.JavaObject %}
-  JLocal<{{ t.jni.jtype }}> val{env, {{ t.jni.ToJavaBegin }}value.value{{ t.jni.ToJavaEnd }}};
-  return env->NewObject(timestamped{{ t.TypeName }}Cls, constructor,
-                        static_cast<jlong>(value.time),
-                        static_cast<jlong>(value.serverTime), val.obj());
-{%- else %}
-  return env->NewObject(timestamped{{ t.TypeName }}Cls, constructor,
-                        static_cast<jlong>(value.time),
-                        static_cast<jlong>(value.serverTime),
-                        {{ t.jni.ToJavaBegin }}value.value{{ t.jni.ToJavaEnd }});
-{%- endif %}
-}
-{% endfor %}
-{%- for t in types %}
-static jobjectArray MakeJObject(JNIEnv* env,
-                                std::span<const nt::Timestamped{{ t.TypeName }}> arr) {
-  jobjectArray jarr =
-      env->NewObjectArray(arr.size(), timestamped{{ t.TypeName }}Cls, nullptr);
-  if (!jarr) {
-    return nullptr;
-  }
-  for (size_t i = 0; i < arr.size(); ++i) {
-    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
-    env->SetObjectArrayElement(jarr, i, elem.obj());
-  }
-  return jarr;
-}
-{% endfor %}
-{%- for t in types %}
-{%- if t.jni.ToJavaArray == "MakeJObjectArray" %}
-static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const {{ t.cpp.ValueType }}> arr) {
-  jobjectArray jarr =
-      env->NewObjectArray(arr.size(), {{ t.jni.jtype }}Cls, nullptr);
-  if (!jarr) {
-    return nullptr;
-  }
-  for (size_t i = 0; i < arr.size(); ++i) {
-    JLocal<jobject> elem{env, {{ t.jni.ToJavaBegin }}arr[i]{{ t.jni.ToJavaEnd }}};
-    env->SetObjectArrayElement(jarr, i, elem.obj());
-  }
-  return jarr;
-}
-{% endif %}
-{%- endfor %}
-
-extern "C" {
-{% for t in types %}
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    getAtomic{{ t.TypeName }}
- * Signature: (I{{ t.jni.jtypestr }})Ledu/wpi/first/networktables/Timestamped{{ t.TypeName }};
- */
-JNIEXPORT jobject JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomic{{ t.TypeName }}
-  (JNIEnv* env, jclass, jint subentry, {{ t.jni.jtype }} defaultValue)
-{
-  return MakeJObject(env, nt::GetAtomic{{ t.TypeName }}(subentry, {{ t.jni.FromJavaBegin }}defaultValue{{ t.jni.FromJavaEnd }}));
-}
-
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    readQueue{{ t.TypeName }}
- * Signature: (I)[Ledu/wpi/first/networktables/Timestamped{{ t.TypeName }};
- */
-JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueue{{ t.TypeName }}
-  (JNIEnv* env, jclass, jint subentry)
-{
-  return MakeJObject(env, nt::ReadQueue{{ t.TypeName }}(subentry));
-}
-
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    readQueueValues{{ t.TypeName }}
- * Signature: (I)[{{ t.jni.jtypestr }}
- */
-JNIEXPORT {% if t.jni.JavaObject %}jobject{% else %}{{ t.jni.jtype }}{% endif %}Array JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValues{{ t.TypeName }}
-  (JNIEnv* env, jclass, jint subentry)
-{
-  return {{ t.jni.ToJavaArray }}(env, nt::ReadQueueValues{{ t.TypeName }}(subentry));
-}
-{% if t.TypeName == "Raw" %}
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    setRaw
- * Signature: (IJ[BII)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_setRaw
-  (JNIEnv* env, jclass, jint entry, jlong time, jbyteArray value, jint start, jint len)
-{
-  if (!value) {
-    nullPointerEx.Throw(env, "value is null");
-    return false;
-  }
-  if (start < 0) {
-    indexOobEx.Throw(env, "start must be >= 0");
-    return false;
-  }
-  if (len < 0) {
-    indexOobEx.Throw(env, "len must be >= 0");
-    return false;
-  }
-  CriticalJSpan<const jbyte> cvalue{env, value};
-  if (static_cast<unsigned int>(start + len) > cvalue.size()) {
-    indexOobEx.Throw(env, "start + len must be smaller than array length");
-    return false;
-  }
-  return nt::SetRaw(entry, cvalue.uarray().subspan(start, len), time);
-}
-
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    setRawBuffer
- * Signature: (IJLjava/nio/ByteBuffer;II)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_setRawBuffer
-  (JNIEnv* env, jclass, jint entry, jlong time, jobject value, jint start, jint len)
-{
-  if (!value) {
-    nullPointerEx.Throw(env, "value is null");
-    return false;
-  }
-  if (start < 0) {
-    indexOobEx.Throw(env, "start must be >= 0");
-    return false;
-  }
-  if (len < 0) {
-    indexOobEx.Throw(env, "len must be >= 0");
-    return false;
-  }
-  JSpan<const jbyte> cvalue{env, value, static_cast<size_t>(start + len)};
-  if (!cvalue) {
-    illegalArgEx.Throw(env, "value must be a native ByteBuffer");
-    return false;
-  }
-  return nt::SetRaw(entry, cvalue.uarray().subspan(start, len), time);
-}
-{% else %}
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    set{{ t.TypeName }}
- * Signature: (IJ{{ t.jni.jtypestr }})Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_set{{ t.TypeName }}
-  (JNIEnv*{% if t.jni.JavaObject %} env{% endif %}, jclass, jint entry, jlong time, {{ t.jni.jtype }} value)
-{
-{%- if t.jni.JavaObject %}
-  if (!value) {
-    nullPointerEx.Throw(env, "value cannot be null");
-    return false;
-  }
-{%- endif %}
-  return nt::Set{{ t.TypeName }}(entry, {{ t.jni.FromJavaBegin }}value{{ t.jni.FromJavaEnd }}, time);
-}
-{% endif %}
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    get{{ t.TypeName }}
- * Signature: (I{{ t.jni.jtypestr }}){{ t.jni.jtypestr }}
- */
-JNIEXPORT {{ t.jni.jtype }} JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_get{{ t.TypeName }}
-  (JNIEnv*{% if t.jni.JavaObject %} env{% endif %}, jclass, jint entry, {{ t.jni.jtype }} defaultValue)
-{
-{%- if t.jni.JavaObject %}
-  auto val = nt::GetEntryValue(entry);
-  if (!val || !val.Is{{ t.TypeName }}()) {
-    return defaultValue;
-  }
-  return {{ t.jni.ToJavaBegin }}val.Get{{ t.TypeName }}(){{ t.jni.ToJavaEnd }};
-{%- else %}
-  return nt::Get{{ t.TypeName }}(entry, defaultValue);
-{%- endif %}
-}
-{% if t.TypeName == "Raw" %}
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    setDefaultRaw
- * Signature: (IJ[BII)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultRaw
-  (JNIEnv* env, jclass, jint entry, jlong, jbyteArray defaultValue, jint start, jint len)
-{
-  if (!defaultValue) {
-    nullPointerEx.Throw(env, "value is null");
-    return false;
-  }
-  if (start < 0) {
-    indexOobEx.Throw(env, "start must be >= 0");
-    return false;
-  }
-  if (len < 0) {
-    indexOobEx.Throw(env, "len must be >= 0");
-    return false;
-  }
-  CriticalJSpan<const jbyte> cvalue{env, defaultValue};
-  if (static_cast<unsigned int>(start + len) > cvalue.size()) {
-    indexOobEx.Throw(env, "start + len must be smaller than array length");
-    return false;
-  }
-  return nt::SetDefaultRaw(entry, cvalue.uarray().subspan(start, len));
-}
-
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    setDefaultRawBuffer
- * Signature: (IJLjava/nio/ByteBuffer;II)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultRawBuffer
-  (JNIEnv* env, jclass, jint entry, jlong, jobject defaultValue, jint start, jint len)
-{
-  if (!defaultValue) {
-    nullPointerEx.Throw(env, "value is null");
-    return false;
-  }
-  if (start < 0) {
-    indexOobEx.Throw(env, "start must be >= 0");
-    return false;
-  }
-  if (len < 0) {
-    indexOobEx.Throw(env, "len must be >= 0");
-    return false;
-  }
-  JSpan<const jbyte> cvalue{env, defaultValue, static_cast<size_t>(start + len)};
-  if (!cvalue) {
-    illegalArgEx.Throw(env, "value must be a native ByteBuffer");
-    return false;
-  }
-  return nt::SetDefaultRaw(entry, cvalue.uarray().subspan(start, len));
-}
-{% else %}
-/*
- * Class:     edu_wpi_first_networktables_NetworkTablesJNI
- * Method:    setDefault{{ t.TypeName }}
- * Signature: (IJ{{ t.jni.jtypestr }})Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefault{{ t.TypeName }}
-  (JNIEnv*{% if t.jni.JavaObject %} env{% endif %}, jclass, jint entry, jlong, {{ t.jni.jtype }} defaultValue)
-{
-{%- if t.jni.JavaObject %}
-  if (!defaultValue) {
-    nullPointerEx.Throw(env, "defaultValue cannot be null");
-    return false;
-  }
-{%- endif %}
-  return nt::SetDefault{{ t.TypeName }}(entry, {{ t.jni.FromJavaBegin }}defaultValue{{ t.jni.FromJavaEnd }});
-}
-{% endif %}
-{% endfor %}
-}  // extern "C"
diff --git a/third_party/allwpilib/ntcore/src/generate/cpp/ntcore_c_types.cpp.jinja b/third_party/allwpilib/ntcore/src/generate/cpp/ntcore_c_types.cpp.jinja
deleted file mode 100644
index e74e5cf..0000000
--- a/third_party/allwpilib/ntcore/src/generate/cpp/ntcore_c_types.cpp.jinja
+++ /dev/null
@@ -1,106 +0,0 @@
-// 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 "ntcore_c_types.h"
-
-#include "Value_internal.h"
-#include "ntcore_cpp.h"
-
-using namespace nt;
-
-template <typename T>
-static inline std::span<const T> ConvertFromC(const T* arr, size_t size) {
-  return {arr, size};
-}
-
-static inline std::string_view ConvertFromC(const char* arr, size_t size) {
-  return {arr, size};
-}
-
-static std::vector<std::string> ConvertFromC(const NT_String* arr, size_t size) {
-  std::vector<std::string> v;
-  v.reserve(size);
-  for (size_t i = 0; i < size; ++i) {
-    v.emplace_back(ConvertFromC(arr[i]));
-  }
-  return v;
-}
-
-{% for t in types %}
-static void ConvertToC(const nt::Timestamped{{ t.TypeName }}& in, NT_Timestamped{{ t.TypeName }}* out) {
-  out->time = in.time;
-  out->serverTime = in.serverTime;
-{%- if t.c.IsArray %}
-  out->value = ConvertToC<{{ t.c.ValueType[:-1] }}>(in.value, &out->len);
-{% else %}
-  out->value = in.value;
-{% endif -%}
-}
-{% endfor %}
-
-extern "C" {
-{% for t in types %}
-NT_Bool NT_Set{{ t.TypeName }}(NT_Handle pubentry, int64_t time, {{ t.c.ParamType }} value{% if t.c.IsArray %}, size_t len{% endif %}) {
-{%- if t.c.IsArray %}
-  return nt::Set{{ t.TypeName }}(pubentry, ConvertFromC(value, len), time);
-{%- else %}
-  return nt::Set{{ t.TypeName }}(pubentry, value, time);
-{%- endif %}
-}
-
-NT_Bool NT_SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %}) {
-{%- if t.c.IsArray %}
-  return nt::SetDefault{{ t.TypeName }}(pubentry, ConvertFromC(defaultValue, defaultValueLen));
-{%- else %}
-  return nt::SetDefault{{ t.TypeName }}(pubentry, defaultValue);
-{%- endif %}
-}
-
-{{ t.c.ValueType }} NT_Get{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen, size_t* len{% endif %}) {
-{%- if t.c.IsArray %}
-  auto cppValue = nt::Get{{ t.TypeName }}(subentry, ConvertFromC(defaultValue, defaultValueLen));
-  return ConvertToC<{{ t.c.ValueType[:-1] }}>(cppValue, len);
-{%- else %}
-  return nt::Get{{ t.TypeName }}(subentry, defaultValue);
-{%- endif %}
-}
-
-void NT_GetAtomic{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %}, struct NT_Timestamped{{ t.TypeName }}* value) {
-{%- if t.c.IsArray %}
-  auto cppValue = nt::GetAtomic{{ t.TypeName }}(subentry, ConvertFromC(defaultValue, defaultValueLen));
-{%- else %}
-  auto cppValue = nt::GetAtomic{{ t.TypeName }}(subentry, defaultValue);
-{%- endif %}
-  ConvertToC(cppValue, value);
-}
-
-void NT_DisposeTimestamped{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* value) {
-{%- if t.TypeName == "StringArray" %}
-  NT_FreeStringArray(value->value, value->len);
-{%- elif t.c.IsArray %}
-  std::free(value->value);
-{%- endif %}
-}
-
-struct NT_Timestamped{{ t.TypeName }}* NT_ReadQueue{{ t.TypeName }}(NT_Handle subentry, size_t* len) {
-  auto arr = nt::ReadQueue{{ t.TypeName }}(subentry);
-  return ConvertToC<NT_Timestamped{{ t.TypeName }}>(arr, len);
-}
-
-void NT_FreeQueue{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* arr, size_t len) {
-  for (size_t i = 0; i < len; ++i) {
-    NT_DisposeTimestamped{{ t.TypeName }}(&arr[i]);
-  }
-  std::free(arr);
-}
-
-{%- if not t.c.IsArray %}
-{{ t.c.ValueType }}* NT_ReadQueueValues{{ t.TypeName }}(NT_Handle subentry, size_t* len) {
-  auto arr = nt::ReadQueueValues{{ t.TypeName }}(subentry);
-  return ConvertToC<{{ t.c.ValueType }}>(arr, len);
-}
-{%- endif %}
-
-{% endfor %}
-}  // extern "C"
diff --git a/third_party/allwpilib/ntcore/src/generate/cpp/ntcore_cpp_types.cpp.jinja b/third_party/allwpilib/ntcore/src/generate/cpp/ntcore_cpp_types.cpp.jinja
deleted file mode 100644
index cdaf27f..0000000
--- a/third_party/allwpilib/ntcore/src/generate/cpp/ntcore_cpp_types.cpp.jinja
+++ /dev/null
@@ -1,131 +0,0 @@
-// 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 "ntcore_cpp_types.h"
-
-#include "Handle.h"
-#include "InstanceImpl.h"
-
-namespace {
-template <nt::ValidType T>
-struct ValuesType {
-  using Vector =
-      std::vector<typename nt::TypeInfo<std::remove_cvref_t<T>>::Value>;
-};
-
-template <>
-struct ValuesType<bool> {
-  using Vector = std::vector<int>;
-};
-}  // namespace
-
-namespace nt {
-
-template <ValidType T>
-static inline bool Set(NT_Handle pubentry, typename TypeInfo<T>::View value,
-                       int64_t time) {
-  if (auto ii = InstanceImpl::Get(Handle{pubentry}.GetInst())) {
-    return ii->localStorage.SetEntryValue(
-        pubentry, MakeValue<T>(value, time == 0 ? Now() : time));
-  } else {
-    return {};
-  }
-}
-
-template <ValidType T>
-static inline bool SetDefault(NT_Handle pubentry,
-                              typename TypeInfo<T>::View defaultValue) {
-  if (auto ii = InstanceImpl::Get(Handle{pubentry}.GetInst())) {
-    return ii->localStorage.SetDefaultEntryValue(pubentry,
-                                                 MakeValue<T>(defaultValue, 1));
-  } else {
-    return {};
-  }
-}
-
-template <ValidType T>
-static inline Timestamped<typename TypeInfo<T>::Value> GetAtomic(
-    NT_Handle subentry, typename TypeInfo<T>::View defaultValue) {
-  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
-    return ii->localStorage.GetAtomic<T>(subentry, defaultValue);
-  } else {
-    return {};
-  }
-}
-
-template <ValidType T>
-inline Timestamped<typename TypeInfo<T>::SmallRet> GetAtomic(
-    NT_Handle subentry,
-    wpi::SmallVectorImpl<typename TypeInfo<T>::SmallElem>& buf,
-    typename TypeInfo<T>::View defaultValue) {
-  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
-    return ii->localStorage.GetAtomic<T>(subentry, buf, defaultValue);
-  } else {
-    return {};
-  }
-}
-
-template <typename T>
-static inline std::vector<Timestamped<typename TypeInfo<T>::Value>> ReadQueue(
-    NT_Handle subentry) {
-  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
-    return ii->localStorage.ReadQueue<T>(subentry);
-  } else {
-    return {};
-  }
-}
-
-template <typename T>
-static inline typename ValuesType<T>::Vector ReadQueueValues(
-    NT_Handle subentry) {
-  typename ValuesType<T>::Vector rv;
-  auto arr = ReadQueue<T>(subentry);
-  rv.reserve(arr.size());
-  for (auto&& elem : arr) {
-    rv.emplace_back(std::move(elem.value));
-  }
-  return rv;
-}
-{% for t in types %}
-bool Set{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} value, int64_t time) {
-  return Set<{{ t.cpp.TemplateType }}>(pubentry, value, time);
-}
-
-bool SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} defaultValue) {
-  return SetDefault<{{ t.cpp.TemplateType }}>(pubentry, defaultValue);
-}
-
-{{ t.cpp.ValueType }} Get{{ t.TypeName }}(NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue) {
-  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, defaultValue).value;
-}
-
-Timestamped{{ t.TypeName }} GetAtomic{{ t.TypeName }}(
-    NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue) {
-  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, defaultValue);
-}
-
-std::vector<Timestamped{{ t.TypeName }}> ReadQueue{{ t.TypeName }}(NT_Handle subentry) {
-  return ReadQueue<{{ t.cpp.TemplateType }}>(subentry);
-}
-
-std::vector<{% if t.cpp.ValueType == "bool" %}int{% else %}{{ t.cpp.ValueType }}{% endif %}> ReadQueueValues{{ t.TypeName }}(NT_Handle subentry) {
-  return ReadQueueValues<{{ t.cpp.TemplateType }}>(subentry);
-}
-{% if t.cpp.SmallRetType and t.cpp.SmallElemType %}
-{{ t.cpp.SmallRetType }} Get{{ t.TypeName }}(
-    NT_Handle subentry,
-    wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf,
-    {{ t.cpp.ParamType }} defaultValue) {
-  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, buf, defaultValue).value;
-}
-
-Timestamped{{ t.TypeName }}View GetAtomic{{ t.TypeName }}(
-    NT_Handle subentry,
-    wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf,
-    {{ t.cpp.ParamType }} defaultValue) {
-  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, buf, defaultValue);
-}
-{% endif %}
-{% endfor %}
-}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generate/include/networktables/Topic.h.jinja b/third_party/allwpilib/ntcore/src/generate/include/networktables/Topic.h.jinja
deleted file mode 100644
index ec2a915..0000000
--- a/third_party/allwpilib/ntcore/src/generate/include/networktables/Topic.h.jinja
+++ /dev/null
@@ -1,437 +0,0 @@
-// 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 <stdint.h>
-
-{{ cpp.INCLUDES }}
-#include <span>
-#include <string_view>
-#include <vector>
-
-#include <wpi/json_fwd.h>
-
-#include "networktables/Topic.h"
-
-namespace wpi {
-template <typename T>
-class SmallVectorImpl;
-}  // namespace wpi
-
-namespace nt {
-
-class {{ TypeName }}Topic;
-
-/**
- * NetworkTables {{ TypeName }} subscriber.
- */
-class {{ TypeName }}Subscriber : public Subscriber {
- public:
-  using TopicType = {{ TypeName }}Topic;
-  using ValueType = {{ cpp.ValueType }};
-  using ParamType = {{ cpp.ParamType }};
-  using TimestampedValueType = Timestamped{{ TypeName }};
-{% if cpp.SmallRetType and cpp.SmallElemType %}
-  using SmallRetType = {{ cpp.SmallRetType }};
-  using SmallElemType = {{ cpp.SmallElemType }};
-  using TimestampedValueViewType = Timestamped{{ TypeName }}View;
-{% endif %}
-
-  {{ TypeName }}Subscriber() = default;
-
-  /**
-   * Construct from a subscriber handle; recommended to use
-   * {{TypeName}}Topic::Subscribe() instead.
-   *
-   * @param handle Native handle
-   * @param defaultValue Default value
-   */
-  {{ TypeName }}Subscriber(NT_Subscriber handle, ParamType defaultValue);
-
-  /**
-   * Get the last published value.
-   * If no value has been published, returns the stored default value.
-   *
-   * @return value
-   */
-  ValueType Get() const;
-
-  /**
-   * Get the last published value.
-   * If no value has been published, returns the passed defaultValue.
-   *
-   * @param defaultValue default value to return if no value has been published
-   * @return value
-   */
-  ValueType Get(ParamType defaultValue) const;
-{% if cpp.SmallRetType and cpp.SmallElemType %}
-  /**
-   * Get the last published value.
-   * If no value has been published, returns the stored default value.
-   *
-   * @param buf storage for returned value
-   * @return value
-   */
-  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
-
-  /**
-   * Get the last published value.
-   * If no value has been published, returns the passed defaultValue.
-   *
-   * @param buf storage for returned value
-   * @param defaultValue default value to return if no value has been published
-   * @return value
-   */
-  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
-{% endif %}
-  /**
-   * Get the last published value along with its timestamp
-   * If no value has been published, returns the stored default value and a
-   * timestamp of 0.
-   *
-   * @return timestamped value
-   */
-  TimestampedValueType GetAtomic() const;
-
-  /**
-   * Get the last published value along with its timestamp.
-   * If no value has been published, returns the passed defaultValue and a
-   * timestamp of 0.
-   *
-   * @param defaultValue default value to return if no value has been published
-   * @return timestamped value
-   */
-  TimestampedValueType GetAtomic(ParamType defaultValue) const;
-{% if cpp.SmallRetType and cpp.SmallElemType %}
-  /**
-   * Get the last published value along with its timestamp.
-   * If no value has been published, returns the stored default value and a
-   * timestamp of 0.
-   *
-   * @param buf storage for returned value
-   * @return timestamped value
-   */
-  TimestampedValueViewType GetAtomic(
-      wpi::SmallVectorImpl<SmallElemType>& buf) const;
-
-  /**
-   * Get the last published value along with its timestamp.
-   * If no value has been published, returns the passed defaultValue and a
-   * timestamp of 0.
-   *
-   * @param buf storage for returned value
-   * @param defaultValue default value to return if no value has been published
-   * @return timestamped value
-   */
-  TimestampedValueViewType GetAtomic(
-      wpi::SmallVectorImpl<SmallElemType>& buf,
-      ParamType defaultValue) const;
-{% endif %}
-  /**
-   * Get an array of all value changes since the last call to ReadQueue.
-   * Also provides a timestamp for each value.
-   *
-   * @note The "poll storage" subscribe option can be used to set the queue
-   *     depth.
-   *
-   * @return Array of timestamped values; empty array if no new changes have
-   *     been published since the previous call.
-   */
-  std::vector<TimestampedValueType> ReadQueue();
-
-  /**
-   * Get the corresponding topic.
-   *
-   * @return Topic
-   */
-  TopicType GetTopic() const;
-
- private:
-  ValueType m_defaultValue;
-};
-
-/**
- * NetworkTables {{ TypeName }} publisher.
- */
-class {{ TypeName }}Publisher : public Publisher {
- public:
-  using TopicType = {{ TypeName }}Topic;
-  using ValueType = {{ cpp.ValueType }};
-  using ParamType = {{ cpp.ParamType }};
-{% if cpp.SmallRetType and cpp.SmallElemType %}
-  using SmallRetType = {{ cpp.SmallRetType }};
-  using SmallElemType = {{ cpp.SmallElemType }};
-{% endif %}
-  using TimestampedValueType = Timestamped{{ TypeName }};
-
-  {{ TypeName }}Publisher() = default;
-
-  /**
-   * Construct from a publisher handle; recommended to use
-   * {{TypeName}}Topic::Publish() instead.
-   *
-   * @param handle Native handle
-   */
-  explicit {{ TypeName }}Publisher(NT_Publisher handle);
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param time timestamp; 0 indicates current NT time should be used
-   */
-  void Set(ParamType value, int64_t time = 0);
-
-  /**
-   * Publish a default value.
-   * On reconnect, a default value will never be used in preference to a
-   * published value.
-   *
-   * @param value value
-   */
-  void SetDefault(ParamType value);
-
-  /**
-   * Get the corresponding topic.
-   *
-   * @return Topic
-   */
-  TopicType GetTopic() const;
-};
-
-/**
- * NetworkTables {{ TypeName }} entry.
- *
- * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
- */
-class {{ TypeName }}Entry final : public {{ TypeName }}Subscriber,
-                                  public {{ TypeName }}Publisher {
- public:
-  using SubscriberType = {{ TypeName }}Subscriber;
-  using PublisherType = {{ TypeName }}Publisher;
-  using TopicType = {{ TypeName }}Topic;
-  using ValueType = {{ cpp.ValueType }};
-  using ParamType = {{ cpp.ParamType }};
-{% if cpp.SmallRetType and cpp.SmallElemType %}
-  using SmallRetType = {{ cpp.SmallRetType }};
-  using SmallElemType = {{ cpp.SmallElemType }};
-{% endif %}
-  using TimestampedValueType = Timestamped{{ TypeName }};
-
-  {{ TypeName }}Entry() = default;
-
-  /**
-   * Construct from an entry handle; recommended to use
-   * {{TypeName}}Topic::GetEntry() instead.
-   *
-   * @param handle Native handle
-   * @param defaultValue Default value
-   */
-  {{ TypeName }}Entry(NT_Entry handle, ParamType defaultValue);
-
-  /**
-   * Determines if the native handle is valid.
-   *
-   * @return True if the native handle is valid, false otherwise.
-   */
-  explicit operator bool() const { return m_subHandle != 0; }
-
-  /**
-   * Gets the native handle for the entry.
-   *
-   * @return Native handle
-   */
-  NT_Entry GetHandle() const { return m_subHandle; }
-
-  /**
-   * Get the corresponding topic.
-   *
-   * @return Topic
-   */
-  TopicType GetTopic() const;
-
-  /**
-   * Stops publishing the entry if it's published.
-   */
-  void Unpublish();
-};
-
-/**
- * NetworkTables {{ TypeName }} topic.
- */
-class {{ TypeName }}Topic final : public Topic {
- public:
-  using SubscriberType = {{ TypeName }}Subscriber;
-  using PublisherType = {{ TypeName }}Publisher;
-  using EntryType = {{ TypeName }}Entry;
-  using ValueType = {{ cpp.ValueType }};
-  using ParamType = {{ cpp.ParamType }};
-  using TimestampedValueType = Timestamped{{ TypeName }};
-{%- if TypeString %}
-  /** The default type string for this topic type. */
-  static constexpr std::string_view kTypeString = {{ TypeString }};
-{%- endif %}
-
-  {{ TypeName }}Topic() = default;
-
-  /**
-   * Construct from a topic handle; recommended to use
-   * NetworkTableInstance::Get{{TypeName}}Topic() instead.
-   *
-   * @param handle Native handle
-   */
-  explicit {{ TypeName }}Topic(NT_Topic handle) : Topic{handle} {}
-
-  /**
-   * Construct from a generic topic.
-   *
-   * @param topic Topic
-   */
-  explicit {{ TypeName }}Topic(Topic topic) : Topic{topic} {}
-
-  /**
-   * Create a new subscriber to the topic.
-   *
-   * <p>The subscriber is only active as long as the returned object
-   * is not destroyed.
-   *
-   * @note Subscribers that do not match the published data type do not return
-   *     any values. To determine if the data type matches, use the appropriate
-   *     Topic functions.
-   *
-{%- if not TypeString %}
-   * @param typeString type string
-{% endif %}
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options subscribe options
-   * @return subscriber
-   */
-  [[nodiscard]]
-  SubscriberType Subscribe(
-      {% if not TypeString %}std::string_view typeString, {% endif %}ParamType defaultValue,
-      const PubSubOptions& options = kDefaultPubSubOptions);
-{%- if TypeString %}
-  /**
-   * Create a new subscriber to the topic, with specific type string.
-   *
-   * <p>The subscriber is only active as long as the returned object
-   * is not destroyed.
-   *
-   * @note Subscribers that do not match the published data type do not return
-   *     any values. To determine if the data type matches, use the appropriate
-   *     Topic functions.
-   *
-   * @param typeString type string
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options subscribe options
-   * @return subscriber
-   */
-  [[nodiscard]]
-  SubscriberType SubscribeEx(
-      std::string_view typeString, ParamType defaultValue,
-      const PubSubOptions& options = kDefaultPubSubOptions);
-{% endif %}
-  /**
-   * Create a new publisher to the topic.
-   *
-   * The publisher is only active as long as the returned object
-   * is not destroyed.
-   *
-   * @note It is not possible to publish two different data types to the same
-   *     topic. Conflicts between publishers are typically resolved by the
-   *     server on a first-come, first-served basis. Any published values that
-   *     do not match the topic's data type are dropped (ignored). To determine
-   *     if the data type matches, use the appropriate Topic functions.
-   *
-{%- if not TypeString %}
-   * @param typeString type string
-{% endif %}
-   * @param options publish options
-   * @return publisher
-   */
-  [[nodiscard]]
-  PublisherType Publish({% if not TypeString %}std::string_view typeString, {% endif %}const PubSubOptions& options = kDefaultPubSubOptions);
-
-  /**
-   * Create a new publisher to the topic, with type string and initial
-   * properties.
-   *
-   * The publisher is only active as long as the returned object
-   * is not destroyed.
-   *
-   * @note It is not possible to publish two different data types to the same
-   *     topic. Conflicts between publishers are typically resolved by the
-   *     server on a first-come, first-served basis. Any published values that
-   *     do not match the topic's data type are dropped (ignored). To determine
-   *     if the data type matches, use the appropriate Topic functions.
-   *
-   * @param typeString type string
-   * @param properties JSON properties
-   * @param options publish options
-   * @return publisher
-   */
-  [[nodiscard]]
-  PublisherType PublishEx(std::string_view typeString,
-    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
-
-  /**
-   * Create a new entry for the topic.
-   *
-   * Entries act as a combination of a subscriber and a weak publisher. The
-   * subscriber is active as long as the entry is not destroyed. The publisher
-   * is created when the entry is first written to, and remains active until
-   * either Unpublish() is called or the entry is destroyed.
-   *
-   * @note It is not possible to use two different data types with the same
-   *     topic. Conflicts between publishers are typically resolved by the
-   *     server on a first-come, first-served basis. Any published values that
-   *     do not match the topic's data type are dropped (ignored), and the entry
-   *     will show no new values if the data type does not match. To determine
-   *     if the data type matches, use the appropriate Topic functions.
-   *
-{%- if not TypeString %}
-   * @param typeString type string
-{% endif %}
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options publish and/or subscribe options
-   * @return entry
-   */
-  [[nodiscard]]
-  EntryType GetEntry({% if not TypeString %}std::string_view typeString, {% endif %}ParamType defaultValue,
-                     const PubSubOptions& options = kDefaultPubSubOptions);
-{%- if TypeString %}
-  /**
-   * Create a new entry for the topic, with specific type string.
-   *
-   * Entries act as a combination of a subscriber and a weak publisher. The
-   * subscriber is active as long as the entry is not destroyed. The publisher
-   * is created when the entry is first written to, and remains active until
-   * either Unpublish() is called or the entry is destroyed.
-   *
-   * @note It is not possible to use two different data types with the same
-   *     topic. Conflicts between publishers are typically resolved by the
-   *     server on a first-come, first-served basis. Any published values that
-   *     do not match the topic's data type are dropped (ignored), and the entry
-   *     will show no new values if the data type does not match. To determine
-   *     if the data type matches, use the appropriate Topic functions.
-   *
-   * @param typeString type string
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options publish and/or subscribe options
-   * @return entry
-   */
-  [[nodiscard]]
-  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
-                       const PubSubOptions& options = kDefaultPubSubOptions);
-{% endif %}
-};
-
-}  // namespace nt
-
-#include "networktables/{{ TypeName }}Topic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generate/include/networktables/Topic.inc.jinja b/third_party/allwpilib/ntcore/src/generate/include/networktables/Topic.inc.jinja
deleted file mode 100644
index 4e7a167..0000000
--- a/third_party/allwpilib/ntcore/src/generate/include/networktables/Topic.inc.jinja
+++ /dev/null
@@ -1,135 +0,0 @@
-// 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 "networktables/{{ TypeName }}Topic.h"
-#include "networktables/NetworkTableType.h"
-#include "ntcore_cpp.h"
-
-namespace nt {
-
-inline {{ TypeName }}Subscriber::{{ TypeName }}Subscriber(
-    NT_Subscriber handle, {{ cpp.ParamType }} defaultValue)
-    : Subscriber{handle},
-      m_defaultValue{{ '{' }}{{ cpp.DefaultValueCopy|default('defaultValue') }}} {}
-
-inline {{ cpp.ValueType }} {{ TypeName }}Subscriber::Get() const {
-  return Get(m_defaultValue);
-}
-
-inline {{ cpp.ValueType }} {{ TypeName }}Subscriber::Get(
-    {{ cpp.ParamType }} defaultValue) const {
-  return ::nt::Get{{ TypeName }}(m_subHandle, defaultValue);
-}
-{% if cpp.SmallRetType and cpp.SmallElemType %}
-inline {{ cpp.SmallRetType }} {{ TypeName }}Subscriber::Get(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf) const {
-  return Get(buf, m_defaultValue);
-}
-
-inline {{ cpp.SmallRetType }} {{ TypeName }}Subscriber::Get(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf, {{ cpp.ParamType }} defaultValue) const {
-  return nt::Get{{ TypeName }}(m_subHandle, buf, defaultValue);
-}
-{% endif %}
-inline Timestamped{{ TypeName }} {{ TypeName }}Subscriber::GetAtomic() const {
-  return GetAtomic(m_defaultValue);
-}
-
-inline Timestamped{{ TypeName }} {{ TypeName }}Subscriber::GetAtomic(
-    {{ cpp.ParamType }} defaultValue) const {
-  return ::nt::GetAtomic{{ TypeName }}(m_subHandle, defaultValue);
-}
-{% if cpp.SmallRetType and cpp.SmallElemType %}
-inline Timestamped{{ TypeName }}View {{ TypeName }}Subscriber::GetAtomic(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf) const {
-  return GetAtomic(buf, m_defaultValue);
-}
-
-inline Timestamped{{ TypeName }}View {{ TypeName }}Subscriber::GetAtomic(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf, {{ cpp.ParamType }} defaultValue) const {
-  return nt::GetAtomic{{ TypeName }}(m_subHandle, buf, defaultValue);
-}
-{% endif %}
-inline std::vector<Timestamped{{ TypeName }}>
-{{ TypeName }}Subscriber::ReadQueue() {
-  return ::nt::ReadQueue{{ TypeName }}(m_subHandle);
-}
-
-inline {{ TypeName }}Topic {{ TypeName }}Subscriber::GetTopic() const {
-  return {{ TypeName }}Topic{::nt::GetTopicFromHandle(m_subHandle)};
-}
-
-inline {{ TypeName }}Publisher::{{ TypeName }}Publisher(NT_Publisher handle)
-    : Publisher{handle} {}
-
-inline void {{ TypeName }}Publisher::Set({{ cpp.ParamType }} value,
-                                         int64_t time) {
-  ::nt::Set{{ TypeName }}(m_pubHandle, value, time);
-}
-
-inline void {{ TypeName }}Publisher::SetDefault({{ cpp.ParamType }} value) {
-  ::nt::SetDefault{{ TypeName }}(m_pubHandle, value);
-}
-
-inline {{ TypeName }}Topic {{ TypeName }}Publisher::GetTopic() const {
-  return {{ TypeName }}Topic{::nt::GetTopicFromHandle(m_pubHandle)};
-}
-
-inline {{ TypeName }}Entry::{{ TypeName }}Entry(
-    NT_Entry handle, {{ cpp.ParamType }} defaultValue)
-    : {{ TypeName }}Subscriber{handle, defaultValue},
-      {{ TypeName }}Publisher{handle} {}
-
-inline {{ TypeName }}Topic {{ TypeName }}Entry::GetTopic() const {
-  return {{ TypeName }}Topic{::nt::GetTopicFromHandle(m_subHandle)};
-}
-
-inline void {{ TypeName }}Entry::Unpublish() {
-  ::nt::Unpublish(m_pubHandle);
-}
-
-inline {{ TypeName }}Subscriber {{ TypeName }}Topic::Subscribe(
-    {% if not TypeString %}std::string_view typeString, {% endif %}{{ cpp.ParamType }} defaultValue,
-    const PubSubOptions& options) {
-  return {{ TypeName }}Subscriber{
-      ::nt::Subscribe(m_handle, NT_{{ cpp.TYPE_NAME }}, {{ TypeString|default('typeString') }}, options),
-      defaultValue};
-}
-{%- if TypeString %}
-inline {{ TypeName }}Subscriber {{ TypeName }}Topic::SubscribeEx(
-    std::string_view typeString, {{ cpp.ParamType }} defaultValue,
-    const PubSubOptions& options) {
-  return {{ TypeName }}Subscriber{
-      ::nt::Subscribe(m_handle, NT_{{ cpp.TYPE_NAME }}, typeString, options),
-      defaultValue};
-}
-{% endif %}
-inline {{ TypeName }}Publisher {{ TypeName }}Topic::Publish(
-    {% if not TypeString %}std::string_view typeString, {% endif %}const PubSubOptions& options) {
-  return {{ TypeName }}Publisher{
-      ::nt::Publish(m_handle, NT_{{ cpp.TYPE_NAME }}, {{ TypeString|default('typeString') }}, options)};
-}
-
-inline {{ TypeName }}Publisher {{ TypeName }}Topic::PublishEx(
-    std::string_view typeString,
-    const wpi::json& properties, const PubSubOptions& options) {
-  return {{ TypeName }}Publisher{
-      ::nt::PublishEx(m_handle, NT_{{ cpp.TYPE_NAME }}, typeString, properties, options)};
-}
-
-inline {{ TypeName }}Entry {{ TypeName }}Topic::GetEntry(
-    {% if not TypeString %}std::string_view typeString, {% endif %}{{ cpp.ParamType }} defaultValue,
-    const PubSubOptions& options) {
-  return {{ TypeName }}Entry{
-      ::nt::GetEntry(m_handle, NT_{{ cpp.TYPE_NAME }}, {{ TypeString|default('typeString') }}, options),
-      defaultValue};
-}
-{%- if TypeString %}
-inline {{ TypeName }}Entry {{ TypeName }}Topic::GetEntryEx(
-    std::string_view typeString, {{ cpp.ParamType }} defaultValue,
-    const PubSubOptions& options) {
-  return {{ TypeName }}Entry{
-      ::nt::GetEntry(m_handle, NT_{{ cpp.TYPE_NAME }}, typeString, options),
-      defaultValue};
-}
-{% endif %}
-}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generate/include/ntcore_c_types.h.jinja b/third_party/allwpilib/ntcore/src/generate/include/ntcore_c_types.h.jinja
deleted file mode 100644
index 83cc807..0000000
--- a/third_party/allwpilib/ntcore/src/generate/include/ntcore_c_types.h.jinja
+++ /dev/null
@@ -1,151 +0,0 @@
-// 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 <stdint.h>
-
-#include "ntcore_c.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% for t in types %}
-/**
- * Timestamped {{ t.TypeName }}.
- * @ingroup ntcore_c_api
- */
-struct NT_Timestamped{{ t.TypeName }} {
-  /**
-   * Time in local time base.
-   */
-  int64_t time;
-
-  /**
-   * Time in server time base.  May be 0 or 1 for locally set values.
-   */
-  int64_t serverTime;
-
-  /**
-   * Value.
-   */
-  {{ t.c.ValueType }} value;
-{%- if t.c.IsArray %}
-  /**
-   * Value length.
-   */
-  size_t len;
-{% endif %}
-};
-
-/**
- * @defgroup ntcore_{{ t.TypeName }}_cfunc {{ t.TypeName }} Functions
- * @ingroup ntcore_c_api
- * @{
- */
-
-/**
- * Publish a new value.
- *
- * @param pubentry publisher or entry handle
- * @param time timestamp; 0 indicates current NT time should be used
- * @param value value to publish
-{%- if t.c.IsArray %}
- * @param len length of value
-{% endif %}
- */
-NT_Bool NT_Set{{ t.TypeName }}(NT_Handle pubentry, int64_t time, {{ t.c.ParamType }} value{% if t.c.IsArray %}, size_t len{% endif %});
-
-/**
- * Publish a default value.
- * On reconnect, a default value will never be used in preference to a
- * published value.
- *
- * @param pubentry publisher or entry handle
- * @param defaultValue default value
-{%- if t.c.IsArray %}
- * @param defaultValueLen length of default value
-{% endif %}
- */
-NT_Bool NT_SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %});
-
-/**
- * Get the last published value.
- * If no value has been published, returns the passed defaultValue.
- *
- * @param subentry subscriber or entry handle
- * @param defaultValue default value to return if no value has been published
-{%- if t.c.IsArray %}
- * @param defaultValueLen length of default value
- * @param len length of returned value (output)
-{% endif %}
- * @return value
- */
-{{ t.c.ValueType }} NT_Get{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen, size_t* len{% endif %});
-
-/**
- * Get the last published value along with its timestamp.
- * If no value has been published, returns the passed defaultValue and a
- * timestamp of 0.
- *
- * @param subentry subscriber or entry handle
- * @param defaultValue default value to return if no value has been published
-{%- if t.c.IsArray %}
- * @param defaultValueLen length of default value
-{% endif %}
- * @param value timestamped value (output)
- */
-void NT_GetAtomic{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %}, struct NT_Timestamped{{ t.TypeName }}* value);
-
-/**
- * Disposes a timestamped value (as returned by NT_GetAtomic{{ t.TypeName }}).
- *
- * @param value timestamped value
- */
-void NT_DisposeTimestamped{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* value);
-
-/**
- * Get an array of all value changes since the last call to ReadQueue.
- * Also provides a timestamp for each value.
- *
- * @note The "poll storage" subscribe option can be used to set the queue
- *     depth.
- *
- * @param subentry subscriber or entry handle
- * @param len length of returned array (output)
- * @return Array of timestamped values; NULL if no new changes have
- *     been published since the previous call.
- */
-struct NT_Timestamped{{ t.TypeName }}* NT_ReadQueue{{ t.TypeName }}(NT_Handle subentry, size_t* len);
-
-/**
- * Frees a timestamped array of values (as returned by NT_ReadQueue{{ t.TypeName }}).
- *
- * @param arr array
- * @param len length of array
- */
-void NT_FreeQueue{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* arr, size_t len);
-
-{%- if not t.c.IsArray %}
-/**
- * Get an array of all value changes since the last call to ReadQueue.
- *
- * @note The "poll storage" subscribe option can be used to set the queue
- *     depth.
- *
- * @param subentry subscriber or entry handle
- * @param len length of returned array (output)
- * @return Array of values; NULL if no new changes have
- *     been published since the previous call.
- */
-{{ t.c.ValueType }}* NT_ReadQueueValues{{ t.TypeName }}(NT_Handle subentry, size_t* len);
-{%- endif %}
-
-/** @} */
-{% endfor %}
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif
diff --git a/third_party/allwpilib/ntcore/src/generate/include/ntcore_cpp_types.h.jinja b/third_party/allwpilib/ntcore/src/generate/include/ntcore_cpp_types.h.jinja
deleted file mode 100644
index df919de..0000000
--- a/third_party/allwpilib/ntcore/src/generate/include/ntcore_cpp_types.h.jinja
+++ /dev/null
@@ -1,141 +0,0 @@
-// 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 <stdint.h>
-
-#include <span>
-#include <string>
-#include <string_view>
-#include <utility>
-#include <vector>
-
-#include "ntcore_c.h"
-
-namespace wpi {
-template <typename T>
-class SmallVectorImpl;
-}  // namespace wpi
-
-namespace nt {
-/**
- * Timestamped value.
- * @ingroup ntcore_cpp_handle_api
- */
-template <typename T>
-struct Timestamped {
-  Timestamped() = default;
-  Timestamped(int64_t time, int64_t serverTime, T value)
-    : time{time}, serverTime{serverTime}, value{std::move(value)} {}
-
-  /**
-   * Time in local time base.
-   */
-  int64_t time = 0;
-
-  /**
-   * Time in server time base.  May be 0 or 1 for locally set values.
-   */
-  int64_t serverTime = 0;
-
-  /**
-   * Value.
-   */
-  T value = {};
-};
-{% for t in types %}
-/**
- * Timestamped {{ t.TypeName }}.
- * @ingroup ntcore_cpp_handle_api
- */
-using Timestamped{{ t.TypeName }} = Timestamped<{{ t.cpp.ValueType }}>;
-{% if t.cpp.SmallRetType %}
-/**
- * Timestamped {{ t.TypeName }} view (for SmallVector-taking functions).
- * @ingroup ntcore_cpp_handle_api
- */
-using Timestamped{{ t.TypeName }}View = Timestamped<{{ t.cpp.SmallRetType }}>;
-{% endif %}
-/**
- * @defgroup ntcore_{{ t.TypeName }}_func {{ t.TypeName }} Functions
- * @ingroup ntcore_cpp_handle_api
- * @{
- */
-
-/**
- * Publish a new value.
- *
- * @param pubentry publisher or entry handle
- * @param value value to publish
- * @param time timestamp; 0 indicates current NT time should be used
- */
-bool Set{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} value, int64_t time = 0);
-
-/**
- * Publish a default value.
- * On reconnect, a default value will never be used in preference to a
- * published value.
- *
- * @param pubentry publisher or entry handle
- * @param defaultValue default value
- */
-bool SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} defaultValue);
-
-/**
- * Get the last published value.
- * If no value has been published, returns the passed defaultValue.
- *
- * @param subentry subscriber or entry handle
- * @param defaultValue default value to return if no value has been published
- * @return value
- */
-{{ t.cpp.ValueType }} Get{{ t.TypeName }}(NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue);
-
-/**
- * Get the last published value along with its timestamp.
- * If no value has been published, returns the passed defaultValue and a
- * timestamp of 0.
- *
- * @param subentry subscriber or entry handle
- * @param defaultValue default value to return if no value has been published
- * @return timestamped value
- */
-Timestamped{{ t.TypeName }} GetAtomic{{ t.TypeName}}(NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue);
-
-/**
- * Get an array of all value changes since the last call to ReadQueue.
- * Also provides a timestamp for each value.
- *
- * @note The "poll storage" subscribe option can be used to set the queue
- *     depth.
- *
- * @param subentry subscriber or entry handle
- * @return Array of timestamped values; empty array if no new changes have
- *     been published since the previous call.
- */
-std::vector<Timestamped{{ t.TypeName }}> ReadQueue{{ t.TypeName }}(NT_Handle subentry);
-
-/**
- * Get an array of all value changes since the last call to ReadQueue.
- *
- * @note The "poll storage" subscribe option can be used to set the queue
- *     depth.
- *
- * @param subentry subscriber or entry handle
- * @return Array of values; empty array if no new changes have
- *     been published since the previous call.
- */
-std::vector<{% if t.cpp.ValueType == "bool" %}int{% else %}{{ t.cpp.ValueType }}{% endif %}> ReadQueueValues{{ t.TypeName }}(NT_Handle subentry);
-{% if t.cpp.SmallRetType and t.cpp.SmallElemType %}
-{{ t.cpp.SmallRetType }} Get{{ t.TypeName }}(NT_Handle subentry, wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf, {{ t.cpp.ParamType }} defaultValue);
-
-Timestamped{{ t.TypeName }}View GetAtomic{{ t.TypeName }}(
-      NT_Handle subentry,
-      wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf,
-      {{ t.cpp.ParamType }} defaultValue);
-{% endif %}
-/** @} */
-{% endfor %}
-}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generate/java/Entry.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/Entry.java.jinja
deleted file mode 100644
index cbaa782..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/Entry.java.jinja
+++ /dev/null
@@ -1,15 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-/**
- * NetworkTables {{ TypeName }} entry.
- *
- * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
- */
-public interface {{ TypeName }}Entry extends {{ TypeName }}Subscriber, {{ TypeName }}Publisher {
-  /** Stops publishing the entry if it's published. */
-  void unpublish();
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/EntryImpl.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/EntryImpl.java.jinja
deleted file mode 100644
index b7432a7..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/EntryImpl.java.jinja
+++ /dev/null
@@ -1,97 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-{% if TypeName == "Raw" %}
-import java.nio.ByteBuffer;
-{% endif %}
-/** NetworkTables {{ TypeName }} implementation. */
-@SuppressWarnings("PMD.ArrayIsStoredDirectly")
-final class {{ TypeName }}EntryImpl extends EntryBase implements {{ TypeName }}Entry {
-  /**
-   * Constructor.
-   *
-   * @param topic Topic
-   * @param handle Native handle
-   * @param defaultValue Default value for get()
-   */
-  {{ TypeName }}EntryImpl({{ TypeName }}Topic topic, int handle, {{ java.ValueType }} defaultValue) {
-    super(handle);
-    m_topic = topic;
-    m_defaultValue = defaultValue;
-  }
-
-  @Override
-  public {{ TypeName }}Topic getTopic() {
-    return m_topic;
-  }
-
-  @Override
-  public {{ java.ValueType }} get() {
-    return NetworkTablesJNI.get{{ TypeName }}(m_handle, m_defaultValue);
-  }
-
-  @Override
-  public {{ java.ValueType }} get({{ java.ValueType }} defaultValue) {
-    return NetworkTablesJNI.get{{TypeName}}(m_handle, defaultValue);
-  }
-
-  @Override
-  public Timestamped{{ TypeName }} getAtomic() {
-    return NetworkTablesJNI.getAtomic{{ TypeName }}(m_handle, m_defaultValue);
-  }
-
-  @Override
-  public Timestamped{{ TypeName }} getAtomic({{ java.ValueType }} defaultValue) {
-    return NetworkTablesJNI.getAtomic{{ TypeName }}(m_handle, defaultValue);
-  }
-
-  @Override
-  public Timestamped{{ TypeName }}[] readQueue() {
-    return NetworkTablesJNI.readQueue{{ TypeName }}(m_handle);
-  }
-
-  @Override
-  public {{ java.ValueType }}[] readQueueValues() {
-    return NetworkTablesJNI.readQueueValues{{ TypeName }}(m_handle);
-  }
-{% if TypeName == "Raw" %}
-  @Override
-  public void set(byte[] value, int start, int len, long time) {
-    NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
-  }
-
-  @Override
-  public void set(ByteBuffer value, int start, int len, long time) {
-    NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
-  }
-
-  @Override
-  public void setDefault(byte[] value, int start, int len) {
-    NetworkTablesJNI.setDefaultRaw(m_handle, 0, value, start, len);
-  }
-
-  @Override
-  public void setDefault(ByteBuffer value, int start, int len) {
-    NetworkTablesJNI.setDefaultRaw(m_handle, 0, value, start, len);
-  }
-{% else %}
-  @Override
-  public void set({{ java.ValueType }} value, long time) {
-    NetworkTablesJNI.set{{ TypeName }}(m_handle, time, value);
-  }
-
-  @Override
-  public void setDefault({{ java.ValueType }} value) {
-    NetworkTablesJNI.setDefault{{ TypeName }}(m_handle, 0, value);
-  }
-{% endif %}
-  @Override
-  public void unpublish() {
-    NetworkTablesJNI.unpublish(m_handle);
-  }
-
-  private final {{ TypeName }}Topic m_topic;
-  private final {{ java.ValueType }} m_defaultValue;
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/GenericEntryImpl.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/GenericEntryImpl.java.jinja
deleted file mode 100644
index 29666bb..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/GenericEntryImpl.java.jinja
+++ /dev/null
@@ -1,375 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import java.nio.ByteBuffer;
-
-/** NetworkTables generic implementation. */
-final class GenericEntryImpl extends EntryBase implements GenericEntry {
-  /**
-   * Constructor.
-   *
-   * @param topic Topic
-   * @param handle Native handle
-   */
-  GenericEntryImpl(Topic topic, int handle) {
-    super(handle);
-    m_topic = topic;
-  }
-
-  @Override
-  public Topic getTopic() {
-    return m_topic;
-  }
-
-  @Override
-  public NetworkTableValue get() {
-    return NetworkTablesJNI.getValue(m_handle);
-  }
-{% for t in types %}
-  /**
-   * Gets the entry's value as a {{ t.java.ValueType }}. If the entry does not exist or is of different type, it
-   * will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  @Override
-  public {{ t.java.ValueType }} get{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
-    return NetworkTablesJNI.get{{ t.TypeName }}(m_handle, defaultValue);
-  }
-{% if t.java.WrapValueType %}
-  /**
-   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
-   * it will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  @Override
-  public {{ t.java.WrapValueType }} get{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
-    return NetworkTableValue.fromNative{{ t.TypeName }}(
-        get{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue)));
-  }
-{% endif -%}
-{% endfor %}
-  @Override
-  public NetworkTableValue[] readQueue() {
-    return NetworkTablesJNI.readQueueValue(m_handle);
-  }
-
-  @Override
-  public boolean set(NetworkTableValue value) {
-    long time = value.getTime();
-    Object otherValue = value.getValue();
-    switch (value.getType()) {
-      case kBoolean:
-        return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue);
-      case kInteger:
-        return NetworkTablesJNI.setInteger(
-            m_handle, time, ((Number) otherValue).longValue());
-      case kFloat:
-        return NetworkTablesJNI.setFloat(
-            m_handle, time, ((Number) otherValue).floatValue());
-      case kDouble:
-        return NetworkTablesJNI.setDouble(
-            m_handle, time, ((Number) otherValue).doubleValue());
-      case kString:
-        return NetworkTablesJNI.setString(m_handle, time, (String) otherValue);
-      case kRaw:
-        return NetworkTablesJNI.setRaw(m_handle, time, (byte[]) otherValue);
-      case kBooleanArray:
-        return NetworkTablesJNI.setBooleanArray(m_handle, time, (boolean[]) otherValue);
-      case kIntegerArray:
-        return NetworkTablesJNI.setIntegerArray(m_handle, time, (long[]) otherValue);
-      case kFloatArray:
-        return NetworkTablesJNI.setFloatArray(m_handle, time, (float[]) otherValue);
-      case kDoubleArray:
-        return NetworkTablesJNI.setDoubleArray(m_handle, time, (double[]) otherValue);
-      case kStringArray:
-        return NetworkTablesJNI.setStringArray(m_handle, time, (String[]) otherValue);
-      default:
-        return true;
-    }
-  }
-
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   * @throws IllegalArgumentException if the value is not a known type
-   */
-  @Override
-  public boolean setValue(Object value, long time) {
-    if (value instanceof NetworkTableValue) {
-      return set((NetworkTableValue) value);
-    } else if (value instanceof Boolean) {
-      return setBoolean((Boolean) value, time);
-    } else if (value instanceof Long) {
-      return setInteger((Long) value, time);
-    } else if (value instanceof Float) {
-      return setFloat((Float) value, time);
-    } else if (value instanceof Number) {
-      return setNumber((Number) value, time);
-    } else if (value instanceof String) {
-      return setString((String) value, time);
-    } else if (value instanceof byte[]) {
-      return setRaw((byte[]) value, time);
-    } else if (value instanceof boolean[]) {
-      return setBooleanArray((boolean[]) value, time);
-    } else if (value instanceof long[]) {
-      return setIntegerArray((long[]) value, time);
-    } else if (value instanceof float[]) {
-      return setFloatArray((float[]) value, time);
-    } else if (value instanceof double[]) {
-      return setDoubleArray((double[]) value, time);
-    } else if (value instanceof Boolean[]) {
-      return setBooleanArray((Boolean[]) value, time);
-    } else if (value instanceof Long[]) {
-      return setIntegerArray((Long[]) value, time);
-    } else if (value instanceof Float[]) {
-      return setFloatArray((Float[]) value, time);
-    } else if (value instanceof Number[]) {
-      return setNumberArray((Number[]) value, time);
-    } else if (value instanceof String[]) {
-      return setStringArray((String[]) value, time);
-    } else {
-      throw new IllegalArgumentException(
-          "Value of type " + value.getClass().getName() + " cannot be put into a table");
-    }
-  }
-{% for t in types %}
-{% if t.TypeName == "Raw" %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean setRaw(byte[] value, int start, int len, long time) {
-    return NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
-  }
-
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean setRaw(ByteBuffer value, int start, int len, long time) {
-    return NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
-  }
-{% else %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean set{{ t.TypeName }}({{ t.java.ValueType }} value, long time) {
-    return NetworkTablesJNI.set{{ t.TypeName }}(m_handle, time, value);
-  }
-{% endif -%}
-{% if t.java.WrapValueType %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value, long time) {
-    return set{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(value), time);
-  }
-{% endif -%}
-{% endfor %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setNumber(Number value, long time) {
-    return setDouble(value.doubleValue(), time);
-  }
-
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setNumberArray(Number[] value, long time) {
-    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value), time);
-  }
-
-  @Override
-  public boolean setDefault(NetworkTableValue defaultValue) {
-    long time = defaultValue.getTime();
-    Object otherValue = defaultValue.getValue();
-    switch (defaultValue.getType()) {
-      case kBoolean:
-        return NetworkTablesJNI.setDefaultBoolean(m_handle, time, (Boolean) otherValue);
-      case kInteger:
-        return NetworkTablesJNI.setDefaultInteger(
-            m_handle, time, ((Number) otherValue).longValue());
-      case kFloat:
-        return NetworkTablesJNI.setDefaultFloat(
-            m_handle, time, ((Number) otherValue).floatValue());
-      case kDouble:
-        return NetworkTablesJNI.setDefaultDouble(
-            m_handle, time, ((Number) otherValue).doubleValue());
-      case kString:
-        return NetworkTablesJNI.setDefaultString(m_handle, time, (String) otherValue);
-      case kRaw:
-        return NetworkTablesJNI.setDefaultRaw(m_handle, time, (byte[]) otherValue);
-      case kBooleanArray:
-        return NetworkTablesJNI.setDefaultBooleanArray(m_handle, time, (boolean[]) otherValue);
-      case kIntegerArray:
-        return NetworkTablesJNI.setDefaultIntegerArray(m_handle, time, (long[]) otherValue);
-      case kFloatArray:
-        return NetworkTablesJNI.setDefaultFloatArray(m_handle, time, (float[]) otherValue);
-      case kDoubleArray:
-        return NetworkTablesJNI.setDefaultDoubleArray(m_handle, time, (double[]) otherValue);
-      case kStringArray:
-        return NetworkTablesJNI.setDefaultStringArray(m_handle, time, (String[]) otherValue);
-      default:
-        return true;
-    }
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   * @throws IllegalArgumentException if the value is not a known type
-   */
-  @Override
-  public boolean setDefaultValue(Object defaultValue) {
-    if (defaultValue instanceof NetworkTableValue) {
-      return setDefault((NetworkTableValue) defaultValue);
-    } else if (defaultValue instanceof Boolean) {
-      return setDefaultBoolean((Boolean) defaultValue);
-    } else if (defaultValue instanceof Integer) {
-      return setDefaultInteger((Integer) defaultValue);
-    } else if (defaultValue instanceof Float) {
-      return setDefaultFloat((Float) defaultValue);
-    } else if (defaultValue instanceof Number) {
-      return setDefaultNumber((Number) defaultValue);
-    } else if (defaultValue instanceof String) {
-      return setDefaultString((String) defaultValue);
-    } else if (defaultValue instanceof byte[]) {
-      return setDefaultRaw((byte[]) defaultValue);
-    } else if (defaultValue instanceof boolean[]) {
-      return setDefaultBooleanArray((boolean[]) defaultValue);
-    } else if (defaultValue instanceof long[]) {
-      return setDefaultIntegerArray((long[]) defaultValue);
-    } else if (defaultValue instanceof float[]) {
-      return setDefaultFloatArray((float[]) defaultValue);
-    } else if (defaultValue instanceof double[]) {
-      return setDefaultDoubleArray((double[]) defaultValue);
-    } else if (defaultValue instanceof Boolean[]) {
-      return setDefaultBooleanArray((Boolean[]) defaultValue);
-    } else if (defaultValue instanceof Long[]) {
-      return setDefaultIntegerArray((Long[]) defaultValue);
-    } else if (defaultValue instanceof Float[]) {
-      return setDefaultFloatArray((Float[]) defaultValue);
-    } else if (defaultValue instanceof Number[]) {
-      return setDefaultNumberArray((Number[]) defaultValue);
-    } else if (defaultValue instanceof String[]) {
-      return setDefaultStringArray((String[]) defaultValue);
-    } else {
-      throw new IllegalArgumentException(
-          "Value of type " + defaultValue.getClass().getName() + " cannot be put into a table");
-    }
-  }
-{% for t in types %}
-{% if t.TypeName == "Raw" %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean setDefaultRaw(byte[] defaultValue, int start, int len) {
-    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len) {
-    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
-  }
-{% else %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean setDefault{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
-    return NetworkTablesJNI.setDefault{{ t.TypeName }}(m_handle, 0, defaultValue);
-  }
-{% endif -%}
-{% if t.java.WrapValueType %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  @Override
-  public boolean setDefault{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
-    return setDefault{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue));
-  }
-{% endif -%}
-{% endfor %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefaultNumber(Number defaultValue) {
-    return setDefaultDouble(defaultValue.doubleValue());
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefaultNumberArray(Number[] defaultValue) {
-    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
-  }
-
-  @Override
-  public void unpublish() {
-    NetworkTablesJNI.unpublish(m_handle);
-  }
-
-  private final Topic m_topic;
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/GenericPublisher.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/GenericPublisher.java.jinja
deleted file mode 100644
index 881aba6..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/GenericPublisher.java.jinja
+++ /dev/null
@@ -1,244 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import java.nio.ByteBuffer;
-import java.util.function.Consumer;
-
-/** NetworkTables generic publisher. */
-public interface GenericPublisher extends Publisher, Consumer<NetworkTableValue> {
-  /**
-   * Get the corresponding topic.
-   *
-   * @return Topic
-   */
-  @Override
-  Topic getTopic();
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @return False if the topic already exists with a different type
-   */
-  boolean set(NetworkTableValue value);
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @return False if the topic already exists with a different type
-   * @throws IllegalArgumentException if the value is not a known type
-   */
-  default boolean setValue(Object value) {
-    return setValue(value, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param time timestamp; 0 indicates current NT time should be used
-   * @return False if the topic already exists with a different type
-   * @throws IllegalArgumentException if the value is not a known type
-   */
-  boolean setValue(Object value, long time);
-{% for t in types %}
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @return False if the topic already exists with a different type
-   */
-  default boolean set{{ t.TypeName }}({{ t.java.ValueType }} value) {
-    return set{{ t.TypeName }}(value, 0);
-  }
-{% if t.TypeName == "Raw" %}
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @return False if the topic already exists with a different type
-   */
-  default boolean setRaw(ByteBuffer value) {
-    return setRaw(value, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param time timestamp; 0 indicates current NT time should be used
-   * @return False if the topic already exists with a different type
-   */
-  default boolean setRaw(byte[] value, long time) {
-    return setRaw(value, 0, value.length, time);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish; will send from value.position() to value.limit()
-   * @param time timestamp; 0 indicates current NT time should be used
-   * @return False if the topic already exists with a different type
-   */
-  default boolean setRaw(ByteBuffer value, long time) {
-    int pos = value.position();
-    return setRaw(value, pos, value.limit() - pos, time);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @return False if the topic already exists with a different type
-   */
-  default boolean setRaw(byte[] value, int start, int len) {
-    return setRaw(value, start, len, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @param time timestamp; 0 indicates current NT time should be used
-   * @return False if the topic already exists with a different type
-   */
-  boolean setRaw(byte[] value, int start, int len, long time);
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @return False if the topic already exists with a different type
-   */
-  default boolean setRaw(ByteBuffer value, int start, int len) {
-    return setRaw(value, start, len, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @param time timestamp; 0 indicates current NT time should be used
-   * @return False if the topic already exists with a different type
-   */
-  boolean setRaw(ByteBuffer value, int start, int len, long time);
-{% else %}
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param time timestamp; 0 indicates current NT time should be used
-   * @return False if the topic already exists with a different type
-   */
-  boolean set{{ t.TypeName }}({{ t.java.ValueType }} value, long time);
-{% endif -%}
-{% if t.java.WrapValueType %}
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @return False if the topic already exists with a different type
-   */
-  default boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value) {
-    return set{{ t.TypeName }}(value, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param time timestamp; 0 indicates current NT time should be used
-   * @return False if the topic already exists with a different type
-   */
-  boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value, long time);
-{% endif -%}
-{% endfor %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  boolean setDefault(NetworkTableValue defaultValue);
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   * @throws IllegalArgumentException if the value is not a known type
-   */
-  boolean setDefaultValue(Object defaultValue);
-{% for t in types %}
-{% if t.TypeName == "Raw" %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  default boolean setDefaultRaw(byte[] defaultValue) {
-    return setDefaultRaw(defaultValue, 0, defaultValue.length);
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set; will send from defaultValue.position() to
-   *                     defaultValue.limit()
-   * @return False if the entry exists with a different type
-   */
-  default boolean setDefaultRaw(ByteBuffer defaultValue) {
-    int pos = defaultValue.position();
-    return setDefaultRaw(defaultValue, pos, defaultValue.limit() - pos);
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @return False if the entry exists with a different type
-   */
-  boolean setDefaultRaw(byte[] defaultValue, int start, int len);
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @return False if the entry exists with a different type
-   */
-  boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len);
-{% else %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  boolean setDefault{{ t.TypeName }}({{ t.java.ValueType }} defaultValue);
-{% endif -%}
-{% if t.java.WrapValueType %}
-  boolean setDefault{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue);
-{% endif -%}
-{% endfor %}
-  @Override
-  default void accept(NetworkTableValue value) {
-    set(value);
-  }
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/GenericSubscriber.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/GenericSubscriber.java.jinja
deleted file mode 100644
index 63ecebc..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/GenericSubscriber.java.jinja
+++ /dev/null
@@ -1,58 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import java.util.function.Supplier;
-
-/** NetworkTables generic subscriber. */
-@SuppressWarnings("PMD.MissingOverride")
-public interface GenericSubscriber extends Subscriber, Supplier<NetworkTableValue> {
-  /**
-   * Get the corresponding topic.
-   *
-   * @return Topic
-   */
-  @Override
-  Topic getTopic();
-
-  /**
-   * Get the last published value.
-   * If no value has been published, returns a value with type NetworkTableType.kUnassigned.
-   *
-   * @return value
-   */
-  NetworkTableValue get();
-{% for t in types %}
-  /**
-   * Gets the entry's value as a {{ t.java.ValueType }}. If the entry does not exist or is of different type, it
-   * will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  {{ t.java.ValueType }} get{{ t.TypeName }}({{ t.java.ValueType }} defaultValue);
-{% if t.java.WrapValueType %}
-  /**
-   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
-   * it will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  {{ t.java.WrapValueType }} get{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue);
-{% endif -%}
-{% endfor %}
-  /**
-   * Get an array of all value changes since the last call to readQueue.
-   * Also provides a timestamp for each value.
-   *
-   * <p>The "poll storage" subscribe option can be used to set the queue
-   * depth.
-   *
-   * @return Array of timestamped values; empty array if no new changes have
-   *     been published since the previous call.
-   */
-  NetworkTableValue[] readQueue();
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/NetworkTableEntry.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/NetworkTableEntry.java.jinja
deleted file mode 100644
index 7ee7d1f..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/NetworkTableEntry.java.jinja
+++ /dev/null
@@ -1,610 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import java.nio.ByteBuffer;
-
-/**
- * NetworkTables Entry.
- *
- * <p>For backwards compatibility, the NetworkTableEntry close() does not release the entry.
- */
-@SuppressWarnings("UnnecessaryParentheses")
-public final class NetworkTableEntry implements Publisher, Subscriber {
-  /**
-   * Flag values (as returned by {@link #getFlags()}).
-   *
-   * @deprecated Use isPersistent() instead.
-   */
-  @Deprecated(since = "2022", forRemoval = true)
-  public static final int kPersistent = 0x01;
-
-  /**
-   * Construct from native handle.
-   *
-   * @param inst Instance
-   * @param handle Native handle
-   */
-  public NetworkTableEntry(NetworkTableInstance inst, int handle) {
-    this(new Topic(inst, NetworkTablesJNI.getTopicFromHandle(handle)), handle);
-  }
-
-  /**
-   * Construct from native handle.
-   *
-   * @param topic Topic
-   * @param handle Native handle
-   */
-  public NetworkTableEntry(Topic topic, int handle) {
-    m_topic = topic;
-    m_handle = handle;
-  }
-
-  @Override
-  public void close() {}
-
-  /**
-   * Determines if the native handle is valid.
-   *
-   * @return True if the native handle is valid, false otherwise.
-   */
-  @Override
-  public boolean isValid() {
-    return m_handle != 0;
-  }
-
-  /**
-   * Gets the native handle for the entry.
-   *
-   * @return Native handle
-   */
-  @Override
-  public int getHandle() {
-    return m_handle;
-  }
-
-  /**
-   * Gets the subscribed-to / published-to topic.
-   *
-   * @return Topic
-   */
-  @Override
-  public Topic getTopic() {
-    return m_topic;
-  }
-
-  /**
-   * Gets the instance for the entry.
-   *
-   * @return Instance
-   */
-  public NetworkTableInstance getInstance() {
-    return m_topic.getInstance();
-  }
-
-  /**
-   * Determines if the entry currently exists.
-   *
-   * @return True if the entry exists, false otherwise.
-   */
-  @Override
-  public boolean exists() {
-    return NetworkTablesJNI.getType(m_handle) != 0;
-  }
-
-  /**
-   * Gets the name of the entry (the key).
-   *
-   * @return the entry's name
-   */
-  public String getName() {
-    return NetworkTablesJNI.getEntryName(m_handle);
-  }
-
-  /**
-   * Gets the type of the entry.
-   *
-   * @return the entry's type
-   */
-  public NetworkTableType getType() {
-    return NetworkTableType.getFromInt(NetworkTablesJNI.getType(m_handle));
-  }
-
-  /**
-   * Returns the flags.
-   *
-   * @return the flags (bitmask)
-   * @deprecated Use isPersistent() or topic properties instead
-   */
-  @Deprecated(since = "2022", forRemoval = true)
-  public int getFlags() {
-    return NetworkTablesJNI.getEntryFlags(m_handle);
-  }
-
-  /**
-   * Gets the last time the entry's value was changed.
-   *
-   * @return Entry last change time
-   */
-  @Override
-  public long getLastChange() {
-    return NetworkTablesJNI.getEntryLastChange(m_handle);
-  }
-
-  /**
-   * Gets the entry's value. Returns a value with type NetworkTableType.kUnassigned if the value
-   * does not exist.
-   *
-   * @return the entry's value
-   */
-  public NetworkTableValue getValue() {
-    return NetworkTablesJNI.getValue(m_handle);
-  }
-{% for t in types %}
-  /**
-   * Gets the entry's value as a {{ t.java.ValueType }}. If the entry does not exist or is of different type, it
-   * will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  public {{ t.java.ValueType }} get{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
-    return NetworkTablesJNI.get{{ t.TypeName }}(m_handle, defaultValue);
-  }
-{% if t.java.WrapValueType %}
-  /**
-   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
-   * it will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  public {{ t.java.WrapValueType }} get{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
-    return NetworkTableValue.fromNative{{ t.TypeName }}(
-        get{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue)));
-  }
-{% endif -%}
-{% endfor %}
-  /**
-   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
-   * will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  public Number getNumber(Number defaultValue) {
-    return getDouble(defaultValue.doubleValue());
-  }
-
-  /**
-   * Gets the entry's value as a double array. If the entry does not exist or is of different type,
-   * it will return the default value.
-   *
-   * @param defaultValue the value to be returned if no value is found
-   * @return the entry's value or the given default value
-   */
-  public Number[] getNumberArray(Number[] defaultValue) {
-    return NetworkTableValue.fromNativeDoubleArray(
-        getDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue)));
-  }
-
-  /**
-   * Get an array of all value changes since the last call to readQueue.
-   *
-   * <p>The "poll storage" subscribe option can be used to set the queue
-   * depth.
-   *
-   * @return Array of values; empty array if no new changes have been
-   *     published since the previous call.
-   */
-  public NetworkTableValue[] readQueue() {
-    return NetworkTablesJNI.readQueueValue(m_handle);
-  }
-
-  /**
-   * Checks if a data value is of a type that can be placed in a NetworkTable entry.
-   *
-   * @param data the data to check
-   * @return true if the data can be placed in an entry, false if it cannot
-   */
-  public static boolean isValidDataType(Object data) {
-    return data instanceof Number
-        || data instanceof Boolean
-        || data instanceof String
-        || data instanceof long[]
-        || data instanceof Long[]
-        || data instanceof float[]
-        || data instanceof Float[]
-        || data instanceof double[]
-        || data instanceof Double[]
-        || data instanceof Number[]
-        || data instanceof boolean[]
-        || data instanceof Boolean[]
-        || data instanceof String[]
-        || data instanceof byte[]
-        || data instanceof Byte[];
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   * @throws IllegalArgumentException if the value is not a known type
-   */
-  public boolean setDefaultValue(Object defaultValue) {
-    if (defaultValue instanceof NetworkTableValue) {
-      long time = ((NetworkTableValue) defaultValue).getTime();
-      Object otherValue = ((NetworkTableValue) defaultValue).getValue();
-      switch (((NetworkTableValue) defaultValue).getType()) {
-        case kBoolean:
-          return NetworkTablesJNI.setDefaultBoolean(m_handle, time, (Boolean) otherValue);
-        case kInteger:
-          return NetworkTablesJNI.setDefaultInteger(
-              m_handle, time, ((Number) otherValue).longValue());
-        case kFloat:
-          return NetworkTablesJNI.setDefaultFloat(
-              m_handle, time, ((Number) otherValue).floatValue());
-        case kDouble:
-          return NetworkTablesJNI.setDefaultDouble(
-              m_handle, time, ((Number) otherValue).doubleValue());
-        case kString:
-          return NetworkTablesJNI.setDefaultString(m_handle, time, (String) otherValue);
-        case kRaw:
-          return NetworkTablesJNI.setDefaultRaw(m_handle, time, (byte[]) otherValue);
-        case kBooleanArray:
-          return NetworkTablesJNI.setDefaultBooleanArray(m_handle, time, (boolean[]) otherValue);
-        case kIntegerArray:
-          return NetworkTablesJNI.setDefaultIntegerArray(m_handle, time, (long[]) otherValue);
-        case kFloatArray:
-          return NetworkTablesJNI.setDefaultFloatArray(m_handle, time, (float[]) otherValue);
-        case kDoubleArray:
-          return NetworkTablesJNI.setDefaultDoubleArray(m_handle, time, (double[]) otherValue);
-        case kStringArray:
-          return NetworkTablesJNI.setDefaultStringArray(m_handle, time, (String[]) otherValue);
-        default:
-          return true;
-      }
-    } else if (defaultValue instanceof Boolean) {
-      return setDefaultBoolean((Boolean) defaultValue);
-    } else if (defaultValue instanceof Integer) {
-      return setDefaultInteger((Integer) defaultValue);
-    } else if (defaultValue instanceof Float) {
-      return setDefaultFloat((Float) defaultValue);
-    } else if (defaultValue instanceof Number) {
-      return setDefaultNumber((Number) defaultValue);
-    } else if (defaultValue instanceof String) {
-      return setDefaultString((String) defaultValue);
-    } else if (defaultValue instanceof byte[]) {
-      return setDefaultRaw((byte[]) defaultValue);
-    } else if (defaultValue instanceof boolean[]) {
-      return setDefaultBooleanArray((boolean[]) defaultValue);
-    } else if (defaultValue instanceof long[]) {
-      return setDefaultIntegerArray((long[]) defaultValue);
-    } else if (defaultValue instanceof float[]) {
-      return setDefaultFloatArray((float[]) defaultValue);
-    } else if (defaultValue instanceof double[]) {
-      return setDefaultDoubleArray((double[]) defaultValue);
-    } else if (defaultValue instanceof Boolean[]) {
-      return setDefaultBooleanArray((Boolean[]) defaultValue);
-    } else if (defaultValue instanceof Long[]) {
-      return setDefaultIntegerArray((Long[]) defaultValue);
-    } else if (defaultValue instanceof Float[]) {
-      return setDefaultFloatArray((Float[]) defaultValue);
-    } else if (defaultValue instanceof Number[]) {
-      return setDefaultNumberArray((Number[]) defaultValue);
-    } else if (defaultValue instanceof String[]) {
-      return setDefaultStringArray((String[]) defaultValue);
-    } else {
-      throw new IllegalArgumentException(
-          "Value of type " + defaultValue.getClass().getName() + " cannot be put into a table");
-    }
-  }
-{% for t in types %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefault{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
-    return NetworkTablesJNI.setDefault{{ t.TypeName }}(m_handle, 0, defaultValue);
-  }
-{% if t.TypeName == "Raw" %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set; will send from defaultValue.position() to
-   *                     defaultValue.capacity()
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefaultRaw(ByteBuffer defaultValue) {
-    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue);
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefaultRaw(byte[] defaultValue, int start, int len) {
-    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len) {
-    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
-  }
-{% endif -%}
-{% if t.java.WrapValueType %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefault{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
-    return setDefault{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue));
-  }
-{% endif -%}
-{% endfor %}
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefaultNumber(Number defaultValue) {
-    return setDefaultDouble(defaultValue.doubleValue());
-  }
-
-  /**
-   * Sets the entry's value if it does not exist.
-   *
-   * @param defaultValue the default value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setDefaultNumberArray(Number[] defaultValue) {
-    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
-  }
-
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   * @throws IllegalArgumentException if the value is not a known type
-   */
-  public boolean setValue(Object value) {
-    if (value instanceof NetworkTableValue) {
-      long time = ((NetworkTableValue) value).getTime();
-      Object otherValue = ((NetworkTableValue) value).getValue();
-      switch (((NetworkTableValue) value).getType()) {
-        case kBoolean:
-          return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue);
-        case kInteger:
-          return NetworkTablesJNI.setInteger(
-              m_handle, time, ((Number) otherValue).longValue());
-        case kFloat:
-          return NetworkTablesJNI.setFloat(
-              m_handle, time, ((Number) otherValue).floatValue());
-        case kDouble:
-          return NetworkTablesJNI.setDouble(
-              m_handle, time, ((Number) otherValue).doubleValue());
-        case kString:
-          return NetworkTablesJNI.setString(m_handle, time, (String) otherValue);
-        case kRaw:
-          return NetworkTablesJNI.setRaw(m_handle, time, (byte[]) otherValue);
-        case kBooleanArray:
-          return NetworkTablesJNI.setBooleanArray(m_handle, time, (boolean[]) otherValue);
-        case kIntegerArray:
-          return NetworkTablesJNI.setIntegerArray(m_handle, time, (long[]) otherValue);
-        case kFloatArray:
-          return NetworkTablesJNI.setFloatArray(m_handle, time, (float[]) otherValue);
-        case kDoubleArray:
-          return NetworkTablesJNI.setDoubleArray(m_handle, time, (double[]) otherValue);
-        case kStringArray:
-          return NetworkTablesJNI.setStringArray(m_handle, time, (String[]) otherValue);
-        default:
-          return true;
-      }
-    } else if (value instanceof Boolean) {
-      return setBoolean((Boolean) value);
-    } else if (value instanceof Long) {
-      return setInteger((Long) value);
-    } else if (value instanceof Float) {
-      return setFloat((Float) value);
-    } else if (value instanceof Number) {
-      return setNumber((Number) value);
-    } else if (value instanceof String) {
-      return setString((String) value);
-    } else if (value instanceof byte[]) {
-      return setRaw((byte[]) value);
-    } else if (value instanceof boolean[]) {
-      return setBooleanArray((boolean[]) value);
-    } else if (value instanceof long[]) {
-      return setIntegerArray((long[]) value);
-    } else if (value instanceof float[]) {
-      return setFloatArray((float[]) value);
-    } else if (value instanceof double[]) {
-      return setDoubleArray((double[]) value);
-    } else if (value instanceof Boolean[]) {
-      return setBooleanArray((Boolean[]) value);
-    } else if (value instanceof Long[]) {
-      return setIntegerArray((Long[]) value);
-    } else if (value instanceof Float[]) {
-      return setFloatArray((Float[]) value);
-    } else if (value instanceof Number[]) {
-      return setNumberArray((Number[]) value);
-    } else if (value instanceof String[]) {
-      return setStringArray((String[]) value);
-    } else {
-      throw new IllegalArgumentException(
-          "Value of type " + value.getClass().getName() + " cannot be put into a table");
-    }
-  }
-{% for t in types %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean set{{ t.TypeName }}({{ t.java.ValueType }} value) {
-    return NetworkTablesJNI.set{{ t.TypeName }}(m_handle, 0, value);
-  }
-{% if t.TypeName == "Raw" %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set; will send from value.position() to value.capacity()
-   * @return False if the entry exists with a different type
-   */
-  public boolean setRaw(ByteBuffer value) {
-    return NetworkTablesJNI.setRaw(m_handle, 0, value);
-  }
-
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @return False if the entry exists with a different type
-   */
-  public boolean setRaw(byte[] value, int start, int len) {
-    return NetworkTablesJNI.setRaw(m_handle, 0, value, start, len);
-  }
-
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @return False if the entry exists with a different type
-   */
-  public boolean setRaw(ByteBuffer value, int start, int len) {
-    return NetworkTablesJNI.setRaw(m_handle, 0, value, start, len);
-  }
-{% endif -%}
-{% if t.java.WrapValueType %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value) {
-    return set{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(value));
-  }
-{% endif -%}
-{% endfor %}
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setNumber(Number value) {
-    return setDouble(value.doubleValue());
-  }
-
-  /**
-   * Sets the entry's value.
-   *
-   * @param value the value to set
-   * @return False if the entry exists with a different type
-   */
-  public boolean setNumberArray(Number[] value) {
-    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value));
-  }
-
-  /**
-   * Sets flags.
-   *
-   * @param flags the flags to set (bitmask)
-   * @deprecated Use setPersistent() or topic properties instead
-   */
-  @Deprecated(since = "2022", forRemoval = true)
-  public void setFlags(int flags) {
-    NetworkTablesJNI.setEntryFlags(m_handle, getFlags() | flags);
-  }
-
-  /**
-   * Clears flags.
-   *
-   * @param flags the flags to clear (bitmask)
-   * @deprecated Use setPersistent() or topic properties instead
-   */
-  @Deprecated(since = "2022", forRemoval = true)
-  public void clearFlags(int flags) {
-    NetworkTablesJNI.setEntryFlags(m_handle, getFlags() & ~flags);
-  }
-
-  /** Make value persistent through program restarts. */
-  public void setPersistent() {
-    NetworkTablesJNI.setTopicPersistent(m_topic.getHandle(), true);
-  }
-
-  /** Stop making value persistent through program restarts. */
-  public void clearPersistent() {
-    NetworkTablesJNI.setTopicPersistent(m_topic.getHandle(), false);
-  }
-
-  /**
-   * Returns whether the value is persistent through program restarts.
-   *
-   * @return True if the value is persistent.
-   */
-  public boolean isPersistent() {
-    return NetworkTablesJNI.getTopicPersistent(m_topic.getHandle());
-  }
-
-  /** Stops publishing the entry if it's been published. */
-  public void unpublish() {
-    NetworkTablesJNI.unpublish(m_handle);
-  }
-
-  /**
-   * Deletes the entry.
-   *
-   * @deprecated Use unpublish() instead.
-   */
-  @Deprecated(since = "2022", forRemoval = true)
-  public void delete() {
-    unpublish();
-  }
-
-  @Override
-  public boolean equals(Object other) {
-    if (other == this) {
-      return true;
-    }
-    if (!(other instanceof NetworkTableEntry)) {
-      return false;
-    }
-
-    return m_handle == ((NetworkTableEntry) other).m_handle;
-  }
-
-  @Override
-  public int hashCode() {
-    return m_handle;
-  }
-
-  private final Topic m_topic;
-  protected int m_handle;
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/NetworkTableInstance.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/NetworkTableInstance.java.jinja
deleted file mode 100644
index 9df129d..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/NetworkTableInstance.java.jinja
+++ /dev/null
@@ -1,1271 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import edu.wpi.first.util.WPIUtilJNI;
-import edu.wpi.first.util.concurrent.Event;
-import edu.wpi.first.util.datalog.DataLog;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.charset.StandardCharsets;
-import java.util.EnumSet;
-import java.util.HashMap;
-import java.util.HashSet;
-import java.util.Map;
-import java.util.OptionalLong;
-import java.util.Set;
-import java.util.concurrent.ConcurrentHashMap;
-import java.util.concurrent.ConcurrentMap;
-import java.util.concurrent.TimeUnit;
-import java.util.concurrent.locks.Condition;
-import java.util.concurrent.locks.ReentrantLock;
-import java.util.function.Consumer;
-import us.hebi.quickbuf.ProtoMessage;
-
-/**
- * NetworkTables Instance.
- *
- * <p>Instances are completely independent from each other. Table operations on one instance will
- * not be visible to other instances unless the instances are connected via the network. The main
- * limitation on instances is that you cannot have two servers on the same network port. The main
- * utility of instances is for unit testing, but they can also enable one program to connect to two
- * different NetworkTables networks.
- *
- * <p>The global "default" instance (as returned by {@link #getDefault()}) is always available, and
- * is intended for the common case when there is only a single NetworkTables instance being used in
- * the program.
- *
- * <p>Additional instances can be created with the {@link #create()} function. A reference must be
- * kept to the NetworkTableInstance returned by this function to keep it from being garbage
- * collected.
- */
-@SuppressWarnings("PMD.CouplingBetweenObjects")
-public final class NetworkTableInstance implements AutoCloseable {
-  /** Client/server mode flag values (as returned by {@link #getNetworkMode()}). */
-  public enum NetworkMode {
-    /** Running in server mode. */
-    kServer(0x01),
-
-    /** Running in NT3 client mode. */
-    kClient3(0x02),
-
-    /** Running in NT4 client mode. */
-    kClient4(0x04),
-
-    /** Currently starting up (either client or server). */
-    kStarting(0x08),
-
-    /** Running in local-only mode. */
-    kLocal(0x10);
-
-    private final int value;
-
-    NetworkMode(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  /** The default port that network tables operates on for NT3. */
-  public static final int kDefaultPort3 = 1735;
-
-  /** The default port that network tables operates on for NT4. */
-  public static final int kDefaultPort4 = 5810;
-
-  /**
-   * Construct from native handle.
-   *
-   * @param handle Native handle
-   */
-  private NetworkTableInstance(int handle) {
-    m_owned = false;
-    m_handle = handle;
-  }
-
-  /** Destroys the instance (if created by {@link #create()}). */
-  @Override
-  public synchronized void close() {
-    if (m_owned && m_handle != 0) {
-      m_listeners.close();
-      m_schemas.forEach((k, v) -> v.close());
-      NetworkTablesJNI.destroyInstance(m_handle);
-      m_handle = 0;
-    }
-  }
-
-  /**
-   * Determines if the native handle is valid.
-   *
-   * @return True if the native handle is valid, false otherwise.
-   */
-  public boolean isValid() {
-    return m_handle != 0;
-  }
-
-  /* The default instance. */
-  private static NetworkTableInstance s_defaultInstance;
-
-  /**
-   * Get global default instance.
-   *
-   * @return Global default instance
-   */
-  public static synchronized NetworkTableInstance getDefault() {
-    if (s_defaultInstance == null) {
-      s_defaultInstance = new NetworkTableInstance(NetworkTablesJNI.getDefaultInstance());
-    }
-    return s_defaultInstance;
-  }
-
-  /**
-   * Create an instance. Note: A reference to the returned instance must be retained to ensure the
-   * instance is not garbage collected.
-   *
-   * @return Newly created instance
-   */
-  public static NetworkTableInstance create() {
-    NetworkTableInstance inst = new NetworkTableInstance(NetworkTablesJNI.createInstance());
-    inst.m_owned = true;
-    return inst;
-  }
-
-  /**
-   * Gets the native handle for the instance.
-   *
-   * @return Native handle
-   */
-  public int getHandle() {
-    return m_handle;
-  }
-
-  /**
-   * Get (generic) topic.
-   *
-   * @param name topic name
-   * @return Topic
-   */
-  public Topic getTopic(String name) {
-    Topic topic = m_topics.get(name);
-    if (topic == null) {
-      int handle = NetworkTablesJNI.getTopic(m_handle, name);
-      topic = new Topic(this, handle);
-      Topic oldTopic = m_topics.putIfAbsent(name, topic);
-      if (oldTopic != null) {
-        topic = oldTopic;
-      }
-      // also cache by handle
-      m_topicsByHandle.putIfAbsent(handle, topic);
-    }
-    return topic;
-  }
-{% for t in types %}
-  /**
-   * Get {{ t.java.ValueType }} topic.
-   *
-   * @param name topic name
-   * @return {{ t.TypeName }}Topic
-   */
-  public {{ t.TypeName }}Topic get{{ t.TypeName }}Topic(String name) {
-    Topic topic = m_topics.get(name);
-    if (topic instanceof {{ t.TypeName }}Topic) {
-      return ({{ t.TypeName }}Topic) topic;
-    }
-
-    int handle;
-    if (topic == null) {
-      handle = NetworkTablesJNI.getTopic(m_handle, name);
-    } else {
-      handle = topic.getHandle();
-    }
-
-    {{ t.TypeName }}Topic wrapTopic = new {{ t.TypeName }}Topic(this, handle);
-    m_topics.put(name, wrapTopic);
-
-    // also cache by handle
-    m_topicsByHandle.put(handle, wrapTopic);
-
-    return wrapTopic;
-  }
-{% endfor %}
-
-  /**
-   * Get protobuf-encoded value topic.
-   *
-   * @param <T> value class (inferred from proto)
-   * @param <MessageType> protobuf message type (inferred from proto)
-   * @param name topic name
-   * @param proto protobuf serialization implementation
-   * @return ProtobufTopic
-   */
-  public <T, MessageType extends ProtoMessage<?>>
-      ProtobufTopic<T> getProtobufTopic(String name, Protobuf<T, MessageType> proto) {
-    Topic topic = m_topics.get(name);
-    if (topic instanceof ProtobufTopic<?>
-        && ((ProtobufTopic<?>) topic).getProto().equals(proto)) {
-      @SuppressWarnings("unchecked")
-      ProtobufTopic<T> wrapTopic = (ProtobufTopic<T>) topic;
-      return wrapTopic;
-    }
-
-    int handle;
-    if (topic == null) {
-      handle = NetworkTablesJNI.getTopic(m_handle, name);
-    } else {
-      handle = topic.getHandle();
-    }
-
-    ProtobufTopic<T> wrapTopic = ProtobufTopic.wrap(this, handle, proto);
-    m_topics.put(name, wrapTopic);
-
-    // also cache by handle
-    m_topicsByHandle.put(handle, wrapTopic);
-
-    return wrapTopic;
-  }
-
-  /**
-   * Get struct-encoded value topic.
-   *
-   * @param <T> value class (inferred from struct)
-   * @param name topic name
-   * @param struct struct serialization implementation
-   * @return StructTopic
-   */
-  public <T>
-      StructTopic<T> getStructTopic(String name, Struct<T> struct) {
-    Topic topic = m_topics.get(name);
-    if (topic instanceof StructTopic<?>
-        && ((StructTopic<?>) topic).getStruct().equals(struct)) {
-      @SuppressWarnings("unchecked")
-      StructTopic<T> wrapTopic = (StructTopic<T>) topic;
-      return wrapTopic;
-    }
-
-    int handle;
-    if (topic == null) {
-      handle = NetworkTablesJNI.getTopic(m_handle, name);
-    } else {
-      handle = topic.getHandle();
-    }
-
-    StructTopic<T> wrapTopic = StructTopic.wrap(this, handle, struct);
-    m_topics.put(name, wrapTopic);
-
-    // also cache by handle
-    m_topicsByHandle.put(handle, wrapTopic);
-
-    return wrapTopic;
-  }
-
-  /**
-   * Get struct-encoded value array topic.
-   *
-   * @param <T> value class (inferred from struct)
-   * @param name topic name
-   * @param struct struct serialization implementation
-   * @return StructArrayTopic
-   */
-  public <T>
-      StructArrayTopic<T> getStructArrayTopic(String name, Struct<T> struct) {
-    Topic topic = m_topics.get(name);
-    if (topic instanceof StructArrayTopic<?>
-        && ((StructArrayTopic<?>) topic).getStruct().equals(struct)) {
-      @SuppressWarnings("unchecked")
-      StructArrayTopic<T> wrapTopic = (StructArrayTopic<T>) topic;
-      return wrapTopic;
-    }
-
-    int handle;
-    if (topic == null) {
-      handle = NetworkTablesJNI.getTopic(m_handle, name);
-    } else {
-      handle = topic.getHandle();
-    }
-
-    StructArrayTopic<T> wrapTopic = StructArrayTopic.wrap(this, handle, struct);
-    m_topics.put(name, wrapTopic);
-
-    // also cache by handle
-    m_topicsByHandle.put(handle, wrapTopic);
-
-    return wrapTopic;
-  }
-
-  private Topic[] topicHandlesToTopics(int[] handles) {
-    Topic[] topics = new Topic[handles.length];
-    for (int i = 0; i < handles.length; i++) {
-      topics[i] = getCachedTopic(handles[i]);
-    }
-    return topics;
-  }
-
-  /**
-   * Get all published topics.
-   *
-   * @return Array of topics.
-   */
-  public Topic[] getTopics() {
-    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, "", 0));
-  }
-
-  /**
-   * Get published topics starting with the given prefix. The results are optionally filtered by
-   * string prefix to only return a subset of all topics.
-   *
-   * @param prefix topic name required prefix; only topics whose name starts with this string are
-   *     returned
-   * @return Array of topic information.
-   */
-  public Topic[] getTopics(String prefix) {
-    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, prefix, 0));
-  }
-
-  /**
-   * Get published topics starting with the given prefix. The results are optionally filtered by
-   * string prefix and data type to only return a subset of all topics.
-   *
-   * @param prefix topic name required prefix; only topics whose name starts with this string are
-   *     returned
-   * @param types bitmask of data types; 0 is treated as a "don't care"
-   * @return Array of topic information.
-   */
-  public Topic[] getTopics(String prefix, int types) {
-    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, prefix, types));
-  }
-
-  /**
-   * Get published topics starting with the given prefix. The results are optionally filtered by
-   * string prefix and data type to only return a subset of all topics.
-   *
-   * @param prefix topic name required prefix; only topics whose name starts with this string are
-   *     returned
-   * @param types array of data type strings
-   * @return Array of topic information.
-   */
-  public Topic[] getTopics(String prefix, String[] types) {
-    return topicHandlesToTopics(NetworkTablesJNI.getTopicsStr(m_handle, prefix, types));
-  }
-
-  /**
-   * Get information about all topics.
-   *
-   * @return Array of topic information.
-   */
-  public TopicInfo[] getTopicInfo() {
-    return NetworkTablesJNI.getTopicInfos(this, m_handle, "", 0);
-  }
-
-  /**
-   * Get information about topics starting with the given prefix. The results are optionally
-   * filtered by string prefix to only return a subset of all topics.
-   *
-   * @param prefix topic name required prefix; only topics whose name starts with this string are
-   *     returned
-   * @return Array of topic information.
-   */
-  public TopicInfo[] getTopicInfo(String prefix) {
-    return NetworkTablesJNI.getTopicInfos(this, m_handle, prefix, 0);
-  }
-
-  /**
-   * Get information about topics starting with the given prefix. The results are optionally
-   * filtered by string prefix and data type to only return a subset of all topics.
-   *
-   * @param prefix topic name required prefix; only topics whose name starts with this string are
-   *     returned
-   * @param types bitmask of data types; 0 is treated as a "don't care"
-   * @return Array of topic information.
-   */
-  public TopicInfo[] getTopicInfo(String prefix, int types) {
-    return NetworkTablesJNI.getTopicInfos(this, m_handle, prefix, types);
-  }
-
-  /**
-   * Get information about topics starting with the given prefix. The results are optionally
-   * filtered by string prefix and data type to only return a subset of all topics.
-   *
-   * @param prefix topic name required prefix; only topics whose name starts with this string are
-   *     returned
-   * @param types array of data type strings
-   * @return Array of topic information.
-   */
-  public TopicInfo[] getTopicInfo(String prefix, String[] types) {
-    return NetworkTablesJNI.getTopicInfosStr(this, m_handle, prefix, types);
-  }
-
-  /* Cache of created entries. */
-  private final ConcurrentMap<String, NetworkTableEntry> m_entries = new ConcurrentHashMap<>();
-
-  /**
-   * Gets the entry for a key.
-   *
-   * @param name Key
-   * @return Network table entry.
-   */
-  public NetworkTableEntry getEntry(String name) {
-    NetworkTableEntry entry = m_entries.get(name);
-    if (entry == null) {
-      entry = new NetworkTableEntry(this, NetworkTablesJNI.getEntry(m_handle, name));
-      NetworkTableEntry oldEntry = m_entries.putIfAbsent(name, entry);
-      if (oldEntry != null) {
-        entry = oldEntry;
-      }
-    }
-    return entry;
-  }
-
-  /* Cache of created topics. */
-  private final ConcurrentMap<String, Topic> m_topics = new ConcurrentHashMap<>();
-  private final ConcurrentMap<Integer, Topic> m_topicsByHandle = new ConcurrentHashMap<>();
-
-  Topic getCachedTopic(String name) {
-    Topic topic = m_topics.get(name);
-    if (topic == null) {
-      int handle = NetworkTablesJNI.getTopic(m_handle, name);
-      topic = new Topic(this, handle);
-      Topic oldTopic = m_topics.putIfAbsent(name, topic);
-      if (oldTopic != null) {
-        topic = oldTopic;
-      }
-      // also cache by handle
-      m_topicsByHandle.putIfAbsent(handle, topic);
-    }
-    return topic;
-  }
-
-  Topic getCachedTopic(int handle) {
-    Topic topic = m_topicsByHandle.get(handle);
-    if (topic == null) {
-      topic = new Topic(this, handle);
-      Topic oldTopic = m_topicsByHandle.putIfAbsent(handle, topic);
-      if (oldTopic != null) {
-        topic = oldTopic;
-      }
-    }
-    return topic;
-  }
-
-  /* Cache of created tables. */
-  private final ConcurrentMap<String, NetworkTable> m_tables = new ConcurrentHashMap<>();
-
-  /**
-   * Gets the table with the specified key.
-   *
-   * @param key the key name
-   * @return The network table
-   */
-  public NetworkTable getTable(String key) {
-    // prepend leading / if not present
-    String theKey;
-    if (key.isEmpty() || "/".equals(key)) {
-      theKey = "";
-    } else if (key.charAt(0) == NetworkTable.PATH_SEPARATOR) {
-      theKey = key;
-    } else {
-      theKey = NetworkTable.PATH_SEPARATOR + key;
-    }
-
-    // cache created tables
-    NetworkTable table = m_tables.get(theKey);
-    if (table == null) {
-      table = new NetworkTable(this, theKey);
-      NetworkTable oldTable = m_tables.putIfAbsent(theKey, table);
-      if (oldTable != null) {
-        table = oldTable;
-      }
-    }
-    return table;
-  }
-
-  /*
-   * Callback Creation Functions
-   */
-
-  private static class ListenerStorage implements AutoCloseable {
-    private final ReentrantLock m_lock = new ReentrantLock();
-    private final Map<Integer, Consumer<NetworkTableEvent>> m_listeners = new HashMap<>();
-    private Thread m_thread;
-    private int m_poller;
-    private boolean m_waitQueue;
-    private final Event m_waitQueueEvent = new Event();
-    private final Condition m_waitQueueCond = m_lock.newCondition();
-    private final NetworkTableInstance m_inst;
-
-    ListenerStorage(NetworkTableInstance inst) {
-      m_inst = inst;
-    }
-
-    int add(
-        String[] prefixes,
-        EnumSet<NetworkTableEvent.Kind> eventKinds,
-        Consumer<NetworkTableEvent> listener) {
-      m_lock.lock();
-      try {
-        if (m_poller == 0) {
-          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
-          startThread();
-        }
-        int h = NetworkTablesJNI.addListener(m_poller, prefixes, eventKinds);
-        m_listeners.put(h, listener);
-        return h;
-      } finally {
-        m_lock.unlock();
-      }
-    }
-
-    int add(
-        int handle,
-        EnumSet<NetworkTableEvent.Kind> eventKinds,
-        Consumer<NetworkTableEvent> listener) {
-      m_lock.lock();
-      try {
-        if (m_poller == 0) {
-          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
-          startThread();
-        }
-        int h = NetworkTablesJNI.addListener(m_poller, handle, eventKinds);
-        m_listeners.put(h, listener);
-        return h;
-      } finally {
-        m_lock.unlock();
-      }
-    }
-
-    int addLogger(int minLevel, int maxLevel, Consumer<NetworkTableEvent> listener) {
-      m_lock.lock();
-      try {
-        if (m_poller == 0) {
-          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
-          startThread();
-        }
-        int h = NetworkTablesJNI.addLogger(m_poller, minLevel, maxLevel);
-        m_listeners.put(h, listener);
-        return h;
-      } finally {
-        m_lock.unlock();
-      }
-    }
-
-    void remove(int listener) {
-      m_lock.lock();
-      try {
-        m_listeners.remove(listener);
-      } finally {
-        m_lock.unlock();
-      }
-      NetworkTablesJNI.removeListener(listener);
-    }
-
-    @Override
-    public void close() {
-      if (m_poller != 0) {
-        NetworkTablesJNI.destroyListenerPoller(m_poller);
-      }
-      m_poller = 0;
-    }
-
-    private void startThread() {
-      m_thread =
-          new Thread(
-              () -> {
-                boolean wasInterrupted = false;
-                int[] handles = new int[] { m_poller, m_waitQueueEvent.getHandle() };
-                while (!Thread.interrupted()) {
-                  try {
-                    WPIUtilJNI.waitForObjects(handles);
-                  } catch (InterruptedException ex) {
-                    m_lock.lock();
-                    try {
-                      if (m_waitQueue) {
-                        m_waitQueue = false;
-                        m_waitQueueCond.signalAll();
-                      }
-                    } finally {
-                      m_lock.unlock();
-                    }
-                    Thread.currentThread().interrupt();
-                    // don't try to destroy poller, as its handle is likely no longer valid
-                    wasInterrupted = true;
-                    break;
-                  }
-                  for (NetworkTableEvent event :
-                      NetworkTablesJNI.readListenerQueue(m_inst, m_poller)) {
-                    Consumer<NetworkTableEvent> listener;
-                    m_lock.lock();
-                    try {
-                      listener = m_listeners.get(event.listener);
-                    } finally {
-                      m_lock.unlock();
-                    }
-                    if (listener != null) {
-                      try {
-                        listener.accept(event);
-                      } catch (Throwable throwable) {
-                        System.err.println(
-                            "Unhandled exception during listener callback: "
-                            + throwable.toString());
-                        throwable.printStackTrace();
-                      }
-                    }
-                  }
-                  m_lock.lock();
-                  try {
-                    if (m_waitQueue) {
-                      m_waitQueue = false;
-                      m_waitQueueCond.signalAll();
-                    }
-                  } finally {
-                    m_lock.unlock();
-                  }
-                }
-                m_lock.lock();
-                try {
-                  if (!wasInterrupted) {
-                    NetworkTablesJNI.destroyListenerPoller(m_poller);
-                  }
-                  m_poller = 0;
-                } finally {
-                  m_lock.unlock();
-                }
-              },
-              "NTListener");
-      m_thread.setDaemon(true);
-      m_thread.start();
-    }
-
-    boolean waitForQueue(double timeout) {
-      m_lock.lock();
-      try {
-        if (m_poller != 0) {
-          m_waitQueue = true;
-          m_waitQueueEvent.set();
-          while (m_waitQueue) {
-            try {
-              if (timeout < 0) {
-                m_waitQueueCond.await();
-              } else {
-                return m_waitQueueCond.await((long) (timeout * 1e9), TimeUnit.NANOSECONDS);
-              }
-            } catch (InterruptedException ex) {
-              Thread.currentThread().interrupt();
-              return true;
-            }
-          }
-        }
-      } finally {
-        m_lock.unlock();
-      }
-      return true;
-    }
-  }
-
-  private final ListenerStorage m_listeners = new ListenerStorage(this);
-
-  /**
-   * Remove a connection listener.
-   *
-   * @param listener Listener handle to remove
-   */
-  public void removeListener(int listener) {
-    m_listeners.remove(listener);
-  }
-
-  /**
-   * Wait for the listener queue to be empty. This is primarily useful for deterministic
-   * testing. This blocks until either the listener queue is empty (e.g. there are no
-   * more events that need to be passed along to callbacks or poll queues) or the timeout expires.
-   *
-   * @param timeout timeout, in seconds. Set to 0 for non-blocking behavior, or a negative value to
-   *     block indefinitely
-   * @return False if timed out, otherwise true.
-   */
-  public boolean waitForListenerQueue(double timeout) {
-    return m_listeners.waitForQueue(timeout);
-  }
-
-  /**
-   * Add a connection listener. The callback function is called asynchronously on a separate
-   * thread, so it's important to use synchronization or atomics when accessing any shared state
-   * from the callback function.
-   *
-   * @param immediateNotify Notify listener of all existing connections
-   * @param listener Listener to add
-   * @return Listener handle
-   */
-  public int addConnectionListener(
-      boolean immediateNotify, Consumer<NetworkTableEvent> listener) {
-    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kConnection);
-    if (immediateNotify) {
-      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
-    }
-    return m_listeners.add(m_handle, eventKinds, listener);
-  }
-
-  /**
-   * Add a time synchronization listener. The callback function is called asynchronously on a
-   * separate thread, so it's important to use synchronization or atomics when accessing any shared
-   * state from the callback function.
-   *
-   * @param immediateNotify Notify listener of current time synchronization value
-   * @param listener Listener to add
-   * @return Listener handle
-   */
-  public int addTimeSyncListener(
-      boolean immediateNotify, Consumer<NetworkTableEvent> listener) {
-    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kTimeSync);
-    if (immediateNotify) {
-      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
-    }
-    return m_listeners.add(m_handle, eventKinds, listener);
-  }
-
-  /**
-   * Add a listener for changes on a particular topic. The callback function is called
-   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
-   * accessing any shared state from the callback function.
-   *
-   * <p>This creates a corresponding internal subscriber with the lifetime of the
-   * listener.
-   *
-   * @param topic Topic
-   * @param eventKinds set of event kinds to listen to
-   * @param listener Listener function
-   * @return Listener handle
-   */
-  public int addListener(
-      Topic topic,
-      EnumSet<NetworkTableEvent.Kind> eventKinds,
-      Consumer<NetworkTableEvent> listener) {
-    if (topic.getInstance().getHandle() != m_handle) {
-      throw new IllegalArgumentException("topic is not from this instance");
-    }
-    return m_listeners.add(topic.getHandle(), eventKinds, listener);
-  }
-
-  /**
-   * Add a listener for changes on a subscriber. The callback function is called
-   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
-   * accessing any shared state from the callback function. This does NOT keep the subscriber
-   * active.
-   *
-   * @param subscriber Subscriber
-   * @param eventKinds set of event kinds to listen to
-   * @param listener Listener function
-   * @return Listener handle
-   */
-  public int addListener(
-      Subscriber subscriber,
-      EnumSet<NetworkTableEvent.Kind> eventKinds,
-      Consumer<NetworkTableEvent> listener) {
-    if (subscriber.getTopic().getInstance().getHandle() != m_handle) {
-      throw new IllegalArgumentException("subscriber is not from this instance");
-    }
-    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
-  }
-
-  /**
-   * Add a listener for changes on a subscriber. The callback function is called
-   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
-   * accessing any shared state from the callback function. This does NOT keep the subscriber
-   * active.
-   *
-   * @param subscriber Subscriber
-   * @param eventKinds set of event kinds to listen to
-   * @param listener Listener function
-   * @return Listener handle
-   */
-  public int addListener(
-      MultiSubscriber subscriber,
-      EnumSet<NetworkTableEvent.Kind> eventKinds,
-      Consumer<NetworkTableEvent> listener) {
-    if (subscriber.getInstance().getHandle() != m_handle) {
-      throw new IllegalArgumentException("subscriber is not from this instance");
-    }
-    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
-  }
-
-  /**
-   * Add a listener for changes on an entry. The callback function is called
-   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
-   * accessing any shared state from the callback function.
-   *
-   * @param entry Entry
-   * @param eventKinds set of event kinds to listen to
-   * @param listener Listener function
-   * @return Listener handle
-   */
-  public int addListener(
-      NetworkTableEntry entry,
-      EnumSet<NetworkTableEvent.Kind> eventKinds,
-      Consumer<NetworkTableEvent> listener) {
-    if (entry.getTopic().getInstance().getHandle() != m_handle) {
-      throw new IllegalArgumentException("entry is not from this instance");
-    }
-    return m_listeners.add(entry.getHandle(), eventKinds, listener);
-  }
-
-  /**
-   * Add a listener for changes to topics with names that start with any of the given
-   * prefixes. The callback function is called asynchronously on a separate thread, so it's
-   * important to use synchronization or atomics when accessing any shared state from the callback
-   * function.
-   *
-   * <p>This creates a corresponding internal subscriber with the lifetime of the
-   * listener.
-   *
-   * @param prefixes Topic name string prefixes
-   * @param eventKinds set of event kinds to listen to
-   * @param listener Listener function
-   * @return Listener handle
-   */
-  public int addListener(
-      String[] prefixes,
-      EnumSet<NetworkTableEvent.Kind> eventKinds,
-      Consumer<NetworkTableEvent> listener) {
-    return m_listeners.add(prefixes, eventKinds, listener);
-  }
-
-  /*
-   * Client/Server Functions
-   */
-
-  /**
-   * Get the current network mode.
-   *
-   * @return Enum set of NetworkMode.
-   */
-  public EnumSet<NetworkMode> getNetworkMode() {
-    int flags = NetworkTablesJNI.getNetworkMode(m_handle);
-    EnumSet<NetworkMode> rv = EnumSet.noneOf(NetworkMode.class);
-    for (NetworkMode mode : NetworkMode.values()) {
-      if ((flags & mode.getValue()) != 0) {
-        rv.add(mode);
-      }
-    }
-    return rv;
-  }
-
-  /**
-   * Starts local-only operation. Prevents calls to startServer or startClient from taking effect.
-   * Has no effect if startServer or startClient has already been called.
-   */
-  public void startLocal() {
-    NetworkTablesJNI.startLocal(m_handle);
-  }
-
-  /**
-   * Stops local-only operation. startServer or startClient can be called after this call to start
-   * a server or client.
-   */
-  public void stopLocal() {
-    NetworkTablesJNI.stopLocal(m_handle);
-  }
-
-  /**
-   * Starts a server using the networktables.json as the persistent file, using the default
-   * listening address and port.
-   */
-  public void startServer() {
-    startServer("networktables.json");
-  }
-
-  /**
-   * Starts a server using the specified persistent filename, using the default listening address
-   * and port.
-   *
-   * @param persistFilename the name of the persist file to use
-   */
-  public void startServer(String persistFilename) {
-    startServer(persistFilename, "");
-  }
-
-  /**
-   * Starts a server using the specified filename and listening address, using the default port.
-   *
-   * @param persistFilename the name of the persist file to use
-   * @param listenAddress the address to listen on, or empty to listen on any address
-   */
-  public void startServer(String persistFilename, String listenAddress) {
-    startServer(persistFilename, listenAddress, kDefaultPort3, kDefaultPort4);
-  }
-
-  /**
-   * Starts a server using the specified filename, listening address, and port.
-   *
-   * @param persistFilename the name of the persist file to use
-   * @param listenAddress the address to listen on, or empty to listen on any address
-   * @param port3 port to communicate over (NT3)
-   */
-  public void startServer(String persistFilename, String listenAddress, int port3) {
-    startServer(persistFilename, listenAddress, port3, kDefaultPort4);
-  }
-
-  /**
-   * Starts a server using the specified filename, listening address, and port.
-   *
-   * @param persistFilename the name of the persist file to use
-   * @param listenAddress the address to listen on, or empty to listen on any address
-   * @param port3 port to communicate over (NT3)
-   * @param port4 port to communicate over (NT4)
-   */
-  public void startServer(String persistFilename, String listenAddress, int port3, int port4) {
-    NetworkTablesJNI.startServer(m_handle, persistFilename, listenAddress, port3, port4);
-  }
-
-  /** Stops the server if it is running. */
-  public void stopServer() {
-    NetworkTablesJNI.stopServer(m_handle);
-  }
-
-  /**
-   * Starts a NT3 client. Use SetServer or SetServerTeam to set the server name and port.
-   *
-   * @param identity network identity to advertise (cannot be empty string)
-   */
-  public void startClient3(String identity) {
-    NetworkTablesJNI.startClient3(m_handle, identity);
-  }
-
-  /**
-   * Starts a NT4 client. Use SetServer or SetServerTeam to set the server name and port.
-   *
-   * @param identity network identity to advertise (cannot be empty string)
-   */
-  public void startClient4(String identity) {
-    NetworkTablesJNI.startClient4(m_handle, identity);
-  }
-
-  /** Stops the client if it is running. */
-  public void stopClient() {
-    NetworkTablesJNI.stopClient(m_handle);
-  }
-
-  /**
-   * Sets server address and port for client (without restarting client). Changes the port to the
-   * default port.
-   *
-   * @param serverName server name
-   */
-  public void setServer(String serverName) {
-    setServer(serverName, 0);
-  }
-
-  /**
-   * Sets server address and port for client (without restarting client).
-   *
-   * @param serverName server name
-   * @param port port to communicate over (0=default)
-   */
-  public void setServer(String serverName, int port) {
-    NetworkTablesJNI.setServer(m_handle, serverName, port);
-  }
-
-  /**
-   * Sets server addresses and port for client (without restarting client). Changes the port to the
-   * default port. The client will attempt to connect to each server in round robin fashion.
-   *
-   * @param serverNames array of server names
-   */
-  public void setServer(String[] serverNames) {
-    setServer(serverNames, 0);
-  }
-
-  /**
-   * Sets server addresses and port for client (without restarting client). The client will attempt
-   * to connect to each server in round robin fashion.
-   *
-   * @param serverNames array of server names
-   * @param port port to communicate over (0=default)
-   */
-  public void setServer(String[] serverNames, int port) {
-    int[] ports = new int[serverNames.length];
-    for (int i = 0; i < serverNames.length; i++) {
-      ports[i] = port;
-    }
-    setServer(serverNames, ports);
-  }
-
-  /**
-   * Sets server addresses and ports for client (without restarting client). The client will
-   * attempt to connect to each server in round robin fashion.
-   *
-   * @param serverNames array of server names
-   * @param ports array of port numbers (0=default)
-   */
-  public void setServer(String[] serverNames, int[] ports) {
-    NetworkTablesJNI.setServer(m_handle, serverNames, ports);
-  }
-
-  /**
-   * Sets server addresses and port for client (without restarting client). Changes the port to the
-   * default port. The client will attempt to connect to each server in round robin fashion.
-   *
-   * @param team team number
-   */
-  public void setServerTeam(int team) {
-    setServerTeam(team, 0);
-  }
-
-  /**
-   * Sets server addresses and port for client (without restarting client). Connects using commonly
-   * known robot addresses for the specified team.
-   *
-   * @param team team number
-   * @param port port to communicate over (0=default)
-   */
-  public void setServerTeam(int team, int port) {
-    NetworkTablesJNI.setServerTeam(m_handle, team, port);
-  }
-
-  /**
-   * Disconnects the client if it's running and connected. This will automatically start
-   * reconnection attempts to the current server list.
-   */
-  public void disconnect() {
-    NetworkTablesJNI.disconnect(m_handle);
-  }
-
-  /**
-   * Starts requesting server address from Driver Station. This connects to the Driver Station
-   * running on localhost to obtain the server IP address, and connects with the default port.
-   */
-  public void startDSClient() {
-    startDSClient(0);
-  }
-
-  /**
-   * Starts requesting server address from Driver Station. This connects to the Driver Station
-   * running on localhost to obtain the server IP address.
-   *
-   * @param port server port to use in combination with IP from DS (0=default)
-   */
-  public void startDSClient(int port) {
-    NetworkTablesJNI.startDSClient(m_handle, port);
-  }
-
-  /** Stops requesting server address from Driver Station. */
-  public void stopDSClient() {
-    NetworkTablesJNI.stopDSClient(m_handle);
-  }
-
-  /**
-   * Flushes all updated values immediately to the local client/server. This does not flush to the
-   * network.
-   */
-  public void flushLocal() {
-    NetworkTablesJNI.flushLocal(m_handle);
-  }
-
-  /**
-   * Flushes all updated values immediately to the network. Note: This is rate-limited to protect
-   * the network from flooding. This is primarily useful for synchronizing network updates with
-   * user code.
-   */
-  public void flush() {
-    NetworkTablesJNI.flush(m_handle);
-  }
-
-  /**
-   * Gets information on the currently established network connections. If operating as a client,
-   * this will return either zero or one values.
-   *
-   * @return array of connection information
-   */
-  public ConnectionInfo[] getConnections() {
-    return NetworkTablesJNI.getConnections(m_handle);
-  }
-
-  /**
-   * Return whether or not the instance is connected to another node.
-   *
-   * @return True if connected.
-   */
-  public boolean isConnected() {
-    return NetworkTablesJNI.isConnected(m_handle);
-  }
-
-  /**
-   * Get the time offset between server time and local time. Add this value to local time to get
-   * the estimated equivalent server time. In server mode, this always returns 0. In client mode,
-   * this returns the time offset only if the client and server are connected and have exchanged
-   * synchronization messages. Note the time offset may change over time as it is periodically
-   * updated; to receive updates as events, add a listener to the "time sync" event.
-   *
-   * @return Time offset in microseconds (optional)
-   */
-  public OptionalLong getServerTimeOffset() {
-    return NetworkTablesJNI.getServerTimeOffset(m_handle);
-  }
-
-  /**
-   * Starts logging entry changes to a DataLog.
-   *
-   * @param log data log object; lifetime must extend until StopEntryDataLog is called or the
-   *     instance is destroyed
-   * @param prefix only store entries with names that start with this prefix; the prefix is not
-   *     included in the data log entry name
-   * @param logPrefix prefix to add to data log entry names
-   * @return Data logger handle
-   */
-  public int startEntryDataLog(DataLog log, String prefix, String logPrefix) {
-    return NetworkTablesJNI.startEntryDataLog(m_handle, log, prefix, logPrefix);
-  }
-
-  /**
-   * Stops logging entry changes to a DataLog.
-   *
-   * @param logger data logger handle
-   */
-  public static void stopEntryDataLog(int logger) {
-    NetworkTablesJNI.stopEntryDataLog(logger);
-  }
-
-  /**
-   * Starts logging connection changes to a DataLog.
-   *
-   * @param log data log object; lifetime must extend until StopConnectionDataLog is called or the
-   *     instance is destroyed
-   * @param name data log entry name
-   * @return Data logger handle
-   */
-  public int startConnectionDataLog(DataLog log, String name) {
-    return NetworkTablesJNI.startConnectionDataLog(m_handle, log, name);
-  }
-
-  /**
-   * Stops logging connection changes to a DataLog.
-   *
-   * @param logger data logger handle
-   */
-  public static void stopConnectionDataLog(int logger) {
-    NetworkTablesJNI.stopConnectionDataLog(logger);
-  }
-
-  /**
-   * Add logger callback function. By default, log messages are sent to stderr; this function sends
-   * log messages with the specified levels to the provided callback function instead. The callback
-   * function will only be called for log messages with level greater than or equal to minLevel and
-   * less than or equal to maxLevel; messages outside this range will be silently ignored.
-   *
-   * @param minLevel minimum log level
-   * @param maxLevel maximum log level
-   * @param func callback function
-   * @return Listener handle
-   */
-  public int addLogger(int minLevel, int maxLevel, Consumer<NetworkTableEvent> func) {
-    return m_listeners.addLogger(minLevel, maxLevel, func);
-  }
-
-  /**
-   * Returns whether there is a data schema already registered with the given name that this
-   * instance has published. This does NOT perform a check as to whether the schema has already
-   * been published by another node on the network.
-   *
-   * @param name Name (the string passed as the data type for topics using this schema)
-   * @return True if schema already registered
-   */
-  public boolean hasSchema(String name) {
-    return m_schemas.containsKey("/.schema/" + name);
-  }
-
-  /**
-   * Registers a data schema. Data schemas provide information for how a certain data type string
-   * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g.
-   * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas
-   * are published just like normal topics, with the name being generated from the provided name:
-   * "/.schema/name". Duplicate calls to this function with the same name are silently ignored.
-   *
-   * @param name Name (the string passed as the data type for topics using this schema)
-   * @param type Type of schema (e.g. "protobuf", "struct", etc)
-   * @param schema Schema data
-   */
-  public void addSchema(String name, String type, byte[] schema) {
-    m_schemas.computeIfAbsent("/.schema/" + name, k -> {
-      RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}");
-      pub.setDefault(schema);
-      return pub;
-    });
-  }
-
-  /**
-   * Registers a data schema. Data schemas provide information for how a certain data type string
-   * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g.
-   * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas
-   * are published just like normal topics, with the name being generated from the provided name:
-   * "/.schema/name". Duplicate calls to this function with the same name are silently ignored.
-   *
-   * @param name Name (the string passed as the data type for topics using this schema)
-   * @param type Type of schema (e.g. "protobuf", "struct", etc)
-   * @param schema Schema data
-   */
-  public void addSchema(String name, String type, String schema) {
-    m_schemas.computeIfAbsent("/.schema/" + name, k -> {
-      RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}");
-      pub.setDefault(StandardCharsets.UTF_8.encode(schema));
-      return pub;
-    });
-  }
-
-  /**
-   * Registers a protobuf schema. Duplicate calls to this function with the same name are silently
-   * ignored.
-   *
-   * @param proto protobuf serialization object
-   */
-  public void addSchema(Protobuf<?, ?> proto) {
-    proto.forEachDescriptor(
-        this::hasSchema,
-        (typeString, schema) -> addSchema(typeString, "proto:FileDescriptorProto", schema));
-  }
-
-  /**
-   * Registers a struct schema. Duplicate calls to this function with the same name are silently
-   * ignored.
-   *
-   * @param struct struct serialization object
-   */
-  public void addSchema(Struct<?> struct) {
-    addSchemaImpl(struct, new HashSet<>());
-  }
-
-  @Override
-  public boolean equals(Object other) {
-    if (other == this) {
-      return true;
-    }
-    if (!(other instanceof NetworkTableInstance)) {
-      return false;
-    }
-
-    return m_handle == ((NetworkTableInstance) other).m_handle;
-  }
-
-  @Override
-  public int hashCode() {
-    return m_handle;
-  }
-
-  private void addSchemaImpl(Struct<?> struct, Set<String> seen) {
-    String typeString = struct.getTypeString();
-    if (hasSchema(typeString)) {
-      return;
-    }
-    if (!seen.add(typeString)) {
-      throw new UnsupportedOperationException(typeString + ": circular reference with " + seen);
-    }
-    addSchema(typeString, "structschema", struct.getSchema());
-    for (Struct<?> inner : struct.getNested()) {
-      addSchemaImpl(inner, seen);
-    }
-    seen.remove(typeString);
-  }
-
-  private boolean m_owned;
-  private int m_handle;
-  private final ConcurrentMap<String, RawPublisher> m_schemas = new ConcurrentHashMap<>();
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/NetworkTableValue.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/NetworkTableValue.java.jinja
deleted file mode 100644
index d2c8d11..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/NetworkTableValue.java.jinja
+++ /dev/null
@@ -1,248 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import java.util.Objects;
-
-/** A network table entry value. */
-@SuppressWarnings({"UnnecessaryParentheses", "PMD.MethodReturnsInternalArray"})
-public final class NetworkTableValue {
-  NetworkTableValue(NetworkTableType type, Object value, long time, long serverTime) {
-    m_type = type;
-    m_value = value;
-    m_time = time;
-    m_serverTime = serverTime;
-  }
-
-  NetworkTableValue(NetworkTableType type, Object value, long time) {
-    this(type, value, time, time == 0 ? 0 : 1);
-  }
-
-  NetworkTableValue(NetworkTableType type, Object value) {
-    this(type, value, NetworkTablesJNI.now(), 1);
-  }
-
-  NetworkTableValue(int type, Object value, long time, long serverTime) {
-    this(NetworkTableType.getFromInt(type), value, time, serverTime);
-  }
-
-  /**
-   * Get the data type.
-   *
-   * @return The type.
-   */
-  public NetworkTableType getType() {
-    return m_type;
-  }
-
-  /**
-   * Get the data value stored.
-   *
-   * @return The type.
-   */
-  public Object getValue() {
-    return m_value;
-  }
-
-  /**
-   * Get the creation time of the value in local time.
-   *
-   * @return The time, in the units returned by NetworkTablesJNI.now().
-   */
-  public long getTime() {
-    return m_time;
-  }
-
-  /**
-   * Get the creation time of the value in server time.
-   *
-   * @return The server time.
-   */
-  public long getServerTime() {
-    return m_serverTime;
-  }
-
-  /*
-   * Type Checkers
-   */
-
-  /**
-   * Determine if entry value contains a value or is unassigned.
-   *
-   * @return True if the entry value contains a value.
-   */
-  public boolean isValid() {
-    return m_type != NetworkTableType.kUnassigned;
-  }
-{% for t in types %}
-  /**
-   * Determine if entry value contains a {{ t.java.ValueType }}.
-   *
-   * @return True if the entry value is of {{ t.java.ValueType }} type.
-   */
-  public boolean is{{ t.TypeName }}() {
-    return m_type == NetworkTableType.k{{ t.TypeName }};
-  }
-{% endfor %}
-  /*
-   * Type-Safe Getters
-   */
-{% for t in types %}
-  /**
-   * Get the {{ t.java.ValueType }} value.
-   *
-   * @return The {{ t.java.ValueType }} value.
-   * @throws ClassCastException if the entry value is not of {{ t.java.ValueType }} type.
-   */
-  public {{ t.java.ValueType }} get{{ t.TypeName }}() {
-    if (m_type != NetworkTableType.k{{ t.TypeName }}) {
-      throw new ClassCastException("cannot convert " + m_type + " to {{ t.java.ValueType }}");
-    }
-    return {{ t.java.FromStorageBegin }}m_value{{ t.java.FromStorageEnd }};
-  }
-{% endfor %}
-  /*
-   * Factory functions.
-   */
-{% for t in types %}
-  /**
-   * Creates a {{ t.java.ValueType }} value.
-   *
-   * @param value the value
-   * @return The entry value
-   */
-  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.ValueType }} value) {
-    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, {{ t.java.ToWrapObject }}(value));
-  }
-
-  /**
-   * Creates a {{ t.java.ValueType }} value.
-   *
-   * @param value the value
-   * @param time the creation time to use (instead of the current time)
-   * @return The entry value
-   */
-  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.ValueType }} value, long time) {
-    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, {{ t.java.ToWrapObject }}(value), time);
-  }
-{% if t.java.WrapValueType %}
-  /**
-   * Creates a {{ t.java.ValueType }} value.
-   *
-   * @param value the value
-   * @return The entry value
-   */
-  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.WrapValueType }} value) {
-    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, toNative{{ t.TypeName }}(value));
-  }
-
-  /**
-   * Creates a {{ t.java.ValueType }} value.
-   *
-   * @param value the value
-   * @param time the creation time to use (instead of the current time)
-   * @return The entry value
-   */
-  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.WrapValueType }} value, long time) {
-    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, toNative{{ t.TypeName }}(value), time);
-  }
-{% endif -%}
-{% endfor %}
-  @Override
-  public boolean equals(Object other) {
-    if (other == this) {
-      return true;
-    }
-    if (!(other instanceof NetworkTableValue)) {
-      return false;
-    }
-    NetworkTableValue ntOther = (NetworkTableValue) other;
-    return m_type == ntOther.m_type && m_value.equals(ntOther.m_value);
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_type, m_value);
-  }
-
-  // arraycopy() doesn't know how to unwrap boxed values; this is a false positive in PMD
-  // (see https://sourceforge.net/p/pmd/bugs/804/)
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static boolean[] toNativeBooleanArray(Boolean[] arr) {
-    boolean[] out = new boolean[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i];
-    }
-    return out;
-  }
-
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static double[] toNativeDoubleArray(Number[] arr) {
-    double[] out = new double[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i].doubleValue();
-    }
-    return out;
-  }
-
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static long[] toNativeIntegerArray(Number[] arr) {
-    long[] out = new long[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i].longValue();
-    }
-    return out;
-  }
-
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static float[] toNativeFloatArray(Number[] arr) {
-    float[] out = new float[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i].floatValue();
-    }
-    return out;
-  }
-
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static Boolean[] fromNativeBooleanArray(boolean[] arr) {
-    Boolean[] out = new Boolean[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i];
-    }
-    return out;
-  }
-
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static Long[] fromNativeIntegerArray(long[] arr) {
-    Long[] out = new Long[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i];
-    }
-    return out;
-  }
-
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static Float[] fromNativeFloatArray(float[] arr) {
-    Float[] out = new Float[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i];
-    }
-    return out;
-  }
-
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  static Double[] fromNativeDoubleArray(double[] arr) {
-    Double[] out = new Double[arr.length];
-    for (int i = 0; i < arr.length; i++) {
-      out[i] = arr[i];
-    }
-    return out;
-  }
-
-  private NetworkTableType m_type;
-  private Object m_value;
-  private long m_time;
-  private long m_serverTime;
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/NetworkTablesJNI.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/NetworkTablesJNI.java.jinja
deleted file mode 100644
index 6ff9c16..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/NetworkTablesJNI.java.jinja
+++ /dev/null
@@ -1,355 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import edu.wpi.first.util.RuntimeLoader;
-import edu.wpi.first.util.datalog.DataLog;
-import java.io.IOException;
-import java.nio.ByteBuffer;
-import java.util.EnumSet;
-import java.util.OptionalLong;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-public final class NetworkTablesJNI {
-  static boolean libraryLoaded = false;
-  static RuntimeLoader<NetworkTablesJNI> loader = null;
-
-  public static class Helper {
-    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
-
-    public static boolean getExtractOnStaticLoad() {
-      return extractOnStaticLoad.get();
-    }
-
-    public static void setExtractOnStaticLoad(boolean load) {
-      extractOnStaticLoad.set(load);
-    }
-  }
-
-  static {
-    if (Helper.getExtractOnStaticLoad()) {
-      try {
-        loader =
-            new RuntimeLoader<>(
-                "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
-        loader.loadLibrary();
-      } catch (IOException ex) {
-        ex.printStackTrace();
-        System.exit(1);
-      }
-      libraryLoaded = true;
-    }
-  }
-
-  /**
-   * Force load the library.
-   *
-   * @throws IOException if the library fails to load
-   */
-  public static synchronized void forceLoad() throws IOException {
-    if (libraryLoaded) {
-      return;
-    }
-    loader =
-        new RuntimeLoader<>(
-            "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
-    loader.loadLibrary();
-    libraryLoaded = true;
-  }
-
-  private static PubSubOptions buildOptions(PubSubOption... options) {
-    if (options.length == 0) {
-      return null;  // optimize common case (JNI checks for null)
-    }
-    return new PubSubOptions(options);
-  }
-
-  public static native int getDefaultInstance();
-
-  public static native int createInstance();
-
-  public static native void destroyInstance(int inst);
-
-  public static native int getInstanceFromHandle(int handle);
-
-  private static native int getEntryImpl(
-      int topic, int type, String typeStr, PubSubOptions options);
-
-  public static native int getEntry(int inst, String key);
-
-  public static int getEntry(
-      int topic, int type, String typeStr, PubSubOptions options) {
-    return getEntryImpl(topic, type, typeStr, options);
-  }
-
-  public static int getEntry(
-      int topic, int type, String typeStr, PubSubOption... options) {
-    return getEntryImpl(topic, type, typeStr, buildOptions(options));
-  }
-
-  public static native String getEntryName(int entry);
-
-  public static native long getEntryLastChange(int entry);
-
-  public static native int getType(int entry);
-
-  /* Topic functions */
-
-  public static native int[] getTopics(int inst, String prefix, int types);
-
-  public static native int[] getTopicsStr(int inst, String prefix, String[] types);
-
-  public static native TopicInfo[] getTopicInfos(
-      NetworkTableInstance instObject, int inst, String prefix, int types);
-
-  public static native TopicInfo[] getTopicInfosStr(
-      NetworkTableInstance instObject, int inst, String prefix, String[] types);
-
-  public static native int getTopic(int inst, String name);
-
-  public static native String getTopicName(int topic);
-
-  public static native int getTopicType(int topic);
-
-  public static native void setTopicPersistent(int topic, boolean value);
-
-  public static native boolean getTopicPersistent(int topic);
-
-  public static native void setTopicRetained(int topic, boolean value);
-
-  public static native boolean getTopicRetained(int topic);
-
-  public static native String getTopicTypeString(int topic);
-
-  public static native boolean getTopicExists(int topic);
-
-  public static native String getTopicProperty(int topic, String name);
-
-  public static native void setTopicProperty(int topic, String name, String value);
-
-  public static native void deleteTopicProperty(int topic, String name);
-
-  public static native String getTopicProperties(int topic);
-
-  public static native void setTopicProperties(int topic, String properties);
-
-  public static native int subscribe(
-      int topic, int type, String typeStr, PubSubOptions options);
-
-  public static int subscribe(
-      int topic, int type, String typeStr, PubSubOption... options) {
-    return subscribe(topic, type, typeStr, buildOptions(options));
-  }
-
-  public static native void unsubscribe(int sub);
-
-  public static native int publish(
-      int topic, int type, String typeStr, PubSubOptions options);
-
-  public static int publish(
-      int topic, int type, String typeStr, PubSubOption... options) {
-    return publish(topic, type, typeStr, buildOptions(options));
-  }
-
-  public static native int publishEx(
-      int topic, int type, String typeStr, String properties, PubSubOptions options);
-
-  public static int publishEx(
-      int topic, int type, String typeStr, String properties, PubSubOption... options) {
-    return publishEx(topic, type, typeStr, properties, buildOptions(options));
-  }
-
-  public static native void unpublish(int pubentry);
-
-  public static native void releaseEntry(int entry);
-
-  public static native void release(int pubsubentry);
-
-  public static native int getTopicFromHandle(int pubsubentry);
-
-  public static native int subscribeMultiple(int inst, String[] prefixes, PubSubOptions options);
-
-  public static int subscribeMultiple(int inst, String[] prefixes, PubSubOption... options) {
-    return subscribeMultiple(inst, prefixes, buildOptions(options));
-  }
-
-  public static native void unsubscribeMultiple(int sub);
-{% for t in types %}
-  public static native Timestamped{{ t.TypeName }} getAtomic{{ t.TypeName }}(
-      int subentry, {{ t.java.ValueType }} defaultValue);
-
-  public static native Timestamped{{ t.TypeName }}[] readQueue{{ t.TypeName }}(int subentry);
-
-  public static native {{ t.java.ValueType }}[] readQueueValues{{ t.TypeName }}(int subentry);
-{% if t.TypeName == "Raw" %}
-  public static boolean setRaw(int entry, long time, byte[] value) {
-    return setRaw(entry, time, value, 0, value.length);
-  }
-
-  public static native boolean setRaw(int entry, long time, byte[] value, int start, int len);
-
-  public static boolean setRaw(int entry, long time, ByteBuffer value) {
-    int pos = value.position();
-    return setRaw(entry, time, value, pos, value.capacity() - pos);
-  }
-
-  public static boolean setRaw(int entry, long time, ByteBuffer value, int start, int len) {
-    if (value.isDirect()) {
-      if (start < 0) {
-        throw new IndexOutOfBoundsException("start must be >= 0");
-      }
-      if (len < 0) {
-        throw new IndexOutOfBoundsException("len must be >= 0");
-      }
-      if ((start + len) > value.capacity()) {
-        throw new IndexOutOfBoundsException("start + len must be smaller than buffer capacity");
-      }
-      return setRawBuffer(entry, time, value, start, len);
-    } else if (value.hasArray()) {
-      return setRaw(entry, time, value.array(), value.arrayOffset() + start, len);
-    } else {
-      throw new UnsupportedOperationException("ByteBuffer must be direct or have a backing array");
-    }
-  }
-
-  private static native boolean setRawBuffer(int entry, long time, ByteBuffer value, int start, int len);
-{% else %}
-  public static native boolean set{{ t.TypeName }}(int entry, long time, {{ t.java.ValueType }} value);
-{% endif %}
-  public static native {{ t.java.ValueType }} get{{ t.TypeName }}(int entry, {{ t.java.ValueType }} defaultValue);
-{% if t.TypeName == "Raw" %}
-  public static boolean setDefaultRaw(int entry, long time, byte[] defaultValue) {
-    return setDefaultRaw(entry, time, defaultValue, 0, defaultValue.length);
-  }
-
-  public static native boolean setDefaultRaw(int entry, long time, byte[] defaultValue, int start, int len);
-
-  public static boolean setDefaultRaw(int entry, long time, ByteBuffer defaultValue) {
-    int pos = defaultValue.position();
-    return setDefaultRaw(entry, time, defaultValue, pos, defaultValue.limit() - pos);
-  }
-
-  public static boolean setDefaultRaw(int entry, long time, ByteBuffer defaultValue, int start, int len) {
-    if (defaultValue.isDirect()) {
-      if (start < 0) {
-        throw new IndexOutOfBoundsException("start must be >= 0");
-      }
-      if (len < 0) {
-        throw new IndexOutOfBoundsException("len must be >= 0");
-      }
-      if ((start + len) > defaultValue.capacity()) {
-        throw new IndexOutOfBoundsException("start + len must be smaller than buffer capacity");
-      }
-      return setDefaultRawBuffer(entry, time, defaultValue, start, len);
-    } else if (defaultValue.hasArray()) {
-      return setDefaultRaw(entry, time, defaultValue.array(), defaultValue.arrayOffset() + start, len);
-    } else {
-      throw new UnsupportedOperationException("ByteBuffer must be direct or have a backing array");
-    }
-  }
-
-  private static native boolean setDefaultRawBuffer(int entry, long time, ByteBuffer defaultValue, int start, int len);
-{% else %}
-  public static native boolean setDefault{{ t.TypeName }}(int entry, long time, {{ t.java.ValueType }} defaultValue);
-{% endif %}
-{% endfor %}
-  public static native NetworkTableValue[] readQueueValue(int subentry);
-
-  public static native NetworkTableValue getValue(int entry);
-
-  public static native void setEntryFlags(int entry, int flags);
-
-  public static native int getEntryFlags(int entry);
-
-  public static native TopicInfo getTopicInfo(NetworkTableInstance inst, int topic);
-
-  public static native int createListenerPoller(int inst);
-
-  public static native void destroyListenerPoller(int poller);
-
-  private static int kindsToMask(EnumSet<NetworkTableEvent.Kind> kinds) {
-    int mask = 0;
-    for (NetworkTableEvent.Kind kind : kinds) {
-      mask |= kind.getValue();
-    }
-    return mask;
-  }
-
-  public static int addListener(int poller, String[] prefixes, EnumSet<NetworkTableEvent.Kind> kinds) {
-    return addListener(poller, prefixes, kindsToMask(kinds));
-  }
-
-  public static int addListener(int poller, int handle, EnumSet<NetworkTableEvent.Kind> kinds) {
-    return addListener(poller, handle, kindsToMask(kinds));
-  }
-
-  public static native int addListener(int poller, String[] prefixes, int mask);
-
-  public static native int addListener(int poller, int handle, int mask);
-
-  public static native NetworkTableEvent[] readListenerQueue(
-      NetworkTableInstance inst, int poller);
-
-  public static native void removeListener(int listener);
-
-  public static native int getNetworkMode(int inst);
-
-  public static native void startLocal(int inst);
-
-  public static native void stopLocal(int inst);
-
-  public static native void startServer(
-      int inst, String persistFilename, String listenAddress, int port3, int port4);
-
-  public static native void stopServer(int inst);
-
-  public static native void startClient3(int inst, String identity);
-
-  public static native void startClient4(int inst, String identity);
-
-  public static native void stopClient(int inst);
-
-  public static native void setServer(int inst, String serverName, int port);
-
-  public static native void setServer(int inst, String[] serverNames, int[] ports);
-
-  public static native void setServerTeam(int inst, int team, int port);
-
-  public static native void disconnect(int inst);
-
-  public static native void startDSClient(int inst, int port);
-
-  public static native void stopDSClient(int inst);
-
-  public static native void flushLocal(int inst);
-
-  public static native void flush(int inst);
-
-  public static native ConnectionInfo[] getConnections(int inst);
-
-  public static native boolean isConnected(int inst);
-
-  public static native OptionalLong getServerTimeOffset(int inst);
-
-  public static native long now();
-
-  private static native int startEntryDataLog(int inst, long log, String prefix, String logPrefix);
-
-  public static int startEntryDataLog(int inst, DataLog log, String prefix, String logPrefix) {
-    return startEntryDataLog(inst, log.getImpl(), prefix, logPrefix);
-  }
-
-  public static native void stopEntryDataLog(int logger);
-
-  private static native int startConnectionDataLog(int inst, long log, String name);
-
-  public static int startConnectionDataLog(int inst, DataLog log, String name) {
-    return startConnectionDataLog(inst, log.getImpl(), name);
-  }
-
-  public static native void stopConnectionDataLog(int logger);
-
-  public static native int addLogger(int poller, int minLevel, int maxLevel);
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/Publisher.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/Publisher.java.jinja
deleted file mode 100644
index a403d91..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/Publisher.java.jinja
+++ /dev/null
@@ -1,170 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-{% if TypeName == "Raw" %}
-import java.nio.ByteBuffer;
-{% endif -%}
-import {{ java.ConsumerFunctionPackage|default('java.util.function') }}.{{ java.FunctionTypePrefix }}Consumer;
-
-/** NetworkTables {{ TypeName }} publisher. */
-public interface {{ TypeName }}Publisher extends Publisher, {{ java.FunctionTypePrefix }}Consumer{{ java.FunctionTypeSuffix }} {
-  /**
-   * Get the corresponding topic.
-   *
-   * @return Topic
-   */
-  @Override
-  {{ TypeName }}Topic getTopic();
-
-  /**
-   * Publish a new value using current NT time.
-   *
-   * @param value value to publish
-   */
-  default void set({{ java.ValueType }} value) {
-    set(value, 0);
-  }
-
-{% if TypeName == "Raw" %}
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param time timestamp; 0 indicates current NT time should be used
-   */
-  default void set(byte[] value, long time) {
-    set(value, 0, value.length, time);
-  }
-
-  /**
-   * Publish a new value using current NT time.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   */
-  default void set(byte[] value, int start, int len) {
-    set(value, start, len, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   * @param time timestamp; 0 indicates current NT time should be used
-   */
-  void set(byte[] value, int start, int len, long time);
-
-  /**
-   * Publish a new value using current NT time.
-   *
-   * @param value value to publish; will send from value.position() to value.limit()
-   */
-  default void set(ByteBuffer value) {
-    set(value, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish; will send from value.position() to value.limit()
-   * @param time timestamp; 0 indicates current NT time should be used
-   */
-  default void set(ByteBuffer value, long time) {
-    int pos = value.position();
-    set(value, pos, value.limit() - pos, time);
-  }
-
-  /**
-   * Publish a new value using current NT time.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   */
-  default void set(ByteBuffer value, int start, int len) {
-    set(value, start, len, 0);
-  }
-
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   * @param time timestamp; 0 indicates current NT time should be used
-   */
-  void set(ByteBuffer value, int start, int len, long time);
-
-  /**
-   * Publish a default value.
-   * On reconnect, a default value will never be used in preference to a
-   * published value.
-   *
-   * @param value value
-   */
-  default void setDefault(byte[] value) {
-    setDefault(value, 0, value.length);
-  }
-
-  /**
-   * Publish a default value.
-   * On reconnect, a default value will never be used in preference to a
-   * published value.
-   *
-   * @param value value
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.length - start)
-   */
-  void setDefault(byte[] value, int start, int len);
-
-  /**
-   * Publish a default value.
-   * On reconnect, a default value will never be used in preference to a
-   * published value.
-   *
-   * @param value value; will send from value.position() to value.limit()
-   */
-  default void setDefault(ByteBuffer value) {
-    int pos = value.position();
-    setDefault(value, pos, value.limit() - pos);
-  }
-
-  /**
-   * Publish a default value.
-   * On reconnect, a default value will never be used in preference to a
-   * published value.
-   *
-   * @param value value
-   * @param start Start position of data (in buffer)
-   * @param len Length of data (must be less than or equal to value.capacity() - start)
-   */
-  void setDefault(ByteBuffer value, int start, int len);
-{% else %}
-  /**
-   * Publish a new value.
-   *
-   * @param value value to publish
-   * @param time timestamp; 0 indicates current NT time should be used
-   */
-  void set({{ java.ValueType }} value, long time);
-
-  /**
-   * Publish a default value.
-   * On reconnect, a default value will never be used in preference to a
-   * published value.
-   *
-   * @param value value
-   */
-  void setDefault({{ java.ValueType }} value);
-{% endif %}
-  @Override
-  default void accept({{ java.ValueType }} value) {
-    set(value);
-  }
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/Subscriber.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/Subscriber.java.jinja
deleted file mode 100644
index 0ea09a3..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/Subscriber.java.jinja
+++ /dev/null
@@ -1,83 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-import {{ java.SupplierFunctionPackage|default('java.util.function') }}.{{ java.FunctionTypePrefix }}Supplier;
-
-/** NetworkTables {{ TypeName }} subscriber. */
-@SuppressWarnings("PMD.MissingOverride")
-public interface {{ TypeName }}Subscriber extends Subscriber, {{ java.FunctionTypePrefix }}Supplier{{ java.FunctionTypeSuffix }} {
-  /**
-   * Get the corresponding topic.
-   *
-   * @return Topic
-   */
-  @Override
-  {{ TypeName }}Topic getTopic();
-
-  /**
-   * Get the last published value.
-   * If no value has been published, returns the stored default value.
-   *
-   * @return value
-   */
-  {{ java.ValueType }} get();
-
-  /**
-   * Get the last published value.
-   * If no value has been published, returns the passed defaultValue.
-   *
-   * @param defaultValue default value to return if no value has been published
-   * @return value
-   */
-  {{ java.ValueType }} get({{ java.ValueType }} defaultValue);
-{% if java.FunctionTypePrefix %}
-  @Override
-  default {{ java.ValueType }} getAs{{ java.FunctionTypePrefix }}() {
-    return get();
-  }
-{% endif %}
-  /**
-   * Get the last published value along with its timestamp
-   * If no value has been published, returns the stored default value and a
-   * timestamp of 0.
-   *
-   * @return timestamped value
-   */
-  Timestamped{{ TypeName }} getAtomic();
-
-  /**
-   * Get the last published value along with its timestamp
-   * If no value has been published, returns the passed defaultValue and a
-   * timestamp of 0.
-   *
-   * @param defaultValue default value to return if no value has been published
-   * @return timestamped value
-   */
-  Timestamped{{ TypeName }} getAtomic({{ java.ValueType }} defaultValue);
-
-  /**
-   * Get an array of all value changes since the last call to readQueue.
-   * Also provides a timestamp for each value.
-   *
-   * <p>The "poll storage" subscribe option can be used to set the queue
-   * depth.
-   *
-   * @return Array of timestamped values; empty array if no new changes have
-   *     been published since the previous call.
-   */
-  Timestamped{{ TypeName }}[] readQueue();
-
-  /**
-   * Get an array of all value changes since the last call to readQueue.
-   *
-   * <p>The "poll storage" subscribe option can be used to set the queue
-   * depth.
-   *
-   * @return Array of values; empty array if no new changes have been
-   *     published since the previous call.
-   */
-  {{ java.ValueType }}[] readQueueValues();
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/Timestamped.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/Timestamped.java.jinja
deleted file mode 100644
index 288af81..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/Timestamped.java.jinja
+++ /dev/null
@@ -1,40 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-/** NetworkTables timestamped {{ TypeName }}. */
-@SuppressWarnings("PMD.ArrayIsStoredDirectly")
-public final class Timestamped{{ TypeName }} {
-  /**
-   * Create a timestamped value.
-   *
-   * @param timestamp timestamp in local time base
-   * @param serverTime timestamp in server time base
-   * @param value value
-   */
-  public Timestamped{{ TypeName }}(long timestamp, long serverTime, {{ java.ValueType }} value) {
-    this.timestamp = timestamp;
-    this.serverTime = serverTime;
-    this.value = value;
-  }
-
-  /**
-   * Timestamp in local time base.
-   */
-  @SuppressWarnings("MemberName")
-  public final long timestamp;
-
-  /**
-   * Timestamp in server time base.  May be 0 or 1 for locally set values.
-   */
-  @SuppressWarnings("MemberName")
-  public final long serverTime;
-
-  /**
-   * Value.
-   */
-  @SuppressWarnings("MemberName")
-  public final {{ java.ValueType }} value;
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/java/Topic.java.jinja b/third_party/allwpilib/ntcore/src/generate/java/Topic.java.jinja
deleted file mode 100644
index e22fa3b..0000000
--- a/third_party/allwpilib/ntcore/src/generate/java/Topic.java.jinja
+++ /dev/null
@@ -1,223 +0,0 @@
-// 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.
-
-package edu.wpi.first.networktables;
-
-/** NetworkTables {{ TypeName }} topic. */
-public final class {{ TypeName }}Topic extends Topic {
-{%- if TypeString %}
-  /** The default type string for this topic type. */
-  public static final String kTypeString = {{ TypeString }};
-{% endif %}
-  /**
-   * Construct from a generic topic.
-   *
-   * @param topic Topic
-   */
-  public {{ TypeName }}Topic(Topic topic) {
-    super(topic.m_inst, topic.m_handle);
-  }
-
-  /**
-   * Constructor; use NetworkTableInstance.get{{TypeName}}Topic() instead.
-   *
-   * @param inst Instance
-   * @param handle Native handle
-   */
-  public {{ TypeName }}Topic(NetworkTableInstance inst, int handle) {
-    super(inst, handle);
-  }
-
-  /**
-   * Create a new subscriber to the topic.
-   *
-   * <p>The subscriber is only active as long as the returned object
-   * is not closed.
-   *
-   * <p>Subscribers that do not match the published data type do not return
-   * any values. To determine if the data type matches, use the appropriate
-   * Topic functions.
-   *
-{%- if not TypeString %}
-   * @param typeString type string
-{% endif %}
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options subscribe options
-   * @return subscriber
-   */
-  public {{ TypeName }}Subscriber subscribe(
-{%- if not TypeString %}
-      String typeString,
-{% endif %}
-      {{ java.ValueType }} defaultValue,
-      PubSubOption... options) {
-    return new {{ TypeName }}EntryImpl(
-        this,
-        NetworkTablesJNI.subscribe(
-            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
-            {{ TypeString|default('typeString') }}, options),
-        defaultValue);
-  }
-{% if TypeString %}
-  /**
-   * Create a new subscriber to the topic, with specified type string.
-   *
-   * <p>The subscriber is only active as long as the returned object
-   * is not closed.
-   *
-   * <p>Subscribers that do not match the published data type do not return
-   * any values. To determine if the data type matches, use the appropriate
-   * Topic functions.
-   *
-   * @param typeString type string
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options subscribe options
-   * @return subscriber
-   */
-  public {{ TypeName }}Subscriber subscribeEx(
-      String typeString,
-      {{ java.ValueType }} defaultValue,
-      PubSubOption... options) {
-    return new {{ TypeName }}EntryImpl(
-        this,
-        NetworkTablesJNI.subscribe(
-            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
-            typeString, options),
-        defaultValue);
-  }
-{% endif %}
-  /**
-   * Create a new publisher to the topic.
-   *
-   * <p>The publisher is only active as long as the returned object
-   * is not closed.
-   *
-   * <p>It is not possible to publish two different data types to the same
-   * topic. Conflicts between publishers are typically resolved by the server on
-   * a first-come, first-served basis. Any published values that do not match
-   * the topic's data type are dropped (ignored). To determine if the data type
-   * matches, use the appropriate Topic functions.
-   *
-{%- if not TypeString %}
-   * @param typeString type string
-{% endif %}
-   * @param options publish options
-   * @return publisher
-   */
-  public {{ TypeName }}Publisher publish(
-{%- if not TypeString %}
-      String typeString,
-{% endif %}
-      PubSubOption... options) {
-    return new {{ TypeName }}EntryImpl(
-        this,
-        NetworkTablesJNI.publish(
-            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
-            {{ TypeString|default('typeString') }}, options),
-        {{ java.EmptyValue }});
-  }
-
-  /**
-   * Create a new publisher to the topic, with type string and initial properties.
-   *
-   * <p>The publisher is only active as long as the returned object
-   * is not closed.
-   *
-   * <p>It is not possible to publish two different data types to the same
-   * topic. Conflicts between publishers are typically resolved by the server on
-   * a first-come, first-served basis. Any published values that do not match
-   * the topic's data type are dropped (ignored). To determine if the data type
-   * matches, use the appropriate Topic functions.
-   *
-   * @param typeString type string
-   * @param properties JSON properties
-   * @param options publish options
-   * @return publisher
-   * @throws IllegalArgumentException if properties is not a JSON object
-   */
-  public {{ TypeName }}Publisher publishEx(
-      String typeString,
-      String properties,
-      PubSubOption... options) {
-    return new {{ TypeName }}EntryImpl(
-        this,
-        NetworkTablesJNI.publishEx(
-            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
-            typeString, properties, options),
-        {{ java.EmptyValue }});
-  }
-
-  /**
-   * Create a new entry for the topic.
-   *
-   * <p>Entries act as a combination of a subscriber and a weak publisher. The
-   * subscriber is active as long as the entry is not closed. The publisher is
-   * created when the entry is first written to, and remains active until either
-   * unpublish() is called or the entry is closed.
-   *
-   * <p>It is not possible to use two different data types with the same
-   * topic. Conflicts between publishers are typically resolved by the server on
-   * a first-come, first-served basis. Any published values that do not match
-   * the topic's data type are dropped (ignored), and the entry will show no new
-   * values if the data type does not match. To determine if the data type
-   * matches, use the appropriate Topic functions.
-   *
-{%- if not TypeString %}
-   * @param typeString type string
-{% endif %}
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options publish and/or subscribe options
-   * @return entry
-   */
-  public {{ TypeName }}Entry getEntry(
-{%- if not TypeString %}
-      String typeString,
-{% endif %}
-      {{ java.ValueType }} defaultValue,
-      PubSubOption... options) {
-    return new {{ TypeName }}EntryImpl(
-        this,
-        NetworkTablesJNI.getEntry(
-            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
-            {{ TypeString|default('typeString') }}, options),
-        defaultValue);
-  }
-{% if TypeString %}
-  /**
-   * Create a new entry for the topic, with specified type string.
-   *
-   * <p>Entries act as a combination of a subscriber and a weak publisher. The
-   * subscriber is active as long as the entry is not closed. The publisher is
-   * created when the entry is first written to, and remains active until either
-   * unpublish() is called or the entry is closed.
-   *
-   * <p>It is not possible to use two different data types with the same
-   * topic. Conflicts between publishers are typically resolved by the server on
-   * a first-come, first-served basis. Any published values that do not match
-   * the topic's data type are dropped (ignored), and the entry will show no new
-   * values if the data type does not match. To determine if the data type
-   * matches, use the appropriate Topic functions.
-   *
-   * @param typeString type string
-   * @param defaultValue default value used when a default is not provided to a
-   *        getter function
-   * @param options publish and/or subscribe options
-   * @return entry
-   */
-  public {{ TypeName }}Entry getEntryEx(
-      String typeString,
-      {{ java.ValueType }} defaultValue,
-      PubSubOption... options) {
-    return new {{ TypeName }}EntryImpl(
-        this,
-        NetworkTablesJNI.getEntry(
-            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
-            typeString, options),
-        defaultValue);
-  }
-{% endif %}
-}
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/Entry.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/Entry.java.jinja
new file mode 100644
index 0000000..43c424f
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/Entry.java.jinja
@@ -0,0 +1,18 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables {{ TypeName }} entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface {{ TypeName }}Entry extends {{ TypeName }}Subscriber, {{ TypeName }}Publisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/EntryImpl.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/EntryImpl.java.jinja
new file mode 100644
index 0000000..5053e10
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/EntryImpl.java.jinja
@@ -0,0 +1,100 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+{% if TypeName == "Raw" %}
+import java.nio.ByteBuffer;
+{% endif %}
+/** NetworkTables {{ TypeName }} implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class {{ TypeName }}EntryImpl extends EntryBase implements {{ TypeName }}Entry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  {{ TypeName }}EntryImpl({{ TypeName }}Topic topic, int handle, {{ java.ValueType }} defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public {{ TypeName }}Topic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public {{ java.ValueType }} get() {
+    return NetworkTablesJNI.get{{ TypeName }}(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public {{ java.ValueType }} get({{ java.ValueType }} defaultValue) {
+    return NetworkTablesJNI.get{{TypeName}}(m_handle, defaultValue);
+  }
+
+  @Override
+  public Timestamped{{ TypeName }} getAtomic() {
+    return NetworkTablesJNI.getAtomic{{ TypeName }}(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public Timestamped{{ TypeName }} getAtomic({{ java.ValueType }} defaultValue) {
+    return NetworkTablesJNI.getAtomic{{ TypeName }}(m_handle, defaultValue);
+  }
+
+  @Override
+  public Timestamped{{ TypeName }}[] readQueue() {
+    return NetworkTablesJNI.readQueue{{ TypeName }}(m_handle);
+  }
+
+  @Override
+  public {{ java.ValueType }}[] readQueueValues() {
+    return NetworkTablesJNI.readQueueValues{{ TypeName }}(m_handle);
+  }
+{% if TypeName == "Raw" %}
+  @Override
+  public void set(byte[] value, int start, int len, long time) {
+    NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+
+  @Override
+  public void set(ByteBuffer value, int start, int len, long time) {
+    NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+
+  @Override
+  public void setDefault(byte[] value, int start, int len) {
+    NetworkTablesJNI.setDefaultRaw(m_handle, 0, value, start, len);
+  }
+
+  @Override
+  public void setDefault(ByteBuffer value, int start, int len) {
+    NetworkTablesJNI.setDefaultRaw(m_handle, 0, value, start, len);
+  }
+{% else %}
+  @Override
+  public void set({{ java.ValueType }} value, long time) {
+    NetworkTablesJNI.set{{ TypeName }}(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault({{ java.ValueType }} value) {
+    NetworkTablesJNI.setDefault{{ TypeName }}(m_handle, 0, value);
+  }
+{% endif %}
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final {{ TypeName }}Topic m_topic;
+  private final {{ java.ValueType }} m_defaultValue;
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/GenericEntryImpl.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/GenericEntryImpl.java.jinja
new file mode 100644
index 0000000..0bc4fed
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/GenericEntryImpl.java.jinja
@@ -0,0 +1,378 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.nio.ByteBuffer;
+
+/** NetworkTables generic implementation. */
+final class GenericEntryImpl extends EntryBase implements GenericEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   */
+  GenericEntryImpl(Topic topic, int handle) {
+    super(handle);
+    m_topic = topic;
+  }
+
+  @Override
+  public Topic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public NetworkTableValue get() {
+    return NetworkTablesJNI.getValue(m_handle);
+  }
+{% for t in types %}
+  /**
+   * Gets the entry's value as a {{ t.java.ValueType }}. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public {{ t.java.ValueType }} get{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
+    return NetworkTablesJNI.get{{ t.TypeName }}(m_handle, defaultValue);
+  }
+{% if t.java.WrapValueType %}
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public {{ t.java.WrapValueType }} get{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
+    return NetworkTableValue.fromNative{{ t.TypeName }}(
+        get{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue)));
+  }
+{% endif -%}
+{% endfor %}
+  @Override
+  public NetworkTableValue[] readQueue() {
+    return NetworkTablesJNI.readQueueValue(m_handle);
+  }
+
+  @Override
+  public boolean set(NetworkTableValue value) {
+    long time = value.getTime();
+    Object otherValue = value.getValue();
+    switch (value.getType()) {
+      case kBoolean:
+        return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue);
+      case kInteger:
+        return NetworkTablesJNI.setInteger(
+            m_handle, time, ((Number) otherValue).longValue());
+      case kFloat:
+        return NetworkTablesJNI.setFloat(
+            m_handle, time, ((Number) otherValue).floatValue());
+      case kDouble:
+        return NetworkTablesJNI.setDouble(
+            m_handle, time, ((Number) otherValue).doubleValue());
+      case kString:
+        return NetworkTablesJNI.setString(m_handle, time, (String) otherValue);
+      case kRaw:
+        return NetworkTablesJNI.setRaw(m_handle, time, (byte[]) otherValue);
+      case kBooleanArray:
+        return NetworkTablesJNI.setBooleanArray(m_handle, time, (boolean[]) otherValue);
+      case kIntegerArray:
+        return NetworkTablesJNI.setIntegerArray(m_handle, time, (long[]) otherValue);
+      case kFloatArray:
+        return NetworkTablesJNI.setFloatArray(m_handle, time, (float[]) otherValue);
+      case kDoubleArray:
+        return NetworkTablesJNI.setDoubleArray(m_handle, time, (double[]) otherValue);
+      case kStringArray:
+        return NetworkTablesJNI.setStringArray(m_handle, time, (String[]) otherValue);
+      default:
+        return true;
+    }
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  @Override
+  public boolean setValue(Object value, long time) {
+    if (value instanceof NetworkTableValue) {
+      return set((NetworkTableValue) value);
+    } else if (value instanceof Boolean) {
+      return setBoolean((Boolean) value, time);
+    } else if (value instanceof Long) {
+      return setInteger((Long) value, time);
+    } else if (value instanceof Float) {
+      return setFloat((Float) value, time);
+    } else if (value instanceof Number) {
+      return setNumber((Number) value, time);
+    } else if (value instanceof String) {
+      return setString((String) value, time);
+    } else if (value instanceof byte[]) {
+      return setRaw((byte[]) value, time);
+    } else if (value instanceof boolean[]) {
+      return setBooleanArray((boolean[]) value, time);
+    } else if (value instanceof long[]) {
+      return setIntegerArray((long[]) value, time);
+    } else if (value instanceof float[]) {
+      return setFloatArray((float[]) value, time);
+    } else if (value instanceof double[]) {
+      return setDoubleArray((double[]) value, time);
+    } else if (value instanceof Boolean[]) {
+      return setBooleanArray((Boolean[]) value, time);
+    } else if (value instanceof Long[]) {
+      return setIntegerArray((Long[]) value, time);
+    } else if (value instanceof Float[]) {
+      return setFloatArray((Float[]) value, time);
+    } else if (value instanceof Number[]) {
+      return setNumberArray((Number[]) value, time);
+    } else if (value instanceof String[]) {
+      return setStringArray((String[]) value, time);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + value.getClass().getName() + " cannot be put into a table");
+    }
+  }
+{% for t in types %}
+{% if t.TypeName == "Raw" %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setRaw(byte[] value, int start, int len, long time) {
+    return NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setRaw(ByteBuffer value, int start, int len, long time) {
+    return NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+{% else %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean set{{ t.TypeName }}({{ t.java.ValueType }} value, long time) {
+    return NetworkTablesJNI.set{{ t.TypeName }}(m_handle, time, value);
+  }
+{% endif -%}
+{% if t.java.WrapValueType %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value, long time) {
+    return set{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(value), time);
+  }
+{% endif -%}
+{% endfor %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumber(Number value, long time) {
+    return setDouble(value.doubleValue(), time);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumberArray(Number[] value, long time) {
+    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value), time);
+  }
+
+  @Override
+  public boolean setDefault(NetworkTableValue defaultValue) {
+    long time = defaultValue.getTime();
+    Object otherValue = defaultValue.getValue();
+    switch (defaultValue.getType()) {
+      case kBoolean:
+        return NetworkTablesJNI.setDefaultBoolean(m_handle, time, (Boolean) otherValue);
+      case kInteger:
+        return NetworkTablesJNI.setDefaultInteger(
+            m_handle, time, ((Number) otherValue).longValue());
+      case kFloat:
+        return NetworkTablesJNI.setDefaultFloat(
+            m_handle, time, ((Number) otherValue).floatValue());
+      case kDouble:
+        return NetworkTablesJNI.setDefaultDouble(
+            m_handle, time, ((Number) otherValue).doubleValue());
+      case kString:
+        return NetworkTablesJNI.setDefaultString(m_handle, time, (String) otherValue);
+      case kRaw:
+        return NetworkTablesJNI.setDefaultRaw(m_handle, time, (byte[]) otherValue);
+      case kBooleanArray:
+        return NetworkTablesJNI.setDefaultBooleanArray(m_handle, time, (boolean[]) otherValue);
+      case kIntegerArray:
+        return NetworkTablesJNI.setDefaultIntegerArray(m_handle, time, (long[]) otherValue);
+      case kFloatArray:
+        return NetworkTablesJNI.setDefaultFloatArray(m_handle, time, (float[]) otherValue);
+      case kDoubleArray:
+        return NetworkTablesJNI.setDefaultDoubleArray(m_handle, time, (double[]) otherValue);
+      case kStringArray:
+        return NetworkTablesJNI.setDefaultStringArray(m_handle, time, (String[]) otherValue);
+      default:
+        return true;
+    }
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  @Override
+  public boolean setDefaultValue(Object defaultValue) {
+    if (defaultValue instanceof NetworkTableValue) {
+      return setDefault((NetworkTableValue) defaultValue);
+    } else if (defaultValue instanceof Boolean) {
+      return setDefaultBoolean((Boolean) defaultValue);
+    } else if (defaultValue instanceof Integer) {
+      return setDefaultInteger((Integer) defaultValue);
+    } else if (defaultValue instanceof Float) {
+      return setDefaultFloat((Float) defaultValue);
+    } else if (defaultValue instanceof Number) {
+      return setDefaultNumber((Number) defaultValue);
+    } else if (defaultValue instanceof String) {
+      return setDefaultString((String) defaultValue);
+    } else if (defaultValue instanceof byte[]) {
+      return setDefaultRaw((byte[]) defaultValue);
+    } else if (defaultValue instanceof boolean[]) {
+      return setDefaultBooleanArray((boolean[]) defaultValue);
+    } else if (defaultValue instanceof long[]) {
+      return setDefaultIntegerArray((long[]) defaultValue);
+    } else if (defaultValue instanceof float[]) {
+      return setDefaultFloatArray((float[]) defaultValue);
+    } else if (defaultValue instanceof double[]) {
+      return setDefaultDoubleArray((double[]) defaultValue);
+    } else if (defaultValue instanceof Boolean[]) {
+      return setDefaultBooleanArray((Boolean[]) defaultValue);
+    } else if (defaultValue instanceof Long[]) {
+      return setDefaultIntegerArray((Long[]) defaultValue);
+    } else if (defaultValue instanceof Float[]) {
+      return setDefaultFloatArray((Float[]) defaultValue);
+    } else if (defaultValue instanceof Number[]) {
+      return setDefaultNumberArray((Number[]) defaultValue);
+    } else if (defaultValue instanceof String[]) {
+      return setDefaultStringArray((String[]) defaultValue);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + defaultValue.getClass().getName() + " cannot be put into a table");
+    }
+  }
+{% for t in types %}
+{% if t.TypeName == "Raw" %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultRaw(byte[] defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+{% else %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefault{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
+    return NetworkTablesJNI.setDefault{{ t.TypeName }}(m_handle, 0, defaultValue);
+  }
+{% endif -%}
+{% if t.java.WrapValueType %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefault{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
+    return setDefault{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue));
+  }
+{% endif -%}
+{% endfor %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumber(Number defaultValue) {
+    return setDefaultDouble(defaultValue.doubleValue());
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumberArray(Number[] defaultValue) {
+    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final Topic m_topic;
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/GenericPublisher.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/GenericPublisher.java.jinja
new file mode 100644
index 0000000..f6d7001
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/GenericPublisher.java.jinja
@@ -0,0 +1,247 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.nio.ByteBuffer;
+import java.util.function.Consumer;
+
+/** NetworkTables generic publisher. */
+public interface GenericPublisher extends Publisher, Consumer<NetworkTableValue> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  Topic getTopic();
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  boolean set(NetworkTableValue value);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  default boolean setValue(Object value) {
+    return setValue(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  boolean setValue(Object value, long time);
+{% for t in types %}
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean set{{ t.TypeName }}({{ t.java.ValueType }} value) {
+    return set{{ t.TypeName }}(value, 0);
+  }
+{% if t.TypeName == "Raw" %}
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(ByteBuffer value) {
+    return setRaw(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(byte[] value, long time) {
+    return setRaw(value, 0, value.length, time);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish; will send from value.position() to value.limit()
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(ByteBuffer value, long time) {
+    int pos = value.position();
+    return setRaw(value, pos, value.limit() - pos, time);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(byte[] value, int start, int len) {
+    return setRaw(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setRaw(byte[] value, int start, int len, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(ByteBuffer value, int start, int len) {
+    return setRaw(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setRaw(ByteBuffer value, int start, int len, long time);
+{% else %}
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean set{{ t.TypeName }}({{ t.java.ValueType }} value, long time);
+{% endif -%}
+{% if t.java.WrapValueType %}
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value) {
+    return set{{ t.TypeName }}(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value, long time);
+{% endif -%}
+{% endfor %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefault(NetworkTableValue defaultValue);
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  boolean setDefaultValue(Object defaultValue);
+{% for t in types %}
+{% if t.TypeName == "Raw" %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  default boolean setDefaultRaw(byte[] defaultValue) {
+    return setDefaultRaw(defaultValue, 0, defaultValue.length);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set; will send from defaultValue.position() to
+   *                     defaultValue.limit()
+   * @return False if the entry exists with a different type
+   */
+  default boolean setDefaultRaw(ByteBuffer defaultValue) {
+    int pos = defaultValue.position();
+    return setDefaultRaw(defaultValue, pos, defaultValue.limit() - pos);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultRaw(byte[] defaultValue, int start, int len);
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len);
+{% else %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefault{{ t.TypeName }}({{ t.java.ValueType }} defaultValue);
+{% endif -%}
+{% if t.java.WrapValueType %}
+  boolean setDefault{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue);
+{% endif -%}
+{% endfor %}
+  @Override
+  default void accept(NetworkTableValue value) {
+    set(value);
+  }
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/GenericSubscriber.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/GenericSubscriber.java.jinja
new file mode 100644
index 0000000..c0ed661
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/GenericSubscriber.java.jinja
@@ -0,0 +1,61 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables generic subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface GenericSubscriber extends Subscriber, Supplier<NetworkTableValue> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  Topic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns a value with type NetworkTableType.kUnassigned.
+   *
+   * @return value
+   */
+  NetworkTableValue get();
+{% for t in types %}
+  /**
+   * Gets the entry's value as a {{ t.java.ValueType }}. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  {{ t.java.ValueType }} get{{ t.TypeName }}({{ t.java.ValueType }} defaultValue);
+{% if t.java.WrapValueType %}
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  {{ t.java.WrapValueType }} get{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue);
+{% endif -%}
+{% endfor %}
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  NetworkTableValue[] readQueue();
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableEntry.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableEntry.java.jinja
new file mode 100644
index 0000000..2c2c737
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableEntry.java.jinja
@@ -0,0 +1,613 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.nio.ByteBuffer;
+
+/**
+ * NetworkTables Entry.
+ *
+ * <p>For backwards compatibility, the NetworkTableEntry close() does not release the entry.
+ */
+@SuppressWarnings("UnnecessaryParentheses")
+public final class NetworkTableEntry implements Publisher, Subscriber {
+  /**
+   * Flag values (as returned by {@link #getFlags()}).
+   *
+   * @deprecated Use isPersistent() instead.
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public static final int kPersistent = 0x01;
+
+  /**
+   * Construct from native handle.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public NetworkTableEntry(NetworkTableInstance inst, int handle) {
+    this(new Topic(inst, NetworkTablesJNI.getTopicFromHandle(handle)), handle);
+  }
+
+  /**
+   * Construct from native handle.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   */
+  public NetworkTableEntry(Topic topic, int handle) {
+    m_topic = topic;
+    m_handle = handle;
+  }
+
+  @Override
+  public void close() {}
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  @Override
+  public boolean isValid() {
+    return m_handle != 0;
+  }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  @Override
+  public int getHandle() {
+    return m_handle;
+  }
+
+  /**
+   * Gets the subscribed-to / published-to topic.
+   *
+   * @return Topic
+   */
+  @Override
+  public Topic getTopic() {
+    return m_topic;
+  }
+
+  /**
+   * Gets the instance for the entry.
+   *
+   * @return Instance
+   */
+  public NetworkTableInstance getInstance() {
+    return m_topic.getInstance();
+  }
+
+  /**
+   * Determines if the entry currently exists.
+   *
+   * @return True if the entry exists, false otherwise.
+   */
+  @Override
+  public boolean exists() {
+    return NetworkTablesJNI.getType(m_handle) != 0;
+  }
+
+  /**
+   * Gets the name of the entry (the key).
+   *
+   * @return the entry's name
+   */
+  public String getName() {
+    return NetworkTablesJNI.getEntryName(m_handle);
+  }
+
+  /**
+   * Gets the type of the entry.
+   *
+   * @return the entry's type
+   */
+  public NetworkTableType getType() {
+    return NetworkTableType.getFromInt(NetworkTablesJNI.getType(m_handle));
+  }
+
+  /**
+   * Returns the flags.
+   *
+   * @return the flags (bitmask)
+   * @deprecated Use isPersistent() or topic properties instead
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public int getFlags() {
+    return NetworkTablesJNI.getEntryFlags(m_handle);
+  }
+
+  /**
+   * Gets the last time the entry's value was changed.
+   *
+   * @return Entry last change time
+   */
+  @Override
+  public long getLastChange() {
+    return NetworkTablesJNI.getEntryLastChange(m_handle);
+  }
+
+  /**
+   * Gets the entry's value. Returns a value with type NetworkTableType.kUnassigned if the value
+   * does not exist.
+   *
+   * @return the entry's value
+   */
+  public NetworkTableValue getValue() {
+    return NetworkTablesJNI.getValue(m_handle);
+  }
+{% for t in types %}
+  /**
+   * Gets the entry's value as a {{ t.java.ValueType }}. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public {{ t.java.ValueType }} get{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
+    return NetworkTablesJNI.get{{ t.TypeName }}(m_handle, defaultValue);
+  }
+{% if t.java.WrapValueType %}
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public {{ t.java.WrapValueType }} get{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
+    return NetworkTableValue.fromNative{{ t.TypeName }}(
+        get{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue)));
+  }
+{% endif -%}
+{% endfor %}
+  /**
+   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Number getNumber(Number defaultValue) {
+    return getDouble(defaultValue.doubleValue());
+  }
+
+  /**
+   * Gets the entry's value as a double array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Number[] getNumberArray(Number[] defaultValue) {
+    return NetworkTableValue.fromNativeDoubleArray(
+        getDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue)));
+  }
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  public NetworkTableValue[] readQueue() {
+    return NetworkTablesJNI.readQueueValue(m_handle);
+  }
+
+  /**
+   * Checks if a data value is of a type that can be placed in a NetworkTable entry.
+   *
+   * @param data the data to check
+   * @return true if the data can be placed in an entry, false if it cannot
+   */
+  public static boolean isValidDataType(Object data) {
+    return data instanceof Number
+        || data instanceof Boolean
+        || data instanceof String
+        || data instanceof long[]
+        || data instanceof Long[]
+        || data instanceof float[]
+        || data instanceof Float[]
+        || data instanceof double[]
+        || data instanceof Double[]
+        || data instanceof Number[]
+        || data instanceof boolean[]
+        || data instanceof Boolean[]
+        || data instanceof String[]
+        || data instanceof byte[]
+        || data instanceof Byte[];
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  public boolean setDefaultValue(Object defaultValue) {
+    if (defaultValue instanceof NetworkTableValue) {
+      long time = ((NetworkTableValue) defaultValue).getTime();
+      Object otherValue = ((NetworkTableValue) defaultValue).getValue();
+      switch (((NetworkTableValue) defaultValue).getType()) {
+        case kBoolean:
+          return NetworkTablesJNI.setDefaultBoolean(m_handle, time, (Boolean) otherValue);
+        case kInteger:
+          return NetworkTablesJNI.setDefaultInteger(
+              m_handle, time, ((Number) otherValue).longValue());
+        case kFloat:
+          return NetworkTablesJNI.setDefaultFloat(
+              m_handle, time, ((Number) otherValue).floatValue());
+        case kDouble:
+          return NetworkTablesJNI.setDefaultDouble(
+              m_handle, time, ((Number) otherValue).doubleValue());
+        case kString:
+          return NetworkTablesJNI.setDefaultString(m_handle, time, (String) otherValue);
+        case kRaw:
+          return NetworkTablesJNI.setDefaultRaw(m_handle, time, (byte[]) otherValue);
+        case kBooleanArray:
+          return NetworkTablesJNI.setDefaultBooleanArray(m_handle, time, (boolean[]) otherValue);
+        case kIntegerArray:
+          return NetworkTablesJNI.setDefaultIntegerArray(m_handle, time, (long[]) otherValue);
+        case kFloatArray:
+          return NetworkTablesJNI.setDefaultFloatArray(m_handle, time, (float[]) otherValue);
+        case kDoubleArray:
+          return NetworkTablesJNI.setDefaultDoubleArray(m_handle, time, (double[]) otherValue);
+        case kStringArray:
+          return NetworkTablesJNI.setDefaultStringArray(m_handle, time, (String[]) otherValue);
+        default:
+          return true;
+      }
+    } else if (defaultValue instanceof Boolean) {
+      return setDefaultBoolean((Boolean) defaultValue);
+    } else if (defaultValue instanceof Integer) {
+      return setDefaultInteger((Integer) defaultValue);
+    } else if (defaultValue instanceof Float) {
+      return setDefaultFloat((Float) defaultValue);
+    } else if (defaultValue instanceof Number) {
+      return setDefaultNumber((Number) defaultValue);
+    } else if (defaultValue instanceof String) {
+      return setDefaultString((String) defaultValue);
+    } else if (defaultValue instanceof byte[]) {
+      return setDefaultRaw((byte[]) defaultValue);
+    } else if (defaultValue instanceof boolean[]) {
+      return setDefaultBooleanArray((boolean[]) defaultValue);
+    } else if (defaultValue instanceof long[]) {
+      return setDefaultIntegerArray((long[]) defaultValue);
+    } else if (defaultValue instanceof float[]) {
+      return setDefaultFloatArray((float[]) defaultValue);
+    } else if (defaultValue instanceof double[]) {
+      return setDefaultDoubleArray((double[]) defaultValue);
+    } else if (defaultValue instanceof Boolean[]) {
+      return setDefaultBooleanArray((Boolean[]) defaultValue);
+    } else if (defaultValue instanceof Long[]) {
+      return setDefaultIntegerArray((Long[]) defaultValue);
+    } else if (defaultValue instanceof Float[]) {
+      return setDefaultFloatArray((Float[]) defaultValue);
+    } else if (defaultValue instanceof Number[]) {
+      return setDefaultNumberArray((Number[]) defaultValue);
+    } else if (defaultValue instanceof String[]) {
+      return setDefaultStringArray((String[]) defaultValue);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + defaultValue.getClass().getName() + " cannot be put into a table");
+    }
+  }
+{% for t in types %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefault{{ t.TypeName }}({{ t.java.ValueType }} defaultValue) {
+    return NetworkTablesJNI.setDefault{{ t.TypeName }}(m_handle, 0, defaultValue);
+  }
+{% if t.TypeName == "Raw" %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set; will send from defaultValue.position() to
+   *                     defaultValue.capacity()
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultRaw(ByteBuffer defaultValue) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultRaw(byte[] defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+{% endif -%}
+{% if t.java.WrapValueType %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefault{{ t.TypeName }}({{ t.java.WrapValueType }} defaultValue) {
+    return setDefault{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(defaultValue));
+  }
+{% endif -%}
+{% endfor %}
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumber(Number defaultValue) {
+    return setDefaultDouble(defaultValue.doubleValue());
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumberArray(Number[] defaultValue) {
+    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  public boolean setValue(Object value) {
+    if (value instanceof NetworkTableValue) {
+      long time = ((NetworkTableValue) value).getTime();
+      Object otherValue = ((NetworkTableValue) value).getValue();
+      switch (((NetworkTableValue) value).getType()) {
+        case kBoolean:
+          return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue);
+        case kInteger:
+          return NetworkTablesJNI.setInteger(
+              m_handle, time, ((Number) otherValue).longValue());
+        case kFloat:
+          return NetworkTablesJNI.setFloat(
+              m_handle, time, ((Number) otherValue).floatValue());
+        case kDouble:
+          return NetworkTablesJNI.setDouble(
+              m_handle, time, ((Number) otherValue).doubleValue());
+        case kString:
+          return NetworkTablesJNI.setString(m_handle, time, (String) otherValue);
+        case kRaw:
+          return NetworkTablesJNI.setRaw(m_handle, time, (byte[]) otherValue);
+        case kBooleanArray:
+          return NetworkTablesJNI.setBooleanArray(m_handle, time, (boolean[]) otherValue);
+        case kIntegerArray:
+          return NetworkTablesJNI.setIntegerArray(m_handle, time, (long[]) otherValue);
+        case kFloatArray:
+          return NetworkTablesJNI.setFloatArray(m_handle, time, (float[]) otherValue);
+        case kDoubleArray:
+          return NetworkTablesJNI.setDoubleArray(m_handle, time, (double[]) otherValue);
+        case kStringArray:
+          return NetworkTablesJNI.setStringArray(m_handle, time, (String[]) otherValue);
+        default:
+          return true;
+      }
+    } else if (value instanceof Boolean) {
+      return setBoolean((Boolean) value);
+    } else if (value instanceof Long) {
+      return setInteger((Long) value);
+    } else if (value instanceof Float) {
+      return setFloat((Float) value);
+    } else if (value instanceof Number) {
+      return setNumber((Number) value);
+    } else if (value instanceof String) {
+      return setString((String) value);
+    } else if (value instanceof byte[]) {
+      return setRaw((byte[]) value);
+    } else if (value instanceof boolean[]) {
+      return setBooleanArray((boolean[]) value);
+    } else if (value instanceof long[]) {
+      return setIntegerArray((long[]) value);
+    } else if (value instanceof float[]) {
+      return setFloatArray((float[]) value);
+    } else if (value instanceof double[]) {
+      return setDoubleArray((double[]) value);
+    } else if (value instanceof Boolean[]) {
+      return setBooleanArray((Boolean[]) value);
+    } else if (value instanceof Long[]) {
+      return setIntegerArray((Long[]) value);
+    } else if (value instanceof Float[]) {
+      return setFloatArray((Float[]) value);
+    } else if (value instanceof Number[]) {
+      return setNumberArray((Number[]) value);
+    } else if (value instanceof String[]) {
+      return setStringArray((String[]) value);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + value.getClass().getName() + " cannot be put into a table");
+    }
+  }
+{% for t in types %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean set{{ t.TypeName }}({{ t.java.ValueType }} value) {
+    return NetworkTablesJNI.set{{ t.TypeName }}(m_handle, 0, value);
+  }
+{% if t.TypeName == "Raw" %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set; will send from value.position() to value.capacity()
+   * @return False if the entry exists with a different type
+   */
+  public boolean setRaw(ByteBuffer value) {
+    return NetworkTablesJNI.setRaw(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setRaw(byte[] value, int start, int len) {
+    return NetworkTablesJNI.setRaw(m_handle, 0, value, start, len);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setRaw(ByteBuffer value, int start, int len) {
+    return NetworkTablesJNI.setRaw(m_handle, 0, value, start, len);
+  }
+{% endif -%}
+{% if t.java.WrapValueType %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean set{{ t.TypeName }}({{ t.java.WrapValueType }} value) {
+    return set{{ t.TypeName }}(NetworkTableValue.toNative{{ t.TypeName }}(value));
+  }
+{% endif -%}
+{% endfor %}
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumber(Number value) {
+    return setDouble(value.doubleValue());
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumberArray(Number[] value) {
+    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value));
+  }
+
+  /**
+   * Sets flags.
+   *
+   * @param flags the flags to set (bitmask)
+   * @deprecated Use setPersistent() or topic properties instead
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public void setFlags(int flags) {
+    NetworkTablesJNI.setEntryFlags(m_handle, getFlags() | flags);
+  }
+
+  /**
+   * Clears flags.
+   *
+   * @param flags the flags to clear (bitmask)
+   * @deprecated Use setPersistent() or topic properties instead
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public void clearFlags(int flags) {
+    NetworkTablesJNI.setEntryFlags(m_handle, getFlags() & ~flags);
+  }
+
+  /** Make value persistent through program restarts. */
+  public void setPersistent() {
+    NetworkTablesJNI.setTopicPersistent(m_topic.getHandle(), true);
+  }
+
+  /** Stop making value persistent through program restarts. */
+  public void clearPersistent() {
+    NetworkTablesJNI.setTopicPersistent(m_topic.getHandle(), false);
+  }
+
+  /**
+   * Returns whether the value is persistent through program restarts.
+   *
+   * @return True if the value is persistent.
+   */
+  public boolean isPersistent() {
+    return NetworkTablesJNI.getTopicPersistent(m_topic.getHandle());
+  }
+
+  /** Stops publishing the entry if it's been published. */
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  /**
+   * Deletes the entry.
+   *
+   * @deprecated Use unpublish() instead.
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public void delete() {
+    unpublish();
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (other == this) {
+      return true;
+    }
+    if (!(other instanceof NetworkTableEntry)) {
+      return false;
+    }
+
+    return m_handle == ((NetworkTableEntry) other).m_handle;
+  }
+
+  @Override
+  public int hashCode() {
+    return m_handle;
+  }
+
+  private final Topic m_topic;
+  protected int m_handle;
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableInstance.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableInstance.java.jinja
new file mode 100644
index 0000000..20861cd
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableInstance.java.jinja
@@ -0,0 +1,1274 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.util.concurrent.Event;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.charset.StandardCharsets;
+import java.util.EnumSet;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.Map;
+import java.util.OptionalLong;
+import java.util.Set;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.ConcurrentMap;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+import us.hebi.quickbuf.ProtoMessage;
+
+/**
+ * NetworkTables Instance.
+ *
+ * <p>Instances are completely independent from each other. Table operations on one instance will
+ * not be visible to other instances unless the instances are connected via the network. The main
+ * limitation on instances is that you cannot have two servers on the same network port. The main
+ * utility of instances is for unit testing, but they can also enable one program to connect to two
+ * different NetworkTables networks.
+ *
+ * <p>The global "default" instance (as returned by {@link #getDefault()}) is always available, and
+ * is intended for the common case when there is only a single NetworkTables instance being used in
+ * the program.
+ *
+ * <p>Additional instances can be created with the {@link #create()} function. A reference must be
+ * kept to the NetworkTableInstance returned by this function to keep it from being garbage
+ * collected.
+ */
+@SuppressWarnings("PMD.CouplingBetweenObjects")
+public final class NetworkTableInstance implements AutoCloseable {
+  /** Client/server mode flag values (as returned by {@link #getNetworkMode()}). */
+  public enum NetworkMode {
+    /** Running in server mode. */
+    kServer(0x01),
+
+    /** Running in NT3 client mode. */
+    kClient3(0x02),
+
+    /** Running in NT4 client mode. */
+    kClient4(0x04),
+
+    /** Currently starting up (either client or server). */
+    kStarting(0x08),
+
+    /** Running in local-only mode. */
+    kLocal(0x10);
+
+    private final int value;
+
+    NetworkMode(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /** The default port that network tables operates on for NT3. */
+  public static final int kDefaultPort3 = 1735;
+
+  /** The default port that network tables operates on for NT4. */
+  public static final int kDefaultPort4 = 5810;
+
+  /**
+   * Construct from native handle.
+   *
+   * @param handle Native handle
+   */
+  private NetworkTableInstance(int handle) {
+    m_owned = false;
+    m_handle = handle;
+  }
+
+  /** Destroys the instance (if created by {@link #create()}). */
+  @Override
+  public synchronized void close() {
+    if (m_owned && m_handle != 0) {
+      m_listeners.close();
+      m_schemas.forEach((k, v) -> v.close());
+      NetworkTablesJNI.destroyInstance(m_handle);
+      m_handle = 0;
+    }
+  }
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  public boolean isValid() {
+    return m_handle != 0;
+  }
+
+  /* The default instance. */
+  private static NetworkTableInstance s_defaultInstance;
+
+  /**
+   * Get global default instance.
+   *
+   * @return Global default instance
+   */
+  public static synchronized NetworkTableInstance getDefault() {
+    if (s_defaultInstance == null) {
+      s_defaultInstance = new NetworkTableInstance(NetworkTablesJNI.getDefaultInstance());
+    }
+    return s_defaultInstance;
+  }
+
+  /**
+   * Create an instance. Note: A reference to the returned instance must be retained to ensure the
+   * instance is not garbage collected.
+   *
+   * @return Newly created instance
+   */
+  public static NetworkTableInstance create() {
+    NetworkTableInstance inst = new NetworkTableInstance(NetworkTablesJNI.createInstance());
+    inst.m_owned = true;
+    return inst;
+  }
+
+  /**
+   * Gets the native handle for the instance.
+   *
+   * @return Native handle
+   */
+  public int getHandle() {
+    return m_handle;
+  }
+
+  /**
+   * Get (generic) topic.
+   *
+   * @param name topic name
+   * @return Topic
+   */
+  public Topic getTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic == null) {
+      int handle = NetworkTablesJNI.getTopic(m_handle, name);
+      topic = new Topic(this, handle);
+      Topic oldTopic = m_topics.putIfAbsent(name, topic);
+      if (oldTopic != null) {
+        topic = oldTopic;
+      }
+      // also cache by handle
+      m_topicsByHandle.putIfAbsent(handle, topic);
+    }
+    return topic;
+  }
+{% for t in types %}
+  /**
+   * Get {{ t.java.ValueType }} topic.
+   *
+   * @param name topic name
+   * @return {{ t.TypeName }}Topic
+   */
+  public {{ t.TypeName }}Topic get{{ t.TypeName }}Topic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof {{ t.TypeName }}Topic) {
+      return ({{ t.TypeName }}Topic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    {{ t.TypeName }}Topic wrapTopic = new {{ t.TypeName }}Topic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+{% endfor %}
+
+  /**
+   * Get protobuf-encoded value topic.
+   *
+   * @param <T> value class (inferred from proto)
+   * @param <MessageType> protobuf message type (inferred from proto)
+   * @param name topic name
+   * @param proto protobuf serialization implementation
+   * @return ProtobufTopic
+   */
+  public <T, MessageType extends ProtoMessage<?>>
+      ProtobufTopic<T> getProtobufTopic(String name, Protobuf<T, MessageType> proto) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof ProtobufTopic<?>
+        && ((ProtobufTopic<?>) topic).getProto().equals(proto)) {
+      @SuppressWarnings("unchecked")
+      ProtobufTopic<T> wrapTopic = (ProtobufTopic<T>) topic;
+      return wrapTopic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    ProtobufTopic<T> wrapTopic = ProtobufTopic.wrap(this, handle, proto);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get struct-encoded value topic.
+   *
+   * @param <T> value class (inferred from struct)
+   * @param name topic name
+   * @param struct struct serialization implementation
+   * @return StructTopic
+   */
+  public <T>
+      StructTopic<T> getStructTopic(String name, Struct<T> struct) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof StructTopic<?>
+        && ((StructTopic<?>) topic).getStruct().equals(struct)) {
+      @SuppressWarnings("unchecked")
+      StructTopic<T> wrapTopic = (StructTopic<T>) topic;
+      return wrapTopic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    StructTopic<T> wrapTopic = StructTopic.wrap(this, handle, struct);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get struct-encoded value array topic.
+   *
+   * @param <T> value class (inferred from struct)
+   * @param name topic name
+   * @param struct struct serialization implementation
+   * @return StructArrayTopic
+   */
+  public <T>
+      StructArrayTopic<T> getStructArrayTopic(String name, Struct<T> struct) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof StructArrayTopic<?>
+        && ((StructArrayTopic<?>) topic).getStruct().equals(struct)) {
+      @SuppressWarnings("unchecked")
+      StructArrayTopic<T> wrapTopic = (StructArrayTopic<T>) topic;
+      return wrapTopic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    StructArrayTopic<T> wrapTopic = StructArrayTopic.wrap(this, handle, struct);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  private Topic[] topicHandlesToTopics(int[] handles) {
+    Topic[] topics = new Topic[handles.length];
+    for (int i = 0; i < handles.length; i++) {
+      topics[i] = getCachedTopic(handles[i]);
+    }
+    return topics;
+  }
+
+  /**
+   * Get all published topics.
+   *
+   * @return Array of topics.
+   */
+  public Topic[] getTopics() {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, "", 0));
+  }
+
+  /**
+   * Get published topics starting with the given prefix. The results are optionally filtered by
+   * string prefix to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @return Array of topic information.
+   */
+  public Topic[] getTopics(String prefix) {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, prefix, 0));
+  }
+
+  /**
+   * Get published topics starting with the given prefix. The results are optionally filtered by
+   * string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types bitmask of data types; 0 is treated as a "don't care"
+   * @return Array of topic information.
+   */
+  public Topic[] getTopics(String prefix, int types) {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, prefix, types));
+  }
+
+  /**
+   * Get published topics starting with the given prefix. The results are optionally filtered by
+   * string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types array of data type strings
+   * @return Array of topic information.
+   */
+  public Topic[] getTopics(String prefix, String[] types) {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopicsStr(m_handle, prefix, types));
+  }
+
+  /**
+   * Get information about all topics.
+   *
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo() {
+    return NetworkTablesJNI.getTopicInfos(this, m_handle, "", 0);
+  }
+
+  /**
+   * Get information about topics starting with the given prefix. The results are optionally
+   * filtered by string prefix to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo(String prefix) {
+    return NetworkTablesJNI.getTopicInfos(this, m_handle, prefix, 0);
+  }
+
+  /**
+   * Get information about topics starting with the given prefix. The results are optionally
+   * filtered by string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types bitmask of data types; 0 is treated as a "don't care"
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo(String prefix, int types) {
+    return NetworkTablesJNI.getTopicInfos(this, m_handle, prefix, types);
+  }
+
+  /**
+   * Get information about topics starting with the given prefix. The results are optionally
+   * filtered by string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types array of data type strings
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo(String prefix, String[] types) {
+    return NetworkTablesJNI.getTopicInfosStr(this, m_handle, prefix, types);
+  }
+
+  /* Cache of created entries. */
+  private final ConcurrentMap<String, NetworkTableEntry> m_entries = new ConcurrentHashMap<>();
+
+  /**
+   * Gets the entry for a key.
+   *
+   * @param name Key
+   * @return Network table entry.
+   */
+  public NetworkTableEntry getEntry(String name) {
+    NetworkTableEntry entry = m_entries.get(name);
+    if (entry == null) {
+      entry = new NetworkTableEntry(this, NetworkTablesJNI.getEntry(m_handle, name));
+      NetworkTableEntry oldEntry = m_entries.putIfAbsent(name, entry);
+      if (oldEntry != null) {
+        entry = oldEntry;
+      }
+    }
+    return entry;
+  }
+
+  /* Cache of created topics. */
+  private final ConcurrentMap<String, Topic> m_topics = new ConcurrentHashMap<>();
+  private final ConcurrentMap<Integer, Topic> m_topicsByHandle = new ConcurrentHashMap<>();
+
+  Topic getCachedTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic == null) {
+      int handle = NetworkTablesJNI.getTopic(m_handle, name);
+      topic = new Topic(this, handle);
+      Topic oldTopic = m_topics.putIfAbsent(name, topic);
+      if (oldTopic != null) {
+        topic = oldTopic;
+      }
+      // also cache by handle
+      m_topicsByHandle.putIfAbsent(handle, topic);
+    }
+    return topic;
+  }
+
+  Topic getCachedTopic(int handle) {
+    Topic topic = m_topicsByHandle.get(handle);
+    if (topic == null) {
+      topic = new Topic(this, handle);
+      Topic oldTopic = m_topicsByHandle.putIfAbsent(handle, topic);
+      if (oldTopic != null) {
+        topic = oldTopic;
+      }
+    }
+    return topic;
+  }
+
+  /* Cache of created tables. */
+  private final ConcurrentMap<String, NetworkTable> m_tables = new ConcurrentHashMap<>();
+
+  /**
+   * Gets the table with the specified key.
+   *
+   * @param key the key name
+   * @return The network table
+   */
+  public NetworkTable getTable(String key) {
+    // prepend leading / if not present
+    String theKey;
+    if (key.isEmpty() || "/".equals(key)) {
+      theKey = "";
+    } else if (key.charAt(0) == NetworkTable.PATH_SEPARATOR) {
+      theKey = key;
+    } else {
+      theKey = NetworkTable.PATH_SEPARATOR + key;
+    }
+
+    // cache created tables
+    NetworkTable table = m_tables.get(theKey);
+    if (table == null) {
+      table = new NetworkTable(this, theKey);
+      NetworkTable oldTable = m_tables.putIfAbsent(theKey, table);
+      if (oldTable != null) {
+        table = oldTable;
+      }
+    }
+    return table;
+  }
+
+  /*
+   * Callback Creation Functions
+   */
+
+  private static class ListenerStorage implements AutoCloseable {
+    private final ReentrantLock m_lock = new ReentrantLock();
+    private final Map<Integer, Consumer<NetworkTableEvent>> m_listeners = new HashMap<>();
+    private Thread m_thread;
+    private int m_poller;
+    private boolean m_waitQueue;
+    private final Event m_waitQueueEvent = new Event();
+    private final Condition m_waitQueueCond = m_lock.newCondition();
+    private final NetworkTableInstance m_inst;
+
+    ListenerStorage(NetworkTableInstance inst) {
+      m_inst = inst;
+    }
+
+    int add(
+        String[] prefixes,
+        EnumSet<NetworkTableEvent.Kind> eventKinds,
+        Consumer<NetworkTableEvent> listener) {
+      m_lock.lock();
+      try {
+        if (m_poller == 0) {
+          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
+          startThread();
+        }
+        int h = NetworkTablesJNI.addListener(m_poller, prefixes, eventKinds);
+        m_listeners.put(h, listener);
+        return h;
+      } finally {
+        m_lock.unlock();
+      }
+    }
+
+    int add(
+        int handle,
+        EnumSet<NetworkTableEvent.Kind> eventKinds,
+        Consumer<NetworkTableEvent> listener) {
+      m_lock.lock();
+      try {
+        if (m_poller == 0) {
+          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
+          startThread();
+        }
+        int h = NetworkTablesJNI.addListener(m_poller, handle, eventKinds);
+        m_listeners.put(h, listener);
+        return h;
+      } finally {
+        m_lock.unlock();
+      }
+    }
+
+    int addLogger(int minLevel, int maxLevel, Consumer<NetworkTableEvent> listener) {
+      m_lock.lock();
+      try {
+        if (m_poller == 0) {
+          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
+          startThread();
+        }
+        int h = NetworkTablesJNI.addLogger(m_poller, minLevel, maxLevel);
+        m_listeners.put(h, listener);
+        return h;
+      } finally {
+        m_lock.unlock();
+      }
+    }
+
+    void remove(int listener) {
+      m_lock.lock();
+      try {
+        m_listeners.remove(listener);
+      } finally {
+        m_lock.unlock();
+      }
+      NetworkTablesJNI.removeListener(listener);
+    }
+
+    @Override
+    public void close() {
+      if (m_poller != 0) {
+        NetworkTablesJNI.destroyListenerPoller(m_poller);
+      }
+      m_poller = 0;
+    }
+
+    private void startThread() {
+      m_thread =
+          new Thread(
+              () -> {
+                boolean wasInterrupted = false;
+                int[] handles = new int[] { m_poller, m_waitQueueEvent.getHandle() };
+                while (!Thread.interrupted()) {
+                  try {
+                    WPIUtilJNI.waitForObjects(handles);
+                  } catch (InterruptedException ex) {
+                    m_lock.lock();
+                    try {
+                      if (m_waitQueue) {
+                        m_waitQueue = false;
+                        m_waitQueueCond.signalAll();
+                      }
+                    } finally {
+                      m_lock.unlock();
+                    }
+                    Thread.currentThread().interrupt();
+                    // don't try to destroy poller, as its handle is likely no longer valid
+                    wasInterrupted = true;
+                    break;
+                  }
+                  for (NetworkTableEvent event :
+                      NetworkTablesJNI.readListenerQueue(m_inst, m_poller)) {
+                    Consumer<NetworkTableEvent> listener;
+                    m_lock.lock();
+                    try {
+                      listener = m_listeners.get(event.listener);
+                    } finally {
+                      m_lock.unlock();
+                    }
+                    if (listener != null) {
+                      try {
+                        listener.accept(event);
+                      } catch (Throwable throwable) {
+                        System.err.println(
+                            "Unhandled exception during listener callback: "
+                            + throwable.toString());
+                        throwable.printStackTrace();
+                      }
+                    }
+                  }
+                  m_lock.lock();
+                  try {
+                    if (m_waitQueue) {
+                      m_waitQueue = false;
+                      m_waitQueueCond.signalAll();
+                    }
+                  } finally {
+                    m_lock.unlock();
+                  }
+                }
+                m_lock.lock();
+                try {
+                  if (!wasInterrupted) {
+                    NetworkTablesJNI.destroyListenerPoller(m_poller);
+                  }
+                  m_poller = 0;
+                } finally {
+                  m_lock.unlock();
+                }
+              },
+              "NTListener");
+      m_thread.setDaemon(true);
+      m_thread.start();
+    }
+
+    boolean waitForQueue(double timeout) {
+      m_lock.lock();
+      try {
+        if (m_poller != 0) {
+          m_waitQueue = true;
+          m_waitQueueEvent.set();
+          while (m_waitQueue) {
+            try {
+              if (timeout < 0) {
+                m_waitQueueCond.await();
+              } else {
+                return m_waitQueueCond.await((long) (timeout * 1e9), TimeUnit.NANOSECONDS);
+              }
+            } catch (InterruptedException ex) {
+              Thread.currentThread().interrupt();
+              return true;
+            }
+          }
+        }
+      } finally {
+        m_lock.unlock();
+      }
+      return true;
+    }
+  }
+
+  private final ListenerStorage m_listeners = new ListenerStorage(this);
+
+  /**
+   * Remove a connection listener.
+   *
+   * @param listener Listener handle to remove
+   */
+  public void removeListener(int listener) {
+    m_listeners.remove(listener);
+  }
+
+  /**
+   * Wait for the listener queue to be empty. This is primarily useful for deterministic
+   * testing. This blocks until either the listener queue is empty (e.g. there are no
+   * more events that need to be passed along to callbacks or poll queues) or the timeout expires.
+   *
+   * @param timeout timeout, in seconds. Set to 0 for non-blocking behavior, or a negative value to
+   *     block indefinitely
+   * @return False if timed out, otherwise true.
+   */
+  public boolean waitForListenerQueue(double timeout) {
+    return m_listeners.waitForQueue(timeout);
+  }
+
+  /**
+   * Add a connection listener. The callback function is called asynchronously on a separate
+   * thread, so it's important to use synchronization or atomics when accessing any shared state
+   * from the callback function.
+   *
+   * @param immediateNotify Notify listener of all existing connections
+   * @param listener Listener to add
+   * @return Listener handle
+   */
+  public int addConnectionListener(
+      boolean immediateNotify, Consumer<NetworkTableEvent> listener) {
+    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kConnection);
+    if (immediateNotify) {
+      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
+    }
+    return m_listeners.add(m_handle, eventKinds, listener);
+  }
+
+  /**
+   * Add a time synchronization listener. The callback function is called asynchronously on a
+   * separate thread, so it's important to use synchronization or atomics when accessing any shared
+   * state from the callback function.
+   *
+   * @param immediateNotify Notify listener of current time synchronization value
+   * @param listener Listener to add
+   * @return Listener handle
+   */
+  public int addTimeSyncListener(
+      boolean immediateNotify, Consumer<NetworkTableEvent> listener) {
+    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kTimeSync);
+    if (immediateNotify) {
+      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
+    }
+    return m_listeners.add(m_handle, eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on a particular topic. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function.
+   *
+   * <p>This creates a corresponding internal subscriber with the lifetime of the
+   * listener.
+   *
+   * @param topic Topic
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      Topic topic,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (topic.getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("topic is not from this instance");
+    }
+    return m_listeners.add(topic.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on a subscriber. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function. This does NOT keep the subscriber
+   * active.
+   *
+   * @param subscriber Subscriber
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      Subscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (subscriber.getTopic().getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("subscriber is not from this instance");
+    }
+    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on a subscriber. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function. This does NOT keep the subscriber
+   * active.
+   *
+   * @param subscriber Subscriber
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      MultiSubscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (subscriber.getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("subscriber is not from this instance");
+    }
+    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on an entry. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function.
+   *
+   * @param entry Entry
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      NetworkTableEntry entry,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (entry.getTopic().getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("entry is not from this instance");
+    }
+    return m_listeners.add(entry.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes to topics with names that start with any of the given
+   * prefixes. The callback function is called asynchronously on a separate thread, so it's
+   * important to use synchronization or atomics when accessing any shared state from the callback
+   * function.
+   *
+   * <p>This creates a corresponding internal subscriber with the lifetime of the
+   * listener.
+   *
+   * @param prefixes Topic name string prefixes
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      String[] prefixes,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    return m_listeners.add(prefixes, eventKinds, listener);
+  }
+
+  /*
+   * Client/Server Functions
+   */
+
+  /**
+   * Get the current network mode.
+   *
+   * @return Enum set of NetworkMode.
+   */
+  public EnumSet<NetworkMode> getNetworkMode() {
+    int flags = NetworkTablesJNI.getNetworkMode(m_handle);
+    EnumSet<NetworkMode> rv = EnumSet.noneOf(NetworkMode.class);
+    for (NetworkMode mode : NetworkMode.values()) {
+      if ((flags & mode.getValue()) != 0) {
+        rv.add(mode);
+      }
+    }
+    return rv;
+  }
+
+  /**
+   * Starts local-only operation. Prevents calls to startServer or startClient from taking effect.
+   * Has no effect if startServer or startClient has already been called.
+   */
+  public void startLocal() {
+    NetworkTablesJNI.startLocal(m_handle);
+  }
+
+  /**
+   * Stops local-only operation. startServer or startClient can be called after this call to start
+   * a server or client.
+   */
+  public void stopLocal() {
+    NetworkTablesJNI.stopLocal(m_handle);
+  }
+
+  /**
+   * Starts a server using the networktables.json as the persistent file, using the default
+   * listening address and port.
+   */
+  public void startServer() {
+    startServer("networktables.json");
+  }
+
+  /**
+   * Starts a server using the specified persistent filename, using the default listening address
+   * and port.
+   *
+   * @param persistFilename the name of the persist file to use
+   */
+  public void startServer(String persistFilename) {
+    startServer(persistFilename, "");
+  }
+
+  /**
+   * Starts a server using the specified filename and listening address, using the default port.
+   *
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
+   */
+  public void startServer(String persistFilename, String listenAddress) {
+    startServer(persistFilename, listenAddress, kDefaultPort3, kDefaultPort4);
+  }
+
+  /**
+   * Starts a server using the specified filename, listening address, and port.
+   *
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
+   * @param port3 port to communicate over (NT3)
+   */
+  public void startServer(String persistFilename, String listenAddress, int port3) {
+    startServer(persistFilename, listenAddress, port3, kDefaultPort4);
+  }
+
+  /**
+   * Starts a server using the specified filename, listening address, and port.
+   *
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
+   * @param port3 port to communicate over (NT3)
+   * @param port4 port to communicate over (NT4)
+   */
+  public void startServer(String persistFilename, String listenAddress, int port3, int port4) {
+    NetworkTablesJNI.startServer(m_handle, persistFilename, listenAddress, port3, port4);
+  }
+
+  /** Stops the server if it is running. */
+  public void stopServer() {
+    NetworkTablesJNI.stopServer(m_handle);
+  }
+
+  /**
+   * Starts a NT3 client. Use SetServer or SetServerTeam to set the server name and port.
+   *
+   * @param identity network identity to advertise (cannot be empty string)
+   */
+  public void startClient3(String identity) {
+    NetworkTablesJNI.startClient3(m_handle, identity);
+  }
+
+  /**
+   * Starts a NT4 client. Use SetServer or SetServerTeam to set the server name and port.
+   *
+   * @param identity network identity to advertise (cannot be empty string)
+   */
+  public void startClient4(String identity) {
+    NetworkTablesJNI.startClient4(m_handle, identity);
+  }
+
+  /** Stops the client if it is running. */
+  public void stopClient() {
+    NetworkTablesJNI.stopClient(m_handle);
+  }
+
+  /**
+   * Sets server address and port for client (without restarting client). Changes the port to the
+   * default port.
+   *
+   * @param serverName server name
+   */
+  public void setServer(String serverName) {
+    setServer(serverName, 0);
+  }
+
+  /**
+   * Sets server address and port for client (without restarting client).
+   *
+   * @param serverName server name
+   * @param port port to communicate over (0=default)
+   */
+  public void setServer(String serverName, int port) {
+    NetworkTablesJNI.setServer(m_handle, serverName, port);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). Changes the port to the
+   * default port. The client will attempt to connect to each server in round robin fashion.
+   *
+   * @param serverNames array of server names
+   */
+  public void setServer(String[] serverNames) {
+    setServer(serverNames, 0);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). The client will attempt
+   * to connect to each server in round robin fashion.
+   *
+   * @param serverNames array of server names
+   * @param port port to communicate over (0=default)
+   */
+  public void setServer(String[] serverNames, int port) {
+    int[] ports = new int[serverNames.length];
+    for (int i = 0; i < serverNames.length; i++) {
+      ports[i] = port;
+    }
+    setServer(serverNames, ports);
+  }
+
+  /**
+   * Sets server addresses and ports for client (without restarting client). The client will
+   * attempt to connect to each server in round robin fashion.
+   *
+   * @param serverNames array of server names
+   * @param ports array of port numbers (0=default)
+   */
+  public void setServer(String[] serverNames, int[] ports) {
+    NetworkTablesJNI.setServer(m_handle, serverNames, ports);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). Changes the port to the
+   * default port. The client will attempt to connect to each server in round robin fashion.
+   *
+   * @param team team number
+   */
+  public void setServerTeam(int team) {
+    setServerTeam(team, 0);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). Connects using commonly
+   * known robot addresses for the specified team.
+   *
+   * @param team team number
+   * @param port port to communicate over (0=default)
+   */
+  public void setServerTeam(int team, int port) {
+    NetworkTablesJNI.setServerTeam(m_handle, team, port);
+  }
+
+  /**
+   * Disconnects the client if it's running and connected. This will automatically start
+   * reconnection attempts to the current server list.
+   */
+  public void disconnect() {
+    NetworkTablesJNI.disconnect(m_handle);
+  }
+
+  /**
+   * Starts requesting server address from Driver Station. This connects to the Driver Station
+   * running on localhost to obtain the server IP address, and connects with the default port.
+   */
+  public void startDSClient() {
+    startDSClient(0);
+  }
+
+  /**
+   * Starts requesting server address from Driver Station. This connects to the Driver Station
+   * running on localhost to obtain the server IP address.
+   *
+   * @param port server port to use in combination with IP from DS (0=default)
+   */
+  public void startDSClient(int port) {
+    NetworkTablesJNI.startDSClient(m_handle, port);
+  }
+
+  /** Stops requesting server address from Driver Station. */
+  public void stopDSClient() {
+    NetworkTablesJNI.stopDSClient(m_handle);
+  }
+
+  /**
+   * Flushes all updated values immediately to the local client/server. This does not flush to the
+   * network.
+   */
+  public void flushLocal() {
+    NetworkTablesJNI.flushLocal(m_handle);
+  }
+
+  /**
+   * Flushes all updated values immediately to the network. Note: This is rate-limited to protect
+   * the network from flooding. This is primarily useful for synchronizing network updates with
+   * user code.
+   */
+  public void flush() {
+    NetworkTablesJNI.flush(m_handle);
+  }
+
+  /**
+   * Gets information on the currently established network connections. If operating as a client,
+   * this will return either zero or one values.
+   *
+   * @return array of connection information
+   */
+  public ConnectionInfo[] getConnections() {
+    return NetworkTablesJNI.getConnections(m_handle);
+  }
+
+  /**
+   * Return whether or not the instance is connected to another node.
+   *
+   * @return True if connected.
+   */
+  public boolean isConnected() {
+    return NetworkTablesJNI.isConnected(m_handle);
+  }
+
+  /**
+   * Get the time offset between server time and local time. Add this value to local time to get
+   * the estimated equivalent server time. In server mode, this always returns 0. In client mode,
+   * this returns the time offset only if the client and server are connected and have exchanged
+   * synchronization messages. Note the time offset may change over time as it is periodically
+   * updated; to receive updates as events, add a listener to the "time sync" event.
+   *
+   * @return Time offset in microseconds (optional)
+   */
+  public OptionalLong getServerTimeOffset() {
+    return NetworkTablesJNI.getServerTimeOffset(m_handle);
+  }
+
+  /**
+   * Starts logging entry changes to a DataLog.
+   *
+   * @param log data log object; lifetime must extend until StopEntryDataLog is called or the
+   *     instance is destroyed
+   * @param prefix only store entries with names that start with this prefix; the prefix is not
+   *     included in the data log entry name
+   * @param logPrefix prefix to add to data log entry names
+   * @return Data logger handle
+   */
+  public int startEntryDataLog(DataLog log, String prefix, String logPrefix) {
+    return NetworkTablesJNI.startEntryDataLog(m_handle, log, prefix, logPrefix);
+  }
+
+  /**
+   * Stops logging entry changes to a DataLog.
+   *
+   * @param logger data logger handle
+   */
+  public static void stopEntryDataLog(int logger) {
+    NetworkTablesJNI.stopEntryDataLog(logger);
+  }
+
+  /**
+   * Starts logging connection changes to a DataLog.
+   *
+   * @param log data log object; lifetime must extend until StopConnectionDataLog is called or the
+   *     instance is destroyed
+   * @param name data log entry name
+   * @return Data logger handle
+   */
+  public int startConnectionDataLog(DataLog log, String name) {
+    return NetworkTablesJNI.startConnectionDataLog(m_handle, log, name);
+  }
+
+  /**
+   * Stops logging connection changes to a DataLog.
+   *
+   * @param logger data logger handle
+   */
+  public static void stopConnectionDataLog(int logger) {
+    NetworkTablesJNI.stopConnectionDataLog(logger);
+  }
+
+  /**
+   * Add logger callback function. By default, log messages are sent to stderr; this function sends
+   * log messages with the specified levels to the provided callback function instead. The callback
+   * function will only be called for log messages with level greater than or equal to minLevel and
+   * less than or equal to maxLevel; messages outside this range will be silently ignored.
+   *
+   * @param minLevel minimum log level
+   * @param maxLevel maximum log level
+   * @param func callback function
+   * @return Listener handle
+   */
+  public int addLogger(int minLevel, int maxLevel, Consumer<NetworkTableEvent> func) {
+    return m_listeners.addLogger(minLevel, maxLevel, func);
+  }
+
+  /**
+   * Returns whether there is a data schema already registered with the given name that this
+   * instance has published. This does NOT perform a check as to whether the schema has already
+   * been published by another node on the network.
+   *
+   * @param name Name (the string passed as the data type for topics using this schema)
+   * @return True if schema already registered
+   */
+  public boolean hasSchema(String name) {
+    return m_schemas.containsKey("/.schema/" + name);
+  }
+
+  /**
+   * Registers a data schema. Data schemas provide information for how a certain data type string
+   * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g.
+   * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas
+   * are published just like normal topics, with the name being generated from the provided name:
+   * "/.schema/name". Duplicate calls to this function with the same name are silently ignored.
+   *
+   * @param name Name (the string passed as the data type for topics using this schema)
+   * @param type Type of schema (e.g. "protobuf", "struct", etc)
+   * @param schema Schema data
+   */
+  public void addSchema(String name, String type, byte[] schema) {
+    m_schemas.computeIfAbsent("/.schema/" + name, k -> {
+      RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}");
+      pub.setDefault(schema);
+      return pub;
+    });
+  }
+
+  /**
+   * Registers a data schema. Data schemas provide information for how a certain data type string
+   * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g.
+   * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas
+   * are published just like normal topics, with the name being generated from the provided name:
+   * "/.schema/name". Duplicate calls to this function with the same name are silently ignored.
+   *
+   * @param name Name (the string passed as the data type for topics using this schema)
+   * @param type Type of schema (e.g. "protobuf", "struct", etc)
+   * @param schema Schema data
+   */
+  public void addSchema(String name, String type, String schema) {
+    m_schemas.computeIfAbsent("/.schema/" + name, k -> {
+      RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}");
+      pub.setDefault(StandardCharsets.UTF_8.encode(schema));
+      return pub;
+    });
+  }
+
+  /**
+   * Registers a protobuf schema. Duplicate calls to this function with the same name are silently
+   * ignored.
+   *
+   * @param proto protobuf serialization object
+   */
+  public void addSchema(Protobuf<?, ?> proto) {
+    proto.forEachDescriptor(
+        this::hasSchema,
+        (typeString, schema) -> addSchema(typeString, "proto:FileDescriptorProto", schema));
+  }
+
+  /**
+   * Registers a struct schema. Duplicate calls to this function with the same name are silently
+   * ignored.
+   *
+   * @param struct struct serialization object
+   */
+  public void addSchema(Struct<?> struct) {
+    addSchemaImpl(struct, new HashSet<>());
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (other == this) {
+      return true;
+    }
+    if (!(other instanceof NetworkTableInstance)) {
+      return false;
+    }
+
+    return m_handle == ((NetworkTableInstance) other).m_handle;
+  }
+
+  @Override
+  public int hashCode() {
+    return m_handle;
+  }
+
+  private void addSchemaImpl(Struct<?> struct, Set<String> seen) {
+    String typeString = struct.getTypeString();
+    if (hasSchema(typeString)) {
+      return;
+    }
+    if (!seen.add(typeString)) {
+      throw new UnsupportedOperationException(typeString + ": circular reference with " + seen);
+    }
+    addSchema(typeString, "structschema", struct.getSchema());
+    for (Struct<?> inner : struct.getNested()) {
+      addSchemaImpl(inner, seen);
+    }
+    seen.remove(typeString);
+  }
+
+  private boolean m_owned;
+  private int m_handle;
+  private final ConcurrentMap<String, RawPublisher> m_schemas = new ConcurrentHashMap<>();
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableValue.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableValue.java.jinja
new file mode 100644
index 0000000..cb97570
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTableValue.java.jinja
@@ -0,0 +1,251 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.Objects;
+
+/** A network table entry value. */
+@SuppressWarnings({"UnnecessaryParentheses", "PMD.MethodReturnsInternalArray"})
+public final class NetworkTableValue {
+  NetworkTableValue(NetworkTableType type, Object value, long time, long serverTime) {
+    m_type = type;
+    m_value = value;
+    m_time = time;
+    m_serverTime = serverTime;
+  }
+
+  NetworkTableValue(NetworkTableType type, Object value, long time) {
+    this(type, value, time, time == 0 ? 0 : 1);
+  }
+
+  NetworkTableValue(NetworkTableType type, Object value) {
+    this(type, value, NetworkTablesJNI.now(), 1);
+  }
+
+  NetworkTableValue(int type, Object value, long time, long serverTime) {
+    this(NetworkTableType.getFromInt(type), value, time, serverTime);
+  }
+
+  /**
+   * Get the data type.
+   *
+   * @return The type.
+   */
+  public NetworkTableType getType() {
+    return m_type;
+  }
+
+  /**
+   * Get the data value stored.
+   *
+   * @return The type.
+   */
+  public Object getValue() {
+    return m_value;
+  }
+
+  /**
+   * Get the creation time of the value in local time.
+   *
+   * @return The time, in the units returned by NetworkTablesJNI.now().
+   */
+  public long getTime() {
+    return m_time;
+  }
+
+  /**
+   * Get the creation time of the value in server time.
+   *
+   * @return The server time.
+   */
+  public long getServerTime() {
+    return m_serverTime;
+  }
+
+  /*
+   * Type Checkers
+   */
+
+  /**
+   * Determine if entry value contains a value or is unassigned.
+   *
+   * @return True if the entry value contains a value.
+   */
+  public boolean isValid() {
+    return m_type != NetworkTableType.kUnassigned;
+  }
+{% for t in types %}
+  /**
+   * Determine if entry value contains a {{ t.java.ValueType }}.
+   *
+   * @return True if the entry value is of {{ t.java.ValueType }} type.
+   */
+  public boolean is{{ t.TypeName }}() {
+    return m_type == NetworkTableType.k{{ t.TypeName }};
+  }
+{% endfor %}
+  /*
+   * Type-Safe Getters
+   */
+{% for t in types %}
+  /**
+   * Get the {{ t.java.ValueType }} value.
+   *
+   * @return The {{ t.java.ValueType }} value.
+   * @throws ClassCastException if the entry value is not of {{ t.java.ValueType }} type.
+   */
+  public {{ t.java.ValueType }} get{{ t.TypeName }}() {
+    if (m_type != NetworkTableType.k{{ t.TypeName }}) {
+      throw new ClassCastException("cannot convert " + m_type + " to {{ t.java.ValueType }}");
+    }
+    return {{ t.java.FromStorageBegin }}m_value{{ t.java.FromStorageEnd }};
+  }
+{% endfor %}
+  /*
+   * Factory functions.
+   */
+{% for t in types %}
+  /**
+   * Creates a {{ t.java.ValueType }} value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.ValueType }} value) {
+    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, {{ t.java.ToWrapObject }}(value));
+  }
+
+  /**
+   * Creates a {{ t.java.ValueType }} value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.ValueType }} value, long time) {
+    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, {{ t.java.ToWrapObject }}(value), time);
+  }
+{% if t.java.WrapValueType %}
+  /**
+   * Creates a {{ t.java.ValueType }} value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.WrapValueType }} value) {
+    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, toNative{{ t.TypeName }}(value));
+  }
+
+  /**
+   * Creates a {{ t.java.ValueType }} value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue make{{ t.TypeName }}({{ t.java.WrapValueType }} value, long time) {
+    return new NetworkTableValue(NetworkTableType.k{{ t.TypeName }}, toNative{{ t.TypeName }}(value), time);
+  }
+{% endif -%}
+{% endfor %}
+  @Override
+  public boolean equals(Object other) {
+    if (other == this) {
+      return true;
+    }
+    if (!(other instanceof NetworkTableValue)) {
+      return false;
+    }
+    NetworkTableValue ntOther = (NetworkTableValue) other;
+    return m_type == ntOther.m_type && m_value.equals(ntOther.m_value);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_type, m_value);
+  }
+
+  // arraycopy() doesn't know how to unwrap boxed values; this is a false positive in PMD
+  // (see https://sourceforge.net/p/pmd/bugs/804/)
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static boolean[] toNativeBooleanArray(Boolean[] arr) {
+    boolean[] out = new boolean[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static double[] toNativeDoubleArray(Number[] arr) {
+    double[] out = new double[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i].doubleValue();
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static long[] toNativeIntegerArray(Number[] arr) {
+    long[] out = new long[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i].longValue();
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static float[] toNativeFloatArray(Number[] arr) {
+    float[] out = new float[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i].floatValue();
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Boolean[] fromNativeBooleanArray(boolean[] arr) {
+    Boolean[] out = new Boolean[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Long[] fromNativeIntegerArray(long[] arr) {
+    Long[] out = new Long[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Float[] fromNativeFloatArray(float[] arr) {
+    Float[] out = new Float[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Double[] fromNativeDoubleArray(double[] arr) {
+    Double[] out = new Double[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  private NetworkTableType m_type;
+  private Object m_value;
+  private long m_time;
+  private long m_serverTime;
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTablesJNI.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTablesJNI.java.jinja
new file mode 100644
index 0000000..e16dbd2
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/NetworkTablesJNI.java.jinja
@@ -0,0 +1,379 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.RuntimeLoader;
+import edu.wpi.first.util.datalog.DataLog;
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.util.EnumSet;
+import java.util.OptionalLong;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+public final class NetworkTablesJNI {
+  static boolean libraryLoaded = false;
+  static RuntimeLoader<NetworkTablesJNI> loader = null;
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader =
+            new RuntimeLoader<>(
+                "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException if the library fails to load
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    loader =
+        new RuntimeLoader<>(
+            "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
+    loader.loadLibrary();
+    libraryLoaded = true;
+  }
+
+  private static PubSubOptions buildOptions(PubSubOption... options) {
+    if (options.length == 0) {
+      return null;  // optimize common case (JNI checks for null)
+    }
+    return new PubSubOptions(options);
+  }
+
+  public static native int getDefaultInstance();
+
+  public static native int createInstance();
+
+  public static native void destroyInstance(int inst);
+
+  public static native int getInstanceFromHandle(int handle);
+
+  private static native int getEntryImpl(
+      int topic, int type, String typeStr, PubSubOptions options);
+
+  public static native int getEntry(int inst, String key);
+
+  public static int getEntry(
+      int topic, int type, String typeStr, PubSubOptions options) {
+    return getEntryImpl(topic, type, typeStr, options);
+  }
+
+  public static int getEntry(
+      int topic, int type, String typeStr, PubSubOption... options) {
+    return getEntryImpl(topic, type, typeStr, buildOptions(options));
+  }
+
+  public static native String getEntryName(int entry);
+
+  public static native long getEntryLastChange(int entry);
+
+  public static native int getType(int entry);
+
+  /* Topic functions */
+
+  public static native int[] getTopics(int inst, String prefix, int types);
+
+  public static native int[] getTopicsStr(int inst, String prefix, String[] types);
+
+  public static native TopicInfo[] getTopicInfos(
+      NetworkTableInstance instObject, int inst, String prefix, int types);
+
+  public static native TopicInfo[] getTopicInfosStr(
+      NetworkTableInstance instObject, int inst, String prefix, String[] types);
+
+  public static native int getTopic(int inst, String name);
+
+  public static native String getTopicName(int topic);
+
+  public static native int getTopicType(int topic);
+
+  public static native void setTopicPersistent(int topic, boolean value);
+
+  public static native boolean getTopicPersistent(int topic);
+
+  public static native void setTopicRetained(int topic, boolean value);
+
+  public static native boolean getTopicRetained(int topic);
+
+  public static native void setTopicCached(int topic, boolean value);
+
+  public static native boolean getTopicCached(int topic);
+
+  public static native String getTopicTypeString(int topic);
+
+  public static native boolean getTopicExists(int topic);
+
+  public static native String getTopicProperty(int topic, String name);
+
+  public static native void setTopicProperty(int topic, String name, String value);
+
+  public static native void deleteTopicProperty(int topic, String name);
+
+  public static native String getTopicProperties(int topic);
+
+  public static native void setTopicProperties(int topic, String properties);
+
+  public static native int subscribe(
+      int topic, int type, String typeStr, PubSubOptions options);
+
+  public static int subscribe(
+      int topic, int type, String typeStr, PubSubOption... options) {
+    return subscribe(topic, type, typeStr, buildOptions(options));
+  }
+
+  public static native void unsubscribe(int sub);
+
+  public static native int publish(
+      int topic, int type, String typeStr, PubSubOptions options);
+
+  public static int publish(
+      int topic, int type, String typeStr, PubSubOption... options) {
+    return publish(topic, type, typeStr, buildOptions(options));
+  }
+
+  public static native int publishEx(
+      int topic, int type, String typeStr, String properties, PubSubOptions options);
+
+  public static int publishEx(
+      int topic, int type, String typeStr, String properties, PubSubOption... options) {
+    return publishEx(topic, type, typeStr, properties, buildOptions(options));
+  }
+
+  public static native void unpublish(int pubentry);
+
+  public static native void releaseEntry(int entry);
+
+  public static native void release(int pubsubentry);
+
+  public static native int getTopicFromHandle(int pubsubentry);
+
+  public static native int subscribeMultiple(int inst, String[] prefixes, PubSubOptions options);
+
+  public static int subscribeMultiple(int inst, String[] prefixes, PubSubOption... options) {
+    return subscribeMultiple(inst, prefixes, buildOptions(options));
+  }
+
+  public static native void unsubscribeMultiple(int sub);
+{% for t in types %}
+  public static native Timestamped{{ t.TypeName }} getAtomic{{ t.TypeName }}(
+      int subentry, {{ t.java.ValueType }} defaultValue);
+
+  public static native Timestamped{{ t.TypeName }}[] readQueue{{ t.TypeName }}(int subentry);
+
+  public static native {{ t.java.ValueType }}[] readQueueValues{{ t.TypeName }}(int subentry);
+{% if t.TypeName == "Raw" %}
+  public static boolean setRaw(int entry, long time, byte[] value) {
+    return setRaw(entry, time, value, 0, value.length);
+  }
+
+  public static native boolean setRaw(int entry, long time, byte[] value, int start, int len);
+
+  public static boolean setRaw(int entry, long time, ByteBuffer value) {
+    int pos = value.position();
+    return setRaw(entry, time, value, pos, value.capacity() - pos);
+  }
+
+  public static boolean setRaw(int entry, long time, ByteBuffer value, int start, int len) {
+    if (value.isDirect()) {
+      if (start < 0) {
+        throw new IndexOutOfBoundsException("start must be >= 0");
+      }
+      if (len < 0) {
+        throw new IndexOutOfBoundsException("len must be >= 0");
+      }
+      if ((start + len) > value.capacity()) {
+        throw new IndexOutOfBoundsException("start + len must be smaller than buffer capacity");
+      }
+      return setRawBuffer(entry, time, value, start, len);
+    } else if (value.hasArray()) {
+      return setRaw(entry, time, value.array(), value.arrayOffset() + start, len);
+    } else {
+      throw new UnsupportedOperationException("ByteBuffer must be direct or have a backing array");
+    }
+  }
+
+  private static native boolean setRawBuffer(int entry, long time, ByteBuffer value, int start, int len);
+{% else %}
+  public static native boolean set{{ t.TypeName }}(int entry, long time, {{ t.java.ValueType }} value);
+{% endif %}
+  public static native {{ t.java.ValueType }} get{{ t.TypeName }}(int entry, {{ t.java.ValueType }} defaultValue);
+{% if t.TypeName == "Raw" %}
+  public static boolean setDefaultRaw(int entry, long time, byte[] defaultValue) {
+    return setDefaultRaw(entry, time, defaultValue, 0, defaultValue.length);
+  }
+
+  public static native boolean setDefaultRaw(int entry, long time, byte[] defaultValue, int start, int len);
+
+  public static boolean setDefaultRaw(int entry, long time, ByteBuffer defaultValue) {
+    int pos = defaultValue.position();
+    return setDefaultRaw(entry, time, defaultValue, pos, defaultValue.limit() - pos);
+  }
+
+  public static boolean setDefaultRaw(int entry, long time, ByteBuffer defaultValue, int start, int len) {
+    if (defaultValue.isDirect()) {
+      if (start < 0) {
+        throw new IndexOutOfBoundsException("start must be >= 0");
+      }
+      if (len < 0) {
+        throw new IndexOutOfBoundsException("len must be >= 0");
+      }
+      if ((start + len) > defaultValue.capacity()) {
+        throw new IndexOutOfBoundsException("start + len must be smaller than buffer capacity");
+      }
+      return setDefaultRawBuffer(entry, time, defaultValue, start, len);
+    } else if (defaultValue.hasArray()) {
+      return setDefaultRaw(entry, time, defaultValue.array(), defaultValue.arrayOffset() + start, len);
+    } else {
+      throw new UnsupportedOperationException("ByteBuffer must be direct or have a backing array");
+    }
+  }
+
+  private static native boolean setDefaultRawBuffer(int entry, long time, ByteBuffer defaultValue, int start, int len);
+{% else %}
+  public static native boolean setDefault{{ t.TypeName }}(int entry, long time, {{ t.java.ValueType }} defaultValue);
+{% endif %}
+{% endfor %}
+  public static native NetworkTableValue[] readQueueValue(int subentry);
+
+  public static native NetworkTableValue getValue(int entry);
+
+  public static native void setEntryFlags(int entry, int flags);
+
+  public static native int getEntryFlags(int entry);
+
+  public static native TopicInfo getTopicInfo(NetworkTableInstance inst, int topic);
+
+  public static native int createListenerPoller(int inst);
+
+  public static native void destroyListenerPoller(int poller);
+
+  private static int kindsToMask(EnumSet<NetworkTableEvent.Kind> kinds) {
+    int mask = 0;
+    for (NetworkTableEvent.Kind kind : kinds) {
+      mask |= kind.getValue();
+    }
+    return mask;
+  }
+
+  public static int addListener(int poller, String[] prefixes, EnumSet<NetworkTableEvent.Kind> kinds) {
+    return addListener(poller, prefixes, kindsToMask(kinds));
+  }
+
+  public static int addListener(int poller, int handle, EnumSet<NetworkTableEvent.Kind> kinds) {
+    return addListener(poller, handle, kindsToMask(kinds));
+  }
+
+  public static native int addListener(int poller, String[] prefixes, int mask);
+
+  public static native int addListener(int poller, int handle, int mask);
+
+  public static native NetworkTableEvent[] readListenerQueue(
+      NetworkTableInstance inst, int poller);
+
+  public static native void removeListener(int listener);
+
+  public static native int getNetworkMode(int inst);
+
+  public static native void startLocal(int inst);
+
+  public static native void stopLocal(int inst);
+
+  public static native void startServer(
+      int inst, String persistFilename, String listenAddress, int port3, int port4);
+
+  public static native void stopServer(int inst);
+
+  public static native void startClient3(int inst, String identity);
+
+  public static native void startClient4(int inst, String identity);
+
+  public static native void stopClient(int inst);
+
+  public static native void setServer(int inst, String serverName, int port);
+
+  public static native void setServer(int inst, String[] serverNames, int[] ports);
+
+  public static native void setServerTeam(int inst, int team, int port);
+
+  public static native void disconnect(int inst);
+
+  public static native void startDSClient(int inst, int port);
+
+  public static native void stopDSClient(int inst);
+
+  public static native void flushLocal(int inst);
+
+  public static native void flush(int inst);
+
+  public static native ConnectionInfo[] getConnections(int inst);
+
+  public static native boolean isConnected(int inst);
+
+  public static native OptionalLong getServerTimeOffset(int inst);
+
+  public static native long now();
+
+  private static native int startEntryDataLog(int inst, long log, String prefix, String logPrefix);
+
+  public static int startEntryDataLog(int inst, DataLog log, String prefix, String logPrefix) {
+    return startEntryDataLog(inst, log.getImpl(), prefix, logPrefix);
+  }
+
+  public static native void stopEntryDataLog(int logger);
+
+  private static native int startConnectionDataLog(int inst, long log, String name);
+
+  public static int startConnectionDataLog(int inst, DataLog log, String name) {
+    return startConnectionDataLog(inst, log.getImpl(), name);
+  }
+
+  public static native void stopConnectionDataLog(int logger);
+
+  public static native int addLogger(int poller, int minLevel, int maxLevel);
+
+  /** Utility class. */
+  private NetworkTablesJNI() {}
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/Publisher.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/Publisher.java.jinja
new file mode 100644
index 0000000..d35c941
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/Publisher.java.jinja
@@ -0,0 +1,173 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+{% if TypeName == "Raw" %}
+import java.nio.ByteBuffer;
+{% endif -%}
+import {{ java.ConsumerFunctionPackage|default('java.util.function') }}.{{ java.FunctionTypePrefix }}Consumer;
+
+/** NetworkTables {{ TypeName }} publisher. */
+public interface {{ TypeName }}Publisher extends Publisher, {{ java.FunctionTypePrefix }}Consumer{{ java.FunctionTypeSuffix }} {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  {{ TypeName }}Topic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set({{ java.ValueType }} value) {
+    set(value, 0);
+  }
+
+{% if TypeName == "Raw" %}
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  default void set(byte[] value, long time) {
+    set(value, 0, value.length, time);
+  }
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   */
+  default void set(byte[] value, int start, int len) {
+    set(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(byte[] value, int start, int len, long time);
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish; will send from value.position() to value.limit()
+   */
+  default void set(ByteBuffer value) {
+    set(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish; will send from value.position() to value.limit()
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  default void set(ByteBuffer value, long time) {
+    int pos = value.position();
+    set(value, pos, value.limit() - pos, time);
+  }
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   */
+  default void set(ByteBuffer value, int start, int len) {
+    set(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(ByteBuffer value, int start, int len, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  default void setDefault(byte[] value) {
+    setDefault(value, 0, value.length);
+  }
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   */
+  void setDefault(byte[] value, int start, int len);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value; will send from value.position() to value.limit()
+   */
+  default void setDefault(ByteBuffer value) {
+    int pos = value.position();
+    setDefault(value, pos, value.limit() - pos);
+  }
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   */
+  void setDefault(ByteBuffer value, int start, int len);
+{% else %}
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set({{ java.ValueType }} value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault({{ java.ValueType }} value);
+{% endif %}
+  @Override
+  default void accept({{ java.ValueType }} value) {
+    set(value);
+  }
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/Subscriber.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/Subscriber.java.jinja
new file mode 100644
index 0000000..73e6190
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/Subscriber.java.jinja
@@ -0,0 +1,86 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import {{ java.SupplierFunctionPackage|default('java.util.function') }}.{{ java.FunctionTypePrefix }}Supplier;
+
+/** NetworkTables {{ TypeName }} subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface {{ TypeName }}Subscriber extends Subscriber, {{ java.FunctionTypePrefix }}Supplier{{ java.FunctionTypeSuffix }} {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  {{ TypeName }}Topic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  {{ java.ValueType }} get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  {{ java.ValueType }} get({{ java.ValueType }} defaultValue);
+{% if java.FunctionTypePrefix %}
+  @Override
+  default {{ java.ValueType }} getAs{{ java.FunctionTypePrefix }}() {
+    return get();
+  }
+{% endif %}
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  Timestamped{{ TypeName }} getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  Timestamped{{ TypeName }} getAtomic({{ java.ValueType }} defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  Timestamped{{ TypeName }}[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  {{ java.ValueType }}[] readQueueValues();
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/Timestamped.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/Timestamped.java.jinja
new file mode 100644
index 0000000..21e50e2
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/Timestamped.java.jinja
@@ -0,0 +1,43 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped {{ TypeName }}. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class Timestamped{{ TypeName }} {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public Timestamped{{ TypeName }}(long timestamp, long serverTime, {{ java.ValueType }} value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final {{ java.ValueType }} value;
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/java/Topic.java.jinja b/third_party/allwpilib/ntcore/src/generate/main/java/Topic.java.jinja
new file mode 100644
index 0000000..6407a4b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/java/Topic.java.jinja
@@ -0,0 +1,226 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables {{ TypeName }} topic. */
+public final class {{ TypeName }}Topic extends Topic {
+{%- if TypeString %}
+  /** The default type string for this topic type. */
+  public static final String kTypeString = {{ TypeString }};
+{% endif %}
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public {{ TypeName }}Topic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.get{{TypeName}}Topic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public {{ TypeName }}Topic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+{%- if not TypeString %}
+   * @param typeString type string
+{% endif %}
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public {{ TypeName }}Subscriber subscribe(
+{%- if not TypeString %}
+      String typeString,
+{% endif %}
+      {{ java.ValueType }} defaultValue,
+      PubSubOption... options) {
+    return new {{ TypeName }}EntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
+            {{ TypeString|default('typeString') }}, options),
+        defaultValue);
+  }
+{% if TypeString %}
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public {{ TypeName }}Subscriber subscribeEx(
+      String typeString,
+      {{ java.ValueType }} defaultValue,
+      PubSubOption... options) {
+    return new {{ TypeName }}EntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+{% endif %}
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+{%- if not TypeString %}
+   * @param typeString type string
+{% endif %}
+   * @param options publish options
+   * @return publisher
+   */
+  public {{ TypeName }}Publisher publish(
+{%- if not TypeString %}
+      String typeString,
+{% endif %}
+      PubSubOption... options) {
+    return new {{ TypeName }}EntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
+            {{ TypeString|default('typeString') }}, options),
+        {{ java.EmptyValue }});
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public {{ TypeName }}Publisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new {{ TypeName }}EntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
+            typeString, properties, options),
+        {{ java.EmptyValue }});
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+{%- if not TypeString %}
+   * @param typeString type string
+{% endif %}
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public {{ TypeName }}Entry getEntry(
+{%- if not TypeString %}
+      String typeString,
+{% endif %}
+      {{ java.ValueType }} defaultValue,
+      PubSubOption... options) {
+    return new {{ TypeName }}EntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
+            {{ TypeString|default('typeString') }}, options),
+        defaultValue);
+  }
+{% if TypeString %}
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public {{ TypeName }}Entry getEntryEx(
+      String typeString,
+      {{ java.ValueType }} defaultValue,
+      PubSubOption... options) {
+    return new {{ TypeName }}EntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.k{{ TypeName }}.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+{% endif %}
+}
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/native/cpp/jni/types_jni.cpp.jinja b/third_party/allwpilib/ntcore/src/generate/main/native/cpp/jni/types_jni.cpp.jinja
new file mode 100644
index 0000000..fc811f5
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/native/cpp/jni/types_jni.cpp.jinja
@@ -0,0 +1,372 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_networktables_NetworkTablesJNI.h"
+#include "ntcore.h"
+
+using namespace wpi::java;
+
+//
+// Globals and load/unload
+//
+{% for t in types %}
+static JClass timestamped{{ t.TypeName }}Cls;
+{%- endfor %}
+{%- for t in types %}
+{%- if t.jni.ToJavaArray == "MakeJObjectArray" %}
+static JClass {{ t.jni.jtype }}Cls;
+{%- endif %}
+{%- endfor %}
+static JException illegalArgEx;
+static JException indexOobEx;
+static JException nullPointerEx;
+
+static const JClassInit classes[] = {
+{%- for t in types %}
+    {"edu/wpi/first/networktables/Timestamped{{ t.TypeName }}", &timestamped{{ t.TypeName }}Cls},
+{%- endfor %}
+{%- for t in types %}
+{%- if t.jni.ToJavaArray == "MakeJObjectArray" %}
+    {"{{ t.jni.jtypestr }}", &{{ t.jni.jtype }}Cls},
+{%- endif %}
+{%- endfor %}
+};
+
+static const JExceptionInit exceptions[] = {
+    {"java/lang/IllegalArgumentException", &illegalArgEx},
+    {"java/lang/IndexOutOfBoundsException", &indexOobEx},
+    {"java/lang/NullPointerException", &nullPointerEx},
+};
+
+namespace nt {
+
+bool JNI_LoadTypes(JNIEnv* env) {
+  // Cache references to classes
+  for (auto& c : classes) {
+    *c.cls = JClass(env, c.name);
+    if (!*c.cls) {
+      return false;
+    }
+  }
+
+  for (auto& c : exceptions) {
+    *c.cls = JException(env, c.name);
+    if (!*c.cls) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+void JNI_UnloadTypes(JNIEnv* env) {
+  // Delete global references
+  for (auto& c : classes) {
+    c.cls->free(env);
+  }
+  for (auto& c : exceptions) {
+    c.cls->free(env);
+  }
+}
+
+}  // namespace nt
+
+static std::vector<int> FromJavaBooleanArray(JNIEnv* env, jbooleanArray jarr) {
+  CriticalJSpan<const jboolean> ref{env, jarr};
+  if (!ref) {
+    return {};
+  }
+  std::span<const jboolean> elements{ref};
+  size_t len = elements.size();
+  std::vector<int> arr;
+  arr.reserve(len);
+  for (size_t i = 0; i < len; ++i) {
+    arr.push_back(elements[i]);
+  }
+  return arr;
+}
+
+static std::vector<std::string> FromJavaStringArray(JNIEnv* env, jobjectArray jarr) {
+  size_t len = env->GetArrayLength(jarr);
+  std::vector<std::string> arr;
+  arr.reserve(len);
+  for (size_t i = 0; i < len; ++i) {
+    JLocal<jstring> elem{
+        env, static_cast<jstring>(env->GetObjectArrayElement(jarr, i))};
+    if (!elem) {
+      return {};
+    }
+    arr.emplace_back(JStringRef{env, elem}.str());
+  }
+  return arr;
+}
+
+{% for t in types %}
+static jobject MakeJObject(JNIEnv* env, nt::Timestamped{{ t.TypeName }} value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestamped{{ t.TypeName }}Cls, "<init>", "(JJ{{ t.jni.jtypestr }})V");
+{%- if t.jni.JavaObject %}
+  JLocal<{{ t.jni.jtype }}> val{env, {{ t.jni.ToJavaBegin }}value.value{{ t.jni.ToJavaEnd }}};
+  return env->NewObject(timestamped{{ t.TypeName }}Cls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+{%- else %}
+  return env->NewObject(timestamped{{ t.TypeName }}Cls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime),
+                        {{ t.jni.ToJavaBegin }}value.value{{ t.jni.ToJavaEnd }});
+{%- endif %}
+}
+{% endfor %}
+{%- for t in types %}
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::Timestamped{{ t.TypeName }}> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestamped{{ t.TypeName }}Cls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+{% endfor %}
+{%- for t in types %}
+{%- if t.jni.ToJavaArray == "MakeJObjectArray" %}
+static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const {{ t.cpp.ValueType }}> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), {{ t.jni.jtype }}Cls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, {{ t.jni.ToJavaBegin }}arr[i]{{ t.jni.ToJavaEnd }}};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+{% endif %}
+{%- endfor %}
+
+extern "C" {
+{% for t in types %}
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomic{{ t.TypeName }}
+ * Signature: (I{{ t.jni.jtypestr }})Ledu/wpi/first/networktables/Timestamped{{ t.TypeName }};
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomic{{ t.TypeName }}
+  (JNIEnv* env, jclass, jint subentry, {{ t.jni.jtype }} defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomic{{ t.TypeName }}(subentry, {{ t.jni.FromJavaBegin }}defaultValue{{ t.jni.FromJavaEnd }}));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueue{{ t.TypeName }}
+ * Signature: (I)[Ledu/wpi/first/networktables/Timestamped{{ t.TypeName }};
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueue{{ t.TypeName }}
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueue{{ t.TypeName }}(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValues{{ t.TypeName }}
+ * Signature: (I)[{{ t.jni.jtypestr }}
+ */
+JNIEXPORT {% if t.jni.JavaObject %}jobject{% else %}{{ t.jni.jtype }}{% endif %}Array JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValues{{ t.TypeName }}
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return {{ t.jni.ToJavaArray }}(env, nt::ReadQueueValues{{ t.TypeName }}(subentry));
+}
+{% if t.TypeName == "Raw" %}
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setRaw
+ * Signature: (IJ[BII)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setRaw
+  (JNIEnv* env, jclass, jint entry, jlong time, jbyteArray value, jint start, jint len)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  CriticalJSpan<const jbyte> cvalue{env, value};
+  if (static_cast<unsigned int>(start + len) > cvalue.size()) {
+    indexOobEx.Throw(env, "start + len must be smaller than array length");
+    return false;
+  }
+  return nt::SetRaw(entry, cvalue.uarray().subspan(start, len), time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setRawBuffer
+ * Signature: (IJLjava/nio/ByteBuffer;II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setRawBuffer
+  (JNIEnv* env, jclass, jint entry, jlong time, jobject value, jint start, jint len)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  JSpan<const jbyte> cvalue{env, value, static_cast<size_t>(start + len)};
+  if (!cvalue) {
+    illegalArgEx.Throw(env, "value must be a native ByteBuffer");
+    return false;
+  }
+  return nt::SetRaw(entry, cvalue.uarray().subspan(start, len), time);
+}
+{% else %}
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    set{{ t.TypeName }}
+ * Signature: (IJ{{ t.jni.jtypestr }})Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_set{{ t.TypeName }}
+  (JNIEnv*{% if t.jni.JavaObject %} env{% endif %}, jclass, jint entry, jlong time, {{ t.jni.jtype }} value)
+{
+{%- if t.jni.JavaObject %}
+  if (!value) {
+    nullPointerEx.Throw(env, "value cannot be null");
+    return false;
+  }
+{%- endif %}
+  return nt::Set{{ t.TypeName }}(entry, {{ t.jni.FromJavaBegin }}value{{ t.jni.FromJavaEnd }}, time);
+}
+{% endif %}
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    get{{ t.TypeName }}
+ * Signature: (I{{ t.jni.jtypestr }}){{ t.jni.jtypestr }}
+ */
+JNIEXPORT {{ t.jni.jtype }} JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_get{{ t.TypeName }}
+  (JNIEnv*{% if t.jni.JavaObject %} env{% endif %}, jclass, jint entry, {{ t.jni.jtype }} defaultValue)
+{
+{%- if t.jni.JavaObject %}
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.Is{{ t.TypeName }}()) {
+    return defaultValue;
+  }
+  return {{ t.jni.ToJavaBegin }}val.Get{{ t.TypeName }}(){{ t.jni.ToJavaEnd }};
+{%- else %}
+  return nt::Get{{ t.TypeName }}(entry, defaultValue);
+{%- endif %}
+}
+{% if t.TypeName == "Raw" %}
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultRaw
+ * Signature: (IJ[BII)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultRaw
+  (JNIEnv* env, jclass, jint entry, jlong, jbyteArray defaultValue, jint start, jint len)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  CriticalJSpan<const jbyte> cvalue{env, defaultValue};
+  if (static_cast<unsigned int>(start + len) > cvalue.size()) {
+    indexOobEx.Throw(env, "start + len must be smaller than array length");
+    return false;
+  }
+  return nt::SetDefaultRaw(entry, cvalue.uarray().subspan(start, len));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultRawBuffer
+ * Signature: (IJLjava/nio/ByteBuffer;II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultRawBuffer
+  (JNIEnv* env, jclass, jint entry, jlong, jobject defaultValue, jint start, jint len)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  JSpan<const jbyte> cvalue{env, defaultValue, static_cast<size_t>(start + len)};
+  if (!cvalue) {
+    illegalArgEx.Throw(env, "value must be a native ByteBuffer");
+    return false;
+  }
+  return nt::SetDefaultRaw(entry, cvalue.uarray().subspan(start, len));
+}
+{% else %}
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefault{{ t.TypeName }}
+ * Signature: (IJ{{ t.jni.jtypestr }})Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefault{{ t.TypeName }}
+  (JNIEnv*{% if t.jni.JavaObject %} env{% endif %}, jclass, jint entry, jlong, {{ t.jni.jtype }} defaultValue)
+{
+{%- if t.jni.JavaObject %}
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "defaultValue cannot be null");
+    return false;
+  }
+{%- endif %}
+  return nt::SetDefault{{ t.TypeName }}(entry, {{ t.jni.FromJavaBegin }}defaultValue{{ t.jni.FromJavaEnd }});
+}
+{% endif %}
+{% endfor %}
+}  // extern "C"
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/native/cpp/ntcore_c_types.cpp.jinja b/third_party/allwpilib/ntcore/src/generate/main/native/cpp/ntcore_c_types.cpp.jinja
new file mode 100644
index 0000000..a225634
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/native/cpp/ntcore_c_types.cpp.jinja
@@ -0,0 +1,109 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#include "ntcore_c_types.h"
+
+#include "Value_internal.h"
+#include "ntcore_cpp.h"
+
+using namespace nt;
+
+template <typename T>
+static inline std::span<const T> ConvertFromC(const T* arr, size_t size) {
+  return {arr, size};
+}
+
+static inline std::string_view ConvertFromC(const char* arr, size_t size) {
+  return {arr, size};
+}
+
+static std::vector<std::string> ConvertFromC(const NT_String* arr, size_t size) {
+  std::vector<std::string> v;
+  v.reserve(size);
+  for (size_t i = 0; i < size; ++i) {
+    v.emplace_back(ConvertFromC(arr[i]));
+  }
+  return v;
+}
+
+{% for t in types %}
+static void ConvertToC(const nt::Timestamped{{ t.TypeName }}& in, NT_Timestamped{{ t.TypeName }}* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+{%- if t.c.IsArray %}
+  out->value = ConvertToC<{{ t.c.ValueType[:-1] }}>(in.value, &out->len);
+{% else %}
+  out->value = in.value;
+{% endif -%}
+}
+{% endfor %}
+
+extern "C" {
+{% for t in types %}
+NT_Bool NT_Set{{ t.TypeName }}(NT_Handle pubentry, int64_t time, {{ t.c.ParamType }} value{% if t.c.IsArray %}, size_t len{% endif %}) {
+{%- if t.c.IsArray %}
+  return nt::Set{{ t.TypeName }}(pubentry, ConvertFromC(value, len), time);
+{%- else %}
+  return nt::Set{{ t.TypeName }}(pubentry, value, time);
+{%- endif %}
+}
+
+NT_Bool NT_SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %}) {
+{%- if t.c.IsArray %}
+  return nt::SetDefault{{ t.TypeName }}(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+{%- else %}
+  return nt::SetDefault{{ t.TypeName }}(pubentry, defaultValue);
+{%- endif %}
+}
+
+{{ t.c.ValueType }} NT_Get{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen, size_t* len{% endif %}) {
+{%- if t.c.IsArray %}
+  auto cppValue = nt::Get{{ t.TypeName }}(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<{{ t.c.ValueType[:-1] }}>(cppValue, len);
+{%- else %}
+  return nt::Get{{ t.TypeName }}(subentry, defaultValue);
+{%- endif %}
+}
+
+void NT_GetAtomic{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %}, struct NT_Timestamped{{ t.TypeName }}* value) {
+{%- if t.c.IsArray %}
+  auto cppValue = nt::GetAtomic{{ t.TypeName }}(subentry, ConvertFromC(defaultValue, defaultValueLen));
+{%- else %}
+  auto cppValue = nt::GetAtomic{{ t.TypeName }}(subentry, defaultValue);
+{%- endif %}
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestamped{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* value) {
+{%- if t.TypeName == "StringArray" %}
+  NT_FreeStringArray(value->value, value->len);
+{%- elif t.c.IsArray %}
+  std::free(value->value);
+{%- endif %}
+}
+
+struct NT_Timestamped{{ t.TypeName }}* NT_ReadQueue{{ t.TypeName }}(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueue{{ t.TypeName }}(subentry);
+  return ConvertToC<NT_Timestamped{{ t.TypeName }}>(arr, len);
+}
+
+void NT_FreeQueue{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestamped{{ t.TypeName }}(&arr[i]);
+  }
+  std::free(arr);
+}
+
+{%- if not t.c.IsArray %}
+{{ t.c.ValueType }}* NT_ReadQueueValues{{ t.TypeName }}(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueValues{{ t.TypeName }}(subentry);
+  return ConvertToC<{{ t.c.ValueType }}>(arr, len);
+}
+{%- endif %}
+
+{% endfor %}
+}  // extern "C"
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/native/cpp/ntcore_cpp_types.cpp.jinja b/third_party/allwpilib/ntcore/src/generate/main/native/cpp/ntcore_cpp_types.cpp.jinja
new file mode 100644
index 0000000..d9501f8
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/native/cpp/ntcore_cpp_types.cpp.jinja
@@ -0,0 +1,134 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#include "ntcore_cpp_types.h"
+
+#include "Handle.h"
+#include "InstanceImpl.h"
+
+namespace {
+template <nt::ValidType T>
+struct ValuesType {
+  using Vector =
+      std::vector<typename nt::TypeInfo<std::remove_cvref_t<T>>::Value>;
+};
+
+template <>
+struct ValuesType<bool> {
+  using Vector = std::vector<int>;
+};
+}  // namespace
+
+namespace nt {
+
+template <ValidType T>
+static inline bool Set(NT_Handle pubentry, typename TypeInfo<T>::View value,
+                       int64_t time) {
+  if (auto ii = InstanceImpl::Get(Handle{pubentry}.GetInst())) {
+    return ii->localStorage.SetEntryValue(
+        pubentry, MakeValue<T>(value, time == 0 ? Now() : time));
+  } else {
+    return {};
+  }
+}
+
+template <ValidType T>
+static inline bool SetDefault(NT_Handle pubentry,
+                              typename TypeInfo<T>::View defaultValue) {
+  if (auto ii = InstanceImpl::Get(Handle{pubentry}.GetInst())) {
+    return ii->localStorage.SetDefaultEntryValue(pubentry,
+                                                 MakeValue<T>(defaultValue, 1));
+  } else {
+    return {};
+  }
+}
+
+template <ValidType T>
+static inline Timestamped<typename TypeInfo<T>::Value> GetAtomic(
+    NT_Handle subentry, typename TypeInfo<T>::View defaultValue) {
+  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
+    return ii->localStorage.GetAtomic<T>(subentry, defaultValue);
+  } else {
+    return {};
+  }
+}
+
+template <ValidType T>
+inline Timestamped<typename TypeInfo<T>::SmallRet> GetAtomic(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<typename TypeInfo<T>::SmallElem>& buf,
+    typename TypeInfo<T>::View defaultValue) {
+  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
+    return ii->localStorage.GetAtomic<T>(subentry, buf, defaultValue);
+  } else {
+    return {};
+  }
+}
+
+template <typename T>
+static inline std::vector<Timestamped<typename TypeInfo<T>::Value>> ReadQueue(
+    NT_Handle subentry) {
+  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
+    return ii->localStorage.ReadQueue<T>(subentry);
+  } else {
+    return {};
+  }
+}
+
+template <typename T>
+static inline typename ValuesType<T>::Vector ReadQueueValues(
+    NT_Handle subentry) {
+  typename ValuesType<T>::Vector rv;
+  auto arr = ReadQueue<T>(subentry);
+  rv.reserve(arr.size());
+  for (auto&& elem : arr) {
+    rv.emplace_back(std::move(elem.value));
+  }
+  return rv;
+}
+{% for t in types %}
+bool Set{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} value, int64_t time) {
+  return Set<{{ t.cpp.TemplateType }}>(pubentry, value, time);
+}
+
+bool SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} defaultValue) {
+  return SetDefault<{{ t.cpp.TemplateType }}>(pubentry, defaultValue);
+}
+
+{{ t.cpp.ValueType }} Get{{ t.TypeName }}(NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue) {
+  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, defaultValue).value;
+}
+
+Timestamped{{ t.TypeName }} GetAtomic{{ t.TypeName }}(
+    NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue) {
+  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, defaultValue);
+}
+
+std::vector<Timestamped{{ t.TypeName }}> ReadQueue{{ t.TypeName }}(NT_Handle subentry) {
+  return ReadQueue<{{ t.cpp.TemplateType }}>(subentry);
+}
+
+std::vector<{% if t.cpp.ValueType == "bool" %}int{% else %}{{ t.cpp.ValueType }}{% endif %}> ReadQueueValues{{ t.TypeName }}(NT_Handle subentry) {
+  return ReadQueueValues<{{ t.cpp.TemplateType }}>(subentry);
+}
+{% if t.cpp.SmallRetType and t.cpp.SmallElemType %}
+{{ t.cpp.SmallRetType }} Get{{ t.TypeName }}(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf,
+    {{ t.cpp.ParamType }} defaultValue) {
+  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, buf, defaultValue).value;
+}
+
+Timestamped{{ t.TypeName }}View GetAtomic{{ t.TypeName }}(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf,
+    {{ t.cpp.ParamType }} defaultValue) {
+  return GetAtomic<{{ t.cpp.TemplateType }}>(subentry, buf, defaultValue);
+}
+{% endif %}
+{% endfor %}
+}  // namespace nt
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/native/include/networktables/Topic.h.jinja b/third_party/allwpilib/ntcore/src/generate/main/native/include/networktables/Topic.h.jinja
new file mode 100644
index 0000000..93abc49
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/native/include/networktables/Topic.h.jinja
@@ -0,0 +1,440 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+{{ cpp.INCLUDES }}
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class {{ TypeName }}Topic;
+
+/**
+ * NetworkTables {{ TypeName }} subscriber.
+ */
+class {{ TypeName }}Subscriber : public Subscriber {
+ public:
+  using TopicType = {{ TypeName }}Topic;
+  using ValueType = {{ cpp.ValueType }};
+  using ParamType = {{ cpp.ParamType }};
+  using TimestampedValueType = Timestamped{{ TypeName }};
+{% if cpp.SmallRetType and cpp.SmallElemType %}
+  using SmallRetType = {{ cpp.SmallRetType }};
+  using SmallElemType = {{ cpp.SmallElemType }};
+  using TimestampedValueViewType = Timestamped{{ TypeName }}View;
+{% endif %}
+
+  {{ TypeName }}Subscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * {{TypeName}}Topic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  {{ TypeName }}Subscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+{% if cpp.SmallRetType and cpp.SmallElemType %}
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @param buf storage for returned value
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
+{% endif %}
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+{% if cpp.SmallRetType and cpp.SmallElemType %}
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf,
+      ParamType defaultValue) const;
+{% endif %}
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables {{ TypeName }} publisher.
+ */
+class {{ TypeName }}Publisher : public Publisher {
+ public:
+  using TopicType = {{ TypeName }}Topic;
+  using ValueType = {{ cpp.ValueType }};
+  using ParamType = {{ cpp.ParamType }};
+{% if cpp.SmallRetType and cpp.SmallElemType %}
+  using SmallRetType = {{ cpp.SmallRetType }};
+  using SmallElemType = {{ cpp.SmallElemType }};
+{% endif %}
+  using TimestampedValueType = Timestamped{{ TypeName }};
+
+  {{ TypeName }}Publisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * {{TypeName}}Topic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit {{ TypeName }}Publisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables {{ TypeName }} entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class {{ TypeName }}Entry final : public {{ TypeName }}Subscriber,
+                                  public {{ TypeName }}Publisher {
+ public:
+  using SubscriberType = {{ TypeName }}Subscriber;
+  using PublisherType = {{ TypeName }}Publisher;
+  using TopicType = {{ TypeName }}Topic;
+  using ValueType = {{ cpp.ValueType }};
+  using ParamType = {{ cpp.ParamType }};
+{% if cpp.SmallRetType and cpp.SmallElemType %}
+  using SmallRetType = {{ cpp.SmallRetType }};
+  using SmallElemType = {{ cpp.SmallElemType }};
+{% endif %}
+  using TimestampedValueType = Timestamped{{ TypeName }};
+
+  {{ TypeName }}Entry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * {{TypeName}}Topic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  {{ TypeName }}Entry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables {{ TypeName }} topic.
+ */
+class {{ TypeName }}Topic final : public Topic {
+ public:
+  using SubscriberType = {{ TypeName }}Subscriber;
+  using PublisherType = {{ TypeName }}Publisher;
+  using EntryType = {{ TypeName }}Entry;
+  using ValueType = {{ cpp.ValueType }};
+  using ParamType = {{ cpp.ParamType }};
+  using TimestampedValueType = Timestamped{{ TypeName }};
+{%- if TypeString %}
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = {{ TypeString }};
+{%- endif %}
+
+  {{ TypeName }}Topic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::Get{{TypeName}}Topic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit {{ TypeName }}Topic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit {{ TypeName }}Topic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+{%- if not TypeString %}
+   * @param typeString type string
+{% endif %}
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      {% if not TypeString %}std::string_view typeString, {% endif %}ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+{%- if TypeString %}
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+{% endif %}
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+{%- if not TypeString %}
+   * @param typeString type string
+{% endif %}
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish({% if not TypeString %}std::string_view typeString, {% endif %}const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+{%- if not TypeString %}
+   * @param typeString type string
+{% endif %}
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry({% if not TypeString %}std::string_view typeString, {% endif %}ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+{%- if TypeString %}
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+{% endif %}
+};
+
+}  // namespace nt
+
+#include "networktables/{{ TypeName }}Topic.inc"
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/native/include/networktables/Topic.inc.jinja b/third_party/allwpilib/ntcore/src/generate/main/native/include/networktables/Topic.inc.jinja
new file mode 100644
index 0000000..ce343d9
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/native/include/networktables/Topic.inc.jinja
@@ -0,0 +1,140 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/{{ TypeName }}Topic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline {{ TypeName }}Subscriber::{{ TypeName }}Subscriber(
+    NT_Subscriber handle, {{ cpp.ParamType }} defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{{ '{' }}{{ cpp.DefaultValueCopy|default('defaultValue') }}} {}
+
+inline {{ cpp.ValueType }} {{ TypeName }}Subscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline {{ cpp.ValueType }} {{ TypeName }}Subscriber::Get(
+    {{ cpp.ParamType }} defaultValue) const {
+  return ::nt::Get{{ TypeName }}(m_subHandle, defaultValue);
+}
+{% if cpp.SmallRetType and cpp.SmallElemType %}
+inline {{ cpp.SmallRetType }} {{ TypeName }}Subscriber::Get(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf) const {
+  return Get(buf, m_defaultValue);
+}
+
+inline {{ cpp.SmallRetType }} {{ TypeName }}Subscriber::Get(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf, {{ cpp.ParamType }} defaultValue) const {
+  return nt::Get{{ TypeName }}(m_subHandle, buf, defaultValue);
+}
+{% endif %}
+inline Timestamped{{ TypeName }} {{ TypeName }}Subscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline Timestamped{{ TypeName }} {{ TypeName }}Subscriber::GetAtomic(
+    {{ cpp.ParamType }} defaultValue) const {
+  return ::nt::GetAtomic{{ TypeName }}(m_subHandle, defaultValue);
+}
+{% if cpp.SmallRetType and cpp.SmallElemType %}
+inline Timestamped{{ TypeName }}View {{ TypeName }}Subscriber::GetAtomic(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf) const {
+  return GetAtomic(buf, m_defaultValue);
+}
+
+inline Timestamped{{ TypeName }}View {{ TypeName }}Subscriber::GetAtomic(wpi::SmallVectorImpl<{{ cpp.SmallElemType }}>& buf, {{ cpp.ParamType }} defaultValue) const {
+  return nt::GetAtomic{{ TypeName }}(m_subHandle, buf, defaultValue);
+}
+{% endif %}
+inline std::vector<Timestamped{{ TypeName }}>
+{{ TypeName }}Subscriber::ReadQueue() {
+  return ::nt::ReadQueue{{ TypeName }}(m_subHandle);
+}
+
+inline {{ TypeName }}Topic {{ TypeName }}Subscriber::GetTopic() const {
+  return {{ TypeName }}Topic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline {{ TypeName }}Publisher::{{ TypeName }}Publisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void {{ TypeName }}Publisher::Set({{ cpp.ParamType }} value,
+                                         int64_t time) {
+  ::nt::Set{{ TypeName }}(m_pubHandle, value, time);
+}
+
+inline void {{ TypeName }}Publisher::SetDefault({{ cpp.ParamType }} value) {
+  ::nt::SetDefault{{ TypeName }}(m_pubHandle, value);
+}
+
+inline {{ TypeName }}Topic {{ TypeName }}Publisher::GetTopic() const {
+  return {{ TypeName }}Topic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline {{ TypeName }}Entry::{{ TypeName }}Entry(
+    NT_Entry handle, {{ cpp.ParamType }} defaultValue)
+    : {{ TypeName }}Subscriber{handle, defaultValue},
+      {{ TypeName }}Publisher{handle} {}
+
+inline {{ TypeName }}Topic {{ TypeName }}Entry::GetTopic() const {
+  return {{ TypeName }}Topic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void {{ TypeName }}Entry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline {{ TypeName }}Subscriber {{ TypeName }}Topic::Subscribe(
+    {% if not TypeString %}std::string_view typeString, {% endif %}{{ cpp.ParamType }} defaultValue,
+    const PubSubOptions& options) {
+  return {{ TypeName }}Subscriber{
+      ::nt::Subscribe(m_handle, NT_{{ cpp.TYPE_NAME }}, {{ TypeString|default('typeString') }}, options),
+      defaultValue};
+}
+{%- if TypeString %}
+inline {{ TypeName }}Subscriber {{ TypeName }}Topic::SubscribeEx(
+    std::string_view typeString, {{ cpp.ParamType }} defaultValue,
+    const PubSubOptions& options) {
+  return {{ TypeName }}Subscriber{
+      ::nt::Subscribe(m_handle, NT_{{ cpp.TYPE_NAME }}, typeString, options),
+      defaultValue};
+}
+{% endif %}
+inline {{ TypeName }}Publisher {{ TypeName }}Topic::Publish(
+    {% if not TypeString %}std::string_view typeString, {% endif %}const PubSubOptions& options) {
+  return {{ TypeName }}Publisher{
+      ::nt::Publish(m_handle, NT_{{ cpp.TYPE_NAME }}, {{ TypeString|default('typeString') }}, options)};
+}
+
+inline {{ TypeName }}Publisher {{ TypeName }}Topic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return {{ TypeName }}Publisher{
+      ::nt::PublishEx(m_handle, NT_{{ cpp.TYPE_NAME }}, typeString, properties, options)};
+}
+
+inline {{ TypeName }}Entry {{ TypeName }}Topic::GetEntry(
+    {% if not TypeString %}std::string_view typeString, {% endif %}{{ cpp.ParamType }} defaultValue,
+    const PubSubOptions& options) {
+  return {{ TypeName }}Entry{
+      ::nt::GetEntry(m_handle, NT_{{ cpp.TYPE_NAME }}, {{ TypeString|default('typeString') }}, options),
+      defaultValue};
+}
+{%- if TypeString %}
+inline {{ TypeName }}Entry {{ TypeName }}Topic::GetEntryEx(
+    std::string_view typeString, {{ cpp.ParamType }} defaultValue,
+    const PubSubOptions& options) {
+  return {{ TypeName }}Entry{
+      ::nt::GetEntry(m_handle, NT_{{ cpp.TYPE_NAME }}, typeString, options),
+      defaultValue};
+}
+{% endif %}
+}  // namespace nt
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/native/include/ntcore_c_types.h.jinja b/third_party/allwpilib/ntcore/src/generate/main/native/include/ntcore_c_types.h.jinja
new file mode 100644
index 0000000..22866fb
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/native/include/ntcore_c_types.h.jinja
@@ -0,0 +1,154 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include "ntcore_c.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% for t in types %}
+/**
+ * Timestamped {{ t.TypeName }}.
+ * @ingroup ntcore_c_api
+ */
+struct NT_Timestamped{{ t.TypeName }} {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  {{ t.c.ValueType }} value;
+{%- if t.c.IsArray %}
+  /**
+   * Value length.
+   */
+  size_t len;
+{% endif %}
+};
+
+/**
+ * @defgroup ntcore_{{ t.TypeName }}_cfunc {{ t.TypeName }} Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+{%- if t.c.IsArray %}
+ * @param len length of value
+{% endif %}
+ */
+NT_Bool NT_Set{{ t.TypeName }}(NT_Handle pubentry, int64_t time, {{ t.c.ParamType }} value{% if t.c.IsArray %}, size_t len{% endif %});
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+{%- if t.c.IsArray %}
+ * @param defaultValueLen length of default value
+{% endif %}
+ */
+NT_Bool NT_SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %});
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+{%- if t.c.IsArray %}
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+{% endif %}
+ * @return value
+ */
+{{ t.c.ValueType }} NT_Get{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen, size_t* len{% endif %});
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+{%- if t.c.IsArray %}
+ * @param defaultValueLen length of default value
+{% endif %}
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomic{{ t.TypeName }}(NT_Handle subentry, {{ t.c.ParamType }} defaultValue{% if t.c.IsArray %}, size_t defaultValueLen{% endif %}, struct NT_Timestamped{{ t.TypeName }}* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomic{{ t.TypeName }}).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestamped{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_Timestamped{{ t.TypeName }}* NT_ReadQueue{{ t.TypeName }}(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueue{{ t.TypeName }}).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueue{{ t.TypeName }}(struct NT_Timestamped{{ t.TypeName }}* arr, size_t len);
+
+{%- if not t.c.IsArray %}
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+{{ t.c.ValueType }}* NT_ReadQueueValues{{ t.TypeName }}(NT_Handle subentry, size_t* len);
+{%- endif %}
+
+/** @} */
+{% endfor %}
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
diff --git a/third_party/allwpilib/ntcore/src/generate/main/native/include/ntcore_cpp_types.h.jinja b/third_party/allwpilib/ntcore/src/generate/main/native/include/ntcore_cpp_types.h.jinja
new file mode 100644
index 0000000..941414d
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generate/main/native/include/ntcore_cpp_types.h.jinja
@@ -0,0 +1,144 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <span>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include "ntcore_c.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+/**
+ * Timestamped value.
+ * @ingroup ntcore_cpp_handle_api
+ */
+template <typename T>
+struct Timestamped {
+  Timestamped() = default;
+  Timestamped(int64_t time, int64_t serverTime, T value)
+    : time{time}, serverTime{serverTime}, value{std::move(value)} {}
+
+  /**
+   * Time in local time base.
+   */
+  int64_t time = 0;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime = 0;
+
+  /**
+   * Value.
+   */
+  T value = {};
+};
+{% for t in types %}
+/**
+ * Timestamped {{ t.TypeName }}.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using Timestamped{{ t.TypeName }} = Timestamped<{{ t.cpp.ValueType }}>;
+{% if t.cpp.SmallRetType %}
+/**
+ * Timestamped {{ t.TypeName }} view (for SmallVector-taking functions).
+ * @ingroup ntcore_cpp_handle_api
+ */
+using Timestamped{{ t.TypeName }}View = Timestamped<{{ t.cpp.SmallRetType }}>;
+{% endif %}
+/**
+ * @defgroup ntcore_{{ t.TypeName }}_func {{ t.TypeName }} Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool Set{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefault{{ t.TypeName }}(NT_Handle pubentry, {{ t.cpp.ParamType }} defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+{{ t.cpp.ValueType }} Get{{ t.TypeName }}(NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+Timestamped{{ t.TypeName }} GetAtomic{{ t.TypeName}}(NT_Handle subentry, {{ t.cpp.ParamType }} defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<Timestamped{{ t.TypeName }}> ReadQueue{{ t.TypeName }}(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<{% if t.cpp.ValueType == "bool" %}int{% else %}{{ t.cpp.ValueType }}{% endif %}> ReadQueueValues{{ t.TypeName }}(NT_Handle subentry);
+{% if t.cpp.SmallRetType and t.cpp.SmallElemType %}
+{{ t.cpp.SmallRetType }} Get{{ t.TypeName }}(NT_Handle subentry, wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf, {{ t.cpp.ParamType }} defaultValue);
+
+Timestamped{{ t.TypeName }}View GetAtomic{{ t.TypeName }}(
+      NT_Handle subentry,
+      wpi::SmallVectorImpl<{{ t.cpp.SmallElemType }}>& buf,
+      {{ t.cpp.ParamType }} defaultValue);
+{% endif %}
+/** @} */
+{% endfor %}
+}  // namespace nt
+
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayEntry.java
new file mode 100644
index 0000000..bcc72e0
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables BooleanArray entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface BooleanArrayEntry extends BooleanArraySubscriber, BooleanArrayPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayEntryImpl.java
new file mode 100644
index 0000000..40e455c
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables BooleanArray implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class BooleanArrayEntryImpl extends EntryBase implements BooleanArrayEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  BooleanArrayEntryImpl(BooleanArrayTopic topic, int handle, boolean[] defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public BooleanArrayTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public boolean[] get() {
+    return NetworkTablesJNI.getBooleanArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public boolean[] get(boolean[] defaultValue) {
+    return NetworkTablesJNI.getBooleanArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedBooleanArray getAtomic() {
+    return NetworkTablesJNI.getAtomicBooleanArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedBooleanArray getAtomic(boolean[] defaultValue) {
+    return NetworkTablesJNI.getAtomicBooleanArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedBooleanArray[] readQueue() {
+    return NetworkTablesJNI.readQueueBooleanArray(m_handle);
+  }
+
+  @Override
+  public boolean[][] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesBooleanArray(m_handle);
+  }
+
+  @Override
+  public void set(boolean[] value, long time) {
+    NetworkTablesJNI.setBooleanArray(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(boolean[] value) {
+    NetworkTablesJNI.setDefaultBooleanArray(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final BooleanArrayTopic m_topic;
+  private final boolean[] m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayPublisher.java
new file mode 100644
index 0000000..3e40bdb
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Consumer;
+
+/** NetworkTables BooleanArray publisher. */
+public interface BooleanArrayPublisher extends Publisher, Consumer<boolean[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  BooleanArrayTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(boolean[] value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(boolean[] value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(boolean[] value);
+
+  @Override
+  default void accept(boolean[] value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArraySubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArraySubscriber.java
new file mode 100644
index 0000000..ff07119
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArraySubscriber.java
@@ -0,0 +1,80 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables BooleanArray subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface BooleanArraySubscriber extends Subscriber, Supplier<boolean[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  BooleanArrayTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  boolean[] get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  boolean[] get(boolean[] defaultValue);
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedBooleanArray getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedBooleanArray getAtomic(boolean[] defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedBooleanArray[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  boolean[][] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayTopic.java
new file mode 100644
index 0000000..7d18f31
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanArrayTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables BooleanArray topic. */
+public final class BooleanArrayTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "boolean[]";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public BooleanArrayTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getBooleanArrayTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public BooleanArrayTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public BooleanArraySubscriber subscribe(
+      boolean[] defaultValue,
+      PubSubOption... options) {
+    return new BooleanArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kBooleanArray.getValue(),
+            "boolean[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public BooleanArraySubscriber subscribeEx(
+      String typeString,
+      boolean[] defaultValue,
+      PubSubOption... options) {
+    return new BooleanArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kBooleanArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public BooleanArrayPublisher publish(
+      PubSubOption... options) {
+    return new BooleanArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kBooleanArray.getValue(),
+            "boolean[]", options),
+        new boolean[] {});
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public BooleanArrayPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new BooleanArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kBooleanArray.getValue(),
+            typeString, properties, options),
+        new boolean[] {});
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public BooleanArrayEntry getEntry(
+      boolean[] defaultValue,
+      PubSubOption... options) {
+    return new BooleanArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kBooleanArray.getValue(),
+            "boolean[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public BooleanArrayEntry getEntryEx(
+      String typeString,
+      boolean[] defaultValue,
+      PubSubOption... options) {
+    return new BooleanArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kBooleanArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanEntry.java
new file mode 100644
index 0000000..1821c06
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables Boolean entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface BooleanEntry extends BooleanSubscriber, BooleanPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanEntryImpl.java
new file mode 100644
index 0000000..f099afc
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Boolean implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class BooleanEntryImpl extends EntryBase implements BooleanEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  BooleanEntryImpl(BooleanTopic topic, int handle, boolean defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public BooleanTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public boolean get() {
+    return NetworkTablesJNI.getBoolean(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public boolean get(boolean defaultValue) {
+    return NetworkTablesJNI.getBoolean(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedBoolean getAtomic() {
+    return NetworkTablesJNI.getAtomicBoolean(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedBoolean getAtomic(boolean defaultValue) {
+    return NetworkTablesJNI.getAtomicBoolean(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedBoolean[] readQueue() {
+    return NetworkTablesJNI.readQueueBoolean(m_handle);
+  }
+
+  @Override
+  public boolean[] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesBoolean(m_handle);
+  }
+
+  @Override
+  public void set(boolean value, long time) {
+    NetworkTablesJNI.setBoolean(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(boolean value) {
+    NetworkTablesJNI.setDefaultBoolean(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final BooleanTopic m_topic;
+  private final boolean m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanPublisher.java
new file mode 100644
index 0000000..1254ff3
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.function.BooleanConsumer;
+
+/** NetworkTables Boolean publisher. */
+public interface BooleanPublisher extends Publisher, BooleanConsumer {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  BooleanTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(boolean value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(boolean value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(boolean value);
+
+  @Override
+  default void accept(boolean value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanSubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanSubscriber.java
new file mode 100644
index 0000000..7f54f4b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanSubscriber.java
@@ -0,0 +1,85 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.BooleanSupplier;
+
+/** NetworkTables Boolean subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface BooleanSubscriber extends Subscriber, BooleanSupplier {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  BooleanTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  boolean get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  boolean get(boolean defaultValue);
+
+  @Override
+  default boolean getAsBoolean() {
+    return get();
+  }
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedBoolean getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedBoolean getAtomic(boolean defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedBoolean[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  boolean[] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanTopic.java
new file mode 100644
index 0000000..8ac0347
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/BooleanTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Boolean topic. */
+public final class BooleanTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "boolean";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public BooleanTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getBooleanTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public BooleanTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public BooleanSubscriber subscribe(
+      boolean defaultValue,
+      PubSubOption... options) {
+    return new BooleanEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kBoolean.getValue(),
+            "boolean", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public BooleanSubscriber subscribeEx(
+      String typeString,
+      boolean defaultValue,
+      PubSubOption... options) {
+    return new BooleanEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kBoolean.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public BooleanPublisher publish(
+      PubSubOption... options) {
+    return new BooleanEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kBoolean.getValue(),
+            "boolean", options),
+        false);
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public BooleanPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new BooleanEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kBoolean.getValue(),
+            typeString, properties, options),
+        false);
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public BooleanEntry getEntry(
+      boolean defaultValue,
+      PubSubOption... options) {
+    return new BooleanEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kBoolean.getValue(),
+            "boolean", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public BooleanEntry getEntryEx(
+      String typeString,
+      boolean defaultValue,
+      PubSubOption... options) {
+    return new BooleanEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kBoolean.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayEntry.java
new file mode 100644
index 0000000..8a40d8b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables DoubleArray entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface DoubleArrayEntry extends DoubleArraySubscriber, DoubleArrayPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayEntryImpl.java
new file mode 100644
index 0000000..7bb270a
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables DoubleArray implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class DoubleArrayEntryImpl extends EntryBase implements DoubleArrayEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  DoubleArrayEntryImpl(DoubleArrayTopic topic, int handle, double[] defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public DoubleArrayTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public double[] get() {
+    return NetworkTablesJNI.getDoubleArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public double[] get(double[] defaultValue) {
+    return NetworkTablesJNI.getDoubleArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedDoubleArray getAtomic() {
+    return NetworkTablesJNI.getAtomicDoubleArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedDoubleArray getAtomic(double[] defaultValue) {
+    return NetworkTablesJNI.getAtomicDoubleArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedDoubleArray[] readQueue() {
+    return NetworkTablesJNI.readQueueDoubleArray(m_handle);
+  }
+
+  @Override
+  public double[][] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesDoubleArray(m_handle);
+  }
+
+  @Override
+  public void set(double[] value, long time) {
+    NetworkTablesJNI.setDoubleArray(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(double[] value) {
+    NetworkTablesJNI.setDefaultDoubleArray(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final DoubleArrayTopic m_topic;
+  private final double[] m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayPublisher.java
new file mode 100644
index 0000000..39b367a
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Consumer;
+
+/** NetworkTables DoubleArray publisher. */
+public interface DoubleArrayPublisher extends Publisher, Consumer<double[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  DoubleArrayTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(double[] value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(double[] value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(double[] value);
+
+  @Override
+  default void accept(double[] value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArraySubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArraySubscriber.java
new file mode 100644
index 0000000..a807d66
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArraySubscriber.java
@@ -0,0 +1,80 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables DoubleArray subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface DoubleArraySubscriber extends Subscriber, Supplier<double[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  DoubleArrayTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  double[] get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  double[] get(double[] defaultValue);
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedDoubleArray getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedDoubleArray getAtomic(double[] defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedDoubleArray[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  double[][] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayTopic.java
new file mode 100644
index 0000000..f012869
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleArrayTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables DoubleArray topic. */
+public final class DoubleArrayTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "double[]";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public DoubleArrayTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getDoubleArrayTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public DoubleArrayTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public DoubleArraySubscriber subscribe(
+      double[] defaultValue,
+      PubSubOption... options) {
+    return new DoubleArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kDoubleArray.getValue(),
+            "double[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public DoubleArraySubscriber subscribeEx(
+      String typeString,
+      double[] defaultValue,
+      PubSubOption... options) {
+    return new DoubleArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kDoubleArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public DoubleArrayPublisher publish(
+      PubSubOption... options) {
+    return new DoubleArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kDoubleArray.getValue(),
+            "double[]", options),
+        new double[] {});
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public DoubleArrayPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new DoubleArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kDoubleArray.getValue(),
+            typeString, properties, options),
+        new double[] {});
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public DoubleArrayEntry getEntry(
+      double[] defaultValue,
+      PubSubOption... options) {
+    return new DoubleArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kDoubleArray.getValue(),
+            "double[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public DoubleArrayEntry getEntryEx(
+      String typeString,
+      double[] defaultValue,
+      PubSubOption... options) {
+    return new DoubleArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kDoubleArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleEntry.java
new file mode 100644
index 0000000..b91e324
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables Double entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface DoubleEntry extends DoubleSubscriber, DoublePublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleEntryImpl.java
new file mode 100644
index 0000000..968686d
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Double implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class DoubleEntryImpl extends EntryBase implements DoubleEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  DoubleEntryImpl(DoubleTopic topic, int handle, double defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public DoubleTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public double get() {
+    return NetworkTablesJNI.getDouble(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public double get(double defaultValue) {
+    return NetworkTablesJNI.getDouble(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedDouble getAtomic() {
+    return NetworkTablesJNI.getAtomicDouble(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedDouble getAtomic(double defaultValue) {
+    return NetworkTablesJNI.getAtomicDouble(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedDouble[] readQueue() {
+    return NetworkTablesJNI.readQueueDouble(m_handle);
+  }
+
+  @Override
+  public double[] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesDouble(m_handle);
+  }
+
+  @Override
+  public void set(double value, long time) {
+    NetworkTablesJNI.setDouble(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(double value) {
+    NetworkTablesJNI.setDefaultDouble(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final DoubleTopic m_topic;
+  private final double m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoublePublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoublePublisher.java
new file mode 100644
index 0000000..d477921
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoublePublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.DoubleConsumer;
+
+/** NetworkTables Double publisher. */
+public interface DoublePublisher extends Publisher, DoubleConsumer {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  DoubleTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(double value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(double value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(double value);
+
+  @Override
+  default void accept(double value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleSubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleSubscriber.java
new file mode 100644
index 0000000..688f6ea
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleSubscriber.java
@@ -0,0 +1,85 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.DoubleSupplier;
+
+/** NetworkTables Double subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface DoubleSubscriber extends Subscriber, DoubleSupplier {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  DoubleTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  double get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  double get(double defaultValue);
+
+  @Override
+  default double getAsDouble() {
+    return get();
+  }
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedDouble getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedDouble getAtomic(double defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedDouble[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  double[] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleTopic.java
new file mode 100644
index 0000000..2456841
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/DoubleTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Double topic. */
+public final class DoubleTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "double";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public DoubleTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getDoubleTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public DoubleTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public DoubleSubscriber subscribe(
+      double defaultValue,
+      PubSubOption... options) {
+    return new DoubleEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kDouble.getValue(),
+            "double", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public DoubleSubscriber subscribeEx(
+      String typeString,
+      double defaultValue,
+      PubSubOption... options) {
+    return new DoubleEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kDouble.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public DoublePublisher publish(
+      PubSubOption... options) {
+    return new DoubleEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kDouble.getValue(),
+            "double", options),
+        0);
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public DoublePublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new DoubleEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kDouble.getValue(),
+            typeString, properties, options),
+        0);
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public DoubleEntry getEntry(
+      double defaultValue,
+      PubSubOption... options) {
+    return new DoubleEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kDouble.getValue(),
+            "double", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public DoubleEntry getEntryEx(
+      String typeString,
+      double defaultValue,
+      PubSubOption... options) {
+    return new DoubleEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kDouble.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayEntry.java
new file mode 100644
index 0000000..b4cd353
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables FloatArray entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface FloatArrayEntry extends FloatArraySubscriber, FloatArrayPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayEntryImpl.java
new file mode 100644
index 0000000..1afe837
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables FloatArray implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class FloatArrayEntryImpl extends EntryBase implements FloatArrayEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  FloatArrayEntryImpl(FloatArrayTopic topic, int handle, float[] defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public FloatArrayTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public float[] get() {
+    return NetworkTablesJNI.getFloatArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public float[] get(float[] defaultValue) {
+    return NetworkTablesJNI.getFloatArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedFloatArray getAtomic() {
+    return NetworkTablesJNI.getAtomicFloatArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedFloatArray getAtomic(float[] defaultValue) {
+    return NetworkTablesJNI.getAtomicFloatArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedFloatArray[] readQueue() {
+    return NetworkTablesJNI.readQueueFloatArray(m_handle);
+  }
+
+  @Override
+  public float[][] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesFloatArray(m_handle);
+  }
+
+  @Override
+  public void set(float[] value, long time) {
+    NetworkTablesJNI.setFloatArray(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(float[] value) {
+    NetworkTablesJNI.setDefaultFloatArray(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final FloatArrayTopic m_topic;
+  private final float[] m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayPublisher.java
new file mode 100644
index 0000000..afaf9f2
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Consumer;
+
+/** NetworkTables FloatArray publisher. */
+public interface FloatArrayPublisher extends Publisher, Consumer<float[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  FloatArrayTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(float[] value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(float[] value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(float[] value);
+
+  @Override
+  default void accept(float[] value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArraySubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArraySubscriber.java
new file mode 100644
index 0000000..b70bece
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArraySubscriber.java
@@ -0,0 +1,80 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables FloatArray subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface FloatArraySubscriber extends Subscriber, Supplier<float[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  FloatArrayTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  float[] get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  float[] get(float[] defaultValue);
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedFloatArray getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedFloatArray getAtomic(float[] defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedFloatArray[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  float[][] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayTopic.java
new file mode 100644
index 0000000..e20ea7f
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatArrayTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables FloatArray topic. */
+public final class FloatArrayTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "float[]";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public FloatArrayTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getFloatArrayTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public FloatArrayTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public FloatArraySubscriber subscribe(
+      float[] defaultValue,
+      PubSubOption... options) {
+    return new FloatArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kFloatArray.getValue(),
+            "float[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public FloatArraySubscriber subscribeEx(
+      String typeString,
+      float[] defaultValue,
+      PubSubOption... options) {
+    return new FloatArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kFloatArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public FloatArrayPublisher publish(
+      PubSubOption... options) {
+    return new FloatArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kFloatArray.getValue(),
+            "float[]", options),
+        new float[] {});
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public FloatArrayPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new FloatArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kFloatArray.getValue(),
+            typeString, properties, options),
+        new float[] {});
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public FloatArrayEntry getEntry(
+      float[] defaultValue,
+      PubSubOption... options) {
+    return new FloatArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kFloatArray.getValue(),
+            "float[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public FloatArrayEntry getEntryEx(
+      String typeString,
+      float[] defaultValue,
+      PubSubOption... options) {
+    return new FloatArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kFloatArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatEntry.java
new file mode 100644
index 0000000..9830123
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables Float entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface FloatEntry extends FloatSubscriber, FloatPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatEntryImpl.java
new file mode 100644
index 0000000..f7efebf
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Float implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class FloatEntryImpl extends EntryBase implements FloatEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  FloatEntryImpl(FloatTopic topic, int handle, float defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public FloatTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public float get() {
+    return NetworkTablesJNI.getFloat(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public float get(float defaultValue) {
+    return NetworkTablesJNI.getFloat(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedFloat getAtomic() {
+    return NetworkTablesJNI.getAtomicFloat(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedFloat getAtomic(float defaultValue) {
+    return NetworkTablesJNI.getAtomicFloat(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedFloat[] readQueue() {
+    return NetworkTablesJNI.readQueueFloat(m_handle);
+  }
+
+  @Override
+  public float[] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesFloat(m_handle);
+  }
+
+  @Override
+  public void set(float value, long time) {
+    NetworkTablesJNI.setFloat(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(float value) {
+    NetworkTablesJNI.setDefaultFloat(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final FloatTopic m_topic;
+  private final float m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatPublisher.java
new file mode 100644
index 0000000..3f4b320
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.function.FloatConsumer;
+
+/** NetworkTables Float publisher. */
+public interface FloatPublisher extends Publisher, FloatConsumer {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  FloatTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(float value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(float value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(float value);
+
+  @Override
+  default void accept(float value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatSubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatSubscriber.java
new file mode 100644
index 0000000..758463b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatSubscriber.java
@@ -0,0 +1,85 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.function.FloatSupplier;
+
+/** NetworkTables Float subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface FloatSubscriber extends Subscriber, FloatSupplier {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  FloatTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  float get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  float get(float defaultValue);
+
+  @Override
+  default float getAsFloat() {
+    return get();
+  }
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedFloat getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedFloat getAtomic(float defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedFloat[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  float[] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatTopic.java
new file mode 100644
index 0000000..2ac7c7c
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/FloatTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Float topic. */
+public final class FloatTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "float";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public FloatTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getFloatTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public FloatTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public FloatSubscriber subscribe(
+      float defaultValue,
+      PubSubOption... options) {
+    return new FloatEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kFloat.getValue(),
+            "float", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public FloatSubscriber subscribeEx(
+      String typeString,
+      float defaultValue,
+      PubSubOption... options) {
+    return new FloatEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kFloat.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public FloatPublisher publish(
+      PubSubOption... options) {
+    return new FloatEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kFloat.getValue(),
+            "float", options),
+        0);
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public FloatPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new FloatEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kFloat.getValue(),
+            typeString, properties, options),
+        0);
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public FloatEntry getEntry(
+      float defaultValue,
+      PubSubOption... options) {
+    return new FloatEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kFloat.getValue(),
+            "float", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public FloatEntry getEntryEx(
+      String typeString,
+      float defaultValue,
+      PubSubOption... options) {
+    return new FloatEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kFloat.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericEntryImpl.java
new file mode 100644
index 0000000..b467c83
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericEntryImpl.java
@@ -0,0 +1,815 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.nio.ByteBuffer;
+
+/** NetworkTables generic implementation. */
+final class GenericEntryImpl extends EntryBase implements GenericEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   */
+  GenericEntryImpl(Topic topic, int handle) {
+    super(handle);
+    m_topic = topic;
+  }
+
+  @Override
+  public Topic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public NetworkTableValue get() {
+    return NetworkTablesJNI.getValue(m_handle);
+  }
+
+  /**
+   * Gets the entry's value as a boolean. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public boolean getBoolean(boolean defaultValue) {
+    return NetworkTablesJNI.getBoolean(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a long. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public long getInteger(long defaultValue) {
+    return NetworkTablesJNI.getInteger(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a float. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public float getFloat(float defaultValue) {
+    return NetworkTablesJNI.getFloat(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public double getDouble(double defaultValue) {
+    return NetworkTablesJNI.getDouble(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a String. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public String getString(String defaultValue) {
+    return NetworkTablesJNI.getString(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a byte[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public byte[] getRaw(byte[] defaultValue) {
+    return NetworkTablesJNI.getRaw(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public boolean[] getBooleanArray(boolean[] defaultValue) {
+    return NetworkTablesJNI.getBooleanArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public Boolean[] getBooleanArray(Boolean[] defaultValue) {
+    return NetworkTableValue.fromNativeBooleanArray(
+        getBooleanArray(NetworkTableValue.toNativeBooleanArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a long[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public long[] getIntegerArray(long[] defaultValue) {
+    return NetworkTablesJNI.getIntegerArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public Long[] getIntegerArray(Long[] defaultValue) {
+    return NetworkTableValue.fromNativeIntegerArray(
+        getIntegerArray(NetworkTableValue.toNativeIntegerArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a float[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public float[] getFloatArray(float[] defaultValue) {
+    return NetworkTablesJNI.getFloatArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public Float[] getFloatArray(Float[] defaultValue) {
+    return NetworkTableValue.fromNativeFloatArray(
+        getFloatArray(NetworkTableValue.toNativeFloatArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a double[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public double[] getDoubleArray(double[] defaultValue) {
+    return NetworkTablesJNI.getDoubleArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public Double[] getDoubleArray(Double[] defaultValue) {
+    return NetworkTableValue.fromNativeDoubleArray(
+        getDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a String[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  @Override
+  public String[] getStringArray(String[] defaultValue) {
+    return NetworkTablesJNI.getStringArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public NetworkTableValue[] readQueue() {
+    return NetworkTablesJNI.readQueueValue(m_handle);
+  }
+
+  @Override
+  public boolean set(NetworkTableValue value) {
+    long time = value.getTime();
+    Object otherValue = value.getValue();
+    switch (value.getType()) {
+      case kBoolean:
+        return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue);
+      case kInteger:
+        return NetworkTablesJNI.setInteger(
+            m_handle, time, ((Number) otherValue).longValue());
+      case kFloat:
+        return NetworkTablesJNI.setFloat(
+            m_handle, time, ((Number) otherValue).floatValue());
+      case kDouble:
+        return NetworkTablesJNI.setDouble(
+            m_handle, time, ((Number) otherValue).doubleValue());
+      case kString:
+        return NetworkTablesJNI.setString(m_handle, time, (String) otherValue);
+      case kRaw:
+        return NetworkTablesJNI.setRaw(m_handle, time, (byte[]) otherValue);
+      case kBooleanArray:
+        return NetworkTablesJNI.setBooleanArray(m_handle, time, (boolean[]) otherValue);
+      case kIntegerArray:
+        return NetworkTablesJNI.setIntegerArray(m_handle, time, (long[]) otherValue);
+      case kFloatArray:
+        return NetworkTablesJNI.setFloatArray(m_handle, time, (float[]) otherValue);
+      case kDoubleArray:
+        return NetworkTablesJNI.setDoubleArray(m_handle, time, (double[]) otherValue);
+      case kStringArray:
+        return NetworkTablesJNI.setStringArray(m_handle, time, (String[]) otherValue);
+      default:
+        return true;
+    }
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  @Override
+  public boolean setValue(Object value, long time) {
+    if (value instanceof NetworkTableValue) {
+      return set((NetworkTableValue) value);
+    } else if (value instanceof Boolean) {
+      return setBoolean((Boolean) value, time);
+    } else if (value instanceof Long) {
+      return setInteger((Long) value, time);
+    } else if (value instanceof Float) {
+      return setFloat((Float) value, time);
+    } else if (value instanceof Number) {
+      return setNumber((Number) value, time);
+    } else if (value instanceof String) {
+      return setString((String) value, time);
+    } else if (value instanceof byte[]) {
+      return setRaw((byte[]) value, time);
+    } else if (value instanceof boolean[]) {
+      return setBooleanArray((boolean[]) value, time);
+    } else if (value instanceof long[]) {
+      return setIntegerArray((long[]) value, time);
+    } else if (value instanceof float[]) {
+      return setFloatArray((float[]) value, time);
+    } else if (value instanceof double[]) {
+      return setDoubleArray((double[]) value, time);
+    } else if (value instanceof Boolean[]) {
+      return setBooleanArray((Boolean[]) value, time);
+    } else if (value instanceof Long[]) {
+      return setIntegerArray((Long[]) value, time);
+    } else if (value instanceof Float[]) {
+      return setFloatArray((Float[]) value, time);
+    } else if (value instanceof Number[]) {
+      return setNumberArray((Number[]) value, time);
+    } else if (value instanceof String[]) {
+      return setStringArray((String[]) value, time);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + value.getClass().getName() + " cannot be put into a table");
+    }
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setBoolean(boolean value, long time) {
+    return NetworkTablesJNI.setBoolean(m_handle, time, value);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setInteger(long value, long time) {
+    return NetworkTablesJNI.setInteger(m_handle, time, value);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setFloat(float value, long time) {
+    return NetworkTablesJNI.setFloat(m_handle, time, value);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDouble(double value, long time) {
+    return NetworkTablesJNI.setDouble(m_handle, time, value);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setString(String value, long time) {
+    return NetworkTablesJNI.setString(m_handle, time, value);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setRaw(byte[] value, int start, int len, long time) {
+    return NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setRaw(ByteBuffer value, int start, int len, long time) {
+    return NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setBooleanArray(boolean[] value, long time) {
+    return NetworkTablesJNI.setBooleanArray(m_handle, time, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setBooleanArray(Boolean[] value, long time) {
+    return setBooleanArray(NetworkTableValue.toNativeBooleanArray(value), time);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setIntegerArray(long[] value, long time) {
+    return NetworkTablesJNI.setIntegerArray(m_handle, time, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setIntegerArray(Long[] value, long time) {
+    return setIntegerArray(NetworkTableValue.toNativeIntegerArray(value), time);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setFloatArray(float[] value, long time) {
+    return NetworkTablesJNI.setFloatArray(m_handle, time, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setFloatArray(Float[] value, long time) {
+    return setFloatArray(NetworkTableValue.toNativeFloatArray(value), time);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDoubleArray(double[] value, long time) {
+    return NetworkTablesJNI.setDoubleArray(m_handle, time, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDoubleArray(Double[] value, long time) {
+    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value), time);
+  }
+
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setStringArray(String[] value, long time) {
+    return NetworkTablesJNI.setStringArray(m_handle, time, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumber(Number value, long time) {
+    return setDouble(value.doubleValue(), time);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumberArray(Number[] value, long time) {
+    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value), time);
+  }
+
+  @Override
+  public boolean setDefault(NetworkTableValue defaultValue) {
+    long time = defaultValue.getTime();
+    Object otherValue = defaultValue.getValue();
+    switch (defaultValue.getType()) {
+      case kBoolean:
+        return NetworkTablesJNI.setDefaultBoolean(m_handle, time, (Boolean) otherValue);
+      case kInteger:
+        return NetworkTablesJNI.setDefaultInteger(
+            m_handle, time, ((Number) otherValue).longValue());
+      case kFloat:
+        return NetworkTablesJNI.setDefaultFloat(
+            m_handle, time, ((Number) otherValue).floatValue());
+      case kDouble:
+        return NetworkTablesJNI.setDefaultDouble(
+            m_handle, time, ((Number) otherValue).doubleValue());
+      case kString:
+        return NetworkTablesJNI.setDefaultString(m_handle, time, (String) otherValue);
+      case kRaw:
+        return NetworkTablesJNI.setDefaultRaw(m_handle, time, (byte[]) otherValue);
+      case kBooleanArray:
+        return NetworkTablesJNI.setDefaultBooleanArray(m_handle, time, (boolean[]) otherValue);
+      case kIntegerArray:
+        return NetworkTablesJNI.setDefaultIntegerArray(m_handle, time, (long[]) otherValue);
+      case kFloatArray:
+        return NetworkTablesJNI.setDefaultFloatArray(m_handle, time, (float[]) otherValue);
+      case kDoubleArray:
+        return NetworkTablesJNI.setDefaultDoubleArray(m_handle, time, (double[]) otherValue);
+      case kStringArray:
+        return NetworkTablesJNI.setDefaultStringArray(m_handle, time, (String[]) otherValue);
+      default:
+        return true;
+    }
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  @Override
+  public boolean setDefaultValue(Object defaultValue) {
+    if (defaultValue instanceof NetworkTableValue) {
+      return setDefault((NetworkTableValue) defaultValue);
+    } else if (defaultValue instanceof Boolean) {
+      return setDefaultBoolean((Boolean) defaultValue);
+    } else if (defaultValue instanceof Integer) {
+      return setDefaultInteger((Integer) defaultValue);
+    } else if (defaultValue instanceof Float) {
+      return setDefaultFloat((Float) defaultValue);
+    } else if (defaultValue instanceof Number) {
+      return setDefaultNumber((Number) defaultValue);
+    } else if (defaultValue instanceof String) {
+      return setDefaultString((String) defaultValue);
+    } else if (defaultValue instanceof byte[]) {
+      return setDefaultRaw((byte[]) defaultValue);
+    } else if (defaultValue instanceof boolean[]) {
+      return setDefaultBooleanArray((boolean[]) defaultValue);
+    } else if (defaultValue instanceof long[]) {
+      return setDefaultIntegerArray((long[]) defaultValue);
+    } else if (defaultValue instanceof float[]) {
+      return setDefaultFloatArray((float[]) defaultValue);
+    } else if (defaultValue instanceof double[]) {
+      return setDefaultDoubleArray((double[]) defaultValue);
+    } else if (defaultValue instanceof Boolean[]) {
+      return setDefaultBooleanArray((Boolean[]) defaultValue);
+    } else if (defaultValue instanceof Long[]) {
+      return setDefaultIntegerArray((Long[]) defaultValue);
+    } else if (defaultValue instanceof Float[]) {
+      return setDefaultFloatArray((Float[]) defaultValue);
+    } else if (defaultValue instanceof Number[]) {
+      return setDefaultNumberArray((Number[]) defaultValue);
+    } else if (defaultValue instanceof String[]) {
+      return setDefaultStringArray((String[]) defaultValue);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + defaultValue.getClass().getName() + " cannot be put into a table");
+    }
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultBoolean(boolean defaultValue) {
+    return NetworkTablesJNI.setDefaultBoolean(m_handle, 0, defaultValue);
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultInteger(long defaultValue) {
+    return NetworkTablesJNI.setDefaultInteger(m_handle, 0, defaultValue);
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultFloat(float defaultValue) {
+    return NetworkTablesJNI.setDefaultFloat(m_handle, 0, defaultValue);
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultDouble(double defaultValue) {
+    return NetworkTablesJNI.setDefaultDouble(m_handle, 0, defaultValue);
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultString(String defaultValue) {
+    return NetworkTablesJNI.setDefaultString(m_handle, 0, defaultValue);
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultRaw(byte[] defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultBooleanArray(boolean[] defaultValue) {
+    return NetworkTablesJNI.setDefaultBooleanArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultBooleanArray(Boolean[] defaultValue) {
+    return setDefaultBooleanArray(NetworkTableValue.toNativeBooleanArray(defaultValue));
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultIntegerArray(long[] defaultValue) {
+    return NetworkTablesJNI.setDefaultIntegerArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultIntegerArray(Long[] defaultValue) {
+    return setDefaultIntegerArray(NetworkTableValue.toNativeIntegerArray(defaultValue));
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultFloatArray(float[] defaultValue) {
+    return NetworkTablesJNI.setDefaultFloatArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultFloatArray(Float[] defaultValue) {
+    return setDefaultFloatArray(NetworkTableValue.toNativeFloatArray(defaultValue));
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultDoubleArray(double[] defaultValue) {
+    return NetworkTablesJNI.setDefaultDoubleArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultDoubleArray(Double[] defaultValue) {
+    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
+  }
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  @Override
+  public boolean setDefaultStringArray(String[] defaultValue) {
+    return NetworkTablesJNI.setDefaultStringArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumber(Number defaultValue) {
+    return setDefaultDouble(defaultValue.doubleValue());
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumberArray(Number[] defaultValue) {
+    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final Topic m_topic;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericPublisher.java
new file mode 100644
index 0000000..8fd03ec
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericPublisher.java
@@ -0,0 +1,568 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.nio.ByteBuffer;
+import java.util.function.Consumer;
+
+/** NetworkTables generic publisher. */
+public interface GenericPublisher extends Publisher, Consumer<NetworkTableValue> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  Topic getTopic();
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  boolean set(NetworkTableValue value);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  default boolean setValue(Object value) {
+    return setValue(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  boolean setValue(Object value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setBoolean(boolean value) {
+    return setBoolean(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setBoolean(boolean value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setInteger(long value) {
+    return setInteger(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setInteger(long value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setFloat(float value) {
+    return setFloat(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setFloat(float value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setDouble(double value) {
+    return setDouble(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setDouble(double value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setString(String value) {
+    return setString(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setString(String value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(byte[] value) {
+    return setRaw(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(ByteBuffer value) {
+    return setRaw(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(byte[] value, long time) {
+    return setRaw(value, 0, value.length, time);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish; will send from value.position() to value.limit()
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(ByteBuffer value, long time) {
+    int pos = value.position();
+    return setRaw(value, pos, value.limit() - pos, time);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(byte[] value, int start, int len) {
+    return setRaw(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setRaw(byte[] value, int start, int len, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setRaw(ByteBuffer value, int start, int len) {
+    return setRaw(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setRaw(ByteBuffer value, int start, int len, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setBooleanArray(boolean[] value) {
+    return setBooleanArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setBooleanArray(boolean[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setBooleanArray(Boolean[] value) {
+    return setBooleanArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setBooleanArray(Boolean[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setIntegerArray(long[] value) {
+    return setIntegerArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setIntegerArray(long[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setIntegerArray(Long[] value) {
+    return setIntegerArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setIntegerArray(Long[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setFloatArray(float[] value) {
+    return setFloatArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setFloatArray(float[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setFloatArray(Float[] value) {
+    return setFloatArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setFloatArray(Float[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setDoubleArray(double[] value) {
+    return setDoubleArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setDoubleArray(double[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setDoubleArray(Double[] value) {
+    return setDoubleArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setDoubleArray(Double[] value, long time);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @return False if the topic already exists with a different type
+   */
+  default boolean setStringArray(String[] value) {
+    return setStringArray(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   * @return False if the topic already exists with a different type
+   */
+  boolean setStringArray(String[] value, long time);
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefault(NetworkTableValue defaultValue);
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  boolean setDefaultValue(Object defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultBoolean(boolean defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultInteger(long defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultFloat(float defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultDouble(double defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultString(String defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  default boolean setDefaultRaw(byte[] defaultValue) {
+    return setDefaultRaw(defaultValue, 0, defaultValue.length);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set; will send from defaultValue.position() to
+   *                     defaultValue.limit()
+   * @return False if the entry exists with a different type
+   */
+  default boolean setDefaultRaw(ByteBuffer defaultValue) {
+    int pos = defaultValue.position();
+    return setDefaultRaw(defaultValue, pos, defaultValue.limit() - pos);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultRaw(byte[] defaultValue, int start, int len);
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultBooleanArray(boolean[] defaultValue);
+
+  boolean setDefaultBooleanArray(Boolean[] defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultIntegerArray(long[] defaultValue);
+
+  boolean setDefaultIntegerArray(Long[] defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultFloatArray(float[] defaultValue);
+
+  boolean setDefaultFloatArray(Float[] defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultDoubleArray(double[] defaultValue);
+
+  boolean setDefaultDoubleArray(Double[] defaultValue);
+
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  boolean setDefaultStringArray(String[] defaultValue);
+
+  @Override
+  default void accept(NetworkTableValue value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericSubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericSubscriber.java
new file mode 100644
index 0000000..7d7ca96
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/GenericSubscriber.java
@@ -0,0 +1,176 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables generic subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface GenericSubscriber extends Subscriber, Supplier<NetworkTableValue> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  Topic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns a value with type NetworkTableType.kUnassigned.
+   *
+   * @return value
+   */
+  NetworkTableValue get();
+
+  /**
+   * Gets the entry's value as a boolean. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  boolean getBoolean(boolean defaultValue);
+
+  /**
+   * Gets the entry's value as a long. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  long getInteger(long defaultValue);
+
+  /**
+   * Gets the entry's value as a float. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  float getFloat(float defaultValue);
+
+  /**
+   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  double getDouble(double defaultValue);
+
+  /**
+   * Gets the entry's value as a String. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  String getString(String defaultValue);
+
+  /**
+   * Gets the entry's value as a byte[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  byte[] getRaw(byte[] defaultValue);
+
+  /**
+   * Gets the entry's value as a boolean[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  boolean[] getBooleanArray(boolean[] defaultValue);
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  Boolean[] getBooleanArray(Boolean[] defaultValue);
+
+  /**
+   * Gets the entry's value as a long[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  long[] getIntegerArray(long[] defaultValue);
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  Long[] getIntegerArray(Long[] defaultValue);
+
+  /**
+   * Gets the entry's value as a float[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  float[] getFloatArray(float[] defaultValue);
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  Float[] getFloatArray(Float[] defaultValue);
+
+  /**
+   * Gets the entry's value as a double[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  double[] getDoubleArray(double[] defaultValue);
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  Double[] getDoubleArray(Double[] defaultValue);
+
+  /**
+   * Gets the entry's value as a String[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  String[] getStringArray(String[] defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  NetworkTableValue[] readQueue();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayEntry.java
new file mode 100644
index 0000000..fd8f9c2
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables IntegerArray entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface IntegerArrayEntry extends IntegerArraySubscriber, IntegerArrayPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayEntryImpl.java
new file mode 100644
index 0000000..e74f489
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables IntegerArray implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class IntegerArrayEntryImpl extends EntryBase implements IntegerArrayEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  IntegerArrayEntryImpl(IntegerArrayTopic topic, int handle, long[] defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public IntegerArrayTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public long[] get() {
+    return NetworkTablesJNI.getIntegerArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public long[] get(long[] defaultValue) {
+    return NetworkTablesJNI.getIntegerArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedIntegerArray getAtomic() {
+    return NetworkTablesJNI.getAtomicIntegerArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedIntegerArray getAtomic(long[] defaultValue) {
+    return NetworkTablesJNI.getAtomicIntegerArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedIntegerArray[] readQueue() {
+    return NetworkTablesJNI.readQueueIntegerArray(m_handle);
+  }
+
+  @Override
+  public long[][] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesIntegerArray(m_handle);
+  }
+
+  @Override
+  public void set(long[] value, long time) {
+    NetworkTablesJNI.setIntegerArray(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(long[] value) {
+    NetworkTablesJNI.setDefaultIntegerArray(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final IntegerArrayTopic m_topic;
+  private final long[] m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayPublisher.java
new file mode 100644
index 0000000..b864ba1
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Consumer;
+
+/** NetworkTables IntegerArray publisher. */
+public interface IntegerArrayPublisher extends Publisher, Consumer<long[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  IntegerArrayTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(long[] value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(long[] value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(long[] value);
+
+  @Override
+  default void accept(long[] value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArraySubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArraySubscriber.java
new file mode 100644
index 0000000..cbb2e6f
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArraySubscriber.java
@@ -0,0 +1,80 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables IntegerArray subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface IntegerArraySubscriber extends Subscriber, Supplier<long[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  IntegerArrayTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  long[] get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  long[] get(long[] defaultValue);
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedIntegerArray getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedIntegerArray getAtomic(long[] defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedIntegerArray[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  long[][] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayTopic.java
new file mode 100644
index 0000000..d020e8a
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerArrayTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables IntegerArray topic. */
+public final class IntegerArrayTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "int[]";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public IntegerArrayTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getIntegerArrayTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public IntegerArrayTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public IntegerArraySubscriber subscribe(
+      long[] defaultValue,
+      PubSubOption... options) {
+    return new IntegerArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kIntegerArray.getValue(),
+            "int[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public IntegerArraySubscriber subscribeEx(
+      String typeString,
+      long[] defaultValue,
+      PubSubOption... options) {
+    return new IntegerArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kIntegerArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public IntegerArrayPublisher publish(
+      PubSubOption... options) {
+    return new IntegerArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kIntegerArray.getValue(),
+            "int[]", options),
+        new long[] {});
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public IntegerArrayPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new IntegerArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kIntegerArray.getValue(),
+            typeString, properties, options),
+        new long[] {});
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public IntegerArrayEntry getEntry(
+      long[] defaultValue,
+      PubSubOption... options) {
+    return new IntegerArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kIntegerArray.getValue(),
+            "int[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public IntegerArrayEntry getEntryEx(
+      String typeString,
+      long[] defaultValue,
+      PubSubOption... options) {
+    return new IntegerArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kIntegerArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerEntry.java
new file mode 100644
index 0000000..ccd614e
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables Integer entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface IntegerEntry extends IntegerSubscriber, IntegerPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerEntryImpl.java
new file mode 100644
index 0000000..a8db1bf
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Integer implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class IntegerEntryImpl extends EntryBase implements IntegerEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  IntegerEntryImpl(IntegerTopic topic, int handle, long defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public IntegerTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public long get() {
+    return NetworkTablesJNI.getInteger(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public long get(long defaultValue) {
+    return NetworkTablesJNI.getInteger(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedInteger getAtomic() {
+    return NetworkTablesJNI.getAtomicInteger(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedInteger getAtomic(long defaultValue) {
+    return NetworkTablesJNI.getAtomicInteger(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedInteger[] readQueue() {
+    return NetworkTablesJNI.readQueueInteger(m_handle);
+  }
+
+  @Override
+  public long[] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesInteger(m_handle);
+  }
+
+  @Override
+  public void set(long value, long time) {
+    NetworkTablesJNI.setInteger(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(long value) {
+    NetworkTablesJNI.setDefaultInteger(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final IntegerTopic m_topic;
+  private final long m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerPublisher.java
new file mode 100644
index 0000000..be734ed
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.LongConsumer;
+
+/** NetworkTables Integer publisher. */
+public interface IntegerPublisher extends Publisher, LongConsumer {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  IntegerTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(long value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(long value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(long value);
+
+  @Override
+  default void accept(long value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerSubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerSubscriber.java
new file mode 100644
index 0000000..81a9740
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerSubscriber.java
@@ -0,0 +1,85 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.LongSupplier;
+
+/** NetworkTables Integer subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface IntegerSubscriber extends Subscriber, LongSupplier {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  IntegerTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  long get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  long get(long defaultValue);
+
+  @Override
+  default long getAsLong() {
+    return get();
+  }
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedInteger getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedInteger getAtomic(long defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedInteger[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  long[] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerTopic.java
new file mode 100644
index 0000000..ebf87c4
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/IntegerTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Integer topic. */
+public final class IntegerTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "int";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public IntegerTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getIntegerTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public IntegerTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public IntegerSubscriber subscribe(
+      long defaultValue,
+      PubSubOption... options) {
+    return new IntegerEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kInteger.getValue(),
+            "int", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public IntegerSubscriber subscribeEx(
+      String typeString,
+      long defaultValue,
+      PubSubOption... options) {
+    return new IntegerEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kInteger.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public IntegerPublisher publish(
+      PubSubOption... options) {
+    return new IntegerEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kInteger.getValue(),
+            "int", options),
+        0);
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public IntegerPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new IntegerEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kInteger.getValue(),
+            typeString, properties, options),
+        0);
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public IntegerEntry getEntry(
+      long defaultValue,
+      PubSubOption... options) {
+    return new IntegerEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kInteger.getValue(),
+            "int", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public IntegerEntry getEntryEx(
+      String typeString,
+      long defaultValue,
+      PubSubOption... options) {
+    return new IntegerEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kInteger.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableEntry.java
new file mode 100644
index 0000000..69d1077
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableEntry.java
@@ -0,0 +1,1013 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.nio.ByteBuffer;
+
+/**
+ * NetworkTables Entry.
+ *
+ * <p>For backwards compatibility, the NetworkTableEntry close() does not release the entry.
+ */
+@SuppressWarnings("UnnecessaryParentheses")
+public final class NetworkTableEntry implements Publisher, Subscriber {
+  /**
+   * Flag values (as returned by {@link #getFlags()}).
+   *
+   * @deprecated Use isPersistent() instead.
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public static final int kPersistent = 0x01;
+
+  /**
+   * Construct from native handle.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public NetworkTableEntry(NetworkTableInstance inst, int handle) {
+    this(new Topic(inst, NetworkTablesJNI.getTopicFromHandle(handle)), handle);
+  }
+
+  /**
+   * Construct from native handle.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   */
+  public NetworkTableEntry(Topic topic, int handle) {
+    m_topic = topic;
+    m_handle = handle;
+  }
+
+  @Override
+  public void close() {}
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  @Override
+  public boolean isValid() {
+    return m_handle != 0;
+  }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  @Override
+  public int getHandle() {
+    return m_handle;
+  }
+
+  /**
+   * Gets the subscribed-to / published-to topic.
+   *
+   * @return Topic
+   */
+  @Override
+  public Topic getTopic() {
+    return m_topic;
+  }
+
+  /**
+   * Gets the instance for the entry.
+   *
+   * @return Instance
+   */
+  public NetworkTableInstance getInstance() {
+    return m_topic.getInstance();
+  }
+
+  /**
+   * Determines if the entry currently exists.
+   *
+   * @return True if the entry exists, false otherwise.
+   */
+  @Override
+  public boolean exists() {
+    return NetworkTablesJNI.getType(m_handle) != 0;
+  }
+
+  /**
+   * Gets the name of the entry (the key).
+   *
+   * @return the entry's name
+   */
+  public String getName() {
+    return NetworkTablesJNI.getEntryName(m_handle);
+  }
+
+  /**
+   * Gets the type of the entry.
+   *
+   * @return the entry's type
+   */
+  public NetworkTableType getType() {
+    return NetworkTableType.getFromInt(NetworkTablesJNI.getType(m_handle));
+  }
+
+  /**
+   * Returns the flags.
+   *
+   * @return the flags (bitmask)
+   * @deprecated Use isPersistent() or topic properties instead
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public int getFlags() {
+    return NetworkTablesJNI.getEntryFlags(m_handle);
+  }
+
+  /**
+   * Gets the last time the entry's value was changed.
+   *
+   * @return Entry last change time
+   */
+  @Override
+  public long getLastChange() {
+    return NetworkTablesJNI.getEntryLastChange(m_handle);
+  }
+
+  /**
+   * Gets the entry's value. Returns a value with type NetworkTableType.kUnassigned if the value
+   * does not exist.
+   *
+   * @return the entry's value
+   */
+  public NetworkTableValue getValue() {
+    return NetworkTablesJNI.getValue(m_handle);
+  }
+
+  /**
+   * Gets the entry's value as a boolean. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public boolean getBoolean(boolean defaultValue) {
+    return NetworkTablesJNI.getBoolean(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a long. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public long getInteger(long defaultValue) {
+    return NetworkTablesJNI.getInteger(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a float. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public float getFloat(float defaultValue) {
+    return NetworkTablesJNI.getFloat(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public double getDouble(double defaultValue) {
+    return NetworkTablesJNI.getDouble(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a String. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public String getString(String defaultValue) {
+    return NetworkTablesJNI.getString(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a byte[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public byte[] getRaw(byte[] defaultValue) {
+    return NetworkTablesJNI.getRaw(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public boolean[] getBooleanArray(boolean[] defaultValue) {
+    return NetworkTablesJNI.getBooleanArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Boolean[] getBooleanArray(Boolean[] defaultValue) {
+    return NetworkTableValue.fromNativeBooleanArray(
+        getBooleanArray(NetworkTableValue.toNativeBooleanArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a long[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public long[] getIntegerArray(long[] defaultValue) {
+    return NetworkTablesJNI.getIntegerArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Long[] getIntegerArray(Long[] defaultValue) {
+    return NetworkTableValue.fromNativeIntegerArray(
+        getIntegerArray(NetworkTableValue.toNativeIntegerArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a float[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public float[] getFloatArray(float[] defaultValue) {
+    return NetworkTablesJNI.getFloatArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Float[] getFloatArray(Float[] defaultValue) {
+    return NetworkTableValue.fromNativeFloatArray(
+        getFloatArray(NetworkTableValue.toNativeFloatArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a double[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public double[] getDoubleArray(double[] defaultValue) {
+    return NetworkTablesJNI.getDoubleArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Double[] getDoubleArray(Double[] defaultValue) {
+    return NetworkTableValue.fromNativeDoubleArray(
+        getDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue)));
+  }
+
+  /**
+   * Gets the entry's value as a String[]. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public String[] getStringArray(String[] defaultValue) {
+    return NetworkTablesJNI.getStringArray(m_handle, defaultValue);
+  }
+
+  /**
+   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Number getNumber(Number defaultValue) {
+    return getDouble(defaultValue.doubleValue());
+  }
+
+  /**
+   * Gets the entry's value as a double array. If the entry does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param defaultValue the value to be returned if no value is found
+   * @return the entry's value or the given default value
+   */
+  public Number[] getNumberArray(Number[] defaultValue) {
+    return NetworkTableValue.fromNativeDoubleArray(
+        getDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue)));
+  }
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  public NetworkTableValue[] readQueue() {
+    return NetworkTablesJNI.readQueueValue(m_handle);
+  }
+
+  /**
+   * Checks if a data value is of a type that can be placed in a NetworkTable entry.
+   *
+   * @param data the data to check
+   * @return true if the data can be placed in an entry, false if it cannot
+   */
+  public static boolean isValidDataType(Object data) {
+    return data instanceof Number
+        || data instanceof Boolean
+        || data instanceof String
+        || data instanceof long[]
+        || data instanceof Long[]
+        || data instanceof float[]
+        || data instanceof Float[]
+        || data instanceof double[]
+        || data instanceof Double[]
+        || data instanceof Number[]
+        || data instanceof boolean[]
+        || data instanceof Boolean[]
+        || data instanceof String[]
+        || data instanceof byte[]
+        || data instanceof Byte[];
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  public boolean setDefaultValue(Object defaultValue) {
+    if (defaultValue instanceof NetworkTableValue) {
+      long time = ((NetworkTableValue) defaultValue).getTime();
+      Object otherValue = ((NetworkTableValue) defaultValue).getValue();
+      switch (((NetworkTableValue) defaultValue).getType()) {
+        case kBoolean:
+          return NetworkTablesJNI.setDefaultBoolean(m_handle, time, (Boolean) otherValue);
+        case kInteger:
+          return NetworkTablesJNI.setDefaultInteger(
+              m_handle, time, ((Number) otherValue).longValue());
+        case kFloat:
+          return NetworkTablesJNI.setDefaultFloat(
+              m_handle, time, ((Number) otherValue).floatValue());
+        case kDouble:
+          return NetworkTablesJNI.setDefaultDouble(
+              m_handle, time, ((Number) otherValue).doubleValue());
+        case kString:
+          return NetworkTablesJNI.setDefaultString(m_handle, time, (String) otherValue);
+        case kRaw:
+          return NetworkTablesJNI.setDefaultRaw(m_handle, time, (byte[]) otherValue);
+        case kBooleanArray:
+          return NetworkTablesJNI.setDefaultBooleanArray(m_handle, time, (boolean[]) otherValue);
+        case kIntegerArray:
+          return NetworkTablesJNI.setDefaultIntegerArray(m_handle, time, (long[]) otherValue);
+        case kFloatArray:
+          return NetworkTablesJNI.setDefaultFloatArray(m_handle, time, (float[]) otherValue);
+        case kDoubleArray:
+          return NetworkTablesJNI.setDefaultDoubleArray(m_handle, time, (double[]) otherValue);
+        case kStringArray:
+          return NetworkTablesJNI.setDefaultStringArray(m_handle, time, (String[]) otherValue);
+        default:
+          return true;
+      }
+    } else if (defaultValue instanceof Boolean) {
+      return setDefaultBoolean((Boolean) defaultValue);
+    } else if (defaultValue instanceof Integer) {
+      return setDefaultInteger((Integer) defaultValue);
+    } else if (defaultValue instanceof Float) {
+      return setDefaultFloat((Float) defaultValue);
+    } else if (defaultValue instanceof Number) {
+      return setDefaultNumber((Number) defaultValue);
+    } else if (defaultValue instanceof String) {
+      return setDefaultString((String) defaultValue);
+    } else if (defaultValue instanceof byte[]) {
+      return setDefaultRaw((byte[]) defaultValue);
+    } else if (defaultValue instanceof boolean[]) {
+      return setDefaultBooleanArray((boolean[]) defaultValue);
+    } else if (defaultValue instanceof long[]) {
+      return setDefaultIntegerArray((long[]) defaultValue);
+    } else if (defaultValue instanceof float[]) {
+      return setDefaultFloatArray((float[]) defaultValue);
+    } else if (defaultValue instanceof double[]) {
+      return setDefaultDoubleArray((double[]) defaultValue);
+    } else if (defaultValue instanceof Boolean[]) {
+      return setDefaultBooleanArray((Boolean[]) defaultValue);
+    } else if (defaultValue instanceof Long[]) {
+      return setDefaultIntegerArray((Long[]) defaultValue);
+    } else if (defaultValue instanceof Float[]) {
+      return setDefaultFloatArray((Float[]) defaultValue);
+    } else if (defaultValue instanceof Number[]) {
+      return setDefaultNumberArray((Number[]) defaultValue);
+    } else if (defaultValue instanceof String[]) {
+      return setDefaultStringArray((String[]) defaultValue);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + defaultValue.getClass().getName() + " cannot be put into a table");
+    }
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultBoolean(boolean defaultValue) {
+    return NetworkTablesJNI.setDefaultBoolean(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultInteger(long defaultValue) {
+    return NetworkTablesJNI.setDefaultInteger(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultFloat(float defaultValue) {
+    return NetworkTablesJNI.setDefaultFloat(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultDouble(double defaultValue) {
+    return NetworkTablesJNI.setDefaultDouble(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultString(String defaultValue) {
+    return NetworkTablesJNI.setDefaultString(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultRaw(byte[] defaultValue) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set; will send from defaultValue.position() to
+   *                     defaultValue.capacity()
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultRaw(ByteBuffer defaultValue) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultRaw(byte[] defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultRaw(ByteBuffer defaultValue, int start, int len) {
+    return NetworkTablesJNI.setDefaultRaw(m_handle, 0, defaultValue, start, len);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultBooleanArray(boolean[] defaultValue) {
+    return NetworkTablesJNI.setDefaultBooleanArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultBooleanArray(Boolean[] defaultValue) {
+    return setDefaultBooleanArray(NetworkTableValue.toNativeBooleanArray(defaultValue));
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultIntegerArray(long[] defaultValue) {
+    return NetworkTablesJNI.setDefaultIntegerArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultIntegerArray(Long[] defaultValue) {
+    return setDefaultIntegerArray(NetworkTableValue.toNativeIntegerArray(defaultValue));
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultFloatArray(float[] defaultValue) {
+    return NetworkTablesJNI.setDefaultFloatArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultFloatArray(Float[] defaultValue) {
+    return setDefaultFloatArray(NetworkTableValue.toNativeFloatArray(defaultValue));
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultDoubleArray(double[] defaultValue) {
+    return NetworkTablesJNI.setDefaultDoubleArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultDoubleArray(Double[] defaultValue) {
+    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultStringArray(String[] defaultValue) {
+    return NetworkTablesJNI.setDefaultStringArray(m_handle, 0, defaultValue);
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumber(Number defaultValue) {
+    return setDefaultDouble(defaultValue.doubleValue());
+  }
+
+  /**
+   * Sets the entry's value if it does not exist.
+   *
+   * @param defaultValue the default value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDefaultNumberArray(Number[] defaultValue) {
+    return setDefaultDoubleArray(NetworkTableValue.toNativeDoubleArray(defaultValue));
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   * @throws IllegalArgumentException if the value is not a known type
+   */
+  public boolean setValue(Object value) {
+    if (value instanceof NetworkTableValue) {
+      long time = ((NetworkTableValue) value).getTime();
+      Object otherValue = ((NetworkTableValue) value).getValue();
+      switch (((NetworkTableValue) value).getType()) {
+        case kBoolean:
+          return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue);
+        case kInteger:
+          return NetworkTablesJNI.setInteger(
+              m_handle, time, ((Number) otherValue).longValue());
+        case kFloat:
+          return NetworkTablesJNI.setFloat(
+              m_handle, time, ((Number) otherValue).floatValue());
+        case kDouble:
+          return NetworkTablesJNI.setDouble(
+              m_handle, time, ((Number) otherValue).doubleValue());
+        case kString:
+          return NetworkTablesJNI.setString(m_handle, time, (String) otherValue);
+        case kRaw:
+          return NetworkTablesJNI.setRaw(m_handle, time, (byte[]) otherValue);
+        case kBooleanArray:
+          return NetworkTablesJNI.setBooleanArray(m_handle, time, (boolean[]) otherValue);
+        case kIntegerArray:
+          return NetworkTablesJNI.setIntegerArray(m_handle, time, (long[]) otherValue);
+        case kFloatArray:
+          return NetworkTablesJNI.setFloatArray(m_handle, time, (float[]) otherValue);
+        case kDoubleArray:
+          return NetworkTablesJNI.setDoubleArray(m_handle, time, (double[]) otherValue);
+        case kStringArray:
+          return NetworkTablesJNI.setStringArray(m_handle, time, (String[]) otherValue);
+        default:
+          return true;
+      }
+    } else if (value instanceof Boolean) {
+      return setBoolean((Boolean) value);
+    } else if (value instanceof Long) {
+      return setInteger((Long) value);
+    } else if (value instanceof Float) {
+      return setFloat((Float) value);
+    } else if (value instanceof Number) {
+      return setNumber((Number) value);
+    } else if (value instanceof String) {
+      return setString((String) value);
+    } else if (value instanceof byte[]) {
+      return setRaw((byte[]) value);
+    } else if (value instanceof boolean[]) {
+      return setBooleanArray((boolean[]) value);
+    } else if (value instanceof long[]) {
+      return setIntegerArray((long[]) value);
+    } else if (value instanceof float[]) {
+      return setFloatArray((float[]) value);
+    } else if (value instanceof double[]) {
+      return setDoubleArray((double[]) value);
+    } else if (value instanceof Boolean[]) {
+      return setBooleanArray((Boolean[]) value);
+    } else if (value instanceof Long[]) {
+      return setIntegerArray((Long[]) value);
+    } else if (value instanceof Float[]) {
+      return setFloatArray((Float[]) value);
+    } else if (value instanceof Number[]) {
+      return setNumberArray((Number[]) value);
+    } else if (value instanceof String[]) {
+      return setStringArray((String[]) value);
+    } else {
+      throw new IllegalArgumentException(
+          "Value of type " + value.getClass().getName() + " cannot be put into a table");
+    }
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setBoolean(boolean value) {
+    return NetworkTablesJNI.setBoolean(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setInteger(long value) {
+    return NetworkTablesJNI.setInteger(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setFloat(float value) {
+    return NetworkTablesJNI.setFloat(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDouble(double value) {
+    return NetworkTablesJNI.setDouble(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setString(String value) {
+    return NetworkTablesJNI.setString(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setRaw(byte[] value) {
+    return NetworkTablesJNI.setRaw(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set; will send from value.position() to value.capacity()
+   * @return False if the entry exists with a different type
+   */
+  public boolean setRaw(ByteBuffer value) {
+    return NetworkTablesJNI.setRaw(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setRaw(byte[] value, int start, int len) {
+    return NetworkTablesJNI.setRaw(m_handle, 0, value, start, len);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @return False if the entry exists with a different type
+   */
+  public boolean setRaw(ByteBuffer value, int start, int len) {
+    return NetworkTablesJNI.setRaw(m_handle, 0, value, start, len);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setBooleanArray(boolean[] value) {
+    return NetworkTablesJNI.setBooleanArray(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setBooleanArray(Boolean[] value) {
+    return setBooleanArray(NetworkTableValue.toNativeBooleanArray(value));
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setIntegerArray(long[] value) {
+    return NetworkTablesJNI.setIntegerArray(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setIntegerArray(Long[] value) {
+    return setIntegerArray(NetworkTableValue.toNativeIntegerArray(value));
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setFloatArray(float[] value) {
+    return NetworkTablesJNI.setFloatArray(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setFloatArray(Float[] value) {
+    return setFloatArray(NetworkTableValue.toNativeFloatArray(value));
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDoubleArray(double[] value) {
+    return NetworkTablesJNI.setDoubleArray(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setDoubleArray(Double[] value) {
+    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value));
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setStringArray(String[] value) {
+    return NetworkTablesJNI.setStringArray(m_handle, 0, value);
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumber(Number value) {
+    return setDouble(value.doubleValue());
+  }
+
+  /**
+   * Sets the entry's value.
+   *
+   * @param value the value to set
+   * @return False if the entry exists with a different type
+   */
+  public boolean setNumberArray(Number[] value) {
+    return setDoubleArray(NetworkTableValue.toNativeDoubleArray(value));
+  }
+
+  /**
+   * Sets flags.
+   *
+   * @param flags the flags to set (bitmask)
+   * @deprecated Use setPersistent() or topic properties instead
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public void setFlags(int flags) {
+    NetworkTablesJNI.setEntryFlags(m_handle, getFlags() | flags);
+  }
+
+  /**
+   * Clears flags.
+   *
+   * @param flags the flags to clear (bitmask)
+   * @deprecated Use setPersistent() or topic properties instead
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public void clearFlags(int flags) {
+    NetworkTablesJNI.setEntryFlags(m_handle, getFlags() & ~flags);
+  }
+
+  /** Make value persistent through program restarts. */
+  public void setPersistent() {
+    NetworkTablesJNI.setTopicPersistent(m_topic.getHandle(), true);
+  }
+
+  /** Stop making value persistent through program restarts. */
+  public void clearPersistent() {
+    NetworkTablesJNI.setTopicPersistent(m_topic.getHandle(), false);
+  }
+
+  /**
+   * Returns whether the value is persistent through program restarts.
+   *
+   * @return True if the value is persistent.
+   */
+  public boolean isPersistent() {
+    return NetworkTablesJNI.getTopicPersistent(m_topic.getHandle());
+  }
+
+  /** Stops publishing the entry if it's been published. */
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  /**
+   * Deletes the entry.
+   *
+   * @deprecated Use unpublish() instead.
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public void delete() {
+    unpublish();
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (other == this) {
+      return true;
+    }
+    if (!(other instanceof NetworkTableEntry)) {
+      return false;
+    }
+
+    return m_handle == ((NetworkTableEntry) other).m_handle;
+  }
+
+  @Override
+  public int hashCode() {
+    return m_handle;
+  }
+
+  private final Topic m_topic;
+  protected int m_handle;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableInstance.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
new file mode 100644
index 0000000..e5872ba
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
@@ -0,0 +1,1553 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.util.concurrent.Event;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.charset.StandardCharsets;
+import java.util.EnumSet;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.Map;
+import java.util.OptionalLong;
+import java.util.Set;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.ConcurrentMap;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+import us.hebi.quickbuf.ProtoMessage;
+
+/**
+ * NetworkTables Instance.
+ *
+ * <p>Instances are completely independent from each other. Table operations on one instance will
+ * not be visible to other instances unless the instances are connected via the network. The main
+ * limitation on instances is that you cannot have two servers on the same network port. The main
+ * utility of instances is for unit testing, but they can also enable one program to connect to two
+ * different NetworkTables networks.
+ *
+ * <p>The global "default" instance (as returned by {@link #getDefault()}) is always available, and
+ * is intended for the common case when there is only a single NetworkTables instance being used in
+ * the program.
+ *
+ * <p>Additional instances can be created with the {@link #create()} function. A reference must be
+ * kept to the NetworkTableInstance returned by this function to keep it from being garbage
+ * collected.
+ */
+@SuppressWarnings("PMD.CouplingBetweenObjects")
+public final class NetworkTableInstance implements AutoCloseable {
+  /** Client/server mode flag values (as returned by {@link #getNetworkMode()}). */
+  public enum NetworkMode {
+    /** Running in server mode. */
+    kServer(0x01),
+
+    /** Running in NT3 client mode. */
+    kClient3(0x02),
+
+    /** Running in NT4 client mode. */
+    kClient4(0x04),
+
+    /** Currently starting up (either client or server). */
+    kStarting(0x08),
+
+    /** Running in local-only mode. */
+    kLocal(0x10);
+
+    private final int value;
+
+    NetworkMode(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /** The default port that network tables operates on for NT3. */
+  public static final int kDefaultPort3 = 1735;
+
+  /** The default port that network tables operates on for NT4. */
+  public static final int kDefaultPort4 = 5810;
+
+  /**
+   * Construct from native handle.
+   *
+   * @param handle Native handle
+   */
+  private NetworkTableInstance(int handle) {
+    m_owned = false;
+    m_handle = handle;
+  }
+
+  /** Destroys the instance (if created by {@link #create()}). */
+  @Override
+  public synchronized void close() {
+    if (m_owned && m_handle != 0) {
+      m_listeners.close();
+      m_schemas.forEach((k, v) -> v.close());
+      NetworkTablesJNI.destroyInstance(m_handle);
+      m_handle = 0;
+    }
+  }
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  public boolean isValid() {
+    return m_handle != 0;
+  }
+
+  /* The default instance. */
+  private static NetworkTableInstance s_defaultInstance;
+
+  /**
+   * Get global default instance.
+   *
+   * @return Global default instance
+   */
+  public static synchronized NetworkTableInstance getDefault() {
+    if (s_defaultInstance == null) {
+      s_defaultInstance = new NetworkTableInstance(NetworkTablesJNI.getDefaultInstance());
+    }
+    return s_defaultInstance;
+  }
+
+  /**
+   * Create an instance. Note: A reference to the returned instance must be retained to ensure the
+   * instance is not garbage collected.
+   *
+   * @return Newly created instance
+   */
+  public static NetworkTableInstance create() {
+    NetworkTableInstance inst = new NetworkTableInstance(NetworkTablesJNI.createInstance());
+    inst.m_owned = true;
+    return inst;
+  }
+
+  /**
+   * Gets the native handle for the instance.
+   *
+   * @return Native handle
+   */
+  public int getHandle() {
+    return m_handle;
+  }
+
+  /**
+   * Get (generic) topic.
+   *
+   * @param name topic name
+   * @return Topic
+   */
+  public Topic getTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic == null) {
+      int handle = NetworkTablesJNI.getTopic(m_handle, name);
+      topic = new Topic(this, handle);
+      Topic oldTopic = m_topics.putIfAbsent(name, topic);
+      if (oldTopic != null) {
+        topic = oldTopic;
+      }
+      // also cache by handle
+      m_topicsByHandle.putIfAbsent(handle, topic);
+    }
+    return topic;
+  }
+
+  /**
+   * Get boolean topic.
+   *
+   * @param name topic name
+   * @return BooleanTopic
+   */
+  public BooleanTopic getBooleanTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof BooleanTopic) {
+      return (BooleanTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    BooleanTopic wrapTopic = new BooleanTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get long topic.
+   *
+   * @param name topic name
+   * @return IntegerTopic
+   */
+  public IntegerTopic getIntegerTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof IntegerTopic) {
+      return (IntegerTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    IntegerTopic wrapTopic = new IntegerTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get float topic.
+   *
+   * @param name topic name
+   * @return FloatTopic
+   */
+  public FloatTopic getFloatTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof FloatTopic) {
+      return (FloatTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    FloatTopic wrapTopic = new FloatTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get double topic.
+   *
+   * @param name topic name
+   * @return DoubleTopic
+   */
+  public DoubleTopic getDoubleTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof DoubleTopic) {
+      return (DoubleTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    DoubleTopic wrapTopic = new DoubleTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get String topic.
+   *
+   * @param name topic name
+   * @return StringTopic
+   */
+  public StringTopic getStringTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof StringTopic) {
+      return (StringTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    StringTopic wrapTopic = new StringTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get byte[] topic.
+   *
+   * @param name topic name
+   * @return RawTopic
+   */
+  public RawTopic getRawTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof RawTopic) {
+      return (RawTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    RawTopic wrapTopic = new RawTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get boolean[] topic.
+   *
+   * @param name topic name
+   * @return BooleanArrayTopic
+   */
+  public BooleanArrayTopic getBooleanArrayTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof BooleanArrayTopic) {
+      return (BooleanArrayTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    BooleanArrayTopic wrapTopic = new BooleanArrayTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get long[] topic.
+   *
+   * @param name topic name
+   * @return IntegerArrayTopic
+   */
+  public IntegerArrayTopic getIntegerArrayTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof IntegerArrayTopic) {
+      return (IntegerArrayTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    IntegerArrayTopic wrapTopic = new IntegerArrayTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get float[] topic.
+   *
+   * @param name topic name
+   * @return FloatArrayTopic
+   */
+  public FloatArrayTopic getFloatArrayTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof FloatArrayTopic) {
+      return (FloatArrayTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    FloatArrayTopic wrapTopic = new FloatArrayTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get double[] topic.
+   *
+   * @param name topic name
+   * @return DoubleArrayTopic
+   */
+  public DoubleArrayTopic getDoubleArrayTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof DoubleArrayTopic) {
+      return (DoubleArrayTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    DoubleArrayTopic wrapTopic = new DoubleArrayTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get String[] topic.
+   *
+   * @param name topic name
+   * @return StringArrayTopic
+   */
+  public StringArrayTopic getStringArrayTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof StringArrayTopic) {
+      return (StringArrayTopic) topic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    StringArrayTopic wrapTopic = new StringArrayTopic(this, handle);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+
+  /**
+   * Get protobuf-encoded value topic.
+   *
+   * @param <T> value class (inferred from proto)
+   * @param <MessageType> protobuf message type (inferred from proto)
+   * @param name topic name
+   * @param proto protobuf serialization implementation
+   * @return ProtobufTopic
+   */
+  public <T, MessageType extends ProtoMessage<?>>
+      ProtobufTopic<T> getProtobufTopic(String name, Protobuf<T, MessageType> proto) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof ProtobufTopic<?>
+        && ((ProtobufTopic<?>) topic).getProto().equals(proto)) {
+      @SuppressWarnings("unchecked")
+      ProtobufTopic<T> wrapTopic = (ProtobufTopic<T>) topic;
+      return wrapTopic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    ProtobufTopic<T> wrapTopic = ProtobufTopic.wrap(this, handle, proto);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get struct-encoded value topic.
+   *
+   * @param <T> value class (inferred from struct)
+   * @param name topic name
+   * @param struct struct serialization implementation
+   * @return StructTopic
+   */
+  public <T>
+      StructTopic<T> getStructTopic(String name, Struct<T> struct) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof StructTopic<?>
+        && ((StructTopic<?>) topic).getStruct().equals(struct)) {
+      @SuppressWarnings("unchecked")
+      StructTopic<T> wrapTopic = (StructTopic<T>) topic;
+      return wrapTopic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    StructTopic<T> wrapTopic = StructTopic.wrap(this, handle, struct);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  /**
+   * Get struct-encoded value array topic.
+   *
+   * @param <T> value class (inferred from struct)
+   * @param name topic name
+   * @param struct struct serialization implementation
+   * @return StructArrayTopic
+   */
+  public <T>
+      StructArrayTopic<T> getStructArrayTopic(String name, Struct<T> struct) {
+    Topic topic = m_topics.get(name);
+    if (topic instanceof StructArrayTopic<?>
+        && ((StructArrayTopic<?>) topic).getStruct().equals(struct)) {
+      @SuppressWarnings("unchecked")
+      StructArrayTopic<T> wrapTopic = (StructArrayTopic<T>) topic;
+      return wrapTopic;
+    }
+
+    int handle;
+    if (topic == null) {
+      handle = NetworkTablesJNI.getTopic(m_handle, name);
+    } else {
+      handle = topic.getHandle();
+    }
+
+    StructArrayTopic<T> wrapTopic = StructArrayTopic.wrap(this, handle, struct);
+    m_topics.put(name, wrapTopic);
+
+    // also cache by handle
+    m_topicsByHandle.put(handle, wrapTopic);
+
+    return wrapTopic;
+  }
+
+  private Topic[] topicHandlesToTopics(int[] handles) {
+    Topic[] topics = new Topic[handles.length];
+    for (int i = 0; i < handles.length; i++) {
+      topics[i] = getCachedTopic(handles[i]);
+    }
+    return topics;
+  }
+
+  /**
+   * Get all published topics.
+   *
+   * @return Array of topics.
+   */
+  public Topic[] getTopics() {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, "", 0));
+  }
+
+  /**
+   * Get published topics starting with the given prefix. The results are optionally filtered by
+   * string prefix to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @return Array of topic information.
+   */
+  public Topic[] getTopics(String prefix) {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, prefix, 0));
+  }
+
+  /**
+   * Get published topics starting with the given prefix. The results are optionally filtered by
+   * string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types bitmask of data types; 0 is treated as a "don't care"
+   * @return Array of topic information.
+   */
+  public Topic[] getTopics(String prefix, int types) {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopics(m_handle, prefix, types));
+  }
+
+  /**
+   * Get published topics starting with the given prefix. The results are optionally filtered by
+   * string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types array of data type strings
+   * @return Array of topic information.
+   */
+  public Topic[] getTopics(String prefix, String[] types) {
+    return topicHandlesToTopics(NetworkTablesJNI.getTopicsStr(m_handle, prefix, types));
+  }
+
+  /**
+   * Get information about all topics.
+   *
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo() {
+    return NetworkTablesJNI.getTopicInfos(this, m_handle, "", 0);
+  }
+
+  /**
+   * Get information about topics starting with the given prefix. The results are optionally
+   * filtered by string prefix to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo(String prefix) {
+    return NetworkTablesJNI.getTopicInfos(this, m_handle, prefix, 0);
+  }
+
+  /**
+   * Get information about topics starting with the given prefix. The results are optionally
+   * filtered by string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types bitmask of data types; 0 is treated as a "don't care"
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo(String prefix, int types) {
+    return NetworkTablesJNI.getTopicInfos(this, m_handle, prefix, types);
+  }
+
+  /**
+   * Get information about topics starting with the given prefix. The results are optionally
+   * filtered by string prefix and data type to only return a subset of all topics.
+   *
+   * @param prefix topic name required prefix; only topics whose name starts with this string are
+   *     returned
+   * @param types array of data type strings
+   * @return Array of topic information.
+   */
+  public TopicInfo[] getTopicInfo(String prefix, String[] types) {
+    return NetworkTablesJNI.getTopicInfosStr(this, m_handle, prefix, types);
+  }
+
+  /* Cache of created entries. */
+  private final ConcurrentMap<String, NetworkTableEntry> m_entries = new ConcurrentHashMap<>();
+
+  /**
+   * Gets the entry for a key.
+   *
+   * @param name Key
+   * @return Network table entry.
+   */
+  public NetworkTableEntry getEntry(String name) {
+    NetworkTableEntry entry = m_entries.get(name);
+    if (entry == null) {
+      entry = new NetworkTableEntry(this, NetworkTablesJNI.getEntry(m_handle, name));
+      NetworkTableEntry oldEntry = m_entries.putIfAbsent(name, entry);
+      if (oldEntry != null) {
+        entry = oldEntry;
+      }
+    }
+    return entry;
+  }
+
+  /* Cache of created topics. */
+  private final ConcurrentMap<String, Topic> m_topics = new ConcurrentHashMap<>();
+  private final ConcurrentMap<Integer, Topic> m_topicsByHandle = new ConcurrentHashMap<>();
+
+  Topic getCachedTopic(String name) {
+    Topic topic = m_topics.get(name);
+    if (topic == null) {
+      int handle = NetworkTablesJNI.getTopic(m_handle, name);
+      topic = new Topic(this, handle);
+      Topic oldTopic = m_topics.putIfAbsent(name, topic);
+      if (oldTopic != null) {
+        topic = oldTopic;
+      }
+      // also cache by handle
+      m_topicsByHandle.putIfAbsent(handle, topic);
+    }
+    return topic;
+  }
+
+  Topic getCachedTopic(int handle) {
+    Topic topic = m_topicsByHandle.get(handle);
+    if (topic == null) {
+      topic = new Topic(this, handle);
+      Topic oldTopic = m_topicsByHandle.putIfAbsent(handle, topic);
+      if (oldTopic != null) {
+        topic = oldTopic;
+      }
+    }
+    return topic;
+  }
+
+  /* Cache of created tables. */
+  private final ConcurrentMap<String, NetworkTable> m_tables = new ConcurrentHashMap<>();
+
+  /**
+   * Gets the table with the specified key.
+   *
+   * @param key the key name
+   * @return The network table
+   */
+  public NetworkTable getTable(String key) {
+    // prepend leading / if not present
+    String theKey;
+    if (key.isEmpty() || "/".equals(key)) {
+      theKey = "";
+    } else if (key.charAt(0) == NetworkTable.PATH_SEPARATOR) {
+      theKey = key;
+    } else {
+      theKey = NetworkTable.PATH_SEPARATOR + key;
+    }
+
+    // cache created tables
+    NetworkTable table = m_tables.get(theKey);
+    if (table == null) {
+      table = new NetworkTable(this, theKey);
+      NetworkTable oldTable = m_tables.putIfAbsent(theKey, table);
+      if (oldTable != null) {
+        table = oldTable;
+      }
+    }
+    return table;
+  }
+
+  /*
+   * Callback Creation Functions
+   */
+
+  private static class ListenerStorage implements AutoCloseable {
+    private final ReentrantLock m_lock = new ReentrantLock();
+    private final Map<Integer, Consumer<NetworkTableEvent>> m_listeners = new HashMap<>();
+    private Thread m_thread;
+    private int m_poller;
+    private boolean m_waitQueue;
+    private final Event m_waitQueueEvent = new Event();
+    private final Condition m_waitQueueCond = m_lock.newCondition();
+    private final NetworkTableInstance m_inst;
+
+    ListenerStorage(NetworkTableInstance inst) {
+      m_inst = inst;
+    }
+
+    int add(
+        String[] prefixes,
+        EnumSet<NetworkTableEvent.Kind> eventKinds,
+        Consumer<NetworkTableEvent> listener) {
+      m_lock.lock();
+      try {
+        if (m_poller == 0) {
+          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
+          startThread();
+        }
+        int h = NetworkTablesJNI.addListener(m_poller, prefixes, eventKinds);
+        m_listeners.put(h, listener);
+        return h;
+      } finally {
+        m_lock.unlock();
+      }
+    }
+
+    int add(
+        int handle,
+        EnumSet<NetworkTableEvent.Kind> eventKinds,
+        Consumer<NetworkTableEvent> listener) {
+      m_lock.lock();
+      try {
+        if (m_poller == 0) {
+          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
+          startThread();
+        }
+        int h = NetworkTablesJNI.addListener(m_poller, handle, eventKinds);
+        m_listeners.put(h, listener);
+        return h;
+      } finally {
+        m_lock.unlock();
+      }
+    }
+
+    int addLogger(int minLevel, int maxLevel, Consumer<NetworkTableEvent> listener) {
+      m_lock.lock();
+      try {
+        if (m_poller == 0) {
+          m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
+          startThread();
+        }
+        int h = NetworkTablesJNI.addLogger(m_poller, minLevel, maxLevel);
+        m_listeners.put(h, listener);
+        return h;
+      } finally {
+        m_lock.unlock();
+      }
+    }
+
+    void remove(int listener) {
+      m_lock.lock();
+      try {
+        m_listeners.remove(listener);
+      } finally {
+        m_lock.unlock();
+      }
+      NetworkTablesJNI.removeListener(listener);
+    }
+
+    @Override
+    public void close() {
+      if (m_poller != 0) {
+        NetworkTablesJNI.destroyListenerPoller(m_poller);
+      }
+      m_poller = 0;
+    }
+
+    private void startThread() {
+      m_thread =
+          new Thread(
+              () -> {
+                boolean wasInterrupted = false;
+                int[] handles = new int[] { m_poller, m_waitQueueEvent.getHandle() };
+                while (!Thread.interrupted()) {
+                  try {
+                    WPIUtilJNI.waitForObjects(handles);
+                  } catch (InterruptedException ex) {
+                    m_lock.lock();
+                    try {
+                      if (m_waitQueue) {
+                        m_waitQueue = false;
+                        m_waitQueueCond.signalAll();
+                      }
+                    } finally {
+                      m_lock.unlock();
+                    }
+                    Thread.currentThread().interrupt();
+                    // don't try to destroy poller, as its handle is likely no longer valid
+                    wasInterrupted = true;
+                    break;
+                  }
+                  for (NetworkTableEvent event :
+                      NetworkTablesJNI.readListenerQueue(m_inst, m_poller)) {
+                    Consumer<NetworkTableEvent> listener;
+                    m_lock.lock();
+                    try {
+                      listener = m_listeners.get(event.listener);
+                    } finally {
+                      m_lock.unlock();
+                    }
+                    if (listener != null) {
+                      try {
+                        listener.accept(event);
+                      } catch (Throwable throwable) {
+                        System.err.println(
+                            "Unhandled exception during listener callback: "
+                            + throwable.toString());
+                        throwable.printStackTrace();
+                      }
+                    }
+                  }
+                  m_lock.lock();
+                  try {
+                    if (m_waitQueue) {
+                      m_waitQueue = false;
+                      m_waitQueueCond.signalAll();
+                    }
+                  } finally {
+                    m_lock.unlock();
+                  }
+                }
+                m_lock.lock();
+                try {
+                  if (!wasInterrupted) {
+                    NetworkTablesJNI.destroyListenerPoller(m_poller);
+                  }
+                  m_poller = 0;
+                } finally {
+                  m_lock.unlock();
+                }
+              },
+              "NTListener");
+      m_thread.setDaemon(true);
+      m_thread.start();
+    }
+
+    boolean waitForQueue(double timeout) {
+      m_lock.lock();
+      try {
+        if (m_poller != 0) {
+          m_waitQueue = true;
+          m_waitQueueEvent.set();
+          while (m_waitQueue) {
+            try {
+              if (timeout < 0) {
+                m_waitQueueCond.await();
+              } else {
+                return m_waitQueueCond.await((long) (timeout * 1e9), TimeUnit.NANOSECONDS);
+              }
+            } catch (InterruptedException ex) {
+              Thread.currentThread().interrupt();
+              return true;
+            }
+          }
+        }
+      } finally {
+        m_lock.unlock();
+      }
+      return true;
+    }
+  }
+
+  private final ListenerStorage m_listeners = new ListenerStorage(this);
+
+  /**
+   * Remove a connection listener.
+   *
+   * @param listener Listener handle to remove
+   */
+  public void removeListener(int listener) {
+    m_listeners.remove(listener);
+  }
+
+  /**
+   * Wait for the listener queue to be empty. This is primarily useful for deterministic
+   * testing. This blocks until either the listener queue is empty (e.g. there are no
+   * more events that need to be passed along to callbacks or poll queues) or the timeout expires.
+   *
+   * @param timeout timeout, in seconds. Set to 0 for non-blocking behavior, or a negative value to
+   *     block indefinitely
+   * @return False if timed out, otherwise true.
+   */
+  public boolean waitForListenerQueue(double timeout) {
+    return m_listeners.waitForQueue(timeout);
+  }
+
+  /**
+   * Add a connection listener. The callback function is called asynchronously on a separate
+   * thread, so it's important to use synchronization or atomics when accessing any shared state
+   * from the callback function.
+   *
+   * @param immediateNotify Notify listener of all existing connections
+   * @param listener Listener to add
+   * @return Listener handle
+   */
+  public int addConnectionListener(
+      boolean immediateNotify, Consumer<NetworkTableEvent> listener) {
+    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kConnection);
+    if (immediateNotify) {
+      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
+    }
+    return m_listeners.add(m_handle, eventKinds, listener);
+  }
+
+  /**
+   * Add a time synchronization listener. The callback function is called asynchronously on a
+   * separate thread, so it's important to use synchronization or atomics when accessing any shared
+   * state from the callback function.
+   *
+   * @param immediateNotify Notify listener of current time synchronization value
+   * @param listener Listener to add
+   * @return Listener handle
+   */
+  public int addTimeSyncListener(
+      boolean immediateNotify, Consumer<NetworkTableEvent> listener) {
+    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kTimeSync);
+    if (immediateNotify) {
+      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
+    }
+    return m_listeners.add(m_handle, eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on a particular topic. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function.
+   *
+   * <p>This creates a corresponding internal subscriber with the lifetime of the
+   * listener.
+   *
+   * @param topic Topic
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      Topic topic,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (topic.getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("topic is not from this instance");
+    }
+    return m_listeners.add(topic.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on a subscriber. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function. This does NOT keep the subscriber
+   * active.
+   *
+   * @param subscriber Subscriber
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      Subscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (subscriber.getTopic().getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("subscriber is not from this instance");
+    }
+    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on a subscriber. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function. This does NOT keep the subscriber
+   * active.
+   *
+   * @param subscriber Subscriber
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      MultiSubscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (subscriber.getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("subscriber is not from this instance");
+    }
+    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes on an entry. The callback function is called
+   * asynchronously on a separate thread, so it's important to use synchronization or atomics when
+   * accessing any shared state from the callback function.
+   *
+   * @param entry Entry
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      NetworkTableEntry entry,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    if (entry.getTopic().getInstance().getHandle() != m_handle) {
+      throw new IllegalArgumentException("entry is not from this instance");
+    }
+    return m_listeners.add(entry.getHandle(), eventKinds, listener);
+  }
+
+  /**
+   * Add a listener for changes to topics with names that start with any of the given
+   * prefixes. The callback function is called asynchronously on a separate thread, so it's
+   * important to use synchronization or atomics when accessing any shared state from the callback
+   * function.
+   *
+   * <p>This creates a corresponding internal subscriber with the lifetime of the
+   * listener.
+   *
+   * @param prefixes Topic name string prefixes
+   * @param eventKinds set of event kinds to listen to
+   * @param listener Listener function
+   * @return Listener handle
+   */
+  public int addListener(
+      String[] prefixes,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
+    return m_listeners.add(prefixes, eventKinds, listener);
+  }
+
+  /*
+   * Client/Server Functions
+   */
+
+  /**
+   * Get the current network mode.
+   *
+   * @return Enum set of NetworkMode.
+   */
+  public EnumSet<NetworkMode> getNetworkMode() {
+    int flags = NetworkTablesJNI.getNetworkMode(m_handle);
+    EnumSet<NetworkMode> rv = EnumSet.noneOf(NetworkMode.class);
+    for (NetworkMode mode : NetworkMode.values()) {
+      if ((flags & mode.getValue()) != 0) {
+        rv.add(mode);
+      }
+    }
+    return rv;
+  }
+
+  /**
+   * Starts local-only operation. Prevents calls to startServer or startClient from taking effect.
+   * Has no effect if startServer or startClient has already been called.
+   */
+  public void startLocal() {
+    NetworkTablesJNI.startLocal(m_handle);
+  }
+
+  /**
+   * Stops local-only operation. startServer or startClient can be called after this call to start
+   * a server or client.
+   */
+  public void stopLocal() {
+    NetworkTablesJNI.stopLocal(m_handle);
+  }
+
+  /**
+   * Starts a server using the networktables.json as the persistent file, using the default
+   * listening address and port.
+   */
+  public void startServer() {
+    startServer("networktables.json");
+  }
+
+  /**
+   * Starts a server using the specified persistent filename, using the default listening address
+   * and port.
+   *
+   * @param persistFilename the name of the persist file to use
+   */
+  public void startServer(String persistFilename) {
+    startServer(persistFilename, "");
+  }
+
+  /**
+   * Starts a server using the specified filename and listening address, using the default port.
+   *
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
+   */
+  public void startServer(String persistFilename, String listenAddress) {
+    startServer(persistFilename, listenAddress, kDefaultPort3, kDefaultPort4);
+  }
+
+  /**
+   * Starts a server using the specified filename, listening address, and port.
+   *
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
+   * @param port3 port to communicate over (NT3)
+   */
+  public void startServer(String persistFilename, String listenAddress, int port3) {
+    startServer(persistFilename, listenAddress, port3, kDefaultPort4);
+  }
+
+  /**
+   * Starts a server using the specified filename, listening address, and port.
+   *
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
+   * @param port3 port to communicate over (NT3)
+   * @param port4 port to communicate over (NT4)
+   */
+  public void startServer(String persistFilename, String listenAddress, int port3, int port4) {
+    NetworkTablesJNI.startServer(m_handle, persistFilename, listenAddress, port3, port4);
+  }
+
+  /** Stops the server if it is running. */
+  public void stopServer() {
+    NetworkTablesJNI.stopServer(m_handle);
+  }
+
+  /**
+   * Starts a NT3 client. Use SetServer or SetServerTeam to set the server name and port.
+   *
+   * @param identity network identity to advertise (cannot be empty string)
+   */
+  public void startClient3(String identity) {
+    NetworkTablesJNI.startClient3(m_handle, identity);
+  }
+
+  /**
+   * Starts a NT4 client. Use SetServer or SetServerTeam to set the server name and port.
+   *
+   * @param identity network identity to advertise (cannot be empty string)
+   */
+  public void startClient4(String identity) {
+    NetworkTablesJNI.startClient4(m_handle, identity);
+  }
+
+  /** Stops the client if it is running. */
+  public void stopClient() {
+    NetworkTablesJNI.stopClient(m_handle);
+  }
+
+  /**
+   * Sets server address and port for client (without restarting client). Changes the port to the
+   * default port.
+   *
+   * @param serverName server name
+   */
+  public void setServer(String serverName) {
+    setServer(serverName, 0);
+  }
+
+  /**
+   * Sets server address and port for client (without restarting client).
+   *
+   * @param serverName server name
+   * @param port port to communicate over (0=default)
+   */
+  public void setServer(String serverName, int port) {
+    NetworkTablesJNI.setServer(m_handle, serverName, port);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). Changes the port to the
+   * default port. The client will attempt to connect to each server in round robin fashion.
+   *
+   * @param serverNames array of server names
+   */
+  public void setServer(String[] serverNames) {
+    setServer(serverNames, 0);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). The client will attempt
+   * to connect to each server in round robin fashion.
+   *
+   * @param serverNames array of server names
+   * @param port port to communicate over (0=default)
+   */
+  public void setServer(String[] serverNames, int port) {
+    int[] ports = new int[serverNames.length];
+    for (int i = 0; i < serverNames.length; i++) {
+      ports[i] = port;
+    }
+    setServer(serverNames, ports);
+  }
+
+  /**
+   * Sets server addresses and ports for client (without restarting client). The client will
+   * attempt to connect to each server in round robin fashion.
+   *
+   * @param serverNames array of server names
+   * @param ports array of port numbers (0=default)
+   */
+  public void setServer(String[] serverNames, int[] ports) {
+    NetworkTablesJNI.setServer(m_handle, serverNames, ports);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). Changes the port to the
+   * default port. The client will attempt to connect to each server in round robin fashion.
+   *
+   * @param team team number
+   */
+  public void setServerTeam(int team) {
+    setServerTeam(team, 0);
+  }
+
+  /**
+   * Sets server addresses and port for client (without restarting client). Connects using commonly
+   * known robot addresses for the specified team.
+   *
+   * @param team team number
+   * @param port port to communicate over (0=default)
+   */
+  public void setServerTeam(int team, int port) {
+    NetworkTablesJNI.setServerTeam(m_handle, team, port);
+  }
+
+  /**
+   * Disconnects the client if it's running and connected. This will automatically start
+   * reconnection attempts to the current server list.
+   */
+  public void disconnect() {
+    NetworkTablesJNI.disconnect(m_handle);
+  }
+
+  /**
+   * Starts requesting server address from Driver Station. This connects to the Driver Station
+   * running on localhost to obtain the server IP address, and connects with the default port.
+   */
+  public void startDSClient() {
+    startDSClient(0);
+  }
+
+  /**
+   * Starts requesting server address from Driver Station. This connects to the Driver Station
+   * running on localhost to obtain the server IP address.
+   *
+   * @param port server port to use in combination with IP from DS (0=default)
+   */
+  public void startDSClient(int port) {
+    NetworkTablesJNI.startDSClient(m_handle, port);
+  }
+
+  /** Stops requesting server address from Driver Station. */
+  public void stopDSClient() {
+    NetworkTablesJNI.stopDSClient(m_handle);
+  }
+
+  /**
+   * Flushes all updated values immediately to the local client/server. This does not flush to the
+   * network.
+   */
+  public void flushLocal() {
+    NetworkTablesJNI.flushLocal(m_handle);
+  }
+
+  /**
+   * Flushes all updated values immediately to the network. Note: This is rate-limited to protect
+   * the network from flooding. This is primarily useful for synchronizing network updates with
+   * user code.
+   */
+  public void flush() {
+    NetworkTablesJNI.flush(m_handle);
+  }
+
+  /**
+   * Gets information on the currently established network connections. If operating as a client,
+   * this will return either zero or one values.
+   *
+   * @return array of connection information
+   */
+  public ConnectionInfo[] getConnections() {
+    return NetworkTablesJNI.getConnections(m_handle);
+  }
+
+  /**
+   * Return whether or not the instance is connected to another node.
+   *
+   * @return True if connected.
+   */
+  public boolean isConnected() {
+    return NetworkTablesJNI.isConnected(m_handle);
+  }
+
+  /**
+   * Get the time offset between server time and local time. Add this value to local time to get
+   * the estimated equivalent server time. In server mode, this always returns 0. In client mode,
+   * this returns the time offset only if the client and server are connected and have exchanged
+   * synchronization messages. Note the time offset may change over time as it is periodically
+   * updated; to receive updates as events, add a listener to the "time sync" event.
+   *
+   * @return Time offset in microseconds (optional)
+   */
+  public OptionalLong getServerTimeOffset() {
+    return NetworkTablesJNI.getServerTimeOffset(m_handle);
+  }
+
+  /**
+   * Starts logging entry changes to a DataLog.
+   *
+   * @param log data log object; lifetime must extend until StopEntryDataLog is called or the
+   *     instance is destroyed
+   * @param prefix only store entries with names that start with this prefix; the prefix is not
+   *     included in the data log entry name
+   * @param logPrefix prefix to add to data log entry names
+   * @return Data logger handle
+   */
+  public int startEntryDataLog(DataLog log, String prefix, String logPrefix) {
+    return NetworkTablesJNI.startEntryDataLog(m_handle, log, prefix, logPrefix);
+  }
+
+  /**
+   * Stops logging entry changes to a DataLog.
+   *
+   * @param logger data logger handle
+   */
+  public static void stopEntryDataLog(int logger) {
+    NetworkTablesJNI.stopEntryDataLog(logger);
+  }
+
+  /**
+   * Starts logging connection changes to a DataLog.
+   *
+   * @param log data log object; lifetime must extend until StopConnectionDataLog is called or the
+   *     instance is destroyed
+   * @param name data log entry name
+   * @return Data logger handle
+   */
+  public int startConnectionDataLog(DataLog log, String name) {
+    return NetworkTablesJNI.startConnectionDataLog(m_handle, log, name);
+  }
+
+  /**
+   * Stops logging connection changes to a DataLog.
+   *
+   * @param logger data logger handle
+   */
+  public static void stopConnectionDataLog(int logger) {
+    NetworkTablesJNI.stopConnectionDataLog(logger);
+  }
+
+  /**
+   * Add logger callback function. By default, log messages are sent to stderr; this function sends
+   * log messages with the specified levels to the provided callback function instead. The callback
+   * function will only be called for log messages with level greater than or equal to minLevel and
+   * less than or equal to maxLevel; messages outside this range will be silently ignored.
+   *
+   * @param minLevel minimum log level
+   * @param maxLevel maximum log level
+   * @param func callback function
+   * @return Listener handle
+   */
+  public int addLogger(int minLevel, int maxLevel, Consumer<NetworkTableEvent> func) {
+    return m_listeners.addLogger(minLevel, maxLevel, func);
+  }
+
+  /**
+   * Returns whether there is a data schema already registered with the given name that this
+   * instance has published. This does NOT perform a check as to whether the schema has already
+   * been published by another node on the network.
+   *
+   * @param name Name (the string passed as the data type for topics using this schema)
+   * @return True if schema already registered
+   */
+  public boolean hasSchema(String name) {
+    return m_schemas.containsKey("/.schema/" + name);
+  }
+
+  /**
+   * Registers a data schema. Data schemas provide information for how a certain data type string
+   * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g.
+   * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas
+   * are published just like normal topics, with the name being generated from the provided name:
+   * "/.schema/name". Duplicate calls to this function with the same name are silently ignored.
+   *
+   * @param name Name (the string passed as the data type for topics using this schema)
+   * @param type Type of schema (e.g. "protobuf", "struct", etc)
+   * @param schema Schema data
+   */
+  public void addSchema(String name, String type, byte[] schema) {
+    m_schemas.computeIfAbsent("/.schema/" + name, k -> {
+      RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}");
+      pub.setDefault(schema);
+      return pub;
+    });
+  }
+
+  /**
+   * Registers a data schema. Data schemas provide information for how a certain data type string
+   * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g.
+   * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas
+   * are published just like normal topics, with the name being generated from the provided name:
+   * "/.schema/name". Duplicate calls to this function with the same name are silently ignored.
+   *
+   * @param name Name (the string passed as the data type for topics using this schema)
+   * @param type Type of schema (e.g. "protobuf", "struct", etc)
+   * @param schema Schema data
+   */
+  public void addSchema(String name, String type, String schema) {
+    m_schemas.computeIfAbsent("/.schema/" + name, k -> {
+      RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}");
+      pub.setDefault(StandardCharsets.UTF_8.encode(schema));
+      return pub;
+    });
+  }
+
+  /**
+   * Registers a protobuf schema. Duplicate calls to this function with the same name are silently
+   * ignored.
+   *
+   * @param proto protobuf serialization object
+   */
+  public void addSchema(Protobuf<?, ?> proto) {
+    proto.forEachDescriptor(
+        this::hasSchema,
+        (typeString, schema) -> addSchema(typeString, "proto:FileDescriptorProto", schema));
+  }
+
+  /**
+   * Registers a struct schema. Duplicate calls to this function with the same name are silently
+   * ignored.
+   *
+   * @param struct struct serialization object
+   */
+  public void addSchema(Struct<?> struct) {
+    addSchemaImpl(struct, new HashSet<>());
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (other == this) {
+      return true;
+    }
+    if (!(other instanceof NetworkTableInstance)) {
+      return false;
+    }
+
+    return m_handle == ((NetworkTableInstance) other).m_handle;
+  }
+
+  @Override
+  public int hashCode() {
+    return m_handle;
+  }
+
+  private void addSchemaImpl(Struct<?> struct, Set<String> seen) {
+    String typeString = struct.getTypeString();
+    if (hasSchema(typeString)) {
+      return;
+    }
+    if (!seen.add(typeString)) {
+      throw new UnsupportedOperationException(typeString + ": circular reference with " + seen);
+    }
+    addSchema(typeString, "structschema", struct.getSchema());
+    for (Struct<?> inner : struct.getNested()) {
+      addSchemaImpl(inner, seen);
+    }
+    seen.remove(typeString);
+  }
+
+  private boolean m_owned;
+  private int m_handle;
+  private final ConcurrentMap<String, RawPublisher> m_schemas = new ConcurrentHashMap<>();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableValue.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableValue.java
new file mode 100644
index 0000000..3045d28
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTableValue.java
@@ -0,0 +1,742 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.Objects;
+
+/** A network table entry value. */
+@SuppressWarnings({"UnnecessaryParentheses", "PMD.MethodReturnsInternalArray"})
+public final class NetworkTableValue {
+  NetworkTableValue(NetworkTableType type, Object value, long time, long serverTime) {
+    m_type = type;
+    m_value = value;
+    m_time = time;
+    m_serverTime = serverTime;
+  }
+
+  NetworkTableValue(NetworkTableType type, Object value, long time) {
+    this(type, value, time, time == 0 ? 0 : 1);
+  }
+
+  NetworkTableValue(NetworkTableType type, Object value) {
+    this(type, value, NetworkTablesJNI.now(), 1);
+  }
+
+  NetworkTableValue(int type, Object value, long time, long serverTime) {
+    this(NetworkTableType.getFromInt(type), value, time, serverTime);
+  }
+
+  /**
+   * Get the data type.
+   *
+   * @return The type.
+   */
+  public NetworkTableType getType() {
+    return m_type;
+  }
+
+  /**
+   * Get the data value stored.
+   *
+   * @return The type.
+   */
+  public Object getValue() {
+    return m_value;
+  }
+
+  /**
+   * Get the creation time of the value in local time.
+   *
+   * @return The time, in the units returned by NetworkTablesJNI.now().
+   */
+  public long getTime() {
+    return m_time;
+  }
+
+  /**
+   * Get the creation time of the value in server time.
+   *
+   * @return The server time.
+   */
+  public long getServerTime() {
+    return m_serverTime;
+  }
+
+  /*
+   * Type Checkers
+   */
+
+  /**
+   * Determine if entry value contains a value or is unassigned.
+   *
+   * @return True if the entry value contains a value.
+   */
+  public boolean isValid() {
+    return m_type != NetworkTableType.kUnassigned;
+  }
+
+  /**
+   * Determine if entry value contains a boolean.
+   *
+   * @return True if the entry value is of boolean type.
+   */
+  public boolean isBoolean() {
+    return m_type == NetworkTableType.kBoolean;
+  }
+
+  /**
+   * Determine if entry value contains a long.
+   *
+   * @return True if the entry value is of long type.
+   */
+  public boolean isInteger() {
+    return m_type == NetworkTableType.kInteger;
+  }
+
+  /**
+   * Determine if entry value contains a float.
+   *
+   * @return True if the entry value is of float type.
+   */
+  public boolean isFloat() {
+    return m_type == NetworkTableType.kFloat;
+  }
+
+  /**
+   * Determine if entry value contains a double.
+   *
+   * @return True if the entry value is of double type.
+   */
+  public boolean isDouble() {
+    return m_type == NetworkTableType.kDouble;
+  }
+
+  /**
+   * Determine if entry value contains a String.
+   *
+   * @return True if the entry value is of String type.
+   */
+  public boolean isString() {
+    return m_type == NetworkTableType.kString;
+  }
+
+  /**
+   * Determine if entry value contains a byte[].
+   *
+   * @return True if the entry value is of byte[] type.
+   */
+  public boolean isRaw() {
+    return m_type == NetworkTableType.kRaw;
+  }
+
+  /**
+   * Determine if entry value contains a boolean[].
+   *
+   * @return True if the entry value is of boolean[] type.
+   */
+  public boolean isBooleanArray() {
+    return m_type == NetworkTableType.kBooleanArray;
+  }
+
+  /**
+   * Determine if entry value contains a long[].
+   *
+   * @return True if the entry value is of long[] type.
+   */
+  public boolean isIntegerArray() {
+    return m_type == NetworkTableType.kIntegerArray;
+  }
+
+  /**
+   * Determine if entry value contains a float[].
+   *
+   * @return True if the entry value is of float[] type.
+   */
+  public boolean isFloatArray() {
+    return m_type == NetworkTableType.kFloatArray;
+  }
+
+  /**
+   * Determine if entry value contains a double[].
+   *
+   * @return True if the entry value is of double[] type.
+   */
+  public boolean isDoubleArray() {
+    return m_type == NetworkTableType.kDoubleArray;
+  }
+
+  /**
+   * Determine if entry value contains a String[].
+   *
+   * @return True if the entry value is of String[] type.
+   */
+  public boolean isStringArray() {
+    return m_type == NetworkTableType.kStringArray;
+  }
+
+  /*
+   * Type-Safe Getters
+   */
+
+  /**
+   * Get the boolean value.
+   *
+   * @return The boolean value.
+   * @throws ClassCastException if the entry value is not of boolean type.
+   */
+  public boolean getBoolean() {
+    if (m_type != NetworkTableType.kBoolean) {
+      throw new ClassCastException("cannot convert " + m_type + " to boolean");
+    }
+    return (Boolean) m_value;
+  }
+
+  /**
+   * Get the long value.
+   *
+   * @return The long value.
+   * @throws ClassCastException if the entry value is not of long type.
+   */
+  public long getInteger() {
+    if (m_type != NetworkTableType.kInteger) {
+      throw new ClassCastException("cannot convert " + m_type + " to long");
+    }
+    return ((Number) m_value).longValue();
+  }
+
+  /**
+   * Get the float value.
+   *
+   * @return The float value.
+   * @throws ClassCastException if the entry value is not of float type.
+   */
+  public float getFloat() {
+    if (m_type != NetworkTableType.kFloat) {
+      throw new ClassCastException("cannot convert " + m_type + " to float");
+    }
+    return ((Number) m_value).floatValue();
+  }
+
+  /**
+   * Get the double value.
+   *
+   * @return The double value.
+   * @throws ClassCastException if the entry value is not of double type.
+   */
+  public double getDouble() {
+    if (m_type != NetworkTableType.kDouble) {
+      throw new ClassCastException("cannot convert " + m_type + " to double");
+    }
+    return ((Number) m_value).doubleValue();
+  }
+
+  /**
+   * Get the String value.
+   *
+   * @return The String value.
+   * @throws ClassCastException if the entry value is not of String type.
+   */
+  public String getString() {
+    if (m_type != NetworkTableType.kString) {
+      throw new ClassCastException("cannot convert " + m_type + " to String");
+    }
+    return (String) m_value;
+  }
+
+  /**
+   * Get the byte[] value.
+   *
+   * @return The byte[] value.
+   * @throws ClassCastException if the entry value is not of byte[] type.
+   */
+  public byte[] getRaw() {
+    if (m_type != NetworkTableType.kRaw) {
+      throw new ClassCastException("cannot convert " + m_type + " to byte[]");
+    }
+    return (byte[]) m_value;
+  }
+
+  /**
+   * Get the boolean[] value.
+   *
+   * @return The boolean[] value.
+   * @throws ClassCastException if the entry value is not of boolean[] type.
+   */
+  public boolean[] getBooleanArray() {
+    if (m_type != NetworkTableType.kBooleanArray) {
+      throw new ClassCastException("cannot convert " + m_type + " to boolean[]");
+    }
+    return (boolean[]) m_value;
+  }
+
+  /**
+   * Get the long[] value.
+   *
+   * @return The long[] value.
+   * @throws ClassCastException if the entry value is not of long[] type.
+   */
+  public long[] getIntegerArray() {
+    if (m_type != NetworkTableType.kIntegerArray) {
+      throw new ClassCastException("cannot convert " + m_type + " to long[]");
+    }
+    return (long[]) m_value;
+  }
+
+  /**
+   * Get the float[] value.
+   *
+   * @return The float[] value.
+   * @throws ClassCastException if the entry value is not of float[] type.
+   */
+  public float[] getFloatArray() {
+    if (m_type != NetworkTableType.kFloatArray) {
+      throw new ClassCastException("cannot convert " + m_type + " to float[]");
+    }
+    return (float[]) m_value;
+  }
+
+  /**
+   * Get the double[] value.
+   *
+   * @return The double[] value.
+   * @throws ClassCastException if the entry value is not of double[] type.
+   */
+  public double[] getDoubleArray() {
+    if (m_type != NetworkTableType.kDoubleArray) {
+      throw new ClassCastException("cannot convert " + m_type + " to double[]");
+    }
+    return (double[]) m_value;
+  }
+
+  /**
+   * Get the String[] value.
+   *
+   * @return The String[] value.
+   * @throws ClassCastException if the entry value is not of String[] type.
+   */
+  public String[] getStringArray() {
+    if (m_type != NetworkTableType.kStringArray) {
+      throw new ClassCastException("cannot convert " + m_type + " to String[]");
+    }
+    return (String[]) m_value;
+  }
+
+  /*
+   * Factory functions.
+   */
+
+  /**
+   * Creates a boolean value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeBoolean(boolean value) {
+    return new NetworkTableValue(NetworkTableType.kBoolean, Boolean.valueOf(value));
+  }
+
+  /**
+   * Creates a boolean value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeBoolean(boolean value, long time) {
+    return new NetworkTableValue(NetworkTableType.kBoolean, Boolean.valueOf(value), time);
+  }
+
+  /**
+   * Creates a long value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeInteger(long value) {
+    return new NetworkTableValue(NetworkTableType.kInteger, Long.valueOf(value));
+  }
+
+  /**
+   * Creates a long value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeInteger(long value, long time) {
+    return new NetworkTableValue(NetworkTableType.kInteger, Long.valueOf(value), time);
+  }
+
+  /**
+   * Creates a float value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeFloat(float value) {
+    return new NetworkTableValue(NetworkTableType.kFloat, Float.valueOf(value));
+  }
+
+  /**
+   * Creates a float value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeFloat(float value, long time) {
+    return new NetworkTableValue(NetworkTableType.kFloat, Float.valueOf(value), time);
+  }
+
+  /**
+   * Creates a double value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeDouble(double value) {
+    return new NetworkTableValue(NetworkTableType.kDouble, Double.valueOf(value));
+  }
+
+  /**
+   * Creates a double value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeDouble(double value, long time) {
+    return new NetworkTableValue(NetworkTableType.kDouble, Double.valueOf(value), time);
+  }
+
+  /**
+   * Creates a String value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeString(String value) {
+    return new NetworkTableValue(NetworkTableType.kString, (value));
+  }
+
+  /**
+   * Creates a String value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeString(String value, long time) {
+    return new NetworkTableValue(NetworkTableType.kString, (value), time);
+  }
+
+  /**
+   * Creates a byte[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeRaw(byte[] value) {
+    return new NetworkTableValue(NetworkTableType.kRaw, (value));
+  }
+
+  /**
+   * Creates a byte[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeRaw(byte[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kRaw, (value), time);
+  }
+
+  /**
+   * Creates a boolean[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeBooleanArray(boolean[] value) {
+    return new NetworkTableValue(NetworkTableType.kBooleanArray, (value));
+  }
+
+  /**
+   * Creates a boolean[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeBooleanArray(boolean[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kBooleanArray, (value), time);
+  }
+
+  /**
+   * Creates a boolean[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeBooleanArray(Boolean[] value) {
+    return new NetworkTableValue(NetworkTableType.kBooleanArray, toNativeBooleanArray(value));
+  }
+
+  /**
+   * Creates a boolean[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeBooleanArray(Boolean[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kBooleanArray, toNativeBooleanArray(value), time);
+  }
+
+  /**
+   * Creates a long[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeIntegerArray(long[] value) {
+    return new NetworkTableValue(NetworkTableType.kIntegerArray, (value));
+  }
+
+  /**
+   * Creates a long[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeIntegerArray(long[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kIntegerArray, (value), time);
+  }
+
+  /**
+   * Creates a long[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeIntegerArray(Long[] value) {
+    return new NetworkTableValue(NetworkTableType.kIntegerArray, toNativeIntegerArray(value));
+  }
+
+  /**
+   * Creates a long[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeIntegerArray(Long[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kIntegerArray, toNativeIntegerArray(value), time);
+  }
+
+  /**
+   * Creates a float[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeFloatArray(float[] value) {
+    return new NetworkTableValue(NetworkTableType.kFloatArray, (value));
+  }
+
+  /**
+   * Creates a float[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeFloatArray(float[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kFloatArray, (value), time);
+  }
+
+  /**
+   * Creates a float[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeFloatArray(Float[] value) {
+    return new NetworkTableValue(NetworkTableType.kFloatArray, toNativeFloatArray(value));
+  }
+
+  /**
+   * Creates a float[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeFloatArray(Float[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kFloatArray, toNativeFloatArray(value), time);
+  }
+
+  /**
+   * Creates a double[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeDoubleArray(double[] value) {
+    return new NetworkTableValue(NetworkTableType.kDoubleArray, (value));
+  }
+
+  /**
+   * Creates a double[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeDoubleArray(double[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kDoubleArray, (value), time);
+  }
+
+  /**
+   * Creates a double[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeDoubleArray(Double[] value) {
+    return new NetworkTableValue(NetworkTableType.kDoubleArray, toNativeDoubleArray(value));
+  }
+
+  /**
+   * Creates a double[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeDoubleArray(Double[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kDoubleArray, toNativeDoubleArray(value), time);
+  }
+
+  /**
+   * Creates a String[] value.
+   *
+   * @param value the value
+   * @return The entry value
+   */
+  public static NetworkTableValue makeStringArray(String[] value) {
+    return new NetworkTableValue(NetworkTableType.kStringArray, (value));
+  }
+
+  /**
+   * Creates a String[] value.
+   *
+   * @param value the value
+   * @param time the creation time to use (instead of the current time)
+   * @return The entry value
+   */
+  public static NetworkTableValue makeStringArray(String[] value, long time) {
+    return new NetworkTableValue(NetworkTableType.kStringArray, (value), time);
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (other == this) {
+      return true;
+    }
+    if (!(other instanceof NetworkTableValue)) {
+      return false;
+    }
+    NetworkTableValue ntOther = (NetworkTableValue) other;
+    return m_type == ntOther.m_type && m_value.equals(ntOther.m_value);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_type, m_value);
+  }
+
+  // arraycopy() doesn't know how to unwrap boxed values; this is a false positive in PMD
+  // (see https://sourceforge.net/p/pmd/bugs/804/)
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static boolean[] toNativeBooleanArray(Boolean[] arr) {
+    boolean[] out = new boolean[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static double[] toNativeDoubleArray(Number[] arr) {
+    double[] out = new double[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i].doubleValue();
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static long[] toNativeIntegerArray(Number[] arr) {
+    long[] out = new long[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i].longValue();
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static float[] toNativeFloatArray(Number[] arr) {
+    float[] out = new float[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i].floatValue();
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Boolean[] fromNativeBooleanArray(boolean[] arr) {
+    Boolean[] out = new Boolean[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Long[] fromNativeIntegerArray(long[] arr) {
+    Long[] out = new Long[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Float[] fromNativeFloatArray(float[] arr) {
+    Float[] out = new Float[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  @SuppressWarnings("PMD.AvoidArrayLoops")
+  static Double[] fromNativeDoubleArray(double[] arr) {
+    Double[] out = new Double[arr.length];
+    for (int i = 0; i < arr.length; i++) {
+      out[i] = arr[i];
+    }
+    return out;
+  }
+
+  private NetworkTableType m_type;
+  private Object m_value;
+  private long m_time;
+  private long m_serverTime;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
new file mode 100644
index 0000000..5aa4060
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
@@ -0,0 +1,514 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.RuntimeLoader;
+import edu.wpi.first.util.datalog.DataLog;
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.util.EnumSet;
+import java.util.OptionalLong;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+public final class NetworkTablesJNI {
+  static boolean libraryLoaded = false;
+  static RuntimeLoader<NetworkTablesJNI> loader = null;
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader =
+            new RuntimeLoader<>(
+                "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException if the library fails to load
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    loader =
+        new RuntimeLoader<>(
+            "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
+    loader.loadLibrary();
+    libraryLoaded = true;
+  }
+
+  private static PubSubOptions buildOptions(PubSubOption... options) {
+    if (options.length == 0) {
+      return null;  // optimize common case (JNI checks for null)
+    }
+    return new PubSubOptions(options);
+  }
+
+  public static native int getDefaultInstance();
+
+  public static native int createInstance();
+
+  public static native void destroyInstance(int inst);
+
+  public static native int getInstanceFromHandle(int handle);
+
+  private static native int getEntryImpl(
+      int topic, int type, String typeStr, PubSubOptions options);
+
+  public static native int getEntry(int inst, String key);
+
+  public static int getEntry(
+      int topic, int type, String typeStr, PubSubOptions options) {
+    return getEntryImpl(topic, type, typeStr, options);
+  }
+
+  public static int getEntry(
+      int topic, int type, String typeStr, PubSubOption... options) {
+    return getEntryImpl(topic, type, typeStr, buildOptions(options));
+  }
+
+  public static native String getEntryName(int entry);
+
+  public static native long getEntryLastChange(int entry);
+
+  public static native int getType(int entry);
+
+  /* Topic functions */
+
+  public static native int[] getTopics(int inst, String prefix, int types);
+
+  public static native int[] getTopicsStr(int inst, String prefix, String[] types);
+
+  public static native TopicInfo[] getTopicInfos(
+      NetworkTableInstance instObject, int inst, String prefix, int types);
+
+  public static native TopicInfo[] getTopicInfosStr(
+      NetworkTableInstance instObject, int inst, String prefix, String[] types);
+
+  public static native int getTopic(int inst, String name);
+
+  public static native String getTopicName(int topic);
+
+  public static native int getTopicType(int topic);
+
+  public static native void setTopicPersistent(int topic, boolean value);
+
+  public static native boolean getTopicPersistent(int topic);
+
+  public static native void setTopicRetained(int topic, boolean value);
+
+  public static native boolean getTopicRetained(int topic);
+
+  public static native void setTopicCached(int topic, boolean value);
+
+  public static native boolean getTopicCached(int topic);
+
+  public static native String getTopicTypeString(int topic);
+
+  public static native boolean getTopicExists(int topic);
+
+  public static native String getTopicProperty(int topic, String name);
+
+  public static native void setTopicProperty(int topic, String name, String value);
+
+  public static native void deleteTopicProperty(int topic, String name);
+
+  public static native String getTopicProperties(int topic);
+
+  public static native void setTopicProperties(int topic, String properties);
+
+  public static native int subscribe(
+      int topic, int type, String typeStr, PubSubOptions options);
+
+  public static int subscribe(
+      int topic, int type, String typeStr, PubSubOption... options) {
+    return subscribe(topic, type, typeStr, buildOptions(options));
+  }
+
+  public static native void unsubscribe(int sub);
+
+  public static native int publish(
+      int topic, int type, String typeStr, PubSubOptions options);
+
+  public static int publish(
+      int topic, int type, String typeStr, PubSubOption... options) {
+    return publish(topic, type, typeStr, buildOptions(options));
+  }
+
+  public static native int publishEx(
+      int topic, int type, String typeStr, String properties, PubSubOptions options);
+
+  public static int publishEx(
+      int topic, int type, String typeStr, String properties, PubSubOption... options) {
+    return publishEx(topic, type, typeStr, properties, buildOptions(options));
+  }
+
+  public static native void unpublish(int pubentry);
+
+  public static native void releaseEntry(int entry);
+
+  public static native void release(int pubsubentry);
+
+  public static native int getTopicFromHandle(int pubsubentry);
+
+  public static native int subscribeMultiple(int inst, String[] prefixes, PubSubOptions options);
+
+  public static int subscribeMultiple(int inst, String[] prefixes, PubSubOption... options) {
+    return subscribeMultiple(inst, prefixes, buildOptions(options));
+  }
+
+  public static native void unsubscribeMultiple(int sub);
+
+  public static native TimestampedBoolean getAtomicBoolean(
+      int subentry, boolean defaultValue);
+
+  public static native TimestampedBoolean[] readQueueBoolean(int subentry);
+
+  public static native boolean[] readQueueValuesBoolean(int subentry);
+
+  public static native boolean setBoolean(int entry, long time, boolean value);
+
+  public static native boolean getBoolean(int entry, boolean defaultValue);
+
+  public static native boolean setDefaultBoolean(int entry, long time, boolean defaultValue);
+
+
+  public static native TimestampedInteger getAtomicInteger(
+      int subentry, long defaultValue);
+
+  public static native TimestampedInteger[] readQueueInteger(int subentry);
+
+  public static native long[] readQueueValuesInteger(int subentry);
+
+  public static native boolean setInteger(int entry, long time, long value);
+
+  public static native long getInteger(int entry, long defaultValue);
+
+  public static native boolean setDefaultInteger(int entry, long time, long defaultValue);
+
+
+  public static native TimestampedFloat getAtomicFloat(
+      int subentry, float defaultValue);
+
+  public static native TimestampedFloat[] readQueueFloat(int subentry);
+
+  public static native float[] readQueueValuesFloat(int subentry);
+
+  public static native boolean setFloat(int entry, long time, float value);
+
+  public static native float getFloat(int entry, float defaultValue);
+
+  public static native boolean setDefaultFloat(int entry, long time, float defaultValue);
+
+
+  public static native TimestampedDouble getAtomicDouble(
+      int subentry, double defaultValue);
+
+  public static native TimestampedDouble[] readQueueDouble(int subentry);
+
+  public static native double[] readQueueValuesDouble(int subentry);
+
+  public static native boolean setDouble(int entry, long time, double value);
+
+  public static native double getDouble(int entry, double defaultValue);
+
+  public static native boolean setDefaultDouble(int entry, long time, double defaultValue);
+
+
+  public static native TimestampedString getAtomicString(
+      int subentry, String defaultValue);
+
+  public static native TimestampedString[] readQueueString(int subentry);
+
+  public static native String[] readQueueValuesString(int subentry);
+
+  public static native boolean setString(int entry, long time, String value);
+
+  public static native String getString(int entry, String defaultValue);
+
+  public static native boolean setDefaultString(int entry, long time, String defaultValue);
+
+
+  public static native TimestampedRaw getAtomicRaw(
+      int subentry, byte[] defaultValue);
+
+  public static native TimestampedRaw[] readQueueRaw(int subentry);
+
+  public static native byte[][] readQueueValuesRaw(int subentry);
+
+  public static boolean setRaw(int entry, long time, byte[] value) {
+    return setRaw(entry, time, value, 0, value.length);
+  }
+
+  public static native boolean setRaw(int entry, long time, byte[] value, int start, int len);
+
+  public static boolean setRaw(int entry, long time, ByteBuffer value) {
+    int pos = value.position();
+    return setRaw(entry, time, value, pos, value.capacity() - pos);
+  }
+
+  public static boolean setRaw(int entry, long time, ByteBuffer value, int start, int len) {
+    if (value.isDirect()) {
+      if (start < 0) {
+        throw new IndexOutOfBoundsException("start must be >= 0");
+      }
+      if (len < 0) {
+        throw new IndexOutOfBoundsException("len must be >= 0");
+      }
+      if ((start + len) > value.capacity()) {
+        throw new IndexOutOfBoundsException("start + len must be smaller than buffer capacity");
+      }
+      return setRawBuffer(entry, time, value, start, len);
+    } else if (value.hasArray()) {
+      return setRaw(entry, time, value.array(), value.arrayOffset() + start, len);
+    } else {
+      throw new UnsupportedOperationException("ByteBuffer must be direct or have a backing array");
+    }
+  }
+
+  private static native boolean setRawBuffer(int entry, long time, ByteBuffer value, int start, int len);
+
+  public static native byte[] getRaw(int entry, byte[] defaultValue);
+
+  public static boolean setDefaultRaw(int entry, long time, byte[] defaultValue) {
+    return setDefaultRaw(entry, time, defaultValue, 0, defaultValue.length);
+  }
+
+  public static native boolean setDefaultRaw(int entry, long time, byte[] defaultValue, int start, int len);
+
+  public static boolean setDefaultRaw(int entry, long time, ByteBuffer defaultValue) {
+    int pos = defaultValue.position();
+    return setDefaultRaw(entry, time, defaultValue, pos, defaultValue.limit() - pos);
+  }
+
+  public static boolean setDefaultRaw(int entry, long time, ByteBuffer defaultValue, int start, int len) {
+    if (defaultValue.isDirect()) {
+      if (start < 0) {
+        throw new IndexOutOfBoundsException("start must be >= 0");
+      }
+      if (len < 0) {
+        throw new IndexOutOfBoundsException("len must be >= 0");
+      }
+      if ((start + len) > defaultValue.capacity()) {
+        throw new IndexOutOfBoundsException("start + len must be smaller than buffer capacity");
+      }
+      return setDefaultRawBuffer(entry, time, defaultValue, start, len);
+    } else if (defaultValue.hasArray()) {
+      return setDefaultRaw(entry, time, defaultValue.array(), defaultValue.arrayOffset() + start, len);
+    } else {
+      throw new UnsupportedOperationException("ByteBuffer must be direct or have a backing array");
+    }
+  }
+
+  private static native boolean setDefaultRawBuffer(int entry, long time, ByteBuffer defaultValue, int start, int len);
+
+
+  public static native TimestampedBooleanArray getAtomicBooleanArray(
+      int subentry, boolean[] defaultValue);
+
+  public static native TimestampedBooleanArray[] readQueueBooleanArray(int subentry);
+
+  public static native boolean[][] readQueueValuesBooleanArray(int subentry);
+
+  public static native boolean setBooleanArray(int entry, long time, boolean[] value);
+
+  public static native boolean[] getBooleanArray(int entry, boolean[] defaultValue);
+
+  public static native boolean setDefaultBooleanArray(int entry, long time, boolean[] defaultValue);
+
+
+  public static native TimestampedIntegerArray getAtomicIntegerArray(
+      int subentry, long[] defaultValue);
+
+  public static native TimestampedIntegerArray[] readQueueIntegerArray(int subentry);
+
+  public static native long[][] readQueueValuesIntegerArray(int subentry);
+
+  public static native boolean setIntegerArray(int entry, long time, long[] value);
+
+  public static native long[] getIntegerArray(int entry, long[] defaultValue);
+
+  public static native boolean setDefaultIntegerArray(int entry, long time, long[] defaultValue);
+
+
+  public static native TimestampedFloatArray getAtomicFloatArray(
+      int subentry, float[] defaultValue);
+
+  public static native TimestampedFloatArray[] readQueueFloatArray(int subentry);
+
+  public static native float[][] readQueueValuesFloatArray(int subentry);
+
+  public static native boolean setFloatArray(int entry, long time, float[] value);
+
+  public static native float[] getFloatArray(int entry, float[] defaultValue);
+
+  public static native boolean setDefaultFloatArray(int entry, long time, float[] defaultValue);
+
+
+  public static native TimestampedDoubleArray getAtomicDoubleArray(
+      int subentry, double[] defaultValue);
+
+  public static native TimestampedDoubleArray[] readQueueDoubleArray(int subentry);
+
+  public static native double[][] readQueueValuesDoubleArray(int subentry);
+
+  public static native boolean setDoubleArray(int entry, long time, double[] value);
+
+  public static native double[] getDoubleArray(int entry, double[] defaultValue);
+
+  public static native boolean setDefaultDoubleArray(int entry, long time, double[] defaultValue);
+
+
+  public static native TimestampedStringArray getAtomicStringArray(
+      int subentry, String[] defaultValue);
+
+  public static native TimestampedStringArray[] readQueueStringArray(int subentry);
+
+  public static native String[][] readQueueValuesStringArray(int subentry);
+
+  public static native boolean setStringArray(int entry, long time, String[] value);
+
+  public static native String[] getStringArray(int entry, String[] defaultValue);
+
+  public static native boolean setDefaultStringArray(int entry, long time, String[] defaultValue);
+
+
+  public static native NetworkTableValue[] readQueueValue(int subentry);
+
+  public static native NetworkTableValue getValue(int entry);
+
+  public static native void setEntryFlags(int entry, int flags);
+
+  public static native int getEntryFlags(int entry);
+
+  public static native TopicInfo getTopicInfo(NetworkTableInstance inst, int topic);
+
+  public static native int createListenerPoller(int inst);
+
+  public static native void destroyListenerPoller(int poller);
+
+  private static int kindsToMask(EnumSet<NetworkTableEvent.Kind> kinds) {
+    int mask = 0;
+    for (NetworkTableEvent.Kind kind : kinds) {
+      mask |= kind.getValue();
+    }
+    return mask;
+  }
+
+  public static int addListener(int poller, String[] prefixes, EnumSet<NetworkTableEvent.Kind> kinds) {
+    return addListener(poller, prefixes, kindsToMask(kinds));
+  }
+
+  public static int addListener(int poller, int handle, EnumSet<NetworkTableEvent.Kind> kinds) {
+    return addListener(poller, handle, kindsToMask(kinds));
+  }
+
+  public static native int addListener(int poller, String[] prefixes, int mask);
+
+  public static native int addListener(int poller, int handle, int mask);
+
+  public static native NetworkTableEvent[] readListenerQueue(
+      NetworkTableInstance inst, int poller);
+
+  public static native void removeListener(int listener);
+
+  public static native int getNetworkMode(int inst);
+
+  public static native void startLocal(int inst);
+
+  public static native void stopLocal(int inst);
+
+  public static native void startServer(
+      int inst, String persistFilename, String listenAddress, int port3, int port4);
+
+  public static native void stopServer(int inst);
+
+  public static native void startClient3(int inst, String identity);
+
+  public static native void startClient4(int inst, String identity);
+
+  public static native void stopClient(int inst);
+
+  public static native void setServer(int inst, String serverName, int port);
+
+  public static native void setServer(int inst, String[] serverNames, int[] ports);
+
+  public static native void setServerTeam(int inst, int team, int port);
+
+  public static native void disconnect(int inst);
+
+  public static native void startDSClient(int inst, int port);
+
+  public static native void stopDSClient(int inst);
+
+  public static native void flushLocal(int inst);
+
+  public static native void flush(int inst);
+
+  public static native ConnectionInfo[] getConnections(int inst);
+
+  public static native boolean isConnected(int inst);
+
+  public static native OptionalLong getServerTimeOffset(int inst);
+
+  public static native long now();
+
+  private static native int startEntryDataLog(int inst, long log, String prefix, String logPrefix);
+
+  public static int startEntryDataLog(int inst, DataLog log, String prefix, String logPrefix) {
+    return startEntryDataLog(inst, log.getImpl(), prefix, logPrefix);
+  }
+
+  public static native void stopEntryDataLog(int logger);
+
+  private static native int startConnectionDataLog(int inst, long log, String name);
+
+  public static int startConnectionDataLog(int inst, DataLog log, String name) {
+    return startConnectionDataLog(inst, log.getImpl(), name);
+  }
+
+  public static native void stopConnectionDataLog(int logger);
+
+  public static native int addLogger(int poller, int minLevel, int maxLevel);
+
+  /** Utility class. */
+  private NetworkTablesJNI() {}
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawEntry.java
new file mode 100644
index 0000000..407cdd5
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables Raw entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface RawEntry extends RawSubscriber, RawPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawEntryImpl.java
new file mode 100644
index 0000000..55767f7
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawEntryImpl.java
@@ -0,0 +1,89 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.nio.ByteBuffer;
+
+/** NetworkTables Raw implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class RawEntryImpl extends EntryBase implements RawEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  RawEntryImpl(RawTopic topic, int handle, byte[] defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public RawTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public byte[] get() {
+    return NetworkTablesJNI.getRaw(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public byte[] get(byte[] defaultValue) {
+    return NetworkTablesJNI.getRaw(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedRaw getAtomic() {
+    return NetworkTablesJNI.getAtomicRaw(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedRaw getAtomic(byte[] defaultValue) {
+    return NetworkTablesJNI.getAtomicRaw(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedRaw[] readQueue() {
+    return NetworkTablesJNI.readQueueRaw(m_handle);
+  }
+
+  @Override
+  public byte[][] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesRaw(m_handle);
+  }
+
+  @Override
+  public void set(byte[] value, int start, int len, long time) {
+    NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+
+  @Override
+  public void set(ByteBuffer value, int start, int len, long time) {
+    NetworkTablesJNI.setRaw(m_handle, time, value, start, len);
+  }
+
+  @Override
+  public void setDefault(byte[] value, int start, int len) {
+    NetworkTablesJNI.setDefaultRaw(m_handle, 0, value, start, len);
+  }
+
+  @Override
+  public void setDefault(ByteBuffer value, int start, int len) {
+    NetworkTablesJNI.setDefaultRaw(m_handle, 0, value, start, len);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final RawTopic m_topic;
+  private final byte[] m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawPublisher.java
new file mode 100644
index 0000000..0cd578e
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawPublisher.java
@@ -0,0 +1,154 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+
+import java.nio.ByteBuffer;
+import java.util.function.Consumer;
+
+/** NetworkTables Raw publisher. */
+public interface RawPublisher extends Publisher, Consumer<byte[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  RawTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(byte[] value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  default void set(byte[] value, long time) {
+    set(value, 0, value.length, time);
+  }
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   */
+  default void set(byte[] value, int start, int len) {
+    set(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(byte[] value, int start, int len, long time);
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish; will send from value.position() to value.limit()
+   */
+  default void set(ByteBuffer value) {
+    set(value, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish; will send from value.position() to value.limit()
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  default void set(ByteBuffer value, long time) {
+    int pos = value.position();
+    set(value, pos, value.limit() - pos, time);
+  }
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   */
+  default void set(ByteBuffer value, int start, int len) {
+    set(value, start, len, 0);
+  }
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(ByteBuffer value, int start, int len, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  default void setDefault(byte[] value) {
+    setDefault(value, 0, value.length);
+  }
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.length - start)
+   */
+  void setDefault(byte[] value, int start, int len);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value; will send from value.position() to value.limit()
+   */
+  default void setDefault(ByteBuffer value) {
+    int pos = value.position();
+    setDefault(value, pos, value.limit() - pos);
+  }
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   * @param start Start position of data (in buffer)
+   * @param len Length of data (must be less than or equal to value.capacity() - start)
+   */
+  void setDefault(ByteBuffer value, int start, int len);
+
+  @Override
+  default void accept(byte[] value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawSubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawSubscriber.java
new file mode 100644
index 0000000..089d999
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawSubscriber.java
@@ -0,0 +1,80 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables Raw subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface RawSubscriber extends Subscriber, Supplier<byte[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  RawTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  byte[] get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  byte[] get(byte[] defaultValue);
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedRaw getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedRaw getAtomic(byte[] defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedRaw[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  byte[][] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawTopic.java
new file mode 100644
index 0000000..173a88b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/RawTopic.java
@@ -0,0 +1,154 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables Raw topic. */
+public final class RawTopic extends Topic {
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public RawTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getRawTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public RawTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public RawSubscriber subscribe(
+      String typeString,
+
+      byte[] defaultValue,
+      PubSubOption... options) {
+    return new RawEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kRaw.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+
+   * @param options publish options
+   * @return publisher
+   */
+  public RawPublisher publish(
+      String typeString,
+
+      PubSubOption... options) {
+    return new RawEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kRaw.getValue(),
+            typeString, options),
+        new byte[] {});
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public RawPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new RawEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kRaw.getValue(),
+            typeString, properties, options),
+        new byte[] {});
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public RawEntry getEntry(
+      String typeString,
+
+      byte[] defaultValue,
+      PubSubOption... options) {
+    return new RawEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kRaw.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayEntry.java
new file mode 100644
index 0000000..9169e90
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables StringArray entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface StringArrayEntry extends StringArraySubscriber, StringArrayPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayEntryImpl.java
new file mode 100644
index 0000000..536db7b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables StringArray implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class StringArrayEntryImpl extends EntryBase implements StringArrayEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  StringArrayEntryImpl(StringArrayTopic topic, int handle, String[] defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public StringArrayTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public String[] get() {
+    return NetworkTablesJNI.getStringArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public String[] get(String[] defaultValue) {
+    return NetworkTablesJNI.getStringArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedStringArray getAtomic() {
+    return NetworkTablesJNI.getAtomicStringArray(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedStringArray getAtomic(String[] defaultValue) {
+    return NetworkTablesJNI.getAtomicStringArray(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedStringArray[] readQueue() {
+    return NetworkTablesJNI.readQueueStringArray(m_handle);
+  }
+
+  @Override
+  public String[][] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesStringArray(m_handle);
+  }
+
+  @Override
+  public void set(String[] value, long time) {
+    NetworkTablesJNI.setStringArray(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(String[] value) {
+    NetworkTablesJNI.setDefaultStringArray(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final StringArrayTopic m_topic;
+  private final String[] m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayPublisher.java
new file mode 100644
index 0000000..d32350c
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Consumer;
+
+/** NetworkTables StringArray publisher. */
+public interface StringArrayPublisher extends Publisher, Consumer<String[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  StringArrayTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(String[] value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(String[] value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(String[] value);
+
+  @Override
+  default void accept(String[] value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArraySubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArraySubscriber.java
new file mode 100644
index 0000000..62a0575
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArraySubscriber.java
@@ -0,0 +1,80 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables StringArray subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface StringArraySubscriber extends Subscriber, Supplier<String[]> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  StringArrayTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  String[] get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  String[] get(String[] defaultValue);
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedStringArray getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedStringArray getAtomic(String[] defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedStringArray[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  String[][] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayTopic.java
new file mode 100644
index 0000000..bde93c0
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringArrayTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables StringArray topic. */
+public final class StringArrayTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "string[]";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public StringArrayTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getStringArrayTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public StringArrayTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public StringArraySubscriber subscribe(
+      String[] defaultValue,
+      PubSubOption... options) {
+    return new StringArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kStringArray.getValue(),
+            "string[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public StringArraySubscriber subscribeEx(
+      String typeString,
+      String[] defaultValue,
+      PubSubOption... options) {
+    return new StringArrayEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kStringArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public StringArrayPublisher publish(
+      PubSubOption... options) {
+    return new StringArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kStringArray.getValue(),
+            "string[]", options),
+        new String[] {});
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public StringArrayPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new StringArrayEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kStringArray.getValue(),
+            typeString, properties, options),
+        new String[] {});
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public StringArrayEntry getEntry(
+      String[] defaultValue,
+      PubSubOption... options) {
+    return new StringArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kStringArray.getValue(),
+            "string[]", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public StringArrayEntry getEntryEx(
+      String typeString,
+      String[] defaultValue,
+      PubSubOption... options) {
+    return new StringArrayEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kStringArray.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringEntry.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringEntry.java
new file mode 100644
index 0000000..6914179
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringEntry.java
@@ -0,0 +1,17 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/**
+ * NetworkTables String entry.
+ *
+ * <p>Unlike NetworkTableEntry, the entry goes away when close() is called.
+ */
+public interface StringEntry extends StringSubscriber, StringPublisher {
+  /** Stops publishing the entry if it's published. */
+  void unpublish();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringEntryImpl.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringEntryImpl.java
new file mode 100644
index 0000000..76c9b98
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringEntryImpl.java
@@ -0,0 +1,77 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables String implementation. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+final class StringEntryImpl extends EntryBase implements StringEntry {
+  /**
+   * Constructor.
+   *
+   * @param topic Topic
+   * @param handle Native handle
+   * @param defaultValue Default value for get()
+   */
+  StringEntryImpl(StringTopic topic, int handle, String defaultValue) {
+    super(handle);
+    m_topic = topic;
+    m_defaultValue = defaultValue;
+  }
+
+  @Override
+  public StringTopic getTopic() {
+    return m_topic;
+  }
+
+  @Override
+  public String get() {
+    return NetworkTablesJNI.getString(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public String get(String defaultValue) {
+    return NetworkTablesJNI.getString(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedString getAtomic() {
+    return NetworkTablesJNI.getAtomicString(m_handle, m_defaultValue);
+  }
+
+  @Override
+  public TimestampedString getAtomic(String defaultValue) {
+    return NetworkTablesJNI.getAtomicString(m_handle, defaultValue);
+  }
+
+  @Override
+  public TimestampedString[] readQueue() {
+    return NetworkTablesJNI.readQueueString(m_handle);
+  }
+
+  @Override
+  public String[] readQueueValues() {
+    return NetworkTablesJNI.readQueueValuesString(m_handle);
+  }
+
+  @Override
+  public void set(String value, long time) {
+    NetworkTablesJNI.setString(m_handle, time, value);
+  }
+
+  @Override
+  public void setDefault(String value) {
+    NetworkTablesJNI.setDefaultString(m_handle, 0, value);
+  }
+
+  @Override
+  public void unpublish() {
+    NetworkTablesJNI.unpublish(m_handle);
+  }
+
+  private final StringTopic m_topic;
+  private final String m_defaultValue;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringPublisher.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringPublisher.java
new file mode 100644
index 0000000..c11f11c
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringPublisher.java
@@ -0,0 +1,52 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Consumer;
+
+/** NetworkTables String publisher. */
+public interface StringPublisher extends Publisher, Consumer<String> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  StringTopic getTopic();
+
+  /**
+   * Publish a new value using current NT time.
+   *
+   * @param value value to publish
+   */
+  default void set(String value) {
+    set(value, 0);
+  }
+
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void set(String value, long time);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void setDefault(String value);
+
+  @Override
+  default void accept(String value) {
+    set(value);
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringSubscriber.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringSubscriber.java
new file mode 100644
index 0000000..8c453b0
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringSubscriber.java
@@ -0,0 +1,80 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+import java.util.function.Supplier;
+
+/** NetworkTables String subscriber. */
+@SuppressWarnings("PMD.MissingOverride")
+public interface StringSubscriber extends Subscriber, Supplier<String> {
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  @Override
+  StringTopic getTopic();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  String get();
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  String get(String defaultValue);
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedString getAtomic();
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedString getAtomic(String defaultValue);
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   * Also provides a timestamp for each value.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  TimestampedString[] readQueue();
+
+  /**
+   * Get an array of all value changes since the last call to readQueue.
+   *
+   * <p>The "poll storage" subscribe option can be used to set the queue
+   * depth.
+   *
+   * @return Array of values; empty array if no new changes have been
+   *     published since the previous call.
+   */
+  String[] readQueueValues();
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringTopic.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringTopic.java
new file mode 100644
index 0000000..a634180
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/StringTopic.java
@@ -0,0 +1,206 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables String topic. */
+public final class StringTopic extends Topic {
+  /** The default type string for this topic type. */
+  public static final String kTypeString = "string";
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  public StringTopic(Topic topic) {
+    super(topic.m_inst, topic.m_handle);
+  }
+
+  /**
+   * Constructor; use NetworkTableInstance.getStringTopic() instead.
+   *
+   * @param inst Instance
+   * @param handle Native handle
+   */
+  public StringTopic(NetworkTableInstance inst, int handle) {
+    super(inst, handle);
+  }
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public StringSubscriber subscribe(
+      String defaultValue,
+      PubSubOption... options) {
+    return new StringEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kString.getValue(),
+            "string", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new subscriber to the topic, with specified type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>Subscribers that do not match the published data type do not return
+   * any values. To determine if the data type matches, use the appropriate
+   * Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  public StringSubscriber subscribeEx(
+      String typeString,
+      String defaultValue,
+      PubSubOption... options) {
+    return new StringEntryImpl(
+        this,
+        NetworkTablesJNI.subscribe(
+            m_handle, NetworkTableType.kString.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  public StringPublisher publish(
+      PubSubOption... options) {
+    return new StringEntryImpl(
+        this,
+        NetworkTablesJNI.publish(
+            m_handle, NetworkTableType.kString.getValue(),
+            "string", options),
+        "");
+  }
+
+  /**
+   * Create a new publisher to the topic, with type string and initial properties.
+   *
+   * <p>The publisher is only active as long as the returned object
+   * is not closed.
+   *
+   * <p>It is not possible to publish two different data types to the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored). To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   * @throws IllegalArgumentException if properties is not a JSON object
+   */
+  public StringPublisher publishEx(
+      String typeString,
+      String properties,
+      PubSubOption... options) {
+    return new StringEntryImpl(
+        this,
+        NetworkTablesJNI.publishEx(
+            m_handle, NetworkTableType.kString.getValue(),
+            typeString, properties, options),
+        "");
+  }
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public StringEntry getEntry(
+      String defaultValue,
+      PubSubOption... options) {
+    return new StringEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kString.getValue(),
+            "string", options),
+        defaultValue);
+  }
+
+  /**
+   * Create a new entry for the topic, with specified type string.
+   *
+   * <p>Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not closed. The publisher is
+   * created when the entry is first written to, and remains active until either
+   * unpublish() is called or the entry is closed.
+   *
+   * <p>It is not possible to use two different data types with the same
+   * topic. Conflicts between publishers are typically resolved by the server on
+   * a first-come, first-served basis. Any published values that do not match
+   * the topic's data type are dropped (ignored), and the entry will show no new
+   * values if the data type does not match. To determine if the data type
+   * matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  public StringEntry getEntryEx(
+      String typeString,
+      String defaultValue,
+      PubSubOption... options) {
+    return new StringEntryImpl(
+        this,
+        NetworkTablesJNI.getEntry(
+            m_handle, NetworkTableType.kString.getValue(),
+            typeString, options),
+        defaultValue);
+  }
+
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedBoolean.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedBoolean.java
new file mode 100644
index 0000000..05497fc
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedBoolean.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped Boolean. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedBoolean {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedBoolean(long timestamp, long serverTime, boolean value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final boolean value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedBooleanArray.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedBooleanArray.java
new file mode 100644
index 0000000..51e2591
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedBooleanArray.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped BooleanArray. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedBooleanArray {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedBooleanArray(long timestamp, long serverTime, boolean[] value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final boolean[] value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedDouble.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedDouble.java
new file mode 100644
index 0000000..586c9fc
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedDouble.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped Double. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedDouble {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedDouble(long timestamp, long serverTime, double value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final double value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedDoubleArray.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedDoubleArray.java
new file mode 100644
index 0000000..aaeafd7
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedDoubleArray.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped DoubleArray. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedDoubleArray {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedDoubleArray(long timestamp, long serverTime, double[] value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final double[] value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedFloat.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedFloat.java
new file mode 100644
index 0000000..4d8aa09
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedFloat.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped Float. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedFloat {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedFloat(long timestamp, long serverTime, float value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final float value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedFloatArray.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedFloatArray.java
new file mode 100644
index 0000000..85fe7bc
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedFloatArray.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped FloatArray. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedFloatArray {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedFloatArray(long timestamp, long serverTime, float[] value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final float[] value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedInteger.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedInteger.java
new file mode 100644
index 0000000..0ee7855
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedInteger.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped Integer. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedInteger {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedInteger(long timestamp, long serverTime, long value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final long value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedIntegerArray.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedIntegerArray.java
new file mode 100644
index 0000000..8686326
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedIntegerArray.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped IntegerArray. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedIntegerArray {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedIntegerArray(long timestamp, long serverTime, long[] value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final long[] value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedRaw.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedRaw.java
new file mode 100644
index 0000000..12ec095
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedRaw.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped Raw. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedRaw {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedRaw(long timestamp, long serverTime, byte[] value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final byte[] value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedString.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedString.java
new file mode 100644
index 0000000..a51432e
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedString.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped String. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedString {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedString(long timestamp, long serverTime, String value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final String value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedStringArray.java b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedStringArray.java
new file mode 100644
index 0000000..420a468
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/java/edu/wpi/first/networktables/TimestampedStringArray.java
@@ -0,0 +1,42 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+package edu.wpi.first.networktables;
+
+/** NetworkTables timestamped StringArray. */
+@SuppressWarnings("PMD.ArrayIsStoredDirectly")
+public final class TimestampedStringArray {
+  /**
+   * Create a timestamped value.
+   *
+   * @param timestamp timestamp in local time base
+   * @param serverTime timestamp in server time base
+   * @param value value
+   */
+  public TimestampedStringArray(long timestamp, long serverTime, String[] value) {
+    this.timestamp = timestamp;
+    this.serverTime = serverTime;
+    this.value = value;
+  }
+
+  /**
+   * Timestamp in local time base.
+   */
+  @SuppressWarnings("MemberName")
+  public final long timestamp;
+
+  /**
+   * Timestamp in server time base.  May be 0 or 1 for locally set values.
+   */
+  @SuppressWarnings("MemberName")
+  public final long serverTime;
+
+  /**
+   * Value.
+   */
+  @SuppressWarnings("MemberName")
+  public final String[] value;
+}
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/cpp/jni/types_jni.cpp b/third_party/allwpilib/ntcore/src/generated/main/native/cpp/jni/types_jni.cpp
new file mode 100644
index 0000000..de14d53
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/cpp/jni/types_jni.cpp
@@ -0,0 +1,1436 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_networktables_NetworkTablesJNI.h"
+#include "ntcore.h"
+
+using namespace wpi::java;
+
+//
+// Globals and load/unload
+//
+
+static JClass timestampedBooleanCls;
+static JClass timestampedIntegerCls;
+static JClass timestampedFloatCls;
+static JClass timestampedDoubleCls;
+static JClass timestampedStringCls;
+static JClass timestampedRawCls;
+static JClass timestampedBooleanArrayCls;
+static JClass timestampedIntegerArrayCls;
+static JClass timestampedFloatArrayCls;
+static JClass timestampedDoubleArrayCls;
+static JClass timestampedStringArrayCls;
+static JClass jbyteArrayCls;
+static JClass jbooleanArrayCls;
+static JClass jlongArrayCls;
+static JClass jfloatArrayCls;
+static JClass jdoubleArrayCls;
+static JClass jobjectArrayCls;
+static JException illegalArgEx;
+static JException indexOobEx;
+static JException nullPointerEx;
+
+static const JClassInit classes[] = {
+    {"edu/wpi/first/networktables/TimestampedBoolean", &timestampedBooleanCls},
+    {"edu/wpi/first/networktables/TimestampedInteger", &timestampedIntegerCls},
+    {"edu/wpi/first/networktables/TimestampedFloat", &timestampedFloatCls},
+    {"edu/wpi/first/networktables/TimestampedDouble", &timestampedDoubleCls},
+    {"edu/wpi/first/networktables/TimestampedString", &timestampedStringCls},
+    {"edu/wpi/first/networktables/TimestampedRaw", &timestampedRawCls},
+    {"edu/wpi/first/networktables/TimestampedBooleanArray", &timestampedBooleanArrayCls},
+    {"edu/wpi/first/networktables/TimestampedIntegerArray", &timestampedIntegerArrayCls},
+    {"edu/wpi/first/networktables/TimestampedFloatArray", &timestampedFloatArrayCls},
+    {"edu/wpi/first/networktables/TimestampedDoubleArray", &timestampedDoubleArrayCls},
+    {"edu/wpi/first/networktables/TimestampedStringArray", &timestampedStringArrayCls},
+    {"[B", &jbyteArrayCls},
+    {"[Z", &jbooleanArrayCls},
+    {"[J", &jlongArrayCls},
+    {"[F", &jfloatArrayCls},
+    {"[D", &jdoubleArrayCls},
+    {"[Ljava/lang/Object;", &jobjectArrayCls},
+};
+
+static const JExceptionInit exceptions[] = {
+    {"java/lang/IllegalArgumentException", &illegalArgEx},
+    {"java/lang/IndexOutOfBoundsException", &indexOobEx},
+    {"java/lang/NullPointerException", &nullPointerEx},
+};
+
+namespace nt {
+
+bool JNI_LoadTypes(JNIEnv* env) {
+  // Cache references to classes
+  for (auto& c : classes) {
+    *c.cls = JClass(env, c.name);
+    if (!*c.cls) {
+      return false;
+    }
+  }
+
+  for (auto& c : exceptions) {
+    *c.cls = JException(env, c.name);
+    if (!*c.cls) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+void JNI_UnloadTypes(JNIEnv* env) {
+  // Delete global references
+  for (auto& c : classes) {
+    c.cls->free(env);
+  }
+  for (auto& c : exceptions) {
+    c.cls->free(env);
+  }
+}
+
+}  // namespace nt
+
+static std::vector<int> FromJavaBooleanArray(JNIEnv* env, jbooleanArray jarr) {
+  CriticalJSpan<const jboolean> ref{env, jarr};
+  if (!ref) {
+    return {};
+  }
+  std::span<const jboolean> elements{ref};
+  size_t len = elements.size();
+  std::vector<int> arr;
+  arr.reserve(len);
+  for (size_t i = 0; i < len; ++i) {
+    arr.push_back(elements[i]);
+  }
+  return arr;
+}
+
+static std::vector<std::string> FromJavaStringArray(JNIEnv* env, jobjectArray jarr) {
+  size_t len = env->GetArrayLength(jarr);
+  std::vector<std::string> arr;
+  arr.reserve(len);
+  for (size_t i = 0; i < len; ++i) {
+    JLocal<jstring> elem{
+        env, static_cast<jstring>(env->GetObjectArrayElement(jarr, i))};
+    if (!elem) {
+      return {};
+    }
+    arr.emplace_back(JStringRef{env, elem}.str());
+  }
+  return arr;
+}
+
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedBoolean value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedBooleanCls, "<init>", "(JJZ)V");
+  return env->NewObject(timestampedBooleanCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime),
+                        static_cast<jboolean>(value.value));
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedInteger value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedIntegerCls, "<init>", "(JJJ)V");
+  return env->NewObject(timestampedIntegerCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime),
+                        static_cast<jlong>(value.value));
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedFloat value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedFloatCls, "<init>", "(JJF)V");
+  return env->NewObject(timestampedFloatCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime),
+                        static_cast<jfloat>(value.value));
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedDouble value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedDoubleCls, "<init>", "(JJD)V");
+  return env->NewObject(timestampedDoubleCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime),
+                        static_cast<jdouble>(value.value));
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedString value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedStringCls, "<init>", "(JJLjava/lang/String;)V");
+  JLocal<jstring> val{env, MakeJString(env, value.value)};
+  return env->NewObject(timestampedStringCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedRaw value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedRawCls, "<init>", "(JJ[B)V");
+  JLocal<jbyteArray> val{env, MakeJByteArray(env, value.value)};
+  return env->NewObject(timestampedRawCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedBooleanArray value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedBooleanArrayCls, "<init>", "(JJ[Z)V");
+  JLocal<jbooleanArray> val{env, MakeJBooleanArray(env, value.value)};
+  return env->NewObject(timestampedBooleanArrayCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedIntegerArray value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedIntegerArrayCls, "<init>", "(JJ[J)V");
+  JLocal<jlongArray> val{env, MakeJLongArray(env, value.value)};
+  return env->NewObject(timestampedIntegerArrayCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedFloatArray value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedFloatArrayCls, "<init>", "(JJ[F)V");
+  JLocal<jfloatArray> val{env, MakeJFloatArray(env, value.value)};
+  return env->NewObject(timestampedFloatArrayCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedDoubleArray value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedDoubleArrayCls, "<init>", "(JJ[D)V");
+  JLocal<jdoubleArray> val{env, MakeJDoubleArray(env, value.value)};
+  return env->NewObject(timestampedDoubleArrayCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, nt::TimestampedStringArray value) {
+  static jmethodID constructor = env->GetMethodID(
+      timestampedStringArrayCls, "<init>", "(JJ[Ljava/lang/Object;)V");
+  JLocal<jobjectArray> val{env, MakeJStringArray(env, value.value)};
+  return env->NewObject(timestampedStringArrayCls, constructor,
+                        static_cast<jlong>(value.time),
+                        static_cast<jlong>(value.serverTime), val.obj());
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedBoolean> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedBooleanCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedInteger> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedIntegerCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedFloat> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedFloatCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedDouble> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedDoubleCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedString> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedStringCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedRaw> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedRawCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedBooleanArray> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedBooleanArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedIntegerArray> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedIntegerArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedFloatArray> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedFloatArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedDoubleArray> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedDoubleArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const nt::TimestampedStringArray> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), timestampedStringArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const std::vector<uint8_t>> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), jbyteArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJByteArray(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const std::vector<int>> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), jbooleanArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJBooleanArray(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const std::vector<int64_t>> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), jlongArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJLongArray(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const std::vector<float>> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), jfloatArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJFloatArray(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const std::vector<double>> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), jdoubleArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJDoubleArray(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobjectArray MakeJObjectArray(JNIEnv* env, std::span<const std::vector<std::string>> arr) {
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), jobjectArrayCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJStringArray(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicBoolean
+ * Signature: (IZ)Ledu/wpi/first/networktables/TimestampedBoolean;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicBoolean
+  (JNIEnv* env, jclass, jint subentry, jboolean defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicBoolean(subentry, defaultValue != JNI_FALSE));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueBoolean
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedBoolean;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueBoolean
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueBoolean(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesBoolean
+ * Signature: (I)[Z
+ */
+JNIEXPORT jbooleanArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesBoolean
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJBooleanArray(env, nt::ReadQueueValuesBoolean(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setBoolean
+ * Signature: (IJZ)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setBoolean
+  (JNIEnv*, jclass, jint entry, jlong time, jboolean value)
+{
+  return nt::SetBoolean(entry, value != JNI_FALSE, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getBoolean
+ * Signature: (IZ)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getBoolean
+  (JNIEnv*, jclass, jint entry, jboolean defaultValue)
+{
+  return nt::GetBoolean(entry, defaultValue);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultBoolean
+ * Signature: (IJZ)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultBoolean
+  (JNIEnv*, jclass, jint entry, jlong, jboolean defaultValue)
+{
+  return nt::SetDefaultBoolean(entry, defaultValue != JNI_FALSE);
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicInteger
+ * Signature: (IJ)Ledu/wpi/first/networktables/TimestampedInteger;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicInteger
+  (JNIEnv* env, jclass, jint subentry, jlong defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicInteger(subentry, defaultValue));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueInteger
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedInteger;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueInteger
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueInteger(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesInteger
+ * Signature: (I)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesInteger
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJLongArray(env, nt::ReadQueueValuesInteger(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setInteger
+ * Signature: (IJJ)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setInteger
+  (JNIEnv*, jclass, jint entry, jlong time, jlong value)
+{
+  return nt::SetInteger(entry, value, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getInteger
+ * Signature: (IJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getInteger
+  (JNIEnv*, jclass, jint entry, jlong defaultValue)
+{
+  return nt::GetInteger(entry, defaultValue);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultInteger
+ * Signature: (IJJ)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultInteger
+  (JNIEnv*, jclass, jint entry, jlong, jlong defaultValue)
+{
+  return nt::SetDefaultInteger(entry, defaultValue);
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicFloat
+ * Signature: (IF)Ledu/wpi/first/networktables/TimestampedFloat;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicFloat
+  (JNIEnv* env, jclass, jint subentry, jfloat defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicFloat(subentry, defaultValue));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueFloat
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedFloat;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueFloat
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueFloat(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesFloat
+ * Signature: (I)[F
+ */
+JNIEXPORT jfloatArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesFloat
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJFloatArray(env, nt::ReadQueueValuesFloat(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setFloat
+ * Signature: (IJF)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setFloat
+  (JNIEnv*, jclass, jint entry, jlong time, jfloat value)
+{
+  return nt::SetFloat(entry, value, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getFloat
+ * Signature: (IF)F
+ */
+JNIEXPORT jfloat JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getFloat
+  (JNIEnv*, jclass, jint entry, jfloat defaultValue)
+{
+  return nt::GetFloat(entry, defaultValue);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultFloat
+ * Signature: (IJF)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultFloat
+  (JNIEnv*, jclass, jint entry, jlong, jfloat defaultValue)
+{
+  return nt::SetDefaultFloat(entry, defaultValue);
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicDouble
+ * Signature: (ID)Ledu/wpi/first/networktables/TimestampedDouble;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicDouble
+  (JNIEnv* env, jclass, jint subentry, jdouble defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicDouble(subentry, defaultValue));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueDouble
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedDouble;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueDouble
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueDouble(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesDouble
+ * Signature: (I)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesDouble
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJDoubleArray(env, nt::ReadQueueValuesDouble(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDouble
+ * Signature: (IJD)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDouble
+  (JNIEnv*, jclass, jint entry, jlong time, jdouble value)
+{
+  return nt::SetDouble(entry, value, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getDouble
+ * Signature: (ID)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getDouble
+  (JNIEnv*, jclass, jint entry, jdouble defaultValue)
+{
+  return nt::GetDouble(entry, defaultValue);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultDouble
+ * Signature: (IJD)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultDouble
+  (JNIEnv*, jclass, jint entry, jlong, jdouble defaultValue)
+{
+  return nt::SetDefaultDouble(entry, defaultValue);
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicString
+ * Signature: (ILjava/lang/String;)Ledu/wpi/first/networktables/TimestampedString;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicString
+  (JNIEnv* env, jclass, jint subentry, jstring defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicString(subentry, JStringRef{env, defaultValue}));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueString
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedString;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueString
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueString(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesString
+ * Signature: (I)[Ljava/lang/String;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesString
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJStringArray(env, nt::ReadQueueValuesString(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setString
+ * Signature: (IJLjava/lang/String;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setString
+  (JNIEnv* env, jclass, jint entry, jlong time, jstring value)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value cannot be null");
+    return false;
+  }
+  return nt::SetString(entry, JStringRef{env, value}, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getString
+ * Signature: (ILjava/lang/String;)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getString
+  (JNIEnv* env, jclass, jint entry, jstring defaultValue)
+{
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.IsString()) {
+    return defaultValue;
+  }
+  return MakeJString(env, val.GetString());
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultString
+ * Signature: (IJLjava/lang/String;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultString
+  (JNIEnv* env, jclass, jint entry, jlong, jstring defaultValue)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "defaultValue cannot be null");
+    return false;
+  }
+  return nt::SetDefaultString(entry, JStringRef{env, defaultValue});
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicRaw
+ * Signature: (I[B)Ledu/wpi/first/networktables/TimestampedRaw;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicRaw
+  (JNIEnv* env, jclass, jint subentry, jbyteArray defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicRaw(subentry, CriticalJSpan<const jbyte>{env, defaultValue}.uarray()));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueRaw
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedRaw;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueRaw
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueRaw(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesRaw
+ * Signature: (I)[[B
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesRaw
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObjectArray(env, nt::ReadQueueValuesRaw(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setRaw
+ * Signature: (IJ[BII)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setRaw
+  (JNIEnv* env, jclass, jint entry, jlong time, jbyteArray value, jint start, jint len)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  CriticalJSpan<const jbyte> cvalue{env, value};
+  if (static_cast<unsigned int>(start + len) > cvalue.size()) {
+    indexOobEx.Throw(env, "start + len must be smaller than array length");
+    return false;
+  }
+  return nt::SetRaw(entry, cvalue.uarray().subspan(start, len), time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setRawBuffer
+ * Signature: (IJLjava/nio/ByteBuffer;II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setRawBuffer
+  (JNIEnv* env, jclass, jint entry, jlong time, jobject value, jint start, jint len)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  JSpan<const jbyte> cvalue{env, value, static_cast<size_t>(start + len)};
+  if (!cvalue) {
+    illegalArgEx.Throw(env, "value must be a native ByteBuffer");
+    return false;
+  }
+  return nt::SetRaw(entry, cvalue.uarray().subspan(start, len), time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getRaw
+ * Signature: (I[B)[B
+ */
+JNIEXPORT jbyteArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getRaw
+  (JNIEnv* env, jclass, jint entry, jbyteArray defaultValue)
+{
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.IsRaw()) {
+    return defaultValue;
+  }
+  return MakeJByteArray(env, val.GetRaw());
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultRaw
+ * Signature: (IJ[BII)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultRaw
+  (JNIEnv* env, jclass, jint entry, jlong, jbyteArray defaultValue, jint start, jint len)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  CriticalJSpan<const jbyte> cvalue{env, defaultValue};
+  if (static_cast<unsigned int>(start + len) > cvalue.size()) {
+    indexOobEx.Throw(env, "start + len must be smaller than array length");
+    return false;
+  }
+  return nt::SetDefaultRaw(entry, cvalue.uarray().subspan(start, len));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultRawBuffer
+ * Signature: (IJLjava/nio/ByteBuffer;II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultRawBuffer
+  (JNIEnv* env, jclass, jint entry, jlong, jobject defaultValue, jint start, jint len)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "value is null");
+    return false;
+  }
+  if (start < 0) {
+    indexOobEx.Throw(env, "start must be >= 0");
+    return false;
+  }
+  if (len < 0) {
+    indexOobEx.Throw(env, "len must be >= 0");
+    return false;
+  }
+  JSpan<const jbyte> cvalue{env, defaultValue, static_cast<size_t>(start + len)};
+  if (!cvalue) {
+    illegalArgEx.Throw(env, "value must be a native ByteBuffer");
+    return false;
+  }
+  return nt::SetDefaultRaw(entry, cvalue.uarray().subspan(start, len));
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicBooleanArray
+ * Signature: (I[Z)Ledu/wpi/first/networktables/TimestampedBooleanArray;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicBooleanArray
+  (JNIEnv* env, jclass, jint subentry, jbooleanArray defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicBooleanArray(subentry, FromJavaBooleanArray(env, defaultValue)));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueBooleanArray
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedBooleanArray;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueBooleanArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueBooleanArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesBooleanArray
+ * Signature: (I)[[Z
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesBooleanArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObjectArray(env, nt::ReadQueueValuesBooleanArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setBooleanArray
+ * Signature: (IJ[Z)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setBooleanArray
+  (JNIEnv* env, jclass, jint entry, jlong time, jbooleanArray value)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value cannot be null");
+    return false;
+  }
+  return nt::SetBooleanArray(entry, FromJavaBooleanArray(env, value), time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getBooleanArray
+ * Signature: (I[Z)[Z
+ */
+JNIEXPORT jbooleanArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getBooleanArray
+  (JNIEnv* env, jclass, jint entry, jbooleanArray defaultValue)
+{
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.IsBooleanArray()) {
+    return defaultValue;
+  }
+  return MakeJBooleanArray(env, val.GetBooleanArray());
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultBooleanArray
+ * Signature: (IJ[Z)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultBooleanArray
+  (JNIEnv* env, jclass, jint entry, jlong, jbooleanArray defaultValue)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "defaultValue cannot be null");
+    return false;
+  }
+  return nt::SetDefaultBooleanArray(entry, FromJavaBooleanArray(env, defaultValue));
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicIntegerArray
+ * Signature: (I[J)Ledu/wpi/first/networktables/TimestampedIntegerArray;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicIntegerArray
+  (JNIEnv* env, jclass, jint subentry, jlongArray defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicIntegerArray(subentry, CriticalJSpan<const jlong>{env, defaultValue}));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueIntegerArray
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedIntegerArray;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueIntegerArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueIntegerArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesIntegerArray
+ * Signature: (I)[[J
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesIntegerArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObjectArray(env, nt::ReadQueueValuesIntegerArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setIntegerArray
+ * Signature: (IJ[J)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setIntegerArray
+  (JNIEnv* env, jclass, jint entry, jlong time, jlongArray value)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value cannot be null");
+    return false;
+  }
+  return nt::SetIntegerArray(entry, CriticalJSpan<const jlong>{env, value}, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getIntegerArray
+ * Signature: (I[J)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getIntegerArray
+  (JNIEnv* env, jclass, jint entry, jlongArray defaultValue)
+{
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.IsIntegerArray()) {
+    return defaultValue;
+  }
+  return MakeJLongArray(env, val.GetIntegerArray());
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultIntegerArray
+ * Signature: (IJ[J)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultIntegerArray
+  (JNIEnv* env, jclass, jint entry, jlong, jlongArray defaultValue)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "defaultValue cannot be null");
+    return false;
+  }
+  return nt::SetDefaultIntegerArray(entry, CriticalJSpan<const jlong>{env, defaultValue});
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicFloatArray
+ * Signature: (I[F)Ledu/wpi/first/networktables/TimestampedFloatArray;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicFloatArray
+  (JNIEnv* env, jclass, jint subentry, jfloatArray defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicFloatArray(subentry, CriticalJSpan<const jfloat>{env, defaultValue}));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueFloatArray
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedFloatArray;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueFloatArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueFloatArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesFloatArray
+ * Signature: (I)[[F
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesFloatArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObjectArray(env, nt::ReadQueueValuesFloatArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setFloatArray
+ * Signature: (IJ[F)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setFloatArray
+  (JNIEnv* env, jclass, jint entry, jlong time, jfloatArray value)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value cannot be null");
+    return false;
+  }
+  return nt::SetFloatArray(entry, CriticalJSpan<const jfloat>{env, value}, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getFloatArray
+ * Signature: (I[F)[F
+ */
+JNIEXPORT jfloatArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getFloatArray
+  (JNIEnv* env, jclass, jint entry, jfloatArray defaultValue)
+{
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.IsFloatArray()) {
+    return defaultValue;
+  }
+  return MakeJFloatArray(env, val.GetFloatArray());
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultFloatArray
+ * Signature: (IJ[F)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultFloatArray
+  (JNIEnv* env, jclass, jint entry, jlong, jfloatArray defaultValue)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "defaultValue cannot be null");
+    return false;
+  }
+  return nt::SetDefaultFloatArray(entry, CriticalJSpan<const jfloat>{env, defaultValue});
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicDoubleArray
+ * Signature: (I[D)Ledu/wpi/first/networktables/TimestampedDoubleArray;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicDoubleArray
+  (JNIEnv* env, jclass, jint subentry, jdoubleArray defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicDoubleArray(subentry, CriticalJSpan<const jdouble>{env, defaultValue}));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueDoubleArray
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedDoubleArray;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueDoubleArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueDoubleArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesDoubleArray
+ * Signature: (I)[[D
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesDoubleArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObjectArray(env, nt::ReadQueueValuesDoubleArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDoubleArray
+ * Signature: (IJ[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDoubleArray
+  (JNIEnv* env, jclass, jint entry, jlong time, jdoubleArray value)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value cannot be null");
+    return false;
+  }
+  return nt::SetDoubleArray(entry, CriticalJSpan<const jdouble>{env, value}, time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getDoubleArray
+ * Signature: (I[D)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getDoubleArray
+  (JNIEnv* env, jclass, jint entry, jdoubleArray defaultValue)
+{
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.IsDoubleArray()) {
+    return defaultValue;
+  }
+  return MakeJDoubleArray(env, val.GetDoubleArray());
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultDoubleArray
+ * Signature: (IJ[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultDoubleArray
+  (JNIEnv* env, jclass, jint entry, jlong, jdoubleArray defaultValue)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "defaultValue cannot be null");
+    return false;
+  }
+  return nt::SetDefaultDoubleArray(entry, CriticalJSpan<const jdouble>{env, defaultValue});
+}
+
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getAtomicStringArray
+ * Signature: (I[Ljava/lang/Object;)Ledu/wpi/first/networktables/TimestampedStringArray;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getAtomicStringArray
+  (JNIEnv* env, jclass, jint subentry, jobjectArray defaultValue)
+{
+  return MakeJObject(env, nt::GetAtomicStringArray(subentry, FromJavaStringArray(env, defaultValue)));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueStringArray
+ * Signature: (I)[Ledu/wpi/first/networktables/TimestampedStringArray;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueStringArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObject(env, nt::ReadQueueStringArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    readQueueValuesStringArray
+ * Signature: (I)[[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_readQueueValuesStringArray
+  (JNIEnv* env, jclass, jint subentry)
+{
+  return MakeJObjectArray(env, nt::ReadQueueValuesStringArray(subentry));
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setStringArray
+ * Signature: (IJ[Ljava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setStringArray
+  (JNIEnv* env, jclass, jint entry, jlong time, jobjectArray value)
+{
+  if (!value) {
+    nullPointerEx.Throw(env, "value cannot be null");
+    return false;
+  }
+  return nt::SetStringArray(entry, FromJavaStringArray(env, value), time);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getStringArray
+ * Signature: (I[Ljava/lang/Object;)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getStringArray
+  (JNIEnv* env, jclass, jint entry, jobjectArray defaultValue)
+{
+  auto val = nt::GetEntryValue(entry);
+  if (!val || !val.IsStringArray()) {
+    return defaultValue;
+  }
+  return MakeJStringArray(env, val.GetStringArray());
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setDefaultStringArray
+ * Signature: (IJ[Ljava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setDefaultStringArray
+  (JNIEnv* env, jclass, jint entry, jlong, jobjectArray defaultValue)
+{
+  if (!defaultValue) {
+    nullPointerEx.Throw(env, "defaultValue cannot be null");
+    return false;
+  }
+  return nt::SetDefaultStringArray(entry, FromJavaStringArray(env, defaultValue));
+}
+
+
+}  // extern "C"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/cpp/ntcore_c_types.cpp b/third_party/allwpilib/ntcore/src/generated/main/native/cpp/ntcore_c_types.cpp
new file mode 100644
index 0000000..ca14c7d
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/cpp/ntcore_c_types.cpp
@@ -0,0 +1,495 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#include "ntcore_c_types.h"
+
+#include "Value_internal.h"
+#include "ntcore_cpp.h"
+
+using namespace nt;
+
+template <typename T>
+static inline std::span<const T> ConvertFromC(const T* arr, size_t size) {
+  return {arr, size};
+}
+
+static inline std::string_view ConvertFromC(const char* arr, size_t size) {
+  return {arr, size};
+}
+
+static std::vector<std::string> ConvertFromC(const NT_String* arr, size_t size) {
+  std::vector<std::string> v;
+  v.reserve(size);
+  for (size_t i = 0; i < size; ++i) {
+    v.emplace_back(ConvertFromC(arr[i]));
+  }
+  return v;
+}
+
+
+static void ConvertToC(const nt::TimestampedBoolean& in, NT_TimestampedBoolean* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = in.value;
+}
+
+static void ConvertToC(const nt::TimestampedInteger& in, NT_TimestampedInteger* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = in.value;
+}
+
+static void ConvertToC(const nt::TimestampedFloat& in, NT_TimestampedFloat* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = in.value;
+}
+
+static void ConvertToC(const nt::TimestampedDouble& in, NT_TimestampedDouble* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = in.value;
+}
+
+static void ConvertToC(const nt::TimestampedString& in, NT_TimestampedString* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = ConvertToC<char>(in.value, &out->len);
+}
+
+static void ConvertToC(const nt::TimestampedRaw& in, NT_TimestampedRaw* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = ConvertToC<uint8_t>(in.value, &out->len);
+}
+
+static void ConvertToC(const nt::TimestampedBooleanArray& in, NT_TimestampedBooleanArray* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = ConvertToC<NT_Bool>(in.value, &out->len);
+}
+
+static void ConvertToC(const nt::TimestampedIntegerArray& in, NT_TimestampedIntegerArray* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = ConvertToC<int64_t>(in.value, &out->len);
+}
+
+static void ConvertToC(const nt::TimestampedFloatArray& in, NT_TimestampedFloatArray* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = ConvertToC<float>(in.value, &out->len);
+}
+
+static void ConvertToC(const nt::TimestampedDoubleArray& in, NT_TimestampedDoubleArray* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = ConvertToC<double>(in.value, &out->len);
+}
+
+static void ConvertToC(const nt::TimestampedStringArray& in, NT_TimestampedStringArray* out) {
+  out->time = in.time;
+  out->serverTime = in.serverTime;
+  out->value = ConvertToC<struct NT_String>(in.value, &out->len);
+}
+
+
+extern "C" {
+
+NT_Bool NT_SetBoolean(NT_Handle pubentry, int64_t time, NT_Bool value) {
+  return nt::SetBoolean(pubentry, value, time);
+}
+
+NT_Bool NT_SetDefaultBoolean(NT_Handle pubentry, NT_Bool defaultValue) {
+  return nt::SetDefaultBoolean(pubentry, defaultValue);
+}
+
+NT_Bool NT_GetBoolean(NT_Handle subentry, NT_Bool defaultValue) {
+  return nt::GetBoolean(subentry, defaultValue);
+}
+
+void NT_GetAtomicBoolean(NT_Handle subentry, NT_Bool defaultValue, struct NT_TimestampedBoolean* value) {
+  auto cppValue = nt::GetAtomicBoolean(subentry, defaultValue);
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedBoolean(struct NT_TimestampedBoolean* value) {
+}
+
+struct NT_TimestampedBoolean* NT_ReadQueueBoolean(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueBoolean(subentry);
+  return ConvertToC<NT_TimestampedBoolean>(arr, len);
+}
+
+void NT_FreeQueueBoolean(struct NT_TimestampedBoolean* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedBoolean(&arr[i]);
+  }
+  std::free(arr);
+}
+NT_Bool* NT_ReadQueueValuesBoolean(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueValuesBoolean(subentry);
+  return ConvertToC<NT_Bool>(arr, len);
+}
+
+
+NT_Bool NT_SetInteger(NT_Handle pubentry, int64_t time, int64_t value) {
+  return nt::SetInteger(pubentry, value, time);
+}
+
+NT_Bool NT_SetDefaultInteger(NT_Handle pubentry, int64_t defaultValue) {
+  return nt::SetDefaultInteger(pubentry, defaultValue);
+}
+
+int64_t NT_GetInteger(NT_Handle subentry, int64_t defaultValue) {
+  return nt::GetInteger(subentry, defaultValue);
+}
+
+void NT_GetAtomicInteger(NT_Handle subentry, int64_t defaultValue, struct NT_TimestampedInteger* value) {
+  auto cppValue = nt::GetAtomicInteger(subentry, defaultValue);
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedInteger(struct NT_TimestampedInteger* value) {
+}
+
+struct NT_TimestampedInteger* NT_ReadQueueInteger(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueInteger(subentry);
+  return ConvertToC<NT_TimestampedInteger>(arr, len);
+}
+
+void NT_FreeQueueInteger(struct NT_TimestampedInteger* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedInteger(&arr[i]);
+  }
+  std::free(arr);
+}
+int64_t* NT_ReadQueueValuesInteger(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueValuesInteger(subentry);
+  return ConvertToC<int64_t>(arr, len);
+}
+
+
+NT_Bool NT_SetFloat(NT_Handle pubentry, int64_t time, float value) {
+  return nt::SetFloat(pubentry, value, time);
+}
+
+NT_Bool NT_SetDefaultFloat(NT_Handle pubentry, float defaultValue) {
+  return nt::SetDefaultFloat(pubentry, defaultValue);
+}
+
+float NT_GetFloat(NT_Handle subentry, float defaultValue) {
+  return nt::GetFloat(subentry, defaultValue);
+}
+
+void NT_GetAtomicFloat(NT_Handle subentry, float defaultValue, struct NT_TimestampedFloat* value) {
+  auto cppValue = nt::GetAtomicFloat(subentry, defaultValue);
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedFloat(struct NT_TimestampedFloat* value) {
+}
+
+struct NT_TimestampedFloat* NT_ReadQueueFloat(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueFloat(subentry);
+  return ConvertToC<NT_TimestampedFloat>(arr, len);
+}
+
+void NT_FreeQueueFloat(struct NT_TimestampedFloat* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedFloat(&arr[i]);
+  }
+  std::free(arr);
+}
+float* NT_ReadQueueValuesFloat(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueValuesFloat(subentry);
+  return ConvertToC<float>(arr, len);
+}
+
+
+NT_Bool NT_SetDouble(NT_Handle pubentry, int64_t time, double value) {
+  return nt::SetDouble(pubentry, value, time);
+}
+
+NT_Bool NT_SetDefaultDouble(NT_Handle pubentry, double defaultValue) {
+  return nt::SetDefaultDouble(pubentry, defaultValue);
+}
+
+double NT_GetDouble(NT_Handle subentry, double defaultValue) {
+  return nt::GetDouble(subentry, defaultValue);
+}
+
+void NT_GetAtomicDouble(NT_Handle subentry, double defaultValue, struct NT_TimestampedDouble* value) {
+  auto cppValue = nt::GetAtomicDouble(subentry, defaultValue);
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedDouble(struct NT_TimestampedDouble* value) {
+}
+
+struct NT_TimestampedDouble* NT_ReadQueueDouble(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueDouble(subentry);
+  return ConvertToC<NT_TimestampedDouble>(arr, len);
+}
+
+void NT_FreeQueueDouble(struct NT_TimestampedDouble* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedDouble(&arr[i]);
+  }
+  std::free(arr);
+}
+double* NT_ReadQueueValuesDouble(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueValuesDouble(subentry);
+  return ConvertToC<double>(arr, len);
+}
+
+
+NT_Bool NT_SetString(NT_Handle pubentry, int64_t time, const char* value, size_t len) {
+  return nt::SetString(pubentry, ConvertFromC(value, len), time);
+}
+
+NT_Bool NT_SetDefaultString(NT_Handle pubentry, const char* defaultValue, size_t defaultValueLen) {
+  return nt::SetDefaultString(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+}
+
+char* NT_GetString(NT_Handle subentry, const char* defaultValue, size_t defaultValueLen, size_t* len) {
+  auto cppValue = nt::GetString(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<char>(cppValue, len);
+}
+
+void NT_GetAtomicString(NT_Handle subentry, const char* defaultValue, size_t defaultValueLen, struct NT_TimestampedString* value) {
+  auto cppValue = nt::GetAtomicString(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedString(struct NT_TimestampedString* value) {
+  std::free(value->value);
+}
+
+struct NT_TimestampedString* NT_ReadQueueString(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueString(subentry);
+  return ConvertToC<NT_TimestampedString>(arr, len);
+}
+
+void NT_FreeQueueString(struct NT_TimestampedString* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedString(&arr[i]);
+  }
+  std::free(arr);
+}
+
+
+NT_Bool NT_SetRaw(NT_Handle pubentry, int64_t time, const uint8_t* value, size_t len) {
+  return nt::SetRaw(pubentry, ConvertFromC(value, len), time);
+}
+
+NT_Bool NT_SetDefaultRaw(NT_Handle pubentry, const uint8_t* defaultValue, size_t defaultValueLen) {
+  return nt::SetDefaultRaw(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+}
+
+uint8_t* NT_GetRaw(NT_Handle subentry, const uint8_t* defaultValue, size_t defaultValueLen, size_t* len) {
+  auto cppValue = nt::GetRaw(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<uint8_t>(cppValue, len);
+}
+
+void NT_GetAtomicRaw(NT_Handle subentry, const uint8_t* defaultValue, size_t defaultValueLen, struct NT_TimestampedRaw* value) {
+  auto cppValue = nt::GetAtomicRaw(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedRaw(struct NT_TimestampedRaw* value) {
+  std::free(value->value);
+}
+
+struct NT_TimestampedRaw* NT_ReadQueueRaw(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueRaw(subentry);
+  return ConvertToC<NT_TimestampedRaw>(arr, len);
+}
+
+void NT_FreeQueueRaw(struct NT_TimestampedRaw* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedRaw(&arr[i]);
+  }
+  std::free(arr);
+}
+
+
+NT_Bool NT_SetBooleanArray(NT_Handle pubentry, int64_t time, const NT_Bool* value, size_t len) {
+  return nt::SetBooleanArray(pubentry, ConvertFromC(value, len), time);
+}
+
+NT_Bool NT_SetDefaultBooleanArray(NT_Handle pubentry, const NT_Bool* defaultValue, size_t defaultValueLen) {
+  return nt::SetDefaultBooleanArray(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+}
+
+NT_Bool* NT_GetBooleanArray(NT_Handle subentry, const NT_Bool* defaultValue, size_t defaultValueLen, size_t* len) {
+  auto cppValue = nt::GetBooleanArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<NT_Bool>(cppValue, len);
+}
+
+void NT_GetAtomicBooleanArray(NT_Handle subentry, const NT_Bool* defaultValue, size_t defaultValueLen, struct NT_TimestampedBooleanArray* value) {
+  auto cppValue = nt::GetAtomicBooleanArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedBooleanArray(struct NT_TimestampedBooleanArray* value) {
+  std::free(value->value);
+}
+
+struct NT_TimestampedBooleanArray* NT_ReadQueueBooleanArray(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueBooleanArray(subentry);
+  return ConvertToC<NT_TimestampedBooleanArray>(arr, len);
+}
+
+void NT_FreeQueueBooleanArray(struct NT_TimestampedBooleanArray* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedBooleanArray(&arr[i]);
+  }
+  std::free(arr);
+}
+
+
+NT_Bool NT_SetIntegerArray(NT_Handle pubentry, int64_t time, const int64_t* value, size_t len) {
+  return nt::SetIntegerArray(pubentry, ConvertFromC(value, len), time);
+}
+
+NT_Bool NT_SetDefaultIntegerArray(NT_Handle pubentry, const int64_t* defaultValue, size_t defaultValueLen) {
+  return nt::SetDefaultIntegerArray(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+}
+
+int64_t* NT_GetIntegerArray(NT_Handle subentry, const int64_t* defaultValue, size_t defaultValueLen, size_t* len) {
+  auto cppValue = nt::GetIntegerArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<int64_t>(cppValue, len);
+}
+
+void NT_GetAtomicIntegerArray(NT_Handle subentry, const int64_t* defaultValue, size_t defaultValueLen, struct NT_TimestampedIntegerArray* value) {
+  auto cppValue = nt::GetAtomicIntegerArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedIntegerArray(struct NT_TimestampedIntegerArray* value) {
+  std::free(value->value);
+}
+
+struct NT_TimestampedIntegerArray* NT_ReadQueueIntegerArray(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueIntegerArray(subentry);
+  return ConvertToC<NT_TimestampedIntegerArray>(arr, len);
+}
+
+void NT_FreeQueueIntegerArray(struct NT_TimestampedIntegerArray* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedIntegerArray(&arr[i]);
+  }
+  std::free(arr);
+}
+
+
+NT_Bool NT_SetFloatArray(NT_Handle pubentry, int64_t time, const float* value, size_t len) {
+  return nt::SetFloatArray(pubentry, ConvertFromC(value, len), time);
+}
+
+NT_Bool NT_SetDefaultFloatArray(NT_Handle pubentry, const float* defaultValue, size_t defaultValueLen) {
+  return nt::SetDefaultFloatArray(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+}
+
+float* NT_GetFloatArray(NT_Handle subentry, const float* defaultValue, size_t defaultValueLen, size_t* len) {
+  auto cppValue = nt::GetFloatArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<float>(cppValue, len);
+}
+
+void NT_GetAtomicFloatArray(NT_Handle subentry, const float* defaultValue, size_t defaultValueLen, struct NT_TimestampedFloatArray* value) {
+  auto cppValue = nt::GetAtomicFloatArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedFloatArray(struct NT_TimestampedFloatArray* value) {
+  std::free(value->value);
+}
+
+struct NT_TimestampedFloatArray* NT_ReadQueueFloatArray(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueFloatArray(subentry);
+  return ConvertToC<NT_TimestampedFloatArray>(arr, len);
+}
+
+void NT_FreeQueueFloatArray(struct NT_TimestampedFloatArray* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedFloatArray(&arr[i]);
+  }
+  std::free(arr);
+}
+
+
+NT_Bool NT_SetDoubleArray(NT_Handle pubentry, int64_t time, const double* value, size_t len) {
+  return nt::SetDoubleArray(pubentry, ConvertFromC(value, len), time);
+}
+
+NT_Bool NT_SetDefaultDoubleArray(NT_Handle pubentry, const double* defaultValue, size_t defaultValueLen) {
+  return nt::SetDefaultDoubleArray(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+}
+
+double* NT_GetDoubleArray(NT_Handle subentry, const double* defaultValue, size_t defaultValueLen, size_t* len) {
+  auto cppValue = nt::GetDoubleArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<double>(cppValue, len);
+}
+
+void NT_GetAtomicDoubleArray(NT_Handle subentry, const double* defaultValue, size_t defaultValueLen, struct NT_TimestampedDoubleArray* value) {
+  auto cppValue = nt::GetAtomicDoubleArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedDoubleArray(struct NT_TimestampedDoubleArray* value) {
+  std::free(value->value);
+}
+
+struct NT_TimestampedDoubleArray* NT_ReadQueueDoubleArray(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueDoubleArray(subentry);
+  return ConvertToC<NT_TimestampedDoubleArray>(arr, len);
+}
+
+void NT_FreeQueueDoubleArray(struct NT_TimestampedDoubleArray* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedDoubleArray(&arr[i]);
+  }
+  std::free(arr);
+}
+
+
+NT_Bool NT_SetStringArray(NT_Handle pubentry, int64_t time, const struct NT_String* value, size_t len) {
+  return nt::SetStringArray(pubentry, ConvertFromC(value, len), time);
+}
+
+NT_Bool NT_SetDefaultStringArray(NT_Handle pubentry, const struct NT_String* defaultValue, size_t defaultValueLen) {
+  return nt::SetDefaultStringArray(pubentry, ConvertFromC(defaultValue, defaultValueLen));
+}
+
+struct NT_String* NT_GetStringArray(NT_Handle subentry, const struct NT_String* defaultValue, size_t defaultValueLen, size_t* len) {
+  auto cppValue = nt::GetStringArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  return ConvertToC<struct NT_String>(cppValue, len);
+}
+
+void NT_GetAtomicStringArray(NT_Handle subentry, const struct NT_String* defaultValue, size_t defaultValueLen, struct NT_TimestampedStringArray* value) {
+  auto cppValue = nt::GetAtomicStringArray(subentry, ConvertFromC(defaultValue, defaultValueLen));
+  ConvertToC(cppValue, value);
+}
+
+void NT_DisposeTimestampedStringArray(struct NT_TimestampedStringArray* value) {
+  NT_FreeStringArray(value->value, value->len);
+}
+
+struct NT_TimestampedStringArray* NT_ReadQueueStringArray(NT_Handle subentry, size_t* len) {
+  auto arr = nt::ReadQueueStringArray(subentry);
+  return ConvertToC<NT_TimestampedStringArray>(arr, len);
+}
+
+void NT_FreeQueueStringArray(struct NT_TimestampedStringArray* arr, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    NT_DisposeTimestampedStringArray(&arr[i]);
+  }
+  std::free(arr);
+}
+
+
+}  // extern "C"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/cpp/ntcore_cpp_types.cpp b/third_party/allwpilib/ntcore/src/generated/main/native/cpp/ntcore_cpp_types.cpp
new file mode 100644
index 0000000..030857e
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/cpp/ntcore_cpp_types.cpp
@@ -0,0 +1,463 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#include "ntcore_cpp_types.h"
+
+#include "Handle.h"
+#include "InstanceImpl.h"
+
+namespace {
+template <nt::ValidType T>
+struct ValuesType {
+  using Vector =
+      std::vector<typename nt::TypeInfo<std::remove_cvref_t<T>>::Value>;
+};
+
+template <>
+struct ValuesType<bool> {
+  using Vector = std::vector<int>;
+};
+}  // namespace
+
+namespace nt {
+
+template <ValidType T>
+static inline bool Set(NT_Handle pubentry, typename TypeInfo<T>::View value,
+                       int64_t time) {
+  if (auto ii = InstanceImpl::Get(Handle{pubentry}.GetInst())) {
+    return ii->localStorage.SetEntryValue(
+        pubentry, MakeValue<T>(value, time == 0 ? Now() : time));
+  } else {
+    return {};
+  }
+}
+
+template <ValidType T>
+static inline bool SetDefault(NT_Handle pubentry,
+                              typename TypeInfo<T>::View defaultValue) {
+  if (auto ii = InstanceImpl::Get(Handle{pubentry}.GetInst())) {
+    return ii->localStorage.SetDefaultEntryValue(pubentry,
+                                                 MakeValue<T>(defaultValue, 1));
+  } else {
+    return {};
+  }
+}
+
+template <ValidType T>
+static inline Timestamped<typename TypeInfo<T>::Value> GetAtomic(
+    NT_Handle subentry, typename TypeInfo<T>::View defaultValue) {
+  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
+    return ii->localStorage.GetAtomic<T>(subentry, defaultValue);
+  } else {
+    return {};
+  }
+}
+
+template <ValidType T>
+inline Timestamped<typename TypeInfo<T>::SmallRet> GetAtomic(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<typename TypeInfo<T>::SmallElem>& buf,
+    typename TypeInfo<T>::View defaultValue) {
+  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
+    return ii->localStorage.GetAtomic<T>(subentry, buf, defaultValue);
+  } else {
+    return {};
+  }
+}
+
+template <typename T>
+static inline std::vector<Timestamped<typename TypeInfo<T>::Value>> ReadQueue(
+    NT_Handle subentry) {
+  if (auto ii = InstanceImpl::Get(Handle{subentry}.GetInst())) {
+    return ii->localStorage.ReadQueue<T>(subentry);
+  } else {
+    return {};
+  }
+}
+
+template <typename T>
+static inline typename ValuesType<T>::Vector ReadQueueValues(
+    NT_Handle subentry) {
+  typename ValuesType<T>::Vector rv;
+  auto arr = ReadQueue<T>(subentry);
+  rv.reserve(arr.size());
+  for (auto&& elem : arr) {
+    rv.emplace_back(std::move(elem.value));
+  }
+  return rv;
+}
+
+bool SetBoolean(NT_Handle pubentry, bool value, int64_t time) {
+  return Set<bool>(pubentry, value, time);
+}
+
+bool SetDefaultBoolean(NT_Handle pubentry, bool defaultValue) {
+  return SetDefault<bool>(pubentry, defaultValue);
+}
+
+bool GetBoolean(NT_Handle subentry, bool defaultValue) {
+  return GetAtomic<bool>(subentry, defaultValue).value;
+}
+
+TimestampedBoolean GetAtomicBoolean(
+    NT_Handle subentry, bool defaultValue) {
+  return GetAtomic<bool>(subentry, defaultValue);
+}
+
+std::vector<TimestampedBoolean> ReadQueueBoolean(NT_Handle subentry) {
+  return ReadQueue<bool>(subentry);
+}
+
+std::vector<int> ReadQueueValuesBoolean(NT_Handle subentry) {
+  return ReadQueueValues<bool>(subentry);
+}
+
+
+bool SetInteger(NT_Handle pubentry, int64_t value, int64_t time) {
+  return Set<int64_t>(pubentry, value, time);
+}
+
+bool SetDefaultInteger(NT_Handle pubentry, int64_t defaultValue) {
+  return SetDefault<int64_t>(pubentry, defaultValue);
+}
+
+int64_t GetInteger(NT_Handle subentry, int64_t defaultValue) {
+  return GetAtomic<int64_t>(subentry, defaultValue).value;
+}
+
+TimestampedInteger GetAtomicInteger(
+    NT_Handle subentry, int64_t defaultValue) {
+  return GetAtomic<int64_t>(subentry, defaultValue);
+}
+
+std::vector<TimestampedInteger> ReadQueueInteger(NT_Handle subentry) {
+  return ReadQueue<int64_t>(subentry);
+}
+
+std::vector<int64_t> ReadQueueValuesInteger(NT_Handle subentry) {
+  return ReadQueueValues<int64_t>(subentry);
+}
+
+
+bool SetFloat(NT_Handle pubentry, float value, int64_t time) {
+  return Set<float>(pubentry, value, time);
+}
+
+bool SetDefaultFloat(NT_Handle pubentry, float defaultValue) {
+  return SetDefault<float>(pubentry, defaultValue);
+}
+
+float GetFloat(NT_Handle subentry, float defaultValue) {
+  return GetAtomic<float>(subentry, defaultValue).value;
+}
+
+TimestampedFloat GetAtomicFloat(
+    NT_Handle subentry, float defaultValue) {
+  return GetAtomic<float>(subentry, defaultValue);
+}
+
+std::vector<TimestampedFloat> ReadQueueFloat(NT_Handle subentry) {
+  return ReadQueue<float>(subentry);
+}
+
+std::vector<float> ReadQueueValuesFloat(NT_Handle subentry) {
+  return ReadQueueValues<float>(subentry);
+}
+
+
+bool SetDouble(NT_Handle pubentry, double value, int64_t time) {
+  return Set<double>(pubentry, value, time);
+}
+
+bool SetDefaultDouble(NT_Handle pubentry, double defaultValue) {
+  return SetDefault<double>(pubentry, defaultValue);
+}
+
+double GetDouble(NT_Handle subentry, double defaultValue) {
+  return GetAtomic<double>(subentry, defaultValue).value;
+}
+
+TimestampedDouble GetAtomicDouble(
+    NT_Handle subentry, double defaultValue) {
+  return GetAtomic<double>(subentry, defaultValue);
+}
+
+std::vector<TimestampedDouble> ReadQueueDouble(NT_Handle subentry) {
+  return ReadQueue<double>(subentry);
+}
+
+std::vector<double> ReadQueueValuesDouble(NT_Handle subentry) {
+  return ReadQueueValues<double>(subentry);
+}
+
+
+bool SetString(NT_Handle pubentry, std::string_view value, int64_t time) {
+  return Set<std::string>(pubentry, value, time);
+}
+
+bool SetDefaultString(NT_Handle pubentry, std::string_view defaultValue) {
+  return SetDefault<std::string>(pubentry, defaultValue);
+}
+
+std::string GetString(NT_Handle subentry, std::string_view defaultValue) {
+  return GetAtomic<std::string>(subentry, defaultValue).value;
+}
+
+TimestampedString GetAtomicString(
+    NT_Handle subentry, std::string_view defaultValue) {
+  return GetAtomic<std::string>(subentry, defaultValue);
+}
+
+std::vector<TimestampedString> ReadQueueString(NT_Handle subentry) {
+  return ReadQueue<std::string>(subentry);
+}
+
+std::vector<std::string> ReadQueueValuesString(NT_Handle subentry) {
+  return ReadQueueValues<std::string>(subentry);
+}
+
+std::string_view GetString(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<char>& buf,
+    std::string_view defaultValue) {
+  return GetAtomic<std::string>(subentry, buf, defaultValue).value;
+}
+
+TimestampedStringView GetAtomicString(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<char>& buf,
+    std::string_view defaultValue) {
+  return GetAtomic<std::string>(subentry, buf, defaultValue);
+}
+
+
+bool SetRaw(NT_Handle pubentry, std::span<const uint8_t> value, int64_t time) {
+  return Set<uint8_t[]>(pubentry, value, time);
+}
+
+bool SetDefaultRaw(NT_Handle pubentry, std::span<const uint8_t> defaultValue) {
+  return SetDefault<uint8_t[]>(pubentry, defaultValue);
+}
+
+std::vector<uint8_t> GetRaw(NT_Handle subentry, std::span<const uint8_t> defaultValue) {
+  return GetAtomic<uint8_t[]>(subentry, defaultValue).value;
+}
+
+TimestampedRaw GetAtomicRaw(
+    NT_Handle subentry, std::span<const uint8_t> defaultValue) {
+  return GetAtomic<uint8_t[]>(subentry, defaultValue);
+}
+
+std::vector<TimestampedRaw> ReadQueueRaw(NT_Handle subentry) {
+  return ReadQueue<uint8_t[]>(subentry);
+}
+
+std::vector<std::vector<uint8_t>> ReadQueueValuesRaw(NT_Handle subentry) {
+  return ReadQueueValues<uint8_t[]>(subentry);
+}
+
+std::span<uint8_t> GetRaw(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<uint8_t>& buf,
+    std::span<const uint8_t> defaultValue) {
+  return GetAtomic<uint8_t[]>(subentry, buf, defaultValue).value;
+}
+
+TimestampedRawView GetAtomicRaw(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<uint8_t>& buf,
+    std::span<const uint8_t> defaultValue) {
+  return GetAtomic<uint8_t[]>(subentry, buf, defaultValue);
+}
+
+
+bool SetBooleanArray(NT_Handle pubentry, std::span<const int> value, int64_t time) {
+  return Set<bool[]>(pubentry, value, time);
+}
+
+bool SetDefaultBooleanArray(NT_Handle pubentry, std::span<const int> defaultValue) {
+  return SetDefault<bool[]>(pubentry, defaultValue);
+}
+
+std::vector<int> GetBooleanArray(NT_Handle subentry, std::span<const int> defaultValue) {
+  return GetAtomic<bool[]>(subentry, defaultValue).value;
+}
+
+TimestampedBooleanArray GetAtomicBooleanArray(
+    NT_Handle subentry, std::span<const int> defaultValue) {
+  return GetAtomic<bool[]>(subentry, defaultValue);
+}
+
+std::vector<TimestampedBooleanArray> ReadQueueBooleanArray(NT_Handle subentry) {
+  return ReadQueue<bool[]>(subentry);
+}
+
+std::vector<std::vector<int>> ReadQueueValuesBooleanArray(NT_Handle subentry) {
+  return ReadQueueValues<bool[]>(subentry);
+}
+
+std::span<int> GetBooleanArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<int>& buf,
+    std::span<const int> defaultValue) {
+  return GetAtomic<bool[]>(subentry, buf, defaultValue).value;
+}
+
+TimestampedBooleanArrayView GetAtomicBooleanArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<int>& buf,
+    std::span<const int> defaultValue) {
+  return GetAtomic<bool[]>(subentry, buf, defaultValue);
+}
+
+
+bool SetIntegerArray(NT_Handle pubentry, std::span<const int64_t> value, int64_t time) {
+  return Set<int64_t[]>(pubentry, value, time);
+}
+
+bool SetDefaultIntegerArray(NT_Handle pubentry, std::span<const int64_t> defaultValue) {
+  return SetDefault<int64_t[]>(pubentry, defaultValue);
+}
+
+std::vector<int64_t> GetIntegerArray(NT_Handle subentry, std::span<const int64_t> defaultValue) {
+  return GetAtomic<int64_t[]>(subentry, defaultValue).value;
+}
+
+TimestampedIntegerArray GetAtomicIntegerArray(
+    NT_Handle subentry, std::span<const int64_t> defaultValue) {
+  return GetAtomic<int64_t[]>(subentry, defaultValue);
+}
+
+std::vector<TimestampedIntegerArray> ReadQueueIntegerArray(NT_Handle subentry) {
+  return ReadQueue<int64_t[]>(subentry);
+}
+
+std::vector<std::vector<int64_t>> ReadQueueValuesIntegerArray(NT_Handle subentry) {
+  return ReadQueueValues<int64_t[]>(subentry);
+}
+
+std::span<int64_t> GetIntegerArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<int64_t>& buf,
+    std::span<const int64_t> defaultValue) {
+  return GetAtomic<int64_t[]>(subentry, buf, defaultValue).value;
+}
+
+TimestampedIntegerArrayView GetAtomicIntegerArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<int64_t>& buf,
+    std::span<const int64_t> defaultValue) {
+  return GetAtomic<int64_t[]>(subentry, buf, defaultValue);
+}
+
+
+bool SetFloatArray(NT_Handle pubentry, std::span<const float> value, int64_t time) {
+  return Set<float[]>(pubentry, value, time);
+}
+
+bool SetDefaultFloatArray(NT_Handle pubentry, std::span<const float> defaultValue) {
+  return SetDefault<float[]>(pubentry, defaultValue);
+}
+
+std::vector<float> GetFloatArray(NT_Handle subentry, std::span<const float> defaultValue) {
+  return GetAtomic<float[]>(subentry, defaultValue).value;
+}
+
+TimestampedFloatArray GetAtomicFloatArray(
+    NT_Handle subentry, std::span<const float> defaultValue) {
+  return GetAtomic<float[]>(subentry, defaultValue);
+}
+
+std::vector<TimestampedFloatArray> ReadQueueFloatArray(NT_Handle subentry) {
+  return ReadQueue<float[]>(subentry);
+}
+
+std::vector<std::vector<float>> ReadQueueValuesFloatArray(NT_Handle subentry) {
+  return ReadQueueValues<float[]>(subentry);
+}
+
+std::span<float> GetFloatArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<float>& buf,
+    std::span<const float> defaultValue) {
+  return GetAtomic<float[]>(subentry, buf, defaultValue).value;
+}
+
+TimestampedFloatArrayView GetAtomicFloatArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<float>& buf,
+    std::span<const float> defaultValue) {
+  return GetAtomic<float[]>(subentry, buf, defaultValue);
+}
+
+
+bool SetDoubleArray(NT_Handle pubentry, std::span<const double> value, int64_t time) {
+  return Set<double[]>(pubentry, value, time);
+}
+
+bool SetDefaultDoubleArray(NT_Handle pubentry, std::span<const double> defaultValue) {
+  return SetDefault<double[]>(pubentry, defaultValue);
+}
+
+std::vector<double> GetDoubleArray(NT_Handle subentry, std::span<const double> defaultValue) {
+  return GetAtomic<double[]>(subentry, defaultValue).value;
+}
+
+TimestampedDoubleArray GetAtomicDoubleArray(
+    NT_Handle subentry, std::span<const double> defaultValue) {
+  return GetAtomic<double[]>(subentry, defaultValue);
+}
+
+std::vector<TimestampedDoubleArray> ReadQueueDoubleArray(NT_Handle subentry) {
+  return ReadQueue<double[]>(subentry);
+}
+
+std::vector<std::vector<double>> ReadQueueValuesDoubleArray(NT_Handle subentry) {
+  return ReadQueueValues<double[]>(subentry);
+}
+
+std::span<double> GetDoubleArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<double>& buf,
+    std::span<const double> defaultValue) {
+  return GetAtomic<double[]>(subentry, buf, defaultValue).value;
+}
+
+TimestampedDoubleArrayView GetAtomicDoubleArray(
+    NT_Handle subentry,
+    wpi::SmallVectorImpl<double>& buf,
+    std::span<const double> defaultValue) {
+  return GetAtomic<double[]>(subentry, buf, defaultValue);
+}
+
+
+bool SetStringArray(NT_Handle pubentry, std::span<const std::string> value, int64_t time) {
+  return Set<std::string[]>(pubentry, value, time);
+}
+
+bool SetDefaultStringArray(NT_Handle pubentry, std::span<const std::string> defaultValue) {
+  return SetDefault<std::string[]>(pubentry, defaultValue);
+}
+
+std::vector<std::string> GetStringArray(NT_Handle subentry, std::span<const std::string> defaultValue) {
+  return GetAtomic<std::string[]>(subentry, defaultValue).value;
+}
+
+TimestampedStringArray GetAtomicStringArray(
+    NT_Handle subentry, std::span<const std::string> defaultValue) {
+  return GetAtomic<std::string[]>(subentry, defaultValue);
+}
+
+std::vector<TimestampedStringArray> ReadQueueStringArray(NT_Handle subentry) {
+  return ReadQueue<std::string[]>(subentry);
+}
+
+std::vector<std::vector<std::string>> ReadQueueValuesStringArray(NT_Handle subentry) {
+  return ReadQueueValues<std::string[]>(subentry);
+}
+
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanArrayTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanArrayTopic.h
new file mode 100644
index 0000000..de89f4b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanArrayTopic.h
@@ -0,0 +1,426 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <utility>
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class BooleanArrayTopic;
+
+/**
+ * NetworkTables BooleanArray subscriber.
+ */
+class BooleanArraySubscriber : public Subscriber {
+ public:
+  using TopicType = BooleanArrayTopic;
+  using ValueType = std::vector<int>;
+  using ParamType = std::span<const int>;
+  using TimestampedValueType = TimestampedBooleanArray;
+
+  using SmallRetType = std::span<int>;
+  using SmallElemType = int;
+  using TimestampedValueViewType = TimestampedBooleanArrayView;
+
+
+  BooleanArraySubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * BooleanArrayTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  BooleanArraySubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @param buf storage for returned value
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf,
+      ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables BooleanArray publisher.
+ */
+class BooleanArrayPublisher : public Publisher {
+ public:
+  using TopicType = BooleanArrayTopic;
+  using ValueType = std::vector<int>;
+  using ParamType = std::span<const int>;
+
+  using SmallRetType = std::span<int>;
+  using SmallElemType = int;
+
+  using TimestampedValueType = TimestampedBooleanArray;
+
+  BooleanArrayPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * BooleanArrayTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit BooleanArrayPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables BooleanArray entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class BooleanArrayEntry final : public BooleanArraySubscriber,
+                                  public BooleanArrayPublisher {
+ public:
+  using SubscriberType = BooleanArraySubscriber;
+  using PublisherType = BooleanArrayPublisher;
+  using TopicType = BooleanArrayTopic;
+  using ValueType = std::vector<int>;
+  using ParamType = std::span<const int>;
+
+  using SmallRetType = std::span<int>;
+  using SmallElemType = int;
+
+  using TimestampedValueType = TimestampedBooleanArray;
+
+  BooleanArrayEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * BooleanArrayTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  BooleanArrayEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables BooleanArray topic.
+ */
+class BooleanArrayTopic final : public Topic {
+ public:
+  using SubscriberType = BooleanArraySubscriber;
+  using PublisherType = BooleanArrayPublisher;
+  using EntryType = BooleanArrayEntry;
+  using ValueType = std::vector<int>;
+  using ParamType = std::span<const int>;
+  using TimestampedValueType = TimestampedBooleanArray;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "boolean[]";
+
+  BooleanArrayTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetBooleanArrayTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit BooleanArrayTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit BooleanArrayTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/BooleanArrayTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanArrayTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanArrayTopic.inc
new file mode 100644
index 0000000..404ec7e
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanArrayTopic.inc
@@ -0,0 +1,137 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/BooleanArrayTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline BooleanArraySubscriber::BooleanArraySubscriber(
+    NT_Subscriber handle, std::span<const int> defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue.begin(), defaultValue.end()} {}
+
+inline std::vector<int> BooleanArraySubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline std::vector<int> BooleanArraySubscriber::Get(
+    std::span<const int> defaultValue) const {
+  return ::nt::GetBooleanArray(m_subHandle, defaultValue);
+}
+
+inline std::span<int> BooleanArraySubscriber::Get(wpi::SmallVectorImpl<int>& buf) const {
+  return Get(buf, m_defaultValue);
+}
+
+inline std::span<int> BooleanArraySubscriber::Get(wpi::SmallVectorImpl<int>& buf, std::span<const int> defaultValue) const {
+  return nt::GetBooleanArray(m_subHandle, buf, defaultValue);
+}
+
+inline TimestampedBooleanArray BooleanArraySubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedBooleanArray BooleanArraySubscriber::GetAtomic(
+    std::span<const int> defaultValue) const {
+  return ::nt::GetAtomicBooleanArray(m_subHandle, defaultValue);
+}
+
+inline TimestampedBooleanArrayView BooleanArraySubscriber::GetAtomic(wpi::SmallVectorImpl<int>& buf) const {
+  return GetAtomic(buf, m_defaultValue);
+}
+
+inline TimestampedBooleanArrayView BooleanArraySubscriber::GetAtomic(wpi::SmallVectorImpl<int>& buf, std::span<const int> defaultValue) const {
+  return nt::GetAtomicBooleanArray(m_subHandle, buf, defaultValue);
+}
+
+inline std::vector<TimestampedBooleanArray>
+BooleanArraySubscriber::ReadQueue() {
+  return ::nt::ReadQueueBooleanArray(m_subHandle);
+}
+
+inline BooleanArrayTopic BooleanArraySubscriber::GetTopic() const {
+  return BooleanArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline BooleanArrayPublisher::BooleanArrayPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void BooleanArrayPublisher::Set(std::span<const int> value,
+                                         int64_t time) {
+  ::nt::SetBooleanArray(m_pubHandle, value, time);
+}
+
+inline void BooleanArrayPublisher::SetDefault(std::span<const int> value) {
+  ::nt::SetDefaultBooleanArray(m_pubHandle, value);
+}
+
+inline BooleanArrayTopic BooleanArrayPublisher::GetTopic() const {
+  return BooleanArrayTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline BooleanArrayEntry::BooleanArrayEntry(
+    NT_Entry handle, std::span<const int> defaultValue)
+    : BooleanArraySubscriber{handle, defaultValue},
+      BooleanArrayPublisher{handle} {}
+
+inline BooleanArrayTopic BooleanArrayEntry::GetTopic() const {
+  return BooleanArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void BooleanArrayEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline BooleanArraySubscriber BooleanArrayTopic::Subscribe(
+    std::span<const int> defaultValue,
+    const PubSubOptions& options) {
+  return BooleanArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_BOOLEAN_ARRAY, "boolean[]", options),
+      defaultValue};
+}
+inline BooleanArraySubscriber BooleanArrayTopic::SubscribeEx(
+    std::string_view typeString, std::span<const int> defaultValue,
+    const PubSubOptions& options) {
+  return BooleanArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_BOOLEAN_ARRAY, typeString, options),
+      defaultValue};
+}
+
+inline BooleanArrayPublisher BooleanArrayTopic::Publish(
+    const PubSubOptions& options) {
+  return BooleanArrayPublisher{
+      ::nt::Publish(m_handle, NT_BOOLEAN_ARRAY, "boolean[]", options)};
+}
+
+inline BooleanArrayPublisher BooleanArrayTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return BooleanArrayPublisher{
+      ::nt::PublishEx(m_handle, NT_BOOLEAN_ARRAY, typeString, properties, options)};
+}
+
+inline BooleanArrayEntry BooleanArrayTopic::GetEntry(
+    std::span<const int> defaultValue,
+    const PubSubOptions& options) {
+  return BooleanArrayEntry{
+      ::nt::GetEntry(m_handle, NT_BOOLEAN_ARRAY, "boolean[]", options),
+      defaultValue};
+}
+inline BooleanArrayEntry BooleanArrayTopic::GetEntryEx(
+    std::string_view typeString, std::span<const int> defaultValue,
+    const PubSubOptions& options) {
+  return BooleanArrayEntry{
+      ::nt::GetEntry(m_handle, NT_BOOLEAN_ARRAY, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanTopic.h
new file mode 100644
index 0000000..22db06b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanTopic.h
@@ -0,0 +1,373 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class BooleanTopic;
+
+/**
+ * NetworkTables Boolean subscriber.
+ */
+class BooleanSubscriber : public Subscriber {
+ public:
+  using TopicType = BooleanTopic;
+  using ValueType = bool;
+  using ParamType = bool;
+  using TimestampedValueType = TimestampedBoolean;
+
+
+  BooleanSubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * BooleanTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  BooleanSubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables Boolean publisher.
+ */
+class BooleanPublisher : public Publisher {
+ public:
+  using TopicType = BooleanTopic;
+  using ValueType = bool;
+  using ParamType = bool;
+
+  using TimestampedValueType = TimestampedBoolean;
+
+  BooleanPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * BooleanTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit BooleanPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables Boolean entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class BooleanEntry final : public BooleanSubscriber,
+                                  public BooleanPublisher {
+ public:
+  using SubscriberType = BooleanSubscriber;
+  using PublisherType = BooleanPublisher;
+  using TopicType = BooleanTopic;
+  using ValueType = bool;
+  using ParamType = bool;
+
+  using TimestampedValueType = TimestampedBoolean;
+
+  BooleanEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * BooleanTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  BooleanEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables Boolean topic.
+ */
+class BooleanTopic final : public Topic {
+ public:
+  using SubscriberType = BooleanSubscriber;
+  using PublisherType = BooleanPublisher;
+  using EntryType = BooleanEntry;
+  using ValueType = bool;
+  using ParamType = bool;
+  using TimestampedValueType = TimestampedBoolean;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "boolean";
+
+  BooleanTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetBooleanTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit BooleanTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit BooleanTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/BooleanTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanTopic.inc
new file mode 100644
index 0000000..2a4d7e6
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/BooleanTopic.inc
@@ -0,0 +1,121 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/BooleanTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline BooleanSubscriber::BooleanSubscriber(
+    NT_Subscriber handle, bool defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue} {}
+
+inline bool BooleanSubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline bool BooleanSubscriber::Get(
+    bool defaultValue) const {
+  return ::nt::GetBoolean(m_subHandle, defaultValue);
+}
+
+inline TimestampedBoolean BooleanSubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedBoolean BooleanSubscriber::GetAtomic(
+    bool defaultValue) const {
+  return ::nt::GetAtomicBoolean(m_subHandle, defaultValue);
+}
+
+inline std::vector<TimestampedBoolean>
+BooleanSubscriber::ReadQueue() {
+  return ::nt::ReadQueueBoolean(m_subHandle);
+}
+
+inline BooleanTopic BooleanSubscriber::GetTopic() const {
+  return BooleanTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline BooleanPublisher::BooleanPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void BooleanPublisher::Set(bool value,
+                                         int64_t time) {
+  ::nt::SetBoolean(m_pubHandle, value, time);
+}
+
+inline void BooleanPublisher::SetDefault(bool value) {
+  ::nt::SetDefaultBoolean(m_pubHandle, value);
+}
+
+inline BooleanTopic BooleanPublisher::GetTopic() const {
+  return BooleanTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline BooleanEntry::BooleanEntry(
+    NT_Entry handle, bool defaultValue)
+    : BooleanSubscriber{handle, defaultValue},
+      BooleanPublisher{handle} {}
+
+inline BooleanTopic BooleanEntry::GetTopic() const {
+  return BooleanTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void BooleanEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline BooleanSubscriber BooleanTopic::Subscribe(
+    bool defaultValue,
+    const PubSubOptions& options) {
+  return BooleanSubscriber{
+      ::nt::Subscribe(m_handle, NT_BOOLEAN, "boolean", options),
+      defaultValue};
+}
+inline BooleanSubscriber BooleanTopic::SubscribeEx(
+    std::string_view typeString, bool defaultValue,
+    const PubSubOptions& options) {
+  return BooleanSubscriber{
+      ::nt::Subscribe(m_handle, NT_BOOLEAN, typeString, options),
+      defaultValue};
+}
+
+inline BooleanPublisher BooleanTopic::Publish(
+    const PubSubOptions& options) {
+  return BooleanPublisher{
+      ::nt::Publish(m_handle, NT_BOOLEAN, "boolean", options)};
+}
+
+inline BooleanPublisher BooleanTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return BooleanPublisher{
+      ::nt::PublishEx(m_handle, NT_BOOLEAN, typeString, properties, options)};
+}
+
+inline BooleanEntry BooleanTopic::GetEntry(
+    bool defaultValue,
+    const PubSubOptions& options) {
+  return BooleanEntry{
+      ::nt::GetEntry(m_handle, NT_BOOLEAN, "boolean", options),
+      defaultValue};
+}
+inline BooleanEntry BooleanTopic::GetEntryEx(
+    std::string_view typeString, bool defaultValue,
+    const PubSubOptions& options) {
+  return BooleanEntry{
+      ::nt::GetEntry(m_handle, NT_BOOLEAN, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleArrayTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleArrayTopic.h
new file mode 100644
index 0000000..6f64d36
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleArrayTopic.h
@@ -0,0 +1,426 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <utility>
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class DoubleArrayTopic;
+
+/**
+ * NetworkTables DoubleArray subscriber.
+ */
+class DoubleArraySubscriber : public Subscriber {
+ public:
+  using TopicType = DoubleArrayTopic;
+  using ValueType = std::vector<double>;
+  using ParamType = std::span<const double>;
+  using TimestampedValueType = TimestampedDoubleArray;
+
+  using SmallRetType = std::span<double>;
+  using SmallElemType = double;
+  using TimestampedValueViewType = TimestampedDoubleArrayView;
+
+
+  DoubleArraySubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * DoubleArrayTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  DoubleArraySubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @param buf storage for returned value
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf,
+      ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables DoubleArray publisher.
+ */
+class DoubleArrayPublisher : public Publisher {
+ public:
+  using TopicType = DoubleArrayTopic;
+  using ValueType = std::vector<double>;
+  using ParamType = std::span<const double>;
+
+  using SmallRetType = std::span<double>;
+  using SmallElemType = double;
+
+  using TimestampedValueType = TimestampedDoubleArray;
+
+  DoubleArrayPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * DoubleArrayTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit DoubleArrayPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables DoubleArray entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class DoubleArrayEntry final : public DoubleArraySubscriber,
+                                  public DoubleArrayPublisher {
+ public:
+  using SubscriberType = DoubleArraySubscriber;
+  using PublisherType = DoubleArrayPublisher;
+  using TopicType = DoubleArrayTopic;
+  using ValueType = std::vector<double>;
+  using ParamType = std::span<const double>;
+
+  using SmallRetType = std::span<double>;
+  using SmallElemType = double;
+
+  using TimestampedValueType = TimestampedDoubleArray;
+
+  DoubleArrayEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * DoubleArrayTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  DoubleArrayEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables DoubleArray topic.
+ */
+class DoubleArrayTopic final : public Topic {
+ public:
+  using SubscriberType = DoubleArraySubscriber;
+  using PublisherType = DoubleArrayPublisher;
+  using EntryType = DoubleArrayEntry;
+  using ValueType = std::vector<double>;
+  using ParamType = std::span<const double>;
+  using TimestampedValueType = TimestampedDoubleArray;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "double[]";
+
+  DoubleArrayTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetDoubleArrayTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit DoubleArrayTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit DoubleArrayTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/DoubleArrayTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleArrayTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleArrayTopic.inc
new file mode 100644
index 0000000..bab8dd0
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleArrayTopic.inc
@@ -0,0 +1,137 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/DoubleArrayTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline DoubleArraySubscriber::DoubleArraySubscriber(
+    NT_Subscriber handle, std::span<const double> defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue.begin(), defaultValue.end()} {}
+
+inline std::vector<double> DoubleArraySubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline std::vector<double> DoubleArraySubscriber::Get(
+    std::span<const double> defaultValue) const {
+  return ::nt::GetDoubleArray(m_subHandle, defaultValue);
+}
+
+inline std::span<double> DoubleArraySubscriber::Get(wpi::SmallVectorImpl<double>& buf) const {
+  return Get(buf, m_defaultValue);
+}
+
+inline std::span<double> DoubleArraySubscriber::Get(wpi::SmallVectorImpl<double>& buf, std::span<const double> defaultValue) const {
+  return nt::GetDoubleArray(m_subHandle, buf, defaultValue);
+}
+
+inline TimestampedDoubleArray DoubleArraySubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedDoubleArray DoubleArraySubscriber::GetAtomic(
+    std::span<const double> defaultValue) const {
+  return ::nt::GetAtomicDoubleArray(m_subHandle, defaultValue);
+}
+
+inline TimestampedDoubleArrayView DoubleArraySubscriber::GetAtomic(wpi::SmallVectorImpl<double>& buf) const {
+  return GetAtomic(buf, m_defaultValue);
+}
+
+inline TimestampedDoubleArrayView DoubleArraySubscriber::GetAtomic(wpi::SmallVectorImpl<double>& buf, std::span<const double> defaultValue) const {
+  return nt::GetAtomicDoubleArray(m_subHandle, buf, defaultValue);
+}
+
+inline std::vector<TimestampedDoubleArray>
+DoubleArraySubscriber::ReadQueue() {
+  return ::nt::ReadQueueDoubleArray(m_subHandle);
+}
+
+inline DoubleArrayTopic DoubleArraySubscriber::GetTopic() const {
+  return DoubleArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline DoubleArrayPublisher::DoubleArrayPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void DoubleArrayPublisher::Set(std::span<const double> value,
+                                         int64_t time) {
+  ::nt::SetDoubleArray(m_pubHandle, value, time);
+}
+
+inline void DoubleArrayPublisher::SetDefault(std::span<const double> value) {
+  ::nt::SetDefaultDoubleArray(m_pubHandle, value);
+}
+
+inline DoubleArrayTopic DoubleArrayPublisher::GetTopic() const {
+  return DoubleArrayTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline DoubleArrayEntry::DoubleArrayEntry(
+    NT_Entry handle, std::span<const double> defaultValue)
+    : DoubleArraySubscriber{handle, defaultValue},
+      DoubleArrayPublisher{handle} {}
+
+inline DoubleArrayTopic DoubleArrayEntry::GetTopic() const {
+  return DoubleArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void DoubleArrayEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline DoubleArraySubscriber DoubleArrayTopic::Subscribe(
+    std::span<const double> defaultValue,
+    const PubSubOptions& options) {
+  return DoubleArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_DOUBLE_ARRAY, "double[]", options),
+      defaultValue};
+}
+inline DoubleArraySubscriber DoubleArrayTopic::SubscribeEx(
+    std::string_view typeString, std::span<const double> defaultValue,
+    const PubSubOptions& options) {
+  return DoubleArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_DOUBLE_ARRAY, typeString, options),
+      defaultValue};
+}
+
+inline DoubleArrayPublisher DoubleArrayTopic::Publish(
+    const PubSubOptions& options) {
+  return DoubleArrayPublisher{
+      ::nt::Publish(m_handle, NT_DOUBLE_ARRAY, "double[]", options)};
+}
+
+inline DoubleArrayPublisher DoubleArrayTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return DoubleArrayPublisher{
+      ::nt::PublishEx(m_handle, NT_DOUBLE_ARRAY, typeString, properties, options)};
+}
+
+inline DoubleArrayEntry DoubleArrayTopic::GetEntry(
+    std::span<const double> defaultValue,
+    const PubSubOptions& options) {
+  return DoubleArrayEntry{
+      ::nt::GetEntry(m_handle, NT_DOUBLE_ARRAY, "double[]", options),
+      defaultValue};
+}
+inline DoubleArrayEntry DoubleArrayTopic::GetEntryEx(
+    std::string_view typeString, std::span<const double> defaultValue,
+    const PubSubOptions& options) {
+  return DoubleArrayEntry{
+      ::nt::GetEntry(m_handle, NT_DOUBLE_ARRAY, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleTopic.h
new file mode 100644
index 0000000..bcb1751
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleTopic.h
@@ -0,0 +1,373 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class DoubleTopic;
+
+/**
+ * NetworkTables Double subscriber.
+ */
+class DoubleSubscriber : public Subscriber {
+ public:
+  using TopicType = DoubleTopic;
+  using ValueType = double;
+  using ParamType = double;
+  using TimestampedValueType = TimestampedDouble;
+
+
+  DoubleSubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * DoubleTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  DoubleSubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables Double publisher.
+ */
+class DoublePublisher : public Publisher {
+ public:
+  using TopicType = DoubleTopic;
+  using ValueType = double;
+  using ParamType = double;
+
+  using TimestampedValueType = TimestampedDouble;
+
+  DoublePublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * DoubleTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit DoublePublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables Double entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class DoubleEntry final : public DoubleSubscriber,
+                                  public DoublePublisher {
+ public:
+  using SubscriberType = DoubleSubscriber;
+  using PublisherType = DoublePublisher;
+  using TopicType = DoubleTopic;
+  using ValueType = double;
+  using ParamType = double;
+
+  using TimestampedValueType = TimestampedDouble;
+
+  DoubleEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * DoubleTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  DoubleEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables Double topic.
+ */
+class DoubleTopic final : public Topic {
+ public:
+  using SubscriberType = DoubleSubscriber;
+  using PublisherType = DoublePublisher;
+  using EntryType = DoubleEntry;
+  using ValueType = double;
+  using ParamType = double;
+  using TimestampedValueType = TimestampedDouble;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "double";
+
+  DoubleTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetDoubleTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit DoubleTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit DoubleTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/DoubleTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleTopic.inc
new file mode 100644
index 0000000..49b1c4c
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/DoubleTopic.inc
@@ -0,0 +1,121 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/DoubleTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline DoubleSubscriber::DoubleSubscriber(
+    NT_Subscriber handle, double defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue} {}
+
+inline double DoubleSubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline double DoubleSubscriber::Get(
+    double defaultValue) const {
+  return ::nt::GetDouble(m_subHandle, defaultValue);
+}
+
+inline TimestampedDouble DoubleSubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedDouble DoubleSubscriber::GetAtomic(
+    double defaultValue) const {
+  return ::nt::GetAtomicDouble(m_subHandle, defaultValue);
+}
+
+inline std::vector<TimestampedDouble>
+DoubleSubscriber::ReadQueue() {
+  return ::nt::ReadQueueDouble(m_subHandle);
+}
+
+inline DoubleTopic DoubleSubscriber::GetTopic() const {
+  return DoubleTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline DoublePublisher::DoublePublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void DoublePublisher::Set(double value,
+                                         int64_t time) {
+  ::nt::SetDouble(m_pubHandle, value, time);
+}
+
+inline void DoublePublisher::SetDefault(double value) {
+  ::nt::SetDefaultDouble(m_pubHandle, value);
+}
+
+inline DoubleTopic DoublePublisher::GetTopic() const {
+  return DoubleTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline DoubleEntry::DoubleEntry(
+    NT_Entry handle, double defaultValue)
+    : DoubleSubscriber{handle, defaultValue},
+      DoublePublisher{handle} {}
+
+inline DoubleTopic DoubleEntry::GetTopic() const {
+  return DoubleTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void DoubleEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline DoubleSubscriber DoubleTopic::Subscribe(
+    double defaultValue,
+    const PubSubOptions& options) {
+  return DoubleSubscriber{
+      ::nt::Subscribe(m_handle, NT_DOUBLE, "double", options),
+      defaultValue};
+}
+inline DoubleSubscriber DoubleTopic::SubscribeEx(
+    std::string_view typeString, double defaultValue,
+    const PubSubOptions& options) {
+  return DoubleSubscriber{
+      ::nt::Subscribe(m_handle, NT_DOUBLE, typeString, options),
+      defaultValue};
+}
+
+inline DoublePublisher DoubleTopic::Publish(
+    const PubSubOptions& options) {
+  return DoublePublisher{
+      ::nt::Publish(m_handle, NT_DOUBLE, "double", options)};
+}
+
+inline DoublePublisher DoubleTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return DoublePublisher{
+      ::nt::PublishEx(m_handle, NT_DOUBLE, typeString, properties, options)};
+}
+
+inline DoubleEntry DoubleTopic::GetEntry(
+    double defaultValue,
+    const PubSubOptions& options) {
+  return DoubleEntry{
+      ::nt::GetEntry(m_handle, NT_DOUBLE, "double", options),
+      defaultValue};
+}
+inline DoubleEntry DoubleTopic::GetEntryEx(
+    std::string_view typeString, double defaultValue,
+    const PubSubOptions& options) {
+  return DoubleEntry{
+      ::nt::GetEntry(m_handle, NT_DOUBLE, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatArrayTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatArrayTopic.h
new file mode 100644
index 0000000..a0d2b66
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatArrayTopic.h
@@ -0,0 +1,426 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <utility>
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class FloatArrayTopic;
+
+/**
+ * NetworkTables FloatArray subscriber.
+ */
+class FloatArraySubscriber : public Subscriber {
+ public:
+  using TopicType = FloatArrayTopic;
+  using ValueType = std::vector<float>;
+  using ParamType = std::span<const float>;
+  using TimestampedValueType = TimestampedFloatArray;
+
+  using SmallRetType = std::span<float>;
+  using SmallElemType = float;
+  using TimestampedValueViewType = TimestampedFloatArrayView;
+
+
+  FloatArraySubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * FloatArrayTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  FloatArraySubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @param buf storage for returned value
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf,
+      ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables FloatArray publisher.
+ */
+class FloatArrayPublisher : public Publisher {
+ public:
+  using TopicType = FloatArrayTopic;
+  using ValueType = std::vector<float>;
+  using ParamType = std::span<const float>;
+
+  using SmallRetType = std::span<float>;
+  using SmallElemType = float;
+
+  using TimestampedValueType = TimestampedFloatArray;
+
+  FloatArrayPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * FloatArrayTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit FloatArrayPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables FloatArray entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class FloatArrayEntry final : public FloatArraySubscriber,
+                                  public FloatArrayPublisher {
+ public:
+  using SubscriberType = FloatArraySubscriber;
+  using PublisherType = FloatArrayPublisher;
+  using TopicType = FloatArrayTopic;
+  using ValueType = std::vector<float>;
+  using ParamType = std::span<const float>;
+
+  using SmallRetType = std::span<float>;
+  using SmallElemType = float;
+
+  using TimestampedValueType = TimestampedFloatArray;
+
+  FloatArrayEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * FloatArrayTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  FloatArrayEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables FloatArray topic.
+ */
+class FloatArrayTopic final : public Topic {
+ public:
+  using SubscriberType = FloatArraySubscriber;
+  using PublisherType = FloatArrayPublisher;
+  using EntryType = FloatArrayEntry;
+  using ValueType = std::vector<float>;
+  using ParamType = std::span<const float>;
+  using TimestampedValueType = TimestampedFloatArray;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "float[]";
+
+  FloatArrayTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetFloatArrayTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit FloatArrayTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit FloatArrayTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/FloatArrayTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatArrayTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatArrayTopic.inc
new file mode 100644
index 0000000..9357076
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatArrayTopic.inc
@@ -0,0 +1,137 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/FloatArrayTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline FloatArraySubscriber::FloatArraySubscriber(
+    NT_Subscriber handle, std::span<const float> defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue.begin(), defaultValue.end()} {}
+
+inline std::vector<float> FloatArraySubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline std::vector<float> FloatArraySubscriber::Get(
+    std::span<const float> defaultValue) const {
+  return ::nt::GetFloatArray(m_subHandle, defaultValue);
+}
+
+inline std::span<float> FloatArraySubscriber::Get(wpi::SmallVectorImpl<float>& buf) const {
+  return Get(buf, m_defaultValue);
+}
+
+inline std::span<float> FloatArraySubscriber::Get(wpi::SmallVectorImpl<float>& buf, std::span<const float> defaultValue) const {
+  return nt::GetFloatArray(m_subHandle, buf, defaultValue);
+}
+
+inline TimestampedFloatArray FloatArraySubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedFloatArray FloatArraySubscriber::GetAtomic(
+    std::span<const float> defaultValue) const {
+  return ::nt::GetAtomicFloatArray(m_subHandle, defaultValue);
+}
+
+inline TimestampedFloatArrayView FloatArraySubscriber::GetAtomic(wpi::SmallVectorImpl<float>& buf) const {
+  return GetAtomic(buf, m_defaultValue);
+}
+
+inline TimestampedFloatArrayView FloatArraySubscriber::GetAtomic(wpi::SmallVectorImpl<float>& buf, std::span<const float> defaultValue) const {
+  return nt::GetAtomicFloatArray(m_subHandle, buf, defaultValue);
+}
+
+inline std::vector<TimestampedFloatArray>
+FloatArraySubscriber::ReadQueue() {
+  return ::nt::ReadQueueFloatArray(m_subHandle);
+}
+
+inline FloatArrayTopic FloatArraySubscriber::GetTopic() const {
+  return FloatArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline FloatArrayPublisher::FloatArrayPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void FloatArrayPublisher::Set(std::span<const float> value,
+                                         int64_t time) {
+  ::nt::SetFloatArray(m_pubHandle, value, time);
+}
+
+inline void FloatArrayPublisher::SetDefault(std::span<const float> value) {
+  ::nt::SetDefaultFloatArray(m_pubHandle, value);
+}
+
+inline FloatArrayTopic FloatArrayPublisher::GetTopic() const {
+  return FloatArrayTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline FloatArrayEntry::FloatArrayEntry(
+    NT_Entry handle, std::span<const float> defaultValue)
+    : FloatArraySubscriber{handle, defaultValue},
+      FloatArrayPublisher{handle} {}
+
+inline FloatArrayTopic FloatArrayEntry::GetTopic() const {
+  return FloatArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void FloatArrayEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline FloatArraySubscriber FloatArrayTopic::Subscribe(
+    std::span<const float> defaultValue,
+    const PubSubOptions& options) {
+  return FloatArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_FLOAT_ARRAY, "float[]", options),
+      defaultValue};
+}
+inline FloatArraySubscriber FloatArrayTopic::SubscribeEx(
+    std::string_view typeString, std::span<const float> defaultValue,
+    const PubSubOptions& options) {
+  return FloatArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_FLOAT_ARRAY, typeString, options),
+      defaultValue};
+}
+
+inline FloatArrayPublisher FloatArrayTopic::Publish(
+    const PubSubOptions& options) {
+  return FloatArrayPublisher{
+      ::nt::Publish(m_handle, NT_FLOAT_ARRAY, "float[]", options)};
+}
+
+inline FloatArrayPublisher FloatArrayTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return FloatArrayPublisher{
+      ::nt::PublishEx(m_handle, NT_FLOAT_ARRAY, typeString, properties, options)};
+}
+
+inline FloatArrayEntry FloatArrayTopic::GetEntry(
+    std::span<const float> defaultValue,
+    const PubSubOptions& options) {
+  return FloatArrayEntry{
+      ::nt::GetEntry(m_handle, NT_FLOAT_ARRAY, "float[]", options),
+      defaultValue};
+}
+inline FloatArrayEntry FloatArrayTopic::GetEntryEx(
+    std::string_view typeString, std::span<const float> defaultValue,
+    const PubSubOptions& options) {
+  return FloatArrayEntry{
+      ::nt::GetEntry(m_handle, NT_FLOAT_ARRAY, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatTopic.h
new file mode 100644
index 0000000..9a83834
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatTopic.h
@@ -0,0 +1,373 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class FloatTopic;
+
+/**
+ * NetworkTables Float subscriber.
+ */
+class FloatSubscriber : public Subscriber {
+ public:
+  using TopicType = FloatTopic;
+  using ValueType = float;
+  using ParamType = float;
+  using TimestampedValueType = TimestampedFloat;
+
+
+  FloatSubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * FloatTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  FloatSubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables Float publisher.
+ */
+class FloatPublisher : public Publisher {
+ public:
+  using TopicType = FloatTopic;
+  using ValueType = float;
+  using ParamType = float;
+
+  using TimestampedValueType = TimestampedFloat;
+
+  FloatPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * FloatTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit FloatPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables Float entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class FloatEntry final : public FloatSubscriber,
+                                  public FloatPublisher {
+ public:
+  using SubscriberType = FloatSubscriber;
+  using PublisherType = FloatPublisher;
+  using TopicType = FloatTopic;
+  using ValueType = float;
+  using ParamType = float;
+
+  using TimestampedValueType = TimestampedFloat;
+
+  FloatEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * FloatTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  FloatEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables Float topic.
+ */
+class FloatTopic final : public Topic {
+ public:
+  using SubscriberType = FloatSubscriber;
+  using PublisherType = FloatPublisher;
+  using EntryType = FloatEntry;
+  using ValueType = float;
+  using ParamType = float;
+  using TimestampedValueType = TimestampedFloat;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "float";
+
+  FloatTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetFloatTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit FloatTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit FloatTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/FloatTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatTopic.inc
new file mode 100644
index 0000000..9df0f79
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/FloatTopic.inc
@@ -0,0 +1,121 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/FloatTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline FloatSubscriber::FloatSubscriber(
+    NT_Subscriber handle, float defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue} {}
+
+inline float FloatSubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline float FloatSubscriber::Get(
+    float defaultValue) const {
+  return ::nt::GetFloat(m_subHandle, defaultValue);
+}
+
+inline TimestampedFloat FloatSubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedFloat FloatSubscriber::GetAtomic(
+    float defaultValue) const {
+  return ::nt::GetAtomicFloat(m_subHandle, defaultValue);
+}
+
+inline std::vector<TimestampedFloat>
+FloatSubscriber::ReadQueue() {
+  return ::nt::ReadQueueFloat(m_subHandle);
+}
+
+inline FloatTopic FloatSubscriber::GetTopic() const {
+  return FloatTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline FloatPublisher::FloatPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void FloatPublisher::Set(float value,
+                                         int64_t time) {
+  ::nt::SetFloat(m_pubHandle, value, time);
+}
+
+inline void FloatPublisher::SetDefault(float value) {
+  ::nt::SetDefaultFloat(m_pubHandle, value);
+}
+
+inline FloatTopic FloatPublisher::GetTopic() const {
+  return FloatTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline FloatEntry::FloatEntry(
+    NT_Entry handle, float defaultValue)
+    : FloatSubscriber{handle, defaultValue},
+      FloatPublisher{handle} {}
+
+inline FloatTopic FloatEntry::GetTopic() const {
+  return FloatTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void FloatEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline FloatSubscriber FloatTopic::Subscribe(
+    float defaultValue,
+    const PubSubOptions& options) {
+  return FloatSubscriber{
+      ::nt::Subscribe(m_handle, NT_FLOAT, "float", options),
+      defaultValue};
+}
+inline FloatSubscriber FloatTopic::SubscribeEx(
+    std::string_view typeString, float defaultValue,
+    const PubSubOptions& options) {
+  return FloatSubscriber{
+      ::nt::Subscribe(m_handle, NT_FLOAT, typeString, options),
+      defaultValue};
+}
+
+inline FloatPublisher FloatTopic::Publish(
+    const PubSubOptions& options) {
+  return FloatPublisher{
+      ::nt::Publish(m_handle, NT_FLOAT, "float", options)};
+}
+
+inline FloatPublisher FloatTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return FloatPublisher{
+      ::nt::PublishEx(m_handle, NT_FLOAT, typeString, properties, options)};
+}
+
+inline FloatEntry FloatTopic::GetEntry(
+    float defaultValue,
+    const PubSubOptions& options) {
+  return FloatEntry{
+      ::nt::GetEntry(m_handle, NT_FLOAT, "float", options),
+      defaultValue};
+}
+inline FloatEntry FloatTopic::GetEntryEx(
+    std::string_view typeString, float defaultValue,
+    const PubSubOptions& options) {
+  return FloatEntry{
+      ::nt::GetEntry(m_handle, NT_FLOAT, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerArrayTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerArrayTopic.h
new file mode 100644
index 0000000..a99823a
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerArrayTopic.h
@@ -0,0 +1,426 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <utility>
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class IntegerArrayTopic;
+
+/**
+ * NetworkTables IntegerArray subscriber.
+ */
+class IntegerArraySubscriber : public Subscriber {
+ public:
+  using TopicType = IntegerArrayTopic;
+  using ValueType = std::vector<int64_t>;
+  using ParamType = std::span<const int64_t>;
+  using TimestampedValueType = TimestampedIntegerArray;
+
+  using SmallRetType = std::span<int64_t>;
+  using SmallElemType = int64_t;
+  using TimestampedValueViewType = TimestampedIntegerArrayView;
+
+
+  IntegerArraySubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * IntegerArrayTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  IntegerArraySubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @param buf storage for returned value
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf,
+      ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables IntegerArray publisher.
+ */
+class IntegerArrayPublisher : public Publisher {
+ public:
+  using TopicType = IntegerArrayTopic;
+  using ValueType = std::vector<int64_t>;
+  using ParamType = std::span<const int64_t>;
+
+  using SmallRetType = std::span<int64_t>;
+  using SmallElemType = int64_t;
+
+  using TimestampedValueType = TimestampedIntegerArray;
+
+  IntegerArrayPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * IntegerArrayTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit IntegerArrayPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables IntegerArray entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class IntegerArrayEntry final : public IntegerArraySubscriber,
+                                  public IntegerArrayPublisher {
+ public:
+  using SubscriberType = IntegerArraySubscriber;
+  using PublisherType = IntegerArrayPublisher;
+  using TopicType = IntegerArrayTopic;
+  using ValueType = std::vector<int64_t>;
+  using ParamType = std::span<const int64_t>;
+
+  using SmallRetType = std::span<int64_t>;
+  using SmallElemType = int64_t;
+
+  using TimestampedValueType = TimestampedIntegerArray;
+
+  IntegerArrayEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * IntegerArrayTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  IntegerArrayEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables IntegerArray topic.
+ */
+class IntegerArrayTopic final : public Topic {
+ public:
+  using SubscriberType = IntegerArraySubscriber;
+  using PublisherType = IntegerArrayPublisher;
+  using EntryType = IntegerArrayEntry;
+  using ValueType = std::vector<int64_t>;
+  using ParamType = std::span<const int64_t>;
+  using TimestampedValueType = TimestampedIntegerArray;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "int[]";
+
+  IntegerArrayTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetIntegerArrayTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit IntegerArrayTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit IntegerArrayTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/IntegerArrayTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerArrayTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerArrayTopic.inc
new file mode 100644
index 0000000..ba3d00d
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerArrayTopic.inc
@@ -0,0 +1,137 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/IntegerArrayTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline IntegerArraySubscriber::IntegerArraySubscriber(
+    NT_Subscriber handle, std::span<const int64_t> defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue.begin(), defaultValue.end()} {}
+
+inline std::vector<int64_t> IntegerArraySubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline std::vector<int64_t> IntegerArraySubscriber::Get(
+    std::span<const int64_t> defaultValue) const {
+  return ::nt::GetIntegerArray(m_subHandle, defaultValue);
+}
+
+inline std::span<int64_t> IntegerArraySubscriber::Get(wpi::SmallVectorImpl<int64_t>& buf) const {
+  return Get(buf, m_defaultValue);
+}
+
+inline std::span<int64_t> IntegerArraySubscriber::Get(wpi::SmallVectorImpl<int64_t>& buf, std::span<const int64_t> defaultValue) const {
+  return nt::GetIntegerArray(m_subHandle, buf, defaultValue);
+}
+
+inline TimestampedIntegerArray IntegerArraySubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedIntegerArray IntegerArraySubscriber::GetAtomic(
+    std::span<const int64_t> defaultValue) const {
+  return ::nt::GetAtomicIntegerArray(m_subHandle, defaultValue);
+}
+
+inline TimestampedIntegerArrayView IntegerArraySubscriber::GetAtomic(wpi::SmallVectorImpl<int64_t>& buf) const {
+  return GetAtomic(buf, m_defaultValue);
+}
+
+inline TimestampedIntegerArrayView IntegerArraySubscriber::GetAtomic(wpi::SmallVectorImpl<int64_t>& buf, std::span<const int64_t> defaultValue) const {
+  return nt::GetAtomicIntegerArray(m_subHandle, buf, defaultValue);
+}
+
+inline std::vector<TimestampedIntegerArray>
+IntegerArraySubscriber::ReadQueue() {
+  return ::nt::ReadQueueIntegerArray(m_subHandle);
+}
+
+inline IntegerArrayTopic IntegerArraySubscriber::GetTopic() const {
+  return IntegerArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline IntegerArrayPublisher::IntegerArrayPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void IntegerArrayPublisher::Set(std::span<const int64_t> value,
+                                         int64_t time) {
+  ::nt::SetIntegerArray(m_pubHandle, value, time);
+}
+
+inline void IntegerArrayPublisher::SetDefault(std::span<const int64_t> value) {
+  ::nt::SetDefaultIntegerArray(m_pubHandle, value);
+}
+
+inline IntegerArrayTopic IntegerArrayPublisher::GetTopic() const {
+  return IntegerArrayTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline IntegerArrayEntry::IntegerArrayEntry(
+    NT_Entry handle, std::span<const int64_t> defaultValue)
+    : IntegerArraySubscriber{handle, defaultValue},
+      IntegerArrayPublisher{handle} {}
+
+inline IntegerArrayTopic IntegerArrayEntry::GetTopic() const {
+  return IntegerArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void IntegerArrayEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline IntegerArraySubscriber IntegerArrayTopic::Subscribe(
+    std::span<const int64_t> defaultValue,
+    const PubSubOptions& options) {
+  return IntegerArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_INTEGER_ARRAY, "int[]", options),
+      defaultValue};
+}
+inline IntegerArraySubscriber IntegerArrayTopic::SubscribeEx(
+    std::string_view typeString, std::span<const int64_t> defaultValue,
+    const PubSubOptions& options) {
+  return IntegerArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_INTEGER_ARRAY, typeString, options),
+      defaultValue};
+}
+
+inline IntegerArrayPublisher IntegerArrayTopic::Publish(
+    const PubSubOptions& options) {
+  return IntegerArrayPublisher{
+      ::nt::Publish(m_handle, NT_INTEGER_ARRAY, "int[]", options)};
+}
+
+inline IntegerArrayPublisher IntegerArrayTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return IntegerArrayPublisher{
+      ::nt::PublishEx(m_handle, NT_INTEGER_ARRAY, typeString, properties, options)};
+}
+
+inline IntegerArrayEntry IntegerArrayTopic::GetEntry(
+    std::span<const int64_t> defaultValue,
+    const PubSubOptions& options) {
+  return IntegerArrayEntry{
+      ::nt::GetEntry(m_handle, NT_INTEGER_ARRAY, "int[]", options),
+      defaultValue};
+}
+inline IntegerArrayEntry IntegerArrayTopic::GetEntryEx(
+    std::string_view typeString, std::span<const int64_t> defaultValue,
+    const PubSubOptions& options) {
+  return IntegerArrayEntry{
+      ::nt::GetEntry(m_handle, NT_INTEGER_ARRAY, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerTopic.h
new file mode 100644
index 0000000..0d5ab21
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerTopic.h
@@ -0,0 +1,373 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class IntegerTopic;
+
+/**
+ * NetworkTables Integer subscriber.
+ */
+class IntegerSubscriber : public Subscriber {
+ public:
+  using TopicType = IntegerTopic;
+  using ValueType = int64_t;
+  using ParamType = int64_t;
+  using TimestampedValueType = TimestampedInteger;
+
+
+  IntegerSubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * IntegerTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  IntegerSubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables Integer publisher.
+ */
+class IntegerPublisher : public Publisher {
+ public:
+  using TopicType = IntegerTopic;
+  using ValueType = int64_t;
+  using ParamType = int64_t;
+
+  using TimestampedValueType = TimestampedInteger;
+
+  IntegerPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * IntegerTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit IntegerPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables Integer entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class IntegerEntry final : public IntegerSubscriber,
+                                  public IntegerPublisher {
+ public:
+  using SubscriberType = IntegerSubscriber;
+  using PublisherType = IntegerPublisher;
+  using TopicType = IntegerTopic;
+  using ValueType = int64_t;
+  using ParamType = int64_t;
+
+  using TimestampedValueType = TimestampedInteger;
+
+  IntegerEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * IntegerTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  IntegerEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables Integer topic.
+ */
+class IntegerTopic final : public Topic {
+ public:
+  using SubscriberType = IntegerSubscriber;
+  using PublisherType = IntegerPublisher;
+  using EntryType = IntegerEntry;
+  using ValueType = int64_t;
+  using ParamType = int64_t;
+  using TimestampedValueType = TimestampedInteger;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "int";
+
+  IntegerTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetIntegerTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit IntegerTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit IntegerTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/IntegerTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerTopic.inc
new file mode 100644
index 0000000..2ca31e5
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/IntegerTopic.inc
@@ -0,0 +1,121 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/IntegerTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline IntegerSubscriber::IntegerSubscriber(
+    NT_Subscriber handle, int64_t defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue} {}
+
+inline int64_t IntegerSubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline int64_t IntegerSubscriber::Get(
+    int64_t defaultValue) const {
+  return ::nt::GetInteger(m_subHandle, defaultValue);
+}
+
+inline TimestampedInteger IntegerSubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedInteger IntegerSubscriber::GetAtomic(
+    int64_t defaultValue) const {
+  return ::nt::GetAtomicInteger(m_subHandle, defaultValue);
+}
+
+inline std::vector<TimestampedInteger>
+IntegerSubscriber::ReadQueue() {
+  return ::nt::ReadQueueInteger(m_subHandle);
+}
+
+inline IntegerTopic IntegerSubscriber::GetTopic() const {
+  return IntegerTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline IntegerPublisher::IntegerPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void IntegerPublisher::Set(int64_t value,
+                                         int64_t time) {
+  ::nt::SetInteger(m_pubHandle, value, time);
+}
+
+inline void IntegerPublisher::SetDefault(int64_t value) {
+  ::nt::SetDefaultInteger(m_pubHandle, value);
+}
+
+inline IntegerTopic IntegerPublisher::GetTopic() const {
+  return IntegerTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline IntegerEntry::IntegerEntry(
+    NT_Entry handle, int64_t defaultValue)
+    : IntegerSubscriber{handle, defaultValue},
+      IntegerPublisher{handle} {}
+
+inline IntegerTopic IntegerEntry::GetTopic() const {
+  return IntegerTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void IntegerEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline IntegerSubscriber IntegerTopic::Subscribe(
+    int64_t defaultValue,
+    const PubSubOptions& options) {
+  return IntegerSubscriber{
+      ::nt::Subscribe(m_handle, NT_INTEGER, "int", options),
+      defaultValue};
+}
+inline IntegerSubscriber IntegerTopic::SubscribeEx(
+    std::string_view typeString, int64_t defaultValue,
+    const PubSubOptions& options) {
+  return IntegerSubscriber{
+      ::nt::Subscribe(m_handle, NT_INTEGER, typeString, options),
+      defaultValue};
+}
+
+inline IntegerPublisher IntegerTopic::Publish(
+    const PubSubOptions& options) {
+  return IntegerPublisher{
+      ::nt::Publish(m_handle, NT_INTEGER, "int", options)};
+}
+
+inline IntegerPublisher IntegerTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return IntegerPublisher{
+      ::nt::PublishEx(m_handle, NT_INTEGER, typeString, properties, options)};
+}
+
+inline IntegerEntry IntegerTopic::GetEntry(
+    int64_t defaultValue,
+    const PubSubOptions& options) {
+  return IntegerEntry{
+      ::nt::GetEntry(m_handle, NT_INTEGER, "int", options),
+      defaultValue};
+}
+inline IntegerEntry IntegerTopic::GetEntryEx(
+    std::string_view typeString, int64_t defaultValue,
+    const PubSubOptions& options) {
+  return IntegerEntry{
+      ::nt::GetEntry(m_handle, NT_INTEGER, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/RawTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/RawTopic.h
new file mode 100644
index 0000000..e124fe1
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/RawTopic.h
@@ -0,0 +1,384 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <utility>
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class RawTopic;
+
+/**
+ * NetworkTables Raw subscriber.
+ */
+class RawSubscriber : public Subscriber {
+ public:
+  using TopicType = RawTopic;
+  using ValueType = std::vector<uint8_t>;
+  using ParamType = std::span<const uint8_t>;
+  using TimestampedValueType = TimestampedRaw;
+
+  using SmallRetType = std::span<uint8_t>;
+  using SmallElemType = uint8_t;
+  using TimestampedValueViewType = TimestampedRawView;
+
+
+  RawSubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * RawTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  RawSubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @param buf storage for returned value
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf,
+      ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables Raw publisher.
+ */
+class RawPublisher : public Publisher {
+ public:
+  using TopicType = RawTopic;
+  using ValueType = std::vector<uint8_t>;
+  using ParamType = std::span<const uint8_t>;
+
+  using SmallRetType = std::span<uint8_t>;
+  using SmallElemType = uint8_t;
+
+  using TimestampedValueType = TimestampedRaw;
+
+  RawPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * RawTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit RawPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables Raw entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class RawEntry final : public RawSubscriber,
+                                  public RawPublisher {
+ public:
+  using SubscriberType = RawSubscriber;
+  using PublisherType = RawPublisher;
+  using TopicType = RawTopic;
+  using ValueType = std::vector<uint8_t>;
+  using ParamType = std::span<const uint8_t>;
+
+  using SmallRetType = std::span<uint8_t>;
+  using SmallElemType = uint8_t;
+
+  using TimestampedValueType = TimestampedRaw;
+
+  RawEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * RawTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  RawEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables Raw topic.
+ */
+class RawTopic final : public Topic {
+ public:
+  using SubscriberType = RawSubscriber;
+  using PublisherType = RawPublisher;
+  using EntryType = RawEntry;
+  using ValueType = std::vector<uint8_t>;
+  using ParamType = std::span<const uint8_t>;
+  using TimestampedValueType = TimestampedRaw;
+
+  RawTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetRawTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit RawTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit RawTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(std::string_view typeString, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(std::string_view typeString, ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+};
+
+}  // namespace nt
+
+#include "networktables/RawTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/RawTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/RawTopic.inc
new file mode 100644
index 0000000..b31ebcb
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/RawTopic.inc
@@ -0,0 +1,121 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/RawTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline RawSubscriber::RawSubscriber(
+    NT_Subscriber handle, std::span<const uint8_t> defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue.begin(), defaultValue.end()} {}
+
+inline std::vector<uint8_t> RawSubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline std::vector<uint8_t> RawSubscriber::Get(
+    std::span<const uint8_t> defaultValue) const {
+  return ::nt::GetRaw(m_subHandle, defaultValue);
+}
+
+inline std::span<uint8_t> RawSubscriber::Get(wpi::SmallVectorImpl<uint8_t>& buf) const {
+  return Get(buf, m_defaultValue);
+}
+
+inline std::span<uint8_t> RawSubscriber::Get(wpi::SmallVectorImpl<uint8_t>& buf, std::span<const uint8_t> defaultValue) const {
+  return nt::GetRaw(m_subHandle, buf, defaultValue);
+}
+
+inline TimestampedRaw RawSubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedRaw RawSubscriber::GetAtomic(
+    std::span<const uint8_t> defaultValue) const {
+  return ::nt::GetAtomicRaw(m_subHandle, defaultValue);
+}
+
+inline TimestampedRawView RawSubscriber::GetAtomic(wpi::SmallVectorImpl<uint8_t>& buf) const {
+  return GetAtomic(buf, m_defaultValue);
+}
+
+inline TimestampedRawView RawSubscriber::GetAtomic(wpi::SmallVectorImpl<uint8_t>& buf, std::span<const uint8_t> defaultValue) const {
+  return nt::GetAtomicRaw(m_subHandle, buf, defaultValue);
+}
+
+inline std::vector<TimestampedRaw>
+RawSubscriber::ReadQueue() {
+  return ::nt::ReadQueueRaw(m_subHandle);
+}
+
+inline RawTopic RawSubscriber::GetTopic() const {
+  return RawTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline RawPublisher::RawPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void RawPublisher::Set(std::span<const uint8_t> value,
+                                         int64_t time) {
+  ::nt::SetRaw(m_pubHandle, value, time);
+}
+
+inline void RawPublisher::SetDefault(std::span<const uint8_t> value) {
+  ::nt::SetDefaultRaw(m_pubHandle, value);
+}
+
+inline RawTopic RawPublisher::GetTopic() const {
+  return RawTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline RawEntry::RawEntry(
+    NT_Entry handle, std::span<const uint8_t> defaultValue)
+    : RawSubscriber{handle, defaultValue},
+      RawPublisher{handle} {}
+
+inline RawTopic RawEntry::GetTopic() const {
+  return RawTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void RawEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline RawSubscriber RawTopic::Subscribe(
+    std::string_view typeString, std::span<const uint8_t> defaultValue,
+    const PubSubOptions& options) {
+  return RawSubscriber{
+      ::nt::Subscribe(m_handle, NT_RAW, typeString, options),
+      defaultValue};
+}
+inline RawPublisher RawTopic::Publish(
+    std::string_view typeString, const PubSubOptions& options) {
+  return RawPublisher{
+      ::nt::Publish(m_handle, NT_RAW, typeString, options)};
+}
+
+inline RawPublisher RawTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return RawPublisher{
+      ::nt::PublishEx(m_handle, NT_RAW, typeString, properties, options)};
+}
+
+inline RawEntry RawTopic::GetEntry(
+    std::string_view typeString, std::span<const uint8_t> defaultValue,
+    const PubSubOptions& options) {
+  return RawEntry{
+      ::nt::GetEntry(m_handle, NT_RAW, typeString, options),
+      defaultValue};
+}
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringArrayTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringArrayTopic.h
new file mode 100644
index 0000000..9339521
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringArrayTopic.h
@@ -0,0 +1,373 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <utility>
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class StringArrayTopic;
+
+/**
+ * NetworkTables StringArray subscriber.
+ */
+class StringArraySubscriber : public Subscriber {
+ public:
+  using TopicType = StringArrayTopic;
+  using ValueType = std::vector<std::string>;
+  using ParamType = std::span<const std::string>;
+  using TimestampedValueType = TimestampedStringArray;
+
+
+  StringArraySubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * StringArrayTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  StringArraySubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables StringArray publisher.
+ */
+class StringArrayPublisher : public Publisher {
+ public:
+  using TopicType = StringArrayTopic;
+  using ValueType = std::vector<std::string>;
+  using ParamType = std::span<const std::string>;
+
+  using TimestampedValueType = TimestampedStringArray;
+
+  StringArrayPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * StringArrayTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit StringArrayPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables StringArray entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class StringArrayEntry final : public StringArraySubscriber,
+                                  public StringArrayPublisher {
+ public:
+  using SubscriberType = StringArraySubscriber;
+  using PublisherType = StringArrayPublisher;
+  using TopicType = StringArrayTopic;
+  using ValueType = std::vector<std::string>;
+  using ParamType = std::span<const std::string>;
+
+  using TimestampedValueType = TimestampedStringArray;
+
+  StringArrayEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * StringArrayTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  StringArrayEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables StringArray topic.
+ */
+class StringArrayTopic final : public Topic {
+ public:
+  using SubscriberType = StringArraySubscriber;
+  using PublisherType = StringArrayPublisher;
+  using EntryType = StringArrayEntry;
+  using ValueType = std::vector<std::string>;
+  using ParamType = std::span<const std::string>;
+  using TimestampedValueType = TimestampedStringArray;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "string[]";
+
+  StringArrayTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetStringArrayTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit StringArrayTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit StringArrayTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/StringArrayTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringArrayTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringArrayTopic.inc
new file mode 100644
index 0000000..f003ff0
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringArrayTopic.inc
@@ -0,0 +1,121 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/StringArrayTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline StringArraySubscriber::StringArraySubscriber(
+    NT_Subscriber handle, std::span<const std::string> defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue.begin(), defaultValue.end()} {}
+
+inline std::vector<std::string> StringArraySubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline std::vector<std::string> StringArraySubscriber::Get(
+    std::span<const std::string> defaultValue) const {
+  return ::nt::GetStringArray(m_subHandle, defaultValue);
+}
+
+inline TimestampedStringArray StringArraySubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedStringArray StringArraySubscriber::GetAtomic(
+    std::span<const std::string> defaultValue) const {
+  return ::nt::GetAtomicStringArray(m_subHandle, defaultValue);
+}
+
+inline std::vector<TimestampedStringArray>
+StringArraySubscriber::ReadQueue() {
+  return ::nt::ReadQueueStringArray(m_subHandle);
+}
+
+inline StringArrayTopic StringArraySubscriber::GetTopic() const {
+  return StringArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline StringArrayPublisher::StringArrayPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void StringArrayPublisher::Set(std::span<const std::string> value,
+                                         int64_t time) {
+  ::nt::SetStringArray(m_pubHandle, value, time);
+}
+
+inline void StringArrayPublisher::SetDefault(std::span<const std::string> value) {
+  ::nt::SetDefaultStringArray(m_pubHandle, value);
+}
+
+inline StringArrayTopic StringArrayPublisher::GetTopic() const {
+  return StringArrayTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline StringArrayEntry::StringArrayEntry(
+    NT_Entry handle, std::span<const std::string> defaultValue)
+    : StringArraySubscriber{handle, defaultValue},
+      StringArrayPublisher{handle} {}
+
+inline StringArrayTopic StringArrayEntry::GetTopic() const {
+  return StringArrayTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void StringArrayEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline StringArraySubscriber StringArrayTopic::Subscribe(
+    std::span<const std::string> defaultValue,
+    const PubSubOptions& options) {
+  return StringArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_STRING_ARRAY, "string[]", options),
+      defaultValue};
+}
+inline StringArraySubscriber StringArrayTopic::SubscribeEx(
+    std::string_view typeString, std::span<const std::string> defaultValue,
+    const PubSubOptions& options) {
+  return StringArraySubscriber{
+      ::nt::Subscribe(m_handle, NT_STRING_ARRAY, typeString, options),
+      defaultValue};
+}
+
+inline StringArrayPublisher StringArrayTopic::Publish(
+    const PubSubOptions& options) {
+  return StringArrayPublisher{
+      ::nt::Publish(m_handle, NT_STRING_ARRAY, "string[]", options)};
+}
+
+inline StringArrayPublisher StringArrayTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return StringArrayPublisher{
+      ::nt::PublishEx(m_handle, NT_STRING_ARRAY, typeString, properties, options)};
+}
+
+inline StringArrayEntry StringArrayTopic::GetEntry(
+    std::span<const std::string> defaultValue,
+    const PubSubOptions& options) {
+  return StringArrayEntry{
+      ::nt::GetEntry(m_handle, NT_STRING_ARRAY, "string[]", options),
+      defaultValue};
+}
+inline StringArrayEntry StringArrayTopic::GetEntryEx(
+    std::string_view typeString, std::span<const std::string> defaultValue,
+    const PubSubOptions& options) {
+  return StringArrayEntry{
+      ::nt::GetEntry(m_handle, NT_STRING_ARRAY, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringTopic.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringTopic.h
new file mode 100644
index 0000000..3e0ff87
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringTopic.h
@@ -0,0 +1,428 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <string>
+#include <string_view>
+#include <utility>
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include <wpi/json_fwd.h>
+
+#include "networktables/Topic.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+
+class StringTopic;
+
+/**
+ * NetworkTables String subscriber.
+ */
+class StringSubscriber : public Subscriber {
+ public:
+  using TopicType = StringTopic;
+  using ValueType = std::string;
+  using ParamType = std::string_view;
+  using TimestampedValueType = TimestampedString;
+
+  using SmallRetType = std::string_view;
+  using SmallElemType = char;
+  using TimestampedValueViewType = TimestampedStringView;
+
+
+  StringSubscriber() = default;
+
+  /**
+   * Construct from a subscriber handle; recommended to use
+   * StringTopic::Subscribe() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  StringSubscriber(NT_Subscriber handle, ParamType defaultValue);
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @return value
+   */
+  ValueType Get() const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  ValueType Get(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the stored default value.
+   *
+   * @param buf storage for returned value
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value.
+   * If no value has been published, returns the passed defaultValue.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return value
+   */
+  SmallRetType Get(wpi::SmallVectorImpl<SmallElemType>& buf, ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic() const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueType GetAtomic(ParamType defaultValue) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the stored default value and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf) const;
+
+  /**
+   * Get the last published value along with its timestamp.
+   * If no value has been published, returns the passed defaultValue and a
+   * timestamp of 0.
+   *
+   * @param buf storage for returned value
+   * @param defaultValue default value to return if no value has been published
+   * @return timestamped value
+   */
+  TimestampedValueViewType GetAtomic(
+      wpi::SmallVectorImpl<SmallElemType>& buf,
+      ParamType defaultValue) const;
+
+  /**
+   * Get an array of all value changes since the last call to ReadQueue.
+   * Also provides a timestamp for each value.
+   *
+   * @note The "poll storage" subscribe option can be used to set the queue
+   *     depth.
+   *
+   * @return Array of timestamped values; empty array if no new changes have
+   *     been published since the previous call.
+   */
+  std::vector<TimestampedValueType> ReadQueue();
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+ private:
+  ValueType m_defaultValue;
+};
+
+/**
+ * NetworkTables String publisher.
+ */
+class StringPublisher : public Publisher {
+ public:
+  using TopicType = StringTopic;
+  using ValueType = std::string;
+  using ParamType = std::string_view;
+
+  using SmallRetType = std::string_view;
+  using SmallElemType = char;
+
+  using TimestampedValueType = TimestampedString;
+
+  StringPublisher() = default;
+
+  /**
+   * Construct from a publisher handle; recommended to use
+   * StringTopic::Publish() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit StringPublisher(NT_Publisher handle);
+
+  /**
+   * Publish a new value.
+   *
+   * @param value value to publish
+   * @param time timestamp; 0 indicates current NT time should be used
+   */
+  void Set(ParamType value, int64_t time = 0);
+
+  /**
+   * Publish a default value.
+   * On reconnect, a default value will never be used in preference to a
+   * published value.
+   *
+   * @param value value
+   */
+  void SetDefault(ParamType value);
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+};
+
+/**
+ * NetworkTables String entry.
+ *
+ * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
+ */
+class StringEntry final : public StringSubscriber,
+                                  public StringPublisher {
+ public:
+  using SubscriberType = StringSubscriber;
+  using PublisherType = StringPublisher;
+  using TopicType = StringTopic;
+  using ValueType = std::string;
+  using ParamType = std::string_view;
+
+  using SmallRetType = std::string_view;
+  using SmallElemType = char;
+
+  using TimestampedValueType = TimestampedString;
+
+  StringEntry() = default;
+
+  /**
+   * Construct from an entry handle; recommended to use
+   * StringTopic::GetEntry() instead.
+   *
+   * @param handle Native handle
+   * @param defaultValue Default value
+   */
+  StringEntry(NT_Entry handle, ParamType defaultValue);
+
+  /**
+   * Determines if the native handle is valid.
+   *
+   * @return True if the native handle is valid, false otherwise.
+   */
+  explicit operator bool() const { return m_subHandle != 0; }
+
+  /**
+   * Gets the native handle for the entry.
+   *
+   * @return Native handle
+   */
+  NT_Entry GetHandle() const { return m_subHandle; }
+
+  /**
+   * Get the corresponding topic.
+   *
+   * @return Topic
+   */
+  TopicType GetTopic() const;
+
+  /**
+   * Stops publishing the entry if it's published.
+   */
+  void Unpublish();
+};
+
+/**
+ * NetworkTables String topic.
+ */
+class StringTopic final : public Topic {
+ public:
+  using SubscriberType = StringSubscriber;
+  using PublisherType = StringPublisher;
+  using EntryType = StringEntry;
+  using ValueType = std::string;
+  using ParamType = std::string_view;
+  using TimestampedValueType = TimestampedString;
+  /** The default type string for this topic type. */
+  static constexpr std::string_view kTypeString = "string";
+
+  StringTopic() = default;
+
+  /**
+   * Construct from a topic handle; recommended to use
+   * NetworkTableInstance::GetStringTopic() instead.
+   *
+   * @param handle Native handle
+   */
+  explicit StringTopic(NT_Topic handle) : Topic{handle} {}
+
+  /**
+   * Construct from a generic topic.
+   *
+   * @param topic Topic
+   */
+  explicit StringTopic(Topic topic) : Topic{topic} {}
+
+  /**
+   * Create a new subscriber to the topic.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType Subscribe(
+      ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new subscriber to the topic, with specific type string.
+   *
+   * <p>The subscriber is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note Subscribers that do not match the published data type do not return
+   *     any values. To determine if the data type matches, use the appropriate
+   *     Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options subscribe options
+   * @return subscriber
+   */
+  [[nodiscard]]
+  SubscriberType SubscribeEx(
+      std::string_view typeString, ParamType defaultValue,
+      const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new publisher to the topic, with type string and initial
+   * properties.
+   *
+   * The publisher is only active as long as the returned object
+   * is not destroyed.
+   *
+   * @note It is not possible to publish two different data types to the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored). To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param properties JSON properties
+   * @param options publish options
+   * @return publisher
+   */
+  [[nodiscard]]
+  PublisherType PublishEx(std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options = kDefaultPubSubOptions);
+
+  /**
+   * Create a new entry for the topic.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntry(ParamType defaultValue,
+                     const PubSubOptions& options = kDefaultPubSubOptions);
+  /**
+   * Create a new entry for the topic, with specific type string.
+   *
+   * Entries act as a combination of a subscriber and a weak publisher. The
+   * subscriber is active as long as the entry is not destroyed. The publisher
+   * is created when the entry is first written to, and remains active until
+   * either Unpublish() is called or the entry is destroyed.
+   *
+   * @note It is not possible to use two different data types with the same
+   *     topic. Conflicts between publishers are typically resolved by the
+   *     server on a first-come, first-served basis. Any published values that
+   *     do not match the topic's data type are dropped (ignored), and the entry
+   *     will show no new values if the data type does not match. To determine
+   *     if the data type matches, use the appropriate Topic functions.
+   *
+   * @param typeString type string
+   * @param defaultValue default value used when a default is not provided to a
+   *        getter function
+   * @param options publish and/or subscribe options
+   * @return entry
+   */
+  [[nodiscard]]
+  EntryType GetEntryEx(std::string_view typeString, ParamType defaultValue,
+                       const PubSubOptions& options = kDefaultPubSubOptions);
+
+};
+
+}  // namespace nt
+
+#include "networktables/StringTopic.inc"
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringTopic.inc b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringTopic.inc
new file mode 100644
index 0000000..a1934e4
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/networktables/StringTopic.inc
@@ -0,0 +1,137 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <vector>
+
+#include "networktables/StringTopic.h"
+#include "networktables/NetworkTableType.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline StringSubscriber::StringSubscriber(
+    NT_Subscriber handle, std::string_view defaultValue)
+    : Subscriber{handle},
+      m_defaultValue{defaultValue} {}
+
+inline std::string StringSubscriber::Get() const {
+  return Get(m_defaultValue);
+}
+
+inline std::string StringSubscriber::Get(
+    std::string_view defaultValue) const {
+  return ::nt::GetString(m_subHandle, defaultValue);
+}
+
+inline std::string_view StringSubscriber::Get(wpi::SmallVectorImpl<char>& buf) const {
+  return Get(buf, m_defaultValue);
+}
+
+inline std::string_view StringSubscriber::Get(wpi::SmallVectorImpl<char>& buf, std::string_view defaultValue) const {
+  return nt::GetString(m_subHandle, buf, defaultValue);
+}
+
+inline TimestampedString StringSubscriber::GetAtomic() const {
+  return GetAtomic(m_defaultValue);
+}
+
+inline TimestampedString StringSubscriber::GetAtomic(
+    std::string_view defaultValue) const {
+  return ::nt::GetAtomicString(m_subHandle, defaultValue);
+}
+
+inline TimestampedStringView StringSubscriber::GetAtomic(wpi::SmallVectorImpl<char>& buf) const {
+  return GetAtomic(buf, m_defaultValue);
+}
+
+inline TimestampedStringView StringSubscriber::GetAtomic(wpi::SmallVectorImpl<char>& buf, std::string_view defaultValue) const {
+  return nt::GetAtomicString(m_subHandle, buf, defaultValue);
+}
+
+inline std::vector<TimestampedString>
+StringSubscriber::ReadQueue() {
+  return ::nt::ReadQueueString(m_subHandle);
+}
+
+inline StringTopic StringSubscriber::GetTopic() const {
+  return StringTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline StringPublisher::StringPublisher(NT_Publisher handle)
+    : Publisher{handle} {}
+
+inline void StringPublisher::Set(std::string_view value,
+                                         int64_t time) {
+  ::nt::SetString(m_pubHandle, value, time);
+}
+
+inline void StringPublisher::SetDefault(std::string_view value) {
+  ::nt::SetDefaultString(m_pubHandle, value);
+}
+
+inline StringTopic StringPublisher::GetTopic() const {
+  return StringTopic{::nt::GetTopicFromHandle(m_pubHandle)};
+}
+
+inline StringEntry::StringEntry(
+    NT_Entry handle, std::string_view defaultValue)
+    : StringSubscriber{handle, defaultValue},
+      StringPublisher{handle} {}
+
+inline StringTopic StringEntry::GetTopic() const {
+  return StringTopic{::nt::GetTopicFromHandle(m_subHandle)};
+}
+
+inline void StringEntry::Unpublish() {
+  ::nt::Unpublish(m_pubHandle);
+}
+
+inline StringSubscriber StringTopic::Subscribe(
+    std::string_view defaultValue,
+    const PubSubOptions& options) {
+  return StringSubscriber{
+      ::nt::Subscribe(m_handle, NT_STRING, "string", options),
+      defaultValue};
+}
+inline StringSubscriber StringTopic::SubscribeEx(
+    std::string_view typeString, std::string_view defaultValue,
+    const PubSubOptions& options) {
+  return StringSubscriber{
+      ::nt::Subscribe(m_handle, NT_STRING, typeString, options),
+      defaultValue};
+}
+
+inline StringPublisher StringTopic::Publish(
+    const PubSubOptions& options) {
+  return StringPublisher{
+      ::nt::Publish(m_handle, NT_STRING, "string", options)};
+}
+
+inline StringPublisher StringTopic::PublishEx(
+    std::string_view typeString,
+    const wpi::json& properties, const PubSubOptions& options) {
+  return StringPublisher{
+      ::nt::PublishEx(m_handle, NT_STRING, typeString, properties, options)};
+}
+
+inline StringEntry StringTopic::GetEntry(
+    std::string_view defaultValue,
+    const PubSubOptions& options) {
+  return StringEntry{
+      ::nt::GetEntry(m_handle, NT_STRING, "string", options),
+      defaultValue};
+}
+inline StringEntry StringTopic::GetEntryEx(
+    std::string_view typeString, std::string_view defaultValue,
+    const PubSubOptions& options) {
+  return StringEntry{
+      ::nt::GetEntry(m_handle, NT_STRING, typeString, options),
+      defaultValue};
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/ntcore_c_types.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/ntcore_c_types.h
new file mode 100644
index 0000000..3b95a09
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/ntcore_c_types.h
@@ -0,0 +1,1245 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include "ntcore_c.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * Timestamped Boolean.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedBoolean {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  NT_Bool value;
+};
+
+/**
+ * @defgroup ntcore_Boolean_cfunc Boolean Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ */
+NT_Bool NT_SetBoolean(NT_Handle pubentry, int64_t time, NT_Bool value);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+NT_Bool NT_SetDefaultBoolean(NT_Handle pubentry, NT_Bool defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+NT_Bool NT_GetBoolean(NT_Handle subentry, NT_Bool defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicBoolean(NT_Handle subentry, NT_Bool defaultValue, struct NT_TimestampedBoolean* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicBoolean).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedBoolean(struct NT_TimestampedBoolean* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedBoolean* NT_ReadQueueBoolean(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueBoolean).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueBoolean(struct NT_TimestampedBoolean* arr, size_t len);
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+NT_Bool* NT_ReadQueueValuesBoolean(NT_Handle subentry, size_t* len);
+
+/** @} */
+
+/**
+ * Timestamped Integer.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedInteger {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  int64_t value;
+};
+
+/**
+ * @defgroup ntcore_Integer_cfunc Integer Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ */
+NT_Bool NT_SetInteger(NT_Handle pubentry, int64_t time, int64_t value);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+NT_Bool NT_SetDefaultInteger(NT_Handle pubentry, int64_t defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+int64_t NT_GetInteger(NT_Handle subentry, int64_t defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicInteger(NT_Handle subentry, int64_t defaultValue, struct NT_TimestampedInteger* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicInteger).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedInteger(struct NT_TimestampedInteger* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedInteger* NT_ReadQueueInteger(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueInteger).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueInteger(struct NT_TimestampedInteger* arr, size_t len);
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+int64_t* NT_ReadQueueValuesInteger(NT_Handle subentry, size_t* len);
+
+/** @} */
+
+/**
+ * Timestamped Float.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedFloat {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  float value;
+};
+
+/**
+ * @defgroup ntcore_Float_cfunc Float Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ */
+NT_Bool NT_SetFloat(NT_Handle pubentry, int64_t time, float value);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+NT_Bool NT_SetDefaultFloat(NT_Handle pubentry, float defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+float NT_GetFloat(NT_Handle subentry, float defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicFloat(NT_Handle subentry, float defaultValue, struct NT_TimestampedFloat* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicFloat).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedFloat(struct NT_TimestampedFloat* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedFloat* NT_ReadQueueFloat(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueFloat).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueFloat(struct NT_TimestampedFloat* arr, size_t len);
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+float* NT_ReadQueueValuesFloat(NT_Handle subentry, size_t* len);
+
+/** @} */
+
+/**
+ * Timestamped Double.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedDouble {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  double value;
+};
+
+/**
+ * @defgroup ntcore_Double_cfunc Double Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ */
+NT_Bool NT_SetDouble(NT_Handle pubentry, int64_t time, double value);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+NT_Bool NT_SetDefaultDouble(NT_Handle pubentry, double defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+double NT_GetDouble(NT_Handle subentry, double defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicDouble(NT_Handle subentry, double defaultValue, struct NT_TimestampedDouble* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicDouble).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedDouble(struct NT_TimestampedDouble* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedDouble* NT_ReadQueueDouble(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueDouble).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueDouble(struct NT_TimestampedDouble* arr, size_t len);
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+double* NT_ReadQueueValuesDouble(NT_Handle subentry, size_t* len);
+
+/** @} */
+
+/**
+ * Timestamped String.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedString {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  char* value;
+  /**
+   * Value length.
+   */
+  size_t len;
+
+};
+
+/**
+ * @defgroup ntcore_String_cfunc String Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ * @param len length of value
+
+ */
+NT_Bool NT_SetString(NT_Handle pubentry, int64_t time, const char* value, size_t len);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ * @param defaultValueLen length of default value
+
+ */
+NT_Bool NT_SetDefaultString(NT_Handle pubentry, const char* defaultValue, size_t defaultValueLen);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+
+ * @return value
+ */
+char* NT_GetString(NT_Handle subentry, const char* defaultValue, size_t defaultValueLen, size_t* len);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicString(NT_Handle subentry, const char* defaultValue, size_t defaultValueLen, struct NT_TimestampedString* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicString).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedString(struct NT_TimestampedString* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedString* NT_ReadQueueString(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueString).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueString(struct NT_TimestampedString* arr, size_t len);
+
+/** @} */
+
+/**
+ * Timestamped Raw.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedRaw {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  uint8_t* value;
+  /**
+   * Value length.
+   */
+  size_t len;
+
+};
+
+/**
+ * @defgroup ntcore_Raw_cfunc Raw Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ * @param len length of value
+
+ */
+NT_Bool NT_SetRaw(NT_Handle pubentry, int64_t time, const uint8_t* value, size_t len);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ * @param defaultValueLen length of default value
+
+ */
+NT_Bool NT_SetDefaultRaw(NT_Handle pubentry, const uint8_t* defaultValue, size_t defaultValueLen);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+
+ * @return value
+ */
+uint8_t* NT_GetRaw(NT_Handle subentry, const uint8_t* defaultValue, size_t defaultValueLen, size_t* len);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicRaw(NT_Handle subentry, const uint8_t* defaultValue, size_t defaultValueLen, struct NT_TimestampedRaw* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicRaw).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedRaw(struct NT_TimestampedRaw* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedRaw* NT_ReadQueueRaw(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueRaw).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueRaw(struct NT_TimestampedRaw* arr, size_t len);
+
+/** @} */
+
+/**
+ * Timestamped BooleanArray.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedBooleanArray {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  NT_Bool* value;
+  /**
+   * Value length.
+   */
+  size_t len;
+
+};
+
+/**
+ * @defgroup ntcore_BooleanArray_cfunc BooleanArray Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ * @param len length of value
+
+ */
+NT_Bool NT_SetBooleanArray(NT_Handle pubentry, int64_t time, const NT_Bool* value, size_t len);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ * @param defaultValueLen length of default value
+
+ */
+NT_Bool NT_SetDefaultBooleanArray(NT_Handle pubentry, const NT_Bool* defaultValue, size_t defaultValueLen);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+
+ * @return value
+ */
+NT_Bool* NT_GetBooleanArray(NT_Handle subentry, const NT_Bool* defaultValue, size_t defaultValueLen, size_t* len);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicBooleanArray(NT_Handle subentry, const NT_Bool* defaultValue, size_t defaultValueLen, struct NT_TimestampedBooleanArray* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicBooleanArray).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedBooleanArray(struct NT_TimestampedBooleanArray* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedBooleanArray* NT_ReadQueueBooleanArray(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueBooleanArray).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueBooleanArray(struct NT_TimestampedBooleanArray* arr, size_t len);
+
+/** @} */
+
+/**
+ * Timestamped IntegerArray.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedIntegerArray {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  int64_t* value;
+  /**
+   * Value length.
+   */
+  size_t len;
+
+};
+
+/**
+ * @defgroup ntcore_IntegerArray_cfunc IntegerArray Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ * @param len length of value
+
+ */
+NT_Bool NT_SetIntegerArray(NT_Handle pubentry, int64_t time, const int64_t* value, size_t len);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ * @param defaultValueLen length of default value
+
+ */
+NT_Bool NT_SetDefaultIntegerArray(NT_Handle pubentry, const int64_t* defaultValue, size_t defaultValueLen);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+
+ * @return value
+ */
+int64_t* NT_GetIntegerArray(NT_Handle subentry, const int64_t* defaultValue, size_t defaultValueLen, size_t* len);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicIntegerArray(NT_Handle subentry, const int64_t* defaultValue, size_t defaultValueLen, struct NT_TimestampedIntegerArray* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicIntegerArray).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedIntegerArray(struct NT_TimestampedIntegerArray* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedIntegerArray* NT_ReadQueueIntegerArray(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueIntegerArray).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueIntegerArray(struct NT_TimestampedIntegerArray* arr, size_t len);
+
+/** @} */
+
+/**
+ * Timestamped FloatArray.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedFloatArray {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  float* value;
+  /**
+   * Value length.
+   */
+  size_t len;
+
+};
+
+/**
+ * @defgroup ntcore_FloatArray_cfunc FloatArray Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ * @param len length of value
+
+ */
+NT_Bool NT_SetFloatArray(NT_Handle pubentry, int64_t time, const float* value, size_t len);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ * @param defaultValueLen length of default value
+
+ */
+NT_Bool NT_SetDefaultFloatArray(NT_Handle pubentry, const float* defaultValue, size_t defaultValueLen);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+
+ * @return value
+ */
+float* NT_GetFloatArray(NT_Handle subentry, const float* defaultValue, size_t defaultValueLen, size_t* len);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicFloatArray(NT_Handle subentry, const float* defaultValue, size_t defaultValueLen, struct NT_TimestampedFloatArray* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicFloatArray).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedFloatArray(struct NT_TimestampedFloatArray* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedFloatArray* NT_ReadQueueFloatArray(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueFloatArray).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueFloatArray(struct NT_TimestampedFloatArray* arr, size_t len);
+
+/** @} */
+
+/**
+ * Timestamped DoubleArray.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedDoubleArray {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  double* value;
+  /**
+   * Value length.
+   */
+  size_t len;
+
+};
+
+/**
+ * @defgroup ntcore_DoubleArray_cfunc DoubleArray Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ * @param len length of value
+
+ */
+NT_Bool NT_SetDoubleArray(NT_Handle pubentry, int64_t time, const double* value, size_t len);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ * @param defaultValueLen length of default value
+
+ */
+NT_Bool NT_SetDefaultDoubleArray(NT_Handle pubentry, const double* defaultValue, size_t defaultValueLen);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+
+ * @return value
+ */
+double* NT_GetDoubleArray(NT_Handle subentry, const double* defaultValue, size_t defaultValueLen, size_t* len);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicDoubleArray(NT_Handle subentry, const double* defaultValue, size_t defaultValueLen, struct NT_TimestampedDoubleArray* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicDoubleArray).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedDoubleArray(struct NT_TimestampedDoubleArray* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedDoubleArray* NT_ReadQueueDoubleArray(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueDoubleArray).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueDoubleArray(struct NT_TimestampedDoubleArray* arr, size_t len);
+
+/** @} */
+
+/**
+ * Timestamped StringArray.
+ * @ingroup ntcore_c_api
+ */
+struct NT_TimestampedStringArray {
+  /**
+   * Time in local time base.
+   */
+  int64_t time;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime;
+
+  /**
+   * Value.
+   */
+  struct NT_String* value;
+  /**
+   * Value length.
+   */
+  size_t len;
+
+};
+
+/**
+ * @defgroup ntcore_StringArray_cfunc StringArray Functions
+ * @ingroup ntcore_c_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param time timestamp; 0 indicates current NT time should be used
+ * @param value value to publish
+ * @param len length of value
+
+ */
+NT_Bool NT_SetStringArray(NT_Handle pubentry, int64_t time, const struct NT_String* value, size_t len);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ * @param defaultValueLen length of default value
+
+ */
+NT_Bool NT_SetDefaultStringArray(NT_Handle pubentry, const struct NT_String* defaultValue, size_t defaultValueLen);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+ * @param len length of returned value (output)
+
+ * @return value
+ */
+struct NT_String* NT_GetStringArray(NT_Handle subentry, const struct NT_String* defaultValue, size_t defaultValueLen, size_t* len);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @param defaultValueLen length of default value
+
+ * @param value timestamped value (output)
+ */
+void NT_GetAtomicStringArray(NT_Handle subentry, const struct NT_String* defaultValue, size_t defaultValueLen, struct NT_TimestampedStringArray* value);
+
+/**
+ * Disposes a timestamped value (as returned by NT_GetAtomicStringArray).
+ *
+ * @param value timestamped value
+ */
+void NT_DisposeTimestampedStringArray(struct NT_TimestampedStringArray* value);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @param len length of returned array (output)
+ * @return Array of timestamped values; NULL if no new changes have
+ *     been published since the previous call.
+ */
+struct NT_TimestampedStringArray* NT_ReadQueueStringArray(NT_Handle subentry, size_t* len);
+
+/**
+ * Frees a timestamped array of values (as returned by NT_ReadQueueStringArray).
+ *
+ * @param arr array
+ * @param len length of array
+ */
+void NT_FreeQueueStringArray(struct NT_TimestampedStringArray* arr, size_t len);
+
+/** @} */
+
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
diff --git a/third_party/allwpilib/ntcore/src/generated/main/native/include/ntcore_cpp_types.h b/third_party/allwpilib/ntcore/src/generated/main/native/include/ntcore_cpp_types.h
new file mode 100644
index 0000000..c0cac5e
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/generated/main/native/include/ntcore_cpp_types.h
@@ -0,0 +1,998 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./ntcore/generate_topics.py. DO NOT MODIFY
+
+#pragma once
+
+#include <stdint.h>
+
+#include <span>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include "ntcore_c.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace nt {
+/**
+ * Timestamped value.
+ * @ingroup ntcore_cpp_handle_api
+ */
+template <typename T>
+struct Timestamped {
+  Timestamped() = default;
+  Timestamped(int64_t time, int64_t serverTime, T value)
+    : time{time}, serverTime{serverTime}, value{std::move(value)} {}
+
+  /**
+   * Time in local time base.
+   */
+  int64_t time = 0;
+
+  /**
+   * Time in server time base.  May be 0 or 1 for locally set values.
+   */
+  int64_t serverTime = 0;
+
+  /**
+   * Value.
+   */
+  T value = {};
+};
+
+/**
+ * Timestamped Boolean.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedBoolean = Timestamped<bool>;
+
+/**
+ * @defgroup ntcore_Boolean_func Boolean Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetBoolean(NT_Handle pubentry, bool value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultBoolean(NT_Handle pubentry, bool defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+bool GetBoolean(NT_Handle subentry, bool defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedBoolean GetAtomicBoolean(NT_Handle subentry, bool defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedBoolean> ReadQueueBoolean(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<int> ReadQueueValuesBoolean(NT_Handle subentry);
+
+/** @} */
+
+/**
+ * Timestamped Integer.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedInteger = Timestamped<int64_t>;
+
+/**
+ * @defgroup ntcore_Integer_func Integer Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetInteger(NT_Handle pubentry, int64_t value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultInteger(NT_Handle pubentry, int64_t defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+int64_t GetInteger(NT_Handle subentry, int64_t defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedInteger GetAtomicInteger(NT_Handle subentry, int64_t defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedInteger> ReadQueueInteger(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<int64_t> ReadQueueValuesInteger(NT_Handle subentry);
+
+/** @} */
+
+/**
+ * Timestamped Float.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedFloat = Timestamped<float>;
+
+/**
+ * @defgroup ntcore_Float_func Float Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetFloat(NT_Handle pubentry, float value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultFloat(NT_Handle pubentry, float defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+float GetFloat(NT_Handle subentry, float defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedFloat GetAtomicFloat(NT_Handle subentry, float defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedFloat> ReadQueueFloat(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<float> ReadQueueValuesFloat(NT_Handle subentry);
+
+/** @} */
+
+/**
+ * Timestamped Double.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedDouble = Timestamped<double>;
+
+/**
+ * @defgroup ntcore_Double_func Double Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetDouble(NT_Handle pubentry, double value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultDouble(NT_Handle pubentry, double defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+double GetDouble(NT_Handle subentry, double defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedDouble GetAtomicDouble(NT_Handle subentry, double defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedDouble> ReadQueueDouble(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<double> ReadQueueValuesDouble(NT_Handle subentry);
+
+/** @} */
+
+/**
+ * Timestamped String.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedString = Timestamped<std::string>;
+
+/**
+ * Timestamped String view (for SmallVector-taking functions).
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedStringView = Timestamped<std::string_view>;
+
+/**
+ * @defgroup ntcore_String_func String Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetString(NT_Handle pubentry, std::string_view value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultString(NT_Handle pubentry, std::string_view defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+std::string GetString(NT_Handle subentry, std::string_view defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedString GetAtomicString(NT_Handle subentry, std::string_view defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedString> ReadQueueString(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<std::string> ReadQueueValuesString(NT_Handle subentry);
+
+std::string_view GetString(NT_Handle subentry, wpi::SmallVectorImpl<char>& buf, std::string_view defaultValue);
+
+TimestampedStringView GetAtomicString(
+      NT_Handle subentry,
+      wpi::SmallVectorImpl<char>& buf,
+      std::string_view defaultValue);
+
+/** @} */
+
+/**
+ * Timestamped Raw.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedRaw = Timestamped<std::vector<uint8_t>>;
+
+/**
+ * Timestamped Raw view (for SmallVector-taking functions).
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedRawView = Timestamped<std::span<uint8_t>>;
+
+/**
+ * @defgroup ntcore_Raw_func Raw Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetRaw(NT_Handle pubentry, std::span<const uint8_t> value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultRaw(NT_Handle pubentry, std::span<const uint8_t> defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+std::vector<uint8_t> GetRaw(NT_Handle subentry, std::span<const uint8_t> defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedRaw GetAtomicRaw(NT_Handle subentry, std::span<const uint8_t> defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedRaw> ReadQueueRaw(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<std::vector<uint8_t>> ReadQueueValuesRaw(NT_Handle subentry);
+
+std::span<uint8_t> GetRaw(NT_Handle subentry, wpi::SmallVectorImpl<uint8_t>& buf, std::span<const uint8_t> defaultValue);
+
+TimestampedRawView GetAtomicRaw(
+      NT_Handle subentry,
+      wpi::SmallVectorImpl<uint8_t>& buf,
+      std::span<const uint8_t> defaultValue);
+
+/** @} */
+
+/**
+ * Timestamped BooleanArray.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedBooleanArray = Timestamped<std::vector<int>>;
+
+/**
+ * Timestamped BooleanArray view (for SmallVector-taking functions).
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedBooleanArrayView = Timestamped<std::span<int>>;
+
+/**
+ * @defgroup ntcore_BooleanArray_func BooleanArray Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetBooleanArray(NT_Handle pubentry, std::span<const int> value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultBooleanArray(NT_Handle pubentry, std::span<const int> defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+std::vector<int> GetBooleanArray(NT_Handle subentry, std::span<const int> defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedBooleanArray GetAtomicBooleanArray(NT_Handle subentry, std::span<const int> defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedBooleanArray> ReadQueueBooleanArray(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<std::vector<int>> ReadQueueValuesBooleanArray(NT_Handle subentry);
+
+std::span<int> GetBooleanArray(NT_Handle subentry, wpi::SmallVectorImpl<int>& buf, std::span<const int> defaultValue);
+
+TimestampedBooleanArrayView GetAtomicBooleanArray(
+      NT_Handle subentry,
+      wpi::SmallVectorImpl<int>& buf,
+      std::span<const int> defaultValue);
+
+/** @} */
+
+/**
+ * Timestamped IntegerArray.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedIntegerArray = Timestamped<std::vector<int64_t>>;
+
+/**
+ * Timestamped IntegerArray view (for SmallVector-taking functions).
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedIntegerArrayView = Timestamped<std::span<int64_t>>;
+
+/**
+ * @defgroup ntcore_IntegerArray_func IntegerArray Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetIntegerArray(NT_Handle pubentry, std::span<const int64_t> value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultIntegerArray(NT_Handle pubentry, std::span<const int64_t> defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+std::vector<int64_t> GetIntegerArray(NT_Handle subentry, std::span<const int64_t> defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedIntegerArray GetAtomicIntegerArray(NT_Handle subentry, std::span<const int64_t> defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedIntegerArray> ReadQueueIntegerArray(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<std::vector<int64_t>> ReadQueueValuesIntegerArray(NT_Handle subentry);
+
+std::span<int64_t> GetIntegerArray(NT_Handle subentry, wpi::SmallVectorImpl<int64_t>& buf, std::span<const int64_t> defaultValue);
+
+TimestampedIntegerArrayView GetAtomicIntegerArray(
+      NT_Handle subentry,
+      wpi::SmallVectorImpl<int64_t>& buf,
+      std::span<const int64_t> defaultValue);
+
+/** @} */
+
+/**
+ * Timestamped FloatArray.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedFloatArray = Timestamped<std::vector<float>>;
+
+/**
+ * Timestamped FloatArray view (for SmallVector-taking functions).
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedFloatArrayView = Timestamped<std::span<float>>;
+
+/**
+ * @defgroup ntcore_FloatArray_func FloatArray Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetFloatArray(NT_Handle pubentry, std::span<const float> value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultFloatArray(NT_Handle pubentry, std::span<const float> defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+std::vector<float> GetFloatArray(NT_Handle subentry, std::span<const float> defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedFloatArray GetAtomicFloatArray(NT_Handle subentry, std::span<const float> defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedFloatArray> ReadQueueFloatArray(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<std::vector<float>> ReadQueueValuesFloatArray(NT_Handle subentry);
+
+std::span<float> GetFloatArray(NT_Handle subentry, wpi::SmallVectorImpl<float>& buf, std::span<const float> defaultValue);
+
+TimestampedFloatArrayView GetAtomicFloatArray(
+      NT_Handle subentry,
+      wpi::SmallVectorImpl<float>& buf,
+      std::span<const float> defaultValue);
+
+/** @} */
+
+/**
+ * Timestamped DoubleArray.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedDoubleArray = Timestamped<std::vector<double>>;
+
+/**
+ * Timestamped DoubleArray view (for SmallVector-taking functions).
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedDoubleArrayView = Timestamped<std::span<double>>;
+
+/**
+ * @defgroup ntcore_DoubleArray_func DoubleArray Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetDoubleArray(NT_Handle pubentry, std::span<const double> value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultDoubleArray(NT_Handle pubentry, std::span<const double> defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+std::vector<double> GetDoubleArray(NT_Handle subentry, std::span<const double> defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedDoubleArray GetAtomicDoubleArray(NT_Handle subentry, std::span<const double> defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedDoubleArray> ReadQueueDoubleArray(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<std::vector<double>> ReadQueueValuesDoubleArray(NT_Handle subentry);
+
+std::span<double> GetDoubleArray(NT_Handle subentry, wpi::SmallVectorImpl<double>& buf, std::span<const double> defaultValue);
+
+TimestampedDoubleArrayView GetAtomicDoubleArray(
+      NT_Handle subentry,
+      wpi::SmallVectorImpl<double>& buf,
+      std::span<const double> defaultValue);
+
+/** @} */
+
+/**
+ * Timestamped StringArray.
+ * @ingroup ntcore_cpp_handle_api
+ */
+using TimestampedStringArray = Timestamped<std::vector<std::string>>;
+
+/**
+ * @defgroup ntcore_StringArray_func StringArray Functions
+ * @ingroup ntcore_cpp_handle_api
+ * @{
+ */
+
+/**
+ * Publish a new value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param value value to publish
+ * @param time timestamp; 0 indicates current NT time should be used
+ */
+bool SetStringArray(NT_Handle pubentry, std::span<const std::string> value, int64_t time = 0);
+
+/**
+ * Publish a default value.
+ * On reconnect, a default value will never be used in preference to a
+ * published value.
+ *
+ * @param pubentry publisher or entry handle
+ * @param defaultValue default value
+ */
+bool SetDefaultStringArray(NT_Handle pubentry, std::span<const std::string> defaultValue);
+
+/**
+ * Get the last published value.
+ * If no value has been published, returns the passed defaultValue.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return value
+ */
+std::vector<std::string> GetStringArray(NT_Handle subentry, std::span<const std::string> defaultValue);
+
+/**
+ * Get the last published value along with its timestamp.
+ * If no value has been published, returns the passed defaultValue and a
+ * timestamp of 0.
+ *
+ * @param subentry subscriber or entry handle
+ * @param defaultValue default value to return if no value has been published
+ * @return timestamped value
+ */
+TimestampedStringArray GetAtomicStringArray(NT_Handle subentry, std::span<const std::string> defaultValue);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ * Also provides a timestamp for each value.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of timestamped values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<TimestampedStringArray> ReadQueueStringArray(NT_Handle subentry);
+
+/**
+ * Get an array of all value changes since the last call to ReadQueue.
+ *
+ * @note The "poll storage" subscribe option can be used to set the queue
+ *     depth.
+ *
+ * @param subentry subscriber or entry handle
+ * @return Array of values; empty array if no new changes have
+ *     been published since the previous call.
+ */
+std::vector<std::vector<std::string>> ReadQueueValuesStringArray(NT_Handle subentry);
+
+/** @} */
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java
index 3c65938..c88ce8b 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java
@@ -574,7 +574,7 @@
     return m_inst.addListener(
         new String[] {m_pathWithSep},
         EnumSet.of(NetworkTableEvent.Kind.kPublish, NetworkTableEvent.Kind.kImmediate),
-        new Consumer<NetworkTableEvent>() {
+        new Consumer<>() {
           final Set<String> m_notifiedTables = new HashSet<>();
 
           @Override
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java
index 2350b49..3fea5a3 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java
@@ -6,17 +6,29 @@
 
 /** Network table data types. */
 public enum NetworkTableType {
+  /** Unassigned data type. */
   kUnassigned(0, ""),
+  /** Boolean data type. */
   kBoolean(0x01, "boolean"),
+  /** Double precision floating-point data type. */
   kDouble(0x02, "double"),
+  /** String data type. */
   kString(0x04, "string"),
+  /** Raw data type. */
   kRaw(0x08, "raw"),
+  /** Boolean array data type. */
   kBooleanArray(0x10, "boolean[]"),
+  /** Double precision floating-point array data type. */
   kDoubleArray(0x20, "double[]"),
+  /** String array data type. */
   kStringArray(0x40, "string[]"),
+  /** Integer data type. */
   kInteger(0x100, "int"),
+  /** Single precision floating-point data type. */
   kFloat(0x200, "float"),
+  /** Integer array data type. */
   kIntegerArray(0x400, "int[]"),
+  /** Single precision floating-point array data type. */
   kFloatArray(0x800, "float[]");
 
   private final int m_value;
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java
index b4359ea..6d891c4 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java
@@ -190,14 +190,14 @@
 
   private TimestampedObject<T> fromRaw(TimestampedRaw raw, T defaultValue) {
     if (raw.value.length == 0) {
-      return new TimestampedObject<T>(0, 0, defaultValue);
+      return new TimestampedObject<>(0, 0, defaultValue);
     }
     try {
       synchronized (m_buf) {
-        return new TimestampedObject<T>(raw.timestamp, raw.serverTime, m_buf.read(raw.value));
+        return new TimestampedObject<>(raw.timestamp, raw.serverTime, m_buf.read(raw.value));
       }
     } catch (IOException e) {
-      return new TimestampedObject<T>(0, 0, defaultValue);
+      return new TimestampedObject<>(0, 0, defaultValue);
     }
   }
 
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java
index c3dad13..15313d7 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java
@@ -32,7 +32,7 @@
    * @return ProtobufTopic for value class
    */
   public static <T> ProtobufTopic<T> wrap(Topic topic, Protobuf<T, ?> proto) {
-    return new ProtobufTopic<T>(topic, proto);
+    return new ProtobufTopic<>(topic, proto);
   }
 
   /**
@@ -47,7 +47,7 @@
    */
   public static <T> ProtobufTopic<T> wrap(
       NetworkTableInstance inst, int handle, Protobuf<T, ?> proto) {
-    return new ProtobufTopic<T>(inst, handle, proto);
+    return new ProtobufTopic<>(inst, handle, proto);
   }
 
   /**
@@ -63,7 +63,7 @@
    * @return subscriber
    */
   public ProtobufSubscriber<T> subscribe(T defaultValue, PubSubOption... options) {
-    return new ProtobufEntryImpl<T>(
+    return new ProtobufEntryImpl<>(
         this,
         ProtobufBuffer.create(m_proto),
         NetworkTablesJNI.subscribe(
@@ -87,7 +87,7 @@
    */
   public ProtobufPublisher<T> publish(PubSubOption... options) {
     m_inst.addSchema(m_proto);
-    return new ProtobufEntryImpl<T>(
+    return new ProtobufEntryImpl<>(
         this,
         ProtobufBuffer.create(m_proto),
         NetworkTablesJNI.publish(
@@ -113,7 +113,7 @@
    */
   public ProtobufPublisher<T> publishEx(String properties, PubSubOption... options) {
     m_inst.addSchema(m_proto);
-    return new ProtobufEntryImpl<T>(
+    return new ProtobufEntryImpl<>(
         this,
         ProtobufBuffer.create(m_proto),
         NetworkTablesJNI.publishEx(
@@ -144,7 +144,7 @@
    * @return entry
    */
   public ProtobufEntry<T> getEntry(T defaultValue, PubSubOption... options) {
-    return new ProtobufEntryImpl<T>(
+    return new ProtobufEntryImpl<>(
         this,
         ProtobufBuffer.create(m_proto),
         NetworkTablesJNI.getEntry(
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java
index 4e8a4a0..1e6b69e 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java
@@ -177,15 +177,14 @@
   @SuppressWarnings("PMD.AvoidCatchingGenericException")
   private TimestampedObject<T[]> fromRaw(TimestampedRaw raw, T[] defaultValue) {
     if (raw.value.length == 0) {
-      return new TimestampedObject<T[]>(0, 0, defaultValue);
+      return new TimestampedObject<>(0, 0, defaultValue);
     }
     try {
       synchronized (m_buf) {
-        return new TimestampedObject<T[]>(
-            raw.timestamp, raw.serverTime, m_buf.readArray(raw.value));
+        return new TimestampedObject<>(raw.timestamp, raw.serverTime, m_buf.readArray(raw.value));
       }
     } catch (RuntimeException e) {
-      return new TimestampedObject<T[]>(0, 0, defaultValue);
+      return new TimestampedObject<>(0, 0, defaultValue);
     }
   }
 
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java
index 247501b..557e9dd 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java
@@ -32,7 +32,7 @@
    * @return StructArrayTopic for value class
    */
   public static <T> StructArrayTopic<T> wrap(Topic topic, Struct<T> struct) {
-    return new StructArrayTopic<T>(topic, struct);
+    return new StructArrayTopic<>(topic, struct);
   }
 
   /**
@@ -47,7 +47,7 @@
    */
   public static <T> StructArrayTopic<T> wrap(
       NetworkTableInstance inst, int handle, Struct<T> struct) {
-    return new StructArrayTopic<T>(inst, handle, struct);
+    return new StructArrayTopic<>(inst, handle, struct);
   }
 
   /**
@@ -63,7 +63,7 @@
    * @return subscriber
    */
   public StructArraySubscriber<T> subscribe(T[] defaultValue, PubSubOption... options) {
-    return new StructArrayEntryImpl<T>(
+    return new StructArrayEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.subscribe(
@@ -87,7 +87,7 @@
    */
   public StructArrayPublisher<T> publish(PubSubOption... options) {
     m_inst.addSchema(m_struct);
-    return new StructArrayEntryImpl<T>(
+    return new StructArrayEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.publish(
@@ -113,7 +113,7 @@
    */
   public StructArrayPublisher<T> publishEx(String properties, PubSubOption... options) {
     m_inst.addSchema(m_struct);
-    return new StructArrayEntryImpl<T>(
+    return new StructArrayEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.publishEx(
@@ -144,7 +144,7 @@
    * @return entry
    */
   public StructArrayEntry<T> getEntry(T[] defaultValue, PubSubOption... options) {
-    return new StructArrayEntryImpl<T>(
+    return new StructArrayEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.getEntry(
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java
index bd02d27..5d37765 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java
@@ -188,14 +188,14 @@
   @SuppressWarnings("PMD.AvoidCatchingGenericException")
   private TimestampedObject<T> fromRaw(TimestampedRaw raw, T defaultValue) {
     if (raw.value.length == 0) {
-      return new TimestampedObject<T>(0, 0, defaultValue);
+      return new TimestampedObject<>(0, 0, defaultValue);
     }
     try {
       synchronized (m_buf) {
-        return new TimestampedObject<T>(raw.timestamp, raw.serverTime, m_buf.read(raw.value));
+        return new TimestampedObject<>(raw.timestamp, raw.serverTime, m_buf.read(raw.value));
       }
     } catch (RuntimeException e) {
-      return new TimestampedObject<T>(0, 0, defaultValue);
+      return new TimestampedObject<>(0, 0, defaultValue);
     }
   }
 
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java
index b1ff026..22dbd2d 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java
@@ -32,7 +32,7 @@
    * @return StructTopic for value class
    */
   public static <T> StructTopic<T> wrap(Topic topic, Struct<T> struct) {
-    return new StructTopic<T>(topic, struct);
+    return new StructTopic<>(topic, struct);
   }
 
   /**
@@ -46,7 +46,7 @@
    * @return StructTopic for value class
    */
   public static <T> StructTopic<T> wrap(NetworkTableInstance inst, int handle, Struct<T> struct) {
-    return new StructTopic<T>(inst, handle, struct);
+    return new StructTopic<>(inst, handle, struct);
   }
 
   /**
@@ -62,7 +62,7 @@
    * @return subscriber
    */
   public StructSubscriber<T> subscribe(T defaultValue, PubSubOption... options) {
-    return new StructEntryImpl<T>(
+    return new StructEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.subscribe(
@@ -86,7 +86,7 @@
    */
   public StructPublisher<T> publish(PubSubOption... options) {
     m_inst.addSchema(m_struct);
-    return new StructEntryImpl<T>(
+    return new StructEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.publish(
@@ -112,7 +112,7 @@
    */
   public StructPublisher<T> publishEx(String properties, PubSubOption... options) {
     m_inst.addSchema(m_struct);
-    return new StructEntryImpl<T>(
+    return new StructEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.publishEx(
@@ -143,7 +143,7 @@
    * @return entry
    */
   public StructEntry<T> getEntry(T defaultValue, PubSubOption... options) {
-    return new StructEntryImpl<T>(
+    return new StructEntryImpl<>(
         this,
         StructBuffer.create(m_struct),
         NetworkTablesJNI.getEntry(
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java
index 37c1544..896ec79 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java
@@ -4,7 +4,11 @@
 
 package edu.wpi.first.networktables;
 
-/** NetworkTables timestamped object. */
+/**
+ * NetworkTables timestamped object.
+ *
+ * @param <T> Value type.
+ */
 public final class TimestampedObject<T> {
   /**
    * Create a timestamped value.
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/Topic.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/Topic.java
index db08a34..6794474 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/Topic.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/Topic.java
@@ -118,6 +118,25 @@
   }
 
   /**
+   * Allow storage of the topic's last value, allowing the value to be read (and not just accessed
+   * through event queues and listeners).
+   *
+   * @param cached True for cached, false for not cached.
+   */
+  public void setCached(boolean cached) {
+    NetworkTablesJNI.setTopicCached(m_handle, cached);
+  }
+
+  /**
+   * Returns whether the topic's last value is stored.
+   *
+   * @return True if the topic is cached.
+   */
+  public boolean isCached() {
+    return NetworkTablesJNI.getTopicCached(m_handle);
+  }
+
+  /**
    * Determines if the topic is currently being published.
    *
    * @return True if the topic exists, false otherwise.
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.cpp
index 0377f4f..9cec611 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.cpp
@@ -130,7 +130,7 @@
     if (!m_dataloggers.empty()) {
       auto now = Now();
       for (auto&& datalogger : m_dataloggers) {
-        if (wpi::starts_with(topic->name, datalogger->prefix)) {
+        if (PrefixMatch(topic->name, datalogger->prefix, topic->special)) {
           auto it = std::find_if(topic->datalogs.begin(), topic->datalogs.end(),
                                  [&](const auto& elem) {
                                    return elem.logger == datalogger->handle;
@@ -175,22 +175,26 @@
 }
 
 bool LocalStorage::Impl::SetValue(TopicData* topic, const Value& value,
-                                  unsigned int eventFlags, bool isDuplicate,
+                                  unsigned int eventFlags,
                                   bool suppressIfDuplicate,
                                   const PublisherData* publisher) {
+  const bool isDuplicate = topic->IsCached() && topic->lastValue == value;
   DEBUG4("SetValue({}, {}, {}, {})", topic->name, value.time(), eventFlags,
          isDuplicate);
   if (topic->type != NT_UNASSIGNED && topic->type != value.type()) {
     return false;
   }
+  // Make sure value isn't older than last value
   if (!topic->lastValue || topic->lastValue.time() == 0 ||
       value.time() >= topic->lastValue.time()) {
     // TODO: notify option even if older value
     if (!(suppressIfDuplicate && isDuplicate)) {
       topic->type = value.type();
-      topic->lastValue = value;
-      topic->lastValueFromNetwork = false;
-      NotifyValue(topic, eventFlags, isDuplicate, publisher);
+      if (topic->IsCached()) {
+        topic->lastValue = value;
+        topic->lastValueFromNetwork = false;
+      }
+      NotifyValue(topic, value, eventFlags, isDuplicate, publisher);
       if (topic->datalogType == value.type()) {
         for (auto&& datalog : topic->datalogs) {
           datalog.Append(value);
@@ -202,8 +206,8 @@
   return true;
 }
 
-void LocalStorage::Impl::NotifyValue(TopicData* topic, unsigned int eventFlags,
-                                     bool isDuplicate,
+void LocalStorage::Impl::NotifyValue(TopicData* topic, const Value& value,
+                                     unsigned int eventFlags, bool isDuplicate,
                                      const PublisherData* publisher) {
   bool isNetwork = (eventFlags & NT_EVENT_VALUE_REMOTE) != 0;
   for (auto&& subscriber : topic->localSubscribers) {
@@ -213,11 +217,11 @@
          (!isNetwork && !subscriber->config.disableLocal)) &&
         (!publisher || (publisher && (subscriber->config.excludePublisher !=
                                       publisher->handle)))) {
-      subscriber->pollStorage.emplace_back(topic->lastValue);
+      subscriber->pollStorage.emplace_back(value);
       subscriber->handle.Set();
       if (!subscriber->valueListeners.empty()) {
         m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
-                                 topic->handle, 0, topic->lastValue);
+                                 topic->handle, 0, value);
       }
     }
   }
@@ -227,7 +231,7 @@
       subscriber->handle.Set();
       if (!subscriber->valueListeners.empty()) {
         m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
-                                 topic->handle, 0, topic->lastValue);
+                                 topic->handle, 0, value);
       }
     }
   }
@@ -249,6 +253,22 @@
     topic->properties.erase("retained");
     update["retained"] = wpi::json();
   }
+  if ((flags & NT_UNCACHED) != 0) {
+    topic->properties["cached"] = false;
+    update["cached"] = false;
+  } else {
+    topic->properties.erase("cached");
+    update["cached"] = wpi::json();
+  }
+  if ((flags & NT_UNCACHED) != 0) {
+    topic->lastValue = {};
+    topic->lastValueNetwork = {};
+    topic->lastValueFromNetwork = false;
+  }
+  if ((flags & NT_UNCACHED) != 0 && (flags & NT_PERSISTENT) != 0) {
+    WARN("topic {}: disabling cached property disables persistent storage",
+         topic->name);
+  }
   topic->flags = flags;
   if (!update.empty()) {
     PropertiesUpdated(topic, update, NT_EVENT_NONE, true, false);
@@ -283,6 +303,20 @@
   PropertiesUpdated(topic, update, NT_EVENT_NONE, true, false);
 }
 
+void LocalStorage::Impl::SetCached(TopicData* topic, bool value) {
+  wpi::json update = wpi::json::object();
+  if (value) {
+    topic->flags &= ~NT_UNCACHED;
+    topic->properties.erase("cached");
+    update["cached"] = wpi::json();
+  } else {
+    topic->flags |= NT_UNCACHED;
+    topic->properties["cached"] = false;
+    update["cached"] = false;
+  }
+  PropertiesUpdated(topic, update, NT_EVENT_NONE, true, false);
+}
+
 void LocalStorage::Impl::SetProperties(TopicData* topic,
                                        const wpi::json& update,
                                        bool sendNetwork) {
@@ -328,6 +362,28 @@
         }
       }
     }
+    it = topic->properties.find("cached");
+    if (it != topic->properties.end()) {
+      if (auto val = it->get_ptr<bool*>()) {
+        if (*val) {
+          topic->flags &= ~NT_UNCACHED;
+        } else {
+          topic->flags |= NT_UNCACHED;
+        }
+      }
+    }
+
+    if ((topic->flags & NT_UNCACHED) != 0) {
+      topic->lastValue = {};
+      topic->lastValueNetwork = {};
+      topic->lastValueFromNetwork = false;
+    }
+
+    if ((topic->flags & NT_UNCACHED) != 0 &&
+        (topic->flags & NT_PERSISTENT) != 0) {
+      WARN("topic {}: disabling cached property disables persistent storage",
+           topic->name);
+    }
   }
 
   topic->propertiesStr = topic->properties.dump();
@@ -895,20 +951,22 @@
     return false;
   }
   if (publisher->active) {
-    bool isDuplicate, isNetworkDuplicate, suppressDuplicates;
+    bool isNetworkDuplicate, suppressDuplicates;
     if (force || publisher->config.keepDuplicates) {
       suppressDuplicates = false;
       isNetworkDuplicate = false;
     } else {
       suppressDuplicates = true;
-      isNetworkDuplicate = (publisher->topic->lastValueNetwork == value);
+      isNetworkDuplicate = publisher->topic->IsCached() &&
+                           (publisher->topic->lastValueNetwork == value);
     }
-    isDuplicate = (publisher->topic->lastValue == value);
     if (!isNetworkDuplicate && m_network) {
-      publisher->topic->lastValueNetwork = value;
+      if (publisher->topic->IsCached()) {
+        publisher->topic->lastValueNetwork = value;
+      }
       m_network->SetValue(publisher->handle, value);
     }
-    return SetValue(publisher->topic, value, NT_EVENT_VALUE_LOCAL, isDuplicate,
+    return SetValue(publisher->topic, value, NT_EVENT_VALUE_LOCAL,
                     suppressDuplicates, publisher);
   } else {
     return false;
@@ -940,6 +998,10 @@
     return false;
   }
   if (auto topic = GetTopic(pubsubentryHandle)) {
+    if (!topic->IsCached()) {
+      WARN("ignoring default value on non-cached topic '{}'", topic->name);
+      return false;
+    }
     if (!topic->lastValue &&
         (topic->type == NT_UNASSIGNED || topic->type == value.type() ||
          IsNumericCompatible(topic->type, value.type()))) {
@@ -1026,10 +1088,11 @@
 void LocalStorage::NetworkSetValue(NT_Topic topicHandle, const Value& value) {
   std::scoped_lock lock{m_mutex};
   if (auto topic = m_impl.m_topics.Get(topicHandle)) {
-    if (m_impl.SetValue(topic, value, NT_EVENT_VALUE_REMOTE,
-                        value == topic->lastValue, false, nullptr)) {
-      topic->lastValueNetwork = value;
-      topic->lastValueFromNetwork = true;
+    if (m_impl.SetValue(topic, value, NT_EVENT_VALUE_REMOTE, false, nullptr)) {
+      if (topic->IsCached()) {
+        topic->lastValueNetwork = value;
+        topic->lastValueFromNetwork = true;
+      }
     }
   }
 }
@@ -1448,7 +1511,7 @@
   // start logging any matching topics
   auto now = nt::Now();
   for (auto&& topic : m_impl.m_topics) {
-    if (!wpi::starts_with(topic->name, prefix) ||
+    if (!PrefixMatch(topic->name, prefix, topic->special) ||
         topic->type == NT_UNASSIGNED || topic->typeStr.empty()) {
       continue;
     }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.h b/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.h
index af2b4de..4283274 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/LocalStorage.h
@@ -136,6 +136,22 @@
     }
   }
 
+  void SetTopicCached(NT_Topic topicHandle, bool value) {
+    std::scoped_lock lock{m_mutex};
+    if (auto topic = m_impl.m_topics.Get(topicHandle)) {
+      m_impl.SetCached(topic, value);
+    }
+  }
+
+  bool GetTopicCached(NT_Topic topicHandle) {
+    std::scoped_lock lock{m_mutex};
+    if (auto topic = m_impl.m_topics.Get(topicHandle)) {
+      return (topic->flags & NT_UNCACHED) == 0;
+    } else {
+      return false;
+    }
+  }
+
   bool GetTopicExists(NT_Handle handle) {
     std::scoped_lock lock{m_mutex};
     TopicData* topic = m_impl.GetTopic(handle);
@@ -361,6 +377,8 @@
 
     bool Exists() const { return onNetwork || !localPublishers.empty(); }
 
+    bool IsCached() const { return (flags & NT_UNCACHED) == 0; }
+
     TopicInfo GetTopicInfo() const;
 
     // invariants
@@ -565,14 +583,15 @@
     void CheckReset(TopicData* topic);
 
     bool SetValue(TopicData* topic, const Value& value, unsigned int eventFlags,
-                  bool isDuplicate, bool suppressIfDuplicate,
-                  const PublisherData* publisher);
-    void NotifyValue(TopicData* topic, unsigned int eventFlags,
-                     bool isDuplicate, const PublisherData* publisher);
+                  bool suppressIfDuplicate, const PublisherData* publisher);
+    void NotifyValue(TopicData* topic, const Value& value,
+                     unsigned int eventFlags, bool isDuplicate,
+                     const PublisherData* publisher);
 
     void SetFlags(TopicData* topic, unsigned int flags);
     void SetPersistent(TopicData* topic, bool value);
     void SetRetained(TopicData* topic, bool value);
+    void SetCached(TopicData* topic, bool value);
     void SetProperties(TopicData* topic, const wpi::json& update,
                        bool sendNetwork);
     void PropertiesUpdated(TopicData* topic, const wpi::json& update,
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkClient.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkClient.cpp
index 7634be9..149dcb6 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkClient.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkClient.cpp
@@ -206,7 +206,9 @@
   auto clientImpl = std::make_shared<net3::ClientImpl3>(
       m_loop.Now().count(), m_inst, *wire, m_logger, [this](uint32_t repeatMs) {
         DEBUG4("Setting periodic timer to {}", repeatMs);
-        if (m_sendOutgoingTimer) {
+        if (m_sendOutgoingTimer &&
+            (!m_sendOutgoingTimer->IsActive() ||
+             uv::Timer::Time{repeatMs} != m_sendOutgoingTimer->GetRepeat())) {
           m_sendOutgoingTimer->Start(uv::Timer::Time{repeatMs},
                                      uv::Timer::Time{repeatMs});
         }
@@ -406,7 +408,9 @@
       m_loop.Now().count(), m_inst, *m_wire, m_logger, m_timeSyncUpdated,
       [this](uint32_t repeatMs) {
         DEBUG4("Setting periodic timer to {}", repeatMs);
-        if (m_sendOutgoingTimer) {
+        if (m_sendOutgoingTimer &&
+            (!m_sendOutgoingTimer->IsActive() ||
+             uv::Timer::Time{repeatMs} != m_sendOutgoingTimer->GetRepeat())) {
           m_sendOutgoingTimer->Start(uv::Timer::Time{repeatMs},
                                      uv::Timer::Time{repeatMs});
         }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkServer.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkServer.cpp
index 9062c7f..5b5e78d 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkServer.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkServer.cpp
@@ -112,7 +112,8 @@
   DEBUG4("Setting periodic timer to {}", repeatMs);
   if (repeatMs == UINT32_MAX) {
     m_outgoingTimer->Stop();
-  } else {
+  } else if (!m_outgoingTimer->IsActive() ||
+             uv::Timer::Time{repeatMs} != m_outgoingTimer->GetRepeat()) {
     m_outgoingTimer->Start(uv::Timer::Time{repeatMs},
                            uv::Timer::Time{repeatMs});
   }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
index 14b5df1..fdec2b9 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
@@ -673,6 +673,30 @@
 
 /*
  * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    setTopicCached
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_setTopicCached
+  (JNIEnv*, jclass, jint topic, jboolean value)
+{
+  nt::SetTopicCached(topic, value);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    getTopicCached
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_getTopicCached
+  (JNIEnv*, jclass, jint topic)
+{
+  return nt::GetTopicCached(topic);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
  * Method:    getTopicTypeString
  * Signature: (I)Ljava/lang/String;
  */
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/net/ClientImpl.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/net/ClientImpl.cpp
index 309b01a..96a6be7 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/net/ClientImpl.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/net/ClientImpl.cpp
@@ -20,7 +20,6 @@
 #include "NetworkInterface.h"
 #include "WireConnection.h"
 #include "WireEncoder.h"
-#include "net/NetworkOutgoingQueue.h"
 #include "networktables/NetworkTableValue.h"
 
 using namespace nt;
@@ -68,26 +67,28 @@
     DEBUG4("BinaryMessage({})", id);
 
     // handle RTT ping response (only use first one)
-    if (!m_haveTimeOffset && id == -1) {
-      if (!value.IsInteger()) {
-        WARN("RTT ping response with non-integer type {}",
-             static_cast<int>(value.type()));
-        continue;
-      }
-      DEBUG4("RTT ping response time {} value {}", value.time(),
-             value.GetInteger());
-      if (m_wire.GetVersion() < 0x0401) {
-        m_pongTimeMs = curTimeMs;
-      }
-      int64_t now = wpi::Now();
-      int64_t rtt2 = (now - value.GetInteger()) / 2;
-      if (rtt2 < m_rtt2Us) {
-        m_rtt2Us = rtt2;
-        int64_t serverTimeOffsetUs = value.server_time() + rtt2 - now;
-        DEBUG3("Time offset: {}", serverTimeOffsetUs);
-        m_outgoing.SetTimeOffset(serverTimeOffsetUs);
-        m_haveTimeOffset = true;
-        m_timeSyncUpdated(serverTimeOffsetUs, m_rtt2Us, true);
+    if (id == -1) {
+      if (!m_haveTimeOffset) {
+        if (!value.IsInteger()) {
+          WARN("RTT ping response with non-integer type {}",
+               static_cast<int>(value.type()));
+          continue;
+        }
+        DEBUG4("RTT ping response time {} value {}", value.time(),
+               value.GetInteger());
+        if (m_wire.GetVersion() < 0x0401) {
+          m_pongTimeMs = curTimeMs;
+        }
+        int64_t now = wpi::Now();
+        int64_t rtt2 = (now - value.GetInteger()) / 2;
+        if (rtt2 < m_rtt2Us) {
+          m_rtt2Us = rtt2;
+          int64_t serverTimeOffsetUs = value.server_time() + rtt2 - now;
+          DEBUG3("Time offset: {}", serverTimeOffsetUs);
+          m_outgoing.SetTimeOffset(serverTimeOffsetUs);
+          m_haveTimeOffset = true;
+          m_timeSyncUpdated(serverTimeOffsetUs, m_rtt2Us, true);
+        }
       }
       continue;
     }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.cpp
index 1119809..8dc5291 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.cpp
@@ -917,6 +917,7 @@
   auto typeStr = TypeToString(value.type());
   wpi::json properties = wpi::json::object();
   properties["retained"] = true;  // treat all NT3 published topics as retained
+  properties["cached"] = true;    // treat all NT3 published topics as cached
   if ((flags & NT_PERSISTENT) != 0) {
     properties["persistent"] = true;
   }
@@ -1095,6 +1096,7 @@
 void ServerImpl::TopicData::RefreshProperties() {
   persistent = false;
   retained = false;
+  cached = true;
 
   auto persistentIt = properties.find("persistent");
   if (persistentIt != properties.end()) {
@@ -1109,6 +1111,23 @@
       retained = *val;
     }
   }
+
+  auto cachedIt = properties.find("cached");
+  if (cachedIt != properties.end()) {
+    if (auto val = cachedIt->get_ptr<bool*>()) {
+      cached = *val;
+    }
+  }
+
+  if (!cached) {
+    lastValue = {};
+    lastValueClient = nullptr;
+  }
+
+  if (!cached && persistent) {
+    WARN("topic {}: disabling cached property disables persistent storage",
+         name);
+  }
 }
 
 bool ServerImpl::TopicData::SetFlags(unsigned int flags_) {
@@ -1122,6 +1141,30 @@
     persistent = false;
     properties.erase("persistent");
   }
+  if ((flags_ & NT_RETAINED) != 0) {
+    updated |= !retained;
+    retained = true;
+    properties["retained"] = true;
+  } else {
+    updated |= retained;
+    retained = false;
+    properties.erase("retained");
+  }
+  if ((flags_ & NT_UNCACHED) != 0) {
+    updated |= cached;
+    cached = false;
+    properties["cached"] = false;
+    lastValue = {};
+    lastValueClient = nullptr;
+  } else {
+    updated |= !cached;
+    cached = true;
+    properties.erase("cached");
+  }
+  if (!cached && persistent) {
+    WARN("topic {}: disabling cached property disables persistent storage",
+         name);
+  }
   return updated;
 }
 
@@ -1642,7 +1685,7 @@
   } else {
     // new topic
     unsigned int id = m_topics.emplace_back(
-        std::make_unique<TopicData>(name, typeStr, properties));
+        std::make_unique<TopicData>(m_logger, name, typeStr, properties));
     topic = m_topics[id].get();
     topic->id = id;
     topic->special = special;
@@ -1663,8 +1706,14 @@
       }
 
       auto& tcd = topic->clients[aClient.get()];
+      bool added = false;
       for (auto subscriber : subscribers) {
-        tcd.AddSubscriber(subscriber);
+        if (tcd.AddSubscriber(subscriber)) {
+          added = true;
+        }
+      }
+      if (added) {
+        aClient->UpdatePeriod(tcd, topic);
       }
 
       if (aClient.get() == client) {
@@ -1707,6 +1756,7 @@
   // unannounce to all subscribers
   for (auto&& tcd : topic->clients) {
     if (!tcd.second.subscribers.empty()) {
+      tcd.first->UpdatePeriod(tcd.second, topic);
       tcd.first->SendUnannounce(topic);
     }
   }
@@ -1751,8 +1801,9 @@
 void ServerImpl::SetValue(ClientData* client, TopicData* topic,
                           const Value& value) {
   // update retained value if from same client or timestamp newer
-  if (!topic->lastValue || topic->lastValueClient == client ||
-      topic->lastValue.time() == 0 || value.time() >= topic->lastValue.time()) {
+  if (topic->cached && (!topic->lastValue || topic->lastValueClient == client ||
+                        topic->lastValue.time() == 0 ||
+                        value.time() >= topic->lastValue.time())) {
     DEBUG4("updating '{}' last value (time was {} is {})", topic->name,
            topic->lastValue.time(), value.time());
     topic->lastValue = value;
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.h b/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.h
index 3ea5a82..d84a06c 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/net/ServerImpl.h
@@ -98,11 +98,15 @@
   struct SubscriberData;
 
   struct TopicData {
-    TopicData(std::string_view name, std::string_view typeStr)
-        : name{name}, typeStr{typeStr} {}
-    TopicData(std::string_view name, std::string_view typeStr,
-              wpi::json properties)
-        : name{name}, typeStr{typeStr}, properties(std::move(properties)) {
+    TopicData(wpi::Logger& logger, std::string_view name,
+              std::string_view typeStr)
+        : m_logger{logger}, name{name}, typeStr{typeStr} {}
+    TopicData(wpi::Logger& logger, std::string_view name,
+              std::string_view typeStr, wpi::json properties)
+        : m_logger{logger},
+          name{name},
+          typeStr{typeStr},
+          properties(std::move(properties)) {
       RefreshProperties();
     }
 
@@ -117,6 +121,7 @@
 
     NT_Handle GetIdHandle() const { return Handle(0, id, Handle::kTopic); }
 
+    wpi::Logger& m_logger;  // Must be m_logger for WARN macro to work
     std::string name;
     unsigned int id;
     Value lastValue;
@@ -126,6 +131,7 @@
     unsigned int publisherCount{0};
     bool persistent{false};
     bool retained{false};
+    bool cached{true};
     bool special{false};
     NT_Topic localHandle{0};
 
@@ -148,10 +154,12 @@
 
       bool AddSubscriber(SubscriberData* sub) {
         bool added = subscribers.insert(sub).second;
-        if (!sub->options.topicsOnly && sendMode == ValueSendMode::kDisabled) {
-          sendMode = ValueSendMode::kNormal;
-        } else if (sub->options.sendAll) {
-          sendMode = ValueSendMode::kAll;
+        if (!sub->options.topicsOnly) {
+          if (sub->options.sendAll) {
+            sendMode = ValueSendMode::kAll;
+          } else if (sendMode == ValueSendMode::kDisabled) {
+            sendMode = ValueSendMode::kNormal;
+          }
         }
         return added;
       }
@@ -200,9 +208,10 @@
     std::string_view GetName() const { return m_name; }
     int GetId() const { return m_id; }
 
-   protected:
-    virtual void UpdatePeriodic(TopicData* topic) {}
+    virtual void UpdatePeriod(TopicData::TopicClientData& tcd,
+                              TopicData* topic) {}
 
+   protected:
     std::string m_name;
     std::string m_connInfo;
     bool m_local;  // local to machine
@@ -245,9 +254,6 @@
 
     void ClientSetValue(int64_t pubuid, const Value& value);
 
-    virtual void UpdatePeriod(TopicData::TopicClientData& tcd,
-                              TopicData* topic) {}
-
     wpi::DenseMap<TopicData*, bool> m_announceSent;
   };
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp
index dcfbabe..74b5ecf 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp
@@ -54,8 +54,14 @@
     // flush_nonempty() case
     m_conn.m_bufs.back().len = len;
     if (!m_disableAlloc) {
+#ifdef NT_ENABLE_WS_FRAG
       m_conn.m_frames.back().opcode &= ~wpi::WebSocket::kFlagFin;
       m_conn.StartFrame(wpi::WebSocket::Frame::kFragment);
+#else
+      m_conn.m_bufs.emplace_back(m_conn.AllocBuf());
+      m_conn.m_bufs.back().len = 0;
+      ++m_conn.m_frames.back().end;
+#endif
       SetBuffer(m_conn.m_bufs.back().base, kAllocSize);
     }
     return;
@@ -76,9 +82,15 @@
       len -= amt;
     }
     if (buf.len >= kAllocSize && (len > 0 || !m_disableAlloc)) {
+#ifdef NT_ENABLE_WS_FRAG
       // fragment the current frame and start a new one
       m_conn.m_frames.back().opcode &= ~wpi::WebSocket::kFlagFin;
       m_conn.StartFrame(wpi::WebSocket::Frame::kFragment);
+#else
+      m_conn.m_bufs.emplace_back(m_conn.AllocBuf());
+      m_conn.m_bufs.back().len = 0;
+      ++m_conn.m_frames.back().end;
+#endif
       updateBuffer = true;
     }
   }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp
index b432664..c4322b3 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp
@@ -295,6 +295,14 @@
   return nt::GetTopicRetained(topic);
 }
 
+void NT_SetTopicCached(NT_Topic topic, NT_Bool value) {
+  nt::SetTopicCached(topic, value);
+}
+
+NT_Bool NT_GetTopicCached(NT_Topic topic) {
+  return nt::GetTopicCached(topic);
+}
+
 NT_Bool NT_GetTopicExists(NT_Handle handle) {
   return nt::GetTopicExists(handle);
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp
index 4b48f05..b27c4f5 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp
@@ -263,6 +263,22 @@
   }
 }
 
+void SetTopicCached(NT_Topic topic, bool value) {
+  if (auto ii = InstanceImpl::GetTyped(topic, Handle::kTopic)) {
+    ii->localStorage.SetTopicCached(topic, value);
+  } else {
+    return;
+  }
+}
+
+bool GetTopicCached(NT_Topic topic) {
+  if (auto ii = InstanceImpl::GetTyped(topic, Handle::kTopic)) {
+    return ii->localStorage.GetTopicCached(topic);
+  } else {
+    return {};
+  }
+}
+
 bool GetTopicExists(NT_Handle handle) {
   if (auto ii = InstanceImpl::GetHandle(handle)) {
     return ii->localStorage.GetTopicExists(handle);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_meta.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_meta.cpp
index 05cb4b7..3b947a0 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_meta.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_meta.cpp
@@ -39,7 +39,7 @@
     std::span<const uint8_t> data) {
   mpack_reader_t r;
   mpack_reader_init_data(&r, data);
-  uint32_t numPub = mpack_expect_array_max(&r, 1000);
+  uint32_t numPub = mpack_expect_array_max(&r, 10000);
   std::vector<ClientPublisher> publishers;
   publishers.reserve(numPub);
   for (uint32_t i = 0; i < numPub; ++i) {
@@ -71,7 +71,7 @@
     std::span<const uint8_t> data) {
   mpack_reader_t r;
   mpack_reader_init_data(&r, data);
-  uint32_t numSub = mpack_expect_array_max(&r, 1000);
+  uint32_t numSub = mpack_expect_array_max(&r, 10000);
   std::vector<ClientSubscriber> subscribers;
   subscribers.reserve(numSub);
   for (uint32_t i = 0; i < numSub; ++i) {
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h
index e03ea9e..1d93c8c 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h
@@ -37,9 +37,11 @@
 class RawTopic;
 class StringArrayTopic;
 class StringTopic;
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructArrayTopic;
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructTopic;
 class Topic;
 
@@ -246,9 +248,10 @@
    * @param name topic name
    * @return Topic
    */
-  template <wpi::StructSerializable T>
-  StructTopic<T> GetStructTopic(std::string_view name) const {
-    return StructTopic<T>{GetTopic(name)};
+  template <typename T, typename... I>
+    requires wpi::StructSerializable<T, I...>
+  StructTopic<T, I...> GetStructTopic(std::string_view name) const {
+    return StructTopic<T, I...>{GetTopic(name)};
   }
 
   /**
@@ -257,9 +260,10 @@
    * @param name topic name
    * @return Topic
    */
-  template <wpi::StructSerializable T>
-  StructArrayTopic<T> GetStructArrayTopic(std::string_view name) const {
-    return StructArrayTopic<T>{GetTopic(name)};
+  template <typename T, typename... I>
+    requires wpi::StructSerializable<T, I...>
+  StructArrayTopic<T, I...> GetStructArrayTopic(std::string_view name) const {
+    return StructArrayTopic<T, I...>{GetTopic(name)};
   }
 
   /**
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
index 06e2cd6..cf8f8e1 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
@@ -37,9 +37,11 @@
 class RawTopic;
 class StringArrayTopic;
 class StringTopic;
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructArrayTopic;
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructTopic;
 class Subscriber;
 class Topic;
@@ -260,19 +262,24 @@
    * Gets a raw struct serialized value topic.
    *
    * @param name topic name
+   * @param info optional struct type info
    * @return Topic
    */
-  template <wpi::StructSerializable T>
-  StructTopic<T> GetStructTopic(std::string_view name) const;
+  template <typename T, typename... I>
+    requires wpi::StructSerializable<T, I...>
+  StructTopic<T, I...> GetStructTopic(std::string_view name, I... info) const;
 
   /**
    * Gets a raw struct serialized array topic.
    *
    * @param name topic name
+   * @param info optional struct type info
    * @return Topic
    */
-  template <wpi::StructSerializable T>
-  StructArrayTopic<T> GetStructArrayTopic(std::string_view name) const;
+  template <typename T, typename... I>
+    requires wpi::StructSerializable<T, I...>
+  StructArrayTopic<T, I...> GetStructArrayTopic(std::string_view name,
+                                                I... info) const;
 
   /**
    * Get Published Topics.
@@ -818,10 +825,12 @@
    * Registers a struct schema. Duplicate calls to this function with the same
    * name are silently ignored.
    *
-   * @param T struct serializable type
+   * @tparam T struct serializable type
+   * @param info optional struct type info
    */
-  template <wpi::StructSerializable T>
-  void AddStructSchema();
+  template <typename T, typename... I>
+    requires wpi::StructSerializable<T, I...>
+  void AddStructSchema(const I&... info);
 
   /**
    * Equality operator.  Returns true if both instances refer to the same
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc
index fdd517e..e583e59 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc
@@ -44,16 +44,18 @@
   return ProtobufTopic<T>{GetTopic(name)};
 }
 
-template <wpi::StructSerializable T>
-inline StructTopic<T> NetworkTableInstance::GetStructTopic(
-    std::string_view name) const {
-  return StructTopic<T>{GetTopic(name)};
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
+inline StructTopic<T, I...> NetworkTableInstance::GetStructTopic(
+    std::string_view name, I... info) const {
+  return StructTopic<T, I...>{GetTopic(name), std::move(info)...};
 }
 
-template <wpi::StructSerializable T>
-inline StructArrayTopic<T> NetworkTableInstance::GetStructArrayTopic(
-    std::string_view name) const {
-  return StructArrayTopic<T>{GetTopic(name)};
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
+inline StructArrayTopic<T, I...> NetworkTableInstance::GetStructArrayTopic(
+    std::string_view name, I... info) const {
+  return StructArrayTopic<T, I...>{GetTopic(name), std::move(info)...};
 }
 
 inline std::vector<Topic> NetworkTableInstance::GetTopics() {
@@ -257,6 +259,12 @@
   ::nt::AddSchema(m_handle, name, type, schema);
 }
 
+// Suppress unused-lambda-capture warning on AddSchema() call
+#ifdef __clang__
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wunused-lambda-capture"
+#endif
+
 template <wpi::ProtobufSerializable T>
 void NetworkTableInstance::AddProtobufSchema(wpi::ProtobufMessage<T>& msg) {
   msg.ForEachProtobufDescriptor(
@@ -266,11 +274,18 @@
       });
 }
 
-template <wpi::StructSerializable T>
-void NetworkTableInstance::AddStructSchema() {
-  wpi::ForEachStructSchema<T>([this](auto typeString, auto schema) {
-    AddSchema(typeString, "structschema", schema);
-  });
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
+void NetworkTableInstance::AddStructSchema(const I&... info) {
+  wpi::ForEachStructSchema<T>(
+      [this](auto typeString, auto schema) {
+        AddSchema(typeString, "structschema", schema);
+      },
+      info...);
 }
 
+#ifdef __clang__
+#pragma clang diagnostic pop
+#endif
+
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h
index 4b60454..3b3e086 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h
@@ -13,17 +13,29 @@
  * @ingroup ntcore_cpp_api
  */
 enum class NetworkTableType {
+  /// Unassigned data type.
   kUnassigned = NT_UNASSIGNED,
+  /// Boolean data type.
   kBoolean = NT_BOOLEAN,
+  /// Double precision floating-point data type.
   kDouble = NT_DOUBLE,
+  /// String data type.
   kString = NT_STRING,
+  /// Raw data type.
   kRaw = NT_RAW,
+  /// Boolean array data type.
   kBooleanArray = NT_BOOLEAN_ARRAY,
+  /// Double precision floating-point array data type.
   kDoubleArray = NT_DOUBLE_ARRAY,
+  /// String array data type.
   kStringArray = NT_STRING_ARRAY,
+  /// Integer data type.
   kInteger = NT_INTEGER,
+  /// Single precision floating-point data type.
   kFloat = NT_FLOAT,
+  /// Integer array data type.
   kIntegerArray = NT_INTEGER_ARRAY,
+  /// Single precision floating-point array data type.
   kFloatArray = NT_FLOAT_ARRAY
 };
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/ProtobufTopic.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/ProtobufTopic.h
index 4c30bf7..7a56759 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/ProtobufTopic.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/ProtobufTopic.h
@@ -14,6 +14,7 @@
 #include <vector>
 
 #include <wpi/SmallVector.h>
+#include <wpi/json_fwd.h>
 #include <wpi/mutex.h>
 #include <wpi/protobuf/Protobuf.h>
 
@@ -21,10 +22,6 @@
 #include "networktables/Topic.h"
 #include "ntcore_cpp.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace nt {
 
 template <wpi::ProtobufSerializable T>
@@ -63,12 +60,12 @@
   ProtobufSubscriber(ProtobufSubscriber&& rhs)
       : Subscriber{std::move(rhs)},
         m_msg{std::move(rhs.m_msg)},
-        m_defaultValue{std::move(rhs.defaultValue)} {}
+        m_defaultValue{std::move(rhs.m_defaultValue)} {}
 
   ProtobufSubscriber& operator=(ProtobufSubscriber&& rhs) {
     Subscriber::operator=(std::move(rhs));
     m_msg = std::move(rhs.m_msg);
-    m_defaultValue = std::move(rhs.defaultValue);
+    m_defaultValue = std::move(rhs.m_defaultValue);
     return *this;
   }
 
@@ -175,8 +172,8 @@
   }
 
  private:
-  wpi::mutex m_mutex;
-  wpi::ProtobufMessage<T> m_msg;
+  mutable wpi::mutex m_mutex;
+  mutable wpi::ProtobufMessage<T> m_msg;
   ValueType m_defaultValue;
 };
 
@@ -215,10 +212,9 @@
   ProtobufPublisher& operator=(ProtobufPublisher&& rhs) {
     Publisher::operator=(std::move(rhs));
     m_msg = std::move(rhs.m_msg);
-    m_schemaPublished.clear();
-    if (rhs.m_schemaPublished.test()) {
-      m_schemaPublished.test_and_set();
-    }
+    m_schemaPublished.store(
+        rhs.m_schemaPublished.load(std::memory_order_relaxed),
+        std::memory_order_relaxed);
     return *this;
   }
 
@@ -232,7 +228,7 @@
     wpi::SmallVector<uint8_t, 128> buf;
     {
       std::scoped_lock lock{m_mutex};
-      if (!m_schemaPublished.test_and_set()) {
+      if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
         GetTopic().GetInstance().template AddProtobufSchema<T>(m_msg);
       }
       m_msg.Pack(buf, value);
@@ -251,7 +247,7 @@
     wpi::SmallVector<uint8_t, 128> buf;
     {
       std::scoped_lock lock{m_mutex};
-      if (!m_schemaPublished.test_and_set()) {
+      if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
         GetTopic().GetInstance().template AddProtobufSchema<T>(m_msg);
       }
       m_msg.Pack(buf, value);
@@ -271,7 +267,7 @@
  private:
   wpi::mutex m_mutex;
   wpi::ProtobufMessage<T> m_msg;
-  std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT;
+  std::atomic_bool m_schemaPublished{false};
 };
 
 /**
@@ -303,7 +299,7 @@
    */
   ProtobufEntry(NT_Entry handle, wpi::ProtobufMessage<T> msg, T defaultValue)
       : ProtobufSubscriber<T>{handle, std::move(msg), std::move(defaultValue)},
-        ProtobufPublisher<T>{handle, {}} {}
+        ProtobufPublisher<T>{handle, wpi::ProtobufMessage<T>{}} {}
 
   /**
    * Determines if the native handle is valid.
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructArrayTopic.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructArrayTopic.h
index 91f4721..667570b 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructArrayTopic.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructArrayTopic.h
@@ -7,13 +7,16 @@
 #include <stdint.h>
 
 #include <atomic>
+#include <functional>
+#include <memory>
 #include <ranges>
 #include <span>
-#include <string_view>
+#include <tuple>
 #include <utility>
 #include <vector>
 
 #include <wpi/SmallVector.h>
+#include <wpi/json_fwd.h>
 #include <wpi/mutex.h>
 #include <wpi/struct/Struct.h>
 
@@ -21,24 +24,22 @@
 #include "networktables/Topic.h"
 #include "ntcore_cpp.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace nt {
 
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructArrayTopic;
 
 /**
  * NetworkTables struct-encoded value array subscriber.
  */
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructArraySubscriber : public Subscriber {
-  using S = wpi::Struct<T>;
+  using S = wpi::Struct<T, I...>;
 
  public:
-  using TopicType = StructArrayTopic<T>;
+  using TopicType = StructArrayTopic<T, I...>;
   using ValueType = std::vector<T>;
   using ParamType = std::span<const T>;
   using TimestampedValueType = Timestamped<ValueType>;
@@ -51,15 +52,17 @@
    *
    * @param handle Native handle
    * @param defaultValue Default value
+   * @param info optional struct type info
    */
   template <typename U>
 #if __cpp_lib_ranges >= 201911L
     requires std::ranges::range<U> &&
                  std::convertible_to<std::ranges::range_value_t<U>, T>
 #endif
-  StructArraySubscriber(NT_Subscriber handle, U&& defaultValue)
+  StructArraySubscriber(NT_Subscriber handle, U&& defaultValue, I... info)
       : Subscriber{handle},
-        m_defaultValue{defaultValue.begin(), defaultValue.end()} {
+        m_defaultValue{defaultValue.begin(), defaultValue.end()},
+        m_info{std::move(info)...} {
   }
 
   /**
@@ -124,16 +127,21 @@
 #endif
   TimestampedValueType GetAtomic(U&& defaultValue) const {
     wpi::SmallVector<uint8_t, 128> buf;
+    size_t size = std::apply(S::GetSize, m_info);
     TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {});
-    if (view.value.size() == 0 || (view.value.size() % S::kSize) != 0) {
+    if (view.value.size() == 0 || (view.value.size() % size) != 0) {
       return {0, 0, std::forward<U>(defaultValue)};
     }
     TimestampedValueType rv{view.time, view.serverTime, {}};
-    rv.value.reserve(view.value.size() / S::kSize);
+    rv.value.reserve(view.value.size() / size);
     for (auto in = view.value.begin(), end = view.value.end(); in != end;
-         in += S::kSize) {
-      rv.value.emplace_back(
-          S::Unpack(std::span<const uint8_t, S::kSize>{in, in + S::kSize}));
+         in += size) {
+      std::apply(
+          [&](const I&... info) {
+            rv.value.emplace_back(S::Unpack(
+                std::span<const uint8_t>{std::to_address(in), size}, info...));
+          },
+          m_info);
     }
     return rv;
   }
@@ -148,16 +156,21 @@
    */
   TimestampedValueType GetAtomic(std::span<const T> defaultValue) const {
     wpi::SmallVector<uint8_t, 128> buf;
+    size_t size = std::apply(S::GetSize, m_info);
     TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {});
-    if (view.value.size() == 0 || (view.value.size() % S::kSize) != 0) {
+    if (view.value.size() == 0 || (view.value.size() % size) != 0) {
       return {0, 0, {defaultValue.begin(), defaultValue.end()}};
     }
     TimestampedValueType rv{view.time, view.serverTime, {}};
-    rv.value.reserve(view.value.size() / S::kSize);
+    rv.value.reserve(view.value.size() / size);
     for (auto in = view.value.begin(), end = view.value.end(); in != end;
-         in += S::kSize) {
-      rv.value.emplace_back(
-          S::Unpack(std::span<const uint8_t, S::kSize>{in, in + S::kSize}));
+         in += size) {
+      std::apply(
+          [&](const I&... info) {
+            rv.value.emplace_back(S::Unpack(
+                std::span<const uint8_t>{std::to_address(in), size}, info...));
+          },
+          m_info);
     }
     return rv;
   }
@@ -177,16 +190,22 @@
     auto raw = ::nt::ReadQueueRaw(m_subHandle);
     std::vector<TimestampedValueType> rv;
     rv.reserve(raw.size());
+    size_t size = std::apply(S::GetSize, m_info);
     for (auto&& r : raw) {
-      if (r.value.size() == 0 || (r.value.size() % S::kSize) != 0) {
+      if (r.value.size() == 0 || (r.value.size() % size) != 0) {
         continue;
       }
       std::vector<T> values;
-      values.reserve(r.value.size() / S::kSize);
+      values.reserve(r.value.size() / size);
       for (auto in = r.value.begin(), end = r.value.end(); in != end;
-           in += S::kSize) {
-        values.emplace_back(
-            S::Unpack(std::span<const uint8_t, S::kSize>{in, in + S::kSize}));
+           in += size) {
+        std::apply(
+            [&](const I&... info) {
+              values.emplace_back(
+                  S::Unpack(std::span<const uint8_t>{std::to_address(in), size},
+                            info...));
+            },
+            m_info);
       }
       rv.emplace_back(r.time, r.serverTime, std::move(values));
     }
@@ -199,22 +218,29 @@
    * @return Topic
    */
   TopicType GetTopic() const {
-    return StructArrayTopic<T>{::nt::GetTopicFromHandle(m_subHandle)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArrayTopic<T, I...>{
+              ::nt::GetTopicFromHandle(m_subHandle), info...};
+        },
+        m_info);
   }
 
  private:
   ValueType m_defaultValue;
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 /**
  * NetworkTables struct-encoded value array publisher.
  */
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructArrayPublisher : public Publisher {
-  using S = wpi::Struct<T>;
+  using S = wpi::Struct<T, I...>;
 
  public:
-  using TopicType = StructArrayTopic<T>;
+  using TopicType = StructArrayTopic<T, I...>;
   using ValueType = std::vector<T>;
   using ParamType = std::span<const T>;
 
@@ -227,8 +253,10 @@
    * StructTopic::Publish() instead.
    *
    * @param handle Native handle
+   * @param info optional struct type info
    */
-  explicit StructArrayPublisher(NT_Publisher handle) : Publisher{handle} {}
+  explicit StructArrayPublisher(NT_Publisher handle, I... info)
+      : Publisher{handle}, m_info{std::move(info)...} {}
 
   StructArrayPublisher(const StructArrayPublisher&) = delete;
   StructArrayPublisher& operator=(const StructArrayPublisher&) = delete;
@@ -236,15 +264,17 @@
   StructArrayPublisher(StructArrayPublisher&& rhs)
       : Publisher{std::move(rhs)},
         m_buf{std::move(rhs.m_buf)},
-        m_schemaPublished{rhs.m_schemaPublished} {}
+        m_schemaPublished{
+            rhs.m_schemaPublished.load(std::memory_order_relaxed)},
+        m_info{std::move(rhs.m_info)} {}
 
   StructArrayPublisher& operator=(StructArrayPublisher&& rhs) {
     Publisher::operator=(std::move(rhs));
     m_buf = std::move(rhs.m_buf);
-    m_schemaPublished.clear();
-    if (rhs.m_schemaPublished.test()) {
-      m_schemaPublished.test_and_set();
-    }
+    m_schemaPublished.store(
+        rhs.m_schemaPublished.load(std::memory_order_relaxed),
+        std::memory_order_relaxed);
+    m_info = std::move(rhs.m_info);
     return *this;
   }
 
@@ -260,11 +290,17 @@
              std::convertible_to<std::ranges::range_value_t<U>, T>
 #endif
   void Set(U&& value, int64_t time = 0) {
-    if (!m_schemaPublished.test_and_set()) {
-      GetTopic().GetInstance().template AddStructSchema<T>();
-    }
-    m_buf.Write(std::forward<U>(value),
-                [&](auto bytes) { ::nt::SetRaw(m_pubHandle, bytes, time); });
+    std::apply(
+        [&](const I&... info) {
+          if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
+            GetTopic().GetInstance().template AddStructSchema<T>(info...);
+          }
+          m_buf.Write(
+              std::forward<U>(value),
+              [&](auto bytes) { ::nt::SetRaw(m_pubHandle, bytes, time); },
+              info...);
+        },
+        m_info);
   }
 
   /**
@@ -274,8 +310,14 @@
    * @param time timestamp; 0 indicates current NT time should be used
    */
   void Set(std::span<const T> value, int64_t time = 0) {
-    m_buf.Write(value,
-                [&](auto bytes) { ::nt::SetRaw(m_pubHandle, bytes, time); });
+    std::apply(
+        [&](const I&... info) {
+          m_buf.Write(
+              value,
+              [&](auto bytes) { ::nt::SetRaw(m_pubHandle, bytes, time); },
+              info...);
+        },
+        m_info);
   }
 
   /**
@@ -291,11 +333,17 @@
              std::convertible_to<std::ranges::range_value_t<U>, T>
 #endif
   void SetDefault(U&& value) {
-    if (!m_schemaPublished.test_and_set()) {
-      GetTopic().GetInstance().template AddStructSchema<T>();
-    }
-    m_buf.Write(std::forward<U>(value),
-                [&](auto bytes) { ::nt::SetDefaultRaw(m_pubHandle, bytes); });
+    std::apply(
+        [&](const I&... info) {
+          if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
+            GetTopic().GetInstance().template AddStructSchema<T>(info...);
+          }
+          m_buf.Write(
+              std::forward<U>(value),
+              [&](auto bytes) { ::nt::SetDefaultRaw(m_pubHandle, bytes); },
+              info...);
+        },
+        m_info);
   }
 
   /**
@@ -306,8 +354,14 @@
    * @param value value
    */
   void SetDefault(std::span<const T> value) {
-    m_buf.Write(value,
-                [&](auto bytes) { ::nt::SetDefaultRaw(m_pubHandle, bytes); });
+    std::apply(
+        [&](const I&... info) {
+          m_buf.Write(
+              value,
+              [&](auto bytes) { ::nt::SetDefaultRaw(m_pubHandle, bytes); },
+              info...);
+        },
+        m_info);
   }
 
   /**
@@ -316,12 +370,18 @@
    * @return Topic
    */
   TopicType GetTopic() const {
-    return StructArrayTopic<T>{::nt::GetTopicFromHandle(m_pubHandle)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArrayTopic<T, I...>{
+              ::nt::GetTopicFromHandle(m_pubHandle), info...};
+        },
+        m_info);
   }
 
  private:
-  wpi::StructArrayBuffer<T> m_buf;
-  std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT;
+  wpi::StructArrayBuffer<T, I...> m_buf;
+  std::atomic_bool m_schemaPublished{false};
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 /**
@@ -329,13 +389,14 @@
  *
  * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
  */
-template <wpi::StructSerializable T>
-class StructArrayEntry final : public StructArraySubscriber<T>,
-                               public StructArrayPublisher<T> {
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
+class StructArrayEntry final : public StructArraySubscriber<T, I...>,
+                               public StructArrayPublisher<T, I...> {
  public:
-  using SubscriberType = StructArraySubscriber<T>;
-  using PublisherType = StructArrayPublisher<T>;
-  using TopicType = StructArrayTopic<T>;
+  using SubscriberType = StructArraySubscriber<T, I...>;
+  using PublisherType = StructArrayPublisher<T, I...>;
+  using TopicType = StructArrayTopic<T, I...>;
   using ValueType = std::vector<T>;
   using ParamType = std::span<const T>;
 
@@ -349,15 +410,16 @@
    *
    * @param handle Native handle
    * @param defaultValue Default value
+   * @param info optional struct type info
    */
   template <typename U>
 #if __cpp_lib_ranges >= 201911L
     requires std::ranges::range<U> &&
                  std::convertible_to<std::ranges::range_value_t<U>, T>
 #endif
-  StructArrayEntry(NT_Entry handle, U&& defaultValue)
-      : StructArraySubscriber<T>{handle, defaultValue},
-        StructArrayPublisher<T>{handle} {
+  StructArrayEntry(NT_Entry handle, U&& defaultValue, const I&... info)
+      : StructArraySubscriber<T, I...>{handle, defaultValue, info...},
+        StructArrayPublisher<T, I...>{handle, info...} {
   }
 
   /**
@@ -380,7 +442,7 @@
    * @return Topic
    */
   TopicType GetTopic() const {
-    return StructArrayTopic<T>{::nt::GetTopicFromHandle(this->m_subHandle)};
+    return StructArraySubscriber<T, I...>::GetTopic();
   }
 
   /**
@@ -392,12 +454,13 @@
 /**
  * NetworkTables struct-encoded value array topic.
  */
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructArrayTopic final : public Topic {
  public:
-  using SubscriberType = StructArraySubscriber<T>;
-  using PublisherType = StructArrayPublisher<T>;
-  using EntryType = StructArrayEntry<T>;
+  using SubscriberType = StructArraySubscriber<T, I...>;
+  using PublisherType = StructArrayPublisher<T, I...>;
+  using EntryType = StructArrayEntry<T, I...>;
   using ValueType = std::vector<T>;
   using ParamType = std::span<const T>;
   using TimestampedValueType = Timestamped<ValueType>;
@@ -409,15 +472,19 @@
    * NetworkTableInstance::GetStructTopic() instead.
    *
    * @param handle Native handle
+   * @param info optional struct type info
    */
-  explicit StructArrayTopic(NT_Topic handle) : Topic{handle} {}
+  explicit StructArrayTopic(NT_Topic handle, I... info)
+      : Topic{handle}, m_info{std::move(info)...} {}
 
   /**
    * Construct from a generic topic.
    *
    * @param topic Topic
+   * @param info optional struct type info
    */
-  explicit StructArrayTopic(Topic topic) : Topic{topic} {}
+  explicit StructArrayTopic(Topic topic, I... info)
+      : Topic{topic}, m_info{std::move(info)...} {}
 
   /**
    * Create a new subscriber to the topic.
@@ -442,11 +509,17 @@
   [[nodiscard]]
   SubscriberType Subscribe(
       U&& defaultValue, const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructArraySubscriber<T>{
-        ::nt::Subscribe(
-            m_handle, NT_RAW,
-            wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(), options),
-        defaultValue};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArraySubscriber<T, I...>{
+              ::nt::Subscribe(
+                  m_handle, NT_RAW,
+                  wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(
+                      info...),
+                  options),
+              defaultValue, info...};
+        },
+        m_info);
   }
 
   /**
@@ -468,11 +541,17 @@
   SubscriberType Subscribe(
       std::span<const T> defaultValue,
       const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructArraySubscriber<T>{
-        ::nt::Subscribe(
-            m_handle, NT_RAW,
-            wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(), options),
-        defaultValue};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArraySubscriber<T, I...>{
+              ::nt::Subscribe(
+                  m_handle, NT_RAW,
+                  wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(
+                      info...),
+                  options),
+              defaultValue, info...};
+        },
+        m_info);
   }
 
   /**
@@ -492,9 +571,17 @@
    */
   [[nodiscard]]
   PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructArrayPublisher<T>{::nt::Publish(
-        m_handle, NT_RAW,
-        wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(), options)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArrayPublisher<T, I...>{
+              ::nt::Publish(
+                  m_handle, NT_RAW,
+                  wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(
+                      info...),
+                  options),
+              info...};
+        },
+        m_info);
   }
 
   /**
@@ -518,10 +605,17 @@
   PublisherType PublishEx(
       const wpi::json& properties,
       const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructArrayPublisher<T>{::nt::PublishEx(
-        m_handle, NT_RAW,
-        wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(), properties,
-        options)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArrayPublisher<T, I...>{
+              ::nt::PublishEx(
+                  m_handle, NT_RAW,
+                  wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(
+                      info...),
+                  properties, options),
+              info...};
+        },
+        m_info);
   }
 
   /**
@@ -552,11 +646,17 @@
   [[nodiscard]]
   EntryType GetEntry(U&& defaultValue,
                      const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructArrayEntry<T>{
-        ::nt::GetEntry(m_handle, NT_RAW,
-                       wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(),
-                       options),
-        defaultValue};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArrayEntry<T, I...>{
+              ::nt::GetEntry(
+                  m_handle, NT_RAW,
+                  wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(
+                      info...),
+                  options),
+              defaultValue, info...};
+        },
+        m_info);
   }
 
   /**
@@ -582,12 +682,21 @@
   [[nodiscard]]
   EntryType GetEntry(std::span<const T> defaultValue,
                      const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructArrayEntry<T>{
-        ::nt::GetEntry(m_handle, NT_RAW,
-                       wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(),
-                       options),
-        defaultValue};
+    return std::apply(
+        [&](const I&... info) {
+          return StructArrayEntry<T, I...>{
+              ::nt::GetEntry(
+                  m_handle, NT_RAW,
+                  wpi::MakeStructArrayTypeString<T, std::dynamic_extent>(
+                      info...),
+                  options),
+              defaultValue, info...};
+        },
+        m_info);
   }
+
+ private:
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructTopic.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructTopic.h
index 88da9f3..b69d0af 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructTopic.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/StructTopic.h
@@ -8,36 +8,37 @@
 
 #include <atomic>
 #include <concepts>
+#include <functional>
 #include <span>
 #include <string_view>
+#include <tuple>
 #include <utility>
 #include <vector>
 
 #include <wpi/SmallVector.h>
+#include <wpi/json_fwd.h>
 #include <wpi/struct/Struct.h>
 
 #include "networktables/NetworkTableInstance.h"
 #include "networktables/Topic.h"
 #include "ntcore_cpp.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace nt {
 
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructTopic;
 
 /**
  * NetworkTables struct-encoded value subscriber.
  */
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructSubscriber : public Subscriber {
-  using S = wpi::Struct<T>;
+  using S = wpi::Struct<T, I...>;
 
  public:
-  using TopicType = StructTopic<T>;
+  using TopicType = StructTopic<T, I...>;
   using ValueType = T;
   using ParamType = const T&;
   using TimestampedValueType = Timestamped<T>;
@@ -50,9 +51,12 @@
    *
    * @param handle Native handle
    * @param defaultValue Default value
+   * @param info optional struct type info
    */
-  StructSubscriber(NT_Subscriber handle, T defaultValue)
-      : Subscriber{handle}, m_defaultValue{std::move(defaultValue)} {}
+  StructSubscriber(NT_Subscriber handle, T defaultValue, I... info)
+      : Subscriber{handle},
+        m_defaultValue{std::move(defaultValue)},
+        m_info{std::move(info)...} {}
 
   /**
    * Get the last published value.
@@ -84,12 +88,16 @@
    * @return true if successful
    */
   bool GetInto(T* out) {
-    wpi::SmallVector<uint8_t, S::kSize> buf;
+    wpi::SmallVector<uint8_t, 128> buf;
     TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {});
-    if (view.value.size() < S::kSize) {
+    if (view.value.size() < std::apply(S::GetSize, m_info)) {
       return false;
     } else {
-      wpi::UnpackStructInto(out, view.value.subspan<0, S::kSize>());
+      std::apply(
+          [&](const I&... info) {
+            wpi::UnpackStructInto(out, view.value, info...);
+          },
+          m_info);
       return true;
     }
   }
@@ -112,13 +120,16 @@
    * @return timestamped value
    */
   TimestampedValueType GetAtomic(const T& defaultValue) const {
-    wpi::SmallVector<uint8_t, S::kSize> buf;
+    wpi::SmallVector<uint8_t, 128> buf;
     TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {});
-    if (view.value.size() < S::kSize) {
+    if (view.value.size() < std::apply(S::GetSize, m_info)) {
       return {0, 0, defaultValue};
     } else {
-      return {view.time, view.serverTime,
-              S::Unpack(view.value.subspan<0, S::kSize>())};
+      return {
+          view.time, view.serverTime,
+          std::apply(
+              [&](const I&... info) { return S::Unpack(view.value, info...); },
+              m_info)};
     }
   }
 
@@ -138,13 +149,16 @@
     std::vector<TimestampedValueType> rv;
     rv.reserve(raw.size());
     for (auto&& r : raw) {
-      if (r.value.size() < S::kSize) {
+      if (r.value.size() < std::apply(S::GetSize, m_info)) {
         continue;
       } else {
-        rv.emplace_back(
-            r.time, r.serverTime,
-            S::Unpack(
-                std::span<const uint8_t>(r.value).subspan<0, S::kSize>()));
+        std::apply(
+            [&](const I&... info) {
+              rv.emplace_back(
+                  r.time, r.serverTime,
+                  S::Unpack(std::span<const uint8_t>(r.value), info...));
+            },
+            m_info);
       }
     }
     return rv;
@@ -156,22 +170,29 @@
    * @return Topic
    */
   TopicType GetTopic() const {
-    return StructTopic<T>{::nt::GetTopicFromHandle(m_subHandle)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructTopic<T, I...>{::nt::GetTopicFromHandle(m_subHandle),
+                                      info...};
+        },
+        m_info);
   }
 
  private:
   ValueType m_defaultValue;
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 /**
  * NetworkTables struct-encoded value publisher.
  */
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructPublisher : public Publisher {
-  using S = wpi::Struct<T>;
+  using S = wpi::Struct<T, I...>;
 
  public:
-  using TopicType = StructTopic<T>;
+  using TopicType = StructTopic<T, I...>;
   using ValueType = T;
   using ParamType = const T&;
 
@@ -183,14 +204,17 @@
   StructPublisher& operator=(const StructPublisher&) = delete;
 
   StructPublisher(StructPublisher&& rhs)
-      : Publisher{std::move(rhs)}, m_schemaPublished{rhs.m_schemaPublished} {}
+      : Publisher{std::move(rhs)},
+        m_schemaPublished{
+            rhs.m_schemaPublished.load(std::memory_order_relaxed)},
+        m_info{std::move(rhs.m_info)} {}
 
   StructPublisher& operator=(StructPublisher&& rhs) {
     Publisher::operator=(std::move(rhs));
-    m_schemaPublished.clear();
-    if (rhs.m_schemaPublished.test()) {
-      m_schemaPublished.test_and_set();
-    }
+    m_schemaPublished.store(
+        rhs.m_schemaPublished.load(std::memory_order_relaxed),
+        std::memory_order_relaxed);
+    m_info = std::move(rhs.m_info);
     return *this;
   }
 
@@ -199,8 +223,10 @@
    * StructTopic::Publish() instead.
    *
    * @param handle Native handle
+   * @param info optional struct type info
    */
-  explicit StructPublisher(NT_Publisher handle) : Publisher{handle} {}
+  explicit StructPublisher(NT_Publisher handle, I... info)
+      : Publisher{handle}, m_info{std::move(info)...} {}
 
   /**
    * Publish a new value.
@@ -209,11 +235,24 @@
    * @param time timestamp; 0 indicates current NT time should be used
    */
   void Set(const T& value, int64_t time = 0) {
-    if (!m_schemaPublished.test_and_set()) {
-      GetTopic().GetInstance().template AddStructSchema<T>();
+    if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
+      std::apply(
+          [&](const I&... info) {
+            GetTopic().GetInstance().template AddStructSchema<T>(info...);
+          },
+          m_info);
     }
-    uint8_t buf[S::kSize];
-    S::Pack(buf, value);
+    if constexpr (sizeof...(I) == 0) {
+      if constexpr (wpi::is_constexpr([] { S::GetSize(); })) {
+        uint8_t buf[S::GetSize()];
+        S::Pack(buf, value);
+        ::nt::SetRaw(m_pubHandle, buf, time);
+        return;
+      }
+    }
+    wpi::SmallVector<uint8_t, 128> buf;
+    buf.resize_for_overwrite(std::apply(S::GetSize, m_info));
+    std::apply([&](const I&... info) { S::Pack(buf, value, info...); }, m_info);
     ::nt::SetRaw(m_pubHandle, buf, time);
   }
 
@@ -225,11 +264,24 @@
    * @param value value
    */
   void SetDefault(const T& value) {
-    if (!m_schemaPublished.test_and_set()) {
-      GetTopic().GetInstance().template AddStructSchema<T>();
+    if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
+      std::apply(
+          [&](const I&... info) {
+            GetTopic().GetInstance().template AddStructSchema<T>(info...);
+          },
+          m_info);
     }
-    uint8_t buf[S::kSize];
-    S::Pack(buf, value);
+    if constexpr (sizeof...(I) == 0) {
+      if constexpr (wpi::is_constexpr([] { S::GetSize(); })) {
+        uint8_t buf[S::GetSize()];
+        S::Pack(buf, value);
+        ::nt::SetDefaultRaw(m_pubHandle, buf);
+        return;
+      }
+    }
+    wpi::SmallVector<uint8_t, 128> buf;
+    buf.resize_for_overwrite(std::apply(S::GetSize, m_info));
+    std::apply([&](const I&... info) { S::Pack(buf, value, info...); }, m_info);
     ::nt::SetDefaultRaw(m_pubHandle, buf);
   }
 
@@ -239,11 +291,17 @@
    * @return Topic
    */
   TopicType GetTopic() const {
-    return StructTopic<T>{::nt::GetTopicFromHandle(m_pubHandle)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructTopic<T, I...>{::nt::GetTopicFromHandle(m_pubHandle),
+                                      info...};
+        },
+        m_info);
   }
 
  private:
-  std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT;
+  std::atomic_bool m_schemaPublished{false};
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 /**
@@ -251,13 +309,14 @@
  *
  * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed.
  */
-template <wpi::StructSerializable T>
-class StructEntry final : public StructSubscriber<T>,
-                          public StructPublisher<T> {
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
+class StructEntry final : public StructSubscriber<T, I...>,
+                          public StructPublisher<T, I...> {
  public:
-  using SubscriberType = StructSubscriber<T>;
-  using PublisherType = StructPublisher<T>;
-  using TopicType = StructTopic<T>;
+  using SubscriberType = StructSubscriber<T, I...>;
+  using PublisherType = StructPublisher<T, I...>;
+  using TopicType = StructTopic<T, I...>;
   using ValueType = T;
   using ParamType = const T&;
 
@@ -271,10 +330,11 @@
    *
    * @param handle Native handle
    * @param defaultValue Default value
+   * @param info optional struct type info
    */
-  StructEntry(NT_Entry handle, T defaultValue)
-      : StructSubscriber<T>{handle, std::move(defaultValue)},
-        StructPublisher<T>{handle} {}
+  StructEntry(NT_Entry handle, T defaultValue, const I&... info)
+      : StructSubscriber<T, I...>{handle, std::move(defaultValue), info...},
+        StructPublisher<T, I...>{handle, info...} {}
 
   /**
    * Determines if the native handle is valid.
@@ -295,9 +355,7 @@
    *
    * @return Topic
    */
-  TopicType GetTopic() const {
-    return StructTopic<T>{::nt::GetTopicFromHandle(this->m_subHandle)};
-  }
+  TopicType GetTopic() const { return StructSubscriber<T, I...>::GetTopic(); }
 
   /**
    * Stops publishing the entry if it's published.
@@ -308,12 +366,13 @@
 /**
  * NetworkTables struct-encoded value topic.
  */
-template <wpi::StructSerializable T>
+template <typename T, typename... I>
+  requires wpi::StructSerializable<T, I...>
 class StructTopic final : public Topic {
  public:
-  using SubscriberType = StructSubscriber<T>;
-  using PublisherType = StructPublisher<T>;
-  using EntryType = StructEntry<T>;
+  using SubscriberType = StructSubscriber<T, I...>;
+  using PublisherType = StructPublisher<T, I...>;
+  using EntryType = StructEntry<T, I...>;
   using ValueType = T;
   using ParamType = const T&;
   using TimestampedValueType = Timestamped<T>;
@@ -325,15 +384,19 @@
    * NetworkTableInstance::GetStructTopic() instead.
    *
    * @param handle Native handle
+   * @param info optional struct type info
    */
-  explicit StructTopic(NT_Topic handle) : Topic{handle} {}
+  explicit StructTopic(NT_Topic handle, I... info)
+      : Topic{handle}, m_info{std::move(info)...} {}
 
   /**
    * Construct from a generic topic.
    *
    * @param topic Topic
+   * @param info optional struct type info
    */
-  explicit StructTopic(Topic topic) : Topic{topic} {}
+  explicit StructTopic(Topic topic, I... info)
+      : Topic{topic}, m_info{std::move(info)...} {}
 
   /**
    * Create a new subscriber to the topic.
@@ -353,10 +416,15 @@
   [[nodiscard]]
   SubscriberType Subscribe(
       T defaultValue, const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructSubscriber<T>{
-        ::nt::Subscribe(m_handle, NT_RAW, wpi::GetStructTypeString<T>(),
-                        options),
-        std::move(defaultValue)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructSubscriber<T, I...>{
+              ::nt::Subscribe(m_handle, NT_RAW,
+                              wpi::GetStructTypeString<T, I...>(info...),
+                              options),
+              std::move(defaultValue), info...};
+        },
+        m_info);
   }
 
   /**
@@ -376,8 +444,15 @@
    */
   [[nodiscard]]
   PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructPublisher<T>{::nt::Publish(
-        m_handle, NT_RAW, wpi::GetStructTypeString<T>(), options)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructPublisher<T, I...>{
+              ::nt::Publish(m_handle, NT_RAW,
+                            wpi::GetStructTypeString<T, I...>(info...),
+                            options),
+              info...};
+        },
+        m_info);
   }
 
   /**
@@ -401,8 +476,15 @@
   PublisherType PublishEx(
       const wpi::json& properties,
       const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructPublisher<T>{::nt::PublishEx(
-        m_handle, NT_RAW, wpi::GetStructTypeString<T>(), properties, options)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructPublisher<T, I...>{
+              ::nt::PublishEx(m_handle, NT_RAW,
+                              wpi::GetStructTypeString<T, I...>(info...),
+                              properties, options),
+              info...};
+        },
+        m_info);
   }
 
   /**
@@ -428,11 +510,19 @@
   [[nodiscard]]
   EntryType GetEntry(T defaultValue,
                      const PubSubOptions& options = kDefaultPubSubOptions) {
-    return StructEntry<T>{
-        ::nt::GetEntry(m_handle, NT_RAW, wpi::GetStructTypeString<T>(),
-                       options),
-        std::move(defaultValue)};
+    return std::apply(
+        [&](const I&... info) {
+          return StructEntry<T, I...>{
+              ::nt::GetEntry(m_handle, NT_RAW,
+                             wpi::GetStructTypeString<T, I...>(info...),
+                             options),
+              std::move(defaultValue), info...};
+        },
+        m_info);
   }
+
+ private:
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.h
index d623fbd..eedd487 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.h
@@ -103,6 +103,21 @@
   bool IsRetained() const;
 
   /**
+   * Allow storage of the topic's last value, allowing the value to be read (and
+   * not just accessed through event queues and listeners).
+   *
+   * @param cached True for cached, false for not cached.
+   */
+  void SetCached(bool cached);
+
+  /**
+   * Returns whether the topic's last value is stored.
+   *
+   * @return True if the topic is cached.
+   */
+  bool IsCached() const;
+
+  /**
    * Determines if the topic is currently being published.
    *
    * @return True if the topic exists, false otherwise.
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.inc b/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.inc
index 166dc6d..605f9d8 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.inc
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/Topic.inc
@@ -41,6 +41,14 @@
   return ::nt::GetTopicRetained(m_handle);
 }
 
+inline void Topic::SetCached(bool cached) {
+  ::nt::SetTopicCached(m_handle, cached);
+}
+
+inline bool Topic::IsCached() const {
+  return ::nt::GetTopicCached(m_handle);
+}
+
 inline bool Topic::Exists() const {
   return nt::GetTopicExists(m_handle);
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h
index 1af0e66..a335f81 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h
@@ -65,7 +65,11 @@
 };
 
 /** NetworkTables entry flags. */
-enum NT_EntryFlags { NT_PERSISTENT = 0x01, NT_RETAINED = 0x02 };
+enum NT_EntryFlags {
+  NT_PERSISTENT = 0x01,
+  NT_RETAINED = 0x02,
+  NT_UNCACHED = 0x04
+};
 
 /** NetworkTables logging levels. */
 enum NT_LogLevel {
@@ -686,6 +690,24 @@
 NT_Bool NT_GetTopicRetained(NT_Topic topic);
 
 /**
+ * Sets the cached property of a topic.  If true, the server and clients will
+ * store the latest value, allowing the value to be read (and not just accessed
+ * through event queues and listeners).
+ *
+ * @param topic topic handle
+ * @param value True for cached, false for not cached
+ */
+void NT_SetTopicCached(NT_Topic topic, NT_Bool value);
+
+/**
+ * Gets the cached property of a topic.
+ *
+ * @param topic topic handle
+ * @return cached property value
+ */
+NT_Bool NT_GetTopicCached(NT_Topic topic);
+
+/**
  * Determine if topic exists (e.g. has at least one publisher).
  *
  * @param handle Topic, entry, or subscriber handle.
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h
index 482d1e3..e2529e1 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h
@@ -682,6 +682,24 @@
 bool GetTopicRetained(NT_Topic topic);
 
 /**
+ * Sets the cached property of a topic.  If true, the server and clients will
+ * store the latest value, allowing the value to be read (and not just accessed
+ * through event queues and listeners).
+ *
+ * @param topic topic handle
+ * @param value True for cached, false for not cached
+ */
+void SetTopicCached(NT_Topic topic, bool value);
+
+/**
+ * Gets the cached property of a topic.
+ *
+ * @param topic topic handle
+ * @return cached property value
+ */
+bool GetTopicCached(NT_Topic topic);
+
+/**
  * Determine if topic exists (e.g. has at least one publisher).
  *
  * @param handle Topic, entry, or subscriber handle.
diff --git a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java
index 4c431af..bca694b 100644
--- a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java
+++ b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java
@@ -6,7 +6,6 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import java.util.Arrays;
 import java.util.Collections;
 import java.util.List;
 import java.util.stream.Stream;
@@ -61,8 +60,8 @@
     return Stream.of(
         Arguments.of(Collections.singletonList("/"), ""),
         Arguments.of(Collections.singletonList("/"), "/"),
-        Arguments.of(Arrays.asList("/", "/foo", "/foo/bar", "/foo/bar/baz"), "/foo/bar/baz"),
-        Arguments.of(Arrays.asList("/", "/foo", "/foo/bar", "/foo/bar/"), "/foo/bar/"));
+        Arguments.of(List.of("/", "/foo", "/foo/bar", "/foo/bar/baz"), "/foo/bar/baz"),
+        Arguments.of(List.of("/", "/foo", "/foo/bar", "/foo/bar/"), "/foo/bar/"));
   }
 
   @ParameterizedTest
diff --git a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/RawTest.java b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/RawTest.java
index 73d5efb..293d443 100644
--- a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/RawTest.java
+++ b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/RawTest.java
@@ -4,11 +4,10 @@
 
 package edu.wpi.first.networktables;
 
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import java.nio.ByteBuffer;
-import java.util.Arrays;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -31,9 +30,9 @@
   void testGenericByteArray() {
     GenericEntry entry = m_inst.getTopic("test").getGenericEntry("raw");
     entry.setRaw(new byte[] {5}, 10);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {5}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {5});
     entry.setRaw(new byte[] {5, 6, 7}, 1, 2, 15);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {6, 7}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {6, 7});
     assertThrows(IndexOutOfBoundsException.class, () -> entry.setRaw(new byte[] {5}, -1, 2, 20));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.setRaw(new byte[] {5}, 1, -2, 20));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.setRaw(new byte[] {5}, 1, 1, 20));
@@ -43,9 +42,9 @@
   void testRawByteArray() {
     RawEntry entry = m_inst.getRawTopic("test").getEntry("raw", new byte[] {});
     entry.set(new byte[] {5}, 10);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {5}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {5});
     entry.set(new byte[] {5, 6, 7}, 1, 2, 15);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {6, 7}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {6, 7});
     assertThrows(IndexOutOfBoundsException.class, () -> entry.set(new byte[] {5}, -1, 1, 20));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.set(new byte[] {5}, 1, -1, 20));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.set(new byte[] {5}, 1, 1, 20));
@@ -55,15 +54,15 @@
   void testGenericByteBuffer() {
     GenericEntry entry = m_inst.getTopic("test").getGenericEntry("raw");
     entry.setRaw(ByteBuffer.wrap(new byte[] {5}), 10);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {5}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {5});
     entry.setRaw(ByteBuffer.wrap(new byte[] {5, 6, 7}).position(1), 15);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {6, 7}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {6, 7});
     entry.setRaw(ByteBuffer.wrap(new byte[] {5, 6, 7}).position(1).limit(2), 16);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {6}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {6});
     entry.setRaw(ByteBuffer.wrap(new byte[] {8, 9, 0}), 1, 2, 20);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {9, 0}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {9, 0});
     entry.setRaw(ByteBuffer.wrap(new byte[] {1, 2, 3}).position(2), 0, 2, 25);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {1, 2}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {1, 2});
     assertThrows(
         IndexOutOfBoundsException.class,
         () -> entry.setRaw(ByteBuffer.wrap(new byte[] {5}), -1, 1, 30));
@@ -79,15 +78,15 @@
   void testRawByteBuffer() {
     RawEntry entry = m_inst.getRawTopic("test").getEntry("raw", new byte[] {});
     entry.set(ByteBuffer.wrap(new byte[] {5}), 10);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {5}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {5});
     entry.set(ByteBuffer.wrap(new byte[] {5, 6, 7}).position(1), 15);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {6, 7}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {6, 7});
     entry.set(ByteBuffer.wrap(new byte[] {5, 6, 7}).position(1).limit(2), 16);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {6}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {6});
     entry.set(ByteBuffer.wrap(new byte[] {8, 9, 0}), 1, 2, 20);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {9, 0}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {9, 0});
     entry.set(ByteBuffer.wrap(new byte[] {1, 2, 3}).position(2), 0, 2, 25);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {1, 2}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {1, 2});
     assertThrows(
         IndexOutOfBoundsException.class,
         () -> entry.set(ByteBuffer.wrap(new byte[] {5}), -1, 1, 30));
@@ -105,13 +104,13 @@
     ByteBuffer bb = ByteBuffer.allocateDirect(3);
     bb.put(new byte[] {5, 6, 7});
     entry.setRaw(bb.position(1), 15);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {6, 7}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {6, 7});
     entry.setRaw(bb.limit(2), 16);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {6}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {6});
     bb.clear();
     bb.put(new byte[] {8, 9, 0});
     entry.setRaw(bb, 1, 2, 20);
-    assertTrue(Arrays.equals(entry.getRaw(new byte[] {}), new byte[] {9, 0}));
+    assertArrayEquals(entry.getRaw(new byte[] {}), new byte[] {9, 0});
     assertThrows(IndexOutOfBoundsException.class, () -> entry.setRaw(bb, -1, 1, 25));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.setRaw(bb, 1, -1, 25));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.setRaw(bb, 2, 2, 25));
@@ -123,13 +122,13 @@
     ByteBuffer bb = ByteBuffer.allocateDirect(3);
     bb.put(new byte[] {5, 6, 7});
     entry.set(bb.position(1), 15);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {6, 7}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {6, 7});
     entry.set(bb.limit(2), 16);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {6}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {6});
     bb.clear();
     bb.put(new byte[] {8, 9, 0});
     entry.set(bb, 1, 2, 20);
-    assertTrue(Arrays.equals(entry.get(new byte[] {}), new byte[] {9, 0}));
+    assertArrayEquals(entry.get(new byte[] {}), new byte[] {9, 0});
     assertThrows(IndexOutOfBoundsException.class, () -> entry.set(bb, -1, 1, 25));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.set(bb, 1, -1, 25));
     assertThrows(IndexOutOfBoundsException.class, () -> entry.set(bb, 2, 2, 25));
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/LocalStorageTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/LocalStorageTest.cpp
index eb57913..cdb5272 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/LocalStorageTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/LocalStorageTest.cpp
@@ -101,6 +101,17 @@
   EXPECT_FALSE(storage.GetTopicExists(fooTopic));
 }
 
+TEST_F(LocalStorageTest, DefaultProps) {
+  EXPECT_CALL(network, Publish(_, fooTopic, std::string_view{"foo"},
+                               std::string_view{"boolean"}, wpi::json::object(),
+                               IsDefaultPubSubOptions()));
+  storage.Publish(fooTopic, NT_BOOLEAN, "boolean", wpi::json::object(), {});
+
+  EXPECT_FALSE(storage.GetTopicPersistent(fooTopic));
+  EXPECT_FALSE(storage.GetTopicRetained(fooTopic));
+  EXPECT_TRUE(storage.GetTopicCached(fooTopic));
+}
+
 TEST_F(LocalStorageTest, PublishNewNoProps) {
   EXPECT_CALL(network, Publish(_, fooTopic, std::string_view{"foo"},
                                std::string_view{"boolean"}, wpi::json::object(),
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/StructTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/StructTest.cpp
new file mode 100644
index 0000000..55b6a90
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/StructTest.cpp
@@ -0,0 +1,450 @@
+// 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 <gtest/gtest.h>
+#include <wpi/SpanMatcher.h>
+#include <wpi/struct/Struct.h>
+
+#include "networktables/NetworkTableInstance.h"
+#include "networktables/StructArrayTopic.h"
+#include "networktables/StructTopic.h"
+
+namespace {
+struct Inner {
+  int a = 0;
+  int b = 0;
+};
+
+struct Outer {
+  Inner inner;
+  int c = 0;
+};
+
+struct Inner2 {
+  int a = 0;
+  int b = 0;
+};
+
+struct Outer2 {
+  Inner2 inner;
+  int c = 0;
+};
+
+struct ThingA {
+  int x = 0;
+};
+
+struct ThingB {
+  int x = 0;
+};
+
+struct Info1 {
+  int info = 0;
+};
+}  // namespace
+
+template <>
+struct wpi::Struct<Inner> {
+  static constexpr std::string_view GetTypeString() { return "struct:Inner"; }
+  static constexpr size_t GetSize() { return 8; }
+  static constexpr std::string_view GetSchema() { return "int32 a; int32 b"; }
+
+  static Inner Unpack(std::span<const uint8_t> data) {
+    return {wpi::UnpackStruct<int32_t, 0>(data),
+            wpi::UnpackStruct<int32_t, 4>(data)};
+  }
+  static void Pack(std::span<uint8_t> data, const Inner& value) {
+    wpi::PackStruct<0>(data, value.a);
+    wpi::PackStruct<4>(data, value.b);
+  }
+};
+
+template <>
+struct wpi::Struct<Outer> {
+  static constexpr std::string_view GetTypeString() { return "struct:Outer"; }
+  static constexpr size_t GetSize() { return wpi::GetStructSize<Inner>() + 4; }
+  static constexpr std::string_view GetSchema() {
+    return "Inner inner; int32 c";
+  }
+
+  static Outer Unpack(std::span<const uint8_t> data) {
+    constexpr size_t innerSize = wpi::GetStructSize<Inner>();
+    return {wpi::UnpackStruct<Inner, 0>(data),
+            wpi::UnpackStruct<int32_t, innerSize>(data)};
+  }
+  static void Pack(std::span<uint8_t> data, const Outer& value) {
+    constexpr size_t innerSize = wpi::GetStructSize<Inner>();
+    wpi::PackStruct<0>(data, value.inner);
+    wpi::PackStruct<innerSize>(data, value.c);
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<Inner>(fn);
+  }
+};
+
+template <>
+struct wpi::Struct<Inner2> {
+  static std::string_view GetTypeString() { return "struct:Inner2"; }
+  static size_t GetSize() { return 8; }
+  static std::string_view GetSchema() { return "int32 a; int32 b"; }
+
+  static Inner2 Unpack(std::span<const uint8_t> data) {
+    return {wpi::UnpackStruct<int32_t, 0>(data),
+            wpi::UnpackStruct<int32_t, 4>(data)};
+  }
+  static void Pack(std::span<uint8_t> data, const Inner2& value) {
+    wpi::PackStruct<0>(data, value.a);
+    wpi::PackStruct<4>(data, value.b);
+  }
+};
+
+template <>
+struct wpi::Struct<Outer2> {
+  static std::string_view GetTypeString() { return "struct:Outer2"; }
+  static size_t GetSize() { return wpi::GetStructSize<Inner>() + 4; }
+  static std::string_view GetSchema() { return "Inner2 inner; int32 c"; }
+
+  static Outer2 Unpack(std::span<const uint8_t> data) {
+    size_t innerSize = wpi::GetStructSize<Inner2>();
+    return {wpi::UnpackStruct<Inner2, 0>(data),
+            wpi::UnpackStruct<int32_t>(data.subspan(innerSize))};
+  }
+  static void Pack(std::span<uint8_t> data, const Outer2& value) {
+    size_t innerSize = wpi::GetStructSize<Inner2>();
+    wpi::PackStruct<0>(data, value.inner);
+    wpi::PackStruct(data.subspan(innerSize), value.c);
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<Inner2>(fn);
+  }
+};
+
+template <>
+struct wpi::Struct<ThingA> {
+  static constexpr std::string_view GetTypeString() { return "struct:ThingA"; }
+  static constexpr size_t GetSize() { return 1; }
+  static constexpr std::string_view GetSchema() { return "uint8 value"; }
+  static ThingA Unpack(std::span<const uint8_t> data) {
+    return ThingA{.x = data[0]};
+  }
+  static void Pack(std::span<uint8_t> data, const ThingA& value) {
+    data[0] = value.x;
+  }
+};
+
+template <>
+struct wpi::Struct<ThingB, Info1> {
+  static constexpr std::string_view GetTypeString(const Info1&) {
+    return "struct:ThingB";
+  }
+  static constexpr size_t GetSize(const Info1&) { return 1; }
+  static constexpr std::string_view GetSchema(const Info1&) {
+    return "uint8 value";
+  }
+  static ThingB Unpack(std::span<const uint8_t> data, const Info1&) {
+    return ThingB{.x = data[0]};
+  }
+  static void Pack(std::span<uint8_t> data, const ThingB& value, const Info1&) {
+    data[0] = value.x;
+  }
+};
+
+namespace nt {
+
+class StructTest : public ::testing::Test {
+ public:
+  StructTest() { inst = nt::NetworkTableInstance::Create(); }
+  ~StructTest() { nt::NetworkTableInstance::Destroy(inst); }
+
+  nt::NetworkTableInstance inst;
+};
+
+TEST_F(StructTest, InnerConstexpr) {
+  nt::StructTopic<Inner> topic = inst.GetStructTopic<Inner>("inner");
+  nt::StructPublisher<Inner> pub = topic.Publish();
+  nt::StructSubscriber<Inner> sub = topic.Subscribe({});
+
+  ASSERT_EQ(topic.GetTypeString(), "struct:Inner");
+
+  pub.SetDefault({0, 1});
+  Inner val = sub.Get();
+  ASSERT_EQ(val.a, 0);
+  ASSERT_EQ(val.b, 1);
+
+  pub.Set({1, 2});
+  auto atomicVal = sub.GetAtomic();
+  ASSERT_EQ(atomicVal.value.a, 1);
+  ASSERT_EQ(atomicVal.value.b, 2);
+
+  Inner val2;
+  sub.GetInto(&val2);
+  ASSERT_EQ(val2.a, 1);
+  ASSERT_EQ(val2.b, 2);
+
+  auto vals = sub.ReadQueue();
+  ASSERT_EQ(vals.size(), 1u);
+  ASSERT_EQ(vals[0].value.a, 1);
+  ASSERT_EQ(vals[0].value.b, 2);
+}
+
+TEST_F(StructTest, InnerNonconstexpr) {
+  nt::StructTopic<Inner2> topic = inst.GetStructTopic<Inner2>("inner2");
+  nt::StructPublisher<Inner2> pub = topic.Publish();
+  nt::StructSubscriber<Inner2> sub = topic.Subscribe({});
+
+  ASSERT_EQ(topic.GetTypeString(), "struct:Inner2");
+
+  pub.SetDefault({0, 1});
+  Inner2 val = sub.Get();
+  ASSERT_EQ(val.a, 0);
+  ASSERT_EQ(val.b, 1);
+
+  pub.Set({1, 2});
+  auto atomicVal = sub.GetAtomic();
+  ASSERT_EQ(atomicVal.value.a, 1);
+  ASSERT_EQ(atomicVal.value.b, 2);
+
+  Inner2 val2;
+  sub.GetInto(&val2);
+  ASSERT_EQ(val2.a, 1);
+  ASSERT_EQ(val2.b, 2);
+
+  auto vals = sub.ReadQueue();
+  ASSERT_EQ(vals.size(), 1u);
+  ASSERT_EQ(vals[0].value.a, 1);
+  ASSERT_EQ(vals[0].value.b, 2);
+}
+
+TEST_F(StructTest, OuterConstexpr) {
+  nt::StructTopic<Outer> topic = inst.GetStructTopic<Outer>("outer");
+  nt::StructPublisher<Outer> pub = topic.Publish();
+  nt::StructSubscriber<Outer> sub = topic.Subscribe({});
+
+  ASSERT_EQ(topic.GetTypeString(), "struct:Outer");
+
+  pub.SetDefault({{0, 1}, 2});
+  Outer val = sub.Get();
+  ASSERT_EQ(val.inner.a, 0);
+  ASSERT_EQ(val.inner.b, 1);
+  ASSERT_EQ(val.c, 2);
+
+  pub.Set({{1, 2}, 3});
+  auto atomicVal = sub.GetAtomic();
+  ASSERT_EQ(atomicVal.value.inner.a, 1);
+  ASSERT_EQ(atomicVal.value.inner.b, 2);
+  ASSERT_EQ(atomicVal.value.c, 3);
+
+  Outer val2;
+  sub.GetInto(&val2);
+  ASSERT_EQ(val2.inner.a, 1);
+  ASSERT_EQ(val2.inner.b, 2);
+  ASSERT_EQ(val2.c, 3);
+
+  auto vals = sub.ReadQueue();
+  ASSERT_EQ(vals.size(), 1u);
+  ASSERT_EQ(vals[0].value.inner.a, 1);
+  ASSERT_EQ(vals[0].value.inner.b, 2);
+  ASSERT_EQ(vals[0].value.c, 3);
+}
+
+TEST_F(StructTest, OuterNonconstexpr) {
+  nt::StructTopic<Outer2> topic = inst.GetStructTopic<Outer2>("outer2");
+  nt::StructPublisher<Outer2> pub = topic.Publish();
+  nt::StructSubscriber<Outer2> sub = topic.Subscribe({});
+
+  ASSERT_EQ(topic.GetTypeString(), "struct:Outer2");
+
+  pub.SetDefault({{0, 1}, 2});
+  Outer2 val = sub.Get();
+  ASSERT_EQ(val.inner.a, 0);
+  ASSERT_EQ(val.inner.b, 1);
+  ASSERT_EQ(val.c, 2);
+
+  pub.Set({{1, 2}, 3});
+  auto atomicVal = sub.GetAtomic();
+  ASSERT_EQ(atomicVal.value.inner.a, 1);
+  ASSERT_EQ(atomicVal.value.inner.b, 2);
+  ASSERT_EQ(atomicVal.value.c, 3);
+
+  Outer2 val2;
+  sub.GetInto(&val2);
+  ASSERT_EQ(val2.inner.a, 1);
+  ASSERT_EQ(val2.inner.b, 2);
+  ASSERT_EQ(val2.c, 3);
+
+  auto vals = sub.ReadQueue();
+  ASSERT_EQ(vals.size(), 1u);
+  ASSERT_EQ(vals[0].value.inner.a, 1);
+  ASSERT_EQ(vals[0].value.inner.b, 2);
+  ASSERT_EQ(vals[0].value.c, 3);
+}
+
+TEST_F(StructTest, InnerArrayConstexpr) {
+  nt::StructArrayTopic<Inner> topic = inst.GetStructArrayTopic<Inner>("innerA");
+  nt::StructArrayPublisher<Inner> pub = topic.Publish();
+  nt::StructArraySubscriber<Inner> sub = topic.Subscribe({});
+
+  ASSERT_EQ(topic.GetTypeString(), "struct:Inner[]");
+
+  pub.SetDefault({{{0, 1}}});
+  auto val = sub.Get();
+  ASSERT_EQ(val.size(), 1u);
+  ASSERT_EQ(val[0].a, 0);
+  ASSERT_EQ(val[0].b, 1);
+
+  pub.Set({{{1, 2}}});
+  auto atomicVal = sub.GetAtomic();
+  ASSERT_EQ(atomicVal.value.size(), 1u);
+  ASSERT_EQ(atomicVal.value[0].a, 1);
+  ASSERT_EQ(atomicVal.value[0].b, 2);
+
+  auto vals = sub.ReadQueue();
+  ASSERT_EQ(vals.size(), 1u);
+  ASSERT_EQ(vals[0].value.size(), 1u);
+  ASSERT_EQ(vals[0].value[0].a, 1);
+  ASSERT_EQ(vals[0].value[0].b, 2);
+}
+
+TEST_F(StructTest, InnerArrayNonconstexpr) {
+  nt::StructArrayTopic<Inner2> topic =
+      inst.GetStructArrayTopic<Inner2>("innerA2");
+  nt::StructArrayPublisher<Inner2> pub = topic.Publish();
+  nt::StructArraySubscriber<Inner2> sub = topic.Subscribe({});
+
+  ASSERT_EQ(topic.GetTypeString(), "struct:Inner2[]");
+
+  pub.SetDefault({{{0, 1}}});
+  auto val = sub.Get();
+  ASSERT_EQ(val.size(), 1u);
+  ASSERT_EQ(val[0].a, 0);
+  ASSERT_EQ(val[0].b, 1);
+
+  pub.Set({{{1, 2}}});
+  auto atomicVal = sub.GetAtomic();
+  ASSERT_EQ(atomicVal.value.size(), 1u);
+  ASSERT_EQ(atomicVal.value[0].a, 1);
+  ASSERT_EQ(atomicVal.value[0].b, 2);
+
+  auto vals = sub.ReadQueue();
+  ASSERT_EQ(vals.size(), 1u);
+  ASSERT_EQ(vals[0].value.size(), 1u);
+  ASSERT_EQ(vals[0].value[0].a, 1);
+  ASSERT_EQ(vals[0].value[0].b, 2);
+}
+
+TEST_F(StructTest, StructA) {
+  nt::StructTopic<ThingA> topic = inst.GetStructTopic<ThingA>("a");
+  nt::StructPublisher<ThingA> pub = topic.Publish();
+  nt::StructPublisher<ThingA> pub2 = topic.PublishEx({{}});
+  nt::StructSubscriber<ThingA> sub = topic.Subscribe({});
+  nt::StructEntry<ThingA> entry = topic.GetEntry({});
+  pub.SetDefault({});
+  pub.Set({}, 5);
+  sub.Get();
+  sub.Get({});
+  sub.GetAtomic();
+  sub.GetAtomic({});
+  entry.SetDefault({});
+  entry.Set({}, 6);
+  entry.Get({});
+}
+
+TEST_F(StructTest, StructArrayA) {
+  nt::StructArrayTopic<ThingA> topic = inst.GetStructArrayTopic<ThingA>("a");
+  nt::StructArrayPublisher<ThingA> pub = topic.Publish();
+  nt::StructArrayPublisher<ThingA> pub2 = topic.PublishEx({{}});
+  nt::StructArraySubscriber<ThingA> sub = topic.Subscribe({});
+  nt::StructArrayEntry<ThingA> entry = topic.GetEntry({});
+  pub.SetDefault({{ThingA{}, ThingA{}}});
+  pub.Set({{ThingA{}, ThingA{}}}, 5);
+  sub.Get();
+  sub.Get({});
+  sub.GetAtomic();
+  sub.GetAtomic({});
+  entry.SetDefault({{ThingA{}, ThingA{}}});
+  entry.Set({{ThingA{}, ThingA{}}}, 6);
+  entry.Get({});
+}
+
+TEST_F(StructTest, StructFixedArrayA) {
+  nt::StructTopic<std::array<ThingA, 2>> topic =
+      inst.GetStructTopic<std::array<ThingA, 2>>("a");
+  nt::StructPublisher<std::array<ThingA, 2>> pub = topic.Publish();
+  nt::StructPublisher<std::array<ThingA, 2>> pub2 = topic.PublishEx({{}});
+  nt::StructSubscriber<std::array<ThingA, 2>> sub = topic.Subscribe({});
+  nt::StructEntry<std::array<ThingA, 2>> entry = topic.GetEntry({});
+  std::array<ThingA, 2> arr;
+  pub.SetDefault(arr);
+  pub.Set(arr, 5);
+  sub.Get();
+  sub.Get(arr);
+  sub.GetAtomic();
+  sub.GetAtomic(arr);
+  entry.SetDefault(arr);
+  entry.Set(arr, 6);
+  entry.Get(arr);
+}
+
+TEST_F(StructTest, StructB) {
+  Info1 info;
+  nt::StructTopic<ThingB, Info1> topic =
+      inst.GetStructTopic<ThingB, Info1>("b", info);
+  nt::StructPublisher<ThingB, Info1> pub = topic.Publish();
+  nt::StructPublisher<ThingB, Info1> pub2 = topic.PublishEx({{}});
+  nt::StructSubscriber<ThingB, Info1> sub = topic.Subscribe({});
+  nt::StructEntry<ThingB, Info1> entry = topic.GetEntry({});
+  pub.SetDefault({});
+  pub.Set({}, 5);
+  sub.Get();
+  sub.Get({});
+  sub.GetAtomic();
+  sub.GetAtomic({});
+  entry.SetDefault({});
+  entry.Set({}, 6);
+  entry.Get({});
+}
+
+TEST_F(StructTest, StructArrayB) {
+  Info1 info;
+  nt::StructArrayTopic<ThingB, Info1> topic =
+      inst.GetStructArrayTopic<ThingB, Info1>("b", info);
+  nt::StructArrayPublisher<ThingB, Info1> pub = topic.Publish();
+  nt::StructArrayPublisher<ThingB, Info1> pub2 = topic.PublishEx({{}});
+  nt::StructArraySubscriber<ThingB, Info1> sub = topic.Subscribe({});
+  nt::StructArrayEntry<ThingB, Info1> entry = topic.GetEntry({});
+  pub.SetDefault({{ThingB{}, ThingB{}}});
+  pub.Set({{ThingB{}, ThingB{}}}, 5);
+  sub.Get();
+  sub.Get({});
+  sub.GetAtomic();
+  sub.GetAtomic({});
+  entry.SetDefault({{ThingB{}, ThingB{}}});
+  entry.Set({{ThingB{}, ThingB{}}}, 6);
+  entry.Get({});
+}
+
+TEST_F(StructTest, StructFixedArrayB) {
+  Info1 info;
+  nt::StructTopic<std::array<ThingB, 2>, Info1> topic =
+      inst.GetStructTopic<std::array<ThingB, 2>, Info1>("b", info);
+  nt::StructPublisher<std::array<ThingB, 2>, Info1> pub = topic.Publish();
+  nt::StructPublisher<std::array<ThingB, 2>, Info1> pub2 =
+      topic.PublishEx({{}});
+  nt::StructSubscriber<std::array<ThingB, 2>, Info1> sub = topic.Subscribe({});
+  nt::StructEntry<std::array<ThingB, 2>, Info1> entry = topic.GetEntry({});
+  std::array<ThingB, 2> arr;
+  pub.SetDefault(arr);
+  pub.Set(arr, 5);
+  sub.Get();
+  sub.Get(arr);
+  sub.GetAtomic();
+  sub.GetAtomic(arr);
+  entry.SetDefault(arr);
+  entry.Set(arr, 6);
+  entry.Get(arr);
+}
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp
index 0f060b0..20bd583 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp
@@ -10,7 +10,7 @@
 #include "ntcore.h"
 
 int main(int argc, char** argv) {
-  wpi::impl::SetupNowRio();
+  wpi::impl::SetupNowDefaultOnRio();
   nt::AddLogger(nt::GetDefaultInstance(), 0, UINT_MAX, [](auto& event) {
     if (auto msg = event.GetLogMessage()) {
       std::fputs(msg->message.c_str(), stderr);
diff --git a/third_party/allwpilib/ntcoreffi/src/main/native/cpp/DataLogManager.cpp b/third_party/allwpilib/ntcoreffi/src/main/native/cpp/DataLogManager.cpp
index e1e49b7..3a67c8d 100644
--- a/third_party/allwpilib/ntcoreffi/src/main/native/cpp/DataLogManager.cpp
+++ b/third_party/allwpilib/ntcoreffi/src/main/native/cpp/DataLogManager.cpp
@@ -217,20 +217,26 @@
   }
 #ifdef __FRC_ROBORIO__
   // prefer a mounted USB drive if one is accessible
-  constexpr std::string_view usbDir{"/u"};
   std::error_code ec;
-  auto s = fs::status(usbDir, ec);
+  auto s = fs::status("/u", ec);
   if (!ec && fs::is_directory(s) &&
       (s.permissions() & fs::perms::others_write) != fs::perms::none) {
-    return std::string{usbDir};
+    fs::create_directory("/u/logs", ec);
+    return "/u/logs";
   }
   if (RobotBase::GetRuntimeType() == kRoboRIO) {
     FRC_ReportError(warn::Warning,
                     "DataLogManager: Logging to RoboRIO 1 internal storage is "
                     "not recommended! Plug in a FAT32 formatted flash drive!");
   }
+  fs::create_directory("/home/lvuser/logs", ec);
+  return "/home/lvuser/logs";
+#else
+  std::string logDir = filesystem::GetOperatingDirectory() + "/logs";
+  std::error_code ec;
+  fs::create_directory(logDir, ec);
+  return logDir;
 #endif
-  return filesystem::GetOperatingDirectory();
 }
 
 static std::string MakeLogFilename(std::string_view filenameOverride) {
@@ -482,7 +488,7 @@
 void DataLogManager::Stop() {
   auto& inst = GetInstance();
   inst.owner.GetThread()->m_log.Stop();
-  inst.owner.Stop();
+  inst.owner.Join();
 }
 
 void DataLogManager::Log(std::string_view message) {
diff --git a/third_party/allwpilib/ntcoreffi/src/main/native/include/DataLogManager.h b/third_party/allwpilib/ntcoreffi/src/main/native/include/DataLogManager.h
index d2635c1..79d73ea 100644
--- a/third_party/allwpilib/ntcoreffi/src/main/native/include/DataLogManager.h
+++ b/third_party/allwpilib/ntcoreffi/src/main/native/include/DataLogManager.h
@@ -20,8 +20,8 @@
  * Centralized data log that provides automatic data log file management. It
  * automatically cleans up old files when disk space is low and renames the file
  * based either on current date/time or (if available) competition match number.
- * The deta file will be saved to a USB flash drive if one is attached, or to
- * /home/lvuser otherwise.
+ * The data file will be saved to a USB flash drive in a folder named "logs" if
+ * one is attached, or to /home/lvuser/logs otherwise.
  *
  * Log files are initially named "FRC_TBD_{random}.wpilog" until the DS
  * connects. After the DS connects, the log file is renamed to
diff --git a/third_party/allwpilib/ntcoreffi/src/main/native/symbols.txt b/third_party/allwpilib/ntcoreffi/src/main/native/symbols.txt
index c18014b..6700936 100644
--- a/third_party/allwpilib/ntcoreffi/src/main/native/symbols.txt
+++ b/third_party/allwpilib/ntcoreffi/src/main/native/symbols.txt
@@ -251,7 +251,8 @@
 WPI_DestroySemaphore
 WPI_DestroySignalObject
 WPI_GetSystemTime
-WPI_Impl_SetupNowRio
+WPI_Impl_SetupNowUseDefaultOnRio
+WPI_Impl_SetupNowRioWithSession
 WPI_Impl_ShutdownNowRio
 WPI_Now
 WPI_NowDefault
diff --git a/third_party/allwpilib/outlineviewer/CMakeLists.txt b/third_party/allwpilib/outlineviewer/CMakeLists.txt
index a92498e..eb22df8 100644
--- a/third_party/allwpilib/outlineviewer/CMakeLists.txt
+++ b/third_party/allwpilib/outlineviewer/CMakeLists.txt
@@ -9,7 +9,7 @@
 
 file(GLOB outlineviewer_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
 
-if (WIN32)
+if(WIN32)
     set(outlineviewer_rc src/main/native/win/outlineviewer.rc)
 elseif(APPLE)
     set(MACOSX_BUNDLE_ICON_FILE ov.icns)
@@ -17,12 +17,18 @@
     set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
 endif()
 
-add_executable(outlineviewer ${outlineviewer_src} ${outlineviewer_resources_src} ${outlineviewer_rc} ${APP_ICON_MACOSX})
+add_executable(
+    outlineviewer
+    ${outlineviewer_src}
+    ${outlineviewer_resources_src}
+    ${outlineviewer_rc}
+    ${APP_ICON_MACOSX}
+)
 wpilib_link_macos_gui(outlineviewer)
 wpilib_target_warnings(outlineviewer)
 target_link_libraries(outlineviewer libglassnt libglass)
 
-if (WIN32)
+if(WIN32)
     set_target_properties(outlineviewer PROPERTIES WIN32_EXECUTABLE YES)
 elseif(APPLE)
     set_target_properties(outlineviewer PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "OutlineViewer")
diff --git a/third_party/allwpilib/outlineviewer/src/main/generate/WPILibVersion.cpp.in b/third_party/allwpilib/outlineviewer/src/main/generate/WPILibVersion.cpp.in
index b0a4490..cfe2441 100644
--- a/third_party/allwpilib/outlineviewer/src/main/generate/WPILibVersion.cpp.in
+++ b/third_party/allwpilib/outlineviewer/src/main/generate/WPILibVersion.cpp.in
@@ -1,4 +1,4 @@
-/*
+/**
  * Autogenerated file! Do not manually edit this file. This version is regenerated
  * any time the publish task is run, or when this file is deleted.
  */
diff --git a/third_party/allwpilib/roborioteamnumbersetter/CMakeLists.txt b/third_party/allwpilib/roborioteamnumbersetter/CMakeLists.txt
index 4e2c5e0..7a43baa 100644
--- a/third_party/allwpilib/roborioteamnumbersetter/CMakeLists.txt
+++ b/third_party/allwpilib/roborioteamnumbersetter/CMakeLists.txt
@@ -9,7 +9,7 @@
 
 file(GLOB rtns_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
 
-if (WIN32)
+if(WIN32)
     set(rtns_rc src/main/native/win/roborioteamnumbersetter.rc)
 elseif(APPLE)
     set(MACOSX_BUNDLE_ICON_FILE rtns.icns)
@@ -17,13 +17,22 @@
     set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
 endif()
 
-add_executable(roborioteamnumbersetter ${rtns_src} ${rtns_resources_src} ${rtns_rc} ${APP_ICON_MACOSX})
+add_executable(
+    roborioteamnumbersetter
+    ${rtns_src}
+    ${rtns_resources_src}
+    ${rtns_rc}
+    ${APP_ICON_MACOSX}
+)
 wpilib_link_macos_gui(roborioteamnumbersetter)
 target_link_libraries(roborioteamnumbersetter libglass wpinet ${LIBSSH_LIBRARIES})
 target_include_directories(roborioteamnumbersetter SYSTEM PRIVATE ${LIBSSH_INCLUDE_DIRS})
 
-if (WIN32)
+if(WIN32)
     set_target_properties(roborioteamnumbersetter PROPERTIES WIN32_EXECUTABLE YES)
 elseif(APPLE)
-    set_target_properties(roborioteamnumbersetter PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "roborioTeamNumberSetter")
+    set_target_properties(
+        roborioteamnumbersetter
+        PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "roborioTeamNumberSetter"
+    )
 endif()
diff --git a/third_party/allwpilib/roborioteamnumbersetter/src/main/generate/WPILibVersion.cpp.in b/third_party/allwpilib/roborioteamnumbersetter/src/main/generate/WPILibVersion.cpp.in
index b0a4490..cfe2441 100644
--- a/third_party/allwpilib/roborioteamnumbersetter/src/main/generate/WPILibVersion.cpp.in
+++ b/third_party/allwpilib/roborioteamnumbersetter/src/main/generate/WPILibVersion.cpp.in
@@ -1,4 +1,4 @@
-/*
+/**
  * Autogenerated file! Do not manually edit this file. This version is regenerated
  * any time the publish task is run, or when this file is deleted.
  */
diff --git a/third_party/allwpilib/romiVendordep/CMakeLists.txt b/third_party/allwpilib/romiVendordep/CMakeLists.txt
index bd5f413..67cb540 100644
--- a/third_party/allwpilib/romiVendordep/CMakeLists.txt
+++ b/third_party/allwpilib/romiVendordep/CMakeLists.txt
@@ -4,38 +4,50 @@
 include(CompileWarnings)
 include(AddTest)
 
-if (WITH_JAVA)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-  add_jar(romiVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME romiVendordep)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+    add_jar(
+        romiVendordep_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS
+            hal_jar
+            ntcore_jar
+            cscore_jar
+            cameraserver_jar
+            wpimath_jar
+            wpiunits_jar
+            wpiutil_jar
+            wpilibj_jar
+        OUTPUT_NAME romiVendordep
+    )
 
-  get_property(ROMIVENDORDEP_JAR_FILE TARGET romiVendordep_jar PROPERTY JAR_FILE)
-  install(FILES ${ROMIVENDORDEP_JAR_FILE} DESTINATION "${java_lib_dest}")
-
-  set_property(TARGET romiVendordep_jar PROPERTY FOLDER "java")
-
-  if (WITH_FLAT_INSTALL)
-      set (romiVendordep_config_dir ${wpilib_dest})
-  else()
-      set (romiVendordep_config_dir share/romiVendordep)
-  endif()
+    install_jar(romiVendordep_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(
+        TARGETS romiVendordep_jar
+        FILE romiVendordep_jar.cmake
+        DESTINATION share/romiVendordep
+    )
 endif()
 
-if (WITH_JAVA_SOURCE)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  file(GLOB_RECURSE ROMIVENDORDEP_SOURCES src/main/java/*.java)
-  add_jar(romiVendordep_src_jar
-  RESOURCES NAMESPACE "edu/wpi/first/wpilibj/romi" ${ROMIVENDORDEP_SOURCES}
-  OUTPUT_NAME romiVendordep-sources)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB_RECURSE ROMIVENDORDEP_SOURCES src/main/java/*.java)
+    add_jar(
+        romiVendordep_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/wpilibj/romi" ${ROMIVENDORDEP_SOURCES}
+        OUTPUT_NAME romiVendordep-sources
+    )
 
-  get_property(ROMIVENDORDEP_SRC_JAR_FILE TARGET romiVendordep_src_jar PROPERTY JAR_FILE)
-  install(FILES ${ROMIVENDORDEP_JAR_FILE} DESTINATION "${java_lib_dest}")
+    get_property(ROMIVENDORDEP_SRC_JAR_FILE TARGET romiVendordep_src_jar PROPERTY JAR_FILE)
+    install(FILES ${ROMIVENDORDEP_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-  set_property(TARGET romiVendordep_src_jar PROPERTY FOLDER "java")
+    set_property(TARGET romiVendordep_src_jar PROPERTY FOLDER "java")
 endif()
 
 file(GLOB_RECURSE romiVendordep_native_src src/main/native/cpp/*.cpp)
@@ -47,25 +59,22 @@
 wpilib_target_warnings(romiVendordep)
 target_link_libraries(romiVendordep wpilibc)
 
-target_include_directories(romiVendordep PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/romiVendordep>)
+target_include_directories(
+    romiVendordep
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/romiVendordep>
+)
 
-install(TARGETS romiVendordep EXPORT romiVendordep DESTINATION "${main_lib_dest}")
+install(TARGETS romiVendordep EXPORT romivendordep DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/romiVendordep")
 
-if (FLAT_INSTALL_WPILIB)
-     set(romiVendordep_config_dir ${wpilib_dest})
- else()
-     set(romiVendordep_config_dir share/romiVendordep)
- endif()
+configure_file(romivendordep-config.cmake.in ${WPILIB_BINARY_DIR}/romivendordep-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/romivendordep-config.cmake DESTINATION share/romiVendordep)
+install(EXPORT romivendordep DESTINATION share/romiVendordep)
 
- configure_file(romiVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake)
- install(FILES ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake DESTINATION ${romiVendordep_config_dir})
- install(EXPORT romiVendordep DESTINATION ${romiVendordep_config_dir})
-
- if (WITH_TESTS)
-     wpilib_add_test(romiVendordep src/test/native/cpp)
-     target_include_directories(romiVendordep_test PRIVATE src/test/native/include)
-     target_link_libraries(romiVendordep_test romiVendordep gmock_main)
- endif()
+if(WITH_TESTS)
+    wpilib_add_test(romiVendordep src/test/native/cpp)
+    target_include_directories(romiVendordep_test PRIVATE src/test/native/include)
+    target_link_libraries(romiVendordep_test romiVendordep gmock_main)
+endif()
diff --git a/third_party/allwpilib/romiVendordep/romiVendordep-config.cmake.in b/third_party/allwpilib/romiVendordep/romiVendordep-config.cmake.in
deleted file mode 100644
index 3711d9c..0000000
--- a/third_party/allwpilib/romiVendordep/romiVendordep-config.cmake.in
+++ /dev/null
@@ -1,11 +0,0 @@
-include(CMakeFindDependencyMacro)
- @WPIUTIL_DEP_REPLACE@
- @NTCORE_DEP_REPLACE@
- @CSCORE_DEP_REPLACE@
- @CAMERASERVER_DEP_REPLACE@
- @HAL_DEP_REPLACE@
- @WPILIBC_DEP_REPLACE@
- @WPIMATH_DEP_REPLACE@
-
- @FILENAME_DEP_REPLACE@
- include(${SELF_DIR}/romiVendordep.cmake)
diff --git a/third_party/allwpilib/romiVendordep/romivendordep-config.cmake.in b/third_party/allwpilib/romiVendordep/romivendordep-config.cmake.in
new file mode 100644
index 0000000..d4a80f0
--- /dev/null
+++ b/third_party/allwpilib/romiVendordep/romivendordep-config.cmake.in
@@ -0,0 +1,14 @@
+include(CMakeFindDependencyMacro)
+@WPIUTIL_DEP_REPLACE@
+@NTCORE_DEP_REPLACE@
+@CSCORE_DEP_REPLACE@
+@CAMERASERVER_DEP_REPLACE@
+@HAL_DEP_REPLACE@
+@WPILIBC_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+
+@FILENAME_DEP_REPLACE@
+include(${SELF_DIR}/romivendordep.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/romiVendordep_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java b/third_party/allwpilib/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java
index 93024f6..1a56bf7 100644
--- a/third_party/allwpilib/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java
+++ b/third_party/allwpilib/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java
@@ -14,7 +14,7 @@
  */
 public class RomiMotor extends PWMMotorController {
   /** Common initialization code called by all constructors. */
-  protected void initRomiMotor() {
+  protected final void initRomiMotor() {
     m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
     m_pwm.setSpeed(0.0);
     m_pwm.setZeroLatch();
diff --git a/third_party/allwpilib/shared/config.gradle b/third_party/allwpilib/shared/config.gradle
index 36aee7f..5acadb0 100644
--- a/third_party/allwpilib/shared/config.gradle
+++ b/third_party/allwpilib/shared/config.gradle
@@ -15,7 +15,7 @@
         configureDependencies {
             opencvYear = "frc2024"
             googleTestYear = "frc2024"
-            niLibVersion = "2024.1.1"
+            niLibVersion = "2024.2.1"
             opencvVersion = "4.8.0-2"
             googleTestVersion = "1.14.0-1"
         }
diff --git a/third_party/allwpilib/shared/java/javacommon.gradle b/third_party/allwpilib/shared/java/javacommon.gradle
index 32078f8..48a5062 100644
--- a/third_party/allwpilib/shared/java/javacommon.gradle
+++ b/third_party/allwpilib/shared/java/javacommon.gradle
@@ -117,9 +117,8 @@
 }
 
 dependencies {
-    testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0'
-    testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0'
-    testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0'
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 
     devImplementation sourceSets.main.output
 }
@@ -147,22 +146,12 @@
     protoc {
         artifact = 'com.google.protobuf:protoc:3.21.12'
     }
-    plugins {
-        quickbuf {
-            artifact = 'us.hebi.quickbuf:protoc-gen-quickbuf:1.3.2'
-        }
-    }
     generateProtoTasks {
         all().configureEach { task ->
             task.builtins {
                 cpp {}
                 remove java
             }
-            task.plugins {
-                quickbuf {
-                    option "gen_descriptors=true"
-                }
-            }
         }
     }
 }
diff --git a/third_party/allwpilib/shared/java/javastyle.gradle b/third_party/allwpilib/shared/java/javastyle.gradle
index 49332bf..41ddb47 100644
--- a/third_party/allwpilib/shared/java/javastyle.gradle
+++ b/third_party/allwpilib/shared/java/javastyle.gradle
@@ -1,83 +1,85 @@
-if (!project.hasProperty('skipJavaFormat')) {
-    apply plugin: 'checkstyle'
+if (project.hasProperty('skipJavaFormat')) {
+    return;
+}
 
-    checkstyle {
-        toolVersion = "10.12.2"
-        configDirectory = file("${project.rootDir}/styleguide")
-        config = resources.text.fromFile(new File(configDirectory.get().getAsFile(), "checkstyle.xml"))
+apply plugin: 'checkstyle'
+
+checkstyle {
+    toolVersion = "10.12.2"
+    configDirectory = file("${project.rootDir}/styleguide")
+    config = resources.text.fromFile(new File(configDirectory.get().getAsFile(), "checkstyle.xml"))
+}
+
+apply plugin: 'pmd'
+
+pmd {
+    toolVersion = '6.55.0'
+    consoleOutput = true
+    reportsDir = file("$project.buildDir/reports/pmd")
+    ruleSetFiles = files(new File(rootDir, "styleguide/pmd-ruleset.xml"))
+    ruleSets = []
+}
+
+apply plugin: 'com.diffplug.spotless'
+
+spotless {
+    java {
+        target fileTree('.') {
+            include '**/*.java'
+            exclude '**/build/**', '**/build-*/**', '**/bin/**', "src/generated/**"
+        }
+        toggleOffOn()
+        googleJavaFormat()
+        removeUnusedImports()
+        trimTrailingWhitespace()
+        endWithNewline()
     }
-
-    apply plugin: 'pmd'
-
-    pmd {
-        toolVersion = '6.55.0'
-        consoleOutput = true
-        reportsDir = file("$project.buildDir/reports/pmd")
-        ruleSetFiles = files(new File(rootDir, "styleguide/pmd-ruleset.xml"))
-        ruleSets = []
+    groovyGradle {
+        target fileTree('.') {
+            include '**/*.gradle'
+            exclude '**/build/**', '**/build-*/**', '**/bin/**'
+        }
+        greclipse()
+        indentWithSpaces(4)
+        trimTrailingWhitespace()
+        endWithNewline()
     }
-
-    apply plugin: 'com.diffplug.spotless'
-
-    spotless {
-        java {
-            target fileTree('.') {
-                include '**/*.java'
-                exclude '**/build/**', '**/build-*/**', '**/bin/**'
-            }
-            toggleOffOn()
-            googleJavaFormat()
-            removeUnusedImports()
-            trimTrailingWhitespace()
-            endWithNewline()
+    json {
+        target fileTree('.') {
+            include '**/*.json'
+            exclude '**/build/**', '**/build-*/**', '**/bin/**'
+            exclude '**/simgui-ds.json', '**/simgui-window.json', '**/simgui.json', '**/networktables.json'
         }
-        groovyGradle {
-            target fileTree('.') {
-                include '**/*.gradle'
-                exclude '**/build/**', '**/build-*/**', '**/bin/**'
-            }
-            greclipse()
-            indentWithSpaces(4)
-            trimTrailingWhitespace()
-            endWithNewline()
-        }
-        json {
-            target fileTree('.') {
-                include '**/*.json'
-                exclude '**/build/**', '**/build-*/**', '**/bin/**'
-                exclude '**/simgui-ds.json', '**/simgui-window.json', '**/simgui.json', '**/networktables.json'
-            }
-            gson()
-              .indentWithSpaces(2)
-        }
-        format 'xml', {
-            target fileTree('.') {
-                include '**/*.xml'
-                exclude '**/build/**', '**/build-*/**', '**/bin/**', '**/.idea/**', '**/.run/**'
-            }
-            eclipseWtp('xml')
-            trimTrailingWhitespace()
-            indentWithSpaces(2)
-            endWithNewline()
-        }
-        format 'misc', {
-            target fileTree('.') {
-                include '**/*.md', '**/.gitignore'
-                exclude '**/build/**', '**/build-*/**', '**/bin/**'
-            }
-            trimTrailingWhitespace()
-            indentWithSpaces(2)
-            endWithNewline()
-        }
+        gson()
+          .indentWithSpaces(2)
     }
-
-    apply plugin: 'com.github.spotbugs'
-
-    spotbugs {
-        ignoreFailures = false
-        effort = 'max'
-        excludeFilter = file("${project.rootDir}/styleguide/spotbugs-exclude.xml")
+    format 'xml', {
+        target fileTree('.') {
+            include '**/*.xml'
+            exclude '**/build/**', '**/build-*/**', '**/bin/**', '**/.idea/**', '**/.run/**'
+        }
+        eclipseWtp('xml')
+        trimTrailingWhitespace()
+        indentWithSpaces(2)
+        endWithNewline()
     }
+    format 'misc', {
+        target fileTree('.') {
+            include '**/*.md', '**/.gitignore'
+            exclude '**/build/**', '**/build-*/**', '**/bin/**'
+        }
+        trimTrailingWhitespace()
+        indentWithSpaces(2)
+        endWithNewline()
+    }
+}
+
+apply plugin: 'com.github.spotbugs'
+
+spotbugs {
+    ignoreFailures = false
+    effort = spotbugsEffort
+    excludeFilter = file("${project.rootDir}/styleguide/spotbugs-exclude.xml")
 }
 
 task javaFormat {
diff --git a/third_party/allwpilib/shared/jni/setupBuild.gradle b/third_party/allwpilib/shared/jni/setupBuild.gradle
index 9f9334f..de7c50f 100644
--- a/third_party/allwpilib/shared/jni/setupBuild.gradle
+++ b/third_party/allwpilib/shared/jni/setupBuild.gradle
@@ -223,6 +223,9 @@
                         nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                     }
                 }
+                it.tasks.withType(CppCompile) {
+                    it.dependsOn generateProto
+                }
                 if (project.hasProperty('exeSplitSetup')) {
                     exeSplitSetup(it)
                 }
@@ -257,7 +260,7 @@
                         include '**/*.cpp'
                     }
                     exportedHeaders {
-                        srcDirs 'src/test/native/include', 'src/main/native/cpp'
+                        srcDirs 'src/test/native/include', 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp"
                         if (project.hasProperty('generatedHeaders')) {
                             srcDir generatedHeaders
                         }
@@ -276,6 +279,9 @@
                     nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                 }
             }
+            it.tasks.withType(CppCompile) {
+                it.dependsOn generateProto
+            }
             if (project.hasProperty('exeSplitSetup')) {
                 exeSplitSetup(it)
             }
diff --git a/third_party/allwpilib/simulation/CMakeLists.txt b/third_party/allwpilib/simulation/CMakeLists.txt
index 0a1a953..e6bc35c 100644
--- a/third_party/allwpilib/simulation/CMakeLists.txt
+++ b/third_party/allwpilib/simulation/CMakeLists.txt
@@ -1,4 +1,4 @@
-if (WITH_GUI)
+if(WITH_GUI)
     add_subdirectory(halsim_gui)
 endif()
 add_subdirectory(halsim_ds_socket)
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
index 4edaa5c..09192f3 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
@@ -609,6 +609,7 @@
 void KeyboardJoystick::SettingsDisplay() {
   if (s_keyEdit) {
     ImGuiIO& io = ImGui::GetIO();
+    // NOLINTNEXTLINE(bugprone-sizeof-expression)
     for (int i = 0; i < IM_ARRAYSIZE(io.KeysDown); ++i) {
       if (io.KeysDown[i]) {
         // remove all other uses
diff --git a/third_party/allwpilib/styleguide/checkstyle-suppressions.xml b/third_party/allwpilib/styleguide/checkstyle-suppressions.xml
index 2197fe2..f42b176 100644
--- a/third_party/allwpilib/styleguide/checkstyle-suppressions.xml
+++ b/third_party/allwpilib/styleguide/checkstyle-suppressions.xml
@@ -10,6 +10,6 @@
     checks="(LocalVariableName|MemberName|MethodName|MethodTypeParameterName|ParameterName)" />
   <suppress files=".*JNI.*"
     checks="(EmptyLineSeparator|LineLength|MissingJavadocMethod|ParameterName)" />
-  <suppress files=".*quickbuf.*"
+  <suppress files=".*math[/\\]proto.*"
     checks="(CustomImportOrder|EmptyLineSeparator|LineLength|JavadocParagraph|MissingJavadocMethod|OverloadMethodsDeclarationOrder|SummaryJavadoc|UnnecessaryParentheses|OperatorWrap|JavadocMethod|JavadocTagContinuationIndentation)" />
 </suppressions>
diff --git a/third_party/allwpilib/styleguide/pmd-ruleset.xml b/third_party/allwpilib/styleguide/pmd-ruleset.xml
index f2ef2bc..1f6577e 100644
--- a/third_party/allwpilib/styleguide/pmd-ruleset.xml
+++ b/third_party/allwpilib/styleguide/pmd-ruleset.xml
@@ -8,7 +8,7 @@
 
   <exclude-pattern>.*/*JNI.*</exclude-pattern>
   <exclude-pattern>.*/*IntegrationTests.*</exclude-pattern>
-  <exclude-pattern>.*/quickbuf/.*</exclude-pattern>
+  <exclude-pattern>.*/math/proto.*</exclude-pattern>
 
   <rule ref="category/java/bestpractices.xml">
     <exclude name="AccessorClassGeneration" />
diff --git a/third_party/allwpilib/styleguide/spotbugs-exclude.xml b/third_party/allwpilib/styleguide/spotbugs-exclude.xml
index f224099..ba4feb2 100644
--- a/third_party/allwpilib/styleguide/spotbugs-exclude.xml
+++ b/third_party/allwpilib/styleguide/spotbugs-exclude.xml
@@ -1,6 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <FindBugsFilter>
   <Match>
+    <Bug pattern="CT_CONSTRUCTOR_THROW" />
+  </Match>
+  <Match>
     <Bug pattern="DCN_NULLPOINTER_EXCEPTION" />
     <Class name="edu.wpi.first.wpilibj.test.TestSuite" />
   </Match>
@@ -73,6 +76,9 @@
     <Source name="SelectCommand.java" />
   </Match>
   <Match>
+    <Bug pattern="PA_PUBLIC_PRIMITIVE_ATTRIBUTE" />
+  </Match>
+  <Match>
     <Bug pattern="RV_RETURN_VALUE_IGNORED_BAD_PRACTICE" />
     <Source name="AntJunitLauncher.java" />
   </Match>
@@ -133,6 +139,10 @@
     <Class name="edu.wpi.first.wpilibj.ADIS16470_IMU" />
   </Match>
   <Match>
+    <Bug pattern="URF_UNREAD_FIELD" />
+    <Class name="edu.wpi.first.wpilibj.AnalogTrigger" />
+  </Match>
+  <Match>
     <Bug pattern="URF_UNREAD_PUBLIC_OR_PROTECTED_FIELD" />
   </Match>
   <Match>
diff --git a/third_party/allwpilib/sysid/CMakeLists.txt b/third_party/allwpilib/sysid/CMakeLists.txt
index 31fbe08..e8555e8 100644
--- a/third_party/allwpilib/sysid/CMakeLists.txt
+++ b/third_party/allwpilib/sysid/CMakeLists.txt
@@ -10,7 +10,7 @@
 
 file(GLOB_RECURSE sysid_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
 
-if (WIN32)
+if(WIN32)
     set(sysid_rc src/main/native/win/sysid.rc)
 elseif(APPLE)
     set(MACOSX_BUNDLE_ICON_FILE sysid.icns)
@@ -19,23 +19,28 @@
 endif()
 
 add_executable(sysid ${sysid_src} ${sysid_resources_src} ${sysid_rc} ${APP_ICON_MACOSX})
+if(MSVC)
+    target_compile_options(sysid PRIVATE /utf-8)
+endif()
 wpilib_link_macos_gui(sysid)
 wpilib_target_warnings(sysid)
 target_include_directories(sysid PRIVATE src/main/native/include)
 target_link_libraries(sysid wpimath libglassnt libglass)
 
-if (WIN32)
+if(WIN32)
     set_target_properties(sysid PROPERTIES WIN32_EXECUTABLE YES)
 elseif(APPLE)
     set_target_properties(sysid PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "SysId")
 endif()
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(sysid src/test/native/cpp)
     wpilib_link_macos_gui(sysid_test)
     target_sources(sysid_test PRIVATE ${sysid_src})
     target_compile_definitions(sysid_test PRIVATE RUNNING_SYSID_TESTS)
-    target_include_directories(sysid_test PRIVATE src/main/native/cpp
-                                                  src/main/native/include)
+    if(MSVC)
+        target_compile_options(sysid_test PRIVATE /utf-8)
+    endif()
+    target_include_directories(sysid_test PRIVATE src/main/native/cpp src/main/native/include)
     target_link_libraries(sysid_test wpimath libglassnt libglass gtest)
 endif()
diff --git a/third_party/allwpilib/sysid/build.gradle b/third_party/allwpilib/sysid/build.gradle
index e6709fc..b9689e9 100644
--- a/third_party/allwpilib/sysid/build.gradle
+++ b/third_party/allwpilib/sysid/build.gradle
@@ -102,8 +102,8 @@
                 lib project: ':glass', library: 'glass', linkage: 'static'
                 project(':ntcore').addNtcoreDependency(it, 'static')
                 lib project: ':wpinet', library: 'wpinet', linkage: 'static'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                 lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                 lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                 nativeUtils.useRequiredLibrary(it, 'imgui')
                 if (it.targetPlatform.operatingSystem.isWindows()) {
@@ -144,8 +144,8 @@
                 lib project: ':glass', library: 'glass', linkage: 'static'
                 project(':ntcore').addNtcoreDependency(it, 'static')
                 lib project: ':wpinet', library: 'wpinet', linkage: 'static'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                 lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                 lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                 nativeUtils.useRequiredLibrary(it, 'imgui')
                 if (it.targetPlatform.operatingSystem.isWindows()) {
diff --git a/third_party/allwpilib/sysid/docs/arm-ols-with-angle-offset.md b/third_party/allwpilib/sysid/docs/arm-ols-with-angle-offset.md
deleted file mode 100644
index ecb5c43..0000000
--- a/third_party/allwpilib/sysid/docs/arm-ols-with-angle-offset.md
+++ /dev/null
@@ -1,66 +0,0 @@
-# Arm OLS with angle offset
-
-If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit
-for `Kg` will be wrong. An angle offset should be added to the model like so.
-```
-dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset)
-```
-Use a trig identity to split the cosine into two terms.
-```
-dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset))
-dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset)
-```
-Reorder multiplicands so the offset trig is absorbed by the OLS terms.
-```
-dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle)
-```
-
-## OLS
-
-Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`.
-```
-dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
-```
-
-### Ks, Kv, Ka
-
-Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
-```
-Ks = -γ/β
-Kv = -α/β
-Ka = 1/β
-```
-
-### Kg
-
-Take the sum of squares of the OLS terms containing the angle offset. The angle
-offset trig functions will form a trig identity that cancels out. Then, just
-solve for `Kg`.
-```
-δ²+ε² = (-Kg/Ka cos(offset))² + (Kg/Ka sin(offset))²
-δ²+ε² = (-Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
-δ²+ε² = (Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
-δ²+ε² = (Kg/Ka)² (cos²(offset) + sin²(offset))
-δ²+ε² = (Kg/Ka)² (1)
-δ²+ε² = (Kg/Ka)²
-√(δ²+ε²) = Kg/Ka
-√(δ²+ε²) = Kg β
-Kg = √(δ²+ε²)/β
-```
-
-As a sanity check, when the offset is zero, ε is zero and the equation for
-`Kg` simplifies to -δ/β, the equation previously used by SysId.
-
-### Angle offset
-
-Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()`
-to preserve the angle quadrant. Maintaining the proper negative signs in the
-numerator and denominator are important for obtaining the correct result.
-```
-δ = -Kg/Ka cos(offset)
-ε = Kg/Ka sin(offset)
-sin(offset)/-cos(offset) = ε/δ
-sin(offset)/cos(offset) = ε/-δ
-tan(offset) = ε/-δ
-offset = atan2(ε, -δ)
-```
diff --git a/third_party/allwpilib/sysid/docs/data-collection.md b/third_party/allwpilib/sysid/docs/data-collection.md
index 9d28a15..538aff4 100644
--- a/third_party/allwpilib/sysid/docs/data-collection.md
+++ b/third_party/allwpilib/sysid/docs/data-collection.md
@@ -15,7 +15,7 @@
 
 ## Telemetry Format
 
-There are two formats used to send telemetry from the robot program. One format is for non-drivetrain mechanisms, whereas the other is for all drivetrain tests (linear and angular).
+There are two formats used to send telemetry from the robot program. One format is for non-drivetrain mechanisms, whereas the other is for all drivetrain tests (linear and angular). All timestamps must be in seconds.
 
 ### Non-Drivetrain Mechanisms
 
diff --git a/third_party/allwpilib/sysid/docs/ols-derivations.md b/third_party/allwpilib/sysid/docs/ols-derivations.md
new file mode 100644
index 0000000..53a8eb2
--- /dev/null
+++ b/third_party/allwpilib/sysid/docs/ols-derivations.md
@@ -0,0 +1,120 @@
+# OLS derivations
+
+## Simple/drivetrain
+
+Here's the ODE for a drivetrain.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+```
+
+### OLS setup
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, and `γ = -Ks/Ka`.
+```
+dx/dt = αx + βu + γ sgn(x)
+```
+
+### Feedforward gains
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+```
+
+## Elevator
+
+Here's the ODE for an elevator.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka
+```
+
+### OLS setup
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, and `δ = -Kg/Ka`.
+```
+dx/dt = αx + βu + γ sgn(x) + δ
+```
+
+### Feedforward gains
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, `Ka`, and `Kg`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+Kg = −δ/β
+```
+
+## Arm
+
+Here's the ODE for an arm:
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle)
+```
+
+If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit
+for `Kg` will be wrong. An angle offset should be added to the model like so.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset)
+```
+
+Use a trig identity to split the cosine into two terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset))
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset)
+```
+
+Reorder multiplicands so the offset trig is absorbed by the OLS terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle)
+```
+
+### OLS setup
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`.
+```
+dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
+```
+
+### Feedforward gains: Ks, Kv, Ka
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+```
+
+### Feedforward gains: Kg
+
+Take the sum of squares of the OLS terms containing the angle offset. The angle
+offset trig functions will form a trig identity that cancels out. Then, just
+solve for `Kg`.
+```
+δ²+ε² = (-Kg/Ka cos(offset))² + (Kg/Ka sin(offset))²
+δ²+ε² = (-Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² (cos²(offset) + sin²(offset))
+δ²+ε² = (Kg/Ka)² (1)
+δ²+ε² = (Kg/Ka)²
+√(δ²+ε²) = Kg/Ka
+hypot(δ, ε) = Kg/Ka
+hypot(δ, ε) = Kg β
+Kg = hypot(δ, ε)/β
+```
+
+### Feedforward gains: offset
+
+Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()`
+to preserve the angle quadrant. Maintaining the proper negative signs in the
+numerator and denominator are important for obtaining the correct result.
+```
+δ = -Kg/Ka cos(offset)
+ε = Kg/Ka sin(offset)
+sin(offset)/-cos(offset) = ε/δ
+sin(offset)/cos(offset) = ε/-δ
+tan(offset) = ε/-δ
+offset = atan2(ε, -δ)
+```
diff --git a/third_party/allwpilib/sysid/src/main/generate/WPILibVersion.cpp.in b/third_party/allwpilib/sysid/src/main/generate/WPILibVersion.cpp.in
index b0a4490..cfe2441 100644
--- a/third_party/allwpilib/sysid/src/main/generate/WPILibVersion.cpp.in
+++ b/third_party/allwpilib/sysid/src/main/generate/WPILibVersion.cpp.in
@@ -1,4 +1,4 @@
-/*
+/**
  * Autogenerated file! Do not manually edit this file. This version is regenerated
  * any time the publish task is run, or when this file is deleted.
  */
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/App.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/App.cpp
index 947ea43..4a60f79 100644
--- a/third_party/allwpilib/sysid/src/main/native/cpp/App.cpp
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/App.cpp
@@ -24,21 +24,20 @@
 #include <wpigui_openurl.h>
 
 #include "sysid/view/Analyzer.h"
-#include "sysid/view/JSONConverter.h"
-#include "sysid/view/Logger.h"
+#include "sysid/view/DataSelector.h"
+#include "sysid/view/LogLoader.h"
 #include "sysid/view/UILayout.h"
 
 namespace gui = wpi::gui;
 
 static std::unique_ptr<glass::WindowManager> gWindowManager;
 
-glass::Window* gLoggerWindow;
+glass::Window* gLogLoaderWindow;
+glass::Window* gDataSelectorWindow;
 glass::Window* gAnalyzerWindow;
 glass::Window* gProgramLogWindow;
 static glass::MainMenuBar gMainMenu;
 
-std::unique_ptr<sysid::JSONConverter> gJSONConverter;
-
 glass::LogData gLog;
 wpi::Logger gLogger;
 
@@ -103,11 +102,23 @@
   gWindowManager = std::make_unique<glass::WindowManager>(storage);
   gWindowManager->GlobalInit();
 
-  gLoggerWindow = gWindowManager->AddWindow(
-      "Logger", std::make_unique<sysid::Logger>(storage, gLogger));
+  auto logLoader = std::make_unique<sysid::LogLoader>(storage, gLogger);
+  auto dataSelector = std::make_unique<sysid::DataSelector>(storage, gLogger);
+  auto analyzer = std::make_unique<sysid::Analyzer>(storage, gLogger);
 
-  gAnalyzerWindow = gWindowManager->AddWindow(
-      "Analyzer", std::make_unique<sysid::Analyzer>(storage, gLogger));
+  logLoader->unload.connect([ds = dataSelector.get()] { ds->Reset(); });
+  dataSelector->testdata = [_analyzer = analyzer.get()](auto data) {
+    _analyzer->m_data = data;
+    _analyzer->AnalyzeData();
+  };
+
+  gLogLoaderWindow =
+      gWindowManager->AddWindow("Log Loader", std::move(logLoader));
+
+  gDataSelectorWindow =
+      gWindowManager->AddWindow("Data Selector", std::move(dataSelector));
+
+  gAnalyzerWindow = gWindowManager->AddWindow("Analyzer", std::move(analyzer));
 
   gProgramLogWindow = gWindowManager->AddWindow(
       "Program Log", std::make_unique<glass::LogView>(&gLog));
@@ -115,10 +126,16 @@
   // Set default positions and sizes for windows.
 
   // Logger window position/size
-  gLoggerWindow->SetDefaultPos(sysid::kLoggerWindowPos.x,
-                               sysid::kLoggerWindowPos.y);
-  gLoggerWindow->SetDefaultSize(sysid::kLoggerWindowSize.x,
-                                sysid::kLoggerWindowSize.y);
+  gLogLoaderWindow->SetDefaultPos(sysid::kLogLoaderWindowPos.x,
+                                  sysid::kLogLoaderWindowPos.y);
+  gLogLoaderWindow->SetDefaultSize(sysid::kLogLoaderWindowSize.x,
+                                   sysid::kLogLoaderWindowSize.y);
+
+  // Data selector window position/size
+  gDataSelectorWindow->SetDefaultPos(sysid::kDataSelectorWindowPos.x,
+                                     sysid::kDataSelectorWindowPos.y);
+  gDataSelectorWindow->SetDefaultSize(sysid::kDataSelectorWindowSize.x,
+                                      sysid::kDataSelectorWindowSize.y);
 
   // Analyzer window position/size
   gAnalyzerWindow->SetDefaultPos(sysid::kAnalyzerWindowPos.x,
@@ -133,8 +150,6 @@
                                     sysid::kProgramLogWindowSize.y);
   gProgramLogWindow->DisableRenamePopup();
 
-  gJSONConverter = std::make_unique<sysid::JSONConverter>(gLogger);
-
   // Configure save file.
   gui::ConfigurePlatformSaveFile("sysid.ini");
 
@@ -157,15 +172,6 @@
       ImGui::EndMenu();
     }
 
-    bool toCSV = false;
-    if (ImGui::BeginMenu("JSON Converters")) {
-      if (ImGui::MenuItem("JSON to CSV Converter")) {
-        toCSV = true;
-      }
-
-      ImGui::EndMenu();
-    }
-
     if (ImGui::BeginMenu("Docs")) {
       if (ImGui::MenuItem("Online documentation")) {
         wpi::gui::OpenURL(
@@ -178,19 +184,6 @@
 
     ImGui::EndMainMenuBar();
 
-    if (toCSV) {
-      ImGui::OpenPopup("SysId JSON to CSV Converter");
-      toCSV = false;
-    }
-
-    if (ImGui::BeginPopupModal("SysId JSON to CSV Converter")) {
-      gJSONConverter->DisplayCSVConvert();
-      if (ImGui::Button("Close")) {
-        ImGui::CloseCurrentPopup();
-      }
-      ImGui::EndPopup();
-    }
-
     if (about) {
       ImGui::OpenPopup("About");
       about = false;
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
index b15cae8..be88b57 100644
--- a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
@@ -5,24 +5,39 @@
 #include "sysid/analysis/AnalysisManager.h"
 
 #include <cmath>
-#include <cstddef>
 #include <functional>
-#include <stdexcept>
 
 #include <fmt/format.h>
 #include <units/angle.h>
-#include <units/math.h>
+#include <wpi/MathExtras.h>
 #include <wpi/MemoryBuffer.h>
 #include <wpi/StringExtras.h>
 #include <wpi/StringMap.h>
 
-#include "sysid/Util.h"
 #include "sysid/analysis/FilteringUtils.h"
-#include "sysid/analysis/JSONConverter.h"
-#include "sysid/analysis/TrackWidthAnalysis.h"
 
 using namespace sysid;
 
+static double Lerp(units::second_t time,
+                   std::vector<MotorData::Run::Sample<double>>& data) {
+  auto next = std::find_if(data.begin(), data.end(), [&](const auto& entry) {
+    return entry.time > time;
+  });
+
+  if (next == data.begin()) {
+    next++;
+  }
+
+  if (next == data.end()) {
+    next--;
+  }
+
+  const auto prev = next - 1;
+
+  return wpi::Lerp(prev->measurement, next->measurement,
+                   (time - prev->time) / (next->time - prev->time));
+}
+
 /**
  * Converts a raw data vector into a PreparedData vector with only the
  * timestamp, voltage, position, and velocity fields filled out.
@@ -38,18 +53,25 @@
  *
  * @return A PreparedData vector
  */
-template <size_t S, size_t Timestamp, size_t Voltage, size_t Position,
-          size_t Velocity>
-static std::vector<PreparedData> ConvertToPrepared(
-    const std::vector<std::array<double, S>>& data) {
+static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
   std::vector<PreparedData> prepared;
-  for (int i = 0; i < static_cast<int>(data.size()) - 1; ++i) {
-    const auto& pt1 = data[i];
-    const auto& pt2 = data[i + 1];
-    prepared.emplace_back(PreparedData{
-        units::second_t{pt1[Timestamp]}, pt1[Voltage], pt1[Position],
-        pt1[Velocity], units::second_t{pt2[Timestamp] - pt1[Timestamp]}});
+  // assume we've selected down to a single contiguous run by this point
+  auto run = data.runs[0];
+
+  for (int i = 0; i < static_cast<int>(run.voltage.size()) - 1; ++i) {
+    const auto& currentVoltage = run.voltage[i];
+    const auto& nextVoltage = run.voltage[i + 1];
+
+    auto currentPosition = Lerp(currentVoltage.time, run.position);
+
+    auto currentVelocity = Lerp(currentVoltage.time, run.velocity);
+
+    prepared.emplace_back(PreparedData{currentVoltage.time,
+                                       currentVoltage.measurement.value(),
+                                       currentPosition, currentVelocity,
+                                       nextVoltage.time - currentVoltage.time});
   }
+
   return prepared;
 }
 
@@ -62,18 +84,16 @@
  *
  * @param dataset A reference to the dataset being used
  */
-template <size_t S>
-static void CopyRawData(
-    wpi::StringMap<std::vector<std::array<double, S>>>* dataset) {
+static void CopyRawData(wpi::StringMap<MotorData>* dataset) {
   auto& data = *dataset;
   // Loads the Raw Data
   for (auto& it : data) {
     auto key = it.first();
-    auto& dataset = it.getValue();
+    auto& motorData = it.getValue();
 
     if (!wpi::contains(key, "raw")) {
-      data[fmt::format("raw-{}", key)] = dataset;
-      data[fmt::format("original-raw-{}", key)] = dataset;
+      data[fmt::format("raw-{}", key)] = motorData;
+      data[fmt::format("original-raw-{}", key)] = motorData;
     }
   }
 }
@@ -94,416 +114,73 @@
 }
 
 void AnalysisManager::PrepareGeneralData() {
-  using Data = std::array<double, 4>;
-  wpi::StringMap<std::vector<Data>> data;
   wpi::StringMap<std::vector<PreparedData>> preparedData;
 
-  // Store the raw data columns.
-  static constexpr size_t kTimeCol = 0;
-  static constexpr size_t kVoltageCol = 1;
-  static constexpr size_t kPosCol = 2;
-  static constexpr size_t kVelCol = 3;
-
-  WPI_INFO(m_logger, "{}", "Reading JSON data.");
-  // Get the major components from the JSON and store them inside a StringMap.
-  for (auto&& key : AnalysisManager::kJsonDataKeys) {
-    data[key] = m_json.at(key).get<std::vector<Data>>();
-  }
-
   WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
-  // Ensure that voltage and velocity have the same sign. Also multiply
-  // positions and velocities by the factor.
-  for (auto it = data.begin(); it != data.end(); ++it) {
-    for (auto&& pt : it->second) {
-      pt[kVoltageCol] = std::copysign(pt[kVoltageCol], pt[kVelCol]);
-      pt[kPosCol] *= m_factor;
-      pt[kVelCol] *= m_factor;
-    }
-  }
 
   WPI_INFO(m_logger, "{}", "Copying raw data.");
-  CopyRawData(&data);
+  CopyRawData(&m_data.motorData);
 
   WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
   // Convert data to PreparedData structs
-  for (auto& it : data) {
+  for (auto& it : m_data.motorData) {
     auto key = it.first();
-    preparedData[key] =
-        ConvertToPrepared<4, kTimeCol, kVoltageCol, kPosCol, kVelCol>(
-            data[key]);
+    preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
+    WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
   }
 
   // Store the original datasets
-  m_originalDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(preparedData["original-raw-slow-forward"],
-                      preparedData["original-raw-slow-backward"],
-                      preparedData["original-raw-fast-forward"],
-                      preparedData["original-raw-fast-backward"]);
+  m_originalDataset =
+      CombineDatasets(preparedData["original-raw-quasistatic-forward"],
+                      preparedData["original-raw-quasistatic-reverse"],
+                      preparedData["original-raw-dynamic-forward"],
+                      preparedData["original-raw-dynamic-reverse"]);
 
   WPI_INFO(m_logger, "{}", "Initial trimming and filtering.");
   sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
                               m_velocityDelays, m_minStepTime, m_maxStepTime,
-                              m_unit);
+                              m_data.distanceUnit);
+
+  WPI_INFO(m_logger, "{}", m_minStepTime);
+  WPI_INFO(m_logger, "{}", m_maxStepTime);
 
   WPI_INFO(m_logger, "{}", "Acceleration filtering.");
   sysid::AccelFilter(&preparedData);
 
   WPI_INFO(m_logger, "{}", "Storing datasets.");
   // Store the raw datasets
-  m_rawDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(
-          preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
-          preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
+  m_rawDataset = CombineDatasets(preparedData["raw-quasistatic-forward"],
+                                 preparedData["raw-quasistatic-reverse"],
+                                 preparedData["raw-dynamic-forward"],
+                                 preparedData["raw-dynamic-reverse"]);
 
   // Store the filtered datasets
-  m_filteredDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(
-          preparedData["slow-forward"], preparedData["slow-backward"],
-          preparedData["fast-forward"], preparedData["fast-backward"]);
+  m_filteredDataset = CombineDatasets(
+      preparedData["quasistatic-forward"], preparedData["quasistatic-reverse"],
+      preparedData["dynamic-forward"], preparedData["dynamic-reverse"]);
 
-  m_startTimes = {preparedData["raw-slow-forward"][0].timestamp,
-                  preparedData["raw-slow-backward"][0].timestamp,
-                  preparedData["raw-fast-forward"][0].timestamp,
-                  preparedData["raw-fast-backward"][0].timestamp};
-}
-
-void AnalysisManager::PrepareAngularDrivetrainData() {
-  using Data = std::array<double, 9>;
-  wpi::StringMap<std::vector<Data>> data;
-  wpi::StringMap<std::vector<PreparedData>> preparedData;
-
-  // Store the relevant raw data columns.
-  static constexpr size_t kTimeCol = 0;
-  static constexpr size_t kLVoltageCol = 1;
-  static constexpr size_t kRVoltageCol = 2;
-  static constexpr size_t kLPosCol = 3;
-  static constexpr size_t kRPosCol = 4;
-  static constexpr size_t kLVelCol = 5;
-  static constexpr size_t kRVelCol = 6;
-  static constexpr size_t kAngleCol = 7;
-  static constexpr size_t kAngularRateCol = 8;
-
-  WPI_INFO(m_logger, "{}", "Reading JSON data.");
-  // Get the major components from the JSON and store them inside a StringMap.
-  for (auto&& key : AnalysisManager::kJsonDataKeys) {
-    data[key] = m_json.at(key).get<std::vector<Data>>();
-  }
-
-  WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
-  // Ensure that voltage and velocity have the same sign. Also multiply
-  // positions and velocities by the factor.
-  for (auto it = data.begin(); it != data.end(); ++it) {
-    for (auto&& pt : it->second) {
-      pt[kLPosCol] *= m_factor;
-      pt[kRPosCol] *= m_factor;
-      pt[kLVelCol] *= m_factor;
-      pt[kRVelCol] *= m_factor;
-
-      // Stores the average voltages in the left voltage column.
-      // This aggregates the left and right voltages into a single voltage
-      // column for the ConvertToPrepared() method. std::copysign() ensures the
-      // polarity of the voltage matches the direction the robot turns.
-      pt[kLVoltageCol] = std::copysign(
-          (std::abs(pt[kLVoltageCol]) + std::abs(pt[kRVoltageCol])) / 2,
-          pt[kAngularRateCol]);
-
-      // ω = (v_r - v_l) / trackwidth
-      // v = ωr => v = ω * trackwidth / 2
-      // (v_r - v_l) / trackwidth * (trackwidth / 2) = (v_r - v_l) / 2
-      // However, since we know this is an angular test, the left and right
-      // wheel velocities will have opposite signs, allowing us to add their
-      // absolute values and get the same result (in terms of magnitude).
-      // std::copysign() is used to make sure the direction of the wheel
-      // velocities matches the direction the robot turns.
-      pt[kAngularRateCol] =
-          std::copysign((std::abs(pt[kRVelCol]) + std::abs(pt[kLVelCol])) / 2,
-                        pt[kAngularRateCol]);
-    }
-  }
-
-  WPI_INFO(m_logger, "{}", "Calculating trackwidth");
-  // Aggregating all the deltas from all the tests
-  double leftDelta = 0.0;
-  double rightDelta = 0.0;
-  double angleDelta = 0.0;
-  for (const auto& it : data) {
-    auto key = it.first();
-    auto& trackWidthData = data[key];
-    leftDelta += std::abs(trackWidthData.back()[kLPosCol] -
-                          trackWidthData.front()[kLPosCol]);
-    rightDelta += std::abs(trackWidthData.back()[kRPosCol] -
-                           trackWidthData.front()[kRPosCol]);
-    angleDelta += std::abs(trackWidthData.back()[kAngleCol] -
-                           trackWidthData.front()[kAngleCol]);
-  }
-  m_trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta,
-                                            units::radian_t{angleDelta});
-
-  WPI_INFO(m_logger, "{}", "Copying raw data.");
-  CopyRawData(&data);
-
-  WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
-  // Convert raw data to prepared data
-  for (const auto& it : data) {
-    auto key = it.first();
-    preparedData[key] = ConvertToPrepared<9, kTimeCol, kLVoltageCol, kAngleCol,
-                                          kAngularRateCol>(data[key]);
-  }
-
-  // Create the distinct datasets and store them
-  m_originalDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(preparedData["original-raw-slow-forward"],
-                      preparedData["original-raw-slow-backward"],
-                      preparedData["original-raw-fast-forward"],
-                      preparedData["original-raw-fast-backward"]);
-
-  WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
-  sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
-                              m_velocityDelays, m_minStepTime, m_maxStepTime);
-
-  WPI_INFO(m_logger, "{}", "Acceleration filtering.");
-  sysid::AccelFilter(&preparedData);
-
-  WPI_INFO(m_logger, "{}", "Storing datasets.");
-  // Create the distinct datasets and store them
-  m_rawDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(
-          preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
-          preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
-  m_filteredDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(
-          preparedData["slow-forward"], preparedData["slow-backward"],
-          preparedData["fast-forward"], preparedData["fast-backward"]);
-
-  m_startTimes = {preparedData["slow-forward"][0].timestamp,
-                  preparedData["slow-backward"][0].timestamp,
-                  preparedData["fast-forward"][0].timestamp,
-                  preparedData["fast-backward"][0].timestamp};
-}
-
-void AnalysisManager::PrepareLinearDrivetrainData() {
-  using Data = std::array<double, 9>;
-  wpi::StringMap<std::vector<Data>> data;
-  wpi::StringMap<std::vector<PreparedData>> preparedData;
-
-  // Store the relevant raw data columns.
-  static constexpr size_t kTimeCol = 0;
-  static constexpr size_t kLVoltageCol = 1;
-  static constexpr size_t kRVoltageCol = 2;
-  static constexpr size_t kLPosCol = 3;
-  static constexpr size_t kRPosCol = 4;
-  static constexpr size_t kLVelCol = 5;
-  static constexpr size_t kRVelCol = 6;
-
-  // Get the major components from the JSON and store them inside a StringMap.
-  WPI_INFO(m_logger, "{}", "Reading JSON data.");
-  for (auto&& key : AnalysisManager::kJsonDataKeys) {
-    data[key] = m_json.at(key).get<std::vector<Data>>();
-  }
-
-  // Ensure that voltage and velocity have the same sign. Also multiply
-  // positions and velocities by the factor.
-  WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
-  for (auto it = data.begin(); it != data.end(); ++it) {
-    for (auto&& pt : it->second) {
-      pt[kLVoltageCol] = std::copysign(pt[kLVoltageCol], pt[kLVelCol]);
-      pt[kRVoltageCol] = std::copysign(pt[kRVoltageCol], pt[kRVelCol]);
-      pt[kLPosCol] *= m_factor;
-      pt[kRPosCol] *= m_factor;
-      pt[kLVelCol] *= m_factor;
-      pt[kRVelCol] *= m_factor;
-    }
-  }
-
-  WPI_INFO(m_logger, "{}", "Copying raw data.");
-  CopyRawData(&data);
-
-  // Convert data to PreparedData
-  WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
-  for (auto& it : data) {
-    auto key = it.first();
-
-    preparedData[fmt::format("left-{}", key)] =
-        ConvertToPrepared<9, kTimeCol, kLVoltageCol, kLPosCol, kLVelCol>(
-            data[key]);
-    preparedData[fmt::format("right-{}", key)] =
-        ConvertToPrepared<9, kTimeCol, kRVoltageCol, kRPosCol, kRVelCol>(
-            data[key]);
-  }
-
-  // Create the distinct raw datasets and store them
-  auto originalSlowForward = AnalysisManager::DataConcat(
-      preparedData["left-original-raw-slow-forward"],
-      preparedData["right-original-raw-slow-forward"]);
-  auto originalSlowBackward = AnalysisManager::DataConcat(
-      preparedData["left-original-raw-slow-backward"],
-      preparedData["right-original-raw-slow-backward"]);
-  auto originalFastForward = AnalysisManager::DataConcat(
-      preparedData["left-original-raw-fast-forward"],
-      preparedData["right-original-raw-fast-forward"]);
-  auto originalFastBackward = AnalysisManager::DataConcat(
-      preparedData["left-original-raw-fast-backward"],
-      preparedData["right-original-raw-fast-backward"]);
-  m_originalDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(originalSlowForward, originalSlowBackward,
-                      originalFastForward, originalFastBackward);
-  m_originalDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
-      CombineDatasets(preparedData["left-original-raw-slow-forward"],
-                      preparedData["left-original-raw-slow-backward"],
-                      preparedData["left-original-raw-fast-forward"],
-                      preparedData["left-original-raw-fast-backward"]);
-  m_originalDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kRight)] =
-      CombineDatasets(preparedData["right-original-raw-slow-forward"],
-                      preparedData["right-original-raw-slow-backward"],
-                      preparedData["right-original-raw-fast-forward"],
-                      preparedData["right-original-raw-fast-backward"]);
-
-  WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
-  sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
-                              m_velocityDelays, m_minStepTime, m_maxStepTime);
-
-  auto slowForward = AnalysisManager::DataConcat(
-      preparedData["left-slow-forward"], preparedData["right-slow-forward"]);
-  auto slowBackward = AnalysisManager::DataConcat(
-      preparedData["left-slow-backward"], preparedData["right-slow-backward"]);
-  auto fastForward = AnalysisManager::DataConcat(
-      preparedData["left-fast-forward"], preparedData["right-fast-forward"]);
-  auto fastBackward = AnalysisManager::DataConcat(
-      preparedData["left-fast-backward"], preparedData["right-fast-backward"]);
-
-  WPI_INFO(m_logger, "{}", "Acceleration filtering.");
-  sysid::AccelFilter(&preparedData);
-
-  WPI_INFO(m_logger, "{}", "Storing datasets.");
-
-  // Create the distinct raw datasets and store them
-  auto rawSlowForward =
-      AnalysisManager::DataConcat(preparedData["left-raw-slow-forward"],
-                                  preparedData["right-raw-slow-forward"]);
-  auto rawSlowBackward =
-      AnalysisManager::DataConcat(preparedData["left-raw-slow-backward"],
-                                  preparedData["right-raw-slow-backward"]);
-  auto rawFastForward =
-      AnalysisManager::DataConcat(preparedData["left-raw-fast-forward"],
-                                  preparedData["right-raw-fast-forward"]);
-  auto rawFastBackward =
-      AnalysisManager::DataConcat(preparedData["left-raw-fast-backward"],
-                                  preparedData["right-raw-fast-backward"]);
-
-  m_rawDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward,
-                      rawFastBackward);
-  m_rawDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
-      CombineDatasets(preparedData["left-raw-slow-forward"],
-                      preparedData["left-raw-slow-backward"],
-                      preparedData["left-raw-fast-forward"],
-                      preparedData["left-raw-fast-backward"]);
-  m_rawDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kRight)] =
-      CombineDatasets(preparedData["right-raw-slow-forward"],
-                      preparedData["right-raw-slow-backward"],
-                      preparedData["right-raw-fast-forward"],
-                      preparedData["right-raw-fast-backward"]);
-
-  // Create the distinct filtered datasets and store them
-  m_filteredDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
-      CombineDatasets(slowForward, slowBackward, fastForward, fastBackward);
-  m_filteredDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
-      CombineDatasets(preparedData["left-slow-forward"],
-                      preparedData["left-slow-backward"],
-                      preparedData["left-fast-forward"],
-                      preparedData["left-fast-backward"]);
-  m_filteredDataset[static_cast<int>(
-      AnalysisManager::Settings::DrivetrainDataset::kRight)] =
-      CombineDatasets(preparedData["right-slow-forward"],
-                      preparedData["right-slow-backward"],
-                      preparedData["right-fast-forward"],
-                      preparedData["right-fast-backward"]);
-
-  m_startTimes = {
-      rawSlowForward.front().timestamp, rawSlowBackward.front().timestamp,
-      rawFastForward.front().timestamp, rawFastBackward.front().timestamp};
+  m_startTimes = {preparedData["raw-quasistatic-forward"][0].timestamp,
+                  preparedData["raw-quasistatic-reverse"][0].timestamp,
+                  preparedData["raw-dynamic-forward"][0].timestamp,
+                  preparedData["raw-dynamic-reverse"][0].timestamp};
 }
 
 AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger)
-    : m_logger{logger},
-      m_settings{settings},
-      m_type{analysis::kSimple},
-      m_unit{"Meters"},
-      m_factor{1} {}
+    : m_logger{logger}, m_settings{settings} {}
 
-AnalysisManager::AnalysisManager(std::string_view path, Settings& settings,
+AnalysisManager::AnalysisManager(TestData data, Settings& settings,
                                  wpi::Logger& logger)
-    : m_logger{logger}, m_settings{settings} {
-  {
-    // Read JSON from the specified path
-    std::error_code ec;
-    std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
-        wpi::MemoryBuffer::GetFile(path, ec);
-    if (fileBuffer == nullptr || ec) {
-      throw FileReadingError(path);
-    }
-
-    m_json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
-
-    WPI_INFO(m_logger, "Read {}", path);
-  }
-
-  // Check that we have a sysid JSON
-  if (m_json.find("sysid") == m_json.end()) {
-    // If it's not a sysid JSON, try converting it from frc-char format
-    std::string newPath = sysid::ConvertJSON(path, logger);
-
-    // Read JSON from the specified path
-    std::error_code ec;
-    std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
-        wpi::MemoryBuffer::GetFile(path, ec);
-    if (fileBuffer == nullptr || ec) {
-      throw FileReadingError(newPath);
-    }
-
-    m_json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
-
-    WPI_INFO(m_logger, "Read {}", newPath);
-  }
-
-  WPI_INFO(m_logger, "Parsing initial data of {}", path);
-  // Get the analysis type from the JSON.
-  m_type = sysid::analysis::FromName(m_json.at("test").get<std::string>());
-
-  // Get the rotation -> output units factor from the JSON.
-  m_unit = m_json.at("units").get<std::string>();
-  m_factor = m_json.at("unitsPerRotation").get<double>();
-  WPI_DEBUG(m_logger, "Parsing units per rotation as {} {} per rotation",
-            m_factor, m_unit);
-
+    : m_data{std::move(data)}, m_logger{logger}, m_settings{settings} {
   // Reset settings for Dynamic Test Limits
   m_settings.stepTestDuration = units::second_t{0.0};
   m_settings.motionThreshold = std::numeric_limits<double>::infinity();
 }
 
 void AnalysisManager::PrepareData() {
-  WPI_INFO(m_logger, "Preparing {} data", m_type.name);
-  if (m_type == analysis::kDrivetrain) {
-    PrepareLinearDrivetrainData();
-  } else if (m_type == analysis::kDrivetrainAngular) {
-    PrepareAngularDrivetrainData();
-  } else {
-    PrepareGeneralData();
-  }
+  //  WPI_INFO(m_logger, "Preparing {} data", m_data.mechanismType.name);
+
+  PrepareGeneralData();
+
   WPI_INFO(m_logger, "{}", "Finished Preparing Data");
 }
 
@@ -515,12 +192,13 @@
 
   WPI_INFO(m_logger, "{}", "Calculating Gains");
   // Calculate feedforward gains from the data.
-  const auto& ff = sysid::CalculateFeedforwardGains(GetFilteredData(), m_type);
-  FeedforwardGains ffGains = {ff, m_trackWidth};
+  const auto& ff = sysid::CalculateFeedforwardGains(
+      GetFilteredData(), m_data.mechanismType, false);
+  FeedforwardGains ffGains = {ff};
 
-  const auto& Ks = std::get<0>(ff)[0];
-  const auto& Kv = std::get<0>(ff)[1];
-  const auto& Ka = std::get<0>(ff)[2];
+  const auto& Ks = ff.coeffs[0];
+  const auto& Kv = ff.coeffs[1];
+  const auto& Ka = ff.coeffs[2];
 
   if (Ka <= 0 || Kv < 0) {
     throw InvalidDataError(
@@ -542,27 +220,20 @@
   if (m_settings.type == FeedbackControllerLoopType::kPosition) {
     fb = sysid::CalculatePositionFeedbackGains(
         m_settings.preset, m_settings.lqr, Kv, Ka,
-        m_settings.convertGainsToEncTicks
-            ? m_settings.gearing * m_settings.cpr * m_factor
-            : 1);
+        m_settings.convertGainsToEncTicks ? m_settings.gearing * m_settings.cpr
+                                          : 1);
   } else {
     fb = sysid::CalculateVelocityFeedbackGains(
         m_settings.preset, m_settings.lqr, Kv, Ka,
-        m_settings.convertGainsToEncTicks
-            ? m_settings.gearing * m_settings.cpr * m_factor
-            : 1);
+        m_settings.convertGainsToEncTicks ? m_settings.gearing * m_settings.cpr
+                                          : 1);
   }
 
   return fb;
 }
 
-void AnalysisManager::OverrideUnits(std::string_view unit,
-                                    double unitsPerRotation) {
-  m_unit = unit;
-  m_factor = unitsPerRotation;
+void AnalysisManager::OverrideUnits(std::string_view unit) {
+  m_data.distanceUnit = unit;
 }
 
-void AnalysisManager::ResetUnitsFromJSON() {
-  m_unit = m_json.at("units").get<std::string>();
-  m_factor = m_json.at("unitsPerRotation").get<double>();
-}
+void AnalysisManager::ResetUnitsFromJSON() {}
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisType.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
index 18b461f..6ef27c9 100644
--- a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
@@ -7,12 +7,6 @@
 using namespace sysid;
 
 AnalysisType sysid::analysis::FromName(std::string_view name) {
-  if (name == "Drivetrain") {
-    return sysid::analysis::kDrivetrain;
-  }
-  if (name == "Drivetrain (Angular)") {
-    return sysid::analysis::kDrivetrainAngular;
-  }
   if (name == "Elevator") {
     return sysid::analysis::kElevator;
   }
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
index b7a9fce..110ff94 100644
--- a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
@@ -4,8 +4,13 @@
 
 #include "sysid/analysis/FeedforwardAnalysis.h"
 
+#include <array>
+#include <bitset>
 #include <cmath>
 
+#include <Eigen/Eigenvalues>
+#include <fmt/format.h>
+#include <fmt/ranges.h>
 #include <units/math.h>
 #include <units/time.h>
 
@@ -13,10 +18,25 @@
 #include "sysid/analysis/FilteringUtils.h"
 #include "sysid/analysis/OLS.h"
 
-using namespace sysid;
+namespace sysid {
 
 /**
- * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ).
+ * Populates OLS data for the following models:
+ *
+ * Simple, Drivetrain, DrivetrainAngular:
+ *
+ *   (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ)
+ *
+ * Elevator:
+ *
+ *   (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ
+ *
+ * Arm:
+ *
+ *   (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ cos(angle) + ε sin(angle)
+ *
+ * OLS performs best with the noisiest variable as the dependent variable, so we
+ * regress acceleration in terms of the other variables.
  *
  * @param d List of characterization data.
  * @param type Type of system being identified.
@@ -27,36 +47,136 @@
                             const AnalysisType& type,
                             Eigen::Block<Eigen::MatrixXd> X,
                             Eigen::VectorBlock<Eigen::VectorXd> y) {
+  // Fill in X and y row-wise
   for (size_t sample = 0; sample < d.size(); ++sample) {
     const auto& pt = d[sample];
 
-    // Add the velocity term (for α)
+    // Set the velocity term (for α)
     X(sample, 0) = pt.velocity;
 
-    // Add the voltage term (for β)
+    // Set the voltage term (for β)
     X(sample, 1) = pt.voltage;
 
-    // Add the intercept term (for γ)
+    // Set the intercept term (for γ)
     X(sample, 2) = std::copysign(1, pt.velocity);
 
-    // Add test-specific variables
+    // Set test-specific variables
     if (type == analysis::kElevator) {
-      // Add the gravity term (for Kg)
+      // Set the gravity term (for δ)
       X(sample, 3) = 1.0;
     } else if (type == analysis::kArm) {
-      // Add the cosine and sine terms (for Kg)
+      // Set the cosine and sine terms (for δ and ε)
       X(sample, 3) = pt.cos;
       X(sample, 4) = pt.sin;
     }
 
-    // Add the dependent variable (acceleration)
+    // Set the dependent variable (acceleration)
     y(sample) = pt.acceleration;
   }
 }
 
-std::tuple<std::vector<double>, double, double>
-sysid::CalculateFeedforwardGains(const Storage& data,
-                                 const AnalysisType& type) {
+/**
+ * Throws an InsufficientSamplesError if the collected data is poor for OLS.
+ *
+ * @param X The collected data in matrix form for OLS.
+ * @param type The analysis type.
+ */
+static void CheckOLSDataQuality(const Eigen::MatrixXd& X,
+                                const AnalysisType& type) {
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigSolver{X.transpose() * X};
+  const Eigen::VectorXd& eigvals = eigSolver.eigenvalues();
+  const Eigen::MatrixXd& eigvecs = eigSolver.eigenvectors();
+
+  // Bits are Ks, Kv, Ka, Kg, offset
+  std::bitset<5> badGains;
+
+  constexpr double threshold = 10.0;
+
+  // For n x n matrix XᵀX, need n nonzero eigenvalues for good fit
+  for (int row = 0; row < eigvals.rows(); ++row) {
+    // Find row of eigenvector with largest magnitude. This determines the
+    // primary regression variable that corresponds to the eigenvalue.
+    int maxIndex;
+    double maxCoeff = eigvecs.col(row).cwiseAbs().maxCoeff(&maxIndex);
+
+    // Check whether the eigenvector component along the regression variable's
+    // direction is below the threshold. If it is, the regression variable's fit
+    // is bad.
+    if (std::abs(eigvals(row) * maxCoeff) <= threshold) {
+      // Fit for α is bad
+      if (maxIndex == 0) {
+        // Affects Kv
+        badGains.set(1);
+      }
+
+      // Fit for β is bad
+      if (maxIndex == 1) {
+        // Affects all gains
+        badGains.set();
+        break;
+      }
+
+      // Fit for γ is bad
+      if (maxIndex == 2) {
+        // Affects Ks
+        badGains.set(0);
+      }
+
+      // Fit for δ is bad
+      if (maxIndex == 3) {
+        if (type == analysis::kElevator) {
+          // Affects Kg
+          badGains.set(3);
+        } else if (type == analysis::kArm) {
+          // Affects Kg and offset
+          badGains.set(3);
+          badGains.set(4);
+        }
+      }
+
+      // Fit for ε is bad
+      if (maxIndex == 4) {
+        // Affects Kg and offset
+        badGains.set(3);
+        badGains.set(4);
+      }
+    }
+  }
+
+  // If any gains are bad, throw an error
+  if (badGains.any()) {
+    // Create list of bad gain names
+    constexpr std::array gainNames{"Ks", "Kv", "Ka", "Kg", "offset"};
+    std::vector<std::string_view> badGainsList;
+    for (size_t i = 0; i < badGains.size(); ++i) {
+      if (badGains.test(i)) {
+        badGainsList.emplace_back(gainNames[i]);
+      }
+    }
+
+    std::string error = fmt::format("Insufficient samples to compute {}.\n\n",
+                                    fmt::join(badGainsList, ", "));
+
+    // If all gains are bad, the robot may not have moved
+    if (badGains.all()) {
+      error += "Either no data was collected or the robot didn't move.\n\n";
+    }
+
+    // Append guidance for fixing the data
+    error +=
+        "Ensure the data has:\n\n"
+        "  * at least 2 steady-state velocity events to separate Ks from Kv\n"
+        "  * at least 1 acceleration event to find Ka\n"
+        "  * for elevators, enough vertical motion to measure gravity\n"
+        "  * for arms, enough range of motion to measure gravity and encoder "
+        "offset\n";
+    throw InsufficientSamplesError{error};
+  }
+}
+
+OLSResult CalculateFeedforwardGains(const Storage& data,
+                                    const AnalysisType& type,
+                                    bool throwOnBadData) {
   // Iterate through the data and add it to our raw vector.
   const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
 
@@ -87,34 +207,68 @@
                   X.block(rowOffset, 0, fastBackward.size(), X.cols()),
                   y.segment(rowOffset, fastBackward.size()));
 
-  // Perform OLS with accel = alpha*vel + beta*voltage + gamma*signum(vel)
-  // OLS performs best with the noisiest variable as the dependent var,
-  // so we regress accel in terms of the other variables.
-  auto ols = sysid::OLS(X, y);
-  double alpha = std::get<0>(ols)[0];  // -Kv/Ka
-  double beta = std::get<0>(ols)[1];   // 1/Ka
-  double gamma = std::get<0>(ols)[2];  // -Ks/Ka
-
-  // Initialize gains list with Ks, Kv, and Ka
-  std::vector<double> gains{-gamma / beta, -alpha / beta, 1 / beta};
-
-  if (type == analysis::kElevator) {
-    // Add Kg to gains list
-    double delta = std::get<0>(ols)[3];  // -Kg/Ka
-    gains.emplace_back(-delta / beta);
+  // Check quality of collected data
+  if (throwOnBadData) {
+    CheckOLSDataQuality(X, type);
   }
 
-  if (type == analysis::kArm) {
-    double delta = std::get<0>(ols)[3];    // -Kg/Ka cos(offset)
-    double epsilon = std::get<0>(ols)[4];  // Kg/Ka sin(offset)
+  std::vector<double> gains;
+  gains.reserve(X.rows());
 
-    // Add Kg to gains list
-    gains.emplace_back(std::hypot(delta, epsilon) / beta);
+  auto ols = OLS(X, y);
 
-    // Add offset to gains list
-    gains.emplace_back(std::atan2(epsilon, -delta));
+  // Calculate feedforward gains
+  //
+  // See docs/ols-derivations.md for more details.
+  {
+    // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+    // dx/dt = αx + βu + γ sgn(x)
+
+    // α = -Kv/Ka
+    // β = 1/Ka
+    // γ = -Ks/Ka
+    double α = ols.coeffs[0];
+    double β = ols.coeffs[1];
+    double γ = ols.coeffs[2];
+
+    // Ks = -γ/β
+    // Kv = -α/β
+    // Ka = 1/β
+    gains.emplace_back(-γ / β);
+    gains.emplace_back(-α / β);
+    gains.emplace_back(1 / β);
+
+    if (type == analysis::kElevator) {
+      // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka
+      // dx/dt = αx + βu + γ sgn(x) + δ
+
+      // δ = -Kg/Ka
+      double δ = ols.coeffs[3];
+
+      // Kg = -δ/β
+      gains.emplace_back(-δ / β);
+    }
+
+    if (type == analysis::kArm) {
+      // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+      //           - Kg/Ka cos(offset) cos(angle)                   NOLINT
+      //           + Kg/Ka sin(offset) sin(angle)                   NOLINT
+      // dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)   NOLINT
+
+      // δ = -Kg/Ka cos(offset)
+      // ε = Kg/Ka sin(offset)
+      double δ = ols.coeffs[3];
+      double ε = ols.coeffs[4];
+
+      // Kg = hypot(δ, ε)/β      NOLINT
+      // offset = atan2(ε, -δ)   NOLINT
+      gains.emplace_back(std::hypot(δ, ε) / β);
+      gains.emplace_back(std::atan2(ε, -δ));
+    }
   }
 
   // Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only)
-  return std::tuple{gains, std::get<1>(ols), std::get<2>(ols)};
+  return OLSResult{gains, ols.rSquared, ols.rmse};
 }
+
+}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
index 6c66ef8..838968f 100644
--- a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
@@ -153,32 +153,24 @@
 
   minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime);
 
-  // If step duration hasn't been set yet, calculate a default (find the entry
-  // before the acceleration first hits zero)
-  if (settings->stepTestDuration <= minStepTime) {
-    // Get noise floor
-    const double accelNoiseFloor = GetNoiseFloor(
-        *data, kNoiseMeanWindow, [](auto&& pt) { return pt.acceleration; });
-    // Find latest element with nonzero acceleration
-    auto endIt = std::find_if(
-        data->rbegin(), data->rend(), [&](const PreparedData& entry) {
-          return std::abs(entry.acceleration) > accelNoiseFloor;
-        });
+  // Find maximum speed reached
+  const auto maxSpeed =
+      GetMaxSpeed(*data, [](auto&& pt) { return pt.velocity; });
+  // Find place where 90% of maximum speed exceeded
+  auto endIt =
+      std::find_if(data->begin(), data->end(), [&](const PreparedData& entry) {
+        return std::abs(entry.velocity) > maxSpeed * 0.9;
+      });
 
-    if (endIt != data->rend()) {
-      // Calculate default duration
-      settings->stepTestDuration = std::min(
-          endIt->timestamp - data->front().timestamp + minStepTime + 1_s,
-          maxStepTime);
-    } else {
-      settings->stepTestDuration = maxStepTime;
-    }
+  if (endIt != data->end()) {
+    settings->stepTestDuration = std::min(
+        endIt->timestamp - data->front().timestamp + minStepTime, maxStepTime);
   }
 
   // Find first entry greater than the step test duration
   auto maxIt =
       std::find_if(data->begin(), data->end(), [&](PreparedData entry) {
-        return entry.timestamp - data->front().timestamp + minStepTime >
+        return entry.timestamp - data->front().timestamp >
                settings->stepTestDuration;
       });
 
@@ -186,6 +178,7 @@
   if (maxIt != data->end()) {
     data->erase(maxIt, data->end());
   }
+
   return std::make_tuple(minStepTime, positionDelay, velocityDelay);
 }
 
@@ -204,6 +197,16 @@
   return std::sqrt(sum / (data.size() - step));
 }
 
+double sysid::GetMaxSpeed(
+    const std::vector<PreparedData>& data,
+    std::function<double(const PreparedData&)> accessorFunction) {
+  double max = 0.0;
+  for (size_t i = 0; i < data.size(); i++) {
+    max = std::max(max, std::abs(accessorFunction(data[i])));
+  }
+  return max;
+}
+
 units::second_t sysid::GetMeanTimeDelta(const std::vector<PreparedData>& data) {
   std::vector<units::second_t> dts;
 
@@ -301,7 +304,7 @@
     auto key = it.first();
     auto& dataset = it.getValue();
 
-    if (IsRaw(key) && wpi::contains(key, "fast")) {
+    if (IsRaw(key) && wpi::contains(key, "dynamic")) {
       auto duration = dataset.back().timestamp - dataset.front().timestamp;
       if (duration > maxStepTime) {
         maxStepTime = duration;
@@ -327,7 +330,7 @@
     for (auto& it : preparedData) {
       auto key = it.first();
       auto& dataset = it.getValue();
-      if (wpi::contains(key, "slow")) {
+      if (wpi::contains(key, "quasistatic")) {
         settings->motionThreshold =
             std::min(settings->motionThreshold,
                      GetNoiseFloor(dataset, kNoiseMeanWindow,
@@ -342,7 +345,7 @@
 
     // Trim quasistatic test data to remove all points where voltage is zero or
     // velocity < motion threshold.
-    if (wpi::contains(key, "slow")) {
+    if (wpi::contains(key, "quasistatic")) {
       dataset.erase(std::remove_if(dataset.begin(), dataset.end(),
                                    [&](const auto& pt) {
                                      return std::abs(pt.voltage) <= 0 ||
@@ -366,7 +369,7 @@
     PrepareMechData(&dataset, unit);
 
     // Trims filtered Dynamic Test Data
-    if (IsFiltered(key) && wpi::contains(key, "fast")) {
+    if (IsFiltered(key) && wpi::contains(key, "dynamic")) {
       // Get the filtered dataset name
       auto filteredKey = RemoveStr(key, "raw-");
 
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/JSONConverter.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
deleted file mode 100644
index 9bab8c6..0000000
--- a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-// 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 "sysid/analysis/JSONConverter.h"
-
-#include <stdexcept>
-#include <string>
-
-#include <fmt/core.h>
-#include <fmt/format.h>
-#include <wpi/Logger.h>
-#include <wpi/MemoryBuffer.h>
-#include <wpi/fmt/raw_ostream.h>
-#include <wpi/json.h>
-#include <wpi/raw_ostream.h>
-
-#include "sysid/Util.h"
-#include "sysid/analysis/AnalysisManager.h"
-#include "sysid/analysis/AnalysisType.h"
-
-// Sizes of the arrays for new sysid data.
-static constexpr size_t kDrivetrainSize = 9;
-static constexpr size_t kGeneralSize = 4;
-
-// Indices for the old data.
-static constexpr size_t kTimestampCol = 0;
-static constexpr size_t kLVoltsCol = 3;
-static constexpr size_t kRVoltsCol = 4;
-static constexpr size_t kLPosCol = 5;
-static constexpr size_t kRPosCol = 6;
-static constexpr size_t kLVelCol = 7;
-static constexpr size_t kRVelCol = 8;
-
-static wpi::json GetJSON(std::string_view path, wpi::Logger& logger) {
-  std::error_code ec;
-  std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
-      wpi::MemoryBuffer::GetFile(path, ec);
-  if (fileBuffer == nullptr || ec) {
-    throw std::runtime_error(fmt::format("Unable to read: {}", path));
-  }
-
-  wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
-  WPI_INFO(logger, "Read frc-characterization JSON from {}", path);
-  return json;
-}
-
-std::string sysid::ConvertJSON(std::string_view path, wpi::Logger& logger) {
-  wpi::json ojson = GetJSON(path, logger);
-
-  auto type = sysid::analysis::FromName(ojson.at("test").get<std::string>());
-  auto factor = ojson.at("unitsPerRotation").get<double>();
-  auto unit = ojson.at("units").get<std::string>();
-
-  wpi::json json;
-  for (auto&& key : AnalysisManager::kJsonDataKeys) {
-    if (type == analysis::kDrivetrain) {
-      // Get the old data; create a vector for the new data; reserve the
-      // appropriate size for the new data.
-      auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
-      std::vector<std::array<double, kDrivetrainSize>> data;
-      data.reserve(odata.size());
-
-      // Transfer the data.
-      for (auto&& pt : odata) {
-        data.push_back(std::array<double, kDrivetrainSize>{
-            pt[kTimestampCol], pt[kLVoltsCol], pt[kRVoltsCol], pt[kLPosCol],
-            pt[kRPosCol], pt[kLVelCol], pt[kRVelCol], 0.0, 0.0});
-      }
-      json[key] = data;
-    } else {
-      // Get the old data; create a vector for the new data; reserve the
-      // appropriate size for the new data.
-      auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
-      std::vector<std::array<double, kGeneralSize>> data;
-      data.reserve(odata.size());
-
-      // Transfer the data.
-      for (auto&& pt : odata) {
-        data.push_back(std::array<double, kGeneralSize>{
-            pt[kTimestampCol], pt[kLVoltsCol], pt[kLPosCol], pt[kLVelCol]});
-      }
-      json[key] = data;
-    }
-  }
-  json["units"] = unit;
-  json["unitsPerRotation"] = factor;
-  json["test"] = type.name;
-  json["sysid"] = true;
-
-  // Write the new file with "_new" appended to it.
-  path.remove_suffix(std::string_view{".json"}.size());
-  std::string loc = fmt::format("{}_new.json", path);
-
-  sysid::SaveFile(json.dump(2), std::filesystem::path{loc});
-
-  WPI_INFO(logger, "Wrote new JSON to: {}", loc);
-  return loc;
-}
-
-std::string sysid::ToCSV(std::string_view path, wpi::Logger& logger) {
-  wpi::json json = GetJSON(path, logger);
-
-  auto type = sysid::analysis::FromName(json.at("test").get<std::string>());
-  auto factor = json.at("unitsPerRotation").get<double>();
-  auto unit = json.at("units").get<std::string>();
-  std::string_view abbreviation = GetAbbreviation(unit);
-
-  std::error_code ec;
-  // Naming: {sysid-json-name}(Test, Units).csv
-  path.remove_suffix(std::string_view{".json"}.size());
-  std::string loc = fmt::format("{} ({}, {}).csv", path, type.name, unit);
-  wpi::raw_fd_ostream outputFile{loc, ec};
-
-  if (ec) {
-    throw std::runtime_error("Unable to write to: " + loc);
-  }
-
-  fmt::print(outputFile, "Timestamp (s),Test,");
-  if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
-    fmt::print(
-        outputFile,
-        "Left Volts (V),Right Volts (V),Left Position ({0}),Right "
-        "Position ({0}),Left Velocity ({0}/s),Right Velocity ({0}/s),Gyro "
-        "Position (deg),Gyro Rate (deg/s)\n",
-        abbreviation);
-  } else {
-    fmt::print(outputFile, "Volts (V),Position({0}),Velocity ({0}/s)\n",
-               abbreviation);
-  }
-  outputFile << "\n";
-
-  for (auto&& key : AnalysisManager::kJsonDataKeys) {
-    if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
-      auto tempData =
-          json.at(key).get<std::vector<std::array<double, kDrivetrainSize>>>();
-      for (auto&& pt : tempData) {
-        fmt::print(outputFile, "{},{},{},{},{},{},{},{},{},{}\n",
-                   pt[0],                           // Timestamp
-                   key,                             // Test
-                   pt[1], pt[2],                    // Left and Right Voltages
-                   pt[3] * factor, pt[4] * factor,  // Left and Right Positions
-                   pt[5] * factor, pt[6] * factor,  // Left and Right Velocity
-                   pt[7], pt[8]  // Gyro Position and Velocity
-        );
-      }
-    } else {
-      auto tempData =
-          json.at(key).get<std::vector<std::array<double, kGeneralSize>>>();
-      for (auto&& pt : tempData) {
-        fmt::print(outputFile, "{},{},{},{},{}\n",
-                   pt[0],           // Timestamp,
-                   key,             // Test
-                   pt[1],           // Voltage
-                   pt[2] * factor,  // Position
-                   pt[3] * factor   // Velocity
-        );
-      }
-    }
-  }
-  outputFile.flush();
-  WPI_INFO(logger, "Wrote CSV to: {}", loc);
-  return loc;
-}
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/OLS.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/OLS.cpp
index d095a48..5d630d0 100644
--- a/third_party/allwpilib/sysid/src/main/native/cpp/analysis/OLS.cpp
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/analysis/OLS.cpp
@@ -4,45 +4,85 @@
 
 #include "sysid/analysis/OLS.h"
 
-#include <tuple>
-#include <vector>
+#include <cassert>
+#include <cmath>
 
 #include <Eigen/Cholesky>
-#include <Eigen/Core>
 
-using namespace sysid;
+namespace sysid {
 
-std::tuple<std::vector<double>, double, double> sysid::OLS(
-    const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
+OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
   assert(X.rows() == y.rows());
 
-  // The linear model can be written as follows:
-  // y = Xβ + u, where y is the dependent observed variable, X is the matrix
-  // of independent variables, β is a vector of coefficients, and u is a
-  // vector of residuals.
+  // The linear regression model can be written as follows:
+  //
+  //   y = Xβ + ε
+  //
+  // where y is the dependent observed variable, X is the matrix of independent
+  // variables, β is a vector of coefficients, and ε is a vector of residuals.
+  //
+  // We want to find the value of β that minimizes εᵀε.
+  //
+  //   ε = y − Xβ
+  //   εᵀε = (y − Xβ)ᵀ(y − Xβ)
+  //
+  //   β̂ = argmin (y − Xβ)ᵀ(y − Xβ)
+  //         β
+  //
+  // Take the partial derivative of the cost function with respect to β and set
+  // it equal to zero, then solve for β̂ .
+  //
+  //   0 = −2Xᵀ(y − Xβ̂)
+  //   0 = Xᵀ(y − Xβ̂)
+  //   0 = Xᵀy − XᵀXβ̂
+  //   XᵀXβ̂ = Xᵀy
+  //   β̂  = (XᵀX)⁻¹Xᵀy
 
-  // We want to minimize u² = uᵀu = (y - Xβ)ᵀ(y - Xβ).
   // β = (XᵀX)⁻¹Xᵀy
+  //
+  // XᵀX is guaranteed to be symmetric positive definite, so an LLT
+  // decomposition can be used.
+  Eigen::MatrixXd β = (X.transpose() * X).llt().solve(X.transpose() * y);
 
-  // Calculate β that minimizes uᵀu.
-  Eigen::MatrixXd beta = (X.transpose() * X).llt().solve(X.transpose() * y);
+  // Error sum of squares
+  double SSE = (y - X * β).squaredNorm();
 
-  // We will now calculate R² or the coefficient of determination, which
-  // tells us how much of the total variation (variation in y) can be
-  // explained by the regression model.
+  // Sample size
+  int n = X.rows();
 
-  // We will first calculate the sum of the squares of the error, or the
-  // variation in error (SSE).
-  double SSE = (y - X * beta).squaredNorm();
+  // Number of explanatory variables
+  int p = β.rows();
 
-  int n = X.cols();
+  // Total sum of squares (total variation in y)
+  //
+  // From slide 24 of
+  // http://www.stat.columbia.edu/~fwood/Teaching/w4315/Fall2009/lecture_11:
+  //
+  //   SSTO = yᵀy - 1/n yᵀJy
+  //
+  // Let J = I.
+  //
+  //   SSTO = yᵀy - 1/n yᵀy
+  //   SSTO = (n − 1)/n yᵀy
+  double SSTO = (n - 1.0) / n * (y.transpose() * y).value();
 
-  // Now we will calculate the total variation in y, known as SSTO.
-  double SSTO = ((y.transpose() * y) - (1.0 / n) * (y.transpose() * y)).value();
+  // R² or the coefficient of determination, which represents how much of the
+  // total variation (variation in y) can be explained by the regression model
+  double rSquared = 1.0 - SSE / SSTO;
 
-  double rSquared = (SSTO - SSE) / SSTO;
-  double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - 3.0));
+  // Adjusted R²
+  //
+  //                       n − 1
+  //   R̅² = 1 − (1 − R²) ---------
+  //                     n − p − 1
+  //
+  // See https://en.wikipedia.org/wiki/Coefficient_of_determination#Adjusted_R2
+  double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - p - 1.0));
+
+  // Root-mean-square error
   double RMSE = std::sqrt(SSE / n);
 
-  return {{beta.data(), beta.data() + beta.rows()}, adjRSquared, RMSE};
+  return {{β.data(), β.data() + β.size()}, adjRSquared, RMSE};
 }
+
+}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
deleted file mode 100644
index ac97cdb..0000000
--- a/third_party/allwpilib/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
+++ /dev/null
@@ -1,275 +0,0 @@
-// 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 "sysid/telemetry/TelemetryManager.h"
-
-#include <algorithm>
-#include <cctype>  // for ::tolower
-#include <numbers>
-#include <stdexcept>
-#include <string>
-#include <utility>
-
-#include <fmt/chrono.h>
-#include <networktables/NetworkTableInstance.h>
-#include <wpi/Logger.h>
-#include <wpi/SmallVector.h>
-#include <wpi/StringExtras.h>
-#include <wpi/raw_ostream.h>
-#include <wpi/timestamp.h>
-
-#include "sysid/Util.h"
-#include "sysid/analysis/AnalysisType.h"
-
-using namespace sysid;
-
-TelemetryManager::TelemetryManager(const Settings& settings,
-                                   wpi::Logger& logger,
-                                   nt::NetworkTableInstance instance)
-    : m_settings(settings), m_logger(logger), m_inst(instance) {}
-
-void TelemetryManager::BeginTest(std::string_view name) {
-  // Create a new test params instance for this test.
-  m_params =
-      TestParameters{name.starts_with("fast"), name.ends_with("forward"),
-                     m_settings.mechanism == analysis::kDrivetrainAngular,
-                     State::WaitingForEnable};
-
-  // Add this test to the list of running tests and set the running flag.
-  m_tests.push_back(std::string{name});
-  m_isRunningTest = true;
-
-  // Set the Voltage Command Entry
-  m_voltageCommand.Set((m_params.fast ? m_settings.stepVoltage
-                                      : m_settings.quasistaticRampRate) *
-                       (m_params.forward ? 1 : -1));
-
-  // Set the test type
-  m_testType.Set(m_params.fast ? "Dynamic" : "Quasistatic");
-
-  // Set the rotate entry
-  m_rotate.Set(m_params.rotate);
-
-  // Set the current mechanism in NT.
-  m_mechanism.Set(m_settings.mechanism.name);
-  // Set Overflow to False
-  m_overflowPub.Set(false);
-  // Set Mechanism Error to False
-  m_mechErrorPub.Set(false);
-  m_inst.Flush();
-
-  // Display the warning message.
-  for (auto&& func : m_callbacks) {
-    func(
-        "Please enable the robot in autonomous mode, and then "
-        "disable it "
-        "before it runs out of space. \n Note: The robot will "
-        "continue "
-        "to move until you disable it - It is your "
-        "responsibility to "
-        "ensure it does not hit anything!");
-  }
-
-  WPI_INFO(m_logger, "Started {} test.", m_tests.back());
-}
-
-void TelemetryManager::EndTest() {
-  // If there is no test running, this is a no-op
-  if (!m_isRunningTest) {
-    return;
-  }
-
-  // Disable the running flag and store the data in the JSON.
-  m_isRunningTest = false;
-  m_data[m_tests.back()] = m_params.data;
-
-  // Call the cancellation callbacks.
-  for (auto&& func : m_callbacks) {
-    std::string msg;
-    if (m_params.mechError) {
-      msg +=
-          "\nERROR: The robot indicated that you are using the wrong project "
-          "for characterizing your mechanism. \nThis most likely means you "
-          "are trying to characterize a mechanism like a Drivetrain with a "
-          "deployed config for a General Mechanism (e.g. Arm, Flywheel, and "
-          "Elevator) or vice versa. Please double check your settings and "
-          "try again.";
-    } else if (!m_params.data.empty()) {
-      std::string units = m_settings.units;
-      std::transform(m_settings.units.begin(), m_settings.units.end(),
-                     units.begin(), ::tolower);
-
-      if (std::string_view{m_settings.mechanism.name}.starts_with(
-              "Drivetrain")) {
-        double p = (m_params.data.back()[3] - m_params.data.front()[3]) *
-                   m_settings.unitsPerRotation;
-        double s = (m_params.data.back()[4] - m_params.data.front()[4]) *
-                   m_settings.unitsPerRotation;
-        double g = m_params.data.back()[7] - m_params.data.front()[7];
-
-        msg = fmt::format(
-            "The left and right encoders traveled {} {} and {} {} "
-            "respectively.\nThe gyro angle delta was {} degrees.",
-            p, units, s, units, g * 180.0 / std::numbers::pi);
-      } else {
-        double p = (m_params.data.back()[2] - m_params.data.front()[2]) *
-                   m_settings.unitsPerRotation;
-        msg = fmt::format("The encoder reported traveling {} {}.", p, units);
-      }
-
-      if (m_params.overflow) {
-        msg +=
-            "\nNOTE: the robot stopped recording data early because the entry "
-            "storage was exceeded.";
-      }
-    } else {
-      msg = "No data was detected.";
-    }
-    func(msg);
-  }
-
-  // Remove previously run test from list of tests if no data was detected.
-  if (m_params.data.empty()) {
-    m_tests.pop_back();
-  }
-
-  // Send a zero command over NT.
-  m_voltageCommand.Set(0.0);
-  m_inst.Flush();
-}
-
-void TelemetryManager::Update() {
-  // If there is no test running, these is nothing to update.
-  if (!m_isRunningTest) {
-    return;
-  }
-
-  // Update the NT entries that we're reading.
-
-  int currAckNumber = m_ackNumberSub.Get();
-  std::string telemetryValue;
-
-  // Get the FMS Control Word.
-  for (auto tsValue : m_fmsControlData.ReadQueue()) {
-    uint32_t ctrl = tsValue.value;
-    m_params.enabled = ctrl & 0x01;
-  }
-
-  // Get the string in the data field.
-  for (auto tsValue : m_telemetry.ReadQueue()) {
-    telemetryValue = tsValue.value;
-  }
-
-  // Get the overflow flag
-  for (auto tsValue : m_overflowSub.ReadQueue()) {
-    m_params.overflow = tsValue.value;
-  }
-
-  // Get the mechanism error flag
-  for (auto tsValue : m_mechErrorSub.ReadQueue()) {
-    m_params.mechError = tsValue.value;
-  }
-
-  // Go through our state machine.
-  if (m_params.state == State::WaitingForEnable) {
-    if (m_params.enabled) {
-      m_params.enableStart = wpi::Now() * 1E-6;
-      m_params.state = State::RunningTest;
-      m_ackNumber = currAckNumber;
-      WPI_INFO(m_logger, "{}", "Transitioned to running test state.");
-    }
-  }
-
-  if (m_params.state == State::RunningTest) {
-    // If for some reason we've disconnected, end the test.
-    if (!m_inst.IsConnected()) {
-      WPI_WARNING(m_logger, "{}",
-                  "NT connection was dropped when executing the test. The test "
-                  "has been canceled.");
-      EndTest();
-    }
-
-    // If the robot has disabled, then we can move on to the next step.
-    if (!m_params.enabled) {
-      m_params.disableStart = wpi::Now() * 1E-6;
-      m_params.state = State::WaitingForData;
-      WPI_INFO(m_logger, "{}", "Transitioned to waiting for data.");
-    }
-  }
-
-  if (m_params.state == State::WaitingForData) {
-    double now = wpi::Now() * 1E-6;
-    m_voltageCommand.Set(0.0);
-    m_inst.Flush();
-
-    // Process valid data
-    if (!telemetryValue.empty() && m_ackNumber < currAckNumber) {
-      m_params.raw = std::move(telemetryValue);
-      m_ackNumber = currAckNumber;
-    }
-
-    // We have the data that we need, so we can parse it and end the test.
-    if (!m_params.raw.empty() &&
-        wpi::starts_with(m_params.raw, m_tests.back())) {
-      // Remove test type from start of string
-      m_params.raw.erase(0, m_params.raw.find(';') + 1);
-
-      // Clean up the string -- remove spaces if there are any.
-      m_params.raw.erase(
-          std::remove_if(m_params.raw.begin(), m_params.raw.end(), ::isspace),
-          m_params.raw.end());
-
-      // Split the string into individual components.
-      wpi::SmallVector<std::string_view, 16> res;
-      wpi::split(m_params.raw, res, ',');
-
-      // Convert each string to double.
-      std::vector<double> values;
-      values.reserve(res.size());
-      for (auto&& str : res) {
-        values.push_back(wpi::parse_float<double>(str).value());
-      }
-
-      // Add the values to our result vector.
-      for (size_t i = 0; i < values.size() - m_settings.mechanism.rawDataSize;
-           i += m_settings.mechanism.rawDataSize) {
-        std::vector<double> d(m_settings.mechanism.rawDataSize);
-
-        std::copy_n(std::make_move_iterator(values.begin() + i),
-                    m_settings.mechanism.rawDataSize, d.begin());
-        m_params.data.push_back(std::move(d));
-      }
-
-      WPI_INFO(m_logger,
-               "Received data with size: {} for the {} test in {} seconds.",
-               m_params.data.size(), m_tests.back(),
-               m_params.data.back()[0] - m_params.data.front()[0]);
-      m_ackNumberPub.Set(++m_ackNumber);
-      EndTest();
-    }
-
-    // If we timed out, end the test and let the user know.
-    if (now - m_params.disableStart > 5.0) {
-      WPI_WARNING(m_logger, "{}",
-                  "TelemetryManager did not receieve data 5 seconds after "
-                  "completing the test...");
-      EndTest();
-    }
-  }
-}
-
-std::string TelemetryManager::SaveJSON(std::string_view location) {
-  m_data["test"] = m_settings.mechanism.name;
-  m_data["units"] = m_settings.units;
-  m_data["unitsPerRotation"] = m_settings.unitsPerRotation;
-  m_data["sysid"] = true;
-
-  std::string loc = fmt::format("{}/sysid_data{:%Y%m%d-%H%M%S}.json", location,
-                                std::chrono::system_clock::now());
-
-  sysid::SaveFile(m_data.dump(2), std::filesystem::path{loc});
-  WPI_INFO(m_logger, "Wrote JSON to: {}", loc);
-
-  return loc;
-}
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/view/Analyzer.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/view/Analyzer.cpp
index 3270918..f121ab5 100644
--- a/third_party/allwpilib/sysid/src/main/native/cpp/view/Analyzer.cpp
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/view/Analyzer.cpp
@@ -28,7 +28,7 @@
 using namespace sysid;
 
 Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger)
-    : m_location(""), m_logger(logger) {
+    : m_logger(logger) {
   // Fill the StringMap with preset values.
   m_presets["Default"] = presets::kDefault;
   m_presets["WPILib (2020-)"] = presets::kWPILibNew;
@@ -48,16 +48,14 @@
 void Analyzer::UpdateFeedforwardGains() {
   WPI_INFO(m_logger, "{}", "Gain calc");
   try {
-    const auto& [ff, trackWidth] = m_manager->CalculateFeedforward();
-    m_ff = std::get<0>(ff);
-    m_accelRSquared = std::get<1>(ff);
-    m_accelRMSE = std::get<2>(ff);
-    m_trackWidth = trackWidth;
+    const auto& [ff] = m_manager->CalculateFeedforward();
+    m_ff = ff.coeffs;
+    m_accelRSquared = ff.rSquared;
+    m_accelRMSE = ff.rmse;
     m_settings.preset.measurementDelay =
         m_settings.type == FeedbackControllerLoopType::kPosition
             ? m_manager->GetPositionDelay()
             : m_manager->GetVelocityDelay();
-    m_conversionFactor = m_manager->GetFactor();
     PrepareGraphs();
   } catch (const sysid::InvalidDataError& e) {
     m_state = AnalyzerState::kGeneralDataError;
@@ -81,6 +79,7 @@
 }
 
 void Analyzer::UpdateFeedbackGains() {
+  WPI_INFO(m_logger, "{}", "Updating feedback gains");
   if (m_ff[1] > 0 && m_ff[2] > 0) {
     const auto& fb = m_manager->CalculateFeedback(m_ff);
     m_timescale = units::second_t{m_ff[2] / m_ff[1]};
@@ -119,27 +118,9 @@
          m_state == AnalyzerState::kGeneralDataError;
 }
 
-void Analyzer::DisplayFileSelector() {
-  // Get the current width of the window. This will be used to scale
-  // our UI elements.
-  float width = ImGui::GetContentRegionAvail().x;
-
-  // Show the file location along with an option to choose.
-  if (ImGui::Button("Select")) {
-    m_selector = std::make_unique<pfd::open_file>(
-        "Select Data", "",
-        std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
-  }
-  ImGui::SameLine();
-  ImGui::SetNextItemWidth(width - ImGui::CalcTextSize("Select").x -
-                          ImGui::GetFontSize() * 5);
-  ImGui::InputText("##location", &m_location, ImGuiInputTextFlags_ReadOnly);
-}
-
 void Analyzer::ResetData() {
   m_plot.ResetData();
   m_manager = std::make_unique<AnalysisManager>(m_settings, m_logger);
-  m_location = "";
   m_ff = std::vector<double>{1, 1, 1};
   UpdateFeedbackGains();
 }
@@ -152,38 +133,15 @@
   ImGui::SameLine(width - ImGui::CalcTextSize("Reset").x);
   if (ImGui::Button("Reset")) {
     ResetData();
-    m_state = AnalyzerState::kWaitingForJSON;
+    m_state = AnalyzerState::kWaitingForData;
     return true;
   }
 
-  if (type == analysis::kDrivetrain) {
-    ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
-    if (ImGui::Combo("Dataset", &m_dataset, kDatasets, 3)) {
-      m_settings.dataset =
-          static_cast<AnalysisManager::Settings::DrivetrainDataset>(m_dataset);
-      PrepareData();
-    }
-    ImGui::SameLine();
-  } else {
-    m_settings.dataset =
-        AnalysisManager::Settings::DrivetrainDataset::kCombined;
-  }
-
   ImGui::Spacing();
   ImGui::Text(
       "Units:              %s\n"
-      "Units Per Rotation: %.4f\n"
       "Type:               %s",
-      std::string(unit).c_str(), m_conversionFactor, type.name);
-
-  if (type == analysis::kDrivetrainAngular) {
-    ImGui::SameLine();
-    sysid::CreateTooltip(
-        "Here, the units and units per rotation represent what the wheel "
-        "positions and velocities were captured in. The track width value "
-        "will reflect the unit selected here. However, the Kv and Ka will "
-        "always be in Vs/rad and Vs^2 / rad respectively.");
-  }
+      std::string(unit).c_str(), type.name);
 
   if (ImGui::Button("Override Units")) {
     ImGui::OpenPopup("Override Units");
@@ -197,24 +155,11 @@
                  IM_ARRAYSIZE(kUnits));
     unit = kUnits[m_selectedOverrideUnit];
 
-    if (unit == "Degrees") {
-      m_conversionFactor = 360.0;
-    } else if (unit == "Radians") {
-      m_conversionFactor = 2 * std::numbers::pi;
-    } else if (unit == "Rotations") {
-      m_conversionFactor = 1.0;
-    }
-
-    bool isRotational = m_selectedOverrideUnit > 2;
-
     ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
-    ImGui::InputDouble(
-        "Units Per Rotation", &m_conversionFactor, 0.0, 0.0, "%.4f",
-        isRotational ? ImGuiInputTextFlags_ReadOnly : ImGuiInputTextFlags_None);
 
     if (ImGui::Button("Close")) {
       ImGui::CloseCurrentPopup();
-      m_manager->OverrideUnits(unit, m_conversionFactor);
+      m_manager->OverrideUnits(unit);
       PrepareData();
     }
 
@@ -234,22 +179,21 @@
   WPI_INFO(m_logger, "{}", "Configuring Params");
   m_stepTestDuration = m_settings.stepTestDuration.to<float>();
 
-  // Estimate qp as 1/8 * units-per-rot
-  m_settings.lqr.qp = 0.125 * m_manager->GetFactor();
+  // Estimate qp as 1/10 native distance unit
+  m_settings.lqr.qp = 0.1;
   // Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV
   m_settings.lqr.qv = 0.25 * (12.0 - m_ff[0]) / m_ff[1];
 }
 
 void Analyzer::Display() {
-  DisplayFileSelector();
   DisplayGraphs();
 
   switch (m_state) {
-    case AnalyzerState::kWaitingForJSON: {
+    case AnalyzerState::kWaitingForData: {
       ImGui::Text(
           "SysId is currently in theoretical analysis mode.\n"
           "To analyze recorded test data, select a "
-          "data JSON.");
+          "data file (.wpilog).");
       sysid::CreateTooltip(
           "Theoretical feedback gains can be calculated from a "
           "physical model of the mechanism being controlled. "
@@ -295,7 +239,7 @@
     case AnalyzerState::kFileError: {
       CreateErrorPopup(m_errorPopup, m_exception);
       if (!m_errorPopup) {
-        m_state = AnalyzerState::kWaitingForJSON;
+        m_state = AnalyzerState::kWaitingForData;
         return;
       }
       break;
@@ -313,20 +257,10 @@
       break;
     }
   }
-
-  // Periodic functions
-  try {
-    SelectFile();
-  } catch (const AnalysisManager::FileReadingError& e) {
-    m_state = AnalyzerState::kFileError;
-    HandleError(e.what());
-  } catch (const wpi::json::exception& e) {
-    m_state = AnalyzerState::kFileError;
-    HandleError(e.what());
-  }
 }
 
 void Analyzer::PrepareData() {
+  WPI_INFO(m_logger, "{}", "Preparing data");
   try {
     m_manager->PrepareData();
     UpdateFeedforwardGains();
@@ -379,9 +313,6 @@
 void Analyzer::HandleError(std::string_view msg) {
   m_exception = msg;
   m_errorPopup = true;
-  if (m_state == AnalyzerState::kFileError) {
-    m_location = "";
-  }
   PrepareRawGraphs();
 }
 
@@ -458,23 +389,12 @@
   ImGui::End();
 }
 
-void Analyzer::SelectFile() {
-  // If the selector exists and is ready with a result, we can store it.
-  if (m_selector && m_selector->ready() && !m_selector->result().empty()) {
-    // Store the location of the file and reset the selector.
-    WPI_INFO(m_logger, "Opening File: {}", m_selector->result()[0]);
-    m_location = m_selector->result()[0];
-    m_selector.reset();
-    WPI_INFO(m_logger, "{}", "Opened File");
-    m_manager =
-        std::make_unique<AnalysisManager>(m_location, m_settings, m_logger);
-    PrepareData();
-    m_dataset = 0;
-    m_settings.dataset =
-        AnalysisManager::Settings::DrivetrainDataset::kCombined;
-    ConfigParamsOnFileSelect();
-    UpdateFeedbackGains();
-  }
+void Analyzer::AnalyzeData() {
+  m_manager = std::make_unique<AnalysisManager>(m_data, m_settings, m_logger);
+  PrepareData();
+  m_dataset = 0;
+  ConfigParamsOnFileSelect();
+  UpdateFeedbackGains();
 }
 
 void Analyzer::AbortDataPrep() {
@@ -625,8 +545,6 @@
         "This is the angle offset which, when added to the angle measurement, "
         "zeroes it out when the arm is horizontal. This is needed for the arm "
         "feedforward to work.");
-  } else if (m_trackWidth) {
-    DisplayGain("Track Width", &*m_trackWidth);
   }
   double endY = ImGui::GetCursorPosY();
 
@@ -790,7 +708,7 @@
                    IM_ARRAYSIZE(kLoopTypes))) {
     m_settings.type =
         static_cast<FeedbackControllerLoopType>(m_selectedLoopType);
-    if (m_state == AnalyzerState::kWaitingForJSON) {
+    if (m_state == AnalyzerState::kWaitingForData) {
       m_settings.preset.measurementDelay = 0_ms;
     } else {
       if (m_settings.type == FeedbackControllerLoopType::kPosition) {
@@ -817,7 +735,7 @@
 
   if (m_selectedLoopType == 0) {
     std::string unit;
-    if (m_state != AnalyzerState::kWaitingForJSON) {
+    if (m_state != AnalyzerState::kWaitingForData) {
       unit = fmt::format(" ({})", GetAbbreviation(m_manager->GetUnit()));
     }
 
@@ -831,7 +749,7 @@
   }
 
   std::string unit;
-  if (m_state != AnalyzerState::kWaitingForJSON) {
+  if (m_state != AnalyzerState::kWaitingForData) {
     unit = fmt::format(" ({}/s)", GetAbbreviation(m_manager->GetUnit()));
   }
 
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/view/DataSelector.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/view/DataSelector.cpp
new file mode 100644
index 0000000..c160490
--- /dev/null
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/view/DataSelector.cpp
@@ -0,0 +1,244 @@
+// 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 "sysid/view/DataSelector.h"
+
+#include <fmt/format.h>
+#include <glass/support/DataLogReaderThread.h>
+#include <imgui.h>
+#include <wpi/DataLogReader.h>
+#include <wpi/Logger.h>
+#include <wpi/StringExtras.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/Storage.h"
+
+using namespace sysid;
+
+static constexpr const char* kAnalysisTypes[] = {"Elevator", "Arm", "Simple"};
+
+static bool EmitEntryTarget(const char* name, bool isString,
+                            const glass::DataLogReaderEntry** entry) {
+  if (*entry) {
+    auto text =
+        fmt::format("{}: {} ({})", name, (*entry)->name, (*entry)->type);
+    ImGui::TextUnformatted(text.c_str());
+  } else {
+    ImGui::Text("%s: <none (DROP HERE)> (%s)", name,
+                isString ? "string" : "number");
+  }
+  bool rv = false;
+  if (ImGui::BeginDragDropTarget()) {
+    if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload(
+            isString ? "DataLogEntryString" : "DataLogEntry")) {
+      assert(payload->DataSize == sizeof(const glass::DataLogReaderEntry*));
+      *entry = *static_cast<const glass::DataLogReaderEntry**>(payload->Data);
+      rv = true;
+    }
+    ImGui::EndDragDropTarget();
+  }
+  return rv;
+}
+
+void DataSelector::Display() {
+  using namespace std::chrono_literals;
+
+  // building test data is modal (due to async access)
+  if (m_testdataFuture.valid()) {
+    if (m_testdataFuture.wait_for(0s) == std::future_status::ready) {
+      TestData data = m_testdataFuture.get();
+      for (auto&& motordata : data.motorData) {
+        m_testdataStats.emplace_back(
+            fmt::format("Test State: {}", motordata.first()));
+        int i = 0;
+        for (auto&& run : motordata.second.runs) {
+          m_testdataStats.emplace_back(fmt::format(
+              "  Run {} samples: {} Volt {} Pos {} Vel", ++i,
+              run.voltage.size(), run.position.size(), run.velocity.size()));
+        }
+      }
+      if (testdata) {
+        testdata(std::move(data));
+      }
+    }
+    ImGui::Text("Loading data...");
+    return;
+  }
+
+  if (!m_testdataStats.empty()) {
+    for (auto&& line : m_testdataStats) {
+      ImGui::TextUnformatted(line.c_str());
+    }
+    if (ImGui::Button("Ok")) {
+      m_testdataStats.clear();
+    }
+    return;
+  }
+
+  if (EmitEntryTarget("Test State", true, &m_testStateEntry)) {
+    m_testsFuture =
+        std::async(std::launch::async, [testStateEntry = m_testStateEntry] {
+          return LoadTests(*testStateEntry);
+        });
+  }
+
+  if (!m_testStateEntry) {
+    return;
+  }
+
+  if (m_testsFuture.valid() &&
+      m_testsFuture.wait_for(0s) == std::future_status::ready) {
+    m_tests = m_testsFuture.get();
+  }
+
+  if (m_tests.empty()) {
+    if (m_testsFuture.valid()) {
+      ImGui::TextUnformatted("Reading tests...");
+    } else {
+      ImGui::TextUnformatted("No tests found");
+    }
+    return;
+  }
+
+#if 0
+  // Test filtering
+  if (ImGui::BeginCombo("Test", m_selectedTest.c_str())) {
+    for (auto&& test : m_tests) {
+      if (ImGui::Selectable(test.first.c_str(), test.first == m_selectedTest)) {
+        m_selectedTest = test.first;
+      }
+    }
+    ImGui::EndCombo();
+  }
+#endif
+
+  ImGui::Combo("Analysis Type", &m_selectedAnalysis, kAnalysisTypes,
+               IM_ARRAYSIZE(kAnalysisTypes));
+
+  // DND targets
+  EmitEntryTarget("Velocity", false, &m_velocityEntry);
+  EmitEntryTarget("Position", false, &m_positionEntry);
+  EmitEntryTarget("Voltage", false, &m_voltageEntry);
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+  ImGui::Combo("Units", &m_selectedUnit, kUnits, IM_ARRAYSIZE(kUnits));
+
+  ImGui::InputDouble("Velocity scaling", &m_velocityScale);
+  ImGui::InputDouble("Position scaling", &m_positionScale);
+
+  if (/*!m_selectedTest.empty() &&*/ m_velocityEntry && m_positionEntry &&
+      m_voltageEntry) {
+    if (ImGui::Button("Load")) {
+      m_testdataFuture =
+          std::async(std::launch::async, [this] { return BuildTestData(); });
+    }
+  }
+}
+
+void DataSelector::Reset() {
+  m_testsFuture = {};
+  m_tests.clear();
+  m_selectedTest.clear();
+  m_testStateEntry = nullptr;
+  m_velocityEntry = nullptr;
+  m_positionEntry = nullptr;
+  m_voltageEntry = nullptr;
+  m_testdataFuture = {};
+}
+
+DataSelector::Tests DataSelector::LoadTests(
+    const glass::DataLogReaderEntry& testStateEntry) {
+  Tests tests;
+  for (auto&& range : testStateEntry.ranges) {
+    std::string_view prevState;
+    Runs* curRuns = nullptr;
+    wpi::log::DataLogReader::iterator lastStart = range.begin();
+    for (auto it = range.begin(), end = range.end(); it != end; ++it) {
+      std::string_view testState;
+      if (it->GetEntry() != testStateEntry.entry ||
+          !it->GetString(&testState)) {
+        continue;
+      }
+
+      // track runs as iterator ranges of the same test
+      if (testState != prevState) {
+        if (curRuns) {
+          curRuns->emplace_back(lastStart, it);
+        }
+        lastStart = it;
+      }
+      prevState = testState;
+
+      if (testState == "none") {
+        curRuns = nullptr;
+        continue;
+      }
+
+      auto [testName, direction] = wpi::rsplit(testState, '-');
+      auto testIt = tests.find(testName);
+      if (testIt == tests.end()) {
+        testIt = tests.emplace(std::string{testName}, State{}).first;
+      }
+      auto stateIt = testIt->second.find(testState);
+      if (stateIt == testIt->second.end()) {
+        stateIt = testIt->second.emplace(std::string{testState}, Runs{}).first;
+      }
+      curRuns = &stateIt->second;
+    }
+
+    if (curRuns) {
+      curRuns->emplace_back(lastStart, range.end());
+    }
+  }
+  return tests;
+}
+
+template <typename T>
+static void AddSample(std::vector<MotorData::Run::Sample<T>>& samples,
+                      const wpi::log::DataLogRecord& record, bool isDouble,
+                      double scale) {
+  if (isDouble) {
+    double val;
+    if (record.GetDouble(&val)) {
+      samples.emplace_back(units::second_t{record.GetTimestamp() * 1.0e-6},
+                           T{val * scale});
+    }
+  } else {
+    float val;
+    if (record.GetFloat(&val)) {
+      samples.emplace_back(units::second_t{record.GetTimestamp() * 1.0e-6},
+                           T{static_cast<double>(val * scale)});
+    }
+  }
+}
+
+TestData DataSelector::BuildTestData() {
+  TestData data;
+  data.distanceUnit = kUnits[m_selectedUnit];
+  data.mechanismType = analysis::FromName(kAnalysisTypes[m_selectedAnalysis]);
+  bool voltageDouble = m_voltageEntry->type == "double";
+  bool positionDouble = m_positionEntry->type == "double";
+  bool velocityDouble = m_velocityEntry->type == "double";
+
+  for (auto&& test : m_tests) {
+    for (auto&& state : test.second) {
+      auto& motorData = data.motorData[state.first];
+      for (auto&& range : state.second) {
+        auto& run = motorData.runs.emplace_back();
+        for (auto&& record : range) {
+          if (record.GetEntry() == m_voltageEntry->entry) {
+            AddSample(run.voltage, record, voltageDouble, 1.0);
+          } else if (record.GetEntry() == m_positionEntry->entry) {
+            AddSample(run.position, record, positionDouble, m_positionScale);
+          } else if (record.GetEntry() == m_velocityEntry->entry) {
+            AddSample(run.velocity, record, velocityDouble, m_velocityScale);
+          }
+        }
+      }
+    }
+  }
+
+  return data;
+}
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/view/JSONConverter.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/view/JSONConverter.cpp
deleted file mode 100644
index 88eaa6a..0000000
--- a/third_party/allwpilib/sysid/src/main/native/cpp/view/JSONConverter.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-// 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 "sysid/analysis/JSONConverter.h"
-#include "sysid/view/JSONConverter.h"
-
-#include <exception>
-
-#include <imgui.h>
-#include <portable-file-dialogs.h>
-#include <wpi/timestamp.h>
-
-#include "sysid/Util.h"
-
-using namespace sysid;
-
-void JSONConverter::DisplayConverter(
-    const char* tooltip,
-    std::function<std::string(std::string_view, wpi::Logger&)> converter) {
-  if (ImGui::Button(tooltip)) {
-    m_opener = std::make_unique<pfd::open_file>(
-        tooltip, "", std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
-  }
-
-  if (m_opener && m_opener->ready()) {
-    if (!m_opener->result().empty()) {
-      m_location = m_opener->result()[0];
-      try {
-        converter(m_location, m_logger);
-        m_timestamp = wpi::Now() * 1E-6;
-      } catch (const std::exception& e) {
-        ImGui::OpenPopup("Exception Caught!");
-        m_exception = e.what();
-      }
-    }
-    m_opener.reset();
-  }
-
-  if (wpi::Now() * 1E-6 - m_timestamp < 5) {
-    ImGui::SameLine();
-    ImGui::Text("Saved!");
-  }
-
-  // Handle exceptions.
-  ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
-  if (ImGui::BeginPopupModal("Exception Caught!")) {
-    ImGui::PushTextWrapPos(0.0f);
-    ImGui::Text(
-        "An error occurred when parsing the JSON. This most likely means that "
-        "the JSON data is incorrectly formatted.");
-    ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s",
-                       m_exception.c_str());
-    ImGui::PopTextWrapPos();
-    if (ImGui::Button("Close")) {
-      ImGui::CloseCurrentPopup();
-    }
-    ImGui::EndPopup();
-  }
-}
-
-void JSONConverter::DisplayCSVConvert() {
-  DisplayConverter("Select SysId JSON", sysid::ToCSV);
-}
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/view/LogLoader.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/view/LogLoader.cpp
new file mode 100644
index 0000000..fdaa3af
--- /dev/null
+++ b/third_party/allwpilib/sysid/src/main/native/cpp/view/LogLoader.cpp
@@ -0,0 +1,208 @@
+// 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 "sysid/view/LogLoader.h"
+
+#include <algorithm>
+#include <memory>
+#include <span>
+#include <string_view>
+
+#include <glass/support/DataLogReaderThread.h>
+#include <imgui.h>
+#include <imgui_stdlib.h>
+#include <portable-file-dialogs.h>
+#include <wpi/SpanExtras.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fs.h>
+
+using namespace sysid;
+
+LogLoader::LogLoader(glass::Storage& storage, wpi::Logger& logger) {}
+
+LogLoader::~LogLoader() = default;
+
+void LogLoader::Display() {
+  if (ImGui::Button("Open data log file...")) {
+    m_opener = std::make_unique<pfd::open_file>(
+        "Select Data Log", "",
+        std::vector<std::string>{"DataLog Files", "*.wpilog"});
+  }
+
+  // Handle opening the file
+  if (m_opener && m_opener->ready(0)) {
+    if (!m_opener->result().empty()) {
+      m_filename = m_opener->result()[0];
+
+      std::error_code ec;
+      auto buf = wpi::MemoryBuffer::GetFile(m_filename, ec);
+      if (ec) {
+        ImGui::OpenPopup("Error");
+        m_error = fmt::format("Could not open file: {}", ec.message());
+        return;
+      }
+
+      wpi::log::DataLogReader reader{std::move(buf)};
+      if (!reader.IsValid()) {
+        ImGui::OpenPopup("Error");
+        m_error = "Not a valid datalog file";
+        return;
+      }
+      unload();
+      m_reader =
+          std::make_unique<glass::DataLogReaderThread>(std::move(reader));
+      m_entryTree.clear();
+    }
+    m_opener.reset();
+  }
+
+  // Handle errors
+  ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
+  if (ImGui::BeginPopupModal("Error")) {
+    ImGui::PushTextWrapPos(0.0f);
+    ImGui::TextUnformatted(m_error.c_str());
+    ImGui::PopTextWrapPos();
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  if (!m_reader) {
+    return;
+  }
+
+  // Summary info
+  ImGui::TextUnformatted(fs::path{m_filename}.stem().string().c_str());
+  ImGui::Text("%u records, %u entries%s", m_reader->GetNumRecords(),
+              m_reader->GetNumEntries(),
+              m_reader->IsDone() ? "" : " (working)");
+
+  if (!m_reader->IsDone()) {
+    return;
+  }
+
+  bool refilter = ImGui::InputText("Filter", &m_filter);
+
+  // Display tree of entries
+  if (m_entryTree.empty() || refilter) {
+    RebuildEntryTree();
+  }
+
+  ImGui::BeginTable(
+      "Entries", 2,
+      ImGuiTableFlags_Borders | ImGuiTableFlags_SizingStretchProp);
+  ImGui::TableSetupColumn("Name");
+  ImGui::TableSetupColumn("Type");
+  // ImGui::TableSetupColumn("Metadata");
+  ImGui::TableHeadersRow();
+  DisplayEntryTree(m_entryTree);
+  ImGui::EndTable();
+}
+
+void LogLoader::RebuildEntryTree() {
+  m_entryTree.clear();
+  wpi::SmallVector<std::string_view, 16> parts;
+  m_reader->ForEachEntryName([&](const glass::DataLogReaderEntry& entry) {
+    // only show double/float/string entries (TODO: support struct/protobuf)
+    if (entry.type != "double" && entry.type != "float" &&
+        entry.type != "string") {
+      return;
+    }
+
+    // filter on name
+    if (!m_filter.empty() && !wpi::contains_lower(entry.name, m_filter)) {
+      return;
+    }
+
+    parts.clear();
+    // split on first : if one is present
+    auto [prefix, mainpart] = wpi::split(entry.name, ':');
+    if (mainpart.empty() || wpi::contains(prefix, '/')) {
+      mainpart = entry.name;
+    } else {
+      parts.emplace_back(prefix);
+    }
+    wpi::split(mainpart, parts, '/', -1, false);
+
+    // ignore a raw "/" key
+    if (parts.empty()) {
+      return;
+    }
+
+    // get to leaf
+    auto nodes = &m_entryTree;
+    for (auto part : wpi::drop_back(std::span{parts.begin(), parts.end()})) {
+      auto it =
+          std::find_if(nodes->begin(), nodes->end(),
+                       [&](const auto& node) { return node.name == part; });
+      if (it == nodes->end()) {
+        nodes->emplace_back(part);
+        // path is from the beginning of the string to the end of the current
+        // part; this works because part is a reference to the internals of
+        // entry.name
+        nodes->back().path.assign(
+            entry.name.data(), part.data() + part.size() - entry.name.data());
+        it = nodes->end() - 1;
+      }
+      nodes = &it->children;
+    }
+
+    auto it = std::find_if(nodes->begin(), nodes->end(), [&](const auto& node) {
+      return node.name == parts.back();
+    });
+    if (it == nodes->end()) {
+      nodes->emplace_back(parts.back());
+      // no need to set path, as it's identical to entry.name
+      it = nodes->end() - 1;
+    }
+    it->entry = &entry;
+  });
+}
+
+static void EmitEntry(const std::string& name,
+                      const glass::DataLogReaderEntry& entry) {
+  ImGui::TableNextColumn();
+  ImGui::Selectable(name.c_str());
+  if (ImGui::BeginDragDropSource()) {
+    auto entryPtr = &entry;
+    ImGui::SetDragDropPayload(
+        entry.type == "string" ? "DataLogEntryString" : "DataLogEntry",
+        &entryPtr,
+        sizeof(entryPtr));  // NOLINT
+    ImGui::TextUnformatted(entry.name.data(),
+                           entry.name.data() + entry.name.size());
+    ImGui::EndDragDropSource();
+  }
+  ImGui::TableNextColumn();
+  ImGui::TextUnformatted(entry.type.data(),
+                         entry.type.data() + entry.type.size());
+#if 0
+  ImGui::TableNextColumn();
+  ImGui::TextUnformatted(entry.metadata.data(),
+                         entry.metadata.data() + entry.metadata.size());
+#endif
+}
+
+void LogLoader::DisplayEntryTree(const std::vector<EntryTreeNode>& tree) {
+  for (auto&& node : tree) {
+    if (node.entry) {
+      EmitEntry(node.name, *node.entry);
+    }
+
+    if (!node.children.empty()) {
+      ImGui::TableNextColumn();
+      bool open = ImGui::TreeNodeEx(node.name.c_str(),
+                                    ImGuiTreeNodeFlags_SpanFullWidth);
+      ImGui::TableNextColumn();
+#if 0
+      ImGui::TableNextColumn();
+#endif
+      if (open) {
+        DisplayEntryTree(node.children);
+        ImGui::TreePop();
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/sysid/src/main/native/cpp/view/Logger.cpp b/third_party/allwpilib/sysid/src/main/native/cpp/view/Logger.cpp
deleted file mode 100644
index 5e7773d..0000000
--- a/third_party/allwpilib/sysid/src/main/native/cpp/view/Logger.cpp
+++ /dev/null
@@ -1,222 +0,0 @@
-// 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 "sysid/view/Logger.h"
-
-#include <exception>
-#include <numbers>
-
-#include <glass/Context.h>
-#include <glass/Storage.h>
-#include <imgui.h>
-#include <imgui_internal.h>
-#include <imgui_stdlib.h>
-#include <networktables/NetworkTable.h>
-#include <units/angle.h>
-#include <wpigui.h>
-
-#include "sysid/Util.h"
-#include "sysid/analysis/AnalysisType.h"
-#include "sysid/view/UILayout.h"
-
-using namespace sysid;
-
-Logger::Logger(glass::Storage& storage, wpi::Logger& logger)
-    : m_logger{logger}, m_ntSettings{"sysid", storage} {
-  wpi::gui::AddEarlyExecute([&] { m_ntSettings.Update(); });
-
-  m_ntSettings.EnableServerOption(false);
-}
-
-void Logger::Display() {
-  // Get the current width of the window. This will be used to scale
-  // our UI elements.
-  float width = ImGui::GetContentRegionAvail().x;
-
-  // Add team number input and apply button for NT connection.
-  m_ntSettings.Display();
-
-  // Reset and clear the internal manager state.
-  ImGui::SameLine();
-  if (ImGui::Button("Reset Telemetry")) {
-    m_settings = TelemetryManager::Settings{};
-    m_manager = std::make_unique<TelemetryManager>(m_settings, m_logger);
-    m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
-  }
-
-  // Add NT connection indicator.
-  static ImVec4 kColorDisconnected{1.0f, 0.4f, 0.4f, 1.0f};
-  static ImVec4 kColorConnected{0.2f, 1.0f, 0.2f, 1.0f};
-  ImGui::SameLine();
-  bool ntConnected = nt::NetworkTableInstance::GetDefault().IsConnected();
-  ImGui::TextColored(ntConnected ? kColorConnected : kColorDisconnected,
-                     ntConnected ? "NT Connected" : "NT Disconnected");
-
-  // Create a Section for project configuration
-  ImGui::Separator();
-  ImGui::Spacing();
-  ImGui::Text("Project Parameters");
-
-  // Add a dropdown for mechanism type.
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
-
-  if (ImGui::Combo("Mechanism", &m_selectedType, kTypes,
-                   IM_ARRAYSIZE(kTypes))) {
-    m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
-  }
-
-  // Add Dropdown for Units
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
-  if (ImGui::Combo("Unit Type", &m_selectedUnit, kUnits,
-                   IM_ARRAYSIZE(kUnits))) {
-    m_settings.units = kUnits[m_selectedUnit];
-  }
-
-  sysid::CreateTooltip(
-      "This is the type of units that your gains will be in. For example, if "
-      "you want your flywheel gains in terms of radians, then use the radians "
-      "unit. On the other hand, if your drivetrain will use gains in meters, "
-      "choose meters.");
-
-  // Rotational units have fixed Units per rotations
-  m_isRotationalUnits =
-      (m_settings.units == "Rotations" || m_settings.units == "Degrees" ||
-       m_settings.units == "Radians");
-  if (m_settings.units == "Degrees") {
-    m_settings.unitsPerRotation = 360.0;
-  } else if (m_settings.units == "Radians") {
-    m_settings.unitsPerRotation = 2 * std::numbers::pi;
-  } else if (m_settings.units == "Rotations") {
-    m_settings.unitsPerRotation = 1.0;
-  }
-
-  // Units Per Rotations entry
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
-  ImGui::InputDouble("Units Per Rotation", &m_settings.unitsPerRotation, 0.0f,
-                     0.0f, "%.4f",
-                     m_isRotationalUnits ? ImGuiInputTextFlags_ReadOnly
-                                         : ImGuiInputTextFlags_None);
-  sysid::CreateTooltip(
-      "The logger assumes that the code will be sending recorded motor shaft "
-      "rotations over NetworkTables. This value will then be multiplied by the "
-      "units per rotation to get the measurement in the units you "
-      "specified.\n\nFor non-rotational units (e.g. meters), this value is "
-      "usually the wheel diameter times pi (should not include gearing).");
-  // Create a section for voltage parameters.
-  ImGui::Separator();
-  ImGui::Spacing();
-  ImGui::Text("Voltage Parameters");
-
-  auto CreateVoltageParameters = [this](const char* text, double* data,
-                                        float min, float max) {
-    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
-    ImGui::PushItemFlag(ImGuiItemFlags_Disabled,
-                        m_manager && m_manager->IsActive());
-    float value = static_cast<float>(*data);
-    if (ImGui::SliderFloat(text, &value, min, max, "%.2f")) {
-      *data = value;
-    }
-    ImGui::PopItemFlag();
-  };
-
-  CreateVoltageParameters("Quasistatic Ramp Rate (V/s)",
-                          &m_settings.quasistaticRampRate, 0.10f, 0.60f);
-  sysid::CreateTooltip(
-      "This is the rate at which the voltage will increase during the "
-      "quasistatic test.");
-
-  CreateVoltageParameters("Dynamic Step Voltage (V)", &m_settings.stepVoltage,
-                          0.0f, 10.0f);
-  sysid::CreateTooltip(
-      "This is the voltage that will be applied for the "
-      "dynamic voltage (acceleration) tests.");
-
-  // Create a section for tests.
-  ImGui::Separator();
-  ImGui::Spacing();
-  ImGui::Text("Tests");
-
-  auto CreateTest = [this, width](const char* text, const char* itext) {
-    // Display buttons if we have an NT connection.
-    if (nt::NetworkTableInstance::GetDefault().IsConnected()) {
-      // Create button to run tests.
-      if (ImGui::Button(text)) {
-        // Open the warning message.
-        ImGui::OpenPopup("Warning");
-        m_manager->BeginTest(itext);
-        m_opened = text;
-      }
-      if (m_opened == text && ImGui::BeginPopupModal("Warning")) {
-        ImGui::TextWrapped("%s", m_popupText.c_str());
-        if (ImGui::Button(m_manager->IsActive() ? "End Test" : "Close")) {
-          m_manager->EndTest();
-          ImGui::CloseCurrentPopup();
-          m_opened = "";
-        }
-        ImGui::EndPopup();
-      }
-    } else {
-      // Show disabled text when there is no connection.
-      ImGui::TextDisabled("%s", text);
-    }
-
-    // Show whether the tests were run or not.
-    bool run = m_manager->HasRunTest(itext);
-    ImGui::SameLine(width * 0.7);
-    ImGui::Text(run ? "Run" : "Not Run");
-  };
-
-  CreateTest("Quasistatic Forward", "slow-forward");
-  CreateTest("Quasistatic Backward", "slow-backward");
-  CreateTest("Dynamic Forward", "fast-forward");
-  CreateTest("Dynamic Backward", "fast-backward");
-
-  m_manager->RegisterDisplayCallback(
-      [this](const auto& str) { m_popupText = str; });
-
-  // Display the path to where the JSON will be saved and a button to select the
-  // location.
-  ImGui::Separator();
-  ImGui::Spacing();
-  ImGui::Text("Save Location");
-  if (ImGui::Button("Choose")) {
-    m_selector = std::make_unique<pfd::select_folder>("Select Folder");
-  }
-  ImGui::SameLine();
-  ImGui::InputText("##savelocation", &m_jsonLocation,
-                   ImGuiInputTextFlags_ReadOnly);
-
-  // Add button to save.
-  ImGui::SameLine(width * 0.9);
-  if (ImGui::Button("Save")) {
-    try {
-      m_manager->SaveJSON(m_jsonLocation);
-    } catch (const std::exception& e) {
-      ImGui::OpenPopup("Exception Caught!");
-      m_exception = e.what();
-    }
-  }
-
-  // Handle exceptions.
-  if (ImGui::BeginPopupModal("Exception Caught!")) {
-    ImGui::Text("%s", m_exception.c_str());
-    if (ImGui::Button("Close")) {
-      ImGui::CloseCurrentPopup();
-    }
-    ImGui::EndPopup();
-  }
-
-  // Run periodic methods.
-  SelectDataFolder();
-  m_ntSettings.Update();
-  m_manager->Update();
-}
-
-void Logger::SelectDataFolder() {
-  // If the selector exists and is ready with a result, we can store it.
-  if (m_selector && m_selector->ready()) {
-    m_jsonLocation = m_selector->result();
-    m_selector.reset();
-  }
-}
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/Util.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/Util.h
index 38cf8b2..1b8ede9 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/Util.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/Util.h
@@ -34,7 +34,7 @@
 #define STRINGIZE(s) #s
 
 namespace sysid {
-static constexpr const char* kUnits[] = {"Meters",  "Feet",      "Inches",
+inline constexpr const char* kUnits[] = {"Meters",  "Feet",      "Inches",
                                          "Radians", "Rotations", "Degrees"};
 
 /**
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
index d572578..d2c38e1 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
@@ -17,6 +17,7 @@
 
 #include <units/time.h>
 #include <wpi/Logger.h>
+#include <wpi/StringMap.h>
 #include <wpi/json.h>
 
 #include "sysid/analysis/AnalysisType.h"
@@ -33,6 +34,9 @@
  */
 class AnalysisManager {
  public:
+  // This contains data for each test (e.g. quasistatic-forward,
+  // quasistatic-backward, etc) indexed by test name
+  TestData m_data;
   /**
    * Represents settings for an instance of the analysis manager. This contains
    * information about the feedback controller preset, loop type, motion
@@ -40,7 +44,6 @@
    * dataset.
    */
   struct Settings {
-    enum class DrivetrainDataset { kCombined = 0, kLeft = 1, kRight = 2 };
     /**
      * The feedback controller preset used to calculate gains.
      */
@@ -87,8 +90,6 @@
      * in a smart motor controller).
      */
     bool convertGainsToEncTicks = false;
-
-    DrivetrainDataset dataset = DrivetrainDataset::kCombined;
   };
 
   /**
@@ -98,18 +99,14 @@
     /**
      * Stores the Feedforward gains.
      */
-    std::tuple<std::vector<double>, double, double> ffGains;
-
-    /**
-     * Stores the trackwidth for angular drivetrain tests.
-     */
-    std::optional<double> trackWidth;
+    OLSResult ffGains;
   };
 
   /**
    * Exception for File Reading Errors.
    */
-  struct FileReadingError : public std::exception {
+  class FileReadingError : public std::exception {
+   public:
     /**
      * Creates a FileReadingError object
      *
@@ -119,18 +116,21 @@
       msg = fmt::format("Unable to read: {}", path);
     }
 
+    const char* what() const noexcept override { return msg.c_str(); }
+
+   private:
     /**
      * The path of the file that was opened.
      */
     std::string msg;
-    const char* what() const noexcept override { return msg.c_str(); }
   };
 
   /**
    * The keys (which contain sysid data) that are in the JSON to analyze.
    */
   static constexpr const char* kJsonDataKeys[] = {
-      "slow-forward", "slow-backward", "fast-forward", "fast-backward"};
+      "quasistatic-forward", "quasistatic-reverse", "dynamic-forward",
+      "dynamic-reverse"};
 
   /**
    * Concatenates a list of vectors. The contents of the source vectors are
@@ -167,12 +167,11 @@
    * Constructs an instance of the analysis manager with the given path (to the
    * JSON) and analysis manager settings.
    *
-   * @param path     The path to the JSON containing the sysid data.
+   * @param data     The data from the SysId routine.
    * @param settings The settings for this instance of the analysis manager.
    * @param logger   The logger instance to use for log data.
    */
-  AnalysisManager(std::string_view path, Settings& settings,
-                  wpi::Logger& logger);
+  AnalysisManager(TestData data, Settings& settings, wpi::Logger& logger);
 
   /**
    * Prepares data from the JSON and stores the output in Storage member
@@ -200,10 +199,8 @@
    * Overrides the units in the JSON with the user-provided ones.
    *
    * @param unit             The unit to output gains in.
-   * @param unitsPerRotation The conversion factor between rotations and the
-   *                         selected unit.
    */
-  void OverrideUnits(std::string_view unit, double unitsPerRotation);
+  void OverrideUnits(std::string_view unit);
 
   /**
    * Resets the units back to those defined in the JSON.
@@ -215,21 +212,14 @@
    *
    * @return The analysis type.
    */
-  const AnalysisType& GetAnalysisType() const { return m_type; }
+  const AnalysisType& GetAnalysisType() const { return m_data.mechanismType; }
 
   /**
    * Returns the units of analysis.
    *
    * @return The units of analysis.
    */
-  std::string_view GetUnit() const { return m_unit; }
-
-  /**
-   * Returns the factor (a.k.a. units per rotation) for analysis.
-   *
-   * @return The factor (a.k.a. units per rotation) for analysis.
-   */
-  double GetFactor() const { return m_factor; }
+  std::string_view GetUnit() const { return m_data.distanceUnit; }
 
   /**
    * Returns a reference to the iterator of the currently selected raw datset.
@@ -238,9 +228,7 @@
    *
    * @return A reference to the raw internal data.
    */
-  Storage& GetRawData() {
-    return m_rawDataset[static_cast<int>(m_settings.dataset)];
-  }
+  Storage& GetRawData() { return m_rawDataset; }
 
   /**
    * Returns a reference to the iterator of the currently selected filtered
@@ -249,18 +237,14 @@
    *
    * @return A reference to the filtered internal data.
    */
-  Storage& GetFilteredData() {
-    return m_filteredDataset[static_cast<int>(m_settings.dataset)];
-  }
+  Storage& GetFilteredData() { return m_filteredDataset; }
 
   /**
    * Returns the original dataset.
    *
    * @return The original (untouched) dataset
    */
-  Storage& GetOriginalData() {
-    return m_originalDataset[static_cast<int>(m_settings.dataset)];
-  }
+  Storage& GetOriginalData() { return m_originalDataset; }
 
   /**
    * Returns the minimum duration of the Step Voltage Test of the currently
@@ -311,22 +295,14 @@
     return m_startTimes;
   }
 
-  bool HasData() const {
-    return !m_originalDataset[static_cast<int>(
-                                  Settings::DrivetrainDataset::kCombined)]
-                .empty();
-  }
+  bool HasData() const { return !m_originalDataset.empty(); }
 
  private:
   wpi::Logger& m_logger;
 
-  // This is used to store the various datasets (i.e. Combined, Forward,
-  // Backward, etc.)
-  wpi::json m_json;
-
-  std::array<Storage, 3> m_originalDataset;
-  std::array<Storage, 3> m_rawDataset;
-  std::array<Storage, 3> m_filteredDataset;
+  Storage m_originalDataset;
+  Storage m_rawDataset;
+  Storage m_filteredDataset;
 
   // Stores the various start times of the different tests.
   std::array<units::second_t, 4> m_startTimes;
@@ -335,24 +311,11 @@
   // controller preset, LQR parameters, acceleration window size, etc.
   Settings& m_settings;
 
-  // Miscellaneous data from the JSON -- the analysis type, the units, and the
-  // units per rotation.
-  AnalysisType m_type;
-  std::string m_unit;
-  double m_factor;
-
   units::second_t m_minStepTime{0};
   units::second_t m_maxStepTime{std::numeric_limits<double>::infinity()};
   std::vector<units::second_t> m_positionDelays;
   std::vector<units::second_t> m_velocityDelays;
 
-  // Stores an optional track width if we are doing the drivetrain angular test.
-  std::optional<double> m_trackWidth;
-
   void PrepareGeneralData();
-
-  void PrepareAngularDrivetrainData();
-
-  void PrepareLinearDrivetrainData();
 };
 }  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisType.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
index 7feedb3..5a30d7c 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
@@ -52,11 +52,9 @@
 };
 
 namespace analysis {
-constexpr AnalysisType kDrivetrain{3, 9, "Drivetrain"};
-constexpr AnalysisType kDrivetrainAngular{3, 9, "Drivetrain (Angular)"};
-constexpr AnalysisType kElevator{4, 4, "Elevator"};
-constexpr AnalysisType kArm{5, 4, "Arm"};
-constexpr AnalysisType kSimple{3, 4, "Simple"};
+inline constexpr AnalysisType kElevator{4, 4, "Elevator"};
+inline constexpr AnalysisType kArm{5, 4, "Arm"};
+inline constexpr AnalysisType kSimple{3, 4, "Simple"};
 
 AnalysisType FromName(std::string_view name);
 }  // namespace analysis
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
index 4b13c6c..d946c48 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
@@ -71,11 +71,11 @@
 enum class FeedbackControllerLoopType { kPosition, kVelocity };
 
 namespace presets {
-constexpr FeedbackControllerPreset kDefault{1.0, 1.0, 20_ms, true, 0_s};
+inline constexpr FeedbackControllerPreset kDefault{1.0, 1.0, 20_ms, true, 0_s};
 
-constexpr FeedbackControllerPreset kWPILibNew{kDefault};
-constexpr FeedbackControllerPreset kWPILibOld{1.0 / 12.0, 1.0, 50_ms, false,
-                                              0_s};
+inline constexpr FeedbackControllerPreset kWPILibNew{kDefault};
+inline constexpr FeedbackControllerPreset kWPILibOld{1.0 / 12.0, 1.0, 50_ms,
+                                                     false, 0_s};
 
 // Measurement delay from a moving average filter:
 //
@@ -117,10 +117,10 @@
  *
  * Total delay = 50 ms + 31.5 ms = 81.5 ms.
  */
-constexpr FeedbackControllerPreset kCTRECANCoder{1.0 / 12.0, 60.0, 1_ms, true,
-                                                 81.5_ms};
-constexpr FeedbackControllerPreset kCTREDefault{1023.0 / 12.0, 0.1, 1_ms, false,
-                                                81.5_ms};
+inline constexpr FeedbackControllerPreset kCTRECANCoder{1.0 / 12.0, 60.0, 1_ms,
+                                                        true, 81.5_ms};
+inline constexpr FeedbackControllerPreset kCTREDefault{1023.0 / 12.0, 0.1, 1_ms,
+                                                       false, 81.5_ms};
 /**
  * https://api.ctr-electronics.com/phoenixpro/release/cpp/classctre_1_1phoenixpro_1_1hardware_1_1core_1_1_core_c_a_ncoder.html#a718a1a214b58d3c4543e88e3cb51ade5
  *
@@ -129,7 +129,8 @@
  * Pro devices make use of Kalman filters default-tuned to lowest latency, which
  * in testing is roughly 1 millisecond
  */
-constexpr FeedbackControllerPreset kCTREProDefault{1.0, 1.0, 1_ms, true, 1_ms};
+inline constexpr FeedbackControllerPreset kCTREProDefault{1.0, 1.0, 1_ms, true,
+                                                          1_ms};
 
 /**
  * https://github.com/wpilibsuite/sysid/issues/258#issuecomment-1010658237
@@ -138,8 +139,8 @@
  *
  * Total delay = 8-tap moving average delay = (8 - 1) / 2 * 32 ms = 112 ms.
  */
-constexpr FeedbackControllerPreset kREVNEOBuiltIn{1.0 / 12.0, 60.0, 1_ms, false,
-                                                  112_ms};
+inline constexpr FeedbackControllerPreset kREVNEOBuiltIn{1.0 / 12.0, 60.0, 1_ms,
+                                                         false, 112_ms};
 
 /**
  * https://www.revrobotics.com/content/sw/max/sw-docs/cpp/classrev_1_1_c_a_n_encoder.html#a7e6ce792bc0c0558fb944771df572e6a
@@ -150,15 +151,15 @@
  *
  * Total delay = 50 ms + 31.5 ms = 81.5 ms.
  */
-constexpr FeedbackControllerPreset kREVNonNEO{1.0 / 12.0, 60.0, 1_ms, false,
-                                              81.5_ms};
+inline constexpr FeedbackControllerPreset kREVNonNEO{1.0 / 12.0, 60.0, 1_ms,
+                                                     false, 81.5_ms};
 
 /**
  * https://github.com/wpilibsuite/sysid/pull/138#issuecomment-841734229
  *
  * Backward finite difference delay = 10 ms / 2 = 5 ms.
  */
-constexpr FeedbackControllerPreset kVenom{4096.0 / 12.0, 60.0, 1_ms, false,
-                                          5_ms};
+inline constexpr FeedbackControllerPreset kVenom{4096.0 / 12.0, 60.0, 1_ms,
+                                                 false, 5_ms};
 }  // namespace presets
 }  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
index fc9e47c..d2503ce 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
@@ -4,22 +4,50 @@
 
 #pragma once
 
+#include <string>
 #include <tuple>
 #include <vector>
 
 #include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/OLS.h"
 #include "sysid/analysis/Storage.h"
 
 namespace sysid {
 
 /**
+ * Exception for data that doesn't sample enough of the state-input space.
+ */
+class InsufficientSamplesError : public std::exception {
+ public:
+  /**
+   * Constructs an InsufficientSamplesError.
+   *
+   * @param message The error message
+   */
+  explicit InsufficientSamplesError(std::string_view message) {
+    m_message = message;
+  }
+
+  const char* what() const noexcept override { return m_message.c_str(); }
+
+ private:
+  /**
+   * Stores the error message
+   */
+  std::string m_message;
+};
+
+/**
  * Calculates feedforward gains given the data and the type of analysis to
  * perform.
  *
- * @return Tuple containing the coefficients of the analysis along with the
- *         r-squared (coefficient of determination) and RMSE (standard deviation
- * of the residuals) of the fit.
+ * @param data The OLS input data.
+ * @param type The analysis type.
+ * @param throwOnRankDeficiency Whether to throw if the fit is going to be poor.
+ *   This option is provided for unit testing purposes.
  */
-std::tuple<std::vector<double>, double, double> CalculateFeedforwardGains(
-    const Storage& data, const AnalysisType& type);
+OLSResult CalculateFeedforwardGains(const Storage& data,
+                                    const AnalysisType& type,
+                                    bool throwOnRankDeficiency = true);
+
 }  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
index 9030c00..28538a1 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
@@ -14,6 +14,7 @@
 #include <utility>
 #include <vector>
 
+#include <fmt/format.h>
 #include <frc/filter/LinearFilter.h>
 #include <units/time.h>
 #include <wpi/StringMap.h>
@@ -30,7 +31,8 @@
  * Exception for Invalid Data Errors in which we can't pin the cause of error to
  * any one specific setting of the GUI.
  */
-struct InvalidDataError : public std::exception {
+class InvalidDataError : public std::exception {
+ public:
   /**
    * Creates an InvalidDataError Exception. It adds additional steps after the
    * initial error message to inform users in the ways that they could fix their
@@ -46,17 +48,20 @@
         message);
   }
 
+  const char* what() const noexcept override { return m_message.c_str(); }
+
+ private:
   /**
    * Stores the error message
    */
   std::string m_message;
-  const char* what() const noexcept override { return m_message.c_str(); }
 };
 
 /**
  * Exception for Quasistatic Data being completely removed.
  */
-struct NoQuasistaticDataError : public std::exception {
+class NoQuasistaticDataError : public std::exception {
+ public:
   const char* what() const noexcept override {
     return "Quasistatic test trimming removed all data. Please adjust your "
            "motion threshold and double check "
@@ -68,7 +73,8 @@
 /**
  * Exception for Dynamic Data being completely removed.
  */
-struct NoDynamicDataError : public std::exception {
+class NoDynamicDataError : public std::exception {
+ public:
   const char* what() const noexcept override {
     return "Dynamic test trimming removed all data. Please adjust your test "
            "duration and double check "
@@ -92,6 +98,9 @@
     const std::vector<PreparedData>& data, int window,
     std::function<double(const PreparedData&)> accessorFunction);
 
+double GetMaxSpeed(const std::vector<PreparedData>& data,
+                   std::function<double(const PreparedData&)> accessorFunction);
+
 /**
  * Reduces noise in velocity data by applying a median filter.
  *
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/JSONConverter.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
deleted file mode 100644
index 7581d25..0000000
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
+++ /dev/null
@@ -1,24 +0,0 @@
-// 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 <string>
-#include <string_view>
-
-#include <wpi/Logger.h>
-
-namespace sysid {
-/**
- * Converts a JSON from the old frc-characterization format to the new sysid
- * format.
- *
- * @param path   The path to the old JSON.
- * @param logger The logger instance for log messages.
- * @return The full file path of the newly saved JSON.
- */
-std::string ConvertJSON(std::string_view path, wpi::Logger& logger);
-
-std::string ToCSV(std::string_view path, wpi::Logger& logger);
-}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/OLS.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/OLS.h
index cf97904..43f447a 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/OLS.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/OLS.h
@@ -5,22 +5,29 @@
 #pragma once
 
 #include <cstddef>
-#include <tuple>
 #include <vector>
 
 #include <Eigen/Core>
 
 namespace sysid {
 
+struct OLSResult {
+  /// Regression coeficients.
+  std::vector<double> coeffs;
+
+  /// R² (coefficient of determination)
+  double rSquared = 0.0;
+
+  /// Root-mean-square error
+  double rmse = 0.0;
+};
+
 /**
- * Performs ordinary least squares multiple regression on the provided data and
- * returns a vector of coefficients along with the r-squared (coefficient of
- * determination) and RMSE (stardard deviation of the residuals) of the fit.
+ * Performs ordinary least squares multiple regression on the provided data.
  *
  * @param X The independent data in y = Xβ.
  * @param y The dependent data in y = Xβ.
  */
-std::tuple<std::vector<double>, double, double> OLS(const Eigen::MatrixXd& X,
-                                                    const Eigen::VectorXd& y);
+OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y);
 
 }  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/Storage.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/Storage.h
index 52899a0..dad38e7 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/Storage.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/analysis/Storage.h
@@ -4,12 +4,46 @@
 
 #pragma once
 
+#include <string>
 #include <vector>
 
 #include <units/time.h>
+#include <units/voltage.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/AnalysisType.h"
 
 namespace sysid {
 
+struct MotorData {
+  // name of the *motor*, not the test
+  std::string name;
+
+  // Data for a single contiguous motor test
+  // Timestamps are not necessarily aligned!
+  struct Run {
+    template <typename T>
+      requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+    struct Sample {
+      Sample(units::second_t time, T measurement)
+          : time{time}, measurement{measurement} {}
+      units::second_t time;
+      T measurement;
+    };
+    std::vector<Sample<units::volt_t>> voltage;
+    std::vector<Sample<double>> position;
+    std::vector<Sample<double>> velocity;
+  };
+
+  std::vector<Run> runs;
+};
+
+struct TestData {
+  std::string distanceUnit;
+  AnalysisType mechanismType;
+  wpi::StringMap<MotorData> motorData;
+};
+
 /**
  * Represents each data point after it is cleaned and various parameters are
  * calculated.
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
deleted file mode 100644
index 85ee09e..0000000
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// 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 <array>
-#include <cstddef>
-#include <functional>
-#include <memory>
-#include <string>
-#include <string_view>
-#include <utility>
-#include <vector>
-
-#include <networktables/BooleanTopic.h>
-#include <networktables/DoubleTopic.h>
-#include <networktables/IntegerTopic.h>
-#include <networktables/NetworkTableInstance.h>
-#include <networktables/StringTopic.h>
-#include <units/time.h>
-#include <wpi/Logger.h>
-#include <wpi/SmallVector.h>
-#include <wpi/json.h>
-
-#include "sysid/analysis/AnalysisType.h"
-
-namespace sysid {
-/**
- * This class is responsible for collecting data from the robot and storing it
- * inside a JSON.
- */
-class TelemetryManager {
- public:
-  /**
-   * Represents settings for an instance of the TelemetryManager class. This
-   * contains information about the quasistatic ramp rate for slow tests, the
-   * step voltage for fast tests, and the mechanism type for characterization.
-   */
-  struct Settings {
-    /**
-     * The rate at which the voltage should increase during the quasistatic test
-     * (V/s).
-     */
-    double quasistaticRampRate = 0.25;
-
-    /**
-     * The voltage that the dynamic test should run at (V).
-     */
-    double stepVoltage = 7.0;
-
-    /**
-     * The units the mechanism moves per recorded rotation. The sysid project
-     * will be recording things in rotations of the shaft so the
-     * unitsPerRotation is to convert those measurements to the units the user
-     * wants to use.
-     */
-    double unitsPerRotation = 1.0;
-
-    /**
-     * The name of the units used.
-     * Valid units:  "Meters", "Feet", "Inches", "Radians", "Degrees",
-     * "Rotations"
-     */
-    std::string units = "Meters";
-
-    /**
-     * The type of mechanism that will be analyzed.
-     * Supported mechanisms: Drivetrain, Angular Drivetrain, Elevator, Arm,
-     * Simple motor.
-     */
-    AnalysisType mechanism = analysis::kDrivetrain;
-  };
-
-  /**
-   * Constructs an instance of the telemetry manager with the provided settings
-   * and NT instance to collect data over.
-   *
-   * @param settings The settings for this instance of the telemetry manager.
-   * @param logger   The logger instance to use for log data.
-   * @param instance The NT instance to collect data over. The default value of
-   *                 this parameter should suffice in production; it should only
-   *                 be changed during unit testing.
-   */
-  explicit TelemetryManager(const Settings& settings, wpi::Logger& logger,
-                            nt::NetworkTableInstance instance =
-                                nt::NetworkTableInstance::GetDefault());
-
-  /**
-   * Begins a test with the given parameters.
-   *
-   * @param name The name of the test.
-   */
-  void BeginTest(std::string_view name);
-
-  /**
-   * Ends the currently running test. If there is no test running, this is a
-   * no-op.
-   */
-  void EndTest();
-
-  /**
-   * Updates the telemetry manager -- this adds a new autospeed entry and
-   * collects newest data from the robot. This must be called periodically by
-   * the user.
-   */
-  void Update();
-
-  /**
-   * Registers a callback that's called by the TelemetryManager when there is a
-   * message to display to the user.
-   *
-   * @param callback Callback function that runs based off of the message
-   */
-  void RegisterDisplayCallback(std::function<void(std::string_view)> callback) {
-    m_callbacks.emplace_back(std::move(callback));
-  }
-
-  /**
-   * Saves a JSON with the stored data at the given location.
-   *
-   * @param location The location to save the JSON at (this is the folder that
-   *                 should contain the saved JSON).
-   * @return The full file path of the saved JSON.
-   */
-  std::string SaveJSON(std::string_view location);
-
-  /**
-   * Returns whether a test is currently running.
-   *
-   * @return Whether a test is currently running.
-   */
-  bool IsActive() const { return m_isRunningTest; }
-
-  /**
-   * Returns whether the specified test is running or has run.
-   *
-   * @param name The test to check.
-   *
-   * @return Whether the specified test is running or has run.
-   */
-  bool HasRunTest(std::string_view name) const {
-    return std::find(m_tests.cbegin(), m_tests.cend(), name) != m_tests.end();
-  }
-
-  /**
-   * Gets the size of the stored data.
-   *
-   * @return The size of the stored data
-   */
-  size_t GetCurrentDataSize() const { return m_params.data.size(); }
-
- private:
-  enum class State { WaitingForEnable, RunningTest, WaitingForData };
-
-  /**
-   * Stores information about a currently running test. This information
-   * includes whether the robot will be traveling quickly (dynamic) or slowly
-   * (quasistatic), the direction of movement, the start time of the test,
-   * whether the robot is enabled, the current speed of the robot, and the
-   * collected data.
-   */
-  struct TestParameters {
-    bool fast = false;
-    bool forward = false;
-    bool rotate = false;
-
-    State state = State::WaitingForEnable;
-
-    double enableStart = 0.0;
-    double disableStart = 0.0;
-
-    bool enabled = false;
-    double speed = 0.0;
-
-    std::string raw;
-    std::vector<std::vector<double>> data{};
-    bool overflow = false;
-    bool mechError = false;
-
-    TestParameters() = default;
-    TestParameters(bool fast, bool forward, bool rotate, State state)
-        : fast{fast}, forward{forward}, rotate{rotate}, state{state} {}
-  };
-
-  // Settings for this instance.
-  const Settings& m_settings;
-
-  // Logger.
-  wpi::Logger& m_logger;
-
-  // Test parameters for the currently running test.
-  TestParameters m_params;
-  bool m_isRunningTest = false;
-
-  // A list of running or already run tests.
-  std::vector<std::string> m_tests;
-
-  // Stores the test data.
-  wpi::json m_data;
-
-  // Display callbacks.
-  wpi::SmallVector<std::function<void(std::string_view)>, 1> m_callbacks;
-
-  // NetworkTables instance and entries.
-  nt::NetworkTableInstance m_inst;
-  std::shared_ptr<nt::NetworkTable> table = m_inst.GetTable("SmartDashboard");
-  nt::DoublePublisher m_voltageCommand =
-      table->GetDoubleTopic("SysIdVoltageCommand").Publish();
-  nt::StringPublisher m_testType =
-      table->GetStringTopic("SysIdTestType").Publish();
-  nt::BooleanPublisher m_rotate =
-      table->GetBooleanTopic("SysIdRotate").Publish();
-  nt::StringPublisher m_mechanism =
-      table->GetStringTopic("SysIdTest").Publish();
-  nt::BooleanPublisher m_overflowPub =
-      table->GetBooleanTopic("SysIdOverflow").Publish();
-  nt::BooleanSubscriber m_overflowSub =
-      table->GetBooleanTopic("SysIdOverflow").Subscribe(false);
-  nt::BooleanPublisher m_mechErrorPub =
-      table->GetBooleanTopic("SysIdWrongMech").Publish();
-  nt::BooleanSubscriber m_mechErrorSub =
-      table->GetBooleanTopic("SysIdWrongMech").Subscribe(false);
-  nt::StringSubscriber m_telemetry =
-      table->GetStringTopic("SysIdTelemetry").Subscribe("");
-  nt::IntegerSubscriber m_fmsControlData =
-      m_inst.GetTable("FMSInfo")
-          ->GetIntegerTopic("FMSControlData")
-          .Subscribe(0);
-  nt::DoublePublisher m_ackNumberPub =
-      table->GetDoubleTopic("SysIdAckNumber").Publish();
-  nt::DoubleSubscriber m_ackNumberSub =
-      table->GetDoubleTopic("SysIdAckNumber").Subscribe(0);
-
-  int m_ackNumber;
-};
-}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/Analyzer.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/Analyzer.h
index 2f30f61..7dcfd41 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/Analyzer.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/Analyzer.h
@@ -40,11 +40,12 @@
  */
 class Analyzer : public glass::View {
  public:
+  TestData m_data;
   /**
    * The different display and processing states for the GUI
    */
   enum class AnalyzerState {
-    kWaitingForJSON,
+    kWaitingForData,
     kNominalDisplay,
     kMotionThresholdError,
     kTestDurationError,
@@ -90,12 +91,12 @@
 
   ~Analyzer() override { AbortDataPrep(); };
 
- private:
   /**
-   * Handles the logic for selecting a json to analyze
+   * Analyzes the selected data.
    */
-  void SelectFile();
+  void AnalyzeData();
 
+ private:
   /**
    * Kills the data preparation thread
    */
@@ -113,11 +114,6 @@
   void DisplayGraphs();
 
   /**
-   * Displays the file selection widget.
-   */
-  void DisplayFileSelector();
-
-  /**
    * Resets the current analysis data.
    */
   void ResetData();
@@ -196,7 +192,7 @@
   void HandleError(std::string_view msg);
 
   // State of the Display GUI
-  AnalyzerState m_state = AnalyzerState::kWaitingForJSON;
+  AnalyzerState m_state = AnalyzerState::kWaitingForData;
 
   // Stores the exception message.
   std::string m_exception;
@@ -221,29 +217,21 @@
   double m_Kd;
   units::millisecond_t m_timescale;
 
-  // Track width
-  std::optional<double> m_trackWidth;
-
   // Units
   int m_selectedOverrideUnit = 0;
-  double m_conversionFactor = 0.0;
 
   // Data analysis
   std::unique_ptr<AnalysisManager> m_manager;
   int m_dataset = 0;
   int m_window = 8;
   double m_threshold = 0.2;
-  float m_stepTestDuration = 0.0;
+  float m_stepTestDuration = 10;
 
   double m_gearingNumerator = 1.0;
   double m_gearingDenominator = 1.0;
 
   bool combinedGraphFit = false;
 
-  // File manipulation
-  std::unique_ptr<pfd::open_file> m_selector;
-  std::string m_location;
-
   // Logger
   wpi::Logger& m_logger;
 
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/DataSelector.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/DataSelector.h
new file mode 100644
index 0000000..95d7e19
--- /dev/null
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/DataSelector.h
@@ -0,0 +1,80 @@
+// 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 <functional>
+#include <future>
+#include <map>
+#include <string>
+#include <vector>
+
+#include <glass/View.h>
+#include <glass/support/DataLogReaderThread.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/Storage.h"
+
+namespace glass {
+class DataLogReaderEntry;
+class Storage;
+}  // namespace glass
+
+namespace wpi {
+class Logger;
+}  // namespace wpi
+
+namespace sysid {
+/**
+ * Helps with loading datalog files.
+ */
+class DataSelector : public glass::View {
+ public:
+  /**
+   * Creates a data selector widget
+   *
+   * @param logger The program logger
+   */
+  explicit DataSelector(glass::Storage& storage, wpi::Logger& logger)
+  /*: m_logger{logger}*/ {}
+
+  /**
+   * Displays the log loader window.
+   */
+  void Display() override;
+
+  /**
+   * Resets view. Must be called whenever the DataLogReader goes away, as this
+   * class keeps references to DataLogReaderEntry objects.
+   */
+  void Reset();
+
+  /**
+   * Called when new test data is loaded.
+   */
+  std::function<void(TestData)> testdata;
+
+ private:
+  // wpi::Logger& m_logger;
+  using Runs = std::vector<glass::DataLogReaderRange>;
+  using State = std::map<std::string, Runs, std::less<>>;   // full name
+  using Tests = std::map<std::string, State, std::less<>>;  // e.g. "dynamic"
+  std::future<Tests> m_testsFuture;
+  Tests m_tests;
+  std::string m_selectedTest;
+  const glass::DataLogReaderEntry* m_testStateEntry = nullptr;
+  const glass::DataLogReaderEntry* m_velocityEntry = nullptr;
+  const glass::DataLogReaderEntry* m_positionEntry = nullptr;
+  const glass::DataLogReaderEntry* m_voltageEntry = nullptr;
+  double m_velocityScale = 1.0;
+  double m_positionScale = 1.0;
+  int m_selectedUnit = 0;
+  int m_selectedAnalysis = 0;
+  std::future<TestData> m_testdataFuture;
+  std::vector<std::string> m_testdataStats;
+
+  static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry);
+  TestData BuildTestData();
+};
+}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/JSONConverter.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/JSONConverter.h
deleted file mode 100644
index 89bfa32..0000000
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/JSONConverter.h
+++ /dev/null
@@ -1,57 +0,0 @@
-// 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 <functional>
-#include <memory>
-#include <string>
-#include <string_view>
-
-#include <glass/View.h>
-#include <portable-file-dialogs.h>
-#include <wpi/Logger.h>
-
-namespace sysid {
-/**
- * Helps with converting different JSONs into different formats. Primarily
- * enables users to convert an old 2020 FRC-Characterization JSON into a SysId
- * JSON or a SysId JSON into a CSV file.
- */
-class JSONConverter {
- public:
-  /**
-   * Creates a JSONConverter widget
-   *
-   * @param logger The program logger
-   */
-  explicit JSONConverter(wpi::Logger& logger) : m_logger(logger) {}
-
-  /**
-   * Function to display the SysId JSON to CSV converter.
-   */
-  void DisplayCSVConvert();
-
- private:
-  /**
-   * Helper method to display a specific JSON converter
-   *
-   * @param tooltip The tooltip describing the JSON converter
-   * @param converter The function that takes a filename path and performs the
-   *                  previously specifid JSON conversion.
-   */
-  void DisplayConverter(
-      const char* tooltip,
-      std::function<std::string(std::string_view, wpi::Logger&)> converter);
-
-  wpi::Logger& m_logger;
-
-  std::string m_location;
-  std::unique_ptr<pfd::open_file> m_opener;
-
-  std::string m_exception;
-
-  double m_timestamp = 0;
-};
-}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/LogLoader.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/LogLoader.h
new file mode 100644
index 0000000..04ddd84
--- /dev/null
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/LogLoader.h
@@ -0,0 +1,78 @@
+// 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 <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <glass/View.h>
+#include <wpi/Signal.h>
+
+namespace glass {
+class DataLogReaderEntry;
+class DataLogReaderThread;
+class Storage;
+}  // namespace glass
+
+namespace pfd {
+class open_file;
+}  // namespace pfd
+
+namespace wpi {
+class Logger;
+}  // namespace wpi
+
+namespace sysid {
+/**
+ * Helps with loading datalog files.
+ */
+class LogLoader : public glass::View {
+ public:
+  /**
+   * Creates a log loader widget
+   *
+   * @param logger The program logger
+   */
+  explicit LogLoader(glass::Storage& storage, wpi::Logger& logger);
+
+  ~LogLoader() override;
+
+  /**
+   * Displays the log loader window.
+   */
+  void Display() override;
+
+  /**
+   * Signal called when the current file is unloaded (invalidates any
+   * LogEntry*).
+   */
+  wpi::sig::Signal<> unload;
+
+ private:
+  // wpi::Logger& m_logger;
+
+  std::string m_filename;
+  std::unique_ptr<pfd::open_file> m_opener;
+  std::unique_ptr<glass::DataLogReaderThread> m_reader;
+
+  std::string m_error;
+
+  std::string m_filter;
+
+  struct EntryTreeNode {
+    explicit EntryTreeNode(std::string_view name) : name{name} {}
+    std::string name;  // name of just this node
+    std::string path;  // full path if entry is nullptr
+    const glass::DataLogReaderEntry* entry = nullptr;
+    std::vector<EntryTreeNode> children;  // children, sorted by name
+  };
+  std::vector<EntryTreeNode> m_entryTree;
+
+  void RebuildEntryTree();
+  void DisplayEntryTree(const std::vector<EntryTreeNode>& nodes);
+};
+}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/Logger.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/Logger.h
deleted file mode 100644
index d06d650..0000000
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/Logger.h
+++ /dev/null
@@ -1,82 +0,0 @@
-// 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 <memory>
-#include <string>
-
-#include <glass/DataSource.h>
-#include <glass/View.h>
-#include <glass/networktables/NetworkTablesSettings.h>
-#include <portable-file-dialogs.h>
-#include <wpi/Logger.h>
-
-#include "sysid/telemetry/TelemetryManager.h"
-
-namespace glass {
-class Storage;
-}  // namespace glass
-
-namespace sysid {
-/**
- * The logger GUI takes care of running the system idenfitication tests over
- * NetworkTables and logging the data. This data is then stored in a JSON file
- * which can be used for analysis.
- */
-class Logger : public glass::View {
- public:
-  /**
-   * Makes a logger widget.
-   *
-   * @param storage The glass storage object
-   * @param logger A logger object that keeps track of the program's logs
-   */
-  Logger(glass::Storage& storage, wpi::Logger& logger);
-
-  /**
-   * Displays the logger widget.
-   */
-  void Display() override;
-
-  /**
-   * The different mechanism / analysis types that are supported.
-   */
-  static constexpr const char* kTypes[] = {"Drivetrain", "Drivetrain (Angular)",
-                                           "Arm", "Elevator", "Simple"};
-
-  /**
-   * The different units that are supported.
-   */
-  static constexpr const char* kUnits[] = {"Meters",  "Feet",      "Inches",
-                                           "Radians", "Rotations", "Degrees"};
-
- private:
-  /**
-   * Handles the logic of selecting a folder to save the SysId JSON to
-   */
-  void SelectDataFolder();
-
-  wpi::Logger& m_logger;
-
-  TelemetryManager::Settings m_settings;
-  int m_selectedType = 0;
-  int m_selectedUnit = 0;
-
-  std::unique_ptr<TelemetryManager> m_manager =
-      std::make_unique<TelemetryManager>(m_settings, m_logger);
-
-  std::unique_ptr<pfd::select_folder> m_selector;
-  std::string m_jsonLocation;
-
-  glass::NetworkTablesSettings m_ntSettings;
-
-  bool m_isRotationalUnits = false;
-
-  std::string m_popupText;
-
-  std::string m_opened;
-  std::string m_exception;
-};
-}  // namespace sysid
diff --git a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/UILayout.h b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/UILayout.h
index 732a1aa..f5d1e44 100644
--- a/third_party/allwpilib/sysid/src/main/native/include/sysid/view/UILayout.h
+++ b/third_party/allwpilib/sysid/src/main/native/include/sysid/view/UILayout.h
@@ -62,9 +62,12 @@
     310, kAppWindowSize.y - kLeftColPos.y - kWindowGap};
 
 // Left column contents
-inline constexpr Vector2d kLoggerWindowPos = kLeftColPos;
-inline constexpr Vector2d kLoggerWindowSize{
-    kLeftColSize.x, kAppWindowSize.y - kWindowGap - kLoggerWindowPos.y};
+inline constexpr Vector2d kLogLoaderWindowPos = kLeftColPos;
+inline constexpr Vector2d kLogLoaderWindowSize{kLeftColSize.x, 450};
+inline constexpr Vector2d kDataSelectorWindowPos =
+    kLogLoaderWindowPos + Vector2d{0, kLogLoaderWindowSize.y + kWindowGap};
+inline constexpr Vector2d kDataSelectorWindowSize{
+    kLeftColSize.x, kAppWindowSize.y - kWindowGap - kDataSelectorWindowPos.y};
 
 // Center column position and size
 inline constexpr Vector2d kCenterColPos =
diff --git a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
index 0abb2a1..51d348c 100644
--- a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
+++ b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
@@ -7,10 +7,6 @@
 #include "sysid/analysis/AnalysisType.h"
 
 TEST(AnalysisTypeTest, FromName) {
-  EXPECT_EQ(sysid::analysis::kDrivetrain,
-            sysid::analysis::FromName("Drivetrain"));
-  EXPECT_EQ(sysid::analysis::kDrivetrainAngular,
-            sysid::analysis::FromName("Drivetrain (Angular)"));
   EXPECT_EQ(sysid::analysis::kElevator, sysid::analysis::FromName("Elevator"));
   EXPECT_EQ(sysid::analysis::kArm, sysid::analysis::FromName("Arm"));
   EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Simple"));
diff --git a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
index a52840d..d8cd79e 100644
--- a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
+++ b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
@@ -2,28 +2,43 @@
 // 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 <stdint.h>
+
+#include <bitset>
 #include <cmath>
+#include <span>
 
 #include <gtest/gtest.h>
 #include <units/time.h>
 #include <units/voltage.h>
 
 #include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
 #include "sysid/analysis/ArmSim.h"
 #include "sysid/analysis/ElevatorSim.h"
 #include "sysid/analysis/FeedforwardAnalysis.h"
 #include "sysid/analysis/SimpleMotorSim.h"
 
+namespace {
+
+enum Movements : uint32_t {
+  kSlowForward,
+  kSlowBackward,
+  kFastForward,
+  kFastBackward
+};
+
+inline constexpr int kMovementCombinations = 16;
+
 /**
  * Return simulated test data for a given simulation model.
  *
- * @param Ks Static friction gain.
- * @param Kv Velocity gain.
- * @param Ka Acceleration gain.
- * @param Kg Gravity cosine gain.
+ * @tparam Model The model type.
+ * @param model The simulation model.
+ * @param movements Which movements to do.
  */
 template <typename Model>
-sysid::Storage CollectData(Model& model) {
+sysid::Storage CollectData(Model& model, std::bitset<4> movements) {
   constexpr auto kUstep = 0.25_V / 1_s;
   constexpr units::volt_t kUmax = 7_V;
   constexpr units::second_t T = 5_ms;
@@ -31,221 +46,247 @@
 
   sysid::Storage storage;
   auto& [slowForward, slowBackward, fastForward, fastBackward] = storage;
-
-  // Slow forward test
   auto voltage = 0_V;
-  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
-    slowForward.emplace_back(sysid::PreparedData{
-        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
-        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
-        std::sin(model.GetPosition())});
 
-    model.Update(voltage, T);
-    voltage += kUstep * T;
+  // Slow forward
+  if (movements.test(Movements::kSlowForward)) {
+    model.Reset();
+    voltage = 0_V;
+    for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+      slowForward.emplace_back(sysid::PreparedData{
+          i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+          model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+          std::sin(model.GetPosition())});
+
+      model.Update(voltage, T);
+      voltage += kUstep * T;
+    }
   }
 
-  // Slow backward test
-  model.Reset();
-  voltage = 0_V;
-  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
-    slowBackward.emplace_back(sysid::PreparedData{
-        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
-        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
-        std::sin(model.GetPosition())});
+  // Slow backward
+  if (movements.test(Movements::kSlowBackward)) {
+    model.Reset();
+    voltage = 0_V;
+    for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+      slowBackward.emplace_back(sysid::PreparedData{
+          i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+          model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+          std::sin(model.GetPosition())});
 
-    model.Update(voltage, T);
-    voltage -= kUstep * T;
+      model.Update(voltage, T);
+      voltage -= kUstep * T;
+    }
   }
 
-  // Fast forward test
-  model.Reset();
-  voltage = 0_V;
-  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
-    fastForward.emplace_back(sysid::PreparedData{
-        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
-        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
-        std::sin(model.GetPosition())});
+  // Fast forward
+  if (movements.test(Movements::kFastForward)) {
+    model.Reset();
+    voltage = 0_V;
+    for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+      fastForward.emplace_back(sysid::PreparedData{
+          i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+          model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+          std::sin(model.GetPosition())});
 
-    model.Update(voltage, T);
-    voltage = kUmax;
+      model.Update(voltage, T);
+      voltage = kUmax;
+    }
   }
 
-  // Fast backward test
-  model.Reset();
-  voltage = 0_V;
-  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
-    fastBackward.emplace_back(sysid::PreparedData{
-        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
-        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
-        std::sin(model.GetPosition())});
+  // Fast backward
+  if (movements.test(Movements::kFastBackward)) {
+    model.Reset();
+    voltage = 0_V;
+    for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+      fastBackward.emplace_back(sysid::PreparedData{
+          i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+          model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+          std::sin(model.GetPosition())});
 
-    model.Update(voltage, T);
-    voltage = -kUmax;
+      model.Update(voltage, T);
+      voltage = -kUmax;
+    }
   }
 
   return storage;
 }
 
-TEST(FeedforwardAnalysisTest, Arm1) {
-  constexpr double Ks = 1.01;
-  constexpr double Kv = 3.060;
-  constexpr double Ka = 0.327;
-  constexpr double Kg = 0.211;
+/**
+ * Asserts success if the gains contain NaNs or are too far from their expected
+ * values.
+ *
+ * @param expectedGains The expected feedforward gains.
+ * @param actualGains The calculated feedforward gains.
+ * @param tolerances The tolerances for the coefficient comparisons.
+ */
+testing::AssertionResult FitIsBad(std::span<const double> expectedGains,
+                                  std::span<const double> actualGains,
+                                  std::span<const double> tolerances) {
+  // Check for NaN
+  for (const auto& coeff : actualGains) {
+    if (std::isnan(coeff)) {
+      return testing::AssertionSuccess();
+    }
+  }
 
-  for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
-    sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
-    auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                               sysid::analysis::kArm);
-    auto& gains = std::get<0>(ff);
+  for (size_t i = 0; i < expectedGains.size(); ++i) {
+    if (std::abs(expectedGains[i] - actualGains[i]) >= tolerances[i]) {
+      return testing::AssertionSuccess();
+    }
+  }
 
-    EXPECT_NEAR(gains[0], Ks, 0.003);
-    EXPECT_NEAR(gains[1], Kv, 0.003);
-    EXPECT_NEAR(gains[2], Ka, 0.003);
-    EXPECT_NEAR(gains[3], Kg, 0.003);
-    EXPECT_NEAR(gains[4], offset, 0.007);
+  auto result = testing::AssertionFailure();
+
+  result << "\n";
+  for (size_t i = 0; i < expectedGains.size(); ++i) {
+    if (i == 0) {
+      result << "Ks";
+    } else if (i == 1) {
+      result << "Kv";
+    } else if (i == 2) {
+      result << "Ka";
+    } else if (i == 3) {
+      result << "Kg";
+    } else if (i == 4) {
+      result << "offset";
+    }
+
+    result << ":\n";
+    result << "  expected " << expectedGains[i] << ",\n";
+    result << "  actual " << actualGains[i] << ",\n";
+    result << "  diff " << std::abs(expectedGains[i] - actualGains[i]) << "\n";
+  }
+
+  return result;
+}
+
+/**
+ * Asserts that two arrays are equal.
+ *
+ * @param expected The expected array.
+ * @param actual The actual array.
+ * @param tolerances The tolerances for the element comparisons.
+ */
+void ExpectArrayNear(std::span<const double> expected,
+                     std::span<const double> actual,
+                     std::span<const double> tolerances) {
+  // Check size
+  const size_t size = expected.size();
+  EXPECT_EQ(size, actual.size());
+  EXPECT_EQ(size, tolerances.size());
+
+  // Check elements
+  for (size_t i = 0; i < size; ++i) {
+    EXPECT_NEAR(expected[i], actual[i], tolerances[i]) << "where i = " << i;
   }
 }
 
-TEST(FeedforwardAnalysisTest, Arm2) {
-  constexpr double Ks = 0.547;
-  constexpr double Kv = 0.0693;
-  constexpr double Ka = 0.1170;
-  constexpr double Kg = 0.122;
+/**
+ * @tparam Model The model type.
+ * @param model The simulation model.
+ * @param type The analysis type.
+ * @param expectedGains The expected feedforward gains.
+ * @param tolerances The tolerances for the coefficient comparisons.
+ */
+template <typename Model>
+void RunTests(Model& model, const sysid::AnalysisType& type,
+              std::span<const double> expectedGains,
+              std::span<const double> tolerances) {
+  // Iterate through all combinations of movements
+  for (int movements = 0; movements < kMovementCombinations; ++movements) {
+    try {
+      auto ff =
+          sysid::CalculateFeedforwardGains(CollectData(model, movements), type);
 
-  for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
-    sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
-    auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                               sysid::analysis::kArm);
-    auto& gains = std::get<0>(ff);
-
-    EXPECT_NEAR(gains[0], Ks, 0.003);
-    EXPECT_NEAR(gains[1], Kv, 0.003);
-    EXPECT_NEAR(gains[2], Ka, 0.003);
-    EXPECT_NEAR(gains[3], Kg, 0.003);
-    EXPECT_NEAR(gains[4], offset, 0.007);
+      ExpectArrayNear(expectedGains, ff.coeffs, tolerances);
+    } catch (sysid::InsufficientSamplesError&) {
+      // If calculation threw an exception, confirm at least one of the gains
+      // doesn't match
+      auto ff = sysid::CalculateFeedforwardGains(CollectData(model, movements),
+                                                 type, false);
+      EXPECT_TRUE(FitIsBad(expectedGains, ff.coeffs, tolerances));
+    }
   }
 }
 
-TEST(FeedforwardAnalysisTest, Drivetrain1) {
-  constexpr double Ks = 1.01;
-  constexpr double Kv = 3.060;
-  constexpr double Ka = 0.327;
+}  // namespace
 
-  sysid::SimpleMotorSim model{Ks, Kv, Ka};
-  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                             sysid::analysis::kDrivetrain);
-  auto& gains = std::get<0>(ff);
+TEST(FeedforwardAnalysisTest, Arm) {
+  {
+    constexpr double Ks = 1.01;
+    constexpr double Kv = 3.060;
+    constexpr double Ka = 0.327;
+    constexpr double Kg = 0.211;
 
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
+    for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
+      sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
+
+      RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}},
+               {{8e-3, 8e-3, 8e-3, 8e-3, 3e-2}});
+    }
+  }
+
+  {
+    constexpr double Ks = 0.547;
+    constexpr double Kv = 0.0693;
+    constexpr double Ka = 0.1170;
+    constexpr double Kg = 0.122;
+
+    for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
+      sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
+
+      RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}},
+               {{8e-3, 8e-3, 8e-3, 8e-3, 5e-2}});
+    }
+  }
 }
 
-TEST(FeedforwardAnalysisTest, Drivetrain2) {
-  constexpr double Ks = 0.547;
-  constexpr double Kv = 0.0693;
-  constexpr double Ka = 0.1170;
+TEST(FeedforwardAnalysisTest, Elevator) {
+  {
+    constexpr double Ks = 1.01;
+    constexpr double Kv = 3.060;
+    constexpr double Ka = 0.327;
+    constexpr double Kg = -0.211;
 
-  sysid::SimpleMotorSim model{Ks, Kv, Ka};
-  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                             sysid::analysis::kDrivetrain);
-  auto& gains = std::get<0>(ff);
+    sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
 
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
+    RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}},
+             {{8e-3, 8e-3, 8e-3, 8e-3}});
+  }
+
+  {
+    constexpr double Ks = 0.547;
+    constexpr double Kv = 0.0693;
+    constexpr double Ka = 0.1170;
+    constexpr double Kg = -0.122;
+
+    sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
+
+    RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}},
+             {{8e-3, 8e-3, 8e-3, 8e-3}});
+  }
 }
 
-TEST(FeedforwardAnalysisTest, DrivetrainAngular1) {
-  constexpr double Ks = 1.01;
-  constexpr double Kv = 3.060;
-  constexpr double Ka = 0.327;
+TEST(FeedforwardAnalysisTest, Simple) {
+  {
+    constexpr double Ks = 1.01;
+    constexpr double Kv = 3.060;
+    constexpr double Ka = 0.327;
 
-  sysid::SimpleMotorSim model{Ks, Kv, Ka};
-  auto ff = sysid::CalculateFeedforwardGains(
-      CollectData(model), sysid::analysis::kDrivetrainAngular);
-  auto& gains = std::get<0>(ff);
+    sysid::SimpleMotorSim model{Ks, Kv, Ka};
 
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
-}
+    RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}},
+             {{8e-3, 8e-3, 8e-3}});
+  }
 
-TEST(FeedforwardAnalysisTest, DrivetrainAngular2) {
-  constexpr double Ks = 0.547;
-  constexpr double Kv = 0.0693;
-  constexpr double Ka = 0.1170;
+  {
+    constexpr double Ks = 0.547;
+    constexpr double Kv = 0.0693;
+    constexpr double Ka = 0.1170;
 
-  sysid::SimpleMotorSim model{Ks, Kv, Ka};
-  auto ff = sysid::CalculateFeedforwardGains(
-      CollectData(model), sysid::analysis::kDrivetrainAngular);
-  auto& gains = std::get<0>(ff);
+    sysid::SimpleMotorSim model{Ks, Kv, Ka};
 
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
-}
-
-TEST(FeedforwardAnalysisTest, Elevator1) {
-  constexpr double Ks = 1.01;
-  constexpr double Kv = 3.060;
-  constexpr double Ka = 0.327;
-  constexpr double Kg = -0.211;
-
-  sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
-  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                             sysid::analysis::kElevator);
-  auto& gains = std::get<0>(ff);
-
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
-  EXPECT_NEAR(gains[3], Kg, 0.003);
-}
-
-TEST(FeedforwardAnalysisTest, Elevator2) {
-  constexpr double Ks = 0.547;
-  constexpr double Kv = 0.0693;
-  constexpr double Ka = 0.1170;
-  constexpr double Kg = -0.122;
-
-  sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
-  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                             sysid::analysis::kElevator);
-  auto& gains = std::get<0>(ff);
-
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
-  EXPECT_NEAR(gains[3], Kg, 0.003);
-}
-
-TEST(FeedforwardAnalysisTest, Simple1) {
-  constexpr double Ks = 1.01;
-  constexpr double Kv = 3.060;
-  constexpr double Ka = 0.327;
-
-  sysid::SimpleMotorSim model{Ks, Kv, Ka};
-  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                             sysid::analysis::kSimple);
-  auto& gains = std::get<0>(ff);
-
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
-}
-
-TEST(FeedforwardAnalysisTest, Simple2) {
-  constexpr double Ks = 0.547;
-  constexpr double Kv = 0.0693;
-  constexpr double Ka = 0.1170;
-
-  sysid::SimpleMotorSim model{Ks, Kv, Ka};
-  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
-                                             sysid::analysis::kSimple);
-  auto& gains = std::get<0>(ff);
-
-  EXPECT_NEAR(gains[0], Ks, 0.003);
-  EXPECT_NEAR(gains[1], Kv, 0.003);
-  EXPECT_NEAR(gains[2], Ka, 0.003);
+    RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}},
+             {{8e-3, 8e-3, 8e-3}});
+  }
 }
diff --git a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FilterTest.cpp b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FilterTest.cpp
index a7b0349..8cfebe4 100644
--- a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FilterTest.cpp
+++ b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/FilterTest.cpp
@@ -45,19 +45,13 @@
 
 TEST(FilterTest, StepTrim) {
   std::vector<sysid::PreparedData> testData = {
-      {0_s, 1, 2, 3, 5_ms, 0, 0},    {1_s, 1, 2, 3, 5_ms, 0.25, 0},
-      {2_s, 1, 2, 3, 5_ms, 0.5, 0},  {3_s, 1, 2, 3, 5_ms, 0.45, 0},
-      {4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
-      {6_s, 1, 2, 3, 5_ms, 0, 0},    {7_s, 1, 2, 3, 5_ms, 0.02, 0},
-      {8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0},
+      {0_s, 1, 2, 0, 5_ms, 0, 0},   {1_s, 1, 2, 3, 5_ms, 0.25, 0},
+      {2_s, 1, 2, 0, 5_ms, 10, 0},  {3_s, 1, 2, 3, 5_ms, 0.45, 0},
+      {4_s, 1, 2, 9.6, 5_ms, 0, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
+      {6_s, 1, 2, 0, 5_ms, 0, 0},   {7_s, 1, 2, 3, 5_ms, 0.02, 0},
+      {8_s, 1, 2, 10, 5_ms, 0, 0},  {9_s, 1, 2, 3, 5_ms, 0, 0},
   };
 
-  std::vector<sysid::PreparedData> expectedData = {
-      {2_s, 1, 2, 3, 5_ms, 0.5, 0},
-      {3_s, 1, 2, 3, 5_ms, 0.45, 0},
-      {4_s, 1, 2, 3, 5_ms, 0.35, 0},
-      {5_s, 1, 2, 3, 5_ms, 0.15, 0}};
-
   auto maxTime = 9_s;
   auto minTime = maxTime;
 
@@ -66,9 +60,7 @@
       sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime);
   minTime = tempMinTime;
 
-  EXPECT_EQ(expectedData[0].acceleration, testData[0].acceleration);
-  EXPECT_EQ(expectedData.back().acceleration, testData.back().acceleration);
-  EXPECT_EQ(5, settings.stepTestDuration.value());
+  EXPECT_EQ(4, settings.stepTestDuration.value());
   EXPECT_EQ(2, minTime.value());
 }
 
diff --git a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/OLSTest.cpp b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/OLSTest.cpp
index bf20516..558b7e3 100644
--- a/third_party/allwpilib/sysid/src/test/native/cpp/analysis/OLSTest.cpp
+++ b/third_party/allwpilib/sysid/src/test/native/cpp/analysis/OLSTest.cpp
@@ -11,12 +11,12 @@
   Eigen::MatrixXd X{{1.0, 1.0}, {1.0, 2.0}};
   Eigen::VectorXd y{{3.0}, {5.0}};
 
-  auto [coefficients, cod, rmse] = sysid::OLS(X, y);
-  EXPECT_EQ(coefficients.size(), 2u);
+  auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
+  EXPECT_EQ(coeffs.size(), 2u);
 
-  EXPECT_NEAR(coefficients[0], 1.0, 0.05);
-  EXPECT_NEAR(coefficients[1], 2.0, 0.05);
-  EXPECT_NEAR(cod, 1.0, 1E-4);
+  EXPECT_NEAR(coeffs[0], 1.0, 0.05);
+  EXPECT_NEAR(coeffs[1], 2.0, 0.05);
+  EXPECT_NEAR(rSquared, 1.0, 1e-4);
 }
 
 TEST(OLSTest, TwoVariablesFivePoints) {
@@ -25,12 +25,12 @@
   Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 5}, {1, 7}, {1, 9}};
   Eigen::VectorXd y{{4}, {5}, {7}, {10}, {15}};
 
-  auto [coefficients, cod, rmse] = sysid::OLS(X, y);
-  EXPECT_EQ(coefficients.size(), 2u);
+  auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
+  EXPECT_EQ(coeffs.size(), 2u);
 
-  EXPECT_NEAR(coefficients[0], 0.305, 0.05);
-  EXPECT_NEAR(coefficients[1], 1.518, 0.05);
-  EXPECT_NEAR(cod, 0.985, 0.05);
+  EXPECT_NEAR(coeffs[0], 0.305, 0.05);
+  EXPECT_NEAR(coeffs[1], 1.518, 0.05);
+  EXPECT_NEAR(rSquared, 0.985, 0.05);
 }
 
 #ifndef NDEBUG
diff --git a/third_party/allwpilib/test-scripts/run-tests-on-robot.sh b/third_party/allwpilib/test-scripts/run-tests-on-robot.sh
index 32b66b5..534eafd 100755
--- a/third_party/allwpilib/test-scripts/run-tests-on-robot.sh
+++ b/third_party/allwpilib/test-scripts/run-tests-on-robot.sh
@@ -11,6 +11,8 @@
 # There is one file (delploy-and-run-test-on-robot.sh) that is designed to
 # deploy this file along with the compiled tests for you.
 
+set -e
+
 # Configurable variables
 source config.sh
 
@@ -114,7 +116,7 @@
 /usr/local/natinst/etc/init.d/systemWebServer stop
 
 # Kill all running robot programs
-killall java FRCUserProgram
+killall java FRCUserProgram || true
 
 # If we are running with the -d argument move the test to the DEFAULT_TEST_DIR
 if [[ ! -e "${TEST_DIR}/${TEST_FILE}" ]]; then
diff --git a/third_party/allwpilib/upstream_utils/eigen_patches/0001-Disable-warnings.patch b/third_party/allwpilib/upstream_utils/eigen_patches/0001-Disable-warnings.patch
index e89fec5..25462a6 100644
--- a/third_party/allwpilib/upstream_utils/eigen_patches/0001-Disable-warnings.patch
+++ b/third_party/allwpilib/upstream_utils/eigen_patches/0001-Disable-warnings.patch
@@ -1,31 +1,29 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 18 May 2022 09:14:24 -0700
-Subject: [PATCH 1/3] Disable warnings
+Subject: [PATCH 1/2] Disable warnings
 
 ---
- Eigen/src/Core/util/DisableStupidWarnings.h | 11 +++++++++++
- 1 file changed, 11 insertions(+)
+ Eigen/src/Core/util/DisableStupidWarnings.h | 9 +++++++++
+ 1 file changed, 9 insertions(+)
 
 diff --git a/Eigen/src/Core/util/DisableStupidWarnings.h b/Eigen/src/Core/util/DisableStupidWarnings.h
-index fe0cfec0bc2461ac44abca8f3d05b468d3c60fd9..9a630e4ae692aee0277d60b3083c968d087920dd 100755
+index 32a427d852355a51dc4263d81498554ff4c3cbba..eb259433c054c21accd2b8a5d744638f8004da40 100644
 --- a/Eigen/src/Core/util/DisableStupidWarnings.h
 +++ b/Eigen/src/Core/util/DisableStupidWarnings.h
-@@ -71,6 +71,17 @@
-     // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
-     #pragma GCC diagnostic ignored "-Wattributes"
-   #endif
-+  #if __GNUC__>=8
-+    #pragma GCC diagnostic ignored "-Wclass-memaccess"
-+  #endif
-+  #if __GNUC__>=11
-+    // This warning is a false positive
-+    #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
-+  #endif
-+  #if __GNUC__>=12
-+    // This warning is a false positive
-+    #pragma GCC diagnostic ignored "-Warray-bounds"
-+  #endif
+@@ -81,6 +81,15 @@
+ // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
+ #pragma GCC diagnostic ignored "-Wattributes"
+ #endif
++#if __GNUC__>=8
++#pragma GCC diagnostic ignored "-Wclass-memaccess"
++#endif
++#if __GNUC__>=11
++#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
++#endif
++#if __GNUC__>=12
++#pragma GCC diagnostic ignored "-Warray-bounds"
++#endif
  #endif
  
  #if defined __NVCC__
diff --git a/third_party/allwpilib/upstream_utils/eigen_patches/0002-Intellisense-fix.patch b/third_party/allwpilib/upstream_utils/eigen_patches/0002-Intellisense-fix.patch
index ce4cdb6..f108fa3 100644
--- a/third_party/allwpilib/upstream_utils/eigen_patches/0002-Intellisense-fix.patch
+++ b/third_party/allwpilib/upstream_utils/eigen_patches/0002-Intellisense-fix.patch
@@ -1,17 +1,17 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Fri, 20 Jan 2023 23:41:56 -0800
-Subject: [PATCH 2/3] Intellisense fix
+Subject: [PATCH 2/2] Intellisense fix
 
 ---
  Eigen/src/Core/util/Macros.h | 10 ++++++++++
  1 file changed, 10 insertions(+)
 
 diff --git a/Eigen/src/Core/util/Macros.h b/Eigen/src/Core/util/Macros.h
-index 986c3d44db94c8ba339792b6738c47cdd2c5acbc..81986b9447824c440e004f38a220393ef5a089c6 100644
+index 5d054e67c4355a549b1351f1ecc5e49a96e91ace..fda6ad91eedacc9958a5a0bc34db33d8b4e57cd6 100644
 --- a/Eigen/src/Core/util/Macros.h
 +++ b/Eigen/src/Core/util/Macros.h
-@@ -58,6 +58,16 @@
+@@ -60,6 +60,16 @@
  // Compiler identification, EIGEN_COMP_*
  //------------------------------------------------------------------------------------------
  
@@ -25,6 +25,6 @@
 +#endif
 +#endif
 +
- /// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
+ /// \internal EIGEN_COMP_GNUC set to version (e.g., 951 for GCC 9.5.1) for all compilers compatible with GCC
  #ifdef __GNUC__
-   #define EIGEN_COMP_GNUC (__GNUC__*10+__GNUC_MINOR__)
+ #define EIGEN_COMP_GNUC (__GNUC__ * 100 + __GNUC_MINOR__ * 10 + __GNUC_PATCHLEVEL__)
diff --git a/third_party/allwpilib/upstream_utils/eigen_patches/0003-Eigen-Sparse-fix-warnings-Wunused-but-set-variable.patch b/third_party/allwpilib/upstream_utils/eigen_patches/0003-Eigen-Sparse-fix-warnings-Wunused-but-set-variable.patch
deleted file mode 100644
index d68f094..0000000
--- a/third_party/allwpilib/upstream_utils/eigen_patches/0003-Eigen-Sparse-fix-warnings-Wunused-but-set-variable.patch
+++ /dev/null
@@ -1,65 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Laurent Rineau <laurent.rineau@cgal.org>
-Date: Tue, 11 Oct 2022 17:37:04 +0000
-Subject: [PATCH 3/3] Eigen/Sparse: fix warnings -Wunused-but-set-variable
-
----
- Eigen/src/SparseCore/TriangularSolver.h        | 4 ++--
- Eigen/src/SparseLU/SparseLU_heap_relax_snode.h | 5 -----
- 2 files changed, 2 insertions(+), 7 deletions(-)
-
-diff --git a/Eigen/src/SparseCore/TriangularSolver.h b/Eigen/src/SparseCore/TriangularSolver.h
-index f9c56ba79800e209dcf3f18ba37dbb8023488bca..7cb2c2665f0e24924da88f11c0fe3ca0c0af52e3 100644
---- a/Eigen/src/SparseCore/TriangularSolver.h
-+++ b/Eigen/src/SparseCore/TriangularSolver.h
-@@ -270,11 +270,11 @@ struct sparse_solve_triangular_sparse_selector<Lhs,Rhs,Mode,UpLo,ColMajor>
-       }
- 
- 
--      Index count = 0;
-+//       Index count = 0;
-       // FIXME compute a reference value to filter zeros
-       for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
-       {
--        ++ count;
-+//         ++ count;
- //         std::cerr << "fill " << it.index() << ", " << col << "\n";
- //         std::cout << it.value() << "  ";
-         // FIXME use insertBack
-diff --git a/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
-index 6f75d500e5f831f414175ce46dbceffa0acd5539..7aecbcad8ed2703000d62cfd5d88d983c69a7423 100644
---- a/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
-+++ b/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
-@@ -75,8 +75,6 @@ void SparseLUImpl<Scalar,StorageIndex>::heap_relax_snode (const Index n, IndexVe
-   // Identify the relaxed supernodes by postorder traversal of the etree
-   Index snode_start; // beginning of a snode 
-   StorageIndex k;
--  Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree 
--  Index nsuper_et = 0; // Number of relaxed snodes in the original etree 
-   StorageIndex l; 
-   for (j = 0; j < n; )
-   {
-@@ -88,7 +86,6 @@ void SparseLUImpl<Scalar,StorageIndex>::heap_relax_snode (const Index n, IndexVe
-       parent = et(j);
-     }
-     // Found a supernode in postordered etree, j is the last column 
--    ++nsuper_et_post;
-     k = StorageIndex(n);
-     for (Index i = snode_start; i <= j; ++i)
-       k = (std::min)(k, inv_post(i));
-@@ -97,7 +94,6 @@ void SparseLUImpl<Scalar,StorageIndex>::heap_relax_snode (const Index n, IndexVe
-     {
-       // This is also a supernode in the original etree
-       relax_end(k) = l; // Record last column 
--      ++nsuper_et; 
-     }
-     else 
-     {
-@@ -107,7 +103,6 @@ void SparseLUImpl<Scalar,StorageIndex>::heap_relax_snode (const Index n, IndexVe
-         if (descendants(i) == 0) 
-         {
-           relax_end(l) = l;
--          ++nsuper_et;
-         }
-       }
-     }
diff --git a/third_party/allwpilib/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch b/third_party/allwpilib/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch
index 5b05dbf..5670adb 100644
--- a/third_party/allwpilib/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch
+++ b/third_party/allwpilib/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 18 May 2022 10:21:49 -0700
-Subject: [PATCH 1/2] Don't throw on write failure
+Subject: [PATCH 1/3] Don't throw on write failure
 
 ---
  include/fmt/format-inl.h | 4 +---
diff --git a/third_party/allwpilib/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch b/third_party/allwpilib/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch
index a3866e2..abc08e6 100644
--- a/third_party/allwpilib/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch
+++ b/third_party/allwpilib/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch
@@ -1,14 +1,14 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Tue, 16 May 2023 13:49:18 -0700
-Subject: [PATCH 2/2] Suppress warnings we can't fix
+Subject: [PATCH 2/3] Suppress warnings we can't fix
 
 ---
  include/fmt/format.h | 7 +++++++
  1 file changed, 7 insertions(+)
 
 diff --git a/include/fmt/format.h b/include/fmt/format.h
-index e5bd8b110efe49e12a12b004ea246a4dba671a6f..f11be0d6d58f3d992d7d06adb3d9576f81ecfe11 100644
+index 87a34b972ce6af4e2209e4d6cf78e8401e8f0037..ac0f52def2f3e2bc88d11903f5532efd89af454d 100644
 --- a/include/fmt/format.h
 +++ b/include/fmt/format.h
 @@ -1324,7 +1324,14 @@ inline auto equal2(const char* lhs, const char* rhs) -> bool {
diff --git a/third_party/allwpilib/upstream_utils/fmt_patches/0003-Remove-this-from-decltype.patch b/third_party/allwpilib/upstream_utils/fmt_patches/0003-Remove-this-from-decltype.patch
new file mode 100644
index 0000000..b1ed6be
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/fmt_patches/0003-Remove-this-from-decltype.patch
@@ -0,0 +1,39 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Peter Johnson <johnson.peter@gmail.com>
+Date: Mon, 20 Nov 2023 23:29:02 -0800
+Subject: [PATCH 3/3] Remove this from decltype
+
+This doesn't work on the most recent version of MSVC.
+---
+ include/fmt/core.h | 6 +++---
+ 1 file changed, 3 insertions(+), 3 deletions(-)
+
+diff --git a/include/fmt/core.h b/include/fmt/core.h
+index 1fe13888a00263a085032272482475e5dafbef26..915d8952d5f6d5255f5a1aab956ef56245b80dc4 100644
+--- a/include/fmt/core.h
++++ b/include/fmt/core.h
+@@ -1435,7 +1435,7 @@ template <typename Context> struct arg_mapper {
+   // Only map owning types because mapping views can be unsafe.
+   template <typename T, typename U = format_as_t<T>,
+             FMT_ENABLE_IF(std::is_arithmetic<U>::value)>
+-  FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(this->map(U())) {
++  FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(map(U())) {
+     return map(format_as(val));
+   }
+ 
+@@ -1459,13 +1459,13 @@ template <typename Context> struct arg_mapper {
+                           !is_string<U>::value && !is_char<U>::value &&
+                           !is_named_arg<U>::value &&
+                           !std::is_arithmetic<format_as_t<U>>::value)>
+-  FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(this->do_map(val)) {
++  FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(do_map(val)) {
+     return do_map(val);
+   }
+ 
+   template <typename T, FMT_ENABLE_IF(is_named_arg<T>::value)>
+   FMT_CONSTEXPR FMT_INLINE auto map(const T& named_arg)
+-      -> decltype(this->map(named_arg.value)) {
++      -> decltype(map(named_arg.value)) {
+     return map(named_arg.value);
+   }
+ 
diff --git a/third_party/allwpilib/upstream_utils/gcem_patches/0001-Call-std-functions-if-not-constant-evaluated.patch b/third_party/allwpilib/upstream_utils/gcem_patches/0001-Call-std-functions-if-not-constant-evaluated.patch
new file mode 100644
index 0000000..b7742f0
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/gcem_patches/0001-Call-std-functions-if-not-constant-evaluated.patch
@@ -0,0 +1,1933 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Sun, 26 Nov 2023 18:09:12 -0800
+Subject: [PATCH 1/2] Call std functions if not constant-evaluated
+
+The subheaders now have namespace declarations so C++ standard library
+headers can be included in the global namespace. Doing otherwise can
+cause ambiguous declarations of cmath functions.
+---
+ include/gcem.hpp                              | 151 +++++++++---------
+ include/gcem_incl/abs.hpp                     |  12 ++
+ include/gcem_incl/acos.hpp                    |  12 ++
+ include/gcem_incl/acosh.hpp                   |  12 ++
+ include/gcem_incl/asin.hpp                    |  12 ++
+ include/gcem_incl/asinh.hpp                   |  11 ++
+ include/gcem_incl/atan.hpp                    |  12 ++
+ include/gcem_incl/atan2.hpp                   |  12 ++
+ include/gcem_incl/atanh.hpp                   |  12 ++
+ include/gcem_incl/beta.hpp                    |  16 ++
+ include/gcem_incl/binomial_coef.hpp           |   7 +-
+ include/gcem_incl/ceil.hpp                    |  12 ++
+ include/gcem_incl/copysign.hpp                |  12 ++
+ include/gcem_incl/cos.hpp                     |  12 ++
+ include/gcem_incl/cosh.hpp                    |  12 ++
+ include/gcem_incl/erf.hpp                     |  12 ++
+ include/gcem_incl/erf_inv.hpp                 |   4 +
+ include/gcem_incl/exp.hpp                     |  12 ++
+ include/gcem_incl/expm1.hpp                   |  12 ++
+ include/gcem_incl/factorial.hpp               |   5 +
+ include/gcem_incl/find_exponent.hpp           |   5 +
+ include/gcem_incl/find_fraction.hpp           |   5 +
+ include/gcem_incl/find_whole.hpp              |   5 +
+ include/gcem_incl/floor.hpp                   |  12 ++
+ include/gcem_incl/fmod.hpp                    |  12 ++
+ include/gcem_incl/gcd.hpp                     |   5 +
+ include/gcem_incl/hypot.hpp                   |  12 ++
+ include/gcem_incl/incomplete_beta.hpp         |   5 +
+ include/gcem_incl/incomplete_beta_inv.hpp     |   5 +
+ include/gcem_incl/incomplete_gamma.hpp        |   5 +
+ include/gcem_incl/incomplete_gamma_inv.hpp    |   5 +
+ include/gcem_incl/inv_sqrt.hpp                |   5 +
+ include/gcem_incl/is_even.hpp                 |   5 +
+ include/gcem_incl/is_finite.hpp               |   5 +
+ include/gcem_incl/is_inf.hpp                  |   5 +
+ include/gcem_incl/is_nan.hpp                  |   5 +
+ include/gcem_incl/is_odd.hpp                  |   5 +
+ include/gcem_incl/lbeta.hpp                   |   5 +
+ include/gcem_incl/lcm.hpp                     |   5 +
+ include/gcem_incl/lgamma.hpp                  |  12 ++
+ include/gcem_incl/lmgamma.hpp                 |   5 +
+ include/gcem_incl/log.hpp                     |  12 ++
+ include/gcem_incl/log10.hpp                   |  12 ++
+ include/gcem_incl/log1p.hpp                   |  12 ++
+ include/gcem_incl/log2.hpp                    |  12 ++
+ include/gcem_incl/log_binomial_coef.hpp       |   7 +-
+ include/gcem_incl/mantissa.hpp                |   5 +
+ include/gcem_incl/max.hpp                     |  12 ++
+ include/gcem_incl/min.hpp                     |  12 ++
+ include/gcem_incl/neg_zero.hpp                |   5 +
+ include/gcem_incl/pow.hpp                     |  12 ++
+ include/gcem_incl/pow_integral.hpp            |   5 +
+ .../quadrature/gauss_legendre_30.hpp          |   5 +
+ .../quadrature/gauss_legendre_50.hpp          |   5 +
+ include/gcem_incl/round.hpp                   |  12 ++
+ include/gcem_incl/sgn.hpp                     |   5 +
+ include/gcem_incl/signbit.hpp                 |  12 ++
+ include/gcem_incl/sin.hpp                     |  12 ++
+ include/gcem_incl/sinh.hpp                    |  12 ++
+ include/gcem_incl/sqrt.hpp                    |  12 ++
+ include/gcem_incl/tan.hpp                     |  12 ++
+ include/gcem_incl/tanh.hpp                    |  12 ++
+ include/gcem_incl/tgamma.hpp                  |  12 ++
+ include/gcem_incl/trunc.hpp                   |  12 ++
+ 64 files changed, 645 insertions(+), 79 deletions(-)
+
+diff --git a/include/gcem.hpp b/include/gcem.hpp
+index 650d05d894786b123aefbc6d88c93604164d56d2..8ea7b2a279ecf4dcf684a39ee7e96db84e61a421 100644
+--- a/include/gcem.hpp
++++ b/include/gcem.hpp
+@@ -23,82 +23,79 @@
+ 
+ #include "gcem_incl/gcem_options.hpp"
+ 
+-namespace gcem
+-{
+-    #include "gcem_incl/quadrature/gauss_legendre_50.hpp"
+-
+-    #include "gcem_incl/is_inf.hpp"
+-    #include "gcem_incl/is_nan.hpp"
+-    #include "gcem_incl/is_finite.hpp"
+-    
+-    #include "gcem_incl/signbit.hpp"
+-    #include "gcem_incl/copysign.hpp"
+-    #include "gcem_incl/neg_zero.hpp"
+-    #include "gcem_incl/sgn.hpp"
+-
+-    #include "gcem_incl/abs.hpp"
+-    #include "gcem_incl/ceil.hpp"
+-    #include "gcem_incl/floor.hpp"
+-    #include "gcem_incl/trunc.hpp"
+-    #include "gcem_incl/is_odd.hpp"
+-    #include "gcem_incl/is_even.hpp"
+-    #include "gcem_incl/max.hpp"
+-    #include "gcem_incl/min.hpp"
+-    #include "gcem_incl/sqrt.hpp"
+-    #include "gcem_incl/inv_sqrt.hpp"
+-    #include "gcem_incl/hypot.hpp"
+-
+-    #include "gcem_incl/find_exponent.hpp"
+-    #include "gcem_incl/find_fraction.hpp"
+-    #include "gcem_incl/find_whole.hpp"
+-    #include "gcem_incl/mantissa.hpp"
+-    #include "gcem_incl/round.hpp"
+-    #include "gcem_incl/fmod.hpp"
+-
+-    #include "gcem_incl/pow_integral.hpp"
+-    #include "gcem_incl/exp.hpp"
+-    #include "gcem_incl/expm1.hpp"
+-    #include "gcem_incl/log.hpp"
+-    #include "gcem_incl/log1p.hpp"
+-    #include "gcem_incl/log2.hpp"
+-    #include "gcem_incl/log10.hpp"
+-    #include "gcem_incl/pow.hpp"
+-
+-    #include "gcem_incl/gcd.hpp"
+-    #include "gcem_incl/lcm.hpp"
+-
+-    #include "gcem_incl/tan.hpp"
+-    #include "gcem_incl/cos.hpp"
+-    #include "gcem_incl/sin.hpp"
+-
+-    #include "gcem_incl/atan.hpp"
+-    #include "gcem_incl/atan2.hpp"
+-    #include "gcem_incl/acos.hpp"
+-    #include "gcem_incl/asin.hpp"
+-
+-    #include "gcem_incl/tanh.hpp"
+-    #include "gcem_incl/cosh.hpp"
+-    #include "gcem_incl/sinh.hpp"
+-
+-    #include "gcem_incl/atanh.hpp"
+-    #include "gcem_incl/acosh.hpp"
+-    #include "gcem_incl/asinh.hpp"
+-
+-    #include "gcem_incl/binomial_coef.hpp"
+-    #include "gcem_incl/lgamma.hpp"
+-    #include "gcem_incl/tgamma.hpp"
+-    #include "gcem_incl/factorial.hpp"
+-    #include "gcem_incl/lbeta.hpp"
+-    #include "gcem_incl/beta.hpp"
+-    #include "gcem_incl/lmgamma.hpp"
+-    #include "gcem_incl/log_binomial_coef.hpp"
+-
+-    #include "gcem_incl/erf.hpp"
+-    #include "gcem_incl/erf_inv.hpp"
+-    #include "gcem_incl/incomplete_beta.hpp"
+-    #include "gcem_incl/incomplete_beta_inv.hpp"
+-    #include "gcem_incl/incomplete_gamma.hpp"
+-    #include "gcem_incl/incomplete_gamma_inv.hpp"
+-}
++#include "gcem_incl/quadrature/gauss_legendre_50.hpp"
++
++#include "gcem_incl/is_inf.hpp"
++#include "gcem_incl/is_nan.hpp"
++#include "gcem_incl/is_finite.hpp"
++
++#include "gcem_incl/signbit.hpp"
++#include "gcem_incl/copysign.hpp"
++#include "gcem_incl/neg_zero.hpp"
++#include "gcem_incl/sgn.hpp"
++
++#include "gcem_incl/abs.hpp"
++#include "gcem_incl/ceil.hpp"
++#include "gcem_incl/floor.hpp"
++#include "gcem_incl/trunc.hpp"
++#include "gcem_incl/is_odd.hpp"
++#include "gcem_incl/is_even.hpp"
++#include "gcem_incl/max.hpp"
++#include "gcem_incl/min.hpp"
++#include "gcem_incl/sqrt.hpp"
++#include "gcem_incl/inv_sqrt.hpp"
++#include "gcem_incl/hypot.hpp"
++
++#include "gcem_incl/find_exponent.hpp"
++#include "gcem_incl/find_fraction.hpp"
++#include "gcem_incl/find_whole.hpp"
++#include "gcem_incl/mantissa.hpp"
++#include "gcem_incl/round.hpp"
++#include "gcem_incl/fmod.hpp"
++
++#include "gcem_incl/pow_integral.hpp"
++#include "gcem_incl/exp.hpp"
++#include "gcem_incl/expm1.hpp"
++#include "gcem_incl/log.hpp"
++#include "gcem_incl/log1p.hpp"
++#include "gcem_incl/log2.hpp"
++#include "gcem_incl/log10.hpp"
++#include "gcem_incl/pow.hpp"
++
++#include "gcem_incl/gcd.hpp"
++#include "gcem_incl/lcm.hpp"
++
++#include "gcem_incl/tan.hpp"
++#include "gcem_incl/cos.hpp"
++#include "gcem_incl/sin.hpp"
++
++#include "gcem_incl/atan.hpp"
++#include "gcem_incl/atan2.hpp"
++#include "gcem_incl/acos.hpp"
++#include "gcem_incl/asin.hpp"
++
++#include "gcem_incl/tanh.hpp"
++#include "gcem_incl/cosh.hpp"
++#include "gcem_incl/sinh.hpp"
++
++#include "gcem_incl/atanh.hpp"
++#include "gcem_incl/acosh.hpp"
++#include "gcem_incl/asinh.hpp"
++
++#include "gcem_incl/binomial_coef.hpp"
++#include "gcem_incl/lgamma.hpp"
++#include "gcem_incl/tgamma.hpp"
++#include "gcem_incl/factorial.hpp"
++#include "gcem_incl/lbeta.hpp"
++#include "gcem_incl/beta.hpp"
++#include "gcem_incl/lmgamma.hpp"
++#include "gcem_incl/log_binomial_coef.hpp"
++
++#include "gcem_incl/erf.hpp"
++#include "gcem_incl/erf_inv.hpp"
++#include "gcem_incl/incomplete_beta.hpp"
++#include "gcem_incl/incomplete_beta_inv.hpp"
++#include "gcem_incl/incomplete_gamma.hpp"
++#include "gcem_incl/incomplete_gamma_inv.hpp"
+ 
+ #endif
+diff --git a/include/gcem_incl/abs.hpp b/include/gcem_incl/abs.hpp
+index 6d7b66d91bc9f4d0c05fd0c3dd1571b552d8a19a..8e8330b198a206ea4f422d6782c66fe92bdd6522 100644
+--- a/include/gcem_incl/abs.hpp
++++ b/include/gcem_incl/abs.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_abs_HPP
+ #define _gcem_abs_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ /**
+  * Compile-time absolute value function
+  *
+@@ -34,12 +40,18 @@ T
+ abs(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return( // deal with signed-zeros
+             x == T(0) ? \
+                 T(0) :
+             // else
+             x < T(0) ? \
+                 - x : x );
++  } else {
++    return std::abs(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/acos.hpp b/include/gcem_incl/acos.hpp
+index a47003d61d8ab2fb7dd0226410e1457809a73458..2e24d2acb61b5a5b647e707950cded8850b0ea66 100644
+--- a/include/gcem_incl/acos.hpp
++++ b/include/gcem_incl/acos.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_acos_HPP
+ #define _gcem_acos_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -78,7 +84,13 @@ return_t<T>
+ acos(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::acos_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::acos(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/acosh.hpp b/include/gcem_incl/acosh.hpp
+index 8767200778168aeb409f16945b2ceac75b69158d..778c36cad6ebd849696aaa68d2697a5145a410a3 100644
+--- a/include/gcem_incl/acosh.hpp
++++ b/include/gcem_incl/acosh.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_acosh_HPP
+ #define _gcem_acosh_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -62,7 +68,13 @@ return_t<T>
+ acosh(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::acosh_compute( static_cast<return_t<T>>(x) );
++  } else {
++    return std::acosh(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/asin.hpp b/include/gcem_incl/asin.hpp
+index 6a79e87df435c7e60b140b7c7133b22deea7fd73..39784aa10eb07ea9328ab819a1f396dd036719b4 100644
+--- a/include/gcem_incl/asin.hpp
++++ b/include/gcem_incl/asin.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_asin_HPP
+ #define _gcem_asin_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -76,7 +82,13 @@ return_t<T>
+ asin(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::asin_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::asin(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/asinh.hpp b/include/gcem_incl/asinh.hpp
+index a5f3ff62a0208d350f4027d736ce3fc035c16ded..0a5969369a7a7856e41de1d891d571d5dd3c481f 100644
+--- a/include/gcem_incl/asinh.hpp
++++ b/include/gcem_incl/asinh.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_asinh_HPP
+ #define _gcem_asinh_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -59,8 +65,13 @@ return_t<T>
+ asinh(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::asinh_compute( static_cast<return_t<T>>(x) );
++  } else {
++    return std::asinh(x);
++  }
+ }
+ 
++}
+ 
+ #endif
+diff --git a/include/gcem_incl/atan.hpp b/include/gcem_incl/atan.hpp
+index 3f46974c07920fca6396e42e93261d8167a27eb1..19b9d4e10d75b067527133060d3b07719f6cfb4c 100644
+--- a/include/gcem_incl/atan.hpp
++++ b/include/gcem_incl/atan.hpp
+@@ -29,6 +29,12 @@
+ #ifndef _gcem_atan_HPP
+ #define _gcem_atan_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -149,7 +155,13 @@ return_t<T>
+ atan(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::atan_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::atan(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/atan2.hpp b/include/gcem_incl/atan2.hpp
+index 5ca55b79c8e11569511fb6d4842c74be50c70c67..07a02cf4a2e134705b514a1031d1d7f47294bdf1 100644
+--- a/include/gcem_incl/atan2.hpp
++++ b/include/gcem_incl/atan2.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_atan2_HPP
+ #define _gcem_atan2_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -82,7 +88,13 @@ common_return_t<T1,T2>
+ atan2(const T1 y, const T2 x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::atan2_type_check(x,y);
++  } else {
++    return std::atan2(y, x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/atanh.hpp b/include/gcem_incl/atanh.hpp
+index dfb4dc312d479e2c6dd837d2391bc658e3b678ed..5c4475bead1034c1b5b1583ad3050a85d380d629 100644
+--- a/include/gcem_incl/atanh.hpp
++++ b/include/gcem_incl/atanh.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_atanh_HPP
+ #define _gcem_atanh_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -73,7 +79,13 @@ return_t<T>
+ atanh(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::atanh_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::atanh(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/beta.hpp b/include/gcem_incl/beta.hpp
+index e43d4fcd9a10e37e16652351ec7092cb7200f808..3439af1b57e0135eb1c53a15d14dbbbfbe57bc13 100644
+--- a/include/gcem_incl/beta.hpp
++++ b/include/gcem_incl/beta.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_beta_HPP
+ #define _gcem_beta_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ /**
+  * Compile-time beta function
+  *
+@@ -36,7 +42,17 @@ common_return_t<T1,T2>
+ beta(const T1 a, const T2 b)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
++    return exp( lbeta(a,b) );
++  } else {
++#ifdef __cpp_lib_math_special_functions
++    return std::beta(a, b);
++#else
+     return exp( lbeta(a,b) );
++#endif
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/binomial_coef.hpp b/include/gcem_incl/binomial_coef.hpp
+index 0fc17f384d708b4bd81905d49b20f8037c4701ae..a1f764b97b78cbaf8fb66117dab374dc55f04d49 100644
+--- a/include/gcem_incl/binomial_coef.hpp
++++ b/include/gcem_incl/binomial_coef.hpp
+@@ -21,6 +21,9 @@
+ #ifndef _gcem_binomial_coef_HPP
+ #define _gcem_binomial_coef_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -88,4 +91,6 @@ noexcept
+     return internal::binomial_coef_type_check(n,k);
+ }
+ 
+-#endif
+\ No newline at end of file
++}
++
++#endif
+diff --git a/include/gcem_incl/ceil.hpp b/include/gcem_incl/ceil.hpp
+index ff1097bac2f53c3f6a4b3d4bfc612e0fcd34a63b..2611e6560bfefc4efe782849c88d74bc3ebf9d76 100644
+--- a/include/gcem_incl/ceil.hpp
++++ b/include/gcem_incl/ceil.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_ceil_HPP
+ #define _gcem_ceil_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -124,7 +130,13 @@ return_t<T>
+ ceil(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::ceil_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::ceil(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/copysign.hpp b/include/gcem_incl/copysign.hpp
+index c1741f7f321110ff3cf52cb9035d5f2dca5f0ffc..d1181995c7d0fd92668c766f7fd9288c17312159 100644
+--- a/include/gcem_incl/copysign.hpp
++++ b/include/gcem_incl/copysign.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_copysign_HPP
+ #define _gcem_copysign_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ /**
+  * Compile-time copy sign function
+  *
+@@ -35,7 +41,13 @@ T1
+ copysign(const T1 x, const T2 y)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return( signbit(x) != signbit(y) ? -x : x );
++  } else {
++    return std::copysign(x, y);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/cos.hpp b/include/gcem_incl/cos.hpp
+index 82f4c609f5268d19771e281987ed1653e81ef173..be73b55e3f2023538d33eedc78c9ed3e93014384 100644
+--- a/include/gcem_incl/cos.hpp
++++ b/include/gcem_incl/cos.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_cos_HPP
+ #define _gcem_cos_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -77,7 +83,13 @@ return_t<T>
+ cos(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::cos_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::cos(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/cosh.hpp b/include/gcem_incl/cosh.hpp
+index fc89c0d06482a687442998da511eb1e7b17f95d5..53e5bb12641b4f13d43aee7a77c0d02bf845688a 100644
+--- a/include/gcem_incl/cosh.hpp
++++ b/include/gcem_incl/cosh.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_cosh_HPP
+ #define _gcem_cosh_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -59,7 +65,13 @@ return_t<T>
+ cosh(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::cosh_compute( static_cast<return_t<T>>(x) );
++  } else {
++    return std::cosh(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/erf.hpp b/include/gcem_incl/erf.hpp
+index d0bc83ada14af7a4b50262541c8558016823303a..319669d7f0d0bfd0cd91e3bd94e9bbd0c92c66ea 100644
+--- a/include/gcem_incl/erf.hpp
++++ b/include/gcem_incl/erf.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_erf_HPP
+ #define _gcem_erf_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -137,7 +143,13 @@ return_t<T>
+ erf(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::erf_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::erf(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/erf_inv.hpp b/include/gcem_incl/erf_inv.hpp
+index 412d686be268dd7668ca517e10e5e4be4263f0d7..c4f0d9d5abb46172fda7c6702bc31561f5680a37 100644
+--- a/include/gcem_incl/erf_inv.hpp
++++ b/include/gcem_incl/erf_inv.hpp
+@@ -28,6 +28,9 @@
+ #ifndef _gcem_erf_inv_HPP
+ #define _gcem_erf_inv_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -260,5 +263,6 @@ noexcept
+     return internal::erf_inv_begin( static_cast<return_t<T>>(p) );
+ }
+ 
++}
+ 
+ #endif
+diff --git a/include/gcem_incl/exp.hpp b/include/gcem_incl/exp.hpp
+index 595ffc2a6202d763560fd879f27fcef32c6b2d64..1708542df5153aeb6c2366078f32660ea002d51c 100644
+--- a/include/gcem_incl/exp.hpp
++++ b/include/gcem_incl/exp.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_exp_HPP
+ #define _gcem_exp_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -124,7 +130,13 @@ return_t<T>
+ exp(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::exp_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::exp(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/expm1.hpp b/include/gcem_incl/expm1.hpp
+index 70c9ecf5a4a241bfef617e6e4ee0000a80978355..6adddc929aff7c87395a6b78070c65646ad61f2f 100644
+--- a/include/gcem_incl/expm1.hpp
++++ b/include/gcem_incl/expm1.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_expm1_HPP
+ #define _gcem_expm1_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -70,7 +76,13 @@ return_t<T>
+ expm1(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::expm1_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::expm1(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/factorial.hpp b/include/gcem_incl/factorial.hpp
+index ffb9c824751a572c0982e11b9cddbe40cb567a3f..977e6970e4479ab39e65affbdd400827d29e7809 100644
+--- a/include/gcem_incl/factorial.hpp
++++ b/include/gcem_incl/factorial.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_factorial_HPP
+ #define _gcem_factorial_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -95,4 +98,6 @@ noexcept
+     return internal::factorial_recur(x);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/find_exponent.hpp b/include/gcem_incl/find_exponent.hpp
+index 200e4e9be1558a61ed608be290bde5a6921363ea..4ffbcf294e61aa847d3e769e50ad9c3eddd8927a 100644
+--- a/include/gcem_incl/find_exponent.hpp
++++ b/include/gcem_incl/find_exponent.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_find_exponent_HPP
+ #define _gcem_find_exponent_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -54,4 +57,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/find_fraction.hpp b/include/gcem_incl/find_fraction.hpp
+index 5ed3d269ab54b3c90b3d681b76e8420e89e4527f..76893135383e5f6f5993d335c7b232114fdd90e3 100644
+--- a/include/gcem_incl/find_fraction.hpp
++++ b/include/gcem_incl/find_fraction.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_find_fraction_HPP
+ #define _gcem_find_fraction_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -43,4 +46,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/find_whole.hpp b/include/gcem_incl/find_whole.hpp
+index d19363219871803f8cfeafadb0a88189d2f98f0e..561050e91f9e43faf4d80fd08b7e63537b622422 100644
+--- a/include/gcem_incl/find_whole.hpp
++++ b/include/gcem_incl/find_whole.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_find_whole_HPP
+ #define _gcem_find_whole_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -43,4 +46,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/floor.hpp b/include/gcem_incl/floor.hpp
+index 8b260ff8a10fb8b9d048307c7d9b18a7e91900c1..3b3b86a12aa49e1c51957d170744a65690aeefdc 100644
+--- a/include/gcem_incl/floor.hpp
++++ b/include/gcem_incl/floor.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_floor_HPP
+ #define _gcem_floor_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -124,7 +130,13 @@ return_t<T>
+ floor(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::floor_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::floor(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/fmod.hpp b/include/gcem_incl/fmod.hpp
+index 02459efd4b3a016939a306c9fd1a7ead12ff123b..58c454455edf132c078779ec15e254c96228b514 100644
+--- a/include/gcem_incl/fmod.hpp
++++ b/include/gcem_incl/fmod.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_fmod_HPP
+ #define _gcem_fmod_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -64,7 +70,13 @@ common_return_t<T1,T2>
+ fmod(const T1 x, const T2 y)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::fmod_type_check(x,y);
++  } else {
++    return std::fmod(x, y);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/gcd.hpp b/include/gcem_incl/gcd.hpp
+index 1e277fbab61a06a20758f55d2c71ca57037f9d57..c5b14a22aafe7738dd14d120cab4dc6b31a5ad91 100644
+--- a/include/gcem_incl/gcd.hpp
++++ b/include/gcem_incl/gcd.hpp
+@@ -21,6 +21,9 @@
+ #ifndef _gcem_gcd_HPP
+ #define _gcem_gcd_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -79,4 +82,6 @@ noexcept
+     return internal::gcd_type_check(a,b);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/hypot.hpp b/include/gcem_incl/hypot.hpp
+index 01ad4e9212547c649113e50954a73ee9f7394334..00e10f899ace8f0da925fa9e46fa3f79f7e83aa0 100644
+--- a/include/gcem_incl/hypot.hpp
++++ b/include/gcem_incl/hypot.hpp
+@@ -27,6 +27,12 @@
+ #ifndef _gcem_hypot_HPP
+ #define _gcem_hypot_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -84,7 +90,13 @@ common_return_t<T1,T2>
+ hypot(const T1 x, const T2 y)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::hypot_type_check(x,y);
++  } else {
++    return std::hypot(x, y);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/incomplete_beta.hpp b/include/gcem_incl/incomplete_beta.hpp
+index dbb9f60341b09102b137de2b9ded75280a315d57..681cc59953f0f2b1bd3603216286edce06ce46cc 100644
+--- a/include/gcem_incl/incomplete_beta.hpp
++++ b/include/gcem_incl/incomplete_beta.hpp
+@@ -27,6 +27,9 @@
+ #ifndef _gcem_incomplete_beta_HPP
+ #define _gcem_incomplete_beta_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -191,4 +194,6 @@ noexcept
+     return internal::incomplete_beta_type_check(a,b,z);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/incomplete_beta_inv.hpp b/include/gcem_incl/incomplete_beta_inv.hpp
+index 9f575a360427d8b4ba9ebb940c2baf7ee242251c..3878d37db3bbef56e14bc053b9577b4f96c6c63c 100644
+--- a/include/gcem_incl/incomplete_beta_inv.hpp
++++ b/include/gcem_incl/incomplete_beta_inv.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_incomplete_beta_inv_HPP
+ #define _gcem_incomplete_beta_inv_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -349,4 +352,6 @@ noexcept
+     return internal::incomplete_beta_inv_type_check(a,b,p);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/incomplete_gamma.hpp b/include/gcem_incl/incomplete_gamma.hpp
+index 9ee4146de0aacffee1f418bd91069c921e0a6a29..0432b7d3c6f75a726847c9d84d52e387df16f618 100644
+--- a/include/gcem_incl/incomplete_gamma.hpp
++++ b/include/gcem_incl/incomplete_gamma.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_incomplete_gamma_HPP
+ #define _gcem_incomplete_gamma_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -244,4 +247,6 @@ noexcept
+     return internal::incomplete_gamma_type_check(a,x);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/incomplete_gamma_inv.hpp b/include/gcem_incl/incomplete_gamma_inv.hpp
+index e5976d08fddb5eb83ad40d200c5973d8ce2d6a36..6b1575b89c8e7284aee55882d12f1ee308d0cba3 100644
+--- a/include/gcem_incl/incomplete_gamma_inv.hpp
++++ b/include/gcem_incl/incomplete_gamma_inv.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_incomplete_gamma_inv_HPP
+ #define _gcem_incomplete_gamma_inv_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -268,4 +271,6 @@ noexcept
+     return internal::incomplete_gamma_inv_type_check(a,p);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/inv_sqrt.hpp b/include/gcem_incl/inv_sqrt.hpp
+index d0e33fb743b09e7c72506599414f7a4e0e7e5058..466e76fc5e844ba4b3c6be1a6a2a5f20b4e3607f 100644
+--- a/include/gcem_incl/inv_sqrt.hpp
++++ b/include/gcem_incl/inv_sqrt.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_inv_sqrt_HPP
+ #define _gcem_inv_sqrt_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -85,4 +88,6 @@ noexcept
+     return internal::inv_sqrt_check( static_cast<return_t<T>>(x) );
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/is_even.hpp b/include/gcem_incl/is_even.hpp
+index de0641d85299a77380ba204e5e1d88538106468f..5aff6a3fa634d84b3fad6c831e3bb93797daee37 100644
+--- a/include/gcem_incl/is_even.hpp
++++ b/include/gcem_incl/is_even.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_is_even_HPP
+ #define _gcem_is_even_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -38,4 +41,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/is_finite.hpp b/include/gcem_incl/is_finite.hpp
+index b632fa34728ea66b595be68d48185474c3fe775e..805cbc4ab6708a51c95186801a1b0b02e0083e92 100644
+--- a/include/gcem_incl/is_finite.hpp
++++ b/include/gcem_incl/is_finite.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_is_finite_HPP
+ #define _gcem_is_finite_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -75,4 +78,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/is_inf.hpp b/include/gcem_incl/is_inf.hpp
+index 568614f8efa0e25fad4f87e1b71080849127378d..fdc31de71a3e73fda17216fb29e962c9f8a00958 100644
+--- a/include/gcem_incl/is_inf.hpp
++++ b/include/gcem_incl/is_inf.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_is_inf_HPP
+ #define _gcem_is_inf_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -169,4 +172,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/is_nan.hpp b/include/gcem_incl/is_nan.hpp
+index a3fcbc6c7cf6593965bee84b0eda0fc148f8f7ff..74092e6b087e47df45f70c6fcb5e62da53317f29 100644
+--- a/include/gcem_incl/is_nan.hpp
++++ b/include/gcem_incl/is_nan.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_is_nan_HPP
+ #define _gcem_is_nan_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -77,4 +80,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/is_odd.hpp b/include/gcem_incl/is_odd.hpp
+index a74a8d34f20da4a060b000b58fe7abd5b53f38b0..a76802f7ffc9b0faecae4f64c981ea76116e7c2a 100644
+--- a/include/gcem_incl/is_odd.hpp
++++ b/include/gcem_incl/is_odd.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_is_odd_HPP
+ #define _gcem_is_odd_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -39,4 +42,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/lbeta.hpp b/include/gcem_incl/lbeta.hpp
+index 60c87b4f53ec120121f1755635531dc4cc5ab26c..78f6849e72f56c35896e7e00ed9c3227a98114ba 100644
+--- a/include/gcem_incl/lbeta.hpp
++++ b/include/gcem_incl/lbeta.hpp
+@@ -21,6 +21,9 @@
+ #ifndef _gcem_lbeta_HPP
+ #define _gcem_lbeta_HPP
+ 
++namespace gcem
++{
++
+ /**
+  * Compile-time log-beta function
+  *
+@@ -39,4 +42,6 @@ noexcept
+     return( (lgamma(a) + lgamma(b)) - lgamma(a+b) );
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/lcm.hpp b/include/gcem_incl/lcm.hpp
+index a7ca7766f8906c1d26c071146aba3543880d8dd9..aa1f3397309a8a4517fed63d954737e043e28d1b 100644
+--- a/include/gcem_incl/lcm.hpp
++++ b/include/gcem_incl/lcm.hpp
+@@ -21,6 +21,9 @@
+ #ifndef _gcem_lcm_HPP
+ #define _gcem_lcm_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -62,4 +65,6 @@ noexcept
+     return internal::lcm_type_check(a,b);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/lgamma.hpp b/include/gcem_incl/lgamma.hpp
+index 507c6d4c91423b31be55426f494f67b0cb085c9f..e349ee88dba2d23982a6d4485bc33da076c70e22 100644
+--- a/include/gcem_incl/lgamma.hpp
++++ b/include/gcem_incl/lgamma.hpp
+@@ -28,6 +28,12 @@
+ #ifndef _gcem_lgamma_HPP
+ #define _gcem_lgamma_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -129,7 +135,13 @@ return_t<T>
+ lgamma(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::lgamma_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::lgamma(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/lmgamma.hpp b/include/gcem_incl/lmgamma.hpp
+index 58915dc1e1a2edbbfdea58b57137cd22aaf4b717..b009de18e71c4360b55d0c1d80763f140190706f 100644
+--- a/include/gcem_incl/lmgamma.hpp
++++ b/include/gcem_incl/lmgamma.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_lmgamma_HPP
+ #define _gcem_lmgamma_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -70,4 +73,6 @@ noexcept
+     return internal::lmgamma_recur(static_cast<return_t<T1>>(a),p);
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/log.hpp b/include/gcem_incl/log.hpp
+index c2e24b006c2fdd01196b189220f35ca982d8bf9e..15577c8bacca3f4d0036997a2a34603ff56b4b24 100644
+--- a/include/gcem_incl/log.hpp
++++ b/include/gcem_incl/log.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_log_HPP
+ #define _gcem_log_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -180,7 +186,13 @@ return_t<T>
+ log(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::log_integral_check( x );
++  } else {
++    return std::log(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/log10.hpp b/include/gcem_incl/log10.hpp
+index cda8894fdd62054f11ec4c3c3e6402c3752e0dd7..0e9a4dbdf58e2ed077e2f682706d98e71d794d67 100644
+--- a/include/gcem_incl/log10.hpp
++++ b/include/gcem_incl/log10.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_log10_HPP
+ #define _gcem_log10_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -53,7 +59,13 @@ return_t<T>
+ log10(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::log10_check( x );
++  } else {
++    return std::log10(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/log1p.hpp b/include/gcem_incl/log1p.hpp
+index ccd08b8cb8799f2d4d34d5634dbc2fcabb3fd5b3..ed41daf39ec7e7062dc1da51ef0b0e106b56cf4e 100644
+--- a/include/gcem_incl/log1p.hpp
++++ b/include/gcem_incl/log1p.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_log1p_HPP
+ #define _gcem_log1p_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -74,7 +80,13 @@ return_t<T>
+ log1p(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::log1p_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::log1p(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/log2.hpp b/include/gcem_incl/log2.hpp
+index a97fed4c7236f816fa07ac2494f243adbac6742b..f8dc729f4f08348b8cc046468f40cfc84f1c9cb2 100644
+--- a/include/gcem_incl/log2.hpp
++++ b/include/gcem_incl/log2.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_log2_HPP
+ #define _gcem_log2_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -53,7 +59,13 @@ return_t<T>
+ log2(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::log2_check( x );
++  } else {
++    return std::log2(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/log_binomial_coef.hpp b/include/gcem_incl/log_binomial_coef.hpp
+index 2bcaadd8bc1c64744eb082955522339d48589aec..c4ba2fa3de9f4f77ac772d24c1432fc93cc51840 100644
+--- a/include/gcem_incl/log_binomial_coef.hpp
++++ b/include/gcem_incl/log_binomial_coef.hpp
+@@ -21,6 +21,9 @@
+ #ifndef _gcem_log_binomial_coef_HPP
+ #define _gcem_log_binomial_coef_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -62,4 +65,6 @@ noexcept
+     return internal::log_binomial_coef_type_check(n,k);
+ }
+ 
+-#endif
+\ No newline at end of file
++}
++
++#endif
+diff --git a/include/gcem_incl/mantissa.hpp b/include/gcem_incl/mantissa.hpp
+index af23ea21d247327fa224370544e5f4410eac214b..4c95110d627568577440e5e662e72fdeffd95ade 100644
+--- a/include/gcem_incl/mantissa.hpp
++++ b/include/gcem_incl/mantissa.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_mantissa_HPP
+ #define _gcem_mantissa_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -44,4 +47,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/max.hpp b/include/gcem_incl/max.hpp
+index ddc3e4e6caff1a781e662a3ded8909cb703729ab..9f3901b4b19eb35331cd22be16c5b3a1ab5f65d6 100644
+--- a/include/gcem_incl/max.hpp
++++ b/include/gcem_incl/max.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_max_HPP
+ #define _gcem_max_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ /**
+  * Compile-time pairwise maximum function
+  *
+@@ -35,7 +41,13 @@ common_t<T1,T2>
+ max(const T1 x, const T2 y)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return( y < x ? x : y );
++  } else {
++    return std::max(x, y);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/min.hpp b/include/gcem_incl/min.hpp
+index 5ce70b38e6d243267a053ec33fae31e59f6a359f..a35bcf6e2cb65f8712b873d3ef2827aca2d4d0f0 100644
+--- a/include/gcem_incl/min.hpp
++++ b/include/gcem_incl/min.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_min_HPP
+ #define _gcem_min_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ /**
+  * Compile-time pairwise minimum function
+  *
+@@ -35,7 +41,13 @@ common_t<T1,T2>
+ min(const T1 x, const T2 y)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return( y > x ? x : y );
++  } else {
++    return std::min(x, y);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/neg_zero.hpp b/include/gcem_incl/neg_zero.hpp
+index 79d24a4dd56f4c361774135bc9450c71a9f61627..90923035419d8b7a88ca46c9dcb3294f601312fb 100644
+--- a/include/gcem_incl/neg_zero.hpp
++++ b/include/gcem_incl/neg_zero.hpp
+@@ -22,6 +22,9 @@
+  * extract signbit for signed zeros
+  */
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -35,3 +38,5 @@ noexcept
+ }
+ 
+ }
++
++}
+diff --git a/include/gcem_incl/pow.hpp b/include/gcem_incl/pow.hpp
+index 3891edef0e8f1e7b64e8632f46e87fd39bdba162..372166d40644ec5403d9308b5325a36a78ad2589 100644
+--- a/include/gcem_incl/pow.hpp
++++ b/include/gcem_incl/pow.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_pow_HPP
+ #define _gcem_pow_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -76,7 +82,13 @@ common_t<T1,T2>
+ pow(const T1 base, const T2 exp_term)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::pow_check(base,exp_term);
++  } else {
++    return std::pow(base, exp_term);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/pow_integral.hpp b/include/gcem_incl/pow_integral.hpp
+index 4e671557071e6377721f3ca721a7b5875f2ffd83..589d16406576bb637963aae33e40aeddf164079a 100644
+--- a/include/gcem_incl/pow_integral.hpp
++++ b/include/gcem_incl/pow_integral.hpp
+@@ -25,6 +25,9 @@
+ #ifndef _gcem_pow_integral_HPP
+ #define _gcem_pow_integral_HPP
+ 
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -125,4 +128,6 @@ noexcept
+ 
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/quadrature/gauss_legendre_30.hpp b/include/gcem_incl/quadrature/gauss_legendre_30.hpp
+index 295f43d9522dcdd7cae358dfc5b6382cfa7bd782..a80d2bb6f9b0dbc9936f3ed3e4f3770b4c307b7d 100644
+--- a/include/gcem_incl/quadrature/gauss_legendre_30.hpp
++++ b/include/gcem_incl/quadrature/gauss_legendre_30.hpp
+@@ -22,6 +22,9 @@
+  * Gauss-Legendre quadrature: 30 points
+  */
+ 
++namespace gcem
++{
++
+ static const long double gauss_legendre_30_points[30] = \
+ {
+     -0.05147184255531769583302521316672L,
+@@ -89,3 +92,5 @@ static const long double gauss_legendre_30_weights[30] = \
+     0.00796819249616660561546588347467L,
+     0.00796819249616660561546588347467L\
+ };
++
++}
+diff --git a/include/gcem_incl/quadrature/gauss_legendre_50.hpp b/include/gcem_incl/quadrature/gauss_legendre_50.hpp
+index d4e448c56e923876f2f8627df4caa5d29eb1c501..85244daf1057aa2f07979a19765bcec3865950e2 100644
+--- a/include/gcem_incl/quadrature/gauss_legendre_50.hpp
++++ b/include/gcem_incl/quadrature/gauss_legendre_50.hpp
+@@ -22,6 +22,9 @@
+  * Gauss-Legendre quadrature: 50 points
+  */
+ 
++namespace gcem
++{
++
+ static const long double gauss_legendre_50_points[50] = \
+ {
+     -0.03109833832718887611232898966595L,
+@@ -129,3 +132,5 @@ static const long double gauss_legendre_50_weights[50] = \
+     0.00290862255315514095840072434286L,
+     0.00290862255315514095840072434286L\
+ };
++
++}
+diff --git a/include/gcem_incl/round.hpp b/include/gcem_incl/round.hpp
+index 9ac4a09f0a9e7b09a303795d30ccee32ebe841b1..10fae649f89b0320e4f38f87fa8f8ffb41d75b9f 100644
+--- a/include/gcem_incl/round.hpp
++++ b/include/gcem_incl/round.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_round_HPP
+ #define _gcem_round_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -119,7 +125,13 @@ return_t<T>
+ round(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::round_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::round(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/sgn.hpp b/include/gcem_incl/sgn.hpp
+index e2eec9e80b57e73396f3cf7f335692d88dcd752b..4c4adfa5a8d9da0e04c55195ec596ce58f63c0a8 100644
+--- a/include/gcem_incl/sgn.hpp
++++ b/include/gcem_incl/sgn.hpp
+@@ -21,6 +21,9 @@
+ #ifndef _gcem_sgn_HPP
+ #define _gcem_sgn_HPP
+ 
++namespace gcem
++{
++
+ /**
+  * Compile-time sign function
+  *
+@@ -42,4 +45,6 @@ noexcept
+                 0 );
+ }
+ 
++}
++
+ #endif
+diff --git a/include/gcem_incl/signbit.hpp b/include/gcem_incl/signbit.hpp
+index 282e24403345dccfa8bf580112c129b43df5aa6d..40ec1dd157a687112f8258f76ea2ceb62d8ae22f 100644
+--- a/include/gcem_incl/signbit.hpp
++++ b/include/gcem_incl/signbit.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_signbit_HPP
+ #define _gcem_signbit_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ /**
+  * Compile-time sign bit detection function
+  *
+@@ -34,11 +40,17 @@ bool
+ signbit(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+ #ifdef _MSC_VER
+     return( (x == T(0)) ? (_fpclass(x) == _FPCLASS_NZ) : (x < T(0)) );
+ #else
+     return GCEM_SIGNBIT(x);
+ #endif
++  } else {
++    return std::signbit(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/sin.hpp b/include/gcem_incl/sin.hpp
+index 56c8dcaf62dc22207ee7f41ec84d272e4535ab4e..c7c17600db0c8cb862c433183139d1f727fcea8b 100644
+--- a/include/gcem_incl/sin.hpp
++++ b/include/gcem_incl/sin.hpp
+@@ -27,6 +27,12 @@
+ #ifndef _gcem_sin_HPP
+ #define _gcem_sin_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -79,7 +85,13 @@ return_t<T>
+ sin(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::sin_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::sin(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/sinh.hpp b/include/gcem_incl/sinh.hpp
+index fe3ecdd0e719856c8bcc0e8d692d93c98c496942..9174d5d64a0622b56efac56fbfdc44994f78e78d 100644
+--- a/include/gcem_incl/sinh.hpp
++++ b/include/gcem_incl/sinh.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_sinh_HPP
+ #define _gcem_sinh_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -59,7 +65,13 @@ return_t<T>
+ sinh(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::sinh_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::sinh(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/sqrt.hpp b/include/gcem_incl/sqrt.hpp
+index 1b2753c99ede5189fc2272080828c83ca63a184c..201d89fa0df5680ceb53ec3aabb20213b4326325 100644
+--- a/include/gcem_incl/sqrt.hpp
++++ b/include/gcem_incl/sqrt.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_sqrt_HPP
+ #define _gcem_sqrt_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -103,7 +109,13 @@ return_t<T>
+ sqrt(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::sqrt_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::sqrt(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/tan.hpp b/include/gcem_incl/tan.hpp
+index 386cce0f412107f65183031b3458455059f55d54..08c12d4ea07ee06d1bd25bfd7cea2c3204a72e6c 100644
+--- a/include/gcem_incl/tan.hpp
++++ b/include/gcem_incl/tan.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_tan_HPP
+ #define _gcem_tan_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -134,7 +140,13 @@ return_t<T>
+ tan(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::tan_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::tan(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/tanh.hpp b/include/gcem_incl/tanh.hpp
+index 30b431886601ec9f77ca9a93a6ca62b3cabd9e57..f25dabdb0f075d5597530cb9c1c41d0ba02e3783 100644
+--- a/include/gcem_incl/tanh.hpp
++++ b/include/gcem_incl/tanh.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_tanh_HPP
+ #define _gcem_tanh_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -83,7 +89,13 @@ return_t<T>
+ tanh(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::tanh_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::tanh(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/tgamma.hpp b/include/gcem_incl/tgamma.hpp
+index deffd3a8e77c34a9ebfd5209ee32594dc5b241e2..1d9fceecf8f9e062f2f1fa6080091b64cf533849 100644
+--- a/include/gcem_incl/tgamma.hpp
++++ b/include/gcem_incl/tgamma.hpp
+@@ -25,6 +25,12 @@
+ #ifndef _gcem_tgamma_HPP
+ #define _gcem_tgamma_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -74,7 +80,13 @@ return_t<T>
+ tgamma(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::tgamma_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::tgamma(x);
++  }
++}
++
+ }
+ 
+ #endif
+diff --git a/include/gcem_incl/trunc.hpp b/include/gcem_incl/trunc.hpp
+index af3f448915f0f9a1b5f2d843596331f7874a6da5..f7a31ce7271339626127d6ca80dfb936fe53cc03 100644
+--- a/include/gcem_incl/trunc.hpp
++++ b/include/gcem_incl/trunc.hpp
+@@ -21,6 +21,12 @@
+ #ifndef _gcem_trunc_HPP
+ #define _gcem_trunc_HPP
+ 
++#include <cmath>
++#include <type_traits>
++
++namespace gcem
++{
++
+ namespace internal
+ {
+ 
+@@ -115,7 +121,13 @@ return_t<T>
+ trunc(const T x)
+ noexcept
+ {
++  if (std::is_constant_evaluated()) {
+     return internal::trunc_check( static_cast<return_t<T>>(x) );
++  } else {
++    return std::trunc(x);
++  }
++}
++
+ }
+ 
+ #endif
diff --git a/third_party/allwpilib/upstream_utils/gcem_patches/0002-Add-hypot-x-y-z.patch b/third_party/allwpilib/upstream_utils/gcem_patches/0002-Add-hypot-x-y-z.patch
new file mode 100644
index 0000000..4757239
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/gcem_patches/0002-Add-hypot-x-y-z.patch
@@ -0,0 +1,135 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Sun, 3 Dec 2023 14:03:58 -0800
+Subject: [PATCH 2/2] Add hypot(x, y, z)
+
+---
+ include/gcem_incl/hypot.hpp | 85 +++++++++++++++++++++++++++++++++++--
+ 1 file changed, 82 insertions(+), 3 deletions(-)
+
+diff --git a/include/gcem_incl/hypot.hpp b/include/gcem_incl/hypot.hpp
+index 00e10f899ace8f0da925fa9e46fa3f79f7e83aa0..13ea80c49d374c23434c1f9859bb6474184dc420 100644
+--- a/include/gcem_incl/hypot.hpp
++++ b/include/gcem_incl/hypot.hpp
+@@ -27,6 +27,7 @@
+ #ifndef _gcem_hypot_HPP
+ #define _gcem_hypot_HPP
+ 
++#include <algorithm>
+ #include <cmath>
+ #include <type_traits>
+ 
+@@ -39,10 +40,29 @@ namespace internal
+ template<typename T>
+ constexpr
+ T
+-hypot_compute(const T x, const T ydx)
++hypot_compute(const T x, const T y)
+ noexcept
+ {
+-    return abs(x) * sqrt( T(1) + (ydx * ydx) );
++    T a = std::max(abs(x), abs(y));
++    if (a) {
++      return a * sqrt((x / a) * (x / a) + (y / a) * (y / a));
++    } else {
++      return {};
++    }
++}
++
++template<typename T>
++constexpr
++T
++hypot_compute(const T x, const T y, const T z)
++noexcept
++{
++    T a = std::max({abs(x), abs(y), abs(z)});
++    if (a) {
++      return a * sqrt((x / a) * (x / a) + (y / a) * (y / a) + (z / a) * (z / a));
++    } else {
++      return {};
++    }
+ }
+ 
+ template<typename T>
+@@ -62,7 +82,35 @@ noexcept
+             GCLIM<T>::min() > abs(y) ? \
+                 abs(x) :
+             // else
+-            hypot_compute(x, y/x) );
++            hypot_compute(x, y) );
++}
++
++template<typename T>
++constexpr
++T
++hypot_vals_check(const T x, const T y, const T z)
++noexcept
++{
++    return( any_nan(x, y, z) ? \
++                GCLIM<T>::quiet_NaN() :
++            //
++            any_inf(x,y,z) ? \
++                GCLIM<T>::infinity() :
++            // indistinguishable from zero or one
++            GCLIM<T>::min() > abs(x) && GCLIM<T>::min() > abs(y) ? \
++                abs(z) :
++            GCLIM<T>::min() > abs(x) && GCLIM<T>::min() > abs(z) ? \
++                abs(y) :
++            GCLIM<T>::min() > abs(y) && GCLIM<T>::min() > abs(z) ? \
++                abs(x) :
++            GCLIM<T>::min() > abs(x) ? \
++                hypot_vals_check(y, z) :
++            GCLIM<T>::min() > abs(y) ? \
++                hypot_vals_check(x, z) :
++            GCLIM<T>::min() > abs(z) ? \
++                hypot_vals_check(x, y) :
++            // else
++            hypot_compute(x, y, z) );
+ }
+ 
+ template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+@@ -74,6 +122,15 @@ noexcept
+     return hypot_vals_check(static_cast<TC>(x),static_cast<TC>(y));
+ }
+ 
++template<typename T1, typename T2, typename T3, typename TC = common_return_t<T1,T2,T3>>
++constexpr
++TC
++hypot_type_check(const T1 x, const T2 y, const T3 z)
++noexcept
++{
++    return hypot_vals_check(static_cast<TC>(x),static_cast<TC>(y),static_cast<TC>(z));
++}
++
+ }
+ 
+ /**
+@@ -97,6 +154,28 @@ noexcept
+   }
+ }
+ 
++/**
++ * Compile-time Pythagorean addition function
++ *
++ * @param x a real-valued input.
++ * @param y a real-valued input.
++ * @param z a real-valued input.
++ * @return Computes \f$ x \oplus y \oplus z = \sqrt{x^2 + y^2 + z^2} \f$.
++ */
++
++template<typename T1, typename T2, typename T3>
++constexpr
++common_return_t<T1,T2,T3>
++hypot(const T1 x, const T2 y, const T3 z)
++noexcept
++{
++  if (std::is_constant_evaluated()) {
++    return internal::hypot_type_check(x,y,z);
++  } else {
++    return std::hypot(x, y, z);
++  }
++}
++
+ }
+ 
+ #endif
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch
index 890b8c3..adc2b3f 100644
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch
@@ -1,42 +1,43 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Fri, 14 Jul 2023 17:33:08 -0700
-Subject: [PATCH 01/10] Revert "win,process: write minidumps when sending
- SIGQUIT (#3840)"
+Subject: [PATCH 1/9] Revert "win,process: write minidumps when sending SIGQUIT
+ (#3840)"
 
 This reverts commit 748d894e82abcdfff7429cf745003e182c47f163.
 ---
- CMakeLists.txt    |   5 +-
+ CMakeLists.txt    |   6 +--
  configure.ac      |   2 +-
  include/uv/win.h  |   1 -
  src/win/process.c | 116 ----------------------------------------------
- 4 files changed, 2 insertions(+), 122 deletions(-)
+ 4 files changed, 2 insertions(+), 123 deletions(-)
 
 diff --git a/CMakeLists.txt b/CMakeLists.txt
-index 93733dd04783436cc1f1a801133e67e315f4af8d..0958dfb1bd93311cd0e20506311e1e41774c5fa4 100644
+index 72377851b69f74c5285fd34ae206ad6bed3745c1..3ec6bd00542f5aacfc6245b1f82e365eb1cff02c 100644
 --- a/CMakeLists.txt
 +++ b/CMakeLists.txt
-@@ -183,10 +183,7 @@ if(WIN32)
+@@ -183,11 +183,7 @@ if(WIN32)
         advapi32
         iphlpapi
         userenv
 -       ws2_32
 -       dbghelp
 -       ole32
--       uuid)
+-       uuid
+-       shell32)
 +       ws2_32)
    list(APPEND uv_sources
         src/win/async.c
         src/win/core.c
 diff --git a/configure.ac b/configure.ac
-index deb083605de639e896df83882715ddca25340fa3..76177a4bc8e5f17bc1e062af3a9028d2dfc76dc9 100644
+index 0a1042ce3d384f6a4392a100275c14cb31ba2816..6c87c36039446e04f6c30c23b8fdb9b957dd610d 100644
 --- a/configure.ac
 +++ b/configure.ac
 @@ -74,7 +74,7 @@ AM_CONDITIONAL([OS400],    [AS_CASE([$host_os],[os400],         [true], [false])
  AM_CONDITIONAL([SUNOS],    [AS_CASE([$host_os],[solaris*],      [true], [false])])
  AM_CONDITIONAL([WINNT],    [AS_CASE([$host_os],[mingw*],        [true], [false])])
  AS_CASE([$host_os],[mingw*], [
--    LIBS="$LIBS -lws2_32 -lpsapi -liphlpapi -lshell32 -luserenv -luser32 -ldbghelp -lole32 -luuid"
+-    LIBS="$LIBS -lws2_32 -lpsapi -liphlpapi -lshell32 -luserenv -luser32 -ldbghelp -lole32 -luuid -lshell32"
 +    LIBS="$LIBS -lws2_32 -lpsapi -liphlpapi -lshell32 -luserenv -luser32"
  ])
  AS_CASE([$host_os], [solaris2.10], [
@@ -54,7 +55,7 @@
  #define SIGWINCH             28
  
 diff --git a/src/win/process.c b/src/win/process.c
-index 3e451e2291d6ed200ec258e874becbbea22bbc27..ed44adc67c6d52785a199206d9ba0357e2d0b045 100644
+index 43059858f3112e7e7185796525697629b72988df..119b46cb3f37122395c172c6e9700d472a2173ed 100644
 --- a/src/win/process.c
 +++ b/src/win/process.c
 @@ -32,9 +32,6 @@
@@ -67,7 +68,7 @@
  
  
  #define SIGKILL         9
-@@ -1197,120 +1194,7 @@ static int uv__kill(HANDLE process_handle, int signum) {
+@@ -1173,120 +1170,7 @@ static int uv__kill(HANDLE process_handle, int signum) {
      return UV_EINVAL;
    }
  
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch
index 4d9603f..917d375 100644
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch
@@ -1,10 +1,11 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 26 Apr 2022 15:01:25 -0400
-Subject: [PATCH 02/10] Fix missing casts
+Subject: [PATCH 2/9] Fix missing casts
 
 ---
  src/fs-poll.c                  | 10 ++++----
+ src/idna.c                     |  2 +-
  src/inet.c                     | 11 ++++----
  src/strscpy.c                  |  2 +-
  src/thread-common.c            |  2 +-
@@ -30,19 +31,23 @@
  src/unix/proctitle.c           |  2 +-
  src/unix/random-sysctl-linux.c |  2 +-
  src/unix/stream.c              | 31 ++++++++++++-----------
+ src/unix/tcp.c                 |  2 +-
  src/unix/thread.c              |  5 ++--
  src/unix/udp.c                 |  8 +++---
  src/uv-common.c                | 16 ++++++------
  src/win/core.c                 |  8 +++---
+ src/win/dl.c                   |  2 +-
  src/win/fs-event.c             |  4 +--
  src/win/fs-fd-hash-inl.h       |  2 +-
- src/win/fs.c                   | 28 ++++++++++-----------
- src/win/pipe.c                 | 12 ++++-----
- src/win/process.c              | 12 ++++-----
+ src/win/fs.c                   | 38 ++++++++++++++--------------
+ src/win/getaddrinfo.c          | 12 ++++-----
+ src/win/pipe.c                 |  8 +++---
+ src/win/process.c              | 22 ++++++++--------
  src/win/tcp.c                  |  2 +-
  src/win/thread.c               |  4 +--
- src/win/util.c                 | 27 ++++++++++----------
- 38 files changed, 183 insertions(+), 178 deletions(-)
+ src/win/tty.c                  |  6 ++---
+ src/win/util.c                 | 35 +++++++++++++-------------
+ 43 files changed, 207 insertions(+), 202 deletions(-)
 
 diff --git a/src/fs-poll.c b/src/fs-poll.c
 index 1bac1c568e36cadd0b68451926c6f045f88342d2..5a39daed095502b2db34f23fcaf0ab04f31f96ff 100644
@@ -93,6 +98,19 @@
           it != ctx;
           last = it, it = it->previous) {
        assert(last->previous != NULL);
+diff --git a/src/idna.c b/src/idna.c
+index 1c0a60cf3e3becc1badafa394e7c422af7f56833..0c952cf605a88136ed9035f9385f9b1080c30c28 100644
+--- a/src/idna.c
++++ b/src/idna.c
+@@ -477,7 +477,7 @@ int uv_utf16_to_wtf8(const uint16_t* w_source_ptr,
+     return 0;
+ 
+   if (*target_ptr == NULL) {
+-    target = uv__malloc(target_len + 1);
++    target = (char*)uv__malloc(target_len + 1);
+     if (target == NULL) {
+       return UV_ENOMEM;
+     }
 diff --git a/src/inet.c b/src/inet.c
 index cd77496846e90e8b8e61c63c10f498f153344fe5..dd94bea3886ca37945fcad7909d765e3700e3c21 100644
 --- a/src/inet.c
@@ -279,7 +297,7 @@
    if (display_name_key == NULL || *display_name_key == NULL)
      goto out;
 diff --git a/src/unix/darwin.c b/src/unix/darwin.c
-index 90790d701c4327518d17230c5aa69b9a74112e73..9ee5cd8eb9d02fb8b71986c47fe8a686f0983847 100644
+index 5e764a65ee4c71efd61876c70b0e999420f24f61..dc93d236b6b7c6da62dc7aa66cb42ddc125575a2 100644
 --- a/src/unix/darwin.c
 +++ b/src/unix/darwin.c
 @@ -217,7 +217,7 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos, int* count) {
@@ -314,10 +332,10 @@
      uv__free(*cpu_infos);
      return UV_ENOMEM;
 diff --git a/src/unix/fs.c b/src/unix/fs.c
-index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7 100644
+index 891306daedcc6a9b493bdfefcdb7d43c7efbb622..bc00c90b07a32a823412b216df6f2d758dbc423b 100644
 --- a/src/unix/fs.c
 +++ b/src/unix/fs.c
-@@ -140,7 +140,7 @@ extern char *mkdtemp(char *template); /* See issue #740 on AIX < 7 */
+@@ -134,7 +134,7 @@ extern char *mkdtemp(char *template); /* See issue #740 on AIX < 7 */
        size_t new_path_len;                                                    \
        path_len = strlen(path) + 1;                                            \
        new_path_len = strlen(new_path) + 1;                                    \
@@ -326,7 +344,7 @@
        if (req->path == NULL)                                                  \
          return UV_ENOMEM;                                                     \
        req->new_path = req->path + path_len;                                   \
-@@ -568,7 +568,7 @@ static ssize_t uv__fs_scandir(uv_fs_t* req) {
+@@ -496,7 +496,7 @@ static ssize_t uv__fs_scandir(uv_fs_t* req) {
  static int uv__fs_opendir(uv_fs_t* req) {
    uv_dir_t* dir;
  
@@ -335,7 +353,7 @@
    if (dir == NULL)
      goto error;
  
-@@ -592,7 +592,7 @@ static int uv__fs_readdir(uv_fs_t* req) {
+@@ -520,7 +520,7 @@ static int uv__fs_readdir(uv_fs_t* req) {
    unsigned int dirent_idx;
    unsigned int i;
  
@@ -344,7 +362,7 @@
    dirent_idx = 0;
  
    while (dirent_idx < dir->nentries) {
-@@ -634,7 +634,7 @@ error:
+@@ -562,7 +562,7 @@ error:
  static int uv__fs_closedir(uv_fs_t* req) {
    uv_dir_t* dir;
  
@@ -353,7 +371,7 @@
  
    if (dir->dir != NULL) {
      closedir(dir->dir);
-@@ -663,7 +663,7 @@ static int uv__fs_statfs(uv_fs_t* req) {
+@@ -591,7 +591,7 @@ static int uv__fs_statfs(uv_fs_t* req) {
  #endif /* defined(__sun) */
      return -1;
  
@@ -362,7 +380,7 @@
    if (stat_fs == NULL) {
      errno = ENOMEM;
      return -1;
-@@ -727,7 +727,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) {
+@@ -655,7 +655,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) {
      maxlen = uv__fs_pathmax_size(req->path);
  #endif
  
@@ -371,7 +389,7 @@
  
    if (buf == NULL) {
      errno = ENOMEM;
-@@ -747,7 +747,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) {
+@@ -675,7 +675,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) {
  
    /* Uncommon case: resize to make room for the trailing nul byte. */
    if (len == maxlen) {
@@ -380,7 +398,7 @@
  
      if (buf == NULL)
        return -1;
-@@ -770,7 +770,7 @@ static ssize_t uv__fs_realpath(uv_fs_t* req) {
+@@ -698,7 +698,7 @@ static ssize_t uv__fs_realpath(uv_fs_t* req) {
    ssize_t len;
  
    len = uv__fs_pathmax_size(req->path);
@@ -389,8 +407,8 @@
  
    if (buf == NULL) {
      errno = ENOMEM;
-@@ -1984,7 +1984,7 @@ int uv_fs_read(uv_loop_t* loop, uv_fs_t* req,
-   req->nbufs = nbufs;
+@@ -1886,7 +1886,7 @@ int uv_fs_read(uv_loop_t* loop, uv_fs_t* req,
+ 
    req->bufs = req->bufsml;
    if (nbufs > ARRAY_SIZE(req->bufsml))
 -    req->bufs = uv__malloc(nbufs * sizeof(*bufs));
@@ -398,7 +416,7 @@
  
    if (req->bufs == NULL)
      return UV_ENOMEM;
-@@ -2171,7 +2171,7 @@ int uv_fs_write(uv_loop_t* loop,
+@@ -2071,7 +2071,7 @@ int uv_fs_write(uv_loop_t* loop,
    req->nbufs = nbufs;
    req->bufs = req->bufsml;
    if (nbufs > ARRAY_SIZE(req->bufsml))
@@ -585,10 +603,10 @@
      return UV_ENOMEM;
    }
 diff --git a/src/unix/kqueue.c b/src/unix/kqueue.c
-index b78242d3be4e3cf6b7b998f56dc65213982d4bc7..28e55aae6c613576ede7024a5c73d746e134d865 100644
+index 94ace58680cf391707f68357d7927173cb1db08e..06fbdb24b4adc4adb781d32150d40836fa745531 100644
 --- a/src/unix/kqueue.c
 +++ b/src/unix/kqueue.c
-@@ -299,8 +299,8 @@ void uv__io_poll(uv_loop_t* loop, int timeout) {
+@@ -303,8 +303,8 @@ void uv__io_poll(uv_loop_t* loop, int timeout) {
      nevents = 0;
  
      assert(loop->watchers != NULL);
@@ -599,7 +617,7 @@
      for (i = 0; i < nfds; i++) {
        ev = events + i;
        fd = ev->ident;
-@@ -322,7 +322,7 @@ void uv__io_poll(uv_loop_t* loop, int timeout) {
+@@ -326,7 +326,7 @@ void uv__io_poll(uv_loop_t* loop, int timeout) {
        /* Skip invalidated events, see uv__platform_invalidate_fd */
        if (fd == -1)
          continue;
@@ -609,10 +627,10 @@
        if (w == NULL) {
          /* File descriptor that we've stopped watching, disarm it. */
 diff --git a/src/unix/linux.c b/src/unix/linux.c
-index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba84762a027a1 100644
+index 8eeb352e6238a9e9557ec4dfc71f192105135cd7..2b8e1d8fe593a181d049aa50ff9edaf6da258a24 100644
 --- a/src/unix/linux.c
 +++ b/src/unix/linux.c
-@@ -456,8 +456,8 @@ static void uv__iou_init(int epollfd,
+@@ -518,8 +518,8 @@ static void uv__iou_init(int epollfd,
    char* sqe;
    int ringfd;
  
@@ -623,7 +641,7 @@
  
    if (!uv__use_io_uring())
      return;
-@@ -496,14 +496,14 @@ static void uv__iou_init(int epollfd,
+@@ -558,14 +558,14 @@ static void uv__iou_init(int epollfd,
    maxlen = sqlen < cqlen ? cqlen : sqlen;
    sqelen = params.sq_entries * sizeof(struct uv__io_uring_sqe);
  
@@ -640,7 +658,7 @@
               sqelen,
               PROT_READ | PROT_WRITE,
               MAP_SHARED | MAP_POPULATE,
-@@ -643,7 +643,7 @@ void uv__platform_invalidate_fd(uv_loop_t* loop, int fd) {
+@@ -705,7 +705,7 @@ void uv__platform_invalidate_fd(uv_loop_t* loop, int fd) {
    int i;
  
    lfields = uv__get_internal_fields(loop);
@@ -649,7 +667,7 @@
  
    /* Invalidate events with same file descriptor */
    if (inv != NULL)
-@@ -718,7 +718,7 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou,
+@@ -780,7 +780,7 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou,
      return NULL;  /* No room in ring buffer. TODO(bnoordhuis) maybe flush it? */
  
    slot = tail & mask;
@@ -658,7 +676,7 @@
    sqe = &sqe[slot];
    memset(sqe, 0, sizeof(*sqe));
    sqe->user_data = (uintptr_t) req;
-@@ -986,7 +986,7 @@ int uv__iou_fs_statx(uv_loop_t* loop,
+@@ -1057,7 +1057,7 @@ int uv__iou_fs_statx(uv_loop_t* loop,
    struct uv__statx* statxbuf;
    struct uv__iou* iou;
  
@@ -667,7 +685,7 @@
    if (statxbuf == NULL)
      return 0;
  
-@@ -1050,7 +1050,7 @@ static void uv__iou_fs_statx_post(uv_fs_t* req) {
+@@ -1121,7 +1121,7 @@ static void uv__iou_fs_statx_post(uv_fs_t* req) {
    uv_stat_t* buf;
  
    buf = &req->statbuf;
@@ -676,7 +694,7 @@
    req->ptr = NULL;
  
    if (req->result == 0) {
-@@ -1079,7 +1079,7 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) {
+@@ -1150,7 +1150,7 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) {
    tail = atomic_load_explicit((_Atomic uint32_t*) iou->cqtail,
                                memory_order_acquire);
    mask = iou->cqmask;
@@ -685,7 +703,7 @@
    nevents = 0;
  
    for (i = head; i != tail; i++) {
-@@ -1170,7 +1170,7 @@ static void uv__epoll_ctl_prep(int epollfd,
+@@ -1241,7 +1241,7 @@ static void uv__epoll_ctl_prep(int epollfd,
      pe = &(*events)[slot];
      *pe = *e;
  
@@ -694,7 +712,7 @@
      sqe = &sqe[slot];
  
      memset(sqe, 0, sizeof(*sqe));
-@@ -1226,7 +1226,7 @@ static void uv__epoll_ctl_flush(int epollfd,
+@@ -1297,7 +1297,7 @@ static void uv__epoll_ctl_flush(int epollfd,
    while (*ctl->cqhead != *ctl->cqtail) {
      slot = (*ctl->cqhead)++ & ctl->cqmask;
  
@@ -703,7 +721,7 @@
      cqe = &cqe[slot];
  
      if (cqe->res == 0)
-@@ -1708,7 +1708,7 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) {
+@@ -1758,7 +1758,7 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) {
    snprintf(*models, sizeof(*models), "unknown");
    maxcpu = 0;
  
@@ -712,7 +730,7 @@
    if (cpus == NULL)
      return UV_ENOMEM;
  
-@@ -1764,9 +1764,9 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) {
+@@ -1816,9 +1816,9 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) {
  
      /* arm64: translate CPU part code to model name. */
      if (*parts) {
@@ -724,7 +742,7 @@
        else
          p += n + 1;
        n = (int) strcspn(p, "\n");
-@@ -1815,7 +1815,7 @@ nocpuinfo:
+@@ -1868,7 +1868,7 @@ nocpuinfo:
    }
  
    size = n * sizeof(**ci) + sizeof(models);
@@ -733,7 +751,7 @@
    *count = 0;
  
    if (*ci == NULL) {
-@@ -1824,7 +1824,7 @@ nocpuinfo:
+@@ -1877,7 +1877,7 @@ nocpuinfo:
    }
  
    *count = n;
@@ -742,7 +760,7 @@
  
    i = 0;
    for (cpu = 0; cpu < maxcpu; cpu++) {
-@@ -1833,19 +1833,19 @@ nocpuinfo:
+@@ -1886,19 +1886,19 @@ nocpuinfo:
  
      c = *cpus + cpu;
  
@@ -766,7 +784,7 @@
      };
    }
  
-@@ -1902,7 +1902,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses, int* count) {
+@@ -1955,7 +1955,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses, int* count) {
    }
  
    /* Make sure the memory is initiallized to zero using calloc() */
@@ -775,7 +793,7 @@
    if (!(*addresses)) {
      freeifaddrs(addrs);
      return UV_ENOMEM;
-@@ -2470,12 +2470,12 @@ int uv_fs_event_start(uv_fs_event_t* handle,
+@@ -2523,12 +2523,12 @@ int uv_fs_event_start(uv_fs_event_t* handle,
      goto no_insert;
  
    len = strlen(path) + 1;
@@ -1058,8 +1076,21 @@
      for (i = 0; i < queued_fds->offset; i++)
        uv__close(queued_fds->fds[i]);
      uv__free(handle->queued_fds);
+diff --git a/src/unix/tcp.c b/src/unix/tcp.c
+index a6b53e5913271d0c83e1d7f7e4cb8140f5f3936d..4c4213a4241b51d245146f1a37a371448d57b3b8 100644
+--- a/src/unix/tcp.c
++++ b/src/unix/tcp.c
+@@ -274,7 +274,7 @@ int uv__tcp_connect(uv_connect_t* req,
+     memcpy(&tmp6, addr, sizeof(tmp6));
+     if (tmp6.sin6_scope_id == 0) {
+       tmp6.sin6_scope_id = uv__ipv6_link_local_scope_id();
+-      addr = (void*) &tmp6;
++      addr = (const struct sockaddr*) &tmp6;
+     }
+   }
+ 
 diff --git a/src/unix/thread.c b/src/unix/thread.c
-index 4d6f4b8232ec6dc0bd27258a1315340e3e272ae9..531c6211bb4321e5f11031a0644b4e3ab9174396 100644
+index f05e6fe0f7dd5ac579f6a9d6f93bffb99e1bcbc2..20409541de3cb300504b823472a73bc95fa38f62 100644
 --- a/src/unix/thread.c
 +++ b/src/unix/thread.c
 @@ -168,8 +168,7 @@ int uv_thread_create_ex(uv_thread_t* tid,
@@ -1118,7 +1149,7 @@
    if (req->bufs == NULL) {
      uv__req_unregister(handle->loop, req);
 diff --git a/src/uv-common.c b/src/uv-common.c
-index 916f3f4e00664ab47f94d2a6ee7c6a9ef0461389..c04f93596ab1f730576256d86e216ccb7f258b72 100644
+index 2200fe3f0a41e295acb426f39ccc9f0133994675..69e95801a18104ea910abf86db236d85f62afb66 100644
 --- a/src/uv-common.c
 +++ b/src/uv-common.c
 @@ -54,10 +54,10 @@ static uv__allocator_t uv__allocator = {
@@ -1148,7 +1179,7 @@
  }
  
  void* uv__malloc(size_t size) {
-@@ -688,7 +688,7 @@ void uv__fs_scandir_cleanup(uv_fs_t* req) {
+@@ -691,7 +691,7 @@ void uv__fs_scandir_cleanup(uv_fs_t* req) {
    unsigned int n;
  
    if (req->result >= 0) {
@@ -1157,7 +1188,7 @@
      nbufs = uv__get_nbufs(req);
  
      i = 0;
-@@ -721,7 +721,7 @@ int uv_fs_scandir_next(uv_fs_t* req, uv_dirent_t* ent) {
+@@ -724,7 +724,7 @@ int uv_fs_scandir_next(uv_fs_t* req, uv_dirent_t* ent) {
    nbufs = uv__get_nbufs(req);
    assert(nbufs);
  
@@ -1166,7 +1197,7 @@
  
    /* Free previous entity */
    if (*nbufs > 0)
-@@ -786,7 +786,7 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) {
+@@ -789,7 +789,7 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) {
    if (req->ptr == NULL)
      return;
  
@@ -1175,7 +1206,7 @@
    dirents = dir->dirents;
    req->ptr = NULL;
  
-@@ -832,7 +832,7 @@ uv_loop_t* uv_default_loop(void) {
+@@ -835,7 +835,7 @@ uv_loop_t* uv_default_loop(void) {
  uv_loop_t* uv_loop_new(void) {
    uv_loop_t* loop;
  
@@ -1217,8 +1248,21 @@
    if (timer_heap == NULL) {
      err = UV_ENOMEM;
      goto fail_timers_alloc;
+diff --git a/src/win/dl.c b/src/win/dl.c
+index 7880c9595be1f66ea0dcbdbcc4a91ce40577587f..d88400f0e819d74998e13f60f13c67a606dec398 100644
+--- a/src/win/dl.c
++++ b/src/win/dl.c
+@@ -37,7 +37,7 @@ int uv_dlopen(const char* filename, uv_lib_t* lib) {
+     return uv__dlerror(lib, filename, ERROR_NO_UNICODE_TRANSLATION);
+   if ((size_t) r > ARRAY_SIZE(filename_w))
+     return uv__dlerror(lib, filename, ERROR_INSUFFICIENT_BUFFER);
+-  uv_wtf8_to_utf16(filename, filename_w, r);
++  uv_wtf8_to_utf16(filename, (uint16_t*)filename_w, r);
+ 
+   lib->handle = LoadLibraryExW(filename_w, NULL, LOAD_WITH_ALTERED_SEARCH_PATH);
+   if (lib->handle == NULL) {
 diff --git a/src/win/fs-event.c b/src/win/fs-event.c
-index 6758c7c78bc1d6a5316a8ae7dc2d1e23cd0f32bc..150467313d576bfe2966b55f3d8cffa23cbb8ea3 100644
+index 4a0ca1f70a22b6342e208124838d6ecf3173f1a8..5a07acfe54efe90cf2ab0bca5b5998a961e72ebd 100644
 --- a/src/win/fs-event.c
 +++ b/src/win/fs-event.c
 @@ -73,7 +73,7 @@ static void uv__relative_path(const WCHAR* filename,
@@ -1230,7 +1274,7 @@
    if (!*relpath)
      uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
    wcsncpy(*relpath, filename + dirlen + 1, relpathlen);
-@@ -242,7 +242,7 @@ int uv_fs_event_start(uv_fs_event_t* handle,
+@@ -229,7 +229,7 @@ int uv_fs_event_start(uv_fs_event_t* handle,
      if (short_path_buffer_len == 0) {
        goto short_path_done;
      }
@@ -1253,28 +1297,53 @@
          uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
        }
 diff --git a/src/win/fs.c b/src/win/fs.c
-index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d0841e14b264 100644
+index 99c8a2bf8bf1244f0db7114c1eaa8bff22564996..d9c2a4f728c7fb491995c6153b2a63a835b206b7 100644
 --- a/src/win/fs.c
 +++ b/src/win/fs.c
-@@ -258,7 +258,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path,
+@@ -280,7 +280,7 @@ INLINE static int fs__readlink_handle(HANDLE handle,
+   }
+ 
+   assert(target_ptr == NULL || *target_ptr == NULL);
+-  return uv_utf16_to_wtf8(w_target, w_target_len, target_ptr, target_len_ptr);
++  return uv_utf16_to_wtf8((const uint16_t*)w_target, w_target_len, target_ptr, target_len_ptr);
+ }
+ 
+ 
+@@ -323,7 +323,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path,
      return 0;
    }
  
 -  buf = uv__malloc(buf_sz);
-+  buf = (WCHAR *)uv__malloc(buf_sz);
++  buf = (WCHAR*)uv__malloc(buf_sz);
    if (buf == NULL) {
      return ERROR_OUTOFMEMORY;
    }
-@@ -376,7 +376,7 @@ static int fs__wide_to_wtf8(WCHAR* w_source_ptr,
-     return 0;
+@@ -331,7 +331,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path,
+   pos = buf;
  
-   if (*target_ptr == NULL) {
--    target = uv__malloc(target_len + 1);
-+    target = (char *)uv__malloc(target_len + 1);
-     if (target == NULL) {
-       SetLastError(ERROR_OUTOFMEMORY);
-       return -1;
-@@ -1575,7 +1575,7 @@ void fs__scandir(uv_fs_t* req) {
+   if (path != NULL) {
+-    uv_wtf8_to_utf16(path, pos, pathw_len);
++    uv_wtf8_to_utf16(path, (uint16_t*)pos, pathw_len);
+     req->file.pathw = pos;
+     pos += pathw_len;
+   } else {
+@@ -339,7 +339,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path,
+   }
+ 
+   if (new_path != NULL) {
+-    uv_wtf8_to_utf16(new_path, pos, new_pathw_len);
++    uv_wtf8_to_utf16(new_path, (uint16_t*)pos, new_pathw_len);
+     req->fs.info.new_pathw = pos;
+     pos += new_pathw_len;
+   } else {
+@@ -1390,13 +1390,13 @@ void fs__scandir(uv_fs_t* req) {
+         continue;
+ 
+       /* Compute the space required to store the filename as WTF-8. */
+-      wtf8_len = uv_utf16_length_as_wtf8(&info->FileName[0], wchar_len);
++      wtf8_len = uv_utf16_length_as_wtf8((const uint16_t*)&info->FileName[0], wchar_len);
+ 
+       /* Resize the dirent array if needed. */
        if (dirents_used >= dirents_size) {
          size_t new_dirents_size =
              dirents_size == 0 ? dirents_initial_size : dirents_size << 1;
@@ -1283,7 +1352,7 @@
              uv__realloc(dirents, new_dirents_size * sizeof *dirents);
  
          if (new_dirents == NULL)
-@@ -1589,7 +1589,7 @@ void fs__scandir(uv_fs_t* req) {
+@@ -1410,7 +1410,7 @@ void fs__scandir(uv_fs_t* req) {
         * includes room for the first character of the filename, but `utf8_len`
         * doesn't count the NULL terminator at this point.
         */
@@ -1292,7 +1361,16 @@
        if (dirent == NULL)
          goto out_of_memory_error;
  
-@@ -1691,7 +1691,7 @@ void fs__opendir(uv_fs_t* req) {
+@@ -1418,7 +1418,7 @@ void fs__scandir(uv_fs_t* req) {
+ 
+       /* Convert file name to UTF-8. */
+       wtf8 = &dirent->d_name[0];
+-      if (uv_utf16_to_wtf8(&info->FileName[0], wchar_len, &wtf8, &wtf8_len) != 0)
++      if (uv_utf16_to_wtf8((const uint16_t*)&info->FileName[0], wchar_len, &wtf8, &wtf8_len) != 0)
+         goto out_of_memory_error;
+ 
+       /* Fill out the type field. */
+@@ -1512,7 +1512,7 @@ void fs__opendir(uv_fs_t* req) {
      goto error;
    }
  
@@ -1301,7 +1379,7 @@
    if (dir == NULL) {
      SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY);
      goto error;
-@@ -1706,7 +1706,7 @@ void fs__opendir(uv_fs_t* req) {
+@@ -1527,7 +1527,7 @@ void fs__opendir(uv_fs_t* req) {
    else
      fmt = L"%s\\*";
  
@@ -1310,7 +1388,7 @@
    if (find_path == NULL) {
      SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY);
      goto error;
-@@ -1743,7 +1743,7 @@ void fs__readdir(uv_fs_t* req) {
+@@ -1564,7 +1564,7 @@ void fs__readdir(uv_fs_t* req) {
    int r;
  
    req->flags |= UV_FS_FREE_PTR;
@@ -1319,7 +1397,7 @@
    dirents = dir->dirents;
    memset(dirents, 0, dir->nentries * sizeof(*dir->dirents));
    find_data = &dir->find_data;
-@@ -1800,7 +1800,7 @@ error:
+@@ -1621,7 +1621,7 @@ error:
  void fs__closedir(uv_fs_t* req) {
    uv_dir_t* dir;
  
@@ -1328,7 +1406,7 @@
    FindClose(dir->dir_handle);
    uv__free(req->ptr);
    SET_REQ_RESULT(req, 0);
-@@ -2791,7 +2791,7 @@ static ssize_t fs__realpath_handle(HANDLE handle, char** realpath_ptr) {
+@@ -2612,7 +2612,7 @@ static ssize_t fs__realpath_handle(HANDLE handle, char** realpath_ptr) {
      return -1;
    }
  
@@ -1337,7 +1415,16 @@
    if (w_realpath_buf == NULL) {
      SetLastError(ERROR_OUTOFMEMORY);
      return -1;
-@@ -2904,7 +2904,7 @@ retry_get_disk_free_space:
+@@ -2645,7 +2645,7 @@ static ssize_t fs__realpath_handle(HANDLE handle, char** realpath_ptr) {
+   }
+ 
+   assert(*realpath_ptr == NULL);
+-  r = uv_utf16_to_wtf8(w_realpath_ptr, w_realpath_len, realpath_ptr, NULL);
++  r = uv_utf16_to_wtf8((const uint16_t*)w_realpath_ptr, w_realpath_len, realpath_ptr, NULL);
+   uv__free(w_realpath_buf);
+   return r;
+ }
+@@ -2725,7 +2725,7 @@ retry_get_disk_free_space:
      }
  
      len = MAX_PATH + 1;
@@ -1346,7 +1433,7 @@
      if (pathw == NULL) {
        SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY);
        return;
-@@ -2920,7 +2920,7 @@ retry_get_full_path_name:
+@@ -2741,7 +2741,7 @@ retry_get_full_path_name:
        return;
      } else if (ret > len) {
        len = ret;
@@ -1355,7 +1442,7 @@
        if (pathw == NULL) {
          SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY);
          return;
-@@ -2936,7 +2936,7 @@ retry_get_full_path_name:
+@@ -2757,7 +2757,7 @@ retry_get_full_path_name:
      uv__free(pathw);
    }
  
@@ -1364,7 +1451,7 @@
    if (stat_fs == NULL) {
      SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY);
      return;
-@@ -3095,7 +3095,7 @@ int uv_fs_read(uv_loop_t* loop,
+@@ -2916,7 +2916,7 @@ int uv_fs_read(uv_loop_t* loop,
    req->fs.info.nbufs = nbufs;
    req->fs.info.bufs = req->fs.info.bufsml;
    if (nbufs > ARRAY_SIZE(req->fs.info.bufsml))
@@ -1373,7 +1460,7 @@
  
    if (req->fs.info.bufs == NULL) {
      SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY);
-@@ -3128,7 +3128,7 @@ int uv_fs_write(uv_loop_t* loop,
+@@ -2949,7 +2949,7 @@ int uv_fs_write(uv_loop_t* loop,
    req->fs.info.nbufs = nbufs;
    req->fs.info.bufs = req->fs.info.bufsml;
    if (nbufs > ARRAY_SIZE(req->fs.info.bufsml))
@@ -1382,38 +1469,64 @@
  
    if (req->fs.info.bufs == NULL) {
      SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY);
+diff --git a/src/win/getaddrinfo.c b/src/win/getaddrinfo.c
+index 8b8406ada8e7434e291b2e50caf4ed7f36613fa0..5bc63d8e19411b967a2acf5c24b34b9b17323ccc 100644
+--- a/src/win/getaddrinfo.c
++++ b/src/win/getaddrinfo.c
+@@ -132,7 +132,7 @@ static void uv__getaddrinfo_done(struct uv__work* w, int status) {
+       addrinfo_len += addrinfo_struct_len +
+           ALIGNED_SIZE(addrinfow_ptr->ai_addrlen);
+       if (addrinfow_ptr->ai_canonname != NULL) {
+-        name_len = uv_utf16_length_as_wtf8(addrinfow_ptr->ai_canonname, -1);
++        name_len = uv_utf16_length_as_wtf8((const uint16_t*)addrinfow_ptr->ai_canonname, -1);
+         if (name_len < 0) {
+           req->retcode = name_len;
+           goto complete;
+@@ -281,9 +281,9 @@ int uv_getaddrinfo(uv_loop_t* loop,
+   }
+ 
+   /* allocate memory for inputs, and partition it as needed */
+-  alloc_ptr = uv__malloc(ALIGNED_SIZE(nodesize * sizeof(WCHAR)) +
+-                         ALIGNED_SIZE(servicesize * sizeof(WCHAR)) +
+-                         hintssize);
++  alloc_ptr = (char*)uv__malloc(ALIGNED_SIZE(nodesize * sizeof(WCHAR)) +
++                                ALIGNED_SIZE(servicesize * sizeof(WCHAR)) +
++                                hintssize);
+   if (!alloc_ptr)
+     return UV_ENOMEM;
+ 
+@@ -294,7 +294,7 @@ int uv_getaddrinfo(uv_loop_t* loop,
+    * request. The node here has been converted to ascii. */
+   if (node != NULL) {
+     req->node = (WCHAR*) alloc_ptr;
+-    uv_wtf8_to_utf16(node, (WCHAR*) alloc_ptr, nodesize);
++    uv_wtf8_to_utf16(node, (uint16_t*) alloc_ptr, nodesize);
+     alloc_ptr += ALIGNED_SIZE(nodesize * sizeof(WCHAR));
+   } else {
+     req->node = NULL;
+@@ -304,7 +304,7 @@ int uv_getaddrinfo(uv_loop_t* loop,
+    * the req. */
+   if (service != NULL) {
+     req->service = (WCHAR*) alloc_ptr;
+-    uv_wtf8_to_utf16(service, (WCHAR*) alloc_ptr, servicesize);
++    uv_wtf8_to_utf16(service, (uint16_t*) alloc_ptr, servicesize);
+     alloc_ptr += ALIGNED_SIZE(servicesize * sizeof(WCHAR));
+   } else {
+     req->service = NULL;
 diff --git a/src/win/pipe.c b/src/win/pipe.c
-index f0cac3822564e14052feb5e1934f54c57c78160d..c1739efe82b8755999145860b4da6b50c73518a2 100644
+index cec72ff750b5a17e139fd85080ccbdfc3c71d8c0..0f045a873073cf9b07feb457ea199990df521e5f 100644
 --- a/src/win/pipe.c
 +++ b/src/win/pipe.c
-@@ -756,7 +756,7 @@ int uv_pipe_bind2(uv_pipe_t* handle,
- 
-   /* Convert name to UTF16. */
-   nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR);
--  handle->name = uv__malloc(nameSize);
-+  handle->name = (WCHAR*)uv__malloc(nameSize);
-   if (!handle->name) {
-     uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
-   }
-@@ -906,7 +906,7 @@ int uv_pipe_connect2(uv_connect_t* req,
- 
-   /* Convert name to UTF16. */
-   nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR);
--  handle->name = uv__malloc(nameSize);
-+  handle->name = (WCHAR*)uv__malloc(nameSize);
-   if (!handle->name) {
-     uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
-   }
-@@ -924,7 +924,7 @@ int uv_pipe_connect2(uv_connect_t* req,
-   pipeHandle = open_named_pipe(handle->name, &duplex_flags);
+@@ -899,7 +899,7 @@ int uv_pipe_connect2(uv_connect_t* req,
    if (pipeHandle == INVALID_HANDLE_VALUE) {
      if (GetLastError() == ERROR_PIPE_BUSY) {
+       nameSize = (wcslen(handle->name) + 1) * sizeof(WCHAR);
 -      req->u.connect.name = uv__malloc(nameSize);
 +      req->u.connect.name = (WCHAR *)uv__malloc(nameSize);
        if (!req->u.connect.name) {
          uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
        }
-@@ -1528,7 +1528,7 @@ static int uv__build_coalesced_write_req(uv_write_t* user_req,
+@@ -1503,7 +1503,7 @@ static int uv__build_coalesced_write_req(uv_write_t* user_req,
                         data_length;                  /* (c) */
  
    /* Allocate buffer. */
@@ -1422,7 +1535,7 @@
    if (heap_buffer == NULL)
      return ERROR_NOT_ENOUGH_MEMORY; /* Maps to UV_ENOMEM. */
  
-@@ -1777,7 +1777,7 @@ int uv__pipe_write_ipc(uv_loop_t* loop,
+@@ -1752,7 +1752,7 @@ int uv__pipe_write_ipc(uv_loop_t* loop,
      bufs = stack_bufs;
    } else {
      /* Use heap-allocated buffer array. */
@@ -1431,7 +1544,7 @@
      if (bufs == NULL)
        return ERROR_NOT_ENOUGH_MEMORY; /* Maps to UV_ENOMEM. */
    }
-@@ -2514,7 +2514,7 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size)
+@@ -2449,7 +2449,7 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size)
                                        FileNameInformation);
    if (nt_status == STATUS_BUFFER_OVERFLOW) {
      name_size = sizeof(*name_info) + tmp_name_info.FileNameLength;
@@ -1439,12 +1552,38 @@
 +    name_info = (FILE_NAME_INFORMATION*)uv__malloc(name_size);
      if (!name_info) {
        *size = 0;
-       err = UV_ENOMEM;
+       return UV_ENOMEM;
 diff --git a/src/win/process.c b/src/win/process.c
-index ed44adc67c6d52785a199206d9ba0357e2d0b045..b383e8b405db56d413985b38df216d09c58ec4a0 100644
+index 119b46cb3f37122395c172c6e9700d472a2173ed..0103004c565bb09b7b9c150e874f9c68486ab26c 100644
 --- a/src/win/process.c
 +++ b/src/win/process.c
-@@ -615,8 +615,8 @@ error:
+@@ -543,14 +543,14 @@ int make_program_args(char** args, int verbatim_arguments, WCHAR** dst_ptr) {
+   dst_len = dst_len * 2 + arg_count * 2;
+ 
+   /* Allocate buffer for the final command line. */
+-  dst = uv__malloc(dst_len * sizeof(WCHAR));
++  dst = (WCHAR*)uv__malloc(dst_len * sizeof(WCHAR));
+   if (dst == NULL) {
+     err = UV_ENOMEM;
+     goto error;
+   }
+ 
+   /* Allocate temporary working buffer. */
+-  temp_buffer = uv__malloc(temp_buffer_len * sizeof(WCHAR));
++  temp_buffer = (WCHAR*)uv__malloc(temp_buffer_len * sizeof(WCHAR));
+   if (temp_buffer == NULL) {
+     err = UV_ENOMEM;
+     goto error;
+@@ -564,7 +564,7 @@ int make_program_args(char** args, int verbatim_arguments, WCHAR** dst_ptr) {
+     arg_len = uv_wtf8_length_as_utf16(*arg);
+     assert(arg_len > 0);
+     assert(temp_buffer_len >= (size_t) arg_len);
+-    uv_wtf8_to_utf16(*arg, temp_buffer, arg_len);
++    uv_wtf8_to_utf16(*arg, (uint16_t*)temp_buffer, arg_len);
+ 
+     if (verbatim_arguments) {
+       /* Copy verbatim. */
+@@ -592,8 +592,8 @@ error:
  
  
  int env_strncmp(const wchar_t* a, int na, const wchar_t* b) {
@@ -1455,7 +1594,7 @@
    wchar_t* A;
    wchar_t* B;
    int nb;
-@@ -633,8 +633,8 @@ int env_strncmp(const wchar_t* a, int na, const wchar_t* b) {
+@@ -610,8 +610,8 @@ int env_strncmp(const wchar_t* a, int na, const wchar_t* b) {
    assert(b_eq);
    nb = b_eq - b;
  
@@ -1466,16 +1605,30 @@
  
    r = LCMapStringW(LOCALE_INVARIANT, LCMAP_UPPERCASE, a, na, A, na);
    assert(r==na);
-@@ -717,7 +717,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) {
+@@ -684,11 +684,11 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) {
+   }
+ 
+   /* second pass: copy to UTF-16 environment block */
+-  dst_copy = uv__malloc(env_len * sizeof(WCHAR));
++  dst_copy = (WCHAR*)uv__malloc(env_len * sizeof(WCHAR));
    if (dst_copy == NULL && env_len > 0) {
-     return ERROR_OUTOFMEMORY;
+     return UV_ENOMEM;
    }
 -  env_copy = alloca(env_block_count * sizeof(WCHAR*));
 +  env_copy = (WCHAR**)alloca(env_block_count * sizeof(WCHAR*));
  
    ptr = dst_copy;
    ptr_copy = env_copy;
-@@ -771,7 +771,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) {
+@@ -698,7 +698,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) {
+       len = uv_wtf8_length_as_utf16(*env);
+       assert(len > 0);
+       assert((size_t) len <= env_len - (ptr - dst_copy));
+-      uv_wtf8_to_utf16(*env, ptr, len);
++      uv_wtf8_to_utf16(*env, (uint16_t*)ptr, len);
+       *ptr_copy++ = ptr;
+       ptr += len;
+     }
+@@ -736,7 +736,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) {
    }
  
    /* final pass: copy, in sort order, and inserting required variables */
@@ -1483,7 +1636,7 @@
 +  dst = (WCHAR*)uv__malloc((1+env_len) * sizeof(WCHAR));
    if (!dst) {
      uv__free(dst_copy);
-     return ERROR_OUTOFMEMORY;
+     return UV_ENOMEM;
 diff --git a/src/win/tcp.c b/src/win/tcp.c
 index 187f36e2a61c870b0d16e17e9d4a9e1161ba8851..d8da4d941a51b0625fc0c072342ec4edf74c0ea3 100644
 --- a/src/win/tcp.c
@@ -1519,11 +1672,51 @@
    if (ctx == NULL)
      return UV_ENOMEM;
  
+diff --git a/src/win/tty.c b/src/win/tty.c
+index ac836930d6f3a185d57ba59fb42c817496aa0fdf..3aacaaf0b514ba71755dc51f1bd7263de5cbc308 100644
+--- a/src/win/tty.c
++++ b/src/win/tty.c
+@@ -528,7 +528,7 @@ static DWORD CALLBACK uv_tty_line_read_thread(void* data) {
+ 
+   if (read_console_success) {
+     read_bytes = bytes;
+-    uv_utf16_to_wtf8(utf16,
++    uv_utf16_to_wtf8((const uint16_t*)utf16,
+                      read_chars,
+                      &handle->tty.rd.read_line_buffer.base,
+                      &read_bytes);
+@@ -827,7 +827,7 @@ void uv_process_tty_read_raw_req(uv_loop_t* loop, uv_tty_t* handle,
+           WCHAR utf16_buffer[2];
+           utf16_buffer[0] = handle->tty.rd.last_utf16_high_surrogate;
+           utf16_buffer[1] = KEV.uChar.UnicodeChar;
+-          if (uv_utf16_to_wtf8(utf16_buffer,
++          if (uv_utf16_to_wtf8((const uint16_t*)utf16_buffer,
+                                2,
+                                &last_key_buf,
+                                &char_len))
+@@ -835,7 +835,7 @@ void uv_process_tty_read_raw_req(uv_loop_t* loop, uv_tty_t* handle,
+           handle->tty.rd.last_utf16_high_surrogate = 0;
+         } else {
+           /* Single UTF-16 character */
+-          if (uv_utf16_to_wtf8(&KEV.uChar.UnicodeChar,
++          if (uv_utf16_to_wtf8((const uint16_t*)&KEV.uChar.UnicodeChar,
+                                1,
+                                &last_key_buf,
+                                &char_len))
 diff --git a/src/win/util.c b/src/win/util.c
-index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4e2b69b04 100644
+index 91d88a54fb24a613df45d7915c345b6764dd6ffd..5fbc70f52952f90b36fb22ee03ec7f70e6ed5954 100644
 --- a/src/win/util.c
 +++ b/src/win/util.c
-@@ -160,7 +160,7 @@ static int uv__cwd(WCHAR** buf, DWORD *len) {
+@@ -124,7 +124,7 @@ int uv_exepath(char* buffer, size_t* size_ptr) {
+ 
+   /* Convert to UTF-8 */
+   utf8_len = *size_ptr - 1; /* Reserve space for NUL */
+-  err = uv_utf16_to_wtf8(utf16_buffer, utf16_len, &buffer, &utf8_len);
++  err = uv_utf16_to_wtf8((const uint16_t*)utf16_buffer, utf16_len, &buffer, &utf8_len);
+   if (err == UV_ENOBUFS) {
+     utf8_len = *size_ptr - 1;
+     err = 0;
+@@ -152,7 +152,7 @@ static int uv__cwd(WCHAR** buf, DWORD *len) {
        return uv_translate_sys_error(GetLastError());
  
      /* |t| is the size of the buffer _including_ nul. */
@@ -1532,16 +1725,7 @@
      if (p == NULL)
        return UV_ENOMEM;
  
-@@ -265,7 +265,7 @@ int uv_chdir(const char* dir) {
-   if (utf16_len == 0) {
-     return uv_translate_sys_error(GetLastError());
-   }
--  utf16_buffer = uv__malloc(utf16_len * sizeof(WCHAR));
-+  utf16_buffer = (WCHAR*)uv__malloc(utf16_len * sizeof(WCHAR));
-   if (utf16_buffer == NULL) {
-     return UV_ENOMEM;
-   }
-@@ -623,14 +623,14 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos_ptr, int* cpu_count_ptr) {
+@@ -548,14 +548,14 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos_ptr, int* cpu_count_ptr) {
    GetSystemInfo(&system_info);
    cpu_count = system_info.dwNumberOfProcessors;
  
@@ -1558,7 +1742,7 @@
    if (sppi == NULL) {
      err = ERROR_OUTOFMEMORY;
      goto error;
-@@ -774,7 +774,8 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr,
+@@ -699,7 +699,8 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr,
        case ERROR_BUFFER_OVERFLOW:
          /* This happens when win_address_buf is NULL or too small to hold all
           * adapters. */
@@ -1568,7 +1752,7 @@
          if (win_address_buf == NULL)
            return UV_ENOMEM;
  
-@@ -782,7 +783,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr,
+@@ -707,7 +708,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr,
  
        case ERROR_NO_DATA: {
          /* No adapters were found. */
@@ -1577,7 +1761,16 @@
          if (uv_address_buf == NULL)
            return UV_ENOMEM;
  
-@@ -859,7 +860,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr,
+@@ -758,7 +759,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr,
+       continue;
+ 
+     /* Compute the size of the interface name. */
+-    name_size = uv_utf16_length_as_wtf8(adapter->FriendlyName, -1);
++    name_size = uv_utf16_length_as_wtf8((const uint16_t*)adapter->FriendlyName, -1);
+     uv_address_buf_size += name_size + 1;
+ 
+     /* Count the number of addresses associated with this interface, and
+@@ -773,7 +774,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr,
    }
  
    /* Allocate space to store interface data plus adapter names. */
@@ -1586,7 +1779,7 @@
    if (uv_address_buf == NULL) {
      uv__free(win_address_buf);
      return UV_ENOMEM;
-@@ -1074,7 +1075,7 @@ int uv_os_tmpdir(char* buffer, size_t* size) {
+@@ -982,7 +983,7 @@ int uv_os_tmpdir(char* buffer, size_t* size) {
    }
    /* Include space for terminating null char. */
    len += 1;
@@ -1595,25 +1788,46 @@
    if (path == NULL) {
      return UV_ENOMEM;
    }
-@@ -1153,7 +1154,7 @@ int uv__convert_utf16_to_utf8(const WCHAR* utf16, int utf16len, char** utf8) {
-   /* Allocate the destination buffer adding an extra byte for the terminating
-    * NULL. If utf16len is not -1 WideCharToMultiByte will not add it, so
-    * we do it ourselves always, just in case. */
--  *utf8 = uv__malloc(bufsize + 1);
-+  *utf8 = (char*)uv__malloc(bufsize + 1);
+@@ -1019,7 +1020,7 @@ int uv__convert_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char** utf8)
+     return UV_EINVAL;
  
-   if (*utf8 == NULL)
-     return UV_ENOMEM;
-@@ -1201,7 +1202,7 @@ int uv__convert_utf8_to_utf16(const char* utf8, int utf8len, WCHAR** utf16) {
-   /* Allocate the destination buffer adding an extra byte for the terminating
-    * NULL. If utf8len is not -1 MultiByteToWideChar will not add it, so
-    * we do it ourselves always, just in case. */
--  *utf16 = uv__malloc(sizeof(WCHAR) * (bufsize + 1));
-+  *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * (bufsize + 1));
+    *utf8 = NULL;
+-   return uv_utf16_to_wtf8(utf16, utf16len, utf8, &utf8_len);
++   return uv_utf16_to_wtf8((const uint16_t*)utf16, utf16len, utf8, &utf8_len);
+ }
+ 
+ 
+@@ -1039,13 +1040,13 @@ int uv__convert_utf8_to_utf16(const char* utf8, WCHAR** utf16) {
+     return UV__EINVAL;
+ 
+   /* Allocate the destination buffer. */
+-  *utf16 = uv__malloc(sizeof(WCHAR) * bufsize);
++  *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * bufsize);
  
    if (*utf16 == NULL)
      return UV_ENOMEM;
-@@ -1242,7 +1243,7 @@ static int uv__getpwuid_r(uv_passwd_t* pwd) {
+ 
+   /* Convert to UTF-16 */
+-  uv_wtf8_to_utf16(utf8, *utf16, bufsize);
++  uv_wtf8_to_utf16(utf8, (uint16_t*)*utf16, bufsize);
+ 
+   return 0;
+ }
+@@ -1065,11 +1066,11 @@ int uv__copy_utf16_to_utf8(const WCHAR* utf16buffer, size_t utf16len, char* utf8
+     return UV_EINVAL;
+ 
+   if (*size == 0) {
+-    *size = uv_utf16_length_as_wtf8(utf16buffer, utf16len);
++    *size = uv_utf16_length_as_wtf8((const uint16_t*)utf16buffer, utf16len);
+     r = UV_ENOBUFS;
+   } else {
+     *size -= 1; /* Reserve space for NUL. */
+-    r = uv_utf16_to_wtf8(utf16buffer, utf16len, &utf8, size);
++    r = uv_utf16_to_wtf8((const uint16_t*)utf16buffer, utf16len, &utf8, size);
+   }
+   if (r == UV_ENOBUFS)
+     *size += 1; /* Add space for NUL. */
+@@ -1099,7 +1100,7 @@ static int uv__getpwuid_r(uv_passwd_t* pwd) {
      return uv_translate_sys_error(r);
    }
  
@@ -1622,7 +1836,7 @@
    if (path == NULL) {
      CloseHandle(token);
      return UV_ENOMEM;
-@@ -1323,7 +1324,7 @@ int uv_os_environ(uv_env_item_t** envitems, int* count) {
+@@ -1180,7 +1181,7 @@ int uv_os_environ(uv_env_item_t** envitems, int* count) {
  
    for (penv = env, i = 0; *penv != L'\0'; penv += wcslen(penv) + 1, i++);
  
@@ -1631,7 +1845,7 @@
    if (*envitems == NULL) {
      FreeEnvironmentStringsW(env);
      return UV_ENOMEM;
-@@ -1413,7 +1414,7 @@ int uv_os_getenv(const char* name, char* buffer, size_t* size) {
+@@ -1269,7 +1270,7 @@ int uv_os_getenv(const char* name, char* buffer, size_t* size) {
        uv__free(var);
  
      varlen = 1 + len;
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0003-Fix-warnings.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0003-Fix-warnings.patch
index cbf5c70..1fdfff8 100644
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0003-Fix-warnings.patch
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0003-Fix-warnings.patch
@@ -1,16 +1,15 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 26 Apr 2022 15:09:43 -0400
-Subject: [PATCH 03/10] Fix warnings
+Subject: [PATCH 3/9] Fix warnings
 
 ---
  include/uv/win.h    |  5 +++++
- src/idna.c          |  2 +-
+ src/idna.c          |  4 ++--
  src/inet.c          |  4 ++++
  src/threadpool.c    |  4 ++++
  src/unix/core.c     | 12 ++++++++++--
  src/unix/internal.h |  4 ++--
- src/unix/linux.c    | 15 ++++++++++++---
  src/unix/thread.c   |  6 ------
  src/uv-common.c     |  8 ++++++++
  src/win/fs-event.c  |  2 ++
@@ -19,7 +18,7 @@
  src/win/process.c   |  2 ++
  src/win/thread.c    |  4 ++--
  src/win/tty.c       |  2 ++
- 15 files changed, 58 insertions(+), 16 deletions(-)
+ 14 files changed, 47 insertions(+), 14 deletions(-)
 
 diff --git a/include/uv/win.h b/include/uv/win.h
 index eb74776978340a4910194bae35a9da6493e8c0a6..6d0afe69e7dd4caf4c9459e548fe75cf0c51b501 100644
@@ -43,10 +42,10 @@
    typedef PVOID CONDITION_VARIABLE, *PCONDITION_VARIABLE;
  #endif
 diff --git a/src/idna.c b/src/idna.c
-index 93d982ca018f2d39d9c0ffab07998c2c637029eb..36a39a089019fb4c2a35ec84516658c392eec0a3 100644
+index 0c952cf605a88136ed9035f9385f9b1080c30c28..fe60fb59bea59986a612cfa1747802b0636482d9 100644
 --- a/src/idna.c
 +++ b/src/idna.c
-@@ -106,7 +106,7 @@ static int uv__idna_toascii_label(const char* s, const char* se,
+@@ -153,7 +153,7 @@ static int uv__idna_toascii_label(const char* s, const char* se,
                                    char** d, char* de) {
    static const char alphabet[] = "abcdefghijklmnopqrstuvwxyz0123456789";
    const char* ss;
@@ -55,6 +54,15 @@
    unsigned h;
    unsigned k;
    unsigned n;
+@@ -382,7 +382,7 @@ ssize_t uv_wtf8_length_as_utf16(const char* source_ptr) {
+ 
+ void uv_wtf8_to_utf16(const char* source_ptr,
+                       uint16_t* w_target,
+-                      size_t w_target_len) {
++                      [[maybe_unused]] size_t w_target_len) {
+   int32_t code_point;
+ 
+   do {
 diff --git a/src/inet.c b/src/inet.c
 index dd94bea3886ca37945fcad7909d765e3700e3c21..71c9e5b774d64d505e6c6d6ed2637178b8532d4d 100644
 --- a/src/inet.c
@@ -132,48 +140,8 @@
  
    s = strrchr(path, '/');
    if (s == NULL)
-diff --git a/src/unix/linux.c b/src/unix/linux.c
-index 9173850bd158eaf9c41deca38f9ba84762a027a1..157443792f1216c83b4221c3810d17c81c5913c4 100644
---- a/src/unix/linux.c
-+++ b/src/unix/linux.c
-@@ -1718,7 +1718,11 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) {
-     return UV__ERR(errno);
-   }
- 
--  fgets(buf, sizeof(buf), fp);  /* Skip first line. */
-+  /* Skip first line. */
-+  if (!fgets(buf, sizeof(buf), fp)) {
-+    uv__free(cpus);
-+    return UV__ERR(errno);
-+  }
- 
-   for (;;) {
-     memset(&t, 0, sizeof(t));
-@@ -1729,7 +1733,10 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) {
-     if (n != 7)
-       break;
- 
--    fgets(buf, sizeof(buf), fp);  /* Skip rest of line. */
-+    /* Skip rest of line. */
-+    if (!fgets(buf, sizeof(buf), fp)) {
-+      break;
-+    }
- 
-     if (cpu >= ARRAY_SIZE(*cpus))
-       continue;
-@@ -1809,7 +1816,9 @@ nocpuinfo:
-     if (fp == NULL)
-       continue;
- 
--    fscanf(fp, "%llu", &(*cpus)[cpu].freq);
-+    if (0 > fscanf(fp, "%llu", &(*cpus)[cpu].freq)) {
-+      (*cpus)[cpu].freq = 0llu;
-+    }
-     fclose(fp);
-     fp = NULL;
-   }
 diff --git a/src/unix/thread.c b/src/unix/thread.c
-index 531c6211bb4321e5f11031a0644b4e3ab9174396..f8600947e3e7df015c4302af4feee740707b2c46 100644
+index 20409541de3cb300504b823472a73bc95fa38f62..688c042e1aedf379264900c29758c8b01a4a90ed 100644
 --- a/src/unix/thread.c
 +++ b/src/unix/thread.c
 @@ -137,12 +137,6 @@ int uv_thread_create_ex(uv_thread_t* tid,
@@ -190,10 +158,10 @@
        params->flags & UV_THREAD_HAS_STACK_SIZE ? params->stack_size : 0;
  
 diff --git a/src/uv-common.c b/src/uv-common.c
-index c04f93596ab1f730576256d86e216ccb7f258b72..cd10b36b4a393e325ea03b93eb9897193ca9800b 100644
+index 69e95801a18104ea910abf86db236d85f62afb66..49126e50f07bac16d198775454b731f40630d1d1 100644
 --- a/src/uv-common.c
 +++ b/src/uv-common.c
-@@ -799,6 +799,10 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) {
+@@ -802,6 +802,10 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) {
    }
  }
  
@@ -204,7 +172,7 @@
  
  int uv_loop_configure(uv_loop_t* loop, uv_loop_option option, ...) {
    va_list ap;
-@@ -812,6 +816,10 @@ int uv_loop_configure(uv_loop_t* loop, uv_loop_option option, ...) {
+@@ -815,6 +819,10 @@ int uv_loop_configure(uv_loop_t* loop, uv_loop_option option, ...) {
    return err;
  }
  
@@ -216,7 +184,7 @@
  static uv_loop_t default_loop_struct;
  static uv_loop_t* default_loop_ptr;
 diff --git a/src/win/fs-event.c b/src/win/fs-event.c
-index 150467313d576bfe2966b55f3d8cffa23cbb8ea3..3244a4e4320d7ce98f226b49b2634c65de89c213 100644
+index 5a07acfe54efe90cf2ab0bca5b5998a961e72ebd..381220bf241d9572ff64576cf25700d9746d2806 100644
 --- a/src/win/fs-event.c
 +++ b/src/win/fs-event.c
 @@ -19,6 +19,8 @@
@@ -229,7 +197,7 @@
  #include <errno.h>
  #include <stdio.h>
 diff --git a/src/win/fs.c b/src/win/fs.c
-index 328c8f2e0513562b53c948ffea59d0841e14b264..565c05fff13c2e6e74091c1da7b31636d7fd370d 100644
+index d9c2a4f728c7fb491995c6153b2a63a835b206b7..7df1df2b9ba23c0ecb96e82a6cfcaae824826b69 100644
 --- a/src/win/fs.c
 +++ b/src/win/fs.c
 @@ -19,6 +19,8 @@
@@ -242,7 +210,7 @@
  #include <stdlib.h>
  #include <direct.h>
 diff --git a/src/win/pipe.c b/src/win/pipe.c
-index c1739efe82b8755999145860b4da6b50c73518a2..258d6a684c67f154096a25e7226f1a7d08b93d5b 100644
+index 0f045a873073cf9b07feb457ea199990df521e5f..b2348bd0430aada98ff10313f6f926ea2f1c4491 100644
 --- a/src/win/pipe.c
 +++ b/src/win/pipe.c
 @@ -19,6 +19,8 @@
@@ -255,7 +223,7 @@
  #include <io.h>
  #include <stdio.h>
 diff --git a/src/win/process.c b/src/win/process.c
-index b383e8b405db56d413985b38df216d09c58ec4a0..2b1b46259959867482079962d0ea44246a42e7cb 100644
+index 0103004c565bb09b7b9c150e874f9c68486ab26c..eeb0f6a984bb0f61b956cb4b907fbfdb3b4ac7e2 100644
 --- a/src/win/process.c
 +++ b/src/win/process.c
 @@ -19,6 +19,8 @@
@@ -283,7 +251,7 @@
          return UV_EINVAL;
      }
 diff --git a/src/win/tty.c b/src/win/tty.c
-index 7e1f15544b177382a774300f832bc982d85bd62b..abbe1315883257d6825b794344dcd4cba9514097 100644
+index 3aacaaf0b514ba71755dc51f1bd7263de5cbc308..c90995afb6166a7cab0a267f5eb513705ce05148 100644
 --- a/src/win/tty.c
 +++ b/src/win/tty.c
 @@ -19,6 +19,8 @@
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch
index c95b28a..2b887e6 100644
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 26 Apr 2022 15:19:14 -0400
-Subject: [PATCH 04/10] Preprocessor cleanup
+Subject: [PATCH 4/9] Preprocessor cleanup
 
 ---
  include/uv.h        | 12 ------------
@@ -14,7 +14,7 @@
  7 files changed, 14 insertions(+), 20 deletions(-)
 
 diff --git a/include/uv.h b/include/uv.h
-index 02397dd0fdd43d51f86c0dde9a62046702f12bdb..d5342b0d52232bbf83825948cc6bc09e5d74a4c7 100644
+index 5642101c10c351a602a195d3b74027b53bf641d2..aff06cb7f4352eb4fcf93c938f09a789318bddc3 100644
 --- a/include/uv.h
 +++ b/include/uv.h
 @@ -23,9 +23,6 @@
@@ -44,7 +44,7 @@
  UV_EXTERN uv_handle_type uv_guess_handle(uv_file file);
  
  enum {
-@@ -1906,7 +1897,4 @@ UV_EXTERN void uv_loop_set_data(uv_loop_t*, void* data);
+@@ -1918,7 +1909,4 @@ void uv_wtf8_to_utf16(const char* wtf8,
  #undef UV_LOOP_PRIVATE_PLATFORM_FIELDS
  #undef UV__ERR
  
@@ -85,19 +85,19 @@
  
  /* core */
 diff --git a/src/win/fs.c b/src/win/fs.c
-index 565c05fff13c2e6e74091c1da7b31636d7fd370d..f415ddc2c39d09eea317fc857777acce1ce7d13e 100644
+index 7df1df2b9ba23c0ecb96e82a6cfcaae824826b69..26ba539936786902af5830c2811594292815d2d7 100644
 --- a/src/win/fs.c
 +++ b/src/win/fs.c
-@@ -40,6 +40,7 @@
- 
- #include <winioctl.h>
+@@ -43,6 +43,7 @@
+ #include "handle-inl.h"
+ #include "fs-fd-hash-inl.h"
  
 +#pragma comment(lib, "Advapi32.lib")
  
  #define UV_FS_FREE_PATHS         0x0002
  #define UV_FS_FREE_PTR           0x0008
 diff --git a/src/win/tty.c b/src/win/tty.c
-index abbe1315883257d6825b794344dcd4cba9514097..9bb3d9e830c901122da5e521e0c6b032dfd5044c 100644
+index c90995afb6166a7cab0a267f5eb513705ce05148..7adf3cd3ba81a9c41064f4ef2af01667092f27d1 100644
 --- a/src/win/tty.c
 +++ b/src/win/tty.c
 @@ -37,6 +37,8 @@
@@ -110,7 +110,7 @@
  # define InterlockedOr _InterlockedOr
  #endif
 diff --git a/src/win/util.c b/src/win/util.c
-index 1cfd7b2caf0d4ad1a6a66df9406c21f4e2b69b04..af18cfa6c106c5de0996e4fff9b4127aaa3b576b 100644
+index 5fbc70f52952f90b36fb22ee03ec7f70e6ed5954..f505cfad4987768fa6470ca27612312c2ba0729d 100644
 --- a/src/win/util.c
 +++ b/src/win/util.c
 @@ -64,12 +64,20 @@
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0005-Cleanup-problematic-language.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0005-Cleanup-problematic-language.patch
index e016c2e..fbe7efe 100644
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0005-Cleanup-problematic-language.patch
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0005-Cleanup-problematic-language.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 26 Apr 2022 15:24:47 -0400
-Subject: [PATCH 05/10] Cleanup problematic language
+Subject: [PATCH 5/9] Cleanup problematic language
 
 ---
  src/unix/tty.c | 21 +++++++++++----------
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0006-Fix-Win32-warning-suppression-pragma.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0006-Fix-Win32-warning-suppression-pragma.patch
new file mode 100644
index 0000000..fe00159
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0006-Fix-Win32-warning-suppression-pragma.patch
@@ -0,0 +1,22 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Sat, 21 May 2022 22:58:06 -0700
+Subject: [PATCH 6/9] Fix Win32 warning suppression pragma
+
+---
+ src/win/util.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/src/win/util.c b/src/win/util.c
+index f505cfad4987768fa6470ca27612312c2ba0729d..8828857db76a54d6e184c7f5714a091e92eba8ed 100644
+--- a/src/win/util.c
++++ b/src/win/util.c
+@@ -1502,7 +1502,7 @@ int uv_os_uname(uv_utsname_t* buffer) {
+   } else {
+     /* Silence GetVersionEx() deprecation warning. */
+     #ifdef _MSC_VER
+-    #pragma warning(suppress : 4996)
++    #pragma warning(disable : 4996)
+     #endif
+     if (GetVersionExW(&os_info) == 0) {
+       r = uv_translate_sys_error(GetLastError());
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0006-Style-comments-cleanup.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0006-Style-comments-cleanup.patch
deleted file mode 100644
index d4aa9a2..0000000
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0006-Style-comments-cleanup.patch
+++ /dev/null
@@ -1,73 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: PJ Reiniger <pj.reiniger@gmail.com>
-Date: Tue, 26 Apr 2022 15:28:52 -0400
-Subject: [PATCH 06/10] Style / comments cleanup
-
----
- src/fs-poll.c     | 1 +
- src/unix/core.c   | 1 +
- src/uv-common.c   | 1 +
- src/win/process.c | 1 -
- src/win/winsock.c | 1 +
- 5 files changed, 4 insertions(+), 1 deletion(-)
-
-diff --git a/src/fs-poll.c b/src/fs-poll.c
-index 5a39daed095502b2db34f23fcaf0ab04f31f96ff..1a7ca70d62c71f6eaef2b9985796cc46a6438869 100644
---- a/src/fs-poll.c
-+++ b/src/fs-poll.c
-@@ -34,6 +34,7 @@
- #include <stdlib.h>
- #include <string.h>
- 
-+
- struct poll_ctx {
-   uv_fs_poll_t* parent_handle;
-   int busy_polling;
-diff --git a/src/unix/core.c b/src/unix/core.c
-index 268fc9652f437eb0d0cda2a9e0b06b9e91eb9742..f53adc156a7c454c492abaeac29d90be436785fc 100644
---- a/src/unix/core.c
-+++ b/src/unix/core.c
-@@ -575,6 +575,7 @@ int uv__accept(int sockfd) {
-   return peerfd;
- }
- 
-+
- #if defined(__APPLE__)
- #pragma GCC diagnostic push
- #pragma GCC diagnostic ignored "-Wdollar-in-identifier-extension"
-diff --git a/src/uv-common.c b/src/uv-common.c
-index cd10b36b4a393e325ea03b93eb9897193ca9800b..bfcc3ef10f4fd7763221638947da6e02e7a17c33 100644
---- a/src/uv-common.c
-+++ b/src/uv-common.c
-@@ -799,6 +799,7 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) {
-   }
- }
- 
-+
- #ifdef __clang__
- # pragma clang diagnostic push
- # pragma clang diagnostic ignored "-Wvarargs"
-diff --git a/src/win/process.c b/src/win/process.c
-index 2b1b46259959867482079962d0ea44246a42e7cb..18816d3b1e8c863f8ca74fe0104de1aecd0ae3fa 100644
---- a/src/win/process.c
-+++ b/src/win/process.c
-@@ -35,7 +35,6 @@
- #include "handle-inl.h"
- #include "req-inl.h"
- 
--
- #define SIGKILL         9
- 
- 
-diff --git a/src/win/winsock.c b/src/win/winsock.c
-index 7843e9f13321c32419cebaacde82d30f471e11d7..cda82bc33c2c6e3dbfa9d978b5b40476228452bd 100644
---- a/src/win/winsock.c
-+++ b/src/win/winsock.c
-@@ -25,6 +25,7 @@
- #include "uv.h"
- #include "internal.h"
- 
-+
- #pragma comment(lib, "Ws2_32.lib")
- 
- /* Whether there are any non-IFS LSPs stacked on TCP */
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0007-Fix-Win32-warning-suppression-pragma.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0007-Fix-Win32-warning-suppression-pragma.patch
deleted file mode 100644
index 5ef597c..0000000
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0007-Fix-Win32-warning-suppression-pragma.patch
+++ /dev/null
@@ -1,22 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Tyler Veness <calcmogul@gmail.com>
-Date: Sat, 21 May 2022 22:58:06 -0700
-Subject: [PATCH 07/10] Fix Win32 warning suppression pragma
-
----
- src/win/util.c | 2 +-
- 1 file changed, 1 insertion(+), 1 deletion(-)
-
-diff --git a/src/win/util.c b/src/win/util.c
-index af18cfa6c106c5de0996e4fff9b4127aaa3b576b..9324992ec521cc3496e3e9304e600963a3f20897 100644
---- a/src/win/util.c
-+++ b/src/win/util.c
-@@ -1692,7 +1692,7 @@ int uv_os_uname(uv_utsname_t* buffer) {
-   } else {
-     /* Silence GetVersionEx() deprecation warning. */
-     #ifdef _MSC_VER
--    #pragma warning(suppress : 4996)
-+    #pragma warning(disable : 4996)
-     #endif
-     if (GetVersionExW(&os_info) == 0) {
-       r = uv_translate_sys_error(GetLastError());
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0007-Use-C-atomics.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0007-Use-C-atomics.patch
new file mode 100644
index 0000000..7fea790
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0007-Use-C-atomics.patch
@@ -0,0 +1,481 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Thu, 13 Jul 2023 22:13:47 -0700
+Subject: [PATCH 7/9] Use C++ atomics
+
+---
+ src/unix/async.c  | 25 +++++++++++++------------
+ src/unix/core.c   |  3 ++-
+ src/unix/fs.c     | 20 +++++++++++---------
+ src/unix/kqueue.c | 10 ++++++----
+ src/unix/linux.c  | 45 +++++++++++++++++++++++----------------------
+ src/unix/tty.c    |  5 +++--
+ src/uv-common.c   |  2 +-
+ src/uv-common.h   |  8 +++-----
+ 8 files changed, 62 insertions(+), 56 deletions(-)
+
+diff --git a/src/unix/async.c b/src/unix/async.c
+index 0ff2669e30a628dbb2df9e28ba14b38cf14114e5..fef4ae93343edc0341179a1c4739dcd831ef6e26 100644
+--- a/src/unix/async.c
++++ b/src/unix/async.c
+@@ -26,7 +26,6 @@
+ #include "internal.h"
+ 
+ #include <errno.h>
+-#include <stdatomic.h>
+ #include <stdio.h>  /* snprintf() */
+ #include <assert.h>
+ #include <stdlib.h>
+@@ -38,6 +37,8 @@
+ #include <sys/eventfd.h>
+ #endif
+ 
++#include <atomic>
++
+ static void uv__async_send(uv_loop_t* loop);
+ static int uv__async_start(uv_loop_t* loop);
+ static void uv__cpu_relax(void);
+@@ -63,14 +64,14 @@ int uv_async_init(uv_loop_t* loop, uv_async_t* handle, uv_async_cb async_cb) {
+ 
+ 
+ int uv_async_send(uv_async_t* handle) {
+-  _Atomic int* pending;
+-  _Atomic int* busy;
++  std::atomic<int>* pending;
++  std::atomic<int>* busy;
+ 
+-  pending = (_Atomic int*) &handle->pending;
+-  busy = (_Atomic int*) &handle->u.fd;
++  pending = (std::atomic<int>*) &handle->pending;
++  busy = (std::atomic<int>*) &handle->u.fd;
+ 
+   /* Do a cheap read first. */
+-  if (atomic_load_explicit(pending, memory_order_relaxed) != 0)
++  if (atomic_load_explicit(pending, std::memory_order_relaxed) != 0)
+     return 0;
+ 
+   /* Set the loop to busy. */
+@@ -90,12 +91,12 @@ int uv_async_send(uv_async_t* handle) {
+ /* Wait for the busy flag to clear before closing.
+  * Only call this from the event loop thread. */
+ static void uv__async_spin(uv_async_t* handle) {
+-  _Atomic int* pending;
+-  _Atomic int* busy;
++  std::atomic<int>* pending;
++  std::atomic<int>* busy;
+   int i;
+ 
+-  pending = (_Atomic int*) &handle->pending;
+-  busy = (_Atomic int*) &handle->u.fd;
++  pending = (std::atomic<int>*) &handle->pending;
++  busy = (std::atomic<int>*) &handle->u.fd;
+ 
+   /* Set the pending flag first, so no new events will be added by other
+    * threads after this function returns. */
+@@ -135,7 +136,7 @@ static void uv__async_io(uv_loop_t* loop, uv__io_t* w, unsigned int events) {
+   struct uv__queue queue;
+   struct uv__queue* q;
+   uv_async_t* h;
+-  _Atomic int *pending;
++  std::atomic<int> *pending;
+ 
+   assert(w == &loop->async_io_watcher);
+ 
+@@ -166,7 +167,7 @@ static void uv__async_io(uv_loop_t* loop, uv__io_t* w, unsigned int events) {
+     uv__queue_insert_tail(&loop->async_handles, q);
+ 
+     /* Atomically fetch and clear pending flag */
+-    pending = (_Atomic int*) &h->pending;
++    pending = (std::atomic<int>*) &h->pending;
+     if (atomic_exchange(pending, 0) == 0)
+       continue;
+ 
+diff --git a/src/unix/core.c b/src/unix/core.c
+index 268fc9652f437eb0d0cda2a9e0b06b9e91eb9742..1a52716792d2dbbb71a4ebdd2255173b7447979b 100644
+--- a/src/unix/core.c
++++ b/src/unix/core.c
+@@ -45,6 +45,7 @@
+ #include <sys/utsname.h>
+ #include <sys/time.h>
+ #include <time.h> /* clock_gettime */
++#include <atomic>
+ 
+ #ifdef __sun
+ # include <sys/filio.h>
+@@ -263,7 +264,7 @@ int uv__getiovmax(void) {
+ #if defined(IOV_MAX)
+   return IOV_MAX;
+ #elif defined(_SC_IOV_MAX)
+-  static _Atomic int iovmax_cached = -1;
++  static std::atomic<int> iovmax_cached = -1;
+   int iovmax;
+ 
+   iovmax = atomic_load_explicit(&iovmax_cached, memory_order_relaxed);
+diff --git a/src/unix/fs.c b/src/unix/fs.c
+index bc00c90b07a32a823412b216df6f2d758dbc423b..afdf0c6e592c69961b1286a87b61e62601331763 100644
+--- a/src/unix/fs.c
++++ b/src/unix/fs.c
+@@ -45,6 +45,8 @@
+ #include <fcntl.h>
+ #include <poll.h>
+ 
++#include <atomic>
++
+ #if defined(__linux__)
+ # include <sys/sendfile.h>
+ #endif
+@@ -307,7 +309,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) {
+   static uv_once_t once = UV_ONCE_INIT;
+   int r;
+ #ifdef O_CLOEXEC
+-  static _Atomic int no_cloexec_support;
++  static std::atomic<int> no_cloexec_support;
+ #endif
+   static const char pattern[] = "XXXXXX";
+   static const size_t pattern_size = sizeof(pattern) - 1;
+@@ -332,7 +334,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) {
+   uv_once(&once, uv__mkostemp_initonce);
+ 
+ #ifdef O_CLOEXEC
+-  if (atomic_load_explicit(&no_cloexec_support, memory_order_relaxed) == 0 &&
++  if (atomic_load_explicit(&no_cloexec_support, std::memory_order_relaxed) == 0 &&
+       uv__mkostemp != NULL) {
+     r = uv__mkostemp(path, O_CLOEXEC);
+ 
+@@ -346,7 +348,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) {
+ 
+     /* We set the static variable so that next calls don't even
+        try to use mkostemp. */
+-    atomic_store_explicit(&no_cloexec_support, 1, memory_order_relaxed);
++    atomic_store_explicit(&no_cloexec_support, 1, std::memory_order_relaxed);
+   }
+ #endif  /* O_CLOEXEC */
+ 
+@@ -867,10 +869,10 @@ static int uv__is_cifs_or_smb(int fd) {
+ 
+ static ssize_t uv__fs_try_copy_file_range(int in_fd, off_t* off,
+                                           int out_fd, size_t len) {
+-  static _Atomic int no_copy_file_range_support;
++  static std::atomic<int> no_copy_file_range_support;
+   ssize_t r;
+ 
+-  if (atomic_load_explicit(&no_copy_file_range_support, memory_order_relaxed)) {
++  if (atomic_load_explicit(&no_copy_file_range_support, std::memory_order_relaxed)) {
+     errno = ENOSYS;
+     return -1;
+   }
+@@ -889,7 +891,7 @@ static ssize_t uv__fs_try_copy_file_range(int in_fd, off_t* off,
+       errno = ENOSYS;  /* Use fallback. */
+     break;
+   case ENOSYS:
+-    atomic_store_explicit(&no_copy_file_range_support, 1, memory_order_relaxed);
++    atomic_store_explicit(&no_copy_file_range_support, 1, std::memory_order_relaxed);
+     break;
+   case EPERM:
+     /* It's been reported that CIFS spuriously fails.
+@@ -1380,14 +1382,14 @@ static int uv__fs_statx(int fd,
+                         uv_stat_t* buf) {
+   STATIC_ASSERT(UV_ENOSYS != -1);
+ #ifdef __linux__
+-  static _Atomic int no_statx;
++  static std::atomic<int> no_statx;
+   struct uv__statx statxbuf;
+   int dirfd;
+   int flags;
+   int mode;
+   int rc;
+ 
+-  if (atomic_load_explicit(&no_statx, memory_order_relaxed))
++  if (atomic_load_explicit(&no_statx, std::memory_order_relaxed))
+     return UV_ENOSYS;
+ 
+   dirfd = AT_FDCWD;
+@@ -1421,7 +1423,7 @@ static int uv__fs_statx(int fd,
+      * implemented, rc might return 1 with 0 set as the error code in which
+      * case we return ENOSYS.
+      */
+-    atomic_store_explicit(&no_statx, 1, memory_order_relaxed);
++    atomic_store_explicit(&no_statx, 1, std::memory_order_relaxed);
+     return UV_ENOSYS;
+   }
+ 
+diff --git a/src/unix/kqueue.c b/src/unix/kqueue.c
+index 06fbdb24b4adc4adb781d32150d40836fa745531..939a42696778741f9184be29dcea7c858736d181 100644
+--- a/src/unix/kqueue.c
++++ b/src/unix/kqueue.c
+@@ -37,6 +37,8 @@
+ #include <fcntl.h>
+ #include <time.h>
+ 
++#include <atomic>
++
+ /*
+  * Required on
+  * - Until at least FreeBSD 11.0
+@@ -63,7 +65,7 @@ int uv__kqueue_init(uv_loop_t* loop) {
+ 
+ 
+ #if defined(__APPLE__) && MAC_OS_X_VERSION_MAX_ALLOWED >= 1070
+-static _Atomic int uv__has_forked_with_cfrunloop;
++static std::atomic<int> uv__has_forked_with_cfrunloop;
+ #endif
+ 
+ int uv__io_fork(uv_loop_t* loop) {
+@@ -87,7 +89,7 @@ int uv__io_fork(uv_loop_t* loop) {
+     */
+     atomic_store_explicit(&uv__has_forked_with_cfrunloop,
+                           1,
+-                          memory_order_relaxed);
++                          std::memory_order_relaxed);
+     uv__free(loop->cf_state);
+     loop->cf_state = NULL;
+   }
+@@ -555,7 +557,7 @@ int uv_fs_event_start(uv_fs_event_t* handle,
+     goto fallback;
+ 
+   if (0 == atomic_load_explicit(&uv__has_forked_with_cfrunloop,
+-                                memory_order_relaxed)) {
++                                std::memory_order_relaxed)) {
+     int r;
+     /* The fallback fd is no longer needed */
+     uv__close_nocheckstdio(fd);
+@@ -591,7 +593,7 @@ int uv_fs_event_stop(uv_fs_event_t* handle) {
+ 
+ #if defined(__APPLE__) && MAC_OS_X_VERSION_MAX_ALLOWED >= 1070
+   if (0 == atomic_load_explicit(&uv__has_forked_with_cfrunloop,
+-                                memory_order_relaxed))
++                                std::memory_order_relaxed))
+     if (handle->cf_cb != NULL)
+       r = uv__fsevents_close(handle);
+ #endif
+diff --git a/src/unix/linux.c b/src/unix/linux.c
+index 2b8e1d8fe593a181d049aa50ff9edaf6da258a24..b23d88bd824843eebc3b439e5a18e6f796a747be 100644
+--- a/src/unix/linux.c
++++ b/src/unix/linux.c
+@@ -27,7 +27,6 @@
+ #include "internal.h"
+ 
+ #include <inttypes.h>
+-#include <stdatomic.h>
+ #include <stddef.h>  /* offsetof */
+ #include <stdint.h>
+ #include <stdio.h>
+@@ -139,6 +138,8 @@
+ # include <netpacket/packet.h>
+ #endif /* HAVE_IFADDRS_H */
+ 
++#include <atomic>
++
+ enum {
+   UV__IORING_SETUP_SQPOLL = 2u,
+ };
+@@ -317,7 +318,7 @@ static struct watcher_root* uv__inotify_watchers(uv_loop_t* loop) {
+ 
+ 
+ unsigned uv__kernel_version(void) {
+-  static _Atomic unsigned cached_version;
++  static std::atomic<unsigned int> cached_version;
+   struct utsname u;
+   unsigned version;
+   unsigned major;
+@@ -326,7 +327,7 @@ unsigned uv__kernel_version(void) {
+   char v_sig[256];
+   char* needle;
+ 
+-  version = atomic_load_explicit(&cached_version, memory_order_relaxed);
++  version = std::atomic_load_explicit(&cached_version, std::memory_order_relaxed);
+   if (version != 0)
+     return version;
+ 
+@@ -382,7 +383,7 @@ unsigned uv__kernel_version(void) {
+ 
+ calculate_version:
+   version = major * 65536 + minor * 256 + patch;
+-  atomic_store_explicit(&cached_version, version, memory_order_relaxed);
++  std::atomic_store_explicit(&cached_version, version, std::memory_order_relaxed);
+ 
+   return version;
+ }
+@@ -480,11 +481,11 @@ static int uv__use_io_uring(void) {
+   return 0;  /* All 32 bits kernels appear buggy. */
+ #else
+   /* Ternary: unknown=0, yes=1, no=-1 */
+-  static _Atomic int use_io_uring;
++  static std::atomic<int> use_io_uring;
+   char* val;
+   int use;
+ 
+-  use = atomic_load_explicit(&use_io_uring, memory_order_relaxed);
++  use = std::atomic_load_explicit(&use_io_uring, std::memory_order_relaxed);
+ 
+   if (use == 0) {
+     /* Older kernels have a bug where the sqpoll thread uses 100% CPU. */
+@@ -495,7 +496,7 @@ static int uv__use_io_uring(void) {
+     if (val != NULL)
+       use = atoi(val) ? 1 : -1;
+ 
+-    atomic_store_explicit(&use_io_uring, use, memory_order_relaxed);
++    std::atomic_store_explicit(&use_io_uring, use, std::memory_order_relaxed);
+   }
+ 
+   return use > 0;
+@@ -771,8 +772,8 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou,
+   if (iou->ringfd == -1)
+     return NULL;
+ 
+-  head = atomic_load_explicit((_Atomic uint32_t*) iou->sqhead,
+-                              memory_order_acquire);
++  head = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->sqhead,
++                              std::memory_order_acquire);
+   tail = *iou->sqtail;
+   mask = iou->sqmask;
+ 
+@@ -801,12 +802,12 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou,
+ static void uv__iou_submit(struct uv__iou* iou) {
+   uint32_t flags;
+ 
+-  atomic_store_explicit((_Atomic uint32_t*) iou->sqtail,
++  std::atomic_store_explicit((std::atomic<uint32_t>*) iou->sqtail,
+                         *iou->sqtail + 1,
+-                        memory_order_release);
++                        std::memory_order_release);
+ 
+-  flags = atomic_load_explicit((_Atomic uint32_t*) iou->sqflags,
+-                               memory_order_acquire);
++  flags = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->sqflags,
++                               std::memory_order_acquire);
+ 
+   if (flags & UV__IORING_SQ_NEED_WAKEUP)
+     if (uv__io_uring_enter(iou->ringfd, 0, 0, UV__IORING_ENTER_SQ_WAKEUP))
+@@ -1147,8 +1148,8 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) {
+   int rc;
+ 
+   head = *iou->cqhead;
+-  tail = atomic_load_explicit((_Atomic uint32_t*) iou->cqtail,
+-                              memory_order_acquire);
++  tail = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->cqtail,
++                              std::memory_order_acquire);
+   mask = iou->cqmask;
+   cqe = (uv__io_uring_cqe*)iou->cqe;
+   nevents = 0;
+@@ -1180,15 +1181,15 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) {
+     nevents++;
+   }
+ 
+-  atomic_store_explicit((_Atomic uint32_t*) iou->cqhead,
++  std::atomic_store_explicit((std::atomic<uint32_t>*) iou->cqhead,
+                         tail,
+-                        memory_order_release);
++                        std::memory_order_release);
+ 
+   /* Check whether CQE's overflowed, if so enter the kernel to make them
+    * available. Don't grab them immediately but in the next loop iteration to
+    * avoid loop starvation. */
+-  flags = atomic_load_explicit((_Atomic uint32_t*) iou->sqflags,
+-                               memory_order_acquire);
++  flags = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->sqflags,
++                               std::memory_order_acquire);
+ 
+   if (flags & UV__IORING_SQ_CQ_OVERFLOW) {
+     do
+@@ -1581,7 +1582,7 @@ update_timeout:
+ }
+ 
+ uint64_t uv__hrtime(uv_clocktype_t type) {
+-  static _Atomic clock_t fast_clock_id = -1;
++  static std::atomic<clock_t> fast_clock_id = -1;
+   struct timespec t;
+   clock_t clock_id;
+ 
+@@ -1597,7 +1598,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) {
+   if (type != UV_CLOCK_FAST)
+     goto done;
+ 
+-  clock_id = atomic_load_explicit(&fast_clock_id, memory_order_relaxed);
++  clock_id = std::atomic_load_explicit(&fast_clock_id, std::memory_order_relaxed);
+   if (clock_id != -1)
+     goto done;
+ 
+@@ -1606,7 +1607,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) {
+     if (t.tv_nsec <= 1 * 1000 * 1000)
+       clock_id = CLOCK_MONOTONIC_COARSE;
+ 
+-  atomic_store_explicit(&fast_clock_id, clock_id, memory_order_relaxed);
++  std::atomic_store_explicit(&fast_clock_id, clock_id, std::memory_order_relaxed);
+ 
+ done:
+ 
+diff --git a/src/unix/tty.c b/src/unix/tty.c
+index 1bd217b5a15eed13a8349c479b53471dd36ca216..1304c6d8685cfd122cffea066dc668d1dfc9ae02 100644
+--- a/src/unix/tty.c
++++ b/src/unix/tty.c
+@@ -22,7 +22,6 @@
+ #include "uv.h"
+ #include "internal.h"
+ 
+-#include <stdatomic.h>
+ #include <stdlib.h>
+ #include <assert.h>
+ #include <unistd.h>
+@@ -30,6 +29,8 @@
+ #include <errno.h>
+ #include <sys/ioctl.h>
+ 
++#include <atomic>
++
+ #if defined(__MVS__) && !defined(IMAXBEL)
+ #define IMAXBEL 0
+ #endif
+@@ -64,7 +65,7 @@ static int isreallyatty(int file) {
+ 
+ static int orig_termios_fd = -1;
+ static struct termios orig_termios;
+-static _Atomic int termios_spinlock;
++static std::atomic<int> termios_spinlock;
+ 
+ int uv__tcsetattr(int fd, int how, const struct termios *term) {
+   int rc;
+diff --git a/src/uv-common.c b/src/uv-common.c
+index 49126e50f07bac16d198775454b731f40630d1d1..1ce25c24d6c046f7aaeaa52dcfc4fafa5a738650 100644
+--- a/src/uv-common.c
++++ b/src/uv-common.c
+@@ -953,7 +953,7 @@ void uv_free_cpu_info(uv_cpu_info_t* cpu_infos, int count) {
+ __attribute__((destructor))
+ #endif
+ void uv_library_shutdown(void) {
+-  static int was_shutdown;
++  static std::atomic<int> was_shutdown;
+ 
+   if (uv__exchange_int_relaxed(&was_shutdown, 1))
+     return;
+diff --git a/src/uv-common.h b/src/uv-common.h
+index cd57e5a35153d0557351b60cce0c5be7a4468b60..5dce8eaf2705b47935b218181f6dd69af0d5b61b 100644
+--- a/src/uv-common.h
++++ b/src/uv-common.h
+@@ -32,15 +32,13 @@
+ #include <stddef.h>
+ #include <stdint.h>
+ 
++#include <atomic>
++
+ #include "uv.h"
+ #include "uv/tree.h"
+ #include "queue.h"
+ #include "strscpy.h"
+ 
+-#ifndef _MSC_VER
+-# include <stdatomic.h>
+-#endif
+-
+ #if EDOM > 0
+ # define UV__ERR(x) (-(x))
+ #else
+@@ -70,7 +68,7 @@ extern int snprintf(char*, size_t, const char*, ...);
+   InterlockedExchangeNoFence((LONG volatile*)(p), v)
+ #else
+ #define uv__exchange_int_relaxed(p, v)                                        \
+-  atomic_exchange_explicit((_Atomic int*)(p), v, memory_order_relaxed)
++  std::atomic_exchange_explicit((std::atomic<int>*)(p), v, std::memory_order_relaxed)
+ #endif
+ 
+ #define UV__UDP_DGRAM_MAXSIZE (64 * 1024)
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0008-Remove-static-from-array-indices.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0008-Remove-static-from-array-indices.patch
new file mode 100644
index 0000000..0b7ab19
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0008-Remove-static-from-array-indices.patch
@@ -0,0 +1,67 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Thu, 13 Jul 2023 23:30:58 -0700
+Subject: [PATCH 8/9] Remove static from array indices
+
+---
+ src/unix/linux.c | 12 ++++++------
+ 1 file changed, 6 insertions(+), 6 deletions(-)
+
+diff --git a/src/unix/linux.c b/src/unix/linux.c
+index b23d88bd824843eebc3b439e5a18e6f796a747be..95a2fe2b42895626f207f740bdccfe938915d836 100644
+--- a/src/unix/linux.c
++++ b/src/unix/linux.c
+@@ -2104,7 +2104,7 @@ static uint64_t uv__read_uint64(const char* filename) {
+  * finds the location and length of the memory controller mount path.
+  * This disregards the leading / for easy concatenation of paths.
+  * Returns NULL if the memory controller wasn't found. */
+-static char* uv__cgroup1_find_memory_controller(char buf[static 1024],
++static char* uv__cgroup1_find_memory_controller(char buf[1024],
+                                                 int* n) {
+   char* p;
+ 
+@@ -2125,7 +2125,7 @@ static char* uv__cgroup1_find_memory_controller(char buf[static 1024],
+   return p;
+ }
+ 
+-static void uv__get_cgroup1_memory_limits(char buf[static 1024], uint64_t* high,
++static void uv__get_cgroup1_memory_limits(char buf[1024], uint64_t* high,
+                                           uint64_t* max) {
+   char filename[4097];
+   char* p;
+@@ -2165,7 +2165,7 @@ update_limits:
+     *max = UINT64_MAX;
+ }
+ 
+-static void uv__get_cgroup2_memory_limits(char buf[static 1024], uint64_t* high,
++static void uv__get_cgroup2_memory_limits(char buf[1024], uint64_t* high,
+                                           uint64_t* max) {
+   char filename[4097];
+   char* p;
+@@ -2182,7 +2182,7 @@ static void uv__get_cgroup2_memory_limits(char buf[static 1024], uint64_t* high,
+   *high = uv__read_uint64(filename);
+ }
+ 
+-static uint64_t uv__get_cgroup_constrained_memory(char buf[static 1024]) {
++static uint64_t uv__get_cgroup_constrained_memory(char buf[1024]) {
+   uint64_t high;
+   uint64_t max;
+ 
+@@ -2208,7 +2208,7 @@ uint64_t uv_get_constrained_memory(void) {
+ }
+ 
+ 
+-static uint64_t uv__get_cgroup1_current_memory(char buf[static 1024]) {
++static uint64_t uv__get_cgroup1_current_memory(char buf[1024]) {
+   char filename[4097];
+   uint64_t current;
+   char* p;
+@@ -2232,7 +2232,7 @@ static uint64_t uv__get_cgroup1_current_memory(char buf[static 1024]) {
+   return uv__read_uint64("/sys/fs/cgroup/memory/memory.usage_in_bytes");
+ }
+ 
+-static uint64_t uv__get_cgroup2_current_memory(char buf[static 1024]) {
++static uint64_t uv__get_cgroup2_current_memory(char buf[1024]) {
+   char filename[4097];
+   char* p;
+   int n;
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0008-Use-C-atomics.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0008-Use-C-atomics.patch
deleted file mode 100644
index ffdc588..0000000
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0008-Use-C-atomics.patch
+++ /dev/null
@@ -1,531 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Tyler Veness <calcmogul@gmail.com>
-Date: Thu, 13 Jul 2023 22:13:47 -0700
-Subject: [PATCH 08/10] Use C++ atomics
-
----
- src/unix/async.c  | 25 +++++++++++++------------
- src/unix/core.c   |  3 ++-
- src/unix/fs.c     | 32 +++++++++++++++++---------------
- src/unix/kqueue.c | 10 ++++++----
- src/unix/linux.c  | 45 +++++++++++++++++++++++----------------------
- src/unix/tty.c    |  5 +++--
- src/uv-common.c   |  2 +-
- src/uv-common.h   |  8 +++-----
- 8 files changed, 68 insertions(+), 62 deletions(-)
-
-diff --git a/src/unix/async.c b/src/unix/async.c
-index 0ff2669e30a628dbb2df9e28ba14b38cf14114e5..fef4ae93343edc0341179a1c4739dcd831ef6e26 100644
---- a/src/unix/async.c
-+++ b/src/unix/async.c
-@@ -26,7 +26,6 @@
- #include "internal.h"
- 
- #include <errno.h>
--#include <stdatomic.h>
- #include <stdio.h>  /* snprintf() */
- #include <assert.h>
- #include <stdlib.h>
-@@ -38,6 +37,8 @@
- #include <sys/eventfd.h>
- #endif
- 
-+#include <atomic>
-+
- static void uv__async_send(uv_loop_t* loop);
- static int uv__async_start(uv_loop_t* loop);
- static void uv__cpu_relax(void);
-@@ -63,14 +64,14 @@ int uv_async_init(uv_loop_t* loop, uv_async_t* handle, uv_async_cb async_cb) {
- 
- 
- int uv_async_send(uv_async_t* handle) {
--  _Atomic int* pending;
--  _Atomic int* busy;
-+  std::atomic<int>* pending;
-+  std::atomic<int>* busy;
- 
--  pending = (_Atomic int*) &handle->pending;
--  busy = (_Atomic int*) &handle->u.fd;
-+  pending = (std::atomic<int>*) &handle->pending;
-+  busy = (std::atomic<int>*) &handle->u.fd;
- 
-   /* Do a cheap read first. */
--  if (atomic_load_explicit(pending, memory_order_relaxed) != 0)
-+  if (atomic_load_explicit(pending, std::memory_order_relaxed) != 0)
-     return 0;
- 
-   /* Set the loop to busy. */
-@@ -90,12 +91,12 @@ int uv_async_send(uv_async_t* handle) {
- /* Wait for the busy flag to clear before closing.
-  * Only call this from the event loop thread. */
- static void uv__async_spin(uv_async_t* handle) {
--  _Atomic int* pending;
--  _Atomic int* busy;
-+  std::atomic<int>* pending;
-+  std::atomic<int>* busy;
-   int i;
- 
--  pending = (_Atomic int*) &handle->pending;
--  busy = (_Atomic int*) &handle->u.fd;
-+  pending = (std::atomic<int>*) &handle->pending;
-+  busy = (std::atomic<int>*) &handle->u.fd;
- 
-   /* Set the pending flag first, so no new events will be added by other
-    * threads after this function returns. */
-@@ -135,7 +136,7 @@ static void uv__async_io(uv_loop_t* loop, uv__io_t* w, unsigned int events) {
-   struct uv__queue queue;
-   struct uv__queue* q;
-   uv_async_t* h;
--  _Atomic int *pending;
-+  std::atomic<int> *pending;
- 
-   assert(w == &loop->async_io_watcher);
- 
-@@ -166,7 +167,7 @@ static void uv__async_io(uv_loop_t* loop, uv__io_t* w, unsigned int events) {
-     uv__queue_insert_tail(&loop->async_handles, q);
- 
-     /* Atomically fetch and clear pending flag */
--    pending = (_Atomic int*) &h->pending;
-+    pending = (std::atomic<int>*) &h->pending;
-     if (atomic_exchange(pending, 0) == 0)
-       continue;
- 
-diff --git a/src/unix/core.c b/src/unix/core.c
-index f53adc156a7c454c492abaeac29d90be436785fc..ce7fd2cdfdd53410dc694450bd56dffc26ff4792 100644
---- a/src/unix/core.c
-+++ b/src/unix/core.c
-@@ -45,6 +45,7 @@
- #include <sys/utsname.h>
- #include <sys/time.h>
- #include <time.h> /* clock_gettime */
-+#include <atomic>
- 
- #ifdef __sun
- # include <sys/filio.h>
-@@ -263,7 +264,7 @@ int uv__getiovmax(void) {
- #if defined(IOV_MAX)
-   return IOV_MAX;
- #elif defined(_SC_IOV_MAX)
--  static _Atomic int iovmax_cached = -1;
-+  static std::atomic<int> iovmax_cached = -1;
-   int iovmax;
- 
-   iovmax = atomic_load_explicit(&iovmax_cached, memory_order_relaxed);
-diff --git a/src/unix/fs.c b/src/unix/fs.c
-index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7fa86214b 100644
---- a/src/unix/fs.c
-+++ b/src/unix/fs.c
-@@ -46,6 +46,8 @@
- #include <fcntl.h>
- #include <poll.h>
- 
-+#include <atomic>
-+
- #if defined(__DragonFly__)        ||                                      \
-     defined(__FreeBSD__)          ||                                      \
-     defined(__OpenBSD__)          ||                                      \
-@@ -313,7 +315,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) {
-   static uv_once_t once = UV_ONCE_INIT;
-   int r;
- #ifdef O_CLOEXEC
--  static _Atomic int no_cloexec_support;
-+  static std::atomic<int> no_cloexec_support;
- #endif
-   static const char pattern[] = "XXXXXX";
-   static const size_t pattern_size = sizeof(pattern) - 1;
-@@ -338,7 +340,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) {
-   uv_once(&once, uv__mkostemp_initonce);
- 
- #ifdef O_CLOEXEC
--  if (atomic_load_explicit(&no_cloexec_support, memory_order_relaxed) == 0 &&
-+  if (atomic_load_explicit(&no_cloexec_support, std::memory_order_relaxed) == 0 &&
-       uv__mkostemp != NULL) {
-     r = uv__mkostemp(path, O_CLOEXEC);
- 
-@@ -352,7 +354,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) {
- 
-     /* We set the static variable so that next calls don't even
-        try to use mkostemp. */
--    atomic_store_explicit(&no_cloexec_support, 1, memory_order_relaxed);
-+    atomic_store_explicit(&no_cloexec_support, 1, std::memory_order_relaxed);
-   }
- #endif  /* O_CLOEXEC */
- 
-@@ -462,7 +464,7 @@ static ssize_t uv__fs_preadv(uv_file fd,
- 
- static ssize_t uv__fs_read(uv_fs_t* req) {
- #if TRY_PREADV
--  static _Atomic int no_preadv;
-+  static std::atomic<int> no_preadv;
- #endif
-   unsigned int iovmax;
-   ssize_t result;
-@@ -486,7 +488,7 @@ static ssize_t uv__fs_read(uv_fs_t* req) {
-     result = preadv(req->file, (struct iovec*) req->bufs, req->nbufs, req->off);
- #else
- # if TRY_PREADV
--    if (atomic_load_explicit(&no_preadv, memory_order_relaxed)) retry:
-+    if (atomic_load_explicit(&no_preadv, std::memory_order_relaxed)) retry:
- # endif
-     {
-       result = uv__fs_preadv(req->file, req->bufs, req->nbufs, req->off);
-@@ -498,7 +500,7 @@ static ssize_t uv__fs_read(uv_fs_t* req) {
-                       req->nbufs,
-                       req->off);
-       if (result == -1 && errno == ENOSYS) {
--        atomic_store_explicit(&no_preadv, 1, memory_order_relaxed);
-+        atomic_store_explicit(&no_preadv, 1, std::memory_order_relaxed);
-         goto retry;
-       }
-     }
-@@ -939,10 +941,10 @@ static int uv__is_cifs_or_smb(int fd) {
- 
- static ssize_t uv__fs_try_copy_file_range(int in_fd, off_t* off,
-                                           int out_fd, size_t len) {
--  static _Atomic int no_copy_file_range_support;
-+  static std::atomic<int> no_copy_file_range_support;
-   ssize_t r;
- 
--  if (atomic_load_explicit(&no_copy_file_range_support, memory_order_relaxed)) {
-+  if (atomic_load_explicit(&no_copy_file_range_support, std::memory_order_relaxed)) {
-     errno = ENOSYS;
-     return -1;
-   }
-@@ -961,7 +963,7 @@ static ssize_t uv__fs_try_copy_file_range(int in_fd, off_t* off,
-       errno = ENOSYS;  /* Use fallback. */
-     break;
-   case ENOSYS:
--    atomic_store_explicit(&no_copy_file_range_support, 1, memory_order_relaxed);
-+    atomic_store_explicit(&no_copy_file_range_support, 1, std::memory_order_relaxed);
-     break;
-   case EPERM:
-     /* It's been reported that CIFS spuriously fails.
-@@ -1162,7 +1164,7 @@ static ssize_t uv__fs_lutime(uv_fs_t* req) {
- 
- static ssize_t uv__fs_write(uv_fs_t* req) {
- #if TRY_PREADV
--  static _Atomic int no_pwritev;
-+  static std::atomic<int> no_pwritev;
- #endif
-   ssize_t r;
- 
-@@ -1191,7 +1193,7 @@ static ssize_t uv__fs_write(uv_fs_t* req) {
-     r = pwritev(req->file, (struct iovec*) req->bufs, req->nbufs, req->off);
- #else
- # if TRY_PREADV
--    if (atomic_load_explicit(&no_pwritev, memory_order_relaxed)) retry:
-+    if (atomic_load_explicit(&no_pwritev, std::memory_order_relaxed)) retry:
- # endif
-     {
-       r = pwrite(req->file, req->bufs[0].base, req->bufs[0].len, req->off);
-@@ -1203,7 +1205,7 @@ static ssize_t uv__fs_write(uv_fs_t* req) {
-                   req->nbufs,
-                   req->off);
-       if (r == -1 && errno == ENOSYS) {
--        atomic_store_explicit(&no_pwritev, 1, memory_order_relaxed);
-+        atomic_store_explicit(&no_pwritev, 1, std::memory_order_relaxed);
-         goto retry;
-       }
-     }
-@@ -1483,14 +1485,14 @@ static int uv__fs_statx(int fd,
-                         uv_stat_t* buf) {
-   STATIC_ASSERT(UV_ENOSYS != -1);
- #ifdef __linux__
--  static _Atomic int no_statx;
-+  static std::atomic<int> no_statx;
-   struct uv__statx statxbuf;
-   int dirfd;
-   int flags;
-   int mode;
-   int rc;
- 
--  if (atomic_load_explicit(&no_statx, memory_order_relaxed))
-+  if (atomic_load_explicit(&no_statx, std::memory_order_relaxed))
-     return UV_ENOSYS;
- 
-   dirfd = AT_FDCWD;
-@@ -1524,7 +1526,7 @@ static int uv__fs_statx(int fd,
-      * implemented, rc might return 1 with 0 set as the error code in which
-      * case we return ENOSYS.
-      */
--    atomic_store_explicit(&no_statx, 1, memory_order_relaxed);
-+    atomic_store_explicit(&no_statx, 1, std::memory_order_relaxed);
-     return UV_ENOSYS;
-   }
- 
-diff --git a/src/unix/kqueue.c b/src/unix/kqueue.c
-index 28e55aae6c613576ede7024a5c73d746e134d865..ffe0f9191cc7b0c233447db358077d8814e0217e 100644
---- a/src/unix/kqueue.c
-+++ b/src/unix/kqueue.c
-@@ -34,6 +34,8 @@
- #include <fcntl.h>
- #include <time.h>
- 
-+#include <atomic>
-+
- /*
-  * Required on
-  * - Until at least FreeBSD 11.0
-@@ -60,7 +62,7 @@ int uv__kqueue_init(uv_loop_t* loop) {
- 
- 
- #if defined(__APPLE__) && MAC_OS_X_VERSION_MAX_ALLOWED >= 1070
--static _Atomic int uv__has_forked_with_cfrunloop;
-+static std::atomic<int> uv__has_forked_with_cfrunloop;
- #endif
- 
- int uv__io_fork(uv_loop_t* loop) {
-@@ -84,7 +86,7 @@ int uv__io_fork(uv_loop_t* loop) {
-     */
-     atomic_store_explicit(&uv__has_forked_with_cfrunloop,
-                           1,
--                          memory_order_relaxed);
-+                          std::memory_order_relaxed);
-     uv__free(loop->cf_state);
-     loop->cf_state = NULL;
-   }
-@@ -541,7 +543,7 @@ int uv_fs_event_start(uv_fs_event_t* handle,
-     goto fallback;
- 
-   if (0 == atomic_load_explicit(&uv__has_forked_with_cfrunloop,
--                                memory_order_relaxed)) {
-+                                std::memory_order_relaxed)) {
-     int r;
-     /* The fallback fd is no longer needed */
-     uv__close_nocheckstdio(fd);
-@@ -577,7 +579,7 @@ int uv_fs_event_stop(uv_fs_event_t* handle) {
- 
- #if defined(__APPLE__) && MAC_OS_X_VERSION_MAX_ALLOWED >= 1070
-   if (0 == atomic_load_explicit(&uv__has_forked_with_cfrunloop,
--                                memory_order_relaxed))
-+                                std::memory_order_relaxed))
-     if (handle->cf_cb != NULL)
-       r = uv__fsevents_close(handle);
- #endif
-diff --git a/src/unix/linux.c b/src/unix/linux.c
-index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d693e0913c6 100644
---- a/src/unix/linux.c
-+++ b/src/unix/linux.c
-@@ -27,7 +27,6 @@
- #include "internal.h"
- 
- #include <inttypes.h>
--#include <stdatomic.h>
- #include <stddef.h>  /* offsetof */
- #include <stdint.h>
- #include <stdio.h>
-@@ -133,6 +132,8 @@
- # include <netpacket/packet.h>
- #endif /* HAVE_IFADDRS_H */
- 
-+#include <atomic>
-+
- enum {
-   UV__IORING_SETUP_SQPOLL = 2u,
- };
-@@ -311,14 +312,14 @@ static struct watcher_root* uv__inotify_watchers(uv_loop_t* loop) {
- 
- 
- unsigned uv__kernel_version(void) {
--  static _Atomic unsigned cached_version;
-+  static std::atomic<unsigned int> cached_version;
-   struct utsname u;
-   unsigned version;
-   unsigned major;
-   unsigned minor;
-   unsigned patch;
- 
--  version = atomic_load_explicit(&cached_version, memory_order_relaxed);
-+  version = std::atomic_load_explicit(&cached_version, std::memory_order_relaxed);
-   if (version != 0)
-     return version;
- 
-@@ -329,7 +330,7 @@ unsigned uv__kernel_version(void) {
-     return 0;
- 
-   version = major * 65536 + minor * 256 + patch;
--  atomic_store_explicit(&cached_version, version, memory_order_relaxed);
-+  std::atomic_store_explicit(&cached_version, version, std::memory_order_relaxed);
- 
-   return version;
- }
-@@ -424,16 +425,16 @@ static int uv__use_io_uring(void) {
-   return 0;  /* Possibly available but blocked by seccomp. */
- #else
-   /* Ternary: unknown=0, yes=1, no=-1 */
--  static _Atomic int use_io_uring;
-+  static std::atomic<int> use_io_uring;
-   char* val;
-   int use;
- 
--  use = atomic_load_explicit(&use_io_uring, memory_order_relaxed);
-+  use = std::atomic_load_explicit(&use_io_uring, std::memory_order_relaxed);
- 
-   if (use == 0) {
-     val = getenv("UV_USE_IO_URING");
-     use = val == NULL || atoi(val) ? 1 : -1;
--    atomic_store_explicit(&use_io_uring, use, memory_order_relaxed);
-+    std::atomic_store_explicit(&use_io_uring, use, std::memory_order_relaxed);
-   }
- 
-   return use > 0;
-@@ -709,8 +710,8 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou,
-   if (iou->ringfd == -1)
-     return NULL;
- 
--  head = atomic_load_explicit((_Atomic uint32_t*) iou->sqhead,
--                              memory_order_acquire);
-+  head = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->sqhead,
-+                              std::memory_order_acquire);
-   tail = *iou->sqtail;
-   mask = iou->sqmask;
- 
-@@ -739,12 +740,12 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou,
- static void uv__iou_submit(struct uv__iou* iou) {
-   uint32_t flags;
- 
--  atomic_store_explicit((_Atomic uint32_t*) iou->sqtail,
-+  std::atomic_store_explicit((std::atomic<uint32_t>*) iou->sqtail,
-                         *iou->sqtail + 1,
--                        memory_order_release);
-+                        std::memory_order_release);
- 
--  flags = atomic_load_explicit((_Atomic uint32_t*) iou->sqflags,
--                               memory_order_acquire);
-+  flags = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->sqflags,
-+                               std::memory_order_acquire);
- 
-   if (flags & UV__IORING_SQ_NEED_WAKEUP)
-     if (uv__io_uring_enter(iou->ringfd, 0, 0, UV__IORING_ENTER_SQ_WAKEUP))
-@@ -1076,8 +1077,8 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) {
-   int rc;
- 
-   head = *iou->cqhead;
--  tail = atomic_load_explicit((_Atomic uint32_t*) iou->cqtail,
--                              memory_order_acquire);
-+  tail = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->cqtail,
-+                              std::memory_order_acquire);
-   mask = iou->cqmask;
-   cqe = (uv__io_uring_cqe*)iou->cqe;
-   nevents = 0;
-@@ -1109,15 +1110,15 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) {
-     nevents++;
-   }
- 
--  atomic_store_explicit((_Atomic uint32_t*) iou->cqhead,
-+  std::atomic_store_explicit((std::atomic<uint32_t>*) iou->cqhead,
-                         tail,
--                        memory_order_release);
-+                        std::memory_order_release);
- 
-   /* Check whether CQE's overflowed, if so enter the kernel to make them
-    * available. Don't grab them immediately but in the next loop iteration to
-    * avoid loop starvation. */
--  flags = atomic_load_explicit((_Atomic uint32_t*) iou->sqflags,
--                               memory_order_acquire);
-+  flags = std::atomic_load_explicit((std::atomic<uint32_t>*) iou->sqflags,
-+                               std::memory_order_acquire);
- 
-   if (flags & UV__IORING_SQ_CQ_OVERFLOW) {
-     do
-@@ -1531,7 +1532,7 @@ update_timeout:
- }
- 
- uint64_t uv__hrtime(uv_clocktype_t type) {
--  static _Atomic clock_t fast_clock_id = -1;
-+  static std::atomic<clock_t> fast_clock_id = -1;
-   struct timespec t;
-   clock_t clock_id;
- 
-@@ -1547,7 +1548,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) {
-   if (type != UV_CLOCK_FAST)
-     goto done;
- 
--  clock_id = atomic_load_explicit(&fast_clock_id, memory_order_relaxed);
-+  clock_id = std::atomic_load_explicit(&fast_clock_id, std::memory_order_relaxed);
-   if (clock_id != -1)
-     goto done;
- 
-@@ -1556,7 +1557,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) {
-     if (t.tv_nsec <= 1 * 1000 * 1000)
-       clock_id = CLOCK_MONOTONIC_COARSE;
- 
--  atomic_store_explicit(&fast_clock_id, clock_id, memory_order_relaxed);
-+  std::atomic_store_explicit(&fast_clock_id, clock_id, std::memory_order_relaxed);
- 
- done:
- 
-diff --git a/src/unix/tty.c b/src/unix/tty.c
-index 1bd217b5a15eed13a8349c479b53471dd36ca216..1304c6d8685cfd122cffea066dc668d1dfc9ae02 100644
---- a/src/unix/tty.c
-+++ b/src/unix/tty.c
-@@ -22,7 +22,6 @@
- #include "uv.h"
- #include "internal.h"
- 
--#include <stdatomic.h>
- #include <stdlib.h>
- #include <assert.h>
- #include <unistd.h>
-@@ -30,6 +29,8 @@
- #include <errno.h>
- #include <sys/ioctl.h>
- 
-+#include <atomic>
-+
- #if defined(__MVS__) && !defined(IMAXBEL)
- #define IMAXBEL 0
- #endif
-@@ -64,7 +65,7 @@ static int isreallyatty(int file) {
- 
- static int orig_termios_fd = -1;
- static struct termios orig_termios;
--static _Atomic int termios_spinlock;
-+static std::atomic<int> termios_spinlock;
- 
- int uv__tcsetattr(int fd, int how, const struct termios *term) {
-   int rc;
-diff --git a/src/uv-common.c b/src/uv-common.c
-index bfcc3ef10f4fd7763221638947da6e02e7a17c33..5c6d84155408ae4f7c3c6ff9b48bd09ccd16a92e 100644
---- a/src/uv-common.c
-+++ b/src/uv-common.c
-@@ -951,7 +951,7 @@ void uv_free_cpu_info(uv_cpu_info_t* cpu_infos, int count) {
- __attribute__((destructor))
- #endif
- void uv_library_shutdown(void) {
--  static int was_shutdown;
-+  static std::atomic<int> was_shutdown;
- 
-   if (uv__exchange_int_relaxed(&was_shutdown, 1))
-     return;
-diff --git a/src/uv-common.h b/src/uv-common.h
-index cd57e5a35153d0557351b60cce0c5be7a4468b60..5dce8eaf2705b47935b218181f6dd69af0d5b61b 100644
---- a/src/uv-common.h
-+++ b/src/uv-common.h
-@@ -32,15 +32,13 @@
- #include <stddef.h>
- #include <stdint.h>
- 
-+#include <atomic>
-+
- #include "uv.h"
- #include "uv/tree.h"
- #include "queue.h"
- #include "strscpy.h"
- 
--#ifndef _MSC_VER
--# include <stdatomic.h>
--#endif
--
- #if EDOM > 0
- # define UV__ERR(x) (-(x))
- #else
-@@ -70,7 +68,7 @@ extern int snprintf(char*, size_t, const char*, ...);
-   InterlockedExchangeNoFence((LONG volatile*)(p), v)
- #else
- #define uv__exchange_int_relaxed(p, v)                                        \
--  atomic_exchange_explicit((_Atomic int*)(p), v, memory_order_relaxed)
-+  std::atomic_exchange_explicit((std::atomic<int>*)(p), v, std::memory_order_relaxed)
- #endif
- 
- #define UV__UDP_DGRAM_MAXSIZE (64 * 1024)
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0009-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0009-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch
new file mode 100644
index 0000000..07407f1
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/libuv_patches/0009-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch
@@ -0,0 +1,42 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Fri, 14 Jul 2023 16:40:18 -0700
+Subject: [PATCH 9/9] Add pragmas for missing libraries and set _WIN32_WINNT to
+ Windows 10
+
+This makes GetSystemTimePreciseAsFileTime() available.
+
+The #define value is from
+https://learn.microsoft.com/en-us/cpp/porting/modifying-winver-and-win32-winnt.
+---
+ include/uv/win.h | 2 +-
+ src/win/util.c   | 2 ++
+ 2 files changed, 3 insertions(+), 1 deletion(-)
+
+diff --git a/include/uv/win.h b/include/uv/win.h
+index 6d0afe69e7dd4caf4c9459e548fe75cf0c51b501..613065df435d813cd517efbc138b13ee46f01f2d 100644
+--- a/include/uv/win.h
++++ b/include/uv/win.h
+@@ -20,7 +20,7 @@
+  */
+ 
+ #ifndef _WIN32_WINNT
+-# define _WIN32_WINNT   0x0600
++# define _WIN32_WINNT   0x0A00
+ #endif
+ 
+ #if !defined(_SSIZE_T_) && !defined(_SSIZE_T_DEFINED)
+diff --git a/src/win/util.c b/src/win/util.c
+index 8828857db76a54d6e184c7f5714a091e92eba8ed..14295c41342b6dda16f39c5c548a6cc8269bfa0b 100644
+--- a/src/win/util.c
++++ b/src/win/util.c
+@@ -73,7 +73,9 @@ static char *process_title;
+ static CRITICAL_SECTION process_title_lock;
+ 
+ #pragma comment(lib, "Advapi32.lib")
++#pragma comment(lib, "Dbghelp.lib")
+ #pragma comment(lib, "IPHLPAPI.lib")
++#pragma comment(lib, "Ole32.lib")
+ #pragma comment(lib, "Psapi.lib")
+ #pragma comment(lib, "Userenv.lib")
+ #pragma comment(lib, "kernel32.lib")
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0009-Remove-static-from-array-indices.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0009-Remove-static-from-array-indices.patch
deleted file mode 100644
index f95fc5f..0000000
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0009-Remove-static-from-array-indices.patch
+++ /dev/null
@@ -1,67 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Tyler Veness <calcmogul@gmail.com>
-Date: Thu, 13 Jul 2023 23:30:58 -0700
-Subject: [PATCH 09/10] Remove static from array indices
-
----
- src/unix/linux.c | 12 ++++++------
- 1 file changed, 6 insertions(+), 6 deletions(-)
-
-diff --git a/src/unix/linux.c b/src/unix/linux.c
-index e3dfb186dc531e5c8197a81681c00d693e0913c6..d365b623a0a25228f0c6acf1fa14a5c7a9f1efbf 100644
---- a/src/unix/linux.c
-+++ b/src/unix/linux.c
-@@ -2060,7 +2060,7 @@ static uint64_t uv__read_uint64(const char* filename) {
-  * finds the location and length of the memory controller mount path.
-  * This disregards the leading / for easy concatenation of paths.
-  * Returns NULL if the memory controller wasn't found. */
--static char* uv__cgroup1_find_memory_controller(char buf[static 1024],
-+static char* uv__cgroup1_find_memory_controller(char buf[1024],
-                                                 int* n) {
-   char* p;
- 
-@@ -2081,7 +2081,7 @@ static char* uv__cgroup1_find_memory_controller(char buf[static 1024],
-   return p;
- }
- 
--static void uv__get_cgroup1_memory_limits(char buf[static 1024], uint64_t* high,
-+static void uv__get_cgroup1_memory_limits(char buf[1024], uint64_t* high,
-                                           uint64_t* max) {
-   char filename[4097];
-   char* p;
-@@ -2121,7 +2121,7 @@ update_limits:
-     *max = UINT64_MAX;
- }
- 
--static void uv__get_cgroup2_memory_limits(char buf[static 1024], uint64_t* high,
-+static void uv__get_cgroup2_memory_limits(char buf[1024], uint64_t* high,
-                                           uint64_t* max) {
-   char filename[4097];
-   char* p;
-@@ -2138,7 +2138,7 @@ static void uv__get_cgroup2_memory_limits(char buf[static 1024], uint64_t* high,
-   *high = uv__read_uint64(filename);
- }
- 
--static uint64_t uv__get_cgroup_constrained_memory(char buf[static 1024]) {
-+static uint64_t uv__get_cgroup_constrained_memory(char buf[1024]) {
-   uint64_t high;
-   uint64_t max;
- 
-@@ -2164,7 +2164,7 @@ uint64_t uv_get_constrained_memory(void) {
- }
- 
- 
--static uint64_t uv__get_cgroup1_current_memory(char buf[static 1024]) {
-+static uint64_t uv__get_cgroup1_current_memory(char buf[1024]) {
-   char filename[4097];
-   uint64_t current;
-   char* p;
-@@ -2188,7 +2188,7 @@ static uint64_t uv__get_cgroup1_current_memory(char buf[static 1024]) {
-   return uv__read_uint64("/sys/fs/cgroup/memory/memory.usage_in_bytes");
- }
- 
--static uint64_t uv__get_cgroup2_current_memory(char buf[static 1024]) {
-+static uint64_t uv__get_cgroup2_current_memory(char buf[1024]) {
-   char filename[4097];
-   char* p;
-   int n;
diff --git a/third_party/allwpilib/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch b/third_party/allwpilib/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch
deleted file mode 100644
index 07d02f5..0000000
--- a/third_party/allwpilib/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch
+++ /dev/null
@@ -1,42 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Tyler Veness <calcmogul@gmail.com>
-Date: Fri, 14 Jul 2023 16:40:18 -0700
-Subject: [PATCH 10/10] Add pragmas for missing libraries and set _WIN32_WINNT
- to Windows 10
-
-This makes GetSystemTimePreciseAsFileTime() available.
-
-The #define value is from
-https://learn.microsoft.com/en-us/cpp/porting/modifying-winver-and-win32-winnt.
----
- include/uv/win.h | 2 +-
- src/win/util.c   | 2 ++
- 2 files changed, 3 insertions(+), 1 deletion(-)
-
-diff --git a/include/uv/win.h b/include/uv/win.h
-index 6d0afe69e7dd4caf4c9459e548fe75cf0c51b501..613065df435d813cd517efbc138b13ee46f01f2d 100644
---- a/include/uv/win.h
-+++ b/include/uv/win.h
-@@ -20,7 +20,7 @@
-  */
- 
- #ifndef _WIN32_WINNT
--# define _WIN32_WINNT   0x0600
-+# define _WIN32_WINNT   0x0A00
- #endif
- 
- #if !defined(_SSIZE_T_) && !defined(_SSIZE_T_DEFINED)
-diff --git a/src/win/util.c b/src/win/util.c
-index 9324992ec521cc3496e3e9304e600963a3f20897..4b76417fcbac2480725471740c037deb859e17ca 100644
---- a/src/win/util.c
-+++ b/src/win/util.c
-@@ -73,7 +73,9 @@ static char *process_title;
- static CRITICAL_SECTION process_title_lock;
- 
- #pragma comment(lib, "Advapi32.lib")
-+#pragma comment(lib, "Dbghelp.lib")
- #pragma comment(lib, "IPHLPAPI.lib")
-+#pragma comment(lib, "Ole32.lib")
- #pragma comment(lib, "Psapi.lib")
- #pragma comment(lib, "Userenv.lib")
- #pragma comment(lib, "kernel32.lib")
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0001-Remove-StringRef-ArrayRef-and-Optional.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0001-Remove-StringRef-ArrayRef-and-Optional.patch
index 48327f8..20b8364 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0001-Remove-StringRef-ArrayRef-and-Optional.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0001-Remove-StringRef-ArrayRef-and-Optional.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:09:18 -0400
-Subject: [PATCH 01/31] Remove StringRef, ArrayRef, and Optional
+Subject: [PATCH 01/34] Remove StringRef, ArrayRef, and Optional
 
 ---
  llvm/include/llvm/ADT/PointerUnion.h          |  1 -
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0002-Wrap-std-min-max-calls-in-parens-for-Windows-warning.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0002-Wrap-std-min-max-calls-in-parens-for-Windows-warning.patch
index fe5c48e..0648e23 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0002-Wrap-std-min-max-calls-in-parens-for-Windows-warning.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0002-Wrap-std-min-max-calls-in-parens-for-Windows-warning.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:12:41 -0400
-Subject: [PATCH 02/31] Wrap std::min/max calls in parens, for Windows warnings
+Subject: [PATCH 02/34] Wrap std::min/max calls in parens, for Windows warnings
 
 ---
  llvm/include/llvm/ADT/DenseMap.h       |  4 ++--
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0003-Change-unique_function-storage-size.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0003-Change-unique_function-storage-size.patch
index c4e5176..7655155 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0003-Change-unique_function-storage-size.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0003-Change-unique_function-storage-size.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:13:55 -0400
-Subject: [PATCH 03/31] Change unique_function storage size
+Subject: [PATCH 03/34] Change unique_function storage size
 
 ---
  llvm/include/llvm/ADT/FunctionExtras.h | 4 ++--
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0004-Threading-updates.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0004-Threading-updates.patch
index 98c1d8a..9cb2cf8 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0004-Threading-updates.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0004-Threading-updates.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:17:19 -0400
-Subject: [PATCH 04/31] Threading updates
+Subject: [PATCH 04/34] Threading updates
 
 - Remove guards for threads and exception
 - Prefer scope gaurd over lock gaurd
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0005-ifdef-guard-safety.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0005-ifdef-guard-safety.patch
index e74cf17..3a9ae56 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0005-ifdef-guard-safety.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0005-ifdef-guard-safety.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:28:13 -0400
-Subject: [PATCH 05/31] \#ifdef guard safety
+Subject: [PATCH 05/34] \#ifdef guard safety
 
 Prevents redefinition if someone is pulling in real LLVM, since the macros are in global namespace
 ---
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0006-Explicitly-use-std.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0006-Explicitly-use-std.patch
index 0d841d9..2217c4d 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0006-Explicitly-use-std.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0006-Explicitly-use-std.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:37:34 -0400
-Subject: [PATCH 06/31] Explicitly use std::
+Subject: [PATCH 06/34] Explicitly use std::
 
 ---
  llvm/include/llvm/ADT/SmallSet.h       |  2 +-
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0007-Remove-format_provider.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0007-Remove-format_provider.patch
index 6495bb9..6249648 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0007-Remove-format_provider.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0007-Remove-format_provider.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:53:50 -0400
-Subject: [PATCH 07/31] Remove format_provider
+Subject: [PATCH 07/34] Remove format_provider
 
 ---
  llvm/include/llvm/Support/Chrono.h      | 109 ------------------------
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0008-Add-compiler-warning-pragmas.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0008-Add-compiler-warning-pragmas.patch
index 15442f1..8a889a7 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0008-Add-compiler-warning-pragmas.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0008-Add-compiler-warning-pragmas.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 13:34:07 -0400
-Subject: [PATCH 08/31] Add compiler warning pragmas
+Subject: [PATCH 08/34] Add compiler warning pragmas
 
 ---
  llvm/include/llvm/ADT/FunctionExtras.h | 11 +++++++++++
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0009-Remove-unused-functions.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0009-Remove-unused-functions.patch
index 13bec69..bff9ff2 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0009-Remove-unused-functions.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0009-Remove-unused-functions.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 13:43:50 -0400
-Subject: [PATCH 09/31] Remove unused functions
+Subject: [PATCH 09/34] Remove unused functions
 
 ---
  llvm/include/llvm/ADT/SmallString.h      |  85 +-----
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0010-Detemplatize-SmallVectorBase.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0010-Detemplatize-SmallVectorBase.patch
index 8d28e0f..484ac22 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0010-Detemplatize-SmallVectorBase.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0010-Detemplatize-SmallVectorBase.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Thu, 5 May 2022 23:18:34 -0400
-Subject: [PATCH 10/31] Detemplatize SmallVectorBase
+Subject: [PATCH 10/34] Detemplatize SmallVectorBase
 
 ---
  llvm/include/llvm/ADT/SmallVector.h | 27 +++++++--------------
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0011-Add-vectors-to-raw_ostream.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0011-Add-vectors-to-raw_ostream.patch
index cba909f..095729e 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0011-Add-vectors-to-raw_ostream.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0011-Add-vectors-to-raw_ostream.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 13:48:59 -0400
-Subject: [PATCH 11/31] Add vectors to raw_ostream
+Subject: [PATCH 11/34] Add vectors to raw_ostream
 
 ---
  llvm/include/llvm/Support/raw_ostream.h | 115 ++++++++++++++++++++++++
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0012-Extra-collections-features.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0012-Extra-collections-features.patch
index 7da2c10..833e80b 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0012-Extra-collections-features.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0012-Extra-collections-features.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 3 May 2022 22:16:10 -0400
-Subject: [PATCH 12/31] Extra collections features
+Subject: [PATCH 12/34] Extra collections features
 
 ---
  llvm/include/llvm/ADT/StringMap.h | 103 +++++++++++++++++++++++++++++-
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0013-EpochTracker-ABI-macro.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0013-EpochTracker-ABI-macro.patch
index a280def..e02790e 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0013-EpochTracker-ABI-macro.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0013-EpochTracker-ABI-macro.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Wed, 4 May 2022 00:01:00 -0400
-Subject: [PATCH 13/31] EpochTracker ABI macro
+Subject: [PATCH 13/34] EpochTracker ABI macro
 
 ---
  llvm/include/llvm/ADT/EpochTracker.h | 2 +-
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0014-Delete-numbers-from-MathExtras.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0014-Delete-numbers-from-MathExtras.patch
index ea27672..d8114c9 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0014-Delete-numbers-from-MathExtras.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0014-Delete-numbers-from-MathExtras.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Thu, 5 May 2022 18:09:45 -0400
-Subject: [PATCH 14/31] Delete numbers from MathExtras
+Subject: [PATCH 14/34] Delete numbers from MathExtras
 
 ---
  llvm/include/llvm/Support/MathExtras.h | 36 --------------------------
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0015-Add-lerp-and-sgn.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0015-Add-lerp-and-sgn.patch
index c2fe9ed..c642e12 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0015-Add-lerp-and-sgn.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0015-Add-lerp-and-sgn.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 3 May 2022 22:50:24 -0400
-Subject: [PATCH 15/31] Add lerp and sgn
+Subject: [PATCH 15/34] Add lerp and sgn
 
 ---
  llvm/include/llvm/Support/MathExtras.h | 20 ++++++++++++++++++++
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0016-Fixup-includes.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0016-Fixup-includes.patch
index 6d32c22..0200b2b 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0016-Fixup-includes.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0016-Fixup-includes.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:38:11 -0400
-Subject: [PATCH 16/31] Fixup includes
+Subject: [PATCH 16/34] Fixup includes
 
 ---
  llvm/include/llvm/ADT/StringMap.h                 |  4 ++++
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0017-Use-std-is_trivially_copy_constructible.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0017-Use-std-is_trivially_copy_constructible.patch
index c78fd42..a2b520b 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0017-Use-std-is_trivially_copy_constructible.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0017-Use-std-is_trivially_copy_constructible.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:42:09 -0400
-Subject: [PATCH 17/31] Use std::is_trivially_copy_constructible
+Subject: [PATCH 17/34] Use std::is_trivially_copy_constructible
 
 ---
  llvm/include/llvm/Support/type_traits.h | 16 ----------------
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0018-Windows-support.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0018-Windows-support.patch
index 43e0a07..13d4275 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0018-Windows-support.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0018-Windows-support.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 3 May 2022 20:22:38 -0400
-Subject: [PATCH 18/31] Windows support
+Subject: [PATCH 18/34] Windows support
 
 ---
  .../llvm/Support/Windows/WindowsSupport.h     | 45 +++++----
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0019-Prefer-fmtlib.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0019-Prefer-fmtlib.patch
index 4d5b81c..e505363 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0019-Prefer-fmtlib.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0019-Prefer-fmtlib.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:46:20 -0400
-Subject: [PATCH 19/31] Prefer fmtlib
+Subject: [PATCH 19/34] Prefer fmtlib
 
 ---
  llvm/lib/Support/ErrorHandling.cpp | 20 ++++++--------------
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0020-Prefer-wpi-s-fs.h.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0020-Prefer-wpi-s-fs.h.patch
index 18ae0e7..cc55df5 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0020-Prefer-wpi-s-fs.h.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0020-Prefer-wpi-s-fs.h.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:49:36 -0400
-Subject: [PATCH 20/31] Prefer wpi's fs.h
+Subject: [PATCH 20/34] Prefer wpi's fs.h
 
 ---
  llvm/include/llvm/Support/raw_ostream.h | 7 ++-----
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0021-Remove-unused-functions.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0021-Remove-unused-functions.patch
index 4f6ba93..53c0811 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0021-Remove-unused-functions.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0021-Remove-unused-functions.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 19:16:51 -0400
-Subject: [PATCH 21/31] Remove unused functions
+Subject: [PATCH 21/34] Remove unused functions
 
 ---
  llvm/include/llvm/Support/raw_ostream.h |  5 +-
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0022-OS-specific-changes.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0022-OS-specific-changes.patch
index 4b3b3b7..c58ddaa 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0022-OS-specific-changes.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0022-OS-specific-changes.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 19:30:43 -0400
-Subject: [PATCH 22/31] OS-specific changes
+Subject: [PATCH 22/34] OS-specific changes
 
 ---
  llvm/lib/Support/ErrorHandling.cpp | 16 +++++++---------
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0023-Use-SmallVector-for-UTF-conversion.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0023-Use-SmallVector-for-UTF-conversion.patch
index d04e93b..f831a90 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0023-Use-SmallVector-for-UTF-conversion.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0023-Use-SmallVector-for-UTF-conversion.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Mon, 9 May 2022 00:04:30 -0400
-Subject: [PATCH 23/31] Use SmallVector for UTF conversion
+Subject: [PATCH 23/34] Use SmallVector for UTF conversion
 
 ---
  llvm/include/llvm/Support/ConvertUTF.h    |  6 +++---
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0024-Prefer-to-use-static-pointers-in-raw_ostream.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0024-Prefer-to-use-static-pointers-in-raw_ostream.patch
index f5dce6b..fd6f137 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0024-Prefer-to-use-static-pointers-in-raw_ostream.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0024-Prefer-to-use-static-pointers-in-raw_ostream.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Thu, 19 May 2022 00:58:36 -0400
-Subject: [PATCH 24/31] Prefer to use static pointers in raw_ostream
+Subject: [PATCH 24/34] Prefer to use static pointers in raw_ostream
 
 See #1401
 ---
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0025-constexpr-endian-byte-swap.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0025-constexpr-endian-byte-swap.patch
index acbded1..6b3367b 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0025-constexpr-endian-byte-swap.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0025-constexpr-endian-byte-swap.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Thu, 19 May 2022 01:12:41 -0400
-Subject: [PATCH 25/31] constexpr endian byte swap
+Subject: [PATCH 25/34] constexpr endian byte swap
 
 ---
  llvm/include/llvm/Support/Endian.h | 4 +++-
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0026-Copy-type-traits-from-STLExtras.h-into-PointerUnion..patch b/third_party/allwpilib/upstream_utils/llvm_patches/0026-Copy-type-traits-from-STLExtras.h-into-PointerUnion..patch
index efa9e6c..52ec43d 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0026-Copy-type-traits-from-STLExtras.h-into-PointerUnion..patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0026-Copy-type-traits-from-STLExtras.h-into-PointerUnion..patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 10 Aug 2022 17:07:52 -0700
-Subject: [PATCH 26/31] Copy type traits from STLExtras.h into PointerUnion.h
+Subject: [PATCH 26/34] Copy type traits from STLExtras.h into PointerUnion.h
 
 ---
  llvm/include/llvm/ADT/PointerUnion.h | 46 ++++++++++++++++++++++++++++
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0027-Remove-StringMap-test-for-llvm-sort.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0027-Remove-StringMap-test-for-llvm-sort.patch
index 977dff3..2c9d4ef 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0027-Remove-StringMap-test-for-llvm-sort.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0027-Remove-StringMap-test-for-llvm-sort.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 10 Aug 2022 22:35:00 -0700
-Subject: [PATCH 27/31] Remove StringMap test for llvm::sort()
+Subject: [PATCH 27/34] Remove StringMap test for llvm::sort()
 
 ---
  llvm/unittests/ADT/StringMapTest.cpp | 14 --------------
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0028-Unused-variable-in-release-mode.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0028-Unused-variable-in-release-mode.patch
index 2492883..44e4f85 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0028-Unused-variable-in-release-mode.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0028-Unused-variable-in-release-mode.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Leander Schulten <Leander.Schulten@rwth-aachen.de>
 Date: Mon, 10 Jul 2023 00:53:43 +0200
-Subject: [PATCH 28/31] Unused variable in release mode
+Subject: [PATCH 28/34] Unused variable in release mode
 
 ---
  llvm/include/llvm/ADT/DenseMap.h | 2 +-
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0029-Use-C-20-bit-header.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0029-Use-C-20-bit-header.patch
index fad61d3..cc66628 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0029-Use-C-20-bit-header.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0029-Use-C-20-bit-header.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Tue, 11 Jul 2023 22:56:09 -0700
-Subject: [PATCH 29/31] Use C++20 <bit> header
+Subject: [PATCH 29/34] Use C++20 <bit> header
 
 ---
  llvm/include/llvm/ADT/DenseMap.h       |   3 +-
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0030-Remove-DenseMap-GTest-printer-test.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0030-Remove-DenseMap-GTest-printer-test.patch
index 8ee47b8..e8c9f97 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0030-Remove-DenseMap-GTest-printer-test.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0030-Remove-DenseMap-GTest-printer-test.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Sun, 30 Jul 2023 14:17:37 -0700
-Subject: [PATCH 30/31] Remove DenseMap GTest printer test
+Subject: [PATCH 30/34] Remove DenseMap GTest printer test
 
 LLVM modifies internal GTest headers to support it, which we can't do.
 ---
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0031-Replace-deprecated-std-aligned_storage_t.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0031-Replace-deprecated-std-aligned_storage_t.patch
index e074f27..a8bc557 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0031-Replace-deprecated-std-aligned_storage_t.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0031-Replace-deprecated-std-aligned_storage_t.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Fri, 15 Sep 2023 18:26:50 -0700
-Subject: [PATCH 31/31] Replace deprecated std::aligned_storage_t
+Subject: [PATCH 31/34] Replace deprecated std::aligned_storage_t
 
 ---
  llvm/include/llvm/ADT/FunctionExtras.h | 4 ++--
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0032-Fix-compilation-of-MathExtras.h-on-Windows-with-sdl.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0032-Fix-compilation-of-MathExtras.h-on-Windows-with-sdl.patch
index 52956f9..09b7c03 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0032-Fix-compilation-of-MathExtras.h-on-Windows-with-sdl.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0032-Fix-compilation-of-MathExtras.h-on-Windows-with-sdl.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Benjamin Hall <bhallctre@gmail.com>
 Date: Mon, 23 Oct 2023 21:36:40 -0400
-Subject: [PATCH 32/32] Fix compilation of MathExtras.h on Windows with /sdl
+Subject: [PATCH 32/34] Fix compilation of MathExtras.h on Windows with /sdl
 
 See https://github.com/llvm/llvm-project/pull/68978
 ---
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0033-raw_ostream-Add-SetNumBytesInBuffer.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0033-raw_ostream-Add-SetNumBytesInBuffer.patch
index a36cfc6..f4699c4 100644
--- a/third_party/allwpilib/upstream_utils/llvm_patches/0033-raw_ostream-Add-SetNumBytesInBuffer.patch
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0033-raw_ostream-Add-SetNumBytesInBuffer.patch
@@ -1,14 +1,14 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sun, 29 Oct 2023 23:00:08 -0700
-Subject: [PATCH 33/33] raw_ostream: Add SetNumBytesInBuffer
+Subject: [PATCH 33/34] raw_ostream: Add SetNumBytesInBuffer
 
 ---
  llvm/include/llvm/Support/raw_ostream.h | 5 +++++
  1 file changed, 5 insertions(+)
 
 diff --git a/llvm/include/llvm/Support/raw_ostream.h b/llvm/include/llvm/Support/raw_ostream.h
-index 9a9a1f688313a5784a58a70f2cb4cc0d6ec70e79..39f98e4e7696e28587779e90a03995463767b02d 100644
+index 9a9a1f688313a5784a58a70f2cb4cc0d6ec70e79..d832bcb97b4131a08ba3692eb438455e4c9764e4 100644
 --- a/llvm/include/llvm/Support/raw_ostream.h
 +++ b/llvm/include/llvm/Support/raw_ostream.h
 @@ -356,6 +356,11 @@ protected:
diff --git a/third_party/allwpilib/upstream_utils/llvm_patches/0034-type_traits.h-Add-is_constexpr.patch b/third_party/allwpilib/upstream_utils/llvm_patches/0034-type_traits.h-Add-is_constexpr.patch
new file mode 100644
index 0000000..b881452
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/llvm_patches/0034-type_traits.h-Add-is_constexpr.patch
@@ -0,0 +1,25 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Peter Johnson <johnson.peter@gmail.com>
+Date: Sat, 2 Dec 2023 15:21:32 -0800
+Subject: [PATCH 34/34] type_traits.h: Add is_constexpr()
+
+---
+ llvm/include/llvm/Support/type_traits.h | 5 +++++
+ 1 file changed, 5 insertions(+)
+
+diff --git a/llvm/include/llvm/Support/type_traits.h b/llvm/include/llvm/Support/type_traits.h
+index 3171af93fa7ffe4707c03289270cf5951e3db7c5..e1b094640876649709abc199f83942dbfef09771 100644
+--- a/llvm/include/llvm/Support/type_traits.h
++++ b/llvm/include/llvm/Support/type_traits.h
+@@ -76,6 +76,11 @@ union trivial_helper {
+ 
+ } // end namespace detail
+ 
++// https://stackoverflow.com/questions/55288555/c-check-if-statement-can-be-evaluated-constexpr
++template<class Lambda, int=(Lambda{}(), 0)>
++constexpr bool is_constexpr(Lambda) { return true; }
++constexpr bool is_constexpr(...) { return false; }
++
+ } // end namespace llvm
+ 
+ #endif // LLVM_SUPPORT_TYPE_TRAITS_H
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0001-Fix-sign-compare-warnings.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0001-Fix-sign-compare-warnings.patch
index e916c0f..d3ac55c 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0001-Fix-sign-compare-warnings.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0001-Fix-sign-compare-warnings.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 10 Jun 2023 14:13:07 -0700
-Subject: [PATCH 01/11] Fix sign-compare warnings
+Subject: [PATCH 01/12] Fix sign-compare warnings
 
 ---
  src/google/protobuf/compiler/importer.cc                  | 2 +-
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0002-Remove-redundant-move.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0002-Remove-redundant-move.patch
index 09ab3c5..6916c98 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0002-Remove-redundant-move.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0002-Remove-redundant-move.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 10 Jun 2023 14:41:39 -0700
-Subject: [PATCH 02/11] Remove redundant move
+Subject: [PATCH 02/12] Remove redundant move
 
 ---
  src/google/protobuf/extension_set.h | 2 +-
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0003-Fix-maybe-uninitialized-warnings.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0003-Fix-maybe-uninitialized-warnings.patch
index 40ee920..6888dd2 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0003-Fix-maybe-uninitialized-warnings.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0003-Fix-maybe-uninitialized-warnings.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 10 Jun 2023 15:00:20 -0700
-Subject: [PATCH 03/11] Fix maybe-uninitialized warnings
+Subject: [PATCH 03/12] Fix maybe-uninitialized warnings
 
 ---
  src/google/protobuf/arena.cc                     |  6 +++---
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0004-Fix-coded_stream-WriteRaw.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0004-Fix-coded_stream-WriteRaw.patch
index 90fe162..dc17497 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0004-Fix-coded_stream-WriteRaw.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0004-Fix-coded_stream-WriteRaw.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 10 Jun 2023 15:03:38 -0700
-Subject: [PATCH 04/11] Fix coded_stream WriteRaw
+Subject: [PATCH 04/12] Fix coded_stream WriteRaw
 
 ---
  src/google/protobuf/implicit_weak_message.h | 2 +-
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0005-Suppress-enum-enum-conversion-warning.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0005-Suppress-enum-enum-conversion-warning.patch
index 85d12a0..2919644 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0005-Suppress-enum-enum-conversion-warning.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0005-Suppress-enum-enum-conversion-warning.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 10 Jun 2023 15:13:45 -0700
-Subject: [PATCH 05/11] Suppress enum-enum conversion warning
+Subject: [PATCH 05/12] Suppress enum-enum conversion warning
 
 ---
  src/google/protobuf/generated_message_tctable_impl.h | 9 +++++++++
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0006-Fix-noreturn-function-returning.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0006-Fix-noreturn-function-returning.patch
index d255d80..abaa0a2 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0006-Fix-noreturn-function-returning.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0006-Fix-noreturn-function-returning.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 10 Jun 2023 15:16:46 -0700
-Subject: [PATCH 06/11] Fix noreturn function returning
+Subject: [PATCH 06/12] Fix noreturn function returning
 
 ---
  src/google/protobuf/generated_message_tctable_impl.h | 3 +++
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0007-Work-around-GCC-12-restrict-warning-compiler-bug.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0007-Work-around-GCC-12-restrict-warning-compiler-bug.patch
index aa67e21..543896c 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0007-Work-around-GCC-12-restrict-warning-compiler-bug.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0007-Work-around-GCC-12-restrict-warning-compiler-bug.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 10 Jun 2023 15:59:45 -0700
-Subject: [PATCH 07/11] Work around GCC 12 restrict warning compiler bug
+Subject: [PATCH 07/12] Work around GCC 12 restrict warning compiler bug
 
 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=105329
 ---
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0008-Disable-MSVC-switch-warning.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0008-Disable-MSVC-switch-warning.patch
index b0c485b..92654f0 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0008-Disable-MSVC-switch-warning.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0008-Disable-MSVC-switch-warning.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Tue, 13 Jun 2023 23:56:15 -0700
-Subject: [PATCH 08/11] Disable MSVC switch warning
+Subject: [PATCH 08/12] Disable MSVC switch warning
 
 ---
  src/google/protobuf/generated_message_reflection.cc | 4 ++++
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0009-Disable-unused-function-warning.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0009-Disable-unused-function-warning.patch
index b3aa64e..9a88649 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0009-Disable-unused-function-warning.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0009-Disable-unused-function-warning.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Tue, 13 Jun 2023 23:58:50 -0700
-Subject: [PATCH 09/11] Disable unused function warning
+Subject: [PATCH 09/12] Disable unused function warning
 
 ---
  src/google/protobuf/generated_message_tctable_lite.cc | 4 ++++
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0010-Disable-pedantic-warning.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0010-Disable-pedantic-warning.patch
index 1555d19..dd1860e 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0010-Disable-pedantic-warning.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0010-Disable-pedantic-warning.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Wed, 14 Jun 2023 00:02:26 -0700
-Subject: [PATCH 10/11] Disable pedantic warning
+Subject: [PATCH 10/12] Disable pedantic warning
 
 ---
  src/google/protobuf/descriptor.h                    | 8 ++++++++
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0011-Avoid-use-of-sprintf.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0011-Avoid-use-of-sprintf.patch
index 27e81af..a0dfad8 100644
--- a/third_party/allwpilib/upstream_utils/protobuf_patches/0011-Avoid-use-of-sprintf.patch
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0011-Avoid-use-of-sprintf.patch
@@ -1,14 +1,14 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Mon, 9 Oct 2023 19:28:08 -0700
-Subject: [PATCH 11/11] Avoid use of sprintf
+Subject: [PATCH 11/12] Avoid use of sprintf
 
 ---
  src/google/protobuf/stubs/strutil.cc | 14 +++++++++++---
  1 file changed, 11 insertions(+), 3 deletions(-)
 
 diff --git a/src/google/protobuf/stubs/strutil.cc b/src/google/protobuf/stubs/strutil.cc
-index 3462e91ff273dc071628f06b91698a0f166514fc..e063d0d3039c87d55a6df0dbfe50f365184fe292 100644
+index 3462e91ff273dc071628f06b91698a0f166514fc..58e03d0c4aa7c1b0337b5f650283f00117c12baa 100644
 --- a/src/google/protobuf/stubs/strutil.cc
 +++ b/src/google/protobuf/stubs/strutil.cc
 @@ -503,10 +503,18 @@ int CEscapeInternal(const char* src, int src_len, char* dest,
diff --git a/third_party/allwpilib/upstream_utils/protobuf_patches/0012-Suppress-stringop-overflow-warning-false-positives.patch b/third_party/allwpilib/upstream_utils/protobuf_patches/0012-Suppress-stringop-overflow-warning-false-positives.patch
new file mode 100644
index 0000000..929c117
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/protobuf_patches/0012-Suppress-stringop-overflow-warning-false-positives.patch
@@ -0,0 +1,50 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Fri, 10 Nov 2023 14:17:53 -0800
+Subject: [PATCH 12/12] Suppress stringop-overflow warning false positives
+
+---
+ src/google/protobuf/io/coded_stream.h    | 7 +++++++
+ src/google/protobuf/unknown_field_set.cc | 7 +++++++
+ 2 files changed, 14 insertions(+)
+
+diff --git a/src/google/protobuf/io/coded_stream.h b/src/google/protobuf/io/coded_stream.h
+index 6c0dd4ab4099d1d748957af8bfc5f8c59c2aa3d6..a102cec8ea0b56926f63cf9ece205c634cb6d528 100644
+--- a/src/google/protobuf/io/coded_stream.h
++++ b/src/google/protobuf/io/coded_stream.h
+@@ -681,7 +681,14 @@ class PROTOBUF_EXPORT EpsCopyOutputStream {
+     if (PROTOBUF_PREDICT_FALSE(end_ - ptr < static_cast<int>(size))) {
+       return WriteRawFallback(data, size, ptr);
+     }
++#if __GNUC__ >= 13
++#pragma GCC diagnostic push
++#pragma GCC diagnostic ignored "-Wstringop-overflow="
++#endif  // __GNUC__ >= 13
+     std::memcpy(ptr, data, size);
++#if __GNUC__ >= 13
++#pragma GCC diagnostic pop
++#endif  // __GNUC__ >= 13
+     return ptr + size;
+   }
+   // Writes the buffer specified by data, size to the stream. Possibly by
+diff --git a/src/google/protobuf/unknown_field_set.cc b/src/google/protobuf/unknown_field_set.cc
+index 74c358e9a22c5475bfaef6c5ac63b05fc61b7074..c0587350b309839f3b8b99506d0417a9fd91b06d 100644
+--- a/src/google/protobuf/unknown_field_set.cc
++++ b/src/google/protobuf/unknown_field_set.cc
+@@ -96,9 +96,16 @@ void UnknownFieldSet::MergeFromAndDestroy(UnknownFieldSet* other) {
+   if (fields_.empty()) {
+     fields_ = std::move(other->fields_);
+   } else {
++#if __GNUC__ >= 13
++#pragma GCC diagnostic push
++#pragma GCC diagnostic ignored "-Wstringop-overflow="
++#endif  // __GNUC__ >= 13
+     fields_.insert(fields_.end(),
+                    std::make_move_iterator(other->fields_.begin()),
+                    std::make_move_iterator(other->fields_.end()));
++#if __GNUC__ >= 13
++#pragma GCC diagnostic pop
++#endif  // __GNUC__ >= 13
+   }
+   other->fields_.clear();
+ }
diff --git a/third_party/allwpilib/upstream_utils/update_eigen.py b/third_party/allwpilib/upstream_utils/update_eigen.py
index ce4c620..9ed255c 100755
--- a/third_party/allwpilib/upstream_utils/update_eigen.py
+++ b/third_party/allwpilib/upstream_utils/update_eigen.py
@@ -95,7 +95,12 @@
 
 
 def main():
-    upstream_root = clone_repo("https://gitlab.com/libeigen/eigen.git", "3.4.0")
+    upstream_root = clone_repo(
+        "https://gitlab.com/libeigen/eigen.git",
+        # master on 2023-12-01
+        "96880810295b65d77057f4a7fb83a99a590122ad",
+        shallow=False,
+    )
     wpilib_root = get_repo_root()
     wpimath = os.path.join(wpilib_root, "wpimath")
 
@@ -104,15 +109,11 @@
     for f in [
         "0001-Disable-warnings.patch",
         "0002-Intellisense-fix.patch",
-        "0003-Eigen-Sparse-fix-warnings-Wunused-but-set-variable.patch",
     ]:
         git_am(os.path.join(wpilib_root, "upstream_utils/eigen_patches", f))
 
     # Delete old install
-    for d in [
-        "src/main/native/thirdparty/eigen/include/Eigen",
-        "src/main/native/thirdparty/eigen/include/unsupported",
-    ]:
+    for d in ["src/main/native/thirdparty/eigen/include"]:
         shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
 
     # Copy Eigen headers into allwpilib
@@ -136,6 +137,11 @@
             f, [os.path.join(wpimath, "src/main/native/thirdparty/eigen/include")]
         )
 
+    shutil.copyfile(
+        os.path.join(upstream_root, ".clang-format"),
+        os.path.join(wpimath, "src/main/native/thirdparty/eigen/include/.clang-format"),
+    )
+
 
 if __name__ == "__main__":
     main()
diff --git a/third_party/allwpilib/upstream_utils/update_fmt.py b/third_party/allwpilib/upstream_utils/update_fmt.py
index 746e879..3af48a2 100755
--- a/third_party/allwpilib/upstream_utils/update_fmt.py
+++ b/third_party/allwpilib/upstream_utils/update_fmt.py
@@ -22,6 +22,7 @@
     for f in [
         "0001-Don-t-throw-on-write-failure.patch",
         "0002-Suppress-warnings-we-can-t-fix.patch",
+        "0003-Remove-this-from-decltype.patch",
     ]:
         git_am(os.path.join(wpilib_root, "upstream_utils/fmt_patches", f))
 
diff --git a/third_party/allwpilib/upstream_utils/update_gcem.py b/third_party/allwpilib/upstream_utils/update_gcem.py
index 0f3fe1f..a198e97 100755
--- a/third_party/allwpilib/upstream_utils/update_gcem.py
+++ b/third_party/allwpilib/upstream_utils/update_gcem.py
@@ -19,7 +19,10 @@
 
     # Apply patches to upstream Git repo
     os.chdir(upstream_root)
-    for f in []:
+    for f in [
+        "0001-Call-std-functions-if-not-constant-evaluated.patch",
+        "0002-Add-hypot-x-y-z.patch",
+    ]:
         git_am(os.path.join(wpilib_root, "upstream_utils/gcem_patches", f))
 
     # Delete old install
diff --git a/third_party/allwpilib/upstream_utils/update_libuv.py b/third_party/allwpilib/upstream_utils/update_libuv.py
index 179632f..f7d13b4 100755
--- a/third_party/allwpilib/upstream_utils/update_libuv.py
+++ b/third_party/allwpilib/upstream_utils/update_libuv.py
@@ -13,7 +13,7 @@
 
 
 def main():
-    upstream_root = clone_repo("https://github.com/libuv/libuv", "v1.46.0")
+    upstream_root = clone_repo("https://github.com/libuv/libuv", "v1.47.0")
     wpilib_root = get_repo_root()
     wpinet = os.path.join(wpilib_root, "wpinet")
 
@@ -25,11 +25,10 @@
         "0003-Fix-warnings.patch",
         "0004-Preprocessor-cleanup.patch",
         "0005-Cleanup-problematic-language.patch",
-        "0006-Style-comments-cleanup.patch",
-        "0007-Fix-Win32-warning-suppression-pragma.patch",
-        "0008-Use-C-atomics.patch",
-        "0009-Remove-static-from-array-indices.patch",
-        "0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch",
+        "0006-Fix-Win32-warning-suppression-pragma.patch",
+        "0007-Use-C-atomics.patch",
+        "0008-Remove-static-from-array-indices.patch",
+        "0009-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch",
     ]:
         git_am(os.path.join(wpilib_root, "upstream_utils/libuv_patches", f))
 
diff --git a/third_party/allwpilib/upstream_utils/update_llvm.py b/third_party/allwpilib/upstream_utils/update_llvm.py
index 5910396..5c221ee 100755
--- a/third_party/allwpilib/upstream_utils/update_llvm.py
+++ b/third_party/allwpilib/upstream_utils/update_llvm.py
@@ -171,7 +171,7 @@
 
 
 def main():
-    upstream_root = clone_repo("https://github.com/llvm/llvm-project", "llvmorg-17.0.4")
+    upstream_root = clone_repo("https://github.com/llvm/llvm-project", "llvmorg-17.0.5")
     wpilib_root = get_repo_root()
     wpiutil = os.path.join(wpilib_root, "wpiutil")
 
@@ -211,6 +211,7 @@
         "0031-Replace-deprecated-std-aligned_storage_t.patch",
         "0032-Fix-compilation-of-MathExtras.h-on-Windows-with-sdl.patch",
         "0033-raw_ostream-Add-SetNumBytesInBuffer.patch",
+        "0034-type_traits.h-Add-is_constexpr.patch",
     ]:
         git_am(
             os.path.join(wpilib_root, "upstream_utils/llvm_patches", f),
diff --git a/third_party/allwpilib/upstream_utils/update_protobuf.py b/third_party/allwpilib/upstream_utils/update_protobuf.py
index fbecb9c..1f004cd 100755
--- a/third_party/allwpilib/upstream_utils/update_protobuf.py
+++ b/third_party/allwpilib/upstream_utils/update_protobuf.py
@@ -276,6 +276,7 @@
         "0009-Disable-unused-function-warning.patch",
         "0010-Disable-pedantic-warning.patch",
         "0011-Avoid-use-of-sprintf.patch",
+        "0012-Suppress-stringop-overflow-warning-false-positives.patch",
     ]:
         git_am(os.path.join(wpilib_root, "upstream_utils/protobuf_patches", f))
 
diff --git a/third_party/allwpilib/wpigui/CMakeLists.txt b/third_party/allwpilib/wpigui/CMakeLists.txt
index 68acbea..beae7ba 100644
--- a/third_party/allwpilib/wpigui/CMakeLists.txt
+++ b/third_party/allwpilib/wpigui/CMakeLists.txt
@@ -8,10 +8,10 @@
 file(GLOB wpigui_mac_src src/main/native/metal/*.mm)
 file(GLOB wpigui_unix_src src/main/native/opengl3/*.cpp)
 
-if (MSVC)
-  add_library(wpigui STATIC ${wpigui_src})
+if(MSVC)
+    add_library(wpigui STATIC ${wpigui_src})
 else()
-  add_library(wpigui ${wpigui_src})
+    add_library(wpigui ${wpigui_src})
 endif()
 set_target_properties(wpigui PROPERTIES DEBUG_POSTFIX "d")
 set_property(TARGET wpigui PROPERTY POSITION_INDEPENDENT_CODE ON)
@@ -21,14 +21,17 @@
 wpilib_target_warnings(wpigui)
 target_link_libraries(wpigui PUBLIC imgui)
 
-target_include_directories(wpigui PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpigui>)
+target_include_directories(
+    wpigui
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpigui>
+)
 
-if (MSVC)
+if(MSVC)
     target_sources(wpigui PRIVATE ${wpigui_windows_src})
 else()
-    if (APPLE)
+    if(APPLE)
         target_compile_options(wpigui PRIVATE -fobjc-arc)
         set_target_properties(wpigui PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore")
         target_sources(wpigui PRIVATE ${wpigui_mac_src})
@@ -44,13 +47,3 @@
 
 install(TARGETS wpigui EXPORT wpigui)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpigui")
-
-#if (WITH_FLAT_INSTALL)
-#    set (wpigui_config_dir ${wpilib_dest})
-#else()
-#    set (wpigui_config_dir share/wpigui)
-#endif()
-
-#configure_file(wpigui-config.cmake.in ${WPILIB_BINARY_DIR}/wpigui-config.cmake )
-#install(FILES ${WPILIB_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir})
-#install(EXPORT wpigui DESTINATION ${wpigui_config_dir})
diff --git a/third_party/allwpilib/wpilib-config.cmake.in b/third_party/allwpilib/wpilib-config.cmake.in
index aaf4bb8..c08e9d2 100644
--- a/third_party/allwpilib/wpilib-config.cmake.in
+++ b/third_party/allwpilib/wpilib-config.cmake.in
@@ -4,11 +4,18 @@
 find_dependency(Threads)
 @LIBUV_SYSTEM_REPLACE@
 @EIGEN_SYSTEM_REPLACE@
-@WPIUTIL_DEP_REPLACE@
-@WPINET_DEP_REPLACE@
-@NTCORE_DEP_REPLACE@
-@CSCORE_DEP_REPLACE@
+
+@APRILTAG_DEP_REPLACE@
 @CAMERASERVER_DEP_REPLACE@
+@CSCORE_DEP_REPLACE@
 @HAL_DEP_REPLACE@
+@NTCORE_DEP_REPLACE@
 @WPILIBC_DEP_REPLACE@
 @WPILIBNEWCOMMANDS_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+@WPINET_DEP_REPLACE@
+@WPIUTIL_DEP_REPLACE@
+if(@WITH_JAVA@)
+    @WPILIBJ_DEP_REPLACE@
+    @WPIUNITS_DEP_REPLACE@
+endif()
diff --git a/third_party/allwpilib/wpilibNewCommands/CMakeLists.txt b/third_party/allwpilib/wpilibNewCommands/CMakeLists.txt
index 34a6bb1..4b42aa5 100644
--- a/third_party/allwpilib/wpilibNewCommands/CMakeLists.txt
+++ b/third_party/allwpilib/wpilibNewCommands/CMakeLists.txt
@@ -4,40 +4,55 @@
 include(CompileWarnings)
 include(AddTest)
 
-if (WITH_JAVA)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-  add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+    add_jar(
+        wpilibNewCommands_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS
+            hal_jar
+            ntcore_jar
+            cscore_jar
+            cameraserver_jar
+            wpimath_jar
+            wpiunits_jar
+            wpiutil_jar
+            wpilibj_jar
+        OUTPUT_NAME wpilibNewCommands
+    )
 
-  get_property(WPILIBNEWCOMMANDS_JAR_FILE TARGET wpilibNewCommands_jar PROPERTY JAR_FILE)
-  install(FILES ${WPILIBNEWCOMMANDS_JAR_FILE} DESTINATION "${java_lib_dest}")
-
-  set_property(TARGET wpilibNewCommands_jar PROPERTY FOLDER "java")
-
-  if (WITH_FLAT_INSTALL)
-      set (wpilibNewCommands_config_dir ${wpilib_dest})
-  else()
-      set (wpilibNewCommands_config_dir share/wpilibNewCommands)
-  endif()
+    install_jar(wpilibNewCommands_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(
+        TARGETS wpilibNewCommands_jar
+        FILE wpilibNewCommands_jar.cmake
+        DESTINATION share/wpilibNewCommands
+    )
 endif()
 
-if (WITH_JAVA_SOURCE)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  file(GLOB WPILIBNEWCOMMANDS_SOURCES src/main/java/edu/wpi/first/wpilibj2/command/*.java)
-  file(GLOB WPILIBNEWCOMMANDS_BUTTON_SOURCES src/main/java/edu/wpi/first/wpilibj2/command/button*.java)
-  add_jar(wpilibNewCommands_src_jar
-  RESOURCES NAMESPACE "edu/wpi/first/wpilibj2/command" ${WPILIBNEWCOMMANDS_SOURCES}
-  NAMESPACE "edu/wpi/first/wpilibj2/command/button" ${WPILIBNEWCOMMANDS_BUTTON_SOURCES}
-  OUTPUT_NAME wpilibNewCommands-sources)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB WPILIBNEWCOMMANDS_SOURCES src/main/java/edu/wpi/first/wpilibj2/command/*.java)
+    file(
+        GLOB WPILIBNEWCOMMANDS_BUTTON_SOURCES
+        src/main/java/edu/wpi/first/wpilibj2/command/button*.java
+    )
+    add_jar(
+        wpilibNewCommands_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/wpilibj2/command" ${WPILIBNEWCOMMANDS_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj2/command/button" ${WPILIBNEWCOMMANDS_BUTTON_SOURCES}
+        OUTPUT_NAME wpilibNewCommands-sources
+    )
 
-  get_property(WPILIBNEWCOMMANDS_SRC_JAR_FILE TARGET wpilibNewCommands_src_jar PROPERTY JAR_FILE)
-  install(FILES ${WPILIBNEWCOMMANDS_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+    get_property(WPILIBNEWCOMMANDS_SRC_JAR_FILE TARGET wpilibNewCommands_src_jar PROPERTY JAR_FILE)
+    install(FILES ${WPILIBNEWCOMMANDS_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-  set_property(TARGET wpilibNewCommands_src_jar PROPERTY FOLDER "java")
+    set_property(TARGET wpilibNewCommands_src_jar PROPERTY FOLDER "java")
 endif()
 
 file(GLOB_RECURSE wpilibNewCommands_native_src src/main/native/cpp/*.cpp)
@@ -49,25 +64,28 @@
 wpilib_target_warnings(wpilibNewCommands)
 target_link_libraries(wpilibNewCommands wpilibc)
 
-target_include_directories(wpilibNewCommands PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpilibNewCommands>)
+target_include_directories(
+    wpilibNewCommands
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpilibNewCommands>
+)
 
-install(TARGETS wpilibNewCommands EXPORT wpilibNewCommands)
+install(TARGETS wpilibNewCommands EXPORT wpilibnewcommands)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibNewCommands")
 
-if (FLAT_INSTALL_WPILIB)
-     set(wpilibNewCommands_config_dir ${wpilib_dest})
- else()
-     set(wpilibNewCommands_config_dir share/wpilibNewCommands)
- endif()
+configure_file(
+    wpilibnewcommands-config.cmake.in
+    ${WPILIB_BINARY_DIR}/wpilibnewcommands-config.cmake
+)
+install(
+    FILES ${WPILIB_BINARY_DIR}/wpilibnewcommands-config.cmake
+    DESTINATION share/wpilibNewCommands
+)
+install(EXPORT wpilibnewcommands DESTINATION share/wpilibNewCommands)
 
- configure_file(wpilibNewCommands-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake)
- install(FILES ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake DESTINATION ${wpilibNewCommands_config_dir})
- install(EXPORT wpilibNewCommands DESTINATION ${wpilibNewCommands_config_dir})
-
- if (WITH_TESTS)
-     wpilib_add_test(wpilibNewCommands src/test/native/cpp)
-     target_include_directories(wpilibNewCommands_test PRIVATE src/test/native/include)
-     target_link_libraries(wpilibNewCommands_test wpilibNewCommands gmock_main)
- endif()
+if(WITH_TESTS)
+    wpilib_add_test(wpilibNewCommands src/test/native/cpp)
+    target_include_directories(wpilibNewCommands_test PRIVATE src/test/native/include)
+    target_link_libraries(wpilibNewCommands_test wpilibNewCommands gmock_main)
+endif()
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
index a913b55..5ea1a36 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -25,8 +25,11 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public abstract class Command implements Sendable {
+  /** Requirements set. */
   protected Set<Subsystem> m_requirements = new HashSet<>();
 
+  /** Default constructor. */
+  @SuppressWarnings("this-escape")
   protected Command() {
     String name = getClass().getName();
     SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
index 12243cb..d75f08a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
@@ -16,4 +16,9 @@
  */
 @Deprecated(since = "2024", forRemoval = true)
 @SuppressWarnings("PMD.AbstractClassWithoutAnyMethod")
-public abstract class CommandBase extends Command {}
+public abstract class CommandBase extends Command {
+  /** Default constructor. */
+  public CommandBase() {
+    super();
+  }
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
index c868539..e61b6f8 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -61,7 +61,7 @@
 
   private static final Optional<Command> kNoInterruptor = Optional.empty();
 
-  private final Set<Command> m_composedCommands = Collections.newSetFromMap(new WeakHashMap<>());
+  private final Map<Command, Exception> m_composedCommands = new WeakHashMap<>();
 
   // A set of the currently-running commands.
   private final Set<Command> m_scheduledCommands = new LinkedHashSet<>();
@@ -261,7 +261,7 @@
       if (RobotBase.isSimulation()) {
         subsystem.simulationPeriodic();
       }
-      m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()");
+      m_watchdog.addEpoch(subsystem.getName() + ".periodic()");
     }
 
     // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from
@@ -581,12 +581,25 @@
    * directly or added to a composition.
    *
    * @param commands the commands to register
-   * @throws IllegalArgumentException if the given commands have already been composed.
+   * @throws IllegalArgumentException if the given commands have already been composed, or the array
+   *     of commands has duplicates.
    */
   public void registerComposedCommands(Command... commands) {
-    var commandSet = Set.of(commands);
-    requireNotComposed(commandSet);
-    m_composedCommands.addAll(commandSet);
+    Set<Command> commandSet;
+    try {
+      commandSet = Set.of(commands);
+    } catch (IllegalArgumentException e) {
+      throw new IllegalArgumentException(
+          "Cannot compose a command twice in the same composition! (Original exception: "
+              + e
+              + ")");
+    }
+    requireNotComposedOrScheduled(commandSet);
+    var exception = new Exception("Originally composed at:");
+    exception.fillInStackTrace();
+    for (var command : commands) {
+      m_composedCommands.put(command, exception);
+    }
   }
 
   /**
@@ -613,30 +626,58 @@
   }
 
   /**
-   * Requires that the specified command hasn't been already added to a composition.
+   * Requires that the specified command hasn't already been added to a composition.
    *
-   * @param command The command to check
+   * @param commands The commands to check
    * @throws IllegalArgumentException if the given commands have already been composed.
    */
-  public void requireNotComposed(Command command) {
-    if (m_composedCommands.contains(command)) {
-      throw new IllegalArgumentException(
-          "Commands that have been composed may not be added to another composition or scheduled "
-              + "individually!");
+  public void requireNotComposed(Command... commands) {
+    for (var command : commands) {
+      var exception = m_composedCommands.getOrDefault(command, null);
+      if (exception != null) {
+        throw new IllegalArgumentException(
+            "Commands that have been composed may not be added to another composition or scheduled "
+                + "individually!",
+            exception);
+      }
     }
   }
 
   /**
-   * Requires that the specified commands not have been already added to a composition.
+   * Requires that the specified commands have not already been added to a composition.
    *
    * @param commands The commands to check
    * @throws IllegalArgumentException if the given commands have already been composed.
    */
   public void requireNotComposed(Collection<Command> commands) {
-    if (!Collections.disjoint(commands, getComposedCommands())) {
+    requireNotComposed(commands.toArray(Command[]::new));
+  }
+
+  /**
+   * Requires that the specified command hasn't already been added to a composition, and is not
+   * currently scheduled.
+   *
+   * @param command The command to check
+   * @throws IllegalArgumentException if the given command has already been composed or scheduled.
+   */
+  public void requireNotComposedOrScheduled(Command command) {
+    if (isScheduled(command)) {
       throw new IllegalArgumentException(
-          "Commands that have been composed may not be added to another composition or scheduled "
-              + "individually!");
+          "Commands that have been scheduled individually may not be added to a composition!");
+    }
+    requireNotComposed(command);
+  }
+
+  /**
+   * Requires that the specified commands have not already been added to a composition, and are not
+   * currently scheduled.
+   *
+   * @param commands The commands to check
+   * @throws IllegalArgumentException if the given commands have already been composed or scheduled.
+   */
+  public void requireNotComposedOrScheduled(Collection<Command> commands) {
+    for (var command : commands) {
+      requireNotComposedOrScheduled(command);
     }
   }
 
@@ -651,7 +692,7 @@
   }
 
   Set<Command> getComposedCommands() {
-    return m_composedCommands;
+    return m_composedCommands.keySet();
   }
 
   @Override
@@ -683,9 +724,7 @@
         null);
     builder.addIntegerArrayProperty(
         "Cancel",
-        () -> {
-          return new long[] {};
-        },
+        () -> new long[] {},
         toCancel -> {
           Map<Long, Command> ids = new LinkedHashMap<>();
           for (Command command : m_scheduledCommands) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
index 7295e3c..9202d19 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
@@ -233,12 +233,13 @@
    * the others.
    *
    * @param deadline the deadline command
-   * @param commands the commands to include
+   * @param otherCommands the other commands to include
    * @return the command group
    * @see ParallelDeadlineGroup
+   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
    */
-  public static Command deadline(Command deadline, Command... commands) {
-    return new ParallelDeadlineGroup(deadline, commands);
+  public static Command deadline(Command deadline, Command... otherCommands) {
+    return new ParallelDeadlineGroup(deadline, otherCommands);
   }
 
   private Commands() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
index 76a5276..099eba3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
@@ -37,6 +37,7 @@
    *     omission of command requirements. Use {@link Set#of()} to easily construct a requirement
    *     set.
    */
+  @SuppressWarnings("this-escape")
   public DeferredCommand(Supplier<Command> supplier, Set<Subsystem> requirements) {
     m_supplier = requireNonNullParam(supplier, "supplier", "DeferredCommand");
     addRequirements(requirements.toArray(new Subsystem[0]));
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
index dde5ae1..9f1d8a8 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
@@ -18,10 +18,10 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public class FunctionalCommand extends Command {
-  protected final Runnable m_onInit;
-  protected final Runnable m_onExecute;
-  protected final Consumer<Boolean> m_onEnd;
-  protected final BooleanSupplier m_isFinished;
+  private final Runnable m_onInit;
+  private final Runnable m_onExecute;
+  private final Consumer<Boolean> m_onEnd;
+  private final BooleanSupplier m_isFinished;
 
   /**
    * Creates a new FunctionalCommand.
@@ -32,6 +32,7 @@
    * @param isFinished the function that determines whether the command has finished
    * @param requirements the subsystems required by this command
    */
+  @SuppressWarnings("this-escape")
   public FunctionalCommand(
       Runnable onInit,
       Runnable onExecute,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
index 16d9c8a..06663c9 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
@@ -86,6 +86,7 @@
    *     voltages.
    * @param requirements The subsystems to require.
    */
+  @SuppressWarnings("this-escape")
   public MecanumControllerCommand(
       Trajectory trajectory,
       Supplier<Pose2d> pose,
@@ -229,6 +230,7 @@
    * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
    * @param requirements The subsystems to require.
    */
+  @SuppressWarnings("this-escape")
   public MecanumControllerCommand(
       Trajectory trajectory,
       Supplier<Pose2d> pose,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
index ad1a12a..4109091 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
@@ -18,8 +18,8 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public class NotifierCommand extends Command {
-  protected final Notifier m_notifier;
-  protected final double m_period;
+  private final Notifier m_notifier;
+  private final double m_period;
 
   /**
    * Creates a new NotifierCommand.
@@ -28,6 +28,7 @@
    * @param period the period at which the notifier should run, in seconds
    * @param requirements the subsystems required by this command
    */
+  @SuppressWarnings("this-escape")
   public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
     m_notifier = new Notifier(toRun);
     m_period = period;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
index c761f3f..26627fa 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
@@ -19,9 +19,16 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public class PIDCommand extends Command {
+  /** PID controller. */
   protected final PIDController m_controller;
+
+  /** Measurement getter. */
   protected DoubleSupplier m_measurement;
+
+  /** Setpoint getter. */
   protected DoubleSupplier m_setpoint;
+
+  /** PID controller output consumer. */
   protected DoubleConsumer m_useOutput;
 
   /**
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
index 84318de..8146ca4 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -15,7 +15,10 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public abstract class PIDSubsystem extends SubsystemBase {
+  /** PID controller. */
   protected final PIDController m_controller;
+
+  /** Whether PID controller output is enabled. */
   protected boolean m_enabled;
 
   /**
@@ -24,6 +27,7 @@
    * @param controller the PIDController to use
    * @param initialPosition the initial setpoint of the subsystem
    */
+  @SuppressWarnings("this-escape")
   public PIDSubsystem(PIDController controller, double initialPosition) {
     m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
     setSetpoint(initialPosition);
@@ -46,6 +50,11 @@
     }
   }
 
+  /**
+   * Returns the PIDController.
+   *
+   * @return The controller.
+   */
   public PIDController getController() {
     return m_controller;
   }
@@ -55,7 +64,7 @@
    *
    * @param setpoint the setpoint for the subsystem
    */
-  public void setSetpoint(double setpoint) {
+  public final void setSetpoint(double setpoint) {
     m_controller.setSetpoint(setpoint);
   }
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
index 6d263e8..8acfea8 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
@@ -29,20 +29,18 @@
   private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
 
   /**
-   * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
+   * Creates a new ParallelDeadlineGroup. The given commands, including the deadline, will be
    * executed simultaneously. The composition will finish when the deadline finishes, interrupting
    * all other still-running commands. If the composition is interrupted, only the commands still
    * running will be interrupted.
    *
    * @param deadline the command that determines when the composition ends
-   * @param commands the commands to be executed
+   * @param otherCommands the other commands to be executed
+   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
    */
-  public ParallelDeadlineGroup(Command deadline, Command... commands) {
-    m_deadline = deadline;
-    addCommands(commands);
-    if (!m_commands.containsKey(deadline)) {
-      addCommands(deadline);
-    }
+  public ParallelDeadlineGroup(Command deadline, Command... otherCommands) {
+    addCommands(otherCommands);
+    setDeadline(deadline);
   }
 
   /**
@@ -50,11 +48,19 @@
    * contained.
    *
    * @param deadline the command that determines when the group ends
+   * @throws IllegalArgumentException if the deadline command is already in the composition
    */
-  public void setDeadline(Command deadline) {
-    if (!m_commands.containsKey(deadline)) {
-      addCommands(deadline);
+  public final void setDeadline(Command deadline) {
+    @SuppressWarnings("PMD.CompareObjectsWithEquals")
+    boolean isAlreadyDeadline = deadline == m_deadline;
+    if (isAlreadyDeadline) {
+      return;
     }
+    if (m_commands.containsKey(deadline)) {
+      throw new IllegalArgumentException(
+          "The deadline command cannot also be in the other commands!");
+    }
+    addCommands(deadline);
     m_deadline = deadline;
   }
 
@@ -74,7 +80,7 @@
     for (Command command : commands) {
       if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
         throw new IllegalArgumentException(
-            "Multiple commands in a parallel group cannot" + "require the same subsystems");
+            "Multiple commands in a parallel group cannot require the same subsystems");
       }
       m_commands.put(command, false);
       m_requirements.addAll(command.getRequirements());
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
index f7175fc..4e82811 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
@@ -21,9 +21,16 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public class ProfiledPIDCommand extends Command {
+  /** Profiled PID controller. */
   protected final ProfiledPIDController m_controller;
+
+  /** Measurement getter. */
   protected DoubleSupplier m_measurement;
+
+  /** Goal getter. */
   protected Supplier<State> m_goal;
+
+  /** Profiled PID controller output consumer. */
   protected BiConsumer<Double, State> m_useOutput;
 
   /**
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
index 283b4bf..05318ed 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
@@ -17,7 +17,10 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public abstract class ProfiledPIDSubsystem extends SubsystemBase {
+  /** Profiled PID controller. */
   protected final ProfiledPIDController m_controller;
+
+  /** Whether the profiled PID controller output is enabled. */
   protected boolean m_enabled;
 
   /**
@@ -47,6 +50,11 @@
     }
   }
 
+  /**
+   * Returns the ProfiledPIDController.
+   *
+   * @return The controller.
+   */
   public ProfiledPIDController getController() {
     return m_controller;
   }
@@ -56,7 +64,7 @@
    *
    * @param goal The goal state for the subsystem's motion profile.
    */
-  public void setGoal(TrapezoidProfile.State goal) {
+  public final void setGoal(TrapezoidProfile.State goal) {
     m_controller.setGoal(goal);
   }
 
@@ -65,7 +73,7 @@
    *
    * @param goal The goal position for the subsystem's motion profile.
    */
-  public void setGoal(double goal) {
+  public final void setGoal(double goal) {
     setGoal(new TrapezoidProfile.State(goal, 0));
   }
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
index d6e93e8..f05b286 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
@@ -35,6 +35,7 @@
    *
    * @param command the command to run by proxy
    */
+  @SuppressWarnings("this-escape")
   public ProxyCommand(Command command) {
     this(() -> command);
     setName("Proxy(" + command.getName() + ")");
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
index 9f19e4a..48b3756 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
@@ -70,6 +70,7 @@
    *     the robot drive.
    * @param requirements The subsystems to require.
    */
+  @SuppressWarnings("this-escape")
   public RamseteCommand(
       Trajectory trajectory,
       Supplier<Pose2d> pose,
@@ -109,6 +110,7 @@
    * @param outputMetersPerSecond A function that consumes the computed left and right wheel speeds.
    * @param requirements The subsystems to require.
    */
+  @SuppressWarnings("this-escape")
   public RamseteCommand(
       Trajectory trajectory,
       Supplier<Pose2d> pose,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java
index 5b49ae0..70ff63c 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java
@@ -20,7 +20,7 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public class RepeatCommand extends Command {
-  protected final Command m_command;
+  private final Command m_command;
   private boolean m_ended;
 
   /**
@@ -29,6 +29,7 @@
    *
    * @param command the command to run repeatedly
    */
+  @SuppressWarnings("this-escape")
   public RepeatCommand(Command command) {
     m_command = requireNonNullParam(command, "command", "RepeatCommand");
     CommandScheduler.getInstance().registerComposedCommands(command);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
index ac89dbd..ed94543 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
@@ -41,6 +41,15 @@
   default void simulationPeriodic() {}
 
   /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  default String getName() {
+    return this.getClass().getSimpleName();
+  }
+
+  /**
    * Sets the default {@link Command} of the subsystem. The default command will be automatically
    * scheduled when no other commands are scheduled that require the subsystem. Default commands
    * should generally not end on their own, i.e. their {@link Command#isFinished()} method should
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
index 6cf926e..3a024e5 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
@@ -15,7 +15,8 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public abstract class SubsystemBase implements Subsystem, Sendable {
-  /** Constructor. */
+  /** Constructor. Telemetry/log name defaults to the classname. */
+  @SuppressWarnings("this-escape")
   public SubsystemBase() {
     String name = this.getClass().getSimpleName();
     name = name.substring(name.lastIndexOf('.') + 1);
@@ -24,10 +25,22 @@
   }
 
   /**
+   * Constructor.
+   *
+   * @param name Name of the subsystem for telemetry and logging.
+   */
+  @SuppressWarnings("this-escape")
+  public SubsystemBase(String name) {
+    SendableRegistry.addLW(this, name, name);
+    CommandScheduler.getInstance().registerSubsystem(this);
+  }
+
+  /**
    * Gets the name of this Subsystem.
    *
    * @return Name
    */
+  @Override
   public String getName() {
     return SendableRegistry.getName(this);
   }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
index 3eacac3..424a288 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
@@ -186,6 +186,7 @@
    * @param outputModuleStates The raw output module states from the position controllers.
    * @param requirements The subsystems to require.
    */
+  @SuppressWarnings("this-escape")
   public SwerveControllerCommand(
       Trajectory trajectory,
       Supplier<Pose2d> pose,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
index 57185a9..3bfdbae 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
@@ -35,6 +35,7 @@
    * @param currentState The current state
    * @param requirements The subsystems required by this command.
    */
+  @SuppressWarnings("this-escape")
   public TrapezoidProfileCommand(
       TrapezoidProfile profile,
       Consumer<State> output,
@@ -60,6 +61,7 @@
    *     This allows you to change goals at runtime.
    */
   @Deprecated(since = "2024", forRemoval = true)
+  @SuppressWarnings("this-escape")
   public TrapezoidProfileCommand(
       TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
     m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
@@ -79,7 +81,7 @@
   @SuppressWarnings("removal")
   public void execute() {
     if (m_newAPI) {
-      m_output.accept(m_profile.calculate(m_timer.get(), m_goal.get(), m_currentState.get()));
+      m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get()));
     } else {
       m_output.accept(m_profile.calculate(m_timer.get()));
     }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
index 35c02e6..54d4ede 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
@@ -74,7 +74,7 @@
    *
    * @param goal The goal state for the subsystem's motion profile.
    */
-  public void setGoal(TrapezoidProfile.State goal) {
+  public final void setGoal(TrapezoidProfile.State goal) {
     m_goal = goal;
   }
 
@@ -83,7 +83,7 @@
    *
    * @param goal The goal position for the subsystem's motion profile.
    */
-  public void setGoal(double goal) {
+  public final void setGoal(double goal) {
     setGoal(new TrapezoidProfile.State(goal, 0));
   }
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
index e7b32be..6d4c52b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
@@ -14,7 +14,9 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public class WaitCommand extends Command {
+  /** The timer used for waiting. */
   protected Timer m_timer = new Timer();
+
   private final double m_duration;
 
   /**
@@ -22,6 +24,7 @@
    *
    * @param seconds the time to wait, in seconds
    */
+  @SuppressWarnings("this-escape")
   public WaitCommand(double seconds) {
     m_duration = seconds;
     SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
index c687f0a..5e6ccae 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
@@ -15,6 +15,7 @@
  * subsystems its components require.
  */
 public abstract class WrapperCommand extends Command {
+  /** Command being wrapped. */
   protected final Command m_command;
 
   /**
@@ -23,6 +24,7 @@
    * @param command the command being wrapped. Trying to directly schedule this command or add it to
    *     a composition will throw an exception.
    */
+  @SuppressWarnings("this-escape")
   protected WrapperCommand(Command command) {
     CommandScheduler.getInstance().registerComposedCommands(command);
     m_command = command;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandStadiaController.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandStadiaController.java
new file mode 100644
index 0000000..0d919e5
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandStadiaController.java
@@ -0,0 +1,405 @@
+// 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.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.StadiaController;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * A version of {@link StadiaController} with {@link Trigger} factories for command-based.
+ *
+ * @see StadiaController
+ */
+@SuppressWarnings("MethodName")
+public class CommandStadiaController extends CommandGenericHID {
+  private final StadiaController m_hid;
+
+  /**
+   * Construct an instance of a controller.
+   *
+   * @param port The port index on the Driver Station that the controller is plugged into.
+   */
+  public CommandStadiaController(int port) {
+    super(port);
+    m_hid = new StadiaController(port);
+  }
+
+  /**
+   * Get the underlying GenericHID object.
+   *
+   * @return the wrapped GenericHID object
+   */
+  @Override
+  public StadiaController getHID() {
+    return m_hid;
+  }
+
+  /**
+   * Constructs an event instance around the left bumper's digital signal.
+   *
+   * @return an event instance representing the left bumper's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #leftBumper(EventLoop)
+   */
+  public Trigger leftBumper() {
+    return leftBumper(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the left bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right bumper's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger leftBumper(EventLoop loop) {
+    return m_hid.leftBumper(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the right bumper's digital signal.
+   *
+   * @return an event instance representing the right bumper's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #rightBumper(EventLoop)
+   */
+  public Trigger rightBumper() {
+    return rightBumper(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the right bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left bumper's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger rightBumper(EventLoop loop) {
+    return m_hid.rightBumper(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the left stick button's digital signal.
+   *
+   * @return an event instance representing the left stick button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #leftStick(EventLoop)
+   */
+  public Trigger leftStick() {
+    return leftStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the left stick button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left stick button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger leftStick(EventLoop loop) {
+    return m_hid.leftStick(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the right stick button's digital signal.
+   *
+   * @return an event instance representing the right stick button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #rightStick(EventLoop)
+   */
+  public Trigger rightStick() {
+    return rightStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the right stick button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right stick button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger rightStick(EventLoop loop) {
+    return m_hid.rightStick(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the right trigger button's digital signal.
+   *
+   * @return an event instance representing the right trigger button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #rightTrigger(EventLoop)
+   */
+  public Trigger rightTrigger() {
+    return rightTrigger(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the right trigger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right trigger button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger rightTrigger(EventLoop loop) {
+    return m_hid.rightTrigger(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the left trigger button's digital signal.
+   *
+   * @return an event instance representing the left trigger button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #leftTrigger(EventLoop)
+   */
+  public Trigger leftTrigger() {
+    return leftTrigger(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the left trigger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left trigger button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger leftTrigger(EventLoop loop) {
+    return m_hid.leftTrigger(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the A button's digital signal.
+   *
+   * @return an event instance representing the A button's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #a(EventLoop)
+   */
+  public Trigger a() {
+    return a(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the A button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the A button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger a(EventLoop loop) {
+    return m_hid.a(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the B button's digital signal.
+   *
+   * @return an event instance representing the B button's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #b(EventLoop)
+   */
+  public Trigger b() {
+    return b(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the B button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the B button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger b(EventLoop loop) {
+    return m_hid.b(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the X button's digital signal.
+   *
+   * @return an event instance representing the X button's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #x(EventLoop)
+   */
+  public Trigger x() {
+    return x(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the X button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the X button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger x(EventLoop loop) {
+    return m_hid.x(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the Y button's digital signal.
+   *
+   * @return an event instance representing the Y button's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #y(EventLoop)
+   */
+  public Trigger y() {
+    return y(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the Y button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the Y button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger y(EventLoop loop) {
+    return m_hid.y(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the ellipses button's digital signal.
+   *
+   * @return an event instance representing the ellipses button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #ellipses(EventLoop)
+   */
+  public Trigger ellipses() {
+    return ellipses(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the ellipses button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the ellipses button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger ellipses(EventLoop loop) {
+    return m_hid.ellipses(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the stadia button's digital signal.
+   *
+   * @return an event instance representing the stadia button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #stadia(EventLoop)
+   */
+  public Trigger stadia() {
+    return stadia(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the stadia button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the stadia button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger stadia(EventLoop loop) {
+    return m_hid.stadia(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the google button's digital signal.
+   *
+   * @return an event instance representing the google button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #google(EventLoop)
+   */
+  public Trigger google() {
+    return google(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the google button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the google button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger google(EventLoop loop) {
+    return m_hid.google(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the frame button's digital signal.
+   *
+   * @return an event instance representing the frame button's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #frame(EventLoop)
+   */
+  public Trigger frame() {
+    return frame(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the frame button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the frame button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger frame(EventLoop loop) {
+    return m_hid.frame(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Constructs an event instance around the hamburger button's digital signal.
+   *
+   * @return an event instance representing the hamburger button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #hamburger(EventLoop)
+   */
+  public Trigger hamburger() {
+    return hamburger(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the hamburger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the hamburger button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger hamburger(EventLoop loop) {
+    return m_hid.hamburger(loop).castTo(Trigger::new);
+  }
+
+  /**
+   * Get the X axis value of left side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getLeftX() {
+    return m_hid.getLeftX();
+  }
+
+  /**
+   * Get the X axis value of right side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getRightX() {
+    return m_hid.getRightX();
+  }
+
+  /**
+   * Get the Y axis value of left side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getLeftY() {
+    return m_hid.getLeftY();
+  }
+
+  /**
+   * Get the Y axis value of right side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getRightY() {
+    return m_hid.getRightY();
+  }
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
index f4897f2..87c9b73 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
@@ -42,10 +42,20 @@
     this.m_inverted = inverted;
   }
 
+  /**
+   * Sets whether to invert button state.
+   *
+   * @param inverted Whether button state should be inverted.
+   */
   public void setInverted(boolean inverted) {
     m_inverted.set(inverted);
   }
 
+  /**
+   * Sets whether button is pressed.
+   *
+   * @param pressed Whether button is pressed.
+   */
   public void setPressed(boolean pressed) {
     m_pressed.set(pressed);
   }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java
new file mode 100644
index 0000000..9bef66a
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java
@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * A class containing static {@link Trigger} factories for running callbacks when the robot mode
+ * changes.
+ */
+public final class RobotModeTriggers {
+  // Utility class
+  private RobotModeTriggers() {}
+
+  /**
+   * Returns a trigger that is true when the robot is enabled in autonomous mode.
+   *
+   * @return A trigger that is true when the robot is enabled in autonomous mode.
+   */
+  public static Trigger autonomous() {
+    return new Trigger(DriverStation::isAutonomousEnabled);
+  }
+
+  /**
+   * Returns a trigger that is true when the robot is enabled in teleop mode.
+   *
+   * @return A trigger that is true when the robot is enabled in teleop mode.
+   */
+  public static Trigger teleop() {
+    return new Trigger(DriverStation::isTeleopEnabled);
+  }
+
+  /**
+   * Returns a trigger that is true when the robot is disabled.
+   *
+   * @return A trigger that is true when the robot is disabled.
+   */
+  public static Trigger disabled() {
+    return new Trigger(DriverStation::isDisabled);
+  }
+
+  /**
+   * Returns a trigger that is true when the robot is enabled in test mode.
+   *
+   * @return A trigger that is true when the robot is enabled in test mode.
+   */
+  public static Trigger test() {
+    return new Trigger(DriverStation::isTestEnabled);
+  }
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java
new file mode 100644
index 0000000..6a049a6
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java
@@ -0,0 +1,285 @@
+// 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.
+
+package edu.wpi.first.wpilibj2.command.sysid;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+import static java.util.Map.entry;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Time;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.Map;
+import java.util.function.Consumer;
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have multiple motors.
+ *
+ * <p>A single subsystem may have multiple mechanisms, but mechanisms should not share test
+ * routines. Each complete test of a mechanism should have its own SysIdRoutine instance, since the
+ * log name of the recorded data is determined by the mechanism name.
+ *
+ * <p>The test state (e.g. "quasistatic-forward") is logged once per iteration during test
+ * execution, and once with state "none" when a test ends. Motor frames are logged every iteration
+ * during test execution.
+ *
+ * <p>Timestamps are not coordinated across data, so motor frames and test state tags may be
+ * recorded on different log frames. Because frame alignment is not guaranteed, SysId parses the log
+ * by using the test state flag to determine the timestamp range for each section of the test, and
+ * then extracts the motor frames within the valid timestamp ranges. If a given test was run
+ * multiple times in a single logfile, the user will need to select which of the tests to use for
+ * the fit in the analysis tool.
+ */
+public class SysIdRoutine extends SysIdRoutineLog {
+  private final Config m_config;
+  private final Mechanism m_mechanism;
+  private final MutableMeasure<Voltage> m_outputVolts = mutable(Volts.of(0));
+  private final Consumer<State> m_recordState;
+
+  /**
+   * Create a new SysId characterization routine.
+   *
+   * @param config Hardware-independent parameters for the SysId routine.
+   * @param mechanism Hardware interface for the SysId routine.
+   */
+  public SysIdRoutine(Config config, Mechanism mechanism) {
+    super(mechanism.m_subsystem.getName());
+    m_config = config;
+    m_mechanism = mechanism;
+    m_recordState = config.m_recordState != null ? config.m_recordState : this::recordState;
+  }
+
+  /** Hardware-independent configuration for a SysId test routine. */
+  public static class Config {
+    /** The voltage ramp rate used for quasistatic test routines. */
+    public final Measure<Velocity<Voltage>> m_rampRate;
+
+    /** The step voltage output used for dynamic test routines. */
+    public final Measure<Voltage> m_stepVoltage;
+
+    /** Safety timeout for the test routine commands. */
+    public final Measure<Time> m_timeout;
+
+    /** Optional handle for recording test state in a third-party logging solution. */
+    public final Consumer<State> m_recordState;
+
+    /**
+     * Create a new configuration for a SysId test routine.
+     *
+     * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+     *     per second if left null.
+     * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+     *     volts if left null.
+     * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+     *     null.
+     * @param recordState Optional handle for recording test state in a third-party logging
+     *     solution. If provided, the test routine state will be passed to this callback instead of
+     *     logged in WPILog.
+     */
+    public Config(
+        Measure<Velocity<Voltage>> rampRate,
+        Measure<Voltage> stepVoltage,
+        Measure<Time> timeout,
+        Consumer<State> recordState) {
+      m_rampRate = rampRate != null ? rampRate : Volts.of(1).per(Seconds.of(1));
+      m_stepVoltage = stepVoltage != null ? stepVoltage : Volts.of(7);
+      m_timeout = timeout != null ? timeout : Seconds.of(10);
+      m_recordState = recordState;
+    }
+
+    /**
+     * Create a new configuration for a SysId test routine.
+     *
+     * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+     *     per second if left null.
+     * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+     *     volts if left null.
+     * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+     *     null.
+     */
+    public Config(
+        Measure<Velocity<Voltage>> rampRate, Measure<Voltage> stepVoltage, Measure<Time> timeout) {
+      this(rampRate, stepVoltage, timeout, null);
+    }
+
+    /**
+     * Create a default configuration for a SysId test routine with all default settings.
+     *
+     * <p>rampRate: 1 volt/sec
+     *
+     * <p>stepVoltage: 7 volts
+     *
+     * <p>timeout: 10 seconds
+     */
+    public Config() {
+      this(null, null, null, null);
+    }
+  }
+
+  /**
+   * A mechanism to be characterized by a SysId routine. Defines callbacks needed for the SysId test
+   * routine to control and record data from the mechanism.
+   */
+  public static class Mechanism {
+    /** Sends the SysId-specified drive signal to the mechanism motors during test routines. */
+    public final Consumer<Measure<Voltage>> m_drive;
+
+    /**
+     * Returns measured data (voltages, positions, velocities) of the mechanism motors during test
+     * routines.
+     */
+    public final Consumer<SysIdRoutineLog> m_log;
+
+    /** The subsystem containing the motor(s) that is (or are) being characterized. */
+    public final Subsystem m_subsystem;
+
+    /** The name of the mechanism being tested. */
+    public final String m_name;
+
+    /**
+     * Create a new mechanism specification for a SysId routine.
+     *
+     * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+     *     routines.
+     * @param log Returns measured data of the mechanism motors during test routines. To return
+     *     data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then
+     *     call one or more of the chainable logging handles (e.g. `voltage`) on the returned
+     *     `MotorLog`. Multiple motors can be logged in a single callback by calling `motor`
+     *     multiple times.
+     * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+     *     Will be declared as a requirement for the returned test commands.
+     * @param name The name of the mechanism being tested. Will be appended to the log entry title
+     *     for the routine's test state, e.g. "sysid-test-state-mechanism". Defaults to the name of
+     *     the subsystem if left null.
+     */
+    public Mechanism(
+        Consumer<Measure<Voltage>> drive,
+        Consumer<SysIdRoutineLog> log,
+        Subsystem subsystem,
+        String name) {
+      m_drive = drive;
+      m_log = log != null ? log : l -> {};
+      m_subsystem = subsystem;
+      m_name = name != null ? name : subsystem.getName();
+    }
+
+    /**
+     * Create a new mechanism specification for a SysId routine. Defaults the mechanism name to the
+     * subsystem name.
+     *
+     * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+     *     routines.
+     * @param log Returns measured data of the mechanism motors during test routines. To return
+     *     data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then
+     *     call one or more of the chainable logging handles (e.g. `voltage`) on the returned
+     *     `MotorLog`. Multiple motors can be logged in a single callback by calling `motor`
+     *     multiple times.
+     * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+     *     Will be declared as a requirement for the returned test commands. The subsystem's `name`
+     *     will be appended to the log entry title for the routine's test state, e.g.
+     *     "sysid-test-state-subsystem".
+     */
+    public Mechanism(
+        Consumer<Measure<Voltage>> drive, Consumer<SysIdRoutineLog> log, Subsystem subsystem) {
+      this(drive, log, subsystem, null);
+    }
+  }
+
+  /** Motor direction for a SysId test. */
+  public enum Direction {
+    /** Forward. */
+    kForward,
+    /** Reverse. */
+    kReverse
+  }
+
+  /**
+   * Returns a command to run a quasistatic test in the specified direction.
+   *
+   * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+   * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+   * 0 volts.
+   *
+   * @param direction The direction in which to run the test.
+   * @return A command to run the test.
+   */
+  public Command quasistatic(Direction direction) {
+    Timer timer = new Timer();
+    double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+    State state =
+        Map.ofEntries(
+                entry(Direction.kForward, State.kQuasistaticForward),
+                entry(Direction.kReverse, State.kQuasistaticReverse))
+            .get(direction);
+
+    return m_mechanism
+        .m_subsystem
+        .runOnce(timer::start)
+        .andThen(
+            m_mechanism.m_subsystem.run(
+                () -> {
+                  m_mechanism.m_drive.accept(
+                      m_outputVolts.mut_replace(
+                          outputSign * timer.get() * m_config.m_rampRate.in(Volts.per(Second)),
+                          Volts));
+                  m_mechanism.m_log.accept(this);
+                  m_recordState.accept(state);
+                }))
+        .finallyDo(
+            () -> {
+              m_mechanism.m_drive.accept(Volts.of(0));
+              m_recordState.accept(State.kNone);
+              timer.stop();
+            })
+        .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+        .withTimeout(m_config.m_timeout.in(Seconds));
+  }
+
+  /**
+   * Returns a command to run a dynamic test in the specified direction.
+   *
+   * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+   * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+   * 0 volts.
+   *
+   * @param direction The direction in which to run the test.
+   * @return A command to run the test.
+   */
+  public Command dynamic(Direction direction) {
+    double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+    State state =
+        Map.ofEntries(
+                entry(Direction.kForward, State.kDynamicForward),
+                entry(Direction.kReverse, State.kDynamicReverse))
+            .get(direction);
+
+    return m_mechanism
+        .m_subsystem
+        .runOnce(
+            () -> m_outputVolts.mut_replace(m_config.m_stepVoltage.in(Volts) * outputSign, Volts))
+        .andThen(
+            m_mechanism.m_subsystem.run(
+                () -> {
+                  m_mechanism.m_drive.accept(m_outputVolts);
+                  m_mechanism.m_log.accept(this);
+                  m_recordState.accept(state);
+                }))
+        .finallyDo(
+            () -> {
+              m_mechanism.m_drive.accept(Volts.of(0));
+              m_recordState.accept(State.kNone);
+            })
+        .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+        .withTimeout(m_config.m_timeout.in(Seconds));
+  }
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
index 66de555..de673f2 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -4,6 +4,7 @@
 
 #include "frc2/command/Command.h"
 
+#include <wpi/StackTrace.h>
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
@@ -31,7 +32,7 @@
 }
 
 Command& Command::operator=(const Command& rhs) {
-  m_isComposed = false;
+  SetComposed(false);
   return *this;
 }
 
@@ -156,11 +157,19 @@
 }
 
 bool Command::IsComposed() const {
-  return m_isComposed;
+  return GetPreviousCompositionSite().has_value();
 }
 
 void Command::SetComposed(bool isComposed) {
-  m_isComposed = isComposed;
+  if (isComposed) {
+    m_previousComposition = wpi::GetStackTrace(1);
+  } else {
+    m_previousComposition.reset();
+  }
+}
+
+std::optional<std::string> Command::GetPreviousCompositionSite() const {
+  return m_previousComposition;
 }
 
 void Command::InitSendable(wpi::SendableBuilder& builder) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp
index 6f7e418..6fa86d3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp
@@ -22,10 +22,22 @@
 
 using namespace frc2;
 
+CommandPtr::CommandPtr(std::unique_ptr<Command>&& command)
+    : m_ptr(std::move(command)) {
+  AssertValid();
+}
+
+CommandPtr::CommandPtr(CommandPtr&& rhs) {
+  m_ptr = std::move(rhs.m_ptr);
+  AssertValid();
+  rhs.m_moveOutSite = wpi::GetStackTrace(1);
+}
+
 void CommandPtr::AssertValid() const {
   if (!m_ptr) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
-                        "Moved-from CommandPtr object used!");
+                        "Moved-from CommandPtr object used!\nMoved out at:\n{}",
+                        m_moveOutSite);
   }
 }
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index d61ffbd..efeee4e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -183,7 +183,7 @@
     if constexpr (frc::RobotBase::IsSimulation()) {
       subsystem.getFirst()->SimulationPeriodic();
     }
-    m_watchdog.AddEpoch("Subsystem Periodic()");
+    m_watchdog.AddEpoch(subsystem.getFirst()->GetName() + ".Periodic()");
   }
 
   // Cache the active instance to avoid concurrency problems if SetActiveLoop()
@@ -453,11 +453,13 @@
 }
 
 void CommandScheduler::RequireUngrouped(const Command* command) {
-  if (command->IsComposed()) {
+  auto stacktrace = command->GetPreviousCompositionSite();
+  if (stacktrace.has_value()) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
                         "Commands that have been composed may not be added to "
-                        "another composition or scheduled "
-                        "individually!");
+                        "another composition or scheduled individually!"
+                        "\nOriginally composed at:\n{}",
+                        stacktrace.value());
   }
 }
 
@@ -475,6 +477,29 @@
   }
 }
 
+void CommandScheduler::RequireUngroupedAndUnscheduled(const Command* command) {
+  if (IsScheduled(command)) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands that have been scheduled individually may "
+                        "not be added to another composition!");
+  }
+  RequireUngrouped(command);
+}
+
+void CommandScheduler::RequireUngroupedAndUnscheduled(
+    std::span<const std::unique_ptr<Command>> commands) {
+  for (auto&& command : commands) {
+    RequireUngroupedAndUnscheduled(command.get());
+  }
+}
+
+void CommandScheduler::RequireUngroupedAndUnscheduled(
+    std::initializer_list<const Command*> commands) {
+  for (auto&& command : commands) {
+    RequireUngroupedAndUnscheduled(command);
+  }
+}
+
 void CommandScheduler::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
   builder.AddStringArrayProperty(
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
index 4c07c7a..6ce33b7 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
@@ -12,7 +12,7 @@
                                        std::unique_ptr<Command>&& onFalse,
                                        std::function<bool()> condition)
     : m_condition{std::move(condition)} {
-  CommandScheduler::GetInstance().RequireUngrouped(
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
       {onTrue.get(), onFalse.get()});
 
   m_onTrue = std::move(onTrue);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
index 902c26b..6059db4 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -6,7 +6,12 @@
 
 #include <utility>
 
+#include <units/velocity.h>
+#include <units/voltage.h>
+
 using namespace frc2;
+using kv_unit = units::compound_unit<units::volts,
+                                     units::inverse<units::meters_per_second>>;
 
 MecanumControllerCommand::MecanumControllerCommand(
     frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
@@ -95,6 +100,7 @@
     Requirements requirements)
     : m_trajectory(std::move(trajectory)),
       m_pose(std::move(pose)),
+      m_feedforward(0_V, units::unit_t<kv_unit>{0}),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
       m_desiredRotation(std::move(desiredRotation)),
@@ -116,6 +122,7 @@
     Requirements requirements)
     : m_trajectory(std::move(trajectory)),
       m_pose(std::move(pose)),
+      m_feedforward(0_V, units::unit_t<kv_unit>{0}),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
       m_maxWheelVelocity(maxWheelVelocity),
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
index 99e0845..4a234b1 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
@@ -63,7 +63,7 @@
 
 void ParallelCommandGroup::AddCommands(
     std::vector<std::unique_ptr<Command>>&& commands) {
-  CommandScheduler::GetInstance().RequireUngrouped(commands);
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
 
   if (isRunning) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
index a6b5c1c..1c9b700 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -62,7 +62,7 @@
 
 void ParallelDeadlineGroup::AddCommands(
     std::vector<std::unique_ptr<Command>>&& commands) {
-  CommandScheduler::GetInstance().RequireUngrouped(commands);
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
 
   if (!m_finished) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
index df658bc..334286f 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -50,7 +50,7 @@
 
 void ParallelRaceGroup::AddCommands(
     std::vector<std::unique_ptr<Command>>&& commands) {
-  CommandScheduler::GetInstance().RequireUngrouped(commands);
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
 
   if (isRunning) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
index 9145985..b78b6ad 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -6,9 +6,13 @@
 
 #include <utility>
 
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/sendable/SendableBuilder.h>
 
 using namespace frc2;
+using kv_unit = units::compound_unit<units::volts,
+                                     units::inverse<units::meters_per_second>>;
 
 RamseteCommand::RamseteCommand(
     frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
@@ -42,6 +46,7 @@
     : m_trajectory(std::move(trajectory)),
       m_pose(std::move(pose)),
       m_controller(controller),
+      m_feedforward(0_V, units::unit_t<kv_unit>{0}),
       m_kinematics(std::move(kinematics)),
       m_outputVel(std::move(output)),
       m_usePID(false) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp
index 2e74c8c..2a08f1b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp
@@ -9,7 +9,7 @@
 using namespace frc2;
 
 RepeatCommand::RepeatCommand(std::unique_ptr<Command>&& command) {
-  CommandScheduler::GetInstance().RequireUngrouped(command.get());
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(command.get());
   m_command = std::move(command);
   m_command->SetComposed(true);
   AddRequirements(m_command->GetRequirements());
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
index b9ea3d5..173b9d9 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
@@ -62,7 +62,7 @@
 
 void SequentialCommandGroup::AddCommands(
     std::vector<std::unique_ptr<Command>>&& commands) {
-  CommandScheduler::GetInstance().RequireUngrouped(commands);
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
 
   if (m_currentCommandIndex != invalid_index) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
index 4c06f1f..a0241bd 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
@@ -4,6 +4,8 @@
 
 #include "frc2/command/Subsystem.h"
 
+#include <wpi/Demangle.h>
+
 #include "frc2/command/CommandPtr.h"
 #include "frc2/command/Commands.h"
 
@@ -16,6 +18,10 @@
 
 void Subsystem::SimulationPeriodic() {}
 
+std::string Subsystem::GetName() const {
+  return wpi::GetTypeName(*this);
+}
+
 void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
   CommandScheduler::GetInstance().SetDefaultCommand(this,
                                                     std::move(defaultCommand));
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
index 8216a07..e5a4038 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -17,6 +17,11 @@
   CommandScheduler::GetInstance().RegisterSubsystem({this});
 }
 
+SubsystemBase::SubsystemBase(std::string_view name) {
+  wpi::SendableRegistry::AddLW(this, name);
+  CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
 void SubsystemBase::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Subsystem");
   builder.AddBooleanProperty(
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
index f7928c9..6391294 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
@@ -9,7 +9,7 @@
 using namespace frc2;
 
 WrapperCommand::WrapperCommand(std::unique_ptr<Command>&& command) {
-  CommandScheduler::GetInstance().RequireUngrouped(command.get());
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(command.get());
   m_command = std::move(command);
   m_command->SetComposed(true);
   // copy the wrapped command's name
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandStadiaController.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandStadiaController.cpp
new file mode 100644
index 0000000..86c34ba
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandStadiaController.cpp
@@ -0,0 +1,72 @@
+// 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 "frc2/command/button/CommandStadiaController.h"
+
+using namespace frc2;
+
+Trigger CommandStadiaController::Button(int button,
+                                        frc::EventLoop* loop) const {
+  return GenericHID::Button(button, loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::LeftBumper(frc::EventLoop* loop) const {
+  return StadiaController::LeftBumper(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::RightBumper(frc::EventLoop* loop) const {
+  return StadiaController::RightBumper(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::LeftStick(frc::EventLoop* loop) const {
+  return StadiaController::LeftStick(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::RightStick(frc::EventLoop* loop) const {
+  return StadiaController::RightStick(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::A(frc::EventLoop* loop) const {
+  return StadiaController::A(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::B(frc::EventLoop* loop) const {
+  return StadiaController::B(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::X(frc::EventLoop* loop) const {
+  return StadiaController::X(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Y(frc::EventLoop* loop) const {
+  return StadiaController::Y(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Ellipses(frc::EventLoop* loop) const {
+  return StadiaController::Ellipses(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Hamburger(frc::EventLoop* loop) const {
+  return StadiaController::Hamburger(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Stadia(frc::EventLoop* loop) const {
+  return StadiaController::Stadia(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Google(frc::EventLoop* loop) const {
+  return StadiaController::Google(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Frame(frc::EventLoop* loop) const {
+  return StadiaController::Frame(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::LeftTrigger(frc::EventLoop* loop) const {
+  return StadiaController::LeftTrigger(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::RightTrigger(frc::EventLoop* loop) const {
+  return StadiaController::RightTrigger(loop).CastTo<Trigger>();
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp
new file mode 100644
index 0000000..ab593d8
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp
@@ -0,0 +1,25 @@
+// 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 "frc2/command/button/RobotModeTriggers.h"
+
+#include <frc/DriverStation.h>
+
+using namespace frc2;
+
+Trigger RobotModeTriggers::Autonomous() {
+  return Trigger{&frc::DriverStation::IsAutonomousEnabled};
+}
+
+Trigger RobotModeTriggers::Teleop() {
+  return Trigger{&frc::DriverStation::IsTeleopEnabled};
+}
+
+Trigger RobotModeTriggers::Disabled() {
+  return Trigger{&frc::DriverStation::IsDisabled};
+}
+
+Trigger RobotModeTriggers::Test() {
+  return Trigger{&frc::DriverStation::IsTestEnabled};
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
index 38ec741..190a544 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -212,3 +212,7 @@
     return debouncer.Calculate(condition());
   });
 }
+
+bool Trigger::Get() const {
+  return m_condition();
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp
new file mode 100644
index 0000000..5f677fe
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp
@@ -0,0 +1,64 @@
+// 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 "frc2/command/sysid/SysIdRoutine.h"
+
+using namespace frc2::sysid;
+
+frc2::CommandPtr SysIdRoutine::Quasistatic(Direction direction) {
+  std::unordered_map<Direction, frc::sysid::State> stateOptions{
+      {Direction::kForward, frc::sysid::State::kQuasistaticForward},
+      {Direction::kReverse, frc::sysid::State::kQuasistaticReverse},
+  };
+  frc::sysid::State state = stateOptions[direction];
+  double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+  return m_mechanism.m_subsystem
+      ->RunOnce([this] {
+        timer.Reset();
+        timer.Start();
+      })
+      .AndThen(
+          m_mechanism.m_subsystem
+              ->Run([this, state, outputSign] {
+                m_outputVolts = outputSign * timer.Get() * m_config.m_rampRate;
+                m_mechanism.m_drive(m_outputVolts);
+                m_mechanism.m_log(this);
+                m_recordState(state);
+              })
+              .FinallyDo([this] {
+                m_mechanism.m_drive(0_V);
+                m_recordState(frc::sysid::State::kNone);
+                timer.Stop();
+              })
+              .WithName("sysid-" +
+                        frc::sysid::SysIdRoutineLog::StateEnumToString(state) +
+                        "-" + m_mechanism.m_name)
+              .WithTimeout(m_config.m_timeout));
+}
+
+frc2::CommandPtr SysIdRoutine::Dynamic(Direction direction) {
+  std::unordered_map<Direction, frc::sysid::State> stateOptions{
+      {Direction::kForward, frc::sysid::State::kDynamicForward},
+      {Direction::kReverse, frc::sysid::State::kDynamicReverse},
+  };
+  frc::sysid::State state = stateOptions[direction];
+  double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+  return m_mechanism.m_subsystem
+      ->RunOnce([this] { m_outputVolts = m_config.m_stepVoltage; })
+      .AndThen(m_mechanism.m_subsystem->Run([this, state, outputSign] {
+        m_mechanism.m_drive(m_outputVolts * outputSign);
+        m_mechanism.m_log(this);
+        m_recordState(state);
+      }))
+      .FinallyDo([this] {
+        m_mechanism.m_drive(0_V);
+        m_recordState(frc::sysid::State::kNone);
+      })
+      .WithName("sysid-" +
+                frc::sysid::SysIdRoutineLog::StateEnumToString(state) + "-" +
+                m_mechanism.m_name)
+      .WithTimeout(m_config.m_timeout);
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index 9a48b2e..ad6aa79 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -6,11 +6,13 @@
 
 #include <functional>
 #include <memory>
+#include <optional>
 #include <string>
 
 #include <units/time.h>
 #include <wpi/Demangle.h>
 #include <wpi/SmallSet.h>
+#include <wpi/StackTrace.h>
 #include <wpi/sendable/Sendable.h>
 
 #include "frc2/command/Requirements.h"
@@ -18,11 +20,6 @@
 
 namespace frc2 {
 
-template <typename T>
-std::string GetTypeName(const T& type) {
-  return wpi::Demangle(typeid(type).name());
-}
-
 /**
  * A state machine representing a complete action to be performed by the robot.
  * Commands are run by the CommandScheduler, and can be composed into
@@ -393,6 +390,14 @@
   void SetComposed(bool isComposed);
 
   /**
+   * Get the stacktrace of where this command was composed, or an empty
+   * optional. Intended for internal use.
+   *
+   * @return optional string representation of the composition site stack trace.
+   */
+  std::optional<std::string> GetPreviousCompositionSite() const;
+
+  /**
    * Whether the given command should run when the robot is disabled.  Override
    * to return true if the command should run when disabled.
    *
@@ -421,15 +426,17 @@
  protected:
   Command();
 
+  /// Requirements set.
   wpi::SmallSet<Subsystem*, 4> m_requirements;
 
   /**
    * Transfers ownership of this command to a unique pointer.  Used for
    * decorator methods.
    */
+  [[deprecated("Use ToPtr() instead")]]
   virtual std::unique_ptr<Command> TransferOwnership() && = 0;
 
-  bool m_isComposed = false;
+  std::optional<std::string> m_previousComposition;
 };
 
 /**
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
index ed93b65..ba6fe92 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
@@ -8,6 +8,8 @@
 #include <memory>
 #include <utility>
 
+#include <wpi/deprecated.h>
+
 #include "frc2/command/Command.h"
 #include "frc2/command/CommandPtr.h"
 
@@ -34,6 +36,7 @@
   }
 
  protected:
+  WPI_DEPRECATED("Use ToPtr() instead")
   std::unique_ptr<Command> TransferOwnership() && override {
     return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
   }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
index ae8c118..2610684 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
@@ -7,6 +7,7 @@
 #include <concepts>
 #include <functional>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -27,8 +28,7 @@
  */
 class CommandPtr final {
  public:
-  explicit CommandPtr(std::unique_ptr<Command>&& command)
-      : m_ptr(std::move(command)) {}
+  explicit CommandPtr(std::unique_ptr<Command>&& command);
 
   template <std::derived_from<Command> T>
   // NOLINTNEXTLINE(bugprone-forwarding-reference-overload)
@@ -36,9 +36,11 @@
       : CommandPtr(
             std::make_unique<std::decay_t<T>>(std::forward<T>(command))) {}
 
-  CommandPtr(CommandPtr&&) = default;
+  CommandPtr(CommandPtr&&);
   CommandPtr& operator=(CommandPtr&&) = default;
 
+  explicit CommandPtr(std::nullptr_t) = delete;
+
   /**
    * Decorates this command to run repeatedly, restarting it when it ends, until
    * this command is interrupted. The decorated command can still be canceled.
@@ -326,6 +328,7 @@
 
  private:
   std::unique_ptr<Command> m_ptr;
+  std::string m_moveOutSite{""};
   void AssertValid() const;
 };
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index ce4dc91..1f8d742 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -374,7 +374,7 @@
   void OnCommandFinish(Action action);
 
   /**
-   * Requires that the specified command hasn't been already added to a
+   * Requires that the specified command hasn't already been added to a
    * composition.
    *
    * @param command The command to check
@@ -383,7 +383,7 @@
   void RequireUngrouped(const Command* command);
 
   /**
-   * Requires that the specified commands not have been already added to a
+   * Requires that the specified commands have not already been added to a
    * composition.
    *
    * @param commands The commands to check
@@ -392,7 +392,7 @@
   void RequireUngrouped(std::span<const std::unique_ptr<Command>> commands);
 
   /**
-   * Requires that the specified commands not have been already added to a
+   * Requires that the specified commands have not already been added to a
    * composition.
    *
    * @param commands The commands to check
@@ -401,6 +401,38 @@
    */
   void RequireUngrouped(std::initializer_list<const Command*> commands);
 
+  /**
+   * Requires that the specified command has not already been added to a
+   * composition and is not currently scheduled.
+   *
+   * @param command The command to check
+   * @throws IllegalArgumentException if the given command has already been
+   * composed or scheduled.
+   */
+  void RequireUngroupedAndUnscheduled(const Command* command);
+
+  /**
+   * Requires that the specified commands have not already been added to a
+   * composition and are not currently scheduled.
+   *
+   * @param commands The commands to check
+   * @throws IllegalArgumentException if the given commands have already been
+   * composed.
+   */
+  void RequireUngroupedAndUnscheduled(
+      std::span<const std::unique_ptr<Command>> commands);
+
+  /**
+   * Requires that the specified commands have not already been added to a
+   * composition and are not currently scheduled.
+   *
+   * @param commands The commands to check
+   * @throws IllegalArgumentException if the given commands have already been
+   * composed or scheduled.
+   */
+  void RequireUngroupedAndUnscheduled(
+      std::initializer_list<const Command*> commands);
+
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
index bede6d0..ab16e7d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -74,9 +74,16 @@
   frc::PIDController& GetController();
 
  protected:
+  /// PID controller.
   frc::PIDController m_controller;
+
+  /// Measurement getter.
   std::function<double()> m_measurement;
+
+  /// Setpoint getter.
   std::function<double()> m_setpoint;
+
+  /// PID controller output consumer.
   std::function<void(double)> m_useOutput;
 };
 }  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index af61430..af7d298 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -69,7 +69,10 @@
   frc::PIDController& GetController();
 
  protected:
+  /// PID controller.
   frc::PIDController m_controller;
+
+  /// Whether PID controller output is enabled.
   bool m_enabled{false};
 
   /**
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index 9ea5db5..6fbb12b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -139,9 +139,16 @@
   frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
 
  protected:
+  /// Profiled PID controller.
   frc::ProfiledPIDController<Distance> m_controller;
+
+  /// Measurement getter.
   std::function<Distance_t()> m_measurement;
+
+  /// Goal getter.
   std::function<State()> m_goal;
+
+  /// Profiled PID controller output consumer.
   std::function<void(double, State)> m_useOutput;
 };
 }  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
index cfdfc6b..1dac7fa 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -91,7 +91,10 @@
   frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
 
  protected:
+  /// Profiled PID controller.
   frc::ProfiledPIDController<Distance> m_controller;
+
+  /// Whether the profiled PID controller output is enabled.
   bool m_enabled{false};
 
   /**
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
index d425d76..cc31106 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
@@ -17,6 +17,7 @@
 #include <utility>
 #include <vector>
 
+#include <wpi/deprecated.h>
 #include <wpi/sendable/SendableBuilder.h>
 
 #include "frc2/command/Command.h"
@@ -56,7 +57,8 @@
 
     m_defaultCommand.SetComposed(true);
     for (auto&& command : foo) {
-      CommandScheduler::GetInstance().RequireUngrouped(command.second.get());
+      CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
+          command.second.get());
       command.second.get()->SetComposed(true);
     }
 
@@ -77,7 +79,8 @@
       : m_selector{std::move(selector)} {
     m_defaultCommand.SetComposed(true);
     for (auto&& command : commands) {
-      CommandScheduler::GetInstance().RequireUngrouped(command.second.get());
+      CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
+          command.second.get());
       command.second.get()->SetComposed(true);
     }
 
@@ -132,6 +135,7 @@
   }
 
  protected:
+  WPI_DEPRECATED("Use ToPtr() instead")
   std::unique_ptr<Command> TransferOwnership() && override {
     return std::make_unique<SelectCommand>(std::move(*this));
   }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
index cdac0c0..b95b837 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -6,6 +6,7 @@
 
 #include <concepts>
 #include <functional>
+#include <string>
 #include <utility>
 
 #include <wpi/FunctionExtras.h>
@@ -60,6 +61,13 @@
   virtual void SimulationPeriodic();
 
   /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  virtual std::string GetName() const;
+
+  /**
    * Sets the default Command of the subsystem.  The default command will be
    * automatically scheduled when no other commands are scheduled that require
    * the subsystem. Default commands should generally not end on their own, i.e.
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
index 86fb026..444aca5 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
@@ -30,7 +30,7 @@
    *
    * @return Name
    */
-  std::string GetName() const;
+  std::string GetName() const override;
 
   /**
    * Sets the name of this Subsystem.
@@ -63,6 +63,15 @@
   void AddChild(std::string name, wpi::Sendable* child);
 
  protected:
+  /**
+   * Constructor.  Telemetry/log name defaults to the classname.
+   */
   SubsystemBase();
+  /**
+   * Constructor.
+   *
+   * @param name Name of the subsystem for telemetry and logging.
+   */
+  explicit SubsystemBase(std::string_view name);
 };
 }  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 69cc8d8..289b1cb 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -79,7 +79,7 @@
   void Initialize() override { m_timer.Restart(); }
 
   void Execute() override {
-    m_output(m_profile.Calculate(m_timer.Get(), m_goal(), m_currentState()));
+    m_output(m_profile.Calculate(m_timer.Get(), m_currentState(), m_goal()));
   }
 
   void End(bool interrupted) override { m_timer.Stop(); }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
index e28615e..6f6db40 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
@@ -41,6 +41,7 @@
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  protected:
+  /// The timer used for waiting.
   frc::Timer m_timer;
 
  private:
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
index 98e8b20..97a8693 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
@@ -69,6 +69,7 @@
   wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
 
  protected:
+  /// Command being wrapped.
   std::unique_ptr<Command> m_command;
 };
 }  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandStadiaController.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandStadiaController.h
new file mode 100644
index 0000000..7a1b549
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandStadiaController.h
@@ -0,0 +1,201 @@
+// 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 <frc/StadiaController.h>
+
+#include "Trigger.h"
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+/**
+ * A version of {@link StadiaController} with {@link Trigger} factories for
+ * command-based.
+ *
+ * @see StadiaController
+ */
+class CommandStadiaController : public frc::StadiaController {
+ public:
+  using StadiaController::StadiaController;
+
+  /**
+   * Constructs an event instance around this button's digital signal.
+   *
+   * @param button the button index
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the button's digital signal attached
+   * to the given loop.
+   */
+  Trigger Button(int button,
+                 frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                            .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the left bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the left bumper's digital signal
+   * attached to the given loop.
+   */
+  Trigger LeftBumper(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the right bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the right bumper's digital signal
+   * attached to the given loop.
+   */
+  Trigger RightBumper(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                 .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the left stick's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the left stick's digital signal
+   * attached to the given loop.
+   */
+  Trigger LeftStick(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the right stick's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the right stick's digital signal
+   * attached to the given loop.
+   */
+  Trigger RightStick(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the A button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the A button's digital signal
+   * attached to the given loop.
+   */
+  Trigger A(frc::EventLoop* loop =
+                CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the B button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the B button's digital signal
+   * attached to the given loop.
+   */
+  Trigger B(frc::EventLoop* loop =
+                CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the X button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the X button's digital signal
+   * attached to the given loop.
+   */
+  Trigger X(frc::EventLoop* loop =
+                CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the Y button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the Y button's digital signal
+   * attached to the given loop.
+   */
+  Trigger Y(frc::EventLoop* loop =
+                CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the ellipses button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the ellipses button's digital signal
+   * attached to the given loop.
+   */
+  Trigger Ellipses(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                              .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the hamburger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the hamburger button's digital
+   * signal attached to the given loop.
+   */
+  Trigger Hamburger(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the stadia button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the stadia button's digital signal
+   * attached to the given loop.
+   */
+  Trigger Stadia(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                            .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the google button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the google button's digital signal
+   * attached to the given loop.
+   */
+  Trigger Google(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                            .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the frame button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the frame button's digital signal
+   * attached to the given loop.
+   */
+  Trigger Frame(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the left trigger's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the left trigger's digital signal
+   * attached to the given loop.
+   */
+  Trigger LeftTrigger(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                 .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the right trigger's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the right trigger's digital signal
+   * attached to the given loop.
+   */
+  Trigger RightTrigger(
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+};
+}  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h
new file mode 100644
index 0000000..2fbcc64
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h
@@ -0,0 +1,50 @@
+// 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 "frc2/command/button/Trigger.h"
+
+namespace frc2 {
+
+/**
+ * A class containing static Trigger factories for running callbacks when robot
+ * mode changes.
+ */
+class RobotModeTriggers {
+ public:
+  RobotModeTriggers() = delete;
+
+  /**
+   * Returns a trigger that is true when the robot is enabled in autonomous
+   * mode.
+   *
+   * @return A trigger that is true when the robot is enabled in autonomous
+   * mode.
+   */
+  static Trigger Autonomous();
+
+  /**
+   * Returns a trigger that is true when the robot is enabled in teleop mode.
+   *
+   * @return A trigger that is true when the robot is enabled in teleop mode.
+   */
+  static Trigger Teleop();
+
+  /**
+   * Returns a trigger that is true when the robot is disabled.
+   *
+   * @return A trigger that is true when the robot is disabled.
+   */
+  static Trigger Disabled();
+
+  /**
+   * Returns a trigger that is true when the robot is enabled in test mode.
+   *
+   * @return A trigger that is true when the robot is enabled in test mode.
+   */
+  static Trigger Test();
+};
+
+}  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
index 533e0a6..4438aac 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -268,6 +268,12 @@
                    frc::Debouncer::DebounceType type =
                        frc::Debouncer::DebounceType::kRising);
 
+  /**
+   * Returns the current state of this trigger.
+   * @return A bool representing the current state of the trigger.
+   */
+  bool Get() const;
+
  private:
   frc::EventLoop* m_loop;
   std::function<bool()> m_condition;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
new file mode 100644
index 0000000..021059f
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
@@ -0,0 +1,200 @@
+// 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 <functional>
+#include <string>
+#include <string_view>
+#include <utility>
+
+#include <frc/Timer.h>
+#include <frc/sysid/SysIdRoutineLog.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Subsystem.h"
+
+namespace frc2::sysid {
+
+using ramp_rate_t = units::unit_t<
+    units::compound_unit<units::volt, units::inverse<units::second>>>;
+
+/** Hardware-independent configuration for a SysId test routine. */
+class Config {
+ public:
+  /// The voltage ramp rate used for quasistatic test routines.
+  ramp_rate_t m_rampRate{1_V / 1_s};
+
+  /// The step voltage output used for dynamic test routines.
+  units::volt_t m_stepVoltage{7_V};
+
+  /// Safety timeout for the test routine commands.
+  units::second_t m_timeout{10_s};
+
+  /// Optional handle for recording test state in a third-party logging
+  /// solution.
+  std::function<void(frc::sysid::State)> m_recordState;
+
+  /**
+   * Create a new configuration for a SysId test routine.
+   *
+   * @param rampRate The voltage ramp rate used for quasistatic test routines.
+   *   Defaults to 1 volt per second if left null.
+   * @param stepVoltage The step voltage output used for dynamic test routines.
+   *   Defaults to 7 volts if left null.
+   * @param timeout Safety timeout for the test routine commands. Defaults to 10
+   *   seconds if left null.
+   * @param recordState Optional handle for recording test state in a
+   *   third-party logging solution. If provided, the test routine state will be
+   *   passed to this callback instead of logged in WPILog.
+   */
+  Config(std::optional<ramp_rate_t> rampRate,
+         std::optional<units::volt_t> stepVoltage,
+         std::optional<units::second_t> timeout,
+         std::optional<std::function<void(frc::sysid::State)>> recordState) {
+    if (rampRate) {
+      m_rampRate = rampRate.value();
+    }
+    if (stepVoltage) {
+      m_stepVoltage = stepVoltage.value();
+    }
+    if (timeout) {
+      m_timeout = timeout.value();
+    }
+    if (recordState) {
+      m_recordState = recordState.value();
+    }
+  }
+};
+
+class Mechanism {
+ public:
+  /// Sends the SysId-specified drive signal to the mechanism motors during test
+  /// routines.
+  std::function<void(units::volt_t)> m_drive;
+
+  /// Returns measured data (voltages, positions, velocities) of the mechanism
+  /// motors during test routines.
+  std::function<void(frc::sysid::SysIdRoutineLog*)> m_log;
+
+  /// The subsystem containing the motor(s) that is (or are) being
+  /// characterized.
+  frc2::Subsystem* m_subsystem;
+
+  /// The name of the mechanism being tested. Will be appended to the log entry
+  /// title for the routine's test state, e.g. "sysid-test-state-mechanism".
+  std::string m_name;
+
+  /**
+   * Create a new mechanism specification for a SysId routine.
+   *
+   * @param drive Sends the SysId-specified drive signal to the mechanism motors
+   * during test routines.
+   * @param log Returns measured data of the mechanism motors during test
+   * routines. To return data, call `Motor(string motorName)` on the supplied
+   * `SysIdRoutineLog` instance, and then call one or more of the chainable
+   * logging handles (e.g. `voltage`) on the returned `MotorLog`.  Multiple
+   * motors can be logged in a single callback by calling `Motor` multiple
+   * times.
+   * @param subsystem The subsystem containing the motor(s) that is (or are)
+   *   being characterized. Will be declared as a requirement for the returned
+   *   test commands.
+   * @param name The name of the mechanism being tested. Will be appended to the
+   *   log entry * title for the routine's test state, e.g.
+   *   "sysid-test-state-mechanism". Defaults to the name of the subsystem if
+   *   left null.
+   */
+  Mechanism(std::function<void(units::volt_t)> drive,
+            std::function<void(frc::sysid::SysIdRoutineLog*)> log,
+            frc2::Subsystem* subsystem, std::string_view name)
+      : m_drive{std::move(drive)},
+        m_log{std::move(log)},
+        m_subsystem{subsystem},
+        m_name{name} {}
+
+  /**
+   * Create a new mechanism specification for a SysId routine. Defaults the
+   * mechanism name to the subsystem name.
+   *
+   * @param drive Sends the SysId-specified drive signal to the mechanism motors
+   * during test routines.
+   * @param log Returns measured data of the mechanism motors during test
+   * routines. To return data, call `Motor(string motorName)` on the supplied
+   * `SysIdRoutineLog` instance, and then call one or more of the chainable
+   * logging handles (e.g. `voltage`) on the returned `MotorLog`.  Multiple
+   * motors can be logged in a single callback by calling `Motor` multiple
+   * times.
+   * @param subsystem The subsystem containing the motor(s) that is (or are)
+   *   being characterized. Will be declared as a requirement for the returned
+   *   test commands. The subsystem's `name` will be appended to the log entry
+   *   title for the routine's test state, e.g. "sysid-test-state-subsystem".
+   */
+  Mechanism(std::function<void(units::volt_t)> drive,
+            std::function<void(frc::sysid::SysIdRoutineLog*)> log,
+            frc2::Subsystem* subsystem)
+      : m_drive{std::move(drive)},
+        m_log{std::move(log)},
+        m_subsystem{subsystem},
+        m_name{m_subsystem->GetName()} {}
+};
+
+/**
+ * Motor direction for a SysId test.
+ */
+enum Direction {
+  /// Forward.
+  kForward,
+  /// Reverse.
+  kReverse
+};
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have
+ * multiple motors.
+ *
+ * A single subsystem may have multiple mechanisms, but mechanisms should not
+ * share test routines. Each complete test of a mechanism should have its own
+ * SysIdRoutine instance, since the log name of the recorded data is determined
+ * by the mechanism name.
+ *
+ * The test state (e.g. "quasistatic-forward") is logged once per iteration
+ * during test execution, and once with state "none" when a test ends. Motor
+ * frames are logged every iteration during test execution.
+ *
+ * Timestamps are not coordinated across data, so motor frames and test state
+ * tags may be recorded on different log frames. Because frame alignment is not
+ * guaranteed, SysId parses the log by using the test state flag to determine
+ * the timestamp range for each section of the test, and then extracts the motor
+ * frames within the valid timestamp ranges. If a given test was run multiple
+ * times in a single logfile, the user will need to select which of the tests to
+ * use for the fit in the analysis tool.
+ */
+class SysIdRoutine : public frc::sysid::SysIdRoutineLog {
+ public:
+  /**
+   * Create a new SysId characterization routine.
+   *
+   * @param config Hardware-independent parameters for the SysId routine.
+   * @param mechanism Hardware interface for the SysId routine.
+   */
+  SysIdRoutine(Config config, Mechanism mechanism)
+      : SysIdRoutineLog(mechanism.m_subsystem->GetName()),
+        m_config(config),
+        m_mechanism(mechanism),
+        m_recordState(config.m_recordState ? config.m_recordState
+                                           : [this](frc::sysid::State state) {
+                                               this->RecordState(state);
+                                             }) {}
+
+  frc2::CommandPtr Quasistatic(Direction direction);
+  frc2::CommandPtr Dynamic(Direction direction);
+
+ private:
+  Config m_config;
+  Mechanism m_mechanism;
+  units::volt_t m_outputVolts{0};
+  std::function<void(frc::sysid::State)> m_recordState;
+  frc::Timer timer;
+};
+}  // namespace frc2::sysid
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
deleted file mode 100644
index ba4a77b..0000000
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
+++ /dev/null
@@ -1,47 +0,0 @@
-// 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.
-
-package edu.wpi.first.wpilibj2.command;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-
-import org.junit.jupiter.api.Test;
-
-class CommandGroupErrorTest extends CommandTestBase {
-  @Test
-  void commandInMultipleGroupsTest() {
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-
-    new ParallelCommandGroup(command1, command2);
-    assertThrows(
-        IllegalArgumentException.class, () -> new ParallelCommandGroup(command1, command2));
-  }
-
-  @Test
-  void commandInGroupExternallyScheduledTest() {
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-
-    new ParallelCommandGroup(command1, command2);
-
-    assertThrows(
-        IllegalArgumentException.class, () -> CommandScheduler.getInstance().schedule(command1));
-  }
-
-  @Test
-  void redecoratedCommandErrorTest() {
-    Command command = new InstantCommand();
-
-    assertDoesNotThrow(() -> command.withTimeout(10).until(() -> false));
-    assertThrows(IllegalArgumentException.class, () -> command.withTimeout(10));
-    CommandScheduler.getInstance().removeComposedCommand(command);
-    assertDoesNotThrow(() -> command.withTimeout(10));
-  }
-}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java
index 2b79550..9fafc59 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj2.command;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.params.provider.Arguments.arguments;
 
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -13,11 +14,11 @@
 import org.junit.jupiter.params.provider.Arguments;
 import org.junit.jupiter.params.provider.MethodSource;
 
-interface MultiCompositionTestBase<T extends Command> extends SingleCompositionTestBase<T> {
-  T compose(Command... members);
+abstract class MultiCompositionTestBase<T extends Command> extends SingleCompositionTestBase<T> {
+  abstract T compose(Command... members);
 
   @Override
-  default T composeSingle(Command member) {
+  T composeSingle(Command member) {
     return compose(member);
   }
 
@@ -63,7 +64,7 @@
 
   @MethodSource
   @ParameterizedTest(name = "interruptible[{index}]: {0}")
-  default void interruptible(
+  void interruptible(
       @SuppressWarnings("unused") String name,
       InterruptionBehavior expected,
       Command command1,
@@ -103,7 +104,7 @@
 
   @MethodSource
   @ParameterizedTest(name = "runsWhenDisabled[{index}]: {0}")
-  default void runsWhenDisabled(
+  void runsWhenDisabled(
       @SuppressWarnings("unused") String name,
       boolean expected,
       Command command1,
@@ -112,4 +113,19 @@
     var command = compose(command1, command2, command3);
     assertEquals(expected, command.runsWhenDisabled());
   }
+
+  static Stream<Arguments> composeDuplicates() {
+    Command a = new InstantCommand(() -> {});
+    Command b = new InstantCommand(() -> {});
+    return Stream.of(
+        arguments("AA", new Command[] {a, a}),
+        arguments("ABA", new Command[] {a, b, a}),
+        arguments("BAA", new Command[] {b, a, a}));
+  }
+
+  @MethodSource
+  @ParameterizedTest(name = "composeDuplicates[{index}]: {0}")
+  void composeDuplicates(@SuppressWarnings("unused") String name, Command[] commands) {
+    assertThrows(IllegalArgumentException.class, () -> compose(commands));
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
index 7dc9110..a837baa 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
@@ -14,8 +14,7 @@
 
 import org.junit.jupiter.api.Test;
 
-class ParallelCommandGroupTest extends CommandTestBase
-    implements MultiCompositionTestBase<ParallelCommandGroup> {
+class ParallelCommandGroupTest extends MultiCompositionTestBase<ParallelCommandGroup> {
   @Test
   void parallelGroupScheduleTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
index 6fa644b..206d801 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -14,8 +15,7 @@
 import java.util.Arrays;
 import org.junit.jupiter.api.Test;
 
-class ParallelDeadlineGroupTest extends CommandTestBase
-    implements MultiCompositionTestBase<ParallelDeadlineGroup> {
+class ParallelDeadlineGroupTest extends MultiCompositionTestBase<ParallelDeadlineGroup> {
   @Test
   void parallelDeadlineScheduleTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -125,6 +125,21 @@
         IllegalArgumentException.class, () -> new ParallelDeadlineGroup(command1, command2));
   }
 
+  @Test
+  void parallelDeadlineSetDeadlineToDeadlineTest() {
+    Command a = new InstantCommand(() -> {});
+    ParallelDeadlineGroup group = new ParallelDeadlineGroup(a);
+    assertDoesNotThrow(() -> group.setDeadline(a));
+  }
+
+  @Test
+  void parallelDeadlineSetDeadlineDuplicateTest() {
+    Command a = new InstantCommand(() -> {});
+    Command b = new InstantCommand(() -> {});
+    ParallelDeadlineGroup group = new ParallelDeadlineGroup(a, b);
+    assertThrows(IllegalArgumentException.class, () -> group.setDeadline(b));
+  }
+
   @Override
   public ParallelDeadlineGroup compose(Command... members) {
     return new ParallelDeadlineGroup(members[0], Arrays.copyOfRange(members, 1, members.length));
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
index 6752496..ea780c6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
@@ -16,8 +16,7 @@
 
 import org.junit.jupiter.api.Test;
 
-class ParallelRaceGroupTest extends CommandTestBase
-    implements MultiCompositionTestBase<ParallelRaceGroup> {
+class ParallelRaceGroupTest extends MultiCompositionTestBase<ParallelRaceGroup> {
   @Test
   void parallelRaceScheduleTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -153,7 +152,7 @@
       scheduler.run();
       command2Holder.setFinished(true);
       // at this point the sequential group should be done
-      assertDoesNotThrow(() -> scheduler.run());
+      assertDoesNotThrow(scheduler::run);
       assertFalse(scheduler.isScheduled(group2));
     }
   }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java
index 53abdd8..2aaecc6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java
@@ -10,8 +10,7 @@
 import java.util.concurrent.atomic.AtomicInteger;
 import org.junit.jupiter.api.Test;
 
-class RepeatCommandTest extends CommandTestBase
-    implements SingleCompositionTestBase<RepeatCommand> {
+class RepeatCommandTest extends SingleCompositionTestBase<RepeatCommand> {
   @Test
   void callsMethodsCorrectly() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
index 5e8fd39..a253740 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
@@ -288,7 +288,7 @@
       scheduler.schedule(cCancelsD);
       scheduler.schedule(dCancelsAll);
 
-      assertDoesNotThrow(() -> scheduler.run());
+      assertDoesNotThrow(scheduler::run);
       assertEquals(4, counter.get());
       assertFalse(scheduler.isScheduled(aCancelsB));
       assertFalse(scheduler.isScheduled(bCancelsC));
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
index 0ce3b79..e364468 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
@@ -13,8 +13,7 @@
 import java.util.Map;
 import org.junit.jupiter.api.Test;
 
-class SelectCommandTest extends CommandTestBase
-    implements MultiCompositionTestBase<SelectCommand<Integer>> {
+class SelectCommandTest extends MultiCompositionTestBase<SelectCommand<Integer>> {
   @Test
   void selectCommandTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
index ff578f4..abbc490 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
@@ -12,8 +12,7 @@
 
 import org.junit.jupiter.api.Test;
 
-class SequentialCommandGroupTest extends CommandTestBase
-    implements MultiCompositionTestBase<SequentialCommandGroup> {
+class SequentialCommandGroupTest extends MultiCompositionTestBase<SequentialCommandGroup> {
   @Test
   void sequentialGroupScheduleTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java
index 4e54e3e..a063a1d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java
@@ -5,17 +5,19 @@
 package edu.wpi.first.wpilibj2.command;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
+import org.junit.jupiter.api.Test;
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.EnumSource;
 import org.junit.jupiter.params.provider.ValueSource;
 
-public interface SingleCompositionTestBase<T extends Command> {
-  T composeSingle(Command member);
+public abstract class SingleCompositionTestBase<T extends Command> extends CommandTestBase {
+  abstract T composeSingle(Command member);
 
   @EnumSource(Command.InterruptionBehavior.class)
   @ParameterizedTest
-  default void interruptible(Command.InterruptionBehavior interruptionBehavior) {
+  void interruptible(Command.InterruptionBehavior interruptionBehavior) {
     var command =
         composeSingle(
             new WaitUntilCommand(() -> false).withInterruptBehavior(interruptionBehavior));
@@ -24,9 +26,38 @@
 
   @ValueSource(booleans = {true, false})
   @ParameterizedTest
-  default void runWhenDisabled(boolean runsWhenDisabled) {
+  void runWhenDisabled(boolean runsWhenDisabled) {
     var command =
         composeSingle(new WaitUntilCommand(() -> false).ignoringDisable(runsWhenDisabled));
     assertEquals(runsWhenDisabled, command.runsWhenDisabled());
   }
+
+  @Test
+  void commandInOtherCompositionTest() {
+    var command = new InstantCommand();
+    new WrapperCommand(command) {};
+    assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+  }
+
+  @Test
+  void commandInMultipleCompositionsTest() {
+    var command = new InstantCommand();
+    composeSingle(command);
+    assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+  }
+
+  @Test
+  void composeThenScheduleTest() {
+    var command = new InstantCommand();
+    composeSingle(command);
+    assertThrows(
+        IllegalArgumentException.class, () -> CommandScheduler.getInstance().schedule(command));
+  }
+
+  @Test
+  void scheduleThenComposeTest() {
+    var command = new RunCommand(() -> {});
+    CommandScheduler.getInstance().schedule(command);
+    assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java
new file mode 100644
index 0000000..b2b41c4
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java
@@ -0,0 +1,58 @@
+// 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.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+import org.junit.jupiter.api.Test;
+
+class RobotModeTriggersTest extends CommandTestBase {
+  @Test
+  void autonomousTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(true);
+    DriverStationSim.setTest(false);
+    DriverStationSim.setEnabled(true);
+    DriverStation.refreshData();
+    Trigger auto = RobotModeTriggers.autonomous();
+    assertTrue(auto.getAsBoolean());
+  }
+
+  @Test
+  void teleopTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.setEnabled(true);
+    DriverStation.refreshData();
+    Trigger teleop = RobotModeTriggers.teleop();
+    assertTrue(teleop.getAsBoolean());
+  }
+
+  @Test
+  void testModeTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(true);
+    DriverStationSim.setEnabled(true);
+    DriverStation.refreshData();
+    Trigger test = RobotModeTriggers.test();
+    assertTrue(test.getAsBoolean());
+  }
+
+  @Test
+  void disabledTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.setEnabled(false);
+    DriverStation.refreshData();
+    Trigger disabled = RobotModeTriggers.disabled();
+    assertTrue(disabled.getAsBoolean());
+  }
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java
new file mode 100644
index 0000000..3d885b2
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java
@@ -0,0 +1,144 @@
+// 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.
+
+package edu.wpi.first.wpilibj2.command.sysid;
+
+import static edu.wpi.first.units.Units.Volts;
+import static org.mockito.ArgumentMatchers.any;
+import static org.mockito.Mockito.atLeastOnce;
+import static org.mockito.Mockito.clearInvocations;
+import static org.mockito.Mockito.inOrder;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SysIdRoutineTest {
+  interface Mechanism extends Subsystem {
+    void recordState(SysIdRoutineLog.State state);
+
+    void drive(Measure<Voltage> voltage);
+
+    void log(SysIdRoutineLog log);
+  }
+
+  Mechanism m_mechanism;
+  SysIdRoutine m_sysidRoutine;
+  Command m_quasistaticForward;
+  Command m_quasistaticReverse;
+  Command m_dynamicForward;
+  Command m_dynamicReverse;
+
+  void runCommand(Command command) {
+    command.initialize();
+    command.execute();
+    command.execute();
+    SimHooks.stepTiming(1);
+    command.execute();
+    command.end(true);
+  }
+
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    m_mechanism = mock(Mechanism.class);
+    m_sysidRoutine =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(null, null, null, m_mechanism::recordState),
+            new SysIdRoutine.Mechanism(m_mechanism::drive, m_mechanism::log, new Subsystem() {}));
+    m_quasistaticForward = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kForward);
+    m_quasistaticReverse = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kReverse);
+    m_dynamicForward = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kForward);
+    m_dynamicReverse = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kReverse);
+  }
+
+  @AfterEach
+  void cleanupAll() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  void recordStateBookendsMotorLogging() {
+    runCommand(m_quasistaticForward);
+
+    var orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+    orderCheck.verify(m_mechanism).drive(any());
+    orderCheck.verify(m_mechanism).log(any());
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+    orderCheck.verifyNoMoreInteractions();
+
+    clearInvocations(m_mechanism);
+    orderCheck = inOrder(m_mechanism);
+    runCommand(m_dynamicForward);
+
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kDynamicForward);
+    orderCheck.verify(m_mechanism).drive(any());
+    orderCheck.verify(m_mechanism).log(any());
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+    orderCheck.verifyNoMoreInteractions();
+  }
+
+  @Test
+  void testsDeclareCorrectState() {
+    runCommand(m_quasistaticForward);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+
+    runCommand(m_quasistaticReverse);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticReverse);
+
+    runCommand(m_dynamicForward);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicForward);
+
+    runCommand(m_dynamicReverse);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicReverse);
+  }
+
+  @Test
+  void testsOutputCorrectVoltage() {
+    runCommand(m_quasistaticForward);
+    var orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(1));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+
+    clearInvocations(m_mechanism);
+    runCommand(m_quasistaticReverse);
+    orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-1));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+
+    clearInvocations(m_mechanism);
+    runCommand(m_dynamicForward);
+    orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(7));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+
+    clearInvocations(m_mechanism);
+    runCommand(m_dynamicForward);
+    orderCheck = inOrder(m_mechanism);
+
+    runCommand(m_dynamicReverse);
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-7));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+  }
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp
index 6c57c7f..75ec040 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp
@@ -34,3 +34,7 @@
 
   EXPECT_EQ(1, counter);
 }
+
+TEST_F(CommandPtrTest, NullInitialization) {
+  EXPECT_THROW(CommandPtr{std::unique_ptr<Command>{}}, frc::RuntimeError);
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
index 28d498c..ff5384c 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
@@ -20,7 +20,7 @@
   MockCommand* command1 = command1Holder.get();
   MockCommand* command2 = command2Holder.get();
 
-  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+  ParallelCommandGroup group(make_vector<std::unique_ptr<Command>>(
       std::move(command1Holder), std::move(command2Holder)));
 
   EXPECT_CALL(*command1, Initialize());
@@ -50,7 +50,7 @@
   MockCommand* command1 = command1Holder.get();
   MockCommand* command2 = command2Holder.get();
 
-  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+  ParallelCommandGroup group(make_vector<std::unique_ptr<Command>>(
       std::move(command1Holder), std::move(command2Holder)));
 
   EXPECT_CALL(*command1, Initialize());
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
index 4abab33..f35bd00 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
@@ -24,8 +24,8 @@
 
   ParallelDeadlineGroup group(
       std::move(command1Holder),
-      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
-                                                 std::move(command3Holder)));
+      make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                            std::move(command3Holder)));
 
   EXPECT_CALL(*command1, Initialize());
   EXPECT_CALL(*command1, Execute()).Times(2);
@@ -64,8 +64,8 @@
 
   ParallelDeadlineGroup group(
       std::move(command1Holder),
-      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
-                                                 std::move(command3Holder)));
+      make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                            std::move(command3Holder)));
 
   EXPECT_CALL(*command1, Initialize());
   EXPECT_CALL(*command1, Execute()).Times(1);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
index 3df2147..be18fc3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
@@ -23,7 +23,7 @@
   MockCommand* command2 = command2Holder.get();
   MockCommand* command3 = command3Holder.get();
 
-  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+  ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
       std::move(command1Holder), std::move(command2Holder),
       std::move(command3Holder))};
 
@@ -59,7 +59,7 @@
   MockCommand* command2 = command2Holder.get();
   MockCommand* command3 = command3Holder.get();
 
-  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+  ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
       std::move(command1Holder), std::move(command2Holder),
       std::move(command3Holder))};
 
@@ -164,7 +164,7 @@
   MockCommand* command2 = command2Holder.get();
   MockCommand* command3 = command3Holder.get();
 
-  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+  ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
       std::move(command1Holder), std::move(command2Holder),
       std::move(command3Holder))};
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
index 0048b24..e128313 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
@@ -22,7 +22,7 @@
   MockCommand* command2 = command2Holder.get();
   MockCommand* command3 = command3Holder.get();
 
-  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+  SequentialCommandGroup group{make_vector<std::unique_ptr<Command>>(
       std::move(command1Holder), std::move(command2Holder),
       std::move(command3Holder))};
 
@@ -61,7 +61,7 @@
   MockCommand* command2 = command2Holder.get();
   MockCommand* command3 = command3Holder.get();
 
-  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+  SequentialCommandGroup group{make_vector<std::unique_ptr<Command>>(
       std::move(command1Holder), std::move(command2Holder),
       std::move(command3Holder))};
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp
new file mode 100644
index 0000000..cb9e966
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp
@@ -0,0 +1,54 @@
+// 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 <frc/DriverStation.h>
+#include <frc/simulation/DriverStationSim.h>
+
+#include "../CommandTestBase.h"
+#include "frc2/command/button/RobotModeTriggers.h"
+#include "frc2/command/button/Trigger.h"
+
+using namespace frc2;
+using namespace frc::sim;
+class RobotModeTriggersTest : public CommandTestBase {};
+
+TEST(RobotModeTriggersTest, Autonomous) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(true);
+  DriverStationSim::SetTest(false);
+  DriverStationSim::SetEnabled(true);
+  frc::DriverStation::RefreshData();
+  Trigger autonomous = RobotModeTriggers::Autonomous();
+  EXPECT_TRUE(autonomous.Get());
+}
+
+TEST(RobotModeTriggersTest, Teleop) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(false);
+  DriverStationSim::SetTest(false);
+  DriverStationSim::SetEnabled(true);
+  frc::DriverStation::RefreshData();
+  Trigger teleop = RobotModeTriggers::Teleop();
+  EXPECT_TRUE(teleop.Get());
+}
+
+TEST(RobotModeTriggersTest, Disabled) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(false);
+  DriverStationSim::SetTest(false);
+  DriverStationSim::SetEnabled(false);
+  frc::DriverStation::RefreshData();
+  Trigger disabled = RobotModeTriggers::Disabled();
+  EXPECT_TRUE(disabled.Get());
+}
+
+TEST(RobotModeTriggersTest, TestMode) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(false);
+  DriverStationSim::SetTest(true);
+  DriverStationSim::SetEnabled(true);
+  frc::DriverStation::RefreshData();
+  Trigger test = RobotModeTriggers::Test();
+  EXPECT_TRUE(test.Get());
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
index 996ddba..295eba7 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
@@ -8,59 +8,27 @@
 #include <utility>
 #include <vector>
 
-namespace tcb {
-
-namespace detail {
-
-template <typename T, typename...>
-struct vec_type_helper {
-  using type = T;
-};
-
-template <typename... Args>
-struct vec_type_helper<void, Args...> {
-  using type = typename std::common_type_t<Args...>;
-};
-
-template <typename T, typename... Args>
-using vec_type_helper_t = typename vec_type_helper<T, Args...>::type;
-
-template <typename, typename...>
-struct all_constructible_and_convertible : std::true_type {};
-
-template <typename T, typename First, typename... Rest>
-struct all_constructible_and_convertible<T, First, Rest...>
-    : std::conditional_t<
-          std::is_constructible_v<T, First> && std::is_convertible_v<First, T>,
-          all_constructible_and_convertible<T, Rest...>, std::false_type> {};
-
-template <typename T, typename First, typename... Rest>
-inline constexpr bool all_constructible_and_convertible_v =
-    all_constructible_and_convertible<T, First, Rest...>::value;
-
-template <typename T, typename... Args>
-  requires(!std::is_trivially_copyable_v<T>)
-std::vector<T> make_vector_impl(Args&&... args) {
-  std::vector<T> vec;
-  vec.reserve(sizeof...(Args));
-  using arr_t = int[];
-  (void)arr_t{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
-  return vec;
-}
-
-template <typename T, typename... Args>
-  requires std::is_trivially_copyable_v<T>
-std::vector<T> make_vector_impl(Args&&... args) {
-  return std::vector<T>{std::forward<Args>(args)...};
-}
-
-}  // namespace detail
+namespace frc2 {
 
 template <typename T = void, typename... Args,
-          typename V = detail::vec_type_helper_t<T, Args...>>
-  requires detail::all_constructible_and_convertible_v<V, Args...>
-std::vector<V> make_vector(Args&&... args) {
-  return detail::make_vector_impl<V>(std::forward<Args>(args)...);
+          typename CommonType = std::conditional_t<
+              std::same_as<T, void>, std::common_type_t<Args...>, T>>
+  requires((std::is_constructible_v<T, Args> &&
+            std::is_convertible_v<Args, T>) &&
+           ...)
+std::vector<CommonType> make_vector(Args&&... args) {
+  if constexpr (std::is_trivially_copyable_v<CommonType>) {
+    return std::vector<CommonType>{std::forward<Args>(args)...};
+  } else {
+    std::vector<CommonType> vec;
+    vec.reserve(sizeof...(Args));
+
+    using arr_t = int[];
+    [[maybe_unused]] arr_t arr{
+        0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
+
+    return vec;
+  }
 }
 
-}  // namespace tcb
+}  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp
new file mode 100644
index 0000000..5e4a582
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp
@@ -0,0 +1,170 @@
+// 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 <frc2/command/Subsystem.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include <numbers>
+
+#include <frc/Timer.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/math.h>
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+enum StateTest {
+  Invalid,
+  InRecordStateQf,
+  InRecordStateQr,
+  InRecordStateDf,
+  InRecordStateDr,
+  InDrive,
+  InLog,
+  DoneWithRecordState
+};
+
+class SysIdRoutineTest : public ::testing::Test {
+ protected:
+  std::vector<StateTest> currentStateList{};
+  std::vector<units::volt_t> sentVoltages{};
+  frc2::Subsystem m_subsystem{};
+  frc2::sysid::SysIdRoutine m_sysidRoutine{
+      frc2::sysid::Config{
+          std::nullopt, std::nullopt, std::nullopt,
+          [this](frc::sysid::State state) {
+            switch (state) {
+              case frc::sysid::State::kQuasistaticForward:
+                currentStateList.emplace_back(StateTest::InRecordStateQf);
+                break;
+              case frc::sysid::State::kQuasistaticReverse:
+                currentStateList.emplace_back(StateTest::InRecordStateQr);
+                break;
+              case frc::sysid::State::kDynamicForward:
+                currentStateList.emplace_back(StateTest::InRecordStateDf);
+                break;
+              case frc::sysid::State::kDynamicReverse:
+                currentStateList.emplace_back(StateTest::InRecordStateDr);
+                break;
+              case frc::sysid::State::kNone:
+                currentStateList.emplace_back(StateTest::DoneWithRecordState);
+                break;
+            }
+          }},
+      frc2::sysid::Mechanism{
+          [this](units::volt_t driveVoltage) {
+            sentVoltages.emplace_back(driveVoltage);
+            currentStateList.emplace_back(StateTest::InDrive);
+          },
+          [this](frc::sysid::SysIdRoutineLog* log) {
+            currentStateList.emplace_back(StateTest::InLog);
+            log->Motor("Mock Motor").position(0_m).velocity(0_mps).voltage(0_V);
+          },
+          &m_subsystem}};
+  frc2::CommandPtr m_quasistaticForward{
+      m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+  frc2::CommandPtr m_quasistaticReverse{
+      m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+  frc2::CommandPtr m_dynamicForward{
+      m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+  frc2::CommandPtr m_dynamicReverse{
+      m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+
+  void RunCommand(frc2::CommandPtr command) {
+    command.get()->Initialize();
+    command.get()->Execute();
+    frc::sim::StepTiming(1_s);
+    command.get()->Execute();
+    command.get()->End(true);
+  }
+
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    frc2::CommandPtr m_quasistaticForward{
+        m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+    frc2::CommandPtr m_quasistaticReverse{
+        m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+    frc2::CommandPtr m_dynamicForward{
+        m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+    frc2::CommandPtr m_dynamicReverse{
+        m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+  }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+TEST_F(SysIdRoutineTest, RecordStateBookendsMotorLogging) {
+  RunCommand(std::move(m_quasistaticForward));
+  std::vector<StateTest> expectedOrder{
+      StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateQf,
+      StateTest::InDrive, StateTest::DoneWithRecordState};
+  EXPECT_TRUE(expectedOrder == currentStateList);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  expectedOrder = std::vector<StateTest>{
+      StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateDf,
+      StateTest::InDrive, StateTest::DoneWithRecordState};
+  RunCommand(std::move(m_dynamicForward));
+  EXPECT_TRUE(expectedOrder == currentStateList);
+  currentStateList.clear();
+  sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, DeclareCorrectState) {
+  RunCommand(std::move(m_quasistaticForward));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateQf) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_quasistaticReverse));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateQr) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicForward));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateDf) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicReverse));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateDr) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, OutputCorrectVoltage) {
+  RunCommand(std::move(m_quasistaticForward));
+  std::vector<units::volt_t> expectedVoltages{1_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_quasistaticReverse));
+  expectedVoltages = std::vector<units::volt_t>{-1_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicForward));
+  expectedVoltages = std::vector<units::volt_t>{7_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicReverse));
+  expectedVoltages = std::vector<units::volt_t>{-7_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/wpilibNewCommands-config.cmake.in b/third_party/allwpilib/wpilibNewCommands/wpilibNewCommands-config.cmake.in
deleted file mode 100644
index 8a8d8d8..0000000
--- a/third_party/allwpilib/wpilibNewCommands/wpilibNewCommands-config.cmake.in
+++ /dev/null
@@ -1,11 +0,0 @@
-include(CMakeFindDependencyMacro)
- @WPIUTIL_DEP_REPLACE@
- @NTCORE_DEP_REPLACE@
- @CSCORE_DEP_REPLACE@
- @CAMERASERVER_DEP_REPLACE@
- @HAL_DEP_REPLACE@
- @WPILIBC_DEP_REPLACE@
- @WPIMATH_DEP_REPLACE@
-
- @FILENAME_DEP_REPLACE@
- include(${SELF_DIR}/wpilibNewCommands.cmake)
diff --git a/third_party/allwpilib/wpilibNewCommands/wpilibnewcommands-config.cmake.in b/third_party/allwpilib/wpilibNewCommands/wpilibnewcommands-config.cmake.in
new file mode 100644
index 0000000..b26b9a7
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/wpilibnewcommands-config.cmake.in
@@ -0,0 +1,14 @@
+include(CMakeFindDependencyMacro)
+@WPIUTIL_DEP_REPLACE@
+@NTCORE_DEP_REPLACE@
+@CSCORE_DEP_REPLACE@
+@CAMERASERVER_DEP_REPLACE@
+@HAL_DEP_REPLACE@
+@WPILIBC_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+
+@FILENAME_DEP_REPLACE@
+include(${SELF_DIR}/wpilibnewcommands.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/wpilibNewCommands_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/wpilibc/CMakeLists.txt b/third_party/allwpilib/wpilibc/CMakeLists.txt
index ed85834..3757d5f 100644
--- a/third_party/allwpilib/wpilibc/CMakeLists.txt
+++ b/third_party/allwpilib/wpilibc/CMakeLists.txt
@@ -5,51 +5,52 @@
 
 configure_file(src/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
 
-file(GLOB_RECURSE
-    wpilibc_native_src src/main/native/cpp/*.cpp src/main/native/cppcs/*.cpp)
+file(GLOB_RECURSE wpilibc_native_src src/main/native/cpp/*.cpp src/main/native/cppcs/*.cpp)
 
 add_library(wpilibc ${wpilibc_native_src} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
 set_target_properties(wpilibc PROPERTIES DEBUG_POSTFIX "d")
 
-target_include_directories(wpilibc PUBLIC
-                $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpilibc>)
+target_include_directories(
+    wpilibc
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpilibc>
+)
 wpilib_target_warnings(wpilibc)
 
-if (WITH_CSCORE)
-    find_package( OpenCV )
+if(WITH_CSCORE)
+    find_package(OpenCV)
     target_link_libraries(wpilibc PUBLIC cameraserver cscore ${OpenCV_LIBS})
 else()
     target_compile_definitions(wpilibc PRIVATE DYNAMIC_CAMERA_SERVER)
     # Add just the camera server include directory
-    target_include_directories(wpilibc PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../cameraserver/src/main/native/include)
+    target_include_directories(
+        wpilibc
+        PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../cameraserver/src/main/native/include
+    )
 endif()
 
 target_link_libraries(wpilibc PUBLIC hal ntcore wpimath wpiutil)
 
-
 set_property(TARGET wpilibc PROPERTY FOLDER "libraries")
 
 install(TARGETS wpilibc EXPORT wpilibc)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibc")
 
-if (WITH_FLAT_INSTALL)
-    set (wpilibc_config_dir ${wpilib_dest})
-else()
-    set (wpilibc_config_dir share/wpilibc)
-endif()
+configure_file(wpilibc-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibc-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/wpilibc-config.cmake DESTINATION share/wpilibc)
+install(EXPORT wpilibc DESTINATION share/wpilibc)
 
-configure_file(wpilibc-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibc-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/wpilibc-config.cmake DESTINATION ${wpilibc_config_dir})
-install(EXPORT wpilibc DESTINATION ${wpilibc_config_dir})
-
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(wpilibc src/test/native/cpp)
     target_include_directories(wpilibc_test PRIVATE src/test/native/include)
     target_link_libraries(wpilibc_test wpilibc gmock_main)
-    if (NOT WITH_CSCORE)
+    if(NOT WITH_CSCORE)
         target_compile_definitions(wpilibc_test PRIVATE DYNAMIC_CAMERA_SERVER)
         # Add just the camera server include directory
-        target_include_directories(wpilibc_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../cameraserver/src/main/native/include)
+        target_include_directories(
+            wpilibc_test
+            PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../cameraserver/src/main/native/include
+        )
     endif()
 endif()
diff --git a/third_party/allwpilib/wpilibc/src/generate/WPILibVersion.cpp.in b/third_party/allwpilib/wpilibc/src/generate/WPILibVersion.cpp.in
index b0a4490..cfe2441 100644
--- a/third_party/allwpilib/wpilibc/src/generate/WPILibVersion.cpp.in
+++ b/third_party/allwpilib/wpilibc/src/generate/WPILibVersion.cpp.in
@@ -1,4 +1,4 @@
-/*
+/**
  * Autogenerated file! Do not manually edit this file. This version is regenerated
  * any time the publish task is run, or when this file is deleted.
  */
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
index 14fd37f..4ce4197 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -679,8 +679,13 @@
   return compAngle;
 }
 
-int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
-  uint16_t writeValue = DecimationSetting;
+int ADIS16448_IMU::ConfigDecRate(uint16_t decimationRate) {
+  // Switches the active SPI port to standard SPI mode, writes a new value to
+  // the DECIMATE register in the IMU, and re-enables auto SPI.
+  //
+  // This function enters standard SPI mode, writes a new DECIMATE setting to
+  // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+  uint16_t writeValue = decimationRate;
   uint16_t readbackValue;
   if (!SwitchToStandardSPI()) {
     REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
@@ -688,14 +693,14 @@
   }
 
   /* Check max */
-  if (DecimationSetting > 9) {
+  if (decimationRate > 9) {
     REPORT_ERROR(
         "Attempted to write an invalid decimation value. Capping at 9");
-    DecimationSetting = 9;
+    decimationRate = 9;
   }
 
   /* Shift decimation setting to correct position and select internal sync */
-  writeValue = (DecimationSetting << 8) | 0x1;
+  writeValue = (decimationRate << 8) | 0x1;
 
   /* Apply to IMU */
   WriteRegister(SMPL_PRD, writeValue);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
index 05dac78..1c5c7a1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -63,14 +63,35 @@
  * Constructor.
  */
 ADIS16470_IMU::ADIS16470_IMU()
-    : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
+    : ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
 
-ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
+                             IMUAxis roll_axis)
+    : ADIS16470_IMU(yaw_axis, pitch_axis, roll_axis, SPI::Port::kOnboardCS0,
+                    CalibrationTime::_4s) {}
+
+ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
+                             IMUAxis roll_axis, SPI::Port port,
                              CalibrationTime cal_time)
     : m_yaw_axis(yaw_axis),
+      m_pitch_axis(pitch_axis),
+      m_roll_axis(roll_axis),
       m_spi_port(port),
       m_calibration_time(static_cast<uint16_t>(cal_time)),
       m_simDevice("Gyro:ADIS16470", port) {
+  if (yaw_axis == kYaw || yaw_axis == kPitch || yaw_axis == kRoll ||
+      pitch_axis == kYaw || pitch_axis == kPitch || pitch_axis == kRoll ||
+      roll_axis == kYaw || roll_axis == kPitch || roll_axis == kRoll) {
+    REPORT_ERROR(
+        "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or "
+        "IMUAxis.kZ as arguments.");
+    REPORT_ERROR(
+        "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)");
+    yaw_axis = kZ;
+    pitch_axis = kY;
+    roll_axis = kX;
+  }
+
   if (m_simDevice) {
     m_connected =
         m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
@@ -266,17 +287,8 @@
     m_auto_configured = true;
   }
   // Do we need to change auto SPI settings?
-  switch (m_yaw_axis) {
-    case kX:
-      m_spi->SetAutoTransmitData(m_autospi_x_packet, 2);
-      break;
-    case kY:
-      m_spi->SetAutoTransmitData(m_autospi_y_packet, 2);
-      break;
-    default:
-      m_spi->SetAutoTransmitData(m_autospi_z_packet, 2);
-      break;
-  }
+  m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2);
+
   // Configure auto stall time
   m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
   // Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is
@@ -336,31 +348,22 @@
   return 0;
 }
 
-/**
- * @brief Switches the active SPI port to standard SPI mode, writes a new value
- *to the DECIMATE register in the IMU, and re-enables auto SPI.
- *
- * @param reg Decimation value to be set.
- *
- * @return An int indicating the success or failure of writing the new DECIMATE
- *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
- *Failure
- *
- * This function enters standard SPI mode, writes a new DECIMATE setting to the
- *IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
- **/
-int ADIS16470_IMU::ConfigDecRate(uint16_t reg) {
-  uint16_t m_reg = reg;
+int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) {
+  // Switches the active SPI port to standard SPI mode, writes a new value to
+  // the DECIMATE register in the IMU, and re-enables auto SPI.
+  //
+  // This function enters standard SPI mode, writes a new DECIMATE setting to
+  // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
   if (!SwitchToStandardSPI()) {
     REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
     return 2;
   }
-  if (m_reg > 1999) {
+  if (decimationRate > 1999) {
     REPORT_ERROR("Attempted to write an invalid decimation value.");
-    m_reg = 1999;
+    decimationRate = 1999;
   }
-  m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
-  WriteRegister(DEC_RATE, m_reg);
+  m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0);
+  WriteRegister(DEC_RATE, decimationRate);
   if (!SwitchToAutoSPI()) {
     REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
     return 2;
@@ -445,7 +448,9 @@
  **/
 void ADIS16470_IMU::Reset() {
   std::scoped_lock sync(m_mutex);
-  m_integ_angle = 0.0;
+  m_integ_angle_x = 0.0;
+  m_integ_angle_y = 0.0;
+  m_integ_angle_z = 0.0;
 }
 
 void ADIS16470_IMU::Close() {
@@ -502,7 +507,7 @@
  **/
 void ADIS16470_IMU::Acquire() {
   // Set data packet length
-  const int dataset_len = 19;  // 18 data points + timestamp
+  const int dataset_len = 27;  // 26 data points + timestamp
 
   /* Fixed buffer size */
   const int BUFFER_SIZE = 4000;
@@ -513,7 +518,9 @@
   int data_remainder = 0;
   int data_to_read = 0;
   uint32_t previous_timestamp = 0;
-  double delta_angle = 0.0;
+  double delta_angle_x = 0.0;
+  double delta_angle_y = 0.0;
+  double delta_angle_z = 0.0;
   double gyro_rate_x = 0.0;
   double gyro_rate_y = 0.0;
   double gyro_rate_z = 0.0;
@@ -562,14 +569,22 @@
         m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
         /* Get delta angle value for selected yaw axis and scale by the elapsed
          * time (based on timestamp) */
-        delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
-                      (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-        gyro_rate_x = (BuffToShort(&buffer[i + 7]) / 10.0);
-        gyro_rate_y = (BuffToShort(&buffer[i + 9]) / 10.0);
-        gyro_rate_z = (BuffToShort(&buffer[i + 11]) / 10.0);
-        accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
-        accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
-        accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
+        delta_angle_x =
+            (ToInt(&buffer[i + 3]) * delta_angle_sf) /
+            (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+        delta_angle_y =
+            (ToInt(&buffer[i + 7]) * delta_angle_sf) /
+            (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+        delta_angle_z =
+            (ToInt(&buffer[i + 11]) * delta_angle_sf) /
+            (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+
+        gyro_rate_x = (BuffToShort(&buffer[i + 15]) / 10.0);
+        gyro_rate_y = (BuffToShort(&buffer[i + 17]) / 10.0);
+        gyro_rate_z = (BuffToShort(&buffer[i + 19]) / 10.0);
+        accel_x = (BuffToShort(&buffer[i + 21]) / 800.0);
+        accel_y = (BuffToShort(&buffer[i + 23]) / 800.0);
+        accel_z = (BuffToShort(&buffer[i + 25]) / 800.0);
 
         // Convert scaled sensor data to SI units
         gyro_rate_x_si = gyro_rate_x * deg_to_rad;
@@ -611,9 +626,13 @@
           if (m_first_run) {
             /* Don't accumulate first run. previous_timestamp will be "very" old
              * and the integration will end up way off */
-            m_integ_angle = 0.0;
+            m_integ_angle_x = 0.0;
+            m_integ_angle_y = 0.0;
+            m_integ_angle_z = 0.0;
           } else {
-            m_integ_angle += delta_angle;
+            m_integ_angle_x += delta_angle_x;
+            m_integ_angle_y += delta_angle_y;
+            m_integ_angle_z += delta_angle_z;
           }
           m_gyro_rate_x = gyro_rate_x;
           m_gyro_rate_y = gyro_rate_y;
@@ -634,7 +653,9 @@
       data_remainder = 0;
       data_to_read = 0;
       previous_timestamp = 0.0;
-      delta_angle = 0.0;
+      delta_angle_x = 0.0;
+      delta_angle_y = 0.0;
+      delta_angle_z = 0.0;
       gyro_rate_x = 0.0;
       gyro_rate_y = 0.0;
       gyro_rate_z = 0.0;
@@ -696,50 +717,143 @@
   return compAngle;
 }
 
-units::degree_t ADIS16470_IMU::GetAngle() const {
-  switch (m_yaw_axis) {
+void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) {
+  switch (axis) {
+    case kYaw:
+      axis = m_yaw_axis;
+      break;
+    case kPitch:
+      axis = m_pitch_axis;
+      break;
+    case kRoll:
+      axis = m_roll_axis;
+      break;
+    default:
+      break;
+  }
+
+  switch (axis) {
+    case kX:
+      SetGyroAngleX(angle);
+      break;
+    case kY:
+      SetGyroAngleY(angle);
+      break;
+    case kZ:
+      SetGyroAngleZ(angle);
+      break;
+    default:
+      break;
+  }
+}
+
+void ADIS16470_IMU::SetGyroAngleX(units::degree_t angle) {
+  std::scoped_lock sync(m_mutex);
+  m_integ_angle_x = angle.value();
+}
+
+void ADIS16470_IMU::SetGyroAngleY(units::degree_t angle) {
+  std::scoped_lock sync(m_mutex);
+  m_integ_angle_y = angle.value();
+}
+
+void ADIS16470_IMU::SetGyroAngleZ(units::degree_t angle) {
+  std::scoped_lock sync(m_mutex);
+  m_integ_angle_z = angle.value();
+}
+
+units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const {
+  switch (axis) {
+    case kYaw:
+      axis = m_yaw_axis;
+      break;
+    case kPitch:
+      axis = m_pitch_axis;
+      break;
+    case kRoll:
+      axis = m_roll_axis;
+      break;
+    default:
+      break;
+  }
+
+  switch (axis) {
     case kX:
       if (m_simGyroAngleX) {
         return units::degree_t{m_simGyroAngleX.Get()};
       }
-      break;
+      {
+        std::scoped_lock sync(m_mutex);
+        return units::degree_t{m_integ_angle_x};
+      }
     case kY:
       if (m_simGyroAngleY) {
         return units::degree_t{m_simGyroAngleY.Get()};
       }
-      break;
+      {
+        std::scoped_lock sync(m_mutex);
+        return units::degree_t{m_integ_angle_y};
+      }
     case kZ:
       if (m_simGyroAngleZ) {
         return units::degree_t{m_simGyroAngleZ.Get()};
       }
+      {
+        std::scoped_lock sync(m_mutex);
+        return units::degree_t{m_integ_angle_z};
+      }
+    default:
       break;
   }
-  std::scoped_lock sync(m_mutex);
-  return units::degree_t{m_integ_angle};
+
+  return units::degree_t{0.0};
 }
 
-units::degrees_per_second_t ADIS16470_IMU::GetRate() const {
-  if (m_yaw_axis == kX) {
-    if (m_simGyroRateX) {
-      return units::degrees_per_second_t{m_simGyroRateX.Get()};
-    }
-    std::scoped_lock sync(m_mutex);
-    return units::degrees_per_second_t{m_gyro_rate_x};
-  } else if (m_yaw_axis == kY) {
-    if (m_simGyroRateY) {
-      return units::degrees_per_second_t{m_simGyroRateY.Get()};
-    }
-    std::scoped_lock sync(m_mutex);
-    return units::degrees_per_second_t{m_gyro_rate_y};
-  } else if (m_yaw_axis == kZ) {
-    if (m_simGyroRateZ) {
-      return units::degrees_per_second_t{m_simGyroRateZ.Get()};
-    }
-    std::scoped_lock sync(m_mutex);
-    return units::degrees_per_second_t{m_gyro_rate_z};
-  } else {
-    return 0_deg_per_s;
+units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const {
+  switch (axis) {
+    case kYaw:
+      axis = m_yaw_axis;
+      break;
+    case kPitch:
+      axis = m_pitch_axis;
+      break;
+    case kRoll:
+      axis = m_roll_axis;
+      break;
+    default:
+      break;
   }
+
+  switch (axis) {
+    case kX:
+      if (m_simGyroRateX) {
+        return units::degrees_per_second_t{m_simGyroRateX.Get()};
+      }
+      {
+        std::scoped_lock sync(m_mutex);
+        return units::degrees_per_second_t{m_gyro_rate_x};
+      }
+    case kY:
+      if (m_simGyroRateY) {
+        return units::degrees_per_second_t{m_simGyroRateY.Get()};
+      }
+      {
+        std::scoped_lock sync(m_mutex);
+        return units::degrees_per_second_t{m_gyro_rate_y};
+      }
+    case kZ:
+      if (m_simGyroRateZ) {
+        return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+      }
+      {
+        std::scoped_lock sync(m_mutex);
+        return units::degrees_per_second_t{m_gyro_rate_z};
+      }
+    default:
+      break;
+  }
+
+  return 0_deg_per_s;
 }
 
 units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
@@ -790,20 +904,12 @@
   return m_yaw_axis;
 }
 
-int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
-  if (m_yaw_axis == yaw_axis) {
-    return 1;
-  }
-  if (!SwitchToStandardSPI()) {
-    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
-    return 2;
-  }
-  m_yaw_axis = yaw_axis;
-  if (!SwitchToAutoSPI()) {
-    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
-    return 2;
-  }
-  return 0;
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetPitchAxis() const {
+  return m_pitch_axis;
+}
+
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetRollAxis() const {
+  return m_roll_axis;
 }
 
 int ADIS16470_IMU::GetPort() const {
@@ -819,5 +925,5 @@
 void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("ADIS16470 IMU");
   builder.AddDoubleProperty(
-      "Yaw Angle", [=, this] { return GetAngle().value(); }, nullptr);
+      "Yaw Angle", [=, this] { return GetAngle(kYaw).value(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp
index ce6c532..da45aa3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp
@@ -40,9 +40,9 @@
   FRC_CheckErrorStatus(status, "SetPause");
 }
 
-void DMA::SetTimedTrigger(units::second_t seconds) {
+void DMA::SetTimedTrigger(units::second_t period) {
   int32_t status = 0;
-  HAL_SetDMATimedTrigger(dmaHandle, seconds.value(), &status);
+  HAL_SetDMATimedTrigger(dmaHandle, period.value(), &status);
   FRC_CheckErrorStatus(status, "SetTimedTrigger");
 }
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DataLogManager.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DataLogManager.cpp
index afe9330..09ac077 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DataLogManager.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DataLogManager.cpp
@@ -64,20 +64,26 @@
   }
 #ifdef __FRC_ROBORIO__
   // prefer a mounted USB drive if one is accessible
-  constexpr std::string_view usbDir{"/u"};
   std::error_code ec;
-  auto s = fs::status(usbDir, ec);
+  auto s = fs::status("/u", ec);
   if (!ec && fs::is_directory(s) &&
       (s.permissions() & fs::perms::others_write) != fs::perms::none) {
-    return std::string{usbDir};
+    fs::create_directory("/u/logs", ec);
+    return "/u/logs";
   }
   if (RobotBase::GetRuntimeType() == kRoboRIO) {
     FRC_ReportError(warn::Warning,
                     "DataLogManager: Logging to RoboRIO 1 internal storage is "
                     "not recommended! Plug in a FAT32 formatted flash drive!");
   }
+  fs::create_directory("/home/lvuser/logs", ec);
+  return "/home/lvuser/logs";
+#else
+  std::string logDir = filesystem::GetOperatingDirectory() + "/logs";
+  std::error_code ec;
+  fs::create_directory(logDir, ec);
+  return logDir;
 #endif
-  return filesystem::GetOperatingDirectory();
 }
 
 static std::string MakeLogFilename(std::string_view filenameOverride) {
@@ -328,7 +334,7 @@
 void DataLogManager::Stop() {
   auto& inst = GetInstance();
   inst.owner.GetThread()->m_log.Stop();
-  inst.owner.Stop();
+  inst.owner.Join();
 }
 
 void DataLogManager::Log(std::string_view message) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp
index da26c0c..232a47f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp
@@ -4,6 +4,7 @@
 
 #include "frc/I2C.h"
 
+#include <algorithm>
 #include <utility>
 
 #include <hal/FRCUsageReporting.h>
@@ -96,7 +97,7 @@
   uint8_t deviceData[4];
   for (int i = 0, curRegisterAddress = registerAddress; i < count;
        i += 4, curRegisterAddress += 4) {
-    int toRead = count - i < 4 ? count - i : 4;
+    int toRead = std::min(count - i, 4);
     // Read the chunk of data.  Return false if the sensor does not respond.
     if (Read(curRegisterAddress, toRead, deviceData)) {
       return false;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index ee941b6..3669bce 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -8,6 +8,7 @@
 
 #include <fmt/format.h>
 #include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
 #include <networktables/NetworkTableInstance.h>
 
 #include "frc/DSControlWord.h"
@@ -97,10 +98,16 @@
 }
 
 void IterativeRobotBase::EnableLiveWindowInTest(bool testLW) {
+  static bool hasReported;
   if (IsTestEnabled()) {
     throw FRC_MakeError(err::IncompatibleMode,
                         "Can't configure test mode while in test mode!");
   }
+  if (!hasReported && testLW) {
+    HAL_Report(HALUsageReporting::kResourceType_SmartDashboard,
+               HALUsageReporting::kSmartDashboard_LiveWindow);
+    hasReported = true;
+  }
   m_lwEnabledInTest = testLW;
 }
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PS4Controller.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PS4Controller.cpp
index 5ac3420..e59e18c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PS4Controller.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PS4Controller.cpp
@@ -11,8 +11,7 @@
 using namespace frc;
 
 PS4Controller::PS4Controller(int port) : GenericHID(port) {
-  // re-enable when PS4Controller is added to Usage Reporting
-  // HAL_Report(HALUsageReporting::kResourceType_PS4Controller, port + 1);
+  HAL_Report(HALUsageReporting::kResourceType_PS4Controller, port + 1);
 }
 
 double PS4Controller::GetLeftX() const {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticHub.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticHub.cpp
index bcbbd55..9101473 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticHub.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticHub.cpp
@@ -23,15 +23,15 @@
 /** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
 units::pounds_per_square_inch_t VoltsToPSI(units::volt_t sensorVoltage,
                                            units::volt_t supplyVoltage) {
-  auto pressure = 250 * (sensorVoltage.value() / supplyVoltage.value()) - 25;
-  return units::pounds_per_square_inch_t{pressure};
+  return units::pounds_per_square_inch_t{
+      250 * (sensorVoltage.value() / supplyVoltage.value()) - 25};
 }
 
 /** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
 units::volt_t PSIToVolts(units::pounds_per_square_inch_t pressure,
                          units::volt_t supplyVoltage) {
-  auto voltage = supplyVoltage.value() * (0.004 * pressure.value() + 0.1);
-  return units::volt_t{voltage};
+  return units::volt_t{supplyVoltage.value() *
+                       (0.004 * pressure.value() + 0.1)};
 }
 
 wpi::mutex PneumaticHub::m_handleLock;
@@ -339,6 +339,46 @@
   return stickyFaults;
 }
 
+bool PneumaticHub::Faults::GetChannelFault(int channel) const {
+  switch (channel) {
+    case 0:
+      return Channel0Fault != 0;
+    case 1:
+      return Channel1Fault != 0;
+    case 2:
+      return Channel2Fault != 0;
+    case 3:
+      return Channel3Fault != 0;
+    case 4:
+      return Channel4Fault != 0;
+    case 5:
+      return Channel5Fault != 0;
+    case 6:
+      return Channel6Fault != 0;
+    case 7:
+      return Channel7Fault != 0;
+    case 8:
+      return Channel8Fault != 0;
+    case 9:
+      return Channel9Fault != 0;
+    case 10:
+      return Channel10Fault != 0;
+    case 11:
+      return Channel11Fault != 0;
+    case 12:
+      return Channel12Fault != 0;
+    case 13:
+      return Channel13Fault != 0;
+    case 14:
+      return Channel14Fault != 0;
+    case 15:
+      return Channel15Fault != 0;
+    default:
+      throw FRC_MakeError(err::ChannelIndexOutOfRange,
+                          "Pneumatics fault channel out of bounds!");
+  }
+}
+
 void PneumaticHub::ClearStickyFaults() {
   int32_t status = 0;
   HAL_ClearREVPHStickyFaults(m_handle, &status);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistribution.cpp
index ba4cb38..d645178 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistribution.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistribution.cpp
@@ -172,6 +172,118 @@
   return faults;
 }
 
+bool PowerDistribution::Faults::GetBreakerFault(int channel) const {
+  switch (channel) {
+    case 0:
+      return Channel0BreakerFault != 0;
+    case 1:
+      return Channel1BreakerFault != 0;
+    case 2:
+      return Channel2BreakerFault != 0;
+    case 3:
+      return Channel3BreakerFault != 0;
+    case 4:
+      return Channel4BreakerFault != 0;
+    case 5:
+      return Channel5BreakerFault != 0;
+    case 6:
+      return Channel6BreakerFault != 0;
+    case 7:
+      return Channel7BreakerFault != 0;
+    case 8:
+      return Channel8BreakerFault != 0;
+    case 9:
+      return Channel9BreakerFault != 0;
+    case 10:
+      return Channel10BreakerFault != 0;
+    case 11:
+      return Channel11BreakerFault != 0;
+    case 12:
+      return Channel12BreakerFault != 0;
+    case 13:
+      return Channel13BreakerFault != 0;
+    case 14:
+      return Channel14BreakerFault != 0;
+    case 15:
+      return Channel15BreakerFault != 0;
+    case 16:
+      return Channel16BreakerFault != 0;
+    case 17:
+      return Channel17BreakerFault != 0;
+    case 18:
+      return Channel18BreakerFault != 0;
+    case 19:
+      return Channel19BreakerFault != 0;
+    case 20:
+      return Channel20BreakerFault != 0;
+    case 21:
+      return Channel21BreakerFault != 0;
+    case 22:
+      return Channel22BreakerFault != 0;
+    case 23:
+      return Channel23BreakerFault != 0;
+    default:
+      throw FRC_MakeError(err::ChannelIndexOutOfRange,
+                          "Power distribution fault channel out of bounds!");
+  }
+}
+
+bool PowerDistribution::StickyFaults::GetBreakerFault(int channel) const {
+  switch (channel) {
+    case 0:
+      return Channel0BreakerFault != 0;
+    case 1:
+      return Channel1BreakerFault != 0;
+    case 2:
+      return Channel2BreakerFault != 0;
+    case 3:
+      return Channel3BreakerFault != 0;
+    case 4:
+      return Channel4BreakerFault != 0;
+    case 5:
+      return Channel5BreakerFault != 0;
+    case 6:
+      return Channel6BreakerFault != 0;
+    case 7:
+      return Channel7BreakerFault != 0;
+    case 8:
+      return Channel8BreakerFault != 0;
+    case 9:
+      return Channel9BreakerFault != 0;
+    case 10:
+      return Channel10BreakerFault != 0;
+    case 11:
+      return Channel11BreakerFault != 0;
+    case 12:
+      return Channel12BreakerFault != 0;
+    case 13:
+      return Channel13BreakerFault != 0;
+    case 14:
+      return Channel14BreakerFault != 0;
+    case 15:
+      return Channel15BreakerFault != 0;
+    case 16:
+      return Channel16BreakerFault != 0;
+    case 17:
+      return Channel17BreakerFault != 0;
+    case 18:
+      return Channel18BreakerFault != 0;
+    case 19:
+      return Channel19BreakerFault != 0;
+    case 20:
+      return Channel20BreakerFault != 0;
+    case 21:
+      return Channel21BreakerFault != 0;
+    case 22:
+      return Channel22BreakerFault != 0;
+    case 23:
+      return Channel23BreakerFault != 0;
+    default:
+      throw FRC_MakeError(err::ChannelIndexOutOfRange,
+                          "Power distribution fault channel out of bounds!");
+  }
+}
+
 PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
   int32_t status = 0;
   HAL_PowerDistributionStickyFaults halStickyFaults;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp
index 8b2b496..2414ed7 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -8,6 +8,7 @@
 
 #include <hal/CAN.h>
 #include <hal/HALBase.h>
+#include <hal/LEDs.h>
 #include <hal/Power.h>
 
 #include "frc/Errors.h"
@@ -230,6 +231,30 @@
   return units::celsius_t{retVal};
 }
 
+static_assert(RadioLEDState::kOff ==
+              static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kOff));
+static_assert(
+    RadioLEDState::kGreen ==
+    static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kGreen));
+static_assert(RadioLEDState::kRed ==
+              static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kRed));
+static_assert(
+    RadioLEDState::kOrange ==
+    static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kOrange));
+
+void RobotController::SetRadioLEDState(RadioLEDState state) {
+  int32_t status = 0;
+  HAL_SetRadioLEDState(static_cast<HAL_RadioLEDState>(state), &status);
+  FRC_CheckErrorStatus(status, "SetRadioLEDState");
+}
+
+RadioLEDState RobotController::GetRadioLEDState() {
+  int32_t status = 0;
+  auto retVal = static_cast<RadioLEDState>(HAL_GetRadioLEDState(&status));
+  FRC_CheckErrorStatus(status, "GetRadioLEDState");
+  return retVal;
+}
+
 CANStatus RobotController::GetCANStatus() {
   int32_t status = 0;
   float percentBusUtilization = 0;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/StadiaController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/StadiaController.cpp
new file mode 100644
index 0000000..4f8d666
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/StadiaController.cpp
@@ -0,0 +1,272 @@
+// 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 "frc/StadiaController.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/event/BooleanEvent.h"
+
+using namespace frc;
+
+StadiaController::StadiaController(int port) : GenericHID(port) {
+  // re-enable when StadiaController is added to Usage Reporting
+  // HAL_Report(HALUsageReporting::kResourceType_StadiaController, port + 1);
+}
+
+double StadiaController::GetLeftX() const {
+  return GetRawAxis(Axis::kLeftX);
+}
+
+double StadiaController::GetRightX() const {
+  return GetRawAxis(Axis::kRightX);
+}
+
+double StadiaController::GetLeftY() const {
+  return GetRawAxis(Axis::kLeftY);
+}
+
+double StadiaController::GetRightY() const {
+  return GetRawAxis(Axis::kRightY);
+}
+
+bool StadiaController::GetLeftBumper() const {
+  return GetRawButton(Button::kLeftBumper);
+}
+
+bool StadiaController::GetRightBumper() const {
+  return GetRawButton(Button::kRightBumper);
+}
+
+bool StadiaController::GetLeftBumperPressed() {
+  return GetRawButtonPressed(Button::kLeftBumper);
+}
+
+bool StadiaController::GetRightBumperPressed() {
+  return GetRawButtonPressed(Button::kRightBumper);
+}
+
+bool StadiaController::GetLeftBumperReleased() {
+  return GetRawButtonReleased(Button::kLeftBumper);
+}
+
+bool StadiaController::GetRightBumperReleased() {
+  return GetRawButtonReleased(Button::kRightBumper);
+}
+
+BooleanEvent StadiaController::LeftBumper(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetLeftBumper(); });
+}
+
+BooleanEvent StadiaController::RightBumper(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetRightBumper(); });
+}
+
+bool StadiaController::GetLeftStickButton() const {
+  return GetRawButton(Button::kLeftStick);
+}
+
+bool StadiaController::GetRightStickButton() const {
+  return GetRawButton(Button::kRightStick);
+}
+
+bool StadiaController::GetLeftStickButtonPressed() {
+  return GetRawButtonPressed(Button::kLeftStick);
+}
+
+bool StadiaController::GetRightStickButtonPressed() {
+  return GetRawButtonPressed(Button::kRightStick);
+}
+
+bool StadiaController::GetLeftStickButtonReleased() {
+  return GetRawButtonReleased(Button::kLeftStick);
+}
+
+bool StadiaController::GetRightStickButtonReleased() {
+  return GetRawButtonReleased(Button::kRightStick);
+}
+
+BooleanEvent StadiaController::LeftStick(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetLeftStickButton(); });
+}
+
+BooleanEvent StadiaController::RightStick(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetRightStickButton(); });
+}
+
+bool StadiaController::GetAButton() const {
+  return GetRawButton(Button::kA);
+}
+
+bool StadiaController::GetAButtonPressed() {
+  return GetRawButtonPressed(Button::kA);
+}
+
+bool StadiaController::GetAButtonReleased() {
+  return GetRawButtonReleased(Button::kA);
+}
+
+BooleanEvent StadiaController::A(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetAButton(); });
+}
+
+bool StadiaController::GetBButton() const {
+  return GetRawButton(Button::kB);
+}
+
+bool StadiaController::GetBButtonPressed() {
+  return GetRawButtonPressed(Button::kB);
+}
+
+bool StadiaController::GetBButtonReleased() {
+  return GetRawButtonReleased(Button::kB);
+}
+
+BooleanEvent StadiaController::B(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetBButton(); });
+}
+
+bool StadiaController::GetXButton() const {
+  return GetRawButton(Button::kX);
+}
+
+bool StadiaController::GetXButtonPressed() {
+  return GetRawButtonPressed(Button::kX);
+}
+
+bool StadiaController::GetXButtonReleased() {
+  return GetRawButtonReleased(Button::kX);
+}
+
+BooleanEvent StadiaController::X(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetXButton(); });
+}
+
+bool StadiaController::GetYButton() const {
+  return GetRawButton(Button::kY);
+}
+
+bool StadiaController::GetYButtonPressed() {
+  return GetRawButtonPressed(Button::kY);
+}
+
+bool StadiaController::GetYButtonReleased() {
+  return GetRawButtonReleased(Button::kY);
+}
+
+BooleanEvent StadiaController::Y(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetYButton(); });
+}
+
+bool StadiaController::GetEllipsesButton() const {
+  return GetRawButton(Button::kEllipses);
+}
+
+bool StadiaController::GetEllipsesButtonPressed() {
+  return GetRawButtonPressed(Button::kEllipses);
+}
+
+bool StadiaController::GetEllipsesButtonReleased() {
+  return GetRawButtonReleased(Button::kEllipses);
+}
+
+BooleanEvent StadiaController::Ellipses(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetEllipsesButton(); });
+}
+
+bool StadiaController::GetHamburgerButton() const {
+  return GetRawButton(Button::kHamburger);
+}
+
+bool StadiaController::GetHamburgerButtonPressed() {
+  return GetRawButtonPressed(Button::kHamburger);
+}
+
+bool StadiaController::GetHamburgerButtonReleased() {
+  return GetRawButtonReleased(Button::kHamburger);
+}
+
+BooleanEvent StadiaController::Hamburger(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetHamburgerButton(); });
+}
+
+bool StadiaController::GetStadiaButton() const {
+  return GetRawButton(Button::kStadia);
+}
+
+bool StadiaController::GetStadiaButtonPressed() {
+  return GetRawButtonPressed(Button::kStadia);
+}
+
+bool StadiaController::GetStadiaButtonReleased() {
+  return GetRawButtonReleased(Button::kStadia);
+}
+
+BooleanEvent StadiaController::Stadia(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetStadiaButton(); });
+}
+
+bool StadiaController::GetGoogleButton() const {
+  return GetRawButton(Button::kGoogle);
+}
+
+bool StadiaController::GetGoogleButtonPressed() {
+  return GetRawButtonPressed(Button::kGoogle);
+}
+
+bool StadiaController::GetGoogleButtonReleased() {
+  return GetRawButtonReleased(Button::kGoogle);
+}
+
+BooleanEvent StadiaController::Google(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetGoogleButton(); });
+}
+
+bool StadiaController::GetFrameButton() const {
+  return GetRawButton(Button::kFrame);
+}
+
+bool StadiaController::GetFrameButtonPressed() {
+  return GetRawButtonPressed(Button::kFrame);
+}
+
+bool StadiaController::GetFrameButtonReleased() {
+  return GetRawButtonReleased(Button::kFrame);
+}
+
+BooleanEvent StadiaController::Frame(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetFrameButton(); });
+}
+
+bool StadiaController::GetLeftTriggerButton() const {
+  return GetRawButton(Button::kLeftTrigger);
+}
+
+bool StadiaController::GetLeftTriggerButtonPressed() {
+  return GetRawButtonPressed(Button::kLeftTrigger);
+}
+
+bool StadiaController::GetLeftTriggerButtonReleased() {
+  return GetRawButtonReleased(Button::kLeftTrigger);
+}
+
+BooleanEvent StadiaController::LeftTrigger(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetLeftTriggerButton(); });
+}
+
+bool StadiaController::GetRightTriggerButton() const {
+  return GetRawButton(Button::kRightTrigger);
+}
+
+bool StadiaController::GetRightTriggerButtonPressed() {
+  return GetRawButtonPressed(Button::kRightTrigger);
+}
+
+bool StadiaController::GetRightTriggerButtonReleased() {
+  return GetRawButtonReleased(Button::kRightTrigger);
+}
+
+BooleanEvent StadiaController::RightTrigger(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetRightTriggerButton(); });
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index 8cce62e..e3a0a8c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -16,11 +16,21 @@
 
 using namespace frc;
 
+WPI_IGNORE_DEPRECATED
+
 DifferentialDrive::DifferentialDrive(MotorController& leftMotor,
                                      MotorController& rightMotor)
-    : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor) {
-  wpi::SendableRegistry::AddChild(this, m_leftMotor);
-  wpi::SendableRegistry::AddChild(this, m_rightMotor);
+    : DifferentialDrive{[&](double output) { leftMotor.Set(output); },
+                        [&](double output) { rightMotor.Set(output); }} {
+  wpi::SendableRegistry::AddChild(this, &leftMotor);
+  wpi::SendableRegistry::AddChild(this, &rightMotor);
+}
+
+WPI_UNIGNORE_DEPRECATED
+
+DifferentialDrive::DifferentialDrive(std::function<void(double)> leftMotor,
+                                     std::function<void(double)> rightMotor)
+    : m_leftMotor{std::move(leftMotor)}, m_rightMotor{std::move(rightMotor)} {
   static int instances = 0;
   ++instances;
   wpi::SendableRegistry::AddLW(this, "DifferentialDrive", instances);
@@ -40,8 +50,11 @@
 
   auto [left, right] = ArcadeDriveIK(xSpeed, zRotation, squareInputs);
 
-  m_leftMotor->Set(left);
-  m_rightMotor->Set(right);
+  m_leftOutput = left * m_maxOutput;
+  m_rightOutput = right * m_maxOutput;
+
+  m_leftMotor(m_leftOutput);
+  m_rightMotor(m_rightOutput);
 
   Feed();
 }
@@ -60,8 +73,11 @@
 
   auto [left, right] = CurvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
 
-  m_leftMotor->Set(left * m_maxOutput);
-  m_rightMotor->Set(right * m_maxOutput);
+  m_leftOutput = left * m_maxOutput;
+  m_rightOutput = right * m_maxOutput;
+
+  m_leftMotor(m_leftOutput);
+  m_rightMotor(m_rightOutput);
 
   Feed();
 }
@@ -80,8 +96,11 @@
 
   auto [left, right] = TankDriveIK(leftSpeed, rightSpeed, squareInputs);
 
-  m_leftMotor->Set(left * m_maxOutput);
-  m_rightMotor->Set(right * m_maxOutput);
+  m_leftOutput = left * m_maxOutput;
+  m_rightOutput = right * m_maxOutput;
+
+  m_leftMotor(m_leftOutput);
+  m_rightMotor(m_rightOutput);
 
   Feed();
 }
@@ -157,8 +176,12 @@
 }
 
 void DifferentialDrive::StopMotor() {
-  m_leftMotor->StopMotor();
-  m_rightMotor->StopMotor();
+  m_leftOutput = 0.0;
+  m_rightOutput = 0.0;
+
+  m_leftMotor(0.0);
+  m_rightMotor(0.0);
+
   Feed();
 }
 
@@ -171,9 +194,7 @@
   builder.SetActuator(true);
   builder.SetSafeState([=, this] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Left Motor Speed", [=, this] { return m_leftMotor->Get(); },
-      [=, this](double value) { m_leftMotor->Set(value); });
+      "Left Motor Speed", [&] { return m_leftOutput; }, m_leftMotor);
   builder.AddDoubleProperty(
-      "Right Motor Speed", [=, this] { return m_rightMotor->Get(); },
-      [=, this](double value) { m_rightMotor->Set(value); });
+      "Right Motor Speed", [&] { return m_rightOutput; }, m_rightMotor);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index 2bf6a3f..aeec27d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -16,18 +16,32 @@
 
 using namespace frc;
 
+WPI_IGNORE_DEPRECATED
+
 MecanumDrive::MecanumDrive(MotorController& frontLeftMotor,
                            MotorController& rearLeftMotor,
                            MotorController& frontRightMotor,
                            MotorController& rearRightMotor)
-    : m_frontLeftMotor(&frontLeftMotor),
-      m_rearLeftMotor(&rearLeftMotor),
-      m_frontRightMotor(&frontRightMotor),
-      m_rearRightMotor(&rearRightMotor) {
-  wpi::SendableRegistry::AddChild(this, m_frontLeftMotor);
-  wpi::SendableRegistry::AddChild(this, m_rearLeftMotor);
-  wpi::SendableRegistry::AddChild(this, m_frontRightMotor);
-  wpi::SendableRegistry::AddChild(this, m_rearRightMotor);
+    : MecanumDrive{[&](double output) { frontLeftMotor.Set(output); },
+                   [&](double output) { rearLeftMotor.Set(output); },
+                   [&](double output) { frontRightMotor.Set(output); },
+                   [&](double output) { rearRightMotor.Set(output); }} {
+  wpi::SendableRegistry::AddChild(this, &frontLeftMotor);
+  wpi::SendableRegistry::AddChild(this, &rearLeftMotor);
+  wpi::SendableRegistry::AddChild(this, &frontRightMotor);
+  wpi::SendableRegistry::AddChild(this, &rearRightMotor);
+}
+
+WPI_UNIGNORE_DEPRECATED
+
+MecanumDrive::MecanumDrive(std::function<void(double)> frontLeftMotor,
+                           std::function<void(double)> rearLeftMotor,
+                           std::function<void(double)> frontRightMotor,
+                           std::function<void(double)> rearRightMotor)
+    : m_frontLeftMotor{std::move(frontLeftMotor)},
+      m_rearLeftMotor{std::move(rearLeftMotor)},
+      m_frontRightMotor{std::move(frontRightMotor)},
+      m_rearRightMotor{std::move(rearRightMotor)} {
   static int instances = 0;
   ++instances;
   wpi::SendableRegistry::AddLW(this, "MecanumDrive", instances);
@@ -47,10 +61,15 @@
   auto [frontLeft, frontRight, rearLeft, rearRight] =
       DriveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
 
-  m_frontLeftMotor->Set(frontLeft * m_maxOutput);
-  m_frontRightMotor->Set(frontRight * m_maxOutput);
-  m_rearLeftMotor->Set(rearLeft * m_maxOutput);
-  m_rearRightMotor->Set(rearRight * m_maxOutput);
+  m_frontLeftOutput = frontLeft * m_maxOutput;
+  m_rearLeftOutput = rearLeft * m_maxOutput;
+  m_frontRightOutput = frontRight * m_maxOutput;
+  m_rearRightOutput = rearRight * m_maxOutput;
+
+  m_frontLeftMotor(m_frontLeftOutput);
+  m_frontRightMotor(m_frontRightOutput);
+  m_rearLeftMotor(m_rearLeftOutput);
+  m_rearRightMotor(m_rearRightOutput);
 
   Feed();
 }
@@ -68,10 +87,16 @@
 }
 
 void MecanumDrive::StopMotor() {
-  m_frontLeftMotor->StopMotor();
-  m_frontRightMotor->StopMotor();
-  m_rearLeftMotor->StopMotor();
-  m_rearRightMotor->StopMotor();
+  m_frontLeftOutput = 0.0;
+  m_frontRightOutput = 0.0;
+  m_rearLeftOutput = 0.0;
+  m_rearRightOutput = 0.0;
+
+  m_frontLeftMotor(0.0);
+  m_frontRightMotor(0.0);
+  m_rearLeftMotor(0.0);
+  m_rearRightMotor(0.0);
+
   Feed();
 }
 
@@ -108,15 +133,15 @@
   builder.SetActuator(true);
   builder.SetSafeState([=, this] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Front Left Motor Speed", [=, this] { return m_frontLeftMotor->Get(); },
-      [=, this](double value) { m_frontLeftMotor->Set(value); });
+      "Front Left Motor Speed", [&] { return m_frontLeftOutput; },
+      m_frontLeftMotor);
   builder.AddDoubleProperty(
-      "Front Right Motor Speed", [=, this] { return m_frontRightMotor->Get(); },
-      [=, this](double value) { m_frontRightMotor->Set(value); });
+      "Front Right Motor Speed", [&] { return m_frontRightOutput; },
+      m_frontRightMotor);
   builder.AddDoubleProperty(
-      "Rear Left Motor Speed", [=, this] { return m_rearLeftMotor->Get(); },
-      [=, this](double value) { m_rearLeftMotor->Set(value); });
+      "Rear Left Motor Speed", [&] { return m_rearLeftOutput; },
+      m_rearLeftMotor);
   builder.AddDoubleProperty(
-      "Rear Right Motor Speed", [=, this] { return m_rearRightMotor->Get(); },
-      [=, this](double value) { m_rearRightMotor->Set(value); });
+      "Rear Right Motor Speed", [&] { return m_rearRightOutput; },
+      m_rearRightMotor);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/event/EventLoop.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/event/EventLoop.cpp
index 5af79c9..c85286b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/event/EventLoop.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/event/EventLoop.cpp
@@ -4,20 +4,41 @@
 
 #include "frc/event/EventLoop.h"
 
+#include "frc/Errors.h"
+
 using namespace frc;
 
+namespace {
+struct RunningSetter {
+  bool& m_running;
+  explicit RunningSetter(bool& running) noexcept : m_running{running} {
+    m_running = true;
+  }
+  ~RunningSetter() noexcept { m_running = false; }
+};
+}  // namespace
+
 EventLoop::EventLoop() {}
 
 void EventLoop::Bind(wpi::unique_function<void()> action) {
+  if (m_running) {
+    throw FRC_MakeError(err::Error,
+                        "Cannot bind EventLoop while it is running");
+  }
   m_bindings.emplace_back(std::move(action));
 }
 
 void EventLoop::Poll() {
+  RunningSetter runSetter{m_running};
   for (wpi::unique_function<void()>& action : m_bindings) {
     action();
   }
 }
 
 void EventLoop::Clear() {
+  if (m_running) {
+    throw FRC_MakeError(err::Error,
+                        "Cannot clear EventLoop while it is running");
+  }
   m_bindings.clear();
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp
index 4085658..78d580e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp
@@ -23,13 +23,14 @@
   }
 }
 
-void DriverStationModeThread::InAutonomous(bool entering) {
-  m_userInAutonomous = entering;
-}
 void DriverStationModeThread::InDisabled(bool entering) {
   m_userInDisabled = entering;
 }
 
+void DriverStationModeThread::InAutonomous(bool entering) {
+  m_userInAutonomous = entering;
+}
+
 void DriverStationModeThread::InTeleop(bool entering) {
   m_userInTeleop = entering;
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp
index 9d20144..2b64da2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp
@@ -9,5 +9,6 @@
 using namespace frc;
 
 void MotorController::SetVoltage(units::volt_t output) {
+  // NOLINTNEXTLINE(bugprone-integer-division)
   Set(output / RobotController::GetBatteryVoltage());
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
index f855d14..cf57f7c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
@@ -12,6 +12,8 @@
 // Can't use a delegated constructor here because of an MSVC bug.
 // https://developercommunity.visualstudio.com/content/problem/583/compiler-bug-with-delegating-a-constructor.html
 
+WPI_IGNORE_DEPRECATED
+
 MotorControllerGroup::MotorControllerGroup(
     std::vector<std::reference_wrapper<MotorController>>&& motorControllers)
     : m_motorControllers(std::move(motorControllers)) {
@@ -74,3 +76,5 @@
       "Value", [=, this] { return Get(); },
       [=, this](double value) { Set(value); });
 }
+
+WPI_UNIGNORE_DEPRECATED
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
index f25aa91..a05b7cc 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
@@ -11,6 +11,8 @@
 
 using namespace frc;
 
+WPI_IGNORE_DEPRECATED
+
 NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
     : m_dio(dioChannel), m_pwm(pwmChannel) {
   wpi::SendableRegistry::AddChild(this, &m_dio);
@@ -26,6 +28,8 @@
   wpi::SendableRegistry::AddLW(this, "Nidec Brushless", pwmChannel);
 }
 
+WPI_UNIGNORE_DEPRECATED
+
 void NidecBrushless::Set(double speed) {
   if (!m_disabled) {
     m_speed = speed;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
index 3692f75..924e4fd 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
@@ -8,13 +8,31 @@
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/RobotController.h"
+
 using namespace frc;
 
 void PWMMotorController::Set(double speed) {
-  m_pwm.SetSpeed(m_isInverted ? -speed : speed);
+  if (m_isInverted) {
+    speed = -speed;
+  }
+  m_pwm.SetSpeed(speed);
+
+  for (auto& follower : m_nonowningFollowers) {
+    follower->Set(speed);
+  }
+  for (auto& follower : m_owningFollowers) {
+    follower->Set(speed);
+  }
+
   Feed();
 }
 
+void PWMMotorController::SetVoltage(units::volt_t output) {
+  // NOLINTNEXTLINE(bugprone-integer-division)
+  Set(output / RobotController::GetBatteryVoltage());
+}
+
 double PWMMotorController::Get() const {
   return m_pwm.GetSpeed() * (m_isInverted ? -1.0 : 1.0);
 }
@@ -48,11 +66,19 @@
   m_pwm.EnableDeadbandElimination(eliminateDeadband);
 }
 
+void PWMMotorController::AddFollower(PWMMotorController& follower) {
+  m_nonowningFollowers.emplace_back(&follower);
+}
+
+WPI_IGNORE_DEPRECATED
+
 PWMMotorController::PWMMotorController(std::string_view name, int channel)
     : m_pwm(channel, false) {
   wpi::SendableRegistry::AddLW(this, name, channel);
 }
 
+WPI_UNIGNORE_DEPRECATED
+
 void PWMMotorController::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Motor Controller");
   builder.SetActuator(true);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkFlex.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkFlex.cpp
new file mode 100644
index 0000000..945a70e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkFlex.cpp
@@ -0,0 +1,20 @@
+// 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 "frc/motorcontrol/PWMSparkFlex.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PWMSparkFlex::PWMSparkFlex(int channel)
+    : PWMMotorController("PWMSparkFlex", channel) {
+  m_pwm.SetBounds(2.003_ms, 1.55_ms, 1.50_ms, 1.46_ms, 0.999_ms);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_RevSparkFlexPWM,
+             GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index a19cc6a..8c0880a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -30,12 +30,17 @@
   m_impl->selectedTabPub =
       m_impl->rootMetaTable->GetStringTopic("Selected")
           .Publish(nt::PubSubOptions{.keepDuplicates = true});
-  HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
 }
 
 ShuffleboardInstance::~ShuffleboardInstance() = default;
 
+static bool gReported = false;
+
 frc::ShuffleboardTab& ShuffleboardInstance::GetTab(std::string_view title) {
+  if (!gReported) {
+    HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
+    gReported = true;
+  }
   if (m_impl->tabs.find(title) == m_impl->tabs.end()) {
     m_impl->tabs.try_emplace(title,
                              std::make_unique<ShuffleboardTab>(*this, title));
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 8c27cdf..d5addb2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -58,7 +58,7 @@
 }
 
 void DifferentialDrivetrainSim::Update(units::second_t dt) {
-  m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
+  m_x = RKDP([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
   m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
 }
 
@@ -92,26 +92,22 @@
 }
 
 units::ampere_t DifferentialDrivetrainSim::GetLeftCurrentDraw() const {
-  auto loadIleft =
-      m_motor.Current(
-          units::radians_per_second_t{m_x(State::kLeftVelocity) *
-                                      m_currentGearing / m_wheelRadius.value()},
-          units::volt_t{m_u(0)}) *
-      wpi::sgn(m_u(0));
-
-  return loadIleft;
+  return m_motor.Current(units::radians_per_second_t{m_x(State::kLeftVelocity) *
+                                                     m_currentGearing /
+                                                     m_wheelRadius.value()},
+                         units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
 }
 
 units::ampere_t DifferentialDrivetrainSim::GetRightCurrentDraw() const {
-  auto loadIRight =
-      m_motor.Current(
-          units::radians_per_second_t{m_x(State::kRightVelocity) *
-                                      m_currentGearing / m_wheelRadius.value()},
-          units::volt_t{m_u(1)}) *
-      wpi::sgn(m_u(1));
-
-  return loadIRight;
+  return m_motor.Current(
+             units::radians_per_second_t{m_x(State::kRightVelocity) *
+                                         m_currentGearing /
+                                         m_wheelRadius.value()},
+             units::volt_t{m_u(1)}) *
+         wpi::sgn(m_u(1));
 }
+
 units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
   return GetLeftCurrentDraw() + GetRightCurrentDraw();
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp
index 476d07a..04aef3a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp
@@ -12,11 +12,6 @@
 using namespace frc;
 using namespace frc::sim;
 
-PneumaticsBaseSim::PneumaticsBaseSim(int module) : m_index{module} {}
-
-PneumaticsBaseSim::PneumaticsBaseSim(const PneumaticsBase& module)
-    : m_index{module.GetModuleNumber()} {}
-
 std::shared_ptr<PneumaticsBaseSim> PneumaticsBaseSim::GetForType(
     int module, PneumaticsModuleType type) {
   switch (type) {
@@ -31,3 +26,8 @@
                           static_cast<int>(module));
   }
 }
+
+PneumaticsBaseSim::PneumaticsBaseSim(int module) : m_index{module} {}
+
+PneumaticsBaseSim::PneumaticsBaseSim(const PneumaticsBase& module)
+    : m_index{module.GetModuleNumber()} {}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
index 90d9651..728d358 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
@@ -338,6 +338,23 @@
   HALSIM_SetRoboRioComments(comments.data(), comments.size());
 }
 
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterRadioLEDStateCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioRadioLEDStateCallback);
+  store->SetUid(HALSIM_RegisterRoboRioRadioLEDStateCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+RadioLEDState RoboRioSim::GetRadioLEDState() {
+  return static_cast<RadioLEDState>(HALSIM_GetRoboRioRadioLEDState());
+}
+
+void RoboRioSim::SetRadioLEDState(RadioLEDState state) {
+  HALSIM_SetRoboRioRadioLEDState(static_cast<HAL_RadioLEDState>(state));
+}
+
 void RoboRioSim::ResetData() {
   HALSIM_ResetRoboRioData();
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
index 30a5650..1016c4b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
@@ -18,8 +18,8 @@
   m_simRange = deviceSim.GetDouble("Range (in)");
 }
 
-void UltrasonicSim::SetRangeValid(bool isValid) {
-  m_simRangeValid.Set(isValid);
+void UltrasonicSim::SetRangeValid(bool valid) {
+  m_simRangeValid.Set(valid);
 }
 
 void UltrasonicSim::SetRange(units::inch_t range) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index e6991d4..d57473f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -19,8 +19,6 @@
 
 namespace {
 struct Instance {
-  Instance() { HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0); }
-
   detail::ListenerExecutor listenerExecutor;
   std::shared_ptr<nt::NetworkTable> table =
       nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
@@ -46,6 +44,8 @@
 }  // namespace frc::impl
 #endif
 
+static bool gReported = false;
+
 void SmartDashboard::init() {
   GetInstance();
 }
@@ -59,18 +59,23 @@
 }
 
 void SmartDashboard::SetPersistent(std::string_view key) {
-  GetInstance().table->GetEntry(key).SetPersistent();
+  GetEntry(key).SetPersistent();
 }
 
 void SmartDashboard::ClearPersistent(std::string_view key) {
-  GetInstance().table->GetEntry(key).ClearPersistent();
+  GetEntry(key).ClearPersistent();
 }
 
 bool SmartDashboard::IsPersistent(std::string_view key) {
-  return GetInstance().table->GetEntry(key).IsPersistent();
+  return GetEntry(key).IsPersistent();
 }
 
 nt::NetworkTableEntry SmartDashboard::GetEntry(std::string_view key) {
+  if (!gReported) {
+    HAL_Report(HALUsageReporting::kResourceType_SmartDashboard,
+               HALUsageReporting::kSmartDashboard_Instance);
+    gReported = true;
+  }
   return GetInstance().table->GetEntry(key);
 }
 
@@ -78,6 +83,11 @@
   if (!data) {
     throw FRC_MakeError(err::NullParameter, "value");
   }
+  if (!gReported) {
+    HAL_Report(HALUsageReporting::kResourceType_SmartDashboard,
+               HALUsageReporting::kSmartDashboard_Instance);
+    gReported = true;
+  }
   auto& inst = GetInstance();
   std::scoped_lock lock(inst.tablesToDataMutex);
   auto& uid = inst.tablesToData[key];
@@ -120,7 +130,7 @@
 
 bool SmartDashboard::SetDefaultBoolean(std::string_view key,
                                        bool defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultBoolean(defaultValue);
+  return GetEntry(key).SetDefaultBoolean(defaultValue);
 }
 
 bool SmartDashboard::GetBoolean(std::string_view keyName, bool defaultValue) {
@@ -133,7 +143,7 @@
 
 bool SmartDashboard::SetDefaultNumber(std::string_view key,
                                       double defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultDouble(defaultValue);
+  return GetEntry(key).SetDefaultDouble(defaultValue);
 }
 
 double SmartDashboard::GetNumber(std::string_view keyName,
@@ -148,7 +158,7 @@
 
 bool SmartDashboard::SetDefaultString(std::string_view key,
                                       std::string_view defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultString(defaultValue);
+  return GetEntry(key).SetDefaultString(defaultValue);
 }
 
 std::string SmartDashboard::GetString(std::string_view keyName,
@@ -158,63 +168,62 @@
 
 bool SmartDashboard::PutBooleanArray(std::string_view key,
                                      std::span<const int> value) {
-  return GetInstance().table->GetEntry(key).SetBooleanArray(value);
+  return GetEntry(key).SetBooleanArray(value);
 }
 
 bool SmartDashboard::SetDefaultBooleanArray(std::string_view key,
                                             std::span<const int> defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultBooleanArray(
-      defaultValue);
+  return GetEntry(key).SetDefaultBooleanArray(defaultValue);
 }
 
 std::vector<int> SmartDashboard::GetBooleanArray(
     std::string_view key, std::span<const int> defaultValue) {
-  return GetInstance().table->GetEntry(key).GetBooleanArray(defaultValue);
+  return GetEntry(key).GetBooleanArray(defaultValue);
 }
 
 bool SmartDashboard::PutNumberArray(std::string_view key,
                                     std::span<const double> value) {
-  return GetInstance().table->GetEntry(key).SetDoubleArray(value);
+  return GetEntry(key).SetDoubleArray(value);
 }
 
 bool SmartDashboard::SetDefaultNumberArray(
     std::string_view key, std::span<const double> defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultDoubleArray(defaultValue);
+  return GetEntry(key).SetDefaultDoubleArray(defaultValue);
 }
 
 std::vector<double> SmartDashboard::GetNumberArray(
     std::string_view key, std::span<const double> defaultValue) {
-  return GetInstance().table->GetEntry(key).GetDoubleArray(defaultValue);
+  return GetEntry(key).GetDoubleArray(defaultValue);
 }
 
 bool SmartDashboard::PutStringArray(std::string_view key,
                                     std::span<const std::string> value) {
-  return GetInstance().table->GetEntry(key).SetStringArray(value);
+  return GetEntry(key).SetStringArray(value);
 }
 
 bool SmartDashboard::SetDefaultStringArray(
     std::string_view key, std::span<const std::string> defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultStringArray(defaultValue);
+  return GetEntry(key).SetDefaultStringArray(defaultValue);
 }
 
 std::vector<std::string> SmartDashboard::GetStringArray(
     std::string_view key, std::span<const std::string> defaultValue) {
-  return GetInstance().table->GetEntry(key).GetStringArray(defaultValue);
+  return GetEntry(key).GetStringArray(defaultValue);
 }
 
 bool SmartDashboard::PutRaw(std::string_view key,
                             std::span<const uint8_t> value) {
-  return GetInstance().table->GetEntry(key).SetRaw(value);
+  return GetEntry(key).SetRaw(value);
 }
 
 bool SmartDashboard::SetDefaultRaw(std::string_view key,
                                    std::span<const uint8_t> defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultRaw(defaultValue);
+  return GetEntry(key).SetDefaultRaw(defaultValue);
 }
 
 std::vector<uint8_t> SmartDashboard::GetRaw(
     std::string_view key, std::span<const uint8_t> defaultValue) {
-  return GetInstance().table->GetEntry(key).GetRaw(defaultValue);
+  return GetEntry(key).GetRaw(defaultValue);
 }
 
 bool SmartDashboard::PutValue(std::string_view keyName,
@@ -224,7 +233,7 @@
 
 bool SmartDashboard::SetDefaultValue(std::string_view key,
                                      const nt::Value& defaultValue) {
-  return GetInstance().table->GetEntry(key).SetDefaultValue(defaultValue);
+  return GetEntry(key).SetDefaultValue(defaultValue);
 }
 
 nt::Value SmartDashboard::GetValue(std::string_view keyName) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp
new file mode 100644
index 0000000..b99b792
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp
@@ -0,0 +1,66 @@
+// 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 "frc/sysid/SysIdRoutineLog.h"
+
+#include <fmt/format.h>
+
+#include "frc/DataLogManager.h"
+
+using namespace frc::sysid;
+
+SysIdRoutineLog::SysIdRoutineLog(std::string_view logName)
+    : m_logName(logName),
+      m_state(wpi::log::StringLogEntry{
+          frc::DataLogManager::GetLog(),
+          fmt::format("sysid-test-state{}", logName)}) {
+  m_state.Append(StateEnumToString(State::kNone));
+}
+
+SysIdRoutineLog::MotorLog::MotorLog(std::string_view motorName,
+                                    std::string_view logName,
+                                    LogEntries* logEntries)
+    : m_motorName(motorName), m_logName(logName), m_logEntries(logEntries) {
+  (*logEntries)[motorName] = MotorEntries();
+}
+
+SysIdRoutineLog::MotorLog& SysIdRoutineLog::MotorLog::value(
+    std::string_view name, double value, std::string_view unit) {
+  auto& motorEntries = (*m_logEntries)[m_motorName];
+
+  if (!motorEntries.contains(name)) {
+    wpi::log::DataLog& log = frc::DataLogManager::GetLog();
+
+    motorEntries[name] = wpi::log::DoubleLogEntry(
+        log, fmt::format("{}-{}-{}", name, m_motorName, m_logName), unit);
+  }
+
+  motorEntries[name].Append(value);
+  return *this;
+}
+
+SysIdRoutineLog::MotorLog SysIdRoutineLog::Motor(std::string_view motorName) {
+  return MotorLog{motorName, m_logName, &m_logEntries};
+}
+
+void SysIdRoutineLog::RecordState(State state) {
+  m_state.Append(StateEnumToString(state));
+}
+
+std::string SysIdRoutineLog::StateEnumToString(State state) {
+  switch (state) {
+    case State::kQuasistaticForward:
+      return "quasistatic-forward";
+    case State::kQuasistaticReverse:
+      return "quasistatic-reverse";
+    case State::kDynamicForward:
+      return "dynamic-forward";
+    case State::kDynamicReverse:
+      return "dynamic-reverse";
+    case State::kNone:
+      return "none";
+    default:
+      return "none";
+  }
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/util/Color.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/util/Color.cpp
deleted file mode 100644
index e3adaf2..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/util/Color.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-// 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 "frc/util/Color.h"
-
-#include <fmt/format.h>
-
-using namespace frc;
-
-std::string Color::HexString() const {
-  return fmt::format("#{:02X}{:02X}{:02X}", static_cast<int>(255.0 * red),
-                     static_cast<int>(255.0 * green),
-                     static_cast<int>(255.0 * blue));
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/util/Color8Bit.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/util/Color8Bit.cpp
deleted file mode 100644
index af200a2..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/util/Color8Bit.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-// 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 "frc/util/Color8Bit.h"
-
-#include <fmt/format.h>
-
-using namespace frc;
-
-std::string Color8Bit::HexString() const {
-  return fmt::format("#{:02X}{:02X}{:02X}", red, green, blue);
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
index 3f4a1c2..41c66c1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
@@ -56,23 +56,47 @@
 class ADIS16448_IMU : public wpi::Sendable,
                       public wpi::SendableHelper<ADIS16448_IMU> {
  public:
-  /* ADIS16448 Calibration Time Enum Class */
+  /**
+   * ADIS16448 calibration times.
+   */
   enum class CalibrationTime {
+    /// 32 ms calibration time.
     _32ms = 0,
+    /// 64 ms calibration time.
     _64ms = 1,
+    /// 128 ms calibration time.
     _128ms = 2,
+    /// 256 ms calibration time.
     _256ms = 3,
+    /// 512 ms calibration time.
     _512ms = 4,
+    /// 1 s calibration time.
     _1s = 5,
+    /// 2 s calibration time.
     _2s = 6,
+    /// 4 s calibration time.
     _4s = 7,
+    /// 8 s calibration time.
     _8s = 8,
+    /// 16 s calibration time.
     _16s = 9,
+    /// 32 s calibration time.
     _32s = 10,
+    /// 64 s calibration time.
     _64s = 11
   };
 
-  enum IMUAxis { kX, kY, kZ };
+  /**
+   * IMU axes.
+   */
+  enum IMUAxis {
+    /// The IMU's X axis.
+    kX,
+    /// The IMU's Y axis.
+    kY,
+    /// The IMU's Z axis.
+    kZ
+  };
 
   /**
    * IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
@@ -183,31 +207,71 @@
    */
   units::meters_per_second_squared_t GetAccelZ() const;
 
+  /**
+   * Returns the complementary angle around the X axis computed from
+   * accelerometer and gyro rate measurements.
+   */
   units::degree_t GetXComplementaryAngle() const;
 
+  /**
+   * Returns the complementary angle around the Y axis computed from
+   * accelerometer and gyro rate measurements.
+   */
   units::degree_t GetYComplementaryAngle() const;
 
+  /**
+   * Returns the X-axis filtered acceleration angle.
+   */
   units::degree_t GetXFilteredAccelAngle() const;
 
+  /**
+   * Returns the Y-axis filtered acceleration angle.
+   */
   units::degree_t GetYFilteredAccelAngle() const;
 
+  /**
+   * Returns the magnetic field strength in the X axis.
+   */
   units::tesla_t GetMagneticFieldX() const;
 
+  /**
+   * Returns the magnetic field strength in the Y axis.
+   */
   units::tesla_t GetMagneticFieldY() const;
 
+  /**
+   * Returns the magnetic field strength in the Z axis.
+   */
   units::tesla_t GetMagneticFieldZ() const;
 
+  /**
+   * Returns the barometric pressure.
+   */
   units::pounds_per_square_inch_t GetBarometricPressure() const;
 
+  /**
+   * Returns the temperature.
+   */
   units::celsius_t GetTemperature() const;
 
   IMUAxis GetYawAxis() const;
 
   int SetYawAxis(IMUAxis yaw_axis);
 
+  /**
+   * Checks the connection status of the IMU.
+   *
+   * @return True if the IMU is connected, false otherwise.
+   */
   bool IsConnected() const;
 
-  int ConfigDecRate(uint16_t DecimationRate);
+  /**
+   * Configures the decimation rate of the IMU.
+   *
+   * @param decimationRate The new decimation value.
+   * @return 0 if success, 1 if no change, 2 if error.
+   */
+  int ConfigDecRate(uint16_t decimationRate);
 
   /**
    * Get the SPI port number.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
index e3b521c..f594c2b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
@@ -53,55 +53,109 @@
 class ADIS16470_IMU : public wpi::Sendable,
                       public wpi::SendableHelper<ADIS16470_IMU> {
  public:
-  /* ADIS16470 Calibration Time Enum Class */
+  /**
+   * ADIS16470 calibration times.
+   */
   enum class CalibrationTime {
+    /// 32 ms calibration time.
     _32ms = 0,
+    /// 64 ms calibration time.
     _64ms = 1,
+    /// 128 ms calibration time.
     _128ms = 2,
+    /// 256 ms calibration time.
     _256ms = 3,
+    /// 512 ms calibration time.
     _512ms = 4,
+    /// 1 s calibration time.
     _1s = 5,
+    /// 2 s calibration time.
     _2s = 6,
+    /// 4 s calibration time.
     _4s = 7,
+    /// 8 s calibration time.
     _8s = 8,
+    /// 16 s calibration time.
     _16s = 9,
+    /// 32 s calibration time.
     _32s = 10,
+    /// 64 s calibration time.
     _64s = 11
   };
 
-  enum IMUAxis { kX, kY, kZ };
+  /**
+   * IMU axes.
+   *
+   * kX, kY, and kZ refer to the IMU's X, Y, and Z axes respectively. kYaw,
+   * kPitch, and kRoll are configured by the user to refer to an X, Y, or Z
+   * axis.
+   */
+  enum IMUAxis {
+    /// The IMU's X axis.
+    kX,
+    /// The IMU's Y axis.
+    kY,
+    /// The IMU's Z axis.
+    kZ,
+    /// The user-configured yaw axis.
+    kYaw,
+    /// The user-configured pitch axis.
+    kPitch,
+    /// The user-configured roll axis.
+    kRoll
+  };
 
   /**
-   * @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis
-   * is set to the IMU Z axis, and calibration time is defaulted to 4 seconds.
+   * Creates a new ADIS16740 IMU object.
+   *
+   * The default setup is the onboard SPI port with a calibration time of 4
+   * seconds. Yaw, pitch, and roll are kZ, kX, and kY respectively.
    */
   ADIS16470_IMU();
 
   /**
-   * @brief Customizable constructor. Allows the SPI port and CS to be
-   * customized, the yaw axis used for GetAngle() is adjustable, and initial
-   * calibration time can be modified.
+   * Creates a new ADIS16740 IMU object.
    *
-   * @param yaw_axis Selects the "default" axis to use for GetAngle() and
-   * GetRate()
+   * The default setup is the onboard SPI port with a calibration time of 4
+   * seconds.
    *
-   * @param port The SPI port and CS where the IMU is connected.
+   * <b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll
+   * will result in an error.</i></b>
    *
-   * @param cal_time The calibration time that should be used on start-up.
+   * @param yaw_axis The axis that measures the yaw
+   * @param pitch_axis The axis that measures the pitch
+   * @param roll_axis The axis that measures the roll
    */
-  explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
-                         CalibrationTime cal_time);
+  ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis);
 
   /**
-   * @brief Destructor. Kills the acquisition loop and closes the SPI
-   * peripheral.
+   * Creates a new ADIS16740 IMU object.
+   *
+   * <b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch, or
+   * kRoll will result in an error.</i></b>
+   *
+   * @param yaw_axis The axis that measures the yaw
+   * @param pitch_axis The axis that measures the pitch
+   * @param roll_axis The axis that measures the roll
+   * @param port The SPI Port the gyro is plugged into
+   * @param cal_time Calibration time
    */
+  explicit ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
+                         IMUAxis roll_axis, frc::SPI::Port port,
+                         CalibrationTime cal_time);
+
   ~ADIS16470_IMU() override;
 
   ADIS16470_IMU(ADIS16470_IMU&&) = default;
   ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
 
-  int ConfigDecRate(uint16_t reg);
+  /**
+   * Configures the decimation rate of the IMU.
+   *
+   * @param decimationRate The new decimation value.
+   * @return 0 if success, 1 if no change, 2 if error.
+   */
+  int ConfigDecRate(uint16_t decimationRate);
 
   /**
    * @brief Switches the active SPI port to standard SPI mode, writes the
@@ -116,22 +170,66 @@
   int ConfigCalTime(CalibrationTime new_cal_time);
 
   /**
-   * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+   * Reset the gyro.
    *
    * Resets the gyro accumulations to a heading of zero. This can be used if
-   * the "zero" orientation of the sensor needs to be changed in runtime.
+   * there is significant drift in the gyro and it needs to be recalibrated
+   * after running.
    */
   void Reset();
 
   /**
-   * Returns the yaw axis angle in degrees (CCW positive).
+   * Allow the designated gyro angle to be set to a given value. This may happen
+   * with unread values in the buffer, it is suggested that the IMU is not
+   * moving when this method is run.
+   *
+   * @param axis IMUAxis that will be changed
+   * @param angle The new angle (CCW positive)
    */
-  units::degree_t GetAngle() const;
+  void SetGyroAngle(IMUAxis axis, units::degree_t angle);
 
   /**
-   * Returns the yaw axis angular rate in degrees per second (CCW positive).
+   * Allow the gyro angle X to be set to a given value. This may happen with
+   * unread values in the buffer, it is suggested that the IMU is not moving
+   * when this method is run.
+   *
+   * @param angle The new angle (CCW positive)
    */
-  units::degrees_per_second_t GetRate() const;
+  void SetGyroAngleX(units::degree_t angle);
+
+  /**
+   * Allow the gyro angle Y to be set to a given value. This may happen with
+   * unread values in the buffer, it is suggested that the IMU is not moving
+   * when this method is run.
+   *
+   * @param angle The new angle (CCW positive)
+   */
+  void SetGyroAngleY(units::degree_t angle);
+
+  /**
+   * Allow the gyro angle Z to be set to a given value. This may happen with
+   * unread values in the buffer, it is suggested that the IMU is not moving
+   * when this method is run.
+   *
+   * @param angle The new angle (CCW positive)
+   */
+  void SetGyroAngleZ(units::degree_t angle);
+
+  /**
+   * Returns the axis angle (CCW positive).
+   *
+   * @param axis The IMUAxis whose angle to return.
+   * @return The axis angle (CCW positive).
+   */
+  units::degree_t GetAngle(IMUAxis axis) const;
+
+  /**
+   * Returns the axis angular rate (CCW positive).
+   *
+   * @param axis The IMUAxis whose rate to return.
+   * @return Axis angular rate (CCW positive).
+   */
+  units::degrees_per_second_t GetRate(IMUAxis axis) const;
 
   /**
    * Returns the acceleration in the X axis.
@@ -148,25 +246,60 @@
    */
   units::meters_per_second_squared_t GetAccelZ() const;
 
+  /**
+   * Returns the X-axis complementary angle.
+   */
   units::degree_t GetXComplementaryAngle() const;
 
+  /**
+   * Returns the Y-axis complementary angle.
+   */
   units::degree_t GetYComplementaryAngle() const;
 
+  /**
+   * Returns the X-axis filtered acceleration angle.
+   */
   units::degree_t GetXFilteredAccelAngle() const;
 
+  /**
+   * Returns the Y-axis filtered acceleration angle.
+   */
   units::degree_t GetYFilteredAccelAngle() const;
 
+  /**
+   * Returns which axis, kX, kY, or kZ, is set to the yaw axis.
+   *
+   * @return IMUAxis Yaw Axis
+   */
   IMUAxis GetYawAxis() const;
 
-  int SetYawAxis(IMUAxis yaw_axis);
-
-  bool IsConnected() const;
-
-  // IMU yaw axis
-  IMUAxis m_yaw_axis;
+  /**
+   * Returns which axis, kX, kY, or kZ, is set to the pitch axis.
+   *
+   * @return IMUAxis Pitch Axis
+   */
+  IMUAxis GetPitchAxis() const;
 
   /**
-   * Get the SPI port number.
+   * Returns which axis, kX, kY, or kZ, is set to the roll axis.
+   *
+   * @return IMUAxis Roll Axis
+   */
+  IMUAxis GetRollAxis() const;
+
+  /**
+   * Checks the connection status of the IMU.
+   *
+   * @return True if the IMU is connected, false otherwise.
+   */
+  bool IsConnected() const;
+
+  IMUAxis m_yaw_axis;
+  IMUAxis m_pitch_axis;
+  IMUAxis m_roll_axis;
+
+  /**
+   * Gets the SPI port number.
    *
    * @return The SPI port number.
    */
@@ -175,7 +308,7 @@
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  /* ADIS16470 Register Map Declaration */
+  // Register Map Declaration
   static constexpr uint8_t FLASH_CNT = 0x00;  // Flash memory write count
   static constexpr uint8_t DIAG_STAT =
       0x02;  // Diagnostic and operational status
@@ -276,25 +409,15 @@
   static constexpr uint8_t FLSHCNT_HIGH =
       0x7E;  // Flash update count, upper word
 
-  /* ADIS16470 Auto SPI Data Packets */
-  static constexpr uint8_t m_autospi_x_packet[16] = {
-      X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
-      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
-      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
+  // Auto SPI Data Packet to read all thrre gyro axes.
+  static constexpr uint8_t m_autospi_allangle_packet[24] = {
+      X_DELTANG_OUT, FLASH_CNT,     X_DELTANG_LOW, FLASH_CNT,     Y_DELTANG_OUT,
+      FLASH_CNT,     Y_DELTANG_LOW, FLASH_CNT,     Z_DELTANG_OUT, FLASH_CNT,
+      Z_DELTANG_LOW, FLASH_CNT,     X_GYRO_OUT,    FLASH_CNT,     Y_GYRO_OUT,
+      FLASH_CNT,     Z_GYRO_OUT,    FLASH_CNT,     X_ACCL_OUT,    FLASH_CNT,
+      Y_ACCL_OUT,    FLASH_CNT,     Z_ACCL_OUT,    FLASH_CNT};
 
-  static constexpr uint8_t m_autospi_y_packet[16] = {
-      Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
-      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
-      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
-
-  static constexpr uint8_t m_autospi_z_packet[16] = {
-      Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
-      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
-      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
-
-  /* ADIS16470 Constants */
-  static constexpr double delta_angle_sf =
-      2160.0 / 2147483648.0; /* 2160 / (2^31) */
+  static constexpr double delta_angle_sf = 2160.0 / 2147483648.0;
   static constexpr double rad_to_deg = 57.2957795;
   static constexpr double deg_to_rad = 0.0174532;
   static constexpr double grav = 9.81;
@@ -350,8 +473,10 @@
 
   void Close();
 
-  // Integrated gyro value
-  double m_integ_angle = 0.0;
+  // Integrated gyro angles.
+  double m_integ_angle_x = 0.0;
+  double m_integ_angle_y = 0.0;
+  double m_integ_angle_z = 0.0;
 
   // Instant raw outputs
   double m_gyro_rate_x = 0.0;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
index 2fe0da4..4c6d9df 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
@@ -26,17 +26,48 @@
 class ADXL345_I2C : public nt::NTSendable,
                     public wpi::SendableHelper<ADXL345_I2C> {
  public:
-  enum Range { kRange_2G = 0, kRange_4G = 1, kRange_8G = 2, kRange_16G = 3 };
-
-  enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
-
-  struct AllAxes {
-    double XAxis;
-    double YAxis;
-    double ZAxis;
+  /**
+   * Accelerometer range.
+   */
+  enum Range {
+    /// 2 Gs max.
+    kRange_2G = 0,
+    /// 4 Gs max.
+    kRange_4G = 1,
+    /// 8 Gs max.
+    kRange_8G = 2,
+    /// 16 Gs max.
+    kRange_16G = 3
   };
 
   /**
+   * Accelerometer axes.
+   */
+  enum Axes {
+    /// X axis.
+    kAxis_X = 0x00,
+    /// Y axis.
+    kAxis_Y = 0x02,
+    /// Z axis.
+    kAxis_Z = 0x04
+  };
+
+  /**
+   * Container type for accelerations from all axes.
+   */
+  struct AllAxes {
+    /// Acceleration along the X axis in g-forces.
+    double XAxis = 0.0;
+    /// Acceleration along the Y axis in g-forces.
+    double YAxis = 0.0;
+    /// Acceleration along the Z axis in g-forces.
+    double ZAxis = 0.0;
+  };
+
+  /// Default I2C device address.
+  static constexpr int kAddress = 0x1D;
+
+  /**
    * Constructs the ADXL345 Accelerometer over I2C.
    *
    * @param port          The I2C port the accelerometer is attached to
@@ -100,7 +131,7 @@
 
   void InitSendable(nt::NTSendableBuilder& builder) override;
 
- protected:
+ private:
   I2C m_i2c;
 
   hal::SimDevice m_simDevice;
@@ -109,7 +140,6 @@
   hal::SimDouble m_simY;
   hal::SimDouble m_simZ;
 
-  static constexpr int kAddress = 0x1D;
   static constexpr int kPowerCtlRegister = 0x2D;
   static constexpr int kDataFormatRegister = 0x31;
   static constexpr int kDataRegister = 0x32;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
index 3f2d8ae..305be92 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
@@ -21,14 +21,42 @@
 class ADXL345_SPI : public nt::NTSendable,
                     public wpi::SendableHelper<ADXL345_SPI> {
  public:
-  enum Range { kRange_2G = 0, kRange_4G = 1, kRange_8G = 2, kRange_16G = 3 };
+  /**
+   * Accelerometer range.
+   */
+  enum Range {
+    /// 2 Gs max.
+    kRange_2G = 0,
+    /// 4 Gs max.
+    kRange_4G = 1,
+    /// 8 Gs max.
+    kRange_8G = 2,
+    /// 16 Gs max.
+    kRange_16G = 3
+  };
 
-  enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
+  /**
+   * Accelerometer axes.
+   */
+  enum Axes {
+    /// X axis.
+    kAxis_X = 0x00,
+    /// Y axis.
+    kAxis_Y = 0x02,
+    /// Z axis.
+    kAxis_Z = 0x04
+  };
 
+  /**
+   * Container type for accelerations from all axes.
+   */
   struct AllAxes {
-    double XAxis;
-    double YAxis;
-    double ZAxis;
+    /// Acceleration along the X axis in g-forces.
+    double XAxis = 0.0;
+    /// Acceleration along the Y axis in g-forces.
+    double YAxis = 0.0;
+    /// Acceleration along the Z axis in g-forces.
+    double ZAxis = 0.0;
   };
 
   /**
@@ -93,7 +121,7 @@
 
   void InitSendable(nt::NTSendableBuilder& builder) override;
 
- protected:
+ private:
   SPI m_spi;
 
   hal::SimDevice m_simDevice;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h
index ddf6ebe..0332b31 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h
@@ -19,13 +19,40 @@
  */
 class ADXL362 : public nt::NTSendable, public wpi::SendableHelper<ADXL362> {
  public:
-  enum Range { kRange_2G = 0, kRange_4G = 1, kRange_8G = 2 };
+  /**
+   * Accelerometer range.
+   */
+  enum Range {
+    /// 2 Gs max.
+    kRange_2G = 0,
+    /// 4 Gs max.
+    kRange_4G = 1,
+    /// 8 Gs max.
+    kRange_8G = 2
+  };
 
-  enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
+  /**
+   * Accelerometer axes.
+   */
+  enum Axes {
+    /// X axis.
+    kAxis_X = 0x00,
+    /// Y axis.
+    kAxis_Y = 0x02,
+    /// Z axis.
+    kAxis_Z = 0x04
+  };
+
+  /**
+   * Container type for accelerations from all axes.
+   */
   struct AllAxes {
-    double XAxis;
-    double YAxis;
-    double ZAxis;
+    /// Acceleration along the X axis in g-forces.
+    double XAxis = 0.0;
+    /// Acceleration along the Y axis in g-forces.
+    double YAxis = 0.0;
+    /// Acceleration along the Z axis in g-forces.
+    double ZAxis = 0.0;
   };
 
  public:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h
index 198eb67..26738bb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -22,7 +22,9 @@
  * By default, the timing supports WS2812B LEDs, but is configurable using
  * SetBitTiming()
  *
- * <p>Only 1 LED driver is currently supported by the roboRIO.
+ * <p>Only 1 LED driver is currently supported by the roboRIO. However,
+ * multiple LED strips can be connected in series and controlled from the
+ * single driver.
  */
 class AddressableLED {
  public:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h
index 0c472fb..3913b40 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h
@@ -218,10 +218,8 @@
 
   void InitSendable(wpi::SendableBuilder& builder) override;
 
- protected:
-  std::shared_ptr<AnalogInput> m_analog;
-
  private:
+  std::shared_ptr<AnalogInput> m_analog;
   hal::Handle<HAL_GyroHandle> m_gyroHandle;
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h
index ccb3c9c..4151ae6 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h
@@ -33,14 +33,14 @@
   /**
    * Set the value of the analog output.
    *
-   * @param voltage The output value in Volts, from 0.0 to +5.0
+   * @param voltage The output value in Volts, from 0.0 to +5.0.
    */
   void SetVoltage(double voltage);
 
   /**
-   * Get the voltage of the analog output
+   * Get the voltage of the analog output.
    *
-   * @return The value in Volts, from 0.0 to +5.0
+   * @return The value in Volts, from 0.0 to +5.0.
    */
   double GetVoltage() const;
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h
index c706c34..0db114d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h
@@ -6,10 +6,15 @@
 
 namespace frc {
 
+/** Defines the state in which the AnalogTrigger triggers. */
 enum class AnalogTriggerType {
+  /// In window.
   kInWindow = 0,
+  /// State.
   kState = 1,
+  /// Rising Pulse.
   kRisingPulse = 2,
+  /// Falling pulse.
   kFallingPulse = 3
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
index a8f0adc..270ed80 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
@@ -17,7 +17,17 @@
 class BuiltInAccelerometer : public wpi::Sendable,
                              public wpi::SendableHelper<BuiltInAccelerometer> {
  public:
-  enum Range { kRange_2G = 0, kRange_4G = 1, kRange_8G = 2 };
+  /**
+   * Accelerometer range.
+   */
+  enum Range {
+    /// 2 Gs max.
+    kRange_2G = 0,
+    /// 4 Gs max.
+    kRange_4G = 1,
+    /// 8 Gs max.
+    kRange_8G = 2
+  };
 
   /**
    * Constructor.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h
index dc04988..1f5d168 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h
@@ -170,7 +170,10 @@
    */
   static uint64_t GetTimestampBaseTime();
 
+  /// Team manufacturer.
   static constexpr HAL_CANManufacturer kTeamManufacturer = HAL_CAN_Man_kTeamUse;
+
+  /// Team device type.
   static constexpr HAL_CANDeviceType kTeamDeviceType =
       HAL_CAN_Dev_kMiscellaneous;
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CompressorConfigType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CompressorConfigType.h
index 5a5a1c1..b933804 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CompressorConfigType.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CompressorConfigType.h
@@ -5,10 +5,17 @@
 #pragma once
 
 namespace frc {
+/**
+ * Compressor config type.
+ */
 enum class CompressorConfigType {
+  /// Disabled.
   Disabled = 0,
+  /// Digital.
   Digital = 1,
+  /// Analog.
   Analog = 2,
+  /// Hybrid.
   Hybrid = 3
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h
index 71eebca..3720dde 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h
@@ -451,17 +451,20 @@
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  protected:
-  // Makes the counter count up.
+  /// Makes the counter count up.
   std::shared_ptr<DigitalSource> m_upSource;
 
-  // Makes the counter count down.
+  /// Makes the counter count down.
   std::shared_ptr<DigitalSource> m_downSource;
 
-  // The FPGA counter object
+  /// The FPGA counter object
   hal::Handle<HAL_CounterHandle> m_counter;
 
  private:
-  int m_index = 0;  // The index of this counter.
+  /// The index of this counter.
+  int m_index = 0;
+
+  /// Distance of travel for each tick.
   double m_distancePerPulse = 1;
 
   friend class DigitalGlitchFilter;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h
index 1bbf268..d09368f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h
@@ -17,6 +17,9 @@
 class PWM;
 class PWMMotorController;
 
+/**
+ * Class for configuring Direct Memory Access (DMA) of FPGA inputs.
+ */
 class DMA {
   friend class DMASample;
 
@@ -27,32 +30,162 @@
   DMA& operator=(DMA&& other) = default;
   DMA(DMA&& other) = default;
 
+  /**
+   * Sets whether DMA is paused.
+   *
+   * @param pause True pauses DMA.
+   */
   void SetPause(bool pause);
-  void SetTimedTrigger(units::second_t seconds);
+
+  /**
+   * Sets DMA to trigger at an interval.
+   *
+   * @param period Period at which to trigger DMA.
+   */
+  void SetTimedTrigger(units::second_t period);
+
+  /**
+   * Sets number of DMA cycles to trigger.
+   *
+   * @param cycles Number of cycles.
+   */
   void SetTimedTriggerCycles(int cycles);
 
+  /**
+   * Adds position data for an encoder to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param encoder Encoder to add to DMA.
+   */
   void AddEncoder(const Encoder* encoder);
+
+  /**
+   * Adds timer data for an encoder to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param encoder Encoder to add to DMA.
+   */
   void AddEncoderPeriod(const Encoder* encoder);
 
+  /**
+   * Adds position data for an counter to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param counter Counter to add to DMA.
+   */
   void AddCounter(const Counter* counter);
+
+  /**
+   * Adds timer data for an counter to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param counter Counter to add to DMA.
+   */
   void AddCounterPeriod(const Counter* counter);
 
+  /**
+   * Adds a digital source to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param digitalSource DigitalSource to add to DMA.
+   */
   void AddDigitalSource(const DigitalSource* digitalSource);
 
+  /**
+   * Adds a digital source to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param digitalSource DigitalSource to add to DMA.
+   */
   void AddDutyCycle(const DutyCycle* digitalSource);
 
+  /**
+   * Adds an analog input to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param analogInput AnalogInput to add to DMA.
+   */
   void AddAnalogInput(const AnalogInput* analogInput);
+
+  /**
+   * Adds averaged data of an analog input to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param analogInput AnalogInput to add to DMA.
+   */
   void AddAveragedAnalogInput(const AnalogInput* analogInput);
+
+  /**
+   * Adds accumulator data of an analog input to be collected by DMA.
+   *
+   * This can only be called if DMA is not started.
+   *
+   * @param analogInput AnalogInput to add to DMA.
+   */
   void AddAnalogAccumulator(const AnalogInput* analogInput);
 
+  /**
+   * Sets an external DMA trigger.
+   *
+   * @param source the source to trigger from.
+   * @param rising trigger on rising edge.
+   * @param falling trigger on falling edge.
+   * @return the index of the trigger
+   */
   int SetExternalTrigger(DigitalSource* source, bool rising, bool falling);
+
+  /**
+   * Sets a DMA PWM edge trigger.
+   *
+   * @param pwm the PWM to trigger from.
+   * @param rising trigger on rising edge.
+   * @param falling trigger on falling edge.
+   * @return the index of the trigger
+   */
   int SetPwmEdgeTrigger(PWM* pwm, bool rising, bool falling);
+
+  /**
+   * Sets a DMA PWMMotorController edge trigger.
+   *
+   * @param pwm the PWMMotorController to trigger from.
+   * @param rising trigger on rising edge.
+   * @param falling trigger on falling edge.
+   * @return the index of the trigger
+   */
   int SetPwmEdgeTrigger(PWMMotorController* pwm, bool rising, bool falling);
 
+  /**
+   * Clear all sensors from the DMA collection list.
+   *
+   * This can only be called if DMA is not started.
+   */
   void ClearSensors();
+
+  /**
+   * Clear all external triggers from the DMA trigger list.
+   *
+   * This can only be called if DMA is not started.
+   */
   void ClearExternalTriggers();
 
+  /**
+   * Starts DMA Collection.
+   *
+   * @param queueDepth The number of objects to be able to queue.
+   */
   void Start(int queueDepth);
+
+  /**
+   * Stops DMA Collection.
+   */
   void Stop();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h
index 48a0a9e..b2bed41 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h
@@ -17,30 +17,71 @@
 #include "frc/Encoder.h"
 
 namespace frc {
+/**
+ * DMA sample.
+ */
 class DMASample : public HAL_DMASample {
  public:
+  /**
+   * DMA read status.
+   */
   enum class DMAReadStatus {
+    /// OK status.
     kOk = HAL_DMA_OK,
+    /// Timeout status.
     kTimeout = HAL_DMA_TIMEOUT,
+    /// Error status.
     kError = HAL_DMA_ERROR
   };
 
+  /**
+   * Retrieves a new DMA sample.
+   *
+   * @param dma DMA object.
+   * @param timeout Timeout for retrieval.
+   * @param remaining Number of remaining samples.
+   * @param status DMA read status.
+   */
   DMAReadStatus Update(const DMA* dma, units::second_t timeout,
                        int32_t* remaining, int32_t* status) {
     return static_cast<DMAReadStatus>(
         HAL_ReadDMA(dma->dmaHandle, this, timeout.value(), remaining, status));
   }
 
+  /**
+   * Returns the DMA sample time in microseconds.
+   *
+   * @return The DMA sample time in microseconds.
+   */
   uint64_t GetTime() const { return timeStamp; }
 
+  /**
+   * Returns the DMA sample timestamp.
+   *
+   * @return The DMA sample timestamp.
+   */
   units::second_t GetTimeStamp() const {
     return units::second_t{static_cast<double>(GetTime()) * 1.0e-6};
   }
 
+  /**
+   * Returns raw encoder value from DMA.
+   *
+   * @param encoder Encoder used for DMA.
+   * @param status DMA read status.
+   * @return Raw encoder value from DMA.
+   */
   int32_t GetEncoderRaw(const Encoder* encoder, int32_t* status) const {
     return HAL_GetDMASampleEncoderRaw(this, encoder->m_encoder, status);
   }
 
+  /**
+   * Returns encoder distance from DMA.
+   *
+   * @param encoder Encoder used for DMA.
+   * @param status DMA read status.
+   * @return Encoder distance from DMA.
+   */
   double GetEncoderDistance(const Encoder* encoder, int32_t* status) const {
     double val = GetEncoderRaw(encoder, status);
     val *= encoder->DecodingScaleFactor();
@@ -48,41 +89,97 @@
     return val;
   }
 
+  /**
+   * Returns raw encoder period from DMA.
+   *
+   * @param encoder Encoder used for DMA.
+   * @param status DMA read status.
+   * @return Raw encoder period from DMA.
+   */
   int32_t GetEncoderPeriodRaw(const Encoder* encoder, int32_t* status) const {
     return HAL_GetDMASampleEncoderPeriodRaw(this, encoder->m_encoder, status);
   }
 
+  /**
+   * Returns counter value from DMA.
+   *
+   * @param counter Counter used for DMA.
+   * @param status DMA read status.
+   * @return Counter value from DMA.
+   */
   int32_t GetCounter(const Counter* counter, int32_t* status) const {
     return HAL_GetDMASampleCounter(this, counter->m_counter, status);
   }
 
+  /**
+   * Returns counter period from DMA.
+   *
+   * @param counter Counter used for DMA.
+   * @param status DMA read status.
+   * @return Counter period from DMA.
+   */
   int32_t GetCounterPeriod(const Counter* counter, int32_t* status) const {
     return HAL_GetDMASampleCounterPeriod(this, counter->m_counter, status);
   }
 
+  /**
+   * Returns digital source value from DMA.
+   *
+   * @param digitalSource DigitalSource used for DMA.
+   * @param status DMA read status.
+   * @return DigitalSource value from DMA.
+   */
   bool GetDigitalSource(const DigitalSource* digitalSource,
                         int32_t* status) const {
     return HAL_GetDMASampleDigitalSource(
         this, digitalSource->GetPortHandleForRouting(), status);
   }
 
+  /**
+   * Returns raw analog input value from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @param status DMA read status.
+   * @return Raw analog input value from DMA.
+   */
   int32_t GetAnalogInputRaw(const AnalogInput* analogInput,
                             int32_t* status) const {
     return HAL_GetDMASampleAnalogInputRaw(this, analogInput->m_port, status);
   }
 
+  /**
+   * Returns analog input voltage from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @param status DMA read status.
+   * @return Analog input voltage from DMA.
+   */
   double GetAnalogInputVoltage(const AnalogInput* analogInput,
                                int32_t* status) {
     return HAL_GetAnalogValueToVolts(
         analogInput->m_port, GetAnalogInputRaw(analogInput, status), status);
   }
 
+  /**
+   * Returns averaged analog input raw value from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @param status DMA read status.
+   * @return Averaged analog input raw value from DMA.
+   */
   int32_t GetAveragedAnalogInputRaw(const AnalogInput* analogInput,
                                     int32_t* status) const {
     return HAL_GetDMASampleAveragedAnalogInputRaw(this, analogInput->m_port,
                                                   status);
   }
 
+  /**
+   * Returns averaged analog input voltage from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @param status DMA read status.
+   * @return Averaged analog input voltage from DMA.
+   */
   double GetAveragedAnalogInputVoltage(const AnalogInput* analogInput,
                                        int32_t* status) {
     return HAL_GetAnalogValueToVolts(
@@ -90,18 +187,40 @@
         status);
   }
 
+  /**
+   * Returns analog accumulator value from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @param count Accumulator sample count.
+   * @param value Accumulator value.
+   * @param status DMA read status.
+   */
   void GetAnalogAccumulator(const AnalogInput* analogInput, int64_t* count,
                             int64_t* value, int32_t* status) const {
     return HAL_GetDMASampleAnalogAccumulator(this, analogInput->m_port, count,
                                              value, status);
   }
 
+  /**
+   * Returns raw duty cycle output from DMA.
+   *
+   * @param dutyCycle DutyCycle used for DMA.
+   * @param status DMA read status.
+   * @return Raw duty cycle output from DMA.
+   */
   int32_t GetDutyCycleOutputRaw(const DutyCycle* dutyCycle,
                                 int32_t* status) const {
     return HAL_GetDMASampleDutyCycleOutputRaw(this, dutyCycle->m_handle,
                                               status);
   }
 
+  /**
+   * Returns duty cycle output (0-1) from DMA.
+   *
+   * @param dutyCycle DutyCycle used for DMA.
+   * @param status DMA read status.
+   * @return Duty cycle output (0-1) from DMA.
+   */
   double GetDutyCycleOutput(const DutyCycle* dutyCycle, int32_t* status) {
     return GetDutyCycleOutputRaw(dutyCycle, status) /
            static_cast<double>(dutyCycle->GetOutputScaleFactor());
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DataLogManager.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DataLogManager.h
index 336af4a..f171cd3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DataLogManager.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DataLogManager.h
@@ -17,8 +17,8 @@
  * Centralized data log that provides automatic data log file management. It
  * automatically cleans up old files when disk space is low and renames the file
  * based either on current date/time or (if available) competition match number.
- * The deta file will be saved to a USB flash drive if one is attached, or to
- * /home/lvuser otherwise.
+ * The data file will be saved to a USB flash drive in a folder named "logs" if
+ * one is attached, or to /home/lvuser/logs otherwise.
  *
  * Log files are initially named "FRC_TBD_{random}.wpilog" until the DS
  * connects. After the DS connects, the log file is renamed to
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
index f02ba5a..57c2c11 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
@@ -25,7 +25,17 @@
 class DoubleSolenoid : public wpi::Sendable,
                        public wpi::SendableHelper<DoubleSolenoid> {
  public:
-  enum Value { kOff, kForward, kReverse };
+  /**
+   * Possible values for a DoubleSolenoid.
+   */
+  enum Value {
+    /// Off position.
+    kOff,
+    /// Forward position.
+    kForward,
+    /// Reverse position.
+    kReverse
+  };
 
   /**
    * Constructs a double solenoid for a specified module of a specific module
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h
index ea568a7..b9b683f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -22,9 +22,31 @@
  */
 class DriverStation final {
  public:
-  enum Alliance { kRed, kBlue };
-  enum MatchType { kNone, kPractice, kQualification, kElimination };
+  /**
+   * The robot alliance that the robot is a part of.
+   */
+  enum Alliance {
+    /// Red alliance.
+    kRed,
+    /// Blue alliance.
+    kBlue
+  };
 
+  /**
+   * The type of robot match that the robot is part of.
+   */
+  enum MatchType {
+    /// None.
+    kNone,
+    /// Practice.
+    kPractice,
+    /// Qualification.
+    kQualification,
+    /// Elimination.
+    kElimination
+  };
+
+  /// Number of Joystick ports.
   static constexpr int kJoystickPorts = 6;
 
   /**
@@ -274,14 +296,13 @@
   static int GetReplayNumber();
 
   /**
-   * Return the alliance that the driver station says it is on from the FMS.
+   * Get the current alliance from the FMS.
    *
    * If the FMS is not connected, it is set from the team alliance setting on
    * the driver station.
    *
-   * This could return kRed or kBlue.
-   *
-   * @return The Alliance enum (kRed, kBlue or kInvalid)
+   * @return The alliance (red or blue) or an empty optional if the alliance is
+   * invalid
    */
   static std::optional<Alliance> GetAlliance();
 
@@ -334,9 +355,25 @@
    */
   static double GetBatteryVoltage();
 
+  /**
+   * Copy data from the DS task for the user. If no new data exists, it will
+   * just be returned, otherwise the data will be copied from the DS polling
+   * loop.
+   */
   static void RefreshData();
 
+  /**
+   * Registers the given handle for DS data refresh notifications.
+   *
+   * @param handle The event handle.
+   */
   static void ProvideRefreshedDataEventHandle(WPI_EventHandle handle);
+
+  /**
+   * Unregisters the given handle from DS data refresh notifications.
+   *
+   * @param handle The event handle.
+   */
   static void RemoveRefreshedDataEventHandle(WPI_EventHandle handle);
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h
index 7315e16..8d6c10a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h
@@ -42,10 +42,17 @@
   friend class DMASample;
 
  public:
+  /**
+   * Encoder indexing types.
+   */
   enum IndexingType {
+    /// Reset while the signal is high.
     kResetWhileHigh,
+    /// Reset while the signal is low.
     kResetWhileLow,
+    /// Reset on falling edge of the signal.
     kResetOnFallingEdge,
+    /// Reset on rising edge of the signal.
     kResetOnRisingEdge
   };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h
index f6db94c..aac4bec 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h
@@ -23,25 +23,55 @@
  */
 class GenericHID {
  public:
-  enum RumbleType { kLeftRumble, kRightRumble, kBothRumble };
+  /**
+   * Represents a rumble output on the Joystick.
+   */
+  enum RumbleType {
+    /// Left rumble motor.
+    kLeftRumble,
+    /// Right rumble motor.
+    kRightRumble,
+    /// Both left and right rumble motors.
+    kBothRumble
+  };
 
+  /**
+   * USB HID interface type.
+   */
   enum HIDType {
+    /// Unknown.
     kUnknown = -1,
+    /// XInputUnknown.
     kXInputUnknown = 0,
+    /// XInputGamepad.
     kXInputGamepad = 1,
+    /// XInputWheel.
     kXInputWheel = 2,
+    /// XInputArcadeStick.
     kXInputArcadeStick = 3,
+    /// XInputFlightStick.
     kXInputFlightStick = 4,
+    /// XInputDancePad.
     kXInputDancePad = 5,
+    /// XInputGuitar.
     kXInputGuitar = 6,
+    /// XInputGuitar2.
     kXInputGuitar2 = 7,
+    /// XInputDrumKit.
     kXInputDrumKit = 8,
+    /// XInputGuitar3.
     kXInputGuitar3 = 11,
+    /// XInputArcadePad.
     kXInputArcadePad = 19,
+    /// HIDJoystick.
     kHIDJoystick = 20,
+    /// HIDGamepad.
     kHIDGamepad = 21,
+    /// HIDDriving.
     kHIDDriving = 22,
+    /// HIDFlight.
     kHIDFlight = 23,
+    /// HID1stPerson.
     kHID1stPerson = 24
   };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h
index 9489fcf..07f3d4a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h
@@ -22,7 +22,15 @@
  */
 class I2C {
  public:
-  enum Port { kOnboard = 0, kMXP };
+  /**
+   * I2C connection ports.
+   */
+  enum Port {
+    /// Onboard I2C port.
+    kOnboard = 0,
+    /// MXP (roboRIO MXP) I2C port.
+    kMXP
+  };
 
   /**
    * Constructor.
@@ -37,7 +45,18 @@
   I2C(I2C&&) = default;
   I2C& operator=(I2C&&) = default;
 
+  /**
+   * Returns I2C port.
+   *
+   * @return I2C port.
+   */
   Port GetPort() const;
+
+  /**
+   * Returns I2C device address.
+   *
+   * @return I2C device address.
+   */
   int GetDeviceAddress() const;
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
index 42b0c71..be7687d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
@@ -215,7 +215,7 @@
   /**
    * Sets whether LiveWindow operation is enabled during test mode.
    *
-   * @param testLW True to enable, false to disable. Defaults to true.
+   * @param testLW True to enable, false to disable. Defaults to false.
    * @throws if called in test mode.
    */
   void EnableLiveWindowInTest(bool testLW);
@@ -243,6 +243,9 @@
   IterativeRobotBase(IterativeRobotBase&&) = default;
   IterativeRobotBase& operator=(IterativeRobotBase&&) = default;
 
+  /**
+   * Loop function.
+   */
   void LoopFunc();
 
  private:
@@ -252,7 +255,7 @@
   units::second_t m_period;
   Watchdog m_watchdog;
   bool m_ntFlushEnabled = true;
-  bool m_lwEnabledInTest = true;
+  bool m_lwEnabledInTest = false;
   bool m_calledDsConnected = false;
 
   void PrintLoopOverrunMessage();
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h
index a955718..d9c4144 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h
@@ -22,14 +22,42 @@
  */
 class Joystick : public GenericHID {
  public:
+  /// Default X axis channel.
   static constexpr int kDefaultXChannel = 0;
+  /// Default Y axis channel.
   static constexpr int kDefaultYChannel = 1;
+  /// Default Z axis channel.
   static constexpr int kDefaultZChannel = 2;
+  /// Default twist axis channel.
   static constexpr int kDefaultTwistChannel = 2;
+  /// Default throttle axis channel.
   static constexpr int kDefaultThrottleChannel = 3;
 
-  enum AxisType { kXAxis, kYAxis, kZAxis, kTwistAxis, kThrottleAxis };
-  enum ButtonType { kTriggerButton, kTopButton };
+  /**
+   * Represents an analog axis on a joystick.
+   */
+  enum AxisType {
+    /// X axis.
+    kXAxis,
+    /// Y axis.
+    kYAxis,
+    /// Z axis.
+    kZAxis,
+    /// Twist axis.
+    kTwistAxis,
+    /// Throttle axis.
+    kThrottleAxis
+  };
+
+  /**
+   * Represents a digital button on a joystick.
+   */
+  enum ButtonType {
+    /// kTrigger.
+    kTriggerButton,
+    /// kTop.
+    kTopButton
+  };
 
   /**
    * Construct an instance of a joystick.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h
index a17cdc0..efe7806 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h
@@ -93,12 +93,15 @@
    */
   static void CheckMotors();
 
+  /**
+   * Called to stop the motor when the timeout expires.
+   */
   virtual void StopMotor() = 0;
 
   /**
-   * The return value from this method is printed out when an error occurs
+   * Returns a description to print when an error occurs.
    *
-   * This method must not throw!
+   * @return Description to print when an error occurs.
    */
   virtual std::string GetDescription() const = 0;
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS4Controller.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS4Controller.h
index 9487634..881f856 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS4Controller.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS4Controller.h
@@ -11,10 +11,14 @@
 /**
  * Handle input from PS4 controllers connected to the Driver Station.
  *
- * <p>This class handles PS4 input that comes from the Driver Station. Each time
+ * This class handles PS4 input that comes from the Driver Station. Each time
  * a value is requested the most recent value is returned. There is a single
  * class instance for each controller and the mapping of ports to hardware
  * buttons depends on the code in the Driver Station.
+ *
+ * Only first party controllers from Sony are guaranteed to have the correct
+ * mapping, and only through the official NI DS. Sim is not guaranteed to have
+ * the same mapping, as well as any 3rd party controllers.
  */
 class PS4Controller : public GenericHID {
  public:
@@ -499,29 +503,55 @@
    */
   BooleanEvent Touchpad(EventLoop* loop) const;
 
+  /**
+   * Represents a digital button on a PS4Controller.
+   */
   struct Button {
+    /// Square button.
     static constexpr int kSquare = 1;
+    /// X button.
     static constexpr int kCross = 2;
+    /// Circle button.
     static constexpr int kCircle = 3;
+    /// Triangle button.
     static constexpr int kTriangle = 4;
+    /// Left Trigger 1 button.
     static constexpr int kL1 = 5;
+    /// Right Trigger 1 button.
     static constexpr int kR1 = 6;
+    /// Left Trigger 2 button.
     static constexpr int kL2 = 7;
+    /// Right Trigger 2 button.
     static constexpr int kR2 = 8;
+    /// Share button.
     static constexpr int kShare = 9;
+    /// Option button.
     static constexpr int kOptions = 10;
+    /// Left stick button.
     static constexpr int kL3 = 11;
+    /// Right stick button.
     static constexpr int kR3 = 12;
+    /// PlayStation button.
     static constexpr int kPS = 13;
+    /// Touchpad click button.
     static constexpr int kTouchpad = 14;
   };
 
+  /**
+   * Represents an axis on a PS4Controller.
+   */
   struct Axis {
+    /// Left X axis.
     static constexpr int kLeftX = 0;
+    /// Left Y axis.
     static constexpr int kLeftY = 1;
+    /// Right X axis.
     static constexpr int kRightX = 2;
+    /// Right Y axis.
     static constexpr int kRightY = 5;
+    /// Left Trigger 2.
     static constexpr int kL2 = 3;
+    /// Right Trigger 2.
     static constexpr int kR2 = 4;
   };
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS5Controller.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS5Controller.h
index 2a24f5e..e9d5235 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS5Controller.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS5Controller.h
@@ -11,10 +11,14 @@
 /**
  * Handle input from PS5 controllers connected to the Driver Station.
  *
- * <p>This class handles PS5 input that comes from the Driver Station. Each time
+ * This class handles PS5 input that comes from the Driver Station. Each time
  * a value is requested the most recent value is returned. There is a single
  * class instance for each controller and the mapping of ports to hardware
  * buttons depends on the code in the Driver Station.
+ *
+ * Only first party controllers from Sony are guaranteed to have the correct
+ * mapping, and only through the official NI DS. Sim is not guaranteed to have
+ * the same mapping, as well as any 3rd party controllers.
  */
 class PS5Controller : public GenericHID {
  public:
@@ -499,30 +503,56 @@
    */
   BooleanEvent Touchpad(EventLoop* loop) const;
 
+  /**
+   * Represents a digital button on a PS5Controller.
+   */
   struct Button {
-    static constexpr int kSquare = 3;
-    static constexpr int kCross = 1;
-    static constexpr int kCircle = 2;
+    /// Square button.
+    static constexpr int kSquare = 1;
+    /// X button.
+    static constexpr int kCross = 2;
+    /// Circle button.
+    static constexpr int kCircle = 3;
+    /// Triangle button.
     static constexpr int kTriangle = 4;
+    /// Left trigger 1 button.
     static constexpr int kL1 = 5;
+    /// Right trigger 1 button.
     static constexpr int kR1 = 6;
+    /// Left trigger 2 button.
     static constexpr int kL2 = 7;
+    /// Right trigger 2 button.
     static constexpr int kR2 = 8;
+    /// Create button.
     static constexpr int kCreate = 9;
+    /// Options button.
     static constexpr int kOptions = 10;
-    static constexpr int kL3 = 12;
-    static constexpr int kR3 = 13;
-    static constexpr int kPS = 11;
+    /// Left stick button.
+    static constexpr int kL3 = 11;
+    /// Right stick button.
+    static constexpr int kR3 = 12;
+    /// PlayStation button.
+    static constexpr int kPS = 13;
+    /// Touchpad click button.
     static constexpr int kTouchpad = 14;
   };
 
+  /**
+   * Represents an axis on a PS5Controller.
+   */
   struct Axis {
+    /// Left X axis.
     static constexpr int kLeftX = 0;
+    /// Left Y axis.
     static constexpr int kLeftY = 1;
-    static constexpr int kRightX = 3;
-    static constexpr int kRightY = 4;
-    static constexpr int kL2 = 2;
-    static constexpr int kR2 = 5;
+    /// Right X axis.
+    static constexpr int kRightX = 2;
+    /// Right Y axis.
+    static constexpr int kRightY = 5;
+    /// Left Trigger 2.
+    static constexpr int kL2 = 3;
+    /// Right Trigger 2.
+    static constexpr int kR2 = 4;
   };
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h
index 0871c9c..c1f162e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h
@@ -144,6 +144,9 @@
    */
   void SetPeriodMultiplier(PeriodMultiplier mult);
 
+  /**
+   * Latches PWM to zero.
+   */
   void SetZeroLatch();
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticHub.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticHub.h
index b69b3d5..790a9d0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticHub.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticHub.h
@@ -7,6 +7,7 @@
 #include <memory>
 
 #include <hal/Types.h>
+#include <units/pressure.h>
 #include <wpi/DenseMap.h>
 #include <wpi/mutex.h>
 
@@ -157,6 +158,15 @@
     uint32_t Brownout : 1;
     uint32_t CanWarning : 1;
     uint32_t HardwareFault : 1;
+
+    /**
+     * Gets whether there is a fault at the specified channel.
+     * @param channel Channel to check for faults.
+     * @return True if a a fault exists at the channel, otherwise false.
+     * @throws A ChannelIndexOutOfRange error if the provided channel is outside
+     * of the range supported by the hardware.
+     */
+    bool GetChannelFault(int channel) const;
   };
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsBase.h
index 59f899f..52ae9b0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsBase.h
@@ -18,6 +18,10 @@
 class Solenoid;
 class DoubleSolenoid;
 class Compressor;
+
+/**
+ * Base class for pneumatics devices.
+ */
 class PneumaticsBase {
  public:
   virtual ~PneumaticsBase() = default;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h
index 7f70662..4fb225a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h
@@ -5,5 +5,13 @@
 #pragma once
 
 namespace frc {
-enum class PneumaticsModuleType { CTREPCM, REVPH };
+/**
+ * Pneumatics module type.
+ */
+enum class PneumaticsModuleType {
+  /// CTRE PCM.
+  CTREPCM,
+  /// REV PH.
+  REVPH
+};
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistribution.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistribution.h
index 763e6bf..4a6eb4f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistribution.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistribution.h
@@ -17,8 +17,18 @@
 class PowerDistribution : public wpi::Sendable,
                           public wpi::SendableHelper<PowerDistribution> {
  public:
+  /// Default module number.
   static constexpr int kDefaultModule = -1;
-  enum class ModuleType { kCTRE = 1, kRev = 2 };
+
+  /**
+   * Power distribution module type.
+   */
+  enum class ModuleType {
+    /// CTRE (Cross The Road Electronics) CTRE Power Distribution Panel (PDP).
+    kCTRE = 1,
+    /// REV Power Distribution Hub (PDH).
+    kRev = 2
+  };
 
   /**
    * Constructs a PowerDistribution object.
@@ -159,8 +169,22 @@
     uint32_t Brownout : 1;
     uint32_t CanWarning : 1;
     uint32_t HardwareFault : 1;
+
+    /**
+     * Gets whether there is a breaker fault at a specified channel.
+     * @param channel Channel to check for faults.
+     * @return If there is a breaker fault.
+     * @throws A ChannelIndexOutOfRange error if the given int is outside of the
+     * range supported by the hardware.
+     */
+    bool GetBreakerFault(int channel) const;
   };
 
+  /**
+   * Returns the power distribution faults.
+   *
+   * @return The power distribution faults.
+   */
   Faults GetFaults() const;
 
   struct StickyFaults {
@@ -192,8 +216,23 @@
     uint32_t CanWarning : 1;
     uint32_t CanBusOff : 1;
     uint32_t HasReset : 1;
+
+    /**
+     * Gets whether there is a sticky breaker fault at the specified channel.
+     * @param channel Index to check for sticky faults.
+     * @return True if there is a sticky breaker fault at the channel, otherwise
+     * false.
+     * @throws A ChannelIndexOutOfRange error if the provided channel is outside
+     * of the range supported by the hardware.
+     */
+    bool GetBreakerFault(int channel) const;
   };
 
+  /**
+   * Returns the power distribution sticky faults.
+   *
+   * @return The power distribution sticky faults.
+   */
   StickyFaults GetStickyFaults() const;
 
   void InitSendable(wpi::SendableBuilder& builder) override;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h
index 4765c64..d08a000 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h
@@ -31,8 +31,31 @@
               public wpi::Sendable,
               public wpi::SendableHelper<Relay> {
  public:
-  enum Value { kOff, kOn, kForward, kReverse };
-  enum Direction { kBothDirections, kForwardOnly, kReverseOnly };
+  /**
+   * The state to drive a Relay to.
+   */
+  enum Value {
+    /// Off.
+    kOff,
+    /// On.
+    kOn,
+    /// Forward.
+    kForward,
+    /// Reverse.
+    kReverse
+  };
+
+  /**
+   * The Direction(s) that a relay is configured to operate in.
+   */
+  enum Direction {
+    /// Both directions are valid.
+    kBothDirections,
+    /// Only forward is valid.
+    kForwardOnly,
+    /// Only reverse is valid.
+    kReverseOnly
+  };
 
   /**
    * Relay constructor given a channel.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h
index 504ac81..6dff369 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -189,7 +189,9 @@
   bool IsTestEnabled() const;
 
   /**
-   * Gets the ID of the main robot thread.
+   * Returns the main thread ID.
+   *
+   * @return The main thread ID.
    */
   static std::thread::id GetThreadId();
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h
index bf65a8c..5d8ddfb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h
@@ -21,6 +21,8 @@
   int transmitErrorCount;
 };
 
+enum RadioLEDState { kOff = 0, kGreen = 1, kRed = 2, kOrange = 3 };
+
 class RobotController {
  public:
   RobotController() = delete;
@@ -81,6 +83,10 @@
   /**
    * Get the state of the "USER" button on the roboRIO.
    *
+   * @warning the User Button is used to stop user programs from automatically
+   * loading if it is held for more then 5 seconds. Because of this, it's not
+   * recommended to be used by teams for any other purpose.
+   *
    * @return True if the button is currently pressed down
    */
   static bool GetUserButton();
@@ -275,6 +281,23 @@
   static units::celsius_t GetCPUTemp();
 
   /**
+   * Set the state of the "Radio" LED. On the RoboRIO, this writes to sysfs, so
+   * this function should not be called multiple times per loop cycle to avoid
+   * overruns.
+   * @param state The state to set the LED to.
+   */
+  static void SetRadioLEDState(RadioLEDState state);
+
+  /**
+   * Get the state of the "Radio" LED. On the RoboRIO, this reads from sysfs, so
+   * this function should not be called multiple times per loop cycle to avoid
+   * overruns.
+   *
+   * @return The state of the LED.
+   */
+  static RadioLEDState GetRadioLEDState();
+
+  /**
    * Get the current status of the CAN bus.
    *
    * @return The status of the CAN bus
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h
index 0489b9b..ec8700e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h
@@ -6,15 +6,53 @@
 
 namespace frc {
 
+/**
+ * Robot state utility functions.
+ */
 class RobotState {
  public:
   RobotState() = delete;
 
+  /**
+   * Returns true if the robot is disabled.
+   *
+   * @return True if the robot is disabled.
+   */
   static bool IsDisabled();
+
+  /**
+   * Returns true if the robot is enabled.
+   *
+   * @return True if the robot is enabled.
+   */
   static bool IsEnabled();
+
+  /**
+   * Returns true if the robot is E-stopped.
+   *
+   * @return True if the robot is E-stopped.
+   */
   static bool IsEStopped();
+
+  /**
+   * Returns true if the robot is in teleop mode.
+   *
+   * @return True if the robot is in teleop mode.
+   */
   static bool IsTeleop();
+
+  /**
+   * Returns true if the robot is in autonomous mode.
+   *
+   * @return True if the robot is in autonomous mode.
+   */
   static bool IsAutonomous();
+
+  /**
+   * Returns true if the robot is in test mode.
+   *
+   * @return True if the robot is in test mode.
+   */
   static bool IsTest();
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RuntimeType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RuntimeType.h
index c3a8a0b..2be444a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RuntimeType.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RuntimeType.h
@@ -5,5 +5,15 @@
 #pragma once
 
 namespace frc {
-enum RuntimeType { kRoboRIO, kRoboRIO2, kSimulation };
+/**
+ * Runtime type.
+ */
+enum RuntimeType {
+  /// roboRIO 1.0.
+  kRoboRIO,
+  /// roboRIO 2.0.
+  kRoboRIO2,
+  /// Simulation runtime.
+  kSimulation
+};
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h
index 46061d4..af29479 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h
@@ -25,14 +25,34 @@
  */
 class SPI {
  public:
-  enum Port { kOnboardCS0 = 0, kOnboardCS1, kOnboardCS2, kOnboardCS3, kMXP };
+  /**
+   * SPI port.
+   */
+  enum Port {
+    /// Onboard SPI bus port CS0.
+    kOnboardCS0 = 0,
+    /// Onboard SPI bus port CS1.
+    kOnboardCS1,
+    /// Onboard SPI bus port CS2.
+    kOnboardCS2,
+    /// Onboard SPI bus port CS3.
+    kOnboardCS3,
+    /// MXP (roboRIO MXP) SPI bus port.
+    kMXP
+  };
+
+  /**
+   * SPI mode.
+   */
   enum Mode {
-    kMode0 = HAL_SPI_kMode0, /*!< Clock idle low, data sampled on rising edge */
-    kMode1 =
-        HAL_SPI_kMode1, /*!< Clock idle low, data sampled on falling edge */
-    kMode2 =
-        HAL_SPI_kMode2, /*!< Clock idle high, data sampled on falling edge */
-    kMode3 = HAL_SPI_kMode3 /*!< Clock idle high, data sampled on rising edge */
+    /// Clock idle low, data sampled on rising edge.
+    kMode0 = HAL_SPI_kMode0,
+    /// Clock idle low, data sampled on falling edge.
+    kMode1 = HAL_SPI_kMode1,
+    /// Clock idle high, data sampled on falling edge.
+    kMode2 = HAL_SPI_kMode2,
+    /// Clock idle high, data sampled on rising edge.
+    kMode3 = HAL_SPI_kMode3
   };
 
   /**
@@ -47,6 +67,11 @@
   SPI(SPI&&) = default;
   SPI& operator=(SPI&&) = default;
 
+  /**
+   * Returns the SPI port.
+   *
+   * @return The SPI port.
+   */
   Port GetPort() const;
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h
index dcc0a2e..b7784da 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h
@@ -26,30 +26,73 @@
  */
 class SerialPort {
  public:
+  /**
+   * Serial port.
+   */
+  enum Port {
+    /// Onboard serial port on the roboRIO.
+    kOnboard = 0,
+    /// MXP (roboRIO MXP) serial port.
+    kMXP = 1,
+    /// USB serial port (same as KUSB1).
+    kUSB = 2,
+    /// USB serial port 1.
+    kUSB1 = 2,
+    /// USB serial port 2.
+    kUSB2 = 3
+  };
+
+  /**
+   * Represents the parity to use for serial communications.
+   */
   enum Parity {
+    /// No parity.
     kParity_None = 0,
+    /// Odd parity.
     kParity_Odd = 1,
+    /// Even parity.
     kParity_Even = 2,
+    /// Parity bit always on.
     kParity_Mark = 3,
+    /// Parity bit always off.
     kParity_Space = 4
   };
 
+  /**
+   * Represents the number of stop bits to use for Serial Communication.
+   */
   enum StopBits {
+    /// One stop bit.
     kStopBits_One = 10,
+    /// One and a half stop bits.
     kStopBits_OnePointFive = 15,
+    /// Two stop bits.
     kStopBits_Two = 20
   };
 
+  /**
+   * Represents what type of flow control to use for serial communication.
+   */
   enum FlowControl {
+    /// No flow control.
     kFlowControl_None = 0,
+    /// XON/XOFF flow control.
     kFlowControl_XonXoff = 1,
+    /// RTS/CTS flow control.
     kFlowControl_RtsCts = 2,
+    /// DTS/DSR flow control.
     kFlowControl_DtrDsr = 4
   };
 
-  enum WriteBufferMode { kFlushOnAccess = 1, kFlushWhenFull = 2 };
-
-  enum Port { kOnboard = 0, kMXP = 1, kUSB = 2, kUSB1 = 2, kUSB2 = 3 };
+  /**
+   * Represents which type of buffer mode to use when writing to a serial port.
+   */
+  enum WriteBufferMode {
+    /// Flush the buffer on each access.
+    kFlushOnAccess = 1,
+    /// Flush the buffer when it is full.
+    kFlushWhenFull = 2
+  };
 
   /**
    * Create an instance of a Serial Port class.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h
index 9383641..76b39a0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h
@@ -19,6 +19,11 @@
 class Servo : public PWM {
  public:
   /**
+   * Constructor.
+   *
+   * By default, 2.4 ms is used as the max PWM value and 0.6 ms is used as the
+   * min PWM value.
+   *
    * @param channel The PWM channel to which the servo is attached. 0-9 are
    *                on-board, 10-19 are on the MXP port
    */
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/StadiaController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/StadiaController.h
new file mode 100644
index 0000000..cc9dbae
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/StadiaController.h
@@ -0,0 +1,568 @@
+// 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 "frc/GenericHID.h"
+
+namespace frc {
+
+/**
+ * Handle input from Stadia controllers connected to the Driver
+ * Station.
+ *
+ * This class handles Stadia input that comes from the Driver Station. Each time
+ * a value is requested the most recent value is returned. There is a single
+ * class instance for each controller and the mapping of ports to hardware
+ * buttons depends on the code in the Driver Station.
+ */
+class StadiaController : public GenericHID {
+ public:
+  /**
+   * Construct an instance of a Stadia controller.
+   *
+   * The controller index is the USB port on the Driver Station.
+   *
+   * @param port The port on the Driver Station that the controller is plugged
+   *             into (0-5).
+   */
+  explicit StadiaController(int port);
+
+  ~StadiaController() override = default;
+
+  StadiaController(StadiaController&&) = default;
+  StadiaController& operator=(StadiaController&&) = default;
+
+  /**
+   * Get the X axis value of left side of the controller.
+   *
+   * @return the axis value
+   */
+  double GetLeftX() const;
+
+  /**
+   * Get the X axis value of right side of the controller.
+   *
+   * @return the axis value
+   */
+  double GetRightX() const;
+
+  /**
+   * Get the Y axis value of left side of the controller.
+   *
+   * @return the axis value
+   */
+  double GetLeftY() const;
+
+  /**
+   * Get the Y axis value of right side of the controller.
+   *
+   * @return the axis value
+   */
+  double GetRightY() const;
+
+  /**
+   * Read the value of the left bumper (LB) button on the controller.
+   *
+   * @return the state of the button
+   */
+  bool GetLeftBumper() const;
+
+  /**
+   * Read the value of the right bumper (RB) button on the controller.
+   *
+   * @return the state of the button
+   */
+  bool GetRightBumper() const;
+
+  /**
+   * Whether the left bumper (LB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check
+   */
+  bool GetLeftBumperPressed();
+
+  /**
+   * Whether the right bumper (RB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check
+   */
+  bool GetRightBumperPressed();
+
+  /**
+   * Whether the left bumper (LB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetLeftBumperReleased();
+
+  /**
+   * Whether the right bumper (RB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetRightBumperReleased();
+
+  /**
+   * Constructs an event instance around the left bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left bumper's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent LeftBumper(EventLoop* loop) const;
+
+  /**
+   * Constructs an event instance around the right bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right bumper's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent RightBumper(EventLoop* loop) const;
+
+  /**
+   * Read the value of the left stick button (LSB) on the controller.
+   *
+   * @return the state of the button
+   */
+  bool GetLeftStickButton() const;
+
+  /**
+   * Read the value of the right stick button (RSB) on the controller.
+   *
+   * @return the state of the button
+   */
+  bool GetRightStickButton() const;
+
+  /**
+   * Whether the left stick button (LSB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetLeftStickButtonPressed();
+
+  /**
+   * Whether the right stick button (RSB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check
+   */
+  bool GetRightStickButtonPressed();
+
+  /**
+   * Whether the left stick button (LSB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetLeftStickButtonReleased();
+
+  /**
+   * Whether the right stick button (RSB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetRightStickButtonReleased();
+
+  /**
+   * Constructs an event instance around the left stick's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left stick's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent LeftStick(EventLoop* loop) const;
+
+  /**
+   * Constructs an event instance around the right stick's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right stick's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent RightStick(EventLoop* loop) const;
+
+  /**
+   * Read the value of the A button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetAButton() const;
+
+  /**
+   * Whether the A button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetAButtonPressed();
+
+  /**
+   * Whether the A button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetAButtonReleased();
+
+  /**
+   * Constructs an event instance around the A button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the A button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent A(EventLoop* loop) const;
+
+  /**
+   * Read the value of the B button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetBButton() const;
+
+  /**
+   * Whether the B button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetBButtonPressed();
+
+  /**
+   * Whether the B button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetBButtonReleased();
+
+  /**
+   * Constructs an event instance around the B button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the B button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent B(EventLoop* loop) const;
+
+  /**
+   * Read the value of the X button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetXButton() const;
+
+  /**
+   * Whether the X button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetXButtonPressed();
+
+  /**
+   * Whether the X button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetXButtonReleased();
+
+  /**
+   * Constructs an event instance around the X button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the X button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent X(EventLoop* loop) const;
+
+  /**
+   * Read the value of the Y button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetYButton() const;
+
+  /**
+   * Whether the Y button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetYButtonPressed();
+
+  /**
+   * Whether the Y button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetYButtonReleased();
+
+  /**
+   * Constructs an event instance around the Y button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the Y button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent Y(EventLoop* loop) const;
+
+  /**
+   * Read the value of the ellipses button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetEllipsesButton() const;
+
+  /**
+   * Whether the ellipses button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetEllipsesButtonPressed();
+
+  /**
+   * Whether the ellipses button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetEllipsesButtonReleased();
+
+  /**
+   * Constructs an event instance around the ellipses button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the ellipses button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent Ellipses(EventLoop* loop) const;
+
+  /**
+   * Read the value of the hamburger button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetHamburgerButton() const;
+
+  /**
+   * Whether the hamburger button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetHamburgerButtonPressed();
+
+  /**
+   * Whether the hamburger button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetHamburgerButtonReleased();
+
+  /**
+   * Constructs an event instance around the hamburger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the hamburger button's digital
+   * signal attached to the given loop.
+   */
+  BooleanEvent Hamburger(EventLoop* loop) const;
+
+  /**
+   * Read the value of the stadia button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetStadiaButton() const;
+
+  /**
+   * Whether the stadia button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetStadiaButtonPressed();
+
+  /**
+   * Whether the stadia button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetStadiaButtonReleased();
+
+  /**
+   * Constructs an event instance around the stadia button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the stadia button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent Stadia(EventLoop* loop) const;
+
+  /**
+   * Read the value of the google button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetGoogleButton() const;
+
+  /**
+   * Whether the google button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetGoogleButtonPressed();
+
+  /**
+   * Whether the google button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetGoogleButtonReleased();
+
+  /**
+   * Constructs an event instance around the google button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the google button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent Google(EventLoop* loop) const;
+
+  /**
+   * Read the value of the frame button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetFrameButton() const;
+
+  /**
+   * Whether the frame button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetFrameButtonPressed();
+
+  /**
+   * Whether the frame button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetFrameButtonReleased();
+
+  /**
+   * Constructs an event instance around the frame button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the frame button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent Frame(EventLoop* loop) const;
+
+  /**
+   * Read the value of the left trigger button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetLeftTriggerButton() const;
+
+  /**
+   * Whether the left trigger button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetLeftTriggerButtonPressed();
+
+  /**
+   * Whether the left trigger button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetLeftTriggerButtonReleased();
+
+  /**
+   * Constructs an event instance around the left trigger button's digital
+   * signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left trigger button's digital
+   * signal attached to the given loop.
+   */
+  BooleanEvent LeftTrigger(EventLoop* loop) const;
+
+  /**
+   * Read the value of the right trigger button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetRightTriggerButton() const;
+
+  /**
+   * Whether the right trigger button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetRightTriggerButtonPressed();
+
+  /**
+   * Whether the right trigger button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetRightTriggerButtonReleased();
+
+  /**
+   * Constructs an event instance around the right trigger button's digital
+   * signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right trigger button's digital
+   * signal attached to the given loop.
+   */
+  BooleanEvent RightTrigger(EventLoop* loop) const;
+
+  /**
+   * Represents a digital button on a StadiaController.
+   */
+  struct Button {
+    /// A button.
+    static constexpr int kA = 1;
+    /// B button.
+    static constexpr int kB = 2;
+    /// X button.
+    static constexpr int kX = 3;
+    /// Y button.
+    static constexpr int kY = 4;
+    /// Left bumper button.
+    static constexpr int kLeftBumper = 5;
+    /// Right bumper button.
+    static constexpr int kRightBumper = 6;
+    /// Left stick button.
+    static constexpr int kLeftStick = 7;
+    /// Right stick button.
+    static constexpr int kRightStick = 8;
+    /// Ellipses button.
+    static constexpr int kEllipses = 9;
+    /// Hamburger button.
+    static constexpr int kHamburger = 10;
+    /// Stadia button.
+    static constexpr int kStadia = 11;
+    /// Right trigger button.
+    static constexpr int kRightTrigger = 12;
+    /// Left trigger button.
+    static constexpr int kLeftTrigger = 13;
+    /// Google button.
+    static constexpr int kGoogle = 14;
+    /// Frame button.
+    static constexpr int kFrame = 15;
+  };
+
+  /**
+   * Represents an axis on a StadiaController.
+   */
+  struct Axis {
+    /// Left X axis.
+    static constexpr int kLeftX = 0;
+    /// Right X axis.
+    static constexpr int kRightX = 4;
+    /// Left Y axis.
+    static constexpr int kLeftY = 1;
+    /// Right Y axis.
+    static constexpr int kRightY = 5;
+  };
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SynchronousInterrupt.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SynchronousInterrupt.h
index fbe0fca..3e396e8 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SynchronousInterrupt.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SynchronousInterrupt.h
@@ -25,9 +25,13 @@
    * Event trigger combinations for a synchronous interrupt.
    */
   enum WaitResult {
+    /// Timeout event.
     kTimeout = 0x0,
+    /// Rising edge event.
     kRisingEdge = 0x1,
+    /// Falling edge event.
     kFallingEdge = 0x100,
+    /// Both rising and falling edge events.
     kBoth = 0x101,
   };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h
index f32e748..dcc510a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -29,6 +29,7 @@
  */
 class TimedRobot : public IterativeRobotBase {
  public:
+  /// Default loop period.
   static constexpr auto kDefaultPeriod = 20_ms;
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h
index 137a5b7..f06392c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h
@@ -90,6 +90,11 @@
   Ultrasonic(Ultrasonic&&) = default;
   Ultrasonic& operator=(Ultrasonic&&) = default;
 
+  /**
+   * Returns the echo channel.
+   *
+   * @return The echo channel.
+   */
   int GetEchoChannel() const;
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h
index 3caba1a..4b720c1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h
@@ -16,6 +16,10 @@
  * value is requested the most recent value is returned. There is a single class
  * instance for each controller and the mapping of ports to hardware buttons
  * depends on the code in the Driver Station.
+ *
+ * Only first party controllers from Microsoft are guaranteed to have the
+ * correct mapping, and only through the official NI DS. Sim is not guaranteed
+ * to have the same mapping, as well as any 3rd party controllers.
  */
 class XboxController : public GenericHID {
  public:
@@ -422,25 +426,43 @@
    */
   BooleanEvent RightTrigger(EventLoop* loop) const;
 
+  /** Represents a digital button on an XboxController. */
   struct Button {
+    /// Left bumper.
     static constexpr int kLeftBumper = 5;
+    /// Right bumper.
     static constexpr int kRightBumper = 6;
+    /// Left stick.
     static constexpr int kLeftStick = 9;
+    /// Right stick.
     static constexpr int kRightStick = 10;
+    /// A.
     static constexpr int kA = 1;
+    /// B.
     static constexpr int kB = 2;
+    /// X.
     static constexpr int kX = 3;
+    /// Y.
     static constexpr int kY = 4;
+    /// Back.
     static constexpr int kBack = 7;
+    /// Start.
     static constexpr int kStart = 8;
   };
 
+  /** Represents an axis on an XboxController. */
   struct Axis {
+    /// Left X.
     static constexpr int kLeftX = 0;
+    /// Right X.
     static constexpr int kRightX = 4;
+    /// Left Y.
     static constexpr int kLeftY = 1;
+    /// Right Y.
     static constexpr int kRightY = 5;
+    /// Left trigger.
     static constexpr int kLeftTrigger = 2;
+    /// Right trigger.
     static constexpr int kRightTrigger = 3;
   };
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h
index 508117d..4f5a039 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h
@@ -5,10 +5,17 @@
 #pragma once
 
 namespace frc {
+/**
+ * Edge configuration.
+ */
 enum class EdgeConfiguration {
+  /// No edge configuration (neither rising nor falling).
   kNone = 0,
+  /// Rising edge configuration.
   kRisingEdge = 0x1,
+  /// Falling edge configuration.
   kFallingEdge = 0x2,
+  /// Both rising and falling edges configuration.
   kBoth = 0x3
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 2e701a3..ccda644 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -4,8 +4,10 @@
 
 #pragma once
 
+#include <functional>
 #include <string>
 
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
@@ -20,43 +22,9 @@
  * the Kit of Parts drive base, "tank drive", or West Coast Drive.
  *
  * These drive bases typically have drop-center / skid-steer with two or more
- * wheels per side (e.g., 6WD or 8WD). This class takes a MotorController per
- * side. For four and six motor drivetrains, construct and pass in
- * MotorControllerGroup instances as follows.
- *
- * Four motor drivetrain:
- * @code{.cpp}
- * class Robot {
- *  public:
- *   frc::PWMVictorSPX m_frontLeft{1};
- *   frc::PWMVictorSPX m_rearLeft{2};
- *   frc::MotorControllerGroup m_left{m_frontLeft, m_rearLeft};
- *
- *   frc::PWMVictorSPX m_frontRight{3};
- *   frc::PWMVictorSPX m_rearRight{4};
- *   frc::MotorControllerGroup m_right{m_frontRight, m_rearRight};
- *
- *   frc::DifferentialDrive m_drive{m_left, m_right};
- * };
- * @endcode
- *
- * Six motor drivetrain:
- * @code{.cpp}
- * class Robot {
- *  public:
- *   frc::PWMVictorSPX m_frontLeft{1};
- *   frc::PWMVictorSPX m_midLeft{2};
- *   frc::PWMVictorSPX m_rearLeft{3};
- *   frc::MotorControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
- *
- *   frc::PWMVictorSPX m_frontRight{4};
- *   frc::PWMVictorSPX m_midRight{5};
- *   frc::PWMVictorSPX m_rearRight{6};
- *   frc::MotorControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
- *
- *   frc::DifferentialDrive m_drive{m_left, m_right};
- * };
- * @endcode
+ * wheels per side (e.g., 6WD or 8WD). This class takes a setter per side. For
+ * four and six motor drivetrains, use CAN motor controller followers or
+ * PWMMotorController::AddFollower().
  *
  * A differential drive robot has left and right wheels separated by an
  * arbitrary width.
@@ -97,17 +65,39 @@
    * Uses normalized voltage [-1.0..1.0].
    */
   struct WheelSpeeds {
+    /// Left wheel speed.
     double left = 0.0;
+    /// Right wheel speed.
     double right = 0.0;
   };
 
+  WPI_IGNORE_DEPRECATED
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * To pass multiple motors per side, use CAN motor controller followers or
+   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * before passing it in.
+   *
+   * @param leftMotor Left motor.
+   * @param rightMotor Right motor.
+   */
+  DifferentialDrive(MotorController& leftMotor, MotorController& rightMotor);
+
+  WPI_UNIGNORE_DEPRECATED
+
   /**
    * Construct a DifferentialDrive.
    *
    * To pass multiple motors per side, use a MotorControllerGroup. If a motor
    * needs to be inverted, do so before passing it in.
+   *
+   * @param leftMotor Left motor setter.
+   * @param rightMotor Right motor setter.
    */
-  DifferentialDrive(MotorController& leftMotor, MotorController& rightMotor);
+  DifferentialDrive(std::function<void(double)> leftMotor,
+                    std::function<void(double)> rightMotor);
 
   ~DifferentialDrive() override = default;
 
@@ -210,8 +200,12 @@
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  MotorController* m_leftMotor;
-  MotorController* m_rightMotor;
+  std::function<void(double)> m_leftMotor;
+  std::function<void(double)> m_rightMotor;
+
+  // Used for Sendable property getters
+  double m_leftOutput = 0.0;
+  double m_rightOutput = 0.0;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
index f4c28f4..6d8ebac 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
@@ -4,10 +4,12 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 #include <string>
 
 #include <units/angle.h>
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
@@ -63,21 +65,49 @@
    * Uses normalized voltage [-1.0..1.0].
    */
   struct WheelSpeeds {
+    /// Front-left wheel speed.
     double frontLeft = 0.0;
+    /// Front-right wheel speed.
     double frontRight = 0.0;
+    /// Rear-left wheel speed.
     double rearLeft = 0.0;
+    /// Rear-right wheel speed.
     double rearRight = 0.0;
   };
 
+  WPI_IGNORE_DEPRECATED
+
   /**
    * Construct a MecanumDrive.
    *
    * If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor Front-left motor.
+   * @param rearLeftMotor Rear-left motor.
+   * @param frontRightMotor Front-right motor.
+   * @param rearRightMotor Rear-right motor.
    */
   MecanumDrive(MotorController& frontLeftMotor, MotorController& rearLeftMotor,
                MotorController& frontRightMotor,
                MotorController& rearRightMotor);
 
+  WPI_UNIGNORE_DEPRECATED
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor Front-left motor setter.
+   * @param rearLeftMotor Rear-left motor setter.
+   * @param frontRightMotor Front-right motor setter.
+   * @param rearRightMotor Rear-right motor setter.
+   */
+  MecanumDrive(std::function<void(double)> frontLeftMotor,
+               std::function<void(double)> rearLeftMotor,
+               std::function<void(double)> frontRightMotor,
+               std::function<void(double)> rearRightMotor);
+
   ~MecanumDrive() override = default;
 
   MecanumDrive(MecanumDrive&&) = default;
@@ -141,10 +171,16 @@
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  MotorController* m_frontLeftMotor;
-  MotorController* m_rearLeftMotor;
-  MotorController* m_frontRightMotor;
-  MotorController* m_rearRightMotor;
+  std::function<void(double)> m_frontLeftMotor;
+  std::function<void(double)> m_rearLeftMotor;
+  std::function<void(double)> m_frontRightMotor;
+  std::function<void(double)> m_rearRightMotor;
+
+  // Used for Sendable property getters
+  double m_frontLeftOutput = 0.0;
+  double m_rearLeftOutput = 0.0;
+  double m_frontRightOutput = 0.0;
+  double m_rearRightOutput = 0.0;
 
   bool reported = false;
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
index b3fb56b..2a74643 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
@@ -23,12 +23,19 @@
    * The location of a motor on the robot for the purpose of driving.
    */
   enum MotorType {
+    /// Front-left motor.
     kFrontLeft = 0,
+    /// Front-right motor.
     kFrontRight = 1,
+    /// Rear-left motor.
     kRearLeft = 2,
+    /// Rear-right motor.
     kRearRight = 3,
+    /// Left motor.
     kLeft = 0,
+    /// Right motor.
     kRight = 1,
+    /// Back motor.
     kBack = 2
   };
 
@@ -70,14 +77,23 @@
   std::string GetDescription() const override = 0;
 
  protected:
+  /// Default input deadband.
+  static constexpr double kDefaultDeadband = 0.02;
+
+  /// Default maximum output.
+  static constexpr double kDefaultMaxOutput = 1.0;
+
   /**
    * Renormalize all wheel speeds if the magnitude of any wheel is greater than
    * 1.0.
    */
   static void Desaturate(std::span<double> wheelSpeeds);
 
-  double m_deadband = 0.02;
-  double m_maxOutput = 1.0;
+  /// Input deadband.
+  double m_deadband = kDefaultDeadband;
+
+  /// Maximum output.
+  double m_maxOutput = kDefaultMaxOutput;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/event/EventLoop.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/event/EventLoop.h
index 224dd3b..dca6122 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/event/EventLoop.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/event/EventLoop.h
@@ -38,5 +38,6 @@
 
  private:
   std::vector<wpi::unique_function<void()>> m_bindings;
+  bool m_running{false};
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h
index 77f1d5b..ceafeba 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h
@@ -20,7 +20,27 @@
   Accelerometer(Accelerometer&&) = default;
   Accelerometer& operator=(Accelerometer&&) = default;
 
-  enum Range { kRange_2G = 0, kRange_4G = 1, kRange_8G = 2, kRange_16G = 3 };
+  /**
+   * Accelerometer range.
+   */
+  enum Range {
+    /**
+     * 2 Gs max.
+     */
+    kRange_2G = 0,
+    /**
+     * 4 Gs max.
+     */
+    kRange_4G = 1,
+    /**
+     * 8 Gs max.
+     */
+    kRange_8G = 2,
+    /**
+     * 16 Gs max.
+     */
+    kRange_16G = 3
+  };
 
   /**
    * Common interface for setting the measuring range of an accelerometer.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h
index f4fd11c..e646e15 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h
@@ -8,9 +8,16 @@
 #include <thread>
 
 namespace frc::internal {
+/**
+ * For internal use only.
+ */
 class DriverStationModeThread {
  public:
+  /**
+   * For internal use only.
+   */
   DriverStationModeThread();
+
   ~DriverStationModeThread();
 
   DriverStationModeThread(const DriverStationModeThread& other) = delete;
@@ -19,9 +26,39 @@
       delete;
   DriverStationModeThread& operator=(DriverStationModeThread&& other) = delete;
 
-  void InAutonomous(bool entering);
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting disabled code; if false, leaving disabled
+   * code
+   */
   void InDisabled(bool entering);
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting autonomous code; if false, leaving
+   * autonomous code
+   */
+  void InAutonomous(bool entering);
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting teleop code; if false, leaving teleop
+   * code
+   */
   void InTeleop(bool entering);
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting test code; if false, leaving test code
+   */
   void InTest(bool entering);
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
index 3714146..2f0a349 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
@@ -19,14 +19,14 @@
 class LiveWindow final {
  public:
   /**
-   * Set function to be called when LiveWindow is enabled.
+   * Sets function to be called when LiveWindow is enabled.
    *
    * @param func function (or nullptr for none)
    */
   static void SetEnabledCallback(std::function<void()> func);
 
   /**
-   * Set function to be called when LiveWindow is disabled.
+   * Sets function to be called when LiveWindow is disabled.
    *
    * @param func function (or nullptr for none)
    */
@@ -56,6 +56,11 @@
    */
   static void EnableAllTelemetry();
 
+  /**
+   * Returns true if LiveWindow is enabled.
+   *
+   * @return True if LiveWindow is enabled.
+   */
   static bool IsEnabled();
 
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
index 99a9e4e..6500fb3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
@@ -7,23 +7,42 @@
 #include <functional>
 #include <vector>
 
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
 #include "frc/motorcontrol/MotorController.h"
 
+WPI_IGNORE_DEPRECATED
+
 namespace frc {
 
 /**
  * Allows multiple MotorController objects to be linked together.
  */
-class MotorControllerGroup : public wpi::Sendable,
-                             public MotorController,
-                             public wpi::SendableHelper<MotorControllerGroup> {
+class [[deprecated(
+    "Use PWMMotorController::AddFollower() or if using CAN motor controllers,"
+    "use their method of following.")]] MotorControllerGroup
+    : public wpi::Sendable,
+      public MotorController,
+      public wpi::SendableHelper<MotorControllerGroup> {
  public:
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @tparam MotorControllers The MotorController types.
+   * @param motorController The first MotorController to add
+   * @param motorControllers The MotorControllers to add
+   */
   template <class... MotorControllers>
   explicit MotorControllerGroup(MotorController& motorController,
                                 MotorControllers&... motorControllers);
+
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @param motorControllers The MotorControllers to add.
+   */
   explicit MotorControllerGroup(
       std::vector<std::reference_wrapper<MotorController>>&& motorControllers);
 
@@ -50,3 +69,5 @@
 }  // namespace frc
 
 #include "frc/motorcontrol/MotorControllerGroup.inc"
+
+WPI_UNIGNORE_DEPRECATED
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/NidecBrushless.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/NidecBrushless.h
index cc95d71..d50c836 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/NidecBrushless.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/NidecBrushless.h
@@ -6,6 +6,7 @@
 
 #include <string>
 
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
@@ -16,6 +17,8 @@
 
 namespace frc {
 
+WPI_IGNORE_DEPRECATED
+
 /**
  * Nidec Brushless Motor.
  */
@@ -95,4 +98,6 @@
   double m_speed = 0.0;
 };
 
+WPI_UNIGNORE_DEPRECATED
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
index bca5d7f..54a16bb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
@@ -4,9 +4,16 @@
 
 #pragma once
 
+#include <concepts>
+#include <memory>
 #include <string>
 #include <string_view>
+#include <type_traits>
+#include <utility>
+#include <vector>
 
+#include <units/voltage.h>
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
@@ -17,6 +24,8 @@
 namespace frc {
 class DMA;
 
+WPI_IGNORE_DEPRECATED
+
 /**
  * Common base class for all PWM Motor Controllers.
  */
@@ -41,6 +50,20 @@
   void Set(double value) override;
 
   /**
+   * Sets the voltage output of the PWMMotorController. Compensates for
+   * the current bus voltage to ensure that the desired voltage is output even
+   * if the battery voltage is below 12V - highly useful when the voltage
+   * outputs are "meaningful" (e.g. they come from a feedforward calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage
+   * compensation to work properly - unlike the ordinary set function, it is not
+   * "set it and forget it."
+   *
+   * @param output The voltage to output.
+   */
+  void SetVoltage(units::volt_t output) override;
+
+  /**
    * Get the recently set value of the PWM. This value is affected by the
    * inversion property. If you want the value that is sent directly to the
    * MotorController, use PWM::GetSpeed() instead.
@@ -71,6 +94,24 @@
    */
   void EnableDeadbandElimination(bool eliminateDeadband);
 
+  /**
+   * Make the given PWM motor controller follow the output of this one.
+   *
+   * @param follower The motor controller follower.
+   */
+  void AddFollower(PWMMotorController& follower);
+
+  /**
+   * Make the given PWM motor controller follow the output of this one.
+   *
+   * @param follower The motor controller follower.
+   */
+  template <std::derived_from<PWMMotorController> T>
+  void AddFollower(T&& follower) {
+    m_owningFollowers.emplace_back(
+        std::make_unique<std::decay_t<T>>(std::forward<T>(follower)));
+  }
+
  protected:
   /**
    * Constructor for a PWM Motor %Controller connected via PWM.
@@ -83,12 +124,17 @@
 
   void InitSendable(wpi::SendableBuilder& builder) override;
 
+  /// PWM instances for motor controller.
   PWM m_pwm;
 
  private:
   bool m_isInverted = false;
+  std::vector<PWMMotorController*> m_nonowningFollowers;
+  std::vector<std::unique_ptr<PWMMotorController>> m_owningFollowers;
 
   PWM* GetPwm() { return &m_pwm; }
 };
 
+WPI_UNIGNORE_DEPRECATED
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMSparkFlex.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMSparkFlex.h
new file mode 100644
index 0000000..3106921
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMSparkFlex.h
@@ -0,0 +1,41 @@
+// 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 "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * REV Robotics SPARK Flex Motor %Controller.
+ *
+ * Note that the SPARK Flex uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the SPARK Flex User
+ * Manual available from REV Robotics.
+ *
+ * \li 2.003ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.460ms = the "low end" of the deadband range
+ * \li 0.999ms = full "reverse"
+ */
+class PWMSparkFlex : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a SPARK Flex.
+   *
+   * @param channel The PWM channel that the SPARK Flex is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit PWMSparkFlex(int channel);
+
+  PWMSparkFlex(PWMSparkFlex&&) = default;
+  PWMSparkFlex& operator=(PWMSparkFlex&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h
index fc15948..8cb8673 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h
@@ -26,6 +26,13 @@
 template <typename Derived>
 class ShuffleboardComponent : public ShuffleboardComponentBase {
  public:
+  /**
+   * Constructs a ShuffleboardComponent.
+   *
+   * @param parent The parent container.
+   * @param title The component title.
+   * @param type The component type.
+   */
   ShuffleboardComponent(ShuffleboardContainer& parent, std::string_view title,
                         std::string_view type = "");
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
index 1fa9c91..ea251ec 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
@@ -14,6 +14,11 @@
 
 enum ShuffleboardEventImportance { kTrivial, kLow, kNormal, kHigh, kCritical };
 
+/**
+ * Returns name of the given enum.
+ *
+ * @return Name of the given enum.
+ */
 inline std::string_view ShuffleboardEventImportanceName(
     ShuffleboardEventImportance importance) {
   switch (importance) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h
index af96b39..3cf6162 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h
@@ -14,7 +14,7 @@
 namespace sim {
 
 /**
- * Class to control a simulated ADXRS450 gyroscope.
+ * Class to control a simulated ADXL345.
  */
 class ADXL345Sim {
  public:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h
index 269732b..cc63af2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h
@@ -13,7 +13,7 @@
 namespace sim {
 
 /**
- * Class to control a simulated ADXRS450 gyroscope.
+ * Class to control a simulated ADXL362.
  */
 class ADXL362Sim {
  public:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
index 78310d7..644e348 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -187,6 +187,13 @@
    */
   void SetPose(const frc::Pose2d& pose);
 
+  /**
+   * The differential drive dynamics function.
+   *
+   * @param x The state.
+   * @param u The input.
+   * @return The state derivative with respect to time.
+   */
   Vectord<7> Dynamics(const Vectord<7>& x, const Eigen::Vector2d& u);
 
   class State {
@@ -210,32 +217,54 @@
    */
   class KitbotGearing {
    public:
+    /// Gear ratio of 12.75:1.
     static constexpr double k12p75 = 12.75;
+    /// Gear ratio of 10.71:1.
     static constexpr double k10p71 = 10.71;
+    /// Gear ratio of 8.45:1.
     static constexpr double k8p45 = 8.45;
+    /// Gear ratio of 7.31:1.
     static constexpr double k7p31 = 7.31;
+    /// Gear ratio of 5.95:1.
     static constexpr double k5p95 = 5.95;
   };
 
+  /**
+   * Represents common motor layouts of the kit drivetrain.
+   */
   class KitbotMotor {
    public:
+    /// One CIM motor per drive side.
     static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1);
+    /// Two CIM motors per drive side.
     static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2);
+    /// One Mini CIM motor per drive side.
     static constexpr frc::DCMotor SingleMiniCIMPerSide =
         frc::DCMotor::MiniCIM(1);
+    /// Two Mini CIM motors per drive side.
     static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
+    /// One Falcon 500 motor per drive side.
     static constexpr frc::DCMotor SingleFalcon500PerSide =
         frc::DCMotor::Falcon500(1);
+    /// Two Falcon 500 motors per drive side.
     static constexpr frc::DCMotor DualFalcon500PerSide =
         frc::DCMotor::Falcon500(2);
+    /// One NEO motor per drive side.
     static constexpr frc::DCMotor SingleNEOPerSide = frc::DCMotor::NEO(1);
+    /// Two NEO motors per drive side.
     static constexpr frc::DCMotor DualNEOPerSide = frc::DCMotor::NEO(2);
   };
 
+  /**
+   * Represents common wheel sizes of the kit drivetrain.
+   */
   class KitbotWheelSize {
    public:
+    /// Six inch diameter wheels.
     static constexpr units::meter_t kSixInch = 6_in;
+    /// Eight inch diameter wheels.
     static constexpr units::meter_t kEightInch = 8_in;
+    /// Ten inch diameter wheels.
     static constexpr units::meter_t kTenInch = 10_in;
   };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
index 399e0c1..ba56c4d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
@@ -136,6 +136,7 @@
   double GetRumble(GenericHID::RumbleType type);
 
  protected:
+  /// GenericHID port.
   int m_port;
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
index cfc87b2..a4fec00 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -23,9 +23,9 @@
  * voltage). Call the Update() method to update the simulation. Set simulated
  * sensor readings with the simulated positions in the GetOutput() method.
  *
- * @tparam States  The number of states of the system.
- * @tparam Inputs  The number of inputs to the system.
- * @tparam Outputs The number of outputs of the system.
+ * @tparam States  Number of states of the system.
+ * @tparam Inputs  Number of inputs to the system.
+ * @tparam Outputs Number of outputs of the system.
  */
 template <int States, int Inputs, int Outputs>
 class LinearSystemSim {
@@ -140,11 +140,20 @@
         u, frc::RobotController::GetInputVoltage());
   }
 
+  /// The plant that represents the linear system.
   LinearSystem<States, Inputs, Outputs> m_plant;
 
+  /// State vector.
   Vectord<States> m_x;
-  Vectord<Outputs> m_y;
+
+  /// Input vector.
   Vectord<Inputs> m_u;
+
+  /// Output vector.
+  Vectord<Outputs> m_y;
+
+  /// The standard deviations of measurements, used for adding noise to the
+  /// measurements.
   std::array<double, Outputs> m_measurementStdDevs;
 };
 }  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PneumaticsBaseSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PneumaticsBaseSim.h
index 510349f..f3000f4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PneumaticsBaseSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PneumaticsBaseSim.h
@@ -177,9 +177,22 @@
   virtual void ResetData() = 0;
 
  protected:
+  /// PneumaticsBase index.
   const int m_index;
-  explicit PneumaticsBaseSim(const PneumaticsBase& module);
+
+  /**
+   * Constructs a PneumaticsBaseSim with the given index.
+   *
+   * @param index The index.
+   */
   explicit PneumaticsBaseSim(const int index);
+
+  /**
+   * Constructs a PneumaticsBaseSim for the given module.
+   *
+   * @param module The module.
+   */
+  explicit PneumaticsBaseSim(const PneumaticsBase& module);
 };
 
 }  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
index 64e81e1..6471347 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
@@ -11,6 +11,7 @@
 #include <units/temperature.h>
 #include <units/voltage.h>
 
+#include "frc/RobotController.h"
 #include "frc/simulation/CallbackStore.h"
 
 namespace frc::sim {
@@ -515,6 +516,32 @@
   static void SetComments(std::string_view comments);
 
   /**
+   * Register a callback to be run whenever the Radio led state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterRadioLEDStateCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the state of the radio led.
+   *
+   * @return The state of the radio led.
+   */
+  static RadioLEDState GetRadioLEDState();
+
+  /**
+   * Set the state of the radio led.
+   *
+   * @param state The state of the radio led.
+   */
+  static void SetRadioLEDState(RadioLEDState state);
+
+  /**
    * Reset all simulation data.
    */
   static void ResetData();
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h
index 5db41f7..0d22d87 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h
@@ -36,14 +36,14 @@
   /**
    * Sets if the range measurement is valid.
    *
-   * @param isValid True if valid
+   * @param valid True if valid
    */
-  void SetRangeValid(bool isValid);
+  void SetRangeValid(bool valid);
 
   /**
-   * Sets the range measurement
+   * Sets the range measurement.
    *
-   * @param range The range
+   * @param range The range.
    */
   void SetRange(units::inch_t range);
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
index baee6ff..2051d1b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
@@ -21,6 +21,9 @@
 
 namespace frc {
 
+/**
+ * Implementation detail for SendableBuilder.
+ */
 class SendableBuilderImpl : public nt::NTSendableBuilder {
  public:
   SendableBuilderImpl() = default;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h
new file mode 100644
index 0000000..7f106bd
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h
@@ -0,0 +1,194 @@
+// 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 <string>
+#include <string_view>
+#include <unordered_map>
+
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/current.h>
+#include <units/length.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+#include <wpi/DataLog.h>
+
+namespace frc::sysid {
+
+/**
+ * Possible state of a SysId routine.
+ */
+enum class State {
+  /// Quasistatic forward test.
+  kQuasistaticForward,
+  /// Quasistatic reverse test.
+  kQuasistaticReverse,
+  /// Dynamic forward test.
+  kDynamicForward,
+  /// Dynamic reverse test.
+  kDynamicReverse,
+  /// No test.
+  kNone
+};
+
+/**
+ * Utility for logging data from a SysId test routine. Each complete routine
+ * (quasistatic and dynamic, forward and reverse) should have its own
+ * SysIdRoutineLog instance, with a unique log name.
+ */
+class SysIdRoutineLog {
+  using MotorEntries = wpi::StringMap<wpi::log::DoubleLogEntry>;
+  using LogEntries = wpi::StringMap<MotorEntries>;
+
+ public:
+  /** Logs data from a single motor during a SysIdRoutine. */
+  class MotorLog {
+   public:
+    /**
+     * Log a generic data value from the motor.
+     *
+     * @param name The name of the data field being recorded.
+     * @param value The numeric value of the data field.
+     * @param unit The unit string of the data field.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& value(std::string_view name, double value, std::string_view unit);
+
+    /**
+     * Log the voltage applied to the motor.
+     *
+     * @param voltage The voltage to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& voltage(units::volt_t voltage) {
+      return value("voltage", voltage.value(), voltage.name());
+    }
+
+    /**
+     * Log the linear position of the motor.
+     *
+     * @param position The linear position to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& position(units::meter_t position) {
+      return value("position", position.value(), position.name());
+    }
+
+    /**
+     * Log the angular position of the motor.
+     *
+     * @param position The angular position to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& position(units::turn_t position) {
+      return value("position", position.value(), position.name());
+    }
+
+    /**
+     * Log the linear velocity of the motor.
+     *
+     * @param velocity The linear velocity to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& velocity(units::meters_per_second_t velocity) {
+      return value("velocity", velocity.value(), velocity.name());
+    }
+
+    /**
+     * Log the angular velocity of the motor.
+     *
+     * @param velocity The angular velocity to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& velocity(units::turns_per_second_t velocity) {
+      return value("velocity", velocity.value(), velocity.name());
+    }
+
+    /**
+     * Log the linear acceleration of the motor.
+     *
+     * @param acceleration The linear acceleration to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& acceleration(units::meters_per_second_squared_t acceleration) {
+      return value("acceleration", acceleration.value(), acceleration.name());
+    }
+
+    /**
+     * Log the angular acceleration of the motor.
+     *
+     * @param acceleration The angular acceleration to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& acceleration(units::turns_per_second_squared_t acceleration) {
+      return value("acceleration", acceleration.value(), acceleration.name());
+    }
+
+    /**
+     * Log the current applied to the motor.
+     *
+     * @param current The current to record.
+     * @return The motor log (for call chaining).
+     */
+    MotorLog& current(units::ampere_t current) {
+      return value("current", current.value(), current.name());
+    }
+
+   private:
+    friend class SysIdRoutineLog;
+    /**
+     * Create a new SysId motor log handle.
+     *
+     * @param motorName The name of the motor whose data is being logged.
+     * @param logName The name of the SysIdRoutineLog that this motor belongs
+     * to.
+     * @param logEntries The DataLog entries of the SysIdRoutineLog that this
+     * motor belongs to.
+     */
+    MotorLog(std::string_view motorName, std::string_view logName,
+             LogEntries* logEntries);
+    std::string m_motorName;
+    std::string m_logName;
+    LogEntries* m_logEntries;
+  };
+
+  /**
+   * Create a new logging utility for a SysId test routine.
+   *
+   * @param logName The name for the test routine in the log. Should be unique
+   *   between complete test routines (quasistatic and dynamic, forward and
+   *   reverse). The current state of this test (e.g. "quasistatic-forward")
+   *   will appear in WPILog under the "sysid-test-state-logName" entry.
+   */
+  explicit SysIdRoutineLog(std::string_view logName);
+
+  /**
+   * Records the current state of the SysId test routine. Should be called once
+   * per iteration during tests with the type of the current test, and once upon
+   * test end with state `none`.
+   *
+   * @param state The current state of the SysId test routine.
+   */
+  void RecordState(State state);
+
+  /**
+   * Log data from a motor during a SysId routine.
+   *
+   * @param motorName The name of the motor.
+   * @return Handle with chainable callbacks to log individual data fields.
+   */
+  MotorLog Motor(std::string_view motorName);
+
+  static std::string StateEnumToString(State state);
+
+ private:
+  LogEntries m_logEntries;
+  std::string m_logName;
+  wpi::log::StringLogEntry m_state;
+};
+}  // namespace frc::sysid
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h
index 87d8c12..f51d3b1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h
@@ -5,7 +5,14 @@
 #pragma once
 
 #include <algorithm>
+#include <stdexcept>
 #include <string>
+#include <string_view>
+
+#include <fmt/core.h>
+#include <gcem.hpp>
+#include <wpi/StringExtras.h>
+#include <wpi/ct_string.h>
 
 namespace frc {
 
@@ -739,6 +746,9 @@
    */
   static const Color kYellowGreen;
 
+  /**
+   * Constructs a default color (black).
+   */
   constexpr Color() = default;
 
   /**
@@ -763,6 +773,33 @@
   constexpr Color(int r, int g, int b)
       : Color(r / 255.0, g / 255.0, b / 255.0) {}
 
+  /**
+   * Constructs a Color from a hex string.
+   *
+   * @param hexString a string of the format <tt>\#RRGGBB</tt>
+   * @throws std::invalid_argument if the hex string is invalid.
+   */
+  explicit constexpr Color(std::string_view hexString) {
+    if (hexString.length() != 7 || !hexString.starts_with("#") ||
+        !wpi::isHexDigit(hexString[1]) || !wpi::isHexDigit(hexString[2]) ||
+        !wpi::isHexDigit(hexString[3]) || !wpi::isHexDigit(hexString[4]) ||
+        !wpi::isHexDigit(hexString[5]) || !wpi::isHexDigit(hexString[6])) {
+      throw std::invalid_argument(
+          fmt::format("Invalid hex string for Color \"{}\"", hexString));
+    }
+
+    int r = wpi::hexDigitValue(hexString[1]) * 16 +
+            wpi::hexDigitValue(hexString[2]);
+    int g = wpi::hexDigitValue(hexString[3]) * 16 +
+            wpi::hexDigitValue(hexString[4]);
+    int b = wpi::hexDigitValue(hexString[5]) * 16 +
+            wpi::hexDigitValue(hexString[6]);
+
+    red = r / 255.0;
+    green = g / 255.0;
+    blue = b / 255.0;
+  }
+
   constexpr bool operator==(const Color&) const = default;
 
   /**
@@ -816,19 +853,29 @@
    *
    * @return a string of the format <tt>\#RRGGBB</tt>
    */
-  std::string HexString() const;
+  constexpr auto HexString() const {
+    const int r = 255.0 * red;
+    const int g = 255.0 * green;
+    const int b = 255.0 * blue;
 
+    return wpi::ct_string<char, std::char_traits<char>, 7>{
+        {'#', wpi::hexdigit(r / 16), wpi::hexdigit(r % 16),
+         wpi::hexdigit(g / 16), wpi::hexdigit(g % 16), wpi::hexdigit(b / 16),
+         wpi::hexdigit(b % 16)}};
+  }
+
+  /// Red component (0-1).
   double red = 0.0;
+
+  /// Green component (0-1).
   double green = 0.0;
+
+  /// Blue component (0-1).
   double blue = 0.0;
 
  private:
-  static constexpr double kPrecision = 1.0 / (1 << 12);
-
   static constexpr double roundAndClamp(double value) {
-    const auto rounded =
-        (static_cast<int>(value / kPrecision) + 0.5) * kPrecision;
-    return std::clamp(rounded, 0.0, 1.0);
+    return std::clamp(gcem::ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0);
   }
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h
index 10afead..f4bc104 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h
@@ -5,7 +5,13 @@
 #pragma once
 
 #include <algorithm>
+#include <stdexcept>
 #include <string>
+#include <string_view>
+
+#include <fmt/core.h>
+#include <wpi/StringExtras.h>
+#include <wpi/ct_string.h>
 
 #include "Color.h"
 
@@ -16,6 +22,9 @@
  */
 class Color8Bit {
  public:
+  /**
+   * Constructs a default color (black).
+   */
   constexpr Color8Bit() = default;
 
   /**
@@ -40,21 +49,79 @@
         green(color.green * 255),
         blue(color.blue * 255) {}
 
+  /**
+   * Constructs a Color8Bit from a hex string.
+   *
+   * @param hexString a string of the format <tt>\#RRGGBB</tt>
+   * @throws std::invalid_argument if the hex string is invalid.
+   */
+  explicit constexpr Color8Bit(std::string_view hexString) {
+    if (hexString.length() != 7 || !hexString.starts_with("#") ||
+        !wpi::isHexDigit(hexString[1]) || !wpi::isHexDigit(hexString[2]) ||
+        !wpi::isHexDigit(hexString[3]) || !wpi::isHexDigit(hexString[4]) ||
+        !wpi::isHexDigit(hexString[5]) || !wpi::isHexDigit(hexString[6])) {
+      throw std::invalid_argument(
+          fmt::format("Invalid hex string for Color \"{}\"", hexString));
+    }
+
+    red = wpi::hexDigitValue(hexString[1]) * 16 +
+          wpi::hexDigitValue(hexString[2]);
+    green = wpi::hexDigitValue(hexString[3]) * 16 +
+            wpi::hexDigitValue(hexString[4]);
+    blue = wpi::hexDigitValue(hexString[5]) * 16 +
+           wpi::hexDigitValue(hexString[6]);
+  }
+
+  constexpr bool operator==(const Color8Bit&) const = default;
+
   constexpr operator Color() const {  // NOLINT
     return Color(red / 255.0, green / 255.0, blue / 255.0);
   }
 
-  constexpr bool operator==(const Color8Bit&) const = default;
+  /**
+   * Create a Color8Bit from a hex string.
+   *
+   * @param hexString a string of the format <tt>\#RRGGBB</tt>
+   * @return Color8Bit object from hex string.
+   * @throws std::invalid_argument if the hex string is invalid.
+   */
+  static constexpr Color8Bit FromHexString(std::string_view hexString) {
+    if (hexString.length() != 7 || !hexString.starts_with("#") ||
+        !wpi::isHexDigit(hexString[1]) || !wpi::isHexDigit(hexString[2]) ||
+        !wpi::isHexDigit(hexString[3]) || !wpi::isHexDigit(hexString[4]) ||
+        !wpi::isHexDigit(hexString[5]) || !wpi::isHexDigit(hexString[6])) {
+      throw std::invalid_argument(
+          fmt::format("Invalid hex string for Color \"{}\"", hexString));
+    }
+
+    int r = wpi::hexDigitValue(hexString[0]) * 16 +
+            wpi::hexDigitValue(hexString[1]);
+    int g = wpi::hexDigitValue(hexString[2]) * 16 +
+            wpi::hexDigitValue(hexString[3]);
+    int b = wpi::hexDigitValue(hexString[4]) * 16 +
+            wpi::hexDigitValue(hexString[5]);
+    return Color8Bit{r, g, b};
+  }
 
   /**
    * Return this color represented as a hex string.
    *
    * @return a string of the format <tt>\#RRGGBB</tt>
    */
-  std::string HexString() const;
+  constexpr auto HexString() const {
+    return wpi::ct_string<char, std::char_traits<char>, 7>{
+        {'#', wpi::hexdigit(red / 16), wpi::hexdigit(red % 16),
+         wpi::hexdigit(green / 16), wpi::hexdigit(green % 16),
+         wpi::hexdigit(blue / 16), wpi::hexdigit(blue % 16)}};
+  }
 
+  /// Red component (0-255).
   int red = 0;
+
+  /// Green component (0-255).
   int green = 0;
+
+  /// Blue component (0-255).
   int blue = 0;
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
index f21f268..9a22987 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
@@ -5,7 +5,7 @@
 #include <gtest/gtest.h>
 
 #include "frc/drive/DifferentialDrive.h"
-#include "motorcontrol/MockMotorController.h"
+#include "motorcontrol/MockPWMMotorController.h"
 
 TEST(DifferentialDriveTest, ArcadeDriveIK) {
   // Forward
@@ -240,9 +240,10 @@
 }
 
 TEST(DifferentialDriveTest, ArcadeDrive) {
-  frc::MockMotorController left;
-  frc::MockMotorController right;
-  frc::DifferentialDrive drive{left, right};
+  frc::MockPWMMotorController left;
+  frc::MockPWMMotorController right;
+  frc::DifferentialDrive drive{[&](double output) { left.Set(output); },
+                               [&](double output) { right.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -277,9 +278,10 @@
 }
 
 TEST(DifferentialDriveTest, ArcadeDriveSquared) {
-  frc::MockMotorController left;
-  frc::MockMotorController right;
-  frc::DifferentialDrive drive{left, right};
+  frc::MockPWMMotorController left;
+  frc::MockPWMMotorController right;
+  frc::DifferentialDrive drive{[&](double output) { left.Set(output); },
+                               [&](double output) { right.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -314,9 +316,10 @@
 }
 
 TEST(DifferentialDriveTest, CurvatureDrive) {
-  frc::MockMotorController left;
-  frc::MockMotorController right;
-  frc::DifferentialDrive drive{left, right};
+  frc::MockPWMMotorController left;
+  frc::MockPWMMotorController right;
+  frc::DifferentialDrive drive{[&](double output) { left.Set(output); },
+                               [&](double output) { right.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -351,9 +354,10 @@
 }
 
 TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) {
-  frc::MockMotorController left;
-  frc::MockMotorController right;
-  frc::DifferentialDrive drive{left, right};
+  frc::MockPWMMotorController left;
+  frc::MockPWMMotorController right;
+  frc::DifferentialDrive drive{[&](double output) { left.Set(output); },
+                               [&](double output) { right.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -388,9 +392,10 @@
 }
 
 TEST(DifferentialDriveTest, TankDrive) {
-  frc::MockMotorController left;
-  frc::MockMotorController right;
-  frc::DifferentialDrive drive{left, right};
+  frc::MockPWMMotorController left;
+  frc::MockPWMMotorController right;
+  frc::DifferentialDrive drive{[&](double output) { left.Set(output); },
+                               [&](double output) { right.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -425,9 +430,10 @@
 }
 
 TEST(DifferentialDriveTest, TankDriveSquared) {
-  frc::MockMotorController left;
-  frc::MockMotorController right;
-  frc::DifferentialDrive drive{left, right};
+  frc::MockPWMMotorController left;
+  frc::MockPWMMotorController right;
+  frc::DifferentialDrive drive{[&](double output) { left.Set(output); },
+                               [&](double output) { right.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
index 1bbf464..b32b03c 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
@@ -5,7 +5,7 @@
 #include <gtest/gtest.h>
 
 #include "frc/drive/MecanumDrive.h"
-#include "motorcontrol/MockMotorController.h"
+#include "motorcontrol/MockPWMMotorController.h"
 
 TEST(MecanumDriveTest, CartesianIK) {
   // Forward
@@ -82,11 +82,14 @@
 }
 
 TEST(MecanumDriveTest, Cartesian) {
-  frc::MockMotorController fl;
-  frc::MockMotorController rl;
-  frc::MockMotorController fr;
-  frc::MockMotorController rr;
-  frc::MecanumDrive drive{fl, rl, fr, rr};
+  frc::MockPWMMotorController fl;
+  frc::MockPWMMotorController rl;
+  frc::MockPWMMotorController fr;
+  frc::MockPWMMotorController rr;
+  frc::MecanumDrive drive{[&](double output) { fl.Set(output); },
+                          [&](double output) { rl.Set(output); },
+                          [&](double output) { fr.Set(output); },
+                          [&](double output) { rr.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -126,11 +129,14 @@
 }
 
 TEST(MecanumDriveTest, CartesianGyro90CW) {
-  frc::MockMotorController fl;
-  frc::MockMotorController fr;
-  frc::MockMotorController rl;
-  frc::MockMotorController rr;
-  frc::MecanumDrive drive{fl, rl, fr, rr};
+  frc::MockPWMMotorController fl;
+  frc::MockPWMMotorController rl;
+  frc::MockPWMMotorController fr;
+  frc::MockPWMMotorController rr;
+  frc::MecanumDrive drive{[&](double output) { fl.Set(output); },
+                          [&](double output) { rl.Set(output); },
+                          [&](double output) { fr.Set(output); },
+                          [&](double output) { rr.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward in global frame; left in robot frame
@@ -170,11 +176,14 @@
 }
 
 TEST(MecanumDriveTest, Polar) {
-  frc::MockMotorController fl;
-  frc::MockMotorController fr;
-  frc::MockMotorController rl;
-  frc::MockMotorController rr;
-  frc::MecanumDrive drive{fl, rl, fr, rr};
+  frc::MockPWMMotorController fl;
+  frc::MockPWMMotorController rl;
+  frc::MockPWMMotorController fr;
+  frc::MockPWMMotorController rr;
+  frc::MecanumDrive drive{[&](double output) { fl.Set(output); },
+                          [&](double output) { rl.Set(output); },
+                          [&](double output) { fr.Set(output); },
+                          [&](double output) { rr.Set(output); }};
   drive.SetDeadband(0.0);
 
   // Forward
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/event/EventLoopTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/event/EventLoopTest.cpp
new file mode 100644
index 0000000..2576a46
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/event/EventLoopTest.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 <gtest/gtest.h>
+
+#include "frc/Errors.h"
+#include "frc/event/EventLoop.h"
+
+using namespace frc;
+
+TEST(EventLoopTest, ConcurrentModification) {
+  EventLoop loop;
+
+  loop.Bind([&loop] { ASSERT_THROW(loop.Bind([] {}), frc::RuntimeError); });
+
+  loop.Poll();
+
+  loop.Clear();
+
+  loop.Bind([&loop] { ASSERT_THROW(loop.Clear(), frc::RuntimeError); });
+
+  loop.Poll();
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/motorcontrol/MockPWMMotorController.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/motorcontrol/MockPWMMotorController.cpp
new file mode 100644
index 0000000..7a51f33
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/motorcontrol/MockPWMMotorController.cpp
@@ -0,0 +1,31 @@
+// 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 "motorcontrol/MockPWMMotorController.h"
+
+using namespace frc;
+
+void MockPWMMotorController::Set(double speed) {
+  m_speed = m_isInverted ? -speed : speed;
+}
+
+double MockPWMMotorController::Get() const {
+  return m_speed;
+}
+
+void MockPWMMotorController::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool MockPWMMotorController::GetInverted() const {
+  return m_isInverted;
+}
+
+void MockPWMMotorController::Disable() {
+  m_speed = 0;
+}
+
+void MockPWMMotorController::StopMotor() {
+  Disable();
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/motorcontrol/MotorControllerGroupTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/motorcontrol/MotorControllerGroupTest.cpp
index 4ff889a..740c46f 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/motorcontrol/MotorControllerGroupTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/motorcontrol/MotorControllerGroupTest.cpp
@@ -8,6 +8,7 @@
 #include <vector>
 
 #include <gtest/gtest.h>
+#include <wpi/deprecated.h>
 
 #include "motorcontrol/MockMotorController.h"
 
@@ -32,6 +33,8 @@
   return os;
 }
 
+WPI_IGNORE_DEPRECATED
+
 /**
  * A fixture used for MotorControllerGroup testing.
  */
@@ -124,3 +127,5 @@
 
 INSTANTIATE_TEST_SUITE_P(Tests, MotorControllerGroupTest,
                          testing::Values(TEST_ONE, TEST_TWO, TEST_THREE));
+
+WPI_UNIGNORE_DEPRECATED
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
index 7e56426..a942d16 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
@@ -55,7 +55,7 @@
     sim.Update(20_ms);
 
     // Update ground truth.
-    groundTruthX = frc::RK4(
+    groundTruthX = frc::RKDP(
         [&sim](const auto& x, const auto& u) -> frc::Vectord<7> {
           return sim.Dynamics(x, u);
         },
@@ -63,7 +63,7 @@
   }
 
   // 2 inch tolerance is OK since our ground truth is an approximation of the
-  // ODE solution using RK4 anyway
+  // ODE solution using RKDP anyway
   EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05);
   EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05);
   EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp
index e42f38f..3b74a91 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp
@@ -274,4 +274,26 @@
   EXPECT_EQ(kCommentsTruncated, RobotController::GetComments());
 }
 
+TEST(RoboRioSimTest, SetRadioLEDState) {
+  RoboRioSim::ResetData();
+
+  EnumCallback callback;
+  auto cbHandle =
+      RoboRioSim::RegisterRadioLEDStateCallback(callback.GetCallback(), false);
+
+  RobotController::SetRadioLEDState(RadioLEDState::kGreen);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(RadioLEDState::kGreen, callback.GetLastValue());
+  EXPECT_EQ(RadioLEDState::kGreen, RoboRioSim::GetRadioLEDState());
+  EXPECT_EQ(RadioLEDState::kGreen, RobotController::GetRadioLEDState());
+
+  callback.Reset();
+
+  RoboRioSim::SetRadioLEDState(RadioLEDState::kOrange);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(RadioLEDState::kOrange, callback.GetLastValue());
+  EXPECT_EQ(RadioLEDState::kOrange, RoboRioSim::GetRadioLEDState());
+  EXPECT_EQ(RadioLEDState::kOrange, RobotController::GetRadioLEDState());
+}
+
 }  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/util/Color8BitTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/util/Color8BitTest.cpp
new file mode 100644
index 0000000..d9027be
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/util/Color8BitTest.cpp
@@ -0,0 +1,69 @@
+// 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 <string>
+
+#include <gtest/gtest.h>
+
+#include "frc/util/Color8Bit.h"
+
+TEST(Color8BitTest, ConstructDefault) {
+  constexpr frc::Color8Bit color;
+
+  EXPECT_EQ(0, color.red);
+  EXPECT_EQ(0, color.green);
+  EXPECT_EQ(0, color.blue);
+}
+
+TEST(Color8BitTest, ConstructFromInts) {
+  constexpr frc::Color8Bit color{255, 128, 64};
+
+  EXPECT_EQ(255, color.red);
+  EXPECT_EQ(128, color.green);
+  EXPECT_EQ(64, color.blue);
+}
+
+TEST(Color8BitTest, ConstructFromColor) {
+  constexpr frc::Color8Bit color{frc::Color{255, 128, 64}};
+
+  EXPECT_EQ(255, color.red);
+  EXPECT_EQ(128, color.green);
+  EXPECT_EQ(64, color.blue);
+}
+
+TEST(Color8BitTest, ConstructFromHexString) {
+  constexpr frc::Color8Bit color{"#FF8040"};
+
+  EXPECT_EQ(255, color.red);
+  EXPECT_EQ(128, color.green);
+  EXPECT_EQ(64, color.blue);
+
+  // No leading #
+  EXPECT_THROW(frc::Color8Bit{"112233"}, std::invalid_argument);
+
+  // Too long
+  EXPECT_THROW(frc::Color8Bit{"#11223344"}, std::invalid_argument);
+
+  // Invalid hex characters
+  EXPECT_THROW(frc::Color8Bit{"#$$$$$$"}, std::invalid_argument);
+}
+
+TEST(Color8BitTest, ImplicitConversionToColor) {
+  frc::Color color = frc::Color8Bit{255, 128, 64};
+
+  EXPECT_NEAR(1.0, color.red, 1e-2);
+  EXPECT_NEAR(0.5, color.green, 1e-2);
+  EXPECT_NEAR(0.25, color.blue, 1e-2);
+}
+
+TEST(Color8BitTest, ToHexString) {
+  constexpr frc::Color8Bit color1{255, 128, 64};
+  EXPECT_EQ("#FF8040", color1.HexString());
+
+  // Ensure conversion to std::string works
+  [[maybe_unused]] std::string str = color1.HexString();
+
+  frc::Color8Bit color2{255, 128, 64};
+  EXPECT_EQ("#FF8040", color2.HexString());
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/util/ColorTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/util/ColorTest.cpp
new file mode 100644
index 0000000..cbde770
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/util/ColorTest.cpp
@@ -0,0 +1,80 @@
+// 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 <string>
+
+#include <gtest/gtest.h>
+
+#include "frc/util/Color.h"
+
+TEST(ColorTest, ConstructDefault) {
+  constexpr frc::Color color;
+
+  EXPECT_DOUBLE_EQ(0.0, color.red);
+  EXPECT_DOUBLE_EQ(0.0, color.green);
+  EXPECT_DOUBLE_EQ(0.0, color.blue);
+}
+
+TEST(ColorTest, ConstructFromDoubles) {
+  {
+    constexpr frc::Color color{1.0, 0.5, 0.25};
+
+    EXPECT_NEAR(1.0, color.red, 1e-2);
+    EXPECT_NEAR(0.5, color.green, 1e-2);
+    EXPECT_NEAR(0.25, color.blue, 1e-2);
+  }
+
+  {
+    constexpr frc::Color color{1.0, 0.0, 0.0};
+
+    // Check for exact match to ensure round-and-clamp is correct
+    EXPECT_EQ(1.0, color.red);
+    EXPECT_EQ(0.0, color.green);
+    EXPECT_EQ(0.0, color.blue);
+  }
+}
+
+TEST(ColorTest, ConstructFromInts) {
+  constexpr frc::Color color{255, 128, 64};
+
+  EXPECT_NEAR(1.0, color.red, 1e-2);
+  EXPECT_NEAR(0.5, color.green, 1e-2);
+  EXPECT_NEAR(0.25, color.blue, 1e-2);
+}
+
+TEST(ColorTest, ConstructFromHexString) {
+  constexpr frc::Color color{"#FF8040"};
+
+  EXPECT_NEAR(1.0, color.red, 1e-2);
+  EXPECT_NEAR(0.5, color.green, 1e-2);
+  EXPECT_NEAR(0.25, color.blue, 1e-2);
+
+  // No leading #
+  EXPECT_THROW(frc::Color{"112233"}, std::invalid_argument);
+
+  // Too long
+  EXPECT_THROW(frc::Color{"#11223344"}, std::invalid_argument);
+
+  // Invalid hex characters
+  EXPECT_THROW(frc::Color{"#$$$$$$"}, std::invalid_argument);
+}
+
+TEST(ColorTest, FromHSV) {
+  constexpr frc::Color color = frc::Color::FromHSV(90, 128, 64);
+
+  EXPECT_DOUBLE_EQ(0.125732421875, color.red);
+  EXPECT_DOUBLE_EQ(0.251220703125, color.green);
+  EXPECT_DOUBLE_EQ(0.251220703125, color.blue);
+}
+
+TEST(ColorTest, ToHexString) {
+  constexpr frc::Color color1{255, 128, 64};
+  EXPECT_EQ("#FF8040", color1.HexString());
+
+  // Ensure conversion to std::string works
+  [[maybe_unused]] std::string str = color1.HexString();
+
+  frc::Color color2{255, 128, 64};
+  EXPECT_EQ("#FF8040", color2.HexString());
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/include/motorcontrol/MockMotorController.h b/third_party/allwpilib/wpilibc/src/test/native/include/motorcontrol/MockMotorController.h
index e17931f..aab6ce4 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/include/motorcontrol/MockMotorController.h
+++ b/third_party/allwpilib/wpilibc/src/test/native/include/motorcontrol/MockMotorController.h
@@ -4,10 +4,14 @@
 
 #pragma once
 
+#include <wpi/deprecated.h>
+
 #include "frc/motorcontrol/MotorController.h"
 
 namespace frc {
 
+WPI_IGNORE_DEPRECATED
+
 class MockMotorController : public MotorController {
  public:
   void Set(double speed) override;
@@ -22,4 +26,6 @@
   bool m_isInverted = false;
 };
 
+WPI_UNIGNORE_DEPRECATED
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/include/motorcontrol/MockPWMMotorController.h b/third_party/allwpilib/wpilibc/src/test/native/include/motorcontrol/MockPWMMotorController.h
new file mode 100644
index 0000000..18bd1f5
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/include/motorcontrol/MockPWMMotorController.h
@@ -0,0 +1,23 @@
+// 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
+
+namespace frc {
+
+class MockPWMMotorController {
+ public:
+  void Set(double speed);
+  double Get() const;
+  void SetInverted(bool isInverted);
+  bool GetInverted() const;
+  void Disable();
+  void StopMotor();
+
+ private:
+  double m_speed = 0.0;
+  bool m_isInverted = false;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibcExamples/CMakeLists.txt b/third_party/allwpilib/wpilibcExamples/CMakeLists.txt
index f46d1c2..4554b94 100644
--- a/third_party/allwpilib/wpilibcExamples/CMakeLists.txt
+++ b/third_party/allwpilib/wpilibcExamples/CMakeLists.txt
@@ -7,29 +7,53 @@
 subdir_list(EXAMPLES  ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/examples)
 
 foreach(example ${EXAMPLES})
-  file(GLOB_RECURSE sources src/main/cpp/examples/${example}/cpp/*.cpp
-                            src/main/cpp/examples/${example}/c/*.c)
-  add_executable(${example} ${sources})
-  wpilib_target_warnings(${example})
-  target_include_directories(${example} PUBLIC src/main/cpp/examples/${example}/include)
-  target_link_libraries(${example} apriltag wpilibc wpilibNewCommands romiVendordep xrpVendordep)
+    file(
+        GLOB_RECURSE sources
+        src/main/cpp/examples/${example}/cpp/*.cpp
+        src/main/cpp/examples/${example}/c/*.c
+    )
+    add_executable(${example} ${sources})
+    wpilib_target_warnings(${example})
+    target_include_directories(${example} PUBLIC src/main/cpp/examples/${example}/include)
+    target_link_libraries(
+        ${example}
+        apriltag
+        wpilibc
+        wpilibNewCommands
+        romiVendordep
+        xrpVendordep
+    )
 
-  if (WITH_TESTS AND EXISTS ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/test/cpp/examples/${example})
-    wpilib_add_test(${example} src/test/cpp/examples/${example}/cpp)
-    target_sources(${example}_test PRIVATE ${sources})
-    target_include_directories(${example}_test PRIVATE
-                               src/main/cpp/examples/${example}/include
-                               src/test/cpp/examples/${example}/include)
-    target_compile_definitions(${example}_test PUBLIC RUNNING_FRC_TESTS)
-    target_link_libraries(${example}_test apriltag wpilibc wpilibNewCommands romiVendordep xrpVendordep gmock_main)
-  endif()
+    if(WITH_TESTS AND EXISTS ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/test/cpp/examples/${example})
+        wpilib_add_test(${example} src/test/cpp/examples/${example}/cpp)
+        target_sources(${example}_test PRIVATE ${sources})
+        target_include_directories(
+            ${example}_test
+            PRIVATE
+                src/main/cpp/examples/${example}/include
+                src/test/cpp/examples/${example}/include
+        )
+        target_compile_definitions(${example}_test PUBLIC RUNNING_FRC_TESTS)
+        target_link_libraries(
+            ${example}_test
+            apriltag
+            wpilibc
+            wpilibNewCommands
+            romiVendordep
+            xrpVendordep
+            gmock_main
+        )
+    endif()
 endforeach()
 
 foreach(template ${TEMPLATES})
-  file(GLOB_RECURSE sources src/main/cpp/templates/${template}/cpp/*.cpp
-                            src/main/cpp/templates/${template}/c/*.c)
-  add_executable(${template} ${sources})
-  wpilib_target_warnings(${template})
-  target_include_directories(${template} PUBLIC src/main/cpp/templates/${template}/include)
-  target_link_libraries(${template} wpilibc wpilibNewCommands romiVendordep xrpVendordep)
+    file(
+        GLOB_RECURSE sources
+        src/main/cpp/templates/${template}/cpp/*.cpp
+        src/main/cpp/templates/${template}/c/*.c
+    )
+    add_executable(${template} ${sources})
+    wpilib_target_warnings(${template})
+    target_include_directories(${template} PUBLIC src/main/cpp/templates/${template}/include)
+    target_link_libraries(${template} wpilibc wpilibNewCommands romiVendordep xrpVendordep)
 endforeach()
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp
index 0ed53fd..443bdd8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp
@@ -37,13 +37,13 @@
  private:
   static void VisionThread() {
     frc::AprilTagDetector detector;
-    // look for tag16h5, don't correct any error bits
-    detector.AddFamily("tag16h5", 0);
+    // look for tag36h11, correct 7 error bits
+    detector.AddFamily("tag36h11", 7);
 
     // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
     // (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
     frc::AprilTagPoseEstimator::Config poseEstConfig = {
-        .tagSize = units::length::inch_t(6.0),
+        .tagSize = units::length::inch_t(6.5),
         .fx = 699.3778103158814,
         .fy = 677.7161226393544,
         .cx = 345.6059345433618,
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
index b636b44..9c8d640 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
@@ -14,10 +14,17 @@
 class Robot : public frc::TimedRobot {
   frc::PWMSparkMax m_leftMotor{0};
   frc::PWMSparkMax m_rightMotor{1};
-  frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
   frc::Joystick m_stick{0};
 
  public:
+  Robot() {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
+  }
+
   void RobotInit() override {
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
index 23c9a56..38ff862 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
@@ -14,10 +14,17 @@
 class Robot : public frc::TimedRobot {
   frc::PWMSparkMax m_leftMotor{0};
   frc::PWMSparkMax m_rightMotor{1};
-  frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
   frc::XboxController m_driverController{0};
 
  public:
+  Robot() {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
+  }
+
   void RobotInit() override {
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
index d3a106b..13eb9f9 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
@@ -39,7 +39,7 @@
 void Robot::AutonomousInit() {
   m_autonomousCommand = m_container.GetAutonomousCommand();
 
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Schedule();
   }
 }
@@ -51,9 +51,9 @@
   // teleop starts running. If you want the autonomous to
   // continue until interrupted by another command, remove
   // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
-    m_autonomousCommand = nullptr;
+    m_autonomousCommand.reset();
   }
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
index 90aa789..1bf4dcf 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
@@ -60,6 +60,6 @@
   m_arm.Disable();
 }
 
-frc2::Command* RobotContainer::GetAutonomousCommand() {
-  return nullptr;
+frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
+  return frc2::cmd::None();
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
index aeeac3d..1352295 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
@@ -13,13 +13,20 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_right1.SetInverted(true);
 }
 
 void DriveSubsystem::Periodic() {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
index cff3932..556ee4c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
@@ -21,54 +21,54 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterInches = 6;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterInches = 6;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace ArmConstants {
-constexpr int kMotorPort = 4;
+inline constexpr int kMotorPort = 4;
 
-constexpr double kP = 1;
+inline constexpr double kP = 1;
 
 // These are fake gains; in actuality these must be determined individually for
 // each robot
-constexpr auto kS = 1_V;
-constexpr auto kG = 1_V;
-constexpr auto kV = 0.5_V * 1_s / 1_rad;
-constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+inline constexpr auto kS = 1_V;
+inline constexpr auto kG = 1_V;
+inline constexpr auto kV = 0.5_V * 1_s / 1_rad;
+inline constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
 
-constexpr auto kMaxVelocity = 3_rad_per_s;
-constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+inline constexpr auto kMaxVelocity = 3_rad_per_s;
+inline constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
 
-constexpr int kEncoderPorts[]{4, 5};
-constexpr int kEncoderPPR = 256;
-constexpr auto kEncoderDistancePerPulse =
+inline constexpr int kEncoderPorts[]{4, 5};
+inline constexpr int kEncoderPPR = 256;
+inline constexpr auto kEncoderDistancePerPulse =
     2.0_rad * std::numbers::pi / kEncoderPPR;
 
 // The offset of the arm from the horizontal in its neutral position,
 // measured from the horizontal
-constexpr auto kArmOffset = 0.5_rad;
+inline constexpr auto kArmOffset = 0.5_rad;
 }  // namespace ArmConstants
 
 namespace AutoConstants {
-constexpr auto kAutoTimeoutSeconds = 12_s;
-constexpr auto kAutoShootTimeSeconds = 7_s;
+inline constexpr auto kAutoTimeoutSeconds = 12_s;
+inline constexpr auto kAutoShootTimeSeconds = 7_s;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
index a82f2ac..de9c814 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
@@ -24,7 +26,7 @@
  private:
   // Have it null by default so that if testing teleop it
   // doesn't have undefined behavior and potentially crash.
-  frc2::Command* m_autonomousCommand = nullptr;
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
index 67d194c..ab77985 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/button/CommandXboxController.h>
 
 #include "Constants.h"
@@ -28,7 +29,7 @@
    */
   void DisablePIDSubsystems();
 
-  frc2::Command* GetAutonomousCommand();
+  frc2::CommandPtr GetAutonomousCommand();
 
  private:
   // The driver's controller
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
index 47bf28e..47d3f3d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
@@ -6,7 +6,6 @@
 
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
@@ -75,14 +74,9 @@
   frc::PWMSparkMax m_right1;
   frc::PWMSparkMax m_right2;
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
index c7eab48..c4c1661 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
@@ -37,7 +37,7 @@
 void Robot::AutonomousInit() {
   m_autonomousCommand = m_container.GetAutonomousCommand();
 
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Schedule();
   }
 }
@@ -49,9 +49,9 @@
   // teleop starts running. If you want the autonomous to
   // continue until interrupted by another command, remove
   // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
-    m_autonomousCommand = nullptr;
+    m_autonomousCommand.reset();
   }
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
index dce4b95..a870416 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
@@ -4,6 +4,7 @@
 
 #include "RobotContainer.h"
 
+#include <frc2/command/Commands.h>
 #include <units/angle.h>
 
 RobotContainer::RobotContainer() {
@@ -34,6 +35,6 @@
       .OnFalse(m_drive.SetMaxOutputCommand(1.0));
 }
 
-frc2::Command* RobotContainer::GetAutonomousCommand() {
-  return nullptr;
+frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
+  return frc2::cmd::None();
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
index 16409ad..236d468 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -13,13 +13,20 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_right1.SetInverted(true);
 }
 
 void DriveSubsystem::Periodic() {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
index cff3932..b91fe19 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
@@ -21,52 +21,52 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterInches = 6;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterInches = 6;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace ArmConstants {
-constexpr int kMotorPort = 4;
+inline constexpr int kMotorPort = 4;
 
-constexpr double kP = 1;
+inline constexpr double kP = 1;
 
 // These are fake gains; in actuality these must be determined individually for
 // each robot
-constexpr auto kS = 1_V;
-constexpr auto kG = 1_V;
-constexpr auto kV = 0.5_V * 1_s / 1_rad;
-constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+inline constexpr auto kS = 1_V;
+inline constexpr auto kG = 1_V;
+inline constexpr auto kV = 0.5_V * 1_s / 1_rad;
+inline constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
 
-constexpr auto kMaxVelocity = 3_rad_per_s;
-constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+inline constexpr auto kMaxVelocity = 3_rad_per_s;
+inline constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
 
-constexpr int kEncoderPorts[]{4, 5};
-constexpr int kEncoderPPR = 256;
-constexpr auto kEncoderDistancePerPulse =
+inline constexpr int kEncoderPorts[]{4, 5};
+inline constexpr int kEncoderPPR = 256;
+inline constexpr auto kEncoderDistancePerPulse =
     2.0_rad * std::numbers::pi / kEncoderPPR;
 
 // The offset of the arm from the horizontal in its neutral position,
 // measured from the horizontal
-constexpr auto kArmOffset = 0.5_rad;
+inline constexpr auto kArmOffset = 0.5_rad;
 }  // namespace ArmConstants
 
 namespace AutoConstants {
-constexpr auto kAutoTimeoutSeconds = 12_s;
-constexpr auto kAutoShootTimeSeconds = 7_s;
+inline constexpr auto kAutoTimeoutSeconds = 12_s;
+inline constexpr auto kAutoShootTimeSeconds = 7_s;
 }  // namespace AutoConstants
 
 namespace OIConstants {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
index 5d55839..bd48d88 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
@@ -4,15 +4,13 @@
 
 #pragma once
 
-#include <frc/motorcontrol/MotorController.h>
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor
  * controller.
  *
  * <p>Has no actual functionality.
  */
-class ExampleSmartMotorController : public frc::MotorController {
+class ExampleSmartMotorController {
  public:
   enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
 
@@ -68,15 +66,15 @@
    */
   void ResetEncoder() {}
 
-  void Set(double speed) override {}
+  void Set(double speed) {}
 
-  double Get() const override { return 0; }
+  double Get() const { return 0; }
 
-  void SetInverted(bool isInverted) override {}
+  void SetInverted(bool isInverted) {}
 
-  bool GetInverted() const override { return false; }
+  bool GetInverted() const { return false; }
 
-  void Disable() override {}
+  void Disable() {}
 
-  void StopMotor() override {}
+  void StopMotor() {}
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
index a82f2ac..de9c814 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
@@ -24,7 +26,7 @@
  private:
   // Have it null by default so that if testing teleop it
   // doesn't have undefined behavior and potentially crash.
-  frc2::Command* m_autonomousCommand = nullptr;
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
index 08a4dbe..7668c03 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/button/CommandXboxController.h>
 
 #include "Constants.h"
@@ -24,7 +25,7 @@
  public:
   RobotContainer();
 
-  frc2::Command* GetAutonomousCommand();
+  frc2::CommandPtr GetAutonomousCommand();
 
  private:
   // The driver's controller
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
index 6830b96..ca7e28e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
@@ -8,7 +8,6 @@
 
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/Commands.h>
 #include <frc2/command/SubsystemBase.h>
@@ -73,14 +72,9 @@
   frc::PWMSparkMax m_right1;
   frc::PWMSparkMax m_right2;
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h
index 0018a8a..c5883db 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h
@@ -22,25 +22,25 @@
  * they are needed.
  */
 
-static constexpr int kMotorPort = 0;
-static constexpr int kEncoderAChannel = 0;
-static constexpr int kEncoderBChannel = 1;
-static constexpr int kJoystickPort = 0;
+inline constexpr int kMotorPort = 0;
+inline constexpr int kEncoderAChannel = 0;
+inline constexpr int kEncoderBChannel = 1;
+inline constexpr int kJoystickPort = 0;
 
-static constexpr std::string_view kArmPositionKey = "ArmPosition";
-static constexpr std::string_view kArmPKey = "ArmP";
+inline constexpr std::string_view kArmPositionKey = "ArmPosition";
+inline constexpr std::string_view kArmPKey = "ArmP";
 
-static constexpr double kDefaultArmKp = 50.0;
-static constexpr units::degree_t kDefaultArmSetpoint = 75.0_deg;
+inline constexpr double kDefaultArmKp = 50.0;
+inline constexpr units::degree_t kDefaultArmSetpoint = 75.0_deg;
 
-static constexpr units::radian_t kMinAngle = -75.0_deg;
-static constexpr units::radian_t kMaxAngle = 255.0_deg;
+inline constexpr units::radian_t kMinAngle = -75.0_deg;
+inline constexpr units::radian_t kMaxAngle = 255.0_deg;
 
-static constexpr double kArmReduction = 200.0;
-static constexpr units::kilogram_t kArmMass = 8.0_kg;
-static constexpr units::meter_t kArmLength = 30.0_in;
+inline constexpr double kArmReduction = 200.0;
+inline constexpr units::kilogram_t kArmMass = 8.0_kg;
+inline constexpr units::meter_t kArmLength = 30.0_in;
 
 // distance per pulse = (angle per revolution) / (pulses per revolution)
 //  = (2 * PI rads) / (4096 pulses)
-static constexpr double kArmEncoderDistPerPulse =
+inline constexpr double kArmEncoderDistPerPulse =
     2.0 * std::numbers::pi / 4096.0;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
index cc7db7b..9470a7a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
@@ -12,8 +12,8 @@
   const double rightOutput = m_rightPIDController.Calculate(
       m_rightEncoder.GetRate(), speeds.right.value());
 
-  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
-  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+  m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
 }
 
 void Drivetrain::Drive(units::meters_per_second_t xSpeed,
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
index 54b2e26..85bccc7 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -12,7 +12,6 @@
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <units/angle.h>
 #include <units/angular_velocity.h>
@@ -25,10 +24,13 @@
 class Drivetrain {
  public:
   Drivetrain() {
+    m_leftLeader.AddFollower(m_leftFollower);
+    m_rightLeader.AddFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.SetInverted(true);
+    m_rightLeader.SetInverted(true);
 
     m_gyro.Reset();
     // Set the distance per pulse for the drive encoders. We can simply use the
@@ -63,9 +65,6 @@
   frc::PWMSparkMax m_rightLeader{3};
   frc::PWMSparkMax m_rightFollower{4};
 
-  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
-  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
-
   frc::Encoder m_leftEncoder{0, 1};
   frc::Encoder m_rightEncoder{2, 3};
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
index 725074a..caba17d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -7,10 +7,13 @@
 #include "ExampleGlobalMeasurementSensor.h"
 
 Drivetrain::Drivetrain() {
+  m_leftLeader.AddFollower(m_leftFollower);
+  m_rightLeader.AddFollower(m_rightFollower);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightGroup.SetInverted(true);
+  m_rightLeader.SetInverted(true);
 
   m_gyro.Reset();
 
@@ -37,8 +40,8 @@
   const double rightOutput = m_rightPIDController.Calculate(
       m_rightEncoder.GetRate(), speeds.right.value());
 
-  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
-  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+  m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
 }
 
 void Drivetrain::Drive(units::meters_per_second_t xSpeed,
@@ -110,9 +113,9 @@
   // To update our simulation, we set motor voltage inputs, update the
   // simulation, and write the simulated positions and velocities to our
   // simulated encoder and gyro.
-  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} *
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} *
                                       frc::RobotController::GetInputVoltage(),
-                                  units::volt_t{m_rightGroup.Get()} *
+                                  units::volt_t{m_rightLeader.Get()} *
                                       frc::RobotController::GetInputVoltage());
   m_drivetrainSimulator.Update(20_ms);
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
index ac4de4c..63a9aea 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
@@ -21,7 +21,6 @@
 #include <frc/geometry/Quaternion.h>
 #include <frc/geometry/Transform3d.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/simulation/AnalogGyroSim.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
@@ -140,9 +139,6 @@
   frc::PWMSparkMax m_rightLeader{3};
   frc::PWMSparkMax m_rightFollower{4};
 
-  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
-  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
-
   frc::Encoder m_leftEncoder{0, 1};
   frc::Encoder m_rightEncoder{2, 3};
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
index c7eab48..c4c1661 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
@@ -37,7 +37,7 @@
 void Robot::AutonomousInit() {
   m_autonomousCommand = m_container.GetAutonomousCommand();
 
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Schedule();
   }
 }
@@ -49,9 +49,9 @@
   // teleop starts running. If you want the autonomous to
   // continue until interrupted by another command, remove
   // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
-    m_autonomousCommand = nullptr;
+    m_autonomousCommand.reset();
   }
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
index bcb7e73..37ae1aa 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
@@ -4,6 +4,8 @@
 
 #include "RobotContainer.h"
 
+#include <frc2/command/Commands.h>
+
 #include "commands/DriveDistanceProfiled.h"
 
 RobotContainer::RobotContainer() {
@@ -60,7 +62,7 @@
           .WithTimeout(10_s));
 }
 
-frc2::Command* RobotContainer::GetAutonomousCommand() {
+frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // Runs the chosen command in autonomous
-  return nullptr;
+  return frc2::cmd::None();
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
index db8ba70..84fae4f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -12,16 +12,14 @@
       m_rightLeader{kRightMotor1Port},
       m_rightFollower{kRightMotor2Port},
       m_feedforward{ks, kv, ka} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
   m_rightLeader.SetInverted(true);
 
-  // You might need to not do this depending on the specific motor controller
-  // that you are using -- contact the respective vendor's documentation for
-  // more details.
-  m_rightFollower.SetInverted(true);
-
   m_leftFollower.Follow(m_leftLeader);
   m_rightFollower.Follow(m_rightLeader);
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
index 7b7de40..c9ec7e7 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
@@ -22,26 +22,26 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
 // theoretically for *your* robot's drive. The SysId tool provides a convenient
 // method for obtaining these values for your robot.
-constexpr auto ks = 1_V;
-constexpr auto kv = 0.8_V * 1_s / 1_m;
-constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
+inline constexpr auto ks = 1_V;
+inline constexpr auto kv = 0.8_V * 1_s / 1_m;
+inline constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
 
-constexpr double kp = 1;
+inline constexpr double kp = 1;
 
-constexpr auto kMaxSpeed = 3_mps;
-constexpr auto kMaxAcceleration = 3_mps_sq;
+inline constexpr auto kMaxSpeed = 3_mps;
+inline constexpr auto kMaxAcceleration = 3_mps_sq;
 
 }  // namespace DriveConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
index 71dc4d4..fe09a15 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
@@ -4,15 +4,13 @@
 
 #pragma once
 
-#include <frc/motorcontrol/MotorController.h>
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor
  * controller.
  *
  * <p>Has no actual functionality.
  */
-class ExampleSmartMotorController : public frc::MotorController {
+class ExampleSmartMotorController {
  public:
   enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
 
@@ -68,17 +66,17 @@
    */
   void ResetEncoder() {}
 
-  void Set(double speed) override { m_value = speed; }
+  void Set(double speed) { m_value = speed; }
 
-  double Get() const override { return m_value; }
+  double Get() const { return m_value; }
 
-  void SetInverted(bool isInverted) override {}
+  void SetInverted(bool isInverted) {}
 
-  bool GetInverted() const override { return false; }
+  bool GetInverted() const { return false; }
 
-  void Disable() override {}
+  void Disable() {}
 
-  void StopMotor() override {}
+  void StopMotor() {}
 
  private:
   double m_value = 0.0;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
index a82f2ac..de9c814 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
@@ -24,7 +26,7 @@
  private:
   // Have it null by default so that if testing teleop it
   // doesn't have undefined behavior and potentially crash.
-  frc2::Command* m_autonomousCommand = nullptr;
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
index 4700a70..6bfda58 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
@@ -23,7 +23,7 @@
  public:
   RobotContainer();
 
-  frc2::Command* GetAutonomousCommand();
+  frc2::CommandPtr GetAutonomousCommand();
 
  private:
   // The driver's controller
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
index 9086353..3dd5f03 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
@@ -83,5 +83,7 @@
   frc::SimpleMotorFeedforward<units::meters> m_feedforward;
 
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftLeader, m_rightLeader};
+  frc::DifferentialDrive m_drive{
+      [&](double output) { m_leftLeader.Set(output); },
+      [&](double output) { m_rightLeader.Set(output); }};
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h
index 5d55839..bd48d88 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h
@@ -4,15 +4,13 @@
 
 #pragma once
 
-#include <frc/motorcontrol/MotorController.h>
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor
  * controller.
  *
  * <p>Has no actual functionality.
  */
-class ExampleSmartMotorController : public frc::MotorController {
+class ExampleSmartMotorController {
  public:
   enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
 
@@ -68,15 +66,15 @@
    */
   void ResetEncoder() {}
 
-  void Set(double speed) override {}
+  void Set(double speed) {}
 
-  double Get() const override { return 0; }
+  double Get() const { return 0; }
 
-  void SetInverted(bool isInverted) override {}
+  void SetInverted(bool isInverted) {}
 
-  bool GetInverted() const override { return false; }
+  bool GetInverted() const { return false; }
 
-  void Disable() override {}
+  void Disable() {}
 
-  void StopMotor() override {}
+  void StopMotor() {}
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h
index 7c53018..65d98157 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h
@@ -25,33 +25,33 @@
 
 namespace Constants {
 
-static constexpr int kMotorPort = 0;
-static constexpr int kEncoderAChannel = 0;
-static constexpr int kEncoderBChannel = 1;
-static constexpr int kJoystickPort = 0;
+inline constexpr int kMotorPort = 0;
+inline constexpr int kEncoderAChannel = 0;
+inline constexpr int kEncoderBChannel = 1;
+inline constexpr int kJoystickPort = 0;
 
-static constexpr double kElevatorKp = 0.75;
-static constexpr double kElevatorKi = 0.0;
-static constexpr double kElevatorKd = 0.0;
+inline constexpr double kElevatorKp = 0.75;
+inline constexpr double kElevatorKi = 0.0;
+inline constexpr double kElevatorKd = 0.0;
 
-static constexpr units::volt_t kElevatorMaxV = 10_V;
-static constexpr units::volt_t kElevatorkS = 0.0_V;
-static constexpr units::volt_t kElevatorkG = 0.62_V;
-static constexpr auto kElevatorkV = 3.9_V / 1_mps;
-static constexpr auto kElevatorkA = 0.06_V / 1_mps_sq;
+inline constexpr units::volt_t kElevatorMaxV = 10_V;
+inline constexpr units::volt_t kElevatorkS = 0.0_V;
+inline constexpr units::volt_t kElevatorkG = 0.62_V;
+inline constexpr auto kElevatorkV = 3.9_V / 1_mps;
+inline constexpr auto kElevatorkA = 0.06_V / 1_mps_sq;
 
-static constexpr double kElevatorGearing = 5.0;
-static constexpr units::meter_t kElevatorDrumRadius = 1_in;
-static constexpr units::kilogram_t kCarriageMass = 12_lb;
+inline constexpr double kElevatorGearing = 5.0;
+inline constexpr units::meter_t kElevatorDrumRadius = 1_in;
+inline constexpr units::kilogram_t kCarriageMass = 12_lb;
 
-static constexpr units::meter_t kSetpoint = 42.875_in;
-static constexpr units::meter_t kLowerSetpoint = 15_in;
-static constexpr units::meter_t kMinElevatorHeight = 0_cm;
-static constexpr units::meter_t kMaxElevatorHeight = 50_in;
+inline constexpr units::meter_t kSetpoint = 42.875_in;
+inline constexpr units::meter_t kLowerSetpoint = 15_in;
+inline constexpr units::meter_t kMinElevatorHeight = 0_cm;
+inline constexpr units::meter_t kMaxElevatorHeight = 50_in;
 
 // distance per pulse = (distance per revolution) / (pulses per revolution)
 //  = (Pi * D) / ppr
-static constexpr double kArmEncoderDistPerPulse =
+inline constexpr double kArmEncoderDistPerPulse =
     2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
 
 }  // namespace Constants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h
index 7be706f..1123dc0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h
@@ -25,31 +25,31 @@
 
 namespace Constants {
 
-static constexpr int kMotorPort = 0;
-static constexpr int kEncoderAChannel = 0;
-static constexpr int kEncoderBChannel = 1;
-static constexpr int kJoystickPort = 0;
+inline constexpr int kMotorPort = 0;
+inline constexpr int kEncoderAChannel = 0;
+inline constexpr int kEncoderBChannel = 1;
+inline constexpr int kJoystickPort = 0;
 
-static constexpr double kElevatorKp = 5.0;
-static constexpr double kElevatorKi = 0.0;
-static constexpr double kElevatorKd = 0.0;
+inline constexpr double kElevatorKp = 5.0;
+inline constexpr double kElevatorKi = 0.0;
+inline constexpr double kElevatorKd = 0.0;
 
-static constexpr units::volt_t kElevatorkS = 0.0_V;
-static constexpr units::volt_t kElevatorkG = 0.762_V;
-static constexpr auto kElevatorkV = 0.762_V / 1_mps;
-static constexpr auto kElevatorkA = 0.0_V / 1_mps_sq;
+inline constexpr units::volt_t kElevatorkS = 0.0_V;
+inline constexpr units::volt_t kElevatorkG = 0.762_V;
+inline constexpr auto kElevatorkV = 0.762_V / 1_mps;
+inline constexpr auto kElevatorkA = 0.0_V / 1_mps_sq;
 
-static constexpr double kElevatorGearing = 10.0;
-static constexpr units::meter_t kElevatorDrumRadius = 2_in;
-static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
+inline constexpr double kElevatorGearing = 10.0;
+inline constexpr units::meter_t kElevatorDrumRadius = 2_in;
+inline constexpr units::kilogram_t kCarriageMass = 4.0_kg;
 
-static constexpr units::meter_t kSetpoint = 75_cm;
-static constexpr units::meter_t kMinElevatorHeight = 0_cm;
-static constexpr units::meter_t kMaxElevatorHeight = 1.25_m;
+inline constexpr units::meter_t kSetpoint = 75_cm;
+inline constexpr units::meter_t kMinElevatorHeight = 0_cm;
+inline constexpr units::meter_t kMaxElevatorHeight = 1.25_m;
 
 // distance per pulse = (distance per revolution) / (pulses per revolution)
 //  = (Pi * D) / ppr
-static constexpr double kArmEncoderDistPerPulse =
+inline constexpr double kArmEncoderDistPerPulse =
     2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
 
 }  // namespace Constants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
index fb3a70d..63cd55d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -34,7 +34,7 @@
 
     // Retrieve the profiled setpoint for the next timestep. This setpoint moves
     // toward the goal while obeying the constraints.
-    m_setpoint = m_profile.Calculate(kDt, m_goal, m_setpoint);
+    m_setpoint = m_profile.Calculate(kDt, m_setpoint, m_goal);
 
     // Send setpoint to offboard controller PID
     m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
index 5d55839..bd48d88 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
@@ -4,15 +4,13 @@
 
 #pragma once
 
-#include <frc/motorcontrol/MotorController.h>
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor
  * controller.
  *
  * <p>Has no actual functionality.
  */
-class ExampleSmartMotorController : public frc::MotorController {
+class ExampleSmartMotorController {
  public:
   enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
 
@@ -68,15 +66,15 @@
    */
   void ResetEncoder() {}
 
-  void Set(double speed) override {}
+  void Set(double speed) {}
 
-  double Get() const override { return 0; }
+  double Get() const { return 0; }
 
-  void SetInverted(bool isInverted) override {}
+  void SetInverted(bool isInverted) {}
 
-  bool GetInverted() const override { return false; }
+  bool GetInverted() const { return false; }
 
-  void Disable() override {}
+  void Disable() {}
 
-  void StopMotor() override {}
+  void StopMotor() {}
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
index f40a649..0e4068f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
@@ -13,10 +13,16 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_leftMotors.SetInverted(true);
+  m_left1.SetInverted(true);
 
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
index 855603a..d3aab41 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -20,56 +20,56 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterInches = 6;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterInches = 6;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace ShooterConstants {
-constexpr int kEncoderPorts[]{4, 5};
-constexpr bool kEncoderReversed = false;
-constexpr int kEncoderCPR = 1024;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderPorts[]{4, 5};
+inline constexpr bool kEncoderReversed = false;
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kEncoderDistancePerPulse =
     // Distance units will be rotations
     1.0 / static_cast<double>(kEncoderCPR);
 
-constexpr int kShooterMotorPort = 4;
-constexpr int kFeederMotorPort = 5;
+inline constexpr int kShooterMotorPort = 4;
+inline constexpr int kFeederMotorPort = 5;
 
-constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
-constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
-constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
+inline constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
+inline constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
+inline constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
 
-constexpr double kP = 1;
-constexpr double kI = 0;
-constexpr double kD = 0;
+inline constexpr double kP = 1;
+inline constexpr double kI = 0;
+inline constexpr double kD = 0;
 
 // On a real robot the feedforward constants should be empirically determined;
 // these are reasonable guesses.
-constexpr auto kS = 0.05_V;
-constexpr auto kV =
+inline constexpr auto kS = 0.05_V;
+inline constexpr auto kV =
     // Should have value 12V at free speed...
     12_V / kShooterFreeRPS;
 
-constexpr double kFeederSpeed = 0.5;
+inline constexpr double kFeederSpeed = 0.5;
 }  // namespace ShooterConstants
 
 namespace AutoConstants {
-constexpr auto kAutoTimeoutSeconds = 12_s;
-constexpr auto kAutoShootTimeSeconds = 7_s;
+inline constexpr auto kAutoTimeoutSeconds = 12_s;
+inline constexpr auto kAutoShootTimeSeconds = 7_s;
 }  // namespace AutoConstants
 
 namespace OIConstants {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
index 47bf28e..47d3f3d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
@@ -6,7 +6,6 @@
 
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
@@ -75,14 +74,9 @@
   frc::PWMSparkMax m_right1;
   frc::PWMSparkMax m_right2;
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp
index 2bfd391..2928036 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp
@@ -11,10 +11,16 @@
 #include <units/length.h>
 
 Drivetrain::Drivetrain() {
+  wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
+  wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
+
+  m_frontLeft.AddFollower(m_rearLeft);
+  m_frontRight.AddFollower(m_rearRight);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_right.SetInverted(true);
+  m_frontRight.SetInverted(true);
 
 // Encoders may measure differently in the real world and in
 // simulation. In this example the robot moves 0.042 barleycorns
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h
index 9e739c4..cc3c95f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h
@@ -8,7 +8,6 @@
 #include <frc/AnalogInput.h>
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
@@ -66,13 +65,13 @@
  private:
   frc::PWMSparkMax m_frontLeft{1};
   frc::PWMSparkMax m_rearLeft{2};
-  frc::MotorControllerGroup m_left{m_frontLeft, m_rearLeft};
 
   frc::PWMSparkMax m_frontRight{3};
   frc::PWMSparkMax m_rearRight{4};
-  frc::MotorControllerGroup m_right{m_frontRight, m_rearRight};
 
-  frc::DifferentialDrive m_robotDrive{m_left, m_right};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_frontLeft.Set(output); },
+      [&](double output) { m_frontRight.Set(output); }};
 
   frc::Encoder m_leftEncoder{1, 2};
   frc::Encoder m_rightEncoder{3, 4};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
index 01b7210..0a73f14 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
@@ -11,6 +11,9 @@
 class Robot : public frc::TimedRobot {
  public:
   Robot() {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_left);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_right);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
@@ -48,7 +51,9 @@
   // Robot drive system
   frc::PWMSparkMax m_left{0};
   frc::PWMSparkMax m_right{1};
-  frc::DifferentialDrive m_robotDrive{m_left, m_right};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_left.Set(output); },
+      [&](double output) { m_right.Set(output); }};
 
   frc::XboxController m_controller{0};
   frc::Timer m_timer;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
index 5230c7c..d8af410 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
@@ -17,6 +17,11 @@
  */
 class Robot : public frc::TimedRobot {
  public:
+  Robot() {
+    wpi::SendableRegistry::AddChild(&m_drive, &m_left);
+    wpi::SendableRegistry::AddChild(&m_drive, &m_right);
+  }
+
   void RobotInit() override {
     m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
     // We need to invert one side of the drivetrain so that positive voltages
@@ -50,7 +55,8 @@
 
   frc::PWMSparkMax m_left{kLeftMotorPort};
   frc::PWMSparkMax m_right{kRightMotorPort};
-  frc::DifferentialDrive m_drive{m_left, m_right};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left.Set(output); },
+                                 [&](double output) { m_right.Set(output); }};
 
   frc::AnalogGyro m_gyro{kGyroPort};
   frc::Joystick m_joystick{kJoystickPort};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
index c7eab48..c4c1661 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
@@ -37,7 +37,7 @@
 void Robot::AutonomousInit() {
   m_autonomousCommand = m_container.GetAutonomousCommand();
 
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Schedule();
   }
 }
@@ -49,9 +49,9 @@
   // teleop starts running. If you want the autonomous to
   // continue until interrupted by another command, remove
   // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
-    m_autonomousCommand = nullptr;
+    m_autonomousCommand.reset();
   }
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
index 50b9900..7b07684 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
@@ -64,7 +64,7 @@
       .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {}));
 }
 
-frc2::Command* RobotContainer::GetAutonomousCommand() {
+frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // no auto
-  return nullptr;
+  return frc2::cmd::None();
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
index 0cbd0e5..a97a5c0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
@@ -13,10 +13,16 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_right1.SetInverted(true);
 
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
index 77673c9..4d25b0b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
@@ -19,46 +19,46 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterInches = 6;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterInches = 6;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 
-constexpr bool kGyroReversed = true;
+inline constexpr bool kGyroReversed = true;
 
-constexpr double kStabilizationP = 1;
-constexpr double kStabilizationI = 0.5;
-constexpr double kStabilizationD = 0;
+inline constexpr double kStabilizationP = 1;
+inline constexpr double kStabilizationI = 0.5;
+inline constexpr double kStabilizationD = 0;
 
-constexpr double kTurnP = 1;
-constexpr double kTurnI = 0;
-constexpr double kTurnD = 0;
+inline constexpr double kTurnP = 1;
+inline constexpr double kTurnI = 0;
+inline constexpr double kTurnD = 0;
 
-constexpr auto kTurnTolerance = 5_deg;
-constexpr auto kTurnRateTolerance = 10_deg_per_s;
+inline constexpr auto kTurnTolerance = 5_deg;
+inline constexpr auto kTurnRateTolerance = 10_deg_per_s;
 
-constexpr auto kMaxTurnRate = 100_deg_per_s;
-constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
+inline constexpr auto kMaxTurnRate = 100_deg_per_s;
+inline constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
 }  // namespace DriveConstants
 
 namespace AutoConstants {
-constexpr double kAutoDriveDistanceInches = 60;
-constexpr double kAutoBackupDistanceInches = 20;
-constexpr double kAutoDriveSpeed = 0.5;
+inline constexpr double kAutoDriveDistanceInches = 60;
+inline constexpr double kAutoBackupDistanceInches = 20;
+inline constexpr double kAutoDriveSpeed = 0.5;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
index a82f2ac..de9c814 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
@@ -24,7 +26,7 @@
  private:
   // Have it null by default so that if testing teleop it
   // doesn't have undefined behavior and potentially crash.
-  frc2::Command* m_autonomousCommand = nullptr;
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
index 041812e..0ecb728 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
@@ -8,6 +8,7 @@
 #include <frc/controller/PIDController.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/InstantCommand.h>
 
 #include "Constants.h"
@@ -28,7 +29,7 @@
  public:
   RobotContainer();
 
-  frc2::Command* GetAutonomousCommand();
+  frc2::CommandPtr GetAutonomousCommand();
 
  private:
   // The driver's controller
@@ -39,8 +40,5 @@
   // The robot's subsystems
   DriveSubsystem m_drive;
 
-  // The chooser for the autonomous routines
-  frc::SendableChooser<frc2::Command*> m_chooser;
-
   void ConfigureButtonBindings();
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
index 96174dd..d7cce6c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
@@ -7,7 +7,6 @@
 #include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/angle.h>
@@ -91,14 +90,9 @@
   frc::PWMSparkMax m_right1;
   frc::PWMSparkMax m_right2;
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
index 2207f79..7c589fd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
@@ -16,6 +16,11 @@
 class Robot : public frc::TimedRobot {
  public:
   void RobotInit() override {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearLeft);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearRight);
+
     // Invert the right side motors. You may need to change or remove this to
     // match your robot.
     m_frontRight.SetInverted(true);
@@ -48,8 +53,11 @@
   frc::PWMSparkMax m_rearLeft{kRearLeftMotorPort};
   frc::PWMSparkMax m_frontRight{kFrontRightMotorPort};
   frc::PWMSparkMax m_rearRight{kRearRightMotorPort};
-  frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
-                                 m_rearRight};
+  frc::MecanumDrive m_robotDrive{
+      [&](double output) { m_frontLeft.Set(output); },
+      [&](double output) { m_rearLeft.Set(output); },
+      [&](double output) { m_frontRight.Set(output); },
+      [&](double output) { m_rearRight.Set(output); }};
 
   frc::AnalogGyro m_gyro{kGyroPort};
   frc::Joystick m_joystick{kJoystickPort};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
index 3372a4d..d7b33ea 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
@@ -15,10 +15,16 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_right1.SetInverted(true);
 
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
index 7a2bdae..f82b8db 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
@@ -16,35 +16,35 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterInches = 6;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterInches = 6;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace HatchConstants {
-constexpr int kHatchSolenoidModule = 0;
-constexpr int kHatchSolenoidPorts[]{0, 1};
+inline constexpr int kHatchSolenoidModule = 0;
+inline constexpr int kHatchSolenoidPorts[]{0, 1};
 }  // namespace HatchConstants
 
 namespace AutoConstants {
-constexpr double kAutoDriveDistanceInches = 60;
-constexpr double kAutoBackupDistanceInches = 20;
-constexpr double kAutoDriveSpeed = 0.5;
+inline constexpr double kAutoDriveDistanceInches = 60;
+inline constexpr double kAutoBackupDistanceInches = 20;
+inline constexpr double kAutoDriveSpeed = 0.5;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
index 5984a1a..6cab580 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
@@ -6,7 +6,6 @@
 
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
@@ -63,14 +62,9 @@
   frc::PWMSparkMax m_right1;
   frc::PWMSparkMax m_right2;
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
index 3372a4d..d7b33ea 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
@@ -15,10 +15,16 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_right1.SetInverted(true);
 
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
index 7a2bdae..534d8f1 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
@@ -16,27 +16,27 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterInches = 6;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterInches = 6;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace HatchConstants {
-constexpr int kHatchSolenoidModule = 0;
-constexpr int kHatchSolenoidPorts[]{0, 1};
+inline constexpr int kHatchSolenoidModule = 0;
+inline constexpr int kHatchSolenoidPorts[]{0, 1};
 }  // namespace HatchConstants
 
 namespace AutoConstants {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
index 5984a1a..6cab580 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
@@ -6,7 +6,6 @@
 
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
@@ -63,14 +62,9 @@
   frc::PWMSparkMax m_right1;
   frc::PWMSparkMax m_right2;
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
index c7eab48..c4c1661 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
@@ -37,7 +37,7 @@
 void Robot::AutonomousInit() {
   m_autonomousCommand = m_container.GetAutonomousCommand();
 
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Schedule();
   }
 }
@@ -49,9 +49,9 @@
   // teleop starts running. If you want the autonomous to
   // continue until interrupted by another command, remove
   // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
-    m_autonomousCommand = nullptr;
+    m_autonomousCommand.reset();
   }
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
index 0f691e0..856ec99 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
@@ -12,6 +12,7 @@
 #include <frc/trajectory/Trajectory.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
+#include <frc2/command/Commands.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/MecanumControllerCommand.h>
 #include <frc2/command/SequentialCommandGroup.h>
@@ -47,7 +48,7 @@
       .OnFalse(&m_driveFullSpeed);
 }
 
-frc2::Command* RobotContainer::GetAutonomousCommand() {
+frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // Set up config for trajectory
   frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
                                AutoConstants::kMaxAcceleration);
@@ -65,48 +66,57 @@
       // Pass the config
       config);
 
-  frc2::MecanumControllerCommand mecanumControllerCommand(
-      exampleTrajectory, [this]() { return m_drive.GetPose(); },
+  frc2::CommandPtr mecanumControllerCommand =
+      frc2::MecanumControllerCommand(
+          exampleTrajectory, [this]() { return m_drive.GetPose(); },
 
-      frc::SimpleMotorFeedforward<units::meters>(ks, kv, ka),
-      DriveConstants::kDriveKinematics,
+          frc::SimpleMotorFeedforward<units::meters>(ks, kv, ka),
+          DriveConstants::kDriveKinematics,
 
-      frc::PIDController{AutoConstants::kPXController, 0, 0},
-      frc::PIDController{AutoConstants::kPYController, 0, 0},
-      frc::ProfiledPIDController<units::radians>(
-          AutoConstants::kPThetaController, 0, 0,
-          AutoConstants::kThetaControllerConstraints),
+          frc::PIDController{AutoConstants::kPXController, 0, 0},
+          frc::PIDController{AutoConstants::kPYController, 0, 0},
+          frc::ProfiledPIDController<units::radians>(
+              AutoConstants::kPThetaController, 0, 0,
+              AutoConstants::kThetaControllerConstraints),
 
-      AutoConstants::kMaxSpeed,
+          AutoConstants::kMaxSpeed,
 
-      [this]() {
-        return frc::MecanumDriveWheelSpeeds{
-            units::meters_per_second_t{m_drive.GetFrontLeftEncoder().GetRate()},
-            units::meters_per_second_t{
-                m_drive.GetFrontRightEncoder().GetRate()},
-            units::meters_per_second_t{m_drive.GetRearLeftEncoder().GetRate()},
-            units::meters_per_second_t{
-                m_drive.GetRearRightEncoder().GetRate()}};
-      },
+          [this]() {
+            return frc::MecanumDriveWheelSpeeds{
+                units::meters_per_second_t{
+                    m_drive.GetFrontLeftEncoder().GetRate()},
+                units::meters_per_second_t{
+                    m_drive.GetFrontRightEncoder().GetRate()},
+                units::meters_per_second_t{
+                    m_drive.GetRearLeftEncoder().GetRate()},
+                units::meters_per_second_t{
+                    m_drive.GetRearRightEncoder().GetRate()}};
+          },
 
-      frc::PIDController{DriveConstants::kPFrontLeftVel, 0, 0},
-      frc::PIDController{DriveConstants::kPRearLeftVel, 0, 0},
-      frc::PIDController{DriveConstants::kPFrontRightVel, 0, 0},
-      frc::PIDController{DriveConstants::kPRearRightVel, 0, 0},
+          frc::PIDController{DriveConstants::kPFrontLeftVel, 0, 0},
+          frc::PIDController{DriveConstants::kPRearLeftVel, 0, 0},
+          frc::PIDController{DriveConstants::kPFrontRightVel, 0, 0},
+          frc::PIDController{DriveConstants::kPRearRightVel, 0, 0},
 
-      [this](units::volt_t frontLeft, units::volt_t rearLeft,
-             units::volt_t frontRight, units::volt_t rearRight) {
-        m_drive.SetMotorControllersVolts(frontLeft, rearLeft, frontRight,
-                                         rearRight);
-      },
+          [this](units::volt_t frontLeft, units::volt_t rearLeft,
+                 units::volt_t frontRight, units::volt_t rearRight) {
+            m_drive.SetMotorControllersVolts(frontLeft, rearLeft, frontRight,
+                                             rearRight);
+          },
 
-      {&m_drive});
+          {&m_drive})
+          .ToPtr();
 
-  // Reset odometry to the starting pose of the trajectory.
-  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
-
-  // no auto
-  return new frc2::SequentialCommandGroup(
+  // Reset odometry to the initial pose of the trajectory, run path following
+  // command, then stop at the end.
+  return frc2::cmd::Sequence(
+      frc2::InstantCommand(
+          [this, &exampleTrajectory]() {
+            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          },
+          {})
+          .ToPtr(),
       std::move(mecanumControllerCommand),
-      frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {}));
+      frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {})
+          .ToPtr());
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
index 292ad1f..8a55821 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -30,6 +30,11 @@
 
       m_odometry{kDriveKinematics, m_gyro.GetRotation2d(),
                  getCurrentWheelDistances(), frc::Pose2d{}} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_frontLeft);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_rearLeft);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_frontRight);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_rearRight);
+
   // Set the distance per pulse for the encoders
   m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
index 527504a..7dde79c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
@@ -28,30 +28,30 @@
  */
 
 namespace DriveConstants {
-constexpr int kFrontLeftMotorPort = 0;
-constexpr int kRearLeftMotorPort = 1;
-constexpr int kFrontRightMotorPort = 2;
-constexpr int kRearRightMotorPort = 3;
+inline constexpr int kFrontLeftMotorPort = 0;
+inline constexpr int kRearLeftMotorPort = 1;
+inline constexpr int kFrontRightMotorPort = 2;
+inline constexpr int kRearRightMotorPort = 3;
 
-constexpr int kFrontLeftEncoderPorts[]{0, 1};
-constexpr int kRearLeftEncoderPorts[]{2, 3};
-constexpr int kFrontRightEncoderPorts[]{4, 5};
-constexpr int kRearRightEncoderPorts[]{6, 7};
+inline constexpr int kFrontLeftEncoderPorts[]{0, 1};
+inline constexpr int kRearLeftEncoderPorts[]{2, 3};
+inline constexpr int kFrontRightEncoderPorts[]{4, 5};
+inline constexpr int kRearRightEncoderPorts[]{6, 7};
 
-constexpr bool kFrontLeftEncoderReversed = false;
-constexpr bool kRearLeftEncoderReversed = true;
-constexpr bool kFrontRightEncoderReversed = false;
-constexpr bool kRearRightEncoderReversed = true;
+inline constexpr bool kFrontLeftEncoderReversed = false;
+inline constexpr bool kRearLeftEncoderReversed = true;
+inline constexpr bool kFrontRightEncoderReversed = false;
+inline constexpr bool kRearRightEncoderReversed = true;
 
-constexpr auto kTrackWidth =
+inline constexpr auto kTrackWidth =
     0.5_m;  // Distance between centers of right and left wheels on robot
-constexpr auto kWheelBase =
+inline constexpr auto kWheelBase =
     0.7_m;  // Distance between centers of front and back wheels on robot
 extern const frc::MecanumDriveKinematics kDriveKinematics;
 
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterMeters = 0.15;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterMeters = 0.15;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterMeters * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
@@ -60,26 +60,26 @@
 // These characterization values MUST be determined either experimentally or
 // theoretically for *your* robot's drive. The SysId tool provides a convenient
 // method for obtaining these values for your robot.
-constexpr auto ks = 1_V;
-constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
-constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+inline constexpr auto ks = 1_V;
+inline constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+inline constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
 
 // Example value only - as above, this must be tuned for your drive!
-constexpr double kPFrontLeftVel = 0.5;
-constexpr double kPRearLeftVel = 0.5;
-constexpr double kPFrontRightVel = 0.5;
-constexpr double kPRearRightVel = 0.5;
+inline constexpr double kPFrontLeftVel = 0.5;
+inline constexpr double kPRearLeftVel = 0.5;
+inline constexpr double kPFrontRightVel = 0.5;
+inline constexpr double kPRearRightVel = 0.5;
 }  // namespace DriveConstants
 
 namespace AutoConstants {
-constexpr auto kMaxSpeed = 3_mps;
-constexpr auto kMaxAcceleration = 3_mps_sq;
-constexpr auto kMaxAngularSpeed = 3_rad_per_s;
-constexpr auto kMaxAngularAcceleration = 3_rad_per_s_sq;
+inline constexpr auto kMaxSpeed = 3_mps;
+inline constexpr auto kMaxAcceleration = 3_mps_sq;
+inline constexpr auto kMaxAngularSpeed = 3_rad_per_s;
+inline constexpr auto kMaxAngularAcceleration = 3_rad_per_s_sq;
 
-constexpr double kPXController = 0.5;
-constexpr double kPYController = 0.5;
-constexpr double kPThetaController = 0.5;
+inline constexpr double kPXController = 0.5;
+inline constexpr double kPYController = 0.5;
+inline constexpr double kPThetaController = 0.5;
 
 extern const frc::TrapezoidProfile<units::radians>::Constraints
     kThetaControllerConstraints;
@@ -87,5 +87,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
index a82f2ac..de9c814 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
@@ -24,7 +26,7 @@
  private:
   // Have it null by default so that if testing teleop it
   // doesn't have undefined behavior and potentially crash.
-  frc2::Command* m_autonomousCommand = nullptr;
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
index 93485e3..c6f8afc 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
@@ -9,6 +9,7 @@
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/PIDCommand.h>
 #include <frc2/command/ParallelRaceGroup.h>
@@ -28,7 +29,7 @@
  public:
   RobotContainer();
 
-  frc2::Command* GetAutonomousCommand();
+  frc2::CommandPtr GetAutonomousCommand();
 
  private:
   // The driver's controller
@@ -44,8 +45,5 @@
   frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
                                         {}};
 
-  // The chooser for the autonomous routines
-  frc::SendableChooser<frc2::Command*> m_chooser;
-
   void ConfigureButtonBindings();
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
index 579a395..7efb225 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
@@ -148,7 +148,10 @@
   frc::PWMSparkMax m_rearRight;
 
   // The robot's drive
-  frc::MecanumDrive m_drive{m_frontLeft, m_rearLeft, m_frontRight, m_rearRight};
+  frc::MecanumDrive m_drive{[&](double output) { m_frontLeft.Set(output); },
+                            [&](double output) { m_rearLeft.Set(output); },
+                            [&](double output) { m_frontRight.Set(output); },
+                            [&](double output) { m_rearRight.Set(output); }};
 
   // The front-left-side drive encoder
   frc::Encoder m_frontLeftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
index 8d9d7ae..eeb9ce1 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
@@ -14,6 +14,11 @@
 class Robot : public frc::TimedRobot {
  public:
   void RobotInit() override {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearLeft);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearRight);
+
     // Invert the right side motors. You may need to change or remove this to
     // match your robot.
     m_frontRight.SetInverted(true);
@@ -40,8 +45,11 @@
   frc::PWMSparkMax m_rearLeft{kRearLeftChannel};
   frc::PWMSparkMax m_frontRight{kFrontRightChannel};
   frc::PWMSparkMax m_rearRight{kRearRightChannel};
-  frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
-                                 m_rearRight};
+  frc::MecanumDrive m_robotDrive{
+      [&](double output) { m_frontLeft.Set(output); },
+      [&](double output) { m_rearLeft.Set(output); },
+      [&](double output) { m_frontRight.Set(output); },
+      [&](double output) { m_rearRight.Set(output); }};
 
   frc::Joystick m_stick{kJoystickChannel};
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
index 9f1b12b..3e5326c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -50,7 +50,8 @@
                        units::second_t period) {
   auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize(
       fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
+                          xSpeed, ySpeed, rot,
+                          m_poseEstimator.GetEstimatedPosition().Rotation())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot},
       period));
   wheelSpeeds.Desaturate(kMaxSpeed);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
index e880af2..5fa8c62 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -81,10 +81,14 @@
       [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
       {&m_drive})};
 
-  // Reset odometry to the starting pose of the trajectory.
-  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
-
-  return std::move(ramseteCommand)
-      .BeforeStarting(
+  // Reset odometry to the initial pose of the trajectory, run path following
+  // command, then stop at the end.
+  return frc2::cmd::RunOnce(
+             [this, &exampleTrajectory] {
+               m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+             },
+             {})
+      .AndThen(std::move(ramseteCommand))
+      .AndThen(
           frc2::cmd::RunOnce([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
index 0bc598e..13e8306 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -17,10 +17,16 @@
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
       m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_right1.SetInverted(true);
 
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
@@ -41,8 +47,8 @@
 }
 
 void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
-  m_leftMotors.SetVoltage(left);
-  m_rightMotors.SetVoltage(right);
+  m_left1.SetVoltage(left);
+  m_right1.SetVoltage(right);
   m_drive.Feed();
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
index f7a061a..d2ca25b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -25,22 +25,22 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr auto kTrackwidth = 0.69_m;
+inline constexpr auto kTrackwidth = 0.69_m;
 extern const frc::DifferentialDriveKinematics kDriveKinematics;
 
-constexpr int kEncoderCPR = 1024;
-constexpr units::meter_t kWheelDiameter = 6_in;
-constexpr auto kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr units::meter_t kWheelDiameter = 6_in;
+inline constexpr auto kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameter * std::numbers::pi) / static_cast<double>(kEncoderCPR);
 
@@ -49,24 +49,24 @@
 // theoretically for *your* robot's drive. The Robot Characterization
 // Toolsuite provides a convenient tool for obtaining these values for your
 // robot.
-constexpr auto ks = 0.22_V;
-constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
-constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
+inline constexpr auto ks = 0.22_V;
+inline constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
+inline constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
 
 // Example value only - as above, this must be tuned for your drive!
-constexpr double kPDriveVel = 8.5;
+inline constexpr double kPDriveVel = 8.5;
 }  // namespace DriveConstants
 
 namespace AutoConstants {
-constexpr auto kMaxSpeed = 3_mps;
-constexpr auto kMaxAcceleration = 1_mps_sq;
+inline constexpr auto kMaxSpeed = 3_mps;
+inline constexpr auto kMaxAcceleration = 1_mps_sq;
 
 // Reasonable baseline values for a RAMSETE follower in units of meters and
 // seconds
-constexpr auto kRamseteB = 2.0 * 1_rad * 1_rad / (1_m * 1_m);
-constexpr auto kRamseteZeta = 0.7 / 1_rad;
+inline constexpr auto kRamseteB = 2.0 * 1_rad * 1_rad / (1_m * 1_m);
+inline constexpr auto kRamseteZeta = 0.7 / 1_rad;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
index 8ea14da..e1580e1 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
@@ -9,7 +9,6 @@
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/voltage.h>
@@ -122,14 +121,9 @@
   frc::PWMSparkMax m_right1;
   frc::PWMSparkMax m_right2;
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
index 8c1e632..9ce08e9 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
@@ -12,8 +12,8 @@
   const double rightOutput = m_rightPIDController.Calculate(
       m_rightEncoder.GetRate(), speeds.right.value());
 
-  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
-  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+  m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
 }
 
 void Drivetrain::Drive(units::meters_per_second_t xSpeed,
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
index 341cd38..39511c7 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
@@ -12,7 +12,6 @@
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <units/angular_velocity.h>
 #include <units/length.h>
@@ -25,10 +24,13 @@
   Drivetrain() {
     m_gyro.Reset();
 
+    m_leftLeader.AddFollower(m_leftFollower);
+    m_rightLeader.AddFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.SetInverted(true);
+    m_rightLeader.SetInverted(true);
 
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
@@ -64,9 +66,6 @@
   frc::PWMSparkMax m_rightLeader{3};
   frc::PWMSparkMax m_rightFollower{4};
 
-  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
-  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
-
   frc::Encoder m_leftEncoder{0, 1};
   frc::Encoder m_rightEncoder{2, 3};
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp
index 98ff0ca..e03ce43 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp
@@ -9,10 +9,16 @@
 #include <frc2/command/Commands.h>
 
 Drive::Drive() {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
+
+  m_leftLeader.AddFollower(m_leftFollower);
+  m_rightLeader.AddFollower(m_rightFollower);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_rightLeader.SetInverted(true);
 
   // Sets the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h
index 330eeba..a552c44 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h
@@ -13,63 +13,63 @@
 #include <units/voltage.h>
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr double kEncoderCPR = 1024;
-constexpr units::meter_t kWheelDiameter = 6.0_in;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr double kEncoderCPR = 1024;
+inline constexpr units::meter_t kWheelDiameter = 6.0_in;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     ((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value();
 
 }  // namespace DriveConstants
 
 namespace IntakeConstants {
-constexpr int kMotorPort = 6;
-constexpr int kSolenoidPorts[]{0, 1};
+inline constexpr int kMotorPort = 6;
+inline constexpr int kSolenoidPorts[]{0, 1};
 }  // namespace IntakeConstants
 
 namespace StorageConstants {
-constexpr int kMotorPort = 7;
-constexpr int kBallSensorPort = 6;
+inline constexpr int kMotorPort = 7;
+inline constexpr int kBallSensorPort = 6;
 }  // namespace StorageConstants
 
 namespace ShooterConstants {
-constexpr int kEncoderPorts[]{4, 5};
-constexpr bool kEncoderReversed = false;
-constexpr double kEncoderCPR = 1024;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderPorts[]{4, 5};
+inline constexpr bool kEncoderReversed = false;
+inline constexpr double kEncoderCPR = 1024;
+inline constexpr double kEncoderDistancePerPulse =
     // Distance units will be rotations
     1.0 / kEncoderCPR;
 
-constexpr int kShooterMotorPort = 4;
-constexpr int kFeederMotorPort = 5;
+inline constexpr int kShooterMotorPort = 4;
+inline constexpr int kFeederMotorPort = 5;
 
-constexpr auto kShooterFree = 5300_tps;
-constexpr auto kShooterTarget = 4000_tps;
-constexpr auto kShooterTolerance = 50_tps;
+inline constexpr auto kShooterFree = 5300_tps;
+inline constexpr auto kShooterTarget = 4000_tps;
+inline constexpr auto kShooterTolerance = 50_tps;
 
 // These are not real PID gains, and will have to be tuned for your specific
 // robot.
-constexpr double kP = 1;
+inline constexpr double kP = 1;
 
-constexpr units::volt_t kS = 0.05_V;
-constexpr auto kV =
+inline constexpr units::volt_t kS = 0.05_V;
+inline constexpr auto kV =
     // Should have value 12V at free speed...
     12.0_V / kShooterFree;
 
-constexpr double kFeederSpeed = 0.5;
+inline constexpr double kFeederSpeed = 0.5;
 }  // namespace ShooterConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
 
 namespace AutoConstants {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h
index ac96c52..8432e57 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h
@@ -8,7 +8,6 @@
 
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/CommandPtr.h>
 #include <frc2/command/SubsystemBase.h>
@@ -45,10 +44,9 @@
   frc::PWMSparkMax m_rightLeader{DriveConstants::kRightMotor1Port};
   frc::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port};
 
-  frc::MotorControllerGroup m_leftMotors{m_leftLeader, m_leftFollower};
-  frc::MotorControllerGroup m_rightMotors{m_rightLeader, m_rightFollower};
-
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{
+      [&](double output) { m_leftLeader.Set(output); },
+      [&](double output) { m_rightLeader.Set(output); }};
 
   frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
                              DriveConstants::kLeftEncoderPorts[1],
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp
index 979f8a0..1d212d5 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp
@@ -15,6 +15,9 @@
 // The Romi has onboard encoders that are hardcoded
 // to use DIO pins 4/5 and 6/7 for the left and right
 Drivetrain::Drivetrain() {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_leftMotor);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_rightMotor);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h
index e5cee33..dddb741 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h
@@ -14,6 +14,6 @@
  */
 
 namespace DriveConstants {
-constexpr double kCountsPerRevolution = 1440.0;
-constexpr double kWheelDiameterInch = 2.75;
+inline constexpr double kCountsPerRevolution = 1440.0;
+inline constexpr double kWheelDiameterInch = 2.75;
 }  // namespace DriveConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
index ace7d33..d679178 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
@@ -114,7 +114,9 @@
   frc::Encoder m_leftEncoder{4, 5};
   frc::Encoder m_rightEncoder{6, 7};
 
-  frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
+  frc::DifferentialDrive m_drive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
 
   frc::RomiGyro m_gyro;
   frc::BuiltInAccelerometer m_accelerometer;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
index 69895cb..df5ba8c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
@@ -4,9 +4,17 @@
 
 #include "RobotContainer.h"
 
+#include <frc/smartdashboard/SmartDashboard.h>
+
 RobotContainer::RobotContainer() {
   // Initialize all of your commands and subsystems here
 
+  m_chooser.SetDefaultOption("ONE", CommandSelector::ONE);
+  m_chooser.AddOption("TWO", CommandSelector::TWO);
+  m_chooser.AddOption("THREE", CommandSelector::THREE);
+
+  frc::SmartDashboard::PutData("Auto Chooser", &m_chooser);
+
   // Configure the button bindings
   ConfigureButtonBindings();
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
index ae5d02d..e07663a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
@@ -15,5 +15,5 @@
  */
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
index a1a2c86..ce5e26d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
@@ -4,7 +4,9 @@
 
 #pragma once
 
+#include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/Commands.h>
 
 /**
@@ -24,10 +26,8 @@
   // The enum used as keys for selecting the command to run.
   enum CommandSelector { ONE, TWO, THREE };
 
-  // An example selector method for the selectcommand.  Returns the selector
-  // that will select which command to run.  Can base this choice on logical
-  // conditions evaluated at runtime.
-  CommandSelector Select() { return ONE; }
+  // An example of how command selector may be used with SendableChooser
+  frc::SendableChooser<CommandSelector> m_chooser;
 
   // The robot's subsystems and commands are defined here...
 
@@ -36,7 +36,7 @@
   // takes a generic type, so the selector does not have to be an enum; it could
   // be any desired type (string, integer, boolean, double...)
   frc2::CommandPtr m_exampleSelectCommand = frc2::cmd::Select<CommandSelector>(
-      [this] { return Select(); },
+      [this] { return m_chooser.GetSelected(); },
       // Maps selector values to commands
       std::pair{ONE, frc2::cmd::Print("Command one was selected!")},
       std::pair{TWO, frc2::cmd::Print("Command two was selected!")},
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
index b951f18..b532bcb 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
@@ -30,6 +30,9 @@
 class Robot : public frc::TimedRobot {
  public:
   void RobotInit() override {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_left);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_right);
+
     // Add a widget titled 'Max Speed' with a number slider.
     m_maxSpeed = frc::Shuffleboard::GetTab("Configuration")
                      .Add("Max Speed", 1)
@@ -65,7 +68,9 @@
   frc::PWMSparkMax m_right{1};
   frc::PWMSparkMax m_elevatorMotor{2};
 
-  frc::DifferentialDrive m_robotDrive{m_left, m_right};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_left.Set(output); },
+      [&](double output) { m_right.Set(output); }};
 
   frc::Joystick m_stick{0};
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
index 93d0889..e0b63ca 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
@@ -14,8 +14,8 @@
   double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
                                                       speeds.right.value());
 
-  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
-  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+  m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
 }
 
 void Drivetrain::Drive(units::meters_per_second_t xSpeed,
@@ -43,9 +43,9 @@
   // simulation, and write the simulated positions and velocities to our
   // simulated encoder and gyro. We negate the right side so that positive
   // voltages make the right side move forward.
-  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} *
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} *
                                       frc::RobotController::GetInputVoltage(),
-                                  units::volt_t{m_rightGroup.Get()} *
+                                  units::volt_t{m_rightLeader.Get()} *
                                       frc::RobotController::GetInputVoltage());
   m_drivetrainSimulator.Update(20_ms);
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
index 4c274fe..ca82547 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
@@ -12,7 +12,6 @@
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/simulation/AnalogGyroSim.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
@@ -33,10 +32,13 @@
   Drivetrain() {
     m_gyro.Reset();
 
+    m_leftLeader.AddFollower(m_leftFollower);
+    m_rightLeader.AddFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.SetInverted(true);
+    m_rightLeader.SetInverted(true);
 
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
@@ -49,7 +51,7 @@
     m_leftEncoder.Reset();
     m_rightEncoder.Reset();
 
-    m_rightGroup.SetInverted(true);
+    m_rightLeader.SetInverted(true);
 
     frc::SmartDashboard::PutData("Field", &m_fieldSim);
   }
@@ -80,9 +82,6 @@
   frc::PWMSparkMax m_rightLeader{3};
   frc::PWMSparkMax m_rightFollower{4};
 
-  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
-  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
-
   frc::Encoder m_leftEncoder{0, 1};
   frc::Encoder m_rightEncoder{2, 3};
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
index f6a9eea..38e9262 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
@@ -117,7 +117,7 @@
       goal = {kLoweredPosition, 0_rad_per_s};
     }
     m_lastProfiledReference =
-        m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
+        m_profile.Calculate(20_ms, m_lastProfiledReference, goal);
 
     m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
                                     m_lastProfiledReference.velocity.value()});
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
index 15d8596..ecef64a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
@@ -14,10 +14,16 @@
 using namespace DriveConstants;
 
 DriveSubsystem::DriveSubsystem() {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
+
+  m_left1.AddFollower(m_left2);
+  m_right1.AddFollower(m_right2);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
-  m_rightMotors.SetInverted(true);
+  m_right1.SetInverted(true);
 
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -40,10 +46,9 @@
   // simulation, and write the simulated positions and velocities to our
   // simulated encoder and gyro. We negate the right side so that positive
   // voltages make the right side move forward.
-  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftMotors.Get()} *
-                                      frc::RobotController::GetInputVoltage(),
-                                  units::volt_t{m_rightMotors.Get()} *
-                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.SetInputs(
+      units::volt_t{m_left1.Get()} * frc::RobotController::GetInputVoltage(),
+      units::volt_t{m_right1.Get()} * frc::RobotController::GetInputVoltage());
   m_drivetrainSimulator.Update(20_ms);
 
   m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
@@ -63,8 +68,8 @@
 }
 
 void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
-  m_leftMotors.SetVoltage(left);
-  m_rightMotors.SetVoltage(right);
+  m_left1.SetVoltage(left);
+  m_right1.SetVoltage(right);
   m_drive.Feed();
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
index d8080ca..103204a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
@@ -27,22 +27,22 @@
  */
 
 namespace DriveConstants {
-constexpr int kLeftMotor1Port = 0;
-constexpr int kLeftMotor2Port = 1;
-constexpr int kRightMotor1Port = 2;
-constexpr int kRightMotor2Port = 3;
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
 
-constexpr int kLeftEncoderPorts[]{0, 1};
-constexpr int kRightEncoderPorts[]{2, 3};
-constexpr bool kLeftEncoderReversed = false;
-constexpr bool kRightEncoderReversed = true;
+inline constexpr int kLeftEncoderPorts[]{0, 1};
+inline constexpr int kRightEncoderPorts[]{2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
 
-constexpr auto kTrackwidth = 0.69_m;
+inline constexpr auto kTrackwidth = 0.69_m;
 extern const frc::DifferentialDriveKinematics kDriveKinematics;
 
-constexpr int kEncoderCPR = 1024;
-constexpr auto kWheelDiameter = 6_in;
-constexpr double kEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr auto kWheelDiameter = 6_in;
+inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameter.value() * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
@@ -52,33 +52,33 @@
 // theoretically for *your* robot's drive. The Robot Characterization
 // Toolsuite provides a convenient tool for obtaining these values for your
 // robot.
-constexpr auto ks = 0.22_V;
-constexpr auto kv = 1.98 * 1_V / 1_mps;
-constexpr auto ka = 0.2 * 1_V / 1_mps_sq;
+inline constexpr auto ks = 0.22_V;
+inline constexpr auto kv = 1.98 * 1_V / 1_mps;
+inline constexpr auto ka = 0.2 * 1_V / 1_mps_sq;
 
-constexpr auto kvAngular = 1.5 * 1_V / 1_mps;
-constexpr auto kaAngular = 0.3 * 1_V / 1_mps_sq;
+inline constexpr auto kvAngular = 1.5 * 1_V / 1_mps;
+inline constexpr auto kaAngular = 0.3 * 1_V / 1_mps_sq;
 
 extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant;
 
 // Example values only -- use what's on your physical robot!
-constexpr auto kDrivetrainGearbox = frc::DCMotor::CIM(2);
-constexpr auto kDrivetrainGearing = 8.0;
+inline constexpr auto kDrivetrainGearbox = frc::DCMotor::CIM(2);
+inline constexpr auto kDrivetrainGearing = 8.0;
 
 // Example value only - as above, this must be tuned for your drive!
-constexpr double kPDriveVel = 8.5;
+inline constexpr double kPDriveVel = 8.5;
 }  // namespace DriveConstants
 
 namespace AutoConstants {
-constexpr auto kMaxSpeed = 3_mps;
-constexpr auto kMaxAcceleration = 3_mps_sq;
+inline constexpr auto kMaxSpeed = 3_mps;
+inline constexpr auto kMaxAcceleration = 3_mps_sq;
 
 // Reasonable baseline values for a RAMSETE follower in units of meters and
 // seconds
-constexpr auto kRamseteB = 2 * 1_rad * 1_rad / (1_m * 1_m);
-constexpr auto kRamseteZeta = 0.7 / 1_rad;
+inline constexpr auto kRamseteB = 2 * 1_rad * 1_rad / (1_m * 1_m);
+inline constexpr auto kRamseteZeta = 0.7 / 1_rad;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
index 57392c7..5e41201 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
@@ -9,7 +9,6 @@
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/simulation/ADXRS450_GyroSim.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
@@ -133,14 +132,9 @@
   frc::PWMSparkMax m_right1{DriveConstants::kRightMotor1Port};
   frc::PWMSparkMax m_right2{DriveConstants::kRightMotor2Port};
 
-  // The motors on the left side of the drive
-  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
-
-  // The motors on the right side of the drive
-  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
-
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
+                                 [&](double output) { m_right1.Set(output); }};
 
   // The left-side drive encoder
   frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
index 3b83793..ceadd83 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
@@ -117,7 +117,7 @@
       goal = {kLoweredPosition, 0_fps};
     }
     m_lastProfiledReference =
-        m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
+        m_profile.Calculate(20_ms, m_lastProfiledReference, goal);
 
     m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
                                     m_lastProfiledReference.velocity.value()});
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
index c7eab48..c4c1661 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
@@ -37,7 +37,7 @@
 void Robot::AutonomousInit() {
   m_autonomousCommand = m_container.GetAutonomousCommand();
 
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Schedule();
   }
 }
@@ -49,9 +49,9 @@
   // teleop starts running. If you want the autonomous to
   // continue until interrupted by another command, remove
   // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
+  if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
-    m_autonomousCommand = nullptr;
+    m_autonomousCommand.reset();
   }
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
index 4bbbe0c..2ca4b71 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
@@ -11,6 +11,7 @@
 #include <frc/shuffleboard/Shuffleboard.h>
 #include <frc/trajectory/Trajectory.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc2/command/Commands.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/SequentialCommandGroup.h>
 #include <frc2/command/SwerveControllerCommand.h>
@@ -48,7 +49,7 @@
 
 void RobotContainer::ConfigureButtonBindings() {}
 
-frc2::Command* RobotContainer::GetAutonomousCommand() {
+frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // Set up config for trajectory
   frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
                                AutoConstants::kMaxAcceleration);
@@ -73,24 +74,32 @@
   thetaController.EnableContinuousInput(units::radian_t{-std::numbers::pi},
                                         units::radian_t{std::numbers::pi});
 
-  frc2::SwerveControllerCommand<4> swerveControllerCommand(
-      exampleTrajectory, [this]() { return m_drive.GetPose(); },
+  frc2::CommandPtr swerveControllerCommand =
+      frc2::SwerveControllerCommand<4>(
+          exampleTrajectory, [this]() { return m_drive.GetPose(); },
 
-      m_drive.kDriveKinematics,
+          m_drive.kDriveKinematics,
 
-      frc::PIDController{AutoConstants::kPXController, 0, 0},
-      frc::PIDController{AutoConstants::kPYController, 0, 0}, thetaController,
+          frc::PIDController{AutoConstants::kPXController, 0, 0},
+          frc::PIDController{AutoConstants::kPYController, 0, 0},
+          thetaController,
 
-      [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
+          [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
 
-      {&m_drive});
+          {&m_drive})
+          .ToPtr();
 
-  // Reset odometry to the starting pose of the trajectory.
-  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
-
-  // no auto
-  return new frc2::SequentialCommandGroup(
+  // Reset odometry to the initial pose of the trajectory, run path following
+  // command, then stop at the end.
+  return frc2::cmd::Sequence(
+      frc2::InstantCommand(
+          [this, &exampleTrajectory]() {
+            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          },
+          {})
+          .ToPtr(),
       std::move(swerveControllerCommand),
       frc2::InstantCommand(
-          [this]() { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {}));
+          [this] { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {})
+          .ToPtr());
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
index dc1cdb0..d565e84 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
@@ -11,8 +11,8 @@
 #include "Constants.h"
 
 SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel,
-                           const int driveEncoderPorts[],
-                           const int turningEncoderPorts[],
+                           const int driveEncoderPorts[2],
+                           const int turningEncoderPorts[2],
                            bool driveEncoderReversed,
                            bool turningEncoderReversed)
     : m_driveMotor(driveMotorChannel),
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
index 779927d..5e95629 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
@@ -29,80 +29,80 @@
  */
 
 namespace DriveConstants {
-constexpr int kFrontLeftDriveMotorPort = 0;
-constexpr int kRearLeftDriveMotorPort = 2;
-constexpr int kFrontRightDriveMotorPort = 4;
-constexpr int kRearRightDriveMotorPort = 6;
+inline constexpr int kFrontLeftDriveMotorPort = 0;
+inline constexpr int kRearLeftDriveMotorPort = 2;
+inline constexpr int kFrontRightDriveMotorPort = 4;
+inline constexpr int kRearRightDriveMotorPort = 6;
 
-constexpr int kFrontLeftTurningMotorPort = 1;
-constexpr int kRearLeftTurningMotorPort = 3;
-constexpr int kFrontRightTurningMotorPort = 5;
-constexpr int kRearRightTurningMotorPort = 7;
+inline constexpr int kFrontLeftTurningMotorPort = 1;
+inline constexpr int kRearLeftTurningMotorPort = 3;
+inline constexpr int kFrontRightTurningMotorPort = 5;
+inline constexpr int kRearRightTurningMotorPort = 7;
 
-constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
-constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
-constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
-constexpr int kRearRightTurningEncoderPorts[2]{6, 7};
+inline constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
+inline constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
+inline constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
+inline constexpr int kRearRightTurningEncoderPorts[2]{6, 7};
 
-constexpr bool kFrontLeftTurningEncoderReversed = false;
-constexpr bool kRearLeftTurningEncoderReversed = true;
-constexpr bool kFrontRightTurningEncoderReversed = false;
-constexpr bool kRearRightTurningEncoderReversed = true;
+inline constexpr bool kFrontLeftTurningEncoderReversed = false;
+inline constexpr bool kRearLeftTurningEncoderReversed = true;
+inline constexpr bool kFrontRightTurningEncoderReversed = false;
+inline constexpr bool kRearRightTurningEncoderReversed = true;
 
-constexpr int kFrontLeftDriveEncoderPorts[2]{8, 9};
-constexpr int kRearLeftDriveEncoderPorts[2]{10, 11};
-constexpr int kFrontRightDriveEncoderPorts[2]{12, 13};
-constexpr int kRearRightDriveEncoderPorts[2]{14, 15};
+inline constexpr int kFrontLeftDriveEncoderPorts[2]{8, 9};
+inline constexpr int kRearLeftDriveEncoderPorts[2]{10, 11};
+inline constexpr int kFrontRightDriveEncoderPorts[2]{12, 13};
+inline constexpr int kRearRightDriveEncoderPorts[2]{14, 15};
 
-constexpr bool kFrontLeftDriveEncoderReversed = false;
-constexpr bool kRearLeftDriveEncoderReversed = true;
-constexpr bool kFrontRightDriveEncoderReversed = false;
-constexpr bool kRearRightDriveEncoderReversed = true;
+inline constexpr bool kFrontLeftDriveEncoderReversed = false;
+inline constexpr bool kRearLeftDriveEncoderReversed = true;
+inline constexpr bool kFrontRightDriveEncoderReversed = false;
+inline constexpr bool kRearRightDriveEncoderReversed = true;
 
 // If you call DriveSubsystem::Drive with a different period make sure to update
 // this.
-constexpr units::second_t kDrivePeriod = frc::TimedRobot::kDefaultPeriod;
+inline constexpr units::second_t kDrivePeriod = frc::TimedRobot::kDefaultPeriod;
 
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
 // theoretically for *your* robot's drive. The SysId tool provides a convenient
 // method for obtaining these values for your robot.
-constexpr auto ks = 1_V;
-constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
-constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+inline constexpr auto ks = 1_V;
+inline constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+inline constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
 
 // Example value only - as above, this must be tuned for your drive!
-constexpr double kPFrontLeftVel = 0.5;
-constexpr double kPRearLeftVel = 0.5;
-constexpr double kPFrontRightVel = 0.5;
-constexpr double kPRearRightVel = 0.5;
+inline constexpr double kPFrontLeftVel = 0.5;
+inline constexpr double kPRearLeftVel = 0.5;
+inline constexpr double kPFrontRightVel = 0.5;
+inline constexpr double kPRearRightVel = 0.5;
 }  // namespace DriveConstants
 
 namespace ModuleConstants {
-constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterMeters = 0.15;
-constexpr double kDriveEncoderDistancePerPulse =
+inline constexpr int kEncoderCPR = 1024;
+inline constexpr double kWheelDiameterMeters = 0.15;
+inline constexpr double kDriveEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterMeters * std::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 
-constexpr double kTurningEncoderDistancePerPulse =
+inline constexpr double kTurningEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (std::numbers::pi * 2) / static_cast<double>(kEncoderCPR);
 
-constexpr double kPModuleTurningController = 1;
-constexpr double kPModuleDriveController = 1;
+inline constexpr double kPModuleTurningController = 1;
+inline constexpr double kPModuleDriveController = 1;
 }  // namespace ModuleConstants
 
 namespace AutoConstants {
-constexpr auto kMaxSpeed = 3_mps;
-constexpr auto kMaxAcceleration = 3_mps_sq;
-constexpr auto kMaxAngularSpeed = 3.142_rad_per_s;
-constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq;
+inline constexpr auto kMaxSpeed = 3_mps;
+inline constexpr auto kMaxAcceleration = 3_mps_sq;
+inline constexpr auto kMaxAngularSpeed = 3.142_rad_per_s;
+inline constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq;
 
-constexpr double kPXController = 0.5;
-constexpr double kPYController = 0.5;
-constexpr double kPThetaController = 0.5;
+inline constexpr double kPXController = 0.5;
+inline constexpr double kPYController = 0.5;
+inline constexpr double kPThetaController = 0.5;
 
 //
 
@@ -112,5 +112,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
index a82f2ac..de9c814 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
@@ -24,7 +26,7 @@
  private:
   // Have it null by default so that if testing teleop it
   // doesn't have undefined behavior and potentially crash.
-  frc2::Command* m_autonomousCommand = nullptr;
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
index 3466121..4c79164 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
@@ -9,6 +9,7 @@
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/PIDCommand.h>
 #include <frc2/command/ParallelRaceGroup.h>
@@ -28,7 +29,7 @@
  public:
   RobotContainer();
 
-  frc2::Command* GetAutonomousCommand();
+  frc2::CommandPtr GetAutonomousCommand();
 
  private:
   // The driver's controller
@@ -39,8 +40,5 @@
   // The robot's subsystems
   DriveSubsystem m_drive;
 
-  // The chooser for the autonomous routines
-  frc::SendableChooser<frc2::Command*> m_chooser;
-
   void ConfigureButtonBindings();
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
index a365f7f..188e944 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -15,7 +15,8 @@
   auto states =
       m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize(
           fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                              xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
+                              xSpeed, ySpeed, rot,
+                              m_poseEstimator.GetEstimatedPosition().Rotation())
                         : frc::ChassisSpeeds{xSpeed, ySpeed, rot},
           period));
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp
new file mode 100644
index 0000000..5ecb68a
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp
@@ -0,0 +1,55 @@
+// 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 "Robot.h"
+
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+  if (m_autonomousCommand) {
+    m_autonomousCommand->Cancel();
+  }
+}
+
+void Robot::TeleopPeriodic() {}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {
+  frc2::CommandScheduler::GetInstance().CancelAll();
+}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp
new file mode 100644
index 0000000..21f6992
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp
@@ -0,0 +1,30 @@
+// 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 "SysIdRoutineBot.h"
+
+#include <frc2/command/Commands.h>
+
+SysIdRoutineBot::SysIdRoutineBot() {
+  ConfigureBindings();
+}
+
+void SysIdRoutineBot::ConfigureBindings() {
+  m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand(
+      [this] { return -m_driverController.GetLeftY(); },
+      [this] { return -m_driverController.GetRightX(); }));
+
+  m_driverController.A().WhileTrue(
+      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  m_driverController.B().WhileTrue(
+      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  m_driverController.X().WhileTrue(
+      m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
+  m_driverController.Y().WhileTrue(
+      m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+}
+
+frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() {
+  return m_drive.Run([] {});
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp
new file mode 100644
index 0000000..c754976
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp
@@ -0,0 +1,37 @@
+// 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 "subsystems/Drive.h"
+
+#include <frc2/command/Commands.h>
+
+Drive::Drive() {
+  m_leftMotor.AddFollower(frc::PWMSparkMax{constants::drive::kLeftMotor2Port});
+  m_rightMotor.AddFollower(
+      frc::PWMSparkMax{constants::drive::kRightMotor2Port});
+
+  m_rightMotor.SetInverted(true);
+
+  m_leftEncoder.SetDistancePerPulse(
+      constants::drive::kEncoderDistancePerPulse.value());
+  m_rightEncoder.SetDistancePerPulse(
+      constants::drive::kEncoderDistancePerPulse.value());
+
+  m_drive.SetSafetyEnabled(false);
+}
+
+frc2::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
+                                           std::function<double()> rot) {
+  return frc2::cmd::Run([this, fwd, rot] { m_drive.ArcadeDrive(fwd(), rot()); },
+                        {this})
+      .WithName("Arcade Drive");
+}
+
+frc2::CommandPtr Drive::SysIdQuasistatic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Quasistatic(direction);
+}
+
+frc2::CommandPtr Drive::SysIdDynamic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Dynamic(direction);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h
new file mode 100644
index 0000000..e01f533
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h
@@ -0,0 +1,81 @@
+// 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 <array>
+#include <numbers>
+
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace constants {
+namespace drive {
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
+
+inline constexpr std::array<int, 2> kLeftEncoderPorts = {0, 1};
+inline constexpr std::array<int, 2> kRightEncoderPorts = {2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
+
+inline constexpr int kEncoderCpr = 1024;
+inline constexpr units::meter_t kWheelDiameter = 6_in;
+inline constexpr units::meter_t kEncoderDistancePerPulse =
+    (kWheelDiameter * std::numbers::pi) / static_cast<double>(kEncoderCpr);
+}  // namespace drive
+
+namespace shooter {
+
+using kv_unit =
+    units::compound_unit<units::compound_unit<units::volts, units::seconds>,
+                         units::inverse<units::turns>>;
+using kv_unit_t = units::unit_t<kv_unit>;
+
+inline constexpr std::array<int, 2> kEncoderPorts = {4, 5};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr int kEncoderCpr = 1024;
+inline constexpr units::turn_t kEncoderDistancePerPulse =
+    units::turn_t{1.0} / static_cast<double>(kEncoderCpr);
+
+inline constexpr int kShooterMotorPort = 4;
+inline constexpr int kFeederMotorPort = 5;
+
+inline constexpr units::turns_per_second_t kShooterFreeSpeed = 5300_tps;
+inline constexpr units::turns_per_second_t kShooterTargetSpeed = 4000_tps;
+inline constexpr units::turns_per_second_t kShooterTolerance = 50_tps;
+
+inline constexpr double kP = 1.0;
+
+inline constexpr units::volt_t kS = 0.05_V;
+inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+
+inline constexpr double kFeederSpeed = 0.5;
+}  // namespace shooter
+
+namespace intake {
+inline constexpr int kMotorPort = 6;
+inline constexpr std::array<int, 2> kSolenoidPorts = {2, 3};
+}  // namespace intake
+
+namespace storage {
+inline constexpr int kMotorPort = 7;
+inline constexpr int kBallSensorPort = 6;
+}  // namespace storage
+
+namespace autonomous {
+inline constexpr units::second_t kTimeout = 3_s;
+inline constexpr units::meter_t kDriveDistance = 2_m;
+inline constexpr double kDriveSpeed = 0.5;
+}  // namespace autonomous
+
+namespace oi {
+inline constexpr int kDriverControllerPort = 0;
+}  // namespace oi
+}  // namespace constants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h
new file mode 100644
index 0000000..b81adf1
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h
@@ -0,0 +1,35 @@
+// 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 <optional>
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/CommandPtr.h>
+
+#include "SysIdRoutineBot.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+
+ private:
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
+
+  SysIdRoutineBot m_container;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h
new file mode 100644
index 0000000..6630abd
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h
@@ -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.
+
+#pragma once
+
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/button/CommandXboxController.h>
+
+#include "Constants.h"
+#include "subsystems/Drive.h"
+
+class SysIdRoutineBot {
+ public:
+  SysIdRoutineBot();
+
+  frc2::CommandPtr GetAutonomousCommand();
+
+ private:
+  void ConfigureBindings();
+  frc2::CommandXboxController m_driverController{
+      constants::oi::kDriverControllerPort};
+  Drive m_drive{};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h
new file mode 100644
index 0000000..61d34f1
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h
@@ -0,0 +1,62 @@
+// 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 <functional>
+
+#include <frc/Encoder.h>
+#include <frc/RobotController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/SubsystemBase.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include "Constants.h"
+
+class Drive : public frc2::SubsystemBase {
+ public:
+  Drive();
+
+  frc2::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
+                                      std::function<double()> rot);
+  frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction);
+  frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction);
+
+ private:
+  frc::PWMSparkMax m_leftMotor{constants::drive::kLeftMotor1Port};
+  frc::PWMSparkMax m_rightMotor{constants::drive::kRightMotor1Port};
+  frc::DifferentialDrive m_drive{[this](auto val) { m_leftMotor.Set(val); },
+                                 [this](auto val) { m_rightMotor.Set(val); }};
+
+  frc::Encoder m_leftEncoder{constants::drive::kLeftEncoderPorts[0],
+                             constants::drive::kLeftEncoderPorts[1],
+                             constants::drive::kLeftEncoderReversed};
+
+  frc::Encoder m_rightEncoder{constants::drive::kRightEncoderPorts[0],
+                              constants::drive::kRightEncoderPorts[1],
+                              constants::drive::kRightEncoderReversed};
+
+  frc2::sysid::SysIdRoutine m_sysIdRoutine{
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
+                          std::nullopt},
+      frc2::sysid::Mechanism{
+          [this](units::volt_t driveVoltage) {
+            m_leftMotor.SetVoltage(driveVoltage);
+            m_rightMotor.SetVoltage(driveVoltage);
+          },
+          [this](frc::sysid::SysIdRoutineLog* log) {
+            log->Motor("drive-left")
+                .voltage(m_leftMotor.Get() *
+                         frc::RobotController::GetBatteryVoltage())
+                .position(units::meter_t{m_leftEncoder.GetDistance()})
+                .velocity(units::meters_per_second_t{m_leftEncoder.GetRate()});
+            log->Motor("drive-right")
+                .voltage(m_rightMotor.Get() *
+                         frc::RobotController::GetBatteryVoltage())
+                .position(units::meter_t{m_rightEncoder.GetDistance()})
+                .velocity(units::meters_per_second_t{m_rightEncoder.GetRate()});
+          },
+          this}};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp
index 4e48d47..f7b3e9c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp
@@ -14,12 +14,17 @@
 class Robot : public frc::TimedRobot {
   frc::PWMSparkMax m_leftMotor{0};
   frc::PWMSparkMax m_rightMotor{1};
-  frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
   frc::Joystick m_leftStick{0};
   frc::Joystick m_rightStick{1};
 
  public:
   void RobotInit() override {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
index b8cff32..7c0706e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
@@ -14,11 +14,16 @@
 class Robot : public frc::TimedRobot {
   frc::PWMSparkMax m_leftMotor{0};
   frc::PWMSparkMax m_rightMotor{1};
-  frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
   frc::XboxController m_driverController{0};
 
  public:
   void RobotInit() override {
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
+    wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
index 9d2b5c2..fecaf97 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -4,6 +4,11 @@
 
 #include "Robot.h"
 
+Robot::Robot() {
+  wpi::SendableRegistry::AddChild(&m_robotDrive, &m_left);
+  wpi::SendableRegistry::AddChild(&m_robotDrive, &m_right);
+}
+
 void Robot::AutonomousInit() {
   // Set setpoint of the pid controller
   m_pidController.SetSetpoint(kHoldDistance.value());
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h
index 4ff2700..696f066 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h
@@ -18,6 +18,7 @@
  */
 class Robot : public frc::TimedRobot {
  public:
+  Robot();
   void AutonomousInit() override;
   void AutonomousPeriodic() override;
 
@@ -31,9 +32,7 @@
 
  private:
   // proportional speed constant
-  // negative because applying positive voltage will bring us closer to the
-  // target
-  static constexpr double kP = -0.001;
+  static constexpr double kP = 0.001;
   // integral speed constant
   static constexpr double kI = 0.0;
   // derivative speed constant
@@ -46,6 +45,8 @@
   frc::Ultrasonic m_ultrasonic{kUltrasonicPingPort, kUltrasonicEchoPort};
   frc::PWMSparkMax m_left{kLeftMotorPort};
   frc::PWMSparkMax m_right{kRightMotorPort};
-  frc::DifferentialDrive m_robotDrive{m_left, m_right};
+  frc::DifferentialDrive m_robotDrive{
+      [&](double output) { m_left.Set(output); },
+      [&](double output) { m_right.Set(output); }};
   frc::PIDController m_pidController{kP, kI, kD};
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UnitTest/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UnitTest/include/Constants.h
index 2ad9b07..f736bd3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UnitTest/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UnitTest/include/Constants.h
@@ -14,13 +14,13 @@
  */
 
 namespace IntakeConstants {
-constexpr int kMotorPort = 1;
+inline constexpr int kMotorPort = 1;
 
-constexpr int kPistonFwdChannel = 0;
-constexpr int kPistonRevChannel = 1;
-constexpr double kIntakeSpeed = 0.5;
+inline constexpr int kPistonFwdChannel = 0;
+inline constexpr int kPistonRevChannel = 1;
+inline constexpr double kIntakeSpeed = 0.5;
 }  // namespace IntakeConstants
 
 namespace OperatorConstants {
-constexpr int kJoystickIndex = 0;
+inline constexpr int kJoystickIndex = 0;
 }  // namespace OperatorConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp
index bcec018..25c520c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp
@@ -15,6 +15,9 @@
 // The XRP has onboard encoders that are hardcoded
 // to use DIO pins 4/5 and 6/7 for the left and right
 Drivetrain::Drivetrain() {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_leftMotor);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_rightMotor);
+
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/Constants.h
index e5cee33..dddb741 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/Constants.h
@@ -14,6 +14,6 @@
  */
 
 namespace DriveConstants {
-constexpr double kCountsPerRevolution = 1440.0;
-constexpr double kWheelDiameterInch = 2.75;
+inline constexpr double kCountsPerRevolution = 1440.0;
+inline constexpr double kWheelDiameterInch = 2.75;
 }  // namespace DriveConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h
index e665601..c2a2ef3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h
@@ -118,7 +118,9 @@
   frc::Encoder m_leftEncoder{4, 5};
   frc::Encoder m_rightEncoder{6, 7};
 
-  frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
+  frc::DifferentialDrive m_drive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
 
   frc::XRPGyro m_gyro;
   frc::BuiltInAccelerometer m_accelerometer;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json
index 8032909..486e2ec 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -927,5 +927,17 @@
     "foldername": "FlywheelBangBangController",
     "gradlebase": "cpp",
     "commandversion": 2
+  },
+  {
+    "name": "SysIdRoutine",
+    "description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
+    "tags": [
+      "SysId",
+      "Command-based",
+      "DataLog"
+    ],
+    "foldername": "SysId",
+    "gradlebase": "cpp",
+    "commandversion": 2
   }
 ]
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
index 963b31c..87dd5b9 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
@@ -16,6 +16,6 @@
 
 namespace OperatorConstants {
 
-constexpr int kDriverControllerPort = 0;
+inline constexpr int kDriverControllerPort = 0;
 
 }  // namespace OperatorConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp b/third_party/allwpilib/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp
index a970a3a..5cfcbe8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp
@@ -57,7 +57,7 @@
     m_driveSim.Update(20_ms);
 
     auto startingDistance = units::meter_t{GetParam()};
-    m_distance = startingDistance - m_driveSim.GetLeftPosition();
+    m_distance = m_driveSim.GetLeftPosition() - startingDistance;
 
     m_ultrasonicSim.SetRange(m_distance);
   }
@@ -99,8 +99,7 @@
   }
 
   {
-    // advance 100 timesteps
-    frc::sim::StepTiming(2_s);
+    frc::sim::StepTiming(5_s);
 
     EXPECT_NEAR(Robot::kHoldDistance.value(), m_distance.value(), 10.0);
   }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
index be7efb8..9499504 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
@@ -6,6 +6,7 @@
 
 #include <gtest/gtest.h>
 #include <units/time.h>
+#include <wpi/deprecated.h>
 
 #include "TestBench.h"
 #include "frc/Encoder.h"
@@ -37,6 +38,8 @@
 
 static constexpr auto kMotorTime = 0.5_s;
 
+WPI_IGNORE_DEPRECATED
+
 /**
  * A fixture that includes a PWM motor controller and an encoder connected to
  * the same motor.
@@ -197,3 +200,5 @@
 
 INSTANTIATE_TEST_SUITE_P(Tests, MotorEncoderTest,
                          testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON));
+
+WPI_UNIGNORE_DEPRECATED
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp
index 870f029..49a419c 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp
@@ -4,6 +4,7 @@
 
 #include <gtest/gtest.h>
 #include <units/time.h>
+#include <wpi/deprecated.h>
 
 #include "TestBench.h"
 #include "frc/Encoder.h"
@@ -32,6 +33,9 @@
 
   return os;
 }
+
+WPI_IGNORE_DEPRECATED
+
 class MotorInvertingTest
     : public testing::TestWithParam<MotorInvertingTestType> {
  protected:
@@ -153,3 +157,5 @@
 
 INSTANTIATE_TEST_SUITE_P(Tests, MotorInvertingTest,
                          testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON));
+
+WPI_UNIGNORE_DEPRECATED
diff --git a/third_party/allwpilib/wpilibj/CMakeLists.txt b/third_party/allwpilib/wpilibj/CMakeLists.txt
index 5924f4d..624e87a 100644
--- a/third_party/allwpilib/wpilibj/CMakeLists.txt
+++ b/third_party/allwpilib/wpilibj/CMakeLists.txt
@@ -1,15 +1,20 @@
-project (wpilibj)
+project(wpilibj)
 
 # Java bindings
-if (WITH_JAVA)
-    find_package( OpenCV REQUIRED )
+if(WITH_JAVA)
+    find_package(OpenCV REQUIRED)
     find_package(Java REQUIRED)
     include(UseJava)
     set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
     set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
 
-    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
+    find_file(
+        OPENCV_JAR_FILE
+        NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
+        PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin
+        NO_DEFAULT_PATH
+    )
 
     configure_file(src/generate/WPILibVersion.java.in WPILibVersion.java)
 
@@ -17,23 +22,31 @@
     file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
     file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
 
-    add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiutil_jar OUTPUT_NAME wpilibj)
+    add_jar(
+        wpilibj_jar
+        ${JAVA_SOURCES}
+        ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java
+        INCLUDE_JARS
+            hal_jar
+            ntcore_jar
+            ${EJML_JARS}
+            ${JACKSON_JARS}
+            ${OPENCV_JAR_FILE}
+            cscore_jar
+            cameraserver_jar
+            wpimath_jar
+            wpiunits_jar
+            wpiutil_jar
+        OUTPUT_NAME wpilibj
+    )
 
-    get_property(WPILIBJ_JAR_FILE TARGET wpilibj_jar PROPERTY JAR_FILE)
-    install(FILES ${WPILIBJ_JAR_FILE} DESTINATION "${java_lib_dest}")
+    install_jar(wpilibj_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS wpilibj_jar FILE wpilibj.cmake DESTINATION share/wpilibj)
 
-    set_property(TARGET wpilibj_jar PROPERTY FOLDER "java")
-
-    if (WITH_FLAT_INSTALL)
-        set (wpilibj_config_dir ${wpilib_dest})
-    else()
-        set (wpilibj_config_dir share/wpilibj)
-    endif()
-
-    install(FILES wpilibj-config.cmake DESTINATION ${wpilibj_config_dir})
+    install(FILES wpilibj-config.cmake DESTINATION share/wpilibj)
 endif()
 
-if (WITH_JAVA_SOURCE)
+if(WITH_JAVA_SOURCE)
     find_package(Java REQUIRED)
     include(UseJava)
     file(GLOB WPILIBJ_SOURCES src/main/java/edu/wpi/first/wpilibj/*.java)
@@ -45,21 +58,32 @@
     file(GLOB WPILIBJ_SHUFFLEBOARD_SOURCES src/main/java/edu/wpi/first/wpilibj/shuffleboard*.java)
     file(GLOB WPILIBJ_SIMULATION_SOURCES src/main/java/edu/wpi/first/wpilibj/simulation*.java)
     file(GLOB WPILIBJ_SMARTDASHBOARD_SOURCES src/main/java/edu/wpi/first/wpilibj/*.java)
-    file(GLOB WPILIBJ_UTIL_SOURCES src/main/java/edu/wpi/first/wpilibj/*.java ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java)
-    add_jar(wpilibj_src_jar
-    RESOURCES NAMESPACE "edu/wpi/first/wpilibj" ${WPILIBJ_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/counter" ${WPILIBJ_COUNTER_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/drive" ${WPILIBJ_DRIVE_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/event" ${WPILIBJ_EVENT_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/interfaces" ${WPILIBJ_INTERFACES_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/internal" src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java
-    NAMESPACE "edu/wpi/first/wpilibj/livewindow" src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
-    NAMESPACE "edu/wpi/first/wpilibj/motorcontrol" ${WPILIBJ_MOTORCONTROL_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/shuffleboard" ${WPILIBJ_SHUFFLEBOARD_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/simulation" ${WPILIBJ_SIMULATION_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/smartdashboard" ${WPILIBJ_SMARTDASHBOARD_SOURCES}
-    NAMESPACE "edu/wpi/first/wpilibj/util" ${WPILIBJ_UTIL_SOURCES}
-    OUTPUT_NAME wpilibj-sources)
+    file(
+        GLOB WPILIBJ_UTIL_SOURCES
+        src/main/java/edu/wpi/first/wpilibj/*.java
+        ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java
+    )
+    add_jar(
+        wpilibj_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/wpilibj" ${WPILIBJ_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/counter" ${WPILIBJ_COUNTER_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/drive" ${WPILIBJ_DRIVE_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/event" ${WPILIBJ_EVENT_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/interfaces" ${WPILIBJ_INTERFACES_SOURCES}
+        NAMESPACE
+            "edu/wpi/first/wpilibj/internal"
+            src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java
+        NAMESPACE
+            "edu/wpi/first/wpilibj/livewindow"
+            src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
+        NAMESPACE "edu/wpi/first/wpilibj/motorcontrol" ${WPILIBJ_MOTORCONTROL_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/shuffleboard" ${WPILIBJ_SHUFFLEBOARD_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/simulation" ${WPILIBJ_SIMULATION_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/smartdashboard" ${WPILIBJ_SMARTDASHBOARD_SOURCES}
+        NAMESPACE "edu/wpi/first/wpilibj/util" ${WPILIBJ_UTIL_SOURCES}
+        OUTPUT_NAME wpilibj-sources
+    )
 
     get_property(WPILIBJ_SRC_JAR_FILE TARGET wpilibj_src_jar PROPERTY JAR_FILE)
     install(FILES ${WPILIBJ_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
diff --git a/third_party/allwpilib/wpilibj/src/generate/WPILibVersion.java.in b/third_party/allwpilib/wpilibj/src/generate/WPILibVersion.java.in
index cc5e6b0..f38d1b0 100644
--- a/third_party/allwpilib/wpilibj/src/generate/WPILibVersion.java.in
+++ b/third_party/allwpilib/wpilibj/src/generate/WPILibVersion.java.in
@@ -1,10 +1,13 @@
 package edu.wpi.first.wpilibj.util;
 
-/*
+/**
  * Autogenerated file! Do not manually edit this file. This version is regenerated
  * any time the publish task is run, or when this file is deleted.
  */
-
 public final class WPILibVersion {
+  /** The version number. */
   public static final String Version = "${wpilib_version}";
+
+  /** Utility class. */
+  private WPILibVersion() {}
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
index 2eddf64..0b55105 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
@@ -94,30 +94,47 @@
   private static final int PROD_ID = 0x56; // Product identifier
   private static final int SERIAL_NUM = 0x58; // Lot-specific serial number
 
+  /** ADIS16448 calibration times. */
   public enum CalibrationTime {
+    /** 32 ms calibration time */
     _32ms(0),
+    /** 64 ms calibration time */
     _64ms(1),
+    /** 128 ms calibration time */
     _128ms(2),
+    /** 256 ms calibration time */
     _256ms(3),
+    /** 512 ms calibration time */
     _512ms(4),
+    /** 1 s calibration time */
     _1s(5),
+    /** 2 s calibration time */
     _2s(6),
+    /** 4 s calibration time */
     _4s(7),
+    /** 8 s calibration time */
     _8s(8),
+    /** 16 s calibration time */
     _16s(9),
+    /** 32 s calibration time */
     _32s(10),
+    /** 64 s calibration time */
     _64s(11);
 
-    private int value;
+    private final int value;
 
-    private CalibrationTime(int value) {
+    CalibrationTime(int value) {
       this.value = value;
     }
   }
 
+  /** IMU axes. */
   public enum IMUAxis {
+    /** The IMU's X axis. */
     kX,
+    /** The IMU's Y axis. */
     kY,
+    /** The IMU's Z axis. */
     kZ
   }
 
@@ -130,9 +147,9 @@
   private IMUAxis m_yaw_axis;
 
   /* Offset data storage */
-  private double m_offset_data_gyro_rate_x[];
-  private double m_offset_data_gyro_rate_y[];
-  private double m_offset_data_gyro_rate_z[];
+  private double[] m_offset_data_gyro_rate_x;
+  private double[] m_offset_data_gyro_rate_y;
+  private double[] m_offset_data_gyro_rate_z;
 
   /* Instant raw output variables */
   private double m_gyro_rate_x = 0.0;
@@ -199,7 +216,7 @@
   private SimDouble m_simAccelZ;
 
   /* CRC-16 Look-Up Table */
-  int adiscrc[] =
+  int[] adiscrc =
       new int[] {
         0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF,
         0x1F3F, 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890,
@@ -248,15 +265,19 @@
     }
   }
 
+  /** Creates a new ADIS16448_IMU object. */
   public ADIS16448_IMU() {
     this(IMUAxis.kZ, SPI.Port.kMXP, CalibrationTime._512ms);
   }
 
   /**
+   * Creates a new ADIS16448_IMU object.
+   *
    * @param yaw_axis The axis that measures the yaw
    * @param port The SPI Port the gyro is plugged into
    * @param cal_time Calibration time
    */
+  @SuppressWarnings("this-escape")
   public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
     m_yaw_axis = yaw_axis;
     m_spi_port = port;
@@ -331,6 +352,11 @@
     m_connected = true;
   }
 
+  /**
+   * Checks the connection status of the IMU.
+   *
+   * @return True if the IMU is connected, false otherwise.
+   */
   public boolean isConnected() {
     if (m_simConnected != null) {
       return m_simConnected.get();
@@ -338,7 +364,6 @@
     return m_connected;
   }
 
-  /** */
   private static int toUShort(byte[] buf) {
     return (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
   }
@@ -347,7 +372,7 @@
     return (buf[0] & 0xFF);
   }
 
-  public static int toUShort(int... buf) {
+  private static int toUShort(int... buf) {
     return (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
   }
 
@@ -358,7 +383,7 @@
 
   /** */
   private static int toShort(int... buf) {
-    return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
+    return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF)));
   }
 
   /** */
@@ -480,8 +505,19 @@
     return true;
   }
 
-  public int configDecRate(int m_decRate) {
-    int writeValue = m_decRate;
+  /**
+   * Configures the decimation rate of the IMU.
+   *
+   * @param decimationRate The new decimation value.
+   * @return 0 if success, 1 if no change, 2 if error.
+   */
+  public int configDecRate(int decimationRate) {
+    // Switches the active SPI port to standard SPI mode, writes a new value to
+    // the DECIMATE register in the IMU, and re-enables auto SPI.
+    //
+    // This function enters standard SPI mode, writes a new DECIMATE setting to
+    // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+    int writeValue;
     int readbackValue;
     if (!switchToStandardSPI()) {
       DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
@@ -489,19 +525,19 @@
     }
 
     /* Check max */
-    if (m_decRate > 9) {
+    if (decimationRate > 9) {
       DriverStation.reportError(
           "Attempted to write an invalid decimation value. Capping at 9", false);
-      m_decRate = 9;
+      decimationRate = 9;
     }
-    if (m_decRate < 0) {
+    if (decimationRate < 0) {
       DriverStation.reportError(
           "Attempted to write an invalid decimation value. Capping at 0", false);
-      m_decRate = 0;
+      decimationRate = 0;
     }
 
     /* Shift decimation setting to correct position and select internal sync */
-    writeValue = (m_decRate << 8) | 0x1;
+    writeValue = (decimationRate << 8) | 0x1;
 
     /* Apply to IMU */
     writeRegister(SMPL_PRD, writeValue);
@@ -527,7 +563,7 @@
    * @param new_cal_time New calibration time
    * @return 1 if the new calibration time is the same as the current one else 0
    */
-  public int configCalTime(CalibrationTime new_cal_time) {
+  public final int configCalTime(CalibrationTime new_cal_time) {
     if (m_calibration_time == new_cal_time) {
       return 1;
     } else {
@@ -623,6 +659,12 @@
     m_spi.write(buf, 2);
   }
 
+  /**
+   * Reset the gyro.
+   *
+   * <p>Resets the gyro accumulations to a heading of zero. This can be used if there is significant
+   * drift in the gyro and it needs to be recalibrated after running.
+   */
   public void reset() {
     synchronized (this) {
       m_integ_gyro_angle_x = 0.0;
@@ -956,7 +998,9 @@
   }
 
   /**
-   * @return Yaw axis angle in degrees (CCW positive)
+   * Returns the yaw axis angle in degrees (CCW positive).
+   *
+   * @return Yaw axis angle in degrees (CCW positive).
    */
   public synchronized double getAngle() {
     switch (m_yaw_axis) {
@@ -972,7 +1016,9 @@
   }
 
   /**
-   * @return Yaw axis angular rate in degrees per second (CCW positive)
+   * Returns the yaw axis angular rate in degrees per second (CCW positive).
+   *
+   * @return Yaw axis angular rate in degrees per second (CCW positive).
    */
   public synchronized double getRate() {
     switch (m_yaw_axis) {
@@ -988,14 +1034,18 @@
   }
 
   /**
-   * @return Yaw Axis
+   * Returns which axis, kX, kY, or kZ, is set to the yaw axis.
+   *
+   * @return IMUAxis Yaw Axis
    */
   public IMUAxis getYawAxis() {
     return m_yaw_axis;
   }
 
   /**
-   * @return accumulated gyro angle in the X axis in degrees
+   * Returns the accumulated gyro angle in the X axis in degrees.
+   *
+   * @return The accumulated gyro angle in the X axis in degrees.
    */
   public synchronized double getGyroAngleX() {
     if (m_simGyroAngleX != null) {
@@ -1005,7 +1055,9 @@
   }
 
   /**
-   * @return accumulated gyro angle in the Y axis in degrees
+   * Returns the accumulated gyro angle in the Y axis in degrees.
+   *
+   * @return The accumulated gyro angle in the Y axis in degrees.
    */
   public synchronized double getGyroAngleY() {
     if (m_simGyroAngleY != null) {
@@ -1015,7 +1067,9 @@
   }
 
   /**
-   * @return accumulated gyro angle in the Z axis in degrees
+   * Returns the accumulated gyro angle in the Z axis in degrees.
+   *
+   * @return The accumulated gyro angle in the Z axis in degrees.
    */
   public synchronized double getGyroAngleZ() {
     if (m_simGyroAngleZ != null) {
@@ -1025,7 +1079,9 @@
   }
 
   /**
-   * @return gyro angular rate in the X axis in degrees per second
+   * Returns the gyro angular rate in the X axis in degrees per second.
+   *
+   * @return The gyro angular rate in the X axis in degrees per second.
    */
   public synchronized double getGyroRateX() {
     if (m_simGyroRateX != null) {
@@ -1035,7 +1091,9 @@
   }
 
   /**
-   * @return gyro angular rate in the Y axis in degrees per second
+   * Returns the gyro angular rate in the Y axis in degrees per second.
+   *
+   * @return The gyro angular rate in the Y axis in degrees per second.
    */
   public synchronized double getGyroRateY() {
     if (m_simGyroRateY != null) {
@@ -1045,7 +1103,9 @@
   }
 
   /**
-   * @return gyro angular rate in the Z axis in degrees per second
+   * Returns the gyro angular rate in the Z axis in degrees per second.
+   *
+   * @return The gyro angular rate in the Z axis in degrees per second.
    */
   public synchronized double getGyroRateZ() {
     if (m_simGyroRateZ != null) {
@@ -1055,7 +1115,9 @@
   }
 
   /**
-   * @return urrent acceleration in the X axis in meters per second squared
+   * Returns the acceleration in the X axis in meters per second squared.
+   *
+   * @return The acceleration in the X axis in meters per second squared.
    */
   public synchronized double getAccelX() {
     if (m_simAccelX != null) {
@@ -1065,7 +1127,9 @@
   }
 
   /**
-   * @return current acceleration in the Y axis in meters per second squared
+   * Returns the acceleration in the Y axis in meters per second squared.
+   *
+   * @return The acceleration in the Y axis in meters per second squared.
    */
   public synchronized double getAccelY() {
     if (m_simAccelY != null) {
@@ -1075,7 +1139,9 @@
   }
 
   /**
-   * @return current acceleration in the Z axis in meters per second squared
+   * Returns the acceleration in the Z axis in meters per second squared.
+   *
+   * @return The acceleration in the Z axis in meters per second squared.
    */
   public synchronized double getAccelZ() {
     if (m_simAccelZ != null) {
@@ -1085,7 +1151,9 @@
   }
 
   /**
-   * @return Magnetic field strength in the X axis in Tesla
+   * Returns the magnetic field strength in the X axis in Tesla.
+   *
+   * @return The magnetic field strength in the X axis in Tesla.
    */
   public synchronized double getMagneticFieldX() {
     // mG to T
@@ -1093,7 +1161,9 @@
   }
 
   /**
-   * @return Magnetic field strength in the Y axis in Tesla
+   * Returns the magnetic field strength in the Y axis in Tesla.
+   *
+   * @return The magnetic field strength in the Y axis in Tesla.
    */
   public synchronized double getMagneticFieldY() {
     // mG to T
@@ -1101,7 +1171,9 @@
   }
 
   /**
-   * @return Magnetic field strength in the Z axis in Tesla
+   * Returns the magnetic field strength in the Z axis in Tesla.
+   *
+   * @return The magnetic field strength in the Z axis in Tesla.
    */
   public synchronized double getMagneticFieldZ() {
     // mG to T
@@ -1109,35 +1181,47 @@
   }
 
   /**
-   * @return X-axis complementary angle in degrees
+   * Returns the complementary angle around the X axis computed from accelerometer and gyro rate
+   * measurements.
+   *
+   * @return The X-axis complementary angle in degrees.
    */
   public synchronized double getXComplementaryAngle() {
     return m_compAngleX;
   }
 
   /**
-   * @return Y-axis complementary angle in degrees
+   * Returns the complementary angle around the Y axis computed from accelerometer and gyro rate
+   * measurements.
+   *
+   * @return The Y-axis complementary angle in degrees.
    */
   public synchronized double getYComplementaryAngle() {
     return m_compAngleY;
   }
 
   /**
-   * @return X-axis filtered acceleration angle in degrees
+   * Returns the X-axis filtered acceleration angle in degrees.
+   *
+   * @return The X-axis filtered acceleration angle in degrees.
    */
   public synchronized double getXFilteredAccelAngle() {
     return m_accelAngleX;
   }
 
   /**
-   * @return Y-axis filtered acceleration angle in degrees
+   * Returns the Y-axis filtered acceleration angle in degrees.
+   *
+   * @return The Y-axis filtered acceleration angle in degrees.
    */
   public synchronized double getYFilteredAccelAngle() {
     return m_accelAngleY;
   }
 
   /**
-   * @return Barometric Pressure in PSI
+   * Returns the barometric pressure in PSI.
+   *
+   * @return The barometric pressure in PSI.
    */
   public synchronized double getBarometricPressure() {
     // mbar to PSI conversion
@@ -1145,7 +1229,9 @@
   }
 
   /**
-   * @return Temperature in degrees Celsius
+   * Returns the temperature in degrees Celsius.
+   *
+   * @return The temperature in degrees Celsius.
    */
   public synchronized double getTemperature() {
     return m_temp;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
index 2fad3f3..b7236a5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
@@ -122,45 +122,21 @@
   private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word
   private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word
 
-  private static final byte[] m_autospi_x_packet = {
+  // Weight between the previous and current gyro angles represents 1 second for the timestamp,
+  // this is the point at which we ignore the previous angle because it is too old to be of use.
+  // The IMU timestamp conversion is 1 = 49.02us, the value 1_000_000 is the number of microseconds
+  // we average over.
+  private static final double AVERAGE_RATE_SCALING_FACTOR = 49.02 / 1_000_000;
+
+  private static final byte[] m_autospi_allAngle_packet = {
     X_DELTANG_OUT,
     FLASH_CNT,
     X_DELTANG_LOW,
     FLASH_CNT,
-    X_GYRO_OUT,
-    FLASH_CNT,
-    Y_GYRO_OUT,
-    FLASH_CNT,
-    Z_GYRO_OUT,
-    FLASH_CNT,
-    X_ACCL_OUT,
-    FLASH_CNT,
-    Y_ACCL_OUT,
-    FLASH_CNT,
-    Z_ACCL_OUT,
-    FLASH_CNT
-  };
-
-  private static final byte[] m_autospi_y_packet = {
     Y_DELTANG_OUT,
     FLASH_CNT,
     Y_DELTANG_LOW,
     FLASH_CNT,
-    X_GYRO_OUT,
-    FLASH_CNT,
-    Y_GYRO_OUT,
-    FLASH_CNT,
-    Z_GYRO_OUT,
-    FLASH_CNT,
-    X_ACCL_OUT,
-    FLASH_CNT,
-    Y_ACCL_OUT,
-    FLASH_CNT,
-    Z_ACCL_OUT,
-    FLASH_CNT
-  };
-
-  private static final byte[] m_autospi_z_packet = {
     Z_DELTANG_OUT,
     FLASH_CNT,
     Z_DELTANG_LOW,
@@ -179,31 +155,59 @@
     FLASH_CNT
   };
 
+  /** ADIS16470 calibration times. */
   public enum CalibrationTime {
+    /** 32 ms calibration time */
     _32ms(0),
+    /** 64 ms calibration time */
     _64ms(1),
+    /** 128 ms calibration time */
     _128ms(2),
+    /** 256 ms calibration time */
     _256ms(3),
+    /** 512 ms calibration time */
     _512ms(4),
+    /** 1 s calibration time */
     _1s(5),
+    /** 2 s calibration time */
     _2s(6),
+    /** 4 s calibration time */
     _4s(7),
+    /** 8 s calibration time */
     _8s(8),
+    /** 16 s calibration time */
     _16s(9),
+    /** 32 s calibration time */
     _32s(10),
+    /** 64 s calibration time */
     _64s(11);
 
-    private int value;
+    private final int value;
 
-    private CalibrationTime(int value) {
+    CalibrationTime(int value) {
       this.value = value;
     }
   }
 
+  /**
+   * IMU axes.
+   *
+   * <p>kX, kY, and kZ refer to the IMU's X, Y, and Z axes respectively. kYaw, kPitch, and kRoll are
+   * configured by the user to refer to an X, Y, or Z axis.
+   */
   public enum IMUAxis {
+    /** The IMU's X axis */
     kX,
+    /** The IMU's Y axis */
     kY,
-    kZ
+    /** The IMU's Z axis */
+    kZ,
+    /** The user-configured yaw axis */
+    kYaw,
+    /** The user-configured pitch axis */
+    kPitch,
+    /** The user-configured roll axis */
+    kRoll,
   }
 
   // Static Constants
@@ -212,19 +216,26 @@
   private static final double deg_to_rad = 0.0174532;
   private static final double grav = 9.81;
 
-  // User-specified yaw axis
+  // User-specified axes
   private IMUAxis m_yaw_axis;
+  private IMUAxis m_pitch_axis;
+  private IMUAxis m_roll_axis;
 
   // Instant raw outputs
   private double m_gyro_rate_x = 0.0;
   private double m_gyro_rate_y = 0.0;
   private double m_gyro_rate_z = 0.0;
+  private double m_average_gyro_rate_x = 0.0;
+  private double m_average_gyro_rate_y = 0.0;
+  private double m_average_gyro_rate_z = 0.0;
   private double m_accel_x = 0.0;
   private double m_accel_y = 0.0;
   private double m_accel_z = 0.0;
 
   // Integrated gyro angle
-  private double m_integ_angle = 0.0;
+  private double m_integ_angle_x = 0.0;
+  private double m_integ_angle_y = 0.0;
+  private double m_integ_angle_z = 0.0;
 
   // Complementary filter variables
   private double m_dt = 0.0;
@@ -278,17 +289,74 @@
     }
   }
 
+  /**
+   * Creates a new ADIS16740 IMU object.
+   *
+   * <p>The default setup is the onboard SPI port with a calibration time of 4 seconds. Yaw, pitch,
+   * and roll are kZ, kX, and kY respectively.
+   */
   public ADIS16470_IMU() {
-    this(IMUAxis.kZ, SPI.Port.kOnboardCS0, CalibrationTime._4s);
+    this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._4s);
   }
 
   /**
+   * Creates a new ADIS16740 IMU object.
+   *
+   * <p>The default setup is the onboard SPI port with a calibration time of 4 seconds.
+   *
+   * <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in
+   * an error.</i></b>
+   *
    * @param yaw_axis The axis that measures the yaw
+   * @param pitch_axis The axis that measures the pitch
+   * @param roll_axis The axis that measures the roll
+   */
+  public ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) {
+    this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._4s);
+  }
+
+  /**
+   * Creates a new ADIS16740 IMU object.
+   *
+   * <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in
+   * an error.</i></b>
+   *
+   * @param yaw_axis The axis that measures the yaw
+   * @param pitch_axis The axis that measures the pitch
+   * @param roll_axis The axis that measures the roll
    * @param port The SPI Port the gyro is plugged into
    * @param cal_time Calibration time
    */
-  public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
+  @SuppressWarnings("this-escape")
+  public ADIS16470_IMU(
+      IMUAxis yaw_axis,
+      IMUAxis pitch_axis,
+      IMUAxis roll_axis,
+      SPI.Port port,
+      CalibrationTime cal_time) {
+    if (yaw_axis == IMUAxis.kYaw
+        || yaw_axis == IMUAxis.kPitch
+        || yaw_axis == IMUAxis.kRoll
+        || pitch_axis == IMUAxis.kYaw
+        || pitch_axis == IMUAxis.kPitch
+        || pitch_axis == IMUAxis.kRoll
+        || roll_axis == IMUAxis.kYaw
+        || roll_axis == IMUAxis.kPitch
+        || roll_axis == IMUAxis.kRoll) {
+      DriverStation.reportError(
+          "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or IMUAxis.kZ as arguments.",
+          false);
+      DriverStation.reportError(
+          "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)", false);
+      yaw_axis = IMUAxis.kZ;
+      pitch_axis = IMUAxis.kY;
+      roll_axis = IMUAxis.kX;
+    }
+
     m_yaw_axis = yaw_axis;
+    m_pitch_axis = pitch_axis;
+    m_roll_axis = roll_axis;
+
     m_calibration_time = cal_time.value;
     m_spi_port = port;
 
@@ -368,6 +436,11 @@
     m_connected = true;
   }
 
+  /**
+   * Checks the connection status of the IMU.
+   *
+   * @return True if the IMU is connected, false otherwise.
+   */
   public boolean isConnected() {
     if (m_simConnected != null) {
       return m_simConnected.get();
@@ -396,7 +469,7 @@
    * @return
    */
   private static int toShort(int... buf) {
-    return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
+    return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF)));
   }
 
   /**
@@ -410,7 +483,7 @@
   /**
    * Switch to standard SPI mode.
    *
-   * @return
+   * @return True if successful, false otherwise.
    */
   private boolean switchToStandardSPI() {
     // Check to see whether the acquire thread is active. If so, wait for it to stop
@@ -476,7 +549,9 @@
   }
 
   /**
-   * @return
+   * Switch to auto SPI mode.
+   *
+   * @return True if successful, false otherwise.
    */
   boolean switchToAutoSPI() {
     // No SPI port has been set up. Go set one up first.
@@ -498,17 +573,7 @@
       m_auto_configured = true;
     }
     // Do we need to change auto SPI settings?
-    switch (m_yaw_axis) {
-      case kX:
-        m_spi.setAutoTransmitData(m_autospi_x_packet, 2);
-        break;
-      case kY:
-        m_spi.setAutoTransmitData(m_autospi_y_packet, 2);
-        break;
-      default:
-        m_spi.setAutoTransmitData(m_autospi_z_packet, 2);
-        break;
-    }
+    m_spi.setAutoTransmitData(m_autospi_allAngle_packet, 2);
     // Configure auto stall time
     m_spi.configureAutoStall(5, 1000, 1);
     // Kick off auto SPI (Note: Device configuration impossible after auto SPI is
@@ -560,18 +625,28 @@
     return 0;
   }
 
-  public int configDecRate(int reg) {
-    int m_reg = reg;
+  /**
+   * Configures the decimation rate of the IMU.
+   *
+   * @param decimationRate The new decimation value.
+   * @return 0 if success, 1 if no change, 2 if error.
+   */
+  public int configDecRate(int decimationRate) {
+    // Switches the active SPI port to standard SPI mode, writes a new value to
+    // the DECIMATE register in the IMU, and re-enables auto SPI.
+    //
+    // This function enters standard SPI mode, writes a new DECIMATE setting to
+    // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
     if (!switchToStandardSPI()) {
       DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
       return 2;
     }
-    if (m_reg > 1999) {
-      DriverStation.reportError("Attempted to write an invalid deimation value.", false);
-      m_reg = 1999;
+    if (decimationRate > 1999) {
+      DriverStation.reportError("Attempted to write an invalid decimation value.", false);
+      decimationRate = 1999;
     }
-    m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
-    writeRegister(DEC_RATE, m_reg);
+    m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0);
+    writeRegister(DEC_RATE, decimationRate);
     System.out.println("Decimation register: " + readRegister(DEC_RATE));
     if (!switchToAutoSPI()) {
       DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
@@ -593,29 +668,6 @@
     if (!switchToAutoSPI()) {
       DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
     }
-    ;
-  }
-
-  /**
-   * Sets the yaw axis
-   *
-   * @param yaw_axis The new yaw axis to use
-   * @return 1 if the new yaw axis is the same as the current one, 2 if the switch to Standard SPI
-   *     failed, else 0.
-   */
-  public int setYawAxis(IMUAxis yaw_axis) {
-    if (m_yaw_axis == yaw_axis) {
-      return 1;
-    }
-    if (!switchToStandardSPI()) {
-      DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
-      return 2;
-    }
-    m_yaw_axis = yaw_axis;
-    if (!switchToAutoSPI()) {
-      DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
-    }
-    return 0;
   }
 
   /**
@@ -650,12 +702,6 @@
     m_spi.write(buf, 2);
   }
 
-  public void reset() {
-    synchronized (this) {
-      m_integ_angle = 0.0;
-    }
-  }
-
   /** Delete (free) the spi port used for the IMU. */
   @Override
   public void close() {
@@ -691,7 +737,7 @@
   /** */
   private void acquire() {
     // Set data packet length
-    final int dataset_len = 19; // 18 data points + timestamp
+    final int dataset_len = 27; // 26 data points + timestamp
     final int BUFFER_SIZE = 4000;
 
     // Set up buffers and variables
@@ -700,7 +746,9 @@
     int data_remainder = 0;
     int data_to_read = 0;
     double previous_timestamp = 0.0;
-    double delta_angle = 0.0;
+    double delta_angle_x = 0.0;
+    double delta_angle_y = 0.0;
+    double delta_angle_z = 0.0;
     double gyro_rate_x = 0.0;
     double gyro_rate_y = 0.0;
     double gyro_rate_z = 0.0;
@@ -730,8 +778,8 @@
 
         data_count =
             m_spi.readAutoReceivedData(
-                buffer, 0, 0); // Read number of bytes currently stored in the
-        // buffer
+                buffer, 0, 0); // Read number of bytes currently stored in the buffer
+
         data_remainder =
             data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
         data_to_read = data_count - data_remainder; // Remove incomplete data from read count
@@ -772,18 +820,27 @@
            */
 
           /*
-           * Get delta angle value for selected yaw axis and scale by the elapsed time
+           * Get delta angle value for all 3 axes and scale by the elapsed time
            * (based on timestamp)
            */
-          delta_angle =
+          delta_angle_x =
               (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
                   / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-          gyro_rate_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0);
-          gyro_rate_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0);
-          gyro_rate_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0);
-          accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0);
-          accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0);
-          accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0);
+          delta_angle_y =
+              (toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf)
+                  / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+          delta_angle_z =
+              (toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14])
+                      * delta_angle_sf)
+                  / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+
+          gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0);
+          gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0);
+          gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0);
+
+          accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0);
+          accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0);
+          accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0);
 
           // Convert scaled sensor data to SI units (for tilt calculations)
           // TODO: Should the unit outputs be selectable?
@@ -809,6 +866,10 @@
                     accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
             compAngleX = accelAngleX;
             compAngleY = accelAngleY;
+
+            m_average_gyro_rate_x = 0.0;
+            m_average_gyro_rate_y = 0.0;
+            m_average_gyro_rate_z = 0.0;
           } else {
             // Run inclinometer calculations
             accelAngleX =
@@ -830,9 +891,13 @@
                * Don't accumulate first run. previous_timestamp will be "very" old and the
                * integration will end up way off
                */
-              m_integ_angle = 0.0;
+              m_integ_angle_x = 0.0;
+              m_integ_angle_y = 0.0;
+              m_integ_angle_z = 0.0;
             } else {
-              m_integ_angle += delta_angle;
+              m_integ_angle_x += delta_angle_x;
+              m_integ_angle_y += delta_angle_y;
+              m_integ_angle_z += delta_angle_z;
             }
             m_gyro_rate_x = gyro_rate_x;
             m_gyro_rate_y = gyro_rate_y;
@@ -844,16 +909,28 @@
             m_compAngleY = compAngleY * rad_to_deg;
             m_accelAngleX = accelAngleX * rad_to_deg;
             m_accelAngleY = accelAngleY * rad_to_deg;
+            m_average_gyro_rate_x += gyro_rate_x;
+            m_average_gyro_rate_y += gyro_rate_y;
+            m_average_gyro_rate_z += gyro_rate_z;
           }
           m_first_run = false;
         }
+
+        // The inverse of data to read divided by dataset length, his is the number of iterations
+        // of the for loop inverted (so multiplication can be used instead of division)
+        double invTotalIterations = (double) dataset_len / data_to_read;
+        m_average_gyro_rate_x *= invTotalIterations;
+        m_average_gyro_rate_y *= invTotalIterations;
+        m_average_gyro_rate_z *= invTotalIterations;
       } else {
         m_thread_idle = true;
         data_count = 0;
         data_remainder = 0;
         data_to_read = 0;
         previous_timestamp = 0.0;
-        delta_angle = 0.0;
+        delta_angle_x = 0.0;
+        delta_angle_y = 0.0;
+        delta_angle_z = 0.0;
         gyro_rate_x = 0.0;
         gyro_rate_y = 0.0;
         gyro_rate_z = 0.0;
@@ -933,111 +1010,267 @@
   }
 
   /**
-   * @return Yaw axis angle in degrees (CCW positive)
+   * Reset the gyro.
+   *
+   * <p>Resets the gyro accumulations to a heading of zero. This can be used if there is significant
+   * drift in the gyro and it needs to be recalibrated after running.
    */
-  public synchronized double getAngle() {
-    switch (m_yaw_axis) {
+  public void reset() {
+    synchronized (this) {
+      m_integ_angle_x = 0.0;
+      m_integ_angle_y = 0.0;
+      m_integ_angle_z = 0.0;
+    }
+  }
+
+  /**
+   * Allow the designated gyro angle to be set to a given value. This may happen with unread values
+   * in the buffer, it is suggested that the IMU is not moving when this method is run.
+   *
+   * @param axis IMUAxis that will be changed
+   * @param angle A double in degrees (CCW positive)
+   */
+  public void setGyroAngle(IMUAxis axis, double angle) {
+    switch (axis) {
+      case kYaw:
+        axis = m_yaw_axis;
+        break;
+      case kPitch:
+        axis = m_pitch_axis;
+        break;
+      case kRoll:
+        axis = m_roll_axis;
+        break;
+      default:
+    }
+
+    switch (axis) {
+      case kX:
+        this.setGyroAngleX(angle);
+        break;
+      case kY:
+        this.setGyroAngleY(angle);
+        break;
+      case kZ:
+        this.setGyroAngleZ(angle);
+        break;
+      default:
+    }
+  }
+
+  /**
+   * Allow the gyro angle X to be set to a given value. This may happen with unread values in the
+   * buffer, it is suggested that the IMU is not moving when this method is run.
+   *
+   * @param angle A double in degrees (CCW positive)
+   */
+  public void setGyroAngleX(double angle) {
+    synchronized (this) {
+      m_integ_angle_x = angle;
+    }
+  }
+
+  /**
+   * Allow the gyro angle Y to be set to a given value. This may happen with unread values in the
+   * buffer, it is suggested that the IMU is not moving when this method is run.
+   *
+   * @param angle A double in degrees (CCW positive)
+   */
+  public void setGyroAngleY(double angle) {
+    synchronized (this) {
+      m_integ_angle_y = angle;
+    }
+  }
+
+  /**
+   * Allow the gyro angle Z to be set to a given value. This may happen with unread values in the
+   * buffer, it is suggested that the IMU is not moving when this method is run.
+   *
+   * @param angle A double in degrees (CCW positive)
+   */
+  public void setGyroAngleZ(double angle) {
+    synchronized (this) {
+      m_integ_angle_z = angle;
+    }
+  }
+
+  /**
+   * Returns the axis angle in degrees (CCW positive).
+   *
+   * @param axis The IMUAxis whose angle to return.
+   * @return The axis angle in degrees (CCW positive).
+   */
+  public synchronized double getAngle(IMUAxis axis) {
+    switch (axis) {
+      case kYaw:
+        axis = m_yaw_axis;
+        break;
+      case kPitch:
+        axis = m_pitch_axis;
+        break;
+      case kRoll:
+        axis = m_roll_axis;
+        break;
+      default:
+    }
+
+    switch (axis) {
       case kX:
         if (m_simGyroAngleX != null) {
           return m_simGyroAngleX.get();
         }
-        break;
+        return m_integ_angle_x;
       case kY:
         if (m_simGyroAngleY != null) {
           return m_simGyroAngleY.get();
         }
-        break;
+        return m_integ_angle_y;
       case kZ:
         if (m_simGyroAngleZ != null) {
           return m_simGyroAngleZ.get();
         }
-        break;
+        return m_integ_angle_z;
+      default:
     }
-    return m_integ_angle;
+
+    return 0.0;
   }
 
   /**
-   * @return Yaw axis angular rate in degrees per second (CCW positive)
+   * Returns the axis angular rate in degrees per second (CCW positive).
+   *
+   * @param axis The IMUAxis whose rate to return.
+   * @return Axis angular rate in degrees per second (CCW positive).
    */
-  public synchronized double getRate() {
-    if (m_yaw_axis == IMUAxis.kX) {
-      if (m_simGyroRateX != null) {
-        return m_simGyroRateX.get();
-      }
-      return m_gyro_rate_x;
-    } else if (m_yaw_axis == IMUAxis.kY) {
-      if (m_simGyroRateY != null) {
-        return m_simGyroRateY.get();
-      }
-      return m_gyro_rate_y;
-    } else if (m_yaw_axis == IMUAxis.kZ) {
-      if (m_simGyroRateZ != null) {
-        return m_simGyroRateZ.get();
-      }
-      return m_gyro_rate_z;
-    } else {
-      return 0.0;
+  public synchronized double getRate(IMUAxis axis) {
+    switch (axis) {
+      case kYaw:
+        axis = m_yaw_axis;
+        break;
+      case kPitch:
+        axis = m_pitch_axis;
+        break;
+      case kRoll:
+        axis = m_roll_axis;
+        break;
+      default:
     }
+
+    switch (axis) {
+      case kX:
+        if (m_simGyroRateX != null) {
+          return m_simGyroRateX.get();
+        }
+        return m_gyro_rate_x;
+      case kY:
+        if (m_simGyroRateY != null) {
+          return m_simGyroRateY.get();
+        }
+        return m_gyro_rate_y;
+      case kZ:
+        if (m_simGyroRateZ != null) {
+          return m_simGyroRateZ.get();
+        }
+        return m_gyro_rate_z;
+      default:
+    }
+    return 0.0;
   }
 
   /**
-   * @return Yaw Axis
+   * Returns which axis, kX, kY, or kZ, is set to the yaw axis.
+   *
+   * @return IMUAxis Yaw Axis
    */
   public IMUAxis getYawAxis() {
     return m_yaw_axis;
   }
 
   /**
-   * @return current acceleration in the X axis
+   * Returns which axis, kX, kY, or kZ, is set to the pitch axis.
+   *
+   * @return IMUAxis Pitch Axis
+   */
+  public IMUAxis getPitchAxis() {
+    return m_pitch_axis;
+  }
+
+  /**
+   * Returns which axis, kX, kY, or kZ, is set to the roll axis.
+   *
+   * @return IMUAxis Roll Axis
+   */
+  public IMUAxis getRollAxis() {
+    return m_roll_axis;
+  }
+
+  /**
+   * Returns the acceleration in the X axis in meters per second squared.
+   *
+   * @return The acceleration in the X axis in meters per second squared.
    */
   public synchronized double getAccelX() {
     return m_accel_x * 9.81;
   }
 
   /**
-   * @return current acceleration in the Y axis
+   * Returns the acceleration in the Y axis in meters per second squared.
+   *
+   * @return The acceleration in the Y axis in meters per second squared.
    */
   public synchronized double getAccelY() {
     return m_accel_y * 9.81;
   }
 
   /**
-   * @return current acceleration in the Z axis
+   * Returns the acceleration in the Z axis in meters per second squared.
+   *
+   * @return The acceleration in the Z axis in meters per second squared.
    */
   public synchronized double getAccelZ() {
     return m_accel_z * 9.81;
   }
 
   /**
-   * @return X-axis complementary angle
+   * Returns the complementary angle around the X axis computed from accelerometer and gyro rate
+   * measurements.
+   *
+   * @return The X-axis complementary angle in degrees.
    */
   public synchronized double getXComplementaryAngle() {
     return m_compAngleX;
   }
 
   /**
-   * @return Y-axis complementary angle
+   * Returns the complementary angle around the Y axis computed from accelerometer and gyro rate
+   * measurements.
+   *
+   * @return The Y-axis complementary angle in degrees.
    */
   public synchronized double getYComplementaryAngle() {
     return m_compAngleY;
   }
 
   /**
-   * @return X-axis filtered acceleration angle
+   * Returns the X-axis filtered acceleration angle in degrees.
+   *
+   * @return The X-axis filtered acceleration angle in degrees.
    */
   public synchronized double getXFilteredAccelAngle() {
     return m_accelAngleX;
   }
 
   /**
-   * @return Y-axis filtered acceleration angle
+   * Returns the Y-axis filtered acceleration angle in degrees.
+   *
+   * @return The Y-axis filtered acceleration angle in degrees.
    */
   public synchronized double getYFilteredAccelAngle() {
     return m_accelAngleY;
   }
 
   /**
-   * Get the SPI port number.
+   * Gets the SPI port number.
    *
    * @return The SPI port number.
    */
@@ -1048,6 +1281,6 @@
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Gyro");
-    builder.addDoubleProperty("Value", this::getAngle, null);
+    builder.addDoubleProperty("Value", () -> getAngle(m_yaw_axis), null);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
index 7ceff95..4f6803d 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
@@ -27,7 +27,9 @@
  */
 @SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
 public class ADXL345_I2C implements NTSendable, AutoCloseable {
-  private static final byte kAddress = 0x1D;
+  /** Default I2C device address. */
+  public static final byte kAddress = 0x1D;
+
   private static final byte kPowerCtlRegister = 0x2D;
   private static final byte kDataFormatRegister = 0x31;
   private static final byte kDataRegister = 0x32;
@@ -43,16 +45,25 @@
   private static final byte kDataFormat_FullRes = 0x08;
   private static final byte kDataFormat_Justify = 0x04;
 
+  /** Accelerometer range. */
   public enum Range {
+    /** 2 Gs max. */
     k2G,
+    /** 4 Gs max. */
     k4G,
+    /** 8 Gs max. */
     k8G,
+    /** 16 Gs max. */
     k16G
   }
 
+  /** Accelerometer axes. */
   public enum Axes {
+    /** X axis. */
     kX((byte) 0x00),
+    /** Y axis. */
     kY((byte) 0x02),
+    /** Z axis. */
     kZ((byte) 0x04);
 
     /** The integer value representing this enumeration. */
@@ -63,20 +74,29 @@
     }
   }
 
+  /** Container type for accelerations from all axes. */
   @SuppressWarnings("MemberName")
   public static class AllAxes {
+    /** Acceleration along the X axis in g-forces. */
     public double XAxis;
+
+    /** Acceleration along the Y axis in g-forces. */
     public double YAxis;
+
+    /** Acceleration along the Z axis in g-forces. */
     public double ZAxis;
+
+    /** Default constructor. */
+    public AllAxes() {}
   }
 
-  protected I2C m_i2c;
+  private I2C m_i2c;
 
-  protected SimDevice m_simDevice;
-  protected SimEnum m_simRange;
-  protected SimDouble m_simX;
-  protected SimDouble m_simY;
-  protected SimDouble m_simZ;
+  private SimDevice m_simDevice;
+  private SimEnum m_simRange;
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
 
   /**
    * Constructs the ADXL345 Accelerometer with I2C address 0x1D.
@@ -95,6 +115,7 @@
    * @param range The range (+ or -) that the accelerometer will measure.
    * @param deviceAddress I2C address of the accelerometer (0x1D or 0x53)
    */
+  @SuppressWarnings("this-escape")
   public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
     m_i2c = new I2C(port, deviceAddress);
 
@@ -122,10 +143,20 @@
     SendableRegistry.addLW(this, "ADXL345_I2C", port.value);
   }
 
+  /**
+   * Returns the I2C port.
+   *
+   * @return The I2C port.
+   */
   public int getPort() {
     return m_i2c.getPort();
   }
 
+  /**
+   * Returns the I2C device address.
+   *
+   * @return The I2C device address.
+   */
   public int getDeviceAddress() {
     return m_i2c.getDeviceAddress();
   }
@@ -149,7 +180,7 @@
    * @param range The maximum acceleration, positive or negative, that the accelerometer will
    *     measure.
    */
-  public void setRange(Range range) {
+  public final void setRange(Range range) {
     final byte value;
     switch (range) {
       case k2G:
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
index 7291443..4c8435a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
@@ -40,16 +40,25 @@
   private static final int kDataFormat_FullRes = 0x08;
   private static final int kDataFormat_Justify = 0x04;
 
+  /** Accelerometer range. */
   public enum Range {
+    /** 2 Gs max. */
     k2G,
+    /** 4 Gs max. */
     k4G,
+    /** 8 Gs max. */
     k8G,
+    /** 16 Gs max. */
     k16G
   }
 
+  /** Accelerometer axes. */
   public enum Axes {
+    /** X axis. */
     kX((byte) 0x00),
+    /** Y axis. */
     kY((byte) 0x02),
+    /** Z axis. */
     kZ((byte) 0x04);
 
     /** The integer value representing this enumeration. */
@@ -60,20 +69,29 @@
     }
   }
 
+  /** Container type for accelerations from all axes. */
   @SuppressWarnings("MemberName")
   public static class AllAxes {
+    /** Acceleration along the X axis in g-forces. */
     public double XAxis;
+
+    /** Acceleration along the Y axis in g-forces. */
     public double YAxis;
+
+    /** Acceleration along the Z axis in g-forces. */
     public double ZAxis;
+
+    /** Default constructor. */
+    public AllAxes() {}
   }
 
-  protected SPI m_spi;
+  private SPI m_spi;
 
-  protected SimDevice m_simDevice;
-  protected SimEnum m_simRange;
-  protected SimDouble m_simX;
-  protected SimDouble m_simY;
-  protected SimDouble m_simZ;
+  private SimDevice m_simDevice;
+  private SimEnum m_simRange;
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
 
   /**
    * Constructor.
@@ -81,6 +99,7 @@
    * @param port The SPI port that the accelerometer is connected to
    * @param range The range (+ or -) that the accelerometer will measure.
    */
+  @SuppressWarnings("this-escape")
   public ADXL345_SPI(SPI.Port port, Range range) {
     m_spi = new SPI(port);
     // simulation
@@ -101,6 +120,11 @@
     SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
   }
 
+  /**
+   * Returns the SPI port.
+   *
+   * @return The SPI port.
+   */
   public int getPort() {
     return m_spi.getPort();
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
index b094be3..c3de6dd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
@@ -43,17 +43,26 @@
 
   private static final byte kPowerCtl_Measure = 0x02;
 
+  /** Accelerometer range. */
   public enum Range {
+    /** 2 Gs max. */
     k2G,
+    /** 4 Gs max. */
     k4G,
+    /** 8 Gs max. */
     k8G
   }
 
+  /** Accelerometer axes. */
   public enum Axes {
+    /** X axis. */
     kX((byte) 0x00),
+    /** Y axis. */
     kY((byte) 0x02),
+    /** Z axis. */
     kZ((byte) 0x04);
 
+    /** Axis value. */
     public final byte value;
 
     Axes(byte value) {
@@ -61,11 +70,20 @@
     }
   }
 
+  /** Container type for accelerations from all axes. */
   @SuppressWarnings("MemberName")
   public static class AllAxes {
+    /** Acceleration along the X axis in g-forces. */
     public double XAxis;
+
+    /** Acceleration along the Y axis in g-forces. */
     public double YAxis;
+
+    /** Acceleration along the Z axis in g-forces. */
     public double ZAxis;
+
+    /** Default constructor. */
+    public AllAxes() {}
   }
 
   private SPI m_spi;
@@ -93,6 +111,7 @@
    * @param port The SPI port that the accelerometer is connected to
    * @param range The range (+ or -) that the accelerometer will measure.
    */
+  @SuppressWarnings("this-escape")
   public ADXL362(SPI.Port port, Range range) {
     m_spi = new SPI(port);
 
@@ -141,6 +160,11 @@
     SendableRegistry.addLW(this, "ADXL362", port.value);
   }
 
+  /**
+   * Returns the SPI port.
+   *
+   * @return The SPI port.
+   */
   public int getPort() {
     return m_spi.getPort();
   }
@@ -164,7 +188,7 @@
    * @param range The maximum acceleration, positive or negative, that the accelerometer will
    *     measure.
    */
-  public void setRange(Range range) {
+  public final void setRange(Range range) {
     if (m_spi == null) {
       return;
     }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
index cbe34c8..799d432 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -59,6 +59,7 @@
    *
    * @param port The SPI port that the gyro is connected to
    */
+  @SuppressWarnings("this-escape")
   public ADXRS450_Gyro(SPI.Port port) {
     m_spi = new SPI(port);
 
@@ -113,7 +114,7 @@
    * are in progress, this is typically done when the robot is first turned on while it's sitting at
    * rest before the competition starts.
    */
-  public void calibrate() {
+  public final void calibrate() {
     if (m_spi == null) {
       return;
     }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
index 61a2fa6..bb097ab 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
@@ -14,7 +14,8 @@
  *
  * <p>By default, the timing supports WS2812B LEDs, but is configurable using setBitTiming()
  *
- * <p>Only 1 LED driver is currently supported by the roboRIO.
+ * <p>Only 1 LED driver is currently supported by the roboRIO. However, multiple LED strips can be
+ * connected in series and controlled from the single driver.
  */
 public class AddressableLED implements AutoCloseable {
   private final int m_pwmHandle;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
index 3ea9ef7..0bdb2ff 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
@@ -36,6 +36,7 @@
    *
    * @param channel The channel number for the analog input the accelerometer is connected to
    */
+  @SuppressWarnings("this-escape")
   public AnalogAccelerometer(final int channel) {
     this(new AnalogInput(channel), true);
     SendableRegistry.addChild(this, m_analogChannel);
@@ -49,10 +50,12 @@
    * @param channel The existing AnalogInput object for the analog input the accelerometer is
    *     connected to
    */
+  @SuppressWarnings("this-escape")
   public AnalogAccelerometer(final AnalogInput channel) {
     this(channel, false);
   }
 
+  @SuppressWarnings("this-escape")
   private AnalogAccelerometer(final AnalogInput channel, final boolean allocatedChannel) {
     requireNonNullParam(channel, "channel", "AnalogAccelerometer");
     m_allocatedChannel = allocatedChannel;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
index 8e45869..727ad2c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
@@ -22,9 +22,9 @@
   private double m_distancePerRotation = 1.0;
   private double m_lastPosition;
 
-  protected SimDevice m_simDevice;
-  protected SimDouble m_simPosition;
-  protected SimDouble m_simAbsolutePosition;
+  private SimDevice m_simDevice;
+  private SimDouble m_simPosition;
+  private SimDouble m_simAbsolutePosition;
 
   /**
    * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
@@ -40,6 +40,7 @@
    *
    * @param analogInput the analog input to attach to
    */
+  @SuppressWarnings("this-escape")
   public AnalogEncoder(AnalogInput analogInput) {
     m_analogInput = analogInput;
     init();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
index f5be4ad..29a17bb 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
@@ -25,13 +25,13 @@
  */
 public class AnalogGyro implements Sendable, AutoCloseable {
   private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
-  protected AnalogInput m_analog;
+  private AnalogInput m_analog;
   private boolean m_channelAllocated;
 
   private int m_gyroHandle;
 
   /** Initialize the gyro. Calibration is handled by calibrate(). */
-  public void initGyro() {
+  private void initGyro() {
     if (m_gyroHandle == 0) {
       m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port);
     }
@@ -78,6 +78,7 @@
    * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
    *     channels 0-1.
    */
+  @SuppressWarnings("this-escape")
   public AnalogGyro(int channel) {
     this(new AnalogInput(channel));
     m_channelAllocated = true;
@@ -91,6 +92,7 @@
    * @param channel The AnalogInput object that the gyro is connected to. Gyros can only be used on
    *     on-board channels 0-1.
    */
+  @SuppressWarnings("this-escape")
   public AnalogGyro(AnalogInput channel) {
     requireNonNullParam(channel, "channel", "AnalogGyro");
 
@@ -108,6 +110,7 @@
    * @param center Preset uncalibrated value to use as the accumulator center value.
    * @param offset Preset uncalibrated value to use as the gyro offset.
    */
+  @SuppressWarnings("this-escape")
   public AnalogGyro(int channel, int center, double offset) {
     this(new AnalogInput(channel), center, offset);
     m_channelAllocated = true;
@@ -123,6 +126,7 @@
    * @param center Preset uncalibrated value to use as the accumulator center value.
    * @param offset Preset uncalibrated value to use as the gyro offset.
    */
+  @SuppressWarnings("this-escape")
   public AnalogGyro(AnalogInput channel, int center, double offset) {
     requireNonNullParam(channel, "channel", "AnalogGyro");
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
index e90bfd2..3c6458b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -38,6 +38,7 @@
    *
    * @param channel The channel number to represent. 0-3 are on-board 4-7 are on the MXP port.
    */
+  @SuppressWarnings("this-escape")
   public AnalogInput(final int channel) {
     AnalogJNI.checkAnalogInputChannel(channel);
     m_channel = channel;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
index 299d89c..354cedc 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
@@ -21,6 +21,7 @@
    *
    * @param channel The channel number to represent.
    */
+  @SuppressWarnings("this-escape")
   public AnalogOutput(final int channel) {
     SensorUtil.checkAnalogOutputChannel(channel);
     m_channel = channel;
@@ -49,10 +50,20 @@
     return m_channel;
   }
 
+  /**
+   * Set the value of the analog output.
+   *
+   * @param voltage The output value in Volts, from 0.0 to +5.0.
+   */
   public void setVoltage(double voltage) {
     AnalogJNI.setAnalogOutput(m_port, voltage);
   }
 
+  /**
+   * Get the voltage of the analog output.
+   *
+   * @return The value in Volts, from 0.0 to +5.0.
+   */
   public double getVoltage() {
     return AnalogJNI.getAnalogOutput(m_port);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
index 7d58585..1547349 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -33,6 +33,7 @@
    * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
    * @param offset The offset to add to the scaled value for controlling the zero value
    */
+  @SuppressWarnings("this-escape")
   public AnalogPotentiometer(final int channel, double fullRange, double offset) {
     this(new AnalogInput(channel), fullRange, offset);
     m_initAnalogInput = true;
@@ -53,6 +54,7 @@
    *     input.
    * @param offset The angular value (in desired units) representing the angular output at 0V.
    */
+  @SuppressWarnings("this-escape")
   public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
     SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel());
     m_analogInput = input;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
index 8baa508..ac25fbb 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
@@ -15,30 +15,22 @@
 
 /** Class for creating and configuring Analog Triggers. */
 public class AnalogTrigger implements Sendable, AutoCloseable {
-  /** Exceptions dealing with improper operation of the Analog trigger. */
-  public static class AnalogTriggerException extends RuntimeException {
-    /**
-     * Create a new exception with the given message.
-     *
-     * @param message the message to pass with the exception
-     */
-    public AnalogTriggerException(String message) {
-      super(message);
-    }
-  }
-
   /** Where the analog trigger is attached. */
   protected int m_port;
 
-  protected AnalogInput m_analogInput;
-  protected DutyCycle m_dutyCycle;
-  protected boolean m_ownsAnalog;
+  private AnalogInput m_analogInput;
+
+  @SuppressWarnings({"PMD.SingularField", "PMD.UnusedPrivateField"})
+  private DutyCycle m_dutyCycle;
+
+  private boolean m_ownsAnalog;
 
   /**
    * Constructor for an analog trigger given a channel number.
    *
    * @param channel the port to use for the analog trigger
    */
+  @SuppressWarnings("this-escape")
   public AnalogTrigger(final int channel) {
     this(new AnalogInput(channel));
     m_ownsAnalog = true;
@@ -51,6 +43,7 @@
    *
    * @param channel the AnalogInput to use for the analog trigger
    */
+  @SuppressWarnings("this-escape")
   public AnalogTrigger(AnalogInput channel) {
     m_analogInput = channel;
 
@@ -67,6 +60,7 @@
    *
    * @param input the DutyCycle to use for the analog trigger
    */
+  @SuppressWarnings("this-escape")
   public AnalogTrigger(DutyCycle input) {
     m_dutyCycle = input;
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
index c80179b..4d459fc 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
@@ -106,9 +106,13 @@
 
   /** Defines the state in which the AnalogTrigger triggers. */
   public enum AnalogTriggerType {
+    /** In window. */
     kInWindow(AnalogJNI.AnalogTriggerType.kInWindow),
+    /** State. */
     kState(AnalogJNI.AnalogTriggerType.kState),
+    /** Rising pulse. */
     kRisingPulse(AnalogJNI.AnalogTriggerType.kRisingPulse),
+    /** Falling pulse. */
     kFallingPulse(AnalogJNI.AnalogTriggerType.kFallingPulse);
 
     private final int value;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
index 1a6b103..522ab88 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
@@ -17,9 +17,13 @@
  * <p>This class allows access to the roboRIO's internal accelerometer.
  */
 public class BuiltInAccelerometer implements Sendable, AutoCloseable {
+  /** Accelerometer range. */
   public enum Range {
+    /** 2 Gs max. */
     k2G,
+    /** 4 Gs max. */
     k4G,
+    /** 8 Gs max. */
     k8G
   }
 
@@ -28,6 +32,7 @@
    *
    * @param range The range the accelerometer will measure
    */
+  @SuppressWarnings("this-escape")
   public BuiltInAccelerometer(Range range) {
     setRange(range);
     HAL.report(tResourceType.kResourceType_Accelerometer, 0, 0, "Built-in accelerometer");
@@ -50,7 +55,7 @@
    * @param range The maximum acceleration, positive or negative, that the accelerometer will
    *     measure.
    */
-  public void setRange(Range range) {
+  public final void setRange(Range range) {
     AccelerometerJNI.setAccelerometerActive(false);
 
     switch (range) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
index 8808a4b..df0be48 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.CANAPIJNI;
+import edu.wpi.first.hal.CANAPITypes;
 import edu.wpi.first.hal.CANData;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -21,8 +22,11 @@
  * calls.
  */
 public class CAN implements Closeable {
-  public static final int kTeamManufacturer = 8;
-  public static final int kTeamDeviceType = 10;
+  /** Team manufacturer. */
+  public static final int kTeamManufacturer = CANAPITypes.CANManufacturer.kTeamUse.id;
+
+  /** Team device type. */
+  public static final int kTeamDeviceType = CANAPITypes.CANDeviceType.kMiscellaneous.id;
 
   private final int m_handle;
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
index f2cfeb3..e7a14bd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -31,6 +31,7 @@
    * @param module The module ID to use.
    * @param moduleType The module type to use.
    */
+  @SuppressWarnings("this-escape")
   public Compressor(int module, PneumaticsModuleType moduleType) {
     m_module = PneumaticsBase.getForType(module, moduleType);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CompressorConfigType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CompressorConfigType.java
index 48bb81f..7735cdd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CompressorConfigType.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CompressorConfigType.java
@@ -6,12 +6,18 @@
 
 import edu.wpi.first.hal.REVPHJNI;
 
+/** Compressor config type. */
 public enum CompressorConfigType {
+  /** Disabled. */
   Disabled(REVPHJNI.COMPRESSOR_CONFIG_TYPE_DISABLED),
+  /** Digital. */
   Digital(REVPHJNI.COMPRESSOR_CONFIG_TYPE_DIGITAL),
+  /** Analog. */
   Analog(REVPHJNI.COMPRESSOR_CONFIG_TYPE_ANALOG),
+  /** Hybrid. */
   Hybrid(REVPHJNI.COMPRESSOR_CONFIG_TYPE_HYBRID);
 
+  /** CompressorConfigType value. */
   public final int value;
 
   CompressorConfigType(int value) {
@@ -37,6 +43,11 @@
     }
   }
 
+  /**
+   * Returns the CompressorConfigType's value.
+   *
+   * @return The CompressorConfigType's value.
+   */
   public int getValue() {
     return value;
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
index d8c5015..eca3dd5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
@@ -38,6 +38,7 @@
     /** mode: external direction. */
     kExternalDirection(3);
 
+    /** Mode value. */
     public final int value;
 
     Mode(int value) {
@@ -45,19 +46,30 @@
     }
   }
 
-  protected DigitalSource m_upSource; // /< What makes the counter count up.
-  protected DigitalSource m_downSource; // /< What makes the counter count down.
+  /** What makes the counter count up. */
+  protected DigitalSource m_upSource;
+
+  /** What makes the counter count down. */
+  protected DigitalSource m_downSource;
+
   private boolean m_allocatedUpSource;
   private boolean m_allocatedDownSource;
-  int m_counter; // /< The FPGA counter object.
-  private int m_index; // /< The index of this counter.
-  private double m_distancePerPulse = 1; // distance of travel for each tick
+
+  /** The FPGA counter object. */
+  int m_counter;
+
+  /** The index of this counter. */
+  private int m_index;
+
+  /** Distance of travel for each tick. */
+  private double m_distancePerPulse = 1;
 
   /**
    * Create an instance of a counter with the given mode.
    *
    * @param mode The counter mode.
    */
+  @SuppressWarnings("this-escape")
   public Counter(final Mode mode) {
     ByteBuffer index = ByteBuffer.allocateDirect(4);
     // set the byte order
@@ -95,6 +107,7 @@
    *
    * @param source the digital source to count
    */
+  @SuppressWarnings("this-escape")
   public Counter(DigitalSource source) {
     this();
 
@@ -109,6 +122,7 @@
    *
    * @param channel the DIO channel to use as the up source. 0-9 are on-board, 10-25 are on the MXP
    */
+  @SuppressWarnings("this-escape")
   public Counter(int channel) {
     this();
     setUpSource(channel);
@@ -125,6 +139,7 @@
    * @param downSource second source for direction
    * @param inverted true to invert the count
    */
+  @SuppressWarnings("this-escape")
   public Counter(
       EncodingType encodingType,
       DigitalSource upSource,
@@ -162,6 +177,7 @@
    *
    * @param trigger the analog trigger to count
    */
+  @SuppressWarnings("this-escape")
   public Counter(AnalogTrigger trigger) {
     this();
 
@@ -200,7 +216,7 @@
    *
    * @param channel the DIO channel to count 0-9 are on-board, 10-25 are on the MXP
    */
-  public void setUpSource(int channel) {
+  public final void setUpSource(int channel) {
     setUpSource(new DigitalInput(channel));
     m_allocatedUpSource = true;
     SendableRegistry.addChild(this, m_upSource);
@@ -401,7 +417,7 @@
    * @param maxPeriod The maximum period where the counted device is considered moving in seconds.
    */
   @Override
-  public void setMaxPeriod(double maxPeriod) {
+  public final void setMaxPeriod(double maxPeriod) {
     CounterJNI.setCounterMaxPeriod(m_counter, maxPeriod);
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
index 60d5690..6e03b72 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
@@ -22,6 +22,7 @@
     /** Count rising and falling on both channels. */
     k4X(2);
 
+    /** EncodingType value. */
     public final int value;
 
     EncodingType(int value) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java
index a919da5..bd26a7b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java
@@ -7,9 +7,11 @@
 import edu.wpi.first.hal.DMAJNI;
 import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
 
+/** Class for configuring Direct Memory Access (DMA) of FPGA inputs. */
 public class DMA implements AutoCloseable {
   final int m_dmaHandle;
 
+  /** Default constructor. */
   public DMA() {
     m_dmaHandle = DMAJNI.initialize();
   }
@@ -19,50 +21,128 @@
     DMAJNI.free(m_dmaHandle);
   }
 
+  /**
+   * Sets whether DMA is paused.
+   *
+   * @param pause True pauses DMA.
+   */
   public void setPause(boolean pause) {
     DMAJNI.setPause(m_dmaHandle, pause);
   }
 
+  /**
+   * Sets DMA to trigger at an interval.
+   *
+   * @param periodSeconds Period at which to trigger DMA in seconds.
+   */
   public void setTimedTrigger(double periodSeconds) {
     DMAJNI.setTimedTrigger(m_dmaHandle, periodSeconds);
   }
 
+  /**
+   * Sets number of DMA cycles to trigger.
+   *
+   * @param cycles Number of cycles.
+   */
   public void setTimedTriggerCycles(int cycles) {
     DMAJNI.setTimedTriggerCycles(m_dmaHandle, cycles);
   }
 
+  /**
+   * Adds position data for an encoder to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param encoder Encoder to add to DMA.
+   */
   public void addEncoder(Encoder encoder) {
     DMAJNI.addEncoder(m_dmaHandle, encoder.m_encoder);
   }
 
+  /**
+   * Adds timer data for an encoder to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param encoder Encoder to add to DMA.
+   */
   public void addEncoderPeriod(Encoder encoder) {
     DMAJNI.addEncoderPeriod(m_dmaHandle, encoder.m_encoder);
   }
 
+  /**
+   * Adds position data for an counter to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param counter Counter to add to DMA.
+   */
   public void addCounter(Counter counter) {
     DMAJNI.addCounter(m_dmaHandle, counter.m_counter);
   }
 
+  /**
+   * Adds timer data for an counter to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param counter Counter to add to DMA.
+   */
   public void addCounterPeriod(Counter counter) {
     DMAJNI.addCounterPeriod(m_dmaHandle, counter.m_counter);
   }
 
+  /**
+   * Adds a digital source to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param digitalSource DigitalSource to add to DMA.
+   */
   public void addDigitalSource(DigitalSource digitalSource) {
     DMAJNI.addDigitalSource(m_dmaHandle, digitalSource.getPortHandleForRouting());
   }
 
+  /**
+   * Adds a duty cycle input to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param dutyCycle DutyCycle to add to DMA.
+   */
   public void addDutyCycle(DutyCycle dutyCycle) {
     DMAJNI.addDutyCycle(m_dmaHandle, dutyCycle.m_handle);
   }
 
+  /**
+   * Adds an analog input to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param analogInput AnalogInput to add to DMA.
+   */
   public void addAnalogInput(AnalogInput analogInput) {
     DMAJNI.addAnalogInput(m_dmaHandle, analogInput.m_port);
   }
 
+  /**
+   * Adds averaged data of an analog input to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param analogInput AnalogInput to add to DMA.
+   */
   public void addAveragedAnalogInput(AnalogInput analogInput) {
     DMAJNI.addAveragedAnalogInput(m_dmaHandle, analogInput.m_port);
   }
 
+  /**
+   * Adds accumulator data of an analog input to be collected by DMA.
+   *
+   * <p>This can only be called if DMA is not started.
+   *
+   * @param analogInput AnalogInput to add to DMA.
+   */
   public void addAnalogAccumulator(AnalogInput analogInput) {
     DMAJNI.addAnalogAccumulator(m_dmaHandle, analogInput.m_port);
   }
@@ -84,26 +164,58 @@
         falling);
   }
 
-  public int setPwmEdgeTrigger(PWMMotorController pwm, boolean rising, boolean falling) {
-    return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getPwmHandle(), 0, rising, falling);
-  }
-
+  /**
+   * Sets a DMA PWM edge trigger.
+   *
+   * @param pwm the PWM to trigger from.
+   * @param rising trigger on rising edge.
+   * @param falling trigger on falling edge.
+   * @return the index of the trigger
+   */
   public int setPwmEdgeTrigger(PWM pwm, boolean rising, boolean falling) {
     return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getHandle(), 0, rising, falling);
   }
 
+  /**
+   * Sets a DMA PWMMotorController edge trigger.
+   *
+   * @param pwm the PWMMotorController to trigger from.
+   * @param rising trigger on rising edge.
+   * @param falling trigger on falling edge.
+   * @return the index of the trigger
+   */
+  public int setPwmEdgeTrigger(PWMMotorController pwm, boolean rising, boolean falling) {
+    return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getPwmHandle(), 0, rising, falling);
+  }
+
+  /**
+   * Clear all sensors from the DMA collection list.
+   *
+   * <p>This can only be called if DMA is not started.
+   */
   public void clearSensors() {
     DMAJNI.clearSensors(m_dmaHandle);
   }
 
+  /**
+   * Clear all external triggers from the DMA trigger list.
+   *
+   * <p>This can only be called if DMA is not started.
+   */
   public void clearExternalTriggers() {
     DMAJNI.clearExternalTriggers(m_dmaHandle);
   }
 
+  /**
+   * Starts DMA Collection.
+   *
+   * @param queueDepth The number of objects to be able to queue.
+   */
   public void start(int queueDepth) {
     DMAJNI.startDMA(m_dmaHandle, queueDepth);
   }
 
+  /** Stops DMA Collection. */
   public void stop() {
     DMAJNI.stopDMA(m_dmaHandle);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java
index 1055dbc..1435cd3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java
@@ -7,10 +7,15 @@
 import edu.wpi.first.hal.AnalogJNI;
 import edu.wpi.first.hal.DMAJNISample;
 
+/** DMA sample. */
 public class DMASample {
+  /** DMA read status. */
   public enum DMAReadStatus {
+    /** OK status. */
     kOk(1),
+    /** Timeout status. */
     kTimeout(2),
+    /** Error status. */
     kError(3);
 
     private final int value;
@@ -19,6 +24,11 @@
       this.value = value;
     }
 
+    /**
+     * Returns the DMAReadStatus value.
+     *
+     * @return The DMAReadStatus value.
+     */
     public int getValue() {
       return value;
     }
@@ -41,39 +51,80 @@
 
   private final DMAJNISample m_dmaSample = new DMAJNISample();
 
+  /** Default constructor. */
+  public DMASample() {}
+
+  /**
+   * Retrieves a new DMA sample.
+   *
+   * @param dma DMA object.
+   * @param timeoutSeconds Timeout in seconds for retrieval.
+   * @return DMA read status.
+   */
   public DMAReadStatus update(DMA dma, double timeoutSeconds) {
     return DMAReadStatus.getValue(m_dmaSample.update(dma.m_dmaHandle, timeoutSeconds));
   }
 
-  public int getCaptureSize() {
-    return m_dmaSample.getCaptureSize();
-  }
-
-  public int getTriggerChannels() {
-    return m_dmaSample.getTriggerChannels();
-  }
-
-  public int getRemaining() {
-    return m_dmaSample.getRemaining();
-  }
-
+  /**
+   * Returns the DMA sample time in microseconds.
+   *
+   * @return The DMA sample time in microseconds.
+   */
   public long getTime() {
     return m_dmaSample.getTime();
   }
 
+  /**
+   * Returns the DMA sample timestamp in seconds.
+   *
+   * @return The DMA sample timestamp in seconds.
+   */
   public double getTimeStamp() {
     return getTime() * 1.0e-6;
   }
 
+  /**
+   * Returns the DMA sample capture size.
+   *
+   * @return The DMA sample capture size.
+   */
+  public int getCaptureSize() {
+    return m_dmaSample.getCaptureSize();
+  }
+
+  /**
+   * Returns the number of DMA trigger channels.
+   *
+   * @return The number of DMA trigger channels.
+   */
+  public int getTriggerChannels() {
+    return m_dmaSample.getTriggerChannels();
+  }
+
+  /**
+   * Returns the number of remaining samples.
+   *
+   * @return The number of remaining samples.
+   */
+  public int getRemaining() {
+    return m_dmaSample.getRemaining();
+  }
+
+  /**
+   * Returns raw encoder value from DMA.
+   *
+   * @param encoder Encoder used for DMA.
+   * @return Raw encoder value from DMA.
+   */
   public int getEncoderRaw(Encoder encoder) {
     return m_dmaSample.getEncoder(encoder.m_encoder);
   }
 
   /**
-   * Gets the scaled encoder distance for this sample.
+   * Returns encoder distance from DMA.
    *
-   * @param encoder the encoder to use to read
-   * @return the distance
+   * @param encoder Encoder used for DMA.
+   * @return Encoder distance from DMA.
    */
   public double getEncoderDistance(Encoder encoder) {
     double val = getEncoderRaw(encoder);
@@ -82,43 +133,103 @@
     return val;
   }
 
+  /**
+   * Returns raw encoder period from DMA.
+   *
+   * @param encoder Encoder used for DMA.
+   * @return Raw encoder period from DMA.
+   */
   public int getEncoderPeriodRaw(Encoder encoder) {
     return m_dmaSample.getEncoderPeriod(encoder.m_encoder);
   }
 
+  /**
+   * Returns counter value from DMA.
+   *
+   * @param counter Counter used for DMA.
+   * @return Counter value from DMA.
+   */
   public int getCounter(Counter counter) {
     return m_dmaSample.getCounter(counter.m_counter);
   }
 
+  /**
+   * Returns counter period from DMA.
+   *
+   * @param counter Counter used for DMA.
+   * @return Counter period from DMA.
+   */
   public int getCounterPeriod(Counter counter) {
     return m_dmaSample.getCounterPeriod(counter.m_counter);
   }
 
+  /**
+   * Returns digital source value from DMA.
+   *
+   * @param digitalSource DigitalSource used for DMA.
+   * @return DigitalSource value from DMA.
+   */
   public boolean getDigitalSource(DigitalSource digitalSource) {
     return m_dmaSample.getDigitalSource(digitalSource.getPortHandleForRouting());
   }
 
+  /**
+   * Returns raw analog input value from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @return Raw analog input value from DMA.
+   */
   public int getAnalogInputRaw(AnalogInput analogInput) {
     return m_dmaSample.getAnalogInput(analogInput.m_port);
   }
 
+  /**
+   * Returns analog input voltage from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @return Analog input voltage from DMA.
+   */
   public double getAnalogInputVoltage(AnalogInput analogInput) {
     return AnalogJNI.getAnalogValueToVolts(analogInput.m_port, getAnalogInputRaw(analogInput));
   }
 
+  /**
+   * Returns averaged raw analog input value from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @return Averaged raw analog input value from DMA.
+   */
   public int getAveragedAnalogInputRaw(AnalogInput analogInput) {
     return m_dmaSample.getAnalogInputAveraged(analogInput.m_port);
   }
 
+  /**
+   * Returns averaged analog input voltage from DMA.
+   *
+   * @param analogInput AnalogInput used for DMA.
+   * @return Averaged analog input voltage from DMA.
+   */
   public double getAveragedAnalogInputVoltage(AnalogInput analogInput) {
     return AnalogJNI.getAnalogValueToVolts(
         analogInput.m_port, getAveragedAnalogInputRaw(analogInput));
   }
 
+  /**
+   * Returns raw duty cycle output from DMA.
+   *
+   * @param dutyCycle DutyCycle used for DMA.
+   * @return Raw duty cycle output from DMA.
+   */
   public int getDutyCycleOutputRaw(DutyCycle dutyCycle) {
     return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle);
   }
 
+  /**
+   * Returns duty cycle output (0-1) from DMA.
+   *
+   * @param dutyCycle DutyCycle used for DMA.
+   * @return Duty cycle output (0-1) from DMA.
+   */
   public double getDutyCycleOutput(DutyCycle dutyCycle) {
     return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle)
         / (double) dutyCycle.getOutputScaleFactor();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
index 8837d41..a656d9c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
@@ -20,7 +20,7 @@
   }
 
   /** Update internal Driver Station control word. */
-  public void refresh() {
+  public final void refresh() {
     DriverStation.refreshControlWordFromCache(m_controlWord);
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java
index 8098ae9..d4ec796 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java
@@ -25,8 +25,8 @@
 /**
  * Centralized data log that provides automatic data log file management. It automatically cleans up
  * old files when disk space is low and renames the file based either on current date/time or (if
- * available) competition match number. The deta file will be saved to a USB flash drive if one is
- * attached, or to /home/lvuser otherwise.
+ * available) competition match number. The data file will be saved to a USB flash drive in a folder
+ * named "logs" if one is attached, or to /home/lvuser/logs otherwise.
  *
  * <p>Log files are initially named "FRC_TBD_{random}.wpilog" until the DS connects. After the DS
  * connects, the log file is renamed to "FRC_yyyyMMdd_HHmmss.wpilog" (where the date/time is UTC).
@@ -214,7 +214,10 @@
         // prefer a mounted USB drive if one is accessible
         Path usbDir = Paths.get("/u").toRealPath();
         if (Files.isWritable(usbDir)) {
-          return usbDir.toString();
+          if (!new File("/u/logs").mkdir()) {
+            // ignored
+          }
+          return "/u/logs";
         }
       } catch (IOException ex) {
         // ignored
@@ -225,8 +228,16 @@
                 + " Plug in a FAT32 formatted flash drive!",
             false);
       }
+      if (!new File("/home/lvuser/logs").mkdir()) {
+        // ignored
+      }
+      return "/home/lvuser/logs";
     }
-    return Filesystem.getOperatingDirectory().getAbsolutePath();
+    String logDir = Filesystem.getOperatingDirectory().getAbsolutePath() + "/logs";
+    if (!new File(logDir).mkdir()) {
+      // ignored
+    }
+    return logDir;
   }
 
   private static String makeLogFilename(String filenameOverride) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
index 3422d34..2fcc9dd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
@@ -20,6 +20,7 @@
  */
 public class DigitalGlitchFilter implements Sendable, AutoCloseable {
   /** Configures the Digital Glitch Filter to its default settings. */
+  @SuppressWarnings("this-escape")
   public DigitalGlitchFilter() {
     m_mutex.lock();
     try {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
index 5b4cf5e..ab3d5e5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
@@ -27,6 +27,7 @@
    *
    * @param channel the DIO channel for the digital input 0-9 are on-board, 10-25 are on the MXP
    */
+  @SuppressWarnings("this-escape")
   public DigitalInput(int channel) {
     SensorUtil.checkDigitalChannel(channel);
     m_channel = channel;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
index 78e8bed..a7446f6 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
@@ -29,6 +29,7 @@
    * @param channel the DIO channel to use for the digital output. 0-9 are on-board, 10-25 are on
    *     the MXP
    */
+  @SuppressWarnings("this-escape")
   public DigitalOutput(int channel) {
     SensorUtil.checkDigitalChannel(channel);
     m_channel = channel;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
index cb8b024..494efe9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
@@ -11,8 +11,21 @@
  * source. The source can either be a digital input or analog trigger but not both.
  */
 public abstract class DigitalSource implements AutoCloseable {
+  /** Default constructor. */
+  public DigitalSource() {}
+
+  /**
+   * Returns true if this DigitalSource is an AnalogTrigger.
+   *
+   * @return True if this DigitalSource is an AnalogTrigger.
+   */
   public abstract boolean isAnalogTrigger();
 
+  /**
+   * The DigitalSource channel.
+   *
+   * @return The DigitalSource channel.
+   */
   public abstract int getChannel();
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
index d749ccc..de0697c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -21,8 +21,11 @@
 public class DoubleSolenoid implements Sendable, AutoCloseable {
   /** Possible values for a DoubleSolenoid. */
   public enum Value {
+    /** Off position. */
     kOff,
+    /** Forward position. */
     kForward,
+    /** Reverse position. */
     kReverse
   }
 
@@ -53,7 +56,7 @@
    * @param forwardChannel The forward channel on the module to control.
    * @param reverseChannel The reverse channel on the module to control.
    */
-  @SuppressWarnings("PMD.UseTryWithResources")
+  @SuppressWarnings({"PMD.UseTryWithResources", "this-escape"})
   public DoubleSolenoid(
       final int module,
       final PneumaticsModuleType moduleType,
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
index ce3a3e0..4ee656b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -29,7 +29,7 @@
 
 /** Provide access to the network communication data to / from the Driver Station. */
 public final class DriverStation {
-  /** Number of Joystick Ports. */
+  /** Number of Joystick ports. */
   public static final int kJoystickPorts = 6;
 
   private static class HALJoystickButtons {
@@ -69,14 +69,21 @@
 
   /** The robot alliance that the robot is a part of. */
   public enum Alliance {
+    /** Red alliance. */
     Red,
+    /** Blue alliance. */
     Blue
   }
 
+  /** The type of robot match that the robot is part of. */
   public enum MatchType {
+    /** None. */
     None,
+    /** Practice. */
     Practice,
+    /** Qualification. */
     Qualification,
+    /** Elimination. */
     Elimination
   }
 
@@ -239,6 +246,7 @@
         for (int i = 0; i < count; i++) {
           if (axes.m_axes[i] != m_prevAxes.m_axes[i]) {
             needToLog = true;
+            break;
           }
         }
       }
@@ -255,6 +263,7 @@
         for (int i = 0; i < count; i++) {
           if (povs.m_povs[i] != m_prevPOVs.m_povs[i]) {
             needToLog = true;
+            break;
           }
         }
       }
@@ -1113,7 +1122,7 @@
    *
    * <p>If the FMS is not connected, it is set from the team alliance setting on the driver station.
    *
-   * @return the current alliance
+   * @return The alliance (red or blue) or an empty optional if the alliance is invalid
    */
   public static Optional<Alliance> getAlliance() {
     AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
@@ -1312,10 +1321,20 @@
     }
   }
 
+  /**
+   * Registers the given handle for DS data refresh notifications.
+   *
+   * @param handle The event handle.
+   */
   public static void provideRefreshedDataEventHandle(int handle) {
     m_refreshEvents.add(handle);
   }
 
+  /**
+   * Unregisters the given handle from DS data refresh notifications.
+   *
+   * @param handle The event handle.
+   */
   public static void removeRefreshedDataEventHandle(int handle) {
     m_refreshEvents.remove(handle);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
index d500ba7..8c26d55 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
@@ -33,6 +33,7 @@
    *
    * @param digitalSource The DigitalSource to use.
    */
+  @SuppressWarnings("this-escape")
   public DutyCycle(DigitalSource digitalSource) {
     m_handle =
         DutyCycleJNI.initialize(
@@ -102,6 +103,11 @@
     return DutyCycleJNI.getFPGAIndex(m_handle);
   }
 
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
   public int getSourceChannel() {
     return m_source.getChannel();
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
index afdcb3f..a0858ce 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
@@ -30,17 +30,18 @@
   private double m_sensorMin;
   private double m_sensorMax = 1.0;
 
-  protected SimDevice m_simDevice;
-  protected SimDouble m_simPosition;
-  protected SimDouble m_simAbsolutePosition;
-  protected SimDouble m_simDistancePerRotation;
-  protected SimBoolean m_simIsConnected;
+  private SimDevice m_simDevice;
+  private SimDouble m_simPosition;
+  private SimDouble m_simAbsolutePosition;
+  private SimDouble m_simDistancePerRotation;
+  private SimBoolean m_simIsConnected;
 
   /**
    * Construct a new DutyCycleEncoder on a specific channel.
    *
    * @param channel the channel to attach to
    */
+  @SuppressWarnings("this-escape")
   public DutyCycleEncoder(int channel) {
     m_digitalInput = new DigitalInput(channel);
     m_ownsDutyCycle = true;
@@ -53,6 +54,7 @@
    *
    * @param dutyCycle the duty cycle to attach to
    */
+  @SuppressWarnings("this-escape")
   public DutyCycleEncoder(DutyCycle dutyCycle) {
     m_dutyCycle = dutyCycle;
     init();
@@ -63,6 +65,7 @@
    *
    * @param source the digital source to attach to
    */
+  @SuppressWarnings("this-escape")
   public DutyCycleEncoder(DigitalSource source) {
     m_ownsDutyCycle = true;
     m_dutyCycle = new DutyCycle(source);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
index 2cbb5c7..0aa667e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -29,12 +29,18 @@
  * before use.
  */
 public class Encoder implements CounterBase, Sendable, AutoCloseable {
+  /** Encoder indexing types. */
   public enum IndexingType {
+    /** Reset while the signal is high. */
     kResetWhileHigh(0),
+    /** Reset while the signal is low. */
     kResetWhileLow(1),
+    /** Reset on falling edge of the signal. */
     kResetOnFallingEdge(2),
+    /** Reset on rising edge of the signal. */
     kResetOnRisingEdge(3);
 
+    /** IndexingType value. */
     public final int value;
 
     IndexingType(int value) {
@@ -122,6 +128,7 @@
    *     selected, then a counter object will be used and the returned value will either exactly
    *     match the spec'd count or be double (2x) the spec'd count.
    */
+  @SuppressWarnings("this-escape")
   public Encoder(
       final int channelA,
       final int channelB,
@@ -152,6 +159,7 @@
    * @param reverseDirection represents the orientation of the encoder and inverts the output values
    *     if necessary so forward represents positive values.
    */
+  @SuppressWarnings("this-escape")
   public Encoder(
       final int channelA, final int channelB, final int indexChannel, boolean reverseDirection) {
     this(channelA, channelB, reverseDirection);
@@ -222,6 +230,7 @@
    *     selected then a counter object will be used and the returned value will either exactly
    *     match the spec'd count or be double (2x) the spec'd count.
    */
+  @SuppressWarnings("this-escape")
   public Encoder(
       DigitalSource sourceA,
       DigitalSource sourceB,
@@ -492,7 +501,7 @@
    *
    * @param channel A DIO channel to set as the encoder index
    */
-  public void setIndexSource(int channel) {
+  public final void setIndexSource(int channel) {
     setIndexSource(channel, IndexingType.kResetOnRisingEdge);
   }
 
@@ -502,7 +511,7 @@
    *
    * @param source A digital source to set as the encoder index
    */
-  public void setIndexSource(DigitalSource source) {
+  public final void setIndexSource(DigitalSource source) {
     setIndexSource(source, IndexingType.kResetOnRisingEdge);
   }
 
@@ -513,7 +522,7 @@
    * @param channel A DIO channel to set as the encoder index
    * @param type The state that will cause the encoder to reset
    */
-  public void setIndexSource(int channel, IndexingType type) {
+  public final void setIndexSource(int channel, IndexingType type) {
     if (m_allocatedI) {
       throw new AllocationException("Digital Input for Indexing already allocated");
     }
@@ -530,7 +539,7 @@
    * @param source A digital source to set as the encoder index
    * @param type The state that will cause the encoder to reset
    */
-  public void setIndexSource(DigitalSource source, IndexingType type) {
+  public final void setIndexSource(DigitalSource source, IndexingType type) {
     EncoderJNI.setEncoderIndexSource(
         m_encoder,
         source.getPortHandleForRouting(),
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
index 58e55f0..e112e95 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
@@ -19,32 +19,54 @@
  * the mapping of ports to hardware buttons depends on the code in the Driver Station.
  */
 public class GenericHID {
-  /** Represents a rumble output on the JoyStick. */
+  /** Represents a rumble output on the Joystick. */
   public enum RumbleType {
+    /** Left rumble motor. */
     kLeftRumble,
+    /** Right rumble motor. */
     kRightRumble,
+    /** Both left and right rumble motors. */
     kBothRumble
   }
 
+  /** USB HID interface type. */
   public enum HIDType {
+    /** Unknown. */
     kUnknown(-1),
+    /** XInputUnknown. */
     kXInputUnknown(0),
+    /** XInputGamepad. */
     kXInputGamepad(1),
+    /** XInputWheel. */
     kXInputWheel(2),
+    /** XInputArcadeStick. */
     kXInputArcadeStick(3),
+    /** XInputFlightStick. */
     kXInputFlightStick(4),
+    /** XInputDancePad. */
     kXInputDancePad(5),
+    /** XInputGuitar. */
     kXInputGuitar(6),
+    /** XInputGuitar2. */
     kXInputGuitar2(7),
+    /** XInputDrumKit. */
     kXInputDrumKit(8),
+    /** XInputGuitar3. */
     kXInputGuitar3(11),
+    /** XInputArcadePad. */
     kXInputArcadePad(19),
+    /** HIDJoystick. */
     kHIDJoystick(20),
+    /** HIDGamepad. */
     kHIDGamepad(21),
+    /** HIDDriving. */
     kHIDDriving(22),
+    /** HIDFlight. */
     kHIDFlight(23),
+    /** HID1stPerson. */
     kHID1stPerson(24);
 
+    /** HIDType value. */
     public final int value;
 
     @SuppressWarnings("PMD.UseConcurrentHashMap")
@@ -60,6 +82,12 @@
       }
     }
 
+    /**
+     * Creates an HIDType with the given value.
+     *
+     * @param value HIDType's value.
+     * @return HIDType with the given value.
+     */
     public static HIDType of(int value) {
       return map.get(value);
     }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
index 1c37ecf..1ed131b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
@@ -23,10 +23,14 @@
  * WPILib Known Issues</a> page for details.
  */
 public class I2C implements AutoCloseable {
+  /** I2C connection ports. */
   public enum Port {
+    /** Onboard I2C port. */
     kOnboard(0),
+    /** MXP (roboRIO MXP) I2C port. */
     kMXP(1);
 
+    /** Port value. */
     public final int value;
 
     Port(int value) {
@@ -58,10 +62,20 @@
     HAL.report(tResourceType.kResourceType_I2C, deviceAddress);
   }
 
+  /**
+   * Returns I2C port.
+   *
+   * @return I2C port.
+   */
   public int getPort() {
     return m_port;
   }
 
+  /**
+   * Returns I2C device address.
+   *
+   * @return I2C device address.
+   */
   public int getDeviceAddress() {
     return m_deviceAddress;
   }
@@ -369,7 +383,7 @@
 
     byte[] deviceData = new byte[4];
     for (int i = 0; i < count; i += 4) {
-      int toRead = count - i < 4 ? count - i : 4;
+      int toRead = Math.min(count - i, 4);
       // Read the chunk of data. Return false if the sensor does not
       // respond.
       dataToSend[0] = (byte) (registerAddress + i);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
index 4afece4..1243393 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
@@ -5,6 +5,8 @@
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
@@ -70,7 +72,7 @@
   private final double m_period;
   private final Watchdog m_watchdog;
   private boolean m_ntFlushEnabled = true;
-  private boolean m_lwEnabledInTest = true;
+  private boolean m_lwEnabledInTest;
   private boolean m_calledDsConnected;
 
   /**
@@ -257,16 +259,22 @@
     m_ntFlushEnabled = enabled;
   }
 
+  private boolean m_reportedLw;
+
   /**
    * Sets whether LiveWindow operation is enabled during test mode. Calling
    *
-   * @param testLW True to enable, false to disable. Defaults to true.
+   * @param testLW True to enable, false to disable. Defaults to false.
    * @throws ConcurrentModificationException if this is called during test mode.
    */
   public void enableLiveWindowInTest(boolean testLW) {
     if (isTestEnabled()) {
       throw new ConcurrentModificationException("Can't configure test mode while in test mode!");
     }
+    if (!m_reportedLw && testLW) {
+      HAL.report(tResourceType.kResourceType_SmartDashboard, tInstances.kSmartDashboard_LiveWindow);
+      m_reportedLw = true;
+    }
     m_lwEnabledInTest = testLW;
   }
 
@@ -288,6 +296,7 @@
     return m_period;
   }
 
+  /** Loop function. */
   protected void loopFunc() {
     DriverStation.refreshData();
     m_watchdog.reset();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
index d73b7bb..5536009 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
@@ -17,20 +17,35 @@
  * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
  */
 public class Joystick extends GenericHID {
+  /** Default X axis channel. */
   public static final byte kDefaultXChannel = 0;
+
+  /** Default Y axis channel. */
   public static final byte kDefaultYChannel = 1;
+
+  /** Default Z axis channel. */
   public static final byte kDefaultZChannel = 2;
+
+  /** Default twist axis channel. */
   public static final byte kDefaultTwistChannel = 2;
+
+  /** Default throttle axis channel. */
   public static final byte kDefaultThrottleChannel = 3;
 
   /** Represents an analog axis on a joystick. */
   public enum AxisType {
+    /** X axis. */
     kX(0),
+    /** Y axis. */
     kY(1),
+    /** Z axis. */
     kZ(2),
+    /** Twist axis. */
     kTwist(3),
+    /** Throttle axis. */
     kThrottle(4);
 
+    /** AxisType value. */
     public final int value;
 
     AxisType(int value) {
@@ -40,9 +55,12 @@
 
   /** Represents a digital button on a joystick. */
   public enum ButtonType {
+    /** kTrigger. */
     kTrigger(1),
+    /** kTop. */
     kTop(2);
 
+    /** ButtonType value. */
     public final int value;
 
     ButtonType(int value) {
@@ -184,7 +202,7 @@
    *
    * @return the z position
    */
-  public double getZ() {
+  public final double getZ() {
     return getRawAxis(m_axes[AxisType.kZ.value]);
   }
 
@@ -194,7 +212,7 @@
    *
    * @return The Twist value of the joystick.
    */
-  public double getTwist() {
+  public final double getTwist() {
     return getRawAxis(m_axes[AxisType.kTwist.value]);
   }
 
@@ -204,7 +222,7 @@
    *
    * @return The Throttle value of the joystick.
    */
-  public double getThrottle() {
+  public final double getThrottle() {
     return getRawAxis(m_axes[AxisType.kThrottle.value]);
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
index c1bf04b..dc969a9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
@@ -62,11 +62,12 @@
   }
 
   /** MotorSafety constructor. */
+  @SuppressWarnings("this-escape")
   public MotorSafety() {
     synchronized (m_listMutex) {
       m_instanceList.add(this);
       if (m_safetyThread == null) {
-        m_safetyThread = new Thread(() -> threadMain(), "MotorSafety Thread");
+        m_safetyThread = new Thread(MotorSafety::threadMain, "MotorSafety Thread");
         m_safetyThread.setDaemon(true);
         m_safetyThread.start();
       }
@@ -185,7 +186,13 @@
     }
   }
 
+  /** Called to stop the motor when the timeout expires. */
   public abstract void stopMotor();
 
+  /**
+   * Returns a description to print when an error occurs.
+   *
+   * @return Description to print when an error occurs.
+   */
   public abstract String getDescription();
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
index 7ed6162..8f381a0 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
@@ -113,7 +113,7 @@
                     updateAlarm();
                   } else {
                     // Need to update the alarm to cause it to wait again
-                    updateAlarm((long) -1);
+                    updateAlarm(-1);
                   }
                 } finally {
                   m_processLock.unlock();
@@ -134,7 +134,7 @@
             error = cause;
           }
           DriverStation.reportError(
-              "Unhandled exception in Notifier thread: " + error.toString(), error.getStackTrace());
+              "Unhandled exception in Notifier thread: " + error, error.getStackTrace());
           DriverStation.reportError(
               "The Runnable for this Notifier (or methods called by it) should have handled "
                   + "the exception above.\n"
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS4Controller.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS4Controller.java
index 2359b7b..e000c6f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS4Controller.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS4Controller.java
@@ -4,6 +4,8 @@
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.event.BooleanEvent;
 import edu.wpi.first.wpilibj.event.EventLoop;
 
@@ -13,6 +15,10 @@
  * <p>This class handles PS4 input that comes from the Driver Station. Each time a value is
  * requested the most recent value is returned. There is a single class instance for each controller
  * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ *
+ * <p>Only first party controllers from Sony are guaranteed to have the correct mapping, and only
+ * through the official NI DS. Sim is not guaranteed to have the same mapping, as well as any 3rd
+ * party controllers.
  */
 public class PS4Controller extends GenericHID {
   /**
@@ -23,27 +29,41 @@
   public PS4Controller(int port) {
     super(port);
 
-    // re-enable when PS4Controller is added to Usage Reporting
-    // HAL.report(tResourceType.kResourceType_PS4Controller, port + 1); /
+    HAL.report(tResourceType.kResourceType_PS4Controller, port + 1);
   }
 
   /** Represents a digital button on a PS4Controller. */
   public enum Button {
+    /** Square button. */
     kSquare(1),
+    /** X button. */
     kCross(2),
+    /** Circle button. */
     kCircle(3),
+    /** Triangle button. */
     kTriangle(4),
+    /** Left Trigger 1 button. */
     kL1(5),
+    /** Right Trigger 1 button. */
     kR1(6),
+    /** Left Trigger 2 button. */
     kL2(7),
+    /** Right Trigger 2 button. */
     kR2(8),
+    /** Share button. */
     kShare(9),
+    /** Option button. */
     kOptions(10),
+    /** Left stick button. */
     kL3(11),
+    /** Right stick button. */
     kR3(12),
+    /** PlayStation button. */
     kPS(13),
+    /** Touchpad click button. */
     kTouchpad(14);
 
+    /** Button value. */
     public final int value;
 
     Button(int index) {
@@ -70,13 +90,20 @@
 
   /** Represents an axis on a PS4Controller. */
   public enum Axis {
+    /** Left X axis. */
     kLeftX(0),
+    /** Left Y axis. */
     kLeftY(1),
+    /** Right X axis. */
     kRightX(2),
+    /** Right Y axis. */
     kRightY(5),
+    /** Left Trigger 2. */
     kL2(3),
+    /** Right Trigger 2. */
     kR2(4);
 
+    /** Axis value. */
     public final int value;
 
     Axis(int index) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS5Controller.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS5Controller.java
index 8af98a5..18cb738 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS5Controller.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS5Controller.java
@@ -13,6 +13,10 @@
  * <p>This class handles PS5 input that comes from the Driver Station. Each time a value is
  * requested the most recent value is returned. There is a single class instance for each controller
  * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ *
+ * <p>Only first party controllers from Sony are guaranteed to have the correct mapping, and only
+ * through the official NI DS. Sim is not guaranteed to have the same mapping, as well as any 3rd
+ * party controllers.
  */
 public class PS5Controller extends GenericHID {
   /**
@@ -27,21 +31,36 @@
 
   /** Represents a digital button on a PS5Controller. */
   public enum Button {
-    kCross(1),
-    kCircle(2),
-    kSquare(3),
+    /** Square button. */
+    kSquare(1),
+    /** X button. */
+    kCross(2),
+    /** Circle button. */
+    kCircle(3),
+    /** Triangle button. */
     kTriangle(4),
+    /** Left trigger 1 button. */
     kL1(5),
+    /** Right trigger 1 button. */
     kR1(6),
+    /** Left trigger 2 button. */
     kL2(7),
+    /** Right trigger 2 button. */
     kR2(8),
+    /** Create button. */
     kCreate(9),
+    /** Options button. */
     kOptions(10),
-    kPS(11),
-    kL3(12),
-    kR3(13),
+    /** Left stick button. */
+    kL3(11),
+    /** Right stick button. */
+    kR3(12),
+    /** PlayStation button. */
+    kPS(13),
+    /** Touchpad click button. */
     kTouchpad(14);
 
+    /** Button value. */
     public final int value;
 
     Button(int index) {
@@ -68,13 +87,20 @@
 
   /** Represents an axis on a PS5Controller. */
   public enum Axis {
+    /** Left X axis. */
     kLeftX(0),
+    /** Left Y axis. */
     kLeftY(1),
-    kL2(2),
-    kRightX(3),
-    kRightY(4),
-    kR2(5);
+    /** Right X axis. */
+    kRightX(2),
+    /** Right Y axis. */
+    kRightY(5),
+    /** Left Trigger 2. */
+    kL2(3),
+    /** Right Trigger 2. */
+    kR2(4);
 
+    /** Axis value. */
     public final int value;
 
     Axis(int index) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
index b0b3810..af600ec 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
@@ -54,6 +54,7 @@
    * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
    * @param registerSendable If true, adds this instance to SendableRegistry and LiveWindow
    */
+  @SuppressWarnings("this-escape")
   public PWM(final int channel, final boolean registerSendable) {
     SensorUtil.checkPWMChannel(channel);
     m_channel = channel;
@@ -200,7 +201,7 @@
   }
 
   /** Temporarily disables the PWM output. The next set call will re-enable the output. */
-  public void setDisabled() {
+  public final void setDisabled() {
     PWMJNI.setPWMDisabled(m_handle);
   }
 
@@ -228,6 +229,7 @@
     }
   }
 
+  /** Latches PWM to zero. */
   public void setZeroLatch() {
     PWMJNI.latchPWMZero(m_handle);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java
index b7df47f..32d2ec5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java
@@ -59,8 +59,7 @@
             output.write(("currentVersion=" + fwVersion).getBytes(StandardCharsets.UTF_8));
           }
         } catch (IOException ex) {
-          DriverStation.reportError(
-              "Could not write " + fileName + ": " + ex.toString(), ex.getStackTrace());
+          DriverStation.reportError("Could not write " + fileName + ": " + ex, ex.getStackTrace());
         }
       }
 
@@ -115,14 +114,12 @@
 
   /** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
   private static double voltsToPsi(double sensorVoltage, double supplyVoltage) {
-    double pressure = 250 * (sensorVoltage / supplyVoltage) - 25;
-    return pressure;
+    return 250 * (sensorVoltage / supplyVoltage) - 25;
   }
 
   /** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
   private static double psiToVolts(double pressure, double supplyVoltage) {
-    double voltage = supplyVoltage * (0.004 * pressure + 0.1);
-    return voltage;
+    return supplyVoltage * (0.004 * pressure + 0.1);
   }
 
   private final DataStore m_dataStore;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java
index d9b493b..90f49f5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj;
 
+/** Interface for pneumatics devices. */
 public interface PneumaticsBase extends AutoCloseable {
   /**
    * For internal use to get a module for a specific type.
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java
index aac16a3..4f1d70c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java
@@ -186,10 +186,23 @@
     return CTREPCMJNI.getSolenoidDisabledList(m_handle);
   }
 
+  /**
+   * Returns whether the solenoid is currently reporting a voltage fault.
+   *
+   * @return True if solenoid is reporting a fault, otherwise false.
+   * @see #getSolenoidVoltageStickyFault()
+   */
   public boolean getSolenoidVoltageFault() {
     return CTREPCMJNI.getSolenoidVoltageFault(m_handle);
   }
 
+  /**
+   * Returns whether the solenoid has reported a voltage fault since sticky faults were last
+   * cleared. This fault is persistent and can be cleared by ClearAllStickyFaults()
+   *
+   * @return True if solenoid is reporting a fault, otherwise false.
+   * @see #getSolenoidVoltageFault()
+   */
   public boolean getSolenoidVoltageStickyFault() {
     return CTREPCMJNI.getSolenoidVoltageStickyFault(m_handle);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java
index a7951e8..e1525c3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java
@@ -4,7 +4,10 @@
 
 package edu.wpi.first.wpilibj;
 
+/** Pneumatics module type. */
 public enum PneumaticsModuleType {
+  /** CTRE PCM. */
   CTREPCM,
-  REVPH;
+  /** REV PH. */
+  REVPH
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java
index a0c2104..57709b7 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java
@@ -22,12 +22,17 @@
   private final int m_handle;
   private final int m_module;
 
+  /** Default module number. */
   public static final int kDefaultModule = PowerDistributionJNI.DEFAULT_MODULE;
 
+  /** Power distribution module type. */
   public enum ModuleType {
+    /** CTRE (Cross The Road Electronics) CTRE Power Distribution Panel (PDP). */
     kCTRE(PowerDistributionJNI.CTRE_TYPE),
+    /** REV Power Distribution Hub (PDH). */
     kRev(PowerDistributionJNI.REV_TYPE);
 
+    /** ModuleType value. */
     public final int value;
 
     ModuleType(int value) {
@@ -41,6 +46,7 @@
    * @param module The CAN ID of the PDP/PDH.
    * @param moduleType Module type (CTRE or REV).
    */
+  @SuppressWarnings("this-escape")
   public PowerDistribution(int module, ModuleType moduleType) {
     m_handle = PowerDistributionJNI.initialize(module, moduleType.value);
     m_module = PowerDistributionJNI.getModuleNumber(m_handle);
@@ -54,6 +60,7 @@
    *
    * <p>Detects the connected PDP/PDH using the default CAN ID (0 for CTRE and 1 for REV).
    */
+  @SuppressWarnings("this-escape")
   public PowerDistribution() {
     m_handle = PowerDistributionJNI.initialize(kDefaultModule, PowerDistributionJNI.AUTOMATIC_TYPE);
     m_module = PowerDistributionJNI.getModuleNumber(m_handle);
@@ -101,9 +108,7 @@
    * @return The current of the channel in Amperes
    */
   public double getCurrent(int channel) {
-    double current = PowerDistributionJNI.getChannelCurrent(m_handle, channel);
-
-    return current;
+    return PowerDistributionJNI.getChannelCurrent(m_handle, channel);
   }
 
   /**
@@ -184,14 +189,29 @@
     PowerDistributionJNI.setSwitchableChannel(m_handle, enabled);
   }
 
+  /**
+   * Returns the power distribution version number.
+   *
+   * @return The power distribution version number.
+   */
   public PowerDistributionVersion getVersion() {
     return PowerDistributionJNI.getVersion(m_handle);
   }
 
+  /**
+   * Returns the power distribution faults.
+   *
+   * @return The power distribution faults.
+   */
   public PowerDistributionFaults getFaults() {
     return PowerDistributionJNI.getFaults(m_handle);
   }
 
+  /**
+   * Returns the power distribution sticky faults.
+   *
+   * @return The power distribution sticky faults.
+   */
   public PowerDistributionStickyFaults getStickyFaults() {
     return PowerDistributionJNI.getStickyFaults(m_handle);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
index 087c156..69d0fb3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
@@ -230,7 +230,7 @@
    */
   public static void setLong(String key, long value) {
     NetworkTableEntry entry = m_table.getEntry(key);
-    entry.setDouble(value);
+    entry.setInteger(value);
     entry.setPersistent();
   }
 
@@ -242,7 +242,7 @@
    */
   public static void initLong(String key, long value) {
     NetworkTableEntry entry = m_table.getEntry(key);
-    entry.setDefaultDouble(value);
+    entry.setDefaultInteger(value);
     entry.setPersistent();
   }
 
@@ -345,6 +345,6 @@
    * @return either the value in the table, or the backup
    */
   public static long getLong(String key, long backup) {
-    return (long) m_table.getEntry(key).getDouble(backup);
+    return m_table.getEntry(key).getInteger(backup);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
index a2a58d3..dce3096 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
@@ -44,9 +44,13 @@
 
   /** The state to drive a Relay to. */
   public enum Value {
+    /** Off. */
     kOff("Off"),
+    /** On. */
     kOn("On"),
+    /** Forward. */
     kForward("Forward"),
+    /** Reverse. */
     kReverse("Reverse");
 
     private final String m_prettyValue;
@@ -55,10 +59,21 @@
       m_prettyValue = prettyValue;
     }
 
+    /**
+     * Returns the pretty string representation of the value.
+     *
+     * @return The pretty string representation of the value.
+     */
     public String getPrettyValue() {
       return m_prettyValue;
     }
 
+    /**
+     * Returns the value for a given pretty string.
+     *
+     * @param value The pretty string.
+     * @return The value or an empty optional if there is no corresponding value.
+     */
     public static Optional<Value> getValueOf(String value) {
       return Arrays.stream(Value.values()).filter(v -> v.m_prettyValue.equals(value)).findFirst();
     }
@@ -66,11 +81,11 @@
 
   /** The Direction(s) that a relay is configured to operate in. */
   public enum Direction {
-    /** direction: both directions are valid. */
+    /** Both directions are valid. */
     kBoth,
-    /** direction: Only forward is valid. */
+    /** Only forward is valid. */
     kForward,
-    /** direction: only reverse is valid. */
+    /** Only reverse is valid. */
     kReverse
   }
 
@@ -110,6 +125,7 @@
    * @param channel The channel number for this relay (0 - 3).
    * @param direction The direction that the Relay object will control.
    */
+  @SuppressWarnings("this-escape")
   public Relay(final int channel, Direction direction) {
     m_channel = channel;
     m_direction = requireNonNullParam(direction, "direction", "Relay");
@@ -279,6 +295,7 @@
    *
    * @param direction The direction for the relay to operate in
    */
+  @SuppressWarnings("this-escape")
   public void setDirection(Direction direction) {
     requireNonNullParam(direction, "direction", "setDirection");
     if (m_direction == direction) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
index 2f59a3d..855b118 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -178,6 +178,11 @@
     Shuffleboard.disableActuatorWidgets();
   }
 
+  /**
+   * Returns the main thread ID.
+   *
+   * @return The main thread ID.
+   */
   public static long getMainThreadId() {
     return m_threadId;
   }
@@ -321,8 +326,7 @@
         robotName = elements[0].getClassName();
       }
       DriverStation.reportError(
-          "Unhandled exception instantiating robot " + robotName + " " + throwable.toString(),
-          elements);
+          "Unhandled exception instantiating robot " + robotName + " " + throwable, elements);
       DriverStation.reportError(
           "The robot program quit unexpectedly."
               + " This is usually due to a code error.\n"
@@ -353,8 +357,7 @@
           output.write(WPILibVersion.Version.getBytes(StandardCharsets.UTF_8));
         }
       } catch (IOException ex) {
-        DriverStation.reportError(
-            "Could not write FRC_Lib_Version.ini: " + ex.toString(), ex.getStackTrace());
+        DriverStation.reportError("Could not write FRC_Lib_Version.ini: " + ex, ex.getStackTrace());
       }
     }
 
@@ -366,8 +369,7 @@
       if (cause != null) {
         throwable = cause;
       }
-      DriverStation.reportError(
-          "Unhandled exception: " + throwable.toString(), throwable.getStackTrace());
+      DriverStation.reportError("Unhandled exception: " + throwable, throwable.getStackTrace());
       errorOnExit = true;
     } finally {
       m_runMutex.lock();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
index 29bcad8..d8faafa 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.hal.LEDJNI;
 import edu.wpi.first.hal.PowerJNI;
 import edu.wpi.first.hal.can.CANJNI;
 import edu.wpi.first.hal.can.CANStatus;
@@ -33,7 +34,7 @@
    * @return FPGA Revision number.
    */
   public static long getFPGARevision() {
-    return (long) HALUtil.getFPGARevision();
+    return HALUtil.getFPGARevision();
   }
 
   /**
@@ -78,6 +79,10 @@
   /**
    * Get the state of the "USER" button on the roboRIO.
    *
+   * <p>Warning: the User Button is used to stop user programs from automatically loading if it is
+   * held for more then 5 seconds. Because of this, it's not recommended to be used by teams for any
+   * other purpose.
+   *
    * @return true if the button is currently pressed down
    */
   public static boolean getUserButton() {
@@ -315,6 +320,66 @@
     return PowerJNI.getCPUTemp();
   }
 
+  /** State for the radio led. */
+  public enum RadioLEDState {
+    /** Off. */
+    kOff(LEDJNI.RADIO_LED_STATE_OFF),
+    /** Green. */
+    kGreen(LEDJNI.RADIO_LED_STATE_GREEN),
+    /** Red. */
+    kRed(LEDJNI.RADIO_LED_STATE_RED),
+    /** Orange. */
+    kOrange(LEDJNI.RADIO_LED_STATE_ORANGE);
+
+    /** The native value for this state. */
+    public final int value;
+
+    RadioLEDState(int value) {
+      this.value = value;
+    }
+
+    /**
+     * Gets a state from an int value.
+     *
+     * @param value int value
+     * @return state
+     */
+    public static RadioLEDState fromValue(int value) {
+      switch (value) {
+        case LEDJNI.RADIO_LED_STATE_OFF:
+          return RadioLEDState.kOff;
+        case LEDJNI.RADIO_LED_STATE_GREEN:
+          return RadioLEDState.kGreen;
+        case LEDJNI.RADIO_LED_STATE_RED:
+          return RadioLEDState.kRed;
+        case LEDJNI.RADIO_LED_STATE_ORANGE:
+          return RadioLEDState.kOrange;
+        default:
+          return RadioLEDState.kOff;
+      }
+    }
+  }
+
+  /**
+   * Set the state of the "Radio" LED. On the RoboRIO, this writes to sysfs, so this function should
+   * not be called multiple times per loop cycle to avoid overruns.
+   *
+   * @param state The state to set the LED to.
+   */
+  public static void setRadioLEDState(RadioLEDState state) {
+    LEDJNI.setRadioLEDState(state.value);
+  }
+
+  /**
+   * Get the state of the "Radio" LED. On the RoboRIO, this reads from sysfs, so this function
+   * should not be called multiple times per loop cycle to avoid overruns.
+   *
+   * @return The state of the LED.
+   */
+  public static RadioLEDState getRadioLEDState() {
+    return RadioLEDState.fromValue(LEDJNI.getRadioLEDState());
+  }
+
   /**
    * Get the current status of the CAN bus.
    *
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
index 44198df..405dfc4 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj;
 
+/** Robot state utility functions. */
 public final class RobotState {
   /**
    * Returns true if the robot is disabled.
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RuntimeType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RuntimeType.java
index e7af118..cad1c51 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RuntimeType.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RuntimeType.java
@@ -6,11 +6,16 @@
 
 import edu.wpi.first.hal.HALUtil;
 
+/** Runtime type. */
 public enum RuntimeType {
+  /** roboRIO 1.0. */
   kRoboRIO(HALUtil.RUNTIME_ROBORIO),
+  /** roboRIO 2.0. */
   kRoboRIO2(HALUtil.RUNTIME_ROBORIO2),
+  /** Simulation runtime. */
   kSimulation(HALUtil.RUNTIME_SIMULATION);
 
+  /** RuntimeType value. */
   public final int value;
 
   RuntimeType(int value) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
index 632770c..1a3bb34 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -14,13 +14,20 @@
 
 /** Represents an SPI bus port. */
 public class SPI implements AutoCloseable {
+  /** SPI port. */
   public enum Port {
+    /** Onboard SPI bus port CS0. */
     kOnboardCS0(SPIJNI.ONBOARD_CS0_PORT),
+    /** Onboard SPI bus port CS1. */
     kOnboardCS1(SPIJNI.ONBOARD_CS1_PORT),
+    /** Onboard SPI bus port CS2. */
     kOnboardCS2(SPIJNI.ONBOARD_CS2_PORT),
+    /** Onboard SPI bus port CS3. */
     kOnboardCS3(SPIJNI.ONBOARD_CS3_PORT),
+    /** MXP (roboRIO MXP) SPI bus port. */
     kMXP(SPIJNI.MXP_PORT);
 
+    /** SPI port value. */
     public final int value;
 
     Port(int value) {
@@ -28,6 +35,7 @@
     }
   }
 
+  /** SPI mode. */
   public enum Mode {
     /** Clock idle low, data sampled on rising edge. */
     kMode0(SPIJNI.SPI_MODE0),
@@ -38,6 +46,7 @@
     /** Clock idle high, data sampled on rising edge. */
     kMode3(SPIJNI.SPI_MODE3);
 
+    /** SPI mode value. */
     public final int value;
 
     Mode(int value) {
@@ -64,6 +73,11 @@
     HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
   }
 
+  /**
+   * Returns the SPI port value.
+   *
+   * @return SPI port value.
+   */
   public int getPort() {
     return m_port;
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
index bcedd81..258fbc7 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
@@ -47,8 +47,10 @@
   /** Number of PCM Modules. */
   public static final int kCTREPCMModules = PortsJNI.getNumCTREPCMModules();
 
+  /** Number of power distribution channels per PH. */
   public static final int kREVPHChannels = PortsJNI.getNumREVPHChannels();
 
+  /** Number of PH modules. */
   public static final int kREVPHModules = PortsJNI.getNumREVPHModules();
 
   /**
@@ -59,12 +61,12 @@
    */
   public static void checkDigitalChannel(final int channel) {
     if (!DIOJNI.checkDIOChannel(channel)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested DIO channel is out of range. Minimum: 0, Maximum: ")
-          .append(kDigitalChannels)
-          .append(", Requested: ")
-          .append(channel);
-      throw new IllegalArgumentException(buf.toString());
+      String buf =
+          "Requested DIO channel is out of range. Minimum: 0, Maximum: "
+              + kDigitalChannels
+              + ", Requested: "
+              + channel;
+      throw new IllegalArgumentException(buf);
     }
   }
 
@@ -76,12 +78,12 @@
    */
   public static void checkRelayChannel(final int channel) {
     if (!RelayJNI.checkRelayChannel(channel)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested relay channel is out of range. Minimum: 0, Maximum: ")
-          .append(kRelayChannels)
-          .append(", Requested: ")
-          .append(channel);
-      throw new IllegalArgumentException(buf.toString());
+      String buf =
+          "Requested relay channel is out of range. Minimum: 0, Maximum: "
+              + kRelayChannels
+              + ", Requested: "
+              + channel;
+      throw new IllegalArgumentException(buf);
     }
   }
 
@@ -93,12 +95,12 @@
    */
   public static void checkPWMChannel(final int channel) {
     if (!PWMJNI.checkPWMChannel(channel)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested PWM channel is out of range. Minimum: 0, Maximum: ")
-          .append(kPwmChannels)
-          .append(", Requested: ")
-          .append(channel);
-      throw new IllegalArgumentException(buf.toString());
+      String buf =
+          "Requested PWM channel is out of range. Minimum: 0, Maximum: "
+              + kPwmChannels
+              + ", Requested: "
+              + channel;
+      throw new IllegalArgumentException(buf);
     }
   }
 
@@ -110,12 +112,12 @@
    */
   public static void checkAnalogInputChannel(final int channel) {
     if (!AnalogJNI.checkAnalogInputChannel(channel)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested analog input channel is out of range. Minimum: 0, Maximum: ")
-          .append(kAnalogInputChannels)
-          .append(", Requested: ")
-          .append(channel);
-      throw new IllegalArgumentException(buf.toString());
+      String buf =
+          "Requested analog input channel is out of range. Minimum: 0, Maximum: "
+              + kAnalogInputChannels
+              + ", Requested: "
+              + channel;
+      throw new IllegalArgumentException(buf);
     }
   }
 
@@ -127,12 +129,12 @@
    */
   public static void checkAnalogOutputChannel(final int channel) {
     if (!AnalogJNI.checkAnalogOutputChannel(channel)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested analog output channel is out of range. Minimum: 0, Maximum: ")
-          .append(kAnalogOutputChannels)
-          .append(", Requested: ")
-          .append(channel);
-      throw new IllegalArgumentException(buf.toString());
+      String buf =
+          "Requested analog output channel is out of range. Minimum: 0, Maximum: "
+              + kAnalogOutputChannels
+              + ", Requested: "
+              + channel;
+      throw new IllegalArgumentException(buf);
     }
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
index 1a05c8a..d978d78 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
@@ -13,13 +13,20 @@
 public class SerialPort implements AutoCloseable {
   private int m_portHandle;
 
+  /** Serial port. */
   public enum Port {
+    /** Onboard serial port on the roboRIO. */
     kOnboard(0),
+    /** MXP (roboRIO MXP) serial port. */
     kMXP(1),
+    /** USB serial port (same as KUSB1). */
     kUSB(2),
+    /** USB serial port 1. */
     kUSB1(2),
+    /** USB serial port 2. */
     kUSB2(3);
 
+    /** Port value. */
     public final int value;
 
     Port(int value) {
@@ -29,12 +36,18 @@
 
   /** Represents the parity to use for serial communications. */
   public enum Parity {
+    /** No parity. */
     kNone(0),
+    /** Odd parity. */
     kOdd(1),
+    /** Even parity. */
     kEven(2),
+    /** Parity bit always on. */
     kMark(3),
+    /** Parity bit always off. */
     kSpace(4);
 
+    /** Parity value. */
     public final int value;
 
     Parity(int value) {
@@ -44,10 +57,14 @@
 
   /** Represents the number of stop bits to use for Serial Communication. */
   public enum StopBits {
+    /** One stop bit. */
     kOne(10),
+    /** One and a half stop bits. */
     kOnePointFive(15),
+    /** Two stop bits. */
     kTwo(20);
 
+    /** StopBits value. */
     public final int value;
 
     StopBits(int value) {
@@ -57,11 +74,16 @@
 
   /** Represents what type of flow control to use for serial communication. */
   public enum FlowControl {
+    /** No flow control. */
     kNone(0),
+    /** XON/XOFF flow control. */
     kXonXoff(1),
+    /** RTS/CTS flow control. */
     kRtsCts(2),
+    /** DTS/DSR flow control. */
     kDtsDsr(4);
 
+    /** FlowControl value. */
     public final int value;
 
     FlowControl(int value) {
@@ -71,9 +93,12 @@
 
   /** Represents which type of buffer mode to use when writing to a serial port. */
   public enum WriteBufferMode {
+    /** Flush the buffer on each access. */
     kFlushOnAccess(1),
+    /** Flush the buffer when it is full. */
     kFlushWhenFull(2);
 
+    /** WriteBufferMode value. */
     public final int value;
 
     WriteBufferMode(int value) {
@@ -186,7 +211,7 @@
   }
 
   /** Disable termination behavior. */
-  public void disableTermination() {
+  public final void disableTermination() {
     SerialPortJNI.serialDisableTermination(m_portHandle);
   }
 
@@ -216,7 +241,7 @@
    */
   public String readString(int count) {
     byte[] out = read(count);
-    return new String(out, 0, out.length, StandardCharsets.US_ASCII);
+    return new String(out, StandardCharsets.US_ASCII);
   }
 
   /**
@@ -268,7 +293,7 @@
    *
    * @param timeout The number of seconds to wait for I/O.
    */
-  public void setTimeout(double timeout) {
+  public final void setTimeout(double timeout) {
     SerialPortJNI.serialSetTimeout(m_portHandle, timeout);
   }
 
@@ -283,7 +308,7 @@
    *
    * @param size The read buffer size.
    */
-  public void setReadBufferSize(int size) {
+  public final void setReadBufferSize(int size) {
     SerialPortJNI.serialSetReadBufferSize(m_portHandle, size);
   }
 
@@ -309,7 +334,7 @@
    *
    * @param mode The write buffer mode.
    */
-  public void setWriteBufferMode(WriteBufferMode mode) {
+  public final void setWriteBufferMode(WriteBufferMode mode) {
     SerialPortJNI.serialSetWriteMode(m_portHandle, (byte) mode.value);
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
index 1f1c747..ebd2a8b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
@@ -19,18 +19,19 @@
   private static final double kMaxServoAngle = 180.0;
   private static final double kMinServoAngle = 0.0;
 
-  protected static final int kDefaultMaxServoPWM = 2400;
-  protected static final int kDefaultMinServoPWM = 600;
+  private static final int kDefaultMaxServoPWM = 2400;
+  private static final int kDefaultMinServoPWM = 600;
 
   /**
-   * Constructor.<br>
+   * Constructor.
    *
-   * <p>By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br>
-   * By default {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
+   * <p>By default, {@value #kDefaultMaxServoPWM} ms is used as the max PWM value and {@value
+   * #kDefaultMinServoPWM} ms is used as the minPWM value.
    *
    * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public Servo(final int channel) {
     super(channel);
     setBoundsMicroseconds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
index be87444..9c516da 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -39,6 +39,7 @@
    * @param moduleType The module type to use.
    * @param channel The channel the solenoid is on.
    */
+  @SuppressWarnings("this-escape")
   public Solenoid(final int module, final PneumaticsModuleType moduleType, final int channel) {
     m_module = PneumaticsBase.getForType(module, moduleType);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/StadiaController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/StadiaController.java
new file mode 100644
index 0000000..d092d8b
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/StadiaController.java
@@ -0,0 +1,737 @@
+// 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.
+
+package edu.wpi.first.wpilibj;
+
+// import edu.wpi.first.hal.FRCNetComm.tResourceType;
+// import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.event.BooleanEvent;
+import edu.wpi.first.wpilibj.event.EventLoop;
+
+/**
+ * Handle input from Stadia controllers connected to the Driver Station.
+ *
+ * <p>This class handles Stadia input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each controller
+ * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class StadiaController extends GenericHID {
+  /** Represents a digital button on a StadiaController. */
+  public enum Button {
+    /** A button. */
+    kA(1),
+    /** B button. */
+    kB(2),
+    /** X Button. */
+    kX(3),
+    /** Y Button. */
+    kY(4),
+    /** Left bumper button. */
+    kLeftBumper(5),
+    /** Right bumper button. */
+    kRightBumper(6),
+    /** Left stick button. */
+    kLeftStick(7),
+    /** Right stick button. */
+    kRightStick(8),
+    /** Ellipses button. */
+    kEllipses(9),
+    /** Hamburger button. */
+    kHamburger(10),
+    /** Stadia button. */
+    kStadia(11),
+    /** Right trigger button. */
+    kRightTrigger(12),
+    /** Left trigger button. */
+    kLeftTrigger(13),
+    /** Google button. */
+    kGoogle(14),
+    /** Frame button. */
+    kFrame(15);
+
+    /** Button value. */
+    public final int value;
+
+    Button(int value) {
+      this.value = value;
+    }
+
+    /**
+     * Get the human-friendly name of the button, matching the relevant methods. This is done by
+     * stripping the leading `k`, and if not a Bumper button append `Button`.
+     *
+     * <p>Primarily used for automated unit tests.
+     *
+     * @return the human-friendly name of the button.
+     */
+    @Override
+    public String toString() {
+      var name = this.name().substring(1); // Remove leading `k`
+      if (name.endsWith("Bumper")) {
+        return name;
+      }
+      return name + "Button";
+    }
+  }
+
+  /** Represents an axis on a StadiaController. */
+  public enum Axis {
+    /** Left X axis. */
+    kLeftX(0),
+    /** Right X axis. */
+    kRightX(3),
+    /** Left Y axis. */
+    kLeftY(1),
+    /** Right Y axis. */
+    kRightY(4);
+
+    /** Axis value. */
+    public final int value;
+
+    Axis(int value) {
+      this.value = value;
+    }
+
+    /**
+     * Get the human-friendly name of the axis, matching the relevant methods. This is done by
+     * stripping the leading `k`, and if a trigger axis append `Axis`.
+     *
+     * <p>Primarily used for automated unit tests.
+     *
+     * @return the human-friendly name of the axis.
+     */
+    @Override
+    public String toString() {
+      var name = this.name().substring(1); // Remove leading `k`
+      if (name.endsWith("Trigger")) {
+        return name + "Axis";
+      }
+      return name;
+    }
+  }
+
+  /**
+   * Construct an instance of a controller.
+   *
+   * @param port The port index on the Driver Station that the controller is plugged into.
+   */
+  public StadiaController(final int port) {
+    super(port);
+    // re-enable when StadiaController is added to Usage Reporting
+    // HAL.report(tResourceType.kResourceType_Joystick, port + 1);
+  }
+
+  /**
+   * Get the X axis value of left side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getLeftX() {
+    return getRawAxis(Axis.kLeftX.value);
+  }
+
+  /**
+   * Get the X axis value of right side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getRightX() {
+    return getRawAxis(Axis.kRightX.value);
+  }
+
+  /**
+   * Get the Y axis value of left side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getLeftY() {
+    return getRawAxis(Axis.kLeftY.value);
+  }
+
+  /**
+   * Get the Y axis value of right side of the controller.
+   *
+   * @return The axis value.
+   */
+  public double getRightY() {
+    return getRawAxis(Axis.kRightY.value);
+  }
+
+  /**
+   * Read the value of the left bumper (LB) button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getLeftBumper() {
+    return getRawButton(Button.kLeftBumper.value);
+  }
+
+  /**
+   * Read the value of the right bumper (RB) button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getRightBumper() {
+    return getRawButton(Button.kRightBumper.value);
+  }
+
+  /**
+   * Whether the left bumper (LB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getLeftBumperPressed() {
+    return getRawButtonPressed(Button.kLeftBumper.value);
+  }
+
+  /**
+   * Whether the right bumper (RB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getRightBumperPressed() {
+    return getRawButtonPressed(Button.kRightBumper.value);
+  }
+
+  /**
+   * Whether the left bumper (LB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getLeftBumperReleased() {
+    return getRawButtonReleased(Button.kLeftBumper.value);
+  }
+
+  /**
+   * Whether the right bumper (RB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getRightBumperReleased() {
+    return getRawButtonReleased(Button.kRightBumper.value);
+  }
+
+  /**
+   * Constructs an event instance around the right bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right bumper's digital signal attached to the given
+   *     loop.
+   */
+  public BooleanEvent leftBumper(EventLoop loop) {
+    return new BooleanEvent(loop, this::getLeftBumper);
+  }
+
+  /**
+   * Constructs an event instance around the left bumper's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left bumper's digital signal attached to the given
+   *     loop.
+   */
+  public BooleanEvent rightBumper(EventLoop loop) {
+    return new BooleanEvent(loop, this::getRightBumper);
+  }
+
+  /**
+   * Read the value of the left stick button (LSB) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getLeftStickButton() {
+    return getRawButton(Button.kLeftStick.value);
+  }
+
+  /**
+   * Read the value of the right stick button (RSB) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getRightStickButton() {
+    return getRawButton(Button.kRightStick.value);
+  }
+
+  /**
+   * Whether the left stick button (LSB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getLeftStickButtonPressed() {
+    return getRawButtonPressed(Button.kLeftStick.value);
+  }
+
+  /**
+   * Whether the right stick button (RSB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getRightStickButtonPressed() {
+    return getRawButtonPressed(Button.kRightStick.value);
+  }
+
+  /**
+   * Whether the left stick button (LSB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getLeftStickButtonReleased() {
+    return getRawButtonReleased(Button.kLeftStick.value);
+  }
+
+  /**
+   * Whether the right stick (RSB) button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getRightStickButtonReleased() {
+    return getRawButtonReleased(Button.kRightStick.value);
+  }
+
+  /**
+   * Constructs an event instance around the left stick button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left stick button's digital signal attached to the
+   *     given loop.
+   */
+  public BooleanEvent leftStick(EventLoop loop) {
+    return new BooleanEvent(loop, this::getLeftStickButton);
+  }
+
+  /**
+   * Constructs an event instance around the right stick button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right stick button's digital signal attached to the
+   *     given loop.
+   */
+  public BooleanEvent rightStick(EventLoop loop) {
+    return new BooleanEvent(loop, this::getRightStickButton);
+  }
+
+  /**
+   * Read the value of the left trigger button (LTB) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getLeftTriggerButton() {
+    return getRawButton(Button.kLeftTrigger.value);
+  }
+
+  /**
+   * Read the value of the right trigger button (RTB) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getRightTriggerButton() {
+    return getRawButton(Button.kRightTrigger.value);
+  }
+
+  /**
+   * Whether the left trigger button (LTB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getLeftTriggerButtonPressed() {
+    return getRawButtonPressed(Button.kLeftTrigger.value);
+  }
+
+  /**
+   * Whether the right trigger button (RTB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getRightTriggerButtonPressed() {
+    return getRawButtonPressed(Button.kRightTrigger.value);
+  }
+
+  /**
+   * Whether the left trigger button (LTB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getLeftTriggerButtonReleased() {
+    return getRawButtonReleased(Button.kLeftTrigger.value);
+  }
+
+  /**
+   * Whether the right trigger (RTB) button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getRightTriggerButtonReleased() {
+    return getRawButtonReleased(Button.kRightTrigger.value);
+  }
+
+  /**
+   * Constructs an event instance around the left trigger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the left trigger button's digital signal attached to the
+   *     given loop.
+   */
+  public BooleanEvent leftTrigger(EventLoop loop) {
+    return new BooleanEvent(loop, this::getLeftTriggerButton);
+  }
+
+  /**
+   * Constructs an event instance around the right trigger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the right trigger button's digital signal attached to
+   *     the given loop.
+   */
+  public BooleanEvent rightTrigger(EventLoop loop) {
+    return new BooleanEvent(loop, this::getRightTriggerButton);
+  }
+
+  /**
+   * Read the value of the A button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getAButton() {
+    return getRawButton(Button.kA.value);
+  }
+
+  /**
+   * Whether the A button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getAButtonPressed() {
+    return getRawButtonPressed(Button.kA.value);
+  }
+
+  /**
+   * Whether the A button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getAButtonReleased() {
+    return getRawButtonReleased(Button.kA.value);
+  }
+
+  /**
+   * Constructs an event instance around the A button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the A button's digital signal attached to the given
+   *     loop.
+   */
+  @SuppressWarnings("MethodName")
+  public BooleanEvent a(EventLoop loop) {
+    return new BooleanEvent(loop, this::getAButton);
+  }
+
+  /**
+   * Read the value of the B button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getBButton() {
+    return getRawButton(Button.kB.value);
+  }
+
+  /**
+   * Whether the B button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getBButtonPressed() {
+    return getRawButtonPressed(Button.kB.value);
+  }
+
+  /**
+   * Whether the B button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getBButtonReleased() {
+    return getRawButtonReleased(Button.kB.value);
+  }
+
+  /**
+   * Constructs an event instance around the B button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the B button's digital signal attached to the given
+   *     loop.
+   */
+  @SuppressWarnings("MethodName")
+  public BooleanEvent b(EventLoop loop) {
+    return new BooleanEvent(loop, this::getBButton);
+  }
+
+  /**
+   * Read the value of the X button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getXButton() {
+    return getRawButton(Button.kX.value);
+  }
+
+  /**
+   * Whether the X button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getXButtonPressed() {
+    return getRawButtonPressed(Button.kX.value);
+  }
+
+  /**
+   * Whether the X button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getXButtonReleased() {
+    return getRawButtonReleased(Button.kX.value);
+  }
+
+  /**
+   * Constructs an event instance around the X button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the X button's digital signal attached to the given
+   *     loop.
+   */
+  @SuppressWarnings("MethodName")
+  public BooleanEvent x(EventLoop loop) {
+    return new BooleanEvent(loop, this::getXButton);
+  }
+
+  /**
+   * Read the value of the Y button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getYButton() {
+    return getRawButton(Button.kY.value);
+  }
+
+  /**
+   * Whether the Y button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getYButtonPressed() {
+    return getRawButtonPressed(Button.kY.value);
+  }
+
+  /**
+   * Whether the Y button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getYButtonReleased() {
+    return getRawButtonReleased(Button.kY.value);
+  }
+
+  /**
+   * Constructs an event instance around the Y button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the Y button's digital signal attached to the given
+   *     loop.
+   */
+  @SuppressWarnings("MethodName")
+  public BooleanEvent y(EventLoop loop) {
+    return new BooleanEvent(loop, this::getYButton);
+  }
+
+  /**
+   * Read the value of the ellipses button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getEllipsesButton() {
+    return getRawButton(Button.kEllipses.value);
+  }
+
+  /**
+   * Whether the ellipses button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getEllipsesButtonPressed() {
+    return getRawButtonPressed(Button.kEllipses.value);
+  }
+
+  /**
+   * Whether the ellipses button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getEllipsesButtonReleased() {
+    return getRawButtonReleased(Button.kEllipses.value);
+  }
+
+  /**
+   * Constructs an event instance around the ellipses button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the ellipses button's digital signal attached to the
+   *     given loop.
+   */
+  public BooleanEvent ellipses(EventLoop loop) {
+    return new BooleanEvent(loop, this::getEllipsesButton);
+  }
+
+  /**
+   * Read the value of the hamburger button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getHamburgerButton() {
+    return getRawButton(Button.kHamburger.value);
+  }
+
+  /**
+   * Whether the hamburger button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getHamburgerButtonPressed() {
+    return getRawButtonPressed(Button.kHamburger.value);
+  }
+
+  /**
+   * Whether the hamburger button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getHamburgerButtonReleased() {
+    return getRawButtonReleased(Button.kHamburger.value);
+  }
+
+  /**
+   * Constructs an event instance around the hamburger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the hamburger button's digital signal attached to the
+   *     given loop.
+   */
+  public BooleanEvent hamburger(EventLoop loop) {
+    return new BooleanEvent(loop, this::getHamburgerButton);
+  }
+
+  /**
+   * Read the value of the stadia button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getStadiaButton() {
+    return getRawButton(Button.kStadia.value);
+  }
+
+  /**
+   * Whether the stadia button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getStadiaButtonPressed() {
+    return getRawButtonPressed(Button.kStadia.value);
+  }
+
+  /**
+   * Whether the stadia button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getStadiaButtonReleased() {
+    return getRawButtonReleased(Button.kStadia.value);
+  }
+
+  /**
+   * Constructs an event instance around the stadia button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the stadia button's digital signal attached to the given
+   *     loop.
+   */
+  @SuppressWarnings("MethodName")
+  public BooleanEvent stadia(EventLoop loop) {
+    return new BooleanEvent(loop, this::getStadiaButton);
+  }
+
+  /**
+   * Read the value of the google button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getGoogleButton() {
+    return getRawButton(Button.kGoogle.value);
+  }
+
+  /**
+   * Whether the google button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getGoogleButtonPressed() {
+    return getRawButtonPressed(Button.kGoogle.value);
+  }
+
+  /**
+   * Whether the google button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getGoogleButtonReleased() {
+    return getRawButtonReleased(Button.kGoogle.value);
+  }
+
+  /**
+   * Constructs an event instance around the google button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the google button's digital signal attached to the given
+   *     loop.
+   */
+  @SuppressWarnings("MethodName")
+  public BooleanEvent google(EventLoop loop) {
+    return new BooleanEvent(loop, this::getGoogleButton);
+  }
+
+  /**
+   * Read the value of the frame button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getFrameButton() {
+    return getRawButton(Button.kFrame.value);
+  }
+
+  /**
+   * Whether the frame button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getFrameButtonPressed() {
+    return getRawButtonPressed(Button.kFrame.value);
+  }
+
+  /**
+   * Whether the frame button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getFrameButtonReleased() {
+    return getRawButtonReleased(Button.kFrame.value);
+  }
+
+  /**
+   * Constructs an event instance around the frame button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the frame button's digital signal attached to the given
+   *     loop.
+   */
+  @SuppressWarnings("MethodName")
+  public BooleanEvent frame(EventLoop loop) {
+    return new BooleanEvent(loop, this::getFrameButton);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java
index 705d6b1..8f78865 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java
@@ -23,11 +23,16 @@
 
   /** Event trigger combinations for a synchronous interrupt. */
   public enum WaitResult {
+    /** Timeout event. */
     kTimeout(0x0),
+    /** Rising edge event. */
     kRisingEdge(0x1),
+    /** Falling edge event. */
     kFallingEdge(0x100),
+    /** Both rising and falling edge events. */
     kBoth(0x101);
 
+    /** WaitResult value. */
     public final int value;
 
     WaitResult(int value) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
index 8b0d167..5ff0540 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.ThreadsJNI;
 
+/** Thread utility functions. */
 public final class Threads {
   /**
    * Get the thread priority for the current thread.
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
index 5a53b14..12346c4 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -65,6 +65,7 @@
     }
   }
 
+  /** Default loop period. */
   public static final double kDefaultPeriod = 0.02;
 
   // The C pointer to the notifier object. We don't use it directly, it is
@@ -159,7 +160,7 @@
    * @param callback The callback to run.
    * @param periodSeconds The period at which to run the callback in seconds.
    */
-  public void addPeriodic(Runnable callback, double periodSeconds) {
+  public final void addPeriodic(Runnable callback, double periodSeconds) {
     m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, 0.0));
   }
 
@@ -174,7 +175,7 @@
    * @param offsetSeconds The offset from the common starting time in seconds. This is useful for
    *     scheduling a callback in a different timeslot relative to TimedRobot.
    */
-  public void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
+  public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
     m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, offsetSeconds));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
index 4f948f5..63e5ed4 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
@@ -83,7 +83,7 @@
    *
    * <p>Make the timer startTime the current time so new requests will be relative now.
    */
-  public void reset() {
+  public final void reset() {
     m_accumulatedTime = 0;
     m_startTime = getMsClock();
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
index 2415079..15ca7ff 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
@@ -36,7 +36,7 @@
   }
 
   /** Restarts the epoch timer. */
-  public void resetTimer() {
+  public final void resetTimer() {
     m_startTime = RobotController.getFPGATime();
   }
 
@@ -75,9 +75,7 @@
       StringBuilder sb = new StringBuilder();
       m_lastEpochsPrintTime = now;
       m_epochs.forEach(
-          (key, value) -> {
-            sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6));
-          });
+          (key, value) -> sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6)));
       if (sb.length() > 0) {
         output.accept(sb.toString());
       }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
index fd2b05b..a7b5e71 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
@@ -108,6 +108,11 @@
     SendableRegistry.addLW(this, "Ultrasonic", m_echoChannel.getChannel());
   }
 
+  /**
+   * Returns the echo channel.
+   *
+   * @return The echo channel.
+   */
   public int getEchoChannel() {
     return m_echoChannel.getChannel();
   }
@@ -121,6 +126,7 @@
    * @param echoChannel The digital input channel that receives the echo. The length of time that
    *     the echo is high represents the round trip time of the ping, and the distance.
    */
+  @SuppressWarnings("this-escape")
   public Ultrasonic(final int pingChannel, final int echoChannel) {
     m_pingChannel = new DigitalOutput(pingChannel);
     m_echoChannel = new DigitalInput(echoChannel);
@@ -138,6 +144,7 @@
    *     10uS pulse to start.
    * @param echoChannel The digital input object that times the return pulse to determine the range.
    */
+  @SuppressWarnings("this-escape")
   public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
     requireNonNullParam(pingChannel, "pingChannel", "Ultrasonic");
     requireNonNullParam(echoChannel, "echoChannel", "Ultrasonic");
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
index f7bf85b..18d4b64 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
@@ -209,7 +209,7 @@
   }
 
   private static void updateAlarm() {
-    if (m_watchdogs.size() == 0) {
+    if (m_watchdogs.isEmpty()) {
       NotifierJNI.cancelNotifierAlarm(m_notifier);
     } else {
       NotifierJNI.updateNotifierAlarm(
@@ -233,7 +233,7 @@
 
       m_queueMutex.lock();
       try {
-        if (m_watchdogs.size() == 0) {
+        if (m_watchdogs.isEmpty()) {
           continue;
         }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
index c346ae3..5bd7a01 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
@@ -15,21 +15,36 @@
  * <p>This class handles Xbox input that comes from the Driver Station. Each time a value is
  * requested the most recent value is returned. There is a single class instance for each controller
  * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ *
+ * <p>Only first party controllers from Microsoft are guaranteed to have the correct mapping, and
+ * only through the official NI DS. Sim is not guaranteed to have the same mapping, as well as any
+ * 3rd party controllers.
  */
 public class XboxController extends GenericHID {
   /** Represents a digital button on an XboxController. */
   public enum Button {
+    /** Left bumper. */
     kLeftBumper(5),
+    /** Right bumper. */
     kRightBumper(6),
+    /** Left stick. */
     kLeftStick(9),
+    /** Right stick. */
     kRightStick(10),
+    /** A. */
     kA(1),
+    /** B. */
     kB(2),
+    /** X. */
     kX(3),
+    /** Y. */
     kY(4),
+    /** Back. */
     kBack(7),
+    /** Start. */
     kStart(8);
 
+    /** Button value. */
     public final int value;
 
     Button(int value) {
@@ -56,13 +71,20 @@
 
   /** Represents an axis on an XboxController. */
   public enum Axis {
+    /** Left X. */
     kLeftX(0),
+    /** Right X. */
     kRightX(4),
+    /** Left Y. */
     kLeftY(1),
+    /** Right Y. */
     kRightY(5),
+    /** Left trigger. */
     kLeftTrigger(2),
+    /** Right trigger. */
     kRightTrigger(3);
 
+    /** Axis value. */
     public final int value;
 
     Axis(int value) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/EdgeConfiguration.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/EdgeConfiguration.java
index 9d58fbf..c969249 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/EdgeConfiguration.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/EdgeConfiguration.java
@@ -4,15 +4,22 @@
 
 package edu.wpi.first.wpilibj.counter;
 
+/** Edge configuration. */
 public enum EdgeConfiguration {
+  /** No edge configuration (neither rising nor falling). */
   kNone(false, false),
+  /** Rising edge configuration. */
   kRisingEdge(true, false),
+  /** Falling edge configuration. */
   kFallingEdge(false, true),
+  /** Both rising and falling edge configuration. */
   kBoth(true, true);
 
+  /** True if triggering on rising edge. */
   @SuppressWarnings("MemberName")
   public final boolean rising;
 
+  /** True if triggering on falling edge. */
   @SuppressWarnings("MemberName")
   public final boolean falling;
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/ExternalDirectionCounter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/ExternalDirectionCounter.java
index 6828b9a..97fbf5b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/ExternalDirectionCounter.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/ExternalDirectionCounter.java
@@ -34,6 +34,7 @@
    * @param countSource The source for counting.
    * @param directionSource The source for selecting count direction.
    */
+  @SuppressWarnings("this-escape")
   public ExternalDirectionCounter(DigitalSource countSource, DigitalSource directionSource) {
     m_countSource = requireNonNullParam(countSource, "countSource", "ExternalDirectionCounter");
     m_directionSource =
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/Tachometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/Tachometer.java
index 5bd8e7e..6bef24d 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/Tachometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/Tachometer.java
@@ -34,6 +34,7 @@
    *
    * @param source The DigitalSource (e.g. DigitalInput) of the Tachometer.
    */
+  @SuppressWarnings("this-escape")
   public Tachometer(DigitalSource source) {
     m_source = requireNonNullParam(source, "source", "Tachometer");
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/UpDownCounter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/UpDownCounter.java
index c89c118..ed9f35e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/UpDownCounter.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/UpDownCounter.java
@@ -32,6 +32,7 @@
    * @param upSource The up count source (can be null).
    * @param downSource The down count source (can be null).
    */
+  @SuppressWarnings("this-escape")
   public UpDownCounter(DigitalSource upSource, DigitalSource downSource) {
     ByteBuffer index = ByteBuffer.allocateDirect(4);
     // set the byte order
@@ -92,7 +93,7 @@
   }
 
   /** Resets the current count. */
-  public void reset() {
+  public final void reset() {
     CounterJNI.resetCounter(m_handle);
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
index 9a1f2ba..2199aa8 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -14,49 +14,16 @@
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.function.DoubleConsumer;
 
 /**
  * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
  * base, "tank drive", or West Coast Drive.
  *
  * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
- * (e.g., 6WD or 8WD). This class takes a MotorController per side. For four and six motor
- * drivetrains, construct and pass in {@link
- * edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup} instances as follows.
- *
- * <p>Four motor drivetrain:
- *
- * <pre><code>
- * public class Robot {
- *   MotorController m_frontLeft = new PWMVictorSPX(1);
- *   MotorController m_rearLeft = new PWMVictorSPX(2);
- *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_rearLeft);
- *
- *   MotorController m_frontRight = new PWMVictorSPX(3);
- *   MotorController m_rearRight = new PWMVictorSPX(4);
- *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_rearRight);
- *
- *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
- * }
- * </code></pre>
- *
- * <p>Six motor drivetrain:
- *
- * <pre><code>
- * public class Robot {
- *   MotorController m_frontLeft = new PWMVictorSPX(1);
- *   MotorController m_midLeft = new PWMVictorSPX(2);
- *   MotorController m_rearLeft = new PWMVictorSPX(3);
- *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
- *
- *   MotorController m_frontRight = new PWMVictorSPX(4);
- *   MotorController m_midRight = new PWMVictorSPX(5);
- *   MotorController m_rearRight = new PWMVictorSPX(6);
- *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_midRight, m_rearRight);
- *
- *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
- * }
- * </code></pre>
+ * (e.g., 6WD or 8WD). This class takes a setter per side. For four and six motor drivetrains, use
+ * CAN motor controller followers or {@link
+ * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}.
  *
  * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
  *
@@ -88,8 +55,12 @@
 public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
   private static int instances;
 
-  private final MotorController m_leftMotor;
-  private final MotorController m_rightMotor;
+  private final DoubleConsumer m_leftMotor;
+  private final DoubleConsumer m_rightMotor;
+
+  // Used for Sendable property getters
+  private double m_leftOutput;
+  private double m_rightOutput;
 
   private boolean m_reported;
 
@@ -100,7 +71,10 @@
    */
   @SuppressWarnings("MemberName")
   public static class WheelSpeeds {
+    /** Left wheel speed. */
     public double left;
+
+    /** Right wheel speed. */
     public double right;
 
     /** Constructs a WheelSpeeds with zeroes for left and right speeds. */
@@ -128,14 +102,30 @@
    * @param leftMotor Left motor.
    * @param rightMotor Right motor.
    */
+  @SuppressWarnings({"removal", "this-escape"})
   public DifferentialDrive(MotorController leftMotor, MotorController rightMotor) {
+    this((double output) -> leftMotor.set(output), (double output) -> rightMotor.set(output));
+    SendableRegistry.addChild(this, leftMotor);
+    SendableRegistry.addChild(this, rightMotor);
+  }
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * <p>To pass multiple motors per side, use CAN motor controller followers or {@link
+   * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
+   * motor needs to be inverted, do so before passing it in.
+   *
+   * @param leftMotor Left motor setter.
+   * @param rightMotor Right motor setter.
+   */
+  @SuppressWarnings("this-escape")
+  public DifferentialDrive(DoubleConsumer leftMotor, DoubleConsumer rightMotor) {
     requireNonNullParam(leftMotor, "leftMotor", "DifferentialDrive");
     requireNonNullParam(rightMotor, "rightMotor", "DifferentialDrive");
 
     m_leftMotor = leftMotor;
     m_rightMotor = rightMotor;
-    SendableRegistry.addChild(this, m_leftMotor);
-    SendableRegistry.addChild(this, m_rightMotor);
     instances++;
     SendableRegistry.addLW(this, "DifferentialDrive", instances);
   }
@@ -177,8 +167,11 @@
 
     var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
 
-    m_leftMotor.set(speeds.left * m_maxOutput);
-    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
 
     feed();
   }
@@ -206,8 +199,11 @@
 
     var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
 
-    m_leftMotor.set(speeds.left * m_maxOutput);
-    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
 
     feed();
   }
@@ -244,8 +240,11 @@
 
     var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
 
-    m_leftMotor.set(speeds.left * m_maxOutput);
-    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
 
     feed();
   }
@@ -350,8 +349,12 @@
 
   @Override
   public void stopMotor() {
-    m_leftMotor.stopMotor();
-    m_rightMotor.stopMotor();
+    m_leftOutput = 0.0;
+    m_rightOutput = 0.0;
+
+    m_leftMotor.accept(0.0);
+    m_rightMotor.accept(0.0);
+
     feed();
   }
 
@@ -365,7 +368,7 @@
     builder.setSmartDashboardType("DifferentialDrive");
     builder.setActuator(true);
     builder.setSafeState(this::stopMotor);
-    builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
-    builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set);
+    builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor::accept);
+    builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor::accept);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
index 181e275..236a4fb 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.function.DoubleConsumer;
 
 /**
  * A class for driving Mecanum drive platforms.
@@ -56,10 +57,16 @@
 public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
   private static int instances;
 
-  private final MotorController m_frontLeftMotor;
-  private final MotorController m_rearLeftMotor;
-  private final MotorController m_frontRightMotor;
-  private final MotorController m_rearRightMotor;
+  private final DoubleConsumer m_frontLeftMotor;
+  private final DoubleConsumer m_rearLeftMotor;
+  private final DoubleConsumer m_frontRightMotor;
+  private final DoubleConsumer m_rearRightMotor;
+
+  // Used for Sendable property getters
+  private double m_frontLeftOutput;
+  private double m_rearLeftOutput;
+  private double m_frontRightOutput;
+  private double m_rearRightOutput;
 
   private boolean m_reported;
 
@@ -70,9 +77,16 @@
    */
   @SuppressWarnings("MemberName")
   public static class WheelSpeeds {
+    /** Front-left wheel speed. */
     public double frontLeft;
+
+    /** Front-right wheel speed. */
     public double frontRight;
+
+    /** Rear-left wheel speed. */
     public double rearLeft;
+
+    /** Rear-right wheel speed. */
     public double rearRight;
 
     /** Constructs a WheelSpeeds with zeroes for all four speeds. */
@@ -104,11 +118,39 @@
    * @param frontRightMotor The motor on the front-right corner.
    * @param rearRightMotor The motor on the rear-right corner.
    */
+  @SuppressWarnings({"removal", "this-escape"})
   public MecanumDrive(
       MotorController frontLeftMotor,
       MotorController rearLeftMotor,
       MotorController frontRightMotor,
       MotorController rearRightMotor) {
+    this(
+        (double output) -> frontLeftMotor.set(output),
+        (double output) -> rearLeftMotor.set(output),
+        (double output) -> frontRightMotor.set(output),
+        (double output) -> rearRightMotor.set(output));
+    SendableRegistry.addChild(this, frontLeftMotor);
+    SendableRegistry.addChild(this, rearLeftMotor);
+    SendableRegistry.addChild(this, frontRightMotor);
+    SendableRegistry.addChild(this, rearRightMotor);
+  }
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * <p>If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor The setter for the motor on the front-left corner.
+   * @param rearLeftMotor The setter for the motor on the rear-left corner.
+   * @param frontRightMotor The setter for the motor on the front-right corner.
+   * @param rearRightMotor The setter for the motor on the rear-right corner.
+   */
+  @SuppressWarnings("this-escape")
+  public MecanumDrive(
+      DoubleConsumer frontLeftMotor,
+      DoubleConsumer rearLeftMotor,
+      DoubleConsumer frontRightMotor,
+      DoubleConsumer rearRightMotor) {
     requireNonNullParam(frontLeftMotor, "frontLeftMotor", "MecanumDrive");
     requireNonNullParam(rearLeftMotor, "rearLeftMotor", "MecanumDrive");
     requireNonNullParam(frontRightMotor, "frontRightMotor", "MecanumDrive");
@@ -118,10 +160,6 @@
     m_rearLeftMotor = rearLeftMotor;
     m_frontRightMotor = frontRightMotor;
     m_rearRightMotor = rearRightMotor;
-    SendableRegistry.addChild(this, m_frontLeftMotor);
-    SendableRegistry.addChild(this, m_rearLeftMotor);
-    SendableRegistry.addChild(this, m_frontRightMotor);
-    SendableRegistry.addChild(this, m_rearRightMotor);
     instances++;
     SendableRegistry.addLW(this, "MecanumDrive", instances);
   }
@@ -171,10 +209,15 @@
 
     var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
 
-    m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput);
-    m_frontRightMotor.set(speeds.frontRight * m_maxOutput);
-    m_rearLeftMotor.set(speeds.rearLeft * m_maxOutput);
-    m_rearRightMotor.set(speeds.rearRight * m_maxOutput);
+    m_frontLeftOutput = speeds.frontLeft * m_maxOutput;
+    m_rearLeftOutput = speeds.rearLeft * m_maxOutput;
+    m_frontRightOutput = speeds.frontRight * m_maxOutput;
+    m_rearRightOutput = speeds.rearRight * m_maxOutput;
+
+    m_frontLeftMotor.accept(m_frontLeftOutput);
+    m_frontRightMotor.accept(m_frontRightOutput);
+    m_rearLeftMotor.accept(m_rearLeftOutput);
+    m_rearRightMotor.accept(m_rearRightOutput);
 
     feed();
   }
@@ -255,10 +298,16 @@
 
   @Override
   public void stopMotor() {
-    m_frontLeftMotor.stopMotor();
-    m_frontRightMotor.stopMotor();
-    m_rearLeftMotor.stopMotor();
-    m_rearRightMotor.stopMotor();
+    m_frontLeftOutput = 0.0;
+    m_frontRightOutput = 0.0;
+    m_rearLeftOutput = 0.0;
+    m_rearRightOutput = 0.0;
+
+    m_frontLeftMotor.accept(0.0);
+    m_frontRightMotor.accept(0.0);
+    m_rearLeftMotor.accept(0.0);
+    m_rearRightMotor.accept(0.0);
+
     feed();
   }
 
@@ -273,11 +322,12 @@
     builder.setActuator(true);
     builder.setSafeState(this::stopMotor);
     builder.addDoubleProperty(
-        "Front Left Motor Speed", m_frontLeftMotor::get, m_frontLeftMotor::set);
+        "Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor::accept);
     builder.addDoubleProperty(
-        "Front Right Motor Speed", m_frontRightMotor::get, m_frontRightMotor::set);
-    builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, m_rearLeftMotor::set);
+        "Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor::accept);
     builder.addDoubleProperty(
-        "Rear Right Motor Speed", m_rearRightMotor::get, m_rearRightMotor::set);
+        "Rear Left Motor Speed", () -> m_rearLeftOutput, m_rearLeftMotor::accept);
+    builder.addDoubleProperty(
+        "Rear Right Motor Speed", () -> m_rearRightOutput, m_rearRightMotor::accept);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
index c4e931d..a952b2e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
@@ -12,22 +12,36 @@
  * <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default.
  */
 public abstract class RobotDriveBase extends MotorSafety {
+  /** Default input deadband. */
   public static final double kDefaultDeadband = 0.02;
+
+  /** Default maximum output. */
   public static final double kDefaultMaxOutput = 1.0;
 
+  /** Input deadband. */
   protected double m_deadband = kDefaultDeadband;
+
+  /** Maximum output. */
   protected double m_maxOutput = kDefaultMaxOutput;
 
   /** The location of a motor on the robot for the purpose of driving. */
   public enum MotorType {
+    /** Front left motor. */
     kFrontLeft(0),
+    /** Front right motor. */
     kFrontRight(1),
+    /** Rear left motor. */
     kRearLeft(2),
+    /** Reat right motor. */
     kRearRight(3),
+    /** Left motor. */
     kLeft(0),
+    /** Right motor. */
     kRight(1),
+    /** Back motor. */
     kBack(2);
 
+    /** MotorType value. */
     public final int value;
 
     MotorType(int value) {
@@ -36,6 +50,7 @@
   }
 
   /** RobotDriveBase constructor. */
+  @SuppressWarnings("this-escape")
   public RobotDriveBase() {
     setSafetyEnabled(true);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/EventLoop.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/EventLoop.java
index 3e220e0..38d21cd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/EventLoop.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/EventLoop.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.event;
 
 import java.util.Collection;
+import java.util.ConcurrentModificationException;
 import java.util.LinkedHashSet;
 
 /**
@@ -12,6 +13,10 @@
  */
 public final class EventLoop {
   private final Collection<Runnable> m_bindings = new LinkedHashSet<>();
+  private boolean m_running;
+
+  /** Default constructor. */
+  public EventLoop() {}
 
   /**
    * Bind a new action to run when the loop is polled.
@@ -19,16 +24,27 @@
    * @param action the action to run.
    */
   public void bind(Runnable action) {
+    if (m_running) {
+      throw new ConcurrentModificationException("Cannot bind EventLoop while it is running");
+    }
     m_bindings.add(action);
   }
 
   /** Poll all bindings. */
   public void poll() {
-    m_bindings.forEach(Runnable::run);
+    try {
+      m_running = true;
+      m_bindings.forEach(Runnable::run);
+    } finally {
+      m_running = false;
+    }
   }
 
   /** Clear all bindings. */
   public void clear() {
+    if (m_running) {
+      throw new ConcurrentModificationException("Cannot clear EventLoop while it is running");
+    }
     m_bindings.clear();
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
index d33b7ae..630d45c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
@@ -11,10 +11,15 @@
  */
 @Deprecated(since = "2024", forRemoval = true)
 public interface Accelerometer {
+  /** Accelerometer range. */
   enum Range {
+    /** 2 Gs max. */
     k2G,
+    /** 4 Gs max. */
     k4G,
+    /** 8 Gs max. */
     k8G,
+    /** 16 Gs max. */
     k16G
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java
index 03d6397..57c6bf7 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java
@@ -9,9 +9,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.concurrent.atomic.AtomicBoolean;
 
-/*
- * For internal use only.
- */
+/** For internal use only. */
 public class DriverStationModeThread implements AutoCloseable {
   private final AtomicBoolean m_keepAlive = new AtomicBoolean();
   private final Thread m_thread;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
index ac8ef1d..1b9d1ea 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
@@ -50,7 +50,7 @@
   private static Runnable disabledListener;
 
   static {
-    SendableRegistry.setLiveWindowBuilderFactory(() -> new SendableBuilderImpl());
+    SendableRegistry.setLiveWindowBuilderFactory(SendableBuilderImpl::new);
     enabledPub.set(false);
   }
 
@@ -67,14 +67,29 @@
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
+  /**
+   * Sets function to be called when LiveWindow is enabled.
+   *
+   * @param runnable function (or null for none)
+   */
   public static synchronized void setEnabledListener(Runnable runnable) {
     enabledListener = runnable;
   }
 
+  /**
+   * Sets function to be called when LiveWindow is disabled.
+   *
+   * @param runnable function (or null for none)
+   */
   public static synchronized void setDisabledListener(Runnable runnable) {
     disabledListener = runnable;
   }
 
+  /**
+   * Returns true if LiveWindow is enabled.
+   *
+   * @return True if LiveWindow is enabled.
+   */
   public static synchronized boolean isEnabled() {
     return liveWindowEnabled;
   }
@@ -105,10 +120,7 @@
       } else {
         System.out.println("stopping live window mode.");
         SendableRegistry.foreachLiveWindow(
-            dataHandle,
-            cbdata -> {
-              ((SendableBuilderImpl) cbdata.builder).stopLiveWindowMode();
-            });
+            dataHandle, cbdata -> ((SendableBuilderImpl) cbdata.builder).stopLiveWindowMode());
         if (disabledListener != null) {
           disabledListener.run();
         }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java
index 5f0f26b..9d66907 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java
@@ -32,6 +32,7 @@
    * @param channel The PWM channel that the DMC60 is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public DMC60(final int channel) {
     super("DMC60", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java
index f072067..4a0ac85 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java
@@ -31,6 +31,7 @@
    * @param channel The PWM channel that the Jaguar is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public Jaguar(final int channel) {
     super("Jaguar", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
index d9da3c8..c56496c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
@@ -9,7 +9,14 @@
 import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.Arrays;
 
-/** Allows multiple {@link MotorController} objects to be linked together. */
+/**
+ * Allows multiple {@link MotorController} objects to be linked together.
+ *
+ * @deprecated Use {@link PWMMotorController#addFollower(PWMMotorController)} or if using CAN motor
+ *     controllers, use their method of following.
+ */
+@SuppressWarnings("removal")
+@Deprecated(forRemoval = true, since = "2024")
 public class MotorControllerGroup implements MotorController, Sendable, AutoCloseable {
   private boolean m_isInverted;
   private final MotorController[] m_motorControllers;
@@ -21,6 +28,7 @@
    * @param motorController The first MotorController to add
    * @param motorControllers The MotorControllers to add
    */
+  @SuppressWarnings("this-escape")
   public MotorControllerGroup(
       MotorController motorController, MotorController... motorControllers) {
     m_motorControllers = new MotorController[motorControllers.length + 1];
@@ -29,6 +37,12 @@
     init();
   }
 
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @param motorControllers The MotorControllers to add.
+   */
+  @SuppressWarnings("this-escape")
   public MotorControllerGroup(MotorController[] motorControllers) {
     m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length);
     init();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java
index 25c29ac..01af765 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java
@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj.PWM;
 
 /** Nidec Brushless Motor. */
+@SuppressWarnings("removal")
 public class NidecBrushless extends MotorSafety
     implements MotorController, Sendable, AutoCloseable {
   private boolean m_isInverted;
@@ -30,6 +31,7 @@
    * @param dioChannel The DIO channel that the Nidec Brushless controller is attached to. 0-9 are
    *     on-board, 10-25 are on the MXP port
    */
+  @SuppressWarnings("this-escape")
   public NidecBrushless(final int pwmChannel, final int dioChannel) {
     setSafetyEnabled(false);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java
index 68817a1..b72cee3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java
@@ -9,11 +9,16 @@
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.MotorSafety;
 import edu.wpi.first.wpilibj.PWM;
+import java.util.ArrayList;
 
 /** Common base class for all PWM Motor Controllers. */
+@SuppressWarnings("removal")
 public abstract class PWMMotorController extends MotorSafety
     implements MotorController, Sendable, AutoCloseable {
   private boolean m_isInverted;
+  private final ArrayList<PWMMotorController> m_followers = new ArrayList<>();
+
+  /** PWM instances for motor controller. */
   protected PWM m_pwm;
 
   /**
@@ -23,6 +28,7 @@
    * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
    *     on the MXP port
    */
+  @SuppressWarnings("this-escape")
   protected PWMMotorController(final String name, final int channel) {
     m_pwm = new PWM(channel, false);
     SendableRegistry.addLW(this, name, channel);
@@ -45,7 +51,15 @@
    */
   @Override
   public void set(double speed) {
-    m_pwm.setSpeed(m_isInverted ? -speed : speed);
+    if (m_isInverted) {
+      speed = -speed;
+    }
+    m_pwm.setSpeed(speed);
+
+    for (var follower : m_followers) {
+      follower.set(speed);
+    }
+
     feed();
   }
 
@@ -116,6 +130,15 @@
     m_pwm.enableDeadbandElimination(eliminateDeadband);
   }
 
+  /**
+   * Make the given PWM motor controller follow the output of this one.
+   *
+   * @param follower The motor controller follower.
+   */
+  public void addFollower(PWMMotorController follower) {
+    m_followers.add(follower);
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Motor Controller");
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkFlex.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkFlex.java
new file mode 100644
index 0000000..059b18a
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkFlex.java
@@ -0,0 +1,45 @@
+// 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.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * REV Robotics SPARK Flex Motor Controller with PWM control.
+ *
+ * <p>Note that the SPARK Flex uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the SPARK Flex User Manual available from
+ * REV Robotics.
+ *
+ * <ul>
+ *   <li>2.003ms = full "forward"
+ *   <li>1.550ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.460ms = the "low end" of the deadband range
+ *   <li>0.999ms = full "reverse"
+ * </ul>
+ */
+public class PWMSparkFlex extends PWMMotorController {
+  /**
+   * Common initialization code called by all constructors.
+   *
+   * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+   */
+  @SuppressWarnings("this-escape")
+  public PWMSparkFlex(final int channel) {
+    super("PWMSparkFlex", channel);
+
+    m_pwm.setBoundsMicroseconds(2003, 1550, 1500, 1460, 999);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_RevSparkFlexPWM, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java
index ede5f5d..205d70a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java
@@ -31,6 +31,7 @@
    *
    * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
    */
+  @SuppressWarnings("this-escape")
   public PWMSparkMax(final int channel) {
     super("PWMSparkMax", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java
index 29ac766..2211fa5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java
@@ -32,6 +32,7 @@
    * @param channel The PWM channel that the Talon FX is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public PWMTalonFX(final int channel) {
     super("PWMTalonFX", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java
index 085cd86..4677627 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java
@@ -32,6 +32,7 @@
    * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are on-board, 10-19 are
    *     on the MXP port
    */
+  @SuppressWarnings("this-escape")
   public PWMTalonSRX(final int channel) {
     super("PWMTalonSRX", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java
index a38e936..f9960b0 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java
@@ -31,6 +31,7 @@
    * @param channel The PWM channel that the Venom is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public PWMVenom(final int channel) {
     super("PWMVenom", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java
index 555614a..daa70f3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java
@@ -32,6 +32,7 @@
    * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9 are on-board, 10-19
    *     are on the MXP port
    */
+  @SuppressWarnings("this-escape")
   public PWMVictorSPX(final int channel) {
     super("PWMVictorSPX", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java
index 7cdb035..31246bc 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java
@@ -32,6 +32,7 @@
    * @param channel The PWM channel that the SD540 is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public SD540(final int channel) {
     super("SD540", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java
index cc92621..d57b626 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java
@@ -32,6 +32,7 @@
    * @param channel The PWM channel that the SPARK is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public Spark(final int channel) {
     super("Spark", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java
index dd9e473..bcbad60 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java
@@ -31,6 +31,7 @@
    * @param channel The PWM channel that the Talon is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public Talon(final int channel) {
     super("Talon", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java
index 5265a8e..1a816ad 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java
@@ -17,7 +17,8 @@
  * 884 controllers also but if users experience issues such as asymmetric behavior around the
  * deadband or inability to saturate the controller in either direction, calibration is recommended.
  * The calibration procedure can be found in the Victor 884 User Manual available from VEX Robotics:
- * http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf
+ * <a
+ * href="http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf">http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf</a>
  *
  * <ul>
  *   <li>2.027ms = full "forward"
@@ -34,6 +35,7 @@
    * @param channel The PWM channel that the Victor is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public Victor(final int channel) {
     super("Victor", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java
index aa262d0..09ba3c6 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java
@@ -32,6 +32,7 @@
    * @param channel The PWM channel that the VictorSP is attached to. 0-9 are on-board, 10-19 are on
    *     the MXP port
    */
+  @SuppressWarnings("this-escape")
   public VictorSP(final int channel) {
     super("VictorSP", channel);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
index 4d64efe..0d85ce6 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
@@ -38,6 +38,11 @@
     m_simpleName = simpleName;
   }
 
+  /**
+   * Returns name of the given enum.
+   *
+   * @return Name of the given enum.
+   */
   public String getSimpleName() {
     return m_simpleName;
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
index 150f0db..bd428cd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
@@ -74,7 +74,7 @@
     }
   }
 
-  /*
+  /**
    * Sets NetworkTable instance used for camera publisher entries.
    *
    * @param inst NetworkTable instance
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
index 39078f4..3e1f75a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
@@ -26,25 +26,53 @@
   private int m_width = -1;
   private int m_height = -1;
 
+  /**
+   * Constructs a ShuffleboardComponent.
+   *
+   * @param parent The parent container.
+   * @param title The component title.
+   * @param type The component type.
+   */
   protected ShuffleboardComponent(ShuffleboardContainer parent, String title, String type) {
     m_parent = requireNonNullParam(parent, "parent", "ShuffleboardComponent");
     m_title = requireNonNullParam(title, "title", "ShuffleboardComponent");
     m_type = type;
   }
 
+  /**
+   * Constructs a ShuffleboardComponent.
+   *
+   * @param parent The parent container.
+   * @param title The component title.
+   */
   protected ShuffleboardComponent(ShuffleboardContainer parent, String title) {
     this(parent, title, null);
   }
 
+  /**
+   * Returns the parent container.
+   *
+   * @return The parent container.
+   */
   public final ShuffleboardContainer getParent() {
     return m_parent;
   }
 
+  /**
+   * Sets the component type.
+   *
+   * @param type The component type.
+   */
   protected final void setType(String type) {
     m_type = type;
     m_metadataDirty = true;
   }
 
+  /**
+   * Returns the component type.
+   *
+   * @return The component type.
+   */
   public final String getType() {
     return m_type;
   }
@@ -109,6 +137,11 @@
     return (C) this;
   }
 
+  /**
+   * Builds NT metadata.
+   *
+   * @param metaTable The NT metadata table.
+   */
   protected final void buildMetadata(NetworkTable metaTable) {
     if (!m_metadataDirty) {
       return;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
index df2f117..88e1e49 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
@@ -18,7 +18,7 @@
 
 final class ShuffleboardInstance implements ShuffleboardRoot {
   private final Map<String, ShuffleboardTab> m_tabs = new LinkedHashMap<>();
-
+  private boolean m_reported = false; // NOPMD redundant field initializer
   private boolean m_tabsChanged = false; // NOPMD redundant field initializer
   private final NetworkTable m_rootTable;
   private final NetworkTable m_rootMetaTable;
@@ -35,12 +35,15 @@
     m_rootMetaTable = m_rootTable.getSubTable(".metadata");
     m_selectedTabPub =
         m_rootMetaTable.getStringTopic("Selected").publish(PubSubOption.keepDuplicates(true));
-    HAL.report(tResourceType.kResourceType_Shuffleboard, 0);
   }
 
   @Override
   public ShuffleboardTab getTab(String title) {
     requireNonNullParam(title, "title", "getTab");
+    if (!m_reported) {
+      HAL.report(tResourceType.kResourceType_Shuffleboard, 0);
+      m_reported = true;
+    }
     if (!m_tabs.containsKey(title)) {
       m_tabs.put(title, new ShuffleboardTab(this, title));
       m_tabsChanged = true;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java
index c50c2f8..bcbf3e2 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java
@@ -9,10 +9,11 @@
 import edu.wpi.first.wpilibj.ADXL345_SPI;
 import java.util.Objects;
 
+/** Class to control a simulated ADXL345. */
 public class ADXL345Sim {
-  protected SimDouble m_simX;
-  protected SimDouble m_simY;
-  protected SimDouble m_simZ;
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
 
   /**
    * Constructor.
@@ -48,15 +49,30 @@
     Objects.requireNonNull(m_simZ);
   }
 
-  public void setX(double x) {
-    m_simX.set(x);
+  /**
+   * Sets the X acceleration.
+   *
+   * @param accel The X acceleration.
+   */
+  public void setX(double accel) {
+    m_simX.set(accel);
   }
 
-  public void setY(double y) {
-    m_simY.set(y);
+  /**
+   * Sets the Y acceleration.
+   *
+   * @param accel The Y acceleration.
+   */
+  public void setY(double accel) {
+    m_simY.set(accel);
   }
 
-  public void setZ(double z) {
-    m_simZ.set(z);
+  /**
+   * Sets the Z acceleration.
+   *
+   * @param accel The Z acceleration.
+   */
+  public void setZ(double accel) {
+    m_simZ.set(accel);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java
index 76df8bd..148f914 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java
@@ -8,10 +8,11 @@
 import edu.wpi.first.wpilibj.ADXL362;
 import java.util.Objects;
 
+/** Class to control a simulated ADXL362. */
 public class ADXL362Sim {
-  protected SimDouble m_simX;
-  protected SimDouble m_simY;
-  protected SimDouble m_simZ;
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
 
   /**
    * Constructor.
@@ -34,15 +35,30 @@
     Objects.requireNonNull(m_simZ);
   }
 
-  public void setX(double x) {
-    m_simX.set(x);
+  /**
+   * Sets the X acceleration.
+   *
+   * @param accel The X acceleration.
+   */
+  public void setX(double accel) {
+    m_simX.set(accel);
   }
 
-  public void setY(double y) {
-    m_simY.set(y);
+  /**
+   * Sets the Y acceleration.
+   *
+   * @param accel The Y acceleration.
+   */
+  public void setY(double accel) {
+    m_simY.set(accel);
   }
 
-  public void setZ(double z) {
-    m_simZ.set(z);
+  /**
+   * Sets the Z acceleration.
+   *
+   * @param accel The Z acceleration.
+   */
+  public void setZ(double accel) {
+    m_simZ.set(accel);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
index 94db6f6..a121659 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
@@ -148,8 +148,7 @@
    * @param dtSeconds the time difference
    */
   public void update(double dtSeconds) {
-    // Update state estimate with RK4
-    m_x = NumericalIntegration.rk4(this::getDynamics, m_x, m_u, dtSeconds);
+    m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds);
     m_y = m_x;
     if (m_measurementStdDevs != null) {
       m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
@@ -237,12 +236,9 @@
    * @return the drivetrain's left side current draw, in amps
    */
   public double getLeftCurrentDrawAmps() {
-    var loadIleft =
-        m_motor.getCurrent(
-                getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters,
-                m_u.get(0, 0))
-            * Math.signum(m_u.get(0, 0));
-    return loadIleft;
+    return m_motor.getCurrent(
+            getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(0, 0))
+        * Math.signum(m_u.get(0, 0));
   }
 
   /**
@@ -251,13 +247,9 @@
    * @return the drivetrain's right side current draw, in amps
    */
   public double getRightCurrentDrawAmps() {
-    var loadIright =
-        m_motor.getCurrent(
-                getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters,
-                m_u.get(1, 0))
-            * Math.signum(m_u.get(1, 0));
-
-    return loadIright;
+    return m_motor.getCurrent(
+            getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(1, 0))
+        * Math.signum(m_u.get(1, 0));
   }
 
   /**
@@ -309,6 +301,13 @@
     m_x.set(State.kRightPosition.value, 0, 0);
   }
 
+  /**
+   * The differential drive dynamics function.
+   *
+   * @param x The state.
+   * @param u The input.
+   * @return The state derivative with respect to time.
+   */
   protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> u) {
     // Because G can be factored out of B, we can divide by the old ratio and multiply
     // by the new ratio to get a new drivetrain model.
@@ -377,12 +376,18 @@
    * and 16:48 8.45:1 -- 14:50 and 19:45 7.31:1 -- 14:50 and 21:43 5.95:1 -- 14:50 and 24:40
    */
   public enum KitbotGearing {
+    /** Gear ratio of 12.75:1. */
     k12p75(12.75),
+    /** Gear ratio of 10.71:1. */
     k10p71(10.71),
+    /** Gear ratio of 8.45:1. */
     k8p45(8.45),
+    /** Gear ratio of 7.31:1. */
     k7p31(7.31),
+    /** Gear ratio of 5.95:1. */
     k5p95(5.95);
 
+    /** KitbotGearing value. */
     public final double value;
 
     KitbotGearing(double i) {
@@ -392,15 +397,24 @@
 
   /** Represents common motor layouts of the kit drivetrain. */
   public enum KitbotMotor {
+    /** One CIM motor per drive side. */
     kSingleCIMPerSide(DCMotor.getCIM(1)),
+    /** Two CIM motors per drive side. */
     kDualCIMPerSide(DCMotor.getCIM(2)),
+    /** One Mini CIM motor per drive side. */
     kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)),
+    /** Two Mini CIM motors per drive side. */
     kDualMiniCIMPerSide(DCMotor.getMiniCIM(2)),
+    /** One Falcon 500 motor per drive side. */
     kSingleFalcon500PerSide(DCMotor.getFalcon500(1)),
+    /** Two Falcon 500 motors per drive side. */
     kDoubleFalcon500PerSide(DCMotor.getFalcon500(2)),
+    /** One NEO motor per drive side. */
     kSingleNEOPerSide(DCMotor.getNEO(1)),
+    /** Two NEO motors per drive side. */
     kDoubleNEOPerSide(DCMotor.getNEO(2));
 
+    /** KitbotMotor value. */
     public final DCMotor value;
 
     KitbotMotor(DCMotor i) {
@@ -410,10 +424,14 @@
 
   /** Represents common wheel sizes of the kit drivetrain. */
   public enum KitbotWheelSize {
+    /** Six inch diameter wheels. */
     kSixInch(Units.inchesToMeters(6)),
+    /** Eight inch diameter wheels. */
     kEightInch(Units.inchesToMeters(8)),
+    /** Ten inch diameter wheels. */
     kTenInch(Units.inchesToMeters(10));
 
+    /** KitbotWheelSize value. */
     public final double value;
 
     KitbotWheelSize(double i) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
index 322d897..6ec8002 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
@@ -41,6 +41,7 @@
    * @param startingHeightMeters The starting height of the elevator.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
+  @SuppressWarnings("this-escape")
   public ElevatorSim(
       LinearSystem<N2, N1, N1> plant,
       DCMotor gearbox,
@@ -221,7 +222,7 @@
    * @param positionMeters The new position in meters.
    * @param velocityMetersPerSecond New velocity in meters per second.
    */
-  public void setState(double positionMeters, double velocityMetersPerSecond) {
+  public final void setState(double positionMeters, double velocityMetersPerSecond) {
     setState(
         VecBuilder.fill(
             MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond));
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
index 8a81f53..9c81be8 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
@@ -8,6 +8,7 @@
 
 /** Class to control a simulated generic joystick. */
 public class GenericHIDSim {
+  /** GenericHID port. */
   protected final int m_port;
 
   /**
@@ -135,7 +136,7 @@
    */
   public boolean getOutput(int outputNumber) {
     long outputs = getOutputs();
-    return (outputs & (1 << (outputNumber - 1))) != 0;
+    return (outputs & (1L << (outputNumber - 1))) != 0;
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
index f55b038..b6f21e3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
@@ -15,6 +15,7 @@
    *
    * @param joystick joystick to simulate
    */
+  @SuppressWarnings("this-escape")
   public JoystickSim(Joystick joystick) {
     super(joystick);
     m_joystick = joystick;
@@ -29,6 +30,7 @@
    *
    * @param port port number
    */
+  @SuppressWarnings("this-escape")
   public JoystickSim(int port) {
     super(port);
     // default to a reasonable joystick configuration
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
index 73b1170..cc4db9c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
@@ -23,21 +23,24 @@
  *
  * <p>Set simulated sensor readings with the simulated positions in {@link #getOutput()}
  *
- * @param <States> The number of states of the system.
- * @param <Inputs> The number of inputs to the system.
- * @param <Outputs> The number of outputs of the system.
+ * @param <States> Number of states of the system.
+ * @param <Inputs> Number of inputs to the system.
+ * @param <Outputs> Number of outputs of the system.
  */
 public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs extends Num> {
-  // The plant that represents the linear system.
+  /** The plant that represents the linear system. */
   protected final LinearSystem<States, Inputs, Outputs> m_plant;
 
-  // Variables for state, output, and input.
+  /** State vector. */
   protected Matrix<States, N1> m_x;
-  protected Matrix<Outputs, N1> m_y;
+
+  /** Input vector. */
   protected Matrix<Inputs, N1> m_u;
 
-  // The standard deviations of measurements, used for adding noise
-  // to the measurements.
+  /** Output vector. */
+  protected Matrix<Outputs, N1> m_y;
+
+  /** The standard deviations of measurements, used for adding noise to the measurements. */
   protected final Matrix<Outputs, N1> m_measurementStdDevs;
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java
index fa19760..822b1db 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java
@@ -13,6 +13,7 @@
    *
    * @param joystick controller to simulate
    */
+  @SuppressWarnings("this-escape")
   public PS4ControllerSim(PS4Controller joystick) {
     super(joystick);
     setAxisCount(6);
@@ -25,6 +26,7 @@
    *
    * @param port port number
    */
+  @SuppressWarnings("this-escape")
   public PS4ControllerSim(int port) {
     super(port);
     setAxisCount(6);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS5ControllerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS5ControllerSim.java
index d886bd6..e11cdd0 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS5ControllerSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS5ControllerSim.java
@@ -13,6 +13,7 @@
    *
    * @param joystick controller to simulate
    */
+  @SuppressWarnings("this-escape")
   public PS5ControllerSim(PS5Controller joystick) {
     super(joystick);
     setAxisCount(6);
@@ -25,6 +26,7 @@
    *
    * @param port port number
    */
+  @SuppressWarnings("this-escape")
   public PS5ControllerSim(int port) {
     super(port);
     setAxisCount(6);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PneumaticsBaseSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PneumaticsBaseSim.java
index 156562b..dde69c1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PneumaticsBaseSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PneumaticsBaseSim.java
@@ -10,6 +10,7 @@
 
 /** Common base class for pneumatics module simulation classes. */
 public abstract class PneumaticsBaseSim {
+  /** PneumaticsBase index. */
   protected final int m_index;
 
   /**
@@ -30,10 +31,20 @@
     }
   }
 
+  /**
+   * Constructs a PneumaticsBaseSim with the given index.
+   *
+   * @param index The index.
+   */
   protected PneumaticsBaseSim(int index) {
     m_index = index;
   }
 
+  /**
+   * Constructs a PneumaticsBaseSim for the given module.
+   *
+   * @param module The module.
+   */
   protected PneumaticsBaseSim(PneumaticsBase module) {
     this(module.getModuleNumber());
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
index 8b28b6d..e2bc895 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
@@ -6,6 +6,8 @@
 
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.hal.simulation.RoboRioDataJNI;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.RobotController.RadioLEDState;
 
 /** A utility class to control a simulated RoboRIO. */
 public final class RoboRioSim {
@@ -625,6 +627,38 @@
     RoboRioDataJNI.setComments(comments);
   }
 
+  /**
+   * Register a callback to be run whenever the Radio led state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerRadioLEDStateCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerRadioLEDStateCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelRadioLEDStateCallback);
+  }
+
+  /**
+   * Get the state of the radio led.
+   *
+   * @return The state of the radio led.
+   */
+  public static RadioLEDState getRadioLEDState() {
+    return RadioLEDState.fromValue(RoboRioDataJNI.getRadioLEDState());
+  }
+
+  /**
+   * Set the state of the radio led.
+   *
+   * @param state The state of the radio led.
+   */
+  public static void setRadioLEDState(RobotController.RadioLEDState state) {
+    RoboRioDataJNI.setRadioLEDState(state.value);
+  }
+
   /** Reset all simulation data. */
   public static void resetData() {
     RoboRioDataJNI.resetData();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
index 6c0b9e9..8ce9554 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
@@ -74,6 +74,13 @@
     return new CallbackStore(m_index, uid, SPIDataJNI::cancelWriteCallback);
   }
 
+  /**
+   * Register a callback to be run whenever an auto receive buffer is received.
+   *
+   * @param callback the callback
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerReadAutoReceiveBufferCallback(
       SpiReadAutoReceiveBufferCallback callback) {
     int uid = SPIDataJNI.registerReadAutoReceiveBufferCallback(m_index, callback);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
index 1664081..f637b01 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.hal.simulation.SimulatorJNI;
 
+/** Simulation hooks. */
 public final class SimHooks {
   private SimHooks() {}
 
@@ -18,14 +19,21 @@
     SimulatorJNI.setRuntimeType(type);
   }
 
+  /** Waits until the user program has started. */
   public static void waitForProgramStart() {
     SimulatorJNI.waitForProgramStart();
   }
 
+  /** Sets that the user program has started. */
   public static void setProgramStarted() {
     SimulatorJNI.setProgramStarted();
   }
 
+  /**
+   * Returns true if the user program has started.
+   *
+   * @return True if the user program has started.
+   */
   public static boolean getProgramStarted() {
     return SimulatorJNI.getProgramStarted();
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
index af2ee85..46e0441 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
@@ -49,6 +49,7 @@
    * @param startingAngleRads The initial position of the Arm simulation in radians.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
+  @SuppressWarnings("this-escape")
   public SingleJointedArmSim(
       LinearSystem<N2, N1, N1> plant,
       DCMotor gearbox,
@@ -180,7 +181,7 @@
    * @param angleRadians The new angle in radians.
    * @param velocityRadPerSec The new angular velocity in radians per second.
    */
-  public void setState(double angleRadians, double velocityRadPerSec) {
+  public final void setState(double angleRadians, double velocityRadPerSec) {
     setState(
         VecBuilder.fill(MathUtil.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec));
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java
index dc4ebaa..2ad42e7 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java
@@ -36,14 +36,29 @@
     m_simRange = simDevice.getDouble("Range (in)");
   }
 
+  /**
+   * Sets if the range measurement is valid.
+   *
+   * @param valid True if valid
+   */
   public void setRangeValid(boolean valid) {
     m_simRangeValid.set(valid);
   }
 
+  /**
+   * Sets the range measurement.
+   *
+   * @param inches The range in inches.
+   */
   public void setRangeInches(double inches) {
     m_simRange.set(inches);
   }
 
+  /**
+   * Sets the range measurement.
+   *
+   * @param meters The range in meters.
+   */
   public void setRangeMeters(double meters) {
     m_simRange.set(Units.metersToInches(meters));
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
index 41d200e..37e0bda 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
@@ -13,6 +13,7 @@
    *
    * @param joystick controller to simulate
    */
+  @SuppressWarnings("this-escape")
   public XboxControllerSim(XboxController joystick) {
     super(joystick);
     setAxisCount(6);
@@ -25,6 +26,7 @@
    *
    * @param port port number
    */
+  @SuppressWarnings("this-escape")
   public XboxControllerSim(int port) {
     super(port);
     setAxisCount(6);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
index bfacb42..4b02c4f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
@@ -31,6 +31,7 @@
  */
 public class Field2d implements NTSendable, AutoCloseable {
   /** Constructor. */
+  @SuppressWarnings("this-escape")
   public Field2d() {
     FieldObject2d obj = new FieldObject2d("Robot");
     obj.setPose(new Pose2d());
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
index 9ffccde..0fecbb6 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.networktables.DoubleArrayEntry;
 import java.util.ArrayList;
+import java.util.Collections;
 import java.util.List;
 
 /** Game field object on a Field2d. */
@@ -70,9 +71,7 @@
    */
   public synchronized void setPoses(List<Pose2d> poses) {
     m_poses.clear();
-    for (Pose2d pose : poses) {
-      m_poses.add(pose);
-    }
+    m_poses.addAll(poses);
     updateEntry();
   }
 
@@ -83,9 +82,7 @@
    */
   public synchronized void setPoses(Pose2d... poses) {
     m_poses.clear();
-    for (Pose2d pose : poses) {
-      m_poses.add(pose);
-    }
+    Collections.addAll(m_poses, poses);
     updateEntry();
   }
 
@@ -109,7 +106,7 @@
    */
   public synchronized List<Pose2d> getPoses() {
     updateFromEntry();
-    return new ArrayList<Pose2d>(m_poses);
+    return new ArrayList<>(m_poses);
   }
 
   void updateEntry() {
@@ -143,7 +140,7 @@
       return;
     }
 
-    double[] arr = m_entry.get((double[]) null);
+    double[] arr = m_entry.get(null);
     if (arr != null) {
       if ((arr.length % 3) != 0) {
         return;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
index 19a3fc1..9d0efa2 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
@@ -31,9 +31,9 @@
   /** Runs all posted tasks. Called periodically from main thread. */
   public void runListenerTasks() {
     // Locally copy tasks from internal list; minimizes blocking time
-    Collection<Runnable> tasks = new ArrayList<>();
+    Collection<Runnable> tasks;
     synchronized (m_lock) {
-      tasks.addAll(m_tasks);
+      tasks = new ArrayList<>(m_tasks);
       m_tasks.clear();
     }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
index 28484a3..ea7fda3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
@@ -82,7 +82,7 @@
    *
    * @param degrees the angle in degrees
    */
-  public synchronized void setAngle(double degrees) {
+  public final synchronized void setAngle(double degrees) {
     m_angle = degrees;
     if (m_angleEntry != null) {
       m_angleEntry.set(degrees);
@@ -115,7 +115,7 @@
    *
    * @param length the line length
    */
-  public synchronized void setLength(double length) {
+  public final synchronized void setLength(double length) {
     m_length = length;
     if (m_lengthEntry != null) {
       m_lengthEntry.set(length);
@@ -139,7 +139,7 @@
    *
    * @param color the color of the line
    */
-  public synchronized void setColor(Color8Bit color) {
+  public final synchronized void setColor(Color8Bit color) {
     m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
     if (m_colorEntry != null) {
       m_colorEntry.set(m_color);
@@ -177,7 +177,7 @@
    *
    * @param weight the line thickness
    */
-  public synchronized void setLineWeight(double weight) {
+  public final synchronized void setLineWeight(double weight) {
     m_weight = weight;
     if (m_weightEntry != null) {
       m_weightEntry.set(weight);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java
index 754bd92..3dbcf5c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java
@@ -74,6 +74,11 @@
    */
   protected abstract void updateEntries(NetworkTable table);
 
+  /**
+   * Retrieve the object's name.
+   *
+   * @return the object's name relative to its parent.
+   */
   public final String getName() {
     return m_name;
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
index 2c409e8..f15198f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
@@ -57,6 +57,7 @@
 import java.util.function.LongSupplier;
 import java.util.function.Supplier;
 
+/** Implementation detail for SendableBuilder. */
 @SuppressWarnings("PMD.CompareObjectsWithEquals")
 public class SendableBuilderImpl implements NTSendableBuilder {
   @FunctionalInterface
@@ -109,6 +110,9 @@
 
   private final List<AutoCloseable> m_closeables = new ArrayList<>();
 
+  /** Default constructor. */
+  public SendableBuilderImpl() {}
+
   @Override
   @SuppressWarnings("PMD.AvoidCatchingGenericException")
   public void close() {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
index 7f3b93c..e905ebc 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
@@ -53,6 +53,7 @@
   private static final AtomicInteger s_instances = new AtomicInteger();
 
   /** Instantiates a {@link SendableChooser}. */
+  @SuppressWarnings("this-escape")
   public SendableChooser() {
     m_instance = s_instances.getAndIncrement();
     SendableRegistry.add(this, "SendableChooser", m_instance);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
index 8a8f147..7da170b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
+import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTable;
@@ -35,9 +36,10 @@
   /** The executor for listener tasks; calls listener tasks synchronously from main thread. */
   private static final ListenerExecutor listenerExecutor = new ListenerExecutor();
 
+  private static boolean m_reported = false; // NOPMD redundant field initializer
+
   static {
     setNetworkTableInstance(NetworkTableInstance.getDefault());
-    HAL.report(tResourceType.kResourceType_SmartDashboard, 0);
   }
 
   private SmartDashboard() {
@@ -64,6 +66,10 @@
    */
   @SuppressWarnings("PMD.CompareObjectsWithEquals")
   public static synchronized void putData(String key, Sendable data) {
+    if (!m_reported) {
+      HAL.report(tResourceType.kResourceType_SmartDashboard, tInstances.kSmartDashboard_Instance);
+      m_reported = true;
+    }
     Sendable sddata = tablesToData.get(key);
     if (sddata == null || sddata != data) {
       tablesToData.put(key, data);
@@ -114,6 +120,10 @@
    * @return Network table entry.
    */
   public static NetworkTableEntry getEntry(String key) {
+    if (!m_reported) {
+      HAL.report(tResourceType.kResourceType_SmartDashboard, tInstances.kSmartDashboard_Instance);
+      m_reported = true;
+    }
     return table.getEntry(key);
   }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java
new file mode 100644
index 0000000..38bc46f
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java
@@ -0,0 +1,220 @@
+// 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.
+
+package edu.wpi.first.wpilibj.sysid;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Current;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.util.datalog.StringLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Utility for logging data from a SysId test routine. Each complete routine (quasistatic and
+ * dynamic, forward and reverse) should have its own SysIdRoutineLog instance, with a unique log
+ * name.
+ */
+public class SysIdRoutineLog {
+  private final Map<String, Map<String, DoubleLogEntry>> m_logEntries = new HashMap<>();
+  private final String m_logName;
+  private final StringLogEntry m_state;
+
+  /**
+   * Create a new logging utility for a SysId test routine.
+   *
+   * @param logName The name for the test routine in the log. Should be unique between complete test
+   *     routines (quasistatic and dynamic, forward and reverse). The current state of this test
+   *     (e.g. "quasistatic-forward") will appear in WPILog under the "sysid-test-state-logName"
+   *     entry.
+   */
+  public SysIdRoutineLog(String logName) {
+    m_logName = logName;
+    m_state = new StringLogEntry(DataLogManager.getLog(), "sysid-test-state-" + logName);
+    m_state.append(State.kNone.toString());
+  }
+
+  /** Possible state of a SysId routine. */
+  public enum State {
+    /** Quasistatic forward test. */
+    kQuasistaticForward("quasistatic-forward"),
+    /** Quasistatic reverse test. */
+    kQuasistaticReverse("quasistatic-reverse"),
+    /** Dynamic forward test. */
+    kDynamicForward("dynamic-forward"),
+    /** Dynamic reverse test. */
+    kDynamicReverse("dynamic-reverse"),
+    /** No test. */
+    kNone("none");
+
+    private final String m_state;
+
+    State(String state) {
+      m_state = state;
+    }
+
+    @Override
+    public String toString() {
+      return m_state;
+    }
+  }
+
+  /** Logs data from a single motor during a SysIdRoutine. */
+  public class MotorLog {
+    private final String m_motorName;
+
+    /**
+     * Create a new SysId motor log handle.
+     *
+     * @param motorName The name of the motor whose data is being logged.
+     */
+    private MotorLog(String motorName) {
+      m_motorName = motorName;
+      m_logEntries.put(motorName, new HashMap<>());
+    }
+
+    /**
+     * Log a generic data value from the motor.
+     *
+     * @param name The name of the data field being recorded.
+     * @param value The numeric value of the data field.
+     * @param unit The unit string of the data field.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog value(String name, double value, String unit) {
+      var motorEntries = m_logEntries.get(m_motorName);
+      var entry = motorEntries.get(name);
+
+      if (entry == null) {
+        var log = DataLogManager.getLog();
+
+        entry = new DoubleLogEntry(log, name + "-" + m_motorName + "-" + m_logName, unit);
+        motorEntries.put(name, entry);
+      }
+
+      entry.append(value);
+      return this;
+    }
+
+    /**
+     * Log the voltage applied to the motor.
+     *
+     * @param voltage The voltage to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog voltage(Measure<Voltage> voltage) {
+      return value("voltage", voltage.in(Volts), Volts.name());
+    }
+
+    /**
+     * Log the linear position of the motor.
+     *
+     * @param position The linear position to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearPosition(Measure<Distance> position) {
+      return value("position", position.in(Meters), Meters.name());
+    }
+
+    /**
+     * Log the angular position of the motor.
+     *
+     * @param position The angular position to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularPosition(Measure<Angle> position) {
+      return value("position", position.in(Rotations), Rotations.name());
+    }
+
+    /**
+     * Log the linear velocity of the motor.
+     *
+     * @param velocity The linear velocity to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearVelocity(Measure<Velocity<Distance>> velocity) {
+      return value("velocity", velocity.in(MetersPerSecond), MetersPerSecond.name());
+    }
+
+    /**
+     * Log the angular velocity of the motor.
+     *
+     * @param velocity The angular velocity to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularVelocity(Measure<Velocity<Angle>> velocity) {
+      return value("velocity", velocity.in(RotationsPerSecond), RotationsPerSecond.name());
+    }
+
+    /**
+     * Log the linear acceleration of the motor.
+     *
+     * @param acceleration The linear acceleration to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearAcceleration(Measure<Velocity<Velocity<Distance>>> acceleration) {
+      return value(
+          "position",
+          acceleration.in(MetersPerSecond.per(Second)),
+          MetersPerSecond.per(Second).name());
+    }
+
+    /**
+     * Log the angular acceleration of the motor.
+     *
+     * @param acceleration The angular acceleration to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularAcceleration(Measure<Velocity<Velocity<Angle>>> acceleration) {
+      return value(
+          "position",
+          acceleration.in(RotationsPerSecond.per(Second)),
+          RotationsPerSecond.per(Second).name());
+    }
+
+    /**
+     * Log the current applied to the motor.
+     *
+     * @param current The current to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog current(Measure<Current> current) {
+      value("current", current.in(Amps), Amps.name());
+      return this;
+    }
+  }
+
+  /**
+   * Log data from a motor during a SysId routine.
+   *
+   * @param motorName The name of the motor.
+   * @return Handle with chainable callbacks to log individual data fields.
+   */
+  public MotorLog motor(String motorName) {
+    return new MotorLog(motorName);
+  }
+
+  /**
+   * Records the current state of the SysId test routine. Should be called once per iteration during
+   * tests with the type of the current test, and once upon test end with state `none`.
+   *
+   * @param state The current state of the SysId test routine.
+   */
+  public void recordState(State state) {
+    m_state.append(state.toString());
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
index 2defbd8..5060567 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
@@ -14,13 +14,24 @@
  */
 @SuppressWarnings("MemberName")
 public class Color {
-  private static final double kPrecision = Math.pow(2, -12);
-
+  /** Red component (0-1). */
   public final double red;
+
+  /** Green component (0-1). */
   public final double green;
+
+  /** Blue component (0-1). */
   public final double blue;
+
   private String m_name;
 
+  /** Constructs a default color (black). */
+  public Color() {
+    red = 0.0;
+    green = 0.0;
+    blue = 0.0;
+  }
+
   /**
    * Constructs a Color from doubles.
    *
@@ -70,6 +81,22 @@
   }
 
   /**
+   * Constructs a Color from a hex string.
+   *
+   * @param hexString a string of the format <code>#RRGGBB</code>
+   * @throws IllegalArgumentException if the hex string is invalid.
+   */
+  public Color(String hexString) {
+    if (hexString.length() != 7 || !hexString.startsWith("#")) {
+      throw new IllegalArgumentException("Invalid hex string \"" + hexString + "\"");
+    }
+
+    this.red = Integer.valueOf(hexString.substring(1, 3), 16) / 255.0;
+    this.green = Integer.valueOf(hexString.substring(3, 5), 16) / 255.0;
+    this.blue = Integer.valueOf(hexString.substring(5, 7), 16) / 255.0;
+  }
+
+  /**
    * Creates a Color from HSV values.
    *
    * @param h The h value [0-180)
@@ -155,8 +182,7 @@
   }
 
   private static double roundAndClamp(double value) {
-    final var rounded = Math.round((value + kPrecision / 2) / kPrecision) * kPrecision;
-    return MathUtil.clamp(rounded, 0.0, 1.0);
+    return MathUtil.clamp(Math.ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0);
   }
 
   /*
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
index 3260673..132ea64 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
@@ -10,10 +10,22 @@
 /** Represents colors with 8 bits of precision. */
 @SuppressWarnings("MemberName")
 public class Color8Bit {
+  /** Red component (0-255). */
   public final int red;
+
+  /** Green component (0-255). */
   public final int green;
+
+  /** Blue component (0-255). */
   public final int blue;
 
+  /** Constructs a default color (black). */
+  public Color8Bit() {
+    red = 0;
+    green = 0;
+    blue = 0;
+  }
+
   /**
    * Constructs a Color8Bit.
    *
@@ -36,6 +48,22 @@
     this((int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255));
   }
 
+  /**
+   * Constructs a Color8Bit from a hex string.
+   *
+   * @param hexString a string of the format <code>#RRGGBB</code>
+   * @throws IllegalArgumentException if the hex string is invalid.
+   */
+  public Color8Bit(String hexString) {
+    if (hexString.length() != 7 || !hexString.startsWith("#")) {
+      throw new IllegalArgumentException("Invalid hex string \"" + hexString + "\"");
+    }
+
+    this.red = Integer.valueOf(hexString.substring(1, 3), 16);
+    this.green = Integer.valueOf(hexString.substring(3, 5), 16);
+    this.blue = Integer.valueOf(hexString.substring(5, 7), 16);
+  }
+
   @Override
   public boolean equals(Object other) {
     if (this == other) {
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
index c1c5f3c..920fb54 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
@@ -144,11 +144,7 @@
   void disabledModeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     DriverStationSim.setEnabled(false);
@@ -231,11 +227,7 @@
   void autonomousModeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     DriverStationSim.setEnabled(true);
@@ -320,11 +312,7 @@
   void teleopModeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     DriverStationSim.setEnabled(true);
@@ -411,11 +399,7 @@
     MockRobot robot = new MockRobot();
     robot.enableLiveWindowInTest(isLW);
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     DriverStationSim.setEnabled(true);
@@ -531,11 +515,7 @@
   void modeChangeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     // Start in disabled
@@ -655,17 +635,9 @@
     MockRobot robot = new MockRobot();
 
     final AtomicInteger callbackCount = new AtomicInteger(0);
-    robot.addPeriodic(
-        () -> {
-          callbackCount.addAndGet(1);
-        },
-        kPeriod / 2.0);
+    robot.addPeriodic(() -> callbackCount.addAndGet(1), kPeriod / 2.0);
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     DriverStationSim.setEnabled(false);
@@ -704,12 +676,7 @@
     MockRobot robot = new MockRobot();
 
     final AtomicInteger callbackCount = new AtomicInteger(0);
-    robot.addPeriodic(
-        () -> {
-          callbackCount.addAndGet(1);
-        },
-        kPeriod / 2.0,
-        kPeriod / 4.0);
+    robot.addPeriodic(() -> callbackCount.addAndGet(1), kPeriod / 2.0, kPeriod / 4.0);
 
     // Expirations in this test (ms)
     //
@@ -720,11 +687,7 @@
     //     p |    0.75p
     //    2p |    1.25p
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     DriverStationSim.setEnabled(false);
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java
index ee4860d..5cff0d0 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java
@@ -54,22 +54,10 @@
     // | RobotPeriodic() |           0 |              2 |
     // | Callback 1      |           2 |            0.5 |
     // | Callback 2      |         2.5 |              1 |
-    robot.schedule(
-        () -> {
-          callbackCount1.addAndGet(1);
-        },
-        0.0005);
-    robot.schedule(
-        () -> {
-          callbackCount2.addAndGet(1);
-        },
-        0.001);
+    robot.schedule(() -> callbackCount1.addAndGet(1), 0.0005);
+    robot.schedule(() -> callbackCount2.addAndGet(1), 0.001);
 
-    Thread robotThread =
-        new Thread(
-            () -> {
-              robot.startCompetition();
-            });
+    Thread robotThread = new Thread(robot::startCompetition);
     robotThread.start();
 
     DriverStationSim.setEnabled(false);
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
index c1676e0..5e9e122 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
@@ -33,12 +33,7 @@
   void enableDisableTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog =
-        new Watchdog(
-            0.4,
-            () -> {
-              watchdogCounter.addAndGet(1);
-            })) {
+    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
       // Run 1
       watchdog.enable();
       SimHooks.stepTiming(0.2);
@@ -71,12 +66,7 @@
   void resetTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog =
-        new Watchdog(
-            0.4,
-            () -> {
-              watchdogCounter.addAndGet(1);
-            })) {
+    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
       watchdog.enable();
       SimHooks.stepTiming(0.2);
       watchdog.reset();
@@ -92,12 +82,7 @@
   void setTimeoutTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog =
-        new Watchdog(
-            1.0,
-            () -> {
-              watchdogCounter.addAndGet(1);
-            })) {
+    try (Watchdog watchdog = new Watchdog(1.0, () -> watchdogCounter.addAndGet(1))) {
       watchdog.enable();
       SimHooks.stepTiming(0.2);
       watchdog.setTimeout(0.2);
@@ -137,12 +122,7 @@
   void epochsTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog =
-        new Watchdog(
-            0.4,
-            () -> {
-              watchdogCounter.addAndGet(1);
-            })) {
+    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
       // Run 1
       watchdog.enable();
       watchdog.addEpoch("Epoch 1");
@@ -173,18 +153,8 @@
     final AtomicInteger watchdogCounter1 = new AtomicInteger(0);
     final AtomicInteger watchdogCounter2 = new AtomicInteger(0);
 
-    try (Watchdog watchdog1 =
-            new Watchdog(
-                0.2,
-                () -> {
-                  watchdogCounter1.addAndGet(1);
-                });
-        Watchdog watchdog2 =
-            new Watchdog(
-                0.6,
-                () -> {
-                  watchdogCounter2.addAndGet(1);
-                })) {
+    try (Watchdog watchdog1 = new Watchdog(0.2, () -> watchdogCounter1.addAndGet(1));
+        Watchdog watchdog2 = new Watchdog(0.6, () -> watchdogCounter2.addAndGet(1))) {
       watchdog2.enable();
       SimHooks.stepTiming(0.25);
       assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java
index 4675a7f..474dc89 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java
@@ -6,7 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MockPWMMotorController;
 import org.junit.jupiter.api.Test;
 
 class DifferentialDriveTest {
@@ -250,9 +250,9 @@
 
   @Test
   void testArcadeDrive() {
-    var left = new MockMotorController();
-    var right = new MockMotorController();
-    var drive = new DifferentialDrive(left, right);
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
     drive.setDeadband(0.0);
 
     // Forward
@@ -288,9 +288,9 @@
 
   @Test
   void testArcadeDriveSquared() {
-    var left = new MockMotorController();
-    var right = new MockMotorController();
-    var drive = new DifferentialDrive(left, right);
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
     drive.setDeadband(0.0);
 
     // Forward
@@ -326,9 +326,9 @@
 
   @Test
   void testCurvatureDrive() {
-    var left = new MockMotorController();
-    var right = new MockMotorController();
-    var drive = new DifferentialDrive(left, right);
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
     drive.setDeadband(0.0);
 
     // Forward
@@ -364,9 +364,9 @@
 
   @Test
   void testCurvatureDriveTurnInPlace() {
-    var left = new MockMotorController();
-    var right = new MockMotorController();
-    var drive = new DifferentialDrive(left, right);
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
     drive.setDeadband(0.0);
 
     // Forward
@@ -402,9 +402,9 @@
 
   @Test
   void testTankDrive() {
-    var left = new MockMotorController();
-    var right = new MockMotorController();
-    var drive = new DifferentialDrive(left, right);
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
     drive.setDeadband(0.0);
 
     // Forward
@@ -440,9 +440,9 @@
 
   @Test
   void testTankDriveSquared() {
-    var left = new MockMotorController();
-    var right = new MockMotorController();
-    var drive = new DifferentialDrive(left, right);
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
     drive.setDeadband(0.0);
 
     // Forward
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java
index 034ba10..a1017d5 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java
@@ -7,7 +7,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MockPWMMotorController;
 import org.junit.jupiter.api.Test;
 
 class MecanumDriveTest {
@@ -89,11 +89,11 @@
 
   @Test
   void testCartesian() {
-    var fl = new MockMotorController();
-    var fr = new MockMotorController();
-    var rl = new MockMotorController();
-    var rr = new MockMotorController();
-    var drive = new MecanumDrive(fl, rl, fr, rr);
+    var fl = new MockPWMMotorController();
+    var rl = new MockPWMMotorController();
+    var fr = new MockPWMMotorController();
+    var rr = new MockPWMMotorController();
+    var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
     drive.setDeadband(0.0);
 
     // Forward
@@ -134,11 +134,11 @@
 
   @Test
   void testCartesianGyro90CW() {
-    var fl = new MockMotorController();
-    var fr = new MockMotorController();
-    var rl = new MockMotorController();
-    var rr = new MockMotorController();
-    var drive = new MecanumDrive(fl, rl, fr, rr);
+    var fl = new MockPWMMotorController();
+    var rl = new MockPWMMotorController();
+    var fr = new MockPWMMotorController();
+    var rr = new MockPWMMotorController();
+    var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
     drive.setDeadband(0.0);
 
     // Forward in global frame; left in robot frame
@@ -179,11 +179,11 @@
 
   @Test
   void testPolar() {
-    var fl = new MockMotorController();
-    var fr = new MockMotorController();
-    var rl = new MockMotorController();
-    var rr = new MockMotorController();
-    var drive = new MecanumDrive(fl, rl, fr, rr);
+    var fl = new MockPWMMotorController();
+    var rl = new MockPWMMotorController();
+    var fr = new MockPWMMotorController();
+    var rr = new MockPWMMotorController();
+    var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
     drive.setDeadband(0.0);
 
     // Forward
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/EventLoopTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/EventLoopTest.java
index ea3c569..644c18e 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/EventLoopTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/EventLoopTest.java
@@ -5,7 +5,9 @@
 package edu.wpi.first.wpilibj.event;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
+import java.util.ConcurrentModificationException;
 import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.concurrent.atomic.AtomicInteger;
 import org.junit.jupiter.api.Test;
@@ -58,4 +60,33 @@
     // shouldn't change
     assertEquals(1, counter.get());
   }
+
+  @Test
+  void testConcurrentModification() {
+    var loop = new EventLoop();
+
+    loop.bind(
+        () -> {
+          assertThrows(
+              ConcurrentModificationException.class,
+              () -> {
+                loop.bind(() -> {});
+              });
+        });
+
+    loop.poll();
+
+    loop.clear();
+
+    loop.bind(
+        () -> {
+          assertThrows(
+              ConcurrentModificationException.class,
+              () -> {
+                loop.clear();
+              });
+        });
+
+    loop.poll();
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
index e7a6b8e..4ac3dc8 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.motorcontrol;
 
+@SuppressWarnings("removal")
 public class MockMotorController implements MotorController {
   private double m_speed;
   private boolean m_isInverted;
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockPWMMotorController.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockPWMMotorController.java
new file mode 100644
index 0000000..bf2c287
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockPWMMotorController.java
@@ -0,0 +1,34 @@
+// 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.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+public class MockPWMMotorController {
+  private double m_speed;
+  private boolean m_isInverted;
+
+  public void set(double speed) {
+    m_speed = m_isInverted ? -speed : speed;
+  }
+
+  public double get() {
+    return m_speed;
+  }
+
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  public void disable() {
+    m_speed = 0;
+  }
+
+  public void stopMotor() {
+    disable();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroupTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroupTest.java
index 6efb3de..a8f71f6 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroupTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroupTest.java
@@ -15,6 +15,7 @@
 import org.junit.jupiter.params.provider.Arguments;
 import org.junit.jupiter.params.provider.MethodSource;
 
+@SuppressWarnings("removal")
 class MotorControllerGroupTest {
   private static Stream<Arguments> motorControllerArguments() {
     return IntStream.of(1, 2, 3)
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
index 04397c2..a630c3e 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
@@ -10,6 +10,7 @@
 
 /** A mock sendable that marks itself as an actuator. */
 public class MockActuatorSendable implements Sendable {
+  @SuppressWarnings("this-escape")
   public MockActuatorSendable(String name) {
     SendableRegistry.add(this, name);
   }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
index 6283c68..25b545b 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
@@ -136,7 +136,7 @@
     try (StringSubscriber subscriber =
         m_ntInstance
             .getStringTopic("/Shuffleboard/.metadata/Selected")
-            .subscribe("", PubSubOption.keepDuplicates(true)); ) {
+            .subscribe("", PubSubOption.keepDuplicates(true))) {
       listener =
           m_ntInstance.addListener(
               subscriber,
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSimTest.java
index 096f77e..4ddadf1 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSimTest.java
@@ -79,7 +79,7 @@
     BufferCallback callback = new BufferCallback();
 
     try (AddressableLED led = new AddressableLED(0);
-        CallbackStore cb = sim.registerDataCallback(callback); ) {
+        CallbackStore cb = sim.registerDataCallback(callback)) {
       assertFalse(sim.getRunning());
 
       assertEquals(1, sim.getLength()); // Defaults to 1 led
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java
index fbe3af0..681e6b4 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java
@@ -63,7 +63,7 @@
 
     try (var motor = new PWMVictorSPX(0);
         var encoder = new Encoder(0, 1);
-        var controller = new PIDController(0.04, 0.0, 0.001); ) {
+        var controller = new PIDController(0.04, 0.0, 0.001)) {
       var encoderSim = new EncoderSim(encoder);
       encoderSim.resetData();
 
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
index 5baed3a..cbb8d18 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
@@ -76,11 +76,11 @@
       sim.update(0.020);
 
       // Update our ground truth
-      groundTruthX = NumericalIntegration.rk4(sim::getDynamics, groundTruthX, voltages, 0.020);
+      groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020);
     }
 
     // 2 inch tolerance is OK since our ground truth is an approximation of the
-    // ODE solution using RK4 anyway
+    // ODE solution using RKDP anyway
     assertEquals(
         groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
         sim.getState(DifferentialDrivetrainSim.State.kX),
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSimTest.java
index 9f29aac..8a0fc7c 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSimTest.java
@@ -25,7 +25,7 @@
       BooleanCallback initializeCallback = new BooleanCallback();
       DoubleCallback dutyCycleCallback = new DoubleCallback();
       try (CallbackStore initCb = sim.registerInitializedCallback(initializeCallback, false);
-          CallbackStore dutyCycleCb = sim.registerDutyCycleCallback(dutyCycleCallback, false); ) {
+          CallbackStore dutyCycleCb = sim.registerDutyCycleCallback(dutyCycleCallback, false)) {
         final double kTestDutyCycle = 0.191;
         output.enablePWM(kTestDutyCycle);
 
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RelaySimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RelaySimTest.java
index 3918361..c7538cc 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RelaySimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RelaySimTest.java
@@ -58,7 +58,7 @@
 
     try (CallbackStore fwdCb = sim.registerInitializedForwardCallback(forwardCallback, false);
         CallbackStore revCb = sim.registerInitializedReverseCallback(reverseCallback, false);
-        Relay relay = new Relay(0, Relay.Direction.kForward); ) {
+        Relay relay = new Relay(0, Relay.Direction.kForward)) {
       assertTrue(sim.getInitializedForward());
       assertFalse(sim.getInitializedReverse());
 
@@ -84,7 +84,7 @@
 
     try (CallbackStore fwdCb = sim.registerInitializedForwardCallback(forwardCallback, false);
         CallbackStore revCb = sim.registerInitializedReverseCallback(reverseCallback, false);
-        Relay relay = new Relay(0, Relay.Direction.kReverse); ) {
+        Relay relay = new Relay(0, Relay.Direction.kReverse)) {
       assertFalse(sim.getInitializedForward());
       assertTrue(sim.getInitializedReverse());
 
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java
index c016ec4..01029cd 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java
@@ -10,8 +10,10 @@
 
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.RobotController.RadioLEDState;
 import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
 import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.EnumCallback;
 import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
 import org.junit.jupiter.api.Test;
 
@@ -279,4 +281,26 @@
     assertEquals(kCommentsTruncated, RoboRioSim.getComments());
     assertEquals(kCommentsTruncated, RobotController.getComments());
   }
+
+  @Test
+  void testRadioLEDState() {
+    RoboRioSim.resetData();
+
+    EnumCallback callback = new EnumCallback();
+    try (CallbackStore cb = RoboRioSim.registerRadioLEDStateCallback(callback, false)) {
+      RobotController.setRadioLEDState(RadioLEDState.kGreen);
+      assertTrue(callback.wasTriggered());
+      assertEquals(RadioLEDState.kGreen.value, callback.getSetValue());
+      assertEquals(RadioLEDState.kGreen, RoboRioSim.getRadioLEDState());
+      assertEquals(RadioLEDState.kGreen, RobotController.getRadioLEDState());
+
+      callback.reset();
+
+      RoboRioSim.setRadioLEDState(RadioLEDState.kOrange);
+      assertTrue(callback.wasTriggered());
+      assertEquals(RadioLEDState.kOrange.value, callback.getSetValue());
+      assertEquals(RadioLEDState.kOrange, RoboRioSim.getRadioLEDState());
+      assertEquals(RadioLEDState.kOrange, RobotController.getRadioLEDState());
+    }
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
index 62a24d1..89b4333 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
@@ -41,18 +41,10 @@
         SimDevice dev1 = SimDevice.create("testDC1")) {
       try (CallbackStore callback1 =
               SimDeviceSim.registerDeviceCreatedCallback(
-                  "testDC",
-                  (name, handle) -> {
-                    callback1Counter.addAndGet(1);
-                  },
-                  false);
+                  "testDC", (name, handle) -> callback1Counter.addAndGet(1), false);
           CallbackStore callback2 =
               SimDeviceSim.registerDeviceCreatedCallback(
-                  "testDC",
-                  (name, handle) -> {
-                    callback2Counter.addAndGet(1);
-                  },
-                  true)) {
+                  "testDC", (name, handle) -> callback2Counter.addAndGet(1), true)) {
         assertEquals(0, callback1Counter.get(), "Callback 1 called early");
         assertEquals(
             1,
@@ -82,11 +74,7 @@
     SimDevice dev1 = SimDevice.create("testDF1");
     try (CallbackStore callback =
         SimDeviceSim.registerDeviceFreedCallback(
-            "testDF",
-            (name, handle) -> {
-              counter.addAndGet(1);
-            },
-            false)) {
+            "testDF", (name, handle) -> counter.addAndGet(1), false)) {
       assertEquals(0, counter.get(), "Callback called early");
       dev1.close();
       assertEquals(1, counter.get(), "Callback called either more than once or not at all");
@@ -108,16 +96,10 @@
       SimDeviceSim sim = new SimDeviceSim("testVM1");
       try (CallbackStore callback1 =
               sim.registerValueCreatedCallback(
-                  (name, handle, readonly, value) -> {
-                    callback1Counter.addAndGet(1);
-                  },
-                  false);
+                  (name, handle, readonly, value) -> callback1Counter.addAndGet(1), false);
           CallbackStore callback2 =
               sim.registerValueCreatedCallback(
-                  (name, handle, readonly, value) -> {
-                    callback2Counter.addAndGet(1);
-                  },
-                  true)) {
+                  (name, handle, readonly, value) -> callback2Counter.addAndGet(1), true)) {
         assertEquals(0, callback1Counter.get(), "Callback 1 called early");
         assertEquals(
             1,
@@ -148,18 +130,10 @@
       SimValue simVal = sim.getValue("v1");
       try (CallbackStore callback1 =
               sim.registerValueChangedCallback(
-                  simVal,
-                  (name, handle, readonly, value) -> {
-                    callback1Counter.addAndGet(1);
-                  },
-                  false);
+                  simVal, (name, handle, readonly, value) -> callback1Counter.addAndGet(1), false);
           CallbackStore callback2 =
               sim.registerValueChangedCallback(
-                  simVal,
-                  (name, handle, readonly, value) -> {
-                    callback2Counter.addAndGet(1);
-                  },
-                  true)) {
+                  simVal, (name, handle, readonly, value) -> callback2Counter.addAndGet(1), true)) {
         assertEquals(0, callback1Counter.get(), "Callback 1 called early");
         assertEquals(
             1,
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooserTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooserTest.java
index 07f09af..d97d54c 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooserTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooserTest.java
@@ -75,7 +75,7 @@
         chooser.addOption(String.valueOf(i), i);
       }
       AtomicInteger currentVal = new AtomicInteger();
-      chooser.onChange(val -> currentVal.set(val));
+      chooser.onChange(currentVal::set);
 
       SmartDashboard.putData("changeListenerChooser", chooser);
       SmartDashboard.updateValues();
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/Color8BitTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/Color8BitTest.java
new file mode 100644
index 0000000..499ecbd
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/Color8BitTest.java
@@ -0,0 +1,65 @@
+// 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.
+
+package edu.wpi.first.wpilibj.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import org.junit.jupiter.api.Test;
+
+class Color8BitTest {
+  @Test
+  void testConstructDefault() {
+    var color = new Color8Bit();
+
+    assertEquals(0, color.red);
+    assertEquals(0, color.green);
+    assertEquals(0, color.blue);
+  }
+
+  @Test
+  void testConstructFromInts() {
+    var color = new Color8Bit(255, 128, 64);
+
+    assertEquals(255, color.red);
+    assertEquals(128, color.green);
+    assertEquals(64, color.blue);
+  }
+
+  @Test
+  void testConstructFromColor() {
+    var color = new Color8Bit(new Color(255, 128, 64));
+
+    assertEquals(255, color.red);
+    assertEquals(128, color.green);
+    assertEquals(64, color.blue);
+  }
+
+  @Test
+  void testConstructFromHexString() {
+    var color = new Color8Bit("#FF8040");
+
+    assertEquals(255, color.red);
+    assertEquals(128, color.green);
+    assertEquals(64, color.blue);
+
+    // No leading #
+    assertThrows(IllegalArgumentException.class, () -> new Color8Bit("112233"));
+
+    // Too long
+    assertThrows(IllegalArgumentException.class, () -> new Color8Bit("#11223344"));
+
+    // Invalid hex characters
+    assertThrows(IllegalArgumentException.class, () -> new Color8Bit("#$$$$$$"));
+  }
+
+  @Test
+  void testToHexString() {
+    var color = new Color8Bit(255, 128, 64);
+
+    assertEquals("#FF8040", color.toHexString());
+    assertEquals("#FF8040", color.toString());
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java
new file mode 100644
index 0000000..1b32afd
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java
@@ -0,0 +1,85 @@
+// 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.
+
+package edu.wpi.first.wpilibj.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import org.junit.jupiter.api.Test;
+
+class ColorTest {
+  @Test
+  void testConstructDefault() {
+    var color = new Color();
+
+    assertEquals(0.0, color.red);
+    assertEquals(0.0, color.green);
+    assertEquals(0.0, color.blue);
+  }
+
+  @Test
+  void testConstructFromDoubles() {
+    {
+      var color = new Color(1.0, 0.5, 0.25);
+
+      assertEquals(1.0, color.red, 1e-2);
+      assertEquals(0.5, color.green, 1e-2);
+      assertEquals(0.25, color.blue, 1e-2);
+    }
+
+    {
+      var color = new Color(1.0, 0.0, 0.0);
+
+      // Check for exact match to ensure round-and-clamp is correct
+      assertEquals(1.0, color.red);
+      assertEquals(0.0, color.green);
+      assertEquals(0.0, color.blue);
+    }
+  }
+
+  @Test
+  void testConstructFromInts() {
+    var color = new Color(255, 128, 64);
+
+    assertEquals(1.0, color.red, 1e-2);
+    assertEquals(0.5, color.green, 1e-2);
+    assertEquals(0.25, color.blue, 1e-2);
+  }
+
+  @Test
+  void testConstructFromHexString() {
+    var color = new Color("#FF8040");
+
+    assertEquals(1.0, color.red, 1e-2);
+    assertEquals(0.5, color.green, 1e-2);
+    assertEquals(0.25, color.blue, 1e-2);
+
+    // No leading #
+    assertThrows(IllegalArgumentException.class, () -> new Color("112233"));
+
+    // Too long
+    assertThrows(IllegalArgumentException.class, () -> new Color("#11223344"));
+
+    // Invalid hex characters
+    assertThrows(IllegalArgumentException.class, () -> new Color("#$$$$$$"));
+  }
+
+  @Test
+  void testFromHSV() {
+    var color = Color.fromHSV(90, 128, 64);
+
+    assertEquals(0.125732421875, color.red);
+    assertEquals(0.251220703125, color.green);
+    assertEquals(0.251220703125, color.blue);
+  }
+
+  @Test
+  void testToHexString() {
+    var color = new Color(255, 128, 64);
+
+    assertEquals("#FF8040", color.toHexString());
+    assertEquals("#FF8040", color.toString());
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/build.gradle b/third_party/allwpilib/wpilibjExamples/build.gradle
index d5f71e9..4caafc2 100644
--- a/third_party/allwpilib/wpilibjExamples/build.gradle
+++ b/third_party/allwpilib/wpilibjExamples/build.gradle
@@ -25,9 +25,8 @@
     implementation project(':romiVendordep')
     implementation project(':xrpVendordep')
 
-    testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0'
-    testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0'
-    testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0'
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 }
 
 jacoco {
@@ -95,11 +94,8 @@
                 }
             }
             binaries.all { binary ->
-                lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
-                lib project: ':romiVendordep', library: 'romiVendordep', linkage: 'shared'
-                lib project: ':xrpVendordep', library: 'xrpVendordep', linkage: 'shared'
                 lib project: ':apriltag', library: 'apriltag', linkage: 'shared'
-                lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                lib project: ':apriltag', library: 'apriltagJNIShared', linkage: 'shared'
                 lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                 lib project: ':wpimath', library: 'wpimathJNIShared', linkage: 'shared'
                 project(':ntcore').addNtcoreDependency(binary, 'shared')
@@ -112,7 +108,6 @@
                 lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
                 lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
                 lib project: ':wpinet', library: 'wpinetJNIShared', linkage: 'shared'
-                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                     nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
                 } else {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
index 5ed9fda..fa55f45 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
@@ -22,8 +22,8 @@
           // Use current trajectory state here
         },
         // Goal state
-        () -> new TrapezoidProfile.State(),
+        TrapezoidProfile.State::new,
         // Current state
-        () -> new TrapezoidProfile.State());
+        TrapezoidProfile.State::new);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java
index a73ea0b..3d265d4 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java
@@ -32,21 +32,21 @@
 public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
-    var visionThread = new Thread(() -> apriltagVisionThreadProc());
+    var visionThread = new Thread(this::apriltagVisionThreadProc);
     visionThread.setDaemon(true);
     visionThread.start();
   }
 
   void apriltagVisionThreadProc() {
     var detector = new AprilTagDetector();
-    // look for tag16h5, don't correct any error bits
-    detector.addFamily("tag16h5", 0);
+    // look for tag136h11, correct 7 error bits
+    detector.addFamily("tag36h11", 7);
 
     // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
     // (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
     var poseEstConfig =
         new AprilTagPoseEstimator.Config(
-            0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522);
+            0.1651, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522);
     var estimator = new AprilTagPoseEstimator(poseEstConfig);
 
     // Get the UsbCamera from CameraServer
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
index 45333a5..aa34379 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.arcadedrive;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -16,9 +17,15 @@
 public class Robot extends TimedRobot {
   private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
   private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
-  private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
   private final Joystick m_stick = new Joystick(0);
 
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+    SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+  }
+
   @Override
   public void robotInit() {
     // We need to invert one side of the drivetrain so that positive voltages
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
index aa0f9a2..a376fe0 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -16,9 +17,15 @@
 public class Robot extends TimedRobot {
   private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
   private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
-  private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
   private final XboxController m_driverController = new XboxController(0);
 
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+    SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+  }
+
   @Override
   public void robotInit() {
     // We need to invert one side of the drivetrain so that positive voltages
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
index 4ecaa77..246b5c4 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
@@ -4,28 +4,25 @@
 
 package edu.wpi.first.wpilibj.examples.armbot.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -43,14 +40,20 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
index 4f95b40..7801a1f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard;
 
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor controller.
  *
  * <p>Has no actual functionality.
  */
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
   public enum PIDMode {
     kPosition,
     kVelocity,
@@ -72,25 +70,19 @@
   /** Resets the encoder to zero distance. */
   public void resetEncoder() {}
 
-  @Override
   public void set(double speed) {}
 
-  @Override
   public double get() {
     return 0;
   }
 
-  @Override
   public void setInverted(boolean isInverted) {}
 
-  @Override
   public boolean getInverted() {
     return false;
   }
 
-  @Override
   public void disable() {}
 
-  @Override
   public void stopMotor() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
index e3c9159..f01ad99 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
@@ -4,10 +4,10 @@
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -16,19 +16,16 @@
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -46,14 +43,20 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
index 8b4734a..adc52e2 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -12,8 +12,6 @@
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /** Represents a differential drive style drivetrain. */
@@ -25,19 +23,14 @@
   private static final double kWheelRadius = 0.0508; // meters
   private static final int kEncoderResolution = 4096;
 
-  private final MotorController m_leftLeader = new PWMSparkMax(1);
-  private final MotorController m_leftFollower = new PWMSparkMax(2);
-  private final MotorController m_rightLeader = new PWMSparkMax(3);
-  private final MotorController m_rightFollower = new PWMSparkMax(4);
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
 
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
-  private final MotorControllerGroup m_leftGroup =
-      new MotorControllerGroup(m_leftLeader, m_leftFollower);
-  private final MotorControllerGroup m_rightGroup =
-      new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
   private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
@@ -58,10 +51,13 @@
   public Drivetrain() {
     m_gyro.reset();
 
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
@@ -90,8 +86,8 @@
         m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
     final double rightOutput =
         m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
-    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
-    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+    m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+    m_rightLeader.setVoltage(rightOutput + rightFeedforward);
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
index ceceaf3..9b7ec75 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
@@ -30,8 +30,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
@@ -48,19 +46,14 @@
   private static final double kWheelRadius = 0.0508; // meters
   private static final int kEncoderResolution = 4096;
 
-  private final MotorController m_leftLeader = new PWMSparkMax(1);
-  private final MotorController m_leftFollower = new PWMSparkMax(2);
-  private final MotorController m_rightLeader = new PWMSparkMax(3);
-  private final MotorController m_rightFollower = new PWMSparkMax(4);
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
 
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
-  private final MotorControllerGroup m_leftGroup =
-      new MotorControllerGroup(m_leftLeader, m_leftFollower);
-  private final MotorControllerGroup m_rightGroup =
-      new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
   private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
@@ -113,10 +106,13 @@
   public Drivetrain(DoubleArrayTopic cameraToObjectTopic) {
     m_gyro.reset();
 
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
@@ -148,8 +144,8 @@
         m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
     final double rightOutput =
         m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
-    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
-    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+    m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+    m_rightLeader.setVoltage(rightOutput + rightFeedforward);
   }
 
   /**
@@ -251,8 +247,8 @@
     // simulation, and write the simulated positions and velocities to our
     // simulated encoder and gyro.
     m_drivetrainSimulator.setInputs(
-        m_leftGroup.get() * RobotController.getInputVoltage(),
-        m_rightGroup.get() * RobotController.getInputVoltage());
+        m_leftLeader.get() * RobotController.getInputVoltage(),
+        m_rightLeader.get() * RobotController.getInputVoltage());
     m_drivetrainSimulator.update(0.02);
 
     m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
index 487974c..12c51ab 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
 
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor controller.
  *
  * <p>Has no actual functionality.
  */
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
   public enum PIDMode {
     kPosition,
     kVelocity,
@@ -74,27 +72,21 @@
   /** Resets the encoder to zero distance. */
   public void resetEncoder() {}
 
-  @Override
   public void set(double speed) {
     m_value = speed;
   }
 
-  @Override
   public double get() {
     return m_value;
   }
 
-  @Override
   public void setInverted(boolean isInverted) {}
 
-  @Override
   public boolean getInverted() {
     return false;
   }
 
-  @Override
   public void disable() {}
 
-  @Override
   public void stopMotor() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
index ab71ff4..ba2ed04 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
@@ -78,7 +78,7 @@
                     // End at desired position in meters; implicitly starts at 0
                     () -> new TrapezoidProfile.State(3, 0),
                     // Current position
-                    () -> new TrapezoidProfile.State(),
+                    TrapezoidProfile.State::new,
                     // Require the drive
                     m_robotDrive)
                 .beforeStarting(m_robotDrive::resetEncoders)
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
index 5c107d2..951cde3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
@@ -29,7 +29,7 @@
         // End at desired position in meters; implicitly starts at 0
         () -> new TrapezoidProfile.State(meters, 0),
         // Current position
-        () -> new TrapezoidProfile.State(),
+        TrapezoidProfile.State::new,
         // Require the drive
         drive);
     // Reset drive encoders since we're starting at 0
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
index 520261d..59d700b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
@@ -34,20 +35,19 @@
           DriveConstants.kaVoltSecondsSquaredPerMeter);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
     m_rightLeader.setInverted(true);
 
-    // You might need to not do this depending on the specific motor controller
-    // that you are using -- contact the respective vendor's documentation for
-    // more details.
-    m_rightFollower.setInverted(true);
-
     m_leftFollower.follow(m_leftLeader);
     m_rightFollower.follow(m_rightLeader);
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java
index e252f85..2b37b96 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
 
 package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
 
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor controller.
  *
  * <p>Has no actual functionality.
  */
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
   public enum PIDMode {
     kPosition,
     kVelocity,
@@ -72,25 +70,19 @@
   /** Resets the encoder to zero distance. */
   public void resetEncoder() {}
 
-  @Override
   public void set(double speed) {}
 
-  @Override
   public double get() {
     return 0;
   }
 
-  @Override
   public void setInverted(boolean isInverted) {}
 
-  @Override
   public boolean getInverted() {
     return false;
   }
 
-  @Override
   public void disable() {}
 
-  @Override
   public void stopMotor() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
index b6228c9..670fd7b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 @SuppressWarnings("PMD.RedundantFieldInitializer")
@@ -27,7 +26,7 @@
 
   private final Joystick m_joystick = new Joystick(1);
   private final Encoder m_encoder = new Encoder(1, 2);
-  private final MotorController m_motor = new PWMSparkMax(1);
+  private final PWMSparkMax m_motor = new PWMSparkMax(1);
 
   // Create a PID controller whose setpoint's change is subject to maximum
   // velocity and acceleration constraints.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
index a1366e6..6d9ac23 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
 
 package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
 
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
 /**
  * A simplified stub class that simulates the API of a common "smart" motor controller.
  *
  * <p>Has no actual functionality.
  */
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
   public enum PIDMode {
     kPosition,
     kVelocity,
@@ -72,25 +70,19 @@
   /** Resets the encoder to zero distance. */
   public void resetEncoder() {}
 
-  @Override
   public void set(double speed) {}
 
-  @Override
   public double get() {
     return 0;
   }
 
-  @Override
   public void setInverted(boolean isInverted) {}
 
-  @Override
   public boolean getInverted() {
     return false;
   }
 
-  @Override
   public void disable() {}
 
-  @Override
   public void stopMotor() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
index 2acb5bc..938132b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -40,7 +40,7 @@
 
     // Retrieve the profiled setpoint for the next timestep. This setpoint moves
     // toward the goal while obeying the constraints.
-    m_setpoint = m_profile.calculate(kDt, m_goal, m_setpoint);
+    m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);
 
     // Send setpoint to offboard controller PID
     m_motor.setSetpoint(
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java
index d6f3150..23a49cb 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java
@@ -5,7 +5,6 @@
 package edu.wpi.first.wpilibj.examples.eventloop;
 
 import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.examples.encoder.Robot;
 
 /**
  * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
index 03c5577..08de65c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
@@ -12,7 +12,6 @@
 import edu.wpi.first.wpilibj.Ultrasonic;
 import edu.wpi.first.wpilibj.event.BooleanEvent;
 import edu.wpi.first.wpilibj.event.EventLoop;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 public class Robot extends TimedRobot {
@@ -20,15 +19,15 @@
   public static final int TOLERANCE = 8; // rpm
   public static final int KICKER_THRESHOLD = 15; // mm
 
-  private final MotorController m_shooter = new PWMSparkMax(0);
+  private final PWMSparkMax m_shooter = new PWMSparkMax(0);
   private final Encoder m_shooterEncoder = new Encoder(0, 1);
   private final PIDController m_controller = new PIDController(0.3, 0, 0);
   private final SimpleMotorFeedforward m_ff = new SimpleMotorFeedforward(0.1, 0.065);
 
-  private final MotorController m_kicker = new PWMSparkMax(1);
+  private final PWMSparkMax m_kicker = new PWMSparkMax(1);
   private final Ultrasonic m_kickerSensor = new Ultrasonic(2, 3);
 
-  private final MotorController m_intake = new PWMSparkMax(2);
+  private final PWMSparkMax m_intake = new PWMSparkMax(2);
 
   private final EventLoop m_loop = new EventLoop();
   private final Joystick m_joystick = new Joystick(0);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 750e4dc..b1ddb6c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -983,5 +983,18 @@
     "gradlebase": "java",
     "commandversion": 2,
     "mainclass": "Main"
+  },
+  {
+    "name": "SysIdRoutine",
+    "description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
+    "tags": [
+      "SysId",
+      "Command-based",
+      "DataLog"
+    ],
+    "foldername": "sysid",
+    "gradlebase": "java",
+    "commandversion": 2,
+    "mainclass": "Main"
   }
 ]
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
index 5773da1..dc3a3c6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
@@ -4,28 +4,25 @@
 
 package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -43,10 +40,16 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
index ffde63d..b632f8d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
@@ -4,34 +4,27 @@
 
 package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {
-  /**
-   * The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis.
-   * These include four drive motors, a left and right encoder and a gyro.
-   */
-  private final MotorController m_leftMotor =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotorPort1),
-          new PWMSparkMax(DriveConstants.kLeftMotorPort1));
+  // The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis.
+  // These include four drive motors, a left and right encoder and a gyro.
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotorPort1);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotorPort2);
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotorPort1);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotorPort2);
 
-  private final MotorController m_rightMotor =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotorPort2),
-          new PWMSparkMax(DriveConstants.kLeftMotorPort2));
-
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   private final Encoder m_leftEncoder =
       new Encoder(
@@ -50,10 +43,16 @@
   public Drivetrain() {
     super();
 
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotor.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Encoders may measure differently in the real world and in
     // simulation. In this example the robot moves 0.042 barleycorns
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
index dd337e6..563235c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.gettingstarted;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
@@ -19,10 +20,16 @@
 public class Robot extends TimedRobot {
   private final PWMSparkMax m_leftDrive = new PWMSparkMax(0);
   private final PWMSparkMax m_rightDrive = new PWMSparkMax(1);
-  private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftDrive, m_rightDrive);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
   private final XboxController m_controller = new XboxController(0);
   private final Timer m_timer = new Timer();
 
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+    SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+  }
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
index 1f9e41d..7d00c27 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.gyro;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -30,10 +31,16 @@
 
   private final PWMSparkMax m_leftDrive = new PWMSparkMax(kLeftMotorPort);
   private final PWMSparkMax m_rightDrive = new PWMSparkMax(kRightMotorPort);
-  private final DifferentialDrive m_myRobot = new DifferentialDrive(m_leftDrive, m_rightDrive);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
   private final AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
   private final Joystick m_joystick = new Joystick(kJoystickPort);
 
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+    SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+  }
+
   @Override
   public void robotInit() {
     m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
@@ -50,6 +57,6 @@
   @Override
   public void teleopPeriodic() {
     double turningValue = (kAngleSetpoint - m_gyro.getAngle()) * kP;
-    m_myRobot.arcadeDrive(-m_joystick.getY(), -turningValue);
+    m_robotDrive.arcadeDrive(-m_joystick.getY(), -turningValue);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
index 9adb6ec..db87410 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
@@ -4,29 +4,26 @@
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -47,10 +44,16 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
index b86277b..a435232 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.gyromecanum;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -37,12 +38,17 @@
     PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
     PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
 
+    SendableRegistry.addChild(m_robotDrive, frontLeft);
+    SendableRegistry.addChild(m_robotDrive, rearLeft);
+    SendableRegistry.addChild(m_robotDrive, frontRight);
+    SendableRegistry.addChild(m_robotDrive, rearRight);
+
     // Invert the right side motors.
     // You may need to change or remove this to match your robot.
     frontRight.setInverted(true);
     rearRight.setInverted(true);
 
-    m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
+    m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
 
     m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
index 1376195..8de4a90 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
@@ -5,28 +5,25 @@
 package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
 
 import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -44,10 +41,16 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
index 8a5296d..8942b41 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
@@ -5,28 +5,25 @@
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
 
 import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -44,10 +41,16 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
index 15e813a..c64c9b3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /** Represents a mecanum drive style drivetrain. */
@@ -22,10 +21,10 @@
   public static final double kMaxSpeed = 3.0; // 3 meters per second
   public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
 
-  private final MotorController m_frontLeftMotor = new PWMSparkMax(1);
-  private final MotorController m_frontRightMotor = new PWMSparkMax(2);
-  private final MotorController m_backLeftMotor = new PWMSparkMax(3);
-  private final MotorController m_backRightMotor = new PWMSparkMax(4);
+  private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
+  private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
+  private final PWMSparkMax m_backLeftMotor = new PWMSparkMax(3);
+  private final PWMSparkMax m_backRightMotor = new PWMSparkMax(4);
 
   private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
   private final Encoder m_frontRightEncoder = new Encoder(2, 3);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
index 74c088f..d278f05 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -85,7 +86,7 @@
             // Add kinematics to ensure max speed is actually obeyed
             .setKinematics(DriveConstants.kDriveKinematics);
 
-    // An example trajectory to follow.  All units in meters.
+    // An example trajectory to follow. All units in meters.
     Trajectory exampleTrajectory =
         TrajectoryGenerator.generateTrajectory(
             // Start at the origin facing the +X direction
@@ -121,10 +122,11 @@
             m_robotDrive::setDriveMotorControllersVolts, // Consumer for the output motor voltages
             m_robotDrive);
 
-    // Reset odometry to the starting pose of the trajectory.
-    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
-
-    // Run path following command, then stop at the end.
-    return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+    // Reset odometry to the initial pose of the trajectory, run path following
+    // command, then stop at the end.
+    return Commands.sequence(
+        new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
+        mecanumControllerCommand,
+        new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
index 6d731ba..b3f9b34 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.MecanumDrive;
@@ -23,7 +24,7 @@
   private final PWMSparkMax m_rearRight = new PWMSparkMax(DriveConstants.kRearRightMotorPort);
 
   private final MecanumDrive m_drive =
-      new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
+      new MecanumDrive(m_frontLeft::set, m_rearLeft::set, m_frontRight::set, m_rearRight::set);
 
   // The front-left-side drive encoder
   private final Encoder m_frontLeftEncoder =
@@ -65,6 +66,11 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_frontLeft);
+    SendableRegistry.addChild(m_drive, m_rearLeft);
+    SendableRegistry.addChild(m_drive, m_frontRight);
+    SendableRegistry.addChild(m_drive, m_rearRight);
+
     // Sets the distance per pulse for the encoders
     m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rearLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
index f376543..77aa0ce 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.mecanumdrive;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.MecanumDrive;
@@ -28,12 +29,17 @@
     PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
     PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
 
+    SendableRegistry.addChild(m_robotDrive, frontLeft);
+    SendableRegistry.addChild(m_robotDrive, rearLeft);
+    SendableRegistry.addChild(m_robotDrive, frontRight);
+    SendableRegistry.addChild(m_robotDrive, rearRight);
+
     // Invert the right side motors.
     // You may need to change or remove this to match your robot.
     frontRight.setInverted(true);
     rearRight.setInverted(true);
 
-    m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
+    m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
 
     m_stick = new Joystick(kJoystickChannel);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
index 4b643bd..0625b3f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
@@ -18,7 +18,6 @@
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /** Represents a mecanum drive style drivetrain. */
@@ -26,10 +25,10 @@
   public static final double kMaxSpeed = 3.0; // 3 meters per second
   public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
 
-  private final MotorController m_frontLeftMotor = new PWMSparkMax(1);
-  private final MotorController m_frontRightMotor = new PWMSparkMax(2);
-  private final MotorController m_backLeftMotor = new PWMSparkMax(3);
-  private final MotorController m_backRightMotor = new PWMSparkMax(4);
+  private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
+  private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
+  private final PWMSparkMax m_backLeftMotor = new PWMSparkMax(3);
+  private final PWMSparkMax m_backRightMotor = new PWMSparkMax(4);
 
   private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
   private final Encoder m_frontRightEncoder = new Encoder(2, 3);
@@ -147,7 +146,7 @@
             ChassisSpeeds.discretize(
                 fieldRelative
                     ? ChassisSpeeds.fromFieldRelativeSpeeds(
-                        xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+                        xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
                     : new ChassisSpeeds(xSpeed, ySpeed, rot),
                 periodSeconds));
     mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
index 82ced20..cbb4bab 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -27,7 +26,7 @@
   private static final int kEncoderPortA = 0;
   private static final int kEncoderPortB = 1;
 
-  private MotorController m_motor;
+  private PWMSparkMax m_motor;
   private Joystick m_joystick;
   private Encoder m_encoder;
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index 9f0980a..4bbf45c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -4,8 +4,6 @@
 
 package edu.wpi.first.wpilibj.examples.ramsetecommand;
 
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.RamseteController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -17,11 +15,13 @@
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -97,7 +97,7 @@
             // Apply the voltage constraint
             .addConstraint(autoVoltageConstraint);
 
-    // An example trajectory to follow.  All units in meters.
+    // An example trajectory to follow. All units in meters.
     Trajectory exampleTrajectory =
         TrajectoryGenerator.generateTrajectory(
             // Start at the origin facing the +X direction
@@ -126,10 +126,10 @@
             m_robotDrive::tankDriveVolts,
             m_robotDrive);
 
-    // Reset odometry to the starting pose of the trajectory.
-    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
-
-    // Run path following command, then stop at the end.
-    return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
+    // Reset odometry to the initial pose of the trajectory, run path following
+    // command, then stop at the end.
+    return Commands.runOnce(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()))
+        .andThen(ramseteCommand)
+        .andThen(Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)));
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index 288baed..f2b0840 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -7,29 +7,26 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -53,10 +50,16 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -121,8 +124,8 @@
    * @param rightVolts the commanded right output
    */
   public void tankDriveVolts(double leftVolts, double rightVolts) {
-    m_leftMotors.setVoltage(leftVolts);
-    m_rightMotors.setVoltage(rightVolts);
+    m_leftLeader.setVoltage(leftVolts);
+    m_rightLeader.setVoltage(rightVolts);
     m_drive.feed();
   }
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
index 69288a1..98d1856 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
@@ -13,8 +13,6 @@
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /** Represents a differential drive style drivetrain. */
@@ -26,19 +24,14 @@
   private static final double kWheelRadius = 0.0508; // meters
   private static final int kEncoderResolution = 4096;
 
-  private final MotorController m_leftLeader = new PWMSparkMax(1);
-  private final MotorController m_leftFollower = new PWMSparkMax(2);
-  private final MotorController m_rightLeader = new PWMSparkMax(3);
-  private final MotorController m_rightFollower = new PWMSparkMax(4);
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
 
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
-  private final MotorControllerGroup m_leftGroup =
-      new MotorControllerGroup(m_leftLeader, m_leftFollower);
-  private final MotorControllerGroup m_rightGroup =
-      new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
   private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
@@ -59,10 +52,13 @@
   public Drivetrain() {
     m_gyro.reset();
 
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
@@ -91,8 +87,8 @@
         m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
     final double rightOutput =
         m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
-    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
-    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+    m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+    m_rightLeader.setVoltage(rightOutput + rightFeedforward);
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
index 736ff44..dd7854d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
@@ -4,10 +4,10 @@
 
 package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -15,19 +15,16 @@
 
 public class Drive extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -45,10 +42,16 @@
 
   /** Creates a new Drive subsystem. */
   public Drive() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java
index 69ec023..b51562f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java
@@ -50,16 +50,12 @@
    */
   public Command disableCompressorCommand() {
     return startEnd(
-            () -> {
-              // Disable closed-loop mode on the compressor.
-              m_compressor.disable();
-            },
-            () -> {
-              // Enable closed-loop mode based on the digital pressure switch connected to the
-              // PCM/PH.
-              // The switch is open when the pressure is over ~120 PSI.
-              m_compressor.enableDigital();
-            })
+            // Disable closed-loop mode on the compressor.
+            m_compressor::disable,
+            // Enable closed-loop mode based on the digital pressure switch connected to the
+            // PCM/PH.
+            // The switch is open when the pressure is over ~120 PSI.
+            m_compressor::enableDigital)
         .withName("Compressor Disabled");
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
index 0e7f9b5..d282c20 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
@@ -10,7 +10,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.ShooterConstants;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
index d805590..42cf45e 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.romireference.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -26,7 +27,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   // Set up the RomiGyro
   private final RomiGyro m_gyro = new RomiGyro();
@@ -36,6 +38,9 @@
 
   /** Creates a new Drivetrain. */
   public Drivetrain() {
+    SendableRegistry.addChild(m_diffDrive, m_leftMotor);
+    SendableRegistry.addChild(m_diffDrive, m_rightMotor);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
index 47109e0..436d726 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.examples.shuffleboard;
 
 import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -16,8 +17,10 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 
 public class Robot extends TimedRobot {
+  private final PWMSparkMax m_leftDriveMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightDriveMotor = new PWMSparkMax(1);
   private final DifferentialDrive m_tankDrive =
-      new DifferentialDrive(new PWMSparkMax(0), new PWMSparkMax(1));
+      new DifferentialDrive(m_leftDriveMotor::set, m_rightDriveMotor::set);
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
@@ -27,6 +30,9 @@
 
   @Override
   public void robotInit() {
+    SendableRegistry.addChild(m_tankDrive, m_leftDriveMotor);
+    SendableRegistry.addChild(m_tankDrive, m_rightDriveMotor);
+
     // Add a 'max speed' widget to a tab named 'Configuration', using a number slider
     // The widget will be placed in the second column and row and will be TWO columns wide
     m_maxSpeed =
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
index a9f53a9..fcae928 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
@@ -18,7 +18,6 @@
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
@@ -41,11 +40,6 @@
   private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
   private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
 
-  private final MotorControllerGroup m_leftGroup =
-      new MotorControllerGroup(m_leftLeader, m_leftFollower);
-  private final MotorControllerGroup m_rightGroup =
-      new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
@@ -77,10 +71,13 @@
 
   /** Subsystem constructor. */
   public Drivetrain() {
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
@@ -91,7 +88,7 @@
     m_leftEncoder.reset();
     m_rightEncoder.reset();
 
-    m_rightGroup.setInverted(true);
+    m_rightLeader.setInverted(true);
     SmartDashboard.putData("Field", m_fieldSim);
   }
 
@@ -104,8 +101,8 @@
     double rightOutput =
         m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
 
-    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
-    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+    m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+    m_rightLeader.setVoltage(rightOutput + rightFeedforward);
   }
 
   /**
@@ -145,8 +142,8 @@
     // simulated encoder and gyro. We negate the right side so that positive
     // voltages make the right side move forward.
     m_drivetrainSimulator.setInputs(
-        m_leftGroup.get() * RobotController.getInputVoltage(),
-        m_rightGroup.get() * RobotController.getInputVoltage());
+        m_leftLeader.get() * RobotController.getInputVoltage(),
+        m_rightLeader.get() * RobotController.getInputVoltage());
     m_drivetrainSimulator.update(0.02);
 
     m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
index d6ae048..0b791c5 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
@@ -53,33 +53,17 @@
     tab.add("Compressor", m_compressor);
 
     // Also publish some raw data
-    tab.addDouble(
-        "PH Pressure [PSI]",
-        () -> {
-          // Get the pressure (in PSI) from the analog sensor connected to the PH.
-          // This function is supported only on the PH!
-          // On a PCM, this function will return 0.
-          return m_compressor.getPressure();
-        });
-    tab.addDouble(
-        "Compressor Current",
-        () -> {
-          // Get compressor current draw.
-          return m_compressor.getCurrent();
-        });
-    tab.addBoolean(
-        "Compressor Active",
-        () -> {
-          // Get whether the compressor is active.
-          return m_compressor.isEnabled();
-        });
-    tab.addBoolean(
-        "Pressure Switch",
-        () -> {
-          // Get the digital pressure switch connected to the PCM/PH.
-          // The switch is open when the pressure is over ~120 PSI.
-          return m_compressor.getPressureSwitchValue();
-        });
+    // Get the pressure (in PSI) from the analog sensor connected to the PH.
+    // This function is supported only on the PH!
+    // On a PCM, this function will return 0.
+    tab.addDouble("PH Pressure [PSI]", m_compressor::getPressure);
+    // Get compressor current draw.
+    tab.addDouble("Compressor Current", m_compressor::getCurrent);
+    // Get whether the compressor is active.
+    tab.addBoolean("Compressor Active", m_compressor::isEnabled);
+    // Get the digital pressure switch connected to the PCM/PH.
+    // The switch is open when the pressure is over ~120 PSI.
+    tab.addBoolean("Pressure Switch", m_compressor::getPressureSwitchValue);
   }
 
   @SuppressWarnings("PMD.UnconditionalIfStatement")
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
index 69997d1..3f91c93 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
@@ -19,7 +19,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
@@ -92,7 +91,7 @@
   // An encoder set up to measure arm position in radians.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -126,7 +125,7 @@
       goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
     }
     // Step our TrapezoidalProfile forward 20ms and set it as our next reference
-    m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
+    m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
     m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
     // Correct our Kalman filter's state vector estimate with encoder data.
     m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
index 406178a..79c871c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
@@ -9,13 +9,13 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
@@ -26,19 +26,16 @@
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final MotorControllerGroup m_leftMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
-          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 
   // The motors on the right side of the drive.
-  private final MotorControllerGroup m_rightMotors =
-      new MotorControllerGroup(
-          new PWMSparkMax(DriveConstants.kRightMotor1Port),
-          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
@@ -70,10 +67,16 @@
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_rightFollower);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
-    m_rightMotors.setInverted(true);
+    m_rightLeader.setInverted(true);
 
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -131,8 +134,8 @@
     // We negate the right side so that positive voltages make the right side
     // move forward.
     m_drivetrainSimulator.setInputs(
-        m_leftMotors.get() * RobotController.getBatteryVoltage(),
-        m_rightMotors.get() * RobotController.getBatteryVoltage());
+        m_leftLeader.get() * RobotController.getBatteryVoltage(),
+        m_rightLeader.get() * RobotController.getBatteryVoltage());
     m_drivetrainSimulator.update(0.020);
 
     m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
@@ -202,8 +205,8 @@
    * @param rightVolts the commanded right output
    */
   public void tankDriveVolts(double leftVolts, double rightVolts) {
-    m_leftMotors.setVoltage(leftVolts);
-    m_rightMotors.setVoltage(rightVolts);
+    m_leftLeader.setVoltage(leftVolts);
+    m_rightLeader.setVoltage(rightVolts);
     m_drive.feed();
   }
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
index 2069521..5294191 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
@@ -19,7 +19,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
@@ -96,7 +95,7 @@
   // An encoder set up to measure elevator height in meters.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -130,7 +129,7 @@
       goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
     }
     // Step our TrapezoidalProfile forward 20ms and set it as our next reference
-    m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
+    m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
     m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
 
     // Correct our Kalman filter's state vector estimate with encoder data.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
index 06e8d29..94566ea 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
@@ -17,7 +17,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
@@ -77,7 +76,7 @@
   // An encoder set up to measure flywheel velocity in radians per second.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
index 26ee8f7..3a073fe 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
@@ -72,7 +71,7 @@
   // An encoder set up to measure flywheel velocity in radians per second.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index 65089f9..c168860 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -12,7 +12,6 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 public class SwerveModule {
@@ -23,8 +22,8 @@
   private static final double kModuleMaxAngularAcceleration =
       2 * Math.PI; // radians per second squared
 
-  private final MotorController m_driveMotor;
-  private final MotorController m_turningMotor;
+  private final PWMSparkMax m_driveMotor;
+  private final PWMSparkMax m_turningMotor;
 
   private final Encoder m_driveEncoder;
   private final Encoder m_turningEncoder;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
index cf0ff8c..1f8e3e5 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
@@ -19,6 +19,8 @@
 import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -82,7 +84,7 @@
             // Add kinematics to ensure max speed is actually obeyed
             .setKinematics(DriveConstants.kDriveKinematics);
 
-    // An example trajectory to follow.  All units in meters.
+    // An example trajectory to follow. All units in meters.
     Trajectory exampleTrajectory =
         TrajectoryGenerator.generateTrajectory(
             // Start at the origin facing the +X direction
@@ -111,10 +113,11 @@
             m_robotDrive::setModuleStates,
             m_robotDrive);
 
-    // Reset odometry to the starting pose of the trajectory.
-    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
-
-    // Run path following command, then stop at the end.
-    return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+    // Reset odometry to the initial pose of the trajectory, run path following
+    // command, then stop at the end.
+    return Commands.sequence(
+        new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
+        swerveControllerCommand,
+        new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
index 923af5b..6eb80e0 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -71,7 +71,7 @@
             ChassisSpeeds.discretize(
                 fieldRelative
                     ? ChassisSpeeds.fromFieldRelativeSpeeds(
-                        xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+                        xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
                     : new ChassisSpeeds(xSpeed, ySpeed, rot),
                 periodSeconds));
     SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
index 938b6c8..8ee8f4b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
@@ -12,7 +12,6 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 public class SwerveModule {
@@ -23,8 +22,8 @@
   private static final double kModuleMaxAngularAcceleration =
       2 * Math.PI; // radians per second squared
 
-  private final MotorController m_driveMotor;
-  private final MotorController m_turningMotor;
+  private final PWMSparkMax m_driveMotor;
+  private final PWMSparkMax m_turningMotor;
 
   private final Encoder m_driveEncoder;
   private final Encoder m_turningEncoder;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java
new file mode 100644
index 0000000..285b98f
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java
@@ -0,0 +1,83 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import edu.wpi.first.math.util.Units;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = {0, 1};
+    public static final int[] kRightEncoderPorts = {2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterMeters = Units.inchesToMeters(6);
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+  }
+
+  public static final class ShooterConstants {
+    public static final int[] kEncoderPorts = {4, 5};
+    public static final boolean kEncoderReversed = false;
+    public static final int kEncoderCPR = 1024;
+    public static final double kEncoderDistancePerPulse =
+        // Distance units will be rotations
+        1.0 / (double) kEncoderCPR;
+
+    public static final int kShooterMotorPort = 4;
+    public static final int kFeederMotorPort = 5;
+
+    public static final double kShooterFreeRPS = 5300;
+    public static final double kShooterTargetRPS = 4000;
+    public static final double kShooterToleranceRPS = 50;
+
+    // These are not real PID gains, and will have to be tuned for your specific robot.
+    public static final double kP = 1;
+
+    // On a real robot the feedforward constants should be empirically determined; these are
+    // reasonable guesses.
+    public static final double kSVolts = 0.05;
+    public static final double kVVoltSecondsPerRotation =
+        // Should have value 12V at free speed...
+        12.0 / kShooterFreeRPS;
+
+    public static final double kFeederSpeed = 0.5;
+  }
+
+  public static final class IntakeConstants {
+    public static final int kMotorPort = 6;
+    public static final int[] kSolenoidPorts = {2, 3};
+  }
+
+  public static final class StorageConstants {
+    public static final int kMotorPort = 7;
+    public static final int kBallSensorPort = 6;
+  }
+
+  public static final class AutoConstants {
+    public static final double kTimeoutSeconds = 3;
+    public static final double kDriveDistanceMeters = 2;
+    public static final double kDriveSpeed = 0.5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java
new file mode 100644
index 0000000..1fdd4b7
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java
@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java
new file mode 100644
index 0000000..0b0f142
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java
@@ -0,0 +1,92 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private final SysIdRoutineBot m_robot = new SysIdRoutineBot();
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Configure default commands and condition bindings on robot startup
+    m_robot.configureBindings();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robot.getAutonomousCommand();
+
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java
new file mode 100644
index 0000000..6665f3f
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java
@@ -0,0 +1,60 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants;
+
+import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class SysIdRoutineBot {
+  // The robot's subsystems
+  private final Drive m_drive = new Drive();
+
+  // The driver's controller
+  CommandXboxController m_driverController =
+      new CommandXboxController(OIConstants.kDriverControllerPort);
+
+  /**
+   * Use this method to define bindings between conditions and commands. These are useful for
+   * automating robot behaviors based on button and sensor input.
+   *
+   * <p>Should be called during {@link Robot#robotInit()}.
+   *
+   * <p>Event binding methods are available on the {@link Trigger} class.
+   */
+  public void configureBindings() {
+    // Control the drive with split-stick arcade controls
+    m_drive.setDefaultCommand(
+        m_drive.arcadeDriveCommand(
+            () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX()));
+
+    // Bind full set of SysId routine tests to buttons; a complete routine should run each of these
+    // once.
+    m_driverController.a().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_driverController.b().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_driverController.x().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_driverController.y().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+  }
+
+  /**
+   * Use this to define the command that runs during autonomous.
+   *
+   * <p>Scheduled during {@link Robot#autonomousInit()}.
+   */
+  public Command getAutonomousCommand() {
+    // Do nothing
+    return m_drive.run(() -> {});
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java
new file mode 100644
index 0000000..3c96108
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java
@@ -0,0 +1,132 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.sysid.subsystems;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import java.util.function.DoubleSupplier;
+
+public class Drive extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+
+  // The motors on the right side of the drive.
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+
+  // The robot's drive
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
+
+  // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+  private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+  // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+  private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
+  // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+  private final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
+
+  // Create a new SysId routine for characterizing the drive.
+  private final SysIdRoutine m_sysIdRoutine =
+      new SysIdRoutine(
+          // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              // Tell SysId how to plumb the driving voltage to the motors.
+              (Measure<Voltage> volts) -> {
+                m_leftMotor.setVoltage(volts.in(Volts));
+                m_rightMotor.setVoltage(volts.in(Volts));
+              },
+              // Tell SysId how to record a frame of data for each motor on the mechanism being
+              // characterized.
+              log -> {
+                // Record a frame for the left motors.  Since these share an encoder, we consider
+                // the entire group to be one motor.
+                log.motor("drive-left")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(
+                            m_leftMotor.get() * RobotController.getBatteryVoltage(), Volts))
+                    .linearPosition(m_distance.mut_replace(m_leftEncoder.getDistance(), Meters))
+                    .linearVelocity(
+                        m_velocity.mut_replace(m_leftEncoder.getRate(), MetersPerSecond));
+                // Record a frame for the right motors.  Since these share an encoder, we consider
+                // the entire group to be one motor.
+                log.motor("drive-right")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(
+                            m_rightMotor.get() * RobotController.getBatteryVoltage(), Volts))
+                    .linearPosition(m_distance.mut_replace(m_rightEncoder.getDistance(), Meters))
+                    .linearVelocity(
+                        m_velocity.mut_replace(m_rightEncoder.getRate(), MetersPerSecond));
+              },
+              // Tell SysId to make generated commands require this subsystem, suffix test state in
+              // WPILog with this subsystem's name ("drive")
+              this));
+
+  /** Creates a new Drive subsystem. */
+  public Drive() {
+    // Add the second motors on each side of the drivetrain
+    m_leftMotor.addFollower(new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+    m_rightMotor.addFollower(new PWMSparkMax(DriveConstants.kRightMotor2Port));
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.setInverted(true);
+
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Returns a command that drives the robot with arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
+    // A split-stick arcade command, with forward/backward controlled by the left
+    // hand, and turning controlled by the right.
+    return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
+        .withName("arcadeDrive");
+  }
+
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.quasistatic(direction);
+  }
+
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.dynamic(direction);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
index 3883d14..a217f51 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
@@ -4,10 +4,10 @@
 
 package edu.wpi.first.wpilibj.examples.tankdrive;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
@@ -15,27 +15,30 @@
  * the code necessary to operate a robot with tank drive.
  */
 public class Robot extends TimedRobot {
-  private DifferentialDrive m_myRobot;
+  private DifferentialDrive m_robotDrive;
   private Joystick m_leftStick;
   private Joystick m_rightStick;
 
-  private final MotorController m_leftMotor = new PWMSparkMax(0);
-  private final MotorController m_rightMotor = new PWMSparkMax(1);
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
 
   @Override
   public void robotInit() {
+    SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+    SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
     m_rightMotor.setInverted(true);
 
-    m_myRobot = new DifferentialDrive(m_leftMotor, m_rightMotor);
+    m_robotDrive = new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
     m_leftStick = new Joystick(0);
     m_rightStick = new Joystick(1);
   }
 
   @Override
   public void teleopPeriodic() {
-    m_myRobot.tankDrive(-m_leftStick.getY(), -m_rightStick.getY());
+    m_robotDrive.tankDrive(-m_leftStick.getY(), -m_rightStick.getY());
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
index c6a985a..c1a850c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.tankdrivexboxcontroller;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -16,11 +17,15 @@
 public class Robot extends TimedRobot {
   private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
   private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
-  private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
   private final XboxController m_driverController = new XboxController(0);
 
   @Override
   public void robotInit() {
+    SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+    SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
index 532e9ac..33681d0 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.MedianFilter;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Ultrasonic;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -21,8 +22,7 @@
   static final double kHoldDistanceMillimeters = 1.0e3;
 
   // proportional speed constant
-  // negative because applying positive voltage will bring us closer to the target
-  private static final double kP = -0.001;
+  private static final double kP = 0.001;
   // integral speed constant
   private static final double kI = 0.0;
   // derivative speed constant
@@ -41,9 +41,15 @@
   private final Ultrasonic m_ultrasonic = new Ultrasonic(kUltrasonicPingPort, kUltrasonicEchoPort);
   private final PWMSparkMax m_leftMotor = new PWMSparkMax(kLeftMotorPort);
   private final PWMSparkMax m_rightMotor = new PWMSparkMax(kRightMotorPort);
-  private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
   private final PIDController m_pidController = new PIDController(kP, kI, kD);
 
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+    SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+  }
+
   @Override
   public void autonomousInit() {
     // Set setpoint of the pid controller
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java
index a51d505..e84a607 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java
@@ -44,7 +44,7 @@
   }
 
   @Override
-  public void close() throws Exception {
+  public void close() {
     m_piston.close();
     m_motor.close();
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java
index f618add..87b985d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.xrpreference.subsystems;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -29,7 +30,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   // Set up the XRPGyro
   private final XRPGyro m_gyro = new XRPGyro();
@@ -39,6 +41,9 @@
 
   /** Creates a new Drivetrain. */
   public Drivetrain() {
+    SendableRegistry.addChild(m_diffDrive, m_leftMotor);
+    SendableRegistry.addChild(m_diffDrive, m_rightMotor);
+
     // We need to invert one side of the drivetrain so that positive voltages
     // result in both sides moving forward. Depending on how your robot's
     // gearbox is constructed, you might have to invert the left side instead.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
index 276e5ce..c1cfac1 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
@@ -24,7 +24,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   /** Creates a new RomiDrivetrain. */
   public RomiDrivetrain() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java
index 4b73a44..7d2a438 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java
@@ -23,7 +23,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   /** Creates a new RomiDrivetrain. */
   public RomiDrivetrain() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
index e4f8559..a0f4f47 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
@@ -23,7 +23,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   /** Creates a new RomiDrivetrain. */
   public RomiDrivetrain() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java
index a3a1063..85febbe 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java
@@ -27,7 +27,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   /** Creates a new XRPDrivetrain. */
   public XRPDrivetrain() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java
index 8f73bae..a245955 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java
@@ -26,7 +26,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   /** Creates a new XRPDrivetrain. */
   public XRPDrivetrain() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java
index e66d750..790acc7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java
@@ -26,7 +26,8 @@
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
   // Set up the differential drive controller
-  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+  private final DifferentialDrive m_diffDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
 
   /** Creates a new XRPDrivetrain. */
   public XRPDrivetrain() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java b/third_party/allwpilib/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java
index 342daa2..f0a27b8 100644
--- a/third_party/allwpilib/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java
+++ b/third_party/allwpilib/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java
@@ -80,7 +80,7 @@
               m_driveSim.update(0.02);
 
               double startingDistance = m_startToObject;
-              double range = startingDistance - m_driveSim.getLeftPositionMeters();
+              double range = m_driveSim.getLeftPositionMeters() - startingDistance;
 
               m_ultrasonicSim.setRangeMeters(range);
               m_distanceMM = range * 1.0e3;
@@ -126,10 +126,9 @@
     }
 
     {
-      // advance 100 timesteps
-      SimHooks.stepTiming(2.0);
+      SimHooks.stepTiming(5.0);
 
-      assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10);
+      assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10.0);
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
index bdc4675..22267b1 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
@@ -4,11 +4,11 @@
 
 package edu.wpi.first.wpilibj;
 
+import static org.hamcrest.MatcherAssert.assertThat;
 import static org.hamcrest.Matchers.both;
 import static org.hamcrest.Matchers.greaterThan;
 import static org.hamcrest.Matchers.lessThan;
 import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertThat;
 
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import java.util.concurrent.atomic.AtomicBoolean;
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java
index e6585e4..eaf975d 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java
@@ -7,8 +7,8 @@
 import static org.junit.Assert.assertEquals;
 
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-import java.util.Arrays;
 import java.util.Collection;
+import java.util.List;
 import java.util.logging.Logger;
 import org.junit.BeforeClass;
 import org.junit.Test;
@@ -38,7 +38,7 @@
   /** Test with all valid ranges to make sure unpacking is always done correctly. */
   @Parameters
   public static Collection<BuiltInAccelerometer.Range[]> generateData() {
-    return Arrays.asList(
+    return List.of(
         new BuiltInAccelerometer.Range[][] {
           {BuiltInAccelerometer.Range.k2G},
           {BuiltInAccelerometer.Range.k4G},
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
index 72c2c55..078ef5a 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
@@ -14,8 +14,8 @@
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import edu.wpi.first.wpilibj.test.TestBench;
-import java.util.Arrays;
 import java.util.Collection;
+import java.util.List;
 import java.util.logging.Logger;
 import org.junit.After;
 import org.junit.AfterClass;
@@ -56,7 +56,7 @@
   @Parameters(name = "{index}: {0}")
   public static Collection<MotorEncoderFixture<?>[]> generateData() {
     // logger.fine("Loading the MotorList");
-    return Arrays.asList(
+    return List.of(
         new MotorEncoderFixture<?>[][] {
           {TestBench.getTalonPair()}, {TestBench.getVictorPair()}, {TestBench.getJaguarPair()}
         });
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java
index 769fa7f..4ec5a72 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java
@@ -10,8 +10,8 @@
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import edu.wpi.first.wpilibj.test.TestBench;
-import java.util.Arrays;
 import java.util.Collection;
+import java.util.List;
 import java.util.logging.Logger;
 import org.junit.AfterClass;
 import org.junit.Before;
@@ -44,7 +44,7 @@
   @Parameters(name = "{index}: {0}")
   public static Collection<MotorEncoderFixture<?>[]> generateData() {
     // logger.fine("Loading the MotorList");
-    return Arrays.asList(
+    return List.of(
         new MotorEncoderFixture<?>[][] {
           {TestBench.getTalonPair()}, {TestBench.getVictorPair()}, {TestBench.getJaguarPair()}
         });
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
index f2bc522..d668d16 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
@@ -4,17 +4,17 @@
 
 package edu.wpi.first.wpilibj;
 
+import static org.hamcrest.MatcherAssert.assertThat;
 import static org.hamcrest.Matchers.greaterThan;
 import static org.hamcrest.Matchers.is;
 import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertThat;
 
 import edu.wpi.first.hal.can.CANMessageNotFoundException;
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import edu.wpi.first.wpilibj.test.TestBench;
-import java.util.Arrays;
 import java.util.Collection;
+import java.util.List;
 import java.util.logging.Logger;
 import org.junit.After;
 import org.junit.AfterClass;
@@ -65,7 +65,7 @@
   @Parameters(name = "{index}: {0}, Expected Stopped Current Draw: {1}")
   public static Collection<Object[]> generateData() {
     // logger.fine("Loading the MotorList");
-    return Arrays.asList(new Object[][] {{TestBench.getTalonPair(), 0.0}});
+    return List.of(new Object[][] {{TestBench.getTalonPair(), 0.0}});
   }
 
   @After
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
index b5e0fc8..f2cd12f 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
@@ -16,8 +16,8 @@
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.Collection;
+import java.util.List;
 import java.util.logging.Logger;
 import org.junit.After;
 import org.junit.AfterClass;
@@ -72,13 +72,13 @@
   @Parameters
   public static Collection<Object[]> generateData() {
     // logger.fine("Loading the MotorList");
-    Collection<Object[]> data = new ArrayList<Object[]>();
+    Collection<Object[]> data = new ArrayList<>();
     double kp = 0.001;
     double ki = 0.0005;
     double kd = 0.0;
     for (int i = 0; i < 1; i++) {
       data.addAll(
-          Arrays.asList(
+          List.of(
               new Object[][] {
                 {kp, ki, kd, TestBench.getTalonPair()},
                 {kp, ki, kd, TestBench.getVictorPair()},
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java
index ca93c4a..d632291 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java
@@ -22,6 +22,7 @@
  * fixture. This allows tests to be mailable so that you can easily reconfigure the physical testbed
  * without breaking the tests.
  */
+@SuppressWarnings("removal")
 public abstract class MotorEncoderFixture<T extends MotorController> implements ITestFixture {
   private static final Logger logger = Logger.getLogger(MotorEncoderFixture.class.getName());
   private boolean m_initialized = false;
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
deleted file mode 100644
index e7a6b8e..0000000
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
+++ /dev/null
@@ -1,40 +0,0 @@
-// 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.
-
-package edu.wpi.first.wpilibj.motorcontrol;
-
-public class MockMotorController implements MotorController {
-  private double m_speed;
-  private boolean m_isInverted;
-
-  @Override
-  public void set(double speed) {
-    m_speed = m_isInverted ? -speed : speed;
-  }
-
-  @Override
-  public double get() {
-    return m_speed;
-  }
-
-  @Override
-  public void setInverted(boolean isInverted) {
-    m_isInverted = isInverted;
-  }
-
-  @Override
-  public boolean getInverted() {
-    return m_isInverted;
-  }
-
-  @Override
-  public void disable() {
-    m_speed = 0;
-  }
-
-  @Override
-  public void stopMotor() {
-    disable();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
index 056ff5f..4a91293 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
@@ -31,7 +31,7 @@
   // We have no way to stop the MockDS, so its thread is daemon.
   private static MockDS ds;
 
-  /**
+  /*
    * This sets up the network communications library to enable the driver station. After starting
    * network coms, it will loop until the driver station returns that the robot is enabled, to
    * ensure that tests will be able to run on the hardware.
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
index ff0542c..b8adb17 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
@@ -34,15 +34,11 @@
    */
   protected List<Class<?>> getAnnotatedTestClasses() {
     SuiteClasses annotation = getClass().getAnnotation(SuiteClasses.class);
-    List<Class<?>> classes = new ArrayList<>();
     if (annotation == null) {
       throw new RuntimeException(
           String.format("class '%s' must have a SuiteClasses annotation", getClass().getName()));
     }
-    for (Class<?> c : annotation.value()) {
-      classes.add(c);
-    }
-    return classes;
+    return List.of(annotation.value());
   }
 
   private boolean areAnySuperClassesOfTypeAbstractTestSuite(Class<?> check) {
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java
index 07d0af7..31da103 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java
@@ -24,9 +24,8 @@
    */
   public static void main(String... args) {
     if (args.length == 0) {
-      String path =
+      String pathToReports =
           String.format("%s/%s", System.getProperty("user.dir"), "/testResults/AntReports");
-      String pathToReports = path;
       Project project = new Project();
 
       try {
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java
index 258d640..508c432 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import java.io.PrintStream;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.Collection;
 import java.util.List;
 
@@ -59,7 +58,7 @@
    * The single constructor for the TestBench. This method is private in order to prevent multiple
    * TestBench objects from being allocated.
    */
-  protected TestBench() {}
+  private TestBench() {}
 
   /**
    * Constructs a new set of objects representing a connected set of Talon controlled Motors and an
@@ -68,7 +67,7 @@
    * @return a freshly allocated Talon, Encoder pair
    */
   public static MotorEncoderFixture<Talon> getTalonPair() {
-    return new MotorEncoderFixture<Talon>() {
+    return new MotorEncoderFixture<>() {
       @Override
       protected Talon giveMotorController() {
         return new Talon(kTalonChannel);
@@ -98,7 +97,7 @@
    * @return a freshly allocated Victor, Encoder pair
    */
   public static MotorEncoderFixture<Victor> getVictorPair() {
-    return new MotorEncoderFixture<Victor>() {
+    return new MotorEncoderFixture<>() {
       @Override
       protected Victor giveMotorController() {
         return new Victor(kVictorChannel);
@@ -128,7 +127,7 @@
    * @return a freshly allocated Jaguar, Encoder pair
    */
   public static MotorEncoderFixture<Jaguar> getJaguarPair() {
-    return new MotorEncoderFixture<Jaguar>() {
+    return new MotorEncoderFixture<>() {
       @Override
       protected Jaguar giveMotorController() {
         return new Jaguar(kJaguarChannel);
@@ -190,9 +189,9 @@
 
   /** Gets two lists of possible DIO pairs for the two pairs. */
   private static List<List<Integer[]>> getDIOCrossConnect() {
-    List<List<Integer[]>> pairs = new ArrayList<List<Integer[]>>();
+    List<List<Integer[]>> pairs = new ArrayList<>();
     List<Integer[]> setA =
-        Arrays.asList(
+        List.of(
             new Integer[][] {
               {DIOCrossConnectA1, DIOCrossConnectA2},
               {DIOCrossConnectA2, DIOCrossConnectA1}
@@ -200,7 +199,7 @@
     pairs.add(setA);
 
     List<Integer[]> setB =
-        Arrays.asList(
+        List.of(
             new Integer[][] {
               {DIOCrossConnectB1, DIOCrossConnectB2},
               {DIOCrossConnectB2, DIOCrossConnectB1}
@@ -252,7 +251,7 @@
    * @return pairs of DIOCrossConnectFixtures
    */
   public static Collection<Integer[]> getDIOCrossConnectCollection() {
-    Collection<Integer[]> pairs = new ArrayList<Integer[]>();
+    Collection<Integer[]> pairs = new ArrayList<>();
     for (Collection<Integer[]> collection : getDIOCrossConnect()) {
       pairs.addAll(collection);
     }
@@ -267,7 +266,7 @@
    */
   private static Collection<Integer[]> getPairArray(
       List<Integer[]> listA, List<Integer[]> listB, boolean flip) {
-    Collection<Integer[]> encoderPortPairs = new ArrayList<Integer[]>();
+    Collection<Integer[]> encoderPortPairs = new ArrayList<>();
     for (Integer[] portPairsA : listA) {
       Integer[] inputs = new Integer[5];
       inputs[0] = portPairsA[0]; // InputA
@@ -279,7 +278,7 @@
         inputs[4] = flip ? 0 : 1; // The flip bit
       }
 
-      ArrayList<Integer[]> construtorInput = new ArrayList<Integer[]>();
+      ArrayList<Integer[]> construtorInput = new ArrayList<>();
       construtorInput.add(inputs);
 
       inputs = inputs.clone();
@@ -300,7 +299,7 @@
    * @return A collection of different input pairs to use for the encoder
    */
   public static Collection<Integer[]> getEncoderDIOCrossConnectCollection() {
-    Collection<Integer[]> encoderPortPairs = new ArrayList<Integer[]>();
+    Collection<Integer[]> encoderPortPairs = new ArrayList<>();
     assert getDIOCrossConnect().size() == 2;
     encoderPortPairs.addAll(
         getPairArray(getDIOCrossConnect().get(0), getDIOCrossConnect().get(1), false));
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java
index 880a97c..befc066 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java
@@ -8,7 +8,6 @@
 import java.io.IOException;
 import java.io.InputStream;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.List;
 import java.util.Objects;
 import java.util.logging.LogManager;
@@ -77,40 +76,36 @@
 
   /** Displays a help message for the user when they use the --help flag at runtime. */
   protected static void displayHelp() {
-    StringBuilder helpMessage = new StringBuilder("Test Parameters help: \n");
-    helpMessage.append(
-        "\t"
+    String helpMessage =
+        "Test Parameters help: \n"
+            + "\t"
             + QUICK_TEST_FLAG
             + " will cause the quick test to be run. Ignores other flags except for "
             + METHOD_REPEAT_FILTER
-            + "\n");
-    helpMessage.append(
-        "\t"
+            + "\n"
+            + "\t"
             + CLASS_NAME_FILTER
             + " will use the supplied regex text to search for suite/test class names "
-            + "matching the regex and run them.\n");
-    helpMessage.append(
-        "\t"
+            + "matching the regex and run them.\n"
+            + "\t"
             + METHOD_NAME_FILTER
             + " will use the supplied regex text to search for test methods (excluding methods "
             + "with the @Ignore annotation) and run only those methods. Can be paired with "
             + METHOD_REPEAT_FILTER
             + " to "
-            + "repeat the selected tests multiple times.\n");
-    helpMessage.append(
-        "\t"
+            + "repeat the selected tests multiple times.\n"
+            + "\t"
             + METHOD_REPEAT_FILTER
             + " will repeat the tests selected with either "
             + QUICK_TEST_FLAG
             + " or "
             + CLASS_NAME_FILTER
-            + " and run them the given number of times.\n");
-    helpMessage.append(
-        "[NOTE] All regex uses the syntax defined by java.util.regex.Pattern. This "
+            + " and run them the given number of times.\n"
+            + "[NOTE] All regex uses the syntax defined by java.util.regex.Pattern. This "
             + "documentation can be found at "
-            + "http://docs.oracle.com/javase/7/docs/api/java/util/regex/Pattern.html\n");
-    helpMessage.append("\n");
-    helpMessage.append("\n");
+            + "http://docs.oracle.com/javase/7/docs/api/java/util/regex/Pattern.html\n"
+            + "\n"
+            + "\n";
 
     TestBench.out().println(helpMessage);
   }
@@ -123,7 +118,7 @@
     StringBuilder invalidMessage = new StringBuilder("Invalid Usage: " + message + "\n");
     invalidMessage.append("Params received: ");
     for (String a : args) {
-      invalidMessage.append(a + " ");
+      invalidMessage.append(a).append(" ");
     }
     invalidMessage.append("\n");
     invalidMessage.append(
@@ -145,9 +140,9 @@
     for (Class<?> c : classes) {
       if (c.getPackage().equals(packagE)) {
         packagE = c.getPackage();
-        loadedTestsMessage.append(packagE.getName() + "\n");
+        loadedTestsMessage.append(packagE.getName()).append("\n");
       }
-      loadedTestsMessage.append("\t" + c.getSimpleName() + "\n");
+      loadedTestsMessage.append("\t").append(c.getSimpleName()).append("\n");
     }
     TestBench.out().println(loadedTestsMessage);
   }
@@ -193,7 +188,7 @@
       }
     }
 
-    ArrayList<String> argsParsed = new ArrayList<String>(Arrays.asList(args));
+    ArrayList<String> argsParsed = new ArrayList<>(List.of(args));
     if (argsParsed.contains(HELP_FLAG)) {
       // If the user inputs the help flag then return the help message and exit
       // without running any tests
@@ -257,7 +252,7 @@
     // If a specific method has been requested
     if (methodFilter) {
       List<ClassMethodPair> pairs = (new TestSuite()).getMethodMatching(methodRegex);
-      if (pairs.size() == 0) {
+      if (pairs.isEmpty()) {
         displayInvalidUsage(
             "None of the arguments passed to the method name filter matched.", args);
         return null;
@@ -292,7 +287,7 @@
     // If a specific class has been requested
     if (classFilter) {
       List<Class<?>> testClasses = (new TestSuite()).getSuiteOrTestMatchingRegex(classRegex);
-      if (testClasses.size() == 0) {
+      if (testClasses.isEmpty()) {
         displayInvalidUsage("None of the arguments passed to the filter matched.", args);
         return null;
       }
diff --git a/third_party/allwpilib/wpimath/.styleguide b/third_party/allwpilib/wpimath/.styleguide
index 257b50b..00437ce 100644
--- a/third_party/allwpilib/wpimath/.styleguide
+++ b/third_party/allwpilib/wpimath/.styleguide
@@ -21,6 +21,7 @@
   src/test/native/cpp/UnitsTest\.cpp$
   src/test/native/cpp/drake/
   src/test/native/include/drake/
+  src/generated/main/java/edu/wpi/first/math/proto
 }
 
 repoRootNameOverride {
diff --git a/third_party/allwpilib/wpimath/CMakeLists.txt b/third_party/allwpilib/wpimath/CMakeLists.txt
index 997ec46..a22ffc0 100644
--- a/third_party/allwpilib/wpimath/CMakeLists.txt
+++ b/third_party/allwpilib/wpimath/CMakeLists.txt
@@ -8,164 +8,144 @@
 # workaround for makefiles - for some reason parent directories aren't created.
 file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/protobuf")
 file(GLOB wpimath_proto_src src/main/proto/*.proto)
-protobuf_generate_cpp(WPIMATH_PROTO_SRCS WPIMATH_PROTO_HDRS PROTOC_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/protobuf" PROTOS ${wpimath_proto_src})
+protobuf_generate_cpp(
+    WPIMATH_PROTO_SRCS
+    WPIMATH_PROTO_HDRS
+    PROTOC_OUT_DIR
+    "${CMAKE_CURRENT_BINARY_DIR}/protobuf"
+    PROTOS
+    ${wpimath_proto_src}
+)
 
-function(quickbuf_generate SRCS JAVA_PACKAGE)
-  if(NOT ARGN)
-    message(SEND_ERROR "Error: PROTOBUF_GENERATE_QUICKBUF() called without any proto files")
-    return()
-  endif()
-
-  set(_generated_srcs_all)
-  foreach(_proto ${ARGN})
-    get_filename_component(_abs_file ${_proto} ABSOLUTE)
-    get_filename_component(_abs_dir ${_abs_file} DIRECTORY)
-    get_filename_component(_basename ${_proto} NAME_WLE)
-    file(RELATIVE_PATH _rel_dir ${CMAKE_CURRENT_SOURCE_DIR} ${_abs_dir})
-
-    # convert to QuickBuffers Java case (geometry2d -> Geometry2D)
-    string(REGEX MATCHALL "[A-Za-z_]+|[0-9]+" _name_components ${_basename})
-    set(_name_components_out)
-    foreach(_part ${_name_components})
-      string(SUBSTRING ${_part} 0 1 _first_letter)
-      string(TOUPPER ${_first_letter} _first_letter)
-      string(REGEX REPLACE "^.(.*)" "${_first_letter}\\1" _part_out "${_part}")
-      list(APPEND _name_components_out ${_part_out})
-    endforeach()
-    list(JOIN _name_components_out "" _basename_title)
-
-    set(_generated_src "${CMAKE_CURRENT_BINARY_DIR}/quickbuf/${JAVA_PACKAGE}/${_basename_title}.java")
-
-    list(APPEND _generated_srcs_all ${_generated_src})
-
-    add_custom_command(
-      OUTPUT ${_generated_src}
-      COMMAND protobuf::protoc
-      ARGS --plugin=protoc-gen-quickbuf=${Quickbuf_EXECUTABLE} --quickbuf_out=gen_descriptors=true:${CMAKE_CURRENT_BINARY_DIR}/quickbuf -I${_abs_dir} ${_abs_file}
-      DEPENDS ${_abs_file} protobuf::protoc
-      COMMENT "Running quickbuf protocol buffer compiler on ${_proto}"
-      VERBATIM )
-  endforeach()
-
-  set(${SRCS} ${_generated_srcs_all} PARENT_SCOPE)
-endfunction()
-
-file(GLOB wpimath_jni_src src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
-                          src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
-                          src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
-                          src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
-                          src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
-                          src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp)
+file(
+    GLOB wpimath_jni_src
+    src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
+    src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
+    src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
+    src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
+    src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
+    src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
+)
 
 # Java bindings
-if (WITH_JAVA)
-  find_package(Java REQUIRED)
-  find_package(JNI REQUIRED)
-  include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    find_package(JNI REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  quickbuf_generate(WPIMATH_QUICKBUF_SRCS "edu/wpi/first/math/proto" ${wpimath_proto_src})
+    if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.43.1.jar")
+        set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
+        set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml")
 
-  if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.43.1.jar")
-      set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
-      set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml")
+        message(STATUS "Downloading EJML jarfiles...")
 
-      message(STATUS "Downloading EJML jarfiles...")
+        download_and_check("${BASE_URL}org/ejml/ejml-cdense/0.43.1/ejml-cdense-0.43.1.jar"
+            "${JAR_ROOT}/ejml-cdense-0.43.1.jar"
+        )
+        download_and_check("${BASE_URL}org/ejml/ejml-core/0.43.1/ejml-core-0.43.1.jar"
+            "${JAR_ROOT}/ejml-core-0.43.1.jar"
+        )
+        download_and_check("${BASE_URL}org/ejml/ejml-ddense/0.43.1/ejml-ddense-0.43.1.jar"
+            "${JAR_ROOT}/ejml-ddense-0.43.1.jar"
+        )
+        download_and_check("${BASE_URL}org/ejml/ejml-dsparse/0.43.1/ejml-dsparse-0.43.1.jar"
+            "${JAR_ROOT}/ejml-dsparse-0.43.1.jar"
+        )
+        download_and_check("${BASE_URL}org/ejml/ejml-fdense/0.43.1/ejml-fdense-0.43.1.jar"
+            "${JAR_ROOT}/ejml-fdense-0.43.1.jar"
+        )
+        download_and_check("${BASE_URL}org/ejml/ejml-simple/0.43.1/ejml-simple-0.43.1.jar"
+            "${JAR_ROOT}/ejml-simple-0.43.1.jar"
+        )
+        download_and_check("${BASE_URL}org/ejml/ejml-zdense/0.43.1/ejml-zdense-0.43.1.jar"
+            "${JAR_ROOT}/ejml-zdense-0.43.1.jar"
+        )
 
-      download_and_check("${BASE_URL}org/ejml/ejml-cdense/0.43.1/ejml-cdense-0.43.1.jar"
-          "${JAR_ROOT}/ejml-cdense-0.43.1.jar")
-      download_and_check("${BASE_URL}org/ejml/ejml-core/0.43.1/ejml-core-0.43.1.jar"
-          "${JAR_ROOT}/ejml-core-0.43.1.jar")
-      download_and_check("${BASE_URL}org/ejml/ejml-ddense/0.43.1/ejml-ddense-0.43.1.jar"
-          "${JAR_ROOT}/ejml-ddense-0.43.1.jar")
-      download_and_check("${BASE_URL}org/ejml/ejml-dsparse/0.43.1/ejml-dsparse-0.43.1.jar"
-          "${JAR_ROOT}/ejml-dsparse-0.43.1.jar")
-      download_and_check("${BASE_URL}org/ejml/ejml-fdense/0.43.1/ejml-fdense-0.43.1.jar"
-          "${JAR_ROOT}/ejml-fdense-0.43.1.jar")
-      download_and_check("${BASE_URL}org/ejml/ejml-simple/0.43.1/ejml-simple-0.43.1.jar"
-          "${JAR_ROOT}/ejml-simple-0.43.1.jar")
-      download_and_check("${BASE_URL}org/ejml/ejml-zdense/0.43.1/ejml-zdense-0.43.1.jar"
-          "${JAR_ROOT}/ejml-zdense-0.43.1.jar")
-
-      message(STATUS "All files downloaded.")
-  endif()
-
-  file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
-  file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
-  file(GLOB QUICKBUF_JAR
-        ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar)
-
-  set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS} ${QUICKBUF_JAR})
-
-  execute_process(COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/generate_numbers.py ${WPILIB_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
-  if(NOT (generateResult EQUAL "0"))
-    # Try python
-    execute_process(COMMAND python ${CMAKE_CURRENT_SOURCE_DIR}/generate_numbers.py ${WPILIB_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
-    if(NOT (generateResult EQUAL "0"))
-      message(FATAL_ERROR "python and python3 generate_numbers.py failed")
+        message(STATUS "All files downloaded.")
     endif()
-  endif()
 
-  set(CMAKE_JNI_TARGET true)
+    file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+    file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+    file(GLOB QUICKBUF_JAR ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar)
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/*.java)
+    set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS} ${QUICKBUF_JAR})
 
-  add_jar(wpimath_jar ${JAVA_SOURCES} ${WPIMATH_QUICKBUF_SRCS} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers)
+    set(CMAKE_JNI_TARGET true)
 
-  get_property(WPIMATH_JAR_FILE TARGET wpimath_jar PROPERTY JAR_FILE)
-  install(FILES ${WPIMATH_JAR_FILE} DESTINATION "${java_lib_dest}")
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java src/generated/main/java/*.java)
 
-  set_property(TARGET wpimath_jar PROPERTY FOLDER "java")
+    add_jar(
+        wpimath_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS ${EJML_JARS} wpiutil_jar wpiunits_jar
+        OUTPUT_NAME wpimath
+        GENERATE_NATIVE_HEADERS wpimath_jni_headers
+    )
 
-  add_library(wpimathjni ${wpimath_jni_src})
-  wpilib_target_warnings(wpimathjni)
-  target_link_libraries(wpimathjni PUBLIC wpimath)
+    install_jar(wpimath_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS wpimath_jar FILE wpimath_jar.cmake DESTINATION share/wpimath)
 
-  set_property(TARGET wpimathjni PROPERTY FOLDER "libraries")
+    add_library(wpimathjni ${wpimath_jni_src})
+    wpilib_target_warnings(wpimathjni)
+    target_link_libraries(wpimathjni PUBLIC wpimath)
 
-  target_link_libraries(wpimathjni PRIVATE wpimath_jni_headers)
-  add_dependencies(wpimathjni wpimath_jar)
+    set_property(TARGET wpimathjni PROPERTY FOLDER "libraries")
 
-  install(TARGETS wpimathjni EXPORT wpimathjni)
+    target_link_libraries(wpimathjni PRIVATE wpimath_jni_headers)
+    add_dependencies(wpimathjni wpimath_jar)
 
+    install(TARGETS wpimathjni EXPORT wpimathjni)
 endif()
 
-if (WITH_JAVA_SOURCE)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  file(GLOB WPIMATH_SOURCES src/main/java/edu/wpi/first/math/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/main/java/edu/wpi/first/math/Nat.java)
-  file(GLOB WPIMATH_CONTROLLER_SOURCES src/main/java/edu/wpi/first/math/controller/*.java)
-  file(GLOB WPIMATH_ESTIMATOR_SOURCES src/main/java/edu/wpi/first/math/estimator/*.java)
-  file(GLOB WPIMATH_FILTER_SOURCES src/main/java/edu/wpi/first/math/filter/*.java)
-  file(GLOB WPIMATH_GEOMETRY_SOURCES src/main/java/edu/wpi/first/math/geometry/*.java)
-  file(GLOB WPIMATH_INTERPOLATION_SOURCES src/main/java/edu/wpi/first/math/interpolation/*.java)
-  file(GLOB WPIMATH_KINEMATICS_SOURCES src/main/java/edu/wpi/first/math/kinematics/*.java)
-  file(GLOB WPIMATH_NUMBERS_SOURCES ${WPILIB_BINARY_DIR}/wpimath/generated/main/java/edu/wpi/first/math/numbers/*.java)
-  file(GLOB WPIMATH_SPLINE_SOURCES src/main/java/edu/wpi/first/math/spline/*.java)
-  file(GLOB WPIMATH_SYSTEM_SOURCES src/main/java/edu/wpi/first/math/system/*.java)
-  file(GLOB WPIMATH_SYSTEM_PLANT_SOURCES src/main/java/edu/wpi/first/math/system/plant/*.java)
-  file(GLOB WPIMATH_TRAJECTORY_SOURCES src/main/java/edu/wpi/first/math/trajectory/*.java)
-  file(GLOB WPIMATH_TRAJECTORY_CONSTRAINT_SOURCES src/main/java/edu/wpi/first/math/trajectory/constraint/*.java)
-  add_jar(wpimath_src_jar
-  RESOURCES NAMESPACE "edu/wpi/first/math" ${WPIMATH_SOURCES}
-  NAMESPACE "edu/wpi/first/math/controller" ${WPIMATH_CONTROLLER_SOURCES}
-  NAMESPACE "edu/wpi/first/math/estimator" ${WPIMATH_ESTIMATOR_SOURCES}
-  NAMESPACE "edu/wpi/first/math/filter" ${WPIMATH_FILTER_SOURCES}
-  NAMESPACE "edu/wpi/first/math/geometry" ${WPIMATH_GEOMETRY_SOURCES}
-  NAMESPACE "edu/wpi/first/math/interpolation" ${WPIMATH_INTERPOLATION_SOURCES}
-  NAMESPACE "edu/wpi/first/math/kinematics" ${WPIMATH_KINEMATICS_SOURCES}
-  NAMESPACE "edu/wpi/first/math/spline" ${WPIMATH_SPLINE_SOURCES}
-  NAMESPACE "edu/wpi/first/math/system" ${WPIMATH_SYSTEM_SOURCES}
-  NAMESPACE "edu/wpi/first/math/system/plant" ${WPIMATH_SYSTEM_PLANT_SOURCES}
-  NAMESPACE "edu/wpi/first/math/trajectory" ${WPIMATH_TRAJECTORY_SOURCES}
-  NAMESPACE "edu/wpi/first/math/trajectory/constraint" ${WPIMATH_TRAJECTORY_CONSTRAINT_SOURCES}
-  NAMESPACE "edu/wpi/first/math/util" src/main/java/edu/wpi/first/math/util/Units.java
-  OUTPUT_NAME wpimath-sources)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(
+        GLOB WPIMATH_SOURCES
+        src/main/java/edu/wpi/first/math/*.java
+        src/generated/main/java/edu/wpi/first/math/Nat.java
+    )
+    file(GLOB WPIMATH_CONTROLLER_SOURCES src/main/java/edu/wpi/first/math/controller/*.java)
+    file(GLOB WPIMATH_ESTIMATOR_SOURCES src/main/java/edu/wpi/first/math/estimator/*.java)
+    file(GLOB WPIMATH_FILTER_SOURCES src/main/java/edu/wpi/first/math/filter/*.java)
+    file(GLOB WPIMATH_GEOMETRY_SOURCES src/main/java/edu/wpi/first/math/geometry/*.java)
+    file(GLOB WPIMATH_INTERPOLATION_SOURCES src/main/java/edu/wpi/first/math/interpolation/*.java)
+    file(GLOB WPIMATH_KINEMATICS_SOURCES src/main/java/edu/wpi/first/math/kinematics/*.java)
+    file(GLOB WPIMATH_NUMBERS_SOURCES src/generated/main/java/edu/wpi/first/math/numbers/*.java)
+    file(GLOB WPIMATH_SPLINE_SOURCES src/main/java/edu/wpi/first/math/spline/*.java)
+    file(GLOB WPIMATH_SYSTEM_SOURCES src/main/java/edu/wpi/first/math/system/*.java)
+    file(GLOB WPIMATH_SYSTEM_PLANT_SOURCES src/main/java/edu/wpi/first/math/system/plant/*.java)
+    file(GLOB WPIMATH_TRAJECTORY_SOURCES src/main/java/edu/wpi/first/math/trajectory/*.java)
+    file(
+        GLOB WPIMATH_TRAJECTORY_CONSTRAINT_SOURCES
+        src/main/java/edu/wpi/first/math/trajectory/constraint/*.java
+    )
+    add_jar(
+        wpimath_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/math" ${WPIMATH_SOURCES}
+        NAMESPACE "edu/wpi/first/math/controller" ${WPIMATH_CONTROLLER_SOURCES}
+        NAMESPACE "edu/wpi/first/math/estimator" ${WPIMATH_ESTIMATOR_SOURCES}
+        NAMESPACE "edu/wpi/first/math/filter" ${WPIMATH_FILTER_SOURCES}
+        NAMESPACE "edu/wpi/first/math/geometry" ${WPIMATH_GEOMETRY_SOURCES}
+        NAMESPACE "edu/wpi/first/math/interpolation" ${WPIMATH_INTERPOLATION_SOURCES}
+        NAMESPACE "edu/wpi/first/math/kinematics" ${WPIMATH_KINEMATICS_SOURCES}
+        NAMESPACE "edu/wpi/first/math/spline" ${WPIMATH_SPLINE_SOURCES}
+        NAMESPACE "edu/wpi/first/math/system" ${WPIMATH_SYSTEM_SOURCES}
+        NAMESPACE "edu/wpi/first/math/system/plant" ${WPIMATH_SYSTEM_PLANT_SOURCES}
+        NAMESPACE "edu/wpi/first/math/trajectory" ${WPIMATH_TRAJECTORY_SOURCES}
+        NAMESPACE
+            "edu/wpi/first/math/trajectory/constraint"
+            ${WPIMATH_TRAJECTORY_CONSTRAINT_SOURCES}
+        NAMESPACE "edu/wpi/first/math/util" src/main/java/edu/wpi/first/math/util/Units.java
+        OUTPUT_NAME wpimath-sources
+    )
 
-  get_property(WPIMATH_SRC_JAR_FILE TARGET wpimath_src_jar PROPERTY JAR_FILE)
-  install(FILES ${WPIMATH_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+    get_property(WPIMATH_SRC_JAR_FILE TARGET wpimath_src_jar PROPERTY JAR_FILE)
+    install(FILES ${WPIMATH_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-  set_property(TARGET wpimath_src_jar PROPERTY FOLDER "java")
+    set_property(TARGET wpimath_src_jar PROPERTY FOLDER "java")
 endif()
 
 file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp)
@@ -180,45 +160,52 @@
 target_compile_definitions(wpimath PRIVATE WPILIB_EXPORTS)
 
 target_compile_features(wpimath PUBLIC cxx_std_20)
-if (MSVC)
-    target_compile_options(wpimath PUBLIC /bigobj)
+if(MSVC)
+    target_compile_options(wpimath PUBLIC /utf-8 /bigobj)
 endif()
 wpilib_target_warnings(wpimath)
 target_link_libraries(wpimath wpiutil)
 
-if (NOT USE_SYSTEM_EIGEN)
-    install(DIRECTORY src/main/native/thirdparty/eigen/include/ DESTINATION "${include_dest}/wpimath")
-    target_include_directories(wpimath SYSTEM PUBLIC
-                               $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/eigen/include>
-                               $<INSTALL_INTERFACE:${include_dest}/wpimath>)
+if(NOT USE_SYSTEM_EIGEN)
+    install(
+        DIRECTORY src/main/native/thirdparty/eigen/include/
+        DESTINATION "${include_dest}/wpimath"
+    )
+    target_include_directories(
+        wpimath
+        SYSTEM
+        PUBLIC
+            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/eigen/include>
+            $<INSTALL_INTERFACE:${include_dest}/wpimath>
+    )
 else()
     find_package(Eigen3 CONFIG REQUIRED)
-    target_link_libraries (wpimath Eigen3::Eigen)
+    target_link_libraries(wpimath Eigen3::Eigen)
 endif()
 
 install(DIRECTORY src/main/native/thirdparty/gcem/include/ DESTINATION "${include_dest}/wpimath")
-target_include_directories(wpimath SYSTEM PUBLIC
-                          $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/gcem/include>)
+target_include_directories(
+    wpimath
+    SYSTEM
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/gcem/include>
+)
 
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpimath")
-target_include_directories(wpimath PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/protobuf>
-                            $<INSTALL_INTERFACE:${include_dest}/wpimath>)
+target_include_directories(
+    wpimath
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/protobuf>
+        $<INSTALL_INTERFACE:${include_dest}/wpimath>
+)
 
 install(TARGETS wpimath EXPORT wpimath)
 
-if (WITH_FLAT_INSTALL)
-    set (wpimath_config_dir ${wpilib_dest})
-else()
-    set (wpimath_config_dir share/wpimath)
-endif()
+configure_file(wpimath-config.cmake.in ${WPILIB_BINARY_DIR}/wpimath-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/wpimath-config.cmake DESTINATION share/wpimath)
+install(EXPORT wpimath DESTINATION share/wpimath)
 
-configure_file(wpimath-config.cmake.in ${WPILIB_BINARY_DIR}/wpimath-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/wpimath-config.cmake DESTINATION ${wpimath_config_dir})
-install(EXPORT wpimath DESTINATION ${wpimath_config_dir})
-
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(wpimath src/test/native/cpp)
     target_include_directories(wpimath_test PRIVATE src/test/native/include)
     target_link_libraries(wpimath_test wpimath gmock_main)
diff --git a/third_party/allwpilib/wpimath/build.gradle b/third_party/allwpilib/wpimath/build.gradle
index 09df80c..e16e5e7 100644
--- a/third_party/allwpilib/wpimath/build.gradle
+++ b/third_party/allwpilib/wpimath/build.gradle
@@ -20,8 +20,14 @@
     from('src/main/native/thirdparty/gcem/include') {
         into '/'
     }
+    from("$buildDir/generated/source/proto/main/cpp") {
+        into '/wpimath/protobuf'
+        include '*.h'
+    }
 }
 
+cppHeadersZip.dependsOn generateProto
+
 model {
     components {
         all {
@@ -36,76 +42,37 @@
     }
 }
 
+nativeUtils.exportsConfigs {
+    wpimath {
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        objectFilterClosure = { file ->
+            return file.name.endsWith('.pb.obj')
+        }
+    }
+}
+
 dependencies {
+    api project(":wpiunits")
     api "org.ejml:ejml-simple:0.43.1"
     api "com.fasterxml.jackson.core:jackson-annotations:2.15.2"
     api "com.fasterxml.jackson.core:jackson-core:2.15.2"
     api "com.fasterxml.jackson.core:jackson-databind:2.15.2"
-    api "us.hebi.quickbuf:quickbuf-runtime:1.3.2"
+    api "us.hebi.quickbuf:quickbuf-runtime:1.3.3"
 }
 
-def wpilibNumberFileInput = file("src/generate/GenericNumber.java.jinja")
-def natFileInput = file("src/generate/Nat.java.jinja")
-def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/math/numbers")
-def wpilibNatFileOutput = file("$buildDir/generated/java/edu/wpi/first/math/Nat.java")
-def maxNum = 20
-
-task generateNumbers() {
-    description = "Generates generic number classes from template"
-    group = "WPILib"
-
-    inputs.file wpilibNumberFileInput
-    outputs.dir wpilibNumberFileOutputDir
-
-    doLast {
-        if(wpilibNumberFileOutputDir.exists()) {
-            wpilibNumberFileOutputDir.delete()
-        }
-        wpilibNumberFileOutputDir.mkdirs()
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        def template = wpilibNumberFileInput.text
-
-        for(i in 0..maxNum) {
-            def outputFile = new File(wpilibNumberFileOutputDir, "N${i}.java")
-            def replacements = new HashMap<String,?>()
-            replacements.put("num", i)
-            def output = jinjava.render(template, replacements)
-            outputFile.write(output)
-        }
-    }
-}
-
-task generateNat() {
-    description = "Generates Nat.java"
-    group = "WPILib"
-    inputs.file natFileInput
-    outputs.file wpilibNatFileOutput
-    dependsOn generateNumbers
-
-    doLast {
-        if(wpilibNatFileOutput.exists()) {
-            wpilibNatFileOutput.delete()
-        }
-
-        def config = new JinjavaConfig()
-        def jinjava = new Jinjava(config)
-
-        def template = natFileInput.text
-
-        def replacements = new HashMap<String,?>()
-        replacements.put("nums", 0..maxNum)
-
-        def output = jinjava.render(template, replacements)
-        wpilibNatFileOutput.write(output)
-    }
-}
-
-sourceSets.main.java.srcDir "${buildDir}/generated/java"
-compileJava.dependsOn generateNumbers
-compileJava.dependsOn generateNat
+sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
 
 task unitsHeaders(type: Zip) {
     destinationDirectory = file("$buildDir/outputs")
diff --git a/third_party/allwpilib/wpimath/generate_numbers.py b/third_party/allwpilib/wpimath/generate_numbers.py
old mode 100644
new mode 100755
index c9da1a4..2aeb45a
--- a/third_party/allwpilib/wpimath/generate_numbers.py
+++ b/third_party/allwpilib/wpimath/generate_numbers.py
@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 # 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.
@@ -19,7 +21,7 @@
                 return
 
     # File either doesn't exist or has different contents
-    with open(outpathname, "w") as f:
+    with open(outpathname, "w", newline="\n") as f:
         f.write(contents)
 
 
@@ -27,23 +29,22 @@
     MAX_NUM = 20
 
     dirname, _ = os.path.split(os.path.abspath(__file__))
-    cmake_binary_dir = sys.argv[1]
 
     env = Environment(
-        loader=FileSystemLoader(f"{dirname}/src/generate"),
+        loader=FileSystemLoader(f"{dirname}/src/generate/main/java"),
         autoescape=False,
         keep_trailing_newline=True,
     )
 
     template = env.get_template("GenericNumber.java.jinja")
-    rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/math/numbers"
+    rootPath = f"{dirname}/src/generated/main/java/edu/wpi/first/math/numbers"
 
     for i in range(MAX_NUM + 1):
         contents = template.render(num=i)
         output(rootPath, f"N{i}.java", contents)
 
     template = env.get_template("Nat.java.jinja")
-    rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/math"
+    rootPath = f"{dirname}/src/generated/main/java/edu/wpi/first/math"
     contents = template.render(nums=range(MAX_NUM + 1))
     output(rootPath, "Nat.java", contents)
 
diff --git a/third_party/allwpilib/wpimath/generate_quickbuf.py b/third_party/allwpilib/wpimath/generate_quickbuf.py
new file mode 100755
index 0000000..506c5bc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/generate_quickbuf.py
@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+
+# 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.
+import os.path
+import subprocess
+import sys
+from glob import glob
+
+if __name__ == "__main__":
+    proto_files = glob("wpimath/src/main/proto/*.proto")
+    for path in proto_files:
+        absolute_filename = os.path.abspath(path)
+        absolute_dir, filename = os.path.split(absolute_filename)
+        subprocess.run(
+            [
+                sys.argv[1],
+                f"--plugin=protoc-gen-quickbuf={sys.argv[2]}",
+                f"--quickbuf_out=gen_descriptors=true:{os.path.abspath('./wpimath/src/generated/main/java')}",
+                f"-I{absolute_dir}",
+                absolute_filename,
+            ]
+        )
+    java_files = glob("wpimath/src/generated/main/java/edu/wpi/first/math/proto/*.java")
+    for java_file in java_files:
+        with open(java_file) as file:
+            content = file.read()
+        with open(java_file, "tw") as file:
+            file.write(
+                "// Copyright (c) FIRST and other WPILib contributors.\n// Open Source Software; you can modify and/or share it under the terms of\n// the WPILib BSD license file in the root directory of this project.\n"
+                + content
+            )
diff --git a/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.jinja b/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.jinja
deleted file mode 100644
index 5e4be85..0000000
--- a/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.jinja
+++ /dev/null
@@ -1,31 +0,0 @@
-// 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.
-
-package edu.wpi.first.math.numbers;
-
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.Num;
-
-/**
- * A class representing the number {{ num }}.
-*/
-public final class N{{ num }} extends Num implements Nat<N{{ num }}> {
-  private N{{ num }}() {
-  }
-
-  /**
-   * The integer this class represents.
-   *
-   * @return The literal number {{ num }}.
-  */
-  @Override
-  public int getNum() {
-    return {{ num }};
-  }
-
-  /**
-   * The singleton instance of this class.
-  */
-  public static final N{{ num }} instance = new N{{ num }}();
-}
diff --git a/third_party/allwpilib/wpimath/src/generate/Nat.java.jinja b/third_party/allwpilib/wpimath/src/generate/Nat.java.jinja
deleted file mode 100644
index cbc0ddb..0000000
--- a/third_party/allwpilib/wpimath/src/generate/Nat.java.jinja
+++ /dev/null
@@ -1,31 +0,0 @@
-// 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.
-
-package edu.wpi.first.math;
-
-//CHECKSTYLE.OFF: ImportOrder
-{% for num in nums %}
-import edu.wpi.first.math.numbers.N{{ num }};
-{%- endfor %}
-//CHECKSTYLE.ON
-
-/**
- * A natural number expressed as a java class.
- * The counterpart to {@link Num} that should be used as a concrete value.
- *
- * @param <T> The {@link Num} this represents.
- */
-public interface Nat<T extends Num> {
-  /**
-   * The number this interface represents.
-   *
-   * @return The number backing this value.
-   */
-  int getNum();
-{% for num in nums %}
-  static Nat<N{{ num }}> N{{ num }}() {
-    return N{{ num }}.instance;
-  }
-{% endfor %}
-}
diff --git a/third_party/allwpilib/wpimath/src/generate/main/java/GenericNumber.java.jinja b/third_party/allwpilib/wpimath/src/generate/main/java/GenericNumber.java.jinja
new file mode 100644
index 0000000..2bd940b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generate/main/java/GenericNumber.java.jinja
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number {{ num }}. */
+public final class N{{ num }} extends Num implements Nat<N{{ num }}> {
+  private N{{ num }}() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number {{ num }}.
+   */
+  @Override
+  public int getNum() {
+    return {{ num }};
+  }
+
+  /** The singleton instance of this class. */
+  public static final N{{ num }} instance = new N{{ num }}();
+}
diff --git a/third_party/allwpilib/wpimath/src/generate/main/java/Nat.java.jinja b/third_party/allwpilib/wpimath/src/generate/main/java/Nat.java.jinja
new file mode 100644
index 0000000..66f0639
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generate/main/java/Nat.java.jinja
@@ -0,0 +1,38 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math;
+
+// CHECKSTYLE.OFF: ImportOrder
+{% for num in nums %}
+import edu.wpi.first.math.numbers.N{{ num }};
+{%- endfor %}
+// CHECKSTYLE.ON
+
+/**
+ * A natural number expressed as a java class.
+ * The counterpart to {@link Num} that should be used as a concrete value.
+ *
+ * @param <T> The {@link Num} this represents.
+ */
+public interface Nat<T extends Num> {
+  /**
+   * The number this interface represents.
+   *
+   * @return The number backing this value.
+   */
+  int getNum();
+{% for num in nums %}
+  /**
+   * Returns the Nat instance for {{ num }}.
+   *
+   * @return The Nat instance for {{ num }}.
+   */
+  static Nat<N{{ num }}> N{{ num }}() {
+    return N{{ num }}.instance;
+  }
+{% endfor -%}
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/Nat.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/Nat.java
new file mode 100644
index 0000000..f6b0b81
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/Nat.java
@@ -0,0 +1,236 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math;
+
+// CHECKSTYLE.OFF: ImportOrder
+
+import edu.wpi.first.math.numbers.N0;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.numbers.N8;
+import edu.wpi.first.math.numbers.N9;
+import edu.wpi.first.math.numbers.N10;
+import edu.wpi.first.math.numbers.N11;
+import edu.wpi.first.math.numbers.N12;
+import edu.wpi.first.math.numbers.N13;
+import edu.wpi.first.math.numbers.N14;
+import edu.wpi.first.math.numbers.N15;
+import edu.wpi.first.math.numbers.N16;
+import edu.wpi.first.math.numbers.N17;
+import edu.wpi.first.math.numbers.N18;
+import edu.wpi.first.math.numbers.N19;
+import edu.wpi.first.math.numbers.N20;
+// CHECKSTYLE.ON
+
+/**
+ * A natural number expressed as a java class.
+ * The counterpart to {@link Num} that should be used as a concrete value.
+ *
+ * @param <T> The {@link Num} this represents.
+ */
+public interface Nat<T extends Num> {
+  /**
+   * The number this interface represents.
+   *
+   * @return The number backing this value.
+   */
+  int getNum();
+
+  /**
+   * Returns the Nat instance for 0.
+   *
+   * @return The Nat instance for 0.
+   */
+  static Nat<N0> N0() {
+    return N0.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 1.
+   *
+   * @return The Nat instance for 1.
+   */
+  static Nat<N1> N1() {
+    return N1.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 2.
+   *
+   * @return The Nat instance for 2.
+   */
+  static Nat<N2> N2() {
+    return N2.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 3.
+   *
+   * @return The Nat instance for 3.
+   */
+  static Nat<N3> N3() {
+    return N3.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 4.
+   *
+   * @return The Nat instance for 4.
+   */
+  static Nat<N4> N4() {
+    return N4.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 5.
+   *
+   * @return The Nat instance for 5.
+   */
+  static Nat<N5> N5() {
+    return N5.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 6.
+   *
+   * @return The Nat instance for 6.
+   */
+  static Nat<N6> N6() {
+    return N6.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 7.
+   *
+   * @return The Nat instance for 7.
+   */
+  static Nat<N7> N7() {
+    return N7.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 8.
+   *
+   * @return The Nat instance for 8.
+   */
+  static Nat<N8> N8() {
+    return N8.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 9.
+   *
+   * @return The Nat instance for 9.
+   */
+  static Nat<N9> N9() {
+    return N9.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 10.
+   *
+   * @return The Nat instance for 10.
+   */
+  static Nat<N10> N10() {
+    return N10.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 11.
+   *
+   * @return The Nat instance for 11.
+   */
+  static Nat<N11> N11() {
+    return N11.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 12.
+   *
+   * @return The Nat instance for 12.
+   */
+  static Nat<N12> N12() {
+    return N12.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 13.
+   *
+   * @return The Nat instance for 13.
+   */
+  static Nat<N13> N13() {
+    return N13.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 14.
+   *
+   * @return The Nat instance for 14.
+   */
+  static Nat<N14> N14() {
+    return N14.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 15.
+   *
+   * @return The Nat instance for 15.
+   */
+  static Nat<N15> N15() {
+    return N15.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 16.
+   *
+   * @return The Nat instance for 16.
+   */
+  static Nat<N16> N16() {
+    return N16.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 17.
+   *
+   * @return The Nat instance for 17.
+   */
+  static Nat<N17> N17() {
+    return N17.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 18.
+   *
+   * @return The Nat instance for 18.
+   */
+  static Nat<N18> N18() {
+    return N18.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 19.
+   *
+   * @return The Nat instance for 19.
+   */
+  static Nat<N19> N19() {
+    return N19.instance;
+  }
+
+  /**
+   * Returns the Nat instance for 20.
+   *
+   * @return The Nat instance for 20.
+   */
+  static Nat<N20> N20() {
+    return N20.instance;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N0.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N0.java
new file mode 100644
index 0000000..85065f7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N0.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 0. */
+public final class N0 extends Num implements Nat<N0> {
+  private N0() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 0.
+   */
+  @Override
+  public int getNum() {
+    return 0;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N0 instance = new N0();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N1.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N1.java
new file mode 100644
index 0000000..8977a7a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N1.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 1. */
+public final class N1 extends Num implements Nat<N1> {
+  private N1() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 1.
+   */
+  @Override
+  public int getNum() {
+    return 1;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N1 instance = new N1();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N10.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N10.java
new file mode 100644
index 0000000..540f1d3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N10.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 10. */
+public final class N10 extends Num implements Nat<N10> {
+  private N10() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 10.
+   */
+  @Override
+  public int getNum() {
+    return 10;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N10 instance = new N10();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N11.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N11.java
new file mode 100644
index 0000000..45113c9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N11.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 11. */
+public final class N11 extends Num implements Nat<N11> {
+  private N11() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 11.
+   */
+  @Override
+  public int getNum() {
+    return 11;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N11 instance = new N11();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N12.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N12.java
new file mode 100644
index 0000000..6a86c80
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N12.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 12. */
+public final class N12 extends Num implements Nat<N12> {
+  private N12() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 12.
+   */
+  @Override
+  public int getNum() {
+    return 12;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N12 instance = new N12();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N13.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N13.java
new file mode 100644
index 0000000..7ae2ca1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N13.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 13. */
+public final class N13 extends Num implements Nat<N13> {
+  private N13() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 13.
+   */
+  @Override
+  public int getNum() {
+    return 13;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N13 instance = new N13();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N14.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N14.java
new file mode 100644
index 0000000..58a4192
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N14.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 14. */
+public final class N14 extends Num implements Nat<N14> {
+  private N14() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 14.
+   */
+  @Override
+  public int getNum() {
+    return 14;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N14 instance = new N14();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N15.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N15.java
new file mode 100644
index 0000000..5794dac
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N15.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 15. */
+public final class N15 extends Num implements Nat<N15> {
+  private N15() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 15.
+   */
+  @Override
+  public int getNum() {
+    return 15;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N15 instance = new N15();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N16.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N16.java
new file mode 100644
index 0000000..3727463
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N16.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 16. */
+public final class N16 extends Num implements Nat<N16> {
+  private N16() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 16.
+   */
+  @Override
+  public int getNum() {
+    return 16;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N16 instance = new N16();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N17.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N17.java
new file mode 100644
index 0000000..e2968d7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N17.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 17. */
+public final class N17 extends Num implements Nat<N17> {
+  private N17() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 17.
+   */
+  @Override
+  public int getNum() {
+    return 17;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N17 instance = new N17();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N18.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N18.java
new file mode 100644
index 0000000..0310893
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N18.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 18. */
+public final class N18 extends Num implements Nat<N18> {
+  private N18() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 18.
+   */
+  @Override
+  public int getNum() {
+    return 18;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N18 instance = new N18();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N19.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N19.java
new file mode 100644
index 0000000..8c5d48d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N19.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 19. */
+public final class N19 extends Num implements Nat<N19> {
+  private N19() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 19.
+   */
+  @Override
+  public int getNum() {
+    return 19;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N19 instance = new N19();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N2.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N2.java
new file mode 100644
index 0000000..27f3257
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N2.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 2. */
+public final class N2 extends Num implements Nat<N2> {
+  private N2() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 2.
+   */
+  @Override
+  public int getNum() {
+    return 2;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N2 instance = new N2();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N20.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N20.java
new file mode 100644
index 0000000..fefbfe6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N20.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 20. */
+public final class N20 extends Num implements Nat<N20> {
+  private N20() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 20.
+   */
+  @Override
+  public int getNum() {
+    return 20;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N20 instance = new N20();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N3.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N3.java
new file mode 100644
index 0000000..1e10c90
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N3.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 3. */
+public final class N3 extends Num implements Nat<N3> {
+  private N3() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 3.
+   */
+  @Override
+  public int getNum() {
+    return 3;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N3 instance = new N3();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N4.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N4.java
new file mode 100644
index 0000000..f7fe57e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N4.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 4. */
+public final class N4 extends Num implements Nat<N4> {
+  private N4() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 4.
+   */
+  @Override
+  public int getNum() {
+    return 4;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N4 instance = new N4();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N5.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N5.java
new file mode 100644
index 0000000..ec5deb5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N5.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 5. */
+public final class N5 extends Num implements Nat<N5> {
+  private N5() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 5.
+   */
+  @Override
+  public int getNum() {
+    return 5;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N5 instance = new N5();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N6.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N6.java
new file mode 100644
index 0000000..fe94f08
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N6.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 6. */
+public final class N6 extends Num implements Nat<N6> {
+  private N6() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 6.
+   */
+  @Override
+  public int getNum() {
+    return 6;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N6 instance = new N6();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N7.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N7.java
new file mode 100644
index 0000000..241c992
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N7.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 7. */
+public final class N7 extends Num implements Nat<N7> {
+  private N7() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 7.
+   */
+  @Override
+  public int getNum() {
+    return 7;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N7 instance = new N7();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N8.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N8.java
new file mode 100644
index 0000000..4930b00
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N8.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 8. */
+public final class N8 extends Num implements Nat<N8> {
+  private N8() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 8.
+   */
+  @Override
+  public int getNum() {
+    return 8;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N8 instance = new N8();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N9.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N9.java
new file mode 100644
index 0000000..2c6e4ee
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/numbers/N9.java
@@ -0,0 +1,28 @@
+// 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.
+
+// THIS FILE WAS AUTO-GENERATED BY ./wpimath/generate_numbers.py. DO NOT MODIFY
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/** A class representing the number 9. */
+public final class N9 extends Num implements Nat<N9> {
+  private N9() {}
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number 9.
+   */
+  @Override
+  public int getNum() {
+    return 9;
+  }
+
+  /** The singleton instance of this class. */
+  public static final N9 instance = new N9();
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java
new file mode 100644
index 0000000..ed423da
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java
@@ -0,0 +1,2309 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+
+public final class Controller {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1684,
+    "ChBjb250cm9sbGVyLnByb3RvEgl3cGkucHJvdG8iWAoWUHJvdG9idWZBcm1GZWVkZm9yd2FyZBIOCgJr" + 
+    "cxgBIAEoAVICa3MSDgoCa2cYAiABKAFSAmtnEg4KAmt2GAMgASgBUgJrdhIOCgJrYRgEIAEoAVICa2Ei" + 
+    "ngEKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVGZWVkZm9yd2FyZBIbCglrdl9saW5lYXIYASABKAFS" + 
+    "CGt2TGluZWFyEhsKCWthX2xpbmVhchgCIAEoAVIIa2FMaW5lYXISHQoKa3ZfYW5ndWxhchgDIAEoAVIJ" + 
+    "a3ZBbmd1bGFyEh0KCmthX2FuZ3VsYXIYBCABKAFSCWthQW5ndWxhciJdChtQcm90b2J1ZkVsZXZhdG9y" + 
+    "RmVlZGZvcndhcmQSDgoCa3MYASABKAFSAmtzEg4KAmtnGAIgASgBUgJrZxIOCgJrdhgDIAEoAVICa3YS" + 
+    "DgoCa2EYBCABKAFSAmthIlAKHlByb3RvYnVmU2ltcGxlTW90b3JGZWVkZm9yd2FyZBIOCgJrcxgBIAEo" + 
+    "AVICa3MSDgoCa3YYAiABKAFSAmt2Eg4KAmthGAMgASgBUgJrYSJSCiZQcm90b2J1ZkRpZmZlcmVudGlh" + 
+    "bERyaXZlV2hlZWxWb2x0YWdlcxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdo" + 
+    "dEIaChhlZHUud3BpLmZpcnN0Lm1hdGgucHJvdG9K0AgKBhIEAAAkAQoICgEMEgMAABIKCAoBAhIDAgAS" + 
+    "CggKAQgSAwQAMQoJCgIIARIDBAAxCgoKAgQAEgQGAAsBCgoKAwQAARIDBggeCgsKBAQAAgASAwcCEAoM" + 
+    "CgUEAAIABRIDBwIICgwKBQQAAgABEgMHCQsKDAoFBAACAAMSAwcODwoLCgQEAAIBEgMIAhAKDAoFBAAC" + 
+    "AQUSAwgCCAoMCgUEAAIBARIDCAkLCgwKBQQAAgEDEgMIDg8KCwoEBAACAhIDCQIQCgwKBQQAAgIFEgMJ" + 
+    "AggKDAoFBAACAgESAwkJCwoMCgUEAAICAxIDCQ4PCgsKBAQAAgMSAwoCEAoMCgUEAAIDBRIDCgIICgwK" + 
+    "BQQAAgMBEgMKCQsKDAoFBAACAwMSAwoODwoKCgIEARIEDQASAQoKCgMEAQESAw0ILAoLCgQEAQIAEgMO" + 
+    "AhcKDAoFBAECAAUSAw4CCAoMCgUEAQIAARIDDgkSCgwKBQQBAgADEgMOFRYKCwoEBAECARIDDwIXCgwK" + 
+    "BQQBAgEFEgMPAggKDAoFBAECAQESAw8JEgoMCgUEAQIBAxIDDxUWCgsKBAQBAgISAxACGAoMCgUEAQIC" + 
+    "BRIDEAIICgwKBQQBAgIBEgMQCRMKDAoFBAECAgMSAxAWFwoLCgQEAQIDEgMRAhgKDAoFBAECAwUSAxEC" + 
+    "CAoMCgUEAQIDARIDEQkTCgwKBQQBAgMDEgMRFhcKCgoCBAISBBQAGQEKCgoDBAIBEgMUCCMKCwoEBAIC" + 
+    "ABIDFQIQCgwKBQQCAgAFEgMVAggKDAoFBAICAAESAxUJCwoMCgUEAgIAAxIDFQ4PCgsKBAQCAgESAxYC",
+    "EAoMCgUEAgIBBRIDFgIICgwKBQQCAgEBEgMWCQsKDAoFBAICAQMSAxYODwoLCgQEAgICEgMXAhAKDAoF" + 
+    "BAICAgUSAxcCCAoMCgUEAgICARIDFwkLCgwKBQQCAgIDEgMXDg8KCwoEBAICAxIDGAIQCgwKBQQCAgMF" + 
+    "EgMYAggKDAoFBAICAwESAxgJCwoMCgUEAgIDAxIDGA4PCgoKAgQDEgQbAB8BCgoKAwQDARIDGwgmCgsK" + 
+    "BAQDAgASAxwCEAoMCgUEAwIABRIDHAIICgwKBQQDAgABEgMcCQsKDAoFBAMCAAMSAxwODwoLCgQEAwIB" + 
+    "EgMdAhAKDAoFBAMCAQUSAx0CCAoMCgUEAwIBARIDHQkLCgwKBQQDAgEDEgMdDg8KCwoEBAMCAhIDHgIQ" + 
+    "CgwKBQQDAgIFEgMeAggKDAoFBAMCAgESAx4JCwoMCgUEAwICAxIDHg4PCgoKAgQEEgQhACQBCgoKAwQE" + 
+    "ARIDIQguCgsKBAQEAgASAyICEgoMCgUEBAIABRIDIgIICgwKBQQEAgABEgMiCQ0KDAoFBAQCAAMSAyIQ" + 
+    "EQoLCgQEBAIBEgMjAhMKDAoFBAQCAQUSAyMCCAoMCgUEBAIBARIDIwkOCgwKBQQEAgEDEgMjERJiBnBy" + 
+    "b3RvMw==");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("controller.proto", "wpi.proto", descriptorData);
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufArmFeedforward_descriptor = descriptor.internalContainedType(31, 88, "ProtobufArmFeedforward", "wpi.proto.ProtobufArmFeedforward");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveFeedforward_descriptor = descriptor.internalContainedType(122, 158, "ProtobufDifferentialDriveFeedforward", "wpi.proto.ProtobufDifferentialDriveFeedforward");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufElevatorFeedforward_descriptor = descriptor.internalContainedType(282, 93, "ProtobufElevatorFeedforward", "wpi.proto.ProtobufElevatorFeedforward");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufSimpleMotorFeedforward_descriptor = descriptor.internalContainedType(377, 80, "ProtobufSimpleMotorFeedforward", "wpi.proto.ProtobufSimpleMotorFeedforward");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor = descriptor.internalContainedType(459, 82, "ProtobufDifferentialDriveWheelVoltages", "wpi.proto.ProtobufDifferentialDriveWheelVoltages");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufArmFeedforward}
+   */
+  public static final class ProtobufArmFeedforward extends ProtoMessage<ProtobufArmFeedforward> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double ks = 1;</code>
+     */
+    private double ks;
+
+    /**
+     * <code>optional double kg = 2;</code>
+     */
+    private double kg;
+
+    /**
+     * <code>optional double kv = 3;</code>
+     */
+    private double kv;
+
+    /**
+     * <code>optional double ka = 4;</code>
+     */
+    private double ka;
+
+    private ProtobufArmFeedforward() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufArmFeedforward}
+     */
+    public static ProtobufArmFeedforward newInstance() {
+      return new ProtobufArmFeedforward();
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return whether the ks field is set
+     */
+    public boolean hasKs() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return this
+     */
+    public ProtobufArmFeedforward clearKs() {
+      bitField0_ &= ~0x00000001;
+      ks = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return the ks
+     */
+    public double getKs() {
+      return ks;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @param value the ks to set
+     * @return this
+     */
+    public ProtobufArmFeedforward setKs(final double value) {
+      bitField0_ |= 0x00000001;
+      ks = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @return whether the kg field is set
+     */
+    public boolean hasKg() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @return this
+     */
+    public ProtobufArmFeedforward clearKg() {
+      bitField0_ &= ~0x00000002;
+      kg = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @return the kg
+     */
+    public double getKg() {
+      return kg;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @param value the kg to set
+     * @return this
+     */
+    public ProtobufArmFeedforward setKg(final double value) {
+      bitField0_ |= 0x00000002;
+      kg = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @return whether the kv field is set
+     */
+    public boolean hasKv() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @return this
+     */
+    public ProtobufArmFeedforward clearKv() {
+      bitField0_ &= ~0x00000004;
+      kv = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @return the kv
+     */
+    public double getKv() {
+      return kv;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @param value the kv to set
+     * @return this
+     */
+    public ProtobufArmFeedforward setKv(final double value) {
+      bitField0_ |= 0x00000004;
+      kv = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @return whether the ka field is set
+     */
+    public boolean hasKa() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @return this
+     */
+    public ProtobufArmFeedforward clearKa() {
+      bitField0_ &= ~0x00000008;
+      ka = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @return the ka
+     */
+    public double getKa() {
+      return ka;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @param value the ka to set
+     * @return this
+     */
+    public ProtobufArmFeedforward setKa(final double value) {
+      bitField0_ |= 0x00000008;
+      ka = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufArmFeedforward copyFrom(final ProtobufArmFeedforward other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        ks = other.ks;
+        kg = other.kg;
+        kv = other.kv;
+        ka = other.ka;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufArmFeedforward mergeFrom(final ProtobufArmFeedforward other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasKs()) {
+        setKs(other.ks);
+      }
+      if (other.hasKg()) {
+        setKg(other.kg);
+      }
+      if (other.hasKv()) {
+        setKv(other.kv);
+      }
+      if (other.hasKa()) {
+        setKa(other.ka);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufArmFeedforward clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      ks = 0D;
+      kg = 0D;
+      kv = 0D;
+      ka = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufArmFeedforward clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufArmFeedforward)) {
+        return false;
+      }
+      ProtobufArmFeedforward other = (ProtobufArmFeedforward) o;
+      return bitField0_ == other.bitField0_
+        && (!hasKs() || ProtoUtil.isEqual(ks, other.ks))
+        && (!hasKg() || ProtoUtil.isEqual(kg, other.kg))
+        && (!hasKv() || ProtoUtil.isEqual(kv, other.kv))
+        && (!hasKa() || ProtoUtil.isEqual(ka, other.ka));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(ks);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(kg);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(kv);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(ka);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufArmFeedforward mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // ks
+            ks = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // kg
+            kg = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // kv
+            kv = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // ka
+            ka = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.ks, ks);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.kg, kg);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.kv, kv);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.ka, ka);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufArmFeedforward mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3432: {
+            if (input.isAtField(FieldNames.ks)) {
+              if (!input.trySkipNullValue()) {
+                ks = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3420: {
+            if (input.isAtField(FieldNames.kg)) {
+              if (!input.trySkipNullValue()) {
+                kg = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3435: {
+            if (input.isAtField(FieldNames.kv)) {
+              if (!input.trySkipNullValue()) {
+                kv = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3414: {
+            if (input.isAtField(FieldNames.ka)) {
+              if (!input.trySkipNullValue()) {
+                ka = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufArmFeedforward clone() {
+      return new ProtobufArmFeedforward().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufArmFeedforward parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufArmFeedforward(), data).checkInitialized();
+    }
+
+    public static ProtobufArmFeedforward parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufArmFeedforward(), input).checkInitialized();
+    }
+
+    public static ProtobufArmFeedforward parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufArmFeedforward(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufArmFeedforward messages
+     */
+    public static MessageFactory<ProtobufArmFeedforward> getFactory() {
+      return ProtobufArmFeedforwardFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Controller.wpi_proto_ProtobufArmFeedforward_descriptor;
+    }
+
+    private enum ProtobufArmFeedforwardFactory implements MessageFactory<ProtobufArmFeedforward> {
+      INSTANCE;
+
+      @Override
+      public ProtobufArmFeedforward create() {
+        return ProtobufArmFeedforward.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName ks = FieldName.forField("ks");
+
+      static final FieldName kg = FieldName.forField("kg");
+
+      static final FieldName kv = FieldName.forField("kv");
+
+      static final FieldName ka = FieldName.forField("ka");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufDifferentialDriveFeedforward}
+   */
+  public static final class ProtobufDifferentialDriveFeedforward extends ProtoMessage<ProtobufDifferentialDriveFeedforward> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double kv_linear = 1;</code>
+     */
+    private double kvLinear;
+
+    /**
+     * <code>optional double ka_linear = 2;</code>
+     */
+    private double kaLinear;
+
+    /**
+     * <code>optional double kv_angular = 3;</code>
+     */
+    private double kvAngular;
+
+    /**
+     * <code>optional double ka_angular = 4;</code>
+     */
+    private double kaAngular;
+
+    private ProtobufDifferentialDriveFeedforward() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufDifferentialDriveFeedforward}
+     */
+    public static ProtobufDifferentialDriveFeedforward newInstance() {
+      return new ProtobufDifferentialDriveFeedforward();
+    }
+
+    /**
+     * <code>optional double kv_linear = 1;</code>
+     * @return whether the kvLinear field is set
+     */
+    public boolean hasKvLinear() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double kv_linear = 1;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward clearKvLinear() {
+      bitField0_ &= ~0x00000001;
+      kvLinear = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv_linear = 1;</code>
+     * @return the kvLinear
+     */
+    public double getKvLinear() {
+      return kvLinear;
+    }
+
+    /**
+     * <code>optional double kv_linear = 1;</code>
+     * @param value the kvLinear to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward setKvLinear(final double value) {
+      bitField0_ |= 0x00000001;
+      kvLinear = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka_linear = 2;</code>
+     * @return whether the kaLinear field is set
+     */
+    public boolean hasKaLinear() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double ka_linear = 2;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward clearKaLinear() {
+      bitField0_ &= ~0x00000002;
+      kaLinear = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka_linear = 2;</code>
+     * @return the kaLinear
+     */
+    public double getKaLinear() {
+      return kaLinear;
+    }
+
+    /**
+     * <code>optional double ka_linear = 2;</code>
+     * @param value the kaLinear to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward setKaLinear(final double value) {
+      bitField0_ |= 0x00000002;
+      kaLinear = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv_angular = 3;</code>
+     * @return whether the kvAngular field is set
+     */
+    public boolean hasKvAngular() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double kv_angular = 3;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward clearKvAngular() {
+      bitField0_ &= ~0x00000004;
+      kvAngular = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv_angular = 3;</code>
+     * @return the kvAngular
+     */
+    public double getKvAngular() {
+      return kvAngular;
+    }
+
+    /**
+     * <code>optional double kv_angular = 3;</code>
+     * @param value the kvAngular to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward setKvAngular(final double value) {
+      bitField0_ |= 0x00000004;
+      kvAngular = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka_angular = 4;</code>
+     * @return whether the kaAngular field is set
+     */
+    public boolean hasKaAngular() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double ka_angular = 4;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward clearKaAngular() {
+      bitField0_ &= ~0x00000008;
+      kaAngular = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka_angular = 4;</code>
+     * @return the kaAngular
+     */
+    public double getKaAngular() {
+      return kaAngular;
+    }
+
+    /**
+     * <code>optional double ka_angular = 4;</code>
+     * @param value the kaAngular to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveFeedforward setKaAngular(final double value) {
+      bitField0_ |= 0x00000008;
+      kaAngular = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveFeedforward copyFrom(
+        final ProtobufDifferentialDriveFeedforward other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        kvLinear = other.kvLinear;
+        kaLinear = other.kaLinear;
+        kvAngular = other.kvAngular;
+        kaAngular = other.kaAngular;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveFeedforward mergeFrom(
+        final ProtobufDifferentialDriveFeedforward other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasKvLinear()) {
+        setKvLinear(other.kvLinear);
+      }
+      if (other.hasKaLinear()) {
+        setKaLinear(other.kaLinear);
+      }
+      if (other.hasKvAngular()) {
+        setKvAngular(other.kvAngular);
+      }
+      if (other.hasKaAngular()) {
+        setKaAngular(other.kaAngular);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveFeedforward clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      kvLinear = 0D;
+      kaLinear = 0D;
+      kvAngular = 0D;
+      kaAngular = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveFeedforward clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufDifferentialDriveFeedforward)) {
+        return false;
+      }
+      ProtobufDifferentialDriveFeedforward other = (ProtobufDifferentialDriveFeedforward) o;
+      return bitField0_ == other.bitField0_
+        && (!hasKvLinear() || ProtoUtil.isEqual(kvLinear, other.kvLinear))
+        && (!hasKaLinear() || ProtoUtil.isEqual(kaLinear, other.kaLinear))
+        && (!hasKvAngular() || ProtoUtil.isEqual(kvAngular, other.kvAngular))
+        && (!hasKaAngular() || ProtoUtil.isEqual(kaAngular, other.kaAngular));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(kvLinear);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(kaLinear);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(kvAngular);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(kaAngular);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufDifferentialDriveFeedforward mergeFrom(final ProtoSource input) throws
+        IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // kvLinear
+            kvLinear = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // kaLinear
+            kaLinear = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // kvAngular
+            kvAngular = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // kaAngular
+            kaAngular = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.kvLinear, kvLinear);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.kaLinear, kaLinear);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.kvAngular, kvAngular);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.kaAngular, kaAngular);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufDifferentialDriveFeedforward mergeFrom(final JsonSource input) throws
+        IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 1424526448:
+          case 974889081: {
+            if (input.isAtField(FieldNames.kvLinear)) {
+              if (!input.trySkipNullValue()) {
+                kvLinear = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -33181669:
+          case -1264389586: {
+            if (input.isAtField(FieldNames.kaLinear)) {
+              if (!input.trySkipNullValue()) {
+                kaLinear = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 175208951:
+          case -878647538: {
+            if (input.isAtField(FieldNames.kvAngular)) {
+              if (!input.trySkipNullValue()) {
+                kvAngular = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -2064069716:
+          case -1576809479: {
+            if (input.isAtField(FieldNames.kaAngular)) {
+              if (!input.trySkipNullValue()) {
+                kaAngular = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveFeedforward clone() {
+      return new ProtobufDifferentialDriveFeedforward().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufDifferentialDriveFeedforward parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveFeedforward(), data).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveFeedforward parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveFeedforward(), input).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveFeedforward parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveFeedforward(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufDifferentialDriveFeedforward messages
+     */
+    public static MessageFactory<ProtobufDifferentialDriveFeedforward> getFactory() {
+      return ProtobufDifferentialDriveFeedforwardFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Controller.wpi_proto_ProtobufDifferentialDriveFeedforward_descriptor;
+    }
+
+    private enum ProtobufDifferentialDriveFeedforwardFactory implements MessageFactory<ProtobufDifferentialDriveFeedforward> {
+      INSTANCE;
+
+      @Override
+      public ProtobufDifferentialDriveFeedforward create() {
+        return ProtobufDifferentialDriveFeedforward.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName kvLinear = FieldName.forField("kvLinear", "kv_linear");
+
+      static final FieldName kaLinear = FieldName.forField("kaLinear", "ka_linear");
+
+      static final FieldName kvAngular = FieldName.forField("kvAngular", "kv_angular");
+
+      static final FieldName kaAngular = FieldName.forField("kaAngular", "ka_angular");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufElevatorFeedforward}
+   */
+  public static final class ProtobufElevatorFeedforward extends ProtoMessage<ProtobufElevatorFeedforward> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double ks = 1;</code>
+     */
+    private double ks;
+
+    /**
+     * <code>optional double kg = 2;</code>
+     */
+    private double kg;
+
+    /**
+     * <code>optional double kv = 3;</code>
+     */
+    private double kv;
+
+    /**
+     * <code>optional double ka = 4;</code>
+     */
+    private double ka;
+
+    private ProtobufElevatorFeedforward() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufElevatorFeedforward}
+     */
+    public static ProtobufElevatorFeedforward newInstance() {
+      return new ProtobufElevatorFeedforward();
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return whether the ks field is set
+     */
+    public boolean hasKs() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return this
+     */
+    public ProtobufElevatorFeedforward clearKs() {
+      bitField0_ &= ~0x00000001;
+      ks = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return the ks
+     */
+    public double getKs() {
+      return ks;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @param value the ks to set
+     * @return this
+     */
+    public ProtobufElevatorFeedforward setKs(final double value) {
+      bitField0_ |= 0x00000001;
+      ks = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @return whether the kg field is set
+     */
+    public boolean hasKg() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @return this
+     */
+    public ProtobufElevatorFeedforward clearKg() {
+      bitField0_ &= ~0x00000002;
+      kg = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @return the kg
+     */
+    public double getKg() {
+      return kg;
+    }
+
+    /**
+     * <code>optional double kg = 2;</code>
+     * @param value the kg to set
+     * @return this
+     */
+    public ProtobufElevatorFeedforward setKg(final double value) {
+      bitField0_ |= 0x00000002;
+      kg = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @return whether the kv field is set
+     */
+    public boolean hasKv() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @return this
+     */
+    public ProtobufElevatorFeedforward clearKv() {
+      bitField0_ &= ~0x00000004;
+      kv = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @return the kv
+     */
+    public double getKv() {
+      return kv;
+    }
+
+    /**
+     * <code>optional double kv = 3;</code>
+     * @param value the kv to set
+     * @return this
+     */
+    public ProtobufElevatorFeedforward setKv(final double value) {
+      bitField0_ |= 0x00000004;
+      kv = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @return whether the ka field is set
+     */
+    public boolean hasKa() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @return this
+     */
+    public ProtobufElevatorFeedforward clearKa() {
+      bitField0_ &= ~0x00000008;
+      ka = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @return the ka
+     */
+    public double getKa() {
+      return ka;
+    }
+
+    /**
+     * <code>optional double ka = 4;</code>
+     * @param value the ka to set
+     * @return this
+     */
+    public ProtobufElevatorFeedforward setKa(final double value) {
+      bitField0_ |= 0x00000008;
+      ka = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufElevatorFeedforward copyFrom(final ProtobufElevatorFeedforward other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        ks = other.ks;
+        kg = other.kg;
+        kv = other.kv;
+        ka = other.ka;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufElevatorFeedforward mergeFrom(final ProtobufElevatorFeedforward other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasKs()) {
+        setKs(other.ks);
+      }
+      if (other.hasKg()) {
+        setKg(other.kg);
+      }
+      if (other.hasKv()) {
+        setKv(other.kv);
+      }
+      if (other.hasKa()) {
+        setKa(other.ka);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufElevatorFeedforward clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      ks = 0D;
+      kg = 0D;
+      kv = 0D;
+      ka = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufElevatorFeedforward clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufElevatorFeedforward)) {
+        return false;
+      }
+      ProtobufElevatorFeedforward other = (ProtobufElevatorFeedforward) o;
+      return bitField0_ == other.bitField0_
+        && (!hasKs() || ProtoUtil.isEqual(ks, other.ks))
+        && (!hasKg() || ProtoUtil.isEqual(kg, other.kg))
+        && (!hasKv() || ProtoUtil.isEqual(kv, other.kv))
+        && (!hasKa() || ProtoUtil.isEqual(ka, other.ka));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(ks);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(kg);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(kv);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(ka);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufElevatorFeedforward mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // ks
+            ks = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // kg
+            kg = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // kv
+            kv = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // ka
+            ka = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.ks, ks);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.kg, kg);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.kv, kv);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.ka, ka);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufElevatorFeedforward mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3432: {
+            if (input.isAtField(FieldNames.ks)) {
+              if (!input.trySkipNullValue()) {
+                ks = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3420: {
+            if (input.isAtField(FieldNames.kg)) {
+              if (!input.trySkipNullValue()) {
+                kg = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3435: {
+            if (input.isAtField(FieldNames.kv)) {
+              if (!input.trySkipNullValue()) {
+                kv = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3414: {
+            if (input.isAtField(FieldNames.ka)) {
+              if (!input.trySkipNullValue()) {
+                ka = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufElevatorFeedforward clone() {
+      return new ProtobufElevatorFeedforward().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufElevatorFeedforward parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufElevatorFeedforward(), data).checkInitialized();
+    }
+
+    public static ProtobufElevatorFeedforward parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufElevatorFeedforward(), input).checkInitialized();
+    }
+
+    public static ProtobufElevatorFeedforward parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufElevatorFeedforward(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufElevatorFeedforward messages
+     */
+    public static MessageFactory<ProtobufElevatorFeedforward> getFactory() {
+      return ProtobufElevatorFeedforwardFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Controller.wpi_proto_ProtobufElevatorFeedforward_descriptor;
+    }
+
+    private enum ProtobufElevatorFeedforwardFactory implements MessageFactory<ProtobufElevatorFeedforward> {
+      INSTANCE;
+
+      @Override
+      public ProtobufElevatorFeedforward create() {
+        return ProtobufElevatorFeedforward.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName ks = FieldName.forField("ks");
+
+      static final FieldName kg = FieldName.forField("kg");
+
+      static final FieldName kv = FieldName.forField("kv");
+
+      static final FieldName ka = FieldName.forField("ka");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufSimpleMotorFeedforward}
+   */
+  public static final class ProtobufSimpleMotorFeedforward extends ProtoMessage<ProtobufSimpleMotorFeedforward> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double ks = 1;</code>
+     */
+    private double ks;
+
+    /**
+     * <code>optional double kv = 2;</code>
+     */
+    private double kv;
+
+    /**
+     * <code>optional double ka = 3;</code>
+     */
+    private double ka;
+
+    private ProtobufSimpleMotorFeedforward() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufSimpleMotorFeedforward}
+     */
+    public static ProtobufSimpleMotorFeedforward newInstance() {
+      return new ProtobufSimpleMotorFeedforward();
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return whether the ks field is set
+     */
+    public boolean hasKs() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return this
+     */
+    public ProtobufSimpleMotorFeedforward clearKs() {
+      bitField0_ &= ~0x00000001;
+      ks = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @return the ks
+     */
+    public double getKs() {
+      return ks;
+    }
+
+    /**
+     * <code>optional double ks = 1;</code>
+     * @param value the ks to set
+     * @return this
+     */
+    public ProtobufSimpleMotorFeedforward setKs(final double value) {
+      bitField0_ |= 0x00000001;
+      ks = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv = 2;</code>
+     * @return whether the kv field is set
+     */
+    public boolean hasKv() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double kv = 2;</code>
+     * @return this
+     */
+    public ProtobufSimpleMotorFeedforward clearKv() {
+      bitField0_ &= ~0x00000002;
+      kv = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double kv = 2;</code>
+     * @return the kv
+     */
+    public double getKv() {
+      return kv;
+    }
+
+    /**
+     * <code>optional double kv = 2;</code>
+     * @param value the kv to set
+     * @return this
+     */
+    public ProtobufSimpleMotorFeedforward setKv(final double value) {
+      bitField0_ |= 0x00000002;
+      kv = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka = 3;</code>
+     * @return whether the ka field is set
+     */
+    public boolean hasKa() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double ka = 3;</code>
+     * @return this
+     */
+    public ProtobufSimpleMotorFeedforward clearKa() {
+      bitField0_ &= ~0x00000004;
+      ka = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ka = 3;</code>
+     * @return the ka
+     */
+    public double getKa() {
+      return ka;
+    }
+
+    /**
+     * <code>optional double ka = 3;</code>
+     * @param value the ka to set
+     * @return this
+     */
+    public ProtobufSimpleMotorFeedforward setKa(final double value) {
+      bitField0_ |= 0x00000004;
+      ka = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufSimpleMotorFeedforward copyFrom(final ProtobufSimpleMotorFeedforward other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        ks = other.ks;
+        kv = other.kv;
+        ka = other.ka;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSimpleMotorFeedforward mergeFrom(final ProtobufSimpleMotorFeedforward other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasKs()) {
+        setKs(other.ks);
+      }
+      if (other.hasKv()) {
+        setKv(other.kv);
+      }
+      if (other.hasKa()) {
+        setKa(other.ka);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSimpleMotorFeedforward clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      ks = 0D;
+      kv = 0D;
+      ka = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufSimpleMotorFeedforward clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufSimpleMotorFeedforward)) {
+        return false;
+      }
+      ProtobufSimpleMotorFeedforward other = (ProtobufSimpleMotorFeedforward) o;
+      return bitField0_ == other.bitField0_
+        && (!hasKs() || ProtoUtil.isEqual(ks, other.ks))
+        && (!hasKv() || ProtoUtil.isEqual(kv, other.kv))
+        && (!hasKa() || ProtoUtil.isEqual(ka, other.ka));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(ks);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(kv);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(ka);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufSimpleMotorFeedforward mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // ks
+            ks = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // kv
+            kv = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // ka
+            ka = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.ks, ks);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.kv, kv);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.ka, ka);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufSimpleMotorFeedforward mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3432: {
+            if (input.isAtField(FieldNames.ks)) {
+              if (!input.trySkipNullValue()) {
+                ks = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3435: {
+            if (input.isAtField(FieldNames.kv)) {
+              if (!input.trySkipNullValue()) {
+                kv = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3414: {
+            if (input.isAtField(FieldNames.ka)) {
+              if (!input.trySkipNullValue()) {
+                ka = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufSimpleMotorFeedforward clone() {
+      return new ProtobufSimpleMotorFeedforward().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufSimpleMotorFeedforward parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufSimpleMotorFeedforward(), data).checkInitialized();
+    }
+
+    public static ProtobufSimpleMotorFeedforward parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSimpleMotorFeedforward(), input).checkInitialized();
+    }
+
+    public static ProtobufSimpleMotorFeedforward parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSimpleMotorFeedforward(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufSimpleMotorFeedforward messages
+     */
+    public static MessageFactory<ProtobufSimpleMotorFeedforward> getFactory() {
+      return ProtobufSimpleMotorFeedforwardFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Controller.wpi_proto_ProtobufSimpleMotorFeedforward_descriptor;
+    }
+
+    private enum ProtobufSimpleMotorFeedforwardFactory implements MessageFactory<ProtobufSimpleMotorFeedforward> {
+      INSTANCE;
+
+      @Override
+      public ProtobufSimpleMotorFeedforward create() {
+        return ProtobufSimpleMotorFeedforward.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName ks = FieldName.forField("ks");
+
+      static final FieldName kv = FieldName.forField("kv");
+
+      static final FieldName ka = FieldName.forField("ka");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufDifferentialDriveWheelVoltages}
+   */
+  public static final class ProtobufDifferentialDriveWheelVoltages extends ProtoMessage<ProtobufDifferentialDriveWheelVoltages> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double left = 1;</code>
+     */
+    private double left;
+
+    /**
+     * <code>optional double right = 2;</code>
+     */
+    private double right;
+
+    private ProtobufDifferentialDriveWheelVoltages() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufDifferentialDriveWheelVoltages}
+     */
+    public static ProtobufDifferentialDriveWheelVoltages newInstance() {
+      return new ProtobufDifferentialDriveWheelVoltages();
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @return whether the left field is set
+     */
+    public boolean hasLeft() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelVoltages clearLeft() {
+      bitField0_ &= ~0x00000001;
+      left = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @return the left
+     */
+    public double getLeft() {
+      return left;
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @param value the left to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelVoltages setLeft(final double value) {
+      bitField0_ |= 0x00000001;
+      left = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @return whether the right field is set
+     */
+    public boolean hasRight() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelVoltages clearRight() {
+      bitField0_ &= ~0x00000002;
+      right = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @return the right
+     */
+    public double getRight() {
+      return right;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @param value the right to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelVoltages setRight(final double value) {
+      bitField0_ |= 0x00000002;
+      right = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelVoltages copyFrom(
+        final ProtobufDifferentialDriveWheelVoltages other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        left = other.left;
+        right = other.right;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelVoltages mergeFrom(
+        final ProtobufDifferentialDriveWheelVoltages other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasLeft()) {
+        setLeft(other.left);
+      }
+      if (other.hasRight()) {
+        setRight(other.right);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelVoltages clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      left = 0D;
+      right = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelVoltages clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufDifferentialDriveWheelVoltages)) {
+        return false;
+      }
+      ProtobufDifferentialDriveWheelVoltages other = (ProtobufDifferentialDriveWheelVoltages) o;
+      return bitField0_ == other.bitField0_
+        && (!hasLeft() || ProtoUtil.isEqual(left, other.left))
+        && (!hasRight() || ProtoUtil.isEqual(right, other.right));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(left);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(right);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufDifferentialDriveWheelVoltages mergeFrom(final ProtoSource input) throws
+        IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // left
+            left = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // right
+            right = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.left, left);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.right, right);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelVoltages mergeFrom(final JsonSource input) throws
+        IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3317767: {
+            if (input.isAtField(FieldNames.left)) {
+              if (!input.trySkipNullValue()) {
+                left = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 108511772: {
+            if (input.isAtField(FieldNames.right)) {
+              if (!input.trySkipNullValue()) {
+                right = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelVoltages clone() {
+      return new ProtobufDifferentialDriveWheelVoltages().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufDifferentialDriveWheelVoltages parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelVoltages(), data).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveWheelVoltages parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelVoltages(), input).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveWheelVoltages parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelVoltages(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufDifferentialDriveWheelVoltages messages
+     */
+    public static MessageFactory<ProtobufDifferentialDriveWheelVoltages> getFactory() {
+      return ProtobufDifferentialDriveWheelVoltagesFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Controller.wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor;
+    }
+
+    private enum ProtobufDifferentialDriveWheelVoltagesFactory implements MessageFactory<ProtobufDifferentialDriveWheelVoltages> {
+      INSTANCE;
+
+      @Override
+      public ProtobufDifferentialDriveWheelVoltages create() {
+        return ProtobufDifferentialDriveWheelVoltages.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName left = FieldName.forField("left");
+
+      static final FieldName right = FieldName.forField("right");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java
new file mode 100644
index 0000000..331ccd1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java
@@ -0,0 +1,1806 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+
+public final class Geometry2D {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1256,
+    "ChBnZW9tZXRyeTJkLnByb3RvEgl3cGkucHJvdG8iMwoVUHJvdG9idWZUcmFuc2xhdGlvbjJkEgwKAXgY" + 
+    "ASABKAFSAXgSDAoBeRgCIAEoAVIBeSIqChJQcm90b2J1ZlJvdGF0aW9uMmQSFAoFdmFsdWUYASABKAFS" + 
+    "BXZhbHVlIo8BCg5Qcm90b2J1ZlBvc2UyZBJCCgt0cmFuc2xhdGlvbhgBIAEoCzIgLndwaS5wcm90by5Q" + 
+    "cm90b2J1ZlRyYW5zbGF0aW9uMmRSC3RyYW5zbGF0aW9uEjkKCHJvdGF0aW9uGAIgASgLMh0ud3BpLnBy" + 
+    "b3RvLlByb3RvYnVmUm90YXRpb24yZFIIcm90YXRpb24ilAEKE1Byb3RvYnVmVHJhbnNmb3JtMmQSQgoL" + 
+    "dHJhbnNsYXRpb24YASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgt0cmFuc2xh" + 
+    "dGlvbhI5Cghyb3RhdGlvbhgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSCHJvdGF0" + 
+    "aW9uIkkKD1Byb3RvYnVmVHdpc3QyZBIOCgJkeBgBIAEoAVICZHgSDgoCZHkYAiABKAFSAmR5EhYKBmR0" + 
+    "aGV0YRgDIAEoAVIGZHRoZXRhQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rPBQoGEgQAAB0BCggK" + 
+    "AQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoCBAASBAYACQEKCgoDBAABEgMG" + 
+    "CB0KCwoEBAACABIDBwIPCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJCgoMCgUEAAIAAxIDBw0OCgsK" + 
+    "BAQAAgESAwgCDwoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICQoKDAoFBAACAQMSAwgNDgoKCgIEARIE" + 
+    "CwANAQoKCgMEAQESAwsIGgoLCgQEAQIAEgMMAhMKDAoFBAECAAUSAwwCCAoMCgUEAQIAARIDDAkOCgwK" + 
+    "BQQBAgADEgMMERIKCgoCBAISBA8AEgEKCgoDBAIBEgMPCBYKCwoEBAICABIDEAIoCgwKBQQCAgAGEgMQ" + 
+    "AhcKDAoFBAICAAESAxAYIwoMCgUEAgIAAxIDECYnCgsKBAQCAgESAxECIgoMCgUEAgIBBhIDEQIUCgwK" + 
+    "BQQCAgEBEgMRFR0KDAoFBAICAQMSAxEgIQoKCgIEAxIEFAAXAQoKCgMEAwESAxQIGwoLCgQEAwIAEgMV" + 
+    "AigKDAoFBAMCAAYSAxUCFwoMCgUEAwIAARIDFRgjCgwKBQQDAgADEgMVJicKCwoEBAMCARIDFgIiCgwK" + 
+    "BQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQEEgQZAB0BCgoKAwQEARID" + 
+    "GQgXCgsKBAQEAgASAxoCEAoMCgUEBAIABRIDGgIICgwKBQQEAgABEgMaCQsKDAoFBAQCAAMSAxoODwoL" + 
+    "CgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQEAgEDEgMbDg8KCwoEBAQC",
+    "AhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxIDHBITYgZwcm90bzM=");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("geometry2d.proto", "wpi.proto", descriptorData);
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTranslation2d_descriptor = descriptor.internalContainedType(31, 51, "ProtobufTranslation2d", "wpi.proto.ProtobufTranslation2d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufRotation2d_descriptor = descriptor.internalContainedType(84, 42, "ProtobufRotation2d", "wpi.proto.ProtobufRotation2d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufPose2d_descriptor = descriptor.internalContainedType(129, 143, "ProtobufPose2d", "wpi.proto.ProtobufPose2d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTransform2d_descriptor = descriptor.internalContainedType(275, 148, "ProtobufTransform2d", "wpi.proto.ProtobufTransform2d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTwist2d_descriptor = descriptor.internalContainedType(425, 73, "ProtobufTwist2d", "wpi.proto.ProtobufTwist2d");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTranslation2d}
+   */
+  public static final class ProtobufTranslation2d extends ProtoMessage<ProtobufTranslation2d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double x = 1;</code>
+     */
+    private double x;
+
+    /**
+     * <code>optional double y = 2;</code>
+     */
+    private double y;
+
+    private ProtobufTranslation2d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTranslation2d}
+     */
+    public static ProtobufTranslation2d newInstance() {
+      return new ProtobufTranslation2d();
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @return whether the x field is set
+     */
+    public boolean hasX() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @return this
+     */
+    public ProtobufTranslation2d clearX() {
+      bitField0_ &= ~0x00000001;
+      x = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @return the x
+     */
+    public double getX() {
+      return x;
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @param value the x to set
+     * @return this
+     */
+    public ProtobufTranslation2d setX(final double value) {
+      bitField0_ |= 0x00000001;
+      x = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @return whether the y field is set
+     */
+    public boolean hasY() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @return this
+     */
+    public ProtobufTranslation2d clearY() {
+      bitField0_ &= ~0x00000002;
+      y = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @return the y
+     */
+    public double getY() {
+      return y;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @param value the y to set
+     * @return this
+     */
+    public ProtobufTranslation2d setY(final double value) {
+      bitField0_ |= 0x00000002;
+      y = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation2d copyFrom(final ProtobufTranslation2d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        x = other.x;
+        y = other.y;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation2d mergeFrom(final ProtobufTranslation2d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasX()) {
+        setX(other.x);
+      }
+      if (other.hasY()) {
+        setY(other.y);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation2d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      x = 0D;
+      y = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation2d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTranslation2d)) {
+        return false;
+      }
+      ProtobufTranslation2d other = (ProtobufTranslation2d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasX() || ProtoUtil.isEqual(x, other.x))
+        && (!hasY() || ProtoUtil.isEqual(y, other.y));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(x);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(y);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTranslation2d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // x
+            x = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // y
+            y = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.x, x);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.y, y);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTranslation2d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 120: {
+            if (input.isAtField(FieldNames.x)) {
+              if (!input.trySkipNullValue()) {
+                x = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 121: {
+            if (input.isAtField(FieldNames.y)) {
+              if (!input.trySkipNullValue()) {
+                y = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation2d clone() {
+      return new ProtobufTranslation2d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTranslation2d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTranslation2d(), data).checkInitialized();
+    }
+
+    public static ProtobufTranslation2d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTranslation2d(), input).checkInitialized();
+    }
+
+    public static ProtobufTranslation2d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTranslation2d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTranslation2d messages
+     */
+    public static MessageFactory<ProtobufTranslation2d> getFactory() {
+      return ProtobufTranslation2dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry2D.wpi_proto_ProtobufTranslation2d_descriptor;
+    }
+
+    private enum ProtobufTranslation2dFactory implements MessageFactory<ProtobufTranslation2d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTranslation2d create() {
+        return ProtobufTranslation2d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName x = FieldName.forField("x");
+
+      static final FieldName y = FieldName.forField("y");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufRotation2d}
+   */
+  public static final class ProtobufRotation2d extends ProtoMessage<ProtobufRotation2d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double value = 1;</code>
+     */
+    private double value_;
+
+    private ProtobufRotation2d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufRotation2d}
+     */
+    public static ProtobufRotation2d newInstance() {
+      return new ProtobufRotation2d();
+    }
+
+    /**
+     * <code>optional double value = 1;</code>
+     * @return whether the value_ field is set
+     */
+    public boolean hasValue() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double value = 1;</code>
+     * @return this
+     */
+    public ProtobufRotation2d clearValue() {
+      bitField0_ &= ~0x00000001;
+      value_ = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double value = 1;</code>
+     * @return the value_
+     */
+    public double getValue() {
+      return value_;
+    }
+
+    /**
+     * <code>optional double value = 1;</code>
+     * @param value the value_ to set
+     * @return this
+     */
+    public ProtobufRotation2d setValue(final double value) {
+      bitField0_ |= 0x00000001;
+      value_ = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation2d copyFrom(final ProtobufRotation2d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        value_ = other.value_;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation2d mergeFrom(final ProtobufRotation2d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasValue()) {
+        setValue(other.value_);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation2d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      value_ = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation2d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufRotation2d)) {
+        return false;
+      }
+      ProtobufRotation2d other = (ProtobufRotation2d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasValue() || ProtoUtil.isEqual(value_, other.value_));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(value_);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufRotation2d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // value_
+            value_ = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.value_, value_);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufRotation2d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 111972721: {
+            if (input.isAtField(FieldNames.value_)) {
+              if (!input.trySkipNullValue()) {
+                value_ = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation2d clone() {
+      return new ProtobufRotation2d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufRotation2d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufRotation2d(), data).checkInitialized();
+    }
+
+    public static ProtobufRotation2d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufRotation2d(), input).checkInitialized();
+    }
+
+    public static ProtobufRotation2d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufRotation2d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufRotation2d messages
+     */
+    public static MessageFactory<ProtobufRotation2d> getFactory() {
+      return ProtobufRotation2dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry2D.wpi_proto_ProtobufRotation2d_descriptor;
+    }
+
+    private enum ProtobufRotation2dFactory implements MessageFactory<ProtobufRotation2d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufRotation2d create() {
+        return ProtobufRotation2d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName value_ = FieldName.forField("value");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufPose2d}
+   */
+  public static final class ProtobufPose2d extends ProtoMessage<ProtobufPose2d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     */
+    private final ProtobufTranslation2d translation = ProtobufTranslation2d.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     */
+    private final ProtobufRotation2d rotation = ProtobufRotation2d.newInstance();
+
+    private ProtobufPose2d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufPose2d}
+     */
+    public static ProtobufPose2d newInstance() {
+      return new ProtobufPose2d();
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     * @return whether the translation field is set
+     */
+    public boolean hasTranslation() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     * @return this
+     */
+    public ProtobufPose2d clearTranslation() {
+      bitField0_ &= ~0x00000001;
+      translation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableTranslation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufTranslation2d getTranslation() {
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufTranslation2d getMutableTranslation() {
+      bitField0_ |= 0x00000001;
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     * @param value the translation to set
+     * @return this
+     */
+    public ProtobufPose2d setTranslation(final ProtobufTranslation2d value) {
+      bitField0_ |= 0x00000001;
+      translation.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     * @return whether the rotation field is set
+     */
+    public boolean hasRotation() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     * @return this
+     */
+    public ProtobufPose2d clearRotation() {
+      bitField0_ &= ~0x00000002;
+      rotation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableRotation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufRotation2d getRotation() {
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufRotation2d getMutableRotation() {
+      bitField0_ |= 0x00000002;
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     * @param value the rotation to set
+     * @return this
+     */
+    public ProtobufPose2d setRotation(final ProtobufRotation2d value) {
+      bitField0_ |= 0x00000002;
+      rotation.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufPose2d copyFrom(final ProtobufPose2d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        translation.copyFrom(other.translation);
+        rotation.copyFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufPose2d mergeFrom(final ProtobufPose2d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasTranslation()) {
+        getMutableTranslation().mergeFrom(other.translation);
+      }
+      if (other.hasRotation()) {
+        getMutableRotation().mergeFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufPose2d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clear();
+      rotation.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufPose2d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clearQuick();
+      rotation.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufPose2d)) {
+        return false;
+      }
+      ProtobufPose2d other = (ProtobufPose2d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasTranslation() || translation.equals(other.translation))
+        && (!hasRotation() || rotation.equals(other.rotation));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 18);
+        output.writeMessageNoTag(rotation);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(rotation);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufPose2d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // translation
+            input.readMessage(translation);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // rotation
+            input.readMessage(rotation);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeMessage(FieldNames.translation, translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeMessage(FieldNames.rotation, rotation);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufPose2d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -1840647503: {
+            if (input.isAtField(FieldNames.translation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(translation);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -40300674: {
+            if (input.isAtField(FieldNames.rotation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(rotation);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufPose2d clone() {
+      return new ProtobufPose2d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufPose2d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufPose2d(), data).checkInitialized();
+    }
+
+    public static ProtobufPose2d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufPose2d(), input).checkInitialized();
+    }
+
+    public static ProtobufPose2d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufPose2d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufPose2d messages
+     */
+    public static MessageFactory<ProtobufPose2d> getFactory() {
+      return ProtobufPose2dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry2D.wpi_proto_ProtobufPose2d_descriptor;
+    }
+
+    private enum ProtobufPose2dFactory implements MessageFactory<ProtobufPose2d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufPose2d create() {
+        return ProtobufPose2d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName translation = FieldName.forField("translation");
+
+      static final FieldName rotation = FieldName.forField("rotation");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTransform2d}
+   */
+  public static final class ProtobufTransform2d extends ProtoMessage<ProtobufTransform2d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     */
+    private final ProtobufTranslation2d translation = ProtobufTranslation2d.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     */
+    private final ProtobufRotation2d rotation = ProtobufRotation2d.newInstance();
+
+    private ProtobufTransform2d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTransform2d}
+     */
+    public static ProtobufTransform2d newInstance() {
+      return new ProtobufTransform2d();
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     * @return whether the translation field is set
+     */
+    public boolean hasTranslation() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     * @return this
+     */
+    public ProtobufTransform2d clearTranslation() {
+      bitField0_ &= ~0x00000001;
+      translation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableTranslation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufTranslation2d getTranslation() {
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufTranslation2d getMutableTranslation() {
+      bitField0_ |= 0x00000001;
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d translation = 1;</code>
+     * @param value the translation to set
+     * @return this
+     */
+    public ProtobufTransform2d setTranslation(final ProtobufTranslation2d value) {
+      bitField0_ |= 0x00000001;
+      translation.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     * @return whether the rotation field is set
+     */
+    public boolean hasRotation() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     * @return this
+     */
+    public ProtobufTransform2d clearRotation() {
+      bitField0_ &= ~0x00000002;
+      rotation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableRotation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufRotation2d getRotation() {
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufRotation2d getMutableRotation() {
+      bitField0_ |= 0x00000002;
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d rotation = 2;</code>
+     * @param value the rotation to set
+     * @return this
+     */
+    public ProtobufTransform2d setRotation(final ProtobufRotation2d value) {
+      bitField0_ |= 0x00000002;
+      rotation.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform2d copyFrom(final ProtobufTransform2d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        translation.copyFrom(other.translation);
+        rotation.copyFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform2d mergeFrom(final ProtobufTransform2d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasTranslation()) {
+        getMutableTranslation().mergeFrom(other.translation);
+      }
+      if (other.hasRotation()) {
+        getMutableRotation().mergeFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform2d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clear();
+      rotation.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform2d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clearQuick();
+      rotation.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTransform2d)) {
+        return false;
+      }
+      ProtobufTransform2d other = (ProtobufTransform2d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasTranslation() || translation.equals(other.translation))
+        && (!hasRotation() || rotation.equals(other.rotation));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 18);
+        output.writeMessageNoTag(rotation);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(rotation);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTransform2d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // translation
+            input.readMessage(translation);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // rotation
+            input.readMessage(rotation);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeMessage(FieldNames.translation, translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeMessage(FieldNames.rotation, rotation);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTransform2d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -1840647503: {
+            if (input.isAtField(FieldNames.translation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(translation);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -40300674: {
+            if (input.isAtField(FieldNames.rotation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(rotation);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform2d clone() {
+      return new ProtobufTransform2d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTransform2d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTransform2d(), data).checkInitialized();
+    }
+
+    public static ProtobufTransform2d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTransform2d(), input).checkInitialized();
+    }
+
+    public static ProtobufTransform2d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTransform2d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTransform2d messages
+     */
+    public static MessageFactory<ProtobufTransform2d> getFactory() {
+      return ProtobufTransform2dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry2D.wpi_proto_ProtobufTransform2d_descriptor;
+    }
+
+    private enum ProtobufTransform2dFactory implements MessageFactory<ProtobufTransform2d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTransform2d create() {
+        return ProtobufTransform2d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName translation = FieldName.forField("translation");
+
+      static final FieldName rotation = FieldName.forField("rotation");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTwist2d}
+   */
+  public static final class ProtobufTwist2d extends ProtoMessage<ProtobufTwist2d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double dx = 1;</code>
+     */
+    private double dx;
+
+    /**
+     * <code>optional double dy = 2;</code>
+     */
+    private double dy;
+
+    /**
+     * <code>optional double dtheta = 3;</code>
+     */
+    private double dtheta;
+
+    private ProtobufTwist2d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTwist2d}
+     */
+    public static ProtobufTwist2d newInstance() {
+      return new ProtobufTwist2d();
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @return whether the dx field is set
+     */
+    public boolean hasDx() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @return this
+     */
+    public ProtobufTwist2d clearDx() {
+      bitField0_ &= ~0x00000001;
+      dx = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @return the dx
+     */
+    public double getDx() {
+      return dx;
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @param value the dx to set
+     * @return this
+     */
+    public ProtobufTwist2d setDx(final double value) {
+      bitField0_ |= 0x00000001;
+      dx = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @return whether the dy field is set
+     */
+    public boolean hasDy() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @return this
+     */
+    public ProtobufTwist2d clearDy() {
+      bitField0_ &= ~0x00000002;
+      dy = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @return the dy
+     */
+    public double getDy() {
+      return dy;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @param value the dy to set
+     * @return this
+     */
+    public ProtobufTwist2d setDy(final double value) {
+      bitField0_ |= 0x00000002;
+      dy = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double dtheta = 3;</code>
+     * @return whether the dtheta field is set
+     */
+    public boolean hasDtheta() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double dtheta = 3;</code>
+     * @return this
+     */
+    public ProtobufTwist2d clearDtheta() {
+      bitField0_ &= ~0x00000004;
+      dtheta = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dtheta = 3;</code>
+     * @return the dtheta
+     */
+    public double getDtheta() {
+      return dtheta;
+    }
+
+    /**
+     * <code>optional double dtheta = 3;</code>
+     * @param value the dtheta to set
+     * @return this
+     */
+    public ProtobufTwist2d setDtheta(final double value) {
+      bitField0_ |= 0x00000004;
+      dtheta = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist2d copyFrom(final ProtobufTwist2d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        dx = other.dx;
+        dy = other.dy;
+        dtheta = other.dtheta;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist2d mergeFrom(final ProtobufTwist2d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasDx()) {
+        setDx(other.dx);
+      }
+      if (other.hasDy()) {
+        setDy(other.dy);
+      }
+      if (other.hasDtheta()) {
+        setDtheta(other.dtheta);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist2d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      dx = 0D;
+      dy = 0D;
+      dtheta = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist2d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTwist2d)) {
+        return false;
+      }
+      ProtobufTwist2d other = (ProtobufTwist2d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasDx() || ProtoUtil.isEqual(dx, other.dx))
+        && (!hasDy() || ProtoUtil.isEqual(dy, other.dy))
+        && (!hasDtheta() || ProtoUtil.isEqual(dtheta, other.dtheta));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(dx);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(dy);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(dtheta);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTwist2d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // dx
+            dx = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // dy
+            dy = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // dtheta
+            dtheta = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.dx, dx);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.dy, dy);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.dtheta, dtheta);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTwist2d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3220: {
+            if (input.isAtField(FieldNames.dx)) {
+              if (!input.trySkipNullValue()) {
+                dx = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3221: {
+            if (input.isAtField(FieldNames.dy)) {
+              if (!input.trySkipNullValue()) {
+                dy = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -1321724742: {
+            if (input.isAtField(FieldNames.dtheta)) {
+              if (!input.trySkipNullValue()) {
+                dtheta = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist2d clone() {
+      return new ProtobufTwist2d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTwist2d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTwist2d(), data).checkInitialized();
+    }
+
+    public static ProtobufTwist2d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTwist2d(), input).checkInitialized();
+    }
+
+    public static ProtobufTwist2d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTwist2d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTwist2d messages
+     */
+    public static MessageFactory<ProtobufTwist2d> getFactory() {
+      return ProtobufTwist2dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry2D.wpi_proto_ProtobufTwist2d_descriptor;
+    }
+
+    private enum ProtobufTwist2dFactory implements MessageFactory<ProtobufTwist2d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTwist2d create() {
+        return ProtobufTwist2d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName dx = FieldName.forField("dx");
+
+      static final FieldName dy = FieldName.forField("dy");
+
+      static final FieldName dtheta = FieldName.forField("dtheta");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry3D.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry3D.java
new file mode 100644
index 0000000..404d7b0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry3D.java
@@ -0,0 +1,2652 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+
+public final class Geometry3D {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1875,
+    "ChBnZW9tZXRyeTNkLnByb3RvEgl3cGkucHJvdG8iQQoVUHJvdG9idWZUcmFuc2xhdGlvbjNkEgwKAXgY" + 
+    "ASABKAFSAXgSDAoBeRgCIAEoAVIBeRIMCgF6GAMgASgBUgF6IkwKElByb3RvYnVmUXVhdGVybmlvbhIM" + 
+    "CgF3GAEgASgBUgF3EgwKAXgYAiABKAFSAXgSDAoBeRgDIAEoAVIBeRIMCgF6GAQgASgBUgF6IkEKElBy" + 
+    "b3RvYnVmUm90YXRpb24zZBIrCgFxGAEgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUXVhdGVybmlvblIB" + 
+    "cSKPAQoOUHJvdG9idWZQb3NlM2QSQgoLdHJhbnNsYXRpb24YASABKAsyIC53cGkucHJvdG8uUHJvdG9i" + 
+    "dWZUcmFuc2xhdGlvbjNkUgt0cmFuc2xhdGlvbhI5Cghyb3RhdGlvbhgCIAEoCzIdLndwaS5wcm90by5Q" + 
+    "cm90b2J1ZlJvdGF0aW9uM2RSCHJvdGF0aW9uIpQBChNQcm90b2J1ZlRyYW5zZm9ybTNkEkIKC3RyYW5z" + 
+    "bGF0aW9uGAEgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24zZFILdHJhbnNsYXRpb24S" + 
+    "OQoIcm90YXRpb24YAiABKAsyHS53cGkucHJvdG8uUHJvdG9idWZSb3RhdGlvbjNkUghyb3RhdGlvbiJx" + 
+    "Cg9Qcm90b2J1ZlR3aXN0M2QSDgoCZHgYASABKAFSAmR4Eg4KAmR5GAIgASgBUgJkeRIOCgJkehgDIAEo" + 
+    "AVICZHoSDgoCcngYBCABKAFSAnJ4Eg4KAnJ5GAUgASgBUgJyeRIOCgJyehgGIAEoAVICcnpCGgoYZWR1" + 
+    "LndwaS5maXJzdC5tYXRoLnByb3RvSp8JCgYSBAAAKAEKCAoBDBIDAAASCggKAQISAwIAEgoICgEIEgME" + 
+    "ADEKCQoCCAESAwQAMQoKCgIEABIEBgAKAQoKCgMEAAESAwYIHQoLCgQEAAIAEgMHAg8KDAoFBAACAAUS" + 
+    "AwcCCAoMCgUEAAIAARIDBwkKCgwKBQQAAgADEgMHDQ4KCwoEBAACARIDCAIPCgwKBQQAAgEFEgMIAggK" + 
+    "DAoFBAACAQESAwgJCgoMCgUEAAIBAxIDCA0OCgsKBAQAAgISAwkCDwoMCgUEAAICBRIDCQIICgwKBQQA" + 
+    "AgIBEgMJCQoKDAoFBAACAgMSAwkNDgoKCgIEARIEDAARAQoKCgMEAQESAwwIGgoLCgQEAQIAEgMNAg8K" + 
+    "DAoFBAECAAUSAw0CCAoMCgUEAQIAARIDDQkKCgwKBQQBAgADEgMNDQ4KCwoEBAECARIDDgIPCgwKBQQB" + 
+    "AgEFEgMOAggKDAoFBAECAQESAw4JCgoMCgUEAQIBAxIDDg0OCgsKBAQBAgISAw8CDwoMCgUEAQICBRID" + 
+    "DwIICgwKBQQBAgIBEgMPCQoKDAoFBAECAgMSAw8NDgoLCgQEAQIDEgMQAg8KDAoFBAECAwUSAxACCAoM" + 
+    "CgUEAQIDARIDEAkKCgwKBQQBAgMDEgMQDQ4KCgoCBAISBBMAFQEKCgoDBAIBEgMTCBoKCwoEBAICABID",
+    "FAIbCgwKBQQCAgAGEgMUAhQKDAoFBAICAAESAxQVFgoMCgUEAgIAAxIDFBkaCgoKAgQDEgQXABoBCgoK" + 
+    "AwQDARIDFwgWCgsKBAQDAgASAxgCKAoMCgUEAwIABhIDGAIXCgwKBQQDAgABEgMYGCMKDAoFBAMCAAMS" + 
+    "AxgmJwoLCgQEAwIBEgMZAiIKDAoFBAMCAQYSAxkCFAoMCgUEAwIBARIDGRUdCgwKBQQDAgEDEgMZICEK" + 
+    "CgoCBAQSBBwAHwEKCgoDBAQBEgMcCBsKCwoEBAQCABIDHQIoCgwKBQQEAgAGEgMdAhcKDAoFBAQCAAES" + 
+    "Ax0YIwoMCgUEBAIAAxIDHSYnCgsKBAQEAgESAx4CIgoMCgUEBAIBBhIDHgIUCgwKBQQEAgEBEgMeFR0K" + 
+    "DAoFBAQCAQMSAx4gIQoKCgIEBRIEIQAoAQoKCgMEBQESAyEIFwoLCgQEBQIAEgMiAhAKDAoFBAUCAAUS" + 
+    "AyICCAoMCgUEBQIAARIDIgkLCgwKBQQFAgADEgMiDg8KCwoEBAUCARIDIwIQCgwKBQQFAgEFEgMjAggK" + 
+    "DAoFBAUCAQESAyMJCwoMCgUEBQIBAxIDIw4PCgsKBAQFAgISAyQCEAoMCgUEBQICBRIDJAIICgwKBQQF" + 
+    "AgIBEgMkCQsKDAoFBAUCAgMSAyQODwoLCgQEBQIDEgMlAhAKDAoFBAUCAwUSAyUCCAoMCgUEBQIDARID" + 
+    "JQkLCgwKBQQFAgMDEgMlDg8KCwoEBAUCBBIDJgIQCgwKBQQFAgQFEgMmAggKDAoFBAUCBAESAyYJCwoM" + 
+    "CgUEBQIEAxIDJg4PCgsKBAQFAgUSAycCEAoMCgUEBQIFBRIDJwIICgwKBQQFAgUBEgMnCQsKDAoFBAUC" + 
+    "BQMSAycOD2IGcHJvdG8z");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("geometry3d.proto", "wpi.proto", descriptorData);
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTranslation3d_descriptor = descriptor.internalContainedType(31, 65, "ProtobufTranslation3d", "wpi.proto.ProtobufTranslation3d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufQuaternion_descriptor = descriptor.internalContainedType(98, 76, "ProtobufQuaternion", "wpi.proto.ProtobufQuaternion");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufRotation3d_descriptor = descriptor.internalContainedType(176, 65, "ProtobufRotation3d", "wpi.proto.ProtobufRotation3d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufPose3d_descriptor = descriptor.internalContainedType(244, 143, "ProtobufPose3d", "wpi.proto.ProtobufPose3d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTransform3d_descriptor = descriptor.internalContainedType(390, 148, "ProtobufTransform3d", "wpi.proto.ProtobufTransform3d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTwist3d_descriptor = descriptor.internalContainedType(540, 113, "ProtobufTwist3d", "wpi.proto.ProtobufTwist3d");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTranslation3d}
+   */
+  public static final class ProtobufTranslation3d extends ProtoMessage<ProtobufTranslation3d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double x = 1;</code>
+     */
+    private double x;
+
+    /**
+     * <code>optional double y = 2;</code>
+     */
+    private double y;
+
+    /**
+     * <code>optional double z = 3;</code>
+     */
+    private double z;
+
+    private ProtobufTranslation3d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTranslation3d}
+     */
+    public static ProtobufTranslation3d newInstance() {
+      return new ProtobufTranslation3d();
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @return whether the x field is set
+     */
+    public boolean hasX() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @return this
+     */
+    public ProtobufTranslation3d clearX() {
+      bitField0_ &= ~0x00000001;
+      x = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @return the x
+     */
+    public double getX() {
+      return x;
+    }
+
+    /**
+     * <code>optional double x = 1;</code>
+     * @param value the x to set
+     * @return this
+     */
+    public ProtobufTranslation3d setX(final double value) {
+      bitField0_ |= 0x00000001;
+      x = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @return whether the y field is set
+     */
+    public boolean hasY() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @return this
+     */
+    public ProtobufTranslation3d clearY() {
+      bitField0_ &= ~0x00000002;
+      y = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @return the y
+     */
+    public double getY() {
+      return y;
+    }
+
+    /**
+     * <code>optional double y = 2;</code>
+     * @param value the y to set
+     * @return this
+     */
+    public ProtobufTranslation3d setY(final double value) {
+      bitField0_ |= 0x00000002;
+      y = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double z = 3;</code>
+     * @return whether the z field is set
+     */
+    public boolean hasZ() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double z = 3;</code>
+     * @return this
+     */
+    public ProtobufTranslation3d clearZ() {
+      bitField0_ &= ~0x00000004;
+      z = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double z = 3;</code>
+     * @return the z
+     */
+    public double getZ() {
+      return z;
+    }
+
+    /**
+     * <code>optional double z = 3;</code>
+     * @param value the z to set
+     * @return this
+     */
+    public ProtobufTranslation3d setZ(final double value) {
+      bitField0_ |= 0x00000004;
+      z = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation3d copyFrom(final ProtobufTranslation3d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        x = other.x;
+        y = other.y;
+        z = other.z;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation3d mergeFrom(final ProtobufTranslation3d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasX()) {
+        setX(other.x);
+      }
+      if (other.hasY()) {
+        setY(other.y);
+      }
+      if (other.hasZ()) {
+        setZ(other.z);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation3d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      x = 0D;
+      y = 0D;
+      z = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation3d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTranslation3d)) {
+        return false;
+      }
+      ProtobufTranslation3d other = (ProtobufTranslation3d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasX() || ProtoUtil.isEqual(x, other.x))
+        && (!hasY() || ProtoUtil.isEqual(y, other.y))
+        && (!hasZ() || ProtoUtil.isEqual(z, other.z));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(x);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(y);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(z);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTranslation3d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // x
+            x = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // y
+            y = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // z
+            z = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.x, x);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.y, y);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.z, z);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTranslation3d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 120: {
+            if (input.isAtField(FieldNames.x)) {
+              if (!input.trySkipNullValue()) {
+                x = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 121: {
+            if (input.isAtField(FieldNames.y)) {
+              if (!input.trySkipNullValue()) {
+                y = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 122: {
+            if (input.isAtField(FieldNames.z)) {
+              if (!input.trySkipNullValue()) {
+                z = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTranslation3d clone() {
+      return new ProtobufTranslation3d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTranslation3d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTranslation3d(), data).checkInitialized();
+    }
+
+    public static ProtobufTranslation3d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTranslation3d(), input).checkInitialized();
+    }
+
+    public static ProtobufTranslation3d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTranslation3d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTranslation3d messages
+     */
+    public static MessageFactory<ProtobufTranslation3d> getFactory() {
+      return ProtobufTranslation3dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry3D.wpi_proto_ProtobufTranslation3d_descriptor;
+    }
+
+    private enum ProtobufTranslation3dFactory implements MessageFactory<ProtobufTranslation3d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTranslation3d create() {
+        return ProtobufTranslation3d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName x = FieldName.forField("x");
+
+      static final FieldName y = FieldName.forField("y");
+
+      static final FieldName z = FieldName.forField("z");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufQuaternion}
+   */
+  public static final class ProtobufQuaternion extends ProtoMessage<ProtobufQuaternion> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double w = 1;</code>
+     */
+    private double w;
+
+    /**
+     * <code>optional double x = 2;</code>
+     */
+    private double x;
+
+    /**
+     * <code>optional double y = 3;</code>
+     */
+    private double y;
+
+    /**
+     * <code>optional double z = 4;</code>
+     */
+    private double z;
+
+    private ProtobufQuaternion() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufQuaternion}
+     */
+    public static ProtobufQuaternion newInstance() {
+      return new ProtobufQuaternion();
+    }
+
+    /**
+     * <code>optional double w = 1;</code>
+     * @return whether the w field is set
+     */
+    public boolean hasW() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double w = 1;</code>
+     * @return this
+     */
+    public ProtobufQuaternion clearW() {
+      bitField0_ &= ~0x00000001;
+      w = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double w = 1;</code>
+     * @return the w
+     */
+    public double getW() {
+      return w;
+    }
+
+    /**
+     * <code>optional double w = 1;</code>
+     * @param value the w to set
+     * @return this
+     */
+    public ProtobufQuaternion setW(final double value) {
+      bitField0_ |= 0x00000001;
+      w = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double x = 2;</code>
+     * @return whether the x field is set
+     */
+    public boolean hasX() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double x = 2;</code>
+     * @return this
+     */
+    public ProtobufQuaternion clearX() {
+      bitField0_ &= ~0x00000002;
+      x = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double x = 2;</code>
+     * @return the x
+     */
+    public double getX() {
+      return x;
+    }
+
+    /**
+     * <code>optional double x = 2;</code>
+     * @param value the x to set
+     * @return this
+     */
+    public ProtobufQuaternion setX(final double value) {
+      bitField0_ |= 0x00000002;
+      x = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double y = 3;</code>
+     * @return whether the y field is set
+     */
+    public boolean hasY() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double y = 3;</code>
+     * @return this
+     */
+    public ProtobufQuaternion clearY() {
+      bitField0_ &= ~0x00000004;
+      y = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double y = 3;</code>
+     * @return the y
+     */
+    public double getY() {
+      return y;
+    }
+
+    /**
+     * <code>optional double y = 3;</code>
+     * @param value the y to set
+     * @return this
+     */
+    public ProtobufQuaternion setY(final double value) {
+      bitField0_ |= 0x00000004;
+      y = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double z = 4;</code>
+     * @return whether the z field is set
+     */
+    public boolean hasZ() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double z = 4;</code>
+     * @return this
+     */
+    public ProtobufQuaternion clearZ() {
+      bitField0_ &= ~0x00000008;
+      z = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double z = 4;</code>
+     * @return the z
+     */
+    public double getZ() {
+      return z;
+    }
+
+    /**
+     * <code>optional double z = 4;</code>
+     * @param value the z to set
+     * @return this
+     */
+    public ProtobufQuaternion setZ(final double value) {
+      bitField0_ |= 0x00000008;
+      z = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufQuaternion copyFrom(final ProtobufQuaternion other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        w = other.w;
+        x = other.x;
+        y = other.y;
+        z = other.z;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufQuaternion mergeFrom(final ProtobufQuaternion other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasW()) {
+        setW(other.w);
+      }
+      if (other.hasX()) {
+        setX(other.x);
+      }
+      if (other.hasY()) {
+        setY(other.y);
+      }
+      if (other.hasZ()) {
+        setZ(other.z);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufQuaternion clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      w = 0D;
+      x = 0D;
+      y = 0D;
+      z = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufQuaternion clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufQuaternion)) {
+        return false;
+      }
+      ProtobufQuaternion other = (ProtobufQuaternion) o;
+      return bitField0_ == other.bitField0_
+        && (!hasW() || ProtoUtil.isEqual(w, other.w))
+        && (!hasX() || ProtoUtil.isEqual(x, other.x))
+        && (!hasY() || ProtoUtil.isEqual(y, other.y))
+        && (!hasZ() || ProtoUtil.isEqual(z, other.z));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(w);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(x);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(y);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(z);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufQuaternion mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // w
+            w = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // x
+            x = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // y
+            y = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // z
+            z = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.w, w);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.x, x);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.y, y);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.z, z);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufQuaternion mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 119: {
+            if (input.isAtField(FieldNames.w)) {
+              if (!input.trySkipNullValue()) {
+                w = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 120: {
+            if (input.isAtField(FieldNames.x)) {
+              if (!input.trySkipNullValue()) {
+                x = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 121: {
+            if (input.isAtField(FieldNames.y)) {
+              if (!input.trySkipNullValue()) {
+                y = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 122: {
+            if (input.isAtField(FieldNames.z)) {
+              if (!input.trySkipNullValue()) {
+                z = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufQuaternion clone() {
+      return new ProtobufQuaternion().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufQuaternion parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufQuaternion(), data).checkInitialized();
+    }
+
+    public static ProtobufQuaternion parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufQuaternion(), input).checkInitialized();
+    }
+
+    public static ProtobufQuaternion parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufQuaternion(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufQuaternion messages
+     */
+    public static MessageFactory<ProtobufQuaternion> getFactory() {
+      return ProtobufQuaternionFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry3D.wpi_proto_ProtobufQuaternion_descriptor;
+    }
+
+    private enum ProtobufQuaternionFactory implements MessageFactory<ProtobufQuaternion> {
+      INSTANCE;
+
+      @Override
+      public ProtobufQuaternion create() {
+        return ProtobufQuaternion.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName w = FieldName.forField("w");
+
+      static final FieldName x = FieldName.forField("x");
+
+      static final FieldName y = FieldName.forField("y");
+
+      static final FieldName z = FieldName.forField("z");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufRotation3d}
+   */
+  public static final class ProtobufRotation3d extends ProtoMessage<ProtobufRotation3d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufQuaternion q = 1;</code>
+     */
+    private final ProtobufQuaternion q = ProtobufQuaternion.newInstance();
+
+    private ProtobufRotation3d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufRotation3d}
+     */
+    public static ProtobufRotation3d newInstance() {
+      return new ProtobufRotation3d();
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufQuaternion q = 1;</code>
+     * @return whether the q field is set
+     */
+    public boolean hasQ() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufQuaternion q = 1;</code>
+     * @return this
+     */
+    public ProtobufRotation3d clearQ() {
+      bitField0_ &= ~0x00000001;
+      q.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufQuaternion q = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableQ()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufQuaternion getQ() {
+      return q;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufQuaternion q = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufQuaternion getMutableQ() {
+      bitField0_ |= 0x00000001;
+      return q;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufQuaternion q = 1;</code>
+     * @param value the q to set
+     * @return this
+     */
+    public ProtobufRotation3d setQ(final ProtobufQuaternion value) {
+      bitField0_ |= 0x00000001;
+      q.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation3d copyFrom(final ProtobufRotation3d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        q.copyFrom(other.q);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation3d mergeFrom(final ProtobufRotation3d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasQ()) {
+        getMutableQ().mergeFrom(other.q);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation3d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      q.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation3d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      q.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufRotation3d)) {
+        return false;
+      }
+      ProtobufRotation3d other = (ProtobufRotation3d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasQ() || q.equals(other.q));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(q);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(q);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufRotation3d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // q
+            input.readMessage(q);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeMessage(FieldNames.q, q);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufRotation3d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 113: {
+            if (input.isAtField(FieldNames.q)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(q);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufRotation3d clone() {
+      return new ProtobufRotation3d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufRotation3d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufRotation3d(), data).checkInitialized();
+    }
+
+    public static ProtobufRotation3d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufRotation3d(), input).checkInitialized();
+    }
+
+    public static ProtobufRotation3d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufRotation3d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufRotation3d messages
+     */
+    public static MessageFactory<ProtobufRotation3d> getFactory() {
+      return ProtobufRotation3dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry3D.wpi_proto_ProtobufRotation3d_descriptor;
+    }
+
+    private enum ProtobufRotation3dFactory implements MessageFactory<ProtobufRotation3d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufRotation3d create() {
+        return ProtobufRotation3d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName q = FieldName.forField("q");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufPose3d}
+   */
+  public static final class ProtobufPose3d extends ProtoMessage<ProtobufPose3d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     */
+    private final ProtobufTranslation3d translation = ProtobufTranslation3d.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     */
+    private final ProtobufRotation3d rotation = ProtobufRotation3d.newInstance();
+
+    private ProtobufPose3d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufPose3d}
+     */
+    public static ProtobufPose3d newInstance() {
+      return new ProtobufPose3d();
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     * @return whether the translation field is set
+     */
+    public boolean hasTranslation() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     * @return this
+     */
+    public ProtobufPose3d clearTranslation() {
+      bitField0_ &= ~0x00000001;
+      translation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableTranslation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufTranslation3d getTranslation() {
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufTranslation3d getMutableTranslation() {
+      bitField0_ |= 0x00000001;
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     * @param value the translation to set
+     * @return this
+     */
+    public ProtobufPose3d setTranslation(final ProtobufTranslation3d value) {
+      bitField0_ |= 0x00000001;
+      translation.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     * @return whether the rotation field is set
+     */
+    public boolean hasRotation() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     * @return this
+     */
+    public ProtobufPose3d clearRotation() {
+      bitField0_ &= ~0x00000002;
+      rotation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableRotation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufRotation3d getRotation() {
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufRotation3d getMutableRotation() {
+      bitField0_ |= 0x00000002;
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     * @param value the rotation to set
+     * @return this
+     */
+    public ProtobufPose3d setRotation(final ProtobufRotation3d value) {
+      bitField0_ |= 0x00000002;
+      rotation.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufPose3d copyFrom(final ProtobufPose3d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        translation.copyFrom(other.translation);
+        rotation.copyFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufPose3d mergeFrom(final ProtobufPose3d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasTranslation()) {
+        getMutableTranslation().mergeFrom(other.translation);
+      }
+      if (other.hasRotation()) {
+        getMutableRotation().mergeFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufPose3d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clear();
+      rotation.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufPose3d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clearQuick();
+      rotation.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufPose3d)) {
+        return false;
+      }
+      ProtobufPose3d other = (ProtobufPose3d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasTranslation() || translation.equals(other.translation))
+        && (!hasRotation() || rotation.equals(other.rotation));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 18);
+        output.writeMessageNoTag(rotation);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(rotation);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufPose3d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // translation
+            input.readMessage(translation);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // rotation
+            input.readMessage(rotation);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeMessage(FieldNames.translation, translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeMessage(FieldNames.rotation, rotation);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufPose3d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -1840647503: {
+            if (input.isAtField(FieldNames.translation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(translation);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -40300674: {
+            if (input.isAtField(FieldNames.rotation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(rotation);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufPose3d clone() {
+      return new ProtobufPose3d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufPose3d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufPose3d(), data).checkInitialized();
+    }
+
+    public static ProtobufPose3d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufPose3d(), input).checkInitialized();
+    }
+
+    public static ProtobufPose3d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufPose3d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufPose3d messages
+     */
+    public static MessageFactory<ProtobufPose3d> getFactory() {
+      return ProtobufPose3dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry3D.wpi_proto_ProtobufPose3d_descriptor;
+    }
+
+    private enum ProtobufPose3dFactory implements MessageFactory<ProtobufPose3d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufPose3d create() {
+        return ProtobufPose3d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName translation = FieldName.forField("translation");
+
+      static final FieldName rotation = FieldName.forField("rotation");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTransform3d}
+   */
+  public static final class ProtobufTransform3d extends ProtoMessage<ProtobufTransform3d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     */
+    private final ProtobufTranslation3d translation = ProtobufTranslation3d.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     */
+    private final ProtobufRotation3d rotation = ProtobufRotation3d.newInstance();
+
+    private ProtobufTransform3d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTransform3d}
+     */
+    public static ProtobufTransform3d newInstance() {
+      return new ProtobufTransform3d();
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     * @return whether the translation field is set
+     */
+    public boolean hasTranslation() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     * @return this
+     */
+    public ProtobufTransform3d clearTranslation() {
+      bitField0_ &= ~0x00000001;
+      translation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableTranslation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufTranslation3d getTranslation() {
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufTranslation3d getMutableTranslation() {
+      bitField0_ |= 0x00000001;
+      return translation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation3d translation = 1;</code>
+     * @param value the translation to set
+     * @return this
+     */
+    public ProtobufTransform3d setTranslation(final ProtobufTranslation3d value) {
+      bitField0_ |= 0x00000001;
+      translation.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     * @return whether the rotation field is set
+     */
+    public boolean hasRotation() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     * @return this
+     */
+    public ProtobufTransform3d clearRotation() {
+      bitField0_ &= ~0x00000002;
+      rotation.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableRotation()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufRotation3d getRotation() {
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufRotation3d getMutableRotation() {
+      bitField0_ |= 0x00000002;
+      return rotation;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation3d rotation = 2;</code>
+     * @param value the rotation to set
+     * @return this
+     */
+    public ProtobufTransform3d setRotation(final ProtobufRotation3d value) {
+      bitField0_ |= 0x00000002;
+      rotation.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform3d copyFrom(final ProtobufTransform3d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        translation.copyFrom(other.translation);
+        rotation.copyFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform3d mergeFrom(final ProtobufTransform3d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasTranslation()) {
+        getMutableTranslation().mergeFrom(other.translation);
+      }
+      if (other.hasRotation()) {
+        getMutableRotation().mergeFrom(other.rotation);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform3d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clear();
+      rotation.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform3d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      translation.clearQuick();
+      rotation.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTransform3d)) {
+        return false;
+      }
+      ProtobufTransform3d other = (ProtobufTransform3d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasTranslation() || translation.equals(other.translation))
+        && (!hasRotation() || rotation.equals(other.rotation));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 18);
+        output.writeMessageNoTag(rotation);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(rotation);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTransform3d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // translation
+            input.readMessage(translation);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // rotation
+            input.readMessage(rotation);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeMessage(FieldNames.translation, translation);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeMessage(FieldNames.rotation, rotation);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTransform3d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -1840647503: {
+            if (input.isAtField(FieldNames.translation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(translation);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -40300674: {
+            if (input.isAtField(FieldNames.rotation)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(rotation);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTransform3d clone() {
+      return new ProtobufTransform3d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTransform3d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTransform3d(), data).checkInitialized();
+    }
+
+    public static ProtobufTransform3d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTransform3d(), input).checkInitialized();
+    }
+
+    public static ProtobufTransform3d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTransform3d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTransform3d messages
+     */
+    public static MessageFactory<ProtobufTransform3d> getFactory() {
+      return ProtobufTransform3dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry3D.wpi_proto_ProtobufTransform3d_descriptor;
+    }
+
+    private enum ProtobufTransform3dFactory implements MessageFactory<ProtobufTransform3d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTransform3d create() {
+        return ProtobufTransform3d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName translation = FieldName.forField("translation");
+
+      static final FieldName rotation = FieldName.forField("rotation");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTwist3d}
+   */
+  public static final class ProtobufTwist3d extends ProtoMessage<ProtobufTwist3d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double dx = 1;</code>
+     */
+    private double dx;
+
+    /**
+     * <code>optional double dy = 2;</code>
+     */
+    private double dy;
+
+    /**
+     * <code>optional double dz = 3;</code>
+     */
+    private double dz;
+
+    /**
+     * <code>optional double rx = 4;</code>
+     */
+    private double rx;
+
+    /**
+     * <code>optional double ry = 5;</code>
+     */
+    private double ry;
+
+    /**
+     * <code>optional double rz = 6;</code>
+     */
+    private double rz;
+
+    private ProtobufTwist3d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTwist3d}
+     */
+    public static ProtobufTwist3d newInstance() {
+      return new ProtobufTwist3d();
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @return whether the dx field is set
+     */
+    public boolean hasDx() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @return this
+     */
+    public ProtobufTwist3d clearDx() {
+      bitField0_ &= ~0x00000001;
+      dx = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @return the dx
+     */
+    public double getDx() {
+      return dx;
+    }
+
+    /**
+     * <code>optional double dx = 1;</code>
+     * @param value the dx to set
+     * @return this
+     */
+    public ProtobufTwist3d setDx(final double value) {
+      bitField0_ |= 0x00000001;
+      dx = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @return whether the dy field is set
+     */
+    public boolean hasDy() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @return this
+     */
+    public ProtobufTwist3d clearDy() {
+      bitField0_ &= ~0x00000002;
+      dy = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @return the dy
+     */
+    public double getDy() {
+      return dy;
+    }
+
+    /**
+     * <code>optional double dy = 2;</code>
+     * @param value the dy to set
+     * @return this
+     */
+    public ProtobufTwist3d setDy(final double value) {
+      bitField0_ |= 0x00000002;
+      dy = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double dz = 3;</code>
+     * @return whether the dz field is set
+     */
+    public boolean hasDz() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double dz = 3;</code>
+     * @return this
+     */
+    public ProtobufTwist3d clearDz() {
+      bitField0_ &= ~0x00000004;
+      dz = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dz = 3;</code>
+     * @return the dz
+     */
+    public double getDz() {
+      return dz;
+    }
+
+    /**
+     * <code>optional double dz = 3;</code>
+     * @param value the dz to set
+     * @return this
+     */
+    public ProtobufTwist3d setDz(final double value) {
+      bitField0_ |= 0x00000004;
+      dz = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rx = 4;</code>
+     * @return whether the rx field is set
+     */
+    public boolean hasRx() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double rx = 4;</code>
+     * @return this
+     */
+    public ProtobufTwist3d clearRx() {
+      bitField0_ &= ~0x00000008;
+      rx = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rx = 4;</code>
+     * @return the rx
+     */
+    public double getRx() {
+      return rx;
+    }
+
+    /**
+     * <code>optional double rx = 4;</code>
+     * @param value the rx to set
+     * @return this
+     */
+    public ProtobufTwist3d setRx(final double value) {
+      bitField0_ |= 0x00000008;
+      rx = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double ry = 5;</code>
+     * @return whether the ry field is set
+     */
+    public boolean hasRy() {
+      return (bitField0_ & 0x00000010) != 0;
+    }
+
+    /**
+     * <code>optional double ry = 5;</code>
+     * @return this
+     */
+    public ProtobufTwist3d clearRy() {
+      bitField0_ &= ~0x00000010;
+      ry = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ry = 5;</code>
+     * @return the ry
+     */
+    public double getRy() {
+      return ry;
+    }
+
+    /**
+     * <code>optional double ry = 5;</code>
+     * @param value the ry to set
+     * @return this
+     */
+    public ProtobufTwist3d setRy(final double value) {
+      bitField0_ |= 0x00000010;
+      ry = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rz = 6;</code>
+     * @return whether the rz field is set
+     */
+    public boolean hasRz() {
+      return (bitField0_ & 0x00000020) != 0;
+    }
+
+    /**
+     * <code>optional double rz = 6;</code>
+     * @return this
+     */
+    public ProtobufTwist3d clearRz() {
+      bitField0_ &= ~0x00000020;
+      rz = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rz = 6;</code>
+     * @return the rz
+     */
+    public double getRz() {
+      return rz;
+    }
+
+    /**
+     * <code>optional double rz = 6;</code>
+     * @param value the rz to set
+     * @return this
+     */
+    public ProtobufTwist3d setRz(final double value) {
+      bitField0_ |= 0x00000020;
+      rz = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist3d copyFrom(final ProtobufTwist3d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        dx = other.dx;
+        dy = other.dy;
+        dz = other.dz;
+        rx = other.rx;
+        ry = other.ry;
+        rz = other.rz;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist3d mergeFrom(final ProtobufTwist3d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasDx()) {
+        setDx(other.dx);
+      }
+      if (other.hasDy()) {
+        setDy(other.dy);
+      }
+      if (other.hasDz()) {
+        setDz(other.dz);
+      }
+      if (other.hasRx()) {
+        setRx(other.rx);
+      }
+      if (other.hasRy()) {
+        setRy(other.ry);
+      }
+      if (other.hasRz()) {
+        setRz(other.rz);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist3d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      dx = 0D;
+      dy = 0D;
+      dz = 0D;
+      rx = 0D;
+      ry = 0D;
+      rz = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist3d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTwist3d)) {
+        return false;
+      }
+      ProtobufTwist3d other = (ProtobufTwist3d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasDx() || ProtoUtil.isEqual(dx, other.dx))
+        && (!hasDy() || ProtoUtil.isEqual(dy, other.dy))
+        && (!hasDz() || ProtoUtil.isEqual(dz, other.dz))
+        && (!hasRx() || ProtoUtil.isEqual(rx, other.rx))
+        && (!hasRy() || ProtoUtil.isEqual(ry, other.ry))
+        && (!hasRz() || ProtoUtil.isEqual(rz, other.rz));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(dx);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(dy);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(dz);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(rx);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeRawByte((byte) 41);
+        output.writeDoubleNoTag(ry);
+      }
+      if ((bitField0_ & 0x00000020) != 0) {
+        output.writeRawByte((byte) 49);
+        output.writeDoubleNoTag(rz);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000020) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTwist3d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // dx
+            dx = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // dy
+            dy = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // dz
+            dz = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // rx
+            rx = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 41) {
+              break;
+            }
+          }
+          case 41: {
+            // ry
+            ry = input.readDouble();
+            bitField0_ |= 0x00000010;
+            tag = input.readTag();
+            if (tag != 49) {
+              break;
+            }
+          }
+          case 49: {
+            // rz
+            rz = input.readDouble();
+            bitField0_ |= 0x00000020;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.dx, dx);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.dy, dy);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.dz, dz);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.rx, rx);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeDouble(FieldNames.ry, ry);
+      }
+      if ((bitField0_ & 0x00000020) != 0) {
+        output.writeDouble(FieldNames.rz, rz);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTwist3d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3220: {
+            if (input.isAtField(FieldNames.dx)) {
+              if (!input.trySkipNullValue()) {
+                dx = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3221: {
+            if (input.isAtField(FieldNames.dy)) {
+              if (!input.trySkipNullValue()) {
+                dy = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3222: {
+            if (input.isAtField(FieldNames.dz)) {
+              if (!input.trySkipNullValue()) {
+                dz = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3654: {
+            if (input.isAtField(FieldNames.rx)) {
+              if (!input.trySkipNullValue()) {
+                rx = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3655: {
+            if (input.isAtField(FieldNames.ry)) {
+              if (!input.trySkipNullValue()) {
+                ry = input.readDouble();
+                bitField0_ |= 0x00000010;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3656: {
+            if (input.isAtField(FieldNames.rz)) {
+              if (!input.trySkipNullValue()) {
+                rz = input.readDouble();
+                bitField0_ |= 0x00000020;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTwist3d clone() {
+      return new ProtobufTwist3d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTwist3d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTwist3d(), data).checkInitialized();
+    }
+
+    public static ProtobufTwist3d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTwist3d(), input).checkInitialized();
+    }
+
+    public static ProtobufTwist3d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTwist3d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTwist3d messages
+     */
+    public static MessageFactory<ProtobufTwist3d> getFactory() {
+      return ProtobufTwist3dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry3D.wpi_proto_ProtobufTwist3d_descriptor;
+    }
+
+    private enum ProtobufTwist3dFactory implements MessageFactory<ProtobufTwist3d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTwist3d create() {
+        return ProtobufTwist3d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName dx = FieldName.forField("dx");
+
+      static final FieldName dy = FieldName.forField("dy");
+
+      static final FieldName dz = FieldName.forField("dz");
+
+      static final FieldName rx = FieldName.forField("rx");
+
+      static final FieldName ry = FieldName.forField("ry");
+
+      static final FieldName rz = FieldName.forField("rz");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java
new file mode 100644
index 0000000..bd0c445
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java
@@ -0,0 +1,4196 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+import us.hebi.quickbuf.RepeatedMessage;
+
+public final class Kinematics {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3208,
+    "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + 
+    "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + 
+    "BW9tZWdhIkYKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh8KC3RyYWNrX3dpZHRo" + 
+    "GAEgASgBUgp0cmFja1dpZHRoIlAKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVXaGVlbFNwZWVkcxIS" + 
+    "CgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCKkAgoeUHJvdG9idWZNZWNhbnVt" + 
+    "RHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFu" + 
+    "c2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmlnaHQYAiABKAsyIC53cGkucHJvdG8uUHJvdG9i" + 
+    "dWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJlYXJfbGVmdBgDIAEoCzIgLndwaS5wcm90by5Q" + 
+    "cm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8KCnJlYXJfcmlnaHQYBCABKAsyIC53cGkucHJv" + 
+    "dG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmlnaHQinwEKIVByb3RvYnVmTWVjYW51bURyaXZl" + 
+    "TW90b3JWb2x0YWdlcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQY" + 
+    "AiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0" + 
+    "GAQgASgBUglyZWFyUmlnaHQioAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoK" + 
+    "ZnJvbnRfbGVmdBgBIAEoAVIJZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0" + 
+    "EhsKCXJlYXJfbGVmdBgDIAEoAVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0" + 
+    "Ip0BCh9Qcm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsU3BlZWRzEh0KCmZyb250X2xlZnQYASABKAFSCWZy" + 
+    "b250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFS" + 
+    "CHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURy" + 
+    "aXZlS2luZW1hdGljcxI6Cgdtb2R1bGVzGAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRp" + 
+    "b24yZFIHbW9kdWxlcyJvChxQcm90b2J1ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEg",
+    "ASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRS" + 
+    "BWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUSFAoFc3BlZWQYASABKAFSBXNwZWVkEjMK" + 
+    "BWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1Lndw" + 
+    "aS5maXJzdC5tYXRoLnByb3RvSocOCgYSBAAAPwEKCAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAa" + 
+    "CggKAQgSAwYAMQoJCgIIARIDBgAxCgoKAgQAEgQIAAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoM" + 
+    "CgUEAAIABRIDCQIICgwKBQQAAgABEgMJCQsKDAoFBAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAAC" + 
+    "AQUSAwoCCAoMCgUEAAIBARIDCgkLCgwKBQQAAgEDEgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgML" + 
+    "AggKDAoFBAACAgESAwsJDgoMCgUEAAICAxIDCxESCgoKAgQBEgQOABABCgoKAwQBARIDDggrCgsKBAQB" + 
+    "AgASAw8CGQoMCgUEAQIABRIDDwIICgwKBQQBAgABEgMPCRQKDAoFBAECAAMSAw8XGAoKCgIEAhIEEgAV" + 
+    "AQoKCgMEAgESAxIILAoLCgQEAgIAEgMTAhIKDAoFBAICAAUSAxMCCAoMCgUEAgIAARIDEwkNCgwKBQQC" + 
+    "AgADEgMTEBEKCwoEBAICARIDFAITCgwKBQQCAgEFEgMUAggKDAoFBAICAQESAxQJDgoMCgUEAgIBAxID" + 
+    "FBESCgoKAgQDEgQXABwBCgoKAwQDARIDFwgmCgsKBAQDAgASAxgCJwoMCgUEAwIABhIDGAIXCgwKBQQD" + 
+    "AgABEgMYGCIKDAoFBAMCAAMSAxglJgoLCgQEAwIBEgMZAigKDAoFBAMCAQYSAxkCFwoMCgUEAwIBARID" + 
+    "GRgjCgwKBQQDAgEDEgMZJicKCwoEBAMCAhIDGgImCgwKBQQDAgIGEgMaAhcKDAoFBAMCAgESAxoYIQoM" + 
+    "CgUEAwICAxIDGiQlCgsKBAQDAgMSAxsCJwoMCgUEAwIDBhIDGwIXCgwKBQQDAgMBEgMbGCIKDAoFBAMC" + 
+    "AwMSAxslJgoKCgIEBBIEHgAjAQoKCgMEBAESAx4IKQoLCgQEBAIAEgMfAhgKDAoFBAQCAAUSAx8CCAoM" + 
+    "CgUEBAIAARIDHwkTCgwKBQQEAgADEgMfFhcKCwoEBAQCARIDIAIZCgwKBQQEAgEFEgMgAggKDAoFBAQC" + 
+    "AQESAyAJFAoMCgUEBAIBAxIDIBcYCgsKBAQEAgISAyECFwoMCgUEBAICBRIDIQIICgwKBQQEAgIBEgMh" + 
+    "CRIKDAoFBAQCAgMSAyEVFgoLCgQEBAIDEgMiAhgKDAoFBAQCAwUSAyICCAoMCgUEBAIDARIDIgkTCgwK" + 
+    "BQQEAgMDEgMiFhcKCgoCBAUSBCUAKgEKCgoDBAUBEgMlCCoKCwoEBAUCABIDJgIYCgwKBQQFAgAFEgMm",
+    "AggKDAoFBAUCAAESAyYJEwoMCgUEBQIAAxIDJhYXCgsKBAQFAgESAycCGQoMCgUEBQIBBRIDJwIICgwK" + 
+    "BQQFAgEBEgMnCRQKDAoFBAUCAQMSAycXGAoLCgQEBQICEgMoAhcKDAoFBAUCAgUSAygCCAoMCgUEBQIC" + 
+    "ARIDKAkSCgwKBQQFAgIDEgMoFRYKCwoEBAUCAxIDKQIYCgwKBQQFAgMFEgMpAggKDAoFBAUCAwESAykJ" + 
+    "EwoMCgUEBQIDAxIDKRYXCgoKAgQGEgQsADEBCgoKAwQGARIDLAgnCgsKBAQGAgASAy0CGAoMCgUEBgIA" + 
+    "BRIDLQIICgwKBQQGAgABEgMtCRMKDAoFBAYCAAMSAy0WFwoLCgQEBgIBEgMuAhkKDAoFBAYCAQUSAy4C" + 
+    "CAoMCgUEBgIBARIDLgkUCgwKBQQGAgEDEgMuFxgKCwoEBAYCAhIDLwIXCgwKBQQGAgIFEgMvAggKDAoF" + 
+    "BAYCAgESAy8JEgoMCgUEBgICAxIDLxUWCgsKBAQGAgMSAzACGAoMCgUEBgIDBRIDMAIICgwKBQQGAgMB" + 
+    "EgMwCRMKDAoFBAYCAwMSAzAWFwoKCgIEBxIEMwA1AQoKCgMEBwESAzMIJQoLCgQEBwIAEgM0Ai0KDAoF" + 
+    "BAcCAAQSAzQCCgoMCgUEBwIABhIDNAsgCgwKBQQHAgABEgM0ISgKDAoFBAcCAAMSAzQrLAoKCgIECBIE" + 
+    "NwA6AQoKCgMECAESAzcIJAoLCgQECAIAEgM4AhYKDAoFBAgCAAUSAzgCCAoMCgUECAIAARIDOAkRCgwK" + 
+    "BQQIAgADEgM4FBUKCwoEBAgCARIDOQIfCgwKBQQIAgEGEgM5AhQKDAoFBAgCAQESAzkVGgoMCgUECAIB" + 
+    "AxIDOR0eCgoKAgQJEgQ8AD8BCgoKAwQJARIDPAghCgsKBAQJAgASAz0CEwoMCgUECQIABRIDPQIICgwK" + 
+    "BQQJAgABEgM9CQ4KDAoFBAkCAAMSAz0REgoLCgQECQIBEgM+Ah8KDAoFBAkCAQYSAz4CFAoMCgUECQIB" + 
+    "ARIDPhUaCgwKBQQJAgEDEgM+HR5iBnByb3RvMw==");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor());
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufChassisSpeeds_descriptor = descriptor.internalContainedType(49, 77, "ProtobufChassisSpeeds", "wpi.proto.ProtobufChassisSpeeds");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(128, 70, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(200, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(283, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor = descriptor.internalContainedType(578, 159, "ProtobufMecanumDriveMotorVoltages", "wpi.proto.ProtobufMecanumDriveMotorVoltages");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(740, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(903, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1062, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1155, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1268, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufChassisSpeeds}
+   */
+  public static final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpeeds> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double vx = 1;</code>
+     */
+    private double vx;
+
+    /**
+     * <code>optional double vy = 2;</code>
+     */
+    private double vy;
+
+    /**
+     * <code>optional double omega = 3;</code>
+     */
+    private double omega;
+
+    private ProtobufChassisSpeeds() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufChassisSpeeds}
+     */
+    public static ProtobufChassisSpeeds newInstance() {
+      return new ProtobufChassisSpeeds();
+    }
+
+    /**
+     * <code>optional double vx = 1;</code>
+     * @return whether the vx field is set
+     */
+    public boolean hasVx() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double vx = 1;</code>
+     * @return this
+     */
+    public ProtobufChassisSpeeds clearVx() {
+      bitField0_ &= ~0x00000001;
+      vx = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double vx = 1;</code>
+     * @return the vx
+     */
+    public double getVx() {
+      return vx;
+    }
+
+    /**
+     * <code>optional double vx = 1;</code>
+     * @param value the vx to set
+     * @return this
+     */
+    public ProtobufChassisSpeeds setVx(final double value) {
+      bitField0_ |= 0x00000001;
+      vx = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double vy = 2;</code>
+     * @return whether the vy field is set
+     */
+    public boolean hasVy() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double vy = 2;</code>
+     * @return this
+     */
+    public ProtobufChassisSpeeds clearVy() {
+      bitField0_ &= ~0x00000002;
+      vy = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double vy = 2;</code>
+     * @return the vy
+     */
+    public double getVy() {
+      return vy;
+    }
+
+    /**
+     * <code>optional double vy = 2;</code>
+     * @param value the vy to set
+     * @return this
+     */
+    public ProtobufChassisSpeeds setVy(final double value) {
+      bitField0_ |= 0x00000002;
+      vy = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double omega = 3;</code>
+     * @return whether the omega field is set
+     */
+    public boolean hasOmega() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double omega = 3;</code>
+     * @return this
+     */
+    public ProtobufChassisSpeeds clearOmega() {
+      bitField0_ &= ~0x00000004;
+      omega = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double omega = 3;</code>
+     * @return the omega
+     */
+    public double getOmega() {
+      return omega;
+    }
+
+    /**
+     * <code>optional double omega = 3;</code>
+     * @param value the omega to set
+     * @return this
+     */
+    public ProtobufChassisSpeeds setOmega(final double value) {
+      bitField0_ |= 0x00000004;
+      omega = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufChassisSpeeds copyFrom(final ProtobufChassisSpeeds other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        vx = other.vx;
+        vy = other.vy;
+        omega = other.omega;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufChassisSpeeds mergeFrom(final ProtobufChassisSpeeds other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasVx()) {
+        setVx(other.vx);
+      }
+      if (other.hasVy()) {
+        setVy(other.vy);
+      }
+      if (other.hasOmega()) {
+        setOmega(other.omega);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufChassisSpeeds clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      vx = 0D;
+      vy = 0D;
+      omega = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufChassisSpeeds clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufChassisSpeeds)) {
+        return false;
+      }
+      ProtobufChassisSpeeds other = (ProtobufChassisSpeeds) o;
+      return bitField0_ == other.bitField0_
+        && (!hasVx() || ProtoUtil.isEqual(vx, other.vx))
+        && (!hasVy() || ProtoUtil.isEqual(vy, other.vy))
+        && (!hasOmega() || ProtoUtil.isEqual(omega, other.omega));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(vx);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(vy);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(omega);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufChassisSpeeds mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // vx
+            vx = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // vy
+            vy = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // omega
+            omega = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.vx, vx);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.vy, vy);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.omega, omega);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufChassisSpeeds mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3778: {
+            if (input.isAtField(FieldNames.vx)) {
+              if (!input.trySkipNullValue()) {
+                vx = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3779: {
+            if (input.isAtField(FieldNames.vy)) {
+              if (!input.trySkipNullValue()) {
+                vy = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 105858401: {
+            if (input.isAtField(FieldNames.omega)) {
+              if (!input.trySkipNullValue()) {
+                omega = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufChassisSpeeds clone() {
+      return new ProtobufChassisSpeeds().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufChassisSpeeds parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufChassisSpeeds(), data).checkInitialized();
+    }
+
+    public static ProtobufChassisSpeeds parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufChassisSpeeds(), input).checkInitialized();
+    }
+
+    public static ProtobufChassisSpeeds parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufChassisSpeeds(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufChassisSpeeds messages
+     */
+    public static MessageFactory<ProtobufChassisSpeeds> getFactory() {
+      return ProtobufChassisSpeedsFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufChassisSpeeds_descriptor;
+    }
+
+    private enum ProtobufChassisSpeedsFactory implements MessageFactory<ProtobufChassisSpeeds> {
+      INSTANCE;
+
+      @Override
+      public ProtobufChassisSpeeds create() {
+        return ProtobufChassisSpeeds.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName vx = FieldName.forField("vx");
+
+      static final FieldName vy = FieldName.forField("vy");
+
+      static final FieldName omega = FieldName.forField("omega");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufDifferentialDriveKinematics}
+   */
+  public static final class ProtobufDifferentialDriveKinematics extends ProtoMessage<ProtobufDifferentialDriveKinematics> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double track_width = 1;</code>
+     */
+    private double trackWidth;
+
+    private ProtobufDifferentialDriveKinematics() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufDifferentialDriveKinematics}
+     */
+    public static ProtobufDifferentialDriveKinematics newInstance() {
+      return new ProtobufDifferentialDriveKinematics();
+    }
+
+    /**
+     * <code>optional double track_width = 1;</code>
+     * @return whether the trackWidth field is set
+     */
+    public boolean hasTrackWidth() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double track_width = 1;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveKinematics clearTrackWidth() {
+      bitField0_ &= ~0x00000001;
+      trackWidth = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double track_width = 1;</code>
+     * @return the trackWidth
+     */
+    public double getTrackWidth() {
+      return trackWidth;
+    }
+
+    /**
+     * <code>optional double track_width = 1;</code>
+     * @param value the trackWidth to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveKinematics setTrackWidth(final double value) {
+      bitField0_ |= 0x00000001;
+      trackWidth = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveKinematics copyFrom(
+        final ProtobufDifferentialDriveKinematics other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        trackWidth = other.trackWidth;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveKinematics mergeFrom(
+        final ProtobufDifferentialDriveKinematics other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasTrackWidth()) {
+        setTrackWidth(other.trackWidth);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveKinematics clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      trackWidth = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveKinematics clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufDifferentialDriveKinematics)) {
+        return false;
+      }
+      ProtobufDifferentialDriveKinematics other = (ProtobufDifferentialDriveKinematics) o;
+      return bitField0_ == other.bitField0_
+        && (!hasTrackWidth() || ProtoUtil.isEqual(trackWidth, other.trackWidth));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(trackWidth);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufDifferentialDriveKinematics mergeFrom(final ProtoSource input) throws
+        IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // trackWidth
+            trackWidth = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.trackWidth, trackWidth);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufDifferentialDriveKinematics mergeFrom(final JsonSource input) throws
+        IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 1152213819:
+          case 1600986578: {
+            if (input.isAtField(FieldNames.trackWidth)) {
+              if (!input.trySkipNullValue()) {
+                trackWidth = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveKinematics clone() {
+      return new ProtobufDifferentialDriveKinematics().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufDifferentialDriveKinematics parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveKinematics(), data).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveKinematics parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveKinematics(), input).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveKinematics parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveKinematics(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufDifferentialDriveKinematics messages
+     */
+    public static MessageFactory<ProtobufDifferentialDriveKinematics> getFactory() {
+      return ProtobufDifferentialDriveKinematicsFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufDifferentialDriveKinematics_descriptor;
+    }
+
+    private enum ProtobufDifferentialDriveKinematicsFactory implements MessageFactory<ProtobufDifferentialDriveKinematics> {
+      INSTANCE;
+
+      @Override
+      public ProtobufDifferentialDriveKinematics create() {
+        return ProtobufDifferentialDriveKinematics.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName trackWidth = FieldName.forField("trackWidth", "track_width");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufDifferentialDriveWheelSpeeds}
+   */
+  public static final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<ProtobufDifferentialDriveWheelSpeeds> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double left = 1;</code>
+     */
+    private double left;
+
+    /**
+     * <code>optional double right = 2;</code>
+     */
+    private double right;
+
+    private ProtobufDifferentialDriveWheelSpeeds() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufDifferentialDriveWheelSpeeds}
+     */
+    public static ProtobufDifferentialDriveWheelSpeeds newInstance() {
+      return new ProtobufDifferentialDriveWheelSpeeds();
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @return whether the left field is set
+     */
+    public boolean hasLeft() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelSpeeds clearLeft() {
+      bitField0_ &= ~0x00000001;
+      left = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @return the left
+     */
+    public double getLeft() {
+      return left;
+    }
+
+    /**
+     * <code>optional double left = 1;</code>
+     * @param value the left to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelSpeeds setLeft(final double value) {
+      bitField0_ |= 0x00000001;
+      left = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @return whether the right field is set
+     */
+    public boolean hasRight() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelSpeeds clearRight() {
+      bitField0_ &= ~0x00000002;
+      right = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @return the right
+     */
+    public double getRight() {
+      return right;
+    }
+
+    /**
+     * <code>optional double right = 2;</code>
+     * @param value the right to set
+     * @return this
+     */
+    public ProtobufDifferentialDriveWheelSpeeds setRight(final double value) {
+      bitField0_ |= 0x00000002;
+      right = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelSpeeds copyFrom(
+        final ProtobufDifferentialDriveWheelSpeeds other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        left = other.left;
+        right = other.right;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelSpeeds mergeFrom(
+        final ProtobufDifferentialDriveWheelSpeeds other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasLeft()) {
+        setLeft(other.left);
+      }
+      if (other.hasRight()) {
+        setRight(other.right);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelSpeeds clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      left = 0D;
+      right = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelSpeeds clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufDifferentialDriveWheelSpeeds)) {
+        return false;
+      }
+      ProtobufDifferentialDriveWheelSpeeds other = (ProtobufDifferentialDriveWheelSpeeds) o;
+      return bitField0_ == other.bitField0_
+        && (!hasLeft() || ProtoUtil.isEqual(left, other.left))
+        && (!hasRight() || ProtoUtil.isEqual(right, other.right));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(left);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(right);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufDifferentialDriveWheelSpeeds mergeFrom(final ProtoSource input) throws
+        IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // left
+            left = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // right
+            right = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.left, left);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.right, right);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelSpeeds mergeFrom(final JsonSource input) throws
+        IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3317767: {
+            if (input.isAtField(FieldNames.left)) {
+              if (!input.trySkipNullValue()) {
+                left = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 108511772: {
+            if (input.isAtField(FieldNames.right)) {
+              if (!input.trySkipNullValue()) {
+                right = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufDifferentialDriveWheelSpeeds clone() {
+      return new ProtobufDifferentialDriveWheelSpeeds().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufDifferentialDriveWheelSpeeds parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelSpeeds(), data).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveWheelSpeeds parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelSpeeds(), input).checkInitialized();
+    }
+
+    public static ProtobufDifferentialDriveWheelSpeeds parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelSpeeds(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufDifferentialDriveWheelSpeeds messages
+     */
+    public static MessageFactory<ProtobufDifferentialDriveWheelSpeeds> getFactory() {
+      return ProtobufDifferentialDriveWheelSpeedsFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor;
+    }
+
+    private enum ProtobufDifferentialDriveWheelSpeedsFactory implements MessageFactory<ProtobufDifferentialDriveWheelSpeeds> {
+      INSTANCE;
+
+      @Override
+      public ProtobufDifferentialDriveWheelSpeeds create() {
+        return ProtobufDifferentialDriveWheelSpeeds.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName left = FieldName.forField("left");
+
+      static final FieldName right = FieldName.forField("right");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufMecanumDriveKinematics}
+   */
+  public static final class ProtobufMecanumDriveKinematics extends ProtoMessage<ProtobufMecanumDriveKinematics> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_left = 1;</code>
+     */
+    private final Geometry2D.ProtobufTranslation2d frontLeft = Geometry2D.ProtobufTranslation2d.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_right = 2;</code>
+     */
+    private final Geometry2D.ProtobufTranslation2d frontRight = Geometry2D.ProtobufTranslation2d.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_left = 3;</code>
+     */
+    private final Geometry2D.ProtobufTranslation2d rearLeft = Geometry2D.ProtobufTranslation2d.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_right = 4;</code>
+     */
+    private final Geometry2D.ProtobufTranslation2d rearRight = Geometry2D.ProtobufTranslation2d.newInstance();
+
+    private ProtobufMecanumDriveKinematics() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufMecanumDriveKinematics}
+     */
+    public static ProtobufMecanumDriveKinematics newInstance() {
+      return new ProtobufMecanumDriveKinematics();
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_left = 1;</code>
+     * @return whether the frontLeft field is set
+     */
+    public boolean hasFrontLeft() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_left = 1;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics clearFrontLeft() {
+      bitField0_ &= ~0x00000001;
+      frontLeft.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_left = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableFrontLeft()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Geometry2D.ProtobufTranslation2d getFrontLeft() {
+      return frontLeft;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_left = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Geometry2D.ProtobufTranslation2d getMutableFrontLeft() {
+      bitField0_ |= 0x00000001;
+      return frontLeft;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_left = 1;</code>
+     * @param value the frontLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics setFrontLeft(
+        final Geometry2D.ProtobufTranslation2d value) {
+      bitField0_ |= 0x00000001;
+      frontLeft.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_right = 2;</code>
+     * @return whether the frontRight field is set
+     */
+    public boolean hasFrontRight() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_right = 2;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics clearFrontRight() {
+      bitField0_ &= ~0x00000002;
+      frontRight.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_right = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableFrontRight()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Geometry2D.ProtobufTranslation2d getFrontRight() {
+      return frontRight;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_right = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Geometry2D.ProtobufTranslation2d getMutableFrontRight() {
+      bitField0_ |= 0x00000002;
+      return frontRight;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d front_right = 2;</code>
+     * @param value the frontRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics setFrontRight(
+        final Geometry2D.ProtobufTranslation2d value) {
+      bitField0_ |= 0x00000002;
+      frontRight.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_left = 3;</code>
+     * @return whether the rearLeft field is set
+     */
+    public boolean hasRearLeft() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_left = 3;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics clearRearLeft() {
+      bitField0_ &= ~0x00000004;
+      rearLeft.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_left = 3;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableRearLeft()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Geometry2D.ProtobufTranslation2d getRearLeft() {
+      return rearLeft;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_left = 3;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Geometry2D.ProtobufTranslation2d getMutableRearLeft() {
+      bitField0_ |= 0x00000004;
+      return rearLeft;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_left = 3;</code>
+     * @param value the rearLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics setRearLeft(
+        final Geometry2D.ProtobufTranslation2d value) {
+      bitField0_ |= 0x00000004;
+      rearLeft.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_right = 4;</code>
+     * @return whether the rearRight field is set
+     */
+    public boolean hasRearRight() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_right = 4;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics clearRearRight() {
+      bitField0_ &= ~0x00000008;
+      rearRight.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_right = 4;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableRearRight()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Geometry2D.ProtobufTranslation2d getRearRight() {
+      return rearRight;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_right = 4;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Geometry2D.ProtobufTranslation2d getMutableRearRight() {
+      bitField0_ |= 0x00000008;
+      return rearRight;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufTranslation2d rear_right = 4;</code>
+     * @param value the rearRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveKinematics setRearRight(
+        final Geometry2D.ProtobufTranslation2d value) {
+      bitField0_ |= 0x00000008;
+      rearRight.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveKinematics copyFrom(final ProtobufMecanumDriveKinematics other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        frontLeft.copyFrom(other.frontLeft);
+        frontRight.copyFrom(other.frontRight);
+        rearLeft.copyFrom(other.rearLeft);
+        rearRight.copyFrom(other.rearRight);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveKinematics mergeFrom(final ProtobufMecanumDriveKinematics other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasFrontLeft()) {
+        getMutableFrontLeft().mergeFrom(other.frontLeft);
+      }
+      if (other.hasFrontRight()) {
+        getMutableFrontRight().mergeFrom(other.frontRight);
+      }
+      if (other.hasRearLeft()) {
+        getMutableRearLeft().mergeFrom(other.rearLeft);
+      }
+      if (other.hasRearRight()) {
+        getMutableRearRight().mergeFrom(other.rearRight);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveKinematics clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      frontLeft.clear();
+      frontRight.clear();
+      rearLeft.clear();
+      rearRight.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveKinematics clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      frontLeft.clearQuick();
+      frontRight.clearQuick();
+      rearLeft.clearQuick();
+      rearRight.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufMecanumDriveKinematics)) {
+        return false;
+      }
+      ProtobufMecanumDriveKinematics other = (ProtobufMecanumDriveKinematics) o;
+      return bitField0_ == other.bitField0_
+        && (!hasFrontLeft() || frontLeft.equals(other.frontLeft))
+        && (!hasFrontRight() || frontRight.equals(other.frontRight))
+        && (!hasRearLeft() || rearLeft.equals(other.rearLeft))
+        && (!hasRearRight() || rearRight.equals(other.rearRight));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 18);
+        output.writeMessageNoTag(frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 26);
+        output.writeMessageNoTag(rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 34);
+        output.writeMessageNoTag(rearRight);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(rearRight);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufMecanumDriveKinematics mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // frontLeft
+            input.readMessage(frontLeft);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // frontRight
+            input.readMessage(frontRight);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 26) {
+              break;
+            }
+          }
+          case 26: {
+            // rearLeft
+            input.readMessage(rearLeft);
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 34) {
+              break;
+            }
+          }
+          case 34: {
+            // rearRight
+            input.readMessage(rearRight);
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeMessage(FieldNames.frontLeft, frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeMessage(FieldNames.frontRight, frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeMessage(FieldNames.rearLeft, rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeMessage(FieldNames.rearRight, rearRight);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 127514064:
+          case -324277155: {
+            if (input.isAtField(FieldNames.frontLeft)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(frontLeft);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -336370317:
+          case -1456996218: {
+            if (input.isAtField(FieldNames.frontRight)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(frontRight);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -854852661:
+          case -712874558: {
+            if (input.isAtField(FieldNames.rearLeft)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(rearLeft);
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -724967720:
+          case -618613823: {
+            if (input.isAtField(FieldNames.rearRight)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(rearRight);
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveKinematics clone() {
+      return new ProtobufMecanumDriveKinematics().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufMecanumDriveKinematics parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), data).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveKinematics parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), input).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveKinematics parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufMecanumDriveKinematics messages
+     */
+    public static MessageFactory<ProtobufMecanumDriveKinematics> getFactory() {
+      return ProtobufMecanumDriveKinematicsFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufMecanumDriveKinematics_descriptor;
+    }
+
+    private enum ProtobufMecanumDriveKinematicsFactory implements MessageFactory<ProtobufMecanumDriveKinematics> {
+      INSTANCE;
+
+      @Override
+      public ProtobufMecanumDriveKinematics create() {
+        return ProtobufMecanumDriveKinematics.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName frontLeft = FieldName.forField("frontLeft", "front_left");
+
+      static final FieldName frontRight = FieldName.forField("frontRight", "front_right");
+
+      static final FieldName rearLeft = FieldName.forField("rearLeft", "rear_left");
+
+      static final FieldName rearRight = FieldName.forField("rearRight", "rear_right");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufMecanumDriveMotorVoltages}
+   */
+  public static final class ProtobufMecanumDriveMotorVoltages extends ProtoMessage<ProtobufMecanumDriveMotorVoltages> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     */
+    private double frontLeft;
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     */
+    private double frontRight;
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     */
+    private double rearLeft;
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     */
+    private double rearRight;
+
+    private ProtobufMecanumDriveMotorVoltages() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufMecanumDriveMotorVoltages}
+     */
+    public static ProtobufMecanumDriveMotorVoltages newInstance() {
+      return new ProtobufMecanumDriveMotorVoltages();
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return whether the frontLeft field is set
+     */
+    public boolean hasFrontLeft() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages clearFrontLeft() {
+      bitField0_ &= ~0x00000001;
+      frontLeft = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return the frontLeft
+     */
+    public double getFrontLeft() {
+      return frontLeft;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @param value the frontLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages setFrontLeft(final double value) {
+      bitField0_ |= 0x00000001;
+      frontLeft = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return whether the frontRight field is set
+     */
+    public boolean hasFrontRight() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages clearFrontRight() {
+      bitField0_ &= ~0x00000002;
+      frontRight = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return the frontRight
+     */
+    public double getFrontRight() {
+      return frontRight;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @param value the frontRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages setFrontRight(final double value) {
+      bitField0_ |= 0x00000002;
+      frontRight = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return whether the rearLeft field is set
+     */
+    public boolean hasRearLeft() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages clearRearLeft() {
+      bitField0_ &= ~0x00000004;
+      rearLeft = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return the rearLeft
+     */
+    public double getRearLeft() {
+      return rearLeft;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @param value the rearLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages setRearLeft(final double value) {
+      bitField0_ |= 0x00000004;
+      rearLeft = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return whether the rearRight field is set
+     */
+    public boolean hasRearRight() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages clearRearRight() {
+      bitField0_ &= ~0x00000008;
+      rearRight = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return the rearRight
+     */
+    public double getRearRight() {
+      return rearRight;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @param value the rearRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveMotorVoltages setRearRight(final double value) {
+      bitField0_ |= 0x00000008;
+      rearRight = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveMotorVoltages copyFrom(
+        final ProtobufMecanumDriveMotorVoltages other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        frontLeft = other.frontLeft;
+        frontRight = other.frontRight;
+        rearLeft = other.rearLeft;
+        rearRight = other.rearRight;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveMotorVoltages mergeFrom(
+        final ProtobufMecanumDriveMotorVoltages other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasFrontLeft()) {
+        setFrontLeft(other.frontLeft);
+      }
+      if (other.hasFrontRight()) {
+        setFrontRight(other.frontRight);
+      }
+      if (other.hasRearLeft()) {
+        setRearLeft(other.rearLeft);
+      }
+      if (other.hasRearRight()) {
+        setRearRight(other.rearRight);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveMotorVoltages clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      frontLeft = 0D;
+      frontRight = 0D;
+      rearLeft = 0D;
+      rearRight = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveMotorVoltages clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufMecanumDriveMotorVoltages)) {
+        return false;
+      }
+      ProtobufMecanumDriveMotorVoltages other = (ProtobufMecanumDriveMotorVoltages) o;
+      return bitField0_ == other.bitField0_
+        && (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft))
+        && (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight))
+        && (!hasRearLeft() || ProtoUtil.isEqual(rearLeft, other.rearLeft))
+        && (!hasRearRight() || ProtoUtil.isEqual(rearRight, other.rearRight));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(rearRight);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufMecanumDriveMotorVoltages mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // frontLeft
+            frontLeft = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // frontRight
+            frontRight = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // rearLeft
+            rearLeft = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // rearRight
+            rearRight = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.frontLeft, frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.frontRight, frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.rearLeft, rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.rearRight, rearRight);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufMecanumDriveMotorVoltages mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 127514064:
+          case -324277155: {
+            if (input.isAtField(FieldNames.frontLeft)) {
+              if (!input.trySkipNullValue()) {
+                frontLeft = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -336370317:
+          case -1456996218: {
+            if (input.isAtField(FieldNames.frontRight)) {
+              if (!input.trySkipNullValue()) {
+                frontRight = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -854852661:
+          case -712874558: {
+            if (input.isAtField(FieldNames.rearLeft)) {
+              if (!input.trySkipNullValue()) {
+                rearLeft = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -724967720:
+          case -618613823: {
+            if (input.isAtField(FieldNames.rearRight)) {
+              if (!input.trySkipNullValue()) {
+                rearRight = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveMotorVoltages clone() {
+      return new ProtobufMecanumDriveMotorVoltages().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufMecanumDriveMotorVoltages parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveMotorVoltages(), data).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveMotorVoltages parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveMotorVoltages(), input).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveMotorVoltages parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveMotorVoltages(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufMecanumDriveMotorVoltages messages
+     */
+    public static MessageFactory<ProtobufMecanumDriveMotorVoltages> getFactory() {
+      return ProtobufMecanumDriveMotorVoltagesFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor;
+    }
+
+    private enum ProtobufMecanumDriveMotorVoltagesFactory implements MessageFactory<ProtobufMecanumDriveMotorVoltages> {
+      INSTANCE;
+
+      @Override
+      public ProtobufMecanumDriveMotorVoltages create() {
+        return ProtobufMecanumDriveMotorVoltages.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName frontLeft = FieldName.forField("frontLeft", "front_left");
+
+      static final FieldName frontRight = FieldName.forField("frontRight", "front_right");
+
+      static final FieldName rearLeft = FieldName.forField("rearLeft", "rear_left");
+
+      static final FieldName rearRight = FieldName.forField("rearRight", "rear_right");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufMecanumDriveWheelPositions}
+   */
+  public static final class ProtobufMecanumDriveWheelPositions extends ProtoMessage<ProtobufMecanumDriveWheelPositions> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     */
+    private double frontLeft;
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     */
+    private double frontRight;
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     */
+    private double rearLeft;
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     */
+    private double rearRight;
+
+    private ProtobufMecanumDriveWheelPositions() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufMecanumDriveWheelPositions}
+     */
+    public static ProtobufMecanumDriveWheelPositions newInstance() {
+      return new ProtobufMecanumDriveWheelPositions();
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return whether the frontLeft field is set
+     */
+    public boolean hasFrontLeft() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions clearFrontLeft() {
+      bitField0_ &= ~0x00000001;
+      frontLeft = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return the frontLeft
+     */
+    public double getFrontLeft() {
+      return frontLeft;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @param value the frontLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions setFrontLeft(final double value) {
+      bitField0_ |= 0x00000001;
+      frontLeft = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return whether the frontRight field is set
+     */
+    public boolean hasFrontRight() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions clearFrontRight() {
+      bitField0_ &= ~0x00000002;
+      frontRight = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return the frontRight
+     */
+    public double getFrontRight() {
+      return frontRight;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @param value the frontRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions setFrontRight(final double value) {
+      bitField0_ |= 0x00000002;
+      frontRight = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return whether the rearLeft field is set
+     */
+    public boolean hasRearLeft() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions clearRearLeft() {
+      bitField0_ &= ~0x00000004;
+      rearLeft = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return the rearLeft
+     */
+    public double getRearLeft() {
+      return rearLeft;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @param value the rearLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions setRearLeft(final double value) {
+      bitField0_ |= 0x00000004;
+      rearLeft = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return whether the rearRight field is set
+     */
+    public boolean hasRearRight() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions clearRearRight() {
+      bitField0_ &= ~0x00000008;
+      rearRight = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return the rearRight
+     */
+    public double getRearRight() {
+      return rearRight;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @param value the rearRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelPositions setRearRight(final double value) {
+      bitField0_ |= 0x00000008;
+      rearRight = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelPositions copyFrom(
+        final ProtobufMecanumDriveWheelPositions other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        frontLeft = other.frontLeft;
+        frontRight = other.frontRight;
+        rearLeft = other.rearLeft;
+        rearRight = other.rearRight;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelPositions mergeFrom(
+        final ProtobufMecanumDriveWheelPositions other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasFrontLeft()) {
+        setFrontLeft(other.frontLeft);
+      }
+      if (other.hasFrontRight()) {
+        setFrontRight(other.frontRight);
+      }
+      if (other.hasRearLeft()) {
+        setRearLeft(other.rearLeft);
+      }
+      if (other.hasRearRight()) {
+        setRearRight(other.rearRight);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelPositions clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      frontLeft = 0D;
+      frontRight = 0D;
+      rearLeft = 0D;
+      rearRight = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelPositions clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufMecanumDriveWheelPositions)) {
+        return false;
+      }
+      ProtobufMecanumDriveWheelPositions other = (ProtobufMecanumDriveWheelPositions) o;
+      return bitField0_ == other.bitField0_
+        && (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft))
+        && (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight))
+        && (!hasRearLeft() || ProtoUtil.isEqual(rearLeft, other.rearLeft))
+        && (!hasRearRight() || ProtoUtil.isEqual(rearRight, other.rearRight));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(rearRight);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufMecanumDriveWheelPositions mergeFrom(final ProtoSource input) throws
+        IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // frontLeft
+            frontLeft = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // frontRight
+            frontRight = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // rearLeft
+            rearLeft = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // rearRight
+            rearRight = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.frontLeft, frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.frontRight, frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.rearLeft, rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.rearRight, rearRight);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelPositions mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 127514064:
+          case -324277155: {
+            if (input.isAtField(FieldNames.frontLeft)) {
+              if (!input.trySkipNullValue()) {
+                frontLeft = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -336370317:
+          case -1456996218: {
+            if (input.isAtField(FieldNames.frontRight)) {
+              if (!input.trySkipNullValue()) {
+                frontRight = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -854852661:
+          case -712874558: {
+            if (input.isAtField(FieldNames.rearLeft)) {
+              if (!input.trySkipNullValue()) {
+                rearLeft = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -724967720:
+          case -618613823: {
+            if (input.isAtField(FieldNames.rearRight)) {
+              if (!input.trySkipNullValue()) {
+                rearRight = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelPositions clone() {
+      return new ProtobufMecanumDriveWheelPositions().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufMecanumDriveWheelPositions parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), data).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveWheelPositions parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), input).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveWheelPositions parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufMecanumDriveWheelPositions messages
+     */
+    public static MessageFactory<ProtobufMecanumDriveWheelPositions> getFactory() {
+      return ProtobufMecanumDriveWheelPositionsFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor;
+    }
+
+    private enum ProtobufMecanumDriveWheelPositionsFactory implements MessageFactory<ProtobufMecanumDriveWheelPositions> {
+      INSTANCE;
+
+      @Override
+      public ProtobufMecanumDriveWheelPositions create() {
+        return ProtobufMecanumDriveWheelPositions.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName frontLeft = FieldName.forField("frontLeft", "front_left");
+
+      static final FieldName frontRight = FieldName.forField("frontRight", "front_right");
+
+      static final FieldName rearLeft = FieldName.forField("rearLeft", "rear_left");
+
+      static final FieldName rearRight = FieldName.forField("rearRight", "rear_right");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufMecanumDriveWheelSpeeds}
+   */
+  public static final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<ProtobufMecanumDriveWheelSpeeds> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     */
+    private double frontLeft;
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     */
+    private double frontRight;
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     */
+    private double rearLeft;
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     */
+    private double rearRight;
+
+    private ProtobufMecanumDriveWheelSpeeds() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufMecanumDriveWheelSpeeds}
+     */
+    public static ProtobufMecanumDriveWheelSpeeds newInstance() {
+      return new ProtobufMecanumDriveWheelSpeeds();
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return whether the frontLeft field is set
+     */
+    public boolean hasFrontLeft() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds clearFrontLeft() {
+      bitField0_ &= ~0x00000001;
+      frontLeft = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @return the frontLeft
+     */
+    public double getFrontLeft() {
+      return frontLeft;
+    }
+
+    /**
+     * <code>optional double front_left = 1;</code>
+     * @param value the frontLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds setFrontLeft(final double value) {
+      bitField0_ |= 0x00000001;
+      frontLeft = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return whether the frontRight field is set
+     */
+    public boolean hasFrontRight() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds clearFrontRight() {
+      bitField0_ &= ~0x00000002;
+      frontRight = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @return the frontRight
+     */
+    public double getFrontRight() {
+      return frontRight;
+    }
+
+    /**
+     * <code>optional double front_right = 2;</code>
+     * @param value the frontRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds setFrontRight(final double value) {
+      bitField0_ |= 0x00000002;
+      frontRight = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return whether the rearLeft field is set
+     */
+    public boolean hasRearLeft() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds clearRearLeft() {
+      bitField0_ &= ~0x00000004;
+      rearLeft = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @return the rearLeft
+     */
+    public double getRearLeft() {
+      return rearLeft;
+    }
+
+    /**
+     * <code>optional double rear_left = 3;</code>
+     * @param value the rearLeft to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds setRearLeft(final double value) {
+      bitField0_ |= 0x00000004;
+      rearLeft = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return whether the rearRight field is set
+     */
+    public boolean hasRearRight() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds clearRearRight() {
+      bitField0_ &= ~0x00000008;
+      rearRight = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @return the rearRight
+     */
+    public double getRearRight() {
+      return rearRight;
+    }
+
+    /**
+     * <code>optional double rear_right = 4;</code>
+     * @param value the rearRight to set
+     * @return this
+     */
+    public ProtobufMecanumDriveWheelSpeeds setRearRight(final double value) {
+      bitField0_ |= 0x00000008;
+      rearRight = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelSpeeds copyFrom(final ProtobufMecanumDriveWheelSpeeds other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        frontLeft = other.frontLeft;
+        frontRight = other.frontRight;
+        rearLeft = other.rearLeft;
+        rearRight = other.rearRight;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtobufMecanumDriveWheelSpeeds other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasFrontLeft()) {
+        setFrontLeft(other.frontLeft);
+      }
+      if (other.hasFrontRight()) {
+        setFrontRight(other.frontRight);
+      }
+      if (other.hasRearLeft()) {
+        setRearLeft(other.rearLeft);
+      }
+      if (other.hasRearRight()) {
+        setRearRight(other.rearRight);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelSpeeds clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      frontLeft = 0D;
+      frontRight = 0D;
+      rearLeft = 0D;
+      rearRight = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelSpeeds clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufMecanumDriveWheelSpeeds)) {
+        return false;
+      }
+      ProtobufMecanumDriveWheelSpeeds other = (ProtobufMecanumDriveWheelSpeeds) o;
+      return bitField0_ == other.bitField0_
+        && (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft))
+        && (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight))
+        && (!hasRearLeft() || ProtoUtil.isEqual(rearLeft, other.rearLeft))
+        && (!hasRearRight() || ProtoUtil.isEqual(rearRight, other.rearRight));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(rearRight);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // frontLeft
+            frontLeft = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // frontRight
+            frontRight = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // rearLeft
+            rearLeft = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // rearRight
+            rearRight = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.frontLeft, frontLeft);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.frontRight, frontRight);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.rearLeft, rearLeft);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.rearRight, rearRight);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelSpeeds mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 127514064:
+          case -324277155: {
+            if (input.isAtField(FieldNames.frontLeft)) {
+              if (!input.trySkipNullValue()) {
+                frontLeft = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -336370317:
+          case -1456996218: {
+            if (input.isAtField(FieldNames.frontRight)) {
+              if (!input.trySkipNullValue()) {
+                frontRight = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -854852661:
+          case -712874558: {
+            if (input.isAtField(FieldNames.rearLeft)) {
+              if (!input.trySkipNullValue()) {
+                rearLeft = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -724967720:
+          case -618613823: {
+            if (input.isAtField(FieldNames.rearRight)) {
+              if (!input.trySkipNullValue()) {
+                rearRight = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufMecanumDriveWheelSpeeds clone() {
+      return new ProtobufMecanumDriveWheelSpeeds().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufMecanumDriveWheelSpeeds parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), data).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveWheelSpeeds parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized();
+    }
+
+    public static ProtobufMecanumDriveWheelSpeeds parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufMecanumDriveWheelSpeeds messages
+     */
+    public static MessageFactory<ProtobufMecanumDriveWheelSpeeds> getFactory() {
+      return ProtobufMecanumDriveWheelSpeedsFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor;
+    }
+
+    private enum ProtobufMecanumDriveWheelSpeedsFactory implements MessageFactory<ProtobufMecanumDriveWheelSpeeds> {
+      INSTANCE;
+
+      @Override
+      public ProtobufMecanumDriveWheelSpeeds create() {
+        return ProtobufMecanumDriveWheelSpeeds.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName frontLeft = FieldName.forField("frontLeft", "front_left");
+
+      static final FieldName frontRight = FieldName.forField("frontRight", "front_right");
+
+      static final FieldName rearLeft = FieldName.forField("rearLeft", "rear_left");
+
+      static final FieldName rearRight = FieldName.forField("rearRight", "rear_right");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufSwerveDriveKinematics}
+   */
+  public static final class ProtobufSwerveDriveKinematics extends ProtoMessage<ProtobufSwerveDriveKinematics> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTranslation2d modules = 1;</code>
+     */
+    private final RepeatedMessage<Geometry2D.ProtobufTranslation2d> modules = RepeatedMessage.newEmptyInstance(Geometry2D.ProtobufTranslation2d.getFactory());
+
+    private ProtobufSwerveDriveKinematics() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufSwerveDriveKinematics}
+     */
+    public static ProtobufSwerveDriveKinematics newInstance() {
+      return new ProtobufSwerveDriveKinematics();
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTranslation2d modules = 1;</code>
+     * @return whether the modules field is set
+     */
+    public boolean hasModules() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTranslation2d modules = 1;</code>
+     * @return this
+     */
+    public ProtobufSwerveDriveKinematics clearModules() {
+      bitField0_ &= ~0x00000001;
+      modules.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTranslation2d modules = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableModules()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedMessage<Geometry2D.ProtobufTranslation2d> getModules() {
+      return modules;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTranslation2d modules = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedMessage<Geometry2D.ProtobufTranslation2d> getMutableModules() {
+      bitField0_ |= 0x00000001;
+      return modules;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTranslation2d modules = 1;</code>
+     * @param value the modules to add
+     * @return this
+     */
+    public ProtobufSwerveDriveKinematics addModules(final Geometry2D.ProtobufTranslation2d value) {
+      bitField0_ |= 0x00000001;
+      modules.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTranslation2d modules = 1;</code>
+     * @param values the modules to add
+     * @return this
+     */
+    public ProtobufSwerveDriveKinematics addAllModules(
+        final Geometry2D.ProtobufTranslation2d... values) {
+      bitField0_ |= 0x00000001;
+      modules.addAll(values);
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveDriveKinematics copyFrom(final ProtobufSwerveDriveKinematics other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        modules.copyFrom(other.modules);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveDriveKinematics mergeFrom(final ProtobufSwerveDriveKinematics other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasModules()) {
+        getMutableModules().addAll(other.modules);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveDriveKinematics clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      modules.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveDriveKinematics clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      modules.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufSwerveDriveKinematics)) {
+        return false;
+      }
+      ProtobufSwerveDriveKinematics other = (ProtobufSwerveDriveKinematics) o;
+      return bitField0_ == other.bitField0_
+        && (!hasModules() || modules.equals(other.modules));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        for (int i = 0; i < modules.length(); i++) {
+          output.writeRawByte((byte) 10);
+          output.writeMessageNoTag(modules.get(i));
+        }
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += (1 * modules.length()) + ProtoSink.computeRepeatedMessageSizeNoTag(modules);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufSwerveDriveKinematics mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // modules
+            tag = input.readRepeatedMessage(modules, tag);
+            bitField0_ |= 0x00000001;
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRepeatedMessage(FieldNames.modules, modules);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufSwerveDriveKinematics mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 1227433863: {
+            if (input.isAtField(FieldNames.modules)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedMessage(modules);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveDriveKinematics clone() {
+      return new ProtobufSwerveDriveKinematics().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufSwerveDriveKinematics parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveDriveKinematics(), data).checkInitialized();
+    }
+
+    public static ProtobufSwerveDriveKinematics parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveDriveKinematics(), input).checkInitialized();
+    }
+
+    public static ProtobufSwerveDriveKinematics parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveDriveKinematics(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufSwerveDriveKinematics messages
+     */
+    public static MessageFactory<ProtobufSwerveDriveKinematics> getFactory() {
+      return ProtobufSwerveDriveKinematicsFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufSwerveDriveKinematics_descriptor;
+    }
+
+    private enum ProtobufSwerveDriveKinematicsFactory implements MessageFactory<ProtobufSwerveDriveKinematics> {
+      INSTANCE;
+
+      @Override
+      public ProtobufSwerveDriveKinematics create() {
+        return ProtobufSwerveDriveKinematics.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName modules = FieldName.forField("modules");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufSwerveModulePosition}
+   */
+  public static final class ProtobufSwerveModulePosition extends ProtoMessage<ProtobufSwerveModulePosition> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double distance = 1;</code>
+     */
+    private double distance;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     */
+    private final Geometry2D.ProtobufRotation2d angle = Geometry2D.ProtobufRotation2d.newInstance();
+
+    private ProtobufSwerveModulePosition() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufSwerveModulePosition}
+     */
+    public static ProtobufSwerveModulePosition newInstance() {
+      return new ProtobufSwerveModulePosition();
+    }
+
+    /**
+     * <code>optional double distance = 1;</code>
+     * @return whether the distance field is set
+     */
+    public boolean hasDistance() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double distance = 1;</code>
+     * @return this
+     */
+    public ProtobufSwerveModulePosition clearDistance() {
+      bitField0_ &= ~0x00000001;
+      distance = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double distance = 1;</code>
+     * @return the distance
+     */
+    public double getDistance() {
+      return distance;
+    }
+
+    /**
+     * <code>optional double distance = 1;</code>
+     * @param value the distance to set
+     * @return this
+     */
+    public ProtobufSwerveModulePosition setDistance(final double value) {
+      bitField0_ |= 0x00000001;
+      distance = value;
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     * @return whether the angle field is set
+     */
+    public boolean hasAngle() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     * @return this
+     */
+    public ProtobufSwerveModulePosition clearAngle() {
+      bitField0_ &= ~0x00000002;
+      angle.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableAngle()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Geometry2D.ProtobufRotation2d getAngle() {
+      return angle;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Geometry2D.ProtobufRotation2d getMutableAngle() {
+      bitField0_ |= 0x00000002;
+      return angle;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     * @param value the angle to set
+     * @return this
+     */
+    public ProtobufSwerveModulePosition setAngle(final Geometry2D.ProtobufRotation2d value) {
+      bitField0_ |= 0x00000002;
+      angle.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModulePosition copyFrom(final ProtobufSwerveModulePosition other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        distance = other.distance;
+        angle.copyFrom(other.angle);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModulePosition mergeFrom(final ProtobufSwerveModulePosition other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasDistance()) {
+        setDistance(other.distance);
+      }
+      if (other.hasAngle()) {
+        getMutableAngle().mergeFrom(other.angle);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModulePosition clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      distance = 0D;
+      angle.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModulePosition clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      angle.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufSwerveModulePosition)) {
+        return false;
+      }
+      ProtobufSwerveModulePosition other = (ProtobufSwerveModulePosition) o;
+      return bitField0_ == other.bitField0_
+        && (!hasDistance() || ProtoUtil.isEqual(distance, other.distance))
+        && (!hasAngle() || angle.equals(other.angle));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(distance);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 18);
+        output.writeMessageNoTag(angle);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(angle);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufSwerveModulePosition mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // distance
+            distance = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // angle
+            input.readMessage(angle);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.distance, distance);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeMessage(FieldNames.angle, angle);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufSwerveModulePosition mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 288459765: {
+            if (input.isAtField(FieldNames.distance)) {
+              if (!input.trySkipNullValue()) {
+                distance = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 92960979: {
+            if (input.isAtField(FieldNames.angle)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(angle);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModulePosition clone() {
+      return new ProtobufSwerveModulePosition().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufSwerveModulePosition parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveModulePosition(), data).checkInitialized();
+    }
+
+    public static ProtobufSwerveModulePosition parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveModulePosition(), input).checkInitialized();
+    }
+
+    public static ProtobufSwerveModulePosition parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveModulePosition(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufSwerveModulePosition messages
+     */
+    public static MessageFactory<ProtobufSwerveModulePosition> getFactory() {
+      return ProtobufSwerveModulePositionFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufSwerveModulePosition_descriptor;
+    }
+
+    private enum ProtobufSwerveModulePositionFactory implements MessageFactory<ProtobufSwerveModulePosition> {
+      INSTANCE;
+
+      @Override
+      public ProtobufSwerveModulePosition create() {
+        return ProtobufSwerveModulePosition.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName distance = FieldName.forField("distance");
+
+      static final FieldName angle = FieldName.forField("angle");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufSwerveModuleState}
+   */
+  public static final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerveModuleState> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double speed = 1;</code>
+     */
+    private double speed;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     */
+    private final Geometry2D.ProtobufRotation2d angle = Geometry2D.ProtobufRotation2d.newInstance();
+
+    private ProtobufSwerveModuleState() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufSwerveModuleState}
+     */
+    public static ProtobufSwerveModuleState newInstance() {
+      return new ProtobufSwerveModuleState();
+    }
+
+    /**
+     * <code>optional double speed = 1;</code>
+     * @return whether the speed field is set
+     */
+    public boolean hasSpeed() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double speed = 1;</code>
+     * @return this
+     */
+    public ProtobufSwerveModuleState clearSpeed() {
+      bitField0_ &= ~0x00000001;
+      speed = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double speed = 1;</code>
+     * @return the speed
+     */
+    public double getSpeed() {
+      return speed;
+    }
+
+    /**
+     * <code>optional double speed = 1;</code>
+     * @param value the speed to set
+     * @return this
+     */
+    public ProtobufSwerveModuleState setSpeed(final double value) {
+      bitField0_ |= 0x00000001;
+      speed = value;
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     * @return whether the angle field is set
+     */
+    public boolean hasAngle() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     * @return this
+     */
+    public ProtobufSwerveModuleState clearAngle() {
+      bitField0_ &= ~0x00000002;
+      angle.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableAngle()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Geometry2D.ProtobufRotation2d getAngle() {
+      return angle;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Geometry2D.ProtobufRotation2d getMutableAngle() {
+      bitField0_ |= 0x00000002;
+      return angle;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
+     * @param value the angle to set
+     * @return this
+     */
+    public ProtobufSwerveModuleState setAngle(final Geometry2D.ProtobufRotation2d value) {
+      bitField0_ |= 0x00000002;
+      angle.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModuleState copyFrom(final ProtobufSwerveModuleState other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        speed = other.speed;
+        angle.copyFrom(other.angle);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModuleState mergeFrom(final ProtobufSwerveModuleState other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasSpeed()) {
+        setSpeed(other.speed);
+      }
+      if (other.hasAngle()) {
+        getMutableAngle().mergeFrom(other.angle);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModuleState clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      speed = 0D;
+      angle.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModuleState clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      angle.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufSwerveModuleState)) {
+        return false;
+      }
+      ProtobufSwerveModuleState other = (ProtobufSwerveModuleState) o;
+      return bitField0_ == other.bitField0_
+        && (!hasSpeed() || ProtoUtil.isEqual(speed, other.speed))
+        && (!hasAngle() || angle.equals(other.angle));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(speed);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 18);
+        output.writeMessageNoTag(angle);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(angle);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufSwerveModuleState mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // speed
+            speed = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // angle
+            input.readMessage(angle);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.speed, speed);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeMessage(FieldNames.angle, angle);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufSwerveModuleState mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 109641799: {
+            if (input.isAtField(FieldNames.speed)) {
+              if (!input.trySkipNullValue()) {
+                speed = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 92960979: {
+            if (input.isAtField(FieldNames.angle)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(angle);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufSwerveModuleState clone() {
+      return new ProtobufSwerveModuleState().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufSwerveModuleState parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveModuleState(), data).checkInitialized();
+    }
+
+    public static ProtobufSwerveModuleState parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveModuleState(), input).checkInitialized();
+    }
+
+    public static ProtobufSwerveModuleState parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufSwerveModuleState(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufSwerveModuleState messages
+     */
+    public static MessageFactory<ProtobufSwerveModuleState> getFactory() {
+      return ProtobufSwerveModuleStateFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Kinematics.wpi_proto_ProtobufSwerveModuleState_descriptor;
+    }
+
+    private enum ProtobufSwerveModuleStateFactory implements MessageFactory<ProtobufSwerveModuleState> {
+      INSTANCE;
+
+      @Override
+      public ProtobufSwerveModuleState create() {
+        return ProtobufSwerveModuleState.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName speed = FieldName.forField("speed");
+
+      static final FieldName angle = FieldName.forField("angle");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Plant.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Plant.java
new file mode 100644
index 0000000..61b758f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Plant.java
@@ -0,0 +1,621 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+
+public final class Plant {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(610,
+    "CgtwbGFudC5wcm90bxIJd3BpLnByb3RvIsQBCg9Qcm90b2J1ZkRDTW90b3ISJwoPbm9taW5hbF92b2x0" + 
+    "YWdlGAEgASgBUg5ub21pbmFsVm9sdGFnZRIhCgxzdGFsbF90b3JxdWUYAiABKAFSC3N0YWxsVG9ycXVl" + 
+    "EiMKDXN0YWxsX2N1cnJlbnQYAyABKAFSDHN0YWxsQ3VycmVudBIhCgxmcmVlX2N1cnJlbnQYBCABKAFS" + 
+    "C2ZyZWVDdXJyZW50Eh0KCmZyZWVfc3BlZWQYBSABKAFSCWZyZWVTcGVlZEIaChhlZHUud3BpLmZpcnN0" + 
+    "Lm1hdGgucHJvdG9K3AIKBhIEAAAMAQoICgEMEgMAABIKCAoBAhIDAgASCggKAQgSAwQAMQoJCgIIARID" + 
+    "BAAxCgoKAgQAEgQGAAwBCgoKAwQAARIDBggXCgsKBAQAAgASAwcCHQoMCgUEAAIABRIDBwIICgwKBQQA" + 
+    "AgABEgMHCRgKDAoFBAACAAMSAwcbHAoLCgQEAAIBEgMIAhoKDAoFBAACAQUSAwgCCAoMCgUEAAIBARID" + 
+    "CAkVCgwKBQQAAgEDEgMIGBkKCwoEBAACAhIDCQIbCgwKBQQAAgIFEgMJAggKDAoFBAACAgESAwkJFgoM" + 
+    "CgUEAAICAxIDCRkaCgsKBAQAAgMSAwoCGgoMCgUEAAIDBRIDCgIICgwKBQQAAgMBEgMKCRUKDAoFBAAC" + 
+    "AwMSAwoYGQoLCgQEAAIEEgMLAhgKDAoFBAACBAUSAwsCCAoMCgUEAAIEARIDCwkTCgwKBQQAAgQDEgML" + 
+    "FhdiBnByb3RvMw==");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("plant.proto", "wpi.proto", descriptorData);
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufDCMotor_descriptor = descriptor.internalContainedType(27, 196, "ProtobufDCMotor", "wpi.proto.ProtobufDCMotor");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufDCMotor}
+   */
+  public static final class ProtobufDCMotor extends ProtoMessage<ProtobufDCMotor> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double nominal_voltage = 1;</code>
+     */
+    private double nominalVoltage;
+
+    /**
+     * <code>optional double stall_torque = 2;</code>
+     */
+    private double stallTorque;
+
+    /**
+     * <code>optional double stall_current = 3;</code>
+     */
+    private double stallCurrent;
+
+    /**
+     * <code>optional double free_current = 4;</code>
+     */
+    private double freeCurrent;
+
+    /**
+     * <code>optional double free_speed = 5;</code>
+     */
+    private double freeSpeed;
+
+    private ProtobufDCMotor() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufDCMotor}
+     */
+    public static ProtobufDCMotor newInstance() {
+      return new ProtobufDCMotor();
+    }
+
+    /**
+     * <code>optional double nominal_voltage = 1;</code>
+     * @return whether the nominalVoltage field is set
+     */
+    public boolean hasNominalVoltage() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double nominal_voltage = 1;</code>
+     * @return this
+     */
+    public ProtobufDCMotor clearNominalVoltage() {
+      bitField0_ &= ~0x00000001;
+      nominalVoltage = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double nominal_voltage = 1;</code>
+     * @return the nominalVoltage
+     */
+    public double getNominalVoltage() {
+      return nominalVoltage;
+    }
+
+    /**
+     * <code>optional double nominal_voltage = 1;</code>
+     * @param value the nominalVoltage to set
+     * @return this
+     */
+    public ProtobufDCMotor setNominalVoltage(final double value) {
+      bitField0_ |= 0x00000001;
+      nominalVoltage = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double stall_torque = 2;</code>
+     * @return whether the stallTorque field is set
+     */
+    public boolean hasStallTorque() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double stall_torque = 2;</code>
+     * @return this
+     */
+    public ProtobufDCMotor clearStallTorque() {
+      bitField0_ &= ~0x00000002;
+      stallTorque = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double stall_torque = 2;</code>
+     * @return the stallTorque
+     */
+    public double getStallTorque() {
+      return stallTorque;
+    }
+
+    /**
+     * <code>optional double stall_torque = 2;</code>
+     * @param value the stallTorque to set
+     * @return this
+     */
+    public ProtobufDCMotor setStallTorque(final double value) {
+      bitField0_ |= 0x00000002;
+      stallTorque = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double stall_current = 3;</code>
+     * @return whether the stallCurrent field is set
+     */
+    public boolean hasStallCurrent() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double stall_current = 3;</code>
+     * @return this
+     */
+    public ProtobufDCMotor clearStallCurrent() {
+      bitField0_ &= ~0x00000004;
+      stallCurrent = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double stall_current = 3;</code>
+     * @return the stallCurrent
+     */
+    public double getStallCurrent() {
+      return stallCurrent;
+    }
+
+    /**
+     * <code>optional double stall_current = 3;</code>
+     * @param value the stallCurrent to set
+     * @return this
+     */
+    public ProtobufDCMotor setStallCurrent(final double value) {
+      bitField0_ |= 0x00000004;
+      stallCurrent = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double free_current = 4;</code>
+     * @return whether the freeCurrent field is set
+     */
+    public boolean hasFreeCurrent() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double free_current = 4;</code>
+     * @return this
+     */
+    public ProtobufDCMotor clearFreeCurrent() {
+      bitField0_ &= ~0x00000008;
+      freeCurrent = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double free_current = 4;</code>
+     * @return the freeCurrent
+     */
+    public double getFreeCurrent() {
+      return freeCurrent;
+    }
+
+    /**
+     * <code>optional double free_current = 4;</code>
+     * @param value the freeCurrent to set
+     * @return this
+     */
+    public ProtobufDCMotor setFreeCurrent(final double value) {
+      bitField0_ |= 0x00000008;
+      freeCurrent = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double free_speed = 5;</code>
+     * @return whether the freeSpeed field is set
+     */
+    public boolean hasFreeSpeed() {
+      return (bitField0_ & 0x00000010) != 0;
+    }
+
+    /**
+     * <code>optional double free_speed = 5;</code>
+     * @return this
+     */
+    public ProtobufDCMotor clearFreeSpeed() {
+      bitField0_ &= ~0x00000010;
+      freeSpeed = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double free_speed = 5;</code>
+     * @return the freeSpeed
+     */
+    public double getFreeSpeed() {
+      return freeSpeed;
+    }
+
+    /**
+     * <code>optional double free_speed = 5;</code>
+     * @param value the freeSpeed to set
+     * @return this
+     */
+    public ProtobufDCMotor setFreeSpeed(final double value) {
+      bitField0_ |= 0x00000010;
+      freeSpeed = value;
+      return this;
+    }
+
+    @Override
+    public ProtobufDCMotor copyFrom(final ProtobufDCMotor other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        nominalVoltage = other.nominalVoltage;
+        stallTorque = other.stallTorque;
+        stallCurrent = other.stallCurrent;
+        freeCurrent = other.freeCurrent;
+        freeSpeed = other.freeSpeed;
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDCMotor mergeFrom(final ProtobufDCMotor other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasNominalVoltage()) {
+        setNominalVoltage(other.nominalVoltage);
+      }
+      if (other.hasStallTorque()) {
+        setStallTorque(other.stallTorque);
+      }
+      if (other.hasStallCurrent()) {
+        setStallCurrent(other.stallCurrent);
+      }
+      if (other.hasFreeCurrent()) {
+        setFreeCurrent(other.freeCurrent);
+      }
+      if (other.hasFreeSpeed()) {
+        setFreeSpeed(other.freeSpeed);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufDCMotor clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      nominalVoltage = 0D;
+      stallTorque = 0D;
+      stallCurrent = 0D;
+      freeCurrent = 0D;
+      freeSpeed = 0D;
+      return this;
+    }
+
+    @Override
+    public ProtobufDCMotor clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufDCMotor)) {
+        return false;
+      }
+      ProtobufDCMotor other = (ProtobufDCMotor) o;
+      return bitField0_ == other.bitField0_
+        && (!hasNominalVoltage() || ProtoUtil.isEqual(nominalVoltage, other.nominalVoltage))
+        && (!hasStallTorque() || ProtoUtil.isEqual(stallTorque, other.stallTorque))
+        && (!hasStallCurrent() || ProtoUtil.isEqual(stallCurrent, other.stallCurrent))
+        && (!hasFreeCurrent() || ProtoUtil.isEqual(freeCurrent, other.freeCurrent))
+        && (!hasFreeSpeed() || ProtoUtil.isEqual(freeSpeed, other.freeSpeed));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(nominalVoltage);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(stallTorque);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(stallCurrent);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 33);
+        output.writeDoubleNoTag(freeCurrent);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeRawByte((byte) 41);
+        output.writeDoubleNoTag(freeSpeed);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        size += 9;
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufDCMotor mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // nominalVoltage
+            nominalVoltage = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // stallTorque
+            stallTorque = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // stallCurrent
+            stallCurrent = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 33) {
+              break;
+            }
+          }
+          case 33: {
+            // freeCurrent
+            freeCurrent = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 41) {
+              break;
+            }
+          }
+          case 41: {
+            // freeSpeed
+            freeSpeed = input.readDouble();
+            bitField0_ |= 0x00000010;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.nominalVoltage, nominalVoltage);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.stallTorque, stallTorque);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.stallCurrent, stallCurrent);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.freeCurrent, freeCurrent);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeDouble(FieldNames.freeSpeed, freeSpeed);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufDCMotor mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 1374862050:
+          case 173092603: {
+            if (input.isAtField(FieldNames.nominalVoltage)) {
+              if (!input.trySkipNullValue()) {
+                nominalVoltage = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 2075810250:
+          case 1238615945: {
+            if (input.isAtField(FieldNames.stallTorque)) {
+              if (!input.trySkipNullValue()) {
+                stallTorque = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -2105262663:
+          case 2006484954: {
+            if (input.isAtField(FieldNames.stallCurrent)) {
+              if (!input.trySkipNullValue()) {
+                stallCurrent = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 1024355693:
+          case 240406182: {
+            if (input.isAtField(FieldNames.freeCurrent)) {
+              if (!input.trySkipNullValue()) {
+                freeCurrent = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -444654277:
+          case -552732492: {
+            if (input.isAtField(FieldNames.freeSpeed)) {
+              if (!input.trySkipNullValue()) {
+                freeSpeed = input.readDouble();
+                bitField0_ |= 0x00000010;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufDCMotor clone() {
+      return new ProtobufDCMotor().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufDCMotor parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufDCMotor(), data).checkInitialized();
+    }
+
+    public static ProtobufDCMotor parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized();
+    }
+
+    public static ProtobufDCMotor parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufDCMotor messages
+     */
+    public static MessageFactory<ProtobufDCMotor> getFactory() {
+      return ProtobufDCMotorFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Plant.wpi_proto_ProtobufDCMotor_descriptor;
+    }
+
+    private enum ProtobufDCMotorFactory implements MessageFactory<ProtobufDCMotor> {
+      INSTANCE;
+
+      @Override
+      public ProtobufDCMotor create() {
+        return ProtobufDCMotor.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName nominalVoltage = FieldName.forField("nominalVoltage", "nominal_voltage");
+
+      static final FieldName stallTorque = FieldName.forField("stallTorque", "stall_torque");
+
+      static final FieldName stallCurrent = FieldName.forField("stallCurrent", "stall_current");
+
+      static final FieldName freeCurrent = FieldName.forField("freeCurrent", "free_current");
+
+      static final FieldName freeSpeed = FieldName.forField("freeSpeed", "free_speed");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Spline.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Spline.java
new file mode 100644
index 0000000..accdecb
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Spline.java
@@ -0,0 +1,1367 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+import us.hebi.quickbuf.RepeatedDouble;
+
+public final class Spline {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(993,
+    "CgxzcGxpbmUucHJvdG8SCXdwaS5wcm90byKIAQoaUHJvdG9idWZDdWJpY0hlcm1pdGVTcGxpbmUSGwoJ" + 
+    "eF9pbml0aWFsGAEgAygBUgh4SW5pdGlhbBIXCgd4X2ZpbmFsGAIgAygBUgZ4RmluYWwSGwoJeV9pbml0" + 
+    "aWFsGAMgAygBUgh5SW5pdGlhbBIXCgd5X2ZpbmFsGAQgAygBUgZ5RmluYWwiigEKHFByb3RvYnVmUXVp" + 
+    "bnRpY0hlcm1pdGVTcGxpbmUSGwoJeF9pbml0aWFsGAEgAygBUgh4SW5pdGlhbBIXCgd4X2ZpbmFsGAIg" + 
+    "AygBUgZ4RmluYWwSGwoJeV9pbml0aWFsGAMgAygBUgh5SW5pdGlhbBIXCgd5X2ZpbmFsGAQgAygBUgZ5" + 
+    "RmluYWxCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3RvSokFCgYSBAAAEgEKCAoBDBIDAAASCggKAQIS" + 
+    "AwIAEgoICgEIEgMEADEKCQoCCAESAwQAMQoKCgIEABIEBgALAQoKCgMEAAESAwYIIgoLCgQEAAIAEgMH" + 
+    "AiAKDAoFBAACAAQSAwcCCgoMCgUEAAIABRIDBwsRCgwKBQQAAgABEgMHEhsKDAoFBAACAAMSAwceHwoL" + 
+    "CgQEAAIBEgMIAh4KDAoFBAACAQQSAwgCCgoMCgUEAAIBBRIDCAsRCgwKBQQAAgEBEgMIEhkKDAoFBAAC" + 
+    "AQMSAwgcHQoLCgQEAAICEgMJAiAKDAoFBAACAgQSAwkCCgoMCgUEAAICBRIDCQsRCgwKBQQAAgIBEgMJ" + 
+    "EhsKDAoFBAACAgMSAwkeHwoLCgQEAAIDEgMKAh4KDAoFBAACAwQSAwoCCgoMCgUEAAIDBRIDCgsRCgwK" + 
+    "BQQAAgMBEgMKEhkKDAoFBAACAwMSAwocHQoKCgIEARIEDQASAQoKCgMEAQESAw0IJAoLCgQEAQIAEgMO" + 
+    "AiAKDAoFBAECAAQSAw4CCgoMCgUEAQIABRIDDgsRCgwKBQQBAgABEgMOEhsKDAoFBAECAAMSAw4eHwoL" + 
+    "CgQEAQIBEgMPAh4KDAoFBAECAQQSAw8CCgoMCgUEAQIBBRIDDwsRCgwKBQQBAgEBEgMPEhkKDAoFBAEC" + 
+    "AQMSAw8cHQoLCgQEAQICEgMQAiAKDAoFBAECAgQSAxACCgoMCgUEAQICBRIDEAsRCgwKBQQBAgIBEgMQ" + 
+    "EhsKDAoFBAECAgMSAxAeHwoLCgQEAQIDEgMRAh4KDAoFBAECAwQSAxECCgoMCgUEAQIDBRIDEQsRCgwK" + 
+    "BQQBAgMBEgMREhkKDAoFBAECAwMSAxEcHWIGcHJvdG8z");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("spline.proto", "wpi.proto", descriptorData);
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufCubicHermiteSpline_descriptor = descriptor.internalContainedType(28, 136, "ProtobufCubicHermiteSpline", "wpi.proto.ProtobufCubicHermiteSpline");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufQuinticHermiteSpline_descriptor = descriptor.internalContainedType(167, 138, "ProtobufQuinticHermiteSpline", "wpi.proto.ProtobufQuinticHermiteSpline");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufCubicHermiteSpline}
+   */
+  public static final class ProtobufCubicHermiteSpline extends ProtoMessage<ProtobufCubicHermiteSpline> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     */
+    private final RepeatedDouble xInitial = RepeatedDouble.newEmptyInstance();
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     */
+    private final RepeatedDouble xFinal = RepeatedDouble.newEmptyInstance();
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     */
+    private final RepeatedDouble yInitial = RepeatedDouble.newEmptyInstance();
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     */
+    private final RepeatedDouble yFinal = RepeatedDouble.newEmptyInstance();
+
+    private ProtobufCubicHermiteSpline() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufCubicHermiteSpline}
+     */
+    public static ProtobufCubicHermiteSpline newInstance() {
+      return new ProtobufCubicHermiteSpline();
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @return whether the xInitial field is set
+     */
+    public boolean hasXInitial() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline clearXInitial() {
+      bitField0_ &= ~0x00000001;
+      xInitial.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableXInitial()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getXInitial() {
+      return xInitial;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableXInitial() {
+      bitField0_ |= 0x00000001;
+      return xInitial;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @param value the xInitial to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addXInitial(final double value) {
+      bitField0_ |= 0x00000001;
+      xInitial.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @param values the xInitial to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addAllXInitial(final double... values) {
+      bitField0_ |= 0x00000001;
+      xInitial.addAll(values);
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @return whether the xFinal field is set
+     */
+    public boolean hasXFinal() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline clearXFinal() {
+      bitField0_ &= ~0x00000002;
+      xFinal.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableXFinal()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getXFinal() {
+      return xFinal;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableXFinal() {
+      bitField0_ |= 0x00000002;
+      return xFinal;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @param value the xFinal to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addXFinal(final double value) {
+      bitField0_ |= 0x00000002;
+      xFinal.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @param values the xFinal to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addAllXFinal(final double... values) {
+      bitField0_ |= 0x00000002;
+      xFinal.addAll(values);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @return whether the yInitial field is set
+     */
+    public boolean hasYInitial() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline clearYInitial() {
+      bitField0_ &= ~0x00000004;
+      yInitial.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableYInitial()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getYInitial() {
+      return yInitial;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableYInitial() {
+      bitField0_ |= 0x00000004;
+      return yInitial;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @param value the yInitial to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addYInitial(final double value) {
+      bitField0_ |= 0x00000004;
+      yInitial.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @param values the yInitial to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addAllYInitial(final double... values) {
+      bitField0_ |= 0x00000004;
+      yInitial.addAll(values);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @return whether the yFinal field is set
+     */
+    public boolean hasYFinal() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline clearYFinal() {
+      bitField0_ &= ~0x00000008;
+      yFinal.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableYFinal()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getYFinal() {
+      return yFinal;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableYFinal() {
+      bitField0_ |= 0x00000008;
+      return yFinal;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @param value the yFinal to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addYFinal(final double value) {
+      bitField0_ |= 0x00000008;
+      yFinal.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @param values the yFinal to add
+     * @return this
+     */
+    public ProtobufCubicHermiteSpline addAllYFinal(final double... values) {
+      bitField0_ |= 0x00000008;
+      yFinal.addAll(values);
+      return this;
+    }
+
+    @Override
+    public ProtobufCubicHermiteSpline copyFrom(final ProtobufCubicHermiteSpline other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        xInitial.copyFrom(other.xInitial);
+        xFinal.copyFrom(other.xFinal);
+        yInitial.copyFrom(other.yInitial);
+        yFinal.copyFrom(other.yFinal);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufCubicHermiteSpline mergeFrom(final ProtobufCubicHermiteSpline other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasXInitial()) {
+        getMutableXInitial().addAll(other.xInitial);
+      }
+      if (other.hasXFinal()) {
+        getMutableXFinal().addAll(other.xFinal);
+      }
+      if (other.hasYInitial()) {
+        getMutableYInitial().addAll(other.yInitial);
+      }
+      if (other.hasYFinal()) {
+        getMutableYFinal().addAll(other.yFinal);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufCubicHermiteSpline clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      xInitial.clear();
+      xFinal.clear();
+      yInitial.clear();
+      yFinal.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufCubicHermiteSpline clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      xInitial.clear();
+      xFinal.clear();
+      yInitial.clear();
+      yFinal.clear();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufCubicHermiteSpline)) {
+        return false;
+      }
+      ProtobufCubicHermiteSpline other = (ProtobufCubicHermiteSpline) o;
+      return bitField0_ == other.bitField0_
+        && (!hasXInitial() || xInitial.equals(other.xInitial))
+        && (!hasXFinal() || xFinal.equals(other.xFinal))
+        && (!hasYInitial() || yInitial.equals(other.yInitial))
+        && (!hasYFinal() || yFinal.equals(other.yFinal));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        for (int i = 0; i < xInitial.length(); i++) {
+          output.writeRawByte((byte) 9);
+          output.writeDoubleNoTag(xInitial.array()[i]);
+        }
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        for (int i = 0; i < xFinal.length(); i++) {
+          output.writeRawByte((byte) 17);
+          output.writeDoubleNoTag(xFinal.array()[i]);
+        }
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        for (int i = 0; i < yInitial.length(); i++) {
+          output.writeRawByte((byte) 25);
+          output.writeDoubleNoTag(yInitial.array()[i]);
+        }
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        for (int i = 0; i < yFinal.length(); i++) {
+          output.writeRawByte((byte) 33);
+          output.writeDoubleNoTag(yFinal.array()[i]);
+        }
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += (1 + 8) * xInitial.length();
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += (1 + 8) * xFinal.length();
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += (1 + 8) * yInitial.length();
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += (1 + 8) * yFinal.length();
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufCubicHermiteSpline mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // xInitial [packed=true]
+            input.readPackedDouble(xInitial);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // xFinal [packed=true]
+            input.readPackedDouble(xFinal);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 26) {
+              break;
+            }
+          }
+          case 26: {
+            // yInitial [packed=true]
+            input.readPackedDouble(yInitial);
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 34) {
+              break;
+            }
+          }
+          case 34: {
+            // yFinal [packed=true]
+            input.readPackedDouble(yFinal);
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+          case 9: {
+            // xInitial [packed=false]
+            tag = input.readRepeatedDouble(xInitial, tag);
+            bitField0_ |= 0x00000001;
+            break;
+          }
+          case 17: {
+            // xFinal [packed=false]
+            tag = input.readRepeatedDouble(xFinal, tag);
+            bitField0_ |= 0x00000002;
+            break;
+          }
+          case 25: {
+            // yInitial [packed=false]
+            tag = input.readRepeatedDouble(yInitial, tag);
+            bitField0_ |= 0x00000004;
+            break;
+          }
+          case 33: {
+            // yFinal [packed=false]
+            tag = input.readRepeatedDouble(yFinal, tag);
+            bitField0_ |= 0x00000008;
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRepeatedDouble(FieldNames.xInitial, xInitial);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRepeatedDouble(FieldNames.xFinal, xFinal);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRepeatedDouble(FieldNames.yInitial, yInitial);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRepeatedDouble(FieldNames.yFinal, yFinal);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufCubicHermiteSpline mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -1998129236:
+          case -2134571395: {
+            if (input.isAtField(FieldNames.xInitial)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(xInitial);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -791585826:
+          case 1943464687: {
+            if (input.isAtField(FieldNames.xFinal)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(xFinal);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -255318901:
+          case 352941438: {
+            if (input.isAtField(FieldNames.yInitial)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(yInitial);
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -762956675:
+          case -1463998928: {
+            if (input.isAtField(FieldNames.yFinal)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(yFinal);
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufCubicHermiteSpline clone() {
+      return new ProtobufCubicHermiteSpline().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufCubicHermiteSpline parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufCubicHermiteSpline(), data).checkInitialized();
+    }
+
+    public static ProtobufCubicHermiteSpline parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufCubicHermiteSpline(), input).checkInitialized();
+    }
+
+    public static ProtobufCubicHermiteSpline parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufCubicHermiteSpline(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufCubicHermiteSpline messages
+     */
+    public static MessageFactory<ProtobufCubicHermiteSpline> getFactory() {
+      return ProtobufCubicHermiteSplineFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Spline.wpi_proto_ProtobufCubicHermiteSpline_descriptor;
+    }
+
+    private enum ProtobufCubicHermiteSplineFactory implements MessageFactory<ProtobufCubicHermiteSpline> {
+      INSTANCE;
+
+      @Override
+      public ProtobufCubicHermiteSpline create() {
+        return ProtobufCubicHermiteSpline.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName xInitial = FieldName.forField("xInitial", "x_initial");
+
+      static final FieldName xFinal = FieldName.forField("xFinal", "x_final");
+
+      static final FieldName yInitial = FieldName.forField("yInitial", "y_initial");
+
+      static final FieldName yFinal = FieldName.forField("yFinal", "y_final");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufQuinticHermiteSpline}
+   */
+  public static final class ProtobufQuinticHermiteSpline extends ProtoMessage<ProtobufQuinticHermiteSpline> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     */
+    private final RepeatedDouble xInitial = RepeatedDouble.newEmptyInstance();
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     */
+    private final RepeatedDouble xFinal = RepeatedDouble.newEmptyInstance();
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     */
+    private final RepeatedDouble yInitial = RepeatedDouble.newEmptyInstance();
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     */
+    private final RepeatedDouble yFinal = RepeatedDouble.newEmptyInstance();
+
+    private ProtobufQuinticHermiteSpline() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufQuinticHermiteSpline}
+     */
+    public static ProtobufQuinticHermiteSpline newInstance() {
+      return new ProtobufQuinticHermiteSpline();
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @return whether the xInitial field is set
+     */
+    public boolean hasXInitial() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline clearXInitial() {
+      bitField0_ &= ~0x00000001;
+      xInitial.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableXInitial()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getXInitial() {
+      return xInitial;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableXInitial() {
+      bitField0_ |= 0x00000001;
+      return xInitial;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @param value the xInitial to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addXInitial(final double value) {
+      bitField0_ |= 0x00000001;
+      xInitial.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_initial = 1;</code>
+     * @param values the xInitial to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addAllXInitial(final double... values) {
+      bitField0_ |= 0x00000001;
+      xInitial.addAll(values);
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @return whether the xFinal field is set
+     */
+    public boolean hasXFinal() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline clearXFinal() {
+      bitField0_ &= ~0x00000002;
+      xFinal.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableXFinal()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getXFinal() {
+      return xFinal;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableXFinal() {
+      bitField0_ |= 0x00000002;
+      return xFinal;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @param value the xFinal to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addXFinal(final double value) {
+      bitField0_ |= 0x00000002;
+      xFinal.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double x_final = 2;</code>
+     * @param values the xFinal to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addAllXFinal(final double... values) {
+      bitField0_ |= 0x00000002;
+      xFinal.addAll(values);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @return whether the yInitial field is set
+     */
+    public boolean hasYInitial() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline clearYInitial() {
+      bitField0_ &= ~0x00000004;
+      yInitial.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableYInitial()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getYInitial() {
+      return yInitial;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableYInitial() {
+      bitField0_ |= 0x00000004;
+      return yInitial;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @param value the yInitial to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addYInitial(final double value) {
+      bitField0_ |= 0x00000004;
+      yInitial.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_initial = 3;</code>
+     * @param values the yInitial to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addAllYInitial(final double... values) {
+      bitField0_ |= 0x00000004;
+      yInitial.addAll(values);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @return whether the yFinal field is set
+     */
+    public boolean hasYFinal() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline clearYFinal() {
+      bitField0_ &= ~0x00000008;
+      yFinal.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableYFinal()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getYFinal() {
+      return yFinal;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableYFinal() {
+      bitField0_ |= 0x00000008;
+      return yFinal;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @param value the yFinal to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addYFinal(final double value) {
+      bitField0_ |= 0x00000008;
+      yFinal.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double y_final = 4;</code>
+     * @param values the yFinal to add
+     * @return this
+     */
+    public ProtobufQuinticHermiteSpline addAllYFinal(final double... values) {
+      bitField0_ |= 0x00000008;
+      yFinal.addAll(values);
+      return this;
+    }
+
+    @Override
+    public ProtobufQuinticHermiteSpline copyFrom(final ProtobufQuinticHermiteSpline other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        xInitial.copyFrom(other.xInitial);
+        xFinal.copyFrom(other.xFinal);
+        yInitial.copyFrom(other.yInitial);
+        yFinal.copyFrom(other.yFinal);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufQuinticHermiteSpline mergeFrom(final ProtobufQuinticHermiteSpline other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasXInitial()) {
+        getMutableXInitial().addAll(other.xInitial);
+      }
+      if (other.hasXFinal()) {
+        getMutableXFinal().addAll(other.xFinal);
+      }
+      if (other.hasYInitial()) {
+        getMutableYInitial().addAll(other.yInitial);
+      }
+      if (other.hasYFinal()) {
+        getMutableYFinal().addAll(other.yFinal);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufQuinticHermiteSpline clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      xInitial.clear();
+      xFinal.clear();
+      yInitial.clear();
+      yFinal.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufQuinticHermiteSpline clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      xInitial.clear();
+      xFinal.clear();
+      yInitial.clear();
+      yFinal.clear();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufQuinticHermiteSpline)) {
+        return false;
+      }
+      ProtobufQuinticHermiteSpline other = (ProtobufQuinticHermiteSpline) o;
+      return bitField0_ == other.bitField0_
+        && (!hasXInitial() || xInitial.equals(other.xInitial))
+        && (!hasXFinal() || xFinal.equals(other.xFinal))
+        && (!hasYInitial() || yInitial.equals(other.yInitial))
+        && (!hasYFinal() || yFinal.equals(other.yFinal));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        for (int i = 0; i < xInitial.length(); i++) {
+          output.writeRawByte((byte) 9);
+          output.writeDoubleNoTag(xInitial.array()[i]);
+        }
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        for (int i = 0; i < xFinal.length(); i++) {
+          output.writeRawByte((byte) 17);
+          output.writeDoubleNoTag(xFinal.array()[i]);
+        }
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        for (int i = 0; i < yInitial.length(); i++) {
+          output.writeRawByte((byte) 25);
+          output.writeDoubleNoTag(yInitial.array()[i]);
+        }
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        for (int i = 0; i < yFinal.length(); i++) {
+          output.writeRawByte((byte) 33);
+          output.writeDoubleNoTag(yFinal.array()[i]);
+        }
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += (1 + 8) * xInitial.length();
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += (1 + 8) * xFinal.length();
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += (1 + 8) * yInitial.length();
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += (1 + 8) * yFinal.length();
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufQuinticHermiteSpline mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // xInitial [packed=true]
+            input.readPackedDouble(xInitial);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 18) {
+              break;
+            }
+          }
+          case 18: {
+            // xFinal [packed=true]
+            input.readPackedDouble(xFinal);
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 26) {
+              break;
+            }
+          }
+          case 26: {
+            // yInitial [packed=true]
+            input.readPackedDouble(yInitial);
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 34) {
+              break;
+            }
+          }
+          case 34: {
+            // yFinal [packed=true]
+            input.readPackedDouble(yFinal);
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+          case 9: {
+            // xInitial [packed=false]
+            tag = input.readRepeatedDouble(xInitial, tag);
+            bitField0_ |= 0x00000001;
+            break;
+          }
+          case 17: {
+            // xFinal [packed=false]
+            tag = input.readRepeatedDouble(xFinal, tag);
+            bitField0_ |= 0x00000002;
+            break;
+          }
+          case 25: {
+            // yInitial [packed=false]
+            tag = input.readRepeatedDouble(yInitial, tag);
+            bitField0_ |= 0x00000004;
+            break;
+          }
+          case 33: {
+            // yFinal [packed=false]
+            tag = input.readRepeatedDouble(yFinal, tag);
+            bitField0_ |= 0x00000008;
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRepeatedDouble(FieldNames.xInitial, xInitial);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRepeatedDouble(FieldNames.xFinal, xFinal);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRepeatedDouble(FieldNames.yInitial, yInitial);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRepeatedDouble(FieldNames.yFinal, yFinal);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufQuinticHermiteSpline mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -1998129236:
+          case -2134571395: {
+            if (input.isAtField(FieldNames.xInitial)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(xInitial);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -791585826:
+          case 1943464687: {
+            if (input.isAtField(FieldNames.xFinal)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(xFinal);
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -255318901:
+          case 352941438: {
+            if (input.isAtField(FieldNames.yInitial)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(yInitial);
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -762956675:
+          case -1463998928: {
+            if (input.isAtField(FieldNames.yFinal)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(yFinal);
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufQuinticHermiteSpline clone() {
+      return new ProtobufQuinticHermiteSpline().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufQuinticHermiteSpline parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufQuinticHermiteSpline(), data).checkInitialized();
+    }
+
+    public static ProtobufQuinticHermiteSpline parseFrom(final ProtoSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufQuinticHermiteSpline(), input).checkInitialized();
+    }
+
+    public static ProtobufQuinticHermiteSpline parseFrom(final JsonSource input) throws
+        IOException {
+      return ProtoMessage.mergeFrom(new ProtobufQuinticHermiteSpline(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufQuinticHermiteSpline messages
+     */
+    public static MessageFactory<ProtobufQuinticHermiteSpline> getFactory() {
+      return ProtobufQuinticHermiteSplineFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Spline.wpi_proto_ProtobufQuinticHermiteSpline_descriptor;
+    }
+
+    private enum ProtobufQuinticHermiteSplineFactory implements MessageFactory<ProtobufQuinticHermiteSpline> {
+      INSTANCE;
+
+      @Override
+      public ProtobufQuinticHermiteSpline create() {
+        return ProtobufQuinticHermiteSpline.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName xInitial = FieldName.forField("xInitial", "x_initial");
+
+      static final FieldName xFinal = FieldName.forField("xFinal", "x_final");
+
+      static final FieldName yInitial = FieldName.forField("yInitial", "y_initial");
+
+      static final FieldName yFinal = FieldName.forField("yFinal", "y_final");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/System.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/System.java
new file mode 100644
index 0000000..09ea3b5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/System.java
@@ -0,0 +1,866 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+
+public final class System {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(832,
+    "CgxzeXN0ZW0ucHJvdG8SCXdwaS5wcm90bxoNd3BpbWF0aC5wcm90byKZAgoUUHJvdG9idWZMaW5lYXJT" + 
+    "eXN0ZW0SHQoKbnVtX3N0YXRlcxgBIAEoDVIJbnVtU3RhdGVzEh0KCm51bV9pbnB1dHMYAiABKA1SCW51" + 
+    "bUlucHV0cxIfCgtudW1fb3V0cHV0cxgDIAEoDVIKbnVtT3V0cHV0cxInCgFhGAQgASgLMhkud3BpLnBy" + 
+    "b3RvLlByb3RvYnVmTWF0cml4UgFhEicKAWIYBSABKAsyGS53cGkucHJvdG8uUHJvdG9idWZNYXRyaXhS" + 
+    "AWISJwoBYxgGIAEoCzIZLndwaS5wcm90by5Qcm90b2J1Zk1hdHJpeFIBYxInCgFkGAcgASgLMhkud3Bp" + 
+    "LnByb3RvLlByb3RvYnVmTWF0cml4UgFkQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rVAwoGEgQA" + 
+    "ABABCggKAQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAFwoICgEIEgMGADEKCQoCCAESAwYAMQoKCgIE" + 
+    "ABIECAAQAQoKCgMEAAESAwgIHAoLCgQEAAIAEgMJAhgKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkT" + 
+    "CgwKBQQAAgADEgMJFhcKCwoEBAACARIDCgIYCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJEwoMCgUE" + 
+    "AAIBAxIDChYXCgsKBAQAAgISAwsCGQoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCRQKDAoFBAACAgMS" + 
+    "AwsXGAoLCgQEAAIDEgMMAhcKDAoFBAACAwYSAwwCEAoMCgUEAAIDARIDDBESCgwKBQQAAgMDEgMMFRYK" + 
+    "CwoEBAACBBIDDQIXCgwKBQQAAgQGEgMNAhAKDAoFBAACBAESAw0REgoMCgUEAAIEAxIDDRUWCgsKBAQA" + 
+    "AgUSAw4CFwoMCgUEAAIFBhIDDgIQCgwKBQQAAgUBEgMOERIKDAoFBAACBQMSAw4VFgoLCgQEAAIGEgMP" + 
+    "AhcKDAoFBAACBgYSAw8CEAoMCgUEAAIGARIDDxESCgwKBQQAAgYDEgMPFRZiBnByb3RvMw==");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("system.proto", "wpi.proto", descriptorData, Wpimath.getDescriptor());
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufLinearSystem_descriptor = descriptor.internalContainedType(43, 281, "ProtobufLinearSystem", "wpi.proto.ProtobufLinearSystem");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufLinearSystem}
+   */
+  public static final class ProtobufLinearSystem extends ProtoMessage<ProtobufLinearSystem> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional uint32 num_states = 1;</code>
+     */
+    private int numStates;
+
+    /**
+     * <code>optional uint32 num_inputs = 2;</code>
+     */
+    private int numInputs;
+
+    /**
+     * <code>optional uint32 num_outputs = 3;</code>
+     */
+    private int numOutputs;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix a = 4;</code>
+     */
+    private final Wpimath.ProtobufMatrix a = Wpimath.ProtobufMatrix.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix b = 5;</code>
+     */
+    private final Wpimath.ProtobufMatrix b = Wpimath.ProtobufMatrix.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix c = 6;</code>
+     */
+    private final Wpimath.ProtobufMatrix c = Wpimath.ProtobufMatrix.newInstance();
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix d = 7;</code>
+     */
+    private final Wpimath.ProtobufMatrix d = Wpimath.ProtobufMatrix.newInstance();
+
+    private ProtobufLinearSystem() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufLinearSystem}
+     */
+    public static ProtobufLinearSystem newInstance() {
+      return new ProtobufLinearSystem();
+    }
+
+    /**
+     * <code>optional uint32 num_states = 1;</code>
+     * @return whether the numStates field is set
+     */
+    public boolean hasNumStates() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional uint32 num_states = 1;</code>
+     * @return this
+     */
+    public ProtobufLinearSystem clearNumStates() {
+      bitField0_ &= ~0x00000001;
+      numStates = 0;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_states = 1;</code>
+     * @return the numStates
+     */
+    public int getNumStates() {
+      return numStates;
+    }
+
+    /**
+     * <code>optional uint32 num_states = 1;</code>
+     * @param value the numStates to set
+     * @return this
+     */
+    public ProtobufLinearSystem setNumStates(final int value) {
+      bitField0_ |= 0x00000001;
+      numStates = value;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_inputs = 2;</code>
+     * @return whether the numInputs field is set
+     */
+    public boolean hasNumInputs() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional uint32 num_inputs = 2;</code>
+     * @return this
+     */
+    public ProtobufLinearSystem clearNumInputs() {
+      bitField0_ &= ~0x00000002;
+      numInputs = 0;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_inputs = 2;</code>
+     * @return the numInputs
+     */
+    public int getNumInputs() {
+      return numInputs;
+    }
+
+    /**
+     * <code>optional uint32 num_inputs = 2;</code>
+     * @param value the numInputs to set
+     * @return this
+     */
+    public ProtobufLinearSystem setNumInputs(final int value) {
+      bitField0_ |= 0x00000002;
+      numInputs = value;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_outputs = 3;</code>
+     * @return whether the numOutputs field is set
+     */
+    public boolean hasNumOutputs() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional uint32 num_outputs = 3;</code>
+     * @return this
+     */
+    public ProtobufLinearSystem clearNumOutputs() {
+      bitField0_ &= ~0x00000004;
+      numOutputs = 0;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_outputs = 3;</code>
+     * @return the numOutputs
+     */
+    public int getNumOutputs() {
+      return numOutputs;
+    }
+
+    /**
+     * <code>optional uint32 num_outputs = 3;</code>
+     * @param value the numOutputs to set
+     * @return this
+     */
+    public ProtobufLinearSystem setNumOutputs(final int value) {
+      bitField0_ |= 0x00000004;
+      numOutputs = value;
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix a = 4;</code>
+     * @return whether the a field is set
+     */
+    public boolean hasA() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix a = 4;</code>
+     * @return this
+     */
+    public ProtobufLinearSystem clearA() {
+      bitField0_ &= ~0x00000008;
+      a.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix a = 4;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableA()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Wpimath.ProtobufMatrix getA() {
+      return a;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix a = 4;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Wpimath.ProtobufMatrix getMutableA() {
+      bitField0_ |= 0x00000008;
+      return a;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix a = 4;</code>
+     * @param value the a to set
+     * @return this
+     */
+    public ProtobufLinearSystem setA(final Wpimath.ProtobufMatrix value) {
+      bitField0_ |= 0x00000008;
+      a.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix b = 5;</code>
+     * @return whether the b field is set
+     */
+    public boolean hasB() {
+      return (bitField0_ & 0x00000010) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix b = 5;</code>
+     * @return this
+     */
+    public ProtobufLinearSystem clearB() {
+      bitField0_ &= ~0x00000010;
+      b.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix b = 5;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableB()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Wpimath.ProtobufMatrix getB() {
+      return b;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix b = 5;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Wpimath.ProtobufMatrix getMutableB() {
+      bitField0_ |= 0x00000010;
+      return b;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix b = 5;</code>
+     * @param value the b to set
+     * @return this
+     */
+    public ProtobufLinearSystem setB(final Wpimath.ProtobufMatrix value) {
+      bitField0_ |= 0x00000010;
+      b.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix c = 6;</code>
+     * @return whether the c field is set
+     */
+    public boolean hasC() {
+      return (bitField0_ & 0x00000020) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix c = 6;</code>
+     * @return this
+     */
+    public ProtobufLinearSystem clearC() {
+      bitField0_ &= ~0x00000020;
+      c.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix c = 6;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableC()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Wpimath.ProtobufMatrix getC() {
+      return c;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix c = 6;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Wpimath.ProtobufMatrix getMutableC() {
+      bitField0_ |= 0x00000020;
+      return c;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix c = 6;</code>
+     * @param value the c to set
+     * @return this
+     */
+    public ProtobufLinearSystem setC(final Wpimath.ProtobufMatrix value) {
+      bitField0_ |= 0x00000020;
+      c.copyFrom(value);
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix d = 7;</code>
+     * @return whether the d field is set
+     */
+    public boolean hasD() {
+      return (bitField0_ & 0x00000040) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix d = 7;</code>
+     * @return this
+     */
+    public ProtobufLinearSystem clearD() {
+      bitField0_ &= ~0x00000040;
+      d.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix d = 7;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableD()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Wpimath.ProtobufMatrix getD() {
+      return d;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix d = 7;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Wpimath.ProtobufMatrix getMutableD() {
+      bitField0_ |= 0x00000040;
+      return d;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufMatrix d = 7;</code>
+     * @param value the d to set
+     * @return this
+     */
+    public ProtobufLinearSystem setD(final Wpimath.ProtobufMatrix value) {
+      bitField0_ |= 0x00000040;
+      d.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufLinearSystem copyFrom(final ProtobufLinearSystem other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        numStates = other.numStates;
+        numInputs = other.numInputs;
+        numOutputs = other.numOutputs;
+        a.copyFrom(other.a);
+        b.copyFrom(other.b);
+        c.copyFrom(other.c);
+        d.copyFrom(other.d);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufLinearSystem mergeFrom(final ProtobufLinearSystem other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasNumStates()) {
+        setNumStates(other.numStates);
+      }
+      if (other.hasNumInputs()) {
+        setNumInputs(other.numInputs);
+      }
+      if (other.hasNumOutputs()) {
+        setNumOutputs(other.numOutputs);
+      }
+      if (other.hasA()) {
+        getMutableA().mergeFrom(other.a);
+      }
+      if (other.hasB()) {
+        getMutableB().mergeFrom(other.b);
+      }
+      if (other.hasC()) {
+        getMutableC().mergeFrom(other.c);
+      }
+      if (other.hasD()) {
+        getMutableD().mergeFrom(other.d);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufLinearSystem clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      numStates = 0;
+      numInputs = 0;
+      numOutputs = 0;
+      a.clear();
+      b.clear();
+      c.clear();
+      d.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufLinearSystem clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      a.clearQuick();
+      b.clearQuick();
+      c.clearQuick();
+      d.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufLinearSystem)) {
+        return false;
+      }
+      ProtobufLinearSystem other = (ProtobufLinearSystem) o;
+      return bitField0_ == other.bitField0_
+        && (!hasNumStates() || numStates == other.numStates)
+        && (!hasNumInputs() || numInputs == other.numInputs)
+        && (!hasNumOutputs() || numOutputs == other.numOutputs)
+        && (!hasA() || a.equals(other.a))
+        && (!hasB() || b.equals(other.b))
+        && (!hasC() || c.equals(other.c))
+        && (!hasD() || d.equals(other.d));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 8);
+        output.writeUInt32NoTag(numStates);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 16);
+        output.writeUInt32NoTag(numInputs);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 24);
+        output.writeUInt32NoTag(numOutputs);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 34);
+        output.writeMessageNoTag(a);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeRawByte((byte) 42);
+        output.writeMessageNoTag(b);
+      }
+      if ((bitField0_ & 0x00000020) != 0) {
+        output.writeRawByte((byte) 50);
+        output.writeMessageNoTag(c);
+      }
+      if ((bitField0_ & 0x00000040) != 0) {
+        output.writeRawByte((byte) 58);
+        output.writeMessageNoTag(d);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeUInt32SizeNoTag(numStates);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeUInt32SizeNoTag(numInputs);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 1 + ProtoSink.computeUInt32SizeNoTag(numOutputs);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(a);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(b);
+      }
+      if ((bitField0_ & 0x00000020) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(c);
+      }
+      if ((bitField0_ & 0x00000040) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(d);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufLinearSystem mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 8: {
+            // numStates
+            numStates = input.readUInt32();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 16) {
+              break;
+            }
+          }
+          case 16: {
+            // numInputs
+            numInputs = input.readUInt32();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 24) {
+              break;
+            }
+          }
+          case 24: {
+            // numOutputs
+            numOutputs = input.readUInt32();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 34) {
+              break;
+            }
+          }
+          case 34: {
+            // a
+            input.readMessage(a);
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 42) {
+              break;
+            }
+          }
+          case 42: {
+            // b
+            input.readMessage(b);
+            bitField0_ |= 0x00000010;
+            tag = input.readTag();
+            if (tag != 50) {
+              break;
+            }
+          }
+          case 50: {
+            // c
+            input.readMessage(c);
+            bitField0_ |= 0x00000020;
+            tag = input.readTag();
+            if (tag != 58) {
+              break;
+            }
+          }
+          case 58: {
+            // d
+            input.readMessage(d);
+            bitField0_ |= 0x00000040;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeUInt32(FieldNames.numStates, numStates);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeUInt32(FieldNames.numInputs, numInputs);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeUInt32(FieldNames.numOutputs, numOutputs);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeMessage(FieldNames.a, a);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeMessage(FieldNames.b, b);
+      }
+      if ((bitField0_ & 0x00000020) != 0) {
+        output.writeMessage(FieldNames.c, c);
+      }
+      if ((bitField0_ & 0x00000040) != 0) {
+        output.writeMessage(FieldNames.d, d);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufLinearSystem mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 1233856808:
+          case 1643330779: {
+            if (input.isAtField(FieldNames.numStates)) {
+              if (!input.trySkipNullValue()) {
+                numStates = input.readUInt32();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 942472463:
+          case 1351946434: {
+            if (input.isAtField(FieldNames.numInputs)) {
+              if (!input.trySkipNullValue()) {
+                numInputs = input.readUInt32();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 385880364:
+          case 194671577: {
+            if (input.isAtField(FieldNames.numOutputs)) {
+              if (!input.trySkipNullValue()) {
+                numOutputs = input.readUInt32();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 97: {
+            if (input.isAtField(FieldNames.a)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(a);
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 98: {
+            if (input.isAtField(FieldNames.b)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(b);
+                bitField0_ |= 0x00000010;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 99: {
+            if (input.isAtField(FieldNames.c)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(c);
+                bitField0_ |= 0x00000020;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 100: {
+            if (input.isAtField(FieldNames.d)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(d);
+                bitField0_ |= 0x00000040;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufLinearSystem clone() {
+      return new ProtobufLinearSystem().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufLinearSystem parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufLinearSystem(), data).checkInitialized();
+    }
+
+    public static ProtobufLinearSystem parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufLinearSystem(), input).checkInitialized();
+    }
+
+    public static ProtobufLinearSystem parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufLinearSystem(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufLinearSystem messages
+     */
+    public static MessageFactory<ProtobufLinearSystem> getFactory() {
+      return ProtobufLinearSystemFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return System.wpi_proto_ProtobufLinearSystem_descriptor;
+    }
+
+    private enum ProtobufLinearSystemFactory implements MessageFactory<ProtobufLinearSystem> {
+      INSTANCE;
+
+      @Override
+      public ProtobufLinearSystem create() {
+        return ProtobufLinearSystem.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName numStates = FieldName.forField("numStates", "num_states");
+
+      static final FieldName numInputs = FieldName.forField("numInputs", "num_inputs");
+
+      static final FieldName numOutputs = FieldName.forField("numOutputs", "num_outputs");
+
+      static final FieldName a = FieldName.forField("a");
+
+      static final FieldName b = FieldName.forField("b");
+
+      static final FieldName c = FieldName.forField("c");
+
+      static final FieldName d = FieldName.forField("d");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Trajectory.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Trajectory.java
new file mode 100644
index 0000000..3ec96f8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Trajectory.java
@@ -0,0 +1,928 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+import us.hebi.quickbuf.RepeatedMessage;
+
+public final class Trajectory {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(809,
+    "ChB0cmFqZWN0b3J5LnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iugEKF1Byb3RvYnVm" + 
+    "VHJhamVjdG9yeVN0YXRlEhIKBHRpbWUYASABKAFSBHRpbWUSGgoIdmVsb2NpdHkYAiABKAFSCHZlbG9j" + 
+    "aXR5EiIKDGFjY2VsZXJhdGlvbhgDIAEoAVIMYWNjZWxlcmF0aW9uEi0KBHBvc2UYBCABKAsyGS53cGku" + 
+    "cHJvdG8uUHJvdG9idWZQb3NlMmRSBHBvc2USHAoJY3VydmF0dXJlGAUgASgBUgljdXJ2YXR1cmUiUAoS" + 
+    "UHJvdG9idWZUcmFqZWN0b3J5EjoKBnN0YXRlcxgCIAMoCzIiLndwaS5wcm90by5Qcm90b2J1ZlRyYWpl" + 
+    "Y3RvcnlTdGF0ZVIGc3RhdGVzQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rEAwoGEgQAABIBCggK" + 
+    "AQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAESAwYAMQoKCgIEABIECAAO" + 
+    "AQoKCgMEAAESAwgIHwoLCgQEAAIAEgMJAhIKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkNCgwKBQQA" + 
+    "AgADEgMJEBEKCwoEBAACARIDCgIWCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJEQoMCgUEAAIBAxID" + 
+    "ChQVCgsKBAQAAgISAwsCGgoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCRUKDAoFBAACAgMSAwsYGQoL" + 
+    "CgQEAAIDEgMMAhoKDAoFBAACAwYSAwwCEAoMCgUEAAIDARIDDBEVCgwKBQQAAgMDEgMMGBkKCwoEBAAC" + 
+    "BBIDDQIXCgwKBQQAAgQFEgMNAggKDAoFBAACBAESAw0JEgoMCgUEAAIEAxIDDRUWCgoKAgQBEgQQABIB" + 
+    "CgoKAwQBARIDEAgaCgsKBAQBAgASAxECLgoMCgUEAQIABBIDEQIKCgwKBQQBAgAGEgMRCyIKDAoFBAEC" + 
+    "AAESAxEjKQoMCgUEAQIAAxIDESwtYgZwcm90bzM=");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("trajectory.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor());
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTrajectoryState_descriptor = descriptor.internalContainedType(50, 186, "ProtobufTrajectoryState", "wpi.proto.ProtobufTrajectoryState");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufTrajectory_descriptor = descriptor.internalContainedType(238, 80, "ProtobufTrajectory", "wpi.proto.ProtobufTrajectory");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTrajectoryState}
+   */
+  public static final class ProtobufTrajectoryState extends ProtoMessage<ProtobufTrajectoryState> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double time = 1;</code>
+     */
+    private double time;
+
+    /**
+     * <code>optional double velocity = 2;</code>
+     */
+    private double velocity;
+
+    /**
+     * <code>optional double acceleration = 3;</code>
+     */
+    private double acceleration;
+
+    /**
+     * <code>optional double curvature = 5;</code>
+     */
+    private double curvature;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d pose = 4;</code>
+     */
+    private final Geometry2D.ProtobufPose2d pose = Geometry2D.ProtobufPose2d.newInstance();
+
+    private ProtobufTrajectoryState() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTrajectoryState}
+     */
+    public static ProtobufTrajectoryState newInstance() {
+      return new ProtobufTrajectoryState();
+    }
+
+    /**
+     * <code>optional double time = 1;</code>
+     * @return whether the time field is set
+     */
+    public boolean hasTime() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double time = 1;</code>
+     * @return this
+     */
+    public ProtobufTrajectoryState clearTime() {
+      bitField0_ &= ~0x00000001;
+      time = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double time = 1;</code>
+     * @return the time
+     */
+    public double getTime() {
+      return time;
+    }
+
+    /**
+     * <code>optional double time = 1;</code>
+     * @param value the time to set
+     * @return this
+     */
+    public ProtobufTrajectoryState setTime(final double value) {
+      bitField0_ |= 0x00000001;
+      time = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double velocity = 2;</code>
+     * @return whether the velocity field is set
+     */
+    public boolean hasVelocity() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double velocity = 2;</code>
+     * @return this
+     */
+    public ProtobufTrajectoryState clearVelocity() {
+      bitField0_ &= ~0x00000002;
+      velocity = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double velocity = 2;</code>
+     * @return the velocity
+     */
+    public double getVelocity() {
+      return velocity;
+    }
+
+    /**
+     * <code>optional double velocity = 2;</code>
+     * @param value the velocity to set
+     * @return this
+     */
+    public ProtobufTrajectoryState setVelocity(final double value) {
+      bitField0_ |= 0x00000002;
+      velocity = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double acceleration = 3;</code>
+     * @return whether the acceleration field is set
+     */
+    public boolean hasAcceleration() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional double acceleration = 3;</code>
+     * @return this
+     */
+    public ProtobufTrajectoryState clearAcceleration() {
+      bitField0_ &= ~0x00000004;
+      acceleration = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double acceleration = 3;</code>
+     * @return the acceleration
+     */
+    public double getAcceleration() {
+      return acceleration;
+    }
+
+    /**
+     * <code>optional double acceleration = 3;</code>
+     * @param value the acceleration to set
+     * @return this
+     */
+    public ProtobufTrajectoryState setAcceleration(final double value) {
+      bitField0_ |= 0x00000004;
+      acceleration = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double curvature = 5;</code>
+     * @return whether the curvature field is set
+     */
+    public boolean hasCurvature() {
+      return (bitField0_ & 0x00000008) != 0;
+    }
+
+    /**
+     * <code>optional double curvature = 5;</code>
+     * @return this
+     */
+    public ProtobufTrajectoryState clearCurvature() {
+      bitField0_ &= ~0x00000008;
+      curvature = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double curvature = 5;</code>
+     * @return the curvature
+     */
+    public double getCurvature() {
+      return curvature;
+    }
+
+    /**
+     * <code>optional double curvature = 5;</code>
+     * @param value the curvature to set
+     * @return this
+     */
+    public ProtobufTrajectoryState setCurvature(final double value) {
+      bitField0_ |= 0x00000008;
+      curvature = value;
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d pose = 4;</code>
+     * @return whether the pose field is set
+     */
+    public boolean hasPose() {
+      return (bitField0_ & 0x00000010) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d pose = 4;</code>
+     * @return this
+     */
+    public ProtobufTrajectoryState clearPose() {
+      bitField0_ &= ~0x00000010;
+      pose.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d pose = 4;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutablePose()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public Geometry2D.ProtobufPose2d getPose() {
+      return pose;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d pose = 4;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public Geometry2D.ProtobufPose2d getMutablePose() {
+      bitField0_ |= 0x00000010;
+      return pose;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d pose = 4;</code>
+     * @param value the pose to set
+     * @return this
+     */
+    public ProtobufTrajectoryState setPose(final Geometry2D.ProtobufPose2d value) {
+      bitField0_ |= 0x00000010;
+      pose.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectoryState copyFrom(final ProtobufTrajectoryState other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        time = other.time;
+        velocity = other.velocity;
+        acceleration = other.acceleration;
+        curvature = other.curvature;
+        pose.copyFrom(other.pose);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectoryState mergeFrom(final ProtobufTrajectoryState other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasTime()) {
+        setTime(other.time);
+      }
+      if (other.hasVelocity()) {
+        setVelocity(other.velocity);
+      }
+      if (other.hasAcceleration()) {
+        setAcceleration(other.acceleration);
+      }
+      if (other.hasCurvature()) {
+        setCurvature(other.curvature);
+      }
+      if (other.hasPose()) {
+        getMutablePose().mergeFrom(other.pose);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectoryState clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      time = 0D;
+      velocity = 0D;
+      acceleration = 0D;
+      curvature = 0D;
+      pose.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectoryState clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      pose.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTrajectoryState)) {
+        return false;
+      }
+      ProtobufTrajectoryState other = (ProtobufTrajectoryState) o;
+      return bitField0_ == other.bitField0_
+        && (!hasTime() || ProtoUtil.isEqual(time, other.time))
+        && (!hasVelocity() || ProtoUtil.isEqual(velocity, other.velocity))
+        && (!hasAcceleration() || ProtoUtil.isEqual(acceleration, other.acceleration))
+        && (!hasCurvature() || ProtoUtil.isEqual(curvature, other.curvature))
+        && (!hasPose() || pose.equals(other.pose));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 9);
+        output.writeDoubleNoTag(time);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(velocity);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(acceleration);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeRawByte((byte) 41);
+        output.writeDoubleNoTag(curvature);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeRawByte((byte) 34);
+        output.writeMessageNoTag(pose);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(pose);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTrajectoryState mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 9: {
+            // time
+            time = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 17) {
+              break;
+            }
+          }
+          case 17: {
+            // velocity
+            velocity = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // acceleration
+            acceleration = input.readDouble();
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 41) {
+              break;
+            }
+          }
+          case 41: {
+            // curvature
+            curvature = input.readDouble();
+            bitField0_ |= 0x00000008;
+            tag = input.readTag();
+            if (tag != 34) {
+              break;
+            }
+          }
+          case 34: {
+            // pose
+            input.readMessage(pose);
+            bitField0_ |= 0x00000010;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.time, time);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.velocity, velocity);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeDouble(FieldNames.acceleration, acceleration);
+      }
+      if ((bitField0_ & 0x00000008) != 0) {
+        output.writeDouble(FieldNames.curvature, curvature);
+      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeMessage(FieldNames.pose, pose);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTrajectoryState mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3560141: {
+            if (input.isAtField(FieldNames.time)) {
+              if (!input.trySkipNullValue()) {
+                time = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 2134260957: {
+            if (input.isAtField(FieldNames.velocity)) {
+              if (!input.trySkipNullValue()) {
+                velocity = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -267299712: {
+            if (input.isAtField(FieldNames.acceleration)) {
+              if (!input.trySkipNullValue()) {
+                acceleration = input.readDouble();
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 768611295: {
+            if (input.isAtField(FieldNames.curvature)) {
+              if (!input.trySkipNullValue()) {
+                curvature = input.readDouble();
+                bitField0_ |= 0x00000008;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3446929: {
+            if (input.isAtField(FieldNames.pose)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(pose);
+                bitField0_ |= 0x00000010;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectoryState clone() {
+      return new ProtobufTrajectoryState().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTrajectoryState parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTrajectoryState(), data).checkInitialized();
+    }
+
+    public static ProtobufTrajectoryState parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTrajectoryState(), input).checkInitialized();
+    }
+
+    public static ProtobufTrajectoryState parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTrajectoryState(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTrajectoryState messages
+     */
+    public static MessageFactory<ProtobufTrajectoryState> getFactory() {
+      return ProtobufTrajectoryStateFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Trajectory.wpi_proto_ProtobufTrajectoryState_descriptor;
+    }
+
+    private enum ProtobufTrajectoryStateFactory implements MessageFactory<ProtobufTrajectoryState> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTrajectoryState create() {
+        return ProtobufTrajectoryState.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName time = FieldName.forField("time");
+
+      static final FieldName velocity = FieldName.forField("velocity");
+
+      static final FieldName acceleration = FieldName.forField("acceleration");
+
+      static final FieldName curvature = FieldName.forField("curvature");
+
+      static final FieldName pose = FieldName.forField("pose");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufTrajectory}
+   */
+  public static final class ProtobufTrajectory extends ProtoMessage<ProtobufTrajectory> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTrajectoryState states = 2;</code>
+     */
+    private final RepeatedMessage<ProtobufTrajectoryState> states = RepeatedMessage.newEmptyInstance(ProtobufTrajectoryState.getFactory());
+
+    private ProtobufTrajectory() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufTrajectory}
+     */
+    public static ProtobufTrajectory newInstance() {
+      return new ProtobufTrajectory();
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTrajectoryState states = 2;</code>
+     * @return whether the states field is set
+     */
+    public boolean hasStates() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTrajectoryState states = 2;</code>
+     * @return this
+     */
+    public ProtobufTrajectory clearStates() {
+      bitField0_ &= ~0x00000001;
+      states.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTrajectoryState states = 2;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableStates()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedMessage<ProtobufTrajectoryState> getStates() {
+      return states;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTrajectoryState states = 2;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedMessage<ProtobufTrajectoryState> getMutableStates() {
+      bitField0_ |= 0x00000001;
+      return states;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTrajectoryState states = 2;</code>
+     * @param value the states to add
+     * @return this
+     */
+    public ProtobufTrajectory addStates(final ProtobufTrajectoryState value) {
+      bitField0_ |= 0x00000001;
+      states.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated .wpi.proto.ProtobufTrajectoryState states = 2;</code>
+     * @param values the states to add
+     * @return this
+     */
+    public ProtobufTrajectory addAllStates(final ProtobufTrajectoryState... values) {
+      bitField0_ |= 0x00000001;
+      states.addAll(values);
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectory copyFrom(final ProtobufTrajectory other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        states.copyFrom(other.states);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectory mergeFrom(final ProtobufTrajectory other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasStates()) {
+        getMutableStates().addAll(other.states);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectory clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      states.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectory clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      states.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufTrajectory)) {
+        return false;
+      }
+      ProtobufTrajectory other = (ProtobufTrajectory) o;
+      return bitField0_ == other.bitField0_
+        && (!hasStates() || states.equals(other.states));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        for (int i = 0; i < states.length(); i++) {
+          output.writeRawByte((byte) 18);
+          output.writeMessageNoTag(states.get(i));
+        }
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += (1 * states.length()) + ProtoSink.computeRepeatedMessageSizeNoTag(states);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufTrajectory mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 18: {
+            // states
+            tag = input.readRepeatedMessage(states, tag);
+            bitField0_ |= 0x00000001;
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRepeatedMessage(FieldNames.states, states);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufTrajectory mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -892482046: {
+            if (input.isAtField(FieldNames.states)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedMessage(states);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufTrajectory clone() {
+      return new ProtobufTrajectory().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufTrajectory parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufTrajectory(), data).checkInitialized();
+    }
+
+    public static ProtobufTrajectory parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTrajectory(), input).checkInitialized();
+    }
+
+    public static ProtobufTrajectory parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufTrajectory(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufTrajectory messages
+     */
+    public static MessageFactory<ProtobufTrajectory> getFactory() {
+      return ProtobufTrajectoryFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Trajectory.wpi_proto_ProtobufTrajectory_descriptor;
+    }
+
+    private enum ProtobufTrajectoryFactory implements MessageFactory<ProtobufTrajectory> {
+      INSTANCE;
+
+      @Override
+      public ProtobufTrajectory create() {
+        return ProtobufTrajectory.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName states = FieldName.forField("states");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Wpimath.java b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Wpimath.java
new file mode 100644
index 0000000..ce964a2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Wpimath.java
@@ -0,0 +1,791 @@
+// 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.
+// Code generated by protocol buffer compiler. Do not edit!
+package edu.wpi.first.math.proto;
+
+import java.io.IOException;
+import us.hebi.quickbuf.Descriptors;
+import us.hebi.quickbuf.FieldName;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+import us.hebi.quickbuf.JsonSink;
+import us.hebi.quickbuf.JsonSource;
+import us.hebi.quickbuf.MessageFactory;
+import us.hebi.quickbuf.ProtoMessage;
+import us.hebi.quickbuf.ProtoSink;
+import us.hebi.quickbuf.ProtoSource;
+import us.hebi.quickbuf.ProtoUtil;
+import us.hebi.quickbuf.RepeatedByte;
+import us.hebi.quickbuf.RepeatedDouble;
+
+public final class Wpimath {
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(540,
+    "Cg13cGltYXRoLnByb3RvEgl3cGkucHJvdG8iWgoOUHJvdG9idWZNYXRyaXgSGQoIbnVtX3Jvd3MYASAB" + 
+    "KA1SB251bVJvd3MSGQoIbnVtX2NvbHMYAiABKA1SB251bUNvbHMSEgoEZGF0YRgDIAMoAVIEZGF0YSIk" + 
+    "Cg5Qcm90b2J1ZlZlY3RvchISCgRyb3dzGAEgAygBUgRyb3dzQhoKGGVkdS53cGkuZmlyc3QubWF0aC5w" + 
+    "cm90b0rZAgoGEgQAAA4BCggKAQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoC" + 
+    "BAASBAYACgEKCgoDBAABEgMGCBYKCwoEBAACABIDBwIWCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJ" + 
+    "EQoMCgUEAAIAAxIDBxQVCgsKBAQAAgESAwgCFgoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICREKDAoF" + 
+    "BAACAQMSAwgUFQoLCgQEAAICEgMJAhsKDAoFBAACAgQSAwkCCgoMCgUEAAICBRIDCQsRCgwKBQQAAgIB" + 
+    "EgMJEhYKDAoFBAACAgMSAwkZGgoKCgIEARIEDAAOAQoKCgMEAQESAwwIFgoLCgQEAQIAEgMNAhsKDAoF" + 
+    "BAECAAQSAw0CCgoMCgUEAQIABRIDDQsRCgwKBQQBAgABEgMNEhYKDAoFBAECAAMSAw0ZGmIGcHJvdG8z");
+
+  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("wpimath.proto", "wpi.proto", descriptorData);
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufMatrix_descriptor = descriptor.internalContainedType(28, 90, "ProtobufMatrix", "wpi.proto.ProtobufMatrix");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufVector_descriptor = descriptor.internalContainedType(120, 36, "ProtobufVector", "wpi.proto.ProtobufVector");
+
+  /**
+   * @return this proto file's descriptor.
+   */
+  public static Descriptors.FileDescriptor getDescriptor() {
+    return descriptor;
+  }
+
+  /**
+   * Protobuf type {@code ProtobufMatrix}
+   */
+  public static final class ProtobufMatrix extends ProtoMessage<ProtobufMatrix> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional uint32 num_rows = 1;</code>
+     */
+    private int numRows;
+
+    /**
+     * <code>optional uint32 num_cols = 2;</code>
+     */
+    private int numCols;
+
+    /**
+     * <code>repeated double data = 3;</code>
+     */
+    private final RepeatedDouble data = RepeatedDouble.newEmptyInstance();
+
+    private ProtobufMatrix() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufMatrix}
+     */
+    public static ProtobufMatrix newInstance() {
+      return new ProtobufMatrix();
+    }
+
+    /**
+     * <code>optional uint32 num_rows = 1;</code>
+     * @return whether the numRows field is set
+     */
+    public boolean hasNumRows() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional uint32 num_rows = 1;</code>
+     * @return this
+     */
+    public ProtobufMatrix clearNumRows() {
+      bitField0_ &= ~0x00000001;
+      numRows = 0;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_rows = 1;</code>
+     * @return the numRows
+     */
+    public int getNumRows() {
+      return numRows;
+    }
+
+    /**
+     * <code>optional uint32 num_rows = 1;</code>
+     * @param value the numRows to set
+     * @return this
+     */
+    public ProtobufMatrix setNumRows(final int value) {
+      bitField0_ |= 0x00000001;
+      numRows = value;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_cols = 2;</code>
+     * @return whether the numCols field is set
+     */
+    public boolean hasNumCols() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional uint32 num_cols = 2;</code>
+     * @return this
+     */
+    public ProtobufMatrix clearNumCols() {
+      bitField0_ &= ~0x00000002;
+      numCols = 0;
+      return this;
+    }
+
+    /**
+     * <code>optional uint32 num_cols = 2;</code>
+     * @return the numCols
+     */
+    public int getNumCols() {
+      return numCols;
+    }
+
+    /**
+     * <code>optional uint32 num_cols = 2;</code>
+     * @param value the numCols to set
+     * @return this
+     */
+    public ProtobufMatrix setNumCols(final int value) {
+      bitField0_ |= 0x00000002;
+      numCols = value;
+      return this;
+    }
+
+    /**
+     * <code>repeated double data = 3;</code>
+     * @return whether the data field is set
+     */
+    public boolean hasData() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>repeated double data = 3;</code>
+     * @return this
+     */
+    public ProtobufMatrix clearData() {
+      bitField0_ &= ~0x00000004;
+      data.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double data = 3;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableData()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getData() {
+      return data;
+    }
+
+    /**
+     * <code>repeated double data = 3;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableData() {
+      bitField0_ |= 0x00000004;
+      return data;
+    }
+
+    /**
+     * <code>repeated double data = 3;</code>
+     * @param value the data to add
+     * @return this
+     */
+    public ProtobufMatrix addData(final double value) {
+      bitField0_ |= 0x00000004;
+      data.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double data = 3;</code>
+     * @param values the data to add
+     * @return this
+     */
+    public ProtobufMatrix addAllData(final double... values) {
+      bitField0_ |= 0x00000004;
+      data.addAll(values);
+      return this;
+    }
+
+    @Override
+    public ProtobufMatrix copyFrom(final ProtobufMatrix other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        numRows = other.numRows;
+        numCols = other.numCols;
+        data.copyFrom(other.data);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMatrix mergeFrom(final ProtobufMatrix other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasNumRows()) {
+        setNumRows(other.numRows);
+      }
+      if (other.hasNumCols()) {
+        setNumCols(other.numCols);
+      }
+      if (other.hasData()) {
+        getMutableData().addAll(other.data);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufMatrix clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      numRows = 0;
+      numCols = 0;
+      data.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufMatrix clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      data.clear();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufMatrix)) {
+        return false;
+      }
+      ProtobufMatrix other = (ProtobufMatrix) o;
+      return bitField0_ == other.bitField0_
+        && (!hasNumRows() || numRows == other.numRows)
+        && (!hasNumCols() || numCols == other.numCols)
+        && (!hasData() || data.equals(other.data));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 8);
+        output.writeUInt32NoTag(numRows);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 16);
+        output.writeUInt32NoTag(numCols);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        for (int i = 0; i < data.length(); i++) {
+          output.writeRawByte((byte) 25);
+          output.writeDoubleNoTag(data.array()[i]);
+        }
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 1 + ProtoSink.computeUInt32SizeNoTag(numRows);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 1 + ProtoSink.computeUInt32SizeNoTag(numCols);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += (1 + 8) * data.length();
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufMatrix mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 8: {
+            // numRows
+            numRows = input.readUInt32();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 16) {
+              break;
+            }
+          }
+          case 16: {
+            // numCols
+            numCols = input.readUInt32();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 26) {
+              break;
+            }
+          }
+          case 26: {
+            // data [packed=true]
+            input.readPackedDouble(data);
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+          case 25: {
+            // data [packed=false]
+            tag = input.readRepeatedDouble(data, tag);
+            bitField0_ |= 0x00000004;
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeUInt32(FieldNames.numRows, numRows);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeUInt32(FieldNames.numCols, numCols);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRepeatedDouble(FieldNames.data, data);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufMatrix mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -2000982401:
+          case -1888824590: {
+            if (input.isAtField(FieldNames.numRows)) {
+              if (!input.trySkipNullValue()) {
+                numRows = input.readUInt32();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -2001429607:
+          case -1889271796: {
+            if (input.isAtField(FieldNames.numCols)) {
+              if (!input.trySkipNullValue()) {
+                numCols = input.readUInt32();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 3076010: {
+            if (input.isAtField(FieldNames.data)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(data);
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufMatrix clone() {
+      return new ProtobufMatrix().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufMatrix parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufMatrix(), data).checkInitialized();
+    }
+
+    public static ProtobufMatrix parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMatrix(), input).checkInitialized();
+    }
+
+    public static ProtobufMatrix parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufMatrix(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufMatrix messages
+     */
+    public static MessageFactory<ProtobufMatrix> getFactory() {
+      return ProtobufMatrixFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Wpimath.wpi_proto_ProtobufMatrix_descriptor;
+    }
+
+    private enum ProtobufMatrixFactory implements MessageFactory<ProtobufMatrix> {
+      INSTANCE;
+
+      @Override
+      public ProtobufMatrix create() {
+        return ProtobufMatrix.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName numRows = FieldName.forField("numRows", "num_rows");
+
+      static final FieldName numCols = FieldName.forField("numCols", "num_cols");
+
+      static final FieldName data = FieldName.forField("data");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufVector}
+   */
+  public static final class ProtobufVector extends ProtoMessage<ProtobufVector> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>repeated double rows = 1;</code>
+     */
+    private final RepeatedDouble rows = RepeatedDouble.newEmptyInstance();
+
+    private ProtobufVector() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufVector}
+     */
+    public static ProtobufVector newInstance() {
+      return new ProtobufVector();
+    }
+
+    /**
+     * <code>repeated double rows = 1;</code>
+     * @return whether the rows field is set
+     */
+    public boolean hasRows() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>repeated double rows = 1;</code>
+     * @return this
+     */
+    public ProtobufVector clearRows() {
+      bitField0_ &= ~0x00000001;
+      rows.clear();
+      return this;
+    }
+
+    /**
+     * <code>repeated double rows = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableRows()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public RepeatedDouble getRows() {
+      return rows;
+    }
+
+    /**
+     * <code>repeated double rows = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public RepeatedDouble getMutableRows() {
+      bitField0_ |= 0x00000001;
+      return rows;
+    }
+
+    /**
+     * <code>repeated double rows = 1;</code>
+     * @param value the rows to add
+     * @return this
+     */
+    public ProtobufVector addRows(final double value) {
+      bitField0_ |= 0x00000001;
+      rows.add(value);
+      return this;
+    }
+
+    /**
+     * <code>repeated double rows = 1;</code>
+     * @param values the rows to add
+     * @return this
+     */
+    public ProtobufVector addAllRows(final double... values) {
+      bitField0_ |= 0x00000001;
+      rows.addAll(values);
+      return this;
+    }
+
+    @Override
+    public ProtobufVector copyFrom(final ProtobufVector other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        rows.copyFrom(other.rows);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufVector mergeFrom(final ProtobufVector other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasRows()) {
+        getMutableRows().addAll(other.rows);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufVector clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      rows.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufVector clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      rows.clear();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufVector)) {
+        return false;
+      }
+      ProtobufVector other = (ProtobufVector) o;
+      return bitField0_ == other.bitField0_
+        && (!hasRows() || rows.equals(other.rows));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        for (int i = 0; i < rows.length(); i++) {
+          output.writeRawByte((byte) 9);
+          output.writeDoubleNoTag(rows.array()[i]);
+        }
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += (1 + 8) * rows.length();
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufVector mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 10: {
+            // rows [packed=true]
+            input.readPackedDouble(rows);
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+          case 9: {
+            // rows [packed=false]
+            tag = input.readRepeatedDouble(rows, tag);
+            bitField0_ |= 0x00000001;
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRepeatedDouble(FieldNames.rows, rows);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufVector mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case 3506649: {
+            if (input.isAtField(FieldNames.rows)) {
+              if (!input.trySkipNullValue()) {
+                input.readRepeatedDouble(rows);
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufVector clone() {
+      return new ProtobufVector().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufVector parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufVector(), data).checkInitialized();
+    }
+
+    public static ProtobufVector parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufVector(), input).checkInitialized();
+    }
+
+    public static ProtobufVector parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufVector(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufVector messages
+     */
+    public static MessageFactory<ProtobufVector> getFactory() {
+      return ProtobufVectorFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Wpimath.wpi_proto_ProtobufVector_descriptor;
+    }
+
+    private enum ProtobufVectorFactory implements MessageFactory<ProtobufVector> {
+      INSTANCE;
+
+      @Override
+      public ProtobufVector create() {
+        return ProtobufVector.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName rows = FieldName.forField("rows");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
index 7b1c377..609cdf5 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 
+/** Computer vision utility functions. */
 public final class ComputerVisionUtil {
   private ComputerVisionUtil() {
     throw new AssertionError("utility class");
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/DARE.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/DARE.java
index ad07d05..44c488d 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/DARE.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/DARE.java
@@ -6,6 +6,7 @@
 
 import org.ejml.simple.SimpleMatrix;
 
+/** DARE solver utility functions. */
 public final class DARE {
   private DARE() {
     throw new UnsupportedOperationException("This is a utility class!");
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
index a7b922c..a0aedef 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
@@ -9,10 +9,17 @@
 /**
  * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
  * from points that are defined. This uses linear interpolation.
+ *
+ * @param <K> Key type.
+ * @param <R> Number of matrix rows.
+ * @param <C> Number of matrix columns.
  */
 public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
   private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
 
+  /** Default constructor. */
+  public InterpolatingMatrixTreeMap() {}
+
   /**
    * Inserts a key-value pair.
    *
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
index 2ee010f..46e42f4 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
@@ -14,8 +14,50 @@
  * @param <C> The number of columns of the desired matrix.
  */
 public class MatBuilder<R extends Num, C extends Num> {
-  final Nat<R> m_rows;
-  final Nat<C> m_cols;
+  /**
+   * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by
+   * row, left to right with the given data).
+   *
+   * @param rows The number of rows of the matrix.
+   * @param cols The number of columns of the matrix.
+   * @param data The data to fill the matrix with.
+   * @param <R> The number of rows of the matrix.
+   * @param <C> The number of columns of the matrix.
+   * @return The constructed matrix.
+   */
+  public static final <R extends Num, C extends Num> Matrix<R, C> fill(
+      Nat<R> rows, Nat<C> cols, double... data) {
+    if (Objects.requireNonNull(data).length != rows.getNum() * cols.getNum()) {
+      throw new IllegalArgumentException(
+          "Invalid matrix data provided. Wanted "
+              + rows.getNum()
+              + " x "
+              + cols.getNum()
+              + " matrix, but got "
+              + data.length
+              + " elements");
+    }
+    return new Matrix<>(new SimpleMatrix(rows.getNum(), cols.getNum(), true, data));
+  }
+
+  /** Number of rows. */
+  protected final Nat<R> m_rows;
+
+  /** Number of columns. */
+  protected final Nat<C> m_cols;
+
+  /**
+   * Creates a new {@link MatBuilder} with the given dimensions.
+   *
+   * @param rows The number of rows of the matrix.
+   * @param cols The number of columns of the matrix.
+   * @deprecated Use {@link MatBuilder#fill} instead.
+   */
+  @Deprecated(since = "2024", forRemoval = true)
+  public MatBuilder(Nat<R> rows, Nat<C> cols) {
+    this.m_rows = Objects.requireNonNull(rows);
+    this.m_cols = Objects.requireNonNull(cols);
+  }
 
   /**
    * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by
@@ -25,28 +67,6 @@
    * @return The constructed matrix.
    */
   public final Matrix<R, C> fill(double... data) {
-    if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
-      throw new IllegalArgumentException(
-          "Invalid matrix data provided. Wanted "
-              + this.m_rows.getNum()
-              + " x "
-              + this.m_cols.getNum()
-              + " matrix, but got "
-              + data.length
-              + " elements");
-    } else {
-      return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
-    }
-  }
-
-  /**
-   * Creates a new {@link MatBuilder} with the given dimensions.
-   *
-   * @param rows The number of rows of the matrix.
-   * @param cols The number of columns of the matrix.
-   */
-  public MatBuilder(Nat<R> rows, Nat<C> cols) {
-    this.m_rows = Objects.requireNonNull(rows);
-    this.m_cols = Objects.requireNonNull(cols);
+    return fill(m_rows, m_cols, data);
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
index 3c2b843..ad4e2e8 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math;
 
+/** WPIMath utility functions. */
 public interface MathShared {
   /**
    * Report an error.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
index d6e3e96..d490a6e 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.util.WPIUtilJNI;
 
+/** Storage for MathShared object. */
 public final class MathSharedStore {
   private static MathShared mathShared;
 
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
index a3cc299..c3449cf 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
@@ -4,15 +4,35 @@
 
 package edu.wpi.first.math;
 
+/** WPIMath usage reporting IDs. */
 public enum MathUsageId {
+  /** DifferentialDriveKinematics. */
   kKinematics_DifferentialDrive,
+
+  /** MecanumDriveKinematics. */
   kKinematics_MecanumDrive,
+
+  /** SwerveDriveKinematics. */
   kKinematics_SwerveDrive,
+
+  /** TrapezoidProfile. */
   kTrajectory_TrapezoidProfile,
+
+  /** LinearFilter. */
   kFilter_Linear,
+
+  /** DifferentialDriveOdometry. */
   kOdometry_DifferentialDrive,
+
+  /** SwerveDriveOdometry. */
   kOdometry_SwerveDrive,
+
+  /** MecanumDriveOdometry. */
   kOdometry_MecanumDrive,
+
+  /** PIDController. */
   kController_PIDController2,
+
+  /** ProfiledPIDController. */
   kController_ProfiledPIDController,
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 3785780..044cddf 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math;
 
+/** Math utility functions. */
 public final class MathUtil {
   private MathUtil() {
     throw new AssertionError("utility class");
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index b7a86fa..c3f796e 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -24,6 +24,7 @@
  * @param <C> The number of columns in this matrix.
  */
 public class Matrix<R extends Num, C extends Num> {
+  /** Storage for underlying EJML matrix. */
   protected final SimpleMatrix m_storage;
 
   /**
@@ -288,7 +289,7 @@
    * @return The resultant matrix.
    */
   public Matrix<R, C> div(int value) {
-    return new Matrix<>(this.m_storage.divide((double) value));
+    return new Matrix<>(this.m_storage.divide(value));
   }
 
   /**
@@ -488,7 +489,7 @@
    * @return The element by element power of "this" and b.
    */
   public final Matrix<R, C> elementPower(int b) {
-    return new Matrix<>(this.m_storage.elementPower((double) b));
+    return new Matrix<>(this.m_storage.elementPower(b));
   }
 
   /**
@@ -545,7 +546,7 @@
    */
   public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
       int height, int width, int startingRow, int startingCol) {
-    return new Matrix<R2, C2>(
+    return new Matrix<>(
         this.m_storage.extractMatrix(
             startingRow, startingRow + height, startingCol, startingCol + width));
   }
@@ -607,8 +608,7 @@
         return new Matrix<>(new SimpleMatrix(temp.getNumRows(), temp.getNumCols()));
       }
 
-      throw new RuntimeException(
-          "Cholesky decomposition failed! Input matrix:\n" + m_storage.toString());
+      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + m_storage);
     }
 
     return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
@@ -654,7 +654,10 @@
    * @param <R> The number of rows of the desired matrix as a generic.
    * @param <C> The number of columns of the desired matrix as a generic.
    * @return A builder to construct the matrix.
+   * @deprecated Use {@link MatBuilder#fill} instead.
    */
+  @Deprecated(since = "2024", forRemoval = true)
+  @SuppressWarnings("removal")
   public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
     return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols));
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Num.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Num.java
index ef0fd2d..aa31dd7 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Num.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Num.java
@@ -6,6 +6,9 @@
 
 /** A number expressed as a java class. */
 public abstract class Num {
+  /** Default constructor. */
+  public Num() {}
+
   /**
    * The number this is backing.
    *
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Pair.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Pair.java
index 8ddf1ae..f854894 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Pair.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Pair.java
@@ -4,23 +4,54 @@
 
 package edu.wpi.first.math;
 
+/**
+ * Represents a pair of two objects.
+ *
+ * @param <A> The first object's type.
+ * @param <B> The second object's type.
+ */
 public class Pair<A, B> {
   private final A m_first;
   private final B m_second;
 
+  /**
+   * Constructs a pair.
+   *
+   * @param first The first object.
+   * @param second The second object.
+   */
   public Pair(A first, B second) {
     m_first = first;
     m_second = second;
   }
 
+  /**
+   * Returns the first object.
+   *
+   * @return The first object.
+   */
   public A getFirst() {
     return m_first;
   }
 
+  /**
+   * Returns the second object.
+   *
+   * @return The second object.
+   */
   public B getSecond() {
     return m_second;
   }
 
+  /**
+   * Returns a pair comprised of the two given objects.
+   *
+   * @param <A> The first object's type.
+   * @param <B> The second object's type.
+   * @param a The first object.
+   * @param b The second object.
+   * @return A pair comprised of the two given objects.
+   */
   public static <A, B> Pair<A, B> of(A a, B b) {
     return new Pair<>(a, b);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
deleted file mode 100644
index 487faf4..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
+++ /dev/null
@@ -1,246 +0,0 @@
-// 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.
-
-package edu.wpi.first.math;
-
-import java.util.function.BiFunction;
-import org.ejml.data.DMatrixRMaj;
-import org.ejml.dense.row.NormOps_DDRM;
-import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
-import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
-import org.ejml.simple.SimpleBase;
-import org.ejml.simple.SimpleMatrix;
-
-public final class SimpleMatrixUtils {
-  private SimpleMatrixUtils() {}
-
-  /**
-   * Compute the matrix exponential, e^M of the given matrix.
-   *
-   * @param matrix The matrix to compute the exponential of.
-   * @return The resultant matrix.
-   */
-  public static SimpleMatrix expm(SimpleMatrix matrix) {
-    BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
-    SimpleMatrix A = matrix;
-    double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
-    int nSquarings = 0;
-
-    if (A_L1 < 1.495585217958292e-002) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = pade3(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
-    } else if (A_L1 < 2.539398330063230e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = pade5(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
-    } else if (A_L1 < 9.504178996162932e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = pade7(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
-    } else if (A_L1 < 2.097847961257068e+000) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = pade9(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
-    } else {
-      double maxNorm = 5.371920351148152;
-      double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
-      nSquarings = (int) Math.max(0, Math.ceil(log));
-      A = A.divide(Math.pow(2.0, nSquarings));
-      Pair<SimpleMatrix, SimpleMatrix> pair = pade13(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
-    }
-  }
-
-  private static SimpleMatrix dispatchPade(
-      SimpleMatrix U,
-      SimpleMatrix V,
-      int nSquarings,
-      BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
-    SimpleMatrix P = U.plus(V);
-    SimpleMatrix Q = U.negative().plus(V);
-
-    SimpleMatrix R = solveProvider.apply(Q, P);
-
-    for (int i = 0; i < nSquarings; i++) {
-      R = R.mult(R);
-    }
-
-    return R;
-  }
-
-  private static Pair<SimpleMatrix, SimpleMatrix> pade3(SimpleMatrix A) {
-    double[] b = new double[] {120, 60, 12, 1};
-    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
-
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
-    SimpleMatrix V = A2.scale(b[2]).plus(ident.scale(b[0]));
-    return new Pair<>(U, V);
-  }
-
-  private static Pair<SimpleMatrix, SimpleMatrix> pade5(SimpleMatrix A) {
-    double[] b = new double[] {30240, 15120, 3360, 420, 30, 1};
-    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-
-    SimpleMatrix U = A.mult(A4.scale(b[5]).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V = A4.scale(b[4]).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  private static Pair<SimpleMatrix, SimpleMatrix> pade7(SimpleMatrix A) {
-    double[] b = new double[] {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
-    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-
-    SimpleMatrix U =
-        A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V =
-        A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  private static Pair<SimpleMatrix, SimpleMatrix> pade9(SimpleMatrix A) {
-    double[] b =
-        new double[] {
-          17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
-        };
-    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-    SimpleMatrix A8 = A6.mult(A2);
-
-    SimpleMatrix U =
-        A.mult(
-            A8.scale(b[9])
-                .plus(A6.scale(b[7]))
-                .plus(A4.scale(b[5]))
-                .plus(A2.scale(b[3]))
-                .plus(ident.scale(b[1])));
-    SimpleMatrix V =
-        A8.scale(b[8])
-            .plus(A6.scale(b[6]))
-            .plus(A4.scale(b[4]))
-            .plus(A2.scale(b[2]))
-            .plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  private static Pair<SimpleMatrix, SimpleMatrix> pade13(SimpleMatrix A) {
-    double[] b =
-        new double[] {
-          64764752532480000.0,
-          32382376266240000.0,
-          7771770303897600.0,
-          1187353796428800.0,
-          129060195264000.0,
-          10559470521600.0,
-          670442572800.0,
-          33522128640.0,
-          1323241920,
-          40840800,
-          960960,
-          16380,
-          182,
-          1
-        };
-    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
-
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-
-    SimpleMatrix U =
-        A.mult(
-            A6.scale(b[13])
-                .plus(A4.scale(b[11]))
-                .plus(A2.scale(b[9]))
-                .plus(A6.scale(b[7]))
-                .plus(A4.scale(b[5]))
-                .plus(A2.scale(b[3]))
-                .plus(ident.scale(b[1])));
-    SimpleMatrix V =
-        A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8])))
-            .plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
-
-    return new Pair<>(U, V);
-  }
-
-  private static SimpleMatrix eye(int rows, int cols) {
-    return SimpleMatrix.identity(Math.min(rows, cols));
-  }
-
-  /**
-   * The identy of a square matrix.
-   *
-   * @param rows the number of rows (and columns)
-   * @return the identiy matrix, rows x rows.
-   */
-  public static SimpleMatrix eye(int rows) {
-    return SimpleMatrix.identity(rows);
-  }
-
-  /**
-   * Decompose the given matrix using Cholesky Decomposition and return a view of the upper
-   * triangular matrix (if you want lower triangular see the other overload of this method.) If the
-   * input matrix is zeros, this will return the zero matrix.
-   *
-   * @param src The matrix to decompose.
-   * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed (i.e. is not positive
-   *     semidefinite).
-   */
-  public static SimpleMatrix lltDecompose(SimpleMatrix src) {
-    return lltDecompose(src, false);
-  }
-
-  /**
-   * Decompose the given matrix using Cholesky Decomposition. If the input matrix is zeros, this
-   * will return the zero matrix.
-   *
-   * @param src The matrix to decompose.
-   * @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
-   * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed (i.e. is not positive
-   *     semidefinite).
-   */
-  public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
-    SimpleMatrix temp = src.copy();
-
-    CholeskyDecomposition_F64<DMatrixRMaj> chol =
-        DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
-    if (!chol.decompose(temp.getMatrix())) {
-      // check that the input is not all zeros -- if they are, we special case and return all
-      // zeros.
-      var matData = temp.getDDRM().data;
-      var isZeros = true;
-      for (double matDatum : matData) {
-        isZeros &= Math.abs(matDatum) < 1e-6;
-      }
-      if (isZeros) {
-        return new SimpleMatrix(temp.getNumRows(), temp.getNumCols());
-      }
-
-      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
-    }
-
-    return SimpleMatrix.wrap(chol.getT(null));
-  }
-
-  /**
-   * Computes the matrix exponential using Eigen's solver.
-   *
-   * @param A the matrix to exponentiate.
-   * @return the exponential of A.
-   */
-  public static SimpleMatrix exp(SimpleMatrix A) {
-    SimpleMatrix toReturn = new SimpleMatrix(A.getNumRows(), A.getNumRows());
-    WPIMathJNI.exp(A.getDDRM().getData(), A.getNumRows(), toReturn.getDDRM().getData());
-    return toReturn;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index 27ed9e0..525d5ef 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -11,6 +11,7 @@
 import java.util.Random;
 import org.ejml.simple.SimpleMatrix;
 
+/** State-space utilities. */
 public final class StateSpaceUtil {
   private static Random rand = new Random();
 
@@ -139,7 +140,7 @@
    * @param u The input to clamp.
    * @param umin The minimum input magnitude.
    * @param umax The maximum input magnitude.
-   * @param <I> The number of inputs.
+   * @param <I> Number of inputs.
    * @return The clamped input.
    */
   public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(
@@ -157,7 +158,7 @@
    *
    * @param u The input vector.
    * @param maxMagnitude The maximum magnitude any input can have.
-   * @param <I> The number of inputs.
+   * @param <I> Number of inputs.
    * @return The normalizedInput
    */
   public static <I extends Num> Matrix<I, N1> desaturateInputVector(
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
index 9ba91b0..0e47ae3 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
@@ -17,26 +17,22 @@
 import java.util.Objects;
 import org.ejml.simple.SimpleMatrix;
 
-/**
- * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
- *
- * @param <N> The dimension of the vector to be constructed.
- */
-public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
-  public VecBuilder(Nat<N> rows) {
-    super(rows, Nat.N1());
+/** A class for constructing vectors (Nx1 matrices). */
+public final class VecBuilder {
+  private VecBuilder() {
+    throw new UnsupportedOperationException("this is a utility class!");
   }
 
-  private Vector<N> fillVec(double... data) {
-    if (Objects.requireNonNull(data).length != this.m_rows.getNum()) {
+  private static <N extends Num> Vector<N> fillVec(Nat<N> rows, double... data) {
+    if (Objects.requireNonNull(data).length != rows.getNum()) {
       throw new IllegalArgumentException(
           "Invalid vector data provided. Wanted "
-              + this.m_rows.getNum()
+              + rows.getNum()
               + " vector, but got "
               + data.length
               + " elements");
     }
-    return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data));
+    return new Vector<>(new SimpleMatrix(data));
   }
 
   /**
@@ -46,7 +42,7 @@
    * @return 1x1 vector
    */
   public static Vector<N1> fill(double n1) {
-    return new VecBuilder<>(Nat.N1()).fillVec(n1);
+    return fillVec(Nat.N1(), n1);
   }
 
   /**
@@ -57,7 +53,7 @@
    * @return 2x1 vector
    */
   public static Vector<N2> fill(double n1, double n2) {
-    return new VecBuilder<>(Nat.N2()).fillVec(n1, n2);
+    return fillVec(Nat.N2(), n1, n2);
   }
 
   /**
@@ -69,7 +65,7 @@
    * @return 3x1 vector
    */
   public static Vector<N3> fill(double n1, double n2, double n3) {
-    return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3);
+    return fillVec(Nat.N3(), n1, n2, n3);
   }
 
   /**
@@ -82,7 +78,7 @@
    * @return 4x1 vector
    */
   public static Vector<N4> fill(double n1, double n2, double n3, double n4) {
-    return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4);
+    return fillVec(Nat.N4(), n1, n2, n3, n4);
   }
 
   /**
@@ -96,7 +92,7 @@
    * @return 5x1 vector
    */
   public static Vector<N5> fill(double n1, double n2, double n3, double n4, double n5) {
-    return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5);
+    return fillVec(Nat.N5(), n1, n2, n3, n4, n5);
   }
 
   /**
@@ -111,7 +107,7 @@
    * @return 6x1 vector
    */
   public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5, double n6) {
-    return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6);
+    return fillVec(Nat.N6(), n1, n2, n3, n4, n5, n6);
   }
 
   /**
@@ -128,7 +124,7 @@
    */
   public static Vector<N7> fill(
       double n1, double n2, double n3, double n4, double n5, double n6, double n7) {
-    return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7);
+    return fillVec(Nat.N7(), n1, n2, n3, n4, n5, n6, n7);
   }
 
   /**
@@ -146,7 +142,7 @@
    */
   public static Vector<N8> fill(
       double n1, double n2, double n3, double n4, double n5, double n6, double n7, double n8) {
-    return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8);
+    return fillVec(Nat.N8(), n1, n2, n3, n4, n5, n6, n7, n8);
   }
 
   /**
@@ -173,7 +169,7 @@
       double n7,
       double n8,
       double n9) {
-    return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
+    return fillVec(Nat.N9(), n1, n2, n3, n4, n5, n6, n7, n8, n9);
   }
 
   /**
@@ -202,6 +198,6 @@
       double n8,
       double n9,
       double n10) {
-    return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
+    return fillVec(Nat.N10(), n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index ef47a3b..a60bcba 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -8,6 +8,7 @@
 import java.io.IOException;
 import java.util.concurrent.atomic.AtomicBoolean;
 
+/** WPIMath JNI. */
 public final class WPIMathJNI {
   static boolean libraryLoaded = false;
   static RuntimeLoader<WPIMathJNI> loader = null;
@@ -385,15 +386,32 @@
    */
   public static native String serializeTrajectory(double[] elements);
 
+  /** Sets whether JNI should be loaded in the static block. */
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
     public static boolean getExtractOnStaticLoad() {
       return extractOnStaticLoad.get();
     }
 
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
     public static void setExtractOnStaticLoad(boolean load) {
       extractOnStaticLoad.set(load);
     }
+
+    /** Utility class. */
+    private Helper() {}
   }
+
+  /** Utility class. */
+  private WPIMathJNI() {}
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
index 2e9a8d8..d7a695f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
@@ -4,16 +4,34 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
+import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
 /**
  * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting
  * against the force of gravity on a beam suspended at an angle).
  */
-public class ArmFeedforward {
+public class ArmFeedforward implements ProtobufSerializable, StructSerializable {
+  /** The static gain, in volts. */
   public final double ks;
+
+  /** The gravity gain, in volts. */
   public final double kg;
+
+  /** The velocity gain, in volt seconds per radian. */
   public final double kv;
+
+  /** The acceleration gain, in volt seconds² per radian. */
   public final double ka;
 
+  /** Arm feedforward protobuf for serialization. */
+  public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
+
+  /** Arm feedforward struct for serialization. */
+  public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
+
   /**
    * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate
    * units of the computed feedforward.
@@ -22,12 +40,20 @@
    * @param kg The gravity gain.
    * @param kv The velocity gain.
    * @param ka The acceleration gain.
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
    */
   public ArmFeedforward(double ks, double kg, double kv, double ka) {
     this.ks = ks;
     this.kg = kg;
     this.kv = kv;
     this.ka = ka;
+    if (kv < 0.0) {
+      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
+    }
+    if (ka < 0.0) {
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!");
+    }
   }
 
   /**
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java
index 7e8c778..cf852c3 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java
@@ -37,6 +37,7 @@
    *
    * @param tolerance Tolerance for {@link #atSetpoint() atSetpoint}.
    */
+  @SuppressWarnings("this-escape")
   public BangBangController(double tolerance) {
     instances++;
 
@@ -89,7 +90,7 @@
    *
    * @param tolerance Position error which is tolerable.
    */
-  public void setTolerance(double tolerance) {
+  public final void setTolerance(double tolerance) {
     m_tolerance = tolerance;
   }
 
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
index a2a5944..0486500 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
@@ -16,9 +16,8 @@
  * Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
  *
  * <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input
- * vector, the B matrix(continuous input matrix) is calculated through a {@link
- * edu.wpi.first.math.system.NumericalJacobian}. In this case f has to be control-affine (of the
- * form f(x) + Bu).
+ * vector, the B matrix(continuous input matrix) is calculated through a {@link NumericalJacobian}.
+ * In this case f has to be control-affine (of the form f(x) + Bu).
  *
  * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
  * <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
@@ -26,8 +25,11 @@
  * <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied
  * when the feedforward is created and remains constant.
  *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
  */
 public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
   /** The current reference state. */
@@ -145,13 +147,13 @@
    *
    * @param initialState The initial state vector.
    */
-  public void reset(Matrix<States, N1> initialState) {
+  public final void reset(Matrix<States, N1> initialState) {
     m_r = initialState;
     m_uff.fill(0.0);
   }
 
   /** Resets the feedforward with a zero initial state vector. */
-  public void reset() {
+  public final void reset() {
     m_r.fill(0.0);
     m_uff.fill(0.0);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
index 8c47765..d2ef273 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
@@ -5,7 +5,10 @@
 package edu.wpi.first.math.controller;
 
 import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 
@@ -80,10 +83,10 @@
    */
   public DifferentialDriveWheelVoltages calculate(
       double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) {
-    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage);
+    Matrix<N2, N1> u = VecBuilder.fill(leftVoltage, rightVoltage);
 
     // Find unconstrained wheel accelerations
-    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity);
+    var x = VecBuilder.fill(leftVelocity, rightVelocity);
     var dxdt = m_system.getA().times(x).plus(m_system.getB().times(u));
 
     // Convert from wheel accelerations to linear and angular accelerations
@@ -98,9 +101,7 @@
     // [α]   [-1/trackwidth  1/trackwidth][dxdt(1)]
     //
     // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
-    var M =
-        new MatBuilder<>(Nat.N2(), Nat.N2())
-            .fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
+    var M = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
     var accels = M.times(dxdt);
 
     // Constrain the linear and angular accelerations
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
index 2fbd0aa..1569be0 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
@@ -4,13 +4,36 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.controller.proto.DifferentialDriveWheelVoltagesProto;
+import edu.wpi.first.math.controller.struct.DifferentialDriveWheelVoltagesStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
 /** Motor voltages for a differential drive. */
-public class DifferentialDriveWheelVoltages {
+public class DifferentialDriveWheelVoltages implements ProtobufSerializable, StructSerializable {
+  /** Left wheel voltage. */
   public double left;
+
+  /** Right wheel voltage. */
   public double right;
 
+  /** DifferentialDriveWheelVoltages protobuf for serialization. */
+  public static final DifferentialDriveWheelVoltagesProto proto =
+      new DifferentialDriveWheelVoltagesProto();
+
+  /** DifferentialDriveWheelVoltages struct for serialization. */
+  public static final DifferentialDriveWheelVoltagesStruct struct =
+      new DifferentialDriveWheelVoltagesStruct();
+
+  /** Default constructor. */
   public DifferentialDriveWheelVoltages() {}
 
+  /**
+   * Constructs a DifferentialDriveWheelVoltages.
+   *
+   * @param left Left wheel voltage.
+   * @param right Right wheel voltage.
+   */
   public DifferentialDriveWheelVoltages(double left, double right) {
     this.left = left;
     this.right = right;
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
index cfe7c2b..854462a 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -4,20 +4,37 @@
 
 package edu.wpi.first.math.controller;
 
-import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
+import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
 import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 
 /**
  * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
  * against the force of gravity).
  */
-public class ElevatorFeedforward {
+public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
+  /** The static gain. */
   public final double ks;
+
+  /** The gravity gain. */
   public final double kg;
+
+  /** The velocity gain. */
   public final double kv;
+
+  /** The acceleration gain. */
   public final double ka;
 
+  /** ElevatorFeedforward protobuf for serialization. */
+  public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
+
+  /** ElevatorFeedforward struct for serialization. */
+  public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
+
   /**
    * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will
    * dictate units of the computed feedforward.
@@ -26,12 +43,20 @@
    * @param kg The gravity gain.
    * @param kv The velocity gain.
    * @param ka The acceleration gain.
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
    */
   public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
     this.ks = ks;
     this.kg = kg;
     this.kv = kv;
     this.ka = ka;
+    if (kv < 0.0) {
+      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
+    }
+    if (ka < 0.0) {
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!");
+    }
   }
 
   /**
@@ -93,8 +118,8 @@
     var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
     var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
 
-    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
-    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+    var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
+    var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
 
     return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
index 7aa4cfa..29bf0a0 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
@@ -17,8 +17,12 @@
  * system and makes it behave like some other system. This can be used to make a drivetrain more
  * controllable during teleop driving by making it behave like a slower or more benign drivetrain.
  *
- * <p>For more on the underlying math, read appendix B.3 in
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ * <p>For more on the underlying math, read appendix B.3 in <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
  */
 public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
   // Computed controller output
@@ -99,7 +103,7 @@
   }
 
   /** Resets the controller. */
-  public void reset() {
+  public final void reset() {
     m_u.fill(0.0);
   }
 
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
index 2be2a47..116513e 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.numbers.N1;
@@ -64,8 +65,8 @@
   /**
    * Constructs a linear time-varying differential drive controller.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
    * for how to select the tolerances.
    *
    * @param plant The differential drive velocity plant.
@@ -88,46 +89,48 @@
     // Control law derivation is in section 8.7 of
     // https://file.tavsys.net/control/controls-engineering-in-frc.pdf
     var A =
-        new MatBuilder<>(Nat.N5(), Nat.N5())
-            .fill(
-                0.0,
-                0.0,
-                0.0,
-                0.5,
-                0.5,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                -1.0 / m_trackwidth,
-                1.0 / m_trackwidth,
-                0.0,
-                0.0,
-                0.0,
-                plant.getA(0, 0),
-                plant.getA(0, 1),
-                0.0,
-                0.0,
-                0.0,
-                plant.getA(1, 0),
-                plant.getA(1, 1));
+        MatBuilder.fill(
+            Nat.N5(),
+            Nat.N5(),
+            0.0,
+            0.0,
+            0.0,
+            0.5,
+            0.5,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            -1.0 / m_trackwidth,
+            1.0 / m_trackwidth,
+            0.0,
+            0.0,
+            0.0,
+            plant.getA(0, 0),
+            plant.getA(0, 1),
+            0.0,
+            0.0,
+            0.0,
+            plant.getA(1, 0),
+            plant.getA(1, 1));
     var B =
-        new MatBuilder<>(Nat.N5(), Nat.N2())
-            .fill(
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                plant.getB(0, 0),
-                plant.getB(0, 1),
-                plant.getB(1, 0),
-                plant.getB(1, 1));
+        MatBuilder.fill(
+            Nat.N5(),
+            Nat.N2(),
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            plant.getB(0, 0),
+            plant.getB(0, 1),
+            plant.getB(1, 0),
+            plant.getB(1, 1));
     var Q = StateSpaceUtil.makeCostMatrix(qelems);
     var R = StateSpaceUtil.makeCostMatrix(relems);
 
@@ -136,11 +139,7 @@
     // Ax = -Bu
     // x = -A⁻¹Bu
     double maxV =
-        plant
-            .getA()
-            .solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)))
-            .times(-1.0)
-            .get(0, 0);
+        plant.getA().solve(plant.getB().times(VecBuilder.fill(12.0, 12.0))).times(-1.0).get(0, 0);
 
     if (maxV <= 0.0) {
       throw new IllegalArgumentException(
@@ -201,13 +200,12 @@
   public void setTolerance(
       Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) {
     m_tolerance =
-        new MatBuilder<>(Nat.N5(), Nat.N1())
-            .fill(
-                poseTolerance.getX(),
-                poseTolerance.getY(),
-                poseTolerance.getRotation().getRadians(),
-                leftVelocityTolerance,
-                rightVelocityTolerance);
+        VecBuilder.fill(
+            poseTolerance.getX(),
+            poseTolerance.getY(),
+            poseTolerance.getRotation().getRadians(),
+            leftVelocityTolerance,
+            rightVelocityTolerance);
   }
 
   /**
@@ -234,13 +232,12 @@
     // This implements the linear time-varying differential drive controller in
     // theorem 9.6.3 of https://tavsys.net/controls-in-frc.
     var x =
-        new MatBuilder<>(Nat.N5(), Nat.N1())
-            .fill(
-                currentPose.getX(),
-                currentPose.getY(),
-                currentPose.getRotation().getRadians(),
-                leftVelocity,
-                rightVelocity);
+        VecBuilder.fill(
+            currentPose.getX(),
+            currentPose.getY(),
+            currentPose.getRotation().getRadians(),
+            leftVelocity,
+            rightVelocity);
 
     var inRobotFrame = Matrix.eye(Nat.N5());
     inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0)));
@@ -249,13 +246,12 @@
     inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0)));
 
     var r =
-        new MatBuilder<>(Nat.N5(), Nat.N1())
-            .fill(
-                poseRef.getX(),
-                poseRef.getY(),
-                poseRef.getRotation().getRadians(),
-                leftVelocityRef,
-                rightVelocityRef);
+        VecBuilder.fill(
+            poseRef.getX(),
+            poseRef.getY(),
+            poseRef.getRotation().getRadians(),
+            leftVelocityRef,
+            rightVelocityRef);
     m_error = r.minus(x);
     m_error.set(
         State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0)));
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
index 7e3391f..c3c54f3 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
@@ -80,8 +80,8 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
    * for how to select the tolerances.
    *
    * @param qelems The maximum desired error tolerance for each state.
@@ -95,8 +95,8 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
    * for how to select the tolerances.
    *
    * @param qelems The maximum desired error tolerance for each state.
@@ -149,7 +149,7 @@
     // A = [0  0  v]          B = [0  0]
     //     [0  0  0]              [0  1]
     var A = new Matrix<>(Nat.N3(), Nat.N3());
-    var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
+    var B = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
     var Q = StateSpaceUtil.makeCostMatrix(qelems);
     var R = StateSpaceUtil.makeCostMatrix(relems);
 
@@ -226,8 +226,12 @@
 
     var K = m_table.get(linearVelocityRef);
     var e =
-        new MatBuilder<>(Nat.N3(), Nat.N1())
-            .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians());
+        MatBuilder.fill(
+            Nat.N3(),
+            Nat.N1(),
+            m_poseError.getX(),
+            m_poseError.getY(),
+            m_poseError.getRotation().getRadians());
     var u = K.times(e);
 
     return new ChassisSpeeds(
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
index b0f23a8..f71465a 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
@@ -17,8 +17,12 @@
  * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
  * where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
  *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
  */
 public class LinearPlantInversionFeedforward<
     States extends Num, Inputs extends Num, Outputs extends Num> {
@@ -105,13 +109,13 @@
    *
    * @param initialState The initial state vector.
    */
-  public void reset(Matrix<States, N1> initialState) {
+  public final void reset(Matrix<States, N1> initialState) {
     m_r = initialState;
     m_uff.fill(0.0);
   }
 
   /** Resets the feedforward with a zero initial state vector. */
-  public void reset() {
+  public final void reset() {
     m_r.fill(0.0);
     m_uff.fill(0.0);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
index 658be69..660fa8f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
@@ -19,8 +19,12 @@
  * Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use
  * the control law u = K(r - x).
  *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
  */
 public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
   /** The current reference state. */
@@ -35,8 +39,8 @@
   /**
    * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
    * for how to select the tolerances.
    *
    * @param plant The plant being controlled.
@@ -61,8 +65,8 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
    * for how to select the tolerances.
    *
    * @param A Continuous system matrix of the plant being controlled.
@@ -107,14 +111,12 @@
     var discB = discABPair.getSecond();
 
     if (!StateSpaceUtil.isStabilizable(discA, discB)) {
-      var builder = new StringBuilder("The system passed to the LQR is uncontrollable!\n\nA =\n");
-      builder
-          .append(discA.getStorage().toString())
-          .append("\nB =\n")
-          .append(discB.getStorage().toString())
-          .append('\n');
-
-      var msg = builder.toString();
+      var msg =
+          "The system passed to the LQR is uncontrollable!\n\nA =\n"
+              + discA.getStorage().toString()
+              + "\nB =\n"
+              + discB.getStorage().toString()
+              + '\n';
       MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
       throw new IllegalArgumentException(msg);
     }
@@ -223,7 +225,7 @@
   }
 
   /** Resets the controller. */
-  public void reset() {
+  public final void reset() {
     m_r.fill(0.0);
     m_u.fill(0.0);
   }
@@ -258,8 +260,9 @@
    * are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we
    * can compute the control based on where the system will be after the time delay.
    *
-   * <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a
-   * derivation.
+   * <p>See <a
+   * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+   * appendix C.4 for a derivation.
    *
    * @param plant The plant being controlled.
    * @param dtSeconds Discretization timestep in seconds.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
index 1d98900..f724684 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -68,6 +68,9 @@
    * @param kp The proportional coefficient.
    * @param ki The integral coefficient.
    * @param kd The derivative coefficient.
+   * @throws IllegalArgumentException if kp &lt; 0
+   * @throws IllegalArgumentException if ki &lt; 0
+   * @throws IllegalArgumentException if kd &lt; 0
    */
   public PIDController(double kp, double ki, double kd) {
     this(kp, ki, kd, 0.02);
@@ -79,15 +82,29 @@
    * @param kp The proportional coefficient.
    * @param ki The integral coefficient.
    * @param kd The derivative coefficient.
-   * @param period The period between controller updates in seconds. Must be non-zero and positive.
+   * @param period The period between controller updates in seconds.
+   * @throws IllegalArgumentException if kp &lt; 0
+   * @throws IllegalArgumentException if ki &lt; 0
+   * @throws IllegalArgumentException if kd &lt; 0
+   * @throws IllegalArgumentException if period &lt;= 0
    */
+  @SuppressWarnings("this-escape")
   public PIDController(double kp, double ki, double kd, double period) {
     m_kp = kp;
     m_ki = ki;
     m_kd = kd;
 
-    if (period <= 0) {
-      throw new IllegalArgumentException("Controller period must be a non-zero positive number!");
+    if (kp < 0.0) {
+      throw new IllegalArgumentException("Kp must be a non-negative number!");
+    }
+    if (ki < 0.0) {
+      throw new IllegalArgumentException("Ki must be a non-negative number!");
+    }
+    if (kd < 0.0) {
+      throw new IllegalArgumentException("Kd must be a non-negative number!");
+    }
+    if (period <= 0.0) {
+      throw new IllegalArgumentException("Controller period must be a positive number!");
     }
     m_period = period;
 
@@ -120,7 +137,7 @@
   /**
    * Sets the Proportional coefficient of the PID controller gain.
    *
-   * @param kp proportional coefficient
+   * @param kp The proportional coefficient. Must be &gt;= 0.
    */
   public void setP(double kp) {
     m_kp = kp;
@@ -129,7 +146,7 @@
   /**
    * Sets the Integral coefficient of the PID controller gain.
    *
-   * @param ki integral coefficient
+   * @param ki The integral coefficient. Must be &gt;= 0.
    */
   public void setI(double ki) {
     m_ki = ki;
@@ -138,7 +155,7 @@
   /**
    * Sets the Differential coefficient of the PID controller gain.
    *
-   * @param kd differential coefficient
+   * @param kd The differential coefficient. Must be &gt;= 0.
    */
   public void setD(double kd) {
     m_kd = kd;
@@ -152,6 +169,7 @@
    * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
    *
    * @param iZone Maximum magnitude of error to allow integral control.
+   * @throws IllegalArgumentException if iZone &lt; 0
    */
   public void setIZone(double iZone) {
     if (iZone < 0) {
@@ -408,7 +426,16 @@
     builder.addDoubleProperty("p", this::getP, this::setP);
     builder.addDoubleProperty("i", this::getI, this::setI);
     builder.addDoubleProperty("d", this::getD, this::setD);
-    builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
+    builder.addDoubleProperty(
+        "izone",
+        this::getIZone,
+        (double toSet) -> {
+          try {
+            setIZone(toSet);
+          } catch (IllegalArgumentException e) {
+            MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace());
+          }
+        });
     builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
index 80311f5..e69c73f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -35,6 +35,9 @@
    * @param Ki The integral coefficient.
    * @param Kd The derivative coefficient.
    * @param constraints Velocity and acceleration constraints for goal.
+   * @throws IllegalArgumentException if kp &lt; 0
+   * @throws IllegalArgumentException if ki &lt; 0
+   * @throws IllegalArgumentException if kd &lt; 0
    */
   public ProfiledPIDController(
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
@@ -49,7 +52,12 @@
    * @param Kd The derivative coefficient.
    * @param constraints Velocity and acceleration constraints for goal.
    * @param period The period between controller updates in seconds. The default is 0.02 seconds.
+   * @throws IllegalArgumentException if kp &lt; 0
+   * @throws IllegalArgumentException if ki &lt; 0
+   * @throws IllegalArgumentException if kd &lt; 0
+   * @throws IllegalArgumentException if period &lt;= 0
    */
+  @SuppressWarnings("this-escape")
   public ProfiledPIDController(
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
     m_controller = new PIDController(Kp, Ki, Kd, period);
@@ -66,9 +74,9 @@
    *
    * <p>Sets the proportional, integral, and differential coefficients.
    *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
+   * @param Kp The proportional coefficient. Must be &gt;= 0.
+   * @param Ki The integral coefficient. Must be &gt;= 0.
+   * @param Kd The differential coefficient. Must be &gt;= 0.
    */
   public void setPID(double Kp, double Ki, double Kd) {
     m_controller.setPID(Kp, Ki, Kd);
@@ -77,7 +85,7 @@
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
-   * @param Kp proportional coefficient
+   * @param Kp The proportional coefficient. Must be &gt;= 0.
    */
   public void setP(double Kp) {
     m_controller.setP(Kp);
@@ -86,7 +94,7 @@
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
-   * @param Ki integral coefficient
+   * @param Ki The integral coefficient. Must be &gt;= 0.
    */
   public void setI(double Ki) {
     m_controller.setI(Ki);
@@ -95,7 +103,7 @@
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
-   * @param Kd differential coefficient
+   * @param Kd The differential coefficient. Must be &gt;= 0.
    */
   public void setD(double Kd) {
     m_controller.setD(Kd);
@@ -109,6 +117,7 @@
    * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
    *
    * @param iZone Maximum magnitude of error to allow integral control.
+   * @throws IllegalArgumentException if iZone &lt;= 0
    */
   public void setIZone(double iZone) {
     m_controller.setIZone(iZone);
@@ -347,7 +356,7 @@
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    m_setpoint = m_profile.calculate(getPeriod(), m_goal, m_setpoint);
+    m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal);
     return m_controller.calculate(measurement, m_setpoint.position);
   }
 
@@ -425,7 +434,16 @@
     builder.addDoubleProperty("p", this::getP, this::setP);
     builder.addDoubleProperty("i", this::getI, this::setI);
     builder.addDoubleProperty("d", this::getD, this::setD);
-    builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
+    builder.addDoubleProperty(
+        "izone",
+        this::getIZone,
+        (double toSet) -> {
+          try {
+            setIZone(toSet);
+          } catch (IllegalArgumentException e) {
+            MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace());
+          }
+        });
     builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
index 714d705..2b25bcc 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -4,14 +4,19 @@
 
 package edu.wpi.first.math.controller;
 
-import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 
 /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
 public class SimpleMotorFeedforward {
+  /** The static gain. */
   public final double ks;
+
+  /** The velocity gain. */
   public final double kv;
+
+  /** The acceleration gain. */
   public final double ka;
 
   /**
@@ -21,11 +26,19 @@
    * @param ks The static gain.
    * @param kv The velocity gain.
    * @param ka The acceleration gain.
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
    */
   public SimpleMotorFeedforward(double ks, double kv, double ka) {
     this.ks = ks;
     this.kv = kv;
     this.ka = ka;
+    if (kv < 0.0) {
+      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
+    }
+    if (ka < 0.0) {
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!");
+    }
   }
 
   /**
@@ -62,8 +75,8 @@
     var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
     var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
 
-    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
-    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+    var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
+    var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
 
     return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java
new file mode 100644
index 0000000..ef3953a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java
@@ -0,0 +1,40 @@
+// 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.
+
+package edu.wpi.first.math.controller.proto;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class ArmFeedforwardProto implements Protobuf<ArmFeedforward, ProtobufArmFeedforward> {
+  @Override
+  public Class<ArmFeedforward> getTypeClass() {
+    return ArmFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufArmFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufArmFeedforward createMessage() {
+    return ProtobufArmFeedforward.newInstance();
+  }
+
+  @Override
+  public ArmFeedforward unpack(ProtobufArmFeedforward msg) {
+    return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa());
+  }
+
+  @Override
+  public void pack(ProtobufArmFeedforward msg, ArmFeedforward value) {
+    msg.setKs(value.ks);
+    msg.setKg(value.kg);
+    msg.setKv(value.kv);
+    msg.setKa(value.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java
new file mode 100644
index 0000000..c22db5d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java
@@ -0,0 +1,40 @@
+// 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.
+
+package edu.wpi.first.math.controller.proto;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DifferentialDriveWheelVoltagesProto
+    implements Protobuf<DifferentialDriveWheelVoltages, ProtobufDifferentialDriveWheelVoltages> {
+  @Override
+  public Class<DifferentialDriveWheelVoltages> getTypeClass() {
+    return DifferentialDriveWheelVoltages.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveWheelVoltages.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveWheelVoltages createMessage() {
+    return ProtobufDifferentialDriveWheelVoltages.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveWheelVoltages unpack(ProtobufDifferentialDriveWheelVoltages msg) {
+    return new DifferentialDriveWheelVoltages(msg.getLeft(), msg.getRight());
+  }
+
+  @Override
+  public void pack(
+      ProtobufDifferentialDriveWheelVoltages msg, DifferentialDriveWheelVoltages value) {
+    msg.setLeft(value.left);
+    msg.setRight(value.right);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java
new file mode 100644
index 0000000..66399e1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java
@@ -0,0 +1,41 @@
+// 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.
+
+package edu.wpi.first.math.controller.proto;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class ElevatorFeedforwardProto
+    implements Protobuf<ElevatorFeedforward, ProtobufElevatorFeedforward> {
+  @Override
+  public Class<ElevatorFeedforward> getTypeClass() {
+    return ElevatorFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufElevatorFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufElevatorFeedforward createMessage() {
+    return ProtobufElevatorFeedforward.newInstance();
+  }
+
+  @Override
+  public ElevatorFeedforward unpack(ProtobufElevatorFeedforward msg) {
+    return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa());
+  }
+
+  @Override
+  public void pack(ProtobufElevatorFeedforward msg, ElevatorFeedforward value) {
+    msg.setKs(value.ks);
+    msg.setKg(value.kg);
+    msg.setKv(value.kv);
+    msg.setKa(value.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java
new file mode 100644
index 0000000..de72fea
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java
@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {
+  @Override
+  public Class<ArmFeedforward> getTypeClass() {
+    return ArmFeedforward.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:ArmFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double ks;double kg;double kv;double ka";
+  }
+
+  @Override
+  public ArmFeedforward unpack(ByteBuffer bb) {
+    double ks = bb.getDouble();
+    double kg = bb.getDouble();
+    double kv = bb.getDouble();
+    double ka = bb.getDouble();
+    return new ArmFeedforward(ks, kg, kv, ka);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, ArmFeedforward value) {
+    bb.putDouble(value.ks);
+    bb.putDouble(value.kg);
+    bb.putDouble(value.kv);
+    bb.putDouble(value.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java
new file mode 100644
index 0000000..1695d81
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java
@@ -0,0 +1,45 @@
+// 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.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DifferentialDriveWheelVoltagesStruct
+    implements Struct<DifferentialDriveWheelVoltages> {
+  @Override
+  public Class<DifferentialDriveWheelVoltages> getTypeClass() {
+    return DifferentialDriveWheelVoltages.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:DifferentialDriveWheelVoltages";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double left;double right";
+  }
+
+  @Override
+  public DifferentialDriveWheelVoltages unpack(ByteBuffer bb) {
+    double left = bb.getDouble();
+    double right = bb.getDouble();
+    return new DifferentialDriveWheelVoltages(left, right);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveWheelVoltages value) {
+    bb.putDouble(value.left);
+    bb.putDouble(value.right);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java
new file mode 100644
index 0000000..f8b5559
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java
@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {
+  @Override
+  public Class<ElevatorFeedforward> getTypeClass() {
+    return ElevatorFeedforward.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:ElevatorFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double ks;double kg;double kv;double ka";
+  }
+
+  @Override
+  public ElevatorFeedforward unpack(ByteBuffer bb) {
+    double ks = bb.getDouble();
+    double kg = bb.getDouble();
+    double kv = bb.getDouble();
+    double ka = bb.getDouble();
+    return new ElevatorFeedforward(ks, kg, kv, ka);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, ElevatorFeedforward value) {
+    bb.putDouble(value.ks);
+    bb.putDouble(value.kg);
+    bb.putDouble(value.kv);
+    bb.putDouble(value.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
index 0f91a1d..8a61526 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
@@ -11,10 +11,10 @@
 import java.util.function.BiFunction;
 import org.ejml.simple.SimpleMatrix;
 
+/** Angle statistics functions. */
 public final class AngleStatistics {
-  private AngleStatistics() {
-    // Utility class
-  }
+  /** Utility class. */
+  private AngleStatistics() {}
 
   /**
    * Subtracts a and b while normalizing the resulting value in the selected row as if it were an
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
index 59323b2..055bf99 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
@@ -30,9 +30,13 @@
  * error covariance by linearizing the models around the state estimate, then applying the linear
  * Kalman filter equations.
  *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
- * theory".
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
  */
 public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
     implements KalmanTypeFilter<States, Inputs, Outputs> {
@@ -59,8 +63,8 @@
   /**
    * Constructs an extended Kalman filter.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
    * for how to select the standard deviations.
    *
    * @param states a Nat representing the number of states.
@@ -97,8 +101,8 @@
   /**
    * Constructs an extended Kalman filter.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
    * for how to select the standard deviations.
    *
    * @param states a Nat representing the number of states.
@@ -236,7 +240,7 @@
   }
 
   @Override
-  public void reset() {
+  public final void reset() {
     m_xHat = new Matrix<>(m_states, Nat.N1());
     m_P = m_initP;
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
index 08e3270..9e28625 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
@@ -25,26 +25,32 @@
  * amount of the difference between the actual measurements and the measurements predicted by the
  * model.
  *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
- * theory".
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
  */
-public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements KalmanTypeFilter<States, Inputs, Outputs> {
   private final Nat<States> m_states;
 
   private final LinearSystem<States, Inputs, Outputs> m_plant;
-
-  /** The steady-state Kalman gain matrix. */
-  private final Matrix<States, Outputs> m_K;
-
-  /** The state estimate. */
   private Matrix<States, N1> m_xHat;
+  private Matrix<States, States> m_P;
+  private final Matrix<States, States> m_contQ;
+  private final Matrix<Outputs, Outputs> m_contR;
+  private double m_dtSeconds;
+
+  private final Matrix<States, States> m_initP;
 
   /**
-   * Constructs a state-space observer with the given plant.
+   * Constructs a Kalman filter with the given plant.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
    * for how to select the standard deviations.
    *
    * @param states A Nat representing the states of the system.
@@ -66,35 +72,163 @@
 
     this.m_plant = plant;
 
-    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_dtSeconds = dtSeconds;
 
-    var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
+    // Find discrete A and Q
+    var pair = Discretization.discretizeAQ(plant.getA(), m_contQ, dtSeconds);
     var discA = pair.getFirst();
     var discQ = pair.getSecond();
 
-    var discR = Discretization.discretizeR(contR, dtSeconds);
+    var discR = Discretization.discretizeR(m_contR, dtSeconds);
 
     var C = plant.getC();
 
     if (!StateSpaceUtil.isDetectable(discA, C)) {
-      var builder =
-          new StringBuilder("The system passed to the Kalman filter is unobservable!\n\nA =\n");
-      builder
-          .append(discA.getStorage().toString())
-          .append("\nC =\n")
-          .append(C.getStorage().toString())
-          .append('\n');
-
-      var msg = builder.toString();
+      var msg =
+          "The system passed to the Kalman filter is unobservable!\n\nA =\n"
+              + discA.getStorage().toString()
+              + "\nC =\n"
+              + C.getStorage().toString()
+              + '\n';
       MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
       throw new IllegalArgumentException(msg);
     }
 
-    var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
+    m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
 
-    // S = CPCᵀ + R
-    var S = C.times(P).times(C.transpose()).plus(discR);
+    reset();
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_P;
+  }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param row Row of P.
+   * @param col Column of P.
+   * @return the value of the error covariance matrix P at (i, j).
+   */
+  @Override
+  public double getP(int row, int col) {
+    return m_P.get(row, col);
+  }
+
+  /**
+   * Sets the entire error covariance matrix P.
+   *
+   * @param newP The new value of P to use.
+   */
+  @Override
+  public void setP(Matrix<States, States> newP) {
+    m_P = newP;
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return the state estimate x-hat.
+   */
+  @Override
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the value of the state estimate x-hat at that row.
+   */
+  @Override
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  @Override
+  public void setXhat(Matrix<States, N1> xHat) {
+    m_xHat = xHat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  @Override
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  @Override
+  public final void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_P = m_initP;
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    // Find discrete A and Q
+    final var discPair = Discretization.discretizeAQ(m_plant.getA(), m_contQ, dtSeconds);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
+
+    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+    m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
+
+    m_dtSeconds = dtSeconds;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  @Override
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    correct(u, y, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    final var C = m_plant.getC();
+    final var D = m_plant.getD();
+
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
+
+    final var S = C.times(m_P).times(C.transpose()).plus(discR);
 
     // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
     // efficiently.
@@ -108,95 +242,18 @@
     //
     // Kᵀ = Sᵀ.solve(CPᵀ)
     // K = (Sᵀ.solve(CPᵀ))ᵀ
-    m_K =
-        new Matrix<>(
-            S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose());
+    final Matrix<States, Outputs> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
 
-    reset();
-  }
-
-  public void reset() {
-    m_xHat = new Matrix<>(m_states, Nat.N1());
-  }
-
-  /**
-   * Returns the steady-state Kalman gain matrix K.
-   *
-   * @return The steady-state Kalman gain matrix K.
-   */
-  public Matrix<States, Outputs> getK() {
-    return m_K;
-  }
-
-  /**
-   * Returns an element of the steady-state Kalman gain matrix K.
-   *
-   * @param row Row of K.
-   * @param col Column of K.
-   * @return the element (i, j) of the steady-state Kalman gain matrix K.
-   */
-  public double getK(int row, int col) {
-    return m_K.get(row, col);
-  }
-
-  /**
-   * Set initial state estimate x-hat.
-   *
-   * @param xhat The state estimate x-hat.
-   */
-  public void setXhat(Matrix<States, N1> xhat) {
-    this.m_xHat = xhat;
-  }
-
-  /**
-   * Set an element of the initial state estimate x-hat.
-   *
-   * @param row Row of x-hat.
-   * @param value Value for element of x-hat.
-   */
-  public void setXhat(int row, double value) {
-    m_xHat.set(row, 0, value);
-  }
-
-  /**
-   * Returns the state estimate x-hat.
-   *
-   * @return The state estimate x-hat.
-   */
-  public Matrix<States, N1> getXhat() {
-    return m_xHat;
-  }
-
-  /**
-   * Returns an element of the state estimate x-hat.
-   *
-   * @param row Row of x-hat.
-   * @return the state estimate x-hat at that row.
-   */
-  public double getXhat(int row) {
-    return m_xHat.get(row, 0);
-  }
-
-  /**
-   * Project the model into the future with a new control input u.
-   *
-   * @param u New control input from controller.
-   * @param dtSeconds Timestep for prediction.
-   */
-  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
-    this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
-  }
-
-  /**
-   * Correct the state estimate x-hat using the measurements in y.
-   *
-   * @param u Same control input used in the last predict step.
-   * @param y Measurement vector.
-   */
-  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
-    final var C = m_plant.getC();
-    final var D = m_plant.getD();
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
-    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+    m_xHat = m_xHat.plus(K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+
+    // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+    // Use Joseph form for numerical stability
+    m_P =
+        Matrix.eye(m_states)
+            .minus(K.times(C))
+            .times(m_P)
+            .times(Matrix.eye(m_states).minus(K.times(C)).transpose())
+            .plus(K.times(discR).times(K.transpose()));
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
index 15e7bd4..ad4db52 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
@@ -13,11 +13,19 @@
 import java.util.Map;
 import java.util.function.BiConsumer;
 
+/**
+ * This class incorporates time-delayed measurements into a Kalman filter's state estimate.
+ *
+ * @param <S> The number of states.
+ * @param <I> The number of inputs.
+ * @param <O> The number of outputs.
+ */
 public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O extends Num> {
   private static final int kMaxPastObserverStates = 300;
 
   private final List<Map.Entry<Double, ObserverSnapshot>> m_pastObserverSnapshots;
 
+  /** Default constructor. */
   KalmanFilterLatencyCompensator() {
     m_pastObserverSnapshots = new ArrayList<>();
   }
@@ -164,9 +172,16 @@
 
   /** This class contains all the information about our observer at a given time. */
   public class ObserverSnapshot {
+    /** The state estimate. */
     public final Matrix<S, N1> xHat;
+
+    /** The error covariance. */
     public final Matrix<S, S> errorCovariances;
+
+    /** The inputs. */
     public final Matrix<I, N1> inputs;
+
+    /** The local measurements. */
     public final Matrix<O, N1> localMeasurements;
 
     private ObserverSnapshot(
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
index 7b14839..ff9856c 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
@@ -8,24 +8,83 @@
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 
+/**
+ * Interface for Kalman filters for use with KalmanFilterLatencyCompensator.
+ *
+ * @param <States> The number of states.
+ * @param <Inputs> The number of inputs.
+ * @param <Outputs> The number of outputs.
+ */
 public interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+  /**
+   * Returns the error covariance matrix.
+   *
+   * @return The error covariance matrix.
+   */
   Matrix<States, States> getP();
 
+  /**
+   * Returns an element of the error covariance matrix.
+   *
+   * @param i The row.
+   * @param j The column.
+   * @return An element of the error covariance matrix.
+   */
   double getP(int i, int j);
 
+  /**
+   * Sets the error covariance matrix.
+   *
+   * @param newP The error covariance matrix.
+   */
   void setP(Matrix<States, States> newP);
 
+  /**
+   * Returns the state estimate.
+   *
+   * @return The state estimate.
+   */
   Matrix<States, N1> getXhat();
 
+  /**
+   * Returns an element of the state estimate.
+   *
+   * @param i The row.
+   * @return An element of the state estimate.
+   */
   double getXhat(int i);
 
+  /**
+   * Sets the state estimate.
+   *
+   * @param xHat The state estimate.
+   */
   void setXhat(Matrix<States, N1> xHat);
 
+  /**
+   * Sets an element of the state estimate.
+   *
+   * @param i The row.
+   * @param value The value.
+   */
   void setXhat(int i, double value);
 
+  /** Resets the observer. */
   void reset();
 
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
   void predict(Matrix<Inputs, N1> u, double dtSeconds);
 
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
   void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
index 24835f6..24fa771 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
@@ -21,6 +21,8 @@
  *
  * <p>[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic
  * State-Space Models" (Doctoral dissertation)
+ *
+ * @param <S> The dimensionality of the state. 2 * States + 1 weights will be generated.
  */
 public class MerweScaledSigmaPoints<S extends Num> {
   private final double m_alpha;
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
index bb30f25..14eee8f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -34,6 +34,8 @@
  *
  * <p>{@link PoseEstimator#addVisionMeasurement} can be called as infrequently as you want; if you
  * never call it then this class will behave exactly like regular encoder odometry.
+ *
+ * @param <T> Wheel positions type.
  */
 public class PoseEstimator<T extends WheelPositions<T>> {
   private final Kinematics<?, T> m_kinematics;
@@ -80,7 +82,7 @@
    *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
    *     theta]ᵀ, with units in meters and radians.
    */
-  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+  public final void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
     var r = new double[3];
     for (int i = 0; i < 3; ++i) {
       r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
new file mode 100644
index 0000000..0f2e4a5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
@@ -0,0 +1,208 @@
+// 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.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.LinearSystem;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
+ * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
+ * of squares error in the state estimate. This K gain is used to correct the state estimate by some
+ * amount of the difference between the actual measurements and the measurements predicted by the
+ * model.
+ *
+ * <p>This class assumes predict() and correct() are called in pairs, so the Kalman gain converges
+ * to a steady-state value. If they aren't, use {@link KalmanFilter} instead.
+ *
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+  private final Nat<States> m_states;
+
+  private final LinearSystem<States, Inputs, Outputs> m_plant;
+
+  /** The steady-state Kalman gain matrix. */
+  private final Matrix<States, Outputs> m_K;
+
+  /** The state estimate. */
+  private Matrix<States, N1> m_xHat;
+
+  /**
+   * Constructs a steady-state Kalman filter with the given plant.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states A Nat representing the states of the system.
+   * @param outputs A Nat representing the outputs of the system.
+   * @param plant The plant used for the prediction step.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds Nominal discretization timestep.
+   * @throws IllegalArgumentException If the system is unobservable.
+   */
+  public SteadyStateKalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      LinearSystem<States, Inputs, Outputs> plant,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dtSeconds) {
+    this.m_states = states;
+
+    this.m_plant = plant;
+
+    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
+    var discA = pair.getFirst();
+    var discQ = pair.getSecond();
+
+    var discR = Discretization.discretizeR(contR, dtSeconds);
+
+    var C = plant.getC();
+
+    if (!StateSpaceUtil.isDetectable(discA, C)) {
+      var msg =
+          "The system passed to the Kalman filter is unobservable!\n\nA =\n"
+              + discA.getStorage().toString()
+              + "\nC =\n"
+              + C.getStorage().toString()
+              + '\n';
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
+
+    // S = CPCᵀ + R
+    var S = C.times(P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    m_K =
+        new Matrix<>(
+            S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose());
+
+    reset();
+  }
+
+  /** Resets the observer. */
+  public final void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+  }
+
+  /**
+   * Returns the steady-state Kalman gain matrix K.
+   *
+   * @return The steady-state Kalman gain matrix K.
+   */
+  public Matrix<States, Outputs> getK() {
+    return m_K;
+  }
+
+  /**
+   * Returns an element of the steady-state Kalman gain matrix K.
+   *
+   * @param row Row of K.
+   * @param col Column of K.
+   * @return the element (i, j) of the steady-state Kalman gain matrix K.
+   */
+  public double getK(int row, int col) {
+    return m_K.get(row, col);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xhat The state estimate x-hat.
+   */
+  public void setXhat(Matrix<States, N1> xhat) {
+    this.m_xHat = xhat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return The state estimate x-hat.
+   */
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the state estimate x-hat at that row.
+   */
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the last predict step.
+   * @param y Measurement vector.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    final var C = m_plant.getC();
+    final var D = m_plant.getD();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
index a5de856..c64ec85 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -31,12 +31,17 @@
  * <p>An unscented Kalman filter uses nonlinear state and measurement models. It propagates the
  * error covariance using sigma points chosen to approximate the true probability distribution.
  *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
- * theory".
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
  *
  * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
- * information about the SR-UKF, see https://www.researchgate.net/publication/3908304.
+ * information about the SR-UKF, see <a
+ * href="https://www.researchgate.net/publication/3908304">https://www.researchgate.net/publication/3908304</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
  */
 public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
     implements KalmanTypeFilter<States, Inputs, Outputs> {
@@ -64,8 +69,8 @@
   /**
    * Constructs an Unscented Kalman Filter.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
    * for how to select the standard deviations.
    *
    * @param states A Nat representing the number of states.
@@ -104,8 +109,8 @@
    * custom functions for arithmetic can be useful if you have angles in the state or measurements,
    * because they allow you to correctly account for the modular nature of angle arithmetic.
    *
-   * <p>See
-   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
    * for how to select the standard deviations.
    *
    * @param states A Nat representing the number of states.
@@ -206,7 +211,7 @@
     var qrStorage = Sbar.transpose().getStorage();
 
     if (!qr.decompose(qrStorage.getDDRM())) {
-      throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage.toString());
+      throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage);
     }
 
     Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
@@ -322,7 +327,7 @@
 
   /** Resets the observer. */
   @Override
-  public void reset() {
+  public final void reset() {
     m_xHat = new Matrix<>(m_states, Nat.N1());
     m_S = new Matrix<>(m_states, m_states);
     m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
index 3472198..c113d96 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
@@ -11,9 +11,13 @@
  * baseline for a specified period of time before the filtered value changes.
  */
 public class Debouncer {
+  /** Type of debouncing to perform. */
   public enum DebounceType {
+    /** Rising edge. */
     kRising,
+    /** Falling edge. */
     kFalling,
+    /** Both rising and falling edges. */
     kBoth
   }
 
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
index 98dd4e1..c26270f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -6,7 +6,7 @@
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.util.CircularBuffer;
+import edu.wpi.first.util.DoubleCircularBuffer;
 import java.util.Arrays;
 import org.ejml.simple.SimpleMatrix;
 
@@ -35,9 +35,13 @@
  * PID controller out of this class!
  *
  * <p>For more on filters, we highly recommend the following articles:<br>
- * https://en.wikipedia.org/wiki/Linear_filter<br>
- * https://en.wikipedia.org/wiki/Iir_filter<br>
- * https://en.wikipedia.org/wiki/Fir_filter<br>
+ * <a
+ * href="https://en.wikipedia.org/wiki/Linear_filter">https://en.wikipedia.org/wiki/Linear_filter</a>
+ * <br>
+ * <a href="https://en.wikipedia.org/wiki/Iir_filter">https://en.wikipedia.org/wiki/Iir_filter</a>
+ * <br>
+ * <a href="https://en.wikipedia.org/wiki/Fir_filter">https://en.wikipedia.org/wiki/Fir_filter</a>
+ * <br>
  *
  * <p>Note 1: calculate() should be called by the user on a known, regular period. You can use a
  * Notifier for this or do it "inline" with code in a periodic function.
@@ -48,8 +52,8 @@
  * to make sure calculate() gets called at the desired, constant frequency!
  */
 public class LinearFilter {
-  private final CircularBuffer m_inputs;
-  private final CircularBuffer m_outputs;
+  private final DoubleCircularBuffer m_inputs;
+  private final DoubleCircularBuffer m_outputs;
   private final double[] m_inputGains;
   private final double[] m_outputGains;
 
@@ -62,8 +66,8 @@
    * @param fbGains The "feedback" or IIR gains.
    */
   public LinearFilter(double[] ffGains, double[] fbGains) {
-    m_inputs = new CircularBuffer(ffGains.length);
-    m_outputs = new CircularBuffer(fbGains.length);
+    m_inputs = new DoubleCircularBuffer(ffGains.length);
+    m_outputs = new DoubleCircularBuffer(fbGains.length);
     m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
     m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
 
@@ -243,6 +247,43 @@
   }
 
   /**
+   * Resets the filter state, initializing internal buffers to the provided values.
+   *
+   * <p>These are the expected lengths of the buffers, depending on what type of linear filter used:
+   *
+   * <table>
+   * <tr><th>Type</th><th>Input Buffer Length</th><th>Output Buffer Length</th></tr>
+   * <tr><td>Unspecified</td><td>length of {@code ffGains}</td><td>length of {@code fbGains}</td>
+   * </tr>
+   * <tr><td>Single Pole IIR</td><td>1</td><td>1</td></tr>
+   * <tr><td>High-Pass</td><td>2</td><td>1</td></tr>
+   * <tr><td>Moving Average</td><td>{@code taps}</td><td>0</td></tr>
+   * <tr><td>Finite Difference</td><td>length of {@code stencil}</td><td>0</td></tr>
+   * <tr><td>Backward Finite Difference</td><td>{@code samples}</td><td>0</td></tr>
+   * </table>
+   *
+   * @param inputBuffer Values to initialize input buffer.
+   * @param outputBuffer Values to initialize output buffer.
+   * @throws IllegalArgumentException if length of inputBuffer or outputBuffer does not match the
+   *     length of ffGains and fbGains provided in the constructor.
+   */
+  public void reset(double[] inputBuffer, double[] outputBuffer) {
+    // Clear buffers
+    reset();
+
+    if (inputBuffer.length != m_inputGains.length || outputBuffer.length != m_outputGains.length) {
+      throw new IllegalArgumentException("Incorrect length of inputBuffer or outputBuffer");
+    }
+
+    for (double input : inputBuffer) {
+      m_inputs.addFirst(input);
+    }
+    for (double output : outputBuffer) {
+      m_outputs.addFirst(output);
+    }
+  }
+
+  /**
    * Calculates the next value of the filter.
    *
    * @param input Current input value.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
index c24f6e9..13c4e10 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.filter;
 
-import edu.wpi.first.util.CircularBuffer;
+import edu.wpi.first.util.DoubleCircularBuffer;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
@@ -15,7 +15,7 @@
  * processing, LIDAR, or ultrasonic sensors).
  */
 public class MedianFilter {
-  private final CircularBuffer m_valueBuffer;
+  private final DoubleCircularBuffer m_valueBuffer;
   private final List<Double> m_orderedValues;
   private final int m_size;
 
@@ -26,7 +26,7 @@
    */
   public MedianFilter(int size) {
     // Circular buffer of values currently in the window, ordered by time
-    m_valueBuffer = new CircularBuffer(size);
+    m_valueBuffer = new DoubleCircularBuffer(size);
     // List of values currently in the window, ordered by value
     m_orderedValues = new ArrayList<>(size);
     // Size of rolling window
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
index bc7f536..97e8307 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
@@ -4,25 +4,28 @@
 
 package edu.wpi.first.math.geometry;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.geometry.proto.Pose2dProto;
+import edu.wpi.first.math.geometry.struct.Pose2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Collections;
 import java.util.Comparator;
 import java.util.List;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a 2D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose2d implements Interpolatable<Pose2d> {
+public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable {
   private final Translation2d m_translation;
   private final Rotation2d m_rotation;
 
@@ -59,6 +62,18 @@
   }
 
   /**
+   * Constructs a pose with x and y translations instead of a separate Translation2d. The X and Y
+   * translations will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  public Pose2d(Measure<Distance> x, Measure<Distance> y, Rotation2d rotation) {
+    this(x.in(Meters), y.in(Meters), rotation);
+  }
+
+  /**
    * Transforms the pose by the given transformation and returns the new transformed pose.
    *
    * <pre>
@@ -311,82 +326,9 @@
     }
   }
 
-  public static final class AStruct implements Struct<Pose2d> {
-    @Override
-    public Class<Pose2d> getTypeClass() {
-      return Pose2d.class;
-    }
+  /** Pose2d protobuf for serialization. */
+  public static final Pose2dProto proto = new Pose2dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Pose2d";
-    }
-
-    @Override
-    public int getSize() {
-      return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
-    }
-
-    @Override
-    public String getSchema() {
-      return "Translation2d translation;Rotation2d rotation";
-    }
-
-    @Override
-    public Struct<?>[] getNested() {
-      return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
-    }
-
-    @Override
-    public Pose2d unpack(ByteBuffer bb) {
-      Translation2d translation = Translation2d.struct.unpack(bb);
-      Rotation2d rotation = Rotation2d.struct.unpack(bb);
-      return new Pose2d(translation, rotation);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Pose2d value) {
-      Translation2d.struct.pack(bb, value.m_translation);
-      Rotation2d.struct.pack(bb, value.m_rotation);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Pose2d, ProtobufPose2d> {
-    @Override
-    public Class<Pose2d> getTypeClass() {
-      return Pose2d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufPose2d.getDescriptor();
-    }
-
-    @Override
-    public Protobuf<?, ?>[] getNested() {
-      return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
-    }
-
-    @Override
-    public ProtobufPose2d createMessage() {
-      return ProtobufPose2d.newInstance();
-    }
-
-    @Override
-    public Pose2d unpack(ProtobufPose2d msg) {
-      return new Pose2d(
-          Translation2d.proto.unpack(msg.getTranslation()),
-          Rotation2d.proto.unpack(msg.getRotation()));
-    }
-
-    @Override
-    public void pack(ProtobufPose2d msg, Pose2d value) {
-      Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
-      Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Pose2d struct for serialization. */
+  public static final Pose2dStruct struct = new Pose2dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
index 2ea7094..282420f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
@@ -9,18 +9,17 @@
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.math.geometry.proto.Pose3dProto;
+import edu.wpi.first.math.geometry.struct.Pose3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a 3D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose3d implements Interpolatable<Pose3d> {
+public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable {
   private final Translation3d m_translation;
   private final Rotation3d m_rotation;
 
@@ -324,82 +323,9 @@
     }
   }
 
-  public static final class AStruct implements Struct<Pose3d> {
-    @Override
-    public Class<Pose3d> getTypeClass() {
-      return Pose3d.class;
-    }
+  /** Pose3d protobuf for serialization. */
+  public static final Pose3dProto proto = new Pose3dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Pose3d";
-    }
-
-    @Override
-    public int getSize() {
-      return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
-    }
-
-    @Override
-    public String getSchema() {
-      return "Translation3d translation;Rotation3d rotation";
-    }
-
-    @Override
-    public Struct<?>[] getNested() {
-      return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
-    }
-
-    @Override
-    public Pose3d unpack(ByteBuffer bb) {
-      Translation3d translation = Translation3d.struct.unpack(bb);
-      Rotation3d rotation = Rotation3d.struct.unpack(bb);
-      return new Pose3d(translation, rotation);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Pose3d value) {
-      Translation3d.struct.pack(bb, value.m_translation);
-      Rotation3d.struct.pack(bb, value.m_rotation);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Pose3d, ProtobufPose3d> {
-    @Override
-    public Class<Pose3d> getTypeClass() {
-      return Pose3d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufPose3d.getDescriptor();
-    }
-
-    @Override
-    public Protobuf<?, ?>[] getNested() {
-      return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
-    }
-
-    @Override
-    public ProtobufPose3d createMessage() {
-      return ProtobufPose3d.newInstance();
-    }
-
-    @Override
-    public Pose3d unpack(ProtobufPose3d msg) {
-      return new Pose3d(
-          Translation3d.proto.unpack(msg.getTranslation()),
-          Rotation3d.proto.unpack(msg.getRotation()));
-    }
-
-    @Override
-    public void pack(ProtobufPose3d msg, Pose3d value) {
-      Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
-      Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Pose3d struct for serialization. */
+  public static final Pose3dStruct struct = new Pose3dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
index 23fd26b..ea0be3c 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
@@ -10,17 +10,17 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.proto.QuaternionProto;
+import edu.wpi.first.math.geometry.struct.QuaternionStruct;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
+/** Represents a quaternion. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Quaternion {
+public class Quaternion implements ProtobufSerializable, StructSerializable {
   // Scalar r in versor form
   private final double m_w;
 
@@ -406,73 +406,9 @@
     return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
   }
 
-  public static final class AStruct implements Struct<Quaternion> {
-    @Override
-    public Class<Quaternion> getTypeClass() {
-      return Quaternion.class;
-    }
+  /** Quaternion protobuf for serialization. */
+  public static final QuaternionProto proto = new QuaternionProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Quaternion";
-    }
-
-    @Override
-    public int getSize() {
-      return kSizeDouble * 4;
-    }
-
-    @Override
-    public String getSchema() {
-      return "double w;double x;double y;double z";
-    }
-
-    @Override
-    public Quaternion unpack(ByteBuffer bb) {
-      double w = bb.getDouble();
-      double x = bb.getDouble();
-      double y = bb.getDouble();
-      double z = bb.getDouble();
-      return new Quaternion(w, x, y, z);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Quaternion value) {
-      bb.putDouble(value.getW());
-      bb.putDouble(value.getX());
-      bb.putDouble(value.getY());
-      bb.putDouble(value.getZ());
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Quaternion, ProtobufQuaternion> {
-    @Override
-    public Class<Quaternion> getTypeClass() {
-      return Quaternion.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufQuaternion.getDescriptor();
-    }
-
-    @Override
-    public ProtobufQuaternion createMessage() {
-      return ProtobufQuaternion.newInstance();
-    }
-
-    @Override
-    public Quaternion unpack(ProtobufQuaternion msg) {
-      return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ());
-    }
-
-    @Override
-    public void pack(ProtobufQuaternion msg, Quaternion value) {
-      msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ());
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Quaternion struct for serialization. */
+  public static final QuaternionStruct struct = new QuaternionStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 0556669..1106a10 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -4,19 +4,22 @@
 
 package edu.wpi.first.math.geometry;
 
+import static edu.wpi.first.units.Units.Radians;
+
 import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.proto.Rotation2dProto;
+import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
@@ -27,7 +30,8 @@
  */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Rotation2d implements Interpolatable<Rotation2d> {
+public class Rotation2d
+    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
   private final double m_value;
   private final double m_cos;
   private final double m_sin;
@@ -70,6 +74,15 @@
   }
 
   /**
+   * Constructs a Rotation2d with the given angle.
+   *
+   * @param angle The angle of the rotation.
+   */
+  public Rotation2d(Measure<Angle> angle) {
+    this(angle.in(Radians));
+  }
+
+  /**
    * Constructs and returns a Rotation2d with the given radian value.
    *
    * @param radians The value of the angle in radians.
@@ -262,66 +275,9 @@
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
-  public static final class AStruct implements Struct<Rotation2d> {
-    @Override
-    public Class<Rotation2d> getTypeClass() {
-      return Rotation2d.class;
-    }
+  /** Rotation2d protobuf for serialization. */
+  public static final Rotation2dProto proto = new Rotation2dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Rotation2d";
-    }
-
-    @Override
-    public int getSize() {
-      return kSizeDouble;
-    }
-
-    @Override
-    public String getSchema() {
-      return "double value";
-    }
-
-    @Override
-    public Rotation2d unpack(ByteBuffer bb) {
-      return new Rotation2d(bb.getDouble());
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Rotation2d value) {
-      bb.putDouble(value.m_value);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Rotation2d, ProtobufRotation2d> {
-    @Override
-    public Class<Rotation2d> getTypeClass() {
-      return Rotation2d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufRotation2d.getDescriptor();
-    }
-
-    @Override
-    public ProtobufRotation2d createMessage() {
-      return ProtobufRotation2d.newInstance();
-    }
-
-    @Override
-    public Rotation2d unpack(ProtobufRotation2d msg) {
-      return new Rotation2d(msg.getValue());
-    }
-
-    @Override
-    public void pack(ProtobufRotation2d msg, Rotation2d value) {
-      msg.setValue(value.m_value);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Rotation2d struct for serialization. */
+  public static final Rotation2dStruct struct = new Rotation2dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
index b434523..33fd34a 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
@@ -8,27 +8,26 @@
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.proto.Rotation3dProto;
+import edu.wpi.first.math.geometry.struct.Rotation3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** A rotation in a 3D coordinate frame represented by a quaternion. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Rotation3d implements Interpolatable<Rotation3d> {
+public class Rotation3d
+    implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable {
   private final Quaternion m_q;
 
   /** Constructs a Rotation3d with a default angle of 0 degrees. */
@@ -121,19 +120,15 @@
     // Require that the rotation matrix is special orthogonal. This is true if
     // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
     if (R.times(R.transpose()).minus(Matrix.eye(Nat.N3())).normF() > 1e-9) {
-      var builder = new StringBuilder("Rotation matrix isn't orthogonal\n\nR =\n");
-      builder.append(R.getStorage().toString()).append('\n');
-
-      var msg = builder.toString();
+      var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n';
       MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
       throw new IllegalArgumentException(msg);
     }
     if (Math.abs(R.det() - 1.0) > 1e-9) {
-      var builder =
-          new StringBuilder("Rotation matrix is orthogonal but not special orthogonal\n\nR =\n");
-      builder.append(R.getStorage().toString()).append('\n');
-
-      var msg = builder.toString();
+      var msg =
+          "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"
+              + R.getStorage().toString()
+              + '\n';
       MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
       throw new IllegalArgumentException(msg);
     }
@@ -195,15 +190,12 @@
       // If the dot product is 1, the two vectors point in the same direction so
       // there's no rotation. The default initialization of m_q will work.
       m_q = new Quaternion();
-      return;
     } else if (dotNorm < -1.0 + 1E-9) {
       // If the dot product is -1, the two vectors point in opposite directions
       // so a 180 degree rotation is required. Any orthogonal vector can be used
       // for it. Q in the QR decomposition is an orthonormal basis, so it
       // contains orthogonal unit vectors.
-      var X =
-          new MatBuilder<>(Nat.N3(), Nat.N1())
-              .fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
+      var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
       final var qr = DecompositionFactory_DDRM.qr(3, 1);
       qr.decompose(X.getStorage().getMatrix());
       final var Q = qr.getQ(null, false);
@@ -440,76 +432,9 @@
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
-  public static final class AStruct implements Struct<Rotation3d> {
-    @Override
-    public Class<Rotation3d> getTypeClass() {
-      return Rotation3d.class;
-    }
+  /** Rotation3d protobuf for serialization. */
+  public static final Rotation3dProto proto = new Rotation3dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Rotation3d";
-    }
-
-    @Override
-    public int getSize() {
-      return Quaternion.struct.getSize();
-    }
-
-    @Override
-    public String getSchema() {
-      return "Quaternion q";
-    }
-
-    @Override
-    public Struct<?>[] getNested() {
-      return new Struct<?>[] {Quaternion.struct};
-    }
-
-    @Override
-    public Rotation3d unpack(ByteBuffer bb) {
-      return new Rotation3d(Quaternion.struct.unpack(bb));
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Rotation3d value) {
-      Quaternion.struct.pack(bb, value.m_q);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Rotation3d, ProtobufRotation3d> {
-    @Override
-    public Class<Rotation3d> getTypeClass() {
-      return Rotation3d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufRotation3d.getDescriptor();
-    }
-
-    @Override
-    public Protobuf<?, ?>[] getNested() {
-      return new Protobuf<?, ?>[] {Quaternion.proto};
-    }
-
-    @Override
-    public ProtobufRotation3d createMessage() {
-      return ProtobufRotation3d.newInstance();
-    }
-
-    @Override
-    public Rotation3d unpack(ProtobufRotation3d msg) {
-      return new Rotation3d(Quaternion.proto.unpack(msg.getQ()));
-    }
-
-    @Override
-    public void pack(ProtobufRotation3d msg, Rotation3d value) {
-      Quaternion.proto.pack(msg.getMutableQ(), value.m_q);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Rotation3d struct for serialization. */
+  public static final Rotation3dStruct struct = new Rotation3dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
index c7959ba..68747af 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
@@ -4,15 +4,18 @@
 
 package edu.wpi.first.math.geometry;
 
-import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.proto.Transform2dProto;
+import edu.wpi.first.math.geometry.struct.Transform2dStruct;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a transformation for a Pose2d in the pose's frame. */
-public class Transform2d {
+public class Transform2d implements ProtobufSerializable, StructSerializable {
   private final Translation2d m_translation;
   private final Rotation2d m_rotation;
 
@@ -57,6 +60,18 @@
     m_rotation = rotation;
   }
 
+  /**
+   * Constructs a transform with x and y translations instead of a separate Translation2d. The X and
+   * Y translations will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform2d(Measure<Distance> x, Measure<Distance> y, Rotation2d rotation) {
+    this(x.in(Meters), y.in(Meters), rotation);
+  }
+
   /** Constructs the identity transform -- maps an initial pose to itself. */
   public Transform2d() {
     m_translation = new Translation2d();
@@ -169,82 +184,9 @@
     return Objects.hash(m_translation, m_rotation);
   }
 
-  public static final class AStruct implements Struct<Transform2d> {
-    @Override
-    public Class<Transform2d> getTypeClass() {
-      return Transform2d.class;
-    }
+  /** Transform2d protobuf for serialization. */
+  public static final Transform2dProto proto = new Transform2dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Transform2d";
-    }
-
-    @Override
-    public int getSize() {
-      return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
-    }
-
-    @Override
-    public String getSchema() {
-      return "Translation2d translation;Rotation2d rotation";
-    }
-
-    @Override
-    public Struct<?>[] getNested() {
-      return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
-    }
-
-    @Override
-    public Transform2d unpack(ByteBuffer bb) {
-      Translation2d translation = Translation2d.struct.unpack(bb);
-      Rotation2d rotation = Rotation2d.struct.unpack(bb);
-      return new Transform2d(translation, rotation);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Transform2d value) {
-      Translation2d.struct.pack(bb, value.m_translation);
-      Rotation2d.struct.pack(bb, value.m_rotation);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Transform2d, ProtobufTransform2d> {
-    @Override
-    public Class<Transform2d> getTypeClass() {
-      return Transform2d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufTransform2d.getDescriptor();
-    }
-
-    @Override
-    public Protobuf<?, ?>[] getNested() {
-      return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
-    }
-
-    @Override
-    public ProtobufTransform2d createMessage() {
-      return ProtobufTransform2d.newInstance();
-    }
-
-    @Override
-    public Transform2d unpack(ProtobufTransform2d msg) {
-      return new Transform2d(
-          Translation2d.proto.unpack(msg.getTranslation()),
-          Rotation2d.proto.unpack(msg.getRotation()));
-    }
-
-    @Override
-    public void pack(ProtobufTransform2d msg, Transform2d value) {
-      Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
-      Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Transform2d struct for serialization. */
+  public static final Transform2dStruct struct = new Transform2dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
index 223a14b..cdd021f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
@@ -4,15 +4,14 @@
 
 package edu.wpi.first.math.geometry;
 
-import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.math.geometry.proto.Transform3dProto;
+import edu.wpi.first.math.geometry.struct.Transform3dStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a transformation for a Pose3d in the pose's frame. */
-public class Transform3d {
+public class Transform3d implements ProtobufSerializable, StructSerializable {
   private final Translation3d m_translation;
   private final Rotation3d m_rotation;
 
@@ -179,82 +178,9 @@
     return Objects.hash(m_translation, m_rotation);
   }
 
-  public static final class AStruct implements Struct<Transform3d> {
-    @Override
-    public Class<Transform3d> getTypeClass() {
-      return Transform3d.class;
-    }
+  /** Transform3d protobuf for serialization. */
+  public static final Transform3dProto proto = new Transform3dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Transform3d";
-    }
-
-    @Override
-    public int getSize() {
-      return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
-    }
-
-    @Override
-    public String getSchema() {
-      return "Translation3d translation;Rotation3d rotation";
-    }
-
-    @Override
-    public Struct<?>[] getNested() {
-      return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
-    }
-
-    @Override
-    public Transform3d unpack(ByteBuffer bb) {
-      Translation3d translation = Translation3d.struct.unpack(bb);
-      Rotation3d rotation = Rotation3d.struct.unpack(bb);
-      return new Transform3d(translation, rotation);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Transform3d value) {
-      Translation3d.struct.pack(bb, value.m_translation);
-      Rotation3d.struct.pack(bb, value.m_rotation);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Transform3d, ProtobufTransform3d> {
-    @Override
-    public Class<Transform3d> getTypeClass() {
-      return Transform3d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufTransform3d.getDescriptor();
-    }
-
-    @Override
-    public Protobuf<?, ?>[] getNested() {
-      return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
-    }
-
-    @Override
-    public ProtobufTransform3d createMessage() {
-      return ProtobufTransform3d.newInstance();
-    }
-
-    @Override
-    public Transform3d unpack(ProtobufTransform3d msg) {
-      return new Transform3d(
-          Translation3d.proto.unpack(msg.getTranslation()),
-          Rotation3d.proto.unpack(msg.getRotation()));
-    }
-
-    @Override
-    public void pack(ProtobufTransform3d msg, Transform3d value) {
-      Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
-      Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Transform3d struct for serialization. */
+  public static final Transform3dStruct struct = new Transform3dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 96e0001..f089f66 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -4,21 +4,24 @@
 
 package edu.wpi.first.math.geometry;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.proto.Translation2dProto;
+import edu.wpi.first.math.geometry.struct.Translation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Collections;
 import java.util.Comparator;
 import java.util.List;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * Represents a translation in 2D space. This object can be used to represent a point or a vector.
@@ -28,7 +31,8 @@
  */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Translation2d implements Interpolatable<Translation2d> {
+public class Translation2d
+    implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable {
   private final double m_x;
   private final double m_y;
 
@@ -64,6 +68,17 @@
   }
 
   /**
+   * Constructs a Translation2d with the X and Y components equal to the provided values. The X and
+   * Y components will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  public Translation2d(Measure<Distance> x, Measure<Distance> y) {
+    this(x.in(Meters), y.in(Meters));
+  }
+
+  /**
    * Calculates the distance between two translations in 2D space.
    *
    * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
@@ -235,69 +250,9 @@
         MathUtil.interpolate(this.getY(), endValue.getY(), t));
   }
 
-  public static final class AStruct implements Struct<Translation2d> {
-    @Override
-    public Class<Translation2d> getTypeClass() {
-      return Translation2d.class;
-    }
+  /** Translation2d protobuf for serialization. */
+  public static final Translation2dProto proto = new Translation2dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Translation2d";
-    }
-
-    @Override
-    public int getSize() {
-      return kSizeDouble * 2;
-    }
-
-    @Override
-    public String getSchema() {
-      return "double x;double y";
-    }
-
-    @Override
-    public Translation2d unpack(ByteBuffer bb) {
-      double x = bb.getDouble();
-      double y = bb.getDouble();
-      return new Translation2d(x, y);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Translation2d value) {
-      bb.putDouble(value.m_x);
-      bb.putDouble(value.m_y);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Translation2d, ProtobufTranslation2d> {
-    @Override
-    public Class<Translation2d> getTypeClass() {
-      return Translation2d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufTranslation2d.getDescriptor();
-    }
-
-    @Override
-    public ProtobufTranslation2d createMessage() {
-      return ProtobufTranslation2d.newInstance();
-    }
-
-    @Override
-    public Translation2d unpack(ProtobufTranslation2d msg) {
-      return new Translation2d(msg.getX(), msg.getY());
-    }
-
-    @Override
-    public void pack(ProtobufTranslation2d msg, Translation2d value) {
-      msg.setX(value.m_x).setY(value.m_y);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Translation2d struct for serialization. */
+  public static final Translation2dStruct struct = new Translation2dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index bc55f65..6d4fa92 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -4,18 +4,21 @@
 
 package edu.wpi.first.math.geometry;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.proto.Translation3dProto;
+import edu.wpi.first.math.geometry.struct.Translation3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * Represents a translation in 3D space. This object can be used to represent a point or a vector.
@@ -26,7 +29,8 @@
  */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Translation3d implements Interpolatable<Translation3d> {
+public class Translation3d
+    implements Interpolatable<Translation3d>, ProtobufSerializable, StructSerializable {
   private final double m_x;
   private final double m_y;
   private final double m_z;
@@ -68,6 +72,18 @@
   }
 
   /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. The
+   * components will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  public Translation3d(Measure<Distance> x, Measure<Distance> y, Measure<Distance> z) {
+    this(x.in(Meters), y.in(Meters), z.in(Meters));
+  }
+
+  /**
    * Calculates the distance between two translations in 3D space.
    *
    * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
@@ -237,71 +253,9 @@
         MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
   }
 
-  public static final class AStruct implements Struct<Translation3d> {
-    @Override
-    public Class<Translation3d> getTypeClass() {
-      return Translation3d.class;
-    }
+  /** Translation3d protobuf for serialization. */
+  public static final Translation3dProto proto = new Translation3dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Translation3d";
-    }
-
-    @Override
-    public int getSize() {
-      return kSizeDouble * 3;
-    }
-
-    @Override
-    public String getSchema() {
-      return "double x;double y;double z";
-    }
-
-    @Override
-    public Translation3d unpack(ByteBuffer bb) {
-      double x = bb.getDouble();
-      double y = bb.getDouble();
-      double z = bb.getDouble();
-      return new Translation3d(x, y, z);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Translation3d value) {
-      bb.putDouble(value.m_x);
-      bb.putDouble(value.m_y);
-      bb.putDouble(value.m_z);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Translation3d, ProtobufTranslation3d> {
-    @Override
-    public Class<Translation3d> getTypeClass() {
-      return Translation3d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufTranslation3d.getDescriptor();
-    }
-
-    @Override
-    public ProtobufTranslation3d createMessage() {
-      return ProtobufTranslation3d.newInstance();
-    }
-
-    @Override
-    public Translation3d unpack(ProtobufTranslation3d msg) {
-      return new Translation3d(msg.getX(), msg.getY(), msg.getZ());
-    }
-
-    @Override
-    public void pack(ProtobufTranslation3d msg, Translation3d value) {
-      msg.setX(value.m_x).setY(value.m_y).setZ(value.m_z);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Translation3d struct for serialization. */
+  public static final Translation3dStruct struct = new Translation3dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
index a4ae9f8..53ab6ba 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
@@ -4,12 +4,11 @@
 
 package edu.wpi.first.math.geometry;
 
-import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.math.geometry.proto.Twist2dProto;
+import edu.wpi.first.math.geometry.struct.Twist2dStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A change in distance along a 2D arc since the last pose update. We can use ideas from
@@ -17,7 +16,7 @@
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */
-public class Twist2d {
+public class Twist2d implements ProtobufSerializable, StructSerializable {
   /** Linear "dx" component. */
   public double dx;
 
@@ -27,6 +26,7 @@
   /** Angular "dtheta" component (radians). */
   public double dtheta;
 
+  /** Default constructor. */
   public Twist2d() {}
 
   /**
@@ -68,71 +68,9 @@
     return Objects.hash(dx, dy, dtheta);
   }
 
-  public static final class AStruct implements Struct<Twist2d> {
-    @Override
-    public Class<Twist2d> getTypeClass() {
-      return Twist2d.class;
-    }
+  /** Twist2d protobuf for serialization. */
+  public static final Twist2dProto proto = new Twist2dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Twist2d";
-    }
-
-    @Override
-    public int getSize() {
-      return kSizeDouble * 3;
-    }
-
-    @Override
-    public String getSchema() {
-      return "double dx;double dy;double dtheta";
-    }
-
-    @Override
-    public Twist2d unpack(ByteBuffer bb) {
-      double dx = bb.getDouble();
-      double dy = bb.getDouble();
-      double dtheta = bb.getDouble();
-      return new Twist2d(dx, dy, dtheta);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Twist2d value) {
-      bb.putDouble(value.dx);
-      bb.putDouble(value.dy);
-      bb.putDouble(value.dtheta);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Twist2d, ProtobufTwist2d> {
-    @Override
-    public Class<Twist2d> getTypeClass() {
-      return Twist2d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufTwist2d.getDescriptor();
-    }
-
-    @Override
-    public ProtobufTwist2d createMessage() {
-      return ProtobufTwist2d.newInstance();
-    }
-
-    @Override
-    public Twist2d unpack(ProtobufTwist2d msg) {
-      return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta());
-    }
-
-    @Override
-    public void pack(ProtobufTwist2d msg, Twist2d value) {
-      msg.setDx(value.dx).setDy(value.dy).setDtheta(value.dtheta);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Twist2d struct for serialization. */
+  public static final Twist2dStruct struct = new Twist2dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
index d08d5cf..8078602 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
@@ -4,12 +4,11 @@
 
 package edu.wpi.first.math.geometry;
 
-import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
-import edu.wpi.first.util.protobuf.Protobuf;
-import edu.wpi.first.util.struct.Struct;
-import java.nio.ByteBuffer;
+import edu.wpi.first.math.geometry.proto.Twist3dProto;
+import edu.wpi.first.math.geometry.struct.Twist3dStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A change in distance along a 3D arc since the last pose update. We can use ideas from
@@ -17,7 +16,7 @@
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */
-public class Twist3d {
+public class Twist3d implements ProtobufSerializable, StructSerializable {
   /** Linear "dx" component. */
   public double dx;
 
@@ -36,6 +35,7 @@
   /** Rotation vector z component (radians). */
   public double rz;
 
+  /** Default constructor. */
   public Twist3d() {}
 
   /**
@@ -88,83 +88,9 @@
     return Objects.hash(dx, dy, dz, rx, ry, rz);
   }
 
-  public static final class AStruct implements Struct<Twist3d> {
-    @Override
-    public Class<Twist3d> getTypeClass() {
-      return Twist3d.class;
-    }
+  /** Twist3d protobuf for serialization. */
+  public static final Twist3dProto proto = new Twist3dProto();
 
-    @Override
-    public String getTypeString() {
-      return "struct:Twist3d";
-    }
-
-    @Override
-    public int getSize() {
-      return kSizeDouble * 6;
-    }
-
-    @Override
-    public String getSchema() {
-      return "double dx;double dy;double dz;double rx;double ry;double rz";
-    }
-
-    @Override
-    public Twist3d unpack(ByteBuffer bb) {
-      double dx = bb.getDouble();
-      double dy = bb.getDouble();
-      double dz = bb.getDouble();
-      double rx = bb.getDouble();
-      double ry = bb.getDouble();
-      double rz = bb.getDouble();
-      return new Twist3d(dx, dy, dz, rx, ry, rz);
-    }
-
-    @Override
-    public void pack(ByteBuffer bb, Twist3d value) {
-      bb.putDouble(value.dx);
-      bb.putDouble(value.dy);
-      bb.putDouble(value.dz);
-      bb.putDouble(value.rx);
-      bb.putDouble(value.ry);
-      bb.putDouble(value.rz);
-    }
-  }
-
-  public static final AStruct struct = new AStruct();
-
-  public static final class AProto implements Protobuf<Twist3d, ProtobufTwist3d> {
-    @Override
-    public Class<Twist3d> getTypeClass() {
-      return Twist3d.class;
-    }
-
-    @Override
-    public Descriptor getDescriptor() {
-      return ProtobufTwist3d.getDescriptor();
-    }
-
-    @Override
-    public ProtobufTwist3d createMessage() {
-      return ProtobufTwist3d.newInstance();
-    }
-
-    @Override
-    public Twist3d unpack(ProtobufTwist3d msg) {
-      return new Twist3d(
-          msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz());
-    }
-
-    @Override
-    public void pack(ProtobufTwist3d msg, Twist3d value) {
-      msg.setDx(value.dx)
-          .setDy(value.dy)
-          .setDz(value.dz)
-          .setRx(value.rx)
-          .setRy(value.ry)
-          .setRz(value.rz);
-    }
-  }
-
-  public static final AProto proto = new AProto();
+  /** Twist3d struct for serialization. */
+  public static final Twist3dStruct struct = new Twist3dStruct();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java
new file mode 100644
index 0000000..7921100
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java
@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Pose2dProto implements Protobuf<Pose2d, ProtobufPose2d> {
+  @Override
+  public Class<Pose2d> getTypeClass() {
+    return Pose2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufPose2d.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+  }
+
+  @Override
+  public ProtobufPose2d createMessage() {
+    return ProtobufPose2d.newInstance();
+  }
+
+  @Override
+  public Pose2d unpack(ProtobufPose2d msg) {
+    return new Pose2d(
+        Translation2d.proto.unpack(msg.getTranslation()),
+        Rotation2d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufPose2d msg, Pose2d value) {
+    Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java
new file mode 100644
index 0000000..69d6ba2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java
@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Pose3dProto implements Protobuf<Pose3d, ProtobufPose3d> {
+  @Override
+  public Class<Pose3d> getTypeClass() {
+    return Pose3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufPose3d.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+  }
+
+  @Override
+  public ProtobufPose3d createMessage() {
+    return ProtobufPose3d.newInstance();
+  }
+
+  @Override
+  public Pose3d unpack(ProtobufPose3d msg) {
+    return new Pose3d(
+        Translation3d.proto.unpack(msg.getTranslation()),
+        Rotation3d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufPose3d msg, Pose3d value) {
+    Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java
new file mode 100644
index 0000000..c464ba2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java
@@ -0,0 +1,40 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class QuaternionProto implements Protobuf<Quaternion, ProtobufQuaternion> {
+  @Override
+  public Class<Quaternion> getTypeClass() {
+    return Quaternion.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufQuaternion.getDescriptor();
+  }
+
+  @Override
+  public ProtobufQuaternion createMessage() {
+    return ProtobufQuaternion.newInstance();
+  }
+
+  @Override
+  public Quaternion unpack(ProtobufQuaternion msg) {
+    return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ());
+  }
+
+  @Override
+  public void pack(ProtobufQuaternion msg, Quaternion value) {
+    msg.setW(value.getW());
+    msg.setX(value.getX());
+    msg.setY(value.getY());
+    msg.setZ(value.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java
new file mode 100644
index 0000000..98ad5c7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java
@@ -0,0 +1,37 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Rotation2dProto implements Protobuf<Rotation2d, ProtobufRotation2d> {
+  @Override
+  public Class<Rotation2d> getTypeClass() {
+    return Rotation2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufRotation2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufRotation2d createMessage() {
+    return ProtobufRotation2d.newInstance();
+  }
+
+  @Override
+  public Rotation2d unpack(ProtobufRotation2d msg) {
+    return new Rotation2d(msg.getValue());
+  }
+
+  @Override
+  public void pack(ProtobufRotation2d msg, Rotation2d value) {
+    msg.setValue(value.getRadians());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java
new file mode 100644
index 0000000..9c9b01a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java
@@ -0,0 +1,43 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Rotation3dProto implements Protobuf<Rotation3d, ProtobufRotation3d> {
+  @Override
+  public Class<Rotation3d> getTypeClass() {
+    return Rotation3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufRotation3d.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Quaternion.proto};
+  }
+
+  @Override
+  public ProtobufRotation3d createMessage() {
+    return ProtobufRotation3d.newInstance();
+  }
+
+  @Override
+  public Rotation3d unpack(ProtobufRotation3d msg) {
+    return new Rotation3d(Quaternion.proto.unpack(msg.getQ()));
+  }
+
+  @Override
+  public void pack(ProtobufRotation3d msg, Rotation3d value) {
+    Quaternion.proto.pack(msg.getMutableQ(), value.getQuaternion());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java
new file mode 100644
index 0000000..ecc0528
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java
@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Transform2dProto implements Protobuf<Transform2d, ProtobufTransform2d> {
+  @Override
+  public Class<Transform2d> getTypeClass() {
+    return Transform2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTransform2d.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+  }
+
+  @Override
+  public ProtobufTransform2d createMessage() {
+    return ProtobufTransform2d.newInstance();
+  }
+
+  @Override
+  public Transform2d unpack(ProtobufTransform2d msg) {
+    return new Transform2d(
+        Translation2d.proto.unpack(msg.getTranslation()),
+        Rotation2d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufTransform2d msg, Transform2d value) {
+    Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java
new file mode 100644
index 0000000..29ea793
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java
@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Transform3dProto implements Protobuf<Transform3d, ProtobufTransform3d> {
+  @Override
+  public Class<Transform3d> getTypeClass() {
+    return Transform3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTransform3d.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+  }
+
+  @Override
+  public ProtobufTransform3d createMessage() {
+    return ProtobufTransform3d.newInstance();
+  }
+
+  @Override
+  public Transform3d unpack(ProtobufTransform3d msg) {
+    return new Transform3d(
+        Translation3d.proto.unpack(msg.getTranslation()),
+        Rotation3d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufTransform3d msg, Transform3d value) {
+    Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java
new file mode 100644
index 0000000..a63fec3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java
@@ -0,0 +1,38 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Translation2dProto implements Protobuf<Translation2d, ProtobufTranslation2d> {
+  @Override
+  public Class<Translation2d> getTypeClass() {
+    return Translation2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTranslation2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTranslation2d createMessage() {
+    return ProtobufTranslation2d.newInstance();
+  }
+
+  @Override
+  public Translation2d unpack(ProtobufTranslation2d msg) {
+    return new Translation2d(msg.getX(), msg.getY());
+  }
+
+  @Override
+  public void pack(ProtobufTranslation2d msg, Translation2d value) {
+    msg.setX(value.getX());
+    msg.setY(value.getY());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java
new file mode 100644
index 0000000..b854a34
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java
@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Translation3dProto implements Protobuf<Translation3d, ProtobufTranslation3d> {
+  @Override
+  public Class<Translation3d> getTypeClass() {
+    return Translation3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTranslation3d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTranslation3d createMessage() {
+    return ProtobufTranslation3d.newInstance();
+  }
+
+  @Override
+  public Translation3d unpack(ProtobufTranslation3d msg) {
+    return new Translation3d(msg.getX(), msg.getY(), msg.getZ());
+  }
+
+  @Override
+  public void pack(ProtobufTranslation3d msg, Translation3d value) {
+    msg.setX(value.getX());
+    msg.setY(value.getY());
+    msg.setZ(value.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java
new file mode 100644
index 0000000..bc9da6b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java
@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Twist2dProto implements Protobuf<Twist2d, ProtobufTwist2d> {
+  @Override
+  public Class<Twist2d> getTypeClass() {
+    return Twist2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTwist2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTwist2d createMessage() {
+    return ProtobufTwist2d.newInstance();
+  }
+
+  @Override
+  public Twist2d unpack(ProtobufTwist2d msg) {
+    return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta());
+  }
+
+  @Override
+  public void pack(ProtobufTwist2d msg, Twist2d value) {
+    msg.setDx(value.dx);
+    msg.setDy(value.dy);
+    msg.setDtheta(value.dtheta);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java
new file mode 100644
index 0000000..356c18f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java
@@ -0,0 +1,43 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Twist3dProto implements Protobuf<Twist3d, ProtobufTwist3d> {
+  @Override
+  public Class<Twist3d> getTypeClass() {
+    return Twist3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTwist3d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTwist3d createMessage() {
+    return ProtobufTwist3d.newInstance();
+  }
+
+  @Override
+  public Twist3d unpack(ProtobufTwist3d msg) {
+    return new Twist3d(
+        msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz());
+  }
+
+  @Override
+  public void pack(ProtobufTwist3d msg, Twist3d value) {
+    msg.setDx(value.dx);
+    msg.setDy(value.dy);
+    msg.setDz(value.dz);
+    msg.setRx(value.rx);
+    msg.setRy(value.ry);
+    msg.setRz(value.rz);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java
new file mode 100644
index 0000000..8435354
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java
@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Pose2dStruct implements Struct<Pose2d> {
+  @Override
+  public Class<Pose2d> getTypeClass() {
+    return Pose2d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Pose2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+  }
+
+  @Override
+  public Pose2d unpack(ByteBuffer bb) {
+    Translation2d translation = Translation2d.struct.unpack(bb);
+    Rotation2d rotation = Rotation2d.struct.unpack(bb);
+    return new Pose2d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Pose2d value) {
+    Translation2d.struct.pack(bb, value.getTranslation());
+    Rotation2d.struct.pack(bb, value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java
new file mode 100644
index 0000000..db380f6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java
@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Pose3dStruct implements Struct<Pose3d> {
+  @Override
+  public Class<Pose3d> getTypeClass() {
+    return Pose3d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Pose3d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+  }
+
+  @Override
+  public Pose3d unpack(ByteBuffer bb) {
+    Translation3d translation = Translation3d.struct.unpack(bb);
+    Rotation3d rotation = Rotation3d.struct.unpack(bb);
+    return new Pose3d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Pose3d value) {
+    Translation3d.struct.pack(bb, value.getTranslation());
+    Rotation3d.struct.pack(bb, value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java
new file mode 100644
index 0000000..fec8452
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java
@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class QuaternionStruct implements Struct<Quaternion> {
+  @Override
+  public Class<Quaternion> getTypeClass() {
+    return Quaternion.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Quaternion";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double w;double x;double y;double z";
+  }
+
+  @Override
+  public Quaternion unpack(ByteBuffer bb) {
+    double w = bb.getDouble();
+    double x = bb.getDouble();
+    double y = bb.getDouble();
+    double z = bb.getDouble();
+    return new Quaternion(w, x, y, z);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Quaternion value) {
+    bb.putDouble(value.getW());
+    bb.putDouble(value.getX());
+    bb.putDouble(value.getY());
+    bb.putDouble(value.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java
new file mode 100644
index 0000000..b22e3d1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java
@@ -0,0 +1,42 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Rotation2dStruct implements Struct<Rotation2d> {
+  @Override
+  public Class<Rotation2d> getTypeClass() {
+    return Rotation2d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Rotation2d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double value";
+  }
+
+  @Override
+  public Rotation2d unpack(ByteBuffer bb) {
+    double value = bb.getDouble();
+    return new Rotation2d(value);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Rotation2d value) {
+    bb.putDouble(value.getRadians());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java
new file mode 100644
index 0000000..4184990
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java
@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Rotation3dStruct implements Struct<Rotation3d> {
+  @Override
+  public Class<Rotation3d> getTypeClass() {
+    return Rotation3d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Rotation3d";
+  }
+
+  @Override
+  public int getSize() {
+    return Quaternion.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Quaternion q";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Quaternion.struct};
+  }
+
+  @Override
+  public Rotation3d unpack(ByteBuffer bb) {
+    Quaternion q = Quaternion.struct.unpack(bb);
+    return new Rotation3d(q);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Rotation3d value) {
+    Quaternion.struct.pack(bb, value.getQuaternion());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java
new file mode 100644
index 0000000..298d6db
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java
@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Transform2dStruct implements Struct<Transform2d> {
+  @Override
+  public Class<Transform2d> getTypeClass() {
+    return Transform2d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Transform2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+  }
+
+  @Override
+  public Transform2d unpack(ByteBuffer bb) {
+    Translation2d translation = Translation2d.struct.unpack(bb);
+    Rotation2d rotation = Rotation2d.struct.unpack(bb);
+    return new Transform2d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Transform2d value) {
+    Translation2d.struct.pack(bb, value.getTranslation());
+    Rotation2d.struct.pack(bb, value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java
new file mode 100644
index 0000000..2ad5bc1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java
@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Transform3dStruct implements Struct<Transform3d> {
+  @Override
+  public Class<Transform3d> getTypeClass() {
+    return Transform3d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Transform3d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+  }
+
+  @Override
+  public Transform3d unpack(ByteBuffer bb) {
+    Translation3d translation = Translation3d.struct.unpack(bb);
+    Rotation3d rotation = Rotation3d.struct.unpack(bb);
+    return new Transform3d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Transform3d value) {
+    Translation3d.struct.pack(bb, value.getTranslation());
+    Rotation3d.struct.pack(bb, value.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java
new file mode 100644
index 0000000..aac179a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java
@@ -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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Translation2dStruct implements Struct<Translation2d> {
+  @Override
+  public Class<Translation2d> getTypeClass() {
+    return Translation2d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Translation2d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double x;double y";
+  }
+
+  @Override
+  public Translation2d unpack(ByteBuffer bb) {
+    double x = bb.getDouble();
+    double y = bb.getDouble();
+    return new Translation2d(x, y);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Translation2d value) {
+    bb.putDouble(value.getX());
+    bb.putDouble(value.getY());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java
new file mode 100644
index 0000000..f4e1029
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java
@@ -0,0 +1,46 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Translation3dStruct implements Struct<Translation3d> {
+  @Override
+  public Class<Translation3d> getTypeClass() {
+    return Translation3d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Translation3d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 3;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double x;double y;double z";
+  }
+
+  @Override
+  public Translation3d unpack(ByteBuffer bb) {
+    double x = bb.getDouble();
+    double y = bb.getDouble();
+    double z = bb.getDouble();
+    return new Translation3d(x, y, z);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Translation3d value) {
+    bb.putDouble(value.getX());
+    bb.putDouble(value.getY());
+    bb.putDouble(value.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java
new file mode 100644
index 0000000..7797f3a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java
@@ -0,0 +1,46 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Twist2dStruct implements Struct<Twist2d> {
+  @Override
+  public Class<Twist2d> getTypeClass() {
+    return Twist2d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Twist2d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 3;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double dx;double dy;double dtheta";
+  }
+
+  @Override
+  public Twist2d unpack(ByteBuffer bb) {
+    double dx = bb.getDouble();
+    double dy = bb.getDouble();
+    double dtheta = bb.getDouble();
+    return new Twist2d(dx, dy, dtheta);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Twist2d value) {
+    bb.putDouble(value.dx);
+    bb.putDouble(value.dy);
+    bb.putDouble(value.dtheta);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java
new file mode 100644
index 0000000..68926a4
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java
@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Twist3dStruct implements Struct<Twist3d> {
+  @Override
+  public Class<Twist3d> getTypeClass() {
+    return Twist3d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Twist3d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 6;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double dx;double dy;double dz;double rx;double ry;double rz";
+  }
+
+  @Override
+  public Twist3d unpack(ByteBuffer bb) {
+    double dx = bb.getDouble();
+    double dy = bb.getDouble();
+    double dz = bb.getDouble();
+    double rx = bb.getDouble();
+    double ry = bb.getDouble();
+    double rz = bb.getDouble();
+    return new Twist3d(dx, dy, dz, rx, ry, rz);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Twist3d value) {
+    bb.putDouble(value.dx);
+    bb.putDouble(value.dy);
+    bb.putDouble(value.dz);
+    bb.putDouble(value.rx);
+    bb.putDouble(value.ry);
+    bb.putDouble(value.rz);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
index bcd57a3..15c55d0 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
@@ -9,6 +9,7 @@
  * from points that are defined. This uses linear interpolation.
  */
 public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+  /** Default constructor. */
   public InterpolatingDoubleTreeMap() {
     super(InverseInterpolator.forDouble(), Interpolator.forDouble());
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
index be6d8a2..5fe1a8f 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
@@ -24,6 +24,11 @@
    */
   T interpolate(T startValue, T endValue, double t);
 
+  /**
+   * Returns interpolator for Double.
+   *
+   * @return Interpolator for Double.
+   */
   static Interpolator<Double> forDouble() {
     return MathUtil::interpolate;
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
index 8278af3..d6d0bc8 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
@@ -24,6 +24,11 @@
    */
   double inverseInterpolate(T startValue, T endValue, T q);
 
+  /**
+   * Returns inverse interpolator for Double.
+   *
+   * @return Inverse interpolator for Double.
+   */
   static InverseInterpolator<Double> forDouble() {
     return MathUtil::inverseInterpolate;
   }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
index ffbce71..33a5edc 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
@@ -4,9 +4,22 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
+import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 
 /**
  * Represents the speed of a robot chassis. Although this class contains similar members compared to
@@ -17,7 +30,7 @@
  * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
  * will often have all three components.
  */
-public class ChassisSpeeds {
+public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
   /** Velocity along the x-axis. (Fwd is +) */
   public double vxMetersPerSecond;
 
@@ -27,6 +40,12 @@
   /** Represents the angular velocity of the robot frame. (CCW is +) */
   public double omegaRadiansPerSecond;
 
+  /** ChassisSpeeds protobuf for serialization. */
+  public static final ChassisSpeedsProto proto = new ChassisSpeedsProto();
+
+  /** ChassisSpeeds struct for serialization. */
+  public static final ChassisSpeedsStruct struct = new ChassisSpeedsStruct();
+
   /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
   public ChassisSpeeds() {}
 
@@ -45,6 +64,20 @@
   }
 
   /**
+   * Constructs a ChassisSpeeds object.
+   *
+   * @param vx Forward velocity.
+   * @param vy Sideways velocity.
+   * @param omega Angular velocity.
+   */
+  public ChassisSpeeds(
+      Measure<Velocity<Distance>> vx,
+      Measure<Velocity<Distance>> vy,
+      Measure<Velocity<Angle>> omega) {
+    this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond));
+  }
+
+  /**
    * Discretizes a continuous-time chassis speed.
    *
    * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
@@ -86,6 +119,32 @@
    * <p>This is useful for compensating for translational skew when translating and rotating a
    * swerve drivetrain.
    *
+   * @param vx Forward velocity.
+   * @param vy Sideways velocity.
+   * @param omega Angular velocity.
+   * @param dt The duration of the timestep the speeds should be applied for.
+   * @return Discretized ChassisSpeeds.
+   */
+  public static ChassisSpeeds discretize(
+      Measure<Velocity<Distance>> vx,
+      Measure<Velocity<Distance>> vy,
+      Measure<Velocity<Angle>> omega,
+      Measure<Time> dt) {
+    return discretize(
+        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), dt.in(Seconds));
+  }
+
+  /**
+   * Discretizes a continuous-time chassis speed.
+   *
+   * <p>This function converts a continous-time chassis speed into a discrete-time one such that
+   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
+   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
+   * along the y-axis, and omega * dt around the z-axis).
+   *
+   * <p>This is useful for compensating for translational skew when translating and rotating a
+   * swerve drivetrain.
+   *
    * @param continuousSpeeds The continuous speeds.
    * @param dtSeconds The duration of the timestep the speeds should be applied for.
    * @return Discretized ChassisSpeeds.
@@ -124,6 +183,29 @@
   }
 
   /**
+   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
+   * object.
+   *
+   * @param vx The component of speed in the x direction relative to the field. Positive x is away
+   *     from your alliance wall.
+   * @param vy The component of speed in the y direction relative to the field. Positive y is to
+   *     your left when standing behind the alliance wall.
+   * @param omega The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
+   *     considered to be zero when it is facing directly away from your alliance station wall.
+   *     Remember that this should be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
+   */
+  public static ChassisSpeeds fromFieldRelativeSpeeds(
+      Measure<Velocity<Distance>> vx,
+      Measure<Velocity<Distance>> vy,
+      Measure<Velocity<Angle>> omega,
+      Rotation2d robotAngle) {
+    return fromFieldRelativeSpeeds(
+        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
+  }
+
+  /**
    * Converts a user provided field-relative ChassisSpeeds object into a robot-relative
    * ChassisSpeeds object.
    *
@@ -169,6 +251,29 @@
   }
 
   /**
+   * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
+   * object.
+   *
+   * @param vx The component of speed in the x direction relative to the robot. Positive x is
+   *     towards the robot's front.
+   * @param vy The component of speed in the y direction relative to the robot. Positive y is
+   *     towards the robot's left.
+   * @param omega The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
+   *     considered to be zero when it is facing directly away from your alliance station wall.
+   *     Remember that this should be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
+   */
+  public static ChassisSpeeds fromRobotRelativeSpeeds(
+      Measure<Velocity<Distance>> vx,
+      Measure<Velocity<Distance>> vy,
+      Measure<Velocity<Angle>> omega,
+      Rotation2d robotAngle) {
+    return fromRobotRelativeSpeeds(
+        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
+  }
+
+  /**
    * Converts a user provided robot-relative ChassisSpeeds object into a field-relative
    * ChassisSpeeds object.
    *
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
index a1fb2db..1d18b00 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -4,9 +4,17 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto;
+import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 
 /**
  * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
@@ -17,9 +25,20 @@
  * chassis speed.
  */
 public class DifferentialDriveKinematics
-    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
+    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>,
+        ProtobufSerializable,
+        StructSerializable {
+  /** Differential drive trackwidth. */
   public final double trackWidthMeters;
 
+  /** DifferentialDriveKinematics protobuf for serialization. */
+  public static final DifferentialDriveKinematicsProto proto =
+      new DifferentialDriveKinematicsProto();
+
+  /** DifferentialDriveKinematics struct for serialization. */
+  public static final DifferentialDriveKinematicsStruct struct =
+      new DifferentialDriveKinematicsStruct();
+
   /**
    * Constructs a differential drive kinematics object.
    *
@@ -33,6 +52,17 @@
   }
 
   /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackWidth The track width of the drivetrain. Theoretically, this is the distance
+   *     between the left wheels and right wheels. However, the empirical value may be larger than
+   *     the physical measured value due to scrubbing effects.
+   */
+  public DifferentialDriveKinematics(Measure<Distance> trackWidth) {
+    this(trackWidth.in(Meters));
+  }
+
+  /**
    * Returns a chassis speed from left and right component velocities using forward kinematics.
    *
    * @param wheelSpeeds The left and right velocities.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
index db5e8a3..87d7bbf 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
@@ -4,10 +4,14 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
 
 /**
  * Class for differential drive odometry. Odometry allows you to track the robot's position on the
@@ -45,6 +49,22 @@
    * Constructs a DifferentialDriveOdometry object.
    *
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public DifferentialDriveOdometry(
+      Rotation2d gyroAngle,
+      Measure<Distance> leftDistance,
+      Measure<Distance> rightDistance,
+      Pose2d initialPoseMeters) {
+    this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters);
+  }
+
+  /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
    * @param leftDistanceMeters The distance traveled by the left encoder.
    * @param rightDistanceMeters The distance traveled by the right encoder.
    */
@@ -54,6 +74,18 @@
   }
 
   /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   */
+  public DifferentialDriveOdometry(
+      Rotation2d gyroAngle, Measure<Distance> leftDistance, Measure<Distance> rightDistance) {
+    this(gyroAngle, leftDistance, rightDistance, new Pose2d());
+  }
+
+  /**
    * Resets the robot's position on the field.
    *
    * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
@@ -76,6 +108,25 @@
   }
 
   /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param poseMeters The position on the field that your robot is at.
+   */
+  public void resetPosition(
+      Rotation2d gyroAngle,
+      Measure<Distance> leftDistance,
+      Measure<Distance> rightDistance,
+      Pose2d poseMeters) {
+    resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters);
+  }
+
+  /**
    * Updates the robot position on the field using distance measurements from encoders. This method
    * is more numerically accurate than using velocities to integrate the pose and is also
    * advantageous for teams that are using lower CPR encoders.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
index 18866af..fbdf54d 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
@@ -4,9 +4,14 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
 import java.util.Objects;
 
+/** Represents the wheel positions for a differential drive drivetrain. */
 public class DifferentialDriveWheelPositions
     implements WheelPositions<DifferentialDriveWheelPositions> {
   /** Distance measured by the left side. */
@@ -26,6 +31,16 @@
     this.rightMeters = rightMeters;
   }
 
+  /**
+   * Constructs a DifferentialDriveWheelPositions.
+   *
+   * @param left Distance measured by the left side.
+   * @param right Distance measured by the right side.
+   */
+  public DifferentialDriveWheelPositions(Measure<Distance> left, Measure<Distance> right) {
+    this(left.in(Meters), right.in(Meters));
+  }
+
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof DifferentialDriveWheelPositions) {
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
index ec874b6..de18817 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
@@ -4,14 +4,32 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
+import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelSpeedsProto;
+import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
 /** Represents the wheel speeds for a differential drive drivetrain. */
-public class DifferentialDriveWheelSpeeds {
+public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
   /** Speed of the left side of the robot. */
   public double leftMetersPerSecond;
 
   /** Speed of the right side of the robot. */
   public double rightMetersPerSecond;
 
+  /** DifferentialDriveWheelSpeeds protobuf for serialization. */
+  public static final DifferentialDriveWheelSpeedsProto proto =
+      new DifferentialDriveWheelSpeedsProto();
+
+  /** DifferentialDriveWheelSpeeds struct for serialization. */
+  public static final DifferentialDriveWheelSpeedsStruct struct =
+      new DifferentialDriveWheelSpeedsStruct();
+
   /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
   public DifferentialDriveWheelSpeeds() {}
 
@@ -27,6 +45,17 @@
   }
 
   /**
+   * Constructs a DifferentialDriveWheelSpeeds.
+   *
+   * @param left The left speed.
+   * @param right The right speed.
+   */
+  public DifferentialDriveWheelSpeeds(
+      Measure<Velocity<Distance>> left, Measure<Velocity<Distance>> right) {
+    this(left.in(MetersPerSecond), right.in(MetersPerSecond));
+  }
+
+  /**
    * Renormalizes the wheel speeds if any either side is above the specified maximum.
    *
    * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
@@ -47,6 +76,20 @@
   }
 
   /**
+   * Renormalizes the wheel speeds if any either side is above the specified maximum.
+   *
+   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
+   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
+   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+   * absolute threshold, while maintaining the ratio of speeds between wheels.
+   *
+   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+   */
+  public void desaturate(Measure<Velocity<Distance>> attainableMaxSpeed) {
+    desaturate(attainableMaxSpeed.in(MetersPerSecond));
+  }
+
+  /**
    * Adds two DifferentialDriveWheelSpeeds and returns the sum.
    *
    * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
index 76c857a..44dbb6b 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -8,6 +8,10 @@
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.proto.MecanumDriveKinematicsProto;
+import edu.wpi.first.math.kinematics.struct.MecanumDriveKinematicsStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -31,7 +35,9 @@
  * field using encoders and a gyro.
  */
 public class MecanumDriveKinematics
-    implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
+    implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>,
+        ProtobufSerializable,
+        StructSerializable {
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
@@ -42,6 +48,12 @@
 
   private Translation2d m_prevCoR = new Translation2d();
 
+  /** MecanumDriveKinematics protobuf for serialization. */
+  public static final MecanumDriveKinematicsProto proto = new MecanumDriveKinematicsProto();
+
+  /** MecanumDriveKinematics struct for serialization. */
+  public static final MecanumDriveKinematicsStruct struct = new MecanumDriveKinematicsStruct();
+
   /**
    * Constructs a mecanum drive kinematics object.
    *
@@ -207,4 +219,40 @@
     m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
     m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
   }
+
+  /**
+   * Returns the front-left wheel translation.
+   *
+   * @return The front-left wheel translation.
+   */
+  public Translation2d getFrontLeft() {
+    return m_frontLeftWheelMeters;
+  }
+
+  /**
+   * Returns the front-right wheel translation.
+   *
+   * @return The front-right wheel translation.
+   */
+  public Translation2d getFrontRight() {
+    return m_frontRightWheelMeters;
+  }
+
+  /**
+   * Returns the rear-left wheel translation.
+   *
+   * @return The rear-left wheel translation.
+   */
+  public Translation2d getRearLeft() {
+    return m_rearLeftWheelMeters;
+  }
+
+  /**
+   * Returns the rear-right wheel translation.
+   *
+   * @return The rear-right wheel translation.
+   */
+  public Translation2d getRearRight() {
+    return m_rearRightWheelMeters;
+  }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
index 2b284cb..179e1bf 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
@@ -4,10 +4,22 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelPositionsProto;
+import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelPositionsStruct;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 
-public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWheelPositions> {
+/** Represents the wheel positions for a mecanum drive drivetrain. */
+public class MecanumDriveWheelPositions
+    implements WheelPositions<MecanumDriveWheelPositions>,
+        ProtobufSerializable,
+        StructSerializable {
   /** Distance measured by the front left wheel. */
   public double frontLeftMeters;
 
@@ -20,6 +32,13 @@
   /** Distance measured by the rear right wheel. */
   public double rearRightMeters;
 
+  /** MecanumDriveWheelPositions protobuf for serialization. */
+  public static final MecanumDriveWheelPositionsProto proto = new MecanumDriveWheelPositionsProto();
+
+  /** MecanumDriveWheelPositions struct for serialization. */
+  public static final MecanumDriveWheelPositionsStruct struct =
+      new MecanumDriveWheelPositionsStruct();
+
   /** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
   public MecanumDriveWheelPositions() {}
 
@@ -42,6 +61,22 @@
     this.rearRightMeters = rearRightMeters;
   }
 
+  /**
+   * Constructs a MecanumDriveWheelPositions.
+   *
+   * @param frontLeft Distance measured by the front left wheel.
+   * @param frontRight Distance measured by the front right wheel.
+   * @param rearLeft Distance measured by the rear left wheel.
+   * @param rearRight Distance measured by the rear right wheel.
+   */
+  public MecanumDriveWheelPositions(
+      Measure<Distance> frontLeft,
+      Measure<Distance> frontRight,
+      Measure<Distance> rearLeft,
+      Measure<Distance> rearRight) {
+    this(frontLeft.in(Meters), frontRight.in(Meters), rearLeft.in(Meters), rearRight.in(Meters));
+  }
+
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof MecanumDriveWheelPositions) {
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
index 63cef18..59a3455 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
@@ -4,9 +4,18 @@
 
 package edu.wpi.first.math.kinematics;
 
-import java.util.stream.DoubleStream;
+import static edu.wpi.first.units.Units.MetersPerSecond;
 
-public class MecanumDriveWheelSpeeds {
+import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelSpeedsProto;
+import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelSpeedsStruct;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
+/** Represents the wheel speeds for a mecanum drive drivetrain. */
+public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
   /** Speed of the front left wheel. */
   public double frontLeftMetersPerSecond;
 
@@ -19,6 +28,12 @@
   /** Speed of the rear right wheel. */
   public double rearRightMetersPerSecond;
 
+  /** MecanumDriveWheelSpeeds protobuf for serialization. */
+  public static final MecanumDriveWheelSpeedsProto proto = new MecanumDriveWheelSpeedsProto();
+
+  /** MecanumDriveWheelSpeeds struct for serialization. */
+  public static final MecanumDriveWheelSpeedsStruct struct = new MecanumDriveWheelSpeedsStruct();
+
   /** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
   public MecanumDriveWheelSpeeds() {}
 
@@ -42,6 +57,26 @@
   }
 
   /**
+   * Constructs a MecanumDriveWheelSpeeds.
+   *
+   * @param frontLeft Speed of the front left wheel.
+   * @param frontRight Speed of the front right wheel.
+   * @param rearLeft Speed of the rear left wheel.
+   * @param rearRight Speed of the rear right wheel.
+   */
+  public MecanumDriveWheelSpeeds(
+      Measure<Velocity<Distance>> frontLeft,
+      Measure<Velocity<Distance>> frontRight,
+      Measure<Velocity<Distance>> rearLeft,
+      Measure<Velocity<Distance>> rearRight) {
+    this(
+        frontLeft.in(MetersPerSecond),
+        frontRight.in(MetersPerSecond),
+        rearLeft.in(MetersPerSecond),
+        rearRight.in(MetersPerSecond));
+  }
+
+  /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    *
    * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
@@ -53,13 +88,9 @@
    */
   public void desaturate(double attainableMaxSpeedMetersPerSecond) {
     double realMaxSpeed =
-        DoubleStream.of(
-                frontLeftMetersPerSecond,
-                frontRightMetersPerSecond,
-                rearLeftMetersPerSecond,
-                rearRightMetersPerSecond)
-            .max()
-            .getAsDouble();
+        Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond));
+    realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond));
+    realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond));
 
     if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
       frontLeftMetersPerSecond =
@@ -74,6 +105,20 @@
   }
 
   /**
+   * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
+   *
+   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
+   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
+   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+   * absolute threshold, while maintaining the ratio of speeds between wheels.
+   *
+   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+   */
+  public void desaturate(Measure<Velocity<Distance>> attainableMaxSpeed) {
+    desaturate(attainableMaxSpeed.in(MetersPerSecond));
+  }
+
+  /**
    * Adds two MecanumDriveWheelSpeeds and returns the sum.
    *
    * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelSpeeds{2.0, 1.5,
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
index b2e5054..427c92d 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
@@ -15,6 +15,8 @@
  *
  * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
  * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
+ *
+ * @param <T> Wheel positions type.
  */
 public class Odometry<T extends WheelPositions<T>> {
   private final Kinematics<?, T> m_kinematics;
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index aa5338e..6648343 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -4,11 +4,18 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
 import java.util.Arrays;
 import org.ejml.simple.SimpleMatrix;
 
@@ -34,7 +41,9 @@
  */
 public class SwerveDriveKinematics
     implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
+  /** Wrapper class for swerve module states. */
   public static class SwerveDriveWheelStates {
+    /** Swerve module states. */
     public SwerveModuleState[] states;
 
     /**
@@ -303,6 +312,23 @@
   }
 
   /**
+   * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
+   *
+   * <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
+   * above the max attainable speed for the driving motor on that module. To fix this issue, one can
+   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+   * absolute threshold, while maintaining the ratio of speeds between modules.
+   *
+   * @param moduleStates Reference to array of module states. The array will be mutated with the
+   *     normalized speeds!
+   * @param attainableMaxSpeed The absolute max speed that a module can reach.
+   */
+  public static void desaturateWheelSpeeds(
+      SwerveModuleState[] moduleStates, Measure<Velocity<Distance>> attainableMaxSpeed) {
+    desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond));
+  }
+
+  /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well
    * as getting rid of joystick saturation at edges of joystick.
    *
@@ -348,4 +374,36 @@
       moduleState.speedMetersPerSecond *= scale;
     }
   }
+
+  /**
+   * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well
+   * as getting rid of joystick saturation at edges of joystick.
+   *
+   * <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
+   * above the max attainable speed for the driving motor on that module. To fix this issue, one can
+   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+   * absolute threshold, while maintaining the ratio of speeds between modules.
+   *
+   * @param moduleStates Reference to array of module states. The array will be mutated with the
+   *     normalized speeds!
+   * @param desiredChassisSpeed The desired speed of the robot
+   * @param attainableMaxModuleSpeed The absolute max speed that a module can reach
+   * @param attainableMaxTranslationalSpeed The absolute max speed that your robot can reach while
+   *     translating
+   * @param attainableMaxRotationalVelocity The absolute max speed the robot can reach while
+   *     rotating
+   */
+  public static void desaturateWheelSpeeds(
+      SwerveModuleState[] moduleStates,
+      ChassisSpeeds desiredChassisSpeed,
+      Measure<Velocity<Distance>> attainableMaxModuleSpeed,
+      Measure<Velocity<Distance>> attainableMaxTranslationalSpeed,
+      Measure<Velocity<Angle>> attainableMaxRotationalVelocity) {
+    desaturateWheelSpeeds(
+        moduleStates,
+        desiredChassisSpeed,
+        attainableMaxModuleSpeed.in(MetersPerSecond),
+        attainableMaxTranslationalSpeed.in(MetersPerSecond),
+        attainableMaxRotationalVelocity.in(RadiansPerSecond));
+  }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
index e88f044..5d0f0af 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
@@ -7,7 +7,9 @@
 import java.util.Arrays;
 import java.util.Objects;
 
+/** Represents the wheel positions for a swerve drive drivetrain. */
 public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
+  /** The distances driven by the wheels. */
   public SwerveModulePosition[] positions;
 
   /**
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
index d058764..3041d42 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
@@ -4,20 +4,37 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.kinematics.proto.SwerveModulePositionProto;
+import edu.wpi.first.math.kinematics.struct.SwerveModulePositionStruct;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 
 /** Represents the state of one swerve module. */
 public class SwerveModulePosition
-    implements Comparable<SwerveModulePosition>, Interpolatable<SwerveModulePosition> {
+    implements Comparable<SwerveModulePosition>,
+        Interpolatable<SwerveModulePosition>,
+        ProtobufSerializable,
+        StructSerializable {
   /** Distance measured by the wheel of the module. */
   public double distanceMeters;
 
   /** Angle of the module. */
   public Rotation2d angle = Rotation2d.fromDegrees(0);
 
+  /** SwerveModulePosition protobuf for serialization. */
+  public static final SwerveModulePositionProto proto = new SwerveModulePositionProto();
+
+  /** SwerveModulePosition struct for serialization. */
+  public static final SwerveModulePositionStruct struct = new SwerveModulePositionStruct();
+
   /** Constructs a SwerveModulePosition with zeros for distance and angle. */
   public SwerveModulePosition() {}
 
@@ -32,6 +49,16 @@
     this.angle = angle;
   }
 
+  /**
+   * Constructs a SwerveModulePosition.
+   *
+   * @param distance The distance measured by the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModulePosition(Measure<Distance> distance, Rotation2d angle) {
+    this(distance.in(Meters), angle);
+  }
+
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModulePosition) {
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
index 10dee4f..9f1604a 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
@@ -4,17 +4,33 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto;
+import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 
 /** Represents the state of one swerve module. */
-public class SwerveModuleState implements Comparable<SwerveModuleState> {
+public class SwerveModuleState
+    implements Comparable<SwerveModuleState>, ProtobufSerializable, StructSerializable {
   /** Speed of the wheel of the module. */
   public double speedMetersPerSecond;
 
   /** Angle of the module. */
   public Rotation2d angle = Rotation2d.fromDegrees(0);
 
+  /** SwerveModuleState protobuf for serialization. */
+  public static final SwerveModuleStateProto proto = new SwerveModuleStateProto();
+
+  /** SwerveModuleState struct for serialization. */
+  public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct();
+
   /** Constructs a SwerveModuleState with zeros for speed and angle. */
   public SwerveModuleState() {}
 
@@ -29,6 +45,16 @@
     this.angle = angle;
   }
 
+  /**
+   * Constructs a SwerveModuleState.
+   *
+   * @param speed The speed of the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModuleState(Measure<Velocity<Distance>> speed, Rotation2d angle) {
+    this(speed.in(MetersPerSecond), angle);
+  }
+
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModuleState) {
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
index 029a0ac..c0167e3 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
@@ -6,6 +6,11 @@
 
 import edu.wpi.first.math.interpolation.Interpolatable;
 
+/**
+ * Interface for wheel positions.
+ *
+ * @param <T> Wheel positions type.
+ */
 public interface WheelPositions<T extends WheelPositions<T>> extends Interpolatable<T> {
   /**
    * Returns a copy of this instance.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java
new file mode 100644
index 0000000..b20d23b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java
@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufChassisSpeeds;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class ChassisSpeedsProto implements Protobuf<ChassisSpeeds, ProtobufChassisSpeeds> {
+  @Override
+  public Class<ChassisSpeeds> getTypeClass() {
+    return ChassisSpeeds.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufChassisSpeeds.getDescriptor();
+  }
+
+  @Override
+  public ProtobufChassisSpeeds createMessage() {
+    return ProtobufChassisSpeeds.newInstance();
+  }
+
+  @Override
+  public ChassisSpeeds unpack(ProtobufChassisSpeeds msg) {
+    return new ChassisSpeeds(msg.getVx(), msg.getVy(), msg.getOmega());
+  }
+
+  @Override
+  public void pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) {
+    msg.setVx(value.vxMetersPerSecond);
+    msg.setVy(value.vyMetersPerSecond);
+    msg.setOmega(value.omegaRadiansPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java
new file mode 100644
index 0000000..d1a8969
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java
@@ -0,0 +1,38 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveKinematics;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DifferentialDriveKinematicsProto
+    implements Protobuf<DifferentialDriveKinematics, ProtobufDifferentialDriveKinematics> {
+  @Override
+  public Class<DifferentialDriveKinematics> getTypeClass() {
+    return DifferentialDriveKinematics.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveKinematics.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveKinematics createMessage() {
+    return ProtobufDifferentialDriveKinematics.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveKinematics unpack(ProtobufDifferentialDriveKinematics msg) {
+    return new DifferentialDriveKinematics(msg.getTrackWidth());
+  }
+
+  @Override
+  public void pack(ProtobufDifferentialDriveKinematics msg, DifferentialDriveKinematics value) {
+    msg.setTrackWidth(value.trackWidthMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java
new file mode 100644
index 0000000..e30df56
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java
@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelSpeeds;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DifferentialDriveWheelSpeedsProto
+    implements Protobuf<DifferentialDriveWheelSpeeds, ProtobufDifferentialDriveWheelSpeeds> {
+  @Override
+  public Class<DifferentialDriveWheelSpeeds> getTypeClass() {
+    return DifferentialDriveWheelSpeeds.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveWheelSpeeds.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveWheelSpeeds createMessage() {
+    return ProtobufDifferentialDriveWheelSpeeds.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveWheelSpeeds unpack(ProtobufDifferentialDriveWheelSpeeds msg) {
+    return new DifferentialDriveWheelSpeeds(msg.getLeft(), msg.getRight());
+  }
+
+  @Override
+  public void pack(ProtobufDifferentialDriveWheelSpeeds msg, DifferentialDriveWheelSpeeds value) {
+    msg.setLeft(value.leftMetersPerSecond);
+    msg.setRight(value.rightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java
new file mode 100644
index 0000000..f5b8f3b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java
@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveKinematics;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class MecanumDriveKinematicsProto
+    implements Protobuf<MecanumDriveKinematics, ProtobufMecanumDriveKinematics> {
+  @Override
+  public Class<MecanumDriveKinematics> getTypeClass() {
+    return MecanumDriveKinematics.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufMecanumDriveKinematics.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Translation2d.proto};
+  }
+
+  @Override
+  public ProtobufMecanumDriveKinematics createMessage() {
+    return ProtobufMecanumDriveKinematics.newInstance();
+  }
+
+  @Override
+  public MecanumDriveKinematics unpack(ProtobufMecanumDriveKinematics msg) {
+    return new MecanumDriveKinematics(
+        Translation2d.proto.unpack(msg.getFrontLeft()),
+        Translation2d.proto.unpack(msg.getFrontRight()),
+        Translation2d.proto.unpack(msg.getRearLeft()),
+        Translation2d.proto.unpack(msg.getRearRight()));
+  }
+
+  @Override
+  public void pack(ProtobufMecanumDriveKinematics msg, MecanumDriveKinematics value) {
+    Translation2d.proto.pack(msg.getMutableFrontLeft(), value.getFrontLeft());
+    Translation2d.proto.pack(msg.getMutableFrontRight(), value.getFrontRight());
+    Translation2d.proto.pack(msg.getMutableRearLeft(), value.getRearLeft());
+    Translation2d.proto.pack(msg.getMutableRearRight(), value.getRearRight());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java
new file mode 100644
index 0000000..d4f0f5a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java
@@ -0,0 +1,42 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelPositions;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class MecanumDriveWheelPositionsProto
+    implements Protobuf<MecanumDriveWheelPositions, ProtobufMecanumDriveWheelPositions> {
+  @Override
+  public Class<MecanumDriveWheelPositions> getTypeClass() {
+    return MecanumDriveWheelPositions.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufMecanumDriveWheelPositions.getDescriptor();
+  }
+
+  @Override
+  public ProtobufMecanumDriveWheelPositions createMessage() {
+    return ProtobufMecanumDriveWheelPositions.newInstance();
+  }
+
+  @Override
+  public MecanumDriveWheelPositions unpack(ProtobufMecanumDriveWheelPositions msg) {
+    return new MecanumDriveWheelPositions(
+        msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight());
+  }
+
+  @Override
+  public void pack(ProtobufMecanumDriveWheelPositions msg, MecanumDriveWheelPositions value) {
+    msg.setFrontLeft(value.frontLeftMeters);
+    msg.setFrontRight(value.frontRightMeters);
+    msg.setRearLeft(value.rearLeftMeters);
+    msg.setRearRight(value.rearRightMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java
new file mode 100644
index 0000000..85e8fd1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java
@@ -0,0 +1,42 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelSpeeds;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class MecanumDriveWheelSpeedsProto
+    implements Protobuf<MecanumDriveWheelSpeeds, ProtobufMecanumDriveWheelSpeeds> {
+  @Override
+  public Class<MecanumDriveWheelSpeeds> getTypeClass() {
+    return MecanumDriveWheelSpeeds.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufMecanumDriveWheelSpeeds.getDescriptor();
+  }
+
+  @Override
+  public ProtobufMecanumDriveWheelSpeeds createMessage() {
+    return ProtobufMecanumDriveWheelSpeeds.newInstance();
+  }
+
+  @Override
+  public MecanumDriveWheelSpeeds unpack(ProtobufMecanumDriveWheelSpeeds msg) {
+    return new MecanumDriveWheelSpeeds(
+        msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight());
+  }
+
+  @Override
+  public void pack(ProtobufMecanumDriveWheelSpeeds msg, MecanumDriveWheelSpeeds value) {
+    msg.setFrontLeft(value.frontLeftMetersPerSecond);
+    msg.setFrontRight(value.frontRightMetersPerSecond);
+    msg.setRearLeft(value.rearLeftMetersPerSecond);
+    msg.setRearRight(value.rearRightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java
new file mode 100644
index 0000000..fe5ec5f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java
@@ -0,0 +1,45 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModulePosition;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class SwerveModulePositionProto
+    implements Protobuf<SwerveModulePosition, ProtobufSwerveModulePosition> {
+  @Override
+  public Class<SwerveModulePosition> getTypeClass() {
+    return SwerveModulePosition.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufSwerveModulePosition.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Rotation2d.proto};
+  }
+
+  @Override
+  public ProtobufSwerveModulePosition createMessage() {
+    return ProtobufSwerveModulePosition.newInstance();
+  }
+
+  @Override
+  public SwerveModulePosition unpack(ProtobufSwerveModulePosition msg) {
+    return new SwerveModulePosition(msg.getDistance(), Rotation2d.proto.unpack(msg.getAngle()));
+  }
+
+  @Override
+  public void pack(ProtobufSwerveModulePosition msg, SwerveModulePosition value) {
+    msg.setDistance(value.distanceMeters);
+    Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java
new file mode 100644
index 0000000..dc30f91
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java
@@ -0,0 +1,45 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleState;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class SwerveModuleStateProto
+    implements Protobuf<SwerveModuleState, ProtobufSwerveModuleState> {
+  @Override
+  public Class<SwerveModuleState> getTypeClass() {
+    return SwerveModuleState.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufSwerveModuleState.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Rotation2d.proto};
+  }
+
+  @Override
+  public ProtobufSwerveModuleState createMessage() {
+    return ProtobufSwerveModuleState.newInstance();
+  }
+
+  @Override
+  public SwerveModuleState unpack(ProtobufSwerveModuleState msg) {
+    return new SwerveModuleState(msg.getSpeed(), Rotation2d.proto.unpack(msg.getAngle()));
+  }
+
+  @Override
+  public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) {
+    msg.setSpeed(value.speedMetersPerSecond);
+    Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java
new file mode 100644
index 0000000..ea26f86
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java
@@ -0,0 +1,46 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class ChassisSpeedsStruct implements Struct<ChassisSpeeds> {
+  @Override
+  public Class<ChassisSpeeds> getTypeClass() {
+    return ChassisSpeeds.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:ChassisSpeeds";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 3;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double vx;double vy;double omega";
+  }
+
+  @Override
+  public ChassisSpeeds unpack(ByteBuffer bb) {
+    double vx = bb.getDouble();
+    double vy = bb.getDouble();
+    double omega = bb.getDouble();
+    return new ChassisSpeeds(vx, vy, omega);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, ChassisSpeeds value) {
+    bb.putDouble(value.vxMetersPerSecond);
+    bb.putDouble(value.vyMetersPerSecond);
+    bb.putDouble(value.omegaRadiansPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java
new file mode 100644
index 0000000..1ddadaa
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java
@@ -0,0 +1,42 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DifferentialDriveKinematicsStruct implements Struct<DifferentialDriveKinematics> {
+  @Override
+  public Class<DifferentialDriveKinematics> getTypeClass() {
+    return DifferentialDriveKinematics.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:DifferentialDriveKinematics";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double track_width";
+  }
+
+  @Override
+  public DifferentialDriveKinematics unpack(ByteBuffer bb) {
+    double trackWidth = bb.getDouble();
+    return new DifferentialDriveKinematics(trackWidth);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveKinematics value) {
+    bb.putDouble(value.trackWidthMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java
new file mode 100644
index 0000000..c124247
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java
@@ -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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DifferentialDriveWheelSpeedsStruct implements Struct<DifferentialDriveWheelSpeeds> {
+  @Override
+  public Class<DifferentialDriveWheelSpeeds> getTypeClass() {
+    return DifferentialDriveWheelSpeeds.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:DifferentialDriveWheelSpeeds";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double left;double right";
+  }
+
+  @Override
+  public DifferentialDriveWheelSpeeds unpack(ByteBuffer bb) {
+    double left = bb.getDouble();
+    double right = bb.getDouble();
+    return new DifferentialDriveWheelSpeeds(left, right);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveWheelSpeeds value) {
+    bb.putDouble(value.leftMetersPerSecond);
+    bb.putDouble(value.rightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java
new file mode 100644
index 0000000..227f80e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java
@@ -0,0 +1,55 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class MecanumDriveKinematicsStruct implements Struct<MecanumDriveKinematics> {
+  @Override
+  public Class<MecanumDriveKinematics> getTypeClass() {
+    return MecanumDriveKinematics.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:MecanumDriveKinematics";
+  }
+
+  @Override
+  public int getSize() {
+    return 4 * Translation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation2d front_left;Translation2d front_right;Translation2d rear_left;"
+        + "Translation2d rear_right";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation2d.struct};
+  }
+
+  @Override
+  public MecanumDriveKinematics unpack(ByteBuffer bb) {
+    Translation2d frontLeft = Translation2d.struct.unpack(bb);
+    Translation2d frontRight = Translation2d.struct.unpack(bb);
+    Translation2d rearLeft = Translation2d.struct.unpack(bb);
+    Translation2d rearRight = Translation2d.struct.unpack(bb);
+    return new MecanumDriveKinematics(frontLeft, frontRight, rearLeft, rearRight);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, MecanumDriveKinematics value) {
+    Translation2d.struct.pack(bb, value.getFrontLeft());
+    Translation2d.struct.pack(bb, value.getFrontRight());
+    Translation2d.struct.pack(bb, value.getRearLeft());
+    Translation2d.struct.pack(bb, value.getRearRight());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java
new file mode 100644
index 0000000..9f8b182
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java
@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class MecanumDriveWheelPositionsStruct implements Struct<MecanumDriveWheelPositions> {
+  @Override
+  public Class<MecanumDriveWheelPositions> getTypeClass() {
+    return MecanumDriveWheelPositions.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:MecanumDriveWheelPositions";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double front_left;double front_right;double rear_left;double rear_right";
+  }
+
+  @Override
+  public MecanumDriveWheelPositions unpack(ByteBuffer bb) {
+    double frontLeft = bb.getDouble();
+    double frontRight = bb.getDouble();
+    double rearLeft = bb.getDouble();
+    double rearRight = bb.getDouble();
+    return new MecanumDriveWheelPositions(frontLeft, frontRight, rearLeft, rearRight);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, MecanumDriveWheelPositions value) {
+    bb.putDouble(value.frontLeftMeters);
+    bb.putDouble(value.frontRightMeters);
+    bb.putDouble(value.rearLeftMeters);
+    bb.putDouble(value.rearRightMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java
new file mode 100644
index 0000000..46d1276
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java
@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class MecanumDriveWheelSpeedsStruct implements Struct<MecanumDriveWheelSpeeds> {
+  @Override
+  public Class<MecanumDriveWheelSpeeds> getTypeClass() {
+    return MecanumDriveWheelSpeeds.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:MecanumDriveWheelSpeeds";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double front_left;double front_right;double rear_left;double rear_right";
+  }
+
+  @Override
+  public MecanumDriveWheelSpeeds unpack(ByteBuffer bb) {
+    double frontLeft = bb.getDouble();
+    double frontRight = bb.getDouble();
+    double rearLeft = bb.getDouble();
+    double rearRight = bb.getDouble();
+    return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, MecanumDriveWheelSpeeds value) {
+    bb.putDouble(value.frontLeftMetersPerSecond);
+    bb.putDouble(value.frontRightMetersPerSecond);
+    bb.putDouble(value.rearLeftMetersPerSecond);
+    bb.putDouble(value.rearRightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java
new file mode 100644
index 0000000..2156732
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java
@@ -0,0 +1,50 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class SwerveModulePositionStruct implements Struct<SwerveModulePosition> {
+  @Override
+  public Class<SwerveModulePosition> getTypeClass() {
+    return SwerveModulePosition.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:SwerveModulePosition";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble + Rotation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "double distance;Rotation2d angle";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Rotation2d.struct};
+  }
+
+  @Override
+  public SwerveModulePosition unpack(ByteBuffer bb) {
+    double distance = bb.getDouble();
+    Rotation2d angle = Rotation2d.struct.unpack(bb);
+    return new SwerveModulePosition(distance, angle);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, SwerveModulePosition value) {
+    bb.putDouble(value.distanceMeters);
+    Rotation2d.struct.pack(bb, value.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java
new file mode 100644
index 0000000..51dd61f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java
@@ -0,0 +1,50 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class SwerveModuleStateStruct implements Struct<SwerveModuleState> {
+  @Override
+  public Class<SwerveModuleState> getTypeClass() {
+    return SwerveModuleState.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:SwerveModuleState";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble + Rotation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "double speed;Rotation2d angle";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Rotation2d.struct};
+  }
+
+  @Override
+  public SwerveModuleState unpack(ByteBuffer bb) {
+    double speed = bb.getDouble();
+    Rotation2d angle = Rotation2d.struct.unpack(bb);
+    return new SwerveModuleState(speed, angle);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, SwerveModuleState value) {
+    bb.putDouble(value.speedMetersPerSecond);
+    Rotation2d.struct.pack(bb, value.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/optimization/SimulatedAnnealing.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/optimization/SimulatedAnnealing.java
new file mode 100644
index 0000000..f0ead5b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/optimization/SimulatedAnnealing.java
@@ -0,0 +1,85 @@
+// 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.
+
+package edu.wpi.first.math.optimization;
+
+import java.util.function.Function;
+import java.util.function.ToDoubleFunction;
+
+/**
+ * An implementation of the Simulated Annealing stochastic nonlinear optimization method.
+ *
+ * <p>Solving optimization problems involves tweaking decision variables to try to minimize some
+ * cost function. Simulated annealing is good for solving optimization problems with many local
+ * minima and a very large search space (it’s a heuristic solver rather than an exact solver like,
+ * say, SQP or interior-point method). Simulated annealing is a popular choice for solving the
+ * traveling salesman problem (see {@link edu.wpi.first.math.path.TravelingSalesman}).
+ *
+ * @see <a
+ *     href="https://en.wikipedia.org/wiki/Simulated_annealing">https://en.wikipedia.org/wiki/Simulated_annealing</a>
+ * @param <State> The type of the state to optimize.
+ */
+public final class SimulatedAnnealing<State> {
+  private final double m_initialTemperature;
+  private final Function<State, State> m_neighbor;
+  private final ToDoubleFunction<State> m_cost;
+
+  /**
+   * Constructor for Simulated Annealing that can be used for the same functions but with different
+   * initial states.
+   *
+   * @param initialTemperature The initial temperature. Higher temperatures make it more likely a
+   *     worse state will be accepted during iteration, helping to avoid local minima. The
+   *     temperature is decreased over time.
+   * @param neighbor Function that generates a random neighbor of the current state.
+   * @param cost Function that returns the scalar cost of a state.
+   */
+  public SimulatedAnnealing(
+      double initialTemperature, Function<State, State> neighbor, ToDoubleFunction<State> cost) {
+    m_initialTemperature = initialTemperature;
+    m_neighbor = neighbor;
+    m_cost = cost;
+  }
+
+  /**
+   * Runs the Simulated Annealing algorithm.
+   *
+   * @param initialGuess The initial state.
+   * @param iterations Number of iterations to run the solver.
+   * @return The optimized stater.
+   */
+  public State solve(State initialGuess, int iterations) {
+    State minState = initialGuess;
+    double minCost = Double.MAX_VALUE;
+
+    State state = initialGuess;
+    double cost = m_cost.applyAsDouble(state);
+
+    for (int i = 0; i < iterations; ++i) {
+      double temperature = m_initialTemperature / i;
+
+      State proposedState = m_neighbor.apply(state);
+      double proposedCost = m_cost.applyAsDouble(proposedState);
+      double deltaCost = proposedCost - cost;
+
+      double acceptanceProbability = Math.exp(-deltaCost / temperature);
+
+      // If cost went down or random number exceeded acceptance probability,
+      // accept the proposed state
+      if (deltaCost < 0 || acceptanceProbability >= Math.random()) {
+        state = proposedState;
+        cost = proposedCost;
+      }
+
+      // If proposed cost is less than minimum, the proposed state becomes the
+      // new minimum
+      if (proposedCost < minCost) {
+        minState = proposedState;
+        minCost = proposedCost;
+      }
+    }
+
+    return minState;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/path/TravelingSalesman.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/path/TravelingSalesman.java
new file mode 100644
index 0000000..83dd93e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/path/TravelingSalesman.java
@@ -0,0 +1,114 @@
+// 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.
+
+package edu.wpi.first.math.path;
+
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.optimization.SimulatedAnnealing;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.function.ToDoubleBiFunction;
+
+/**
+ * Given a list of poses, this class finds the shortest possible route that visits each pose exactly
+ * once and returns to the origin pose.
+ *
+ * @see <a
+ *     href="https://en.wikipedia.org/wiki/Travelling_salesman_problem">https://en.wikipedia.org/wiki/Travelling_salesman_problem</a>
+ */
+public class TravelingSalesman {
+  // Default cost is 2D distance between poses
+  private final ToDoubleBiFunction<Pose2d, Pose2d> m_cost;
+
+  /**
+   * Constructs a traveling salesman problem solver with a cost function defined as the 2D distance
+   * between poses.
+   */
+  public TravelingSalesman() {
+    this((Pose2d a, Pose2d b) -> Math.hypot(a.getX() - b.getX(), a.getY() - b.getY()));
+  }
+
+  /**
+   * Constructs a traveling salesman problem solver with a user-provided cost function.
+   *
+   * @param cost Function that returns the cost between two poses. The sum of the costs for every
+   *     pair of poses is minimized.
+   */
+  public TravelingSalesman(ToDoubleBiFunction<Pose2d, Pose2d> cost) {
+    m_cost = cost;
+  }
+
+  /**
+   * Finds the path through every pose that minimizes the cost. The first pose in the returned array
+   * is the first pose that was passed in.
+   *
+   * @param <Poses> A Num defining the length of the path and the number of poses.
+   * @param poses An array of Pose2ds the path must pass through.
+   * @param iterations The number of times the solver attempts to find a better random neighbor.
+   * @return The optimized path as an array of Pose2ds.
+   */
+  public <Poses extends Num> Pose2d[] solve(Pose2d[] poses, int iterations) {
+    var solver =
+        new SimulatedAnnealing<>(
+            1.0,
+            this::neighbor,
+            // Total cost is sum of all costs between adjacent pose pairs in path
+            (Vector<Poses> state) -> {
+              double sum = 0.0;
+              for (int i = 0; i < state.getNumRows(); ++i) {
+                sum +=
+                    m_cost.applyAsDouble(
+                        poses[(int) state.get(i, 0)],
+                        poses[(int) (state.get((i + 1) % poses.length, 0))]);
+              }
+              return sum;
+            });
+
+    var initial = new Vector<Poses>(() -> poses.length);
+    for (int i = 0; i < poses.length; ++i) {
+      initial.set(i, 0, i);
+    }
+
+    var indices = solver.solve(initial, iterations);
+
+    var solution = new Pose2d[poses.length];
+    for (int i = 0; i < poses.length; ++i) {
+      solution[i] = poses[(int) indices.get(i, 0)];
+    }
+
+    // Rotate solution list until solution[0] = poses[0]
+    Collections.rotate(Arrays.asList(solution), -Arrays.asList(solution).indexOf(poses[0]));
+
+    return solution;
+  }
+
+  /**
+   * A random neighbor is generated to try to replace the current one.
+   *
+   * @param state A vector that is a list of indices that defines the path through the path array.
+   * @return Generates a random neighbor of the current state by flipping a random range in the path
+   *     array.
+   */
+  private <Poses extends Num> Vector<Poses> neighbor(Vector<Poses> state) {
+    var proposedState = new Vector<>(state);
+
+    int rangeStart = (int) (Math.random() * (state.getNumRows() - 1));
+    int rangeEnd = (int) (Math.random() * (state.getNumRows() - 1));
+    if (rangeEnd < rangeStart) {
+      int temp = rangeEnd;
+      rangeEnd = rangeStart;
+      rangeStart = temp;
+    }
+
+    for (int i = rangeStart; i <= (rangeStart + rangeEnd) / 2; ++i) {
+      double temp = proposedState.get(i, 0);
+      proposedState.set(i, 0, state.get(rangeEnd - (i - rangeStart), 0));
+      proposedState.set(rangeEnd - (i - rangeStart), 0, temp);
+    }
+
+    return proposedState;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
index 6e13039..72108a5 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
@@ -6,10 +6,14 @@
 
 import org.ejml.simple.SimpleMatrix;
 
+/** Represents a hermite spline of degree 3. */
 public class CubicHermiteSpline extends Spline {
   private static SimpleMatrix hermiteBasis;
   private final SimpleMatrix m_coefficients;
 
+  private final ControlVector m_initialControlVector;
+  private final ControlVector m_finalControlVector;
+
   /**
    * Constructs a cubic hermite spline with the specified control vectors. Each control vector
    * contains info about the location of the point and its first derivative.
@@ -60,6 +64,10 @@
       m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
       m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
     }
+
+    // Assign member variables.
+    m_initialControlVector = new ControlVector(xInitialControlVector, yInitialControlVector);
+    m_finalControlVector = new ControlVector(xFinalControlVector, yFinalControlVector);
   }
 
   /**
@@ -68,11 +76,31 @@
    * @return The coefficients matrix.
    */
   @Override
-  protected SimpleMatrix getCoefficients() {
+  public SimpleMatrix getCoefficients() {
     return m_coefficients;
   }
 
   /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  @Override
+  public ControlVector getInitialControlVector() {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  @Override
+  public ControlVector getFinalControlVector() {
+    return m_finalControlVector;
+  }
+
+  /**
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
    *
    * @return The hermite basis matrix for cubic hermite spline interpolation.
@@ -121,8 +149,8 @@
    * @return The control vector matrix for a dimension.
    */
   private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
-    if (initialVector.length != 2 || finalVector.length != 2) {
-      throw new IllegalArgumentException("Size of vectors must be 2");
+    if (initialVector.length < 2 || finalVector.length < 2) {
+      throw new IllegalArgumentException("Size of vectors must be 2 or greater.");
     }
     return new SimpleMatrix(
         4,
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
index 30b3cae..fbeeb35 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
@@ -8,10 +8,10 @@
 
 /** Represents a pair of a pose and a curvature. */
 public class PoseWithCurvature {
-  // Represents the pose.
+  /** Represents the pose. */
   public Pose2d poseMeters;
 
-  // Represents the curvature.
+  /** Represents the curvature. */
   public double curvatureRadPerMeter;
 
   /**
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
index be90999..5d38bef 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
@@ -6,10 +6,14 @@
 
 import org.ejml.simple.SimpleMatrix;
 
+/** Represents a hermite spline of degree 5. */
 public class QuinticHermiteSpline extends Spline {
   private static SimpleMatrix hermiteBasis;
   private final SimpleMatrix m_coefficients;
 
+  private final ControlVector m_initialControlVector;
+  private final ControlVector m_finalControlVector;
+
   /**
    * Constructs a quintic hermite spline with the specified control vectors. Each control vector
    * contains into about the location of the point, its first derivative, and its second derivative.
@@ -60,6 +64,10 @@
       m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
       m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
     }
+
+    // Assign member variables.
+    m_initialControlVector = new ControlVector(xInitialControlVector, yInitialControlVector);
+    m_finalControlVector = new ControlVector(xFinalControlVector, yFinalControlVector);
   }
 
   /**
@@ -68,11 +76,31 @@
    * @return The coefficients matrix.
    */
   @Override
-  protected SimpleMatrix getCoefficients() {
+  public SimpleMatrix getCoefficients() {
     return m_coefficients;
   }
 
   /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  @Override
+  public ControlVector getInitialControlVector() {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  @Override
+  public ControlVector getFinalControlVector() {
+    return m_finalControlVector;
+  }
+
+  /**
    * Returns the hermite basis matrix for quintic hermite spline interpolation.
    *
    * @return The hermite basis matrix for quintic hermite spline interpolation.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
index 83b35f3..dc6c5e6 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
@@ -27,7 +27,21 @@
    *
    * @return The coefficients of the spline.
    */
-  protected abstract SimpleMatrix getCoefficients();
+  public abstract SimpleMatrix getCoefficients();
+
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  public abstract ControlVector getInitialControlVector();
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  public abstract ControlVector getFinalControlVector();
 
   /**
    * Gets the pose and curvature at some point t on the spline.
@@ -85,7 +99,10 @@
    * the value of x[2] is the second derivative in the x dimension.
    */
   public static class ControlVector {
+    /** The x components of the control vector. */
     public double[] x;
+
+    /** The y components of the control vector. */
     public double[] y;
 
     /**
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
index 651b028..3ba2714 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
@@ -8,7 +8,9 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import java.util.Arrays;
 import java.util.List;
+import org.ejml.simple.SimpleMatrix;
 
+/** Helper class that is used to generate cubic and quintic splines from user provided waypoints. */
 public final class SplineHelper {
   /** Private constructor because this is a utility class. */
   private SplineHelper() {}
@@ -218,6 +220,79 @@
   }
 
   /**
+   * Optimizes the curvature of 2 or more quintic splines at knot points. Overall, this reduces the
+   * integral of the absolute value of the second derivative across the set of splines.
+   *
+   * @param splines An array of un-optimized quintic splines.
+   * @return An array of optimized quintic splines.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static QuinticHermiteSpline[] optimizeCurvature(QuinticHermiteSpline[] splines) {
+    // If there's only spline in the array, we can't optimize anything so just return that.
+    if (splines.length < 2) {
+      return splines;
+    }
+
+    // Implements Section 4.1.2 of
+    // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf.
+
+    // Cubic splines minimize the integral of the second derivative's absolute value. Therefore, we
+    // can create cubic splines with the same 0th and 1st derivatives and the provided quintic
+    // splines, find the second derivative of those cubic splines and then use a weighted average
+    // for the second derivatives of the quintic splines.
+
+    QuinticHermiteSpline[] optimizedSplines = new QuinticHermiteSpline[splines.length];
+    for (int i = 0; i < splines.length - 1; ++i) {
+      QuinticHermiteSpline a = splines[i];
+      QuinticHermiteSpline b = splines[i + 1];
+
+      // Get the control vectors that created the quintic splines above.
+      Spline.ControlVector aInitial = a.getInitialControlVector();
+      Spline.ControlVector aFinal = a.getFinalControlVector();
+      Spline.ControlVector bInitial = b.getInitialControlVector();
+      Spline.ControlVector bFinal = b.getFinalControlVector();
+
+      // Create cubic splines with the same control vectors.
+      CubicHermiteSpline ca = new CubicHermiteSpline(aInitial.x, aFinal.x, aInitial.y, aFinal.y);
+      CubicHermiteSpline cb = new CubicHermiteSpline(bInitial.x, bFinal.x, bInitial.y, bFinal.y);
+
+      // Calculate the second derivatives at the knot points.
+      SimpleMatrix bases = new SimpleMatrix(4, 1, true, new double[] {1, 1, 1, 1});
+      SimpleMatrix combinedA = ca.getCoefficients().mult(bases);
+
+      double ddxA = combinedA.get(4, 0);
+      double ddyA = combinedA.get(5, 0);
+      double ddxB = cb.getCoefficients().get(4, 1);
+      double ddyB = cb.getCoefficients().get(5, 1);
+
+      // Calculate the parameters for the weighted average.
+      double dAB = Math.hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]);
+      double dBC = Math.hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]);
+      double alpha = dBC / (dAB + dBC);
+      double beta = dAB / (dAB + dBC);
+
+      // Calculate the weighted average.
+      double ddx = alpha * ddxA + beta * ddxB;
+      double ddy = alpha * ddyA + beta * ddyB;
+
+      // Create new splines.
+      optimizedSplines[i] =
+          new QuinticHermiteSpline(
+              aInitial.x,
+              new double[] {aFinal.x[0], aFinal.x[1], ddx},
+              aInitial.y,
+              new double[] {aFinal.y[0], aFinal.y[1], ddy});
+      optimizedSplines[i + 1] =
+          new QuinticHermiteSpline(
+              new double[] {bInitial.x[0], bInitial.x[1], ddx},
+              bFinal.x,
+              new double[] {bInitial.y[0], bInitial.y[1], ddy},
+              bFinal.y);
+    }
+    return optimizedSplines;
+  }
+
+  /**
    * Thomas algorithm for solving tridiagonal systems Af = d.
    *
    * @param a the values of A above the diagonal
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
index b2a34a5..5cf742b 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
@@ -56,6 +56,7 @@
     }
   }
 
+  /** Exception for malformed splines. */
   public static class MalformedSplineException extends RuntimeException {
     /**
      * Create a new exception with the given message.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
index b2fe9c8..b4afdf4 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.math.Pair;
 import org.ejml.simple.SimpleMatrix;
 
+/** Discretization helper functions. */
 public final class Discretization {
   private Discretization() {
     // Utility class
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
index d7b6602..18deaa9 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
@@ -8,6 +8,18 @@
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 
+/**
+ * A plant defined using state-space notation.
+ *
+ * <p>A plant is a mathematical model of a system's dynamics.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
 public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
   /** Continuous system matrix. */
   private final Matrix<States, States> m_A;
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
index 02b1da0..94017fc 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
@@ -25,8 +25,12 @@
  * of the controller (U is an output because that's what goes to the motors and Y is an input
  * because that's what comes back from the sensors).
  *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
  */
 public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs extends Num> {
   private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
@@ -265,7 +269,7 @@
    *
    * @param initialState The initial state.
    */
-  public void reset(Matrix<States, N1> initialState) {
+  public final void reset(Matrix<States, N1> initialState) {
     m_nextR.fill(0.0);
     m_controller.reset();
     m_feedforward.reset(initialState);
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
index e9b46fc..36d7bcb 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
@@ -11,6 +11,7 @@
 import java.util.function.DoubleFunction;
 import java.util.function.Function;
 
+/** Numerical integration utilities. */
 public final class NumericalIntegration {
   private NumericalIntegration() {
     // utility Class
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
index 43eba93..1be22a8 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
@@ -11,6 +11,7 @@
 import java.util.function.BiFunction;
 import java.util.function.Function;
 
+/** Numerical Jacobian utilities. */
 public final class NumericalJacobian {
   private NumericalJacobian() {
     // Utility Class.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
index 0c6e334..6d18846 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
@@ -4,19 +4,44 @@
 
 package edu.wpi.first.math.system.plant;
 
+import edu.wpi.first.math.system.plant.proto.DCMotorProto;
+import edu.wpi.first.math.system.plant.struct.DCMotorStruct;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 
 /** Holds the constants for a DC motor. */
-public class DCMotor {
+public class DCMotor implements ProtobufSerializable, StructSerializable {
+  /** Voltage at which the motor constants were measured. */
   public final double nominalVoltageVolts;
+
+  /** Torque when stalled. */
   public final double stallTorqueNewtonMeters;
+
+  /** Current draw when stalled. */
   public final double stallCurrentAmps;
+
+  /** Current draw under no load. */
   public final double freeCurrentAmps;
+
+  /** Angular velocity under no load. */
   public final double freeSpeedRadPerSec;
+
+  /** Motor internal resistance. */
   public final double rOhms;
+
+  /** Motor velocity constant. */
   public final double KvRadPerSecPerVolt;
+
+  /** Motor torque constant. */
   public final double KtNMPerAmp;
 
+  /** DCMotor protobuf for serialization. */
+  public static final DCMotorProto proto = new DCMotorProto();
+
+  /** DCMotor struct for serialization. */
+  public static final DCMotorStruct struct = new DCMotorStruct();
+
   /**
    * Constructs a DC motor.
    *
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
index 8377022..2e7d4b9 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.system.plant;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
@@ -11,6 +12,7 @@
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 
+/** Linear system ID utility functions. */
 public final class LinearSystemId {
   private LinearSystemId() {
     // Utility class
@@ -23,37 +25,34 @@
    * @param motor The motor (or gearbox) attached to the carriage.
    * @param massKg The mass of the elevator carriage, in kilograms.
    * @param radiusMeters The radius of the elevator's driving drum, in meters.
-   * @param G The reduction between motor and drum, as a ratio of output to input.
+   * @param gearing The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or G &lt;= 0.
+   * @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or gearing &lt;= 0.
    */
   public static LinearSystem<N2, N1, N1> createElevatorSystem(
-      DCMotor motor, double massKg, double radiusMeters, double G) {
+      DCMotor motor, double massKg, double radiusMeters, double gearing) {
     if (massKg <= 0.0) {
       throw new IllegalArgumentException("massKg must be greater than zero.");
     }
     if (radiusMeters <= 0.0) {
       throw new IllegalArgumentException("radiusMeters must be greater than zero.");
     }
-    if (G <= 0) {
-      throw new IllegalArgumentException("G must be greater than zero.");
+    if (gearing <= 0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
     }
 
     return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2())
-            .fill(
-                0,
-                1,
-                0,
-                -Math.pow(G, 2)
-                    * motor.KtNMPerAmp
-                    / (motor.rOhms
-                        * radiusMeters
-                        * radiusMeters
-                        * massKg
-                        * motor.KvRadPerSecPerVolt)),
-        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
-        Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
+        MatBuilder.fill(
+            Nat.N2(),
+            Nat.N2(),
+            0,
+            1,
+            0,
+            -Math.pow(gearing, 2)
+                * motor.KtNMPerAmp
+                / (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)),
+        VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
+        MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
 
@@ -63,26 +62,26 @@
    *
    * @param motor The motor (or gearbox) attached to the flywheel.
    * @param JKgMetersSquared The moment of inertia J of the flywheel.
-   * @param G The reduction between motor and drum, as a ratio of output to input.
+   * @param gearing The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or G &lt;= 0.
+   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or gearing &lt;= 0.
    */
   public static LinearSystem<N1, N1, N1> createFlywheelSystem(
-      DCMotor motor, double JKgMetersSquared, double G) {
+      DCMotor motor, double JKgMetersSquared, double gearing) {
     if (JKgMetersSquared <= 0.0) {
       throw new IllegalArgumentException("J must be greater than zero.");
     }
-    if (G <= 0.0) {
-      throw new IllegalArgumentException("G must be greater than zero.");
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
     }
 
     return new LinearSystem<>(
         VecBuilder.fill(
-            -G
-                * G
+            -gearing
+                * gearing
                 * motor.KtNMPerAmp
                 / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
-        VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
+        VecBuilder.fill(gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
         Matrix.eye(Nat.N1()),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
@@ -94,30 +93,31 @@
    *
    * @param motor The motor (or gearbox) attached to system.
    * @param JKgMetersSquared The moment of inertia J of the DC motor.
-   * @param G The reduction between motor and drum, as a ratio of output to input.
+   * @param gearing The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or G &lt;= 0.
+   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or gearing &lt;= 0.
    */
   public static LinearSystem<N2, N1, N2> createDCMotorSystem(
-      DCMotor motor, double JKgMetersSquared, double G) {
+      DCMotor motor, double JKgMetersSquared, double gearing) {
     if (JKgMetersSquared <= 0.0) {
       throw new IllegalArgumentException("J must be greater than zero.");
     }
-    if (G <= 0.0) {
-      throw new IllegalArgumentException("G must be greater than zero.");
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
     }
 
     return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2())
-            .fill(
-                0,
-                1,
-                0,
-                -G
-                    * G
-                    * motor.KtNMPerAmp
-                    / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
-        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
+        MatBuilder.fill(
+            Nat.N2(),
+            Nat.N2(),
+            0,
+            1,
+            0,
+            -gearing
+                * gearing
+                * motor.KtNMPerAmp
+                / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
+        VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
         Matrix.eye(Nat.N2()),
         new Matrix<>(Nat.N2(), Nat.N1()));
   }
@@ -149,7 +149,7 @@
     }
 
     return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA),
         VecBuilder.fill(0, 1 / kA),
         Matrix.eye(Nat.N2()),
         new Matrix<>(Nat.N2(), Nat.N1()));
@@ -165,9 +165,10 @@
    * @param rMeters The radius of the wheels in meters.
    * @param rbMeters The radius of the base (half the track width) in meters.
    * @param JKgMetersSquared The moment of inertia of the robot.
-   * @param G The gearing reduction as output over input.
+   * @param gearing The gearing reduction as output over input.
    * @return A LinearSystem representing a differential drivetrain.
-   * @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or G &lt;= 0.
+   * @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or gearing
+   *     &lt;= 0.
    */
   public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
       DCMotor motor,
@@ -175,7 +176,7 @@
       double rMeters,
       double rbMeters,
       double JKgMetersSquared,
-      double G) {
+      double gearing) {
     if (massKg <= 0.0) {
       throw new IllegalArgumentException("massKg must be greater than zero.");
     }
@@ -188,20 +189,22 @@
     if (JKgMetersSquared <= 0.0) {
       throw new IllegalArgumentException("JKgMetersSquared must be greater than zero.");
     }
-    if (G <= 0.0) {
-      throw new IllegalArgumentException("G must be greater than zero.");
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
     }
 
     var C1 =
-        -(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
-    var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters);
+        -(gearing * gearing)
+            * motor.KtNMPerAmp
+            / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
+    var C2 = gearing * motor.KtNMPerAmp / (motor.rOhms * rMeters);
 
     final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
     final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
-    var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C1, C4 * C1, C4 * C1, C3 * C1);
-    var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C2, C4 * C2, C4 * C2, C3 * C2);
-    var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0);
-    var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0);
+    var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1);
+    var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2);
+    var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
+    var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
 
     return new LinearSystem<>(A, B, C, D);
   }
@@ -212,30 +215,31 @@
    *
    * @param motor The motor (or gearbox) attached to the arm.
    * @param JKgSquaredMeters The moment of inertia J of the arm.
-   * @param G The gearing between the motor and arm, in output over input. Most of the time this
-   *     will be greater than 1.
+   * @param gearing The gearing between the motor and arm, in output over input. Most of the time
+   *     this will be greater than 1.
    * @return A LinearSystem representing the given characterized constants.
    */
   public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
-      DCMotor motor, double JKgSquaredMeters, double G) {
+      DCMotor motor, double JKgSquaredMeters, double gearing) {
     if (JKgSquaredMeters <= 0.0) {
       throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero.");
     }
-    if (G <= 0.0) {
-      throw new IllegalArgumentException("G must be greater than zero.");
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
     }
 
     return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2())
-            .fill(
-                0,
-                1,
-                0,
-                -Math.pow(G, 2)
-                    * motor.KtNMPerAmp
-                    / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
-        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
-        Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
+        MatBuilder.fill(
+            Nat.N2(),
+            Nat.N2(),
+            0,
+            1,
+            0,
+            -Math.pow(gearing, 2)
+                * motor.KtNMPerAmp
+                / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
+        VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
+        MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
 
@@ -299,9 +303,9 @@
     }
 
     return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
         VecBuilder.fill(0.0, 1.0 / kA),
-        Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0),
+        MatBuilder.fill(Nat.N1(), Nat.N2(), 1.0, 0.0),
         VecBuilder.fill(0.0));
   }
 
@@ -344,10 +348,10 @@
     final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
 
     return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
+        MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0));
   }
 
   /**
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java
new file mode 100644
index 0000000..f9e4aa3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java
@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.system.plant.proto;
+
+import edu.wpi.first.math.proto.Plant.ProtobufDCMotor;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DCMotorProto implements Protobuf<DCMotor, ProtobufDCMotor> {
+  @Override
+  public Class<DCMotor> getTypeClass() {
+    return DCMotor.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDCMotor.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDCMotor createMessage() {
+    return ProtobufDCMotor.newInstance();
+  }
+
+  @Override
+  public DCMotor unpack(ProtobufDCMotor msg) {
+    return new DCMotor(
+        msg.getNominalVoltage(),
+        msg.getStallTorque(),
+        msg.getStallCurrent(),
+        msg.getFreeCurrent(),
+        msg.getFreeSpeed(),
+        1);
+  }
+
+  @Override
+  public void pack(ProtobufDCMotor msg, DCMotor value) {
+    msg.setNominalVoltage(value.nominalVoltageVolts);
+    msg.setStallTorque(value.stallTorqueNewtonMeters);
+    msg.setStallCurrent(value.stallCurrentAmps);
+    msg.setFreeCurrent(value.freeCurrentAmps);
+    msg.setFreeSpeed(value.freeSpeedRadPerSec);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java
new file mode 100644
index 0000000..da159fc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java
@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.math.system.plant.struct;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DCMotorStruct implements Struct<DCMotor> {
+  @Override
+  public Class<DCMotor> getTypeClass() {
+    return DCMotor.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:DCMotor";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 5;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double nominal_voltage;double stall_torque;double stall_current;"
+        + "double free_current;double free_speed";
+  }
+
+  @Override
+  public DCMotor unpack(ByteBuffer bb) {
+    double nominalVoltage = bb.getDouble();
+    double stallTorque = bb.getDouble();
+    double stallCurrent = bb.getDouble();
+    double freeCurrent = bb.getDouble();
+    double freeSpeed = bb.getDouble();
+    return new DCMotor(nominalVoltage, stallTorque, stallCurrent, freeCurrent, freeSpeed, 1);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DCMotor value) {
+    bb.putDouble(value.nominalVoltageVolts);
+    bb.putDouble(value.stallTorqueNewtonMeters);
+    bb.putDouble(value.stallCurrentAmps);
+    bb.putDouble(value.freeCurrentAmps);
+    bb.putDouble(value.freeSpeedRadPerSec);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
index 1580e85..46b0c14 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
@@ -39,10 +39,20 @@
 public class ExponentialProfile {
   private final Constraints m_constraints;
 
+  /** Profile timing. */
   public static class ProfileTiming {
+    /** Profile inflection time. */
     public final double inflectionTime;
+
+    /** Total profile time. */
     public final double totalTime;
 
+    /**
+     * Constructs a ProfileTiming.
+     *
+     * @param inflectionTime Profile inflection time.
+     * @param totalTime Total profile time.
+     */
     protected ProfileTiming(double inflectionTime, double totalTime) {
       this.inflectionTime = inflectionTime;
       this.totalTime = totalTime;
@@ -55,14 +65,19 @@
      * @return if the profile is finished at time t.
      */
     public boolean isFinished(double t) {
-      return t > inflectionTime;
+      return t >= inflectionTime;
     }
   }
 
+  /** Profile constraints. */
   public static class Constraints {
+    /** Maximum unsigned input voltage. */
     public final double maxInput;
 
+    /** The State-Space 1x1 system matrix. */
     public final double A;
+
+    /** The State-Space 1x1 input matrix. */
     public final double B;
 
     /**
@@ -112,10 +127,16 @@
     }
   }
 
+  /** Profile state. */
   public static class State {
-    public final double position;
+    /** The position at this state. */
+    public double position;
 
-    public final double velocity;
+    /** The velocity at this state. */
+    public double velocity;
+
+    /** Default constructor. */
+    public State() {}
 
     /**
      * Construct a state within an exponential profile.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
index f7120be..c0dc037 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
@@ -7,6 +7,9 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.trajectory.proto.TrajectoryProto;
+import edu.wpi.first.math.trajectory.proto.TrajectoryStateProto;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Objects;
@@ -16,7 +19,10 @@
  * Represents a time-parameterized trajectory. The trajectory contains of various States that
  * represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
  */
-public class Trajectory {
+public class Trajectory implements ProtobufSerializable {
+  /** Trajectory protobuf for serialization. */
+  public static final TrajectoryProto proto = new TrajectoryProto();
+
   private final double m_totalTimeSeconds;
   private final List<State> m_states;
 
@@ -263,27 +269,31 @@
    * Represents a time-parameterized trajectory. The trajectory contains of various States that
    * represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
    */
-  public static class State {
-    // The time elapsed since the beginning of the trajectory.
+  public static class State implements ProtobufSerializable {
+    /** Trajectory.State protobuf for serialization. */
+    public static final TrajectoryStateProto proto = new TrajectoryStateProto();
+
+    /** The time elapsed since the beginning of the trajectory. */
     @JsonProperty("time")
     public double timeSeconds;
 
-    // The speed at that point of the trajectory.
+    /** The speed at that point of the trajectory. */
     @JsonProperty("velocity")
     public double velocityMetersPerSecond;
 
-    // The acceleration at that point of the trajectory.
+    /** The acceleration at that point of the trajectory. */
     @JsonProperty("acceleration")
     public double accelerationMetersPerSecondSq;
 
-    // The pose at that point of the trajectory.
+    /** The pose at that point of the trajectory. */
     @JsonProperty("pose")
     public Pose2d poseMeters;
 
-    // The curvature at that point of the trajectory.
+    /** The curvature at that point of the trajectory. */
     @JsonProperty("curvature")
     public double curvatureRadPerMeter;
 
+    /** Default constructor. */
     public State() {
       poseMeters = new Pose2d();
     }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java
index fbf734f..931efe5 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java
@@ -4,6 +4,9 @@
 
 package edu.wpi.first.math.trajectory;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
+
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -11,6 +14,9 @@
 import edu.wpi.first.math.trajectory.constraint.MecanumDriveKinematicsConstraint;
 import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
 import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -44,6 +50,18 @@
   }
 
   /**
+   * Constructs the trajectory configuration class.
+   *
+   * @param maxVelocity The max velocity for the trajectory.
+   * @param maxAcceleration The max acceleration for the trajectory.
+   */
+  public TrajectoryConfig(
+      Measure<Velocity<Distance>> maxVelocity,
+      Measure<Velocity<Velocity<Distance>>> maxAcceleration) {
+    this(maxVelocity.in(MetersPerSecond), maxAcceleration.in(MetersPerSecondPerSecond));
+  }
+
+  /**
    * Adds a user-defined constraint to the trajectory.
    *
    * @param constraint The user-defined constraint.
@@ -122,6 +140,16 @@
   }
 
   /**
+   * Sets the start velocity of the trajectory.
+   *
+   * @param startVelocity The start velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setStartVelocity(Measure<Velocity<Distance>> startVelocity) {
+    return setStartVelocity(startVelocity.in(MetersPerSecond));
+  }
+
+  /**
    * Returns the starting velocity of the trajectory.
    *
    * @return The starting velocity of the trajectory.
@@ -142,6 +170,16 @@
   }
 
   /**
+   * Sets the end velocity of the trajectory.
+   *
+   * @param endVelocity The end velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setEndVelocity(Measure<Velocity<Distance>> endVelocity) {
+    return setEndVelocity(endVelocity.in(MetersPerSecond));
+  }
+
+  /**
    * Returns the maximum velocity of the trajectory.
    *
    * @return The maximum velocity of the trajectory.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
index ce90ce6..fcfef45 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
@@ -19,6 +19,7 @@
 import java.util.List;
 import java.util.function.BiConsumer;
 
+/** Helper class used to generate trajectories with various constraints. */
 public final class TrajectoryGenerator {
   private static final Trajectory kDoNothingTrajectory =
       new Trajectory(List.of(new Trajectory.State()));
@@ -207,7 +208,10 @@
     // Get the spline points
     List<PoseWithCurvature> points;
     try {
-      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
+      points =
+          splinePointsFromSplines(
+              SplineHelper.optimizeCurvature(
+                  SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints)));
     } catch (MalformedSplineException ex) {
       reportError(ex.getMessage(), ex.getStackTrace());
       return kDoNothingTrajectory;
@@ -260,16 +264,27 @@
     return splinePoints;
   }
 
-  // Work around type erasure signatures
+  /** Control vector list type that works around type erasure signatures. */
   public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
-    public ControlVectorList(int initialCapacity) {
-      super(initialCapacity);
-    }
-
+    /** Default constructor. */
     public ControlVectorList() {
       super();
     }
 
+    /**
+     * Constructs a ControlVectorList.
+     *
+     * @param initialCapacity The initial list capacity.
+     */
+    public ControlVectorList(int initialCapacity) {
+      super(initialCapacity);
+    }
+
+    /**
+     * Constructs a ControlVectorList.
+     *
+     * @param collection A collection of spline control vectors.
+     */
     public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
       super(collection);
     }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
index a741aa0..35b2a46 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
@@ -315,7 +315,13 @@
     }
   }
 
+  /** Exception for trajectory generation failure. */
   public static class TrajectoryGenerationException extends RuntimeException {
+    /**
+     * Constructs a TrajectoryGenerationException.
+     *
+     * @param message Exception message.
+     */
     public TrajectoryGenerationException(String message) {
       super(message);
     }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
index 2cbba90..f1b2d6a 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
@@ -12,6 +12,7 @@
 import java.util.ArrayList;
 import java.util.List;
 
+/** Trajectory utilities. */
 public final class TrajectoryUtil {
   private TrajectoryUtil() {
     throw new UnsupportedOperationException("This is a utility class!");
@@ -111,7 +112,13 @@
     return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
   }
 
+  /** Exception for trajectory serialization failure. */
   public static class TrajectorySerializationException extends RuntimeException {
+    /**
+     * Constructs a TrajectorySerializationException.
+     *
+     * @param message The exception message.
+     */
     public TrajectorySerializationException(String message) {
       super(message);
     }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
index fd7494c..5215d3a 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -6,6 +6,9 @@
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
+import edu.wpi.first.units.Velocity;
 import java.util.Objects;
 
 /**
@@ -29,7 +32,7 @@
  *
  * <pre><code>
  * previousProfiledReference =
- * profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
+ * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
  * </code></pre>
  *
  * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
@@ -51,9 +54,12 @@
   private double m_endFullSpeed;
   private double m_endDeccel;
 
+  /** Profile constraints. */
   public static class Constraints {
+    /** Maximum velocity. */
     public final double maxVelocity;
 
+    /** Maximum acceleration. */
     public final double maxAcceleration;
 
     /**
@@ -67,20 +73,53 @@
       this.maxAcceleration = maxAcceleration;
       MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
     }
+
+    /**
+     * Construct constraints for a TrapezoidProfile.
+     *
+     * @param <U> Unit type.
+     * @param maxVelocity maximum velocity
+     * @param maxAcceleration maximum acceleration
+     */
+    public <U extends Unit<U>> Constraints(
+        Measure<Velocity<U>> maxVelocity, Measure<Velocity<Velocity<U>>> maxAcceleration) {
+      this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude());
+    }
   }
 
+  /** Profile state. */
   public static class State {
+    /** The position at this state. */
     public double position;
 
+    /** The velocity at this state. */
     public double velocity;
 
+    /** Default constructor. */
     public State() {}
 
+    /**
+     * Constructs constraints for a Trapezoid Profile.
+     *
+     * @param position The position at this state.
+     * @param velocity The velocity at this state.
+     */
     public State(double position, double velocity) {
       this.position = position;
       this.velocity = velocity;
     }
 
+    /**
+     * Constructs constraints for a Trapezoid Profile.
+     *
+     * @param <U> Unit type.
+     * @param position The position at this state.
+     * @param velocity The velocity at this state.
+     */
+    public <U extends Unit<U>> State(Measure<U> position, Measure<Velocity<U>> velocity) {
+      this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude());
+    }
+
     @Override
     public boolean equals(Object other) {
       if (other instanceof State) {
@@ -212,11 +251,11 @@
    * the profile was at time t = 0.
    *
    * @param t The time since the beginning of the profile.
-   * @param goal The desired state when the profile is complete.
    * @param current The current state.
+   * @param goal The desired state when the profile is complete.
    * @return The position and velocity of the profile at time t.
    */
-  public State calculate(double t, State goal, State current) {
+  public State calculate(double t, State current, State goal) {
     m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
     m_current = direct(current);
     goal = direct(goal);
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
index cc0087c..48490d4 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
@@ -37,8 +37,11 @@
 
   /** Represents a minimum and maximum acceleration. */
   class MinMax {
+    /** The minimum acceleration. */
     public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
-    public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
+
+    /** The maximum acceleration. */
+    public double maxAccelerationMetersPerSecondSq = Double.MAX_VALUE;
 
     /**
      * Constructs a MinMax.
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java
new file mode 100644
index 0000000..92e5ac7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java
@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.math.trajectory.proto;
+
+import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectory;
+import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.util.protobuf.Protobuf;
+import java.util.ArrayList;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class TrajectoryProto implements Protobuf<Trajectory, ProtobufTrajectory> {
+  @Override
+  public Class<Trajectory> getTypeClass() {
+    return Trajectory.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTrajectory.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Trajectory.State.proto};
+  }
+
+  @Override
+  public ProtobufTrajectory createMessage() {
+    return ProtobufTrajectory.newInstance();
+  }
+
+  @Override
+  public Trajectory unpack(ProtobufTrajectory msg) {
+    ArrayList<Trajectory.State> states = new ArrayList<>(msg.getStates().length());
+    for (ProtobufTrajectoryState protoState : msg.getStates()) {
+      states.add(Trajectory.State.proto.unpack(protoState));
+    }
+
+    return new Trajectory(states);
+  }
+
+  @Override
+  public void pack(ProtobufTrajectory msg, Trajectory value) {
+    var states = msg.getMutableStates().reserve(value.getStates().size());
+    for (var item : value.getStates()) {
+      Trajectory.State.proto.pack(states.next(), item);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java
new file mode 100644
index 0000000..e8900bf
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java
@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.math.trajectory.proto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class TrajectoryStateProto implements Protobuf<Trajectory.State, ProtobufTrajectoryState> {
+  @Override
+  public Class<Trajectory.State> getTypeClass() {
+    return Trajectory.State.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTrajectoryState.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Pose2d.proto};
+  }
+
+  @Override
+  public ProtobufTrajectoryState createMessage() {
+    return ProtobufTrajectoryState.newInstance();
+  }
+
+  @Override
+  public Trajectory.State unpack(ProtobufTrajectoryState msg) {
+    return new Trajectory.State(
+        msg.getTime(),
+        msg.getVelocity(),
+        msg.getAcceleration(),
+        Pose2d.proto.unpack(msg.getPose()),
+        msg.getCurvature());
+  }
+
+  @Override
+  public void pack(ProtobufTrajectoryState msg, Trajectory.State value) {
+    msg.setTime(value.timeSeconds);
+    msg.setVelocity(value.velocityMetersPerSecond);
+    msg.setAcceleration(value.accelerationMetersPerSecondSq);
+    Pose2d.proto.pack(msg.getMutablePose(), value.poseMeters);
+    msg.setCurvature(value.curvatureRadPerMeter);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index 758d2e7..5dcf1a1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -18,21 +18,12 @@
                          pose.Rotation().Sin()};
 }
 
-template <>
-bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) {
-  return detail::IsStabilizableImpl<1, 1>(A, B);
-}
-
-template <>
-bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) {
-  return detail::IsStabilizableImpl<2, 1>(A, B);
-}
-
-template <>
-bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
-                                                    const Eigen::MatrixXd& B) {
-  return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(A, B);
-}
+template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
+                                   const Matrixd<1, 1>& B);
+template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
+                                   const Matrixd<2, 1>& B);
+template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
+    const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
 
 Eigen::Vector3d PoseToVector(const Pose2d& pose) {
   return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/PIDController.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 2638f9e..a67d56d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -18,10 +18,32 @@
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
     : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+  bool invalidGains = false;
+  if (Kp < 0.0) {
+    wpi::math::MathSharedStore::ReportError(
+        "Kp must be a non-negative number, got {}!", Kp);
+    invalidGains = true;
+  }
+  if (Ki < 0.0) {
+    wpi::math::MathSharedStore::ReportError(
+        "Ki must be a non-negative number, got {}!", Ki);
+    invalidGains = true;
+  }
+  if (Kd < 0.0) {
+    wpi::math::MathSharedStore::ReportError(
+        "Kd must be a non-negative number, got {}!", Kd);
+    invalidGains = true;
+  }
+  if (invalidGains) {
+    m_Kp = 0.0;
+    m_Ki = 0.0;
+    m_Kd = 0.0;
+    wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
+  }
+
   if (period <= 0_s) {
     wpi::math::MathSharedStore::ReportError(
-        "Controller period must be a non-zero positive number, got {}!",
-        period.value());
+        "Controller period must be a positive number, got {}!", period.value());
     m_period = 20_ms;
     wpi::math::MathSharedStore::ReportWarning(
         "Controller period defaulted to 20ms.");
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp
new file mode 100644
index 0000000..37a55cf
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp
@@ -0,0 +1,33 @@
+// 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 "frc/controller/proto/ArmFeedforwardProto.h"
+
+#include "controller.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::ArmFeedforward>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufArmFeedforward>(arena);
+}
+
+frc::ArmFeedforward wpi::Protobuf<frc::ArmFeedforward>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufArmFeedforward*>(&msg);
+  return frc::ArmFeedforward{
+      units::volt_t{m->ks()},
+      units::volt_t{m->kg()},
+      units::unit_t<frc::ArmFeedforward::kv_unit>{m->kv()},
+      units::unit_t<frc::ArmFeedforward::ka_unit>{m->ka()},
+  };
+}
+
+void wpi::Protobuf<frc::ArmFeedforward>::Pack(
+    google::protobuf::Message* msg, const frc::ArmFeedforward& value) {
+  auto m = static_cast<wpi::proto::ProtobufArmFeedforward*>(msg);
+  m->set_ks(value.kS.value());
+  m->set_kg(value.kG.value());
+  m->set_kv(value.kV.value());
+  m->set_ka(value.kA.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp
new file mode 100644
index 0000000..b0b16b9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp
@@ -0,0 +1,34 @@
+// 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 "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h"
+
+#include "controller.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<
+    frc::DifferentialDriveWheelVoltages>::New(google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufDifferentialDriveWheelVoltages>(arena);
+}
+
+frc::DifferentialDriveWheelVoltages
+wpi::Protobuf<frc::DifferentialDriveWheelVoltages>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const wpi::proto::ProtobufDifferentialDriveWheelVoltages*>(
+          &msg);
+  return frc::DifferentialDriveWheelVoltages{
+      units::volt_t{m->left()},
+      units::volt_t{m->right()},
+  };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveWheelVoltages>::Pack(
+    google::protobuf::Message* msg,
+    const frc::DifferentialDriveWheelVoltages& value) {
+  auto m =
+      static_cast<wpi::proto::ProtobufDifferentialDriveWheelVoltages*>(msg);
+  m->set_left(value.left.value());
+  m->set_right(value.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp
new file mode 100644
index 0000000..4b4bce2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp
@@ -0,0 +1,33 @@
+// 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 "frc/controller/proto/ElevatorFeedforwardProto.h"
+
+#include "controller.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::ElevatorFeedforward>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufElevatorFeedforward>(arena);
+}
+
+frc::ElevatorFeedforward wpi::Protobuf<frc::ElevatorFeedforward>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufElevatorFeedforward*>(&msg);
+  return frc::ElevatorFeedforward{
+      units::volt_t{m->ks()},
+      units::volt_t{m->kg()},
+      units::unit_t<frc::ElevatorFeedforward::kv_unit>{m->kv()},
+      units::unit_t<frc::ElevatorFeedforward::ka_unit>{m->ka()},
+  };
+}
+
+void wpi::Protobuf<frc::ElevatorFeedforward>::Pack(
+    google::protobuf::Message* msg, const frc::ElevatorFeedforward& value) {
+  auto m = static_cast<wpi::proto::ProtobufElevatorFeedforward*>(msg);
+  m->set_ks(value.kS());
+  m->set_kg(value.kG());
+  m->set_kv(value.kV());
+  m->set_ka(value.kA());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp
new file mode 100644
index 0000000..6b0070c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp
@@ -0,0 +1,33 @@
+// 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 "frc/controller/struct/ArmFeedforwardStruct.h"
+
+namespace {
+constexpr size_t kKsOff = 0;
+constexpr size_t kKgOff = kKsOff + 8;
+constexpr size_t kKvOff = kKgOff + 8;
+constexpr size_t kKaOff = kKvOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::ArmFeedforward>;
+
+frc::ArmFeedforward StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::ArmFeedforward{
+      units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
+      units::volt_t{wpi::UnpackStruct<double, kKgOff>(data)},
+      units::unit_t<frc::ArmFeedforward::kv_unit>{
+          wpi::UnpackStruct<double, kKvOff>(data)},
+      units::unit_t<frc::ArmFeedforward::ka_unit>{
+          wpi::UnpackStruct<double, kKaOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::ArmFeedforward& value) {
+  wpi::PackStruct<kKsOff>(data, value.kS());
+  wpi::PackStruct<kKgOff>(data, value.kG());
+  wpi::PackStruct<kKvOff>(data, value.kV());
+  wpi::PackStruct<kKaOff>(data, value.kA());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp
new file mode 100644
index 0000000..22a7930
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp
@@ -0,0 +1,26 @@
+// 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 "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h"
+
+namespace {
+constexpr size_t kLeftOff = 0;
+constexpr size_t kRightOff = kLeftOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;
+
+frc::DifferentialDriveWheelVoltages StructType::Unpack(
+    std::span<const uint8_t> data) {
+  return frc::DifferentialDriveWheelVoltages{
+      units::volt_t{wpi::UnpackStruct<double, kLeftOff>(data)},
+      units::volt_t{wpi::UnpackStruct<double, kRightOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::DifferentialDriveWheelVoltages& value) {
+  wpi::PackStruct<kLeftOff>(data, value.left.value());
+  wpi::PackStruct<kRightOff>(data, value.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp
new file mode 100644
index 0000000..ff28357
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp
@@ -0,0 +1,33 @@
+// 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 "frc/controller/struct/ElevatorFeedforwardStruct.h"
+
+namespace {
+constexpr size_t kKsOff = 0;
+constexpr size_t kKgOff = kKsOff + 8;
+constexpr size_t kKvOff = kKgOff + 8;
+constexpr size_t kKaOff = kKvOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::ElevatorFeedforward>;
+
+frc::ElevatorFeedforward StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::ElevatorFeedforward{
+      units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
+      units::volt_t{wpi::UnpackStruct<double, kKgOff>(data)},
+      units::unit_t<frc::ElevatorFeedforward::kv_unit>{
+          wpi::UnpackStruct<double, kKvOff>(data)},
+      units::unit_t<frc::ElevatorFeedforward::ka_unit>{
+          wpi::UnpackStruct<double, kKaOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::ElevatorFeedforward& value) {
+  wpi::PackStruct<kKsOff>(data, value.kS());
+  wpi::PackStruct<kKgOff>(data, value.kG());
+  wpi::PackStruct<kKvOff>(data, value.kV());
+  wpi::PackStruct<kKaOff>(data, value.kA());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp
new file mode 100644
index 0000000..2458623
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp
@@ -0,0 +1,14 @@
+// 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 "frc/estimator/SteadyStateKalmanFilter.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    SteadyStateKalmanFilter<1, 1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    SteadyStateKalmanFilter<2, 1, 1>;
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index 2e15216..53779d0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -9,7 +9,6 @@
 #include <wpi/json.h>
 
 #include "frc/MathUtil.h"
-#include "geometry2d.pb.h"
 
 using namespace frc;
 
@@ -109,23 +108,3 @@
   pose = Pose2d{json.at("translation").get<Translation2d>(),
                 json.at("rotation").get<Rotation2d>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
-      arena);
-}
-
-frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
-  return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
-                wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
-                                      const frc::Pose2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
index ffbaecb..90f7b49 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
@@ -189,23 +189,3 @@
   pose = Pose3d{json.at("translation").get<Translation3d>(),
                 json.at("rotation").get<Rotation3d>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
-      arena);
-}
-
-frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
-  return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
-                wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
-                                      const frc::Pose3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
index 37afbb8..3aad64c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
@@ -232,24 +232,3 @@
       Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
                  json.at("Y").get<double>(), json.at("Z").get<double>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
-      arena);
-}
-
-frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
-  return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
-}
-
-void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
-                                          const frc::Quaternion& value) {
-  auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
-  m->set_w(value.W());
-  m->set_x(value.X());
-  m->set_y(value.Y());
-  m->set_z(value.Z());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 05a644e..6919302 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -20,21 +20,3 @@
 void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
   rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
-      arena);
-}
-
-frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
-  return frc::Rotation2d{units::radian_t{m->value()}};
-}
-
-void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
-                                          const frc::Rotation2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
-  m->set_value(value.Radians().value());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
index 3a68870..072d023 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
@@ -262,21 +262,3 @@
 void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
   rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
-      arena);
-}
-
-frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
-  return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
-}
-
-void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
-                                          const frc::Rotation3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
-  wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index 77f3cee..157359b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -22,23 +22,3 @@
 Transform2d Transform2d::operator+(const Transform2d& other) const {
   return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTransform2d>(arena);
-}
-
-frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
-  return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
-                     wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
-                                           const frc::Transform2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
index de6c253..556a302 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
@@ -5,7 +5,6 @@
 #include "frc/geometry/Transform3d.h"
 
 #include "frc/geometry/Pose3d.h"
-#include "geometry3d.pb.h"
 
 using namespace frc;
 
@@ -36,23 +35,3 @@
 Transform3d Transform3d::operator+(const Transform3d& other) const {
   return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTransform3d>(arena);
-}
-
-frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
-  return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
-                     wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
-                                           const frc::Transform3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 6d5f315..c875582 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -49,22 +49,3 @@
   translation = Translation2d{units::meter_t{json.at("x").get<double>()},
                               units::meter_t{json.at("y").get<double>()}};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTranslation2d>(arena);
-}
-
-frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
-  return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
-}
-
-void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
-                                             const frc::Translation2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
-  m->set_x(value.X().value());
-  m->set_y(value.Y().value());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index 90e94ae..ecfee1c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
@@ -53,24 +53,3 @@
                               units::meter_t{json.at("y").get<double>()},
                               units::meter_t{json.at("z").get<double>()}};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTranslation3d>(arena);
-}
-
-frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
-  return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
-                            units::meter_t{m->z()}};
-}
-
-void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
-                                             const frc::Translation3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
-  m->set_x(value.X().value());
-  m->set_y(value.Y().value());
-  m->set_z(value.Z().value());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
deleted file mode 100644
index 6c106eb..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-// 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 "frc/geometry/Twist2d.h"
-
-#include "geometry2d.pb.h"
-
-using namespace frc;
-
-google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
-      arena);
-}
-
-frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
-  return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
-                      units::radian_t{m->dtheta()}};
-}
-
-void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,
-                                       const frc::Twist2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTwist2d*>(msg);
-  m->set_dx(value.dx.value());
-  m->set_dy(value.dy.value());
-  m->set_dtheta(value.dtheta.value());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
deleted file mode 100644
index 4f4ce86..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-// 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 "frc/geometry/Twist3d.h"
-
-#include "geometry3d.pb.h"
-
-using namespace frc;
-
-google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
-      arena);
-}
-
-frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
-  return frc::Twist3d{units::meter_t{m->dx()},  units::meter_t{m->dy()},
-                      units::meter_t{m->dz()},  units::radian_t{m->rx()},
-                      units::radian_t{m->ry()}, units::radian_t{m->rz()}};
-}
-
-void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,
-                                       const frc::Twist3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTwist3d*>(msg);
-  m->set_dx(value.dx.value());
-  m->set_dy(value.dy.value());
-  m->set_dz(value.dz.value());
-  m->set_rx(value.rx.value());
-  m->set_ry(value.ry.value());
-  m->set_rz(value.rz.value());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp
new file mode 100644
index 0000000..2b8bf5f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp
@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Pose2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
+      arena);
+}
+
+frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
+  return frc::Pose2d{
+      wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp
new file mode 100644
index 0000000..581cafb
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp
@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Pose3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
+      arena);
+}
+
+frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
+  return frc::Pose3d{
+      wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp
new file mode 100644
index 0000000..aadefa0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp
@@ -0,0 +1,33 @@
+// 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 "frc/geometry/proto/QuaternionProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
+      arena);
+}
+
+frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
+  return frc::Quaternion{
+      m->w(),
+      m->x(),
+      m->y(),
+      m->z(),
+  };
+}
+
+void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
+                                          const frc::Quaternion& value) {
+  auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
+  m->set_w(value.W());
+  m->set_x(value.X());
+  m->set_y(value.Y());
+  m->set_z(value.Z());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp
new file mode 100644
index 0000000..c9005f2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp
@@ -0,0 +1,27 @@
+// 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 "frc/geometry/proto/Rotation2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
+      arena);
+}
+
+frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
+  return frc::Rotation2d{
+      units::radian_t{m->value()},
+  };
+}
+
+void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
+  m->set_value(value.Radians().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp
new file mode 100644
index 0000000..72645e1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp
@@ -0,0 +1,27 @@
+// 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 "frc/geometry/proto/Rotation3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
+      arena);
+}
+
+frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
+  return frc::Rotation3d{
+      wpi::UnpackProtobuf<frc::Quaternion>(m->q()),
+  };
+}
+
+void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
+  wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp
new file mode 100644
index 0000000..4d38b81
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp
@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Transform2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform2d>(arena);
+}
+
+frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
+  return frc::Transform2d{
+      wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp
new file mode 100644
index 0000000..4a8fbfd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp
@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Transform3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform3d>(arena);
+}
+
+frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
+  return frc::Transform3d{
+      wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp
new file mode 100644
index 0000000..739dc99
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp
@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Translation2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation2d>(arena);
+}
+
+frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
+  return frc::Translation2d{
+      units::meter_t{m->x()},
+      units::meter_t{m->y()},
+  };
+}
+
+void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp
new file mode 100644
index 0000000..6285b2b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp
@@ -0,0 +1,31 @@
+// 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 "frc/geometry/proto/Translation3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation3d>(arena);
+}
+
+frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
+  return frc::Translation3d{
+      units::meter_t{m->x()},
+      units::meter_t{m->y()},
+      units::meter_t{m->z()},
+  };
+}
+
+void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+  m->set_z(value.Z().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp
new file mode 100644
index 0000000..2590fc9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp
@@ -0,0 +1,31 @@
+// 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 "frc/geometry/proto/Twist2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
+      arena);
+}
+
+frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
+  return frc::Twist2d{
+      units::meter_t{m->dx()},
+      units::meter_t{m->dy()},
+      units::radian_t{m->dtheta()},
+  };
+}
+
+void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist2d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dtheta(value.dtheta.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp
new file mode 100644
index 0000000..e2d91e4
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp
@@ -0,0 +1,34 @@
+// 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 "frc/geometry/proto/Twist3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
+      arena);
+}
+
+frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
+  return frc::Twist3d{
+      units::meter_t{m->dx()},  units::meter_t{m->dy()},
+      units::meter_t{m->dz()},  units::radian_t{m->rx()},
+      units::radian_t{m->ry()}, units::radian_t{m->rz()},
+  };
+}
+
+void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist3d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dz(value.dz.value());
+  m->set_rx(value.rx.value());
+  m->set_ry(value.ry.value());
+  m->set_rz(value.rz.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp
new file mode 100644
index 0000000..b1bb9ae
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp
@@ -0,0 +1,25 @@
+// 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 "frc/geometry/struct/Pose2dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
+}  // namespace
+
+using StructType = wpi::Struct<frc::Pose2d>;
+
+frc::Pose2d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Pose2d{
+      wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Pose2d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp
new file mode 100644
index 0000000..104a51b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp
@@ -0,0 +1,25 @@
+// 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 "frc/geometry/struct/Pose3dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
+}  // namespace
+
+using StructType = wpi::Struct<frc::Pose3d>;
+
+frc::Pose3d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Pose3d{
+      wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Pose3d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp
new file mode 100644
index 0000000..df5b781
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp
@@ -0,0 +1,30 @@
+// 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 "frc/geometry/struct/QuaternionStruct.h"
+
+namespace {
+constexpr size_t kWOff = 0;
+constexpr size_t kXOff = kWOff + 8;
+constexpr size_t kYOff = kXOff + 8;
+constexpr size_t kZOff = kYOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Quaternion>;
+
+frc::Quaternion StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Quaternion{
+      wpi::UnpackStruct<double, kWOff>(data),
+      wpi::UnpackStruct<double, kXOff>(data),
+      wpi::UnpackStruct<double, kYOff>(data),
+      wpi::UnpackStruct<double, kZOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Quaternion& value) {
+  wpi::PackStruct<kWOff>(data, value.W());
+  wpi::PackStruct<kXOff>(data, value.X());
+  wpi::PackStruct<kYOff>(data, value.Y());
+  wpi::PackStruct<kZOff>(data, value.Z());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp
new file mode 100644
index 0000000..16d3b40
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp
@@ -0,0 +1,21 @@
+// 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 "frc/geometry/struct/Rotation2dStruct.h"
+
+namespace {
+constexpr size_t kValueOff = 0;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Rotation2d>;
+
+frc::Rotation2d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Rotation2d{
+      units::radian_t{wpi::UnpackStruct<double, kValueOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Rotation2d& value) {
+  wpi::PackStruct<kValueOff>(data, value.Radians().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp
new file mode 100644
index 0000000..926c7c1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp
@@ -0,0 +1,21 @@
+// 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 "frc/geometry/struct/Rotation3dStruct.h"
+
+namespace {
+constexpr size_t kQOff = 0;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Rotation3d>;
+
+frc::Rotation3d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Rotation3d{
+      wpi::UnpackStruct<frc::Quaternion, kQOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Rotation3d& value) {
+  wpi::PackStruct<kQOff>(data, value.GetQuaternion());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp
new file mode 100644
index 0000000..35fa6bf
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp
@@ -0,0 +1,25 @@
+// 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 "frc/geometry/struct/Transform2dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
+}  // namespace
+
+using StructType = wpi::Struct<frc::Transform2d>;
+
+frc::Transform2d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Transform2d{
+      wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Transform2d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp
new file mode 100644
index 0000000..b428346
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp
@@ -0,0 +1,25 @@
+// 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 "frc/geometry/struct/Transform3dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
+}  // namespace
+
+using StructType = wpi::Struct<frc::Transform3d>;
+
+frc::Transform3d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Transform3d{
+      wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Transform3d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp
new file mode 100644
index 0000000..edf2574
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp
@@ -0,0 +1,25 @@
+// 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 "frc/geometry/struct/Translation2dStruct.h"
+
+namespace {
+constexpr size_t kXOff = 0;
+constexpr size_t kYOff = kXOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Translation2d>;
+
+frc::Translation2d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Translation2d{
+      units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::Translation2d& value) {
+  wpi::PackStruct<kXOff>(data, value.X().value());
+  wpi::PackStruct<kYOff>(data, value.Y().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp
new file mode 100644
index 0000000..f69306c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp
@@ -0,0 +1,28 @@
+// 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 "frc/geometry/struct/Translation3dStruct.h"
+
+namespace {
+constexpr size_t kXOff = 0;
+constexpr size_t kYOff = kXOff + 8;
+constexpr size_t kZOff = kYOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Translation3d>;
+
+frc::Translation3d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Translation3d{
+      units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kZOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::Translation3d& value) {
+  wpi::PackStruct<kXOff>(data, value.X().value());
+  wpi::PackStruct<kYOff>(data, value.Y().value());
+  wpi::PackStruct<kZOff>(data, value.Z().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp
new file mode 100644
index 0000000..5c71c62
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp
@@ -0,0 +1,27 @@
+// 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 "frc/geometry/struct/Twist2dStruct.h"
+
+namespace {
+constexpr size_t kDxOff = 0;
+constexpr size_t kDyOff = kDxOff + 8;
+constexpr size_t kDthetaOff = kDyOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Twist2d>;
+
+frc::Twist2d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Twist2d{
+      units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kDthetaOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Twist2d& value) {
+  wpi::PackStruct<kDxOff>(data, value.dx.value());
+  wpi::PackStruct<kDyOff>(data, value.dy.value());
+  wpi::PackStruct<kDthetaOff>(data, value.dtheta.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp
new file mode 100644
index 0000000..d933d4b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp
@@ -0,0 +1,36 @@
+// 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 "frc/geometry/struct/Twist3dStruct.h"
+
+namespace {
+constexpr size_t kDxOff = 0;
+constexpr size_t kDyOff = kDxOff + 8;
+constexpr size_t kDzOff = kDyOff + 8;
+constexpr size_t kRxOff = kDzOff + 8;
+constexpr size_t kRyOff = kRxOff + 8;
+constexpr size_t kRzOff = kRyOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Twist3d>;
+
+frc::Twist3d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Twist3d{
+      units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kDzOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kRxOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kRyOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kRzOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Twist3d& value) {
+  wpi::PackStruct<kDxOff>(data, value.dx.value());
+  wpi::PackStruct<kDyOff>(data, value.dy.value());
+  wpi::PackStruct<kDzOff>(data, value.dz.value());
+  wpi::PackStruct<kRxOff>(data, value.rx.value());
+  wpi::PackStruct<kRyOff>(data, value.ry.value());
+  wpi::PackStruct<kRzOff>(data, value.rz.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
index dd30263..0f5de42 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -16,10 +16,10 @@
     units::meters_per_second_t attainableMaxSpeed) {
   std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
                                                         rearLeft, rearRight};
-  units::meters_per_second_t realMaxSpeed = *std::max_element(
+  units::meters_per_second_t realMaxSpeed = units::math::abs(*std::max_element(
       wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
         return units::math::abs(a) < units::math::abs(b);
-      });
+      }));
 
   if (realMaxSpeed > attainableMaxSpeed) {
     for (int i = 0; i < 4; ++i) {
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp
new file mode 100644
index 0000000..097a2fb
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp
@@ -0,0 +1,31 @@
+// 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 "frc/kinematics/proto/ChassisSpeedsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::ChassisSpeeds>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufChassisSpeeds>(arena);
+}
+
+frc::ChassisSpeeds wpi::Protobuf<frc::ChassisSpeeds>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufChassisSpeeds*>(&msg);
+  return frc::ChassisSpeeds{
+      units::meters_per_second_t{m->vx()},
+      units::meters_per_second_t{m->vy()},
+      units::radians_per_second_t{m->omega()},
+  };
+}
+
+void wpi::Protobuf<frc::ChassisSpeeds>::Pack(google::protobuf::Message* msg,
+                                             const frc::ChassisSpeeds& value) {
+  auto m = static_cast<wpi::proto::ProtobufChassisSpeeds*>(msg);
+  m->set_vx(value.vx.value());
+  m->set_vy(value.vy.value());
+  m->set_omega(value.omega.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp
new file mode 100644
index 0000000..a79e565
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp
@@ -0,0 +1,30 @@
+// 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 "frc/kinematics/proto/DifferentialDriveKinematicsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::DifferentialDriveKinematics>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufDifferentialDriveKinematics>(arena);
+}
+
+frc::DifferentialDriveKinematics
+wpi::Protobuf<frc::DifferentialDriveKinematics>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const wpi::proto::ProtobufDifferentialDriveKinematics*>(&msg);
+  return frc::DifferentialDriveKinematics{
+      units::meter_t{m->track_width()},
+  };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveKinematics>::Pack(
+    google::protobuf::Message* msg,
+    const frc::DifferentialDriveKinematics& value) {
+  auto m = static_cast<wpi::proto::ProtobufDifferentialDriveKinematics*>(msg);
+  m->set_track_width(value.trackWidth.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp
new file mode 100644
index 0000000..cb38b84
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp
@@ -0,0 +1,32 @@
+// 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 "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<
+    frc::DifferentialDriveWheelSpeeds>::New(google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufDifferentialDriveWheelSpeeds>(arena);
+}
+
+frc::DifferentialDriveWheelSpeeds
+wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufDifferentialDriveWheelSpeeds*>(
+      &msg);
+  return frc::DifferentialDriveWheelSpeeds{
+      units::meters_per_second_t{m->left()},
+      units::meters_per_second_t{m->right()},
+  };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>::Pack(
+    google::protobuf::Message* msg,
+    const frc::DifferentialDriveWheelSpeeds& value) {
+  auto m = static_cast<wpi::proto::ProtobufDifferentialDriveWheelSpeeds*>(msg);
+  m->set_left(value.left.value());
+  m->set_right(value.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp
new file mode 100644
index 0000000..e17102f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp
@@ -0,0 +1,33 @@
+// 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 "frc/kinematics/proto/MecanumDriveKinematicsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::MecanumDriveKinematics>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufMecanumDriveKinematics>(arena);
+}
+
+frc::MecanumDriveKinematics wpi::Protobuf<frc::MecanumDriveKinematics>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufMecanumDriveKinematics*>(&msg);
+  return frc::MecanumDriveKinematics{
+      wpi::UnpackProtobuf<frc::Translation2d>(m->front_left()),
+      wpi::UnpackProtobuf<frc::Translation2d>(m->front_right()),
+      wpi::UnpackProtobuf<frc::Translation2d>(m->rear_left()),
+      wpi::UnpackProtobuf<frc::Translation2d>(m->rear_right()),
+  };
+}
+
+void wpi::Protobuf<frc::MecanumDriveKinematics>::Pack(
+    google::protobuf::Message* msg, const frc::MecanumDriveKinematics& value) {
+  auto m = static_cast<wpi::proto::ProtobufMecanumDriveKinematics*>(msg);
+  wpi::PackProtobuf(m->mutable_front_left(), value.GetFrontLeft());
+  wpi::PackProtobuf(m->mutable_front_right(), value.GetFrontRight());
+  wpi::PackProtobuf(m->mutable_rear_left(), value.GetRearLeft());
+  wpi::PackProtobuf(m->mutable_rear_right(), value.GetRearRight());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp
new file mode 100644
index 0000000..94ca982
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp
@@ -0,0 +1,36 @@
+// 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 "frc/kinematics/proto/MecanumDriveWheelPositionsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::MecanumDriveWheelPositions>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufMecanumDriveWheelPositions>(arena);
+}
+
+frc::MecanumDriveWheelPositions
+wpi::Protobuf<frc::MecanumDriveWheelPositions>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const wpi::proto::ProtobufMecanumDriveWheelPositions*>(&msg);
+  return frc::MecanumDriveWheelPositions{
+      units::meter_t{m->front_left()},
+      units::meter_t{m->front_right()},
+      units::meter_t{m->rear_left()},
+      units::meter_t{m->rear_right()},
+  };
+}
+
+void wpi::Protobuf<frc::MecanumDriveWheelPositions>::Pack(
+    google::protobuf::Message* msg,
+    const frc::MecanumDriveWheelPositions& value) {
+  auto m = static_cast<wpi::proto::ProtobufMecanumDriveWheelPositions*>(msg);
+  m->set_front_left(value.frontLeft.value());
+  m->set_front_right(value.frontRight.value());
+  m->set_rear_left(value.rearLeft.value());
+  m->set_rear_right(value.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp
new file mode 100644
index 0000000..049a088
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp
@@ -0,0 +1,35 @@
+// 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 "frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufMecanumDriveWheelSpeeds>(arena);
+}
+
+frc::MecanumDriveWheelSpeeds
+wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const wpi::proto::ProtobufMecanumDriveWheelSpeeds*>(&msg);
+  return frc::MecanumDriveWheelSpeeds{
+      units::meters_per_second_t{m->front_left()},
+      units::meters_per_second_t{m->front_right()},
+      units::meters_per_second_t{m->rear_left()},
+      units::meters_per_second_t{m->rear_right()},
+  };
+}
+
+void wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::Pack(
+    google::protobuf::Message* msg, const frc::MecanumDriveWheelSpeeds& value) {
+  auto m = static_cast<wpi::proto::ProtobufMecanumDriveWheelSpeeds*>(msg);
+  m->set_front_left(value.frontLeft.value());
+  m->set_front_right(value.frontRight.value());
+  m->set_rear_left(value.rearLeft.value());
+  m->set_rear_right(value.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp
new file mode 100644
index 0000000..4e85ec8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp
@@ -0,0 +1,29 @@
+// 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 "frc/kinematics/proto/SwerveModulePositionProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::SwerveModulePosition>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufSwerveModulePosition>(arena);
+}
+
+frc::SwerveModulePosition wpi::Protobuf<frc::SwerveModulePosition>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufSwerveModulePosition*>(&msg);
+  return frc::SwerveModulePosition{
+      units::meter_t{m->distance()},
+      wpi::UnpackProtobuf<frc::Rotation2d>(m->angle()),
+  };
+}
+
+void wpi::Protobuf<frc::SwerveModulePosition>::Pack(
+    google::protobuf::Message* msg, const frc::SwerveModulePosition& value) {
+  auto m = static_cast<wpi::proto::ProtobufSwerveModulePosition*>(msg);
+  m->set_distance(value.distance.value());
+  wpi::PackProtobuf(m->mutable_angle(), value.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp
new file mode 100644
index 0000000..f5d5a0f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp
@@ -0,0 +1,29 @@
+// 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 "frc/kinematics/proto/SwerveModuleStateProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::SwerveModuleState>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufSwerveModuleState>(arena);
+}
+
+frc::SwerveModuleState wpi::Protobuf<frc::SwerveModuleState>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufSwerveModuleState*>(&msg);
+  return frc::SwerveModuleState{
+      units::meters_per_second_t{m->speed()},
+      wpi::UnpackProtobuf<frc::Rotation2d>(m->angle()),
+  };
+}
+
+void wpi::Protobuf<frc::SwerveModuleState>::Pack(
+    google::protobuf::Message* msg, const frc::SwerveModuleState& value) {
+  auto m = static_cast<wpi::proto::ProtobufSwerveModuleState*>(msg);
+  m->set_speed(value.speed.value());
+  wpi::PackProtobuf(m->mutable_angle(), value.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp
new file mode 100644
index 0000000..3b9ec2b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp
@@ -0,0 +1,28 @@
+// 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 "frc/kinematics/struct/ChassisSpeedsStruct.h"
+
+namespace {
+constexpr size_t kVxOff = 0;
+constexpr size_t kVyOff = kVxOff + 8;
+constexpr size_t kOmegaOff = kVyOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::ChassisSpeeds>;
+
+frc::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::ChassisSpeeds{
+      units::meters_per_second_t{wpi::UnpackStruct<double, kVxOff>(data)},
+      units::meters_per_second_t{wpi::UnpackStruct<double, kVyOff>(data)},
+      units::radians_per_second_t{wpi::UnpackStruct<double, kOmegaOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::ChassisSpeeds& value) {
+  wpi::PackStruct<kVxOff>(data, value.vx.value());
+  wpi::PackStruct<kVyOff>(data, value.vy.value());
+  wpi::PackStruct<kOmegaOff>(data, value.omega.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp
new file mode 100644
index 0000000..b7be0d2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp
@@ -0,0 +1,23 @@
+// 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 "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h"
+
+namespace {
+constexpr size_t kTrackWidthOff = 0;
+}  // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveKinematics>;
+
+frc::DifferentialDriveKinematics StructType::Unpack(
+    std::span<const uint8_t> data) {
+  return frc::DifferentialDriveKinematics{
+      units::meter_t{wpi::UnpackStruct<double, kTrackWidthOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::DifferentialDriveKinematics& value) {
+  wpi::PackStruct<kTrackWidthOff>(data, value.trackWidth.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp
new file mode 100644
index 0000000..e2ae131
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp
@@ -0,0 +1,26 @@
+// 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 "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h"
+
+namespace {
+constexpr size_t kLeftOff = 0;
+constexpr size_t kRightOff = kLeftOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelSpeeds>;
+
+frc::DifferentialDriveWheelSpeeds StructType::Unpack(
+    std::span<const uint8_t> data) {
+  return frc::DifferentialDriveWheelSpeeds{
+      units::meters_per_second_t{wpi::UnpackStruct<double, kLeftOff>(data)},
+      units::meters_per_second_t{wpi::UnpackStruct<double, kRightOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::DifferentialDriveWheelSpeeds& value) {
+  wpi::PackStruct<kLeftOff>(data, value.left.value());
+  wpi::PackStruct<kRightOff>(data, value.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp
new file mode 100644
index 0000000..1a27a28
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp
@@ -0,0 +1,34 @@
+// 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 "frc/kinematics/struct/MecanumDriveKinematicsStruct.h"
+
+namespace {
+constexpr size_t kFrontLeftOff = 0;
+constexpr size_t kFrontRightOff =
+    kFrontLeftOff + wpi::GetStructSize<frc::Translation2d>();
+constexpr size_t kRearLeftOff =
+    kFrontRightOff + wpi::GetStructSize<frc::Translation2d>();
+constexpr size_t kRearRightOff =
+    kRearLeftOff + wpi::GetStructSize<frc::Translation2d>();
+}  // namespace
+
+using StructType = wpi::Struct<frc::MecanumDriveKinematics>;
+
+frc::MecanumDriveKinematics StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::MecanumDriveKinematics{
+      wpi::UnpackStruct<frc::Translation2d, kFrontLeftOff>(data),
+      wpi::UnpackStruct<frc::Translation2d, kFrontRightOff>(data),
+      wpi::UnpackStruct<frc::Translation2d, kRearLeftOff>(data),
+      wpi::UnpackStruct<frc::Translation2d, kRearRightOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::MecanumDriveKinematics& value) {
+  wpi::PackStruct<kFrontLeftOff>(data, value.GetFrontLeft());
+  wpi::PackStruct<kFrontRightOff>(data, value.GetFrontRight());
+  wpi::PackStruct<kRearLeftOff>(data, value.GetRearLeft());
+  wpi::PackStruct<kRearRightOff>(data, value.GetRearRight());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp
new file mode 100644
index 0000000..c857f4d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp
@@ -0,0 +1,32 @@
+// 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 "frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h"
+
+namespace {
+constexpr size_t kFrontLeftOff = 0;
+constexpr size_t kFrontRightOff = kFrontLeftOff + 8;
+constexpr size_t kRearLeftOff = kFrontRightOff + 8;
+constexpr size_t kRearRightOff = kRearLeftOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelPositions>;
+
+frc::MecanumDriveWheelPositions StructType::Unpack(
+    std::span<const uint8_t> data) {
+  return frc::MecanumDriveWheelPositions{
+      units::meter_t{wpi::UnpackStruct<double, kFrontLeftOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kFrontRightOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kRearLeftOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kRearRightOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::MecanumDriveWheelPositions& value) {
+  wpi::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
+  wpi::PackStruct<kFrontRightOff>(data, value.frontRight.value());
+  wpi::PackStruct<kRearLeftOff>(data, value.rearLeft.value());
+  wpi::PackStruct<kRearRightOff>(data, value.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp
new file mode 100644
index 0000000..c232cc9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp
@@ -0,0 +1,34 @@
+// 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 "frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h"
+
+namespace {
+constexpr size_t kFrontLeftOff = 0;
+constexpr size_t kFrontRightOff = kFrontLeftOff + 8;
+constexpr size_t kRearLeftOff = kFrontRightOff + 8;
+constexpr size_t kRearRightOff = kRearLeftOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelSpeeds>;
+
+frc::MecanumDriveWheelSpeeds StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::MecanumDriveWheelSpeeds{
+      units::meters_per_second_t{
+          wpi::UnpackStruct<double, kFrontLeftOff>(data)},
+      units::meters_per_second_t{
+          wpi::UnpackStruct<double, kFrontRightOff>(data)},
+      units::meters_per_second_t{wpi::UnpackStruct<double, kRearLeftOff>(data)},
+      units::meters_per_second_t{
+          wpi::UnpackStruct<double, kRearRightOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::MecanumDriveWheelSpeeds& value) {
+  wpi::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
+  wpi::PackStruct<kFrontRightOff>(data, value.frontRight.value());
+  wpi::PackStruct<kRearLeftOff>(data, value.rearLeft.value());
+  wpi::PackStruct<kRearRightOff>(data, value.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp
new file mode 100644
index 0000000..12ae0a1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp
@@ -0,0 +1,25 @@
+// 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 "frc/kinematics/struct/SwerveModulePositionStruct.h"
+
+namespace {
+constexpr size_t kDistanceOff = 0;
+constexpr size_t kAngleOff = kDistanceOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::SwerveModulePosition>;
+
+frc::SwerveModulePosition StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::SwerveModulePosition{
+      units::meter_t{wpi::UnpackStruct<double, kDistanceOff>(data)},
+      wpi::UnpackStruct<frc::Rotation2d, kAngleOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::SwerveModulePosition& value) {
+  wpi::PackStruct<kDistanceOff>(data, value.distance.value());
+  wpi::PackStruct<kAngleOff>(data, value.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp
new file mode 100644
index 0000000..b045178
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp
@@ -0,0 +1,25 @@
+// 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 "frc/kinematics/struct/SwerveModuleStateStruct.h"
+
+namespace {
+constexpr size_t kSpeedOff = 0;
+constexpr size_t kAngleOff = kSpeedOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::SwerveModuleState>;
+
+frc::SwerveModuleState StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::SwerveModuleState{
+      units::meters_per_second_t{wpi::UnpackStruct<double, kSpeedOff>(data)},
+      wpi::UnpackStruct<frc::Rotation2d, kAngleOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::SwerveModuleState& value) {
+  wpi::PackStruct<kSpeedOff>(data, value.speed.value());
+  wpi::PackStruct<kAngleOff>(data, value.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
index b643849..3885443 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -10,7 +10,9 @@
     wpi::array<double, 2> xInitialControlVector,
     wpi::array<double, 2> xFinalControlVector,
     wpi::array<double, 2> yInitialControlVector,
-    wpi::array<double, 2> yFinalControlVector) {
+    wpi::array<double, 2> yFinalControlVector)
+    : m_initialControlVector{xInitialControlVector, yInitialControlVector},
+      m_finalControlVector{xFinalControlVector, yFinalControlVector} {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
index 5362b7c..65d7986 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -10,7 +10,9 @@
     wpi::array<double, 3> xInitialControlVector,
     wpi::array<double, 3> xFinalControlVector,
     wpi::array<double, 3> yInitialControlVector,
-    wpi::array<double, 3> yFinalControlVector) {
+    wpi::array<double, 3> yFinalControlVector)
+    : m_initialControlVector{xInitialControlVector, yInitialControlVector},
+      m_finalControlVector{xFinalControlVector, yFinalControlVector} {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
index e8bbb46..c78a54f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
@@ -169,6 +169,81 @@
   return splines;
 }
 
+std::vector<QuinticHermiteSpline> SplineHelper::OptimizeCurvature(
+    const std::vector<QuinticHermiteSpline>& splines) {
+  // If there's only one spline in the vector, we can't optimize anything so
+  // just return that.
+  if (splines.size() < 2) {
+    return splines;
+  }
+
+  // Implements Section 4.1.2 of
+  // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf.
+
+  // Cubic splines minimize the integral of the second derivative's absolute
+  // value. Therefore, we can create cubic splines with the same 0th and 1st
+  // derivatives and the provided quintic splines, find the second derivative of
+  // those cubic splines and then use a weighted average for the second
+  // derivatives of the quintic splines.
+
+  std::vector<QuinticHermiteSpline> optimizedSplines;
+  optimizedSplines.reserve(splines.size());
+  optimizedSplines.push_back(splines[0]);
+
+  for (size_t i = 0; i < splines.size() - 1; ++i) {
+    const auto& a = splines[i];
+    const auto& b = splines[i + 1];
+
+    // Get the control vectors that created the quintic splines above.
+    const auto& aInitial = a.GetInitialControlVector();
+    const auto& aFinal = a.GetFinalControlVector();
+    const auto& bInitial = b.GetInitialControlVector();
+    const auto& bFinal = b.GetFinalControlVector();
+
+    // Create cubic splines with the same control vectors.
+    auto Trim = [](const wpi::array<double, 3>& a) {
+      return wpi::array<double, 2>{a[0], a[1]};
+    };
+    CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y),
+                          Trim(aFinal.y)};
+    CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y),
+                          Trim(bFinal.y)};
+
+    // Calculate the second derivatives at the knot points.
+    frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0};
+    frc::Vectord<6> combinedA = ca.Coefficients() * bases;
+
+    double ddxA = combinedA(4);
+    double ddyA = combinedA(5);
+    double ddxB = cb.Coefficients()(4, 1);
+    double ddyB = cb.Coefficients()(5, 1);
+
+    // Calculate the parameters for weighted average.
+    double dAB =
+        std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]);
+    double dBC =
+        std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]);
+    double alpha = dBC / (dAB + dBC);
+    double beta = dAB / (dAB + dBC);
+
+    // Calculate the weighted average.
+    double ddx = alpha * ddxA + beta * ddxB;
+    double ddy = alpha * ddyA + beta * ddyB;
+
+    // Create new splines.
+    optimizedSplines[i] = {aInitial.x,
+                           {aFinal.x[0], aFinal.x[1], ddx},
+                           aInitial.y,
+                           {aFinal.y[0], aFinal.y[1], ddy}};
+    optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx},
+                                bFinal.x,
+                                {bInitial.y[0], bInitial.y[1], ddy},
+                                bFinal.y});
+  }
+
+  return optimizedSplines;
+}
+
 void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
                                    const std::vector<double>& b,
                                    const std::vector<double>& c,
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
index 3527092..11c65ea 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
@@ -9,23 +9,24 @@
 LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
                                                      units::kilogram_t mass,
                                                      units::meter_t radius,
-                                                     double G) {
+                                                     double gearing) {
   if (mass <= 0_kg) {
     throw std::domain_error("mass must be greater than zero.");
   }
   if (radius <= 0_m) {
     throw std::domain_error("radius must be greater than zero.");
   }
-  if (G <= 0.0) {
-    throw std::domain_error("G must be greater than zero.");
+  if (gearing <= 0.0) {
+    throw std::domain_error("gearing must be greater than zero.");
   }
 
   Matrixd<2, 2> A{
       {0.0, 1.0},
-      {0.0, (-std::pow(G, 2) * motor.Kt /
+      {0.0, (-std::pow(gearing, 2) * motor.Kt /
              (motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
                 .value()}};
-  Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * radius * mass)).value()};
+  Matrixd<2, 1> B{0.0,
+                  (gearing * motor.Kt / (motor.R * radius * mass)).value()};
   Matrixd<1, 2> C{1.0, 0.0};
   Matrixd<1, 1> D{0.0};
 
@@ -33,18 +34,19 @@
 }
 
 LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
-    DCMotor motor, units::kilogram_square_meter_t J, double G) {
+    DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
   if (J <= 0_kg_sq_m) {
     throw std::domain_error("J must be greater than zero.");
   }
-  if (G <= 0.0) {
-    throw std::domain_error("G must be greater than zero.");
+  if (gearing <= 0.0) {
+    throw std::domain_error("gearing must be greater than zero.");
   }
 
   Matrixd<2, 2> A{
       {0.0, 1.0},
-      {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
-  Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+      {0.0,
+       (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+  Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
   Matrixd<1, 2> C{1.0, 0.0};
   Matrixd<1, 1> D{0.0};
 
@@ -119,17 +121,17 @@
 }
 
 LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem(
-    DCMotor motor, units::kilogram_square_meter_t J, double G) {
+    DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
   if (J <= 0_kg_sq_m) {
     throw std::domain_error("J must be greater than zero.");
   }
-  if (G <= 0.0) {
-    throw std::domain_error("G must be greater than zero.");
+  if (gearing <= 0.0) {
+    throw std::domain_error("gearing must be greater than zero.");
   }
 
   Matrixd<1, 1> A{
-      (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
-  Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
+      (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
+  Matrixd<1, 1> B{(gearing * motor.Kt / (motor.R * J)).value()};
   Matrixd<1, 1> C{1.0};
   Matrixd<1, 1> D{0.0};
 
@@ -137,18 +139,19 @@
 }
 
 LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem(
-    DCMotor motor, units::kilogram_square_meter_t J, double G) {
+    DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
   if (J <= 0_kg_sq_m) {
     throw std::domain_error("J must be greater than zero.");
   }
-  if (G <= 0.0) {
-    throw std::domain_error("G must be greater than zero.");
+  if (gearing <= 0.0) {
+    throw std::domain_error("gearing must be greater than zero.");
   }
 
   Matrixd<2, 2> A{
       {0.0, 1.0},
-      {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
-  Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+      {0.0,
+       (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+  Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
   Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
   Matrixd<2, 1> D{0.0, 0.0};
 
@@ -157,7 +160,7 @@
 
 LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem(
     const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
-    units::meter_t rb, units::kilogram_square_meter_t J, double G) {
+    units::meter_t rb, units::kilogram_square_meter_t J, double gearing) {
   if (mass <= 0_kg) {
     throw std::domain_error("mass must be greater than zero.");
   }
@@ -170,13 +173,13 @@
   if (J <= 0_kg_sq_m) {
     throw std::domain_error("J must be greater than zero.");
   }
-  if (G <= 0.0) {
-    throw std::domain_error("G must be greater than zero.");
+  if (gearing <= 0.0) {
+    throw std::domain_error("gearing must be greater than zero.");
   }
 
-  auto C1 = -std::pow(G, 2) * motor.Kt /
+  auto C1 = -std::pow(gearing, 2) * motor.Kt /
             (motor.Kv * motor.R * units::math::pow<2>(r));
-  auto C2 = G * motor.Kt / (motor.R * r);
+  auto C2 = gearing * motor.Kt / (motor.R * r);
 
   Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
                    ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp
new file mode 100644
index 0000000..8363270
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp
@@ -0,0 +1,35 @@
+// 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 "frc/system/plant/proto/DCMotorProto.h"
+
+#include "plant.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::DCMotor>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufDCMotor>(
+      arena);
+}
+
+frc::DCMotor wpi::Protobuf<frc::DCMotor>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufDCMotor*>(&msg);
+  return frc::DCMotor{
+      units::volt_t{m->nominal_voltage()},
+      units::newton_meter_t{m->stall_torque()},
+      units::ampere_t{m->stall_current()},
+      units::ampere_t{m->free_current()},
+      units::radians_per_second_t{m->free_speed()},
+  };
+}
+
+void wpi::Protobuf<frc::DCMotor>::Pack(google::protobuf::Message* msg,
+                                       const frc::DCMotor& value) {
+  auto m = static_cast<wpi::proto::ProtobufDCMotor*>(msg);
+  m->set_nominal_voltage(value.nominalVoltage.value());
+  m->set_stall_torque(value.stallTorque.value());
+  m->set_stall_current(value.stallCurrent.value());
+  m->set_free_current(value.freeCurrent.value());
+  m->set_free_speed(value.freeSpeed.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp
new file mode 100644
index 0000000..e7389a1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp
@@ -0,0 +1,34 @@
+// 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 "frc/system/plant/struct/DCMotorStruct.h"
+
+namespace {
+constexpr size_t kNominalVoltageOff = 0;
+constexpr size_t kStallTorqueOff = kNominalVoltageOff + 8;
+constexpr size_t kStallCurrentOff = kStallTorqueOff + 8;
+constexpr size_t kFreeCurrentOff = kStallCurrentOff + 8;
+constexpr size_t kFreeSpeedOff = kFreeCurrentOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::DCMotor>;
+
+frc::DCMotor StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::DCMotor{
+      units::volt_t{wpi::UnpackStruct<double, kNominalVoltageOff>(data)},
+      units::newton_meter_t{wpi::UnpackStruct<double, kStallTorqueOff>(data)},
+      units::ampere_t{wpi::UnpackStruct<double, kStallCurrentOff>(data)},
+      units::ampere_t{wpi::UnpackStruct<double, kFreeCurrentOff>(data)},
+      units::radians_per_second_t{
+          wpi::UnpackStruct<double, kFreeSpeedOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::DCMotor& value) {
+  wpi::PackStruct<kNominalVoltageOff>(data, value.nominalVoltage.value());
+  wpi::PackStruct<kStallTorqueOff>(data, value.stallTorque.value());
+  wpi::PackStruct<kStallCurrentOff>(data, value.stallCurrent.value());
+  wpi::PackStruct<kFreeCurrentOff>(data, value.freeCurrent.value());
+  wpi::PackStruct<kFreeSpeedOff>(data, value.freeSpeed.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index c7a7e9a..922ace3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -121,8 +121,8 @@
 
   std::vector<SplineParameterizer::PoseWithCurvature> points;
   try {
-    points = SplinePointsFromSplines(
-        SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
+    points = SplinePointsFromSplines(SplineHelper::OptimizeCurvature(
+        SplineHelper::QuinticSplinesFromWaypoints(newWaypoints)));
   } catch (SplineParameterizer::MalformedSplineException& e) {
     ReportError(e.what());
     return kDoNothingTrajectory;
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
index da9c955..d9cb853 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -35,7 +35,7 @@
     throw std::runtime_error(fmt::format("Cannot open file: {}", path));
   }
 
-  wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+  wpi::json json = wpi::json::parse(fileBuffer->GetCharBuffer());
 
   return Trajectory{json.get<std::vector<Trajectory::State>>()};
 }
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp
new file mode 100644
index 0000000..d5bbe2b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp
@@ -0,0 +1,33 @@
+// 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 "frc/trajectory/proto/TrajectoryProto.h"
+
+#include "trajectory.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Trajectory>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTrajectory>(
+      arena);
+}
+
+frc::Trajectory wpi::Protobuf<frc::Trajectory>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTrajectory*>(&msg);
+  std::vector<frc::Trajectory::State> states;
+  states.reserve(m->states().size());
+  for (const auto& protoState : m->states()) {
+    states.push_back(wpi::UnpackProtobuf<frc::Trajectory::State>(protoState));
+  }
+  return frc::Trajectory{states};
+}
+
+void wpi::Protobuf<frc::Trajectory>::Pack(google::protobuf::Message* msg,
+                                          const frc::Trajectory& value) {
+  auto m = static_cast<wpi::proto::ProtobufTrajectory*>(msg);
+  m->mutable_states()->Reserve(value.States().size());
+  for (const auto& state : value.States()) {
+    wpi::PackProtobuf(m->add_states(), state);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp
new file mode 100644
index 0000000..f2b80ab
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp
@@ -0,0 +1,35 @@
+// 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 "frc/trajectory/proto/TrajectoryStateProto.h"
+
+#include "trajectory.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Trajectory::State>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTrajectoryState>(arena);
+}
+
+frc::Trajectory::State wpi::Protobuf<frc::Trajectory::State>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTrajectoryState*>(&msg);
+  return frc::Trajectory::State{
+      units::second_t{m->time()},
+      units::meters_per_second_t{m->velocity()},
+      units::meters_per_second_squared_t{m->acceleration()},
+      wpi::UnpackProtobuf<frc::Pose2d>(m->pose()),
+      units::curvature_t{m->curvature()},
+  };
+}
+
+void wpi::Protobuf<frc::Trajectory::State>::Pack(
+    google::protobuf::Message* msg, const frc::Trajectory::State& value) {
+  auto m = static_cast<wpi::proto::ProtobufTrajectoryState*>(msg);
+  m->set_time(value.t.value());
+  m->set_velocity(value.velocity.value());
+  m->set_acceleration(value.acceleration.value());
+  wpi::PackProtobuf(m->mutable_pose(), value.pose);
+  m->set_curvature(value.curvature.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/DARE.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/DARE.h
index 6a3104e..4681d3c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/DARE.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/DARE.h
@@ -75,14 +75,12 @@
   // Require (A, C) pair be detectable where Q = CᵀC
   //
   // Q = CᵀC = PᵀLDLᵀP
-  // Cᵀ = PᵀL√(D)
-  // C = (PᵀL√(D))ᵀ
+  // C = √(D)LᵀP
   {
     Eigen::Matrix<double, States, States> C =
-        (Q_ldlt.transpositionsP().transpose() *
-         Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
-         Q_ldlt.vectorD().cwiseSqrt().asDiagonal())
-            .transpose();
+        Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
+        Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
+        Q_ldlt.transpositionsP();
 
     if (!IsDetectable<States, States>(A, C)) {
       std::string msg = fmt::format(
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 3aa2e75..65bed6f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -12,87 +12,13 @@
 
 #include <Eigen/Eigenvalues>
 #include <Eigen/QR>
+#include <wpi/Algorithm.h>
 #include <wpi/SymbolExports.h>
-#include <wpi/deprecated.h>
 
 #include "frc/EigenCore.h"
 #include "frc/geometry/Pose2d.h"
 
 namespace frc {
-namespace detail {
-
-template <typename Matrix, typename T, typename... Ts>
-void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
-  if (elem == std::numeric_limits<double>::infinity()) {
-    result(result.rows() - (sizeof...(Ts) + 1)) = 0.0;
-  } else {
-    result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
-  }
-  if constexpr (sizeof...(Ts) > 0) {
-    CostMatrixImpl(result, elems...);
-  }
-}
-
-template <typename Matrix, typename T, typename... Ts>
-void CovMatrixImpl(Matrix& result, T elem, Ts... elems) {
-  result(result.rows() - (sizeof...(Ts) + 1)) = std::pow(elem, 2);
-  if constexpr (sizeof...(Ts) > 0) {
-    CovMatrixImpl(result, elems...);
-  }
-}
-
-template <typename Matrix, typename T, typename... Ts>
-void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
-  std::random_device rd;
-  std::mt19937 gen{rd()};
-  std::normal_distribution<> distr{0.0, elem};
-
-  result(result.rows() - (sizeof...(Ts) + 1)) = distr(gen);
-  if constexpr (sizeof...(Ts) > 0) {
-    WhiteNoiseVectorImpl(result, elems...);
-  }
-}
-
-template <int States, int Inputs>
-bool IsStabilizableImpl(const Matrixd<States, States>& A,
-                        const Matrixd<States, Inputs>& B) {
-  Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
-
-  for (int i = 0; i < A.rows(); ++i) {
-    if (std::norm(es.eigenvalues()[i]) < 1) {
-      continue;
-    }
-
-    if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
-      Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
-      E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
-                                               States>::Identity() -
-               A,
-          B;
-
-      Eigen::ColPivHouseholderQR<
-          Eigen::Matrix<std::complex<double>, States, States + Inputs>>
-          qr{E};
-      if (qr.rank() < States) {
-        return false;
-      }
-    } else {
-      Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
-      E << es.eigenvalues()[i] *
-                   Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
-               A,
-          B;
-
-      Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
-      if (qr.rank() < A.rows()) {
-        return false;
-      }
-    }
-  }
-  return true;
-}
-
-}  // namespace detail
 
 /**
  * Creates a cost matrix from the given vector for use with LQR.
@@ -110,7 +36,16 @@
 template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
-  detail::CostMatrixImpl(result.diagonal(), tolerances...);
+  auto& diag = result.diagonal();
+  wpi::for_each(
+      [&](int i, double tolerance) {
+        if (tolerance == std::numeric_limits<double>::infinity()) {
+          diag(i) = 0.0;
+        } else {
+          diag(i) = 1.0 / std::pow(tolerance, 2);
+        }
+      },
+      tolerances...);
   return result;
 }
 
@@ -129,7 +64,9 @@
 template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
-  detail::CovMatrixImpl(result.diagonal(), stdDevs...);
+  auto& diag = result.diagonal();
+  wpi::for_each([&](int i, double stdDev) { diag(i) = std::pow(stdDev, 2); },
+                stdDevs...);
   return result;
 }
 
@@ -150,7 +87,7 @@
 Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
   Eigen::DiagonalMatrix<double, N> result;
   auto& diag = result.diagonal();
-  for (size_t i = 0; i < N; ++i) {
+  for (size_t i = 0; i < costs.size(); ++i) {
     if (costs[i] == std::numeric_limits<double>::infinity()) {
       diag(i) = 0.0;
     } else {
@@ -183,9 +120,23 @@
 }
 
 template <std::same_as<double>... Ts>
-Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
-  Matrixd<sizeof...(Ts), 1> result;
-  detail::WhiteNoiseVectorImpl(result, stdDevs...);
+Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+
+  Vectord<sizeof...(Ts)> result;
+  wpi::for_each(
+      [&](int i, double stdDev) {
+        // Passing a standard deviation of 0.0 to std::normal_distribution is
+        // undefined behavior
+        if (stdDev == 0.0) {
+          result(i) = 0.0;
+        } else {
+          std::normal_distribution distr{0.0, stdDev};
+          result(i) = distr(gen);
+        }
+      },
+      stdDevs...);
   return result;
 }
 
@@ -203,7 +154,7 @@
   std::mt19937 gen{rd()};
 
   Vectord<N> result;
-  for (int i = 0; i < N; ++i) {
+  for (size_t i = 0; i < stdDevs.size(); ++i) {
     // Passing a standard deviation of 0.0 to std::normal_distribution is
     // undefined behavior
     if (stdDevs[i] == 0.0) {
@@ -243,17 +194,58 @@
  * any, have absolute values less than one, where an eigenvalue is
  * uncontrollable if rank([λI - A, B]) < n where n is the number of states.
  *
- * @tparam States The number of states.
- * @tparam Inputs The number of inputs.
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
  * @param A System matrix.
  * @param B Input matrix.
  */
 template <int States, int Inputs>
 bool IsStabilizable(const Matrixd<States, States>& A,
                     const Matrixd<States, Inputs>& B) {
-  return detail::IsStabilizableImpl<States, Inputs>(A, B);
+  Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
+
+  for (int i = 0; i < A.rows(); ++i) {
+    if (std::norm(es.eigenvalues()[i]) < 1) {
+      continue;
+    }
+
+    if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
+      Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
+      E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
+                                               States>::Identity() -
+               A,
+          B;
+
+      Eigen::ColPivHouseholderQR<
+          Eigen::Matrix<std::complex<double>, States, States + Inputs>>
+          qr{E};
+      if (qr.rank() < States) {
+        return false;
+      }
+    } else {
+      Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
+      E << es.eigenvalues()[i] *
+                   Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
+               A,
+          B;
+
+      Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+      if (qr.rank() < A.rows()) {
+        return false;
+      }
+    }
+  }
+  return true;
 }
 
+extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
+    const Matrixd<1, 1>& A, const Matrixd<1, 1>& B);
+extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
+    const Matrixd<2, 2>& A, const Matrixd<2, 1>& B);
+extern template WPILIB_DLLEXPORT bool
+IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
+                                               const Eigen::MatrixXd& B);
+
 /**
  * Returns true if (A, C) is a detectable pair.
  *
@@ -261,36 +253,17 @@
  * any, have absolute values less than one, where an eigenvalue is unobservable
  * if rank([λI - A; C]) < n where n is the number of states.
  *
- * @tparam States The number of states.
- * @tparam Outputs The number of outputs.
+ * @tparam States Number of states.
+ * @tparam Outputs Number of outputs.
  * @param A System matrix.
  * @param C Output matrix.
  */
 template <int States, int Outputs>
 bool IsDetectable(const Matrixd<States, States>& A,
                   const Matrixd<Outputs, States>& C) {
-  return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
-                                                     C.transpose());
+  return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
 }
 
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
-                                           const Matrixd<1, 1>& B);
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
-                                           const Matrixd<2, 1>& B);
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-WPILIB_DLLEXPORT bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
-    const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
-
 /**
  * Converts a Pose2d into a vector of [x, y, theta].
  *
@@ -304,7 +277,7 @@
 /**
  * Clamps input vector between system's minimum and maximum allowable input.
  *
- * @tparam Inputs The number of inputs.
+ * @tparam Inputs Number of inputs.
  * @param u Input vector to clamp.
  * @param umin The minimum input magnitude.
  * @param umax The maximum input magnitude.
@@ -325,7 +298,7 @@
  * Renormalize all inputs if any exceeds the maximum magnitude. Useful for
  * systems such as differential drivetrains.
  *
- * @tparam Inputs      The number of inputs.
+ * @tparam Inputs      Number of inputs.
  * @param u            The input vector.
  * @param maxMagnitude The maximum magnitude any input can have.
  * @return The normalizedInput
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
index 5621803..69091cc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -11,6 +11,7 @@
 #include "units/angular_velocity.h"
 #include "units/math.h"
 #include "units/voltage.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -29,20 +30,31 @@
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
-  constexpr ArmFeedforward() = default;
-
   /**
    * Creates a new ArmFeedforward with the specified gains.
    *
-   * @param kS   The static gain, in volts.
+   * @param kS The static gain, in volts.
    * @param kG The gravity gain, in volts.
-   * @param kV   The velocity gain, in volt seconds per radian.
-   * @param kA   The acceleration gain, in volt seconds² per radian.
+   * @param kV The velocity gain, in volt seconds per radian.
+   * @param kA The acceleration gain, in volt seconds² per radian.
    */
   constexpr ArmFeedforward(
       units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kG(kG), kV(kV), kA(kA) {}
+      : kS(kS), kG(kG), kV(kV), kA(kA) {
+    if (kV.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kV must be a non-negative number, got {}!", kV.value());
+      kV = units::unit_t<kv_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
+    }
+    if (kA.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kA must be a non-negative number, got {}!", kA.value());
+      kA = units::unit_t<ka_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+  }
 
   /**
    * Calculates the feedforward from the gains and setpoints.
@@ -163,9 +175,19 @@
     return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
   }
 
-  units::volt_t kS{0};
-  units::volt_t kG{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
+  /// The static gain, in volts.
+  const units::volt_t kS;
+
+  /// The gravity gain, in volts.
+  const units::volt_t kG;
+
+  /// The velocity gain, in volt seconds per radian.
+  const units::unit_t<kv_unit> kV;
+
+  /// The acceleration gain, in volt seconds² per radian.
+  const units::unit_t<ka_unit> kA;
 };
 }  // namespace frc
+
+#include "frc/controller/proto/ArmFeedforwardProto.h"
+#include "frc/controller/struct/ArmFeedforwardStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 6306457..c3c282d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -34,8 +34,8 @@
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
  *
- * @tparam States The number of states.
- * @tparam Inputs the number of inputs.
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
  */
 template <int States, int Inputs>
 class ControlAffinePlantInversionFeedforward {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h
index 48f341e..424cd2d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h
@@ -12,8 +12,14 @@
  * Motor voltages for a differential drive.
  */
 struct DifferentialDriveWheelVoltages {
+  /// Left wheel voltage.
   units::volt_t left = 0_V;
+
+  /// Right wheel voltage.
   units::volt_t right = 0_V;
 };
 
 }  // namespace frc
+
+#include "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h"
+#include "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
index 62a7bad..07a0499 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -12,6 +12,7 @@
 #include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -29,8 +30,6 @@
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
-  ElevatorFeedforward() = default;
-
   /**
    * Creates a new ElevatorFeedforward with the specified gains.
    *
@@ -42,7 +41,20 @@
   constexpr ElevatorFeedforward(
       units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kG(kG), kV(kV), kA(kA) {}
+      : kS(kS), kG(kG), kV(kV), kA(kA) {
+    if (kV.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kV must be a non-negative number, got {}!", kV.value());
+      kV = units::unit_t<kv_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
+    }
+    if (kA.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kA must be a non-negative number, got {}!", kA.value());
+      kA = units::unit_t<ka_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+  }
 
   /**
    * Calculates the feedforward from the gains and setpoints.
@@ -170,9 +182,19 @@
     return MaxAchievableAcceleration(-maxVoltage, velocity);
   }
 
-  units::volt_t kS{0};
-  units::volt_t kG{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
+  /// The static gain.
+  const units::volt_t kS;
+
+  /// The gravity gain.
+  const units::volt_t kG;
+
+  /// The velocity gain.
+  const units::unit_t<kv_unit> kV;
+
+  /// The acceleration gain.
+  const units::unit_t<ka_unit> kA;
 };
 }  // namespace frc
+
+#include "frc/controller/proto/ElevatorFeedforwardProto.h"
+#include "frc/controller/struct/ElevatorFeedforwardStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
index 3a1230d..f030d48 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
@@ -24,6 +24,9 @@
  *
  * For more on the underlying math, read appendix B.3 in
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
  */
 template <int States, int Inputs>
 class ImplicitModelFollower {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index 1d905e2..fb46b03 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -26,8 +26,8 @@
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
  *
- * @tparam States The number of states.
- * @tparam Inputs The number of inputs.
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
  */
 template <int States, int Inputs>
 class LinearPlantInversionFeedforward {
@@ -38,7 +38,7 @@
   /**
    * Constructs a feedforward with the given plant.
    *
-   * @tparam Outputs The number of outputs.
+   * @tparam Outputs Number of outputs.
    * @param plant The plant being controlled.
    * @param dt    Discretization timestep.
    */
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 979e98a..8389a88 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -40,6 +40,7 @@
    * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
    * for how to select the tolerances.
    *
+   * @tparam Outputs Number of outputs.
    * @param plant  The plant being controlled.
    * @param Qelems The maximum desired error tolerance for each state.
    * @param Relems The maximum desired control effort for each input.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/PIDController.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/PIDController.h
index 0d5b0a3..30ab8f0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/PIDController.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -25,11 +25,11 @@
   /**
    * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
    *
-   * @param Kp     The proportional coefficient.
-   * @param Ki     The integral coefficient.
-   * @param Kd     The derivative coefficient.
+   * @param Kp     The proportional coefficient. Must be >= 0.
+   * @param Ki     The integral coefficient. Must be >= 0.
+   * @param Kd     The derivative coefficient. Must be >= 0.
    * @param period The period between controller updates in seconds. The
-   *               default is 20 milliseconds. Must be non-zero and positive.
+   *               default is 20 milliseconds. Must be positive.
    */
   PIDController(double Kp, double Ki, double Kd,
                 units::second_t period = 20_ms);
@@ -46,30 +46,30 @@
    *
    * Sets the proportional, integral, and differential coefficients.
    *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
+   * @param Ki The integral coefficient. Must be >= 0.
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetPID(double Kp, double Ki, double Kd);
 
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
-   * @param Kp proportional coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
    */
   void SetP(double Kp);
 
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
-   * @param Ki integral coefficient
+   * @param Ki The integral coefficient. Must be >= 0.
    */
   void SetI(double Ki);
 
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
-   * @param Kd differential coefficient
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetD(double Kd);
 
@@ -81,7 +81,8 @@
    * non-negative. Passing a value of zero will effectively disable integral
    * gain. Passing a value of infinity disables IZone functionality.
    *
-   * @param iZone Maximum magnitude of error to allow integral control.
+   * @param iZone Maximum magnitude of error to allow integral control. Must be
+   *   >= 0.
    */
   void SetIZone(double iZone);
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index 8f211c6..72278af 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -49,12 +49,12 @@
    * Kd. Users should call reset() when they first start running the controller
    * to avoid unwanted behavior.
    *
-   * @param Kp          The proportional coefficient.
-   * @param Ki          The integral coefficient.
-   * @param Kd          The derivative coefficient.
+   * @param Kp          The proportional coefficient. Must be >= 0.
+   * @param Ki          The integral coefficient. Must be >= 0.
+   * @param Kd          The derivative coefficient. Must be >= 0.
    * @param constraints Velocity and acceleration constraints for goal.
    * @param period      The period between controller updates in seconds. The
-   *                    default is 20 milliseconds.
+   *                    default is 20 milliseconds. Must be positive.
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
                         Constraints constraints, units::second_t period = 20_ms)
@@ -79,9 +79,9 @@
    *
    * Sets the proportional, integral, and differential coefficients.
    *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
+   * @param Ki The integral coefficient. Must be >= 0.
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetPID(double Kp, double Ki, double Kd) {
     m_controller.SetPID(Kp, Ki, Kd);
@@ -90,21 +90,21 @@
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
-   * @param Kp proportional coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
    */
   void SetP(double Kp) { m_controller.SetP(Kp); }
 
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
-   * @param Ki integral coefficient
+   * @param Ki The integral coefficient. Must be >= 0.
    */
   void SetI(double Ki) { m_controller.SetI(Ki); }
 
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
-   * @param Kd differential coefficient
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetD(double Kd) { m_controller.SetD(Kd); }
 
@@ -116,7 +116,8 @@
    * non-negative. Passing a value of zero will effectively disable integral
    * gain. Passing a value of infinity disables IZone functionality.
    *
-   * @param iZone Maximum magnitude of error to allow integral control.
+   * @param iZone Maximum magnitude of error to allow integral control. Must be
+   *   >= 0.
    */
   void SetIZone(double iZone) { m_controller.SetIZone(iZone); }
 
@@ -322,7 +323,7 @@
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    m_setpoint = m_profile.Calculate(GetPeriod(), m_goal, m_setpoint);
+    m_setpoint = m_profile.Calculate(GetPeriod(), m_setpoint, m_goal);
     return m_controller.Calculate(measurement.value(),
                                   m_setpoint.position.value());
   }
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
index 86b40ea..cc31fc1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -11,6 +11,7 @@
 #include "frc/system/plant/LinearSystemId.h"
 #include "units/time.h"
 #include "units/voltage.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -28,8 +29,6 @@
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
-  constexpr SimpleMotorFeedforward() = default;
-
   /**
    * Creates a new SimpleMotorFeedforward with the specified gains.
    *
@@ -40,7 +39,20 @@
   constexpr SimpleMotorFeedforward(
       units::volt_t kS, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kV(kV), kA(kA) {}
+      : kS(kS), kV(kV), kA(kA) {
+    if (kV.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kV must be a non-negative number, got {}!", kV.value());
+      kV = units::unit_t<kv_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
+    }
+    if (kA.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kA must be a non-negative number, got {}!", kA.value());
+      kA = units::unit_t<ka_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+  }
 
   /**
    * Calculates the feedforward from the gains and setpoints.
@@ -148,8 +160,13 @@
     return MaxAchievableAcceleration(-maxVoltage, velocity);
   }
 
-  units::volt_t kS{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
+  /** The static gain. */
+  const units::volt_t kS;
+
+  /** The velocity gain. */
+  const units::unit_t<kv_unit> kV;
+
+  /** The acceleration gain. */
+  const units::unit_t<ka_unit> kA;
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h
new file mode 100644
index 0000000..bc893aa
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/ArmFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::ArmFeedforward> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::ArmFeedforward Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::ArmFeedforward& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h
new file mode 100644
index 0000000..486fd17
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h
@@ -0,0 +1,19 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveWheelVoltages> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::DifferentialDriveWheelVoltages Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::DifferentialDriveWheelVoltages& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h
new file mode 100644
index 0000000..377f62a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/ElevatorFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::ElevatorFeedforward> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::ElevatorFeedforward Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::ElevatorFeedforward& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h
new file mode 100644
index 0000000..cfc585c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h
@@ -0,0 +1,26 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/ArmFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::ArmFeedforward> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:ArmFeedforward";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double ks;double kg;double kv;double ka";
+  }
+
+  static frc::ArmFeedforward Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::ArmFeedforward& value);
+};
+
+static_assert(wpi::StructSerializable<frc::ArmFeedforward>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h
new file mode 100644
index 0000000..cb5311f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h
@@ -0,0 +1,28 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelVoltages> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:DifferentialDriveWheelVoltages";
+  }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() {
+    return "double left;double right";
+  }
+
+  static frc::DifferentialDriveWheelVoltages Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveWheelVoltages& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelVoltages>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h
new file mode 100644
index 0000000..fafb7d7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h
@@ -0,0 +1,27 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/ElevatorFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::ElevatorFeedforward> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:ElevatorFeedforward";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double ks;double kg;double kv;double ka";
+  }
+
+  static frc::ElevatorFeedforward Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::ElevatorFeedforward& value);
+};
+
+static_assert(wpi::StructSerializable<frc::ElevatorFeedforward>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 026cc67..de0de37 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -16,7 +16,7 @@
  * Subtracts a and b while normalizing the resulting value in the selected row
  * as if it were an angle.
  *
- * @tparam States The number of states.
+ * @tparam States Number of states.
  * @param a A vector to subtract from.
  * @param b A vector to subtract with.
  * @param angleStateIdx The row containing angles to be normalized.
@@ -34,7 +34,7 @@
  * Returns a function that subtracts two vectors while normalizing the resulting
  * value in the selected row as if it were an angle.
  *
- * @tparam States The number of states.
+ * @tparam States Number of states.
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
@@ -49,7 +49,7 @@
  * Adds a and b while normalizing the resulting value in the selected row as an
  * angle.
  *
- * @tparam States The number of states.
+ * @tparam States Number of states.
  * @param a A vector to add with.
  * @param b A vector to add with.
  * @param angleStateIdx The row containing angles to be normalized.
@@ -67,7 +67,7 @@
  * Returns a function that adds two vectors while normalizing the resulting
  * value in the selected row as an angle.
  *
- * @tparam States The number of states.
+ * @tparam States Number of states.
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
@@ -82,7 +82,7 @@
  *
  * @tparam CovDim Dimension of covariance of sigma points after passing through
  *                the transform.
- * @tparam States The number of states.
+ * @tparam States Number of states.
  * @param sigmas Sigma points.
  * @param Wm Weights for the mean.
  * @param angleStatesIdx The row containing the angles.
@@ -113,7 +113,7 @@
  *
  * @tparam CovDim Dimension of covariance of sigma points after passing through
  *                the transform.
- * @tparam States The number of states.
+ * @tparam States Number of states.
  * @param angleStateIdx The row containing the angles.
  */
 template <int CovDim, int States>
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index 32ed558..68eb75b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -33,9 +33,9 @@
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
  * "Stochastic control theory".
  *
- * @tparam States The number of states.
- * @tparam Inputs The number of inputs.
- * @tparam Outputs The number of outputs.
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
  */
 template <int States, int Inputs, int Outputs>
 class ExtendedKalmanFilter {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index f143493..56dce0c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -4,7 +4,6 @@
 
 #pragma once
 
-#include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
 #include "frc/EigenCore.h"
@@ -29,9 +28,9 @@
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
  * "Stochastic control theory".
  *
- * @tparam States The number of states.
- * @tparam Inputs The number of inputs.
- * @tparam Outputs The number of outputs.
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
  */
 template <int States, int Inputs, int Outputs>
 class KalmanFilter {
@@ -43,8 +42,10 @@
   using StateArray = wpi::array<double, States>;
   using OutputArray = wpi::array<double, Outputs>;
 
+  using StateMatrix = Matrixd<States, States>;
+
   /**
-   * Constructs a state-space observer with the given plant.
+   * Constructs a Kalman filter with the given plant.
    *
    * See
    * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
@@ -60,21 +61,25 @@
                const StateArray& stateStdDevs,
                const OutputArray& measurementStdDevs, units::second_t dt);
 
-  KalmanFilter(KalmanFilter&&) = default;
-  KalmanFilter& operator=(KalmanFilter&&) = default;
-
   /**
-   * Returns the steady-state Kalman gain matrix K.
+   * Returns the error covariance matrix P.
    */
-  const Matrixd<States, Outputs>& K() const { return m_K; }
+  const StateMatrix& P() const { return m_P; }
 
   /**
-   * Returns an element of the steady-state Kalman gain matrix K.
+   * Returns an element of the error covariance matrix P.
    *
-   * @param i Row of K.
-   * @param j Column of K.
+   * @param i Row of P.
+   * @param j Column of P.
    */
-  double K(int i, int j) const { return m_K(i, j); }
+  double P(int i, int j) const { return m_P(i, j); }
+
+  /**
+   * Set the current error covariance matrix P.
+   *
+   * @param P The error covariance matrix P.
+   */
+  void SetP(const StateMatrix& P) { m_P = P; }
 
   /**
    * Returns the state estimate x-hat.
@@ -106,7 +111,10 @@
   /**
    * Resets the observer.
    */
-  void Reset() { m_xHat.setZero(); }
+  void Reset() {
+    m_xHat.setZero();
+    m_P = m_initP;
+  }
 
   /**
    * Project the model into the future with a new control input u.
@@ -119,23 +127,34 @@
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
-   * @param u Same control input used in the last predict step.
+   * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const InputVector& u, const OutputVector& y);
+  void Correct(const InputVector& u, const OutputVector& y) {
+    Correct(u, y, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R);
 
  private:
   LinearSystem<States, Inputs, Outputs>* m_plant;
-
-  /**
-   * The steady-state Kalman gain matrix.
-   */
-  Matrixd<States, Outputs> m_K;
-
-  /**
-   * The state estimate.
-   */
   StateVector m_xHat;
+  StateMatrix m_P;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
+  units::second_t m_dt;
+
+  StateMatrix m_initP;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
index 7506c0d..cfb8bd9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
@@ -4,8 +4,6 @@
 
 #pragma once
 
-#include <frc/fmt/Eigen.h>
-
 #include <cmath>
 #include <stdexcept>
 #include <string>
@@ -15,6 +13,7 @@
 #include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/KalmanFilter.h"
+#include "frc/fmt/Eigen.h"
 #include "frc/system/Discretization.h"
 #include "wpimath/MathShared.h"
 
@@ -27,14 +26,16 @@
     units::second_t dt) {
   m_plant = &plant;
 
-  auto contQ = MakeCovMatrix(stateStdDevs);
-  auto contR = MakeCovMatrix(measurementStdDevs);
+  m_contQ = MakeCovMatrix(stateStdDevs);
+  m_contR = MakeCovMatrix(measurementStdDevs);
+  m_dt = dt;
 
+  // Find discrete A and Q
   Matrixd<States, States> discA;
   Matrixd<States, States> discQ;
-  DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(plant.A(), m_contQ, dt, &discA, &discQ);
 
-  auto discR = DiscretizeR<Outputs>(contR, dt);
+  Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
 
   const auto& C = plant.C();
 
@@ -48,11 +49,38 @@
     throw std::invalid_argument(msg);
   }
 
-  Matrixd<States, States> P =
+  m_initP =
       DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
 
-  // S = CPCᵀ + R
-  Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
+  Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
+                                                    units::second_t dt) {
+  // Find discrete A and Q
+  StateMatrix discA;
+  StateMatrix discQ;
+  DiscretizeAQ<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
+
+  m_xHat = m_plant->CalculateX(m_xHat, u, dt);
+
+  // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+  m_P = discA * m_P * discA.transpose() + discQ;
+
+  m_dt = dt;
+}
+
+template <int States, int Inputs, int Outputs>
+void KalmanFilter<States, Inputs, Outputs>::Correct(
+    const InputVector& u, const OutputVector& y,
+    const Matrixd<Outputs, Outputs>& R) {
+  const auto& C = m_plant->C();
+  const auto& D = m_plant->D();
+
+  const Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(R, m_dt);
+
+  Matrixd<Outputs, Outputs> S = C * m_P * C.transpose() + discR;
 
   // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
   // efficiently.
@@ -66,22 +94,17 @@
   //
   // Kᵀ = Sᵀ.solve(CPᵀ)
   // K = (Sᵀ.solve(CPᵀ))ᵀ
-  m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
+  Matrixd<States, Outputs> K =
+      S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
 
-  Reset();
-}
-
-template <int States, int Inputs, int Outputs>
-void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
-                                                    units::second_t dt) {
-  m_xHat = m_plant->CalculateX(m_xHat, u, dt);
-}
-
-template <int States, int Inputs, int Outputs>
-void KalmanFilter<States, Inputs, Outputs>::Correct(const InputVector& u,
-                                                    const OutputVector& y) {
   // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
-  m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
+  m_xHat += K * (y - (C * m_xHat + D * u));
+
+  // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+  // Use Joseph form for numerical stability
+  m_P = (StateMatrix::Identity() - K * C) * m_P *
+            (StateMatrix::Identity() - K * C).transpose() +
+        K * discR * K.transpose();
 }
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index d6e4127..b93d614 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -16,13 +16,28 @@
 
 namespace frc {
 
+/**
+ * This class incorporates time-delayed measurements into a Kalman filter's
+ * state estimate.
+ *
+ * @tparam States The number of states.
+ * @tparam Inputs The number of inputs.
+ * @tparam Outputs The number of outputs.
+ */
 template <int States, int Inputs, int Outputs, typename KalmanFilterType>
 class KalmanFilterLatencyCompensator {
  public:
+  /**
+   * This class contains all the information about our observer at a given time.
+   */
   struct ObserverSnapshot {
+    /// The state estimate.
     Vectord<States> xHat;
+    /// The square root error covariance.
     Matrixd<States, States> squareRootErrorCovariances;
+    /// The inputs.
     Vectord<Inputs> inputs;
+    /// The local measurements.
     Vectord<Outputs> localMeasurements;
 
     ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
index b2e0e45..6395d8d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -21,8 +21,8 @@
  * [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
  *     Inference in Dynamic State-Space Models" (Doctoral dissertation)
  *
- * @tparam States The dimensionality of the state. 2*States+1 weights will be
- *                generated.
+ * @tparam States The dimensionality of the state. 2 * States + 1 weights will
+ *                be generated.
  */
 template <int States>
 class MerweScaledSigmaPoints {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index eb437a4..6e38d8c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -30,6 +30,9 @@
  *
  * AddVisionMeasurement() can be called as infrequently as you want; if you
  * never call it, then this class will behave like regular encoder odometry.
+ *
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelPositions Wheel positions type.
  */
 template <typename WheelSpeeds, WheelPositions WheelPositions>
 class WPILIB_DLLEXPORT PoseEstimator {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
new file mode 100644
index 0000000..64b1782
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
@@ -0,0 +1,153 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * This class assumes predict() and correct() are called in pairs, so the Kalman
+ * gain converges to a steady-state value. If they aren't, use KalmanFilter
+ * instead.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
+ */
+template <int States, int Inputs, int Outputs>
+class SteadyStateKalmanFilter {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  /**
+   * Constructs a staeady-state Kalman filter with the given plant.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
+   * @param plant              The plant used for the prediction step.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   * @throws std::invalid_argument If the system is unobservable.
+   */
+  SteadyStateKalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
+                          const StateArray& stateStdDevs,
+                          const OutputArray& measurementStdDevs,
+                          units::second_t dt);
+
+  SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;
+  SteadyStateKalmanFilter& operator=(SteadyStateKalmanFilter&&) = default;
+
+  /**
+   * Returns the steady-state Kalman gain matrix K.
+   */
+  const Matrixd<States, Outputs>& K() const { return m_K; }
+
+  /**
+   * Returns an element of the steady-state Kalman gain matrix K.
+   *
+   * @param i Row of K.
+   * @param j Column of K.
+   */
+  double K(int i, int j) const { return m_K(i, j); }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const StateVector& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() { m_xHat.setZero(); }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const InputVector& u, units::second_t dt);
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the last predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const InputVector& u, const OutputVector& y);
+
+ private:
+  LinearSystem<States, Inputs, Outputs>* m_plant;
+
+  /**
+   * The steady-state Kalman gain matrix.
+   */
+  Matrixd<States, Outputs> m_K;
+
+  /**
+   * The state estimate.
+   */
+  StateVector m_xHat;
+};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SteadyStateKalmanFilter<1, 1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SteadyStateKalmanFilter<2, 1, 1>;
+
+}  // namespace frc
+
+#include "SteadyStateKalmanFilter.inc"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc
new file mode 100644
index 0000000..1f2f2a2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc
@@ -0,0 +1,89 @@
+// 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 <cmath>
+#include <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/SteadyStateKalmanFilter.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/system/Discretization.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+SteadyStateKalmanFilter<States, Inputs, Outputs>::SteadyStateKalmanFilter(
+    LinearSystem<States, Inputs, Outputs>& plant,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    units::second_t dt) {
+  m_plant = &plant;
+
+  auto contQ = MakeCovMatrix(stateStdDevs);
+  auto contR = MakeCovMatrix(measurementStdDevs);
+
+  Matrixd<States, States> discA;
+  Matrixd<States, States> discQ;
+  DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
+
+  auto discR = DiscretizeR<Outputs>(contR, dt);
+
+  const auto& C = plant.C();
+
+  if (!IsDetectable<States, Outputs>(discA, C)) {
+    std::string msg = fmt::format(
+        "The system passed to the Kalman filter is "
+        "unobservable!\n\nA =\n{}\nC =\n{}\n",
+        discA, C);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::invalid_argument(msg);
+  }
+
+  Matrixd<States, States> P =
+      DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
+
+  // S = CPCᵀ + R
+  Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
+
+  // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+  // efficiently.
+  //
+  // K = PCᵀS⁻¹
+  // KS = PCᵀ
+  // (KS)ᵀ = (PCᵀ)ᵀ
+  // SᵀKᵀ = CPᵀ
+  //
+  // The solution of Ax = b can be found via x = A.solve(b).
+  //
+  // Kᵀ = Sᵀ.solve(CPᵀ)
+  // K = (Sᵀ.solve(CPᵀ))ᵀ
+  m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
+
+  Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+void SteadyStateKalmanFilter<States, Inputs, Outputs>::Predict(
+    const InputVector& u, units::second_t dt) {
+  m_xHat = m_plant->CalculateX(m_xHat, u, dt);
+}
+
+template <int States, int Inputs, int Outputs>
+void SteadyStateKalmanFilter<States, Inputs, Outputs>::Correct(
+    const InputVector& u, const OutputVector& y) {
+  const auto& C = m_plant->C();
+  const auto& D = m_plant->D();
+
+  // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+  m_xHat += m_K * (y - (C * m_xHat + D * u));
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 9526f0c..df68e15 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -39,9 +39,9 @@
  * (SR-UKF). For more information about the SR-UKF, see
  * https://www.researchgate.net/publication/3908304.
  *
- * @tparam States The number of states.
- * @tparam Inputs The number of inputs.
- * @tparam Outputs The number of outputs.
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
  */
 template <int States, int Inputs, int Outputs>
 class UnscentedKalmanFilter {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/Debouncer.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/Debouncer.h
index a929f37..9ab3e0e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/Debouncer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/Debouncer.h
@@ -17,7 +17,17 @@
  */
 class WPILIB_DLLEXPORT Debouncer {
  public:
-  enum DebounceType { kRising, kFalling, kBoth };
+  /**
+   * Type of debouncing to perform.
+   */
+  enum DebounceType {
+    /// Rising edge.
+    kRising,
+    /// Falling edge.
+    kFalling,
+    /// Both rising and falling edges.
+    kBoth
+  };
 
   /**
    * Creates a new Debouncer.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index fc558df..051a6dd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -272,6 +272,75 @@
   }
 
   /**
+   * Resets the filter state, initializing internal buffers to the provided
+   * values.
+   *
+   * These are the expected lengths of the buffers, depending on what type of
+   * linear filter used:
+   *
+   * <table>
+   *   <tr>
+   *     <th>Type</th>
+   *     <th>Input Buffer Size</th>
+   *     <th>Output Buffer Size</th>
+   *   </tr>
+   *   <tr>
+   *     <td>Unspecified</td>
+   *     <td>size of {@code ffGains}</td>
+   *     <td>size of {@code fbGains}</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Single Pole IIR</td>
+   *     <td>1</td>
+   *     <td>1</td>
+   *   </tr>
+   *   <tr>
+   *     <td>High-Pass</td>
+   *     <td>2</td>
+   *     <td>1</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Moving Average</td>
+   *     <td>{@code taps}</td>
+   *     <td>0</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Finite Difference</td>
+   *     <td>size of {@code stencil}</td>
+   *     <td>0</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Backward Finite Difference</td>
+   *     <td>{@code Samples}</td>
+   *     <td>0</td>
+   *   </tr>
+   * </table>
+   *
+   * @param inputBuffer Values to initialize input buffer.
+   * @param outputBuffer Values to initialize output buffer.
+   * @throws std::runtime_error if size of inputBuffer or outputBuffer does not
+   *   match the size of ffGains and fbGains provided in the constructor.
+   */
+  void Reset(std::span<const double> inputBuffer,
+             std::span<const double> outputBuffer) {
+    // Clear buffers
+    Reset();
+
+    if (inputBuffer.size() != m_inputGains.size() ||
+        outputBuffer.size() != m_outputGains.size()) {
+      throw std::runtime_error(
+          "Incorrect length of inputBuffer or outputBuffer");
+    }
+
+    for (size_t i = 0; i < inputBuffer.size(); ++i) {
+      m_inputs.push_front(inputBuffer[i]);
+    }
+    for (size_t i = 0; i < outputBuffer.size(); ++i) {
+      m_outputs.push_front(outputBuffer[i]);
+    }
+  }
+
+  /**
    * Calculates the next value of the filter.
    *
    * @param input Current input value.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
index 17d0d21..9f7c674 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
@@ -47,7 +47,7 @@
         m_negativeRateLimit{negativeRateLimit},
         m_prevVal{initialValue},
         m_prevTime{
-            units::microsecond_t(wpi::math::MathSharedStore::GetTimestamp())} {}
+            units::microsecond_t{wpi::math::MathSharedStore::GetTimestamp()}} {}
 
   /**
    * Creates a new SlewRateLimiter with the given positive rate limit and
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index 970f792..65a146c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -9,8 +9,6 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Transform2d.h"
@@ -215,38 +213,6 @@
 
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Pose2d> {
-  static constexpr std::string_view kTypeString = "struct:Pose2d";
-  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
-                                  wpi::Struct<frc::Rotation2d>::kSize;
-  static constexpr std::string_view kSchema =
-      "Translation2d translation;Rotation2d rotation";
-  static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data) {
-    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
-            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
-  }
-  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value) {
-    wpi::PackStruct<0>(data, value.Translation());
-    wpi::PackStruct<kRotationOff>(data, value.Rotation());
-  }
-  static void ForEachNested(
-      std::invocable<std::string_view, std::string_view> auto fn) {
-    wpi::ForEachStructSchema<frc::Translation2d>(fn);
-    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
-  }
-
- private:
-  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
-};
-
-static_assert(wpi::HasNestedStruct<frc::Pose2d>);
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Pose2d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value);
-};
-
+#include "frc/geometry/proto/Pose2dProto.h"
+#include "frc/geometry/struct/Pose2dStruct.h"
 #include "frc/geometry/Pose2d.inc"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose3d.h
index b5a0e03..526dced 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose3d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose3d.h
@@ -6,8 +6,6 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation3d.h"
@@ -217,36 +215,5 @@
 
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Pose3d> {
-  static constexpr std::string_view kTypeString = "struct:Pose3d";
-  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
-                                  wpi::Struct<frc::Rotation3d>::kSize;
-  static constexpr std::string_view kSchema =
-      "Translation3d translation;Rotation3d rotation";
-  static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data) {
-    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
-            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
-  }
-  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value) {
-    wpi::PackStruct<0>(data, value.Translation());
-    wpi::PackStruct<kRotationOff>(data, value.Rotation());
-  }
-  static void ForEachNested(
-      std::invocable<std::string_view, std::string_view> auto fn) {
-    wpi::ForEachStructSchema<frc::Translation3d>(fn);
-    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
-  }
-
- private:
-  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
-};
-
-static_assert(wpi::HasNestedStruct<frc::Pose3d>);
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Pose3d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value);
-};
+#include "frc/geometry/proto/Pose3dProto.h"
+#include "frc/geometry/struct/Pose3dStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Quaternion.h
index 63a3af2..4fe694c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Quaternion.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Quaternion.h
@@ -7,11 +7,12 @@
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 namespace frc {
 
+/**
+ * Represents a quaternion.
+ */
 class WPILIB_DLLEXPORT Quaternion {
  public:
   /**
@@ -190,31 +191,5 @@
 
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Quaternion> {
-  static constexpr std::string_view kTypeString = "struct:Quaternion";
-  static constexpr size_t kSize = 32;
-  static constexpr std::string_view kSchema =
-      "double w;double x;double y;double z";
-  static frc::Quaternion Unpack(std::span<const uint8_t, 32> data) {
-    return {wpi::UnpackStruct<double, 0>(data),
-            wpi::UnpackStruct<double, 8>(data),
-            wpi::UnpackStruct<double, 16>(data),
-            wpi::UnpackStruct<double, 24>(data)};
-  }
-  static void Pack(std::span<uint8_t, 32> data, const frc::Quaternion& value) {
-    wpi::PackStruct<0>(data, value.W());
-    wpi::PackStruct<8>(data, value.X());
-    wpi::PackStruct<16>(data, value.Y());
-    wpi::PackStruct<24>(data, value.Z());
-  }
-};
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
-  static constexpr std::string_view kTypeString = "proto:Quaternion";
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Quaternion Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg,
-                   const frc::Quaternion& value);
-};
+#include "frc/geometry/proto/QuaternionProto.h"
+#include "frc/geometry/struct/QuaternionStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index 96041a4..8f89556 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -6,8 +6,6 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 
@@ -178,25 +176,6 @@
 
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Rotation2d> {
-  static constexpr std::string_view kTypeString = "struct:Rotation2d";
-  static constexpr size_t kSize = 8;
-  static constexpr std::string_view kSchema = "double value";
-  static frc::Rotation2d Unpack(std::span<const uint8_t, 8> data) {
-    return units::radian_t{wpi::UnpackStruct<double>(data)};
-  }
-  static void Pack(std::span<uint8_t, 8> data, const frc::Rotation2d& value) {
-    wpi::PackStruct(data, value.Radians().value());
-  }
-};
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Rotation2d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg,
-                   const frc::Rotation2d& value);
-};
-
+#include "frc/geometry/proto/Rotation2dProto.h"
+#include "frc/geometry/struct/Rotation2dStruct.h"
 #include "frc/geometry/Rotation2d.inc"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
index dbe9e35..0d9e97d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
@@ -4,27 +4,23 @@
 
 #pragma once
 
-#include <type_traits>
+#include <gcem.hpp>
 
 #include "frc/geometry/Rotation2d.h"
-#include "gcem.hpp"
 #include "units/angle.h"
 
 namespace frc {
 
 constexpr Rotation2d::Rotation2d(units::radian_t value)
     : m_value(value),
-      m_cos(std::is_constant_evaluated() ? gcem::cos(value.to<double>())
-                                         : std::cos(value.to<double>())),
-      m_sin(std::is_constant_evaluated() ? gcem::sin(value.to<double>())
-                                         : std::sin(value.to<double>())) {}
+      m_cos(gcem::cos(value.to<double>())),
+      m_sin(gcem::sin(value.to<double>())) {}
 
 constexpr Rotation2d::Rotation2d(units::degree_t value)
     : Rotation2d(units::radian_t{value}) {}
 
 constexpr Rotation2d::Rotation2d(double x, double y) {
-  double magnitude =
-      std::is_constant_evaluated() ? gcem::hypot(x, y) : std::hypot(x, y);
+  double magnitude = gcem::hypot(x, y);
   if (magnitude > 1e-6) {
     m_sin = y / magnitude;
     m_cos = x / magnitude;
@@ -32,9 +28,7 @@
     m_sin = 0.0;
     m_cos = 1.0;
   }
-  m_value =
-      units::radian_t{std::is_constant_evaluated() ? gcem::atan2(m_sin, m_cos)
-                                                   : std::atan2(m_sin, m_cos)};
+  m_value = units::radian_t{gcem::atan2(m_sin, m_cos)};
 }
 
 constexpr Rotation2d Rotation2d::operator-() const {
@@ -58,9 +52,7 @@
 }
 
 constexpr bool Rotation2d::operator==(const Rotation2d& other) const {
-  return (std::is_constant_evaluated()
-              ? gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin())
-              : std::hypot(Cos() - other.Cos(), Sin() - other.Sin())) < 1E-9;
+  return gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin()) < 1E-9;
 }
 
 constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
index 6d04715..98abe00 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
@@ -7,8 +7,6 @@
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Quaternion.h"
 #include "frc/geometry/Rotation2d.h"
@@ -197,30 +195,5 @@
 
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Rotation3d> {
-  static constexpr std::string_view kTypeString = "struct:Rotation3d";
-  static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
-  static constexpr std::string_view kSchema = "Quaternion q";
-  static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data) {
-    return frc::Rotation3d{wpi::UnpackStruct<frc::Quaternion, 0>(data)};
-  }
-  static void Pack(std::span<uint8_t, kSize> data,
-                   const frc::Rotation3d& value) {
-    wpi::PackStruct<0>(data, value.GetQuaternion());
-  }
-  static void ForEachNested(
-      std::invocable<std::string_view, std::string_view> auto fn) {
-    wpi::ForEachStructSchema<frc::Quaternion>(fn);
-  }
-};
-
-static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Rotation3d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg,
-                   const frc::Rotation3d& value);
-};
+#include "frc/geometry/proto/Rotation3dProto.h"
+#include "frc/geometry/struct/Rotation3dStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index 593dce0..dccc0e1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -5,8 +5,6 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Translation2d.h"
 
@@ -126,40 +124,6 @@
 };
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Transform2d> {
-  static constexpr std::string_view kTypeString = "struct:Transform2d";
-  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
-                                  wpi::Struct<frc::Rotation2d>::kSize;
-  static constexpr std::string_view kSchema =
-      "Translation2d translation;Rotation2d rotation";
-  static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data) {
-    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
-            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
-  }
-  static void Pack(std::span<uint8_t, kSize> data,
-                   const frc::Transform2d& value) {
-    wpi::PackStruct<0>(data, value.Translation());
-    wpi::PackStruct<kRotationOff>(data, value.Rotation());
-  }
-  static void ForEachNested(
-      std::invocable<std::string_view, std::string_view> auto fn) {
-    wpi::ForEachStructSchema<frc::Translation2d>(fn);
-    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
-  }
-
- private:
-  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
-};
-
-static_assert(wpi::HasNestedStruct<frc::Transform2d>);
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Transform2d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg,
-                   const frc::Transform2d& value);
-};
-
+#include "frc/geometry/proto/Transform2dProto.h"
+#include "frc/geometry/struct/Transform2dStruct.h"
 #include "frc/geometry/Transform2d.inc"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform3d.h
index d5af97d..94ecfe3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform3d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform3d.h
@@ -5,8 +5,6 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Translation3d.h"
 
@@ -132,38 +130,5 @@
 };
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Transform3d> {
-  static constexpr std::string_view kTypeString = "struct:Transform3d";
-  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
-                                  wpi::Struct<frc::Rotation3d>::kSize;
-  static constexpr std::string_view kSchema =
-      "Translation3d translation;Rotation3d rotation";
-  static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data) {
-    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
-            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
-  }
-  static void Pack(std::span<uint8_t, kSize> data,
-                   const frc::Transform3d& value) {
-    wpi::PackStruct<0>(data, value.Translation());
-    wpi::PackStruct<kRotationOff>(data, value.Rotation());
-  }
-  static void ForEachNested(
-      std::invocable<std::string_view, std::string_view> auto fn) {
-    wpi::ForEachStructSchema<frc::Translation3d>(fn);
-    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
-  }
-
- private:
-  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
-};
-
-static_assert(wpi::HasNestedStruct<frc::Transform3d>);
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Transform3d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg,
-                   const frc::Transform3d& value);
-};
+#include "frc/geometry/proto/Transform3dProto.h"
+#include "frc/geometry/struct/Transform3dStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index 1568586..a99492d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -9,8 +9,6 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation2d.h"
 #include "units/length.h"
@@ -200,28 +198,6 @@
 
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Translation2d> {
-  static constexpr std::string_view kTypeString = "struct:Translation2d";
-  static constexpr size_t kSize = 16;
-  static constexpr std::string_view kSchema = "double x;double y";
-  static frc::Translation2d Unpack(std::span<const uint8_t, 16> data) {
-    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
-            units::meter_t{wpi::UnpackStruct<double, 8>(data)}};
-  }
-  static void Pack(std::span<uint8_t, 16> data,
-                   const frc::Translation2d& value) {
-    wpi::PackStruct<0>(data, value.X().value());
-    wpi::PackStruct<8>(data, value.Y().value());
-  }
-};
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Translation2d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg,
-                   const frc::Translation2d& value);
-};
-
+#include "frc/geometry/proto/Translation2dProto.h"
+#include "frc/geometry/struct/Translation2dStruct.h"
 #include "frc/geometry/Translation2d.inc"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index 47ba1f6..b0b3359 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -6,8 +6,6 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation3d.h"
 #include "frc/geometry/Translation2d.h"
@@ -185,30 +183,6 @@
 
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Translation3d> {
-  static constexpr std::string_view kTypeString = "struct:Translation3d";
-  static constexpr size_t kSize = 24;
-  static constexpr std::string_view kSchema = "double x;double y;double z";
-  static frc::Translation3d Unpack(std::span<const uint8_t, 24> data) {
-    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
-            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
-            units::meter_t{wpi::UnpackStruct<double, 16>(data)}};
-  }
-  static void Pack(std::span<uint8_t, 24> data,
-                   const frc::Translation3d& value) {
-    wpi::PackStruct<0>(data, value.X().value());
-    wpi::PackStruct<8>(data, value.Y().value());
-    wpi::PackStruct<16>(data, value.Z().value());
-  }
-};
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Translation3d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg,
-                   const frc::Translation3d& value);
-};
-
+#include "frc/geometry/proto/Translation3dProto.h"
+#include "frc/geometry/struct/Translation3dStruct.h"
 #include "frc/geometry/Translation3d.inc"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index 257f1b6..d13cc4e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -5,8 +5,6 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 #include "units/length.h"
@@ -60,27 +58,5 @@
 };
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Twist2d> {
-  static constexpr std::string_view kTypeString = "struct:Twist2d";
-  static constexpr size_t kSize = 24;
-  static constexpr std::string_view kSchema =
-      "double dx;double dy;double dtheta";
-  static frc::Twist2d Unpack(std::span<const uint8_t, 24> data) {
-    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
-            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
-            units::radian_t{wpi::UnpackStruct<double, 16>(data)}};
-  }
-  static void Pack(std::span<uint8_t, 24> data, const frc::Twist2d& value) {
-    wpi::PackStruct<0>(data, value.dx.value());
-    wpi::PackStruct<8>(data, value.dy.value());
-    wpi::PackStruct<16>(data, value.dtheta.value());
-  }
-};
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Twist2d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value);
-};
+#include "frc/geometry/proto/Twist2dProto.h"
+#include "frc/geometry/struct/Twist2dStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist3d.h
index 4d902df..3262367 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist3d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist3d.h
@@ -5,8 +5,6 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
-#include <wpi/protobuf/Protobuf.h>
-#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation3d.h"
 #include "units/angle.h"
@@ -80,34 +78,5 @@
 };
 }  // namespace frc
 
-template <>
-struct wpi::Struct<frc::Twist3d> {
-  static constexpr std::string_view kTypeString = "struct:Twist3d";
-  static constexpr size_t kSize = 48;
-  static constexpr std::string_view kSchema =
-      "double dx;double dy;double dz;double rx;double ry;double rz";
-  static frc::Twist3d Unpack(std::span<const uint8_t, 48> data) {
-    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
-            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
-            units::meter_t{wpi::UnpackStruct<double, 16>(data)},
-            units::radian_t{wpi::UnpackStruct<double, 24>(data)},
-            units::radian_t{wpi::UnpackStruct<double, 32>(data)},
-            units::radian_t{wpi::UnpackStruct<double, 40>(data)}};
-  }
-  static void Pack(std::span<uint8_t, 48> data, const frc::Twist3d& value) {
-    wpi::PackStruct<0>(data, value.dx.value());
-    wpi::PackStruct<8>(data, value.dy.value());
-    wpi::PackStruct<16>(data, value.dz.value());
-    wpi::PackStruct<24>(data, value.rx.value());
-    wpi::PackStruct<32>(data, value.ry.value());
-    wpi::PackStruct<40>(data, value.rz.value());
-  }
-};
-
-template <>
-struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
-  static constexpr std::string_view kTypeString = "proto:Twist3d";
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
-  static frc::Twist3d Unpack(const google::protobuf::Message& msg);
-  static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value);
-};
+#include "frc/geometry/proto/Twist3dProto.h"
+#include "frc/geometry/struct/Twist3dStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h
new file mode 100644
index 0000000..9934b4a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h
@@ -0,0 +1,17 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Pose2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h
new file mode 100644
index 0000000..9941568
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h
@@ -0,0 +1,17 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Pose3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h
new file mode 100644
index 0000000..77f371d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Quaternion.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Quaternion Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Quaternion& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h
new file mode 100644
index 0000000..33c348e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation2d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h
new file mode 100644
index 0000000..f6f5662
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Rotation3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation3d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h
new file mode 100644
index 0000000..5ff19bd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Transform2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform2d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h
new file mode 100644
index 0000000..d27fb9a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Transform3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform3d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h
new file mode 100644
index 0000000..6516fc8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Translation2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation2d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h
new file mode 100644
index 0000000..2d317fc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Translation3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation3d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h
new file mode 100644
index 0000000..328798e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h
@@ -0,0 +1,17 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Twist2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h
new file mode 100644
index 0000000..24f4969
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h
@@ -0,0 +1,17 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Twist3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h
new file mode 100644
index 0000000..66e3295
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h
@@ -0,0 +1,33 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Pose2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose2d> {
+  static constexpr std::string_view GetTypeString() { return "struct:Pose2d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation2d>() +
+           wpi::GetStructSize<frc::Rotation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  static frc::Pose2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Pose2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Pose2d>);
+static_assert(wpi::HasNestedStruct<frc::Pose2d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h
new file mode 100644
index 0000000..f47ffec
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h
@@ -0,0 +1,33 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Pose3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose3d> {
+  static constexpr std::string_view GetTypeString() { return "struct:Pose3d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation3d>() +
+           wpi::GetStructSize<frc::Rotation3d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  static frc::Pose3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Pose3d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Pose3d>);
+static_assert(wpi::HasNestedStruct<frc::Pose3d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h
new file mode 100644
index 0000000..80420c0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h
@@ -0,0 +1,26 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Quaternion.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Quaternion> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Quaternion";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double w;double x;double y;double z";
+  }
+
+  static frc::Quaternion Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Quaternion& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Quaternion>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h
new file mode 100644
index 0000000..85b6ae5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h
@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation2d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Rotation2d";
+  }
+  static constexpr size_t GetSize() { return 8; }
+  static constexpr std::string_view GetSchema() { return "double value"; }
+
+  static frc::Rotation2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Rotation2d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Rotation2d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h
new file mode 100644
index 0000000..e94b75e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h
@@ -0,0 +1,31 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Rotation3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation3d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Rotation3d";
+  }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Quaternion>();
+  }
+  static constexpr std::string_view GetSchema() { return "Quaternion q"; }
+
+  static frc::Rotation3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Rotation3d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Quaternion>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Rotation3d>);
+static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h
new file mode 100644
index 0000000..28ef463
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h
@@ -0,0 +1,35 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Transform2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform2d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Transform2d";
+  }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation2d>() +
+           wpi::GetStructSize<frc::Rotation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  static frc::Transform2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Transform2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Transform2d>);
+static_assert(wpi::HasNestedStruct<frc::Transform2d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h
new file mode 100644
index 0000000..e4e88bc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h
@@ -0,0 +1,35 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Transform3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform3d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Transform3d";
+  }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation3d>() +
+           wpi::GetStructSize<frc::Rotation3d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  static frc::Transform3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Transform3d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Transform3d>);
+static_assert(wpi::HasNestedStruct<frc::Transform3d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h
new file mode 100644
index 0000000..df12760
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h
@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Translation2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation2d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Translation2d";
+  }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() { return "double x;double y"; }
+
+  static frc::Translation2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Translation2d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Translation2d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h
new file mode 100644
index 0000000..b61a875
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h
@@ -0,0 +1,26 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Translation3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation3d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Translation3d";
+  }
+  static constexpr size_t GetSize() { return 24; }
+  static constexpr std::string_view GetSchema() {
+    return "double x;double y;double z";
+  }
+
+  static frc::Translation3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Translation3d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Translation3d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h
new file mode 100644
index 0000000..86bb4ad
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h
@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Twist2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist2d> {
+  static constexpr std::string_view GetTypeString() { return "struct:Twist2d"; }
+  static constexpr size_t GetSize() { return 24; }
+  static constexpr std::string_view GetSchema() {
+    return "double dx;double dy;double dtheta";
+  }
+
+  static frc::Twist2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Twist2d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Twist2d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h
new file mode 100644
index 0000000..a346a1a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h
@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Twist3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist3d> {
+  static constexpr std::string_view GetTypeString() { return "struct:Twist3d"; }
+  static constexpr size_t GetSize() { return 48; }
+  static constexpr std::string_view GetSchema() {
+    return "double dx;double dy;double dz;double rx;double ry;double rz";
+  }
+
+  static frc::Twist3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Twist3d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Twist3d>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index 9dd8e62..94d2818 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -64,7 +64,7 @@
    * @param sample The sample object.
    */
   void AddSample(units::second_t time, T sample) {
-    // Add the new state into the vector.
+    // Add the new state into the vector
     if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
       m_pastSnapshots.emplace_back(time, sample);
     } else {
@@ -72,20 +72,17 @@
           m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
           [](auto t, const auto& pair) { return t < pair.first; });
 
-      // Don't access this before ensuring first_after isn't first.
-      auto last_not_greater_than = first_after - 1;
-
-      if (first_after == m_pastSnapshots.begin() ||
-          last_not_greater_than == m_pastSnapshots.begin() ||
-          last_not_greater_than->first < time) {
-        // Two cases handled together:
-        // 1. All entries come after the sample
-        // 2. Some entries come before the sample, but none are recorded with
-        // the same time
-        m_pastSnapshots.insert(first_after, std::pair(time, sample));
+      if (first_after == m_pastSnapshots.begin()) {
+        // All entries come after the sample
+        m_pastSnapshots.insert(first_after, std::pair{time, sample});
+      } else if (auto last_not_greater_than = first_after - 1;
+                 last_not_greater_than == m_pastSnapshots.begin() ||
+                 last_not_greater_than->first < time) {
+        // Some entries come before the sample, but none are recorded with the
+        // same time
+        m_pastSnapshots.insert(first_after, std::pair{time, sample});
       } else {
-        // Final case:
-        // 3. An entry exists with the same recorded time.
+        // An entry exists with the same recorded time
         last_not_greater_than->second = sample;
       }
     }
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index 93c9044..ac117b6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -251,3 +251,6 @@
   }
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/ChassisSpeedsProto.h"
+#include "frc/kinematics/struct/ChassisSpeedsStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 95f53fa..0f0cff9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -88,6 +88,10 @@
     return ToTwist2d(end.left - start.left, end.right - start.right);
   }
 
+  /// Differential drive trackwidth.
   units::meter_t trackWidth;
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/DifferentialDriveKinematicsProto.h"
+#include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
index 9d9e705..676b8ad 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
@@ -112,3 +112,6 @@
   }
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h"
+#include "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index fe6eb51..f5a53ca 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -138,6 +138,34 @@
    */
   Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
 
+  /**
+   * Returns the front-left wheel translation.
+   *
+   * @return The front-left wheel translation.
+   */
+  const Translation2d& GetFrontLeft() const { return m_frontLeftWheel; }
+
+  /**
+   * Returns the front-right wheel translation.
+   *
+   * @return The front-right wheel translation.
+   */
+  const Translation2d& GetFrontRight() const { return m_frontRightWheel; }
+
+  /**
+   * Returns the rear-left wheel translation.
+   *
+   * @return The rear-left wheel translation.
+   */
+  const Translation2d& GetRearLeft() const { return m_rearLeftWheel; }
+
+  /**
+   * Returns the rear-right wheel translation.
+   *
+   * @return The rear-right wheel translation.
+   */
+  const Translation2d& GetRearRight() const { return m_rearRightWheel; }
+
  private:
   mutable Matrixd<4, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;
@@ -165,3 +193,6 @@
 };
 
 }  // namespace frc
+
+#include "frc/kinematics/proto/MecanumDriveKinematicsProto.h"
+#include "frc/kinematics/struct/MecanumDriveKinematicsStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
index 76c8b9d..77d7919 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
@@ -60,3 +60,6 @@
   }
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/MecanumDriveWheelPositionsProto.h"
+#include "frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
index 80e8460..5df198c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
@@ -120,3 +120,6 @@
   }
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h"
+#include "frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/Odometry.h
index 55c074c..9e242e2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/Odometry.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/Odometry.h
@@ -20,6 +20,9 @@
  * Teams can use odometry during the autonomous period for complex tasks like
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
+ *
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelPositions Wheel positions type.
  */
 template <typename WheelSpeeds, WheelPositions WheelPositions>
 class WPILIB_DLLEXPORT Odometry {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index db85d99..b9bf340 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -166,7 +166,7 @@
   auto k = units::math::max(translationalK, rotationalK);
 
   auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
-                                units::scalar_t(1));
+                                units::scalar_t{1});
   for (auto& module : states) {
     module.speed = module.speed * scale;
   }
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
index 93f7465..576bfcf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
@@ -42,3 +42,6 @@
   }
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/SwerveModulePositionProto.h"
+#include "frc/kinematics/struct/SwerveModulePositionStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
index 2f95d9b..3060d35 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -47,3 +47,6 @@
                                     const Rotation2d& currentAngle);
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/SwerveModuleStateProto.h"
+#include "frc/kinematics/struct/SwerveModuleStateStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h
new file mode 100644
index 0000000..cefc83f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::ChassisSpeeds> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::ChassisSpeeds Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::ChassisSpeeds& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h
new file mode 100644
index 0000000..9108f33
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h
@@ -0,0 +1,19 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveKinematics> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::DifferentialDriveKinematics Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::DifferentialDriveKinematics& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h
new file mode 100644
index 0000000..f310892
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h
@@ -0,0 +1,19 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveWheelSpeeds> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::DifferentialDriveWheelSpeeds Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::DifferentialDriveWheelSpeeds& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h
new file mode 100644
index 0000000..16e8288
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h
@@ -0,0 +1,19 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::MecanumDriveKinematics> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::MecanumDriveKinematics Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::MecanumDriveKinematics& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h
new file mode 100644
index 0000000..a4b4bd6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h
@@ -0,0 +1,19 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::MecanumDriveWheelPositions> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::MecanumDriveWheelPositions Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::MecanumDriveWheelPositions& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h
new file mode 100644
index 0000000..4d099d3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h
@@ -0,0 +1,19 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::MecanumDriveWheelSpeeds> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::MecanumDriveWheelSpeeds Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::MecanumDriveWheelSpeeds& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h
new file mode 100644
index 0000000..3e864cb
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::SwerveModulePosition> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::SwerveModulePosition Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::SwerveModulePosition& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h
new file mode 100644
index 0000000..3e7b3ec
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/SwerveModuleState.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::SwerveModuleState> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::SwerveModuleState Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::SwerveModuleState& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h
new file mode 100644
index 0000000..8c65057
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h
@@ -0,0 +1,26 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::ChassisSpeeds> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:ChassisSpeeds";
+  }
+  static constexpr size_t GetSize() { return 24; }
+  static constexpr std::string_view GetSchema() {
+    return "double vx;double vy;double omega";
+  }
+
+  static frc::ChassisSpeeds Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::ChassisSpeeds& value);
+};
+
+static_assert(wpi::StructSerializable<frc::ChassisSpeeds>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h
new file mode 100644
index 0000000..0acbc76
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h
@@ -0,0 +1,25 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveKinematics> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:DifferentialDriveKinematics";
+  }
+  static constexpr size_t GetSize() { return 8; }
+  static constexpr std::string_view GetSchema() { return "double track_width"; }
+
+  static frc::DifferentialDriveKinematics Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveKinematics& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveKinematics>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h
new file mode 100644
index 0000000..12dc843
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h
@@ -0,0 +1,28 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelSpeeds> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:DifferentialDriveWheelSpeeds";
+  }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() {
+    return "double left;double right";
+  }
+
+  static frc::DifferentialDriveWheelSpeeds Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveWheelSpeeds& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelSpeeds>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h
new file mode 100644
index 0000000..a4fe3bd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h
@@ -0,0 +1,35 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveKinematics> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:MecanumDriveKinematics";
+  }
+  static constexpr size_t GetSize() {
+    return 4 * wpi::GetStructSize<frc::Translation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation2d front_left;Translation2d front_right;Translation2d "
+           "rear_left;Translation2d rear_right";
+  }
+
+  static frc::MecanumDriveKinematics Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::MecanumDriveKinematics& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::MecanumDriveKinematics>);
+static_assert(wpi::HasNestedStruct<frc::MecanumDriveKinematics>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h
new file mode 100644
index 0000000..87b5f38
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h
@@ -0,0 +1,28 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveWheelPositions> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:MecanumDriveWheelPositions";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double front_left;double front_right;double rear_left;double "
+           "rear_right";
+  }
+
+  static frc::MecanumDriveWheelPositions Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::MecanumDriveWheelPositions& value);
+};
+
+static_assert(wpi::StructSerializable<frc::MecanumDriveWheelPositions>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h
new file mode 100644
index 0000000..28f28e8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h
@@ -0,0 +1,28 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveWheelSpeeds> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:MecanumDriveWheelSpeeds";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double front_left;double front_right;double rear_left;"
+           "double rear_right";
+  }
+
+  static frc::MecanumDriveWheelSpeeds Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::MecanumDriveWheelSpeeds& value);
+};
+
+static_assert(wpi::StructSerializable<frc::MecanumDriveWheelSpeeds>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h
new file mode 100644
index 0000000..ee58a2c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h
@@ -0,0 +1,34 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::SwerveModulePosition> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:SwerveModulePosition";
+  }
+  static constexpr size_t GetSize() {
+    return 8 + wpi::GetStructSize<frc::Rotation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "double distance;Rotation2d angle";
+  }
+
+  static frc::SwerveModulePosition Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::SwerveModulePosition& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::SwerveModulePosition>);
+static_assert(wpi::HasNestedStruct<frc::SwerveModulePosition>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h
new file mode 100644
index 0000000..06885fe
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h
@@ -0,0 +1,34 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/SwerveModuleState.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::SwerveModuleState> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:SwerveModuleState";
+  }
+  static constexpr size_t GetSize() {
+    return 8 + wpi::GetStructSize<frc::Rotation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "double speed;Rotation2d angle";
+  }
+
+  static frc::SwerveModuleState Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::SwerveModuleState& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::SwerveModuleState>);
+static_assert(wpi::HasNestedStruct<frc::SwerveModuleState>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/optimization/SimulatedAnnealing.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/optimization/SimulatedAnnealing.h
new file mode 100644
index 0000000..0a16dd9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/optimization/SimulatedAnnealing.h
@@ -0,0 +1,101 @@
+// 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 <cmath>
+#include <functional>
+#include <limits>
+#include <random>
+
+namespace frc {
+
+/**
+ * An implementation of the Simulated Annealing stochastic nonlinear
+ * optimization method.
+ *
+ * Solving optimization problems involves tweaking decision variables to try to
+ * minimize some cost function. Simulated annealing is good for solving
+ * optimization problems with many local minima and a very large search space
+ * (it’s a heuristic solver rather than an exact solver like, say, SQP or
+ * interior-point method). Simulated annealing is a popular choice for solving
+ * the traveling salesman problem (see TravelingSalesman).
+ *
+ * @see <a
+ * href="https://en.wikipedia.org/wiki/Simulated_annealing">https://en.wikipedia.org/wiki/Simulated_annealing</a>
+ * @tparam State The type of the state to optimize.
+ */
+template <typename State>
+class SimulatedAnnealing {
+ public:
+  /**
+   * Constructor for Simulated Annealing that can be used for the same functions
+   * but with different initial states.
+   *
+   * @param initialTemperature The initial temperature. Higher temperatures make
+   *     it more likely a worse state will be accepted during iteration, helping
+   *     to avoid local minima. The temperature is decreased over time.
+   * @param neighbor Function that generates a random neighbor of the current
+   *     state.
+   * @param cost Function that returns the scalar cost of a state.
+   */
+  constexpr SimulatedAnnealing(double initialTemperature,
+                               std::function<State(const State&)> neighbor,
+                               std::function<double(const State&)> cost)
+      : m_initialTemperature{initialTemperature},
+        m_neighbor{neighbor},
+        m_cost{cost} {}
+
+  /**
+   * Runs the Simulated Annealing algorithm.
+   *
+   * @param initialGuess The initial state.
+   * @param iterations Number of iterations to run the solver.
+   * @return The optimized state.
+   */
+  State Solve(const State& initialGuess, int iterations) {
+    State minState = initialGuess;
+    double minCost = std::numeric_limits<double>::infinity();
+
+    std::random_device rd;
+    std::mt19937 gen{rd()};
+    std::uniform_real_distribution<> distr{0.0, 1.0};
+
+    State state = initialGuess;
+    double cost = m_cost(state);
+
+    for (int i = 0; i < iterations; ++i) {
+      double temperature = m_initialTemperature / i;
+
+      State proposedState = m_neighbor(state);
+      double proposedCost = m_cost(proposedState);
+      double deltaCost = proposedCost - cost;
+
+      double acceptanceProbability = std::exp(-deltaCost / temperature);
+
+      // If cost went down or random number exceeded acceptance probability,
+      // accept the proposed state
+      if (deltaCost < 0 || acceptanceProbability >= distr(gen)) {
+        state = proposedState;
+        cost = proposedCost;
+      }
+
+      // If proposed cost is less than minimum, the proposed state becomes the
+      // new minimum
+      if (proposedCost < minCost) {
+        minState = proposedState;
+        minCost = proposedCost;
+      }
+    }
+
+    return minState;
+  }
+
+ private:
+  double m_initialTemperature;
+  std::function<State(const State&)> m_neighbor;
+  std::function<double(const State&)> m_cost;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/path/TravelingSalesman.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/path/TravelingSalesman.h
new file mode 100644
index 0000000..d45f620
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/path/TravelingSalesman.h
@@ -0,0 +1,179 @@
+// 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 <cmath>
+#include <functional>
+#include <random>
+#include <utility>
+#include <vector>
+
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/optimization/SimulatedAnnealing.h"
+
+namespace frc {
+
+/**
+ * Given a list of poses, this class finds the shortest possible route that
+ * visits each pose exactly once and returns to the origin pose.
+ *
+ * @see <a
+ * href="https://en.wikipedia.org/wiki/Travelling_salesman_problem">https://en.wikipedia.org/wiki/Travelling_salesman_problem</a>
+ */
+class TravelingSalesman {
+ public:
+  /**
+   * Constructs a traveling salesman problem solver with a cost function defined
+   * as the 2D distance between poses.
+   */
+  constexpr TravelingSalesman() = default;
+
+  /**
+   * Constructs a traveling salesman problem solver with a user-provided cost
+   * function.
+   *
+   * @param cost Function that returns the cost between two poses. The sum of
+   *     the costs for every pair of poses is minimized.
+   */
+  explicit TravelingSalesman(std::function<double(Pose2d, Pose2d)> cost)
+      : m_cost{std::move(cost)} {}
+
+  /**
+   * Finds the path through every pose that minimizes the cost. The first pose
+   * in the returned array is the first pose that was passed in.
+   *
+   * This overload supports a statically-sized list of poses.
+   *
+   * @tparam Poses The length of the path and the number of poses.
+   * @param poses An array of Pose2ds the path must pass through.
+   * @param iterations The number of times the solver attempts to find a better
+   *     random neighbor.
+   * @return The optimized path as an array of Pose2ds.
+   */
+  template <size_t Poses>
+  wpi::array<Pose2d, Poses> Solve(const wpi::array<Pose2d, Poses>& poses,
+                                  int iterations) {
+    SimulatedAnnealing<Vectord<Poses>> solver{
+        1, &Neighbor<Poses>, [&](const Vectord<Poses>& state) {
+          // Total cost is sum of all costs between adjacent pairs in path
+          double sum = 0.0;
+          for (int i = 0; i < state.rows(); ++i) {
+            sum +=
+                m_cost(poses[static_cast<int>(state(i))],
+                       poses[static_cast<int>(state((i + 1) % poses.size()))]);
+          }
+          return sum;
+        }};
+
+    Eigen::Vector<double, Poses> initial;
+    for (int i = 0; i < initial.rows(); ++i) {
+      initial(i) = i;
+    }
+
+    auto indices = solver.Solve(initial, iterations);
+
+    wpi::array<Pose2d, Poses> solution{wpi::empty_array};
+    for (size_t i = 0; i < poses.size(); ++i) {
+      solution[i] = poses[static_cast<int>(indices[i])];
+    }
+
+    // Rotate solution list until solution[0] = poses[0]
+    std::rotate(solution.begin(),
+                std::find(solution.begin(), solution.end(), poses[0]),
+                solution.end());
+
+    return solution;
+  }
+
+  /**
+   * Finds the path through every pose that minimizes the cost. The first pose
+   * in the returned array is the first pose that was passed in.
+   *
+   * This overload supports a dynamically-sized list of poses for Python to use.
+   *
+   * @param poses An array of Pose2ds the path must pass through.
+   * @param iterations The number of times the solver attempts to find a better
+   *     random neighbor.
+   * @return The optimized path as an array of Pose2ds.
+   */
+  std::vector<Pose2d> Solve(std::span<const Pose2d> poses, int iterations) {
+    SimulatedAnnealing<Eigen::VectorXd> solver{
+        1.0, &Neighbor<Eigen::Dynamic>, [&](const Eigen::VectorXd& state) {
+          // Total cost is sum of all costs between adjacent pairs in path
+          double sum = 0.0;
+          for (int i = 0; i < state.rows(); ++i) {
+            sum +=
+                m_cost(poses[static_cast<int>(state(i))],
+                       poses[static_cast<int>(state((i + 1) % poses.size()))]);
+          }
+          return sum;
+        }};
+
+    Eigen::VectorXd initial{poses.size()};
+    for (int i = 0; i < initial.rows(); ++i) {
+      initial(i) = i;
+    }
+
+    auto indices = solver.Solve(initial, iterations);
+
+    std::vector<Pose2d> solution;
+    for (size_t i = 0; i < poses.size(); ++i) {
+      solution.emplace_back(poses[static_cast<int>(indices[i])]);
+    }
+
+    // Rotate solution list until solution[0] = poses[0]
+    std::rotate(solution.begin(),
+                std::find(solution.begin(), solution.end(), poses[0]),
+                solution.end());
+
+    return solution;
+  }
+
+ private:
+  // Default cost is distance between poses
+  std::function<double(const Pose2d&, const Pose2d&)> m_cost =
+      [](const Pose2d& a, const Pose2d& b) -> double {
+    return units::math::hypot(a.X() - b.X(), a.Y() - b.Y()).value();
+  };
+
+  /**
+   * A random neighbor is generated to try to replace the current one.
+   *
+   * @tparam Poses The length of the path and the number of poses.
+   * @param state A vector that is a list of indices that defines the path
+   *     through the path array.
+   * @return Generates a random neighbor of the current state by flipping a
+   *     random range in the path array.
+   */
+  template <int Poses>
+  static Eigen::Vector<double, Poses> Neighbor(
+      const Eigen::Vector<double, Poses>& state) {
+    Eigen::Vector<double, Poses> proposedState = state;
+
+    std::random_device rd;
+    std::mt19937 gen{rd()};
+    std::uniform_int_distribution<> distr{0,
+                                          static_cast<int>(state.rows()) - 1};
+
+    int rangeStart = distr(gen);
+    int rangeEnd = distr(gen);
+    if (rangeEnd < rangeStart) {
+      std::swap(rangeStart, rangeEnd);
+    }
+
+    for (int i = rangeStart; i <= (rangeStart + rangeEnd) / 2; ++i) {
+      double temp = proposedState(i, 0);
+      proposedState(i, 0) = state(rangeEnd - (i - rangeStart), 0);
+      proposedState(rangeEnd - (i - rangeStart), 0) = temp;
+    }
+
+    return proposedState;
+  }
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index 1d6aaeb..07cc13c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -35,16 +35,36 @@
                      wpi::array<double, 2> yInitialControlVector,
                      wpi::array<double, 2> yFinalControlVector);
 
- protected:
   /**
    * Returns the coefficients matrix.
    * @return The coefficients matrix.
    */
   Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
 
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  const ControlVector& GetInitialControlVector() const override {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  const ControlVector& GetFinalControlVector() const override {
+    return m_finalControlVector;
+  }
+
  private:
   Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
 
+  ControlVector m_initialControlVector;
+  ControlVector m_finalControlVector;
+
   /**
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
    * @return The hermite basis matrix for cubic hermite spline interpolation.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index ad03a23..37513cc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -35,16 +35,36 @@
                        wpi::array<double, 3> yInitialControlVector,
                        wpi::array<double, 3> yFinalControlVector);
 
- protected:
   /**
    * Returns the coefficients matrix.
    * @return The coefficients matrix.
    */
   Matrixd<6, 6> Coefficients() const override { return m_coefficients; }
 
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  const ControlVector& GetInitialControlVector() const override {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  const ControlVector& GetFinalControlVector() const override {
+    return m_finalControlVector;
+  }
+
  private:
   Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero();
 
+  ControlVector m_initialControlVector;
+  ControlVector m_finalControlVector;
+
   /**
    * Returns the hermite basis matrix for quintic hermite spline interpolation.
    * @return The hermite basis matrix for quintic hermite spline interpolation.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h
index b5ff024..257b167 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h
@@ -44,7 +44,10 @@
    * dimension.
    */
   struct ControlVector {
+    /// The x components of the control vector.
     wpi::array<double, (Degree + 1) / 2> x;
+
+    /// The y components of the control vector.
     wpi::array<double, (Degree + 1) / 2> y;
   };
 
@@ -94,7 +97,6 @@
         units::curvature_t{curvature}};
   }
 
- protected:
   /**
    * Returns the coefficients of the spline.
    *
@@ -103,6 +105,21 @@
   virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
 
   /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  virtual const ControlVector& GetInitialControlVector() const = 0;
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  virtual const ControlVector& GetFinalControlVector() const = 0;
+
+ protected:
+  /**
    * Converts a Translation2d into a vector that is compatible with Eigen.
    *
    * @param translation The Translation2d to convert.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h
index 90b6107..b023f12 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h
@@ -77,6 +77,17 @@
   static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
       const std::vector<Spline<5>::ControlVector>& controlVectors);
 
+  /**
+   * Optimizes the curvature of 2 or more quintic splines at knot points.
+   * Overall, this reduces the integral of the absolute value of the second
+   * derivative across the set of splines.
+   *
+   * @param splines A vector of un-optimized quintic splines.
+   * @return A vector of optimized quintic splines.
+   */
+  static std::vector<QuinticHermiteSpline> OptimizeCurvature(
+      const std::vector<QuinticHermiteSpline>& splines);
+
  private:
   static Spline<3>::ControlVector CubicControlVector(double scalar,
                                                      const Pose2d& point) {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
index ad711ee..a267945 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
@@ -26,19 +26,28 @@
       units::unit_t<units::compound_unit<units::newton_meters,
                                          units::inverse<units::ampere>>>;
 
+  /// Voltage at which the motor constants were measured.
   units::volt_t nominalVoltage;
+
+  /// Torque when stalled.
   units::newton_meter_t stallTorque;
+
+  /// Current draw when stalled.
   units::ampere_t stallCurrent;
+
+  /// Current draw under no load.
   units::ampere_t freeCurrent;
+
+  /// Angular velocity under no load.
   units::radians_per_second_t freeSpeed;
 
-  // Resistance of motor
+  /// Motor internal resistance.
   units::ohm_t R;
 
-  // Motor velocity constant
+  /// Motor velocity constant.
   radians_per_second_per_volt_t Kv;
 
-  // Torque constant
+  /// Motor torque constant.
   newton_meters_per_ampere_t Kt;
 
   /**
@@ -239,3 +248,6 @@
 };
 
 }  // namespace frc
+
+#include "frc/system/plant/proto/DCMotorProto.h"
+#include "frc/system/plant/struct/DCMotorStruct.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index 64f3496..ad5d3b0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -20,6 +20,9 @@
 #include "units/voltage.h"
 
 namespace frc {
+/**
+ * Linear system ID utility functions.
+ */
 class WPILIB_DLLEXPORT LinearSystemId {
  public:
   template <typename Distance>
@@ -38,12 +41,13 @@
    * @param motor The motor (or gearbox) attached to the carriage.
    * @param mass The mass of the elevator carriage, in kilograms.
    * @param radius The radius of the elevator's driving drum, in meters.
-   * @param G Gear ratio from motor to carriage.
-   * @throws std::domain_error if mass <= 0, radius <= 0, or G <= 0.
+   * @param gearing Gear ratio from motor to carriage.
+   * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
    */
   static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
                                               units::kilogram_t mass,
-                                              units::meter_t radius, double G);
+                                              units::meter_t radius,
+                                              double gearing);
 
   /**
    * Create a state-space model of a single-jointed arm system.The states of the
@@ -52,11 +56,11 @@
    *
    * @param motor The motor (or gearbox) attached to the arm.
    * @param J The moment of inertia J of the arm.
-   * @param G Gear ratio from motor to arm.
-   * @throws std::domain_error if J <= 0 or G <= 0.
+   * @param gearing Gear ratio from motor to arm.
+   * @throws std::domain_error if J <= 0 or gearing <= 0.
    */
   static LinearSystem<2, 1, 1> SingleJointedArmSystem(
-      DCMotor motor, units::kilogram_square_meter_t J, double G);
+      DCMotor motor, units::kilogram_square_meter_t J, double gearing);
 
   /**
    * Create a state-space model for a 1 DOF velocity system from its kV
@@ -76,6 +80,8 @@
    * @param kV The velocity gain, in volts/(unit/sec).
    * @param kA The acceleration gain, in volts/(unit/sec²).
    * @throws std::domain_error if kV <= 0 or kA <= 0.
+   * @see <a
+   * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   template <typename Distance>
     requires std::same_as<units::meter, Distance> ||
@@ -117,6 +123,8 @@
    * @param kA The acceleration gain, in volts/(unit/sec²).
    *
    * @throws std::domain_error if kV <= 0 or kA <= 0.
+   * @see <a
+   * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   template <typename Distance>
     requires std::same_as<units::meter, Distance> ||
@@ -158,6 +166,8 @@
    *                  second squared).
    * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
    *         or kAAngular <= 0.
+   * @see <a
+   * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
       decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
@@ -185,6 +195,8 @@
    *                   right wheels, in meters.
    * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
    *         kAAngular <= 0, or trackwidth <= 0.
+   * @see <a
+   * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
       decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
@@ -198,12 +210,12 @@
    *
    * @param motor The motor (or gearbox) attached to the flywheel.
    * @param J The moment of inertia J of the flywheel.
-   * @param G Gear ratio from motor to flywheel.
-   * @throws std::domain_error if J <= 0 or G <= 0.
+   * @param gearing Gear ratio from motor to flywheel.
+   * @throws std::domain_error if J <= 0 or gearing <= 0.
    */
   static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
                                               units::kilogram_square_meter_t J,
-                                              double G);
+                                              double gearing);
 
   /**
    * Create a state-space model of a DC motor system. The states of the system
@@ -212,12 +224,14 @@
    *
    * @param motor The motor (or gearbox) attached to the system.
    * @param J the moment of inertia J of the DC motor.
-   * @param G Gear ratio from motor to output.
-   * @throws std::domain_error if J <= 0 or G <= 0.
+   * @param gearing Gear ratio from motor to output.
+   * @throws std::domain_error if J <= 0 or gearing <= 0.
+   * @see <a
+   * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
                                              units::kilogram_square_meter_t J,
-                                             double G);
+                                             double gearing);
 
   /**
    * Create a state-space model of a DC motor system from its kV
@@ -271,13 +285,13 @@
    * @param r The radius of the wheels in meters.
    * @param rb The radius of the base (half of the track width), in meters.
    * @param J The moment of inertia of the robot.
-   * @param G Gear ratio from motor to wheel.
+   * @param gearing Gear ratio from motor to wheel.
    * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
-   *         G <= 0.
+   *         gearing <= 0.
    */
   static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
       const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
-      units::meter_t rb, units::kilogram_square_meter_t J, double G);
+      units::meter_t rb, units::kilogram_square_meter_t J, double gearing);
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h
new file mode 100644
index 0000000..acae1db
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h
@@ -0,0 +1,17 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/system/plant/DCMotor.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DCMotor> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::DCMotor Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::DCMotor& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h
new file mode 100644
index 0000000..22a6405
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h
@@ -0,0 +1,26 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/system/plant/DCMotor.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DCMotor> {
+  static constexpr std::string_view GetTypeString() { return "struct:DCMotor"; }
+  static constexpr size_t GetSize() { return 40; }
+  static constexpr std::string_view GetSchema() {
+    return "double nominal_voltage;double stall_torque;double "
+           "stall_current;double "
+           "free_current;double free_speed";
+  }
+
+  static frc::DCMotor Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::DCMotor& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DCMotor>);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
index f45987b..3622aa7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
@@ -55,36 +55,80 @@
   using KA = units::compound_unit<Input, units::inverse<Acceleration>>;
   using kA_t = units::unit_t<KA>;
 
+  /**
+   * Profile timing.
+   */
+  class ProfileTiming {
+   public:
+    /// Profile inflection time.
+    units::second_t inflectionTime;
+
+    /// Total profile time.
+    units::second_t totalTime;
+
+    /**
+     * Decides if the profile is finished by time t.
+     *
+     * @param t The time since the beginning of the profile.
+     * @return if the profile is finished at time t.
+     */
+    bool IsFinished(const units::second_t& t) const { return t >= totalTime; }
+  };
+
+  /**
+   * Profile constraints.
+   */
   class Constraints {
    public:
+    /**
+     * Construct constraints for an ExponentialProfile.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param A The State-Space 1x1 system matrix.
+     * @param B The State-Space 1x1 input matrix.
+     */
     Constraints(Input_t maxInput, A_t A, B_t B)
         : maxInput{maxInput}, A{A}, B{B} {}
+
+    /**
+     * Construct constraints for an ExponentialProfile from characteristics.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param kV The velocity gain.
+     * @param kA The acceleration gain.
+     */
     Constraints(Input_t maxInput, kV_t kV, kA_t kA)
         : maxInput{maxInput}, A{-kV / kA}, B{1 / kA} {}
+
+    /**
+     * Computes the max achievable velocity for an Exponential Profile.
+     *
+     * @return The seady-state velocity achieved by this profile.
+     */
     Velocity_t MaxVelocity() const { return -maxInput * B / A; }
 
+    /// Maximum unsigned input voltage.
     Input_t maxInput{0};
+
+    /// The State-Space 1x1 system matrix.
     A_t A{0};
+
+    /// The State-Space 1x1 input matrix.
     B_t B{0};
   };
 
+  /** Profile state. */
   class State {
    public:
+    /// The position at this state.
     Distance_t position{0};
+
+    /// The velocity at this state.
     Velocity_t velocity{0};
+
     bool operator==(const State&) const = default;
   };
 
-  class ProfileTiming {
-   public:
-    units::second_t inflectionTime;
-    units::second_t totalTime;
-
-    bool IsFinished(const units::second_t& time) const {
-      return time > totalTime;
-    }
-  };
-
   /**
    * Construct a ExponentialProfile.
    *
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
index ca97593..753bf9e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -28,19 +28,19 @@
    * Represents one point on the trajectory.
    */
   struct WPILIB_DLLEXPORT State {
-    // The time elapsed since the beginning of the trajectory.
+    /// The time elapsed since the beginning of the trajectory.
     units::second_t t = 0_s;
 
-    // The speed at that point of the trajectory.
+    /// The speed at that point of the trajectory.
     units::meters_per_second_t velocity = 0_mps;
 
-    // The acceleration at that point of the trajectory.
+    /// The acceleration at that point of the trajectory.
     units::meters_per_second_squared_t acceleration = 0_mps_sq;
 
-    // The pose at that point of the trajectory.
+    /// The pose at that point of the trajectory.
     Pose2d pose;
 
-    // The curvature at that point of the trajectory.
+    /// The curvature at that point of the trajectory.
     units::curvature_t curvature{0.0};
 
     /**
@@ -145,3 +145,6 @@
 void from_json(const wpi::json& json, Trajectory::State& state);
 
 }  // namespace frc
+
+#include "frc/trajectory/proto/TrajectoryProto.h"
+#include "frc/trajectory/proto/TrajectoryStateProto.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
index 3ba882f..48a0a49 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -12,6 +12,9 @@
 #include "frc/trajectory/Trajectory.h"
 
 namespace frc {
+/**
+ * Trajectory utilities.
+ */
 class WPILIB_DLLEXPORT TrajectoryUtil {
  public:
   TrajectoryUtil() = delete;
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 73aab38..08640fb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -29,8 +29,8 @@
  * Run on update:
  * @code{.cpp}
  * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
- *                                               unprofiledReference,
- *                                               previousProfiledReference);
+ *                                               previousProfiledReference,
+ *                                               unprofiledReference);
  * @endcode
  *
  * where `unprofiledReference` is free to change between calls. Note that when
@@ -52,25 +52,49 @@
       units::compound_unit<Velocity, units::inverse<units::seconds>>;
   using Acceleration_t = units::unit_t<Acceleration>;
 
+  /**
+   * Profile constraints.
+   */
   class Constraints {
    public:
+    /// Maximum velocity.
+    Velocity_t maxVelocity{0};
+
+    /// Maximum acceleration.
+    Acceleration_t maxAcceleration{0};
+
+    /**
+     * Default constructor.
+     */
     Constraints() {
       wpi::math::MathSharedStore::ReportUsage(
           wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1);
     }
-    Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
-        : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
+
+    /**
+     * Constructs constraints for a Trapezoid Profile.
+     *
+     * @param maxVelocity Maximum velocity.
+     * @param maxAcceleration Maximum acceleration.
+     */
+    Constraints(Velocity_t maxVelocity, Acceleration_t maxAcceleration)
+        : maxVelocity{maxVelocity}, maxAcceleration{maxAcceleration} {
       wpi::math::MathSharedStore::ReportUsage(
           wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1);
     }
-    Velocity_t maxVelocity{0};
-    Acceleration_t maxAcceleration{0};
   };
 
+  /**
+   * Profile state.
+   */
   class State {
    public:
+    /// The position at this state.
     Distance_t position{0};
+
+    /// The velocity at this state.
     Velocity_t velocity{0};
+
     bool operator==(const State&) const = default;
   };
 
@@ -121,10 +145,10 @@
    * where the beginning of the profile was at time t = 0.
    *
    * @param t The time since the beginning of the profile.
-   * @param goal        The desired state when the profile is complete.
    * @param current     The initial state (usually the current state).
+   * @param goal        The desired state when the profile is complete.
    */
-  State Calculate(units::second_t t, State goal, State current);
+  State Calculate(units::second_t t, State current, State goal);
 
   /**
    * Returns the time left until a target distance in the profile is reached.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 24e0a46..c970a79 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -97,8 +97,8 @@
 }
 template <class Distance>
 typename TrapezoidProfile<Distance>::State
-TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
-                                      State current) {
+TrapezoidProfile<Distance>::Calculate(units::second_t t, State current,
+                                      State goal) {
   m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
   m_current = Direct(current);
   goal = Direct(goal);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h
new file mode 100644
index 0000000..4b485b2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/trajectory/Trajectory.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Trajectory> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Trajectory Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Trajectory& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h
new file mode 100644
index 0000000..150837b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h
@@ -0,0 +1,18 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/trajectory/Trajectory.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Trajectory::State> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Trajectory::State Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Trajectory::State& value);
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/.clang-format b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/.clang-format
new file mode 100644
index 0000000..28251c6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/.clang-format
@@ -0,0 +1,12 @@
+---
+Language:     Cpp
+BasedOnStyle: Google
+ColumnLimit:  120
+SortIncludes: false
+AttributeMacros:
+- EIGEN_STRONG_INLINE
+- EIGEN_ALWAYS_INLINE
+- EIGEN_DEVICE_FUNC
+- EIGEN_DONT_INLINE
+- EIGEN_DEPRECATED
+- EIGEN_UNUSED
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky
index ef249de..c3bf845 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky
@@ -14,32 +14,30 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup Cholesky_Module Cholesky module
-  *
-  *
-  *
-  * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
-  * Those decompositions are also accessible via the following methods:
-  *  - MatrixBase::llt()
-  *  - MatrixBase::ldlt()
-  *  - SelfAdjointView::llt()
-  *  - SelfAdjointView::ldlt()
-  *
-  * \code
-  * #include <Eigen/Cholesky>
-  * \endcode
-  */
+ *
+ *
+ *
+ * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
+ * Those decompositions are also accessible via the following methods:
+ *  - MatrixBase::llt()
+ *  - MatrixBase::ldlt()
+ *  - SelfAdjointView::llt()
+ *  - SelfAdjointView::ldlt()
+ *
+ * \code
+ * #include <Eigen/Cholesky>
+ * \endcode
+ */
 
+// IWYU pragma: begin_exports
 #include "src/Cholesky/LLT.h"
 #include "src/Cholesky/LDLT.h"
 #ifdef EIGEN_USE_LAPACKE
-#ifdef EIGEN_USE_MKL
-// #include "mkl_lapacke.h"
-#else
-// #include "src/misc/lapacke.h"
-#endif
+// #include "src/misc/lapacke_helpers.h"
 // #include "src/Cholesky/LLT_LAPACKE.h"
 #endif
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_CHOLESKY_MODULE_H
+#endif  // EIGEN_CHOLESKY_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
index fd5e098..a30eeda 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
@@ -8,8 +8,8 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-#ifndef EIGEN_CORE_H
-#define EIGEN_CORE_H
+#ifndef EIGEN_CORE_MODULE_H
+#define EIGEN_CORE_MODULE_H
 
 // first thing Eigen does: stop the compiler from reporting useless warnings.
 #include "src/Core/util/DisableStupidWarnings.h"
@@ -24,27 +24,25 @@
 // We need cuda_runtime.h/hip_runtime.h to ensure that
 // the EIGEN_USING_STD macro works properly on the device side
 #if defined(EIGEN_CUDACC)
-  #include <cuda_runtime.h>
+#include <cuda_runtime.h>
 #elif defined(EIGEN_HIPCC)
-  #include <hip/hip_runtime.h>
+#include <hip/hip_runtime.h>
 #endif
 
-
 #ifdef EIGEN_EXCEPTIONS
-  #include <new>
+#include <new>
 #endif
 
-// Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3)
+// Disable the ipa-cp-clone optimization flag with MinGW 6.x or older (enabled by default with -O3)
 // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details.
-#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6) && EIGEN_GNUC_AT_MOST(5,5)
-  #pragma GCC optimize ("-fno-ipa-cp-clone")
+#if EIGEN_COMP_MINGW && EIGEN_GNUC_STRICT_LESS_THAN(6, 0, 0)
+#pragma GCC optimize("-fno-ipa-cp-clone")
 #endif
 
 // Prevent ICC from specializing std::complex operators that silently fail
 // on device. This allows us to use our own device-compatible specializations
 // instead.
-#if defined(EIGEN_COMP_ICC) && defined(EIGEN_GPU_COMPILE_PHASE) \
-    && !defined(_OVERRIDE_COMPLEX_SPECIALIZATION_)
+#if EIGEN_COMP_ICC && defined(EIGEN_GPU_COMPILE_PHASE) && !defined(_OVERRIDE_COMPLEX_SPECIALIZATION_)
 #define _OVERRIDE_COMPLEX_SPECIALIZATION_ 1
 #endif
 #include <complex>
@@ -53,20 +51,20 @@
 // and inclusion of their respective header files
 // #include "src/Core/util/MKL_support.h"
 
-
 #if defined(EIGEN_HAS_CUDA_FP16) || defined(EIGEN_HAS_HIP_FP16)
-  #define EIGEN_HAS_GPU_FP16
+#define EIGEN_HAS_GPU_FP16
 #endif
 
 #if defined(EIGEN_HAS_CUDA_BF16) || defined(EIGEN_HAS_HIP_BF16)
-  #define EIGEN_HAS_GPU_BF16
+#define EIGEN_HAS_GPU_BF16
 #endif
 
 #if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE)
-  #define EIGEN_HAS_OPENMP
+#define EIGEN_HAS_OPENMP
 #endif
 
 #ifdef EIGEN_HAS_OPENMP
+#include <atomic>
 #include <omp.h>
 #endif
 
@@ -81,27 +79,23 @@
 #include <cstddef>
 #include <cstdlib>
 #include <cmath>
-#include <cassert>
 #include <functional>
-#include <sstream>
 #ifndef EIGEN_NO_IO
-  #include <iosfwd>
+#include <sstream>
+#include <iosfwd>
 #endif
 #include <cstring>
 #include <string>
 #include <limits>
-#include <climits> // for CHAR_BIT
+#include <climits>  // for CHAR_BIT
 // for min/max:
 #include <algorithm>
 
-#if EIGEN_HAS_CXX11
 #include <array>
-#endif
+#include <vector>
 
 // for std::is_nothrow_move_assignable
-#ifdef EIGEN_INCLUDE_TYPE_TRAITS
 #include <type_traits>
-#endif
 
 // for outputting debug info
 #ifdef EIGEN_DEBUG_ASSIGN
@@ -109,31 +103,33 @@
 #endif
 
 // required for __cpuid, needs to be included after cmath
-#if EIGEN_COMP_MSVC && EIGEN_ARCH_i386_OR_x86_64 && !EIGEN_OS_WINCE
-  #include <intrin.h>
+// also required for _BitScanReverse on Windows on ARM
+#if EIGEN_COMP_MSVC && (EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM64) && !EIGEN_OS_WINCE
+#include <intrin.h>
 #endif
 
 #if defined(EIGEN_USE_SYCL)
-  #undef min
-  #undef max
-  #undef isnan
-  #undef isinf
-  #undef isfinite
-  #include <CL/sycl.hpp>
-  #include <map>
-  #include <memory>
-  #include <utility>
-  #include <thread>
-  #ifndef EIGEN_SYCL_LOCAL_THREAD_DIM0
-  #define EIGEN_SYCL_LOCAL_THREAD_DIM0 16
-  #endif
-  #ifndef EIGEN_SYCL_LOCAL_THREAD_DIM1
-  #define EIGEN_SYCL_LOCAL_THREAD_DIM1 16
-  #endif
+#undef min
+#undef max
+#undef isnan
+#undef isinf
+#undef isfinite
+#include <CL/sycl.hpp>
+#include <map>
+#include <memory>
+#include <utility>
+#include <thread>
+#ifndef EIGEN_SYCL_LOCAL_THREAD_DIM0
+#define EIGEN_SYCL_LOCAL_THREAD_DIM0 16
+#endif
+#ifndef EIGEN_SYCL_LOCAL_THREAD_DIM1
+#define EIGEN_SYCL_LOCAL_THREAD_DIM1 16
+#endif
 #endif
 
-
-#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API || defined EIGEN2_SUPPORT
+#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || \
+    defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API ||  \
+    defined EIGEN2_SUPPORT
 // This will generate an error message:
 #error Eigen2-support is only available up to version 3.2. Please go to "http://eigen.tuxfamily.org/index.php?title=Eigen2" for further information
 #endif
@@ -146,26 +142,39 @@
 // gcc 4.6.0 wants std:: for ptrdiff_t
 using std::ptrdiff_t;
 
-}
+}  // namespace Eigen
 
 /** \defgroup Core_Module Core module
-  * This is the main module of Eigen providing dense matrix and vector support
-  * (both fixed and dynamic size) with all the features corresponding to a BLAS library
-  * and much more...
-  *
-  * \code
-  * #include <Eigen/Core>
-  * \endcode
-  */
+ * This is the main module of Eigen providing dense matrix and vector support
+ * (both fixed and dynamic size) with all the features corresponding to a BLAS library
+ * and much more...
+ *
+ * \code
+ * #include <Eigen/Core>
+ * \endcode
+ */
 
+#ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+// #include "mkl_lapacke.h"
+#else
+// #include "src/misc/lapacke.h"
+#endif
+#endif
+
+// IWYU pragma: begin_exports
 #include "src/Core/util/Constants.h"
 #include "src/Core/util/Meta.h"
+#include "src/Core/util/Assert.h"
 #include "src/Core/util/ForwardDeclarations.h"
 #include "src/Core/util/StaticAssert.h"
 #include "src/Core/util/XprHelper.h"
 #include "src/Core/util/Memory.h"
 #include "src/Core/util/IntegralConstant.h"
+#include "src/Core/util/Serializer.h"
 #include "src/Core/util/SymbolicIndex.h"
+#include "src/Core/util/EmulateArray.h"
+#include "src/Core/util/MoreMeta.h"
 
 #include "src/Core/NumTraits.h"
 #include "src/Core/MathFunctions.h"
@@ -175,73 +184,78 @@
 // Generic half float support
 #include "src/Core/arch/Default/Half.h"
 #include "src/Core/arch/Default/BFloat16.h"
-#include "src/Core/arch/Default/TypeCasting.h"
 #include "src/Core/arch/Default/GenericPacketMathFunctionsFwd.h"
 
 #if defined EIGEN_VECTORIZE_AVX512
-  #include "src/Core/arch/SSE/PacketMath.h"
-  #include "src/Core/arch/SSE/TypeCasting.h"
-  #include "src/Core/arch/SSE/Complex.h"
-  #include "src/Core/arch/AVX/PacketMath.h"
-  #include "src/Core/arch/AVX/TypeCasting.h"
-  #include "src/Core/arch/AVX/Complex.h"
-  // #include "src/Core/arch/AVX512/PacketMath.h"
-  // #include "src/Core/arch/AVX512/TypeCasting.h"
-  // #include "src/Core/arch/AVX512/Complex.h"
-  #include "src/Core/arch/SSE/MathFunctions.h"
-  #include "src/Core/arch/AVX/MathFunctions.h"
-  // #include "src/Core/arch/AVX512/MathFunctions.h"
+#if defined EIGEN_VECTORIZE_AVX512FP16
+// #include "src/Core/arch/AVX512/PacketMathFP16.h"
+#endif
+#include "src/Core/arch/SSE/PacketMath.h"
+#include "src/Core/arch/SSE/TypeCasting.h"
+#include "src/Core/arch/SSE/Complex.h"
+#include "src/Core/arch/AVX/PacketMath.h"
+#include "src/Core/arch/AVX/TypeCasting.h"
+#include "src/Core/arch/AVX/Complex.h"
+// #include "src/Core/arch/AVX512/PacketMath.h"
+// #include "src/Core/arch/AVX512/TypeCasting.h"
+// #include "src/Core/arch/AVX512/Complex.h"
+#include "src/Core/arch/SSE/MathFunctions.h"
+#include "src/Core/arch/AVX/MathFunctions.h"
+// #include "src/Core/arch/AVX512/MathFunctions.h"
+// #include "src/Core/arch/AVX512/TrsmKernel.h"
 #elif defined EIGEN_VECTORIZE_AVX
-  // Use AVX for floats and doubles, SSE for integers
-  #include "src/Core/arch/SSE/PacketMath.h"
-  #include "src/Core/arch/SSE/TypeCasting.h"
-  #include "src/Core/arch/SSE/Complex.h"
-  #include "src/Core/arch/AVX/PacketMath.h"
-  #include "src/Core/arch/AVX/TypeCasting.h"
-  #include "src/Core/arch/AVX/Complex.h"
-  #include "src/Core/arch/SSE/MathFunctions.h"
-  #include "src/Core/arch/AVX/MathFunctions.h"
+   // Use AVX for floats and doubles, SSE for integers
+#include "src/Core/arch/SSE/PacketMath.h"
+#include "src/Core/arch/SSE/TypeCasting.h"
+#include "src/Core/arch/SSE/Complex.h"
+#include "src/Core/arch/AVX/PacketMath.h"
+#include "src/Core/arch/AVX/TypeCasting.h"
+#include "src/Core/arch/AVX/Complex.h"
+#include "src/Core/arch/SSE/MathFunctions.h"
+#include "src/Core/arch/AVX/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_SSE
-  #include "src/Core/arch/SSE/PacketMath.h"
-  #include "src/Core/arch/SSE/TypeCasting.h"
-  #include "src/Core/arch/SSE/MathFunctions.h"
-  #include "src/Core/arch/SSE/Complex.h"
+#include "src/Core/arch/SSE/PacketMath.h"
+#include "src/Core/arch/SSE/TypeCasting.h"
+#include "src/Core/arch/SSE/MathFunctions.h"
+#include "src/Core/arch/SSE/Complex.h"
 #elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
-  // #include "src/Core/arch/AltiVec/PacketMath.h"
-  // #include "src/Core/arch/AltiVec/MathFunctions.h"
-  // #include "src/Core/arch/AltiVec/Complex.h"
+// #include "src/Core/arch/AltiVec/PacketMath.h"
+// #include "src/Core/arch/AltiVec/TypeCasting.h"
+// #include "src/Core/arch/AltiVec/MathFunctions.h"
+// #include "src/Core/arch/AltiVec/Complex.h"
 #elif defined EIGEN_VECTORIZE_NEON
-  #include "src/Core/arch/NEON/PacketMath.h"
-  #include "src/Core/arch/NEON/TypeCasting.h"
-  #include "src/Core/arch/NEON/MathFunctions.h"
-  #include "src/Core/arch/NEON/Complex.h"
+#include "src/Core/arch/NEON/PacketMath.h"
+#include "src/Core/arch/NEON/TypeCasting.h"
+#include "src/Core/arch/NEON/MathFunctions.h"
+#include "src/Core/arch/NEON/Complex.h"
 #elif defined EIGEN_VECTORIZE_SVE
-  // #include "src/Core/arch/SVE/PacketMath.h"
-  // #include "src/Core/arch/SVE/TypeCasting.h"
-  // #include "src/Core/arch/SVE/MathFunctions.h"
+// #include "src/Core/arch/SVE/PacketMath.h"
+// #include "src/Core/arch/SVE/TypeCasting.h"
+// #include "src/Core/arch/SVE/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_ZVECTOR
-  // #include "src/Core/arch/ZVector/PacketMath.h"
-  // #include "src/Core/arch/ZVector/MathFunctions.h"
-  // #include "src/Core/arch/ZVector/Complex.h"
+// #include "src/Core/arch/ZVector/PacketMath.h"
+// #include "src/Core/arch/ZVector/MathFunctions.h"
+// #include "src/Core/arch/ZVector/Complex.h"
 #elif defined EIGEN_VECTORIZE_MSA
-  // #include "src/Core/arch/MSA/PacketMath.h"
-  // #include "src/Core/arch/MSA/MathFunctions.h"
-  // #include "src/Core/arch/MSA/Complex.h"
+// #include "src/Core/arch/MSA/PacketMath.h"
+// #include "src/Core/arch/MSA/MathFunctions.h"
+// #include "src/Core/arch/MSA/Complex.h"
+#elif defined EIGEN_VECTORIZE_HVX
+// #include "src/Core/arch/HVX/PacketMath.h"
 #endif
 
 #if defined EIGEN_VECTORIZE_GPU
-  // #include "src/Core/arch/GPU/PacketMath.h"
-  // #include "src/Core/arch/GPU/MathFunctions.h"
-  // #include "src/Core/arch/GPU/TypeCasting.h"
+// #include "src/Core/arch/GPU/PacketMath.h"
+// #include "src/Core/arch/GPU/MathFunctions.h"
+// #include "src/Core/arch/GPU/TypeCasting.h"
 #endif
 
 #if defined(EIGEN_USE_SYCL)
-  // #include "src/Core/arch/SYCL/SyclMemoryModel.h"
-  // #include "src/Core/arch/SYCL/InteropHeaders.h"
+// #include "src/Core/arch/SYCL/InteropHeaders.h"
 #if !defined(EIGEN_DONT_VECTORIZE_SYCL)
-  // #include "src/Core/arch/SYCL/PacketMath.h"
-  // #include "src/Core/arch/SYCL/MathFunctions.h"
-  // #include "src/Core/arch/SYCL/TypeCasting.h"
+// #include "src/Core/arch/SYCL/PacketMath.h"
+// #include "src/Core/arch/SYCL/MathFunctions.h"
+// #include "src/Core/arch/SYCL/TypeCasting.h"
 #endif
 #endif
 
@@ -256,17 +270,21 @@
 #include "src/Core/functors/StlFunctors.h"
 #include "src/Core/functors/AssignmentFunctors.h"
 
-// Specialized functors to enable the processing of complex numbers
-// on CUDA devices
-#ifdef EIGEN_CUDACC
-// #include "src/Core/arch/CUDA/Complex.h"
+// Specialized functors for GPU.
+#ifdef EIGEN_GPUCC
+// #include "src/Core/arch/GPU/Complex.h"
+#endif
+
+// Specializations of vectorized activation functions for NEON.
+#ifdef EIGEN_VECTORIZE_NEON
+#include "src/Core/arch/NEON/UnaryFunctors.h"
 #endif
 
 #include "src/Core/util/IndexedViewHelper.h"
 #include "src/Core/util/ReshapedHelper.h"
 #include "src/Core/ArithmeticSequence.h"
 #ifndef EIGEN_NO_IO
-  #include "src/Core/IO.h"
+#include "src/Core/IO.h"
 #endif
 #include "src/Core/DenseCoeffsBase.h"
 #include "src/Core/DenseBase.h"
@@ -277,9 +295,9 @@
 #include "src/Core/CoreEvaluators.h"
 #include "src/Core/AssignEvaluator.h"
 
-#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874
-                                // at least confirmed with Doxygen 1.5.5 and 1.5.6
-  #include "src/Core/Assign.h"
+#ifndef EIGEN_PARSED_BY_DOXYGEN  // work around Doxygen bug triggered by Assign.h r814874
+                                 // at least confirmed with Doxygen 1.5.5 and 1.5.6
+#include "src/Core/Assign.h"
 #endif
 
 #include "src/Core/ArrayBase.h"
@@ -314,6 +332,7 @@
 #include "src/Core/DiagonalMatrix.h"
 #include "src/Core/Diagonal.h"
 #include "src/Core/DiagonalProduct.h"
+#include "src/Core/SkewSymmetricMatrix3.h"
 #include "src/Core/Redux.h"
 #include "src/Core/Visitor.h"
 #include "src/Core/Fuzzy.h"
@@ -328,6 +347,9 @@
 #include "src/Core/TriangularMatrix.h"
 #include "src/Core/SelfAdjointView.h"
 #include "src/Core/products/GeneralBlockPanelKernel.h"
+#ifdef EIGEN_GEMM_THREADPOOL
+// #include "ThreadPool"
+#endif
 #include "src/Core/products/Parallelizer.h"
 #include "src/Core/ProductEvaluators.h"
 #include "src/Core/products/GeneralMatrixVector.h"
@@ -346,13 +368,20 @@
 #include "src/Core/CoreIterators.h"
 #include "src/Core/ConditionEstimator.h"
 
-#if defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
-  // #include "src/Core/arch/AltiVec/MatrixProduct.h"
+#if defined(EIGEN_VECTORIZE_VSX)
+// #include "src/Core/arch/AltiVec/MatrixProduct.h"
 #elif defined EIGEN_VECTORIZE_NEON
-  #include "src/Core/arch/NEON/GeneralBlockPanelKernel.h"
+#include "src/Core/arch/NEON/GeneralBlockPanelKernel.h"
 #endif
 
-#include "src/Core/BooleanRedux.h"
+#if defined(EIGEN_VECTORIZE_AVX512)
+// #include "src/Core/arch/AVX512/GemmKernel.h"
+#endif
+
+#if defined(EIGEN_VECTORIZE_HVX)
+// #include "src/Core/arch/HVX/GeneralBlockPanelKernel.h"
+#endif
+
 #include "src/Core/Select.h"
 #include "src/Core/VectorwiseOp.h"
 #include "src/Core/PartialReduxEvaluator.h"
@@ -371,14 +400,15 @@
 // #include "src/Core/products/TriangularMatrixMatrix_BLAS.h"
 // #include "src/Core/products/TriangularMatrixVector_BLAS.h"
 // #include "src/Core/products/TriangularSolverMatrix_BLAS.h"
-#endif // EIGEN_USE_BLAS
+#endif  // EIGEN_USE_BLAS
 
 #ifdef EIGEN_USE_MKL_VML
 // #include "src/Core/Assign_MKL.h"
 #endif
 
 #include "src/Core/GlobalFunctions.h"
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_CORE_H
+#endif  // EIGEN_CORE_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
index c6defe3..51438ef 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
@@ -19,20 +19,22 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup Eigenvalues_Module Eigenvalues module
-  *
-  *
-  *
-  * This module mainly provides various eigenvalue solvers.
-  * This module also provides some MatrixBase methods, including:
-  *  - MatrixBase::eigenvalues(),
-  *  - MatrixBase::operatorNorm()
-  *
-  * \code
-  * #include <Eigen/Eigenvalues>
-  * \endcode
-  */
+ *
+ *
+ *
+ * This module mainly provides various eigenvalue solvers.
+ * This module also provides some MatrixBase methods, including:
+ *  - MatrixBase::eigenvalues(),
+ *  - MatrixBase::operatorNorm()
+ *
+ * \code
+ * #include <Eigen/Eigenvalues>
+ * \endcode
+ */
 
 #include "src/misc/RealSvd2x2.h"
+
+// IWYU pragma: begin_exports
 #include "src/Eigenvalues/Tridiagonalization.h"
 #include "src/Eigenvalues/RealSchur.h"
 #include "src/Eigenvalues/EigenSolver.h"
@@ -54,7 +56,8 @@
 // #include "src/Eigenvalues/ComplexSchur_LAPACKE.h"
 // #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h"
 #endif
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_EIGENVALUES_MODULE_H
+#endif  // EIGEN_EIGENVALUES_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder
index f2fa799..5070e07 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder
@@ -13,17 +13,19 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup Householder_Module Householder module
-  * This module provides Householder transformations.
-  *
-  * \code
-  * #include <Eigen/Householder>
-  * \endcode
-  */
+ * This module provides Householder transformations.
+ *
+ * \code
+ * #include <Eigen/Householder>
+ * \endcode
+ */
 
+// IWYU pragma: begin_exports
 #include "src/Householder/Householder.h"
 #include "src/Householder/HouseholderSequence.h"
 #include "src/Householder/BlockHouseholder.h"
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_HOUSEHOLDER_MODULE_H
+#endif  // EIGEN_HOUSEHOLDER_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers
index 957d575..fe5159e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers
@@ -13,10 +13,11 @@
 
 #include "src/Core/util/DisableStupidWarnings.h"
 
-/** 
+/**
   * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module
   *
-  * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse.
+  * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a
+  squared matrix, usually very large and sparse.
   * Those solvers are accessible via the following classes:
   *  - ConjugateGradient for selfadjoint (hermitian) matrices,
   *  - LeastSquaresConjugateGradient for rectangular least-square problems,
@@ -27,13 +28,15 @@
   *  - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices.
   *  - IncompleteLUT - incomplete LU factorization with dual thresholding
   *
-  * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport.
+  * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport,
+  UmfPackSupport, SuperLUSupport, AccelerateSupport.
   *
     \code
     #include <Eigen/IterativeLinearSolvers>
     \endcode
   */
 
+// IWYU pragma: begin_exports
 #include "src/IterativeLinearSolvers/SolveWithGuess.h"
 #include "src/IterativeLinearSolvers/IterativeSolverBase.h"
 #include "src/IterativeLinearSolvers/BasicPreconditioners.h"
@@ -42,7 +45,8 @@
 #include "src/IterativeLinearSolvers/BiCGSTAB.h"
 #include "src/IterativeLinearSolvers/IncompleteLUT.h"
 #include "src/IterativeLinearSolvers/IncompleteCholesky.h"
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
+#endif  // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi
index 43edc7a..31eb36a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi
@@ -13,20 +13,21 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup Jacobi_Module Jacobi module
-  * This module provides Jacobi and Givens rotations.
-  *
-  * \code
-  * #include <Eigen/Jacobi>
-  * \endcode
-  *
-  * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation:
-  *  - MatrixBase::applyOnTheLeft()
-  *  - MatrixBase::applyOnTheRight().
-  */
+ * This module provides Jacobi and Givens rotations.
+ *
+ * \code
+ * #include <Eigen/Jacobi>
+ * \endcode
+ *
+ * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation:
+ *  - MatrixBase::applyOnTheLeft()
+ *  - MatrixBase::applyOnTheRight().
+ */
 
+// IWYU pragma: begin_exports
 #include "src/Jacobi/Jacobi.h"
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_JACOBI_MODULE_H
-
+#endif  // EIGEN_JACOBI_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU
index a1b5d46..e58e895 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU
@@ -13,35 +13,34 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup LU_Module LU module
-  * This module includes %LU decomposition and related notions such as matrix inversion and determinant.
-  * This module defines the following MatrixBase methods:
-  *  - MatrixBase::inverse()
-  *  - MatrixBase::determinant()
-  *
-  * \code
-  * #include <Eigen/LU>
-  * \endcode
-  */
+ * This module includes %LU decomposition and related notions such as matrix inversion and determinant.
+ * This module defines the following MatrixBase methods:
+ *  - MatrixBase::inverse()
+ *  - MatrixBase::determinant()
+ *
+ * \code
+ * #include <Eigen/LU>
+ * \endcode
+ */
 
 #include "src/misc/Kernel.h"
 #include "src/misc/Image.h"
+
+// IWYU pragma: begin_exports
 #include "src/LU/FullPivLU.h"
 #include "src/LU/PartialPivLU.h"
 #ifdef EIGEN_USE_LAPACKE
-#ifdef EIGEN_USE_MKL
-// #include "mkl_lapacke.h"
-#else
-// #include "src/misc/lapacke.h"
-#endif
+// #include "src/misc/lapacke_helpers.h"
 // #include "src/LU/PartialPivLU_LAPACKE.h"
 #endif
 #include "src/LU/Determinant.h"
 #include "src/LU/InverseImpl.h"
 
 #if defined EIGEN_VECTORIZE_SSE || defined EIGEN_VECTORIZE_NEON
-  #include "src/LU/arch/InverseSize4.h"
+#include "src/LU/arch/InverseSize4.h"
 #endif
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_LU_MODULE_H
+#endif  // EIGEN_LU_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods
index 29691a6..921b8a0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods
@@ -12,59 +12,62 @@
 
 #include "src/Core/util/DisableStupidWarnings.h"
 
-/** 
-  * \defgroup OrderingMethods_Module OrderingMethods module
-  *
-  * This module is currently for internal use only
-  * 
-  * It defines various built-in and external ordering methods for sparse matrices. 
-  * They are typically used to reduce the number of elements during 
-  * the sparse matrix decomposition (LLT, LU, QR).
-  * Precisely, in a preprocessing step, a permutation matrix P is computed using 
-  * those ordering methods and applied to the columns of the matrix. 
-  * Using for instance the sparse Cholesky decomposition, it is expected that 
-  * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A).
-  * 
-  * 
-  * Usage : 
-  * \code
-  * #include <Eigen/OrderingMethods>
-  * \endcode
-  * 
-  * A simple usage is as a template parameter in the sparse decomposition classes : 
-  * 
-  * \code 
-  * SparseLU<MatrixType, COLAMDOrdering<int> > solver;
-  * \endcode 
-  * 
-  * \code 
-  * SparseQR<MatrixType, COLAMDOrdering<int> > solver;
-  * \endcode
-  * 
-  * It is possible as well to call directly a particular ordering method for your own purpose, 
-  * \code 
-  * AMDOrdering<int> ordering;
-  * PermutationMatrix<Dynamic, Dynamic, int> perm;
-  * SparseMatrix<double> A; 
-  * //Fill the matrix ...
-  * 
-  * ordering(A, perm); // Call AMD
-  * \endcode
-  * 
-  * \note Some of these methods (like AMD or METIS), need the sparsity pattern 
-  * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 
-  * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method.
-  * If your matrix is already symmetric (at leat in structure), you can avoid that
-  * by calling the method with a SelfAdjointView type.
-  * 
-  * \code
-  *  // Call the ordering on the pattern of the lower triangular matrix A
-  * ordering(A.selfadjointView<Lower>(), perm);
-  * \endcode
-  */
+/**
+ * \defgroup OrderingMethods_Module OrderingMethods module
+ *
+ * This module is currently for internal use only
+ *
+ * It defines various built-in and external ordering methods for sparse matrices.
+ * They are typically used to reduce the number of elements during
+ * the sparse matrix decomposition (LLT, LU, QR).
+ * Precisely, in a preprocessing step, a permutation matrix P is computed using
+ * those ordering methods and applied to the columns of the matrix.
+ * Using for instance the sparse Cholesky decomposition, it is expected that
+ * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A).
+ *
+ *
+ * Usage :
+ * \code
+ * #include <Eigen/OrderingMethods>
+ * \endcode
+ *
+ * A simple usage is as a template parameter in the sparse decomposition classes :
+ *
+ * \code
+ * SparseLU<MatrixType, COLAMDOrdering<int> > solver;
+ * \endcode
+ *
+ * \code
+ * SparseQR<MatrixType, COLAMDOrdering<int> > solver;
+ * \endcode
+ *
+ * It is possible as well to call directly a particular ordering method for your own purpose,
+ * \code
+ * AMDOrdering<int> ordering;
+ * PermutationMatrix<Dynamic, Dynamic, int> perm;
+ * SparseMatrix<double> A;
+ * //Fill the matrix ...
+ *
+ * ordering(A, perm); // Call AMD
+ * \endcode
+ *
+ * \note Some of these methods (like AMD or METIS), need the sparsity pattern
+ * of the input matrix to be symmetric. When the matrix is structurally unsymmetric,
+ * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method.
+ * If your matrix is already symmetric (at leat in structure), you can avoid that
+ * by calling the method with a SelfAdjointView type.
+ *
+ * \code
+ *  // Call the ordering on the pattern of the lower triangular matrix A
+ * ordering(A.selfadjointView<Lower>(), perm);
+ * \endcode
+ */
 
+// IWYU pragma: begin_exports
 #include "src/OrderingMethods/Amd.h"
 #include "src/OrderingMethods/Ordering.h"
+// IWYU pragma: end_exports
+
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_ORDERINGMETHODS_MODULE_H
+#endif  // EIGEN_ORDERINGMETHODS_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR
index 42a3fa8..9392ecb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR
@@ -17,34 +17,32 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup QR_Module QR module
-  *
-  *
-  *
-  * This module provides various QR decompositions
-  * This module also provides some MatrixBase methods, including:
-  *  - MatrixBase::householderQr()
-  *  - MatrixBase::colPivHouseholderQr()
-  *  - MatrixBase::fullPivHouseholderQr()
-  *
-  * \code
-  * #include <Eigen/QR>
-  * \endcode
-  */
+ *
+ *
+ *
+ * This module provides various QR decompositions
+ * This module also provides some MatrixBase methods, including:
+ *  - MatrixBase::householderQr()
+ *  - MatrixBase::colPivHouseholderQr()
+ *  - MatrixBase::fullPivHouseholderQr()
+ *
+ * \code
+ * #include <Eigen/QR>
+ * \endcode
+ */
 
+// IWYU pragma: begin_exports
 #include "src/QR/HouseholderQR.h"
 #include "src/QR/FullPivHouseholderQR.h"
 #include "src/QR/ColPivHouseholderQR.h"
 #include "src/QR/CompleteOrthogonalDecomposition.h"
 #ifdef EIGEN_USE_LAPACKE
-#ifdef EIGEN_USE_MKL
-// #include "mkl_lapacke.h"
-#else
-// #include "src/misc/lapacke.h"
-#endif
+// #include "src/misc/lapacke_helpers.h"
 // #include "src/QR/HouseholderQR_LAPACKE.h"
 // #include "src/QR/ColPivHouseholderQR_LAPACKE.h"
 #endif
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_QR_MODULE_H
+#endif  // EIGEN_QR_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD
index 4441a38..66a7678 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD
@@ -15,36 +15,42 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup SVD_Module SVD module
-  *
-  *
-  *
-  * This module provides SVD decomposition for matrices (both real and complex).
-  * Two decomposition algorithms are provided:
-  *  - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones.
-  *  - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems.
-  * These decompositions are accessible via the respective classes and following MatrixBase methods:
-  *  - MatrixBase::jacobiSvd()
-  *  - MatrixBase::bdcSvd()
-  *
-  * \code
-  * #include <Eigen/SVD>
-  * \endcode
-  */
+ *
+ *
+ *
+ * This module provides SVD decomposition for matrices (both real and complex).
+ * Two decomposition algorithms are provided:
+ *  - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very
+ * slow for larger ones.
+ *  - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast
+ * for large problems. These decompositions are accessible via the respective classes and following MatrixBase methods:
+ *  - MatrixBase::jacobiSvd()
+ *  - MatrixBase::bdcSvd()
+ *
+ * \code
+ * #include <Eigen/SVD>
+ * \endcode
+ */
 
+// IWYU pragma: begin_exports
 #include "src/misc/RealSvd2x2.h"
 #include "src/SVD/UpperBidiagonalization.h"
 #include "src/SVD/SVDBase.h"
 #include "src/SVD/JacobiSVD.h"
 #include "src/SVD/BDCSVD.h"
-#if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT)
+#ifdef EIGEN_USE_LAPACKE
 #ifdef EIGEN_USE_MKL
 // #include "mkl_lapacke.h"
 #else
 // #include "src/misc/lapacke.h"
 #endif
+#ifndef EIGEN_USE_LAPACKE_STRICT
 // #include "src/SVD/JacobiSVD_LAPACKE.h"
 #endif
+// #include "src/SVD/BDCSVD_LAPACKE.h"
+#endif
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_SVD_MODULE_H
+#endif  // EIGEN_SVD_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky
index d2b1f12..6abdcd6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky
@@ -15,23 +15,26 @@
 
 #include "src/Core/util/DisableStupidWarnings.h"
 
-/** 
-  * \defgroup SparseCholesky_Module SparseCholesky module
-  *
-  * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices.
-  * Those decompositions are accessible via the following classes:
-  *  - SimplicialLLt,
-  *  - SimplicialLDLt
-  *
-  * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module.
-  *
-  * \code
-  * #include <Eigen/SparseCholesky>
-  * \endcode
-  */
+/**
+ * \defgroup SparseCholesky_Module SparseCholesky module
+ *
+ * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian)
+ * matrices. Those decompositions are accessible via the following classes:
+ *  - SimplicialLLt,
+ *  - SimplicialLDLt
+ *
+ * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module.
+ *
+ * \code
+ * #include <Eigen/SparseCholesky>
+ * \endcode
+ */
 
+// IWYU pragma: begin_exports
 #include "src/SparseCholesky/SimplicialCholesky.h"
 #include "src/SparseCholesky/SimplicialCholesky_impl.h"
+// IWYU pragma: end_exports
+
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_SPARSECHOLESKY_MODULE_H
+#endif  // EIGEN_SPARSECHOLESKY_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore
index 76966c4..56a9401 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore
@@ -17,22 +17,24 @@
 #include <cstdlib>
 #include <cstring>
 #include <algorithm>
+#include <numeric>
 
-/** 
-  * \defgroup SparseCore_Module SparseCore module
-  *
-  * This module provides a sparse matrix representation, and basic associated matrix manipulations
-  * and operations.
-  *
-  * See the \ref TutorialSparse "Sparse tutorial"
-  *
-  * \code
-  * #include <Eigen/SparseCore>
-  * \endcode
-  *
-  * This module depends on: Core.
-  */
+/**
+ * \defgroup SparseCore_Module SparseCore module
+ *
+ * This module provides a sparse matrix representation, and basic associated matrix manipulations
+ * and operations.
+ *
+ * See the \ref TutorialSparse "Sparse tutorial"
+ *
+ * \code
+ * #include <Eigen/SparseCore>
+ * \endcode
+ *
+ * This module depends on: Core.
+ */
 
+// IWYU pragma: begin_exports
 #include "src/SparseCore/SparseUtil.h"
 #include "src/SparseCore/SparseMatrixBase.h"
 #include "src/SparseCore/SparseAssign.h"
@@ -41,7 +43,6 @@
 #include "src/SparseCore/SparseCompressedBase.h"
 #include "src/SparseCore/SparseMatrix.h"
 #include "src/SparseCore/SparseMap.h"
-#include "src/SparseCore/MappedSparseMatrix.h"
 #include "src/SparseCore/SparseVector.h"
 #include "src/SparseCore/SparseRef.h"
 #include "src/SparseCore/SparseCwiseUnaryOp.h"
@@ -62,8 +63,8 @@
 #include "src/SparseCore/SparsePermutation.h"
 #include "src/SparseCore/SparseFuzzy.h"
 #include "src/SparseCore/SparseSolverBase.h"
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_SPARSECORE_MODULE_H
-
+#endif  // EIGEN_SPARSECORE_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU
index 37c4a5c..6faf130 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU
@@ -13,20 +13,19 @@
 
 #include "SparseCore"
 
-/** 
-  * \defgroup SparseLU_Module SparseLU module
-  * This module defines a supernodal factorization of general sparse matrices.
-  * The code is fully optimized for supernode-panel updates with specialized kernels.
-  * Please, see the documentation of the SparseLU class for more details.
-  */
+/**
+ * \defgroup SparseLU_Module SparseLU module
+ * This module defines a supernodal factorization of general sparse matrices.
+ * The code is fully optimized for supernode-panel updates with specialized kernels.
+ * Please, see the documentation of the SparseLU class for more details.
+ */
 
 // Ordering interface
 #include "OrderingMethods"
 
 #include "src/Core/util/DisableStupidWarnings.h"
 
-#include "src/SparseLU/SparseLU_gemm_kernel.h"
-
+// IWYU pragma: begin_exports
 #include "src/SparseLU/SparseLU_Structs.h"
 #include "src/SparseLU/SparseLU_SupernodalMatrix.h"
 #include "src/SparseLU/SparseLUImpl.h"
@@ -44,7 +43,8 @@
 #include "src/SparseLU/SparseLU_pruneL.h"
 #include "src/SparseLU/SparseLU_Utils.h"
 #include "src/SparseLU/SparseLU.h"
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
-#endif // EIGEN_SPARSELU_MODULE_H
+#endif  // EIGEN_SPARSELU_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR
index f5fc5fa..b4f1cad 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR
@@ -13,23 +13,25 @@
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup SparseQR_Module SparseQR module
-  * \brief Provides QR decomposition for sparse matrices
-  * 
-  * This module provides a simplicial version of the left-looking Sparse QR decomposition. 
-  * The columns of the input matrix should be reordered to limit the fill-in during the 
-  * decomposition. Built-in methods (COLAMD, AMD) or external  methods (METIS) can be used to this end.
-  * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 
-  * of built-in and external ordering methods.
-  * 
-  * \code
-  * #include <Eigen/SparseQR>
-  * \endcode
-  * 
-  * 
-  */
+ * \brief Provides QR decomposition for sparse matrices
+ *
+ * This module provides a simplicial version of the left-looking Sparse QR decomposition.
+ * The columns of the input matrix should be reordered to limit the fill-in during the
+ * decomposition. Built-in methods (COLAMD, AMD) or external  methods (METIS) can be used to this end.
+ * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list
+ * of built-in and external ordering methods.
+ *
+ * \code
+ * #include <Eigen/SparseQR>
+ * \endcode
+ *
+ *
+ */
 
+// IWYU pragma: begin_exports
 #include "src/SparseCore/SparseColEtree.h"
 #include "src/SparseQR/SparseQR.h"
+// IWYU pragma: end_exports
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/InternalHeaderCheck.h
new file mode 100644
index 0000000..5de2b21
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_CHOLESKY_MODULE_H
+#error "Please include Eigen/Cholesky instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h
index 1013ca0..5d52ab2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h
@@ -13,335 +13,314 @@
 #ifndef EIGEN_LDLT_H
 #define EIGEN_LDLT_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-  template<typename _MatrixType, int _UpLo> struct traits<LDLT<_MatrixType, _UpLo> >
-   : traits<_MatrixType>
-  {
-    typedef MatrixXpr XprKind;
-    typedef SolverStorage StorageKind;
-    typedef int StorageIndex;
-    enum { Flags = 0 };
-  };
+template <typename MatrixType_, int UpLo_>
+struct traits<LDLT<MatrixType_, UpLo_> > : traits<MatrixType_> {
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
+  enum { Flags = 0 };
+};
 
-  template<typename MatrixType, int UpLo> struct LDLT_Traits;
+template <typename MatrixType, int UpLo>
+struct LDLT_Traits;
 
-  // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
-  enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite };
-}
+// PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
+enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite };
+}  // namespace internal
 
 /** \ingroup Cholesky_Module
-  *
-  * \class LDLT
-  *
-  * \brief Robust Cholesky decomposition of a matrix with pivoting
-  *
-  * \tparam _MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition
-  * \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper.
-  *             The other triangular part won't be read.
-  *
-  * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite
-  * matrix \f$ A \f$ such that \f$ A =  P^TLDL^*P \f$, where P is a permutation matrix, L
-  * is lower triangular with a unit diagonal and D is a diagonal matrix.
-  *
-  * The decomposition uses pivoting to ensure stability, so that D will have
-  * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
-  * on D also stabilizes the computation.
-  *
-  * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky
-  * decomposition to determine whether a system of equations has a solution.
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  *
-  * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt(), class LLT
-  */
-template<typename _MatrixType, int _UpLo> class LDLT
-        : public SolverBase<LDLT<_MatrixType, _UpLo> >
-{
-  public:
-    typedef _MatrixType MatrixType;
-    typedef SolverBase<LDLT> Base;
-    friend class SolverBase<LDLT>;
+ *
+ * \class LDLT
+ *
+ * \brief Robust Cholesky decomposition of a matrix with pivoting
+ *
+ * \tparam MatrixType_ the type of the matrix of which to compute the LDL^T Cholesky decomposition
+ * \tparam UpLo_ the triangular part that will be used for the decomposition: Lower (default) or Upper.
+ *             The other triangular part won't be read.
+ *
+ * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite
+ * matrix \f$ A \f$ such that \f$ A =  P^TLDL^*P \f$, where P is a permutation matrix, L
+ * is lower triangular with a unit diagonal and D is a diagonal matrix.
+ *
+ * The decomposition uses pivoting to ensure stability, so that D will have
+ * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
+ * on D also stabilizes the computation.
+ *
+ * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky
+ * decomposition to determine whether a system of equations has a solution.
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt(), class LLT
+ */
+template <typename MatrixType_, int UpLo_>
+class LDLT : public SolverBase<LDLT<MatrixType_, UpLo_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef SolverBase<LDLT> Base;
+  friend class SolverBase<LDLT>;
 
-    EIGEN_GENERIC_PUBLIC_INTERFACE(LDLT)
-    enum {
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-      UpLo = _UpLo
-    };
-    typedef Matrix<Scalar, RowsAtCompileTime, 1, 0, MaxRowsAtCompileTime, 1> TmpMatrixType;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(LDLT)
+  enum {
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
+    UpLo = UpLo_
+  };
+  typedef Matrix<Scalar, RowsAtCompileTime, 1, 0, MaxRowsAtCompileTime, 1> TmpMatrixType;
 
-    typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
-    typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
+  typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
+  typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
 
-    typedef internal::LDLT_Traits<MatrixType,UpLo> Traits;
+  typedef internal::LDLT_Traits<MatrixType, UpLo> Traits;
 
-    /** \brief Default Constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via LDLT::compute(const MatrixType&).
-      */
-    LDLT()
-      : m_matrix(),
-        m_transpositions(),
-        m_sign(internal::ZeroSign),
-        m_isInitialized(false)
-    {}
+  /** \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via LDLT::compute(const MatrixType&).
+   */
+  LDLT() : m_matrix(), m_transpositions(), m_sign(internal::ZeroSign), m_isInitialized(false) {}
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa LDLT()
-      */
-    explicit LDLT(Index size)
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa LDLT()
+   */
+  explicit LDLT(Index size)
       : m_matrix(size, size),
         m_transpositions(size),
         m_temporary(size),
         m_sign(internal::ZeroSign),
-        m_isInitialized(false)
-    {}
+        m_isInitialized(false) {}
 
-    /** \brief Constructor with decomposition
-      *
-      * This calculates the decomposition for the input \a matrix.
-      *
-      * \sa LDLT(Index size)
-      */
-    template<typename InputType>
-    explicit LDLT(const EigenBase<InputType>& matrix)
+  /** \brief Constructor with decomposition
+   *
+   * This calculates the decomposition for the input \a matrix.
+   *
+   * \sa LDLT(Index size)
+   */
+  template <typename InputType>
+  explicit LDLT(const EigenBase<InputType>& matrix)
       : m_matrix(matrix.rows(), matrix.cols()),
         m_transpositions(matrix.rows()),
         m_temporary(matrix.rows()),
         m_sign(internal::ZeroSign),
-        m_isInitialized(false)
-    {
-      compute(matrix.derived());
-    }
+        m_isInitialized(false) {
+    compute(matrix.derived());
+  }
 
-    /** \brief Constructs a LDLT factorization from a given matrix
-      *
-      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref.
-      *
-      * \sa LDLT(const EigenBase&)
-      */
-    template<typename InputType>
-    explicit LDLT(EigenBase<InputType>& matrix)
+  /** \brief Constructs a LDLT factorization from a given matrix
+   *
+   * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c
+   * MatrixType is a Eigen::Ref.
+   *
+   * \sa LDLT(const EigenBase&)
+   */
+  template <typename InputType>
+  explicit LDLT(EigenBase<InputType>& matrix)
       : m_matrix(matrix.derived()),
         m_transpositions(matrix.rows()),
         m_temporary(matrix.rows()),
         m_sign(internal::ZeroSign),
-        m_isInitialized(false)
-    {
-      compute(matrix.derived());
-    }
+        m_isInitialized(false) {
+    compute(matrix.derived());
+  }
 
-    /** Clear any existing decomposition
-     * \sa rankUpdate(w,sigma)
-     */
-    void setZero()
-    {
-      m_isInitialized = false;
-    }
+  /** Clear any existing decomposition
+   * \sa rankUpdate(w,sigma)
+   */
+  void setZero() { m_isInitialized = false; }
 
-    /** \returns a view of the upper triangular matrix U */
-    inline typename Traits::MatrixU matrixU() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return Traits::getU(m_matrix);
-    }
+  /** \returns a view of the upper triangular matrix U */
+  inline typename Traits::MatrixU matrixU() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return Traits::getU(m_matrix);
+  }
 
-    /** \returns a view of the lower triangular matrix L */
-    inline typename Traits::MatrixL matrixL() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return Traits::getL(m_matrix);
-    }
+  /** \returns a view of the lower triangular matrix L */
+  inline typename Traits::MatrixL matrixL() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return Traits::getL(m_matrix);
+  }
 
-    /** \returns the permutation matrix P as a transposition sequence.
-      */
-    inline const TranspositionType& transpositionsP() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_transpositions;
-    }
+  /** \returns the permutation matrix P as a transposition sequence.
+   */
+  inline const TranspositionType& transpositionsP() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return m_transpositions;
+  }
 
-    /** \returns the coefficients of the diagonal matrix D */
-    inline Diagonal<const MatrixType> vectorD() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_matrix.diagonal();
-    }
+  /** \returns the coefficients of the diagonal matrix D */
+  inline Diagonal<const MatrixType> vectorD() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return m_matrix.diagonal();
+  }
 
-    /** \returns true if the matrix is positive (semidefinite) */
-    inline bool isPositive() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign;
-    }
+  /** \returns true if the matrix is positive (semidefinite) */
+  inline bool isPositive() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign;
+  }
 
-    /** \returns true if the matrix is negative (semidefinite) */
-    inline bool isNegative(void) const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
-    }
+  /** \returns true if the matrix is negative (semidefinite) */
+  inline bool isNegative(void) const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
-      *
-      * This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> .
-      *
-      * \note_about_checking_solutions
-      *
-      * More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$
-      * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$,
-      * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then
-      * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the
-      * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function
-      * computes the least-square solution of \f$ A x = b \f$ if \f$ A \f$ is singular.
-      *
-      * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt()
-      */
-    template<typename Rhs>
-    inline const Solve<LDLT, Rhs>
-    solve(const MatrixBase<Rhs>& b) const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
+   *
+   * This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> .
+   *
+   * \note_about_checking_solutions
+   *
+   * More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$
+   * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$,
+   * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then
+   * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the
+   * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function
+   * computes the least-square solution of \f$ A x = b \f$ if \f$ A \f$ is singular.
+   *
+   * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt()
+   */
+  template <typename Rhs>
+  inline const Solve<LDLT, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
-    template<typename Derived>
-    bool solveInPlace(MatrixBase<Derived> &bAndX) const;
+  template <typename Derived>
+  bool solveInPlace(MatrixBase<Derived>& bAndX) const;
 
-    template<typename InputType>
-    LDLT& compute(const EigenBase<InputType>& matrix);
+  template <typename InputType>
+  LDLT& compute(const EigenBase<InputType>& matrix);
 
-    /** \returns an estimate of the reciprocal condition number of the matrix of
-     *  which \c *this is the LDLT decomposition.
-     */
-    RealScalar rcond() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return internal::rcond_estimate_helper(m_l1_norm, *this);
-    }
+  /** \returns an estimate of the reciprocal condition number of the matrix of
+   *  which \c *this is the LDLT decomposition.
+   */
+  RealScalar rcond() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return internal::rcond_estimate_helper(m_l1_norm, *this);
+  }
 
-    template <typename Derived>
-    LDLT& rankUpdate(const MatrixBase<Derived>& w, const RealScalar& alpha=1);
+  template <typename Derived>
+  LDLT& rankUpdate(const MatrixBase<Derived>& w, const RealScalar& alpha = 1);
 
-    /** \returns the internal LDLT decomposition matrix
-      *
-      * TODO: document the storage layout
-      */
-    inline const MatrixType& matrixLDLT() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_matrix;
-    }
+  /** \returns the internal LDLT decomposition matrix
+   *
+   * TODO: document the storage layout
+   */
+  inline const MatrixType& matrixLDLT() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return m_matrix;
+  }
 
-    MatrixType reconstructedMatrix() const;
+  MatrixType reconstructedMatrix() const;
 
-    /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix is self-adjoint.
-      *
-      * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
-      * \code x = decomposition.adjoint().solve(b) \endcode
-      */
-    const LDLT& adjoint() const { return *this; };
+  /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix
+   * is self-adjoint.
+   *
+   * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
+   * \code x = decomposition.adjoint().solve(b) \endcode
+   */
+  const LDLT& adjoint() const { return *this; }
 
-    EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful,
-      *          \c NumericalIssue if the factorization failed because of a zero pivot.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_info;
-    }
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful,
+   *          \c NumericalIssue if the factorization failed because of a zero pivot.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "LDLT is not initialized.");
+    return m_info;
+  }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename RhsType, typename DstType>
-    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-    template<bool Conjugate, typename RhsType, typename DstType>
-    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
-    #endif
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
+#endif
 
-  protected:
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
-
-    /** \internal
-      * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U.
-      * The strict upper part is used during the decomposition, the strict lower
-      * part correspond to the coefficients of L (its diagonal is equal to 1 and
-      * is not stored), and the diagonal entries correspond to D.
-      */
-    MatrixType m_matrix;
-    RealScalar m_l1_norm;
-    TranspositionType m_transpositions;
-    TmpMatrixType m_temporary;
-    internal::SignMatrix m_sign;
-    bool m_isInitialized;
-    ComputationInfo m_info;
+  /** \internal
+   * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U.
+   * The strict upper part is used during the decomposition, the strict lower
+   * part correspond to the coefficients of L (its diagonal is equal to 1 and
+   * is not stored), and the diagonal entries correspond to D.
+   */
+  MatrixType m_matrix;
+  RealScalar m_l1_norm;
+  TranspositionType m_transpositions;
+  TmpMatrixType m_temporary;
+  internal::SignMatrix m_sign;
+  bool m_isInitialized;
+  ComputationInfo m_info;
 };
 
 namespace internal {
 
-template<int UpLo> struct ldlt_inplace;
+template <int UpLo>
+struct ldlt_inplace;
 
-template<> struct ldlt_inplace<Lower>
-{
-  template<typename MatrixType, typename TranspositionType, typename Workspace>
-  static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
-  {
+template <>
+struct ldlt_inplace<Lower> {
+  template <typename MatrixType, typename TranspositionType, typename Workspace>
+  static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) {
     using std::abs;
     typedef typename MatrixType::Scalar Scalar;
     typedef typename MatrixType::RealScalar RealScalar;
     typedef typename TranspositionType::StorageIndex IndexType;
-    eigen_assert(mat.rows()==mat.cols());
+    eigen_assert(mat.rows() == mat.cols());
     const Index size = mat.rows();
     bool found_zero_pivot = false;
     bool ret = true;
 
-    if (size <= 1)
-    {
+    if (size <= 1) {
       transpositions.setIdentity();
-      if(size==0) sign = ZeroSign;
-      else if (numext::real(mat.coeff(0,0)) > static_cast<RealScalar>(0) ) sign = PositiveSemiDef;
-      else if (numext::real(mat.coeff(0,0)) < static_cast<RealScalar>(0)) sign = NegativeSemiDef;
-      else sign = ZeroSign;
+      if (size == 0)
+        sign = ZeroSign;
+      else if (numext::real(mat.coeff(0, 0)) > static_cast<RealScalar>(0))
+        sign = PositiveSemiDef;
+      else if (numext::real(mat.coeff(0, 0)) < static_cast<RealScalar>(0))
+        sign = NegativeSemiDef;
+      else
+        sign = ZeroSign;
       return true;
     }
 
-    for (Index k = 0; k < size; ++k)
-    {
+    for (Index k = 0; k < size; ++k) {
       // Find largest diagonal element
       Index index_of_biggest_in_corner;
-      mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
+      mat.diagonal().tail(size - k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
       index_of_biggest_in_corner += k;
 
       transpositions.coeffRef(k) = IndexType(index_of_biggest_in_corner);
-      if(k != index_of_biggest_in_corner)
-      {
+      if (k != index_of_biggest_in_corner) {
         // apply the transposition while taking care to consider only
         // the lower triangular part
-        Index s = size-index_of_biggest_in_corner-1; // trailing size after the biggest element
+        Index s = size - index_of_biggest_in_corner - 1;  // trailing size after the biggest element
         mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k));
         mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s));
-        std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner));
-        for(Index i=k+1;i<index_of_biggest_in_corner;++i)
-        {
-          Scalar tmp = mat.coeffRef(i,k);
-          mat.coeffRef(i,k) = numext::conj(mat.coeffRef(index_of_biggest_in_corner,i));
-          mat.coeffRef(index_of_biggest_in_corner,i) = numext::conj(tmp);
+        std::swap(mat.coeffRef(k, k), mat.coeffRef(index_of_biggest_in_corner, index_of_biggest_in_corner));
+        for (Index i = k + 1; i < index_of_biggest_in_corner; ++i) {
+          Scalar tmp = mat.coeffRef(i, k);
+          mat.coeffRef(i, k) = numext::conj(mat.coeffRef(index_of_biggest_in_corner, i));
+          mat.coeffRef(index_of_biggest_in_corner, i) = numext::conj(tmp);
         }
-        if(NumTraits<Scalar>::IsComplex)
-          mat.coeffRef(index_of_biggest_in_corner,k) = numext::conj(mat.coeff(index_of_biggest_in_corner,k));
+        if (NumTraits<Scalar>::IsComplex)
+          mat.coeffRef(index_of_biggest_in_corner, k) = numext::conj(mat.coeff(index_of_biggest_in_corner, k));
       }
 
       // partition the matrix:
@@ -349,53 +328,53 @@
       // lu  = A10 | A11 |  -
       //       A20 | A21 | A22
       Index rs = size - k - 1;
-      Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
-      Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
-      Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
+      Block<MatrixType, Dynamic, 1> A21(mat, k + 1, k, rs, 1);
+      Block<MatrixType, 1, Dynamic> A10(mat, k, 0, 1, k);
+      Block<MatrixType, Dynamic, Dynamic> A20(mat, k + 1, 0, rs, k);
 
-      if(k>0)
-      {
+      if (k > 0) {
         temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint();
-        mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
-        if(rs>0)
-          A21.noalias() -= A20 * temp.head(k);
+        mat.coeffRef(k, k) -= (A10 * temp.head(k)).value();
+        if (rs > 0) A21.noalias() -= A20 * temp.head(k);
       }
 
       // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot
       // was smaller than the cutoff value. However, since LDLT is not rank-revealing
       // we should only make sure that we do not introduce INF or NaN values.
       // Remark that LAPACK also uses 0 as the cutoff value.
-      RealScalar realAkk = numext::real(mat.coeffRef(k,k));
+      RealScalar realAkk = numext::real(mat.coeffRef(k, k));
       bool pivot_is_valid = (abs(realAkk) > RealScalar(0));
 
-      if(k==0 && !pivot_is_valid)
-      {
+      if (k == 0 && !pivot_is_valid) {
         // The entire diagonal is zero, there is nothing more to do
         // except filling the transpositions, and checking whether the matrix is zero.
         sign = ZeroSign;
-        for(Index j = 0; j<size; ++j)
-        {
+        for (Index j = 0; j < size; ++j) {
           transpositions.coeffRef(j) = IndexType(j);
-          ret = ret && (mat.col(j).tail(size-j-1).array()==Scalar(0)).all();
+          ret = ret && (mat.col(j).tail(size - j - 1).array() == Scalar(0)).all();
         }
         return ret;
       }
 
-      if((rs>0) && pivot_is_valid)
+      if ((rs > 0) && pivot_is_valid)
         A21 /= realAkk;
-      else if(rs>0)
-        ret = ret && (A21.array()==Scalar(0)).all();
+      else if (rs > 0)
+        ret = ret && (A21.array() == Scalar(0)).all();
 
-      if(found_zero_pivot && pivot_is_valid) ret = false; // factorization failed
-      else if(!pivot_is_valid) found_zero_pivot = true;
+      if (found_zero_pivot && pivot_is_valid)
+        ret = false;  // factorization failed
+      else if (!pivot_is_valid)
+        found_zero_pivot = true;
 
       if (sign == PositiveSemiDef) {
         if (realAkk < static_cast<RealScalar>(0)) sign = Indefinite;
       } else if (sign == NegativeSemiDef) {
         if (realAkk > static_cast<RealScalar>(0)) sign = Indefinite;
       } else if (sign == ZeroSign) {
-        if (realAkk > static_cast<RealScalar>(0)) sign = PositiveSemiDef;
-        else if (realAkk < static_cast<RealScalar>(0)) sign = NegativeSemiDef;
+        if (realAkk > static_cast<RealScalar>(0))
+          sign = PositiveSemiDef;
+        else if (realAkk < static_cast<RealScalar>(0))
+          sign = NegativeSemiDef;
       }
     }
 
@@ -409,98 +388,91 @@
   // original matrix is not of full rank.
   // Here only rank-1 updates are implemented, to reduce the
   // requirement for intermediate storage and improve accuracy
-  template<typename MatrixType, typename WDerived>
-  static bool updateInPlace(MatrixType& mat, MatrixBase<WDerived>& w, const typename MatrixType::RealScalar& sigma=1)
-  {
+  template <typename MatrixType, typename WDerived>
+  static bool updateInPlace(MatrixType& mat, MatrixBase<WDerived>& w,
+                            const typename MatrixType::RealScalar& sigma = 1) {
     using numext::isfinite;
     typedef typename MatrixType::Scalar Scalar;
     typedef typename MatrixType::RealScalar RealScalar;
 
     const Index size = mat.rows();
-    eigen_assert(mat.cols() == size && w.size()==size);
+    eigen_assert(mat.cols() == size && w.size() == size);
 
     RealScalar alpha = 1;
 
     // Apply the update
-    for (Index j = 0; j < size; j++)
-    {
+    for (Index j = 0; j < size; j++) {
       // Check for termination due to an original decomposition of low-rank
-      if (!(isfinite)(alpha))
-        break;
+      if (!(isfinite)(alpha)) break;
 
       // Update the diagonal terms
-      RealScalar dj = numext::real(mat.coeff(j,j));
+      RealScalar dj = numext::real(mat.coeff(j, j));
       Scalar wj = w.coeff(j);
-      RealScalar swj2 = sigma*numext::abs2(wj);
-      RealScalar gamma = dj*alpha + swj2;
+      RealScalar swj2 = sigma * numext::abs2(wj);
+      RealScalar gamma = dj * alpha + swj2;
 
-      mat.coeffRef(j,j) += swj2/alpha;
-      alpha += swj2/dj;
-
+      mat.coeffRef(j, j) += swj2 / alpha;
+      alpha += swj2 / dj;
 
       // Update the terms of L
-      Index rs = size-j-1;
+      Index rs = size - j - 1;
       w.tail(rs) -= wj * mat.col(j).tail(rs);
-      if(gamma != 0)
-        mat.col(j).tail(rs) += (sigma*numext::conj(wj)/gamma)*w.tail(rs);
+      if (!numext::is_exactly_zero(gamma)) mat.col(j).tail(rs) += (sigma * numext::conj(wj) / gamma) * w.tail(rs);
     }
     return true;
   }
 
-  template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
-  static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, const typename MatrixType::RealScalar& sigma=1)
-  {
+  template <typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
+  static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w,
+                     const typename MatrixType::RealScalar& sigma = 1) {
     // Apply the permutation to the input w
     tmp = transpositions * w;
 
-    return ldlt_inplace<Lower>::updateInPlace(mat,tmp,sigma);
+    return ldlt_inplace<Lower>::updateInPlace(mat, tmp, sigma);
   }
 };
 
-template<> struct ldlt_inplace<Upper>
-{
-  template<typename MatrixType, typename TranspositionType, typename Workspace>
-  static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
-  {
+template <>
+struct ldlt_inplace<Upper> {
+  template <typename MatrixType, typename TranspositionType, typename Workspace>
+  static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp,
+                                            SignMatrix& sign) {
     Transpose<MatrixType> matt(mat);
     return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
   }
 
-  template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
-  static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, const typename MatrixType::RealScalar& sigma=1)
-  {
+  template <typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
+  static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w,
+                                         const typename MatrixType::RealScalar& sigma = 1) {
     Transpose<MatrixType> matt(mat);
     return ldlt_inplace<Lower>::update(matt, transpositions, tmp, w.conjugate(), sigma);
   }
 };
 
-template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower>
-{
+template <typename MatrixType>
+struct LDLT_Traits<MatrixType, Lower> {
   typedef const TriangularView<const MatrixType, UnitLower> MatrixL;
   typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitUpper> MatrixU;
   static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); }
   static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); }
 };
 
-template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
-{
+template <typename MatrixType>
+struct LDLT_Traits<MatrixType, Upper> {
   typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitLower> MatrixL;
   typedef const TriangularView<const MatrixType, UnitUpper> MatrixU;
   static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); }
   static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix
-  */
-template<typename MatrixType, int _UpLo>
-template<typename InputType>
-LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const EigenBase<InputType>& a)
-{
-  check_template_parameters();
-
-  eigen_assert(a.rows()==a.cols());
+ */
+template <typename MatrixType, int UpLo_>
+template <typename InputType>
+LDLT<MatrixType, UpLo_>& LDLT<MatrixType, UpLo_>::compute(const EigenBase<InputType>& a) {
+  eigen_assert(a.rows() == a.cols());
   const Index size = a.rows();
 
   m_matrix = a.derived();
@@ -510,12 +482,13 @@
   // TODO move this code to SelfAdjointView
   for (Index col = 0; col < size; ++col) {
     RealScalar abs_col_sum;
-    if (_UpLo == Lower)
-      abs_col_sum = m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>();
+    if (UpLo_ == Lower)
+      abs_col_sum =
+          m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>();
     else
-      abs_col_sum = m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>();
-    if (abs_col_sum > m_l1_norm)
-      m_l1_norm = abs_col_sum;
+      abs_col_sum =
+          m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>();
+    if (abs_col_sum > m_l1_norm) m_l1_norm = abs_col_sum;
   }
 
   m_transpositions.resize(size);
@@ -523,7 +496,8 @@
   m_temporary.resize(size);
   m_sign = internal::ZeroSign;
 
-  m_info = internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign) ? Success : NumericalIssue;
+  m_info = internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign) ? Success
+                                                                                                    : NumericalIssue;
 
   m_isInitialized = true;
   return *this;
@@ -531,28 +505,24 @@
 
 /** Update the LDLT decomposition:  given A = L D L^T, efficiently compute the decomposition of A + sigma w w^T.
  * \param w a vector to be incorporated into the decomposition.
- * \param sigma a scalar, +1 for updates and -1 for "downdates," which correspond to removing previously-added column vectors. Optional; default value is +1.
- * \sa setZero()
-  */
-template<typename MatrixType, int _UpLo>
-template<typename Derived>
-LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Derived>& w, const typename LDLT<MatrixType,_UpLo>::RealScalar& sigma)
-{
+ * \param sigma a scalar, +1 for updates and -1 for "downdates," which correspond to removing previously-added column
+ * vectors. Optional; default value is +1. \sa setZero()
+ */
+template <typename MatrixType, int UpLo_>
+template <typename Derived>
+LDLT<MatrixType, UpLo_>& LDLT<MatrixType, UpLo_>::rankUpdate(
+    const MatrixBase<Derived>& w, const typename LDLT<MatrixType, UpLo_>::RealScalar& sigma) {
   typedef typename TranspositionType::StorageIndex IndexType;
   const Index size = w.rows();
-  if (m_isInitialized)
-  {
-    eigen_assert(m_matrix.rows()==size);
-  }
-  else
-  {
-    m_matrix.resize(size,size);
+  if (m_isInitialized) {
+    eigen_assert(m_matrix.rows() == size);
+  } else {
+    m_matrix.resize(size, size);
     m_matrix.setZero();
     m_transpositions.resize(size);
-    for (Index i = 0; i < size; i++)
-      m_transpositions.coeffRef(i) = IndexType(i);
+    for (Index i = 0; i < size; i++) m_transpositions.coeffRef(i) = IndexType(i);
     m_temporary.resize(size);
-    m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef;
+    m_sign = sigma >= 0 ? internal::PositiveSemiDef : internal::NegativeSemiDef;
     m_isInitialized = true;
   }
 
@@ -562,17 +532,15 @@
 }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename _MatrixType, int _UpLo>
-template<typename RhsType, typename DstType>
-void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, int UpLo_>
+template <typename RhsType, typename DstType>
+void LDLT<MatrixType_, UpLo_>::_solve_impl(const RhsType& rhs, DstType& dst) const {
   _solve_impl_transposed<true>(rhs, dst);
 }
 
-template<typename _MatrixType,int _UpLo>
-template<bool Conjugate, typename RhsType, typename DstType>
-void LDLT<_MatrixType,_UpLo>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, int UpLo_>
+template <bool Conjugate, typename RhsType, typename DstType>
+void LDLT<MatrixType_, UpLo_>::_solve_impl_transposed(const RhsType& rhs, DstType& dst) const {
   // dst = P b
   dst = m_transpositions * rhs;
 
@@ -587,15 +555,13 @@
   const typename Diagonal<const MatrixType>::RealReturnType vecD(vectorD());
   // In some previous versions, tolerance was set to the max of 1/highest (or rather numeric_limits::min())
   // and the maximal diagonal entry * epsilon as motivated by LAPACK's xGELSS:
-  // RealScalar tolerance = numext::maxi(vecD.array().abs().maxCoeff() * NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
-  // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
-  // diagonal element is not well justified and leads to numerical issues in some cases.
-  // Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
-  // Using numeric_limits::min() gives us more robustness to denormals.
+  // RealScalar tolerance = numext::maxi(vecD.array().abs().maxCoeff() * NumTraits<RealScalar>::epsilon(),RealScalar(1)
+  // / NumTraits<RealScalar>::highest()); However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the
+  // highest diagonal element is not well justified and leads to numerical issues in some cases. Moreover, Lapack's
+  // xSYTRS routines use 0 for the tolerance. Using numeric_limits::min() gives us more robustness to denormals.
   RealScalar tolerance = (std::numeric_limits<RealScalar>::min)();
-  for (Index i = 0; i < vecD.size(); ++i)
-  {
-    if(abs(vecD(i)) > tolerance)
+  for (Index i = 0; i < vecD.size(); ++i) {
+    if (abs(vecD(i)) > tolerance)
       dst.row(i) /= vecD(i);
     else
       dst.row(i).setZero();
@@ -612,22 +578,21 @@
 #endif
 
 /** \internal use x = ldlt_object.solve(x);
-  *
-  * This is the \em in-place version of solve().
-  *
-  * \param bAndX represents both the right-hand side matrix b and result x.
-  *
-  * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
-  *
-  * This version avoids a copy when the right hand side matrix b is not
-  * needed anymore.
-  *
-  * \sa LDLT::solve(), MatrixBase::ldlt()
-  */
-template<typename MatrixType,int _UpLo>
-template<typename Derived>
-bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
-{
+ *
+ * This is the \em in-place version of solve().
+ *
+ * \param bAndX represents both the right-hand side matrix b and result x.
+ *
+ * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
+ *
+ * This version avoids a copy when the right hand side matrix b is not
+ * needed anymore.
+ *
+ * \sa LDLT::solve(), MatrixBase::ldlt()
+ */
+template <typename MatrixType, int UpLo_>
+template <typename Derived>
+bool LDLT<MatrixType, UpLo_>::solveInPlace(MatrixBase<Derived>& bAndX) const {
   eigen_assert(m_isInitialized && "LDLT is not initialized.");
   eigen_assert(m_matrix.rows() == bAndX.rows());
 
@@ -639,12 +604,11 @@
 /** \returns the matrix represented by the decomposition,
  * i.e., it returns the product: P^T L D L^* P.
  * This function is provided for debug purpose. */
-template<typename MatrixType, int _UpLo>
-MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
-{
+template <typename MatrixType, int UpLo_>
+MatrixType LDLT<MatrixType, UpLo_>::reconstructedMatrix() const {
   eigen_assert(m_isInitialized && "LDLT is not initialized.");
   const Index size = m_matrix.rows();
-  MatrixType res(size,size);
+  MatrixType res(size, size);
 
   // P
   res.setIdentity();
@@ -662,27 +626,24 @@
 }
 
 /** \cholesky_module
-  * \returns the Cholesky decomposition with full pivoting without square root of \c *this
-  * \sa MatrixBase::ldlt()
-  */
-template<typename MatrixType, unsigned int UpLo>
+ * \returns the Cholesky decomposition with full pivoting without square root of \c *this
+ * \sa MatrixBase::ldlt()
+ */
+template <typename MatrixType, unsigned int UpLo>
 inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
-SelfAdjointView<MatrixType, UpLo>::ldlt() const
-{
-  return LDLT<PlainObject,UpLo>(m_matrix);
+SelfAdjointView<MatrixType, UpLo>::ldlt() const {
+  return LDLT<PlainObject, UpLo>(m_matrix);
 }
 
 /** \cholesky_module
-  * \returns the Cholesky decomposition with full pivoting without square root of \c *this
-  * \sa SelfAdjointView::ldlt()
-  */
-template<typename Derived>
-inline const LDLT<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::ldlt() const
-{
+ * \returns the Cholesky decomposition with full pivoting without square root of \c *this
+ * \sa SelfAdjointView::ldlt()
+ */
+template <typename Derived>
+inline const LDLT<typename MatrixBase<Derived>::PlainObject> MatrixBase<Derived>::ldlt() const {
   return LDLT<PlainObject>(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_LDLT_H
+#endif  // EIGEN_LDLT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h
index 8c9b2b3..01b4476 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h
@@ -10,446 +10,410 @@
 #ifndef EIGEN_LLT_H
 #define EIGEN_LLT_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-namespace internal{
+namespace internal {
 
-template<typename _MatrixType, int _UpLo> struct traits<LLT<_MatrixType, _UpLo> >
- : traits<_MatrixType>
-{
+template <typename MatrixType_, int UpLo_>
+struct traits<LLT<MatrixType_, UpLo_> > : traits<MatrixType_> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
   typedef int StorageIndex;
   enum { Flags = 0 };
 };
 
-template<typename MatrixType, int UpLo> struct LLT_Traits;
-}
+template <typename MatrixType, int UpLo>
+struct LLT_Traits;
+}  // namespace internal
 
 /** \ingroup Cholesky_Module
-  *
-  * \class LLT
-  *
-  * \brief Standard Cholesky decomposition (LL^T) of a matrix and associated features
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition
-  * \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper.
-  *               The other triangular part won't be read.
-  *
-  * This class performs a LL^T Cholesky decomposition of a symmetric, positive definite
-  * matrix A such that A = LL^* = U^*U, where L is lower triangular.
-  *
-  * While the Cholesky decomposition is particularly useful to solve selfadjoint problems like  D^*D x = b,
-  * for that purpose, we recommend the Cholesky decomposition without square root which is more stable
-  * and even faster. Nevertheless, this standard Cholesky decomposition remains useful in many other
-  * situations like generalised eigen problems with hermitian matrices.
-  *
-  * Remember that Cholesky decompositions are not rank-revealing. This LLT decomposition is only stable on positive definite matrices,
-  * use LDLT instead for the semidefinite case. Also, do not use a Cholesky decomposition to determine whether a system of equations
-  * has a solution.
-  *
-  * Example: \include LLT_example.cpp
-  * Output: \verbinclude LLT_example.out
-  *
-  * \b Performance: for best performance, it is recommended to use a column-major storage format
-  * with the Lower triangular part (the default), or, equivalently, a row-major storage format
-  * with the Upper triangular part. Otherwise, you might get a 20% slowdown for the full factorization
-  * step, and rank-updates can be up to 3 times slower.
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  *
-  * Note that during the decomposition, only the lower (or upper, as defined by _UpLo) triangular part of A is considered.
-  * Therefore, the strict lower part does not have to store correct values.
-  *
-  * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT
-  */
-template<typename _MatrixType, int _UpLo> class LLT
-        : public SolverBase<LLT<_MatrixType, _UpLo> >
-{
-  public:
-    typedef _MatrixType MatrixType;
-    typedef SolverBase<LLT> Base;
-    friend class SolverBase<LLT>;
+ *
+ * \class LLT
+ *
+ * \brief Standard Cholesky decomposition (LL^T) of a matrix and associated features
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the LL^T Cholesky decomposition
+ * \tparam UpLo_ the triangular part that will be used for the decomposition: Lower (default) or Upper.
+ *               The other triangular part won't be read.
+ *
+ * This class performs a LL^T Cholesky decomposition of a symmetric, positive definite
+ * matrix A such that A = LL^* = U^*U, where L is lower triangular.
+ *
+ * While the Cholesky decomposition is particularly useful to solve selfadjoint problems like  D^*D x = b,
+ * for that purpose, we recommend the Cholesky decomposition without square root which is more stable
+ * and even faster. Nevertheless, this standard Cholesky decomposition remains useful in many other
+ * situations like generalised eigen problems with hermitian matrices.
+ *
+ * Remember that Cholesky decompositions are not rank-revealing. This LLT decomposition is only stable on positive
+ * definite matrices, use LDLT instead for the semidefinite case. Also, do not use a Cholesky decomposition to determine
+ * whether a system of equations has a solution.
+ *
+ * Example: \include LLT_example.cpp
+ * Output: \verbinclude LLT_example.out
+ *
+ * \b Performance: for best performance, it is recommended to use a column-major storage format
+ * with the Lower triangular part (the default), or, equivalently, a row-major storage format
+ * with the Upper triangular part. Otherwise, you might get a 20% slowdown for the full factorization
+ * step, and rank-updates can be up to 3 times slower.
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * Note that during the decomposition, only the lower (or upper, as defined by UpLo_) triangular part of A is
+ * considered. Therefore, the strict lower part does not have to store correct values.
+ *
+ * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT
+ */
+template <typename MatrixType_, int UpLo_>
+class LLT : public SolverBase<LLT<MatrixType_, UpLo_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef SolverBase<LLT> Base;
+  friend class SolverBase<LLT>;
 
-    EIGEN_GENERIC_PUBLIC_INTERFACE(LLT)
-    enum {
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
+  EIGEN_GENERIC_PUBLIC_INTERFACE(LLT)
+  enum { MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime };
 
-    enum {
-      PacketSize = internal::packet_traits<Scalar>::size,
-      AlignmentMask = int(PacketSize)-1,
-      UpLo = _UpLo
-    };
+  enum { PacketSize = internal::packet_traits<Scalar>::size, AlignmentMask = int(PacketSize) - 1, UpLo = UpLo_ };
 
-    typedef internal::LLT_Traits<MatrixType,UpLo> Traits;
+  typedef internal::LLT_Traits<MatrixType, UpLo> Traits;
 
-    /**
-      * \brief Default Constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via LLT::compute(const MatrixType&).
-      */
-    LLT() : m_matrix(), m_isInitialized(false) {}
+  /**
+   * \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via LLT::compute(const MatrixType&).
+   */
+  LLT() : m_matrix(), m_isInitialized(false) {}
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa LLT()
-      */
-    explicit LLT(Index size) : m_matrix(size, size),
-                    m_isInitialized(false) {}
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa LLT()
+   */
+  explicit LLT(Index size) : m_matrix(size, size), m_isInitialized(false) {}
 
-    template<typename InputType>
-    explicit LLT(const EigenBase<InputType>& matrix)
-      : m_matrix(matrix.rows(), matrix.cols()),
-        m_isInitialized(false)
-    {
-      compute(matrix.derived());
-    }
+  template <typename InputType>
+  explicit LLT(const EigenBase<InputType>& matrix) : m_matrix(matrix.rows(), matrix.cols()), m_isInitialized(false) {
+    compute(matrix.derived());
+  }
 
-    /** \brief Constructs a LLT factorization from a given matrix
-      *
-      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
-      * \c MatrixType is a Eigen::Ref.
-      *
-      * \sa LLT(const EigenBase&)
-      */
-    template<typename InputType>
-    explicit LLT(EigenBase<InputType>& matrix)
-      : m_matrix(matrix.derived()),
-        m_isInitialized(false)
-    {
-      compute(matrix.derived());
-    }
+  /** \brief Constructs a LLT factorization from a given matrix
+   *
+   * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
+   * \c MatrixType is a Eigen::Ref.
+   *
+   * \sa LLT(const EigenBase&)
+   */
+  template <typename InputType>
+  explicit LLT(EigenBase<InputType>& matrix) : m_matrix(matrix.derived()), m_isInitialized(false) {
+    compute(matrix.derived());
+  }
 
-    /** \returns a view of the upper triangular matrix U */
-    inline typename Traits::MatrixU matrixU() const
-    {
-      eigen_assert(m_isInitialized && "LLT is not initialized.");
-      return Traits::getU(m_matrix);
-    }
+  /** \returns a view of the upper triangular matrix U */
+  inline typename Traits::MatrixU matrixU() const {
+    eigen_assert(m_isInitialized && "LLT is not initialized.");
+    return Traits::getU(m_matrix);
+  }
 
-    /** \returns a view of the lower triangular matrix L */
-    inline typename Traits::MatrixL matrixL() const
-    {
-      eigen_assert(m_isInitialized && "LLT is not initialized.");
-      return Traits::getL(m_matrix);
-    }
+  /** \returns a view of the lower triangular matrix L */
+  inline typename Traits::MatrixL matrixL() const {
+    eigen_assert(m_isInitialized && "LLT is not initialized.");
+    return Traits::getL(m_matrix);
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
-      *
-      * Since this LLT class assumes anyway that the matrix A is invertible, the solution
-      * theoretically exists and is unique regardless of b.
-      *
-      * Example: \include LLT_solve.cpp
-      * Output: \verbinclude LLT_solve.out
-      *
-      * \sa solveInPlace(), MatrixBase::llt(), SelfAdjointView::llt()
-      */
-    template<typename Rhs>
-    inline const Solve<LLT, Rhs>
-    solve(const MatrixBase<Rhs>& b) const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
+   *
+   * Since this LLT class assumes anyway that the matrix A is invertible, the solution
+   * theoretically exists and is unique regardless of b.
+   *
+   * Example: \include LLT_solve.cpp
+   * Output: \verbinclude LLT_solve.out
+   *
+   * \sa solveInPlace(), MatrixBase::llt(), SelfAdjointView::llt()
+   */
+  template <typename Rhs>
+  inline const Solve<LLT, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
-    template<typename Derived>
-    void solveInPlace(const MatrixBase<Derived> &bAndX) const;
+  template <typename Derived>
+  void solveInPlace(const MatrixBase<Derived>& bAndX) const;
 
-    template<typename InputType>
-    LLT& compute(const EigenBase<InputType>& matrix);
+  template <typename InputType>
+  LLT& compute(const EigenBase<InputType>& matrix);
 
-    /** \returns an estimate of the reciprocal condition number of the matrix of
-      *  which \c *this is the Cholesky decomposition.
-      */
-    RealScalar rcond() const
-    {
-      eigen_assert(m_isInitialized && "LLT is not initialized.");
-      eigen_assert(m_info == Success && "LLT failed because matrix appears to be negative");
-      return internal::rcond_estimate_helper(m_l1_norm, *this);
-    }
+  /** \returns an estimate of the reciprocal condition number of the matrix of
+   *  which \c *this is the Cholesky decomposition.
+   */
+  RealScalar rcond() const {
+    eigen_assert(m_isInitialized && "LLT is not initialized.");
+    eigen_assert(m_info == Success && "LLT failed because matrix appears to be negative");
+    return internal::rcond_estimate_helper(m_l1_norm, *this);
+  }
 
-    /** \returns the LLT decomposition matrix
-      *
-      * TODO: document the storage layout
-      */
-    inline const MatrixType& matrixLLT() const
-    {
-      eigen_assert(m_isInitialized && "LLT is not initialized.");
-      return m_matrix;
-    }
+  /** \returns the LLT decomposition matrix
+   *
+   * TODO: document the storage layout
+   */
+  inline const MatrixType& matrixLLT() const {
+    eigen_assert(m_isInitialized && "LLT is not initialized.");
+    return m_matrix;
+  }
 
-    MatrixType reconstructedMatrix() const;
+  MatrixType reconstructedMatrix() const;
 
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful,
+   *          \c NumericalIssue if the matrix.appears not to be positive definite.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "LLT is not initialized.");
+    return m_info;
+  }
 
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful,
-      *          \c NumericalIssue if the matrix.appears not to be positive definite.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "LLT is not initialized.");
-      return m_info;
-    }
+  /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix
+   * is self-adjoint.
+   *
+   * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
+   * \code x = decomposition.adjoint().solve(b) \endcode
+   */
+  const LLT& adjoint() const EIGEN_NOEXCEPT { return *this; }
 
-    /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix is self-adjoint.
-      *
-      * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
-      * \code x = decomposition.adjoint().solve(b) \endcode
-      */
-    const LLT& adjoint() const EIGEN_NOEXCEPT { return *this; };
+  inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
-    inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  template <typename VectorType>
+  LLT& rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
 
-    template<typename VectorType>
-    LLT & rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename RhsType, typename DstType>
-    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
+#endif
 
-    template<bool Conjugate, typename RhsType, typename DstType>
-    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
-    #endif
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-  protected:
-
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
-
-    /** \internal
-      * Used to compute and store L
-      * The strict upper part is not used and even not initialized.
-      */
-    MatrixType m_matrix;
-    RealScalar m_l1_norm;
-    bool m_isInitialized;
-    ComputationInfo m_info;
+  /** \internal
+   * Used to compute and store L
+   * The strict upper part is not used and even not initialized.
+   */
+  MatrixType m_matrix;
+  RealScalar m_l1_norm;
+  bool m_isInitialized;
+  ComputationInfo m_info;
 };
 
 namespace internal {
 
-template<typename Scalar, int UpLo> struct llt_inplace;
+template <typename Scalar, int UpLo>
+struct llt_inplace;
 
-template<typename MatrixType, typename VectorType>
-static Index llt_rank_update_lower(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma)
-{
+template <typename MatrixType, typename VectorType>
+static Index llt_rank_update_lower(MatrixType& mat, const VectorType& vec,
+                                   const typename MatrixType::RealScalar& sigma) {
   using std::sqrt;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   typedef typename MatrixType::ColXpr ColXpr;
-  typedef typename internal::remove_all<ColXpr>::type ColXprCleaned;
+  typedef internal::remove_all_t<ColXpr> ColXprCleaned;
   typedef typename ColXprCleaned::SegmentReturnType ColXprSegment;
-  typedef Matrix<Scalar,Dynamic,1> TempVectorType;
+  typedef Matrix<Scalar, Dynamic, 1> TempVectorType;
   typedef typename TempVectorType::SegmentReturnType TempVecSegment;
 
   Index n = mat.cols();
-  eigen_assert(mat.rows()==n && vec.size()==n);
+  eigen_assert(mat.rows() == n && vec.size() == n);
 
   TempVectorType temp;
 
-  if(sigma>0)
-  {
+  if (sigma > 0) {
     // This version is based on Givens rotations.
     // It is faster than the other one below, but only works for updates,
     // i.e., for sigma > 0
     temp = sqrt(sigma) * vec;
 
-    for(Index i=0; i<n; ++i)
-    {
+    for (Index i = 0; i < n; ++i) {
       JacobiRotation<Scalar> g;
-      g.makeGivens(mat(i,i), -temp(i), &mat(i,i));
+      g.makeGivens(mat(i, i), -temp(i), &mat(i, i));
 
-      Index rs = n-i-1;
-      if(rs>0)
-      {
+      Index rs = n - i - 1;
+      if (rs > 0) {
         ColXprSegment x(mat.col(i).tail(rs));
         TempVecSegment y(temp.tail(rs));
         apply_rotation_in_the_plane(x, y, g);
       }
     }
-  }
-  else
-  {
+  } else {
     temp = vec;
     RealScalar beta = 1;
-    for(Index j=0; j<n; ++j)
-    {
-      RealScalar Ljj = numext::real(mat.coeff(j,j));
+    for (Index j = 0; j < n; ++j) {
+      RealScalar Ljj = numext::real(mat.coeff(j, j));
       RealScalar dj = numext::abs2(Ljj);
       Scalar wj = temp.coeff(j);
-      RealScalar swj2 = sigma*numext::abs2(wj);
-      RealScalar gamma = dj*beta + swj2;
+      RealScalar swj2 = sigma * numext::abs2(wj);
+      RealScalar gamma = dj * beta + swj2;
 
-      RealScalar x = dj + swj2/beta;
-      if (x<=RealScalar(0))
-        return j;
+      RealScalar x = dj + swj2 / beta;
+      if (x <= RealScalar(0)) return j;
       RealScalar nLjj = sqrt(x);
-      mat.coeffRef(j,j) = nLjj;
-      beta += swj2/dj;
+      mat.coeffRef(j, j) = nLjj;
+      beta += swj2 / dj;
 
       // Update the terms of L
-      Index rs = n-j-1;
-      if(rs)
-      {
-        temp.tail(rs) -= (wj/Ljj) * mat.col(j).tail(rs);
-        if(gamma != 0)
-          mat.col(j).tail(rs) = (nLjj/Ljj) * mat.col(j).tail(rs) + (nLjj * sigma*numext::conj(wj)/gamma)*temp.tail(rs);
+      Index rs = n - j - 1;
+      if (rs) {
+        temp.tail(rs) -= (wj / Ljj) * mat.col(j).tail(rs);
+        if (!numext::is_exactly_zero(gamma))
+          mat.col(j).tail(rs) =
+              (nLjj / Ljj) * mat.col(j).tail(rs) + (nLjj * sigma * numext::conj(wj) / gamma) * temp.tail(rs);
       }
     }
   }
   return -1;
 }
 
-template<typename Scalar> struct llt_inplace<Scalar, Lower>
-{
+template <typename Scalar>
+struct llt_inplace<Scalar, Lower> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  template<typename MatrixType>
-  static Index unblocked(MatrixType& mat)
-  {
+  template <typename MatrixType>
+  static Index unblocked(MatrixType& mat) {
     using std::sqrt;
 
-    eigen_assert(mat.rows()==mat.cols());
+    eigen_assert(mat.rows() == mat.cols());
     const Index size = mat.rows();
-    for(Index k = 0; k < size; ++k)
-    {
-      Index rs = size-k-1; // remaining size
+    for (Index k = 0; k < size; ++k) {
+      Index rs = size - k - 1;  // remaining size
 
-      Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
-      Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
-      Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
+      Block<MatrixType, Dynamic, 1> A21(mat, k + 1, k, rs, 1);
+      Block<MatrixType, 1, Dynamic> A10(mat, k, 0, 1, k);
+      Block<MatrixType, Dynamic, Dynamic> A20(mat, k + 1, 0, rs, k);
 
-      RealScalar x = numext::real(mat.coeff(k,k));
-      if (k>0) x -= A10.squaredNorm();
-      if (x<=RealScalar(0))
-        return k;
-      mat.coeffRef(k,k) = x = sqrt(x);
-      if (k>0 && rs>0) A21.noalias() -= A20 * A10.adjoint();
-      if (rs>0) A21 /= x;
+      RealScalar x = numext::real(mat.coeff(k, k));
+      if (k > 0) x -= A10.squaredNorm();
+      if (x <= RealScalar(0)) return k;
+      mat.coeffRef(k, k) = x = sqrt(x);
+      if (k > 0 && rs > 0) A21.noalias() -= A20 * A10.adjoint();
+      if (rs > 0) A21 /= x;
     }
     return -1;
   }
 
-  template<typename MatrixType>
-  static Index blocked(MatrixType& m)
-  {
-    eigen_assert(m.rows()==m.cols());
+  template <typename MatrixType>
+  static Index blocked(MatrixType& m) {
+    eigen_assert(m.rows() == m.cols());
     Index size = m.rows();
-    if(size<32)
-      return unblocked(m);
+    if (size < 32) return unblocked(m);
 
-    Index blockSize = size/8;
-    blockSize = (blockSize/16)*16;
-    blockSize = (std::min)((std::max)(blockSize,Index(8)), Index(128));
+    Index blockSize = size / 8;
+    blockSize = (blockSize / 16) * 16;
+    blockSize = (std::min)((std::max)(blockSize, Index(8)), Index(128));
 
-    for (Index k=0; k<size; k+=blockSize)
-    {
+    for (Index k = 0; k < size; k += blockSize) {
       // partition the matrix:
       //       A00 |  -  |  -
       // lu  = A10 | A11 |  -
       //       A20 | A21 | A22
-      Index bs = (std::min)(blockSize, size-k);
+      Index bs = (std::min)(blockSize, size - k);
       Index rs = size - k - bs;
-      Block<MatrixType,Dynamic,Dynamic> A11(m,k,   k,   bs,bs);
-      Block<MatrixType,Dynamic,Dynamic> A21(m,k+bs,k,   rs,bs);
-      Block<MatrixType,Dynamic,Dynamic> A22(m,k+bs,k+bs,rs,rs);
+      Block<MatrixType, Dynamic, Dynamic> A11(m, k, k, bs, bs);
+      Block<MatrixType, Dynamic, Dynamic> A21(m, k + bs, k, rs, bs);
+      Block<MatrixType, Dynamic, Dynamic> A22(m, k + bs, k + bs, rs, rs);
 
       Index ret;
-      if((ret=unblocked(A11))>=0) return k+ret;
-      if(rs>0) A11.adjoint().template triangularView<Upper>().template solveInPlace<OnTheRight>(A21);
-      if(rs>0) A22.template selfadjointView<Lower>().rankUpdate(A21,typename NumTraits<RealScalar>::Literal(-1)); // bottleneck
+      if ((ret = unblocked(A11)) >= 0) return k + ret;
+      if (rs > 0) A11.adjoint().template triangularView<Upper>().template solveInPlace<OnTheRight>(A21);
+      if (rs > 0)
+        A22.template selfadjointView<Lower>().rankUpdate(A21,
+                                                         typename NumTraits<RealScalar>::Literal(-1));  // bottleneck
     }
     return -1;
   }
 
-  template<typename MatrixType, typename VectorType>
-  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma)
-  {
+  template <typename MatrixType, typename VectorType>
+  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) {
     return Eigen::internal::llt_rank_update_lower(mat, vec, sigma);
   }
 };
 
-template<typename Scalar> struct llt_inplace<Scalar, Upper>
-{
+template <typename Scalar>
+struct llt_inplace<Scalar, Upper> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
 
-  template<typename MatrixType>
-  static EIGEN_STRONG_INLINE Index unblocked(MatrixType& mat)
-  {
+  template <typename MatrixType>
+  static EIGEN_STRONG_INLINE Index unblocked(MatrixType& mat) {
     Transpose<MatrixType> matt(mat);
     return llt_inplace<Scalar, Lower>::unblocked(matt);
   }
-  template<typename MatrixType>
-  static EIGEN_STRONG_INLINE Index blocked(MatrixType& mat)
-  {
+  template <typename MatrixType>
+  static EIGEN_STRONG_INLINE Index blocked(MatrixType& mat) {
     Transpose<MatrixType> matt(mat);
     return llt_inplace<Scalar, Lower>::blocked(matt);
   }
-  template<typename MatrixType, typename VectorType>
-  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma)
-  {
+  template <typename MatrixType, typename VectorType>
+  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) {
     Transpose<MatrixType> matt(mat);
     return llt_inplace<Scalar, Lower>::rankUpdate(matt, vec.conjugate(), sigma);
   }
 };
 
-template<typename MatrixType> struct LLT_Traits<MatrixType,Lower>
-{
+template <typename MatrixType>
+struct LLT_Traits<MatrixType, Lower> {
   typedef const TriangularView<const MatrixType, Lower> MatrixL;
   typedef const TriangularView<const typename MatrixType::AdjointReturnType, Upper> MatrixU;
   static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); }
   static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); }
-  static bool inplace_decomposition(MatrixType& m)
-  { return llt_inplace<typename MatrixType::Scalar, Lower>::blocked(m)==-1; }
+  static bool inplace_decomposition(MatrixType& m) {
+    return llt_inplace<typename MatrixType::Scalar, Lower>::blocked(m) == -1;
+  }
 };
 
-template<typename MatrixType> struct LLT_Traits<MatrixType,Upper>
-{
+template <typename MatrixType>
+struct LLT_Traits<MatrixType, Upper> {
   typedef const TriangularView<const typename MatrixType::AdjointReturnType, Lower> MatrixL;
   typedef const TriangularView<const MatrixType, Upper> MatrixU;
   static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); }
   static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); }
-  static bool inplace_decomposition(MatrixType& m)
-  { return llt_inplace<typename MatrixType::Scalar, Upper>::blocked(m)==-1; }
+  static bool inplace_decomposition(MatrixType& m) {
+    return llt_inplace<typename MatrixType::Scalar, Upper>::blocked(m) == -1;
+  }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** Computes / recomputes the Cholesky decomposition A = LL^* = U^*U of \a matrix
-  *
-  * \returns a reference to *this
-  *
-  * Example: \include TutorialLinAlgComputeTwice.cpp
-  * Output: \verbinclude TutorialLinAlgComputeTwice.out
-  */
-template<typename MatrixType, int _UpLo>
-template<typename InputType>
-LLT<MatrixType,_UpLo>& LLT<MatrixType,_UpLo>::compute(const EigenBase<InputType>& a)
-{
-  check_template_parameters();
-
-  eigen_assert(a.rows()==a.cols());
+ *
+ * \returns a reference to *this
+ *
+ * Example: \include TutorialLinAlgComputeTwice.cpp
+ * Output: \verbinclude TutorialLinAlgComputeTwice.out
+ */
+template <typename MatrixType, int UpLo_>
+template <typename InputType>
+LLT<MatrixType, UpLo_>& LLT<MatrixType, UpLo_>::compute(const EigenBase<InputType>& a) {
+  eigen_assert(a.rows() == a.cols());
   const Index size = a.rows();
   m_matrix.resize(size, size);
-  if (!internal::is_same_dense(m_matrix, a.derived()))
-    m_matrix = a.derived();
+  if (!internal::is_same_dense(m_matrix, a.derived())) m_matrix = a.derived();
 
   // Compute matrix L1 norm = max abs column sum.
   m_l1_norm = RealScalar(0);
   // TODO move this code to SelfAdjointView
   for (Index col = 0; col < size; ++col) {
     RealScalar abs_col_sum;
-    if (_UpLo == Lower)
-      abs_col_sum = m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>();
+    if (UpLo_ == Lower)
+      abs_col_sum =
+          m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>();
     else
-      abs_col_sum = m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>();
-    if (abs_col_sum > m_l1_norm)
-      m_l1_norm = abs_col_sum;
+      abs_col_sum =
+          m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>();
+    if (abs_col_sum > m_l1_norm) m_l1_norm = abs_col_sum;
   }
 
   m_isInitialized = true;
@@ -460,18 +424,17 @@
 }
 
 /** Performs a rank one update (or dowdate) of the current decomposition.
-  * If A = LL^* before the rank one update,
-  * then after it we have LL^* = A + sigma * v v^* where \a v must be a vector
-  * of same dimension.
-  */
-template<typename _MatrixType, int _UpLo>
-template<typename VectorType>
-LLT<_MatrixType,_UpLo> & LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, const RealScalar& sigma)
-{
+ * If A = LL^* before the rank one update,
+ * then after it we have LL^* = A + sigma * v v^* where \a v must be a vector
+ * of same dimension.
+ */
+template <typename MatrixType_, int UpLo_>
+template <typename VectorType>
+LLT<MatrixType_, UpLo_>& LLT<MatrixType_, UpLo_>::rankUpdate(const VectorType& v, const RealScalar& sigma) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType);
-  eigen_assert(v.size()==m_matrix.cols());
+  eigen_assert(v.size() == m_matrix.cols());
   eigen_assert(m_isInitialized);
-  if(internal::llt_inplace<typename MatrixType::Scalar, UpLo>::rankUpdate(m_matrix,v,sigma)>=0)
+  if (internal::llt_inplace<typename MatrixType::Scalar, UpLo>::rankUpdate(m_matrix, v, sigma) >= 0)
     m_info = NumericalIssue;
   else
     m_info = Success;
@@ -480,43 +443,40 @@
 }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename _MatrixType,int _UpLo>
-template<typename RhsType, typename DstType>
-void LLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, int UpLo_>
+template <typename RhsType, typename DstType>
+void LLT<MatrixType_, UpLo_>::_solve_impl(const RhsType& rhs, DstType& dst) const {
   _solve_impl_transposed<true>(rhs, dst);
 }
 
-template<typename _MatrixType,int _UpLo>
-template<bool Conjugate, typename RhsType, typename DstType>
-void LLT<_MatrixType,_UpLo>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
-    dst = rhs;
+template <typename MatrixType_, int UpLo_>
+template <bool Conjugate, typename RhsType, typename DstType>
+void LLT<MatrixType_, UpLo_>::_solve_impl_transposed(const RhsType& rhs, DstType& dst) const {
+  dst = rhs;
 
-    matrixL().template conjugateIf<!Conjugate>().solveInPlace(dst);
-    matrixU().template conjugateIf<!Conjugate>().solveInPlace(dst);
+  matrixL().template conjugateIf<!Conjugate>().solveInPlace(dst);
+  matrixU().template conjugateIf<!Conjugate>().solveInPlace(dst);
 }
 #endif
 
 /** \internal use x = llt_object.solve(x);
-  *
-  * This is the \em in-place version of solve().
-  *
-  * \param bAndX represents both the right-hand side matrix b and result x.
-  *
-  * This version avoids a copy when the right hand side matrix b is not needed anymore.
-  *
-  * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
-  * This function will const_cast it, so constness isn't honored here.
-  *
-  * \sa LLT::solve(), MatrixBase::llt()
-  */
-template<typename MatrixType, int _UpLo>
-template<typename Derived>
-void LLT<MatrixType,_UpLo>::solveInPlace(const MatrixBase<Derived> &bAndX) const
-{
+ *
+ * This is the \em in-place version of solve().
+ *
+ * \param bAndX represents both the right-hand side matrix b and result x.
+ *
+ * This version avoids a copy when the right hand side matrix b is not needed anymore.
+ *
+ * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
+ * This function will const_cast it, so constness isn't honored here.
+ *
+ * \sa LLT::solve(), MatrixBase::llt()
+ */
+template <typename MatrixType, int UpLo_>
+template <typename Derived>
+void LLT<MatrixType, UpLo_>::solveInPlace(const MatrixBase<Derived>& bAndX) const {
   eigen_assert(m_isInitialized && "LLT is not initialized.");
-  eigen_assert(m_matrix.rows()==bAndX.rows());
+  eigen_assert(m_matrix.rows() == bAndX.rows());
   matrixL().solveInPlace(bAndX);
   matrixU().solveInPlace(bAndX);
 }
@@ -524,35 +484,31 @@
 /** \returns the matrix represented by the decomposition,
  * i.e., it returns the product: L L^*.
  * This function is provided for debug purpose. */
-template<typename MatrixType, int _UpLo>
-MatrixType LLT<MatrixType,_UpLo>::reconstructedMatrix() const
-{
+template <typename MatrixType, int UpLo_>
+MatrixType LLT<MatrixType, UpLo_>::reconstructedMatrix() const {
   eigen_assert(m_isInitialized && "LLT is not initialized.");
   return matrixL() * matrixL().adjoint().toDenseMatrix();
 }
 
 /** \cholesky_module
-  * \returns the LLT decomposition of \c *this
-  * \sa SelfAdjointView::llt()
-  */
-template<typename Derived>
-inline const LLT<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::llt() const
-{
+ * \returns the LLT decomposition of \c *this
+ * \sa SelfAdjointView::llt()
+ */
+template <typename Derived>
+inline const LLT<typename MatrixBase<Derived>::PlainObject> MatrixBase<Derived>::llt() const {
   return LLT<PlainObject>(derived());
 }
 
 /** \cholesky_module
-  * \returns the LLT decomposition of \c *this
-  * \sa SelfAdjointView::llt()
-  */
-template<typename MatrixType, unsigned int UpLo>
-inline const LLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
-SelfAdjointView<MatrixType, UpLo>::llt() const
-{
-  return LLT<PlainObject,UpLo>(m_matrix);
+ * \returns the LLT decomposition of \c *this
+ * \sa SelfAdjointView::llt()
+ */
+template <typename MatrixType, unsigned int UpLo>
+inline const LLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo> SelfAdjointView<MatrixType, UpLo>::llt()
+    const {
+  return LLT<PlainObject, UpLo>(m_matrix);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_LLT_H
+#endif  // EIGEN_LLT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h
index b6200fa..0f45e89 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h
@@ -10,374 +10,227 @@
 #ifndef EIGEN_ARITHMETIC_SEQUENCE_H
 #define EIGEN_ARITHMETIC_SEQUENCE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-#if (!EIGEN_HAS_CXX11) || !((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48)
-template<typename T> struct aseq_negate {};
-
-template<> struct aseq_negate<Index> {
-  typedef Index type;
-};
-
-template<int N> struct aseq_negate<FixedInt<N> > {
-  typedef FixedInt<-N> type;
-};
-
-// Compilation error in the following case:
-template<> struct aseq_negate<FixedInt<DynamicIndex> > {};
-
-template<typename FirstType,typename SizeType,typename IncrType,
-         bool FirstIsSymbolic=symbolic::is_symbolic<FirstType>::value,
-         bool SizeIsSymbolic =symbolic::is_symbolic<SizeType>::value>
-struct aseq_reverse_first_type {
-  typedef Index type;
-};
-
-template<typename FirstType,typename SizeType,typename IncrType>
-struct aseq_reverse_first_type<FirstType,SizeType,IncrType,true,true> {
-  typedef symbolic::AddExpr<FirstType,
-                            symbolic::ProductExpr<symbolic::AddExpr<SizeType,symbolic::ValueExpr<FixedInt<-1> > >,
-                                                  symbolic::ValueExpr<IncrType> >
-                           > type;
-};
-
-template<typename SizeType,typename IncrType,typename EnableIf = void>
-struct aseq_reverse_first_type_aux {
-  typedef Index type;
-};
-
-template<typename SizeType,typename IncrType>
-struct aseq_reverse_first_type_aux<SizeType,IncrType,typename internal::enable_if<bool((SizeType::value+IncrType::value)|0x1)>::type> {
-  typedef FixedInt<(SizeType::value-1)*IncrType::value> type;
-};
-
-template<typename FirstType,typename SizeType,typename IncrType>
-struct aseq_reverse_first_type<FirstType,SizeType,IncrType,true,false> {
-  typedef typename aseq_reverse_first_type_aux<SizeType,IncrType>::type Aux;
-  typedef symbolic::AddExpr<FirstType,symbolic::ValueExpr<Aux> > type;
-};
-
-template<typename FirstType,typename SizeType,typename IncrType>
-struct aseq_reverse_first_type<FirstType,SizeType,IncrType,false,true> {
-  typedef symbolic::AddExpr<symbolic::ProductExpr<symbolic::AddExpr<SizeType,symbolic::ValueExpr<FixedInt<-1> > >,
-                                                  symbolic::ValueExpr<IncrType> >,
-                            symbolic::ValueExpr<> > type;
-};
-#endif
-
 // Helper to cleanup the type of the increment:
-template<typename T> struct cleanup_seq_incr {
-  typedef typename cleanup_index_type<T,DynamicIndex>::type type;
+template <typename T>
+struct cleanup_seq_incr {
+  typedef typename cleanup_index_type<T, DynamicIndex>::type type;
 };
 
-}
+}  // namespace internal
 
 //--------------------------------------------------------------------------------
 // seq(first,last,incr) and seqN(first,size,incr)
 //--------------------------------------------------------------------------------
 
-template<typename FirstType=Index,typename SizeType=Index,typename IncrType=internal::FixedInt<1> >
+template <typename FirstType = Index, typename SizeType = Index, typename IncrType = internal::FixedInt<1> >
 class ArithmeticSequence;
 
-template<typename FirstType,typename SizeType,typename IncrType>
+template <typename FirstType, typename SizeType, typename IncrType>
 ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
                    typename internal::cleanup_index_type<SizeType>::type,
-                   typename internal::cleanup_seq_incr<IncrType>::type >
+                   typename internal::cleanup_seq_incr<IncrType>::type>
 seqN(FirstType first, SizeType size, IncrType incr);
 
 /** \class ArithmeticSequence
-  * \ingroup Core_Module
-  *
-  * This class represents an arithmetic progression \f$ a_0, a_1, a_2, ..., a_{n-1}\f$ defined by
-  * its \em first value \f$ a_0 \f$, its \em size (aka length) \em n, and the \em increment (aka stride)
-  * that is equal to \f$ a_{i+1}-a_{i}\f$ for any \em i.
-  *
-  * It is internally used as the return type of the Eigen::seq and Eigen::seqN functions, and as the input arguments
-  * of DenseBase::operator()(const RowIndices&, const ColIndices&), and most of the time this is the
-  * only way it is used.
-  *
-  * \tparam FirstType type of the first element, usually an Index,
-  *                   but internally it can be a symbolic expression
-  * \tparam SizeType type representing the size of the sequence, usually an Index
-  *                  or a compile time integral constant. Internally, it can also be a symbolic expression
-  * \tparam IncrType type of the increment, can be a runtime Index, or a compile time integral constant (default is compile-time 1)
-  *
-  * \sa Eigen::seq, Eigen::seqN, DenseBase::operator()(const RowIndices&, const ColIndices&), class IndexedView
-  */
-template<typename FirstType,typename SizeType,typename IncrType>
-class ArithmeticSequence
-{
-public:
+ * \ingroup Core_Module
+ *
+ * This class represents an arithmetic progression \f$ a_0, a_1, a_2, ..., a_{n-1}\f$ defined by
+ * its \em first value \f$ a_0 \f$, its \em size (aka length) \em n, and the \em increment (aka stride)
+ * that is equal to \f$ a_{i+1}-a_{i}\f$ for any \em i.
+ *
+ * It is internally used as the return type of the Eigen::seq and Eigen::seqN functions, and as the input arguments
+ * of DenseBase::operator()(const RowIndices&, const ColIndices&), and most of the time this is the
+ * only way it is used.
+ *
+ * \tparam FirstType type of the first element, usually an Index,
+ *                   but internally it can be a symbolic expression
+ * \tparam SizeType type representing the size of the sequence, usually an Index
+ *                  or a compile time integral constant. Internally, it can also be a symbolic expression
+ * \tparam IncrType type of the increment, can be a runtime Index, or a compile time integral constant (default is
+ * compile-time 1)
+ *
+ * \sa Eigen::seq, Eigen::seqN, DenseBase::operator()(const RowIndices&, const ColIndices&), class IndexedView
+ */
+template <typename FirstType, typename SizeType, typename IncrType>
+class ArithmeticSequence {
+ public:
   ArithmeticSequence(FirstType first, SizeType size) : m_first(first), m_size(size) {}
   ArithmeticSequence(FirstType first, SizeType size, IncrType incr) : m_first(first), m_size(size), m_incr(incr) {}
 
   enum {
     SizeAtCompileTime = internal::get_fixed_value<SizeType>::value,
-    IncrAtCompileTime = internal::get_fixed_value<IncrType,DynamicIndex>::value
+    IncrAtCompileTime = internal::get_fixed_value<IncrType, DynamicIndex>::value
   };
 
   /** \returns the size, i.e., number of elements, of the sequence */
-  Index size()  const { return m_size; }
+  Index size() const { return m_size; }
 
   /** \returns the first element \f$ a_0 \f$ in the sequence */
-  Index first()  const { return m_first; }
+  Index first() const { return m_first; }
 
   /** \returns the value \f$ a_i \f$ at index \a i in the sequence. */
   Index operator[](Index i) const { return m_first + i * m_incr; }
 
   const FirstType& firstObject() const { return m_first; }
-  const SizeType&  sizeObject()  const { return m_size; }
-  const IncrType&  incrObject()  const { return m_incr; }
+  const SizeType& sizeObject() const { return m_size; }
+  const IncrType& incrObject() const { return m_incr; }
 
-protected:
+ protected:
   FirstType m_first;
-  SizeType  m_size;
-  IncrType  m_incr;
+  SizeType m_size;
+  IncrType m_incr;
 
-public:
-
-#if EIGEN_HAS_CXX11 && ((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48)
-  auto reverse() const -> decltype(Eigen::seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr)) {
-    return seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr);
+ public:
+  auto reverse() const -> decltype(Eigen::seqN(m_first + (m_size + fix<-1>()) * m_incr, m_size, -m_incr)) {
+    return seqN(m_first + (m_size + fix<-1>()) * m_incr, m_size, -m_incr);
   }
-#else
-protected:
-  typedef typename internal::aseq_negate<IncrType>::type ReverseIncrType;
-  typedef typename internal::aseq_reverse_first_type<FirstType,SizeType,IncrType>::type ReverseFirstType;
-public:
-  ArithmeticSequence<ReverseFirstType,SizeType,ReverseIncrType>
-  reverse() const {
-    return seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr);
-  }
-#endif
 };
 
 /** \returns an ArithmeticSequence starting at \a first, of length \a size, and increment \a incr
-  *
-  * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */
-template<typename FirstType,typename SizeType,typename IncrType>
-ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type,typename internal::cleanup_seq_incr<IncrType>::type >
-seqN(FirstType first, SizeType size, IncrType incr)  {
-  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type,typename internal::cleanup_seq_incr<IncrType>::type>(first,size,incr);
+ *
+ * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */
+template <typename FirstType, typename SizeType, typename IncrType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                   typename internal::cleanup_index_type<SizeType>::type,
+                   typename internal::cleanup_seq_incr<IncrType>::type>
+seqN(FirstType first, SizeType size, IncrType incr) {
+  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                            typename internal::cleanup_index_type<SizeType>::type,
+                            typename internal::cleanup_seq_incr<IncrType>::type>(first, size, incr);
 }
 
 /** \returns an ArithmeticSequence starting at \a first, of length \a size, and unit increment
-  *
-  * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType) */
-template<typename FirstType,typename SizeType>
-ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type >
-seqN(FirstType first, SizeType size)  {
-  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type>(first,size);
+ *
+ * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType) */
+template <typename FirstType, typename SizeType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                   typename internal::cleanup_index_type<SizeType>::type>
+seqN(FirstType first, SizeType size) {
+  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                            typename internal::cleanup_index_type<SizeType>::type>(first, size);
 }
 
 #ifdef EIGEN_PARSED_BY_DOXYGEN
 
-/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and with positive (or negative) increment \a incr
-  *
-  * It is essentially an alias to:
-  * \code
-  * seqN(f, (l-f+incr)/incr, incr);
-  * \endcode
-  *
-  * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType)
-  */
-template<typename FirstType,typename LastType, typename IncrType>
+/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and with positive (or negative) increment \a
+ * incr
+ *
+ * It is essentially an alias to:
+ * \code
+ * seqN(f, (l-f+incr)/incr, incr);
+ * \endcode
+ *
+ * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType)
+ */
+template <typename FirstType, typename LastType, typename IncrType>
 auto seq(FirstType f, LastType l, IncrType incr);
 
 /** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and unit increment
-  *
-  * It is essentially an alias to:
-  * \code
-  * seqN(f,l-f+1);
-  * \endcode
-  *
-  * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType)
-  */
-template<typename FirstType,typename LastType>
+ *
+ * It is essentially an alias to:
+ * \code
+ * seqN(f,l-f+1);
+ * \endcode
+ *
+ * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType)
+ */
+template <typename FirstType, typename LastType>
 auto seq(FirstType f, LastType l);
 
-#else // EIGEN_PARSED_BY_DOXYGEN
+#else  // EIGEN_PARSED_BY_DOXYGEN
 
-#if EIGEN_HAS_CXX11
-template<typename FirstType,typename LastType>
-auto seq(FirstType f, LastType l) -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
-                                                   (  typename internal::cleanup_index_type<LastType>::type(l)
-                                                    - typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>())))
-{
+template <typename FirstType, typename LastType>
+auto seq(FirstType f, LastType l)
+    -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+                     (typename internal::cleanup_index_type<LastType>::type(l) -
+                      typename internal::cleanup_index_type<FirstType>::type(f) + fix<1>()))) {
   return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
-              (typename internal::cleanup_index_type<LastType>::type(l)
-               -typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>()));
+              (typename internal::cleanup_index_type<LastType>::type(l) -
+               typename internal::cleanup_index_type<FirstType>::type(f) + fix<1>()));
 }
 
-template<typename FirstType,typename LastType, typename IncrType>
+template <typename FirstType, typename LastType, typename IncrType>
 auto seq(FirstType f, LastType l, IncrType incr)
-  -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
-                   (   typename internal::cleanup_index_type<LastType>::type(l)
-                     - typename internal::cleanup_index_type<FirstType>::type(f)+typename internal::cleanup_seq_incr<IncrType>::type(incr)
-                   ) / typename internal::cleanup_seq_incr<IncrType>::type(incr),
-                   typename internal::cleanup_seq_incr<IncrType>::type(incr)))
-{
+    -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+                     (typename internal::cleanup_index_type<LastType>::type(l) -
+                      typename internal::cleanup_index_type<FirstType>::type(f) +
+                      typename internal::cleanup_seq_incr<IncrType>::type(incr)) /
+                         typename internal::cleanup_seq_incr<IncrType>::type(incr),
+                     typename internal::cleanup_seq_incr<IncrType>::type(incr))) {
   typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
   return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
-              ( typename internal::cleanup_index_type<LastType>::type(l)
-               -typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr)) / CleanedIncrType(incr),
+              (typename internal::cleanup_index_type<LastType>::type(l) -
+               typename internal::cleanup_index_type<FirstType>::type(f) + CleanedIncrType(incr)) /
+                  CleanedIncrType(incr),
               CleanedIncrType(incr));
 }
 
-#else // EIGEN_HAS_CXX11
+#endif  // EIGEN_PARSED_BY_DOXYGEN
 
-template<typename FirstType,typename LastType>
-typename internal::enable_if<!(symbolic::is_symbolic<FirstType>::value || symbolic::is_symbolic<LastType>::value),
-                             ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,Index> >::type
-seq(FirstType f, LastType l)
-{
-  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
-              Index((typename internal::cleanup_index_type<LastType>::type(l)-typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>())));
-}
+namespace placeholders {
 
-template<typename FirstTypeDerived,typename LastType>
-typename internal::enable_if<!symbolic::is_symbolic<LastType>::value,
-    ArithmeticSequence<FirstTypeDerived, symbolic::AddExpr<symbolic::AddExpr<symbolic::NegateExpr<FirstTypeDerived>,symbolic::ValueExpr<> >,
-                                                            symbolic::ValueExpr<internal::FixedInt<1> > > > >::type
-seq(const symbolic::BaseExpr<FirstTypeDerived> &f, LastType l)
-{
-  return seqN(f.derived(),(typename internal::cleanup_index_type<LastType>::type(l)-f.derived()+fix<1>()));
-}
-
-template<typename FirstType,typename LastTypeDerived>
-typename internal::enable_if<!symbolic::is_symbolic<FirstType>::value,
-    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
-                        symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,symbolic::ValueExpr<> >,
-                                          symbolic::ValueExpr<internal::FixedInt<1> > > > >::type
-seq(FirstType f, const symbolic::BaseExpr<LastTypeDerived> &l)
-{
-  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),(l.derived()-typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>()));
-}
-
-template<typename FirstTypeDerived,typename LastTypeDerived>
-ArithmeticSequence<FirstTypeDerived,
-                    symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,symbolic::NegateExpr<FirstTypeDerived> >,symbolic::ValueExpr<internal::FixedInt<1> > > >
-seq(const symbolic::BaseExpr<FirstTypeDerived> &f, const symbolic::BaseExpr<LastTypeDerived> &l)
-{
-  return seqN(f.derived(),(l.derived()-f.derived()+fix<1>()));
-}
-
-
-template<typename FirstType,typename LastType, typename IncrType>
-typename internal::enable_if<!(symbolic::is_symbolic<FirstType>::value || symbolic::is_symbolic<LastType>::value),
-    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,Index,typename internal::cleanup_seq_incr<IncrType>::type> >::type
-seq(FirstType f, LastType l, IncrType incr)
-{
-  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
-  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
-              Index((typename internal::cleanup_index_type<LastType>::type(l)-typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr)), incr);
-}
-
-template<typename FirstTypeDerived,typename LastType, typename IncrType>
-typename internal::enable_if<!symbolic::is_symbolic<LastType>::value,
-    ArithmeticSequence<FirstTypeDerived,
-                        symbolic::QuotientExpr<symbolic::AddExpr<symbolic::AddExpr<symbolic::NegateExpr<FirstTypeDerived>,
-                                                                                   symbolic::ValueExpr<> >,
-                                                                 symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
-                                              symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
-                        typename internal::cleanup_seq_incr<IncrType>::type> >::type
-seq(const symbolic::BaseExpr<FirstTypeDerived> &f, LastType l, IncrType incr)
-{
-  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
-  return seqN(f.derived(),(typename internal::cleanup_index_type<LastType>::type(l)-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
-}
-
-template<typename FirstType,typename LastTypeDerived, typename IncrType>
-typename internal::enable_if<!symbolic::is_symbolic<FirstType>::value,
-    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
-                        symbolic::QuotientExpr<symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,symbolic::ValueExpr<> >,
-                                                                 symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
-                                               symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
-                        typename internal::cleanup_seq_incr<IncrType>::type> >::type
-seq(FirstType f, const symbolic::BaseExpr<LastTypeDerived> &l, IncrType incr)
-{
-  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
-  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
-              (l.derived()-typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
-}
-
-template<typename FirstTypeDerived,typename LastTypeDerived, typename IncrType>
-ArithmeticSequence<FirstTypeDerived,
-                    symbolic::QuotientExpr<symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,
-                                                                               symbolic::NegateExpr<FirstTypeDerived> >,
-                                                             symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
-                                          symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
-                    typename internal::cleanup_seq_incr<IncrType>::type>
-seq(const symbolic::BaseExpr<FirstTypeDerived> &f, const symbolic::BaseExpr<LastTypeDerived> &l, IncrType incr)
-{
-  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
-  return seqN(f.derived(),(l.derived()-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
-}
-#endif // EIGEN_HAS_CXX11
-
-#endif // EIGEN_PARSED_BY_DOXYGEN
-
-
-#if EIGEN_HAS_CXX11 || defined(EIGEN_PARSED_BY_DOXYGEN)
 /** \cpp11
-  * \returns a symbolic ArithmeticSequence representing the last \a size elements with increment \a incr.
-  *
-  * It is a shortcut for: \code seqN(last-(size-fix<1>)*incr, size, incr) \endcode
-  * 
-  * \sa lastN(SizeType), seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */
-template<typename SizeType,typename IncrType>
+ * \returns a symbolic ArithmeticSequence representing the last \a size elements with increment \a incr.
+ *
+ * It is a shortcut for: \code seqN(last-(size-fix<1>)*incr, size, incr) \endcode
+ *
+ * \sa lastN(SizeType), seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */
+template <typename SizeType, typename IncrType>
 auto lastN(SizeType size, IncrType incr)
--> decltype(seqN(Eigen::last-(size-fix<1>())*incr, size, incr))
-{
-  return seqN(Eigen::last-(size-fix<1>())*incr, size, incr);
+    -> decltype(seqN(Eigen::placeholders::last - (size - fix<1>()) * incr, size, incr)) {
+  return seqN(Eigen::placeholders::last - (size - fix<1>()) * incr, size, incr);
 }
 
 /** \cpp11
-  * \returns a symbolic ArithmeticSequence representing the last \a size elements with a unit increment.
-  *
-  *  It is a shortcut for: \code seq(last+fix<1>-size, last) \endcode
-  * 
-  * \sa lastN(SizeType,IncrType, seqN(FirstType,SizeType), seq(FirstType,LastType) */
-template<typename SizeType>
-auto lastN(SizeType size)
--> decltype(seqN(Eigen::last+fix<1>()-size, size))
-{
-  return seqN(Eigen::last+fix<1>()-size, size);
+ * \returns a symbolic ArithmeticSequence representing the last \a size elements with a unit increment.
+ *
+ *  It is a shortcut for: \code seq(last+fix<1>-size, last) \endcode
+ *
+ * \sa lastN(SizeType,IncrType, seqN(FirstType,SizeType), seq(FirstType,LastType) */
+template <typename SizeType>
+auto lastN(SizeType size) -> decltype(seqN(Eigen::placeholders::last + fix<1>() - size, size)) {
+  return seqN(Eigen::placeholders::last + fix<1>() - size, size);
 }
-#endif
+
+}  // namespace placeholders
 
 namespace internal {
 
 // Convert a symbolic span into a usable one (i.e., remove last/end "keywords")
-template<typename T>
+template <typename T>
 struct make_size_type {
-  typedef typename internal::conditional<symbolic::is_symbolic<T>::value, Index, T>::type type;
+  typedef std::conditional_t<symbolic::is_symbolic<T>::value, Index, T> type;
 };
 
-template<typename FirstType,typename SizeType,typename IncrType,int XprSize>
-struct IndexedViewCompatibleType<ArithmeticSequence<FirstType,SizeType,IncrType>, XprSize> {
-  typedef ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType> type;
+template <typename FirstType, typename SizeType, typename IncrType, int XprSize>
+struct IndexedViewCompatibleType<ArithmeticSequence<FirstType, SizeType, IncrType>, XprSize> {
+  typedef ArithmeticSequence<Index, typename make_size_type<SizeType>::type, IncrType> type;
 };
 
-template<typename FirstType,typename SizeType,typename IncrType>
-ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType>
-makeIndexedViewCompatible(const ArithmeticSequence<FirstType,SizeType,IncrType>& ids, Index size,SpecializedType) {
-  return ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType>(
-            eval_expr_given_size(ids.firstObject(),size),eval_expr_given_size(ids.sizeObject(),size),ids.incrObject());
+template <typename FirstType, typename SizeType, typename IncrType>
+ArithmeticSequence<Index, typename make_size_type<SizeType>::type, IncrType> makeIndexedViewCompatible(
+    const ArithmeticSequence<FirstType, SizeType, IncrType>& ids, Index size, SpecializedType) {
+  return ArithmeticSequence<Index, typename make_size_type<SizeType>::type, IncrType>(
+      eval_expr_given_size(ids.firstObject(), size), eval_expr_given_size(ids.sizeObject(), size), ids.incrObject());
 }
 
-template<typename FirstType,typename SizeType,typename IncrType>
-struct get_compile_time_incr<ArithmeticSequence<FirstType,SizeType,IncrType> > {
-  enum { value = get_fixed_value<IncrType,DynamicIndex>::value };
+template <typename FirstType, typename SizeType, typename IncrType>
+struct get_compile_time_incr<ArithmeticSequence<FirstType, SizeType, IncrType> > {
+  enum { value = get_fixed_value<IncrType, DynamicIndex>::value };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \namespace Eigen::indexing
   * \ingroup Core_Module
-  * 
+  *
   * The sole purpose of this namespace is to be able to import all functions
   * and symbols that are expected to be used within operator() for indexing
   * and slicing. If you already imported the whole Eigen namespace:
@@ -387,27 +240,25 @@
   * \code using namespace Eigen::indexing; \endcode
   * is equivalent to:
   * \code
-  using Eigen::all;
+  using Eigen::fix;
   using Eigen::seq;
   using Eigen::seqN;
-  using Eigen::lastN; // c++11 only
-  using Eigen::last;
-  using Eigen::lastp1;
-  using Eigen::fix;
+  using Eigen::placeholders::all;
+  using Eigen::placeholders::last;
+  using Eigen::placeholders::lastN;  // c++11 only
+  using Eigen::placeholders::lastp1;
   \endcode
   */
 namespace indexing {
-  using Eigen::all;
-  using Eigen::seq;
-  using Eigen::seqN;
-  #if EIGEN_HAS_CXX11
-  using Eigen::lastN;
-  #endif
-  using Eigen::last;
-  using Eigen::lastp1;
-  using Eigen::fix;
-}
+using Eigen::fix;
+using Eigen::seq;
+using Eigen::seqN;
+using Eigen::placeholders::all;
+using Eigen::placeholders::last;
+using Eigen::placeholders::lastN;
+using Eigen::placeholders::lastp1;
+}  // namespace indexing
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ARITHMETIC_SEQUENCE_H
+#endif  // EIGEN_ARITHMETIC_SEQUENCE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h
index 20c789b..29c9682 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h
@@ -10,376 +10,330 @@
 #ifndef EIGEN_ARRAY_H
 #define EIGEN_ARRAY_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct traits<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > : traits<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-{
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+struct traits<Array<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>>
+    : traits<Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> {
   typedef ArrayXpr XprKind;
-  typedef ArrayBase<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > XprBase;
+  typedef ArrayBase<Array<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> XprBase;
 };
-}
+}  // namespace internal
 
 /** \class Array
-  * \ingroup Core_Module
-  *
-  * \brief General-purpose arrays with easy API for coefficient-wise operations
-  *
-  * The %Array class is very similar to the Matrix class. It provides
-  * general-purpose one- and two-dimensional arrays. The difference between the
-  * %Array and the %Matrix class is primarily in the API: the API for the
-  * %Array class provides easy access to coefficient-wise operations, while the
-  * API for the %Matrix class provides easy access to linear-algebra
-  * operations.
-  *
-  * See documentation of class Matrix for detailed information on the template parameters
-  * storage layout.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAY_PLUGIN.
-  *
-  * \sa \blank \ref TutorialArrayClass, \ref TopicClassHierarchy
-  */
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-class Array
-  : public PlainObjectBase<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief General-purpose arrays with easy API for coefficient-wise operations
+ *
+ * The %Array class is very similar to the Matrix class. It provides
+ * general-purpose one- and two-dimensional arrays. The difference between the
+ * %Array and the %Matrix class is primarily in the API: the API for the
+ * %Array class provides easy access to coefficient-wise operations, while the
+ * API for the %Matrix class provides easy access to linear-algebra
+ * operations.
+ *
+ * See documentation of class Matrix for detailed information on the template parameters
+ * storage layout.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAY_PLUGIN.
+ *
+ * \sa \blank \ref TutorialArrayClass, \ref TopicClassHierarchy
+ */
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+class Array : public PlainObjectBase<Array<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> {
+ public:
+  typedef PlainObjectBase<Array> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Array)
 
-    typedef PlainObjectBase<Array> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Array)
+  enum { Options = Options_ };
+  typedef typename Base::PlainObject PlainObject;
 
-    enum { Options = _Options };
-    typedef typename Base::PlainObject PlainObject;
+ protected:
+  template <typename Derived, typename OtherDerived, bool IsVector>
+  friend struct internal::conservative_resize_like_impl;
 
-  protected:
-    template <typename Derived, typename OtherDerived, bool IsVector>
-    friend struct internal::conservative_resize_like_impl;
+  using Base::m_storage;
 
-    using Base::m_storage;
+ public:
+  using Base::base;
+  using Base::coeff;
+  using Base::coeffRef;
 
-  public:
+  /**
+   * The usage of
+   *   using Base::operator=;
+   * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped
+   * the usage of 'using'. This should be done only for operator=.
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const EigenBase<OtherDerived>& other) {
+    return Base::operator=(other);
+  }
 
-    using Base::base;
-    using Base::coeff;
-    using Base::coeffRef;
+  /** Set all the entries to \a value.
+   * \sa DenseBase::setConstant(), DenseBase::fill()
+   */
+  /* This overload is needed because the usage of
+   *   using Base::operator=;
+   * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped
+   * the usage of 'using'. This should be done only for operator=.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const Scalar& value) {
+    Base::setConstant(value);
+    return *this;
+  }
 
-    /**
-      * The usage of
-      *   using Base::operator=;
-      * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped
-      * the usage of 'using'. This should be done only for operator=.
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array& operator=(const EigenBase<OtherDerived> &other)
-    {
-      return Base::operator=(other);
-    }
+  /** Copies the value of the expression \a other into \c *this with automatic resizing.
+   *
+   * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
+   * it will be initialized.
+   *
+   * Note that copying a row-vector into a vector (and conversely) is allowed.
+   * The resizing, if any, is then done in the appropriate way so that row-vectors
+   * remain row-vectors and vectors remain vectors.
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const DenseBase<OtherDerived>& other) {
+    return Base::_set(other);
+  }
 
-    /** Set all the entries to \a value.
-      * \sa DenseBase::setConstant(), DenseBase::fill()
-      */
-    /* This overload is needed because the usage of
-      *   using Base::operator=;
-      * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped
-      * the usage of 'using'. This should be done only for operator=.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array& operator=(const Scalar &value)
-    {
-      Base::setConstant(value);
-      return *this;
-    }
+  /** This is a special case of the templated operator=. Its purpose is to
+   * prevent a default operator= from hiding the templated operator=.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const Array& other) { return Base::_set(other); }
 
-    /** Copies the value of the expression \a other into \c *this with automatic resizing.
-      *
-      * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
-      * it will be initialized.
-      *
-      * Note that copying a row-vector into a vector (and conversely) is allowed.
-      * The resizing, if any, is then done in the appropriate way so that row-vectors
-      * remain row-vectors and vectors remain vectors.
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array& operator=(const DenseBase<OtherDerived>& other)
-    {
-      return Base::_set(other);
-    }
-
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array& operator=(const Array& other)
-    {
-      return Base::_set(other);
-    }
-
-    /** Default constructor.
-      *
-      * For fixed-size matrices, does nothing.
-      *
-      * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix
-      * is called a null matrix. This constructor is the unique way to create null matrices: resizing
-      * a matrix to 0 is not supported.
-      *
-      * \sa resize(Index,Index)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array() : Base()
-    {
-      Base::_check_template_params();
-      EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-    }
+  /** Default constructor.
+   *
+   * For fixed-size matrices, does nothing.
+   *
+   * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix
+   * is called a null matrix. This constructor is the unique way to create null matrices: resizing
+   * a matrix to 0 is not supported.
+   *
+   * \sa resize(Index,Index)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array() : Base() { EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    // FIXME is it still needed ??
-    /** \internal */
-    EIGEN_DEVICE_FUNC
-    Array(internal::constructor_without_unaligned_array_assert)
-      : Base(internal::constructor_without_unaligned_array_assert())
-    {
-      Base::_check_template_params();
-      EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-    }
+  // FIXME is it still needed ??
+  /** \internal */
+  EIGEN_DEVICE_FUNC Array(internal::constructor_without_unaligned_array_assert)
+      : Base(internal::constructor_without_unaligned_array_assert()){EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED}
 #endif
 
-#if EIGEN_HAS_RVALUE_REFERENCES
-    EIGEN_DEVICE_FUNC
-    Array(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value)
-      : Base(std::move(other))
-    {
-      Base::_check_template_params();
-    }
-    EIGEN_DEVICE_FUNC
-    Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
-    {
-      Base::operator=(std::move(other));
-      return *this;
-    }
-#endif
+        EIGEN_DEVICE_FUNC Array(Array && other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value)
+      : Base(std::move(other)) {
+  }
+  EIGEN_DEVICE_FUNC Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value) {
+    Base::operator=(std::move(other));
+    return *this;
+  }
 
-    #if EIGEN_HAS_CXX11
-    /** \copydoc PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
-     *
-     * Example: \include Array_variadic_ctor_cxx11.cpp
-     * Output: \verbinclude Array_variadic_ctor_cxx11.out
-     *
-     * \sa Array(const std::initializer_list<std::initializer_list<Scalar>>&)
-     * \sa Array(const Scalar&), Array(const Scalar&,const Scalar&)
-     */
-    template <typename... ArgTypes>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+  /** \copydoc PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const
+   * ArgTypes&... args)
+   *
+   * Example: \include Array_variadic_ctor_cxx11.cpp
+   * Output: \verbinclude Array_variadic_ctor_cxx11.out
+   *
+   * \sa Array(const std::initializer_list<std::initializer_list<Scalar>>&)
+   * \sa Array(const Scalar&), Array(const Scalar&,const Scalar&)
+   */
+  template <typename... ArgTypes>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3,
+                                              const ArgTypes&... args)
       : Base(a0, a1, a2, a3, args...) {}
 
-    /** \brief Constructs an array and initializes it from the coefficients given as initializer-lists grouped by row. \cpp11
-      *
-      * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients:
-      *
-      * Example: \include Array_initializer_list_23_cxx11.cpp
-      * Output: \verbinclude Array_initializer_list_23_cxx11.out
-      *
-      * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is triggered.
-      *
-      * In the case of a compile-time column 1D array, implicit transposition from a single row is allowed.
-      * Therefore <code> Array<int,Dynamic,1>{{1,2,3,4,5}}</code> is legal and the more verbose syntax
-      * <code>Array<int,Dynamic,1>{{1},{2},{3},{4},{5}}</code> can be avoided:
-      *
-      * Example: \include Array_initializer_list_vector_cxx11.cpp
-      * Output: \verbinclude Array_initializer_list_vector_cxx11.out
-      *
-      * In the case of fixed-sized arrays, the initializer list sizes must exactly match the array sizes,
-      * and implicit transposition is allowed for compile-time 1D arrays only.
-      *
-      * \sa  Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array(const std::initializer_list<std::initializer_list<Scalar>>& list) : Base(list) {}
-    #endif // end EIGEN_HAS_CXX11
+  /** \brief Constructs an array and initializes it from the coefficients given as initializer-lists grouped by row.
+   * \cpp11
+   *
+   * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients:
+   *
+   * Example: \include Array_initializer_list_23_cxx11.cpp
+   * Output: \verbinclude Array_initializer_list_23_cxx11.out
+   *
+   * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is
+   * triggered.
+   *
+   * In the case of a compile-time column 1D array, implicit transposition from a single row is allowed.
+   * Therefore <code> Array<int,Dynamic,1>{{1,2,3,4,5}}</code> is legal and the more verbose syntax
+   * <code>Array<int,Dynamic,1>{{1},{2},{3},{4},{5}}</code> can be avoided:
+   *
+   * Example: \include Array_initializer_list_vector_cxx11.cpp
+   * Output: \verbinclude Array_initializer_list_vector_cxx11.out
+   *
+   * In the case of fixed-sized arrays, the initializer list sizes must exactly match the array sizes,
+   * and implicit transposition is allowed for compile-time 1D arrays only.
+   *
+   * \sa  Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Array(
+      const std::initializer_list<std::initializer_list<Scalar>>& list)
+      : Base(list) {}
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE explicit Array(const T& x)
-    {
-      Base::_check_template_params();
-      Base::template _init1<T>(x);
-    }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Array(const T& x) {
+    Base::template _init1<T>(x);
+  }
 
-    template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array(const T0& val0, const T1& val1)
-    {
-      Base::_check_template_params();
-      this->template _init2<T0,T1>(val0, val1);
-    }
+  template <typename T0, typename T1>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const T0& val0, const T1& val1) {
+    this->template _init2<T0, T1>(val0, val1);
+  }
 
-    #else
-    /** \brief Constructs a fixed-sized array initialized with coefficients starting at \a data */
-    EIGEN_DEVICE_FUNC explicit Array(const Scalar *data);
-    /** Constructs a vector or row-vector with given dimension. \only_for_vectors
-      *
-      * Note that this is only useful for dynamic-size vectors. For fixed-size vectors,
-      * it is redundant to pass the dimension here, so it makes more sense to use the default
-      * constructor Array() instead.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE explicit Array(Index dim);
-    /** constructs an initialized 1x1 Array with the given coefficient
-      * \sa const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args */
-    Array(const Scalar& value);
-    /** constructs an uninitialized array with \a rows rows and \a cols columns.
-      *
-      * This is useful for dynamic-size arrays. For fixed-size arrays,
-      * it is redundant to pass these parameters, so one should use the default constructor
-      * Array() instead. */
-    Array(Index rows, Index cols);
-    /** constructs an initialized 2D vector with given coefficients
-      * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) */
-    Array(const Scalar& val0, const Scalar& val1);
-    #endif  // end EIGEN_PARSED_BY_DOXYGEN
+#else
+  /** \brief Constructs a fixed-sized array initialized with coefficients starting at \a data */
+  EIGEN_DEVICE_FUNC explicit Array(const Scalar* data);
+  /** Constructs a vector or row-vector with given dimension. \only_for_vectors
+   *
+   * Note that this is only useful for dynamic-size vectors. For fixed-size vectors,
+   * it is redundant to pass the dimension here, so it makes more sense to use the default
+   * constructor Array() instead.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Array(Index dim);
+  /** constructs an initialized 1x1 Array with the given coefficient
+   * \sa const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args */
+  Array(const Scalar& value);
+  /** constructs an uninitialized array with \a rows rows and \a cols columns.
+   *
+   * This is useful for dynamic-size arrays. For fixed-size arrays,
+   * it is redundant to pass these parameters, so one should use the default constructor
+   * Array() instead. */
+  Array(Index rows, Index cols);
+  /** constructs an initialized 2D vector with given coefficients
+   * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) */
+  Array(const Scalar& val0, const Scalar& val1);
+#endif  // end EIGEN_PARSED_BY_DOXYGEN
 
-    /** constructs an initialized 3D vector with given coefficients
-      * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2)
-    {
-      Base::_check_template_params();
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 3)
-      m_storage.data()[0] = val0;
-      m_storage.data()[1] = val1;
-      m_storage.data()[2] = val2;
-    }
-    /** constructs an initialized 4D vector with given coefficients
-      * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2, const Scalar& val3)
-    {
-      Base::_check_template_params();
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 4)
-      m_storage.data()[0] = val0;
-      m_storage.data()[1] = val1;
-      m_storage.data()[2] = val2;
-      m_storage.data()[3] = val3;
-    }
+  /** constructs an initialized 3D vector with given coefficients
+   * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 3)
+    m_storage.data()[0] = val0;
+    m_storage.data()[1] = val1;
+    m_storage.data()[2] = val2;
+  }
+  /** constructs an initialized 4D vector with given coefficients
+   * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2,
+                                              const Scalar& val3) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 4)
+    m_storage.data()[0] = val0;
+    m_storage.data()[1] = val1;
+    m_storage.data()[2] = val2;
+    m_storage.data()[3] = val3;
+  }
 
-    /** Copy constructor */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array(const Array& other)
-            : Base(other)
-    { }
+  /** Copy constructor */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Array& other) : Base(other) {}
 
-  private:
-    struct PrivateType {};
-  public:
+ private:
+  struct PrivateType {};
 
-    /** \sa MatrixBase::operator=(const EigenBase<OtherDerived>&) */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Array(const EigenBase<OtherDerived> &other,
-                              typename internal::enable_if<internal::is_convertible<typename OtherDerived::Scalar,Scalar>::value,
-                                                           PrivateType>::type = PrivateType())
-      : Base(other.derived())
-    { }
+ public:
+  /** \sa MatrixBase::operator=(const EigenBase<OtherDerived>&) */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(
+      const EigenBase<OtherDerived>& other,
+      std::enable_if_t<internal::is_convertible<typename OtherDerived::Scalar, Scalar>::value, PrivateType> =
+          PrivateType())
+      : Base(other.derived()) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT{ return 1; }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return 1; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); }
 
-    #ifdef EIGEN_ARRAY_PLUGIN
-    #include EIGEN_ARRAY_PLUGIN
-    #endif
+#ifdef EIGEN_ARRAY_PLUGIN
+#include EIGEN_ARRAY_PLUGIN
+#endif
 
-  private:
-
-    template<typename MatrixType, typename OtherDerived, bool SwapPointers>
-    friend struct internal::matrix_swap_impl;
+ private:
+  template <typename MatrixType, typename OtherDerived, bool SwapPointers>
+  friend struct internal::matrix_swap_impl;
 };
 
 /** \defgroup arraytypedefs Global array typedefs
-  * \ingroup Core_Module
-  *
-  * %Eigen defines several typedef shortcuts for most common 1D and 2D array types.
-  *
-  * The general patterns are the following:
-  *
-  * \c ArrayRowsColsType where \c Rows and \c Cols can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size,
-  * and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c cd
-  * for complex double.
-  *
-  * For example, \c Array33d is a fixed-size 3x3 array type of doubles, and \c ArrayXXf is a dynamic-size matrix of floats.
-  *
-  * There are also \c ArraySizeType which are self-explanatory. For example, \c Array4cf is
-  * a fixed-size 1D array of 4 complex floats.
-  *
-  * With \cpp11, template alias are also defined for common sizes.
-  * They follow the same pattern as above except that the scalar type suffix is replaced by a
-  * template parameter, i.e.:
-  *   - `ArrayRowsCols<Type>` where `Rows` and `Cols` can be \c 2,\c 3,\c 4, or \c X for fixed or dynamic size.
-  *   - `ArraySize<Type>` where `Size` can be \c 2,\c 3,\c 4 or \c X for fixed or dynamic size 1D arrays.
-  *
-  * \sa class Array
-  */
+ * \ingroup Core_Module
+ *
+ * %Eigen defines several typedef shortcuts for most common 1D and 2D array types.
+ *
+ * The general patterns are the following:
+ *
+ * \c ArrayRowsColsType where \c Rows and \c Cols can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for
+ * dynamic size, and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c
+ * cd for complex double.
+ *
+ * For example, \c Array33d is a fixed-size 3x3 array type of doubles, and \c ArrayXXf is a dynamic-size matrix of
+ * floats.
+ *
+ * There are also \c ArraySizeType which are self-explanatory. For example, \c Array4cf is
+ * a fixed-size 1D array of 4 complex floats.
+ *
+ * With \cpp11, template alias are also defined for common sizes.
+ * They follow the same pattern as above except that the scalar type suffix is replaced by a
+ * template parameter, i.e.:
+ *   - `ArrayRowsCols<Type>` where `Rows` and `Cols` can be \c 2,\c 3,\c 4, or \c X for fixed or dynamic size.
+ *   - `ArraySize<Type>` where `Size` can be \c 2,\c 3,\c 4 or \c X for fixed or dynamic size 1D arrays.
+ *
+ * \sa class Array
+ */
 
-#define EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)   \
-/** \ingroup arraytypedefs */                                    \
-typedef Array<Type, Size, Size> Array##SizeSuffix##SizeSuffix##TypeSuffix;  \
-/** \ingroup arraytypedefs */                                    \
-typedef Array<Type, Size, 1>    Array##SizeSuffix##TypeSuffix;
+#define EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)        \
+  /** \ingroup arraytypedefs */                                              \
+  typedef Array<Type, Size, Size> Array##SizeSuffix##SizeSuffix##TypeSuffix; \
+  /** \ingroup arraytypedefs */                                              \
+  typedef Array<Type, Size, 1> Array##SizeSuffix##TypeSuffix;
 
-#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, Size)         \
-/** \ingroup arraytypedefs */                                    \
-typedef Array<Type, Size, Dynamic> Array##Size##X##TypeSuffix;  \
-/** \ingroup arraytypedefs */                                    \
-typedef Array<Type, Dynamic, Size> Array##X##Size##TypeSuffix;
+#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, Size)  \
+  /** \ingroup arraytypedefs */                                  \
+  typedef Array<Type, Size, Dynamic> Array##Size##X##TypeSuffix; \
+  /** \ingroup arraytypedefs */                                  \
+  typedef Array<Type, Dynamic, Size> Array##X##Size##TypeSuffix;
 
 #define EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
-EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 2, 2) \
-EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 3, 3) \
-EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 4, 4) \
-EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Dynamic, X) \
-EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 2) \
-EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 3) \
-EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 4)
+  EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 2, 2)           \
+  EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 3, 3)           \
+  EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 4, 4)           \
+  EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Dynamic, X)     \
+  EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 2)        \
+  EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 3)        \
+  EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 4)
 
-EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(int,                  i)
-EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(float,                f)
-EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(double,               d)
-EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(std::complex<float>,  cf)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(int, i)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(float, f)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(double, d)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
 EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
 
 #undef EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES
 #undef EIGEN_MAKE_ARRAY_TYPEDEFS
 #undef EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS
 
-#if EIGEN_HAS_CXX11
+#define EIGEN_MAKE_ARRAY_TYPEDEFS(Size, SizeSuffix)              \
+  /** \ingroup arraytypedefs */                                  \
+  /** \brief \cpp11 */                                           \
+  template <typename Type>                                       \
+  using Array##SizeSuffix##SizeSuffix = Array<Type, Size, Size>; \
+  /** \ingroup arraytypedefs */                                  \
+  /** \brief \cpp11 */                                           \
+  template <typename Type>                                       \
+  using Array##SizeSuffix = Array<Type, Size, 1>;
 
-#define EIGEN_MAKE_ARRAY_TYPEDEFS(Size, SizeSuffix)               \
-/** \ingroup arraytypedefs */                                     \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Array##SizeSuffix##SizeSuffix = Array<Type, Size, Size>;    \
-/** \ingroup arraytypedefs */                                     \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Array##SizeSuffix = Array<Type, Size, 1>;
-
-#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Size)                     \
-/** \ingroup arraytypedefs */                                     \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Array##Size##X = Array<Type, Size, Dynamic>;                \
-/** \ingroup arraytypedefs */                                     \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Array##X##Size = Array<Type, Dynamic, Size>;
+#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Size)        \
+  /** \ingroup arraytypedefs */                      \
+  /** \brief \cpp11 */                               \
+  template <typename Type>                           \
+  using Array##Size##X = Array<Type, Size, Dynamic>; \
+  /** \ingroup arraytypedefs */                      \
+  /** \brief \cpp11 */                               \
+  template <typename Type>                           \
+  using Array##X##Size = Array<Type, Dynamic, Size>;
 
 EIGEN_MAKE_ARRAY_TYPEDEFS(2, 2)
 EIGEN_MAKE_ARRAY_TYPEDEFS(3, 3)
@@ -392,26 +346,24 @@
 #undef EIGEN_MAKE_ARRAY_TYPEDEFS
 #undef EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS
 
-#endif // EIGEN_HAS_CXX11
-
 #define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
-using Eigen::Matrix##SizeSuffix##TypeSuffix; \
-using Eigen::Vector##SizeSuffix##TypeSuffix; \
-using Eigen::RowVector##SizeSuffix##TypeSuffix;
+  using Eigen::Matrix##SizeSuffix##TypeSuffix;                               \
+  using Eigen::Vector##SizeSuffix##TypeSuffix;                               \
+  using Eigen::RowVector##SizeSuffix##TypeSuffix;
 
-#define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(TypeSuffix) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \
+#define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(TypeSuffix)       \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X)
 
-#define EIGEN_USING_ARRAY_TYPEDEFS \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(i) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(f) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(d) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cf) \
-EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cd)
+#define EIGEN_USING_ARRAY_TYPEDEFS        \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(i)  \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(f)  \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(d)  \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cf) \
+  EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cd)
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ARRAY_H
+#endif  // EIGEN_ARRAY_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h
index ea3dd1c..6237df4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h
@@ -10,217 +10,213 @@
 #ifndef EIGEN_ARRAYBASE_H
 #define EIGEN_ARRAYBASE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-template<typename ExpressionType> class MatrixWrapper;
+namespace Eigen {
+
+template <typename ExpressionType>
+class MatrixWrapper;
 
 /** \class ArrayBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for all 1D and 2D array, and related expressions
-  *
-  * An array is similar to a dense vector or matrix. While matrices are mathematical
-  * objects with well defined linear algebra operators, an array is just a collection
-  * of scalar values arranged in a one or two dimensionnal fashion. As the main consequence,
-  * all operations applied to an array are performed coefficient wise. Furthermore,
-  * arrays support scalar math functions of the c++ standard library (e.g., std::sin(x)), and convenient
-  * constructors allowing to easily write generic code working for both scalar values
-  * and arrays.
-  *
-  * This class is the base that is inherited by all array expression types.
-  *
-  * \tparam Derived is the derived type, e.g., an array or an expression type.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAYBASE_PLUGIN.
-  *
-  * \sa class MatrixBase, \ref TopicClassHierarchy
-  */
-template<typename Derived> class ArrayBase
-  : public DenseBase<Derived>
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Base class for all 1D and 2D array, and related expressions
+ *
+ * An array is similar to a dense vector or matrix. While matrices are mathematical
+ * objects with well defined linear algebra operators, an array is just a collection
+ * of scalar values arranged in a one or two dimensional fashion. As the main consequence,
+ * all operations applied to an array are performed coefficient wise. Furthermore,
+ * arrays support scalar math functions of the c++ standard library (e.g., std::sin(x)), and convenient
+ * constructors allowing to easily write generic code working for both scalar values
+ * and arrays.
+ *
+ * This class is the base that is inherited by all array expression types.
+ *
+ * \tparam Derived is the derived type, e.g., an array or an expression type.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAYBASE_PLUGIN.
+ *
+ * \sa class MatrixBase, \ref TopicClassHierarchy
+ */
+template <typename Derived>
+class ArrayBase : public DenseBase<Derived> {
+ public:
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** The base class for a given storage type. */
-    typedef ArrayBase StorageBaseType;
+  /** The base class for a given storage type. */
+  typedef ArrayBase StorageBaseType;
 
-    typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl;
+  typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl;
 
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    typedef DenseBase<Derived> Base;
-    using Base::RowsAtCompileTime;
-    using Base::ColsAtCompileTime;
-    using Base::SizeAtCompileTime;
-    using Base::MaxRowsAtCompileTime;
-    using Base::MaxColsAtCompileTime;
-    using Base::MaxSizeAtCompileTime;
-    using Base::IsVectorAtCompileTime;
-    using Base::Flags;
-    
-    using Base::derived;
-    using Base::const_cast_derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
-    using Base::lazyAssign;
-    using Base::operator-;
-    using Base::operator=;
-    using Base::operator+=;
-    using Base::operator-=;
-    using Base::operator*=;
-    using Base::operator/=;
+  typedef DenseBase<Derived> Base;
+  using Base::ColsAtCompileTime;
+  using Base::Flags;
+  using Base::IsVectorAtCompileTime;
+  using Base::MaxColsAtCompileTime;
+  using Base::MaxRowsAtCompileTime;
+  using Base::MaxSizeAtCompileTime;
+  using Base::RowsAtCompileTime;
+  using Base::SizeAtCompileTime;
 
-    typedef typename Base::CoeffReturnType CoeffReturnType;
+  using Base::coeff;
+  using Base::coeffRef;
+  using Base::cols;
+  using Base::const_cast_derived;
+  using Base::derived;
+  using Base::lazyAssign;
+  using Base::rows;
+  using Base::size;
+  using Base::operator-;
+  using Base::operator=;
+  using Base::operator+=;
+  using Base::operator-=;
+  using Base::operator*=;
+  using Base::operator/=;
 
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+  typedef typename Base::CoeffReturnType CoeffReturnType;
+
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename Base::PlainObject PlainObject;
+  typedef typename Base::PlainObject PlainObject;
 
-    /** \internal Represents a matrix with all coefficients equal to one another*/
-    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+  /** \internal Represents a matrix with all coefficients equal to one another*/
+  typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> ConstantReturnType;
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::ArrayBase
-#define EIGEN_DOC_UNARY_ADDONS(X,Y)
-#   include "../plugins/MatrixCwiseUnaryOps.h"
-#   include "../plugins/ArrayCwiseUnaryOps.h"
-#   include "../plugins/CommonCwiseBinaryOps.h"
-#   include "../plugins/MatrixCwiseBinaryOps.h"
-#   include "../plugins/ArrayCwiseBinaryOps.h"
-#   ifdef EIGEN_ARRAYBASE_PLUGIN
-#     include EIGEN_ARRAYBASE_PLUGIN
-#   endif
+#define EIGEN_DOC_UNARY_ADDONS(X, Y)
+#include "../plugins/MatrixCwiseUnaryOps.inc"
+#include "../plugins/ArrayCwiseUnaryOps.inc"
+#include "../plugins/CommonCwiseBinaryOps.inc"
+#include "../plugins/MatrixCwiseBinaryOps.inc"
+#include "../plugins/ArrayCwiseBinaryOps.inc"
+#ifdef EIGEN_ARRAYBASE_PLUGIN
+#include EIGEN_ARRAYBASE_PLUGIN
+#endif
 #undef EIGEN_CURRENT_STORAGE_BASE_CLASS
 #undef EIGEN_DOC_UNARY_ADDONS
 
-    /** Special case of the template operator=, in order to prevent the compiler
-      * from generating a default operator= (issue hit with g++ 4.1)
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const ArrayBase& other)
-    {
-      internal::call_assignment(derived(), other.derived());
-      return derived();
-    }
-    
-    /** Set all the entries to \a value.
-      * \sa DenseBase::setConstant(), DenseBase::fill() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const Scalar &value)
-    { Base::setConstant(value); return derived(); }
+  /** Special case of the template operator=, in order to prevent the compiler
+   * from generating a default operator= (issue hit with g++ 4.1)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const ArrayBase& other) {
+    internal::call_assignment(derived(), other.derived());
+    return derived();
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator+=(const Scalar& scalar);
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator-=(const Scalar& scalar);
+  /** Set all the entries to \a value.
+   * \sa DenseBase::setConstant(), DenseBase::fill() */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Scalar& value) {
+    Base::setConstant(value);
+    return derived();
+  }
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator+=(const ArrayBase<OtherDerived>& other);
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator-=(const ArrayBase<OtherDerived>& other);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const Scalar& scalar);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const Scalar& scalar);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator*=(const ArrayBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const ArrayBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const ArrayBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator/=(const ArrayBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const ArrayBase<OtherDerived>& other);
 
-  public:
-    EIGEN_DEVICE_FUNC
-    ArrayBase<Derived>& array() { return *this; }
-    EIGEN_DEVICE_FUNC
-    const ArrayBase<Derived>& array() const { return *this; }
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const ArrayBase<OtherDerived>& other);
 
-    /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array
-      * \sa MatrixBase::array() */
-    EIGEN_DEVICE_FUNC
-    MatrixWrapper<Derived> matrix() { return MatrixWrapper<Derived>(derived()); }
-    EIGEN_DEVICE_FUNC
-    const MatrixWrapper<const Derived> matrix() const { return MatrixWrapper<const Derived>(derived()); }
+ public:
+  EIGEN_DEVICE_FUNC ArrayBase<Derived>& array() { return *this; }
+  EIGEN_DEVICE_FUNC const ArrayBase<Derived>& array() const { return *this; }
 
-//     template<typename Dest>
-//     inline void evalTo(Dest& dst) const { dst = matrix(); }
+  /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array
+   * \sa MatrixBase::array() */
+  EIGEN_DEVICE_FUNC MatrixWrapper<Derived> matrix() { return MatrixWrapper<Derived>(derived()); }
+  EIGEN_DEVICE_FUNC const MatrixWrapper<const Derived> matrix() const {
+    return MatrixWrapper<const Derived>(derived());
+  }
 
-  protected:
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(ArrayBase)
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(ArrayBase)
+  //     template<typename Dest>
+  //     inline void evalTo(Dest& dst) const { dst = matrix(); }
 
-  private:
-    explicit ArrayBase(Index);
-    ArrayBase(Index,Index);
-    template<typename OtherDerived> explicit ArrayBase(const ArrayBase<OtherDerived>&);
-  protected:
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator+=(const MatrixBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator-=(const MatrixBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+ protected:
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(ArrayBase)
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(ArrayBase)
+
+ private:
+  explicit ArrayBase(Index);
+  ArrayBase(Index, Index);
+  template <typename OtherDerived>
+  explicit ArrayBase(const ArrayBase<OtherDerived>&);
+
+ protected:
+  // mixing arrays and matrices is not legal
+  template <typename OtherDerived>
+  Derived& operator+=(const MatrixBase<OtherDerived>&) {
+    EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
+                        YOU_CANNOT_MIX_ARRAYS_AND_MATRICES);
+    return *this;
+  }
+  // mixing arrays and matrices is not legal
+  template <typename OtherDerived>
+  Derived& operator-=(const MatrixBase<OtherDerived>&) {
+    EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
+                        YOU_CANNOT_MIX_ARRAYS_AND_MATRICES);
+    return *this;
+  }
 };
 
 /** replaces \c *this by \c *this - \a other.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator-=(const ArrayBase<OtherDerived> &other)
-{
-  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+ *
+ * \returns a reference to \c *this
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator-=(const ArrayBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
 /** replaces \c *this by \c *this + \a other.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator+=(const ArrayBase<OtherDerived>& other)
-{
-  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+ *
+ * \returns a reference to \c *this
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator+=(const ArrayBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
 /** replaces \c *this by \c *this * \a other coefficient wise.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator*=(const ArrayBase<OtherDerived>& other)
-{
-  call_assignment(derived(), other.derived(), internal::mul_assign_op<Scalar,typename OtherDerived::Scalar>());
+ *
+ * \returns a reference to \c *this
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator*=(const ArrayBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::mul_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
 /** replaces \c *this by \c *this / \a other coefficient wise.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator/=(const ArrayBase<OtherDerived>& other)
-{
-  call_assignment(derived(), other.derived(), internal::div_assign_op<Scalar,typename OtherDerived::Scalar>());
+ *
+ * \returns a reference to \c *this
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator/=(const ArrayBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::div_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ARRAYBASE_H
+#endif  // EIGEN_ARRAYBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h
index 2e9555b..b45395d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h
@@ -10,200 +10,164 @@
 #ifndef EIGEN_ARRAYWRAPPER_H
 #define EIGEN_ARRAYWRAPPER_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class ArrayWrapper
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a mathematical vector or matrix as an array object
-  *
-  * This class is the return type of MatrixBase::array(), and most of the time
-  * this is the only way it is use.
-  *
-  * \sa MatrixBase::array(), class MatrixWrapper
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a mathematical vector or matrix as an array object
+ *
+ * This class is the return type of MatrixBase::array(), and most of the time
+ * this is the only way it is use.
+ *
+ * \sa MatrixBase::array(), class MatrixWrapper
+ */
 
 namespace internal {
-template<typename ExpressionType>
-struct traits<ArrayWrapper<ExpressionType> >
-  : public traits<typename remove_all<typename ExpressionType::Nested>::type >
-{
+template <typename ExpressionType>
+struct traits<ArrayWrapper<ExpressionType> > : public traits<remove_all_t<typename ExpressionType::Nested> > {
   typedef ArrayXpr XprKind;
   // Let's remove NestByRefBit
   enum {
-    Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
+    Flags0 = traits<remove_all_t<typename ExpressionType::Nested> >::Flags,
     LvalueBitFlag = is_lvalue<ExpressionType>::value ? LvalueBit : 0,
     Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag
   };
 };
-}
+}  // namespace internal
 
-template<typename ExpressionType>
-class ArrayWrapper : public ArrayBase<ArrayWrapper<ExpressionType> >
-{
-  public:
-    typedef ArrayBase<ArrayWrapper> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(ArrayWrapper)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ArrayWrapper)
-    typedef typename internal::remove_all<ExpressionType>::type NestedExpression;
+template <typename ExpressionType>
+class ArrayWrapper : public ArrayBase<ArrayWrapper<ExpressionType> > {
+ public:
+  typedef ArrayBase<ArrayWrapper> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(ArrayWrapper)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ArrayWrapper)
+  typedef internal::remove_all_t<ExpressionType> NestedExpression;
 
-    typedef typename internal::conditional<
-                       internal::is_lvalue<ExpressionType>::value,
-                       Scalar,
-                       const Scalar
-                     >::type ScalarWithConstIfNotLvalue;
+  typedef std::conditional_t<internal::is_lvalue<ExpressionType>::value, Scalar, const Scalar>
+      ScalarWithConstIfNotLvalue;
 
-    typedef typename internal::ref_selector<ExpressionType>::non_const_type NestedExpressionType;
+  typedef typename internal::ref_selector<ExpressionType>::non_const_type NestedExpressionType;
 
-    using Base::coeffRef;
+  using Base::coeffRef;
 
-    EIGEN_DEVICE_FUNC
-    explicit EIGEN_STRONG_INLINE ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {}
+  EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return m_expression.outerStride(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT { return m_expression.innerStride(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT {
+    return m_expression.outerStride();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT {
+    return m_expression.innerStride();
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
-    EIGEN_DEVICE_FUNC
-    inline const Scalar* data() const { return m_expression.data(); }
+  EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_expression.data(); }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      return m_expression.coeffRef(rowId, colId);
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const {
+    return m_expression.coeffRef(rowId, colId);
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index index) const
-    {
-      return m_expression.coeffRef(index);
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { return m_expression.coeffRef(index); }
 
-    template<typename Dest>
-    EIGEN_DEVICE_FUNC
-    inline void evalTo(Dest& dst) const { dst = m_expression; }
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const {
+    dst = m_expression;
+  }
 
-    EIGEN_DEVICE_FUNC
-    const typename internal::remove_all<NestedExpressionType>::type&
-    nestedExpression() const
-    {
-      return m_expression;
-    }
+  EIGEN_DEVICE_FUNC const internal::remove_all_t<NestedExpressionType>& nestedExpression() const {
+    return m_expression;
+  }
 
-    /** Forwards the resizing request to the nested expression
-      * \sa DenseBase::resize(Index)  */
-    EIGEN_DEVICE_FUNC
-    void resize(Index newSize) { m_expression.resize(newSize); }
-    /** Forwards the resizing request to the nested expression
-      * \sa DenseBase::resize(Index,Index)*/
-    EIGEN_DEVICE_FUNC
-    void resize(Index rows, Index cols) { m_expression.resize(rows,cols); }
+  /** Forwards the resizing request to the nested expression
+   * \sa DenseBase::resize(Index)  */
+  EIGEN_DEVICE_FUNC void resize(Index newSize) { m_expression.resize(newSize); }
+  /** Forwards the resizing request to the nested expression
+   * \sa DenseBase::resize(Index,Index)*/
+  EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) { m_expression.resize(rows, cols); }
 
-  protected:
-    NestedExpressionType m_expression;
+ protected:
+  NestedExpressionType m_expression;
 };
 
 /** \class MatrixWrapper
-  * \ingroup Core_Module
-  *
-  * \brief Expression of an array as a mathematical vector or matrix
-  *
-  * This class is the return type of ArrayBase::matrix(), and most of the time
-  * this is the only way it is use.
-  *
-  * \sa MatrixBase::matrix(), class ArrayWrapper
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Expression of an array as a mathematical vector or matrix
+ *
+ * This class is the return type of ArrayBase::matrix(), and most of the time
+ * this is the only way it is use.
+ *
+ * \sa MatrixBase::matrix(), class ArrayWrapper
+ */
 
 namespace internal {
-template<typename ExpressionType>
-struct traits<MatrixWrapper<ExpressionType> >
- : public traits<typename remove_all<typename ExpressionType::Nested>::type >
-{
+template <typename ExpressionType>
+struct traits<MatrixWrapper<ExpressionType> > : public traits<remove_all_t<typename ExpressionType::Nested> > {
   typedef MatrixXpr XprKind;
   // Let's remove NestByRefBit
   enum {
-    Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
+    Flags0 = traits<remove_all_t<typename ExpressionType::Nested> >::Flags,
     LvalueBitFlag = is_lvalue<ExpressionType>::value ? LvalueBit : 0,
     Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag
   };
 };
-}
+}  // namespace internal
 
-template<typename ExpressionType>
-class MatrixWrapper : public MatrixBase<MatrixWrapper<ExpressionType> >
-{
-  public:
-    typedef MatrixBase<MatrixWrapper<ExpressionType> > Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(MatrixWrapper)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MatrixWrapper)
-    typedef typename internal::remove_all<ExpressionType>::type NestedExpression;
+template <typename ExpressionType>
+class MatrixWrapper : public MatrixBase<MatrixWrapper<ExpressionType> > {
+ public:
+  typedef MatrixBase<MatrixWrapper<ExpressionType> > Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(MatrixWrapper)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MatrixWrapper)
+  typedef internal::remove_all_t<ExpressionType> NestedExpression;
 
-    typedef typename internal::conditional<
-                       internal::is_lvalue<ExpressionType>::value,
-                       Scalar,
-                       const Scalar
-                     >::type ScalarWithConstIfNotLvalue;
+  typedef std::conditional_t<internal::is_lvalue<ExpressionType>::value, Scalar, const Scalar>
+      ScalarWithConstIfNotLvalue;
 
-    typedef typename internal::ref_selector<ExpressionType>::non_const_type NestedExpressionType;
+  typedef typename internal::ref_selector<ExpressionType>::non_const_type NestedExpressionType;
 
-    using Base::coeffRef;
+  using Base::coeffRef;
 
-    EIGEN_DEVICE_FUNC
-    explicit inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {}
+  EIGEN_DEVICE_FUNC explicit inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return m_expression.outerStride(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT { return m_expression.innerStride(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT {
+    return m_expression.outerStride();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT {
+    return m_expression.innerStride();
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
-    EIGEN_DEVICE_FUNC
-    inline const Scalar* data() const { return m_expression.data(); }
+  EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_expression.data(); }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      return m_expression.derived().coeffRef(rowId, colId);
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const {
+    return m_expression.derived().coeffRef(rowId, colId);
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index index) const
-    {
-      return m_expression.coeffRef(index);
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { return m_expression.coeffRef(index); }
 
-    EIGEN_DEVICE_FUNC
-    const typename internal::remove_all<NestedExpressionType>::type&
-    nestedExpression() const
-    {
-      return m_expression;
-    }
+  EIGEN_DEVICE_FUNC const internal::remove_all_t<NestedExpressionType>& nestedExpression() const {
+    return m_expression;
+  }
 
-    /** Forwards the resizing request to the nested expression
-      * \sa DenseBase::resize(Index)  */
-    EIGEN_DEVICE_FUNC
-    void resize(Index newSize) { m_expression.resize(newSize); }
-    /** Forwards the resizing request to the nested expression
-      * \sa DenseBase::resize(Index,Index)*/
-    EIGEN_DEVICE_FUNC
-    void resize(Index rows, Index cols) { m_expression.resize(rows,cols); }
+  /** Forwards the resizing request to the nested expression
+   * \sa DenseBase::resize(Index)  */
+  EIGEN_DEVICE_FUNC void resize(Index newSize) { m_expression.resize(newSize); }
+  /** Forwards the resizing request to the nested expression
+   * \sa DenseBase::resize(Index,Index)*/
+  EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) { m_expression.resize(rows, cols); }
 
-  protected:
-    NestedExpressionType m_expression;
+ protected:
+  NestedExpressionType m_expression;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ARRAYWRAPPER_H
+#endif  // EIGEN_ARRAYWRAPPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h
index 655412e..4b30f7b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h
@@ -12,79 +12,69 @@
 #ifndef EIGEN_ASSIGN_H
 #define EIGEN_ASSIGN_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>
-  ::lazyAssign(const DenseBase<OtherDerived>& other)
-{
-  enum{
-    SameType = internal::is_same<typename Derived::Scalar,typename OtherDerived::Scalar>::value
-  };
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::lazyAssign(const DenseBase<OtherDerived>& other) {
+  enum { SameType = internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value };
 
   EIGEN_STATIC_ASSERT_LVALUE(Derived)
-  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
-  EIGEN_STATIC_ASSERT(SameType,YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived, OtherDerived)
+  EIGEN_STATIC_ASSERT(
+      SameType,
+      YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
 
   eigen_assert(rows() == other.rows() && cols() == other.cols());
-  internal::call_assignment_no_alias(derived(),other.derived());
-  
+  internal::call_assignment_no_alias(derived(), other.derived());
+
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator=(const DenseBase<OtherDerived>& other)
-{
-  internal::call_assignment(derived(), other.derived());
-  return derived();
-}
-
-template<typename Derived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator=(const DenseBase& other)
-{
-  internal::call_assignment(derived(), other.derived());
-  return derived();
-}
-
-template<typename Derived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const MatrixBase& other)
-{
-  internal::call_assignment(derived(), other.derived());
-  return derived();
-}
-
-template<typename Derived>
+template <typename Derived>
 template <typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const DenseBase<OtherDerived>& other)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator=(const DenseBase<OtherDerived>& other) {
   internal::call_assignment(derived(), other.derived());
   return derived();
 }
 
-template<typename Derived>
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator=(const DenseBase& other) {
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const MatrixBase& other) {
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template <typename Derived>
 template <typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const EigenBase<OtherDerived>& other)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const DenseBase<OtherDerived>& other) {
   internal::call_assignment(derived(), other.derived());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
-{
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const EigenBase<OtherDerived>& other) {
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(
+    const ReturnByValue<OtherDerived>& other) {
   other.derived().evalTo(derived());
   return derived();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ASSIGN_H
+#endif  // EIGEN_ASSIGN_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h
index 7d76f0c..f7f0b23 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h
@@ -12,6 +12,9 @@
 #ifndef EIGEN_ASSIGN_EVALUATOR_H
 #define EIGEN_ASSIGN_EVALUATOR_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 // This implementation is based on Assign.h
@@ -19,143 +22,140 @@
 namespace internal {
 
 /***************************************************************************
-* Part 1 : the logic deciding a strategy for traversal and unrolling       *
-***************************************************************************/
+ * Part 1 : the logic deciding a strategy for traversal and unrolling       *
+ ***************************************************************************/
 
 // copy_using_evaluator_traits is based on assign_traits
 
 template <typename DstEvaluator, typename SrcEvaluator, typename AssignFunc, int MaxPacketSize = -1>
-struct copy_using_evaluator_traits
-{
+struct copy_using_evaluator_traits {
   typedef typename DstEvaluator::XprType Dst;
   typedef typename Dst::Scalar DstScalar;
 
-  enum {
-    DstFlags = DstEvaluator::Flags,
-    SrcFlags = SrcEvaluator::Flags
-  };
+  enum { DstFlags = DstEvaluator::Flags, SrcFlags = SrcEvaluator::Flags };
 
-public:
+ public:
   enum {
     DstAlignment = DstEvaluator::Alignment,
     SrcAlignment = SrcEvaluator::Alignment,
     DstHasDirectAccess = (DstFlags & DirectAccessBit) == DirectAccessBit,
-    JointAlignment = EIGEN_PLAIN_ENUM_MIN(DstAlignment,SrcAlignment)
+    JointAlignment = plain_enum_min(DstAlignment, SrcAlignment)
   };
 
-private:
+ private:
   enum {
     InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime)
-              : int(DstFlags)&RowMajorBit ? int(Dst::ColsAtCompileTime)
-              : int(Dst::RowsAtCompileTime),
+                : int(DstFlags) & RowMajorBit   ? int(Dst::ColsAtCompileTime)
+                                                : int(Dst::RowsAtCompileTime),
     InnerMaxSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime)
-              : int(DstFlags)&RowMajorBit ? int(Dst::MaxColsAtCompileTime)
-              : int(Dst::MaxRowsAtCompileTime),
-    RestrictedInnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(InnerSize,MaxPacketSize),
-    RestrictedLinearSize = EIGEN_SIZE_MIN_PREFER_FIXED(Dst::SizeAtCompileTime,MaxPacketSize),
+                   : int(DstFlags) & RowMajorBit   ? int(Dst::MaxColsAtCompileTime)
+                                                   : int(Dst::MaxRowsAtCompileTime),
+    RestrictedInnerSize = min_size_prefer_fixed(InnerSize, MaxPacketSize),
+    RestrictedLinearSize = min_size_prefer_fixed(Dst::SizeAtCompileTime, MaxPacketSize),
     OuterStride = int(outer_stride_at_compile_time<Dst>::ret),
     MaxSizeAtCompileTime = Dst::SizeAtCompileTime
   };
 
   // TODO distinguish between linear traversal and inner-traversals
-  typedef typename find_best_packet<DstScalar,RestrictedLinearSize>::type LinearPacketType;
-  typedef typename find_best_packet<DstScalar,RestrictedInnerSize>::type InnerPacketType;
+  typedef typename find_best_packet<DstScalar, RestrictedLinearSize>::type LinearPacketType;
+  typedef typename find_best_packet<DstScalar, RestrictedInnerSize>::type InnerPacketType;
 
   enum {
     LinearPacketSize = unpacket_traits<LinearPacketType>::size,
     InnerPacketSize = unpacket_traits<InnerPacketType>::size
   };
 
-public:
+ public:
   enum {
     LinearRequiredAlignment = unpacket_traits<LinearPacketType>::alignment,
     InnerRequiredAlignment = unpacket_traits<InnerPacketType>::alignment
   };
 
-private:
+ private:
   enum {
-    DstIsRowMajor = DstFlags&RowMajorBit,
-    SrcIsRowMajor = SrcFlags&RowMajorBit,
+    DstIsRowMajor = DstFlags & RowMajorBit,
+    SrcIsRowMajor = SrcFlags & RowMajorBit,
     StorageOrdersAgree = (int(DstIsRowMajor) == int(SrcIsRowMajor)),
-    MightVectorize = bool(StorageOrdersAgree)
-                  && (int(DstFlags) & int(SrcFlags) & ActualPacketAccessBit)
-                  && bool(functor_traits<AssignFunc>::PacketAccess),
-    MayInnerVectorize  = MightVectorize
-                       && int(InnerSize)!=Dynamic && int(InnerSize)%int(InnerPacketSize)==0
-                       && int(OuterStride)!=Dynamic && int(OuterStride)%int(InnerPacketSize)==0
-                       && (EIGEN_UNALIGNED_VECTORIZE  || int(JointAlignment)>=int(InnerRequiredAlignment)),
+    MightVectorize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & ActualPacketAccessBit) &&
+                     bool(functor_traits<AssignFunc>::PacketAccess),
+    MayInnerVectorize = MightVectorize && int(InnerSize) != Dynamic && int(InnerSize) % int(InnerPacketSize) == 0 &&
+                        int(OuterStride) != Dynamic && int(OuterStride) % int(InnerPacketSize) == 0 &&
+                        (EIGEN_UNALIGNED_VECTORIZE || int(JointAlignment) >= int(InnerRequiredAlignment)),
     MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit),
-    MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize) && bool(DstHasDirectAccess)
-                       && (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)) || MaxSizeAtCompileTime == Dynamic),
-      /* If the destination isn't aligned, we have to do runtime checks and we don't unroll,
-         so it's only good for large enough sizes. */
-    MaySliceVectorize  = bool(MightVectorize) && bool(DstHasDirectAccess)
-                       && (int(InnerMaxSize)==Dynamic || int(InnerMaxSize)>=(EIGEN_UNALIGNED_VECTORIZE?InnerPacketSize:(3*InnerPacketSize)))
-      /* slice vectorization can be slow, so we only want it if the slices are big, which is
-         indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block
-         in a fixed-size matrix
-         However, with EIGEN_UNALIGNED_VECTORIZE and unrolling, slice vectorization is still worth it */
+    MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize) && bool(DstHasDirectAccess) &&
+                         (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment) >= int(LinearRequiredAlignment)) ||
+                          MaxSizeAtCompileTime == Dynamic),
+    /* If the destination isn't aligned, we have to do runtime checks and we don't unroll,
+       so it's only good for large enough sizes. */
+    MaySliceVectorize = bool(MightVectorize) && bool(DstHasDirectAccess) &&
+                        (int(InnerMaxSize) == Dynamic ||
+                         int(InnerMaxSize) >= (EIGEN_UNALIGNED_VECTORIZE ? InnerPacketSize : (3 * InnerPacketSize)))
+    /* slice vectorization can be slow, so we only want it if the slices are big, which is
+       indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block
+       in a fixed-size matrix
+       However, with EIGEN_UNALIGNED_VECTORIZE and unrolling, slice vectorization is still worth it */
   };
 
-public:
+ public:
   enum {
-    Traversal =  int(Dst::SizeAtCompileTime) == 0 ? int(AllAtOnceTraversal) // If compile-size is zero, traversing will fail at compile-time.
-              : (int(MayLinearVectorize) && (LinearPacketSize>InnerPacketSize)) ? int(LinearVectorizedTraversal)
-              : int(MayInnerVectorize)   ? int(InnerVectorizedTraversal)
-              : int(MayLinearVectorize)  ? int(LinearVectorizedTraversal)
-              : int(MaySliceVectorize)   ? int(SliceVectorizedTraversal)
-              : int(MayLinearize)        ? int(LinearTraversal)
-                                         : int(DefaultTraversal),
-    Vectorized = int(Traversal) == InnerVectorizedTraversal
-              || int(Traversal) == LinearVectorizedTraversal
-              || int(Traversal) == SliceVectorizedTraversal
+    Traversal = int(Dst::SizeAtCompileTime) == 0
+                    ? int(AllAtOnceTraversal)  // If compile-size is zero, traversing will fail at compile-time.
+                : (int(MayLinearVectorize) && (LinearPacketSize > InnerPacketSize)) ? int(LinearVectorizedTraversal)
+                : int(MayInnerVectorize)                                            ? int(InnerVectorizedTraversal)
+                : int(MayLinearVectorize)                                           ? int(LinearVectorizedTraversal)
+                : int(MaySliceVectorize)                                            ? int(SliceVectorizedTraversal)
+                : int(MayLinearize)                                                 ? int(LinearTraversal)
+                                                                                    : int(DefaultTraversal),
+    Vectorized = int(Traversal) == InnerVectorizedTraversal || int(Traversal) == LinearVectorizedTraversal ||
+                 int(Traversal) == SliceVectorizedTraversal
   };
 
-  typedef typename conditional<int(Traversal)==LinearVectorizedTraversal, LinearPacketType, InnerPacketType>::type PacketType;
+  typedef std::conditional_t<int(Traversal) == LinearVectorizedTraversal, LinearPacketType, InnerPacketType> PacketType;
 
-private:
+ private:
   enum {
-    ActualPacketSize    = int(Traversal)==LinearVectorizedTraversal ? LinearPacketSize
-                        : Vectorized ? InnerPacketSize
-                        : 1,
-    UnrollingLimit      = EIGEN_UNROLLING_LIMIT * ActualPacketSize,
-    MayUnrollCompletely = int(Dst::SizeAtCompileTime) != Dynamic
-                       && int(Dst::SizeAtCompileTime) * (int(DstEvaluator::CoeffReadCost)+int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit),
-    MayUnrollInner      = int(InnerSize) != Dynamic
-                       && int(InnerSize) * (int(DstEvaluator::CoeffReadCost)+int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit)
+    ActualPacketSize = int(Traversal) == LinearVectorizedTraversal ? LinearPacketSize
+                       : Vectorized                                ? InnerPacketSize
+                                                                   : 1,
+    UnrollingLimit = EIGEN_UNROLLING_LIMIT * ActualPacketSize,
+    MayUnrollCompletely =
+        int(Dst::SizeAtCompileTime) != Dynamic &&
+        int(Dst::SizeAtCompileTime) * (int(DstEvaluator::CoeffReadCost) + int(SrcEvaluator::CoeffReadCost)) <=
+            int(UnrollingLimit),
+    MayUnrollInner =
+        int(InnerSize) != Dynamic &&
+        int(InnerSize) * (int(DstEvaluator::CoeffReadCost) + int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit)
   };
 
-public:
+ public:
   enum {
     Unrolling = (int(Traversal) == int(InnerVectorizedTraversal) || int(Traversal) == int(DefaultTraversal))
-                ? (
-                    int(MayUnrollCompletely) ? int(CompleteUnrolling)
-                  : int(MayUnrollInner)      ? int(InnerUnrolling)
-                                             : int(NoUnrolling)
-                  )
-              : int(Traversal) == int(LinearVectorizedTraversal)
-                ? ( bool(MayUnrollCompletely) && ( EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)))
-                          ? int(CompleteUnrolling)
-                          : int(NoUnrolling) )
-              : int(Traversal) == int(LinearTraversal)
-                ? ( bool(MayUnrollCompletely) ? int(CompleteUnrolling)
-                                              : int(NoUnrolling) )
+                    ? (int(MayUnrollCompletely) ? int(CompleteUnrolling)
+                       : int(MayUnrollInner)    ? int(InnerUnrolling)
+                                                : int(NoUnrolling))
+                : int(Traversal) == int(LinearVectorizedTraversal)
+                    ? (bool(MayUnrollCompletely) &&
+                               (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment) >= int(LinearRequiredAlignment)))
+                           ? int(CompleteUnrolling)
+                           : int(NoUnrolling))
+                : int(Traversal) == int(LinearTraversal)
+                    ? (bool(MayUnrollCompletely) ? int(CompleteUnrolling) : int(NoUnrolling))
 #if EIGEN_UNALIGNED_VECTORIZE
-              : int(Traversal) == int(SliceVectorizedTraversal)
-                ? ( bool(MayUnrollInner) ? int(InnerUnrolling)
-                                         : int(NoUnrolling) )
+                : int(Traversal) == int(SliceVectorizedTraversal)
+                    ? (bool(MayUnrollInner) ? int(InnerUnrolling) : int(NoUnrolling))
 #endif
-              : int(NoUnrolling)
+                    : int(NoUnrolling)
   };
 
 #ifdef EIGEN_DEBUG_ASSIGN
-  static void debug()
-  {
+  static void debug() {
     std::cerr << "DstXpr: " << typeid(typename DstEvaluator::XprType).name() << std::endl;
     std::cerr << "SrcXpr: " << typeid(typename SrcEvaluator::XprType).name() << std::endl;
     std::cerr.setf(std::ios::hex, std::ios::basefield);
-    std::cerr << "DstFlags" << " = " << DstFlags << " (" << demangle_flags(DstFlags) << " )" << std::endl;
-    std::cerr << "SrcFlags" << " = " << SrcFlags << " (" << demangle_flags(SrcFlags) << " )" << std::endl;
+    std::cerr << "DstFlags"
+              << " = " << DstFlags << " (" << demangle_flags(DstFlags) << " )" << std::endl;
+    std::cerr << "SrcFlags"
+              << " = " << SrcFlags << " (" << demangle_flags(SrcFlags) << " )" << std::endl;
     std::cerr.unsetf(std::ios::hex);
     EIGEN_DEBUG_VAR(DstAlignment)
     EIGEN_DEBUG_VAR(SrcAlignment)
@@ -173,95 +173,84 @@
     EIGEN_DEBUG_VAR(MayInnerVectorize)
     EIGEN_DEBUG_VAR(MayLinearVectorize)
     EIGEN_DEBUG_VAR(MaySliceVectorize)
-    std::cerr << "Traversal" << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl;
+    std::cerr << "Traversal"
+              << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl;
     EIGEN_DEBUG_VAR(SrcEvaluator::CoeffReadCost)
     EIGEN_DEBUG_VAR(DstEvaluator::CoeffReadCost)
     EIGEN_DEBUG_VAR(Dst::SizeAtCompileTime)
     EIGEN_DEBUG_VAR(UnrollingLimit)
     EIGEN_DEBUG_VAR(MayUnrollCompletely)
     EIGEN_DEBUG_VAR(MayUnrollInner)
-    std::cerr << "Unrolling" << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl;
+    std::cerr << "Unrolling"
+              << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl;
     std::cerr << std::endl;
   }
 #endif
 };
 
 /***************************************************************************
-* Part 2 : meta-unrollers
-***************************************************************************/
+ * Part 2 : meta-unrollers
+ ***************************************************************************/
 
 /************************
 *** Default traversal ***
 ************************/
 
-template<typename Kernel, int Index, int Stop>
-struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling
-{
+template <typename Kernel, int Index, int Stop>
+struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling {
   // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
   typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
   typedef typename DstEvaluatorType::XprType DstXprType;
 
-  enum {
-    outer = Index / DstXprType::InnerSizeAtCompileTime,
-    inner = Index % DstXprType::InnerSizeAtCompileTime
-  };
+  enum { outer = Index / DstXprType::InnerSizeAtCompileTime, inner = Index % DstXprType::InnerSizeAtCompileTime };
 
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
     kernel.assignCoeffByOuterInner(outer, inner);
-    copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, Index+1, Stop>::run(kernel);
+    copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, Index + 1, Stop>::run(kernel);
   }
 };
 
-template<typename Kernel, int Stop>
-struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, Stop, Stop>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { }
+template <typename Kernel, int Stop>
+struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, Stop, Stop> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {}
 };
 
-template<typename Kernel, int Index_, int Stop>
-struct copy_using_evaluator_DefaultTraversal_InnerUnrolling
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel, Index outer)
-  {
+template <typename Kernel, int Index_, int Stop>
+struct copy_using_evaluator_DefaultTraversal_InnerUnrolling {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel, Index outer) {
     kernel.assignCoeffByOuterInner(outer, Index_);
-    copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, Index_+1, Stop>::run(kernel, outer);
+    copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, Index_ + 1, Stop>::run(kernel, outer);
   }
 };
 
-template<typename Kernel, int Stop>
-struct copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, Stop, Stop>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) { }
+template <typename Kernel, int Stop>
+struct copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, Stop, Stop> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) {}
 };
 
 /***********************
 *** Linear traversal ***
 ***********************/
 
-template<typename Kernel, int Index, int Stop>
-struct copy_using_evaluator_LinearTraversal_CompleteUnrolling
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel)
-  {
+template <typename Kernel, int Index, int Stop>
+struct copy_using_evaluator_LinearTraversal_CompleteUnrolling {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
     kernel.assignCoeff(Index);
-    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, Index+1, Stop>::run(kernel);
+    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, Index + 1, Stop>::run(kernel);
   }
 };
 
-template<typename Kernel, int Stop>
-struct copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, Stop, Stop>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { }
+template <typename Kernel, int Stop>
+struct copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, Stop, Stop> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) {}
 };
 
 /**************************
 *** Inner vectorization ***
 **************************/
 
-template<typename Kernel, int Index, int Stop>
-struct copy_using_evaluator_innervec_CompleteUnrolling
-{
+template <typename Kernel, int Index, int Stop>
+struct copy_using_evaluator_innervec_CompleteUnrolling {
   // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
   typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
   typedef typename DstEvaluatorType::XprType DstXprType;
@@ -274,47 +263,42 @@
     DstAlignment = Kernel::AssignmentTraits::DstAlignment
   };
 
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
     kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
     enum { NextIndex = Index + unpacket_traits<PacketType>::size };
     copy_using_evaluator_innervec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
   }
 };
 
-template<typename Kernel, int Stop>
-struct copy_using_evaluator_innervec_CompleteUnrolling<Kernel, Stop, Stop>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { }
+template <typename Kernel, int Stop>
+struct copy_using_evaluator_innervec_CompleteUnrolling<Kernel, Stop, Stop> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {}
 };
 
-template<typename Kernel, int Index_, int Stop, int SrcAlignment, int DstAlignment>
-struct copy_using_evaluator_innervec_InnerUnrolling
-{
+template <typename Kernel, int Index_, int Stop, int SrcAlignment, int DstAlignment>
+struct copy_using_evaluator_innervec_InnerUnrolling {
   typedef typename Kernel::PacketType PacketType;
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel, Index outer)
-  {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel, Index outer) {
     kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, Index_);
     enum { NextIndex = Index_ + unpacket_traits<PacketType>::size };
-    copy_using_evaluator_innervec_InnerUnrolling<Kernel, NextIndex, Stop, SrcAlignment, DstAlignment>::run(kernel, outer);
+    copy_using_evaluator_innervec_InnerUnrolling<Kernel, NextIndex, Stop, SrcAlignment, DstAlignment>::run(kernel,
+                                                                                                           outer);
   }
 };
 
-template<typename Kernel, int Stop, int SrcAlignment, int DstAlignment>
-struct copy_using_evaluator_innervec_InnerUnrolling<Kernel, Stop, Stop, SrcAlignment, DstAlignment>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &, Index) { }
+template <typename Kernel, int Stop, int SrcAlignment, int DstAlignment>
+struct copy_using_evaluator_innervec_InnerUnrolling<Kernel, Stop, Stop, SrcAlignment, DstAlignment> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) {}
 };
 
 /***************************************************************************
-* Part 3 : implementation of all cases
-***************************************************************************/
+ * Part 3 : implementation of all cases
+ ***************************************************************************/
 
 // dense_assignment_loop is based on assign_impl
 
-template<typename Kernel,
-         int Traversal = Kernel::AssignmentTraits::Traversal,
-         int Unrolling = Kernel::AssignmentTraits::Unrolling>
+template <typename Kernel, int Traversal = Kernel::AssignmentTraits::Traversal,
+          int Unrolling = Kernel::AssignmentTraits::Unrolling>
 struct dense_assignment_loop;
 
 /************************
@@ -322,14 +306,11 @@
 ************************/
 
 // Zero-sized assignment is a no-op.
-template<typename Kernel, int Unrolling>
-struct dense_assignment_loop<Kernel, AllAtOnceTraversal, Unrolling>
-{
-  EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel& /*kernel*/)
-  {
-    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
-    EIGEN_STATIC_ASSERT(int(DstXprType::SizeAtCompileTime) == 0,
-      EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT)
+template <typename Kernel, int Unrolling>
+struct dense_assignment_loop<Kernel, AllAtOnceTraversal, Unrolling> {
+  EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE EIGEN_CONSTEXPR run(Kernel& /*kernel*/) {
+    EIGEN_STATIC_ASSERT(int(Kernel::DstEvaluatorType::XprType::SizeAtCompileTime) == 0,
+                        EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT)
   }
 };
 
@@ -337,39 +318,34 @@
 *** Default traversal ***
 ************************/
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>
-{
-  EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel &kernel)
-  {
-    for(Index outer = 0; outer < kernel.outerSize(); ++outer) {
-      for(Index inner = 0; inner < kernel.innerSize(); ++inner) {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling> {
+  EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel& kernel) {
+    for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
+      for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
         kernel.assignCoeffByOuterInner(outer, inner);
       }
     }
   }
 };
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, DefaultTraversal, CompleteUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, DefaultTraversal, CompleteUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
   }
 };
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, DefaultTraversal, InnerUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, DefaultTraversal, InnerUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
 
     const Index outerSize = kernel.outerSize();
-    for(Index outer = 0; outer < outerSize; ++outer)
-      copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, 0, DstXprType::InnerSizeAtCompileTime>::run(kernel, outer);
+    for (Index outer = 0; outer < outerSize; ++outer)
+      copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, 0, DstXprType::InnerSizeAtCompileTime>::run(kernel,
+                                                                                                               outer);
   }
 };
 
@@ -377,83 +353,95 @@
 *** Linear vectorization ***
 ***************************/
 
-
 // The goal of unaligned_dense_assignment_loop is simply to factorize the handling
 // of the non vectorizable beginning and ending parts
 
 template <bool IsAligned = false>
-struct unaligned_dense_assignment_loop
-{
+struct unaligned_dense_assignment_loop {
   // if IsAligned = true, then do nothing
   template <typename Kernel>
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index, Index) {}
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&, Index, Index) {}
 };
 
 template <>
-struct unaligned_dense_assignment_loop<false>
-{
+struct unaligned_dense_assignment_loop<false> {
   // MSVC must not inline this functions. If it does, it fails to optimize the
   // packet access path.
   // FIXME check which version exhibits this issue
 #if EIGEN_COMP_MSVC
   template <typename Kernel>
-  static EIGEN_DONT_INLINE void run(Kernel &kernel,
-                                    Index start,
-                                    Index end)
+  static EIGEN_DONT_INLINE void run(Kernel& kernel, Index start, Index end)
 #else
   template <typename Kernel>
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel,
-                                      Index start,
-                                      Index end)
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel, Index start, Index end)
 #endif
   {
-    for (Index index = start; index < end; ++index)
-      kernel.assignCoeff(index);
+    for (Index index = start; index < end; ++index) kernel.assignCoeff(index);
   }
 };
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel, int Index, int Stop>
+struct copy_using_evaluator_linearvec_CompleteUnrolling {
+  // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
+  typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
+  typedef typename DstEvaluatorType::XprType DstXprType;
+  typedef typename Kernel::PacketType PacketType;
+
+  enum { SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, DstAlignment = Kernel::AssignmentTraits::DstAlignment };
+
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
+    kernel.template assignPacket<DstAlignment, SrcAlignment, PacketType>(Index);
+    enum { NextIndex = Index + unpacket_traits<PacketType>::size };
+    copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
+  }
+};
+
+template <typename Kernel, int Stop>
+struct copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, Stop, Stop> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {}
+};
+
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
     const Index size = kernel.size();
     typedef typename Kernel::Scalar Scalar;
     typedef typename Kernel::PacketType PacketType;
     enum {
       requestedAlignment = Kernel::AssignmentTraits::LinearRequiredAlignment,
       packetSize = unpacket_traits<PacketType>::size,
-      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment)>=int(requestedAlignment),
+      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
       dstAlignment = packet_traits<Scalar>::AlignedOnScalar ? int(requestedAlignment)
                                                             : int(Kernel::AssignmentTraits::DstAlignment),
       srcAlignment = Kernel::AssignmentTraits::JointAlignment
     };
-    const Index alignedStart = dstIsAligned ? 0 : internal::first_aligned<requestedAlignment>(kernel.dstDataPtr(), size);
-    const Index alignedEnd = alignedStart + ((size-alignedStart)/packetSize)*packetSize;
+    const Index alignedStart =
+        dstIsAligned ? 0 : internal::first_aligned<requestedAlignment>(kernel.dstDataPtr(), size);
+    const Index alignedEnd = alignedStart + ((size - alignedStart) / packetSize) * packetSize;
 
-    unaligned_dense_assignment_loop<dstIsAligned!=0>::run(kernel, 0, alignedStart);
+    unaligned_dense_assignment_loop<dstIsAligned != 0>::run(kernel, 0, alignedStart);
 
-    for(Index index = alignedStart; index < alignedEnd; index += packetSize)
+    for (Index index = alignedStart; index < alignedEnd; index += packetSize)
       kernel.template assignPacket<dstAlignment, srcAlignment, PacketType>(index);
 
     unaligned_dense_assignment_loop<>::run(kernel, alignedEnd, size);
   }
 };
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     typedef typename Kernel::PacketType PacketType;
 
-    enum { size = DstXprType::SizeAtCompileTime,
-           packetSize =unpacket_traits<PacketType>::size,
-           alignedSize = (int(size)/packetSize)*packetSize };
+    enum {
+      size = DstXprType::SizeAtCompileTime,
+      packetSize = unpacket_traits<PacketType>::size,
+      alignedSize = (int(size) / packetSize) * packetSize
+    };
 
-    copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
-    copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
+    copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
+    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
   }
 };
 
@@ -461,46 +449,37 @@
 *** Inner vectorization ***
 **************************/
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling>
-{
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling> {
   typedef typename Kernel::PacketType PacketType;
-  enum {
-    SrcAlignment = Kernel::AssignmentTraits::SrcAlignment,
-    DstAlignment = Kernel::AssignmentTraits::DstAlignment
-  };
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+  enum { SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, DstAlignment = Kernel::AssignmentTraits::DstAlignment };
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
     const Index innerSize = kernel.innerSize();
     const Index outerSize = kernel.outerSize();
     const Index packetSize = unpacket_traits<PacketType>::size;
-    for(Index outer = 0; outer < outerSize; ++outer)
-      for(Index inner = 0; inner < innerSize; inner+=packetSize)
+    for (Index outer = 0; outer < outerSize; ++outer)
+      for (Index inner = 0; inner < innerSize; inner += packetSize)
         kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
   }
 };
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
   }
 };
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, InnerUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, InnerUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     typedef typename Kernel::AssignmentTraits Traits;
     const Index outerSize = kernel.outerSize();
-    for(Index outer = 0; outer < outerSize; ++outer)
-      copy_using_evaluator_innervec_InnerUnrolling<Kernel, 0, DstXprType::InnerSizeAtCompileTime,
-                                                   Traits::SrcAlignment, Traits::DstAlignment>::run(kernel, outer);
+    for (Index outer = 0; outer < outerSize; ++outer)
+      copy_using_evaluator_innervec_InnerUnrolling<Kernel, 0, DstXprType::InnerSizeAtCompileTime, Traits::SrcAlignment,
+                                                   Traits::DstAlignment>::run(kernel, outer);
   }
 };
 
@@ -508,22 +487,17 @@
 *** Linear traversal ***
 ***********************/
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
     const Index size = kernel.size();
-    for(Index i = 0; i < size; ++i)
-      kernel.assignCoeff(i);
+    for (Index i = 0; i < size; ++i) kernel.assignCoeff(i);
   }
 };
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
   }
@@ -533,69 +507,63 @@
 *** Slice vectorization ***
 ***************************/
 
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
     typedef typename Kernel::Scalar Scalar;
     typedef typename Kernel::PacketType PacketType;
     enum {
       packetSize = unpacket_traits<PacketType>::size,
       requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
-      alignable = packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment)>=sizeof(Scalar),
-      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment)>=int(requestedAlignment),
-      dstAlignment = alignable ? int(requestedAlignment)
-                               : int(Kernel::AssignmentTraits::DstAlignment)
+      alignable =
+          packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
+      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
+      dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
     };
-    const Scalar *dst_ptr = kernel.dstDataPtr();
-    if((!bool(dstIsAligned)) && (UIntPtr(dst_ptr) % sizeof(Scalar))>0)
-    {
+    const Scalar* dst_ptr = kernel.dstDataPtr();
+    if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
       // the pointer is not aligned-on scalar, so alignment is not possible
-      return dense_assignment_loop<Kernel,DefaultTraversal,NoUnrolling>::run(kernel);
+      return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
     }
     const Index packetAlignedMask = packetSize - 1;
     const Index innerSize = kernel.innerSize();
     const Index outerSize = kernel.outerSize();
     const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
-    Index alignedStart = ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
+    Index alignedStart =
+        ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
 
-    for(Index outer = 0; outer < outerSize; ++outer)
-    {
-      const Index alignedEnd = alignedStart + ((innerSize-alignedStart) & ~packetAlignedMask);
+    for (Index outer = 0; outer < outerSize; ++outer) {
+      const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
       // do the non-vectorizable part of the assignment
-      for(Index inner = 0; inner<alignedStart ; ++inner)
-        kernel.assignCoeffByOuterInner(outer, inner);
+      for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
 
       // do the vectorizable part of the assignment
-      for(Index inner = alignedStart; inner<alignedEnd; inner+=packetSize)
+      for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
         kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
 
       // do the non-vectorizable part of the assignment
-      for(Index inner = alignedEnd; inner<innerSize ; ++inner)
-        kernel.assignCoeffByOuterInner(outer, inner);
+      for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
 
-      alignedStart = numext::mini((alignedStart+alignedStep)%packetSize, innerSize);
+      alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
     }
   }
 };
 
 #if EIGEN_UNALIGNED_VECTORIZE
-template<typename Kernel>
-struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, InnerUnrolling>
-{
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
-  {
+template <typename Kernel>
+struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, InnerUnrolling> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     typedef typename Kernel::PacketType PacketType;
 
-    enum { innerSize = DstXprType::InnerSizeAtCompileTime,
-           packetSize =unpacket_traits<PacketType>::size,
-           vectorizableSize = (int(innerSize) / int(packetSize)) * int(packetSize),
-           size = DstXprType::SizeAtCompileTime };
+    enum {
+      innerSize = DstXprType::InnerSizeAtCompileTime,
+      packetSize = unpacket_traits<PacketType>::size,
+      vectorizableSize = (int(innerSize) / int(packetSize)) * int(packetSize),
+      size = DstXprType::SizeAtCompileTime
+    };
 
-    for(Index outer = 0; outer < kernel.outerSize(); ++outer)
-    {
+    for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
       copy_using_evaluator_innervec_InnerUnrolling<Kernel, 0, vectorizableSize, 0, 0>::run(kernel, outer);
       copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, vectorizableSize, innerSize>::run(kernel, outer);
     }
@@ -603,10 +571,9 @@
 };
 #endif
 
-
 /***************************************************************************
-* Part 4 : Generic dense assignment kernel
-***************************************************************************/
+ * Part 4 : Generic dense assignment kernel
+ ***************************************************************************/
 
 // This class generalize the assignment of a coefficient (or packet) from one dense evaluator
 // to another dense writable evaluator.
@@ -614,28 +581,26 @@
 // This abstraction level permits to keep the evaluation loops as simple and as generic as possible.
 // One can customize the assignment using this generic dense_assignment_kernel with different
 // functors, or by completely overloading it, by-passing a functor.
-template<typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor, int Version = Specialized>
-class generic_dense_assignment_kernel
-{
-protected:
+template <typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor, int Version = Specialized>
+class generic_dense_assignment_kernel {
+ protected:
   typedef typename DstEvaluatorTypeT::XprType DstXprType;
   typedef typename SrcEvaluatorTypeT::XprType SrcXprType;
-public:
 
+ public:
   typedef DstEvaluatorTypeT DstEvaluatorType;
   typedef SrcEvaluatorTypeT SrcEvaluatorType;
   typedef typename DstEvaluatorType::Scalar Scalar;
   typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
   typedef typename AssignmentTraits::PacketType PacketType;
 
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  generic_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
-    : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr)
-  {
-    #ifdef EIGEN_DEBUG_ASSIGN
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst,
+                                                                        const SrcEvaluatorType& src,
+                                                                        const Functor& func, DstXprType& dstExpr)
+      : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) {
+#ifdef EIGEN_DEBUG_ASSIGN
     AssignmentTraits::debug();
-    #endif
+#endif
   }
 
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_dstExpr.size(); }
@@ -649,73 +614,62 @@
   EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; }
 
   /// Assign src(row,col) to dst(row,col) through the assignment functor.
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col)
-  {
-    m_functor.assignCoeff(m_dst.coeffRef(row,col), m_src.coeff(row,col));
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) {
+    m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col));
   }
 
   /// \sa assignCoeff(Index,Index)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index index) {
     m_functor.assignCoeff(m_dst.coeffRef(index), m_src.coeff(index));
   }
 
   /// \sa assignCoeff(Index,Index)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) {
     Index row = rowIndexByOuterInner(outer, inner);
     Index col = colIndexByOuterInner(outer, inner);
     assignCoeff(row, col);
   }
 
-
-  template<int StoreMode, int LoadMode, typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index row, Index col)
-  {
-    m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(row,col), m_src.template packet<LoadMode,PacketType>(row,col));
+  template <int StoreMode, int LoadMode, typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index row, Index col) {
+    m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(row, col),
+                                               m_src.template packet<LoadMode, Packet>(row, col));
   }
 
-  template<int StoreMode, int LoadMode, typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index index)
-  {
-    m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(index), m_src.template packet<LoadMode,PacketType>(index));
+  template <int StoreMode, int LoadMode, typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index index) {
+    m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(index), m_src.template packet<LoadMode, Packet>(index));
   }
 
-  template<int StoreMode, int LoadMode, typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner)
-  {
+  template <int StoreMode, int LoadMode, typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner) {
     Index row = rowIndexByOuterInner(outer, inner);
     Index col = colIndexByOuterInner(outer, inner);
-    assignPacket<StoreMode,LoadMode,PacketType>(row, col);
+    assignPacket<StoreMode, LoadMode, Packet>(row, col);
   }
 
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner)
-  {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) {
     typedef typename DstEvaluatorType::ExpressionTraits Traits;
-    return int(Traits::RowsAtCompileTime) == 1 ? 0
-      : int(Traits::ColsAtCompileTime) == 1 ? inner
-      : int(DstEvaluatorType::Flags)&RowMajorBit ? outer
-      : inner;
+    return int(Traits::RowsAtCompileTime) == 1          ? 0
+           : int(Traits::ColsAtCompileTime) == 1        ? inner
+           : int(DstEvaluatorType::Flags) & RowMajorBit ? outer
+                                                        : inner;
   }
 
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner)
-  {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) {
     typedef typename DstEvaluatorType::ExpressionTraits Traits;
-    return int(Traits::ColsAtCompileTime) == 1 ? 0
-      : int(Traits::RowsAtCompileTime) == 1 ? inner
-      : int(DstEvaluatorType::Flags)&RowMajorBit ? inner
-      : outer;
+    return int(Traits::ColsAtCompileTime) == 1          ? 0
+           : int(Traits::RowsAtCompileTime) == 1        ? inner
+           : int(DstEvaluatorType::Flags) & RowMajorBit ? inner
+                                                        : outer;
   }
 
-  EIGEN_DEVICE_FUNC const Scalar* dstDataPtr() const
-  {
-    return m_dstExpr.data();
-  }
+  EIGEN_DEVICE_FUNC const Scalar* dstDataPtr() const { return m_dstExpr.data(); }
 
-protected:
+ protected:
   DstEvaluatorType& m_dst;
   const SrcEvaluatorType& m_src;
-  const Functor &m_functor;
+  const Functor& m_functor;
   // TODO find a way to avoid the needs of the original expression
   DstXprType& m_dstExpr;
 };
@@ -724,50 +678,48 @@
 // PacketSize used is no larger than 4, thereby increasing the chance that vectorized instructions will be used
 // when computing the product.
 
-template<typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor>
-class restricted_packet_dense_assignment_kernel : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, BuiltIn>
-{
-protected:
+template <typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor>
+class restricted_packet_dense_assignment_kernel
+    : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, BuiltIn> {
+ protected:
   typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, BuiltIn> Base;
- public:
-    typedef typename Base::Scalar Scalar;
-    typedef typename Base::DstXprType DstXprType;
-    typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, 4> AssignmentTraits;
-    typedef typename AssignmentTraits::PacketType PacketType;
 
-    EIGEN_DEVICE_FUNC restricted_packet_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr)
-    : Base(dst, src, func, dstExpr)
-  {
-  }
- };
+ public:
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::DstXprType DstXprType;
+  typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, 4> AssignmentTraits;
+  typedef typename AssignmentTraits::PacketType PacketType;
+
+  EIGEN_DEVICE_FUNC restricted_packet_dense_assignment_kernel(DstEvaluatorTypeT& dst, const SrcEvaluatorTypeT& src,
+                                                              const Functor& func, DstXprType& dstExpr)
+      : Base(dst, src, func, dstExpr) {}
+};
 
 /***************************************************************************
-* Part 5 : Entry point for dense rectangular assignment
-***************************************************************************/
+ * Part 5 : Entry point for dense rectangular assignment
+ ***************************************************************************/
 
-template<typename DstXprType,typename SrcXprType, typename Functor>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void resize_if_allowed(DstXprType &dst, const SrcXprType& src, const Functor &/*func*/)
-{
+template <typename DstXprType, typename SrcXprType, typename Functor>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
+                                                             const Functor& /*func*/) {
   EIGEN_ONLY_USED_FOR_DEBUG(dst);
   EIGEN_ONLY_USED_FOR_DEBUG(src);
   eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
 }
 
-template<typename DstXprType,typename SrcXprType, typename T1, typename T2>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void resize_if_allowed(DstXprType &dst, const SrcXprType& src, const internal::assign_op<T1,T2> &/*func*/)
-{
+template <typename DstXprType, typename SrcXprType, typename T1, typename T2>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
+                                                             const internal::assign_op<T1, T2>& /*func*/) {
   Index dstRows = src.rows();
   Index dstCols = src.cols();
-  if(((dst.rows()!=dstRows) || (dst.cols()!=dstCols)))
-    dst.resize(dstRows, dstCols);
+  if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols);
   eigen_assert(dst.rows() == dstRows && dst.cols() == dstCols);
 }
 
-template<typename DstXprType, typename SrcXprType, typename Functor>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src, const Functor &func)
-{
+template <typename DstXprType, typename SrcXprType, typename Functor>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_dense_assignment_loop(DstXprType& dst,
+                                                                                      const SrcXprType& src,
+                                                                                      const Functor& func) {
   typedef evaluator<DstXprType> DstEvaluatorType;
   typedef evaluator<SrcXprType> SrcEvaluatorType;
 
@@ -779,7 +731,7 @@
 
   DstEvaluatorType dstEvaluator(dst);
 
-  typedef generic_dense_assignment_kernel<DstEvaluatorType,SrcEvaluatorType,Functor> Kernel;
+  typedef generic_dense_assignment_kernel<DstEvaluatorType, SrcEvaluatorType, Functor> Kernel;
   Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
 
   dense_assignment_loop<Kernel>::run(kernel);
@@ -787,166 +739,159 @@
 
 // Specialization for filling the destination with a constant value.
 #ifndef EIGEN_GPU_COMPILE_PHASE
-template<typename DstXprType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<typename DstXprType::Scalar>, DstXprType>& src, const internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>& func)
-{
+template <typename DstXprType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(
+    DstXprType& dst,
+    const Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<typename DstXprType::Scalar>, DstXprType>& src,
+    const internal::assign_op<typename DstXprType::Scalar, typename DstXprType::Scalar>& func) {
   resize_if_allowed(dst, src, func);
   std::fill_n(dst.data(), dst.size(), src.functor()());
 }
 #endif
 
-template<typename DstXprType, typename SrcXprType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src)
-{
-  call_dense_assignment_loop(dst, src, internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>());
+template <typename DstXprType, typename SrcXprType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src) {
+  call_dense_assignment_loop(dst, src, internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>());
 }
 
 /***************************************************************************
-* Part 6 : Generic assignment
-***************************************************************************/
+ * Part 6 : Generic assignment
+ ***************************************************************************/
 
 // Based on the respective shapes of the destination and source,
 // the class AssignmentKind determine the kind of assignment mechanism.
 // AssignmentKind must define a Kind typedef.
-template<typename DstShape, typename SrcShape> struct AssignmentKind;
+template <typename DstShape, typename SrcShape>
+struct AssignmentKind;
 
 // Assignment kind defined in this file:
 struct Dense2Dense {};
 struct EigenBase2EigenBase {};
 
-template<typename,typename> struct AssignmentKind { typedef EigenBase2EigenBase Kind; };
-template<> struct AssignmentKind<DenseShape,DenseShape> { typedef Dense2Dense Kind; };
+template <typename, typename>
+struct AssignmentKind {
+  typedef EigenBase2EigenBase Kind;
+};
+template <>
+struct AssignmentKind<DenseShape, DenseShape> {
+  typedef Dense2Dense Kind;
+};
 
 // This is the main assignment class
-template< typename DstXprType, typename SrcXprType, typename Functor,
-          typename Kind = typename AssignmentKind< typename evaluator_traits<DstXprType>::Shape , typename evaluator_traits<SrcXprType>::Shape >::Kind,
+template <typename DstXprType, typename SrcXprType, typename Functor,
+          typename Kind = typename AssignmentKind<typename evaluator_traits<DstXprType>::Shape,
+                                                  typename evaluator_traits<SrcXprType>::Shape>::Kind,
           typename EnableIf = void>
 struct Assignment;
 
+// The only purpose of this call_assignment() function is to deal with noalias() / "assume-aliasing" and automatic
+// transposition. Indeed, I (Gael) think that this concept of "assume-aliasing" was a mistake, and it makes thing quite
+// complicated. So this intermediate function removes everything related to "assume-aliasing" such that Assignment does
+// not has to bother about these annoying details.
 
-// The only purpose of this call_assignment() function is to deal with noalias() / "assume-aliasing" and automatic transposition.
-// Indeed, I (Gael) think that this concept of "assume-aliasing" was a mistake, and it makes thing quite complicated.
-// So this intermediate function removes everything related to "assume-aliasing" such that Assignment
-// does not has to bother about these annoying details.
-
-template<typename Dst, typename Src>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment(Dst& dst, const Src& src)
-{
-  call_assignment(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+template <typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) {
+  call_assignment(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
 }
-template<typename Dst, typename Src>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment(const Dst& dst, const Src& src)
-{
-  call_assignment(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+template <typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(const Dst& dst, const Src& src) {
+  call_assignment(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
 }
 
 // Deal with "assume-aliasing"
-template<typename Dst, typename Src, typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment(Dst& dst, const Src& src, const Func& func, typename enable_if< evaluator_assume_aliasing<Src>::value, void*>::type = 0)
-{
+template <typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(
+    Dst& dst, const Src& src, const Func& func, std::enable_if_t<evaluator_assume_aliasing<Src>::value, void*> = 0) {
   typename plain_matrix_type<Src>::type tmp(src);
   call_assignment_no_alias(dst, tmp, func);
 }
 
-template<typename Dst, typename Src, typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment(Dst& dst, const Src& src, const Func& func, typename enable_if<!evaluator_assume_aliasing<Src>::value, void*>::type = 0)
-{
+template <typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(
+    Dst& dst, const Src& src, const Func& func, std::enable_if_t<!evaluator_assume_aliasing<Src>::value, void*> = 0) {
   call_assignment_no_alias(dst, src, func);
 }
 
 // by-pass "assume-aliasing"
 // When there is no aliasing, we require that 'dst' has been properly resized
-template<typename Dst, template <typename> class StorageBase, typename Src, typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment(NoAlias<Dst,StorageBase>& dst, const Src& src, const Func& func)
-{
+template <typename Dst, template <typename> class StorageBase, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(NoAlias<Dst, StorageBase>& dst,
+                                                                           const Src& src, const Func& func) {
   call_assignment_no_alias(dst.expression(), src, func);
 }
 
-
-template<typename Dst, typename Src, typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment_no_alias(Dst& dst, const Src& src, const Func& func)
-{
+template <typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias(Dst& dst, const Src& src,
+                                                                                    const Func& func) {
   enum {
-    NeedToTranspose = (    (int(Dst::RowsAtCompileTime) == 1 && int(Src::ColsAtCompileTime) == 1)
-                        || (int(Dst::ColsAtCompileTime) == 1 && int(Src::RowsAtCompileTime) == 1)
-                      ) && int(Dst::SizeAtCompileTime) != 1
+    NeedToTranspose = ((int(Dst::RowsAtCompileTime) == 1 && int(Src::ColsAtCompileTime) == 1) ||
+                       (int(Dst::ColsAtCompileTime) == 1 && int(Src::RowsAtCompileTime) == 1)) &&
+                      int(Dst::SizeAtCompileTime) != 1
   };
 
-  typedef typename internal::conditional<NeedToTranspose, Transpose<Dst>, Dst>::type ActualDstTypeCleaned;
-  typedef typename internal::conditional<NeedToTranspose, Transpose<Dst>, Dst&>::type ActualDstType;
+  typedef std::conditional_t<NeedToTranspose, Transpose<Dst>, Dst> ActualDstTypeCleaned;
+  typedef std::conditional_t<NeedToTranspose, Transpose<Dst>, Dst&> ActualDstType;
   ActualDstType actualDst(dst);
 
   // TODO check whether this is the right place to perform these checks:
   EIGEN_STATIC_ASSERT_LVALUE(Dst)
-  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src)
-  EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename ActualDstTypeCleaned::Scalar,typename Src::Scalar);
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned, Src)
+  EIGEN_CHECK_BINARY_COMPATIBILIY(Func, typename ActualDstTypeCleaned::Scalar, typename Src::Scalar);
 
-  Assignment<ActualDstTypeCleaned,Src,Func>::run(actualDst, src, func);
+  Assignment<ActualDstTypeCleaned, Src, Func>::run(actualDst, src, func);
 }
 
-template<typename Dst, typename Src, typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_restricted_packet_assignment_no_alias(Dst& dst, const Src& src, const Func& func)
-{
-    typedef evaluator<Dst> DstEvaluatorType;
-    typedef evaluator<Src> SrcEvaluatorType;
-    typedef restricted_packet_dense_assignment_kernel<DstEvaluatorType,SrcEvaluatorType,Func> Kernel;
+template <typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_restricted_packet_assignment_no_alias(Dst& dst, const Src& src,
+                                                                                      const Func& func) {
+  typedef evaluator<Dst> DstEvaluatorType;
+  typedef evaluator<Src> SrcEvaluatorType;
+  typedef restricted_packet_dense_assignment_kernel<DstEvaluatorType, SrcEvaluatorType, Func> Kernel;
 
-    EIGEN_STATIC_ASSERT_LVALUE(Dst)
-    EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename Dst::Scalar,typename Src::Scalar);
+  EIGEN_STATIC_ASSERT_LVALUE(Dst)
+  EIGEN_CHECK_BINARY_COMPATIBILIY(Func, typename Dst::Scalar, typename Src::Scalar);
 
-    SrcEvaluatorType srcEvaluator(src);
-    resize_if_allowed(dst, src, func);
+  SrcEvaluatorType srcEvaluator(src);
+  resize_if_allowed(dst, src, func);
 
-    DstEvaluatorType dstEvaluator(dst);
-    Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
+  DstEvaluatorType dstEvaluator(dst);
+  Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
 
-    dense_assignment_loop<Kernel>::run(kernel);
+  dense_assignment_loop<Kernel>::run(kernel);
 }
 
-template<typename Dst, typename Src>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment_no_alias(Dst& dst, const Src& src)
-{
-  call_assignment_no_alias(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+template <typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias(Dst& dst, const Src& src) {
+  call_assignment_no_alias(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
 }
 
-template<typename Dst, typename Src, typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src, const Func& func)
-{
+template <typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias_no_transpose(Dst& dst,
+                                                                                                 const Src& src,
+                                                                                                 const Func& func) {
   // TODO check whether this is the right place to perform these checks:
   EIGEN_STATIC_ASSERT_LVALUE(Dst)
-  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Dst,Src)
-  EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename Dst::Scalar,typename Src::Scalar);
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Dst, Src)
+  EIGEN_CHECK_BINARY_COMPATIBILIY(Func, typename Dst::Scalar, typename Src::Scalar);
 
-  Assignment<Dst,Src,Func>::run(dst, src, func);
+  Assignment<Dst, Src, Func>::run(dst, src, func);
 }
-template<typename Dst, typename Src>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src)
-{
-  call_assignment_no_alias_no_transpose(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+template <typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias_no_transpose(Dst& dst,
+                                                                                                 const Src& src) {
+  call_assignment_no_alias_no_transpose(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
 }
 
 // forward declaration
-template<typename Dst, typename Src> void check_for_aliasing(const Dst &dst, const Src &src);
+template <typename Dst, typename Src>
+EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src);
 
 // Generic Dense to Dense assignment
 // Note that the last template argument "Weak" is needed to make it possible to perform
 // both partial specialization+SFINAE without ambiguous specialization
-template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
-struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak>
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
+template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
 #ifndef EIGEN_NO_DEBUG
     internal::check_for_aliasing(dst, src);
 #endif
@@ -959,16 +904,14 @@
 // TODO: not sure we have to keep that one, but it helps porting current code to new evaluator mechanism.
 // Note that the last template argument "Weak" is needed to make it possible to perform
 // both partial specialization+SFINAE without ambiguous specialization
-template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
-struct Assignment<DstXprType, SrcXprType, Functor, EigenBase2EigenBase, Weak>
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  {
+template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+struct Assignment<DstXprType, SrcXprType, Functor, EigenBase2EigenBase, Weak> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(
+      DstXprType& dst, const SrcXprType& src,
+      const internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
     src.evalTo(dst);
@@ -976,35 +919,33 @@
 
   // NOTE The following two functions are templated to avoid their instantiation if not needed
   //      This is needed because some expressions supports evalTo only and/or have 'void' as scalar type.
-  template<typename SrcScalarType>
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,SrcScalarType> &/*func*/)
-  {
+  template <typename SrcScalarType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(
+      DstXprType& dst, const SrcXprType& src,
+      const internal::add_assign_op<typename DstXprType::Scalar, SrcScalarType>& /*func*/) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
     src.addTo(dst);
   }
 
-  template<typename SrcScalarType>
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,SrcScalarType> &/*func*/)
-  {
+  template <typename SrcScalarType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(
+      DstXprType& dst, const SrcXprType& src,
+      const internal::sub_assign_op<typename DstXprType::Scalar, SrcScalarType>& /*func*/) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
     src.subTo(dst);
   }
 };
 
-} // namespace internal
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ASSIGN_EVALUATOR_H
+#endif  // EIGEN_ASSIGN_EVALUATOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h
index 878c024..ca991ca 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h
@@ -10,344 +10,329 @@
 #ifndef EIGEN_BANDMATRIX_H
 #define EIGEN_BANDMATRIX_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename Derived>
-class BandMatrixBase : public EigenBase<Derived>
-{
-  public:
+template <typename Derived>
+class BandMatrixBase : public EigenBase<Derived> {
+ public:
+  enum {
+    Flags = internal::traits<Derived>::Flags,
+    CoeffReadCost = internal::traits<Derived>::CoeffReadCost,
+    RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+    ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+    MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
+    Supers = internal::traits<Derived>::Supers,
+    Subs = internal::traits<Derived>::Subs,
+    Options = internal::traits<Derived>::Options
+  };
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> DenseMatrixType;
+  typedef typename DenseMatrixType::StorageIndex StorageIndex;
+  typedef typename internal::traits<Derived>::CoefficientsType CoefficientsType;
+  typedef EigenBase<Derived> Base;
 
+ protected:
+  enum {
+    DataRowsAtCompileTime = ((Supers != Dynamic) && (Subs != Dynamic)) ? 1 + Supers + Subs : Dynamic,
+    SizeAtCompileTime = min_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime)
+  };
+
+ public:
+  using Base::cols;
+  using Base::derived;
+  using Base::rows;
+
+  /** \returns the number of super diagonals */
+  inline Index supers() const { return derived().supers(); }
+
+  /** \returns the number of sub diagonals */
+  inline Index subs() const { return derived().subs(); }
+
+  /** \returns an expression of the underlying coefficient matrix */
+  inline const CoefficientsType& coeffs() const { return derived().coeffs(); }
+
+  /** \returns an expression of the underlying coefficient matrix */
+  inline CoefficientsType& coeffs() { return derived().coeffs(); }
+
+  /** \returns a vector expression of the \a i -th column,
+   * only the meaningful part is returned.
+   * \warning the internal storage must be column major. */
+  inline Block<CoefficientsType, Dynamic, 1> col(Index i) {
+    EIGEN_STATIC_ASSERT((int(Options) & int(RowMajor)) == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+    Index start = 0;
+    Index len = coeffs().rows();
+    if (i <= supers()) {
+      start = supers() - i;
+      len = (std::min)(rows(), std::max<Index>(0, coeffs().rows() - (supers() - i)));
+    } else if (i >= rows() - subs())
+      len = std::max<Index>(0, coeffs().rows() - (i + 1 - rows() + subs()));
+    return Block<CoefficientsType, Dynamic, 1>(coeffs(), start, i, len, 1);
+  }
+
+  /** \returns a vector expression of the main diagonal */
+  inline Block<CoefficientsType, 1, SizeAtCompileTime> diagonal() {
+    return Block<CoefficientsType, 1, SizeAtCompileTime>(coeffs(), supers(), 0, 1, (std::min)(rows(), cols()));
+  }
+
+  /** \returns a vector expression of the main diagonal (const version) */
+  inline const Block<const CoefficientsType, 1, SizeAtCompileTime> diagonal() const {
+    return Block<const CoefficientsType, 1, SizeAtCompileTime>(coeffs(), supers(), 0, 1, (std::min)(rows(), cols()));
+  }
+
+  template <int Index>
+  struct DiagonalIntReturnType {
     enum {
-      Flags = internal::traits<Derived>::Flags,
-      CoeffReadCost = internal::traits<Derived>::CoeffReadCost,
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
-      Supers = internal::traits<Derived>::Supers,
-      Subs   = internal::traits<Derived>::Subs,
-      Options = internal::traits<Derived>::Options
+      ReturnOpposite =
+          (int(Options) & int(SelfAdjoint)) && (((Index) > 0 && Supers == 0) || ((Index) < 0 && Subs == 0)),
+      Conjugate = ReturnOpposite && NumTraits<Scalar>::IsComplex,
+      ActualIndex = ReturnOpposite ? -Index : Index,
+      DiagonalSize =
+          (RowsAtCompileTime == Dynamic || ColsAtCompileTime == Dynamic)
+              ? Dynamic
+              : (ActualIndex < 0 ? min_size_prefer_dynamic(ColsAtCompileTime, RowsAtCompileTime + ActualIndex)
+                                 : min_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime - ActualIndex))
     };
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef Matrix<Scalar,RowsAtCompileTime,ColsAtCompileTime> DenseMatrixType;
-    typedef typename DenseMatrixType::StorageIndex StorageIndex;
-    typedef typename internal::traits<Derived>::CoefficientsType CoefficientsType;
-    typedef EigenBase<Derived> Base;
+    typedef Block<CoefficientsType, 1, DiagonalSize> BuildType;
+    typedef std::conditional_t<Conjugate, CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, BuildType>, BuildType>
+        Type;
+  };
 
-  protected:
-    enum {
-      DataRowsAtCompileTime = ((Supers!=Dynamic) && (Subs!=Dynamic))
-                            ? 1 + Supers + Subs
-                            : Dynamic,
-      SizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime,ColsAtCompileTime)
-    };
+  /** \returns a vector expression of the \a N -th sub or super diagonal */
+  template <int N>
+  inline typename DiagonalIntReturnType<N>::Type diagonal() {
+    return typename DiagonalIntReturnType<N>::BuildType(coeffs(), supers() - N, (std::max)(0, N), 1, diagonalLength(N));
+  }
 
-  public:
+  /** \returns a vector expression of the \a N -th sub or super diagonal */
+  template <int N>
+  inline const typename DiagonalIntReturnType<N>::Type diagonal() const {
+    return typename DiagonalIntReturnType<N>::BuildType(coeffs(), supers() - N, (std::max)(0, N), 1, diagonalLength(N));
+  }
 
-    using Base::derived;
-    using Base::rows;
-    using Base::cols;
+  /** \returns a vector expression of the \a i -th sub or super diagonal */
+  inline Block<CoefficientsType, 1, Dynamic> diagonal(Index i) {
+    eigen_assert((i < 0 && -i <= subs()) || (i >= 0 && i <= supers()));
+    return Block<CoefficientsType, 1, Dynamic>(coeffs(), supers() - i, std::max<Index>(0, i), 1, diagonalLength(i));
+  }
 
-    /** \returns the number of super diagonals */
-    inline Index supers() const { return derived().supers(); }
+  /** \returns a vector expression of the \a i -th sub or super diagonal */
+  inline const Block<const CoefficientsType, 1, Dynamic> diagonal(Index i) const {
+    eigen_assert((i < 0 && -i <= subs()) || (i >= 0 && i <= supers()));
+    return Block<const CoefficientsType, 1, Dynamic>(coeffs(), supers() - i, std::max<Index>(0, i), 1,
+                                                     diagonalLength(i));
+  }
 
-    /** \returns the number of sub diagonals */
-    inline Index subs() const { return derived().subs(); }
+  template <typename Dest>
+  inline void evalTo(Dest& dst) const {
+    dst.resize(rows(), cols());
+    dst.setZero();
+    dst.diagonal() = diagonal();
+    for (Index i = 1; i <= supers(); ++i) dst.diagonal(i) = diagonal(i);
+    for (Index i = 1; i <= subs(); ++i) dst.diagonal(-i) = diagonal(-i);
+  }
 
-    /** \returns an expression of the underlying coefficient matrix */
-    inline const CoefficientsType& coeffs() const { return derived().coeffs(); }
+  DenseMatrixType toDenseMatrix() const {
+    DenseMatrixType res(rows(), cols());
+    evalTo(res);
+    return res;
+  }
 
-    /** \returns an expression of the underlying coefficient matrix */
-    inline CoefficientsType& coeffs() { return derived().coeffs(); }
-
-    /** \returns a vector expression of the \a i -th column,
-      * only the meaningful part is returned.
-      * \warning the internal storage must be column major. */
-    inline Block<CoefficientsType,Dynamic,1> col(Index i)
-    {
-      EIGEN_STATIC_ASSERT((int(Options) & int(RowMajor)) == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
-      Index start = 0;
-      Index len = coeffs().rows();
-      if (i<=supers())
-      {
-        start = supers()-i;
-        len = (std::min)(rows(),std::max<Index>(0,coeffs().rows() - (supers()-i)));
-      }
-      else if (i>=rows()-subs())
-        len = std::max<Index>(0,coeffs().rows() - (i + 1 - rows() + subs()));
-      return Block<CoefficientsType,Dynamic,1>(coeffs(), start, i, len, 1);
-    }
-
-    /** \returns a vector expression of the main diagonal */
-    inline Block<CoefficientsType,1,SizeAtCompileTime> diagonal()
-    { return Block<CoefficientsType,1,SizeAtCompileTime>(coeffs(),supers(),0,1,(std::min)(rows(),cols())); }
-
-    /** \returns a vector expression of the main diagonal (const version) */
-    inline const Block<const CoefficientsType,1,SizeAtCompileTime> diagonal() const
-    { return Block<const CoefficientsType,1,SizeAtCompileTime>(coeffs(),supers(),0,1,(std::min)(rows(),cols())); }
-
-    template<int Index> struct DiagonalIntReturnType {
-      enum {
-        ReturnOpposite = (int(Options) & int(SelfAdjoint)) && (((Index) > 0 && Supers == 0) || ((Index) < 0 && Subs == 0)),
-        Conjugate = ReturnOpposite && NumTraits<Scalar>::IsComplex,
-        ActualIndex = ReturnOpposite ? -Index : Index,
-        DiagonalSize = (RowsAtCompileTime==Dynamic || ColsAtCompileTime==Dynamic)
-                     ? Dynamic
-                     : (ActualIndex<0
-                     ? EIGEN_SIZE_MIN_PREFER_DYNAMIC(ColsAtCompileTime, RowsAtCompileTime + ActualIndex)
-                     : EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime, ColsAtCompileTime - ActualIndex))
-      };
-      typedef Block<CoefficientsType,1, DiagonalSize> BuildType;
-      typedef typename internal::conditional<Conjugate,
-                 CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>,BuildType >,
-                 BuildType>::type Type;
-    };
-
-    /** \returns a vector expression of the \a N -th sub or super diagonal */
-    template<int N> inline typename DiagonalIntReturnType<N>::Type diagonal()
-    {
-      return typename DiagonalIntReturnType<N>::BuildType(coeffs(), supers()-N, (std::max)(0,N), 1, diagonalLength(N));
-    }
-
-    /** \returns a vector expression of the \a N -th sub or super diagonal */
-    template<int N> inline const typename DiagonalIntReturnType<N>::Type diagonal() const
-    {
-      return typename DiagonalIntReturnType<N>::BuildType(coeffs(), supers()-N, (std::max)(0,N), 1, diagonalLength(N));
-    }
-
-    /** \returns a vector expression of the \a i -th sub or super diagonal */
-    inline Block<CoefficientsType,1,Dynamic> diagonal(Index i)
-    {
-      eigen_assert((i<0 && -i<=subs()) || (i>=0 && i<=supers()));
-      return Block<CoefficientsType,1,Dynamic>(coeffs(), supers()-i, std::max<Index>(0,i), 1, diagonalLength(i));
-    }
-
-    /** \returns a vector expression of the \a i -th sub or super diagonal */
-    inline const Block<const CoefficientsType,1,Dynamic> diagonal(Index i) const
-    {
-      eigen_assert((i<0 && -i<=subs()) || (i>=0 && i<=supers()));
-      return Block<const CoefficientsType,1,Dynamic>(coeffs(), supers()-i, std::max<Index>(0,i), 1, diagonalLength(i));
-    }
-
-    template<typename Dest> inline void evalTo(Dest& dst) const
-    {
-      dst.resize(rows(),cols());
-      dst.setZero();
-      dst.diagonal() = diagonal();
-      for (Index i=1; i<=supers();++i)
-        dst.diagonal(i) = diagonal(i);
-      for (Index i=1; i<=subs();++i)
-        dst.diagonal(-i) = diagonal(-i);
-    }
-
-    DenseMatrixType toDenseMatrix() const
-    {
-      DenseMatrixType res(rows(),cols());
-      evalTo(res);
-      return res;
-    }
-
-  protected:
-
-    inline Index diagonalLength(Index i) const
-    { return i<0 ? (std::min)(cols(),rows()+i) : (std::min)(rows(),cols()-i); }
+ protected:
+  inline Index diagonalLength(Index i) const {
+    return i < 0 ? (std::min)(cols(), rows() + i) : (std::min)(rows(), cols() - i);
+  }
 };
 
 /**
-  * \class BandMatrix
-  * \ingroup Core_Module
-  *
-  * \brief Represents a rectangular matrix with a banded storage
-  *
-  * \tparam _Scalar Numeric type, i.e. float, double, int
-  * \tparam _Rows Number of rows, or \b Dynamic
-  * \tparam _Cols Number of columns, or \b Dynamic
-  * \tparam _Supers Number of super diagonal
-  * \tparam _Subs Number of sub diagonal
-  * \tparam _Options A combination of either \b #RowMajor or \b #ColMajor, and of \b #SelfAdjoint
-  *                  The former controls \ref TopicStorageOrders "storage order", and defaults to
-  *                  column-major. The latter controls whether the matrix represents a selfadjoint
-  *                  matrix in which case either Supers of Subs have to be null.
-  *
-  * \sa class TridiagonalMatrix
-  */
+ * \class BandMatrix
+ * \ingroup Core_Module
+ *
+ * \brief Represents a rectangular matrix with a banded storage
+ *
+ * \tparam Scalar_ Numeric type, i.e. float, double, int
+ * \tparam Rows_ Number of rows, or \b Dynamic
+ * \tparam Cols_ Number of columns, or \b Dynamic
+ * \tparam Supers_ Number of super diagonal
+ * \tparam Subs_ Number of sub diagonal
+ * \tparam Options_ A combination of either \b #RowMajor or \b #ColMajor, and of \b #SelfAdjoint
+ *                  The former controls \ref TopicStorageOrders "storage order", and defaults to
+ *                  column-major. The latter controls whether the matrix represents a selfadjoint
+ *                  matrix in which case either Supers of Subs have to be null.
+ *
+ * \sa class TridiagonalMatrix
+ */
 
-template<typename _Scalar, int _Rows, int _Cols, int _Supers, int _Subs, int _Options>
-struct traits<BandMatrix<_Scalar,_Rows,_Cols,_Supers,_Subs,_Options> >
-{
-  typedef _Scalar Scalar;
+template <typename Scalar_, int Rows_, int Cols_, int Supers_, int Subs_, int Options_>
+struct traits<BandMatrix<Scalar_, Rows_, Cols_, Supers_, Subs_, Options_> > {
+  typedef Scalar_ Scalar;
   typedef Dense StorageKind;
   typedef Eigen::Index StorageIndex;
   enum {
     CoeffReadCost = NumTraits<Scalar>::ReadCost,
-    RowsAtCompileTime = _Rows,
-    ColsAtCompileTime = _Cols,
-    MaxRowsAtCompileTime = _Rows,
-    MaxColsAtCompileTime = _Cols,
+    RowsAtCompileTime = Rows_,
+    ColsAtCompileTime = Cols_,
+    MaxRowsAtCompileTime = Rows_,
+    MaxColsAtCompileTime = Cols_,
     Flags = LvalueBit,
-    Supers = _Supers,
-    Subs = _Subs,
-    Options = _Options,
-    DataRowsAtCompileTime = ((Supers!=Dynamic) && (Subs!=Dynamic)) ? 1 + Supers + Subs : Dynamic
+    Supers = Supers_,
+    Subs = Subs_,
+    Options = Options_,
+    DataRowsAtCompileTime = ((Supers != Dynamic) && (Subs != Dynamic)) ? 1 + Supers + Subs : Dynamic
   };
-  typedef Matrix<Scalar, DataRowsAtCompileTime, ColsAtCompileTime, int(Options) & int(RowMajor) ? RowMajor : ColMajor> CoefficientsType;
+  typedef Matrix<Scalar, DataRowsAtCompileTime, ColsAtCompileTime, int(Options) & int(RowMajor) ? RowMajor : ColMajor>
+      CoefficientsType;
 };
 
-template<typename _Scalar, int Rows, int Cols, int Supers, int Subs, int Options>
-class BandMatrix : public BandMatrixBase<BandMatrix<_Scalar,Rows,Cols,Supers,Subs,Options> >
-{
-  public:
+template <typename Scalar_, int Rows, int Cols, int Supers, int Subs, int Options>
+class BandMatrix : public BandMatrixBase<BandMatrix<Scalar_, Rows, Cols, Supers, Subs, Options> > {
+ public:
+  typedef typename internal::traits<BandMatrix>::Scalar Scalar;
+  typedef typename internal::traits<BandMatrix>::StorageIndex StorageIndex;
+  typedef typename internal::traits<BandMatrix>::CoefficientsType CoefficientsType;
 
-    typedef typename internal::traits<BandMatrix>::Scalar Scalar;
-    typedef typename internal::traits<BandMatrix>::StorageIndex StorageIndex;
-    typedef typename internal::traits<BandMatrix>::CoefficientsType CoefficientsType;
+  explicit inline BandMatrix(Index rows = Rows, Index cols = Cols, Index supers = Supers, Index subs = Subs)
+      : m_coeffs(1 + supers + subs, cols), m_rows(rows), m_supers(supers), m_subs(subs) {}
 
-    explicit inline BandMatrix(Index rows=Rows, Index cols=Cols, Index supers=Supers, Index subs=Subs)
-      : m_coeffs(1+supers+subs,cols),
-        m_rows(rows), m_supers(supers), m_subs(subs)
-    {
-    }
+  /** \returns the number of columns */
+  inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); }
 
-    /** \returns the number of columns */
-    inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); }
+  /** \returns the number of rows */
+  inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); }
 
-    /** \returns the number of rows */
-    inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); }
+  /** \returns the number of super diagonals */
+  inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); }
 
-    /** \returns the number of super diagonals */
-    inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); }
+  /** \returns the number of sub diagonals */
+  inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); }
 
-    /** \returns the number of sub diagonals */
-    inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); }
+  inline const CoefficientsType& coeffs() const { return m_coeffs; }
+  inline CoefficientsType& coeffs() { return m_coeffs; }
 
-    inline const CoefficientsType& coeffs() const { return m_coeffs; }
-    inline CoefficientsType& coeffs() { return m_coeffs; }
-
-  protected:
-
-    CoefficientsType m_coeffs;
-    internal::variable_if_dynamic<Index, Rows>   m_rows;
-    internal::variable_if_dynamic<Index, Supers> m_supers;
-    internal::variable_if_dynamic<Index, Subs>   m_subs;
+ protected:
+  CoefficientsType m_coeffs;
+  internal::variable_if_dynamic<Index, Rows> m_rows;
+  internal::variable_if_dynamic<Index, Supers> m_supers;
+  internal::variable_if_dynamic<Index, Subs> m_subs;
 };
 
-template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
+template <typename CoefficientsType_, int Rows_, int Cols_, int Supers_, int Subs_, int Options_>
 class BandMatrixWrapper;
 
-template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
-struct traits<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
-{
-  typedef typename _CoefficientsType::Scalar Scalar;
-  typedef typename _CoefficientsType::StorageKind StorageKind;
-  typedef typename _CoefficientsType::StorageIndex StorageIndex;
+template <typename CoefficientsType_, int Rows_, int Cols_, int Supers_, int Subs_, int Options_>
+struct traits<BandMatrixWrapper<CoefficientsType_, Rows_, Cols_, Supers_, Subs_, Options_> > {
+  typedef typename CoefficientsType_::Scalar Scalar;
+  typedef typename CoefficientsType_::StorageKind StorageKind;
+  typedef typename CoefficientsType_::StorageIndex StorageIndex;
   enum {
-    CoeffReadCost = internal::traits<_CoefficientsType>::CoeffReadCost,
-    RowsAtCompileTime = _Rows,
-    ColsAtCompileTime = _Cols,
-    MaxRowsAtCompileTime = _Rows,
-    MaxColsAtCompileTime = _Cols,
+    CoeffReadCost = internal::traits<CoefficientsType_>::CoeffReadCost,
+    RowsAtCompileTime = Rows_,
+    ColsAtCompileTime = Cols_,
+    MaxRowsAtCompileTime = Rows_,
+    MaxColsAtCompileTime = Cols_,
     Flags = LvalueBit,
-    Supers = _Supers,
-    Subs = _Subs,
-    Options = _Options,
-    DataRowsAtCompileTime = ((Supers!=Dynamic) && (Subs!=Dynamic)) ? 1 + Supers + Subs : Dynamic
+    Supers = Supers_,
+    Subs = Subs_,
+    Options = Options_,
+    DataRowsAtCompileTime = ((Supers != Dynamic) && (Subs != Dynamic)) ? 1 + Supers + Subs : Dynamic
   };
-  typedef _CoefficientsType CoefficientsType;
+  typedef CoefficientsType_ CoefficientsType;
 };
 
-template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
-class BandMatrixWrapper : public BandMatrixBase<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
-{
-  public:
+template <typename CoefficientsType_, int Rows_, int Cols_, int Supers_, int Subs_, int Options_>
+class BandMatrixWrapper
+    : public BandMatrixBase<BandMatrixWrapper<CoefficientsType_, Rows_, Cols_, Supers_, Subs_, Options_> > {
+ public:
+  typedef typename internal::traits<BandMatrixWrapper>::Scalar Scalar;
+  typedef typename internal::traits<BandMatrixWrapper>::CoefficientsType CoefficientsType;
+  typedef typename internal::traits<BandMatrixWrapper>::StorageIndex StorageIndex;
 
-    typedef typename internal::traits<BandMatrixWrapper>::Scalar Scalar;
-    typedef typename internal::traits<BandMatrixWrapper>::CoefficientsType CoefficientsType;
-    typedef typename internal::traits<BandMatrixWrapper>::StorageIndex StorageIndex;
+  explicit inline BandMatrixWrapper(const CoefficientsType& coeffs, Index rows = Rows_, Index cols = Cols_,
+                                    Index supers = Supers_, Index subs = Subs_)
+      : m_coeffs(coeffs), m_rows(rows), m_supers(supers), m_subs(subs) {
+    EIGEN_UNUSED_VARIABLE(cols);
+    // eigen_assert(coeffs.cols()==cols() && (supers()+subs()+1)==coeffs.rows());
+  }
 
-    explicit inline BandMatrixWrapper(const CoefficientsType& coeffs, Index rows=_Rows, Index cols=_Cols, Index supers=_Supers, Index subs=_Subs)
-      : m_coeffs(coeffs),
-        m_rows(rows), m_supers(supers), m_subs(subs)
-    {
-      EIGEN_UNUSED_VARIABLE(cols);
-      //internal::assert(coeffs.cols()==cols() && (supers()+subs()+1)==coeffs.rows());
-    }
+  /** \returns the number of columns */
+  inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); }
 
-    /** \returns the number of columns */
-    inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); }
+  /** \returns the number of rows */
+  inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); }
 
-    /** \returns the number of rows */
-    inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); }
+  /** \returns the number of super diagonals */
+  inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); }
 
-    /** \returns the number of super diagonals */
-    inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); }
+  /** \returns the number of sub diagonals */
+  inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); }
 
-    /** \returns the number of sub diagonals */
-    inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); }
+  inline const CoefficientsType& coeffs() const { return m_coeffs; }
 
-    inline const CoefficientsType& coeffs() const { return m_coeffs; }
-
-  protected:
-
-    const CoefficientsType& m_coeffs;
-    internal::variable_if_dynamic<Index, _Rows>   m_rows;
-    internal::variable_if_dynamic<Index, _Supers> m_supers;
-    internal::variable_if_dynamic<Index, _Subs>   m_subs;
+ protected:
+  const CoefficientsType& m_coeffs;
+  internal::variable_if_dynamic<Index, Rows_> m_rows;
+  internal::variable_if_dynamic<Index, Supers_> m_supers;
+  internal::variable_if_dynamic<Index, Subs_> m_subs;
 };
 
 /**
-  * \class TridiagonalMatrix
-  * \ingroup Core_Module
-  *
-  * \brief Represents a tridiagonal matrix with a compact banded storage
-  *
-  * \tparam Scalar Numeric type, i.e. float, double, int
-  * \tparam Size Number of rows and cols, or \b Dynamic
-  * \tparam Options Can be 0 or \b SelfAdjoint
-  *
-  * \sa class BandMatrix
-  */
-template<typename Scalar, int Size, int Options>
-class TridiagonalMatrix : public BandMatrix<Scalar,Size,Size,Options&SelfAdjoint?0:1,1,Options|RowMajor>
-{
-    typedef BandMatrix<Scalar,Size,Size,Options&SelfAdjoint?0:1,1,Options|RowMajor> Base;
-    typedef typename Base::StorageIndex StorageIndex;
-  public:
-    explicit TridiagonalMatrix(Index size = Size) : Base(size,size,Options&SelfAdjoint?0:1,1) {}
+ * \class TridiagonalMatrix
+ * \ingroup Core_Module
+ *
+ * \brief Represents a tridiagonal matrix with a compact banded storage
+ *
+ * \tparam Scalar Numeric type, i.e. float, double, int
+ * \tparam Size Number of rows and cols, or \b Dynamic
+ * \tparam Options Can be 0 or \b SelfAdjoint
+ *
+ * \sa class BandMatrix
+ */
+template <typename Scalar, int Size, int Options>
+class TridiagonalMatrix : public BandMatrix<Scalar, Size, Size, Options & SelfAdjoint ? 0 : 1, 1, Options | RowMajor> {
+  typedef BandMatrix<Scalar, Size, Size, Options & SelfAdjoint ? 0 : 1, 1, Options | RowMajor> Base;
+  typedef typename Base::StorageIndex StorageIndex;
 
-    inline typename Base::template DiagonalIntReturnType<1>::Type super()
-    { return Base::template diagonal<1>(); }
-    inline const typename Base::template DiagonalIntReturnType<1>::Type super() const
-    { return Base::template diagonal<1>(); }
-    inline typename Base::template DiagonalIntReturnType<-1>::Type sub()
-    { return Base::template diagonal<-1>(); }
-    inline const typename Base::template DiagonalIntReturnType<-1>::Type sub() const
-    { return Base::template diagonal<-1>(); }
-  protected:
+ public:
+  explicit TridiagonalMatrix(Index size = Size) : Base(size, size, Options & SelfAdjoint ? 0 : 1, 1) {}
+
+  inline typename Base::template DiagonalIntReturnType<1>::Type super() { return Base::template diagonal<1>(); }
+  inline const typename Base::template DiagonalIntReturnType<1>::Type super() const {
+    return Base::template diagonal<1>();
+  }
+  inline typename Base::template DiagonalIntReturnType<-1>::Type sub() { return Base::template diagonal<-1>(); }
+  inline const typename Base::template DiagonalIntReturnType<-1>::Type sub() const {
+    return Base::template diagonal<-1>();
+  }
+
+ protected:
 };
 
-
 struct BandShape {};
 
-template<typename _Scalar, int _Rows, int _Cols, int _Supers, int _Subs, int _Options>
-struct evaluator_traits<BandMatrix<_Scalar,_Rows,_Cols,_Supers,_Subs,_Options> >
-  : public evaluator_traits_base<BandMatrix<_Scalar,_Rows,_Cols,_Supers,_Subs,_Options> >
-{
+template <typename Scalar_, int Rows_, int Cols_, int Supers_, int Subs_, int Options_>
+struct evaluator_traits<BandMatrix<Scalar_, Rows_, Cols_, Supers_, Subs_, Options_> >
+    : public evaluator_traits_base<BandMatrix<Scalar_, Rows_, Cols_, Supers_, Subs_, Options_> > {
   typedef BandShape Shape;
 };
 
-template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
-struct evaluator_traits<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
-  : public evaluator_traits_base<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
-{
+template <typename CoefficientsType_, int Rows_, int Cols_, int Supers_, int Subs_, int Options_>
+struct evaluator_traits<BandMatrixWrapper<CoefficientsType_, Rows_, Cols_, Supers_, Subs_, Options_> >
+    : public evaluator_traits_base<BandMatrixWrapper<CoefficientsType_, Rows_, Cols_, Supers_, Subs_, Options_> > {
   typedef BandShape Shape;
 };
 
-template<> struct AssignmentKind<DenseShape,BandShape> { typedef EigenBase2EigenBase Kind; };
+template <>
+struct AssignmentKind<DenseShape, BandShape> {
+  typedef EigenBase2EigenBase Kind;
+};
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_BANDMATRIX_H
+#endif  // EIGEN_BANDMATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h
index 3206d66..9b16ed2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h
@@ -11,438 +11,429 @@
 #ifndef EIGEN_BLOCK_H
 #define EIGEN_BLOCK_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
-struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprType>
-{
-  typedef typename traits<XprType>::Scalar Scalar;
-  typedef typename traits<XprType>::StorageKind StorageKind;
-  typedef typename traits<XprType>::XprKind XprKind;
-  typedef typename ref_selector<XprType>::type XprTypeNested;
-  typedef typename remove_reference<XprTypeNested>::type _XprTypeNested;
-  enum{
-    MatrixRows = traits<XprType>::RowsAtCompileTime,
-    MatrixCols = traits<XprType>::ColsAtCompileTime,
+template <typename XprType_, int BlockRows, int BlockCols, bool InnerPanel_>
+struct traits<Block<XprType_, BlockRows, BlockCols, InnerPanel_>> : traits<XprType_> {
+  typedef typename traits<XprType_>::Scalar Scalar;
+  typedef typename traits<XprType_>::StorageKind StorageKind;
+  typedef typename traits<XprType_>::XprKind XprKind;
+  typedef typename ref_selector<XprType_>::type XprTypeNested;
+  typedef std::remove_reference_t<XprTypeNested> XprTypeNested_;
+  enum {
+    MatrixRows = traits<XprType_>::RowsAtCompileTime,
+    MatrixCols = traits<XprType_>::ColsAtCompileTime,
     RowsAtCompileTime = MatrixRows == 0 ? 0 : BlockRows,
     ColsAtCompileTime = MatrixCols == 0 ? 0 : BlockCols,
-    MaxRowsAtCompileTime = BlockRows==0 ? 0
-                         : RowsAtCompileTime != Dynamic ? int(RowsAtCompileTime)
-                         : int(traits<XprType>::MaxRowsAtCompileTime),
-    MaxColsAtCompileTime = BlockCols==0 ? 0
-                         : ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime)
-                         : int(traits<XprType>::MaxColsAtCompileTime),
+    MaxRowsAtCompileTime = BlockRows == 0                 ? 0
+                           : RowsAtCompileTime != Dynamic ? int(RowsAtCompileTime)
+                                                          : int(traits<XprType_>::MaxRowsAtCompileTime),
+    MaxColsAtCompileTime = BlockCols == 0                 ? 0
+                           : ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime)
+                                                          : int(traits<XprType_>::MaxColsAtCompileTime),
 
-    XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0,
-    IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
-               : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
-               : XprTypeIsRowMajor,
+    XprTypeIsRowMajor = (int(traits<XprType_>::Flags) & RowMajorBit) != 0,
+    IsRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1)   ? 1
+                 : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0
+                                                                            : XprTypeIsRowMajor,
     HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor),
     InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
-    InnerStrideAtCompileTime = HasSameStorageOrderAsXprType
-                             ? int(inner_stride_at_compile_time<XprType>::ret)
-                             : int(outer_stride_at_compile_time<XprType>::ret),
-    OuterStrideAtCompileTime = HasSameStorageOrderAsXprType
-                             ? int(outer_stride_at_compile_time<XprType>::ret)
-                             : int(inner_stride_at_compile_time<XprType>::ret),
+    InnerStrideAtCompileTime = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time<XprType_>::ret)
+                                                            : int(outer_stride_at_compile_time<XprType_>::ret),
+    OuterStrideAtCompileTime = HasSameStorageOrderAsXprType ? int(outer_stride_at_compile_time<XprType_>::ret)
+                                                            : int(inner_stride_at_compile_time<XprType_>::ret),
 
     // FIXME, this traits is rather specialized for dense object and it needs to be cleaned further
-    FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
+    FlagsLvalueBit = is_lvalue<XprType_>::value ? LvalueBit : 0,
     FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
-    Flags = (traits<XprType>::Flags & (DirectAccessBit | (InnerPanel?CompressedAccessBit:0))) | FlagsLvalueBit | FlagsRowMajorBit,
+    Flags = (traits<XprType_>::Flags & (DirectAccessBit | (InnerPanel_ ? CompressedAccessBit : 0))) | FlagsLvalueBit |
+            FlagsRowMajorBit,
     // FIXME DirectAccessBit should not be handled by expressions
     //
     // Alignment is needed by MapBase's assertions
-    // We can sefely set it to false here. Internal alignment errors will be detected by an eigen_internal_assert in the respective evaluator
-    Alignment = 0
+    // We can sefely set it to false here. Internal alignment errors will be detected by an eigen_internal_assert in the
+    // respective evaluator
+    Alignment = 0,
+    InnerPanel = InnerPanel_ ? 1 : 0
   };
 };
 
-template<typename XprType, int BlockRows=Dynamic, int BlockCols=Dynamic, bool InnerPanel = false,
-         bool HasDirectAccess = internal::has_direct_access<XprType>::ret> class BlockImpl_dense;
+template <typename XprType, int BlockRows = Dynamic, int BlockCols = Dynamic, bool InnerPanel = false,
+          bool HasDirectAccess = internal::has_direct_access<XprType>::ret>
+class BlockImpl_dense;
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel, typename StorageKind> class BlockImpl;
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel, typename StorageKind>
+class BlockImpl;
 
 /** \class Block
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a fixed-size or dynamic-size block
-  *
-  * \tparam XprType the type of the expression in which we are taking a block
-  * \tparam BlockRows the number of rows of the block we are taking at compile time (optional)
-  * \tparam BlockCols the number of columns of the block we are taking at compile time (optional)
-  * \tparam InnerPanel is true, if the block maps to a set of rows of a row major matrix or
-  *         to set of columns of a column major matrix (optional). The parameter allows to determine
-  *         at compile time whether aligned access is possible on the block expression.
-  *
-  * This class represents an expression of either a fixed-size or dynamic-size block. It is the return
-  * type of DenseBase::block(Index,Index,Index,Index) and DenseBase::block<int,int>(Index,Index) and
-  * most of the time this is the only way it is used.
-  *
-  * However, if you want to directly maniputate block expressions,
-  * for instance if you want to write a function returning such an expression, you
-  * will need to use this class.
-  *
-  * Here is an example illustrating the dynamic case:
-  * \include class_Block.cpp
-  * Output: \verbinclude class_Block.out
-  *
-  * \note Even though this expression has dynamic size, in the case where \a XprType
-  * has fixed size, this expression inherits a fixed maximal size which means that evaluating
-  * it does not cause a dynamic memory allocation.
-  *
-  * Here is an example illustrating the fixed-size case:
-  * \include class_FixedBlock.cpp
-  * Output: \verbinclude class_FixedBlock.out
-  *
-  * \sa DenseBase::block(Index,Index,Index,Index), DenseBase::block(Index,Index), class VectorBlock
-  */
-template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel> class Block
-  : public BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, typename internal::traits<XprType>::StorageKind>
-{
-    typedef BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, typename internal::traits<XprType>::StorageKind> Impl;
-  public:
-    //typedef typename Impl::Base Base;
-    typedef Impl Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(Block)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a fixed-size or dynamic-size block
+ *
+ * \tparam XprType the type of the expression in which we are taking a block
+ * \tparam BlockRows the number of rows of the block we are taking at compile time (optional)
+ * \tparam BlockCols the number of columns of the block we are taking at compile time (optional)
+ * \tparam InnerPanel is true, if the block maps to a set of rows of a row major matrix or
+ *         to set of columns of a column major matrix (optional). The parameter allows to determine
+ *         at compile time whether aligned access is possible on the block expression.
+ *
+ * This class represents an expression of either a fixed-size or dynamic-size block. It is the return
+ * type of DenseBase::block(Index,Index,Index,Index) and DenseBase::block<int,int>(Index,Index) and
+ * most of the time this is the only way it is used.
+ *
+ * However, if you want to directly manipulate block expressions,
+ * for instance if you want to write a function returning such an expression, you
+ * will need to use this class.
+ *
+ * Here is an example illustrating the dynamic case:
+ * \include class_Block.cpp
+ * Output: \verbinclude class_Block.out
+ *
+ * \note Even though this expression has dynamic size, in the case where \a XprType
+ * has fixed size, this expression inherits a fixed maximal size which means that evaluating
+ * it does not cause a dynamic memory allocation.
+ *
+ * Here is an example illustrating the fixed-size case:
+ * \include class_FixedBlock.cpp
+ * Output: \verbinclude class_FixedBlock.out
+ *
+ * \sa DenseBase::block(Index,Index,Index,Index), DenseBase::block(Index,Index), class VectorBlock
+ */
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+class Block
+    : public BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, typename internal::traits<XprType>::StorageKind> {
+  typedef BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, typename internal::traits<XprType>::StorageKind> Impl;
+  using BlockHelper = internal::block_xpr_helper<Block>;
 
-    typedef typename internal::remove_all<XprType>::type NestedExpression;
+ public:
+  // typedef typename Impl::Base Base;
+  typedef Impl Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(Block)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
 
-    /** Column or Row constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Block(XprType& xpr, Index i) : Impl(xpr,i)
-    {
-      eigen_assert( (i>=0) && (
-          ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i<xpr.rows())
-        ||((BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) && i<xpr.cols())));
-    }
+  typedef internal::remove_all_t<XprType> NestedExpression;
 
-    /** Fixed-size constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Block(XprType& xpr, Index startRow, Index startCol)
-      : Impl(xpr, startRow, startCol)
-    {
-      EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
-      eigen_assert(startRow >= 0 && BlockRows >= 0 && startRow + BlockRows <= xpr.rows()
-             && startCol >= 0 && BlockCols >= 0 && startCol + BlockCols <= xpr.cols());
-    }
+  /** Column or Row constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Block(XprType& xpr, Index i) : Impl(xpr, i) {
+    eigen_assert((i >= 0) && (((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) && i < xpr.rows()) ||
+                              ((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) && i < xpr.cols())));
+  }
 
-    /** Dynamic-size constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Block(XprType& xpr,
-          Index startRow, Index startCol,
-          Index blockRows, Index blockCols)
-      : Impl(xpr, startRow, startCol, blockRows, blockCols)
-    {
-      eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows)
-          && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols));
-      eigen_assert(startRow >= 0 && blockRows >= 0 && startRow  <= xpr.rows() - blockRows
-          && startCol >= 0 && blockCols >= 0 && startCol <= xpr.cols() - blockCols);
-    }
+  /** Fixed-size constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Block(XprType& xpr, Index startRow, Index startCol)
+      : Impl(xpr, startRow, startCol) {
+    EIGEN_STATIC_ASSERT(RowsAtCompileTime != Dynamic && ColsAtCompileTime != Dynamic,
+                        THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
+    eigen_assert(startRow >= 0 && BlockRows >= 0 && startRow + BlockRows <= xpr.rows() && startCol >= 0 &&
+                 BlockCols >= 0 && startCol + BlockCols <= xpr.cols());
+  }
+
+  /** Dynamic-size constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Block(XprType& xpr, Index startRow, Index startCol, Index blockRows,
+                                              Index blockCols)
+      : Impl(xpr, startRow, startCol, blockRows, blockCols) {
+    eigen_assert((RowsAtCompileTime == Dynamic || RowsAtCompileTime == blockRows) &&
+                 (ColsAtCompileTime == Dynamic || ColsAtCompileTime == blockCols));
+    eigen_assert(startRow >= 0 && blockRows >= 0 && startRow <= xpr.rows() - blockRows && startCol >= 0 &&
+                 blockCols >= 0 && startCol <= xpr.cols() - blockCols);
+  }
+
+  // convert nested blocks (e.g. Block<Block<MatrixType>>) to a simple block expression (Block<MatrixType>)
+
+  using ConstUnwindReturnType = Block<const typename BlockHelper::BaseType, BlockRows, BlockCols, InnerPanel>;
+  using UnwindReturnType = Block<typename BlockHelper::BaseType, BlockRows, BlockCols, InnerPanel>;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ConstUnwindReturnType unwind() const {
+    return ConstUnwindReturnType(BlockHelper::base(*this), BlockHelper::row(*this, 0), BlockHelper::col(*this, 0),
+                                 this->rows(), this->cols());
+  }
+
+  template <typename T = Block, typename EnableIf = std::enable_if_t<!std::is_const<T>::value>>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE UnwindReturnType unwind() {
+    return UnwindReturnType(BlockHelper::base(*this), BlockHelper::row(*this, 0), BlockHelper::col(*this, 0),
+                            this->rows(), this->cols());
+  }
 };
 
-// The generic default implementation for dense block simplu forward to the internal::BlockImpl_dense
+// The generic default implementation for dense block simply forward to the internal::BlockImpl_dense
 // that must be specialized for direct and non-direct access...
-template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
 class BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, Dense>
-  : public internal::BlockImpl_dense<XprType, BlockRows, BlockCols, InnerPanel>
-{
-    typedef internal::BlockImpl_dense<XprType, BlockRows, BlockCols, InnerPanel> Impl;
-    typedef typename XprType::StorageIndex StorageIndex;
-  public:
-    typedef Impl Base;
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {}
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol) : Impl(xpr, startRow, startCol) {}
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+    : public internal::BlockImpl_dense<XprType, BlockRows, BlockCols, InnerPanel> {
+  typedef internal::BlockImpl_dense<XprType, BlockRows, BlockCols, InnerPanel> Impl;
+  typedef typename XprType::StorageIndex StorageIndex;
+
+ public:
+  typedef Impl Base;
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index i) : Impl(xpr, i) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol)
+      : Impl(xpr, startRow, startCol) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows,
+                                                  Index blockCols)
       : Impl(xpr, startRow, startCol, blockRows, blockCols) {}
 };
 
 namespace internal {
 
 /** \internal Internal implementation of dense Blocks in the general case. */
-template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel, bool HasDirectAccess> class BlockImpl_dense
-  : public internal::dense_xpr_base<Block<XprType, BlockRows, BlockCols, InnerPanel> >::type
-{
-    typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
-    typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
-  public:
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel, bool HasDirectAccess>
+class BlockImpl_dense : public internal::dense_xpr_base<Block<XprType, BlockRows, BlockCols, InnerPanel>>::type {
+  typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
+  typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
 
-    typedef typename internal::dense_xpr_base<BlockType>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(BlockType)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense)
+ public:
+  typedef typename internal::dense_xpr_base<BlockType>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(BlockType)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense)
 
-    // class InnerIterator; // FIXME apparently never used
+  // class InnerIterator; // FIXME apparently never used
 
-    /** Column or Row constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline BlockImpl_dense(XprType& xpr, Index i)
+  /** Column or Row constructor
+   */
+  EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index i)
       : m_xpr(xpr),
         // It is a row if and only if BlockRows==1 and BlockCols==XprType::ColsAtCompileTime,
         // and it is a column if and only if BlockRows==XprType::RowsAtCompileTime and BlockCols==1,
         // all other cases are invalid.
         // The case a 1x1 matrix seems ambiguous, but the result is the same anyway.
-        m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0),
-        m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
-        m_blockRows(BlockRows==1 ? 1 : xpr.rows()),
-        m_blockCols(BlockCols==1 ? 1 : xpr.cols())
-    {}
+        m_startRow((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) ? i : 0),
+        m_startCol((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) ? i : 0),
+        m_blockRows(BlockRows == 1 ? 1 : xpr.rows()),
+        m_blockCols(BlockCols == 1 ? 1 : xpr.cols()) {}
 
-    /** Fixed-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
-      : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol),
-                    m_blockRows(BlockRows), m_blockCols(BlockCols)
-    {}
+  /** Fixed-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
+      : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(BlockRows), m_blockCols(BlockCols) {}
 
-    /** Dynamic-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline BlockImpl_dense(XprType& xpr,
-          Index startRow, Index startCol,
-          Index blockRows, Index blockCols)
-      : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol),
-                    m_blockRows(blockRows), m_blockCols(blockCols)
-    {}
+  /** Dynamic-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol, Index blockRows,
+                                           Index blockCols)
+      : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(blockRows), m_blockCols(blockCols) {}
 
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_blockRows.value(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_blockCols.value(); }
+  EIGEN_DEVICE_FUNC inline Index rows() const { return m_blockRows.value(); }
+  EIGEN_DEVICE_FUNC inline Index cols() const { return m_blockCols.value(); }
 
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index rowId, Index colId)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(XprType)
-      return m_xpr.coeffRef(rowId + m_startRow.value(), colId + m_startCol.value());
-    }
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index rowId, Index colId) {
+    EIGEN_STATIC_ASSERT_LVALUE(XprType)
+    return m_xpr.coeffRef(rowId + m_startRow.value(), colId + m_startCol.value());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      return m_xpr.derived().coeffRef(rowId + m_startRow.value(), colId + m_startCol.value());
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const {
+    return m_xpr.derived().coeffRef(rowId + m_startRow.value(), colId + m_startCol.value());
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const
-    {
-      return m_xpr.coeff(rowId + m_startRow.value(), colId + m_startCol.value());
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const {
+    return m_xpr.coeff(rowId + m_startRow.value(), colId + m_startCol.value());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index index)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(XprType)
-      return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
-                            m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
-    }
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) {
+    EIGEN_STATIC_ASSERT_LVALUE(XprType)
+    return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                          m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index index) const
-    {
-      return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
-                            m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const {
+    return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                          m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const CoeffReturnType coeff(Index index) const
-    {
-      return m_xpr.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
-                         m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
-    }
+  EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const {
+    return m_xpr.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                       m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+  }
 
-    template<int LoadMode>
-    inline PacketScalar packet(Index rowId, Index colId) const
-    {
-      return m_xpr.template packet<Unaligned>(rowId + m_startRow.value(), colId + m_startCol.value());
-    }
+  template <int LoadMode>
+  EIGEN_DEVICE_FUNC inline PacketScalar packet(Index rowId, Index colId) const {
+    return m_xpr.template packet<Unaligned>(rowId + m_startRow.value(), colId + m_startCol.value());
+  }
 
-    template<int LoadMode>
-    inline void writePacket(Index rowId, Index colId, const PacketScalar& val)
-    {
-      m_xpr.template writePacket<Unaligned>(rowId + m_startRow.value(), colId + m_startCol.value(), val);
-    }
+  template <int LoadMode>
+  EIGEN_DEVICE_FUNC inline void writePacket(Index rowId, Index colId, const PacketScalar& val) {
+    m_xpr.template writePacket<Unaligned>(rowId + m_startRow.value(), colId + m_startCol.value(), val);
+  }
 
-    template<int LoadMode>
-    inline PacketScalar packet(Index index) const
-    {
-      return m_xpr.template packet<Unaligned>
-              (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
-               m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
-    }
+  template <int LoadMode>
+  EIGEN_DEVICE_FUNC inline PacketScalar packet(Index index) const {
+    return m_xpr.template packet<Unaligned>(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                                            m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+  }
 
-    template<int LoadMode>
-    inline void writePacket(Index index, const PacketScalar& val)
-    {
-      m_xpr.template writePacket<Unaligned>
-         (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
-          m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), val);
-    }
+  template <int LoadMode>
+  EIGEN_DEVICE_FUNC inline void writePacket(Index index, const PacketScalar& val) {
+    m_xpr.template writePacket<Unaligned>(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                                          m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), val);
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** \sa MapBase::data() */
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const;
-    EIGEN_DEVICE_FUNC inline Index innerStride() const;
-    EIGEN_DEVICE_FUNC inline Index outerStride() const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** \sa MapBase::data() */
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const;
+  EIGEN_DEVICE_FUNC inline Index innerStride() const;
+  EIGEN_DEVICE_FUNC inline Index outerStride() const;
+#endif
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const
-    {
-      return m_xpr;
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t<XprTypeNested>& nestedExpression() const {
+    return m_xpr;
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    XprType& nestedExpression() { return m_xpr; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE XprType& nestedExpression() { return m_xpr; }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    StorageIndex startRow() const EIGEN_NOEXCEPT
-    {
-      return m_startRow.value();
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startRow() const EIGEN_NOEXCEPT {
+    return m_startRow.value();
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    StorageIndex startCol() const EIGEN_NOEXCEPT
-    {
-      return m_startCol.value();
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startCol() const EIGEN_NOEXCEPT {
+    return m_startCol.value();
+  }
 
-  protected:
-
-    XprTypeNested m_xpr;
-    const internal::variable_if_dynamic<StorageIndex, (XprType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
-    const internal::variable_if_dynamic<StorageIndex, (XprType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
-    const internal::variable_if_dynamic<StorageIndex, RowsAtCompileTime> m_blockRows;
-    const internal::variable_if_dynamic<StorageIndex, ColsAtCompileTime> m_blockCols;
+ protected:
+  XprTypeNested m_xpr;
+  const internal::variable_if_dynamic<StorageIndex, (XprType::RowsAtCompileTime == 1 && BlockRows == 1) ? 0 : Dynamic>
+      m_startRow;
+  const internal::variable_if_dynamic<StorageIndex, (XprType::ColsAtCompileTime == 1 && BlockCols == 1) ? 0 : Dynamic>
+      m_startCol;
+  const internal::variable_if_dynamic<StorageIndex, RowsAtCompileTime> m_blockRows;
+  const internal::variable_if_dynamic<StorageIndex, ColsAtCompileTime> m_blockCols;
 };
 
 /** \internal Internal implementation of dense Blocks in the direct access case.*/
-template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
-class BlockImpl_dense<XprType,BlockRows,BlockCols, InnerPanel,true>
-  : public MapBase<Block<XprType, BlockRows, BlockCols, InnerPanel> >
-{
-    typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
-    typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
-    enum {
-      XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0
-    };
-  public:
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+class BlockImpl_dense<XprType, BlockRows, BlockCols, InnerPanel, true>
+    : public MapBase<Block<XprType, BlockRows, BlockCols, InnerPanel>> {
+  typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
+  typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
+  enum { XprTypeIsRowMajor = (int(traits<XprType>::Flags) & RowMajorBit) != 0 };
 
-    typedef MapBase<BlockType> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(BlockType)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense)
+  /** \internal Returns base+offset (unless base is null, in which case returns null).
+   * Adding an offset to nullptr is undefined behavior, so we must avoid it.
+   */
+  template <typename Scalar>
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE static Scalar* add_to_nullable_pointer(Scalar* base,
+                                                                                               Index offset) {
+    return base != nullptr ? base + offset : nullptr;
+  }
 
-    /** Column or Row constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    BlockImpl_dense(XprType& xpr, Index i)
-      : Base(xpr.data() + i * (    ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && (!XprTypeIsRowMajor))
-                                || ((BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) && ( XprTypeIsRowMajor)) ? xpr.innerStride() : xpr.outerStride()),
-             BlockRows==1 ? 1 : xpr.rows(),
-             BlockCols==1 ? 1 : xpr.cols()),
+ public:
+  typedef MapBase<BlockType> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(BlockType)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense)
+
+  /** Column or Row constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, Index i)
+      : Base((BlockRows == 0 || BlockCols == 0)
+                 ? nullptr
+                 : add_to_nullable_pointer(
+                       xpr.data(),
+                       i * (((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) && (!XprTypeIsRowMajor)) ||
+                                    ((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) &&
+                                     (XprTypeIsRowMajor))
+                                ? xpr.innerStride()
+                                : xpr.outerStride())),
+             BlockRows == 1 ? 1 : xpr.rows(), BlockCols == 1 ? 1 : xpr.cols()),
         m_xpr(xpr),
-        m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0),
-        m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0)
-    {
-      init();
-    }
+        m_startRow((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) ? i : 0),
+        m_startCol((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) ? i : 0) {
+    init();
+  }
 
-    /** Fixed-size constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
-      : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol)),
-        m_xpr(xpr), m_startRow(startRow), m_startCol(startCol)
-    {
-      init();
-    }
+  /** Fixed-size constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
+      : Base((BlockRows == 0 || BlockCols == 0)
+                 ? nullptr
+                 : add_to_nullable_pointer(xpr.data(),
+                                           xpr.innerStride() * (XprTypeIsRowMajor ? startCol : startRow) +
+                                               xpr.outerStride() * (XprTypeIsRowMajor ? startRow : startCol))),
+        m_xpr(xpr),
+        m_startRow(startRow),
+        m_startCol(startCol) {
+    init();
+  }
 
-    /** Dynamic-size constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    BlockImpl_dense(XprType& xpr,
-          Index startRow, Index startCol,
-          Index blockRows, Index blockCols)
-      : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol), blockRows, blockCols),
-        m_xpr(xpr), m_startRow(startRow), m_startCol(startCol)
-    {
-      init();
-    }
+  /** Dynamic-size constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, Index startRow, Index startCol, Index blockRows,
+                                                        Index blockCols)
+      : Base((blockRows == 0 || blockCols == 0)
+                 ? nullptr
+                 : add_to_nullable_pointer(xpr.data(),
+                                           xpr.innerStride() * (XprTypeIsRowMajor ? startCol : startRow) +
+                                               xpr.outerStride() * (XprTypeIsRowMajor ? startRow : startCol)),
+             blockRows, blockCols),
+        m_xpr(xpr),
+        m_startRow(startRow),
+        m_startCol(startCol) {
+    init();
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const EIGEN_NOEXCEPT
-    {
-      return m_xpr;
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t<XprTypeNested>& nestedExpression() const
+      EIGEN_NOEXCEPT {
+    return m_xpr;
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    XprType& nestedExpression() { return m_xpr; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE XprType& nestedExpression() { return m_xpr; }
 
-    /** \sa MapBase::innerStride() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index innerStride() const EIGEN_NOEXCEPT
-    {
-      return internal::traits<BlockType>::HasSameStorageOrderAsXprType
-             ? m_xpr.innerStride()
-             : m_xpr.outerStride();
-    }
+  /** \sa MapBase::innerStride() */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index innerStride() const EIGEN_NOEXCEPT {
+    return internal::traits<BlockType>::HasSameStorageOrderAsXprType ? m_xpr.innerStride() : m_xpr.outerStride();
+  }
 
-    /** \sa MapBase::outerStride() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index outerStride() const EIGEN_NOEXCEPT
-    {
-      return internal::traits<BlockType>::HasSameStorageOrderAsXprType
-                    ? m_xpr.outerStride()
-                    : m_xpr.innerStride();
-    }
+  /** \sa MapBase::outerStride() */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT {
+    return internal::traits<BlockType>::HasSameStorageOrderAsXprType ? m_xpr.outerStride() : m_xpr.innerStride();
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    StorageIndex startRow() const EIGEN_NOEXCEPT { return m_startRow.value(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startRow() const EIGEN_NOEXCEPT {
+    return m_startRow.value();
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    StorageIndex startCol() const EIGEN_NOEXCEPT { return m_startCol.value(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startCol() const EIGEN_NOEXCEPT {
+    return m_startCol.value();
+  }
 
-  #ifndef __SUNPRO_CC
+#ifndef __SUNPRO_CC
   // FIXME sunstudio is not friendly with the above friend...
   // META-FIXME there is no 'friend' keyword around here. Is this obsolete?
-  protected:
-  #endif
+ protected:
+#endif
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal used by allowAligned() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows, Index blockCols)
-      : Base(data, blockRows, blockCols), m_xpr(xpr)
-    {
-      init();
-    }
-    #endif
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** \internal used by allowAligned() */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows,
+                                                        Index blockCols)
+      : Base(data, blockRows, blockCols), m_xpr(xpr) {
+    init();
+  }
+#endif
 
-  protected:
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void init()
-    {
-      m_outerStride = internal::traits<BlockType>::HasSameStorageOrderAsXprType
-                    ? m_xpr.outerStride()
-                    : m_xpr.innerStride();
-    }
+ protected:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void init() {
+    m_outerStride =
+        internal::traits<BlockType>::HasSameStorageOrderAsXprType ? m_xpr.outerStride() : m_xpr.innerStride();
+  }
 
-    XprTypeNested m_xpr;
-    const internal::variable_if_dynamic<StorageIndex, (XprType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
-    const internal::variable_if_dynamic<StorageIndex, (XprType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
-    Index m_outerStride;
+  XprTypeNested m_xpr;
+  const internal::variable_if_dynamic<StorageIndex, (XprType::RowsAtCompileTime == 1 && BlockRows == 1) ? 0 : Dynamic>
+      m_startRow;
+  const internal::variable_if_dynamic<StorageIndex, (XprType::ColsAtCompileTime == 1 && BlockCols == 1) ? 0 : Dynamic>
+      m_startCol;
+  Index m_outerStride;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_BLOCK_H
+#endif  // EIGEN_BLOCK_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BooleanRedux.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BooleanRedux.h
deleted file mode 100644
index 852de8b..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BooleanRedux.h
+++ /dev/null
@@ -1,162 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_ALLANDANY_H
-#define EIGEN_ALLANDANY_H
-
-namespace Eigen { 
-
-namespace internal {
-
-template<typename Derived, int UnrollCount, int Rows>
-struct all_unroller
-{
-  enum {
-    col = (UnrollCount-1) / Rows,
-    row = (UnrollCount-1) % Rows
-  };
-
-  EIGEN_DEVICE_FUNC static inline bool run(const Derived &mat)
-  {
-    return all_unroller<Derived, UnrollCount-1, Rows>::run(mat) && mat.coeff(row, col);
-  }
-};
-
-template<typename Derived, int Rows>
-struct all_unroller<Derived, 0, Rows>
-{
-  EIGEN_DEVICE_FUNC static inline bool run(const Derived &/*mat*/) { return true; }
-};
-
-template<typename Derived, int Rows>
-struct all_unroller<Derived, Dynamic, Rows>
-{
-  EIGEN_DEVICE_FUNC static inline bool run(const Derived &) { return false; }
-};
-
-template<typename Derived, int UnrollCount, int Rows>
-struct any_unroller
-{
-  enum {
-    col = (UnrollCount-1) / Rows,
-    row = (UnrollCount-1) % Rows
-  };
-  
-  EIGEN_DEVICE_FUNC static inline bool run(const Derived &mat)
-  {
-    return any_unroller<Derived, UnrollCount-1, Rows>::run(mat) || mat.coeff(row, col);
-  }
-};
-
-template<typename Derived, int Rows>
-struct any_unroller<Derived, 0, Rows>
-{
-  EIGEN_DEVICE_FUNC static inline bool run(const Derived & /*mat*/) { return false; }
-};
-
-template<typename Derived, int Rows>
-struct any_unroller<Derived, Dynamic, Rows>
-{
-  EIGEN_DEVICE_FUNC static inline bool run(const Derived &) { return false; }
-};
-
-} // end namespace internal
-
-/** \returns true if all coefficients are true
-  *
-  * Example: \include MatrixBase_all.cpp
-  * Output: \verbinclude MatrixBase_all.out
-  *
-  * \sa any(), Cwise::operator<()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::all() const
-{
-  typedef internal::evaluator<Derived> Evaluator;
-  enum {
-    unroll = SizeAtCompileTime != Dynamic
-          && SizeAtCompileTime * (int(Evaluator::CoeffReadCost) + int(NumTraits<Scalar>::AddCost)) <= EIGEN_UNROLLING_LIMIT
-  };
-  Evaluator evaluator(derived());
-  if(unroll)
-    return internal::all_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic, internal::traits<Derived>::RowsAtCompileTime>::run(evaluator);
-  else
-  {
-    for(Index j = 0; j < cols(); ++j)
-      for(Index i = 0; i < rows(); ++i)
-        if (!evaluator.coeff(i, j)) return false;
-    return true;
-  }
-}
-
-/** \returns true if at least one coefficient is true
-  *
-  * \sa all()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::any() const
-{
-  typedef internal::evaluator<Derived> Evaluator;
-  enum {
-    unroll = SizeAtCompileTime != Dynamic
-          && SizeAtCompileTime * (int(Evaluator::CoeffReadCost) + int(NumTraits<Scalar>::AddCost)) <= EIGEN_UNROLLING_LIMIT
-  };
-  Evaluator evaluator(derived());
-  if(unroll)
-    return internal::any_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic, internal::traits<Derived>::RowsAtCompileTime>::run(evaluator);
-  else
-  {
-    for(Index j = 0; j < cols(); ++j)
-      for(Index i = 0; i < rows(); ++i)
-        if (evaluator.coeff(i, j)) return true;
-    return false;
-  }
-}
-
-/** \returns the number of coefficients which evaluate to true
-  *
-  * \sa all(), any()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline Eigen::Index DenseBase<Derived>::count() const
-{
-  return derived().template cast<bool>().template cast<Index>().sum();
-}
-
-/** \returns true is \c *this contains at least one Not A Number (NaN).
-  *
-  * \sa allFinite()
-  */
-template<typename Derived>
-inline bool DenseBase<Derived>::hasNaN() const
-{
-#if EIGEN_COMP_MSVC || (defined __FAST_MATH__)
-  return derived().array().isNaN().any();
-#else
-  return !((derived().array()==derived().array()).all());
-#endif
-}
-
-/** \returns true if \c *this contains only finite numbers, i.e., no NaN and no +/-INF values.
-  *
-  * \sa hasNaN()
-  */
-template<typename Derived>
-inline bool DenseBase<Derived>::allFinite() const
-{
-#if EIGEN_COMP_MSVC || (defined __FAST_MATH__)
-  return derived().array().isFinite().all();
-#else
-  return !((derived()-derived()).hasNaN());
-#endif
-}
-    
-} // end namespace Eigen
-
-#endif // EIGEN_ALLANDANY_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h
index c0e29c7..c629123 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h
@@ -11,49 +11,46 @@
 #ifndef EIGEN_COMMAINITIALIZER_H
 #define EIGEN_COMMAINITIALIZER_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \class CommaInitializer
-  * \ingroup Core_Module
-  *
-  * \brief Helper class used by the comma initializer operator
-  *
-  * This class is internally used to implement the comma initializer feature. It is
-  * the return type of MatrixBase::operator<<, and most of the time this is the only
-  * way it is used.
-  *
-  * \sa \blank \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
-  */
-template<typename XprType>
-struct CommaInitializer
-{
+ * \ingroup Core_Module
+ *
+ * \brief Helper class used by the comma initializer operator
+ *
+ * This class is internally used to implement the comma initializer feature. It is
+ * the return type of MatrixBase::operator<<, and most of the time this is the only
+ * way it is used.
+ *
+ * \sa \blank \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
+ */
+template <typename XprType>
+struct CommaInitializer {
   typedef typename XprType::Scalar Scalar;
 
-  EIGEN_DEVICE_FUNC
-  inline CommaInitializer(XprType& xpr, const Scalar& s)
-    : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
-  {
-    eigen_assert(m_xpr.rows() > 0 && m_xpr.cols() > 0
-      && "Cannot comma-initialize a 0x0 matrix (operator<<)");
-    m_xpr.coeffRef(0,0) = s;
+  EIGEN_DEVICE_FUNC inline CommaInitializer(XprType& xpr, const Scalar& s)
+      : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) {
+    eigen_assert(m_xpr.rows() > 0 && m_xpr.cols() > 0 && "Cannot comma-initialize a 0x0 matrix (operator<<)");
+    m_xpr.coeffRef(0, 0) = s;
   }
 
-  template<typename OtherDerived>
-  EIGEN_DEVICE_FUNC
-  inline CommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other)
-    : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
-  {
-    eigen_assert(m_xpr.rows() >= other.rows() && m_xpr.cols() >= other.cols()
-      && "Cannot comma-initialize a 0x0 matrix (operator<<)");
-    m_xpr.block(0, 0, other.rows(), other.cols()) = other;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline CommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other)
+      : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows()) {
+    eigen_assert(m_xpr.rows() >= other.rows() && m_xpr.cols() >= other.cols() &&
+                 "Cannot comma-initialize a 0x0 matrix (operator<<)");
+    m_xpr.template block<OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime>(0, 0, other.rows(),
+                                                                                           other.cols()) = other;
   }
 
-  /* Copy/Move constructor which transfers ownership. This is crucial in 
+  /* Copy/Move constructor which transfers ownership. This is crucial in
    * absence of return value optimization to avoid assertions during destruction. */
   // FIXME in C++11 mode this could be replaced by a proper RValue constructor
-  EIGEN_DEVICE_FUNC
-  inline CommaInitializer(const CommaInitializer& o)
-  : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
+  EIGEN_DEVICE_FUNC inline CommaInitializer(const CommaInitializer& o)
+      : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
     // Mark original object as finished. In absence of R-value references we need to const_cast:
     const_cast<CommaInitializer&>(o).m_row = m_xpr.rows();
     const_cast<CommaInitializer&>(o).m_col = m_xpr.cols();
@@ -61,104 +58,92 @@
   }
 
   /* inserts a scalar value in the target matrix */
-  EIGEN_DEVICE_FUNC
-  CommaInitializer& operator,(const Scalar& s)
-  {
-    if (m_col==m_xpr.cols())
-    {
-      m_row+=m_currentBlockRows;
+  EIGEN_DEVICE_FUNC CommaInitializer &operator,(const Scalar& s) {
+    if (m_col == m_xpr.cols()) {
+      m_row += m_currentBlockRows;
       m_col = 0;
       m_currentBlockRows = 1;
-      eigen_assert(m_row<m_xpr.rows()
-        && "Too many rows passed to comma initializer (operator<<)");
+      eigen_assert(m_row < m_xpr.rows() && "Too many rows passed to comma initializer (operator<<)");
     }
-    eigen_assert(m_col<m_xpr.cols()
-      && "Too many coefficients passed to comma initializer (operator<<)");
-    eigen_assert(m_currentBlockRows==1);
+    eigen_assert(m_col < m_xpr.cols() && "Too many coefficients passed to comma initializer (operator<<)");
+    eigen_assert(m_currentBlockRows == 1);
     m_xpr.coeffRef(m_row, m_col++) = s;
     return *this;
   }
 
   /* inserts a matrix expression in the target matrix */
-  template<typename OtherDerived>
-  EIGEN_DEVICE_FUNC
-  CommaInitializer& operator,(const DenseBase<OtherDerived>& other)
-  {
-    if (m_col==m_xpr.cols() && (other.cols()!=0 || other.rows()!=m_currentBlockRows))
-    {
-      m_row+=m_currentBlockRows;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC CommaInitializer &operator,(const DenseBase<OtherDerived>& other) {
+    if (m_col == m_xpr.cols() && (other.cols() != 0 || other.rows() != m_currentBlockRows)) {
+      m_row += m_currentBlockRows;
       m_col = 0;
       m_currentBlockRows = other.rows();
-      eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows()
-        && "Too many rows passed to comma initializer (operator<<)");
+      eigen_assert(m_row + m_currentBlockRows <= m_xpr.rows() &&
+                   "Too many rows passed to comma initializer (operator<<)");
     }
-    eigen_assert((m_col + other.cols() <= m_xpr.cols())
-      && "Too many coefficients passed to comma initializer (operator<<)");
-    eigen_assert(m_currentBlockRows==other.rows());
-    m_xpr.template block<OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime>
-                    (m_row, m_col, other.rows(), other.cols()) = other;
+    eigen_assert((m_col + other.cols() <= m_xpr.cols()) &&
+                 "Too many coefficients passed to comma initializer (operator<<)");
+    eigen_assert(m_currentBlockRows == other.rows());
+    m_xpr.template block<OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime>(m_row, m_col, other.rows(),
+                                                                                           other.cols()) = other;
     m_col += other.cols();
     return *this;
   }
 
-  EIGEN_DEVICE_FUNC
-  inline ~CommaInitializer()
+  EIGEN_DEVICE_FUNC inline ~CommaInitializer()
 #if defined VERIFY_RAISES_ASSERT && (!defined EIGEN_NO_ASSERTION_CHECKING) && defined EIGEN_EXCEPTIONS
-  EIGEN_EXCEPTION_SPEC(Eigen::eigen_assert_exception)
+      EIGEN_EXCEPTION_SPEC(Eigen::eigen_assert_exception)
 #endif
   {
     finished();
   }
 
   /** \returns the built matrix once all its coefficients have been set.
-    * Calling finished is 100% optional. Its purpose is to write expressions
-    * like this:
-    * \code
-    * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
-    * \endcode
-    */
-  EIGEN_DEVICE_FUNC
-  inline XprType& finished() {
-      eigen_assert(((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0)
-           && m_col == m_xpr.cols()
-           && "Too few coefficients passed to comma initializer (operator<<)");
-      return m_xpr;
+   * Calling finished is 100% optional. Its purpose is to write expressions
+   * like this:
+   * \code
+   * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
+   * \endcode
+   */
+  EIGEN_DEVICE_FUNC inline XprType& finished() {
+    eigen_assert(((m_row + m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0) && m_col == m_xpr.cols() &&
+                 "Too few coefficients passed to comma initializer (operator<<)");
+    return m_xpr;
   }
 
-  XprType& m_xpr;           // target expression
-  Index m_row;              // current row id
-  Index m_col;              // current col id
-  Index m_currentBlockRows; // current block height
+  XprType& m_xpr;            // target expression
+  Index m_row;               // current row id
+  Index m_col;               // current col id
+  Index m_currentBlockRows;  // current block height
 };
 
 /** \anchor MatrixBaseCommaInitRef
-  * Convenient operator to set the coefficients of a matrix.
-  *
-  * The coefficients must be provided in a row major order and exactly match
-  * the size of the matrix. Otherwise an assertion is raised.
-  *
-  * Example: \include MatrixBase_set.cpp
-  * Output: \verbinclude MatrixBase_set.out
-  * 
-  * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order.
-  *
-  * \sa CommaInitializer::finished(), class CommaInitializer
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline CommaInitializer<Derived> DenseBase<Derived>::operator<< (const Scalar& s)
-{
+ * Convenient operator to set the coefficients of a matrix.
+ *
+ * The coefficients must be provided in a row major order and exactly match
+ * the size of the matrix. Otherwise an assertion is raised.
+ *
+ * Example: \include MatrixBase_set.cpp
+ * Output: \verbinclude MatrixBase_set.out
+ *
+ * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary
+ * order.
+ *
+ * \sa CommaInitializer::finished(), class CommaInitializer
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline CommaInitializer<Derived> DenseBase<Derived>::operator<<(const Scalar& s) {
   return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
 }
 
 /** \sa operator<<(const Scalar&) */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC inline CommaInitializer<Derived>
-DenseBase<Derived>::operator<<(const DenseBase<OtherDerived>& other)
-{
-  return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline CommaInitializer<Derived> DenseBase<Derived>::operator<<(
+    const DenseBase<OtherDerived>& other) {
+  return CommaInitializer<Derived>(*static_cast<Derived*>(this), other);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COMMAINITIALIZER_H
+#endif  // EIGEN_COMMAINITIALIZER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h
index 51a2e5f..dd1770b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h
@@ -10,6 +10,9 @@
 #ifndef EIGEN_CONDITIONESTIMATOR_H
 #define EIGEN_CONDITIONESTIMATOR_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
@@ -19,7 +22,7 @@
   static inline Vector run(const Vector& v) {
     const RealVector v_abs = v.cwiseAbs();
     return (v_abs.array() == static_cast<typename Vector::RealScalar>(0))
-            .select(Vector::Ones(v.size()), v.cwiseQuotient(v_abs));
+        .select(Vector::Ones(v.size()), v.cwiseQuotient(v_abs));
   }
 };
 
@@ -28,33 +31,32 @@
 struct rcond_compute_sign<Vector, Vector, false> {
   static inline Vector run(const Vector& v) {
     return (v.array() < static_cast<typename Vector::RealScalar>(0))
-           .select(-Vector::Ones(v.size()), Vector::Ones(v.size()));
+        .select(-Vector::Ones(v.size()), Vector::Ones(v.size()));
   }
 };
 
 /**
-  * \returns an estimate of ||inv(matrix)||_1 given a decomposition of
-  * \a matrix that implements .solve() and .adjoint().solve() methods.
-  *
-  * This function implements Algorithms 4.1 and 5.1 from
-  *   http://www.maths.manchester.ac.uk/~higham/narep/narep135.pdf
-  * which also forms the basis for the condition number estimators in
-  * LAPACK. Since at most 10 calls to the solve method of dec are
-  * performed, the total cost is O(dims^2), as opposed to O(dims^3)
-  * needed to compute the inverse matrix explicitly.
-  *
-  * The most common usage is in estimating the condition number
-  * ||matrix||_1 * ||inv(matrix)||_1. The first term ||matrix||_1 can be
-  * computed directly in O(n^2) operations.
-  *
-  * Supports the following decompositions: FullPivLU, PartialPivLU, LDLT, and
-  * LLT.
-  *
-  * \sa FullPivLU, PartialPivLU, LDLT, LLT.
-  */
+ * \returns an estimate of ||inv(matrix)||_1 given a decomposition of
+ * \a matrix that implements .solve() and .adjoint().solve() methods.
+ *
+ * This function implements Algorithms 4.1 and 5.1 from
+ *   http://www.maths.manchester.ac.uk/~higham/narep/narep135.pdf
+ * which also forms the basis for the condition number estimators in
+ * LAPACK. Since at most 10 calls to the solve method of dec are
+ * performed, the total cost is O(dims^2), as opposed to O(dims^3)
+ * needed to compute the inverse matrix explicitly.
+ *
+ * The most common usage is in estimating the condition number
+ * ||matrix||_1 * ||inv(matrix)||_1. The first term ||matrix||_1 can be
+ * computed directly in O(n^2) operations.
+ *
+ * Supports the following decompositions: FullPivLU, PartialPivLU, LDLT, and
+ * LLT.
+ *
+ * \sa FullPivLU, PartialPivLU, LDLT, LLT.
+ */
 template <typename Decomposition>
-typename Decomposition::RealScalar rcond_invmatrix_L1_norm_estimate(const Decomposition& dec)
-{
+typename Decomposition::RealScalar rcond_invmatrix_L1_norm_estimate(const Decomposition& dec) {
   typedef typename Decomposition::MatrixType MatrixType;
   typedef typename Decomposition::Scalar Scalar;
   typedef typename Decomposition::RealScalar RealScalar;
@@ -64,17 +66,16 @@
 
   eigen_assert(dec.rows() == dec.cols());
   const Index n = dec.rows();
-  if (n == 0)
-    return 0;
+  if (n == 0) return 0;
 
-  // Disable Index to float conversion warning
+    // Disable Index to float conversion warning
 #ifdef __INTEL_COMPILER
-  #pragma warning push
-  #pragma warning ( disable : 2259 )
+#pragma warning push
+#pragma warning(disable : 2259)
 #endif
   Vector v = dec.solve(Vector::Ones(n) / Scalar(n));
 #ifdef __INTEL_COMPILER
-  #pragma warning pop
+#pragma warning pop
 #endif
 
   // lower_bound is a lower bound on
@@ -82,8 +83,7 @@
   // and is the objective maximized by the ("super-") gradient ascent
   // algorithm below.
   RealScalar lower_bound = v.template lpNorm<1>();
-  if (n == 1)
-    return lower_bound;
+  if (n == 1) return lower_bound;
 
   // Gradient ascent algorithm follows: We know that the optimum is achieved at
   // one of the simplices v = e_i, so in each iteration we follow a
@@ -93,8 +93,7 @@
   Vector old_sign_vector;
   Index v_max_abs_index = -1;
   Index old_v_max_abs_index = v_max_abs_index;
-  for (int k = 0; k < 4; ++k)
-  {
+  for (int k = 0; k < 4; ++k) {
     sign_vector = internal::rcond_compute_sign<Vector, RealVector, is_complex>::run(v);
     if (k > 0 && !is_complex && sign_vector == old_sign_vector) {
       // Break if the solution stagnated.
@@ -142,30 +141,29 @@
 }
 
 /** \brief Reciprocal condition number estimator.
-  *
-  * Computing a decomposition of a dense matrix takes O(n^3) operations, while
-  * this method estimates the condition number quickly and reliably in O(n^2)
-  * operations.
-  *
-  * \returns an estimate of the reciprocal condition number
-  * (1 / (||matrix||_1 * ||inv(matrix)||_1)) of matrix, given ||matrix||_1 and
-  * its decomposition. Supports the following decompositions: FullPivLU,
-  * PartialPivLU, LDLT, and LLT.
-  *
-  * \sa FullPivLU, PartialPivLU, LDLT, LLT.
-  */
+ *
+ * Computing a decomposition of a dense matrix takes O(n^3) operations, while
+ * this method estimates the condition number quickly and reliably in O(n^2)
+ * operations.
+ *
+ * \returns an estimate of the reciprocal condition number
+ * (1 / (||matrix||_1 * ||inv(matrix)||_1)) of matrix, given ||matrix||_1 and
+ * its decomposition. Supports the following decompositions: FullPivLU,
+ * PartialPivLU, LDLT, and LLT.
+ *
+ * \sa FullPivLU, PartialPivLU, LDLT, LLT.
+ */
 template <typename Decomposition>
-typename Decomposition::RealScalar
-rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, const Decomposition& dec)
-{
+typename Decomposition::RealScalar rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm,
+                                                         const Decomposition& dec) {
   typedef typename Decomposition::RealScalar RealScalar;
   eigen_assert(dec.rows() == dec.cols());
-  if (dec.rows() == 0)              return NumTraits<RealScalar>::infinity();
-  if (matrix_norm == RealScalar(0)) return RealScalar(0);
-  if (dec.rows() == 1)              return RealScalar(1);
+  if (dec.rows() == 0) return NumTraits<RealScalar>::infinity();
+  if (numext::is_exactly_zero(matrix_norm)) return RealScalar(0);
+  if (dec.rows() == 1) return RealScalar(1);
   const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec);
-  return (inverse_matrix_norm == RealScalar(0) ? RealScalar(0)
-                                               : (RealScalar(1) / inverse_matrix_norm) / matrix_norm);
+  return (numext::is_exactly_zero(inverse_matrix_norm) ? RealScalar(0)
+                                                       : (RealScalar(1) / inverse_matrix_norm) / matrix_norm);
 }
 
 }  // namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
index 0ff8c8d..c620600 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
@@ -9,29 +9,44 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 #ifndef EIGEN_COREEVALUATORS_H
 #define EIGEN_COREEVALUATORS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 // This class returns the evaluator kind from the expression storage kind.
 // Default assumes index based accessors
-template<typename StorageKind>
+template <typename StorageKind>
 struct storage_kind_to_evaluator_kind {
   typedef IndexBased Kind;
 };
 
 // This class returns the evaluator shape from the expression storage kind.
 // It can be Dense, Sparse, Triangular, Diagonal, SelfAdjoint, Band, etc.
-template<typename StorageKind> struct storage_kind_to_shape;
+template <typename StorageKind>
+struct storage_kind_to_shape;
 
-template<> struct storage_kind_to_shape<Dense>                  { typedef DenseShape Shape;           };
-template<> struct storage_kind_to_shape<SolverStorage>          { typedef SolverShape Shape;           };
-template<> struct storage_kind_to_shape<PermutationStorage>     { typedef PermutationShape Shape;     };
-template<> struct storage_kind_to_shape<TranspositionsStorage>  { typedef TranspositionsShape Shape;  };
+template <>
+struct storage_kind_to_shape<Dense> {
+  typedef DenseShape Shape;
+};
+template <>
+struct storage_kind_to_shape<SolverStorage> {
+  typedef SolverShape Shape;
+};
+template <>
+struct storage_kind_to_shape<PermutationStorage> {
+  typedef PermutationShape Shape;
+};
+template <>
+struct storage_kind_to_shape<TranspositionsStorage> {
+  typedef TranspositionsShape Shape;
+};
 
 // Evaluators have to be specialized with respect to various criteria such as:
 //  - storage/structure/shape
@@ -39,88 +54,80 @@
 //  - etc.
 // Therefore, we need specialization of evaluator providing additional template arguments for each kind of evaluators.
 // We currently distinguish the following kind of evaluators:
-// - unary_evaluator    for expressions taking only one arguments (CwiseUnaryOp, CwiseUnaryView, Transpose, MatrixWrapper, ArrayWrapper, Reverse, Replicate)
+// - unary_evaluator    for expressions taking only one arguments (CwiseUnaryOp, CwiseUnaryView, Transpose,
+// MatrixWrapper, ArrayWrapper, Reverse, Replicate)
 // - binary_evaluator   for expression taking two arguments (CwiseBinaryOp)
 // - ternary_evaluator   for expression taking three arguments (CwiseTernaryOp)
-// - product_evaluator  for linear algebra products (Product); special case of binary_evaluator because it requires additional tags for dispatching.
+// - product_evaluator  for linear algebra products (Product); special case of binary_evaluator because it requires
+// additional tags for dispatching.
 // - mapbase_evaluator  for Map, Block, Ref
 // - block_evaluator    for Block (special dispatching to a mapbase_evaluator or unary_evaluator)
 
-template< typename T,
-          typename Arg1Kind   = typename evaluator_traits<typename T::Arg1>::Kind,
-          typename Arg2Kind   = typename evaluator_traits<typename T::Arg2>::Kind,
-          typename Arg3Kind   = typename evaluator_traits<typename T::Arg3>::Kind,
+template <typename T, typename Arg1Kind = typename evaluator_traits<typename T::Arg1>::Kind,
+          typename Arg2Kind = typename evaluator_traits<typename T::Arg2>::Kind,
+          typename Arg3Kind = typename evaluator_traits<typename T::Arg3>::Kind,
           typename Arg1Scalar = typename traits<typename T::Arg1>::Scalar,
           typename Arg2Scalar = typename traits<typename T::Arg2>::Scalar,
-          typename Arg3Scalar = typename traits<typename T::Arg3>::Scalar> struct ternary_evaluator;
+          typename Arg3Scalar = typename traits<typename T::Arg3>::Scalar>
+struct ternary_evaluator;
 
-template< typename T,
-          typename LhsKind   = typename evaluator_traits<typename T::Lhs>::Kind,
-          typename RhsKind   = typename evaluator_traits<typename T::Rhs>::Kind,
+template <typename T, typename LhsKind = typename evaluator_traits<typename T::Lhs>::Kind,
+          typename RhsKind = typename evaluator_traits<typename T::Rhs>::Kind,
           typename LhsScalar = typename traits<typename T::Lhs>::Scalar,
-          typename RhsScalar = typename traits<typename T::Rhs>::Scalar> struct binary_evaluator;
+          typename RhsScalar = typename traits<typename T::Rhs>::Scalar>
+struct binary_evaluator;
 
-template< typename T,
-          typename Kind   = typename evaluator_traits<typename T::NestedExpression>::Kind,
-          typename Scalar = typename T::Scalar> struct unary_evaluator;
+template <typename T, typename Kind = typename evaluator_traits<typename T::NestedExpression>::Kind,
+          typename Scalar = typename T::Scalar>
+struct unary_evaluator;
 
 // evaluator_traits<T> contains traits for evaluator<T>
 
-template<typename T>
-struct evaluator_traits_base
-{
+template <typename T>
+struct evaluator_traits_base {
   // by default, get evaluator kind and shape from storage
   typedef typename storage_kind_to_evaluator_kind<typename traits<T>::StorageKind>::Kind Kind;
   typedef typename storage_kind_to_shape<typename traits<T>::StorageKind>::Shape Shape;
 };
 
 // Default evaluator traits
-template<typename T>
-struct evaluator_traits : public evaluator_traits_base<T>
-{
-};
+template <typename T>
+struct evaluator_traits : public evaluator_traits_base<T> {};
 
-template<typename T, typename Shape = typename evaluator_traits<T>::Shape >
+template <typename T, typename Shape = typename evaluator_traits<T>::Shape>
 struct evaluator_assume_aliasing {
   static const bool value = false;
 };
 
 // By default, we assume a unary expression:
-template<typename T>
-struct evaluator : public unary_evaluator<T>
-{
+template <typename T>
+struct evaluator : public unary_evaluator<T> {
   typedef unary_evaluator<T> Base;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const T& xpr) : Base(xpr) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const T& xpr) : Base(xpr) {}
 };
 
-
 // TODO: Think about const-correctness
-template<typename T>
-struct evaluator<const T>
-  : evaluator<T>
-{
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const T& xpr) : evaluator<T>(xpr) {}
+template <typename T>
+struct evaluator<const T> : evaluator<T> {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const T& xpr) : evaluator<T>(xpr) {}
 };
 
 // ---------- base class for all evaluators ----------
 
-template<typename ExpressionType>
-struct evaluator_base
-{
-  // TODO that's not very nice to have to propagate all these traits. They are currently only needed to handle outer,inner indices.
+template <typename ExpressionType>
+struct evaluator_base {
+  // TODO that's not very nice to have to propagate all these traits. They are currently only needed to handle
+  // outer,inner indices.
   typedef traits<ExpressionType> ExpressionTraits;
 
-  enum {
-    Alignment = 0
-  };
+  enum { Alignment = 0 };
   // noncopyable:
   // Don't make this class inherit noncopyable as this kills EBO (Empty Base Optimization)
   // and make complex evaluator much larger than then should do.
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator_base() {}
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ~evaluator_base() {}
-private:
+
+ private:
   EIGEN_DEVICE_FUNC evaluator_base(const evaluator_base&);
   EIGEN_DEVICE_FUNC const evaluator_base& operator=(const evaluator_base&);
 };
@@ -133,36 +140,34 @@
 // so no need for more sophisticated dispatching.
 
 // this helper permits to completely eliminate m_outerStride if it is known at compiletime.
-template<typename Scalar,int OuterStride> class plainobjectbase_evaluator_data {
-public:
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) : data(ptr)
-  {
+template <typename Scalar, int OuterStride>
+class plainobjectbase_evaluator_data {
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride)
+      : data(ptr) {
 #ifndef EIGEN_INTERNAL_DEBUGGING
     EIGEN_UNUSED_VARIABLE(outerStride);
 #endif
-    eigen_internal_assert(outerStride==OuterStride);
+    eigen_internal_assert(outerStride == OuterStride);
   }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-  Index outerStride() const EIGEN_NOEXCEPT { return OuterStride; }
-  const Scalar *data;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { return OuterStride; }
+  const Scalar* data;
 };
 
-template<typename Scalar> class plainobjectbase_evaluator_data<Scalar,Dynamic> {
-public:
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) : data(ptr), m_outerStride(outerStride) {}
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Index outerStride() const { return m_outerStride; }
-  const Scalar *data;
-protected:
+template <typename Scalar>
+class plainobjectbase_evaluator_data<Scalar, Dynamic> {
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride)
+      : data(ptr), m_outerStride(outerStride) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index outerStride() const { return m_outerStride; }
+  const Scalar* data;
+
+ protected:
   Index m_outerStride;
 };
 
-template<typename Derived>
-struct evaluator<PlainObjectBase<Derived> >
-  : evaluator_base<Derived>
-{
+template <typename Derived>
+struct evaluator<PlainObjectBase<Derived> > : evaluator_base<Derived> {
   typedef PlainObjectBase<Derived> PlainObjectType;
   typedef typename PlainObjectType::Scalar Scalar;
   typedef typename PlainObjectType::CoeffReturnType CoeffReturnType;
@@ -179,132 +184,94 @@
   };
   enum {
     // We do not need to know the outer stride for vectors
-    OuterStrideAtCompileTime = IsVectorAtCompileTime  ? 0
-                                                      : int(IsRowMajor) ? ColsAtCompileTime
-                                                                        : RowsAtCompileTime
+    OuterStrideAtCompileTime = IsVectorAtCompileTime ? 0
+                               : int(IsRowMajor)     ? ColsAtCompileTime
+                                                     : RowsAtCompileTime
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  evaluator()
-    : m_d(0,OuterStrideAtCompileTime)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator() : m_d(0, OuterStrideAtCompileTime) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const PlainObjectType& m)
-    : m_d(m.data(),IsVectorAtCompileTime ? 0 : m.outerStride())
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const PlainObjectType& m)
+      : m_d(m.data(), IsVectorAtCompileTime ? 0 : m.outerStride()) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     if (IsRowMajor)
       return m_d.data[row * m_d.outerStride() + col];
     else
       return m_d.data[row + col * m_d.outerStride()];
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
-    return m_d.data[index];
-  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { return m_d.data[index]; }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
     if (IsRowMajor)
       return const_cast<Scalar*>(m_d.data)[row * m_d.outerStride() + col];
     else
       return const_cast<Scalar*>(m_d.data)[row + col * m_d.outerStride()];
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
-    return const_cast<Scalar*>(m_d.data)[index];
-  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { return const_cast<Scalar*>(m_d.data)[index]; }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
     if (IsRowMajor)
       return ploadt<PacketType, LoadMode>(m_d.data + row * m_d.outerStride() + col);
     else
       return ploadt<PacketType, LoadMode>(m_d.data + row + col * m_d.outerStride());
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
     return ploadt<PacketType, LoadMode>(m_d.data + index);
   }
 
-  template<int StoreMode,typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index row, Index col, const PacketType& x)
-  {
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) {
     if (IsRowMajor)
-      return pstoret<Scalar, PacketType, StoreMode>
-	            (const_cast<Scalar*>(m_d.data) + row * m_d.outerStride() + col, x);
+      return pstoret<Scalar, PacketType, StoreMode>(const_cast<Scalar*>(m_d.data) + row * m_d.outerStride() + col, x);
     else
-      return pstoret<Scalar, PacketType, StoreMode>
-                    (const_cast<Scalar*>(m_d.data) + row + col * m_d.outerStride(), x);
+      return pstoret<Scalar, PacketType, StoreMode>(const_cast<Scalar*>(m_d.data) + row + col * m_d.outerStride(), x);
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index index, const PacketType& x)
-  {
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) {
     return pstoret<Scalar, PacketType, StoreMode>(const_cast<Scalar*>(m_d.data) + index, x);
   }
 
-protected:
-
-  plainobjectbase_evaluator_data<Scalar,OuterStrideAtCompileTime> m_d;
+ protected:
+  plainobjectbase_evaluator_data<Scalar, OuterStrideAtCompileTime> m_d;
 };
 
-template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
 struct evaluator<Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> >
-  : evaluator<PlainObjectBase<Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> > >
-{
+    : evaluator<PlainObjectBase<Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> > > {
   typedef Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> XprType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  evaluator() {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator() {}
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const XprType& m)
-    : evaluator<PlainObjectBase<XprType> >(m)
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& m)
+      : evaluator<PlainObjectBase<XprType> >(m) {}
 };
 
-template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
 struct evaluator<Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> >
-  : evaluator<PlainObjectBase<Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> > >
-{
+    : evaluator<PlainObjectBase<Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> > > {
   typedef Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> XprType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  evaluator() {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator() {}
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const XprType& m)
-    : evaluator<PlainObjectBase<XprType> >(m)
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& m)
+      : evaluator<PlainObjectBase<XprType> >(m) {}
 };
 
 // -------------------- Transpose --------------------
 
-template<typename ArgType>
-struct unary_evaluator<Transpose<ArgType>, IndexBased>
-  : evaluator_base<Transpose<ArgType> >
-{
+template <typename ArgType>
+struct unary_evaluator<Transpose<ArgType>, IndexBased> : evaluator_base<Transpose<ArgType> > {
   typedef Transpose<ArgType> XprType;
 
   enum {
@@ -313,65 +280,44 @@
     Alignment = evaluator<ArgType>::Alignment
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& t) : m_argImpl(t.nestedExpression()) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& t) : m_argImpl(t.nestedExpression()) {}
 
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_argImpl.coeff(col, row);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
-    return m_argImpl.coeff(index);
-  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { return m_argImpl.coeff(index); }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
-    return m_argImpl.coeffRef(col, row);
-  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { return m_argImpl.coeffRef(col, row); }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  typename XprType::Scalar& coeffRef(Index index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename XprType::Scalar& coeffRef(Index index) {
     return m_argImpl.coeffRef(index);
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
-    return m_argImpl.template packet<LoadMode,PacketType>(col, row);
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    return m_argImpl.template packet<LoadMode, PacketType>(col, row);
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
-    return m_argImpl.template packet<LoadMode,PacketType>(index);
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
+    return m_argImpl.template packet<LoadMode, PacketType>(index);
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index row, Index col, const PacketType& x)
-  {
-    m_argImpl.template writePacket<StoreMode,PacketType>(col, row, x);
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) {
+    m_argImpl.template writePacket<StoreMode, PacketType>(col, row, x);
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index index, const PacketType& x)
-  {
-    m_argImpl.template writePacket<StoreMode,PacketType>(index, x);
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) {
+    m_argImpl.template writePacket<StoreMode, PacketType>(index, x);
   }
 
-protected:
+ protected:
   evaluator<ArgType> m_argImpl;
 };
 
@@ -379,63 +325,83 @@
 // Like Matrix and Array, this is not really a unary expression, so we directly specialize evaluator.
 // Likewise, there is not need to more sophisticated dispatching here.
 
-template<typename Scalar,typename NullaryOp,
-         bool has_nullary = has_nullary_operator<NullaryOp>::value,
-         bool has_unary   = has_unary_operator<NullaryOp>::value,
-         bool has_binary  = has_binary_operator<NullaryOp>::value>
-struct nullary_wrapper
-{
+template <typename Scalar, typename NullaryOp, bool has_nullary = has_nullary_operator<NullaryOp>::value,
+          bool has_unary = has_unary_operator<NullaryOp>::value,
+          bool has_binary = has_binary_operator<NullaryOp>::value>
+struct nullary_wrapper {
   template <typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const { return op(i,j); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const {
+    return op(i, j);
+  }
   template <typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { return op(i); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const {
+    return op(i);
+  }
 
-  template <typename T, typename IndexType> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const { return op.template packetOp<T>(i,j); }
-  template <typename T, typename IndexType> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { return op.template packetOp<T>(i); }
+  template <typename T, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const {
+    return op.template packetOp<T>(i, j);
+  }
+  template <typename T, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const {
+    return op.template packetOp<T>(i);
+  }
 };
 
-template<typename Scalar,typename NullaryOp>
-struct nullary_wrapper<Scalar,NullaryOp,true,false,false>
-{
+template <typename Scalar, typename NullaryOp>
+struct nullary_wrapper<Scalar, NullaryOp, true, false, false> {
   template <typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType=0, IndexType=0) const { return op(); }
-  template <typename T, typename IndexType> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType=0, IndexType=0) const { return op.template packetOp<T>(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType = 0, IndexType = 0) const {
+    return op();
+  }
+  template <typename T, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType = 0, IndexType = 0) const {
+    return op.template packetOp<T>();
+  }
 };
 
-template<typename Scalar,typename NullaryOp>
-struct nullary_wrapper<Scalar,NullaryOp,false,false,true>
-{
+template <typename Scalar, typename NullaryOp>
+struct nullary_wrapper<Scalar, NullaryOp, false, false, true> {
   template <typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j=0) const { return op(i,j); }
-  template <typename T, typename IndexType> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j=0) const { return op.template packetOp<T>(i,j); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j = 0) const {
+    return op(i, j);
+  }
+  template <typename T, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j = 0) const {
+    return op.template packetOp<T>(i, j);
+  }
 };
 
 // We need the following specialization for vector-only functors assigned to a runtime vector,
 // for instance, using linspace and assigning a RowVectorXd to a MatrixXd or even a row of a MatrixXd.
 // In this case, i==0 and j is used for the actual iteration.
-template<typename Scalar,typename NullaryOp>
-struct nullary_wrapper<Scalar,NullaryOp,false,true,false>
-{
+template <typename Scalar, typename NullaryOp>
+struct nullary_wrapper<Scalar, NullaryOp, false, true, false> {
   template <typename IndexType>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const {
-    eigen_assert(i==0 || j==0);
-    return op(i+j);
+    eigen_assert(i == 0 || j == 0);
+    return op(i + j);
   }
-  template <typename T, typename IndexType> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const {
-    eigen_assert(i==0 || j==0);
-    return op.template packetOp<T>(i+j);
+  template <typename T, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const {
+    eigen_assert(i == 0 || j == 0);
+    return op.template packetOp<T>(i + j);
   }
 
   template <typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { return op(i); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const {
+    return op(i);
+  }
   template <typename T, typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { return op.template packetOp<T>(i); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const {
+    return op.template packetOp<T>(i);
+  }
 };
 
-template<typename Scalar,typename NullaryOp>
-struct nullary_wrapper<Scalar,NullaryOp,false,false,false> {};
+template <typename Scalar, typename NullaryOp>
+struct nullary_wrapper<Scalar, NullaryOp, false, false, false> {};
 
-#if 0 && EIGEN_COMP_MSVC>0
+#if 0 && EIGEN_COMP_MSVC > 0
 // Disable this ugly workaround. This is now handled in traits<Ref>::match,
 // but this piece of code might still become handly if some other weird compilation
 // erros pop up again.
@@ -491,127 +457,100 @@
     has_binary_operator<NullaryOp,nullary_wrapper_workaround_msvc<IndexType> >::value>().template packetOp<T>(op,i);
   }
 };
-#endif // MSVC workaround
+#endif  // MSVC workaround
 
-template<typename NullaryOp, typename PlainObjectType>
-struct evaluator<CwiseNullaryOp<NullaryOp,PlainObjectType> >
-  : evaluator_base<CwiseNullaryOp<NullaryOp,PlainObjectType> >
-{
-  typedef CwiseNullaryOp<NullaryOp,PlainObjectType> XprType;
-  typedef typename internal::remove_all<PlainObjectType>::type PlainObjectTypeCleaned;
+template <typename NullaryOp, typename PlainObjectType>
+struct evaluator<CwiseNullaryOp<NullaryOp, PlainObjectType> >
+    : evaluator_base<CwiseNullaryOp<NullaryOp, PlainObjectType> > {
+  typedef CwiseNullaryOp<NullaryOp, PlainObjectType> XprType;
+  typedef internal::remove_all_t<PlainObjectType> PlainObjectTypeCleaned;
 
   enum {
     CoeffReadCost = internal::functor_traits<NullaryOp>::Cost,
 
-    Flags = (evaluator<PlainObjectTypeCleaned>::Flags
-          &  (  HereditaryBits
-              | (functor_has_linear_access<NullaryOp>::ret  ? LinearAccessBit : 0)
-              | (functor_traits<NullaryOp>::PacketAccess    ? PacketAccessBit : 0)))
-          | (functor_traits<NullaryOp>::IsRepeatable ? 0 : EvalBeforeNestingBit),
+    Flags = (evaluator<PlainObjectTypeCleaned>::Flags &
+             (HereditaryBits | (functor_has_linear_access<NullaryOp>::ret ? LinearAccessBit : 0) |
+              (functor_traits<NullaryOp>::PacketAccess ? PacketAccessBit : 0))) |
+            (functor_traits<NullaryOp>::IsRepeatable ? 0 : EvalBeforeNestingBit),
     Alignment = AlignedMax
   };
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& n)
-    : m_functor(n.functor()), m_wrapper()
-  {
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& n) : m_functor(n.functor()), m_wrapper() {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
   template <typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(IndexType row, IndexType col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(IndexType row, IndexType col) const {
     return m_wrapper(m_functor, row, col);
   }
 
   template <typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(IndexType index) const
-  {
-    return m_wrapper(m_functor,index);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(IndexType index) const {
+    return m_wrapper(m_functor, index);
   }
 
-  template<int LoadMode, typename PacketType, typename IndexType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(IndexType row, IndexType col) const
-  {
+  template <int LoadMode, typename PacketType, typename IndexType>
+  EIGEN_STRONG_INLINE PacketType packet(IndexType row, IndexType col) const {
     return m_wrapper.template packetOp<PacketType>(m_functor, row, col);
   }
 
-  template<int LoadMode, typename PacketType, typename IndexType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(IndexType index) const
-  {
+  template <int LoadMode, typename PacketType, typename IndexType>
+  EIGEN_STRONG_INLINE PacketType packet(IndexType index) const {
     return m_wrapper.template packetOp<PacketType>(m_functor, index);
   }
 
-protected:
+ protected:
   const NullaryOp m_functor;
-  const internal::nullary_wrapper<CoeffReturnType,NullaryOp> m_wrapper;
+  const internal::nullary_wrapper<CoeffReturnType, NullaryOp> m_wrapper;
 };
 
 // -------------------- CwiseUnaryOp --------------------
 
-template<typename UnaryOp, typename ArgType>
-struct unary_evaluator<CwiseUnaryOp<UnaryOp, ArgType>, IndexBased >
-  : evaluator_base<CwiseUnaryOp<UnaryOp, ArgType> >
-{
+template <typename UnaryOp, typename ArgType>
+struct unary_evaluator<CwiseUnaryOp<UnaryOp, ArgType>, IndexBased> : evaluator_base<CwiseUnaryOp<UnaryOp, ArgType> > {
   typedef CwiseUnaryOp<UnaryOp, ArgType> XprType;
 
   enum {
     CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
 
-    Flags = evaluator<ArgType>::Flags
-          & (HereditaryBits | LinearAccessBit | (functor_traits<UnaryOp>::PacketAccess ? PacketAccessBit : 0)),
+    Flags = evaluator<ArgType>::Flags &
+            (HereditaryBits | LinearAccessBit | (functor_traits<UnaryOp>::PacketAccess ? PacketAccessBit : 0)),
     Alignment = evaluator<ArgType>::Alignment
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& op) : m_d(op)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& op) : m_d(op) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_d.func()(m_d.argImpl.coeff(row, col));
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return m_d.func()(m_d.argImpl.coeff(index));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
     return m_d.func().packetOp(m_d.argImpl.template packet<LoadMode, PacketType>(row, col));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
     return m_d.func().packetOp(m_d.argImpl.template packet<LoadMode, PacketType>(index));
   }
 
-protected:
-
+ protected:
   // this helper permits to completely eliminate the functor if it is empty
-  struct Data
-  {
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Data(const XprType& xpr) : op(xpr.functor()), argImpl(xpr.nestedExpression()) {}
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const UnaryOp& func() const { return op; }
+  struct Data {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr)
+        : op(xpr.functor()), argImpl(xpr.nestedExpression()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryOp& func() const { return op; }
     UnaryOp op;
     evaluator<ArgType> argImpl;
   };
@@ -619,93 +558,279 @@
   Data m_d;
 };
 
+// ----------------------- Casting ---------------------
+
+template <typename SrcType, typename DstType, typename ArgType>
+struct unary_evaluator<CwiseUnaryOp<core_cast_op<SrcType, DstType>, ArgType>, IndexBased> {
+  using CastOp = core_cast_op<SrcType, DstType>;
+  using XprType = CwiseUnaryOp<CastOp, ArgType>;
+
+  // Use the largest packet type by default
+  using SrcPacketType = typename packet_traits<SrcType>::type;
+  static constexpr int SrcPacketSize = unpacket_traits<SrcPacketType>::size;
+  static constexpr int SrcPacketBytes = SrcPacketSize * sizeof(SrcType);
+
+  enum {
+    CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<CastOp>::Cost),
+    PacketAccess = functor_traits<CastOp>::PacketAccess,
+    ActualPacketAccessBit = PacketAccess ? PacketAccessBit : 0,
+    Flags = evaluator<ArgType>::Flags & (HereditaryBits | LinearAccessBit | ActualPacketAccessBit),
+    IsRowMajor = (evaluator<ArgType>::Flags & RowMajorBit),
+    Alignment = evaluator<ArgType>::Alignment
+  };
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& xpr)
+      : m_argImpl(xpr.nestedExpression()), m_rows(xpr.rows()), m_cols(xpr.cols()) {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<CastOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  template <typename DstPacketType>
+  using AltSrcScalarOp = std::enable_if_t<(unpacket_traits<DstPacketType>::size < SrcPacketSize &&
+                                           !find_packet_by_size<SrcType, unpacket_traits<DstPacketType>::size>::value),
+                                          bool>;
+  template <typename DstPacketType>
+  using SrcPacketArgs1 =
+      std::enable_if_t<(find_packet_by_size<SrcType, unpacket_traits<DstPacketType>::size>::value), bool>;
+  template <typename DstPacketType>
+  using SrcPacketArgs2 = std::enable_if_t<(unpacket_traits<DstPacketType>::size) == (2 * SrcPacketSize), bool>;
+  template <typename DstPacketType>
+  using SrcPacketArgs4 = std::enable_if_t<(unpacket_traits<DstPacketType>::size) == (4 * SrcPacketSize), bool>;
+  template <typename DstPacketType>
+  using SrcPacketArgs8 = std::enable_if_t<(unpacket_traits<DstPacketType>::size) == (8 * SrcPacketSize), bool>;
+
+  template <bool UseRowMajor = IsRowMajor, std::enable_if_t<UseRowMajor, bool> = true>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index, Index col, Index packetSize) const {
+    return col + packetSize <= cols();
+  }
+  template <bool UseRowMajor = IsRowMajor, std::enable_if_t<!UseRowMajor, bool> = true>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index row, Index, Index packetSize) const {
+    return row + packetSize <= rows();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index index, Index packetSize) const {
+    return index + packetSize <= size();
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE SrcType srcCoeff(Index row, Index col, Index offset) const {
+    Index actualRow = IsRowMajor ? row : row + offset;
+    Index actualCol = IsRowMajor ? col + offset : col;
+    return m_argImpl.coeff(actualRow, actualCol);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE SrcType srcCoeff(Index index, Index offset) const {
+    Index actualIndex = index + offset;
+    return m_argImpl.coeff(actualIndex);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstType coeff(Index row, Index col) const {
+    return cast<SrcType, DstType>(srcCoeff(row, col, 0));
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstType coeff(Index index) const {
+    return cast<SrcType, DstType>(srcCoeff(index, 0));
+  }
+
+  template <int LoadMode, typename PacketType = SrcPacketType>
+  EIGEN_STRONG_INLINE PacketType srcPacket(Index row, Index col, Index offset) const {
+    constexpr int PacketSize = unpacket_traits<PacketType>::size;
+    Index actualRow = IsRowMajor ? row : row + (offset * PacketSize);
+    Index actualCol = IsRowMajor ? col + (offset * PacketSize) : col;
+    eigen_assert(check_array_bounds(actualRow, actualCol, PacketSize) && "Array index out of bounds");
+    return m_argImpl.template packet<LoadMode, PacketType>(actualRow, actualCol);
+  }
+  template <int LoadMode, typename PacketType = SrcPacketType>
+  EIGEN_STRONG_INLINE PacketType srcPacket(Index index, Index offset) const {
+    constexpr int PacketSize = unpacket_traits<PacketType>::size;
+    Index actualIndex = index + (offset * PacketSize);
+    eigen_assert(check_array_bounds(actualIndex, PacketSize) && "Array index out of bounds");
+    return m_argImpl.template packet<LoadMode, PacketType>(actualIndex);
+  }
+
+  // There is no source packet type with equal or fewer elements than DstPacketType.
+  // This is problematic as the evaluation loop may attempt to access data outside the bounds of the array.
+  // For example, consider the cast utilizing pcast<Packet4f,Packet2d> with an array of size 4: {0.0f,1.0f,2.0f,3.0f}.
+  // The first iteration of the evaulation loop will load 16 bytes: {0.0f,1.0f,2.0f,3.0f} and cast to {0.0,1.0}, which
+  // is acceptable. The second iteration will load 16 bytes: {2.0f,3.0f,?,?}, which is outside the bounds of the array.
+
+  // Instead, perform runtime check to determine if the load would access data outside the bounds of the array.
+  // If not, perform full load. Otherwise, revert to a scalar loop to perform a partial load.
+  // In either case, perform a vectorized cast of the source packet.
+  template <int LoadMode, typename DstPacketType, AltSrcScalarOp<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const {
+    constexpr int DstPacketSize = unpacket_traits<DstPacketType>::size;
+    constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType);
+    constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode);
+    SrcPacketType src;
+    if (EIGEN_PREDICT_TRUE(check_array_bounds(row, col, SrcPacketSize))) {
+      src = srcPacket<SrcLoadMode>(row, col, 0);
+    } else {
+      Array<SrcType, SrcPacketSize, 1> srcArray;
+      for (size_t k = 0; k < DstPacketSize; k++) srcArray[k] = srcCoeff(row, col, k);
+      for (size_t k = DstPacketSize; k < SrcPacketSize; k++) srcArray[k] = SrcType(0);
+      src = pload<SrcPacketType>(srcArray.data());
+    }
+    return pcast<SrcPacketType, DstPacketType>(src);
+  }
+  // Use the source packet type with the same size as DstPacketType, if it exists
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs1<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const {
+    constexpr int DstPacketSize = unpacket_traits<DstPacketType>::size;
+    using SizedSrcPacketType = typename find_packet_by_size<SrcType, DstPacketSize>::type;
+    constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType);
+    constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode);
+    return pcast<SizedSrcPacketType, DstPacketType>(srcPacket<SrcLoadMode, SizedSrcPacketType>(row, col, 0));
+  }
+  // unpacket_traits<DstPacketType>::size == 2 * SrcPacketSize
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs2<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const {
+    constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode);
+    return pcast<SrcPacketType, DstPacketType>(srcPacket<SrcLoadMode>(row, col, 0),
+                                               srcPacket<SrcLoadMode>(row, col, 1));
+  }
+  // unpacket_traits<DstPacketType>::size == 4 * SrcPacketSize
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs4<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const {
+    constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode);
+    return pcast<SrcPacketType, DstPacketType>(srcPacket<SrcLoadMode>(row, col, 0), srcPacket<SrcLoadMode>(row, col, 1),
+                                               srcPacket<SrcLoadMode>(row, col, 2),
+                                               srcPacket<SrcLoadMode>(row, col, 3));
+  }
+  // unpacket_traits<DstPacketType>::size == 8 * SrcPacketSize
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs8<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const {
+    constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode);
+    return pcast<SrcPacketType, DstPacketType>(
+        srcPacket<SrcLoadMode>(row, col, 0), srcPacket<SrcLoadMode>(row, col, 1), srcPacket<SrcLoadMode>(row, col, 2),
+        srcPacket<SrcLoadMode>(row, col, 3), srcPacket<SrcLoadMode>(row, col, 4), srcPacket<SrcLoadMode>(row, col, 5),
+        srcPacket<SrcLoadMode>(row, col, 6), srcPacket<SrcLoadMode>(row, col, 7));
+  }
+
+  // Analagous routines for linear access.
+  template <int LoadMode, typename DstPacketType, AltSrcScalarOp<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index index) const {
+    constexpr int DstPacketSize = unpacket_traits<DstPacketType>::size;
+    constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType);
+    constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode);
+    SrcPacketType src;
+    if (EIGEN_PREDICT_TRUE(check_array_bounds(index, SrcPacketSize))) {
+      src = srcPacket<SrcLoadMode>(index, 0);
+    } else {
+      Array<SrcType, SrcPacketSize, 1> srcArray;
+      for (size_t k = 0; k < DstPacketSize; k++) srcArray[k] = srcCoeff(index, k);
+      for (size_t k = DstPacketSize; k < SrcPacketSize; k++) srcArray[k] = SrcType(0);
+      src = pload<SrcPacketType>(srcArray.data());
+    }
+    return pcast<SrcPacketType, DstPacketType>(src);
+  }
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs1<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index index) const {
+    constexpr int DstPacketSize = unpacket_traits<DstPacketType>::size;
+    using SizedSrcPacketType = typename find_packet_by_size<SrcType, DstPacketSize>::type;
+    constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType);
+    constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode);
+    return pcast<SizedSrcPacketType, DstPacketType>(srcPacket<SrcLoadMode, SizedSrcPacketType>(index, 0));
+  }
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs2<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index index) const {
+    constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode);
+    return pcast<SrcPacketType, DstPacketType>(srcPacket<SrcLoadMode>(index, 0), srcPacket<SrcLoadMode>(index, 1));
+  }
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs4<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index index) const {
+    constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode);
+    return pcast<SrcPacketType, DstPacketType>(srcPacket<SrcLoadMode>(index, 0), srcPacket<SrcLoadMode>(index, 1),
+                                               srcPacket<SrcLoadMode>(index, 2), srcPacket<SrcLoadMode>(index, 3));
+  }
+  template <int LoadMode, typename DstPacketType, SrcPacketArgs8<DstPacketType> = true>
+  EIGEN_STRONG_INLINE DstPacketType packet(Index index) const {
+    constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode);
+    return pcast<SrcPacketType, DstPacketType>(srcPacket<SrcLoadMode>(index, 0), srcPacket<SrcLoadMode>(index, 1),
+                                               srcPacket<SrcLoadMode>(index, 2), srcPacket<SrcLoadMode>(index, 3),
+                                               srcPacket<SrcLoadMode>(index, 4), srcPacket<SrcLoadMode>(index, 5),
+                                               srcPacket<SrcLoadMode>(index, 6), srcPacket<SrcLoadMode>(index, 7));
+  }
+
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { return m_rows; }
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { return m_cols; }
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const { return m_rows * m_cols; }
+
+ protected:
+  const evaluator<ArgType> m_argImpl;
+  const variable_if_dynamic<Index, XprType::RowsAtCompileTime> m_rows;
+  const variable_if_dynamic<Index, XprType::ColsAtCompileTime> m_cols;
+};
+
 // -------------------- CwiseTernaryOp --------------------
 
 // this is a ternary expression
-template<typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>
 struct evaluator<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >
-  : public ternary_evaluator<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >
-{
+    : public ternary_evaluator<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> > {
   typedef CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> XprType;
   typedef ternary_evaluator<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> > Base;
 
   EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
-template<typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>
 struct ternary_evaluator<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3>, IndexBased, IndexBased>
-  : evaluator_base<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >
-{
+    : evaluator_base<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> > {
   typedef CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> XprType;
 
   enum {
-    CoeffReadCost = int(evaluator<Arg1>::CoeffReadCost) + int(evaluator<Arg2>::CoeffReadCost) + int(evaluator<Arg3>::CoeffReadCost) + int(functor_traits<TernaryOp>::Cost),
+    CoeffReadCost = int(evaluator<Arg1>::CoeffReadCost) + int(evaluator<Arg2>::CoeffReadCost) +
+                    int(evaluator<Arg3>::CoeffReadCost) + int(functor_traits<TernaryOp>::Cost),
 
     Arg1Flags = evaluator<Arg1>::Flags,
     Arg2Flags = evaluator<Arg2>::Flags,
     Arg3Flags = evaluator<Arg3>::Flags,
-    SameType = is_same<typename Arg1::Scalar,typename Arg2::Scalar>::value && is_same<typename Arg1::Scalar,typename Arg3::Scalar>::value,
-    StorageOrdersAgree = (int(Arg1Flags)&RowMajorBit)==(int(Arg2Flags)&RowMajorBit) && (int(Arg1Flags)&RowMajorBit)==(int(Arg3Flags)&RowMajorBit),
-    Flags0 = (int(Arg1Flags) | int(Arg2Flags) | int(Arg3Flags)) & (
-        HereditaryBits
-        | (int(Arg1Flags) & int(Arg2Flags) & int(Arg3Flags) &
-           ( (StorageOrdersAgree ? LinearAccessBit : 0)
-           | (functor_traits<TernaryOp>::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0)
-           )
-        )
-     ),
+    SameType = is_same<typename Arg1::Scalar, typename Arg2::Scalar>::value &&
+               is_same<typename Arg1::Scalar, typename Arg3::Scalar>::value,
+    StorageOrdersAgree = (int(Arg1Flags) & RowMajorBit) == (int(Arg2Flags) & RowMajorBit) &&
+                         (int(Arg1Flags) & RowMajorBit) == (int(Arg3Flags) & RowMajorBit),
+    Flags0 = (int(Arg1Flags) | int(Arg2Flags) | int(Arg3Flags)) &
+             (HereditaryBits |
+              (int(Arg1Flags) & int(Arg2Flags) & int(Arg3Flags) &
+               ((StorageOrdersAgree ? LinearAccessBit : 0) |
+                (functor_traits<TernaryOp>::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0)))),
     Flags = (Flags0 & ~RowMajorBit) | (Arg1Flags & RowMajorBit),
-    Alignment = EIGEN_PLAIN_ENUM_MIN(
-        EIGEN_PLAIN_ENUM_MIN(evaluator<Arg1>::Alignment, evaluator<Arg2>::Alignment),
-        evaluator<Arg3>::Alignment)
+    Alignment = plain_enum_min(plain_enum_min(evaluator<Arg1>::Alignment, evaluator<Arg2>::Alignment),
+                               evaluator<Arg3>::Alignment)
   };
 
-  EIGEN_DEVICE_FUNC explicit ternary_evaluator(const XprType& xpr) : m_d(xpr)
-  {
+  EIGEN_DEVICE_FUNC explicit ternary_evaluator(const XprType& xpr) : m_d(xpr) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<TernaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_d.func()(m_d.arg1Impl.coeff(row, col), m_d.arg2Impl.coeff(row, col), m_d.arg3Impl.coeff(row, col));
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return m_d.func()(m_d.arg1Impl.coeff(index), m_d.arg2Impl.coeff(index), m_d.arg3Impl.coeff(index));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
-    return m_d.func().packetOp(m_d.arg1Impl.template packet<LoadMode,PacketType>(row, col),
-                               m_d.arg2Impl.template packet<LoadMode,PacketType>(row, col),
-                               m_d.arg3Impl.template packet<LoadMode,PacketType>(row, col));
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    return m_d.func().packetOp(m_d.arg1Impl.template packet<LoadMode, PacketType>(row, col),
+                               m_d.arg2Impl.template packet<LoadMode, PacketType>(row, col),
+                               m_d.arg3Impl.template packet<LoadMode, PacketType>(row, col));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
-    return m_d.func().packetOp(m_d.arg1Impl.template packet<LoadMode,PacketType>(index),
-                               m_d.arg2Impl.template packet<LoadMode,PacketType>(index),
-                               m_d.arg3Impl.template packet<LoadMode,PacketType>(index));
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
+    return m_d.func().packetOp(m_d.arg1Impl.template packet<LoadMode, PacketType>(index),
+                               m_d.arg2Impl.template packet<LoadMode, PacketType>(index),
+                               m_d.arg3Impl.template packet<LoadMode, PacketType>(index));
   }
 
-protected:
+ protected:
   // this helper permits to completely eliminate the functor if it is empty
-  struct Data
-  {
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Data(const XprType& xpr) : op(xpr.functor()), arg1Impl(xpr.arg1()), arg2Impl(xpr.arg2()), arg3Impl(xpr.arg3()) {}
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const TernaryOp& func() const { return op; }
+  struct Data {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr)
+        : op(xpr.functor()), arg1Impl(xpr.arg1()), arg2Impl(xpr.arg2()), arg3Impl(xpr.arg3()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const TernaryOp& func() const { return op; }
     TernaryOp op;
     evaluator<Arg1> arg1Impl;
     evaluator<Arg2> arg2Impl;
@@ -718,88 +843,69 @@
 // -------------------- CwiseBinaryOp --------------------
 
 // this is a binary expression
-template<typename BinaryOp, typename Lhs, typename Rhs>
-struct evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-  : public binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-{
+template <typename BinaryOp, typename Lhs, typename Rhs>
+struct evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > : public binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > {
   typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
   typedef binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > Base;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const XprType& xpr) : Base(xpr) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
-template<typename BinaryOp, typename Lhs, typename Rhs>
+template <typename BinaryOp, typename Lhs, typename Rhs>
 struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IndexBased, IndexBased>
-  : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-{
+    : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > {
   typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
 
   enum {
-    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    CoeffReadCost =
+        int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
 
     LhsFlags = evaluator<Lhs>::Flags,
     RhsFlags = evaluator<Rhs>::Flags,
-    SameType = is_same<typename Lhs::Scalar,typename Rhs::Scalar>::value,
-    StorageOrdersAgree = (int(LhsFlags)&RowMajorBit)==(int(RhsFlags)&RowMajorBit),
-    Flags0 = (int(LhsFlags) | int(RhsFlags)) & (
-        HereditaryBits
-      | (int(LhsFlags) & int(RhsFlags) &
-           ( (StorageOrdersAgree ? LinearAccessBit : 0)
-           | (functor_traits<BinaryOp>::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0)
-           )
-        )
-     ),
+    SameType = is_same<typename Lhs::Scalar, typename Rhs::Scalar>::value,
+    StorageOrdersAgree = (int(LhsFlags) & RowMajorBit) == (int(RhsFlags) & RowMajorBit),
+    Flags0 = (int(LhsFlags) | int(RhsFlags)) &
+             (HereditaryBits |
+              (int(LhsFlags) & int(RhsFlags) &
+               ((StorageOrdersAgree ? LinearAccessBit : 0) |
+                (functor_traits<BinaryOp>::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0)))),
     Flags = (Flags0 & ~RowMajorBit) | (LhsFlags & RowMajorBit),
-    Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<Lhs>::Alignment,evaluator<Rhs>::Alignment)
+    Alignment = plain_enum_min(evaluator<Lhs>::Alignment, evaluator<Rhs>::Alignment)
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit binary_evaluator(const XprType& xpr) : m_d(xpr)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit binary_evaluator(const XprType& xpr) : m_d(xpr) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_d.func()(m_d.lhsImpl.coeff(row, col), m_d.rhsImpl.coeff(row, col));
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return m_d.func()(m_d.lhsImpl.coeff(index), m_d.rhsImpl.coeff(index));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
-    return m_d.func().packetOp(m_d.lhsImpl.template packet<LoadMode,PacketType>(row, col),
-                               m_d.rhsImpl.template packet<LoadMode,PacketType>(row, col));
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    return m_d.func().packetOp(m_d.lhsImpl.template packet<LoadMode, PacketType>(row, col),
+                               m_d.rhsImpl.template packet<LoadMode, PacketType>(row, col));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
-    return m_d.func().packetOp(m_d.lhsImpl.template packet<LoadMode,PacketType>(index),
-                               m_d.rhsImpl.template packet<LoadMode,PacketType>(index));
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
+    return m_d.func().packetOp(m_d.lhsImpl.template packet<LoadMode, PacketType>(index),
+                               m_d.rhsImpl.template packet<LoadMode, PacketType>(index));
   }
 
-protected:
-
+ protected:
   // this helper permits to completely eliminate the functor if it is empty
-  struct Data
-  {
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Data(const XprType& xpr) : op(xpr.functor()), lhsImpl(xpr.lhs()), rhsImpl(xpr.rhs()) {}
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const BinaryOp& func() const { return op; }
+  struct Data {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr)
+        : op(xpr.functor()), lhsImpl(xpr.lhs()), rhsImpl(xpr.rhs()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const BinaryOp& func() const { return op; }
     BinaryOp op;
     evaluator<Lhs> lhsImpl;
     evaluator<Rhs> rhsImpl;
@@ -810,22 +916,20 @@
 
 // -------------------- CwiseUnaryView --------------------
 
-template<typename UnaryOp, typename ArgType>
-struct unary_evaluator<CwiseUnaryView<UnaryOp, ArgType>, IndexBased>
-  : evaluator_base<CwiseUnaryView<UnaryOp, ArgType> >
-{
-  typedef CwiseUnaryView<UnaryOp, ArgType> XprType;
+template <typename UnaryOp, typename ArgType, typename StrideType>
+struct unary_evaluator<CwiseUnaryView<UnaryOp, ArgType, StrideType>, IndexBased>
+    : evaluator_base<CwiseUnaryView<UnaryOp, ArgType, StrideType> > {
+  typedef CwiseUnaryView<UnaryOp, ArgType, StrideType> XprType;
 
   enum {
     CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
 
     Flags = (evaluator<ArgType>::Flags & (HereditaryBits | LinearAccessBit | DirectAccessBit)),
 
-    Alignment = 0 // FIXME it is not very clear why alignment is necessarily lost...
+    Alignment = 0  // FIXME it is not very clear why alignment is necessarily lost...
   };
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) : m_d(op)
-  {
+  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) : m_d(op) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
@@ -833,39 +937,28 @@
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_d.func()(m_d.argImpl.coeff(row, col));
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return m_d.func()(m_d.argImpl.coeff(index));
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
     return m_d.func()(m_d.argImpl.coeffRef(row, col));
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) {
     return m_d.func()(m_d.argImpl.coeffRef(index));
   }
 
-protected:
-
+ protected:
   // this helper permits to completely eliminate the functor if it is empty
-  struct Data
-  {
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Data(const XprType& xpr) : op(xpr.functor()), argImpl(xpr.nestedExpression()) {}
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const UnaryOp& func() const { return op; }
+  struct Data {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr)
+        : op(xpr.functor()), argImpl(xpr.nestedExpression()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryOp& func() const { return op; }
     UnaryOp op;
     evaluator<ArgType> argImpl;
   };
@@ -877,13 +970,12 @@
 
 // FIXME perhaps the PlainObjectType could be provided by Derived::PlainObject ?
 // but that might complicate template specialization
-template<typename Derived, typename PlainObjectType>
+template <typename Derived, typename PlainObjectType>
 struct mapbase_evaluator;
 
-template<typename Derived, typename PlainObjectType>
-struct mapbase_evaluator : evaluator_base<Derived>
-{
-  typedef Derived  XprType;
+template <typename Derived, typename PlainObjectType>
+struct mapbase_evaluator : evaluator_base<Derived> {
+  typedef Derived XprType;
   typedef typename XprType::PointerType PointerType;
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
@@ -894,78 +986,58 @@
     CoeffReadCost = NumTraits<Scalar>::ReadCost
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit mapbase_evaluator(const XprType& map)
-    : m_data(const_cast<PointerType>(map.data())),
-      m_innerStride(map.innerStride()),
-      m_outerStride(map.outerStride())
-  {
-    EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(evaluator<Derived>::Flags&PacketAccessBit, internal::inner_stride_at_compile_time<Derived>::ret==1),
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit mapbase_evaluator(const XprType& map)
+      : m_data(const_cast<PointerType>(map.data())),
+        m_innerStride(map.innerStride()),
+        m_outerStride(map.outerStride()) {
+    EIGEN_STATIC_ASSERT(check_implication((evaluator<Derived>::Flags & PacketAccessBit) != 0,
+                                          internal::inner_stride_at_compile_time<Derived>::ret == 1),
                         PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_data[col * colStride() + row * rowStride()];
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return m_data[index * m_innerStride.value()];
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
     return m_data[col * colStride() + row * rowStride()];
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
-    return m_data[index * m_innerStride.value()];
-  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { return m_data[index * m_innerStride.value()]; }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
     PointerType ptr = m_data + row * rowStride() + col * colStride();
     return internal::ploadt<PacketType, LoadMode>(ptr);
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
     return internal::ploadt<PacketType, LoadMode>(m_data + index * m_innerStride.value());
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index row, Index col, const PacketType& x)
-  {
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) {
     PointerType ptr = m_data + row * rowStride() + col * colStride();
     return internal::pstoret<Scalar, PacketType, StoreMode>(ptr, x);
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index index, const PacketType& x)
-  {
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) {
     internal::pstoret<Scalar, PacketType, StoreMode>(m_data + index * m_innerStride.value(), x);
   }
-protected:
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-  Index rowStride() const EIGEN_NOEXCEPT {
+
+ protected:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowStride() const EIGEN_NOEXCEPT {
     return XprType::IsRowMajor ? m_outerStride.value() : m_innerStride.value();
   }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-  Index colStride() const EIGEN_NOEXCEPT {
-     return XprType::IsRowMajor ? m_innerStride.value() : m_outerStride.value();
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colStride() const EIGEN_NOEXCEPT {
+    return XprType::IsRowMajor ? m_innerStride.value() : m_outerStride.value();
   }
 
   PointerType m_data;
@@ -973,10 +1045,9 @@
   const internal::variable_if_dynamic<Index, XprType::OuterStrideAtCompileTime> m_outerStride;
 };
 
-template<typename PlainObjectType, int MapOptions, typename StrideType>
+template <typename PlainObjectType, int MapOptions, typename StrideType>
 struct evaluator<Map<PlainObjectType, MapOptions, StrideType> >
-  : public mapbase_evaluator<Map<PlainObjectType, MapOptions, StrideType>, PlainObjectType>
-{
+    : public mapbase_evaluator<Map<PlainObjectType, MapOptions, StrideType>, PlainObjectType> {
   typedef Map<PlainObjectType, MapOptions, StrideType> XprType;
   typedef typename XprType::Scalar Scalar;
   // TODO: should check for smaller packet types once we can handle multi-sized packet types
@@ -984,34 +1055,32 @@
 
   enum {
     InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0
-                             ? int(PlainObjectType::InnerStrideAtCompileTime)
-                             : int(StrideType::InnerStrideAtCompileTime),
+                                   ? int(PlainObjectType::InnerStrideAtCompileTime)
+                                   : int(StrideType::InnerStrideAtCompileTime),
     OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0
-                             ? int(PlainObjectType::OuterStrideAtCompileTime)
-                             : int(StrideType::OuterStrideAtCompileTime),
+                                   ? int(PlainObjectType::OuterStrideAtCompileTime)
+                                   : int(StrideType::OuterStrideAtCompileTime),
     HasNoInnerStride = InnerStrideAtCompileTime == 1,
     HasNoOuterStride = StrideType::OuterStrideAtCompileTime == 0,
     HasNoStride = HasNoInnerStride && HasNoOuterStride,
-    IsDynamicSize = PlainObjectType::SizeAtCompileTime==Dynamic,
+    IsDynamicSize = PlainObjectType::SizeAtCompileTime == Dynamic,
 
     PacketAccessMask = bool(HasNoInnerStride) ? ~int(0) : ~int(PacketAccessBit),
-    LinearAccessMask = bool(HasNoStride) || bool(PlainObjectType::IsVectorAtCompileTime) ? ~int(0) : ~int(LinearAccessBit),
-    Flags = int( evaluator<PlainObjectType>::Flags) & (LinearAccessMask&PacketAccessMask),
+    LinearAccessMask =
+        bool(HasNoStride) || bool(PlainObjectType::IsVectorAtCompileTime) ? ~int(0) : ~int(LinearAccessBit),
+    Flags = int(evaluator<PlainObjectType>::Flags) & (LinearAccessMask & PacketAccessMask),
 
-    Alignment = int(MapOptions)&int(AlignedMask)
+    Alignment = int(MapOptions) & int(AlignedMask)
   };
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& map)
-    : mapbase_evaluator<XprType, PlainObjectType>(map)
-  { }
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& map) : mapbase_evaluator<XprType, PlainObjectType>(map) {}
 };
 
 // -------------------- Ref --------------------
 
-template<typename PlainObjectType, int RefOptions, typename StrideType>
+template <typename PlainObjectType, int RefOptions, typename StrideType>
 struct evaluator<Ref<PlainObjectType, RefOptions, StrideType> >
-  : public mapbase_evaluator<Ref<PlainObjectType, RefOptions, StrideType>, PlainObjectType>
-{
+    : public mapbase_evaluator<Ref<PlainObjectType, RefOptions, StrideType>, PlainObjectType> {
   typedef Ref<PlainObjectType, RefOptions, StrideType> XprType;
 
   enum {
@@ -1019,21 +1088,19 @@
     Alignment = evaluator<Map<PlainObjectType, RefOptions, StrideType> >::Alignment
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const XprType& ref)
-    : mapbase_evaluator<XprType, PlainObjectType>(ref)
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& ref)
+      : mapbase_evaluator<XprType, PlainObjectType>(ref) {}
 };
 
 // -------------------- Block --------------------
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel,
-         bool HasDirectAccess = internal::has_direct_access<ArgType>::ret> struct block_evaluator;
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel,
+          bool HasDirectAccess = internal::has_direct_access<ArgType>::ret>
+struct block_evaluator;
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
 struct evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
-  : block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel>
-{
+    : block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel> {
   typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
   typedef typename XprType::Scalar Scalar;
   // TODO: should check for smaller packet types once we can handle multi-sized packet types
@@ -1047,323 +1114,272 @@
     MaxRowsAtCompileTime = traits<XprType>::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = traits<XprType>::MaxColsAtCompileTime,
 
-    ArgTypeIsRowMajor = (int(evaluator<ArgType>::Flags)&RowMajorBit) != 0,
-    IsRowMajor = (MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1) ? 1
-               : (MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1) ? 0
-               : ArgTypeIsRowMajor,
+    ArgTypeIsRowMajor = (int(evaluator<ArgType>::Flags) & RowMajorBit) != 0,
+    IsRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1)   ? 1
+                 : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0
+                                                                            : ArgTypeIsRowMajor,
     HasSameStorageOrderAsArgType = (IsRowMajor == ArgTypeIsRowMajor),
     InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
-    InnerStrideAtCompileTime = HasSameStorageOrderAsArgType
-                             ? int(inner_stride_at_compile_time<ArgType>::ret)
-                             : int(outer_stride_at_compile_time<ArgType>::ret),
-    OuterStrideAtCompileTime = HasSameStorageOrderAsArgType
-                             ? int(outer_stride_at_compile_time<ArgType>::ret)
-                             : int(inner_stride_at_compile_time<ArgType>::ret),
+    InnerStrideAtCompileTime = HasSameStorageOrderAsArgType ? int(inner_stride_at_compile_time<ArgType>::ret)
+                                                            : int(outer_stride_at_compile_time<ArgType>::ret),
+    OuterStrideAtCompileTime = HasSameStorageOrderAsArgType ? int(outer_stride_at_compile_time<ArgType>::ret)
+                                                            : int(inner_stride_at_compile_time<ArgType>::ret),
     MaskPacketAccessBit = (InnerStrideAtCompileTime == 1 || HasSameStorageOrderAsArgType) ? PacketAccessBit : 0,
 
-    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (evaluator<ArgType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,
-    FlagsRowMajorBit = XprType::Flags&RowMajorBit,
-    Flags0 = evaluator<ArgType>::Flags & ( (HereditaryBits & ~RowMajorBit) |
-                                           DirectAccessBit |
-                                           MaskPacketAccessBit),
+    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 ||
+                            (InnerPanel && (evaluator<ArgType>::Flags & LinearAccessBit)))
+                               ? LinearAccessBit
+                               : 0,
+    FlagsRowMajorBit = XprType::Flags & RowMajorBit,
+    Flags0 = evaluator<ArgType>::Flags & ((HereditaryBits & ~RowMajorBit) | DirectAccessBit | MaskPacketAccessBit),
     Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit,
 
     PacketAlignment = unpacket_traits<PacketScalar>::alignment,
-    Alignment0 = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic)
-                             && (OuterStrideAtCompileTime!=0)
-                             && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) ? int(PacketAlignment) : 0,
-    Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<ArgType>::Alignment, Alignment0)
+    Alignment0 = (InnerPanel && (OuterStrideAtCompileTime != Dynamic) && (OuterStrideAtCompileTime != 0) &&
+                  (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0))
+                     ? int(PacketAlignment)
+                     : 0,
+    Alignment = plain_enum_min(evaluator<ArgType>::Alignment, Alignment0)
   };
   typedef block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel> block_evaluator_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const XprType& block) : block_evaluator_type(block)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& block) : block_evaluator_type(block) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 };
 
 // no direct-access => dispatch to a unary evaluator
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
 struct block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel, /*HasDirectAccess*/ false>
-  : unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
-{
+    : unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> > {
   typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit block_evaluator(const XprType& block)
-    : unary_evaluator<XprType>(block)
-  {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit block_evaluator(const XprType& block)
+      : unary_evaluator<XprType>(block) {}
 };
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
 struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBased>
-  : evaluator_base<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
-{
+    : evaluator_base<Block<ArgType, BlockRows, BlockCols, InnerPanel> > {
   typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& block)
-    : m_argImpl(block.nestedExpression()),
-      m_startRow(block.startRow()),
-      m_startCol(block.startCol()),
-      m_linear_offset(ForwardLinearAccess?(ArgType::IsRowMajor ? block.startRow()*block.nestedExpression().cols() + block.startCol() : block.startCol()*block.nestedExpression().rows() + block.startRow()):0)
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& block)
+      : m_argImpl(block.nestedExpression()),
+        m_startRow(block.startRow()),
+        m_startCol(block.startCol()),
+        m_linear_offset(ForwardLinearAccess
+                            ? (ArgType::IsRowMajor
+                                   ? block.startRow() * block.nestedExpression().cols() + block.startCol()
+                                   : block.startCol() * block.nestedExpression().rows() + block.startRow())
+                            : 0) {}
 
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
   enum {
     RowsAtCompileTime = XprType::RowsAtCompileTime,
-    ForwardLinearAccess = (InnerPanel || int(XprType::IsRowMajor)==int(ArgType::IsRowMajor)) && bool(evaluator<ArgType>::Flags&LinearAccessBit)
+    ForwardLinearAccess = (InnerPanel || int(XprType::IsRowMajor) == int(ArgType::IsRowMajor)) &&
+                          bool(evaluator<ArgType>::Flags & LinearAccessBit)
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_argImpl.coeff(m_startRow.value() + row, m_startCol.value() + col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return linear_coeff_impl(index, bool_constant<ForwardLinearAccess>());
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
     return m_argImpl.coeffRef(m_startRow.value() + row, m_startCol.value() + col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) {
     return linear_coeffRef_impl(index, bool_constant<ForwardLinearAccess>());
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
-    return m_argImpl.template packet<LoadMode,PacketType>(m_startRow.value() + row, m_startCol.value() + col);
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    return m_argImpl.template packet<LoadMode, PacketType>(m_startRow.value() + row, m_startCol.value() + col);
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
     if (ForwardLinearAccess)
-      return m_argImpl.template packet<LoadMode,PacketType>(m_linear_offset.value() + index);
+      return m_argImpl.template packet<LoadMode, PacketType>(m_linear_offset.value() + index);
     else
-      return packet<LoadMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
-                                         RowsAtCompileTime == 1 ? index : 0);
+      return packet<LoadMode, PacketType>(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index row, Index col, const PacketType& x)
-  {
-    return m_argImpl.template writePacket<StoreMode,PacketType>(m_startRow.value() + row, m_startCol.value() + col, x);
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) {
+    return m_argImpl.template writePacket<StoreMode, PacketType>(m_startRow.value() + row, m_startCol.value() + col, x);
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index index, const PacketType& x)
-  {
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) {
     if (ForwardLinearAccess)
-      return m_argImpl.template writePacket<StoreMode,PacketType>(m_linear_offset.value() + index, x);
+      return m_argImpl.template writePacket<StoreMode, PacketType>(m_linear_offset.value() + index, x);
     else
-      return writePacket<StoreMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
-                                              RowsAtCompileTime == 1 ? index : 0,
-                                              x);
+      return writePacket<StoreMode, PacketType>(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0,
+                                                x);
   }
 
-protected:
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType linear_coeff_impl(Index index, internal::true_type /* ForwardLinearAccess */) const
-  {
+ protected:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType
+  linear_coeff_impl(Index index, internal::true_type /* ForwardLinearAccess */) const {
     return m_argImpl.coeff(m_linear_offset.value() + index);
   }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType linear_coeff_impl(Index index, internal::false_type /* not ForwardLinearAccess */) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType
+  linear_coeff_impl(Index index, internal::false_type /* not ForwardLinearAccess */) const {
     return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& linear_coeffRef_impl(Index index, internal::true_type /* ForwardLinearAccess */)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& linear_coeffRef_impl(Index index,
+                                                                     internal::true_type /* ForwardLinearAccess */) {
     return m_argImpl.coeffRef(m_linear_offset.value() + index);
   }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& linear_coeffRef_impl(Index index, internal::false_type /* not ForwardLinearAccess */)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& linear_coeffRef_impl(
+      Index index, internal::false_type /* not ForwardLinearAccess */) {
     return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
   }
 
   evaluator<ArgType> m_argImpl;
-  const variable_if_dynamic<Index, (ArgType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
-  const variable_if_dynamic<Index, (ArgType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
+  const variable_if_dynamic<Index, (ArgType::RowsAtCompileTime == 1 && BlockRows == 1) ? 0 : Dynamic> m_startRow;
+  const variable_if_dynamic<Index, (ArgType::ColsAtCompileTime == 1 && BlockCols == 1) ? 0 : Dynamic> m_startCol;
   const variable_if_dynamic<Index, ForwardLinearAccess ? Dynamic : 0> m_linear_offset;
 };
 
 // TODO: This evaluator does not actually use the child evaluator;
 // all action is via the data() as returned by the Block expression.
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
 struct block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel, /* HasDirectAccess */ true>
-  : mapbase_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>,
-                      typename Block<ArgType, BlockRows, BlockCols, InnerPanel>::PlainObject>
-{
+    : mapbase_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>,
+                        typename Block<ArgType, BlockRows, BlockCols, InnerPanel>::PlainObject> {
   typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
   typedef typename XprType::Scalar Scalar;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit block_evaluator(const XprType& block)
-    : mapbase_evaluator<XprType, typename XprType::PlainObject>(block)
-  {
-    // TODO: for the 3.3 release, this should be turned to an internal assertion, but let's keep it as is for the beta lifetime
-    eigen_assert(((internal::UIntPtr(block.data()) % EIGEN_PLAIN_ENUM_MAX(1,evaluator<XprType>::Alignment)) == 0) && "data is not aligned");
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit block_evaluator(const XprType& block)
+      : mapbase_evaluator<XprType, typename XprType::PlainObject>(block) {
+    eigen_internal_assert((internal::is_constant_evaluated() ||
+                           (std::uintptr_t(block.data()) % plain_enum_max(1, evaluator<XprType>::Alignment)) == 0) &&
+                          "data is not aligned");
   }
 };
 
-
 // -------------------- Select --------------------
 // NOTE shall we introduce a ternary_evaluator?
 
 // TODO enable vectorization for Select
-template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
+template <typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
 struct evaluator<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >
-  : evaluator_base<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >
-{
+    : evaluator_base<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> > {
   typedef Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> XprType;
   enum {
-    CoeffReadCost = evaluator<ConditionMatrixType>::CoeffReadCost
-                  + EIGEN_PLAIN_ENUM_MAX(evaluator<ThenMatrixType>::CoeffReadCost,
-                                         evaluator<ElseMatrixType>::CoeffReadCost),
+    CoeffReadCost = evaluator<ConditionMatrixType>::CoeffReadCost +
+                    plain_enum_max(evaluator<ThenMatrixType>::CoeffReadCost, evaluator<ElseMatrixType>::CoeffReadCost),
 
     Flags = (unsigned int)evaluator<ThenMatrixType>::Flags & evaluator<ElseMatrixType>::Flags & HereditaryBits,
 
-    Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<ThenMatrixType>::Alignment, evaluator<ElseMatrixType>::Alignment)
+    Alignment = plain_enum_min(evaluator<ThenMatrixType>::Alignment, evaluator<ElseMatrixType>::Alignment)
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const XprType& select)
-    : m_conditionImpl(select.conditionMatrix()),
-      m_thenImpl(select.thenMatrix()),
-      m_elseImpl(select.elseMatrix())
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& select)
+      : m_conditionImpl(select.conditionMatrix()), m_thenImpl(select.thenMatrix()), m_elseImpl(select.elseMatrix()) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     if (m_conditionImpl.coeff(row, col))
       return m_thenImpl.coeff(row, col);
     else
       return m_elseImpl.coeff(row, col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     if (m_conditionImpl.coeff(index))
       return m_thenImpl.coeff(index);
     else
       return m_elseImpl.coeff(index);
   }
 
-protected:
+ protected:
   evaluator<ConditionMatrixType> m_conditionImpl;
   evaluator<ThenMatrixType> m_thenImpl;
   evaluator<ElseMatrixType> m_elseImpl;
 };
 
-
 // -------------------- Replicate --------------------
 
-template<typename ArgType, int RowFactor, int ColFactor>
+template <typename ArgType, int RowFactor, int ColFactor>
 struct unary_evaluator<Replicate<ArgType, RowFactor, ColFactor> >
-  : evaluator_base<Replicate<ArgType, RowFactor, ColFactor> >
-{
+    : evaluator_base<Replicate<ArgType, RowFactor, ColFactor> > {
   typedef Replicate<ArgType, RowFactor, ColFactor> XprType;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
-  enum {
-    Factor = (RowFactor==Dynamic || ColFactor==Dynamic) ? Dynamic : RowFactor*ColFactor
-  };
-  typedef typename internal::nested_eval<ArgType,Factor>::type ArgTypeNested;
-  typedef typename internal::remove_all<ArgTypeNested>::type ArgTypeNestedCleaned;
+  enum { Factor = (RowFactor == Dynamic || ColFactor == Dynamic) ? Dynamic : RowFactor * ColFactor };
+  typedef typename internal::nested_eval<ArgType, Factor>::type ArgTypeNested;
+  typedef internal::remove_all_t<ArgTypeNested> ArgTypeNestedCleaned;
 
   enum {
     CoeffReadCost = evaluator<ArgTypeNestedCleaned>::CoeffReadCost,
     LinearAccessMask = XprType::IsVectorAtCompileTime ? LinearAccessBit : 0,
-    Flags = (evaluator<ArgTypeNestedCleaned>::Flags & (HereditaryBits|LinearAccessMask) & ~RowMajorBit) | (traits<XprType>::Flags & RowMajorBit),
+    Flags = (evaluator<ArgTypeNestedCleaned>::Flags & (HereditaryBits | LinearAccessMask) & ~RowMajorBit) |
+            (traits<XprType>::Flags & RowMajorBit),
 
     Alignment = evaluator<ArgTypeNestedCleaned>::Alignment
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& replicate)
-    : m_arg(replicate.nestedExpression()),
-      m_argImpl(m_arg),
-      m_rows(replicate.nestedExpression().rows()),
-      m_cols(replicate.nestedExpression().cols())
-  {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& replicate)
+      : m_arg(replicate.nestedExpression()),
+        m_argImpl(m_arg),
+        m_rows(replicate.nestedExpression().rows()),
+        m_cols(replicate.nestedExpression().cols()) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     // try to avoid using modulo; this is a pure optimization strategy
-    const Index actual_row = internal::traits<XprType>::RowsAtCompileTime==1 ? 0
-                           : RowFactor==1 ? row
-                           : row % m_rows.value();
-    const Index actual_col = internal::traits<XprType>::ColsAtCompileTime==1 ? 0
-                           : ColFactor==1 ? col
-                           : col % m_cols.value();
+    const Index actual_row = internal::traits<XprType>::RowsAtCompileTime == 1 ? 0
+                             : RowFactor == 1                                  ? row
+                                                                               : row % m_rows.value();
+    const Index actual_col = internal::traits<XprType>::ColsAtCompileTime == 1 ? 0
+                             : ColFactor == 1                                  ? col
+                                                                               : col % m_cols.value();
 
     return m_argImpl.coeff(actual_row, actual_col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     // try to avoid using modulo; this is a pure optimization strategy
-    const Index actual_index = internal::traits<XprType>::RowsAtCompileTime==1
-                                  ? (ColFactor==1 ?  index : index%m_cols.value())
-                                  : (RowFactor==1 ?  index : index%m_rows.value());
+    const Index actual_index = internal::traits<XprType>::RowsAtCompileTime == 1
+                                   ? (ColFactor == 1 ? index : index % m_cols.value())
+                                   : (RowFactor == 1 ? index : index % m_rows.value());
 
     return m_argImpl.coeff(actual_index);
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
-    const Index actual_row = internal::traits<XprType>::RowsAtCompileTime==1 ? 0
-                           : RowFactor==1 ? row
-                           : row % m_rows.value();
-    const Index actual_col = internal::traits<XprType>::ColsAtCompileTime==1 ? 0
-                           : ColFactor==1 ? col
-                           : col % m_cols.value();
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    const Index actual_row = internal::traits<XprType>::RowsAtCompileTime == 1 ? 0
+                             : RowFactor == 1                                  ? row
+                                                                               : row % m_rows.value();
+    const Index actual_col = internal::traits<XprType>::ColsAtCompileTime == 1 ? 0
+                             : ColFactor == 1                                  ? col
+                                                                               : col % m_cols.value();
 
-    return m_argImpl.template packet<LoadMode,PacketType>(actual_row, actual_col);
+    return m_argImpl.template packet<LoadMode, PacketType>(actual_row, actual_col);
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
-    const Index actual_index = internal::traits<XprType>::RowsAtCompileTime==1
-                                  ? (ColFactor==1 ?  index : index%m_cols.value())
-                                  : (RowFactor==1 ?  index : index%m_rows.value());
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
+    const Index actual_index = internal::traits<XprType>::RowsAtCompileTime == 1
+                                   ? (ColFactor == 1 ? index : index % m_cols.value())
+                                   : (RowFactor == 1 ? index : index % m_rows.value());
 
-    return m_argImpl.template packet<LoadMode,PacketType>(actual_index);
+    return m_argImpl.template packet<LoadMode, PacketType>(actual_index);
   }
 
-protected:
+ protected:
   const ArgTypeNested m_arg;
   evaluator<ArgTypeNestedCleaned> m_argImpl;
   const variable_if_dynamic<Index, ArgType::RowsAtCompileTime> m_rows;
@@ -1375,113 +1391,78 @@
 // evaluator_wrapper_base<T> is a common base class for the
 // MatrixWrapper and ArrayWrapper evaluators.
 
-template<typename XprType>
-struct evaluator_wrapper_base
-  : evaluator_base<XprType>
-{
-  typedef typename remove_all<typename XprType::NestedExpressionType>::type ArgType;
+template <typename XprType>
+struct evaluator_wrapper_base : evaluator_base<XprType> {
+  typedef remove_all_t<typename XprType::NestedExpressionType> ArgType;
   enum {
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
     Flags = evaluator<ArgType>::Flags,
     Alignment = evaluator<ArgType>::Alignment
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator_wrapper_base(const ArgType& arg) : m_argImpl(arg) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator_wrapper_base(const ArgType& arg) : m_argImpl(arg) {}
 
   typedef typename ArgType::Scalar Scalar;
   typedef typename ArgType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
     return m_argImpl.coeff(row, col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
-    return m_argImpl.coeff(index);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { return m_argImpl.coeff(index); }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { return m_argImpl.coeffRef(row, col); }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { return m_argImpl.coeffRef(index); }
+
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    return m_argImpl.template packet<LoadMode, PacketType>(row, col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
-    return m_argImpl.coeffRef(row, col);
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
+    return m_argImpl.template packet<LoadMode, PacketType>(index);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
-    return m_argImpl.coeffRef(index);
-  }
-
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
-    return m_argImpl.template packet<LoadMode,PacketType>(row, col);
-  }
-
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
-    return m_argImpl.template packet<LoadMode,PacketType>(index);
-  }
-
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index row, Index col, const PacketType& x)
-  {
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) {
     m_argImpl.template writePacket<StoreMode>(row, col, x);
   }
 
-  template<int StoreMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index index, const PacketType& x)
-  {
+  template <int StoreMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) {
     m_argImpl.template writePacket<StoreMode>(index, x);
   }
 
-protected:
+ protected:
   evaluator<ArgType> m_argImpl;
 };
 
-template<typename TArgType>
-struct unary_evaluator<MatrixWrapper<TArgType> >
-  : evaluator_wrapper_base<MatrixWrapper<TArgType> >
-{
+template <typename TArgType>
+struct unary_evaluator<MatrixWrapper<TArgType> > : evaluator_wrapper_base<MatrixWrapper<TArgType> > {
   typedef MatrixWrapper<TArgType> XprType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& wrapper)
-    : evaluator_wrapper_base<MatrixWrapper<TArgType> >(wrapper.nestedExpression())
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& wrapper)
+      : evaluator_wrapper_base<MatrixWrapper<TArgType> >(wrapper.nestedExpression()) {}
 };
 
-template<typename TArgType>
-struct unary_evaluator<ArrayWrapper<TArgType> >
-  : evaluator_wrapper_base<ArrayWrapper<TArgType> >
-{
+template <typename TArgType>
+struct unary_evaluator<ArrayWrapper<TArgType> > : evaluator_wrapper_base<ArrayWrapper<TArgType> > {
   typedef ArrayWrapper<TArgType> XprType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& wrapper)
-    : evaluator_wrapper_base<ArrayWrapper<TArgType> >(wrapper.nestedExpression())
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& wrapper)
+      : evaluator_wrapper_base<ArrayWrapper<TArgType> >(wrapper.nestedExpression()) {}
 };
 
-
 // -------------------- Reverse --------------------
 
 // defined in Reverse.h:
-template<typename PacketType, bool ReversePacket> struct reverse_packet_cond;
+template <typename PacketType, bool ReversePacket>
+struct reverse_packet_cond;
 
-template<typename ArgType, int Direction>
-struct unary_evaluator<Reverse<ArgType, Direction> >
-  : evaluator_base<Reverse<ArgType, Direction> >
-{
+template <typename ArgType, int Direction>
+struct unary_evaluator<Reverse<ArgType, Direction> > : evaluator_base<Reverse<ArgType, Direction> > {
   typedef Reverse<ArgType, Direction> XprType;
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
@@ -1489,109 +1470,88 @@
   enum {
     IsRowMajor = XprType::IsRowMajor,
     IsColMajor = !IsRowMajor,
-    ReverseRow = (Direction == Vertical)   || (Direction == BothDirections),
+    ReverseRow = (Direction == Vertical) || (Direction == BothDirections),
     ReverseCol = (Direction == Horizontal) || (Direction == BothDirections),
-    ReversePacket = (Direction == BothDirections)
-                    || ((Direction == Vertical)   && IsColMajor)
-                    || ((Direction == Horizontal) && IsRowMajor),
+    ReversePacket = (Direction == BothDirections) || ((Direction == Vertical) && IsColMajor) ||
+                    ((Direction == Horizontal) && IsRowMajor),
 
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
 
     // let's enable LinearAccess only with vectorization because of the product overhead
     // FIXME enable DirectAccess with negative strides?
     Flags0 = evaluator<ArgType>::Flags,
-    LinearAccess = ( (Direction==BothDirections) && (int(Flags0)&PacketAccessBit) )
-                  || ((ReverseRow && XprType::ColsAtCompileTime==1) || (ReverseCol && XprType::RowsAtCompileTime==1))
-                 ? LinearAccessBit : 0,
+    LinearAccess =
+        ((Direction == BothDirections) && (int(Flags0) & PacketAccessBit)) ||
+                ((ReverseRow && XprType::ColsAtCompileTime == 1) || (ReverseCol && XprType::RowsAtCompileTime == 1))
+            ? LinearAccessBit
+            : 0,
 
     Flags = int(Flags0) & (HereditaryBits | PacketAccessBit | LinearAccess),
 
-    Alignment = 0 // FIXME in some rare cases, Alignment could be preserved, like a Vector4f.
+    Alignment = 0  // FIXME in some rare cases, Alignment could be preserved, like a Vector4f.
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& reverse)
-    : m_argImpl(reverse.nestedExpression()),
-      m_rows(ReverseRow ? reverse.nestedExpression().rows() : 1),
-      m_cols(ReverseCol ? reverse.nestedExpression().cols() : 1)
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& reverse)
+      : m_argImpl(reverse.nestedExpression()),
+        m_rows(ReverseRow ? reverse.nestedExpression().rows() : 1),
+        m_cols(ReverseCol ? reverse.nestedExpression().cols() : 1) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
-    return m_argImpl.coeff(ReverseRow ? m_rows.value() - row - 1 : row,
-                           ReverseCol ? m_cols.value() - col - 1 : col);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
+    return m_argImpl.coeff(ReverseRow ? m_rows.value() - row - 1 : row, ReverseCol ? m_cols.value() - col - 1 : col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return m_argImpl.coeff(m_rows.value() * m_cols.value() - index - 1);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
-    return m_argImpl.coeffRef(ReverseRow ? m_rows.value() - row - 1 : row,
-                              ReverseCol ? m_cols.value() - col - 1 : col);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
+    return m_argImpl.coeffRef(ReverseRow ? m_rows.value() - row - 1 : row, ReverseCol ? m_cols.value() - col - 1 : col);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) {
     return m_argImpl.coeffRef(m_rows.value() * m_cols.value() - index - 1);
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
     enum {
       PacketSize = unpacket_traits<PacketType>::size,
-      OffsetRow  = ReverseRow && IsColMajor ? PacketSize : 1,
-      OffsetCol  = ReverseCol && IsRowMajor ? PacketSize : 1
+      OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1,
+      OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1
     };
-    typedef internal::reverse_packet_cond<PacketType,ReversePacket> reverse_packet;
-    return reverse_packet::run(m_argImpl.template packet<LoadMode,PacketType>(
-                                  ReverseRow ? m_rows.value() - row - OffsetRow : row,
-                                  ReverseCol ? m_cols.value() - col - OffsetCol : col));
+    typedef internal::reverse_packet_cond<PacketType, ReversePacket> reverse_packet;
+    return reverse_packet::run(m_argImpl.template packet<LoadMode, PacketType>(
+        ReverseRow ? m_rows.value() - row - OffsetRow : row, ReverseCol ? m_cols.value() - col - OffsetCol : col));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index index) const {
     enum { PacketSize = unpacket_traits<PacketType>::size };
-    return preverse(m_argImpl.template packet<LoadMode,PacketType>(m_rows.value() * m_cols.value() - index - PacketSize));
+    return preverse(
+        m_argImpl.template packet<LoadMode, PacketType>(m_rows.value() * m_cols.value() - index - PacketSize));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index row, Index col, const PacketType& x)
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) {
     // FIXME we could factorize some code with packet(i,j)
     enum {
       PacketSize = unpacket_traits<PacketType>::size,
-      OffsetRow  = ReverseRow && IsColMajor ? PacketSize : 1,
-      OffsetCol  = ReverseCol && IsRowMajor ? PacketSize : 1
+      OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1,
+      OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1
     };
-    typedef internal::reverse_packet_cond<PacketType,ReversePacket> reverse_packet;
-    m_argImpl.template writePacket<LoadMode>(
-                                  ReverseRow ? m_rows.value() - row - OffsetRow : row,
-                                  ReverseCol ? m_cols.value() - col - OffsetCol : col,
-                                  reverse_packet::run(x));
+    typedef internal::reverse_packet_cond<PacketType, ReversePacket> reverse_packet;
+    m_argImpl.template writePacket<LoadMode>(ReverseRow ? m_rows.value() - row - OffsetRow : row,
+                                             ReverseCol ? m_cols.value() - col - OffsetCol : col,
+                                             reverse_packet::run(x));
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  void writePacket(Index index, const PacketType& x)
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) {
     enum { PacketSize = unpacket_traits<PacketType>::size };
-    m_argImpl.template writePacket<LoadMode>
-      (m_rows.value() * m_cols.value() - index - PacketSize, preverse(x));
+    m_argImpl.template writePacket<LoadMode>(m_rows.value() * m_cols.value() - index - PacketSize, preverse(x));
   }
 
-protected:
+ protected:
   evaluator<ArgType> m_argImpl;
 
   // If we do not reverse rows, then we do not need to know the number of rows; same for columns
@@ -1600,68 +1560,56 @@
   const variable_if_dynamic<Index, ReverseCol ? ArgType::ColsAtCompileTime : 1> m_cols;
 };
 
-
 // -------------------- Diagonal --------------------
 
-template<typename ArgType, int DiagIndex>
-struct evaluator<Diagonal<ArgType, DiagIndex> >
-  : evaluator_base<Diagonal<ArgType, DiagIndex> >
-{
+template <typename ArgType, int DiagIndex>
+struct evaluator<Diagonal<ArgType, DiagIndex> > : evaluator_base<Diagonal<ArgType, DiagIndex> > {
   typedef Diagonal<ArgType, DiagIndex> XprType;
 
   enum {
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
 
-    Flags = (unsigned int)(evaluator<ArgType>::Flags & (HereditaryBits | DirectAccessBit) & ~RowMajorBit) | LinearAccessBit,
+    Flags =
+        (unsigned int)(evaluator<ArgType>::Flags & (HereditaryBits | DirectAccessBit) & ~RowMajorBit) | LinearAccessBit,
 
     Alignment = 0
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit evaluator(const XprType& diagonal)
-    : m_argImpl(diagonal.nestedExpression()),
-      m_index(diagonal.index())
-  { }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& diagonal)
+      : m_argImpl(diagonal.nestedExpression()), m_index(diagonal.index()) {}
 
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index) const {
     return m_argImpl.coeff(row + rowOffset(), row + colOffset());
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
     return m_argImpl.coeff(index + rowOffset(), index + colOffset());
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index) {
     return m_argImpl.coeffRef(row + rowOffset(), row + colOffset());
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) {
     return m_argImpl.coeffRef(index + rowOffset(), index + colOffset());
   }
 
-protected:
+ protected:
   evaluator<ArgType> m_argImpl;
   const internal::variable_if_dynamicindex<Index, XprType::DiagIndex> m_index;
 
-private:
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-  Index rowOffset() const { return m_index.value() > 0 ? 0 : -m_index.value(); }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-  Index colOffset() const { return m_index.value() > 0 ? m_index.value() : 0; }
+ private:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowOffset() const {
+    return m_index.value() > 0 ? 0 : -m_index.value();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colOffset() const {
+    return m_index.value() > 0 ? m_index.value() : 0;
+  }
 };
 
-
 //----------------------------------------------------------------------
 // deprecated code
 //----------------------------------------------------------------------
@@ -1670,72 +1618,49 @@
 
 // expression class for evaluating nested expression to a temporary
 
-template<typename ArgType> class EvalToTemp;
+template <typename ArgType>
+class EvalToTemp;
 
-template<typename ArgType>
-struct traits<EvalToTemp<ArgType> >
-  : public traits<ArgType>
-{ };
+template <typename ArgType>
+struct traits<EvalToTemp<ArgType> > : public traits<ArgType> {};
 
-template<typename ArgType>
-class EvalToTemp
-  : public dense_xpr_base<EvalToTemp<ArgType> >::type
-{
+template <typename ArgType>
+class EvalToTemp : public dense_xpr_base<EvalToTemp<ArgType> >::type {
  public:
-
   typedef typename dense_xpr_base<EvalToTemp>::type Base;
   EIGEN_GENERIC_PUBLIC_INTERFACE(EvalToTemp)
 
-  explicit EvalToTemp(const ArgType& arg)
-    : m_arg(arg)
-  { }
+  explicit EvalToTemp(const ArgType& arg) : m_arg(arg) {}
 
-  const ArgType& arg() const
-  {
-    return m_arg;
-  }
+  const ArgType& arg() const { return m_arg; }
 
-  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
-  {
-    return m_arg.rows();
-  }
+  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_arg.rows(); }
 
-  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
-  {
-    return m_arg.cols();
-  }
+  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_arg.cols(); }
 
  private:
   const ArgType& m_arg;
 };
 
-template<typename ArgType>
-struct evaluator<EvalToTemp<ArgType> >
-  : public evaluator<typename ArgType::PlainObject>
-{
-  typedef EvalToTemp<ArgType>                   XprType;
-  typedef typename ArgType::PlainObject         PlainObject;
+template <typename ArgType>
+struct evaluator<EvalToTemp<ArgType> > : public evaluator<typename ArgType::PlainObject> {
+  typedef EvalToTemp<ArgType> XprType;
+  typedef typename ArgType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr)
-    : m_result(xpr.arg())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : m_result(xpr.arg()) {
+    internal::construct_at<Base>(this, m_result);
   }
 
   // This constructor is used when nesting an EvalTo evaluator in another evaluator
-  EIGEN_DEVICE_FUNC evaluator(const ArgType& arg)
-    : m_result(arg)
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
-  }
+  EIGEN_DEVICE_FUNC evaluator(const ArgType& arg) : m_result(arg) { internal::construct_at<Base>(this, m_result); }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
-} // namespace internal
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COREEVALUATORS_H
+#endif  // EIGEN_COREEVALUATORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h
index b967196..f62cf23 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h
@@ -10,100 +10,111 @@
 #ifndef EIGEN_COREITERATORS_H
 #define EIGEN_COREITERATORS_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /* This file contains the respective InnerIterator definition of the expressions defined in Eigen/Core
  */
 
 namespace internal {
 
-template<typename XprType, typename EvaluatorKind>
+template <typename XprType, typename EvaluatorKind>
 class inner_iterator_selector;
 
 }
 
 /** \class InnerIterator
-  * \brief An InnerIterator allows to loop over the element of any matrix expression.
-  * 
-  * \warning To be used with care because an evaluator is constructed every time an InnerIterator iterator is constructed.
-  * 
-  * TODO: add a usage example
-  */
-template<typename XprType>
-class InnerIterator
-{
-protected:
+ * \brief An InnerIterator allows to loop over the element of any matrix expression.
+ *
+ * \warning To be used with care because an evaluator is constructed every time an InnerIterator iterator is
+ * constructed.
+ *
+ * TODO: add a usage example
+ */
+template <typename XprType>
+class InnerIterator {
+ protected:
   typedef internal::inner_iterator_selector<XprType, typename internal::evaluator_traits<XprType>::Kind> IteratorType;
   typedef internal::evaluator<XprType> EvaluatorType;
   typedef typename internal::traits<XprType>::Scalar Scalar;
-public:
+
+ public:
   /** Construct an iterator over the \a outerId -th row or column of \a xpr */
-  InnerIterator(const XprType &xpr, const Index &outerId)
-    : m_eval(xpr), m_iter(m_eval, outerId, xpr.innerSize())
-  {}
-  
+  InnerIterator(const XprType &xpr, const Index &outerId) : m_eval(xpr), m_iter(m_eval, outerId, xpr.innerSize()) {}
+
   /// \returns the value of the current coefficient.
-  EIGEN_STRONG_INLINE Scalar value() const          { return m_iter.value(); }
+  EIGEN_STRONG_INLINE Scalar value() const { return m_iter.value(); }
   /** Increment the iterator \c *this to the next non-zero coefficient.
-    * Explicit zeros are not skipped over. To skip explicit zeros, see class SparseView
-    */
-  EIGEN_STRONG_INLINE InnerIterator& operator++()   { m_iter.operator++(); return *this; }
-  EIGEN_STRONG_INLINE InnerIterator& operator+=(Index i) { m_iter.operator+=(i); return *this; }
-  EIGEN_STRONG_INLINE InnerIterator operator+(Index i) 
-  { InnerIterator result(*this); result+=i; return result; }
-    
+   * Explicit zeros are not skipped over. To skip explicit zeros, see class SparseView
+   */
+  EIGEN_STRONG_INLINE InnerIterator &operator++() {
+    m_iter.operator++();
+    return *this;
+  }
+  EIGEN_STRONG_INLINE InnerIterator &operator+=(Index i) {
+    m_iter.operator+=(i);
+    return *this;
+  }
+  EIGEN_STRONG_INLINE InnerIterator operator+(Index i) {
+    InnerIterator result(*this);
+    result += i;
+    return result;
+  }
 
   /// \returns the column or row index of the current coefficient.
-  EIGEN_STRONG_INLINE Index index() const           { return m_iter.index(); }
+  EIGEN_STRONG_INLINE Index index() const { return m_iter.index(); }
   /// \returns the row index of the current coefficient.
-  EIGEN_STRONG_INLINE Index row() const             { return m_iter.row(); }
+  EIGEN_STRONG_INLINE Index row() const { return m_iter.row(); }
   /// \returns the column index of the current coefficient.
-  EIGEN_STRONG_INLINE Index col() const             { return m_iter.col(); }
+  EIGEN_STRONG_INLINE Index col() const { return m_iter.col(); }
   /// \returns \c true if the iterator \c *this still references a valid coefficient.
-  EIGEN_STRONG_INLINE operator bool() const         { return m_iter; }
-  
-protected:
+  EIGEN_STRONG_INLINE operator bool() const { return m_iter; }
+
+ protected:
   EvaluatorType m_eval;
   IteratorType m_iter;
-private:
+
+ private:
   // If you get here, then you're not using the right InnerIterator type, e.g.:
   //   SparseMatrix<double,RowMajor> A;
   //   SparseMatrix<double>::InnerIterator it(A,0);
-  template<typename T> InnerIterator(const EigenBase<T>&,Index outer);
+  template <typename T>
+  InnerIterator(const EigenBase<T> &, Index outer);
 };
 
 namespace internal {
 
 // Generic inner iterator implementation for dense objects
-template<typename XprType>
-class inner_iterator_selector<XprType, IndexBased>
-{
-protected:
+template <typename XprType>
+class inner_iterator_selector<XprType, IndexBased> {
+ protected:
   typedef evaluator<XprType> EvaluatorType;
   typedef typename traits<XprType>::Scalar Scalar;
-  enum { IsRowMajor = (XprType::Flags&RowMajorBit)==RowMajorBit };
-  
-public:
-  EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &innerSize)
-    : m_eval(eval), m_inner(0), m_outer(outerId), m_end(innerSize)
-  {}
+  enum { IsRowMajor = (XprType::Flags & RowMajorBit) == RowMajorBit };
 
-  EIGEN_STRONG_INLINE Scalar value() const
-  {
-    return (IsRowMajor) ? m_eval.coeff(m_outer, m_inner)
-                        : m_eval.coeff(m_inner, m_outer);
+ public:
+  EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &innerSize)
+      : m_eval(eval), m_inner(0), m_outer(outerId), m_end(innerSize) {}
+
+  EIGEN_STRONG_INLINE Scalar value() const {
+    return (IsRowMajor) ? m_eval.coeff(m_outer, m_inner) : m_eval.coeff(m_inner, m_outer);
   }
 
-  EIGEN_STRONG_INLINE inner_iterator_selector& operator++() { m_inner++; return *this; }
+  EIGEN_STRONG_INLINE inner_iterator_selector &operator++() {
+    m_inner++;
+    return *this;
+  }
 
   EIGEN_STRONG_INLINE Index index() const { return m_inner; }
   inline Index row() const { return IsRowMajor ? m_outer : index(); }
   inline Index col() const { return IsRowMajor ? index() : m_outer; }
 
-  EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; }
+  EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner >= 0; }
 
-protected:
-  const EvaluatorType& m_eval;
+ protected:
+  const EvaluatorType &m_eval;
   Index m_inner;
   const Index m_outer;
   const Index m_end;
@@ -111,22 +122,20 @@
 
 // For iterator-based evaluator, inner-iterator is already implemented as
 // evaluator<>::InnerIterator
-template<typename XprType>
-class inner_iterator_selector<XprType, IteratorBased>
- : public evaluator<XprType>::InnerIterator
-{
-protected:
+template <typename XprType>
+class inner_iterator_selector<XprType, IteratorBased> : public evaluator<XprType>::InnerIterator {
+ protected:
   typedef typename evaluator<XprType>::InnerIterator Base;
   typedef evaluator<XprType> EvaluatorType;
-  
-public:
-  EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &/*innerSize*/)
-    : Base(eval, outerId)
-  {}  
+
+ public:
+  EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId,
+                                              const Index & /*innerSize*/)
+      : Base(eval, outerId) {}
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COREITERATORS_H
+#endif  // EIGEN_COREITERATORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h
index 2202b1c..aa79b60 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h
@@ -11,15 +11,17 @@
 #ifndef EIGEN_CWISE_BINARY_OP_H
 #define EIGEN_CWISE_BINARY_OP_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename BinaryOp, typename Lhs, typename Rhs>
-struct traits<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-{
+template <typename BinaryOp, typename Lhs, typename Rhs>
+struct traits<CwiseBinaryOp<BinaryOp, Lhs, Rhs>> {
   // we must not inherit from traits<Lhs> since it has
   // the potential to cause problems with MSVC
-  typedef typename remove_all<Lhs>::type Ancestor;
+  typedef remove_all_t<Lhs> Ancestor;
   typedef typename traits<Ancestor>::XprKind XprKind;
   enum {
     RowsAtCompileTime = traits<Ancestor>::RowsAtCompileTime,
@@ -30,154 +32,135 @@
 
   // even though we require Lhs and Rhs to have the same scalar type (see CwiseBinaryOp constructor),
   // we still want to handle the case when the result type is different.
-  typedef typename result_of<
-                     BinaryOp(
-                       const typename Lhs::Scalar&,
-                       const typename Rhs::Scalar&
-                     )
-                   >::type Scalar;
-  typedef typename cwise_promote_storage_type<typename traits<Lhs>::StorageKind,
-                                              typename traits<Rhs>::StorageKind,
+  typedef typename result_of<BinaryOp(const typename Lhs::Scalar&, const typename Rhs::Scalar&)>::type Scalar;
+  typedef typename cwise_promote_storage_type<typename traits<Lhs>::StorageKind, typename traits<Rhs>::StorageKind,
                                               BinaryOp>::ret StorageKind;
-  typedef typename promote_index_type<typename traits<Lhs>::StorageIndex,
-                                      typename traits<Rhs>::StorageIndex>::type StorageIndex;
+  typedef typename promote_index_type<typename traits<Lhs>::StorageIndex, typename traits<Rhs>::StorageIndex>::type
+      StorageIndex;
   typedef typename Lhs::Nested LhsNested;
   typedef typename Rhs::Nested RhsNested;
-  typedef typename remove_reference<LhsNested>::type _LhsNested;
-  typedef typename remove_reference<RhsNested>::type _RhsNested;
+  typedef std::remove_reference_t<LhsNested> LhsNested_;
+  typedef std::remove_reference_t<RhsNested> RhsNested_;
   enum {
-    Flags = cwise_promote_storage_order<typename traits<Lhs>::StorageKind,typename traits<Rhs>::StorageKind,_LhsNested::Flags & RowMajorBit,_RhsNested::Flags & RowMajorBit>::value
+    Flags = cwise_promote_storage_order<typename traits<Lhs>::StorageKind, typename traits<Rhs>::StorageKind,
+                                        LhsNested_::Flags & RowMajorBit, RhsNested_::Flags & RowMajorBit>::value
   };
 };
-} // end namespace internal
+}  // end namespace internal
 
-template<typename BinaryOp, typename Lhs, typename Rhs, typename StorageKind>
+template <typename BinaryOp, typename Lhs, typename Rhs, typename StorageKind>
 class CwiseBinaryOpImpl;
 
 /** \class CwiseBinaryOp
-  * \ingroup Core_Module
-  *
-  * \brief Generic expression where a coefficient-wise binary operator is applied to two expressions
-  *
-  * \tparam BinaryOp template functor implementing the operator
-  * \tparam LhsType the type of the left-hand side
-  * \tparam RhsType the type of the right-hand side
-  *
-  * This class represents an expression  where a coefficient-wise binary operator is applied to two expressions.
-  * It is the return type of binary operators, by which we mean only those binary operators where
-  * both the left-hand side and the right-hand side are Eigen expressions.
-  * For example, the return type of matrix1+matrix2 is a CwiseBinaryOp.
-  *
-  * Most of the time, this is the only way that it is used, so you typically don't have to name
-  * CwiseBinaryOp types explicitly.
-  *
-  * \sa MatrixBase::binaryExpr(const MatrixBase<OtherDerived> &,const CustomBinaryOp &) const, class CwiseUnaryOp, class CwiseNullaryOp
-  */
-template<typename BinaryOp, typename LhsType, typename RhsType>
-class CwiseBinaryOp :
-  public CwiseBinaryOpImpl<
-          BinaryOp, LhsType, RhsType,
-          typename internal::cwise_promote_storage_type<typename internal::traits<LhsType>::StorageKind,
-                                                        typename internal::traits<RhsType>::StorageKind,
-                                                        BinaryOp>::ret>,
-  internal::no_assignment_operator
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Generic expression where a coefficient-wise binary operator is applied to two expressions
+ *
+ * \tparam BinaryOp template functor implementing the operator
+ * \tparam LhsType the type of the left-hand side
+ * \tparam RhsType the type of the right-hand side
+ *
+ * This class represents an expression  where a coefficient-wise binary operator is applied to two expressions.
+ * It is the return type of binary operators, by which we mean only those binary operators where
+ * both the left-hand side and the right-hand side are Eigen expressions.
+ * For example, the return type of matrix1+matrix2 is a CwiseBinaryOp.
+ *
+ * Most of the time, this is the only way that it is used, so you typically don't have to name
+ * CwiseBinaryOp types explicitly.
+ *
+ * \sa MatrixBase::binaryExpr(const MatrixBase<OtherDerived> &,const CustomBinaryOp &) const, class CwiseUnaryOp, class
+ * CwiseNullaryOp
+ */
+template <typename BinaryOp, typename LhsType, typename RhsType>
+class CwiseBinaryOp : public CwiseBinaryOpImpl<BinaryOp, LhsType, RhsType,
+                                               typename internal::cwise_promote_storage_type<
+                                                   typename internal::traits<LhsType>::StorageKind,
+                                                   typename internal::traits<RhsType>::StorageKind, BinaryOp>::ret>,
+                      internal::no_assignment_operator {
+ public:
+  typedef internal::remove_all_t<BinaryOp> Functor;
+  typedef internal::remove_all_t<LhsType> Lhs;
+  typedef internal::remove_all_t<RhsType> Rhs;
 
-    typedef typename internal::remove_all<BinaryOp>::type Functor;
-    typedef typename internal::remove_all<LhsType>::type Lhs;
-    typedef typename internal::remove_all<RhsType>::type Rhs;
+  typedef typename CwiseBinaryOpImpl<
+      BinaryOp, LhsType, RhsType,
+      typename internal::cwise_promote_storage_type<typename internal::traits<LhsType>::StorageKind,
+                                                    typename internal::traits<Rhs>::StorageKind, BinaryOp>::ret>::Base
+      Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseBinaryOp)
 
-    typedef typename CwiseBinaryOpImpl<
-        BinaryOp, LhsType, RhsType,
-        typename internal::cwise_promote_storage_type<typename internal::traits<LhsType>::StorageKind,
-                                                      typename internal::traits<Rhs>::StorageKind,
-                                                      BinaryOp>::ret>::Base Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseBinaryOp)
+  EIGEN_CHECK_BINARY_COMPATIBILIY(BinaryOp, typename Lhs::Scalar, typename Rhs::Scalar)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs)
 
-    typedef typename internal::ref_selector<LhsType>::type LhsNested;
-    typedef typename internal::ref_selector<RhsType>::type RhsNested;
-    typedef typename internal::remove_reference<LhsNested>::type _LhsNested;
-    typedef typename internal::remove_reference<RhsNested>::type _RhsNested;
+  typedef typename internal::ref_selector<LhsType>::type LhsNested;
+  typedef typename internal::ref_selector<RhsType>::type RhsNested;
+  typedef std::remove_reference_t<LhsNested> LhsNested_;
+  typedef std::remove_reference_t<RhsNested> RhsNested_;
 
-#if EIGEN_COMP_MSVC && EIGEN_HAS_CXX11
-    //Required for Visual Studio or the Copy constructor will probably not get inlined!
-    EIGEN_STRONG_INLINE
-    CwiseBinaryOp(const CwiseBinaryOp<BinaryOp,LhsType,RhsType>&) = default;
+#if EIGEN_COMP_MSVC
+  // Required for Visual Studio or the Copy constructor will probably not get inlined!
+  EIGEN_STRONG_INLINE CwiseBinaryOp(const CwiseBinaryOp<BinaryOp, LhsType, RhsType>&) = default;
 #endif
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs, const BinaryOp& func = BinaryOp())
-      : m_lhs(aLhs), m_rhs(aRhs), m_functor(func)
-    {
-      EIGEN_CHECK_BINARY_COMPATIBILIY(BinaryOp,typename Lhs::Scalar,typename Rhs::Scalar);
-      // require the sizes to match
-      EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs)
-      eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols());
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs,
+                                                      const BinaryOp& func = BinaryOp())
+      : m_lhs(aLhs), m_rhs(aRhs), m_functor(func) {
+    eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols());
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT {
-      // return the fixed size type if available to enable compile time optimizations
-      return internal::traits<typename internal::remove_all<LhsNested>::type>::RowsAtCompileTime==Dynamic ? m_rhs.rows() : m_lhs.rows();
-    }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT {
-      // return the fixed size type if available to enable compile time optimizations
-      return internal::traits<typename internal::remove_all<LhsNested>::type>::ColsAtCompileTime==Dynamic ? m_rhs.cols() : m_lhs.cols();
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT {
+    // return the fixed size type if available to enable compile time optimizations
+    return internal::traits<internal::remove_all_t<LhsNested>>::RowsAtCompileTime == Dynamic ? m_rhs.rows()
+                                                                                             : m_lhs.rows();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT {
+    // return the fixed size type if available to enable compile time optimizations
+    return internal::traits<internal::remove_all_t<LhsNested>>::ColsAtCompileTime == Dynamic ? m_rhs.cols()
+                                                                                             : m_lhs.cols();
+  }
 
-    /** \returns the left hand side nested expression */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const _LhsNested& lhs() const { return m_lhs; }
-    /** \returns the right hand side nested expression */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const _RhsNested& rhs() const { return m_rhs; }
-    /** \returns the functor representing the binary operation */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const BinaryOp& functor() const { return m_functor; }
+  /** \returns the left hand side nested expression */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const LhsNested_& lhs() const { return m_lhs; }
+  /** \returns the right hand side nested expression */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const RhsNested_& rhs() const { return m_rhs; }
+  /** \returns the functor representing the binary operation */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const BinaryOp& functor() const { return m_functor; }
 
-  protected:
-    LhsNested m_lhs;
-    RhsNested m_rhs;
-    const BinaryOp m_functor;
+ protected:
+  LhsNested m_lhs;
+  RhsNested m_rhs;
+  const BinaryOp m_functor;
 };
 
 // Generic API dispatcher
-template<typename BinaryOp, typename Lhs, typename Rhs, typename StorageKind>
-class CwiseBinaryOpImpl
-  : public internal::generic_xpr_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >::type
-{
-public:
-  typedef typename internal::generic_xpr_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >::type Base;
+template <typename BinaryOp, typename Lhs, typename Rhs, typename StorageKind>
+class CwiseBinaryOpImpl : public internal::generic_xpr_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs>>::type {
+ public:
+  typedef typename internal::generic_xpr_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs>>::type Base;
 };
 
 /** replaces \c *this by \c *this - \a other.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-MatrixBase<Derived>::operator-=(const MatrixBase<OtherDerived> &other)
-{
-  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+ *
+ * \returns a reference to \c *this
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator-=(const MatrixBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
 /** replaces \c *this by \c *this + \a other.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-MatrixBase<Derived>::operator+=(const MatrixBase<OtherDerived>& other)
-{
-  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+ *
+ * \returns a reference to \c *this
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator+=(const MatrixBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_CWISE_BINARY_OP_H
+#endif  // EIGEN_CWISE_BINARY_OP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h
index 289ec51..39c33cf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h
@@ -10,18 +10,18 @@
 #ifndef EIGEN_CWISE_NULLARY_OP_H
 #define EIGEN_CWISE_NULLARY_OP_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename NullaryOp, typename PlainObjectType>
-struct traits<CwiseNullaryOp<NullaryOp, PlainObjectType> > : traits<PlainObjectType>
-{
-  enum {
-    Flags = traits<PlainObjectType>::Flags & RowMajorBit
-  };
+template <typename NullaryOp, typename PlainObjectType>
+struct traits<CwiseNullaryOp<NullaryOp, PlainObjectType> > : traits<PlainObjectType> {
+  enum { Flags = traits<PlainObjectType>::Flags & RowMajorBit };
 };
 
-} // namespace internal
+}  // namespace internal
 
 /** \class CwiseNullaryOp
   * \ingroup Core_Module
@@ -40,11 +40,14 @@
   *
   * The functor NullaryOp must expose one of the following method:
     <table class="manual">
-    <tr            ><td>\c operator()() </td><td>if the procedural generation does not depend on the coefficient entries (e.g., random numbers)</td></tr>
-    <tr class="alt"><td>\c operator()(Index i)</td><td>if the procedural generation makes sense for vectors only and that it depends on the coefficient index \c i (e.g., linspace) </td></tr>
-    <tr            ><td>\c operator()(Index i,Index j)</td><td>if the procedural generation depends on the matrix coordinates \c i, \c j (e.g., to generate a checkerboard with 0 and 1)</td></tr>
+    <tr            ><td>\c operator()() </td><td>if the procedural generation does not depend on the coefficient entries
+  (e.g., random numbers)</td></tr> <tr class="alt"><td>\c operator()(Index i)</td><td>if the procedural generation makes
+  sense for vectors only and that it depends on the coefficient index \c i (e.g., linspace) </td></tr> <tr ><td>\c
+  operator()(Index i,Index j)</td><td>if the procedural generation depends on the matrix coordinates \c i, \c j (e.g.,
+  to generate a checkerboard with 0 and 1)</td></tr>
     </table>
-  * It is also possible to expose the last two operators if the generation makes sense for matrices but can be optimized for vectors.
+  * It is also possible to expose the last two operators if the generation makes sense for matrices but can be optimized
+  for vectors.
   *
   * See DenseBase::NullaryExpr(Index,const CustomNullaryOp&) for an example binding
   * C++11 random number generators.
@@ -56,779 +59,748 @@
   *
   * \sa class CwiseUnaryOp, class CwiseBinaryOp, DenseBase::NullaryExpr
   */
-template<typename NullaryOp, typename PlainObjectType>
-class CwiseNullaryOp : public internal::dense_xpr_base< CwiseNullaryOp<NullaryOp, PlainObjectType> >::type, internal::no_assignment_operator
-{
-  public:
+template <typename NullaryOp, typename PlainObjectType>
+class CwiseNullaryOp : public internal::dense_xpr_base<CwiseNullaryOp<NullaryOp, PlainObjectType> >::type,
+                       internal::no_assignment_operator {
+ public:
+  typedef typename internal::dense_xpr_base<CwiseNullaryOp>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(CwiseNullaryOp)
 
-    typedef typename internal::dense_xpr_base<CwiseNullaryOp>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(CwiseNullaryOp)
+  EIGEN_DEVICE_FUNC CwiseNullaryOp(Index rows, Index cols, const NullaryOp& func = NullaryOp())
+      : m_rows(rows), m_cols(cols), m_functor(func) {
+    eigen_assert(rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) && cols >= 0 &&
+                 (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
+  }
 
-    EIGEN_DEVICE_FUNC
-    CwiseNullaryOp(Index rows, Index cols, const NullaryOp& func = NullaryOp())
-      : m_rows(rows), m_cols(cols), m_functor(func)
-    {
-      eigen_assert(rows >= 0
-            && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
-            &&  cols >= 0
-            && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const { return m_cols.value(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rows() const { return m_rows.value(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index cols() const { return m_cols.value(); }
+  /** \returns the functor representing the nullary operation */
+  EIGEN_DEVICE_FUNC const NullaryOp& functor() const { return m_functor; }
 
-    /** \returns the functor representing the nullary operation */
-    EIGEN_DEVICE_FUNC
-    const NullaryOp& functor() const { return m_functor; }
-
-  protected:
-    const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
-    const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
-    const NullaryOp m_functor;
+ protected:
+  const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
+  const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
+  const NullaryOp m_functor;
 };
 
-
 /** \returns an expression of a matrix defined by a custom functor \a func
-  *
-  * The parameters \a rows and \a cols are the number of rows and of columns of
-  * the returned matrix. Must be compatible with this MatrixBase type.
-  *
-  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
-  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
-  * instead.
-  *
-  * The template parameter \a CustomNullaryOp is the type of the functor.
-  *
-  * \sa class CwiseNullaryOp
-  */
-template<typename Derived>
-template<typename CustomNullaryOp>
+ *
+ * The parameters \a rows and \a cols are the number of rows and of columns of
+ * the returned matrix. Must be compatible with this MatrixBase type.
+ *
+ * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+ * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+ * instead.
+ *
+ * The template parameter \a CustomNullaryOp is the type of the functor.
+ *
+ * \sa class CwiseNullaryOp
+ */
+template <typename Derived>
+template <typename CustomNullaryOp>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-const CwiseNullaryOp<CustomNullaryOp,typename DenseBase<Derived>::PlainObject>
+    const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
 #else
-const CwiseNullaryOp<CustomNullaryOp,PlainObject>
+    const CwiseNullaryOp<CustomNullaryOp, PlainObject>
 #endif
-DenseBase<Derived>::NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func)
-{
+    DenseBase<Derived>::NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func) {
   return CwiseNullaryOp<CustomNullaryOp, PlainObject>(rows, cols, func);
 }
 
 /** \returns an expression of a matrix defined by a custom functor \a func
-  *
-  * The parameter \a size is the size of the returned vector.
-  * Must be compatible with this MatrixBase type.
-  *
-  * \only_for_vectors
-  *
-  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
-  * it is redundant to pass \a size as argument, so Zero() should be used
-  * instead.
-  *
-  * The template parameter \a CustomNullaryOp is the type of the functor.
-  *
-  * Here is an example with C++11 random generators: \include random_cpp11.cpp
-  * Output: \verbinclude random_cpp11.out
-  *
-  * \sa class CwiseNullaryOp
-  */
-template<typename Derived>
-template<typename CustomNullaryOp>
+ *
+ * The parameter \a size is the size of the returned vector.
+ * Must be compatible with this MatrixBase type.
+ *
+ * \only_for_vectors
+ *
+ * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+ * it is redundant to pass \a size as argument, so Zero() should be used
+ * instead.
+ *
+ * The template parameter \a CustomNullaryOp is the type of the functor.
+ *
+ * Here is an example with C++11 random generators: \include random_cpp11.cpp
+ * Output: \verbinclude random_cpp11.out
+ *
+ * \sa class CwiseNullaryOp
+ */
+template <typename Derived>
+template <typename CustomNullaryOp>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+    const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
 #else
-const CwiseNullaryOp<CustomNullaryOp, PlainObject>
+    const CwiseNullaryOp<CustomNullaryOp, PlainObject>
 #endif
-DenseBase<Derived>::NullaryExpr(Index size, const CustomNullaryOp& func)
-{
+    DenseBase<Derived>::NullaryExpr(Index size, const CustomNullaryOp& func) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  if(RowsAtCompileTime == 1) return CwiseNullaryOp<CustomNullaryOp, PlainObject>(1, size, func);
-  else return CwiseNullaryOp<CustomNullaryOp, PlainObject>(size, 1, func);
+  if (RowsAtCompileTime == 1)
+    return CwiseNullaryOp<CustomNullaryOp, PlainObject>(1, size, func);
+  else
+    return CwiseNullaryOp<CustomNullaryOp, PlainObject>(size, 1, func);
 }
 
 /** \returns an expression of a matrix defined by a custom functor \a func
-  *
-  * This variant is only for fixed-size DenseBase types. For dynamic-size types, you
-  * need to use the variants taking size arguments.
-  *
-  * The template parameter \a CustomNullaryOp is the type of the functor.
-  *
-  * \sa class CwiseNullaryOp
-  */
-template<typename Derived>
-template<typename CustomNullaryOp>
+ *
+ * This variant is only for fixed-size DenseBase types. For dynamic-size types, you
+ * need to use the variants taking size arguments.
+ *
+ * The template parameter \a CustomNullaryOp is the type of the functor.
+ *
+ * \sa class CwiseNullaryOp
+ */
+template <typename Derived>
+template <typename CustomNullaryOp>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+    const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
 #else
-const CwiseNullaryOp<CustomNullaryOp, PlainObject>
+    const CwiseNullaryOp<CustomNullaryOp, PlainObject>
 #endif
-DenseBase<Derived>::NullaryExpr(const CustomNullaryOp& func)
-{
+    DenseBase<Derived>::NullaryExpr(const CustomNullaryOp& func) {
   return CwiseNullaryOp<CustomNullaryOp, PlainObject>(RowsAtCompileTime, ColsAtCompileTime, func);
 }
 
 /** \returns an expression of a constant matrix of value \a value
-  *
-  * The parameters \a rows and \a cols are the number of rows and of columns of
-  * the returned matrix. Must be compatible with this DenseBase type.
-  *
-  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
-  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
-  * instead.
-  *
-  * The template parameter \a CustomNullaryOp is the type of the functor.
-  *
-  * \sa class CwiseNullaryOp
-  */
-template<typename Derived>
+ *
+ * The parameters \a rows and \a cols are the number of rows and of columns of
+ * the returned matrix. Must be compatible with this DenseBase type.
+ *
+ * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+ * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+ * instead.
+ *
+ * The template parameter \a CustomNullaryOp is the type of the functor.
+ *
+ * \sa class CwiseNullaryOp
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Constant(Index rows, Index cols, const Scalar& value)
-{
+DenseBase<Derived>::Constant(Index rows, Index cols, const Scalar& value) {
   return DenseBase<Derived>::NullaryExpr(rows, cols, internal::scalar_constant_op<Scalar>(value));
 }
 
 /** \returns an expression of a constant matrix of value \a value
-  *
-  * The parameter \a size is the size of the returned vector.
-  * Must be compatible with this DenseBase type.
-  *
-  * \only_for_vectors
-  *
-  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
-  * it is redundant to pass \a size as argument, so Zero() should be used
-  * instead.
-  *
-  * The template parameter \a CustomNullaryOp is the type of the functor.
-  *
-  * \sa class CwiseNullaryOp
-  */
-template<typename Derived>
+ *
+ * The parameter \a size is the size of the returned vector.
+ * Must be compatible with this DenseBase type.
+ *
+ * \only_for_vectors
+ *
+ * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+ * it is redundant to pass \a size as argument, so Zero() should be used
+ * instead.
+ *
+ * The template parameter \a CustomNullaryOp is the type of the functor.
+ *
+ * \sa class CwiseNullaryOp
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Constant(Index size, const Scalar& value)
-{
+DenseBase<Derived>::Constant(Index size, const Scalar& value) {
   return DenseBase<Derived>::NullaryExpr(size, internal::scalar_constant_op<Scalar>(value));
 }
 
 /** \returns an expression of a constant matrix of value \a value
-  *
-  * This variant is only for fixed-size DenseBase types. For dynamic-size types, you
-  * need to use the variants taking size arguments.
-  *
-  * The template parameter \a CustomNullaryOp is the type of the functor.
-  *
-  * \sa class CwiseNullaryOp
-  */
-template<typename Derived>
+ *
+ * This variant is only for fixed-size DenseBase types. For dynamic-size types, you
+ * need to use the variants taking size arguments.
+ *
+ * The template parameter \a CustomNullaryOp is the type of the functor.
+ *
+ * \sa class CwiseNullaryOp
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Constant(const Scalar& value)
-{
+DenseBase<Derived>::Constant(const Scalar& value) {
   EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
-  return DenseBase<Derived>::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_constant_op<Scalar>(value));
+  return DenseBase<Derived>::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime,
+                                         internal::scalar_constant_op<Scalar>(value));
 }
 
 /** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&)
-  *
-  * \only_for_vectors
-  *
-  * Example: \include DenseBase_LinSpaced_seq_deprecated.cpp
-  * Output: \verbinclude DenseBase_LinSpaced_seq_deprecated.out
-  *
-  * \sa LinSpaced(Index,const Scalar&, const Scalar&), setLinSpaced(Index,const Scalar&,const Scalar&)
-  */
-template<typename Derived>
-EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
-DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high)
-{
+ *
+ * \only_for_vectors
+ *
+ * Example: \include DenseBase_LinSpaced_seq_deprecated.cpp
+ * Output: \verbinclude DenseBase_LinSpaced_seq_deprecated.out
+ *
+ * \sa LinSpaced(Index,const Scalar&, const Scalar&), setLinSpaced(Index,const Scalar&,const Scalar&)
+ */
+template <typename Derived>
+EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<
+    Derived>::RandomAccessLinSpacedReturnType
+DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low,high,size));
+  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low, high, size));
 }
 
 /** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&)
-  *
-  * \sa LinSpaced(const Scalar&, const Scalar&)
-  */
-template<typename Derived>
-EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
-DenseBase<Derived>::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high)
-{
+ *
+ * \sa LinSpaced(const Scalar&, const Scalar&)
+ */
+template <typename Derived>
+EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<
+    Derived>::RandomAccessLinSpacedReturnType
+DenseBase<Derived>::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
-  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar>(low,high,Derived::SizeAtCompileTime));
+  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime,
+                                         internal::linspaced_op<Scalar>(low, high, Derived::SizeAtCompileTime));
 }
 
 /**
-  * \brief Sets a linearly spaced vector.
-  *
-  * The function generates 'size' equally spaced values in the closed interval [low,high].
-  * When size is set to 1, a vector of length 1 containing 'high' is returned.
-  *
-  * \only_for_vectors
-  *
-  * Example: \include DenseBase_LinSpaced.cpp
-  * Output: \verbinclude DenseBase_LinSpaced.out
-  *
-  * For integer scalar types, an even spacing is possible if and only if the length of the range,
-  * i.e., \c high-low is a scalar multiple of \c size-1, or if \c size is a scalar multiple of the
-  * number of values \c high-low+1 (meaning each value can be repeated the same number of time).
-  * If one of these two considions is not satisfied, then \c high is lowered to the largest value
-  * satisfying one of this constraint.
-  * Here are some examples:
-  *
-  * Example: \include DenseBase_LinSpacedInt.cpp
-  * Output: \verbinclude DenseBase_LinSpacedInt.out
-  *
-  * \sa setLinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp
-  */
-template<typename Derived>
+ * \brief Sets a linearly spaced vector.
+ *
+ * The function generates 'size' equally spaced values in the closed interval [low,high].
+ * When size is set to 1, a vector of length 1 containing 'high' is returned.
+ *
+ * \only_for_vectors
+ *
+ * Example: \include DenseBase_LinSpaced.cpp
+ * Output: \verbinclude DenseBase_LinSpaced.out
+ *
+ * For integer scalar types, an even spacing is possible if and only if the length of the range,
+ * i.e., \c high-low is a scalar multiple of \c size-1, or if \c size is a scalar multiple of the
+ * number of values \c high-low+1 (meaning each value can be repeated the same number of time).
+ * If one of these two considions is not satisfied, then \c high is lowered to the largest value
+ * satisfying one of this constraint.
+ * Here are some examples:
+ *
+ * Example: \include DenseBase_LinSpacedInt.cpp
+ * Output: \verbinclude DenseBase_LinSpacedInt.out
+ *
+ * \sa setLinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
-DenseBase<Derived>::LinSpaced(Index size, const Scalar& low, const Scalar& high)
-{
+DenseBase<Derived>::LinSpaced(Index size, const Scalar& low, const Scalar& high) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low,high,size));
+  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low, high, size));
 }
 
 /**
-  * \copydoc DenseBase::LinSpaced(Index, const Scalar&, const Scalar&)
-  * Special version for fixed size types which does not require the size parameter.
-  */
-template<typename Derived>
+ * \copydoc DenseBase::LinSpaced(Index, const Scalar&, const Scalar&)
+ * Special version for fixed size types which does not require the size parameter.
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
-DenseBase<Derived>::LinSpaced(const Scalar& low, const Scalar& high)
-{
+DenseBase<Derived>::LinSpaced(const Scalar& low, const Scalar& high) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
-  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar>(low,high,Derived::SizeAtCompileTime));
+  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime,
+                                         internal::linspaced_op<Scalar>(low, high, Derived::SizeAtCompileTime));
+}
+
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessEqualSpacedReturnType
+DenseBase<Derived>::EqualSpaced(Index size, const Scalar& low, const Scalar& step) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return DenseBase<Derived>::NullaryExpr(size, internal::equalspaced_op<Scalar>(low, step));
+}
+
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessEqualSpacedReturnType
+DenseBase<Derived>::EqualSpaced(const Scalar& low, const Scalar& step) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::equalspaced_op<Scalar>(low, step));
 }
 
 /** \returns true if all coefficients in this matrix are approximately equal to \a val, to within precision \a prec */
-template<typename Derived>
-EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isApproxToConstant
-(const Scalar& val, const RealScalar& prec) const
-{
-  typename internal::nested_eval<Derived,1>::type self(derived());
-  for(Index j = 0; j < cols(); ++j)
-    for(Index i = 0; i < rows(); ++i)
-      if(!internal::isApprox(self.coeff(i, j), val, prec))
-        return false;
+template <typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isApproxToConstant(const Scalar& val, const RealScalar& prec) const {
+  typename internal::nested_eval<Derived, 1>::type self(derived());
+  for (Index j = 0; j < cols(); ++j)
+    for (Index i = 0; i < rows(); ++i)
+      if (!internal::isApprox(self.coeff(i, j), val, prec)) return false;
   return true;
 }
 
 /** This is just an alias for isApproxToConstant().
-  *
-  * \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */
-template<typename Derived>
-EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isConstant
-(const Scalar& val, const RealScalar& prec) const
-{
+ *
+ * \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */
+template <typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isConstant(const Scalar& val, const RealScalar& prec) const {
   return isApproxToConstant(val, prec);
 }
 
 /** Alias for setConstant(): sets all coefficients in this expression to \a val.
-  *
-  * \sa setConstant(), Constant(), class CwiseNullaryOp
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void DenseBase<Derived>::fill(const Scalar& val)
-{
+ *
+ * \sa setConstant(), Constant(), class CwiseNullaryOp
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void DenseBase<Derived>::fill(const Scalar& val) {
   setConstant(val);
 }
 
 /** Sets all coefficients in this expression to value \a val.
-  *
-  * \sa fill(), setConstant(Index,const Scalar&), setConstant(Index,Index,const Scalar&), setZero(), setOnes(), Constant(), class CwiseNullaryOp, setZero(), setOnes()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setConstant(const Scalar& val)
-{
+ *
+ * \sa fill(), setConstant(Index,const Scalar&), setConstant(Index,Index,const Scalar&), setZero(), setOnes(),
+ * Constant(), class CwiseNullaryOp, setZero(), setOnes()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setConstant(const Scalar& val) {
   return derived() = Constant(rows(), cols(), val);
 }
 
 /** Resizes to the given \a size, and sets all coefficients in this expression to the given value \a val.
-  *
-  * \only_for_vectors
-  *
-  * Example: \include Matrix_setConstant_int.cpp
-  * Output: \verbinclude Matrix_setConstant_int.out
-  *
-  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setConstant(Index size, const Scalar& val)
-{
+ *
+ * \only_for_vectors
+ *
+ * Example: \include Matrix_setConstant_int.cpp
+ * Output: \verbinclude Matrix_setConstant_int.out
+ *
+ * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,Index,const Scalar&), class CwiseNullaryOp,
+ * MatrixBase::Constant(const Scalar&)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setConstant(Index size, const Scalar& val) {
   resize(size);
   return setConstant(val);
 }
 
 /** Resizes to the given size, and sets all coefficients in this expression to the given value \a val.
-  *
-  * \param rows the new number of rows
-  * \param cols the new number of columns
-  * \param val the value to which all coefficients are set
-  *
-  * Example: \include Matrix_setConstant_int_int.cpp
-  * Output: \verbinclude Matrix_setConstant_int_int.out
-  *
-  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setConstant(Index rows, Index cols, const Scalar& val)
-{
+ *
+ * \param rows the new number of rows
+ * \param cols the new number of columns
+ * \param val the value to which all coefficients are set
+ *
+ * Example: \include Matrix_setConstant_int_int.cpp
+ * Output: \verbinclude Matrix_setConstant_int_int.out
+ *
+ * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp,
+ * MatrixBase::Constant(const Scalar&)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setConstant(Index rows, Index cols,
+                                                                                     const Scalar& val) {
   resize(rows, cols);
   return setConstant(val);
 }
 
 /** Resizes to the given size, changing only the number of columns, and sets all
-  * coefficients in this expression to the given value \a val. For the parameter
-  * of type NoChange_t, just pass the special value \c NoChange.
-  *
-  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setConstant(NoChange_t, Index cols, const Scalar& val)
-{
+ * coefficients in this expression to the given value \a val. For the parameter
+ * of type NoChange_t, just pass the special value \c NoChange.
+ *
+ * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp,
+ * MatrixBase::Constant(const Scalar&)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setConstant(NoChange_t, Index cols,
+                                                                                     const Scalar& val) {
   return setConstant(rows(), cols, val);
 }
 
 /** Resizes to the given size, changing only the number of rows, and sets all
-  * coefficients in this expression to the given value \a val. For the parameter
-  * of type NoChange_t, just pass the special value \c NoChange.
-  *
-  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setConstant(Index rows, NoChange_t, const Scalar& val)
-{
+ * coefficients in this expression to the given value \a val. For the parameter
+ * of type NoChange_t, just pass the special value \c NoChange.
+ *
+ * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp,
+ * MatrixBase::Constant(const Scalar&)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setConstant(Index rows, NoChange_t,
+                                                                                     const Scalar& val) {
   return setConstant(rows, cols(), val);
 }
 
-
 /**
-  * \brief Sets a linearly spaced vector.
-  *
-  * The function generates 'size' equally spaced values in the closed interval [low,high].
-  * When size is set to 1, a vector of length 1 containing 'high' is returned.
-  *
-  * \only_for_vectors
-  *
-  * Example: \include DenseBase_setLinSpaced.cpp
-  * Output: \verbinclude DenseBase_setLinSpaced.out
-  *
-  * For integer scalar types, do not miss the explanations on the definition
-  * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink.
-  *
-  * \sa LinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setLinSpaced(Index newSize, const Scalar& low, const Scalar& high)
-{
+ * \brief Sets a linearly spaced vector.
+ *
+ * The function generates 'size' equally spaced values in the closed interval [low,high].
+ * When size is set to 1, a vector of length 1 containing 'high' is returned.
+ *
+ * \only_for_vectors
+ *
+ * Example: \include DenseBase_setLinSpaced.cpp
+ * Output: \verbinclude DenseBase_setLinSpaced.out
+ *
+ * For integer scalar types, do not miss the explanations on the definition
+ * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink.
+ *
+ * \sa LinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setLinSpaced(Index newSize, const Scalar& low,
+                                                                                const Scalar& high) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op<Scalar>(low,high,newSize));
+  return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op<Scalar>(low, high, newSize));
 }
 
 /**
-  * \brief Sets a linearly spaced vector.
-  *
-  * The function fills \c *this with equally spaced values in the closed interval [low,high].
-  * When size is set to 1, a vector of length 1 containing 'high' is returned.
-  *
-  * \only_for_vectors
-  *
-  * For integer scalar types, do not miss the explanations on the definition
-  * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink.
-  *
-  * \sa LinSpaced(Index,const Scalar&,const Scalar&), setLinSpaced(Index, const Scalar&, const Scalar&), CwiseNullaryOp
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setLinSpaced(const Scalar& low, const Scalar& high)
-{
+ * \brief Sets a linearly spaced vector.
+ *
+ * The function fills \c *this with equally spaced values in the closed interval [low,high].
+ * When size is set to 1, a vector of length 1 containing 'high' is returned.
+ *
+ * \only_for_vectors
+ *
+ * For integer scalar types, do not miss the explanations on the definition
+ * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink.
+ *
+ * \sa LinSpaced(Index,const Scalar&,const Scalar&), setLinSpaced(Index, const Scalar&, const Scalar&), CwiseNullaryOp
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setLinSpaced(const Scalar& low, const Scalar& high) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   return setLinSpaced(size(), low, high);
 }
 
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setEqualSpaced(Index newSize, const Scalar& low,
+                                                                                  const Scalar& step) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return derived() = Derived::NullaryExpr(newSize, internal::equalspaced_op<Scalar>(low, step));
+}
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setEqualSpaced(const Scalar& low,
+                                                                                  const Scalar& step) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return setEqualSpaced(size(), low, step);
+}
+
 // zero:
 
 /** \returns an expression of a zero matrix.
-  *
-  * The parameters \a rows and \a cols are the number of rows and of columns of
-  * the returned matrix. Must be compatible with this MatrixBase type.
-  *
-  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
-  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
-  * instead.
-  *
-  * Example: \include MatrixBase_zero_int_int.cpp
-  * Output: \verbinclude MatrixBase_zero_int_int.out
-  *
-  * \sa Zero(), Zero(Index)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Zero(Index rows, Index cols)
-{
+ *
+ * The parameters \a rows and \a cols are the number of rows and of columns of
+ * the returned matrix. Must be compatible with this MatrixBase type.
+ *
+ * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+ * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+ * instead.
+ *
+ * Example: \include MatrixBase_zero_int_int.cpp
+ * Output: \verbinclude MatrixBase_zero_int_int.out
+ *
+ * \sa Zero(), Zero(Index)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType DenseBase<Derived>::Zero(
+    Index rows, Index cols) {
   return Constant(rows, cols, Scalar(0));
 }
 
 /** \returns an expression of a zero vector.
-  *
-  * The parameter \a size is the size of the returned vector.
-  * Must be compatible with this MatrixBase type.
-  *
-  * \only_for_vectors
-  *
-  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
-  * it is redundant to pass \a size as argument, so Zero() should be used
-  * instead.
-  *
-  * Example: \include MatrixBase_zero_int.cpp
-  * Output: \verbinclude MatrixBase_zero_int.out
-  *
-  * \sa Zero(), Zero(Index,Index)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Zero(Index size)
-{
+ *
+ * The parameter \a size is the size of the returned vector.
+ * Must be compatible with this MatrixBase type.
+ *
+ * \only_for_vectors
+ *
+ * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+ * it is redundant to pass \a size as argument, so Zero() should be used
+ * instead.
+ *
+ * Example: \include MatrixBase_zero_int.cpp
+ * Output: \verbinclude MatrixBase_zero_int.out
+ *
+ * \sa Zero(), Zero(Index,Index)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType DenseBase<Derived>::Zero(
+    Index size) {
   return Constant(size, Scalar(0));
 }
 
 /** \returns an expression of a fixed-size zero matrix or vector.
-  *
-  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
-  * need to use the variants taking size arguments.
-  *
-  * Example: \include MatrixBase_zero.cpp
-  * Output: \verbinclude MatrixBase_zero.out
-  *
-  * \sa Zero(Index), Zero(Index,Index)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Zero()
-{
+ *
+ * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+ * need to use the variants taking size arguments.
+ *
+ * Example: \include MatrixBase_zero.cpp
+ * Output: \verbinclude MatrixBase_zero.out
+ *
+ * \sa Zero(Index), Zero(Index,Index)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType DenseBase<Derived>::Zero() {
   return Constant(Scalar(0));
 }
 
 /** \returns true if *this is approximately equal to the zero matrix,
-  *          within the precision given by \a prec.
-  *
-  * Example: \include MatrixBase_isZero.cpp
-  * Output: \verbinclude MatrixBase_isZero.out
-  *
-  * \sa class CwiseNullaryOp, Zero()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isZero(const RealScalar& prec) const
-{
-  typename internal::nested_eval<Derived,1>::type self(derived());
-  for(Index j = 0; j < cols(); ++j)
-    for(Index i = 0; i < rows(); ++i)
-      if(!internal::isMuchSmallerThan(self.coeff(i, j), static_cast<Scalar>(1), prec))
-        return false;
+ *          within the precision given by \a prec.
+ *
+ * Example: \include MatrixBase_isZero.cpp
+ * Output: \verbinclude MatrixBase_isZero.out
+ *
+ * \sa class CwiseNullaryOp, Zero()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isZero(const RealScalar& prec) const {
+  typename internal::nested_eval<Derived, 1>::type self(derived());
+  for (Index j = 0; j < cols(); ++j)
+    for (Index i = 0; i < rows(); ++i)
+      if (!internal::isMuchSmallerThan(self.coeff(i, j), static_cast<Scalar>(1), prec)) return false;
   return true;
 }
 
 /** Sets all coefficients in this expression to zero.
-  *
-  * Example: \include MatrixBase_setZero.cpp
-  * Output: \verbinclude MatrixBase_setZero.out
-  *
-  * \sa class CwiseNullaryOp, Zero()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setZero()
-{
+ *
+ * Example: \include MatrixBase_setZero.cpp
+ * Output: \verbinclude MatrixBase_setZero.out
+ *
+ * \sa class CwiseNullaryOp, Zero()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setZero() {
   return setConstant(Scalar(0));
 }
 
 /** Resizes to the given \a size, and sets all coefficients in this expression to zero.
-  *
-  * \only_for_vectors
-  *
-  * Example: \include Matrix_setZero_int.cpp
-  * Output: \verbinclude Matrix_setZero_int.out
-  *
-  * \sa DenseBase::setZero(), setZero(Index,Index), class CwiseNullaryOp, DenseBase::Zero()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setZero(Index newSize)
-{
+ *
+ * \only_for_vectors
+ *
+ * Example: \include Matrix_setZero_int.cpp
+ * Output: \verbinclude Matrix_setZero_int.out
+ *
+ * \sa DenseBase::setZero(), setZero(Index,Index), class CwiseNullaryOp, DenseBase::Zero()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setZero(Index newSize) {
   resize(newSize);
   return setConstant(Scalar(0));
 }
 
 /** Resizes to the given size, and sets all coefficients in this expression to zero.
-  *
-  * \param rows the new number of rows
-  * \param cols the new number of columns
-  *
-  * Example: \include Matrix_setZero_int_int.cpp
-  * Output: \verbinclude Matrix_setZero_int_int.out
-  *
-  * \sa DenseBase::setZero(), setZero(Index), class CwiseNullaryOp, DenseBase::Zero()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setZero(Index rows, Index cols)
-{
+ *
+ * \param rows the new number of rows
+ * \param cols the new number of columns
+ *
+ * Example: \include Matrix_setZero_int_int.cpp
+ * Output: \verbinclude Matrix_setZero_int_int.out
+ *
+ * \sa DenseBase::setZero(), setZero(Index), class CwiseNullaryOp, DenseBase::Zero()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setZero(Index rows, Index cols) {
   resize(rows, cols);
   return setConstant(Scalar(0));
 }
 
 /** Resizes to the given size, changing only the number of columns, and sets all
-  * coefficients in this expression to zero. For the parameter of type NoChange_t,
-  * just pass the special value \c NoChange.
-  *
-  * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(Index, NoChange_t), class CwiseNullaryOp, DenseBase::Zero()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setZero(NoChange_t, Index cols)
-{
+ * coefficients in this expression to zero. For the parameter of type NoChange_t,
+ * just pass the special value \c NoChange.
+ *
+ * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(Index, NoChange_t), class CwiseNullaryOp,
+ * DenseBase::Zero()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setZero(NoChange_t, Index cols) {
   return setZero(rows(), cols);
 }
 
 /** Resizes to the given size, changing only the number of rows, and sets all
-  * coefficients in this expression to zero. For the parameter of type NoChange_t,
-  * just pass the special value \c NoChange.
-  *
-  * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(NoChange_t, Index), class CwiseNullaryOp, DenseBase::Zero()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setZero(Index rows, NoChange_t)
-{
+ * coefficients in this expression to zero. For the parameter of type NoChange_t,
+ * just pass the special value \c NoChange.
+ *
+ * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(NoChange_t, Index), class CwiseNullaryOp,
+ * DenseBase::Zero()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setZero(Index rows, NoChange_t) {
   return setZero(rows, cols());
 }
 
 // ones:
 
 /** \returns an expression of a matrix where all coefficients equal one.
-  *
-  * The parameters \a rows and \a cols are the number of rows and of columns of
-  * the returned matrix. Must be compatible with this MatrixBase type.
-  *
-  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
-  * it is redundant to pass \a rows and \a cols as arguments, so Ones() should be used
-  * instead.
-  *
-  * Example: \include MatrixBase_ones_int_int.cpp
-  * Output: \verbinclude MatrixBase_ones_int_int.out
-  *
-  * \sa Ones(), Ones(Index), isOnes(), class Ones
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Ones(Index rows, Index cols)
-{
+ *
+ * The parameters \a rows and \a cols are the number of rows and of columns of
+ * the returned matrix. Must be compatible with this MatrixBase type.
+ *
+ * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+ * it is redundant to pass \a rows and \a cols as arguments, so Ones() should be used
+ * instead.
+ *
+ * Example: \include MatrixBase_ones_int_int.cpp
+ * Output: \verbinclude MatrixBase_ones_int_int.out
+ *
+ * \sa Ones(), Ones(Index), isOnes(), class Ones
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType DenseBase<Derived>::Ones(
+    Index rows, Index cols) {
   return Constant(rows, cols, Scalar(1));
 }
 
 /** \returns an expression of a vector where all coefficients equal one.
-  *
-  * The parameter \a newSize is the size of the returned vector.
-  * Must be compatible with this MatrixBase type.
-  *
-  * \only_for_vectors
-  *
-  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
-  * it is redundant to pass \a size as argument, so Ones() should be used
-  * instead.
-  *
-  * Example: \include MatrixBase_ones_int.cpp
-  * Output: \verbinclude MatrixBase_ones_int.out
-  *
-  * \sa Ones(), Ones(Index,Index), isOnes(), class Ones
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Ones(Index newSize)
-{
+ *
+ * The parameter \a newSize is the size of the returned vector.
+ * Must be compatible with this MatrixBase type.
+ *
+ * \only_for_vectors
+ *
+ * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+ * it is redundant to pass \a size as argument, so Ones() should be used
+ * instead.
+ *
+ * Example: \include MatrixBase_ones_int.cpp
+ * Output: \verbinclude MatrixBase_ones_int.out
+ *
+ * \sa Ones(), Ones(Index,Index), isOnes(), class Ones
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType DenseBase<Derived>::Ones(
+    Index newSize) {
   return Constant(newSize, Scalar(1));
 }
 
 /** \returns an expression of a fixed-size matrix or vector where all coefficients equal one.
-  *
-  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
-  * need to use the variants taking size arguments.
-  *
-  * Example: \include MatrixBase_ones.cpp
-  * Output: \verbinclude MatrixBase_ones.out
-  *
-  * \sa Ones(Index), Ones(Index,Index), isOnes(), class Ones
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
-DenseBase<Derived>::Ones()
-{
+ *
+ * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+ * need to use the variants taking size arguments.
+ *
+ * Example: \include MatrixBase_ones.cpp
+ * Output: \verbinclude MatrixBase_ones.out
+ *
+ * \sa Ones(Index), Ones(Index,Index), isOnes(), class Ones
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType DenseBase<Derived>::Ones() {
   return Constant(Scalar(1));
 }
 
 /** \returns true if *this is approximately equal to the matrix where all coefficients
-  *          are equal to 1, within the precision given by \a prec.
-  *
-  * Example: \include MatrixBase_isOnes.cpp
-  * Output: \verbinclude MatrixBase_isOnes.out
-  *
-  * \sa class CwiseNullaryOp, Ones()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isOnes
-(const RealScalar& prec) const
-{
+ *          are equal to 1, within the precision given by \a prec.
+ *
+ * Example: \include MatrixBase_isOnes.cpp
+ * Output: \verbinclude MatrixBase_isOnes.out
+ *
+ * \sa class CwiseNullaryOp, Ones()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isOnes(const RealScalar& prec) const {
   return isApproxToConstant(Scalar(1), prec);
 }
 
 /** Sets all coefficients in this expression to one.
-  *
-  * Example: \include MatrixBase_setOnes.cpp
-  * Output: \verbinclude MatrixBase_setOnes.out
-  *
-  * \sa class CwiseNullaryOp, Ones()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setOnes()
-{
+ *
+ * Example: \include MatrixBase_setOnes.cpp
+ * Output: \verbinclude MatrixBase_setOnes.out
+ *
+ * \sa class CwiseNullaryOp, Ones()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setOnes() {
   return setConstant(Scalar(1));
 }
 
 /** Resizes to the given \a newSize, and sets all coefficients in this expression to one.
-  *
-  * \only_for_vectors
-  *
-  * Example: \include Matrix_setOnes_int.cpp
-  * Output: \verbinclude Matrix_setOnes_int.out
-  *
-  * \sa MatrixBase::setOnes(), setOnes(Index,Index), class CwiseNullaryOp, MatrixBase::Ones()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setOnes(Index newSize)
-{
+ *
+ * \only_for_vectors
+ *
+ * Example: \include Matrix_setOnes_int.cpp
+ * Output: \verbinclude Matrix_setOnes_int.out
+ *
+ * \sa MatrixBase::setOnes(), setOnes(Index,Index), class CwiseNullaryOp, MatrixBase::Ones()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setOnes(Index newSize) {
   resize(newSize);
   return setConstant(Scalar(1));
 }
 
 /** Resizes to the given size, and sets all coefficients in this expression to one.
-  *
-  * \param rows the new number of rows
-  * \param cols the new number of columns
-  *
-  * Example: \include Matrix_setOnes_int_int.cpp
-  * Output: \verbinclude Matrix_setOnes_int_int.out
-  *
-  * \sa MatrixBase::setOnes(), setOnes(Index), class CwiseNullaryOp, MatrixBase::Ones()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setOnes(Index rows, Index cols)
-{
+ *
+ * \param rows the new number of rows
+ * \param cols the new number of columns
+ *
+ * Example: \include Matrix_setOnes_int_int.cpp
+ * Output: \verbinclude Matrix_setOnes_int_int.out
+ *
+ * \sa MatrixBase::setOnes(), setOnes(Index), class CwiseNullaryOp, MatrixBase::Ones()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setOnes(Index rows, Index cols) {
   resize(rows, cols);
   return setConstant(Scalar(1));
 }
 
 /** Resizes to the given size, changing only the number of rows, and sets all
-  * coefficients in this expression to one. For the parameter of type NoChange_t,
-  * just pass the special value \c NoChange.
-  *
- * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(NoChange_t, Index), class CwiseNullaryOp, MatrixBase::Ones()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setOnes(Index rows, NoChange_t)
-{
+ * coefficients in this expression to one. For the parameter of type NoChange_t,
+ * just pass the special value \c NoChange.
+ *
+ * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(NoChange_t, Index), class CwiseNullaryOp,
+ * MatrixBase::Ones()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setOnes(Index rows, NoChange_t) {
   return setOnes(rows, cols());
 }
 
 /** Resizes to the given size, changing only the number of columns, and sets all
-  * coefficients in this expression to one. For the parameter of type NoChange_t,
-  * just pass the special value \c NoChange.
-  *
- * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(Index, NoChange_t) class CwiseNullaryOp, MatrixBase::Ones()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setOnes(NoChange_t, Index cols)
-{
+ * coefficients in this expression to one. For the parameter of type NoChange_t,
+ * just pass the special value \c NoChange.
+ *
+ * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(Index, NoChange_t) class CwiseNullaryOp,
+ * MatrixBase::Ones()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setOnes(NoChange_t, Index cols) {
   return setOnes(rows(), cols);
 }
 
 // Identity:
 
 /** \returns an expression of the identity matrix (not necessarily square).
-  *
-  * The parameters \a rows and \a cols are the number of rows and of columns of
-  * the returned matrix. Must be compatible with this MatrixBase type.
-  *
-  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
-  * it is redundant to pass \a rows and \a cols as arguments, so Identity() should be used
-  * instead.
-  *
-  * Example: \include MatrixBase_identity_int_int.cpp
-  * Output: \verbinclude MatrixBase_identity_int_int.out
-  *
-  * \sa Identity(), setIdentity(), isIdentity()
-  */
-template<typename Derived>
+ *
+ * The parameters \a rows and \a cols are the number of rows and of columns of
+ * the returned matrix. Must be compatible with this MatrixBase type.
+ *
+ * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+ * it is redundant to pass \a rows and \a cols as arguments, so Identity() should be used
+ * instead.
+ *
+ * Example: \include MatrixBase_identity_int_int.cpp
+ * Output: \verbinclude MatrixBase_identity_int_int.out
+ *
+ * \sa Identity(), setIdentity(), isIdentity()
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
-MatrixBase<Derived>::Identity(Index rows, Index cols)
-{
+MatrixBase<Derived>::Identity(Index rows, Index cols) {
   return DenseBase<Derived>::NullaryExpr(rows, cols, internal::scalar_identity_op<Scalar>());
 }
 
 /** \returns an expression of the identity matrix (not necessarily square).
-  *
-  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
-  * need to use the variant taking size arguments.
-  *
-  * Example: \include MatrixBase_identity.cpp
-  * Output: \verbinclude MatrixBase_identity.out
-  *
-  * \sa Identity(Index,Index), setIdentity(), isIdentity()
-  */
-template<typename Derived>
+ *
+ * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+ * need to use the variant taking size arguments.
+ *
+ * Example: \include MatrixBase_identity.cpp
+ * Output: \verbinclude MatrixBase_identity.out
+ *
+ * \sa Identity(Index,Index), setIdentity(), isIdentity()
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
-MatrixBase<Derived>::Identity()
-{
+MatrixBase<Derived>::Identity() {
   EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
   return MatrixBase<Derived>::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_identity_op<Scalar>());
 }
 
 /** \returns true if *this is approximately equal to the identity matrix
-  *          (not necessarily square),
-  *          within the precision given by \a prec.
-  *
-  * Example: \include MatrixBase_isIdentity.cpp
-  * Output: \verbinclude MatrixBase_isIdentity.out
-  *
-  * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), setIdentity()
-  */
-template<typename Derived>
-bool MatrixBase<Derived>::isIdentity
-(const RealScalar& prec) const
-{
-  typename internal::nested_eval<Derived,1>::type self(derived());
-  for(Index j = 0; j < cols(); ++j)
-  {
-    for(Index i = 0; i < rows(); ++i)
-    {
-      if(i == j)
-      {
-        if(!internal::isApprox(self.coeff(i, j), static_cast<Scalar>(1), prec))
-          return false;
-      }
-      else
-      {
-        if(!internal::isMuchSmallerThan(self.coeff(i, j), static_cast<RealScalar>(1), prec))
-          return false;
+ *          (not necessarily square),
+ *          within the precision given by \a prec.
+ *
+ * Example: \include MatrixBase_isIdentity.cpp
+ * Output: \verbinclude MatrixBase_isIdentity.out
+ *
+ * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), setIdentity()
+ */
+template <typename Derived>
+bool MatrixBase<Derived>::isIdentity(const RealScalar& prec) const {
+  typename internal::nested_eval<Derived, 1>::type self(derived());
+  for (Index j = 0; j < cols(); ++j) {
+    for (Index i = 0; i < rows(); ++i) {
+      if (i == j) {
+        if (!internal::isApprox(self.coeff(i, j), static_cast<Scalar>(1), prec)) return false;
+      } else {
+        if (!internal::isMuchSmallerThan(self.coeff(i, j), static_cast<RealScalar>(1), prec)) return false;
       }
     }
   }
@@ -837,165 +809,163 @@
 
 namespace internal {
 
-template<typename Derived, bool Big = (Derived::SizeAtCompileTime>=16)>
-struct setIdentity_impl
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Derived& run(Derived& m)
-  {
+template <typename Derived, bool Big = (Derived::SizeAtCompileTime >= 16)>
+struct setIdentity_impl {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Derived& run(Derived& m) {
     return m = Derived::Identity(m.rows(), m.cols());
   }
 };
 
-template<typename Derived>
-struct setIdentity_impl<Derived, true>
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Derived& run(Derived& m)
-  {
+template <typename Derived>
+struct setIdentity_impl<Derived, true> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Derived& run(Derived& m) {
     m.setZero();
     const Index size = numext::mini(m.rows(), m.cols());
-    for(Index i = 0; i < size; ++i) m.coeffRef(i,i) = typename Derived::Scalar(1);
+    for (Index i = 0; i < size; ++i) m.coeffRef(i, i) = typename Derived::Scalar(1);
     return m;
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** Writes the identity expression (not necessarily square) into *this.
-  *
-  * Example: \include MatrixBase_setIdentity.cpp
-  * Output: \verbinclude MatrixBase_setIdentity.out
-  *
-  * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), isIdentity()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity()
-{
+ *
+ * Example: \include MatrixBase_setIdentity.cpp
+ * Output: \verbinclude MatrixBase_setIdentity.out
+ *
+ * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), isIdentity()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity() {
   return internal::setIdentity_impl<Derived>::run(derived());
 }
 
 /** \brief Resizes to the given size, and writes the identity expression (not necessarily square) into *this.
-  *
-  * \param rows the new number of rows
-  * \param cols the new number of columns
-  *
-  * Example: \include Matrix_setIdentity_int_int.cpp
-  * Output: \verbinclude Matrix_setIdentity_int_int.out
-  *
-  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity(Index rows, Index cols)
-{
+ *
+ * \param rows the new number of rows
+ * \param cols the new number of columns
+ *
+ * Example: \include Matrix_setIdentity_int_int.cpp
+ * Output: \verbinclude Matrix_setIdentity_int_int.out
+ *
+ * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity(Index rows, Index cols) {
   derived().resize(rows, cols);
   return setIdentity();
 }
 
 /** \returns an expression of the i-th unit (basis) vector.
-  *
-  * \only_for_vectors
-  *
-  * \sa MatrixBase::Unit(Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(Index newSize, Index i)
-{
+ *
+ * \only_for_vectors
+ *
+ * \sa MatrixBase::Unit(Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(
+    Index newSize, Index i) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return BasisReturnType(SquareMatrixType::Identity(newSize,newSize), i);
+  return BasisReturnType(SquareMatrixType::Identity(newSize, newSize), i);
 }
 
 /** \returns an expression of the i-th unit (basis) vector.
-  *
-  * \only_for_vectors
-  *
-  * This variant is for fixed-size vector only.
-  *
-  * \sa MatrixBase::Unit(Index,Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(Index i)
-{
+ *
+ * \only_for_vectors
+ *
+ * This variant is for fixed-size vector only.
+ *
+ * \sa MatrixBase::Unit(Index,Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(
+    Index i) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return BasisReturnType(SquareMatrixType::Identity(),i);
+  return BasisReturnType(SquareMatrixType::Identity(), i);
 }
 
 /** \returns an expression of the X axis unit vector (1{,0}^*)
-  *
-  * \only_for_vectors
-  *
-  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitX()
-{ return Derived::Unit(0); }
+ *
+ * \only_for_vectors
+ *
+ * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(),
+ * MatrixBase::UnitW()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitX() {
+  return Derived::Unit(0);
+}
 
 /** \returns an expression of the Y axis unit vector (0,1{,0}^*)
-  *
-  * \only_for_vectors
-  *
-  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitY()
-{ return Derived::Unit(1); }
+ *
+ * \only_for_vectors
+ *
+ * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(),
+ * MatrixBase::UnitW()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitY() {
+  return Derived::Unit(1);
+}
 
 /** \returns an expression of the Z axis unit vector (0,0,1{,0}^*)
-  *
-  * \only_for_vectors
-  *
-  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitZ()
-{ return Derived::Unit(2); }
+ *
+ * \only_for_vectors
+ *
+ * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(),
+ * MatrixBase::UnitW()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitZ() {
+  return Derived::Unit(2);
+}
 
 /** \returns an expression of the W axis unit vector (0,0,0,1)
-  *
-  * \only_for_vectors
-  *
-  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitW()
-{ return Derived::Unit(3); }
+ *
+ * \only_for_vectors
+ *
+ * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(),
+ * MatrixBase::UnitW()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitW() {
+  return Derived::Unit(3);
+}
 
 /** \brief Set the coefficients of \c *this to the i-th unit (basis) vector
-  *
-  * \param i index of the unique coefficient to be set to 1
-  *
-  * \only_for_vectors
-  *
-  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index i)
-{
+ *
+ * \param i index of the unique coefficient to be set to 1
+ *
+ * \only_for_vectors
+ *
+ * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index i) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-  eigen_assert(i<size());
+  eigen_assert(i < size());
   derived().setZero();
   derived().coeffRef(i) = Scalar(1);
   return derived();
 }
 
 /** \brief Resizes to the given \a newSize, and writes the i-th unit (basis) vector into *this.
-  *
-  * \param newSize the new size of the vector
-  * \param i index of the unique coefficient to be set to 1
-  *
-  * \only_for_vectors
-  *
-  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index newSize, Index i)
-{
+ *
+ * \param newSize the new size of the vector
+ * \param i index of the unique coefficient to be set to 1
+ *
+ * \only_for_vectors
+ *
+ * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index newSize, Index i) {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-  eigen_assert(i<newSize);
+  eigen_assert(i < newSize);
   derived().resize(newSize);
   return setUnit(i);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_CWISE_NULLARY_OP_H
+#endif  // EIGEN_CWISE_NULLARY_OP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h
index 9f3576f..9bb0d40 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h
@@ -12,14 +12,17 @@
 #ifndef EIGEN_CWISE_TERNARY_OP_H
 #define EIGEN_CWISE_TERNARY_OP_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>
-struct traits<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> > {
+struct traits<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3>> {
   // we must not inherit from traits<Arg1> since it has
   // the potential to cause problems with MSVC
-  typedef typename remove_all<Arg1>::type Ancestor;
+  typedef remove_all_t<Arg1> Ancestor;
   typedef typename traits<Ancestor>::XprKind XprKind;
   enum {
     RowsAtCompileTime = traits<Ancestor>::RowsAtCompileTime,
@@ -31,9 +34,8 @@
   // even though we require Arg1, Arg2, and Arg3 to have the same scalar type
   // (see CwiseTernaryOp constructor),
   // we still want to handle the case when the result type is different.
-  typedef typename result_of<TernaryOp(
-      const typename Arg1::Scalar&, const typename Arg2::Scalar&,
-      const typename Arg3::Scalar&)>::type Scalar;
+  typedef typename result_of<TernaryOp(const typename Arg1::Scalar&, const typename Arg2::Scalar&,
+                                       const typename Arg3::Scalar&)>::type Scalar;
 
   typedef typename internal::traits<Arg1>::StorageKind StorageKind;
   typedef typename internal::traits<Arg1>::StorageIndex StorageIndex;
@@ -41,138 +43,114 @@
   typedef typename Arg1::Nested Arg1Nested;
   typedef typename Arg2::Nested Arg2Nested;
   typedef typename Arg3::Nested Arg3Nested;
-  typedef typename remove_reference<Arg1Nested>::type _Arg1Nested;
-  typedef typename remove_reference<Arg2Nested>::type _Arg2Nested;
-  typedef typename remove_reference<Arg3Nested>::type _Arg3Nested;
-  enum { Flags = _Arg1Nested::Flags & RowMajorBit };
+  typedef std::remove_reference_t<Arg1Nested> Arg1Nested_;
+  typedef std::remove_reference_t<Arg2Nested> Arg2Nested_;
+  typedef std::remove_reference_t<Arg3Nested> Arg3Nested_;
+  enum { Flags = Arg1Nested_::Flags & RowMajorBit };
 };
 }  // end namespace internal
 
-template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3,
-          typename StorageKind>
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3, typename StorageKind>
 class CwiseTernaryOpImpl;
 
 /** \class CwiseTernaryOp
-  * \ingroup Core_Module
-  *
-  * \brief Generic expression where a coefficient-wise ternary operator is
+ * \ingroup Core_Module
+ *
+ * \brief Generic expression where a coefficient-wise ternary operator is
  * applied to two expressions
-  *
-  * \tparam TernaryOp template functor implementing the operator
-  * \tparam Arg1Type the type of the first argument
-  * \tparam Arg2Type the type of the second argument
-  * \tparam Arg3Type the type of the third argument
-  *
-  * This class represents an expression where a coefficient-wise ternary
+ *
+ * \tparam TernaryOp template functor implementing the operator
+ * \tparam Arg1Type the type of the first argument
+ * \tparam Arg2Type the type of the second argument
+ * \tparam Arg3Type the type of the third argument
+ *
+ * This class represents an expression where a coefficient-wise ternary
  * operator is applied to three expressions.
-  * It is the return type of ternary operators, by which we mean only those
+ * It is the return type of ternary operators, by which we mean only those
  * ternary operators where
-  * all three arguments are Eigen expressions.
-  * For example, the return type of betainc(matrix1, matrix2, matrix3) is a
+ * all three arguments are Eigen expressions.
+ * For example, the return type of betainc(matrix1, matrix2, matrix3) is a
  * CwiseTernaryOp.
-  *
-  * Most of the time, this is the only way that it is used, so you typically
+ *
+ * Most of the time, this is the only way that it is used, so you typically
  * don't have to name
-  * CwiseTernaryOp types explicitly.
-  *
-  * \sa MatrixBase::ternaryExpr(const MatrixBase<Argument2> &, const
+ * CwiseTernaryOp types explicitly.
+ *
+ * \sa MatrixBase::ternaryExpr(const MatrixBase<Argument2> &, const
  * MatrixBase<Argument3> &, const CustomTernaryOp &) const, class CwiseBinaryOp,
  * class CwiseUnaryOp, class CwiseNullaryOp
-  */
-template <typename TernaryOp, typename Arg1Type, typename Arg2Type,
-          typename Arg3Type>
-class CwiseTernaryOp : public CwiseTernaryOpImpl<
-                           TernaryOp, Arg1Type, Arg2Type, Arg3Type,
-                           typename internal::traits<Arg1Type>::StorageKind>,
-                       internal::no_assignment_operator
-{
+ */
+template <typename TernaryOp, typename Arg1Type, typename Arg2Type, typename Arg3Type>
+class CwiseTernaryOp : public CwiseTernaryOpImpl<TernaryOp, Arg1Type, Arg2Type, Arg3Type,
+                                                 typename internal::traits<Arg1Type>::StorageKind>,
+                       internal::no_assignment_operator {
  public:
-  typedef typename internal::remove_all<Arg1Type>::type Arg1;
-  typedef typename internal::remove_all<Arg2Type>::type Arg2;
-  typedef typename internal::remove_all<Arg3Type>::type Arg3;
+  typedef internal::remove_all_t<Arg1Type> Arg1;
+  typedef internal::remove_all_t<Arg2Type> Arg2;
+  typedef internal::remove_all_t<Arg3Type> Arg3;
 
-  typedef typename CwiseTernaryOpImpl<
-      TernaryOp, Arg1Type, Arg2Type, Arg3Type,
-      typename internal::traits<Arg1Type>::StorageKind>::Base Base;
+  // require the sizes to match
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg2)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg3)
+
+  // The index types should match
+  EIGEN_STATIC_ASSERT((internal::is_same<typename internal::traits<Arg1Type>::StorageKind,
+                                         typename internal::traits<Arg2Type>::StorageKind>::value),
+                      STORAGE_KIND_MUST_MATCH)
+  EIGEN_STATIC_ASSERT((internal::is_same<typename internal::traits<Arg1Type>::StorageKind,
+                                         typename internal::traits<Arg3Type>::StorageKind>::value),
+                      STORAGE_KIND_MUST_MATCH)
+
+  typedef typename CwiseTernaryOpImpl<TernaryOp, Arg1Type, Arg2Type, Arg3Type,
+                                      typename internal::traits<Arg1Type>::StorageKind>::Base Base;
   EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseTernaryOp)
 
   typedef typename internal::ref_selector<Arg1Type>::type Arg1Nested;
   typedef typename internal::ref_selector<Arg2Type>::type Arg2Nested;
   typedef typename internal::ref_selector<Arg3Type>::type Arg3Nested;
-  typedef typename internal::remove_reference<Arg1Nested>::type _Arg1Nested;
-  typedef typename internal::remove_reference<Arg2Nested>::type _Arg2Nested;
-  typedef typename internal::remove_reference<Arg3Nested>::type _Arg3Nested;
+  typedef std::remove_reference_t<Arg1Nested> Arg1Nested_;
+  typedef std::remove_reference_t<Arg2Nested> Arg2Nested_;
+  typedef std::remove_reference_t<Arg3Nested> Arg3Nested_;
 
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE CwiseTernaryOp(const Arg1& a1, const Arg2& a2,
-                                     const Arg3& a3,
-                                     const TernaryOp& func = TernaryOp())
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CwiseTernaryOp(const Arg1& a1, const Arg2& a2, const Arg3& a3,
+                                                       const TernaryOp& func = TernaryOp())
       : m_arg1(a1), m_arg2(a2), m_arg3(a3), m_functor(func) {
-    // require the sizes to match
-    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg2)
-    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg3)
-
-    // The index types should match
-    EIGEN_STATIC_ASSERT((internal::is_same<
-                         typename internal::traits<Arg1Type>::StorageKind,
-                         typename internal::traits<Arg2Type>::StorageKind>::value),
-                        STORAGE_KIND_MUST_MATCH)
-    EIGEN_STATIC_ASSERT((internal::is_same<
-                         typename internal::traits<Arg1Type>::StorageKind,
-                         typename internal::traits<Arg3Type>::StorageKind>::value),
-                        STORAGE_KIND_MUST_MATCH)
-
-    eigen_assert(a1.rows() == a2.rows() && a1.cols() == a2.cols() &&
-                 a1.rows() == a3.rows() && a1.cols() == a3.cols());
+    eigen_assert(a1.rows() == a2.rows() && a1.cols() == a2.cols() && a1.rows() == a3.rows() && a1.cols() == a3.cols());
   }
 
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE Index rows() const {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const {
     // return the fixed size type if available to enable compile time
     // optimizations
-    if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
-                RowsAtCompileTime == Dynamic &&
-        internal::traits<typename internal::remove_all<Arg2Nested>::type>::
-                RowsAtCompileTime == Dynamic)
+    if (internal::traits<internal::remove_all_t<Arg1Nested>>::RowsAtCompileTime == Dynamic &&
+        internal::traits<internal::remove_all_t<Arg2Nested>>::RowsAtCompileTime == Dynamic)
       return m_arg3.rows();
-    else if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
-                     RowsAtCompileTime == Dynamic &&
-             internal::traits<typename internal::remove_all<Arg3Nested>::type>::
-                     RowsAtCompileTime == Dynamic)
+    else if (internal::traits<internal::remove_all_t<Arg1Nested>>::RowsAtCompileTime == Dynamic &&
+             internal::traits<internal::remove_all_t<Arg3Nested>>::RowsAtCompileTime == Dynamic)
       return m_arg2.rows();
     else
       return m_arg1.rows();
   }
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE Index cols() const {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const {
     // return the fixed size type if available to enable compile time
     // optimizations
-    if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
-                ColsAtCompileTime == Dynamic &&
-        internal::traits<typename internal::remove_all<Arg2Nested>::type>::
-                ColsAtCompileTime == Dynamic)
+    if (internal::traits<internal::remove_all_t<Arg1Nested>>::ColsAtCompileTime == Dynamic &&
+        internal::traits<internal::remove_all_t<Arg2Nested>>::ColsAtCompileTime == Dynamic)
       return m_arg3.cols();
-    else if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
-                     ColsAtCompileTime == Dynamic &&
-             internal::traits<typename internal::remove_all<Arg3Nested>::type>::
-                     ColsAtCompileTime == Dynamic)
+    else if (internal::traits<internal::remove_all_t<Arg1Nested>>::ColsAtCompileTime == Dynamic &&
+             internal::traits<internal::remove_all_t<Arg3Nested>>::ColsAtCompileTime == Dynamic)
       return m_arg2.cols();
     else
       return m_arg1.cols();
   }
 
   /** \returns the first argument nested expression */
-  EIGEN_DEVICE_FUNC
-  const _Arg1Nested& arg1() const { return m_arg1; }
+  EIGEN_DEVICE_FUNC const Arg1Nested_& arg1() const { return m_arg1; }
   /** \returns the first argument nested expression */
-  EIGEN_DEVICE_FUNC
-  const _Arg2Nested& arg2() const { return m_arg2; }
+  EIGEN_DEVICE_FUNC const Arg2Nested_& arg2() const { return m_arg2; }
   /** \returns the third argument nested expression */
-  EIGEN_DEVICE_FUNC
-  const _Arg3Nested& arg3() const { return m_arg3; }
+  EIGEN_DEVICE_FUNC const Arg3Nested_& arg3() const { return m_arg3; }
   /** \returns the functor representing the ternary operation */
-  EIGEN_DEVICE_FUNC
-  const TernaryOp& functor() const { return m_functor; }
+  EIGEN_DEVICE_FUNC const TernaryOp& functor() const { return m_functor; }
 
  protected:
   Arg1Nested m_arg1;
@@ -182,14 +160,10 @@
 };
 
 // Generic API dispatcher
-template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3,
-          typename StorageKind>
-class CwiseTernaryOpImpl
-    : public internal::generic_xpr_base<
-          CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >::type {
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3, typename StorageKind>
+class CwiseTernaryOpImpl : public internal::generic_xpr_base<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3>>::type {
  public:
-  typedef typename internal::generic_xpr_base<
-      CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >::type Base;
+  typedef typename internal::generic_xpr_base<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3>>::type Base;
 };
 
 }  // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h
index e68c4f7..42ed459 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h
@@ -11,93 +11,81 @@
 #ifndef EIGEN_CWISE_UNARY_OP_H
 #define EIGEN_CWISE_UNARY_OP_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename UnaryOp, typename XprType>
-struct traits<CwiseUnaryOp<UnaryOp, XprType> >
- : traits<XprType>
-{
-  typedef typename result_of<
-                     UnaryOp(const typename XprType::Scalar&)
-                   >::type Scalar;
+template <typename UnaryOp, typename XprType>
+struct traits<CwiseUnaryOp<UnaryOp, XprType> > : traits<XprType> {
+  typedef typename result_of<UnaryOp(const typename XprType::Scalar&)>::type Scalar;
   typedef typename XprType::Nested XprTypeNested;
-  typedef typename remove_reference<XprTypeNested>::type _XprTypeNested;
-  enum {
-    Flags = _XprTypeNested::Flags & RowMajorBit
-  };
+  typedef std::remove_reference_t<XprTypeNested> XprTypeNested_;
+  enum { Flags = XprTypeNested_::Flags & RowMajorBit };
 };
-}
+}  // namespace internal
 
-template<typename UnaryOp, typename XprType, typename StorageKind>
+template <typename UnaryOp, typename XprType, typename StorageKind>
 class CwiseUnaryOpImpl;
 
 /** \class CwiseUnaryOp
-  * \ingroup Core_Module
-  *
-  * \brief Generic expression where a coefficient-wise unary operator is applied to an expression
-  *
-  * \tparam UnaryOp template functor implementing the operator
-  * \tparam XprType the type of the expression to which we are applying the unary operator
-  *
-  * This class represents an expression where a unary operator is applied to an expression.
-  * It is the return type of all operations taking exactly 1 input expression, regardless of the
-  * presence of other inputs such as scalars. For example, the operator* in the expression 3*matrix
-  * is considered unary, because only the right-hand side is an expression, and its
-  * return type is a specialization of CwiseUnaryOp.
-  *
-  * Most of the time, this is the only way that it is used, so you typically don't have to name
-  * CwiseUnaryOp types explicitly.
-  *
-  * \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp
-  */
-template<typename UnaryOp, typename XprType>
-class CwiseUnaryOp : public CwiseUnaryOpImpl<UnaryOp, XprType, typename internal::traits<XprType>::StorageKind>, internal::no_assignment_operator
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Generic expression where a coefficient-wise unary operator is applied to an expression
+ *
+ * \tparam UnaryOp template functor implementing the operator
+ * \tparam XprType the type of the expression to which we are applying the unary operator
+ *
+ * This class represents an expression where a unary operator is applied to an expression.
+ * It is the return type of all operations taking exactly 1 input expression, regardless of the
+ * presence of other inputs such as scalars. For example, the operator* in the expression 3*matrix
+ * is considered unary, because only the right-hand side is an expression, and its
+ * return type is a specialization of CwiseUnaryOp.
+ *
+ * Most of the time, this is the only way that it is used, so you typically don't have to name
+ * CwiseUnaryOp types explicitly.
+ *
+ * \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp
+ */
+template <typename UnaryOp, typename XprType>
+class CwiseUnaryOp : public CwiseUnaryOpImpl<UnaryOp, XprType, typename internal::traits<XprType>::StorageKind>,
+                     internal::no_assignment_operator {
+ public:
+  typedef typename CwiseUnaryOpImpl<UnaryOp, XprType, typename internal::traits<XprType>::StorageKind>::Base Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryOp)
+  typedef typename internal::ref_selector<XprType>::type XprTypeNested;
+  typedef internal::remove_all_t<XprType> NestedExpression;
 
-    typedef typename CwiseUnaryOpImpl<UnaryOp, XprType,typename internal::traits<XprType>::StorageKind>::Base Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryOp)
-    typedef typename internal::ref_selector<XprType>::type XprTypeNested;
-    typedef typename internal::remove_all<XprType>::type NestedExpression;
-
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    explicit CwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp())
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit CwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp())
       : m_xpr(xpr), m_functor(func) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
 
-    /** \returns the functor representing the unary operation */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const UnaryOp& functor() const { return m_functor; }
+  /** \returns the functor representing the unary operation */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryOp& functor() const { return m_functor; }
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const typename internal::remove_all<XprTypeNested>::type&
-    nestedExpression() const { return m_xpr; }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t<XprTypeNested>& nestedExpression() const {
+    return m_xpr;
+  }
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    typename internal::remove_all<XprTypeNested>::type&
-    nestedExpression() { return m_xpr; }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE internal::remove_all_t<XprTypeNested>& nestedExpression() { return m_xpr; }
 
-  protected:
-    XprTypeNested m_xpr;
-    const UnaryOp m_functor;
+ protected:
+  XprTypeNested m_xpr;
+  const UnaryOp m_functor;
 };
 
 // Generic API dispatcher
-template<typename UnaryOp, typename XprType, typename StorageKind>
-class CwiseUnaryOpImpl
-  : public internal::generic_xpr_base<CwiseUnaryOp<UnaryOp, XprType> >::type
-{
-public:
+template <typename UnaryOp, typename XprType, typename StorageKind>
+class CwiseUnaryOpImpl : public internal::generic_xpr_base<CwiseUnaryOp<UnaryOp, XprType> >::type {
+ public:
   typedef typename internal::generic_xpr_base<CwiseUnaryOp<UnaryOp, XprType> >::type Base;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_CWISE_UNARY_OP_H
+#endif  // EIGEN_CWISE_UNARY_OP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h
index a06d762..725b337 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h
@@ -10,123 +10,128 @@
 #ifndef EIGEN_CWISE_UNARY_VIEW_H
 #define EIGEN_CWISE_UNARY_VIEW_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename ViewOp, typename MatrixType>
-struct traits<CwiseUnaryView<ViewOp, MatrixType> >
- : traits<MatrixType>
-{
-  typedef typename result_of<
-                     ViewOp(const typename traits<MatrixType>::Scalar&)
-                   >::type Scalar;
+template <typename ViewOp, typename MatrixType, typename StrideType>
+struct traits<CwiseUnaryView<ViewOp, MatrixType, StrideType> > : traits<MatrixType> {
+  typedef typename result_of<ViewOp(const typename traits<MatrixType>::Scalar&)>::type Scalar;
   typedef typename MatrixType::Nested MatrixTypeNested;
-  typedef typename remove_all<MatrixTypeNested>::type _MatrixTypeNested;
+  typedef remove_all_t<MatrixTypeNested> MatrixTypeNested_;
   enum {
     FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
-    Flags = traits<_MatrixTypeNested>::Flags & (RowMajorBit | FlagsLvalueBit | DirectAccessBit), // FIXME DirectAccessBit should not be handled by expressions
-    MatrixTypeInnerStride =  inner_stride_at_compile_time<MatrixType>::ret,
+    Flags =
+        traits<MatrixTypeNested_>::Flags &
+        (RowMajorBit | FlagsLvalueBit | DirectAccessBit),  // FIXME DirectAccessBit should not be handled by expressions
+    MatrixTypeInnerStride = inner_stride_at_compile_time<MatrixType>::ret,
     // need to cast the sizeof's from size_t to int explicitly, otherwise:
     // "error: no integral type can represent all of the enumerator values
-    InnerStrideAtCompileTime = MatrixTypeInnerStride == Dynamic
-                             ? int(Dynamic)
-                             : int(MatrixTypeInnerStride) * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar)),
-    OuterStrideAtCompileTime = outer_stride_at_compile_time<MatrixType>::ret == Dynamic
-                             ? int(Dynamic)
-                             : outer_stride_at_compile_time<MatrixType>::ret * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar))
+    InnerStrideAtCompileTime =
+        StrideType::InnerStrideAtCompileTime == 0
+            ? (MatrixTypeInnerStride == Dynamic
+                   ? int(Dynamic)
+                   : int(MatrixTypeInnerStride) * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar)))
+            : int(StrideType::InnerStrideAtCompileTime),
+
+    OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0
+                                   ? (outer_stride_at_compile_time<MatrixType>::ret == Dynamic
+                                          ? int(Dynamic)
+                                          : outer_stride_at_compile_time<MatrixType>::ret *
+                                                int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar)))
+                                   : int(StrideType::OuterStrideAtCompileTime)
   };
 };
-}
+}  // namespace internal
 
-template<typename ViewOp, typename MatrixType, typename StorageKind>
+template <typename ViewOp, typename MatrixType, typename StrideType, typename StorageKind>
 class CwiseUnaryViewImpl;
 
 /** \class CwiseUnaryView
-  * \ingroup Core_Module
-  *
-  * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector
-  *
-  * \tparam ViewOp template functor implementing the view
-  * \tparam MatrixType the type of the matrix we are applying the unary operator
-  *
-  * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector.
-  * It is the return type of real() and imag(), and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp
-  */
-template<typename ViewOp, typename MatrixType>
-class CwiseUnaryView : public CwiseUnaryViewImpl<ViewOp, MatrixType, typename internal::traits<MatrixType>::StorageKind>
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector
+ *
+ * \tparam ViewOp template functor implementing the view
+ * \tparam MatrixType the type of the matrix we are applying the unary operator
+ *
+ * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector.
+ * It is the return type of real() and imag(), and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp
+ */
+template <typename ViewOp, typename MatrixType, typename StrideType>
+class CwiseUnaryView
+    : public CwiseUnaryViewImpl<ViewOp, MatrixType, StrideType, typename internal::traits<MatrixType>::StorageKind> {
+ public:
+  typedef typename CwiseUnaryViewImpl<ViewOp, MatrixType, StrideType,
+                                      typename internal::traits<MatrixType>::StorageKind>::Base Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryView)
+  typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+  typedef internal::remove_all_t<MatrixType> NestedExpression;
 
-    typedef typename CwiseUnaryViewImpl<ViewOp, MatrixType,typename internal::traits<MatrixType>::StorageKind>::Base Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryView)
-    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
-    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
-
-    explicit EIGEN_DEVICE_FUNC inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp())
+  explicit EIGEN_DEVICE_FUNC inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp())
       : m_matrix(mat), m_functor(func) {}
 
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView)
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
-    /** \returns the functor representing unary operation */
-    EIGEN_DEVICE_FUNC const ViewOp& functor() const { return m_functor; }
+  /** \returns the functor representing unary operation */
+  EIGEN_DEVICE_FUNC const ViewOp& functor() const { return m_functor; }
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC const typename internal::remove_all<MatrixTypeNested>::type&
-    nestedExpression() const { return m_matrix; }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC const internal::remove_all_t<MatrixTypeNested>& nestedExpression() const { return m_matrix; }
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC typename internal::remove_reference<MatrixTypeNested>::type&
-    nestedExpression() { return m_matrix; }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC std::remove_reference_t<MatrixTypeNested>& nestedExpression() { return m_matrix; }
 
-  protected:
-    MatrixTypeNested m_matrix;
-    ViewOp m_functor;
+ protected:
+  MatrixTypeNested m_matrix;
+  ViewOp m_functor;
 };
 
 // Generic API dispatcher
-template<typename ViewOp, typename XprType, typename StorageKind>
-class CwiseUnaryViewImpl
-  : public internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type
-{
-public:
-  typedef typename internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type Base;
+template <typename ViewOp, typename XprType, typename StrideType, typename StorageKind>
+class CwiseUnaryViewImpl : public internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType, StrideType> >::type {
+ public:
+  typedef typename internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType, StrideType> >::type Base;
 };
 
-template<typename ViewOp, typename MatrixType>
-class CwiseUnaryViewImpl<ViewOp,MatrixType,Dense>
-  : public internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type
-{
-  public:
+template <typename ViewOp, typename MatrixType, typename StrideType>
+class CwiseUnaryViewImpl<ViewOp, MatrixType, StrideType, Dense>
+    : public internal::dense_xpr_base<CwiseUnaryView<ViewOp, MatrixType, StrideType> >::type {
+ public:
+  typedef CwiseUnaryView<ViewOp, MatrixType, StrideType> Derived;
+  typedef typename internal::dense_xpr_base<CwiseUnaryView<ViewOp, MatrixType, StrideType> >::type Base;
 
-    typedef CwiseUnaryView<ViewOp, MatrixType> Derived;
-    typedef typename internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl)
 
-    EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl)
+  EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); }
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); }
 
-    EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); }
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const {
+    return StrideType::InnerStrideAtCompileTime != 0
+               ? int(StrideType::InnerStrideAtCompileTime)
+               : derived().nestedExpression().innerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) /
+                     sizeof(Scalar);
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const
-    {
-      return derived().nestedExpression().innerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const {
+    return StrideType::OuterStrideAtCompileTime != 0
+               ? int(StrideType::OuterStrideAtCompileTime)
+               : derived().nestedExpression().outerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) /
+                     sizeof(Scalar);
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const
-    {
-      return derived().nestedExpression().outerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
-    }
-  protected:
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(CwiseUnaryViewImpl)
+ protected:
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(CwiseUnaryViewImpl)
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_CWISE_UNARY_VIEW_H
+#endif  // EIGEN_CWISE_UNARY_VIEW_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
index 9b16db6..5ab54ef 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
@@ -11,691 +11,635 @@
 #ifndef EIGEN_DENSEBASE_H
 #define EIGEN_DENSEBASE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-namespace internal {
-
 // The index type defined by EIGEN_DEFAULT_DENSE_INDEX_TYPE must be a signed type.
-// This dummy function simply aims at checking that at compile time.
-static inline void check_DenseIndex_is_signed() {
-  EIGEN_STATIC_ASSERT(NumTraits<DenseIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE)
-}
-
-} // end namespace internal
+EIGEN_STATIC_ASSERT(NumTraits<DenseIndex>::IsSigned, THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE)
 
 /** \class DenseBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for all dense matrices, vectors, and arrays
-  *
-  * This class is the base that is inherited by all dense objects (matrix, vector, arrays,
-  * and related expression types). The common Eigen API for dense objects is contained in this class.
-  *
-  * \tparam Derived is the derived type, e.g., a matrix type or an expression.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_DENSEBASE_PLUGIN.
-  *
-  * \sa \blank \ref TopicClassHierarchy
-  */
-template<typename Derived> class DenseBase
+ * \ingroup Core_Module
+ *
+ * \brief Base class for all dense matrices, vectors, and arrays
+ *
+ * This class is the base that is inherited by all dense objects (matrix, vector, arrays,
+ * and related expression types). The common Eigen API for dense objects is contained in this class.
+ *
+ * \tparam Derived is the derived type, e.g., a matrix type or an expression.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_DENSEBASE_PLUGIN.
+ *
+ * \sa \blank \ref TopicClassHierarchy
+ */
+template <typename Derived>
+class DenseBase
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-  : public DenseCoeffsBase<Derived, internal::accessors_level<Derived>::value>
+    : public DenseCoeffsBase<Derived, internal::accessors_level<Derived>::value>
 #else
-  : public DenseCoeffsBase<Derived,DirectWriteAccessors>
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+    : public DenseCoeffsBase<Derived, DirectWriteAccessors>
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 {
-  public:
+ public:
+  /** Inner iterator type to iterate over the coefficients of a row or column.
+   * \sa class InnerIterator
+   */
+  typedef Eigen::InnerIterator<Derived> InnerIterator;
 
-    /** Inner iterator type to iterate over the coefficients of a row or column.
-      * \sa class InnerIterator
-      */
-    typedef Eigen::InnerIterator<Derived> InnerIterator;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
 
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  /**
+   * \brief The type used to store indices
+   * \details This typedef is relevant for types that store multiple indices such as
+   *          PermutationMatrix or Transpositions, otherwise it defaults to Eigen::Index
+   * \sa \blank \ref TopicPreprocessorDirectives, Eigen::Index, SparseMatrixBase.
+   */
+  typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
 
-    /**
-      * \brief The type used to store indices
-      * \details This typedef is relevant for types that store multiple indices such as
-      *          PermutationMatrix or Transpositions, otherwise it defaults to Eigen::Index
-      * \sa \blank \ref TopicPreprocessorDirectives, Eigen::Index, SparseMatrixBase.
+  /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc. */
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+
+  /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
+   *
+   * It is an alias for the Scalar type */
+  typedef Scalar value_type;
+
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef DenseCoeffsBase<Derived, internal::accessors_level<Derived>::value> Base;
+
+  using Base::coeff;
+  using Base::coeffByOuterInner;
+  using Base::colIndexByOuterInner;
+  using Base::cols;
+  using Base::const_cast_derived;
+  using Base::derived;
+  using Base::rowIndexByOuterInner;
+  using Base::rows;
+  using Base::size;
+  using Base::operator();
+  using Base::operator[];
+  using Base::colStride;
+  using Base::innerStride;
+  using Base::outerStride;
+  using Base::rowStride;
+  using Base::stride;
+  using Base::w;
+  using Base::x;
+  using Base::y;
+  using Base::z;
+  typedef typename Base::CoeffReturnType CoeffReturnType;
+
+  enum {
+
+    RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+    /**< The number of rows at compile-time. This is just a copy of the value provided
+     * by the \a Derived type. If a value is not known at compile-time,
+     * it is set to the \a Dynamic constant.
+     * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+
+    ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+    /**< The number of columns at compile-time. This is just a copy of the value provided
+     * by the \a Derived type. If a value is not known at compile-time,
+     * it is set to the \a Dynamic constant.
+     * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+
+    SizeAtCompileTime = (internal::size_of_xpr_at_compile_time<Derived>::ret),
+    /**< This is equal to the number of coefficients, i.e. the number of
+     * rows times the number of columns, or to \a Dynamic if this is not
+     * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+
+    MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
+    /**< This value is equal to the maximum possible number of rows that this expression
+     * might have. If this expression might have an arbitrarily high number of rows,
+     * this value is set to \a Dynamic.
+     *
+     * This value is useful to know when evaluating an expression, in order to determine
+     * whether it is possible to avoid doing a dynamic memory allocation.
+     *
+     * \sa RowsAtCompileTime, MaxColsAtCompileTime, MaxSizeAtCompileTime
      */
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
 
-    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc. */
-    typedef typename internal::traits<Derived>::Scalar Scalar;
+    MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
+    /**< This value is equal to the maximum possible number of columns that this expression
+     * might have. If this expression might have an arbitrarily high number of columns,
+     * this value is set to \a Dynamic.
+     *
+     * This value is useful to know when evaluating an expression, in order to determine
+     * whether it is possible to avoid doing a dynamic memory allocation.
+     *
+     * \sa ColsAtCompileTime, MaxRowsAtCompileTime, MaxSizeAtCompileTime
+     */
 
-    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
-      *
-      * It is an alias for the Scalar type */
-    typedef Scalar value_type;
+    MaxSizeAtCompileTime = internal::size_at_compile_time(internal::traits<Derived>::MaxRowsAtCompileTime,
+                                                          internal::traits<Derived>::MaxColsAtCompileTime),
+    /**< This value is equal to the maximum possible number of coefficients that this expression
+     * might have. If this expression might have an arbitrarily high number of coefficients,
+     * this value is set to \a Dynamic.
+     *
+     * This value is useful to know when evaluating an expression, in order to determine
+     * whether it is possible to avoid doing a dynamic memory allocation.
+     *
+     * \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime
+     */
 
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef DenseCoeffsBase<Derived, internal::accessors_level<Derived>::value> Base;
+    IsVectorAtCompileTime =
+        internal::traits<Derived>::RowsAtCompileTime == 1 || internal::traits<Derived>::ColsAtCompileTime == 1,
+    /**< This is set to true if either the number of rows or the number of
+     * columns is known at compile-time to be equal to 1. Indeed, in that case,
+     * we are dealing with a column-vector (if there is only one column) or with
+     * a row-vector (if there is only one row). */
 
-    using Base::derived;
-    using Base::const_cast_derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::rowIndexByOuterInner;
-    using Base::colIndexByOuterInner;
-    using Base::coeff;
-    using Base::coeffByOuterInner;
-    using Base::operator();
-    using Base::operator[];
-    using Base::x;
-    using Base::y;
-    using Base::z;
-    using Base::w;
-    using Base::stride;
-    using Base::innerStride;
-    using Base::outerStride;
-    using Base::rowStride;
-    using Base::colStride;
-    typedef typename Base::CoeffReturnType CoeffReturnType;
+    NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0
+                    : bool(IsVectorAtCompileTime)  ? 1
+                                                   : 2,
+    /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors,
+     * and 2 for matrices.
+     */
 
-    enum {
+    Flags = internal::traits<Derived>::Flags,
+    /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
+     * constructed from this one. See the \ref flags "list of flags".
+     */
 
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-        /**< The number of rows at compile-time. This is just a copy of the value provided
-          * by the \a Derived type. If a value is not known at compile-time,
-          * it is set to the \a Dynamic constant.
-          * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+    IsRowMajor = int(Flags) & RowMajorBit, /**< True if this expression has row-major storage order. */
 
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-        /**< The number of columns at compile-time. This is just a copy of the value provided
-          * by the \a Derived type. If a value is not known at compile-time,
-          * it is set to the \a Dynamic constant.
-          * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+    InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
+                             : int(IsRowMajor)          ? int(ColsAtCompileTime)
+                                                        : int(RowsAtCompileTime),
 
+    InnerStrideAtCompileTime = internal::inner_stride_at_compile_time<Derived>::ret,
+    OuterStrideAtCompileTime = internal::outer_stride_at_compile_time<Derived>::ret
+  };
 
-      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
-                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
-        /**< This is equal to the number of coefficients, i.e. the number of
-          * rows times the number of columns, or to \a Dynamic if this is not
-          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+  typedef typename internal::find_best_packet<Scalar, SizeAtCompileTime>::type PacketScalar;
 
-      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
-        /**< This value is equal to the maximum possible number of rows that this expression
-          * might have. If this expression might have an arbitrarily high number of rows,
-          * this value is set to \a Dynamic.
-          *
-          * This value is useful to know when evaluating an expression, in order to determine
-          * whether it is possible to avoid doing a dynamic memory allocation.
-          *
-          * \sa RowsAtCompileTime, MaxColsAtCompileTime, MaxSizeAtCompileTime
-          */
+  enum { IsPlainObjectBase = 0 };
 
-      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
-        /**< This value is equal to the maximum possible number of columns that this expression
-          * might have. If this expression might have an arbitrarily high number of columns,
-          * this value is set to \a Dynamic.
-          *
-          * This value is useful to know when evaluating an expression, in order to determine
-          * whether it is possible to avoid doing a dynamic memory allocation.
-          *
-          * \sa ColsAtCompileTime, MaxRowsAtCompileTime, MaxSizeAtCompileTime
-          */
+  /** The plain matrix type corresponding to this expression.
+   * \sa PlainObject */
+  typedef Matrix<typename internal::traits<Derived>::Scalar, internal::traits<Derived>::RowsAtCompileTime,
+                 internal::traits<Derived>::ColsAtCompileTime,
+                 AutoAlign | (internal::traits<Derived>::Flags & RowMajorBit ? RowMajor : ColMajor),
+                 internal::traits<Derived>::MaxRowsAtCompileTime, internal::traits<Derived>::MaxColsAtCompileTime>
+      PlainMatrix;
 
-      MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
-                                                      internal::traits<Derived>::MaxColsAtCompileTime>::ret),
-        /**< This value is equal to the maximum possible number of coefficients that this expression
-          * might have. If this expression might have an arbitrarily high number of coefficients,
-          * this value is set to \a Dynamic.
-          *
-          * This value is useful to know when evaluating an expression, in order to determine
-          * whether it is possible to avoid doing a dynamic memory allocation.
-          *
-          * \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime
-          */
-
-      IsVectorAtCompileTime = internal::traits<Derived>::RowsAtCompileTime == 1
-                           || internal::traits<Derived>::ColsAtCompileTime == 1,
-        /**< This is set to true if either the number of rows or the number of
-          * columns is known at compile-time to be equal to 1. Indeed, in that case,
-          * we are dealing with a column-vector (if there is only one column) or with
-          * a row-vector (if there is only one row). */
-
-      NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2,
-        /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors,
-         * and 2 for matrices.
-         */
-
-      Flags = internal::traits<Derived>::Flags,
-        /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
-          * constructed from this one. See the \ref flags "list of flags".
-          */
-
-      IsRowMajor = int(Flags) & RowMajorBit, /**< True if this expression has row-major storage order. */
-
-      InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
-                             : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
-
-      InnerStrideAtCompileTime = internal::inner_stride_at_compile_time<Derived>::ret,
-      OuterStrideAtCompileTime = internal::outer_stride_at_compile_time<Derived>::ret
-    };
-
-    typedef typename internal::find_best_packet<Scalar,SizeAtCompileTime>::type PacketScalar;
-
-    enum { IsPlainObjectBase = 0 };
-
-    /** The plain matrix type corresponding to this expression.
-      * \sa PlainObject */
-    typedef Matrix<typename internal::traits<Derived>::Scalar,
-                internal::traits<Derived>::RowsAtCompileTime,
+  /** The plain array type corresponding to this expression.
+   * \sa PlainObject */
+  typedef Array<typename internal::traits<Derived>::Scalar, internal::traits<Derived>::RowsAtCompileTime,
                 internal::traits<Derived>::ColsAtCompileTime,
-                AutoAlign | (internal::traits<Derived>::Flags&RowMajorBit ? RowMajor : ColMajor),
-                internal::traits<Derived>::MaxRowsAtCompileTime,
-                internal::traits<Derived>::MaxColsAtCompileTime
-          > PlainMatrix;
+                AutoAlign | (internal::traits<Derived>::Flags & RowMajorBit ? RowMajor : ColMajor),
+                internal::traits<Derived>::MaxRowsAtCompileTime, internal::traits<Derived>::MaxColsAtCompileTime>
+      PlainArray;
 
-    /** The plain array type corresponding to this expression.
-      * \sa PlainObject */
-    typedef Array<typename internal::traits<Derived>::Scalar,
-                internal::traits<Derived>::RowsAtCompileTime,
-                internal::traits<Derived>::ColsAtCompileTime,
-                AutoAlign | (internal::traits<Derived>::Flags&RowMajorBit ? RowMajor : ColMajor),
-                internal::traits<Derived>::MaxRowsAtCompileTime,
-                internal::traits<Derived>::MaxColsAtCompileTime
-          > PlainArray;
+  /** \brief The plain matrix or array type corresponding to this expression.
+   *
+   * This is not necessarily exactly the return type of eval(). In the case of plain matrices,
+   * the return type of eval() is a const reference to a matrix, not a matrix! It is however guaranteed
+   * that the return type of eval() is either PlainObject or const PlainObject&.
+   */
+  typedef std::conditional_t<internal::is_same<typename internal::traits<Derived>::XprKind, MatrixXpr>::value,
+                             PlainMatrix, PlainArray>
+      PlainObject;
 
-    /** \brief The plain matrix or array type corresponding to this expression.
-      *
-      * This is not necessarily exactly the return type of eval(). In the case of plain matrices,
-      * the return type of eval() is a const reference to a matrix, not a matrix! It is however guaranteed
-      * that the return type of eval() is either PlainObject or const PlainObject&.
-      */
-    typedef typename internal::conditional<internal::is_same<typename internal::traits<Derived>::XprKind,MatrixXpr >::value,
-                                 PlainMatrix, PlainArray>::type PlainObject;
+  /** \returns the outer size.
+   *
+   * \note For a vector, this returns just 1. For a matrix (non-vector), this is the major dimension
+   * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of columns for a
+   * column-major matrix, and the number of rows for a row-major matrix. */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerSize() const {
+    return IsVectorAtCompileTime ? 1 : int(IsRowMajor) ? this->rows() : this->cols();
+  }
 
-    /** \returns the number of nonzero coefficients which is in practice the number
-      * of stored coefficients. */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index nonZeros() const { return size(); }
+  /** \returns the inner size.
+   *
+   * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension
+   * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a
+   * column-major matrix, and the number of columns for a row-major matrix. */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index innerSize() const {
+    return IsVectorAtCompileTime ? this->size() : int(IsRowMajor) ? this->cols() : this->rows();
+  }
 
-    /** \returns the outer size.
-      *
-      * \note For a vector, this returns just 1. For a matrix (non-vector), this is the major dimension
-      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of columns for a
-      * column-major matrix, and the number of rows for a row-major matrix. */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index outerSize() const
-    {
-      return IsVectorAtCompileTime ? 1
-           : int(IsRowMajor) ? this->rows() : this->cols();
-    }
-
-    /** \returns the inner size.
-      *
-      * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension
-      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a
-      * column-major matrix, and the number of columns for a row-major matrix. */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index innerSize() const
-    {
-      return IsVectorAtCompileTime ? this->size()
-           : int(IsRowMajor) ? this->cols() : this->rows();
-    }
-
-    /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
-      * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does
-      * nothing else.
-      */
-    EIGEN_DEVICE_FUNC
-    void resize(Index newSize)
-    {
-      EIGEN_ONLY_USED_FOR_DEBUG(newSize);
-      eigen_assert(newSize == this->size()
-                && "DenseBase::resize() does not actually allow to resize.");
-    }
-    /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
-      * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does
-      * nothing else.
-      */
-    EIGEN_DEVICE_FUNC
-    void resize(Index rows, Index cols)
-    {
-      EIGEN_ONLY_USED_FOR_DEBUG(rows);
-      EIGEN_ONLY_USED_FOR_DEBUG(cols);
-      eigen_assert(rows == this->rows() && cols == this->cols()
-                && "DenseBase::resize() does not actually allow to resize.");
-    }
+  /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
+   * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and
+   * does nothing else.
+   */
+  EIGEN_DEVICE_FUNC void resize(Index newSize) {
+    EIGEN_ONLY_USED_FOR_DEBUG(newSize);
+    eigen_assert(newSize == this->size() && "DenseBase::resize() does not actually allow to resize.");
+  }
+  /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
+   * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and
+   * does nothing else.
+   */
+  EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) {
+    EIGEN_ONLY_USED_FOR_DEBUG(rows);
+    EIGEN_ONLY_USED_FOR_DEBUG(cols);
+    eigen_assert(rows == this->rows() && cols == this->cols() &&
+                 "DenseBase::resize() does not actually allow to resize.");
+  }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal Represents a matrix with all coefficients equal to one another*/
-    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
-    /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */
-    EIGEN_DEPRECATED typedef CwiseNullaryOp<internal::linspaced_op<Scalar>,PlainObject> SequentialLinSpacedReturnType;
-    /** \internal Represents a vector with linearly spaced coefficients that allows random access. */
-    typedef CwiseNullaryOp<internal::linspaced_op<Scalar>,PlainObject> RandomAccessLinSpacedReturnType;
-    /** \internal the return type of MatrixBase::eigenvalues() */
-    typedef Matrix<typename NumTraits<typename internal::traits<Derived>::Scalar>::Real, internal::traits<Derived>::ColsAtCompileTime, 1> EigenvaluesReturnType;
+  /** \internal Represents a matrix with all coefficients equal to one another*/
+  typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> ConstantReturnType;
+  /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */
+  EIGEN_DEPRECATED typedef CwiseNullaryOp<internal::linspaced_op<Scalar>, PlainObject> SequentialLinSpacedReturnType;
+  /** \internal Represents a vector with linearly spaced coefficients that allows random access. */
+  typedef CwiseNullaryOp<internal::linspaced_op<Scalar>, PlainObject> RandomAccessLinSpacedReturnType;
+  /** \internal Represents a vector with equally spaced coefficients that allows random access. */
+  typedef CwiseNullaryOp<internal::equalspaced_op<Scalar>, PlainObject> RandomAccessEqualSpacedReturnType;
+  /** \internal the return type of MatrixBase::eigenvalues() */
+  typedef Matrix<typename NumTraits<typename internal::traits<Derived>::Scalar>::Real,
+                 internal::traits<Derived>::ColsAtCompileTime, 1>
+      EigenvaluesReturnType;
 
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
-    /** Copies \a other into *this. \returns a reference to *this. */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const DenseBase<OtherDerived>& other);
+  /** Copies \a other into *this. \returns a reference to *this. */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase<OtherDerived>& other);
 
-    /** Special case of the template operator=, in order to prevent the compiler
-      * from generating a default operator= (issue hit with g++ 4.1)
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const DenseBase& other);
+  /** Special case of the template operator=, in order to prevent the compiler
+   * from generating a default operator= (issue hit with g++ 4.1)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const EigenBase<OtherDerived> &other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC Derived& operator=(const EigenBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator+=(const EigenBase<OtherDerived> &other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC Derived& operator+=(const EigenBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator-=(const EigenBase<OtherDerived> &other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC Derived& operator-=(const EigenBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const ReturnByValue<OtherDerived>& func);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC Derived& operator=(const ReturnByValue<OtherDerived>& func);
 
-    /** \internal
-      * Copies \a other into *this without evaluating other. \returns a reference to *this. */
-    template<typename OtherDerived>
-    /** \deprecated */
-    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
-    Derived& lazyAssign(const DenseBase<OtherDerived>& other);
+  /** \internal
+   * Copies \a other into *this without evaluating other. \returns a reference to *this. */
+  template <typename OtherDerived>
+  /** \deprecated */
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC Derived& lazyAssign(const DenseBase<OtherDerived>& other);
 
-    EIGEN_DEVICE_FUNC
-    CommaInitializer<Derived> operator<< (const Scalar& s);
+  EIGEN_DEVICE_FUNC CommaInitializer<Derived> operator<<(const Scalar& s);
 
-    template<unsigned int Added,unsigned int Removed>
-    /** \deprecated it now returns \c *this */
-    EIGEN_DEPRECATED
-    const Derived& flagged() const
-    { return derived(); }
+  template <unsigned int Added, unsigned int Removed>
+  /** \deprecated it now returns \c *this */
+  EIGEN_DEPRECATED const Derived& flagged() const {
+    return derived();
+  }
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    CommaInitializer<Derived> operator<< (const DenseBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC CommaInitializer<Derived> operator<<(const DenseBase<OtherDerived>& other);
 
-    typedef Transpose<Derived> TransposeReturnType;
-    EIGEN_DEVICE_FUNC
-    TransposeReturnType transpose();
-    typedef typename internal::add_const<Transpose<const Derived> >::type ConstTransposeReturnType;
-    EIGEN_DEVICE_FUNC
-    ConstTransposeReturnType transpose() const;
-    EIGEN_DEVICE_FUNC
-    void transposeInPlace();
+  typedef Transpose<Derived> TransposeReturnType;
+  EIGEN_DEVICE_FUNC TransposeReturnType transpose();
+  typedef Transpose<const Derived> ConstTransposeReturnType;
+  EIGEN_DEVICE_FUNC const ConstTransposeReturnType transpose() const;
+  EIGEN_DEVICE_FUNC void transposeInPlace();
 
-    EIGEN_DEVICE_FUNC static const ConstantReturnType
-    Constant(Index rows, Index cols, const Scalar& value);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType
-    Constant(Index size, const Scalar& value);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType
-    Constant(const Scalar& value);
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index rows, Index cols, const Scalar& value);
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index size, const Scalar& value);
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(const Scalar& value);
 
-    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
-    LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high);
-    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
-    LinSpaced(Sequential_t, const Scalar& low, const Scalar& high);
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, Index size,
+                                                                                            const Scalar& low,
+                                                                                            const Scalar& high);
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t,
+                                                                                            const Scalar& low,
+                                                                                            const Scalar& high);
 
-    EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
-    LinSpaced(Index size, const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
-    LinSpaced(const Scalar& low, const Scalar& high);
+  EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Index size, const Scalar& low,
+                                                                           const Scalar& high);
+  EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(const Scalar& low, const Scalar& high);
 
-    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
-    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
-    NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func);
-    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
-    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
-    NullaryExpr(Index size, const CustomNullaryOp& func);
-    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
-    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
-    NullaryExpr(const CustomNullaryOp& func);
+  EIGEN_DEVICE_FUNC static const RandomAccessEqualSpacedReturnType EqualSpaced(Index size, const Scalar& low,
+                                                                               const Scalar& step);
+  EIGEN_DEVICE_FUNC static const RandomAccessEqualSpacedReturnType EqualSpaced(const Scalar& low, const Scalar& step);
 
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index size);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero();
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index size);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones();
+  template <typename CustomNullaryOp>
+  EIGEN_DEVICE_FUNC static const CwiseNullaryOp<CustomNullaryOp, PlainObject> NullaryExpr(Index rows, Index cols,
+                                                                                          const CustomNullaryOp& func);
+  template <typename CustomNullaryOp>
+  EIGEN_DEVICE_FUNC static const CwiseNullaryOp<CustomNullaryOp, PlainObject> NullaryExpr(Index size,
+                                                                                          const CustomNullaryOp& func);
+  template <typename CustomNullaryOp>
+  EIGEN_DEVICE_FUNC static const CwiseNullaryOp<CustomNullaryOp, PlainObject> NullaryExpr(const CustomNullaryOp& func);
 
-    EIGEN_DEVICE_FUNC void fill(const Scalar& value);
-    EIGEN_DEVICE_FUNC Derived& setConstant(const Scalar& value);
-    EIGEN_DEVICE_FUNC Derived& setLinSpaced(Index size, const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC Derived& setLinSpaced(const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC Derived& setZero();
-    EIGEN_DEVICE_FUNC Derived& setOnes();
-    EIGEN_DEVICE_FUNC Derived& setRandom();
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index rows, Index cols);
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index size);
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Zero();
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index rows, Index cols);
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index size);
+  EIGEN_DEVICE_FUNC static const ConstantReturnType Ones();
 
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC
-    bool isApprox(const DenseBase<OtherDerived>& other,
-                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC
-    bool isMuchSmallerThan(const RealScalar& other,
-                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC
-    bool isMuchSmallerThan(const DenseBase<OtherDerived>& other,
-                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  EIGEN_DEVICE_FUNC void fill(const Scalar& value);
+  EIGEN_DEVICE_FUNC Derived& setConstant(const Scalar& value);
+  EIGEN_DEVICE_FUNC Derived& setLinSpaced(Index size, const Scalar& low, const Scalar& high);
+  EIGEN_DEVICE_FUNC Derived& setLinSpaced(const Scalar& low, const Scalar& high);
+  EIGEN_DEVICE_FUNC Derived& setEqualSpaced(Index size, const Scalar& low, const Scalar& step);
+  EIGEN_DEVICE_FUNC Derived& setEqualSpaced(const Scalar& low, const Scalar& step);
+  EIGEN_DEVICE_FUNC Derived& setZero();
+  EIGEN_DEVICE_FUNC Derived& setOnes();
+  EIGEN_DEVICE_FUNC Derived& setRandom();
 
-    EIGEN_DEVICE_FUNC bool isApproxToConstant(const Scalar& value, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC bool isConstant(const Scalar& value, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC bool isZero(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC bool isOnes(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC bool isApprox(const DenseBase<OtherDerived>& other,
+                                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  EIGEN_DEVICE_FUNC bool isMuchSmallerThan(const RealScalar& other,
+                                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC bool isMuchSmallerThan(const DenseBase<OtherDerived>& other,
+                                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
 
-    inline bool hasNaN() const;
-    inline bool allFinite() const;
+  EIGEN_DEVICE_FUNC bool isApproxToConstant(const Scalar& value,
+                                            const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  EIGEN_DEVICE_FUNC bool isConstant(const Scalar& value,
+                                    const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  EIGEN_DEVICE_FUNC bool isZero(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  EIGEN_DEVICE_FUNC bool isOnes(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator*=(const Scalar& other);
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator/=(const Scalar& other);
+  EIGEN_DEVICE_FUNC inline bool hasNaN() const;
+  EIGEN_DEVICE_FUNC inline bool allFinite() const;
 
-    typedef typename internal::add_const_on_value_type<typename internal::eval<Derived>::type>::type EvalReturnType;
-    /** \returns the matrix or vector obtained by evaluating this expression.
-      *
-      * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
-      * a const reference, in order to avoid a useless copy.
-      *
-      * \warning Be careful with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page \endlink.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE EvalReturnType eval() const
-    {
-      // Even though MSVC does not honor strong inlining when the return type
-      // is a dynamic matrix, we desperately need strong inlining for fixed
-      // size types on MSVC.
-      return typename internal::eval<Derived>::type(derived());
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const Scalar& other);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const Scalar& other);
 
-    /** swaps *this with the expression \a other.
-      *
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void swap(const DenseBase<OtherDerived>& other)
-    {
-      EIGEN_STATIC_ASSERT(!OtherDerived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
-      eigen_assert(rows()==other.rows() && cols()==other.cols());
-      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
-    }
+  typedef internal::add_const_on_value_type_t<typename internal::eval<Derived>::type> EvalReturnType;
+  /** \returns the matrix or vector obtained by evaluating this expression.
+   *
+   * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
+   * a const reference, in order to avoid a useless copy.
+   *
+   * \warning Be careful with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page
+   * \endlink.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EvalReturnType eval() const {
+    // Even though MSVC does not honor strong inlining when the return type
+    // is a dynamic matrix, we desperately need strong inlining for fixed
+    // size types on MSVC.
+    return typename internal::eval<Derived>::type(derived());
+  }
 
-    /** swaps *this with the matrix or array \a other.
-      *
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void swap(PlainObjectBase<OtherDerived>& other)
-    {
-      eigen_assert(rows()==other.rows() && cols()==other.cols());
-      call_assignment(derived(), other.derived(), internal::swap_assign_op<Scalar>());
-    }
+  /** swaps *this with the expression \a other.
+   *
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(const DenseBase<OtherDerived>& other) {
+    EIGEN_STATIC_ASSERT(!OtherDerived::IsPlainObjectBase, THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+    eigen_assert(rows() == other.rows() && cols() == other.cols());
+    call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
+  }
 
-    EIGEN_DEVICE_FUNC inline const NestByValue<Derived> nestByValue() const;
-    EIGEN_DEVICE_FUNC inline const ForceAlignedAccess<Derived> forceAlignedAccess() const;
-    EIGEN_DEVICE_FUNC inline ForceAlignedAccess<Derived> forceAlignedAccess();
-    template<bool Enable> EIGEN_DEVICE_FUNC
-    inline const typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type forceAlignedAccessIf() const;
-    template<bool Enable> EIGEN_DEVICE_FUNC
-    inline typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type forceAlignedAccessIf();
+  /** swaps *this with the matrix or array \a other.
+   *
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(PlainObjectBase<OtherDerived>& other) {
+    eigen_assert(rows() == other.rows() && cols() == other.cols());
+    call_assignment(derived(), other.derived(), internal::swap_assign_op<Scalar>());
+  }
 
-    EIGEN_DEVICE_FUNC Scalar sum() const;
-    EIGEN_DEVICE_FUNC Scalar mean() const;
-    EIGEN_DEVICE_FUNC Scalar trace() const;
+  EIGEN_DEVICE_FUNC inline const NestByValue<Derived> nestByValue() const;
+  EIGEN_DEVICE_FUNC inline const ForceAlignedAccess<Derived> forceAlignedAccess() const;
+  EIGEN_DEVICE_FUNC inline ForceAlignedAccess<Derived> forceAlignedAccess();
+  template <bool Enable>
+  EIGEN_DEVICE_FUNC inline const std::conditional_t<Enable, ForceAlignedAccess<Derived>, Derived&>
+  forceAlignedAccessIf() const;
+  template <bool Enable>
+  EIGEN_DEVICE_FUNC inline std::conditional_t<Enable, ForceAlignedAccess<Derived>, Derived&> forceAlignedAccessIf();
 
-    EIGEN_DEVICE_FUNC Scalar prod() const;
+  EIGEN_DEVICE_FUNC Scalar sum() const;
+  EIGEN_DEVICE_FUNC Scalar mean() const;
+  EIGEN_DEVICE_FUNC Scalar trace() const;
 
-    template<int NaNPropagation>
-    EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar minCoeff() const;
-    template<int NaNPropagation>
-    EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar maxCoeff() const;
+  EIGEN_DEVICE_FUNC Scalar prod() const;
 
+  template <int NaNPropagation>
+  EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar minCoeff() const;
+  template <int NaNPropagation>
+  EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar maxCoeff() const;
 
-    // By default, the fastest version with undefined NaN propagation semantics is
-    // used.
-    // TODO(rmlarsen): Replace with default template argument when we move to
-    // c++11 or beyond.
-    EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar minCoeff() const {
-      return minCoeff<PropagateFast>();
-    }
-    EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar maxCoeff() const {
-      return maxCoeff<PropagateFast>();
-    }
+  // By default, the fastest version with undefined NaN propagation semantics is
+  // used.
+  // TODO(rmlarsen): Replace with default template argument when we move to
+  // c++11 or beyond.
+  EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar minCoeff() const {
+    return minCoeff<PropagateFast>();
+  }
+  EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar maxCoeff() const {
+    return maxCoeff<PropagateFast>();
+  }
 
-    template<int NaNPropagation, typename IndexType>
-    EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const;
-    template<int NaNPropagation, typename IndexType>
-    EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const;
-    template<int NaNPropagation, typename IndexType>
-    EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const;
-    template<int NaNPropagation, typename IndexType>
-    EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const;
+  template <int NaNPropagation, typename IndexType>
+  EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const;
+  template <int NaNPropagation, typename IndexType>
+  EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const;
+  template <int NaNPropagation, typename IndexType>
+  EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const;
+  template <int NaNPropagation, typename IndexType>
+  EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const;
 
-    // TODO(rmlarsen): Replace these methods with a default template argument.
-    template<typename IndexType>
-    EIGEN_DEVICE_FUNC inline
-    typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const {
-      return minCoeff<PropagateFast>(row, col);
-    }
-    template<typename IndexType>
-    EIGEN_DEVICE_FUNC inline
-    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const {
-      return maxCoeff<PropagateFast>(row, col);
-    }
-    template<typename IndexType>
-     EIGEN_DEVICE_FUNC inline
-    typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const {
-      return minCoeff<PropagateFast>(index);
-    }
-    template<typename IndexType>
-    EIGEN_DEVICE_FUNC inline
-    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const {
-      return maxCoeff<PropagateFast>(index);
-    }
-  
-    template<typename BinaryOp>
-    EIGEN_DEVICE_FUNC
-    Scalar redux(const BinaryOp& func) const;
+  // TODO(rmlarsen): Replace these methods with a default template argument.
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const {
+    return minCoeff<PropagateFast>(row, col);
+  }
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const {
+    return maxCoeff<PropagateFast>(row, col);
+  }
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const {
+    return minCoeff<PropagateFast>(index);
+  }
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const {
+    return maxCoeff<PropagateFast>(index);
+  }
 
-    template<typename Visitor>
-    EIGEN_DEVICE_FUNC
-    void visit(Visitor& func) const;
+  template <typename BinaryOp>
+  EIGEN_DEVICE_FUNC Scalar redux(const BinaryOp& func) const;
 
-    /** \returns a WithFormat proxy object allowing to print a matrix the with given
-      * format \a fmt.
-      *
-      * See class IOFormat for some examples.
-      *
-      * \sa class IOFormat, class WithFormat
-      */
-    inline const WithFormat<Derived> format(const IOFormat& fmt) const
-    {
-      return WithFormat<Derived>(derived(), fmt);
-    }
+  template <typename Visitor>
+  EIGEN_DEVICE_FUNC void visit(Visitor& func) const;
 
-    /** \returns the unique coefficient of a 1x1 expression */
-    EIGEN_DEVICE_FUNC
-    CoeffReturnType value() const
-    {
-      EIGEN_STATIC_ASSERT_SIZE_1x1(Derived)
-      eigen_assert(this->rows() == 1 && this->cols() == 1);
-      return derived().coeff(0,0);
-    }
+  /** \returns a WithFormat proxy object allowing to print a matrix the with given
+   * format \a fmt.
+   *
+   * See class IOFormat for some examples.
+   *
+   * \sa class IOFormat, class WithFormat
+   */
+  inline const WithFormat<Derived> format(const IOFormat& fmt) const { return WithFormat<Derived>(derived(), fmt); }
 
-    EIGEN_DEVICE_FUNC bool all() const;
-    EIGEN_DEVICE_FUNC bool any() const;
-    EIGEN_DEVICE_FUNC Index count() const;
+  /** \returns the unique coefficient of a 1x1 expression */
+  EIGEN_DEVICE_FUNC CoeffReturnType value() const {
+    EIGEN_STATIC_ASSERT_SIZE_1x1(Derived) eigen_assert(this->rows() == 1 && this->cols() == 1);
+    return derived().coeff(0, 0);
+  }
 
-    typedef VectorwiseOp<Derived, Horizontal> RowwiseReturnType;
-    typedef const VectorwiseOp<const Derived, Horizontal> ConstRowwiseReturnType;
-    typedef VectorwiseOp<Derived, Vertical> ColwiseReturnType;
-    typedef const VectorwiseOp<const Derived, Vertical> ConstColwiseReturnType;
+  EIGEN_DEVICE_FUNC bool all() const;
+  EIGEN_DEVICE_FUNC bool any() const;
+  EIGEN_DEVICE_FUNC Index count() const;
 
-    /** \returns a VectorwiseOp wrapper of *this for broadcasting and partial reductions
-    *
-    * Example: \include MatrixBase_rowwise.cpp
-    * Output: \verbinclude MatrixBase_rowwise.out
-    *
-    * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
-    */
-    //Code moved here due to a CUDA compiler bug
-    EIGEN_DEVICE_FUNC inline ConstRowwiseReturnType rowwise() const {
-      return ConstRowwiseReturnType(derived());
-    }
-    EIGEN_DEVICE_FUNC RowwiseReturnType rowwise();
+  typedef VectorwiseOp<Derived, Horizontal> RowwiseReturnType;
+  typedef const VectorwiseOp<const Derived, Horizontal> ConstRowwiseReturnType;
+  typedef VectorwiseOp<Derived, Vertical> ColwiseReturnType;
+  typedef const VectorwiseOp<const Derived, Vertical> ConstColwiseReturnType;
 
-    /** \returns a VectorwiseOp wrapper of *this broadcasting and partial reductions
-    *
-    * Example: \include MatrixBase_colwise.cpp
-    * Output: \verbinclude MatrixBase_colwise.out
-    *
-    * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
-    */
-    EIGEN_DEVICE_FUNC inline ConstColwiseReturnType colwise() const {
-      return ConstColwiseReturnType(derived());
-    }
-    EIGEN_DEVICE_FUNC ColwiseReturnType colwise();
+  /** \returns a VectorwiseOp wrapper of *this for broadcasting and partial reductions
+   *
+   * Example: \include MatrixBase_rowwise.cpp
+   * Output: \verbinclude MatrixBase_rowwise.out
+   *
+   * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
+   */
+  // Code moved here due to a CUDA compiler bug
+  EIGEN_DEVICE_FUNC inline ConstRowwiseReturnType rowwise() const { return ConstRowwiseReturnType(derived()); }
+  EIGEN_DEVICE_FUNC RowwiseReturnType rowwise();
 
-    typedef CwiseNullaryOp<internal::scalar_random_op<Scalar>,PlainObject> RandomReturnType;
-    static const RandomReturnType Random(Index rows, Index cols);
-    static const RandomReturnType Random(Index size);
-    static const RandomReturnType Random();
+  /** \returns a VectorwiseOp wrapper of *this broadcasting and partial reductions
+   *
+   * Example: \include MatrixBase_colwise.cpp
+   * Output: \verbinclude MatrixBase_colwise.out
+   *
+   * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
+   */
+  EIGEN_DEVICE_FUNC inline ConstColwiseReturnType colwise() const { return ConstColwiseReturnType(derived()); }
+  EIGEN_DEVICE_FUNC ColwiseReturnType colwise();
 
-    template<typename ThenDerived,typename ElseDerived>
-    inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived,ElseDerived>
-    select(const DenseBase<ThenDerived>& thenMatrix,
-           const DenseBase<ElseDerived>& elseMatrix) const;
+  typedef CwiseNullaryOp<internal::scalar_random_op<Scalar>, PlainObject> RandomReturnType;
+  static const RandomReturnType Random(Index rows, Index cols);
+  static const RandomReturnType Random(Index size);
+  static const RandomReturnType Random();
 
-    template<typename ThenDerived>
-    inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
-    select(const DenseBase<ThenDerived>& thenMatrix, const typename ThenDerived::Scalar& elseScalar) const;
+  template <typename ThenDerived, typename ElseDerived>
+  inline EIGEN_DEVICE_FUNC
+      CwiseTernaryOp<internal::scalar_boolean_select_op<typename DenseBase<ThenDerived>::Scalar,
+                                                        typename DenseBase<ElseDerived>::Scalar, Scalar>,
+                     ThenDerived, ElseDerived, Derived>
+      select(const DenseBase<ThenDerived>& thenMatrix, const DenseBase<ElseDerived>& elseMatrix) const;
 
-    template<typename ElseDerived>
-    inline EIGEN_DEVICE_FUNC const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
-    select(const typename ElseDerived::Scalar& thenScalar, const DenseBase<ElseDerived>& elseMatrix) const;
+  template <typename ThenDerived>
+  inline EIGEN_DEVICE_FUNC
+      CwiseTernaryOp<internal::scalar_boolean_select_op<typename DenseBase<ThenDerived>::Scalar,
+                                                        typename DenseBase<ThenDerived>::Scalar, Scalar>,
+                     ThenDerived, typename DenseBase<ThenDerived>::ConstantReturnType, Derived>
+      select(const DenseBase<ThenDerived>& thenMatrix, const typename DenseBase<ThenDerived>::Scalar& elseScalar) const;
 
-    template<int p> RealScalar lpNorm() const;
+  template <typename ElseDerived>
+  inline EIGEN_DEVICE_FUNC
+      CwiseTernaryOp<internal::scalar_boolean_select_op<typename DenseBase<ElseDerived>::Scalar,
+                                                        typename DenseBase<ElseDerived>::Scalar, Scalar>,
+                     typename DenseBase<ElseDerived>::ConstantReturnType, ElseDerived, Derived>
+      select(const typename DenseBase<ElseDerived>::Scalar& thenScalar, const DenseBase<ElseDerived>& elseMatrix) const;
 
-    template<int RowFactor, int ColFactor>
-    EIGEN_DEVICE_FUNC
-    const Replicate<Derived,RowFactor,ColFactor> replicate() const;
-    /**
-    * \return an expression of the replication of \c *this
-    *
-    * Example: \include MatrixBase_replicate_int_int.cpp
-    * Output: \verbinclude MatrixBase_replicate_int_int.out
-    *
-    * \sa VectorwiseOp::replicate(), DenseBase::replicate<int,int>(), class Replicate
-    */
-    //Code moved here due to a CUDA compiler bug
-    EIGEN_DEVICE_FUNC
-    const Replicate<Derived, Dynamic, Dynamic> replicate(Index rowFactor, Index colFactor) const
-    {
-      return Replicate<Derived, Dynamic, Dynamic>(derived(), rowFactor, colFactor);
-    }
+  template <int p>
+  RealScalar lpNorm() const;
 
-    typedef Reverse<Derived, BothDirections> ReverseReturnType;
-    typedef const Reverse<const Derived, BothDirections> ConstReverseReturnType;
-    EIGEN_DEVICE_FUNC ReverseReturnType reverse();
-    /** This is the const version of reverse(). */
-    //Code moved here due to a CUDA compiler bug
-    EIGEN_DEVICE_FUNC ConstReverseReturnType reverse() const
-    {
-      return ConstReverseReturnType(derived());
-    }
-    EIGEN_DEVICE_FUNC void reverseInPlace();
+  template <int RowFactor, int ColFactor>
+  EIGEN_DEVICE_FUNC const Replicate<Derived, RowFactor, ColFactor> replicate() const;
+  /**
+   * \return an expression of the replication of \c *this
+   *
+   * Example: \include MatrixBase_replicate_int_int.cpp
+   * Output: \verbinclude MatrixBase_replicate_int_int.out
+   *
+   * \sa VectorwiseOp::replicate(), DenseBase::replicate<int,int>(), class Replicate
+   */
+  // Code moved here due to a CUDA compiler bug
+  EIGEN_DEVICE_FUNC const Replicate<Derived, Dynamic, Dynamic> replicate(Index rowFactor, Index colFactor) const {
+    return Replicate<Derived, Dynamic, Dynamic>(derived(), rowFactor, colFactor);
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** STL-like <a href="https://en.cppreference.com/w/cpp/named_req/RandomAccessIterator">RandomAccessIterator</a>
-      * iterator type as returned by the begin() and end() methods.
-      */
-    typedef random_access_iterator_type iterator;
-    /** This is the const version of iterator (aka read-only) */
-    typedef random_access_iterator_type const_iterator;
-    #else
-    typedef typename internal::conditional< (Flags&DirectAccessBit)==DirectAccessBit,
-                                            internal::pointer_based_stl_iterator<Derived>,
-                                            internal::generic_randaccess_stl_iterator<Derived>
-                                          >::type iterator_type;
+  typedef Reverse<Derived, BothDirections> ReverseReturnType;
+  typedef const Reverse<const Derived, BothDirections> ConstReverseReturnType;
+  EIGEN_DEVICE_FUNC ReverseReturnType reverse();
+  /** This is the const version of reverse(). */
+  // Code moved here due to a CUDA compiler bug
+  EIGEN_DEVICE_FUNC ConstReverseReturnType reverse() const { return ConstReverseReturnType(derived()); }
+  EIGEN_DEVICE_FUNC void reverseInPlace();
 
-    typedef typename internal::conditional< (Flags&DirectAccessBit)==DirectAccessBit,
-                                            internal::pointer_based_stl_iterator<const Derived>,
-                                            internal::generic_randaccess_stl_iterator<const Derived>
-                                          >::type const_iterator_type;
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** STL-like <a href="https://en.cppreference.com/w/cpp/named_req/RandomAccessIterator">RandomAccessIterator</a>
+   * iterator type as returned by the begin() and end() methods.
+   */
+  typedef random_access_iterator_type iterator;
+  /** This is the const version of iterator (aka read-only) */
+  typedef random_access_iterator_type const_iterator;
+#else
+  typedef std::conditional_t<(Flags & DirectAccessBit) == DirectAccessBit,
+                             internal::pointer_based_stl_iterator<Derived>,
+                             internal::generic_randaccess_stl_iterator<Derived> >
+      iterator_type;
 
-    // Stl-style iterators are supported only for vectors.
+  typedef std::conditional_t<(Flags & DirectAccessBit) == DirectAccessBit,
+                             internal::pointer_based_stl_iterator<const Derived>,
+                             internal::generic_randaccess_stl_iterator<const Derived> >
+      const_iterator_type;
 
-    typedef typename internal::conditional< IsVectorAtCompileTime,
-                                            iterator_type,
-                                            void
-                                          >::type iterator;
+  // Stl-style iterators are supported only for vectors.
 
-    typedef typename internal::conditional< IsVectorAtCompileTime,
-                                            const_iterator_type,
-                                            void
-                                          >::type const_iterator;
-    #endif
+  typedef std::conditional_t<IsVectorAtCompileTime, iterator_type, void> iterator;
 
-    inline iterator begin();
-    inline const_iterator begin() const;
-    inline const_iterator cbegin() const;
-    inline iterator end();
-    inline const_iterator end() const;
-    inline const_iterator cend() const;
+  typedef std::conditional_t<IsVectorAtCompileTime, const_iterator_type, void> const_iterator;
+#endif
+
+  inline iterator begin();
+  inline const_iterator begin() const;
+  inline const_iterator cbegin() const;
+  inline iterator end();
+  inline const_iterator end() const;
+  inline const_iterator cend() const;
 
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::DenseBase
 #define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 #define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
-#define EIGEN_DOC_UNARY_ADDONS(X,Y)
-#   include "../plugins/CommonCwiseUnaryOps.h"
-#   include "../plugins/BlockMethods.h"
-#   include "../plugins/IndexedViewMethods.h"
-#   include "../plugins/ReshapedMethods.h"
-#   ifdef EIGEN_DENSEBASE_PLUGIN
-#     include EIGEN_DENSEBASE_PLUGIN
-#   endif
+#define EIGEN_DOC_UNARY_ADDONS(X, Y)
+#include "../plugins/CommonCwiseUnaryOps.inc"
+#include "../plugins/BlockMethods.inc"
+#include "../plugins/IndexedViewMethods.inc"
+#include "../plugins/ReshapedMethods.inc"
+#ifdef EIGEN_DENSEBASE_PLUGIN
+#include EIGEN_DENSEBASE_PLUGIN
+#endif
 #undef EIGEN_CURRENT_STORAGE_BASE_CLASS
 #undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 #undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
 #undef EIGEN_DOC_UNARY_ADDONS
 
-    // disable the use of evalTo for dense objects with a nice compilation error
-    template<typename Dest>
-    EIGEN_DEVICE_FUNC
-    inline void evalTo(Dest& ) const
-    {
-      EIGEN_STATIC_ASSERT((internal::is_same<Dest,void>::value),THE_EVAL_EVALTO_FUNCTION_SHOULD_NEVER_BE_CALLED_FOR_DENSE_OBJECTS);
-    }
+  // disable the use of evalTo for dense objects with a nice compilation error
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void evalTo(Dest&) const {
+    EIGEN_STATIC_ASSERT((internal::is_same<Dest, void>::value),
+                        THE_EVAL_EVALTO_FUNCTION_SHOULD_NEVER_BE_CALLED_FOR_DENSE_OBJECTS);
+  }
 
-  protected:
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(DenseBase)
-    /** Default constructor. Do nothing. */
-    EIGEN_DEVICE_FUNC DenseBase()
-    {
-      /* Just checks for self-consistency of the flags.
-       * Only do it when debugging Eigen, as this borders on paranoia and could slow compilation down
-       */
+ protected:
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(DenseBase)
+  /** Default constructor. Do nothing. */
+  EIGEN_DEVICE_FUNC constexpr DenseBase() {
+    /* Just checks for self-consistency of the flags.
+     * Only do it when debugging Eigen, as this borders on paranoia and could slow compilation down
+     */
 #ifdef EIGEN_INTERNAL_DEBUGGING
-      EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, int(IsRowMajor))
-                        && EIGEN_IMPLIES(MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1, int(!IsRowMajor))),
-                          INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION)
+    EIGEN_STATIC_ASSERT(
+        (internal::check_implication(MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1, int(IsRowMajor)) &&
+         internal::check_implication(MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1, int(!IsRowMajor))),
+        INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION)
 #endif
-    }
+  }
 
-  private:
-    EIGEN_DEVICE_FUNC explicit DenseBase(int);
-    EIGEN_DEVICE_FUNC DenseBase(int,int);
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC explicit DenseBase(const DenseBase<OtherDerived>&);
+ private:
+  EIGEN_DEVICE_FUNC explicit DenseBase(int);
+  EIGEN_DEVICE_FUNC DenseBase(int, int);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC explicit DenseBase(const DenseBase<OtherDerived>&);
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DENSEBASE_H
+#endif  // EIGEN_DENSEBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
index 37fcdb5..48c6d73 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
@@ -10,676 +10,559 @@
 #ifndef EIGEN_DENSECOEFFSBASE_H
 #define EIGEN_DENSECOEFFSBASE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename T> struct add_const_on_value_type_if_arithmetic
-{
-  typedef typename conditional<is_arithmetic<T>::value, T, typename add_const_on_value_type<T>::type>::type type;
+template <typename T>
+struct add_const_on_value_type_if_arithmetic {
+  typedef std::conditional_t<is_arithmetic<T>::value, T, add_const_on_value_type_t<T>> type;
 };
-}
+}  // namespace internal
 
 /** \brief Base class providing read-only coefficient access to matrices and arrays.
-  * \ingroup Core_Module
-  * \tparam Derived Type of the derived class
-  *
-  * \note #ReadOnlyAccessors Constant indicating read-only access
-  *
-  * This class defines the \c operator() \c const function and friends, which can be used to read specific
-  * entries of a matrix or array.
-  *
-  * \sa DenseCoeffsBase<Derived, WriteAccessors>, DenseCoeffsBase<Derived, DirectAccessors>,
-  *     \ref TopicClassHierarchy
-  */
-template<typename Derived>
-class DenseCoeffsBase<Derived,ReadOnlyAccessors> : public EigenBase<Derived>
-{
-  public:
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+ * \ingroup Core_Module
+ * \tparam Derived Type of the derived class
+ *
+ * \note #ReadOnlyAccessors Constant indicating read-only access
+ *
+ * This class defines the \c operator() \c const function and friends, which can be used to read specific
+ * entries of a matrix or array.
+ *
+ * \sa DenseCoeffsBase<Derived, WriteAccessors>, DenseCoeffsBase<Derived, DirectAccessors>,
+ *     \ref TopicClassHierarchy
+ */
+template <typename Derived>
+class DenseCoeffsBase<Derived, ReadOnlyAccessors> : public EigenBase<Derived> {
+ public:
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename internal::packet_traits<Scalar>::type PacketScalar;
 
-    // Explanation for this CoeffReturnType typedef.
-    // - This is the return type of the coeff() method.
-    // - The LvalueBit means exactly that we can offer a coeffRef() method, which means exactly that we can get references
-    // to coeffs, which means exactly that we can have coeff() return a const reference (as opposed to returning a value).
-    // - The is_artihmetic check is required since "const int", "const double", etc. will cause warnings on some systems
-    // while the declaration of "const T", where T is a non arithmetic type does not. Always returning "const Scalar&" is
-    // not possible, since the underlying expressions might not offer a valid address the reference could be referring to.
-    typedef typename internal::conditional<bool(internal::traits<Derived>::Flags&LvalueBit),
-                         const Scalar&,
-                         typename internal::conditional<internal::is_arithmetic<Scalar>::value, Scalar, const Scalar>::type
-                     >::type CoeffReturnType;
+  // Explanation for this CoeffReturnType typedef.
+  // - This is the return type of the coeff() method.
+  // - The LvalueBit means exactly that we can offer a coeffRef() method, which means exactly that we can get references
+  // to coeffs, which means exactly that we can have coeff() return a const reference (as opposed to returning a value).
+  // - The is_arithmetic check is required since "const int", "const double", etc. will cause warnings on some systems
+  // while the declaration of "const T", where T is a non arithmetic type does not. Always returning "const Scalar&" is
+  // not possible, since the underlying expressions might not offer a valid address the reference could be referring to.
+  typedef std::conditional_t<bool(internal::traits<Derived>::Flags& LvalueBit), const Scalar&,
+                             std::conditional_t<internal::is_arithmetic<Scalar>::value, Scalar, const Scalar>>
+      CoeffReturnType;
 
-    typedef typename internal::add_const_on_value_type_if_arithmetic<
-                         typename internal::packet_traits<Scalar>::type
-                     >::type PacketReturnType;
+  typedef typename internal::add_const_on_value_type_if_arithmetic<typename internal::packet_traits<Scalar>::type>::type
+      PacketReturnType;
 
-    typedef EigenBase<Derived> Base;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::derived;
+  typedef EigenBase<Derived> Base;
+  using Base::cols;
+  using Base::derived;
+  using Base::rows;
+  using Base::size;
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) const
-    {
-      return int(Derived::RowsAtCompileTime) == 1 ? 0
-          : int(Derived::ColsAtCompileTime) == 1 ? inner
-          : int(Derived::Flags)&RowMajorBit ? outer
-          : inner;
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) const {
+    return int(Derived::RowsAtCompileTime) == 1   ? 0
+           : int(Derived::ColsAtCompileTime) == 1 ? inner
+           : int(Derived::Flags) & RowMajorBit    ? outer
+                                                  : inner;
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) const
-    {
-      return int(Derived::ColsAtCompileTime) == 1 ? 0
-          : int(Derived::RowsAtCompileTime) == 1 ? inner
-          : int(Derived::Flags)&RowMajorBit ? inner
-          : outer;
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) const {
+    return int(Derived::ColsAtCompileTime) == 1   ? 0
+           : int(Derived::RowsAtCompileTime) == 1 ? inner
+           : int(Derived::Flags) & RowMajorBit    ? inner
+                                                  : outer;
+  }
 
-    /** Short version: don't use this function, use
-      * \link operator()(Index,Index) const \endlink instead.
-      *
-      * Long version: this function is similar to
-      * \link operator()(Index,Index) const \endlink, but without the assertion.
-      * Use this for limiting the performance cost of debugging code when doing
-      * repeated coefficient access. Only use this when it is guaranteed that the
-      * parameters \a row and \a col are in range.
-      *
-      * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
-      * function equivalent to \link operator()(Index,Index) const \endlink.
-      *
-      * \sa operator()(Index,Index) const, coeffRef(Index,Index), coeff(Index) const
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const
-    {
-      eigen_internal_assert(row >= 0 && row < rows()
-                         && col >= 0 && col < cols());
-      return internal::evaluator<Derived>(derived()).coeff(row,col);
-    }
+  /** Short version: don't use this function, use
+   * \link operator()(Index,Index) const \endlink instead.
+   *
+   * Long version: this function is similar to
+   * \link operator()(Index,Index) const \endlink, but without the assertion.
+   * Use this for limiting the performance cost of debugging code when doing
+   * repeated coefficient access. Only use this when it is guaranteed that the
+   * parameters \a row and \a col are in range.
+   *
+   * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+   * function equivalent to \link operator()(Index,Index) const \endlink.
+   *
+   * \sa operator()(Index,Index) const, coeffRef(Index,Index), coeff(Index) const
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
+    eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
+    return internal::evaluator<Derived>(derived()).coeff(row, col);
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType coeffByOuterInner(Index outer, Index inner) const
-    {
-      return coeff(rowIndexByOuterInner(outer, inner),
-                   colIndexByOuterInner(outer, inner));
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeffByOuterInner(Index outer, Index inner) const {
+    return coeff(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner));
+  }
 
-    /** \returns the coefficient at given the given row and column.
-      *
-      * \sa operator()(Index,Index), operator[](Index)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType operator()(Index row, Index col) const
-    {
-      eigen_assert(row >= 0 && row < rows()
-          && col >= 0 && col < cols());
-      return coeff(row, col);
-    }
+  /** \returns the coefficient at given the given row and column.
+   *
+   * \sa operator()(Index,Index), operator[](Index)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator()(Index row, Index col) const {
+    eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
+    return coeff(row, col);
+  }
 
-    /** Short version: don't use this function, use
-      * \link operator[](Index) const \endlink instead.
-      *
-      * Long version: this function is similar to
-      * \link operator[](Index) const \endlink, but without the assertion.
-      * Use this for limiting the performance cost of debugging code when doing
-      * repeated coefficient access. Only use this when it is guaranteed that the
-      * parameter \a index is in range.
-      *
-      * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
-      * function equivalent to \link operator[](Index) const \endlink.
-      *
-      * \sa operator[](Index) const, coeffRef(Index), coeff(Index,Index) const
-      */
+  /** Short version: don't use this function, use
+   * \link operator[](Index) const \endlink instead.
+   *
+   * Long version: this function is similar to
+   * \link operator[](Index) const \endlink, but without the assertion.
+   * Use this for limiting the performance cost of debugging code when doing
+   * repeated coefficient access. Only use this when it is guaranteed that the
+   * parameter \a index is in range.
+   *
+   * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+   * function equivalent to \link operator[](Index) const \endlink.
+   *
+   * \sa operator[](Index) const, coeffRef(Index), coeff(Index,Index) const
+   */
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType
-    coeff(Index index) const
-    {
-      EIGEN_STATIC_ASSERT(internal::evaluator<Derived>::Flags & LinearAccessBit,
-                          THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS)
-      eigen_internal_assert(index >= 0 && index < size());
-      return internal::evaluator<Derived>(derived()).coeff(index);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
+    EIGEN_STATIC_ASSERT(internal::evaluator<Derived>::Flags & LinearAccessBit,
+                        THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS)
+    eigen_internal_assert(index >= 0 && index < size());
+    return internal::evaluator<Derived>(derived()).coeff(index);
+  }
 
+  /** \returns the coefficient at given index.
+   *
+   * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+   *
+   * \sa operator[](Index), operator()(Index,Index) const, x() const, y() const,
+   * z() const, w() const
+   */
 
-    /** \returns the coefficient at given index.
-      *
-      * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
-      *
-      * \sa operator[](Index), operator()(Index,Index) const, x() const, y() const,
-      * z() const, w() const
-      */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator[](Index index) const {
+    EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime,
+                        THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD)
+    eigen_assert(index >= 0 && index < size());
+    return coeff(index);
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType
-    operator[](Index index) const
-    {
-      EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime,
-                          THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD)
-      eigen_assert(index >= 0 && index < size());
-      return coeff(index);
-    }
+  /** \returns the coefficient at given index.
+   *
+   * This is synonymous to operator[](Index) const.
+   *
+   * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+   *
+   * \sa operator[](Index), operator()(Index,Index) const, x() const, y() const,
+   * z() const, w() const
+   */
 
-    /** \returns the coefficient at given index.
-      *
-      * This is synonymous to operator[](Index) const.
-      *
-      * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
-      *
-      * \sa operator[](Index), operator()(Index,Index) const, x() const, y() const,
-      * z() const, w() const
-      */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator()(Index index) const {
+    eigen_assert(index >= 0 && index < size());
+    return coeff(index);
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType
-    operator()(Index index) const
-    {
-      eigen_assert(index >= 0 && index < size());
-      return coeff(index);
-    }
+  /** equivalent to operator[](0).  */
 
-    /** equivalent to operator[](0).  */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType x() const { return (*this)[0]; }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType
-    x() const { return (*this)[0]; }
+  /** equivalent to operator[](1).  */
 
-    /** equivalent to operator[](1).  */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType y() const {
+    EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 2, OUT_OF_RANGE_ACCESS);
+    return (*this)[1];
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType
-    y() const
-    {
-      EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=2, OUT_OF_RANGE_ACCESS);
-      return (*this)[1];
-    }
+  /** equivalent to operator[](2).  */
 
-    /** equivalent to operator[](2).  */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType z() const {
+    EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 3, OUT_OF_RANGE_ACCESS);
+    return (*this)[2];
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType
-    z() const
-    {
-      EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=3, OUT_OF_RANGE_ACCESS);
-      return (*this)[2];
-    }
+  /** equivalent to operator[](3).  */
 
-    /** equivalent to operator[](3).  */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType w() const {
+    EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 4, OUT_OF_RANGE_ACCESS);
+    return (*this)[3];
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CoeffReturnType
-    w() const
-    {
-      EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=4, OUT_OF_RANGE_ACCESS);
-      return (*this)[3];
-    }
+  /** \internal
+   * \returns the packet of coefficients starting at the given row and column. It is your responsibility
+   * to ensure that a packet really starts there. This method is only available on expressions having the
+   * PacketAccessBit.
+   *
+   * The \a LoadMode parameter may have the value \a #Aligned or \a #Unaligned. Its effect is to select
+   * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
+   * starting at an address which is a multiple of the packet size.
+   */
 
-    /** \internal
-      * \returns the packet of coefficients starting at the given row and column. It is your responsibility
-      * to ensure that a packet really starts there. This method is only available on expressions having the
-      * PacketAccessBit.
-      *
-      * The \a LoadMode parameter may have the value \a #Aligned or \a #Unaligned. Its effect is to select
-      * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
-      * starting at an address which is a multiple of the packet size.
-      */
+  template <int LoadMode>
+  EIGEN_STRONG_INLINE PacketReturnType packet(Index row, Index col) const {
+    typedef typename internal::packet_traits<Scalar>::type DefaultPacketType;
+    eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
+    return internal::evaluator<Derived>(derived()).template packet<LoadMode, DefaultPacketType>(row, col);
+  }
 
-    template<int LoadMode>
-    EIGEN_STRONG_INLINE PacketReturnType packet(Index row, Index col) const
-    {
-      typedef typename internal::packet_traits<Scalar>::type DefaultPacketType;
-      eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
-      return internal::evaluator<Derived>(derived()).template packet<LoadMode,DefaultPacketType>(row,col);
-    }
+  /** \internal */
+  template <int LoadMode>
+  EIGEN_STRONG_INLINE PacketReturnType packetByOuterInner(Index outer, Index inner) const {
+    return packet<LoadMode>(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner));
+  }
 
+  /** \internal
+   * \returns the packet of coefficients starting at the given index. It is your responsibility
+   * to ensure that a packet really starts there. This method is only available on expressions having the
+   * PacketAccessBit and the LinearAccessBit.
+   *
+   * The \a LoadMode parameter may have the value \a #Aligned or \a #Unaligned. Its effect is to select
+   * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
+   * starting at an address which is a multiple of the packet size.
+   */
 
-    /** \internal */
-    template<int LoadMode>
-    EIGEN_STRONG_INLINE PacketReturnType packetByOuterInner(Index outer, Index inner) const
-    {
-      return packet<LoadMode>(rowIndexByOuterInner(outer, inner),
-                              colIndexByOuterInner(outer, inner));
-    }
+  template <int LoadMode>
+  EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const {
+    EIGEN_STATIC_ASSERT(internal::evaluator<Derived>::Flags & LinearAccessBit,
+                        THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS)
+    typedef typename internal::packet_traits<Scalar>::type DefaultPacketType;
+    eigen_internal_assert(index >= 0 && index < size());
+    return internal::evaluator<Derived>(derived()).template packet<LoadMode, DefaultPacketType>(index);
+  }
 
-    /** \internal
-      * \returns the packet of coefficients starting at the given index. It is your responsibility
-      * to ensure that a packet really starts there. This method is only available on expressions having the
-      * PacketAccessBit and the LinearAccessBit.
-      *
-      * The \a LoadMode parameter may have the value \a #Aligned or \a #Unaligned. Its effect is to select
-      * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
-      * starting at an address which is a multiple of the packet size.
-      */
-
-    template<int LoadMode>
-    EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const
-    {
-      EIGEN_STATIC_ASSERT(internal::evaluator<Derived>::Flags & LinearAccessBit,
-                          THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS)
-      typedef typename internal::packet_traits<Scalar>::type DefaultPacketType;
-      eigen_internal_assert(index >= 0 && index < size());
-      return internal::evaluator<Derived>(derived()).template packet<LoadMode,DefaultPacketType>(index);
-    }
-
-  protected:
-    // explanation: DenseBase is doing "using ..." on the methods from DenseCoeffsBase.
-    // But some methods are only available in the DirectAccess case.
-    // So we add dummy methods here with these names, so that "using... " doesn't fail.
-    // It's not private so that the child class DenseBase can access them, and it's not public
-    // either since it's an implementation detail, so has to be protected.
-    void coeffRef();
-    void coeffRefByOuterInner();
-    void writePacket();
-    void writePacketByOuterInner();
-    void copyCoeff();
-    void copyCoeffByOuterInner();
-    void copyPacket();
-    void copyPacketByOuterInner();
-    void stride();
-    void innerStride();
-    void outerStride();
-    void rowStride();
-    void colStride();
+ protected:
+  // explanation: DenseBase is doing "using ..." on the methods from DenseCoeffsBase.
+  // But some methods are only available in the DirectAccess case.
+  // So we add dummy methods here with these names, so that "using... " doesn't fail.
+  // It's not private so that the child class DenseBase can access them, and it's not public
+  // either since it's an implementation detail, so has to be protected.
+  void coeffRef();
+  void coeffRefByOuterInner();
+  void writePacket();
+  void writePacketByOuterInner();
+  void copyCoeff();
+  void copyCoeffByOuterInner();
+  void copyPacket();
+  void copyPacketByOuterInner();
+  void stride();
+  void innerStride();
+  void outerStride();
+  void rowStride();
+  void colStride();
 };
 
 /** \brief Base class providing read/write coefficient access to matrices and arrays.
-  * \ingroup Core_Module
-  * \tparam Derived Type of the derived class
-  *
-  * \note #WriteAccessors Constant indicating read/write access
-  *
-  * This class defines the non-const \c operator() function and friends, which can be used to write specific
-  * entries of a matrix or array. This class inherits DenseCoeffsBase<Derived, ReadOnlyAccessors> which
-  * defines the const variant for reading specific entries.
-  *
-  * \sa DenseCoeffsBase<Derived, DirectAccessors>, \ref TopicClassHierarchy
-  */
-template<typename Derived>
-class DenseCoeffsBase<Derived, WriteAccessors> : public DenseCoeffsBase<Derived, ReadOnlyAccessors>
-{
-  public:
+ * \ingroup Core_Module
+ * \tparam Derived Type of the derived class
+ *
+ * \note #WriteAccessors Constant indicating read/write access
+ *
+ * This class defines the non-const \c operator() function and friends, which can be used to write specific
+ * entries of a matrix or array. This class inherits DenseCoeffsBase<Derived, ReadOnlyAccessors> which
+ * defines the const variant for reading specific entries.
+ *
+ * \sa DenseCoeffsBase<Derived, DirectAccessors>, \ref TopicClassHierarchy
+ */
+template <typename Derived>
+class DenseCoeffsBase<Derived, WriteAccessors> : public DenseCoeffsBase<Derived, ReadOnlyAccessors> {
+ public:
+  typedef DenseCoeffsBase<Derived, ReadOnlyAccessors> Base;
 
-    typedef DenseCoeffsBase<Derived, ReadOnlyAccessors> Base;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+  using Base::coeff;
+  using Base::colIndexByOuterInner;
+  using Base::cols;
+  using Base::derived;
+  using Base::rowIndexByOuterInner;
+  using Base::rows;
+  using Base::size;
+  using Base::operator[];
+  using Base::operator();
+  using Base::w;
+  using Base::x;
+  using Base::y;
+  using Base::z;
 
-    using Base::coeff;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::derived;
-    using Base::rowIndexByOuterInner;
-    using Base::colIndexByOuterInner;
-    using Base::operator[];
-    using Base::operator();
-    using Base::x;
-    using Base::y;
-    using Base::z;
-    using Base::w;
+  /** Short version: don't use this function, use
+   * \link operator()(Index,Index) \endlink instead.
+   *
+   * Long version: this function is similar to
+   * \link operator()(Index,Index) \endlink, but without the assertion.
+   * Use this for limiting the performance cost of debugging code when doing
+   * repeated coefficient access. Only use this when it is guaranteed that the
+   * parameters \a row and \a col are in range.
+   *
+   * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+   * function equivalent to \link operator()(Index,Index) \endlink.
+   *
+   * \sa operator()(Index,Index), coeff(Index, Index) const, coeffRef(Index)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
+    eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
+    return internal::evaluator<Derived>(derived()).coeffRef(row, col);
+  }
 
-    /** Short version: don't use this function, use
-      * \link operator()(Index,Index) \endlink instead.
-      *
-      * Long version: this function is similar to
-      * \link operator()(Index,Index) \endlink, but without the assertion.
-      * Use this for limiting the performance cost of debugging code when doing
-      * repeated coefficient access. Only use this when it is guaranteed that the
-      * parameters \a row and \a col are in range.
-      *
-      * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
-      * function equivalent to \link operator()(Index,Index) \endlink.
-      *
-      * \sa operator()(Index,Index), coeff(Index, Index) const, coeffRef(Index)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col)
-    {
-      eigen_internal_assert(row >= 0 && row < rows()
-                         && col >= 0 && col < cols());
-      return internal::evaluator<Derived>(derived()).coeffRef(row,col);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRefByOuterInner(Index outer, Index inner) {
+    return coeffRef(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner));
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    coeffRefByOuterInner(Index outer, Index inner)
-    {
-      return coeffRef(rowIndexByOuterInner(outer, inner),
-                      colIndexByOuterInner(outer, inner));
-    }
+  /** \returns a reference to the coefficient at given the given row and column.
+   *
+   * \sa operator[](Index)
+   */
 
-    /** \returns a reference to the coefficient at given the given row and column.
-      *
-      * \sa operator[](Index)
-      */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(Index row, Index col) {
+    eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
+    return coeffRef(row, col);
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    operator()(Index row, Index col)
-    {
-      eigen_assert(row >= 0 && row < rows()
-          && col >= 0 && col < cols());
-      return coeffRef(row, col);
-    }
+  /** Short version: don't use this function, use
+   * \link operator[](Index) \endlink instead.
+   *
+   * Long version: this function is similar to
+   * \link operator[](Index) \endlink, but without the assertion.
+   * Use this for limiting the performance cost of debugging code when doing
+   * repeated coefficient access. Only use this when it is guaranteed that the
+   * parameters \a row and \a col are in range.
+   *
+   * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+   * function equivalent to \link operator[](Index) \endlink.
+   *
+   * \sa operator[](Index), coeff(Index) const, coeffRef(Index,Index)
+   */
 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) {
+    EIGEN_STATIC_ASSERT(internal::evaluator<Derived>::Flags & LinearAccessBit,
+                        THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS)
+    eigen_internal_assert(index >= 0 && index < size());
+    return internal::evaluator<Derived>(derived()).coeffRef(index);
+  }
 
-    /** Short version: don't use this function, use
-      * \link operator[](Index) \endlink instead.
-      *
-      * Long version: this function is similar to
-      * \link operator[](Index) \endlink, but without the assertion.
-      * Use this for limiting the performance cost of debugging code when doing
-      * repeated coefficient access. Only use this when it is guaranteed that the
-      * parameters \a row and \a col are in range.
-      *
-      * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
-      * function equivalent to \link operator[](Index) \endlink.
-      *
-      * \sa operator[](Index), coeff(Index) const, coeffRef(Index,Index)
-      */
+  /** \returns a reference to the coefficient at given index.
+   *
+   * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+   *
+   * \sa operator[](Index) const, operator()(Index,Index), x(), y(), z(), w()
+   */
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    coeffRef(Index index)
-    {
-      EIGEN_STATIC_ASSERT(internal::evaluator<Derived>::Flags & LinearAccessBit,
-                          THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS)
-      eigen_internal_assert(index >= 0 && index < size());
-      return internal::evaluator<Derived>(derived()).coeffRef(index);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator[](Index index) {
+    EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime,
+                        THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD)
+    eigen_assert(index >= 0 && index < size());
+    return coeffRef(index);
+  }
 
-    /** \returns a reference to the coefficient at given index.
-      *
-      * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
-      *
-      * \sa operator[](Index) const, operator()(Index,Index), x(), y(), z(), w()
-      */
+  /** \returns a reference to the coefficient at given index.
+   *
+   * This is synonymous to operator[](Index).
+   *
+   * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+   *
+   * \sa operator[](Index) const, operator()(Index,Index), x(), y(), z(), w()
+   */
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    operator[](Index index)
-    {
-      EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime,
-                          THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD)
-      eigen_assert(index >= 0 && index < size());
-      return coeffRef(index);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(Index index) {
+    eigen_assert(index >= 0 && index < size());
+    return coeffRef(index);
+  }
 
-    /** \returns a reference to the coefficient at given index.
-      *
-      * This is synonymous to operator[](Index).
-      *
-      * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
-      *
-      * \sa operator[](Index) const, operator()(Index,Index), x(), y(), z(), w()
-      */
+  /** equivalent to operator[](0).  */
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    operator()(Index index)
-    {
-      eigen_assert(index >= 0 && index < size());
-      return coeffRef(index);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& x() { return (*this)[0]; }
 
-    /** equivalent to operator[](0).  */
+  /** equivalent to operator[](1).  */
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    x() { return (*this)[0]; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& y() {
+    EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 2, OUT_OF_RANGE_ACCESS);
+    return (*this)[1];
+  }
 
-    /** equivalent to operator[](1).  */
+  /** equivalent to operator[](2).  */
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    y()
-    {
-      EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=2, OUT_OF_RANGE_ACCESS);
-      return (*this)[1];
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& z() {
+    EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 3, OUT_OF_RANGE_ACCESS);
+    return (*this)[2];
+  }
 
-    /** equivalent to operator[](2).  */
+  /** equivalent to operator[](3).  */
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    z()
-    {
-      EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=3, OUT_OF_RANGE_ACCESS);
-      return (*this)[2];
-    }
-
-    /** equivalent to operator[](3).  */
-
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar&
-    w()
-    {
-      EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=4, OUT_OF_RANGE_ACCESS);
-      return (*this)[3];
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& w() {
+    EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 4, OUT_OF_RANGE_ACCESS);
+    return (*this)[3];
+  }
 };
 
 /** \brief Base class providing direct read-only coefficient access to matrices and arrays.
-  * \ingroup Core_Module
-  * \tparam Derived Type of the derived class
-  *
-  * \note #DirectAccessors Constant indicating direct access
-  *
-  * This class defines functions to work with strides which can be used to access entries directly. This class
-  * inherits DenseCoeffsBase<Derived, ReadOnlyAccessors> which defines functions to access entries read-only using
-  * \c operator() .
-  *
-  * \sa \blank \ref TopicClassHierarchy
-  */
-template<typename Derived>
-class DenseCoeffsBase<Derived, DirectAccessors> : public DenseCoeffsBase<Derived, ReadOnlyAccessors>
-{
-  public:
+ * \ingroup Core_Module
+ * \tparam Derived Type of the derived class
+ *
+ * \note #DirectAccessors Constant indicating direct access
+ *
+ * This class defines functions to work with strides which can be used to access entries directly. This class
+ * inherits DenseCoeffsBase<Derived, ReadOnlyAccessors> which defines functions to access entries read-only using
+ * \c operator() .
+ *
+ * \sa \blank \ref TopicClassHierarchy
+ */
+template <typename Derived>
+class DenseCoeffsBase<Derived, DirectAccessors> : public DenseCoeffsBase<Derived, ReadOnlyAccessors> {
+ public:
+  typedef DenseCoeffsBase<Derived, ReadOnlyAccessors> Base;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    typedef DenseCoeffsBase<Derived, ReadOnlyAccessors> Base;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+  using Base::cols;
+  using Base::derived;
+  using Base::rows;
+  using Base::size;
 
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::derived;
+  /** \returns the pointer increment between two consecutive elements within a slice in the inner direction.
+   *
+   * \sa outerStride(), rowStride(), colStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { return derived().innerStride(); }
 
-    /** \returns the pointer increment between two consecutive elements within a slice in the inner direction.
-      *
-      * \sa outerStride(), rowStride(), colStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const
-    {
-      return derived().innerStride();
-    }
+  /** \returns the pointer increment between two consecutive inner slices (for example, between two consecutive columns
+   *          in a column-major matrix).
+   *
+   * \sa innerStride(), rowStride(), colStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { return derived().outerStride(); }
 
-    /** \returns the pointer increment between two consecutive inner slices (for example, between two consecutive columns
-      *          in a column-major matrix).
-      *
-      * \sa innerStride(), rowStride(), colStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const
-    {
-      return derived().outerStride();
-    }
+  // FIXME shall we remove it ?
+  EIGEN_CONSTEXPR inline Index stride() const { return Derived::IsVectorAtCompileTime ? innerStride() : outerStride(); }
 
-    // FIXME shall we remove it ?
-    EIGEN_CONSTEXPR inline Index stride() const
-    {
-      return Derived::IsVectorAtCompileTime ? innerStride() : outerStride();
-    }
+  /** \returns the pointer increment between two consecutive rows.
+   *
+   * \sa innerStride(), outerStride(), colStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rowStride() const {
+    return Derived::IsRowMajor ? outerStride() : innerStride();
+  }
 
-    /** \returns the pointer increment between two consecutive rows.
-      *
-      * \sa innerStride(), outerStride(), colStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rowStride() const
-    {
-      return Derived::IsRowMajor ? outerStride() : innerStride();
-    }
-
-    /** \returns the pointer increment between two consecutive columns.
-      *
-      * \sa innerStride(), outerStride(), rowStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index colStride() const
-    {
-      return Derived::IsRowMajor ? innerStride() : outerStride();
-    }
+  /** \returns the pointer increment between two consecutive columns.
+   *
+   * \sa innerStride(), outerStride(), rowStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index colStride() const {
+    return Derived::IsRowMajor ? innerStride() : outerStride();
+  }
 };
 
 /** \brief Base class providing direct read/write coefficient access to matrices and arrays.
-  * \ingroup Core_Module
-  * \tparam Derived Type of the derived class
-  *
-  * \note #DirectWriteAccessors Constant indicating direct access
-  *
-  * This class defines functions to work with strides which can be used to access entries directly. This class
-  * inherits DenseCoeffsBase<Derived, WriteAccessors> which defines functions to access entries read/write using
-  * \c operator().
-  *
-  * \sa \blank \ref TopicClassHierarchy
-  */
-template<typename Derived>
-class DenseCoeffsBase<Derived, DirectWriteAccessors>
-  : public DenseCoeffsBase<Derived, WriteAccessors>
-{
-  public:
+ * \ingroup Core_Module
+ * \tparam Derived Type of the derived class
+ *
+ * \note #DirectWriteAccessors Constant indicating direct access
+ *
+ * This class defines functions to work with strides which can be used to access entries directly. This class
+ * inherits DenseCoeffsBase<Derived, WriteAccessors> which defines functions to access entries read/write using
+ * \c operator().
+ *
+ * \sa \blank \ref TopicClassHierarchy
+ */
+template <typename Derived>
+class DenseCoeffsBase<Derived, DirectWriteAccessors> : public DenseCoeffsBase<Derived, WriteAccessors> {
+ public:
+  typedef DenseCoeffsBase<Derived, WriteAccessors> Base;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    typedef DenseCoeffsBase<Derived, WriteAccessors> Base;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+  using Base::cols;
+  using Base::derived;
+  using Base::rows;
+  using Base::size;
 
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::derived;
+  /** \returns the pointer increment between two consecutive elements within a slice in the inner direction.
+   *
+   * \sa outerStride(), rowStride(), colStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return derived().innerStride(); }
 
-    /** \returns the pointer increment between two consecutive elements within a slice in the inner direction.
-      *
-      * \sa outerStride(), rowStride(), colStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT
-    {
-      return derived().innerStride();
-    }
+  /** \returns the pointer increment between two consecutive inner slices (for example, between two consecutive columns
+   *          in a column-major matrix).
+   *
+   * \sa innerStride(), rowStride(), colStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return derived().outerStride(); }
 
-    /** \returns the pointer increment between two consecutive inner slices (for example, between two consecutive columns
-      *          in a column-major matrix).
-      *
-      * \sa innerStride(), rowStride(), colStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT
-    {
-      return derived().outerStride();
-    }
+  // FIXME shall we remove it ?
+  EIGEN_CONSTEXPR inline Index stride() const EIGEN_NOEXCEPT {
+    return Derived::IsVectorAtCompileTime ? innerStride() : outerStride();
+  }
 
-    // FIXME shall we remove it ?
-    EIGEN_CONSTEXPR inline Index stride() const EIGEN_NOEXCEPT
-    {
-      return Derived::IsVectorAtCompileTime ? innerStride() : outerStride();
-    }
+  /** \returns the pointer increment between two consecutive rows.
+   *
+   * \sa innerStride(), outerStride(), colStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rowStride() const EIGEN_NOEXCEPT {
+    return Derived::IsRowMajor ? outerStride() : innerStride();
+  }
 
-    /** \returns the pointer increment between two consecutive rows.
-      *
-      * \sa innerStride(), outerStride(), colStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rowStride() const EIGEN_NOEXCEPT
-    {
-      return Derived::IsRowMajor ? outerStride() : innerStride();
-    }
-
-    /** \returns the pointer increment between two consecutive columns.
-      *
-      * \sa innerStride(), outerStride(), rowStride()
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index colStride() const EIGEN_NOEXCEPT
-    {
-      return Derived::IsRowMajor ? innerStride() : outerStride();
-    }
+  /** \returns the pointer increment between two consecutive columns.
+   *
+   * \sa innerStride(), outerStride(), rowStride()
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index colStride() const EIGEN_NOEXCEPT {
+    return Derived::IsRowMajor ? innerStride() : outerStride();
+  }
 };
 
 namespace internal {
 
-template<int Alignment, typename Derived, bool JustReturnZero>
-struct first_aligned_impl
-{
-  static EIGEN_CONSTEXPR inline Index run(const Derived&) EIGEN_NOEXCEPT
-  { return 0; }
+template <int Alignment, typename Derived, bool JustReturnZero>
+struct first_aligned_impl {
+  static EIGEN_CONSTEXPR inline Index run(const Derived&) EIGEN_NOEXCEPT { return 0; }
 };
 
-template<int Alignment, typename Derived>
-struct first_aligned_impl<Alignment, Derived, false>
-{
-  static inline Index run(const Derived& m)
-  {
-    return internal::first_aligned<Alignment>(m.data(), m.size());
-  }
+template <int Alignment, typename Derived>
+struct first_aligned_impl<Alignment, Derived, false> {
+  static inline Index run(const Derived& m) { return internal::first_aligned<Alignment>(m.data(), m.size()); }
 };
 
-/** \internal \returns the index of the first element of the array stored by \a m that is properly aligned with respect to \a Alignment for vectorization.
-  *
-  * \tparam Alignment requested alignment in Bytes.
-  *
-  * There is also the variant first_aligned(const Scalar*, Integer) defined in Memory.h. See it for more
-  * documentation.
-  */
-template<int Alignment, typename Derived>
-static inline Index first_aligned(const DenseBase<Derived>& m)
-{
+/** \internal \returns the index of the first element of the array stored by \a m that is properly aligned with respect
+ * to \a Alignment for vectorization.
+ *
+ * \tparam Alignment requested alignment in Bytes.
+ *
+ * There is also the variant first_aligned(const Scalar*, Integer) defined in Memory.h. See it for more
+ * documentation.
+ */
+template <int Alignment, typename Derived>
+static inline Index first_aligned(const DenseBase<Derived>& m) {
   enum { ReturnZero = (int(evaluator<Derived>::Alignment) >= Alignment) || !(Derived::Flags & DirectAccessBit) };
   return first_aligned_impl<Alignment, Derived, ReturnZero>::run(m.derived());
 }
 
-template<typename Derived>
-static inline Index first_default_aligned(const DenseBase<Derived>& m)
-{
+template <typename Derived>
+static inline Index first_default_aligned(const DenseBase<Derived>& m) {
   typedef typename Derived::Scalar Scalar;
   typedef typename packet_traits<Scalar>::type DefaultPacketType;
-  return internal::first_aligned<int(unpacket_traits<DefaultPacketType>::alignment),Derived>(m);
+  return internal::first_aligned<int(unpacket_traits<DefaultPacketType>::alignment), Derived>(m);
 }
 
-template<typename Derived, bool HasDirectAccess = has_direct_access<Derived>::ret>
-struct inner_stride_at_compile_time
-{
+template <typename Derived, bool HasDirectAccess = has_direct_access<Derived>::ret>
+struct inner_stride_at_compile_time {
   enum { ret = traits<Derived>::InnerStrideAtCompileTime };
 };
 
-template<typename Derived>
-struct inner_stride_at_compile_time<Derived, false>
-{
+template <typename Derived>
+struct inner_stride_at_compile_time<Derived, false> {
   enum { ret = 0 };
 };
 
-template<typename Derived, bool HasDirectAccess = has_direct_access<Derived>::ret>
-struct outer_stride_at_compile_time
-{
+template <typename Derived, bool HasDirectAccess = has_direct_access<Derived>::ret>
+struct outer_stride_at_compile_time {
   enum { ret = traits<Derived>::OuterStrideAtCompileTime };
 };
 
-template<typename Derived>
-struct outer_stride_at_compile_time<Derived, false>
-{
+template <typename Derived>
+struct outer_stride_at_compile_time<Derived, false> {
   enum { ret = 0 };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DENSECOEFFSBASE_H
+#endif  // EIGEN_DENSECOEFFSBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h
index 08ef6c5..f616939 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h
@@ -13,168 +13,132 @@
 #define EIGEN_MATRIXSTORAGE_H
 
 #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
-  #define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X) X; EIGEN_DENSE_STORAGE_CTOR_PLUGIN;
+#define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X) \
+  X;                                                \
+  EIGEN_DENSE_STORAGE_CTOR_PLUGIN;
 #else
-  #define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X)
+#define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X)
 #endif
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 struct constructor_without_unaligned_array_assert {};
 
-template<typename T, int Size>
-EIGEN_DEVICE_FUNC
-void check_static_allocation_size()
-{
-  // if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit
-  #if EIGEN_STACK_ALLOCATION_LIMIT
+template <typename T, int Size>
+EIGEN_DEVICE_FUNC constexpr void check_static_allocation_size() {
+// if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit
+#if EIGEN_STACK_ALLOCATION_LIMIT
   EIGEN_STATIC_ASSERT(Size * sizeof(T) <= EIGEN_STACK_ALLOCATION_LIMIT, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
-  #endif
+#endif
 }
 
 /** \internal
-  * Static array. If the MatrixOrArrayOptions require auto-alignment, the array will be automatically aligned:
-  * to 16 bytes boundary if the total size is a multiple of 16 bytes.
-  */
+ * Static array. If the MatrixOrArrayOptions require auto-alignment, the array will be automatically aligned:
+ * to 16 bytes boundary if the total size is a multiple of 16 bytes.
+ */
 template <typename T, int Size, int MatrixOrArrayOptions,
-          int Alignment = (MatrixOrArrayOptions&DontAlign) ? 0
-                        : compute_default_alignment<T,Size>::value >
-struct plain_array
-{
+          int Alignment = (MatrixOrArrayOptions & DontAlign) ? 0 : compute_default_alignment<T, Size>::value>
+struct plain_array {
   T array[Size];
 
-  EIGEN_DEVICE_FUNC
-  plain_array()
-  {
-    check_static_allocation_size<T,Size>();
-  }
+  EIGEN_DEVICE_FUNC constexpr plain_array() { check_static_allocation_size<T, Size>(); }
 
-  EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert)
-  {
-    check_static_allocation_size<T,Size>();
+  EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) {
+    check_static_allocation_size<T, Size>();
   }
 };
 
 #if defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
-  #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask)
-#elif EIGEN_GNUC_AT_LEAST(4,7)
-  // GCC 4.7 is too aggressive in its optimizations and remove the alignment test based on the fact the array is declared to be aligned.
-  // See this bug report: http://gcc.gnu.org/bugzilla/show_bug.cgi?id=53900
-  // Hiding the origin of the array pointer behind a function argument seems to do the trick even if the function is inlined:
-  template<typename PtrType>
-  EIGEN_ALWAYS_INLINE PtrType eigen_unaligned_array_assert_workaround_gcc47(PtrType array) { return array; }
-  #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \
-    eigen_assert((internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (sizemask)) == 0 \
-              && "this assertion is explained here: " \
-              "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \
-              " **** READ THIS WEB PAGE !!! ****");
+#define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask)
 #else
-  #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \
-    eigen_assert((internal::UIntPtr(array) & (sizemask)) == 0 \
-              && "this assertion is explained here: " \
-              "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \
-              " **** READ THIS WEB PAGE !!! ****");
+#define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask)                                                \
+  eigen_assert((internal::is_constant_evaluated() || (std::uintptr_t(array) & (sizemask)) == 0) && \
+               "this assertion is explained here: "                                                \
+               "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html"        \
+               " **** READ THIS WEB PAGE !!! ****");
 #endif
 
 template <typename T, int Size, int MatrixOrArrayOptions>
-struct plain_array<T, Size, MatrixOrArrayOptions, 8>
-{
+struct plain_array<T, Size, MatrixOrArrayOptions, 8> {
   EIGEN_ALIGN_TO_BOUNDARY(8) T array[Size];
 
-  EIGEN_DEVICE_FUNC
-  plain_array()
-  {
+  EIGEN_DEVICE_FUNC constexpr plain_array() {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(7);
-    check_static_allocation_size<T,Size>();
+    check_static_allocation_size<T, Size>();
   }
 
-  EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert)
-  {
-    check_static_allocation_size<T,Size>();
+  EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) {
+    check_static_allocation_size<T, Size>();
   }
 };
 
 template <typename T, int Size, int MatrixOrArrayOptions>
-struct plain_array<T, Size, MatrixOrArrayOptions, 16>
-{
+struct plain_array<T, Size, MatrixOrArrayOptions, 16> {
   EIGEN_ALIGN_TO_BOUNDARY(16) T array[Size];
 
-  EIGEN_DEVICE_FUNC
-  plain_array()
-  {
+  EIGEN_DEVICE_FUNC constexpr plain_array() {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(15);
-    check_static_allocation_size<T,Size>();
+    check_static_allocation_size<T, Size>();
   }
 
-  EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert)
-  {
-    check_static_allocation_size<T,Size>();
+  EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) {
+    check_static_allocation_size<T, Size>();
   }
 };
 
 template <typename T, int Size, int MatrixOrArrayOptions>
-struct plain_array<T, Size, MatrixOrArrayOptions, 32>
-{
+struct plain_array<T, Size, MatrixOrArrayOptions, 32> {
   EIGEN_ALIGN_TO_BOUNDARY(32) T array[Size];
 
-  EIGEN_DEVICE_FUNC
-  plain_array()
-  {
+  EIGEN_DEVICE_FUNC constexpr plain_array() {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(31);
-    check_static_allocation_size<T,Size>();
+    check_static_allocation_size<T, Size>();
   }
 
-  EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert)
-  {
-    check_static_allocation_size<T,Size>();
+  EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) {
+    check_static_allocation_size<T, Size>();
   }
 };
 
 template <typename T, int Size, int MatrixOrArrayOptions>
-struct plain_array<T, Size, MatrixOrArrayOptions, 64>
-{
+struct plain_array<T, Size, MatrixOrArrayOptions, 64> {
   EIGEN_ALIGN_TO_BOUNDARY(64) T array[Size];
 
-  EIGEN_DEVICE_FUNC
-  plain_array()
-  {
+  EIGEN_DEVICE_FUNC constexpr plain_array() {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(63);
-    check_static_allocation_size<T,Size>();
+    check_static_allocation_size<T, Size>();
   }
 
-  EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert)
-  {
-    check_static_allocation_size<T,Size>();
+  EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) {
+    check_static_allocation_size<T, Size>();
   }
 };
 
 template <typename T, int MatrixOrArrayOptions, int Alignment>
-struct plain_array<T, 0, MatrixOrArrayOptions, Alignment>
-{
+struct plain_array<T, 0, MatrixOrArrayOptions, Alignment> {
   T array[1];
-  EIGEN_DEVICE_FUNC plain_array() {}
-  EIGEN_DEVICE_FUNC plain_array(constructor_without_unaligned_array_assert) {}
+  EIGEN_DEVICE_FUNC constexpr plain_array() {}
+  EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) {}
 };
 
 struct plain_array_helper {
-  template<typename T, int Size, int MatrixOrArrayOptions, int Alignment>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  static void copy(const plain_array<T, Size, MatrixOrArrayOptions, Alignment>& src, const Eigen::Index size,
-                         plain_array<T, Size, MatrixOrArrayOptions, Alignment>& dst) {
+  template <typename T, int Size, int MatrixOrArrayOptions, int Alignment>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static void copy(
+      const plain_array<T, Size, MatrixOrArrayOptions, Alignment>& src, const Eigen::Index size,
+      plain_array<T, Size, MatrixOrArrayOptions, Alignment>& dst) {
     smart_copy(src.array, src.array + size, dst.array);
   }
-  
-  template<typename T, int Size, int MatrixOrArrayOptions, int Alignment>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  static void swap(plain_array<T, Size, MatrixOrArrayOptions, Alignment>& a, const Eigen::Index a_size,
-                   plain_array<T, Size, MatrixOrArrayOptions, Alignment>& b, const Eigen::Index b_size) {
+
+  template <typename T, int Size, int MatrixOrArrayOptions, int Alignment>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static void swap(plain_array<T, Size, MatrixOrArrayOptions, Alignment>& a,
+                                                         const Eigen::Index a_size,
+                                                         plain_array<T, Size, MatrixOrArrayOptions, Alignment>& b,
+                                                         const Eigen::Index b_size) {
     if (a_size < b_size) {
       std::swap_ranges(b.array, b.array + a_size, a.array);
       smart_move(b.array + a_size, b.array + b_size, a.array + a_size);
@@ -187,466 +151,500 @@
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \internal
-  *
-  * \class DenseStorage
-  * \ingroup Core_Module
-  *
-  * \brief Stores the data of a matrix
-  *
-  * This class stores the data of fixed-size, dynamic-size or mixed matrices
-  * in a way as compact as possible.
-  *
-  * \sa Matrix
-  */
-template<typename T, int Size, int _Rows, int _Cols, int _Options> class DenseStorage;
+ *
+ * \class DenseStorage
+ * \ingroup Core_Module
+ *
+ * \brief Stores the data of a matrix
+ *
+ * This class stores the data of fixed-size, dynamic-size or mixed matrices
+ * in a way as compact as possible.
+ *
+ * \sa Matrix
+ */
+template <typename T, int Size, int Rows_, int Cols_, int Options_>
+class DenseStorage;
 
 // purely fixed-size matrix
-template<typename T, int Size, int _Rows, int _Cols, int _Options> class DenseStorage
-{
-    internal::plain_array<T,Size,_Options> m_data;
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() {
-      EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size)
-    }
-    EIGEN_DEVICE_FUNC
-    explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
+template <typename T, int Size, int Rows_, int Cols_, int Options_>
+class DenseStorage {
+  internal::plain_array<T, Size, Options_> m_data;
+
+ public:
+  constexpr EIGEN_DEVICE_FUNC DenseStorage(){EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(
+      Index size =
+          Size)} EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()) {}
-#if !EIGEN_HAS_CXX11 || defined(EIGEN_DENSE_STORAGE_CTOR_PLUGIN)
-    EIGEN_DEVICE_FUNC
-    DenseStorage(const DenseStorage& other) : m_data(other.m_data) {
-      EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size)
-    }
+#if defined(EIGEN_DENSE_STORAGE_CTOR_PLUGIN)
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other)
+      : m_data(other.m_data){EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size)}
 #else
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage&) = default;
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage&) = default;
 #endif
-#if !EIGEN_HAS_CXX11
-    EIGEN_DEVICE_FUNC
-    DenseStorage& operator=(const DenseStorage& other)
-    {
-      if (this != &other) m_data = other.m_data;
-      return *this;
-    }
-#else
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage&) = default;
-#endif
-#if EIGEN_HAS_RVALUE_REFERENCES
-#if !EIGEN_HAS_CXX11
-    EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT
-      : m_data(std::move(other.m_data))
-    {
-    }
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
-    {
-      if (this != &other)
-        m_data = std::move(other.m_data);
-      return *this;
-    }
-#else
-    EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&&) = default;
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&&) = default;
-#endif
-#endif
-    EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) {
-      EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
-      eigen_internal_assert(size==rows*cols && rows==_Rows && cols==_Cols);
-      EIGEN_UNUSED_VARIABLE(size);
-      EIGEN_UNUSED_VARIABLE(rows);
-      EIGEN_UNUSED_VARIABLE(cols);
-    }
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
-      numext::swap(m_data, other.m_data);
-    }
-    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT {return _Rows;}
-    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) EIGEN_NOEXCEPT {return _Cols;}
-    EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {}
-    EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {}
-    EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; }
-    EIGEN_DEVICE_FUNC T *data() { return m_data.array; }
+        EIGEN_DEVICE_FUNC constexpr DenseStorage
+        &
+        operator=(const DenseStorage&) = default;
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(DenseStorage&&) = default;
+  EIGEN_DEVICE_FUNC constexpr DenseStorage& operator=(DenseStorage&&) = default;
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(Index size, Index rows, Index cols) {
+    EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
+    eigen_internal_assert(size == rows * cols && rows == Rows_ && cols == Cols_);
+    EIGEN_UNUSED_VARIABLE(size);
+    EIGEN_UNUSED_VARIABLE(rows);
+    EIGEN_UNUSED_VARIABLE(cols);
+  }
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { numext::swap(m_data, other.m_data); }
+  EIGEN_DEVICE_FUNC static constexpr Index rows(void) EIGEN_NOEXCEPT { return Rows_; }
+  EIGEN_DEVICE_FUNC static constexpr Index cols(void) EIGEN_NOEXCEPT { return Cols_; }
+  EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index, Index) {}
+  EIGEN_DEVICE_FUNC constexpr void resize(Index, Index, Index) {}
+  EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; }
+  EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; }
 };
 
 // null matrix
-template<typename T, int _Rows, int _Cols, int _Options> class DenseStorage<T, 0, _Rows, _Cols, _Options>
-{
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() {}
-    EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) {}
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage&) {}
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage&) { return *this; }
-    EIGEN_DEVICE_FUNC DenseStorage(Index,Index,Index) {}
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& ) {}
-    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT {return _Rows;}
-    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) EIGEN_NOEXCEPT {return _Cols;}
-    EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {}
-    EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {}
-    EIGEN_DEVICE_FUNC const T *data() const { return 0; }
-    EIGEN_DEVICE_FUNC T *data() { return 0; }
+template <typename T, int Rows_, int Cols_, int Options_>
+class DenseStorage<T, 0, Rows_, Cols_, Options_> {
+ public:
+  static_assert(Rows_ * Cols_ == 0, "The fixed number of rows times columns must equal the storage size.");
+  EIGEN_DEVICE_FUNC constexpr DenseStorage() {}
+  EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) {}
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage&) {}
+  EIGEN_DEVICE_FUNC constexpr DenseStorage& operator=(const DenseStorage&) { return *this; }
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(Index, Index, Index) {}
+  EIGEN_DEVICE_FUNC constexpr void swap(DenseStorage&) {}
+  EIGEN_DEVICE_FUNC static constexpr Index rows(void) EIGEN_NOEXCEPT { return Rows_; }
+  EIGEN_DEVICE_FUNC static constexpr Index cols(void) EIGEN_NOEXCEPT { return Cols_; }
+  EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index, Index) {}
+  EIGEN_DEVICE_FUNC constexpr void resize(Index, Index, Index) {}
+  EIGEN_DEVICE_FUNC constexpr const T* data() const { return 0; }
+  EIGEN_DEVICE_FUNC constexpr T* data() { return 0; }
 };
 
 // more specializations for null matrices; these are necessary to resolve ambiguities
-template<typename T, int _Options> class DenseStorage<T, 0, Dynamic, Dynamic, _Options>
-: public DenseStorage<T, 0, 0, 0, _Options> { };
+template <typename T, int Options_>
+class DenseStorage<T, 0, Dynamic, Dynamic, Options_> {
+  Index m_rows;
+  Index m_cols;
 
-template<typename T, int _Rows, int _Options> class DenseStorage<T, 0, _Rows, Dynamic, _Options>
-: public DenseStorage<T, 0, 0, 0, _Options> { };
+ public:
+  EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0), m_cols(0) {}
+  EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : DenseStorage() {}
+  EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_rows(other.m_rows), m_cols(other.m_cols) {}
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    m_rows = other.m_rows;
+    m_cols = other.m_cols;
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index cols) : m_rows(rows), m_cols(cols) {
+    eigen_assert(m_rows * m_cols == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+    numext::swap(m_rows, other.m_rows);
+    numext::swap(m_cols, other.m_cols);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_rows; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_cols; }
+  EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index cols) {
+    m_rows = rows;
+    m_cols = cols;
+    eigen_assert(m_rows * m_cols == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index cols) {
+    m_rows = rows;
+    m_cols = cols;
+    eigen_assert(m_rows * m_cols == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC const T* data() const { return nullptr; }
+  EIGEN_DEVICE_FUNC T* data() { return nullptr; }
+};
 
-template<typename T, int _Cols, int _Options> class DenseStorage<T, 0, Dynamic, _Cols, _Options>
-: public DenseStorage<T, 0, 0, 0, _Options> { };
+template <typename T, int Rows_, int Options_>
+class DenseStorage<T, 0, Rows_, Dynamic, Options_> {
+  Index m_cols;
+
+ public:
+  EIGEN_DEVICE_FUNC DenseStorage() : m_cols(0) {}
+  EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : DenseStorage() {}
+  EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_cols(other.m_cols) {}
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    m_cols = other.m_cols;
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(Index, Index, Index cols) : m_cols(cols) {
+    eigen_assert(Rows_ * m_cols == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { numext::swap(m_cols, other.m_cols); }
+  EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT { return Rows_; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols(void) const EIGEN_NOEXCEPT { return m_cols; }
+  EIGEN_DEVICE_FUNC void conservativeResize(Index, Index, Index cols) {
+    m_cols = cols;
+    eigen_assert(Rows_ * m_cols == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC void resize(Index, Index, Index cols) {
+    m_cols = cols;
+    eigen_assert(Rows_ * m_cols == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC const T* data() const { return nullptr; }
+  EIGEN_DEVICE_FUNC T* data() { return nullptr; }
+};
+
+template <typename T, int Cols_, int Options_>
+class DenseStorage<T, 0, Dynamic, Cols_, Options_> {
+  Index m_rows;
+
+ public:
+  EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0) {}
+  EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : DenseStorage() {}
+  EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_rows(other.m_rows) {}
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    m_rows = other.m_rows;
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index) : m_rows(rows) {
+    eigen_assert(m_rows * Cols_ == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { numext::swap(m_rows, other.m_rows); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows(void) const EIGEN_NOEXCEPT { return m_rows; }
+  EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) EIGEN_NOEXCEPT { return Cols_; }
+  EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index) {
+    m_rows = rows;
+    eigen_assert(m_rows * Cols_ == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index) {
+    m_rows = rows;
+    eigen_assert(m_rows * Cols_ == 0 && "The number of rows times columns must equal the storage size.");
+  }
+  EIGEN_DEVICE_FUNC const T* data() const { return nullptr; }
+  EIGEN_DEVICE_FUNC T* data() { return nullptr; }
+};
 
 // dynamic-size matrix with fixed-size storage
-template<typename T, int Size, int _Options> class DenseStorage<T, Size, Dynamic, Dynamic, _Options>
-{
-    internal::plain_array<T,Size,_Options> m_data;
-    Index m_rows;
-    Index m_cols;
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0), m_cols(0) {}
-    EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
+template <typename T, int Size, int Options_>
+class DenseStorage<T, Size, Dynamic, Dynamic, Options_> {
+  internal::plain_array<T, Size, Options_> m_data;
+  Index m_rows;
+  Index m_cols;
+
+ public:
+  EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(), m_rows(0), m_cols(0) {}
+  EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
-      : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows), m_cols(other.m_cols)
-    {
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other)
+      : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows), m_cols(other.m_cols) {
+    internal::plain_array_helper::copy(other.m_data, m_rows * m_cols, m_data);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    if (this != &other) {
+      m_rows = other.m_rows;
+      m_cols = other.m_cols;
       internal::plain_array_helper::copy(other.m_data, m_rows * m_cols, m_data);
     }
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
-    {
-      if (this != &other)
-      {
-        m_rows = other.m_rows;
-        m_cols = other.m_cols;
-        internal::plain_array_helper::copy(other.m_data, m_rows * m_cols, m_data);
-      }
-      return *this;
-    }
-    EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index cols) : m_rows(rows), m_cols(cols) {}
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other)
-    {
-      internal::plain_array_helper::swap(m_data, m_rows * m_cols, other.m_data, other.m_rows * other.m_cols);
-      numext::swap(m_rows,other.m_rows);
-      numext::swap(m_cols,other.m_cols);
-    }
-    EIGEN_DEVICE_FUNC Index rows() const {return m_rows;}
-    EIGEN_DEVICE_FUNC Index cols() const {return m_cols;}
-    EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index cols) { m_rows = rows; m_cols = cols; }
-    EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index cols) { m_rows = rows; m_cols = cols; }
-    EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; }
-    EIGEN_DEVICE_FUNC T *data() { return m_data.array; }
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(Index, Index rows, Index cols) : m_rows(rows), m_cols(cols) {}
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+    internal::plain_array_helper::swap(m_data, m_rows * m_cols, other.m_data, other.m_rows * other.m_cols);
+    numext::swap(m_rows, other.m_rows);
+    numext::swap(m_cols, other.m_cols);
+  }
+  EIGEN_DEVICE_FUNC constexpr Index rows() const { return m_rows; }
+  EIGEN_DEVICE_FUNC constexpr Index cols() const { return m_cols; }
+  EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index rows, Index cols) {
+    m_rows = rows;
+    m_cols = cols;
+  }
+  EIGEN_DEVICE_FUNC constexpr void resize(Index, Index rows, Index cols) {
+    m_rows = rows;
+    m_cols = cols;
+  }
+  EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; }
+  EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; }
 };
 
 // dynamic-size matrix with fixed-size storage and fixed width
-template<typename T, int Size, int _Cols, int _Options> class DenseStorage<T, Size, Dynamic, _Cols, _Options>
-{
-    internal::plain_array<T,Size,_Options> m_data;
-    Index m_rows;
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0) {}
-    EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
+template <typename T, int Size, int Cols_, int Options_>
+class DenseStorage<T, Size, Dynamic, Cols_, Options_> {
+  internal::plain_array<T, Size, Options_> m_data;
+  Index m_rows;
+
+ public:
+  EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_rows(0) {}
+  EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
-      : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows)
-    {
-      internal::plain_array_helper::copy(other.m_data, m_rows * _Cols, m_data);
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other)
+      : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows) {
+    internal::plain_array_helper::copy(other.m_data, m_rows * Cols_, m_data);
+  }
+
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    if (this != &other) {
+      m_rows = other.m_rows;
+      internal::plain_array_helper::copy(other.m_data, m_rows * Cols_, m_data);
     }
-    
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
-    {
-      if (this != &other)
-      {
-        m_rows = other.m_rows;
-        internal::plain_array_helper::copy(other.m_data, m_rows * _Cols, m_data);
-      }
-      return *this;
-    }
-    EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index) : m_rows(rows) {}
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other)
-    { 
-      internal::plain_array_helper::swap(m_data, m_rows * _Cols, other.m_data, other.m_rows * _Cols);
-      numext::swap(m_rows, other.m_rows);
-    }
-    EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT {return m_rows;}
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols(void) const EIGEN_NOEXCEPT {return _Cols;}
-    EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index) { m_rows = rows; }
-    EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index) { m_rows = rows; }
-    EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; }
-    EIGEN_DEVICE_FUNC T *data() { return m_data.array; }
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(Index, Index rows, Index) : m_rows(rows) {}
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+    internal::plain_array_helper::swap(m_data, m_rows * Cols_, other.m_data, other.m_rows * Cols_);
+    numext::swap(m_rows, other.m_rows);
+  }
+  EIGEN_DEVICE_FUNC constexpr Index rows(void) const EIGEN_NOEXCEPT { return m_rows; }
+  EIGEN_DEVICE_FUNC constexpr Index cols(void) const EIGEN_NOEXCEPT { return Cols_; }
+  EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index rows, Index) { m_rows = rows; }
+  EIGEN_DEVICE_FUNC constexpr void resize(Index, Index rows, Index) { m_rows = rows; }
+  EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; }
+  EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; }
 };
 
 // dynamic-size matrix with fixed-size storage and fixed height
-template<typename T, int Size, int _Rows, int _Options> class DenseStorage<T, Size, _Rows, Dynamic, _Options>
-{
-    internal::plain_array<T,Size,_Options> m_data;
-    Index m_cols;
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() : m_cols(0) {}
-    EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
+template <typename T, int Size, int Rows_, int Options_>
+class DenseStorage<T, Size, Rows_, Dynamic, Options_> {
+  internal::plain_array<T, Size, Options_> m_data;
+  Index m_cols;
+
+ public:
+  EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_cols(0) {}
+  EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) 
-      : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(other.m_cols)
-    {
-      internal::plain_array_helper::copy(other.m_data, _Rows * m_cols, m_data);
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other)
+      : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(other.m_cols) {
+    internal::plain_array_helper::copy(other.m_data, Rows_ * m_cols, m_data);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    if (this != &other) {
+      m_cols = other.m_cols;
+      internal::plain_array_helper::copy(other.m_data, Rows_ * m_cols, m_data);
     }
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
-    {
-      if (this != &other)
-      {
-        m_cols = other.m_cols;
-        internal::plain_array_helper::copy(other.m_data, _Rows * m_cols, m_data);
-      }
-      return *this;
-    }
-    EIGEN_DEVICE_FUNC DenseStorage(Index, Index, Index cols) : m_cols(cols) {}
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
-      internal::plain_array_helper::swap(m_data, _Rows * m_cols, other.m_data, _Rows * other.m_cols);
-      numext::swap(m_cols, other.m_cols);
-    }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows(void) const EIGEN_NOEXCEPT {return _Rows;}
-    EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT {return m_cols;}
-    EIGEN_DEVICE_FUNC void conservativeResize(Index, Index, Index cols) { m_cols = cols; }
-    EIGEN_DEVICE_FUNC void resize(Index, Index, Index cols) { m_cols = cols; }
-    EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; }
-    EIGEN_DEVICE_FUNC T *data() { return m_data.array; }
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(Index, Index, Index cols) : m_cols(cols) {}
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+    internal::plain_array_helper::swap(m_data, Rows_ * m_cols, other.m_data, Rows_ * other.m_cols);
+    numext::swap(m_cols, other.m_cols);
+  }
+  EIGEN_DEVICE_FUNC constexpr Index rows(void) const EIGEN_NOEXCEPT { return Rows_; }
+  EIGEN_DEVICE_FUNC constexpr Index cols(void) const EIGEN_NOEXCEPT { return m_cols; }
+  EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index, Index cols) { m_cols = cols; }
+  EIGEN_DEVICE_FUNC constexpr void resize(Index, Index, Index cols) { m_cols = cols; }
+  EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; }
+  EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; }
 };
 
 // purely dynamic matrix.
-template<typename T, int _Options> class DenseStorage<T, Dynamic, Dynamic, Dynamic, _Options>
-{
-    T *m_data;
-    Index m_rows;
-    Index m_cols;
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() : m_data(0), m_rows(0), m_cols(0) {}
-    EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
-       : m_data(0), m_rows(0), m_cols(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols)
-      : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_rows(rows), m_cols(cols)
-    {
+template <typename T, int Options_>
+class DenseStorage<T, Dynamic, Dynamic, Dynamic, Options_> {
+  T* m_data;
+  Index m_rows;
+  Index m_cols;
+
+ public:
+  EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(0), m_rows(0), m_cols(0) {}
+  EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert)
+      : m_data(0), m_rows(0), m_cols(0) {}
+  EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols)
+      : m_data(internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(size)),
+        m_rows(rows),
+        m_cols(cols) {
+    EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
+    eigen_internal_assert(size == rows * cols && rows >= 0 && cols >= 0);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
+      : m_data(internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(other.m_rows * other.m_cols)),
+        m_rows(other.m_rows),
+        m_cols(other.m_cols) {
+    EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows * m_cols)
+    internal::smart_copy(other.m_data, other.m_data + other.m_rows * other.m_cols, m_data);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    if (this != &other) {
+      DenseStorage tmp(other);
+      this->swap(tmp);
+    }
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)),
+                                                                        m_rows(std::move(other.m_rows)),
+                                                                        m_cols(std::move(other.m_cols)) {
+    other.m_data = nullptr;
+    other.m_rows = 0;
+    other.m_cols = 0;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT {
+    numext::swap(m_data, other.m_data);
+    numext::swap(m_rows, other.m_rows);
+    numext::swap(m_cols, other.m_cols);
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC ~DenseStorage() {
+    internal::conditional_aligned_delete_auto<T, (Options_ & DontAlign) == 0>(m_data, m_rows * m_cols);
+  }
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+    numext::swap(m_data, other.m_data);
+    numext::swap(m_rows, other.m_rows);
+    numext::swap(m_cols, other.m_cols);
+  }
+  EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT { return m_rows; }
+  EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT { return m_cols; }
+  void conservativeResize(Index size, Index rows, Index cols) {
+    m_data =
+        internal::conditional_aligned_realloc_new_auto<T, (Options_ & DontAlign) == 0>(m_data, size, m_rows * m_cols);
+    m_rows = rows;
+    m_cols = cols;
+  }
+  EIGEN_DEVICE_FUNC void resize(Index size, Index rows, Index cols) {
+    if (size != m_rows * m_cols) {
+      internal::conditional_aligned_delete_auto<T, (Options_ & DontAlign) == 0>(m_data, m_rows * m_cols);
+      if (size > 0)  // >0 and not simply !=0 to let the compiler knows that size cannot be negative
+        m_data = internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(size);
+      else
+        m_data = 0;
       EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
-      eigen_internal_assert(size==rows*cols && rows>=0 && cols >=0);
     }
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
-      : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(other.m_rows*other.m_cols))
-      , m_rows(other.m_rows)
-      , m_cols(other.m_cols)
-    {
-      EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows*m_cols)
-      internal::smart_copy(other.m_data, other.m_data+other.m_rows*other.m_cols, m_data);
-    }
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
-    {
-      if (this != &other)
-      {
-        DenseStorage tmp(other);
-        this->swap(tmp);
-      }
-      return *this;
-    }
-#if EIGEN_HAS_RVALUE_REFERENCES
-    EIGEN_DEVICE_FUNC
-    DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT
-      : m_data(std::move(other.m_data))
-      , m_rows(std::move(other.m_rows))
-      , m_cols(std::move(other.m_cols))
-    {
-      other.m_data = nullptr;
-      other.m_rows = 0;
-      other.m_cols = 0;
-    }
-    EIGEN_DEVICE_FUNC
-    DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
-    {
-      numext::swap(m_data, other.m_data);
-      numext::swap(m_rows, other.m_rows);
-      numext::swap(m_cols, other.m_cols);
-      return *this;
-    }
-#endif
-    EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, m_rows*m_cols); }
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other)
-    {
-      numext::swap(m_data,other.m_data);
-      numext::swap(m_rows,other.m_rows);
-      numext::swap(m_cols,other.m_cols);
-    }
-    EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT {return m_rows;}
-    EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT {return m_cols;}
-    void conservativeResize(Index size, Index rows, Index cols)
-    {
-      m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, m_rows*m_cols);
-      m_rows = rows;
-      m_cols = cols;
-    }
-    EIGEN_DEVICE_FUNC void resize(Index size, Index rows, Index cols)
-    {
-      if(size != m_rows*m_cols)
-      {
-        internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, m_rows*m_cols);
-        if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative
-          m_data = internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size);
-        else
-          m_data = 0;
-        EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
-      }
-      m_rows = rows;
-      m_cols = cols;
-    }
-    EIGEN_DEVICE_FUNC const T *data() const { return m_data; }
-    EIGEN_DEVICE_FUNC T *data() { return m_data; }
+    m_rows = rows;
+    m_cols = cols;
+  }
+  EIGEN_DEVICE_FUNC const T* data() const { return m_data; }
+  EIGEN_DEVICE_FUNC T* data() { return m_data; }
 };
 
 // matrix with dynamic width and fixed height (so that matrix has dynamic size).
-template<typename T, int _Rows, int _Options> class DenseStorage<T, Dynamic, _Rows, Dynamic, _Options>
-{
-    T *m_data;
-    Index m_cols;
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() : m_data(0), m_cols(0) {}
-    explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_cols(cols)
-    {
+template <typename T, int Rows_, int Options_>
+class DenseStorage<T, Dynamic, Rows_, Dynamic, Options_> {
+  T* m_data;
+  Index m_cols;
+
+ public:
+  EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(0), m_cols(0) {}
+  explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {}
+  EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols)
+      : m_data(internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(size)), m_cols(cols) {
+    EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
+    eigen_internal_assert(size == rows * cols && rows == Rows_ && cols >= 0);
+    EIGEN_UNUSED_VARIABLE(rows);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
+      : m_data(internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(Rows_ * other.m_cols)),
+        m_cols(other.m_cols) {
+    EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_cols * Rows_)
+    internal::smart_copy(other.m_data, other.m_data + Rows_ * m_cols, m_data);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    if (this != &other) {
+      DenseStorage tmp(other);
+      this->swap(tmp);
+    }
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)),
+                                                                        m_cols(std::move(other.m_cols)) {
+    other.m_data = nullptr;
+    other.m_cols = 0;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT {
+    numext::swap(m_data, other.m_data);
+    numext::swap(m_cols, other.m_cols);
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC ~DenseStorage() {
+    internal::conditional_aligned_delete_auto<T, (Options_ & DontAlign) == 0>(m_data, Rows_ * m_cols);
+  }
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+    numext::swap(m_data, other.m_data);
+    numext::swap(m_cols, other.m_cols);
+  }
+  EIGEN_DEVICE_FUNC static constexpr Index rows(void) EIGEN_NOEXCEPT { return Rows_; }
+  EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT { return m_cols; }
+  EIGEN_DEVICE_FUNC void conservativeResize(Index size, Index, Index cols) {
+    m_data =
+        internal::conditional_aligned_realloc_new_auto<T, (Options_ & DontAlign) == 0>(m_data, size, Rows_ * m_cols);
+    m_cols = cols;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index, Index cols) {
+    if (size != Rows_ * m_cols) {
+      internal::conditional_aligned_delete_auto<T, (Options_ & DontAlign) == 0>(m_data, Rows_ * m_cols);
+      if (size > 0)  // >0 and not simply !=0 to let the compiler knows that size cannot be negative
+        m_data = internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(size);
+      else
+        m_data = 0;
       EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
-      eigen_internal_assert(size==rows*cols && rows==_Rows && cols >=0);
-      EIGEN_UNUSED_VARIABLE(rows);
     }
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
-      : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(_Rows*other.m_cols))
-      , m_cols(other.m_cols)
-    {
-      EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_cols*_Rows)
-      internal::smart_copy(other.m_data, other.m_data+_Rows*m_cols, m_data);
-    }
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
-    {
-      if (this != &other)
-      {
-        DenseStorage tmp(other);
-        this->swap(tmp);
-      }
-      return *this;
-    }
-#if EIGEN_HAS_RVALUE_REFERENCES
-    EIGEN_DEVICE_FUNC
-    DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT
-      : m_data(std::move(other.m_data))
-      , m_cols(std::move(other.m_cols))
-    {
-      other.m_data = nullptr;
-      other.m_cols = 0;
-    }
-    EIGEN_DEVICE_FUNC
-    DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
-    {
-      numext::swap(m_data, other.m_data);
-      numext::swap(m_cols, other.m_cols);
-      return *this;
-    }
-#endif
-    EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Rows*m_cols); }
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
-      numext::swap(m_data,other.m_data);
-      numext::swap(m_cols,other.m_cols);
-    }
-    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT {return _Rows;}
-    EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT {return m_cols;}
-    EIGEN_DEVICE_FUNC void conservativeResize(Index size, Index, Index cols)
-    {
-      m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, _Rows*m_cols);
-      m_cols = cols;
-    }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index, Index cols)
-    {
-      if(size != _Rows*m_cols)
-      {
-        internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Rows*m_cols);
-        if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative
-          m_data = internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size);
-        else
-          m_data = 0;
-        EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
-      }
-      m_cols = cols;
-    }
-    EIGEN_DEVICE_FUNC const T *data() const { return m_data; }
-    EIGEN_DEVICE_FUNC T *data() { return m_data; }
+    m_cols = cols;
+  }
+  EIGEN_DEVICE_FUNC const T* data() const { return m_data; }
+  EIGEN_DEVICE_FUNC T* data() { return m_data; }
 };
 
 // matrix with dynamic height and fixed width (so that matrix has dynamic size).
-template<typename T, int _Cols, int _Options> class DenseStorage<T, Dynamic, Dynamic, _Cols, _Options>
-{
-    T *m_data;
-    Index m_rows;
-  public:
-    EIGEN_DEVICE_FUNC DenseStorage() : m_data(0), m_rows(0) {}
-    explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_rows(rows)
-    {
+template <typename T, int Cols_, int Options_>
+class DenseStorage<T, Dynamic, Dynamic, Cols_, Options_> {
+  T* m_data;
+  Index m_rows;
+
+ public:
+  EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(0), m_rows(0) {}
+  explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {}
+  EIGEN_DEVICE_FUNC constexpr DenseStorage(Index size, Index rows, Index cols)
+      : m_data(internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(size)), m_rows(rows) {
+    EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
+    eigen_internal_assert(size == rows * cols && rows >= 0 && cols == Cols_);
+    EIGEN_UNUSED_VARIABLE(cols);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
+      : m_data(internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(other.m_rows * Cols_)),
+        m_rows(other.m_rows) {
+    EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows * Cols_)
+    internal::smart_copy(other.m_data, other.m_data + other.m_rows * Cols_, m_data);
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) {
+    if (this != &other) {
+      DenseStorage tmp(other);
+      this->swap(tmp);
+    }
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)),
+                                                                        m_rows(std::move(other.m_rows)) {
+    other.m_data = nullptr;
+    other.m_rows = 0;
+  }
+  EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT {
+    numext::swap(m_data, other.m_data);
+    numext::swap(m_rows, other.m_rows);
+    return *this;
+  }
+  EIGEN_DEVICE_FUNC ~DenseStorage() {
+    internal::conditional_aligned_delete_auto<T, (Options_ & DontAlign) == 0>(m_data, Cols_ * m_rows);
+  }
+  EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+    numext::swap(m_data, other.m_data);
+    numext::swap(m_rows, other.m_rows);
+  }
+  EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT { return m_rows; }
+  EIGEN_DEVICE_FUNC static constexpr Index cols(void) { return Cols_; }
+  void conservativeResize(Index size, Index rows, Index) {
+    m_data =
+        internal::conditional_aligned_realloc_new_auto<T, (Options_ & DontAlign) == 0>(m_data, size, m_rows * Cols_);
+    m_rows = rows;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index rows, Index) {
+    if (size != m_rows * Cols_) {
+      internal::conditional_aligned_delete_auto<T, (Options_ & DontAlign) == 0>(m_data, Cols_ * m_rows);
+      if (size > 0)  // >0 and not simply !=0 to let the compiler knows that size cannot be negative
+        m_data = internal::conditional_aligned_new_auto<T, (Options_ & DontAlign) == 0>(size);
+      else
+        m_data = 0;
       EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
-      eigen_internal_assert(size==rows*cols && rows>=0 && cols == _Cols);
-      EIGEN_UNUSED_VARIABLE(cols);
     }
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
-      : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(other.m_rows*_Cols))
-      , m_rows(other.m_rows)
-    {
-      EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows*_Cols)
-      internal::smart_copy(other.m_data, other.m_data+other.m_rows*_Cols, m_data);
-    }
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
-    {
-      if (this != &other)
-      {
-        DenseStorage tmp(other);
-        this->swap(tmp);
-      }
-      return *this;
-    }
-#if EIGEN_HAS_RVALUE_REFERENCES
-    EIGEN_DEVICE_FUNC
-    DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT
-      : m_data(std::move(other.m_data))
-      , m_rows(std::move(other.m_rows))
-    {
-      other.m_data = nullptr;
-      other.m_rows = 0;
-    }
-    EIGEN_DEVICE_FUNC
-    DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
-    {
-      numext::swap(m_data, other.m_data);
-      numext::swap(m_rows, other.m_rows);
-      return *this;
-    }
-#endif
-    EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Cols*m_rows); }
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
-      numext::swap(m_data,other.m_data);
-      numext::swap(m_rows,other.m_rows);
-    }
-    EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT {return m_rows;}
-    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) {return _Cols;}
-    void conservativeResize(Index size, Index rows, Index)
-    {
-      m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, m_rows*_Cols);
-      m_rows = rows;
-    }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index rows, Index)
-    {
-      if(size != m_rows*_Cols)
-      {
-        internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Cols*m_rows);
-        if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative
-          m_data = internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size);
-        else
-          m_data = 0;
-        EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
-      }
-      m_rows = rows;
-    }
-    EIGEN_DEVICE_FUNC const T *data() const { return m_data; }
-    EIGEN_DEVICE_FUNC T *data() { return m_data; }
+    m_rows = rows;
+  }
+  EIGEN_DEVICE_FUNC const T* data() const { return m_data; }
+  EIGEN_DEVICE_FUNC T* data() { return m_data; }
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATRIX_H
+#endif  // EIGEN_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h
index 3112d2c..8d27857 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h
@@ -11,248 +11,211 @@
 #ifndef EIGEN_DIAGONAL_H
 #define EIGEN_DIAGONAL_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class Diagonal
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a diagonal/subdiagonal/superdiagonal in a matrix
-  *
-  * \param MatrixType the type of the object in which we are taking a sub/main/super diagonal
-  * \param DiagIndex the index of the sub/super diagonal. The default is 0 and it means the main diagonal.
-  *              A positive value means a superdiagonal, a negative value means a subdiagonal.
-  *              You can also use DynamicIndex so the index can be set at runtime.
-  *
-  * The matrix is not required to be square.
-  *
-  * This class represents an expression of the main diagonal, or any sub/super diagonal
-  * of a square matrix. It is the return type of MatrixBase::diagonal() and MatrixBase::diagonal(Index) and most of the
-  * time this is the only way it is used.
-  *
-  * \sa MatrixBase::diagonal(), MatrixBase::diagonal(Index)
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a diagonal/subdiagonal/superdiagonal in a matrix
+ *
+ * \tparam MatrixType the type of the object in which we are taking a sub/main/super diagonal
+ * \tparam DiagIndex the index of the sub/super diagonal. The default is 0 and it means the main diagonal.
+ *              A positive value means a superdiagonal, a negative value means a subdiagonal.
+ *              You can also use DynamicIndex so the index can be set at runtime.
+ *
+ * The matrix is not required to be square.
+ *
+ * This class represents an expression of the main diagonal, or any sub/super diagonal
+ * of a square matrix. It is the return type of MatrixBase::diagonal() and MatrixBase::diagonal(Index) and most of the
+ * time this is the only way it is used.
+ *
+ * \sa MatrixBase::diagonal(), MatrixBase::diagonal(Index)
+ */
 
 namespace internal {
-template<typename MatrixType, int DiagIndex>
-struct traits<Diagonal<MatrixType,DiagIndex> >
- : traits<MatrixType>
-{
+template <typename MatrixType, int DiagIndex>
+struct traits<Diagonal<MatrixType, DiagIndex> > : traits<MatrixType> {
   typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested;
+  typedef std::remove_reference_t<MatrixTypeNested> MatrixTypeNested_;
   typedef typename MatrixType::StorageKind StorageKind;
   enum {
-    RowsAtCompileTime = (int(DiagIndex) == DynamicIndex || int(MatrixType::SizeAtCompileTime) == Dynamic) ? Dynamic
-                      : (EIGEN_PLAIN_ENUM_MIN(MatrixType::RowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0),
-                                              MatrixType::ColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))),
+    RowsAtCompileTime = (int(DiagIndex) == DynamicIndex || int(MatrixType::SizeAtCompileTime) == Dynamic)
+                            ? Dynamic
+                            : (plain_enum_min(MatrixType::RowsAtCompileTime - plain_enum_max(-DiagIndex, 0),
+                                              MatrixType::ColsAtCompileTime - plain_enum_max(DiagIndex, 0))),
     ColsAtCompileTime = 1,
-    MaxRowsAtCompileTime = int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic
-                         : DiagIndex == DynamicIndex ? EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::MaxRowsAtCompileTime,
-                                                                              MatrixType::MaxColsAtCompileTime)
-                         : (EIGEN_PLAIN_ENUM_MIN(MatrixType::MaxRowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0),
-                                                 MatrixType::MaxColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))),
+    MaxRowsAtCompileTime =
+        int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic
+        : DiagIndex == DynamicIndex
+            ? min_size_prefer_fixed(MatrixType::MaxRowsAtCompileTime, MatrixType::MaxColsAtCompileTime)
+            : (plain_enum_min(MatrixType::MaxRowsAtCompileTime - plain_enum_max(-DiagIndex, 0),
+                              MatrixType::MaxColsAtCompileTime - plain_enum_max(DiagIndex, 0))),
     MaxColsAtCompileTime = 1,
     MaskLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
-    Flags = (unsigned int)_MatrixTypeNested::Flags & (RowMajorBit | MaskLvalueBit | DirectAccessBit) & ~RowMajorBit, // FIXME DirectAccessBit should not be handled by expressions
+    Flags = (unsigned int)MatrixTypeNested_::Flags & (RowMajorBit | MaskLvalueBit | DirectAccessBit) &
+            ~RowMajorBit,  // FIXME DirectAccessBit should not be handled by expressions
     MatrixTypeOuterStride = outer_stride_at_compile_time<MatrixType>::ret,
-    InnerStrideAtCompileTime = MatrixTypeOuterStride == Dynamic ? Dynamic : MatrixTypeOuterStride+1,
+    InnerStrideAtCompileTime = MatrixTypeOuterStride == Dynamic ? Dynamic : MatrixTypeOuterStride + 1,
     OuterStrideAtCompileTime = 0
   };
 };
-}
+}  // namespace internal
 
-template<typename MatrixType, int _DiagIndex> class Diagonal
-   : public internal::dense_xpr_base< Diagonal<MatrixType,_DiagIndex> >::type
-{
-  public:
+template <typename MatrixType, int DiagIndex_>
+class Diagonal : public internal::dense_xpr_base<Diagonal<MatrixType, DiagIndex_> >::type {
+ public:
+  enum { DiagIndex = DiagIndex_ };
+  typedef typename internal::dense_xpr_base<Diagonal>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Diagonal)
 
-    enum { DiagIndex = _DiagIndex };
-    typedef typename internal::dense_xpr_base<Diagonal>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Diagonal)
+  EIGEN_DEVICE_FUNC explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex)
+      : m_matrix(matrix), m_index(a_index) {
+    eigen_assert(a_index <= m_matrix.cols() && -a_index <= m_matrix.rows());
+  }
 
-    EIGEN_DEVICE_FUNC
-    explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index)
-    {
-      eigen_assert( a_index <= m_matrix.cols() && -a_index <= m_matrix.rows() );
-    }
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Diagonal)
 
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Diagonal)
+  EIGEN_DEVICE_FUNC inline Index rows() const {
+    return m_index.value() < 0 ? numext::mini<Index>(m_matrix.cols(), m_matrix.rows() + m_index.value())
+                               : numext::mini<Index>(m_matrix.rows(), m_matrix.cols() - m_index.value());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const
-    {
-      return m_index.value()<0 ? numext::mini<Index>(m_matrix.cols(),m_matrix.rows()+m_index.value())
-                               : numext::mini<Index>(m_matrix.rows(),m_matrix.cols()-m_index.value());
-    }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return 1; }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return 1; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT {
+    return m_matrix.outerStride() + 1;
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT {
-      return m_matrix.outerStride() + 1;
-    }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return 0; }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return 0; }
+  typedef std::conditional_t<internal::is_lvalue<MatrixType>::value, Scalar, const Scalar> ScalarWithConstIfNotLvalue;
 
-    typedef typename internal::conditional<
-                       internal::is_lvalue<MatrixType>::value,
-                       Scalar,
-                       const Scalar
-                     >::type ScalarWithConstIfNotLvalue;
+  EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return &(m_matrix.coeffRef(rowOffset(), colOffset())); }
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(m_matrix.coeffRef(rowOffset(), colOffset())); }
 
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue* data() { return &(m_matrix.coeffRef(rowOffset(), colOffset())); }
-    EIGEN_DEVICE_FUNC
-    inline const Scalar* data() const { return &(m_matrix.coeffRef(rowOffset(), colOffset())); }
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index) {
+    EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
+    return m_matrix.coeffRef(row + rowOffset(), row + colOffset());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index row, Index)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
-      return m_matrix.coeffRef(row+rowOffset(), row+colOffset());
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index row, Index) const {
+    return m_matrix.coeffRef(row + rowOffset(), row + colOffset());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index row, Index) const
-    {
-      return m_matrix.coeffRef(row+rowOffset(), row+colOffset());
-    }
+  EIGEN_DEVICE_FUNC inline CoeffReturnType coeff(Index row, Index) const {
+    return m_matrix.coeff(row + rowOffset(), row + colOffset());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline CoeffReturnType coeff(Index row, Index) const
-    {
-      return m_matrix.coeff(row+rowOffset(), row+colOffset());
-    }
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index idx) {
+    EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
+    return m_matrix.coeffRef(idx + rowOffset(), idx + colOffset());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index idx)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
-      return m_matrix.coeffRef(idx+rowOffset(), idx+colOffset());
-    }
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index idx) const {
+    return m_matrix.coeffRef(idx + rowOffset(), idx + colOffset());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index idx) const
-    {
-      return m_matrix.coeffRef(idx+rowOffset(), idx+colOffset());
-    }
+  EIGEN_DEVICE_FUNC inline CoeffReturnType coeff(Index idx) const {
+    return m_matrix.coeff(idx + rowOffset(), idx + colOffset());
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline CoeffReturnType coeff(Index idx) const
-    {
-      return m_matrix.coeff(idx+rowOffset(), idx+colOffset());
-    }
+  EIGEN_DEVICE_FUNC inline const internal::remove_all_t<typename MatrixType::Nested>& nestedExpression() const {
+    return m_matrix;
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline const typename internal::remove_all<typename MatrixType::Nested>::type&
-    nestedExpression() const
-    {
-      return m_matrix;
-    }
+  EIGEN_DEVICE_FUNC inline Index index() const { return m_index.value(); }
 
-    EIGEN_DEVICE_FUNC
-    inline Index index() const
-    {
-      return m_index.value();
-    }
+ protected:
+  typename internal::ref_selector<MatrixType>::non_const_type m_matrix;
+  const internal::variable_if_dynamicindex<Index, DiagIndex> m_index;
 
-  protected:
-    typename internal::ref_selector<MatrixType>::non_const_type m_matrix;
-    const internal::variable_if_dynamicindex<Index, DiagIndex> m_index;
-
-  private:
-    // some compilers may fail to optimize std::max etc in case of compile-time constants...
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index absDiagIndex() const EIGEN_NOEXCEPT { return m_index.value()>0 ? m_index.value() : -m_index.value(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rowOffset() const EIGEN_NOEXCEPT { return m_index.value()>0 ? 0 : -m_index.value(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index colOffset() const EIGEN_NOEXCEPT { return m_index.value()>0 ? m_index.value() : 0; }
-    // trigger a compile-time error if someone try to call packet
-    template<int LoadMode> typename MatrixType::PacketReturnType packet(Index) const;
-    template<int LoadMode> typename MatrixType::PacketReturnType packet(Index,Index) const;
+ private:
+  // some compilers may fail to optimize std::max etc in case of compile-time constants...
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index absDiagIndex() const EIGEN_NOEXCEPT {
+    return m_index.value() > 0 ? m_index.value() : -m_index.value();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowOffset() const EIGEN_NOEXCEPT {
+    return m_index.value() > 0 ? 0 : -m_index.value();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colOffset() const EIGEN_NOEXCEPT {
+    return m_index.value() > 0 ? m_index.value() : 0;
+  }
+  // trigger a compile-time error if someone try to call packet
+  template <int LoadMode>
+  typename MatrixType::PacketReturnType packet(Index) const;
+  template <int LoadMode>
+  typename MatrixType::PacketReturnType packet(Index, Index) const;
 };
 
 /** \returns an expression of the main diagonal of the matrix \c *this
-  *
-  * \c *this is not required to be square.
-  *
-  * Example: \include MatrixBase_diagonal.cpp
-  * Output: \verbinclude MatrixBase_diagonal.out
-  *
-  * \sa class Diagonal */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::DiagonalReturnType
-MatrixBase<Derived>::diagonal()
-{
+ *
+ * \c *this is not required to be square.
+ *
+ * Example: \include MatrixBase_diagonal.cpp
+ * Output: \verbinclude MatrixBase_diagonal.out
+ *
+ * \sa class Diagonal */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::DiagonalReturnType MatrixBase<Derived>::diagonal() {
   return DiagonalReturnType(derived());
 }
 
 /** This is the const version of diagonal(). */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::ConstDiagonalReturnType
-MatrixBase<Derived>::diagonal() const
-{
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::ConstDiagonalReturnType MatrixBase<Derived>::diagonal()
+    const {
   return ConstDiagonalReturnType(derived());
 }
 
 /** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this
-  *
-  * \c *this is not required to be square.
-  *
-  * The template parameter \a DiagIndex represent a super diagonal if \a DiagIndex > 0
-  * and a sub diagonal otherwise. \a DiagIndex == 0 is equivalent to the main diagonal.
-  *
-  * Example: \include MatrixBase_diagonal_int.cpp
-  * Output: \verbinclude MatrixBase_diagonal_int.out
-  *
-  * \sa MatrixBase::diagonal(), class Diagonal */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::DiagonalDynamicIndexReturnType
-MatrixBase<Derived>::diagonal(Index index)
-{
-  return DiagonalDynamicIndexReturnType(derived(), index);
+ *
+ * \c *this is not required to be square.
+ *
+ * The template parameter \a DiagIndex represent a super diagonal if \a DiagIndex > 0
+ * and a sub diagonal otherwise. \a DiagIndex == 0 is equivalent to the main diagonal.
+ *
+ * Example: \include MatrixBase_diagonal_int.cpp
+ * Output: \verbinclude MatrixBase_diagonal_int.out
+ *
+ * \sa MatrixBase::diagonal(), class Diagonal */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline Diagonal<Derived, DynamicIndex> MatrixBase<Derived>::diagonal(Index index) {
+  return Diagonal<Derived, DynamicIndex>(derived(), index);
 }
 
 /** This is the const version of diagonal(Index). */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::ConstDiagonalDynamicIndexReturnType
-MatrixBase<Derived>::diagonal(Index index) const
-{
-  return ConstDiagonalDynamicIndexReturnType(derived(), index);
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const Diagonal<const Derived, DynamicIndex> MatrixBase<Derived>::diagonal(Index index) const {
+  return Diagonal<const Derived, DynamicIndex>(derived(), index);
 }
 
 /** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this
-  *
-  * \c *this is not required to be square.
-  *
-  * The template parameter \a DiagIndex represent a super diagonal if \a DiagIndex > 0
-  * and a sub diagonal otherwise. \a DiagIndex == 0 is equivalent to the main diagonal.
-  *
-  * Example: \include MatrixBase_diagonal_template_int.cpp
-  * Output: \verbinclude MatrixBase_diagonal_template_int.out
-  *
-  * \sa MatrixBase::diagonal(), class Diagonal */
-template<typename Derived>
-template<int Index_>
-EIGEN_DEVICE_FUNC
-inline typename MatrixBase<Derived>::template DiagonalIndexReturnType<Index_>::Type
-MatrixBase<Derived>::diagonal()
-{
-  return typename DiagonalIndexReturnType<Index_>::Type(derived());
+ *
+ * \c *this is not required to be square.
+ *
+ * The template parameter \a DiagIndex represent a super diagonal if \a DiagIndex > 0
+ * and a sub diagonal otherwise. \a DiagIndex == 0 is equivalent to the main diagonal.
+ *
+ * Example: \include MatrixBase_diagonal_template_int.cpp
+ * Output: \verbinclude MatrixBase_diagonal_template_int.out
+ *
+ * \sa MatrixBase::diagonal(), class Diagonal */
+template <typename Derived>
+template <int Index_>
+EIGEN_DEVICE_FUNC inline Diagonal<Derived, Index_> MatrixBase<Derived>::diagonal() {
+  return Diagonal<Derived, Index_>(derived());
 }
 
 /** This is the const version of diagonal<int>(). */
-template<typename Derived>
-template<int Index_>
-EIGEN_DEVICE_FUNC
-inline typename MatrixBase<Derived>::template ConstDiagonalIndexReturnType<Index_>::Type
-MatrixBase<Derived>::diagonal() const
-{
-  return typename ConstDiagonalIndexReturnType<Index_>::Type(derived());
+template <typename Derived>
+template <int Index_>
+EIGEN_DEVICE_FUNC inline const Diagonal<const Derived, Index_> MatrixBase<Derived>::diagonal() const {
+  return Diagonal<const Derived, Index_>(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DIAGONAL_H
+#endif  // EIGEN_DIAGONAL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h
index 542685c..fd61bb7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h
@@ -11,270 +11,294 @@
 #ifndef EIGEN_DIAGONALMATRIX_H
 #define EIGEN_DIAGONALMATRIX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename Derived>
-class DiagonalBase : public EigenBase<Derived>
-{
-  public:
-    typedef typename internal::traits<Derived>::DiagonalVectorType DiagonalVectorType;
-    typedef typename DiagonalVectorType::Scalar Scalar;
-    typedef typename DiagonalVectorType::RealScalar RealScalar;
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+namespace Eigen {
 
-    enum {
-      RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime,
-      ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime,
-      MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime,
-      MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime,
-      IsVectorAtCompileTime = 0,
-      Flags = NoPreferredStorageOrderBit
-    };
+/** \class DiagonalBase
+ * \ingroup Core_Module
+ *
+ * \brief Base class for diagonal matrices and expressions
+ *
+ * This is the base class that is inherited by diagonal matrix and related expression
+ * types, which internally use a vector for storing the diagonal entries. Diagonal
+ * types always represent square matrices.
+ *
+ * \tparam Derived is the derived type, a DiagonalMatrix or DiagonalWrapper.
+ *
+ * \sa class DiagonalMatrix, class DiagonalWrapper
+ */
+template <typename Derived>
+class DiagonalBase : public EigenBase<Derived> {
+ public:
+  typedef typename internal::traits<Derived>::DiagonalVectorType DiagonalVectorType;
+  typedef typename DiagonalVectorType::Scalar Scalar;
+  typedef typename DiagonalVectorType::RealScalar RealScalar;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
 
-    typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime, 0, MaxRowsAtCompileTime, MaxColsAtCompileTime> DenseMatrixType;
-    typedef DenseMatrixType DenseType;
-    typedef DiagonalMatrix<Scalar,DiagonalVectorType::SizeAtCompileTime,DiagonalVectorType::MaxSizeAtCompileTime> PlainObject;
+  enum {
+    RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime,
+    ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime,
+    MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime,
+    MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime,
+    IsVectorAtCompileTime = 0,
+    Flags = NoPreferredStorageOrderBit
+  };
 
-    EIGEN_DEVICE_FUNC
-    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
-    EIGEN_DEVICE_FUNC
-    inline Derived& derived() { return *static_cast<Derived*>(this); }
+  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime, 0, MaxRowsAtCompileTime, MaxColsAtCompileTime>
+      DenseMatrixType;
+  typedef DenseMatrixType DenseType;
+  typedef DiagonalMatrix<Scalar, DiagonalVectorType::SizeAtCompileTime, DiagonalVectorType::MaxSizeAtCompileTime>
+      PlainObject;
 
-    EIGEN_DEVICE_FUNC
-    DenseMatrixType toDenseMatrix() const { return derived(); }
+  /** \returns a reference to the derived object. */
+  EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+  /** \returns a const reference to the derived object. */
+  EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast<Derived*>(this); }
 
-    EIGEN_DEVICE_FUNC
-    inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); }
-    EIGEN_DEVICE_FUNC
-    inline DiagonalVectorType& diagonal() { return derived().diagonal(); }
+  /**
+   * Constructs a dense matrix from \c *this. Note, this directly returns a dense matrix type,
+   * not an expression.
+   * \returns A dense matrix, with its diagonal entries set from the the derived object. */
+  EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const { return derived(); }
 
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return diagonal().size(); }
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return diagonal().size(); }
+  /** \returns a reference to the derived object's vector of diagonal coefficients. */
+  EIGEN_DEVICE_FUNC inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); }
+  /** \returns a const reference to the derived object's vector of diagonal coefficients. */
+  EIGEN_DEVICE_FUNC inline DiagonalVectorType& diagonal() { return derived().diagonal(); }
 
-    template<typename MatrixDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<Derived,MatrixDerived,LazyProduct>
-    operator*(const MatrixBase<MatrixDerived> &matrix) const
-    {
-      return Product<Derived, MatrixDerived, LazyProduct>(derived(),matrix.derived());
-    }
+  /** \returns the value of the coefficient as if \c *this was a dense matrix. */
+  EIGEN_DEVICE_FUNC inline Scalar coeff(Index row, Index col) const {
+    eigen_assert(row >= 0 && col >= 0 && row < rows() && col <= cols());
+    return row == col ? diagonal().coeff(row) : Scalar(0);
+  }
 
-    typedef DiagonalWrapper<const CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const DiagonalVectorType> > InverseReturnType;
-    EIGEN_DEVICE_FUNC
-    inline const InverseReturnType
-    inverse() const
-    {
-      return InverseReturnType(diagonal().cwiseInverse());
-    }
-    
-    EIGEN_DEVICE_FUNC
-    inline const DiagonalWrapper<const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DiagonalVectorType,Scalar,product) >
-    operator*(const Scalar& scalar) const
-    {
-      return DiagonalWrapper<const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DiagonalVectorType,Scalar,product) >(diagonal() * scalar);
-    }
-    EIGEN_DEVICE_FUNC
-    friend inline const DiagonalWrapper<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,DiagonalVectorType,product) >
-    operator*(const Scalar& scalar, const DiagonalBase& other)
-    {
-      return DiagonalWrapper<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,DiagonalVectorType,product) >(scalar * other.diagonal());
-    }
+  /** \returns the number of rows. */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const { return diagonal().size(); }
+  /** \returns the number of columns. */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const { return diagonal().size(); }
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    inline unspecified_expression_type
-    #else
-    inline const DiagonalWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(DiagonalVectorType,typename OtherDerived::DiagonalVectorType,sum) >
-    #endif
-    operator+(const DiagonalBase<OtherDerived>& other) const
-    {
-      return (diagonal() + other.diagonal()).asDiagonal();
-    }
+  /** \returns the diagonal matrix product of \c *this by the dense matrix, \a matrix */
+  template <typename MatrixDerived>
+  EIGEN_DEVICE_FUNC const Product<Derived, MatrixDerived, LazyProduct> operator*(
+      const MatrixBase<MatrixDerived>& matrix) const {
+    return Product<Derived, MatrixDerived, LazyProduct>(derived(), matrix.derived());
+  }
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    inline unspecified_expression_type
-    #else
-    inline const DiagonalWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(DiagonalVectorType,typename OtherDerived::DiagonalVectorType,difference) >
-    #endif
-    operator-(const DiagonalBase<OtherDerived>& other) const
-    {
-      return (diagonal() - other.diagonal()).asDiagonal();
-    }
+  template <typename OtherDerived>
+  using DiagonalProductReturnType = DiagonalWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+      DiagonalVectorType, typename OtherDerived::DiagonalVectorType, product)>;
+
+  /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC const DiagonalProductReturnType<OtherDerived> operator*(
+      const DiagonalBase<OtherDerived>& other) const {
+    return diagonal().cwiseProduct(other.diagonal()).asDiagonal();
+  }
+
+  using DiagonalInverseReturnType =
+      DiagonalWrapper<const CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const DiagonalVectorType>>;
+
+  /** \returns the inverse \c *this. Computed as the coefficient-wise inverse of the diagonal. */
+  EIGEN_DEVICE_FUNC inline const DiagonalInverseReturnType inverse() const {
+    return diagonal().cwiseInverse().asDiagonal();
+  }
+
+  using DiagonalScaleReturnType =
+      DiagonalWrapper<const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DiagonalVectorType, Scalar, product)>;
+
+  /** \returns the product of \c *this by the scalar \a scalar */
+  EIGEN_DEVICE_FUNC inline const DiagonalScaleReturnType operator*(const Scalar& scalar) const {
+    return (diagonal() * scalar).asDiagonal();
+  }
+
+  using ScaleDiagonalReturnType =
+      DiagonalWrapper<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar, DiagonalVectorType, product)>;
+
+  /** \returns the product of a scalar and the diagonal matrix \a other */
+  EIGEN_DEVICE_FUNC friend inline const ScaleDiagonalReturnType operator*(const Scalar& scalar,
+                                                                          const DiagonalBase& other) {
+    return (scalar * other.diagonal()).asDiagonal();
+  }
+
+  template <typename OtherDerived>
+  using DiagonalSumReturnType = DiagonalWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+      DiagonalVectorType, typename OtherDerived::DiagonalVectorType, sum)>;
+
+  /** \returns the sum of \c *this and the diagonal matrix \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline const DiagonalSumReturnType<OtherDerived> operator+(
+      const DiagonalBase<OtherDerived>& other) const {
+    return (diagonal() + other.diagonal()).asDiagonal();
+  }
+
+  template <typename OtherDerived>
+  using DiagonalDifferenceReturnType = DiagonalWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+      DiagonalVectorType, typename OtherDerived::DiagonalVectorType, difference)>;
+
+  /** \returns the difference of \c *this and the diagonal matrix \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline const DiagonalDifferenceReturnType<OtherDerived> operator-(
+      const DiagonalBase<OtherDerived>& other) const {
+    return (diagonal() - other.diagonal()).asDiagonal();
+  }
 };
 
-#endif
-
 /** \class DiagonalMatrix
-  * \ingroup Core_Module
-  *
-  * \brief Represents a diagonal matrix with its storage
-  *
-  * \param _Scalar the type of coefficients
-  * \param SizeAtCompileTime the dimension of the matrix, or Dynamic
-  * \param MaxSizeAtCompileTime the dimension of the matrix, or Dynamic. This parameter is optional and defaults
-  *        to SizeAtCompileTime. Most of the time, you do not need to specify it.
-  *
-  * \sa class DiagonalWrapper
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Represents a diagonal matrix with its storage
+ *
+ * \tparam Scalar_ the type of coefficients
+ * \tparam SizeAtCompileTime the dimension of the matrix, or Dynamic
+ * \tparam MaxSizeAtCompileTime the dimension of the matrix, or Dynamic. This parameter is optional and defaults
+ *        to SizeAtCompileTime. Most of the time, you do not need to specify it.
+ *
+ * \sa class DiagonalBase, class DiagonalWrapper
+ */
 
 namespace internal {
-template<typename _Scalar, int SizeAtCompileTime, int MaxSizeAtCompileTime>
-struct traits<DiagonalMatrix<_Scalar,SizeAtCompileTime,MaxSizeAtCompileTime> >
- : traits<Matrix<_Scalar,SizeAtCompileTime,SizeAtCompileTime,0,MaxSizeAtCompileTime,MaxSizeAtCompileTime> >
-{
-  typedef Matrix<_Scalar,SizeAtCompileTime,1,0,MaxSizeAtCompileTime,1> DiagonalVectorType;
+template <typename Scalar_, int SizeAtCompileTime, int MaxSizeAtCompileTime>
+struct traits<DiagonalMatrix<Scalar_, SizeAtCompileTime, MaxSizeAtCompileTime>>
+    : traits<Matrix<Scalar_, SizeAtCompileTime, SizeAtCompileTime, 0, MaxSizeAtCompileTime, MaxSizeAtCompileTime>> {
+  typedef Matrix<Scalar_, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1> DiagonalVectorType;
   typedef DiagonalShape StorageKind;
-  enum {
-    Flags = LvalueBit | NoPreferredStorageOrderBit
-  };
+  enum { Flags = LvalueBit | NoPreferredStorageOrderBit | NestByRefBit };
 };
-}
-template<typename _Scalar, int SizeAtCompileTime, int MaxSizeAtCompileTime>
-class DiagonalMatrix
-  : public DiagonalBase<DiagonalMatrix<_Scalar,SizeAtCompileTime,MaxSizeAtCompileTime> >
-{
-  public:
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename internal::traits<DiagonalMatrix>::DiagonalVectorType DiagonalVectorType;
-    typedef const DiagonalMatrix& Nested;
-    typedef _Scalar Scalar;
-    typedef typename internal::traits<DiagonalMatrix>::StorageKind StorageKind;
-    typedef typename internal::traits<DiagonalMatrix>::StorageIndex StorageIndex;
-    #endif
+}  // namespace internal
+template <typename Scalar_, int SizeAtCompileTime, int MaxSizeAtCompileTime>
+class DiagonalMatrix : public DiagonalBase<DiagonalMatrix<Scalar_, SizeAtCompileTime, MaxSizeAtCompileTime>> {
+ public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  typedef typename internal::traits<DiagonalMatrix>::DiagonalVectorType DiagonalVectorType;
+  typedef const DiagonalMatrix& Nested;
+  typedef Scalar_ Scalar;
+  typedef typename internal::traits<DiagonalMatrix>::StorageKind StorageKind;
+  typedef typename internal::traits<DiagonalMatrix>::StorageIndex StorageIndex;
+#endif
 
-  protected:
+ protected:
+  DiagonalVectorType m_diagonal;
 
-    DiagonalVectorType m_diagonal;
+ public:
+  /** const version of diagonal(). */
+  EIGEN_DEVICE_FUNC inline const DiagonalVectorType& diagonal() const { return m_diagonal; }
+  /** \returns a reference to the stored vector of diagonal coefficients. */
+  EIGEN_DEVICE_FUNC inline DiagonalVectorType& diagonal() { return m_diagonal; }
 
-  public:
+  /** Default constructor without initialization */
+  EIGEN_DEVICE_FUNC inline DiagonalMatrix() {}
 
-    /** const version of diagonal(). */
-    EIGEN_DEVICE_FUNC
-    inline const DiagonalVectorType& diagonal() const { return m_diagonal; }
-    /** \returns a reference to the stored vector of diagonal coefficients. */
-    EIGEN_DEVICE_FUNC
-    inline DiagonalVectorType& diagonal() { return m_diagonal; }
+  /** Constructs a diagonal matrix with given dimension  */
+  EIGEN_DEVICE_FUNC explicit inline DiagonalMatrix(Index dim) : m_diagonal(dim) {}
 
-    /** Default constructor without initialization */
-    EIGEN_DEVICE_FUNC
-    inline DiagonalMatrix() {}
+  /** 2D constructor. */
+  EIGEN_DEVICE_FUNC inline DiagonalMatrix(const Scalar& x, const Scalar& y) : m_diagonal(x, y) {}
 
-    /** Constructs a diagonal matrix with given dimension  */
-    EIGEN_DEVICE_FUNC
-    explicit inline DiagonalMatrix(Index dim) : m_diagonal(dim) {}
+  /** 3D constructor. */
+  EIGEN_DEVICE_FUNC inline DiagonalMatrix(const Scalar& x, const Scalar& y, const Scalar& z) : m_diagonal(x, y, z) {}
 
-    /** 2D constructor. */
-    EIGEN_DEVICE_FUNC
-    inline DiagonalMatrix(const Scalar& x, const Scalar& y) : m_diagonal(x,y) {}
-
-    /** 3D constructor. */
-    EIGEN_DEVICE_FUNC
-    inline DiagonalMatrix(const Scalar& x, const Scalar& y, const Scalar& z) : m_diagonal(x,y,z) {}
-
-    #if EIGEN_HAS_CXX11
-    /** \brief Construct a diagonal matrix with fixed size from an arbitrary number of coefficients. \cpp11
-      * 
-      * There exists C++98 anologue constructors for fixed-size diagonal matrices having 2 or 3 coefficients.
-      * 
-      * \warning To construct a diagonal matrix of fixed size, the number of values passed to this 
-      * constructor must match the fixed dimension of \c *this.
-      * 
-      * \sa DiagonalMatrix(const Scalar&, const Scalar&)
-      * \sa DiagonalMatrix(const Scalar&, const Scalar&, const Scalar&)
-      */
-    template <typename... ArgTypes>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    DiagonalMatrix(const Scalar& a0, const Scalar& a1, const Scalar& a2, const ArgTypes&... args)
+  /** \brief Construct a diagonal matrix with fixed size from an arbitrary number of coefficients.
+   *
+   * \warning To construct a diagonal matrix of fixed size, the number of values passed to this
+   * constructor must match the fixed dimension of \c *this.
+   *
+   * \sa DiagonalMatrix(const Scalar&, const Scalar&)
+   * \sa DiagonalMatrix(const Scalar&, const Scalar&, const Scalar&)
+   */
+  template <typename... ArgTypes>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DiagonalMatrix(const Scalar& a0, const Scalar& a1, const Scalar& a2,
+                                                       const ArgTypes&... args)
       : m_diagonal(a0, a1, a2, args...) {}
 
-    /** \brief Constructs a DiagonalMatrix and initializes it by elements given by an initializer list of initializer
-      * lists \cpp11
-      */
-    EIGEN_DEVICE_FUNC
-    explicit EIGEN_STRONG_INLINE DiagonalMatrix(const std::initializer_list<std::initializer_list<Scalar>>& list)
+  /** \brief Constructs a DiagonalMatrix and initializes it by elements given by an initializer list of initializer
+   * lists \cpp11
+   */
+  EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE DiagonalMatrix(
+      const std::initializer_list<std::initializer_list<Scalar>>& list)
       : m_diagonal(list) {}
-    #endif  // EIGEN_HAS_CXX11
 
-    /** Copy constructor. */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    inline DiagonalMatrix(const DiagonalBase<OtherDerived>& other) : m_diagonal(other.diagonal()) {}
+  /** \brief Constructs a DiagonalMatrix from an r-value diagonal vector type */
+  EIGEN_DEVICE_FUNC explicit inline DiagonalMatrix(DiagonalVectorType&& diag) : m_diagonal(std::move(diag)) {}
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** copy constructor. prevent a default copy constructor from hiding the other templated constructor */
-    inline DiagonalMatrix(const DiagonalMatrix& other) : m_diagonal(other.diagonal()) {}
-    #endif
+  /** Copy constructor. */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline DiagonalMatrix(const DiagonalBase<OtherDerived>& other) : m_diagonal(other.diagonal()) {}
 
-    /** generic constructor from expression of the diagonal coefficients */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    explicit inline DiagonalMatrix(const MatrixBase<OtherDerived>& other) : m_diagonal(other)
-    {}
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** copy constructor. prevent a default copy constructor from hiding the other templated constructor */
+  inline DiagonalMatrix(const DiagonalMatrix& other) : m_diagonal(other.diagonal()) {}
+#endif
 
-    /** Copy operator. */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    DiagonalMatrix& operator=(const DiagonalBase<OtherDerived>& other)
-    {
-      m_diagonal = other.diagonal();
-      return *this;
-    }
+  /** generic constructor from expression of the diagonal coefficients */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC explicit inline DiagonalMatrix(const MatrixBase<OtherDerived>& other) : m_diagonal(other) {}
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    EIGEN_DEVICE_FUNC
-    DiagonalMatrix& operator=(const DiagonalMatrix& other)
-    {
-      m_diagonal = other.diagonal();
-      return *this;
-    }
-    #endif
+  /** Copy operator. */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC DiagonalMatrix& operator=(const DiagonalBase<OtherDerived>& other) {
+    m_diagonal = other.diagonal();
+    return *this;
+  }
 
-    /** Resizes to given size. */
-    EIGEN_DEVICE_FUNC
-    inline void resize(Index size) { m_diagonal.resize(size); }
-    /** Sets all coefficients to zero. */
-    EIGEN_DEVICE_FUNC
-    inline void setZero() { m_diagonal.setZero(); }
-    /** Resizes and sets all coefficients to zero. */
-    EIGEN_DEVICE_FUNC
-    inline void setZero(Index size) { m_diagonal.setZero(size); }
-    /** Sets this matrix to be the identity matrix of the current size. */
-    EIGEN_DEVICE_FUNC
-    inline void setIdentity() { m_diagonal.setOnes(); }
-    /** Sets this matrix to be the identity matrix of the given size. */
-    EIGEN_DEVICE_FUNC
-    inline void setIdentity(Index size) { m_diagonal.setOnes(size); }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** This is a special case of the templated operator=. Its purpose is to
+   * prevent a default operator= from hiding the templated operator=.
+   */
+  EIGEN_DEVICE_FUNC DiagonalMatrix& operator=(const DiagonalMatrix& other) {
+    m_diagonal = other.diagonal();
+    return *this;
+  }
+#endif
+
+  typedef DiagonalWrapper<const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, DiagonalVectorType>>
+      InitializeReturnType;
+
+  /** Initializes a diagonal matrix of size SizeAtCompileTime with coefficients set to zero */
+  EIGEN_DEVICE_FUNC static const InitializeReturnType Zero() { return DiagonalVectorType::Zero().asDiagonal(); }
+  /** Initializes a diagonal matrix of size dim with coefficients set to zero */
+  EIGEN_DEVICE_FUNC static const InitializeReturnType Zero(Index size) {
+    return DiagonalVectorType::Zero(size).asDiagonal();
+  }
+  /** Initializes a identity matrix of size SizeAtCompileTime */
+  EIGEN_DEVICE_FUNC static const InitializeReturnType Identity() { return DiagonalVectorType::Ones().asDiagonal(); }
+  /** Initializes a identity matrix of size dim */
+  EIGEN_DEVICE_FUNC static const InitializeReturnType Identity(Index size) {
+    return DiagonalVectorType::Ones(size).asDiagonal();
+  }
+
+  /** Resizes to given size. */
+  EIGEN_DEVICE_FUNC inline void resize(Index size) { m_diagonal.resize(size); }
+  /** Sets all coefficients to zero. */
+  EIGEN_DEVICE_FUNC inline void setZero() { m_diagonal.setZero(); }
+  /** Resizes and sets all coefficients to zero. */
+  EIGEN_DEVICE_FUNC inline void setZero(Index size) { m_diagonal.setZero(size); }
+  /** Sets this matrix to be the identity matrix of the current size. */
+  EIGEN_DEVICE_FUNC inline void setIdentity() { m_diagonal.setOnes(); }
+  /** Sets this matrix to be the identity matrix of the given size. */
+  EIGEN_DEVICE_FUNC inline void setIdentity(Index size) { m_diagonal.setOnes(size); }
 };
 
 /** \class DiagonalWrapper
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a diagonal matrix
-  *
-  * \param _DiagonalVectorType the type of the vector of diagonal coefficients
-  *
-  * This class is an expression of a diagonal matrix, but not storing its own vector of diagonal coefficients,
-  * instead wrapping an existing vector expression. It is the return type of MatrixBase::asDiagonal()
-  * and most of the time this is the only way that it is used.
-  *
-  * \sa class DiagonalMatrix, class DiagonalBase, MatrixBase::asDiagonal()
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a diagonal matrix
+ *
+ * \tparam DiagonalVectorType_ the type of the vector of diagonal coefficients
+ *
+ * This class is an expression of a diagonal matrix, but not storing its own vector of diagonal coefficients,
+ * instead wrapping an existing vector expression. It is the return type of MatrixBase::asDiagonal()
+ * and most of the time this is the only way that it is used.
+ *
+ * \sa class DiagonalMatrix, class DiagonalBase, MatrixBase::asDiagonal()
+ */
 
 namespace internal {
-template<typename _DiagonalVectorType>
-struct traits<DiagonalWrapper<_DiagonalVectorType> >
-{
-  typedef _DiagonalVectorType DiagonalVectorType;
+template <typename DiagonalVectorType_>
+struct traits<DiagonalWrapper<DiagonalVectorType_>> {
+  typedef DiagonalVectorType_ DiagonalVectorType;
   typedef typename DiagonalVectorType::Scalar Scalar;
   typedef typename DiagonalVectorType::StorageIndex StorageIndex;
   typedef DiagonalShape StorageKind;
@@ -284,108 +308,107 @@
     ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime,
     MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime,
     MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime,
-    Flags =  (traits<DiagonalVectorType>::Flags & LvalueBit) | NoPreferredStorageOrderBit
+    Flags = (traits<DiagonalVectorType>::Flags & LvalueBit) | NoPreferredStorageOrderBit
   };
 };
-}
+}  // namespace internal
 
-template<typename _DiagonalVectorType>
-class DiagonalWrapper
-  : public DiagonalBase<DiagonalWrapper<_DiagonalVectorType> >, internal::no_assignment_operator
-{
-  public:
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef _DiagonalVectorType DiagonalVectorType;
-    typedef DiagonalWrapper Nested;
-    #endif
+template <typename DiagonalVectorType_>
+class DiagonalWrapper : public DiagonalBase<DiagonalWrapper<DiagonalVectorType_>>, internal::no_assignment_operator {
+ public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  typedef DiagonalVectorType_ DiagonalVectorType;
+  typedef DiagonalWrapper Nested;
+#endif
 
-    /** Constructor from expression of diagonal coefficients to wrap. */
-    EIGEN_DEVICE_FUNC
-    explicit inline DiagonalWrapper(DiagonalVectorType& a_diagonal) : m_diagonal(a_diagonal) {}
+  /** Constructor from expression of diagonal coefficients to wrap. */
+  EIGEN_DEVICE_FUNC explicit inline DiagonalWrapper(DiagonalVectorType& a_diagonal) : m_diagonal(a_diagonal) {}
 
-    /** \returns a const reference to the wrapped expression of diagonal coefficients. */
-    EIGEN_DEVICE_FUNC
-    const DiagonalVectorType& diagonal() const { return m_diagonal; }
+  /** \returns a const reference to the wrapped expression of diagonal coefficients. */
+  EIGEN_DEVICE_FUNC const DiagonalVectorType& diagonal() const { return m_diagonal; }
 
-  protected:
-    typename DiagonalVectorType::Nested m_diagonal;
+ protected:
+  typename DiagonalVectorType::Nested m_diagonal;
 };
 
 /** \returns a pseudo-expression of a diagonal matrix with *this as vector of diagonal coefficients
-  *
-  * \only_for_vectors
-  *
-  * Example: \include MatrixBase_asDiagonal.cpp
-  * Output: \verbinclude MatrixBase_asDiagonal.out
-  *
-  * \sa class DiagonalWrapper, class DiagonalMatrix, diagonal(), isDiagonal()
-  **/
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline const DiagonalWrapper<const Derived>
-MatrixBase<Derived>::asDiagonal() const
-{
+ *
+ * \only_for_vectors
+ *
+ * Example: \include MatrixBase_asDiagonal.cpp
+ * Output: \verbinclude MatrixBase_asDiagonal.out
+ *
+ * \sa class DiagonalWrapper, class DiagonalMatrix, diagonal(), isDiagonal()
+ **/
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const DiagonalWrapper<const Derived> MatrixBase<Derived>::asDiagonal() const {
   return DiagonalWrapper<const Derived>(derived());
 }
 
 /** \returns true if *this is approximately equal to a diagonal matrix,
-  *          within the precision given by \a prec.
-  *
-  * Example: \include MatrixBase_isDiagonal.cpp
-  * Output: \verbinclude MatrixBase_isDiagonal.out
-  *
-  * \sa asDiagonal()
-  */
-template<typename Derived>
-bool MatrixBase<Derived>::isDiagonal(const RealScalar& prec) const
-{
-  if(cols() != rows()) return false;
+ *          within the precision given by \a prec.
+ *
+ * Example: \include MatrixBase_isDiagonal.cpp
+ * Output: \verbinclude MatrixBase_isDiagonal.out
+ *
+ * \sa asDiagonal()
+ */
+template <typename Derived>
+bool MatrixBase<Derived>::isDiagonal(const RealScalar& prec) const {
+  if (cols() != rows()) return false;
   RealScalar maxAbsOnDiagonal = static_cast<RealScalar>(-1);
-  for(Index j = 0; j < cols(); ++j)
-  {
-    RealScalar absOnDiagonal = numext::abs(coeff(j,j));
-    if(absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal;
+  for (Index j = 0; j < cols(); ++j) {
+    RealScalar absOnDiagonal = numext::abs(coeff(j, j));
+    if (absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal;
   }
-  for(Index j = 0; j < cols(); ++j)
-    for(Index i = 0; i < j; ++i)
-    {
-      if(!internal::isMuchSmallerThan(coeff(i, j), maxAbsOnDiagonal, prec)) return false;
-      if(!internal::isMuchSmallerThan(coeff(j, i), maxAbsOnDiagonal, prec)) return false;
+  for (Index j = 0; j < cols(); ++j)
+    for (Index i = 0; i < j; ++i) {
+      if (!internal::isMuchSmallerThan(coeff(i, j), maxAbsOnDiagonal, prec)) return false;
+      if (!internal::isMuchSmallerThan(coeff(j, i), maxAbsOnDiagonal, prec)) return false;
     }
   return true;
 }
 
 namespace internal {
 
-template<> struct storage_kind_to_shape<DiagonalShape> { typedef DiagonalShape Shape; };
+template <>
+struct storage_kind_to_shape<DiagonalShape> {
+  typedef DiagonalShape Shape;
+};
 
 struct Diagonal2Dense {};
 
-template<> struct AssignmentKind<DenseShape,DiagonalShape> { typedef Diagonal2Dense Kind; };
+template <>
+struct AssignmentKind<DenseShape, DiagonalShape> {
+  typedef Diagonal2Dense Kind;
+};
 
 // Diagonal matrix to Dense assignment
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Diagonal2Dense>
-{
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  {
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Diagonal2Dense> {
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
-    
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
+
     dst.setZero();
     dst.diagonal() = src.diagonal();
   }
-  
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  { dst.diagonal() += src.diagonal(); }
-  
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  { dst.diagonal() -= src.diagonal(); }
+
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::add_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
+    dst.diagonal() += src.diagonal();
+  }
+
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::sub_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
+    dst.diagonal() -= src.diagonal();
+  }
 };
 
-} // namespace internal
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DIAGONALMATRIX_H
+#endif  // EIGEN_DIAGONALMATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h
index 7911d1c..bd0feea 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h
@@ -11,18 +11,20 @@
 #ifndef EIGEN_DIAGONALPRODUCT_H
 #define EIGEN_DIAGONALPRODUCT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal.
-  */
-template<typename Derived>
-template<typename DiagonalDerived>
-EIGEN_DEVICE_FUNC inline const Product<Derived, DiagonalDerived, LazyProduct>
-MatrixBase<Derived>::operator*(const DiagonalBase<DiagonalDerived> &a_diagonal) const
-{
-  return Product<Derived, DiagonalDerived, LazyProduct>(derived(),a_diagonal.derived());
+ */
+template <typename Derived>
+template <typename DiagonalDerived>
+EIGEN_DEVICE_FUNC inline const Product<Derived, DiagonalDerived, LazyProduct> MatrixBase<Derived>::operator*(
+    const DiagonalBase<DiagonalDerived> &a_diagonal) const {
+  return Product<Derived, DiagonalDerived, LazyProduct>(derived(), a_diagonal.derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DIAGONALPRODUCT_H
+#endif  // EIGEN_DIAGONALPRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h
index 5c3441b..82eb9c7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h
@@ -10,309 +10,280 @@
 #ifndef EIGEN_DOT_H
 #define EIGEN_DOT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 // helper function for dot(). The problem is that if we put that in the body of dot(), then upon calling dot
 // with mismatched types, the compiler emits errors about failing to instantiate cwiseProduct BEFORE
 // looking at the static assertions. Thus this is a trick to get better compile errors.
-template<typename T, typename U,
-// the NeedToTranspose condition here is taken straight from Assign.h
-         bool NeedToTranspose = T::IsVectorAtCompileTime
-                && U::IsVectorAtCompileTime
-                && ((int(T::RowsAtCompileTime) == 1 && int(U::ColsAtCompileTime) == 1)
-                      |  // FIXME | instead of || to please GCC 4.4.0 stupid warning "suggest parentheses around &&".
-                         // revert to || as soon as not needed anymore.
-                    (int(T::ColsAtCompileTime) == 1 && int(U::RowsAtCompileTime) == 1))
->
-struct dot_nocheck
-{
-  typedef scalar_conj_product_op<typename traits<T>::Scalar,typename traits<U>::Scalar> conj_prod;
+template <typename T, typename U,
+          bool NeedToTranspose = T::IsVectorAtCompileTime && U::IsVectorAtCompileTime &&
+                                 ((int(T::RowsAtCompileTime) == 1 && int(U::ColsAtCompileTime) == 1) ||
+                                  (int(T::ColsAtCompileTime) == 1 && int(U::RowsAtCompileTime) == 1))>
+struct dot_nocheck {
+  typedef scalar_conj_product_op<typename traits<T>::Scalar, typename traits<U>::Scalar> conj_prod;
   typedef typename conj_prod::result_type ResScalar;
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE
-  static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b) {
     return a.template binaryExpr<conj_prod>(b).sum();
   }
 };
 
-template<typename T, typename U>
-struct dot_nocheck<T, U, true>
-{
-  typedef scalar_conj_product_op<typename traits<T>::Scalar,typename traits<U>::Scalar> conj_prod;
+template <typename T, typename U>
+struct dot_nocheck<T, U, true> {
+  typedef scalar_conj_product_op<typename traits<T>::Scalar, typename traits<U>::Scalar> conj_prod;
   typedef typename conj_prod::result_type ResScalar;
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE
-  static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b) {
     return a.transpose().template binaryExpr<conj_prod>(b).sum();
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \fn MatrixBase::dot
-  * \returns the dot product of *this with other.
-  *
-  * \only_for_vectors
-  *
-  * \note If the scalar type is complex numbers, then this function returns the hermitian
-  * (sesquilinear) dot product, conjugate-linear in the first variable and linear in the
-  * second variable.
-  *
-  * \sa squaredNorm(), norm()
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE
-typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
-MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
-{
+ * \returns the dot product of *this with other.
+ *
+ * \only_for_vectors
+ *
+ * \note If the scalar type is complex numbers, then this function returns the hermitian
+ * (sesquilinear) dot product, conjugate-linear in the first variable and linear in the
+ * second variable.
+ *
+ * \sa squaredNorm(), norm()
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,
+                                  typename internal::traits<OtherDerived>::Scalar>::ReturnType
+    MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived, OtherDerived)
 #if !(defined(EIGEN_NO_STATIC_ASSERT) && defined(EIGEN_NO_DEBUG))
-  typedef internal::scalar_conj_product_op<Scalar,typename OtherDerived::Scalar> func;
-  EIGEN_CHECK_BINARY_COMPATIBILIY(func,Scalar,typename OtherDerived::Scalar);
+  EIGEN_CHECK_BINARY_COMPATIBILIY(
+      Eigen::internal::scalar_conj_product_op<Scalar EIGEN_COMMA typename OtherDerived::Scalar>, Scalar,
+      typename OtherDerived::Scalar);
 #endif
-  
+
   eigen_assert(size() == other.size());
 
-  return internal::dot_nocheck<Derived,OtherDerived>::run(*this, other);
+  return internal::dot_nocheck<Derived, OtherDerived>::run(*this, other);
 }
 
 //---------- implementation of L2 norm and related functions ----------
 
 /** \returns, for vectors, the squared \em l2 norm of \c *this, and for matrices the squared Frobenius norm.
-  * In both cases, it consists in the sum of the square of all the matrix entries.
-  * For vectors, this is also equals to the dot product of \c *this with itself.
-  *
-  * \sa dot(), norm(), lpNorm()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::squaredNorm() const
-{
+ * In both cases, it consists in the sum of the square of all the matrix entries.
+ * For vectors, this is also equals to the dot product of \c *this with itself.
+ *
+ * \sa dot(), norm(), lpNorm()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+MatrixBase<Derived>::squaredNorm() const {
   return numext::real((*this).cwiseAbs2().sum());
 }
 
 /** \returns, for vectors, the \em l2 norm of \c *this, and for matrices the Frobenius norm.
-  * In both cases, it consists in the square root of the sum of the square of all the matrix entries.
-  * For vectors, this is also equals to the square root of the dot product of \c *this with itself.
-  *
-  * \sa lpNorm(), dot(), squaredNorm()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
-{
+ * In both cases, it consists in the square root of the sum of the square of all the matrix entries.
+ * For vectors, this is also equals to the square root of the dot product of \c *this with itself.
+ *
+ * \sa lpNorm(), dot(), squaredNorm()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+MatrixBase<Derived>::norm() const {
   return numext::sqrt(squaredNorm());
 }
 
 /** \returns an expression of the quotient of \c *this by its own norm.
-  *
-  * \warning If the input vector is too small (i.e., this->norm()==0),
-  *          then this function returns a copy of the input.
-  *
-  * \only_for_vectors
-  *
-  * \sa norm(), normalize()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
-MatrixBase<Derived>::normalized() const
-{
-  typedef typename internal::nested_eval<Derived,2>::type _Nested;
-  _Nested n(derived());
+ *
+ * \warning If the input vector is too small (i.e., this->norm()==0),
+ *          then this function returns a copy of the input.
+ *
+ * \only_for_vectors
+ *
+ * \sa norm(), normalize()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject MatrixBase<Derived>::normalized()
+    const {
+  typedef typename internal::nested_eval<Derived, 2>::type Nested_;
+  Nested_ n(derived());
   RealScalar z = n.squaredNorm();
   // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU
-  if(z>RealScalar(0))
+  if (z > RealScalar(0))
     return n / numext::sqrt(z);
   else
     return n;
 }
 
 /** Normalizes the vector, i.e. divides it by its own norm.
-  *
-  * \only_for_vectors
-  *
-  * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged.
-  *
-  * \sa norm(), normalized()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase<Derived>::normalize()
-{
+ *
+ * \only_for_vectors
+ *
+ * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged.
+ *
+ * \sa norm(), normalized()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase<Derived>::normalize() {
   RealScalar z = squaredNorm();
   // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU
-  if(z>RealScalar(0))
-    derived() /= numext::sqrt(z);
+  if (z > RealScalar(0)) derived() /= numext::sqrt(z);
 }
 
 /** \returns an expression of the quotient of \c *this by its own norm while avoiding underflow and overflow.
-  *
-  * \only_for_vectors
-  *
-  * This method is analogue to the normalized() method, but it reduces the risk of
-  * underflow and overflow when computing the norm.
-  *
-  * \warning If the input vector is too small (i.e., this->norm()==0),
-  *          then this function returns a copy of the input.
-  *
-  * \sa stableNorm(), stableNormalize(), normalized()
-  */
-template<typename Derived>
+ *
+ * \only_for_vectors
+ *
+ * This method is analogue to the normalized() method, but it reduces the risk of
+ * underflow and overflow when computing the norm.
+ *
+ * \warning If the input vector is too small (i.e., this->norm()==0),
+ *          then this function returns a copy of the input.
+ *
+ * \sa stableNorm(), stableNormalize(), normalized()
+ */
+template <typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
-MatrixBase<Derived>::stableNormalized() const
-{
-  typedef typename internal::nested_eval<Derived,3>::type _Nested;
-  _Nested n(derived());
+MatrixBase<Derived>::stableNormalized() const {
+  typedef typename internal::nested_eval<Derived, 3>::type Nested_;
+  Nested_ n(derived());
   RealScalar w = n.cwiseAbs().maxCoeff();
-  RealScalar z = (n/w).squaredNorm();
-  if(z>RealScalar(0))
-    return n / (numext::sqrt(z)*w);
+  RealScalar z = (n / w).squaredNorm();
+  if (z > RealScalar(0))
+    return n / (numext::sqrt(z) * w);
   else
     return n;
 }
 
 /** Normalizes the vector while avoid underflow and overflow
-  *
-  * \only_for_vectors
-  *
-  * This method is analogue to the normalize() method, but it reduces the risk of
-  * underflow and overflow when computing the norm.
-  *
-  * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged.
-  *
-  * \sa stableNorm(), stableNormalized(), normalize()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase<Derived>::stableNormalize()
-{
+ *
+ * \only_for_vectors
+ *
+ * This method is analogue to the normalize() method, but it reduces the risk of
+ * underflow and overflow when computing the norm.
+ *
+ * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged.
+ *
+ * \sa stableNorm(), stableNormalized(), normalize()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase<Derived>::stableNormalize() {
   RealScalar w = cwiseAbs().maxCoeff();
-  RealScalar z = (derived()/w).squaredNorm();
-  if(z>RealScalar(0))
-    derived() /= numext::sqrt(z)*w;
+  RealScalar z = (derived() / w).squaredNorm();
+  if (z > RealScalar(0)) derived() /= numext::sqrt(z) * w;
 }
 
 //---------- implementation of other norms ----------
 
 namespace internal {
 
-template<typename Derived, int p>
-struct lpNorm_selector
-{
+template <typename Derived, int p>
+struct lpNorm_selector {
   typedef typename NumTraits<typename traits<Derived>::Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const MatrixBase<Derived>& m)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const MatrixBase<Derived>& m) {
     EIGEN_USING_STD(pow)
-    return pow(m.cwiseAbs().array().pow(p).sum(), RealScalar(1)/p);
+    return pow(m.cwiseAbs().array().pow(p).sum(), RealScalar(1) / p);
   }
 };
 
-template<typename Derived>
-struct lpNorm_selector<Derived, 1>
-{
-  EIGEN_DEVICE_FUNC
-  static inline typename NumTraits<typename traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
-  {
+template <typename Derived>
+struct lpNorm_selector<Derived, 1> {
+  EIGEN_DEVICE_FUNC static inline typename NumTraits<typename traits<Derived>::Scalar>::Real run(
+      const MatrixBase<Derived>& m) {
     return m.cwiseAbs().sum();
   }
 };
 
-template<typename Derived>
-struct lpNorm_selector<Derived, 2>
-{
-  EIGEN_DEVICE_FUNC
-  static inline typename NumTraits<typename traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
-  {
+template <typename Derived>
+struct lpNorm_selector<Derived, 2> {
+  EIGEN_DEVICE_FUNC static inline typename NumTraits<typename traits<Derived>::Scalar>::Real run(
+      const MatrixBase<Derived>& m) {
     return m.norm();
   }
 };
 
-template<typename Derived>
-struct lpNorm_selector<Derived, Infinity>
-{
+template <typename Derived>
+struct lpNorm_selector<Derived, Infinity> {
   typedef typename NumTraits<typename traits<Derived>::Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const MatrixBase<Derived>& m)
-  {
-    if(Derived::SizeAtCompileTime==0 || (Derived::SizeAtCompileTime==Dynamic && m.size()==0))
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const MatrixBase<Derived>& m) {
+    if (Derived::SizeAtCompileTime == 0 || (Derived::SizeAtCompileTime == Dynamic && m.size() == 0))
       return RealScalar(0);
     return m.cwiseAbs().maxCoeff();
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-/** \returns the \b coefficient-wise \f$ \ell^p \f$ norm of \c *this, that is, returns the p-th root of the sum of the p-th powers of the absolute values
-  *          of the coefficients of \c *this. If \a p is the special value \a Eigen::Infinity, this function returns the \f$ \ell^\infty \f$
-  *          norm, that is the maximum of the absolute values of the coefficients of \c *this.
-  *
-  * In all cases, if \c *this is empty, then the value 0 is returned.
-  *
-  * \note For matrices, this function does not compute the <a href="https://en.wikipedia.org/wiki/Operator_norm">operator-norm</a>. That is, if \c *this is a matrix, then its coefficients are interpreted as a 1D vector. Nonetheless, you can easily compute the 1-norm and \f$\infty\f$-norm matrix operator norms using \link TutorialReductionsVisitorsBroadcastingReductionsNorm partial reductions \endlink.
-  *
-  * \sa norm()
-  */
-template<typename Derived>
-template<int p>
+/** \returns the \b coefficient-wise \f$ \ell^p \f$ norm of \c *this, that is, returns the p-th root of the sum of the
+ * p-th powers of the absolute values of the coefficients of \c *this. If \a p is the special value \a Eigen::Infinity,
+ * this function returns the \f$ \ell^\infty \f$ norm, that is the maximum of the absolute values of the coefficients of
+ * \c *this.
+ *
+ * In all cases, if \c *this is empty, then the value 0 is returned.
+ *
+ * \note For matrices, this function does not compute the <a
+ * href="https://en.wikipedia.org/wiki/Operator_norm">operator-norm</a>. That is, if \c *this is a matrix, then its
+ * coefficients are interpreted as a 1D vector. Nonetheless, you can easily compute the 1-norm and \f$\infty\f$-norm
+ * matrix operator norms using \link TutorialReductionsVisitorsBroadcastingReductionsNorm partial reductions \endlink.
+ *
+ * \sa norm()
+ */
+template <typename Derived>
+template <int p>
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 EIGEN_DEVICE_FUNC inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
 #else
 EIGEN_DEVICE_FUNC MatrixBase<Derived>::RealScalar
 #endif
-MatrixBase<Derived>::lpNorm() const
-{
+MatrixBase<Derived>::lpNorm() const {
   return internal::lpNorm_selector<Derived, p>::run(*this);
 }
 
 //---------- implementation of isOrthogonal / isUnitary ----------
 
 /** \returns true if *this is approximately orthogonal to \a other,
-  *          within the precision given by \a prec.
-  *
-  * Example: \include MatrixBase_isOrthogonal.cpp
-  * Output: \verbinclude MatrixBase_isOrthogonal.out
-  */
-template<typename Derived>
-template<typename OtherDerived>
-bool MatrixBase<Derived>::isOrthogonal
-(const MatrixBase<OtherDerived>& other, const RealScalar& prec) const
-{
-  typename internal::nested_eval<Derived,2>::type nested(derived());
-  typename internal::nested_eval<OtherDerived,2>::type otherNested(other.derived());
+ *          within the precision given by \a prec.
+ *
+ * Example: \include MatrixBase_isOrthogonal.cpp
+ * Output: \verbinclude MatrixBase_isOrthogonal.out
+ */
+template <typename Derived>
+template <typename OtherDerived>
+bool MatrixBase<Derived>::isOrthogonal(const MatrixBase<OtherDerived>& other, const RealScalar& prec) const {
+  typename internal::nested_eval<Derived, 2>::type nested(derived());
+  typename internal::nested_eval<OtherDerived, 2>::type otherNested(other.derived());
   return numext::abs2(nested.dot(otherNested)) <= prec * prec * nested.squaredNorm() * otherNested.squaredNorm();
 }
 
 /** \returns true if *this is approximately an unitary matrix,
-  *          within the precision given by \a prec. In the case where the \a Scalar
-  *          type is real numbers, a unitary matrix is an orthogonal matrix, whence the name.
-  *
-  * \note This can be used to check whether a family of vectors forms an orthonormal basis.
-  *       Indeed, \c m.isUnitary() returns true if and only if the columns (equivalently, the rows) of m form an
-  *       orthonormal basis.
-  *
-  * Example: \include MatrixBase_isUnitary.cpp
-  * Output: \verbinclude MatrixBase_isUnitary.out
-  */
-template<typename Derived>
-bool MatrixBase<Derived>::isUnitary(const RealScalar& prec) const
-{
-  typename internal::nested_eval<Derived,1>::type self(derived());
-  for(Index i = 0; i < cols(); ++i)
-  {
-    if(!internal::isApprox(self.col(i).squaredNorm(), static_cast<RealScalar>(1), prec))
-      return false;
-    for(Index j = 0; j < i; ++j)
-      if(!internal::isMuchSmallerThan(self.col(i).dot(self.col(j)), static_cast<Scalar>(1), prec))
-        return false;
+ *          within the precision given by \a prec. In the case where the \a Scalar
+ *          type is real numbers, a unitary matrix is an orthogonal matrix, whence the name.
+ *
+ * \note This can be used to check whether a family of vectors forms an orthonormal basis.
+ *       Indeed, \c m.isUnitary() returns true if and only if the columns (equivalently, the rows) of m form an
+ *       orthonormal basis.
+ *
+ * Example: \include MatrixBase_isUnitary.cpp
+ * Output: \verbinclude MatrixBase_isUnitary.out
+ */
+template <typename Derived>
+bool MatrixBase<Derived>::isUnitary(const RealScalar& prec) const {
+  typename internal::nested_eval<Derived, 1>::type self(derived());
+  for (Index i = 0; i < cols(); ++i) {
+    if (!internal::isApprox(self.col(i).squaredNorm(), static_cast<RealScalar>(1), prec)) return false;
+    for (Index j = 0; j < i; ++j)
+      if (!internal::isMuchSmallerThan(self.col(i).dot(self.col(j)), static_cast<Scalar>(1), prec)) return false;
   }
   return true;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DOT_H
+#endif  // EIGEN_DOT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h
index 6b3c7d3..f485016 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h
@@ -11,150 +11,134 @@
 #ifndef EIGEN_EIGENBASE_H
 #define EIGEN_EIGENBASE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class EigenBase
-  * \ingroup Core_Module
-  *
-  * Common base class for all classes T such that MatrixBase has an operator=(T) and a constructor MatrixBase(T).
-  *
-  * In other words, an EigenBase object is an object that can be copied into a MatrixBase.
-  *
-  * Besides MatrixBase-derived classes, this also includes special matrix classes such as diagonal matrices, etc.
-  *
-  * Notice that this class is trivial, it is only used to disambiguate overloaded functions.
-  *
-  * \sa \blank \ref TopicClassHierarchy
-  */
-template<typename Derived> struct EigenBase
-{
-//   typedef typename internal::plain_matrix_type<Derived>::type PlainObject;
+ * \ingroup Core_Module
+ *
+ * Common base class for all classes T such that MatrixBase has an operator=(T) and a constructor MatrixBase(T).
+ *
+ * In other words, an EigenBase object is an object that can be copied into a MatrixBase.
+ *
+ * Besides MatrixBase-derived classes, this also includes special matrix classes such as diagonal matrices, etc.
+ *
+ * Notice that this class is trivial, it is only used to disambiguate overloaded functions.
+ *
+ * \sa \blank \ref TopicClassHierarchy
+ */
+template <typename Derived>
+struct EigenBase {
+  //   typedef typename internal::plain_matrix_type<Derived>::type PlainObject;
 
   /** \brief The interface type of indices
-    * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE.
-    * \sa StorageIndex, \ref TopicPreprocessorDirectives.
-    * DEPRECATED: Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead.
-    * Deprecation is not marked with a doxygen comment because there are too many existing usages to add the deprecation attribute.
-    */
+   * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE.
+   * \sa StorageIndex, \ref TopicPreprocessorDirectives.
+   * DEPRECATED: Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead.
+   * Deprecation is not marked with a doxygen comment because there are too many existing usages to add the deprecation
+   * attribute.
+   */
   typedef Eigen::Index Index;
 
   // FIXME is it needed?
   typedef typename internal::traits<Derived>::StorageKind StorageKind;
 
   /** \returns a reference to the derived object */
-  EIGEN_DEVICE_FUNC
-  Derived& derived() { return *static_cast<Derived*>(this); }
+  EIGEN_DEVICE_FUNC Derived& derived() { return *static_cast<Derived*>(this); }
   /** \returns a const reference to the derived object */
-  EIGEN_DEVICE_FUNC
-  const Derived& derived() const { return *static_cast<const Derived*>(this); }
+  EIGEN_DEVICE_FUNC const Derived& derived() const { return *static_cast<const Derived*>(this); }
 
-  EIGEN_DEVICE_FUNC
-  inline Derived& const_cast_derived() const
-  { return *static_cast<Derived*>(const_cast<EigenBase*>(this)); }
-  EIGEN_DEVICE_FUNC
-  inline const Derived& const_derived() const
-  { return *static_cast<const Derived*>(this); }
+  EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const {
+    return *static_cast<Derived*>(const_cast<EigenBase*>(this));
+  }
+  EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast<const Derived*>(this); }
 
   /** \returns the number of rows. \sa cols(), RowsAtCompileTime */
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); }
   /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); }
   /** \returns the number of coefficients, which is rows()*cols().
-    * \sa rows(), cols(), SizeAtCompileTime. */
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  inline Index size() const EIGEN_NOEXCEPT { return rows() * cols(); }
+   * \sa rows(), cols(), SizeAtCompileTime. */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index size() const EIGEN_NOEXCEPT { return rows() * cols(); }
 
   /** \internal Don't use it, but do the equivalent: \code dst = *this; \endcode */
-  template<typename Dest>
-  EIGEN_DEVICE_FUNC
-  inline void evalTo(Dest& dst) const
-  { derived().evalTo(dst); }
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const {
+    derived().evalTo(dst);
+  }
 
   /** \internal Don't use it, but do the equivalent: \code dst += *this; \endcode */
-  template<typename Dest>
-  EIGEN_DEVICE_FUNC
-  inline void addTo(Dest& dst) const
-  {
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void addTo(Dest& dst) const {
     // This is the default implementation,
     // derived class can reimplement it in a more optimized way.
-    typename Dest::PlainObject res(rows(),cols());
+    typename Dest::PlainObject res(rows(), cols());
     evalTo(res);
     dst += res;
   }
 
   /** \internal Don't use it, but do the equivalent: \code dst -= *this; \endcode */
-  template<typename Dest>
-  EIGEN_DEVICE_FUNC
-  inline void subTo(Dest& dst) const
-  {
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void subTo(Dest& dst) const {
     // This is the default implementation,
     // derived class can reimplement it in a more optimized way.
-    typename Dest::PlainObject res(rows(),cols());
+    typename Dest::PlainObject res(rows(), cols());
     evalTo(res);
     dst -= res;
   }
 
   /** \internal Don't use it, but do the equivalent: \code dst.applyOnTheRight(*this); \endcode */
-  template<typename Dest>
-  EIGEN_DEVICE_FUNC inline void applyThisOnTheRight(Dest& dst) const
-  {
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void applyThisOnTheRight(Dest& dst) const {
     // This is the default implementation,
     // derived class can reimplement it in a more optimized way.
     dst = dst * this->derived();
   }
 
   /** \internal Don't use it, but do the equivalent: \code dst.applyOnTheLeft(*this); \endcode */
-  template<typename Dest>
-  EIGEN_DEVICE_FUNC inline void applyThisOnTheLeft(Dest& dst) const
-  {
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void applyThisOnTheLeft(Dest& dst) const {
     // This is the default implementation,
     // derived class can reimplement it in a more optimized way.
     dst = this->derived() * dst;
   }
-
 };
 
 /***************************************************************************
-* Implementation of matrix base methods
-***************************************************************************/
+ * Implementation of matrix base methods
+ ***************************************************************************/
 
 /** \brief Copies the generic expression \a other into *this.
-  *
-  * \details The expression must provide a (templated) evalTo(Derived& dst) const
-  * function which does the actual job. In practice, this allows any user to write
-  * its own special matrix without having to modify MatrixBase
-  *
-  * \returns a reference to *this.
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-Derived& DenseBase<Derived>::operator=(const EigenBase<OtherDerived> &other)
-{
+ *
+ * \details The expression must provide a (templated) evalTo(Derived& dst) const
+ * function which does the actual job. In practice, this allows any user to write
+ * its own special matrix without having to modify MatrixBase
+ *
+ * \returns a reference to *this.
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC Derived& DenseBase<Derived>::operator=(const EigenBase<OtherDerived>& other) {
   call_assignment(derived(), other.derived());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-Derived& DenseBase<Derived>::operator+=(const EigenBase<OtherDerived> &other)
-{
-  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC Derived& DenseBase<Derived>::operator+=(const EigenBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-Derived& DenseBase<Derived>::operator-=(const EigenBase<OtherDerived> &other)
-{
-  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC Derived& DenseBase<Derived>::operator-=(const EigenBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_EIGENBASE_H
+#endif  // EIGEN_EIGENBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h
index 817a43a..a91b0da 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h
@@ -10,141 +10,122 @@
 #ifndef EIGEN_FORCEALIGNEDACCESS_H
 #define EIGEN_FORCEALIGNEDACCESS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class ForceAlignedAccess
-  * \ingroup Core_Module
-  *
-  * \brief Enforce aligned packet loads and stores regardless of what is requested
-  *
-  * \param ExpressionType the type of the object of which we are forcing aligned packet access
-  *
-  * This class is the return type of MatrixBase::forceAlignedAccess()
-  * and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::forceAlignedAccess()
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Enforce aligned packet loads and stores regardless of what is requested
+ *
+ * \param ExpressionType the type of the object of which we are forcing aligned packet access
+ *
+ * This class is the return type of MatrixBase::forceAlignedAccess()
+ * and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::forceAlignedAccess()
+ */
 
 namespace internal {
-template<typename ExpressionType>
-struct traits<ForceAlignedAccess<ExpressionType> > : public traits<ExpressionType>
-{};
-}
+template <typename ExpressionType>
+struct traits<ForceAlignedAccess<ExpressionType>> : public traits<ExpressionType> {};
+}  // namespace internal
 
-template<typename ExpressionType> class ForceAlignedAccess
-  : public internal::dense_xpr_base< ForceAlignedAccess<ExpressionType> >::type
-{
-  public:
+template <typename ExpressionType>
+class ForceAlignedAccess : public internal::dense_xpr_base<ForceAlignedAccess<ExpressionType>>::type {
+ public:
+  typedef typename internal::dense_xpr_base<ForceAlignedAccess>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(ForceAlignedAccess)
 
-    typedef typename internal::dense_xpr_base<ForceAlignedAccess>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(ForceAlignedAccess)
+  EIGEN_DEVICE_FUNC explicit inline ForceAlignedAccess(const ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC explicit inline ForceAlignedAccess(const ExpressionType& matrix) : m_expression(matrix) {}
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT {
+    return m_expression.outerStride();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT {
+    return m_expression.innerStride();
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return m_expression.outerStride(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT { return m_expression.innerStride(); }
+  EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const {
+    return m_expression.coeff(row, col);
+  }
 
-    EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const
-    {
-      return m_expression.coeff(row, col);
-    }
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) {
+    return m_expression.const_cast_derived().coeffRef(row, col);
+  }
 
-    EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col)
-    {
-      return m_expression.const_cast_derived().coeffRef(row, col);
-    }
+  EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const { return m_expression.coeff(index); }
 
-    EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const
-    {
-      return m_expression.coeff(index);
-    }
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) { return m_expression.const_cast_derived().coeffRef(index); }
 
-    EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index)
-    {
-      return m_expression.const_cast_derived().coeffRef(index);
-    }
+  template <int LoadMode>
+  inline const PacketScalar packet(Index row, Index col) const {
+    return m_expression.template packet<Aligned>(row, col);
+  }
 
-    template<int LoadMode>
-    inline const PacketScalar packet(Index row, Index col) const
-    {
-      return m_expression.template packet<Aligned>(row, col);
-    }
+  template <int LoadMode>
+  inline void writePacket(Index row, Index col, const PacketScalar& x) {
+    m_expression.const_cast_derived().template writePacket<Aligned>(row, col, x);
+  }
 
-    template<int LoadMode>
-    inline void writePacket(Index row, Index col, const PacketScalar& x)
-    {
-      m_expression.const_cast_derived().template writePacket<Aligned>(row, col, x);
-    }
+  template <int LoadMode>
+  inline const PacketScalar packet(Index index) const {
+    return m_expression.template packet<Aligned>(index);
+  }
 
-    template<int LoadMode>
-    inline const PacketScalar packet(Index index) const
-    {
-      return m_expression.template packet<Aligned>(index);
-    }
+  template <int LoadMode>
+  inline void writePacket(Index index, const PacketScalar& x) {
+    m_expression.const_cast_derived().template writePacket<Aligned>(index, x);
+  }
 
-    template<int LoadMode>
-    inline void writePacket(Index index, const PacketScalar& x)
-    {
-      m_expression.const_cast_derived().template writePacket<Aligned>(index, x);
-    }
+  EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; }
 
-    EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; }
+ protected:
+  const ExpressionType& m_expression;
 
-  protected:
-    const ExpressionType& m_expression;
-
-  private:
-    ForceAlignedAccess& operator=(const ForceAlignedAccess&);
+ private:
+  ForceAlignedAccess& operator=(const ForceAlignedAccess&);
 };
 
 /** \returns an expression of *this with forced aligned access
-  * \sa forceAlignedAccessIf(),class ForceAlignedAccess
-  */
-template<typename Derived>
-inline const ForceAlignedAccess<Derived>
-MatrixBase<Derived>::forceAlignedAccess() const
-{
+ * \sa forceAlignedAccessIf(),class ForceAlignedAccess
+ */
+template <typename Derived>
+inline const ForceAlignedAccess<Derived> MatrixBase<Derived>::forceAlignedAccess() const {
   return ForceAlignedAccess<Derived>(derived());
 }
 
 /** \returns an expression of *this with forced aligned access
-  * \sa forceAlignedAccessIf(), class ForceAlignedAccess
-  */
-template<typename Derived>
-inline ForceAlignedAccess<Derived>
-MatrixBase<Derived>::forceAlignedAccess()
-{
+ * \sa forceAlignedAccessIf(), class ForceAlignedAccess
+ */
+template <typename Derived>
+inline ForceAlignedAccess<Derived> MatrixBase<Derived>::forceAlignedAccess() {
   return ForceAlignedAccess<Derived>(derived());
 }
 
 /** \returns an expression of *this with forced aligned access if \a Enable is true.
-  * \sa forceAlignedAccess(), class ForceAlignedAccess
-  */
-template<typename Derived>
-template<bool Enable>
-inline typename internal::add_const_on_value_type<typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type>::type
-MatrixBase<Derived>::forceAlignedAccessIf() const
-{
+ * \sa forceAlignedAccess(), class ForceAlignedAccess
+ */
+template <typename Derived>
+template <bool Enable>
+inline add_const_on_value_type_t<std::conditional_t<Enable, ForceAlignedAccess<Derived>, Derived&>>
+MatrixBase<Derived>::forceAlignedAccessIf() const {
   return derived();  // FIXME This should not work but apparently is never used
 }
 
 /** \returns an expression of *this with forced aligned access if \a Enable is true.
-  * \sa forceAlignedAccess(), class ForceAlignedAccess
-  */
-template<typename Derived>
-template<bool Enable>
-inline typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type
-MatrixBase<Derived>::forceAlignedAccessIf()
-{
+ * \sa forceAlignedAccess(), class ForceAlignedAccess
+ */
+template <typename Derived>
+template <bool Enable>
+inline std::conditional_t<Enable, ForceAlignedAccess<Derived>, Derived&> MatrixBase<Derived>::forceAlignedAccessIf() {
   return derived();  // FIXME This should not work but apparently is never used
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_FORCEALIGNEDACCESS_H
+#endif  // EIGEN_FORCEALIGNEDACCESS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h
index 43aa49b..ed6b4ff 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h
@@ -11,145 +11,122 @@
 #ifndef EIGEN_FUZZY_H
 #define EIGEN_FUZZY_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-namespace internal
-{
+namespace Eigen {
 
-template<typename Derived, typename OtherDerived, bool is_integer = NumTraits<typename Derived::Scalar>::IsInteger>
-struct isApprox_selector
-{
-  EIGEN_DEVICE_FUNC
-  static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec)
-  {
-    typename internal::nested_eval<Derived,2>::type nested(x);
-    typename internal::nested_eval<OtherDerived,2>::type otherNested(y);
-    return (nested - otherNested).cwiseAbs2().sum() <= prec * prec * numext::mini(nested.cwiseAbs2().sum(), otherNested.cwiseAbs2().sum());
+namespace internal {
+
+template <typename Derived, typename OtherDerived, bool is_integer = NumTraits<typename Derived::Scalar>::IsInteger>
+struct isApprox_selector {
+  EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) {
+    typename internal::nested_eval<Derived, 2>::type nested(x);
+    typename internal::nested_eval<OtherDerived, 2>::type otherNested(y);
+    return (nested.matrix() - otherNested.matrix()).cwiseAbs2().sum() <=
+           prec * prec * numext::mini(nested.cwiseAbs2().sum(), otherNested.cwiseAbs2().sum());
   }
 };
 
-template<typename Derived, typename OtherDerived>
-struct isApprox_selector<Derived, OtherDerived, true>
-{
-  EIGEN_DEVICE_FUNC
-  static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar&)
-  {
+template <typename Derived, typename OtherDerived>
+struct isApprox_selector<Derived, OtherDerived, true> {
+  EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar&) {
     return x.matrix() == y.matrix();
   }
 };
 
-template<typename Derived, typename OtherDerived, bool is_integer = NumTraits<typename Derived::Scalar>::IsInteger>
-struct isMuchSmallerThan_object_selector
-{
-  EIGEN_DEVICE_FUNC
-  static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec)
-  {
+template <typename Derived, typename OtherDerived, bool is_integer = NumTraits<typename Derived::Scalar>::IsInteger>
+struct isMuchSmallerThan_object_selector {
+  EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) {
     return x.cwiseAbs2().sum() <= numext::abs2(prec) * y.cwiseAbs2().sum();
   }
 };
 
-template<typename Derived, typename OtherDerived>
-struct isMuchSmallerThan_object_selector<Derived, OtherDerived, true>
-{
-  EIGEN_DEVICE_FUNC
-  static bool run(const Derived& x, const OtherDerived&, const typename Derived::RealScalar&)
-  {
+template <typename Derived, typename OtherDerived>
+struct isMuchSmallerThan_object_selector<Derived, OtherDerived, true> {
+  EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived&, const typename Derived::RealScalar&) {
     return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix();
   }
 };
 
-template<typename Derived, bool is_integer = NumTraits<typename Derived::Scalar>::IsInteger>
-struct isMuchSmallerThan_scalar_selector
-{
-  EIGEN_DEVICE_FUNC
-  static bool run(const Derived& x, const typename Derived::RealScalar& y, const typename Derived::RealScalar& prec)
-  {
+template <typename Derived, bool is_integer = NumTraits<typename Derived::Scalar>::IsInteger>
+struct isMuchSmallerThan_scalar_selector {
+  EIGEN_DEVICE_FUNC static bool run(const Derived& x, const typename Derived::RealScalar& y,
+                                    const typename Derived::RealScalar& prec) {
     return x.cwiseAbs2().sum() <= numext::abs2(prec * y);
   }
 };
 
-template<typename Derived>
-struct isMuchSmallerThan_scalar_selector<Derived, true>
-{
-  EIGEN_DEVICE_FUNC
-  static bool run(const Derived& x, const typename Derived::RealScalar&, const typename Derived::RealScalar&)
-  {
+template <typename Derived>
+struct isMuchSmallerThan_scalar_selector<Derived, true> {
+  EIGEN_DEVICE_FUNC static bool run(const Derived& x, const typename Derived::RealScalar&,
+                                    const typename Derived::RealScalar&) {
     return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix();
   }
 };
 
-} // end namespace internal
-
+}  // end namespace internal
 
 /** \returns \c true if \c *this is approximately equal to \a other, within the precision
-  * determined by \a prec.
-  *
-  * \note The fuzzy compares are done multiplicatively. Two vectors \f$ v \f$ and \f$ w \f$
-  * are considered to be approximately equal within precision \f$ p \f$ if
-  * \f[ \Vert v - w \Vert \leqslant p\,\min(\Vert v\Vert, \Vert w\Vert). \f]
-  * For matrices, the comparison is done using the Hilbert-Schmidt norm (aka Frobenius norm
-  * L2 norm).
-  *
-  * \note Because of the multiplicativeness of this comparison, one can't use this function
-  * to check whether \c *this is approximately equal to the zero matrix or vector.
-  * Indeed, \c isApprox(zero) returns false unless \c *this itself is exactly the zero matrix
-  * or vector. If you want to test whether \c *this is zero, use internal::isMuchSmallerThan(const
-  * RealScalar&, RealScalar) instead.
-  *
-  * \sa internal::isMuchSmallerThan(const RealScalar&, RealScalar) const
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isApprox(
-  const DenseBase<OtherDerived>& other,
-  const RealScalar& prec
-) const
-{
+ * determined by \a prec.
+ *
+ * \note The fuzzy compares are done multiplicatively. Two vectors \f$ v \f$ and \f$ w \f$
+ * are considered to be approximately equal within precision \f$ p \f$ if
+ * \f[ \Vert v - w \Vert \leqslant p\,\min(\Vert v\Vert, \Vert w\Vert). \f]
+ * For matrices, the comparison is done using the Hilbert-Schmidt norm (aka Frobenius norm
+ * L2 norm).
+ *
+ * \note Because of the multiplicativeness of this comparison, one can't use this function
+ * to check whether \c *this is approximately equal to the zero matrix or vector.
+ * Indeed, \c isApprox(zero) returns false unless \c *this itself is exactly the zero matrix
+ * or vector. If you want to test whether \c *this is zero, use internal::isMuchSmallerThan(const
+ * RealScalar&, RealScalar) instead.
+ *
+ * \sa internal::isMuchSmallerThan(const RealScalar&, RealScalar) const
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isApprox(const DenseBase<OtherDerived>& other,
+                                                    const RealScalar& prec) const {
   return internal::isApprox_selector<Derived, OtherDerived>::run(derived(), other.derived(), prec);
 }
 
 /** \returns \c true if the norm of \c *this is much smaller than \a other,
-  * within the precision determined by \a prec.
-  *
-  * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
-  * considered to be much smaller than \f$ x \f$ within precision \f$ p \f$ if
-  * \f[ \Vert v \Vert \leqslant p\,\vert x\vert. \f]
-  *
-  * For matrices, the comparison is done using the Hilbert-Schmidt norm. For this reason,
-  * the value of the reference scalar \a other should come from the Hilbert-Schmidt norm
-  * of a reference matrix of same dimensions.
-  *
-  * \sa isApprox(), isMuchSmallerThan(const DenseBase<OtherDerived>&, RealScalar) const
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isMuchSmallerThan(
-  const typename NumTraits<Scalar>::Real& other,
-  const RealScalar& prec
-) const
-{
+ * within the precision determined by \a prec.
+ *
+ * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
+ * considered to be much smaller than \f$ x \f$ within precision \f$ p \f$ if
+ * \f[ \Vert v \Vert \leqslant p\,\vert x\vert. \f]
+ *
+ * For matrices, the comparison is done using the Hilbert-Schmidt norm. For this reason,
+ * the value of the reference scalar \a other should come from the Hilbert-Schmidt norm
+ * of a reference matrix of same dimensions.
+ *
+ * \sa isApprox(), isMuchSmallerThan(const DenseBase<OtherDerived>&, RealScalar) const
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isMuchSmallerThan(const typename NumTraits<Scalar>::Real& other,
+                                                             const RealScalar& prec) const {
   return internal::isMuchSmallerThan_scalar_selector<Derived>::run(derived(), other, prec);
 }
 
 /** \returns \c true if the norm of \c *this is much smaller than the norm of \a other,
-  * within the precision determined by \a prec.
-  *
-  * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
-  * considered to be much smaller than a vector \f$ w \f$ within precision \f$ p \f$ if
-  * \f[ \Vert v \Vert \leqslant p\,\Vert w\Vert. \f]
-  * For matrices, the comparison is done using the Hilbert-Schmidt norm.
-  *
-  * \sa isApprox(), isMuchSmallerThan(const RealScalar&, RealScalar) const
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isMuchSmallerThan(
-  const DenseBase<OtherDerived>& other,
-  const RealScalar& prec
-) const
-{
+ * within the precision determined by \a prec.
+ *
+ * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
+ * considered to be much smaller than a vector \f$ w \f$ within precision \f$ p \f$ if
+ * \f[ \Vert v \Vert \leqslant p\,\Vert w\Vert. \f]
+ * For matrices, the comparison is done using the Hilbert-Schmidt norm.
+ *
+ * \sa isApprox(), isMuchSmallerThan(const RealScalar&, RealScalar) const
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isMuchSmallerThan(const DenseBase<OtherDerived>& other,
+                                                             const RealScalar& prec) const {
   return internal::isMuchSmallerThan_object_selector<Derived, OtherDerived>::run(derived(), other.derived(), prec);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_FUZZY_H
+#endif  // EIGEN_FUZZY_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h
index 6906aa7..3ec6852 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h
@@ -11,12 +11,12 @@
 #ifndef EIGEN_GENERAL_PRODUCT_H
 #define EIGEN_GENERAL_PRODUCT_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-enum {
-  Large = 2,
-  Small = 3
-};
+enum { Large = 2, Small = 3 };
 
 // Define the threshold value to fallback from the generic matrix-matrix product
 // implementation (heavy) to the lightweight coeff-based product one.
@@ -30,64 +30,58 @@
 
 namespace internal {
 
-template<int Rows, int Cols, int Depth> struct product_type_selector;
+template <int Rows, int Cols, int Depth>
+struct product_type_selector;
 
-template<int Size, int MaxSize> struct product_size_category
-{
+template <int Size, int MaxSize>
+struct product_size_category {
   enum {
-    #ifndef EIGEN_GPU_COMPILE_PHASE
-    is_large = MaxSize == Dynamic ||
-               Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD ||
-               (Size==Dynamic && MaxSize>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD),
-    #else
+#ifndef EIGEN_GPU_COMPILE_PHASE
+    is_large = MaxSize == Dynamic || Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD ||
+               (Size == Dynamic && MaxSize >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD),
+#else
     is_large = 0,
-    #endif
-    value = is_large  ? Large
-          : Size == 1 ? 1
-                      : Small
+#endif
+    value = is_large    ? Large
+            : Size == 1 ? 1
+                        : Small
   };
 };
 
-template<typename Lhs, typename Rhs> struct product_type
-{
-  typedef typename remove_all<Lhs>::type _Lhs;
-  typedef typename remove_all<Rhs>::type _Rhs;
+template <typename Lhs, typename Rhs>
+struct product_type {
+  typedef remove_all_t<Lhs> Lhs_;
+  typedef remove_all_t<Rhs> Rhs_;
   enum {
-    MaxRows = traits<_Lhs>::MaxRowsAtCompileTime,
-    Rows    = traits<_Lhs>::RowsAtCompileTime,
-    MaxCols = traits<_Rhs>::MaxColsAtCompileTime,
-    Cols    = traits<_Rhs>::ColsAtCompileTime,
-    MaxDepth = EIGEN_SIZE_MIN_PREFER_FIXED(traits<_Lhs>::MaxColsAtCompileTime,
-                                           traits<_Rhs>::MaxRowsAtCompileTime),
-    Depth = EIGEN_SIZE_MIN_PREFER_FIXED(traits<_Lhs>::ColsAtCompileTime,
-                                        traits<_Rhs>::RowsAtCompileTime)
+    MaxRows = traits<Lhs_>::MaxRowsAtCompileTime,
+    Rows = traits<Lhs_>::RowsAtCompileTime,
+    MaxCols = traits<Rhs_>::MaxColsAtCompileTime,
+    Cols = traits<Rhs_>::ColsAtCompileTime,
+    MaxDepth = min_size_prefer_fixed(traits<Lhs_>::MaxColsAtCompileTime, traits<Rhs_>::MaxRowsAtCompileTime),
+    Depth = min_size_prefer_fixed(traits<Lhs_>::ColsAtCompileTime, traits<Rhs_>::RowsAtCompileTime)
   };
 
   // the splitting into different lines of code here, introducing the _select enums and the typedef below,
   // is to work around an internal compiler error with gcc 4.1 and 4.2.
-private:
+ private:
   enum {
-    rows_select = product_size_category<Rows,MaxRows>::value,
-    cols_select = product_size_category<Cols,MaxCols>::value,
-    depth_select = product_size_category<Depth,MaxDepth>::value
+    rows_select = product_size_category<Rows, MaxRows>::value,
+    cols_select = product_size_category<Cols, MaxCols>::value,
+    depth_select = product_size_category<Depth, MaxDepth>::value
   };
   typedef product_type_selector<rows_select, cols_select, depth_select> selector;
 
-public:
-  enum {
-    value = selector::ret,
-    ret = selector::ret
-  };
+ public:
+  enum { value = selector::ret, ret = selector::ret };
 #ifdef EIGEN_DEBUG_PRODUCT
-  static void debug()
-  {
-      EIGEN_DEBUG_VAR(Rows);
-      EIGEN_DEBUG_VAR(Cols);
-      EIGEN_DEBUG_VAR(Depth);
-      EIGEN_DEBUG_VAR(rows_select);
-      EIGEN_DEBUG_VAR(cols_select);
-      EIGEN_DEBUG_VAR(depth_select);
-      EIGEN_DEBUG_VAR(value);
+  static void debug() {
+    EIGEN_DEBUG_VAR(Rows);
+    EIGEN_DEBUG_VAR(Cols);
+    EIGEN_DEBUG_VAR(Depth);
+    EIGEN_DEBUG_VAR(rows_select);
+    EIGEN_DEBUG_VAR(cols_select);
+    EIGEN_DEBUG_VAR(depth_select);
+    EIGEN_DEBUG_VAR(value);
   }
 #endif
 };
@@ -96,36 +90,108 @@
  * based on the three dimensions of the product.
  * This is a compile time mapping from {1,Small,Large}^3 -> {product types} */
 // FIXME I'm not sure the current mapping is the ideal one.
-template<int M, int N>  struct product_type_selector<M,N,1>              { enum { ret = OuterProduct }; };
-template<int M>         struct product_type_selector<M, 1, 1>            { enum { ret = LazyCoeffBasedProductMode }; };
-template<int N>         struct product_type_selector<1, N, 1>            { enum { ret = LazyCoeffBasedProductMode }; };
-template<int Depth>     struct product_type_selector<1,    1,    Depth>  { enum { ret = InnerProduct }; };
-template<>              struct product_type_selector<1,    1,    1>      { enum { ret = InnerProduct }; };
-template<>              struct product_type_selector<Small,1,    Small>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<1,    Small,Small>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<Small,Small,Small>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<Small, Small, 1>    { enum { ret = LazyCoeffBasedProductMode }; };
-template<>              struct product_type_selector<Small, Large, 1>    { enum { ret = LazyCoeffBasedProductMode }; };
-template<>              struct product_type_selector<Large, Small, 1>    { enum { ret = LazyCoeffBasedProductMode }; };
-template<>              struct product_type_selector<1,    Large,Small>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<1,    Large,Large>  { enum { ret = GemvProduct }; };
-template<>              struct product_type_selector<1,    Small,Large>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<Large,1,    Small>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<Large,1,    Large>  { enum { ret = GemvProduct }; };
-template<>              struct product_type_selector<Small,1,    Large>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<Small,Small,Large>  { enum { ret = GemmProduct }; };
-template<>              struct product_type_selector<Large,Small,Large>  { enum { ret = GemmProduct }; };
-template<>              struct product_type_selector<Small,Large,Large>  { enum { ret = GemmProduct }; };
-template<>              struct product_type_selector<Large,Large,Large>  { enum { ret = GemmProduct }; };
-template<>              struct product_type_selector<Large,Small,Small>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<Small,Large,Small>  { enum { ret = CoeffBasedProductMode }; };
-template<>              struct product_type_selector<Large,Large,Small>  { enum { ret = GemmProduct }; };
+template <int M, int N>
+struct product_type_selector<M, N, 1> {
+  enum { ret = OuterProduct };
+};
+template <int M>
+struct product_type_selector<M, 1, 1> {
+  enum { ret = LazyCoeffBasedProductMode };
+};
+template <int N>
+struct product_type_selector<1, N, 1> {
+  enum { ret = LazyCoeffBasedProductMode };
+};
+template <int Depth>
+struct product_type_selector<1, 1, Depth> {
+  enum { ret = InnerProduct };
+};
+template <>
+struct product_type_selector<1, 1, 1> {
+  enum { ret = InnerProduct };
+};
+template <>
+struct product_type_selector<Small, 1, Small> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<1, Small, Small> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Small, Small, Small> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Small, Small, 1> {
+  enum { ret = LazyCoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Small, Large, 1> {
+  enum { ret = LazyCoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Large, Small, 1> {
+  enum { ret = LazyCoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<1, Large, Small> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<1, Large, Large> {
+  enum { ret = GemvProduct };
+};
+template <>
+struct product_type_selector<1, Small, Large> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Large, 1, Small> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Large, 1, Large> {
+  enum { ret = GemvProduct };
+};
+template <>
+struct product_type_selector<Small, 1, Large> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Small, Small, Large> {
+  enum { ret = GemmProduct };
+};
+template <>
+struct product_type_selector<Large, Small, Large> {
+  enum { ret = GemmProduct };
+};
+template <>
+struct product_type_selector<Small, Large, Large> {
+  enum { ret = GemmProduct };
+};
+template <>
+struct product_type_selector<Large, Large, Large> {
+  enum { ret = GemmProduct };
+};
+template <>
+struct product_type_selector<Large, Small, Small> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Small, Large, Small> {
+  enum { ret = CoeffBasedProductMode };
+};
+template <>
+struct product_type_selector<Large, Large, Small> {
+  enum { ret = GemmProduct };
+};
 
-} // end namespace internal
+}  // end namespace internal
 
 /***********************************************************************
-*  Implementation of Inner Vector Vector Product
-***********************************************************************/
+ *  Implementation of Inner Vector Vector Product
+ ***********************************************************************/
 
 // FIXME : maybe the "inner product" could return a Scalar
 // instead of a 1x1 matrix ??
@@ -135,12 +201,12 @@
 // case, we could have a specialization for Block<MatrixType,1,1> with: operator=(Scalar x);
 
 /***********************************************************************
-*  Implementation of Outer Vector Vector Product
-***********************************************************************/
+ *  Implementation of Outer Vector Vector Product
+ ***********************************************************************/
 
 /***********************************************************************
-*  Implementation of General Matrix Vector Product
-***********************************************************************/
+ *  Implementation of General Matrix Vector Product
+ ***********************************************************************/
 
 /*  According to the shape/flags of the matrix we have to distinghish 3 different cases:
  *   1 - the matrix is col-major, BLAS compatible and M is large => call fast BLAS-like colmajor routine
@@ -151,79 +217,82 @@
  */
 namespace internal {
 
-template<int Side, int StorageOrder, bool BlasCompatible>
+template <int Side, int StorageOrder, bool BlasCompatible>
 struct gemv_dense_selector;
 
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
-template<typename Scalar,int Size,int MaxSize,bool Cond> struct gemv_static_vector_if;
+template <typename Scalar, int Size, int MaxSize, bool Cond>
+struct gemv_static_vector_if;
 
-template<typename Scalar,int Size,int MaxSize>
-struct gemv_static_vector_if<Scalar,Size,MaxSize,false>
-{
-  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Scalar* data() { eigen_internal_assert(false && "should never be called"); return 0; }
+template <typename Scalar, int Size, int MaxSize>
+struct gemv_static_vector_if<Scalar, Size, MaxSize, false> {
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Scalar* data() {
+    eigen_internal_assert(false && "should never be called");
+    return 0;
+  }
 };
 
-template<typename Scalar,int Size>
-struct gemv_static_vector_if<Scalar,Size,Dynamic,true>
-{
+template <typename Scalar, int Size>
+struct gemv_static_vector_if<Scalar, Size, Dynamic, true> {
   EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Scalar* data() { return 0; }
 };
 
-template<typename Scalar,int Size,int MaxSize>
-struct gemv_static_vector_if<Scalar,Size,MaxSize,true>
-{
+template <typename Scalar, int Size, int MaxSize>
+struct gemv_static_vector_if<Scalar, Size, MaxSize, true> {
   enum {
-    ForceAlignment  = internal::packet_traits<Scalar>::Vectorizable,
-    PacketSize      = internal::packet_traits<Scalar>::size
+    ForceAlignment = internal::packet_traits<Scalar>::Vectorizable,
+    PacketSize = internal::packet_traits<Scalar>::size
   };
-  #if EIGEN_MAX_STATIC_ALIGN_BYTES!=0
-  internal::plain_array<Scalar,EIGEN_SIZE_MIN_PREFER_FIXED(Size,MaxSize),0,EIGEN_PLAIN_ENUM_MIN(AlignedMax,PacketSize)> m_data;
+#if EIGEN_MAX_STATIC_ALIGN_BYTES != 0
+  internal::plain_array<Scalar, internal::min_size_prefer_fixed(Size, MaxSize), 0,
+                        internal::plain_enum_min(AlignedMax, PacketSize)>
+      m_data;
   EIGEN_STRONG_INLINE Scalar* data() { return m_data.array; }
-  #else
+#else
   // Some architectures cannot align on the stack,
   // => let's manually enforce alignment by allocating more data and return the address of the first aligned element.
-  internal::plain_array<Scalar,EIGEN_SIZE_MIN_PREFER_FIXED(Size,MaxSize)+(ForceAlignment?EIGEN_MAX_ALIGN_BYTES:0),0> m_data;
+  internal::plain_array<
+      Scalar, internal::min_size_prefer_fixed(Size, MaxSize) + (ForceAlignment ? EIGEN_MAX_ALIGN_BYTES : 0), 0>
+      m_data;
   EIGEN_STRONG_INLINE Scalar* data() {
     return ForceAlignment
-            ? reinterpret_cast<Scalar*>((internal::UIntPtr(m_data.array) & ~(std::size_t(EIGEN_MAX_ALIGN_BYTES-1))) + EIGEN_MAX_ALIGN_BYTES)
-            : m_data.array;
+               ? reinterpret_cast<Scalar*>((std::uintptr_t(m_data.array) & ~(std::size_t(EIGEN_MAX_ALIGN_BYTES - 1))) +
+                                           EIGEN_MAX_ALIGN_BYTES)
+               : m_data.array;
   }
-  #endif
+#endif
 };
 
 // The vector is on the left => transposition
-template<int StorageOrder, bool BlasCompatible>
-struct gemv_dense_selector<OnTheLeft,StorageOrder,BlasCompatible>
-{
-  template<typename Lhs, typename Rhs, typename Dest>
-  static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha)
-  {
+template <int StorageOrder, bool BlasCompatible>
+struct gemv_dense_selector<OnTheLeft, StorageOrder, BlasCompatible> {
+  template <typename Lhs, typename Rhs, typename Dest>
+  static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) {
     Transpose<Dest> destT(dest);
     enum { OtherStorageOrder = StorageOrder == RowMajor ? ColMajor : RowMajor };
-    gemv_dense_selector<OnTheRight,OtherStorageOrder,BlasCompatible>
-      ::run(rhs.transpose(), lhs.transpose(), destT, alpha);
+    gemv_dense_selector<OnTheRight, OtherStorageOrder, BlasCompatible>::run(rhs.transpose(), lhs.transpose(), destT,
+                                                                            alpha);
   }
 };
 
-template<> struct gemv_dense_selector<OnTheRight,ColMajor,true>
-{
-  template<typename Lhs, typename Rhs, typename Dest>
-  static inline void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha)
-  {
-    typedef typename Lhs::Scalar   LhsScalar;
-    typedef typename Rhs::Scalar   RhsScalar;
-    typedef typename Dest::Scalar  ResScalar;
-    typedef typename Dest::RealScalar  RealScalar;
-    
+template <>
+struct gemv_dense_selector<OnTheRight, ColMajor, true> {
+  template <typename Lhs, typename Rhs, typename Dest>
+  static inline void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) {
+    typedef typename Lhs::Scalar LhsScalar;
+    typedef typename Rhs::Scalar RhsScalar;
+    typedef typename Dest::Scalar ResScalar;
+
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
     typedef internal::blas_traits<Rhs> RhsBlasTraits;
     typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-  
-    typedef Map<Matrix<ResScalar,Dynamic,1>, EIGEN_PLAIN_ENUM_MIN(AlignedMax,internal::packet_traits<ResScalar>::size)> MappedDest;
+
+    typedef Map<Matrix<ResScalar, Dynamic, 1>, plain_enum_min(AlignedMax, internal::packet_traits<ResScalar>::size)>
+        MappedDest;
 
     ActualLhsType actualLhs = LhsBlasTraits::extract(lhs);
     ActualRhsType actualRhs = RhsBlasTraits::extract(rhs);
@@ -231,68 +300,63 @@
     ResScalar actualAlpha = combine_scalar_factors(alpha, lhs, rhs);
 
     // make sure Dest is a compile-time vector type (bug 1166)
-    typedef typename conditional<Dest::IsVectorAtCompileTime, Dest, typename Dest::ColXpr>::type ActualDest;
+    typedef std::conditional_t<Dest::IsVectorAtCompileTime, Dest, typename Dest::ColXpr> ActualDest;
 
     enum {
       // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
       // on, the other hand it is good for the cache to pack the vector anyways...
-      EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime==1),
+      EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime == 1),
       ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex),
-      MightCannotUseDest = ((!EvalToDestAtCompileTime) || ComplexByReal) && (ActualDest::MaxSizeAtCompileTime!=0)
+      MightCannotUseDest = ((!EvalToDestAtCompileTime) || ComplexByReal) && (ActualDest::MaxSizeAtCompileTime != 0)
     };
 
-    typedef const_blas_data_mapper<LhsScalar,Index,ColMajor> LhsMapper;
-    typedef const_blas_data_mapper<RhsScalar,Index,RowMajor> RhsMapper;
-    RhsScalar compatibleAlpha = get_factor<ResScalar,RhsScalar>::run(actualAlpha);
+    typedef const_blas_data_mapper<LhsScalar, Index, ColMajor> LhsMapper;
+    typedef const_blas_data_mapper<RhsScalar, Index, RowMajor> RhsMapper;
+    RhsScalar compatibleAlpha = get_factor<ResScalar, RhsScalar>::run(actualAlpha);
 
-    if(!MightCannotUseDest)
-    {
+    if (!MightCannotUseDest) {
       // shortcut if we are sure to be able to use dest directly,
       // this ease the compiler to generate cleaner and more optimzized code for most common cases
-      general_matrix_vector_product
-          <Index,LhsScalar,LhsMapper,ColMajor,LhsBlasTraits::NeedToConjugate,RhsScalar,RhsMapper,RhsBlasTraits::NeedToConjugate>::run(
-          actualLhs.rows(), actualLhs.cols(),
-          LhsMapper(actualLhs.data(), actualLhs.outerStride()),
-          RhsMapper(actualRhs.data(), actualRhs.innerStride()),
-          dest.data(), 1,
-          compatibleAlpha);
-    }
-    else
-    {
-      gemv_static_vector_if<ResScalar,ActualDest::SizeAtCompileTime,ActualDest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
+      general_matrix_vector_product<Index, LhsScalar, LhsMapper, ColMajor, LhsBlasTraits::NeedToConjugate, RhsScalar,
+                                    RhsMapper, RhsBlasTraits::NeedToConjugate>::run(actualLhs.rows(), actualLhs.cols(),
+                                                                                    LhsMapper(actualLhs.data(),
+                                                                                              actualLhs.outerStride()),
+                                                                                    RhsMapper(actualRhs.data(),
+                                                                                              actualRhs.innerStride()),
+                                                                                    dest.data(), 1, compatibleAlpha);
+    } else {
+      gemv_static_vector_if<ResScalar, ActualDest::SizeAtCompileTime, ActualDest::MaxSizeAtCompileTime,
+                            MightCannotUseDest>
+          static_dest;
 
-      const bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0));
+      const bool alphaIsCompatible = (!ComplexByReal) || (numext::is_exactly_zero(numext::imag(actualAlpha)));
       const bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible;
 
-      ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(),
+      ei_declare_aligned_stack_constructed_variable(ResScalar, actualDestPtr, dest.size(),
                                                     evalToDest ? dest.data() : static_dest.data());
 
-      if(!evalToDest)
-      {
-        #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+      if (!evalToDest) {
+#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
         Index size = dest.size();
         EIGEN_DENSE_STORAGE_CTOR_PLUGIN
-        #endif
-        if(!alphaIsCompatible)
-        {
+#endif
+        if (!alphaIsCompatible) {
           MappedDest(actualDestPtr, dest.size()).setZero();
           compatibleAlpha = RhsScalar(1);
-        }
-        else
+        } else
           MappedDest(actualDestPtr, dest.size()) = dest;
       }
 
-      general_matrix_vector_product
-          <Index,LhsScalar,LhsMapper,ColMajor,LhsBlasTraits::NeedToConjugate,RhsScalar,RhsMapper,RhsBlasTraits::NeedToConjugate>::run(
-          actualLhs.rows(), actualLhs.cols(),
-          LhsMapper(actualLhs.data(), actualLhs.outerStride()),
-          RhsMapper(actualRhs.data(), actualRhs.innerStride()),
-          actualDestPtr, 1,
-          compatibleAlpha);
+      general_matrix_vector_product<Index, LhsScalar, LhsMapper, ColMajor, LhsBlasTraits::NeedToConjugate, RhsScalar,
+                                    RhsMapper, RhsBlasTraits::NeedToConjugate>::run(actualLhs.rows(), actualLhs.cols(),
+                                                                                    LhsMapper(actualLhs.data(),
+                                                                                              actualLhs.outerStride()),
+                                                                                    RhsMapper(actualRhs.data(),
+                                                                                              actualRhs.innerStride()),
+                                                                                    actualDestPtr, 1, compatibleAlpha);
 
-      if (!evalToDest)
-      {
-        if(!alphaIsCompatible)
+      if (!evalToDest) {
+        if (!alphaIsCompatible)
           dest.matrix() += actualAlpha * MappedDest(actualDestPtr, dest.size());
         else
           dest = MappedDest(actualDestPtr, dest.size());
@@ -301,165 +365,163 @@
   }
 };
 
-template<> struct gemv_dense_selector<OnTheRight,RowMajor,true>
-{
-  template<typename Lhs, typename Rhs, typename Dest>
-  static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha)
-  {
-    typedef typename Lhs::Scalar   LhsScalar;
-    typedef typename Rhs::Scalar   RhsScalar;
-    typedef typename Dest::Scalar  ResScalar;
-    
+template <>
+struct gemv_dense_selector<OnTheRight, RowMajor, true> {
+  template <typename Lhs, typename Rhs, typename Dest>
+  static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) {
+    typedef typename Lhs::Scalar LhsScalar;
+    typedef typename Rhs::Scalar RhsScalar;
+    typedef typename Dest::Scalar ResScalar;
+
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
     typedef internal::blas_traits<Rhs> RhsBlasTraits;
     typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-    typedef typename internal::remove_all<ActualRhsType>::type ActualRhsTypeCleaned;
+    typedef internal::remove_all_t<ActualRhsType> ActualRhsTypeCleaned;
 
-    typename add_const<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(lhs);
-    typename add_const<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(rhs);
+    std::add_const_t<ActualLhsType> actualLhs = LhsBlasTraits::extract(lhs);
+    std::add_const_t<ActualRhsType> actualRhs = RhsBlasTraits::extract(rhs);
 
     ResScalar actualAlpha = combine_scalar_factors(alpha, lhs, rhs);
 
     enum {
       // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
       // on, the other hand it is good for the cache to pack the vector anyways...
-      DirectlyUseRhs = ActualRhsTypeCleaned::InnerStrideAtCompileTime==1 || ActualRhsTypeCleaned::MaxSizeAtCompileTime==0
+      DirectlyUseRhs =
+          ActualRhsTypeCleaned::InnerStrideAtCompileTime == 1 || ActualRhsTypeCleaned::MaxSizeAtCompileTime == 0
     };
 
-    gemv_static_vector_if<RhsScalar,ActualRhsTypeCleaned::SizeAtCompileTime,ActualRhsTypeCleaned::MaxSizeAtCompileTime,!DirectlyUseRhs> static_rhs;
+    gemv_static_vector_if<RhsScalar, ActualRhsTypeCleaned::SizeAtCompileTime,
+                          ActualRhsTypeCleaned::MaxSizeAtCompileTime, !DirectlyUseRhs>
+        static_rhs;
 
-    ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,actualRhs.size(),
+    ei_declare_aligned_stack_constructed_variable(
+        RhsScalar, actualRhsPtr, actualRhs.size(),
         DirectlyUseRhs ? const_cast<RhsScalar*>(actualRhs.data()) : static_rhs.data());
 
-    if(!DirectlyUseRhs)
-    {
-      #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+    if (!DirectlyUseRhs) {
+#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
       Index size = actualRhs.size();
       EIGEN_DENSE_STORAGE_CTOR_PLUGIN
-      #endif
+#endif
       Map<typename ActualRhsTypeCleaned::PlainObject>(actualRhsPtr, actualRhs.size()) = actualRhs;
     }
 
-    typedef const_blas_data_mapper<LhsScalar,Index,RowMajor> LhsMapper;
-    typedef const_blas_data_mapper<RhsScalar,Index,ColMajor> RhsMapper;
-    general_matrix_vector_product
-        <Index,LhsScalar,LhsMapper,RowMajor,LhsBlasTraits::NeedToConjugate,RhsScalar,RhsMapper,RhsBlasTraits::NeedToConjugate>::run(
-        actualLhs.rows(), actualLhs.cols(),
-        LhsMapper(actualLhs.data(), actualLhs.outerStride()),
-        RhsMapper(actualRhsPtr, 1),
-        dest.data(), dest.col(0).innerStride(), //NOTE  if dest is not a vector at compile-time, then dest.innerStride() might be wrong. (bug 1166)
-        actualAlpha);
+    typedef const_blas_data_mapper<LhsScalar, Index, RowMajor> LhsMapper;
+    typedef const_blas_data_mapper<RhsScalar, Index, ColMajor> RhsMapper;
+    general_matrix_vector_product<Index, LhsScalar, LhsMapper, RowMajor, LhsBlasTraits::NeedToConjugate, RhsScalar,
+                                  RhsMapper, RhsBlasTraits::NeedToConjugate>::
+        run(actualLhs.rows(), actualLhs.cols(), LhsMapper(actualLhs.data(), actualLhs.outerStride()),
+            RhsMapper(actualRhsPtr, 1), dest.data(),
+            dest.col(0).innerStride(),  // NOTE  if dest is not a vector at compile-time, then dest.innerStride() might
+                                        // be wrong. (bug 1166)
+            actualAlpha);
   }
 };
 
-template<> struct gemv_dense_selector<OnTheRight,ColMajor,false>
-{
-  template<typename Lhs, typename Rhs, typename Dest>
-  static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha)
-  {
-    EIGEN_STATIC_ASSERT((!nested_eval<Lhs,1>::Evaluate),EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE);
-    // TODO if rhs is large enough it might be beneficial to make sure that dest is sequentially stored in memory, otherwise use a temp
-    typename nested_eval<Rhs,1>::type actual_rhs(rhs);
+template <>
+struct gemv_dense_selector<OnTheRight, ColMajor, false> {
+  template <typename Lhs, typename Rhs, typename Dest>
+  static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) {
+    EIGEN_STATIC_ASSERT((!nested_eval<Lhs, 1>::Evaluate),
+                        EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE);
+    // TODO if rhs is large enough it might be beneficial to make sure that dest is sequentially stored in memory,
+    // otherwise use a temp
+    typename nested_eval<Rhs, 1>::type actual_rhs(rhs);
     const Index size = rhs.rows();
-    for(Index k=0; k<size; ++k)
-      dest += (alpha*actual_rhs.coeff(k)) * lhs.col(k);
+    for (Index k = 0; k < size; ++k) dest += (alpha * actual_rhs.coeff(k)) * lhs.col(k);
   }
 };
 
-template<> struct gemv_dense_selector<OnTheRight,RowMajor,false>
-{
-  template<typename Lhs, typename Rhs, typename Dest>
-  static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha)
-  {
-    EIGEN_STATIC_ASSERT((!nested_eval<Lhs,1>::Evaluate),EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE);
-    typename nested_eval<Rhs,Lhs::RowsAtCompileTime>::type actual_rhs(rhs);
+template <>
+struct gemv_dense_selector<OnTheRight, RowMajor, false> {
+  template <typename Lhs, typename Rhs, typename Dest>
+  static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) {
+    EIGEN_STATIC_ASSERT((!nested_eval<Lhs, 1>::Evaluate),
+                        EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE);
+    typename nested_eval<Rhs, Lhs::RowsAtCompileTime>::type actual_rhs(rhs);
     const Index rows = dest.rows();
-    for(Index i=0; i<rows; ++i)
+    for (Index i = 0; i < rows; ++i)
       dest.coeffRef(i) += alpha * (lhs.row(i).cwiseProduct(actual_rhs.transpose())).sum();
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /***************************************************************************
-* Implementation of matrix base methods
-***************************************************************************/
+ * Implementation of matrix base methods
+ ***************************************************************************/
 
 /** \returns the matrix product of \c *this and \a other.
-  *
-  * \note If instead of the matrix product you want the coefficient-wise product, see Cwise::operator*().
-  *
-  * \sa lazyProduct(), operator*=(const MatrixBase&), Cwise::operator*()
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const Product<Derived, OtherDerived>
-MatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
-{
+ *
+ * \note If instead of the matrix product you want the coefficient-wise product, see Cwise::operator*().
+ *
+ * \sa lazyProduct(), operator*=(const MatrixBase&), Cwise::operator*()
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Product<Derived, OtherDerived> MatrixBase<Derived>::operator*(
+    const MatrixBase<OtherDerived>& other) const {
   // A note regarding the function declaration: In MSVC, this function will sometimes
   // not be inlined since DenseStorage is an unwindable object for dynamic
   // matrices and product types are holding a member to store the result.
   // Thus it does not help tagging this function with EIGEN_STRONG_INLINE.
   enum {
-    ProductIsValid =  Derived::ColsAtCompileTime==Dynamic
-                   || OtherDerived::RowsAtCompileTime==Dynamic
-                   || int(Derived::ColsAtCompileTime)==int(OtherDerived::RowsAtCompileTime),
+    ProductIsValid = Derived::ColsAtCompileTime == Dynamic || OtherDerived::RowsAtCompileTime == Dynamic ||
+                     int(Derived::ColsAtCompileTime) == int(OtherDerived::RowsAtCompileTime),
     AreVectors = Derived::IsVectorAtCompileTime && OtherDerived::IsVectorAtCompileTime,
-    SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived,OtherDerived)
+    SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived, OtherDerived)
   };
   // note to the lost user:
   //    * for a dot product use: v1.dot(v2)
   //    * for a coeff-wise product use: v1.cwiseProduct(v2)
-  EIGEN_STATIC_ASSERT(ProductIsValid || !(AreVectors && SameSizes),
-    INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS)
+  EIGEN_STATIC_ASSERT(
+      ProductIsValid || !(AreVectors && SameSizes),
+      INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS)
   EIGEN_STATIC_ASSERT(ProductIsValid || !(SameSizes && !AreVectors),
-    INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION)
+                      INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION)
   EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT)
 #ifdef EIGEN_DEBUG_PRODUCT
-  internal::product_type<Derived,OtherDerived>::debug();
+  internal::product_type<Derived, OtherDerived>::debug();
 #endif
 
   return Product<Derived, OtherDerived>(derived(), other.derived());
 }
 
 /** \returns an expression of the matrix product of \c *this and \a other without implicit evaluation.
-  *
-  * The returned product will behave like any other expressions: the coefficients of the product will be
-  * computed once at a time as requested. This might be useful in some extremely rare cases when only
-  * a small and no coherent fraction of the result's coefficients have to be computed.
-  *
-  * \warning This version of the matrix product can be much much slower. So use it only if you know
-  * what you are doing and that you measured a true speed improvement.
-  *
-  * \sa operator*(const MatrixBase&)
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const Product<Derived,OtherDerived,LazyProduct>
-MatrixBase<Derived>::lazyProduct(const MatrixBase<OtherDerived> &other) const
-{
+ *
+ * The returned product will behave like any other expressions: the coefficients of the product will be
+ * computed once at a time as requested. This might be useful in some extremely rare cases when only
+ * a small and no coherent fraction of the result's coefficients have to be computed.
+ *
+ * \warning This version of the matrix product can be much much slower. So use it only if you know
+ * what you are doing and that you measured a true speed improvement.
+ *
+ * \sa operator*(const MatrixBase&)
+ */
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Product<Derived, OtherDerived, LazyProduct>
+MatrixBase<Derived>::lazyProduct(const MatrixBase<OtherDerived>& other) const {
   enum {
-    ProductIsValid =  Derived::ColsAtCompileTime==Dynamic
-                   || OtherDerived::RowsAtCompileTime==Dynamic
-                   || int(Derived::ColsAtCompileTime)==int(OtherDerived::RowsAtCompileTime),
+    ProductIsValid = Derived::ColsAtCompileTime == Dynamic || OtherDerived::RowsAtCompileTime == Dynamic ||
+                     int(Derived::ColsAtCompileTime) == int(OtherDerived::RowsAtCompileTime),
     AreVectors = Derived::IsVectorAtCompileTime && OtherDerived::IsVectorAtCompileTime,
-    SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived,OtherDerived)
+    SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived, OtherDerived)
   };
   // note to the lost user:
   //    * for a dot product use: v1.dot(v2)
   //    * for a coeff-wise product use: v1.cwiseProduct(v2)
-  EIGEN_STATIC_ASSERT(ProductIsValid || !(AreVectors && SameSizes),
-    INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS)
+  EIGEN_STATIC_ASSERT(
+      ProductIsValid || !(AreVectors && SameSizes),
+      INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS)
   EIGEN_STATIC_ASSERT(ProductIsValid || !(SameSizes && !AreVectors),
-    INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION)
+                      INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION)
   EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT)
 
-  return Product<Derived,OtherDerived,LazyProduct>(derived(), other.derived());
+  return Product<Derived, OtherDerived, LazyProduct>(derived(), other.derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PRODUCT_H
+#endif  // EIGEN_PRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
index cf677a1..5936336 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
@@ -11,17 +11,20 @@
 #ifndef EIGEN_GENERIC_PACKET_MATH_H
 #define EIGEN_GENERIC_PACKET_MATH_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 /** \internal
-  * \file GenericPacketMath.h
-  *
-  * Default implementation for types not supported by the vectorization.
-  * In practice these functions are provided to make easier the writing
-  * of generic vectorized code.
-  */
+ * \file GenericPacketMath.h
+ *
+ * Default implementation for types not supported by the vectorization.
+ * In practice these functions are provided to make easier the writing
+ * of generic vectorized code.
+ */
 
 #ifndef EIGEN_DEBUG_ALIGNED_LOAD
 #define EIGEN_DEBUG_ALIGNED_LOAD
@@ -39,48 +42,48 @@
 #define EIGEN_DEBUG_UNALIGNED_STORE
 #endif
 
-struct default_packet_traits
-{
+struct default_packet_traits {
   enum {
-    HasHalfPacket = 0,
-
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 0,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 0,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
     HasSetLinear = 1,
-    HasBlend     = 0,
+    HasSign = 1,
+    HasBlend = 0,
     // This flag is used to indicate whether packet comparison is supported.
     // pcmp_eq, pcmp_lt and pcmp_le should be defined for it to be true.
-    HasCmp       = 0,
+    HasCmp = 0,
 
-    HasDiv    = 0,
-    HasSqrt   = 0,
-    HasRsqrt  = 0,
-    HasExp    = 0,
-    HasExpm1  = 0,
-    HasLog    = 0,
-    HasLog1p  = 0,
-    HasLog10  = 0,
-    HasPow    = 0,
+    HasDiv = 0,
+    HasReciprocal = 0,
+    HasSqrt = 0,
+    HasRsqrt = 0,
+    HasExp = 0,
+    HasExpm1 = 0,
+    HasLog = 0,
+    HasLog1p = 0,
+    HasLog10 = 0,
+    HasPow = 0,
 
-    HasSin    = 0,
-    HasCos    = 0,
-    HasTan    = 0,
-    HasASin   = 0,
-    HasACos   = 0,
-    HasATan   = 0,
-    HasSinh   = 0,
-    HasCosh   = 0,
-    HasTanh   = 0,
+    HasSin = 0,
+    HasCos = 0,
+    HasTan = 0,
+    HasASin = 0,
+    HasACos = 0,
+    HasATan = 0,
+    HasATanh = 0,
+    HasSinh = 0,
+    HasCosh = 0,
+    HasTanh = 0,
     HasLGamma = 0,
     HasDiGamma = 0,
     HasZeta = 0,
@@ -95,74 +98,130 @@
     HasIGammac = 0,
     HasBetaInc = 0,
 
-    HasRound  = 0,
-    HasRint   = 0,
-    HasFloor  = 0,
-    HasCeil   = 0,
-    HasSign   = 0
+    HasRound = 0,
+    HasRint = 0,
+    HasFloor = 0,
+    HasCeil = 0
   };
 };
 
-template<typename T> struct packet_traits : default_packet_traits
-{
+template <typename T>
+struct packet_traits : default_packet_traits {
   typedef T type;
   typedef T half;
   enum {
     Vectorizable = 0,
     size = 1,
     AlignedOnScalar = 0,
-    HasHalfPacket = 0
   };
   enum {
-    HasAdd    = 0,
-    HasSub    = 0,
-    HasMul    = 0,
+    HasAdd = 0,
+    HasSub = 0,
+    HasMul = 0,
     HasNegate = 0,
-    HasAbs    = 0,
-    HasAbs2   = 0,
-    HasMin    = 0,
-    HasMax    = 0,
-    HasConj   = 0,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
+    HasConj = 0,
     HasSetLinear = 0
   };
 };
 
-template<typename T> struct packet_traits<const T> : packet_traits<T> { };
+template <typename T>
+struct packet_traits<const T> : packet_traits<T> {};
 
-template<typename T> struct unpacket_traits
-{
+template <typename T>
+struct unpacket_traits {
   typedef T type;
   typedef T half;
-  enum
-  {
-    size = 1,
-    alignment = 1,
-    vectorizable = false,
-    masked_load_available=false,
-    masked_store_available=false
-  };
+  enum { size = 1, alignment = 1, vectorizable = false, masked_load_available = false, masked_store_available = false };
 };
 
-template<typename T> struct unpacket_traits<const T> : unpacket_traits<T> { };
+template <typename T>
+struct unpacket_traits<const T> : unpacket_traits<T> {};
 
-template <typename Src, typename Tgt> struct type_casting_traits {
+/** \internal A convenience utility for determining if the type is a scalar.
+ * This is used to enable some generic packet implementations.
+ */
+template <typename Packet>
+struct is_scalar {
+  using Scalar = typename unpacket_traits<Packet>::type;
+  enum { value = internal::is_same<Packet, Scalar>::value };
+};
+
+// automatically and succinctly define combinations of pcast<SrcPacket,TgtPacket> when
+// 1) the packets are the same type, or
+// 2) the packets differ only in sign.
+// In both of these cases, preinterpret (bit_cast) is equivalent to pcast (static_cast)
+template <typename SrcPacket, typename TgtPacket,
+          bool Scalar = is_scalar<SrcPacket>::value && is_scalar<TgtPacket>::value>
+struct is_degenerate_helper : is_same<SrcPacket, TgtPacket> {};
+template <>
+struct is_degenerate_helper<int8_t, uint8_t, true> : std::true_type {};
+template <>
+struct is_degenerate_helper<int16_t, uint16_t, true> : std::true_type {};
+template <>
+struct is_degenerate_helper<int32_t, uint32_t, true> : std::true_type {};
+template <>
+struct is_degenerate_helper<int64_t, uint64_t, true> : std::true_type {};
+
+template <typename SrcPacket, typename TgtPacket>
+struct is_degenerate_helper<SrcPacket, TgtPacket, false> {
+  using SrcScalar = typename unpacket_traits<SrcPacket>::type;
+  static constexpr int SrcSize = unpacket_traits<SrcPacket>::size;
+  using TgtScalar = typename unpacket_traits<TgtPacket>::type;
+  static constexpr int TgtSize = unpacket_traits<TgtPacket>::size;
+  static constexpr bool value = is_degenerate_helper<SrcScalar, TgtScalar, true>::value && (SrcSize == TgtSize);
+};
+
+// is_degenerate<T1,T2>::value == is_degenerate<T2,T1>::value
+template <typename SrcPacket, typename TgtPacket>
+struct is_degenerate {
+  static constexpr bool value =
+      is_degenerate_helper<SrcPacket, TgtPacket>::value || is_degenerate_helper<TgtPacket, SrcPacket>::value;
+};
+
+template <typename Packet>
+struct is_half {
+  using Scalar = typename unpacket_traits<Packet>::type;
+  static constexpr int Size = unpacket_traits<Packet>::size;
+  using DefaultPacket = typename packet_traits<Scalar>::type;
+  static constexpr int DefaultSize = unpacket_traits<DefaultPacket>::size;
+  static constexpr bool value = Size < DefaultSize;
+};
+
+template <typename Src, typename Tgt>
+struct type_casting_traits {
   enum {
-    VectorizedCast = 0,
+    VectorizedCast =
+        is_degenerate<Src, Tgt>::value && packet_traits<Src>::Vectorizable && packet_traits<Tgt>::Vectorizable,
     SrcCoeffRatio = 1,
     TgtCoeffRatio = 1
   };
 };
 
+// provides a succint template to define vectorized casting traits with respect to the largest accessible packet types
+template <typename Src, typename Tgt>
+struct vectorized_type_casting_traits {
+  enum : int {
+    DefaultSrcPacketSize = packet_traits<Src>::size,
+    DefaultTgtPacketSize = packet_traits<Tgt>::size,
+    VectorizedCast = 1,
+    SrcCoeffRatio = plain_enum_max(DefaultTgtPacketSize / DefaultSrcPacketSize, 1),
+    TgtCoeffRatio = plain_enum_max(DefaultSrcPacketSize / DefaultTgtPacketSize, 1)
+  };
+};
+
 /** \internal Wrapper to ensure that multiple packet types can map to the same
     same underlying vector type. */
-template<typename T, int unique_id = 0>
-struct eigen_packet_wrapper
-{
+template <typename T, int unique_id = 0>
+struct eigen_packet_wrapper {
   EIGEN_ALWAYS_INLINE operator T&() { return m_val; }
   EIGEN_ALWAYS_INLINE operator const T&() const { return m_val; }
-  EIGEN_ALWAYS_INLINE eigen_packet_wrapper() {}
-  EIGEN_ALWAYS_INLINE eigen_packet_wrapper(const T &v) : m_val(v) {}
-  EIGEN_ALWAYS_INLINE eigen_packet_wrapper& operator=(const T &v) {
+  EIGEN_ALWAYS_INLINE eigen_packet_wrapper() = default;
+  EIGEN_ALWAYS_INLINE eigen_packet_wrapper(const T& v) : m_val(v) {}
+  EIGEN_ALWAYS_INLINE eigen_packet_wrapper& operator=(const T& v) {
     m_val = v;
     return *this;
   }
@@ -170,83 +229,147 @@
   T m_val;
 };
 
+template <typename Target, typename Packet, bool IsSame = is_same<Target, Packet>::value>
+struct preinterpret_generic;
 
-/** \internal A convenience utility for determining if the type is a scalar.
- * This is used to enable some generic packet implementations.
- */
-template<typename Packet>
-struct is_scalar {
-  typedef typename unpacket_traits<Packet>::type Scalar;
-  enum {
-    value = internal::is_same<Packet, Scalar>::value
-  };
+template <typename Target, typename Packet>
+struct preinterpret_generic<Target, Packet, false> {
+  // the packets are not the same, attempt scalar bit_cast
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Target run(const Packet& a) {
+    return numext::bit_cast<Target, Packet>(a);
+  }
+};
+
+template <typename Packet>
+struct preinterpret_generic<Packet, Packet, true> {
+  // the packets are the same type: do nothing
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& a) { return a; }
+};
+
+/** \internal \returns reinterpret_cast<Target>(a) */
+template <typename Target, typename Packet>
+EIGEN_DEVICE_FUNC inline Target preinterpret(const Packet& a) {
+  return preinterpret_generic<Target, Packet>::run(a);
+}
+
+template <typename SrcPacket, typename TgtPacket, bool Degenerate = is_degenerate<SrcPacket, TgtPacket>::value,
+          bool TgtIsHalf = is_half<TgtPacket>::value>
+struct pcast_generic;
+
+template <typename SrcPacket, typename TgtPacket>
+struct pcast_generic<SrcPacket, TgtPacket, false, false> {
+  // the packets are not degenerate: attempt scalar static_cast
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket run(const SrcPacket& a) {
+    return cast_impl<SrcPacket, TgtPacket>::run(a);
+  }
+};
+
+template <typename Packet>
+struct pcast_generic<Packet, Packet, true, false> {
+  // the packets are the same: do nothing
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& a) { return a; }
+};
+
+template <typename SrcPacket, typename TgtPacket, bool TgtIsHalf>
+struct pcast_generic<SrcPacket, TgtPacket, true, TgtIsHalf> {
+  // the packets are degenerate: preinterpret is equivalent to pcast
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket run(const SrcPacket& a) { return preinterpret<TgtPacket>(a); }
 };
 
 /** \internal \returns static_cast<TgtType>(a) (coeff-wise) */
 template <typename SrcPacket, typename TgtPacket>
-EIGEN_DEVICE_FUNC inline TgtPacket
-pcast(const SrcPacket& a) {
-  return static_cast<TgtPacket>(a);
+EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a) {
+  return pcast_generic<SrcPacket, TgtPacket>::run(a);
 }
 template <typename SrcPacket, typename TgtPacket>
-EIGEN_DEVICE_FUNC inline TgtPacket
-pcast(const SrcPacket& a, const SrcPacket& /*b*/) {
-  return static_cast<TgtPacket>(a);
+EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a, const SrcPacket& b) {
+  return pcast_generic<SrcPacket, TgtPacket>::run(a, b);
 }
 template <typename SrcPacket, typename TgtPacket>
-EIGEN_DEVICE_FUNC inline TgtPacket
-pcast(const SrcPacket& a, const SrcPacket& /*b*/, const SrcPacket& /*c*/, const SrcPacket& /*d*/) {
-  return static_cast<TgtPacket>(a);
+EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a, const SrcPacket& b, const SrcPacket& c,
+                                         const SrcPacket& d) {
+  return pcast_generic<SrcPacket, TgtPacket>::run(a, b, c, d);
 }
 template <typename SrcPacket, typename TgtPacket>
-EIGEN_DEVICE_FUNC inline TgtPacket
-pcast(const SrcPacket& a, const SrcPacket& /*b*/, const SrcPacket& /*c*/, const SrcPacket& /*d*/,
-      const SrcPacket& /*e*/, const SrcPacket& /*f*/, const SrcPacket& /*g*/, const SrcPacket& /*h*/) {
-  return static_cast<TgtPacket>(a);
+EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a, const SrcPacket& b, const SrcPacket& c, const SrcPacket& d,
+                                         const SrcPacket& e, const SrcPacket& f, const SrcPacket& g,
+                                         const SrcPacket& h) {
+  return pcast_generic<SrcPacket, TgtPacket>::run(a, b, c, d, e, f, g, h);
 }
 
-/** \internal \returns reinterpret_cast<Target>(a) */
-template <typename Target, typename Packet>
-EIGEN_DEVICE_FUNC inline Target
-preinterpret(const Packet& a); /* { return reinterpret_cast<const Target&>(a); } */
+template <typename SrcPacket, typename TgtPacket>
+struct pcast_generic<SrcPacket, TgtPacket, false, true> {
+  // TgtPacket is a half packet of some other type
+  // perform cast and truncate result
+  using DefaultTgtPacket = typename is_half<TgtPacket>::DefaultPacket;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket run(const SrcPacket& a) {
+    return preinterpret<TgtPacket>(pcast<SrcPacket, DefaultTgtPacket>(a));
+  }
+};
 
 /** \internal \returns a + b (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-padd(const Packet& a, const Packet& b) { return a+b; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet padd(const Packet& a, const Packet& b) {
+  return a + b;
+}
 // Avoid compiler warning for boolean algebra.
-template<> EIGEN_DEVICE_FUNC inline bool
-padd(const bool& a, const bool& b) { return a || b; }
+template <>
+EIGEN_DEVICE_FUNC inline bool padd(const bool& a, const bool& b) {
+  return a || b;
+}
+
+/** \internal \returns a packet version of \a *from, (un-aligned masked add)
+ * There is no generic implementation. We only have implementations for specialized
+ * cases. Generic case should not be called.
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline std::enable_if_t<unpacket_traits<Packet>::masked_fpops_available, Packet> padd(
+    const Packet& a, const Packet& b, typename unpacket_traits<Packet>::mask_t umask);
 
 /** \internal \returns a - b (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-psub(const Packet& a, const Packet& b) { return a-b; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet psub(const Packet& a, const Packet& b) {
+  return a - b;
+}
 
 /** \internal \returns -a (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pnegate(const Packet& a) { return -a; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pnegate(const Packet& a) {
+  return -a;
+}
 
-template<> EIGEN_DEVICE_FUNC inline bool
-pnegate(const bool& a) { return !a; }
+template <>
+EIGEN_DEVICE_FUNC inline bool pnegate(const bool& a) {
+  return !a;
+}
 
 /** \internal \returns conj(a) (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pconj(const Packet& a) { return numext::conj(a); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pconj(const Packet& a) {
+  return numext::conj(a);
+}
 
 /** \internal \returns a * b (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pmul(const Packet& a, const Packet& b) { return a*b; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pmul(const Packet& a, const Packet& b) {
+  return a * b;
+}
 // Avoid compiler warning for boolean algebra.
-template<> EIGEN_DEVICE_FUNC inline bool
-pmul(const bool& a, const bool& b) { return a && b; }
+template <>
+EIGEN_DEVICE_FUNC inline bool pmul(const bool& a, const bool& b) {
+  return a && b;
+}
 
 /** \internal \returns a / b (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pdiv(const Packet& a, const Packet& b) { return a/b; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pdiv(const Packet& a, const Packet& b) {
+  return a / b;
+}
 
 // In the generic case, memset to all one bits.
-template<typename Packet, typename EnableIf = void>
+template <typename Packet, typename EnableIf = void>
 struct ptrue_impl {
-  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/){
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/) {
     Packet b;
     memset(static_cast<void*>(&b), 0xff, sizeof(Packet));
     return b;
@@ -257,22 +380,19 @@
 // Although this is technically not a valid bitmask, the scalar path for pselect
 // uses a comparison to zero, so this should still work in most cases. We don't
 // have another option, since the scalar type requires initialization.
-template<typename T>
-struct ptrue_impl<T, 
-    typename internal::enable_if<is_scalar<T>::value && NumTraits<T>::RequireInitialization>::type > {
-  static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/){
-    return T(1);
-  }
+template <typename T>
+struct ptrue_impl<T, std::enable_if_t<is_scalar<T>::value && NumTraits<T>::RequireInitialization>> {
+  static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/) { return T(1); }
 };
 
 /** \internal \returns one bits. */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-ptrue(const Packet& a) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet ptrue(const Packet& a) {
   return ptrue_impl<Packet>::run(a);
 }
 
 // In the general case, memset to zero.
-template<typename Packet, typename EnableIf = void>
+template <typename Packet, typename EnableIf = void>
 struct pzero_impl {
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/) {
     Packet b;
@@ -283,66 +403,63 @@
 
 // For scalars, explicitly set to Scalar(0), since the underlying representation
 // for zero may not consist of all-zero bits.
-template<typename T>
-struct pzero_impl<T,
-    typename internal::enable_if<is_scalar<T>::value>::type> {
-  static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/) {
-    return T(0);
-  }
+template <typename T>
+struct pzero_impl<T, std::enable_if_t<is_scalar<T>::value>> {
+  static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/) { return T(0); }
 };
 
 /** \internal \returns packet of zeros */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pzero(const Packet& a) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pzero(const Packet& a) {
   return pzero_impl<Packet>::run(a);
 }
 
 /** \internal \returns a <= b as a bit mask */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pcmp_le(const Packet& a, const Packet& b)  { return a<=b ? ptrue(a) : pzero(a); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pcmp_le(const Packet& a, const Packet& b) {
+  return a <= b ? ptrue(a) : pzero(a);
+}
 
 /** \internal \returns a < b as a bit mask */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pcmp_lt(const Packet& a, const Packet& b)  { return a<b ? ptrue(a) : pzero(a); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pcmp_lt(const Packet& a, const Packet& b) {
+  return a < b ? ptrue(a) : pzero(a);
+}
 
 /** \internal \returns a == b as a bit mask */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pcmp_eq(const Packet& a, const Packet& b) { return a==b ? ptrue(a) : pzero(a); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pcmp_eq(const Packet& a, const Packet& b) {
+  return a == b ? ptrue(a) : pzero(a);
+}
 
 /** \internal \returns a < b or a==NaN or b==NaN as a bit mask */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pcmp_lt_or_nan(const Packet& a, const Packet& b) { return a>=b ? pzero(a) : ptrue(a); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) {
+  return a >= b ? pzero(a) : ptrue(a);
+}
 
-template<typename T>
+template <typename T>
 struct bit_and {
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const {
-    return a & b;
-  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a & b; }
 };
 
-template<typename T>
+template <typename T>
 struct bit_or {
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const {
-    return a | b;
-  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a | b; }
 };
 
-template<typename T>
+template <typename T>
 struct bit_xor {
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const {
-    return a ^ b;
-  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a ^ b; }
 };
 
-template<typename T>
+template <typename T>
 struct bit_not {
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a) const {
-    return ~a;
-  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a) const { return ~a; }
 };
 
 // Use operators &, |, ^, ~.
-template<typename T>
+template <typename T>
 struct operator_bitwise_helper {
   EIGEN_DEVICE_FUNC static inline T bitwise_and(const T& a, const T& b) { return bit_and<T>()(a, b); }
   EIGEN_DEVICE_FUNC static inline T bitwise_or(const T& a, const T& b) { return bit_or<T>()(a, b); }
@@ -351,23 +468,19 @@
 };
 
 // Apply binary operations byte-by-byte
-template<typename T>
+template <typename T>
 struct bytewise_bitwise_helper {
   EIGEN_DEVICE_FUNC static inline T bitwise_and(const T& a, const T& b) {
     return binary(a, b, bit_and<unsigned char>());
   }
-  EIGEN_DEVICE_FUNC static inline T bitwise_or(const T& a, const T& b) { 
-    return binary(a, b, bit_or<unsigned char>());
-   }
+  EIGEN_DEVICE_FUNC static inline T bitwise_or(const T& a, const T& b) { return binary(a, b, bit_or<unsigned char>()); }
   EIGEN_DEVICE_FUNC static inline T bitwise_xor(const T& a, const T& b) {
     return binary(a, b, bit_xor<unsigned char>());
   }
-  EIGEN_DEVICE_FUNC static inline T bitwise_not(const T& a) { 
-    return unary(a,bit_not<unsigned char>());
-   }
-  
+  EIGEN_DEVICE_FUNC static inline T bitwise_not(const T& a) { return unary(a, bit_not<unsigned char>()); }
+
  private:
-  template<typename Op>
+  template <typename Op>
   EIGEN_DEVICE_FUNC static inline T unary(const T& a, Op op) {
     const unsigned char* a_ptr = reinterpret_cast<const unsigned char*>(&a);
     T c;
@@ -378,7 +491,7 @@
     return c;
   }
 
-  template<typename Op>
+  template <typename Op>
   EIGEN_DEVICE_FUNC static inline T binary(const T& a, const T& b, Op op) {
     const unsigned char* a_ptr = reinterpret_cast<const unsigned char*>(&a);
     const unsigned char* b_ptr = reinterpret_cast<const unsigned char*>(&b);
@@ -392,124 +505,125 @@
 };
 
 // In the general case, use byte-by-byte manipulation.
-template<typename T, typename EnableIf = void>
+template <typename T, typename EnableIf = void>
 struct bitwise_helper : public bytewise_bitwise_helper<T> {};
 
 // For integers or non-trivial scalars, use binary operators.
-template<typename T>
-struct bitwise_helper<T,
-  typename internal::enable_if<
-    is_scalar<T>::value && (NumTraits<T>::IsInteger || NumTraits<T>::RequireInitialization)>::type
-  > : public operator_bitwise_helper<T> {};
+template <typename T>
+struct bitwise_helper<T, typename std::enable_if_t<is_scalar<T>::value &&
+                                                   (NumTraits<T>::IsInteger || NumTraits<T>::RequireInitialization)>>
+    : public operator_bitwise_helper<T> {};
 
 /** \internal \returns the bitwise and of \a a and \a b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pand(const Packet& a, const Packet& b) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pand(const Packet& a, const Packet& b) {
   return bitwise_helper<Packet>::bitwise_and(a, b);
 }
 
 /** \internal \returns the bitwise or of \a a and \a b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-por(const Packet& a, const Packet& b) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet por(const Packet& a, const Packet& b) {
   return bitwise_helper<Packet>::bitwise_or(a, b);
 }
 
 /** \internal \returns the bitwise xor of \a a and \a b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pxor(const Packet& a, const Packet& b) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pxor(const Packet& a, const Packet& b) {
   return bitwise_helper<Packet>::bitwise_xor(a, b);
 }
 
 /** \internal \returns the bitwise not of \a a */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pnot(const Packet& a) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pnot(const Packet& a) {
   return bitwise_helper<Packet>::bitwise_not(a);
 }
 
 /** \internal \returns the bitwise and of \a a and not \a b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pandnot(const Packet& a, const Packet& b) { return pand(a, pnot(b)); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pandnot(const Packet& a, const Packet& b) {
+  return pand(a, pnot(b));
+}
+
+/** \internal \returns isnan(a) */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pisnan(const Packet& a) {
+  return pandnot(ptrue(a), pcmp_eq(a, a));
+}
 
 // In the general case, use bitwise select.
-template<typename Packet, typename EnableIf = void>
+template <typename Packet, typename EnableIf = void>
 struct pselect_impl {
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) {
-    return por(pand(a,mask),pandnot(b,mask));
+    return por(pand(a, mask), pandnot(b, mask));
   }
 };
 
 // For scalars, use ternary select.
-template<typename Packet>
-struct pselect_impl<Packet, 
-    typename internal::enable_if<is_scalar<Packet>::value>::type > {
+template <typename Packet>
+struct pselect_impl<Packet, std::enable_if_t<is_scalar<Packet>::value>> {
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) {
     return numext::equal_strict(mask, Packet(0)) ? b : a;
   }
 };
 
 /** \internal \returns \a or \b for each field in packet according to \mask */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pselect(const Packet& mask, const Packet& a, const Packet& b) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pselect(const Packet& mask, const Packet& a, const Packet& b) {
   return pselect_impl<Packet>::run(mask, a, b);
 }
 
-template<> EIGEN_DEVICE_FUNC inline bool pselect<bool>(
-    const bool& cond, const bool& a, const bool& b) {
+template <>
+EIGEN_DEVICE_FUNC inline bool pselect<bool>(const bool& cond, const bool& a, const bool& b) {
   return cond ? a : b;
 }
 
 /** \internal \returns the min or of \a a and \a b (coeff-wise)
     If either \a a or \a b are NaN, the result is implementation defined. */
-template<int NaNPropagation>
+template <int NaNPropagation>
 struct pminmax_impl {
   template <typename Packet, typename Op>
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
-    return op(a,b);
+    return op(a, b);
   }
 };
 
 /** \internal \returns the min or max of \a a and \a b (coeff-wise)
     If either \a a or \a b are NaN, NaN is returned. */
-template<>
+template <>
 struct pminmax_impl<PropagateNaN> {
   template <typename Packet, typename Op>
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
-  Packet not_nan_mask_a = pcmp_eq(a, a);
-  Packet not_nan_mask_b = pcmp_eq(b, b);
-  return pselect(not_nan_mask_a,
-                 pselect(not_nan_mask_b, op(a, b), b),
-                 a);
+    Packet not_nan_mask_a = pcmp_eq(a, a);
+    Packet not_nan_mask_b = pcmp_eq(b, b);
+    return pselect(not_nan_mask_a, pselect(not_nan_mask_b, op(a, b), b), a);
   }
 };
 
 /** \internal \returns the min or max of \a a and \a b (coeff-wise)
     If both \a a and \a b are NaN, NaN is returned.
     Equivalent to std::fmin(a, b).  */
-template<>
+template <>
 struct pminmax_impl<PropagateNumbers> {
   template <typename Packet, typename Op>
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
-  Packet not_nan_mask_a = pcmp_eq(a, a);
-  Packet not_nan_mask_b = pcmp_eq(b, b);
-  return pselect(not_nan_mask_a,
-                 pselect(not_nan_mask_b, op(a, b), a),
-                 b);
+    Packet not_nan_mask_a = pcmp_eq(a, a);
+    Packet not_nan_mask_b = pcmp_eq(b, b);
+    return pselect(not_nan_mask_a, pselect(not_nan_mask_b, op(a, b), a), b);
   }
 };
 
-
 #ifndef SYCL_DEVICE_ONLY
 #define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) Func
 #else
-#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) \
-[](const Type& a, const Type& b) { \
-        return Func(a, b);}
+#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) [](const Type& a, const Type& b) { return Func(a, b); }
 #endif
 
 /** \internal \returns the min of \a a and \a b  (coeff-wise).
     If \a a or \b b is NaN, the return value is implementation defined. */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pmin(const Packet& a, const Packet& b) { return numext::mini(a,b); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pmin(const Packet& a, const Packet& b) {
+  return numext::mini(a, b);
+}
 
 /** \internal \returns the min of \a a and \a b  (coeff-wise).
     NaNPropagation determines the NaN propagation semantics. */
@@ -520,58 +634,82 @@
 
 /** \internal \returns the max of \a a and \a b  (coeff-wise)
     If \a a or \b b is NaN, the return value is implementation defined. */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pmax(const Packet& a, const Packet& b) { return numext::maxi(a, b); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, const Packet& b) {
+  return numext::maxi(a, b);
+}
 
 /** \internal \returns the max of \a a and \a b  (coeff-wise).
     NaNPropagation determines the NaN propagation semantics. */
 template <int NaNPropagation, typename Packet>
 EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, const Packet& b) {
-  return pminmax_impl<NaNPropagation>::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet,(pmax<Packet>)));
+  return pminmax_impl<NaNPropagation>::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmax<Packet>)));
 }
 
 /** \internal \returns the absolute value of \a a */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pabs(const Packet& a) { return numext::abs(a); }
-template<> EIGEN_DEVICE_FUNC inline unsigned int
-pabs(const unsigned int& a) { return a; }
-template<> EIGEN_DEVICE_FUNC inline unsigned long
-pabs(const unsigned long& a) { return a; }
-template<> EIGEN_DEVICE_FUNC inline unsigned long long
-pabs(const unsigned long long& a) { return a; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pabs(const Packet& a) {
+  return numext::abs(a);
+}
+template <>
+EIGEN_DEVICE_FUNC inline unsigned int pabs(const unsigned int& a) {
+  return a;
+}
+template <>
+EIGEN_DEVICE_FUNC inline unsigned long pabs(const unsigned long& a) {
+  return a;
+}
+template <>
+EIGEN_DEVICE_FUNC inline unsigned long long pabs(const unsigned long long& a) {
+  return a;
+}
 
 /** \internal \returns the addsub value of \a a,b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-paddsub(const Packet& a, const Packet& b) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet paddsub(const Packet& a, const Packet& b) {
   return pselect(peven_mask(a), padd(a, b), psub(a, b));
- }
+}
 
 /** \internal \returns the phase angle of \a a */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-parg(const Packet& a) { using numext::arg; return arg(a); }
-
-
-/** \internal \returns \a a logically shifted by N bits to the right */
-template<int N> EIGEN_DEVICE_FUNC inline int
-parithmetic_shift_right(const int& a) { return a >> N; }
-template<int N> EIGEN_DEVICE_FUNC inline long int
-parithmetic_shift_right(const long int& a) { return a >> N; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet parg(const Packet& a) {
+  using numext::arg;
+  return arg(a);
+}
 
 /** \internal \returns \a a arithmetically shifted by N bits to the right */
-template<int N> EIGEN_DEVICE_FUNC inline int
-plogical_shift_right(const int& a) { return static_cast<int>(static_cast<unsigned int>(a) >> N); }
-template<int N> EIGEN_DEVICE_FUNC inline long int
-plogical_shift_right(const long int& a) { return static_cast<long>(static_cast<unsigned long>(a) >> N); }
+template <int N>
+EIGEN_DEVICE_FUNC inline int parithmetic_shift_right(const int& a) {
+  return a >> N;
+}
+template <int N>
+EIGEN_DEVICE_FUNC inline long int parithmetic_shift_right(const long int& a) {
+  return a >> N;
+}
+
+/** \internal \returns \a a logically shifted by N bits to the right */
+template <int N>
+EIGEN_DEVICE_FUNC inline int plogical_shift_right(const int& a) {
+  return static_cast<int>(static_cast<unsigned int>(a) >> N);
+}
+template <int N>
+EIGEN_DEVICE_FUNC inline long int plogical_shift_right(const long int& a) {
+  return static_cast<long>(static_cast<unsigned long>(a) >> N);
+}
 
 /** \internal \returns \a a shifted by N bits to the left */
-template<int N> EIGEN_DEVICE_FUNC inline int
-plogical_shift_left(const int& a) { return a << N; }
-template<int N> EIGEN_DEVICE_FUNC inline long int
-plogical_shift_left(const long int& a) { return a << N; }
+template <int N>
+EIGEN_DEVICE_FUNC inline int plogical_shift_left(const int& a) {
+  return a << N;
+}
+template <int N>
+EIGEN_DEVICE_FUNC inline long int plogical_shift_left(const long int& a) {
+  return a << N;
+}
 
 /** \internal \returns the significant and exponent of the underlying floating point numbers
-  * See https://en.cppreference.com/w/cpp/numeric/math/frexp
-  */
+ * See https://en.cppreference.com/w/cpp/numeric/math/frexp
+ */
 template <typename Packet>
 EIGEN_DEVICE_FUNC inline Packet pfrexp(const Packet& a, Packet& exponent) {
   int exp;
@@ -582,142 +720,238 @@
 }
 
 /** \internal \returns a * 2^((int)exponent)
-  * See https://en.cppreference.com/w/cpp/numeric/math/ldexp
-  */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pldexp(const Packet &a, const Packet &exponent) {
+ * See https://en.cppreference.com/w/cpp/numeric/math/ldexp
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pldexp(const Packet& a, const Packet& exponent) {
   EIGEN_USING_STD(ldexp)
   return static_cast<Packet>(ldexp(a, static_cast<int>(exponent)));
 }
 
 /** \internal \returns the min of \a a and \a b  (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pabsdiff(const Packet& a, const Packet& b) { return pselect(pcmp_lt(a, b), psub(b, a), psub(a, b)); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pabsdiff(const Packet& a, const Packet& b) {
+  return pselect(pcmp_lt(a, b), psub(b, a), psub(a, b));
+}
 
-/** \internal \returns a packet version of \a *from, from must be 16 bytes aligned */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pload(const typename unpacket_traits<Packet>::type* from) { return *from; }
+/** \internal \returns a packet version of \a *from, from must be properly aligned */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pload(const typename unpacket_traits<Packet>::type* from) {
+  return *from;
+}
+
+/** \internal \returns n elements of a packet version of \a *from, from must be properly aligned
+ * offset indicates the starting element in which to load and
+ * offset + n <= unpacket_traits::size
+ * All elements before offset and after the last element loaded will initialized with zero */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pload_partial(const typename unpacket_traits<Packet>::type* from, const Index n,
+                                              const Index offset = 0) {
+  const Index packet_size = unpacket_traits<Packet>::size;
+  eigen_assert(n + offset <= packet_size && "number of elements plus offset will read past end of packet");
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  EIGEN_ALIGN_MAX Scalar elements[packet_size] = {Scalar(0)};
+  for (Index i = offset; i < numext::mini(n + offset, packet_size); i++) {
+    elements[i] = from[i - offset];
+  }
+  return pload<Packet>(elements);
+}
 
 /** \internal \returns a packet version of \a *from, (un-aligned load) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-ploadu(const typename unpacket_traits<Packet>::type* from) { return *from; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet ploadu(const typename unpacket_traits<Packet>::type* from) {
+  return *from;
+}
+
+/** \internal \returns n elements of a packet version of \a *from, (un-aligned load)
+ * All elements after the last element loaded will initialized with zero */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet ploadu_partial(const typename unpacket_traits<Packet>::type* from, const Index n,
+                                               const Index offset = 0) {
+  const Index packet_size = unpacket_traits<Packet>::size;
+  eigen_assert(n + offset <= packet_size && "number of elements plus offset will read past end of packet");
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  EIGEN_ALIGN_MAX Scalar elements[packet_size] = {Scalar(0)};
+  for (Index i = offset; i < numext::mini(n + offset, packet_size); i++) {
+    elements[i] = from[i - offset];
+  }
+  return pload<Packet>(elements);
+}
 
 /** \internal \returns a packet version of \a *from, (un-aligned masked load)
  * There is no generic implementation. We only have implementations for specialized
  * cases. Generic case should not be called.
  */
-template<typename Packet> EIGEN_DEVICE_FUNC inline
-typename enable_if<unpacket_traits<Packet>::masked_load_available, Packet>::type
-ploadu(const typename unpacket_traits<Packet>::type* from, typename unpacket_traits<Packet>::mask_t umask);
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline std::enable_if_t<unpacket_traits<Packet>::masked_load_available, Packet> ploadu(
+    const typename unpacket_traits<Packet>::type* from, typename unpacket_traits<Packet>::mask_t umask);
 
 /** \internal \returns a packet with constant coefficients \a a, e.g.: (a,a,a,a) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pset1(const typename unpacket_traits<Packet>::type& a) { return a; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pset1(const typename unpacket_traits<Packet>::type& a) {
+  return a;
+}
 
 /** \internal \returns a packet with constant coefficients set from bits */
-template<typename Packet,typename BitsType> EIGEN_DEVICE_FUNC inline Packet
-pset1frombits(BitsType a);
+template <typename Packet, typename BitsType>
+EIGEN_DEVICE_FUNC inline Packet pset1frombits(BitsType a);
 
 /** \internal \returns a packet with constant coefficients \a a[0], e.g.: (a[0],a[0],a[0],a[0]) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pload1(const typename unpacket_traits<Packet>::type  *a) { return pset1<Packet>(*a); }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pload1(const typename unpacket_traits<Packet>::type* a) {
+  return pset1<Packet>(*a);
+}
 
 /** \internal \returns a packet with elements of \a *from duplicated.
-  * For instance, for a packet of 8 elements, 4 scalars will be read from \a *from and
-  * duplicated to form: {from[0],from[0],from[1],from[1],from[2],from[2],from[3],from[3]}
-  * Currently, this function is only used for scalar * complex products.
-  */
-template<typename Packet> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet
-ploaddup(const typename unpacket_traits<Packet>::type* from) { return *from; }
+ * For instance, for a packet of 8 elements, 4 scalars will be read from \a *from and
+ * duplicated to form: {from[0],from[0],from[1],from[1],from[2],from[2],from[3],from[3]}
+ * Currently, this function is only used for scalar * complex products.
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet ploaddup(const typename unpacket_traits<Packet>::type* from) {
+  return *from;
+}
 
 /** \internal \returns a packet with elements of \a *from quadrupled.
-  * For instance, for a packet of 8 elements, 2 scalars will be read from \a *from and
-  * replicated to form: {from[0],from[0],from[0],from[0],from[1],from[1],from[1],from[1]}
-  * Currently, this function is only used in matrix products.
-  * For packet-size smaller or equal to 4, this function is equivalent to pload1
-  */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-ploadquad(const typename unpacket_traits<Packet>::type* from)
-{ return pload1<Packet>(from); }
-
-/** \internal equivalent to
-  * \code
-  * a0 = pload1(a+0);
-  * a1 = pload1(a+1);
-  * a2 = pload1(a+2);
-  * a3 = pload1(a+3);
-  * \endcode
-  * \sa pset1, pload1, ploaddup, pbroadcast2
-  */
-template<typename Packet> EIGEN_DEVICE_FUNC
-inline void pbroadcast4(const typename unpacket_traits<Packet>::type *a,
-                        Packet& a0, Packet& a1, Packet& a2, Packet& a3)
-{
-  a0 = pload1<Packet>(a+0);
-  a1 = pload1<Packet>(a+1);
-  a2 = pload1<Packet>(a+2);
-  a3 = pload1<Packet>(a+3);
+ * For instance, for a packet of 8 elements, 2 scalars will be read from \a *from and
+ * replicated to form: {from[0],from[0],from[0],from[0],from[1],from[1],from[1],from[1]}
+ * Currently, this function is only used in matrix products.
+ * For packet-size smaller or equal to 4, this function is equivalent to pload1
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet ploadquad(const typename unpacket_traits<Packet>::type* from) {
+  return pload1<Packet>(from);
 }
 
 /** \internal equivalent to
-  * \code
-  * a0 = pload1(a+0);
-  * a1 = pload1(a+1);
-  * \endcode
-  * \sa pset1, pload1, ploaddup, pbroadcast4
-  */
-template<typename Packet> EIGEN_DEVICE_FUNC
-inline void pbroadcast2(const typename unpacket_traits<Packet>::type *a,
-                        Packet& a0, Packet& a1)
-{
-  a0 = pload1<Packet>(a+0);
-  a1 = pload1<Packet>(a+1);
+ * \code
+ * a0 = pload1(a+0);
+ * a1 = pload1(a+1);
+ * a2 = pload1(a+2);
+ * a3 = pload1(a+3);
+ * \endcode
+ * \sa pset1, pload1, ploaddup, pbroadcast2
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline void pbroadcast4(const typename unpacket_traits<Packet>::type* a, Packet& a0, Packet& a1,
+                                          Packet& a2, Packet& a3) {
+  a0 = pload1<Packet>(a + 0);
+  a1 = pload1<Packet>(a + 1);
+  a2 = pload1<Packet>(a + 2);
+  a3 = pload1<Packet>(a + 3);
+}
+
+/** \internal equivalent to
+ * \code
+ * a0 = pload1(a+0);
+ * a1 = pload1(a+1);
+ * \endcode
+ * \sa pset1, pload1, ploaddup, pbroadcast4
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline void pbroadcast2(const typename unpacket_traits<Packet>::type* a, Packet& a0, Packet& a1) {
+  a0 = pload1<Packet>(a + 0);
+  a1 = pload1<Packet>(a + 1);
 }
 
 /** \internal \brief Returns a packet with coefficients (a,a+1,...,a+packet_size-1). */
-template<typename Packet> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet
-plset(const typename unpacket_traits<Packet>::type& a) { return a; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet plset(const typename unpacket_traits<Packet>::type& a) {
+  return a;
+}
 
 /** \internal \returns a packet with constant coefficients \a a, e.g.: (x, 0, x, 0),
      where x is the value of all 1-bits. */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-peven_mask(const Packet& /*a*/) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet peven_mask(const Packet& /*a*/) {
   typedef typename unpacket_traits<Packet>::type Scalar;
   const size_t n = unpacket_traits<Packet>::size;
   EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n];
-  for(size_t i = 0; i < n; ++i) {
-    memset(elements+i, ((i & 1) == 0 ? 0xff : 0), sizeof(Scalar));
+  for (size_t i = 0; i < n; ++i) {
+    memset(elements + i, ((i & 1) == 0 ? 0xff : 0), sizeof(Scalar));
   }
   return ploadu<Packet>(elements);
 }
 
+/** \internal copy the packet \a from to \a *to, \a to must be properly aligned */
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline void pstore(Scalar* to, const Packet& from) {
+  (*to) = from;
+}
 
-/** \internal copy the packet \a from to \a *to, \a to must be 16 bytes aligned */
-template<typename Scalar, typename Packet> EIGEN_DEVICE_FUNC inline void pstore(Scalar* to, const Packet& from)
-{ (*to) = from; }
+/** \internal copy n elements of the packet \a from to \a *to, \a to must be properly aligned
+ * offset indicates the starting element in which to store and
+ * offset + n <= unpacket_traits::size */
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline void pstore_partial(Scalar* to, const Packet& from, const Index n, const Index offset = 0) {
+  const Index packet_size = unpacket_traits<Packet>::size;
+  eigen_assert(n + offset <= packet_size && "number of elements plus offset will write past end of packet");
+  EIGEN_ALIGN_MAX Scalar elements[packet_size];
+  pstore<Scalar>(elements, from);
+  for (Index i = 0; i < numext::mini(n, packet_size - offset); i++) {
+    to[i] = elements[i + offset];
+  }
+}
 
 /** \internal copy the packet \a from to \a *to, (un-aligned store) */
-template<typename Scalar, typename Packet> EIGEN_DEVICE_FUNC inline void pstoreu(Scalar* to, const Packet& from)
-{  (*to) = from; }
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline void pstoreu(Scalar* to, const Packet& from) {
+  (*to) = from;
+}
+
+/** \internal copy n elements of the packet \a from to \a *to, (un-aligned store) */
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline void pstoreu_partial(Scalar* to, const Packet& from, const Index n, const Index offset = 0) {
+  const Index packet_size = unpacket_traits<Packet>::size;
+  eigen_assert(n + offset <= packet_size && "number of elements plus offset will write past end of packet");
+  EIGEN_ALIGN_MAX Scalar elements[packet_size];
+  pstore<Scalar>(elements, from);
+  for (Index i = 0; i < numext::mini(n, packet_size - offset); i++) {
+    to[i] = elements[i + offset];
+  }
+}
 
 /** \internal copy the packet \a from to \a *to, (un-aligned store with a mask)
  * There is no generic implementation. We only have implementations for specialized
  * cases. Generic case should not be called.
  */
-template<typename Scalar, typename Packet>
-EIGEN_DEVICE_FUNC inline
-typename enable_if<unpacket_traits<Packet>::masked_store_available, void>::type
-pstoreu(Scalar* to, const Packet& from, typename unpacket_traits<Packet>::mask_t umask);
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline std::enable_if_t<unpacket_traits<Packet>::masked_store_available, void> pstoreu(
+    Scalar* to, const Packet& from, typename unpacket_traits<Packet>::mask_t umask);
 
- template<typename Scalar, typename Packet> EIGEN_DEVICE_FUNC inline Packet pgather(const Scalar* from, Index /*stride*/)
- { return ploadu<Packet>(from); }
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pgather(const Scalar* from, Index /*stride*/) {
+  return ploadu<Packet>(from);
+}
 
- template<typename Scalar, typename Packet> EIGEN_DEVICE_FUNC inline void pscatter(Scalar* to, const Packet& from, Index /*stride*/)
- { pstore(to, from); }
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pgather_partial(const Scalar* from, Index stride, const Index n) {
+  const Index packet_size = unpacket_traits<Packet>::size;
+  EIGEN_ALIGN_MAX Scalar elements[packet_size] = {Scalar(0)};
+  for (Index i = 0; i < numext::mini(n, packet_size); i++) {
+    elements[i] = from[i * stride];
+  }
+  return pload<Packet>(elements);
+}
+
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline void pscatter(Scalar* to, const Packet& from, Index /*stride*/) {
+  pstore(to, from);
+}
+
+template <typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline void pscatter_partial(Scalar* to, const Packet& from, Index stride, const Index n) {
+  const Index packet_size = unpacket_traits<Packet>::size;
+  EIGEN_ALIGN_MAX Scalar elements[packet_size];
+  pstore<Scalar>(elements, from);
+  for (Index i = 0; i < numext::mini(n, packet_size); i++) {
+    to[i * stride] = elements[i];
+  }
+}
 
 /** \internal tries to do cache prefetching of \a addr */
-template<typename Scalar> EIGEN_DEVICE_FUNC inline void prefetch(const Scalar* addr)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline void prefetch(const Scalar* addr) {
 #if defined(EIGEN_HIP_DEVICE_COMPILE)
   // do nothing
 #elif defined(EIGEN_CUDA_ARCH)
@@ -734,135 +968,214 @@
 }
 
 /** \internal \returns the reversed elements of \a a*/
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet preverse(const Packet& a)
-{ return a; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet preverse(const Packet& a) {
+  return a;
+}
 
 /** \internal \returns \a a with real and imaginary part flipped (for complex type only) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet pcplxflip(const Packet& a)
-{
-  return Packet(numext::imag(a),numext::real(a));
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pcplxflip(const Packet& a) {
+  return Packet(numext::imag(a), numext::real(a));
 }
 
 /**************************
-* Special math functions
-***************************/
+ * Special math functions
+ ***************************/
 
 /** \internal \returns the sine of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet psin(const Packet& a) { EIGEN_USING_STD(sin); return sin(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psin(const Packet& a) {
+  EIGEN_USING_STD(sin);
+  return sin(a);
+}
 
 /** \internal \returns the cosine of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pcos(const Packet& a) { EIGEN_USING_STD(cos); return cos(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcos(const Packet& a) {
+  EIGEN_USING_STD(cos);
+  return cos(a);
+}
 
 /** \internal \returns the tan of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet ptan(const Packet& a) { EIGEN_USING_STD(tan); return tan(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet ptan(const Packet& a) {
+  EIGEN_USING_STD(tan);
+  return tan(a);
+}
 
 /** \internal \returns the arc sine of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pasin(const Packet& a) { EIGEN_USING_STD(asin); return asin(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pasin(const Packet& a) {
+  EIGEN_USING_STD(asin);
+  return asin(a);
+}
 
 /** \internal \returns the arc cosine of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pacos(const Packet& a) { EIGEN_USING_STD(acos); return acos(a); }
-
-/** \internal \returns the arc tangent of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet patan(const Packet& a) { EIGEN_USING_STD(atan); return atan(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pacos(const Packet& a) {
+  EIGEN_USING_STD(acos);
+  return acos(a);
+}
 
 /** \internal \returns the hyperbolic sine of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet psinh(const Packet& a) { EIGEN_USING_STD(sinh); return sinh(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psinh(const Packet& a) {
+  EIGEN_USING_STD(sinh);
+  return sinh(a);
+}
 
 /** \internal \returns the hyperbolic cosine of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pcosh(const Packet& a) { EIGEN_USING_STD(cosh); return cosh(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcosh(const Packet& a) {
+  EIGEN_USING_STD(cosh);
+  return cosh(a);
+}
+
+/** \internal \returns the arc tangent of \a a (coeff-wise) */
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan(const Packet& a) {
+  EIGEN_USING_STD(atan);
+  return atan(a);
+}
 
 /** \internal \returns the hyperbolic tan of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet ptanh(const Packet& a) { EIGEN_USING_STD(tanh); return tanh(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet ptanh(const Packet& a) {
+  EIGEN_USING_STD(tanh);
+  return tanh(a);
+}
+
+/** \internal \returns the arc tangent of \a a (coeff-wise) */
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patanh(const Packet& a) {
+  EIGEN_USING_STD(atanh);
+  return atanh(a);
+}
 
 /** \internal \returns the exp of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pexp(const Packet& a) { EIGEN_USING_STD(exp); return exp(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp(const Packet& a) {
+  EIGEN_USING_STD(exp);
+  return exp(a);
+}
 
 /** \internal \returns the expm1 of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pexpm1(const Packet& a) { return numext::expm1(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexpm1(const Packet& a) {
+  return numext::expm1(a);
+}
 
 /** \internal \returns the log of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet plog(const Packet& a) { EIGEN_USING_STD(log); return log(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog(const Packet& a) {
+  EIGEN_USING_STD(log);
+  return log(a);
+}
 
 /** \internal \returns the log1p of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet plog1p(const Packet& a) { return numext::log1p(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog1p(const Packet& a) {
+  return numext::log1p(a);
+}
 
 /** \internal \returns the log10 of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet plog10(const Packet& a) { EIGEN_USING_STD(log10); return log10(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog10(const Packet& a) {
+  EIGEN_USING_STD(log10);
+  return log10(a);
+}
 
 /** \internal \returns the log10 of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet plog2(const Packet& a) {
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog2(const Packet& a) {
   typedef typename internal::unpacket_traits<Packet>::type Scalar;
-  return pmul(pset1<Packet>(Scalar(EIGEN_LOG2E)), plog(a)); 
+  return pmul(pset1<Packet>(Scalar(EIGEN_LOG2E)), plog(a));
 }
 
 /** \internal \returns the square-root of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet psqrt(const Packet& a) { return numext::sqrt(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psqrt(const Packet& a) {
+  return numext::sqrt(a);
+}
 
-/** \internal \returns the reciprocal square-root of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet prsqrt(const Packet& a) {
-  typedef typename internal::unpacket_traits<Packet>::type Scalar;
-  return pdiv(pset1<Packet>(Scalar(1)), psqrt(a));
+/** \internal \returns the cube-root of \a a (coeff-wise) */
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcbrt(const Packet& a) {
+  return numext::cbrt(a);
 }
 
 /** \internal \returns the rounded value of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pround(const Packet& a) { using numext::round; return round(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pround(const Packet& a) {
+  using numext::round;
+  return round(a);
+}
 
 /** \internal \returns the floor of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pfloor(const Packet& a) { using numext::floor; return floor(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pfloor(const Packet& a) {
+  using numext::floor;
+  return floor(a);
+}
 
 /** \internal \returns the rounded value of \a a (coeff-wise) with current
  * rounding mode */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet print(const Packet& a) { using numext::rint; return rint(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet print(const Packet& a) {
+  using numext::rint;
+  return rint(a);
+}
 
 /** \internal \returns the ceil of \a a (coeff-wise) */
-template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pceil(const Packet& a) { using numext::ceil; return ceil(a); }
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pceil(const Packet& a) {
+  using numext::ceil;
+  return ceil(a);
+}
+
+template <typename Packet, typename EnableIf = void>
+struct psign_impl {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a) { return numext::sign(a); }
+};
+
+/** \internal \returns the sign of \a a (coeff-wise) */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet psign(const Packet& a) {
+  return psign_impl<Packet>::run(a);
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline bool psign(const bool& a) {
+  return a;
+}
 
 /** \internal \returns the first element of a packet */
-template<typename Packet>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type
-pfirst(const Packet& a)
-{ return a; }
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type pfirst(const Packet& a) {
+  return a;
+}
 
 /** \internal \returns the sum of the elements of upper and lower half of \a a if \a a is larger than 4.
-  * For a packet {a0, a1, a2, a3, a4, a5, a6, a7}, it returns a half packet {a0+a4, a1+a5, a2+a6, a3+a7}
-  * For packet-size smaller or equal to 4, this boils down to a noop.
-  */
-template<typename Packet>
-EIGEN_DEVICE_FUNC inline typename conditional<(unpacket_traits<Packet>::size%8)==0,typename unpacket_traits<Packet>::half,Packet>::type
-predux_half_dowto4(const Packet& a)
-{ return a; }
+ * For a packet {a0, a1, a2, a3, a4, a5, a6, a7}, it returns a half packet {a0+a4, a1+a5, a2+a6, a3+a7}
+ * For packet-size smaller or equal to 4, this boils down to a noop.
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline std::conditional_t<(unpacket_traits<Packet>::size % 8) == 0,
+                                            typename unpacket_traits<Packet>::half, Packet>
+predux_half_dowto4(const Packet& a) {
+  return a;
+}
 
 // Slow generic implementation of Packet reduction.
 template <typename Packet, typename Op>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type
-predux_helper(const Packet& a, Op op) {
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_helper(const Packet& a, Op op) {
   typedef typename unpacket_traits<Packet>::type Scalar;
   const size_t n = unpacket_traits<Packet>::size;
   EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n];
   pstoreu<Scalar>(elements, a);
-  for(size_t k = n / 2; k > 0; k /= 2)  {
-    for(size_t i = 0; i < k; ++i) {
+  for (size_t k = n / 2; k > 0; k /= 2) {
+    for (size_t i = 0; i < k; ++i) {
       elements[i] = op(elements[i], elements[i + k]);
     }
   }
@@ -870,65 +1183,58 @@
 }
 
 /** \internal \returns the sum of the elements of \a a*/
-template<typename Packet>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type
-predux(const Packet& a)
-{
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux(const Packet& a) {
   return a;
 }
 
 /** \internal \returns the product of the elements of \a a */
 template <typename Packet>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_mul(
-    const Packet& a) {
-  typedef typename unpacket_traits<Packet>::type Scalar; 
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_mul(const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
   return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmul<Scalar>)));
 }
 
 /** \internal \returns the min of the elements of \a a */
 template <typename Packet>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_min(
-    const Packet &a) {
-  typedef typename unpacket_traits<Packet>::type Scalar; 
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_min(const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
   return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin<PropagateFast, Scalar>)));
 }
 
 template <int NaNPropagation, typename Packet>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_min(
-    const Packet& a) {
-  typedef typename unpacket_traits<Packet>::type Scalar; 
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_min(const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
   return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin<NaNPropagation, Scalar>)));
 }
 
 /** \internal \returns the min of the elements of \a a */
 template <typename Packet>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_max(
-    const Packet &a) {
-  typedef typename unpacket_traits<Packet>::type Scalar; 
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_max(const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
   return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax<PropagateFast, Scalar>)));
 }
 
 template <int NaNPropagation, typename Packet>
-EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_max(
-    const Packet& a) {
-  typedef typename unpacket_traits<Packet>::type Scalar; 
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_max(const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
   return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax<NaNPropagation, Scalar>)));
 }
 
 #undef EIGEN_BINARY_OP_NAN_PROPAGATION
 
 /** \internal \returns true if all coeffs of \a a means "true"
-  * It is supposed to be called on values returned by pcmp_*.
-  */
+ * It is supposed to be called on values returned by pcmp_*.
+ */
 // not needed yet
 // template<typename Packet> EIGEN_DEVICE_FUNC inline bool predux_all(const Packet& a)
 // { return bool(a); }
 
 /** \internal \returns true if any coeffs of \a a means "true"
-  * It is supposed to be called on values returned by pcmp_*.
-  */
-template<typename Packet> EIGEN_DEVICE_FUNC inline bool predux_any(const Packet& a)
-{
+ * It is supposed to be called on values returned by pcmp_*.
+ */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline bool predux_any(const Packet& a) {
   // Dirty but generic implementation where "true" is assumed to be non 0 and all the sames.
   // It is expected that "true" is either:
   //  - Scalar(1)
@@ -940,101 +1246,242 @@
 }
 
 /***************************************************************************
-* The following functions might not have to be overwritten for vectorized types
-***************************************************************************/
+ * The following functions might not have to be overwritten for vectorized types
+ ***************************************************************************/
 
-/** \internal copy a packet with constant coefficient \a a (e.g., [a,a,a,a]) to \a *to. \a to must be 16 bytes aligned */
-// NOTE: this function must really be templated on the packet type (think about different packet types for the same scalar type)
-template<typename Packet>
-inline void pstore1(typename unpacket_traits<Packet>::type* to, const typename unpacket_traits<Packet>::type& a)
-{
+// FMA instructions.
+/** \internal \returns a * b + c (coeff-wise) */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pmadd(const Packet& a, const Packet& b, const Packet& c) {
+  return padd(pmul(a, b), c);
+}
+
+/** \internal \returns a * b - c (coeff-wise) */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pmsub(const Packet& a, const Packet& b, const Packet& c) {
+  return psub(pmul(a, b), c);
+}
+
+/** \internal \returns -(a * b) + c (coeff-wise) */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pnmadd(const Packet& a, const Packet& b, const Packet& c) {
+  return padd(pnegate(pmul(a, b)), c);
+}
+
+/** \internal \returns -(a * b) - c (coeff-wise) */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pnmsub(const Packet& a, const Packet& b, const Packet& c) {
+  return psub(pnegate(pmul(a, b)), c);
+}
+
+/** \internal copy a packet with constant coefficient \a a (e.g., [a,a,a,a]) to \a *to. \a to must be 16 bytes aligned
+ */
+// NOTE: this function must really be templated on the packet type (think about different packet types for the same
+// scalar type)
+template <typename Packet>
+inline void pstore1(typename unpacket_traits<Packet>::type* to, const typename unpacket_traits<Packet>::type& a) {
   pstore(to, pset1<Packet>(a));
 }
 
-/** \internal \returns a * b + c (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pmadd(const Packet&  a,
-         const Packet&  b,
-         const Packet&  c)
-{ return padd(pmul(a, b),c); }
-
 /** \internal \returns a packet version of \a *from.
-  * The pointer \a from must be aligned on a \a Alignment bytes boundary. */
-template<typename Packet, int Alignment>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt(const typename unpacket_traits<Packet>::type* from)
-{
-  if(Alignment >= unpacket_traits<Packet>::alignment)
+ * The pointer \a from must be aligned on a \a Alignment bytes boundary. */
+template <typename Packet, int Alignment>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt(const typename unpacket_traits<Packet>::type* from) {
+  if (Alignment >= unpacket_traits<Packet>::alignment)
     return pload<Packet>(from);
   else
     return ploadu<Packet>(from);
 }
 
+/** \internal \returns n elements of a packet version of \a *from.
+ * The pointer \a from must be aligned on a \a Alignment bytes boundary. */
+template <typename Packet, int Alignment>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt_partial(const typename unpacket_traits<Packet>::type* from,
+                                                            const Index n, const Index offset = 0) {
+  if (Alignment >= unpacket_traits<Packet>::alignment)
+    return pload_partial<Packet>(from, n, offset);
+  else
+    return ploadu_partial<Packet>(from, n, offset);
+}
+
 /** \internal copy the packet \a from to \a *to.
-  * The pointer \a from must be aligned on a \a Alignment bytes boundary. */
-template<typename Scalar, typename Packet, int Alignment>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void pstoret(Scalar* to, const Packet& from)
-{
-  if(Alignment >= unpacket_traits<Packet>::alignment)
+ * The pointer \a from must be aligned on a \a Alignment bytes boundary. */
+template <typename Scalar, typename Packet, int Alignment>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void pstoret(Scalar* to, const Packet& from) {
+  if (Alignment >= unpacket_traits<Packet>::alignment)
     pstore(to, from);
   else
     pstoreu(to, from);
 }
 
+/** \internal copy n elements of the packet \a from to \a *to.
+ * The pointer \a from must be aligned on a \a Alignment bytes boundary. */
+template <typename Scalar, typename Packet, int Alignment>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void pstoret_partial(Scalar* to, const Packet& from, const Index n,
+                                                           const Index offset = 0) {
+  if (Alignment >= unpacket_traits<Packet>::alignment)
+    pstore_partial(to, from, n, offset);
+  else
+    pstoreu_partial(to, from, n, offset);
+}
+
 /** \internal \returns a packet version of \a *from.
-  * Unlike ploadt, ploadt_ro takes advantage of the read-only memory path on the
-  * hardware if available to speedup the loading of data that won't be modified
-  * by the current computation.
-  */
-template<typename Packet, int LoadMode>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt_ro(const typename unpacket_traits<Packet>::type* from)
-{
+ * Unlike ploadt, ploadt_ro takes advantage of the read-only memory path on the
+ * hardware if available to speedup the loading of data that won't be modified
+ * by the current computation.
+ */
+template <typename Packet, int LoadMode>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt_ro(const typename unpacket_traits<Packet>::type* from) {
   return ploadt<Packet, LoadMode>(from);
 }
 
 /***************************************************************************
-* Fast complex products (GCC generates a function call which is very slow)
-***************************************************************************/
+ * Fast complex products (GCC generates a function call which is very slow)
+ ***************************************************************************/
 
 // Eigen+CUDA does not support complexes.
 #if !defined(EIGEN_GPUCC)
 
-template<> inline std::complex<float> pmul(const std::complex<float>& a, const std::complex<float>& b)
-{ return std::complex<float>(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); }
+template <>
+inline std::complex<float> pmul(const std::complex<float>& a, const std::complex<float>& b) {
+  return std::complex<float>(a.real() * b.real() - a.imag() * b.imag(), a.imag() * b.real() + a.real() * b.imag());
+}
 
-template<> inline std::complex<double> pmul(const std::complex<double>& a, const std::complex<double>& b)
-{ return std::complex<double>(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); }
+template <>
+inline std::complex<double> pmul(const std::complex<double>& a, const std::complex<double>& b) {
+  return std::complex<double>(a.real() * b.real() - a.imag() * b.imag(), a.imag() * b.real() + a.real() * b.imag());
+}
 
 #endif
 
-
 /***************************************************************************
  * PacketBlock, that is a collection of N packets where the number of words
  * in the packet is a multiple of N.
-***************************************************************************/
-template <typename Packet,int N=unpacket_traits<Packet>::size> struct PacketBlock {
+ ***************************************************************************/
+template <typename Packet, int N = unpacket_traits<Packet>::size>
+struct PacketBlock {
   Packet packet[N];
 };
 
-template<typename Packet> EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet,1>& /*kernel*/) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet, 1>& /*kernel*/) {
   // Nothing to do in the scalar case, i.e. a 1x1 matrix.
 }
 
 /***************************************************************************
  * Selector, i.e. vector of N boolean values used to select (i.e. blend)
  * words from 2 packets.
-***************************************************************************/
-template <size_t N> struct Selector {
+ ***************************************************************************/
+template <size_t N>
+struct Selector {
   bool select[N];
 };
 
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pblend(const Selector<unpacket_traits<Packet>::size>& ifPacket, const Packet& thenPacket, const Packet& elsePacket) {
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pblend(const Selector<unpacket_traits<Packet>::size>& ifPacket,
+                                       const Packet& thenPacket, const Packet& elsePacket) {
   return ifPacket.select[0] ? thenPacket : elsePacket;
 }
 
-} // end namespace internal
+/** \internal \returns 1 / a (coeff-wise) */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet preciprocal(const Packet& a) {
+  using Scalar = typename unpacket_traits<Packet>::type;
+  return pdiv(pset1<Packet>(Scalar(1)), a);
+}
 
-} // end namespace Eigen
+/** \internal \returns the reciprocal square-root of \a a (coeff-wise) */
+template <typename Packet>
+EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet prsqrt(const Packet& a) {
+  return preciprocal<Packet>(psqrt(a));
+}
 
-#endif // EIGEN_GENERIC_PACKET_MATH_H
+template <typename Packet, bool IsScalar = is_scalar<Packet>::value,
+          bool IsInteger = NumTraits<typename unpacket_traits<Packet>::type>::IsInteger>
+struct psignbit_impl;
+template <typename Packet, bool IsInteger>
+struct psignbit_impl<Packet, true, IsInteger> {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Packet run(const Packet& a) { return numext::signbit(a); }
+};
+template <typename Packet>
+struct psignbit_impl<Packet, false, false> {
+  // generic implementation if not specialized in PacketMath.h
+  // slower than arithmetic shift
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static Packet run(const Packet& a) {
+    const Packet cst_pos_one = pset1<Packet>(Scalar(1));
+    const Packet cst_neg_one = pset1<Packet>(Scalar(-1));
+    return pcmp_eq(por(pand(a, cst_neg_one), cst_pos_one), cst_neg_one);
+  }
+};
+template <typename Packet>
+struct psignbit_impl<Packet, false, true> {
+  // generic implementation for integer packets
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Packet run(const Packet& a) { return pcmp_lt(a, pzero(a)); }
+};
+/** \internal \returns the sign bit of \a a as a bitmask*/
+template <typename Packet>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE constexpr Packet psignbit(const Packet& a) {
+  return psignbit_impl<Packet>::run(a);
+}
+
+/** \internal \returns the 2-argument arc tangent of \a y and \a x (coeff-wise) */
+template <typename Packet, std::enable_if_t<is_scalar<Packet>::value, int> = 0>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet patan2(const Packet& y, const Packet& x) {
+  return numext::atan2(y, x);
+}
+
+/** \internal \returns the 2-argument arc tangent of \a y and \a x (coeff-wise) */
+template <typename Packet, std::enable_if_t<!is_scalar<Packet>::value, int> = 0>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet patan2(const Packet& y, const Packet& x) {
+  typedef typename internal::unpacket_traits<Packet>::type Scalar;
+
+  // See https://en.cppreference.com/w/cpp/numeric/math/atan2
+  // for how corner cases are supposed to be handled according to the
+  // IEEE floating-point standard (IEC 60559).
+  const Packet kSignMask = pset1<Packet>(-Scalar(0));
+  const Packet kZero = pzero(x);
+  const Packet kOne = pset1<Packet>(Scalar(1));
+  const Packet kPi = pset1<Packet>(Scalar(EIGEN_PI));
+
+  const Packet x_has_signbit = psignbit(x);
+  const Packet y_signmask = pand(y, kSignMask);
+  const Packet x_signmask = pand(x, kSignMask);
+  const Packet result_signmask = pxor(y_signmask, x_signmask);
+  const Packet shift = por(pand(x_has_signbit, kPi), y_signmask);
+
+  const Packet x_and_y_are_same = pcmp_eq(pabs(x), pabs(y));
+  const Packet x_and_y_are_zero = pcmp_eq(por(x, y), kZero);
+
+  Packet arg = pdiv(y, x);
+  arg = pselect(x_and_y_are_same, por(kOne, result_signmask), arg);
+  arg = pselect(x_and_y_are_zero, result_signmask, arg);
+
+  Packet result = patan(arg);
+  result = padd(result, shift);
+  return result;
+}
+
+/** \internal \returns the argument of \a a as a complex number */
+template <typename Packet, std::enable_if_t<is_scalar<Packet>::value, int> = 0>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pcarg(const Packet& a) {
+  return Packet(numext::arg(a));
+}
+
+/** \internal \returns the argument of \a a as a complex number */
+template <typename Packet, std::enable_if_t<!is_scalar<Packet>::value, int> = 0>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pcarg(const Packet& a) {
+  EIGEN_STATIC_ASSERT(NumTraits<typename unpacket_traits<Packet>::type>::IsComplex,
+                      THIS METHOD IS FOR COMPLEX TYPES ONLY)
+  using RealPacket = typename unpacket_traits<Packet>::as_real;
+  // a                                              // r     i    r     i    ...
+  RealPacket aflip = pcplxflip(a).v;                // i     r    i     r    ...
+  RealPacket result = patan2(aflip, a.v);           // atan2 crap atan2 crap ...
+  return (Packet)pand(result, peven_mask(result));  // atan2 0    atan2 0    ...
+}
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_GENERIC_PACKET_MATH_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h
index 629af94..f0ae5a8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h
@@ -13,182 +13,214 @@
 
 #ifdef EIGEN_PARSED_BY_DOXYGEN
 
-#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME,FUNCTOR,DOC_OP,DOC_DETAILS) \
-  /** \returns an expression of the coefficient-wise DOC_OP of \a x
-
-    DOC_DETAILS
-
-    \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_##NAME">Math functions</a>, class CwiseUnaryOp
-    */ \
-  template<typename Derived> \
-  inline const Eigen::CwiseUnaryOp<Eigen::internal::FUNCTOR<typename Derived::Scalar>, const Derived> \
-  NAME(const Eigen::ArrayBase<Derived>& x);
+#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME, FUNCTOR, DOC_OP, DOC_DETAILS)                                    \
+  /** \returns an expression of the coefficient-wise DOC_OP of \a x                                             \
+                                                                                                              \ \
+    DOC_DETAILS                                                                                                 \
+                                                                                                              \ \
+    \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_##NAME">Math functions</a>, class CwiseUnaryOp   \
+    */                                                                                                          \
+  template <typename Derived>                                                                                   \
+  inline const Eigen::CwiseUnaryOp<Eigen::internal::FUNCTOR<typename Derived::Scalar>, const Derived> NAME(     \
+      const Eigen::ArrayBase<Derived>& x);
 
 #else
 
-#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME,FUNCTOR,DOC_OP,DOC_DETAILS) \
-  template<typename Derived> \
-  inline const Eigen::CwiseUnaryOp<Eigen::internal::FUNCTOR<typename Derived::Scalar>, const Derived> \
-  (NAME)(const Eigen::ArrayBase<Derived>& x) { \
+#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME, FUNCTOR, DOC_OP, DOC_DETAILS)                                    \
+  template <typename Derived>                                                                                   \
+  inline const Eigen::CwiseUnaryOp<Eigen::internal::FUNCTOR<typename Derived::Scalar>, const Derived>(NAME)(    \
+      const Eigen::ArrayBase<Derived>& x) {                                                                     \
     return Eigen::CwiseUnaryOp<Eigen::internal::FUNCTOR<typename Derived::Scalar>, const Derived>(x.derived()); \
   }
 
-#endif // EIGEN_PARSED_BY_DOXYGEN
+#endif  // EIGEN_PARSED_BY_DOXYGEN
 
-#define EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(NAME,FUNCTOR) \
-  \
-  template<typename Derived> \
-  struct NAME##_retval<ArrayBase<Derived> > \
-  { \
+#define EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(NAME, FUNCTOR)                                                  \
+                                                                                                               \
+  template <typename Derived>                                                                                  \
+  struct NAME##_retval<ArrayBase<Derived> > {                                                                  \
     typedef const Eigen::CwiseUnaryOp<Eigen::internal::FUNCTOR<typename Derived::Scalar>, const Derived> type; \
-  }; \
-  template<typename Derived> \
-  struct NAME##_impl<ArrayBase<Derived> > \
-  { \
-    static inline typename NAME##_retval<ArrayBase<Derived> >::type run(const Eigen::ArrayBase<Derived>& x) \
-    { \
-      return typename NAME##_retval<ArrayBase<Derived> >::type(x.derived()); \
-    } \
+  };                                                                                                           \
+  template <typename Derived>                                                                                  \
+  struct NAME##_impl<ArrayBase<Derived> > {                                                                    \
+    static inline typename NAME##_retval<ArrayBase<Derived> >::type run(const Eigen::ArrayBase<Derived>& x) {  \
+      return typename NAME##_retval<ArrayBase<Derived> >::type(x.derived());                                   \
+    }                                                                                                          \
   };
 
-namespace Eigen
-{
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(real,scalar_real_op,real part,\sa ArrayBase::real)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(imag,scalar_imag_op,imaginary part,\sa ArrayBase::imag)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(conj,scalar_conjugate_op,complex conjugate,\sa ArrayBase::conjugate)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(inverse,scalar_inverse_op,inverse,\sa ArrayBase::inverse)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sin,scalar_sin_op,sine,\sa ArrayBase::sin)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cos,scalar_cos_op,cosine,\sa ArrayBase::cos)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tan,scalar_tan_op,tangent,\sa ArrayBase::tan)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atan,scalar_atan_op,arc-tangent,\sa ArrayBase::atan)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asin,scalar_asin_op,arc-sine,\sa ArrayBase::asin)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acos,scalar_acos_op,arc-consine,\sa ArrayBase::acos)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sinh,scalar_sinh_op,hyperbolic sine,\sa ArrayBase::sinh)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cosh,scalar_cosh_op,hyperbolic cosine,\sa ArrayBase::cosh)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tanh,scalar_tanh_op,hyperbolic tangent,\sa ArrayBase::tanh)
-#if EIGEN_HAS_CXX11_MATH
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asinh,scalar_asinh_op,inverse hyperbolic sine,\sa ArrayBase::asinh)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acosh,scalar_acosh_op,inverse hyperbolic cosine,\sa ArrayBase::acosh)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atanh,scalar_atanh_op,inverse hyperbolic tangent,\sa ArrayBase::atanh)
-#endif
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(logistic,scalar_logistic_op,logistic function,\sa ArrayBase::logistic)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(lgamma,scalar_lgamma_op,natural logarithm of the gamma function,\sa ArrayBase::lgamma)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(digamma,scalar_digamma_op,derivative of lgamma,\sa ArrayBase::digamma)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erf,scalar_erf_op,error function,\sa ArrayBase::erf)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erfc,scalar_erfc_op,complement error function,\sa ArrayBase::erfc)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(ndtri,scalar_ndtri_op,inverse normal distribution function,\sa ArrayBase::ndtri)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(exp,scalar_exp_op,exponential,\sa ArrayBase::exp)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(expm1,scalar_expm1_op,exponential of a value minus 1,\sa ArrayBase::expm1)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log,scalar_log_op,natural logarithm,\sa Eigen::log10 DOXCOMMA ArrayBase::log)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log1p,scalar_log1p_op,natural logarithm of 1 plus the value,\sa ArrayBase::log1p)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log10,scalar_log10_op,base 10 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log10)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log2,scalar_log2_op,base 2 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log2)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs,scalar_abs_op,absolute value,\sa ArrayBase::abs DOXCOMMA MatrixBase::cwiseAbs)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs2,scalar_abs2_op,squared absolute value,\sa ArrayBase::abs2 DOXCOMMA MatrixBase::cwiseAbs2)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(arg,scalar_arg_op,complex argument,\sa ArrayBase::arg DOXCOMMA MatrixBase::cwiseArg)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt,scalar_sqrt_op,square root,\sa ArrayBase::sqrt DOXCOMMA MatrixBase::cwiseSqrt)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rsqrt,scalar_rsqrt_op,reciprocal square root,\sa ArrayBase::rsqrt)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(square,scalar_square_op,square (power 2),\sa Eigen::abs2 DOXCOMMA Eigen::pow DOXCOMMA ArrayBase::square)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cube,scalar_cube_op,cube (power 3),\sa Eigen::pow DOXCOMMA ArrayBase::cube)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rint,scalar_rint_op,nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(round,scalar_round_op,nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(floor,scalar_floor_op,nearest integer not greater than the giben value,\sa Eigen::ceil DOXCOMMA ArrayBase::floor)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(ceil,scalar_ceil_op,nearest integer not less than the giben value,\sa Eigen::floor DOXCOMMA ArrayBase::ceil)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isnan,scalar_isnan_op,not-a-number test,\sa Eigen::isinf DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isnan)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isinf,scalar_isinf_op,infinite value test,\sa Eigen::isnan DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isinf)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isfinite,scalar_isfinite_op,finite value test,\sa Eigen::isinf DOXCOMMA Eigen::isnan DOXCOMMA ArrayBase::isfinite)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sign,scalar_sign_op,sign (or 0),\sa ArrayBase::sign)
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-  /** \returns an expression of the coefficient-wise power of \a x to the given constant \a exponent.
-    *
-    * \tparam ScalarExponent is the scalar type of \a exponent. It must be compatible with the scalar type of the given expression (\c Derived::Scalar).
-    *
-    * \sa ArrayBase::pow()
-    *
-    * \relates ArrayBase
-    */
+namespace Eigen {
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(real, scalar_real_op, real part,\sa ArrayBase::real)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(imag, scalar_imag_op, imaginary part,\sa ArrayBase::imag)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(conj, scalar_conjugate_op, complex conjugate,\sa ArrayBase::conjugate)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(inverse, scalar_inverse_op, inverse,\sa ArrayBase::inverse)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sin, scalar_sin_op, sine,\sa ArrayBase::sin)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cos, scalar_cos_op, cosine,\sa ArrayBase::cos)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tan, scalar_tan_op, tangent,\sa ArrayBase::tan)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atan, scalar_atan_op, arc - tangent,\sa ArrayBase::atan)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asin, scalar_asin_op, arc - sine,\sa ArrayBase::asin)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acos, scalar_acos_op, arc - consine,\sa ArrayBase::acos)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sinh, scalar_sinh_op, hyperbolic sine,\sa ArrayBase::sinh)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cosh, scalar_cosh_op, hyperbolic cosine,\sa ArrayBase::cosh)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tanh, scalar_tanh_op, hyperbolic tangent,\sa ArrayBase::tanh)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asinh, scalar_asinh_op, inverse hyperbolic sine,\sa ArrayBase::asinh)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acosh, scalar_acosh_op, inverse hyperbolic cosine,\sa ArrayBase::acosh)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atanh, scalar_atanh_op, inverse hyperbolic tangent,\sa ArrayBase::atanh)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(logistic, scalar_logistic_op, logistic function,\sa ArrayBase::logistic)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(lgamma, scalar_lgamma_op,
+                                 natural logarithm of the gamma function,\sa ArrayBase::lgamma)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(digamma, scalar_digamma_op, derivative of lgamma,\sa ArrayBase::digamma)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erf, scalar_erf_op, error function,\sa ArrayBase::erf)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erfc, scalar_erfc_op, complement error function,\sa ArrayBase::erfc)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(ndtri, scalar_ndtri_op, inverse normal distribution function,\sa ArrayBase::ndtri)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(exp, scalar_exp_op, exponential,\sa ArrayBase::exp)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(expm1, scalar_expm1_op, exponential of a value minus 1,\sa ArrayBase::expm1)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log, scalar_log_op, natural logarithm,\sa Eigen::log10 DOXCOMMA ArrayBase::log)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log1p, scalar_log1p_op, natural logarithm of 1 plus the value,\sa ArrayBase::log1p)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log10, scalar_log10_op, base 10 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log10)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log2, scalar_log2_op, base 2 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log2)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs, scalar_abs_op, absolute value,\sa ArrayBase::abs DOXCOMMA MatrixBase::cwiseAbs)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs2, scalar_abs2_op,
+                                 squared absolute value,\sa ArrayBase::abs2 DOXCOMMA MatrixBase::cwiseAbs2)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(arg, scalar_arg_op, complex argument,\sa ArrayBase::arg DOXCOMMA MatrixBase::cwiseArg)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(carg, scalar_carg_op,
+                                 complex argument, \sa ArrayBase::carg DOXCOMMA MatrixBase::cwiseCArg)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt, scalar_sqrt_op, square root,\sa ArrayBase::sqrt DOXCOMMA MatrixBase::cwiseSqrt)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cbrt, scalar_cbrt_op, cube root,\sa ArrayBase::cbrt DOXCOMMA MatrixBase::cwiseCbrt)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rsqrt, scalar_rsqrt_op, reciprocal square root,\sa ArrayBase::rsqrt)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(square, scalar_square_op,
+                                 square(power 2),\sa Eigen::abs2 DOXCOMMA Eigen::pow DOXCOMMA ArrayBase::square)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cube, scalar_cube_op, cube(power 3),\sa Eigen::pow DOXCOMMA ArrayBase::cube)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rint, scalar_rint_op,
+                                 nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(round, scalar_round_op,
+                                 nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(
+    floor, scalar_floor_op, nearest integer not greater than the giben value,\sa Eigen::ceil DOXCOMMA ArrayBase::floor)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(
+    ceil, scalar_ceil_op, nearest integer not less than the giben value,\sa Eigen::floor DOXCOMMA ArrayBase::ceil)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(
+    isnan, scalar_isnan_op, not -a - number test,\sa Eigen::isinf DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isnan)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(
+    isinf, scalar_isinf_op, infinite value test,\sa Eigen::isnan DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isinf)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isfinite, scalar_isfinite_op,
+                                 finite value test,\sa Eigen::isinf DOXCOMMA Eigen::isnan DOXCOMMA ArrayBase::isfinite)
+EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sign, scalar_sign_op, sign(or 0),\sa ArrayBase::sign)
+
+template <typename Derived, typename ScalarExponent>
+using GlobalUnaryPowReturnType = std::enable_if_t<
+    !internal::is_arithmetic<typename NumTraits<Derived>::Real>::value &&
+        internal::is_arithmetic<typename NumTraits<ScalarExponent>::Real>::value,
+    CwiseUnaryOp<internal::scalar_unary_pow_op<typename Derived::Scalar, ScalarExponent>, const Derived> >;
+
+/** \returns an expression of the coefficient-wise power of \a x to the given constant \a exponent.
+ *
+ * \tparam ScalarExponent is the scalar type of \a exponent. It must be compatible with the scalar type of the given
+ * expression (\c Derived::Scalar).
+ *
+ * \sa ArrayBase::pow()
+ *
+ * \relates ArrayBase
+ */
 #ifdef EIGEN_PARSED_BY_DOXYGEN
-  template<typename Derived,typename ScalarExponent>
-  inline const CwiseBinaryOp<internal::scalar_pow_op<Derived::Scalar,ScalarExponent>,Derived,Constant<ScalarExponent> >
-  pow(const Eigen::ArrayBase<Derived>& x, const ScalarExponent& exponent);
+template <typename Derived, typename ScalarExponent>
+EIGEN_DEVICE_FUNC inline const GlobalUnaryPowReturnType<Derived, ScalarExponent> pow(const Eigen::ArrayBase<Derived>& x,
+                                                                                     const ScalarExponent& exponent);
 #else
-  template <typename Derived,typename ScalarExponent>
-  EIGEN_DEVICE_FUNC inline
-  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(
-    const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename internal::promote_scalar_arg<typename Derived::Scalar
-                                                 EIGEN_COMMA ScalarExponent EIGEN_COMMA
-                                                 EIGEN_SCALAR_BINARY_SUPPORTED(pow,typename Derived::Scalar,ScalarExponent)>::type,pow))
-  pow(const Eigen::ArrayBase<Derived>& x, const ScalarExponent& exponent)
-  {
-    typedef typename internal::promote_scalar_arg<typename Derived::Scalar,ScalarExponent,
-                                                  EIGEN_SCALAR_BINARY_SUPPORTED(pow,typename Derived::Scalar,ScalarExponent)>::type PromotedExponent;
-    return EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,PromotedExponent,pow)(x.derived(),
-           typename internal::plain_constant_type<Derived,PromotedExponent>::type(x.derived().rows(), x.derived().cols(), internal::scalar_constant_op<PromotedExponent>(exponent)));
-  }
+template <typename Derived, typename ScalarExponent>
+EIGEN_DEVICE_FUNC inline const GlobalUnaryPowReturnType<Derived, ScalarExponent> pow(const Eigen::ArrayBase<Derived>& x,
+                                                                                     const ScalarExponent& exponent) {
+  return GlobalUnaryPowReturnType<Derived, ScalarExponent>(
+      x.derived(), internal::scalar_unary_pow_op<typename Derived::Scalar, ScalarExponent>(exponent));
+}
 #endif
 
-  /** \returns an expression of the coefficient-wise power of \a x to the given array of \a exponents.
-    *
-    * This function computes the coefficient-wise power.
-    *
-    * Example: \include Cwise_array_power_array.cpp
-    * Output: \verbinclude Cwise_array_power_array.out
-    *
-    * \sa ArrayBase::pow()
-    *
-    * \relates ArrayBase
-    */
-  template<typename Derived,typename ExponentDerived>
-  inline const Eigen::CwiseBinaryOp<Eigen::internal::scalar_pow_op<typename Derived::Scalar, typename ExponentDerived::Scalar>, const Derived, const ExponentDerived>
-  pow(const Eigen::ArrayBase<Derived>& x, const Eigen::ArrayBase<ExponentDerived>& exponents)
-  {
-    return Eigen::CwiseBinaryOp<Eigen::internal::scalar_pow_op<typename Derived::Scalar, typename ExponentDerived::Scalar>, const Derived, const ExponentDerived>(
-      x.derived(),
-      exponents.derived()
-    );
-  }
-
-  /** \returns an expression of the coefficient-wise power of the scalar \a x to the given array of \a exponents.
-    *
-    * This function computes the coefficient-wise power between a scalar and an array of exponents.
-    *
-    * \tparam Scalar is the scalar type of \a x. It must be compatible with the scalar type of the given array expression (\c Derived::Scalar).
-    *
-    * Example: \include Cwise_scalar_power_array.cpp
-    * Output: \verbinclude Cwise_scalar_power_array.out
-    *
-    * \sa ArrayBase::pow()
-    *
-    * \relates ArrayBase
-    */
-#ifdef EIGEN_PARSED_BY_DOXYGEN
-  template<typename Scalar,typename Derived>
-  inline const CwiseBinaryOp<internal::scalar_pow_op<Scalar,Derived::Scalar>,Constant<Scalar>,Derived>
-  pow(const Scalar& x,const Eigen::ArrayBase<Derived>& x);
-#else
-  template <typename Scalar, typename Derived>
-  EIGEN_DEVICE_FUNC inline
-  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(
-    const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename internal::promote_scalar_arg<typename Derived::Scalar
-                                                 EIGEN_COMMA Scalar EIGEN_COMMA
-                                                 EIGEN_SCALAR_BINARY_SUPPORTED(pow,Scalar,typename Derived::Scalar)>::type,Derived,pow))
-  pow(const Scalar& x, const Eigen::ArrayBase<Derived>& exponents) {
-    typedef typename internal::promote_scalar_arg<typename Derived::Scalar,Scalar,
-                                                  EIGEN_SCALAR_BINARY_SUPPORTED(pow,Scalar,typename Derived::Scalar)>::type PromotedScalar;
-    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedScalar,Derived,pow)(
-           typename internal::plain_constant_type<Derived,PromotedScalar>::type(exponents.derived().rows(), exponents.derived().cols(), internal::scalar_constant_op<PromotedScalar>(x)), exponents.derived());
-  }
-#endif
-
-
-  namespace internal
-  {
-    EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(real,scalar_real_op)
-    EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(imag,scalar_imag_op)
-    EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(abs2,scalar_abs2_op)
-  }
+/** \returns an expression of the coefficient-wise power of \a x to the given array of \a exponents.
+ *
+ * This function computes the coefficient-wise power.
+ *
+ * Example: \include Cwise_array_power_array.cpp
+ * Output: \verbinclude Cwise_array_power_array.out
+ *
+ * \sa ArrayBase::pow()
+ *
+ * \relates ArrayBase
+ */
+template <typename Derived, typename ExponentDerived>
+inline const Eigen::CwiseBinaryOp<
+    Eigen::internal::scalar_pow_op<typename Derived::Scalar, typename ExponentDerived::Scalar>, const Derived,
+    const ExponentDerived>
+pow(const Eigen::ArrayBase<Derived>& x, const Eigen::ArrayBase<ExponentDerived>& exponents) {
+  return Eigen::CwiseBinaryOp<
+      Eigen::internal::scalar_pow_op<typename Derived::Scalar, typename ExponentDerived::Scalar>, const Derived,
+      const ExponentDerived>(x.derived(), exponents.derived());
 }
 
-// TODO: cleanly disable those functions that are not supported on Array (numext::real_ref, internal::random, internal::isApprox...)
+/** \returns an expression of the coefficient-wise power of the scalar \a x to the given array of \a exponents.
+ *
+ * This function computes the coefficient-wise power between a scalar and an array of exponents.
+ *
+ * \tparam Scalar is the scalar type of \a x. It must be compatible with the scalar type of the given array expression
+ * (\c Derived::Scalar).
+ *
+ * Example: \include Cwise_scalar_power_array.cpp
+ * Output: \verbinclude Cwise_scalar_power_array.out
+ *
+ * \sa ArrayBase::pow()
+ *
+ * \relates ArrayBase
+ */
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+template <typename Scalar, typename Derived>
+inline const CwiseBinaryOp<internal::scalar_pow_op<Scalar, Derived::Scalar>, Constant<Scalar>, Derived> pow(
+    const Scalar& x, const Eigen::ArrayBase<Derived>& x);
+#else
+template <typename Scalar, typename Derived>
+EIGEN_DEVICE_FUNC inline const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(
+    typename internal::promote_scalar_arg<typename Derived::Scalar EIGEN_COMMA Scalar EIGEN_COMMA
+                                              EIGEN_SCALAR_BINARY_SUPPORTED(pow, Scalar,
+                                                                            typename Derived::Scalar)>::type,
+    Derived, pow) pow(const Scalar& x, const Eigen::ArrayBase<Derived>& exponents) {
+  typedef
+      typename internal::promote_scalar_arg<typename Derived::Scalar, Scalar,
+                                            EIGEN_SCALAR_BINARY_SUPPORTED(pow, Scalar, typename Derived::Scalar)>::type
+          PromotedScalar;
+  return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedScalar, Derived, pow)(
+      typename internal::plain_constant_type<Derived, PromotedScalar>::type(
+          exponents.derived().rows(), exponents.derived().cols(), internal::scalar_constant_op<PromotedScalar>(x)),
+      exponents.derived());
+}
+#endif
 
-#endif // EIGEN_GLOBAL_FUNCTIONS_H
+/** \returns an expression of the coefficient-wise atan2(\a x, \a y). \a x and \a y must be of the same type.
+ *
+ * This function computes the coefficient-wise atan2().
+ *
+ * \sa ArrayBase::atan2()
+ *
+ * \relates ArrayBase
+ */
+template <typename LhsDerived, typename RhsDerived>
+inline const std::enable_if_t<
+    std::is_same<typename LhsDerived::Scalar, typename RhsDerived::Scalar>::value,
+    Eigen::CwiseBinaryOp<Eigen::internal::scalar_atan2_op<typename LhsDerived::Scalar, typename RhsDerived::Scalar>,
+                         const LhsDerived, const RhsDerived> >
+atan2(const Eigen::ArrayBase<LhsDerived>& x, const Eigen::ArrayBase<RhsDerived>& exponents) {
+  return Eigen::CwiseBinaryOp<
+      Eigen::internal::scalar_atan2_op<typename LhsDerived::Scalar, typename RhsDerived::Scalar>, const LhsDerived,
+      const RhsDerived>(x.derived(), exponents.derived());
+}
+
+namespace internal {
+EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(real, scalar_real_op)
+EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(imag, scalar_imag_op)
+EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(abs2, scalar_abs2_op)
+}  // namespace internal
+}  // namespace Eigen
+
+// TODO: cleanly disable those functions that are not supported on Array (numext::real_ref, internal::random,
+// internal::isApprox...)
+
+#endif  // EIGEN_GLOBAL_FUNCTIONS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h
index e81c315..ca5f247 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h
@@ -11,60 +11,65 @@
 #ifndef EIGEN_IO_H
 #define EIGEN_IO_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 enum { DontAlignCols = 1 };
-enum { StreamPrecision = -1,
-       FullPrecision = -2 };
+enum { StreamPrecision = -1, FullPrecision = -2 };
 
 namespace internal {
-template<typename Derived>
-std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& fmt);
+template <typename Derived>
+std::ostream& print_matrix(std::ostream& s, const Derived& _m, const IOFormat& fmt);
 }
 
 /** \class IOFormat
-  * \ingroup Core_Module
-  *
-  * \brief Stores a set of parameters controlling the way matrices are printed
-  *
-  * List of available parameters:
-  *  - \b precision number of digits for floating point values, or one of the special constants \c StreamPrecision and \c FullPrecision.
-  *                 The default is the special value \c StreamPrecision which means to use the
-  *                 stream's own precision setting, as set for instance using \c cout.precision(3). The other special value
-  *                 \c FullPrecision means that the number of digits will be computed to match the full precision of each floating-point
-  *                 type.
-  *  - \b flags an OR-ed combination of flags, the default value is 0, the only currently available flag is \c DontAlignCols which
-  *             allows to disable the alignment of columns, resulting in faster code.
-  *  - \b coeffSeparator string printed between two coefficients of the same row
-  *  - \b rowSeparator string printed between two rows
-  *  - \b rowPrefix string printed at the beginning of each row
-  *  - \b rowSuffix string printed at the end of each row
-  *  - \b matPrefix string printed at the beginning of the matrix
-  *  - \b matSuffix string printed at the end of the matrix
-  *  - \b fill character printed to fill the empty space in aligned columns
-  *
-  * Example: \include IOFormat.cpp
-  * Output: \verbinclude IOFormat.out
-  *
-  * \sa DenseBase::format(), class WithFormat
-  */
-struct IOFormat
-{
+ * \ingroup Core_Module
+ *
+ * \brief Stores a set of parameters controlling the way matrices are printed
+ *
+ * List of available parameters:
+ *  - \b precision number of digits for floating point values, or one of the special constants \c StreamPrecision and \c
+ * FullPrecision. The default is the special value \c StreamPrecision which means to use the stream's own precision
+ * setting, as set for instance using \c cout.precision(3). The other special value \c FullPrecision means that the
+ * number of digits will be computed to match the full precision of each floating-point type.
+ *  - \b flags an OR-ed combination of flags, the default value is 0, the only currently available flag is \c
+ * DontAlignCols which allows to disable the alignment of columns, resulting in faster code.
+ *  - \b coeffSeparator string printed between two coefficients of the same row
+ *  - \b rowSeparator string printed between two rows
+ *  - \b rowPrefix string printed at the beginning of each row
+ *  - \b rowSuffix string printed at the end of each row
+ *  - \b matPrefix string printed at the beginning of the matrix
+ *  - \b matSuffix string printed at the end of the matrix
+ *  - \b fill character printed to fill the empty space in aligned columns
+ *
+ * Example: \include IOFormat.cpp
+ * Output: \verbinclude IOFormat.out
+ *
+ * \sa DenseBase::format(), class WithFormat
+ */
+struct IOFormat {
   /** Default constructor, see class IOFormat for the meaning of the parameters */
-  IOFormat(int _precision = StreamPrecision, int _flags = 0,
-    const std::string& _coeffSeparator = " ",
-    const std::string& _rowSeparator = "\n", const std::string& _rowPrefix="", const std::string& _rowSuffix="",
-    const std::string& _matPrefix="", const std::string& _matSuffix="", const char _fill=' ')
-  : matPrefix(_matPrefix), matSuffix(_matSuffix), rowPrefix(_rowPrefix), rowSuffix(_rowSuffix), rowSeparator(_rowSeparator),
-    rowSpacer(""), coeffSeparator(_coeffSeparator), fill(_fill), precision(_precision), flags(_flags)
-  {
+  IOFormat(int _precision = StreamPrecision, int _flags = 0, const std::string& _coeffSeparator = " ",
+           const std::string& _rowSeparator = "\n", const std::string& _rowPrefix = "",
+           const std::string& _rowSuffix = "", const std::string& _matPrefix = "", const std::string& _matSuffix = "",
+           const char _fill = ' ')
+      : matPrefix(_matPrefix),
+        matSuffix(_matSuffix),
+        rowPrefix(_rowPrefix),
+        rowSuffix(_rowSuffix),
+        rowSeparator(_rowSeparator),
+        rowSpacer(""),
+        coeffSeparator(_coeffSeparator),
+        fill(_fill),
+        precision(_precision),
+        flags(_flags) {
     // TODO check if rowPrefix, rowSuffix or rowSeparator contains a newline
     // don't add rowSpacer if columns are not to be aligned
-    if((flags & DontAlignCols))
-      return;
-    int i = int(matSuffix.length())-1;
-    while (i>=0 && matSuffix[i]!='\n')
-    {
+    if ((flags & DontAlignCols)) return;
+    int i = int(matSuffix.length()) - 1;
+    while (i >= 0 && matSuffix[i] != '\n') {
       rowSpacer += ' ';
       i--;
     }
@@ -78,181 +83,151 @@
 };
 
 /** \class WithFormat
-  * \ingroup Core_Module
-  *
-  * \brief Pseudo expression providing matrix output with given format
-  *
-  * \tparam ExpressionType the type of the object on which IO stream operations are performed
-  *
-  * This class represents an expression with stream operators controlled by a given IOFormat.
-  * It is the return type of DenseBase::format()
-  * and most of the time this is the only way it is used.
-  *
-  * See class IOFormat for some examples.
-  *
-  * \sa DenseBase::format(), class IOFormat
-  */
-template<typename ExpressionType>
-class WithFormat
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Pseudo expression providing matrix output with given format
+ *
+ * \tparam ExpressionType the type of the object on which IO stream operations are performed
+ *
+ * This class represents an expression with stream operators controlled by a given IOFormat.
+ * It is the return type of DenseBase::format()
+ * and most of the time this is the only way it is used.
+ *
+ * See class IOFormat for some examples.
+ *
+ * \sa DenseBase::format(), class IOFormat
+ */
+template <typename ExpressionType>
+class WithFormat {
+ public:
+  WithFormat(const ExpressionType& matrix, const IOFormat& format) : m_matrix(matrix), m_format(format) {}
 
-    WithFormat(const ExpressionType& matrix, const IOFormat& format)
-      : m_matrix(matrix), m_format(format)
-    {}
+  friend std::ostream& operator<<(std::ostream& s, const WithFormat& wf) {
+    return internal::print_matrix(s, wf.m_matrix.eval(), wf.m_format);
+  }
 
-    friend std::ostream & operator << (std::ostream & s, const WithFormat& wf)
-    {
-      return internal::print_matrix(s, wf.m_matrix.eval(), wf.m_format);
-    }
-
-  protected:
-    typename ExpressionType::Nested m_matrix;
-    IOFormat m_format;
+ protected:
+  typename ExpressionType::Nested m_matrix;
+  IOFormat m_format;
 };
 
 namespace internal {
 
 // NOTE: This helper is kept for backward compatibility with previous code specializing
 //       this internal::significant_decimals_impl structure. In the future we should directly
-//       call digits10() which has been introduced in July 2016 in 3.3.
-template<typename Scalar>
-struct significant_decimals_impl
-{
-  static inline int run()
-  {
-    return NumTraits<Scalar>::digits10();
-  }
+//       call max_digits10().
+template <typename Scalar>
+struct significant_decimals_impl {
+  static inline int run() { return NumTraits<Scalar>::max_digits10(); }
 };
 
 /** \internal
-  * print the matrix \a _m to the output stream \a s using the output format \a fmt */
-template<typename Derived>
-std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& fmt)
-{
+ * print the matrix \a _m to the output stream \a s using the output format \a fmt */
+template <typename Derived>
+std::ostream& print_matrix(std::ostream& s, const Derived& _m, const IOFormat& fmt) {
   using internal::is_same;
-  using internal::conditional;
 
-  if(_m.size() == 0)
-  {
+  if (_m.size() == 0) {
     s << fmt.matPrefix << fmt.matSuffix;
     return s;
   }
-  
+
   typename Derived::Nested m = _m;
   typedef typename Derived::Scalar Scalar;
-  typedef typename
-      conditional<
-          is_same<Scalar, char>::value ||
-            is_same<Scalar, unsigned char>::value ||
-            is_same<Scalar, numext::int8_t>::value ||
-            is_same<Scalar, numext::uint8_t>::value,
-          int,
-          typename conditional<
-              is_same<Scalar, std::complex<char> >::value ||
-                is_same<Scalar, std::complex<unsigned char> >::value ||
-                is_same<Scalar, std::complex<numext::int8_t> >::value ||
-                is_same<Scalar, std::complex<numext::uint8_t> >::value,
-              std::complex<int>,
-              const Scalar&
-            >::type
-        >::type PrintType;
+  typedef std::conditional_t<is_same<Scalar, char>::value || is_same<Scalar, unsigned char>::value ||
+                                 is_same<Scalar, numext::int8_t>::value || is_same<Scalar, numext::uint8_t>::value,
+                             int,
+                             std::conditional_t<is_same<Scalar, std::complex<char> >::value ||
+                                                    is_same<Scalar, std::complex<unsigned char> >::value ||
+                                                    is_same<Scalar, std::complex<numext::int8_t> >::value ||
+                                                    is_same<Scalar, std::complex<numext::uint8_t> >::value,
+                                                std::complex<int>, const Scalar&> >
+      PrintType;
 
   Index width = 0;
 
   std::streamsize explicit_precision;
-  if(fmt.precision == StreamPrecision)
-  {
+  if (fmt.precision == StreamPrecision) {
     explicit_precision = 0;
-  }
-  else if(fmt.precision == FullPrecision)
-  {
-    if (NumTraits<Scalar>::IsInteger)
-    {
+  } else if (fmt.precision == FullPrecision) {
+    if (NumTraits<Scalar>::IsInteger) {
       explicit_precision = 0;
-    }
-    else
-    {
+    } else {
       explicit_precision = significant_decimals_impl<Scalar>::run();
     }
-  }
-  else
-  {
+  } else {
     explicit_precision = fmt.precision;
   }
 
   std::streamsize old_precision = 0;
-  if(explicit_precision) old_precision = s.precision(explicit_precision);
+  if (explicit_precision) old_precision = s.precision(explicit_precision);
 
   bool align_cols = !(fmt.flags & DontAlignCols);
-  if(align_cols)
-  {
+  if (align_cols) {
     // compute the largest width
-    for(Index j = 0; j < m.cols(); ++j)
-      for(Index i = 0; i < m.rows(); ++i)
-      {
+    for (Index j = 0; j < m.cols(); ++j)
+      for (Index i = 0; i < m.rows(); ++i) {
         std::stringstream sstr;
         sstr.copyfmt(s);
-        sstr << static_cast<PrintType>(m.coeff(i,j));
+        sstr << static_cast<PrintType>(m.coeff(i, j));
         width = std::max<Index>(width, Index(sstr.str().length()));
       }
   }
   std::streamsize old_width = s.width();
   char old_fill_character = s.fill();
   s << fmt.matPrefix;
-  for(Index i = 0; i < m.rows(); ++i)
-  {
-    if (i)
-      s << fmt.rowSpacer;
+  for (Index i = 0; i < m.rows(); ++i) {
+    if (i) s << fmt.rowSpacer;
     s << fmt.rowPrefix;
-    if(width) {
+    if (width) {
       s.fill(fmt.fill);
       s.width(width);
     }
     s << static_cast<PrintType>(m.coeff(i, 0));
-    for(Index j = 1; j < m.cols(); ++j)
-    {
+    for (Index j = 1; j < m.cols(); ++j) {
       s << fmt.coeffSeparator;
-      if(width) {
+      if (width) {
         s.fill(fmt.fill);
         s.width(width);
       }
       s << static_cast<PrintType>(m.coeff(i, j));
     }
     s << fmt.rowSuffix;
-    if( i < m.rows() - 1)
-      s << fmt.rowSeparator;
+    if (i < m.rows() - 1) s << fmt.rowSeparator;
   }
   s << fmt.matSuffix;
-  if(explicit_precision) s.precision(old_precision);
-  if(width) {
+  if (explicit_precision) s.precision(old_precision);
+  if (width) {
     s.fill(old_fill_character);
     s.width(old_width);
   }
   return s;
 }
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \relates DenseBase
-  *
-  * Outputs the matrix, to the given stream.
-  *
-  * If you wish to print the matrix with a format different than the default, use DenseBase::format().
-  *
-  * It is also possible to change the default format by defining EIGEN_DEFAULT_IO_FORMAT before including Eigen headers.
-  * If not defined, this will automatically be defined to Eigen::IOFormat(), that is the Eigen::IOFormat with default parameters.
-  *
-  * \sa DenseBase::format()
-  */
-template<typename Derived>
-std::ostream & operator <<
-(std::ostream & s,
- const DenseBase<Derived> & m)
-{
+ *
+ * Outputs the matrix, to the given stream.
+ *
+ * If you wish to print the matrix with a format different than the default, use DenseBase::format().
+ *
+ * It is also possible to change the default format by defining EIGEN_DEFAULT_IO_FORMAT before including Eigen headers.
+ * If not defined, this will automatically be defined to Eigen::IOFormat(), that is the Eigen::IOFormat with default
+ * parameters.
+ *
+ * \sa DenseBase::format()
+ */
+template <typename Derived>
+std::ostream& operator<<(std::ostream& s, const DenseBase<Derived>& m) {
   return internal::print_matrix(s, m.eval(), EIGEN_DEFAULT_IO_FORMAT);
 }
 
-} // end namespace Eigen
+template <typename Derived>
+std::ostream& operator<<(std::ostream& s, const DiagonalBase<Derived>& m) {
+  return internal::print_matrix(s, m.derived(), EIGEN_DEFAULT_IO_FORMAT);
+}
 
-#endif // EIGEN_IO_H
+}  // end namespace Eigen
+
+#endif  // EIGEN_IO_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
index 0847625..0a02417 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
@@ -10,24 +10,25 @@
 #ifndef EIGEN_INDEXED_VIEW_H
 #define EIGEN_INDEXED_VIEW_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename XprType, typename RowIndices, typename ColIndices>
-struct traits<IndexedView<XprType, RowIndices, ColIndices> >
- : traits<XprType>
-{
+template <typename XprType, typename RowIndices, typename ColIndices>
+struct traits<IndexedView<XprType, RowIndices, ColIndices>> : traits<XprType> {
   enum {
     RowsAtCompileTime = int(array_size<RowIndices>::value),
     ColsAtCompileTime = int(array_size<ColIndices>::value),
-    MaxRowsAtCompileTime = RowsAtCompileTime != Dynamic ? int(RowsAtCompileTime) : Dynamic,
-    MaxColsAtCompileTime = ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime) : Dynamic,
+    MaxRowsAtCompileTime = RowsAtCompileTime,
+    MaxColsAtCompileTime = ColsAtCompileTime,
 
-    XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0,
-    IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
-               : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
-               : XprTypeIsRowMajor,
+    XprTypeIsRowMajor = (int(traits<XprType>::Flags) & RowMajorBit) != 0,
+    IsRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1)   ? 1
+                 : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0
+                                                                            : XprTypeIsRowMajor,
 
     RowIncr = int(get_compile_time_incr<RowIndices>::value),
     ColIncr = int(get_compile_time_incr<ColIndices>::value),
@@ -35,105 +36,116 @@
     OuterIncr = IsRowMajor ? RowIncr : ColIncr,
 
     HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor),
-    XprInnerStride = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time<XprType>::ret) : int(outer_stride_at_compile_time<XprType>::ret),
-    XprOuterstride = HasSameStorageOrderAsXprType ? int(outer_stride_at_compile_time<XprType>::ret) : int(inner_stride_at_compile_time<XprType>::ret),
+    XprInnerStride = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time<XprType>::ret)
+                                                  : int(outer_stride_at_compile_time<XprType>::ret),
+    XprOuterstride = HasSameStorageOrderAsXprType ? int(outer_stride_at_compile_time<XprType>::ret)
+                                                  : int(inner_stride_at_compile_time<XprType>::ret),
 
     InnerSize = XprTypeIsRowMajor ? ColsAtCompileTime : RowsAtCompileTime,
-    IsBlockAlike = InnerIncr==1 && OuterIncr==1,
-    IsInnerPannel = HasSameStorageOrderAsXprType && is_same<AllRange<InnerSize>,typename conditional<XprTypeIsRowMajor,ColIndices,RowIndices>::type>::value,
+    IsBlockAlike = InnerIncr == 1 && OuterIncr == 1,
+    IsInnerPannel = HasSameStorageOrderAsXprType &&
+                    is_same<AllRange<InnerSize>, std::conditional_t<XprTypeIsRowMajor, ColIndices, RowIndices>>::value,
 
-    InnerStrideAtCompileTime = InnerIncr<0 || InnerIncr==DynamicIndex || XprInnerStride==Dynamic ? Dynamic : XprInnerStride * InnerIncr,
-    OuterStrideAtCompileTime = OuterIncr<0 || OuterIncr==DynamicIndex || XprOuterstride==Dynamic ? Dynamic : XprOuterstride * OuterIncr,
+    InnerStrideAtCompileTime =
+        InnerIncr < 0 || InnerIncr == DynamicIndex || XprInnerStride == Dynamic || InnerIncr == UndefinedIncr
+            ? Dynamic
+            : XprInnerStride * InnerIncr,
+    OuterStrideAtCompileTime =
+        OuterIncr < 0 || OuterIncr == DynamicIndex || XprOuterstride == Dynamic || OuterIncr == UndefinedIncr
+            ? Dynamic
+            : XprOuterstride * OuterIncr,
 
-    ReturnAsScalar = is_same<RowIndices,SingleRange>::value && is_same<ColIndices,SingleRange>::value,
+    ReturnAsScalar = is_same<RowIndices, SingleRange>::value && is_same<ColIndices, SingleRange>::value,
     ReturnAsBlock = (!ReturnAsScalar) && IsBlockAlike,
     ReturnAsIndexedView = (!ReturnAsScalar) && (!ReturnAsBlock),
 
     // FIXME we deal with compile-time strides if and only if we have DirectAccessBit flag,
     // but this is too strict regarding negative strides...
-    DirectAccessMask = (int(InnerIncr)!=UndefinedIncr && int(OuterIncr)!=UndefinedIncr && InnerIncr>=0 && OuterIncr>=0) ? DirectAccessBit : 0,
+    DirectAccessMask =
+        (int(InnerIncr) != UndefinedIncr && int(OuterIncr) != UndefinedIncr && InnerIncr >= 0 && OuterIncr >= 0)
+            ? DirectAccessBit
+            : 0,
     FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
     FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
     FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
-    Flags = (traits<XprType>::Flags & (HereditaryBits | DirectAccessMask )) | FlagsLvalueBit | FlagsRowMajorBit | FlagsLinearAccessBit
+    Flags = (traits<XprType>::Flags & (HereditaryBits | DirectAccessMask)) | FlagsLvalueBit | FlagsRowMajorBit |
+            FlagsLinearAccessBit
   };
 
-  typedef Block<XprType,RowsAtCompileTime,ColsAtCompileTime,IsInnerPannel> BlockType;
+  typedef Block<XprType, RowsAtCompileTime, ColsAtCompileTime, IsInnerPannel> BlockType;
 };
 
-}
+}  // namespace internal
 
-template<typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
+template <typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
 class IndexedViewImpl;
 
-
 /** \class IndexedView
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a non-sequential sub-matrix defined by arbitrary sequences of row and column indices
-  *
-  * \tparam XprType the type of the expression in which we are taking the intersections of sub-rows and sub-columns
-  * \tparam RowIndices the type of the object defining the sequence of row indices
-  * \tparam ColIndices the type of the object defining the sequence of column indices
-  *
-  * This class represents an expression of a sub-matrix (or sub-vector) defined as the intersection
-  * of sub-sets of rows and columns, that are themself defined by generic sequences of row indices \f$ \{r_0,r_1,..r_{m-1}\} \f$
-  * and column indices \f$ \{c_0,c_1,..c_{n-1} \}\f$. Let \f$ A \f$  be the nested matrix, then the resulting matrix \f$ B \f$ has \c m
-  * rows and \c n columns, and its entries are given by: \f$ B(i,j) = A(r_i,c_j) \f$.
-  *
-  * The \c RowIndices and \c ColIndices types must be compatible with the following API:
-  * \code
-  * <integral type> operator[](Index) const;
-  * Index size() const;
-  * \endcode
-  *
-  * Typical supported types thus include:
-  *  - std::vector<int>
-  *  - std::valarray<int>
-  *  - std::array<int>
-  *  - Plain C arrays: int[N]
-  *  - Eigen::ArrayXi
-  *  - decltype(ArrayXi::LinSpaced(...))
-  *  - Any view/expressions of the previous types
-  *  - Eigen::ArithmeticSequence
-  *  - Eigen::internal::AllRange      (helper for Eigen::all)
-  *  - Eigen::internal::SingleRange  (helper for single index)
-  *  - etc.
-  *
-  * In typical usages of %Eigen, this class should never be used directly. It is the return type of
-  * DenseBase::operator()(const RowIndices&, const ColIndices&).
-  *
-  * \sa class Block
-  */
-template<typename XprType, typename RowIndices, typename ColIndices>
-class IndexedView : public IndexedViewImpl<XprType, RowIndices, ColIndices, typename internal::traits<XprType>::StorageKind>
-{
-public:
-  typedef typename IndexedViewImpl<XprType, RowIndices, ColIndices, typename internal::traits<XprType>::StorageKind>::Base Base;
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a non-sequential sub-matrix defined by arbitrary sequences of row and column indices
+ *
+ * \tparam XprType the type of the expression in which we are taking the intersections of sub-rows and sub-columns
+ * \tparam RowIndices the type of the object defining the sequence of row indices
+ * \tparam ColIndices the type of the object defining the sequence of column indices
+ *
+ * This class represents an expression of a sub-matrix (or sub-vector) defined as the intersection
+ * of sub-sets of rows and columns, that are themself defined by generic sequences of row indices \f$
+ * \{r_0,r_1,..r_{m-1}\} \f$ and column indices \f$ \{c_0,c_1,..c_{n-1} \}\f$. Let \f$ A \f$  be the nested matrix, then
+ * the resulting matrix \f$ B \f$ has \c m rows and \c n columns, and its entries are given by: \f$ B(i,j) = A(r_i,c_j)
+ * \f$.
+ *
+ * The \c RowIndices and \c ColIndices types must be compatible with the following API:
+ * \code
+ * <integral type> operator[](Index) const;
+ * Index size() const;
+ * \endcode
+ *
+ * Typical supported types thus include:
+ *  - std::vector<int>
+ *  - std::valarray<int>
+ *  - std::array<int>
+ *  - Eigen::ArrayXi
+ *  - decltype(ArrayXi::LinSpaced(...))
+ *  - Any view/expressions of the previous types
+ *  - Eigen::ArithmeticSequence
+ *  - Eigen::internal::AllRange     (helper for Eigen::placeholders::all)
+ *  - Eigen::internal::SingleRange  (helper for single index)
+ *  - etc.
+ *
+ * In typical usages of %Eigen, this class should never be used directly. It is the return type of
+ * DenseBase::operator()(const RowIndices&, const ColIndices&).
+ *
+ * \sa class Block
+ */
+template <typename XprType, typename RowIndices, typename ColIndices>
+class IndexedView
+    : public IndexedViewImpl<XprType, RowIndices, ColIndices, typename internal::traits<XprType>::StorageKind> {
+ public:
+  typedef
+      typename IndexedViewImpl<XprType, RowIndices, ColIndices, typename internal::traits<XprType>::StorageKind>::Base
+          Base;
   EIGEN_GENERIC_PUBLIC_INTERFACE(IndexedView)
   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(IndexedView)
 
   typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
-  typedef typename internal::remove_all<XprType>::type NestedExpression;
+  typedef internal::remove_all_t<XprType> NestedExpression;
 
-  template<typename T0, typename T1>
+  template <typename T0, typename T1>
   IndexedView(XprType& xpr, const T0& rowIndices, const T1& colIndices)
-    : m_xpr(xpr), m_rowIndices(rowIndices), m_colIndices(colIndices)
-  {}
+      : m_xpr(xpr), m_rowIndices(rowIndices), m_colIndices(colIndices) {}
 
   /** \returns number of rows */
-  Index rows() const { return internal::size(m_rowIndices); }
+  Index rows() const { return internal::index_list_size(m_rowIndices); }
 
   /** \returns number of columns */
-  Index cols() const { return internal::size(m_colIndices); }
+  Index cols() const { return internal::index_list_size(m_colIndices); }
 
   /** \returns the nested expression */
-  const typename internal::remove_all<XprType>::type&
-  nestedExpression() const { return m_xpr; }
+  const internal::remove_all_t<XprType>& nestedExpression() const { return m_xpr; }
 
   /** \returns the nested expression */
-  typename internal::remove_reference<XprType>::type&
-  nestedExpression() { return m_xpr; }
+  std::remove_reference_t<XprType>& nestedExpression() { return m_xpr; }
 
   /** \returns a const reference to the object storing/generating the row indices */
   const RowIndices& rowIndices() const { return m_rowIndices; }
@@ -141,97 +153,91 @@
   /** \returns a const reference to the object storing/generating the column indices */
   const ColIndices& colIndices() const { return m_colIndices; }
 
-protected:
+ protected:
   MatrixTypeNested m_xpr;
   RowIndices m_rowIndices;
   ColIndices m_colIndices;
 };
 
-
 // Generic API dispatcher
-template<typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
-class IndexedViewImpl
-  : public internal::generic_xpr_base<IndexedView<XprType, RowIndices, ColIndices> >::type
-{
-public:
-  typedef typename internal::generic_xpr_base<IndexedView<XprType, RowIndices, ColIndices> >::type Base;
+template <typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
+class IndexedViewImpl : public internal::generic_xpr_base<IndexedView<XprType, RowIndices, ColIndices>>::type {
+ public:
+  typedef typename internal::generic_xpr_base<IndexedView<XprType, RowIndices, ColIndices>>::type Base;
 };
 
 namespace internal {
 
-
-template<typename ArgType, typename RowIndices, typename ColIndices>
+template <typename ArgType, typename RowIndices, typename ColIndices>
 struct unary_evaluator<IndexedView<ArgType, RowIndices, ColIndices>, IndexBased>
-  : evaluator_base<IndexedView<ArgType, RowIndices, ColIndices> >
-{
+    : evaluator_base<IndexedView<ArgType, RowIndices, ColIndices>> {
   typedef IndexedView<ArgType, RowIndices, ColIndices> XprType;
 
   enum {
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost /* TODO + cost of row/col index */,
 
-    FlagsLinearAccessBit = (traits<XprType>::RowsAtCompileTime == 1 || traits<XprType>::ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+    FlagsLinearAccessBit =
+        (traits<XprType>::RowsAtCompileTime == 1 || traits<XprType>::ColsAtCompileTime == 1) ? LinearAccessBit : 0,
 
-    FlagsRowMajorBit = traits<XprType>::FlagsRowMajorBit, 
+    FlagsRowMajorBit = traits<XprType>::FlagsRowMajorBit,
 
-    Flags = (evaluator<ArgType>::Flags & (HereditaryBits & ~RowMajorBit /*| LinearAccessBit | DirectAccessBit*/)) | FlagsLinearAccessBit | FlagsRowMajorBit,
+    Flags = (evaluator<ArgType>::Flags & (HereditaryBits & ~RowMajorBit /*| LinearAccessBit | DirectAccessBit*/)) |
+            FlagsLinearAccessBit | FlagsRowMajorBit,
 
     Alignment = 0
   };
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
-  {
+  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
+    eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() &&
+                 m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols());
     return m_argImpl.coeff(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index row, Index col)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
+    eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() &&
+                 m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols());
     return m_argImpl.coeffRef(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Scalar& coeffRef(Index index)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) {
     EIGEN_STATIC_ASSERT_LVALUE(XprType)
     Index row = XprType::RowsAtCompileTime == 1 ? 0 : index;
     Index col = XprType::RowsAtCompileTime == 1 ? index : 0;
-    return m_argImpl.coeffRef( m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+    eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() &&
+                 m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols());
+    return m_argImpl.coeffRef(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const Scalar& coeffRef(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeffRef(Index index) const {
     Index row = XprType::RowsAtCompileTime == 1 ? 0 : index;
     Index col = XprType::RowsAtCompileTime == 1 ? index : 0;
-    return m_argImpl.coeffRef( m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+    eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() &&
+                 m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols());
+    return m_argImpl.coeffRef(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const CoeffReturnType coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index index) const {
     Index row = XprType::RowsAtCompileTime == 1 ? 0 : index;
     Index col = XprType::RowsAtCompileTime == 1 ? index : 0;
-    return m_argImpl.coeff( m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+    eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() &&
+                 m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols());
+    return m_argImpl.coeff(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
   }
 
-protected:
-
+ protected:
   evaluator<ArgType> m_argImpl;
   const XprType& m_xpr;
-
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_INDEXED_VIEW_H
+#endif  // EIGEN_INDEXED_VIEW_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/InternalHeaderCheck.h
new file mode 100644
index 0000000..1cea572
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_CORE_MODULE_H
+#error "Please include Eigen/Core instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h
index c514438..cfb3b20 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h
@@ -10,69 +10,64 @@
 #ifndef EIGEN_INVERSE_H
 #define EIGEN_INVERSE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename XprType,typename StorageKind> class InverseImpl;
+template <typename XprType, typename StorageKind>
+class InverseImpl;
 
 namespace internal {
 
-template<typename XprType>
-struct traits<Inverse<XprType> >
-  : traits<typename XprType::PlainObject>
-{
+template <typename XprType>
+struct traits<Inverse<XprType> > : traits<typename XprType::PlainObject> {
   typedef typename XprType::PlainObject PlainObject;
   typedef traits<PlainObject> BaseTraits;
-  enum {
-    Flags = BaseTraits::Flags & RowMajorBit
-  };
+  enum { Flags = BaseTraits::Flags & RowMajorBit };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \class Inverse
-  *
-  * \brief Expression of the inverse of another expression
-  *
-  * \tparam XprType the type of the expression we are taking the inverse
-  *
-  * This class represents an abstract expression of A.inverse()
-  * and most of the time this is the only way it is used.
-  *
-  */
-template<typename XprType>
-class Inverse : public InverseImpl<XprType,typename internal::traits<XprType>::StorageKind>
-{
-public:
+ *
+ * \brief Expression of the inverse of another expression
+ *
+ * \tparam XprType the type of the expression we are taking the inverse
+ *
+ * This class represents an abstract expression of A.inverse()
+ * and most of the time this is the only way it is used.
+ *
+ */
+template <typename XprType>
+class Inverse : public InverseImpl<XprType, typename internal::traits<XprType>::StorageKind> {
+ public:
   typedef typename XprType::StorageIndex StorageIndex;
-  typedef typename XprType::Scalar                            Scalar;
-  typedef typename internal::ref_selector<XprType>::type      XprTypeNested;
-  typedef typename internal::remove_all<XprTypeNested>::type  XprTypeNestedCleaned;
+  typedef typename XprType::Scalar Scalar;
+  typedef typename internal::ref_selector<XprType>::type XprTypeNested;
+  typedef internal::remove_all_t<XprTypeNested> XprTypeNestedCleaned;
   typedef typename internal::ref_selector<Inverse>::type Nested;
-  typedef typename internal::remove_all<XprType>::type NestedExpression;
+  typedef internal::remove_all_t<XprType> NestedExpression;
 
-  explicit EIGEN_DEVICE_FUNC Inverse(const XprType &xpr)
-    : m_xpr(xpr)
-  {}
+  explicit EIGEN_DEVICE_FUNC Inverse(const XprType& xpr) : m_xpr(xpr) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR  Index rows() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR  Index cols() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
 
   EIGEN_DEVICE_FUNC const XprTypeNestedCleaned& nestedExpression() const { return m_xpr; }
 
-protected:
+ protected:
   XprTypeNested m_xpr;
 };
 
 // Generic API dispatcher
-template<typename XprType, typename StorageKind>
-class InverseImpl
-  : public internal::generic_xpr_base<Inverse<XprType> >::type
-{
-public:
+template <typename XprType, typename StorageKind>
+class InverseImpl : public internal::generic_xpr_base<Inverse<XprType> >::type {
+ public:
   typedef typename internal::generic_xpr_base<Inverse<XprType> >::type Base;
   typedef typename XprType::Scalar Scalar;
-private:
 
+ private:
   Scalar coeff(Index row, Index col) const;
   Scalar coeff(Index i) const;
 };
@@ -80,38 +75,34 @@
 namespace internal {
 
 /** \internal
-  * \brief Default evaluator for Inverse expression.
-  *
-  * This default evaluator for Inverse expression simply evaluate the inverse into a temporary
-  * by a call to internal::call_assignment_no_alias.
-  * Therefore, inverse implementers only have to specialize Assignment<Dst,Inverse<...>, ...> for
-  * there own nested expression.
-  *
-  * \sa class Inverse
-  */
-template<typename ArgType>
-struct unary_evaluator<Inverse<ArgType> >
-  : public evaluator<typename Inverse<ArgType>::PlainObject>
-{
+ * \brief Default evaluator for Inverse expression.
+ *
+ * This default evaluator for Inverse expression simply evaluate the inverse into a temporary
+ * by a call to internal::call_assignment_no_alias.
+ * Therefore, inverse implementers only have to specialize Assignment<Dst,Inverse<...>, ...> for
+ * there own nested expression.
+ *
+ * \sa class Inverse
+ */
+template <typename ArgType>
+struct unary_evaluator<Inverse<ArgType> > : public evaluator<typename Inverse<ArgType>::PlainObject> {
   typedef Inverse<ArgType> InverseType;
   typedef typename InverseType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
 
   enum { Flags = Base::Flags | EvalBeforeNestingBit };
 
-  unary_evaluator(const InverseType& inv_xpr)
-    : m_result(inv_xpr.rows(), inv_xpr.cols())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
+  unary_evaluator(const InverseType& inv_xpr) : m_result(inv_xpr.rows(), inv_xpr.cols()) {
+    internal::construct_at<Base>(this, m_result);
     internal::call_assignment_no_alias(m_result, inv_xpr);
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_INVERSE_H
+#endif  // EIGEN_INVERSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h
index 218cc15..df7b7ca 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h
@@ -11,161 +11,143 @@
 #ifndef EIGEN_MAP_H
 #define EIGEN_MAP_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename PlainObjectType, int MapOptions, typename StrideType>
-struct traits<Map<PlainObjectType, MapOptions, StrideType> >
-  : public traits<PlainObjectType>
-{
+template <typename PlainObjectType, int MapOptions, typename StrideType>
+struct traits<Map<PlainObjectType, MapOptions, StrideType> > : public traits<PlainObjectType> {
   typedef traits<PlainObjectType> TraitsBase;
   enum {
-    PlainObjectTypeInnerSize = ((traits<PlainObjectType>::Flags&RowMajorBit)==RowMajorBit)
-                             ? PlainObjectType::ColsAtCompileTime
-                             : PlainObjectType::RowsAtCompileTime,
+    PlainObjectTypeInnerSize = ((traits<PlainObjectType>::Flags & RowMajorBit) == RowMajorBit)
+                                   ? PlainObjectType::ColsAtCompileTime
+                                   : PlainObjectType::RowsAtCompileTime,
 
     InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0
-                             ? int(PlainObjectType::InnerStrideAtCompileTime)
-                             : int(StrideType::InnerStrideAtCompileTime),
+                                   ? int(PlainObjectType::InnerStrideAtCompileTime)
+                                   : int(StrideType::InnerStrideAtCompileTime),
     OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0
-                             ? (InnerStrideAtCompileTime==Dynamic || PlainObjectTypeInnerSize==Dynamic
-                                ? Dynamic
-                                : int(InnerStrideAtCompileTime) * int(PlainObjectTypeInnerSize))
-                             : int(StrideType::OuterStrideAtCompileTime),
-    Alignment = int(MapOptions)&int(AlignedMask),
+                                   ? (InnerStrideAtCompileTime == Dynamic || PlainObjectTypeInnerSize == Dynamic
+                                          ? Dynamic
+                                          : int(InnerStrideAtCompileTime) * int(PlainObjectTypeInnerSize))
+                                   : int(StrideType::OuterStrideAtCompileTime),
+    Alignment = int(MapOptions) & int(AlignedMask),
     Flags0 = TraitsBase::Flags & (~NestByRefBit),
     Flags = is_lvalue<PlainObjectType>::value ? int(Flags0) : (int(Flags0) & ~LvalueBit)
   };
-private:
-  enum { Options }; // Expressions don't have Options
+
+ private:
+  enum { Options };  // Expressions don't have Options
 };
-}
+}  // namespace internal
 
 /** \class Map
-  * \ingroup Core_Module
-  *
-  * \brief A matrix or vector expression mapping an existing array of data.
-  *
-  * \tparam PlainObjectType the equivalent matrix type of the mapped data
-  * \tparam MapOptions specifies the pointer alignment in bytes. It can be: \c #Aligned128, \c #Aligned64, \c #Aligned32, \c #Aligned16, \c #Aligned8 or \c #Unaligned.
-  *                The default is \c #Unaligned.
-  * \tparam StrideType optionally specifies strides. By default, Map assumes the memory layout
-  *                   of an ordinary, contiguous array. This can be overridden by specifying strides.
-  *                   The type passed here must be a specialization of the Stride template, see examples below.
-  *
-  * This class represents a matrix or vector expression mapping an existing array of data.
-  * It can be used to let Eigen interface without any overhead with non-Eigen data structures,
-  * such as plain C arrays or structures from other libraries. By default, it assumes that the
-  * data is laid out contiguously in memory. You can however override this by explicitly specifying
-  * inner and outer strides.
-  *
-  * Here's an example of simply mapping a contiguous array as a \ref TopicStorageOrders "column-major" matrix:
-  * \include Map_simple.cpp
-  * Output: \verbinclude Map_simple.out
-  *
-  * If you need to map non-contiguous arrays, you can do so by specifying strides:
-  *
-  * Here's an example of mapping an array as a vector, specifying an inner stride, that is, the pointer
-  * increment between two consecutive coefficients. Here, we're specifying the inner stride as a compile-time
-  * fixed value.
-  * \include Map_inner_stride.cpp
-  * Output: \verbinclude Map_inner_stride.out
-  *
-  * Here's an example of mapping an array while specifying an outer stride. Here, since we're mapping
-  * as a column-major matrix, 'outer stride' means the pointer increment between two consecutive columns.
-  * Here, we're specifying the outer stride as a runtime parameter. Note that here \c OuterStride<> is
-  * a short version of \c OuterStride<Dynamic> because the default template parameter of OuterStride
-  * is  \c Dynamic
-  * \include Map_outer_stride.cpp
-  * Output: \verbinclude Map_outer_stride.out
-  *
-  * For more details and for an example of specifying both an inner and an outer stride, see class Stride.
-  *
-  * \b Tip: to change the array of data mapped by a Map object, you can use the C++
-  * placement new syntax:
-  *
-  * Example: \include Map_placement_new.cpp
-  * Output: \verbinclude Map_placement_new.out
-  *
-  * This class is the return type of PlainObjectBase::Map() but can also be used directly.
-  *
-  * \sa PlainObjectBase::Map(), \ref TopicStorageOrders
-  */
-template<typename PlainObjectType, int MapOptions, typename StrideType> class Map
-  : public MapBase<Map<PlainObjectType, MapOptions, StrideType> >
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief A matrix or vector expression mapping an existing array of data.
+ *
+ * \tparam PlainObjectType the equivalent matrix type of the mapped data
+ * \tparam MapOptions specifies the pointer alignment in bytes. It can be: \c #Aligned128, \c #Aligned64, \c #Aligned32,
+ * \c #Aligned16, \c #Aligned8 or \c #Unaligned. The default is \c #Unaligned. \tparam StrideType optionally specifies
+ * strides. By default, Map assumes the memory layout of an ordinary, contiguous array. This can be overridden by
+ * specifying strides. The type passed here must be a specialization of the Stride template, see examples below.
+ *
+ * This class represents a matrix or vector expression mapping an existing array of data.
+ * It can be used to let Eigen interface without any overhead with non-Eigen data structures,
+ * such as plain C arrays or structures from other libraries. By default, it assumes that the
+ * data is laid out contiguously in memory. You can however override this by explicitly specifying
+ * inner and outer strides.
+ *
+ * Here's an example of simply mapping a contiguous array as a \ref TopicStorageOrders "column-major" matrix:
+ * \include Map_simple.cpp
+ * Output: \verbinclude Map_simple.out
+ *
+ * If you need to map non-contiguous arrays, you can do so by specifying strides:
+ *
+ * Here's an example of mapping an array as a vector, specifying an inner stride, that is, the pointer
+ * increment between two consecutive coefficients. Here, we're specifying the inner stride as a compile-time
+ * fixed value.
+ * \include Map_inner_stride.cpp
+ * Output: \verbinclude Map_inner_stride.out
+ *
+ * Here's an example of mapping an array while specifying an outer stride. Here, since we're mapping
+ * as a column-major matrix, 'outer stride' means the pointer increment between two consecutive columns.
+ * Here, we're specifying the outer stride as a runtime parameter. Note that here \c OuterStride<> is
+ * a short version of \c OuterStride<Dynamic> because the default template parameter of OuterStride
+ * is  \c Dynamic
+ * \include Map_outer_stride.cpp
+ * Output: \verbinclude Map_outer_stride.out
+ *
+ * For more details and for an example of specifying both an inner and an outer stride, see class Stride.
+ *
+ * \b Tip: to change the array of data mapped by a Map object, you can use the C++
+ * placement new syntax:
+ *
+ * Example: \include Map_placement_new.cpp
+ * Output: \verbinclude Map_placement_new.out
+ *
+ * This class is the return type of PlainObjectBase::Map() but can also be used directly.
+ *
+ * \sa PlainObjectBase::Map(), \ref TopicStorageOrders
+ */
+template <typename PlainObjectType, int MapOptions, typename StrideType>
+class Map : public MapBase<Map<PlainObjectType, MapOptions, StrideType> > {
+ public:
+  typedef MapBase<Map> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Map)
 
-    typedef MapBase<Map> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Map)
+  typedef typename Base::PointerType PointerType;
+  typedef PointerType PointerArgType;
+  EIGEN_DEVICE_FUNC inline PointerType cast_to_pointer_type(PointerArgType ptr) { return ptr; }
 
-    typedef typename Base::PointerType PointerType;
-    typedef PointerType PointerArgType;
-    EIGEN_DEVICE_FUNC
-    inline PointerType cast_to_pointer_type(PointerArgType ptr) { return ptr; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const {
+    return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1;
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const
-    {
-      return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1;
-    }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const {
+    return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer()
+           : internal::traits<Map>::OuterStrideAtCompileTime != Dynamic
+               ? Index(internal::traits<Map>::OuterStrideAtCompileTime)
+           : IsVectorAtCompileTime    ? (this->size() * innerStride())
+           : int(Flags) & RowMajorBit ? (this->cols() * innerStride())
+                                      : (this->rows() * innerStride());
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const
-    {
-      return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer()
-           : internal::traits<Map>::OuterStrideAtCompileTime != Dynamic ? Index(internal::traits<Map>::OuterStrideAtCompileTime)
-           : IsVectorAtCompileTime ? (this->size() * innerStride())
-           : int(Flags)&RowMajorBit ? (this->cols() * innerStride())
-           : (this->rows() * innerStride());
-    }
+  /** Constructor in the fixed-size case.
+   *
+   * \param dataPtr pointer to the array to map
+   * \param stride optional Stride object, passing the strides.
+   */
+  EIGEN_DEVICE_FUNC explicit inline Map(PointerArgType dataPtr, const StrideType& stride = StrideType())
+      : Base(cast_to_pointer_type(dataPtr)), m_stride(stride) {}
 
-    /** Constructor in the fixed-size case.
-      *
-      * \param dataPtr pointer to the array to map
-      * \param stride optional Stride object, passing the strides.
-      */
-    EIGEN_DEVICE_FUNC
-    explicit inline Map(PointerArgType dataPtr, const StrideType& stride = StrideType())
-      : Base(cast_to_pointer_type(dataPtr)), m_stride(stride)
-    {
-      PlainObjectType::Base::_check_template_params();
-    }
+  /** Constructor in the dynamic-size vector case.
+   *
+   * \param dataPtr pointer to the array to map
+   * \param size the size of the vector expression
+   * \param stride optional Stride object, passing the strides.
+   */
+  EIGEN_DEVICE_FUNC inline Map(PointerArgType dataPtr, Index size, const StrideType& stride = StrideType())
+      : Base(cast_to_pointer_type(dataPtr), size), m_stride(stride) {}
 
-    /** Constructor in the dynamic-size vector case.
-      *
-      * \param dataPtr pointer to the array to map
-      * \param size the size of the vector expression
-      * \param stride optional Stride object, passing the strides.
-      */
-    EIGEN_DEVICE_FUNC
-    inline Map(PointerArgType dataPtr, Index size, const StrideType& stride = StrideType())
-      : Base(cast_to_pointer_type(dataPtr), size), m_stride(stride)
-    {
-      PlainObjectType::Base::_check_template_params();
-    }
+  /** Constructor in the dynamic-size matrix case.
+   *
+   * \param dataPtr pointer to the array to map
+   * \param rows the number of rows of the matrix expression
+   * \param cols the number of columns of the matrix expression
+   * \param stride optional Stride object, passing the strides.
+   */
+  EIGEN_DEVICE_FUNC inline Map(PointerArgType dataPtr, Index rows, Index cols, const StrideType& stride = StrideType())
+      : Base(cast_to_pointer_type(dataPtr), rows, cols), m_stride(stride) {}
 
-    /** Constructor in the dynamic-size matrix case.
-      *
-      * \param dataPtr pointer to the array to map
-      * \param rows the number of rows of the matrix expression
-      * \param cols the number of columns of the matrix expression
-      * \param stride optional Stride object, passing the strides.
-      */
-    EIGEN_DEVICE_FUNC
-    inline Map(PointerArgType dataPtr, Index rows, Index cols, const StrideType& stride = StrideType())
-      : Base(cast_to_pointer_type(dataPtr), rows, cols), m_stride(stride)
-    {
-      PlainObjectType::Base::_check_template_params();
-    }
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
 
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
-
-  protected:
-    StrideType m_stride;
+ protected:
+  StrideType m_stride;
 };
 
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_MAP_H
+#endif  // EIGEN_MAP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h
index d856447..da95b5c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h
@@ -11,300 +11,273 @@
 #ifndef EIGEN_MAPBASE_H
 #define EIGEN_MAPBASE_H
 
-#define EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) \
-      EIGEN_STATIC_ASSERT((int(internal::evaluator<Derived>::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \
-                          YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT)
+#define EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)                                                               \
+  EIGEN_STATIC_ASSERT((int(internal::evaluator<Derived>::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \
+                      YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT)
+
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
 namespace Eigen {
 
 /** \ingroup Core_Module
-  *
-  * \brief Base class for dense Map and Block expression with direct access
-  *
-  * This base class provides the const low-level accessors (e.g. coeff, coeffRef) of dense
-  * Map and Block objects with direct access.
-  * Typical users do not have to directly deal with this class.
-  *
-  * This class can be extended by through the macro plugin \c EIGEN_MAPBASE_PLUGIN.
-  * See \link TopicCustomizing_Plugins customizing Eigen \endlink for details.
-  *
-  * The \c Derived class has to provide the following two methods describing the memory layout:
-  *  \code Index innerStride() const; \endcode
-  *  \code Index outerStride() const; \endcode
-  *
-  * \sa class Map, class Block
-  */
-template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
-  : public internal::dense_xpr_base<Derived>::type
-{
-  public:
+ *
+ * \brief Base class for dense Map and Block expression with direct access
+ *
+ * This base class provides the const low-level accessors (e.g. coeff, coeffRef) of dense
+ * Map and Block objects with direct access.
+ * Typical users do not have to directly deal with this class.
+ *
+ * This class can be extended by through the macro plugin \c EIGEN_MAPBASE_PLUGIN.
+ * See \link TopicCustomizing_Plugins customizing Eigen \endlink for details.
+ *
+ * The \c Derived class has to provide the following two methods describing the memory layout:
+ *  \code Index innerStride() const; \endcode
+ *  \code Index outerStride() const; \endcode
+ *
+ * \sa class Map, class Block
+ */
+template <typename Derived>
+class MapBase<Derived, ReadOnlyAccessors> : public internal::dense_xpr_base<Derived>::type {
+ public:
+  typedef typename internal::dense_xpr_base<Derived>::type Base;
+  enum {
+    RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+    ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+    InnerStrideAtCompileTime = internal::traits<Derived>::InnerStrideAtCompileTime,
+    SizeAtCompileTime = Base::SizeAtCompileTime
+  };
 
-    typedef typename internal::dense_xpr_base<Derived>::type Base;
-    enum {
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-      InnerStrideAtCompileTime = internal::traits<Derived>::InnerStrideAtCompileTime,
-      SizeAtCompileTime = Base::SizeAtCompileTime
-    };
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef std::conditional_t<bool(internal::is_lvalue<Derived>::value), Scalar*, const Scalar*> PointerType;
 
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef typename internal::conditional<
-                         bool(internal::is_lvalue<Derived>::value),
-                         Scalar *,
-                         const Scalar *>::type
-                     PointerType;
+  using Base::derived;
+  //    using Base::RowsAtCompileTime;
+  //    using Base::ColsAtCompileTime;
+  //    using Base::SizeAtCompileTime;
+  using Base::Flags;
+  using Base::IsRowMajor;
+  using Base::IsVectorAtCompileTime;
+  using Base::MaxColsAtCompileTime;
+  using Base::MaxRowsAtCompileTime;
+  using Base::MaxSizeAtCompileTime;
 
-    using Base::derived;
-//    using Base::RowsAtCompileTime;
-//    using Base::ColsAtCompileTime;
-//    using Base::SizeAtCompileTime;
-    using Base::MaxRowsAtCompileTime;
-    using Base::MaxColsAtCompileTime;
-    using Base::MaxSizeAtCompileTime;
-    using Base::IsVectorAtCompileTime;
-    using Base::Flags;
-    using Base::IsRowMajor;
+  using Base::coeff;
+  using Base::coeffRef;
+  using Base::cols;
+  using Base::eval;
+  using Base::lazyAssign;
+  using Base::rows;
+  using Base::size;
 
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
-    using Base::lazyAssign;
-    using Base::eval;
+  using Base::colStride;
+  using Base::innerStride;
+  using Base::outerStride;
+  using Base::rowStride;
 
-    using Base::innerStride;
-    using Base::outerStride;
-    using Base::rowStride;
-    using Base::colStride;
+  // bug 217 - compile error on ICC 11.1
+  using Base::operator=;
 
-    // bug 217 - compile error on ICC 11.1
-    using Base::operator=;
+  typedef typename Base::CoeffReturnType CoeffReturnType;
 
-    typedef typename Base::CoeffReturnType CoeffReturnType;
+  /** \copydoc DenseBase::rows() */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_rows.value(); }
+  /** \copydoc DenseBase::cols() */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_cols.value(); }
 
-    /** \copydoc DenseBase::rows() */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_rows.value(); }
-    /** \copydoc DenseBase::cols() */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_cols.value(); }
+  /** Returns a pointer to the first coefficient of the matrix or vector.
+   *
+   * \note When addressing this data, make sure to honor the strides returned by innerStride() and outerStride().
+   *
+   * \sa innerStride(), outerStride()
+   */
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_data; }
 
-    /** Returns a pointer to the first coefficient of the matrix or vector.
-      *
-      * \note When addressing this data, make sure to honor the strides returned by innerStride() and outerStride().
-      *
-      * \sa innerStride(), outerStride()
-      */
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_data; }
+  /** \copydoc PlainObjectBase::coeff(Index,Index) const */
+  EIGEN_DEVICE_FUNC inline const Scalar& coeff(Index rowId, Index colId) const {
+    return m_data[colId * colStride() + rowId * rowStride()];
+  }
 
-    /** \copydoc PlainObjectBase::coeff(Index,Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeff(Index rowId, Index colId) const
-    {
-      return m_data[colId * colStride() + rowId * rowStride()];
-    }
+  /** \copydoc PlainObjectBase::coeff(Index) const */
+  EIGEN_DEVICE_FUNC inline const Scalar& coeff(Index index) const {
+    EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+    return m_data[index * innerStride()];
+  }
 
-    /** \copydoc PlainObjectBase::coeff(Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeff(Index index) const
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return m_data[index * innerStride()];
-    }
+  /** \copydoc PlainObjectBase::coeffRef(Index,Index) const */
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const {
+    return this->m_data[colId * colStride() + rowId * rowStride()];
+  }
 
-    /** \copydoc PlainObjectBase::coeffRef(Index,Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      return this->m_data[colId * colStride() + rowId * rowStride()];
-    }
+  /** \copydoc PlainObjectBase::coeffRef(Index) const */
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const {
+    EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+    return this->m_data[index * innerStride()];
+  }
 
-    /** \copydoc PlainObjectBase::coeffRef(Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index index) const
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return this->m_data[index * innerStride()];
-    }
+  /** \internal */
+  template <int LoadMode>
+  inline PacketScalar packet(Index rowId, Index colId) const {
+    return internal::ploadt<PacketScalar, LoadMode>(m_data + (colId * colStride() + rowId * rowStride()));
+  }
 
-    /** \internal */
-    template<int LoadMode>
-    inline PacketScalar packet(Index rowId, Index colId) const
-    {
-      return internal::ploadt<PacketScalar, LoadMode>
-               (m_data + (colId * colStride() + rowId * rowStride()));
-    }
+  /** \internal */
+  template <int LoadMode>
+  inline PacketScalar packet(Index index) const {
+    EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+    return internal::ploadt<PacketScalar, LoadMode>(m_data + index * innerStride());
+  }
 
-    /** \internal */
-    template<int LoadMode>
-    inline PacketScalar packet(Index index) const
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return internal::ploadt<PacketScalar, LoadMode>(m_data + index * innerStride());
-    }
+  /** \internal Constructor for fixed size matrices or vectors */
+  EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr)
+      : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) {
+    EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+    checkSanity<Derived>();
+  }
 
-    /** \internal Constructor for fixed size matrices or vectors */
-    EIGEN_DEVICE_FUNC
-    explicit inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime)
-    {
-      EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
-      checkSanity<Derived>();
-    }
+  /** \internal Constructor for dynamically sized vectors */
+  EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize)
+      : m_data(dataPtr),
+        m_rows(RowsAtCompileTime == Dynamic ? vecSize : Index(RowsAtCompileTime)),
+        m_cols(ColsAtCompileTime == Dynamic ? vecSize : Index(ColsAtCompileTime)) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+    eigen_assert(vecSize >= 0);
+    eigen_assert(dataPtr == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == vecSize);
+    checkSanity<Derived>();
+  }
 
-    /** \internal Constructor for dynamically sized vectors */
-    EIGEN_DEVICE_FUNC
-    inline MapBase(PointerType dataPtr, Index vecSize)
-            : m_data(dataPtr),
-              m_rows(RowsAtCompileTime == Dynamic ? vecSize : Index(RowsAtCompileTime)),
-              m_cols(ColsAtCompileTime == Dynamic ? vecSize : Index(ColsAtCompileTime))
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-      eigen_assert(vecSize >= 0);
-      eigen_assert(dataPtr == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == vecSize);
-      checkSanity<Derived>();
-    }
+  /** \internal Constructor for dynamically sized matrices */
+  EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols)
+      : m_data(dataPtr), m_rows(rows), m_cols(cols) {
+    eigen_assert((dataPtr == 0) || (rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) &&
+                                    cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)));
+    checkSanity<Derived>();
+  }
 
-    /** \internal Constructor for dynamically sized matrices */
-    EIGEN_DEVICE_FUNC
-    inline MapBase(PointerType dataPtr, Index rows, Index cols)
-            : m_data(dataPtr), m_rows(rows), m_cols(cols)
-    {
-      eigen_assert( (dataPtr == 0)
-              || (   rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
-                  && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)));
-      checkSanity<Derived>();
-    }
-
-    #ifdef EIGEN_MAPBASE_PLUGIN
-    #include EIGEN_MAPBASE_PLUGIN
-    #endif
-
-  protected:
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase)
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase)
-
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    void checkSanity(typename internal::enable_if<(internal::traits<T>::Alignment>0),void*>::type = 0) const
-    {
-#if EIGEN_MAX_ALIGN_BYTES>0
-      // innerStride() is not set yet when this function is called, so we optimistically assume the lowest plausible value:
-      const Index minInnerStride = InnerStrideAtCompileTime == Dynamic ? 1 : Index(InnerStrideAtCompileTime);
-      EIGEN_ONLY_USED_FOR_DEBUG(minInnerStride);
-      eigen_assert((   ((internal::UIntPtr(m_data) % internal::traits<Derived>::Alignment) == 0)
-                    || (cols() * rows() * minInnerStride * sizeof(Scalar)) < internal::traits<Derived>::Alignment ) && "data is not aligned");
+#ifdef EIGEN_MAPBASE_PLUGIN
+#include EIGEN_MAPBASE_PLUGIN
 #endif
-    }
 
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    void checkSanity(typename internal::enable_if<internal::traits<T>::Alignment==0,void*>::type = 0) const
-    {}
+ protected:
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase)
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase)
 
-    PointerType m_data;
-    const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
-    const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
+  template <typename T>
+  EIGEN_DEVICE_FUNC void checkSanity(std::enable_if_t<(internal::traits<T>::Alignment > 0), void*> = 0) const {
+// Temporary macro to allow scalars to not be properly aligned.  This is while we sort out failures
+// in TensorFlow Lite that are currently relying on this UB.
+#ifndef EIGEN_ALLOW_UNALIGNED_SCALARS
+    // Pointer must be aligned to the Scalar type, otherwise we get UB.
+    eigen_assert((std::uintptr_t(m_data) % alignof(Scalar) == 0) && "data is not scalar-aligned");
+#endif
+#if EIGEN_MAX_ALIGN_BYTES > 0
+    // innerStride() is not set yet when this function is called, so we optimistically assume the lowest plausible
+    // value:
+    const Index minInnerStride = InnerStrideAtCompileTime == Dynamic ? 1 : Index(InnerStrideAtCompileTime);
+    EIGEN_ONLY_USED_FOR_DEBUG(minInnerStride);
+    eigen_assert((((std::uintptr_t(m_data) % internal::traits<Derived>::Alignment) == 0) ||
+                  (cols() * rows() * minInnerStride * sizeof(Scalar)) < internal::traits<Derived>::Alignment) &&
+                 "data is not aligned");
+#endif
+  }
+
+  template <typename T>
+  EIGEN_DEVICE_FUNC void checkSanity(std::enable_if_t<internal::traits<T>::Alignment == 0, void*> = 0) const {
+#ifndef EIGEN_ALLOW_UNALIGNED_SCALARS
+    // Pointer must be aligned to the Scalar type, otherwise we get UB.
+    eigen_assert((std::uintptr_t(m_data) % alignof(Scalar) == 0) && "data is not scalar-aligned");
+#endif
+  }
+
+  PointerType m_data;
+  const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
+  const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
 };
 
 /** \ingroup Core_Module
-  *
-  * \brief Base class for non-const dense Map and Block expression with direct access
-  *
-  * This base class provides the non-const low-level accessors (e.g. coeff and coeffRef) of
-  * dense Map and Block objects with direct access.
-  * It inherits MapBase<Derived, ReadOnlyAccessors> which defines the const variant for reading specific entries.
-  *
-  * \sa class Map, class Block
-  */
-template<typename Derived> class MapBase<Derived, WriteAccessors>
-  : public MapBase<Derived, ReadOnlyAccessors>
-{
-    typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
-  public:
+ *
+ * \brief Base class for non-const dense Map and Block expression with direct access
+ *
+ * This base class provides the non-const low-level accessors (e.g. coeff and coeffRef) of
+ * dense Map and Block objects with direct access.
+ * It inherits MapBase<Derived, ReadOnlyAccessors> which defines the const variant for reading specific entries.
+ *
+ * \sa class Map, class Block
+ */
+template <typename Derived>
+class MapBase<Derived, WriteAccessors> : public MapBase<Derived, ReadOnlyAccessors> {
+  typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
 
-    typedef MapBase<Derived, ReadOnlyAccessors> Base;
+ public:
+  typedef MapBase<Derived, ReadOnlyAccessors> Base;
 
-    typedef typename Base::Scalar Scalar;
-    typedef typename Base::PacketScalar PacketScalar;
-    typedef typename Base::StorageIndex StorageIndex;
-    typedef typename Base::PointerType PointerType;
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::PacketScalar PacketScalar;
+  typedef typename Base::StorageIndex StorageIndex;
+  typedef typename Base::PointerType PointerType;
 
-    using Base::derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
+  using Base::coeff;
+  using Base::coeffRef;
+  using Base::cols;
+  using Base::derived;
+  using Base::rows;
+  using Base::size;
 
-    using Base::innerStride;
-    using Base::outerStride;
-    using Base::rowStride;
-    using Base::colStride;
+  using Base::colStride;
+  using Base::innerStride;
+  using Base::outerStride;
+  using Base::rowStride;
 
-    typedef typename internal::conditional<
-                    internal::is_lvalue<Derived>::value,
-                    Scalar,
-                    const Scalar
-                  >::type ScalarWithConstIfNotLvalue;
+  typedef std::conditional_t<internal::is_lvalue<Derived>::value, Scalar, const Scalar> ScalarWithConstIfNotLvalue;
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar* data() const { return this->m_data; }
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue* data() { return this->m_data; } // no const-cast here so non-const-correct code will give a compile error
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const { return this->m_data; }
+  EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() {
+    return this->m_data;
+  }  // no const-cast here so non-const-correct code will give a compile error
 
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue& coeffRef(Index row, Index col)
-    {
-      return this->m_data[col * colStride() + row * rowStride()];
-    }
+  EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue& coeffRef(Index row, Index col) {
+    return this->m_data[col * colStride() + row * rowStride()];
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue& coeffRef(Index index)
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return this->m_data[index * innerStride()];
-    }
+  EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue& coeffRef(Index index) {
+    EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+    return this->m_data[index * innerStride()];
+  }
 
-    template<int StoreMode>
-    inline void writePacket(Index row, Index col, const PacketScalar& val)
-    {
-      internal::pstoret<Scalar, PacketScalar, StoreMode>
-               (this->m_data + (col * colStride() + row * rowStride()), val);
-    }
+  template <int StoreMode>
+  inline void writePacket(Index row, Index col, const PacketScalar& val) {
+    internal::pstoret<Scalar, PacketScalar, StoreMode>(this->m_data + (col * colStride() + row * rowStride()), val);
+  }
 
-    template<int StoreMode>
-    inline void writePacket(Index index, const PacketScalar& val)
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      internal::pstoret<Scalar, PacketScalar, StoreMode>
-                (this->m_data + index * innerStride(), val);
-    }
+  template <int StoreMode>
+  inline void writePacket(Index index, const PacketScalar& val) {
+    EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+    internal::pstoret<Scalar, PacketScalar, StoreMode>(this->m_data + index * innerStride(), val);
+  }
 
-    EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {}
-    EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {}
-    EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols) : Base(dataPtr, rows, cols) {}
+  EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {}
+  EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {}
+  EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols) : Base(dataPtr, rows, cols) {}
 
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const MapBase& other)
-    {
-      ReadOnlyMapBase::Base::operator=(other);
-      return derived();
-    }
+  EIGEN_DEVICE_FUNC Derived& operator=(const MapBase& other) {
+    ReadOnlyMapBase::Base::operator=(other);
+    return derived();
+  }
 
-    // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base,
-    // see bugs 821 and 920.
-    using ReadOnlyMapBase::Base::operator=;
-  protected:
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase)
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase)
+  // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base,
+  // see bugs 821 and 920.
+  using ReadOnlyMapBase::Base::operator=;
+
+ protected:
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase)
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase)
 };
 
 #undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MAPBASE_H
+#endif  // EIGEN_MAPBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
index 61b78f4..95f9b97 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
@@ -13,441 +13,344 @@
 
 // TODO this should better be moved to NumTraits
 // Source: WolframAlpha
-#define EIGEN_PI    3.141592653589793238462643383279502884197169399375105820974944592307816406L
+#define EIGEN_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406L
 #define EIGEN_LOG2E 1.442695040888963407359924681001892137426645954152985934135449406931109219L
-#define EIGEN_LN2   0.693147180559945309417232121458176568075500134360255254120680009493393621L
+#define EIGEN_LN2 0.693147180559945309417232121458176568075500134360255254120680009493393621L
+
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
 namespace Eigen {
 
-// On WINCE, std::abs is defined for int only, so let's defined our own overloads:
-// This issue has been confirmed with MSVC 2008 only, but the issue might exist for more recent versions too.
-#if EIGEN_OS_WINCE && EIGEN_COMP_MSVC && EIGEN_COMP_MSVC<=1500
-long        abs(long        x) { return (labs(x));  }
-double      abs(double      x) { return (fabs(x));  }
-float       abs(float       x) { return (fabsf(x)); }
-long double abs(long double x) { return (fabsl(x)); }
-#endif
-
 namespace internal {
 
 /** \internal \class global_math_functions_filtering_base
-  *
-  * What it does:
-  * Defines a typedef 'type' as follows:
-  * - if type T has a member typedef Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl, then
-  *   global_math_functions_filtering_base<T>::type is a typedef for it.
-  * - otherwise, global_math_functions_filtering_base<T>::type is a typedef for T.
-  *
-  * How it's used:
-  * To allow to defined the global math functions (like sin...) in certain cases, like the Array expressions.
-  * When you do sin(array1+array2), the object array1+array2 has a complicated expression type, all what you want to know
-  * is that it inherits ArrayBase. So we implement a partial specialization of sin_impl for ArrayBase<Derived>.
-  * So we must make sure to use sin_impl<ArrayBase<Derived> > and not sin_impl<Derived>, otherwise our partial specialization
-  * won't be used. How does sin know that? That's exactly what global_math_functions_filtering_base tells it.
-  *
-  * How it's implemented:
-  * SFINAE in the style of enable_if. Highly susceptible of breaking compilers. With GCC, it sure does work, but if you replace
-  * the typename dummy by an integer template parameter, it doesn't work anymore!
-  */
+ *
+ * What it does:
+ * Defines a typedef 'type' as follows:
+ * - if type T has a member typedef Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl, then
+ *   global_math_functions_filtering_base<T>::type is a typedef for it.
+ * - otherwise, global_math_functions_filtering_base<T>::type is a typedef for T.
+ *
+ * How it's used:
+ * To allow to defined the global math functions (like sin...) in certain cases, like the Array expressions.
+ * When you do sin(array1+array2), the object array1+array2 has a complicated expression type, all what you want to know
+ * is that it inherits ArrayBase. So we implement a partial specialization of sin_impl for ArrayBase<Derived>.
+ * So we must make sure to use sin_impl<ArrayBase<Derived> > and not sin_impl<Derived>, otherwise our partial
+ * specialization won't be used. How does sin know that? That's exactly what global_math_functions_filtering_base tells
+ * it.
+ *
+ * How it's implemented:
+ * SFINAE in the style of enable_if. Highly susceptible of breaking compilers. With GCC, it sure does work, but if you
+ * replace the typename dummy by an integer template parameter, it doesn't work anymore!
+ */
 
-template<typename T, typename dummy = void>
-struct global_math_functions_filtering_base
-{
+template <typename T, typename dummy = void>
+struct global_math_functions_filtering_base {
   typedef T type;
 };
 
-template<typename T> struct always_void { typedef void type; };
+template <typename T>
+struct always_void {
+  typedef void type;
+};
 
-template<typename T>
-struct global_math_functions_filtering_base
-  <T,
-   typename always_void<typename T::Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl>::type
-  >
-{
+template <typename T>
+struct global_math_functions_filtering_base<
+    T, typename always_void<typename T::Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl>::type> {
   typedef typename T::Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl type;
 };
 
-#define EIGEN_MATHFUNC_IMPL(func, scalar) Eigen::internal::func##_impl<typename Eigen::internal::global_math_functions_filtering_base<scalar>::type>
-#define EIGEN_MATHFUNC_RETVAL(func, scalar) typename Eigen::internal::func##_retval<typename Eigen::internal::global_math_functions_filtering_base<scalar>::type>::type
+#define EIGEN_MATHFUNC_IMPL(func, scalar) \
+  Eigen::internal::func##_impl<typename Eigen::internal::global_math_functions_filtering_base<scalar>::type>
+#define EIGEN_MATHFUNC_RETVAL(func, scalar) \
+  typename Eigen::internal::func##_retval<  \
+      typename Eigen::internal::global_math_functions_filtering_base<scalar>::type>::type
 
 /****************************************************************************
-* Implementation of real                                                 *
-****************************************************************************/
+ * Implementation of real                                                 *
+ ****************************************************************************/
 
-template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
-struct real_default_impl
-{
+template <typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct real_default_impl {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
-    return x;
-  }
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x; }
 };
 
-template<typename Scalar>
-struct real_default_impl<Scalar,true>
-{
+template <typename Scalar>
+struct real_default_impl<Scalar, true> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
     using std::real;
     return real(x);
   }
 };
 
-template<typename Scalar> struct real_impl : real_default_impl<Scalar> {};
+template <typename Scalar>
+struct real_impl : real_default_impl<Scalar> {};
 
 #if defined(EIGEN_GPU_COMPILE_PHASE)
-template<typename T>
-struct real_impl<std::complex<T> >
-{
+template <typename T>
+struct real_impl<std::complex<T>> {
   typedef T RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline T run(const std::complex<T>& x)
-  {
-    return x.real();
-  }
+  EIGEN_DEVICE_FUNC static inline T run(const std::complex<T>& x) { return x.real(); }
 };
 #endif
 
-template<typename Scalar>
-struct real_retval
-{
+template <typename Scalar>
+struct real_retval {
   typedef typename NumTraits<Scalar>::Real type;
 };
 
 /****************************************************************************
-* Implementation of imag                                                 *
-****************************************************************************/
+ * Implementation of imag                                                 *
+ ****************************************************************************/
 
-template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
-struct imag_default_impl
-{
+template <typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct imag_default_impl {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar&)
-  {
-    return RealScalar(0);
-  }
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar&) { return RealScalar(0); }
 };
 
-template<typename Scalar>
-struct imag_default_impl<Scalar,true>
-{
+template <typename Scalar>
+struct imag_default_impl<Scalar, true> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
     using std::imag;
     return imag(x);
   }
 };
 
-template<typename Scalar> struct imag_impl : imag_default_impl<Scalar> {};
+template <typename Scalar>
+struct imag_impl : imag_default_impl<Scalar> {};
 
 #if defined(EIGEN_GPU_COMPILE_PHASE)
-template<typename T>
-struct imag_impl<std::complex<T> >
-{
+template <typename T>
+struct imag_impl<std::complex<T>> {
   typedef T RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline T run(const std::complex<T>& x)
-  {
-    return x.imag();
-  }
+  EIGEN_DEVICE_FUNC static inline T run(const std::complex<T>& x) { return x.imag(); }
 };
 #endif
 
-template<typename Scalar>
-struct imag_retval
-{
+template <typename Scalar>
+struct imag_retval {
   typedef typename NumTraits<Scalar>::Real type;
 };
 
 /****************************************************************************
-* Implementation of real_ref                                             *
-****************************************************************************/
+ * Implementation of real_ref                                             *
+ ****************************************************************************/
 
-template<typename Scalar>
-struct real_ref_impl
-{
+template <typename Scalar>
+struct real_ref_impl {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar& run(Scalar& x)
-  {
-    return reinterpret_cast<RealScalar*>(&x)[0];
-  }
-  EIGEN_DEVICE_FUNC
-  static inline const RealScalar& run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar& run(Scalar& x) { return reinterpret_cast<RealScalar*>(&x)[0]; }
+  EIGEN_DEVICE_FUNC static inline const RealScalar& run(const Scalar& x) {
     return reinterpret_cast<const RealScalar*>(&x)[0];
   }
 };
 
-template<typename Scalar>
-struct real_ref_retval
-{
-  typedef typename NumTraits<Scalar>::Real & type;
+template <typename Scalar>
+struct real_ref_retval {
+  typedef typename NumTraits<Scalar>::Real& type;
 };
 
 /****************************************************************************
-* Implementation of imag_ref                                             *
-****************************************************************************/
+ * Implementation of imag_ref                                             *
+ ****************************************************************************/
 
-template<typename Scalar, bool IsComplex>
-struct imag_ref_default_impl
-{
+template <typename Scalar, bool IsComplex>
+struct imag_ref_default_impl {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar& run(Scalar& x)
-  {
-    return reinterpret_cast<RealScalar*>(&x)[1];
-  }
-  EIGEN_DEVICE_FUNC
-  static inline const RealScalar& run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar& run(Scalar& x) { return reinterpret_cast<RealScalar*>(&x)[1]; }
+  EIGEN_DEVICE_FUNC static inline const RealScalar& run(const Scalar& x) {
     return reinterpret_cast<RealScalar*>(&x)[1];
   }
 };
 
-template<typename Scalar>
-struct imag_ref_default_impl<Scalar, false>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline Scalar run(Scalar&)
-  {
-    return Scalar(0);
-  }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline const Scalar run(const Scalar&)
-  {
-    return Scalar(0);
-  }
+template <typename Scalar>
+struct imag_ref_default_impl<Scalar, false> {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Scalar run(Scalar&) { return Scalar(0); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline const Scalar run(const Scalar&) { return Scalar(0); }
 };
 
-template<typename Scalar>
+template <typename Scalar>
 struct imag_ref_impl : imag_ref_default_impl<Scalar, NumTraits<Scalar>::IsComplex> {};
 
-template<typename Scalar>
-struct imag_ref_retval
-{
-  typedef typename NumTraits<Scalar>::Real & type;
+template <typename Scalar>
+struct imag_ref_retval {
+  typedef typename NumTraits<Scalar>::Real& type;
 };
 
 /****************************************************************************
-* Implementation of conj                                                 *
-****************************************************************************/
+ * Implementation of conj                                                 *
+ ****************************************************************************/
 
-template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
-struct conj_default_impl
-{
-  EIGEN_DEVICE_FUNC
-  static inline Scalar run(const Scalar& x)
-  {
-    return x;
-  }
+template <typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct conj_default_impl {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { return x; }
 };
 
-template<typename Scalar>
-struct conj_default_impl<Scalar,true>
-{
-  EIGEN_DEVICE_FUNC
-  static inline Scalar run(const Scalar& x)
-  {
+template <typename Scalar>
+struct conj_default_impl<Scalar, true> {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) {
     using std::conj;
     return conj(x);
   }
 };
 
-template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+template <typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
 struct conj_impl : conj_default_impl<Scalar, IsComplex> {};
 
-template<typename Scalar>
-struct conj_retval
-{
+template <typename Scalar>
+struct conj_retval {
   typedef Scalar type;
 };
 
 /****************************************************************************
-* Implementation of abs2                                                 *
-****************************************************************************/
+ * Implementation of abs2                                                 *
+ ****************************************************************************/
 
-template<typename Scalar,bool IsComplex>
-struct abs2_impl_default
+template <typename Scalar, bool IsComplex>
+struct abs2_impl_default {
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x * x; }
+};
+
+template <typename Scalar>
+struct abs2_impl_default<Scalar, true>  // IsComplex
 {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
-    return x*x;
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x.real() * x.real() + x.imag() * x.imag(); }
+};
+
+template <typename Scalar>
+struct abs2_impl {
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
+    return abs2_impl_default<Scalar, NumTraits<Scalar>::IsComplex>::run(x);
   }
 };
 
-template<typename Scalar>
-struct abs2_impl_default<Scalar, true> // IsComplex
-{
-  typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
-    return x.real()*x.real() + x.imag()*x.imag();
-  }
-};
-
-template<typename Scalar>
-struct abs2_impl
-{
-  typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
-    return abs2_impl_default<Scalar,NumTraits<Scalar>::IsComplex>::run(x);
-  }
-};
-
-template<typename Scalar>
-struct abs2_retval
-{
+template <typename Scalar>
+struct abs2_retval {
   typedef typename NumTraits<Scalar>::Real type;
 };
 
 /****************************************************************************
-* Implementation of sqrt/rsqrt                                             *
-****************************************************************************/
+ * Implementation of sqrt/rsqrt                                             *
+ ****************************************************************************/
 
-template<typename Scalar>
-struct sqrt_impl
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_ALWAYS_INLINE Scalar run(const Scalar& x)
-  {
+template <typename Scalar>
+struct sqrt_impl {
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE Scalar run(const Scalar& x) {
     EIGEN_USING_STD(sqrt);
     return sqrt(x);
   }
 };
 
 // Complex sqrt defined in MathFunctionsImpl.h.
-template<typename T> EIGEN_DEVICE_FUNC std::complex<T> complex_sqrt(const std::complex<T>& a_x);
+template <typename T>
+EIGEN_DEVICE_FUNC std::complex<T> complex_sqrt(const std::complex<T>& a_x);
 
 // Custom implementation is faster than `std::sqrt`, works on
 // GPU, and correctly handles special cases (unlike MSVC).
-template<typename T>
-struct sqrt_impl<std::complex<T> >
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_ALWAYS_INLINE std::complex<T> run(const std::complex<T>& x)
-  {
+template <typename T>
+struct sqrt_impl<std::complex<T>> {
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE std::complex<T> run(const std::complex<T>& x) {
     return complex_sqrt<T>(x);
   }
 };
 
-template<typename Scalar>
-struct sqrt_retval
-{
+template <typename Scalar>
+struct sqrt_retval {
   typedef Scalar type;
 };
 
 // Default implementation relies on numext::sqrt, at bottom of file.
-template<typename T>
+template <typename T>
 struct rsqrt_impl;
 
 // Complex rsqrt defined in MathFunctionsImpl.h.
-template<typename T> EIGEN_DEVICE_FUNC std::complex<T> complex_rsqrt(const std::complex<T>& a_x);
+template <typename T>
+EIGEN_DEVICE_FUNC std::complex<T> complex_rsqrt(const std::complex<T>& a_x);
 
-template<typename T>
-struct rsqrt_impl<std::complex<T> >
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_ALWAYS_INLINE std::complex<T> run(const std::complex<T>& x)
-  {
+template <typename T>
+struct rsqrt_impl<std::complex<T>> {
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE std::complex<T> run(const std::complex<T>& x) {
     return complex_rsqrt<T>(x);
   }
 };
 
-template<typename Scalar>
-struct rsqrt_retval
-{
+template <typename Scalar>
+struct rsqrt_retval {
   typedef Scalar type;
 };
 
 /****************************************************************************
-* Implementation of norm1                                                *
-****************************************************************************/
+ * Implementation of norm1                                                *
+ ****************************************************************************/
 
-template<typename Scalar, bool IsComplex>
+template <typename Scalar, bool IsComplex>
 struct norm1_default_impl;
 
-template<typename Scalar>
-struct norm1_default_impl<Scalar,true>
-{
+template <typename Scalar>
+struct norm1_default_impl<Scalar, true> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
     EIGEN_USING_STD(abs);
     return abs(x.real()) + abs(x.imag());
   }
 };
 
-template<typename Scalar>
-struct norm1_default_impl<Scalar, false>
-{
-  EIGEN_DEVICE_FUNC
-  static inline Scalar run(const Scalar& x)
-  {
+template <typename Scalar>
+struct norm1_default_impl<Scalar, false> {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) {
     EIGEN_USING_STD(abs);
     return abs(x);
   }
 };
 
-template<typename Scalar>
+template <typename Scalar>
 struct norm1_impl : norm1_default_impl<Scalar, NumTraits<Scalar>::IsComplex> {};
 
-template<typename Scalar>
-struct norm1_retval
-{
+template <typename Scalar>
+struct norm1_retval {
   typedef typename NumTraits<Scalar>::Real type;
 };
 
 /****************************************************************************
-* Implementation of hypot                                                *
-****************************************************************************/
+ * Implementation of hypot                                                *
+ ****************************************************************************/
 
-template<typename Scalar> struct hypot_impl;
+template <typename Scalar>
+struct hypot_impl;
 
-template<typename Scalar>
-struct hypot_retval
-{
+template <typename Scalar>
+struct hypot_retval {
   typedef typename NumTraits<Scalar>::Real type;
 };
 
 /****************************************************************************
-* Implementation of cast                                                 *
-****************************************************************************/
+ * Implementation of cast                                                 *
+ ****************************************************************************/
 
-template<typename OldType, typename NewType, typename EnableIf = void>
-struct cast_impl
-{
-  EIGEN_DEVICE_FUNC
-  static inline NewType run(const OldType& x)
-  {
-    return static_cast<NewType>(x);
-  }
+template <typename OldType, typename NewType, typename EnableIf = void>
+struct cast_impl {
+  EIGEN_DEVICE_FUNC static inline NewType run(const OldType& x) { return static_cast<NewType>(x); }
+};
+
+template <typename OldType>
+struct cast_impl<OldType, bool> {
+  EIGEN_DEVICE_FUNC static inline bool run(const OldType& x) { return x != OldType(0); }
 };
 
 // Casting from S -> Complex<T> leads to an implicit conversion from S to T,
 // generating warnings on clang.  Here we explicitly cast the real component.
-template<typename OldType, typename NewType>
+template <typename OldType, typename NewType>
 struct cast_impl<OldType, NewType,
-  typename internal::enable_if<
-    !NumTraits<OldType>::IsComplex && NumTraits<NewType>::IsComplex
-  >::type>
-{
-  EIGEN_DEVICE_FUNC
-  static inline NewType run(const OldType& x)
-  {
+                 typename std::enable_if_t<!NumTraits<OldType>::IsComplex && NumTraits<NewType>::IsComplex>> {
+  EIGEN_DEVICE_FUNC static inline NewType run(const OldType& x) {
     typedef typename NumTraits<NewType>::Real NewReal;
     return static_cast<NewType>(static_cast<NewReal>(x));
   }
@@ -455,345 +358,205 @@
 
 // here, for once, we're plainly returning NewType: we don't want cast to do weird things.
 
-template<typename OldType, typename NewType>
-EIGEN_DEVICE_FUNC
-inline NewType cast(const OldType& x)
-{
+template <typename OldType, typename NewType>
+EIGEN_DEVICE_FUNC inline NewType cast(const OldType& x) {
   return cast_impl<OldType, NewType>::run(x);
 }
 
 /****************************************************************************
-* Implementation of round                                                   *
-****************************************************************************/
-
-template<typename Scalar>
-struct round_impl
-{
-  EIGEN_DEVICE_FUNC
-  static inline Scalar run(const Scalar& x)
-  {
-    EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
-#if EIGEN_HAS_CXX11_MATH
-    EIGEN_USING_STD(round);
-#endif
-    return Scalar(round(x));
-  }
-};
-
-#if !EIGEN_HAS_CXX11_MATH
-#if EIGEN_HAS_C99_MATH
-// Use ::roundf for float.
-template<>
-struct round_impl<float> {
-  EIGEN_DEVICE_FUNC
-  static inline float run(const float& x)
-  {
-    return ::roundf(x);
-  }
-};
-#else
-template<typename Scalar>
-struct round_using_floor_ceil_impl
-{
-  EIGEN_DEVICE_FUNC
-  static inline Scalar run(const Scalar& x)
-  {
-    EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
-    // Without C99 round/roundf, resort to floor/ceil.
-    EIGEN_USING_STD(floor);
-    EIGEN_USING_STD(ceil);
-    // If not enough precision to resolve a decimal at all, return the input.
-    // Otherwise, adding 0.5 can trigger an increment by 1.
-    const Scalar limit = Scalar(1ull << (NumTraits<Scalar>::digits() - 1));
-    if (x >= limit || x <= -limit) {
-      return x;
-    }
-    return (x > Scalar(0)) ? Scalar(floor(x + Scalar(0.5))) : Scalar(ceil(x - Scalar(0.5)));
-  }
-};
-
-template<>
-struct round_impl<float> : round_using_floor_ceil_impl<float> {};
-
-template<>
-struct round_impl<double> : round_using_floor_ceil_impl<double> {};
-#endif // EIGEN_HAS_C99_MATH
-#endif // !EIGEN_HAS_CXX11_MATH
-
-template<typename Scalar>
-struct round_retval
-{
-  typedef Scalar type;
-};
-
-/****************************************************************************
-* Implementation of rint                                                    *
-****************************************************************************/
-
-template<typename Scalar>
-struct rint_impl {
-  EIGEN_DEVICE_FUNC
-  static inline Scalar run(const Scalar& x)
-  {
-    EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
-#if EIGEN_HAS_CXX11_MATH
-      EIGEN_USING_STD(rint);
-#endif
-    return rint(x);
-  }
-};
-
-#if !EIGEN_HAS_CXX11_MATH
-template<>
-struct rint_impl<double> {
-  EIGEN_DEVICE_FUNC
-  static inline double run(const double& x)
-  {
-    return ::rint(x);
-  }
-};
-template<>
-struct rint_impl<float> {
-  EIGEN_DEVICE_FUNC
-  static inline float run(const float& x)
-  {
-    return ::rintf(x);
-  }
-};
-#endif
-
-template<typename Scalar>
-struct rint_retval
-{
-  typedef Scalar type;
-};
-
-/****************************************************************************
-* Implementation of arg                                                     *
-****************************************************************************/
+ * Implementation of arg                                                     *
+ ****************************************************************************/
 
 // Visual Studio 2017 has a bug where arg(float) returns 0 for negative inputs.
 // This seems to be fixed in VS 2019.
-#if EIGEN_HAS_CXX11_MATH && (!EIGEN_COMP_MSVC || EIGEN_COMP_MSVC >= 1920)
+#if (!EIGEN_COMP_MSVC || EIGEN_COMP_MSVC >= 1920)
 // std::arg is only defined for types of std::complex, or integer types or float/double/long double
-template<typename Scalar,
-          bool HasStdImpl = NumTraits<Scalar>::IsComplex || is_integral<Scalar>::value
-                            || is_same<Scalar, float>::value || is_same<Scalar, double>::value
-                            || is_same<Scalar, long double>::value >
+template <typename Scalar, bool HasStdImpl = NumTraits<Scalar>::IsComplex || is_integral<Scalar>::value ||
+                                             is_same<Scalar, float>::value || is_same<Scalar, double>::value ||
+                                             is_same<Scalar, long double>::value>
 struct arg_default_impl;
 
-template<typename Scalar>
+template <typename Scalar>
 struct arg_default_impl<Scalar, true> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
-    #if defined(EIGEN_HIP_DEVICE_COMPILE)
-    // HIP does not seem to have a native device side implementation for the math routine "arg"
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
+    // There is no official ::arg on device in CUDA/HIP, so we always need to use std::arg.
     using std::arg;
-    #else
-    EIGEN_USING_STD(arg);
-    #endif
     return static_cast<RealScalar>(arg(x));
   }
 };
 
 // Must be non-complex floating-point type (e.g. half/bfloat16).
-template<typename Scalar>
+template <typename Scalar>
 struct arg_default_impl<Scalar, false> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
     return (x < Scalar(0)) ? RealScalar(EIGEN_PI) : RealScalar(0);
   }
 };
 #else
-template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
-struct arg_default_impl
-{
+template <typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct arg_default_impl {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
     return (x < RealScalar(0)) ? RealScalar(EIGEN_PI) : RealScalar(0);
   }
 };
 
-template<typename Scalar>
-struct arg_default_impl<Scalar,true>
-{
+template <typename Scalar>
+struct arg_default_impl<Scalar, true> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  EIGEN_DEVICE_FUNC
-  static inline RealScalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) {
     EIGEN_USING_STD(arg);
     return arg(x);
   }
 };
 #endif
-template<typename Scalar> struct arg_impl : arg_default_impl<Scalar> {};
+template <typename Scalar>
+struct arg_impl : arg_default_impl<Scalar> {};
 
-template<typename Scalar>
-struct arg_retval
-{
+template <typename Scalar>
+struct arg_retval {
   typedef typename NumTraits<Scalar>::Real type;
 };
 
 /****************************************************************************
-* Implementation of expm1                                                   *
-****************************************************************************/
+ * Implementation of expm1                                                   *
+ ****************************************************************************/
 
 // This implementation is based on GSL Math's expm1.
 namespace std_fallback {
-  // fallback expm1 implementation in case there is no expm1(Scalar) function in namespace of Scalar,
-  // or that there is no suitable std::expm1 function available. Implementation
-  // attributed to Kahan. See: http://www.plunk.org/~hatch/rightway.php.
-  template<typename Scalar>
-  EIGEN_DEVICE_FUNC inline Scalar expm1(const Scalar& x) {
-    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+// fallback expm1 implementation in case there is no expm1(Scalar) function in namespace of Scalar,
+// or that there is no suitable std::expm1 function available. Implementation
+// attributed to Kahan. See: http://www.plunk.org/~hatch/rightway.php.
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline Scalar expm1(const Scalar& x) {
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    EIGEN_USING_STD(exp);
-    Scalar u = exp(x);
-    if (numext::equal_strict(u, Scalar(1))) {
-      return x;
-    }
-    Scalar um1 = u - RealScalar(1);
-    if (numext::equal_strict(um1, Scalar(-1))) {
-      return RealScalar(-1);
-    }
-
-    EIGEN_USING_STD(log);
-    Scalar logu = log(u);
-    return numext::equal_strict(u, logu) ? u : (u - RealScalar(1)) * x / logu;
+  EIGEN_USING_STD(exp);
+  Scalar u = exp(x);
+  if (numext::equal_strict(u, Scalar(1))) {
+    return x;
   }
-}
+  Scalar um1 = u - RealScalar(1);
+  if (numext::equal_strict(um1, Scalar(-1))) {
+    return RealScalar(-1);
+  }
 
-template<typename Scalar>
+  EIGEN_USING_STD(log);
+  Scalar logu = log(u);
+  return numext::equal_strict(u, logu) ? u : (u - RealScalar(1)) * x / logu;
+}
+}  // namespace std_fallback
+
+template <typename Scalar>
 struct expm1_impl {
-  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) {
     EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
-    #if EIGEN_HAS_CXX11_MATH
-    using std::expm1;
-    #else
-    using std_fallback::expm1;
-    #endif
+    EIGEN_USING_STD(expm1);
     return expm1(x);
   }
 };
 
-template<typename Scalar>
-struct expm1_retval
-{
+template <typename Scalar>
+struct expm1_retval {
   typedef Scalar type;
 };
 
 /****************************************************************************
-* Implementation of log                                                     *
-****************************************************************************/
+ * Implementation of log                                                     *
+ ****************************************************************************/
 
 // Complex log defined in MathFunctionsImpl.h.
-template<typename T> EIGEN_DEVICE_FUNC std::complex<T> complex_log(const std::complex<T>& z);
+template <typename T>
+EIGEN_DEVICE_FUNC std::complex<T> complex_log(const std::complex<T>& z);
 
-template<typename Scalar>
+template <typename Scalar>
 struct log_impl {
-  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x)
-  {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) {
     EIGEN_USING_STD(log);
     return static_cast<Scalar>(log(x));
   }
 };
 
-template<typename Scalar>
-struct log_impl<std::complex<Scalar> > {
-  EIGEN_DEVICE_FUNC static inline std::complex<Scalar> run(const std::complex<Scalar>& z)
-  {
-    return complex_log(z);
-  }
+template <typename Scalar>
+struct log_impl<std::complex<Scalar>> {
+  EIGEN_DEVICE_FUNC static inline std::complex<Scalar> run(const std::complex<Scalar>& z) { return complex_log(z); }
 };
 
 /****************************************************************************
-* Implementation of log1p                                                   *
-****************************************************************************/
+ * Implementation of log1p                                                   *
+ ****************************************************************************/
 
 namespace std_fallback {
-  // fallback log1p implementation in case there is no log1p(Scalar) function in namespace of Scalar,
-  // or that there is no suitable std::log1p function available
-  template<typename Scalar>
-  EIGEN_DEVICE_FUNC inline Scalar log1p(const Scalar& x) {
-    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    EIGEN_USING_STD(log);
-    Scalar x1p = RealScalar(1) + x;
-    Scalar log_1p = log_impl<Scalar>::run(x1p);
-    const bool is_small = numext::equal_strict(x1p, Scalar(1));
-    const bool is_inf = numext::equal_strict(x1p, log_1p);
-    return (is_small || is_inf) ? x : x * (log_1p / (x1p - RealScalar(1)));
-  }
+// fallback log1p implementation in case there is no log1p(Scalar) function in namespace of Scalar,
+// or that there is no suitable std::log1p function available
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline Scalar log1p(const Scalar& x) {
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  EIGEN_USING_STD(log);
+  Scalar x1p = RealScalar(1) + x;
+  Scalar log_1p = log_impl<Scalar>::run(x1p);
+  const bool is_small = numext::equal_strict(x1p, Scalar(1));
+  const bool is_inf = numext::equal_strict(x1p, log_1p);
+  return (is_small || is_inf) ? x : x * (log_1p / (x1p - RealScalar(1)));
 }
+}  // namespace std_fallback
 
-template<typename Scalar>
+template <typename Scalar>
 struct log1p_impl {
-  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x)
-  {
-    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
-    #if EIGEN_HAS_CXX11_MATH
-    using std::log1p;
-    #else
-    using std_fallback::log1p;
-    #endif
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
+
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) {
+    EIGEN_USING_STD(log1p);
     return log1p(x);
   }
 };
 
 // Specialization for complex types that are not supported by std::log1p.
 template <typename RealScalar>
-struct log1p_impl<std::complex<RealScalar> > {
-  EIGEN_DEVICE_FUNC static inline std::complex<RealScalar> run(
-      const std::complex<RealScalar>& x) {
-    EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar)
+struct log1p_impl<std::complex<RealScalar>> {
+  EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar)
+
+  EIGEN_DEVICE_FUNC static inline std::complex<RealScalar> run(const std::complex<RealScalar>& x) {
     return std_fallback::log1p(x);
   }
 };
 
-template<typename Scalar>
-struct log1p_retval
-{
+template <typename Scalar>
+struct log1p_retval {
   typedef Scalar type;
 };
 
 /****************************************************************************
-* Implementation of pow                                                  *
-****************************************************************************/
+ * Implementation of pow                                                  *
+ ****************************************************************************/
 
-template<typename ScalarX,typename ScalarY, bool IsInteger = NumTraits<ScalarX>::IsInteger&&NumTraits<ScalarY>::IsInteger>
-struct pow_impl
-{
-  //typedef Scalar retval;
-  typedef typename ScalarBinaryOpTraits<ScalarX,ScalarY,internal::scalar_pow_op<ScalarX,ScalarY> >::ReturnType result_type;
-  static EIGEN_DEVICE_FUNC inline result_type run(const ScalarX& x, const ScalarY& y)
-  {
+template <typename ScalarX, typename ScalarY,
+          bool IsInteger = NumTraits<ScalarX>::IsInteger && NumTraits<ScalarY>::IsInteger>
+struct pow_impl {
+  // typedef Scalar retval;
+  typedef typename ScalarBinaryOpTraits<ScalarX, ScalarY, internal::scalar_pow_op<ScalarX, ScalarY>>::ReturnType
+      result_type;
+  static EIGEN_DEVICE_FUNC inline result_type run(const ScalarX& x, const ScalarY& y) {
     EIGEN_USING_STD(pow);
     return pow(x, y);
   }
 };
 
-template<typename ScalarX,typename ScalarY>
-struct pow_impl<ScalarX,ScalarY, true>
-{
+template <typename ScalarX, typename ScalarY>
+struct pow_impl<ScalarX, ScalarY, true> {
   typedef ScalarX result_type;
-  static EIGEN_DEVICE_FUNC inline ScalarX run(ScalarX x, ScalarY y)
-  {
+  static EIGEN_DEVICE_FUNC inline ScalarX run(ScalarX x, ScalarY y) {
     ScalarX res(1);
     eigen_assert(!NumTraits<ScalarY>::IsSigned || y >= 0);
-    if(y & 1) res *= x;
+    if (y & 1) res *= x;
     y >>= 1;
-    while(y)
-    {
+    while (y) {
       x *= x;
-      if(y&1) res *= x;
+      if (y & 1) res *= x;
       y >>= 1;
     }
     return res;
@@ -801,99 +564,80 @@
 };
 
 /****************************************************************************
-* Implementation of random                                               *
-****************************************************************************/
+ * Implementation of random                                               *
+ ****************************************************************************/
 
-template<typename Scalar,
-         bool IsComplex,
-         bool IsInteger>
+template <typename Scalar, bool IsComplex, bool IsInteger>
 struct random_default_impl {};
 
-template<typename Scalar>
+template <typename Scalar>
 struct random_impl : random_default_impl<Scalar, NumTraits<Scalar>::IsComplex, NumTraits<Scalar>::IsInteger> {};
 
-template<typename Scalar>
-struct random_retval
-{
+template <typename Scalar>
+struct random_retval {
   typedef Scalar type;
 };
 
-template<typename Scalar> inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random(const Scalar& x, const Scalar& y);
-template<typename Scalar> inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random();
+template <typename Scalar>
+inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random(const Scalar& x, const Scalar& y);
+template <typename Scalar>
+inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random();
 
-template<typename Scalar>
-struct random_default_impl<Scalar, false, false>
-{
-  static inline Scalar run(const Scalar& x, const Scalar& y)
-  {
-    return x + (y-x) * Scalar(std::rand()) / Scalar(RAND_MAX);
+template <typename Scalar>
+struct random_default_impl<Scalar, false, false> {
+  static inline Scalar run(const Scalar& x, const Scalar& y) {
+    return x + (y - x) * Scalar(std::rand()) / Scalar(RAND_MAX);
   }
-  static inline Scalar run()
-  {
-    return run(Scalar(NumTraits<Scalar>::IsSigned ? -1 : 0), Scalar(1));
-  }
+  static inline Scalar run() { return run(Scalar(NumTraits<Scalar>::IsSigned ? -1 : 0), Scalar(1)); }
 };
 
-enum {
-  meta_floor_log2_terminate,
-  meta_floor_log2_move_up,
-  meta_floor_log2_move_down,
-  meta_floor_log2_bogus
-};
+enum { meta_floor_log2_terminate, meta_floor_log2_move_up, meta_floor_log2_move_down, meta_floor_log2_bogus };
 
-template<unsigned int n, int lower, int upper> struct meta_floor_log2_selector
-{
-  enum { middle = (lower + upper) / 2,
-         value = (upper <= lower + 1) ? int(meta_floor_log2_terminate)
-               : (n < (1 << middle)) ? int(meta_floor_log2_move_down)
-               : (n==0) ? int(meta_floor_log2_bogus)
-               : int(meta_floor_log2_move_up)
+template <unsigned int n, int lower, int upper>
+struct meta_floor_log2_selector {
+  enum {
+    middle = (lower + upper) / 2,
+    value = (upper <= lower + 1)  ? int(meta_floor_log2_terminate)
+            : (n < (1 << middle)) ? int(meta_floor_log2_move_down)
+            : (n == 0)            ? int(meta_floor_log2_bogus)
+                                  : int(meta_floor_log2_move_up)
   };
 };
 
-template<unsigned int n,
-         int lower = 0,
-         int upper = sizeof(unsigned int) * CHAR_BIT - 1,
-         int selector = meta_floor_log2_selector<n, lower, upper>::value>
+template <unsigned int n, int lower = 0, int upper = sizeof(unsigned int) * CHAR_BIT - 1,
+          int selector = meta_floor_log2_selector<n, lower, upper>::value>
 struct meta_floor_log2 {};
 
-template<unsigned int n, int lower, int upper>
-struct meta_floor_log2<n, lower, upper, meta_floor_log2_move_down>
-{
+template <unsigned int n, int lower, int upper>
+struct meta_floor_log2<n, lower, upper, meta_floor_log2_move_down> {
   enum { value = meta_floor_log2<n, lower, meta_floor_log2_selector<n, lower, upper>::middle>::value };
 };
 
-template<unsigned int n, int lower, int upper>
-struct meta_floor_log2<n, lower, upper, meta_floor_log2_move_up>
-{
+template <unsigned int n, int lower, int upper>
+struct meta_floor_log2<n, lower, upper, meta_floor_log2_move_up> {
   enum { value = meta_floor_log2<n, meta_floor_log2_selector<n, lower, upper>::middle, upper>::value };
 };
 
-template<unsigned int n, int lower, int upper>
-struct meta_floor_log2<n, lower, upper, meta_floor_log2_terminate>
-{
-  enum { value = (n >= ((unsigned int)(1) << (lower+1))) ? lower+1 : lower };
+template <unsigned int n, int lower, int upper>
+struct meta_floor_log2<n, lower, upper, meta_floor_log2_terminate> {
+  enum { value = (n >= ((unsigned int)(1) << (lower + 1))) ? lower + 1 : lower };
 };
 
-template<unsigned int n, int lower, int upper>
-struct meta_floor_log2<n, lower, upper, meta_floor_log2_bogus>
-{
+template <unsigned int n, int lower, int upper>
+struct meta_floor_log2<n, lower, upper, meta_floor_log2_bogus> {
   // no value, error at compile time
 };
 
-template<typename Scalar>
-struct random_default_impl<Scalar, false, true>
-{
-  static inline Scalar run(const Scalar& x, const Scalar& y)
-  {
-    if (y <= x)
-      return x;
+template <typename Scalar>
+struct random_default_impl<Scalar, false, true> {
+  static inline Scalar run(const Scalar& x, const Scalar& y) {
+    if (y <= x) return x;
     // ScalarU is the unsigned counterpart of Scalar, possibly Scalar itself.
     typedef typename make_unsigned<Scalar>::type ScalarU;
     // ScalarX is the widest of ScalarU and unsigned int.
     // We'll deal only with ScalarX and unsigned int below thus avoiding signed
     // types and arithmetic and signed overflows (which are undefined behavior).
-    typedef typename conditional<(ScalarU(-1) > unsigned(-1)), ScalarU, unsigned>::type ScalarX;
+    typedef std::conditional_t<(ScalarU(-1) > unsigned(-1)), ScalarU, unsigned> ScalarX;
     // The following difference doesn't overflow, provided our integer types are two's
     // complement and have the same number of padding bits in signed and unsigned variants.
     // This is the case in most modern implementations of C++.
@@ -902,8 +646,10 @@
     ScalarX divisor = 1;
     ScalarX multiplier = 1;
     const unsigned rand_max = RAND_MAX;
-    if (range <= rand_max) divisor = (rand_max + 1) / (range + 1);
-    else                   multiplier = 1 + range / (rand_max + 1);
+    if (range <= rand_max)
+      divisor = (rand_max + 1) / (range + 1);
+    else
+      multiplier = 1 + range / (rand_max + 1);
     // Rejection sampling.
     do {
       offset = (unsigned(std::rand()) * multiplier) / divisor;
@@ -911,211 +657,199 @@
     return Scalar(ScalarX(x) + offset);
   }
 
-  static inline Scalar run()
-  {
+  static inline Scalar run() {
 #ifdef EIGEN_MAKING_DOCS
     return run(Scalar(NumTraits<Scalar>::IsSigned ? -10 : 0), Scalar(10));
 #else
-    enum { rand_bits = meta_floor_log2<(unsigned int)(RAND_MAX)+1>::value,
-           scalar_bits = sizeof(Scalar) * CHAR_BIT,
-           shift = EIGEN_PLAIN_ENUM_MAX(0, int(rand_bits) - int(scalar_bits)),
-           offset = NumTraits<Scalar>::IsSigned ? (1 << (EIGEN_PLAIN_ENUM_MIN(rand_bits,scalar_bits)-1)) : 0
+    enum {
+      rand_bits = meta_floor_log2<(unsigned int)(RAND_MAX) + 1>::value,
+      scalar_bits = sizeof(Scalar) * CHAR_BIT,
+      shift = plain_enum_max(0, int(rand_bits) - int(scalar_bits)),
+      offset = NumTraits<Scalar>::IsSigned ? (1 << (plain_enum_min(rand_bits, scalar_bits) - 1)) : 0
     };
     return Scalar((std::rand() >> shift) - offset);
 #endif
   }
 };
 
-template<typename Scalar>
-struct random_default_impl<Scalar, true, false>
-{
-  static inline Scalar run(const Scalar& x, const Scalar& y)
-  {
-    return Scalar(random(x.real(), y.real()),
-                  random(x.imag(), y.imag()));
+template <typename Scalar>
+struct random_default_impl<Scalar, true, false> {
+  static inline Scalar run(const Scalar& x, const Scalar& y) {
+    return Scalar(random(x.real(), y.real()), random(x.imag(), y.imag()));
   }
-  static inline Scalar run()
-  {
+  static inline Scalar run() {
     typedef typename NumTraits<Scalar>::Real RealScalar;
     return Scalar(random<RealScalar>(), random<RealScalar>());
   }
 };
 
-template<typename Scalar>
-inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random(const Scalar& x, const Scalar& y)
-{
+template <typename Scalar>
+inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random(const Scalar& x, const Scalar& y) {
   return EIGEN_MATHFUNC_IMPL(random, Scalar)::run(x, y);
 }
 
-template<typename Scalar>
-inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random()
-{
+template <typename Scalar>
+inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random() {
   return EIGEN_MATHFUNC_IMPL(random, Scalar)::run();
 }
 
 // Implementation of is* functions
 
-// std::is* do not work with fast-math and gcc, std::is* are available on MSVC 2013 and newer, as well as in clang.
-#if (EIGEN_HAS_CXX11_MATH && !(EIGEN_COMP_GNUC_STRICT && __FINITE_MATH_ONLY__)) || (EIGEN_COMP_MSVC>=1800) || (EIGEN_COMP_CLANG)
-#define EIGEN_USE_STD_FPCLASSIFY 1
-#else
-#define EIGEN_USE_STD_FPCLASSIFY 0
-#endif
-
-template<typename T>
-EIGEN_DEVICE_FUNC
-typename internal::enable_if<internal::is_integral<T>::value,bool>::type
-isnan_impl(const T&) { return false; }
-
-template<typename T>
-EIGEN_DEVICE_FUNC
-typename internal::enable_if<internal::is_integral<T>::value,bool>::type
-isinf_impl(const T&) { return false; }
-
-template<typename T>
-EIGEN_DEVICE_FUNC
-typename internal::enable_if<internal::is_integral<T>::value,bool>::type
-isfinite_impl(const T&) { return true; }
-
-template<typename T>
-EIGEN_DEVICE_FUNC
-typename internal::enable_if<(!internal::is_integral<T>::value)&&(!NumTraits<T>::IsComplex),bool>::type
-isfinite_impl(const T& x)
-{
-  #if defined(EIGEN_GPU_COMPILE_PHASE)
-    return (::isfinite)(x);
-  #elif EIGEN_USE_STD_FPCLASSIFY
-    using std::isfinite;
-    return isfinite EIGEN_NOT_A_MACRO (x);
-  #else
-    return x<=NumTraits<T>::highest() && x>=NumTraits<T>::lowest();
-  #endif
+template <typename T>
+EIGEN_DEVICE_FUNC std::enable_if_t<!(std::numeric_limits<T>::has_infinity || std::numeric_limits<T>::has_quiet_NaN ||
+                                     std::numeric_limits<T>::has_signaling_NaN),
+                                   bool>
+isfinite_impl(const T&) {
+  return true;
 }
 
-template<typename T>
+template <typename T>
+EIGEN_DEVICE_FUNC std::enable_if_t<(std::numeric_limits<T>::has_infinity || std::numeric_limits<T>::has_quiet_NaN ||
+                                    std::numeric_limits<T>::has_signaling_NaN) &&
+                                       (!NumTraits<T>::IsComplex),
+                                   bool>
+isfinite_impl(const T& x) {
+  EIGEN_USING_STD(isfinite);
+  return isfinite EIGEN_NOT_A_MACRO(x);
+}
+
+template <typename T>
+EIGEN_DEVICE_FUNC std::enable_if_t<!std::numeric_limits<T>::has_infinity, bool> isinf_impl(const T&) {
+  return false;
+}
+
+template <typename T>
+EIGEN_DEVICE_FUNC std::enable_if_t<(std::numeric_limits<T>::has_infinity && !NumTraits<T>::IsComplex), bool> isinf_impl(
+    const T& x) {
+  EIGEN_USING_STD(isinf);
+  return isinf EIGEN_NOT_A_MACRO(x);
+}
+
+template <typename T>
 EIGEN_DEVICE_FUNC
-typename internal::enable_if<(!internal::is_integral<T>::value)&&(!NumTraits<T>::IsComplex),bool>::type
-isinf_impl(const T& x)
-{
-  #if defined(EIGEN_GPU_COMPILE_PHASE)
-    return (::isinf)(x);
-  #elif EIGEN_USE_STD_FPCLASSIFY
-    using std::isinf;
-    return isinf EIGEN_NOT_A_MACRO (x);
-  #else
-    return x>NumTraits<T>::highest() || x<NumTraits<T>::lowest();
-  #endif
+    std::enable_if_t<!(std::numeric_limits<T>::has_quiet_NaN || std::numeric_limits<T>::has_signaling_NaN), bool>
+    isnan_impl(const T&) {
+  return false;
 }
 
-template<typename T>
-EIGEN_DEVICE_FUNC
-typename internal::enable_if<(!internal::is_integral<T>::value)&&(!NumTraits<T>::IsComplex),bool>::type
-isnan_impl(const T& x)
-{
-  #if defined(EIGEN_GPU_COMPILE_PHASE)
-    return (::isnan)(x);
-  #elif EIGEN_USE_STD_FPCLASSIFY
-    using std::isnan;
-    return isnan EIGEN_NOT_A_MACRO (x);
-  #else
-    return x != x;
-  #endif
+template <typename T>
+EIGEN_DEVICE_FUNC std::enable_if_t<
+    (std::numeric_limits<T>::has_quiet_NaN || std::numeric_limits<T>::has_signaling_NaN) && (!NumTraits<T>::IsComplex),
+    bool>
+isnan_impl(const T& x) {
+  EIGEN_USING_STD(isnan);
+  return isnan EIGEN_NOT_A_MACRO(x);
 }
 
-#if (!EIGEN_USE_STD_FPCLASSIFY)
-
-#if EIGEN_COMP_MSVC
-
-template<typename T> EIGEN_DEVICE_FUNC bool isinf_msvc_helper(T x)
-{
-  return _fpclass(x)==_FPCLASS_NINF || _fpclass(x)==_FPCLASS_PINF;
-}
-
-//MSVC defines a _isnan builtin function, but for double only
-EIGEN_DEVICE_FUNC inline bool isnan_impl(const long double& x) { return _isnan(x)!=0; }
-EIGEN_DEVICE_FUNC inline bool isnan_impl(const double& x)      { return _isnan(x)!=0; }
-EIGEN_DEVICE_FUNC inline bool isnan_impl(const float& x)       { return _isnan(x)!=0; }
-
-EIGEN_DEVICE_FUNC inline bool isinf_impl(const long double& x) { return isinf_msvc_helper(x); }
-EIGEN_DEVICE_FUNC inline bool isinf_impl(const double& x)      { return isinf_msvc_helper(x); }
-EIGEN_DEVICE_FUNC inline bool isinf_impl(const float& x)       { return isinf_msvc_helper(x); }
-
-#elif (defined __FINITE_MATH_ONLY__ && __FINITE_MATH_ONLY__ && EIGEN_COMP_GNUC)
-
-#if EIGEN_GNUC_AT_LEAST(5,0)
-  #define EIGEN_TMP_NOOPT_ATTRIB EIGEN_DEVICE_FUNC inline __attribute__((optimize("no-finite-math-only")))
-#else
-  // NOTE the inline qualifier and noinline attribute are both needed: the former is to avoid linking issue (duplicate symbol),
-  //      while the second prevent too aggressive optimizations in fast-math mode:
-  #define EIGEN_TMP_NOOPT_ATTRIB EIGEN_DEVICE_FUNC inline __attribute__((noinline,optimize("no-finite-math-only")))
-#endif
-
-template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const long double& x) { return __builtin_isnan(x); }
-template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const double& x)      { return __builtin_isnan(x); }
-template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const float& x)       { return __builtin_isnan(x); }
-template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const double& x)      { return __builtin_isinf(x); }
-template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const float& x)       { return __builtin_isinf(x); }
-template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const long double& x) { return __builtin_isinf(x); }
-
-#undef EIGEN_TMP_NOOPT_ATTRIB
-
-#endif
-
-#endif
-
 // The following overload are defined at the end of this file
-template<typename T> EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex<T>& x);
-template<typename T> EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex<T>& x);
-template<typename T> EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex<T>& x);
-
-template<typename T> T generic_fast_tanh_float(const T& a_x);
-} // end namespace internal
+template <typename T>
+EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex<T>& x);
+template <typename T>
+EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex<T>& x);
+template <typename T>
+EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex<T>& x);
+template <typename T>
+T generic_fast_tanh_float(const T& a_x);
 
 /****************************************************************************
-* Generic math functions                                                    *
-****************************************************************************/
+ * Implementation of sign                                                 *
+ ****************************************************************************/
+template <typename Scalar, bool IsComplex = (NumTraits<Scalar>::IsComplex != 0),
+          bool IsInteger = (NumTraits<Scalar>::IsInteger != 0)>
+struct sign_impl {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& a) { return Scalar((a > Scalar(0)) - (a < Scalar(0))); }
+};
+
+template <typename Scalar>
+struct sign_impl<Scalar, false, false> {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& a) {
+    return (isnan_impl<Scalar>)(a) ? a : Scalar((a > Scalar(0)) - (a < Scalar(0)));
+  }
+};
+
+template <typename Scalar, bool IsInteger>
+struct sign_impl<Scalar, true, IsInteger> {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& a) {
+    using real_type = typename NumTraits<Scalar>::Real;
+    EIGEN_USING_STD(abs);
+    real_type aa = abs(a);
+    if (aa == real_type(0)) return Scalar(0);
+    aa = real_type(1) / aa;
+    return Scalar(a.real() * aa, a.imag() * aa);
+  }
+};
+
+// The sign function for bool is the identity.
+template <>
+struct sign_impl<bool, false, true> {
+  EIGEN_DEVICE_FUNC static inline bool run(const bool& a) { return a; }
+};
+
+template <typename Scalar>
+struct sign_retval {
+  typedef Scalar type;
+};
+
+template <typename Scalar, bool IsInteger = NumTraits<typename unpacket_traits<Scalar>::type>::IsInteger>
+struct nearest_integer_impl {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_floor(const Scalar& x) {
+    EIGEN_USING_STD(floor) return floor(x);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_ceil(const Scalar& x) {
+    EIGEN_USING_STD(ceil) return ceil(x);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_rint(const Scalar& x) {
+    EIGEN_USING_STD(rint) return rint(x);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_round(const Scalar& x) {
+    EIGEN_USING_STD(round) return round(x);
+  }
+};
+template <typename Scalar>
+struct nearest_integer_impl<Scalar, true> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_floor(const Scalar& x) { return x; }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_ceil(const Scalar& x) { return x; }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_rint(const Scalar& x) { return x; }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_round(const Scalar& x) { return x; }
+};
+
+}  // end namespace internal
+
+/****************************************************************************
+ * Generic math functions                                                    *
+ ****************************************************************************/
 
 namespace numext {
 
 #if (!defined(EIGEN_GPUCC) || defined(EIGEN_CONSTEXPR_ARE_DEVICE_FUNC))
-template<typename T>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) {
   EIGEN_USING_STD(min)
-  return min EIGEN_NOT_A_MACRO (x,y);
+  return min EIGEN_NOT_A_MACRO(x, y);
 }
 
-template<typename T>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y) {
   EIGEN_USING_STD(max)
-  return max EIGEN_NOT_A_MACRO (x,y);
+  return max EIGEN_NOT_A_MACRO(x, y);
 }
 #else
-template<typename T>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) {
   return y < x ? y : x;
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE float mini(const float& x, const float& y)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float mini(const float& x, const float& y) {
   return fminf(x, y);
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE double mini(const double& x, const double& y)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double mini(const double& x, const double& y) {
   return fmin(x, y);
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE long double mini(const long double& x, const long double& y)
-{
+
+#ifndef EIGEN_GPU_COMPILE_PHASE
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE long double mini(const long double& x, const long double& y) {
 #if defined(EIGEN_HIPCC)
   // no "fminl" on HIP yet
   return (x < y) ? x : y;
@@ -1123,29 +857,23 @@
   return fminl(x, y);
 #endif
 }
+#endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y) {
   return x < y ? y : x;
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE float maxi(const float& x, const float& y)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float maxi(const float& x, const float& y) {
   return fmaxf(x, y);
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE double maxi(const double& x, const double& y)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double maxi(const double& x, const double& y) {
   return fmax(x, y);
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE long double maxi(const long double& x, const long double& y)
-{
+#ifndef EIGEN_GPU_COMPILE_PHASE
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE long double maxi(const long double& x, const long double& y) {
 #if defined(EIGEN_HIPCC)
   // no "fmaxl" on HIP yet
   return (x > y) ? x : y;
@@ -1154,68 +882,64 @@
 #endif
 }
 #endif
+#endif
 
 #if defined(SYCL_DEVICE_ONLY)
 
-
 #define SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \
-  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_char)   \
-  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_short)  \
-  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_int)    \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_char)    \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_short)   \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_int)     \
   SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_long)
 #define SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \
-  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_char)   \
-  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_short)  \
-  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_int)    \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_char)    \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_short)   \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_int)     \
   SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_long)
 #define SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \
-  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar)  \
-  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort) \
-  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uint)   \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar)     \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort)    \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uint)      \
   SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_ulong)
 #define SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \
-  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar)  \
-  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort) \
-  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uint)   \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar)     \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort)    \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uint)      \
   SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_ulong)
-#define SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(NAME, FUNC) \
+#define SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(NAME, FUNC)  \
   SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \
   SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY(NAME, FUNC)
-#define SYCL_SPECIALIZE_INTEGER_TYPES_UNARY(NAME, FUNC) \
+#define SYCL_SPECIALIZE_INTEGER_TYPES_UNARY(NAME, FUNC)  \
   SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \
   SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY(NAME, FUNC)
-#define SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(NAME, FUNC) \
+#define SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(NAME, FUNC)     \
   SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_float) \
-  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC,cl::sycl::cl_double)
-#define SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_double)
+#define SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(NAME, FUNC)     \
   SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_float) \
-  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC,cl::sycl::cl_double)
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_double)
 #define SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(NAME, FUNC, RET_TYPE) \
-  SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, cl::sycl::cl_float) \
+  SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, cl::sycl::cl_float)       \
   SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, cl::sycl::cl_double)
 
-#define SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE) \
-template<>                                               \
-  EIGEN_DEVICE_FUNC                                      \
-  EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE& x) { \
-    return cl::sycl::FUNC(x);                            \
+#define SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE)     \
+  template <>                                                              \
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE& x) { \
+    return cl::sycl::FUNC(x);                                              \
   }
 
-#define SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, TYPE) \
-  SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, TYPE, TYPE)
+#define SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, TYPE) SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, TYPE, TYPE)
 
-#define SYCL_SPECIALIZE_GEN1_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE1, ARG_TYPE2) \
-  template<>                                                                  \
-  EIGEN_DEVICE_FUNC                                                           \
-  EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE1& x, const ARG_TYPE2& y) { \
-    return cl::sycl::FUNC(x, y);                                              \
+#define SYCL_SPECIALIZE_GEN1_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE1, ARG_TYPE2)            \
+  template <>                                                                                   \
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE1& x, const ARG_TYPE2& y) { \
+    return cl::sycl::FUNC(x, y);                                                                \
   }
 
 #define SYCL_SPECIALIZE_GEN2_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE) \
   SYCL_SPECIALIZE_GEN1_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE, ARG_TYPE)
 
-#define SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, TYPE) \
-  SYCL_SPECIALIZE_GEN2_BINARY_FUNC(NAME, FUNC, TYPE, TYPE)
+#define SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, TYPE) SYCL_SPECIALIZE_GEN2_BINARY_FUNC(NAME, FUNC, TYPE, TYPE)
 
 SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(mini, min)
 SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(mini, fmin)
@@ -1224,123 +948,97 @@
 
 #endif
 
-
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(real, Scalar)::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) >::type real_ref(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline internal::add_const_on_value_type_t<EIGEN_MATHFUNC_RETVAL(real_ref, Scalar)> real_ref(
+    const Scalar& x) {
   return internal::real_ref_impl<Scalar>::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(real_ref, Scalar)::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(arg, Scalar) arg(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(arg, Scalar) arg(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(arg, Scalar)::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) >::type imag_ref(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline internal::add_const_on_value_type_t<EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar)> imag_ref(
+    const Scalar& x) {
   return internal::imag_ref_impl<Scalar>::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) imag_ref(Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) imag_ref(Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(imag_ref, Scalar)::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(conj, Scalar) conj(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(conj, Scalar) conj(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(conj, Scalar)::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(sign, Scalar) sign(const Scalar& x) {
+  return EIGEN_MATHFUNC_IMPL(sign, Scalar)::run(x);
+}
+
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x);
 }
 
-EIGEN_DEVICE_FUNC
-inline bool abs2(bool x) { return x; }
+EIGEN_DEVICE_FUNC inline bool abs2(bool x) { return x; }
 
-template<typename T>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE T absdiff(const T& x, const T& y)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T absdiff(const T& x, const T& y) {
   return x > y ? x - y : y - x;
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE float absdiff(const float& x, const float& y)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float absdiff(const float& x, const float& y) {
   return fabsf(x - y);
 }
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE double absdiff(const double& x, const double& y)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double absdiff(const double& x, const double& y) {
   return fabs(x - y);
 }
 
-#if !defined(EIGEN_GPUCC)
 // HIP and CUDA do not support long double.
-template<>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE long double absdiff(const long double& x, const long double& y) {
+#ifndef EIGEN_GPU_COMPILE_PHASE
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE long double absdiff(const long double& x, const long double& y) {
   return fabsl(x - y);
 }
 #endif
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(norm1, Scalar)::run(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(hypot, Scalar) hypot(const Scalar& x, const Scalar& y)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(hypot, Scalar) hypot(const Scalar& x, const Scalar& y) {
   return EIGEN_MATHFUNC_IMPL(hypot, Scalar)::run(x, y);
 }
 
 #if defined(SYCL_DEVICE_ONLY)
-  SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(hypot, hypot)
+SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(hypot, hypot)
 #endif
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(log1p, Scalar) log1p(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(log1p, Scalar) log1p(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(log1p, Scalar)::run(x);
 }
 
@@ -1349,27 +1047,39 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float log1p(const float &x) { return ::log1pf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float log1p(const float& x) {
+  return ::log1pf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double log1p(const double &x) { return ::log1p(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double log1p(const double& x) {
+  return ::log1p(x);
+}
 #endif
 
-template<typename ScalarX,typename ScalarY>
-EIGEN_DEVICE_FUNC
-inline typename internal::pow_impl<ScalarX,ScalarY>::result_type pow(const ScalarX& x, const ScalarY& y)
-{
-  return internal::pow_impl<ScalarX,ScalarY>::run(x, y);
+template <typename ScalarX, typename ScalarY>
+EIGEN_DEVICE_FUNC inline typename internal::pow_impl<ScalarX, ScalarY>::result_type pow(const ScalarX& x,
+                                                                                        const ScalarY& y) {
+  return internal::pow_impl<ScalarX, ScalarY>::run(x, y);
 }
 
 #if defined(SYCL_DEVICE_ONLY)
 SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(pow, pow)
 #endif
 
-template<typename T> EIGEN_DEVICE_FUNC bool (isnan)   (const T &x) { return internal::isnan_impl(x); }
-template<typename T> EIGEN_DEVICE_FUNC bool (isinf)   (const T &x) { return internal::isinf_impl(x); }
-template<typename T> EIGEN_DEVICE_FUNC bool (isfinite)(const T &x) { return internal::isfinite_impl(x); }
+template <typename T>
+EIGEN_DEVICE_FUNC bool(isnan)(const T& x) {
+  return internal::isnan_impl(x);
+}
+template <typename T>
+EIGEN_DEVICE_FUNC bool(isinf)(const T& x) {
+  return internal::isinf_impl(x);
+}
+template <typename T>
+EIGEN_DEVICE_FUNC bool(isfinite)(const T& x) {
+  return internal::isfinite_impl(x);
+}
 
 #if defined(SYCL_DEVICE_ONLY)
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isnan, isnan, bool)
@@ -1377,30 +1087,23 @@
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isfinite, isfinite, bool)
 #endif
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(rint, Scalar) rint(const Scalar& x)
-{
-  return EIGEN_MATHFUNC_IMPL(rint, Scalar)::run(x);
+template <typename Scalar>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar rint(const Scalar& x) {
+  return internal::nearest_integer_impl<Scalar>::run_rint(x);
 }
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(round, Scalar) round(const Scalar& x)
-{
-  return EIGEN_MATHFUNC_IMPL(round, Scalar)::run(x);
+template <typename Scalar>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar round(const Scalar& x) {
+  return internal::nearest_integer_impl<Scalar>::run_round(x);
 }
 
 #if defined(SYCL_DEVICE_ONLY)
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(round, round)
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC
-T (floor)(const T& x)
-{
-  EIGEN_USING_STD(floor)
-  return floor(x);
+template <typename Scalar>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar(floor)(const Scalar& x) {
+  return internal::nearest_integer_impl<Scalar>::run_floor(x);
 }
 
 #if defined(SYCL_DEVICE_ONLY)
@@ -1408,19 +1111,20 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float floor(const float &x) { return ::floorf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float floor(const float& x) {
+  return ::floorf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double floor(const double &x) { return ::floor(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double floor(const double& x) {
+  return ::floor(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC
-T (ceil)(const T& x)
-{
-  EIGEN_USING_STD(ceil);
-  return ceil(x);
+template <typename Scalar>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar(ceil)(const Scalar& x) {
+  return internal::nearest_integer_impl<Scalar>::run_ceil(x);
 }
 
 #if defined(SYCL_DEVICE_ONLY)
@@ -1428,21 +1132,35 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float ceil(const float &x) { return ::ceilf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float ceil(const float& x) {
+  return ::ceilf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double ceil(const double &x) { return ::ceil(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double ceil(const double& x) {
+  return ::ceil(x);
+}
 #endif
 
+// Integer division with rounding up.
+// T is assumed to be an integer type with a>=0, and b>0
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_CONSTEXPR T div_ceil(T a, T b) {
+  EIGEN_STATIC_ASSERT((NumTraits<T>::IsInteger), THIS FUNCTION IS FOR INTEGER TYPES)
+  eigen_assert(a >= 0);
+  eigen_assert(b > 0);
+  // Note: This form is used because it cannot overflow.
+  return a == 0 ? 0 : (a - 1) / b + 1;
+}
 
 /** Log base 2 for 32 bits positive integers.
-  * Conveniently returns 0 for x==0. */
-inline int log2(int x)
-{
-  eigen_assert(x>=0);
+ * Conveniently returns 0 for x==0. */
+inline int log2(int x) {
+  eigen_assert(x >= 0);
   unsigned int v(x);
-  static const int table[32] = { 0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30, 8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31 };
+  static const int table[32] = {0, 9,  1,  10, 13, 21, 2,  29, 11, 14, 16, 18, 22, 25, 3, 30,
+                                8, 12, 20, 28, 15, 17, 24, 7,  19, 27, 23, 6,  26, 5,  4, 31};
   v |= v >> 1;
   v |= v >> 2;
   v |= v >> 4;
@@ -1452,41 +1170,44 @@
 }
 
 /** \returns the square root of \a x.
-  *
-  * It is essentially equivalent to
-  * \code using std::sqrt; return sqrt(x); \endcode
-  * but slightly faster for float/double and some compilers (e.g., gcc), thanks to
-  * specializations when SSE is enabled.
-  *
-  * It's usage is justified in performance critical functions, like norm/normalize.
-  */
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-EIGEN_ALWAYS_INLINE EIGEN_MATHFUNC_RETVAL(sqrt, Scalar) sqrt(const Scalar& x)
-{
+ *
+ * It is essentially equivalent to
+ * \code using std::sqrt; return sqrt(x); \endcode
+ * but slightly faster for float/double and some compilers (e.g., gcc), thanks to
+ * specializations when SSE is enabled.
+ *
+ * It's usage is justified in performance critical functions, like norm/normalize.
+ */
+template <typename Scalar>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_MATHFUNC_RETVAL(sqrt, Scalar) sqrt(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(sqrt, Scalar)::run(x);
 }
 
 // Boolean specialization, avoids implicit float to bool conversion (-Wimplicit-conversion-floating-point-to-bool).
-template<>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_DEVICE_FUNC
-bool sqrt<bool>(const bool &x) { return x; }
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_DEVICE_FUNC bool sqrt<bool>(const bool& x) {
+  return x;
+}
 
 #if defined(SYCL_DEVICE_ONLY)
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sqrt, sqrt)
 #endif
 
+/** \returns the cube root of \a x. **/
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cbrt(const T& x) {
+  EIGEN_USING_STD(cbrt);
+  return static_cast<T>(cbrt(x));
+}
+
 /** \returns the reciprocal square root of \a x. **/
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T rsqrt(const T& x)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T rsqrt(const T& x) {
   return internal::rsqrt_impl<T>::run(x);
 }
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T log(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T log(const T& x) {
   return internal::log_impl<T>::run(x);
 }
 
@@ -1494,27 +1215,30 @@
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(log, log)
 #endif
 
-
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float log(const float &x) { return ::logf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float log(const float& x) {
+  return ::logf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double log(const double &x) { return ::log(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double log(const double& x) {
+  return ::log(x);
+}
 #endif
 
-template<typename T>
+template <typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-typename internal::enable_if<NumTraits<T>::IsSigned || NumTraits<T>::IsComplex,typename NumTraits<T>::Real>::type
-abs(const T &x) {
+    std::enable_if_t<NumTraits<T>::IsSigned || NumTraits<T>::IsComplex, typename NumTraits<T>::Real>
+    abs(const T& x) {
   EIGEN_USING_STD(abs);
   return abs(x);
 }
 
-template<typename T>
+template <typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-typename internal::enable_if<!(NumTraits<T>::IsSigned || NumTraits<T>::IsComplex),typename NumTraits<T>::Real>::type
-abs(const T &x) {
+    std::enable_if_t<!(NumTraits<T>::IsSigned || NumTraits<T>::IsComplex), typename NumTraits<T>::Real>
+    abs(const T& x) {
   return x;
 }
 
@@ -1524,26 +1248,58 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float abs(const float &x) { return ::fabsf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float abs(const float& x) {
+  return ::fabsf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double abs(const double &x) { return ::fabs(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double abs(const double& x) {
+  return ::fabs(x);
+}
 
-template <> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float abs(const std::complex<float>& x) {
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float abs(const std::complex<float>& x) {
   return ::hypotf(x.real(), x.imag());
 }
 
-template <> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double abs(const std::complex<double>& x) {
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double abs(const std::complex<double>& x) {
   return ::hypot(x.real(), x.imag());
 }
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T exp(const T &x) {
+template <typename Scalar, bool IsInteger = NumTraits<Scalar>::IsInteger, bool IsSigned = NumTraits<Scalar>::IsSigned>
+struct signbit_impl;
+template <typename Scalar>
+struct signbit_impl<Scalar, false, true> {
+  static constexpr size_t Size = sizeof(Scalar);
+  static constexpr size_t Shift = (CHAR_BIT * Size) - 1;
+  using intSize_t = typename get_integer_by_size<Size>::signed_type;
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static Scalar run(const Scalar& x) {
+    intSize_t a = bit_cast<intSize_t, Scalar>(x);
+    a = a >> Shift;
+    Scalar result = bit_cast<Scalar, intSize_t>(a);
+    return result;
+  }
+};
+template <typename Scalar>
+struct signbit_impl<Scalar, true, true> {
+  static constexpr size_t Size = sizeof(Scalar);
+  static constexpr size_t Shift = (CHAR_BIT * Size) - 1;
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Scalar run(const Scalar& x) { return x >> Shift; }
+};
+template <typename Scalar>
+struct signbit_impl<Scalar, true, false> {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Scalar run(const Scalar&) { return Scalar(0); }
+};
+template <typename Scalar>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Scalar signbit(const Scalar& x) {
+  return signbit_impl<Scalar>::run(x);
+}
+
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T exp(const T& x) {
   EIGEN_USING_STD(exp);
   return exp(x);
 }
@@ -1553,22 +1309,26 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float exp(const float &x) { return ::expf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float exp(const float& x) {
+  return ::expf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double exp(const double &x) { return ::exp(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double exp(const double& x) {
+  return ::exp(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-std::complex<float> exp(const std::complex<float>& x) {
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE std::complex<float> exp(const std::complex<float>& x) {
   float com = ::expf(x.real());
   float res_real = com * ::cosf(x.imag());
   float res_imag = com * ::sinf(x.imag());
   return std::complex<float>(res_real, res_imag);
 }
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-std::complex<double> exp(const std::complex<double>& x) {
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE std::complex<double> exp(const std::complex<double>& x) {
   double com = ::exp(x.real());
   double res_real = com * ::cos(x.imag());
   double res_imag = com * ::sin(x.imag());
@@ -1576,10 +1336,8 @@
 }
 #endif
 
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-inline EIGEN_MATHFUNC_RETVAL(expm1, Scalar) expm1(const Scalar& x)
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(expm1, Scalar) expm1(const Scalar& x) {
   return EIGEN_MATHFUNC_IMPL(expm1, Scalar)::run(x);
 }
 
@@ -1588,35 +1346,41 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float expm1(const float &x) { return ::expm1f(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float expm1(const float& x) {
+  return ::expm1f(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double expm1(const double &x) { return ::expm1(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double expm1(const double& x) {
+  return ::expm1(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T cos(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cos(const T& x) {
   EIGEN_USING_STD(cos);
   return cos(x);
 }
 
 #if defined(SYCL_DEVICE_ONLY)
-SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(cos,cos)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(cos, cos)
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float cos(const float &x) { return ::cosf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float cos(const float& x) {
+  return ::cosf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double cos(const double &x) { return ::cos(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double cos(const double& x) {
+  return ::cos(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T sin(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T sin(const T& x) {
   EIGEN_USING_STD(sin);
   return sin(x);
 }
@@ -1626,16 +1390,19 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float sin(const float &x) { return ::sinf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float sin(const float& x) {
+  return ::sinf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double sin(const double &x) { return ::sin(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double sin(const double& x) {
+  return ::sin(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T tan(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T tan(const T& x) {
   EIGEN_USING_STD(tan);
   return tan(x);
 }
@@ -1645,28 +1412,28 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float tan(const float &x) { return ::tanf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float tan(const float& x) {
+  return ::tanf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double tan(const double &x) { return ::tan(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double tan(const double& x) {
+  return ::tan(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T acos(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T acos(const T& x) {
   EIGEN_USING_STD(acos);
   return acos(x);
 }
 
-#if EIGEN_HAS_CXX11_MATH
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T acosh(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T acosh(const T& x) {
   EIGEN_USING_STD(acosh);
   return static_cast<T>(acosh(x));
 }
-#endif
 
 #if defined(SYCL_DEVICE_ONLY)
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(acos, acos)
@@ -1674,28 +1441,28 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float acos(const float &x) { return ::acosf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float acos(const float& x) {
+  return ::acosf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double acos(const double &x) { return ::acos(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double acos(const double& x) {
+  return ::acos(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T asin(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T asin(const T& x) {
   EIGEN_USING_STD(asin);
   return asin(x);
 }
 
-#if EIGEN_HAS_CXX11_MATH
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T asinh(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T asinh(const T& x) {
   EIGEN_USING_STD(asinh);
   return static_cast<T>(asinh(x));
 }
-#endif
 
 #if defined(SYCL_DEVICE_ONLY)
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(asin, asin)
@@ -1703,28 +1470,34 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float asin(const float &x) { return ::asinf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float asin(const float& x) {
+  return ::asinf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double asin(const double &x) { return ::asin(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double asin(const double& x) {
+  return ::asin(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T atan(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T atan(const T& x) {
   EIGEN_USING_STD(atan);
   return static_cast<T>(atan(x));
 }
 
-#if EIGEN_HAS_CXX11_MATH
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T atanh(const T &x) {
+template <typename T, std::enable_if_t<!NumTraits<T>::IsComplex, int> = 0>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T atan2(const T& y, const T& x) {
+  EIGEN_USING_STD(atan2);
+  return static_cast<T>(atan2(y, x));
+}
+
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T atanh(const T& x) {
   EIGEN_USING_STD(atanh);
   return static_cast<T>(atanh(x));
 }
-#endif
 
 #if defined(SYCL_DEVICE_ONLY)
 SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(atan, atan)
@@ -1732,17 +1505,19 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float atan(const float &x) { return ::atanf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float atan(const float& x) {
+  return ::atanf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double atan(const double &x) { return ::atan(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double atan(const double& x) {
+  return ::atan(x);
+}
 #endif
 
-
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T cosh(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cosh(const T& x) {
   EIGEN_USING_STD(cosh);
   return static_cast<T>(cosh(x));
 }
@@ -1752,16 +1527,19 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float cosh(const float &x) { return ::coshf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float cosh(const float& x) {
+  return ::coshf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double cosh(const double &x) { return ::cosh(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double cosh(const double& x) {
+  return ::cosh(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T sinh(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T sinh(const T& x) {
   EIGEN_USING_STD(sinh);
   return static_cast<T>(sinh(x));
 }
@@ -1771,23 +1549,25 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float sinh(const float &x) { return ::sinhf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float sinh(const float& x) {
+  return ::sinhf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double sinh(const double &x) { return ::sinh(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double sinh(const double& x) {
+  return ::sinh(x);
+}
 #endif
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T tanh(const T &x) {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T tanh(const T& x) {
   EIGEN_USING_STD(tanh);
   return tanh(x);
 }
 
 #if (!defined(EIGEN_GPUCC)) && EIGEN_FAST_MATH && !defined(SYCL_DEVICE_ONLY)
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float tanh(float x) { return internal::generic_fast_tanh_float(x); }
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float tanh(float x) { return internal::generic_fast_tanh_float(x); }
 #endif
 
 #if defined(SYCL_DEVICE_ONLY)
@@ -1795,16 +1575,19 @@
 #endif
 
 #if defined(EIGEN_GPUCC)
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float tanh(const float &x) { return ::tanhf(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float tanh(const float& x) {
+  return ::tanhf(x);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double tanh(const double &x) { return ::tanh(x); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double tanh(const double& x) {
+  return ::tanh(x);
+}
 #endif
 
 template <typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T fmod(const T& a, const T& b) {
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T fmod(const T& a, const T& b) {
   EIGEN_USING_STD(fmod);
   return fmod(a, b);
 }
@@ -1815,14 +1598,12 @@
 
 #if defined(EIGEN_GPUCC)
 template <>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float fmod(const float& a, const float& b) {
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float fmod(const float& a, const float& b) {
   return ::fmodf(a, b);
 }
 
 template <>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double fmod(const double& a, const double& b) {
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double fmod(const double& a, const double& b) {
   return ::fmod(a, b);
 }
 #endif
@@ -1844,116 +1625,96 @@
 #undef SYCL_SPECIALIZE_BINARY_FUNC
 #endif
 
-} // end namespace numext
+}  // end namespace numext
 
 namespace internal {
 
-template<typename T>
-EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex<T>& x)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex<T>& x) {
   return (numext::isfinite)(numext::real(x)) && (numext::isfinite)(numext::imag(x));
 }
 
-template<typename T>
-EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex<T>& x)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex<T>& x) {
   return (numext::isnan)(numext::real(x)) || (numext::isnan)(numext::imag(x));
 }
 
-template<typename T>
-EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex<T>& x)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex<T>& x) {
   return ((numext::isinf)(numext::real(x)) || (numext::isinf)(numext::imag(x))) && (!(numext::isnan)(x));
 }
 
 /****************************************************************************
-* Implementation of fuzzy comparisons                                       *
-****************************************************************************/
+ * Implementation of fuzzy comparisons                                       *
+ ****************************************************************************/
 
-template<typename Scalar,
-         bool IsComplex,
-         bool IsInteger>
+template <typename Scalar, bool IsComplex, bool IsInteger>
 struct scalar_fuzzy_default_impl {};
 
-template<typename Scalar>
-struct scalar_fuzzy_default_impl<Scalar, false, false>
-{
+template <typename Scalar>
+struct scalar_fuzzy_default_impl<Scalar, false, false> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  template<typename OtherScalar> EIGEN_DEVICE_FUNC
-  static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, const RealScalar& prec)
-  {
+  template <typename OtherScalar>
+  EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y,
+                                                         const RealScalar& prec) {
     return numext::abs(x) <= numext::abs(y) * prec;
   }
-  EIGEN_DEVICE_FUNC
-  static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec)
-  {
+  EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) {
     return numext::abs(x - y) <= numext::mini(numext::abs(x), numext::abs(y)) * prec;
   }
-  EIGEN_DEVICE_FUNC
-  static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar& prec)
-  {
+  EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar& prec) {
     return x <= y || isApprox(x, y, prec);
   }
 };
 
-template<typename Scalar>
-struct scalar_fuzzy_default_impl<Scalar, false, true>
-{
+template <typename Scalar>
+struct scalar_fuzzy_default_impl<Scalar, false, true> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  template<typename OtherScalar> EIGEN_DEVICE_FUNC
-  static inline bool isMuchSmallerThan(const Scalar& x, const Scalar&, const RealScalar&)
-  {
+  template <typename OtherScalar>
+  EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const Scalar&, const RealScalar&) {
     return x == Scalar(0);
   }
-  EIGEN_DEVICE_FUNC
-  static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar&)
-  {
-    return x == y;
-  }
-  EIGEN_DEVICE_FUNC
-  static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar&)
-  {
+  EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar&) { return x == y; }
+  EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar&) {
     return x <= y;
   }
 };
 
-template<typename Scalar>
-struct scalar_fuzzy_default_impl<Scalar, true, false>
-{
+template <typename Scalar>
+struct scalar_fuzzy_default_impl<Scalar, true, false> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  template<typename OtherScalar> EIGEN_DEVICE_FUNC
-  static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, const RealScalar& prec)
-  {
+  template <typename OtherScalar>
+  EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y,
+                                                         const RealScalar& prec) {
     return numext::abs2(x) <= numext::abs2(y) * prec * prec;
   }
-  EIGEN_DEVICE_FUNC
-  static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec)
-  {
+  EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) {
     return numext::abs2(x - y) <= numext::mini(numext::abs2(x), numext::abs2(y)) * prec * prec;
   }
 };
 
-template<typename Scalar>
-struct scalar_fuzzy_impl : scalar_fuzzy_default_impl<Scalar, NumTraits<Scalar>::IsComplex, NumTraits<Scalar>::IsInteger> {};
+template <typename Scalar>
+struct scalar_fuzzy_impl
+    : scalar_fuzzy_default_impl<Scalar, NumTraits<Scalar>::IsComplex, NumTraits<Scalar>::IsInteger> {};
 
-template<typename Scalar, typename OtherScalar> EIGEN_DEVICE_FUNC
-inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y,
-                              const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
-{
+template <typename Scalar, typename OtherScalar>
+EIGEN_DEVICE_FUNC inline bool isMuchSmallerThan(
+    const Scalar& x, const OtherScalar& y,
+    const typename NumTraits<Scalar>::Real& precision = NumTraits<Scalar>::dummy_precision()) {
   return scalar_fuzzy_impl<Scalar>::template isMuchSmallerThan<OtherScalar>(x, y, precision);
 }
 
-template<typename Scalar> EIGEN_DEVICE_FUNC
-inline bool isApprox(const Scalar& x, const Scalar& y,
-                     const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline bool isApprox(
+    const Scalar& x, const Scalar& y,
+    const typename NumTraits<Scalar>::Real& precision = NumTraits<Scalar>::dummy_precision()) {
   return scalar_fuzzy_impl<Scalar>::isApprox(x, y, precision);
 }
 
-template<typename Scalar> EIGEN_DEVICE_FUNC
-inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y,
-                               const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
-{
+template <typename Scalar>
+EIGEN_DEVICE_FUNC inline bool isApproxOrLessThan(
+    const Scalar& x, const Scalar& y,
+    const typename NumTraits<Scalar>::Real& precision = NumTraits<Scalar>::dummy_precision()) {
   return scalar_fuzzy_impl<Scalar>::isApproxOrLessThan(x, y, precision);
 }
 
@@ -1961,54 +1722,40 @@
 ***  The special case of the  bool type ***
 ******************************************/
 
-template<> struct random_impl<bool>
-{
-  static inline bool run()
-  {
-    return random<int>(0,1)==0 ? false : true;
-  }
+template <>
+struct random_impl<bool> {
+  static inline bool run() { return random<int>(0, 1) == 0 ? false : true; }
 
-  static inline bool run(const bool& a, const bool& b)
-  {
-    return random<int>(a, b)==0 ? false : true;
-  }
+  static inline bool run(const bool& a, const bool& b) { return random<int>(a, b) == 0 ? false : true; }
 };
 
-template<> struct scalar_fuzzy_impl<bool>
-{
+template <>
+struct scalar_fuzzy_impl<bool> {
   typedef bool RealScalar;
 
-  template<typename OtherScalar> EIGEN_DEVICE_FUNC
-  static inline bool isMuchSmallerThan(const bool& x, const bool&, const bool&)
-  {
+  template <typename OtherScalar>
+  EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const bool& x, const bool&, const bool&) {
     return !x;
   }
 
-  EIGEN_DEVICE_FUNC
-  static inline bool isApprox(bool x, bool y, bool)
-  {
-    return x == y;
-  }
+  EIGEN_DEVICE_FUNC static inline bool isApprox(bool x, bool y, bool) { return x == y; }
 
-  EIGEN_DEVICE_FUNC
-  static inline bool isApproxOrLessThan(const bool& x, const bool& y, const bool&)
-  {
+  EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const bool& x, const bool& y, const bool&) {
     return (!x) || y;
   }
-
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 // Default implementations that rely on other numext implementations
 namespace internal {
 
 // Specialization for complex types that are not supported by std::expm1.
 template <typename RealScalar>
-struct expm1_impl<std::complex<RealScalar> > {
-  EIGEN_DEVICE_FUNC static inline std::complex<RealScalar> run(
-      const std::complex<RealScalar>& x) {
-    EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar)
+struct expm1_impl<std::complex<RealScalar>> {
+  EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar)
+
+  EIGEN_DEVICE_FUNC static inline std::complex<RealScalar> run(const std::complex<RealScalar>& x) {
     RealScalar xr = x.real();
     RealScalar xi = x.imag();
     // expm1(z) = exp(z) - 1
@@ -2030,28 +1777,22 @@
   }
 };
 
-template<typename T>
+template <typename T>
 struct rsqrt_impl {
-  EIGEN_DEVICE_FUNC
-  static EIGEN_ALWAYS_INLINE T run(const T& x) {
-    return T(1)/numext::sqrt(x);
-  }
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE T run(const T& x) { return T(1) / numext::sqrt(x); }
 };
 
 #if defined(EIGEN_GPU_COMPILE_PHASE)
-template<typename T>
-struct conj_impl<std::complex<T>, true>
-{
-  EIGEN_DEVICE_FUNC
-  static inline std::complex<T> run(const std::complex<T>& x)
-  {
+template <typename T>
+struct conj_impl<std::complex<T>, true> {
+  EIGEN_DEVICE_FUNC static inline std::complex<T> run(const std::complex<T>& x) {
     return std::complex<T>(numext::real(x), -numext::imag(x));
   }
 };
 #endif
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATHFUNCTIONS_H
+#endif  // EIGEN_MATHFUNCTIONS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
index 4eaaaa7..ed44089 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
@@ -11,23 +11,153 @@
 #ifndef EIGEN_MATHFUNCTIONSIMPL_H
 #define EIGEN_MATHFUNCTIONSIMPL_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
+/** \internal Fast reciprocal using Newton-Raphson's method.
+
+ Preconditions:
+   1. The starting guess provided in approx_a_recip must have at least half
+      the leading mantissa bits in the correct result, such that a single
+      Newton-Raphson step is sufficient to get within 1-2 ulps of the currect
+      result.
+   2. If a is zero, approx_a_recip must be infinite with the same sign as a.
+   3. If a is infinite, approx_a_recip must be zero with the same sign as a.
+
+   If the preconditions are satisfied, which they are for for the _*_rcp_ps
+   instructions on x86, the result has a maximum relative error of 2 ulps,
+   and correctly handles reciprocals of zero, infinity, and NaN.
+*/
+template <typename Packet, int Steps>
+struct generic_reciprocal_newton_step {
+  static_assert(Steps > 0, "Steps must be at least 1.");
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& a, const Packet& approx_a_recip) {
+    using Scalar = typename unpacket_traits<Packet>::type;
+    const Packet two = pset1<Packet>(Scalar(2));
+    // Refine the approximation using one Newton-Raphson step:
+    //   x_{i} = x_{i-1} * (2 - a * x_{i-1})
+    const Packet x = generic_reciprocal_newton_step<Packet, Steps - 1>::run(a, approx_a_recip);
+    const Packet tmp = pnmadd(a, x, two);
+    // If tmp is NaN, it means that a is either +/-0 or +/-Inf.
+    // In this case return the approximation directly.
+    const Packet is_not_nan = pcmp_eq(tmp, tmp);
+    return pselect(is_not_nan, pmul(x, tmp), x);
+  }
+};
+
+template <typename Packet>
+struct generic_reciprocal_newton_step<Packet, 0> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& /*unused*/, const Packet& approx_rsqrt) {
+    return approx_rsqrt;
+  }
+};
+
+/** \internal Fast reciprocal sqrt using Newton-Raphson's method.
+
+ Preconditions:
+   1. The starting guess provided in approx_a_recip must have at least half
+      the leading mantissa bits in the correct result, such that a single
+      Newton-Raphson step is sufficient to get within 1-2 ulps of the currect
+      result.
+   2. If a is zero, approx_a_recip must be infinite with the same sign as a.
+   3. If a is infinite, approx_a_recip must be zero with the same sign as a.
+
+   If the preconditions are satisfied, which they are for for the _*_rcp_ps
+   instructions on x86, the result has a maximum relative error of 2 ulps,
+   and correctly handles zero, infinity, and NaN. Positive denormals are
+   treated as zero.
+*/
+template <typename Packet, int Steps>
+struct generic_rsqrt_newton_step {
+  static_assert(Steps > 0, "Steps must be at least 1.");
+  using Scalar = typename unpacket_traits<Packet>::type;
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& a, const Packet& approx_rsqrt) {
+    constexpr Scalar kMinusHalf = Scalar(-1) / Scalar(2);
+    const Packet cst_minus_half = pset1<Packet>(kMinusHalf);
+    const Packet cst_minus_one = pset1<Packet>(Scalar(-1));
+
+    Packet inv_sqrt = approx_rsqrt;
+    for (int step = 0; step < Steps; ++step) {
+      // Refine the approximation using one Newton-Raphson step:
+      // h_n = (x * inv_sqrt) * inv_sqrt - 1 (so that h_n is nearly 0).
+      // inv_sqrt = inv_sqrt - 0.5 * inv_sqrt * h_n
+      Packet r2 = pmul(a, inv_sqrt);
+      Packet half_r = pmul(inv_sqrt, cst_minus_half);
+      Packet h_n = pmadd(r2, inv_sqrt, cst_minus_one);
+      inv_sqrt = pmadd(half_r, h_n, inv_sqrt);
+    }
+
+    // If x is NaN, then either:
+    // 1) the input is NaN
+    // 2) zero and infinity were multiplied
+    // In either of these cases, return approx_rsqrt
+    return pselect(pisnan(inv_sqrt), approx_rsqrt, inv_sqrt);
+  }
+};
+
+template <typename Packet>
+struct generic_rsqrt_newton_step<Packet, 0> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& /*unused*/, const Packet& approx_rsqrt) {
+    return approx_rsqrt;
+  }
+};
+
+/** \internal Fast sqrt using Newton-Raphson's method.
+
+ Preconditions:
+   1. The starting guess for the reciprocal sqrt provided in approx_rsqrt must
+      have at least half the leading mantissa bits in the correct result, such
+      that a single Newton-Raphson step is sufficient to get within 1-2 ulps of
+      the currect result.
+   2. If a is zero, approx_rsqrt must be infinite.
+   3. If a is infinite, approx_rsqrt must be zero.
+
+   If the preconditions are satisfied, which they are for for the _*_rsqrt_ps
+   instructions on x86, the result has a maximum relative error of 2 ulps,
+   and correctly handles zero and infinity, and NaN. Positive denormal inputs
+   are treated as zero.
+*/
+template <typename Packet, int Steps = 1>
+struct generic_sqrt_newton_step {
+  static_assert(Steps > 0, "Steps must be at least 1.");
+
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& a, const Packet& approx_rsqrt) {
+    using Scalar = typename unpacket_traits<Packet>::type;
+    const Packet one_point_five = pset1<Packet>(Scalar(1.5));
+    const Packet minus_half = pset1<Packet>(Scalar(-0.5));
+    // If a is inf or zero, return a directly.
+    const Packet inf_mask = pcmp_eq(a, pset1<Packet>(NumTraits<Scalar>::infinity()));
+    const Packet return_a = por(pcmp_eq(a, pzero(a)), inf_mask);
+    // Do a single step of Newton's iteration for reciprocal square root:
+    //   x_{n+1} = x_n * (1.5 + (-0.5 * x_n) * (a * x_n))).
+    // The Newton's step is computed this way to avoid over/under-flows.
+    Packet rsqrt = pmul(approx_rsqrt, pmadd(pmul(minus_half, approx_rsqrt), pmul(a, approx_rsqrt), one_point_five));
+    for (int step = 1; step < Steps; ++step) {
+      rsqrt = pmul(rsqrt, pmadd(pmul(minus_half, rsqrt), pmul(a, rsqrt), one_point_five));
+    }
+
+    // Return sqrt(x) = x * rsqrt(x) for non-zero finite positive arguments.
+    // Return a itself for 0 or +inf, NaN for negative arguments.
+    return pselect(return_a, a, pmul(a, rsqrt));
+  }
+};
+
 /** \internal \returns the hyperbolic tan of \a a (coeff-wise)
     Doesn't do anything fancy, just a 13/6-degree rational interpolant which
     is accurate up to a couple of ulps in the (approximate) range [-8, 8],
     outside of which tanh(x) = +/-1 in single precision. The input is clamped
     to the range [-c, c]. The value c is chosen as the smallest value where
     the approximation evaluates to exactly 1. In the reange [-0.0004, 0.0004]
-    the approxmation tanh(x) ~= x is used for better accuracy as x tends to zero.
+    the approximation tanh(x) ~= x is used for better accuracy as x tends to zero.
 
     This implementation works on both scalars and packets.
 */
-template<typename T>
-T generic_fast_tanh_float(const T& a_x)
-{
+template <typename T>
+T generic_fast_tanh_float(const T& a_x) {
   // Clamp the inputs to the range [-c, c]
 #ifdef EIGEN_VECTORIZE_FMA
   const T plus_clamp = pset1<T>(7.99881172180175781f);
@@ -75,31 +205,24 @@
   return pselect(tiny_mask, x, pdiv(p, q));
 }
 
-template<typename RealScalar>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-RealScalar positive_real_hypot(const RealScalar& x, const RealScalar& y)
-{
+template <typename RealScalar>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE RealScalar positive_real_hypot(const RealScalar& x, const RealScalar& y) {
   // IEEE IEC 6059 special cases.
-  if ((numext::isinf)(x) || (numext::isinf)(y))
-    return NumTraits<RealScalar>::infinity();
-  if ((numext::isnan)(x) || (numext::isnan)(y))
-    return NumTraits<RealScalar>::quiet_NaN();
-    
+  if ((numext::isinf)(x) || (numext::isinf)(y)) return NumTraits<RealScalar>::infinity();
+  if ((numext::isnan)(x) || (numext::isnan)(y)) return NumTraits<RealScalar>::quiet_NaN();
+
   EIGEN_USING_STD(sqrt);
   RealScalar p, qp;
-  p = numext::maxi(x,y);
-  if(p==RealScalar(0)) return RealScalar(0);
-  qp = numext::mini(y,x) / p;
-  return p * sqrt(RealScalar(1) + qp*qp);
+  p = numext::maxi(x, y);
+  if (numext::is_exactly_zero(p)) return RealScalar(0);
+  qp = numext::mini(y, x) / p;
+  return p * sqrt(RealScalar(1) + qp * qp);
 }
 
-template<typename Scalar>
-struct hypot_impl
-{
+template <typename Scalar>
+struct hypot_impl {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  static EIGEN_DEVICE_FUNC
-  inline RealScalar run(const Scalar& x, const Scalar& y)
-  {
+  static EIGEN_DEVICE_FUNC inline RealScalar run(const Scalar& x, const Scalar& y) {
     EIGEN_USING_STD(abs);
     return positive_real_hypot<RealScalar>(abs(x), abs(y));
   }
@@ -107,7 +230,7 @@
 
 // Generic complex sqrt implementation that correctly handles corner cases
 // according to https://en.cppreference.com/w/cpp/numeric/complex/sqrt
-template<typename T>
+template <typename T>
 EIGEN_DEVICE_FUNC std::complex<T> complex_sqrt(const std::complex<T>& z) {
   // Computes the principal sqrt of the input.
   //
@@ -136,15 +259,14 @@
   const T zero = T(0);
   const T w = numext::sqrt(T(0.5) * (numext::abs(x) + numext::hypot(x, y)));
 
-  return
-    (numext::isinf)(y) ? std::complex<T>(NumTraits<T>::infinity(), y)
-      : x == zero ? std::complex<T>(w, y < zero ? -w : w)
-      : x > zero ? std::complex<T>(w, y / (2 * w))
-      : std::complex<T>(numext::abs(y) / (2 * w), y < zero ? -w : w );
+  return (numext::isinf)(y)           ? std::complex<T>(NumTraits<T>::infinity(), y)
+         : numext::is_exactly_zero(x) ? std::complex<T>(w, y < zero ? -w : w)
+         : x > zero                   ? std::complex<T>(w, y / (2 * w))
+                                      : std::complex<T>(numext::abs(y) / (2 * w), y < zero ? -w : w);
 }
 
 // Generic complex rsqrt implementation.
-template<typename T>
+template <typename T>
 EIGEN_DEVICE_FUNC std::complex<T> complex_rsqrt(const std::complex<T>& z) {
   // Computes the principal reciprocal sqrt of the input.
   //
@@ -176,15 +298,14 @@
   const T w = numext::sqrt(T(0.5) * (numext::abs(x) + abs_z));
   const T woz = w / abs_z;
   // Corner cases consistent with 1/sqrt(z) on gcc/clang.
-  return
-    abs_z == zero ? std::complex<T>(NumTraits<T>::infinity(), NumTraits<T>::quiet_NaN())
-      : ((numext::isinf)(x) || (numext::isinf)(y)) ? std::complex<T>(zero, zero)
-      : x == zero ? std::complex<T>(woz, y < zero ? woz : -woz)
-      : x > zero ? std::complex<T>(woz, -y / (2 * w * abs_z))
-      : std::complex<T>(numext::abs(y) / (2 * w * abs_z), y < zero ? woz : -woz );
+  return numext::is_exactly_zero(abs_z) ? std::complex<T>(NumTraits<T>::infinity(), NumTraits<T>::quiet_NaN())
+         : ((numext::isinf)(x) || (numext::isinf)(y)) ? std::complex<T>(zero, zero)
+         : numext::is_exactly_zero(x)                 ? std::complex<T>(woz, y < zero ? woz : -woz)
+         : x > zero                                   ? std::complex<T>(woz, -y / (2 * w * abs_z))
+                    : std::complex<T>(numext::abs(y) / (2 * w * abs_z), y < zero ? woz : -woz);
 }
 
-template<typename T>
+template <typename T>
 EIGEN_DEVICE_FUNC std::complex<T> complex_log(const std::complex<T>& z) {
   // Computes complex log.
   T a = numext::abs(z);
@@ -193,8 +314,8 @@
   return std::complex<T>(numext::log(a), b);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATHFUNCTIONSIMPL_H
+#endif  // EIGEN_MATHFUNCTIONSIMPL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h
index f0e59a9..ce0e4e6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h
@@ -11,531 +11,495 @@
 #ifndef EIGEN_MATRIX_H
 #define EIGEN_MATRIX_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct traits<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-{
-private:
-  enum { size = internal::size_at_compile_time<_Rows,_Cols>::ret };
-  typedef typename find_best_packet<_Scalar,size>::type PacketScalar;
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+struct traits<Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> {
+ private:
+  constexpr static int size = internal::size_at_compile_time(Rows_, Cols_);
+  typedef typename find_best_packet<Scalar_, size>::type PacketScalar;
   enum {
-      row_major_bit = _Options&RowMajor ? RowMajorBit : 0,
-      is_dynamic_size_storage = _MaxRows==Dynamic || _MaxCols==Dynamic,
-      max_size = is_dynamic_size_storage ? Dynamic : _MaxRows*_MaxCols,
-      default_alignment = compute_default_alignment<_Scalar,max_size>::value,
-      actual_alignment = ((_Options&DontAlign)==0) ? default_alignment : 0,
-      required_alignment = unpacket_traits<PacketScalar>::alignment,
-      packet_access_bit = (packet_traits<_Scalar>::Vectorizable && (EIGEN_UNALIGNED_VECTORIZE || (actual_alignment>=required_alignment))) ? PacketAccessBit : 0
-    };
+    row_major_bit = Options_ & RowMajor ? RowMajorBit : 0,
+    is_dynamic_size_storage = MaxRows_ == Dynamic || MaxCols_ == Dynamic,
+    max_size = is_dynamic_size_storage ? Dynamic : MaxRows_ * MaxCols_,
+    default_alignment = compute_default_alignment<Scalar_, max_size>::value,
+    actual_alignment = ((Options_ & DontAlign) == 0) ? default_alignment : 0,
+    required_alignment = unpacket_traits<PacketScalar>::alignment,
+    packet_access_bit = (packet_traits<Scalar_>::Vectorizable &&
+                         (EIGEN_UNALIGNED_VECTORIZE || (actual_alignment >= required_alignment)))
+                            ? PacketAccessBit
+                            : 0
+  };
 
-public:
-  typedef _Scalar Scalar;
+ public:
+  typedef Scalar_ Scalar;
   typedef Dense StorageKind;
   typedef Eigen::Index StorageIndex;
   typedef MatrixXpr XprKind;
   enum {
-    RowsAtCompileTime = _Rows,
-    ColsAtCompileTime = _Cols,
-    MaxRowsAtCompileTime = _MaxRows,
-    MaxColsAtCompileTime = _MaxCols,
-    Flags = compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret,
-    Options = _Options,
+    RowsAtCompileTime = Rows_,
+    ColsAtCompileTime = Cols_,
+    MaxRowsAtCompileTime = MaxRows_,
+    MaxColsAtCompileTime = MaxCols_,
+    Flags = compute_matrix_flags(Options_),
+    Options = Options_,
     InnerStrideAtCompileTime = 1,
-    OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime,
+    OuterStrideAtCompileTime = (Options & RowMajor) ? ColsAtCompileTime : RowsAtCompileTime,
 
     // FIXME, the following flag in only used to define NeedsToAlign in PlainObjectBase
     EvaluatorFlags = LinearAccessBit | DirectAccessBit | packet_access_bit | row_major_bit,
     Alignment = actual_alignment
   };
 };
-}
+}  // namespace internal
 
 /** \class Matrix
-  * \ingroup Core_Module
-  *
-  * \brief The matrix class, also used for vectors and row-vectors
-  *
-  * The %Matrix class is the work-horse for all \em dense (\ref dense "note") matrices and vectors within Eigen.
-  * Vectors are matrices with one column, and row-vectors are matrices with one row.
-  *
-  * The %Matrix class encompasses \em both fixed-size and dynamic-size objects (\ref fixedsize "note").
-  *
-  * The first three template parameters are required:
-  * \tparam _Scalar Numeric type, e.g. float, double, int or std::complex<float>.
-  *                 User defined scalar types are supported as well (see \ref user_defined_scalars "here").
-  * \tparam _Rows Number of rows, or \b Dynamic
-  * \tparam _Cols Number of columns, or \b Dynamic
-  *
-  * The remaining template parameters are optional -- in most cases you don't have to worry about them.
-  * \tparam _Options A combination of either \b #RowMajor or \b #ColMajor, and of either
-  *                 \b #AutoAlign or \b #DontAlign.
-  *                 The former controls \ref TopicStorageOrders "storage order", and defaults to column-major. The latter controls alignment, which is required
-  *                 for vectorization. It defaults to aligning matrices except for fixed sizes that aren't a multiple of the packet size.
-  * \tparam _MaxRows Maximum number of rows. Defaults to \a _Rows (\ref maxrows "note").
-  * \tparam _MaxCols Maximum number of columns. Defaults to \a _Cols (\ref maxrows "note").
-  *
-  * Eigen provides a number of typedefs covering the usual cases. Here are some examples:
-  *
-  * \li \c Matrix2d is a 2x2 square matrix of doubles (\c Matrix<double, 2, 2>)
-  * \li \c Vector4f is a vector of 4 floats (\c Matrix<float, 4, 1>)
-  * \li \c RowVector3i is a row-vector of 3 ints (\c Matrix<int, 1, 3>)
-  *
-  * \li \c MatrixXf is a dynamic-size matrix of floats (\c Matrix<float, Dynamic, Dynamic>)
-  * \li \c VectorXf is a dynamic-size vector of floats (\c Matrix<float, Dynamic, 1>)
-  *
-  * \li \c Matrix2Xf is a partially fixed-size (dynamic-size) matrix of floats (\c Matrix<float, 2, Dynamic>)
-  * \li \c MatrixX3d is a partially dynamic-size (fixed-size) matrix of double (\c Matrix<double, Dynamic, 3>)
-  *
-  * See \link matrixtypedefs this page \endlink for a complete list of predefined \em %Matrix and \em Vector typedefs.
-  *
-  * You can access elements of vectors and matrices using normal subscripting:
-  *
-  * \code
-  * Eigen::VectorXd v(10);
-  * v[0] = 0.1;
-  * v[1] = 0.2;
-  * v(0) = 0.3;
-  * v(1) = 0.4;
-  *
-  * Eigen::MatrixXi m(10, 10);
-  * m(0, 1) = 1;
-  * m(0, 2) = 2;
-  * m(0, 3) = 3;
-  * \endcode
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_MATRIX_PLUGIN.
-  *
-  * <i><b>Some notes:</b></i>
-  *
-  * <dl>
-  * <dt><b>\anchor dense Dense versus sparse:</b></dt>
-  * <dd>This %Matrix class handles dense, not sparse matrices and vectors. For sparse matrices and vectors, see the Sparse module.
-  *
-  * Dense matrices and vectors are plain usual arrays of coefficients. All the coefficients are stored, in an ordinary contiguous array.
-  * This is unlike Sparse matrices and vectors where the coefficients are stored as a list of nonzero coefficients.</dd>
-  *
-  * <dt><b>\anchor fixedsize Fixed-size versus dynamic-size:</b></dt>
-  * <dd>Fixed-size means that the numbers of rows and columns are known are compile-time. In this case, Eigen allocates the array
-  * of coefficients as a fixed-size array, as a class member. This makes sense for very small matrices, typically up to 4x4, sometimes up
-  * to 16x16. Larger matrices should be declared as dynamic-size even if one happens to know their size at compile-time.
-  *
-  * Dynamic-size means that the numbers of rows or columns are not necessarily known at compile-time. In this case they are runtime
-  * variables, and the array of coefficients is allocated dynamically on the heap.
-  *
-  * Note that \em dense matrices, be they Fixed-size or Dynamic-size, <em>do not</em> expand dynamically in the sense of a std::map.
-  * If you want this behavior, see the Sparse module.</dd>
-  *
-  * <dt><b>\anchor maxrows _MaxRows and _MaxCols:</b></dt>
-  * <dd>In most cases, one just leaves these parameters to the default values.
-  * These parameters mean the maximum size of rows and columns that the matrix may have. They are useful in cases
-  * when the exact numbers of rows and columns are not known are compile-time, but it is known at compile-time that they cannot
-  * exceed a certain value. This happens when taking dynamic-size blocks inside fixed-size matrices: in this case _MaxRows and _MaxCols
-  * are the dimensions of the original matrix, while _Rows and _Cols are Dynamic.</dd>
-  * </dl>
-  *
-  * <i><b>ABI and storage layout</b></i>
-  *
-  * The table below summarizes the ABI of some possible Matrix instances which is fixed thorough the lifetime of Eigen 3.
-  * <table  class="manual">
-  * <tr><th>Matrix type</th><th>Equivalent C structure</th></tr>
-  * <tr><td>\code Matrix<T,Dynamic,Dynamic> \endcode</td><td>\code
-  * struct {
-  *   T *data;                  // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0
-  *   Eigen::Index rows, cols;
-  *  };
-  * \endcode</td></tr>
-  * <tr class="alt"><td>\code
-  * Matrix<T,Dynamic,1>
-  * Matrix<T,1,Dynamic> \endcode</td><td>\code
-  * struct {
-  *   T *data;                  // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0
-  *   Eigen::Index size;
-  *  };
-  * \endcode</td></tr>
-  * <tr><td>\code Matrix<T,Rows,Cols> \endcode</td><td>\code
-  * struct {
-  *   T data[Rows*Cols];        // with (size_t(data)%A(Rows*Cols*sizeof(T)))==0
-  *  };
-  * \endcode</td></tr>
-  * <tr class="alt"><td>\code Matrix<T,Dynamic,Dynamic,0,MaxRows,MaxCols> \endcode</td><td>\code
-  * struct {
-  *   T data[MaxRows*MaxCols];  // with (size_t(data)%A(MaxRows*MaxCols*sizeof(T)))==0
-  *   Eigen::Index rows, cols;
-  *  };
-  * \endcode</td></tr>
-  * </table>
-  * Note that in this table Rows, Cols, MaxRows and MaxCols are all positive integers. A(S) is defined to the largest possible power-of-two
-  * smaller to EIGEN_MAX_STATIC_ALIGN_BYTES.
-  *
-  * \see MatrixBase for the majority of the API methods for matrices, \ref TopicClassHierarchy,
-  * \ref TopicStorageOrders
-  */
+ * \ingroup Core_Module
+ *
+ * \brief The matrix class, also used for vectors and row-vectors
+ *
+ * The %Matrix class is the work-horse for all \em dense (\ref dense "note") matrices and vectors within Eigen.
+ * Vectors are matrices with one column, and row-vectors are matrices with one row.
+ *
+ * The %Matrix class encompasses \em both fixed-size and dynamic-size objects (\ref fixedsize "note").
+ *
+ * The first three template parameters are required:
+ * \tparam Scalar_ Numeric type, e.g. float, double, int or std::complex<float>.
+ *                 User defined scalar types are supported as well (see \ref user_defined_scalars "here").
+ * \tparam Rows_ Number of rows, or \b Dynamic
+ * \tparam Cols_ Number of columns, or \b Dynamic
+ *
+ * The remaining template parameters are optional -- in most cases you don't have to worry about them.
+ * \tparam Options_ A combination of either \b #RowMajor or \b #ColMajor, and of either
+ *                 \b #AutoAlign or \b #DontAlign.
+ *                 The former controls \ref TopicStorageOrders "storage order", and defaults to column-major. The latter
+ * controls alignment, which is required for vectorization. It defaults to aligning matrices except for fixed sizes that
+ * aren't a multiple of the packet size. \tparam MaxRows_ Maximum number of rows. Defaults to \a Rows_ (\ref maxrows
+ * "note"). \tparam MaxCols_ Maximum number of columns. Defaults to \a Cols_ (\ref maxrows "note").
+ *
+ * Eigen provides a number of typedefs covering the usual cases. Here are some examples:
+ *
+ * \li \c Matrix2d is a 2x2 square matrix of doubles (\c Matrix<double, 2, 2>)
+ * \li \c Vector4f is a vector of 4 floats (\c Matrix<float, 4, 1>)
+ * \li \c RowVector3i is a row-vector of 3 ints (\c Matrix<int, 1, 3>)
+ *
+ * \li \c MatrixXf is a dynamic-size matrix of floats (\c Matrix<float, Dynamic, Dynamic>)
+ * \li \c VectorXf is a dynamic-size vector of floats (\c Matrix<float, Dynamic, 1>)
+ *
+ * \li \c Matrix2Xf is a partially fixed-size (dynamic-size) matrix of floats (\c Matrix<float, 2, Dynamic>)
+ * \li \c MatrixX3d is a partially dynamic-size (fixed-size) matrix of double (\c Matrix<double, Dynamic, 3>)
+ *
+ * See \link matrixtypedefs this page \endlink for a complete list of predefined \em %Matrix and \em Vector typedefs.
+ *
+ * You can access elements of vectors and matrices using normal subscripting:
+ *
+ * \code
+ * Eigen::VectorXd v(10);
+ * v[0] = 0.1;
+ * v[1] = 0.2;
+ * v(0) = 0.3;
+ * v(1) = 0.4;
+ *
+ * Eigen::MatrixXi m(10, 10);
+ * m(0, 1) = 1;
+ * m(0, 2) = 2;
+ * m(0, 3) = 3;
+ * \endcode
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_MATRIX_PLUGIN.
+ *
+ * <i><b>Some notes:</b></i>
+ *
+ * <dl>
+ * <dt><b>\anchor dense Dense versus sparse:</b></dt>
+ * <dd>This %Matrix class handles dense, not sparse matrices and vectors. For sparse matrices and vectors, see the
+ * Sparse module.
+ *
+ * Dense matrices and vectors are plain usual arrays of coefficients. All the coefficients are stored, in an ordinary
+ * contiguous array. This is unlike Sparse matrices and vectors where the coefficients are stored as a list of nonzero
+ * coefficients.</dd>
+ *
+ * <dt><b>\anchor fixedsize Fixed-size versus dynamic-size:</b></dt>
+ * <dd>Fixed-size means that the numbers of rows and columns are known are compile-time. In this case, Eigen allocates
+ * the array of coefficients as a fixed-size array, as a class member. This makes sense for very small matrices,
+ * typically up to 4x4, sometimes up to 16x16. Larger matrices should be declared as dynamic-size even if one happens to
+ * know their size at compile-time.
+ *
+ * Dynamic-size means that the numbers of rows or columns are not necessarily known at compile-time. In this case they
+ * are runtime variables, and the array of coefficients is allocated dynamically on the heap.
+ *
+ * Note that \em dense matrices, be they Fixed-size or Dynamic-size, <em>do not</em> expand dynamically in the sense of
+ * a std::map. If you want this behavior, see the Sparse module.</dd>
+ *
+ * <dt><b>\anchor maxrows MaxRows_ and MaxCols_:</b></dt>
+ * <dd>In most cases, one just leaves these parameters to the default values.
+ * These parameters mean the maximum size of rows and columns that the matrix may have. They are useful in cases
+ * when the exact numbers of rows and columns are not known are compile-time, but it is known at compile-time that they
+ * cannot exceed a certain value. This happens when taking dynamic-size blocks inside fixed-size matrices: in this case
+ * MaxRows_ and MaxCols_ are the dimensions of the original matrix, while Rows_ and Cols_ are Dynamic.</dd>
+ * </dl>
+ *
+ * <i><b>ABI and storage layout</b></i>
+ *
+ * The table below summarizes the ABI of some possible Matrix instances which is fixed thorough the lifetime of Eigen 3.
+ * <table  class="manual">
+ * <tr><th>Matrix type</th><th>Equivalent C structure</th></tr>
+ * <tr><td>\code Matrix<T,Dynamic,Dynamic> \endcode</td><td>\code
+ * struct {
+ *   T *data;                  // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0
+ *   Eigen::Index rows, cols;
+ *  };
+ * \endcode</td></tr>
+ * <tr class="alt"><td>\code
+ * Matrix<T,Dynamic,1>
+ * Matrix<T,1,Dynamic> \endcode</td><td>\code
+ * struct {
+ *   T *data;                  // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0
+ *   Eigen::Index size;
+ *  };
+ * \endcode</td></tr>
+ * <tr><td>\code Matrix<T,Rows,Cols> \endcode</td><td>\code
+ * struct {
+ *   T data[Rows*Cols];        // with (size_t(data)%A(Rows*Cols*sizeof(T)))==0
+ *  };
+ * \endcode</td></tr>
+ * <tr class="alt"><td>\code Matrix<T,Dynamic,Dynamic,0,MaxRows,MaxCols> \endcode</td><td>\code
+ * struct {
+ *   T data[MaxRows*MaxCols];  // with (size_t(data)%A(MaxRows*MaxCols*sizeof(T)))==0
+ *   Eigen::Index rows, cols;
+ *  };
+ * \endcode</td></tr>
+ * </table>
+ * Note that in this table Rows, Cols, MaxRows and MaxCols are all positive integers. A(S) is defined to the largest
+ * possible power-of-two smaller to EIGEN_MAX_STATIC_ALIGN_BYTES.
+ *
+ * \see MatrixBase for the majority of the API methods for matrices, \ref TopicClassHierarchy,
+ * \ref TopicStorageOrders
+ */
 
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-class Matrix
-  : public PlainObjectBase<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-{
-  public:
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+class Matrix : public PlainObjectBase<Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> {
+ public:
+  /** \brief Base class typedef.
+   * \sa PlainObjectBase
+   */
+  typedef PlainObjectBase<Matrix> Base;
 
-    /** \brief Base class typedef.
-      * \sa PlainObjectBase
-      */
-    typedef PlainObjectBase<Matrix> Base;
+  enum { Options = Options_ };
 
-    enum { Options = _Options };
+  EIGEN_DENSE_PUBLIC_INTERFACE(Matrix)
 
-    EIGEN_DENSE_PUBLIC_INTERFACE(Matrix)
+  typedef typename Base::PlainObject PlainObject;
 
-    typedef typename Base::PlainObject PlainObject;
+  using Base::base;
+  using Base::coeffRef;
 
-    using Base::base;
-    using Base::coeffRef;
+  /**
+   * \brief Assigns matrices to each other.
+   *
+   * \note This is a special case of the templated operator=. Its purpose is
+   * to prevent a default operator= from hiding the templated operator=.
+   *
+   * \callgraph
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const Matrix& other) { return Base::_set(other); }
 
-    /**
-      * \brief Assigns matrices to each other.
-      *
-      * \note This is a special case of the templated operator=. Its purpose is
-      * to prevent a default operator= from hiding the templated operator=.
-      *
-      * \callgraph
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix& operator=(const Matrix& other)
-    {
-      return Base::_set(other);
-    }
+  /** \internal
+   * \brief Copies the value of the expression \a other into \c *this with automatic resizing.
+   *
+   * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
+   * it will be initialized.
+   *
+   * Note that copying a row-vector into a vector (and conversely) is allowed.
+   * The resizing, if any, is then done in the appropriate way so that row-vectors
+   * remain row-vectors and vectors remain vectors.
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const DenseBase<OtherDerived>& other) {
+    return Base::_set(other);
+  }
 
-    /** \internal
-      * \brief Copies the value of the expression \a other into \c *this with automatic resizing.
-      *
-      * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
-      * it will be initialized.
-      *
-      * Note that copying a row-vector into a vector (and conversely) is allowed.
-      * The resizing, if any, is then done in the appropriate way so that row-vectors
-      * remain row-vectors and vectors remain vectors.
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix& operator=(const DenseBase<OtherDerived>& other)
-    {
-      return Base::_set(other);
-    }
+  /* Here, doxygen failed to copy the brief information when using \copydoc */
 
-    /* Here, doxygen failed to copy the brief information when using \copydoc */
+  /**
+   * \brief Copies the generic expression \a other into *this.
+   * \copydetails DenseBase::operator=(const EigenBase<OtherDerived> &other)
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const EigenBase<OtherDerived>& other) {
+    return Base::operator=(other);
+  }
 
-    /**
-      * \brief Copies the generic expression \a other into *this.
-      * \copydetails DenseBase::operator=(const EigenBase<OtherDerived> &other)
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix& operator=(const EigenBase<OtherDerived> &other)
-    {
-      return Base::operator=(other);
-    }
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const ReturnByValue<OtherDerived>& func) {
+    return Base::operator=(func);
+  }
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix& operator=(const ReturnByValue<OtherDerived>& func)
-    {
-      return Base::operator=(func);
-    }
+  /** \brief Default constructor.
+   *
+   * For fixed-size matrices, does nothing.
+   *
+   * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix
+   * is called a null matrix. This constructor is the unique way to create null matrices: resizing
+   * a matrix to 0 is not supported.
+   *
+   * \sa resize(Index,Index)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix()
+      : Base(){EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED}
 
-    /** \brief Default constructor.
-      *
-      * For fixed-size matrices, does nothing.
-      *
-      * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix
-      * is called a null matrix. This constructor is the unique way to create null matrices: resizing
-      * a matrix to 0 is not supported.
-      *
-      * \sa resize(Index,Index)
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Matrix() : Base()
-    {
-      Base::_check_template_params();
-      EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-    }
+        // FIXME is it still needed
+        EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Matrix(internal::constructor_without_unaligned_array_assert)
+      : Base(internal::constructor_without_unaligned_array_assert()){EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED}
 
-    // FIXME is it still needed
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    explicit Matrix(internal::constructor_without_unaligned_array_assert)
-      : Base(internal::constructor_without_unaligned_array_assert())
-    { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED }
+        EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(Matrix && other)
+            EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value)
+      : Base(std::move(other)) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(Matrix&& other)
+      EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value) {
+    Base::operator=(std::move(other));
+    return *this;
+  }
 
-#if EIGEN_HAS_RVALUE_REFERENCES
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Matrix(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value)
-      : Base(std::move(other))
-    {
-      Base::_check_template_params();
-    }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Matrix& operator=(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
-    {
-      Base::operator=(std::move(other));
-      return *this;
-    }
-#endif
-
-#if EIGEN_HAS_CXX11
-    /** \copydoc PlainObjectBase(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&... args)
-     *
-     * Example: \include Matrix_variadic_ctor_cxx11.cpp
-     * Output: \verbinclude Matrix_variadic_ctor_cxx11.out
-     *
-     * \sa Matrix(const std::initializer_list<std::initializer_list<Scalar>>&)
-     */
-    template <typename... ArgTypes>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2,  const Scalar& a3, const ArgTypes&... args)
+  /** \copydoc PlainObjectBase(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&... args)
+   *
+   * Example: \include Matrix_variadic_ctor_cxx11.cpp
+   * Output: \verbinclude Matrix_variadic_ctor_cxx11.out
+   *
+   * \sa Matrix(const std::initializer_list<std::initializer_list<Scalar>>&)
+   */
+  template <typename... ArgTypes>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3,
+                                               const ArgTypes&... args)
       : Base(a0, a1, a2, a3, args...) {}
 
-    /** \brief Constructs a Matrix and initializes it from the coefficients given as initializer-lists grouped by row. \cpp11
-      *
-      * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients:
-      *
-      * Example: \include Matrix_initializer_list_23_cxx11.cpp
-      * Output: \verbinclude Matrix_initializer_list_23_cxx11.out
-      *
-      * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is triggered.
-      *
-      * In the case of a compile-time column vector, implicit transposition from a single row is allowed.
-      * Therefore <code>VectorXd{{1,2,3,4,5}}</code> is legal and the more verbose syntax
-      * <code>RowVectorXd{{1},{2},{3},{4},{5}}</code> can be avoided:
-      *
-      * Example: \include Matrix_initializer_list_vector_cxx11.cpp
-      * Output: \verbinclude Matrix_initializer_list_vector_cxx11.out
-      *
-      * In the case of fixed-sized matrices, the initializer list sizes must exactly match the matrix sizes,
-      * and implicit transposition is allowed for compile-time vectors only.
-      *
-      * \sa Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2,  const Scalar& a3, const ArgTypes&... args)
-      */
-    EIGEN_DEVICE_FUNC
-    explicit EIGEN_STRONG_INLINE Matrix(const std::initializer_list<std::initializer_list<Scalar>>& list) : Base(list) {}
-#endif // end EIGEN_HAS_CXX11
+  /** \brief Constructs a Matrix and initializes it from the coefficients given as initializer-lists grouped by row.
+   * \cpp11
+   *
+   * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients:
+   *
+   * Example: \include Matrix_initializer_list_23_cxx11.cpp
+   * Output: \verbinclude Matrix_initializer_list_23_cxx11.out
+   *
+   * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is
+   * triggered.
+   *
+   * In the case of a compile-time column vector, implicit transposition from a single row is allowed.
+   * Therefore <code>VectorXd{{1,2,3,4,5}}</code> is legal and the more verbose syntax
+   * <code>RowVectorXd{{1},{2},{3},{4},{5}}</code> can be avoided:
+   *
+   * Example: \include Matrix_initializer_list_vector_cxx11.cpp
+   * Output: \verbinclude Matrix_initializer_list_vector_cxx11.out
+   *
+   * In the case of fixed-sized matrices, the initializer list sizes must exactly match the matrix sizes,
+   * and implicit transposition is allowed for compile-time vectors only.
+   *
+   * \sa Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2,  const Scalar& a3, const ArgTypes&... args)
+   */
+  EIGEN_DEVICE_FUNC explicit constexpr EIGEN_STRONG_INLINE Matrix(
+      const std::initializer_list<std::initializer_list<Scalar>>& list)
+      : Base(list) {}
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 
-    // This constructor is for both 1x1 matrices and dynamic vectors
-    template<typename T>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    explicit Matrix(const T& x)
-    {
-      Base::_check_template_params();
-      Base::template _init1<T>(x);
-    }
+  // This constructor is for both 1x1 matrices and dynamic vectors
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Matrix(const T& x) {
+    Base::template _init1<T>(x);
+  }
 
-    template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Matrix(const T0& x, const T1& y)
-    {
-      Base::_check_template_params();
-      Base::template _init2<T0,T1>(x, y);
-    }
-
+  template <typename T0, typename T1>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const T0& x, const T1& y) {
+    Base::template _init2<T0, T1>(x, y);
+  }
 
 #else
-    /** \brief Constructs a fixed-sized matrix initialized with coefficients starting at \a data */
-    EIGEN_DEVICE_FUNC
-    explicit Matrix(const Scalar *data);
+  /** \brief Constructs a fixed-sized matrix initialized with coefficients starting at \a data */
+  EIGEN_DEVICE_FUNC explicit Matrix(const Scalar* data);
 
-    /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors
-      *
-      * This is useful for dynamic-size vectors. For fixed-size vectors,
-      * it is redundant to pass these parameters, so one should use the default constructor
-      * Matrix() instead.
-      *
-      * \warning This constructor is disabled for fixed-size \c 1x1 matrices. For instance,
-      * calling Matrix<double,1,1>(1) will call the initialization constructor: Matrix(const Scalar&).
-      * For fixed-size \c 1x1 matrices it is therefore recommended to use the default
-      * constructor Matrix() instead, especially when using one of the non standard
-      * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives).
-      */
-    EIGEN_STRONG_INLINE explicit Matrix(Index dim);
-    /** \brief Constructs an initialized 1x1 matrix with the given coefficient
-      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...) */
-    Matrix(const Scalar& x);
-    /** \brief Constructs an uninitialized matrix with \a rows rows and \a cols columns.
-      *
-      * This is useful for dynamic-size matrices. For fixed-size matrices,
-      * it is redundant to pass these parameters, so one should use the default constructor
-      * Matrix() instead.
-      *
-      * \warning This constructor is disabled for fixed-size \c 1x2 and \c 2x1 vectors. For instance,
-      * calling Matrix2f(2,1) will call the initialization constructor: Matrix(const Scalar& x, const Scalar& y).
-      * For fixed-size \c 1x2 or \c 2x1 vectors it is therefore recommended to use the default
-      * constructor Matrix() instead, especially when using one of the non standard
-      * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives).
-      */
-    EIGEN_DEVICE_FUNC
-    Matrix(Index rows, Index cols);
+  /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors
+   *
+   * This is useful for dynamic-size vectors. For fixed-size vectors,
+   * it is redundant to pass these parameters, so one should use the default constructor
+   * Matrix() instead.
+   *
+   * \warning This constructor is disabled for fixed-size \c 1x1 matrices. For instance,
+   * calling Matrix<double,1,1>(1) will call the initialization constructor: Matrix(const Scalar&).
+   * For fixed-size \c 1x1 matrices it is therefore recommended to use the default
+   * constructor Matrix() instead, especially when using one of the non standard
+   * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives).
+   */
+  EIGEN_STRONG_INLINE explicit Matrix(Index dim);
+  /** \brief Constructs an initialized 1x1 matrix with the given coefficient
+   * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...) */
+  Matrix(const Scalar& x);
+  /** \brief Constructs an uninitialized matrix with \a rows rows and \a cols columns.
+   *
+   * This is useful for dynamic-size matrices. For fixed-size matrices,
+   * it is redundant to pass these parameters, so one should use the default constructor
+   * Matrix() instead.
+   *
+   * \warning This constructor is disabled for fixed-size \c 1x2 and \c 2x1 vectors. For instance,
+   * calling Matrix2f(2,1) will call the initialization constructor: Matrix(const Scalar& x, const Scalar& y).
+   * For fixed-size \c 1x2 or \c 2x1 vectors it is therefore recommended to use the default
+   * constructor Matrix() instead, especially when using one of the non standard
+   * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives).
+   */
+  EIGEN_DEVICE_FUNC Matrix(Index rows, Index cols);
 
-    /** \brief Constructs an initialized 2D vector with given coefficients
-      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...) */
-    Matrix(const Scalar& x, const Scalar& y);
-    #endif  // end EIGEN_PARSED_BY_DOXYGEN
+  /** \brief Constructs an initialized 2D vector with given coefficients
+   * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...) */
+  Matrix(const Scalar& x, const Scalar& y);
+#endif  // end EIGEN_PARSED_BY_DOXYGEN
 
-    /** \brief Constructs an initialized 3D vector with given coefficients
-      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z)
-    {
-      Base::_check_template_params();
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 3)
-      m_storage.data()[0] = x;
-      m_storage.data()[1] = y;
-      m_storage.data()[2] = z;
-    }
-    /** \brief Constructs an initialized 4D vector with given coefficients
-      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
-    {
-      Base::_check_template_params();
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 4)
-      m_storage.data()[0] = x;
-      m_storage.data()[1] = y;
-      m_storage.data()[2] = z;
-      m_storage.data()[3] = w;
-    }
+  /** \brief Constructs an initialized 3D vector with given coefficients
+   * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 3)
+    m_storage.data()[0] = x;
+    m_storage.data()[1] = y;
+    m_storage.data()[2] = z;
+  }
+  /** \brief Constructs an initialized 4D vector with given coefficients
+   * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 4)
+    m_storage.data()[0] = x;
+    m_storage.data()[1] = y;
+    m_storage.data()[2] = z;
+    m_storage.data()[3] = w;
+  }
 
+  /** \brief Copy constructor */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Matrix& other) : Base(other) {}
 
-    /** \brief Copy constructor */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix(const Matrix& other) : Base(other)
-    { }
+  /** \brief Copy constructor for generic expressions.
+   * \sa MatrixBase::operator=(const EigenBase<OtherDerived>&)
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const EigenBase<OtherDerived>& other) : Base(other.derived()) {}
 
-    /** \brief Copy constructor for generic expressions.
-      * \sa MatrixBase::operator=(const EigenBase<OtherDerived>&)
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix(const EigenBase<OtherDerived> &other)
-      : Base(other.derived())
-    { }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return 1; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT { return 1; }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); }
+  /////////// Geometry module ///////////
 
-    /////////// Geometry module ///////////
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC explicit Matrix(const RotationBase<OtherDerived, ColsAtCompileTime>& r);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC Matrix& operator=(const RotationBase<OtherDerived, ColsAtCompileTime>& r);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    explicit Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r);
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Matrix& operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r);
+// allow to extend Matrix outside Eigen
+#ifdef EIGEN_MATRIX_PLUGIN
+#include EIGEN_MATRIX_PLUGIN
+#endif
 
-    // allow to extend Matrix outside Eigen
-    #ifdef EIGEN_MATRIX_PLUGIN
-    #include EIGEN_MATRIX_PLUGIN
-    #endif
+ protected:
+  template <typename Derived, typename OtherDerived, bool IsVector>
+  friend struct internal::conservative_resize_like_impl;
 
-  protected:
-    template <typename Derived, typename OtherDerived, bool IsVector>
-    friend struct internal::conservative_resize_like_impl;
-
-    using Base::m_storage;
+  using Base::m_storage;
 };
 
 /** \defgroup matrixtypedefs Global matrix typedefs
-  *
-  * \ingroup Core_Module
-  *
-  * %Eigen defines several typedef shortcuts for most common matrix and vector types.
-  *
-  * The general patterns are the following:
-  *
-  * \c MatrixSizeType where \c Size can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size,
-  * and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c cd
-  * for complex double.
-  *
-  * For example, \c Matrix3d is a fixed-size 3x3 matrix type of doubles, and \c MatrixXf is a dynamic-size matrix of floats.
-  *
-  * There are also \c VectorSizeType and \c RowVectorSizeType which are self-explanatory. For example, \c Vector4cf is
-  * a fixed-size vector of 4 complex floats.
-  *
-  * With \cpp11, template alias are also defined for common sizes.
-  * They follow the same pattern as above except that the scalar type suffix is replaced by a
-  * template parameter, i.e.:
-  *   - `MatrixSize<Type>` where `Size` can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size.
-  *   - `MatrixXSize<Type>` and `MatrixSizeX<Type>` where `Size` can be \c 2,\c 3,\c 4 for hybrid dynamic/fixed matrices.
-  *   - `VectorSize<Type>` and `RowVectorSize<Type>` for column and row vectors.
-  *
-  * With \cpp11, you can also use fully generic column and row vector types: `Vector<Type,Size>` and `RowVector<Type,Size>`.
-  *
-  * \sa class Matrix
-  */
+ *
+ * \ingroup Core_Module
+ *
+ * %Eigen defines several typedef shortcuts for most common matrix and vector types.
+ *
+ * The general patterns are the following:
+ *
+ * \c MatrixSizeType where \c Size can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size,
+ * and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c cd
+ * for complex double.
+ *
+ * For example, \c Matrix3d is a fixed-size 3x3 matrix type of doubles, and \c MatrixXf is a dynamic-size matrix of
+ * floats.
+ *
+ * There are also \c VectorSizeType and \c RowVectorSizeType which are self-explanatory. For example, \c Vector4cf is
+ * a fixed-size vector of 4 complex floats.
+ *
+ * With \cpp11, template alias are also defined for common sizes.
+ * They follow the same pattern as above except that the scalar type suffix is replaced by a
+ * template parameter, i.e.:
+ *   - `MatrixSize<Type>` where `Size` can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size.
+ *   - `MatrixXSize<Type>` and `MatrixSizeX<Type>` where `Size` can be \c 2,\c 3,\c 4 for hybrid dynamic/fixed matrices.
+ *   - `VectorSize<Type>` and `RowVectorSize<Type>` for column and row vectors.
+ *
+ * With \cpp11, you can also use fully generic column and row vector types: `Vector<Type,Size>` and
+ * `RowVector<Type,Size>`.
+ *
+ * \sa class Matrix
+ */
 
-#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)   \
-/** \ingroup matrixtypedefs */                                    \
-typedef Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix;  \
-/** \ingroup matrixtypedefs */                                    \
-typedef Matrix<Type, Size, 1>    Vector##SizeSuffix##TypeSuffix;  \
-/** \ingroup matrixtypedefs */                                    \
-typedef Matrix<Type, 1, Size>    RowVector##SizeSuffix##TypeSuffix;
+#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)    \
+  /** \ingroup matrixtypedefs */                                   \
+  /** \brief `Size`&times;`Size` matrix of type `Type`. */         \
+  typedef Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
+  /** \ingroup matrixtypedefs */                                   \
+  /** \brief `Size`&times;`1` vector of type `Type`. */            \
+  typedef Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix;    \
+  /** \ingroup matrixtypedefs */                                   \
+  /** \brief `1`&times;`Size` vector of type `Type`. */            \
+  typedef Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
 
-#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size)         \
-/** \ingroup matrixtypedefs */                                    \
-typedef Matrix<Type, Size, Dynamic> Matrix##Size##X##TypeSuffix;  \
-/** \ingroup matrixtypedefs */                                    \
-typedef Matrix<Type, Dynamic, Size> Matrix##X##Size##TypeSuffix;
+#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size)          \
+  /** \ingroup matrixtypedefs */                                   \
+  /** \brief `Size`&times;`Dynamic` matrix of type `Type`. */      \
+  typedef Matrix<Type, Size, Dynamic> Matrix##Size##X##TypeSuffix; \
+  /** \ingroup matrixtypedefs */                                   \
+  /** \brief `Dynamic`&times;`Size` matrix of type `Type`. */      \
+  typedef Matrix<Type, Dynamic, Size> Matrix##X##Size##TypeSuffix;
 
 #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
-EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
-EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
-EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
-EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X) \
-EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 2) \
-EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 3) \
-EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 4)
+  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2)           \
+  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3)           \
+  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4)           \
+  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)     \
+  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 2)        \
+  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 3)        \
+  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 4)
 
-EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int,                  i)
-EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float,                f)
-EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double,               d)
-EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>,  cf)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
 
 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
 #undef EIGEN_MAKE_TYPEDEFS
 #undef EIGEN_MAKE_FIXED_TYPEDEFS
 
-#if EIGEN_HAS_CXX11
+#define EIGEN_MAKE_TYPEDEFS(Size, SizeSuffix)                    \
+  /** \ingroup matrixtypedefs */                                 \
+  /** \brief \cpp11 `Size`&times;`Size` matrix of type `Type`.*/ \
+  template <typename Type>                                       \
+  using Matrix##SizeSuffix = Matrix<Type, Size, Size>;           \
+  /** \ingroup matrixtypedefs */                                 \
+  /** \brief \cpp11 `Size`&times;`1` vector of type `Type`.*/    \
+  template <typename Type>                                       \
+  using Vector##SizeSuffix = Matrix<Type, Size, 1>;              \
+  /** \ingroup matrixtypedefs */                                 \
+  /** \brief \cpp11 `1`&times;`Size` vector of type `Type`.*/    \
+  template <typename Type>                                       \
+  using RowVector##SizeSuffix = Matrix<Type, 1, Size>;
 
-#define EIGEN_MAKE_TYPEDEFS(Size, SizeSuffix)                     \
-/** \ingroup matrixtypedefs */                                    \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Matrix##SizeSuffix = Matrix<Type, Size, Size>;              \
-/** \ingroup matrixtypedefs */                                    \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Vector##SizeSuffix = Matrix<Type, Size, 1>;                 \
-/** \ingroup matrixtypedefs */                                    \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using RowVector##SizeSuffix = Matrix<Type, 1, Size>;
-
-#define EIGEN_MAKE_FIXED_TYPEDEFS(Size)                           \
-/** \ingroup matrixtypedefs */                                    \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Matrix##Size##X = Matrix<Type, Size, Dynamic>;              \
-/** \ingroup matrixtypedefs */                                    \
-/** \brief \cpp11 */                                              \
-template <typename Type>                                          \
-using Matrix##X##Size = Matrix<Type, Dynamic, Size>;
+#define EIGEN_MAKE_FIXED_TYPEDEFS(Size)                              \
+  /** \ingroup matrixtypedefs */                                     \
+  /** \brief \cpp11 `Size`&times;`Dynamic` matrix of type `Type` */  \
+  template <typename Type>                                           \
+  using Matrix##Size##X = Matrix<Type, Size, Dynamic>;               \
+  /** \ingroup matrixtypedefs */                                     \
+  /** \brief \cpp11 `Dynamic`&times;`Size` matrix of type `Type`. */ \
+  template <typename Type>                                           \
+  using Matrix##X##Size = Matrix<Type, Dynamic, Size>;
 
 EIGEN_MAKE_TYPEDEFS(2, 2)
 EIGEN_MAKE_TYPEDEFS(3, 3)
@@ -546,20 +510,18 @@
 EIGEN_MAKE_FIXED_TYPEDEFS(4)
 
 /** \ingroup matrixtypedefs
-  * \brief \cpp11 */
+ * \brief \cpp11 `Size`&times;`1` vector of type `Type`. */
 template <typename Type, int Size>
 using Vector = Matrix<Type, Size, 1>;
 
 /** \ingroup matrixtypedefs
-  * \brief \cpp11 */
+ * \brief \cpp11 `1`&times;`Size` vector of type `Type`. */
 template <typename Type, int Size>
 using RowVector = Matrix<Type, 1, Size>;
 
 #undef EIGEN_MAKE_TYPEDEFS
 #undef EIGEN_MAKE_FIXED_TYPEDEFS
 
-#endif // EIGEN_HAS_CXX11
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_MATRIX_H
+#endif  // EIGEN_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h
index 45c3a59..81d5a97 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h
@@ -11,6 +11,9 @@
 #ifndef EIGEN_MATRIXBASE_H
 #define EIGEN_MATRIXBASE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class MatrixBase
@@ -45,503 +48,495 @@
   *
   * \sa \blank \ref TopicClassHierarchy
   */
-template<typename Derived> class MatrixBase
-  : public DenseBase<Derived>
-{
-  public:
+template <typename Derived>
+class MatrixBase : public DenseBase<Derived> {
+ public:
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef MatrixBase StorageBaseType;
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef MatrixBase StorageBaseType;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    typedef DenseBase<Derived> Base;
-    using Base::RowsAtCompileTime;
-    using Base::ColsAtCompileTime;
-    using Base::SizeAtCompileTime;
-    using Base::MaxRowsAtCompileTime;
-    using Base::MaxColsAtCompileTime;
-    using Base::MaxSizeAtCompileTime;
-    using Base::IsVectorAtCompileTime;
-    using Base::Flags;
+  typedef DenseBase<Derived> Base;
+  using Base::ColsAtCompileTime;
+  using Base::Flags;
+  using Base::IsVectorAtCompileTime;
+  using Base::MaxColsAtCompileTime;
+  using Base::MaxRowsAtCompileTime;
+  using Base::MaxSizeAtCompileTime;
+  using Base::RowsAtCompileTime;
+  using Base::SizeAtCompileTime;
 
-    using Base::derived;
-    using Base::const_cast_derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
-    using Base::lazyAssign;
-    using Base::eval;
-    using Base::operator-;
-    using Base::operator+=;
-    using Base::operator-=;
-    using Base::operator*=;
-    using Base::operator/=;
+  using Base::coeff;
+  using Base::coeffRef;
+  using Base::cols;
+  using Base::const_cast_derived;
+  using Base::derived;
+  using Base::eval;
+  using Base::lazyAssign;
+  using Base::rows;
+  using Base::size;
+  using Base::operator-;
+  using Base::operator+=;
+  using Base::operator-=;
+  using Base::operator*=;
+  using Base::operator/=;
 
-    typedef typename Base::CoeffReturnType CoeffReturnType;
-    typedef typename Base::ConstTransposeReturnType ConstTransposeReturnType;
-    typedef typename Base::RowXpr RowXpr;
-    typedef typename Base::ColXpr ColXpr;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-
+  typedef typename Base::CoeffReturnType CoeffReturnType;
+  typedef typename Base::ConstTransposeReturnType ConstTransposeReturnType;
+  typedef typename Base::RowXpr RowXpr;
+  typedef typename Base::ColXpr ColXpr;
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** type of the equivalent square matrix */
-    typedef Matrix<Scalar,EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime),
-                          EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+  /** type of the equivalent square matrix */
+  typedef Matrix<Scalar, internal::max_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime),
+                 internal::max_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime)>
+      SquareMatrixType;
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
-    /** \returns the size of the main diagonal, which is min(rows(),cols()).
-      * \sa rows(), cols(), SizeAtCompileTime. */
-    EIGEN_DEVICE_FUNC
-    inline Index diagonalSize() const { return (numext::mini)(rows(),cols()); }
+  /** \returns the size of the main diagonal, which is min(rows(),cols()).
+   * \sa rows(), cols(), SizeAtCompileTime. */
+  EIGEN_DEVICE_FUNC inline Index diagonalSize() const { return (numext::mini)(rows(), cols()); }
 
-    typedef typename Base::PlainObject PlainObject;
+  typedef typename Base::PlainObject PlainObject;
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal Represents a matrix with all coefficients equal to one another*/
-    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
-    /** \internal the return type of MatrixBase::adjoint() */
-    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-                        CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, ConstTransposeReturnType>,
-                        ConstTransposeReturnType
-                     >::type AdjointReturnType;
-    /** \internal Return type of eigenvalues() */
-    typedef Matrix<std::complex<RealScalar>, internal::traits<Derived>::ColsAtCompileTime, 1, ColMajor> EigenvaluesReturnType;
-    /** \internal the return type of identity */
-    typedef CwiseNullaryOp<internal::scalar_identity_op<Scalar>,PlainObject> IdentityReturnType;
-    /** \internal the return type of unit vectors */
-    typedef Block<const CwiseNullaryOp<internal::scalar_identity_op<Scalar>, SquareMatrixType>,
-                  internal::traits<Derived>::RowsAtCompileTime,
-                  internal::traits<Derived>::ColsAtCompileTime> BasisReturnType;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+  /** \internal Represents a matrix with all coefficients equal to one another*/
+  typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> ConstantReturnType;
+  /** \internal the return type of MatrixBase::adjoint() */
+  typedef std::conditional_t<NumTraits<Scalar>::IsComplex,
+                             CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, ConstTransposeReturnType>,
+                             ConstTransposeReturnType>
+      AdjointReturnType;
+  /** \internal Return type of eigenvalues() */
+  typedef Matrix<std::complex<RealScalar>, internal::traits<Derived>::ColsAtCompileTime, 1, ColMajor>
+      EigenvaluesReturnType;
+  /** \internal the return type of identity */
+  typedef CwiseNullaryOp<internal::scalar_identity_op<Scalar>, PlainObject> IdentityReturnType;
+  /** \internal the return type of unit vectors */
+  typedef Block<const CwiseNullaryOp<internal::scalar_identity_op<Scalar>, SquareMatrixType>,
+                internal::traits<Derived>::RowsAtCompileTime, internal::traits<Derived>::ColsAtCompileTime>
+      BasisReturnType;
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::MatrixBase
-#define EIGEN_DOC_UNARY_ADDONS(X,Y)
-#   include "../plugins/CommonCwiseBinaryOps.h"
-#   include "../plugins/MatrixCwiseUnaryOps.h"
-#   include "../plugins/MatrixCwiseBinaryOps.h"
-#   ifdef EIGEN_MATRIXBASE_PLUGIN
-#     include EIGEN_MATRIXBASE_PLUGIN
-#   endif
+#define EIGEN_DOC_UNARY_ADDONS(X, Y)
+#include "../plugins/CommonCwiseBinaryOps.inc"
+#include "../plugins/MatrixCwiseUnaryOps.inc"
+#include "../plugins/MatrixCwiseBinaryOps.inc"
+#ifdef EIGEN_MATRIXBASE_PLUGIN
+#include EIGEN_MATRIXBASE_PLUGIN
+#endif
 #undef EIGEN_CURRENT_STORAGE_BASE_CLASS
 #undef EIGEN_DOC_UNARY_ADDONS
 
-    /** Special case of the template operator=, in order to prevent the compiler
-      * from generating a default operator= (issue hit with g++ 4.1)
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const MatrixBase& other);
+  /** Special case of the template operator=, in order to prevent the compiler
+   * from generating a default operator= (issue hit with g++ 4.1)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const MatrixBase& other);
 
-    // We cannot inherit here via Base::operator= since it is causing
-    // trouble with MSVC.
+  // We cannot inherit here via Base::operator= since it is causing
+  // trouble with MSVC.
 
-    template <typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const DenseBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase<OtherDerived>& other);
 
-    template <typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const EigenBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC Derived& operator=(const EigenBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const ReturnByValue<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC Derived& operator=(const ReturnByValue<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator+=(const MatrixBase<OtherDerived>& other);
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator-=(const MatrixBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const MatrixBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const MatrixBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<Derived,OtherDerived>
-    operator*(const MatrixBase<OtherDerived> &other) const;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC const Product<Derived, OtherDerived> operator*(const MatrixBase<OtherDerived>& other) const;
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<Derived,OtherDerived,LazyProduct>
-    lazyProduct(const MatrixBase<OtherDerived> &other) const;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC const Product<Derived, OtherDerived, LazyProduct> lazyProduct(
+      const MatrixBase<OtherDerived>& other) const;
 
-    template<typename OtherDerived>
-    Derived& operator*=(const EigenBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  Derived& operator*=(const EigenBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    void applyOnTheLeft(const EigenBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  void applyOnTheLeft(const EigenBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    void applyOnTheRight(const EigenBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  void applyOnTheRight(const EigenBase<OtherDerived>& other);
 
-    template<typename DiagonalDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<Derived, DiagonalDerived, LazyProduct>
-    operator*(const DiagonalBase<DiagonalDerived> &diagonal) const;
+  template <typename DiagonalDerived>
+  EIGEN_DEVICE_FUNC const Product<Derived, DiagonalDerived, LazyProduct> operator*(
+      const DiagonalBase<DiagonalDerived>& diagonal) const;
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
-    dot(const MatrixBase<OtherDerived>& other) const;
+  template <typename SkewDerived>
+  EIGEN_DEVICE_FUNC const Product<Derived, SkewDerived, LazyProduct> operator*(
+      const SkewSymmetricBase<SkewDerived>& skew) const;
 
-    EIGEN_DEVICE_FUNC RealScalar squaredNorm() const;
-    EIGEN_DEVICE_FUNC RealScalar norm() const;
-    RealScalar stableNorm() const;
-    RealScalar blueNorm() const;
-    RealScalar hypotNorm() const;
-    EIGEN_DEVICE_FUNC const PlainObject normalized() const;
-    EIGEN_DEVICE_FUNC const PlainObject stableNormalized() const;
-    EIGEN_DEVICE_FUNC void normalize();
-    EIGEN_DEVICE_FUNC void stableNormalize();
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,
+                                                  typename internal::traits<OtherDerived>::Scalar>::ReturnType
+  dot(const MatrixBase<OtherDerived>& other) const;
 
-    EIGEN_DEVICE_FUNC const AdjointReturnType adjoint() const;
-    EIGEN_DEVICE_FUNC void adjointInPlace();
+  EIGEN_DEVICE_FUNC RealScalar squaredNorm() const;
+  EIGEN_DEVICE_FUNC RealScalar norm() const;
+  RealScalar stableNorm() const;
+  RealScalar blueNorm() const;
+  RealScalar hypotNorm() const;
+  EIGEN_DEVICE_FUNC const PlainObject normalized() const;
+  EIGEN_DEVICE_FUNC const PlainObject stableNormalized() const;
+  EIGEN_DEVICE_FUNC void normalize();
+  EIGEN_DEVICE_FUNC void stableNormalize();
 
-    typedef Diagonal<Derived> DiagonalReturnType;
-    EIGEN_DEVICE_FUNC
-    DiagonalReturnType diagonal();
+  EIGEN_DEVICE_FUNC const AdjointReturnType adjoint() const;
+  EIGEN_DEVICE_FUNC void adjointInPlace();
 
-    typedef typename internal::add_const<Diagonal<const Derived> >::type ConstDiagonalReturnType;
-    EIGEN_DEVICE_FUNC
-    ConstDiagonalReturnType diagonal() const;
+  typedef Diagonal<Derived> DiagonalReturnType;
+  EIGEN_DEVICE_FUNC DiagonalReturnType diagonal();
 
-    template<int Index> struct DiagonalIndexReturnType { typedef Diagonal<Derived,Index> Type; };
-    template<int Index> struct ConstDiagonalIndexReturnType { typedef const Diagonal<const Derived,Index> Type; };
+  typedef Diagonal<const Derived> ConstDiagonalReturnType;
+  EIGEN_DEVICE_FUNC const ConstDiagonalReturnType diagonal() const;
 
-    template<int Index>
-    EIGEN_DEVICE_FUNC
-    typename DiagonalIndexReturnType<Index>::Type diagonal();
+  template <int Index>
+  EIGEN_DEVICE_FUNC Diagonal<Derived, Index> diagonal();
 
-    template<int Index>
-    EIGEN_DEVICE_FUNC
-    typename ConstDiagonalIndexReturnType<Index>::Type diagonal() const;
+  template <int Index>
+  EIGEN_DEVICE_FUNC const Diagonal<const Derived, Index> diagonal() const;
 
-    typedef Diagonal<Derived,DynamicIndex> DiagonalDynamicIndexReturnType;
-    typedef typename internal::add_const<Diagonal<const Derived,DynamicIndex> >::type ConstDiagonalDynamicIndexReturnType;
+  EIGEN_DEVICE_FUNC Diagonal<Derived, DynamicIndex> diagonal(Index index);
+  EIGEN_DEVICE_FUNC const Diagonal<const Derived, DynamicIndex> diagonal(Index index) const;
 
-    EIGEN_DEVICE_FUNC
-    DiagonalDynamicIndexReturnType diagonal(Index index);
-    EIGEN_DEVICE_FUNC
-    ConstDiagonalDynamicIndexReturnType diagonal(Index index) const;
+  template <unsigned int Mode>
+  struct TriangularViewReturnType {
+    typedef TriangularView<Derived, Mode> Type;
+  };
+  template <unsigned int Mode>
+  struct ConstTriangularViewReturnType {
+    typedef const TriangularView<const Derived, Mode> Type;
+  };
 
-    template<unsigned int Mode> struct TriangularViewReturnType { typedef TriangularView<Derived, Mode> Type; };
-    template<unsigned int Mode> struct ConstTriangularViewReturnType { typedef const TriangularView<const Derived, Mode> Type; };
+  template <unsigned int Mode>
+  EIGEN_DEVICE_FUNC typename TriangularViewReturnType<Mode>::Type triangularView();
+  template <unsigned int Mode>
+  EIGEN_DEVICE_FUNC typename ConstTriangularViewReturnType<Mode>::Type triangularView() const;
 
-    template<unsigned int Mode>
-    EIGEN_DEVICE_FUNC
-    typename TriangularViewReturnType<Mode>::Type triangularView();
-    template<unsigned int Mode>
-    EIGEN_DEVICE_FUNC
-    typename ConstTriangularViewReturnType<Mode>::Type triangularView() const;
+  template <unsigned int UpLo>
+  struct SelfAdjointViewReturnType {
+    typedef SelfAdjointView<Derived, UpLo> Type;
+  };
+  template <unsigned int UpLo>
+  struct ConstSelfAdjointViewReturnType {
+    typedef const SelfAdjointView<const Derived, UpLo> Type;
+  };
 
-    template<unsigned int UpLo> struct SelfAdjointViewReturnType { typedef SelfAdjointView<Derived, UpLo> Type; };
-    template<unsigned int UpLo> struct ConstSelfAdjointViewReturnType { typedef const SelfAdjointView<const Derived, UpLo> Type; };
+  template <unsigned int UpLo>
+  EIGEN_DEVICE_FUNC typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
+  template <unsigned int UpLo>
+  EIGEN_DEVICE_FUNC typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
 
-    template<unsigned int UpLo>
-    EIGEN_DEVICE_FUNC
-    typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
-    template<unsigned int UpLo>
-    EIGEN_DEVICE_FUNC
-    typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
+  const SparseView<Derived> sparseView(
+      const Scalar& m_reference = Scalar(0),
+      const typename NumTraits<Scalar>::Real& m_epsilon = NumTraits<Scalar>::dummy_precision()) const;
+  EIGEN_DEVICE_FUNC static const IdentityReturnType Identity();
+  EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(Index rows, Index cols);
+  EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index size, Index i);
+  EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index i);
+  EIGEN_DEVICE_FUNC static const BasisReturnType UnitX();
+  EIGEN_DEVICE_FUNC static const BasisReturnType UnitY();
+  EIGEN_DEVICE_FUNC static const BasisReturnType UnitZ();
+  EIGEN_DEVICE_FUNC static const BasisReturnType UnitW();
 
-    const SparseView<Derived> sparseView(const Scalar& m_reference = Scalar(0),
-                                         const typename NumTraits<Scalar>::Real& m_epsilon = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC static const IdentityReturnType Identity();
-    EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index size, Index i);
-    EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index i);
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitX();
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitY();
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitZ();
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitW();
+  EIGEN_DEVICE_FUNC const DiagonalWrapper<const Derived> asDiagonal() const;
+  const PermutationWrapper<const Derived> asPermutation() const;
+  EIGEN_DEVICE_FUNC const SkewSymmetricWrapper<const Derived> asSkewSymmetric() const;
 
-    EIGEN_DEVICE_FUNC
-    const DiagonalWrapper<const Derived> asDiagonal() const;
-    const PermutationWrapper<const Derived> asPermutation() const;
+  EIGEN_DEVICE_FUNC Derived& setIdentity();
+  EIGEN_DEVICE_FUNC Derived& setIdentity(Index rows, Index cols);
+  EIGEN_DEVICE_FUNC Derived& setUnit(Index i);
+  EIGEN_DEVICE_FUNC Derived& setUnit(Index newSize, Index i);
 
-    EIGEN_DEVICE_FUNC
-    Derived& setIdentity();
-    EIGEN_DEVICE_FUNC
-    Derived& setIdentity(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC Derived& setUnit(Index i);
-    EIGEN_DEVICE_FUNC Derived& setUnit(Index newSize, Index i);
+  bool isIdentity(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  bool isDiagonal(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
 
-    bool isIdentity(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    bool isDiagonal(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  bool isUpperTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  bool isLowerTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
 
-    bool isUpperTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    bool isLowerTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  bool isSkewSymmetric(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
 
-    template<typename OtherDerived>
-    bool isOrthogonal(const MatrixBase<OtherDerived>& other,
-                      const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    bool isUnitary(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  template <typename OtherDerived>
+  bool isOrthogonal(const MatrixBase<OtherDerived>& other,
+                    const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  bool isUnitary(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
 
-    /** \returns true if each coefficients of \c *this and \a other are all exactly equal.
-      * \warning When using floating point scalar values you probably should rather use a
-      *          fuzzy comparison such as isApprox()
-      * \sa isApprox(), operator!= */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC inline bool operator==(const MatrixBase<OtherDerived>& other) const
-    { return cwiseEqual(other).all(); }
+  /** \returns true if each coefficients of \c *this and \a other are all exactly equal.
+   * \warning When using floating point scalar values you probably should rather use a
+   *          fuzzy comparison such as isApprox()
+   * \sa isApprox(), operator!= */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline bool operator==(const MatrixBase<OtherDerived>& other) const {
+    return cwiseEqual(other).all();
+  }
 
-    /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other.
-      * \warning When using floating point scalar values you probably should rather use a
-      *          fuzzy comparison such as isApprox()
-      * \sa isApprox(), operator== */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC inline bool operator!=(const MatrixBase<OtherDerived>& other) const
-    { return cwiseNotEqual(other).any(); }
+  /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other.
+   * \warning When using floating point scalar values you probably should rather use a
+   *          fuzzy comparison such as isApprox()
+   * \sa isApprox(), operator== */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline bool operator!=(const MatrixBase<OtherDerived>& other) const {
+    return cwiseNotEqual(other).any();
+  }
 
-    NoAlias<Derived,Eigen::MatrixBase > EIGEN_DEVICE_FUNC noalias();
+  NoAlias<Derived, Eigen::MatrixBase> EIGEN_DEVICE_FUNC noalias();
 
-    // TODO forceAlignedAccess is temporarily disabled
-    // Need to find a nicer workaround.
-    inline const Derived& forceAlignedAccess() const { return derived(); }
-    inline Derived& forceAlignedAccess() { return derived(); }
-    template<bool Enable> inline const Derived& forceAlignedAccessIf() const { return derived(); }
-    template<bool Enable> inline Derived& forceAlignedAccessIf() { return derived(); }
+  // TODO forceAlignedAccess is temporarily disabled
+  // Need to find a nicer workaround.
+  inline const Derived& forceAlignedAccess() const { return derived(); }
+  inline Derived& forceAlignedAccess() { return derived(); }
+  template <bool Enable>
+  inline const Derived& forceAlignedAccessIf() const {
+    return derived();
+  }
+  template <bool Enable>
+  inline Derived& forceAlignedAccessIf() {
+    return derived();
+  }
 
-    EIGEN_DEVICE_FUNC Scalar trace() const;
+  EIGEN_DEVICE_FUNC Scalar trace() const;
+
+  template <int p>
+  EIGEN_DEVICE_FUNC RealScalar lpNorm() const;
+
+  EIGEN_DEVICE_FUNC MatrixBase<Derived>& matrix() { return *this; }
+  EIGEN_DEVICE_FUNC const MatrixBase<Derived>& matrix() const { return *this; }
+
+  /** \returns an \link Eigen::ArrayBase Array \endlink expression of this matrix
+   * \sa ArrayBase::matrix() */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ArrayWrapper<Derived> array() { return ArrayWrapper<Derived>(derived()); }
+  /** \returns a const \link Eigen::ArrayBase Array \endlink expression of this matrix
+   * \sa ArrayBase::matrix() */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ArrayWrapper<const Derived> array() const {
+    return ArrayWrapper<const Derived>(derived());
+  }
 
-    template<int p> EIGEN_DEVICE_FUNC RealScalar lpNorm() const;
+  /////////// LU module ///////////
 
-    EIGEN_DEVICE_FUNC MatrixBase<Derived>& matrix() { return *this; }
-    EIGEN_DEVICE_FUNC const MatrixBase<Derived>& matrix() const { return *this; }
+  template <typename PermutationIndex = DefaultPermutationIndex>
+  inline const FullPivLU<PlainObject, PermutationIndex> fullPivLu() const;
+  template <typename PermutationIndex = DefaultPermutationIndex>
+  inline const PartialPivLU<PlainObject, PermutationIndex> partialPivLu() const;
 
-    /** \returns an \link Eigen::ArrayBase Array \endlink expression of this matrix
-      * \sa ArrayBase::matrix() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ArrayWrapper<Derived> array() { return ArrayWrapper<Derived>(derived()); }
-    /** \returns a const \link Eigen::ArrayBase Array \endlink expression of this matrix
-      * \sa ArrayBase::matrix() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ArrayWrapper<const Derived> array() const { return ArrayWrapper<const Derived>(derived()); }
+  template <typename PermutationIndex = DefaultPermutationIndex>
+  inline const PartialPivLU<PlainObject, PermutationIndex> lu() const;
 
-/////////// LU module ///////////
+  EIGEN_DEVICE_FUNC inline const Inverse<Derived> inverse() const;
 
-    inline const FullPivLU<PlainObject> fullPivLu() const;
-    inline const PartialPivLU<PlainObject> partialPivLu() const;
+  template <typename ResultType>
+  inline void computeInverseAndDetWithCheck(
+      ResultType& inverse, typename ResultType::Scalar& determinant, bool& invertible,
+      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()) const;
 
-    inline const PartialPivLU<PlainObject> lu() const;
+  template <typename ResultType>
+  inline void computeInverseWithCheck(
+      ResultType& inverse, bool& invertible,
+      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()) const;
 
-    EIGEN_DEVICE_FUNC
-    inline const Inverse<Derived> inverse() const;
+  EIGEN_DEVICE_FUNC Scalar determinant() const;
 
-    template<typename ResultType>
-    inline void computeInverseAndDetWithCheck(
-      ResultType& inverse,
-      typename ResultType::Scalar& determinant,
-      bool& invertible,
-      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
-    ) const;
+  /////////// Cholesky module ///////////
 
-    template<typename ResultType>
-    inline void computeInverseWithCheck(
-      ResultType& inverse,
-      bool& invertible,
-      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
-    ) const;
+  inline const LLT<PlainObject> llt() const;
+  inline const LDLT<PlainObject> ldlt() const;
 
-    EIGEN_DEVICE_FUNC
-    Scalar determinant() const;
+  /////////// QR module ///////////
 
-/////////// Cholesky module ///////////
+  inline const HouseholderQR<PlainObject> householderQr() const;
+  template <typename PermutationIndex = DefaultPermutationIndex>
+  inline const ColPivHouseholderQR<PlainObject, PermutationIndex> colPivHouseholderQr() const;
+  template <typename PermutationIndex = DefaultPermutationIndex>
+  inline const FullPivHouseholderQR<PlainObject, PermutationIndex> fullPivHouseholderQr() const;
+  template <typename PermutationIndex = DefaultPermutationIndex>
+  inline const CompleteOrthogonalDecomposition<PlainObject, PermutationIndex> completeOrthogonalDecomposition() const;
 
-    inline const LLT<PlainObject>  llt() const;
-    inline const LDLT<PlainObject> ldlt() const;
+  /////////// Eigenvalues module ///////////
 
-/////////// QR module ///////////
+  inline EigenvaluesReturnType eigenvalues() const;
+  inline RealScalar operatorNorm() const;
 
-    inline const HouseholderQR<PlainObject> householderQr() const;
-    inline const ColPivHouseholderQR<PlainObject> colPivHouseholderQr() const;
-    inline const FullPivHouseholderQR<PlainObject> fullPivHouseholderQr() const;
-    inline const CompleteOrthogonalDecomposition<PlainObject> completeOrthogonalDecomposition() const;
+  /////////// SVD module ///////////
 
-/////////// Eigenvalues module ///////////
+  template <int Options = 0>
+  inline JacobiSVD<PlainObject, Options> jacobiSvd() const;
+  template <int Options = 0>
+  EIGEN_DEPRECATED inline JacobiSVD<PlainObject, Options> jacobiSvd(unsigned int computationOptions) const;
 
-    inline EigenvaluesReturnType eigenvalues() const;
-    inline RealScalar operatorNorm() const;
+  template <int Options = 0>
+  inline BDCSVD<PlainObject, Options> bdcSvd() const;
+  template <int Options = 0>
+  EIGEN_DEPRECATED inline BDCSVD<PlainObject, Options> bdcSvd(unsigned int computationOptions) const;
 
-/////////// SVD module ///////////
+  /////////// Geometry module ///////////
 
-    inline JacobiSVD<PlainObject> jacobiSvd(unsigned int computationOptions = 0) const;
-    inline BDCSVD<PlainObject>    bdcSvd(unsigned int computationOptions = 0) const;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline typename internal::cross_impl<Derived, OtherDerived>::return_type cross(
+      const MatrixBase<OtherDerived>& other) const;
 
-/////////// Geometry module ///////////
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline PlainObject cross3(const MatrixBase<OtherDerived>& other) const;
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /// \internal helper struct to form the return type of the cross product
-    template<typename OtherDerived> struct cross_product_return_type {
-      typedef typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType Scalar;
-      typedef Matrix<Scalar,MatrixBase::RowsAtCompileTime,MatrixBase::ColsAtCompileTime> type;
-    };
-    #endif // EIGEN_PARSED_BY_DOXYGEN
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    inline typename cross_product_return_type<OtherDerived>::type
-#else
-    inline PlainObject
-#endif
-    cross(const MatrixBase<OtherDerived>& other) const;
+  EIGEN_DEVICE_FUNC inline PlainObject unitOrthogonal(void) const;
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    inline PlainObject cross3(const MatrixBase<OtherDerived>& other) const;
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> eulerAngles(Index a0, Index a1, Index a2) const;
 
-    EIGEN_DEVICE_FUNC
-    inline PlainObject unitOrthogonal(void) const;
+  EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> canonicalEulerAngles(Index a0, Index a1, Index a2) const;
 
-    EIGEN_DEVICE_FUNC
-    inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2) const;
+  // put this as separate enum value to work around possible GCC 4.3 bug (?)
+  enum {
+    HomogeneousReturnTypeDirection =
+        ColsAtCompileTime == 1 && RowsAtCompileTime == 1
+            ? ((internal::traits<Derived>::Flags & RowMajorBit) == RowMajorBit ? Horizontal : Vertical)
+        : ColsAtCompileTime == 1 ? Vertical
+                                 : Horizontal
+  };
+  typedef Homogeneous<Derived, HomogeneousReturnTypeDirection> HomogeneousReturnType;
+  EIGEN_DEVICE_FUNC inline HomogeneousReturnType homogeneous() const;
 
-    // put this as separate enum value to work around possible GCC 4.3 bug (?)
-    enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits<Derived>::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical)
-                                          : ColsAtCompileTime==1 ? Vertical : Horizontal };
-    typedef Homogeneous<Derived, HomogeneousReturnTypeDirection> HomogeneousReturnType;
-    EIGEN_DEVICE_FUNC
-    inline HomogeneousReturnType homogeneous() const;
+  enum { SizeMinusOne = SizeAtCompileTime == Dynamic ? Dynamic : SizeAtCompileTime - 1 };
+  typedef Block<const Derived, internal::traits<Derived>::ColsAtCompileTime == 1 ? SizeMinusOne : 1,
+                internal::traits<Derived>::ColsAtCompileTime == 1 ? 1 : SizeMinusOne>
+      ConstStartMinusOne;
+  typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(ConstStartMinusOne, Scalar, quotient) HNormalizedReturnType;
+  EIGEN_DEVICE_FUNC inline const HNormalizedReturnType hnormalized() const;
 
-    enum {
-      SizeMinusOne = SizeAtCompileTime==Dynamic ? Dynamic : SizeAtCompileTime-1
-    };
-    typedef Block<const Derived,
-                  internal::traits<Derived>::ColsAtCompileTime==1 ? SizeMinusOne : 1,
-                  internal::traits<Derived>::ColsAtCompileTime==1 ? 1 : SizeMinusOne> ConstStartMinusOne;
-    typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(ConstStartMinusOne,Scalar,quotient) HNormalizedReturnType;
-    EIGEN_DEVICE_FUNC
-    inline const HNormalizedReturnType hnormalized() const;
+  ////////// Householder module ///////////
 
-////////// Householder module ///////////
+  EIGEN_DEVICE_FUNC void makeHouseholderInPlace(Scalar& tau, RealScalar& beta);
+  template <typename EssentialPart>
+  EIGEN_DEVICE_FUNC void makeHouseholder(EssentialPart& essential, Scalar& tau, RealScalar& beta) const;
+  template <typename EssentialPart>
+  EIGEN_DEVICE_FUNC void applyHouseholderOnTheLeft(const EssentialPart& essential, const Scalar& tau,
+                                                   Scalar* workspace);
+  template <typename EssentialPart>
+  EIGEN_DEVICE_FUNC void applyHouseholderOnTheRight(const EssentialPart& essential, const Scalar& tau,
+                                                    Scalar* workspace);
 
-    EIGEN_DEVICE_FUNC
-    void makeHouseholderInPlace(Scalar& tau, RealScalar& beta);
-    template<typename EssentialPart>
-    EIGEN_DEVICE_FUNC
-    void makeHouseholder(EssentialPart& essential,
-                         Scalar& tau, RealScalar& beta) const;
-    template<typename EssentialPart>
-    EIGEN_DEVICE_FUNC
-    void applyHouseholderOnTheLeft(const EssentialPart& essential,
-                                   const Scalar& tau,
-                                   Scalar* workspace);
-    template<typename EssentialPart>
-    EIGEN_DEVICE_FUNC
-    void applyHouseholderOnTheRight(const EssentialPart& essential,
-                                    const Scalar& tau,
-                                    Scalar* workspace);
+  ///////// Jacobi module /////////
 
-///////// Jacobi module /////////
+  template <typename OtherScalar>
+  EIGEN_DEVICE_FUNC void applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j);
+  template <typename OtherScalar>
+  EIGEN_DEVICE_FUNC void applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j);
 
-    template<typename OtherScalar>
-    EIGEN_DEVICE_FUNC
-    void applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j);
-    template<typename OtherScalar>
-    EIGEN_DEVICE_FUNC
-    void applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j);
+  ///////// SparseCore module /////////
 
-///////// SparseCore module /////////
+  template <typename OtherDerived>
+  EIGEN_STRONG_INLINE const typename SparseMatrixBase<OtherDerived>::template CwiseProductDenseReturnType<Derived>::Type
+  cwiseProduct(const SparseMatrixBase<OtherDerived>& other) const {
+    return other.cwiseProduct(derived());
+  }
 
-    template<typename OtherDerived>
-    EIGEN_STRONG_INLINE const typename SparseMatrixBase<OtherDerived>::template CwiseProductDenseReturnType<Derived>::Type
-    cwiseProduct(const SparseMatrixBase<OtherDerived> &other) const
-    {
-      return other.cwiseProduct(derived());
-    }
+  ///////// MatrixFunctions module /////////
 
-///////// MatrixFunctions module /////////
+  typedef typename internal::stem_function<Scalar>::type StemFunction;
+#define EIGEN_MATRIX_FUNCTION(ReturnType, Name, Description)                                                        \
+  /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a            \
+   * href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the \
+   * coefficient-wise Description use ArrayBase::##Name . */                                                        \
+  const ReturnType<Derived> Name() const;
+#define EIGEN_MATRIX_FUNCTION_1(ReturnType, Name, Description, Argument)                                            \
+  /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a            \
+   * href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the \
+   * coefficient-wise Description use ArrayBase::##Name . */                                                        \
+  const ReturnType<Derived> Name(Argument) const;
 
-    typedef typename internal::stem_function<Scalar>::type StemFunction;
-#define EIGEN_MATRIX_FUNCTION(ReturnType, Name, Description) \
-    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
-    const ReturnType<Derived> Name() const;
-#define EIGEN_MATRIX_FUNCTION_1(ReturnType, Name, Description, Argument) \
-    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
-    const ReturnType<Derived> Name(Argument) const;
+  EIGEN_MATRIX_FUNCTION(MatrixExponentialReturnValue, exp, exponential)
+  /** \brief Helper function for the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported
+   * MatrixFunctions module</a>.*/
+  const MatrixFunctionReturnValue<Derived> matrixFunction(StemFunction f) const;
+  EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cosh, hyperbolic cosine)
+  EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sinh, hyperbolic sine)
+  EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, atanh, inverse hyperbolic cosine)
+  EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, acosh, inverse hyperbolic cosine)
+  EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, asinh, inverse hyperbolic sine)
+  EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cos, cosine)
+  EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sin, sine)
+  EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root)
+  EIGEN_MATRIX_FUNCTION(MatrixLogarithmReturnValue, log, logarithm)
+  EIGEN_MATRIX_FUNCTION_1(MatrixPowerReturnValue, pow, power to \c p, const RealScalar& p)
+  EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex<RealScalar>& p)
 
-    EIGEN_MATRIX_FUNCTION(MatrixExponentialReturnValue, exp, exponential)
-    /** \brief Helper function for the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>.*/
-    const MatrixFunctionReturnValue<Derived> matrixFunction(StemFunction f) const;
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cosh, hyperbolic cosine)
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sinh, hyperbolic sine)
-#if EIGEN_HAS_CXX11_MATH
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, atanh, inverse hyperbolic cosine)
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, acosh, inverse hyperbolic cosine)
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, asinh, inverse hyperbolic sine)
-#endif
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cos, cosine)
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sin, sine)
-    EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root)
-    EIGEN_MATRIX_FUNCTION(MatrixLogarithmReturnValue, log, logarithm)
-    EIGEN_MATRIX_FUNCTION_1(MatrixPowerReturnValue,        pow, power to \c p, const RealScalar& p)
-    EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex<RealScalar>& p)
+ protected:
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(MatrixBase)
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MatrixBase)
 
-  protected:
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(MatrixBase)
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MatrixBase)
+ private:
+  EIGEN_DEVICE_FUNC explicit MatrixBase(int);
+  EIGEN_DEVICE_FUNC MatrixBase(int, int);
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC explicit MatrixBase(const MatrixBase<OtherDerived>&);
 
-  private:
-    EIGEN_DEVICE_FUNC explicit MatrixBase(int);
-    EIGEN_DEVICE_FUNC MatrixBase(int,int);
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC explicit MatrixBase(const MatrixBase<OtherDerived>&);
-  protected:
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator+=(const ArrayBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator-=(const ArrayBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+ protected:
+  // mixing arrays and matrices is not legal
+  template <typename OtherDerived>
+  Derived& operator+=(const ArrayBase<OtherDerived>&) {
+    EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
+                        YOU_CANNOT_MIX_ARRAYS_AND_MATRICES);
+    return *this;
+  }
+  // mixing arrays and matrices is not legal
+  template <typename OtherDerived>
+  Derived& operator-=(const ArrayBase<OtherDerived>&) {
+    EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
+                        YOU_CANNOT_MIX_ARRAYS_AND_MATRICES);
+    return *this;
+  }
 };
 
-
 /***************************************************************************
-* Implementation of matrix base methods
-***************************************************************************/
+ * Implementation of matrix base methods
+ ***************************************************************************/
 
 /** replaces \c *this by \c *this * \a other.
-  *
-  * \returns a reference to \c *this
-  *
-  * Example: \include MatrixBase_applyOnTheRight.cpp
-  * Output: \verbinclude MatrixBase_applyOnTheRight.out
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline Derived&
-MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived> &other)
-{
+ *
+ * \returns a reference to \c *this
+ *
+ * Example: \include MatrixBase_applyOnTheRight.cpp
+ * Output: \verbinclude MatrixBase_applyOnTheRight.out
+ */
+template <typename Derived>
+template <typename OtherDerived>
+inline Derived& MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived>& other) {
   other.derived().applyThisOnTheRight(derived());
   return derived();
 }
 
 /** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=().
-  *
-  * Example: \include MatrixBase_applyOnTheRight.cpp
-  * Output: \verbinclude MatrixBase_applyOnTheRight.out
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived> &other)
-{
+ *
+ * Example: \include MatrixBase_applyOnTheRight.cpp
+ * Output: \verbinclude MatrixBase_applyOnTheRight.out
+ */
+template <typename Derived>
+template <typename OtherDerived>
+inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived>& other) {
   other.derived().applyThisOnTheRight(derived());
 }
 
 /** replaces \c *this by \a other * \c *this.
-  *
-  * Example: \include MatrixBase_applyOnTheLeft.cpp
-  * Output: \verbinclude MatrixBase_applyOnTheLeft.out
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived> &other)
-{
+ *
+ * Example: \include MatrixBase_applyOnTheLeft.cpp
+ * Output: \verbinclude MatrixBase_applyOnTheLeft.out
+ */
+template <typename Derived>
+template <typename OtherDerived>
+inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived>& other) {
   other.derived().applyThisOnTheLeft(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATRIXBASE_H
+#endif  // EIGEN_MATRIXBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h
index b427576..ec360eb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h
@@ -11,75 +11,81 @@
 #ifndef EIGEN_NESTBYVALUE_H
 #define EIGEN_NESTBYVALUE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename ExpressionType>
-struct traits<NestByValue<ExpressionType> > : public traits<ExpressionType>
-{
-  enum {
-    Flags = traits<ExpressionType>::Flags & ~NestByRefBit
-  };
+template <typename ExpressionType>
+struct traits<NestByValue<ExpressionType> > : public traits<ExpressionType> {
+  enum { Flags = traits<ExpressionType>::Flags & ~NestByRefBit };
 };
-}
+}  // namespace internal
 
 /** \class NestByValue
-  * \ingroup Core_Module
-  *
-  * \brief Expression which must be nested by value
-  *
-  * \tparam ExpressionType the type of the object of which we are requiring nesting-by-value
-  *
-  * This class is the return type of MatrixBase::nestByValue()
-  * and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::nestByValue()
-  */
-template<typename ExpressionType> class NestByValue
-  : public internal::dense_xpr_base< NestByValue<ExpressionType> >::type
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Expression which must be nested by value
+ *
+ * \tparam ExpressionType the type of the object of which we are requiring nesting-by-value
+ *
+ * This class is the return type of MatrixBase::nestByValue()
+ * and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::nestByValue()
+ */
+template <typename ExpressionType>
+class NestByValue : public internal::dense_xpr_base<NestByValue<ExpressionType> >::type {
+ public:
+  typedef typename internal::dense_xpr_base<NestByValue>::type Base;
+  static constexpr bool HasDirectAccess = internal::has_direct_access<ExpressionType>::ret;
 
-    typedef typename internal::dense_xpr_base<NestByValue>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(NestByValue)
+  EIGEN_DENSE_PUBLIC_INTERFACE(NestByValue)
 
-    EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {}
+  EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
 
-    EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; }
+  EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; }
 
-    EIGEN_DEVICE_FUNC const ExpressionType& nestedExpression() const { return m_expression; }
+  EIGEN_DEVICE_FUNC const ExpressionType& nestedExpression() const { return m_expression; }
 
-  protected:
-    const ExpressionType m_expression;
+  EIGEN_DEVICE_FUNC typename std::enable_if<HasDirectAccess, const Scalar*>::type data() const {
+    return m_expression.data();
+  }
+
+  EIGEN_DEVICE_FUNC typename std::enable_if<HasDirectAccess, Index>::type innerStride() const {
+    return m_expression.innerStride();
+  }
+
+  EIGEN_DEVICE_FUNC typename std::enable_if<HasDirectAccess, Index>::type outerStride() const {
+    return m_expression.outerStride();
+  }
+
+ protected:
+  const ExpressionType m_expression;
 };
 
 /** \returns an expression of the temporary version of *this.
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline const NestByValue<Derived>
-DenseBase<Derived>::nestByValue() const
-{
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const NestByValue<Derived> DenseBase<Derived>::nestByValue() const {
   return NestByValue<Derived>(derived());
 }
 
 namespace internal {
 
 // Evaluator of Solve -> eval into a temporary
-template<typename ArgType>
-struct evaluator<NestByValue<ArgType> >
-  : public evaluator<ArgType>
-{
+template <typename ArgType>
+struct evaluator<NestByValue<ArgType> > : public evaluator<ArgType> {
   typedef evaluator<ArgType> Base;
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const NestByValue<ArgType>& xpr)
-    : Base(xpr.nestedExpression())
-  {}
+  EIGEN_DEVICE_FUNC explicit evaluator(const NestByValue<ArgType>& xpr) : Base(xpr.nestedExpression()) {}
 };
-}
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_NESTBYVALUE_H
+#endif  // EIGEN_NESTBYVALUE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h
index 570283d..b6c7209 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h
@@ -10,100 +10,93 @@
 #ifndef EIGEN_NOALIAS_H
 #define EIGEN_NOALIAS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class NoAlias
-  * \ingroup Core_Module
-  *
-  * \brief Pseudo expression providing an operator = assuming no aliasing
-  *
-  * \tparam ExpressionType the type of the object on which to do the lazy assignment
-  *
-  * This class represents an expression with special assignment operators
-  * assuming no aliasing between the target expression and the source expression.
-  * More precisely it alloas to bypass the EvalBeforeAssignBit flag of the source expression.
-  * It is the return type of MatrixBase::noalias()
-  * and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::noalias()
-  */
-template<typename ExpressionType, template <typename> class StorageBase>
-class NoAlias
-{
-  public:
-    typedef typename ExpressionType::Scalar Scalar;
-    
-    EIGEN_DEVICE_FUNC
-    explicit NoAlias(ExpressionType& expression) : m_expression(expression) {}
-    
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE ExpressionType& operator=(const StorageBase<OtherDerived>& other)
-    {
-      call_assignment_no_alias(m_expression, other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
-      return m_expression;
-    }
-    
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE ExpressionType& operator+=(const StorageBase<OtherDerived>& other)
-    {
-      call_assignment_no_alias(m_expression, other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
-      return m_expression;
-    }
-    
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE ExpressionType& operator-=(const StorageBase<OtherDerived>& other)
-    {
-      call_assignment_no_alias(m_expression, other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
-      return m_expression;
-    }
+ * \ingroup Core_Module
+ *
+ * \brief Pseudo expression providing an operator = assuming no aliasing
+ *
+ * \tparam ExpressionType the type of the object on which to do the lazy assignment
+ *
+ * This class represents an expression with special assignment operators
+ * assuming no aliasing between the target expression and the source expression.
+ * More precisely it alloas to bypass the EvalBeforeAssignBit flag of the source expression.
+ * It is the return type of MatrixBase::noalias()
+ * and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::noalias()
+ */
+template <typename ExpressionType, template <typename> class StorageBase>
+class NoAlias {
+ public:
+  typedef typename ExpressionType::Scalar Scalar;
 
-    EIGEN_DEVICE_FUNC
-    ExpressionType& expression() const
-    {
-      return m_expression;
-    }
+  EIGEN_DEVICE_FUNC explicit NoAlias(ExpressionType& expression) : m_expression(expression) {}
 
-  protected:
-    ExpressionType& m_expression;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator=(const StorageBase<OtherDerived>& other) {
+    call_assignment_no_alias(m_expression, other.derived(),
+                             internal::assign_op<Scalar, typename OtherDerived::Scalar>());
+    return m_expression;
+  }
+
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator+=(const StorageBase<OtherDerived>& other) {
+    call_assignment_no_alias(m_expression, other.derived(),
+                             internal::add_assign_op<Scalar, typename OtherDerived::Scalar>());
+    return m_expression;
+  }
+
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator-=(const StorageBase<OtherDerived>& other) {
+    call_assignment_no_alias(m_expression, other.derived(),
+                             internal::sub_assign_op<Scalar, typename OtherDerived::Scalar>());
+    return m_expression;
+  }
+
+  EIGEN_DEVICE_FUNC ExpressionType& expression() const { return m_expression; }
+
+ protected:
+  ExpressionType& m_expression;
 };
 
 /** \returns a pseudo expression of \c *this with an operator= assuming
-  * no aliasing between \c *this and the source expression.
-  *
-  * More precisely, noalias() allows to bypass the EvalBeforeAssignBit flag.
-  * Currently, even though several expressions may alias, only product
-  * expressions have this flag. Therefore, noalias() is only useful when
-  * the source expression contains a matrix product.
-  *
-  * Here are some examples where noalias is useful:
-  * \code
-  * D.noalias()  = A * B;
-  * D.noalias() += A.transpose() * B;
-  * D.noalias() -= 2 * A * B.adjoint();
-  * \endcode
-  *
-  * On the other hand the following example will lead to a \b wrong result:
-  * \code
-  * A.noalias() = A * B;
-  * \endcode
-  * because the result matrix A is also an operand of the matrix product. Therefore,
-  * there is no alternative than evaluating A * B in a temporary, that is the default
-  * behavior when you write:
-  * \code
-  * A = A * B;
-  * \endcode
-  *
-  * \sa class NoAlias
-  */
-template<typename Derived>
-NoAlias<Derived,MatrixBase> EIGEN_DEVICE_FUNC MatrixBase<Derived>::noalias()
-{
-  return NoAlias<Derived, Eigen::MatrixBase >(derived());
+ * no aliasing between \c *this and the source expression.
+ *
+ * More precisely, noalias() allows to bypass the EvalBeforeAssignBit flag.
+ * Currently, even though several expressions may alias, only product
+ * expressions have this flag. Therefore, noalias() is only useful when
+ * the source expression contains a matrix product.
+ *
+ * Here are some examples where noalias is useful:
+ * \code
+ * D.noalias()  = A * B;
+ * D.noalias() += A.transpose() * B;
+ * D.noalias() -= 2 * A * B.adjoint();
+ * \endcode
+ *
+ * On the other hand the following example will lead to a \b wrong result:
+ * \code
+ * A.noalias() = A * B;
+ * \endcode
+ * because the result matrix A is also an operand of the matrix product. Therefore,
+ * there is no alternative than evaluating A * B in a temporary, that is the default
+ * behavior when you write:
+ * \code
+ * A = A * B;
+ * \endcode
+ *
+ * \sa class NoAlias
+ */
+template <typename Derived>
+NoAlias<Derived, MatrixBase> EIGEN_DEVICE_FUNC MatrixBase<Derived>::noalias() {
+  return NoAlias<Derived, Eigen::MatrixBase>(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_NOALIAS_H
+#endif  // EIGEN_NOALIAS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
index 72eac5a..80f74e9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
@@ -10,72 +10,89 @@
 #ifndef EIGEN_NUMTRAITS_H
 #define EIGEN_NUMTRAITS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-// default implementation of digits10(), based on numeric_limits if specialized,
-// 0 for integer types, and log10(epsilon()) otherwise.
-template< typename T,
-          bool use_numeric_limits = std::numeric_limits<T>::is_specialized,
-          bool is_integer = NumTraits<T>::IsInteger>
-struct default_digits10_impl
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int run() { return std::numeric_limits<T>::digits10; }
-};
-
-template<typename T>
-struct default_digits10_impl<T,false,false> // Floating point
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int run() {
-    using std::log10;
-    using std::ceil;
-    typedef typename NumTraits<T>::Real Real;
-    return int(ceil(-log10(NumTraits<Real>::epsilon())));
-  }
-};
-
-template<typename T>
-struct default_digits10_impl<T,false,true> // Integer
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int run() { return 0; }
-};
-
-
 // default implementation of digits(), based on numeric_limits if specialized,
 // 0 for integer types, and log2(epsilon()) otherwise.
-template< typename T,
-          bool use_numeric_limits = std::numeric_limits<T>::is_specialized,
+template <typename T, bool use_numeric_limits = std::numeric_limits<T>::is_specialized,
           bool is_integer = NumTraits<T>::IsInteger>
-struct default_digits_impl
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int run() { return std::numeric_limits<T>::digits; }
+struct default_digits_impl {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits<T>::digits; }
 };
 
-template<typename T>
-struct default_digits_impl<T,false,false> // Floating point
+template <typename T>
+struct default_digits_impl<T, false, false>  // Floating point
 {
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int run() {
-    using std::log;
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() {
     using std::ceil;
+    using std::log2;
     typedef typename NumTraits<T>::Real Real;
-    return int(ceil(-log(NumTraits<Real>::epsilon())/log(static_cast<Real>(2))));
+    return int(ceil(-log2(NumTraits<Real>::epsilon())));
   }
 };
 
-template<typename T>
-struct default_digits_impl<T,false,true> // Integer
+template <typename T>
+struct default_digits_impl<T, false, true>  // Integer
 {
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int run() { return 0; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; }
 };
 
-} // end namespace internal
+// default implementation of digits10(), based on numeric_limits if specialized,
+// 0 for integer types, and floor((digits()-1)*log10(2)) otherwise.
+template <typename T, bool use_numeric_limits = std::numeric_limits<T>::is_specialized,
+          bool is_integer = NumTraits<T>::IsInteger>
+struct default_digits10_impl {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits<T>::digits10; }
+};
+
+template <typename T>
+struct default_digits10_impl<T, false, false>  // Floating point
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() {
+    using std::floor;
+    using std::log10;
+    typedef typename NumTraits<T>::Real Real;
+    return int(floor((internal::default_digits_impl<Real>::run() - 1) * log10(2)));
+  }
+};
+
+template <typename T>
+struct default_digits10_impl<T, false, true>  // Integer
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; }
+};
+
+// default implementation of max_digits10(), based on numeric_limits if specialized,
+// 0 for integer types, and log10(2) * digits() + 1 otherwise.
+template <typename T, bool use_numeric_limits = std::numeric_limits<T>::is_specialized,
+          bool is_integer = NumTraits<T>::IsInteger>
+struct default_max_digits10_impl {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits<T>::max_digits10; }
+};
+
+template <typename T>
+struct default_max_digits10_impl<T, false, false>  // Floating point
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() {
+    using std::ceil;
+    using std::log10;
+    typedef typename NumTraits<T>::Real Real;
+    return int(ceil(internal::default_digits_impl<Real>::run() * log10(2) + 1));
+  }
+};
+
+template <typename T>
+struct default_max_digits10_impl<T, false, true>  // Integer
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; }
+};
+
+}  // end namespace internal
 
 namespace numext {
 /** \internal bit-wise cast without changing the underlying bit representation. */
@@ -83,74 +100,76 @@
 // TODO: Replace by std::bit_cast (available in C++20)
 template <typename Tgt, typename Src>
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Tgt bit_cast(const Src& src) {
-#if EIGEN_HAS_TYPE_TRAITS
   // The behaviour of memcpy is not specified for non-trivially copyable types
   EIGEN_STATIC_ASSERT(std::is_trivially_copyable<Src>::value, THIS_TYPE_IS_NOT_SUPPORTED);
   EIGEN_STATIC_ASSERT(std::is_trivially_copyable<Tgt>::value && std::is_default_constructible<Tgt>::value,
                       THIS_TYPE_IS_NOT_SUPPORTED);
-#endif
-
   EIGEN_STATIC_ASSERT(sizeof(Src) == sizeof(Tgt), THIS_TYPE_IS_NOT_SUPPORTED);
+
   Tgt tgt;
+  // Load src into registers first. This allows the memcpy to be elided by CUDA.
+  const Src staged = src;
   EIGEN_USING_STD(memcpy)
-  memcpy(&tgt, &src, sizeof(Tgt));
+  memcpy(static_cast<void*>(&tgt), static_cast<const void*>(&staged), sizeof(Tgt));
   return tgt;
 }
 }  // namespace numext
 
 /** \class NumTraits
-  * \ingroup Core_Module
-  *
-  * \brief Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
-  *
-  * \tparam T the numeric type at hand
-  *
-  * This class stores enums, typedefs and static methods giving information about a numeric type.
-  *
-  * The provided data consists of:
-  * \li A typedef \c Real, giving the "real part" type of \a T. If \a T is already real,
-  *     then \c Real is just a typedef to \a T. If \a T is \c std::complex<U> then \c Real
-  *     is a typedef to \a U.
-  * \li A typedef \c NonInteger, giving the type that should be used for operations producing non-integral values,
-  *     such as quotients, square roots, etc. If \a T is a floating-point type, then this typedef just gives
-  *     \a T again. Note however that many Eigen functions such as internal::sqrt simply refuse to
-  *     take integers. Outside of a few cases, Eigen doesn't do automatic type promotion. Thus, this typedef is
-  *     only intended as a helper for code that needs to explicitly promote types.
-  * \li A typedef \c Literal giving the type to use for numeric literals such as "2" or "0.5". For instance, for \c std::complex<U>, Literal is defined as \c U.
-  *     Of course, this type must be fully compatible with \a T. In doubt, just use \a T here.
-  * \li A typedef \a Nested giving the type to use to nest a value inside of the expression tree. If you don't know what
-  *     this means, just use \a T here.
-  * \li An enum value \a IsComplex. It is equal to 1 if \a T is a \c std::complex
-  *     type, and to 0 otherwise.
-  * \li An enum value \a IsInteger. It is equal to \c 1 if \a T is an integer type such as \c int,
-  *     and to \c 0 otherwise.
-  * \li Enum values ReadCost, AddCost and MulCost representing a rough estimate of the number of CPU cycles needed
-  *     to by move / add / mul instructions respectively, assuming the data is already stored in CPU registers.
-  *     Stay vague here. No need to do architecture-specific stuff. If you don't know what this means, just use \c Eigen::HugeCost.
-  * \li An enum value \a IsSigned. It is equal to \c 1 if \a T is a signed type and to 0 if \a T is unsigned.
-  * \li An enum value \a RequireInitialization. It is equal to \c 1 if the constructor of the numeric type \a T must
-  *     be called, and to 0 if it is safe not to call it. Default is 0 if \a T is an arithmetic type, and 1 otherwise.
-  * \li An epsilon() function which, unlike <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/epsilon">std::numeric_limits::epsilon()</a>,
-  *     it returns a \a Real instead of a \a T.
-  * \li A dummy_precision() function returning a weak epsilon value. It is mainly used as a default
-  *     value by the fuzzy comparison operators.
-  * \li highest() and lowest() functions returning the highest and lowest possible values respectively.
-  * \li digits() function returning the number of radix digits (non-sign digits for integers, mantissa for floating-point). This is
-  *     the analogue of <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/digits">std::numeric_limits<T>::digits</a>
-  *     which is used as the default implementation if specialized.
-  * \li digits10() function returning the number of decimal digits that can be represented without change. This is
-  *     the analogue of <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/digits10">std::numeric_limits<T>::digits10</a>
-  *     which is used as the default implementation if specialized.
-  * \li min_exponent() and max_exponent() functions returning the highest and lowest possible values, respectively,
-  *     such that the radix raised to the power exponent-1 is a normalized floating-point number.  These are equivalent to
-  *     <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/min_exponent">std::numeric_limits<T>::min_exponent</a>/
-  *     <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/max_exponent">std::numeric_limits<T>::max_exponent</a>.
-  * \li infinity() function returning a representation of positive infinity, if available.
-  * \li quiet_NaN function returning a non-signaling "not-a-number", if available.
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
+ *
+ * \tparam T the numeric type at hand
+ *
+ * This class stores enums, typedefs and static methods giving information about a numeric type.
+ *
+ * The provided data consists of:
+ * \li A typedef \c Real, giving the "real part" type of \a T. If \a T is already real,
+ *     then \c Real is just a typedef to \a T. If \a T is \c std::complex<U> then \c Real
+ *     is a typedef to \a U.
+ * \li A typedef \c NonInteger, giving the type that should be used for operations producing non-integral values,
+ *     such as quotients, square roots, etc. If \a T is a floating-point type, then this typedef just gives
+ *     \a T again. Note however that many Eigen functions such as internal::sqrt simply refuse to
+ *     take integers. Outside of a few cases, Eigen doesn't do automatic type promotion. Thus, this typedef is
+ *     only intended as a helper for code that needs to explicitly promote types.
+ * \li A typedef \c Literal giving the type to use for numeric literals such as "2" or "0.5". For instance, for \c
+ * std::complex<U>, Literal is defined as \c U. Of course, this type must be fully compatible with \a T. In doubt, just
+ * use \a T here. \li A typedef \a Nested giving the type to use to nest a value inside of the expression tree. If you
+ * don't know what this means, just use \a T here. \li An enum value \a IsComplex. It is equal to 1 if \a T is a \c
+ * std::complex type, and to 0 otherwise. \li An enum value \a IsInteger. It is equal to \c 1 if \a T is an integer type
+ * such as \c int, and to \c 0 otherwise. \li Enum values ReadCost, AddCost and MulCost representing a rough estimate of
+ * the number of CPU cycles needed to by move / add / mul instructions respectively, assuming the data is already stored
+ * in CPU registers. Stay vague here. No need to do architecture-specific stuff. If you don't know what this means, just
+ * use \c Eigen::HugeCost. \li An enum value \a IsSigned. It is equal to \c 1 if \a T is a signed type and to 0 if \a T
+ * is unsigned. \li An enum value \a RequireInitialization. It is equal to \c 1 if the constructor of the numeric type
+ * \a T must be called, and to 0 if it is safe not to call it. Default is 0 if \a T is an arithmetic type, and 1
+ * otherwise. \li An epsilon() function which, unlike <a
+ * href="http://en.cppreference.com/w/cpp/types/numeric_limits/epsilon">std::numeric_limits::epsilon()</a>, it returns a
+ * \a Real instead of a \a T. \li A dummy_precision() function returning a weak epsilon value. It is mainly used as a
+ * default value by the fuzzy comparison operators. \li highest() and lowest() functions returning the highest and
+ * lowest possible values respectively. \li digits() function returning the number of radix digits (non-sign digits for
+ * integers, mantissa for floating-point). This is the analogue of <a
+ * href="http://en.cppreference.com/w/cpp/types/numeric_limits/digits">std::numeric_limits<T>::digits</a> which is used
+ * as the default implementation if specialized. \li digits10() function returning the number of decimal digits that can
+ * be represented without change. This is the analogue of <a
+ * href="http://en.cppreference.com/w/cpp/types/numeric_limits/digits10">std::numeric_limits<T>::digits10</a> which is
+ * used as the default implementation if specialized. \li max_digits10() function returning the number of decimal digits
+ * required to uniquely represent all distinct values of the type. This is the analogue of <a
+ * href="http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10">std::numeric_limits<T>::max_digits10</a>
+ *     which is used as the default implementation if specialized.
+ * \li min_exponent() and max_exponent() functions returning the highest and lowest possible values, respectively,
+ *     such that the radix raised to the power exponent-1 is a normalized floating-point number.  These are equivalent
+ * to <a
+ * href="http://en.cppreference.com/w/cpp/types/numeric_limits/min_exponent">std::numeric_limits<T>::min_exponent</a>/
+ *     <a
+ * href="http://en.cppreference.com/w/cpp/types/numeric_limits/max_exponent">std::numeric_limits<T>::max_exponent</a>.
+ * \li infinity() function returning a representation of positive infinity, if available.
+ * \li quiet_NaN function returning a non-signaling "not-a-number", if available.
+ */
 
-template<typename T> struct GenericNumTraits
-{
+template <typename T>
+struct GenericNumTraits {
   enum {
     IsInteger = std::numeric_limits<T>::is_integer,
     IsSigned = std::numeric_limits<T>::is_signed,
@@ -162,161 +181,134 @@
   };
 
   typedef T Real;
-  typedef typename internal::conditional<
-                     IsInteger,
-                     typename internal::conditional<sizeof(T)<=2, float, double>::type,
-                     T
-                   >::type NonInteger;
+  typedef std::conditional_t<IsInteger, std::conditional_t<sizeof(T) <= 2, float, double>, T> NonInteger;
   typedef T Nested;
   typedef T Literal;
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline Real epsilon()
-  {
-    return numext::numeric_limits<T>::epsilon();
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real epsilon() { return numext::numeric_limits<T>::epsilon(); }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits10() { return internal::default_digits10_impl<T>::run(); }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_digits10() {
+    return internal::default_max_digits10_impl<T>::run();
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline int digits10()
-  {
-    return internal::default_digits10_impl<T>::run();
-  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits() { return internal::default_digits_impl<T>::run(); }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline int digits()
-  {
-    return internal::default_digits_impl<T>::run();
-  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int min_exponent() { return numext::numeric_limits<T>::min_exponent; }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline int min_exponent()
-  {
-    return numext::numeric_limits<T>::min_exponent;
-  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_exponent() { return numext::numeric_limits<T>::max_exponent; }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline int max_exponent()
-  {
-    return numext::numeric_limits<T>::max_exponent;
-  }
-
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline Real dummy_precision()
-  {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real dummy_precision() {
     // make sure to override this for floating-point types
     return Real(0);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline T highest() {
-    return (numext::numeric_limits<T>::max)();
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T highest() { return (numext::numeric_limits<T>::max)(); }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T lowest() {
+    return IsInteger ? (numext::numeric_limits<T>::min)() : static_cast<T>(-(numext::numeric_limits<T>::max)());
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline T lowest()  {
-    return IsInteger ? (numext::numeric_limits<T>::min)()
-                     : static_cast<T>(-(numext::numeric_limits<T>::max)());
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T infinity() { return numext::numeric_limits<T>::infinity(); }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T quiet_NaN() { return numext::numeric_limits<T>::quiet_NaN(); }
+};
+
+template <typename T>
+struct NumTraits : GenericNumTraits<T> {};
+
+template <>
+struct NumTraits<float> : GenericNumTraits<float> {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline float dummy_precision() { return 1e-5f; }
+};
+
+template <>
+struct NumTraits<double> : GenericNumTraits<double> {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline double dummy_precision() { return 1e-12; }
+};
+
+// GPU devices treat `long double` as `double`.
+#ifndef EIGEN_GPU_COMPILE_PHASE
+template <>
+struct NumTraits<long double> : GenericNumTraits<long double> {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline long double dummy_precision() {
+    return static_cast<long double>(1e-15l);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline T infinity() {
-    return numext::numeric_limits<T>::infinity();
+#if defined(EIGEN_ARCH_PPC) && (__LDBL_MANT_DIG__ == 106)
+  // PowerPC double double causes issues with some values
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline long double epsilon() {
+    // 2^(-(__LDBL_MANT_DIG__)+1)
+    return static_cast<long double>(2.4651903288156618919116517665087e-32l);
   }
-
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline T quiet_NaN() {
-    return numext::numeric_limits<T>::quiet_NaN();
-  }
+#endif
 };
+#endif
 
-template<typename T> struct NumTraits : GenericNumTraits<T>
-{};
-
-template<> struct NumTraits<float>
-  : GenericNumTraits<float>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline float dummy_precision() { return 1e-5f; }
-};
-
-template<> struct NumTraits<double> : GenericNumTraits<double>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline double dummy_precision() { return 1e-12; }
-};
-
-template<> struct NumTraits<long double>
-  : GenericNumTraits<long double>
-{
-  EIGEN_CONSTEXPR
-  static inline long double dummy_precision() { return 1e-15l; }
-};
-
-template<typename _Real> struct NumTraits<std::complex<_Real> >
-  : GenericNumTraits<std::complex<_Real> >
-{
-  typedef _Real Real;
-  typedef typename NumTraits<_Real>::Literal Literal;
+template <typename Real_>
+struct NumTraits<std::complex<Real_> > : GenericNumTraits<std::complex<Real_> > {
+  typedef Real_ Real;
+  typedef typename NumTraits<Real_>::Literal Literal;
   enum {
     IsComplex = 1,
-    RequireInitialization = NumTraits<_Real>::RequireInitialization,
-    ReadCost = 2 * NumTraits<_Real>::ReadCost,
+    RequireInitialization = NumTraits<Real_>::RequireInitialization,
+    ReadCost = 2 * NumTraits<Real_>::ReadCost,
     AddCost = 2 * NumTraits<Real>::AddCost,
     MulCost = 4 * NumTraits<Real>::MulCost + 2 * NumTraits<Real>::AddCost
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline Real epsilon() { return NumTraits<Real>::epsilon(); }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline Real dummy_precision() { return NumTraits<Real>::dummy_precision(); }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline int digits10() { return NumTraits<Real>::digits10(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real epsilon() { return NumTraits<Real>::epsilon(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real dummy_precision() { return NumTraits<Real>::dummy_precision(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits10() { return NumTraits<Real>::digits10(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_digits10() { return NumTraits<Real>::max_digits10(); }
 };
 
-template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-struct NumTraits<Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> >
-{
+template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+struct NumTraits<Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> > {
   typedef Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> ArrayType;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef Array<RealScalar, Rows, Cols, Options, MaxRows, MaxCols> Real;
   typedef typename NumTraits<Scalar>::NonInteger NonIntegerScalar;
   typedef Array<NonIntegerScalar, Rows, Cols, Options, MaxRows, MaxCols> NonInteger;
-  typedef ArrayType & Nested;
+  typedef ArrayType& Nested;
   typedef typename NumTraits<Scalar>::Literal Literal;
 
   enum {
     IsComplex = NumTraits<Scalar>::IsComplex,
     IsInteger = NumTraits<Scalar>::IsInteger,
-    IsSigned  = NumTraits<Scalar>::IsSigned,
+    IsSigned = NumTraits<Scalar>::IsSigned,
     RequireInitialization = 1,
-    ReadCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::ReadCost),
-    AddCost  = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::AddCost),
-    MulCost  = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::MulCost)
+    ReadCost = ArrayType::SizeAtCompileTime == Dynamic
+                   ? HugeCost
+                   : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::ReadCost),
+    AddCost = ArrayType::SizeAtCompileTime == Dynamic ? HugeCost
+                                                      : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::AddCost),
+    MulCost = ArrayType::SizeAtCompileTime == Dynamic ? HugeCost
+                                                      : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::MulCost)
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline RealScalar epsilon() { return NumTraits<RealScalar>::epsilon(); }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static inline RealScalar dummy_precision() { return NumTraits<RealScalar>::dummy_precision(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline RealScalar epsilon() { return NumTraits<RealScalar>::epsilon(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline RealScalar dummy_precision() {
+    return NumTraits<RealScalar>::dummy_precision();
+  }
 
   EIGEN_CONSTEXPR
   static inline int digits10() { return NumTraits<Scalar>::digits10(); }
+  EIGEN_CONSTEXPR
+  static inline int max_digits10() { return NumTraits<Scalar>::max_digits10(); }
 };
 
-template<> struct NumTraits<std::string>
-  : GenericNumTraits<std::string>
-{
-  enum {
-    RequireInitialization = 1,
-    ReadCost = HugeCost,
-    AddCost  = HugeCost,
-    MulCost  = HugeCost
-  };
+template <>
+struct NumTraits<std::string> : GenericNumTraits<std::string> {
+  enum { RequireInitialization = 1, ReadCost = HugeCost, AddCost = HugeCost, MulCost = HugeCost };
 
   EIGEN_CONSTEXPR
   static inline int digits10() { return 0; }
+  EIGEN_CONSTEXPR
+  static inline int max_digits10() { return 0; }
 
-private:
+ private:
   static inline std::string epsilon();
   static inline std::string dummy_precision();
   static inline std::string lowest();
@@ -326,10 +318,12 @@
 };
 
 // Empty specialization for void to allow template specialization based on NumTraits<T>::Real with T==void and SFINAE.
-template<> struct NumTraits<void> {};
+template <>
+struct NumTraits<void> {};
 
-template<> struct NumTraits<bool> : GenericNumTraits<bool> {};
+template <>
+struct NumTraits<bool> : GenericNumTraits<bool> {};
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_NUMTRAITS_H
+#endif  // EIGEN_NUMTRAITS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h
index 29abf35..7b2c8dc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h
@@ -10,75 +10,74 @@
 #ifndef EIGEN_PARTIALREDUX_H
 #define EIGEN_PARTIALREDUX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-
 /***************************************************************************
-*
-* This file provides evaluators for partial reductions.
-* There are two modes:
-*
-*  - scalar path: simply calls the respective function on the column or row.
-*    -> nothing special here, all the tricky part is handled by the return
-*       types of VectorwiseOp's members. They embed the functor calling the
-*       respective DenseBase's member function.
-*
-*  - vectorized path: implements a packet-wise reductions followed by
-*    some (optional) processing of the outcome, e.g., division by n for mean.
-*
-* For the vectorized path let's observe that the packet-size and outer-unrolling
-* are both decided by the assignement logic. So all we have to do is to decide
-* on the inner unrolling.
-*
-* For the unrolling, we can reuse "internal::redux_vec_unroller" from Redux.h,
-* but be need to be careful to specify correct increment.
-*
-***************************************************************************/
-
+ *
+ * This file provides evaluators for partial reductions.
+ * There are two modes:
+ *
+ *  - scalar path: simply calls the respective function on the column or row.
+ *    -> nothing special here, all the tricky part is handled by the return
+ *       types of VectorwiseOp's members. They embed the functor calling the
+ *       respective DenseBase's member function.
+ *
+ *  - vectorized path: implements a packet-wise reductions followed by
+ *    some (optional) processing of the outcome, e.g., division by n for mean.
+ *
+ * For the vectorized path let's observe that the packet-size and outer-unrolling
+ * are both decided by the assignment logic. So all we have to do is to decide
+ * on the inner unrolling.
+ *
+ * For the unrolling, we can reuse "internal::redux_vec_unroller" from Redux.h,
+ * but be need to be careful to specify correct increment.
+ *
+ ***************************************************************************/
 
 /* logic deciding a strategy for unrolling of vectorized paths */
-template<typename Func, typename Evaluator>
-struct packetwise_redux_traits
-{
+template <typename Func, typename Evaluator>
+struct packetwise_redux_traits {
   enum {
     OuterSize = int(Evaluator::IsRowMajor) ? Evaluator::RowsAtCompileTime : Evaluator::ColsAtCompileTime,
     Cost = OuterSize == Dynamic ? HugeCost
-         : OuterSize * Evaluator::CoeffReadCost + (OuterSize-1) * functor_traits<Func>::Cost,
+                                : OuterSize * Evaluator::CoeffReadCost + (OuterSize - 1) * functor_traits<Func>::Cost,
     Unrolling = Cost <= EIGEN_UNROLLING_LIMIT ? CompleteUnrolling : NoUnrolling
   };
-
 };
 
 /* Value to be returned when size==0 , by default let's return 0 */
-template<typename PacketType,typename Func>
-EIGEN_DEVICE_FUNC
-PacketType packetwise_redux_empty_value(const Func& ) { return pset1<PacketType>(0); }
+template <typename PacketType, typename Func>
+EIGEN_DEVICE_FUNC PacketType packetwise_redux_empty_value(const Func&) {
+  const typename unpacket_traits<PacketType>::type zero(0);
+  return pset1<PacketType>(zero);
+}
 
 /* For products the default is 1 */
-template<typename PacketType,typename Scalar>
-EIGEN_DEVICE_FUNC
-PacketType packetwise_redux_empty_value(const scalar_product_op<Scalar,Scalar>& ) { return pset1<PacketType>(1); }
+template <typename PacketType, typename Scalar>
+EIGEN_DEVICE_FUNC PacketType packetwise_redux_empty_value(const scalar_product_op<Scalar, Scalar>&) {
+  return pset1<PacketType>(Scalar(1));
+}
 
 /* Perform the actual reduction */
-template<typename Func, typename Evaluator,
-         int Unrolling = packetwise_redux_traits<Func, Evaluator>::Unrolling
->
+template <typename Func, typename Evaluator, int Unrolling = packetwise_redux_traits<Func, Evaluator>::Unrolling>
 struct packetwise_redux_impl;
 
 /* Perform the actual reduction with unrolling */
-template<typename Func, typename Evaluator>
-struct packetwise_redux_impl<Func, Evaluator, CompleteUnrolling>
-{
-  typedef redux_novec_unroller<Func,Evaluator, 0, Evaluator::SizeAtCompileTime> Base;
+template <typename Func, typename Evaluator>
+struct packetwise_redux_impl<Func, Evaluator, CompleteUnrolling> {
+  typedef redux_novec_unroller<Func, Evaluator, 0, Evaluator::SizeAtCompileTime> Base;
   typedef typename Evaluator::Scalar Scalar;
 
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
-  PacketType run(const Evaluator &eval, const Func& func, Index /*size*/)
-  {
-    return redux_vec_unroller<Func, Evaluator, 0, packetwise_redux_traits<Func, Evaluator>::OuterSize>::template run<PacketType>(eval,func);
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func& func, Index /*size*/) {
+    return redux_vec_unroller<Func, Evaluator, 0,
+                              packetwise_redux_traits<Func, Evaluator>::OuterSize>::template run<PacketType>(eval,
+                                                                                                             func);
   }
 };
 
@@ -86,147 +85,125 @@
  * This specialization is not required for general reductions, which is
  * why it is defined here.
  */
-template<typename Func, typename Evaluator, int Start>
-struct redux_vec_unroller<Func, Evaluator, Start, 0>
-{
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE PacketType run(const Evaluator &, const Func& f)
-  {
+template <typename Func, typename Evaluator, Index Start>
+struct redux_vec_unroller<Func, Evaluator, Start, 0> {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator&, const Func& f) {
     return packetwise_redux_empty_value<PacketType>(f);
   }
 };
 
 /* Perform the actual reduction for dynamic sizes */
-template<typename Func, typename Evaluator>
-struct packetwise_redux_impl<Func, Evaluator, NoUnrolling>
-{
+template <typename Func, typename Evaluator>
+struct packetwise_redux_impl<Func, Evaluator, NoUnrolling> {
   typedef typename Evaluator::Scalar Scalar;
   typedef typename redux_traits<Func, Evaluator>::PacketType PacketScalar;
 
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC
-  static PacketType run(const Evaluator &eval, const Func& func, Index size)
-  {
-    if(size==0)
-      return packetwise_redux_empty_value<PacketType>(func);
-    
-    const Index size4 = (size-1)&(~3);
-    PacketType p = eval.template packetByOuterInner<Unaligned,PacketType>(0,0);
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC static PacketType run(const Evaluator& eval, const Func& func, Index size) {
+    if (size == 0) return packetwise_redux_empty_value<PacketType>(func);
+
+    const Index size4 = (size - 1) & (~3);
+    PacketType p = eval.template packetByOuterInner<Unaligned, PacketType>(0, 0);
     Index i = 1;
     // This loop is optimized for instruction pipelining:
     // - each iteration generates two independent instructions
     // - thanks to branch prediction and out-of-order execution we have independent instructions across loops
-    for(; i<size4; i+=4)
-      p = func.packetOp(p,
-            func.packetOp(
-              func.packetOp(eval.template packetByOuterInner<Unaligned,PacketType>(i+0,0),eval.template packetByOuterInner<Unaligned,PacketType>(i+1,0)),
-              func.packetOp(eval.template packetByOuterInner<Unaligned,PacketType>(i+2,0),eval.template packetByOuterInner<Unaligned,PacketType>(i+3,0))));
-    for(; i<size; ++i)
-      p = func.packetOp(p, eval.template packetByOuterInner<Unaligned,PacketType>(i,0));
+    for (; i < size4; i += 4)
+      p = func.packetOp(
+          p, func.packetOp(func.packetOp(eval.template packetByOuterInner<Unaligned, PacketType>(i + 0, 0),
+                                         eval.template packetByOuterInner<Unaligned, PacketType>(i + 1, 0)),
+                           func.packetOp(eval.template packetByOuterInner<Unaligned, PacketType>(i + 2, 0),
+                                         eval.template packetByOuterInner<Unaligned, PacketType>(i + 3, 0))));
+    for (; i < size; ++i) p = func.packetOp(p, eval.template packetByOuterInner<Unaligned, PacketType>(i, 0));
     return p;
   }
 };
 
-template< typename ArgType, typename MemberOp, int Direction>
+template <typename ArgType, typename MemberOp, int Direction>
 struct evaluator<PartialReduxExpr<ArgType, MemberOp, Direction> >
-  : evaluator_base<PartialReduxExpr<ArgType, MemberOp, Direction> >
-{
+    : evaluator_base<PartialReduxExpr<ArgType, MemberOp, Direction> > {
   typedef PartialReduxExpr<ArgType, MemberOp, Direction> XprType;
-  typedef typename internal::nested_eval<ArgType,1>::type ArgTypeNested;
-  typedef typename internal::add_const_on_value_type<ArgTypeNested>::type ConstArgTypeNested;
-  typedef typename internal::remove_all<ArgTypeNested>::type ArgTypeNestedCleaned;
+  typedef typename internal::nested_eval<ArgType, 1>::type ArgTypeNested;
+  typedef add_const_on_value_type_t<ArgTypeNested> ConstArgTypeNested;
+  typedef internal::remove_all_t<ArgTypeNested> ArgTypeNestedCleaned;
   typedef typename ArgType::Scalar InputScalar;
   typedef typename XprType::Scalar Scalar;
   enum {
-    TraversalSize = Direction==int(Vertical) ? int(ArgType::RowsAtCompileTime) :  int(ArgType::ColsAtCompileTime)
+    TraversalSize = Direction == int(Vertical) ? int(ArgType::RowsAtCompileTime) : int(ArgType::ColsAtCompileTime)
   };
   typedef typename MemberOp::template Cost<int(TraversalSize)> CostOpType;
   enum {
-    CoeffReadCost = TraversalSize==Dynamic ? HugeCost
-                  : TraversalSize==0 ? 1
-                  : int(TraversalSize) * int(evaluator<ArgType>::CoeffReadCost) + int(CostOpType::value),
-    
-    _ArgFlags = evaluator<ArgType>::Flags,
+    CoeffReadCost = TraversalSize == Dynamic ? HugeCost
+                    : TraversalSize == 0
+                        ? 1
+                        : int(TraversalSize) * int(evaluator<ArgType>::CoeffReadCost) + int(CostOpType::value),
 
-    _Vectorizable =  bool(int(_ArgFlags)&PacketAccessBit)
-                  && bool(MemberOp::Vectorizable)
-                  && (Direction==int(Vertical) ? bool(_ArgFlags&RowMajorBit) : (_ArgFlags&RowMajorBit)==0)
-                  && (TraversalSize!=0),
-                  
-    Flags = (traits<XprType>::Flags&RowMajorBit)
-          | (evaluator<ArgType>::Flags&(HereditaryBits&(~RowMajorBit)))
-          | (_Vectorizable ? PacketAccessBit : 0)
-          | LinearAccessBit,
-    
-    Alignment = 0 // FIXME this will need to be improved once PartialReduxExpr is vectorized
+    ArgFlags_ = evaluator<ArgType>::Flags,
+
+    Vectorizable_ = bool(int(ArgFlags_) & PacketAccessBit) && bool(MemberOp::Vectorizable) &&
+                    (Direction == int(Vertical) ? bool(ArgFlags_ & RowMajorBit) : (ArgFlags_ & RowMajorBit) == 0) &&
+                    (TraversalSize != 0),
+
+    Flags = (traits<XprType>::Flags & RowMajorBit) | (evaluator<ArgType>::Flags & (HereditaryBits & (~RowMajorBit))) |
+            (Vectorizable_ ? PacketAccessBit : 0) | LinearAccessBit,
+
+    Alignment = 0  // FIXME this will need to be improved once PartialReduxExpr is vectorized
   };
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType xpr)
-    : m_arg(xpr.nestedExpression()), m_functor(xpr.functor())
-  {
-    EIGEN_INTERNAL_CHECK_COST_VALUE(TraversalSize==Dynamic ? HugeCost : (TraversalSize==0 ? 1 : int(CostOpType::value)));
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType xpr) : m_arg(xpr.nestedExpression()), m_functor(xpr.functor()) {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(TraversalSize == Dynamic ? HugeCost
+                                                             : (TraversalSize == 0 ? 1 : int(CostOpType::value)));
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const Scalar coeff(Index i, Index j) const
-  {
-    return coeff(Direction==Vertical ? j : i);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index i, Index j) const {
+    return coeff(Direction == Vertical ? j : i);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const Scalar coeff(Index index) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index index) const {
     return m_functor(m_arg.template subVector<DirectionType(Direction)>(index));
   }
 
-  template<int LoadMode,typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  PacketType packet(Index i, Index j) const
-  {
-    return packet<LoadMode,PacketType>(Direction==Vertical ? j : i);
+  template <int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index i, Index j) const {
+    return packet<LoadMode, PacketType>(Direction == Vertical ? j : i);
   }
-  
-  template<int LoadMode,typename PacketType>
-  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-  PacketType packet(Index idx) const
-  {
+
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC PacketType packet(Index idx) const {
     enum { PacketSize = internal::unpacket_traits<PacketType>::size };
-    typedef Block<const ArgTypeNestedCleaned,
-                  Direction==Vertical ? int(ArgType::RowsAtCompileTime) : int(PacketSize),
-                  Direction==Vertical ? int(PacketSize) : int(ArgType::ColsAtCompileTime),
-                  true /* InnerPanel */> PanelType;
-    
-    PanelType panel(m_arg,
-                    Direction==Vertical ? 0 : idx,
-                    Direction==Vertical ? idx : 0,
-                    Direction==Vertical ? m_arg.rows() : Index(PacketSize),
-                    Direction==Vertical ? Index(PacketSize) : m_arg.cols());
+    typedef Block<const ArgTypeNestedCleaned, Direction == Vertical ? int(ArgType::RowsAtCompileTime) : int(PacketSize),
+                  Direction == Vertical ? int(PacketSize) : int(ArgType::ColsAtCompileTime), true /* InnerPanel */>
+        PanelType;
+
+    PanelType panel(m_arg, Direction == Vertical ? 0 : idx, Direction == Vertical ? idx : 0,
+                    Direction == Vertical ? m_arg.rows() : Index(PacketSize),
+                    Direction == Vertical ? Index(PacketSize) : m_arg.cols());
 
     // FIXME
-    // See bug 1612, currently if PacketSize==1 (i.e. complex<double> with 128bits registers) then the storage-order of panel get reversed
-    // and methods like packetByOuterInner do not make sense anymore in this context.
-    // So let's just by pass "vectorization" in this case:
-    if(PacketSize==1)
-      return internal::pset1<PacketType>(coeff(idx));
-    
+    // See bug 1612, currently if PacketSize==1 (i.e. complex<double> with 128bits registers) then the storage-order of
+    // panel get reversed and methods like packetByOuterInner do not make sense anymore in this context. So let's just
+    // by pass "vectorization" in this case:
+    if (PacketSize == 1) return internal::pset1<PacketType>(coeff(idx));
+
     typedef typename internal::redux_evaluator<PanelType> PanelEvaluator;
     PanelEvaluator panel_eval(panel);
     typedef typename MemberOp::BinaryOp BinaryOp;
-    PacketType p = internal::packetwise_redux_impl<BinaryOp,PanelEvaluator>::template run<PacketType>(panel_eval,m_functor.binaryFunc(),m_arg.outerSize());
+    PacketType p = internal::packetwise_redux_impl<BinaryOp, PanelEvaluator>::template run<PacketType>(
+        panel_eval, m_functor.binaryFunc(), m_arg.outerSize());
     return p;
   }
 
-protected:
+ protected:
   ConstArgTypeNested m_arg;
   const MemberOp m_functor;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PARTIALREDUX_H
+#endif  // EIGEN_PARTIALREDUX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
index 69401bf..6945964 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
@@ -11,595 +11,542 @@
 #ifndef EIGEN_PERMUTATIONMATRIX_H
 #define EIGEN_PERMUTATIONMATRIX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-enum PermPermProduct_t {PermPermProduct};
+enum PermPermProduct_t { PermPermProduct };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \class PermutationBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for permutations
-  *
-  * \tparam Derived the derived class
-  *
-  * This class is the base class for all expressions representing a permutation matrix,
-  * internally stored as a vector of integers.
-  * The convention followed here is that if \f$ \sigma \f$ is a permutation, the corresponding permutation matrix
-  * \f$ P_\sigma \f$ is such that if \f$ (e_1,\ldots,e_p) \f$ is the canonical basis, we have:
-  *  \f[ P_\sigma(e_i) = e_{\sigma(i)}. \f]
-  * This convention ensures that for any two permutations \f$ \sigma, \tau \f$, we have:
-  *  \f[ P_{\sigma\circ\tau} = P_\sigma P_\tau. \f]
-  *
-  * Permutation matrices are square and invertible.
-  *
-  * Notice that in addition to the member functions and operators listed here, there also are non-member
-  * operator* to multiply any kind of permutation object with any kind of matrix expression (MatrixBase)
-  * on either side.
-  *
-  * \sa class PermutationMatrix, class PermutationWrapper
-  */
-template<typename Derived>
-class PermutationBase : public EigenBase<Derived>
-{
-    typedef internal::traits<Derived> Traits;
-    typedef EigenBase<Derived> Base;
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Base class for permutations
+ *
+ * \tparam Derived the derived class
+ *
+ * This class is the base class for all expressions representing a permutation matrix,
+ * internally stored as a vector of integers.
+ * The convention followed here is that if \f$ \sigma \f$ is a permutation, the corresponding permutation matrix
+ * \f$ P_\sigma \f$ is such that if \f$ (e_1,\ldots,e_p) \f$ is the canonical basis, we have:
+ *  \f[ P_\sigma(e_i) = e_{\sigma(i)}. \f]
+ * This convention ensures that for any two permutations \f$ \sigma, \tau \f$, we have:
+ *  \f[ P_{\sigma\circ\tau} = P_\sigma P_\tau. \f]
+ *
+ * Permutation matrices are square and invertible.
+ *
+ * Notice that in addition to the member functions and operators listed here, there also are non-member
+ * operator* to multiply any kind of permutation object with any kind of matrix expression (MatrixBase)
+ * on either side.
+ *
+ * \sa class PermutationMatrix, class PermutationWrapper
+ */
+template <typename Derived>
+class PermutationBase : public EigenBase<Derived> {
+  typedef internal::traits<Derived> Traits;
+  typedef EigenBase<Derived> Base;
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename Traits::IndicesType IndicesType;
-    enum {
-      Flags = Traits::Flags,
-      RowsAtCompileTime = Traits::RowsAtCompileTime,
-      ColsAtCompileTime = Traits::ColsAtCompileTime,
-      MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = Traits::MaxColsAtCompileTime
-    };
-    typedef typename Traits::StorageIndex StorageIndex;
-    typedef Matrix<StorageIndex,RowsAtCompileTime,ColsAtCompileTime,0,MaxRowsAtCompileTime,MaxColsAtCompileTime>
-            DenseMatrixType;
-    typedef PermutationMatrix<IndicesType::SizeAtCompileTime,IndicesType::MaxSizeAtCompileTime,StorageIndex>
-            PlainPermutationType;
-    typedef PlainPermutationType PlainObject;
-    using Base::derived;
-    typedef Inverse<Derived> InverseReturnType;
-    typedef void Scalar;
-    #endif
-
-    /** Copies the other permutation into *this */
-    template<typename OtherDerived>
-    Derived& operator=(const PermutationBase<OtherDerived>& other)
-    {
-      indices() = other.indices();
-      return derived();
-    }
-
-    /** Assignment from the Transpositions \a tr */
-    template<typename OtherDerived>
-    Derived& operator=(const TranspositionsBase<OtherDerived>& tr)
-    {
-      setIdentity(tr.size());
-      for(Index k=size()-1; k>=0; --k)
-        applyTranspositionOnTheRight(k,tr.coeff(k));
-      return derived();
-    }
-
-    /** \returns the number of rows */
-    inline EIGEN_DEVICE_FUNC Index rows() const { return Index(indices().size()); }
-
-    /** \returns the number of columns */
-    inline EIGEN_DEVICE_FUNC Index cols() const { return Index(indices().size()); }
-
-    /** \returns the size of a side of the respective square matrix, i.e., the number of indices */
-    inline EIGEN_DEVICE_FUNC Index size() const { return Index(indices().size()); }
-
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename DenseDerived>
-    void evalTo(MatrixBase<DenseDerived>& other) const
-    {
-      other.setZero();
-      for (Index i=0; i<rows(); ++i)
-        other.coeffRef(indices().coeff(i),i) = typename DenseDerived::Scalar(1);
-    }
-    #endif
-
-    /** \returns a Matrix object initialized from this permutation matrix. Notice that it
-      * is inefficient to return this Matrix object by value. For efficiency, favor using
-      * the Matrix constructor taking EigenBase objects.
-      */
-    DenseMatrixType toDenseMatrix() const
-    {
-      return derived();
-    }
-
-    /** const version of indices(). */
-    const IndicesType& indices() const { return derived().indices(); }
-    /** \returns a reference to the stored array representing the permutation. */
-    IndicesType& indices() { return derived().indices(); }
-
-    /** Resizes to given size.
-      */
-    inline void resize(Index newSize)
-    {
-      indices().resize(newSize);
-    }
-
-    /** Sets *this to be the identity permutation matrix */
-    void setIdentity()
-    {
-      StorageIndex n = StorageIndex(size());
-      for(StorageIndex i = 0; i < n; ++i)
-        indices().coeffRef(i) = i;
-    }
-
-    /** Sets *this to be the identity permutation matrix of given size.
-      */
-    void setIdentity(Index newSize)
-    {
-      resize(newSize);
-      setIdentity();
-    }
-
-    /** Multiplies *this by the transposition \f$(ij)\f$ on the left.
-      *
-      * \returns a reference to *this.
-      *
-      * \warning This is much slower than applyTranspositionOnTheRight(Index,Index):
-      * this has linear complexity and requires a lot of branching.
-      *
-      * \sa applyTranspositionOnTheRight(Index,Index)
-      */
-    Derived& applyTranspositionOnTheLeft(Index i, Index j)
-    {
-      eigen_assert(i>=0 && j>=0 && i<size() && j<size());
-      for(Index k = 0; k < size(); ++k)
-      {
-        if(indices().coeff(k) == i) indices().coeffRef(k) = StorageIndex(j);
-        else if(indices().coeff(k) == j) indices().coeffRef(k) = StorageIndex(i);
-      }
-      return derived();
-    }
-
-    /** Multiplies *this by the transposition \f$(ij)\f$ on the right.
-      *
-      * \returns a reference to *this.
-      *
-      * This is a fast operation, it only consists in swapping two indices.
-      *
-      * \sa applyTranspositionOnTheLeft(Index,Index)
-      */
-    Derived& applyTranspositionOnTheRight(Index i, Index j)
-    {
-      eigen_assert(i>=0 && j>=0 && i<size() && j<size());
-      std::swap(indices().coeffRef(i), indices().coeffRef(j));
-      return derived();
-    }
-
-    /** \returns the inverse permutation matrix.
-      *
-      * \note \blank \note_try_to_help_rvo
-      */
-    inline InverseReturnType inverse() const
-    { return InverseReturnType(derived()); }
-    /** \returns the tranpose permutation matrix.
-      *
-      * \note \blank \note_try_to_help_rvo
-      */
-    inline InverseReturnType transpose() const
-    { return InverseReturnType(derived()); }
-
-    /**** multiplication helpers to hopefully get RVO ****/
-
-  
+ public:
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-  protected:
-    template<typename OtherDerived>
-    void assignTranspose(const PermutationBase<OtherDerived>& other)
-    {
-      for (Index i=0; i<rows();++i) indices().coeffRef(other.indices().coeff(i)) = i;
-    }
-    template<typename Lhs,typename Rhs>
-    void assignProduct(const Lhs& lhs, const Rhs& rhs)
-    {
-      eigen_assert(lhs.cols() == rhs.rows());
-      for (Index i=0; i<rows();++i) indices().coeffRef(i) = lhs.indices().coeff(rhs.indices().coeff(i));
-    }
+  typedef typename Traits::IndicesType IndicesType;
+  enum {
+    Flags = Traits::Flags,
+    RowsAtCompileTime = Traits::RowsAtCompileTime,
+    ColsAtCompileTime = Traits::ColsAtCompileTime,
+    MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = Traits::MaxColsAtCompileTime
+  };
+  typedef typename Traits::StorageIndex StorageIndex;
+  typedef Matrix<StorageIndex, RowsAtCompileTime, ColsAtCompileTime, 0, MaxRowsAtCompileTime, MaxColsAtCompileTime>
+      DenseMatrixType;
+  typedef PermutationMatrix<IndicesType::SizeAtCompileTime, IndicesType::MaxSizeAtCompileTime, StorageIndex>
+      PlainPermutationType;
+  typedef PlainPermutationType PlainObject;
+  using Base::derived;
+  typedef Inverse<Derived> InverseReturnType;
+  typedef void Scalar;
 #endif
 
-  public:
+  /** Copies the other permutation into *this */
+  template <typename OtherDerived>
+  Derived& operator=(const PermutationBase<OtherDerived>& other) {
+    indices() = other.indices();
+    return derived();
+  }
 
-    /** \returns the product permutation matrix.
-      *
-      * \note \blank \note_try_to_help_rvo
-      */
-    template<typename Other>
-    inline PlainPermutationType operator*(const PermutationBase<Other>& other) const
-    { return PlainPermutationType(internal::PermPermProduct, derived(), other.derived()); }
+  /** Assignment from the Transpositions \a tr */
+  template <typename OtherDerived>
+  Derived& operator=(const TranspositionsBase<OtherDerived>& tr) {
+    setIdentity(tr.size());
+    for (Index k = size() - 1; k >= 0; --k) applyTranspositionOnTheRight(k, tr.coeff(k));
+    return derived();
+  }
 
-    /** \returns the product of a permutation with another inverse permutation.
-      *
-      * \note \blank \note_try_to_help_rvo
-      */
-    template<typename Other>
-    inline PlainPermutationType operator*(const InverseImpl<Other,PermutationStorage>& other) const
-    { return PlainPermutationType(internal::PermPermProduct, *this, other.eval()); }
+  /** \returns the number of rows */
+  inline EIGEN_DEVICE_FUNC Index rows() const { return Index(indices().size()); }
 
-    /** \returns the product of an inverse permutation with another permutation.
-      *
-      * \note \blank \note_try_to_help_rvo
-      */
-    template<typename Other> friend
-    inline PlainPermutationType operator*(const InverseImpl<Other, PermutationStorage>& other, const PermutationBase& perm)
-    { return PlainPermutationType(internal::PermPermProduct, other.eval(), perm); }
-    
-    /** \returns the determinant of the permutation matrix, which is either 1 or -1 depending on the parity of the permutation.
-      *
-      * This function is O(\c n) procedure allocating a buffer of \c n booleans.
-      */
-    Index determinant() const
-    {
-      Index res = 1;
-      Index n = size();
-      Matrix<bool,RowsAtCompileTime,1,0,MaxRowsAtCompileTime> mask(n);
-      mask.fill(false);
-      Index r = 0;
-      while(r < n)
-      {
-        // search for the next seed
-        while(r<n && mask[r]) r++;
-        if(r>=n)
-          break;
-        // we got one, let's follow it until we are back to the seed
-        Index k0 = r++;
-        mask.coeffRef(k0) = true;
-        for(Index k=indices().coeff(k0); k!=k0; k=indices().coeff(k))
-        {
-          mask.coeffRef(k) = true;
-          res = -res;
-        }
-      }
-      return res;
+  /** \returns the number of columns */
+  inline EIGEN_DEVICE_FUNC Index cols() const { return Index(indices().size()); }
+
+  /** \returns the size of a side of the respective square matrix, i.e., the number of indices */
+  inline EIGEN_DEVICE_FUNC Index size() const { return Index(indices().size()); }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename DenseDerived>
+  void evalTo(MatrixBase<DenseDerived>& other) const {
+    other.setZero();
+    for (Index i = 0; i < rows(); ++i) other.coeffRef(indices().coeff(i), i) = typename DenseDerived::Scalar(1);
+  }
+#endif
+
+  /** \returns a Matrix object initialized from this permutation matrix. Notice that it
+   * is inefficient to return this Matrix object by value. For efficiency, favor using
+   * the Matrix constructor taking EigenBase objects.
+   */
+  DenseMatrixType toDenseMatrix() const { return derived(); }
+
+  /** const version of indices(). */
+  const IndicesType& indices() const { return derived().indices(); }
+  /** \returns a reference to the stored array representing the permutation. */
+  IndicesType& indices() { return derived().indices(); }
+
+  /** Resizes to given size.
+   */
+  inline void resize(Index newSize) { indices().resize(newSize); }
+
+  /** Sets *this to be the identity permutation matrix */
+  void setIdentity() {
+    StorageIndex n = StorageIndex(size());
+    for (StorageIndex i = 0; i < n; ++i) indices().coeffRef(i) = i;
+  }
+
+  /** Sets *this to be the identity permutation matrix of given size.
+   */
+  void setIdentity(Index newSize) {
+    resize(newSize);
+    setIdentity();
+  }
+
+  /** Multiplies *this by the transposition \f$(ij)\f$ on the left.
+   *
+   * \returns a reference to *this.
+   *
+   * \warning This is much slower than applyTranspositionOnTheRight(Index,Index):
+   * this has linear complexity and requires a lot of branching.
+   *
+   * \sa applyTranspositionOnTheRight(Index,Index)
+   */
+  Derived& applyTranspositionOnTheLeft(Index i, Index j) {
+    eigen_assert(i >= 0 && j >= 0 && i < size() && j < size());
+    for (Index k = 0; k < size(); ++k) {
+      if (indices().coeff(k) == i)
+        indices().coeffRef(k) = StorageIndex(j);
+      else if (indices().coeff(k) == j)
+        indices().coeffRef(k) = StorageIndex(i);
     }
+    return derived();
+  }
 
-  protected:
+  /** Multiplies *this by the transposition \f$(ij)\f$ on the right.
+   *
+   * \returns a reference to *this.
+   *
+   * This is a fast operation, it only consists in swapping two indices.
+   *
+   * \sa applyTranspositionOnTheLeft(Index,Index)
+   */
+  Derived& applyTranspositionOnTheRight(Index i, Index j) {
+    eigen_assert(i >= 0 && j >= 0 && i < size() && j < size());
+    std::swap(indices().coeffRef(i), indices().coeffRef(j));
+    return derived();
+  }
 
+  /** \returns the inverse permutation matrix.
+   *
+   * \note \blank \note_try_to_help_rvo
+   */
+  inline InverseReturnType inverse() const { return InverseReturnType(derived()); }
+  /** \returns the tranpose permutation matrix.
+   *
+   * \note \blank \note_try_to_help_rvo
+   */
+  inline InverseReturnType transpose() const { return InverseReturnType(derived()); }
+
+  /**** multiplication helpers to hopefully get RVO ****/
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+ protected:
+  template <typename OtherDerived>
+  void assignTranspose(const PermutationBase<OtherDerived>& other) {
+    for (Index i = 0; i < rows(); ++i) indices().coeffRef(other.indices().coeff(i)) = i;
+  }
+  template <typename Lhs, typename Rhs>
+  void assignProduct(const Lhs& lhs, const Rhs& rhs) {
+    eigen_assert(lhs.cols() == rhs.rows());
+    for (Index i = 0; i < rows(); ++i) indices().coeffRef(i) = lhs.indices().coeff(rhs.indices().coeff(i));
+  }
+#endif
+
+ public:
+  /** \returns the product permutation matrix.
+   *
+   * \note \blank \note_try_to_help_rvo
+   */
+  template <typename Other>
+  inline PlainPermutationType operator*(const PermutationBase<Other>& other) const {
+    return PlainPermutationType(internal::PermPermProduct, derived(), other.derived());
+  }
+
+  /** \returns the product of a permutation with another inverse permutation.
+   *
+   * \note \blank \note_try_to_help_rvo
+   */
+  template <typename Other>
+  inline PlainPermutationType operator*(const InverseImpl<Other, PermutationStorage>& other) const {
+    return PlainPermutationType(internal::PermPermProduct, *this, other.eval());
+  }
+
+  /** \returns the product of an inverse permutation with another permutation.
+   *
+   * \note \blank \note_try_to_help_rvo
+   */
+  template <typename Other>
+  friend inline PlainPermutationType operator*(const InverseImpl<Other, PermutationStorage>& other,
+                                               const PermutationBase& perm) {
+    return PlainPermutationType(internal::PermPermProduct, other.eval(), perm);
+  }
+
+  /** \returns the determinant of the permutation matrix, which is either 1 or -1 depending on the parity of the
+   * permutation.
+   *
+   * This function is O(\c n) procedure allocating a buffer of \c n booleans.
+   */
+  Index determinant() const {
+    Index res = 1;
+    Index n = size();
+    Matrix<bool, RowsAtCompileTime, 1, 0, MaxRowsAtCompileTime> mask(n);
+    mask.fill(false);
+    Index r = 0;
+    while (r < n) {
+      // search for the next seed
+      while (r < n && mask[r]) r++;
+      if (r >= n) break;
+      // we got one, let's follow it until we are back to the seed
+      Index k0 = r++;
+      mask.coeffRef(k0) = true;
+      for (Index k = indices().coeff(k0); k != k0; k = indices().coeff(k)) {
+        mask.coeffRef(k) = true;
+        res = -res;
+      }
+    }
+    return res;
+  }
+
+ protected:
 };
 
 namespace internal {
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex>
-struct traits<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, _StorageIndex> >
- : traits<Matrix<_StorageIndex,SizeAtCompileTime,SizeAtCompileTime,0,MaxSizeAtCompileTime,MaxSizeAtCompileTime> >
-{
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_>
+struct traits<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_> >
+    : traits<
+          Matrix<StorageIndex_, SizeAtCompileTime, SizeAtCompileTime, 0, MaxSizeAtCompileTime, MaxSizeAtCompileTime> > {
   typedef PermutationStorage StorageKind;
-  typedef Matrix<_StorageIndex, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1> IndicesType;
-  typedef _StorageIndex StorageIndex;
+  typedef Matrix<StorageIndex_, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1> IndicesType;
+  typedef StorageIndex_ StorageIndex;
   typedef void Scalar;
 };
-}
+}  // namespace internal
 
 /** \class PermutationMatrix
-  * \ingroup Core_Module
-  *
-  * \brief Permutation matrix
-  *
-  * \tparam SizeAtCompileTime the number of rows/cols, or Dynamic
-  * \tparam MaxSizeAtCompileTime the maximum number of rows/cols, or Dynamic. This optional parameter defaults to SizeAtCompileTime. Most of the time, you should not have to specify it.
-  * \tparam _StorageIndex the integer type of the indices
-  *
-  * This class represents a permutation matrix, internally stored as a vector of integers.
-  *
-  * \sa class PermutationBase, class PermutationWrapper, class DiagonalMatrix
-  */
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex>
-class PermutationMatrix : public PermutationBase<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, _StorageIndex> >
-{
-    typedef PermutationBase<PermutationMatrix> Base;
-    typedef internal::traits<PermutationMatrix> Traits;
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Permutation matrix
+ *
+ * \tparam SizeAtCompileTime the number of rows/cols, or Dynamic
+ * \tparam MaxSizeAtCompileTime the maximum number of rows/cols, or Dynamic. This optional parameter defaults to
+ * SizeAtCompileTime. Most of the time, you should not have to specify it. \tparam StorageIndex_ the integer type of the
+ * indices
+ *
+ * This class represents a permutation matrix, internally stored as a vector of integers.
+ *
+ * \sa class PermutationBase, class PermutationWrapper, class DiagonalMatrix
+ */
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_>
+class PermutationMatrix
+    : public PermutationBase<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_> > {
+  typedef PermutationBase<PermutationMatrix> Base;
+  typedef internal::traits<PermutationMatrix> Traits;
 
-    typedef const PermutationMatrix& Nested;
-
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename Traits::IndicesType IndicesType;
-    typedef typename Traits::StorageIndex StorageIndex;
-    #endif
-
-    inline PermutationMatrix()
-    {}
-
-    /** Constructs an uninitialized permutation matrix of given size.
-      */
-    explicit inline PermutationMatrix(Index size) : m_indices(size)
-    {
-      eigen_internal_assert(size <= NumTraits<StorageIndex>::highest());
-    }
-
-    /** Copy constructor. */
-    template<typename OtherDerived>
-    inline PermutationMatrix(const PermutationBase<OtherDerived>& other)
-      : m_indices(other.indices()) {}
-
-    /** Generic constructor from expression of the indices. The indices
-      * array has the meaning that the permutations sends each integer i to indices[i].
-      *
-      * \warning It is your responsibility to check that the indices array that you passes actually
-      * describes a permutation, i.e., each value between 0 and n-1 occurs exactly once, where n is the
-      * array's size.
-      */
-    template<typename Other>
-    explicit inline PermutationMatrix(const MatrixBase<Other>& indices) : m_indices(indices)
-    {}
-
-    /** Convert the Transpositions \a tr to a permutation matrix */
-    template<typename Other>
-    explicit PermutationMatrix(const TranspositionsBase<Other>& tr)
-      : m_indices(tr.size())
-    {
-      *this = tr;
-    }
-
-    /** Copies the other permutation into *this */
-    template<typename Other>
-    PermutationMatrix& operator=(const PermutationBase<Other>& other)
-    {
-      m_indices = other.indices();
-      return *this;
-    }
-
-    /** Assignment from the Transpositions \a tr */
-    template<typename Other>
-    PermutationMatrix& operator=(const TranspositionsBase<Other>& tr)
-    {
-      return Base::operator=(tr.derived());
-    }
-
-    /** const version of indices(). */
-    const IndicesType& indices() const { return m_indices; }
-    /** \returns a reference to the stored array representing the permutation. */
-    IndicesType& indices() { return m_indices; }
-
-
-    /**** multiplication helpers to hopefully get RVO ****/
+ public:
+  typedef const PermutationMatrix& Nested;
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename Other>
-    PermutationMatrix(const InverseImpl<Other,PermutationStorage>& other)
-      : m_indices(other.derived().nestedExpression().size())
-    {
-      eigen_internal_assert(m_indices.size() <= NumTraits<StorageIndex>::highest());
-      StorageIndex end = StorageIndex(m_indices.size());
-      for (StorageIndex i=0; i<end;++i)
-        m_indices.coeffRef(other.derived().nestedExpression().indices().coeff(i)) = i;
-    }
-    template<typename Lhs,typename Rhs>
-    PermutationMatrix(internal::PermPermProduct_t, const Lhs& lhs, const Rhs& rhs)
-      : m_indices(lhs.indices().size())
-    {
-      Base::assignProduct(lhs,rhs);
-    }
+  typedef typename Traits::IndicesType IndicesType;
+  typedef typename Traits::StorageIndex StorageIndex;
 #endif
 
-  protected:
+  inline PermutationMatrix() {}
 
-    IndicesType m_indices;
+  /** Constructs an uninitialized permutation matrix of given size.
+   */
+  explicit inline PermutationMatrix(Index size) : m_indices(size) {
+    eigen_internal_assert(size <= NumTraits<StorageIndex>::highest());
+  }
+
+  /** Copy constructor. */
+  template <typename OtherDerived>
+  inline PermutationMatrix(const PermutationBase<OtherDerived>& other) : m_indices(other.indices()) {}
+
+  /** Generic constructor from expression of the indices. The indices
+   * array has the meaning that the permutations sends each integer i to indices[i].
+   *
+   * \warning It is your responsibility to check that the indices array that you passes actually
+   * describes a permutation, i.e., each value between 0 and n-1 occurs exactly once, where n is the
+   * array's size.
+   */
+  template <typename Other>
+  explicit inline PermutationMatrix(const MatrixBase<Other>& indices) : m_indices(indices) {}
+
+  /** Convert the Transpositions \a tr to a permutation matrix */
+  template <typename Other>
+  explicit PermutationMatrix(const TranspositionsBase<Other>& tr) : m_indices(tr.size()) {
+    *this = tr;
+  }
+
+  /** Copies the other permutation into *this */
+  template <typename Other>
+  PermutationMatrix& operator=(const PermutationBase<Other>& other) {
+    m_indices = other.indices();
+    return *this;
+  }
+
+  /** Assignment from the Transpositions \a tr */
+  template <typename Other>
+  PermutationMatrix& operator=(const TranspositionsBase<Other>& tr) {
+    return Base::operator=(tr.derived());
+  }
+
+  /** const version of indices(). */
+  const IndicesType& indices() const { return m_indices; }
+  /** \returns a reference to the stored array representing the permutation. */
+  IndicesType& indices() { return m_indices; }
+
+  /**** multiplication helpers to hopefully get RVO ****/
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename Other>
+  PermutationMatrix(const InverseImpl<Other, PermutationStorage>& other)
+      : m_indices(other.derived().nestedExpression().size()) {
+    eigen_internal_assert(m_indices.size() <= NumTraits<StorageIndex>::highest());
+    StorageIndex end = StorageIndex(m_indices.size());
+    for (StorageIndex i = 0; i < end; ++i)
+      m_indices.coeffRef(other.derived().nestedExpression().indices().coeff(i)) = i;
+  }
+  template <typename Lhs, typename Rhs>
+  PermutationMatrix(internal::PermPermProduct_t, const Lhs& lhs, const Rhs& rhs) : m_indices(lhs.indices().size()) {
+    Base::assignProduct(lhs, rhs);
+  }
+#endif
+
+ protected:
+  IndicesType m_indices;
 };
 
-
 namespace internal {
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex, int _PacketAccess>
-struct traits<Map<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, _StorageIndex>,_PacketAccess> >
- : traits<Matrix<_StorageIndex,SizeAtCompileTime,SizeAtCompileTime,0,MaxSizeAtCompileTime,MaxSizeAtCompileTime> >
-{
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_, int PacketAccess_>
+struct traits<Map<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_>, PacketAccess_> >
+    : traits<
+          Matrix<StorageIndex_, SizeAtCompileTime, SizeAtCompileTime, 0, MaxSizeAtCompileTime, MaxSizeAtCompileTime> > {
   typedef PermutationStorage StorageKind;
-  typedef Map<const Matrix<_StorageIndex, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1>, _PacketAccess> IndicesType;
-  typedef _StorageIndex StorageIndex;
+  typedef Map<const Matrix<StorageIndex_, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1>, PacketAccess_> IndicesType;
+  typedef StorageIndex_ StorageIndex;
   typedef void Scalar;
 };
-}
+}  // namespace internal
 
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex, int _PacketAccess>
-class Map<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, _StorageIndex>,_PacketAccess>
-  : public PermutationBase<Map<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, _StorageIndex>,_PacketAccess> >
-{
-    typedef PermutationBase<Map> Base;
-    typedef internal::traits<Map> Traits;
-  public:
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_, int PacketAccess_>
+class Map<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_>, PacketAccess_>
+    : public PermutationBase<
+          Map<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_>, PacketAccess_> > {
+  typedef PermutationBase<Map> Base;
+  typedef internal::traits<Map> Traits;
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename Traits::IndicesType IndicesType;
-    typedef typename IndicesType::Scalar StorageIndex;
-    #endif
+ public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  typedef typename Traits::IndicesType IndicesType;
+  typedef typename IndicesType::Scalar StorageIndex;
+#endif
 
-    inline Map(const StorageIndex* indicesPtr)
-      : m_indices(indicesPtr)
-    {}
+  inline Map(const StorageIndex* indicesPtr) : m_indices(indicesPtr) {}
 
-    inline Map(const StorageIndex* indicesPtr, Index size)
-      : m_indices(indicesPtr,size)
-    {}
+  inline Map(const StorageIndex* indicesPtr, Index size) : m_indices(indicesPtr, size) {}
 
-    /** Copies the other permutation into *this */
-    template<typename Other>
-    Map& operator=(const PermutationBase<Other>& other)
-    { return Base::operator=(other.derived()); }
+  /** Copies the other permutation into *this */
+  template <typename Other>
+  Map& operator=(const PermutationBase<Other>& other) {
+    return Base::operator=(other.derived());
+  }
 
-    /** Assignment from the Transpositions \a tr */
-    template<typename Other>
-    Map& operator=(const TranspositionsBase<Other>& tr)
-    { return Base::operator=(tr.derived()); }
+  /** Assignment from the Transpositions \a tr */
+  template <typename Other>
+  Map& operator=(const TranspositionsBase<Other>& tr) {
+    return Base::operator=(tr.derived());
+  }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    Map& operator=(const Map& other)
-    {
-      m_indices = other.m_indices;
-      return *this;
-    }
-    #endif
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** This is a special case of the templated operator=. Its purpose is to
+   * prevent a default operator= from hiding the templated operator=.
+   */
+  Map& operator=(const Map& other) {
+    m_indices = other.m_indices;
+    return *this;
+  }
+#endif
 
-    /** const version of indices(). */
-    const IndicesType& indices() const { return m_indices; }
-    /** \returns a reference to the stored array representing the permutation. */
-    IndicesType& indices() { return m_indices; }
+  /** const version of indices(). */
+  const IndicesType& indices() const { return m_indices; }
+  /** \returns a reference to the stored array representing the permutation. */
+  IndicesType& indices() { return m_indices; }
 
-  protected:
-
-    IndicesType m_indices;
+ protected:
+  IndicesType m_indices;
 };
 
-template<typename _IndicesType> class TranspositionsWrapper;
+template <typename IndicesType_>
+class TranspositionsWrapper;
 namespace internal {
-template<typename _IndicesType>
-struct traits<PermutationWrapper<_IndicesType> >
-{
+template <typename IndicesType_>
+struct traits<PermutationWrapper<IndicesType_> > {
   typedef PermutationStorage StorageKind;
   typedef void Scalar;
-  typedef typename _IndicesType::Scalar StorageIndex;
-  typedef _IndicesType IndicesType;
+  typedef typename IndicesType_::Scalar StorageIndex;
+  typedef IndicesType_ IndicesType;
   enum {
-    RowsAtCompileTime = _IndicesType::SizeAtCompileTime,
-    ColsAtCompileTime = _IndicesType::SizeAtCompileTime,
+    RowsAtCompileTime = IndicesType_::SizeAtCompileTime,
+    ColsAtCompileTime = IndicesType_::SizeAtCompileTime,
     MaxRowsAtCompileTime = IndicesType::MaxSizeAtCompileTime,
     MaxColsAtCompileTime = IndicesType::MaxSizeAtCompileTime,
     Flags = 0
   };
 };
-}
+}  // namespace internal
 
 /** \class PermutationWrapper
-  * \ingroup Core_Module
-  *
-  * \brief Class to view a vector of integers as a permutation matrix
-  *
-  * \tparam _IndicesType the type of the vector of integer (can be any compatible expression)
-  *
-  * This class allows to view any vector expression of integers as a permutation matrix.
-  *
-  * \sa class PermutationBase, class PermutationMatrix
-  */
-template<typename _IndicesType>
-class PermutationWrapper : public PermutationBase<PermutationWrapper<_IndicesType> >
-{
-    typedef PermutationBase<PermutationWrapper> Base;
-    typedef internal::traits<PermutationWrapper> Traits;
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Class to view a vector of integers as a permutation matrix
+ *
+ * \tparam IndicesType_ the type of the vector of integer (can be any compatible expression)
+ *
+ * This class allows to view any vector expression of integers as a permutation matrix.
+ *
+ * \sa class PermutationBase, class PermutationMatrix
+ */
+template <typename IndicesType_>
+class PermutationWrapper : public PermutationBase<PermutationWrapper<IndicesType_> > {
+  typedef PermutationBase<PermutationWrapper> Base;
+  typedef internal::traits<PermutationWrapper> Traits;
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename Traits::IndicesType IndicesType;
-    #endif
+ public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  typedef typename Traits::IndicesType IndicesType;
+#endif
 
-    inline PermutationWrapper(const IndicesType& indices)
-      : m_indices(indices)
-    {}
+  inline PermutationWrapper(const IndicesType& indices) : m_indices(indices) {}
 
-    /** const version of indices(). */
-    const typename internal::remove_all<typename IndicesType::Nested>::type&
-    indices() const { return m_indices; }
+  /** const version of indices(). */
+  const internal::remove_all_t<typename IndicesType::Nested>& indices() const { return m_indices; }
 
-  protected:
-
-    typename IndicesType::Nested m_indices;
+ protected:
+  typename IndicesType::Nested m_indices;
 };
 
-
 /** \returns the matrix with the permutation applied to the columns.
-  */
-template<typename MatrixDerived, typename PermutationDerived>
-EIGEN_DEVICE_FUNC
-const Product<MatrixDerived, PermutationDerived, AliasFreeProduct>
-operator*(const MatrixBase<MatrixDerived> &matrix,
-          const PermutationBase<PermutationDerived>& permutation)
-{
-  return Product<MatrixDerived, PermutationDerived, AliasFreeProduct>
-            (matrix.derived(), permutation.derived());
+ */
+template <typename MatrixDerived, typename PermutationDerived>
+EIGEN_DEVICE_FUNC const Product<MatrixDerived, PermutationDerived, AliasFreeProduct> operator*(
+    const MatrixBase<MatrixDerived>& matrix, const PermutationBase<PermutationDerived>& permutation) {
+  return Product<MatrixDerived, PermutationDerived, AliasFreeProduct>(matrix.derived(), permutation.derived());
 }
 
 /** \returns the matrix with the permutation applied to the rows.
-  */
-template<typename PermutationDerived, typename MatrixDerived>
-EIGEN_DEVICE_FUNC
-const Product<PermutationDerived, MatrixDerived, AliasFreeProduct>
-operator*(const PermutationBase<PermutationDerived> &permutation,
-          const MatrixBase<MatrixDerived>& matrix)
-{
-  return Product<PermutationDerived, MatrixDerived, AliasFreeProduct>
-            (permutation.derived(), matrix.derived());
+ */
+template <typename PermutationDerived, typename MatrixDerived>
+EIGEN_DEVICE_FUNC const Product<PermutationDerived, MatrixDerived, AliasFreeProduct> operator*(
+    const PermutationBase<PermutationDerived>& permutation, const MatrixBase<MatrixDerived>& matrix) {
+  return Product<PermutationDerived, MatrixDerived, AliasFreeProduct>(permutation.derived(), matrix.derived());
 }
 
+template <typename PermutationType>
+class InverseImpl<PermutationType, PermutationStorage> : public EigenBase<Inverse<PermutationType> > {
+  typedef typename PermutationType::PlainPermutationType PlainPermutationType;
+  typedef internal::traits<PermutationType> PermTraits;
 
-template<typename PermutationType>
-class InverseImpl<PermutationType, PermutationStorage>
-  : public EigenBase<Inverse<PermutationType> >
-{
-    typedef typename PermutationType::PlainPermutationType PlainPermutationType;
-    typedef internal::traits<PermutationType> PermTraits;
-  protected:
-    InverseImpl() {}
-  public:
-    typedef Inverse<PermutationType> InverseType;
-    using EigenBase<Inverse<PermutationType> >::derived;
+ protected:
+  InverseImpl() {}
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename PermutationType::DenseMatrixType DenseMatrixType;
-    enum {
-      RowsAtCompileTime = PermTraits::RowsAtCompileTime,
-      ColsAtCompileTime = PermTraits::ColsAtCompileTime,
-      MaxRowsAtCompileTime = PermTraits::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = PermTraits::MaxColsAtCompileTime
-    };
-    #endif
+ public:
+  typedef Inverse<PermutationType> InverseType;
+  using EigenBase<Inverse<PermutationType> >::derived;
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename DenseDerived>
-    void evalTo(MatrixBase<DenseDerived>& other) const
-    {
-      other.setZero();
-      for (Index i=0; i<derived().rows();++i)
-        other.coeffRef(i, derived().nestedExpression().indices().coeff(i)) = typename DenseDerived::Scalar(1);
-    }
-    #endif
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  typedef typename PermutationType::DenseMatrixType DenseMatrixType;
+  enum {
+    RowsAtCompileTime = PermTraits::RowsAtCompileTime,
+    ColsAtCompileTime = PermTraits::ColsAtCompileTime,
+    MaxRowsAtCompileTime = PermTraits::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = PermTraits::MaxColsAtCompileTime
+  };
+#endif
 
-    /** \return the equivalent permutation matrix */
-    PlainPermutationType eval() const { return derived(); }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename DenseDerived>
+  void evalTo(MatrixBase<DenseDerived>& other) const {
+    other.setZero();
+    for (Index i = 0; i < derived().rows(); ++i)
+      other.coeffRef(i, derived().nestedExpression().indices().coeff(i)) = typename DenseDerived::Scalar(1);
+  }
+#endif
 
-    DenseMatrixType toDenseMatrix() const { return derived(); }
+  /** \return the equivalent permutation matrix */
+  PlainPermutationType eval() const { return derived(); }
 
-    /** \returns the matrix with the inverse permutation applied to the columns.
-      */
-    template<typename OtherDerived> friend
-    const Product<OtherDerived, InverseType, AliasFreeProduct>
-    operator*(const MatrixBase<OtherDerived>& matrix, const InverseType& trPerm)
-    {
-      return Product<OtherDerived, InverseType, AliasFreeProduct>(matrix.derived(), trPerm.derived());
-    }
+  DenseMatrixType toDenseMatrix() const { return derived(); }
 
-    /** \returns the matrix with the inverse permutation applied to the rows.
-      */
-    template<typename OtherDerived>
-    const Product<InverseType, OtherDerived, AliasFreeProduct>
-    operator*(const MatrixBase<OtherDerived>& matrix) const
-    {
-      return Product<InverseType, OtherDerived, AliasFreeProduct>(derived(), matrix.derived());
-    }
+  /** \returns the matrix with the inverse permutation applied to the columns.
+   */
+  template <typename OtherDerived>
+  friend const Product<OtherDerived, InverseType, AliasFreeProduct> operator*(const MatrixBase<OtherDerived>& matrix,
+                                                                              const InverseType& trPerm) {
+    return Product<OtherDerived, InverseType, AliasFreeProduct>(matrix.derived(), trPerm.derived());
+  }
+
+  /** \returns the matrix with the inverse permutation applied to the rows.
+   */
+  template <typename OtherDerived>
+  const Product<InverseType, OtherDerived, AliasFreeProduct> operator*(const MatrixBase<OtherDerived>& matrix) const {
+    return Product<InverseType, OtherDerived, AliasFreeProduct>(derived(), matrix.derived());
+  }
 };
 
-template<typename Derived>
-const PermutationWrapper<const Derived> MatrixBase<Derived>::asPermutation() const
-{
+template <typename Derived>
+const PermutationWrapper<const Derived> MatrixBase<Derived>::asPermutation() const {
   return derived();
 }
 
 namespace internal {
 
-template<> struct AssignmentKind<DenseShape,PermutationShape> { typedef EigenBase2EigenBase Kind; };
+template <>
+struct AssignmentKind<DenseShape, PermutationShape> {
+  typedef EigenBase2EigenBase Kind;
+};
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PERMUTATIONMATRIX_H
+#endif  // EIGEN_PERMUTATIONMATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h
index e2ddbd1..a8307c7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h
@@ -12,51 +12,71 @@
 #define EIGEN_DENSESTORAGEBASE_H
 
 #if defined(EIGEN_INITIALIZE_MATRICES_BY_ZERO)
-# define EIGEN_INITIALIZE_COEFFS
-# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED for(Index i=0;i<base().size();++i) coeffRef(i)=Scalar(0);
+#define EIGEN_INITIALIZE_COEFFS
+#define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED \
+  for (Index i = 0; i < base().size(); ++i) coeffRef(i) = Scalar(0);
 #elif defined(EIGEN_INITIALIZE_MATRICES_BY_NAN)
-# define EIGEN_INITIALIZE_COEFFS
-# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED for(Index i=0;i<base().size();++i) coeffRef(i)=std::numeric_limits<Scalar>::quiet_NaN();
+#define EIGEN_INITIALIZE_COEFFS
+#define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED \
+  for (Index i = 0; i < base().size(); ++i) coeffRef(i) = std::numeric_limits<Scalar>::quiet_NaN();
 #else
-# undef EIGEN_INITIALIZE_COEFFS
-# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+#undef EIGEN_INITIALIZE_COEFFS
+#define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
 #endif
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<int MaxSizeAtCompileTime> struct check_rows_cols_for_overflow {
-  template<typename Index>
-  EIGEN_DEVICE_FUNC
-  static EIGEN_ALWAYS_INLINE void run(Index, Index)
-  {
+template <int MaxSizeAtCompileTime, int MaxRowsAtCompileTime, int MaxColsAtCompileTime>
+struct check_rows_cols_for_overflow {
+  EIGEN_STATIC_ASSERT(MaxRowsAtCompileTime* MaxColsAtCompileTime == MaxSizeAtCompileTime,
+                      YOU MADE A PROGRAMMING MISTAKE)
+  template <typename Index>
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index, Index) {}
+};
+
+template <int MaxRowsAtCompileTime>
+struct check_rows_cols_for_overflow<Dynamic, MaxRowsAtCompileTime, Dynamic> {
+  template <typename Index>
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index, Index cols) {
+    constexpr Index MaxIndex = NumTraits<Index>::highest();
+    bool error = cols > MaxIndex / MaxRowsAtCompileTime;
+    if (error) throw_std_bad_alloc();
   }
 };
 
-template<> struct check_rows_cols_for_overflow<Dynamic> {
-  template<typename Index>
-  EIGEN_DEVICE_FUNC
-  static EIGEN_ALWAYS_INLINE void run(Index rows, Index cols)
-  {
-    // http://hg.mozilla.org/mozilla-central/file/6c8a909977d3/xpcom/ds/CheckedInt.h#l242
-    // we assume Index is signed
-    Index max_index = (std::size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed
-    bool error = (rows == 0 || cols == 0) ? false
-               : (rows > max_index / cols);
-    if (error)
-      throw_std_bad_alloc();
+template <int MaxColsAtCompileTime>
+struct check_rows_cols_for_overflow<Dynamic, Dynamic, MaxColsAtCompileTime> {
+  template <typename Index>
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index rows, Index) {
+    constexpr Index MaxIndex = NumTraits<Index>::highest();
+    bool error = rows > MaxIndex / MaxColsAtCompileTime;
+    if (error) throw_std_bad_alloc();
   }
 };
 
-template <typename Derived,
-          typename OtherDerived = Derived,
+template <>
+struct check_rows_cols_for_overflow<Dynamic, Dynamic, Dynamic> {
+  template <typename Index>
+  EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index rows, Index cols) {
+    constexpr Index MaxIndex = NumTraits<Index>::highest();
+    bool error = cols == 0 ? false : (rows > MaxIndex / cols);
+    if (error) throw_std_bad_alloc();
+  }
+};
+
+template <typename Derived, typename OtherDerived = Derived,
           bool IsVector = bool(Derived::IsVectorAtCompileTime) && bool(OtherDerived::IsVectorAtCompileTime)>
 struct conservative_resize_like_impl;
 
-template<typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers> struct matrix_swap_impl;
+template <typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers>
+struct matrix_swap_impl;
 
-} // end namespace internal
+}  // end namespace internal
 
 #ifdef EIGEN_PARSED_BY_DOXYGEN
 namespace doxygen {
@@ -64,971 +84,875 @@
 // This is a workaround to doxygen not being able to understand the inheritance logic
 // when it is hidden by the dense_xpr_base helper struct.
 // Moreover, doxygen fails to include members that are not documented in the declaration body of
-// MatrixBase if we inherits MatrixBase<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >,
+// MatrixBase if we inherits MatrixBase<Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_> >,
 // this is why we simply inherits MatrixBase, though this does not make sense.
 
 /** This class is just a workaround for Doxygen and it does not not actually exist. */
-template<typename Derived> struct dense_xpr_base_dispatcher;
+template <typename Derived>
+struct dense_xpr_base_dispatcher;
 /** This class is just a workaround for Doxygen and it does not not actually exist. */
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct dense_xpr_base_dispatcher<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-    : public MatrixBase {};
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+struct dense_xpr_base_dispatcher<Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> : public MatrixBase {};
 /** This class is just a workaround for Doxygen and it does not not actually exist. */
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct dense_xpr_base_dispatcher<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-    : public ArrayBase {};
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+struct dense_xpr_base_dispatcher<Array<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> : public ArrayBase {};
 
-} // namespace doxygen
+}  // namespace doxygen
 
 /** \class PlainObjectBase
-  * \ingroup Core_Module
-  * \brief %Dense storage base class for matrices and arrays.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_PLAINOBJECTBASE_PLUGIN.
-  *
-  * \tparam Derived is the derived type, e.g., a Matrix or Array
-  *
-  * \sa \ref TopicClassHierarchy
-  */
-template<typename Derived>
+ * \ingroup Core_Module
+ * \brief %Dense storage base class for matrices and arrays.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_PLAINOBJECTBASE_PLUGIN.
+ *
+ * \tparam Derived is the derived type, e.g., a Matrix or Array
+ *
+ * \sa \ref TopicClassHierarchy
+ */
+template <typename Derived>
 class PlainObjectBase : public doxygen::dense_xpr_base_dispatcher<Derived>
 #else
-template<typename Derived>
+template <typename Derived>
 class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
 #endif
 {
-  public:
-    enum { Options = internal::traits<Derived>::Options };
-    typedef typename internal::dense_xpr_base<Derived>::type Base;
+ public:
+  enum { Options = internal::traits<Derived>::Options };
+  typedef typename internal::dense_xpr_base<Derived>::type Base;
 
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
 
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef Derived DenseType;
+  typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Derived DenseType;
 
-    using Base::RowsAtCompileTime;
-    using Base::ColsAtCompileTime;
-    using Base::SizeAtCompileTime;
-    using Base::MaxRowsAtCompileTime;
-    using Base::MaxColsAtCompileTime;
-    using Base::MaxSizeAtCompileTime;
-    using Base::IsVectorAtCompileTime;
-    using Base::Flags;
+  using Base::ColsAtCompileTime;
+  using Base::Flags;
+  using Base::IsVectorAtCompileTime;
+  using Base::MaxColsAtCompileTime;
+  using Base::MaxRowsAtCompileTime;
+  using Base::MaxSizeAtCompileTime;
+  using Base::RowsAtCompileTime;
+  using Base::SizeAtCompileTime;
 
-    typedef Eigen::Map<Derived, Unaligned>  MapType;
-    typedef const Eigen::Map<const Derived, Unaligned> ConstMapType;
-    typedef Eigen::Map<Derived, AlignedMax> AlignedMapType;
-    typedef const Eigen::Map<const Derived, AlignedMax> ConstAlignedMapType;
-    template<typename StrideType> struct StridedMapType { typedef Eigen::Map<Derived, Unaligned, StrideType> type; };
-    template<typename StrideType> struct StridedConstMapType { typedef Eigen::Map<const Derived, Unaligned, StrideType> type; };
-    template<typename StrideType> struct StridedAlignedMapType { typedef Eigen::Map<Derived, AlignedMax, StrideType> type; };
-    template<typename StrideType> struct StridedConstAlignedMapType { typedef Eigen::Map<const Derived, AlignedMax, StrideType> type; };
+  typedef Eigen::Map<Derived, Unaligned> MapType;
+  typedef const Eigen::Map<const Derived, Unaligned> ConstMapType;
+  typedef Eigen::Map<Derived, AlignedMax> AlignedMapType;
+  typedef const Eigen::Map<const Derived, AlignedMax> ConstAlignedMapType;
+  template <typename StrideType>
+  struct StridedMapType {
+    typedef Eigen::Map<Derived, Unaligned, StrideType> type;
+  };
+  template <typename StrideType>
+  struct StridedConstMapType {
+    typedef Eigen::Map<const Derived, Unaligned, StrideType> type;
+  };
+  template <typename StrideType>
+  struct StridedAlignedMapType {
+    typedef Eigen::Map<Derived, AlignedMax, StrideType> type;
+  };
+  template <typename StrideType>
+  struct StridedConstAlignedMapType {
+    typedef Eigen::Map<const Derived, AlignedMax, StrideType> type;
+  };
 
-  protected:
-    DenseStorage<Scalar, Base::MaxSizeAtCompileTime, Base::RowsAtCompileTime, Base::ColsAtCompileTime, Options> m_storage;
+ protected:
+  DenseStorage<Scalar, Base::MaxSizeAtCompileTime, Base::RowsAtCompileTime, Base::ColsAtCompileTime, Options> m_storage;
 
-  public:
-    enum { NeedsToAlign = (SizeAtCompileTime != Dynamic) && (internal::traits<Derived>::Alignment>0) };
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+ public:
+  enum { NeedsToAlign = (SizeAtCompileTime != Dynamic) && (internal::traits<Derived>::Alignment > 0) };
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 
-    EIGEN_DEVICE_FUNC
-    Base& base() { return *static_cast<Base*>(this); }
-    EIGEN_DEVICE_FUNC
-    const Base& base() const { return *static_cast<const Base*>(this); }
+  EIGEN_STATIC_ASSERT(internal::check_implication(MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1,
+                                                  (int(Options) & RowMajor) == RowMajor),
+                      INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT(internal::check_implication(MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1,
+                                                  (int(Options) & RowMajor) == 0),
+                      INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT((RowsAtCompileTime == Dynamic) || (RowsAtCompileTime >= 0), INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT((ColsAtCompileTime == Dynamic) || (ColsAtCompileTime >= 0), INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT((MaxRowsAtCompileTime == Dynamic) || (MaxRowsAtCompileTime >= 0),
+                      INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT((MaxColsAtCompileTime == Dynamic) || (MaxColsAtCompileTime >= 0),
+                      INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT((MaxRowsAtCompileTime == RowsAtCompileTime || RowsAtCompileTime == Dynamic),
+                      INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT((MaxColsAtCompileTime == ColsAtCompileTime || ColsAtCompileTime == Dynamic),
+                      INVALID_MATRIX_TEMPLATE_PARAMETERS)
+  EIGEN_STATIC_ASSERT(((Options & (DontAlign | RowMajor)) == Options), INVALID_MATRIX_TEMPLATE_PARAMETERS)
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return m_storage.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return m_storage.cols(); }
+  EIGEN_DEVICE_FUNC Base& base() { return *static_cast<Base*>(this); }
+  EIGEN_DEVICE_FUNC const Base& base() const { return *static_cast<const Base*>(this); }
 
-    /** This is an overloaded version of DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index,Index) const
-      * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
-      *
-      * See DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index) const for details. */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE const Scalar& coeff(Index rowId, Index colId) const
-    {
-      if(Flags & RowMajorBit)
-        return m_storage.data()[colId + rowId * m_storage.cols()];
-      else // column-major
-        return m_storage.data()[rowId + colId * m_storage.rows()];
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_storage.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_storage.cols(); }
 
-    /** This is an overloaded version of DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index) const
-      * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
-      *
-      * See DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index) const for details. */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const
-    {
-      return m_storage.data()[index];
-    }
+  /** This is an overloaded version of DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index,Index) const
+   * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
+   *
+   * See DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index) const for details. */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr const Scalar& coeff(Index rowId, Index colId) const {
+    if (Flags & RowMajorBit)
+      return m_storage.data()[colId + rowId * m_storage.cols()];
+    else  // column-major
+      return m_storage.data()[rowId + colId * m_storage.rows()];
+  }
 
-    /** This is an overloaded version of DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index,Index) const
-      * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
-      *
-      * See DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index,Index) const for details. */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar& coeffRef(Index rowId, Index colId)
-    {
-      if(Flags & RowMajorBit)
-        return m_storage.data()[colId + rowId * m_storage.cols()];
-      else // column-major
-        return m_storage.data()[rowId + colId * m_storage.rows()];
-    }
+  /** This is an overloaded version of DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index) const
+   * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
+   *
+   * See DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index) const for details. */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const { return m_storage.data()[index]; }
 
-    /** This is an overloaded version of DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index) const
-      * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
-      *
-      * See DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index) const for details. */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Scalar& coeffRef(Index index)
-    {
-      return m_storage.data()[index];
-    }
+  /** This is an overloaded version of DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index,Index) const
+   * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
+   *
+   * See DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index,Index) const for details. */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& coeffRef(Index rowId, Index colId) {
+    if (Flags & RowMajorBit)
+      return m_storage.data()[colId + rowId * m_storage.cols()];
+    else  // column-major
+      return m_storage.data()[rowId + colId * m_storage.rows()];
+  }
 
-    /** This is the const version of coeffRef(Index,Index) which is thus synonym of coeff(Index,Index).
-      * It is provided for convenience. */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      if(Flags & RowMajorBit)
-        return m_storage.data()[colId + rowId * m_storage.cols()];
-      else // column-major
-        return m_storage.data()[rowId + colId * m_storage.rows()];
-    }
+  /** This is an overloaded version of DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index) const
+   * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
+   *
+   * See DenseCoeffsBase<Derived,WriteAccessors>::coeffRef(Index) const for details. */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& coeffRef(Index index) { return m_storage.data()[index]; }
 
-    /** This is the const version of coeffRef(Index) which is thus synonym of coeff(Index).
-      * It is provided for convenience. */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE const Scalar& coeffRef(Index index) const
-    {
-      return m_storage.data()[index];
-    }
+  /** This is the const version of coeffRef(Index,Index) which is thus synonym of coeff(Index,Index).
+   * It is provided for convenience. */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr const Scalar& coeffRef(Index rowId, Index colId) const {
+    if (Flags & RowMajorBit)
+      return m_storage.data()[colId + rowId * m_storage.cols()];
+    else  // column-major
+      return m_storage.data()[rowId + colId * m_storage.rows()];
+  }
 
-    /** \internal */
-    template<int LoadMode>
-    EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const
-    {
-      return internal::ploadt<PacketScalar, LoadMode>
-               (m_storage.data() + (Flags & RowMajorBit
-                                   ? colId + rowId * m_storage.cols()
-                                   : rowId + colId * m_storage.rows()));
-    }
+  /** This is the const version of coeffRef(Index) which is thus synonym of coeff(Index).
+   * It is provided for convenience. */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr const Scalar& coeffRef(Index index) const {
+    return m_storage.data()[index];
+  }
 
-    /** \internal */
-    template<int LoadMode>
-    EIGEN_STRONG_INLINE PacketScalar packet(Index index) const
-    {
-      return internal::ploadt<PacketScalar, LoadMode>(m_storage.data() + index);
-    }
+  /** \internal */
+  template <int LoadMode>
+  EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const {
+    return internal::ploadt<PacketScalar, LoadMode>(
+        m_storage.data() + (Flags & RowMajorBit ? colId + rowId * m_storage.cols() : rowId + colId * m_storage.rows()));
+  }
 
-    /** \internal */
-    template<int StoreMode>
-    EIGEN_STRONG_INLINE void writePacket(Index rowId, Index colId, const PacketScalar& val)
-    {
-      internal::pstoret<Scalar, PacketScalar, StoreMode>
-              (m_storage.data() + (Flags & RowMajorBit
-                                   ? colId + rowId * m_storage.cols()
-                                   : rowId + colId * m_storage.rows()), val);
-    }
+  /** \internal */
+  template <int LoadMode>
+  EIGEN_STRONG_INLINE PacketScalar packet(Index index) const {
+    return internal::ploadt<PacketScalar, LoadMode>(m_storage.data() + index);
+  }
 
-    /** \internal */
-    template<int StoreMode>
-    EIGEN_STRONG_INLINE void writePacket(Index index, const PacketScalar& val)
-    {
-      internal::pstoret<Scalar, PacketScalar, StoreMode>(m_storage.data() + index, val);
-    }
+  /** \internal */
+  template <int StoreMode>
+  EIGEN_STRONG_INLINE void writePacket(Index rowId, Index colId, const PacketScalar& val) {
+    internal::pstoret<Scalar, PacketScalar, StoreMode>(
+        m_storage.data() + (Flags & RowMajorBit ? colId + rowId * m_storage.cols() : rowId + colId * m_storage.rows()),
+        val);
+  }
 
-    /** \returns a const pointer to the data array of this matrix */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar *data() const
-    { return m_storage.data(); }
+  /** \internal */
+  template <int StoreMode>
+  EIGEN_STRONG_INLINE void writePacket(Index index, const PacketScalar& val) {
+    internal::pstoret<Scalar, PacketScalar, StoreMode>(m_storage.data() + index, val);
+  }
 
-    /** \returns a pointer to the data array of this matrix */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar *data()
-    { return m_storage.data(); }
+  /** \returns a const pointer to the data array of this matrix */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar* data() const { return m_storage.data(); }
 
-    /** Resizes \c *this to a \a rows x \a cols matrix.
-      *
-      * This method is intended for dynamic-size matrices, although it is legal to call it on any
-      * matrix as long as fixed dimensions are left unchanged. If you only want to change the number
-      * of rows and/or of columns, you can use resize(NoChange_t, Index), resize(Index, NoChange_t).
-      *
-      * If the current number of coefficients of \c *this exactly matches the
-      * product \a rows * \a cols, then no memory allocation is performed and
-      * the current values are left unchanged. In all other cases, including
-      * shrinking, the data is reallocated and all previous values are lost.
-      *
-      * Example: \include Matrix_resize_int_int.cpp
-      * Output: \verbinclude Matrix_resize_int_int.out
-      *
-      * \sa resize(Index) for vectors, resize(NoChange_t, Index), resize(Index, NoChange_t)
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
-    {
-      eigen_assert(   EIGEN_IMPLIES(RowsAtCompileTime!=Dynamic,rows==RowsAtCompileTime)
-                   && EIGEN_IMPLIES(ColsAtCompileTime!=Dynamic,cols==ColsAtCompileTime)
-                   && EIGEN_IMPLIES(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic,rows<=MaxRowsAtCompileTime)
-                   && EIGEN_IMPLIES(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic,cols<=MaxColsAtCompileTime)
-                   && rows>=0 && cols>=0 && "Invalid sizes when resizing a matrix or array.");
-      internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(rows, cols);
-      #ifdef EIGEN_INITIALIZE_COEFFS
-        Index size = rows*cols;
-        bool size_changed = size != this->size();
-        m_storage.resize(size, rows, cols);
-        if(size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-      #else
-        m_storage.resize(rows*cols, rows, cols);
-      #endif
-    }
+  /** \returns a pointer to the data array of this matrix */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar* data() { return m_storage.data(); }
 
-    /** Resizes \c *this to a vector of length \a size
-      *
-      * \only_for_vectors. This method does not work for
-      * partially dynamic matrices when the static dimension is anything other
-      * than 1. For example it will not work with Matrix<double, 2, Dynamic>.
-      *
-      * Example: \include Matrix_resize_int.cpp
-      * Output: \verbinclude Matrix_resize_int.out
-      *
-      * \sa resize(Index,Index), resize(NoChange_t, Index), resize(Index, NoChange_t)
-      */
-    EIGEN_DEVICE_FUNC
-    inline void resize(Index size)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(PlainObjectBase)
-      eigen_assert(((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime==Dynamic || size<=MaxSizeAtCompileTime)) || SizeAtCompileTime == size) && size>=0);
-      #ifdef EIGEN_INITIALIZE_COEFFS
-        bool size_changed = size != this->size();
-      #endif
-      if(RowsAtCompileTime == 1)
-        m_storage.resize(size, 1, size);
-      else
-        m_storage.resize(size, size, 1);
-      #ifdef EIGEN_INITIALIZE_COEFFS
-        if(size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-      #endif
-    }
+  /** Resizes \c *this to a \a rows x \a cols matrix.
+   *
+   * This method is intended for dynamic-size matrices, although it is legal to call it on any
+   * matrix as long as fixed dimensions are left unchanged. If you only want to change the number
+   * of rows and/or of columns, you can use resize(NoChange_t, Index), resize(Index, NoChange_t).
+   *
+   * If the current number of coefficients of \c *this exactly matches the
+   * product \a rows * \a cols, then no memory allocation is performed and
+   * the current values are left unchanged. In all other cases, including
+   * shrinking, the data is reallocated and all previous values are lost.
+   *
+   * Example: \include Matrix_resize_int_int.cpp
+   * Output: \verbinclude Matrix_resize_int_int.out
+   *
+   * \sa resize(Index) for vectors, resize(NoChange_t, Index), resize(Index, NoChange_t)
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize(Index rows, Index cols) {
+    eigen_assert(internal::check_implication(RowsAtCompileTime != Dynamic, rows == RowsAtCompileTime) &&
+                 internal::check_implication(ColsAtCompileTime != Dynamic, cols == ColsAtCompileTime) &&
+                 internal::check_implication(RowsAtCompileTime == Dynamic && MaxRowsAtCompileTime != Dynamic,
+                                             rows <= MaxRowsAtCompileTime) &&
+                 internal::check_implication(ColsAtCompileTime == Dynamic && MaxColsAtCompileTime != Dynamic,
+                                             cols <= MaxColsAtCompileTime) &&
+                 rows >= 0 && cols >= 0 && "Invalid sizes when resizing a matrix or array.");
+    internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime>::run(rows,
+                                                                                                                  cols);
+#ifdef EIGEN_INITIALIZE_COEFFS
+    Index size = rows * cols;
+    bool size_changed = size != this->size();
+    m_storage.resize(size, rows, cols);
+    if (size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+#else
+    m_storage.resize(rows * cols, rows, cols);
+#endif
+  }
 
-    /** Resizes the matrix, changing only the number of columns. For the parameter of type NoChange_t, just pass the special value \c NoChange
-      * as in the example below.
-      *
-      * Example: \include Matrix_resize_NoChange_int.cpp
-      * Output: \verbinclude Matrix_resize_NoChange_int.out
-      *
-      * \sa resize(Index,Index)
-      */
-    EIGEN_DEVICE_FUNC
-    inline void resize(NoChange_t, Index cols)
-    {
-      resize(rows(), cols);
-    }
+  /** Resizes \c *this to a vector of length \a size
+   *
+   * \only_for_vectors. This method does not work for
+   * partially dynamic matrices when the static dimension is anything other
+   * than 1. For example it will not work with Matrix<double, 2, Dynamic>.
+   *
+   * Example: \include Matrix_resize_int.cpp
+   * Output: \verbinclude Matrix_resize_int.out
+   *
+   * \sa resize(Index,Index), resize(NoChange_t, Index), resize(Index, NoChange_t)
+   */
+  EIGEN_DEVICE_FUNC inline constexpr void resize(Index size) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(PlainObjectBase)
+    eigen_assert(((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime == Dynamic || size <= MaxSizeAtCompileTime)) ||
+                  SizeAtCompileTime == size) &&
+                 size >= 0);
+#ifdef EIGEN_INITIALIZE_COEFFS
+    bool size_changed = size != this->size();
+#endif
+    if (RowsAtCompileTime == 1)
+      m_storage.resize(size, 1, size);
+    else
+      m_storage.resize(size, size, 1);
+#ifdef EIGEN_INITIALIZE_COEFFS
+    if (size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+#endif
+  }
 
-    /** Resizes the matrix, changing only the number of rows. For the parameter of type NoChange_t, just pass the special value \c NoChange
-      * as in the example below.
-      *
-      * Example: \include Matrix_resize_int_NoChange.cpp
-      * Output: \verbinclude Matrix_resize_int_NoChange.out
-      *
-      * \sa resize(Index,Index)
-      */
-    EIGEN_DEVICE_FUNC
-    inline void resize(Index rows, NoChange_t)
-    {
-      resize(rows, cols());
-    }
+  /** Resizes the matrix, changing only the number of columns. For the parameter of type NoChange_t, just pass the
+   * special value \c NoChange as in the example below.
+   *
+   * Example: \include Matrix_resize_NoChange_int.cpp
+   * Output: \verbinclude Matrix_resize_NoChange_int.out
+   *
+   * \sa resize(Index,Index)
+   */
+  EIGEN_DEVICE_FUNC inline constexpr void resize(NoChange_t, Index cols) { resize(rows(), cols); }
 
-    /** Resizes \c *this to have the same dimensions as \a other.
-      * Takes care of doing all the checking that's needed.
-      *
-      * Note that copying a row-vector into a vector (and conversely) is allowed.
-      * The resizing, if any, is then done in the appropriate way so that row-vectors
-      * remain row-vectors and vectors remain vectors.
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void resizeLike(const EigenBase<OtherDerived>& _other)
-    {
-      const OtherDerived& other = _other.derived();
-      internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(other.rows(), other.cols());
-      const Index othersize = other.rows()*other.cols();
-      if(RowsAtCompileTime == 1)
-      {
-        eigen_assert(other.rows() == 1 || other.cols() == 1);
-        resize(1, othersize);
-      }
-      else if(ColsAtCompileTime == 1)
-      {
-        eigen_assert(other.rows() == 1 || other.cols() == 1);
-        resize(othersize, 1);
-      }
-      else resize(other.rows(), other.cols());
-    }
+  /** Resizes the matrix, changing only the number of rows. For the parameter of type NoChange_t, just pass the special
+   * value \c NoChange as in the example below.
+   *
+   * Example: \include Matrix_resize_int_NoChange.cpp
+   * Output: \verbinclude Matrix_resize_int_NoChange.out
+   *
+   * \sa resize(Index,Index)
+   */
+  EIGEN_DEVICE_FUNC inline constexpr void resize(Index rows, NoChange_t) { resize(rows, cols()); }
 
-    /** Resizes the matrix to \a rows x \a cols while leaving old values untouched.
-      *
-      * The method is intended for matrices of dynamic size. If you only want to change the number
-      * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or
-      * conservativeResize(Index, NoChange_t).
-      *
-      * Matrices are resized relative to the top-left element. In case values need to be
-      * appended to the matrix they will be uninitialized.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void conservativeResize(Index rows, Index cols)
-    {
-      internal::conservative_resize_like_impl<Derived>::run(*this, rows, cols);
-    }
+  /** Resizes \c *this to have the same dimensions as \a other.
+   * Takes care of doing all the checking that's needed.
+   *
+   * Note that copying a row-vector into a vector (and conversely) is allowed.
+   * The resizing, if any, is then done in the appropriate way so that row-vectors
+   * remain row-vectors and vectors remain vectors.
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resizeLike(const EigenBase<OtherDerived>& _other) {
+    const OtherDerived& other = _other.derived();
+    internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime>::run(
+        other.rows(), other.cols());
+    const Index othersize = other.rows() * other.cols();
+    if (RowsAtCompileTime == 1) {
+      eigen_assert(other.rows() == 1 || other.cols() == 1);
+      resize(1, othersize);
+    } else if (ColsAtCompileTime == 1) {
+      eigen_assert(other.rows() == 1 || other.cols() == 1);
+      resize(othersize, 1);
+    } else
+      resize(other.rows(), other.cols());
+  }
 
-    /** Resizes the matrix to \a rows x \a cols while leaving old values untouched.
-      *
-      * As opposed to conservativeResize(Index rows, Index cols), this version leaves
-      * the number of columns unchanged.
-      *
-      * In case the matrix is growing, new rows will be uninitialized.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void conservativeResize(Index rows, NoChange_t)
-    {
-      // Note: see the comment in conservativeResize(Index,Index)
-      conservativeResize(rows, cols());
-    }
+  /** Resizes the matrix to \a rows x \a cols while leaving old values untouched.
+   *
+   * The method is intended for matrices of dynamic size. If you only want to change the number
+   * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or
+   * conservativeResize(Index, NoChange_t).
+   *
+   * Matrices are resized relative to the top-left element. In case values need to be
+   * appended to the matrix they will be uninitialized.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(Index rows, Index cols) {
+    internal::conservative_resize_like_impl<Derived>::run(*this, rows, cols);
+  }
 
-    /** Resizes the matrix to \a rows x \a cols while leaving old values untouched.
-      *
-      * As opposed to conservativeResize(Index rows, Index cols), this version leaves
-      * the number of rows unchanged.
-      *
-      * In case the matrix is growing, new columns will be uninitialized.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void conservativeResize(NoChange_t, Index cols)
-    {
-      // Note: see the comment in conservativeResize(Index,Index)
-      conservativeResize(rows(), cols);
-    }
+  /** Resizes the matrix to \a rows x \a cols while leaving old values untouched.
+   *
+   * As opposed to conservativeResize(Index rows, Index cols), this version leaves
+   * the number of columns unchanged.
+   *
+   * In case the matrix is growing, new rows will be uninitialized.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(Index rows, NoChange_t) {
+    // Note: see the comment in conservativeResize(Index,Index)
+    conservativeResize(rows, cols());
+  }
 
-    /** Resizes the vector to \a size while retaining old values.
-      *
-      * \only_for_vectors. This method does not work for
-      * partially dynamic matrices when the static dimension is anything other
-      * than 1. For example it will not work with Matrix<double, 2, Dynamic>.
-      *
-      * When values are appended, they will be uninitialized.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void conservativeResize(Index size)
-    {
-      internal::conservative_resize_like_impl<Derived>::run(*this, size);
-    }
+  /** Resizes the matrix to \a rows x \a cols while leaving old values untouched.
+   *
+   * As opposed to conservativeResize(Index rows, Index cols), this version leaves
+   * the number of rows unchanged.
+   *
+   * In case the matrix is growing, new columns will be uninitialized.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(NoChange_t, Index cols) {
+    // Note: see the comment in conservativeResize(Index,Index)
+    conservativeResize(rows(), cols);
+  }
 
-    /** Resizes the matrix to \a rows x \a cols of \c other, while leaving old values untouched.
-      *
-      * The method is intended for matrices of dynamic size. If you only want to change the number
-      * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or
-      * conservativeResize(Index, NoChange_t).
-      *
-      * Matrices are resized relative to the top-left element. In case values need to be
-      * appended to the matrix they will copied from \c other.
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void conservativeResizeLike(const DenseBase<OtherDerived>& other)
-    {
-      internal::conservative_resize_like_impl<Derived,OtherDerived>::run(*this, other);
-    }
+  /** Resizes the vector to \a size while retaining old values.
+   *
+   * \only_for_vectors. This method does not work for
+   * partially dynamic matrices when the static dimension is anything other
+   * than 1. For example it will not work with Matrix<double, 2, Dynamic>.
+   *
+   * When values are appended, they will be uninitialized.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(Index size) {
+    internal::conservative_resize_like_impl<Derived>::run(*this, size);
+  }
 
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Derived& operator=(const PlainObjectBase& other)
-    {
-      return _set(other);
-    }
+  /** Resizes the matrix to \a rows x \a cols of \c other, while leaving old values untouched.
+   *
+   * The method is intended for matrices of dynamic size. If you only want to change the number
+   * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or
+   * conservativeResize(Index, NoChange_t).
+   *
+   * Matrices are resized relative to the top-left element. In case values need to be
+   * appended to the matrix they will copied from \c other.
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResizeLike(const DenseBase<OtherDerived>& other) {
+    internal::conservative_resize_like_impl<Derived, OtherDerived>::run(*this, other);
+  }
 
-    /** \sa MatrixBase::lazyAssign() */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Derived& lazyAssign(const DenseBase<OtherDerived>& other)
-    {
-      _resize_to_match(other);
-      return Base::lazyAssign(other.derived());
-    }
+  /** This is a special case of the templated operator=. Its purpose is to
+   * prevent a default operator= from hiding the templated operator=.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const PlainObjectBase& other) { return _set(other); }
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Derived& operator=(const ReturnByValue<OtherDerived>& func)
-    {
-      resize(func.rows(), func.cols());
-      return Base::operator=(func);
-    }
+  /** \sa MatrixBase::lazyAssign() */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& lazyAssign(const DenseBase<OtherDerived>& other) {
+    _resize_to_match(other);
+    return Base::lazyAssign(other.derived());
+  }
 
-    // Prevent user from trying to instantiate PlainObjectBase objects
-    // by making all its constructor protected. See bug 1074.
-  protected:
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const ReturnByValue<OtherDerived>& func) {
+    resize(func.rows(), func.cols());
+    return Base::operator=(func);
+  }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE PlainObjectBase() : m_storage()
-    {
-//       _check_template_params();
-//       EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-    }
+  // Prevent user from trying to instantiate PlainObjectBase objects
+  // by making all its constructor protected. See bug 1074.
+ protected:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase() : m_storage() {
+    //       EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+  }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    // FIXME is it still needed ?
-    /** \internal */
-    EIGEN_DEVICE_FUNC
-    explicit PlainObjectBase(internal::constructor_without_unaligned_array_assert)
-      : m_storage(internal::constructor_without_unaligned_array_assert())
-    {
-//       _check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-    }
+  // FIXME is it still needed ?
+  /** \internal */
+  EIGEN_DEVICE_FUNC explicit PlainObjectBase(internal::constructor_without_unaligned_array_assert)
+      : m_storage(internal::constructor_without_unaligned_array_assert()) {
+    // EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+  }
 #endif
 
-#if EIGEN_HAS_RVALUE_REFERENCES
-    EIGEN_DEVICE_FUNC
-    PlainObjectBase(PlainObjectBase&& other) EIGEN_NOEXCEPT
-      : m_storage( std::move(other.m_storage) )
-    {
+  EIGEN_DEVICE_FUNC PlainObjectBase(PlainObjectBase&& other) EIGEN_NOEXCEPT : m_storage(std::move(other.m_storage)) {}
+
+  EIGEN_DEVICE_FUNC PlainObjectBase& operator=(PlainObjectBase&& other) EIGEN_NOEXCEPT {
+    m_storage = std::move(other.m_storage);
+    return *this;
+  }
+
+  /** Copy constructor */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const PlainObjectBase& other)
+      : Base(), m_storage(other.m_storage) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(Index size, Index rows, Index cols)
+      : m_storage(size, rows, cols) {
+    //       EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+  }
+
+  /** \brief Construct a row of column vector with fixed size from an arbitrary number of coefficients.
+   *
+   * \only_for_vectors
+   *
+   * This constructor is for 1D array or vectors with more than 4 coefficients.
+   *
+   * \warning To construct a column (resp. row) vector of fixed length, the number of values passed to this
+   * constructor must match the the fixed number of rows (resp. columns) of \c *this.
+   */
+  template <typename... ArgTypes>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2,
+                                                        const Scalar& a3, const ArgTypes&... args)
+      : m_storage() {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, sizeof...(args) + 4);
+    m_storage.data()[0] = a0;
+    m_storage.data()[1] = a1;
+    m_storage.data()[2] = a2;
+    m_storage.data()[3] = a3;
+    Index i = 4;
+    auto x = {(m_storage.data()[i++] = args, 0)...};
+    static_cast<void>(x);
+  }
+
+  /** \brief Constructs a Matrix or Array and initializes it by elements given by an initializer list of initializer
+   * lists
+   */
+  EIGEN_DEVICE_FUNC explicit constexpr EIGEN_STRONG_INLINE PlainObjectBase(
+      const std::initializer_list<std::initializer_list<Scalar>>& list)
+      : m_storage() {
+    size_t list_size = 0;
+    if (list.begin() != list.end()) {
+      list_size = list.begin()->size();
     }
 
-    EIGEN_DEVICE_FUNC
-    PlainObjectBase& operator=(PlainObjectBase&& other) EIGEN_NOEXCEPT
-    {
-      _check_template_params();
-      m_storage = std::move(other.m_storage);
-      return *this;
-    }
-#endif
-
-    /** Copy constructor */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE PlainObjectBase(const PlainObjectBase& other)
-      : Base(), m_storage(other.m_storage) { }
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE PlainObjectBase(Index size, Index rows, Index cols)
-      : m_storage(size, rows, cols)
-    {
-//       _check_template_params();
-//       EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
-    }
-
-    #if EIGEN_HAS_CXX11
-    /** \brief Construct a row of column vector with fixed size from an arbitrary number of coefficients. \cpp11
-      *
-      * \only_for_vectors
-      *
-      * This constructor is for 1D array or vectors with more than 4 coefficients.
-      * There exists C++98 analogue constructors for fixed-size array/vector having 1, 2, 3, or 4 coefficients.
-      *
-      * \warning To construct a column (resp. row) vector of fixed length, the number of values passed to this
-      * constructor must match the the fixed number of rows (resp. columns) of \c *this.
-      */
-    template <typename... ArgTypes>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2,  const Scalar& a3, const ArgTypes&... args)
-      : m_storage()
-    {
-      _check_template_params();
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, sizeof...(args) + 4);
-      m_storage.data()[0] = a0;
-      m_storage.data()[1] = a1;
-      m_storage.data()[2] = a2;
-      m_storage.data()[3] = a3;
-      Index i = 4;
-      auto x = {(m_storage.data()[i++] = args, 0)...};
-      static_cast<void>(x);
-    }
-
-    /** \brief Constructs a Matrix or Array and initializes it by elements given by an initializer list of initializer
-      * lists \cpp11
-      */
-    EIGEN_DEVICE_FUNC
-    explicit EIGEN_STRONG_INLINE PlainObjectBase(const std::initializer_list<std::initializer_list<Scalar>>& list)
-      : m_storage()
-    {
-      _check_template_params();
-
-      size_t list_size = 0;
-      if (list.begin() != list.end()) {
-        list_size = list.begin()->size();
-      }
-
-      // This is to allow syntax like VectorXi {{1, 2, 3, 4}}
-      if (ColsAtCompileTime == 1 && list.size() == 1) {
-        eigen_assert(list_size == static_cast<size_t>(RowsAtCompileTime) || RowsAtCompileTime == Dynamic);
-        resize(list_size, ColsAtCompileTime);
+    // This is to allow syntax like VectorXi {{1, 2, 3, 4}}
+    if (ColsAtCompileTime == 1 && list.size() == 1) {
+      eigen_assert(list_size == static_cast<size_t>(RowsAtCompileTime) || RowsAtCompileTime == Dynamic);
+      resize(list_size, ColsAtCompileTime);
+      if (list.begin()->begin() != nullptr) {
         std::copy(list.begin()->begin(), list.begin()->end(), m_storage.data());
-      } else {
-        eigen_assert(list.size() == static_cast<size_t>(RowsAtCompileTime) || RowsAtCompileTime == Dynamic);
-        eigen_assert(list_size == static_cast<size_t>(ColsAtCompileTime) || ColsAtCompileTime == Dynamic);
-        resize(list.size(), list_size);
+      }
+    } else {
+      eigen_assert(list.size() == static_cast<size_t>(RowsAtCompileTime) || RowsAtCompileTime == Dynamic);
+      eigen_assert(list_size == static_cast<size_t>(ColsAtCompileTime) || ColsAtCompileTime == Dynamic);
+      resize(list.size(), list_size);
 
-        Index row_index = 0;
-        for (const std::initializer_list<Scalar>& row : list) {
-          eigen_assert(list_size == row.size());
-          Index col_index = 0;
-          for (const Scalar& e : row) {
-            coeffRef(row_index, col_index) = e;
-            ++col_index;
-          }
-          ++row_index;
+      Index row_index = 0;
+      for (const std::initializer_list<Scalar>& row : list) {
+        eigen_assert(list_size == row.size());
+        Index col_index = 0;
+        for (const Scalar& e : row) {
+          coeffRef(row_index, col_index) = e;
+          ++col_index;
         }
+        ++row_index;
       }
     }
-    #endif  // end EIGEN_HAS_CXX11
+  }
 
-    /** \sa PlainObjectBase::operator=(const EigenBase<OtherDerived>&) */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase<OtherDerived> &other)
-      : m_storage()
-    {
-      _check_template_params();
-      resizeLike(other);
-      _set_noalias(other);
-    }
+  /** \sa PlainObjectBase::operator=(const EigenBase<OtherDerived>&) */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase<OtherDerived>& other) : m_storage() {
+    resizeLike(other);
+    _set_noalias(other);
+  }
 
-    /** \sa PlainObjectBase::operator=(const EigenBase<OtherDerived>&) */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase<OtherDerived> &other)
-      : m_storage()
-    {
-      _check_template_params();
-      resizeLike(other);
-      *this = other.derived();
-    }
-    /** \brief Copy constructor with in-place evaluation */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE PlainObjectBase(const ReturnByValue<OtherDerived>& other)
-    {
-      _check_template_params();
-      // FIXME this does not automatically transpose vectors if necessary
-      resize(other.rows(), other.cols());
-      other.evalTo(this->derived());
-    }
+  /** \sa PlainObjectBase::operator=(const EigenBase<OtherDerived>&) */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase<OtherDerived>& other) : m_storage() {
+    resizeLike(other);
+    *this = other.derived();
+  }
+  /** \brief Copy constructor with in-place evaluation */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const ReturnByValue<OtherDerived>& other) {
+    // FIXME this does not automatically transpose vectors if necessary
+    resize(other.rows(), other.cols());
+    other.evalTo(this->derived());
+  }
 
-  public:
+ public:
+  /** \brief Copies the generic expression \a other into *this.
+   * \copydetails DenseBase::operator=(const EigenBase<OtherDerived> &other)
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const EigenBase<OtherDerived>& other) {
+    _resize_to_match(other);
+    Base::operator=(other.derived());
+    return this->derived();
+  }
 
-    /** \brief Copies the generic expression \a other into *this.
-      * \copydetails DenseBase::operator=(const EigenBase<OtherDerived> &other)
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Derived& operator=(const EigenBase<OtherDerived> &other)
-    {
-      _resize_to_match(other);
-      Base::operator=(other.derived());
-      return this->derived();
-    }
+  /** \name Map
+   * These are convenience functions returning Map objects. The Map() static functions return unaligned Map objects,
+   * while the AlignedMap() functions return aligned Map objects and thus should be called only with 16-byte-aligned
+   * \a data pointers.
+   *
+   * Here is an example using strides:
+   * \include Matrix_Map_stride.cpp
+   * Output: \verbinclude Matrix_Map_stride.out
+   *
+   * \see class Map
+   */
+  ///@{
+  static inline ConstMapType Map(const Scalar* data) { return ConstMapType(data); }
+  static inline MapType Map(Scalar* data) { return MapType(data); }
+  static inline ConstMapType Map(const Scalar* data, Index size) { return ConstMapType(data, size); }
+  static inline MapType Map(Scalar* data, Index size) { return MapType(data, size); }
+  static inline ConstMapType Map(const Scalar* data, Index rows, Index cols) { return ConstMapType(data, rows, cols); }
+  static inline MapType Map(Scalar* data, Index rows, Index cols) { return MapType(data, rows, cols); }
 
-    /** \name Map
-      * These are convenience functions returning Map objects. The Map() static functions return unaligned Map objects,
-      * while the AlignedMap() functions return aligned Map objects and thus should be called only with 16-byte-aligned
-      * \a data pointers.
-      *
-      * Here is an example using strides:
-      * \include Matrix_Map_stride.cpp
-      * Output: \verbinclude Matrix_Map_stride.out
-      *
-      * \see class Map
-      */
-    //@{
-    static inline ConstMapType Map(const Scalar* data)
-    { return ConstMapType(data); }
-    static inline MapType Map(Scalar* data)
-    { return MapType(data); }
-    static inline ConstMapType Map(const Scalar* data, Index size)
-    { return ConstMapType(data, size); }
-    static inline MapType Map(Scalar* data, Index size)
-    { return MapType(data, size); }
-    static inline ConstMapType Map(const Scalar* data, Index rows, Index cols)
-    { return ConstMapType(data, rows, cols); }
-    static inline MapType Map(Scalar* data, Index rows, Index cols)
-    { return MapType(data, rows, cols); }
+  static inline ConstAlignedMapType MapAligned(const Scalar* data) { return ConstAlignedMapType(data); }
+  static inline AlignedMapType MapAligned(Scalar* data) { return AlignedMapType(data); }
+  static inline ConstAlignedMapType MapAligned(const Scalar* data, Index size) {
+    return ConstAlignedMapType(data, size);
+  }
+  static inline AlignedMapType MapAligned(Scalar* data, Index size) { return AlignedMapType(data, size); }
+  static inline ConstAlignedMapType MapAligned(const Scalar* data, Index rows, Index cols) {
+    return ConstAlignedMapType(data, rows, cols);
+  }
+  static inline AlignedMapType MapAligned(Scalar* data, Index rows, Index cols) {
+    return AlignedMapType(data, rows, cols);
+  }
 
-    static inline ConstAlignedMapType MapAligned(const Scalar* data)
-    { return ConstAlignedMapType(data); }
-    static inline AlignedMapType MapAligned(Scalar* data)
-    { return AlignedMapType(data); }
-    static inline ConstAlignedMapType MapAligned(const Scalar* data, Index size)
-    { return ConstAlignedMapType(data, size); }
-    static inline AlignedMapType MapAligned(Scalar* data, Index size)
-    { return AlignedMapType(data, size); }
-    static inline ConstAlignedMapType MapAligned(const Scalar* data, Index rows, Index cols)
-    { return ConstAlignedMapType(data, rows, cols); }
-    static inline AlignedMapType MapAligned(Scalar* data, Index rows, Index cols)
-    { return AlignedMapType(data, rows, cols); }
+  template <int Outer, int Inner>
+  static inline typename StridedConstMapType<Stride<Outer, Inner>>::type Map(const Scalar* data,
+                                                                             const Stride<Outer, Inner>& stride) {
+    return typename StridedConstMapType<Stride<Outer, Inner>>::type(data, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedMapType<Stride<Outer, Inner>>::type Map(Scalar* data,
+                                                                        const Stride<Outer, Inner>& stride) {
+    return typename StridedMapType<Stride<Outer, Inner>>::type(data, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedConstMapType<Stride<Outer, Inner>>::type Map(const Scalar* data, Index size,
+                                                                             const Stride<Outer, Inner>& stride) {
+    return typename StridedConstMapType<Stride<Outer, Inner>>::type(data, size, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedMapType<Stride<Outer, Inner>>::type Map(Scalar* data, Index size,
+                                                                        const Stride<Outer, Inner>& stride) {
+    return typename StridedMapType<Stride<Outer, Inner>>::type(data, size, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedConstMapType<Stride<Outer, Inner>>::type Map(const Scalar* data, Index rows, Index cols,
+                                                                             const Stride<Outer, Inner>& stride) {
+    return typename StridedConstMapType<Stride<Outer, Inner>>::type(data, rows, cols, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedMapType<Stride<Outer, Inner>>::type Map(Scalar* data, Index rows, Index cols,
+                                                                        const Stride<Outer, Inner>& stride) {
+    return typename StridedMapType<Stride<Outer, Inner>>::type(data, rows, cols, stride);
+  }
 
-    template<int Outer, int Inner>
-    static inline typename StridedConstMapType<Stride<Outer, Inner> >::type Map(const Scalar* data, const Stride<Outer, Inner>& stride)
-    { return typename StridedConstMapType<Stride<Outer, Inner> >::type(data, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedMapType<Stride<Outer, Inner> >::type Map(Scalar* data, const Stride<Outer, Inner>& stride)
-    { return typename StridedMapType<Stride<Outer, Inner> >::type(data, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedConstMapType<Stride<Outer, Inner> >::type Map(const Scalar* data, Index size, const Stride<Outer, Inner>& stride)
-    { return typename StridedConstMapType<Stride<Outer, Inner> >::type(data, size, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedMapType<Stride<Outer, Inner> >::type Map(Scalar* data, Index size, const Stride<Outer, Inner>& stride)
-    { return typename StridedMapType<Stride<Outer, Inner> >::type(data, size, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedConstMapType<Stride<Outer, Inner> >::type Map(const Scalar* data, Index rows, Index cols, const Stride<Outer, Inner>& stride)
-    { return typename StridedConstMapType<Stride<Outer, Inner> >::type(data, rows, cols, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedMapType<Stride<Outer, Inner> >::type Map(Scalar* data, Index rows, Index cols, const Stride<Outer, Inner>& stride)
-    { return typename StridedMapType<Stride<Outer, Inner> >::type(data, rows, cols, stride); }
+  template <int Outer, int Inner>
+  static inline typename StridedConstAlignedMapType<Stride<Outer, Inner>>::type MapAligned(
+      const Scalar* data, const Stride<Outer, Inner>& stride) {
+    return typename StridedConstAlignedMapType<Stride<Outer, Inner>>::type(data, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedAlignedMapType<Stride<Outer, Inner>>::type MapAligned(
+      Scalar* data, const Stride<Outer, Inner>& stride) {
+    return typename StridedAlignedMapType<Stride<Outer, Inner>>::type(data, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedConstAlignedMapType<Stride<Outer, Inner>>::type MapAligned(
+      const Scalar* data, Index size, const Stride<Outer, Inner>& stride) {
+    return typename StridedConstAlignedMapType<Stride<Outer, Inner>>::type(data, size, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedAlignedMapType<Stride<Outer, Inner>>::type MapAligned(
+      Scalar* data, Index size, const Stride<Outer, Inner>& stride) {
+    return typename StridedAlignedMapType<Stride<Outer, Inner>>::type(data, size, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedConstAlignedMapType<Stride<Outer, Inner>>::type MapAligned(
+      const Scalar* data, Index rows, Index cols, const Stride<Outer, Inner>& stride) {
+    return typename StridedConstAlignedMapType<Stride<Outer, Inner>>::type(data, rows, cols, stride);
+  }
+  template <int Outer, int Inner>
+  static inline typename StridedAlignedMapType<Stride<Outer, Inner>>::type MapAligned(
+      Scalar* data, Index rows, Index cols, const Stride<Outer, Inner>& stride) {
+    return typename StridedAlignedMapType<Stride<Outer, Inner>>::type(data, rows, cols, stride);
+  }
+  ///@}
 
-    template<int Outer, int Inner>
-    static inline typename StridedConstAlignedMapType<Stride<Outer, Inner> >::type MapAligned(const Scalar* data, const Stride<Outer, Inner>& stride)
-    { return typename StridedConstAlignedMapType<Stride<Outer, Inner> >::type(data, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedAlignedMapType<Stride<Outer, Inner> >::type MapAligned(Scalar* data, const Stride<Outer, Inner>& stride)
-    { return typename StridedAlignedMapType<Stride<Outer, Inner> >::type(data, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedConstAlignedMapType<Stride<Outer, Inner> >::type MapAligned(const Scalar* data, Index size, const Stride<Outer, Inner>& stride)
-    { return typename StridedConstAlignedMapType<Stride<Outer, Inner> >::type(data, size, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedAlignedMapType<Stride<Outer, Inner> >::type MapAligned(Scalar* data, Index size, const Stride<Outer, Inner>& stride)
-    { return typename StridedAlignedMapType<Stride<Outer, Inner> >::type(data, size, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedConstAlignedMapType<Stride<Outer, Inner> >::type MapAligned(const Scalar* data, Index rows, Index cols, const Stride<Outer, Inner>& stride)
-    { return typename StridedConstAlignedMapType<Stride<Outer, Inner> >::type(data, rows, cols, stride); }
-    template<int Outer, int Inner>
-    static inline typename StridedAlignedMapType<Stride<Outer, Inner> >::type MapAligned(Scalar* data, Index rows, Index cols, const Stride<Outer, Inner>& stride)
-    { return typename StridedAlignedMapType<Stride<Outer, Inner> >::type(data, rows, cols, stride); }
-    //@}
+  using Base::setConstant;
+  EIGEN_DEVICE_FUNC Derived& setConstant(Index size, const Scalar& val);
+  EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, Index cols, const Scalar& val);
+  EIGEN_DEVICE_FUNC Derived& setConstant(NoChange_t, Index cols, const Scalar& val);
+  EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, NoChange_t, const Scalar& val);
 
-    using Base::setConstant;
-    EIGEN_DEVICE_FUNC Derived& setConstant(Index size, const Scalar& val);
-    EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, Index cols, const Scalar& val);
-    EIGEN_DEVICE_FUNC Derived& setConstant(NoChange_t, Index cols, const Scalar& val);
-    EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, NoChange_t, const Scalar& val);
+  using Base::setZero;
+  EIGEN_DEVICE_FUNC Derived& setZero(Index size);
+  EIGEN_DEVICE_FUNC Derived& setZero(Index rows, Index cols);
+  EIGEN_DEVICE_FUNC Derived& setZero(NoChange_t, Index cols);
+  EIGEN_DEVICE_FUNC Derived& setZero(Index rows, NoChange_t);
 
-    using Base::setZero;
-    EIGEN_DEVICE_FUNC Derived& setZero(Index size);
-    EIGEN_DEVICE_FUNC Derived& setZero(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC Derived& setZero(NoChange_t, Index cols);
-    EIGEN_DEVICE_FUNC Derived& setZero(Index rows, NoChange_t);
+  using Base::setOnes;
+  EIGEN_DEVICE_FUNC Derived& setOnes(Index size);
+  EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, Index cols);
+  EIGEN_DEVICE_FUNC Derived& setOnes(NoChange_t, Index cols);
+  EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, NoChange_t);
 
-    using Base::setOnes;
-    EIGEN_DEVICE_FUNC Derived& setOnes(Index size);
-    EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC Derived& setOnes(NoChange_t, Index cols);
-    EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, NoChange_t);
+  using Base::setRandom;
+  Derived& setRandom(Index size);
+  Derived& setRandom(Index rows, Index cols);
+  Derived& setRandom(NoChange_t, Index cols);
+  Derived& setRandom(Index rows, NoChange_t);
 
-    using Base::setRandom;
-    Derived& setRandom(Index size);
-    Derived& setRandom(Index rows, Index cols);
-    Derived& setRandom(NoChange_t, Index cols);
-    Derived& setRandom(Index rows, NoChange_t);
-
-    #ifdef EIGEN_PLAINOBJECTBASE_PLUGIN
-    #include EIGEN_PLAINOBJECTBASE_PLUGIN
-    #endif
-
-  protected:
-    /** \internal Resizes *this in preparation for assigning \a other to it.
-      * Takes care of doing all the checking that's needed.
-      *
-      * Note that copying a row-vector into a vector (and conversely) is allowed.
-      * The resizing, if any, is then done in the appropriate way so that row-vectors
-      * remain row-vectors and vectors remain vectors.
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _resize_to_match(const EigenBase<OtherDerived>& other)
-    {
-      #ifdef EIGEN_NO_AUTOMATIC_RESIZING
-      eigen_assert((this->size()==0 || (IsVectorAtCompileTime ? (this->size() == other.size())
-                 : (rows() == other.rows() && cols() == other.cols())))
-        && "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined");
-      EIGEN_ONLY_USED_FOR_DEBUG(other);
-      #else
-      resizeLike(other);
-      #endif
-    }
-
-    /**
-      * \brief Copies the value of the expression \a other into \c *this with automatic resizing.
-      *
-      * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
-      * it will be initialized.
-      *
-      * Note that copying a row-vector into a vector (and conversely) is allowed.
-      * The resizing, if any, is then done in the appropriate way so that row-vectors
-      * remain row-vectors and vectors remain vectors.
-      *
-      * \sa operator=(const MatrixBase<OtherDerived>&), _set_noalias()
-      *
-      * \internal
-      */
-    // aliasing is dealt once in internal::call_assignment
-    // so at this stage we have to assume aliasing... and resising has to be done later.
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Derived& _set(const DenseBase<OtherDerived>& other)
-    {
-      internal::call_assignment(this->derived(), other.derived());
-      return this->derived();
-    }
-
-    /** \internal Like _set() but additionally makes the assumption that no aliasing effect can happen (which
-      * is the case when creating a new matrix) so one can enforce lazy evaluation.
-      *
-      * \sa operator=(const MatrixBase<OtherDerived>&), _set()
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Derived& _set_noalias(const DenseBase<OtherDerived>& other)
-    {
-      // I don't think we need this resize call since the lazyAssign will anyways resize
-      // and lazyAssign will be called by the assign selector.
-      //_resize_to_match(other);
-      // the 'false' below means to enforce lazy evaluation. We don't use lazyAssign() because
-      // it wouldn't allow to copy a row-vector into a column-vector.
-      internal::call_assignment_no_alias(this->derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
-      return this->derived();
-    }
-
-    template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init2(Index rows, Index cols, typename internal::enable_if<Base::SizeAtCompileTime!=2,T0>::type* = 0)
-    {
-      const bool t0_is_integer_alike = internal::is_valid_index_type<T0>::value;
-      const bool t1_is_integer_alike = internal::is_valid_index_type<T1>::value;
-      EIGEN_STATIC_ASSERT(t0_is_integer_alike &&
-                          t1_is_integer_alike,
-                          FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED)
-      resize(rows,cols);
-    }
-
-    template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init2(const T0& val0, const T1& val1, typename internal::enable_if<Base::SizeAtCompileTime==2,T0>::type* = 0)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2)
-      m_storage.data()[0] = Scalar(val0);
-      m_storage.data()[1] = Scalar(val1);
-    }
-
-    template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init2(const Index& val0, const Index& val1,
-                                    typename internal::enable_if<    (!internal::is_same<Index,Scalar>::value)
-                                                                  && (internal::is_same<T0,Index>::value)
-                                                                  && (internal::is_same<T1,Index>::value)
-                                                                  && Base::SizeAtCompileTime==2,T1>::type* = 0)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2)
-      m_storage.data()[0] = Scalar(val0);
-      m_storage.data()[1] = Scalar(val1);
-    }
-
-    // The argument is convertible to the Index type and we either have a non 1x1 Matrix, or a dynamic-sized Array,
-    // then the argument is meant to be the size of the object.
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(Index size, typename internal::enable_if<    (Base::SizeAtCompileTime!=1 || !internal::is_convertible<T, Scalar>::value)
-                                                                              && ((!internal::is_same<typename internal::traits<Derived>::XprKind,ArrayXpr>::value || Base::SizeAtCompileTime==Dynamic)),T>::type* = 0)
-    {
-      // NOTE MSVC 2008 complains if we directly put bool(NumTraits<T>::IsInteger) as the EIGEN_STATIC_ASSERT argument.
-      const bool is_integer_alike = internal::is_valid_index_type<T>::value;
-      EIGEN_UNUSED_VARIABLE(is_integer_alike);
-      EIGEN_STATIC_ASSERT(is_integer_alike,
-                          FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED)
-      resize(size);
-    }
-
-    // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar type can be implicitly converted)
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const Scalar& val0, typename internal::enable_if<Base::SizeAtCompileTime==1 && internal::is_convertible<T, Scalar>::value,T>::type* = 0)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1)
-      m_storage.data()[0] = val0;
-    }
-
-    // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar type match the index type)
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const Index& val0,
-                                    typename internal::enable_if<    (!internal::is_same<Index,Scalar>::value)
-                                                                  && (internal::is_same<Index,T>::value)
-                                                                  && Base::SizeAtCompileTime==1
-                                                                  && internal::is_convertible<T, Scalar>::value,T*>::type* = 0)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1)
-      m_storage.data()[0] = Scalar(val0);
-    }
-
-    // Initialize a fixed size matrix from a pointer to raw data
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const Scalar* data){
-      this->_set_noalias(ConstMapType(data));
-    }
-
-    // Initialize an arbitrary matrix from a dense expression
-    template<typename T, typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const DenseBase<OtherDerived>& other){
-      this->_set_noalias(other);
-    }
-
-    // Initialize an arbitrary matrix from an object convertible to the Derived type.
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const Derived& other){
-      this->_set_noalias(other);
-    }
-
-    // Initialize an arbitrary matrix from a generic Eigen expression
-    template<typename T, typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const EigenBase<OtherDerived>& other){
-      this->derived() = other;
-    }
-
-    template<typename T, typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const ReturnByValue<OtherDerived>& other)
-    {
-      resize(other.rows(), other.cols());
-      other.evalTo(this->derived());
-    }
-
-    template<typename T, typename OtherDerived, int ColsAtCompileTime>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const RotationBase<OtherDerived,ColsAtCompileTime>& r)
-    {
-      this->derived() = r;
-    }
-
-    // For fixed-size Array<Scalar,...>
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const Scalar& val0,
-                                    typename internal::enable_if<    Base::SizeAtCompileTime!=Dynamic
-                                                                  && Base::SizeAtCompileTime!=1
-                                                                  && internal::is_convertible<T, Scalar>::value
-                                                                  && internal::is_same<typename internal::traits<Derived>::XprKind,ArrayXpr>::value,T>::type* = 0)
-    {
-      Base::setConstant(val0);
-    }
-
-    // For fixed-size Array<Index,...>
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _init1(const Index& val0,
-                                    typename internal::enable_if<    (!internal::is_same<Index,Scalar>::value)
-                                                                  && (internal::is_same<Index,T>::value)
-                                                                  && Base::SizeAtCompileTime!=Dynamic
-                                                                  && Base::SizeAtCompileTime!=1
-                                                                  && internal::is_convertible<T, Scalar>::value
-                                                                  && internal::is_same<typename internal::traits<Derived>::XprKind,ArrayXpr>::value,T*>::type* = 0)
-    {
-      Base::setConstant(val0);
-    }
-
-    template<typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers>
-    friend struct internal::matrix_swap_impl;
-
-  public:
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal
-      * \brief Override DenseBase::swap() since for dynamic-sized matrices
-      * of same type it is enough to swap the data pointers.
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void swap(DenseBase<OtherDerived> & other)
-    {
-      enum { SwapPointers = internal::is_same<Derived, OtherDerived>::value && Base::SizeAtCompileTime==Dynamic };
-      internal::matrix_swap_impl<Derived, OtherDerived, bool(SwapPointers)>::run(this->derived(), other.derived());
-    }
-
-    /** \internal
-      * \brief const version forwarded to DenseBase::swap
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void swap(DenseBase<OtherDerived> const & other)
-    { Base::swap(other.derived()); }
-
-    EIGEN_DEVICE_FUNC
-    static EIGEN_STRONG_INLINE void _check_template_params()
-    {
-      EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, (int(Options)&RowMajor)==RowMajor)
-                        && EIGEN_IMPLIES(MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1, (int(Options)&RowMajor)==0)
-                        && ((RowsAtCompileTime == Dynamic) || (RowsAtCompileTime >= 0))
-                        && ((ColsAtCompileTime == Dynamic) || (ColsAtCompileTime >= 0))
-                        && ((MaxRowsAtCompileTime == Dynamic) || (MaxRowsAtCompileTime >= 0))
-                        && ((MaxColsAtCompileTime == Dynamic) || (MaxColsAtCompileTime >= 0))
-                        && (MaxRowsAtCompileTime == RowsAtCompileTime || RowsAtCompileTime==Dynamic)
-                        && (MaxColsAtCompileTime == ColsAtCompileTime || ColsAtCompileTime==Dynamic)
-                        && (Options & (DontAlign|RowMajor)) == Options),
-        INVALID_MATRIX_TEMPLATE_PARAMETERS)
-    }
-
-    enum { IsPlainObjectBase = 1 };
+#ifdef EIGEN_PLAINOBJECTBASE_PLUGIN
+#include EIGEN_PLAINOBJECTBASE_PLUGIN
 #endif
-  public:
-    // These apparently need to be down here for nvcc+icc to prevent duplicate
-    // Map symbol.
-    template<typename PlainObjectType, int MapOptions, typename StrideType> friend class Eigen::Map;
-    friend class Eigen::Map<Derived, Unaligned>;
-    friend class Eigen::Map<const Derived, Unaligned>;
-#if EIGEN_MAX_ALIGN_BYTES>0
-    // for EIGEN_MAX_ALIGN_BYTES==0, AlignedMax==Unaligned, and many compilers generate warnings for friend-ing a class twice.
-    friend class Eigen::Map<Derived, AlignedMax>;
-    friend class Eigen::Map<const Derived, AlignedMax>;
+
+ protected:
+  /** \internal Resizes *this in preparation for assigning \a other to it.
+   * Takes care of doing all the checking that's needed.
+   *
+   * Note that copying a row-vector into a vector (and conversely) is allowed.
+   * The resizing, if any, is then done in the appropriate way so that row-vectors
+   * remain row-vectors and vectors remain vectors.
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _resize_to_match(const EigenBase<OtherDerived>& other) {
+#ifdef EIGEN_NO_AUTOMATIC_RESIZING
+    eigen_assert((this->size() == 0 || (IsVectorAtCompileTime ? (this->size() == other.size())
+                                                              : (rows() == other.rows() && cols() == other.cols()))) &&
+                 "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined");
+    EIGEN_ONLY_USED_FOR_DEBUG(other);
+#else
+    resizeLike(other);
+#endif
+  }
+
+  /**
+   * \brief Copies the value of the expression \a other into \c *this with automatic resizing.
+   *
+   * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
+   * it will be initialized.
+   *
+   * Note that copying a row-vector into a vector (and conversely) is allowed.
+   * The resizing, if any, is then done in the appropriate way so that row-vectors
+   * remain row-vectors and vectors remain vectors.
+   *
+   * \sa operator=(const MatrixBase<OtherDerived>&), _set_noalias()
+   *
+   * \internal
+   */
+  // aliasing is dealt once in internal::call_assignment
+  // so at this stage we have to assume aliasing... and resising has to be done later.
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& _set(const DenseBase<OtherDerived>& other) {
+    internal::call_assignment(this->derived(), other.derived());
+    return this->derived();
+  }
+
+  /** \internal Like _set() but additionally makes the assumption that no aliasing effect can happen (which
+   * is the case when creating a new matrix) so one can enforce lazy evaluation.
+   *
+   * \sa operator=(const MatrixBase<OtherDerived>&), _set()
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& _set_noalias(const DenseBase<OtherDerived>& other) {
+    // I don't think we need this resize call since the lazyAssign will anyways resize
+    // and lazyAssign will be called by the assign selector.
+    //_resize_to_match(other);
+    // the 'false' below means to enforce lazy evaluation. We don't use lazyAssign() because
+    // it wouldn't allow to copy a row-vector into a column-vector.
+    internal::call_assignment_no_alias(this->derived(), other.derived(),
+                                       internal::assign_op<Scalar, typename OtherDerived::Scalar>());
+    return this->derived();
+  }
+
+  template <typename T0, typename T1>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2(Index rows, Index cols,
+                                                    std::enable_if_t<Base::SizeAtCompileTime != 2, T0>* = 0) {
+    EIGEN_STATIC_ASSERT(internal::is_valid_index_type<T0>::value && internal::is_valid_index_type<T1>::value,
+                        T0 AND T1 MUST BE INTEGER TYPES)
+    resize(rows, cols);
+  }
+
+  template <typename T0, typename T1>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2(const T0& val0, const T1& val1,
+                                                    std::enable_if_t<Base::SizeAtCompileTime == 2, T0>* = 0) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2)
+    m_storage.data()[0] = Scalar(val0);
+    m_storage.data()[1] = Scalar(val1);
+  }
+
+  template <typename T0, typename T1>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2(
+      const Index& val0, const Index& val1,
+      std::enable_if_t<(!internal::is_same<Index, Scalar>::value) && (internal::is_same<T0, Index>::value) &&
+                           (internal::is_same<T1, Index>::value) && Base::SizeAtCompileTime == 2,
+                       T1>* = 0) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2)
+    m_storage.data()[0] = Scalar(val0);
+    m_storage.data()[1] = Scalar(val1);
+  }
+
+  // The argument is convertible to the Index type and we either have a non 1x1 Matrix, or a dynamic-sized Array,
+  // then the argument is meant to be the size of the object.
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(
+      Index size,
+      std::enable_if_t<(Base::SizeAtCompileTime != 1 || !internal::is_convertible<T, Scalar>::value) &&
+                           ((!internal::is_same<typename internal::traits<Derived>::XprKind, ArrayXpr>::value ||
+                             Base::SizeAtCompileTime == Dynamic)),
+                       T>* = 0) {
+    // NOTE MSVC 2008 complains if we directly put bool(NumTraits<T>::IsInteger) as the EIGEN_STATIC_ASSERT argument.
+    const bool is_integer_alike = internal::is_valid_index_type<T>::value;
+    EIGEN_UNUSED_VARIABLE(is_integer_alike);
+    EIGEN_STATIC_ASSERT(is_integer_alike, FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED)
+    resize(size);
+  }
+
+  // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar
+  // type can be implicitly converted)
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(
+      const Scalar& val0,
+      std::enable_if_t<Base::SizeAtCompileTime == 1 && internal::is_convertible<T, Scalar>::value, T>* = 0) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1)
+    m_storage.data()[0] = val0;
+  }
+
+  // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar
+  // type match the index type)
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(
+      const Index& val0,
+      std::enable_if_t<(!internal::is_same<Index, Scalar>::value) && (internal::is_same<Index, T>::value) &&
+                           Base::SizeAtCompileTime == 1 && internal::is_convertible<T, Scalar>::value,
+                       T*>* = 0) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1)
+    m_storage.data()[0] = Scalar(val0);
+  }
+
+  // Initialize a fixed size matrix from a pointer to raw data
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const Scalar* data) {
+    this->_set_noalias(ConstMapType(data));
+  }
+
+  // Initialize an arbitrary matrix from a dense expression
+  template <typename T, typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const DenseBase<OtherDerived>& other) {
+    this->_set_noalias(other);
+  }
+
+  // Initialize an arbitrary matrix from an object convertible to the Derived type.
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const Derived& other) {
+    this->_set_noalias(other);
+  }
+
+  // Initialize an arbitrary matrix from a generic Eigen expression
+  template <typename T, typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const EigenBase<OtherDerived>& other) {
+    this->derived() = other;
+  }
+
+  template <typename T, typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const ReturnByValue<OtherDerived>& other) {
+    resize(other.rows(), other.cols());
+    other.evalTo(this->derived());
+  }
+
+  template <typename T, typename OtherDerived, int ColsAtCompileTime>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const RotationBase<OtherDerived, ColsAtCompileTime>& r) {
+    this->derived() = r;
+  }
+
+  // For fixed-size Array<Scalar,...>
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(
+      const Scalar& val0,
+      std::enable_if_t<Base::SizeAtCompileTime != Dynamic && Base::SizeAtCompileTime != 1 &&
+                           internal::is_convertible<T, Scalar>::value &&
+                           internal::is_same<typename internal::traits<Derived>::XprKind, ArrayXpr>::value,
+                       T>* = 0) {
+    Base::setConstant(val0);
+  }
+
+  // For fixed-size Array<Index,...>
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(
+      const Index& val0,
+      std::enable_if_t<(!internal::is_same<Index, Scalar>::value) && (internal::is_same<Index, T>::value) &&
+                           Base::SizeAtCompileTime != Dynamic && Base::SizeAtCompileTime != 1 &&
+                           internal::is_convertible<T, Scalar>::value &&
+                           internal::is_same<typename internal::traits<Derived>::XprKind, ArrayXpr>::value,
+                       T*>* = 0) {
+    Base::setConstant(val0);
+  }
+
+  template <typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers>
+  friend struct internal::matrix_swap_impl;
+
+ public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** \internal
+   * \brief Override DenseBase::swap() since for dynamic-sized matrices
+   * of same type it is enough to swap the data pointers.
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(DenseBase<OtherDerived>& other) {
+    enum {SwapPointers = internal::is_same<Derived, OtherDerived>::value && Base::SizeAtCompileTime == Dynamic};
+    internal::matrix_swap_impl<Derived, OtherDerived, bool(SwapPointers)>::run(this->derived(), other.derived());
+  }
+
+  /** \internal
+   * \brief const version forwarded to DenseBase::swap
+   */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(DenseBase<OtherDerived> const& other) {
+    Base::swap(other.derived());
+  }
+
+  enum {IsPlainObjectBase = 1};
+#endif
+ public:
+  // These apparently need to be down here for nvcc+icc to prevent duplicate
+  // Map symbol.
+  template <typename PlainObjectType, int MapOptions, typename StrideType>
+  friend class Eigen::Map;
+  friend class Eigen::Map<Derived, Unaligned>;
+  friend class Eigen::Map<const Derived, Unaligned>;
+#if EIGEN_MAX_ALIGN_BYTES > 0
+  // for EIGEN_MAX_ALIGN_BYTES==0, AlignedMax==Unaligned, and many compilers generate warnings for friend-ing a class
+  // twice.
+  friend class Eigen::Map<Derived, AlignedMax>;
+  friend class Eigen::Map<const Derived, AlignedMax>;
 #endif
 };
 
 namespace internal {
 
 template <typename Derived, typename OtherDerived, bool IsVector>
-struct conservative_resize_like_impl
-{
-  #if EIGEN_HAS_TYPE_TRAITS
-  static const bool IsRelocatable = std::is_trivially_copyable<typename Derived::Scalar>::value;
-  #else
-  static const bool IsRelocatable = !NumTraits<typename Derived::Scalar>::RequireInitialization;
-  #endif
-  static void run(DenseBase<Derived>& _this, Index rows, Index cols)
-  {
+struct conservative_resize_like_impl {
+  static constexpr bool IsRelocatable = std::is_trivially_copyable<typename Derived::Scalar>::value;
+  static void run(DenseBase<Derived>& _this, Index rows, Index cols) {
     if (_this.rows() == rows && _this.cols() == cols) return;
     EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(Derived)
 
-    if ( IsRelocatable
-          && (( Derived::IsRowMajor && _this.cols() == cols) ||  // row-major and we change only the number of rows
-              (!Derived::IsRowMajor && _this.rows() == rows) ))  // column-major and we change only the number of columns
+    if (IsRelocatable &&
+        ((Derived::IsRowMajor && _this.cols() == cols) ||  // row-major and we change only the number of rows
+         (!Derived::IsRowMajor && _this.rows() == rows)))  // column-major and we change only the number of columns
     {
-      internal::check_rows_cols_for_overflow<Derived::MaxSizeAtCompileTime>::run(rows, cols);
-      _this.derived().m_storage.conservativeResize(rows*cols,rows,cols);
-    }
-    else
-    {
+      internal::check_rows_cols_for_overflow<Derived::MaxSizeAtCompileTime, Derived::MaxRowsAtCompileTime,
+                                             Derived::MaxColsAtCompileTime>::run(rows, cols);
+      _this.derived().m_storage.conservativeResize(rows * cols, rows, cols);
+    } else {
       // The storage order does not allow us to use reallocation.
-      Derived tmp(rows,cols);
+      Derived tmp(rows, cols);
       const Index common_rows = numext::mini(rows, _this.rows());
       const Index common_cols = numext::mini(cols, _this.cols());
-      tmp.block(0,0,common_rows,common_cols) = _this.block(0,0,common_rows,common_cols);
+      tmp.block(0, 0, common_rows, common_cols) = _this.block(0, 0, common_rows, common_cols);
       _this.derived().swap(tmp);
     }
   }
 
-  static void run(DenseBase<Derived>& _this, const DenseBase<OtherDerived>& other)
-  {
+  static void run(DenseBase<Derived>& _this, const DenseBase<OtherDerived>& other) {
     if (_this.rows() == other.rows() && _this.cols() == other.cols()) return;
 
     // Note: Here is space for improvement. Basically, for conservativeResize(Index,Index),
@@ -1039,25 +963,24 @@
     EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(Derived)
     EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(OtherDerived)
 
-    if ( IsRelocatable &&
-          (( Derived::IsRowMajor && _this.cols() == other.cols()) ||  // row-major and we change only the number of rows
-           (!Derived::IsRowMajor && _this.rows() == other.rows()) ))  // column-major and we change only the number of columns
+    if (IsRelocatable &&
+        ((Derived::IsRowMajor && _this.cols() == other.cols()) ||  // row-major and we change only the number of rows
+         (!Derived::IsRowMajor &&
+          _this.rows() == other.rows())))  // column-major and we change only the number of columns
     {
       const Index new_rows = other.rows() - _this.rows();
       const Index new_cols = other.cols() - _this.cols();
-      _this.derived().m_storage.conservativeResize(other.size(),other.rows(),other.cols());
-      if (new_rows>0)
+      _this.derived().m_storage.conservativeResize(other.size(), other.rows(), other.cols());
+      if (new_rows > 0)
         _this.bottomRightCorner(new_rows, other.cols()) = other.bottomRows(new_rows);
-      else if (new_cols>0)
+      else if (new_cols > 0)
         _this.bottomRightCorner(other.rows(), new_cols) = other.rightCols(new_cols);
-    }
-    else
-    {
+    } else {
       // The storage order does not allow us to use reallocation.
       Derived tmp(other);
       const Index common_rows = numext::mini(tmp.rows(), _this.rows());
       const Index common_cols = numext::mini(tmp.cols(), _this.cols());
-      tmp.block(0,0,common_rows,common_cols) = _this.block(0,0,common_rows,common_cols);
+      tmp.block(0, 0, common_rows, common_cols) = _this.block(0, 0, common_rows, common_cols);
       _this.derived().swap(tmp);
     }
   }
@@ -1066,63 +989,51 @@
 // Here, the specialization for vectors inherits from the general matrix case
 // to allow calling .conservativeResize(rows,cols) on vectors.
 template <typename Derived, typename OtherDerived>
-struct conservative_resize_like_impl<Derived,OtherDerived,true>
-  : conservative_resize_like_impl<Derived,OtherDerived,false>
-{
-  typedef conservative_resize_like_impl<Derived,OtherDerived,false> Base;
-  using Base::run;
+struct conservative_resize_like_impl<Derived, OtherDerived, true>
+    : conservative_resize_like_impl<Derived, OtherDerived, false> {
+  typedef conservative_resize_like_impl<Derived, OtherDerived, false> Base;
   using Base::IsRelocatable;
+  using Base::run;
 
-  static void run(DenseBase<Derived>& _this, Index size)
-  {
-    const Index new_rows = Derived::RowsAtCompileTime==1 ? 1 : size;
-    const Index new_cols = Derived::RowsAtCompileTime==1 ? size : 1;
-    if(IsRelocatable)
-      _this.derived().m_storage.conservativeResize(size,new_rows,new_cols);
+  static void run(DenseBase<Derived>& _this, Index size) {
+    const Index new_rows = Derived::RowsAtCompileTime == 1 ? 1 : size;
+    const Index new_cols = Derived::RowsAtCompileTime == 1 ? size : 1;
+    if (IsRelocatable)
+      _this.derived().m_storage.conservativeResize(size, new_rows, new_cols);
     else
       Base::run(_this.derived(), new_rows, new_cols);
   }
 
-  static void run(DenseBase<Derived>& _this, const DenseBase<OtherDerived>& other)
-  {
+  static void run(DenseBase<Derived>& _this, const DenseBase<OtherDerived>& other) {
     if (_this.rows() == other.rows() && _this.cols() == other.cols()) return;
 
     const Index num_new_elements = other.size() - _this.size();
 
-    const Index new_rows = Derived::RowsAtCompileTime==1 ? 1 : other.rows();
-    const Index new_cols = Derived::RowsAtCompileTime==1 ? other.cols() : 1;
-    if(IsRelocatable)
-      _this.derived().m_storage.conservativeResize(other.size(),new_rows,new_cols);
+    const Index new_rows = Derived::RowsAtCompileTime == 1 ? 1 : other.rows();
+    const Index new_cols = Derived::RowsAtCompileTime == 1 ? other.cols() : 1;
+    if (IsRelocatable)
+      _this.derived().m_storage.conservativeResize(other.size(), new_rows, new_cols);
     else
       Base::run(_this.derived(), new_rows, new_cols);
 
-    if (num_new_elements > 0)
-      _this.tail(num_new_elements) = other.tail(num_new_elements);
+    if (num_new_elements > 0) _this.tail(num_new_elements) = other.tail(num_new_elements);
   }
 };
 
-template<typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers>
-struct matrix_swap_impl
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE void run(MatrixTypeA& a, MatrixTypeB& b)
-  {
-    a.base().swap(b);
-  }
+template <typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers>
+struct matrix_swap_impl {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(MatrixTypeA& a, MatrixTypeB& b) { a.base().swap(b); }
 };
 
-template<typename MatrixTypeA, typename MatrixTypeB>
-struct matrix_swap_impl<MatrixTypeA, MatrixTypeB, true>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(MatrixTypeA& a, MatrixTypeB& b)
-  {
+template <typename MatrixTypeA, typename MatrixTypeB>
+struct matrix_swap_impl<MatrixTypeA, MatrixTypeB, true> {
+  EIGEN_DEVICE_FUNC static inline void run(MatrixTypeA& a, MatrixTypeB& b) {
     static_cast<typename MatrixTypeA::Base&>(a).m_storage.swap(static_cast<typename MatrixTypeB::Base&>(b).m_storage);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DENSESTORAGEBASE_H
+#endif  // EIGEN_DENSESTORAGEBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h
index 70a6c10..6bad832 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h
@@ -10,182 +10,165 @@
 #ifndef EIGEN_PRODUCT_H
 #define EIGEN_PRODUCT_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename Lhs, typename Rhs, int Option, typename StorageKind> class ProductImpl;
+template <typename Lhs, typename Rhs, int Option, typename StorageKind>
+class ProductImpl;
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, int Option>
-struct traits<Product<Lhs, Rhs, Option> >
-{
-  typedef typename remove_all<Lhs>::type LhsCleaned;
-  typedef typename remove_all<Rhs>::type RhsCleaned;
+template <typename Lhs, typename Rhs, int Option>
+struct traits<Product<Lhs, Rhs, Option> > {
+  typedef remove_all_t<Lhs> LhsCleaned;
+  typedef remove_all_t<Rhs> RhsCleaned;
   typedef traits<LhsCleaned> LhsTraits;
   typedef traits<RhsCleaned> RhsTraits;
 
   typedef MatrixXpr XprKind;
 
-  typedef typename ScalarBinaryOpTraits<typename traits<LhsCleaned>::Scalar, typename traits<RhsCleaned>::Scalar>::ReturnType Scalar;
-  typedef typename product_promote_storage_type<typename LhsTraits::StorageKind,
-                                                typename RhsTraits::StorageKind,
-                                                internal::product_type<Lhs,Rhs>::ret>::ret StorageKind;
-  typedef typename promote_index_type<typename LhsTraits::StorageIndex,
-                                      typename RhsTraits::StorageIndex>::type StorageIndex;
+  typedef typename ScalarBinaryOpTraits<typename traits<LhsCleaned>::Scalar,
+                                        typename traits<RhsCleaned>::Scalar>::ReturnType Scalar;
+  typedef typename product_promote_storage_type<typename LhsTraits::StorageKind, typename RhsTraits::StorageKind,
+                                                internal::product_type<Lhs, Rhs>::ret>::ret StorageKind;
+  typedef typename promote_index_type<typename LhsTraits::StorageIndex, typename RhsTraits::StorageIndex>::type
+      StorageIndex;
 
   enum {
-    RowsAtCompileTime    = LhsTraits::RowsAtCompileTime,
-    ColsAtCompileTime    = RhsTraits::ColsAtCompileTime,
+    RowsAtCompileTime = LhsTraits::RowsAtCompileTime,
+    ColsAtCompileTime = RhsTraits::ColsAtCompileTime,
     MaxRowsAtCompileTime = LhsTraits::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = RhsTraits::MaxColsAtCompileTime,
 
     // FIXME: only needed by GeneralMatrixMatrixTriangular
-    InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(LhsTraits::ColsAtCompileTime, RhsTraits::RowsAtCompileTime),
+    InnerSize = min_size_prefer_fixed(LhsTraits::ColsAtCompileTime, RhsTraits::RowsAtCompileTime),
 
     // The storage order is somewhat arbitrary here. The correct one will be determined through the evaluator.
-    Flags = (MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1) ? RowMajorBit
-          : (MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1) ? 0
-          : (   ((LhsTraits::Flags&NoPreferredStorageOrderBit) && (RhsTraits::Flags&RowMajorBit))
-             || ((RhsTraits::Flags&NoPreferredStorageOrderBit) && (LhsTraits::Flags&RowMajorBit)) ) ? RowMajorBit
-          : NoPreferredStorageOrderBit
+    Flags = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1)   ? RowMajorBit
+            : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0
+            : (((LhsTraits::Flags & NoPreferredStorageOrderBit) && (RhsTraits::Flags & RowMajorBit)) ||
+               ((RhsTraits::Flags & NoPreferredStorageOrderBit) && (LhsTraits::Flags & RowMajorBit)))
+                ? RowMajorBit
+                : NoPreferredStorageOrderBit
   };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \class Product
-  * \ingroup Core_Module
-  *
-  * \brief Expression of the product of two arbitrary matrices or vectors
-  *
-  * \tparam _Lhs the type of the left-hand side expression
-  * \tparam _Rhs the type of the right-hand side expression
-  *
-  * This class represents an expression of the product of two arbitrary matrices.
-  *
-  * The other template parameters are:
-  * \tparam Option     can be DefaultProduct, AliasFreeProduct, or LazyProduct
-  *
-  */
-template<typename _Lhs, typename _Rhs, int Option>
-class Product : public ProductImpl<_Lhs,_Rhs,Option,
-                                   typename internal::product_promote_storage_type<typename internal::traits<_Lhs>::StorageKind,
-                                                                                   typename internal::traits<_Rhs>::StorageKind,
-                                                                                   internal::product_type<_Lhs,_Rhs>::ret>::ret>
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Expression of the product of two arbitrary matrices or vectors
+ *
+ * \tparam Lhs_ the type of the left-hand side expression
+ * \tparam Rhs_ the type of the right-hand side expression
+ *
+ * This class represents an expression of the product of two arbitrary matrices.
+ *
+ * The other template parameters are:
+ * \tparam Option     can be DefaultProduct, AliasFreeProduct, or LazyProduct
+ *
+ */
+template <typename Lhs_, typename Rhs_, int Option>
+class Product
+    : public ProductImpl<Lhs_, Rhs_, Option,
+                         typename internal::product_promote_storage_type<
+                             typename internal::traits<Lhs_>::StorageKind, typename internal::traits<Rhs_>::StorageKind,
+                             internal::product_type<Lhs_, Rhs_>::ret>::ret> {
+ public:
+  typedef Lhs_ Lhs;
+  typedef Rhs_ Rhs;
 
-    typedef _Lhs Lhs;
-    typedef _Rhs Rhs;
+  typedef
+      typename ProductImpl<Lhs, Rhs, Option,
+                           typename internal::product_promote_storage_type<
+                               typename internal::traits<Lhs>::StorageKind, typename internal::traits<Rhs>::StorageKind,
+                               internal::product_type<Lhs, Rhs>::ret>::ret>::Base Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(Product)
 
-    typedef typename ProductImpl<
-        Lhs, Rhs, Option,
-        typename internal::product_promote_storage_type<typename internal::traits<Lhs>::StorageKind,
-                                                        typename internal::traits<Rhs>::StorageKind,
-                                                        internal::product_type<Lhs,Rhs>::ret>::ret>::Base Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(Product)
+  typedef typename internal::ref_selector<Lhs>::type LhsNested;
+  typedef typename internal::ref_selector<Rhs>::type RhsNested;
+  typedef internal::remove_all_t<LhsNested> LhsNestedCleaned;
+  typedef internal::remove_all_t<RhsNested> RhsNestedCleaned;
 
-    typedef typename internal::ref_selector<Lhs>::type LhsNested;
-    typedef typename internal::ref_selector<Rhs>::type RhsNested;
-    typedef typename internal::remove_all<LhsNested>::type LhsNestedCleaned;
-    typedef typename internal::remove_all<RhsNested>::type RhsNestedCleaned;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Product(const Lhs& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) {
+    eigen_assert(lhs.cols() == rhs.rows() && "invalid matrix product" &&
+                 "if you wanted a coeff-wise or a dot product use the respective explicit functions");
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Product(const Lhs& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs)
-    {
-      eigen_assert(lhs.cols() == rhs.rows()
-        && "invalid matrix product"
-        && "if you wanted a coeff-wise or a dot product use the respective explicit functions");
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const LhsNestedCleaned& lhs() const { return m_lhs; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const RhsNestedCleaned& rhs() const { return m_rhs; }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const LhsNestedCleaned& lhs() const { return m_lhs; }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const RhsNestedCleaned& rhs() const { return m_rhs; }
-
-  protected:
-
-    LhsNested m_lhs;
-    RhsNested m_rhs;
+ protected:
+  LhsNested m_lhs;
+  RhsNested m_rhs;
 };
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, int Option, int ProductTag = internal::product_type<Lhs,Rhs>::ret>
-class dense_product_base
- : public internal::dense_xpr_base<Product<Lhs,Rhs,Option> >::type
-{};
+template <typename Lhs, typename Rhs, int Option, int ProductTag = internal::product_type<Lhs, Rhs>::ret>
+class dense_product_base : public internal::dense_xpr_base<Product<Lhs, Rhs, Option> >::type {};
 
 /** Conversion to scalar for inner-products */
-template<typename Lhs, typename Rhs, int Option>
+template <typename Lhs, typename Rhs, int Option>
 class dense_product_base<Lhs, Rhs, Option, InnerProduct>
- : public internal::dense_xpr_base<Product<Lhs,Rhs,Option> >::type
-{
-  typedef Product<Lhs,Rhs,Option> ProductXpr;
+    : public internal::dense_xpr_base<Product<Lhs, Rhs, Option> >::type {
+  typedef Product<Lhs, Rhs, Option> ProductXpr;
   typedef typename internal::dense_xpr_base<ProductXpr>::type Base;
-public:
+
+ public:
   using Base::derived;
   typedef typename Base::Scalar Scalar;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator const Scalar() const
-  {
-    return internal::evaluator<ProductXpr>(derived()).coeff(0,0);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator const Scalar() const {
+    return internal::evaluator<ProductXpr>(derived()).coeff(0, 0);
   }
 };
 
-} // namespace internal
+}  // namespace internal
 
 // Generic API dispatcher
-template<typename Lhs, typename Rhs, int Option, typename StorageKind>
-class ProductImpl : public internal::generic_xpr_base<Product<Lhs,Rhs,Option>, MatrixXpr, StorageKind>::type
-{
-  public:
-    typedef typename internal::generic_xpr_base<Product<Lhs,Rhs,Option>, MatrixXpr, StorageKind>::type Base;
+template <typename Lhs, typename Rhs, int Option, typename StorageKind>
+class ProductImpl : public internal::generic_xpr_base<Product<Lhs, Rhs, Option>, MatrixXpr, StorageKind>::type {
+ public:
+  typedef typename internal::generic_xpr_base<Product<Lhs, Rhs, Option>, MatrixXpr, StorageKind>::type Base;
 };
 
-template<typename Lhs, typename Rhs, int Option>
-class ProductImpl<Lhs,Rhs,Option,Dense>
-  : public internal::dense_product_base<Lhs,Rhs,Option>
-{
-    typedef Product<Lhs, Rhs, Option> Derived;
+template <typename Lhs, typename Rhs, int Option>
+class ProductImpl<Lhs, Rhs, Option, Dense> : public internal::dense_product_base<Lhs, Rhs, Option> {
+  typedef Product<Lhs, Rhs, Option> Derived;
 
-  public:
+ public:
+  typedef typename internal::dense_product_base<Lhs, Rhs, Option> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
+ protected:
+  enum {
+    IsOneByOne = (RowsAtCompileTime == 1 || RowsAtCompileTime == Dynamic) &&
+                 (ColsAtCompileTime == 1 || ColsAtCompileTime == Dynamic),
+    EnableCoeff = IsOneByOne || Option == LazyProduct
+  };
 
-    typedef typename internal::dense_product_base<Lhs, Rhs, Option> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
-  protected:
-    enum {
-      IsOneByOne = (RowsAtCompileTime == 1 || RowsAtCompileTime == Dynamic) &&
-                   (ColsAtCompileTime == 1 || ColsAtCompileTime == Dynamic),
-      EnableCoeff = IsOneByOne || Option==LazyProduct
-    };
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index row, Index col) const {
+    EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
+    eigen_assert((Option == LazyProduct) || (this->rows() == 1 && this->cols() == 1));
 
-  public:
+    return internal::evaluator<Derived>(derived()).coeff(row, col);
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index row, Index col) const
-    {
-      EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
-      eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) );
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index i) const {
+    EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
+    eigen_assert((Option == LazyProduct) || (this->rows() == 1 && this->cols() == 1));
 
-      return internal::evaluator<Derived>(derived()).coeff(row,col);
-    }
-
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index i) const
-    {
-      EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
-      eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) );
-
-      return internal::evaluator<Derived>(derived()).coeff(i);
-    }
-
-
+    return internal::evaluator<Derived>(derived()).coeff(i);
+  }
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PRODUCT_H
+#endif  // EIGEN_PRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
index 8cf294b..19c2560 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
@@ -9,26 +9,26 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 #ifndef EIGEN_PRODUCTEVALUATORS_H
 #define EIGEN_PRODUCTEVALUATORS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 /** \internal
-  * Evaluator of a product expression.
-  * Since products require special treatments to handle all possible cases,
-  * we simply defer the evaluation logic to a product_evaluator class
-  * which offers more partial specialization possibilities.
-  *
-  * \sa class product_evaluator
-  */
-template<typename Lhs, typename Rhs, int Options>
-struct evaluator<Product<Lhs, Rhs, Options> >
- : public product_evaluator<Product<Lhs, Rhs, Options> >
-{
+ * Evaluator of a product expression.
+ * Since products require special treatments to handle all possible cases,
+ * we simply defer the evaluation logic to a product_evaluator class
+ * which offers more partial specialization possibilities.
+ *
+ * \sa class product_evaluator
+ */
+template <typename Lhs, typename Rhs, int Options>
+struct evaluator<Product<Lhs, Rhs, Options>> : public product_evaluator<Product<Lhs, Rhs, Options>> {
   typedef Product<Lhs, Rhs, Options> XprType;
   typedef product_evaluator<XprType> Base;
 
@@ -37,94 +37,82 @@
 
 // Catch "scalar * ( A * B )" and transform it to "(A*scalar) * B"
 // TODO we should apply that rule only if that's really helpful
-template<typename Lhs, typename Rhs, typename Scalar1, typename Scalar2, typename Plain1>
-struct evaluator_assume_aliasing<CwiseBinaryOp<internal::scalar_product_op<Scalar1,Scalar2>,
+template <typename Lhs, typename Rhs, typename Scalar1, typename Scalar2, typename Plain1>
+struct evaluator_assume_aliasing<CwiseBinaryOp<internal::scalar_product_op<Scalar1, Scalar2>,
                                                const CwiseNullaryOp<internal::scalar_constant_op<Scalar1>, Plain1>,
-                                               const Product<Lhs, Rhs, DefaultProduct> > >
-{
+                                               const Product<Lhs, Rhs, DefaultProduct>>> {
   static const bool value = true;
 };
-template<typename Lhs, typename Rhs, typename Scalar1, typename Scalar2, typename Plain1>
-struct evaluator<CwiseBinaryOp<internal::scalar_product_op<Scalar1,Scalar2>,
+template <typename Lhs, typename Rhs, typename Scalar1, typename Scalar2, typename Plain1>
+struct evaluator<CwiseBinaryOp<internal::scalar_product_op<Scalar1, Scalar2>,
                                const CwiseNullaryOp<internal::scalar_constant_op<Scalar1>, Plain1>,
-                               const Product<Lhs, Rhs, DefaultProduct> > >
- : public evaluator<Product<EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar1,Lhs,product), Rhs, DefaultProduct> >
-{
-  typedef CwiseBinaryOp<internal::scalar_product_op<Scalar1,Scalar2>,
-                               const CwiseNullaryOp<internal::scalar_constant_op<Scalar1>, Plain1>,
-                               const Product<Lhs, Rhs, DefaultProduct> > XprType;
-  typedef evaluator<Product<EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar1,Lhs,product), Rhs, DefaultProduct> > Base;
+                               const Product<Lhs, Rhs, DefaultProduct>>>
+    : public evaluator<Product<EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar1, Lhs, product), Rhs, DefaultProduct>> {
+  typedef CwiseBinaryOp<internal::scalar_product_op<Scalar1, Scalar2>,
+                        const CwiseNullaryOp<internal::scalar_constant_op<Scalar1>, Plain1>,
+                        const Product<Lhs, Rhs, DefaultProduct>>
+      XprType;
+  typedef evaluator<Product<EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar1, Lhs, product), Rhs, DefaultProduct>> Base;
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr)
-    : Base(xpr.lhs().functor().m_other * xpr.rhs().lhs() * xpr.rhs().rhs())
-  {}
+      : Base(xpr.lhs().functor().m_other * xpr.rhs().lhs() * xpr.rhs().rhs()) {}
 };
 
-
-template<typename Lhs, typename Rhs, int DiagIndex>
-struct evaluator<Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex> >
- : public evaluator<Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex> >
-{
+template <typename Lhs, typename Rhs, int DiagIndex>
+struct evaluator<Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex>>
+    : public evaluator<Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex>> {
   typedef Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex> XprType;
-  typedef evaluator<Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex> > Base;
+  typedef evaluator<Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex>> Base;
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr)
-    : Base(Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex>(
-        Product<Lhs, Rhs, LazyProduct>(xpr.nestedExpression().lhs(), xpr.nestedExpression().rhs()),
-        xpr.index() ))
-  {}
+      : Base(Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex>(
+            Product<Lhs, Rhs, LazyProduct>(xpr.nestedExpression().lhs(), xpr.nestedExpression().rhs()), xpr.index())) {}
 };
 
-
 // Helper class to perform a matrix product with the destination at hand.
 // Depending on the sizes of the factors, there are different evaluation strategies
 // as controlled by internal::product_type.
-template< typename Lhs, typename Rhs,
-          typename LhsShape = typename evaluator_traits<Lhs>::Shape,
+template <typename Lhs, typename Rhs, typename LhsShape = typename evaluator_traits<Lhs>::Shape,
           typename RhsShape = typename evaluator_traits<Rhs>::Shape,
-          int ProductType = internal::product_type<Lhs,Rhs>::value>
+          int ProductType = internal::product_type<Lhs, Rhs>::value>
 struct generic_product_impl;
 
-template<typename Lhs, typename Rhs>
-struct evaluator_assume_aliasing<Product<Lhs, Rhs, DefaultProduct> > {
+template <typename Lhs, typename Rhs>
+struct evaluator_assume_aliasing<Product<Lhs, Rhs, DefaultProduct>> {
   static const bool value = true;
 };
 
 // This is the default evaluator implementation for products:
 // It creates a temporary and call generic_product_impl
-template<typename Lhs, typename Rhs, int Options, int ProductTag, typename LhsShape, typename RhsShape>
+template <typename Lhs, typename Rhs, int Options, int ProductTag, typename LhsShape, typename RhsShape>
 struct product_evaluator<Product<Lhs, Rhs, Options>, ProductTag, LhsShape, RhsShape>
-  : public evaluator<typename Product<Lhs, Rhs, Options>::PlainObject>
-{
+    : public evaluator<typename Product<Lhs, Rhs, Options>::PlainObject> {
   typedef Product<Lhs, Rhs, Options> XprType;
   typedef typename XprType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
-  enum {
-    Flags = Base::Flags | EvalBeforeNestingBit
-  };
+  enum { Flags = Base::Flags | EvalBeforeNestingBit };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit product_evaluator(const XprType& xpr)
-    : m_result(xpr.rows(), xpr.cols())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit product_evaluator(const XprType& xpr)
+      : m_result(xpr.rows(), xpr.cols()) {
+    internal::construct_at<Base>(this, m_result);
 
-// FIXME shall we handle nested_eval here?,
-// if so, then we must take care at removing the call to nested_eval in the specializations (e.g., in permutation_matrix_product, transposition_matrix_product, etc.)
-//     typedef typename internal::nested_eval<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
-//     typedef typename internal::nested_eval<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
-//     typedef typename internal::remove_all<LhsNested>::type LhsNestedCleaned;
-//     typedef typename internal::remove_all<RhsNested>::type RhsNestedCleaned;
-//
-//     const LhsNested lhs(xpr.lhs());
-//     const RhsNested rhs(xpr.rhs());
-//
-//     generic_product_impl<LhsNestedCleaned, RhsNestedCleaned>::evalTo(m_result, lhs, rhs);
+    // FIXME shall we handle nested_eval here?,
+    // if so, then we must take care at removing the call to nested_eval in the specializations (e.g., in
+    // permutation_matrix_product, transposition_matrix_product, etc.)
+    //     typedef typename internal::nested_eval<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
+    //     typedef typename internal::nested_eval<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
+    //     typedef internal::remove_all_t<LhsNested> LhsNestedCleaned;
+    //     typedef internal::remove_all_t<RhsNested> RhsNestedCleaned;
+    //
+    //     const LhsNested lhs(xpr.lhs());
+    //     const RhsNested rhs(xpr.rhs());
+    //
+    //     generic_product_impl<LhsNestedCleaned, RhsNestedCleaned>::evalTo(m_result, lhs, rhs);
 
     generic_product_impl<Lhs, Rhs, LhsShape, RhsShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
@@ -132,32 +120,27 @@
 // TODO: we could enable them for different scalar types when the product is not vectorized.
 
 // Dense = Product
-template< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,Options>, internal::assign_op<Scalar,Scalar>, Dense2Dense,
-  typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type>
-{
-  typedef Product<Lhs,Rhs,Options> SrcXprType;
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
-  {
+template <typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs, Rhs, Options>, internal::assign_op<Scalar, Scalar>, Dense2Dense,
+                  std::enable_if_t<(Options == DefaultProduct || Options == AliasFreeProduct)>> {
+  typedef Product<Lhs, Rhs, Options> SrcXprType;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src,
+                                                        const internal::assign_op<Scalar, Scalar>&) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
     // FIXME shall we handle nested_eval here?
     generic_product_impl<Lhs, Rhs>::evalTo(dst, src.lhs(), src.rhs());
   }
 };
 
 // Dense += Product
-template< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,Options>, internal::add_assign_op<Scalar,Scalar>, Dense2Dense,
-  typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type>
-{
-  typedef Product<Lhs,Rhs,Options> SrcXprType;
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<Scalar,Scalar> &)
-  {
+template <typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs, Rhs, Options>, internal::add_assign_op<Scalar, Scalar>, Dense2Dense,
+                  std::enable_if_t<(Options == DefaultProduct || Options == AliasFreeProduct)>> {
+  typedef Product<Lhs, Rhs, Options> SrcXprType;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src,
+                                                        const internal::add_assign_op<Scalar, Scalar>&) {
     eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
     // FIXME shall we handle nested_eval here?
     generic_product_impl<Lhs, Rhs>::addTo(dst, src.lhs(), src.rhs());
@@ -165,35 +148,35 @@
 };
 
 // Dense -= Product
-template< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,Options>, internal::sub_assign_op<Scalar,Scalar>, Dense2Dense,
-  typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type>
-{
-  typedef Product<Lhs,Rhs,Options> SrcXprType;
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<Scalar,Scalar> &)
-  {
+template <typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs, Rhs, Options>, internal::sub_assign_op<Scalar, Scalar>, Dense2Dense,
+                  std::enable_if_t<(Options == DefaultProduct || Options == AliasFreeProduct)>> {
+  typedef Product<Lhs, Rhs, Options> SrcXprType;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src,
+                                                        const internal::sub_assign_op<Scalar, Scalar>&) {
     eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
     // FIXME shall we handle nested_eval here?
     generic_product_impl<Lhs, Rhs>::subTo(dst, src.lhs(), src.rhs());
   }
 };
 
-
 // Dense ?= scalar * Product
 // TODO we should apply that rule if that's really helpful
 // for instance, this is not good for inner products
-template< typename DstXprType, typename Lhs, typename Rhs, typename AssignFunc, typename Scalar, typename ScalarBis, typename Plain>
-struct Assignment<DstXprType, CwiseBinaryOp<internal::scalar_product_op<ScalarBis,Scalar>, const CwiseNullaryOp<internal::scalar_constant_op<ScalarBis>,Plain>,
-                                           const Product<Lhs,Rhs,DefaultProduct> >, AssignFunc, Dense2Dense>
-{
-  typedef CwiseBinaryOp<internal::scalar_product_op<ScalarBis,Scalar>,
-                        const CwiseNullaryOp<internal::scalar_constant_op<ScalarBis>,Plain>,
-                        const Product<Lhs,Rhs,DefaultProduct> > SrcXprType;
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void run(DstXprType &dst, const SrcXprType &src, const AssignFunc& func)
-  {
-    call_assignment_no_alias(dst, (src.lhs().functor().m_other * src.rhs().lhs())*src.rhs().rhs(), func);
+template <typename DstXprType, typename Lhs, typename Rhs, typename AssignFunc, typename Scalar, typename ScalarBis,
+          typename Plain>
+struct Assignment<DstXprType,
+                  CwiseBinaryOp<internal::scalar_product_op<ScalarBis, Scalar>,
+                                const CwiseNullaryOp<internal::scalar_constant_op<ScalarBis>, Plain>,
+                                const Product<Lhs, Rhs, DefaultProduct>>,
+                  AssignFunc, Dense2Dense> {
+  typedef CwiseBinaryOp<internal::scalar_product_op<ScalarBis, Scalar>,
+                        const CwiseNullaryOp<internal::scalar_constant_op<ScalarBis>, Plain>,
+                        const Product<Lhs, Rhs, DefaultProduct>>
+      SrcXprType;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src,
+                                                        const AssignFunc& func) {
+    call_assignment_no_alias(dst, (src.lhs().functor().m_other * src.rhs().lhs()) * src.rhs().rhs(), func);
   }
 };
 
@@ -201,219 +184,232 @@
 // Catch "Dense ?= xpr + Product<>" expression to save one temporary
 // FIXME we could probably enable these rules for any product, i.e., not only Dense and DefaultProduct
 
-template<typename OtherXpr, typename Lhs, typename Rhs>
-struct evaluator_assume_aliasing<CwiseBinaryOp<internal::scalar_sum_op<typename OtherXpr::Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, const OtherXpr,
-                                               const Product<Lhs,Rhs,DefaultProduct> >, DenseShape > {
+template <typename OtherXpr, typename Lhs, typename Rhs>
+struct evaluator_assume_aliasing<
+    CwiseBinaryOp<
+        internal::scalar_sum_op<typename OtherXpr::Scalar, typename Product<Lhs, Rhs, DefaultProduct>::Scalar>,
+        const OtherXpr, const Product<Lhs, Rhs, DefaultProduct>>,
+    DenseShape> {
   static const bool value = true;
 };
 
-template<typename OtherXpr, typename Lhs, typename Rhs>
-struct evaluator_assume_aliasing<CwiseBinaryOp<internal::scalar_difference_op<typename OtherXpr::Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, const OtherXpr,
-                                               const Product<Lhs,Rhs,DefaultProduct> >, DenseShape > {
+template <typename OtherXpr, typename Lhs, typename Rhs>
+struct evaluator_assume_aliasing<
+    CwiseBinaryOp<
+        internal::scalar_difference_op<typename OtherXpr::Scalar, typename Product<Lhs, Rhs, DefaultProduct>::Scalar>,
+        const OtherXpr, const Product<Lhs, Rhs, DefaultProduct>>,
+    DenseShape> {
   static const bool value = true;
 };
 
-template<typename DstXprType, typename OtherXpr, typename ProductType, typename Func1, typename Func2>
-struct assignment_from_xpr_op_product
-{
-  template<typename SrcXprType, typename InitialFunc>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void run(DstXprType &dst, const SrcXprType &src, const InitialFunc& /*func*/)
-  {
+template <typename DstXprType, typename OtherXpr, typename ProductType, typename Func1, typename Func2>
+struct assignment_from_xpr_op_product {
+  template <typename SrcXprType, typename InitialFunc>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src,
+                                                        const InitialFunc& /*func*/) {
     call_assignment_no_alias(dst, src.lhs(), Func1());
     call_assignment_no_alias(dst, src.rhs(), Func2());
   }
 };
 
-#define EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(ASSIGN_OP,BINOP,ASSIGN_OP2) \
-  template< typename DstXprType, typename OtherXpr, typename Lhs, typename Rhs, typename DstScalar, typename SrcScalar, typename OtherScalar,typename ProdScalar> \
-  struct Assignment<DstXprType, CwiseBinaryOp<internal::BINOP<OtherScalar,ProdScalar>, const OtherXpr, \
-                                            const Product<Lhs,Rhs,DefaultProduct> >, internal::ASSIGN_OP<DstScalar,SrcScalar>, Dense2Dense> \
-    : assignment_from_xpr_op_product<DstXprType, OtherXpr, Product<Lhs,Rhs,DefaultProduct>, internal::ASSIGN_OP<DstScalar,OtherScalar>, internal::ASSIGN_OP2<DstScalar,ProdScalar> > \
-  {}
+#define EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(ASSIGN_OP, BINOP, ASSIGN_OP2)                             \
+  template <typename DstXprType, typename OtherXpr, typename Lhs, typename Rhs, typename DstScalar, \
+            typename SrcScalar, typename OtherScalar, typename ProdScalar>                          \
+  struct Assignment<DstXprType,                                                                     \
+                    CwiseBinaryOp<internal::BINOP<OtherScalar, ProdScalar>, const OtherXpr,         \
+                                  const Product<Lhs, Rhs, DefaultProduct>>,                         \
+                    internal::ASSIGN_OP<DstScalar, SrcScalar>, Dense2Dense>                         \
+      : assignment_from_xpr_op_product<DstXprType, OtherXpr, Product<Lhs, Rhs, DefaultProduct>,     \
+                                       internal::ASSIGN_OP<DstScalar, OtherScalar>,                 \
+                                       internal::ASSIGN_OP2<DstScalar, ProdScalar>> {}
 
-EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op,    scalar_sum_op,add_assign_op);
-EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op,scalar_sum_op,add_assign_op);
-EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op,scalar_sum_op,sub_assign_op);
+EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op, scalar_sum_op, add_assign_op);
+EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op, scalar_sum_op, add_assign_op);
+EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op, scalar_sum_op, sub_assign_op);
 
-EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op,    scalar_difference_op,sub_assign_op);
-EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op,scalar_difference_op,sub_assign_op);
-EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op,scalar_difference_op,add_assign_op);
+EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op, scalar_difference_op, sub_assign_op);
+EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op, scalar_difference_op, sub_assign_op);
+EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op, scalar_difference_op, add_assign_op);
 
 //----------------------------------------
 
-template<typename Lhs, typename Rhs>
-struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,InnerProduct>
-{
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
-    dst.coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum();
+template <typename Lhs, typename Rhs>
+struct generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, InnerProduct> {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    dst.coeffRef(0, 0) = (lhs.transpose().cwiseProduct(rhs)).sum();
   }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
-    dst.coeffRef(0,0) += (lhs.transpose().cwiseProduct(rhs)).sum();
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    dst.coeffRef(0, 0) += (lhs.transpose().cwiseProduct(rhs)).sum();
   }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  { dst.coeffRef(0,0) -= (lhs.transpose().cwiseProduct(rhs)).sum(); }
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    dst.coeffRef(0, 0) -= (lhs.transpose().cwiseProduct(rhs)).sum();
+  }
 };
 
-
 /***********************************************************************
-*  Implementation of outer dense * dense vector product
-***********************************************************************/
+ *  Implementation of outer dense * dense vector product
+ ***********************************************************************/
 
 // Column major result
-template<typename Dst, typename Lhs, typename Rhs, typename Func>
-void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const false_type&)
-{
+template <typename Dst, typename Lhs, typename Rhs, typename Func>
+void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Func& func,
+                                                  const false_type&) {
   evaluator<Rhs> rhsEval(rhs);
-  ei_declare_local_nested_eval(Lhs,lhs,Rhs::SizeAtCompileTime,actual_lhs);
+  ei_declare_local_nested_eval(Lhs, lhs, Rhs::SizeAtCompileTime, actual_lhs);
   // FIXME if cols is large enough, then it might be useful to make sure that lhs is sequentially stored
   // FIXME not very good if rhs is real and lhs complex while alpha is real too
   const Index cols = dst.cols();
-  for (Index j=0; j<cols; ++j)
-    func(dst.col(j), rhsEval.coeff(Index(0),j) * actual_lhs);
+  for (Index j = 0; j < cols; ++j) func(dst.col(j), rhsEval.coeff(Index(0), j) * actual_lhs);
 }
 
 // Row major result
-template<typename Dst, typename Lhs, typename Rhs, typename Func>
-void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const true_type&)
-{
+template <typename Dst, typename Lhs, typename Rhs, typename Func>
+void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Func& func,
+                                                  const true_type&) {
   evaluator<Lhs> lhsEval(lhs);
-  ei_declare_local_nested_eval(Rhs,rhs,Lhs::SizeAtCompileTime,actual_rhs);
+  ei_declare_local_nested_eval(Rhs, rhs, Lhs::SizeAtCompileTime, actual_rhs);
   // FIXME if rows is large enough, then it might be useful to make sure that rhs is sequentially stored
   // FIXME not very good if lhs is real and rhs complex while alpha is real too
   const Index rows = dst.rows();
-  for (Index i=0; i<rows; ++i)
-    func(dst.row(i), lhsEval.coeff(i,Index(0)) * actual_rhs);
+  for (Index i = 0; i < rows; ++i) func(dst.row(i), lhsEval.coeff(i, Index(0)) * actual_rhs);
 }
 
-template<typename Lhs, typename Rhs>
-struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,OuterProduct>
-{
-  template<typename T> struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {};
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs>
+struct generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, OuterProduct> {
+  template <typename T>
+  struct is_row_major : std::conditional_t<(int(T::Flags) & RowMajorBit), internal::true_type, internal::false_type> {};
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
 
   // TODO it would be nice to be able to exploit our *_assign_op functors for that purpose
-  struct set  { template<typename Dst, typename Src> EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived()  = src; } };
-  struct add  { template<typename Dst, typename Src> EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; } };
-  struct sub  { template<typename Dst, typename Src> EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() -= src; } };
+  struct set {
+    template <typename Dst, typename Src>
+    EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const {
+      dst.const_cast_derived() = src;
+    }
+  };
+  struct add {
+    template <typename Dst, typename Src>
+    EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const {
+      dst.const_cast_derived() += src;
+    }
+  };
+  struct sub {
+    template <typename Dst, typename Src>
+    EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const {
+      dst.const_cast_derived() -= src;
+    }
+  };
   struct adds {
     Scalar m_scale;
     explicit adds(const Scalar& s) : m_scale(s) {}
-    template<typename Dst, typename Src> void EIGEN_DEVICE_FUNC operator()(const Dst& dst, const Src& src) const {
+    template <typename Dst, typename Src>
+    void EIGEN_DEVICE_FUNC operator()(const Dst& dst, const Src& src) const {
       dst.const_cast_derived() += m_scale * src;
     }
   };
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
     internal::outer_product_selector_run(dst, lhs, rhs, set(), is_row_major<Dst>());
   }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
     internal::outer_product_selector_run(dst, lhs, rhs, add(), is_row_major<Dst>());
   }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
     internal::outer_product_selector_run(dst, lhs, rhs, sub(), is_row_major<Dst>());
   }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs,
+                                                                  const Scalar& alpha) {
     internal::outer_product_selector_run(dst, lhs, rhs, adds(alpha), is_row_major<Dst>());
   }
-
 };
 
-
 // This base class provides default implementations for evalTo, addTo, subTo, in terms of scaleAndAddTo
-template<typename Lhs, typename Rhs, typename Derived>
-struct generic_product_impl_base
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs, typename Derived>
+struct generic_product_impl_base {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  { dst.setZero(); scaleAndAddTo(dst, lhs, rhs, Scalar(1)); }
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    dst.setZero();
+    scaleAndAddTo(dst, lhs, rhs, Scalar(1));
+  }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  { scaleAndAddTo(dst,lhs, rhs, Scalar(1)); }
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    scaleAndAddTo(dst, lhs, rhs, Scalar(1));
+  }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  { scaleAndAddTo(dst, lhs, rhs, Scalar(-1)); }
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    scaleAndAddTo(dst, lhs, rhs, Scalar(-1));
+  }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  { Derived::scaleAndAddTo(dst,lhs,rhs,alpha); }
-
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs,
+                                                                  const Scalar& alpha) {
+    Derived::scaleAndAddTo(dst, lhs, rhs, alpha);
+  }
 };
 
-template<typename Lhs, typename Rhs>
-struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,GemvProduct>
-  : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,GemvProduct> >
-{
-  typedef typename nested_eval<Lhs,1>::type LhsNested;
-  typedef typename nested_eval<Rhs,1>::type RhsNested;
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs>
+struct generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, GemvProduct>
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, GemvProduct>> {
+  typedef typename nested_eval<Lhs, 1>::type LhsNested;
+  typedef typename nested_eval<Rhs, 1>::type RhsNested;
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
   enum { Side = Lhs::IsVectorAtCompileTime ? OnTheLeft : OnTheRight };
-  typedef typename internal::remove_all<typename internal::conditional<int(Side)==OnTheRight,LhsNested,RhsNested>::type>::type MatrixType;
+  typedef internal::remove_all_t<std::conditional_t<int(Side) == OnTheRight, LhsNested, RhsNested>> MatrixType;
 
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs,
+                                                                  const Scalar& alpha) {
     // Fallback to inner product if both the lhs and rhs is a runtime vector.
     if (lhs.rows() == 1 && rhs.cols() == 1) {
-      dst.coeffRef(0,0) += alpha * lhs.row(0).conjugate().dot(rhs.col(0));
+      dst.coeffRef(0, 0) += alpha * lhs.row(0).conjugate().dot(rhs.col(0));
       return;
     }
     LhsNested actual_lhs(lhs);
     RhsNested actual_rhs(rhs);
-    internal::gemv_dense_selector<Side,
-                            (int(MatrixType::Flags)&RowMajorBit) ? RowMajor : ColMajor,
-                            bool(internal::blas_traits<MatrixType>::HasUsableDirectAccess)
-                           >::run(actual_lhs, actual_rhs, dst, alpha);
+    internal::gemv_dense_selector<Side, (int(MatrixType::Flags) & RowMajorBit) ? RowMajor : ColMajor,
+                                  bool(internal::blas_traits<MatrixType>::HasUsableDirectAccess)>::run(actual_lhs,
+                                                                                                       actual_rhs, dst,
+                                                                                                       alpha);
   }
 };
 
-template<typename Lhs, typename Rhs>
-struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,CoeffBasedProductMode>
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs>
+struct generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, CoeffBasedProductMode> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
     // Same as: dst.noalias() = lhs.lazyProduct(rhs);
     // but easier on the compiler side
-    call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::assign_op<typename Dst::Scalar,Scalar>());
+    call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::assign_op<typename Dst::Scalar, Scalar>());
   }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
     // dst.noalias() += lhs.lazyProduct(rhs);
-    call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::add_assign_op<typename Dst::Scalar,Scalar>());
+    call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::add_assign_op<typename Dst::Scalar, Scalar>());
   }
 
-  template<typename Dst>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+  template <typename Dst>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
     // dst.noalias() -= lhs.lazyProduct(rhs);
-    call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::sub_assign_op<typename Dst::Scalar,Scalar>());
+    call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::sub_assign_op<typename Dst::Scalar, Scalar>());
   }
 
   // This is a special evaluation path called from generic_product_impl<...,GemmProduct> in file GeneralMatrixMatrix.h
@@ -427,13 +423,12 @@
   //  3 - it makes this fallback consistent with the heavy GEMM routine.
   //  4 - it fully by-passes huge stack allocation attempts when multiplying huge fixed-size matrices.
   //      (see https://stackoverflow.com/questions/54738495)
-  // For small fixed sizes matrices, howver, the gains are less obvious, it is sometimes x2 faster, but sometimes x3 slower,
-  // and the behavior depends also a lot on the compiler... This is why this re-writting strategy is currently
+  // For small fixed sizes matrices, however, the gains are less obvious, it is sometimes x2 faster, but sometimes x3
+  // slower, and the behavior depends also a lot on the compiler... This is why this re-writing strategy is currently
   // enabled only when falling back from the main GEMM.
-  template<typename Dst, typename Func>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void eval_dynamic(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Func &func)
-  {
+  template <typename Dst, typename Func>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void eval_dynamic(Dst& dst, const Lhs& lhs, const Rhs& rhs,
+                                                                 const Func& func) {
     enum {
       HasScalarFactor = blas_traits<Lhs>::HasScalarFactor || blas_traits<Rhs>::HasScalarFactor,
       ConjLhs = blas_traits<Lhs>::NeedToConjugate,
@@ -443,37 +438,32 @@
     //        this is important for real*complex_mat
     Scalar actualAlpha = combine_scalar_factors<Scalar>(lhs, rhs);
 
-    eval_dynamic_impl(dst,
-                      blas_traits<Lhs>::extract(lhs).template conjugateIf<ConjLhs>(),
-                      blas_traits<Rhs>::extract(rhs).template conjugateIf<ConjRhs>(),
-                      func,
-                      actualAlpha,
-                      typename conditional<HasScalarFactor,true_type,false_type>::type());
+    eval_dynamic_impl(dst, blas_traits<Lhs>::extract(lhs).template conjugateIf<ConjLhs>(),
+                      blas_traits<Rhs>::extract(rhs).template conjugateIf<ConjRhs>(), func, actualAlpha,
+                      std::conditional_t<HasScalarFactor, true_type, false_type>());
   }
 
-protected:
-
-  template<typename Dst, typename LhsT, typename RhsT, typename Func, typename Scalar>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs, const Func &func, const Scalar&  s /* == 1 */, false_type)
-  {
+ protected:
+  template <typename Dst, typename LhsT, typename RhsT, typename Func, typename Scalar>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs,
+                                                                      const Func& func, const Scalar& s /* == 1 */,
+                                                                      false_type) {
     EIGEN_UNUSED_VARIABLE(s);
-    eigen_internal_assert(s==Scalar(1));
+    eigen_internal_assert(numext::is_exactly_one(s));
     call_restricted_packet_assignment_no_alias(dst, lhs.lazyProduct(rhs), func);
   }
 
-  template<typename Dst, typename LhsT, typename RhsT, typename Func, typename Scalar>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs, const Func &func, const Scalar& s, true_type)
-  {
+  template <typename Dst, typename LhsT, typename RhsT, typename Func, typename Scalar>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs,
+                                                                      const Func& func, const Scalar& s, true_type) {
     call_restricted_packet_assignment_no_alias(dst, s * lhs.lazyProduct(rhs), func);
   }
 };
 
 // This specialization enforces the use of a coefficient-based evaluation strategy
-template<typename Lhs, typename Rhs>
-struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,LazyCoeffBasedProductMode>
-  : generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,CoeffBasedProductMode> {};
+template <typename Lhs, typename Rhs>
+struct generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, LazyCoeffBasedProductMode>
+    : generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, CoeffBasedProductMode> {};
 
 // Case 2: Evaluate coeff by coeff
 //
@@ -481,29 +471,27 @@
 // The main difference is that we add an extra argument to the etor_product_*_impl::run() function
 // for the inner dimension of the product, because evaluator object do not know their size.
 
-template<int Traversal, int UnrollingIndex, typename Lhs, typename Rhs, typename RetScalar>
+template <int Traversal, int UnrollingIndex, typename Lhs, typename Rhs, typename RetScalar>
 struct etor_product_coeff_impl;
 
-template<int StorageOrder, int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
+template <int StorageOrder, int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl;
 
-template<typename Lhs, typename Rhs, int ProductTag>
+template <typename Lhs, typename Rhs, int ProductTag>
 struct product_evaluator<Product<Lhs, Rhs, LazyProduct>, ProductTag, DenseShape, DenseShape>
-    : evaluator_base<Product<Lhs, Rhs, LazyProduct> >
-{
+    : evaluator_base<Product<Lhs, Rhs, LazyProduct>> {
   typedef Product<Lhs, Rhs, LazyProduct> XprType;
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit product_evaluator(const XprType& xpr)
-    : m_lhs(xpr.lhs()),
-      m_rhs(xpr.rhs()),
-      m_lhsImpl(m_lhs),     // FIXME the creation of the evaluator objects should result in a no-op, but check that!
-      m_rhsImpl(m_rhs),     //       Moreover, they are only useful for the packet path, so we could completely disable them when not needed,
-                            //       or perhaps declare them on the fly on the packet method... We have experiment to check what's best.
-      m_innerDim(xpr.lhs().cols())
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit product_evaluator(const XprType& xpr)
+      : m_lhs(xpr.lhs()),
+        m_rhs(xpr.rhs()),
+        m_lhsImpl(m_lhs),  // FIXME the creation of the evaluator objects should result in a no-op, but check that!
+        m_rhsImpl(m_rhs),  //       Moreover, they are only useful for the packet path, so we could completely disable
+                           //       them when not needed, or perhaps declare them on the fly on the packet method... We
+                           //       have experiment to check what's best.
+        m_innerDim(xpr.lhs().cols()) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits<Scalar>::MulCost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits<Scalar>::AddCost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
@@ -523,11 +511,11 @@
 
   // Everything below here is taken from CoeffBasedProduct.h
 
-  typedef typename internal::nested_eval<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
-  typedef typename internal::nested_eval<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
+  typedef typename internal::nested_eval<Lhs, Rhs::ColsAtCompileTime>::type LhsNested;
+  typedef typename internal::nested_eval<Rhs, Lhs::RowsAtCompileTime>::type RhsNested;
 
-  typedef typename internal::remove_all<LhsNested>::type LhsNestedCleaned;
-  typedef typename internal::remove_all<RhsNested>::type RhsNestedCleaned;
+  typedef internal::remove_all_t<LhsNested> LhsNestedCleaned;
+  typedef internal::remove_all_t<RhsNested> RhsNestedCleaned;
 
   typedef evaluator<LhsNestedCleaned> LhsEtorType;
   typedef evaluator<RhsNestedCleaned> RhsEtorType;
@@ -535,22 +523,23 @@
   enum {
     RowsAtCompileTime = LhsNestedCleaned::RowsAtCompileTime,
     ColsAtCompileTime = RhsNestedCleaned::ColsAtCompileTime,
-    InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(LhsNestedCleaned::ColsAtCompileTime, RhsNestedCleaned::RowsAtCompileTime),
+    InnerSize = min_size_prefer_fixed(LhsNestedCleaned::ColsAtCompileTime, RhsNestedCleaned::RowsAtCompileTime),
     MaxRowsAtCompileTime = LhsNestedCleaned::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = RhsNestedCleaned::MaxColsAtCompileTime
   };
 
-  typedef typename find_best_packet<Scalar,RowsAtCompileTime>::type LhsVecPacketType;
-  typedef typename find_best_packet<Scalar,ColsAtCompileTime>::type RhsVecPacketType;
+  typedef typename find_best_packet<Scalar, RowsAtCompileTime>::type LhsVecPacketType;
+  typedef typename find_best_packet<Scalar, ColsAtCompileTime>::type RhsVecPacketType;
 
   enum {
 
     LhsCoeffReadCost = LhsEtorType::CoeffReadCost,
     RhsCoeffReadCost = RhsEtorType::CoeffReadCost,
-    CoeffReadCost = InnerSize==0 ? NumTraits<Scalar>::ReadCost
-                  : InnerSize == Dynamic ? HugeCost
-                    : InnerSize * (NumTraits<Scalar>::MulCost + int(LhsCoeffReadCost) + int(RhsCoeffReadCost))
-                    + (InnerSize - 1) * NumTraits<Scalar>::AddCost,
+    CoeffReadCost = InnerSize == 0 ? NumTraits<Scalar>::ReadCost
+                    : InnerSize == Dynamic
+                        ? HugeCost
+                        : InnerSize * (NumTraits<Scalar>::MulCost + int(LhsCoeffReadCost) + int(RhsCoeffReadCost)) +
+                              (InnerSize - 1) * NumTraits<Scalar>::AddCost,
 
     Unroll = CoeffReadCost <= EIGEN_UNROLLING_LIMIT,
 
@@ -564,84 +553,86 @@
     RhsVecPacketSize = unpacket_traits<RhsVecPacketType>::size,
 
     // Here, we don't care about alignment larger than the usable packet size.
-    LhsAlignment = EIGEN_PLAIN_ENUM_MIN(LhsEtorType::Alignment,LhsVecPacketSize*int(sizeof(typename LhsNestedCleaned::Scalar))),
-    RhsAlignment = EIGEN_PLAIN_ENUM_MIN(RhsEtorType::Alignment,RhsVecPacketSize*int(sizeof(typename RhsNestedCleaned::Scalar))),
+    LhsAlignment =
+        plain_enum_min(LhsEtorType::Alignment, LhsVecPacketSize* int(sizeof(typename LhsNestedCleaned::Scalar))),
+    RhsAlignment =
+        plain_enum_min(RhsEtorType::Alignment, RhsVecPacketSize* int(sizeof(typename RhsNestedCleaned::Scalar))),
 
-    SameType = is_same<typename LhsNestedCleaned::Scalar,typename RhsNestedCleaned::Scalar>::value,
+    SameType = is_same<typename LhsNestedCleaned::Scalar, typename RhsNestedCleaned::Scalar>::value,
 
-    CanVectorizeRhs = bool(RhsRowMajor) && (RhsFlags & PacketAccessBit) && (ColsAtCompileTime!=1),
-    CanVectorizeLhs = (!LhsRowMajor) && (LhsFlags & PacketAccessBit) && (RowsAtCompileTime!=1),
+    CanVectorizeRhs = bool(RhsRowMajor) && (RhsFlags & PacketAccessBit) && (ColsAtCompileTime != 1),
+    CanVectorizeLhs = (!LhsRowMajor) && (LhsFlags & PacketAccessBit) && (RowsAtCompileTime != 1),
 
-    EvalToRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
-                    : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
-                    : (bool(RhsRowMajor) && !CanVectorizeLhs),
+    EvalToRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1
+                     : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1)
+                         ? 0
+                         : (bool(RhsRowMajor) && !CanVectorizeLhs),
 
-    Flags = ((int(LhsFlags) | int(RhsFlags)) & HereditaryBits & ~RowMajorBit)
-          | (EvalToRowMajor ? RowMajorBit : 0)
-          // TODO enable vectorization for mixed types
-          | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0)
-          | (XprType::IsVectorAtCompileTime ? LinearAccessBit : 0),
+    Flags = ((int(LhsFlags) | int(RhsFlags)) & HereditaryBits & ~RowMajorBit) |
+            (EvalToRowMajor ? RowMajorBit : 0)
+            // TODO enable vectorization for mixed types
+            | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0) |
+            (XprType::IsVectorAtCompileTime ? LinearAccessBit : 0),
 
-    LhsOuterStrideBytes = int(LhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename LhsNestedCleaned::Scalar)),
-    RhsOuterStrideBytes = int(RhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename RhsNestedCleaned::Scalar)),
+    LhsOuterStrideBytes =
+        int(LhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename LhsNestedCleaned::Scalar)),
+    RhsOuterStrideBytes =
+        int(RhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename RhsNestedCleaned::Scalar)),
 
-    Alignment = bool(CanVectorizeLhs) ? (LhsOuterStrideBytes<=0 || (int(LhsOuterStrideBytes) % EIGEN_PLAIN_ENUM_MAX(1,LhsAlignment))!=0 ? 0 : LhsAlignment)
-              : bool(CanVectorizeRhs) ? (RhsOuterStrideBytes<=0 || (int(RhsOuterStrideBytes) % EIGEN_PLAIN_ENUM_MAX(1,RhsAlignment))!=0 ? 0 : RhsAlignment)
-              : 0,
+    Alignment = bool(CanVectorizeLhs)
+                    ? (LhsOuterStrideBytes <= 0 || (int(LhsOuterStrideBytes) % plain_enum_max(1, LhsAlignment)) != 0
+                           ? 0
+                           : LhsAlignment)
+                : bool(CanVectorizeRhs)
+                    ? (RhsOuterStrideBytes <= 0 || (int(RhsOuterStrideBytes) % plain_enum_max(1, RhsAlignment)) != 0
+                           ? 0
+                           : RhsAlignment)
+                    : 0,
 
     /* CanVectorizeInner deserves special explanation. It does not affect the product flags. It is not used outside
      * of Product. If the Product itself is not a packet-access expression, there is still a chance that the inner
      * loop of the product might be vectorized. This is the meaning of CanVectorizeInner. Since it doesn't affect
      * the Flags, it is safe to make this value depend on ActualPacketAccessBit, that doesn't affect the ABI.
      */
-    CanVectorizeInner =    SameType
-                        && LhsRowMajor
-                        && (!RhsRowMajor)
-                        && (int(LhsFlags) & int(RhsFlags) & ActualPacketAccessBit)
-                        && (int(InnerSize) % packet_traits<Scalar>::size == 0)
+    CanVectorizeInner = SameType && LhsRowMajor && (!RhsRowMajor) &&
+                        (int(LhsFlags) & int(RhsFlags) & ActualPacketAccessBit) &&
+                        (int(InnerSize) % packet_traits<Scalar>::size == 0)
   };
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index row, Index col) const
-  {
-    return (m_lhs.row(row).transpose().cwiseProduct( m_rhs.col(col) )).sum();
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index row, Index col) const {
+    return (m_lhs.row(row).transpose().cwiseProduct(m_rhs.col(col))).sum();
   }
 
   /* Allow index-based non-packet access. It is impossible though to allow index-based packed access,
    * which is why we don't set the LinearAccessBit.
    * TODO: this seems possible when the result is a vector
    */
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const CoeffReturnType coeff(Index index) const
-  {
-    const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? 0 : index;
-    const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? index : 0;
-    return (m_lhs.row(row).transpose().cwiseProduct( m_rhs.col(col) )).sum();
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index index) const {
+    const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? 0 : index;
+    const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? index : 0;
+    return (m_lhs.row(row).transpose().cwiseProduct(m_rhs.col(col))).sum();
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const PacketType packet(Index row, Index col) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packet(Index row, Index col) const {
     PacketType res;
-    typedef etor_product_packet_impl<bool(int(Flags)&RowMajorBit) ? RowMajor : ColMajor,
-                                     Unroll ? int(InnerSize) : Dynamic,
-                                     LhsEtorType, RhsEtorType, PacketType, LoadMode> PacketImpl;
+    typedef etor_product_packet_impl<bool(int(Flags) & RowMajorBit) ? RowMajor : ColMajor,
+                                     Unroll ? int(InnerSize) : Dynamic, LhsEtorType, RhsEtorType, PacketType, LoadMode>
+        PacketImpl;
     PacketImpl::run(row, col, m_lhsImpl, m_rhsImpl, m_innerDim, res);
     return res;
   }
 
-  template<int LoadMode, typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const PacketType packet(Index index) const
-  {
-    const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? 0 : index;
-    const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? index : 0;
-    return packet<LoadMode,PacketType>(row,col);
+  template <int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packet(Index index) const {
+    const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? 0 : index;
+    const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? index : 0;
+    return packet<LoadMode, PacketType>(row, col);
   }
 
-protected:
-  typename internal::add_const_on_value_type<LhsNested>::type m_lhs;
-  typename internal::add_const_on_value_type<RhsNested>::type m_rhs;
+ protected:
+  add_const_on_value_type_t<LhsNested> m_lhs;
+  add_const_on_value_type_t<RhsNested> m_rhs;
 
   LhsEtorType m_lhsImpl;
   RhsEtorType m_rhsImpl;
@@ -650,530 +641,515 @@
   Index m_innerDim;
 };
 
-template<typename Lhs, typename Rhs>
+template <typename Lhs, typename Rhs>
 struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, LazyCoeffBasedProductMode, DenseShape, DenseShape>
-  : product_evaluator<Product<Lhs, Rhs, LazyProduct>, CoeffBasedProductMode, DenseShape, DenseShape>
-{
+    : product_evaluator<Product<Lhs, Rhs, LazyProduct>, CoeffBasedProductMode, DenseShape, DenseShape> {
   typedef Product<Lhs, Rhs, DefaultProduct> XprType;
   typedef Product<Lhs, Rhs, LazyProduct> BaseProduct;
   typedef product_evaluator<BaseProduct, CoeffBasedProductMode, DenseShape, DenseShape> Base;
-  enum {
-    Flags = Base::Flags | EvalBeforeNestingBit
-  };
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit product_evaluator(const XprType& xpr)
-    : Base(BaseProduct(xpr.lhs(),xpr.rhs()))
-  {}
+  enum { Flags = Base::Flags | EvalBeforeNestingBit };
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit product_evaluator(const XprType& xpr)
+      : Base(BaseProduct(xpr.lhs(), xpr.rhs())) {}
 };
 
 /****************************************
 *** Coeff based product, Packet path  ***
 ****************************************/
 
-template<int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<RowMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res)
-  {
-    etor_product_packet_impl<RowMajor, UnrollingIndex-1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs, innerDim, res);
-    res =  pmadd(pset1<Packet>(lhs.coeff(row, Index(UnrollingIndex-1))), rhs.template packet<LoadMode,Packet>(Index(UnrollingIndex-1), col), res);
+template <int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<RowMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs,
+                                                        Index innerDim, Packet& res) {
+    etor_product_packet_impl<RowMajor, UnrollingIndex - 1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs,
+                                                                                            innerDim, res);
+    res = pmadd(pset1<Packet>(lhs.coeff(row, Index(UnrollingIndex - 1))),
+                rhs.template packet<LoadMode, Packet>(Index(UnrollingIndex - 1), col), res);
   }
 };
 
-template<int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<ColMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res)
-  {
-    etor_product_packet_impl<ColMajor, UnrollingIndex-1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs, innerDim, res);
-    res =  pmadd(lhs.template packet<LoadMode,Packet>(row, Index(UnrollingIndex-1)), pset1<Packet>(rhs.coeff(Index(UnrollingIndex-1), col)), res);
+template <int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<ColMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs,
+                                                        Index innerDim, Packet& res) {
+    etor_product_packet_impl<ColMajor, UnrollingIndex - 1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs,
+                                                                                            innerDim, res);
+    res = pmadd(lhs.template packet<LoadMode, Packet>(row, Index(UnrollingIndex - 1)),
+                pset1<Packet>(rhs.coeff(Index(UnrollingIndex - 1), col)), res);
   }
 };
 
-template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<RowMajor, 1, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res)
-  {
-    res = pmul(pset1<Packet>(lhs.coeff(row, Index(0))),rhs.template packet<LoadMode,Packet>(Index(0), col));
+template <typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<RowMajor, 1, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs,
+                                                        Index /*innerDim*/, Packet& res) {
+    res = pmul(pset1<Packet>(lhs.coeff(row, Index(0))), rhs.template packet<LoadMode, Packet>(Index(0), col));
   }
 };
 
-template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<ColMajor, 1, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res)
-  {
-    res = pmul(lhs.template packet<LoadMode,Packet>(row, Index(0)), pset1<Packet>(rhs.coeff(Index(0), col)));
+template <typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<ColMajor, 1, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs,
+                                                        Index /*innerDim*/, Packet& res) {
+    res = pmul(lhs.template packet<LoadMode, Packet>(row, Index(0)), pset1<Packet>(rhs.coeff(Index(0), col)));
   }
 };
 
-template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<RowMajor, 0, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res)
-  {
+template <typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<RowMajor, 0, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/,
+                                                        const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res) {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
   }
 };
 
-template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<ColMajor, 0, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res)
-  {
+template <typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<ColMajor, 0, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/,
+                                                        const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res) {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
   }
 };
 
-template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<RowMajor, Dynamic, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res)
-  {
+template <typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<RowMajor, Dynamic, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs,
+                                                        Index innerDim, Packet& res) {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
-    for(Index i = 0; i < innerDim; ++i)
-      res =  pmadd(pset1<Packet>(lhs.coeff(row, i)), rhs.template packet<LoadMode,Packet>(i, col), res);
+    for (Index i = 0; i < innerDim; ++i)
+      res = pmadd(pset1<Packet>(lhs.coeff(row, i)), rhs.template packet<LoadMode, Packet>(i, col), res);
   }
 };
 
-template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct etor_product_packet_impl<ColMajor, Dynamic, Lhs, Rhs, Packet, LoadMode>
-{
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res)
-  {
+template <typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct etor_product_packet_impl<ColMajor, Dynamic, Lhs, Rhs, Packet, LoadMode> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs,
+                                                        Index innerDim, Packet& res) {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
-    for(Index i = 0; i < innerDim; ++i)
-      res =  pmadd(lhs.template packet<LoadMode,Packet>(row, i), pset1<Packet>(rhs.coeff(i, col)), res);
+    for (Index i = 0; i < innerDim; ++i)
+      res = pmadd(lhs.template packet<LoadMode, Packet>(row, i), pset1<Packet>(rhs.coeff(i, col)), res);
   }
 };
 
-
 /***************************************************************************
-* Triangular products
-***************************************************************************/
-template<int Mode, bool LhsIsTriangular,
-         typename Lhs, bool LhsIsVector,
-         typename Rhs, bool RhsIsVector>
+ * Triangular products
+ ***************************************************************************/
+template <int Mode, bool LhsIsTriangular, typename Lhs, bool LhsIsVector, typename Rhs, bool RhsIsVector>
 struct triangular_product_impl;
 
-template<typename Lhs, typename Rhs, int ProductTag>
-struct generic_product_impl<Lhs,Rhs,TriangularShape,DenseShape,ProductTag>
-  : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,TriangularShape,DenseShape,ProductTag> >
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs, int ProductTag>
+struct generic_product_impl<Lhs, Rhs, TriangularShape, DenseShape, ProductTag>
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, TriangularShape, DenseShape, ProductTag>> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
 
-  template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
-    triangular_product_impl<Lhs::Mode,true,typename Lhs::MatrixType,false,Rhs, Rhs::ColsAtCompileTime==1>
-        ::run(dst, lhs.nestedExpression(), rhs, alpha);
+  template <typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
+    triangular_product_impl<Lhs::Mode, true, typename Lhs::MatrixType, false, Rhs, Rhs::ColsAtCompileTime == 1>::run(
+        dst, lhs.nestedExpression(), rhs, alpha);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag>
-struct generic_product_impl<Lhs,Rhs,DenseShape,TriangularShape,ProductTag>
-: generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,TriangularShape,ProductTag> >
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs, int ProductTag>
+struct generic_product_impl<Lhs, Rhs, DenseShape, TriangularShape, ProductTag>
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, DenseShape, TriangularShape, ProductTag>> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
 
-  template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
-    triangular_product_impl<Rhs::Mode,false,Lhs,Lhs::RowsAtCompileTime==1, typename Rhs::MatrixType, false>::run(dst, lhs, rhs.nestedExpression(), alpha);
+  template <typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
+    triangular_product_impl<Rhs::Mode, false, Lhs, Lhs::RowsAtCompileTime == 1, typename Rhs::MatrixType, false>::run(
+        dst, lhs, rhs.nestedExpression(), alpha);
   }
 };
 
-
 /***************************************************************************
-* SelfAdjoint products
-***************************************************************************/
-template <typename Lhs, int LhsMode, bool LhsIsVector,
-          typename Rhs, int RhsMode, bool RhsIsVector>
+ * SelfAdjoint products
+ ***************************************************************************/
+template <typename Lhs, int LhsMode, bool LhsIsVector, typename Rhs, int RhsMode, bool RhsIsVector>
 struct selfadjoint_product_impl;
 
-template<typename Lhs, typename Rhs, int ProductTag>
-struct generic_product_impl<Lhs,Rhs,SelfAdjointShape,DenseShape,ProductTag>
-  : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,SelfAdjointShape,DenseShape,ProductTag> >
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs, int ProductTag>
+struct generic_product_impl<Lhs, Rhs, SelfAdjointShape, DenseShape, ProductTag>
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, SelfAdjointShape, DenseShape, ProductTag>> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
 
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC
-  void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
-    selfadjoint_product_impl<typename Lhs::MatrixType,Lhs::Mode,false,Rhs,0,Rhs::IsVectorAtCompileTime>::run(dst, lhs.nestedExpression(), rhs, alpha);
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
+    selfadjoint_product_impl<typename Lhs::MatrixType, Lhs::Mode, false, Rhs, 0, Rhs::IsVectorAtCompileTime>::run(
+        dst, lhs.nestedExpression(), rhs, alpha);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag>
-struct generic_product_impl<Lhs,Rhs,DenseShape,SelfAdjointShape,ProductTag>
-: generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,SelfAdjointShape,ProductTag> >
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs, int ProductTag>
+struct generic_product_impl<Lhs, Rhs, DenseShape, SelfAdjointShape, ProductTag>
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, DenseShape, SelfAdjointShape, ProductTag>> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
 
-  template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
-    selfadjoint_product_impl<Lhs,0,Lhs::IsVectorAtCompileTime,typename Rhs::MatrixType,Rhs::Mode,false>::run(dst, lhs, rhs.nestedExpression(), alpha);
+  template <typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
+    selfadjoint_product_impl<Lhs, 0, Lhs::IsVectorAtCompileTime, typename Rhs::MatrixType, Rhs::Mode, false>::run(
+        dst, lhs, rhs.nestedExpression(), alpha);
   }
 };
 
-
 /***************************************************************************
-* Diagonal products
-***************************************************************************/
+ * Diagonal products
+ ***************************************************************************/
 
-template<typename MatrixType, typename DiagonalType, typename Derived, int ProductOrder>
-struct diagonal_product_evaluator_base
-  : evaluator_base<Derived>
-{
-   typedef typename ScalarBinaryOpTraits<typename MatrixType::Scalar, typename DiagonalType::Scalar>::ReturnType Scalar;
-public:
+template <typename MatrixType, typename DiagonalType, typename Derived, int ProductOrder>
+struct diagonal_product_evaluator_base : evaluator_base<Derived> {
+  typedef typename ScalarBinaryOpTraits<typename MatrixType::Scalar, typename DiagonalType::Scalar>::ReturnType Scalar;
+
+ public:
   enum {
-    CoeffReadCost = int(NumTraits<Scalar>::MulCost) + int(evaluator<MatrixType>::CoeffReadCost) + int(evaluator<DiagonalType>::CoeffReadCost),
+    CoeffReadCost = int(NumTraits<Scalar>::MulCost) + int(evaluator<MatrixType>::CoeffReadCost) +
+                    int(evaluator<DiagonalType>::CoeffReadCost),
 
     MatrixFlags = evaluator<MatrixType>::Flags,
     DiagFlags = evaluator<DiagonalType>::Flags,
 
-    _StorageOrder = (Derived::MaxRowsAtCompileTime==1 && Derived::MaxColsAtCompileTime!=1) ? RowMajor
-                  : (Derived::MaxColsAtCompileTime==1 && Derived::MaxRowsAtCompileTime!=1) ? ColMajor
-                  : MatrixFlags & RowMajorBit ? RowMajor : ColMajor,
-    _SameStorageOrder = _StorageOrder == (MatrixFlags & RowMajorBit ? RowMajor : ColMajor),
+    StorageOrder_ = (Derived::MaxRowsAtCompileTime == 1 && Derived::MaxColsAtCompileTime != 1)   ? RowMajor
+                    : (Derived::MaxColsAtCompileTime == 1 && Derived::MaxRowsAtCompileTime != 1) ? ColMajor
+                    : MatrixFlags & RowMajorBit                                                  ? RowMajor
+                                                                                                 : ColMajor,
+    SameStorageOrder_ = StorageOrder_ == (MatrixFlags & RowMajorBit ? RowMajor : ColMajor),
 
-    _ScalarAccessOnDiag =  !((int(_StorageOrder) == ColMajor && int(ProductOrder) == OnTheLeft)
-                           ||(int(_StorageOrder) == RowMajor && int(ProductOrder) == OnTheRight)),
-    _SameTypes = is_same<typename MatrixType::Scalar, typename DiagonalType::Scalar>::value,
+    ScalarAccessOnDiag_ = !((int(StorageOrder_) == ColMajor && int(ProductOrder) == OnTheLeft) ||
+                            (int(StorageOrder_) == RowMajor && int(ProductOrder) == OnTheRight)),
+    SameTypes_ = is_same<typename MatrixType::Scalar, typename DiagonalType::Scalar>::value,
     // FIXME currently we need same types, but in the future the next rule should be the one
-    //_Vectorizable = bool(int(MatrixFlags)&PacketAccessBit) && ((!_PacketOnDiag) || (_SameTypes && bool(int(DiagFlags)&PacketAccessBit))),
-    _Vectorizable =   bool(int(MatrixFlags)&PacketAccessBit)
-                  &&  _SameTypes
-                  && (_SameStorageOrder || (MatrixFlags&LinearAccessBit)==LinearAccessBit)
-                  && (_ScalarAccessOnDiag || (bool(int(DiagFlags)&PacketAccessBit))),
-    _LinearAccessMask = (MatrixType::RowsAtCompileTime==1 || MatrixType::ColsAtCompileTime==1) ? LinearAccessBit : 0,
-    Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixFlags)) | (_Vectorizable ? PacketAccessBit : 0),
+    // Vectorizable_ = bool(int(MatrixFlags)&PacketAccessBit) && ((!_PacketOnDiag) || (SameTypes_ &&
+    // bool(int(DiagFlags)&PacketAccessBit))),
+    Vectorizable_ = bool(int(MatrixFlags) & PacketAccessBit) && SameTypes_ &&
+                    (SameStorageOrder_ || (MatrixFlags & LinearAccessBit) == LinearAccessBit) &&
+                    (ScalarAccessOnDiag_ || (bool(int(DiagFlags) & PacketAccessBit))),
+    LinearAccessMask_ =
+        (MatrixType::RowsAtCompileTime == 1 || MatrixType::ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+    Flags =
+        ((HereditaryBits | LinearAccessMask_) & (unsigned int)(MatrixFlags)) | (Vectorizable_ ? PacketAccessBit : 0),
     Alignment = evaluator<MatrixType>::Alignment,
 
-    AsScalarProduct =     (DiagonalType::SizeAtCompileTime==1)
-                      ||  (DiagonalType::SizeAtCompileTime==Dynamic && MatrixType::RowsAtCompileTime==1 && ProductOrder==OnTheLeft)
-                      ||  (DiagonalType::SizeAtCompileTime==Dynamic && MatrixType::ColsAtCompileTime==1 && ProductOrder==OnTheRight)
+    AsScalarProduct =
+        (DiagonalType::SizeAtCompileTime == 1) ||
+        (DiagonalType::SizeAtCompileTime == Dynamic && MatrixType::RowsAtCompileTime == 1 &&
+         ProductOrder == OnTheLeft) ||
+        (DiagonalType::SizeAtCompileTime == Dynamic && MatrixType::ColsAtCompileTime == 1 && ProductOrder == OnTheRight)
   };
 
-  EIGEN_DEVICE_FUNC diagonal_product_evaluator_base(const MatrixType &mat, const DiagonalType &diag)
-    : m_diagImpl(diag), m_matImpl(mat)
-  {
+  EIGEN_DEVICE_FUNC diagonal_product_evaluator_base(const MatrixType& mat, const DiagonalType& diag)
+      : m_diagImpl(diag), m_matImpl(mat) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits<Scalar>::MulCost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const
-  {
-    if(AsScalarProduct)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const {
+    if (AsScalarProduct)
       return m_diagImpl.coeff(0) * m_matImpl.coeff(idx);
     else
       return m_diagImpl.coeff(idx) * m_matImpl.coeff(idx);
   }
 
-protected:
-  template<int LoadMode,typename PacketType>
-  EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::true_type) const
-  {
-    return internal::pmul(m_matImpl.template packet<LoadMode,PacketType>(row, col),
+ protected:
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::true_type) const {
+    return internal::pmul(m_matImpl.template packet<LoadMode, PacketType>(row, col),
                           internal::pset1<PacketType>(m_diagImpl.coeff(id)));
   }
 
-  template<int LoadMode,typename PacketType>
-  EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::false_type) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::false_type) const {
     enum {
       InnerSize = (MatrixType::Flags & RowMajorBit) ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime,
-      DiagonalPacketLoadMode = EIGEN_PLAIN_ENUM_MIN(LoadMode,((InnerSize%16) == 0) ? int(Aligned16) : int(evaluator<DiagonalType>::Alignment)) // FIXME hardcoded 16!!
+      DiagonalPacketLoadMode = plain_enum_min(
+          LoadMode,
+          ((InnerSize % 16) == 0) ? int(Aligned16) : int(evaluator<DiagonalType>::Alignment))  // FIXME hardcoded 16!!
     };
-    return internal::pmul(m_matImpl.template packet<LoadMode,PacketType>(row, col),
-                          m_diagImpl.template packet<DiagonalPacketLoadMode,PacketType>(id));
+    return internal::pmul(m_matImpl.template packet<LoadMode, PacketType>(row, col),
+                          m_diagImpl.template packet<DiagonalPacketLoadMode, PacketType>(id));
   }
 
   evaluator<DiagonalType> m_diagImpl;
-  evaluator<MatrixType>   m_matImpl;
+  evaluator<MatrixType> m_matImpl;
 };
 
 // diagonal * dense
-template<typename Lhs, typename Rhs, int ProductKind, int ProductTag>
+template <typename Lhs, typename Rhs, int ProductKind, int ProductTag>
 struct product_evaluator<Product<Lhs, Rhs, ProductKind>, ProductTag, DiagonalShape, DenseShape>
-  : diagonal_product_evaluator_base<Rhs, typename Lhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>, OnTheLeft>
-{
-  typedef diagonal_product_evaluator_base<Rhs, typename Lhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>, OnTheLeft> Base;
+    : diagonal_product_evaluator_base<Rhs, typename Lhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>,
+                                      OnTheLeft> {
+  typedef diagonal_product_evaluator_base<Rhs, typename Lhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>,
+                                          OnTheLeft>
+      Base;
+  using Base::coeff;
   using Base::m_diagImpl;
   using Base::m_matImpl;
-  using Base::coeff;
   typedef typename Base::Scalar Scalar;
 
   typedef Product<Lhs, Rhs, ProductKind> XprType;
   typedef typename XprType::PlainObject PlainObject;
   typedef typename Lhs::DiagonalVectorType DiagonalType;
 
+  enum { StorageOrder = Base::StorageOrder_ };
 
-  enum { StorageOrder = Base::_StorageOrder };
+  EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {}
 
-  EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr)
-    : Base(xpr.rhs(), xpr.lhs().diagonal())
-  {
-  }
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const {
     return m_diagImpl.coeff(row) * m_matImpl.coeff(row, col);
   }
 
 #ifndef EIGEN_GPUCC
-  template<int LoadMode,typename PacketType>
-  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const
-  {
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
     // FIXME: NVCC used to complain about the template keyword, but we have to check whether this is still the case.
     // See also similar calls below.
-    return this->template packet_impl<LoadMode,PacketType>(row,col, row,
-                                 typename internal::conditional<int(StorageOrder)==RowMajor, internal::true_type, internal::false_type>::type());
+    return this->template packet_impl<LoadMode, PacketType>(
+        row, col, row, std::conditional_t<int(StorageOrder) == RowMajor, internal::true_type, internal::false_type>());
   }
 
-  template<int LoadMode,typename PacketType>
-  EIGEN_STRONG_INLINE PacketType packet(Index idx) const
-  {
-    return packet<LoadMode,PacketType>(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx);
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index idx) const {
+    return packet<LoadMode, PacketType>(int(StorageOrder) == ColMajor ? idx : 0,
+                                        int(StorageOrder) == ColMajor ? 0 : idx);
   }
 #endif
 };
 
 // dense * diagonal
-template<typename Lhs, typename Rhs, int ProductKind, int ProductTag>
+template <typename Lhs, typename Rhs, int ProductKind, int ProductTag>
 struct product_evaluator<Product<Lhs, Rhs, ProductKind>, ProductTag, DenseShape, DiagonalShape>
-  : diagonal_product_evaluator_base<Lhs, typename Rhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>, OnTheRight>
-{
-  typedef diagonal_product_evaluator_base<Lhs, typename Rhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>, OnTheRight> Base;
+    : diagonal_product_evaluator_base<Lhs, typename Rhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>,
+                                      OnTheRight> {
+  typedef diagonal_product_evaluator_base<Lhs, typename Rhs::DiagonalVectorType, Product<Lhs, Rhs, LazyProduct>,
+                                          OnTheRight>
+      Base;
+  using Base::coeff;
   using Base::m_diagImpl;
   using Base::m_matImpl;
-  using Base::coeff;
   typedef typename Base::Scalar Scalar;
 
   typedef Product<Lhs, Rhs, ProductKind> XprType;
   typedef typename XprType::PlainObject PlainObject;
 
-  enum { StorageOrder = Base::_StorageOrder };
+  enum { StorageOrder = Base::StorageOrder_ };
 
-  EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr)
-    : Base(xpr.lhs(), xpr.rhs().diagonal())
-  {
-  }
+  EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs().diagonal()) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const {
     return m_matImpl.coeff(row, col) * m_diagImpl.coeff(col);
   }
 
 #ifndef EIGEN_GPUCC
-  template<int LoadMode,typename PacketType>
-  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const
-  {
-    return this->template packet_impl<LoadMode,PacketType>(row,col, col,
-                                 typename internal::conditional<int(StorageOrder)==ColMajor, internal::true_type, internal::false_type>::type());
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    return this->template packet_impl<LoadMode, PacketType>(
+        row, col, col, std::conditional_t<int(StorageOrder) == ColMajor, internal::true_type, internal::false_type>());
   }
 
-  template<int LoadMode,typename PacketType>
-  EIGEN_STRONG_INLINE PacketType packet(Index idx) const
-  {
-    return packet<LoadMode,PacketType>(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx);
+  template <int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE PacketType packet(Index idx) const {
+    return packet<LoadMode, PacketType>(int(StorageOrder) == ColMajor ? idx : 0,
+                                        int(StorageOrder) == ColMajor ? 0 : idx);
   }
 #endif
 };
 
 /***************************************************************************
-* Products with permutation matrices
-***************************************************************************/
+ * Products with permutation matrices
+ ***************************************************************************/
 
 /** \internal
-  * \class permutation_matrix_product
-  * Internal helper class implementing the product between a permutation matrix and a matrix.
-  * This class is specialized for DenseShape below and for SparseShape in SparseCore/SparsePermutation.h
-  */
-template<typename ExpressionType, int Side, bool Transposed, typename ExpressionShape>
+ * \class permutation_matrix_product
+ * Internal helper class implementing the product between a permutation matrix and a matrix.
+ * This class is specialized for DenseShape below and for SparseShape in SparseCore/SparsePermutation.h
+ */
+template <typename ExpressionType, int Side, bool Transposed, typename ExpressionShape>
 struct permutation_matrix_product;
 
-template<typename ExpressionType, int Side, bool Transposed>
-struct permutation_matrix_product<ExpressionType, Side, Transposed, DenseShape>
-{
-    typedef typename nested_eval<ExpressionType, 1>::type MatrixType;
-    typedef typename remove_all<MatrixType>::type MatrixTypeCleaned;
+template <typename ExpressionType, int Side, bool Transposed>
+struct permutation_matrix_product<ExpressionType, Side, Transposed, DenseShape> {
+  typedef typename nested_eval<ExpressionType, 1>::type MatrixType;
+  typedef remove_all_t<MatrixType> MatrixTypeCleaned;
 
-    template<typename Dest, typename PermutationType>
-    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr)
-    {
-      MatrixType mat(xpr);
-      const Index n = Side==OnTheLeft ? mat.rows() : mat.cols();
-      // FIXME we need an is_same for expression that is not sensitive to constness. For instance
-      // is_same_xpr<Block<const Matrix>, Block<Matrix> >::value should be true.
-      //if(is_same<MatrixTypeCleaned,Dest>::value && extract_data(dst) == extract_data(mat))
-      if(is_same_dense(dst, mat))
-      {
-        // apply the permutation inplace
-        Matrix<bool,PermutationType::RowsAtCompileTime,1,0,PermutationType::MaxRowsAtCompileTime> mask(perm.size());
-        mask.fill(false);
-        Index r = 0;
-        while(r < perm.size())
-        {
-          // search for the next seed
-          while(r<perm.size() && mask[r]) r++;
-          if(r>=perm.size())
-            break;
-          // we got one, let's follow it until we are back to the seed
-          Index k0 = r++;
-          Index kPrev = k0;
-          mask.coeffRef(k0) = true;
-          for(Index k=perm.indices().coeff(k0); k!=k0; k=perm.indices().coeff(k))
-          {
-                  Block<Dest, Side==OnTheLeft ? 1 : Dest::RowsAtCompileTime, Side==OnTheRight ? 1 : Dest::ColsAtCompileTime>(dst, k)
-            .swap(Block<Dest, Side==OnTheLeft ? 1 : Dest::RowsAtCompileTime, Side==OnTheRight ? 1 : Dest::ColsAtCompileTime>
-                       (dst,((Side==OnTheLeft) ^ Transposed) ? k0 : kPrev));
+  template <typename Dest, typename PermutationType>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const PermutationType& perm,
+                                                        const ExpressionType& xpr) {
+    MatrixType mat(xpr);
+    const Index n = Side == OnTheLeft ? mat.rows() : mat.cols();
+    // FIXME we need an is_same for expression that is not sensitive to constness. For instance
+    // is_same_xpr<Block<const Matrix>, Block<Matrix> >::value should be true.
+    // if(is_same<MatrixTypeCleaned,Dest>::value && extract_data(dst) == extract_data(mat))
+    if (is_same_dense(dst, mat)) {
+      // apply the permutation inplace
+      Matrix<bool, PermutationType::RowsAtCompileTime, 1, 0, PermutationType::MaxRowsAtCompileTime> mask(perm.size());
+      mask.fill(false);
+      Index r = 0;
+      while (r < perm.size()) {
+        // search for the next seed
+        while (r < perm.size() && mask[r]) r++;
+        if (r >= perm.size()) break;
+        // we got one, let's follow it until we are back to the seed
+        Index k0 = r++;
+        Index kPrev = k0;
+        mask.coeffRef(k0) = true;
+        for (Index k = perm.indices().coeff(k0); k != k0; k = perm.indices().coeff(k)) {
+          Block<Dest, Side == OnTheLeft ? 1 : Dest::RowsAtCompileTime,
+                Side == OnTheRight ? 1 : Dest::ColsAtCompileTime>(dst, k)
+              .swap(Block < Dest, Side == OnTheLeft ? 1 : Dest::RowsAtCompileTime,
+                    Side == OnTheRight
+                        ? 1
+                        : Dest::ColsAtCompileTime > (dst, ((Side == OnTheLeft) ^ Transposed) ? k0 : kPrev));
 
-            mask.coeffRef(k) = true;
-            kPrev = k;
-          }
+          mask.coeffRef(k) = true;
+          kPrev = k;
         }
       }
-      else
-      {
-        for(Index i = 0; i < n; ++i)
-        {
-          Block<Dest, Side==OnTheLeft ? 1 : Dest::RowsAtCompileTime, Side==OnTheRight ? 1 : Dest::ColsAtCompileTime>
-               (dst, ((Side==OnTheLeft) ^ Transposed) ? perm.indices().coeff(i) : i)
+    } else {
+      for (Index i = 0; i < n; ++i) {
+        Block<Dest, Side == OnTheLeft ? 1 : Dest::RowsAtCompileTime, Side == OnTheRight ? 1 : Dest::ColsAtCompileTime>(
+            dst, ((Side == OnTheLeft) ^ Transposed) ? perm.indices().coeff(i) : i)
 
-          =
+            =
 
-          Block<const MatrixTypeCleaned,Side==OnTheLeft ? 1 : MatrixTypeCleaned::RowsAtCompileTime,Side==OnTheRight ? 1 : MatrixTypeCleaned::ColsAtCompileTime>
-               (mat, ((Side==OnTheRight) ^ Transposed) ? perm.indices().coeff(i) : i);
-        }
+                Block < const MatrixTypeCleaned,
+            Side == OnTheLeft ? 1 : MatrixTypeCleaned::RowsAtCompileTime,
+            Side == OnTheRight ? 1
+                               : MatrixTypeCleaned::ColsAtCompileTime >
+                                     (mat, ((Side == OnTheRight) ^ Transposed) ? perm.indices().coeff(i) : i);
       }
     }
+  }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Lhs, Rhs, PermutationShape, MatrixShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, PermutationShape, MatrixShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
     permutation_matrix_product<Rhs, OnTheLeft, false, MatrixShape>::run(dst, lhs, rhs);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Lhs, Rhs, MatrixShape, PermutationShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, MatrixShape, PermutationShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
     permutation_matrix_product<Lhs, OnTheRight, false, MatrixShape>::run(dst, rhs, lhs);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Inverse<Lhs>, Rhs, PermutationShape, MatrixShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Inverse<Lhs>& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Inverse<Lhs>, Rhs, PermutationShape, MatrixShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Inverse<Lhs>& lhs, const Rhs& rhs) {
     permutation_matrix_product<Rhs, OnTheLeft, true, MatrixShape>::run(dst, lhs.nestedExpression(), rhs);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Lhs, Inverse<Rhs>, MatrixShape, PermutationShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Inverse<Rhs>& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Inverse<Rhs>, MatrixShape, PermutationShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Inverse<Rhs>& rhs) {
     permutation_matrix_product<Lhs, OnTheRight, true, MatrixShape>::run(dst, rhs.nestedExpression(), lhs);
   }
 };
 
-
 /***************************************************************************
-* Products with transpositions matrices
-***************************************************************************/
+ * Products with transpositions matrices
+ ***************************************************************************/
 
 // FIXME could we unify Transpositions and Permutation into a single "shape"??
 
 /** \internal
-  * \class transposition_matrix_product
-  * Internal helper class implementing the product between a permutation matrix and a matrix.
-  */
-template<typename ExpressionType, int Side, bool Transposed, typename ExpressionShape>
-struct transposition_matrix_product
-{
+ * \class transposition_matrix_product
+ * Internal helper class implementing the product between a permutation matrix and a matrix.
+ */
+template <typename ExpressionType, int Side, bool Transposed, typename ExpressionShape>
+struct transposition_matrix_product {
   typedef typename nested_eval<ExpressionType, 1>::type MatrixType;
-  typedef typename remove_all<MatrixType>::type MatrixTypeCleaned;
+  typedef remove_all_t<MatrixType> MatrixTypeCleaned;
 
-  template<typename Dest, typename TranspositionType>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const TranspositionType& tr, const ExpressionType& xpr)
-  {
+  template <typename Dest, typename TranspositionType>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const TranspositionType& tr,
+                                                        const ExpressionType& xpr) {
     MatrixType mat(xpr);
     typedef typename TranspositionType::StorageIndex StorageIndex;
     const Index size = tr.size();
     StorageIndex j = 0;
 
-    if(!is_same_dense(dst,mat))
-      dst = mat;
+    if (!is_same_dense(dst, mat)) dst = mat;
 
-    for(Index k=(Transposed?size-1:0) ; Transposed?k>=0:k<size ; Transposed?--k:++k)
-      if(Index(j=tr.coeff(k))!=k)
-      {
-        if(Side==OnTheLeft)        dst.row(k).swap(dst.row(j));
-        else if(Side==OnTheRight)  dst.col(k).swap(dst.col(j));
+    for (Index k = (Transposed ? size - 1 : 0); Transposed ? k >= 0 : k < size; Transposed ? --k : ++k)
+      if (Index(j = tr.coeff(k)) != k) {
+        if (Side == OnTheLeft)
+          dst.row(k).swap(dst.row(j));
+        else if (Side == OnTheRight)
+          dst.col(k).swap(dst.col(j));
       }
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Lhs, Rhs, TranspositionsShape, MatrixShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, TranspositionsShape, MatrixShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
     transposition_matrix_product<Rhs, OnTheLeft, false, MatrixShape>::run(dst, lhs, rhs);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Lhs, Rhs, MatrixShape, TranspositionsShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, MatrixShape, TranspositionsShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
     transposition_matrix_product<Lhs, OnTheRight, false, MatrixShape>::run(dst, rhs, lhs);
   }
 };
 
-
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Transpose<Lhs>, Rhs, TranspositionsShape, MatrixShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Transpose<Lhs>& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Transpose<Lhs>, Rhs, TranspositionsShape, MatrixShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Transpose<Lhs>& lhs, const Rhs& rhs) {
     transposition_matrix_product<Rhs, OnTheLeft, true, MatrixShape>::run(dst, lhs.nestedExpression(), rhs);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
-struct generic_product_impl<Lhs, Transpose<Rhs>, MatrixShape, TranspositionsShape, ProductTag>
-{
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Transpose<Rhs>& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Transpose<Rhs>, MatrixShape, TranspositionsShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Transpose<Rhs>& rhs) {
     transposition_matrix_product<Lhs, OnTheRight, true, MatrixShape>::run(dst, rhs.nestedExpression(), lhs);
   }
 };
 
-} // end namespace internal
+/***************************************************************************
+ * skew symmetric products
+ * for now we just call the generic implementation
+ ***************************************************************************/
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, SkewSymmetricShape, MatrixShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
+    generic_product_impl<typename Lhs::DenseMatrixType, Rhs, DenseShape, MatrixShape, ProductTag>::evalTo(dst, lhs,
+                                                                                                          rhs);
+  }
+};
 
-} // end namespace Eigen
+template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, MatrixShape, SkewSymmetricShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
+    generic_product_impl<Lhs, typename Rhs::DenseMatrixType, MatrixShape, DenseShape, ProductTag>::evalTo(dst, lhs,
+                                                                                                          rhs);
+  }
+};
 
-#endif // EIGEN_PRODUCT_EVALUATORS_H
+template <typename Lhs, typename Rhs, int ProductTag>
+struct generic_product_impl<Lhs, Rhs, SkewSymmetricShape, SkewSymmetricShape, ProductTag> {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
+    generic_product_impl<typename Lhs::DenseMatrixType, typename Rhs::DenseMatrixType, DenseShape, DenseShape,
+                         ProductTag>::evalTo(dst, lhs, rhs);
+  }
+};
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_PRODUCT_EVALUATORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h
index dab2ac8..f8a5435 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h
@@ -10,209 +10,198 @@
 #ifndef EIGEN_RANDOM_H
 #define EIGEN_RANDOM_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename Scalar> struct scalar_random_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_random_op)
-  inline const Scalar operator() () const { return random<Scalar>(); }
+template <typename Scalar>
+struct scalar_random_op {
+  inline const Scalar operator()() const { return random<Scalar>(); }
 };
 
-template<typename Scalar>
-struct functor_traits<scalar_random_op<Scalar> >
-{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
+template <typename Scalar>
+struct functor_traits<scalar_random_op<Scalar> > {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false };
+};
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \returns a random matrix expression
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  * 
-  * The parameters \a rows and \a cols are the number of rows and of columns of
-  * the returned matrix. Must be compatible with this MatrixBase type.
-  *
-  * \not_reentrant
-  * 
-  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
-  * it is redundant to pass \a rows and \a cols as arguments, so Random() should be used
-  * instead.
-  * 
-  *
-  * Example: \include MatrixBase_random_int_int.cpp
-  * Output: \verbinclude MatrixBase_random_int_int.out
-  *
-  * This expression has the "evaluate before nesting" flag so that it will be evaluated into
-  * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected
-  * behavior with expressions involving random matrices.
-  * 
-  * See DenseBase::NullaryExpr(Index, const CustomNullaryOp&) for an example using C++11 random generators.
-  *
-  * \sa DenseBase::setRandom(), DenseBase::Random(Index), DenseBase::Random()
-  */
-template<typename Derived>
-inline const typename DenseBase<Derived>::RandomReturnType
-DenseBase<Derived>::Random(Index rows, Index cols)
-{
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * The parameters \a rows and \a cols are the number of rows and of columns of
+ * the returned matrix. Must be compatible with this MatrixBase type.
+ *
+ * \not_reentrant
+ *
+ * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+ * it is redundant to pass \a rows and \a cols as arguments, so Random() should be used
+ * instead.
+ *
+ *
+ * Example: \include MatrixBase_random_int_int.cpp
+ * Output: \verbinclude MatrixBase_random_int_int.out
+ *
+ * This expression has the "evaluate before nesting" flag so that it will be evaluated into
+ * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected
+ * behavior with expressions involving random matrices.
+ *
+ * See DenseBase::NullaryExpr(Index, const CustomNullaryOp&) for an example using C++11 random generators.
+ *
+ * \sa DenseBase::setRandom(), DenseBase::Random(Index), DenseBase::Random()
+ */
+template <typename Derived>
+inline const typename DenseBase<Derived>::RandomReturnType DenseBase<Derived>::Random(Index rows, Index cols) {
   return NullaryExpr(rows, cols, internal::scalar_random_op<Scalar>());
 }
 
 /** \returns a random vector expression
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  *
-  * The parameter \a size is the size of the returned vector.
-  * Must be compatible with this MatrixBase type.
-  *
-  * \only_for_vectors
-  * \not_reentrant
-  *
-  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
-  * it is redundant to pass \a size as argument, so Random() should be used
-  * instead.
-  *
-  * Example: \include MatrixBase_random_int.cpp
-  * Output: \verbinclude MatrixBase_random_int.out
-  *
-  * This expression has the "evaluate before nesting" flag so that it will be evaluated into
-  * a temporary vector whenever it is nested in a larger expression. This prevents unexpected
-  * behavior with expressions involving random matrices.
-  *
-  * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random()
-  */
-template<typename Derived>
-inline const typename DenseBase<Derived>::RandomReturnType
-DenseBase<Derived>::Random(Index size)
-{
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * The parameter \a size is the size of the returned vector.
+ * Must be compatible with this MatrixBase type.
+ *
+ * \only_for_vectors
+ * \not_reentrant
+ *
+ * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+ * it is redundant to pass \a size as argument, so Random() should be used
+ * instead.
+ *
+ * Example: \include MatrixBase_random_int.cpp
+ * Output: \verbinclude MatrixBase_random_int.out
+ *
+ * This expression has the "evaluate before nesting" flag so that it will be evaluated into
+ * a temporary vector whenever it is nested in a larger expression. This prevents unexpected
+ * behavior with expressions involving random matrices.
+ *
+ * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random()
+ */
+template <typename Derived>
+inline const typename DenseBase<Derived>::RandomReturnType DenseBase<Derived>::Random(Index size) {
   return NullaryExpr(size, internal::scalar_random_op<Scalar>());
 }
 
 /** \returns a fixed-size random matrix or vector expression
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  * 
-  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
-  * need to use the variants taking size arguments.
-  *
-  * Example: \include MatrixBase_random.cpp
-  * Output: \verbinclude MatrixBase_random.out
-  *
-  * This expression has the "evaluate before nesting" flag so that it will be evaluated into
-  * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected
-  * behavior with expressions involving random matrices.
-  * 
-  * \not_reentrant
-  *
-  * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random(Index)
-  */
-template<typename Derived>
-inline const typename DenseBase<Derived>::RandomReturnType
-DenseBase<Derived>::Random()
-{
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+ * need to use the variants taking size arguments.
+ *
+ * Example: \include MatrixBase_random.cpp
+ * Output: \verbinclude MatrixBase_random.out
+ *
+ * This expression has the "evaluate before nesting" flag so that it will be evaluated into
+ * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected
+ * behavior with expressions involving random matrices.
+ *
+ * \not_reentrant
+ *
+ * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random(Index)
+ */
+template <typename Derived>
+inline const typename DenseBase<Derived>::RandomReturnType DenseBase<Derived>::Random() {
   return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_random_op<Scalar>());
 }
 
 /** Sets all coefficients in this expression to random values.
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  * 
-  * \not_reentrant
-  * 
-  * Example: \include MatrixBase_setRandom.cpp
-  * Output: \verbinclude MatrixBase_setRandom.out
-  *
-  * \sa class CwiseNullaryOp, setRandom(Index), setRandom(Index,Index)
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline Derived& DenseBase<Derived>::setRandom()
-{
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * \not_reentrant
+ *
+ * Example: \include MatrixBase_setRandom.cpp
+ * Output: \verbinclude MatrixBase_setRandom.out
+ *
+ * \sa class CwiseNullaryOp, setRandom(Index), setRandom(Index,Index)
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline Derived& DenseBase<Derived>::setRandom() {
   return *this = Random(rows(), cols());
 }
 
 /** Resizes to the given \a newSize, and sets all coefficients in this expression to random values.
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  * 
-  * \only_for_vectors
-  * \not_reentrant
-  *
-  * Example: \include Matrix_setRandom_int.cpp
-  * Output: \verbinclude Matrix_setRandom_int.out
-  *
-  * \sa DenseBase::setRandom(), setRandom(Index,Index), class CwiseNullaryOp, DenseBase::Random()
-  */
-template<typename Derived>
-EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setRandom(Index newSize)
-{
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * \only_for_vectors
+ * \not_reentrant
+ *
+ * Example: \include Matrix_setRandom_int.cpp
+ * Output: \verbinclude Matrix_setRandom_int.out
+ *
+ * \sa DenseBase::setRandom(), setRandom(Index,Index), class CwiseNullaryOp, DenseBase::Random()
+ */
+template <typename Derived>
+EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setRandom(Index newSize) {
   resize(newSize);
   return setRandom();
 }
 
 /** Resizes to the given size, and sets all coefficients in this expression to random values.
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  *
-  * \not_reentrant
-  * 
-  * \param rows the new number of rows
-  * \param cols the new number of columns
-  *
-  * Example: \include Matrix_setRandom_int_int.cpp
-  * Output: \verbinclude Matrix_setRandom_int_int.out
-  *
-  * \sa DenseBase::setRandom(), setRandom(Index), class CwiseNullaryOp, DenseBase::Random()
-  */
-template<typename Derived>
-EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setRandom(Index rows, Index cols)
-{
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * \not_reentrant
+ *
+ * \param rows the new number of rows
+ * \param cols the new number of columns
+ *
+ * Example: \include Matrix_setRandom_int_int.cpp
+ * Output: \verbinclude Matrix_setRandom_int_int.out
+ *
+ * \sa DenseBase::setRandom(), setRandom(Index), class CwiseNullaryOp, DenseBase::Random()
+ */
+template <typename Derived>
+EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setRandom(Index rows, Index cols) {
   resize(rows, cols);
   return setRandom();
 }
 
 /** Resizes to the given size, changing only the number of columns, and sets all
-  * coefficients in this expression to random values. For the parameter of type
-  * NoChange_t, just pass the special value \c NoChange.
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  *
-  * \not_reentrant
-  *
-  * \sa DenseBase::setRandom(), setRandom(Index), setRandom(Index, NoChange_t), class CwiseNullaryOp, DenseBase::Random()
-  */
-template<typename Derived>
-EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setRandom(NoChange_t, Index cols)
-{
+ * coefficients in this expression to random values. For the parameter of type
+ * NoChange_t, just pass the special value \c NoChange.
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * \not_reentrant
+ *
+ * \sa DenseBase::setRandom(), setRandom(Index), setRandom(Index, NoChange_t), class CwiseNullaryOp, DenseBase::Random()
+ */
+template <typename Derived>
+EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setRandom(NoChange_t, Index cols) {
   return setRandom(rows(), cols);
 }
 
 /** Resizes to the given size, changing only the number of rows, and sets all
-  * coefficients in this expression to random values. For the parameter of type
-  * NoChange_t, just pass the special value \c NoChange.
-  *
-  * Numbers are uniformly spread through their whole definition range for integer types,
-  * and in the [-1:1] range for floating point scalar types.
-  *
-  * \not_reentrant
-  *
-  * \sa DenseBase::setRandom(), setRandom(Index), setRandom(NoChange_t, Index), class CwiseNullaryOp, DenseBase::Random()
-  */
-template<typename Derived>
-EIGEN_STRONG_INLINE Derived&
-PlainObjectBase<Derived>::setRandom(Index rows, NoChange_t)
-{
+ * coefficients in this expression to random values. For the parameter of type
+ * NoChange_t, just pass the special value \c NoChange.
+ *
+ * Numbers are uniformly spread through their whole definition range for integer types,
+ * and in the [-1:1] range for floating point scalar types.
+ *
+ * \not_reentrant
+ *
+ * \sa DenseBase::setRandom(), setRandom(Index), setRandom(NoChange_t, Index), class CwiseNullaryOp, DenseBase::Random()
+ */
+template <typename Derived>
+EIGEN_STRONG_INLINE Derived& PlainObjectBase<Derived>::setRandom(Index rows, NoChange_t) {
   return setRandom(rows, cols());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_RANDOM_H
+#endif  // EIGEN_RANDOM_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h
index b6790d1..0c5f2d9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h
@@ -11,7 +11,10 @@
 #ifndef EIGEN_REDUX_H
 #define EIGEN_REDUX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
@@ -20,56 +23,51 @@
 //  * factorize code
 
 /***************************************************************************
-* Part 1 : the logic deciding a strategy for vectorization and unrolling
-***************************************************************************/
+ * Part 1 : the logic deciding a strategy for vectorization and unrolling
+ ***************************************************************************/
 
-template<typename Func, typename Evaluator>
-struct redux_traits
-{
-public:
-    typedef typename find_best_packet<typename Evaluator::Scalar,Evaluator::SizeAtCompileTime>::type PacketType;
+template <typename Func, typename Evaluator>
+struct redux_traits {
+ public:
+  typedef typename find_best_packet<typename Evaluator::Scalar, Evaluator::SizeAtCompileTime>::type PacketType;
   enum {
     PacketSize = unpacket_traits<PacketType>::size,
-    InnerMaxSize = int(Evaluator::IsRowMajor)
-                 ? Evaluator::MaxColsAtCompileTime
-                 : Evaluator::MaxRowsAtCompileTime,
-    OuterMaxSize = int(Evaluator::IsRowMajor)
-                 ? Evaluator::MaxRowsAtCompileTime
-                 : Evaluator::MaxColsAtCompileTime,
-    SliceVectorizedWork = int(InnerMaxSize)==Dynamic ? Dynamic
-                        : int(OuterMaxSize)==Dynamic ? (int(InnerMaxSize)>=int(PacketSize) ? Dynamic : 0)
-                        : (int(InnerMaxSize)/int(PacketSize)) * int(OuterMaxSize)
+    InnerMaxSize = int(Evaluator::IsRowMajor) ? Evaluator::MaxColsAtCompileTime : Evaluator::MaxRowsAtCompileTime,
+    OuterMaxSize = int(Evaluator::IsRowMajor) ? Evaluator::MaxRowsAtCompileTime : Evaluator::MaxColsAtCompileTime,
+    SliceVectorizedWork = int(InnerMaxSize) == Dynamic   ? Dynamic
+                          : int(OuterMaxSize) == Dynamic ? (int(InnerMaxSize) >= int(PacketSize) ? Dynamic : 0)
+                                                         : (int(InnerMaxSize) / int(PacketSize)) * int(OuterMaxSize)
   };
 
   enum {
-    MightVectorize = (int(Evaluator::Flags)&ActualPacketAccessBit)
-                  && (functor_traits<Func>::PacketAccess),
-    MayLinearVectorize = bool(MightVectorize) && (int(Evaluator::Flags)&LinearAccessBit),
-    MaySliceVectorize  = bool(MightVectorize) && (int(SliceVectorizedWork)==Dynamic || int(SliceVectorizedWork)>=3)
+    MayLinearize = (int(Evaluator::Flags) & LinearAccessBit),
+    MightVectorize = (int(Evaluator::Flags) & ActualPacketAccessBit) && (functor_traits<Func>::PacketAccess),
+    MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize),
+    MaySliceVectorize = bool(MightVectorize) && (int(SliceVectorizedWork) == Dynamic || int(SliceVectorizedWork) >= 3)
   };
 
-public:
+ public:
   enum {
-    Traversal = int(MayLinearVectorize) ? int(LinearVectorizedTraversal)
-              : int(MaySliceVectorize)  ? int(SliceVectorizedTraversal)
-                                        : int(DefaultTraversal)
+    Traversal = int(MayLinearVectorize)  ? int(LinearVectorizedTraversal)
+                : int(MaySliceVectorize) ? int(SliceVectorizedTraversal)
+                : int(MayLinearize)      ? int(LinearTraversal)
+                                         : int(DefaultTraversal)
   };
 
-public:
+ public:
   enum {
-    Cost = Evaluator::SizeAtCompileTime == Dynamic ? HugeCost
-         : int(Evaluator::SizeAtCompileTime) * int(Evaluator::CoeffReadCost) + (Evaluator::SizeAtCompileTime-1) * functor_traits<Func>::Cost,
+    Cost = Evaluator::SizeAtCompileTime == Dynamic
+               ? HugeCost
+               : int(Evaluator::SizeAtCompileTime) * int(Evaluator::CoeffReadCost) +
+                     (Evaluator::SizeAtCompileTime - 1) * functor_traits<Func>::Cost,
     UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Traversal) == int(DefaultTraversal) ? 1 : int(PacketSize))
   };
 
-public:
-  enum {
-    Unrolling = Cost <= UnrollingLimit ? CompleteUnrolling : NoUnrolling
-  };
-  
+ public:
+  enum { Unrolling = Cost <= UnrollingLimit ? CompleteUnrolling : NoUnrolling };
+
 #ifdef EIGEN_DEBUG_ASSIGN
-  static void debug()
-  {
+  static void debug() {
     std::cerr << "Xpr: " << typeid(typename Evaluator::XprType).name() << std::endl;
     std::cerr.setf(std::ios::hex, std::ios::basefield);
     EIGEN_DEBUG_VAR(Evaluator::Flags)
@@ -81,50 +79,42 @@
     EIGEN_DEBUG_VAR(MightVectorize)
     EIGEN_DEBUG_VAR(MayLinearVectorize)
     EIGEN_DEBUG_VAR(MaySliceVectorize)
-    std::cerr << "Traversal" << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl;
+    std::cerr << "Traversal"
+              << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl;
     EIGEN_DEBUG_VAR(UnrollingLimit)
-    std::cerr << "Unrolling" << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl;
+    std::cerr << "Unrolling"
+              << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl;
     std::cerr << std::endl;
   }
 #endif
 };
 
 /***************************************************************************
-* Part 2 : unrollers
-***************************************************************************/
+ * Part 2 : unrollers
+ ***************************************************************************/
 
 /*** no vectorization ***/
 
-template<typename Func, typename Evaluator, int Start, int Length>
-struct redux_novec_unroller
-{
-  enum {
-    HalfLength = Length/2
-  };
+template <typename Func, typename Evaluator, Index Start, Index Length>
+struct redux_novec_unroller {
+  static constexpr Index HalfLength = Length / 2;
 
   typedef typename Evaluator::Scalar Scalar;
 
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Scalar run(const Evaluator &eval, const Func& func)
-  {
-    return func(redux_novec_unroller<Func, Evaluator, Start, HalfLength>::run(eval,func),
-                redux_novec_unroller<Func, Evaluator, Start+HalfLength, Length-HalfLength>::run(eval,func));
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func) {
+    return func(redux_novec_unroller<Func, Evaluator, Start, HalfLength>::run(eval, func),
+                redux_novec_unroller<Func, Evaluator, Start + HalfLength, Length - HalfLength>::run(eval, func));
   }
 };
 
-template<typename Func, typename Evaluator, int Start>
-struct redux_novec_unroller<Func, Evaluator, Start, 1>
-{
-  enum {
-    outer = Start / Evaluator::InnerSizeAtCompileTime,
-    inner = Start % Evaluator::InnerSizeAtCompileTime
-  };
+template <typename Func, typename Evaluator, Index Start>
+struct redux_novec_unroller<Func, Evaluator, Start, 1> {
+  static constexpr Index outer = Start / Evaluator::InnerSizeAtCompileTime;
+  static constexpr Index inner = Start % Evaluator::InnerSizeAtCompileTime;
 
   typedef typename Evaluator::Scalar Scalar;
 
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Scalar run(const Evaluator &eval, const Func&)
-  {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func&) {
     return eval.coeffByOuterInner(outer, inner);
   }
 };
@@ -132,150 +122,201 @@
 // This is actually dead code and will never be called. It is required
 // to prevent false warnings regarding failed inlining though
 // for 0 length run() will never be called at all.
-template<typename Func, typename Evaluator, int Start>
-struct redux_novec_unroller<Func, Evaluator, Start, 0>
-{
+template <typename Func, typename Evaluator, Index Start>
+struct redux_novec_unroller<Func, Evaluator, Start, 0> {
   typedef typename Evaluator::Scalar Scalar;
-  EIGEN_DEVICE_FUNC 
-  static EIGEN_STRONG_INLINE Scalar run(const Evaluator&, const Func&) { return Scalar(); }
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator&, const Func&) { return Scalar(); }
+};
+
+template <typename Func, typename Evaluator, Index Start, Index Length>
+struct redux_novec_linear_unroller {
+  static constexpr Index HalfLength = Length / 2;
+
+  typedef typename Evaluator::Scalar Scalar;
+
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func) {
+    return func(redux_novec_linear_unroller<Func, Evaluator, Start, HalfLength>::run(eval, func),
+                redux_novec_linear_unroller<Func, Evaluator, Start + HalfLength, Length - HalfLength>::run(eval, func));
+  }
+};
+
+template <typename Func, typename Evaluator, Index Start>
+struct redux_novec_linear_unroller<Func, Evaluator, Start, 1> {
+  typedef typename Evaluator::Scalar Scalar;
+
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func&) {
+    return eval.coeff(Start);
+  }
+};
+
+// This is actually dead code and will never be called. It is required
+// to prevent false warnings regarding failed inlining though
+// for 0 length run() will never be called at all.
+template <typename Func, typename Evaluator, Index Start>
+struct redux_novec_linear_unroller<Func, Evaluator, Start, 0> {
+  typedef typename Evaluator::Scalar Scalar;
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator&, const Func&) { return Scalar(); }
 };
 
 /*** vectorization ***/
 
-template<typename Func, typename Evaluator, int Start, int Length>
-struct redux_vec_unroller
-{
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE PacketType run(const Evaluator &eval, const Func& func)
-  {
-    enum {
-      PacketSize = unpacket_traits<PacketType>::size,
-      HalfLength = Length/2
-    };
+template <typename Func, typename Evaluator, Index Start, Index Length>
+struct redux_vec_unroller {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func& func) {
+    constexpr Index HalfLength = Length / 2;
 
     return func.packetOp(
-            redux_vec_unroller<Func, Evaluator, Start, HalfLength>::template run<PacketType>(eval,func),
-            redux_vec_unroller<Func, Evaluator, Start+HalfLength, Length-HalfLength>::template run<PacketType>(eval,func) );
+        redux_vec_unroller<Func, Evaluator, Start, HalfLength>::template run<PacketType>(eval, func),
+        redux_vec_unroller<Func, Evaluator, Start + HalfLength, Length - HalfLength>::template run<PacketType>(eval,
+                                                                                                               func));
   }
 };
 
-template<typename Func, typename Evaluator, int Start>
-struct redux_vec_unroller<Func, Evaluator, Start, 1>
-{
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE PacketType run(const Evaluator &eval, const Func&)
-  {
-    enum {
-      PacketSize = unpacket_traits<PacketType>::size,
-      index = Start * PacketSize,
-      outer = index / int(Evaluator::InnerSizeAtCompileTime),
-      inner = index % int(Evaluator::InnerSizeAtCompileTime),
-      alignment = Evaluator::Alignment
-    };
-    return eval.template packetByOuterInner<alignment,PacketType>(outer, inner);
+template <typename Func, typename Evaluator, Index Start>
+struct redux_vec_unroller<Func, Evaluator, Start, 1> {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func&) {
+    constexpr Index PacketSize = unpacket_traits<PacketType>::size;
+    constexpr Index index = Start * PacketSize;
+    constexpr Index outer = index / int(Evaluator::InnerSizeAtCompileTime);
+    constexpr Index inner = index % int(Evaluator::InnerSizeAtCompileTime);
+    constexpr int alignment = Evaluator::Alignment;
+
+    return eval.template packetByOuterInner<alignment, PacketType>(outer, inner);
+  }
+};
+
+template <typename Func, typename Evaluator, Index Start, Index Length>
+struct redux_vec_linear_unroller {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func& func) {
+    constexpr Index HalfLength = Length / 2;
+
+    return func.packetOp(
+        redux_vec_linear_unroller<Func, Evaluator, Start, HalfLength>::template run<PacketType>(eval, func),
+        redux_vec_linear_unroller<Func, Evaluator, Start + HalfLength, Length - HalfLength>::template run<PacketType>(
+            eval, func));
+  }
+};
+
+template <typename Func, typename Evaluator, Index Start>
+struct redux_vec_linear_unroller<Func, Evaluator, Start, 1> {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func&) {
+    constexpr Index PacketSize = unpacket_traits<PacketType>::size;
+    constexpr Index index = (Start * PacketSize);
+    constexpr int alignment = Evaluator::Alignment;
+    return eval.template packet<alignment, PacketType>(index);
   }
 };
 
 /***************************************************************************
-* Part 3 : implementation of all cases
-***************************************************************************/
+ * Part 3 : implementation of all cases
+ ***************************************************************************/
 
-template<typename Func, typename Evaluator,
-         int Traversal = redux_traits<Func, Evaluator>::Traversal,
-         int Unrolling = redux_traits<Func, Evaluator>::Unrolling
->
+template <typename Func, typename Evaluator, int Traversal = redux_traits<Func, Evaluator>::Traversal,
+          int Unrolling = redux_traits<Func, Evaluator>::Unrolling>
 struct redux_impl;
 
-template<typename Func, typename Evaluator>
-struct redux_impl<Func, Evaluator, DefaultTraversal, NoUnrolling>
-{
+template <typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, DefaultTraversal, NoUnrolling> {
   typedef typename Evaluator::Scalar Scalar;
 
-  template<typename XprType>
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
-  Scalar run(const Evaluator &eval, const Func& func, const XprType& xpr)
-  {
-    eigen_assert(xpr.rows()>0 && xpr.cols()>0 && "you are using an empty matrix");
-    Scalar res;
-    res = eval.coeffByOuterInner(0, 0);
-    for(Index i = 1; i < xpr.innerSize(); ++i)
-      res = func(res, eval.coeffByOuterInner(0, i));
-    for(Index i = 1; i < xpr.outerSize(); ++i)
-      for(Index j = 0; j < xpr.innerSize(); ++j)
-        res = func(res, eval.coeffByOuterInner(i, j));
+  template <typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) {
+    eigen_assert(xpr.rows() > 0 && xpr.cols() > 0 && "you are using an empty matrix");
+    Scalar res = eval.coeffByOuterInner(0, 0);
+    for (Index i = 1; i < xpr.innerSize(); ++i) res = func(res, eval.coeffByOuterInner(0, i));
+    for (Index i = 1; i < xpr.outerSize(); ++i)
+      for (Index j = 0; j < xpr.innerSize(); ++j) res = func(res, eval.coeffByOuterInner(i, j));
     return res;
   }
 };
 
-template<typename Func, typename Evaluator>
-struct redux_impl<Func,Evaluator, DefaultTraversal, CompleteUnrolling>
-  : redux_novec_unroller<Func,Evaluator, 0, Evaluator::SizeAtCompileTime>
-{
-  typedef redux_novec_unroller<Func,Evaluator, 0, Evaluator::SizeAtCompileTime> Base;
+template <typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, LinearTraversal, NoUnrolling> {
   typedef typename Evaluator::Scalar Scalar;
-  template<typename XprType>
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
-  Scalar run(const Evaluator &eval, const Func& func, const XprType& /*xpr*/)
-  {
-    return Base::run(eval,func);
+
+  template <typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) {
+    eigen_assert(xpr.size() > 0 && "you are using an empty matrix");
+    Scalar res = eval.coeff(0);
+    for (Index k = 1; k < xpr.size(); ++k) res = func(res, eval.coeff(k));
+    return res;
   }
 };
 
-template<typename Func, typename Evaluator>
-struct redux_impl<Func, Evaluator, LinearVectorizedTraversal, NoUnrolling>
-{
+template <typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, DefaultTraversal, CompleteUnrolling>
+    : redux_novec_unroller<Func, Evaluator, 0, Evaluator::SizeAtCompileTime> {
+  typedef redux_novec_unroller<Func, Evaluator, 0, Evaluator::SizeAtCompileTime> Base;
+  typedef typename Evaluator::Scalar Scalar;
+  template <typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func,
+                                                          const XprType& /*xpr*/) {
+    return Base::run(eval, func);
+  }
+};
+
+template <typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, LinearTraversal, CompleteUnrolling>
+    : redux_novec_linear_unroller<Func, Evaluator, 0, Evaluator::SizeAtCompileTime> {
+  typedef redux_novec_linear_unroller<Func, Evaluator, 0, Evaluator::SizeAtCompileTime> Base;
+  typedef typename Evaluator::Scalar Scalar;
+  template <typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func,
+                                                          const XprType& /*xpr*/) {
+    return Base::run(eval, func);
+  }
+};
+
+template <typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, LinearVectorizedTraversal, NoUnrolling> {
   typedef typename Evaluator::Scalar Scalar;
   typedef typename redux_traits<Func, Evaluator>::PacketType PacketScalar;
 
-  template<typename XprType>
-  static Scalar run(const Evaluator &eval, const Func& func, const XprType& xpr)
-  {
+  template <typename XprType>
+  static Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) {
     const Index size = xpr.size();
-    
-    const Index packetSize = redux_traits<Func, Evaluator>::PacketSize;
-    const int packetAlignment = unpacket_traits<PacketScalar>::alignment;
-    enum {
-      alignment0 = (bool(Evaluator::Flags & DirectAccessBit) && bool(packet_traits<Scalar>::AlignedOnScalar)) ? int(packetAlignment) : int(Unaligned),
-      alignment = EIGEN_PLAIN_ENUM_MAX(alignment0, Evaluator::Alignment)
-    };
+
+    constexpr Index packetSize = redux_traits<Func, Evaluator>::PacketSize;
+    constexpr int packetAlignment = unpacket_traits<PacketScalar>::alignment;
+    constexpr int alignment0 =
+        (bool(Evaluator::Flags & DirectAccessBit) && bool(packet_traits<Scalar>::AlignedOnScalar))
+            ? int(packetAlignment)
+            : int(Unaligned);
+    constexpr int alignment = plain_enum_max(alignment0, Evaluator::Alignment);
     const Index alignedStart = internal::first_default_aligned(xpr);
-    const Index alignedSize2 = ((size-alignedStart)/(2*packetSize))*(2*packetSize);
-    const Index alignedSize = ((size-alignedStart)/(packetSize))*(packetSize);
+    const Index alignedSize2 = ((size - alignedStart) / (2 * packetSize)) * (2 * packetSize);
+    const Index alignedSize = ((size - alignedStart) / (packetSize)) * (packetSize);
     const Index alignedEnd2 = alignedStart + alignedSize2;
-    const Index alignedEnd  = alignedStart + alignedSize;
+    const Index alignedEnd = alignedStart + alignedSize;
     Scalar res;
-    if(alignedSize)
-    {
-      PacketScalar packet_res0 = eval.template packet<alignment,PacketScalar>(alignedStart);
-      if(alignedSize>packetSize) // we have at least two packets to partly unroll the loop
+    if (alignedSize) {
+      PacketScalar packet_res0 = eval.template packet<alignment, PacketScalar>(alignedStart);
+      if (alignedSize > packetSize)  // we have at least two packets to partly unroll the loop
       {
-        PacketScalar packet_res1 = eval.template packet<alignment,PacketScalar>(alignedStart+packetSize);
-        for(Index index = alignedStart + 2*packetSize; index < alignedEnd2; index += 2*packetSize)
-        {
-          packet_res0 = func.packetOp(packet_res0, eval.template packet<alignment,PacketScalar>(index));
-          packet_res1 = func.packetOp(packet_res1, eval.template packet<alignment,PacketScalar>(index+packetSize));
+        PacketScalar packet_res1 = eval.template packet<alignment, PacketScalar>(alignedStart + packetSize);
+        for (Index index = alignedStart + 2 * packetSize; index < alignedEnd2; index += 2 * packetSize) {
+          packet_res0 = func.packetOp(packet_res0, eval.template packet<alignment, PacketScalar>(index));
+          packet_res1 = func.packetOp(packet_res1, eval.template packet<alignment, PacketScalar>(index + packetSize));
         }
 
-        packet_res0 = func.packetOp(packet_res0,packet_res1);
-        if(alignedEnd>alignedEnd2)
-          packet_res0 = func.packetOp(packet_res0, eval.template packet<alignment,PacketScalar>(alignedEnd2));
+        packet_res0 = func.packetOp(packet_res0, packet_res1);
+        if (alignedEnd > alignedEnd2)
+          packet_res0 = func.packetOp(packet_res0, eval.template packet<alignment, PacketScalar>(alignedEnd2));
       }
       res = func.predux(packet_res0);
 
-      for(Index index = 0; index < alignedStart; ++index)
-        res = func(res,eval.coeff(index));
+      for (Index index = 0; index < alignedStart; ++index) res = func(res, eval.coeff(index));
 
-      for(Index index = alignedEnd; index < size; ++index)
-        res = func(res,eval.coeff(index));
-    }
-    else // too small to vectorize anything.
-         // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
+      for (Index index = alignedEnd; index < size; ++index) res = func(res, eval.coeff(index));
+    } else  // too small to vectorize anything.
+            // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
     {
       res = eval.coeff(0);
-      for(Index index = 1; index < size; ++index)
-        res = func(res,eval.coeff(index));
+      for (Index index = 1; index < size; ++index) res = func(res, eval.coeff(index));
     }
 
     return res;
@@ -283,37 +324,30 @@
 };
 
 // NOTE: for SliceVectorizedTraversal we simply bypass unrolling
-template<typename Func, typename Evaluator, int Unrolling>
-struct redux_impl<Func, Evaluator, SliceVectorizedTraversal, Unrolling>
-{
+template <typename Func, typename Evaluator, int Unrolling>
+struct redux_impl<Func, Evaluator, SliceVectorizedTraversal, Unrolling> {
   typedef typename Evaluator::Scalar Scalar;
   typedef typename redux_traits<Func, Evaluator>::PacketType PacketType;
 
-  template<typename XprType>
-  EIGEN_DEVICE_FUNC static Scalar run(const Evaluator &eval, const Func& func, const XprType& xpr)
-  {
-    eigen_assert(xpr.rows()>0 && xpr.cols()>0 && "you are using an empty matrix");
+  template <typename XprType>
+  EIGEN_DEVICE_FUNC static Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) {
+    eigen_assert(xpr.rows() > 0 && xpr.cols() > 0 && "you are using an empty matrix");
+    constexpr Index packetSize = redux_traits<Func, Evaluator>::PacketSize;
     const Index innerSize = xpr.innerSize();
     const Index outerSize = xpr.outerSize();
-    enum {
-      packetSize = redux_traits<Func, Evaluator>::PacketSize
-    };
-    const Index packetedInnerSize = ((innerSize)/packetSize)*packetSize;
+    const Index packetedInnerSize = ((innerSize) / packetSize) * packetSize;
     Scalar res;
-    if(packetedInnerSize)
-    {
-      PacketType packet_res = eval.template packet<Unaligned,PacketType>(0,0);
-      for(Index j=0; j<outerSize; ++j)
-        for(Index i=(j==0?packetSize:0); i<packetedInnerSize; i+=Index(packetSize))
-          packet_res = func.packetOp(packet_res, eval.template packetByOuterInner<Unaligned,PacketType>(j,i));
+    if (packetedInnerSize) {
+      PacketType packet_res = eval.template packet<Unaligned, PacketType>(0, 0);
+      for (Index j = 0; j < outerSize; ++j)
+        for (Index i = (j == 0 ? packetSize : 0); i < packetedInnerSize; i += Index(packetSize))
+          packet_res = func.packetOp(packet_res, eval.template packetByOuterInner<Unaligned, PacketType>(j, i));
 
       res = func.predux(packet_res);
-      for(Index j=0; j<outerSize; ++j)
-        for(Index i=packetedInnerSize; i<innerSize; ++i)
-          res = func(res, eval.coeffByOuterInner(j,i));
-    }
-    else // too small to vectorize anything.
-         // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
+      for (Index j = 0; j < outerSize; ++j)
+        for (Index i = packetedInnerSize; i < innerSize; ++i) res = func(res, eval.coeffByOuterInner(j, i));
+    } else  // too small to vectorize anything.
+            // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
     {
       res = redux_impl<Func, Evaluator, DefaultTraversal, NoUnrolling>::run(eval, func, xpr);
     }
@@ -322,194 +356,173 @@
   }
 };
 
-template<typename Func, typename Evaluator>
-struct redux_impl<Func, Evaluator, LinearVectorizedTraversal, CompleteUnrolling>
-{
+template <typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, LinearVectorizedTraversal, CompleteUnrolling> {
   typedef typename Evaluator::Scalar Scalar;
 
   typedef typename redux_traits<Func, Evaluator>::PacketType PacketType;
-  enum {
-    PacketSize = redux_traits<Func, Evaluator>::PacketSize,
-    Size = Evaluator::SizeAtCompileTime,
-    VectorizedSize = (int(Size) / int(PacketSize)) * int(PacketSize)
-  };
+  static constexpr Index PacketSize = redux_traits<Func, Evaluator>::PacketSize;
+  static constexpr Index Size = Evaluator::SizeAtCompileTime;
+  static constexpr Index VectorizedSize = (int(Size) / int(PacketSize)) * int(PacketSize);
 
-  template<typename XprType>
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
-  Scalar run(const Evaluator &eval, const Func& func, const XprType &xpr)
-  {
+  template <typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) {
     EIGEN_ONLY_USED_FOR_DEBUG(xpr)
-    eigen_assert(xpr.rows()>0 && xpr.cols()>0 && "you are using an empty matrix");
+    eigen_assert(xpr.rows() > 0 && xpr.cols() > 0 && "you are using an empty matrix");
     if (VectorizedSize > 0) {
-      Scalar res = func.predux(redux_vec_unroller<Func, Evaluator, 0, Size / PacketSize>::template run<PacketType>(eval,func));
+      Scalar res = func.predux(
+          redux_vec_linear_unroller<Func, Evaluator, 0, Size / PacketSize>::template run<PacketType>(eval, func));
       if (VectorizedSize != Size)
-        res = func(res,redux_novec_unroller<Func, Evaluator, VectorizedSize, Size-VectorizedSize>::run(eval,func));
+        res = func(
+            res, redux_novec_linear_unroller<Func, Evaluator, VectorizedSize, Size - VectorizedSize>::run(eval, func));
       return res;
-    }
-    else {
-      return redux_novec_unroller<Func, Evaluator, 0, Size>::run(eval,func);
+    } else {
+      return redux_novec_linear_unroller<Func, Evaluator, 0, Size>::run(eval, func);
     }
   }
 };
 
 // evaluator adaptor
-template<typename _XprType>
-class redux_evaluator : public internal::evaluator<_XprType>
-{
-  typedef internal::evaluator<_XprType> Base;
-public:
-  typedef _XprType XprType;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit redux_evaluator(const XprType &xpr) : Base(xpr) {}
-  
+template <typename XprType_>
+class redux_evaluator : public internal::evaluator<XprType_> {
+  typedef internal::evaluator<XprType_> Base;
+
+ public:
+  typedef XprType_ XprType;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit redux_evaluator(const XprType& xpr) : Base(xpr) {}
+
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
   typedef typename XprType::PacketScalar PacketScalar;
-  
+
   enum {
     MaxRowsAtCompileTime = XprType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = XprType::MaxColsAtCompileTime,
-    // TODO we should not remove DirectAccessBit and rather find an elegant way to query the alignment offset at runtime from the evaluator
+    // TODO we should not remove DirectAccessBit and rather find an elegant way to query the alignment offset at runtime
+    // from the evaluator
     Flags = Base::Flags & ~DirectAccessBit,
     IsRowMajor = XprType::IsRowMajor,
     SizeAtCompileTime = XprType::SizeAtCompileTime,
     InnerSizeAtCompileTime = XprType::InnerSizeAtCompileTime
   };
-  
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  CoeffReturnType coeffByOuterInner(Index outer, Index inner) const
-  { return Base::coeff(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); }
-  
-  template<int LoadMode, typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  PacketType packetByOuterInner(Index outer, Index inner) const
-  { return Base::template packet<LoadMode,PacketType>(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); }
-  
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeffByOuterInner(Index outer, Index inner) const {
+    return Base::coeff(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer);
+  }
+
+  template <int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetByOuterInner(Index outer, Index inner) const {
+    return Base::template packet<LoadMode, PacketType>(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer);
+  }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /***************************************************************************
-* Part 4 : public API
-***************************************************************************/
-
+ * Part 4 : public API
+ ***************************************************************************/
 
 /** \returns the result of a full redux operation on the whole matrix or vector using \a func
-  *
-  * The template parameter \a BinaryOp is the type of the functor \a func which must be
-  * an associative operator. Both current C++98 and C++11 functor styles are handled.
-  *
-  * \warning the matrix must be not empty, otherwise an assertion is triggered.
-  *
-  * \sa DenseBase::sum(), DenseBase::minCoeff(), DenseBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise()
-  */
-template<typename Derived>
-template<typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::redux(const Func& func) const
-{
-  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+ *
+ * The template parameter \a BinaryOp is the type of the functor \a func which must be
+ * an associative operator. Both current C++98 and C++11 functor styles are handled.
+ *
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::sum(), DenseBase::minCoeff(), DenseBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise()
+ */
+template <typename Derived>
+template <typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar DenseBase<Derived>::redux(
+    const Func& func) const {
+  eigen_assert(this->rows() > 0 && this->cols() > 0 && "you are using an empty matrix");
 
   typedef typename internal::redux_evaluator<Derived> ThisEvaluator;
   ThisEvaluator thisEval(derived());
 
   // The initial expression is passed to the reducer as an additional argument instead of
-  // passing it as a member of redux_evaluator to help  
+  // passing it as a member of redux_evaluator to help
   return internal::redux_impl<Func, ThisEvaluator>::run(thisEval, func, derived());
 }
 
 /** \returns the minimum of all coefficients of \c *this.
-  * In case \c *this contains NaN, NaNPropagation determines the behavior:
-  *   NaNPropagation == PropagateFast : undefined
-  *   NaNPropagation == PropagateNaN : result is NaN
-  *   NaNPropagation == PropagateNumbers : result is minimum of elements that are not NaN
-  * \warning the matrix must be not empty, otherwise an assertion is triggered.
-  */
-template<typename Derived>
-template<int NaNPropagation>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::minCoeff() const
-{
-  return derived().redux(Eigen::internal::scalar_min_op<Scalar,Scalar, NaNPropagation>());
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is minimum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ */
+template <typename Derived>
+template <int NaNPropagation>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar DenseBase<Derived>::minCoeff() const {
+  return derived().redux(Eigen::internal::scalar_min_op<Scalar, Scalar, NaNPropagation>());
 }
 
-/** \returns the maximum of all coefficients of \c *this. 
-  * In case \c *this contains NaN, NaNPropagation determines the behavior:
-  *   NaNPropagation == PropagateFast : undefined
-  *   NaNPropagation == PropagateNaN : result is NaN
-  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
-  * \warning the matrix must be not empty, otherwise an assertion is triggered.
-  */
-template<typename Derived>
-template<int NaNPropagation>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::maxCoeff() const
-{
-  return derived().redux(Eigen::internal::scalar_max_op<Scalar,Scalar, NaNPropagation>());
+/** \returns the maximum of all coefficients of \c *this.
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ */
+template <typename Derived>
+template <int NaNPropagation>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar DenseBase<Derived>::maxCoeff() const {
+  return derived().redux(Eigen::internal::scalar_max_op<Scalar, Scalar, NaNPropagation>());
 }
 
 /** \returns the sum of all coefficients of \c *this
-  *
-  * If \c *this is empty, then the value 0 is returned.
-  *
-  * \sa trace(), prod(), mean()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::sum() const
-{
-  if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0))
-    return Scalar(0);
-  return derived().redux(Eigen::internal::scalar_sum_op<Scalar,Scalar>());
+ *
+ * If \c *this is empty, then the value 0 is returned.
+ *
+ * \sa trace(), prod(), mean()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar DenseBase<Derived>::sum() const {
+  if (SizeAtCompileTime == 0 || (SizeAtCompileTime == Dynamic && size() == 0)) return Scalar(0);
+  return derived().redux(Eigen::internal::scalar_sum_op<Scalar, Scalar>());
 }
 
 /** \returns the mean of all coefficients of *this
-*
-* \sa trace(), prod(), sum()
-*/
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::mean() const
-{
+ *
+ * \sa trace(), prod(), sum()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar DenseBase<Derived>::mean() const {
 #ifdef __INTEL_COMPILER
-  #pragma warning push
-  #pragma warning ( disable : 2259 )
+#pragma warning push
+#pragma warning(disable : 2259)
 #endif
-  return Scalar(derived().redux(Eigen::internal::scalar_sum_op<Scalar,Scalar>())) / Scalar(this->size());
+  return Scalar(derived().redux(Eigen::internal::scalar_sum_op<Scalar, Scalar>())) / Scalar(this->size());
 #ifdef __INTEL_COMPILER
-  #pragma warning pop
+#pragma warning pop
 #endif
 }
 
 /** \returns the product of all coefficients of *this
-  *
-  * Example: \include MatrixBase_prod.cpp
-  * Output: \verbinclude MatrixBase_prod.out
-  *
-  * \sa sum(), mean(), trace()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::prod() const
-{
-  if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0))
-    return Scalar(1);
+ *
+ * Example: \include MatrixBase_prod.cpp
+ * Output: \verbinclude MatrixBase_prod.out
+ *
+ * \sa sum(), mean(), trace()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar DenseBase<Derived>::prod() const {
+  if (SizeAtCompileTime == 0 || (SizeAtCompileTime == Dynamic && size() == 0)) return Scalar(1);
   return derived().redux(Eigen::internal::scalar_product_op<Scalar>());
 }
 
 /** \returns the trace of \c *this, i.e. the sum of the coefficients on the main diagonal.
-  *
-  * \c *this can be any matrix, not necessarily square.
-  *
-  * \sa diagonal(), sum()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
-MatrixBase<Derived>::trace() const
-{
+ *
+ * \c *this can be any matrix, not necessarily square.
+ *
+ * \sa diagonal(), sum()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar MatrixBase<Derived>::trace() const {
   return derived().diagonal().sum();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_REDUX_H
+#endif  // EIGEN_REDUX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h
index c2a37ea..129bc85 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h
@@ -10,197 +10,185 @@
 #ifndef EIGEN_REF_H
 #define EIGEN_REF_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename _PlainObjectType, int _Options, typename _StrideType>
-struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
-  : public traits<Map<_PlainObjectType, _Options, _StrideType> >
-{
-  typedef _PlainObjectType PlainObjectType;
-  typedef _StrideType StrideType;
+template <typename PlainObjectType_, int Options_, typename StrideType_>
+struct traits<Ref<PlainObjectType_, Options_, StrideType_> >
+    : public traits<Map<PlainObjectType_, Options_, StrideType_> > {
+  typedef PlainObjectType_ PlainObjectType;
+  typedef StrideType_ StrideType;
   enum {
-    Options = _Options,
-    Flags = traits<Map<_PlainObjectType, _Options, _StrideType> >::Flags | NestByRefBit,
-    Alignment = traits<Map<_PlainObjectType, _Options, _StrideType> >::Alignment
+    Options = Options_,
+    Flags = traits<Map<PlainObjectType_, Options_, StrideType_> >::Flags | NestByRefBit,
+    Alignment = traits<Map<PlainObjectType_, Options_, StrideType_> >::Alignment,
+    InnerStrideAtCompileTime = traits<Map<PlainObjectType_, Options_, StrideType_> >::InnerStrideAtCompileTime,
+    OuterStrideAtCompileTime = traits<Map<PlainObjectType_, Options_, StrideType_> >::OuterStrideAtCompileTime
   };
 
-  template<typename Derived> struct match {
+  template <typename Derived>
+  struct match {
     enum {
       IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime,
       HasDirectAccess = internal::has_direct_access<Derived>::ret,
-      StorageOrderMatch = IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
-      InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic)
-                      || int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime)
-                      || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1),
-      OuterStrideMatch = IsVectorAtCompileTime
-                      || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime),
+      StorageOrderMatch =
+          IsVectorAtCompileTime || ((PlainObjectType::Flags & RowMajorBit) == (Derived::Flags & RowMajorBit)),
+      InnerStrideMatch = int(InnerStrideAtCompileTime) == int(Dynamic) ||
+                         int(InnerStrideAtCompileTime) == int(Derived::InnerStrideAtCompileTime) ||
+                         (int(InnerStrideAtCompileTime) == 0 && int(Derived::InnerStrideAtCompileTime) == 1),
+      OuterStrideMatch = IsVectorAtCompileTime || int(OuterStrideAtCompileTime) == int(Dynamic) ||
+                         int(OuterStrideAtCompileTime) == int(Derived::OuterStrideAtCompileTime),
       // NOTE, this indirection of evaluator<Derived>::Alignment is needed
       // to workaround a very strange bug in MSVC related to the instantiation
       // of has_*ary_operator in evaluator<CwiseNullaryOp>.
       // This line is surprisingly very sensitive. For instance, simply adding parenthesis
       // as "DerivedAlignment = (int(evaluator<Derived>::Alignment))," will make MSVC fail...
       DerivedAlignment = int(evaluator<Derived>::Alignment),
-      AlignmentMatch = (int(traits<PlainObjectType>::Alignment)==int(Unaligned)) || (DerivedAlignment >= int(Alignment)), // FIXME the first condition is not very clear, it should be replaced by the required alignment
+      AlignmentMatch = (int(traits<PlainObjectType>::Alignment) == int(Unaligned)) ||
+                       (DerivedAlignment >= int(Alignment)),  // FIXME the first condition is not very clear, it should
+                                                              // be replaced by the required alignment
       ScalarTypeMatch = internal::is_same<typename PlainObjectType::Scalar, typename Derived::Scalar>::value,
-      MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch && ScalarTypeMatch
+      MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch &&
+                           AlignmentMatch && ScalarTypeMatch
     };
-    typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
+    typedef std::conditional_t<MatchAtCompileTime, internal::true_type, internal::false_type> type;
   };
-
 };
 
-template<typename Derived>
+template <typename Derived>
 struct traits<RefBase<Derived> > : public traits<Derived> {};
 
-}
+}  // namespace internal
 
-template<typename Derived> class RefBase
- : public MapBase<Derived>
-{
+template <typename Derived>
+class RefBase : public MapBase<Derived> {
   typedef typename internal::traits<Derived>::PlainObjectType PlainObjectType;
   typedef typename internal::traits<Derived>::StrideType StrideType;
 
-public:
-
+ public:
   typedef MapBase<Derived> Base;
   EIGEN_DENSE_PUBLIC_INTERFACE(RefBase)
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const {
     return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1;
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const {
     return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer()
-         : IsVectorAtCompileTime ? this->size()
-         : int(Flags)&RowMajorBit ? this->cols()
-         : this->rows();
+           : IsVectorAtCompileTime                   ? this->size()
+           : int(Flags) & RowMajorBit                ? this->cols()
+                                                     : this->rows();
   }
 
   EIGEN_DEVICE_FUNC RefBase()
-    : Base(0,RowsAtCompileTime==Dynamic?0:RowsAtCompileTime,ColsAtCompileTime==Dynamic?0:ColsAtCompileTime),
-      // Stride<> does not allow default ctor for Dynamic strides, so let' initialize it with dummy values:
-      m_stride(StrideType::OuterStrideAtCompileTime==Dynamic?0:StrideType::OuterStrideAtCompileTime,
-               StrideType::InnerStrideAtCompileTime==Dynamic?0:StrideType::InnerStrideAtCompileTime)
-  {}
+      : Base(0, RowsAtCompileTime == Dynamic ? 0 : RowsAtCompileTime,
+             ColsAtCompileTime == Dynamic ? 0 : ColsAtCompileTime),
+        // Stride<> does not allow default ctor for Dynamic strides, so let' initialize it with dummy values:
+        m_stride(StrideType::OuterStrideAtCompileTime == Dynamic ? 0 : StrideType::OuterStrideAtCompileTime,
+                 StrideType::InnerStrideAtCompileTime == Dynamic ? 0 : StrideType::InnerStrideAtCompileTime) {}
 
   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(RefBase)
 
-protected:
-
-  typedef Stride<StrideType::OuterStrideAtCompileTime,StrideType::InnerStrideAtCompileTime> StrideBase;
+ protected:
+  typedef Stride<StrideType::OuterStrideAtCompileTime, StrideType::InnerStrideAtCompileTime> StrideBase;
 
   // Resolves inner stride if default 0.
-  static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveInnerStride(Index inner) {
-    return inner == 0 ? 1 : inner;
-  }
+  static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveInnerStride(Index inner) { return inner == 0 ? 1 : inner; }
 
   // Resolves outer stride if default 0.
-  static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveOuterStride(Index inner, Index outer, Index rows, Index cols, bool isVectorAtCompileTime, bool isRowMajor) {
+  static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveOuterStride(Index inner, Index outer, Index rows, Index cols,
+                                                                    bool isVectorAtCompileTime, bool isRowMajor) {
     return outer == 0 ? isVectorAtCompileTime ? inner * rows * cols : isRowMajor ? inner * cols : inner * rows : outer;
   }
 
   // Returns true if construction is valid, false if there is a stride mismatch,
   // and fails if there is a size mismatch.
-  template<typename Expression>
-  EIGEN_DEVICE_FUNC bool construct(Expression& expr)
-  {
+  template <typename Expression>
+  EIGEN_DEVICE_FUNC bool construct(Expression& expr) {
     // Check matrix sizes.  If this is a compile-time vector, we do allow
     // implicitly transposing.
-    EIGEN_STATIC_ASSERT(
-      EIGEN_PREDICATE_SAME_MATRIX_SIZE(PlainObjectType, Expression)
-      // If it is a vector, the transpose sizes might match.
-      || ( PlainObjectType::IsVectorAtCompileTime
-            && ((int(PlainObjectType::RowsAtCompileTime)==Eigen::Dynamic
-              || int(Expression::ColsAtCompileTime)==Eigen::Dynamic
-              || int(PlainObjectType::RowsAtCompileTime)==int(Expression::ColsAtCompileTime))
-            &&  (int(PlainObjectType::ColsAtCompileTime)==Eigen::Dynamic
-              || int(Expression::RowsAtCompileTime)==Eigen::Dynamic
-              || int(PlainObjectType::ColsAtCompileTime)==int(Expression::RowsAtCompileTime)))),
-      YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES
-    )
+    EIGEN_STATIC_ASSERT(EIGEN_PREDICATE_SAME_MATRIX_SIZE(PlainObjectType, Expression)
+                            // If it is a vector, the transpose sizes might match.
+                            || (PlainObjectType::IsVectorAtCompileTime &&
+                                ((int(PlainObjectType::RowsAtCompileTime) == Eigen::Dynamic ||
+                                  int(Expression::ColsAtCompileTime) == Eigen::Dynamic ||
+                                  int(PlainObjectType::RowsAtCompileTime) == int(Expression::ColsAtCompileTime)) &&
+                                 (int(PlainObjectType::ColsAtCompileTime) == Eigen::Dynamic ||
+                                  int(Expression::RowsAtCompileTime) == Eigen::Dynamic ||
+                                  int(PlainObjectType::ColsAtCompileTime) == int(Expression::RowsAtCompileTime)))),
+                        YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES)
 
     // Determine runtime rows and columns.
     Index rows = expr.rows();
     Index cols = expr.cols();
-    if(PlainObjectType::RowsAtCompileTime==1)
-    {
-      eigen_assert(expr.rows()==1 || expr.cols()==1);
+    if (PlainObjectType::RowsAtCompileTime == 1) {
+      eigen_assert(expr.rows() == 1 || expr.cols() == 1);
       rows = 1;
       cols = expr.size();
-    }
-    else if(PlainObjectType::ColsAtCompileTime==1)
-    {
-      eigen_assert(expr.rows()==1 || expr.cols()==1);
+    } else if (PlainObjectType::ColsAtCompileTime == 1) {
+      eigen_assert(expr.rows() == 1 || expr.cols() == 1);
       rows = expr.size();
       cols = 1;
     }
     // Verify that the sizes are valid.
-    eigen_assert(
-      (PlainObjectType::RowsAtCompileTime == Dynamic) || (PlainObjectType::RowsAtCompileTime == rows));
-    eigen_assert(
-      (PlainObjectType::ColsAtCompileTime == Dynamic) || (PlainObjectType::ColsAtCompileTime == cols));
-
+    eigen_assert((PlainObjectType::RowsAtCompileTime == Dynamic) || (PlainObjectType::RowsAtCompileTime == rows));
+    eigen_assert((PlainObjectType::ColsAtCompileTime == Dynamic) || (PlainObjectType::ColsAtCompileTime == cols));
 
     // If this is a vector, we might be transposing, which means that stride should swap.
     const bool transpose = PlainObjectType::IsVectorAtCompileTime && (rows != expr.rows());
     // If the storage format differs, we also need to swap the stride.
     const bool row_major = ((PlainObjectType::Flags)&RowMajorBit) != 0;
-    const bool expr_row_major = (Expression::Flags&RowMajorBit) != 0;
-    const bool storage_differs =  (row_major != expr_row_major);
+    const bool expr_row_major = (Expression::Flags & RowMajorBit) != 0;
+    const bool storage_differs = (row_major != expr_row_major);
 
     const bool swap_stride = (transpose != storage_differs);
 
     // Determine expr's actual strides, resolving any defaults if zero.
     const Index expr_inner_actual = resolveInnerStride(expr.innerStride());
-    const Index expr_outer_actual = resolveOuterStride(expr_inner_actual,
-                                                       expr.outerStride(),
-                                                       expr.rows(),
-                                                       expr.cols(),
-                                                       Expression::IsVectorAtCompileTime != 0,
-                                                       expr_row_major);
+    const Index expr_outer_actual = resolveOuterStride(expr_inner_actual, expr.outerStride(), expr.rows(), expr.cols(),
+                                                       Expression::IsVectorAtCompileTime != 0, expr_row_major);
 
     // If this is a column-major row vector or row-major column vector, the inner-stride
     // is arbitrary, so set it to either the compile-time inner stride or 1.
     const bool row_vector = (rows == 1);
     const bool col_vector = (cols == 1);
     const Index inner_stride =
-        ( (!row_major && row_vector) || (row_major && col_vector) ) ?
-            ( StrideType::InnerStrideAtCompileTime > 0 ? Index(StrideType::InnerStrideAtCompileTime) : 1)
-            : swap_stride ? expr_outer_actual : expr_inner_actual;
+        ((!row_major && row_vector) || (row_major && col_vector))
+            ? (StrideType::InnerStrideAtCompileTime > 0 ? Index(StrideType::InnerStrideAtCompileTime) : 1)
+        : swap_stride ? expr_outer_actual
+                      : expr_inner_actual;
 
     // If this is a column-major column vector or row-major row vector, the outer-stride
     // is arbitrary, so set it to either the compile-time outer stride or vector size.
     const Index outer_stride =
-      ( (!row_major && col_vector) || (row_major && row_vector) ) ?
-          ( StrideType::OuterStrideAtCompileTime > 0 ? Index(StrideType::OuterStrideAtCompileTime) : rows * cols * inner_stride)
-          : swap_stride ? expr_inner_actual : expr_outer_actual;
+        ((!row_major && col_vector) || (row_major && row_vector))
+            ? (StrideType::OuterStrideAtCompileTime > 0 ? Index(StrideType::OuterStrideAtCompileTime)
+                                                        : rows * cols * inner_stride)
+        : swap_stride ? expr_inner_actual
+                      : expr_outer_actual;
 
     // Check if given inner/outer strides are compatible with compile-time strides.
-    const bool inner_valid = (StrideType::InnerStrideAtCompileTime == Dynamic)
-        || (resolveInnerStride(Index(StrideType::InnerStrideAtCompileTime)) == inner_stride);
+    const bool inner_valid = (StrideType::InnerStrideAtCompileTime == Dynamic) ||
+                             (resolveInnerStride(Index(StrideType::InnerStrideAtCompileTime)) == inner_stride);
     if (!inner_valid) {
       return false;
     }
 
-    const bool outer_valid = (StrideType::OuterStrideAtCompileTime == Dynamic)
-        || (resolveOuterStride(
-              inner_stride,
-              Index(StrideType::OuterStrideAtCompileTime),
-              rows, cols, PlainObjectType::IsVectorAtCompileTime != 0,
-              row_major)
-            == outer_stride);
+    const bool outer_valid =
+        (StrideType::OuterStrideAtCompileTime == Dynamic) ||
+        (resolveOuterStride(inner_stride, Index(StrideType::OuterStrideAtCompileTime), rows, cols,
+                            PlainObjectType::IsVectorAtCompileTime != 0, row_major) == outer_stride);
     if (!outer_valid) {
       return false;
     }
 
-    ::new (static_cast<Base*>(this)) Base(expr.data(), rows, cols);
-    ::new (&m_stride) StrideBase(
-      (StrideType::OuterStrideAtCompileTime == 0) ? 0 : outer_stride,
-      (StrideType::InnerStrideAtCompileTime == 0) ? 0 : inner_stride );
+    internal::construct_at<Base>(this, expr.data(), rows, cols);
+    internal::construct_at(&m_stride, (StrideType::OuterStrideAtCompileTime == 0) ? 0 : outer_stride,
+                           (StrideType::InnerStrideAtCompileTime == 0) ? 0 : inner_stride);
     return true;
   }
 
@@ -208,174 +196,188 @@
 };
 
 /** \class Ref
-  * \ingroup Core_Module
-  *
-  * \brief A matrix or vector expression mapping an existing expression
-  *
-  * \tparam PlainObjectType the equivalent matrix type of the mapped data
-  * \tparam Options specifies the pointer alignment in bytes. It can be: \c #Aligned128, , \c #Aligned64, \c #Aligned32, \c #Aligned16, \c #Aligned8 or \c #Unaligned.
-  *                 The default is \c #Unaligned.
-  * \tparam StrideType optionally specifies strides. By default, Ref implies a contiguous storage along the inner dimension (inner stride==1),
-  *                   but accepts a variable outer stride (leading dimension).
-  *                   This can be overridden by specifying strides.
-  *                   The type passed here must be a specialization of the Stride template, see examples below.
-  *
-  * This class provides a way to write non-template functions taking Eigen objects as parameters while limiting the number of copies.
-  * A Ref<> object can represent either a const expression or a l-value:
-  * \code
-  * // in-out argument:
-  * void foo1(Ref<VectorXf> x);
-  *
-  * // read-only const argument:
-  * void foo2(const Ref<const VectorXf>& x);
-  * \endcode
-  *
-  * In the in-out case, the input argument must satisfy the constraints of the actual Ref<> type, otherwise a compilation issue will be triggered.
-  * By default, a Ref<VectorXf> can reference any dense vector expression of float having a contiguous memory layout.
-  * Likewise, a Ref<MatrixXf> can reference any column-major dense matrix expression of float whose column's elements are contiguously stored with
-  * the possibility to have a constant space in-between each column, i.e. the inner stride must be equal to 1, but the outer stride (or leading dimension)
-  * can be greater than the number of rows.
-  *
-  * In the const case, if the input expression does not match the above requirement, then it is evaluated into a temporary before being passed to the function.
-  * Here are some examples:
-  * \code
-  * MatrixXf A;
-  * VectorXf a;
-  * foo1(a.head());             // OK
-  * foo1(A.col());              // OK
-  * foo1(A.row());              // Compilation error because here innerstride!=1
-  * foo2(A.row());              // Compilation error because A.row() is a 1xN object while foo2 is expecting a Nx1 object
-  * foo2(A.row().transpose());  // The row is copied into a contiguous temporary
-  * foo2(2*a);                  // The expression is evaluated into a temporary
-  * foo2(A.col().segment(2,4)); // No temporary
-  * \endcode
-  *
-  * The range of inputs that can be referenced without temporary can be enlarged using the last two template parameters.
-  * Here is an example accepting an innerstride!=1:
-  * \code
-  * // in-out argument:
-  * void foo3(Ref<VectorXf,0,InnerStride<> > x);
-  * foo3(A.row());              // OK
-  * \endcode
-  * The downside here is that the function foo3 might be significantly slower than foo1 because it won't be able to exploit vectorization, and will involve more
-  * expensive address computations even if the input is contiguously stored in memory. To overcome this issue, one might propose to overload internally calling a
-  * template function, e.g.:
-  * \code
-  * // in the .h:
-  * void foo(const Ref<MatrixXf>& A);
-  * void foo(const Ref<MatrixXf,0,Stride<> >& A);
-  *
-  * // in the .cpp:
-  * template<typename TypeOfA> void foo_impl(const TypeOfA& A) {
-  *     ... // crazy code goes here
-  * }
-  * void foo(const Ref<MatrixXf>& A) { foo_impl(A); }
-  * void foo(const Ref<MatrixXf,0,Stride<> >& A) { foo_impl(A); }
-  * \endcode
-  *
-  * See also the following stackoverflow questions for further references:
-  *  - <a href="http://stackoverflow.com/questions/21132538/correct-usage-of-the-eigenref-class">Correct usage of the Eigen::Ref<> class</a>
-  *
-  * \sa PlainObjectBase::Map(), \ref TopicStorageOrders
-  */
-template<typename PlainObjectType, int Options, typename StrideType> class Ref
-  : public RefBase<Ref<PlainObjectType, Options, StrideType> >
-{
-  private:
-    typedef internal::traits<Ref> Traits;
-    template<typename Derived>
-    EIGEN_DEVICE_FUNC inline Ref(const PlainObjectBase<Derived>& expr,
-                                 typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0);
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief A matrix or vector expression mapping an existing expression
+ *
+ * \tparam PlainObjectType the equivalent matrix type of the mapped data
+ * \tparam Options specifies the pointer alignment in bytes. It can be: \c #Aligned128, , \c #Aligned64, \c #Aligned32,
+ * \c #Aligned16, \c #Aligned8 or \c #Unaligned. The default is \c #Unaligned. \tparam StrideType optionally specifies
+ * strides. By default, Ref implies a contiguous storage along the inner dimension (inner stride==1), but accepts a
+ * variable outer stride (leading dimension). This can be overridden by specifying strides. The type passed here must be
+ * a specialization of the Stride template, see examples below.
+ *
+ * This class provides a way to write non-template functions taking Eigen objects as parameters while limiting the
+ * number of copies. A Ref<> object can represent either a const expression or a l-value: \code
+ * // in-out argument:
+ * void foo1(Ref<VectorXf> x);
+ *
+ * // read-only const argument:
+ * void foo2(const Ref<const VectorXf>& x);
+ * \endcode
+ *
+ * In the in-out case, the input argument must satisfy the constraints of the actual Ref<> type, otherwise a compilation
+ * issue will be triggered. By default, a Ref<VectorXf> can reference any dense vector expression of float having a
+ * contiguous memory layout. Likewise, a Ref<MatrixXf> can reference any column-major dense matrix expression of float
+ * whose column's elements are contiguously stored with the possibility to have a constant space in-between each column,
+ * i.e. the inner stride must be equal to 1, but the outer stride (or leading dimension) can be greater than the number
+ * of rows.
+ *
+ * In the const case, if the input expression does not match the above requirement, then it is evaluated into a
+ * temporary before being passed to the function. Here are some examples: \code MatrixXf A; VectorXf a; foo1(a.head());
+ * // OK foo1(A.col());              // OK foo1(A.row());              // Compilation error because here innerstride!=1
+ * foo2(A.row());              // Compilation error because A.row() is a 1xN object while foo2 is expecting a Nx1 object
+ * foo2(A.row().transpose());  // The row is copied into a contiguous temporary
+ * foo2(2*a);                  // The expression is evaluated into a temporary
+ * foo2(A.col().segment(2,4)); // No temporary
+ * \endcode
+ *
+ * The range of inputs that can be referenced without temporary can be enlarged using the last two template parameters.
+ * Here is an example accepting an innerstride!=1:
+ * \code
+ * // in-out argument:
+ * void foo3(Ref<VectorXf,0,InnerStride<> > x);
+ * foo3(A.row());              // OK
+ * \endcode
+ * The downside here is that the function foo3 might be significantly slower than foo1 because it won't be able to
+ * exploit vectorization, and will involve more expensive address computations even if the input is contiguously stored
+ * in memory. To overcome this issue, one might propose to overload internally calling a template function, e.g.: \code
+ * // in the .h:
+ * void foo(const Ref<MatrixXf>& A);
+ * void foo(const Ref<MatrixXf,0,Stride<> >& A);
+ *
+ * // in the .cpp:
+ * template<typename TypeOfA> void foo_impl(const TypeOfA& A) {
+ *     ... // crazy code goes here
+ * }
+ * void foo(const Ref<MatrixXf>& A) { foo_impl(A); }
+ * void foo(const Ref<MatrixXf,0,Stride<> >& A) { foo_impl(A); }
+ * \endcode
+ *
+ * See also the following stackoverflow questions for further references:
+ *  - <a href="http://stackoverflow.com/questions/21132538/correct-usage-of-the-eigenref-class">Correct usage of the
+ * Eigen::Ref<> class</a>
+ *
+ * \sa PlainObjectBase::Map(), \ref TopicStorageOrders
+ */
+template <typename PlainObjectType, int Options, typename StrideType>
+class Ref : public RefBase<Ref<PlainObjectType, Options, StrideType> > {
+ private:
+  typedef internal::traits<Ref> Traits;
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline Ref(
+      const PlainObjectBase<Derived>& expr,
+      std::enable_if_t<bool(Traits::template match<Derived>::MatchAtCompileTime), Derived>* = 0);
 
-    typedef RefBase<Ref> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Ref)
+ public:
+  typedef RefBase<Ref> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Ref)
 
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline Ref(
+      PlainObjectBase<Derived>& expr,
+      std::enable_if_t<bool(Traits::template match<Derived>::MatchAtCompileTime), Derived>* = 0) {
+    EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+    // Construction must pass since we will not create temporary storage in the non-const case.
+    const bool success = Base::construct(expr.derived());
+    EIGEN_UNUSED_VARIABLE(success)
+    eigen_assert(success);
+  }
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline Ref(
+      const DenseBase<Derived>& expr,
+      std::enable_if_t<bool(Traits::template match<Derived>::MatchAtCompileTime), Derived>* = 0)
+#else
+  /** Implicit constructor from any dense expression */
+  template <typename Derived>
+  inline Ref(DenseBase<Derived>& expr)
+#endif
+  {
+    EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+    EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+    EIGEN_STATIC_ASSERT(!Derived::IsPlainObjectBase, THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+    // Construction must pass since we will not create temporary storage in the non-const case.
+    const bool success = Base::construct(expr.const_cast_derived());
+    EIGEN_UNUSED_VARIABLE(success)
+    eigen_assert(success);
+  }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename Derived>
-    EIGEN_DEVICE_FUNC inline Ref(PlainObjectBase<Derived>& expr,
-                                 typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0)
-    {
-      EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      // Construction must pass since we will not create temprary storage in the non-const case.
-      const bool success = Base::construct(expr.derived());
-      EIGEN_UNUSED_VARIABLE(success)
-      eigen_assert(success);
-    }
-    template<typename Derived>
-    EIGEN_DEVICE_FUNC inline Ref(const DenseBase<Derived>& expr,
-                                 typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0)
-    #else
-    /** Implicit constructor from any dense expression */
-    template<typename Derived>
-    inline Ref(DenseBase<Derived>& expr)
-    #endif
-    {
-      EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
-      EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      EIGEN_STATIC_ASSERT(!Derived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
-      // Construction must pass since we will not create temporary storage in the non-const case.
-      const bool success = Base::construct(expr.const_cast_derived());
-      EIGEN_UNUSED_VARIABLE(success)
-      eigen_assert(success);
-    }
-
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Ref)
-
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Ref)
 };
 
 // this is the const ref version
-template<typename TPlainObjectType, int Options, typename StrideType> class Ref<const TPlainObjectType, Options, StrideType>
-  : public RefBase<Ref<const TPlainObjectType, Options, StrideType> >
-{
-    typedef internal::traits<Ref> Traits;
-  public:
+template <typename TPlainObjectType, int Options, typename StrideType>
+class Ref<const TPlainObjectType, Options, StrideType>
+    : public RefBase<Ref<const TPlainObjectType, Options, StrideType> > {
+  typedef internal::traits<Ref> Traits;
 
-    typedef RefBase<Ref> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Ref)
+  static constexpr bool may_map_m_object_successfully =
+      (static_cast<int>(StrideType::InnerStrideAtCompileTime) == 0 ||
+       static_cast<int>(StrideType::InnerStrideAtCompileTime) == 1 ||
+       static_cast<int>(StrideType::InnerStrideAtCompileTime) == Dynamic) &&
+      (TPlainObjectType::IsVectorAtCompileTime || static_cast<int>(StrideType::OuterStrideAtCompileTime) == 0 ||
+       static_cast<int>(StrideType::OuterStrideAtCompileTime) == Dynamic ||
+       static_cast<int>(StrideType::OuterStrideAtCompileTime) ==
+           static_cast<int>(TPlainObjectType::InnerSizeAtCompileTime) ||
+       static_cast<int>(TPlainObjectType::InnerSizeAtCompileTime) == Dynamic);
 
-    template<typename Derived>
-    EIGEN_DEVICE_FUNC inline Ref(const DenseBase<Derived>& expr,
-                                 typename internal::enable_if<bool(Traits::template match<Derived>::ScalarTypeMatch),Derived>::type* = 0)
-    {
-//      std::cout << match_helper<Derived>::HasDirectAccess << "," << match_helper<Derived>::OuterStrideMatch << "," << match_helper<Derived>::InnerStrideMatch << "\n";
-//      std::cout << int(StrideType::OuterStrideAtCompileTime) << " - " << int(Derived::OuterStrideAtCompileTime) << "\n";
-//      std::cout << int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n";
-      construct(expr.derived(), typename Traits::template match<Derived>::type());
-    }
+ public:
+  typedef RefBase<Ref> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Ref)
 
-    EIGEN_DEVICE_FUNC inline Ref(const Ref& other) : Base(other) {
-      // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
-    }
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline Ref(const DenseBase<Derived>& expr,
+                               std::enable_if_t<bool(Traits::template match<Derived>::ScalarTypeMatch), Derived>* = 0) {
+    //      std::cout << match_helper<Derived>::HasDirectAccess << "," << match_helper<Derived>::OuterStrideMatch << ","
+    //      << match_helper<Derived>::InnerStrideMatch << "\n"; std::cout << int(StrideType::OuterStrideAtCompileTime)
+    //      << " - " << int(Derived::OuterStrideAtCompileTime) << "\n"; std::cout <<
+    //      int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n";
+    EIGEN_STATIC_ASSERT(Traits::template match<Derived>::type::value || may_map_m_object_successfully,
+                        STORAGE_LAYOUT_DOES_NOT_MATCH);
+    construct(expr.derived(), typename Traits::template match<Derived>::type());
+  }
 
-    template<typename OtherRef>
-    EIGEN_DEVICE_FUNC inline Ref(const RefBase<OtherRef>& other) {
-      construct(other.derived(), typename Traits::template match<OtherRef>::type());
-    }
+  EIGEN_DEVICE_FUNC inline Ref(const Ref& other) : Base(other) {
+    // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+  }
 
-  protected:
-
-    template<typename Expression>
-    EIGEN_DEVICE_FUNC void construct(const Expression& expr,internal::true_type)
-    {
-      // Check if we can use the underlying expr's storage directly, otherwise call the copy version.
-      if (!Base::construct(expr)) {
-        construct(expr, internal::false_type());
-      }
-    }
-
-    template<typename Expression>
-    EIGEN_DEVICE_FUNC void construct(const Expression& expr, internal::false_type)
-    {
-      internal::call_assignment_no_alias(m_object,expr,internal::assign_op<Scalar,Scalar>());
+  EIGEN_DEVICE_FUNC inline Ref(Ref&& other) {
+    if (other.data() == other.m_object.data()) {
+      m_object = std::move(other.m_object);
       Base::construct(m_object);
-    }
+    } else
+      Base::construct(other);
+  }
 
-  protected:
-    TPlainObjectType m_object;
+  template <typename OtherRef>
+  EIGEN_DEVICE_FUNC inline Ref(const RefBase<OtherRef>& other) {
+    EIGEN_STATIC_ASSERT(Traits::template match<OtherRef>::type::value || may_map_m_object_successfully,
+                        STORAGE_LAYOUT_DOES_NOT_MATCH);
+    construct(other.derived(), typename Traits::template match<OtherRef>::type());
+  }
+
+ protected:
+  template <typename Expression>
+  EIGEN_DEVICE_FUNC void construct(const Expression& expr, internal::true_type) {
+    // Check if we can use the underlying expr's storage directly, otherwise call the copy version.
+    if (!Base::construct(expr)) {
+      construct(expr, internal::false_type());
+    }
+  }
+
+  template <typename Expression>
+  EIGEN_DEVICE_FUNC void construct(const Expression& expr, internal::false_type) {
+    internal::call_assignment_no_alias(m_object, expr, internal::assign_op<Scalar, Scalar>());
+    const bool success = Base::construct(m_object);
+    EIGEN_ONLY_USED_FOR_DEBUG(success)
+    eigen_assert(success);
+  }
+
+ protected:
+  TPlainObjectType m_object;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_REF_H
+#endif  // EIGEN_REF_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h
index ab5be7e..c01c627 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h
@@ -10,133 +10,124 @@
 #ifndef EIGEN_REPLICATE_H
 #define EIGEN_REPLICATE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename MatrixType,int RowFactor,int ColFactor>
-struct traits<Replicate<MatrixType,RowFactor,ColFactor> >
- : traits<MatrixType>
-{
+template <typename MatrixType, int RowFactor, int ColFactor>
+struct traits<Replicate<MatrixType, RowFactor, ColFactor> > : traits<MatrixType> {
   typedef typename MatrixType::Scalar Scalar;
   typedef typename traits<MatrixType>::StorageKind StorageKind;
   typedef typename traits<MatrixType>::XprKind XprKind;
   typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested;
+  typedef std::remove_reference_t<MatrixTypeNested> MatrixTypeNested_;
   enum {
-    RowsAtCompileTime = RowFactor==Dynamic || int(MatrixType::RowsAtCompileTime)==Dynamic
-                      ? Dynamic
-                      : RowFactor * MatrixType::RowsAtCompileTime,
-    ColsAtCompileTime = ColFactor==Dynamic || int(MatrixType::ColsAtCompileTime)==Dynamic
-                      ? Dynamic
-                      : ColFactor * MatrixType::ColsAtCompileTime,
-   //FIXME we don't propagate the max sizes !!!
+    RowsAtCompileTime = RowFactor == Dynamic || int(MatrixType::RowsAtCompileTime) == Dynamic
+                            ? Dynamic
+                            : RowFactor * MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = ColFactor == Dynamic || int(MatrixType::ColsAtCompileTime) == Dynamic
+                            ? Dynamic
+                            : ColFactor * MatrixType::ColsAtCompileTime,
+    // FIXME we don't propagate the max sizes !!!
     MaxRowsAtCompileTime = RowsAtCompileTime,
     MaxColsAtCompileTime = ColsAtCompileTime,
-    IsRowMajor = MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1 ? 1
-               : MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1 ? 0
-               : (MatrixType::Flags & RowMajorBit) ? 1 : 0,
+    IsRowMajor = MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1   ? 1
+                 : MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1 ? 0
+                 : (MatrixType::Flags & RowMajorBit)                      ? 1
+                                                                          : 0,
 
     // FIXME enable DirectAccess with negative strides?
     Flags = IsRowMajor ? RowMajorBit : 0
   };
 };
-}
+}  // namespace internal
 
 /**
-  * \class Replicate
-  * \ingroup Core_Module
-  *
-  * \brief Expression of the multiple replication of a matrix or vector
-  *
-  * \tparam MatrixType the type of the object we are replicating
-  * \tparam RowFactor number of repetitions at compile time along the vertical direction, can be Dynamic.
-  * \tparam ColFactor number of repetitions at compile time along the horizontal direction, can be Dynamic.
-  *
-  * This class represents an expression of the multiple replication of a matrix or vector.
-  * It is the return type of DenseBase::replicate() and most of the time
-  * this is the only way it is used.
-  *
-  * \sa DenseBase::replicate()
-  */
-template<typename MatrixType,int RowFactor,int ColFactor> class Replicate
-  : public internal::dense_xpr_base< Replicate<MatrixType,RowFactor,ColFactor> >::type
-{
-    typedef typename internal::traits<Replicate>::MatrixTypeNested MatrixTypeNested;
-    typedef typename internal::traits<Replicate>::_MatrixTypeNested _MatrixTypeNested;
-  public:
+ * \class Replicate
+ * \ingroup Core_Module
+ *
+ * \brief Expression of the multiple replication of a matrix or vector
+ *
+ * \tparam MatrixType the type of the object we are replicating
+ * \tparam RowFactor number of repetitions at compile time along the vertical direction, can be Dynamic.
+ * \tparam ColFactor number of repetitions at compile time along the horizontal direction, can be Dynamic.
+ *
+ * This class represents an expression of the multiple replication of a matrix or vector.
+ * It is the return type of DenseBase::replicate() and most of the time
+ * this is the only way it is used.
+ *
+ * \sa DenseBase::replicate()
+ */
+template <typename MatrixType, int RowFactor, int ColFactor>
+class Replicate : public internal::dense_xpr_base<Replicate<MatrixType, RowFactor, ColFactor> >::type {
+  typedef typename internal::traits<Replicate>::MatrixTypeNested MatrixTypeNested;
+  typedef typename internal::traits<Replicate>::MatrixTypeNested_ MatrixTypeNested_;
 
-    typedef typename internal::dense_xpr_base<Replicate>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Replicate)
-    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+ public:
+  typedef typename internal::dense_xpr_base<Replicate>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Replicate)
+  typedef internal::remove_all_t<MatrixType> NestedExpression;
 
-    template<typename OriginalMatrixType>
-    EIGEN_DEVICE_FUNC
-    inline explicit Replicate(const OriginalMatrixType& matrix)
-      : m_matrix(matrix), m_rowFactor(RowFactor), m_colFactor(ColFactor)
-    {
-      EIGEN_STATIC_ASSERT((internal::is_same<typename internal::remove_const<MatrixType>::type,OriginalMatrixType>::value),
-                          THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE)
-      eigen_assert(RowFactor!=Dynamic && ColFactor!=Dynamic);
-    }
+  template <typename OriginalMatrixType>
+  EIGEN_DEVICE_FUNC inline explicit Replicate(const OriginalMatrixType& matrix)
+      : m_matrix(matrix), m_rowFactor(RowFactor), m_colFactor(ColFactor) {
+    EIGEN_STATIC_ASSERT((internal::is_same<std::remove_const_t<MatrixType>, OriginalMatrixType>::value),
+                        THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE)
+    eigen_assert(RowFactor != Dynamic && ColFactor != Dynamic);
+  }
 
-    template<typename OriginalMatrixType>
-    EIGEN_DEVICE_FUNC
-    inline Replicate(const OriginalMatrixType& matrix, Index rowFactor, Index colFactor)
-      : m_matrix(matrix), m_rowFactor(rowFactor), m_colFactor(colFactor)
-    {
-      EIGEN_STATIC_ASSERT((internal::is_same<typename internal::remove_const<MatrixType>::type,OriginalMatrixType>::value),
-                          THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE)
-    }
+  template <typename OriginalMatrixType>
+  EIGEN_DEVICE_FUNC inline Replicate(const OriginalMatrixType& matrix, Index rowFactor, Index colFactor)
+      : m_matrix(matrix),
+        m_rowFactor(rowFactor),
+        m_colFactor(colFactor){
+            EIGEN_STATIC_ASSERT((internal::is_same<std::remove_const_t<MatrixType>, OriginalMatrixType>::value),
+                                THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE)}
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const { return m_matrix.rows() * m_rowFactor.value(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const { return m_matrix.cols() * m_colFactor.value(); }
+        EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const {
+    return m_matrix.rows() * m_rowFactor.value();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const { return m_matrix.cols() * m_colFactor.value(); }
 
-    EIGEN_DEVICE_FUNC
-    const _MatrixTypeNested& nestedExpression() const
-    {
-      return m_matrix;
-    }
+  EIGEN_DEVICE_FUNC const MatrixTypeNested_& nestedExpression() const { return m_matrix; }
 
-  protected:
-    MatrixTypeNested m_matrix;
-    const internal::variable_if_dynamic<Index, RowFactor> m_rowFactor;
-    const internal::variable_if_dynamic<Index, ColFactor> m_colFactor;
+ protected:
+  MatrixTypeNested m_matrix;
+  const internal::variable_if_dynamic<Index, RowFactor> m_rowFactor;
+  const internal::variable_if_dynamic<Index, ColFactor> m_colFactor;
 };
 
 /**
-  * \return an expression of the replication of \c *this
-  *
-  * Example: \include MatrixBase_replicate.cpp
-  * Output: \verbinclude MatrixBase_replicate.out
-  *
-  * \sa VectorwiseOp::replicate(), DenseBase::replicate(Index,Index), class Replicate
-  */
-template<typename Derived>
-template<int RowFactor, int ColFactor>
-EIGEN_DEVICE_FUNC const Replicate<Derived,RowFactor,ColFactor>
-DenseBase<Derived>::replicate() const
-{
-  return Replicate<Derived,RowFactor,ColFactor>(derived());
+ * \return an expression of the replication of \c *this
+ *
+ * Example: \include MatrixBase_replicate.cpp
+ * Output: \verbinclude MatrixBase_replicate.out
+ *
+ * \sa VectorwiseOp::replicate(), DenseBase::replicate(Index,Index), class Replicate
+ */
+template <typename Derived>
+template <int RowFactor, int ColFactor>
+EIGEN_DEVICE_FUNC const Replicate<Derived, RowFactor, ColFactor> DenseBase<Derived>::replicate() const {
+  return Replicate<Derived, RowFactor, ColFactor>(derived());
 }
 
 /**
-  * \return an expression of the replication of each column (or row) of \c *this
-  *
-  * Example: \include DirectionWise_replicate_int.cpp
-  * Output: \verbinclude DirectionWise_replicate_int.out
-  *
-  * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate
-  */
-template<typename ExpressionType, int Direction>
-EIGEN_DEVICE_FUNC const typename VectorwiseOp<ExpressionType,Direction>::ReplicateReturnType
-VectorwiseOp<ExpressionType,Direction>::replicate(Index factor) const
-{
-  return typename VectorwiseOp<ExpressionType,Direction>::ReplicateReturnType
-          (_expression(),Direction==Vertical?factor:1,Direction==Horizontal?factor:1);
+ * \return an expression of the replication of each column (or row) of \c *this
+ *
+ * Example: \include DirectionWise_replicate_int.cpp
+ * Output: \verbinclude DirectionWise_replicate_int.out
+ *
+ * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate
+ */
+template <typename ExpressionType, int Direction>
+EIGEN_DEVICE_FUNC const typename VectorwiseOp<ExpressionType, Direction>::ReplicateReturnType
+VectorwiseOp<ExpressionType, Direction>::replicate(Index factor) const {
+  return typename VectorwiseOp<ExpressionType, Direction>::ReplicateReturnType(
+      _expression(), Direction == Vertical ? factor : 1, Direction == Horizontal ? factor : 1);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_REPLICATE_H
+#endif  // EIGEN_REPLICATE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h
index 52de73b..b881dd6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h
@@ -11,47 +11,48 @@
 #ifndef EIGEN_RESHAPED_H
 #define EIGEN_RESHAPED_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class Reshaped
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a fixed-size or dynamic-size reshape
-  *
-  * \tparam XprType the type of the expression in which we are taking a reshape
-  * \tparam Rows the number of rows of the reshape we are taking at compile time (optional)
-  * \tparam Cols the number of columns of the reshape we are taking at compile time (optional)
-  * \tparam Order can be ColMajor or RowMajor, default is ColMajor.
-  *
-  * This class represents an expression of either a fixed-size or dynamic-size reshape.
-  * It is the return type of DenseBase::reshaped(NRowsType,NColsType) and
-  * most of the time this is the only way it is used.
-  *
-  * However, in C++98, if you want to directly maniputate reshaped expressions,
-  * for instance if you want to write a function returning such an expression, you
-  * will need to use this class. In C++11, it is advised to use the \em auto
-  * keyword for such use cases.
-  *
-  * Here is an example illustrating the dynamic case:
-  * \include class_Reshaped.cpp
-  * Output: \verbinclude class_Reshaped.out
-  *
-  * Here is an example illustrating the fixed-size case:
-  * \include class_FixedReshaped.cpp
-  * Output: \verbinclude class_FixedReshaped.out
-  *
-  * \sa DenseBase::reshaped(NRowsType,NColsType)
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a fixed-size or dynamic-size reshape
+ *
+ * \tparam XprType the type of the expression in which we are taking a reshape
+ * \tparam Rows the number of rows of the reshape we are taking at compile time (optional)
+ * \tparam Cols the number of columns of the reshape we are taking at compile time (optional)
+ * \tparam Order can be ColMajor or RowMajor, default is ColMajor.
+ *
+ * This class represents an expression of either a fixed-size or dynamic-size reshape.
+ * It is the return type of DenseBase::reshaped(NRowsType,NColsType) and
+ * most of the time this is the only way it is used.
+ *
+ * If you want to directly manipulate reshaped expressions,
+ * for instance if you want to write a function returning such an expression,
+ * it is advised to use the \em auto keyword for such use cases.
+ *
+ * Here is an example illustrating the dynamic case:
+ * \include class_Reshaped.cpp
+ * Output: \verbinclude class_Reshaped.out
+ *
+ * Here is an example illustrating the fixed-size case:
+ * \include class_FixedReshaped.cpp
+ * Output: \verbinclude class_FixedReshaped.out
+ *
+ * \sa DenseBase::reshaped(NRowsType,NColsType)
+ */
 
 namespace internal {
 
-template<typename XprType, int Rows, int Cols, int Order>
-struct traits<Reshaped<XprType, Rows, Cols, Order> > : traits<XprType>
-{
+template <typename XprType, int Rows, int Cols, int Order>
+struct traits<Reshaped<XprType, Rows, Cols, Order> > : traits<XprType> {
   typedef typename traits<XprType>::Scalar Scalar;
   typedef typename traits<XprType>::StorageKind StorageKind;
   typedef typename traits<XprType>::XprKind XprKind;
-  enum{
+  enum {
     MatrixRows = traits<XprType>::RowsAtCompileTime,
     MatrixCols = traits<XprType>::ColsAtCompileTime,
     RowsAtCompileTime = Rows,
@@ -59,212 +60,179 @@
     MaxRowsAtCompileTime = Rows,
     MaxColsAtCompileTime = Cols,
     XpxStorageOrder = ((int(traits<XprType>::Flags) & RowMajorBit) == RowMajorBit) ? RowMajor : ColMajor,
-    ReshapedStorageOrder = (RowsAtCompileTime == 1 && ColsAtCompileTime != 1) ? RowMajor
-                         : (ColsAtCompileTime == 1 && RowsAtCompileTime != 1) ? ColMajor
-                         : XpxStorageOrder,
+    ReshapedStorageOrder = (RowsAtCompileTime == 1 && ColsAtCompileTime != 1)   ? RowMajor
+                           : (ColsAtCompileTime == 1 && RowsAtCompileTime != 1) ? ColMajor
+                                                                                : XpxStorageOrder,
     HasSameStorageOrderAsXprType = (ReshapedStorageOrder == XpxStorageOrder),
-    InnerSize = (ReshapedStorageOrder==int(RowMajor)) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
-    InnerStrideAtCompileTime = HasSameStorageOrderAsXprType
-                             ? int(inner_stride_at_compile_time<XprType>::ret)
-                             : Dynamic,
+    InnerSize = (ReshapedStorageOrder == int(RowMajor)) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+    InnerStrideAtCompileTime = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time<XprType>::ret) : Dynamic,
     OuterStrideAtCompileTime = Dynamic,
 
-    HasDirectAccess = internal::has_direct_access<XprType>::ret
-                    && (Order==int(XpxStorageOrder))
-                    && ((evaluator<XprType>::Flags&LinearAccessBit)==LinearAccessBit),
+    HasDirectAccess = internal::has_direct_access<XprType>::ret && (Order == int(XpxStorageOrder)) &&
+                      ((evaluator<XprType>::Flags & LinearAccessBit) == LinearAccessBit),
 
-    MaskPacketAccessBit = (InnerSize == Dynamic || (InnerSize % packet_traits<Scalar>::size) == 0)
-                       && (InnerStrideAtCompileTime == 1)
-                        ? PacketAccessBit : 0,
-    //MaskAlignedBit = ((OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0,
+    MaskPacketAccessBit =
+        (InnerSize == Dynamic || (InnerSize % packet_traits<Scalar>::size) == 0) && (InnerStrideAtCompileTime == 1)
+            ? PacketAccessBit
+            : 0,
+    // MaskAlignedBit = ((OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16)
+    // == 0)) ? AlignedBit : 0,
     FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
     FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
-    FlagsRowMajorBit = (ReshapedStorageOrder==int(RowMajor)) ? RowMajorBit : 0,
+    FlagsRowMajorBit = (ReshapedStorageOrder == int(RowMajor)) ? RowMajorBit : 0,
     FlagsDirectAccessBit = HasDirectAccess ? DirectAccessBit : 0,
-    Flags0 = traits<XprType>::Flags & ( (HereditaryBits & ~RowMajorBit) | MaskPacketAccessBit),
+    Flags0 = traits<XprType>::Flags & ((HereditaryBits & ~RowMajorBit) | MaskPacketAccessBit),
 
     Flags = (Flags0 | FlagsLinearAccessBit | FlagsLvalueBit | FlagsRowMajorBit | FlagsDirectAccessBit)
   };
 };
 
-template<typename XprType, int Rows, int Cols, int Order, bool HasDirectAccess> class ReshapedImpl_dense;
+template <typename XprType, int Rows, int Cols, int Order, bool HasDirectAccess>
+class ReshapedImpl_dense;
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename XprType, int Rows, int Cols, int Order, typename StorageKind> class ReshapedImpl;
+template <typename XprType, int Rows, int Cols, int Order, typename StorageKind>
+class ReshapedImpl;
 
-template<typename XprType, int Rows, int Cols, int Order> class Reshaped
-  : public ReshapedImpl<XprType, Rows, Cols, Order, typename internal::traits<XprType>::StorageKind>
-{
-    typedef ReshapedImpl<XprType, Rows, Cols, Order, typename internal::traits<XprType>::StorageKind> Impl;
-  public:
-    //typedef typename Impl::Base Base;
-    typedef Impl Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(Reshaped)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reshaped)
+template <typename XprType, int Rows, int Cols, int Order>
+class Reshaped : public ReshapedImpl<XprType, Rows, Cols, Order, typename internal::traits<XprType>::StorageKind> {
+  typedef ReshapedImpl<XprType, Rows, Cols, Order, typename internal::traits<XprType>::StorageKind> Impl;
 
-    /** Fixed-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline Reshaped(XprType& xpr)
-      : Impl(xpr)
-    {
-      EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
-      eigen_assert(Rows * Cols == xpr.rows() * xpr.cols());
-    }
+ public:
+  // typedef typename Impl::Base Base;
+  typedef Impl Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(Reshaped)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reshaped)
 
-    /** Dynamic-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline Reshaped(XprType& xpr,
-          Index reshapeRows, Index reshapeCols)
-      : Impl(xpr, reshapeRows, reshapeCols)
-    {
-      eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==reshapeRows)
-          && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==reshapeCols));
-      eigen_assert(reshapeRows * reshapeCols == xpr.rows() * xpr.cols());
-    }
+  /** Fixed-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline Reshaped(XprType& xpr) : Impl(xpr) {
+    EIGEN_STATIC_ASSERT(RowsAtCompileTime != Dynamic && ColsAtCompileTime != Dynamic,
+                        THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
+    eigen_assert(Rows * Cols == xpr.rows() * xpr.cols());
+  }
+
+  /** Dynamic-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline Reshaped(XprType& xpr, Index reshapeRows, Index reshapeCols)
+      : Impl(xpr, reshapeRows, reshapeCols) {
+    eigen_assert((RowsAtCompileTime == Dynamic || RowsAtCompileTime == reshapeRows) &&
+                 (ColsAtCompileTime == Dynamic || ColsAtCompileTime == reshapeCols));
+    eigen_assert(reshapeRows * reshapeCols == xpr.rows() * xpr.cols());
+  }
 };
 
 // The generic default implementation for dense reshape simply forward to the internal::ReshapedImpl_dense
 // that must be specialized for direct and non-direct access...
-template<typename XprType, int Rows, int Cols, int Order>
+template <typename XprType, int Rows, int Cols, int Order>
 class ReshapedImpl<XprType, Rows, Cols, Order, Dense>
-  : public internal::ReshapedImpl_dense<XprType, Rows, Cols, Order,internal::traits<Reshaped<XprType,Rows,Cols,Order> >::HasDirectAccess>
-{
-    typedef internal::ReshapedImpl_dense<XprType, Rows, Cols, Order,internal::traits<Reshaped<XprType,Rows,Cols,Order> >::HasDirectAccess> Impl;
-  public:
-    typedef Impl Base;
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl)
-    EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr) : Impl(xpr) {}
-    EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr, Index reshapeRows, Index reshapeCols)
+    : public internal::ReshapedImpl_dense<XprType, Rows, Cols, Order,
+                                          internal::traits<Reshaped<XprType, Rows, Cols, Order> >::HasDirectAccess> {
+  typedef internal::ReshapedImpl_dense<XprType, Rows, Cols, Order,
+                                       internal::traits<Reshaped<XprType, Rows, Cols, Order> >::HasDirectAccess>
+      Impl;
+
+ public:
+  typedef Impl Base;
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl)
+  EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr) : Impl(xpr) {}
+  EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr, Index reshapeRows, Index reshapeCols)
       : Impl(xpr, reshapeRows, reshapeCols) {}
 };
 
 namespace internal {
 
 /** \internal Internal implementation of dense Reshaped in the general case. */
-template<typename XprType, int Rows, int Cols, int Order>
-class ReshapedImpl_dense<XprType,Rows,Cols,Order,false>
-  : public internal::dense_xpr_base<Reshaped<XprType, Rows, Cols, Order> >::type
-{
-    typedef Reshaped<XprType, Rows, Cols, Order> ReshapedType;
-  public:
+template <typename XprType, int Rows, int Cols, int Order>
+class ReshapedImpl_dense<XprType, Rows, Cols, Order, false>
+    : public internal::dense_xpr_base<Reshaped<XprType, Rows, Cols, Order> >::type {
+  typedef Reshaped<XprType, Rows, Cols, Order> ReshapedType;
 
-    typedef typename internal::dense_xpr_base<ReshapedType>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense)
+ public:
+  typedef typename internal::dense_xpr_base<ReshapedType>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense)
 
-    typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
-    typedef typename internal::remove_all<XprType>::type NestedExpression;
+  typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
+  typedef internal::remove_all_t<XprType> NestedExpression;
 
-    class InnerIterator;
+  class InnerIterator;
 
-    /** Fixed-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline ReshapedImpl_dense(XprType& xpr)
-      : m_xpr(xpr), m_rows(Rows), m_cols(Cols)
-    {}
+  /** Fixed-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr) : m_xpr(xpr), m_rows(Rows), m_cols(Cols) {}
 
-    /** Dynamic-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols)
-      : m_xpr(xpr), m_rows(nRows), m_cols(nCols)
-    {}
+  /** Dynamic-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols)
+      : m_xpr(xpr), m_rows(nRows), m_cols(nCols) {}
 
-    EIGEN_DEVICE_FUNC Index rows() const { return m_rows; }
-    EIGEN_DEVICE_FUNC Index cols() const { return m_cols; }
+  EIGEN_DEVICE_FUNC Index rows() const { return m_rows; }
+  EIGEN_DEVICE_FUNC Index cols() const { return m_cols; }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** \sa MapBase::data() */
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const;
-    EIGEN_DEVICE_FUNC inline Index innerStride() const;
-    EIGEN_DEVICE_FUNC inline Index outerStride() const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** \sa MapBase::data() */
+  EIGEN_DEVICE_FUNC inline const Scalar* data() const;
+  EIGEN_DEVICE_FUNC inline Index innerStride() const;
+  EIGEN_DEVICE_FUNC inline Index outerStride() const;
+#endif
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC
-    const typename internal::remove_all<XprType>::type&
-    nestedExpression() const { return m_xpr; }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC const internal::remove_all_t<XprType>& nestedExpression() const { return m_xpr; }
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC
-    typename internal::remove_reference<XprType>::type&
-    nestedExpression() { return m_xpr; }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC std::remove_reference_t<XprType>& nestedExpression() { return m_xpr; }
 
-  protected:
-
-    MatrixTypeNested m_xpr;
-    const internal::variable_if_dynamic<Index, Rows> m_rows;
-    const internal::variable_if_dynamic<Index, Cols> m_cols;
+ protected:
+  MatrixTypeNested m_xpr;
+  const internal::variable_if_dynamic<Index, Rows> m_rows;
+  const internal::variable_if_dynamic<Index, Cols> m_cols;
 };
 
-
 /** \internal Internal implementation of dense Reshaped in the direct access case. */
-template<typename XprType, int Rows, int Cols, int Order>
-class ReshapedImpl_dense<XprType, Rows, Cols, Order, true>
-  : public MapBase<Reshaped<XprType, Rows, Cols, Order> >
-{
-    typedef Reshaped<XprType, Rows, Cols, Order> ReshapedType;
-    typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
-  public:
+template <typename XprType, int Rows, int Cols, int Order>
+class ReshapedImpl_dense<XprType, Rows, Cols, Order, true> : public MapBase<Reshaped<XprType, Rows, Cols, Order> > {
+  typedef Reshaped<XprType, Rows, Cols, Order> ReshapedType;
+  typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
 
-    typedef MapBase<ReshapedType> Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense)
+ public:
+  typedef MapBase<ReshapedType> Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense)
 
-    /** Fixed-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline ReshapedImpl_dense(XprType& xpr)
-      : Base(xpr.data()), m_xpr(xpr)
-    {}
+  /** Fixed-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr) : Base(xpr.data()), m_xpr(xpr) {}
 
-    /** Dynamic-size constructor
-      */
-    EIGEN_DEVICE_FUNC
-    inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols)
-      : Base(xpr.data(), nRows, nCols),
-        m_xpr(xpr)
-    {}
+  /** Dynamic-size constructor
+   */
+  EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols)
+      : Base(xpr.data(), nRows, nCols), m_xpr(xpr) {}
 
-    EIGEN_DEVICE_FUNC
-    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const
-    {
-      return m_xpr;
-    }
+  EIGEN_DEVICE_FUNC const internal::remove_all_t<XprTypeNested>& nestedExpression() const { return m_xpr; }
 
-    EIGEN_DEVICE_FUNC
-    XprType& nestedExpression() { return m_xpr; }
+  EIGEN_DEVICE_FUNC XprType& nestedExpression() { return m_xpr; }
 
-    /** \sa MapBase::innerStride() */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const
-    {
-      return m_xpr.innerStride();
-    }
+  /** \sa MapBase::innerStride() */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { return m_xpr.innerStride(); }
 
-    /** \sa MapBase::outerStride() */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const
-    {
-      return ((Flags&RowMajorBit)==RowMajorBit) ? this->cols() : this->rows();
-    }
+  /** \sa MapBase::outerStride() */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const {
+    return (((Flags & RowMajorBit) == RowMajorBit) ? this->cols() : this->rows()) * m_xpr.innerStride();
+  }
 
-  protected:
-
-    XprTypeNested m_xpr;
+ protected:
+  XprTypeNested m_xpr;
 };
 
 // Evaluators
-template<typename ArgType, int Rows, int Cols, int Order, bool HasDirectAccess> struct reshaped_evaluator;
+template <typename ArgType, int Rows, int Cols, int Order, bool HasDirectAccess>
+struct reshaped_evaluator;
 
-template<typename ArgType, int Rows, int Cols, int Order>
+template <typename ArgType, int Rows, int Cols, int Order>
 struct evaluator<Reshaped<ArgType, Rows, Cols, Order> >
-  : reshaped_evaluator<ArgType, Rows, Cols, Order, traits<Reshaped<ArgType,Rows,Cols,Order> >::HasDirectAccess>
-{
+    : reshaped_evaluator<ArgType, Rows, Cols, Order, traits<Reshaped<ArgType, Rows, Cols, Order> >::HasDirectAccess> {
   typedef Reshaped<ArgType, Rows, Cols, Order> XprType;
   typedef typename XprType::Scalar Scalar;
   // TODO: should check for smaller packet types
@@ -274,19 +242,22 @@
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
     HasDirectAccess = traits<XprType>::HasDirectAccess,
 
-//     RowsAtCompileTime = traits<XprType>::RowsAtCompileTime,
-//     ColsAtCompileTime = traits<XprType>::ColsAtCompileTime,
-//     MaxRowsAtCompileTime = traits<XprType>::MaxRowsAtCompileTime,
-//     MaxColsAtCompileTime = traits<XprType>::MaxColsAtCompileTime,
-//
-//     InnerStrideAtCompileTime = traits<XprType>::HasSameStorageOrderAsXprType
-//                              ? int(inner_stride_at_compile_time<ArgType>::ret)
-//                              : Dynamic,
-//     OuterStrideAtCompileTime = Dynamic,
+    //     RowsAtCompileTime = traits<XprType>::RowsAtCompileTime,
+    //     ColsAtCompileTime = traits<XprType>::ColsAtCompileTime,
+    //     MaxRowsAtCompileTime = traits<XprType>::MaxRowsAtCompileTime,
+    //     MaxColsAtCompileTime = traits<XprType>::MaxColsAtCompileTime,
+    //
+    //     InnerStrideAtCompileTime = traits<XprType>::HasSameStorageOrderAsXprType
+    //                              ? int(inner_stride_at_compile_time<ArgType>::ret)
+    //                              : Dynamic,
+    //     OuterStrideAtCompileTime = Dynamic,
 
-    FlagsLinearAccessBit = (traits<XprType>::RowsAtCompileTime == 1 || traits<XprType>::ColsAtCompileTime == 1 || HasDirectAccess) ? LinearAccessBit : 0,
-    FlagsRowMajorBit = (traits<XprType>::ReshapedStorageOrder==int(RowMajor)) ? RowMajorBit : 0,
-    FlagsDirectAccessBit =  HasDirectAccess ? DirectAccessBit : 0,
+    FlagsLinearAccessBit =
+        (traits<XprType>::RowsAtCompileTime == 1 || traits<XprType>::ColsAtCompileTime == 1 || HasDirectAccess)
+            ? LinearAccessBit
+            : 0,
+    FlagsRowMajorBit = (traits<XprType>::ReshapedStorageOrder == int(RowMajor)) ? RowMajorBit : 0,
+    FlagsDirectAccessBit = HasDirectAccess ? DirectAccessBit : 0,
     Flags0 = evaluator<ArgType>::Flags & (HereditaryBits & ~RowMajorBit),
     Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit | FlagsDirectAccessBit,
 
@@ -294,16 +265,14 @@
     Alignment = evaluator<ArgType>::Alignment
   };
   typedef reshaped_evaluator<ArgType, Rows, Cols, Order, HasDirectAccess> reshaped_evaluator_type;
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : reshaped_evaluator_type(xpr)
-  {
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : reshaped_evaluator_type(xpr) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 };
 
-template<typename ArgType, int Rows, int Cols, int Order>
+template <typename ArgType, int Rows, int Cols, int Order>
 struct reshaped_evaluator<ArgType, Rows, Cols, Order, /* HasDirectAccess */ false>
-  : evaluator_base<Reshaped<ArgType, Rows, Cols, Order> >
-{
+    : evaluator_base<Reshaped<ArgType, Rows, Cols, Order> > {
   typedef Reshaped<ArgType, Rows, Cols, Order> XprType;
 
   enum {
@@ -314,8 +283,7 @@
     Alignment = 0
   };
 
-  EIGEN_DEVICE_FUNC explicit reshaped_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
-  {
+  EIGEN_DEVICE_FUNC explicit reshaped_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
@@ -324,67 +292,45 @@
 
   typedef std::pair<Index, Index> RowCol;
 
-  inline RowCol index_remap(Index rowId, Index colId) const
-  {
-    if(Order==ColMajor)
-    {
+  EIGEN_DEVICE_FUNC inline RowCol index_remap(Index rowId, Index colId) const {
+    if (Order == ColMajor) {
       const Index nth_elem_idx = colId * m_xpr.rows() + rowId;
-      return RowCol(nth_elem_idx % m_xpr.nestedExpression().rows(),
-                    nth_elem_idx / m_xpr.nestedExpression().rows());
-    }
-    else
-    {
+      return RowCol(nth_elem_idx % m_xpr.nestedExpression().rows(), nth_elem_idx / m_xpr.nestedExpression().rows());
+    } else {
       const Index nth_elem_idx = colId + rowId * m_xpr.cols();
-      return RowCol(nth_elem_idx / m_xpr.nestedExpression().cols(),
-                    nth_elem_idx % m_xpr.nestedExpression().cols());
+      return RowCol(nth_elem_idx / m_xpr.nestedExpression().cols(), nth_elem_idx % m_xpr.nestedExpression().cols());
     }
   }
 
-  EIGEN_DEVICE_FUNC
-  inline Scalar& coeffRef(Index rowId, Index colId)
-  {
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index rowId, Index colId) {
     EIGEN_STATIC_ASSERT_LVALUE(XprType)
     const RowCol row_col = index_remap(rowId, colId);
     return m_argImpl.coeffRef(row_col.first, row_col.second);
   }
 
-  EIGEN_DEVICE_FUNC
-  inline const Scalar& coeffRef(Index rowId, Index colId) const
-  {
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const {
     const RowCol row_col = index_remap(rowId, colId);
     return m_argImpl.coeffRef(row_col.first, row_col.second);
   }
 
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const
-  {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const {
     const RowCol row_col = index_remap(rowId, colId);
     return m_argImpl.coeff(row_col.first, row_col.second);
   }
 
-  EIGEN_DEVICE_FUNC
-  inline Scalar& coeffRef(Index index)
-  {
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) {
     EIGEN_STATIC_ASSERT_LVALUE(XprType)
-    const RowCol row_col = index_remap(Rows == 1 ? 0 : index,
-                                       Rows == 1 ? index : 0);
-    return m_argImpl.coeffRef(row_col.first, row_col.second);
-
-  }
-
-  EIGEN_DEVICE_FUNC
-  inline const Scalar& coeffRef(Index index) const
-  {
-    const RowCol row_col = index_remap(Rows == 1 ? 0 : index,
-                                       Rows == 1 ? index : 0);
+    const RowCol row_col = index_remap(Rows == 1 ? 0 : index, Rows == 1 ? index : 0);
     return m_argImpl.coeffRef(row_col.first, row_col.second);
   }
 
-  EIGEN_DEVICE_FUNC
-  inline const CoeffReturnType coeff(Index index) const
-  {
-    const RowCol row_col = index_remap(Rows == 1 ? 0 : index,
-                                       Rows == 1 ? index : 0);
+  EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const {
+    const RowCol row_col = index_remap(Rows == 1 ? 0 : index, Rows == 1 ? index : 0);
+    return m_argImpl.coeffRef(row_col.first, row_col.second);
+  }
+
+  EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const {
+    const RowCol row_col = index_remap(Rows == 1 ? 0 : index, Rows == 1 ? index : 0);
     return m_argImpl.coeff(row_col.first, row_col.second);
   }
 #if 0
@@ -424,31 +370,29 @@
     return m_argImpl.template packet<Unaligned>(row_col.first, row_col.second, val);
   }
 #endif
-protected:
-
+ protected:
   evaluator<ArgType> m_argImpl;
   const XprType& m_xpr;
-
 };
 
-template<typename ArgType, int Rows, int Cols, int Order>
+template <typename ArgType, int Rows, int Cols, int Order>
 struct reshaped_evaluator<ArgType, Rows, Cols, Order, /* HasDirectAccess */ true>
-: mapbase_evaluator<Reshaped<ArgType, Rows, Cols, Order>,
-                      typename Reshaped<ArgType, Rows, Cols, Order>::PlainObject>
-{
+    : mapbase_evaluator<Reshaped<ArgType, Rows, Cols, Order>,
+                        typename Reshaped<ArgType, Rows, Cols, Order>::PlainObject> {
   typedef Reshaped<ArgType, Rows, Cols, Order> XprType;
   typedef typename XprType::Scalar Scalar;
 
   EIGEN_DEVICE_FUNC explicit reshaped_evaluator(const XprType& xpr)
-    : mapbase_evaluator<XprType, typename XprType::PlainObject>(xpr)
-  {
-    // TODO: for the 3.4 release, this should be turned to an internal assertion, but let's keep it as is for the beta lifetime
-    eigen_assert(((internal::UIntPtr(xpr.data()) % EIGEN_PLAIN_ENUM_MAX(1,evaluator<XprType>::Alignment)) == 0) && "data is not aligned");
+      : mapbase_evaluator<XprType, typename XprType::PlainObject>(xpr) {
+    // TODO: for the 3.4 release, this should be turned to an internal assertion, but let's keep it as is for the beta
+    // lifetime
+    eigen_assert(((std::uintptr_t(xpr.data()) % plain_enum_max(1, evaluator<XprType>::Alignment)) == 0) &&
+                 "data is not aligned");
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_RESHAPED_H
+#endif  // EIGEN_RESHAPED_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h
index 4dad13e..3b5e470 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h
@@ -11,20 +11,20 @@
 #ifndef EIGEN_RETURNBYVALUE_H
 #define EIGEN_RETURNBYVALUE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename Derived>
-struct traits<ReturnByValue<Derived> >
-  : public traits<typename traits<Derived>::ReturnType>
-{
+template <typename Derived>
+struct traits<ReturnByValue<Derived> > : public traits<typename traits<Derived>::ReturnType> {
   enum {
     // We're disabling the DirectAccess because e.g. the constructor of
     // the Block-with-DirectAccess expression requires to have a coeffRef method.
     // Also, we don't want to have to implement the stride stuff.
-    Flags = (traits<typename traits<Derived>::ReturnType>::Flags
-             | EvalBeforeNestingBit) & ~DirectAccessBit
+    Flags = (traits<typename traits<Derived>::ReturnType>::Flags | EvalBeforeNestingBit) & ~DirectAccessBit
   };
 };
 
@@ -35,54 +35,54 @@
  * FIXME: I don't understand why we need this specialization: isn't this taken care of by the EvalBeforeNestingBit ??
  * Answer: EvalBeforeNestingBit should be deprecated since we have the evaluators
  */
-template<typename Derived,int n,typename PlainObject>
-struct nested_eval<ReturnByValue<Derived>, n, PlainObject>
-{
+template <typename Derived, int n, typename PlainObject>
+struct nested_eval<ReturnByValue<Derived>, n, PlainObject> {
   typedef typename traits<Derived>::ReturnType type;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \class ReturnByValue
-  * \ingroup Core_Module
-  *
-  */
-template<typename Derived> class ReturnByValue
-  : public internal::dense_xpr_base< ReturnByValue<Derived> >::type, internal::no_assignment_operator
-{
-  public:
-    typedef typename internal::traits<Derived>::ReturnType ReturnType;
+ * \ingroup Core_Module
+ *
+ */
+template <typename Derived>
+class ReturnByValue : public internal::dense_xpr_base<ReturnByValue<Derived> >::type, internal::no_assignment_operator {
+ public:
+  typedef typename internal::traits<Derived>::ReturnType ReturnType;
 
-    typedef typename internal::dense_xpr_base<ReturnByValue>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(ReturnByValue)
+  typedef typename internal::dense_xpr_base<ReturnByValue>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(ReturnByValue)
 
-    template<typename Dest>
-    EIGEN_DEVICE_FUNC
-    inline void evalTo(Dest& dst) const
-    { static_cast<const Derived*>(this)->evalTo(dst); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return static_cast<const Derived*>(this)->rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return static_cast<const Derived*>(this)->cols(); }
+  template <typename Dest>
+  EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const {
+    static_cast<const Derived*>(this)->evalTo(dst);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT {
+    return static_cast<const Derived*>(this)->rows();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT {
+    return static_cast<const Derived*>(this)->cols();
+  }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-#define Unusable YOU_ARE_TRYING_TO_ACCESS_A_SINGLE_COEFFICIENT_IN_A_SPECIAL_EXPRESSION_WHERE_THAT_IS_NOT_ALLOWED_BECAUSE_THAT_WOULD_BE_INEFFICIENT
-    class Unusable{
-      Unusable(const Unusable&) {}
-      Unusable& operator=(const Unusable&) {return *this;}
-    };
-    const Unusable& coeff(Index) const { return *reinterpret_cast<const Unusable*>(this); }
-    const Unusable& coeff(Index,Index) const { return *reinterpret_cast<const Unusable*>(this); }
-    Unusable& coeffRef(Index) { return *reinterpret_cast<Unusable*>(this); }
-    Unusable& coeffRef(Index,Index) { return *reinterpret_cast<Unusable*>(this); }
+#define Unusable \
+  YOU_ARE_TRYING_TO_ACCESS_A_SINGLE_COEFFICIENT_IN_A_SPECIAL_EXPRESSION_WHERE_THAT_IS_NOT_ALLOWED_BECAUSE_THAT_WOULD_BE_INEFFICIENT
+  class Unusable {
+    Unusable(const Unusable&) {}
+    Unusable& operator=(const Unusable&) { return *this; }
+  };
+  const Unusable& coeff(Index) const { return *reinterpret_cast<const Unusable*>(this); }
+  const Unusable& coeff(Index, Index) const { return *reinterpret_cast<const Unusable*>(this); }
+  Unusable& coeffRef(Index) { return *reinterpret_cast<Unusable*>(this); }
+  Unusable& coeffRef(Index, Index) { return *reinterpret_cast<Unusable*>(this); }
 #undef Unusable
 #endif
 };
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC Derived& DenseBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
-{
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC Derived& DenseBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other) {
   other.evalTo(derived());
   return derived();
 }
@@ -93,27 +93,23 @@
 // when a ReturnByValue expression is assigned, the evaluator is not constructed.
 // TODO: Finalize port to new regime; ReturnByValue should not exist in the expression world
 
-template<typename Derived>
-struct evaluator<ReturnByValue<Derived> >
-  : public evaluator<typename internal::traits<Derived>::ReturnType>
-{
+template <typename Derived>
+struct evaluator<ReturnByValue<Derived> > : public evaluator<typename internal::traits<Derived>::ReturnType> {
   typedef ReturnByValue<Derived> XprType;
   typedef typename internal::traits<Derived>::ReturnType PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr)
-    : m_result(xpr.rows(), xpr.cols())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : m_result(xpr.rows(), xpr.cols()) {
+    internal::construct_at<Base>(this, m_result);
     xpr.evalTo(m_result);
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_RETURNBYVALUE_H
+#endif  // EIGEN_RETURNBYVALUE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h
index 28cdd76..66116aa 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h
@@ -12,151 +12,133 @@
 #ifndef EIGEN_REVERSE_H
 #define EIGEN_REVERSE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename MatrixType, int Direction>
-struct traits<Reverse<MatrixType, Direction> >
- : traits<MatrixType>
-{
+template <typename MatrixType, int Direction>
+struct traits<Reverse<MatrixType, Direction> > : traits<MatrixType> {
   typedef typename MatrixType::Scalar Scalar;
   typedef typename traits<MatrixType>::StorageKind StorageKind;
   typedef typename traits<MatrixType>::XprKind XprKind;
   typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested;
+  typedef std::remove_reference_t<MatrixTypeNested> MatrixTypeNested_;
   enum {
     RowsAtCompileTime = MatrixType::RowsAtCompileTime,
     ColsAtCompileTime = MatrixType::ColsAtCompileTime,
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    Flags = _MatrixTypeNested::Flags & (RowMajorBit | LvalueBit)
+    Flags = MatrixTypeNested_::Flags & (RowMajorBit | LvalueBit)
   };
 };
 
-template<typename PacketType, bool ReversePacket> struct reverse_packet_cond
-{
+template <typename PacketType, bool ReversePacket>
+struct reverse_packet_cond {
   static inline PacketType run(const PacketType& x) { return preverse(x); }
 };
 
-template<typename PacketType> struct reverse_packet_cond<PacketType,false>
-{
+template <typename PacketType>
+struct reverse_packet_cond<PacketType, false> {
   static inline PacketType run(const PacketType& x) { return x; }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \class Reverse
-  * \ingroup Core_Module
-  *
-  * \brief Expression of the reverse of a vector or matrix
-  *
-  * \tparam MatrixType the type of the object of which we are taking the reverse
-  * \tparam Direction defines the direction of the reverse operation, can be Vertical, Horizontal, or BothDirections
-  *
-  * This class represents an expression of the reverse of a vector.
-  * It is the return type of MatrixBase::reverse() and VectorwiseOp::reverse()
-  * and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::reverse(), VectorwiseOp::reverse()
-  */
-template<typename MatrixType, int Direction> class Reverse
-  : public internal::dense_xpr_base< Reverse<MatrixType, Direction> >::type
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Expression of the reverse of a vector or matrix
+ *
+ * \tparam MatrixType the type of the object of which we are taking the reverse
+ * \tparam Direction defines the direction of the reverse operation, can be Vertical, Horizontal, or BothDirections
+ *
+ * This class represents an expression of the reverse of a vector.
+ * It is the return type of MatrixBase::reverse() and VectorwiseOp::reverse()
+ * and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::reverse(), VectorwiseOp::reverse()
+ */
+template <typename MatrixType, int Direction>
+class Reverse : public internal::dense_xpr_base<Reverse<MatrixType, Direction> >::type {
+ public:
+  typedef typename internal::dense_xpr_base<Reverse>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Reverse)
+  typedef internal::remove_all_t<MatrixType> NestedExpression;
+  using Base::IsRowMajor;
 
-    typedef typename internal::dense_xpr_base<Reverse>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Reverse)
-    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
-    using Base::IsRowMajor;
+ protected:
+  enum {
+    PacketSize = internal::packet_traits<Scalar>::size,
+    IsColMajor = !IsRowMajor,
+    ReverseRow = (Direction == Vertical) || (Direction == BothDirections),
+    ReverseCol = (Direction == Horizontal) || (Direction == BothDirections),
+    OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1,
+    OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1,
+    ReversePacket = (Direction == BothDirections) || ((Direction == Vertical) && IsColMajor) ||
+                    ((Direction == Horizontal) && IsRowMajor)
+  };
+  typedef internal::reverse_packet_cond<PacketScalar, ReversePacket> reverse_packet;
 
-  protected:
-    enum {
-      PacketSize = internal::packet_traits<Scalar>::size,
-      IsColMajor = !IsRowMajor,
-      ReverseRow = (Direction == Vertical)   || (Direction == BothDirections),
-      ReverseCol = (Direction == Horizontal) || (Direction == BothDirections),
-      OffsetRow  = ReverseRow && IsColMajor ? PacketSize : 1,
-      OffsetCol  = ReverseCol && IsRowMajor ? PacketSize : 1,
-      ReversePacket = (Direction == BothDirections)
-                    || ((Direction == Vertical)   && IsColMajor)
-                    || ((Direction == Horizontal) && IsRowMajor)
-    };
-    typedef internal::reverse_packet_cond<PacketScalar,ReversePacket> reverse_packet;
-  public:
+ public:
+  EIGEN_DEVICE_FUNC explicit inline Reverse(const MatrixType& matrix) : m_matrix(matrix) {}
 
-    EIGEN_DEVICE_FUNC explicit inline Reverse(const MatrixType& matrix) : m_matrix(matrix) { }
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reverse)
 
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reverse)
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  EIGEN_DEVICE_FUNC inline Index innerStride() const { return -m_matrix.innerStride(); }
 
-    EIGEN_DEVICE_FUNC inline Index innerStride() const
-    {
-      return -m_matrix.innerStride();
-    }
+  EIGEN_DEVICE_FUNC const internal::remove_all_t<typename MatrixType::Nested>& nestedExpression() const {
+    return m_matrix;
+  }
 
-    EIGEN_DEVICE_FUNC const typename internal::remove_all<typename MatrixType::Nested>::type&
-    nestedExpression() const
-    {
-      return m_matrix;
-    }
-
-  protected:
-    typename MatrixType::Nested m_matrix;
+ protected:
+  typename MatrixType::Nested m_matrix;
 };
 
 /** \returns an expression of the reverse of *this.
-  *
-  * Example: \include MatrixBase_reverse.cpp
-  * Output: \verbinclude MatrixBase_reverse.out
-  *
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::ReverseReturnType
-DenseBase<Derived>::reverse()
-{
+ *
+ * Example: \include MatrixBase_reverse.cpp
+ * Output: \verbinclude MatrixBase_reverse.out
+ *
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::ReverseReturnType DenseBase<Derived>::reverse() {
   return ReverseReturnType(derived());
 }
 
-
-//reverse const overload moved DenseBase.h due to a CUDA compiler bug
+// reverse const overload moved DenseBase.h due to a CUDA compiler bug
 
 /** This is the "in place" version of reverse: it reverses \c *this.
-  *
-  * In most cases it is probably better to simply use the reversed expression
-  * of a matrix. However, when reversing the matrix data itself is really needed,
-  * then this "in-place" version is probably the right choice because it provides
-  * the following additional benefits:
-  *  - less error prone: doing the same operation with .reverse() requires special care:
-  *    \code m = m.reverse().eval(); \endcode
-  *  - this API enables reverse operations without the need for a temporary
-  *  - it allows future optimizations (cache friendliness, etc.)
-  *
-  * \sa VectorwiseOp::reverseInPlace(), reverse() */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline void DenseBase<Derived>::reverseInPlace()
-{
-  if(cols()>rows())
-  {
-    Index half = cols()/2;
+ *
+ * In most cases it is probably better to simply use the reversed expression
+ * of a matrix. However, when reversing the matrix data itself is really needed,
+ * then this "in-place" version is probably the right choice because it provides
+ * the following additional benefits:
+ *  - less error prone: doing the same operation with .reverse() requires special care:
+ *    \code m = m.reverse().eval(); \endcode
+ *  - this API enables reverse operations without the need for a temporary
+ *  - it allows future optimizations (cache friendliness, etc.)
+ *
+ * \sa VectorwiseOp::reverseInPlace(), reverse() */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline void DenseBase<Derived>::reverseInPlace() {
+  if (cols() > rows()) {
+    Index half = cols() / 2;
     leftCols(half).swap(rightCols(half).reverse());
-    if((cols()%2)==1)
-    {
-      Index half2 = rows()/2;
+    if ((cols() % 2) == 1) {
+      Index half2 = rows() / 2;
       col(half).head(half2).swap(col(half).tail(half2).reverse());
     }
-  }
-  else
-  {
-    Index half = rows()/2;
+  } else {
+    Index half = rows() / 2;
     topRows(half).swap(bottomRows(half).reverse());
-    if((rows()%2)==1)
-    {
-      Index half2 = cols()/2;
+    if ((rows() % 2) == 1) {
+      Index half2 = cols() / 2;
       row(half).head(half2).swap(row(half).tail(half2).reverse());
     }
   }
@@ -164,54 +146,51 @@
 
 namespace internal {
 
-template<int Direction>
+template <int Direction>
 struct vectorwise_reverse_inplace_impl;
 
-template<>
-struct vectorwise_reverse_inplace_impl<Vertical>
-{
-  template<typename ExpressionType>
-  static void run(ExpressionType &xpr)
-  {
-    const int HalfAtCompileTime = ExpressionType::RowsAtCompileTime==Dynamic?Dynamic:ExpressionType::RowsAtCompileTime/2;
-    Index half = xpr.rows()/2;
-    xpr.topRows(fix<HalfAtCompileTime>(half))
-       .swap(xpr.bottomRows(fix<HalfAtCompileTime>(half)).colwise().reverse());
+template <>
+struct vectorwise_reverse_inplace_impl<Vertical> {
+  template <typename ExpressionType>
+  static void run(ExpressionType& xpr) {
+    constexpr Index HalfAtCompileTime =
+        ExpressionType::RowsAtCompileTime == Dynamic ? Dynamic : ExpressionType::RowsAtCompileTime / 2;
+    Index half = xpr.rows() / 2;
+    xpr.template topRows<HalfAtCompileTime>(half).swap(
+        xpr.template bottomRows<HalfAtCompileTime>(half).colwise().reverse());
   }
 };
 
-template<>
-struct vectorwise_reverse_inplace_impl<Horizontal>
-{
-  template<typename ExpressionType>
-  static void run(ExpressionType &xpr)
-  {
-    const int HalfAtCompileTime = ExpressionType::ColsAtCompileTime==Dynamic?Dynamic:ExpressionType::ColsAtCompileTime/2;
-    Index half = xpr.cols()/2;
-    xpr.leftCols(fix<HalfAtCompileTime>(half))
-       .swap(xpr.rightCols(fix<HalfAtCompileTime>(half)).rowwise().reverse());
+template <>
+struct vectorwise_reverse_inplace_impl<Horizontal> {
+  template <typename ExpressionType>
+  static void run(ExpressionType& xpr) {
+    constexpr Index HalfAtCompileTime =
+        ExpressionType::ColsAtCompileTime == Dynamic ? Dynamic : ExpressionType::ColsAtCompileTime / 2;
+    Index half = xpr.cols() / 2;
+    xpr.template leftCols<HalfAtCompileTime>(half).swap(
+        xpr.template rightCols<HalfAtCompileTime>(half).rowwise().reverse());
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** This is the "in place" version of VectorwiseOp::reverse: it reverses each column or row of \c *this.
-  *
-  * In most cases it is probably better to simply use the reversed expression
-  * of a matrix. However, when reversing the matrix data itself is really needed,
-  * then this "in-place" version is probably the right choice because it provides
-  * the following additional benefits:
-  *  - less error prone: doing the same operation with .reverse() requires special care:
-  *    \code m = m.reverse().eval(); \endcode
-  *  - this API enables reverse operations without the need for a temporary
-  *
-  * \sa DenseBase::reverseInPlace(), reverse() */
-template<typename ExpressionType, int Direction>
-EIGEN_DEVICE_FUNC void VectorwiseOp<ExpressionType,Direction>::reverseInPlace()
-{
+ *
+ * In most cases it is probably better to simply use the reversed expression
+ * of a matrix. However, when reversing the matrix data itself is really needed,
+ * then this "in-place" version is probably the right choice because it provides
+ * the following additional benefits:
+ *  - less error prone: doing the same operation with .reverse() requires special care:
+ *    \code m = m.reverse().eval(); \endcode
+ *  - this API enables reverse operations without the need for a temporary
+ *
+ * \sa DenseBase::reverseInPlace(), reverse() */
+template <typename ExpressionType, int Direction>
+EIGEN_DEVICE_FUNC void VectorwiseOp<ExpressionType, Direction>::reverseInPlace() {
   internal::vectorwise_reverse_inplace_impl<Direction>::run(m_matrix);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_REVERSE_H
+#endif  // EIGEN_REVERSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
index 7c86bf8..9f46120 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
@@ -10,28 +10,29 @@
 #ifndef EIGEN_SELECT_H
 #define EIGEN_SELECT_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class Select
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a coefficient wise version of the C++ ternary operator ?:
-  *
-  * \param ConditionMatrixType the type of the \em condition expression which must be a boolean matrix
-  * \param ThenMatrixType the type of the \em then expression
-  * \param ElseMatrixType the type of the \em else expression
-  *
-  * This class represents an expression of a coefficient wise version of the C++ ternary operator ?:.
-  * It is the return type of DenseBase::select() and most of the time this is the only way it is used.
-  *
-  * \sa DenseBase::select(const DenseBase<ThenDerived>&, const DenseBase<ElseDerived>&) const
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a coefficient wise version of the C++ ternary operator ?:
+ *
+ * \tparam ConditionMatrixType the type of the \em condition expression which must be a boolean matrix
+ * \tparam ThenMatrixType the type of the \em then expression
+ * \tparam ElseMatrixType the type of the \em else expression
+ *
+ * This class represents an expression of a coefficient wise version of the C++ ternary operator ?:.
+ * It is the return type of DenseBase::select() and most of the time this is the only way it is used.
+ *
+ * \sa DenseBase::select(const DenseBase<ThenDerived>&, const DenseBase<ElseDerived>&) const
+ */
 
 namespace internal {
-template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
-struct traits<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >
- : traits<ThenMatrixType>
-{
+template <typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
+struct traits<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> > : traits<ThenMatrixType> {
   typedef typename traits<ThenMatrixType>::Scalar Scalar;
   typedef Dense StorageKind;
   typedef typename traits<ThenMatrixType>::XprKind XprKind;
@@ -46,119 +47,110 @@
     Flags = (unsigned int)ThenMatrixType::Flags & ElseMatrixType::Flags & RowMajorBit
   };
 };
-}
+}  // namespace internal
 
-template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
-class Select : public internal::dense_xpr_base< Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >::type,
-               internal::no_assignment_operator
-{
-  public:
+template <typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
+class Select : public internal::dense_xpr_base<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >::type,
+               internal::no_assignment_operator {
+ public:
+  typedef typename internal::dense_xpr_base<Select>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Select)
 
-    typedef typename internal::dense_xpr_base<Select>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Select)
+  inline EIGEN_DEVICE_FUNC Select(const ConditionMatrixType& a_conditionMatrix, const ThenMatrixType& a_thenMatrix,
+                                  const ElseMatrixType& a_elseMatrix)
+      : m_condition(a_conditionMatrix), m_then(a_thenMatrix), m_else(a_elseMatrix) {
+    eigen_assert(m_condition.rows() == m_then.rows() && m_condition.rows() == m_else.rows());
+    eigen_assert(m_condition.cols() == m_then.cols() && m_condition.cols() == m_else.cols());
+  }
 
-    inline EIGEN_DEVICE_FUNC
-    Select(const ConditionMatrixType& a_conditionMatrix,
-           const ThenMatrixType& a_thenMatrix,
-           const ElseMatrixType& a_elseMatrix)
-      : m_condition(a_conditionMatrix), m_then(a_thenMatrix), m_else(a_elseMatrix)
-    {
-      eigen_assert(m_condition.rows() == m_then.rows() && m_condition.rows() == m_else.rows());
-      eigen_assert(m_condition.cols() == m_then.cols() && m_condition.cols() == m_else.cols());
-    }
+  inline EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_condition.rows(); }
+  inline EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_condition.cols(); }
 
-    inline EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return m_condition.rows(); }
-    inline EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return m_condition.cols(); }
+  inline EIGEN_DEVICE_FUNC const Scalar coeff(Index i, Index j) const {
+    if (m_condition.coeff(i, j))
+      return m_then.coeff(i, j);
+    else
+      return m_else.coeff(i, j);
+  }
 
-    inline EIGEN_DEVICE_FUNC
-    const Scalar coeff(Index i, Index j) const
-    {
-      if (m_condition.coeff(i,j))
-        return m_then.coeff(i,j);
-      else
-        return m_else.coeff(i,j);
-    }
+  inline EIGEN_DEVICE_FUNC const Scalar coeff(Index i) const {
+    if (m_condition.coeff(i))
+      return m_then.coeff(i);
+    else
+      return m_else.coeff(i);
+  }
 
-    inline EIGEN_DEVICE_FUNC
-    const Scalar coeff(Index i) const
-    {
-      if (m_condition.coeff(i))
-        return m_then.coeff(i);
-      else
-        return m_else.coeff(i);
-    }
+  inline EIGEN_DEVICE_FUNC const ConditionMatrixType& conditionMatrix() const { return m_condition; }
 
-    inline EIGEN_DEVICE_FUNC const ConditionMatrixType& conditionMatrix() const
-    {
-      return m_condition;
-    }
+  inline EIGEN_DEVICE_FUNC const ThenMatrixType& thenMatrix() const { return m_then; }
 
-    inline EIGEN_DEVICE_FUNC const ThenMatrixType& thenMatrix() const
-    {
-      return m_then;
-    }
+  inline EIGEN_DEVICE_FUNC const ElseMatrixType& elseMatrix() const { return m_else; }
 
-    inline EIGEN_DEVICE_FUNC const ElseMatrixType& elseMatrix() const
-    {
-      return m_else;
-    }
-
-  protected:
-    typename ConditionMatrixType::Nested m_condition;
-    typename ThenMatrixType::Nested m_then;
-    typename ElseMatrixType::Nested m_else;
+ protected:
+  typename ConditionMatrixType::Nested m_condition;
+  typename ThenMatrixType::Nested m_then;
+  typename ElseMatrixType::Nested m_else;
 };
 
-
 /** \returns a matrix where each coefficient (i,j) is equal to \a thenMatrix(i,j)
-  * if \c *this(i,j), and \a elseMatrix(i,j) otherwise.
-  *
-  * Example: \include MatrixBase_select.cpp
-  * Output: \verbinclude MatrixBase_select.out
-  *
-  * \sa class Select
-  */
-template<typename Derived>
-template<typename ThenDerived,typename ElseDerived>
-inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived,ElseDerived>
-DenseBase<Derived>::select(const DenseBase<ThenDerived>& thenMatrix,
-                            const DenseBase<ElseDerived>& elseMatrix) const
-{
-  return Select<Derived,ThenDerived,ElseDerived>(derived(), thenMatrix.derived(), elseMatrix.derived());
+ * if \c *this(i,j) != Scalar(0), and \a elseMatrix(i,j) otherwise.
+ *
+ * Example: \include MatrixBase_select.cpp
+ * Output: \verbinclude MatrixBase_select.out
+ *
+ * \sa DenseBase::bitwiseSelect(const DenseBase<ThenDerived>&, const DenseBase<ElseDerived>&)
+ */
+template <typename Derived>
+template <typename ThenDerived, typename ElseDerived>
+inline EIGEN_DEVICE_FUNC CwiseTernaryOp<
+    internal::scalar_boolean_select_op<typename DenseBase<ThenDerived>::Scalar, typename DenseBase<ElseDerived>::Scalar,
+                                       typename DenseBase<Derived>::Scalar>,
+    ThenDerived, ElseDerived, Derived>
+DenseBase<Derived>::select(const DenseBase<ThenDerived>& thenMatrix, const DenseBase<ElseDerived>& elseMatrix) const {
+  using Op = internal::scalar_boolean_select_op<typename DenseBase<ThenDerived>::Scalar,
+                                                typename DenseBase<ElseDerived>::Scalar, Scalar>;
+  return CwiseTernaryOp<Op, ThenDerived, ElseDerived, Derived>(thenMatrix.derived(), elseMatrix.derived(), derived(),
+                                                               Op());
 }
-
 /** Version of DenseBase::select(const DenseBase&, const DenseBase&) with
-  * the \em else expression being a scalar value.
-  *
-  * \sa DenseBase::select(const DenseBase<ThenDerived>&, const DenseBase<ElseDerived>&) const, class Select
-  */
-template<typename Derived>
-template<typename ThenDerived>
-inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
+ * the \em else expression being a scalar value.
+ *
+ * \sa DenseBase::booleanSelect(const DenseBase<ThenDerived>&, const DenseBase<ElseDerived>&) const, class Select
+ */
+template <typename Derived>
+template <typename ThenDerived>
+inline EIGEN_DEVICE_FUNC CwiseTernaryOp<
+    internal::scalar_boolean_select_op<typename DenseBase<ThenDerived>::Scalar, typename DenseBase<ThenDerived>::Scalar,
+                                       typename DenseBase<Derived>::Scalar>,
+    ThenDerived, typename DenseBase<ThenDerived>::ConstantReturnType, Derived>
 DenseBase<Derived>::select(const DenseBase<ThenDerived>& thenMatrix,
-                           const typename ThenDerived::Scalar& elseScalar) const
-{
-  return Select<Derived,ThenDerived,typename ThenDerived::ConstantReturnType>(
-    derived(), thenMatrix.derived(), ThenDerived::Constant(rows(),cols(),elseScalar));
+                           const typename DenseBase<ThenDerived>::Scalar& elseScalar) const {
+  using ElseConstantType = typename DenseBase<ThenDerived>::ConstantReturnType;
+  using Op = internal::scalar_boolean_select_op<typename DenseBase<ThenDerived>::Scalar,
+                                                typename DenseBase<ThenDerived>::Scalar, Scalar>;
+  return CwiseTernaryOp<Op, ThenDerived, ElseConstantType, Derived>(
+      thenMatrix.derived(), ElseConstantType(rows(), cols(), elseScalar), derived(), Op());
 }
-
 /** Version of DenseBase::select(const DenseBase&, const DenseBase&) with
-  * the \em then expression being a scalar value.
-  *
-  * \sa DenseBase::select(const DenseBase<ThenDerived>&, const DenseBase<ElseDerived>&) const, class Select
-  */
-template<typename Derived>
-template<typename ElseDerived>
-inline EIGEN_DEVICE_FUNC const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
-DenseBase<Derived>::select(const typename ElseDerived::Scalar& thenScalar,
-                           const DenseBase<ElseDerived>& elseMatrix) const
-{
-  return Select<Derived,typename ElseDerived::ConstantReturnType,ElseDerived>(
-    derived(), ElseDerived::Constant(rows(),cols(),thenScalar), elseMatrix.derived());
+ * the \em then expression being a scalar value.
+ *
+ * \sa DenseBase::booleanSelect(const DenseBase<ThenDerived>&, const DenseBase<ElseDerived>&) const, class Select
+ */
+template <typename Derived>
+template <typename ElseDerived>
+inline EIGEN_DEVICE_FUNC CwiseTernaryOp<
+    internal::scalar_boolean_select_op<typename DenseBase<ElseDerived>::Scalar, typename DenseBase<ElseDerived>::Scalar,
+                                       typename DenseBase<Derived>::Scalar>,
+    typename DenseBase<ElseDerived>::ConstantReturnType, ElseDerived, Derived>
+DenseBase<Derived>::select(const typename DenseBase<ElseDerived>::Scalar& thenScalar,
+                           const DenseBase<ElseDerived>& elseMatrix) const {
+  using ThenConstantType = typename DenseBase<ElseDerived>::ConstantReturnType;
+  using Op = internal::scalar_boolean_select_op<typename DenseBase<ElseDerived>::Scalar,
+                                                typename DenseBase<ElseDerived>::Scalar, Scalar>;
+  return CwiseTernaryOp<Op, ThenConstantType, ElseDerived, Derived>(ThenConstantType(rows(), cols(), thenScalar),
+                                                                    elseMatrix.derived(), derived(), Op());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELECT_H
+#endif  // EIGEN_SELECT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h
index 8ce3b37..4e9a923 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h
@@ -10,268 +10,238 @@
 #ifndef EIGEN_SELFADJOINTMATRIX_H
 #define EIGEN_SELFADJOINTMATRIX_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class SelfAdjointView
-  * \ingroup Core_Module
-  *
-  *
-  * \brief Expression of a selfadjoint matrix from a triangular part of a dense matrix
-  *
-  * \param MatrixType the type of the dense matrix storing the coefficients
-  * \param TriangularPart can be either \c #Lower or \c #Upper
-  *
-  * This class is an expression of a sefladjoint matrix from a triangular part of a matrix
-  * with given dense storage of the coefficients. It is the return type of MatrixBase::selfadjointView()
-  * and most of the time this is the only way that it is used.
-  *
-  * \sa class TriangularBase, MatrixBase::selfadjointView()
-  */
+ * \ingroup Core_Module
+ *
+ *
+ * \brief Expression of a selfadjoint matrix from a triangular part of a dense matrix
+ *
+ * \tparam MatrixType the type of the dense matrix storing the coefficients
+ * \tparam TriangularPart can be either \c #Lower or \c #Upper
+ *
+ * This class is an expression of a sefladjoint matrix from a triangular part of a matrix
+ * with given dense storage of the coefficients. It is the return type of MatrixBase::selfadjointView()
+ * and most of the time this is the only way that it is used.
+ *
+ * \sa class TriangularBase, MatrixBase::selfadjointView()
+ */
 
 namespace internal {
-template<typename MatrixType, unsigned int UpLo>
-struct traits<SelfAdjointView<MatrixType, UpLo> > : traits<MatrixType>
-{
+template <typename MatrixType, unsigned int UpLo>
+struct traits<SelfAdjointView<MatrixType, UpLo> > : traits<MatrixType> {
   typedef typename ref_selector<MatrixType>::non_const_type MatrixTypeNested;
-  typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
+  typedef remove_all_t<MatrixTypeNested> MatrixTypeNestedCleaned;
   typedef MatrixType ExpressionType;
   typedef typename MatrixType::PlainObject FullMatrixType;
   enum {
     Mode = UpLo | SelfAdjoint,
     FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
-    Flags =  MatrixTypeNestedCleaned::Flags & (HereditaryBits|FlagsLvalueBit)
-           & (~(PacketAccessBit | DirectAccessBit | LinearAccessBit)) // FIXME these flags should be preserved
+    Flags = MatrixTypeNestedCleaned::Flags & (HereditaryBits | FlagsLvalueBit) &
+            (~(PacketAccessBit | DirectAccessBit | LinearAccessBit))  // FIXME these flags should be preserved
   };
 };
-}
+}  // namespace internal
 
+template <typename MatrixType_, unsigned int UpLo>
+class SelfAdjointView : public TriangularBase<SelfAdjointView<MatrixType_, UpLo> > {
+ public:
+  EIGEN_STATIC_ASSERT(UpLo == Lower || UpLo == Upper, SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY)
 
-template<typename _MatrixType, unsigned int UpLo> class SelfAdjointView
-  : public TriangularBase<SelfAdjointView<_MatrixType, UpLo> >
-{
-  public:
+  typedef MatrixType_ MatrixType;
+  typedef TriangularBase<SelfAdjointView> Base;
+  typedef typename internal::traits<SelfAdjointView>::MatrixTypeNested MatrixTypeNested;
+  typedef typename internal::traits<SelfAdjointView>::MatrixTypeNestedCleaned MatrixTypeNestedCleaned;
+  typedef MatrixTypeNestedCleaned NestedExpression;
 
-    typedef _MatrixType MatrixType;
-    typedef TriangularBase<SelfAdjointView> Base;
-    typedef typename internal::traits<SelfAdjointView>::MatrixTypeNested MatrixTypeNested;
-    typedef typename internal::traits<SelfAdjointView>::MatrixTypeNestedCleaned MatrixTypeNestedCleaned;
-    typedef MatrixTypeNestedCleaned NestedExpression;
+  /** \brief The type of coefficients in this matrix */
+  typedef typename internal::traits<SelfAdjointView>::Scalar Scalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef internal::remove_all_t<typename MatrixType::ConjugateReturnType> MatrixConjugateReturnType;
+  typedef SelfAdjointView<std::add_const_t<MatrixType>, UpLo> ConstSelfAdjointView;
 
-    /** \brief The type of coefficients in this matrix */
-    typedef typename internal::traits<SelfAdjointView>::Scalar Scalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef typename internal::remove_all<typename MatrixType::ConjugateReturnType>::type MatrixConjugateReturnType;
-    typedef SelfAdjointView<typename internal::add_const<MatrixType>::type, UpLo> ConstSelfAdjointView;
+  enum {
+    Mode = internal::traits<SelfAdjointView>::Mode,
+    Flags = internal::traits<SelfAdjointView>::Flags,
+    TransposeMode = ((int(Mode) & int(Upper)) ? Lower : 0) | ((int(Mode) & int(Lower)) ? Upper : 0)
+  };
+  typedef typename MatrixType::PlainObject PlainObject;
 
-    enum {
-      Mode = internal::traits<SelfAdjointView>::Mode,
-      Flags = internal::traits<SelfAdjointView>::Flags,
-      TransposeMode = ((int(Mode) & int(Upper)) ? Lower : 0) | ((int(Mode) & int(Lower)) ? Upper : 0)
-    };
-    typedef typename MatrixType::PlainObject PlainObject;
+  EIGEN_DEVICE_FUNC explicit inline SelfAdjointView(MatrixType& matrix) : m_matrix(matrix) {}
 
-    EIGEN_DEVICE_FUNC
-    explicit inline SelfAdjointView(MatrixType& matrix) : m_matrix(matrix)
-    {
-      EIGEN_STATIC_ASSERT(UpLo==Lower || UpLo==Upper,SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return m_matrix.outerStride(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return m_matrix.innerStride(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return m_matrix.outerStride(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT { return m_matrix.innerStride(); }
+  /** \sa MatrixBase::coeff()
+   * \warning the coordinates must fit into the referenced triangular part
+   */
+  EIGEN_DEVICE_FUNC inline Scalar coeff(Index row, Index col) const {
+    Base::check_coordinates_internal(row, col);
+    return m_matrix.coeff(row, col);
+  }
 
-    /** \sa MatrixBase::coeff()
-      * \warning the coordinates must fit into the referenced triangular part
-      */
-    EIGEN_DEVICE_FUNC
-    inline Scalar coeff(Index row, Index col) const
-    {
-      Base::check_coordinates_internal(row, col);
-      return m_matrix.coeff(row, col);
-    }
+  /** \sa MatrixBase::coeffRef()
+   * \warning the coordinates must fit into the referenced triangular part
+   */
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) {
+    EIGEN_STATIC_ASSERT_LVALUE(SelfAdjointView);
+    Base::check_coordinates_internal(row, col);
+    return m_matrix.coeffRef(row, col);
+  }
 
-    /** \sa MatrixBase::coeffRef()
-      * \warning the coordinates must fit into the referenced triangular part
-      */
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(SelfAdjointView);
-      Base::check_coordinates_internal(row, col);
-      return m_matrix.coeffRef(row, col);
-    }
+  /** \internal */
+  EIGEN_DEVICE_FUNC const MatrixTypeNestedCleaned& _expression() const { return m_matrix; }
 
-    /** \internal */
-    EIGEN_DEVICE_FUNC
-    const MatrixTypeNestedCleaned& _expression() const { return m_matrix; }
+  EIGEN_DEVICE_FUNC const MatrixTypeNestedCleaned& nestedExpression() const { return m_matrix; }
+  EIGEN_DEVICE_FUNC MatrixTypeNestedCleaned& nestedExpression() { return m_matrix; }
 
-    EIGEN_DEVICE_FUNC
-    const MatrixTypeNestedCleaned& nestedExpression() const { return m_matrix; }
-    EIGEN_DEVICE_FUNC
-    MatrixTypeNestedCleaned& nestedExpression() { return m_matrix; }
+  /** Efficient triangular matrix times vector/matrix product */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC const Product<SelfAdjointView, OtherDerived> operator*(const MatrixBase<OtherDerived>& rhs) const {
+    return Product<SelfAdjointView, OtherDerived>(*this, rhs.derived());
+  }
 
-    /** Efficient triangular matrix times vector/matrix product */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<SelfAdjointView,OtherDerived>
-    operator*(const MatrixBase<OtherDerived>& rhs) const
-    {
-      return Product<SelfAdjointView,OtherDerived>(*this, rhs.derived());
-    }
+  /** Efficient vector/matrix times triangular matrix product */
+  template <typename OtherDerived>
+  friend EIGEN_DEVICE_FUNC const Product<OtherDerived, SelfAdjointView> operator*(const MatrixBase<OtherDerived>& lhs,
+                                                                                  const SelfAdjointView& rhs) {
+    return Product<OtherDerived, SelfAdjointView>(lhs.derived(), rhs);
+  }
 
-    /** Efficient vector/matrix times triangular matrix product */
-    template<typename OtherDerived> friend
-    EIGEN_DEVICE_FUNC
-    const Product<OtherDerived,SelfAdjointView>
-    operator*(const MatrixBase<OtherDerived>& lhs, const SelfAdjointView& rhs)
-    {
-      return Product<OtherDerived,SelfAdjointView>(lhs.derived(),rhs);
-    }
+  friend EIGEN_DEVICE_FUNC const
+      SelfAdjointView<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar, MatrixType, product), UpLo>
+      operator*(const Scalar& s, const SelfAdjointView& mat) {
+    return (s * mat.nestedExpression()).template selfadjointView<UpLo>();
+  }
 
-    friend EIGEN_DEVICE_FUNC
-    const SelfAdjointView<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,MatrixType,product),UpLo>
-    operator*(const Scalar& s, const SelfAdjointView& mat)
-    {
-      return (s*mat.nestedExpression()).template selfadjointView<UpLo>();
-    }
+  /** Perform a symmetric rank 2 update of the selfadjoint matrix \c *this:
+   * \f$ this = this + \alpha u v^* + conj(\alpha) v u^* \f$
+   * \returns a reference to \c *this
+   *
+   * The vectors \a u and \c v \b must be column vectors, however they can be
+   * a adjoint expression without any overhead. Only the meaningful triangular
+   * part of the matrix is updated, the rest is left unchanged.
+   *
+   * \sa rankUpdate(const MatrixBase<DerivedU>&, Scalar)
+   */
+  template <typename DerivedU, typename DerivedV>
+  EIGEN_DEVICE_FUNC SelfAdjointView& rankUpdate(const MatrixBase<DerivedU>& u, const MatrixBase<DerivedV>& v,
+                                                const Scalar& alpha = Scalar(1));
 
-    /** Perform a symmetric rank 2 update of the selfadjoint matrix \c *this:
-      * \f$ this = this + \alpha u v^* + conj(\alpha) v u^* \f$
-      * \returns a reference to \c *this
-      *
-      * The vectors \a u and \c v \b must be column vectors, however they can be
-      * a adjoint expression without any overhead. Only the meaningful triangular
-      * part of the matrix is updated, the rest is left unchanged.
-      *
-      * \sa rankUpdate(const MatrixBase<DerivedU>&, Scalar)
-      */
-    template<typename DerivedU, typename DerivedV>
-    EIGEN_DEVICE_FUNC
-    SelfAdjointView& rankUpdate(const MatrixBase<DerivedU>& u, const MatrixBase<DerivedV>& v, const Scalar& alpha = Scalar(1));
+  /** Perform a symmetric rank K update of the selfadjoint matrix \c *this:
+   * \f$ this = this + \alpha ( u u^* ) \f$ where \a u is a vector or matrix.
+   *
+   * \returns a reference to \c *this
+   *
+   * Note that to perform \f$ this = this + \alpha ( u^* u ) \f$ you can simply
+   * call this function with u.adjoint().
+   *
+   * \sa rankUpdate(const MatrixBase<DerivedU>&, const MatrixBase<DerivedV>&, Scalar)
+   */
+  template <typename DerivedU>
+  EIGEN_DEVICE_FUNC SelfAdjointView& rankUpdate(const MatrixBase<DerivedU>& u, const Scalar& alpha = Scalar(1));
 
-    /** Perform a symmetric rank K update of the selfadjoint matrix \c *this:
-      * \f$ this = this + \alpha ( u u^* ) \f$ where \a u is a vector or matrix.
-      *
-      * \returns a reference to \c *this
-      *
-      * Note that to perform \f$ this = this + \alpha ( u^* u ) \f$ you can simply
-      * call this function with u.adjoint().
-      *
-      * \sa rankUpdate(const MatrixBase<DerivedU>&, const MatrixBase<DerivedV>&, Scalar)
-      */
-    template<typename DerivedU>
-    EIGEN_DEVICE_FUNC
-    SelfAdjointView& rankUpdate(const MatrixBase<DerivedU>& u, const Scalar& alpha = Scalar(1));
+  /** \returns an expression of a triangular view extracted from the current selfadjoint view of a given triangular part
+   *
+   * The parameter \a TriMode can have the following values: \c #Upper, \c #StrictlyUpper, \c #UnitUpper,
+   * \c #Lower, \c #StrictlyLower, \c #UnitLower.
+   *
+   * If \c TriMode references the same triangular part than \c *this, then this method simply return a \c TriangularView
+   * of the nested expression, otherwise, the nested expression is first transposed, thus returning a \c
+   * TriangularView<Transpose<MatrixType>> object.
+   *
+   * \sa MatrixBase::triangularView(), class TriangularView
+   */
+  template <unsigned int TriMode>
+  EIGEN_DEVICE_FUNC
+      std::conditional_t<(TriMode & (Upper | Lower)) == (UpLo & (Upper | Lower)), TriangularView<MatrixType, TriMode>,
+                         TriangularView<typename MatrixType::AdjointReturnType, TriMode> >
+      triangularView() const {
+    std::conditional_t<(TriMode & (Upper | Lower)) == (UpLo & (Upper | Lower)), MatrixType&,
+                       typename MatrixType::ConstTransposeReturnType>
+        tmp1(m_matrix);
+    std::conditional_t<(TriMode & (Upper | Lower)) == (UpLo & (Upper | Lower)), MatrixType&,
+                       typename MatrixType::AdjointReturnType>
+        tmp2(tmp1);
+    return std::conditional_t<(TriMode & (Upper | Lower)) == (UpLo & (Upper | Lower)),
+                              TriangularView<MatrixType, TriMode>,
+                              TriangularView<typename MatrixType::AdjointReturnType, TriMode> >(tmp2);
+  }
 
-    /** \returns an expression of a triangular view extracted from the current selfadjoint view of a given triangular part
-      *
-      * The parameter \a TriMode can have the following values: \c #Upper, \c #StrictlyUpper, \c #UnitUpper,
-      * \c #Lower, \c #StrictlyLower, \c #UnitLower.
-      *
-      * If \c TriMode references the same triangular part than \c *this, then this method simply return a \c TriangularView of the nested expression,
-      * otherwise, the nested expression is first transposed, thus returning a \c TriangularView<Transpose<MatrixType>> object.
-      *
-      * \sa MatrixBase::triangularView(), class TriangularView
-      */
-    template<unsigned int TriMode>
-    EIGEN_DEVICE_FUNC
-    typename internal::conditional<(TriMode&(Upper|Lower))==(UpLo&(Upper|Lower)),
-                                   TriangularView<MatrixType,TriMode>,
-                                   TriangularView<typename MatrixType::AdjointReturnType,TriMode> >::type
-    triangularView() const
-    {
-      typename internal::conditional<(TriMode&(Upper|Lower))==(UpLo&(Upper|Lower)), MatrixType&, typename MatrixType::ConstTransposeReturnType>::type tmp1(m_matrix);
-      typename internal::conditional<(TriMode&(Upper|Lower))==(UpLo&(Upper|Lower)), MatrixType&, typename MatrixType::AdjointReturnType>::type tmp2(tmp1);
-      return typename internal::conditional<(TriMode&(Upper|Lower))==(UpLo&(Upper|Lower)),
-                                   TriangularView<MatrixType,TriMode>,
-                                   TriangularView<typename MatrixType::AdjointReturnType,TriMode> >::type(tmp2);
-    }
+  typedef SelfAdjointView<const MatrixConjugateReturnType, UpLo> ConjugateReturnType;
+  /** \sa MatrixBase::conjugate() const */
+  EIGEN_DEVICE_FUNC inline const ConjugateReturnType conjugate() const {
+    return ConjugateReturnType(m_matrix.conjugate());
+  }
 
-    typedef SelfAdjointView<const MatrixConjugateReturnType,UpLo> ConjugateReturnType;
-    /** \sa MatrixBase::conjugate() const */
-    EIGEN_DEVICE_FUNC
-    inline const ConjugateReturnType conjugate() const
-    { return ConjugateReturnType(m_matrix.conjugate()); }
+  /** \returns an expression of the complex conjugate of \c *this if Cond==true,
+   *           returns \c *this otherwise.
+   */
+  template <bool Cond>
+  EIGEN_DEVICE_FUNC inline std::conditional_t<Cond, ConjugateReturnType, ConstSelfAdjointView> conjugateIf() const {
+    typedef std::conditional_t<Cond, ConjugateReturnType, ConstSelfAdjointView> ReturnType;
+    return ReturnType(m_matrix.template conjugateIf<Cond>());
+  }
 
-    /** \returns an expression of the complex conjugate of \c *this if Cond==true,
-     *           returns \c *this otherwise.
-     */
-    template<bool Cond>
-    EIGEN_DEVICE_FUNC
-    inline typename internal::conditional<Cond,ConjugateReturnType,ConstSelfAdjointView>::type
-    conjugateIf() const
-    {
-      typedef typename internal::conditional<Cond,ConjugateReturnType,ConstSelfAdjointView>::type ReturnType;
-      return ReturnType(m_matrix.template conjugateIf<Cond>());
-    }
+  typedef SelfAdjointView<const typename MatrixType::AdjointReturnType, TransposeMode> AdjointReturnType;
+  /** \sa MatrixBase::adjoint() const */
+  EIGEN_DEVICE_FUNC inline const AdjointReturnType adjoint() const { return AdjointReturnType(m_matrix.adjoint()); }
 
-    typedef SelfAdjointView<const typename MatrixType::AdjointReturnType,TransposeMode> AdjointReturnType;
-    /** \sa MatrixBase::adjoint() const */
-    EIGEN_DEVICE_FUNC
-    inline const AdjointReturnType adjoint() const
-    { return AdjointReturnType(m_matrix.adjoint()); }
+  typedef SelfAdjointView<typename MatrixType::TransposeReturnType, TransposeMode> TransposeReturnType;
+  /** \sa MatrixBase::transpose() */
+  template <class Dummy = int>
+  EIGEN_DEVICE_FUNC inline TransposeReturnType transpose(
+      std::enable_if_t<Eigen::internal::is_lvalue<MatrixType>::value, Dummy*> = nullptr) {
+    typename MatrixType::TransposeReturnType tmp(m_matrix);
+    return TransposeReturnType(tmp);
+  }
 
-    typedef SelfAdjointView<typename MatrixType::TransposeReturnType,TransposeMode> TransposeReturnType;
-     /** \sa MatrixBase::transpose() */
-    EIGEN_DEVICE_FUNC
-    inline TransposeReturnType transpose()
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
-      typename MatrixType::TransposeReturnType tmp(m_matrix);
-      return TransposeReturnType(tmp);
-    }
+  typedef SelfAdjointView<const typename MatrixType::ConstTransposeReturnType, TransposeMode> ConstTransposeReturnType;
+  /** \sa MatrixBase::transpose() const */
+  EIGEN_DEVICE_FUNC inline const ConstTransposeReturnType transpose() const {
+    return ConstTransposeReturnType(m_matrix.transpose());
+  }
 
-    typedef SelfAdjointView<const typename MatrixType::ConstTransposeReturnType,TransposeMode> ConstTransposeReturnType;
-    /** \sa MatrixBase::transpose() const */
-    EIGEN_DEVICE_FUNC
-    inline const ConstTransposeReturnType transpose() const
-    {
-      return ConstTransposeReturnType(m_matrix.transpose());
-    }
+  /** \returns a const expression of the main diagonal of the matrix \c *this
+   *
+   * This method simply returns the diagonal of the nested expression, thus by-passing the SelfAdjointView decorator.
+   *
+   * \sa MatrixBase::diagonal(), class Diagonal */
+  EIGEN_DEVICE_FUNC typename MatrixType::ConstDiagonalReturnType diagonal() const {
+    return typename MatrixType::ConstDiagonalReturnType(m_matrix);
+  }
 
-    /** \returns a const expression of the main diagonal of the matrix \c *this
-      *
-      * This method simply returns the diagonal of the nested expression, thus by-passing the SelfAdjointView decorator.
-      *
-      * \sa MatrixBase::diagonal(), class Diagonal */
-    EIGEN_DEVICE_FUNC
-    typename MatrixType::ConstDiagonalReturnType diagonal() const
-    {
-      return typename MatrixType::ConstDiagonalReturnType(m_matrix);
-    }
+  /////////// Cholesky module ///////////
 
-/////////// Cholesky module ///////////
+  const LLT<PlainObject, UpLo> llt() const;
+  const LDLT<PlainObject, UpLo> ldlt() const;
 
-    const LLT<PlainObject, UpLo> llt() const;
-    const LDLT<PlainObject, UpLo> ldlt() const;
+  /////////// Eigenvalue module ///////////
 
-/////////// Eigenvalue module ///////////
+  /** Real part of #Scalar */
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  /** Return type of eigenvalues() */
+  typedef Matrix<RealScalar, internal::traits<MatrixType>::ColsAtCompileTime, 1> EigenvaluesReturnType;
 
-    /** Real part of #Scalar */
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    /** Return type of eigenvalues() */
-    typedef Matrix<RealScalar, internal::traits<MatrixType>::ColsAtCompileTime, 1> EigenvaluesReturnType;
+  EIGEN_DEVICE_FUNC EigenvaluesReturnType eigenvalues() const;
+  EIGEN_DEVICE_FUNC RealScalar operatorNorm() const;
 
-    EIGEN_DEVICE_FUNC
-    EigenvaluesReturnType eigenvalues() const;
-    EIGEN_DEVICE_FUNC
-    RealScalar operatorNorm() const;
-
-  protected:
-    MatrixTypeNested m_matrix;
+ protected:
+  MatrixTypeNested m_matrix;
 };
 
-
 // template<typename OtherDerived, typename MatrixType, unsigned int UpLo>
 // internal::selfadjoint_matrix_product_returntype<OtherDerived,SelfAdjointView<MatrixType,UpLo> >
 // operator*(const MatrixBase<OtherDerived>& lhs, const SelfAdjointView<MatrixType,UpLo>& rhs)
 // {
-//   return internal::matrix_selfadjoint_product_returntype<OtherDerived,SelfAdjointView<MatrixType,UpLo> >(lhs.derived(),rhs);
+//   return internal::matrix_selfadjoint_product_returntype<OtherDerived,SelfAdjointView<MatrixType,UpLo>
+//   >(lhs.derived(),rhs);
 // }
 
 // selfadjoint to dense matrix
@@ -280,86 +250,80 @@
 
 // TODO currently a selfadjoint expression has the form SelfAdjointView<.,.>
 //      in the future selfadjoint-ness should be defined by the expression traits
-//      such that Transpose<SelfAdjointView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
-template<typename MatrixType, unsigned int Mode>
-struct evaluator_traits<SelfAdjointView<MatrixType,Mode> >
-{
+//      such that Transpose<SelfAdjointView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to
+//      make it work)
+template <typename MatrixType, unsigned int Mode>
+struct evaluator_traits<SelfAdjointView<MatrixType, Mode> > {
   typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
   typedef SelfAdjointShape Shape;
 };
 
-template<int UpLo, int SetOpposite, typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor, int Version>
-class triangular_dense_assignment_kernel<UpLo,SelfAdjoint,SetOpposite,DstEvaluatorTypeT,SrcEvaluatorTypeT,Functor,Version>
-  : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version>
-{
-protected:
+template <int UpLo, int SetOpposite, typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor,
+          int Version>
+class triangular_dense_assignment_kernel<UpLo, SelfAdjoint, SetOpposite, DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor,
+                                         Version>
+    : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version> {
+ protected:
   typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version> Base;
   typedef typename Base::DstXprType DstXprType;
   typedef typename Base::SrcXprType SrcXprType;
   using Base::m_dst;
-  using Base::m_src;
   using Base::m_functor;
-public:
+  using Base::m_src;
 
+ public:
   typedef typename Base::DstEvaluatorType DstEvaluatorType;
   typedef typename Base::SrcEvaluatorType SrcEvaluatorType;
   typedef typename Base::Scalar Scalar;
   typedef typename Base::AssignmentTraits AssignmentTraits;
 
+  EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src,
+                                                       const Functor& func, DstXprType& dstExpr)
+      : Base(dst, src, func, dstExpr) {}
 
-  EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
-    : Base(dst, src, func, dstExpr)
-  {}
-
-  EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col)
-  {
-    eigen_internal_assert(row!=col);
-    Scalar tmp = m_src.coeff(row,col);
-    m_functor.assignCoeff(m_dst.coeffRef(row,col), tmp);
-    m_functor.assignCoeff(m_dst.coeffRef(col,row), numext::conj(tmp));
+  EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col) {
+    eigen_internal_assert(row != col);
+    Scalar tmp = m_src.coeff(row, col);
+    m_functor.assignCoeff(m_dst.coeffRef(row, col), tmp);
+    m_functor.assignCoeff(m_dst.coeffRef(col, row), numext::conj(tmp));
   }
 
-  EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id)
-  {
-    Base::assignCoeff(id,id);
-  }
+  EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id) { Base::assignCoeff(id, id); }
 
-  EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index, Index)
-  { eigen_internal_assert(false && "should never be called"); }
+  EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index, Index) { eigen_internal_assert(false && "should never be called"); }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /***************************************************************************
-* Implementation of MatrixBase methods
-***************************************************************************/
+ * Implementation of MatrixBase methods
+ ***************************************************************************/
 
 /** This is the const version of MatrixBase::selfadjointView() */
-template<typename Derived>
-template<unsigned int UpLo>
+template <typename Derived>
+template <unsigned int UpLo>
 EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::template ConstSelfAdjointViewReturnType<UpLo>::Type
-MatrixBase<Derived>::selfadjointView() const
-{
+MatrixBase<Derived>::selfadjointView() const {
   return typename ConstSelfAdjointViewReturnType<UpLo>::Type(derived());
 }
 
-/** \returns an expression of a symmetric/self-adjoint view extracted from the upper or lower triangular part of the current matrix
-  *
-  * The parameter \a UpLo can be either \c #Upper or \c #Lower
-  *
-  * Example: \include MatrixBase_selfadjointView.cpp
-  * Output: \verbinclude MatrixBase_selfadjointView.out
-  *
-  * \sa class SelfAdjointView
-  */
-template<typename Derived>
-template<unsigned int UpLo>
+/** \returns an expression of a symmetric/self-adjoint view extracted from the upper or lower triangular part of the
+ * current matrix
+ *
+ * The parameter \a UpLo can be either \c #Upper or \c #Lower
+ *
+ * Example: \include MatrixBase_selfadjointView.cpp
+ * Output: \verbinclude MatrixBase_selfadjointView.out
+ *
+ * \sa class SelfAdjointView
+ */
+template <typename Derived>
+template <unsigned int UpLo>
 EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::template SelfAdjointViewReturnType<UpLo>::Type
-MatrixBase<Derived>::selfadjointView()
-{
+MatrixBase<Derived>::selfadjointView() {
   return typename SelfAdjointViewReturnType<UpLo>::Type(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELFADJOINTMATRIX_H
+#endif  // EIGEN_SELFADJOINTMATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h
index 7c89c2e..4dc92f1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h
@@ -10,38 +10,41 @@
 #ifndef EIGEN_SELFCWISEBINARYOP_H
 #define EIGEN_SELFCWISEBINARYOP_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 // TODO generalize the scalar type of 'other'
 
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator*=(const Scalar& other)
-{
-  internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op<Scalar,Scalar>());
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator*=(const Scalar& other) {
+  internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other),
+                            internal::mul_assign_op<Scalar, Scalar>());
   return derived();
 }
 
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator+=(const Scalar& other)
-{
-  internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op<Scalar,Scalar>());
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator+=(const Scalar& other) {
+  internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other),
+                            internal::add_assign_op<Scalar, Scalar>());
   return derived();
 }
 
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator-=(const Scalar& other)
-{
-  internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op<Scalar,Scalar>());
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator-=(const Scalar& other) {
+  internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other),
+                            internal::sub_assign_op<Scalar, Scalar>());
   return derived();
 }
 
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator/=(const Scalar& other)
-{
-  internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op<Scalar,Scalar>());
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator/=(const Scalar& other) {
+  internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other),
+                            internal::div_assign_op<Scalar, Scalar>());
   return derived();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELFCWISEBINARYOP_H
+#endif  // EIGEN_SELFCWISEBINARYOP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h
new file mode 100644
index 0000000..b3fcc3a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h
@@ -0,0 +1,382 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2007-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SKEWSYMMETRICMATRIX3_H
+#define EIGEN_SKEWSYMMETRICMATRIX3_H
+
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
+
+/** \class SkewSymmetricBase
+ * \ingroup Core_Module
+ *
+ * \brief Base class for skew symmetric matrices and expressions
+ *
+ * This is the base class that is inherited by SkewSymmetricMatrix3 and related expression
+ * types, which internally use a three vector for storing the entries. SkewSymmetric
+ * types always represent square three times three matrices.
+ *
+ * This implementations follows class DiagonalMatrix
+ *
+ * \tparam Derived is the derived type, a SkewSymmetricMatrix3 or SkewSymmetricWrapper.
+ *
+ * \sa class SkewSymmetricMatrix3, class SkewSymmetricWrapper
+ */
+template <typename Derived>
+class SkewSymmetricBase : public EigenBase<Derived> {
+ public:
+  typedef typename internal::traits<Derived>::SkewSymmetricVectorType SkewSymmetricVectorType;
+  typedef typename SkewSymmetricVectorType::Scalar Scalar;
+  typedef typename SkewSymmetricVectorType::RealScalar RealScalar;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+
+  enum {
+    RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+    ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+    MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+    MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+    IsVectorAtCompileTime = 0,
+    Flags = NoPreferredStorageOrderBit
+  };
+
+  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime, 0, MaxRowsAtCompileTime, MaxColsAtCompileTime>
+      DenseMatrixType;
+  typedef DenseMatrixType DenseType;
+  typedef SkewSymmetricMatrix3<Scalar> PlainObject;
+
+  /** \returns a reference to the derived object. */
+  EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+  /** \returns a const reference to the derived object. */
+  EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast<Derived*>(this); }
+
+  /**
+   * Constructs a dense matrix from \c *this. Note, this directly returns a dense matrix type,
+   * not an expression.
+   * \returns A dense matrix, with its entries set from the the derived object. */
+  EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const { return derived(); }
+
+  /** Determinant vanishes */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar determinant() const { return 0; }
+
+  /** A.transpose() = -A */
+  EIGEN_DEVICE_FUNC PlainObject transpose() const { return (-vector()).asSkewSymmetric(); }
+
+  /** \returns the exponential of this matrix using Rodrigues’ formula */
+  EIGEN_DEVICE_FUNC DenseMatrixType exponential() const {
+    DenseMatrixType retVal = DenseMatrixType::Identity();
+    const SkewSymmetricVectorType& v = vector();
+    if (v.isZero()) {
+      return retVal;
+    }
+    const Scalar norm2 = v.squaredNorm();
+    const Scalar norm = numext::sqrt(norm2);
+    retVal += ((((1 - numext::cos(norm)) / norm2) * derived()) * derived()) +
+              (numext::sin(norm) / norm) * derived().toDenseMatrix();
+    return retVal;
+  }
+
+  /** \returns a reference to the derived object's vector of coefficients. */
+  EIGEN_DEVICE_FUNC inline const SkewSymmetricVectorType& vector() const { return derived().vector(); }
+  /** \returns a const reference to the derived object's vector of coefficients. */
+  EIGEN_DEVICE_FUNC inline SkewSymmetricVectorType& vector() { return derived().vector(); }
+
+  /** \returns the number of rows. */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const { return 3; }
+  /** \returns the number of columns. */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const { return 3; }
+
+  /** \returns the matrix product of \c *this by the dense matrix, \a matrix */
+  template <typename MatrixDerived>
+  EIGEN_DEVICE_FUNC Product<Derived, MatrixDerived, LazyProduct> operator*(
+      const MatrixBase<MatrixDerived>& matrix) const {
+    return Product<Derived, MatrixDerived, LazyProduct>(derived(), matrix.derived());
+  }
+
+  /** \returns the matrix product of \c *this by the skew symmetric matrix, \a matrix */
+  template <typename MatrixDerived>
+  EIGEN_DEVICE_FUNC Product<Derived, MatrixDerived, LazyProduct> operator*(
+      const SkewSymmetricBase<MatrixDerived>& matrix) const {
+    return Product<Derived, MatrixDerived, LazyProduct>(derived(), matrix.derived());
+  }
+
+  template <typename OtherDerived>
+  using SkewSymmetricProductReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+      SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, product)>;
+
+  /** \returns the wedge product of \c *this by the skew symmetric matrix \a other
+   *  A wedge B = AB - BA */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC SkewSymmetricProductReturnType<OtherDerived> wedge(
+      const SkewSymmetricBase<OtherDerived>& other) const {
+    return vector().cross(other.vector()).asSkewSymmetric();
+  }
+
+  using SkewSymmetricScaleReturnType =
+      SkewSymmetricWrapper<const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(SkewSymmetricVectorType, Scalar, product)>;
+
+  /** \returns the product of \c *this by the scalar \a scalar */
+  EIGEN_DEVICE_FUNC inline SkewSymmetricScaleReturnType operator*(const Scalar& scalar) const {
+    return (vector() * scalar).asSkewSymmetric();
+  }
+
+  using ScaleSkewSymmetricReturnType =
+      SkewSymmetricWrapper<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar, SkewSymmetricVectorType, product)>;
+
+  /** \returns the product of a scalar and the skew symmetric matrix \a other */
+  EIGEN_DEVICE_FUNC friend inline ScaleSkewSymmetricReturnType operator*(const Scalar& scalar,
+                                                                         const SkewSymmetricBase& other) {
+    return (scalar * other.vector()).asSkewSymmetric();
+  }
+
+  template <typename OtherDerived>
+  using SkewSymmetricSumReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+      SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, sum)>;
+
+  /** \returns the sum of \c *this and the skew symmetric matrix \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline SkewSymmetricSumReturnType<OtherDerived> operator+(
+      const SkewSymmetricBase<OtherDerived>& other) const {
+    return (vector() + other.vector()).asSkewSymmetric();
+  }
+
+  template <typename OtherDerived>
+  using SkewSymmetricDifferenceReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+      SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, difference)>;
+
+  /** \returns the difference of \c *this and the skew symmetric matrix \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline SkewSymmetricDifferenceReturnType<OtherDerived> operator-(
+      const SkewSymmetricBase<OtherDerived>& other) const {
+    return (vector() - other.vector()).asSkewSymmetric();
+  }
+};
+
+/** \class SkewSymmetricMatrix3
+ * \ingroup Core_Module
+ *
+ * \brief Represents a 3x3 skew symmetric matrix with its storage
+ *
+ * \tparam Scalar_ the type of coefficients
+ *
+ * \sa class SkewSymmetricBase, class SkewSymmetricWrapper
+ */
+
+namespace internal {
+template <typename Scalar_>
+struct traits<SkewSymmetricMatrix3<Scalar_>> : traits<Matrix<Scalar_, 3, 3, 0, 3, 3>> {
+  typedef Matrix<Scalar_, 3, 1, 0, 3, 1> SkewSymmetricVectorType;
+  typedef SkewSymmetricShape StorageKind;
+  enum { Flags = LvalueBit | NoPreferredStorageOrderBit | NestByRefBit };
+};
+}  // namespace internal
+template <typename Scalar_>
+class SkewSymmetricMatrix3 : public SkewSymmetricBase<SkewSymmetricMatrix3<Scalar_>> {
+ public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  typedef typename internal::traits<SkewSymmetricMatrix3>::SkewSymmetricVectorType SkewSymmetricVectorType;
+  typedef const SkewSymmetricMatrix3& Nested;
+  typedef Scalar_ Scalar;
+  typedef typename internal::traits<SkewSymmetricMatrix3>::StorageKind StorageKind;
+  typedef typename internal::traits<SkewSymmetricMatrix3>::StorageIndex StorageIndex;
+#endif
+
+ protected:
+  SkewSymmetricVectorType m_vector;
+
+ public:
+  /** const version of vector(). */
+  EIGEN_DEVICE_FUNC inline const SkewSymmetricVectorType& vector() const { return m_vector; }
+  /** \returns a reference to the stored vector of coefficients. */
+  EIGEN_DEVICE_FUNC inline SkewSymmetricVectorType& vector() { return m_vector; }
+
+  /** Default constructor without initialization */
+  EIGEN_DEVICE_FUNC inline SkewSymmetricMatrix3() {}
+
+  /** Constructor from three scalars */
+  EIGEN_DEVICE_FUNC inline SkewSymmetricMatrix3(const Scalar& x, const Scalar& y, const Scalar& z)
+      : m_vector(x, y, z) {}
+
+  /** \brief Constructs a SkewSymmetricMatrix3 from an r-value vector type */
+  EIGEN_DEVICE_FUNC explicit inline SkewSymmetricMatrix3(SkewSymmetricVectorType&& vec) : m_vector(std::move(vec)) {}
+
+  /** generic constructor from expression of the coefficients */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC explicit inline SkewSymmetricMatrix3(const MatrixBase<OtherDerived>& other) : m_vector(other) {}
+
+  /** Copy constructor. */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC inline SkewSymmetricMatrix3(const SkewSymmetricBase<OtherDerived>& other)
+      : m_vector(other.vector()) {}
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** copy constructor. prevent a default copy constructor from hiding the other templated constructor */
+  inline SkewSymmetricMatrix3(const SkewSymmetricMatrix3& other) : m_vector(other.vector()) {}
+#endif
+
+  /** Copy operator. */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC SkewSymmetricMatrix3& operator=(const SkewSymmetricBase<OtherDerived>& other) {
+    m_vector = other.vector();
+    return *this;
+  }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** This is a special case of the templated operator=. Its purpose is to
+   * prevent a default operator= from hiding the templated operator=.
+   */
+  EIGEN_DEVICE_FUNC SkewSymmetricMatrix3& operator=(const SkewSymmetricMatrix3& other) {
+    m_vector = other.vector();
+    return *this;
+  }
+#endif
+
+  typedef SkewSymmetricWrapper<const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, SkewSymmetricVectorType>>
+      InitializeReturnType;
+
+  /** Initializes a skew symmetric matrix with coefficients set to zero */
+  EIGEN_DEVICE_FUNC static InitializeReturnType Zero() { return SkewSymmetricVectorType::Zero().asSkewSymmetric(); }
+
+  /** Sets all coefficients to zero. */
+  EIGEN_DEVICE_FUNC inline void setZero() { m_vector.setZero(); }
+};
+
+/** \class SkewSymmetricWrapper
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a skew symmetric matrix
+ *
+ * \tparam SkewSymmetricVectorType_ the type of the vector of coefficients
+ *
+ * This class is an expression of a skew symmetric matrix, but not storing its own vector of coefficients,
+ * instead wrapping an existing vector expression. It is the return type of MatrixBase::asSkewSymmetric()
+ * and most of the time this is the only way that it is used.
+ *
+ * \sa class SkewSymmetricMatrix3, class SkewSymmetricBase, MatrixBase::asSkewSymmetric()
+ */
+
+namespace internal {
+template <typename SkewSymmetricVectorType_>
+struct traits<SkewSymmetricWrapper<SkewSymmetricVectorType_>> {
+  typedef SkewSymmetricVectorType_ SkewSymmetricVectorType;
+  typedef typename SkewSymmetricVectorType::Scalar Scalar;
+  typedef typename SkewSymmetricVectorType::StorageIndex StorageIndex;
+  typedef SkewSymmetricShape StorageKind;
+  typedef typename traits<SkewSymmetricVectorType>::XprKind XprKind;
+  enum {
+    RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+    ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+    MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+    MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+    Flags = (traits<SkewSymmetricVectorType>::Flags & LvalueBit) | NoPreferredStorageOrderBit
+  };
+};
+}  // namespace internal
+
+template <typename SkewSymmetricVectorType_>
+class SkewSymmetricWrapper : public SkewSymmetricBase<SkewSymmetricWrapper<SkewSymmetricVectorType_>>,
+                             internal::no_assignment_operator {
+ public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  typedef SkewSymmetricVectorType_ SkewSymmetricVectorType;
+  typedef SkewSymmetricWrapper Nested;
+#endif
+
+  /** Constructor from expression of coefficients to wrap. */
+  EIGEN_DEVICE_FUNC explicit inline SkewSymmetricWrapper(SkewSymmetricVectorType& a_vector) : m_vector(a_vector) {}
+
+  /** \returns a const reference to the wrapped expression of coefficients. */
+  EIGEN_DEVICE_FUNC const SkewSymmetricVectorType& vector() const { return m_vector; }
+
+ protected:
+  typename SkewSymmetricVectorType::Nested m_vector;
+};
+
+/** \returns a pseudo-expression of a skew symmetric matrix with *this as vector of coefficients
+ *
+ * \only_for_vectors
+ *
+ * \sa class SkewSymmetricWrapper, class SkewSymmetricMatrix3, vector(), isSkewSymmetric()
+ **/
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const SkewSymmetricWrapper<const Derived> MatrixBase<Derived>::asSkewSymmetric() const {
+  return SkewSymmetricWrapper<const Derived>(derived());
+}
+
+/** \returns true if *this is approximately equal to a skew symmetric matrix,
+ *          within the precision given by \a prec.
+ */
+template <typename Derived>
+bool MatrixBase<Derived>::isSkewSymmetric(const RealScalar& prec) const {
+  if (cols() != rows()) return false;
+  return (this->transpose() + *this).isZero(prec);
+}
+
+/** \returns the matrix product of \c *this by the skew symmetric matrix \skew.
+ */
+template <typename Derived>
+template <typename SkewDerived>
+EIGEN_DEVICE_FUNC inline const Product<Derived, SkewDerived, LazyProduct> MatrixBase<Derived>::operator*(
+    const SkewSymmetricBase<SkewDerived>& skew) const {
+  return Product<Derived, SkewDerived, LazyProduct>(derived(), skew.derived());
+}
+
+namespace internal {
+
+template <>
+struct storage_kind_to_shape<SkewSymmetricShape> {
+  typedef SkewSymmetricShape Shape;
+};
+
+struct SkewSymmetric2Dense {};
+
+template <>
+struct AssignmentKind<DenseShape, SkewSymmetricShape> {
+  typedef SkewSymmetric2Dense Kind;
+};
+
+// SkewSymmetric matrix to Dense assignment
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, SkewSymmetric2Dense> {
+  EIGEN_DEVICE_FUNC static void run(
+      DstXprType& dst, const SrcXprType& src,
+      const internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
+    if ((dst.rows() != 3) || (dst.cols() != 3)) {
+      dst.resize(3, 3);
+    }
+    dst.diagonal().setZero();
+    const typename SrcXprType::SkewSymmetricVectorType v = src.vector();
+    dst(0, 1) = -v(2);
+    dst(1, 0) = v(2);
+    dst(0, 2) = v(1);
+    dst(2, 0) = -v(1);
+    dst(1, 2) = -v(0);
+    dst(2, 1) = v(0);
+  }
+  EIGEN_DEVICE_FUNC static void run(
+      DstXprType& dst, const SrcXprType& src,
+      const internal::add_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
+    dst.vector() += src.vector();
+  }
+
+  EIGEN_DEVICE_FUNC static void run(
+      DstXprType& dst, const SrcXprType& src,
+      const internal::sub_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
+    dst.vector() -= src.vector();
+  }
+};
+
+}  // namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_SKEWSYMMETRICMATRIX3_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h
index 23d5cb7..dfea9c6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h
@@ -10,179 +10,165 @@
 #ifndef EIGEN_SOLVE_H
 #define EIGEN_SOLVE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename Decomposition, typename RhsType, typename StorageKind> class SolveImpl;
+template <typename Decomposition, typename RhsType, typename StorageKind>
+class SolveImpl;
 
 /** \class Solve
-  * \ingroup Core_Module
-  *
-  * \brief Pseudo expression representing a solving operation
-  *
-  * \tparam Decomposition the type of the matrix or decomposition object
-  * \tparam Rhstype the type of the right-hand side
-  *
-  * This class represents an expression of A.solve(B)
-  * and most of the time this is the only way it is used.
-  *
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Pseudo expression representing a solving operation
+ *
+ * \tparam Decomposition the type of the matrix or decomposition object
+ * \tparam Rhstype the type of the right-hand side
+ *
+ * This class represents an expression of A.solve(B)
+ * and most of the time this is the only way it is used.
+ *
+ */
 namespace internal {
 
 // this solve_traits class permits to determine the evaluation type with respect to storage kind (Dense vs Sparse)
-template<typename Decomposition, typename RhsType,typename StorageKind> struct solve_traits;
+template <typename Decomposition, typename RhsType, typename StorageKind>
+struct solve_traits;
 
-template<typename Decomposition, typename RhsType>
-struct solve_traits<Decomposition,RhsType,Dense>
-{
-  typedef typename make_proper_matrix_type<typename RhsType::Scalar,
-                 Decomposition::ColsAtCompileTime,
-                 RhsType::ColsAtCompileTime,
-                 RhsType::PlainObject::Options,
-                 Decomposition::MaxColsAtCompileTime,
-                 RhsType::MaxColsAtCompileTime>::type PlainObject;
+template <typename Decomposition, typename RhsType>
+struct solve_traits<Decomposition, RhsType, Dense> {
+  typedef typename make_proper_matrix_type<typename RhsType::Scalar, Decomposition::ColsAtCompileTime,
+                                           RhsType::ColsAtCompileTime, RhsType::PlainObject::Options,
+                                           Decomposition::MaxColsAtCompileTime, RhsType::MaxColsAtCompileTime>::type
+      PlainObject;
 };
 
-template<typename Decomposition, typename RhsType>
+template <typename Decomposition, typename RhsType>
 struct traits<Solve<Decomposition, RhsType> >
-  : traits<typename solve_traits<Decomposition,RhsType,typename internal::traits<RhsType>::StorageKind>::PlainObject>
-{
-  typedef typename solve_traits<Decomposition,RhsType,typename internal::traits<RhsType>::StorageKind>::PlainObject PlainObject;
-  typedef typename promote_index_type<typename Decomposition::StorageIndex, typename RhsType::StorageIndex>::type StorageIndex;
+    : traits<
+          typename solve_traits<Decomposition, RhsType, typename internal::traits<RhsType>::StorageKind>::PlainObject> {
+  typedef typename solve_traits<Decomposition, RhsType, typename internal::traits<RhsType>::StorageKind>::PlainObject
+      PlainObject;
+  typedef typename promote_index_type<typename Decomposition::StorageIndex, typename RhsType::StorageIndex>::type
+      StorageIndex;
   typedef traits<PlainObject> BaseTraits;
-  enum {
-    Flags = BaseTraits::Flags & RowMajorBit,
-    CoeffReadCost = HugeCost
-  };
+  enum { Flags = BaseTraits::Flags & RowMajorBit, CoeffReadCost = HugeCost };
 };
 
-}
+}  // namespace internal
 
-
-template<typename Decomposition, typename RhsType>
-class Solve : public SolveImpl<Decomposition,RhsType,typename internal::traits<RhsType>::StorageKind>
-{
-public:
+template <typename Decomposition, typename RhsType>
+class Solve : public SolveImpl<Decomposition, RhsType, typename internal::traits<RhsType>::StorageKind> {
+ public:
   typedef typename internal::traits<Solve>::PlainObject PlainObject;
   typedef typename internal::traits<Solve>::StorageIndex StorageIndex;
 
-  Solve(const Decomposition &dec, const RhsType &rhs)
-    : m_dec(dec), m_rhs(rhs)
-  {}
+  Solve(const Decomposition &dec, const RhsType &rhs) : m_dec(dec), m_rhs(rhs) {}
 
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_dec.cols(); }
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
 
-  EIGEN_DEVICE_FUNC const Decomposition& dec() const { return m_dec; }
-  EIGEN_DEVICE_FUNC const RhsType&       rhs() const { return m_rhs; }
+  EIGEN_DEVICE_FUNC const Decomposition &dec() const { return m_dec; }
+  EIGEN_DEVICE_FUNC const RhsType &rhs() const { return m_rhs; }
 
-protected:
+ protected:
   const Decomposition &m_dec;
-  const RhsType       &m_rhs;
+  const typename internal::ref_selector<RhsType>::type m_rhs;
 };
 
-
 // Specialization of the Solve expression for dense results
-template<typename Decomposition, typename RhsType>
-class SolveImpl<Decomposition,RhsType,Dense>
-  : public MatrixBase<Solve<Decomposition,RhsType> >
-{
-  typedef Solve<Decomposition,RhsType> Derived;
+template <typename Decomposition, typename RhsType>
+class SolveImpl<Decomposition, RhsType, Dense> : public MatrixBase<Solve<Decomposition, RhsType> > {
+  typedef Solve<Decomposition, RhsType> Derived;
 
-public:
-
-  typedef MatrixBase<Solve<Decomposition,RhsType> > Base;
+ public:
+  typedef MatrixBase<Solve<Decomposition, RhsType> > Base;
   EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
 
-private:
-
+ private:
   Scalar coeff(Index row, Index col) const;
   Scalar coeff(Index i) const;
 };
 
 // Generic API dispatcher
-template<typename Decomposition, typename RhsType, typename StorageKind>
-class SolveImpl : public internal::generic_xpr_base<Solve<Decomposition,RhsType>, MatrixXpr, StorageKind>::type
-{
-  public:
-    typedef typename internal::generic_xpr_base<Solve<Decomposition,RhsType>, MatrixXpr, StorageKind>::type Base;
+template <typename Decomposition, typename RhsType, typename StorageKind>
+class SolveImpl : public internal::generic_xpr_base<Solve<Decomposition, RhsType>, MatrixXpr, StorageKind>::type {
+ public:
+  typedef typename internal::generic_xpr_base<Solve<Decomposition, RhsType>, MatrixXpr, StorageKind>::type Base;
 };
 
 namespace internal {
 
 // Evaluator of Solve -> eval into a temporary
-template<typename Decomposition, typename RhsType>
-struct evaluator<Solve<Decomposition,RhsType> >
-  : public evaluator<typename Solve<Decomposition,RhsType>::PlainObject>
-{
-  typedef Solve<Decomposition,RhsType> SolveType;
+template <typename Decomposition, typename RhsType>
+struct evaluator<Solve<Decomposition, RhsType> >
+    : public evaluator<typename Solve<Decomposition, RhsType>::PlainObject> {
+  typedef Solve<Decomposition, RhsType> SolveType;
   typedef typename SolveType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
 
   enum { Flags = Base::Flags | EvalBeforeNestingBit };
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const SolveType& solve)
-    : m_result(solve.rows(), solve.cols())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
+  EIGEN_DEVICE_FUNC explicit evaluator(const SolveType &solve) : m_result(solve.rows(), solve.cols()) {
+    internal::construct_at<Base>(this, m_result);
     solve.dec()._solve_impl(solve.rhs(), m_result);
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
 // Specialization for "dst = dec.solve(rhs)"
-// NOTE we need to specialize it for Dense2Dense to avoid ambiguous specialization error and a Sparse2Sparse specialization must exist somewhere
-template<typename DstXprType, typename DecType, typename RhsType, typename Scalar>
-struct Assignment<DstXprType, Solve<DecType,RhsType>, internal::assign_op<Scalar,Scalar>, Dense2Dense>
-{
-  typedef Solve<DecType,RhsType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
-  {
+// NOTE we need to specialize it for Dense2Dense to avoid ambiguous specialization error and a Sparse2Sparse
+// specialization must exist somewhere
+template <typename DstXprType, typename DecType, typename RhsType, typename Scalar>
+struct Assignment<DstXprType, Solve<DecType, RhsType>, internal::assign_op<Scalar, Scalar>, Dense2Dense> {
+  typedef Solve<DecType, RhsType> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar, Scalar> &) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     src.dec()._solve_impl(src.rhs(), dst);
   }
 };
 
 // Specialization for "dst = dec.transpose().solve(rhs)"
-template<typename DstXprType, typename DecType, typename RhsType, typename Scalar>
-struct Assignment<DstXprType, Solve<Transpose<const DecType>,RhsType>, internal::assign_op<Scalar,Scalar>, Dense2Dense>
-{
-  typedef Solve<Transpose<const DecType>,RhsType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
-  {
+template <typename DstXprType, typename DecType, typename RhsType, typename Scalar>
+struct Assignment<DstXprType, Solve<Transpose<const DecType>, RhsType>, internal::assign_op<Scalar, Scalar>,
+                  Dense2Dense> {
+  typedef Solve<Transpose<const DecType>, RhsType> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar, Scalar> &) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     src.dec().nestedExpression().template _solve_impl_transposed<false>(src.rhs(), dst);
   }
 };
 
 // Specialization for "dst = dec.adjoint().solve(rhs)"
-template<typename DstXprType, typename DecType, typename RhsType, typename Scalar>
-struct Assignment<DstXprType, Solve<CwiseUnaryOp<internal::scalar_conjugate_op<typename DecType::Scalar>, const Transpose<const DecType> >,RhsType>,
-                  internal::assign_op<Scalar,Scalar>, Dense2Dense>
-{
-  typedef Solve<CwiseUnaryOp<internal::scalar_conjugate_op<typename DecType::Scalar>, const Transpose<const DecType> >,RhsType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
-  {
+template <typename DstXprType, typename DecType, typename RhsType, typename Scalar>
+struct Assignment<
+    DstXprType,
+    Solve<CwiseUnaryOp<internal::scalar_conjugate_op<typename DecType::Scalar>, const Transpose<const DecType> >,
+          RhsType>,
+    internal::assign_op<Scalar, Scalar>, Dense2Dense> {
+  typedef Solve<CwiseUnaryOp<internal::scalar_conjugate_op<typename DecType::Scalar>, const Transpose<const DecType> >,
+                RhsType>
+      SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar, Scalar> &) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     src.dec().nestedExpression().nestedExpression().template _solve_impl_transposed<true>(src.rhs(), dst);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SOLVE_H
+#endif  // EIGEN_SOLVE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h
index dfbf995..26d62ff 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h
@@ -10,226 +10,228 @@
 #ifndef EIGEN_SOLVETRIANGULAR_H
 #define EIGEN_SOLVETRIANGULAR_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 // Forward declarations:
 // The following two routines are implemented in the products/TriangularSolver*.h files
-template<typename LhsScalar, typename RhsScalar, typename Index, int Side, int Mode, bool Conjugate, int StorageOrder>
+template <typename LhsScalar, typename RhsScalar, typename Index, int Side, int Mode, bool Conjugate, int StorageOrder>
 struct triangular_solve_vector;
 
-template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder, int OtherStorageOrder, int OtherInnerStride>
+template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder,
+          int OtherStorageOrder, int OtherInnerStride>
 struct triangular_solve_matrix;
 
 // small helper struct extracting some traits on the underlying solver operation
-template<typename Lhs, typename Rhs, int Side>
-class trsolve_traits
-{
-  private:
-    enum {
-      RhsIsVectorAtCompileTime = (Side==OnTheLeft ? Rhs::ColsAtCompileTime : Rhs::RowsAtCompileTime)==1
-    };
-  public:
-    enum {
-      Unrolling   = (RhsIsVectorAtCompileTime && Rhs::SizeAtCompileTime != Dynamic && Rhs::SizeAtCompileTime <= 8)
-                  ? CompleteUnrolling : NoUnrolling,
-      RhsVectors  = RhsIsVectorAtCompileTime ? 1 : Dynamic
-    };
+template <typename Lhs, typename Rhs, int Side>
+class trsolve_traits {
+ private:
+  enum { RhsIsVectorAtCompileTime = (Side == OnTheLeft ? Rhs::ColsAtCompileTime : Rhs::RowsAtCompileTime) == 1 };
+
+ public:
+  enum {
+    Unrolling = (RhsIsVectorAtCompileTime && Rhs::SizeAtCompileTime != Dynamic && Rhs::SizeAtCompileTime <= 8)
+                    ? CompleteUnrolling
+                    : NoUnrolling,
+    RhsVectors = RhsIsVectorAtCompileTime ? 1 : Dynamic
+  };
 };
 
-template<typename Lhs, typename Rhs,
-  int Side, // can be OnTheLeft/OnTheRight
-  int Mode, // can be Upper/Lower | UnitDiag
-  int Unrolling = trsolve_traits<Lhs,Rhs,Side>::Unrolling,
-  int RhsVectors = trsolve_traits<Lhs,Rhs,Side>::RhsVectors
-  >
+template <typename Lhs, typename Rhs,
+          int Side,  // can be OnTheLeft/OnTheRight
+          int Mode,  // can be Upper/Lower | UnitDiag
+          int Unrolling = trsolve_traits<Lhs, Rhs, Side>::Unrolling,
+          int RhsVectors = trsolve_traits<Lhs, Rhs, Side>::RhsVectors>
 struct triangular_solver_selector;
 
-template<typename Lhs, typename Rhs, int Side, int Mode>
-struct triangular_solver_selector<Lhs,Rhs,Side,Mode,NoUnrolling,1>
-{
+template <typename Lhs, typename Rhs, int Side, int Mode>
+struct triangular_solver_selector<Lhs, Rhs, Side, Mode, NoUnrolling, 1> {
   typedef typename Lhs::Scalar LhsScalar;
   typedef typename Rhs::Scalar RhsScalar;
   typedef blas_traits<Lhs> LhsProductTraits;
   typedef typename LhsProductTraits::ExtractType ActualLhsType;
-  typedef Map<Matrix<RhsScalar,Dynamic,1>, Aligned> MappedRhs;
-  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
-  {
+  typedef Map<Matrix<RhsScalar, Dynamic, 1>, Aligned> MappedRhs;
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs) {
     ActualLhsType actualLhs = LhsProductTraits::extract(lhs);
 
     // FIXME find a way to allow an inner stride if packet_traits<Scalar>::size==1
 
-    bool useRhsDirectly = Rhs::InnerStrideAtCompileTime==1 || rhs.innerStride()==1;
+    bool useRhsDirectly = Rhs::InnerStrideAtCompileTime == 1 || rhs.innerStride() == 1;
 
-    ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhs,rhs.size(),
-                                                  (useRhsDirectly ? rhs.data() : 0));
+    ei_declare_aligned_stack_constructed_variable(RhsScalar, actualRhs, rhs.size(), (useRhsDirectly ? rhs.data() : 0));
 
-    if(!useRhsDirectly)
-      MappedRhs(actualRhs,rhs.size()) = rhs;
+    if (!useRhsDirectly) MappedRhs(actualRhs, rhs.size()) = rhs;
 
     triangular_solve_vector<LhsScalar, RhsScalar, Index, Side, Mode, LhsProductTraits::NeedToConjugate,
-                            (int(Lhs::Flags) & RowMajorBit) ? RowMajor : ColMajor>
-      ::run(actualLhs.cols(), actualLhs.data(), actualLhs.outerStride(), actualRhs);
+                            (int(Lhs::Flags) & RowMajorBit) ? RowMajor : ColMajor>::run(actualLhs.cols(),
+                                                                                        actualLhs.data(),
+                                                                                        actualLhs.outerStride(),
+                                                                                        actualRhs);
 
-    if(!useRhsDirectly)
-      rhs = MappedRhs(actualRhs, rhs.size());
+    if (!useRhsDirectly) rhs = MappedRhs(actualRhs, rhs.size());
   }
 };
 
 // the rhs is a matrix
-template<typename Lhs, typename Rhs, int Side, int Mode>
-struct triangular_solver_selector<Lhs,Rhs,Side,Mode,NoUnrolling,Dynamic>
-{
+template <typename Lhs, typename Rhs, int Side, int Mode>
+struct triangular_solver_selector<Lhs, Rhs, Side, Mode, NoUnrolling, Dynamic> {
   typedef typename Rhs::Scalar Scalar;
   typedef blas_traits<Lhs> LhsProductTraits;
   typedef typename LhsProductTraits::DirectLinearAccessType ActualLhsType;
 
-  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
-  {
-    typename internal::add_const_on_value_type<ActualLhsType>::type actualLhs = LhsProductTraits::extract(lhs);
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs) {
+    add_const_on_value_type_t<ActualLhsType> actualLhs = LhsProductTraits::extract(lhs);
 
     const Index size = lhs.rows();
-    const Index othersize = Side==OnTheLeft? rhs.cols() : rhs.rows();
+    const Index othersize = Side == OnTheLeft ? rhs.cols() : rhs.rows();
 
-    typedef internal::gemm_blocking_space<(Rhs::Flags&RowMajorBit) ? RowMajor : ColMajor,Scalar,Scalar,
-              Rhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime, Lhs::MaxRowsAtCompileTime,4> BlockingType;
+    typedef internal::gemm_blocking_space<(Rhs::Flags & RowMajorBit) ? RowMajor : ColMajor, Scalar, Scalar,
+                                          Rhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime,
+                                          Lhs::MaxRowsAtCompileTime, 4>
+        BlockingType;
+
+    // Nothing to solve.
+    if (actualLhs.size() == 0 || rhs.size() == 0) {
+      return;
+    }
 
     BlockingType blocking(rhs.rows(), rhs.cols(), size, 1, false);
 
-    triangular_solve_matrix<Scalar,Index,Side,Mode,LhsProductTraits::NeedToConjugate,(int(Lhs::Flags) & RowMajorBit) ? RowMajor : ColMajor,
-                               (Rhs::Flags&RowMajorBit) ? RowMajor : ColMajor, Rhs::InnerStrideAtCompileTime>
-      ::run(size, othersize, &actualLhs.coeffRef(0,0), actualLhs.outerStride(), &rhs.coeffRef(0,0), rhs.innerStride(), rhs.outerStride(), blocking);
+    triangular_solve_matrix<Scalar, Index, Side, Mode, LhsProductTraits::NeedToConjugate,
+                            (int(Lhs::Flags) & RowMajorBit) ? RowMajor : ColMajor,
+                            (Rhs::Flags & RowMajorBit) ? RowMajor : ColMajor,
+                            Rhs::InnerStrideAtCompileTime>::run(size, othersize, &actualLhs.coeffRef(0, 0),
+                                                                actualLhs.outerStride(), &rhs.coeffRef(0, 0),
+                                                                rhs.innerStride(), rhs.outerStride(), blocking);
   }
 };
 
 /***************************************************************************
-* meta-unrolling implementation
-***************************************************************************/
+ * meta-unrolling implementation
+ ***************************************************************************/
 
-template<typename Lhs, typename Rhs, int Mode, int LoopIndex, int Size,
-         bool Stop = LoopIndex==Size>
+template <typename Lhs, typename Rhs, int Mode, int LoopIndex, int Size, bool Stop = LoopIndex == Size>
 struct triangular_solver_unroller;
 
-template<typename Lhs, typename Rhs, int Mode, int LoopIndex, int Size>
-struct triangular_solver_unroller<Lhs,Rhs,Mode,LoopIndex,Size,false> {
+template <typename Lhs, typename Rhs, int Mode, int LoopIndex, int Size>
+struct triangular_solver_unroller<Lhs, Rhs, Mode, LoopIndex, Size, false> {
   enum {
-    IsLower = ((Mode&Lower)==Lower),
-    DiagIndex  = IsLower ? LoopIndex : Size - LoopIndex - 1,
-    StartIndex = IsLower ? 0         : DiagIndex+1
+    IsLower = ((Mode & Lower) == Lower),
+    DiagIndex = IsLower ? LoopIndex : Size - LoopIndex - 1,
+    StartIndex = IsLower ? 0 : DiagIndex + 1
   };
-  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
-  {
-    if (LoopIndex>0)
-      rhs.coeffRef(DiagIndex) -= lhs.row(DiagIndex).template segment<LoopIndex>(StartIndex).transpose()
-                                .cwiseProduct(rhs.template segment<LoopIndex>(StartIndex)).sum();
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs) {
+    if (LoopIndex > 0)
+      rhs.coeffRef(DiagIndex) -= lhs.row(DiagIndex)
+                                     .template segment<LoopIndex>(StartIndex)
+                                     .transpose()
+                                     .cwiseProduct(rhs.template segment<LoopIndex>(StartIndex))
+                                     .sum();
 
-    if(!(Mode & UnitDiag))
-      rhs.coeffRef(DiagIndex) /= lhs.coeff(DiagIndex,DiagIndex);
+    if (!(Mode & UnitDiag)) rhs.coeffRef(DiagIndex) /= lhs.coeff(DiagIndex, DiagIndex);
 
-    triangular_solver_unroller<Lhs,Rhs,Mode,LoopIndex+1,Size>::run(lhs,rhs);
+    triangular_solver_unroller<Lhs, Rhs, Mode, LoopIndex + 1, Size>::run(lhs, rhs);
   }
 };
 
-template<typename Lhs, typename Rhs, int Mode, int LoopIndex, int Size>
-struct triangular_solver_unroller<Lhs,Rhs,Mode,LoopIndex,Size,true> {
+template <typename Lhs, typename Rhs, int Mode, int LoopIndex, int Size>
+struct triangular_solver_unroller<Lhs, Rhs, Mode, LoopIndex, Size, true> {
   static EIGEN_DEVICE_FUNC void run(const Lhs&, Rhs&) {}
 };
 
-template<typename Lhs, typename Rhs, int Mode>
-struct triangular_solver_selector<Lhs,Rhs,OnTheLeft,Mode,CompleteUnrolling,1> {
-  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
-  { triangular_solver_unroller<Lhs,Rhs,Mode,0,Rhs::SizeAtCompileTime>::run(lhs,rhs); }
-};
-
-template<typename Lhs, typename Rhs, int Mode>
-struct triangular_solver_selector<Lhs,Rhs,OnTheRight,Mode,CompleteUnrolling,1> {
-  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
-  {
-    Transpose<const Lhs> trLhs(lhs);
-    Transpose<Rhs> trRhs(rhs);
-
-    triangular_solver_unroller<Transpose<const Lhs>,Transpose<Rhs>,
-                              ((Mode&Upper)==Upper ? Lower : Upper) | (Mode&UnitDiag),
-                              0,Rhs::SizeAtCompileTime>::run(trLhs,trRhs);
+template <typename Lhs, typename Rhs, int Mode>
+struct triangular_solver_selector<Lhs, Rhs, OnTheLeft, Mode, CompleteUnrolling, 1> {
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs) {
+    triangular_solver_unroller<Lhs, Rhs, Mode, 0, Rhs::SizeAtCompileTime>::run(lhs, rhs);
   }
 };
 
-} // end namespace internal
+template <typename Lhs, typename Rhs, int Mode>
+struct triangular_solver_selector<Lhs, Rhs, OnTheRight, Mode, CompleteUnrolling, 1> {
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs) {
+    Transpose<const Lhs> trLhs(lhs);
+    Transpose<Rhs> trRhs(rhs);
+
+    triangular_solver_unroller<Transpose<const Lhs>, Transpose<Rhs>,
+                               ((Mode & Upper) == Upper ? Lower : Upper) | (Mode & UnitDiag), 0,
+                               Rhs::SizeAtCompileTime>::run(trLhs, trRhs);
+  }
+};
+
+}  // end namespace internal
 
 /***************************************************************************
-* TriangularView methods
-***************************************************************************/
+ * TriangularView methods
+ ***************************************************************************/
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename MatrixType, unsigned int Mode>
-template<int Side, typename OtherDerived>
-EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType,Mode,Dense>::solveInPlace(const MatrixBase<OtherDerived>& _other) const
-{
+template <typename MatrixType, unsigned int Mode>
+template <int Side, typename OtherDerived>
+EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType, Mode, Dense>::solveInPlace(
+    const MatrixBase<OtherDerived>& _other) const {
   OtherDerived& other = _other.const_cast_derived();
-  eigen_assert( derived().cols() == derived().rows() && ((Side==OnTheLeft && derived().cols() == other.rows()) || (Side==OnTheRight && derived().cols() == other.cols())) );
+  eigen_assert(derived().cols() == derived().rows() && ((Side == OnTheLeft && derived().cols() == other.rows()) ||
+                                                        (Side == OnTheRight && derived().cols() == other.cols())));
   eigen_assert((!(int(Mode) & int(ZeroDiag))) && bool(int(Mode) & (int(Upper) | int(Lower))));
   // If solving for a 0x0 matrix, nothing to do, simply return.
-  if (derived().cols() == 0)
-    return;
+  if (derived().cols() == 0) return;
 
-  enum { copy = (internal::traits<OtherDerived>::Flags & RowMajorBit)  && OtherDerived::IsVectorAtCompileTime && OtherDerived::SizeAtCompileTime!=1};
-  typedef typename internal::conditional<copy,
-    typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::type OtherCopy;
+  enum {
+    copy = (internal::traits<OtherDerived>::Flags & RowMajorBit) && OtherDerived::IsVectorAtCompileTime &&
+           OtherDerived::SizeAtCompileTime != 1
+  };
+  typedef std::conditional_t<copy, typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>
+      OtherCopy;
   OtherCopy otherCopy(other);
 
-  internal::triangular_solver_selector<MatrixType, typename internal::remove_reference<OtherCopy>::type,
-    Side, Mode>::run(derived().nestedExpression(), otherCopy);
+  internal::triangular_solver_selector<MatrixType, std::remove_reference_t<OtherCopy>, Side, Mode>::run(
+      derived().nestedExpression(), otherCopy);
 
-  if (copy)
-    other = otherCopy;
+  if (copy) other = otherCopy;
 }
 
-template<typename Derived, unsigned int Mode>
-template<int Side, typename Other>
-const internal::triangular_solve_retval<Side,TriangularView<Derived,Mode>,Other>
-TriangularViewImpl<Derived,Mode,Dense>::solve(const MatrixBase<Other>& other) const
-{
-  return internal::triangular_solve_retval<Side,TriangularViewType,Other>(derived(), other.derived());
+template <typename Derived, unsigned int Mode>
+template <int Side, typename Other>
+const internal::triangular_solve_retval<Side, TriangularView<Derived, Mode>, Other>
+TriangularViewImpl<Derived, Mode, Dense>::solve(const MatrixBase<Other>& other) const {
+  return internal::triangular_solve_retval<Side, TriangularViewType, Other>(derived(), other.derived());
 }
 #endif
 
 namespace internal {
 
-
-template<int Side, typename TriangularType, typename Rhs>
-struct traits<triangular_solve_retval<Side, TriangularType, Rhs> >
-{
+template <int Side, typename TriangularType, typename Rhs>
+struct traits<triangular_solve_retval<Side, TriangularType, Rhs> > {
   typedef typename internal::plain_matrix_type_column_major<Rhs>::type ReturnType;
 };
 
-template<int Side, typename TriangularType, typename Rhs> struct triangular_solve_retval
- : public ReturnByValue<triangular_solve_retval<Side, TriangularType, Rhs> >
-{
-  typedef typename remove_all<typename Rhs::Nested>::type RhsNestedCleaned;
+template <int Side, typename TriangularType, typename Rhs>
+struct triangular_solve_retval : public ReturnByValue<triangular_solve_retval<Side, TriangularType, Rhs> > {
+  typedef remove_all_t<typename Rhs::Nested> RhsNestedCleaned;
   typedef ReturnByValue<triangular_solve_retval> Base;
 
-  triangular_solve_retval(const TriangularType& tri, const Rhs& rhs)
-    : m_triangularMatrix(tri), m_rhs(rhs)
-  {}
+  triangular_solve_retval(const TriangularType& tri, const Rhs& rhs) : m_triangularMatrix(tri), m_rhs(rhs) {}
 
   inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_rhs.rows(); }
   inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
 
-  template<typename Dest> inline void evalTo(Dest& dst) const
-  {
-    if(!is_same_dense(dst,m_rhs))
-      dst = m_rhs;
+  template <typename Dest>
+  inline void evalTo(Dest& dst) const {
+    if (!is_same_dense(dst, m_rhs)) dst = m_rhs;
     m_triangularMatrix.template solveInPlace<Side>(dst);
   }
 
-  protected:
-    const TriangularType& m_triangularMatrix;
-    typename Rhs::Nested m_rhs;
+ protected:
+  const TriangularType& m_triangularMatrix;
+  typename Rhs::Nested m_rhs;
 };
 
-} // namespace internal
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SOLVETRIANGULAR_H
+#endif  // EIGEN_SOLVETRIANGULAR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h
index 5014610..df2ac83 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h
@@ -10,159 +10,150 @@
 #ifndef EIGEN_SOLVERBASE_H
 #define EIGEN_SOLVERBASE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename Derived>
+template <typename Derived>
 struct solve_assertion {
-    template<bool Transpose_, typename Rhs>
-    static void run(const Derived& solver, const Rhs& b) { solver.template _check_solve_assertion<Transpose_>(b); }
+  template <bool Transpose_, typename Rhs>
+  static void run(const Derived& solver, const Rhs& b) {
+    solver.template _check_solve_assertion<Transpose_>(b);
+  }
 };
 
-template<typename Derived>
-struct solve_assertion<Transpose<Derived> >
-{
-    typedef Transpose<Derived> type;
+template <typename Derived>
+struct solve_assertion<Transpose<Derived>> {
+  typedef Transpose<Derived> type;
 
-    template<bool Transpose_, typename Rhs>
-    static void run(const type& transpose, const Rhs& b)
-    {
-        internal::solve_assertion<typename internal::remove_all<Derived>::type>::template run<true>(transpose.nestedExpression(), b);
-    }
+  template <bool Transpose_, typename Rhs>
+  static void run(const type& transpose, const Rhs& b) {
+    internal::solve_assertion<internal::remove_all_t<Derived>>::template run<true>(transpose.nestedExpression(), b);
+  }
 };
 
-template<typename Scalar, typename Derived>
-struct solve_assertion<CwiseUnaryOp<Eigen::internal::scalar_conjugate_op<Scalar>, const Transpose<Derived> > >
-{
-    typedef CwiseUnaryOp<Eigen::internal::scalar_conjugate_op<Scalar>, const Transpose<Derived> > type;
+template <typename Scalar, typename Derived>
+struct solve_assertion<CwiseUnaryOp<Eigen::internal::scalar_conjugate_op<Scalar>, const Transpose<Derived>>> {
+  typedef CwiseUnaryOp<Eigen::internal::scalar_conjugate_op<Scalar>, const Transpose<Derived>> type;
 
-    template<bool Transpose_, typename Rhs>
-    static void run(const type& adjoint, const Rhs& b)
-    {
-        internal::solve_assertion<typename internal::remove_all<Transpose<Derived> >::type>::template run<true>(adjoint.nestedExpression(), b);
-    }
+  template <bool Transpose_, typename Rhs>
+  static void run(const type& adjoint, const Rhs& b) {
+    internal::solve_assertion<internal::remove_all_t<Transpose<Derived>>>::template run<true>(
+        adjoint.nestedExpression(), b);
+  }
 };
-} // end namespace internal
+}  // end namespace internal
 
 /** \class SolverBase
-  * \brief A base class for matrix decomposition and solvers
-  *
-  * \tparam Derived the actual type of the decomposition/solver.
-  *
-  * Any matrix decomposition inheriting this base class provide the following API:
-  *
-  * \code
-  * MatrixType A, b, x;
-  * DecompositionType dec(A);
-  * x = dec.solve(b);             // solve A   * x = b
-  * x = dec.transpose().solve(b); // solve A^T * x = b
-  * x = dec.adjoint().solve(b);   // solve A'  * x = b
-  * \endcode
-  *
-  * \warning Currently, any other usage of transpose() and adjoint() are not supported and will produce compilation errors.
-  *
-  * \sa class PartialPivLU, class FullPivLU, class HouseholderQR, class ColPivHouseholderQR, class FullPivHouseholderQR, class CompleteOrthogonalDecomposition, class LLT, class LDLT, class SVDBase
-  */
-template<typename Derived>
-class SolverBase : public EigenBase<Derived>
-{
-  public:
+ * \brief A base class for matrix decomposition and solvers
+ *
+ * \tparam Derived the actual type of the decomposition/solver.
+ *
+ * Any matrix decomposition inheriting this base class provide the following API:
+ *
+ * \code
+ * MatrixType A, b, x;
+ * DecompositionType dec(A);
+ * x = dec.solve(b);             // solve A   * x = b
+ * x = dec.transpose().solve(b); // solve A^T * x = b
+ * x = dec.adjoint().solve(b);   // solve A'  * x = b
+ * \endcode
+ *
+ * \warning Currently, any other usage of transpose() and adjoint() are not supported and will produce compilation
+ * errors.
+ *
+ * \sa class PartialPivLU, class FullPivLU, class HouseholderQR, class ColPivHouseholderQR, class FullPivHouseholderQR,
+ * class CompleteOrthogonalDecomposition, class LLT, class LDLT, class SVDBase
+ */
+template <typename Derived>
+class SolverBase : public EigenBase<Derived> {
+ public:
+  typedef EigenBase<Derived> Base;
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef Scalar CoeffReturnType;
 
-    typedef EigenBase<Derived> Base;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef Scalar CoeffReturnType;
+  template <typename Derived_>
+  friend struct internal::solve_assertion;
 
-    template<typename Derived_>
-    friend struct internal::solve_assertion;
+  enum {
+    RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+    ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+    SizeAtCompileTime = (internal::size_of_xpr_at_compile_time<Derived>::ret),
+    MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
+    MaxSizeAtCompileTime = internal::size_at_compile_time(internal::traits<Derived>::MaxRowsAtCompileTime,
+                                                          internal::traits<Derived>::MaxColsAtCompileTime),
+    IsVectorAtCompileTime =
+        internal::traits<Derived>::MaxRowsAtCompileTime == 1 || internal::traits<Derived>::MaxColsAtCompileTime == 1,
+    NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0
+                    : bool(IsVectorAtCompileTime)  ? 1
+                                                   : 2
+  };
 
-    enum {
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
-                                                          internal::traits<Derived>::ColsAtCompileTime>::ret),
-      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
-      MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
-                                                             internal::traits<Derived>::MaxColsAtCompileTime>::ret),
-      IsVectorAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime == 1
-                           || internal::traits<Derived>::MaxColsAtCompileTime == 1,
-      NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2
-    };
+  /** Default constructor */
+  SolverBase() {}
 
-    /** Default constructor */
-    SolverBase()
-    {}
+  ~SolverBase() {}
 
-    ~SolverBase()
-    {}
+  using Base::derived;
 
-    using Base::derived;
+  /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
+   */
+  template <typename Rhs>
+  inline const Solve<Derived, Rhs> solve(const MatrixBase<Rhs>& b) const {
+    internal::solve_assertion<internal::remove_all_t<Derived>>::template run<false>(derived(), b);
+    return Solve<Derived, Rhs>(derived(), b.derived());
+  }
 
-    /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
-      */
-    template<typename Rhs>
-    inline const Solve<Derived, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      internal::solve_assertion<typename internal::remove_all<Derived>::type>::template run<false>(derived(), b);
-      return Solve<Derived, Rhs>(derived(), b.derived());
-    }
+  /** \internal the return type of transpose() */
+  typedef Transpose<const Derived> ConstTransposeReturnType;
+  /** \returns an expression of the transposed of the factored matrix.
+   *
+   * A typical usage is to solve for the transposed problem A^T x = b:
+   * \code x = dec.transpose().solve(b); \endcode
+   *
+   * \sa adjoint(), solve()
+   */
+  inline const ConstTransposeReturnType transpose() const { return ConstTransposeReturnType(derived()); }
 
-    /** \internal the return type of transpose() */
-    typedef typename internal::add_const<Transpose<const Derived> >::type ConstTransposeReturnType;
-    /** \returns an expression of the transposed of the factored matrix.
-      *
-      * A typical usage is to solve for the transposed problem A^T x = b:
-      * \code x = dec.transpose().solve(b); \endcode
-      *
-      * \sa adjoint(), solve()
-      */
-    inline ConstTransposeReturnType transpose() const
-    {
-      return ConstTransposeReturnType(derived());
-    }
+  /** \internal the return type of adjoint() */
+  typedef std::conditional_t<NumTraits<Scalar>::IsComplex,
+                             CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, const ConstTransposeReturnType>,
+                             const ConstTransposeReturnType>
+      AdjointReturnType;
+  /** \returns an expression of the adjoint of the factored matrix
+   *
+   * A typical usage is to solve for the adjoint problem A' x = b:
+   * \code x = dec.adjoint().solve(b); \endcode
+   *
+   * For real scalar types, this function is equivalent to transpose().
+   *
+   * \sa transpose(), solve()
+   */
+  inline const AdjointReturnType adjoint() const { return AdjointReturnType(derived().transpose()); }
 
-    /** \internal the return type of adjoint() */
-    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-                        CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, ConstTransposeReturnType>,
-                        ConstTransposeReturnType
-                     >::type AdjointReturnType;
-    /** \returns an expression of the adjoint of the factored matrix
-      *
-      * A typical usage is to solve for the adjoint problem A' x = b:
-      * \code x = dec.adjoint().solve(b); \endcode
-      *
-      * For real scalar types, this function is equivalent to transpose().
-      *
-      * \sa transpose(), solve()
-      */
-    inline AdjointReturnType adjoint() const
-    {
-      return AdjointReturnType(derived().transpose());
-    }
-
-  protected:
-
-    template<bool Transpose_, typename Rhs>
-    void _check_solve_assertion(const Rhs& b) const {
-        EIGEN_ONLY_USED_FOR_DEBUG(b);
-        eigen_assert(derived().m_isInitialized && "Solver is not initialized.");
-        eigen_assert((Transpose_?derived().cols():derived().rows())==b.rows() && "SolverBase::solve(): invalid number of rows of the right hand side matrix b");
-    }
+ protected:
+  template <bool Transpose_, typename Rhs>
+  void _check_solve_assertion(const Rhs& b) const {
+    EIGEN_ONLY_USED_FOR_DEBUG(b);
+    eigen_assert(derived().m_isInitialized && "Solver is not initialized.");
+    eigen_assert((Transpose_ ? derived().cols() : derived().rows()) == b.rows() &&
+                 "SolverBase::solve(): invalid number of rows of the right hand side matrix b");
+  }
 };
 
 namespace internal {
 
-template<typename Derived>
-struct generic_xpr_base<Derived, MatrixXpr, SolverStorage>
-{
+template <typename Derived>
+struct generic_xpr_base<Derived, MatrixXpr, SolverStorage> {
   typedef SolverBase<Derived> type;
-
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SOLVERBASE_H
+#endif  // EIGEN_SOLVERBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h
index 4a3f0cc..6513120 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h
@@ -10,119 +10,114 @@
 #ifndef EIGEN_STABLENORM_H
 #define EIGEN_STABLENORM_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename ExpressionType, typename Scalar>
-inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale)
-{
+template <typename ExpressionType, typename Scalar>
+inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale) {
   Scalar maxCoeff = bl.cwiseAbs().maxCoeff();
-  
-  if(maxCoeff>scale)
-  {
-    ssq = ssq * numext::abs2(scale/maxCoeff);
-    Scalar tmp = Scalar(1)/maxCoeff;
-    if(tmp > NumTraits<Scalar>::highest())
-    {
+
+  if (maxCoeff > scale) {
+    ssq = ssq * numext::abs2(scale / maxCoeff);
+    Scalar tmp = Scalar(1) / maxCoeff;
+    if (tmp > NumTraits<Scalar>::highest()) {
       invScale = NumTraits<Scalar>::highest();
-      scale = Scalar(1)/invScale;
-    }
-    else if(maxCoeff>NumTraits<Scalar>::highest()) // we got a INF
+      scale = Scalar(1) / invScale;
+    } else if (maxCoeff > NumTraits<Scalar>::highest())  // we got a INF
     {
       invScale = Scalar(1);
       scale = maxCoeff;
-    }
-    else
-    {
+    } else {
       scale = maxCoeff;
       invScale = tmp;
     }
-  }
-  else if(maxCoeff!=maxCoeff) // we got a NaN
+  } else if (maxCoeff != maxCoeff)  // we got a NaN
   {
     scale = maxCoeff;
   }
-  
+
   // TODO if the maxCoeff is much much smaller than the current scale,
   // then we can neglect this sub vector
-  if(scale>Scalar(0)) // if scale==0, then bl is 0 
-    ssq += (bl*invScale).squaredNorm();
+  if (scale > Scalar(0))  // if scale==0, then bl is 0
+    ssq += (bl * invScale).squaredNorm();
 }
 
-template<typename VectorType, typename RealScalar>
-void stable_norm_impl_inner_step(const VectorType &vec, RealScalar& ssq, RealScalar& scale, RealScalar& invScale)
-{
+template <typename VectorType, typename RealScalar>
+void stable_norm_impl_inner_step(const VectorType& vec, RealScalar& ssq, RealScalar& scale, RealScalar& invScale) {
   typedef typename VectorType::Scalar Scalar;
   const Index blockSize = 4096;
-  
-  typedef typename internal::nested_eval<VectorType,2>::type VectorTypeCopy;
-  typedef typename internal::remove_all<VectorTypeCopy>::type VectorTypeCopyClean;
+
+  typedef typename internal::nested_eval<VectorType, 2>::type VectorTypeCopy;
+  typedef internal::remove_all_t<VectorTypeCopy> VectorTypeCopyClean;
   const VectorTypeCopy copy(vec);
-  
+
   enum {
-    CanAlign = (   (int(VectorTypeCopyClean::Flags)&DirectAccessBit)
-                || (int(internal::evaluator<VectorTypeCopyClean>::Alignment)>0) // FIXME Alignment)>0 might not be enough
-               ) && (blockSize*sizeof(Scalar)*2<EIGEN_STACK_ALLOCATION_LIMIT)
-                 && (EIGEN_MAX_STATIC_ALIGN_BYTES>0) // if we cannot allocate on the stack, then let's not bother about this optimization
+    CanAlign =
+        ((int(VectorTypeCopyClean::Flags) & DirectAccessBit) ||
+         (int(internal::evaluator<VectorTypeCopyClean>::Alignment) > 0)  // FIXME Alignment)>0 might not be enough
+         ) &&
+        (blockSize * sizeof(Scalar) * 2 < EIGEN_STACK_ALLOCATION_LIMIT) &&
+        (EIGEN_MAX_STATIC_ALIGN_BYTES >
+         0)  // if we cannot allocate on the stack, then let's not bother about this optimization
   };
-  typedef typename internal::conditional<CanAlign, Ref<const Matrix<Scalar,Dynamic,1,0,blockSize,1>, internal::evaluator<VectorTypeCopyClean>::Alignment>,
-                                                   typename VectorTypeCopyClean::ConstSegmentReturnType>::type SegmentWrapper;
+  typedef std::conditional_t<
+      CanAlign,
+      Ref<const Matrix<Scalar, Dynamic, 1, 0, blockSize, 1>, internal::evaluator<VectorTypeCopyClean>::Alignment>,
+      typename VectorTypeCopyClean::ConstSegmentReturnType>
+      SegmentWrapper;
   Index n = vec.size();
-  
+
   Index bi = internal::first_default_aligned(copy);
-  if (bi>0)
-    internal::stable_norm_kernel(copy.head(bi), ssq, scale, invScale);
-  for (; bi<n; bi+=blockSize)
-    internal::stable_norm_kernel(SegmentWrapper(copy.segment(bi,numext::mini(blockSize, n - bi))), ssq, scale, invScale);
+  if (bi > 0) internal::stable_norm_kernel(copy.head(bi), ssq, scale, invScale);
+  for (; bi < n; bi += blockSize)
+    internal::stable_norm_kernel(SegmentWrapper(copy.segment(bi, numext::mini(blockSize, n - bi))), ssq, scale,
+                                 invScale);
 }
 
-template<typename VectorType>
-typename VectorType::RealScalar
-stable_norm_impl(const VectorType &vec, typename enable_if<VectorType::IsVectorAtCompileTime>::type* = 0 )
-{
-  using std::sqrt;
+template <typename VectorType>
+typename VectorType::RealScalar stable_norm_impl(const VectorType& vec,
+                                                 std::enable_if_t<VectorType::IsVectorAtCompileTime>* = 0) {
   using std::abs;
+  using std::sqrt;
 
   Index n = vec.size();
 
-  if(n==1)
-    return abs(vec.coeff(0));
+  if (n == 1) return abs(vec.coeff(0));
 
   typedef typename VectorType::RealScalar RealScalar;
   RealScalar scale(0);
   RealScalar invScale(1);
-  RealScalar ssq(0); // sum of squares
+  RealScalar ssq(0);  // sum of squares
 
   stable_norm_impl_inner_step(vec, ssq, scale, invScale);
-  
+
   return scale * sqrt(ssq);
 }
 
-template<typename MatrixType>
-typename MatrixType::RealScalar
-stable_norm_impl(const MatrixType &mat, typename enable_if<!MatrixType::IsVectorAtCompileTime>::type* = 0 )
-{
+template <typename MatrixType>
+typename MatrixType::RealScalar stable_norm_impl(const MatrixType& mat,
+                                                 std::enable_if_t<!MatrixType::IsVectorAtCompileTime>* = 0) {
   using std::sqrt;
 
   typedef typename MatrixType::RealScalar RealScalar;
   RealScalar scale(0);
   RealScalar invScale(1);
-  RealScalar ssq(0); // sum of squares
+  RealScalar ssq(0);  // sum of squares
 
-  for(Index j=0; j<mat.outerSize(); ++j)
-    stable_norm_impl_inner_step(mat.innerVector(j), ssq, scale, invScale);
+  for (Index j = 0; j < mat.outerSize(); ++j) stable_norm_impl_inner_step(mat.innerVector(j), ssq, scale, invScale);
   return scale * sqrt(ssq);
 }
 
-template<typename Derived>
-inline typename NumTraits<typename traits<Derived>::Scalar>::Real
-blueNorm_impl(const EigenBase<Derived>& _vec)
-{
-  typedef typename Derived::RealScalar RealScalar;  
+template <typename Derived>
+inline typename NumTraits<typename traits<Derived>::Scalar>::Real blueNorm_impl(const EigenBase<Derived>& _vec) {
+  typedef typename Derived::RealScalar RealScalar;
+  using std::abs;
   using std::pow;
   using std::sqrt;
-  using std::abs;
 
   // This program calculates the machine-dependent constants
   // bl, b2, slm, s2m, relerr overfl
@@ -133,15 +128,19 @@
   // are used. For any specific computer, each of the assignment
   // statements can be replaced
   static const int ibeta = std::numeric_limits<RealScalar>::radix;  // base for floating-point numbers
-  static const int it    = NumTraits<RealScalar>::digits();  // number of base-beta digits in mantissa
-  static const int iemin = NumTraits<RealScalar>::min_exponent();  // minimum exponent
-  static const int iemax = NumTraits<RealScalar>::max_exponent();  // maximum exponent
-  static const RealScalar rbig   = NumTraits<RealScalar>::highest();  // largest floating-point number
-  static const RealScalar b1     = RealScalar(pow(RealScalar(ibeta),RealScalar(-((1-iemin)/2))));  // lower boundary of midrange
-  static const RealScalar b2     = RealScalar(pow(RealScalar(ibeta),RealScalar((iemax + 1 - it)/2)));  // upper boundary of midrange
-  static const RealScalar s1m    = RealScalar(pow(RealScalar(ibeta),RealScalar((2-iemin)/2)));  // scaling factor for lower range
-  static const RealScalar s2m    = RealScalar(pow(RealScalar(ibeta),RealScalar(- ((iemax+it)/2))));  // scaling factor for upper range
-  static const RealScalar eps    = RealScalar(pow(double(ibeta), 1-it));
+  static const int it = NumTraits<RealScalar>::digits();            // number of base-beta digits in mantissa
+  static const int iemin = NumTraits<RealScalar>::min_exponent();   // minimum exponent
+  static const int iemax = NumTraits<RealScalar>::max_exponent();   // maximum exponent
+  static const RealScalar rbig = NumTraits<RealScalar>::highest();  // largest floating-point number
+  static const RealScalar b1 =
+      RealScalar(pow(RealScalar(ibeta), RealScalar(-((1 - iemin) / 2))));  // lower boundary of midrange
+  static const RealScalar b2 =
+      RealScalar(pow(RealScalar(ibeta), RealScalar((iemax + 1 - it) / 2)));  // upper boundary of midrange
+  static const RealScalar s1m =
+      RealScalar(pow(RealScalar(ibeta), RealScalar((2 - iemin) / 2)));  // scaling factor for lower range
+  static const RealScalar s2m =
+      RealScalar(pow(RealScalar(ibeta), RealScalar(-((iemax + it) / 2))));  // scaling factor for upper range
+  static const RealScalar eps = RealScalar(pow(double(ibeta), 1 - it));
   static const RealScalar relerr = sqrt(eps);  // tolerance for neglecting asml
 
   const Derived& vec(_vec.derived());
@@ -151,101 +150,87 @@
   RealScalar amed = RealScalar(0);
   RealScalar abig = RealScalar(0);
 
-  for(Index j=0; j<vec.outerSize(); ++j)
-  {
-    for(typename Derived::InnerIterator iter(vec, j); iter; ++iter)
-    {
+  for (Index j = 0; j < vec.outerSize(); ++j) {
+    for (typename Derived::InnerIterator iter(vec, j); iter; ++iter) {
       RealScalar ax = abs(iter.value());
-      if(ax > ab2)     abig += numext::abs2(ax*s2m);
-      else if(ax < b1) asml += numext::abs2(ax*s1m);
-      else             amed += numext::abs2(ax);
+      if (ax > ab2)
+        abig += numext::abs2(ax * s2m);
+      else if (ax < b1)
+        asml += numext::abs2(ax * s1m);
+      else
+        amed += numext::abs2(ax);
     }
   }
-  if(amed!=amed)
-    return amed;  // we got a NaN
-  if(abig > RealScalar(0))
-  {
+  if (amed != amed) return amed;  // we got a NaN
+  if (abig > RealScalar(0)) {
     abig = sqrt(abig);
-    if(abig > rbig) // overflow, or *this contains INF values
-      return abig;  // return INF
-    if(amed > RealScalar(0))
-    {
-      abig = abig/s2m;
+    if (abig > rbig)  // overflow, or *this contains INF values
+      return abig;    // return INF
+    if (amed > RealScalar(0)) {
+      abig = abig / s2m;
       amed = sqrt(amed);
-    }
-    else
-      return abig/s2m;
-  }
-  else if(asml > RealScalar(0))
-  {
-    if (amed > RealScalar(0))
-    {
+    } else
+      return abig / s2m;
+  } else if (asml > RealScalar(0)) {
+    if (amed > RealScalar(0)) {
       abig = sqrt(amed);
       amed = sqrt(asml) / s1m;
-    }
-    else
-      return sqrt(asml)/s1m;
-  }
-  else
+    } else
+      return sqrt(asml) / s1m;
+  } else
     return sqrt(amed);
   asml = numext::mini(abig, amed);
   abig = numext::maxi(abig, amed);
-  if(asml <= abig*relerr)
+  if (asml <= abig * relerr)
     return abig;
   else
-    return abig * sqrt(RealScalar(1) + numext::abs2(asml/abig));
+    return abig * sqrt(RealScalar(1) + numext::abs2(asml / abig));
 }
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \returns the \em l2 norm of \c *this avoiding underflow and overflow.
-  * This version use a blockwise two passes algorithm:
-  *  1 - find the absolute largest coefficient \c s
-  *  2 - compute \f$ s \Vert \frac{*this}{s} \Vert \f$ in a standard way
-  *
-  * For architecture/scalar types supporting vectorization, this version
-  * is faster than blueNorm(). Otherwise the blueNorm() is much faster.
-  *
-  * \sa norm(), blueNorm(), hypotNorm()
-  */
-template<typename Derived>
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
-MatrixBase<Derived>::stableNorm() const
-{
+ * This version use a blockwise two passes algorithm:
+ *  1 - find the absolute largest coefficient \c s
+ *  2 - compute \f$ s \Vert \frac{*this}{s} \Vert \f$ in a standard way
+ *
+ * For architecture/scalar types supporting vectorization, this version
+ * is faster than blueNorm(). Otherwise the blueNorm() is much faster.
+ *
+ * \sa norm(), blueNorm(), hypotNorm()
+ */
+template <typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::stableNorm() const {
   return internal::stable_norm_impl(derived());
 }
 
 /** \returns the \em l2 norm of \c *this using the Blue's algorithm.
-  * A Portable Fortran Program to Find the Euclidean Norm of a Vector,
-  * ACM TOMS, Vol 4, Issue 1, 1978.
-  *
-  * For architecture/scalar types without vectorization, this version
-  * is much faster than stableNorm(). Otherwise the stableNorm() is faster.
-  *
-  * \sa norm(), stableNorm(), hypotNorm()
-  */
-template<typename Derived>
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
-MatrixBase<Derived>::blueNorm() const
-{
+ * A Portable Fortran Program to Find the Euclidean Norm of a Vector,
+ * ACM TOMS, Vol 4, Issue 1, 1978.
+ *
+ * For architecture/scalar types without vectorization, this version
+ * is much faster than stableNorm(). Otherwise the stableNorm() is faster.
+ *
+ * \sa norm(), stableNorm(), hypotNorm()
+ */
+template <typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::blueNorm() const {
   return internal::blueNorm_impl(*this);
 }
 
 /** \returns the \em l2 norm of \c *this avoiding undeflow and overflow.
-  * This version use a concatenation of hypot() calls, and it is very slow.
-  *
-  * \sa norm(), stableNorm()
-  */
-template<typename Derived>
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
-MatrixBase<Derived>::hypotNorm() const
-{
-  if(size()==1)
-    return numext::abs(coeff(0,0));
+ * This version use a concatenation of hypot() calls, and it is very slow.
+ *
+ * \sa norm(), stableNorm()
+ */
+template <typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::hypotNorm() const {
+  if (size() == 1)
+    return numext::abs(coeff(0, 0));
   else
     return this->cwiseAbs().redux(internal::scalar_hypot_op<RealScalar>());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_STABLENORM_H
+#endif  // EIGEN_STABLENORM_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h
index 09041db..3ab7d21 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h
@@ -10,105 +10,175 @@
 #ifndef EIGEN_STLITERATORS_H
 #define EIGEN_STLITERATORS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename IteratorType>
+template <typename IteratorType>
 struct indexed_based_stl_iterator_traits;
 
-template<typename  Derived>
-class indexed_based_stl_iterator_base
-{
-protected:
+template <typename Derived>
+class indexed_based_stl_iterator_base {
+ protected:
   typedef indexed_based_stl_iterator_traits<Derived> traits;
   typedef typename traits::XprType XprType;
   typedef indexed_based_stl_iterator_base<typename traits::non_const_iterator> non_const_iterator;
   typedef indexed_based_stl_iterator_base<typename traits::const_iterator> const_iterator;
-  typedef typename internal::conditional<internal::is_const<XprType>::value,non_const_iterator,const_iterator>::type other_iterator;
+  typedef std::conditional_t<internal::is_const<XprType>::value, non_const_iterator, const_iterator> other_iterator;
   // NOTE: in C++03 we cannot declare friend classes through typedefs because we need to write friend class:
   friend class indexed_based_stl_iterator_base<typename traits::const_iterator>;
   friend class indexed_based_stl_iterator_base<typename traits::non_const_iterator>;
-public:
+
+ public:
   typedef Index difference_type;
   typedef std::random_access_iterator_tag iterator_category;
 
   indexed_based_stl_iterator_base() EIGEN_NO_THROW : mp_xpr(0), m_index(0) {}
   indexed_based_stl_iterator_base(XprType& xpr, Index index) EIGEN_NO_THROW : mp_xpr(&xpr), m_index(index) {}
 
-  indexed_based_stl_iterator_base(const non_const_iterator& other) EIGEN_NO_THROW
-    : mp_xpr(other.mp_xpr), m_index(other.m_index)
-  {}
+  indexed_based_stl_iterator_base(const non_const_iterator& other) EIGEN_NO_THROW : mp_xpr(other.mp_xpr),
+                                                                                    m_index(other.m_index) {}
 
-  indexed_based_stl_iterator_base& operator=(const non_const_iterator& other)
-  {
+  indexed_based_stl_iterator_base& operator=(const non_const_iterator& other) {
     mp_xpr = other.mp_xpr;
     m_index = other.m_index;
     return *this;
   }
 
-  Derived& operator++() { ++m_index; return derived(); }
-  Derived& operator--() { --m_index; return derived(); }
+  Derived& operator++() {
+    ++m_index;
+    return derived();
+  }
+  Derived& operator--() {
+    --m_index;
+    return derived();
+  }
 
-  Derived operator++(int) { Derived prev(derived()); operator++(); return prev;}
-  Derived operator--(int) { Derived prev(derived()); operator--(); return prev;}
+  Derived operator++(int) {
+    Derived prev(derived());
+    operator++();
+    return prev;
+  }
+  Derived operator--(int) {
+    Derived prev(derived());
+    operator--();
+    return prev;
+  }
 
-  friend Derived operator+(const indexed_based_stl_iterator_base& a, Index b) { Derived ret(a.derived()); ret += b; return ret; }
-  friend Derived operator-(const indexed_based_stl_iterator_base& a, Index b) { Derived ret(a.derived()); ret -= b; return ret; }
-  friend Derived operator+(Index a, const indexed_based_stl_iterator_base& b) { Derived ret(b.derived()); ret += a; return ret; }
-  friend Derived operator-(Index a, const indexed_based_stl_iterator_base& b) { Derived ret(b.derived()); ret -= a; return ret; }
-  
-  Derived& operator+=(Index b) { m_index += b; return derived(); }
-  Derived& operator-=(Index b) { m_index -= b; return derived(); }
+  friend Derived operator+(const indexed_based_stl_iterator_base& a, Index b) {
+    Derived ret(a.derived());
+    ret += b;
+    return ret;
+  }
+  friend Derived operator-(const indexed_based_stl_iterator_base& a, Index b) {
+    Derived ret(a.derived());
+    ret -= b;
+    return ret;
+  }
+  friend Derived operator+(Index a, const indexed_based_stl_iterator_base& b) {
+    Derived ret(b.derived());
+    ret += a;
+    return ret;
+  }
+  friend Derived operator-(Index a, const indexed_based_stl_iterator_base& b) {
+    Derived ret(b.derived());
+    ret -= a;
+    return ret;
+  }
 
-  difference_type operator-(const indexed_based_stl_iterator_base& other) const
-  {
+  Derived& operator+=(Index b) {
+    m_index += b;
+    return derived();
+  }
+  Derived& operator-=(Index b) {
+    m_index -= b;
+    return derived();
+  }
+
+  difference_type operator-(const indexed_based_stl_iterator_base& other) const {
     eigen_assert(mp_xpr == other.mp_xpr);
     return m_index - other.m_index;
   }
 
-  difference_type operator-(const other_iterator& other) const
-  {
+  difference_type operator-(const other_iterator& other) const {
     eigen_assert(mp_xpr == other.mp_xpr);
     return m_index - other.m_index;
   }
 
-  bool operator==(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
-  bool operator!=(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
-  bool operator< (const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
-  bool operator<=(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
-  bool operator> (const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
-  bool operator>=(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
+  bool operator==(const indexed_based_stl_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index == other.m_index;
+  }
+  bool operator!=(const indexed_based_stl_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index != other.m_index;
+  }
+  bool operator<(const indexed_based_stl_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index < other.m_index;
+  }
+  bool operator<=(const indexed_based_stl_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index <= other.m_index;
+  }
+  bool operator>(const indexed_based_stl_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index > other.m_index;
+  }
+  bool operator>=(const indexed_based_stl_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index >= other.m_index;
+  }
 
-  bool operator==(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
-  bool operator!=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
-  bool operator< (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
-  bool operator<=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
-  bool operator> (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
-  bool operator>=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
+  bool operator==(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index == other.m_index;
+  }
+  bool operator!=(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index != other.m_index;
+  }
+  bool operator<(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index < other.m_index;
+  }
+  bool operator<=(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index <= other.m_index;
+  }
+  bool operator>(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index > other.m_index;
+  }
+  bool operator>=(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index >= other.m_index;
+  }
 
-protected:
-
+ protected:
   Derived& derived() { return static_cast<Derived&>(*this); }
   const Derived& derived() const { return static_cast<const Derived&>(*this); }
 
-  XprType *mp_xpr;
+  XprType* mp_xpr;
   Index m_index;
 };
 
-template<typename  Derived>
-class indexed_based_stl_reverse_iterator_base
-{
-protected:
+template <typename Derived>
+class indexed_based_stl_reverse_iterator_base {
+ protected:
   typedef indexed_based_stl_iterator_traits<Derived> traits;
   typedef typename traits::XprType XprType;
   typedef indexed_based_stl_reverse_iterator_base<typename traits::non_const_iterator> non_const_iterator;
   typedef indexed_based_stl_reverse_iterator_base<typename traits::const_iterator> const_iterator;
-  typedef typename internal::conditional<internal::is_const<XprType>::value,non_const_iterator,const_iterator>::type other_iterator;
+  typedef std::conditional_t<internal::is_const<XprType>::value, non_const_iterator, const_iterator> other_iterator;
   // NOTE: in C++03 we cannot declare friend classes through typedefs because we need to write friend class:
   friend class indexed_based_stl_reverse_iterator_base<typename traits::const_iterator>;
   friend class indexed_based_stl_reverse_iterator_base<typename traits::non_const_iterator>;
-public:
+
+ public:
   typedef Index difference_type;
   typedef std::random_access_iterator_tag iterator_category;
 
@@ -116,165 +186,259 @@
   indexed_based_stl_reverse_iterator_base(XprType& xpr, Index index) : mp_xpr(&xpr), m_index(index) {}
 
   indexed_based_stl_reverse_iterator_base(const non_const_iterator& other)
-    : mp_xpr(other.mp_xpr), m_index(other.m_index)
-  {}
+      : mp_xpr(other.mp_xpr), m_index(other.m_index) {}
 
-  indexed_based_stl_reverse_iterator_base& operator=(const non_const_iterator& other)
-  {
+  indexed_based_stl_reverse_iterator_base& operator=(const non_const_iterator& other) {
     mp_xpr = other.mp_xpr;
     m_index = other.m_index;
     return *this;
   }
 
-  Derived& operator++() { --m_index; return derived(); }
-  Derived& operator--() { ++m_index; return derived(); }
+  Derived& operator++() {
+    --m_index;
+    return derived();
+  }
+  Derived& operator--() {
+    ++m_index;
+    return derived();
+  }
 
-  Derived operator++(int) { Derived prev(derived()); operator++(); return prev;}
-  Derived operator--(int) { Derived prev(derived()); operator--(); return prev;}
+  Derived operator++(int) {
+    Derived prev(derived());
+    operator++();
+    return prev;
+  }
+  Derived operator--(int) {
+    Derived prev(derived());
+    operator--();
+    return prev;
+  }
 
-  friend Derived operator+(const indexed_based_stl_reverse_iterator_base& a, Index b) { Derived ret(a.derived()); ret += b; return ret; }
-  friend Derived operator-(const indexed_based_stl_reverse_iterator_base& a, Index b) { Derived ret(a.derived()); ret -= b; return ret; }
-  friend Derived operator+(Index a, const indexed_based_stl_reverse_iterator_base& b) { Derived ret(b.derived()); ret += a; return ret; }
-  friend Derived operator-(Index a, const indexed_based_stl_reverse_iterator_base& b) { Derived ret(b.derived()); ret -= a; return ret; }
-  
-  Derived& operator+=(Index b) { m_index -= b; return derived(); }
-  Derived& operator-=(Index b) { m_index += b; return derived(); }
+  friend Derived operator+(const indexed_based_stl_reverse_iterator_base& a, Index b) {
+    Derived ret(a.derived());
+    ret += b;
+    return ret;
+  }
+  friend Derived operator-(const indexed_based_stl_reverse_iterator_base& a, Index b) {
+    Derived ret(a.derived());
+    ret -= b;
+    return ret;
+  }
+  friend Derived operator+(Index a, const indexed_based_stl_reverse_iterator_base& b) {
+    Derived ret(b.derived());
+    ret += a;
+    return ret;
+  }
+  friend Derived operator-(Index a, const indexed_based_stl_reverse_iterator_base& b) {
+    Derived ret(b.derived());
+    ret -= a;
+    return ret;
+  }
 
-  difference_type operator-(const indexed_based_stl_reverse_iterator_base& other) const
-  {
+  Derived& operator+=(Index b) {
+    m_index -= b;
+    return derived();
+  }
+  Derived& operator-=(Index b) {
+    m_index += b;
+    return derived();
+  }
+
+  difference_type operator-(const indexed_based_stl_reverse_iterator_base& other) const {
     eigen_assert(mp_xpr == other.mp_xpr);
     return other.m_index - m_index;
   }
 
-  difference_type operator-(const other_iterator& other) const
-  {
+  difference_type operator-(const other_iterator& other) const {
     eigen_assert(mp_xpr == other.mp_xpr);
     return other.m_index - m_index;
   }
 
-  bool operator==(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
-  bool operator!=(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
-  bool operator< (const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
-  bool operator<=(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
-  bool operator> (const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
-  bool operator>=(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
+  bool operator==(const indexed_based_stl_reverse_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index == other.m_index;
+  }
+  bool operator!=(const indexed_based_stl_reverse_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index != other.m_index;
+  }
+  bool operator<(const indexed_based_stl_reverse_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index > other.m_index;
+  }
+  bool operator<=(const indexed_based_stl_reverse_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index >= other.m_index;
+  }
+  bool operator>(const indexed_based_stl_reverse_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index < other.m_index;
+  }
+  bool operator>=(const indexed_based_stl_reverse_iterator_base& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index <= other.m_index;
+  }
 
-  bool operator==(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
-  bool operator!=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
-  bool operator< (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
-  bool operator<=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
-  bool operator> (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
-  bool operator>=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
+  bool operator==(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index == other.m_index;
+  }
+  bool operator!=(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index != other.m_index;
+  }
+  bool operator<(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index > other.m_index;
+  }
+  bool operator<=(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index >= other.m_index;
+  }
+  bool operator>(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index < other.m_index;
+  }
+  bool operator>=(const other_iterator& other) const {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index <= other.m_index;
+  }
 
-protected:
-
+ protected:
   Derived& derived() { return static_cast<Derived&>(*this); }
   const Derived& derived() const { return static_cast<const Derived&>(*this); }
 
-  XprType *mp_xpr;
+  XprType* mp_xpr;
   Index m_index;
 };
 
-template<typename XprType>
-class pointer_based_stl_iterator
-{
-  enum { is_lvalue  = internal::is_lvalue<XprType>::value };
-  typedef pointer_based_stl_iterator<typename internal::remove_const<XprType>::type> non_const_iterator;
-  typedef pointer_based_stl_iterator<typename internal::add_const<XprType>::type> const_iterator;
-  typedef typename internal::conditional<internal::is_const<XprType>::value,non_const_iterator,const_iterator>::type other_iterator;
+template <typename XprType>
+class pointer_based_stl_iterator {
+  enum { is_lvalue = internal::is_lvalue<XprType>::value };
+  typedef pointer_based_stl_iterator<std::remove_const_t<XprType>> non_const_iterator;
+  typedef pointer_based_stl_iterator<std::add_const_t<XprType>> const_iterator;
+  typedef std::conditional_t<internal::is_const<XprType>::value, non_const_iterator, const_iterator> other_iterator;
   // NOTE: in C++03 we cannot declare friend classes through typedefs because we need to write friend class:
-  friend class pointer_based_stl_iterator<typename internal::add_const<XprType>::type>;
-  friend class pointer_based_stl_iterator<typename internal::remove_const<XprType>::type>;
-public:
+  friend class pointer_based_stl_iterator<std::add_const_t<XprType>>;
+  friend class pointer_based_stl_iterator<std::remove_const_t<XprType>>;
+
+ public:
   typedef Index difference_type;
   typedef typename XprType::Scalar value_type;
   typedef std::random_access_iterator_tag iterator_category;
-  typedef typename internal::conditional<bool(is_lvalue), value_type*, const value_type*>::type pointer;
-  typedef typename internal::conditional<bool(is_lvalue), value_type&, const value_type&>::type reference;
-
+  typedef std::conditional_t<bool(is_lvalue), value_type*, const value_type*> pointer;
+  typedef std::conditional_t<bool(is_lvalue), value_type&, const value_type&> reference;
 
   pointer_based_stl_iterator() EIGEN_NO_THROW : m_ptr(0) {}
-  pointer_based_stl_iterator(XprType& xpr, Index index) EIGEN_NO_THROW : m_incr(xpr.innerStride())
-  {
+  pointer_based_stl_iterator(XprType& xpr, Index index) EIGEN_NO_THROW : m_incr(xpr.innerStride()) {
     m_ptr = xpr.data() + index * m_incr.value();
   }
 
-  pointer_based_stl_iterator(const non_const_iterator& other) EIGEN_NO_THROW
-    : m_ptr(other.m_ptr), m_incr(other.m_incr)
-  {}
+  pointer_based_stl_iterator(const non_const_iterator& other) EIGEN_NO_THROW : m_ptr(other.m_ptr),
+                                                                               m_incr(other.m_incr) {}
 
-  pointer_based_stl_iterator& operator=(const non_const_iterator& other) EIGEN_NO_THROW
-  {
+  pointer_based_stl_iterator& operator=(const non_const_iterator& other) EIGEN_NO_THROW {
     m_ptr = other.m_ptr;
     m_incr.setValue(other.m_incr);
     return *this;
   }
 
-  reference operator*()         const { return *m_ptr;   }
-  reference operator[](Index i) const { return *(m_ptr+i*m_incr.value()); }
-  pointer   operator->()        const { return m_ptr;    }
+  reference operator*() const { return *m_ptr; }
+  reference operator[](Index i) const { return *(m_ptr + i * m_incr.value()); }
+  pointer operator->() const { return m_ptr; }
 
-  pointer_based_stl_iterator& operator++() { m_ptr += m_incr.value(); return *this; }
-  pointer_based_stl_iterator& operator--() { m_ptr -= m_incr.value(); return *this; }
+  pointer_based_stl_iterator& operator++() {
+    m_ptr += m_incr.value();
+    return *this;
+  }
+  pointer_based_stl_iterator& operator--() {
+    m_ptr -= m_incr.value();
+    return *this;
+  }
 
-  pointer_based_stl_iterator operator++(int) { pointer_based_stl_iterator prev(*this); operator++(); return prev;}
-  pointer_based_stl_iterator operator--(int) { pointer_based_stl_iterator prev(*this); operator--(); return prev;}
+  pointer_based_stl_iterator operator++(int) {
+    pointer_based_stl_iterator prev(*this);
+    operator++();
+    return prev;
+  }
+  pointer_based_stl_iterator operator--(int) {
+    pointer_based_stl_iterator prev(*this);
+    operator--();
+    return prev;
+  }
 
-  friend pointer_based_stl_iterator operator+(const pointer_based_stl_iterator& a, Index b) { pointer_based_stl_iterator ret(a); ret += b; return ret; }
-  friend pointer_based_stl_iterator operator-(const pointer_based_stl_iterator& a, Index b) { pointer_based_stl_iterator ret(a); ret -= b; return ret; }
-  friend pointer_based_stl_iterator operator+(Index a, const pointer_based_stl_iterator& b) { pointer_based_stl_iterator ret(b); ret += a; return ret; }
-  friend pointer_based_stl_iterator operator-(Index a, const pointer_based_stl_iterator& b) { pointer_based_stl_iterator ret(b); ret -= a; return ret; }
-  
-  pointer_based_stl_iterator& operator+=(Index b) { m_ptr += b*m_incr.value(); return *this; }
-  pointer_based_stl_iterator& operator-=(Index b) { m_ptr -= b*m_incr.value(); return *this; }
+  friend pointer_based_stl_iterator operator+(const pointer_based_stl_iterator& a, Index b) {
+    pointer_based_stl_iterator ret(a);
+    ret += b;
+    return ret;
+  }
+  friend pointer_based_stl_iterator operator-(const pointer_based_stl_iterator& a, Index b) {
+    pointer_based_stl_iterator ret(a);
+    ret -= b;
+    return ret;
+  }
+  friend pointer_based_stl_iterator operator+(Index a, const pointer_based_stl_iterator& b) {
+    pointer_based_stl_iterator ret(b);
+    ret += a;
+    return ret;
+  }
+  friend pointer_based_stl_iterator operator-(Index a, const pointer_based_stl_iterator& b) {
+    pointer_based_stl_iterator ret(b);
+    ret -= a;
+    return ret;
+  }
+
+  pointer_based_stl_iterator& operator+=(Index b) {
+    m_ptr += b * m_incr.value();
+    return *this;
+  }
+  pointer_based_stl_iterator& operator-=(Index b) {
+    m_ptr -= b * m_incr.value();
+    return *this;
+  }
 
   difference_type operator-(const pointer_based_stl_iterator& other) const {
-    return (m_ptr - other.m_ptr)/m_incr.value();
+    return (m_ptr - other.m_ptr) / m_incr.value();
   }
 
-  difference_type operator-(const other_iterator& other) const {
-    return (m_ptr - other.m_ptr)/m_incr.value();
-  }
+  difference_type operator-(const other_iterator& other) const { return (m_ptr - other.m_ptr) / m_incr.value(); }
 
   bool operator==(const pointer_based_stl_iterator& other) const { return m_ptr == other.m_ptr; }
   bool operator!=(const pointer_based_stl_iterator& other) const { return m_ptr != other.m_ptr; }
-  bool operator< (const pointer_based_stl_iterator& other) const { return m_ptr <  other.m_ptr; }
+  bool operator<(const pointer_based_stl_iterator& other) const { return m_ptr < other.m_ptr; }
   bool operator<=(const pointer_based_stl_iterator& other) const { return m_ptr <= other.m_ptr; }
-  bool operator> (const pointer_based_stl_iterator& other) const { return m_ptr >  other.m_ptr; }
+  bool operator>(const pointer_based_stl_iterator& other) const { return m_ptr > other.m_ptr; }
   bool operator>=(const pointer_based_stl_iterator& other) const { return m_ptr >= other.m_ptr; }
 
   bool operator==(const other_iterator& other) const { return m_ptr == other.m_ptr; }
   bool operator!=(const other_iterator& other) const { return m_ptr != other.m_ptr; }
-  bool operator< (const other_iterator& other) const { return m_ptr <  other.m_ptr; }
+  bool operator<(const other_iterator& other) const { return m_ptr < other.m_ptr; }
   bool operator<=(const other_iterator& other) const { return m_ptr <= other.m_ptr; }
-  bool operator> (const other_iterator& other) const { return m_ptr >  other.m_ptr; }
+  bool operator>(const other_iterator& other) const { return m_ptr > other.m_ptr; }
   bool operator>=(const other_iterator& other) const { return m_ptr >= other.m_ptr; }
 
-protected:
-
+ protected:
   pointer m_ptr;
   internal::variable_if_dynamic<Index, XprType::InnerStrideAtCompileTime> m_incr;
 };
 
-template<typename _XprType>
-struct indexed_based_stl_iterator_traits<generic_randaccess_stl_iterator<_XprType> >
-{
-  typedef _XprType XprType;
-  typedef generic_randaccess_stl_iterator<typename internal::remove_const<XprType>::type> non_const_iterator;
-  typedef generic_randaccess_stl_iterator<typename internal::add_const<XprType>::type> const_iterator;
+template <typename XprType_>
+struct indexed_based_stl_iterator_traits<generic_randaccess_stl_iterator<XprType_>> {
+  typedef XprType_ XprType;
+  typedef generic_randaccess_stl_iterator<std::remove_const_t<XprType>> non_const_iterator;
+  typedef generic_randaccess_stl_iterator<std::add_const_t<XprType>> const_iterator;
 };
 
-template<typename XprType>
-class generic_randaccess_stl_iterator : public indexed_based_stl_iterator_base<generic_randaccess_stl_iterator<XprType> >
-{
-public:
+template <typename XprType>
+class generic_randaccess_stl_iterator
+    : public indexed_based_stl_iterator_base<generic_randaccess_stl_iterator<XprType>> {
+ public:
   typedef typename XprType::Scalar value_type;
 
-protected:
-
+ protected:
   enum {
     has_direct_access = (internal::traits<XprType>::Flags & DirectAccessBit) ? 1 : 0,
-    is_lvalue  = internal::is_lvalue<XprType>::value
+    is_lvalue = internal::is_lvalue<XprType>::value
   };
 
   typedef indexed_based_stl_iterator_base<generic_randaccess_stl_iterator> Base;
@@ -283,181 +447,168 @@
 
   // TODO currently const Transpose/Reshape expressions never returns const references,
   // so lets return by value too.
-  //typedef typename internal::conditional<bool(has_direct_access), const value_type&, const value_type>::type read_only_ref_t;
+  // typedef std::conditional_t<bool(has_direct_access), const value_type&, const value_type> read_only_ref_t;
   typedef const value_type read_only_ref_t;
 
-public:
-  
-  typedef typename internal::conditional<bool(is_lvalue), value_type *, const value_type *>::type pointer;
-  typedef typename internal::conditional<bool(is_lvalue), value_type&, read_only_ref_t>::type reference;
-  
+ public:
+  typedef std::conditional_t<bool(is_lvalue), value_type*, const value_type*> pointer;
+  typedef std::conditional_t<bool(is_lvalue), value_type&, read_only_ref_t> reference;
+
   generic_randaccess_stl_iterator() : Base() {}
-  generic_randaccess_stl_iterator(XprType& xpr, Index index) : Base(xpr,index) {}
+  generic_randaccess_stl_iterator(XprType& xpr, Index index) : Base(xpr, index) {}
   generic_randaccess_stl_iterator(const typename Base::non_const_iterator& other) : Base(other) {}
   using Base::operator=;
 
-  reference operator*()         const { return   (*mp_xpr)(m_index);   }
-  reference operator[](Index i) const { return   (*mp_xpr)(m_index+i); }
-  pointer   operator->()        const { return &((*mp_xpr)(m_index)); }
+  reference operator*() const { return (*mp_xpr)(m_index); }
+  reference operator[](Index i) const { return (*mp_xpr)(m_index + i); }
+  pointer operator->() const { return &((*mp_xpr)(m_index)); }
 };
 
-template<typename _XprType, DirectionType Direction>
-struct indexed_based_stl_iterator_traits<subvector_stl_iterator<_XprType,Direction> >
-{
-  typedef _XprType XprType;
-  typedef subvector_stl_iterator<typename internal::remove_const<XprType>::type, Direction> non_const_iterator;
-  typedef subvector_stl_iterator<typename internal::add_const<XprType>::type, Direction> const_iterator;
+template <typename XprType_, DirectionType Direction>
+struct indexed_based_stl_iterator_traits<subvector_stl_iterator<XprType_, Direction>> {
+  typedef XprType_ XprType;
+  typedef subvector_stl_iterator<std::remove_const_t<XprType>, Direction> non_const_iterator;
+  typedef subvector_stl_iterator<std::add_const_t<XprType>, Direction> const_iterator;
 };
 
-template<typename XprType, DirectionType Direction>
-class subvector_stl_iterator : public indexed_based_stl_iterator_base<subvector_stl_iterator<XprType,Direction> >
-{
-protected:
-
-  enum { is_lvalue  = internal::is_lvalue<XprType>::value };
+template <typename XprType, DirectionType Direction>
+class subvector_stl_iterator : public indexed_based_stl_iterator_base<subvector_stl_iterator<XprType, Direction>> {
+ protected:
+  enum { is_lvalue = internal::is_lvalue<XprType>::value };
 
   typedef indexed_based_stl_iterator_base<subvector_stl_iterator> Base;
   using Base::m_index;
   using Base::mp_xpr;
 
-  typedef typename internal::conditional<Direction==Vertical,typename XprType::ColXpr,typename XprType::RowXpr>::type SubVectorType;
-  typedef typename internal::conditional<Direction==Vertical,typename XprType::ConstColXpr,typename XprType::ConstRowXpr>::type ConstSubVectorType;
+  typedef std::conditional_t<Direction == Vertical, typename XprType::ColXpr, typename XprType::RowXpr> SubVectorType;
+  typedef std::conditional_t<Direction == Vertical, typename XprType::ConstColXpr, typename XprType::ConstRowXpr>
+      ConstSubVectorType;
 
-
-public:
-  typedef typename internal::conditional<bool(is_lvalue), SubVectorType, ConstSubVectorType>::type reference;
+ public:
+  typedef std::conditional_t<bool(is_lvalue), SubVectorType, ConstSubVectorType> reference;
   typedef typename reference::PlainObject value_type;
 
-private:
-  class subvector_stl_iterator_ptr
-  {
-  public:
-      subvector_stl_iterator_ptr(const reference &subvector) : m_subvector(subvector) {}
-      reference* operator->() { return &m_subvector; }
-  private:
-      reference m_subvector;
+ private:
+  class subvector_stl_iterator_ptr {
+   public:
+    subvector_stl_iterator_ptr(const reference& subvector) : m_subvector(subvector) {}
+    reference* operator->() { return &m_subvector; }
+
+   private:
+    reference m_subvector;
   };
-public:
 
+ public:
   typedef subvector_stl_iterator_ptr pointer;
-  
+
   subvector_stl_iterator() : Base() {}
-  subvector_stl_iterator(XprType& xpr, Index index) : Base(xpr,index) {}
+  subvector_stl_iterator(XprType& xpr, Index index) : Base(xpr, index) {}
 
-  reference operator*()         const { return (*mp_xpr).template subVector<Direction>(m_index); }
-  reference operator[](Index i) const { return (*mp_xpr).template subVector<Direction>(m_index+i); }
-  pointer   operator->()        const { return (*mp_xpr).template subVector<Direction>(m_index); }
+  reference operator*() const { return (*mp_xpr).template subVector<Direction>(m_index); }
+  reference operator[](Index i) const { return (*mp_xpr).template subVector<Direction>(m_index + i); }
+  pointer operator->() const { return (*mp_xpr).template subVector<Direction>(m_index); }
 };
 
-template<typename _XprType, DirectionType Direction>
-struct indexed_based_stl_iterator_traits<subvector_stl_reverse_iterator<_XprType,Direction> >
-{
-  typedef _XprType XprType;
-  typedef subvector_stl_reverse_iterator<typename internal::remove_const<XprType>::type, Direction> non_const_iterator;
-  typedef subvector_stl_reverse_iterator<typename internal::add_const<XprType>::type, Direction> const_iterator;
+template <typename XprType_, DirectionType Direction>
+struct indexed_based_stl_iterator_traits<subvector_stl_reverse_iterator<XprType_, Direction>> {
+  typedef XprType_ XprType;
+  typedef subvector_stl_reverse_iterator<std::remove_const_t<XprType>, Direction> non_const_iterator;
+  typedef subvector_stl_reverse_iterator<std::add_const_t<XprType>, Direction> const_iterator;
 };
 
-template<typename XprType, DirectionType Direction>
-class subvector_stl_reverse_iterator : public indexed_based_stl_reverse_iterator_base<subvector_stl_reverse_iterator<XprType,Direction> >
-{
-protected:
-
-  enum { is_lvalue  = internal::is_lvalue<XprType>::value };
+template <typename XprType, DirectionType Direction>
+class subvector_stl_reverse_iterator
+    : public indexed_based_stl_reverse_iterator_base<subvector_stl_reverse_iterator<XprType, Direction>> {
+ protected:
+  enum { is_lvalue = internal::is_lvalue<XprType>::value };
 
   typedef indexed_based_stl_reverse_iterator_base<subvector_stl_reverse_iterator> Base;
   using Base::m_index;
   using Base::mp_xpr;
 
-  typedef typename internal::conditional<Direction==Vertical,typename XprType::ColXpr,typename XprType::RowXpr>::type SubVectorType;
-  typedef typename internal::conditional<Direction==Vertical,typename XprType::ConstColXpr,typename XprType::ConstRowXpr>::type ConstSubVectorType;
+  typedef std::conditional_t<Direction == Vertical, typename XprType::ColXpr, typename XprType::RowXpr> SubVectorType;
+  typedef std::conditional_t<Direction == Vertical, typename XprType::ConstColXpr, typename XprType::ConstRowXpr>
+      ConstSubVectorType;
 
-
-public:
-  typedef typename internal::conditional<bool(is_lvalue), SubVectorType, ConstSubVectorType>::type reference;
+ public:
+  typedef std::conditional_t<bool(is_lvalue), SubVectorType, ConstSubVectorType> reference;
   typedef typename reference::PlainObject value_type;
 
-private:
-  class subvector_stl_reverse_iterator_ptr
-  {
-  public:
-      subvector_stl_reverse_iterator_ptr(const reference &subvector) : m_subvector(subvector) {}
-      reference* operator->() { return &m_subvector; }
-  private:
-      reference m_subvector;
+ private:
+  class subvector_stl_reverse_iterator_ptr {
+   public:
+    subvector_stl_reverse_iterator_ptr(const reference& subvector) : m_subvector(subvector) {}
+    reference* operator->() { return &m_subvector; }
+
+   private:
+    reference m_subvector;
   };
-public:
 
+ public:
   typedef subvector_stl_reverse_iterator_ptr pointer;
-  
-  subvector_stl_reverse_iterator() : Base() {}
-  subvector_stl_reverse_iterator(XprType& xpr, Index index) : Base(xpr,index) {}
 
-  reference operator*()         const { return (*mp_xpr).template subVector<Direction>(m_index); }
-  reference operator[](Index i) const { return (*mp_xpr).template subVector<Direction>(m_index+i); }
-  pointer   operator->()        const { return (*mp_xpr).template subVector<Direction>(m_index); }
+  subvector_stl_reverse_iterator() : Base() {}
+  subvector_stl_reverse_iterator(XprType& xpr, Index index) : Base(xpr, index) {}
+
+  reference operator*() const { return (*mp_xpr).template subVector<Direction>(m_index); }
+  reference operator[](Index i) const { return (*mp_xpr).template subVector<Direction>(m_index + i); }
+  pointer operator->() const { return (*mp_xpr).template subVector<Direction>(m_index); }
 };
 
-} // namespace internal
-
+}  // namespace internal
 
 /** returns an iterator to the first element of the 1D vector or array
-  * \only_for_vectors
-  * \sa end(), cbegin()
-  */
-template<typename Derived>
-inline typename DenseBase<Derived>::iterator DenseBase<Derived>::begin()
-{
+ * \only_for_vectors
+ * \sa end(), cbegin()
+ */
+template <typename Derived>
+inline typename DenseBase<Derived>::iterator DenseBase<Derived>::begin() {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
   return iterator(derived(), 0);
 }
 
 /** const version of begin() */
-template<typename Derived>
-inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::begin() const
-{
+template <typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::begin() const {
   return cbegin();
 }
 
 /** returns a read-only const_iterator to the first element of the 1D vector or array
-  * \only_for_vectors
-  * \sa cend(), begin()
-  */
-template<typename Derived>
-inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::cbegin() const
-{
+ * \only_for_vectors
+ * \sa cend(), begin()
+ */
+template <typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::cbegin() const {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
   return const_iterator(derived(), 0);
 }
 
 /** returns an iterator to the element following the last element of the 1D vector or array
-  * \only_for_vectors
-  * \sa begin(), cend()
-  */
-template<typename Derived>
-inline typename DenseBase<Derived>::iterator DenseBase<Derived>::end()
-{
+ * \only_for_vectors
+ * \sa begin(), cend()
+ */
+template <typename Derived>
+inline typename DenseBase<Derived>::iterator DenseBase<Derived>::end() {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
   return iterator(derived(), size());
 }
 
 /** const version of end() */
-template<typename Derived>
-inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::end() const
-{
+template <typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::end() const {
   return cend();
 }
 
 /** returns a read-only const_iterator to the element following the last element of the 1D vector or array
-  * \only_for_vectors
-  * \sa begin(), cend()
-  */
-template<typename Derived>
-inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::cend() const
-{
+ * \only_for_vectors
+ * \sa begin(), cend()
+ */
+template <typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::cend() const {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
   return const_iterator(derived(), size());
 }
 
-} // namespace Eigen
+}  // namespace Eigen
 
-#endif // EIGEN_STLITERATORS_H
+#endif  // EIGEN_STLITERATORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h
index 6494d51..a8fdeaf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h
@@ -10,107 +10,98 @@
 #ifndef EIGEN_STRIDE_H
 #define EIGEN_STRIDE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class Stride
-  * \ingroup Core_Module
-  *
-  * \brief Holds strides information for Map
-  *
-  * This class holds the strides information for mapping arrays with strides with class Map.
-  *
-  * It holds two values: the inner stride and the outer stride.
-  *
-  * The inner stride is the pointer increment between two consecutive entries within a given row of a
-  * row-major matrix or within a given column of a column-major matrix.
-  *
-  * The outer stride is the pointer increment between two consecutive rows of a row-major matrix or
-  * between two consecutive columns of a column-major matrix.
-  *
-  * These two values can be passed either at compile-time as template parameters, or at runtime as
-  * arguments to the constructor.
-  *
-  * Indeed, this class takes two template parameters:
-  *  \tparam _OuterStrideAtCompileTime the outer stride, or Dynamic if you want to specify it at runtime.
-  *  \tparam _InnerStrideAtCompileTime the inner stride, or Dynamic if you want to specify it at runtime.
-  *
-  * Here is an example:
-  * \include Map_general_stride.cpp
-  * Output: \verbinclude Map_general_stride.out
-  *
-  * Both strides can be negative, however, a negative stride of -1 cannot be specified at compiletime
-  * because of the ambiguity with Dynamic which is defined to -1 (historically, negative strides were
-  * not allowed).
-  *
-  * \sa class InnerStride, class OuterStride, \ref TopicStorageOrders
-  */
-template<int _OuterStrideAtCompileTime, int _InnerStrideAtCompileTime>
-class Stride
-{
-  public:
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    enum {
-      InnerStrideAtCompileTime = _InnerStrideAtCompileTime,
-      OuterStrideAtCompileTime = _OuterStrideAtCompileTime
-    };
+ * \ingroup Core_Module
+ *
+ * \brief Holds strides information for Map
+ *
+ * This class holds the strides information for mapping arrays with strides with class Map.
+ *
+ * It holds two values: the inner stride and the outer stride.
+ *
+ * The inner stride is the pointer increment between two consecutive entries within a given row of a
+ * row-major matrix or within a given column of a column-major matrix.
+ *
+ * The outer stride is the pointer increment between two consecutive rows of a row-major matrix or
+ * between two consecutive columns of a column-major matrix.
+ *
+ * These two values can be passed either at compile-time as template parameters, or at runtime as
+ * arguments to the constructor.
+ *
+ * Indeed, this class takes two template parameters:
+ *  \tparam OuterStrideAtCompileTime_ the outer stride, or Dynamic if you want to specify it at runtime.
+ *  \tparam InnerStrideAtCompileTime_ the inner stride, or Dynamic if you want to specify it at runtime.
+ *
+ * Here is an example:
+ * \include Map_general_stride.cpp
+ * Output: \verbinclude Map_general_stride.out
+ *
+ * Both strides can be negative. However, a negative stride of -1 cannot be specified at compile time
+ * because of the ambiguity with Dynamic which is defined to -1 (historically, negative strides were
+ * not allowed).
+ *
+ * Note that for compile-time vectors (ColsAtCompileTime==1 or RowsAtCompile==1),
+ * the inner stride is the pointer increment between two consecutive elements,
+ * regardless of storage layout.
+ *
+ * \sa class InnerStride, class OuterStride, \ref TopicStorageOrders
+ */
+template <int OuterStrideAtCompileTime_, int InnerStrideAtCompileTime_>
+class Stride {
+ public:
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
+  enum { InnerStrideAtCompileTime = InnerStrideAtCompileTime_, OuterStrideAtCompileTime = OuterStrideAtCompileTime_ };
 
-    /** Default constructor, for use when strides are fixed at compile time */
-    EIGEN_DEVICE_FUNC
-    Stride()
-      : m_outer(OuterStrideAtCompileTime), m_inner(InnerStrideAtCompileTime)
-    {
-      // FIXME: for Eigen 4 we should use DynamicIndex instead of Dynamic.
-      // FIXME: for Eigen 4 we should also unify this API with fix<>
-      eigen_assert(InnerStrideAtCompileTime != Dynamic && OuterStrideAtCompileTime != Dynamic);
-    }
+  /** Default constructor, for use when strides are fixed at compile time */
+  EIGEN_DEVICE_FUNC Stride() : m_outer(OuterStrideAtCompileTime), m_inner(InnerStrideAtCompileTime) {
+    // FIXME: for Eigen 4 we should use DynamicIndex instead of Dynamic.
+    // FIXME: for Eigen 4 we should also unify this API with fix<>
+    eigen_assert(InnerStrideAtCompileTime != Dynamic && OuterStrideAtCompileTime != Dynamic);
+  }
 
-    /** Constructor allowing to pass the strides at runtime */
-    EIGEN_DEVICE_FUNC
-    Stride(Index outerStride, Index innerStride)
-      : m_outer(outerStride), m_inner(innerStride)
-    {
-    }
+  /** Constructor allowing to pass the strides at runtime */
+  EIGEN_DEVICE_FUNC Stride(Index outerStride, Index innerStride) : m_outer(outerStride), m_inner(innerStride) {}
 
-    /** Copy constructor */
-    EIGEN_DEVICE_FUNC
-    Stride(const Stride& other)
-      : m_outer(other.outer()), m_inner(other.inner())
-    {}
+  /** Copy constructor */
+  EIGEN_DEVICE_FUNC Stride(const Stride& other) : m_outer(other.outer()), m_inner(other.inner()) {}
 
-    /** \returns the outer stride */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outer() const { return m_outer.value(); }
-    /** \returns the inner stride */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index inner() const { return m_inner.value(); }
+  /** \returns the outer stride */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outer() const { return m_outer.value(); }
+  /** \returns the inner stride */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index inner() const { return m_inner.value(); }
 
-  protected:
-    internal::variable_if_dynamic<Index, OuterStrideAtCompileTime> m_outer;
-    internal::variable_if_dynamic<Index, InnerStrideAtCompileTime> m_inner;
+ protected:
+  internal::variable_if_dynamic<Index, OuterStrideAtCompileTime> m_outer;
+  internal::variable_if_dynamic<Index, InnerStrideAtCompileTime> m_inner;
 };
 
 /** \brief Convenience specialization of Stride to specify only an inner stride
-  * See class Map for some examples */
-template<int Value>
-class InnerStride : public Stride<0, Value>
-{
-    typedef Stride<0, Value> Base;
-  public:
-    EIGEN_DEVICE_FUNC InnerStride() : Base() {}
-    EIGEN_DEVICE_FUNC InnerStride(Index v) : Base(0, v) {} // FIXME making this explicit could break valid code
+ * See class Map for some examples */
+template <int Value>
+class InnerStride : public Stride<0, Value> {
+  typedef Stride<0, Value> Base;
+
+ public:
+  EIGEN_DEVICE_FUNC InnerStride() : Base() {}
+  EIGEN_DEVICE_FUNC InnerStride(Index v) : Base(0, v) {}  // FIXME making this explicit could break valid code
 };
 
 /** \brief Convenience specialization of Stride to specify only an outer stride
-  * See class Map for some examples */
-template<int Value>
-class OuterStride : public Stride<Value, 0>
-{
-    typedef Stride<Value, 0> Base;
-  public:
-    EIGEN_DEVICE_FUNC OuterStride() : Base() {}
-    EIGEN_DEVICE_FUNC OuterStride(Index v) : Base(v,0) {} // FIXME making this explicit could break valid code
+ * See class Map for some examples */
+template <int Value>
+class OuterStride : public Stride<Value, 0> {
+  typedef Stride<Value, 0> Base;
+
+ public:
+  EIGEN_DEVICE_FUNC OuterStride() : Base() {}
+  EIGEN_DEVICE_FUNC OuterStride(Index v) : Base(v, 0) {}  // FIXME making this explicit could break valid code
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_STRIDE_H
+#endif  // EIGEN_STRIDE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h
index 180a4e5..d417c1a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h
@@ -10,59 +10,65 @@
 #ifndef EIGEN_SWAP_H
 #define EIGEN_SWAP_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 // Overload default assignPacket behavior for swapping them
-template<typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT>
-class generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, swap_assign_op<typename DstEvaluatorTypeT::Scalar>, Specialized>
- : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, swap_assign_op<typename DstEvaluatorTypeT::Scalar>, BuiltIn>
-{
-protected:
-  typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, swap_assign_op<typename DstEvaluatorTypeT::Scalar>, BuiltIn> Base;
+template <typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT>
+class generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT,
+                                      swap_assign_op<typename DstEvaluatorTypeT::Scalar>, Specialized>
+    : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT,
+                                             swap_assign_op<typename DstEvaluatorTypeT::Scalar>, BuiltIn> {
+ protected:
+  typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT,
+                                          swap_assign_op<typename DstEvaluatorTypeT::Scalar>, BuiltIn>
+      Base;
   using Base::m_dst;
-  using Base::m_src;
   using Base::m_functor;
-  
-public:
+  using Base::m_src;
+
+ public:
   typedef typename Base::Scalar Scalar;
   typedef typename Base::DstXprType DstXprType;
   typedef swap_assign_op<Scalar> Functor;
-  
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  generic_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr)
-    : Base(dst, src, func, dstExpr)
-  {}
-  
-  template<int StoreMode, int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE void assignPacket(Index row, Index col)
-  {
-    PacketType tmp = m_src.template packet<LoadMode,PacketType>(row,col);
-    const_cast<SrcEvaluatorTypeT&>(m_src).template writePacket<LoadMode>(row,col, m_dst.template packet<StoreMode,PacketType>(row,col));
-    m_dst.template writePacket<StoreMode>(row,col,tmp);
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorTypeT &dst,
+                                                                        const SrcEvaluatorTypeT &src,
+                                                                        const Functor &func, DstXprType &dstExpr)
+      : Base(dst, src, func, dstExpr) {}
+
+  template <int StoreMode, int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE void assignPacket(Index row, Index col) {
+    PacketType tmp = m_src.template packet<LoadMode, PacketType>(row, col);
+    const_cast<SrcEvaluatorTypeT &>(m_src).template writePacket<LoadMode>(
+        row, col, m_dst.template packet<StoreMode, PacketType>(row, col));
+    m_dst.template writePacket<StoreMode>(row, col, tmp);
   }
-  
-  template<int StoreMode, int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE void assignPacket(Index index)
-  {
-    PacketType tmp = m_src.template packet<LoadMode,PacketType>(index);
-    const_cast<SrcEvaluatorTypeT&>(m_src).template writePacket<LoadMode>(index, m_dst.template packet<StoreMode,PacketType>(index));
-    m_dst.template writePacket<StoreMode>(index,tmp);
+
+  template <int StoreMode, int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE void assignPacket(Index index) {
+    PacketType tmp = m_src.template packet<LoadMode, PacketType>(index);
+    const_cast<SrcEvaluatorTypeT &>(m_src).template writePacket<LoadMode>(
+        index, m_dst.template packet<StoreMode, PacketType>(index));
+    m_dst.template writePacket<StoreMode>(index, tmp);
   }
-  
-  // TODO find a simple way not to have to copy/paste this function from generic_dense_assignment_kernel, by simple I mean no CRTP (Gael)
-  template<int StoreMode, int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner)
-  {
-    Index row = Base::rowIndexByOuterInner(outer, inner); 
+
+  // TODO find a simple way not to have to copy/paste this function from generic_dense_assignment_kernel, by simple I
+  // mean no CRTP (Gael)
+  template <int StoreMode, int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner) {
+    Index row = Base::rowIndexByOuterInner(outer, inner);
     Index col = Base::colIndexByOuterInner(outer, inner);
-    assignPacket<StoreMode,LoadMode,PacketType>(row, col);
+    assignPacket<StoreMode, LoadMode, PacketType>(row, col);
   }
 };
 
-} // namespace internal
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SWAP_H
+#endif  // EIGEN_SWAP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h
index 2bc658f..1cc7a28 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h
@@ -11,14 +11,16 @@
 #ifndef EIGEN_TRANSPOSE_H
 #define EIGEN_TRANSPOSE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename MatrixType>
-struct traits<Transpose<MatrixType> > : public traits<MatrixType>
-{
+template <typename MatrixType>
+struct traits<Transpose<MatrixType> > : public traits<MatrixType> {
   typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type MatrixTypeNestedPlain;
+  typedef std::remove_reference_t<MatrixTypeNested> MatrixTypeNestedPlain;
   enum {
     RowsAtCompileTime = MatrixType::ColsAtCompileTime,
     ColsAtCompileTime = MatrixType::RowsAtCompileTime,
@@ -32,234 +34,205 @@
     OuterStrideAtCompileTime = outer_stride_at_compile_time<MatrixType>::ret
   };
 };
-}
+}  // namespace internal
 
-template<typename MatrixType, typename StorageKind> class TransposeImpl;
+template <typename MatrixType, typename StorageKind>
+class TransposeImpl;
 
 /** \class Transpose
-  * \ingroup Core_Module
-  *
-  * \brief Expression of the transpose of a matrix
-  *
-  * \tparam MatrixType the type of the object of which we are taking the transpose
-  *
-  * This class represents an expression of the transpose of a matrix.
-  * It is the return type of MatrixBase::transpose() and MatrixBase::adjoint()
-  * and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::transpose(), MatrixBase::adjoint()
-  */
-template<typename MatrixType> class Transpose
-  : public TransposeImpl<MatrixType,typename internal::traits<MatrixType>::StorageKind>
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Expression of the transpose of a matrix
+ *
+ * \tparam MatrixType the type of the object of which we are taking the transpose
+ *
+ * This class represents an expression of the transpose of a matrix.
+ * It is the return type of MatrixBase::transpose() and MatrixBase::adjoint()
+ * and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::transpose(), MatrixBase::adjoint()
+ */
+template <typename MatrixType>
+class Transpose : public TransposeImpl<MatrixType, typename internal::traits<MatrixType>::StorageKind> {
+ public:
+  typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
 
-    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+  typedef typename TransposeImpl<MatrixType, typename internal::traits<MatrixType>::StorageKind>::Base Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(Transpose)
+  typedef internal::remove_all_t<MatrixType> NestedExpression;
 
-    typedef typename TransposeImpl<MatrixType,typename internal::traits<MatrixType>::StorageKind>::Base Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(Transpose)
-    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+  EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Transpose(MatrixType& matrix) : m_matrix(matrix) {}
 
-    EIGEN_DEVICE_FUNC
-    explicit EIGEN_STRONG_INLINE Transpose(MatrixType& matrix) : m_matrix(matrix) {}
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Transpose)
 
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Transpose)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t<MatrixTypeNested>& nestedExpression() const {
+    return m_matrix;
+  }
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const typename internal::remove_all<MatrixTypeNested>::type&
-    nestedExpression() const { return m_matrix; }
+  /** \returns the nested expression */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::remove_reference_t<MatrixTypeNested>& nestedExpression() {
+    return m_matrix;
+  }
 
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    typename internal::remove_reference<MatrixTypeNested>::type&
-    nestedExpression() { return m_matrix; }
+  /** \internal */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index nrows, Index ncols) { m_matrix.resize(ncols, nrows); }
 
-    /** \internal */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void resize(Index nrows, Index ncols) {
-      m_matrix.resize(ncols,nrows);
-    }
-
-  protected:
-    typename internal::ref_selector<MatrixType>::non_const_type m_matrix;
+ protected:
+  typename internal::ref_selector<MatrixType>::non_const_type m_matrix;
 };
 
 namespace internal {
 
-template<typename MatrixType, bool HasDirectAccess = has_direct_access<MatrixType>::ret>
-struct TransposeImpl_base
-{
+template <typename MatrixType, bool HasDirectAccess = has_direct_access<MatrixType>::ret>
+struct TransposeImpl_base {
   typedef typename dense_xpr_base<Transpose<MatrixType> >::type type;
 };
 
-template<typename MatrixType>
-struct TransposeImpl_base<MatrixType, false>
-{
+template <typename MatrixType>
+struct TransposeImpl_base<MatrixType, false> {
   typedef typename dense_xpr_base<Transpose<MatrixType> >::type type;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 // Generic API dispatcher
-template<typename XprType, typename StorageKind>
-class TransposeImpl
-  : public internal::generic_xpr_base<Transpose<XprType> >::type
-{
-public:
+template <typename XprType, typename StorageKind>
+class TransposeImpl : public internal::generic_xpr_base<Transpose<XprType> >::type {
+ public:
   typedef typename internal::generic_xpr_base<Transpose<XprType> >::type Base;
 };
 
-template<typename MatrixType> class TransposeImpl<MatrixType,Dense>
-  : public internal::TransposeImpl_base<MatrixType>::type
-{
-  public:
+template <typename MatrixType>
+class TransposeImpl<MatrixType, Dense> : public internal::TransposeImpl_base<MatrixType>::type {
+ public:
+  typedef typename internal::TransposeImpl_base<MatrixType>::type Base;
+  using Base::coeffRef;
+  EIGEN_DENSE_PUBLIC_INTERFACE(Transpose<MatrixType>)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TransposeImpl)
 
-    typedef typename internal::TransposeImpl_base<MatrixType>::type Base;
-    using Base::coeffRef;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Transpose<MatrixType>)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TransposeImpl)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index innerStride() const { return derived().nestedExpression().innerStride(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index outerStride() const { return derived().nestedExpression().outerStride(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Index innerStride() const { return derived().nestedExpression().innerStride(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Index outerStride() const { return derived().nestedExpression().outerStride(); }
+  typedef std::conditional_t<internal::is_lvalue<MatrixType>::value, Scalar, const Scalar> ScalarWithConstIfNotLvalue;
 
-    typedef typename internal::conditional<
-                       internal::is_lvalue<MatrixType>::value,
-                       Scalar,
-                       const Scalar
-                     >::type ScalarWithConstIfNotLvalue;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ScalarWithConstIfNotLvalue* data() {
+    return derived().nestedExpression().data();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar* data() const { return derived().nestedExpression().data(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    ScalarWithConstIfNotLvalue* data() { return derived().nestedExpression().data(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const Scalar* data() const { return derived().nestedExpression().data(); }
+  // FIXME: shall we keep the const version of coeffRef?
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeffRef(Index rowId, Index colId) const {
+    return derived().nestedExpression().coeffRef(colId, rowId);
+  }
 
-    // FIXME: shall we keep the const version of coeffRef?
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      return derived().nestedExpression().coeffRef(colId, rowId);
-    }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeffRef(Index index) const {
+    return derived().nestedExpression().coeffRef(index);
+  }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    const Scalar& coeffRef(Index index) const
-    {
-      return derived().nestedExpression().coeffRef(index);
-    }
-  protected:
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TransposeImpl)
+ protected:
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TransposeImpl)
 };
 
 /** \returns an expression of the transpose of *this.
-  *
-  * Example: \include MatrixBase_transpose.cpp
-  * Output: \verbinclude MatrixBase_transpose.out
-  *
-  * \warning If you want to replace a matrix by its own transpose, do \b NOT do this:
-  * \code
-  * m = m.transpose(); // bug!!! caused by aliasing effect
-  * \endcode
-  * Instead, use the transposeInPlace() method:
-  * \code
-  * m.transposeInPlace();
-  * \endcode
-  * which gives Eigen good opportunities for optimization, or alternatively you can also do:
-  * \code
-  * m = m.transpose().eval();
-  * \endcode
-  *
-  * \sa transposeInPlace(), adjoint() */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-Transpose<Derived>
-DenseBase<Derived>::transpose()
-{
+ *
+ * Example: \include MatrixBase_transpose.cpp
+ * Output: \verbinclude MatrixBase_transpose.out
+ *
+ * \warning If you want to replace a matrix by its own transpose, do \b NOT do this:
+ * \code
+ * m = m.transpose(); // bug!!! caused by aliasing effect
+ * \endcode
+ * Instead, use the transposeInPlace() method:
+ * \code
+ * m.transposeInPlace();
+ * \endcode
+ * which gives Eigen good opportunities for optimization, or alternatively you can also do:
+ * \code
+ * m = m.transpose().eval();
+ * \endcode
+ *
+ * \sa transposeInPlace(), adjoint() */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename DenseBase<Derived>::TransposeReturnType DenseBase<Derived>::transpose() {
   return TransposeReturnType(derived());
 }
 
 /** This is the const version of transpose().
-  *
-  * Make sure you read the warning for transpose() !
-  *
-  * \sa transposeInPlace(), adjoint() */
-template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename DenseBase<Derived>::ConstTransposeReturnType
-DenseBase<Derived>::transpose() const
-{
+ *
+ * Make sure you read the warning for transpose() !
+ *
+ * \sa transposeInPlace(), adjoint() */
+template <typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstTransposeReturnType
+DenseBase<Derived>::transpose() const {
   return ConstTransposeReturnType(derived());
 }
 
 /** \returns an expression of the adjoint (i.e. conjugate transpose) of *this.
-  *
-  * Example: \include MatrixBase_adjoint.cpp
-  * Output: \verbinclude MatrixBase_adjoint.out
-  *
-  * \warning If you want to replace a matrix by its own adjoint, do \b NOT do this:
-  * \code
-  * m = m.adjoint(); // bug!!! caused by aliasing effect
-  * \endcode
-  * Instead, use the adjointInPlace() method:
-  * \code
-  * m.adjointInPlace();
-  * \endcode
-  * which gives Eigen good opportunities for optimization, or alternatively you can also do:
-  * \code
-  * m = m.adjoint().eval();
-  * \endcode
-  *
-  * \sa adjointInPlace(), transpose(), conjugate(), class Transpose, class internal::scalar_conjugate_op */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::AdjointReturnType
-MatrixBase<Derived>::adjoint() const
-{
+ *
+ * Example: \include MatrixBase_adjoint.cpp
+ * Output: \verbinclude MatrixBase_adjoint.out
+ *
+ * \warning If you want to replace a matrix by its own adjoint, do \b NOT do this:
+ * \code
+ * m = m.adjoint(); // bug!!! caused by aliasing effect
+ * \endcode
+ * Instead, use the adjointInPlace() method:
+ * \code
+ * m.adjointInPlace();
+ * \endcode
+ * which gives Eigen good opportunities for optimization, or alternatively you can also do:
+ * \code
+ * m = m.adjoint().eval();
+ * \endcode
+ *
+ * \sa adjointInPlace(), transpose(), conjugate(), class Transpose, class internal::scalar_conjugate_op */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::AdjointReturnType MatrixBase<Derived>::adjoint() const {
   return AdjointReturnType(this->transpose());
 }
 
 /***************************************************************************
-* "in place" transpose implementation
-***************************************************************************/
+ * "in place" transpose implementation
+ ***************************************************************************/
 
 namespace internal {
 
-template<typename MatrixType,
-  bool IsSquare = (MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime) && MatrixType::RowsAtCompileTime!=Dynamic,
-  bool MatchPacketSize =
-        (int(MatrixType::RowsAtCompileTime) == int(internal::packet_traits<typename MatrixType::Scalar>::size))
-    &&  (internal::evaluator<MatrixType>::Flags&PacketAccessBit) >
+template <typename MatrixType,
+          bool IsSquare = (MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime) &&
+                          MatrixType::RowsAtCompileTime != Dynamic,
+          bool MatchPacketSize =
+              (int(MatrixType::RowsAtCompileTime) == int(internal::packet_traits<typename MatrixType::Scalar>::size)) &&
+              (internal::evaluator<MatrixType>::Flags & PacketAccessBit)>
 struct inplace_transpose_selector;
 
-template<typename MatrixType>
-struct inplace_transpose_selector<MatrixType,true,false> { // square matrix
+template <typename MatrixType>
+struct inplace_transpose_selector<MatrixType, true, false> {  // square matrix
   static void run(MatrixType& m) {
-    m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose().template triangularView<StrictlyUpper>());
+    m.matrix().template triangularView<StrictlyUpper>().swap(
+        m.matrix().transpose().template triangularView<StrictlyUpper>());
   }
 };
 
-template<typename MatrixType>
-struct inplace_transpose_selector<MatrixType,true,true> { // PacketSize x PacketSize
+template <typename MatrixType>
+struct inplace_transpose_selector<MatrixType, true, true> {  // PacketSize x PacketSize
   static void run(MatrixType& m) {
     typedef typename MatrixType::Scalar Scalar;
     typedef typename internal::packet_traits<typename MatrixType::Scalar>::type Packet;
     const Index PacketSize = internal::packet_traits<Scalar>::size;
     const Index Alignment = internal::evaluator<MatrixType>::Alignment;
     PacketBlock<Packet> A;
-    for (Index i=0; i<PacketSize; ++i)
-      A.packet[i] = m.template packetByOuterInner<Alignment>(i,0);
+    for (Index i = 0; i < PacketSize; ++i) A.packet[i] = m.template packetByOuterInner<Alignment>(i, 0);
     internal::ptranspose(A);
-    for (Index i=0; i<PacketSize; ++i)
-      m.template writePacket<Alignment>(m.rowIndexByOuterInner(i,0), m.colIndexByOuterInner(i,0), A.packet[i]);
+    for (Index i = 0; i < PacketSize; ++i)
+      m.template writePacket<Alignment>(m.rowIndexByOuterInner(i, 0), m.colIndexByOuterInner(i, 0), A.packet[i]);
   }
 };
 
-
 template <typename MatrixType, Index Alignment>
 void BlockedInPlaceTranspose(MatrixType& m) {
   typedef typename MatrixType::Scalar Scalar;
@@ -271,46 +244,48 @@
     for (int col_start = row_start; col_start + PacketSize <= m.cols(); col_start += PacketSize) {
       PacketBlock<Packet> A;
       if (row_start == col_start) {
-        for (Index i=0; i<PacketSize; ++i)
-          A.packet[i] = m.template packetByOuterInner<Alignment>(row_start + i,col_start);
+        for (Index i = 0; i < PacketSize; ++i)
+          A.packet[i] = m.template packetByOuterInner<Alignment>(row_start + i, col_start);
         internal::ptranspose(A);
-        for (Index i=0; i<PacketSize; ++i)
-          m.template writePacket<Alignment>(m.rowIndexByOuterInner(row_start + i, col_start), m.colIndexByOuterInner(row_start + i,col_start), A.packet[i]);
+        for (Index i = 0; i < PacketSize; ++i)
+          m.template writePacket<Alignment>(m.rowIndexByOuterInner(row_start + i, col_start),
+                                            m.colIndexByOuterInner(row_start + i, col_start), A.packet[i]);
       } else {
         PacketBlock<Packet> B;
-        for (Index i=0; i<PacketSize; ++i) {
-          A.packet[i] = m.template packetByOuterInner<Alignment>(row_start + i,col_start);
+        for (Index i = 0; i < PacketSize; ++i) {
+          A.packet[i] = m.template packetByOuterInner<Alignment>(row_start + i, col_start);
           B.packet[i] = m.template packetByOuterInner<Alignment>(col_start + i, row_start);
         }
         internal::ptranspose(A);
         internal::ptranspose(B);
-        for (Index i=0; i<PacketSize; ++i) {
-          m.template writePacket<Alignment>(m.rowIndexByOuterInner(row_start + i, col_start), m.colIndexByOuterInner(row_start + i,col_start), B.packet[i]);
-          m.template writePacket<Alignment>(m.rowIndexByOuterInner(col_start + i, row_start), m.colIndexByOuterInner(col_start + i,row_start), A.packet[i]);
+        for (Index i = 0; i < PacketSize; ++i) {
+          m.template writePacket<Alignment>(m.rowIndexByOuterInner(row_start + i, col_start),
+                                            m.colIndexByOuterInner(row_start + i, col_start), B.packet[i]);
+          m.template writePacket<Alignment>(m.rowIndexByOuterInner(col_start + i, row_start),
+                                            m.colIndexByOuterInner(col_start + i, row_start), A.packet[i]);
         }
       }
     }
   }
   for (Index row = row_start; row < m.rows(); ++row) {
-    m.matrix().row(row).head(row).swap(
-        m.matrix().col(row).head(row).transpose());
+    m.matrix().row(row).head(row).swap(m.matrix().col(row).head(row).transpose());
   }
 }
 
-template<typename MatrixType,bool MatchPacketSize>
-struct inplace_transpose_selector<MatrixType,false,MatchPacketSize> { // non square or dynamic matrix
+template <typename MatrixType, bool MatchPacketSize>
+struct inplace_transpose_selector<MatrixType, false, MatchPacketSize> {  // non square or dynamic matrix
   static void run(MatrixType& m) {
     typedef typename MatrixType::Scalar Scalar;
     if (m.rows() == m.cols()) {
       const Index PacketSize = internal::packet_traits<Scalar>::size;
       if (!NumTraits<Scalar>::IsComplex && m.rows() >= PacketSize) {
         if ((m.rows() % PacketSize) == 0)
-          BlockedInPlaceTranspose<MatrixType,internal::evaluator<MatrixType>::Alignment>(m);
+          BlockedInPlaceTranspose<MatrixType, internal::evaluator<MatrixType>::Alignment>(m);
         else
-          BlockedInPlaceTranspose<MatrixType,Unaligned>(m);
-      }
-      else {
-        m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose().template triangularView<StrictlyUpper>());
+          BlockedInPlaceTranspose<MatrixType, Unaligned>(m);
+      } else {
+        m.matrix().template triangularView<StrictlyUpper>().swap(
+            m.matrix().transpose().template triangularView<StrictlyUpper>());
       }
     } else {
       m = m.transpose().eval();
@@ -318,62 +293,59 @@
   }
 };
 
-
-} // end namespace internal
+}  // end namespace internal
 
 /** This is the "in place" version of transpose(): it replaces \c *this by its own transpose.
-  * Thus, doing
-  * \code
-  * m.transposeInPlace();
-  * \endcode
-  * has the same effect on m as doing
-  * \code
-  * m = m.transpose().eval();
-  * \endcode
-  * and is faster and also safer because in the latter line of code, forgetting the eval() results
-  * in a bug caused by \ref TopicAliasing "aliasing".
-  *
-  * Notice however that this method is only useful if you want to replace a matrix by its own transpose.
-  * If you just need the transpose of a matrix, use transpose().
-  *
-  * \note if the matrix is not square, then \c *this must be a resizable matrix.
-  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
-  *
-  * \sa transpose(), adjoint(), adjointInPlace() */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline void DenseBase<Derived>::transposeInPlace()
-{
-  eigen_assert((rows() == cols() || (RowsAtCompileTime == Dynamic && ColsAtCompileTime == Dynamic))
-               && "transposeInPlace() called on a non-square non-resizable matrix");
+ * Thus, doing
+ * \code
+ * m.transposeInPlace();
+ * \endcode
+ * has the same effect on m as doing
+ * \code
+ * m = m.transpose().eval();
+ * \endcode
+ * and is faster and also safer because in the latter line of code, forgetting the eval() results
+ * in a bug caused by \ref TopicAliasing "aliasing".
+ *
+ * Notice however that this method is only useful if you want to replace a matrix by its own transpose.
+ * If you just need the transpose of a matrix, use transpose().
+ *
+ * \note if the matrix is not square, then \c *this must be a resizable matrix.
+ * This excludes (non-square) fixed-size matrices, block-expressions and maps.
+ *
+ * \sa transpose(), adjoint(), adjointInPlace() */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline void DenseBase<Derived>::transposeInPlace() {
+  eigen_assert((rows() == cols() || (RowsAtCompileTime == Dynamic && ColsAtCompileTime == Dynamic)) &&
+               "transposeInPlace() called on a non-square non-resizable matrix");
   internal::inplace_transpose_selector<Derived>::run(derived());
 }
 
 /***************************************************************************
-* "in place" adjoint implementation
-***************************************************************************/
+ * "in place" adjoint implementation
+ ***************************************************************************/
 
 /** This is the "in place" version of adjoint(): it replaces \c *this by its own transpose.
-  * Thus, doing
-  * \code
-  * m.adjointInPlace();
-  * \endcode
-  * has the same effect on m as doing
-  * \code
-  * m = m.adjoint().eval();
-  * \endcode
-  * and is faster and also safer because in the latter line of code, forgetting the eval() results
-  * in a bug caused by aliasing.
-  *
-  * Notice however that this method is only useful if you want to replace a matrix by its own adjoint.
-  * If you just need the adjoint of a matrix, use adjoint().
-  *
-  * \note if the matrix is not square, then \c *this must be a resizable matrix.
-  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
-  *
-  * \sa transpose(), adjoint(), transposeInPlace() */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline void MatrixBase<Derived>::adjointInPlace()
-{
+ * Thus, doing
+ * \code
+ * m.adjointInPlace();
+ * \endcode
+ * has the same effect on m as doing
+ * \code
+ * m = m.adjoint().eval();
+ * \endcode
+ * and is faster and also safer because in the latter line of code, forgetting the eval() results
+ * in a bug caused by aliasing.
+ *
+ * Notice however that this method is only useful if you want to replace a matrix by its own adjoint.
+ * If you just need the adjoint of a matrix, use adjoint().
+ *
+ * \note if the matrix is not square, then \c *this must be a resizable matrix.
+ * This excludes (non-square) fixed-size matrices, block-expressions and maps.
+ *
+ * \sa transpose(), adjoint(), transposeInPlace() */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline void MatrixBase<Derived>::adjointInPlace() {
   derived() = adjoint().eval();
 }
 
@@ -383,36 +355,34 @@
 
 namespace internal {
 
-template<bool DestIsTransposed, typename OtherDerived>
-struct check_transpose_aliasing_compile_time_selector
-{
+template <bool DestIsTransposed, typename OtherDerived>
+struct check_transpose_aliasing_compile_time_selector {
   enum { ret = bool(blas_traits<OtherDerived>::IsTransposed) != DestIsTransposed };
 };
 
-template<bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
-struct check_transpose_aliasing_compile_time_selector<DestIsTransposed,CwiseBinaryOp<BinOp,DerivedA,DerivedB> >
-{
-  enum { ret =    bool(blas_traits<DerivedA>::IsTransposed) != DestIsTransposed
-               || bool(blas_traits<DerivedB>::IsTransposed) != DestIsTransposed
+template <bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
+struct check_transpose_aliasing_compile_time_selector<DestIsTransposed, CwiseBinaryOp<BinOp, DerivedA, DerivedB> > {
+  enum {
+    ret = bool(blas_traits<DerivedA>::IsTransposed) != DestIsTransposed ||
+          bool(blas_traits<DerivedB>::IsTransposed) != DestIsTransposed
   };
 };
 
-template<typename Scalar, bool DestIsTransposed, typename OtherDerived>
-struct check_transpose_aliasing_run_time_selector
-{
-  static bool run(const Scalar* dest, const OtherDerived& src)
-  {
-    return (bool(blas_traits<OtherDerived>::IsTransposed) != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src));
+template <typename Scalar, bool DestIsTransposed, typename OtherDerived>
+struct check_transpose_aliasing_run_time_selector {
+  EIGEN_DEVICE_FUNC static bool run(const Scalar* dest, const OtherDerived& src) {
+    return (bool(blas_traits<OtherDerived>::IsTransposed) != DestIsTransposed) &&
+           (dest != 0 && dest == (const Scalar*)extract_data(src));
   }
 };
 
-template<typename Scalar, bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
-struct check_transpose_aliasing_run_time_selector<Scalar,DestIsTransposed,CwiseBinaryOp<BinOp,DerivedA,DerivedB> >
-{
-  static bool run(const Scalar* dest, const CwiseBinaryOp<BinOp,DerivedA,DerivedB>& src)
-  {
-    return ((blas_traits<DerivedA>::IsTransposed != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src.lhs())))
-        || ((blas_traits<DerivedB>::IsTransposed != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src.rhs())));
+template <typename Scalar, bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
+struct check_transpose_aliasing_run_time_selector<Scalar, DestIsTransposed, CwiseBinaryOp<BinOp, DerivedA, DerivedB> > {
+  EIGEN_DEVICE_FUNC static bool run(const Scalar* dest, const CwiseBinaryOp<BinOp, DerivedA, DerivedB>& src) {
+    return ((blas_traits<DerivedA>::IsTransposed != DestIsTransposed) &&
+            (dest != 0 && dest == (const Scalar*)extract_data(src.lhs()))) ||
+           ((blas_traits<DerivedB>::IsTransposed != DestIsTransposed) &&
+            (dest != 0 && dest == (const Scalar*)extract_data(src.rhs())));
   }
 };
 
@@ -422,43 +392,34 @@
 // known at compile time to be false, and using that, we can avoid generating the code of the assert again
 // and again for all these expressions that don't need it.
 
-template<typename Derived, typename OtherDerived,
-         bool MightHaveTransposeAliasing
-                 = check_transpose_aliasing_compile_time_selector
-                     <blas_traits<Derived>::IsTransposed,OtherDerived>::ret
-        >
-struct checkTransposeAliasing_impl
-{
-    static void run(const Derived& dst, const OtherDerived& other)
-    {
-        eigen_assert((!check_transpose_aliasing_run_time_selector
-                      <typename Derived::Scalar,blas_traits<Derived>::IsTransposed,OtherDerived>
-                      ::run(extract_data(dst), other))
-          && "aliasing detected during transposition, use transposeInPlace() "
-             "or evaluate the rhs into a temporary using .eval()");
-
-    }
+template <typename Derived, typename OtherDerived,
+          bool MightHaveTransposeAliasing =
+              check_transpose_aliasing_compile_time_selector<blas_traits<Derived>::IsTransposed, OtherDerived>::ret>
+struct checkTransposeAliasing_impl {
+  EIGEN_DEVICE_FUNC static void run(const Derived& dst, const OtherDerived& other) {
+    eigen_assert(
+        (!check_transpose_aliasing_run_time_selector<typename Derived::Scalar, blas_traits<Derived>::IsTransposed,
+                                                     OtherDerived>::run(extract_data(dst), other)) &&
+        "aliasing detected during transposition, use transposeInPlace() "
+        "or evaluate the rhs into a temporary using .eval()");
+  }
 };
 
-template<typename Derived, typename OtherDerived>
-struct checkTransposeAliasing_impl<Derived, OtherDerived, false>
-{
-    static void run(const Derived&, const OtherDerived&)
-    {
-    }
+template <typename Derived, typename OtherDerived>
+struct checkTransposeAliasing_impl<Derived, OtherDerived, false> {
+  EIGEN_DEVICE_FUNC static void run(const Derived&, const OtherDerived&) {}
 };
 
-template<typename Dst, typename Src>
-void check_for_aliasing(const Dst &dst, const Src &src)
-{
-  if((!Dst::IsVectorAtCompileTime) && dst.rows()>1 && dst.cols()>1)
+template <typename Dst, typename Src>
+EIGEN_DEVICE_FUNC inline void check_for_aliasing(const Dst& dst, const Src& src) {
+  if ((!Dst::IsVectorAtCompileTime) && dst.rows() > 1 && dst.cols() > 1)
     internal::checkTransposeAliasing_impl<Dst, Src>::run(dst, src);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-#endif // EIGEN_NO_DEBUG
+#endif  // EIGEN_NO_DEBUG
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TRANSPOSE_H
+#endif  // EIGEN_TRANSPOSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h
index 38a7b01..ad136d3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h
@@ -10,377 +10,314 @@
 #ifndef EIGEN_TRANSPOSITIONS_H
 #define EIGEN_TRANSPOSITIONS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename Derived>
-class TranspositionsBase
-{
-    typedef internal::traits<Derived> Traits;
+template <typename Derived>
+class TranspositionsBase {
+  typedef internal::traits<Derived> Traits;
 
-  public:
+ public:
+  typedef typename Traits::IndicesType IndicesType;
+  typedef typename IndicesType::Scalar StorageIndex;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    typedef typename Traits::IndicesType IndicesType;
-    typedef typename IndicesType::Scalar StorageIndex;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  EIGEN_DEVICE_FUNC Derived& derived() { return *static_cast<Derived*>(this); }
+  EIGEN_DEVICE_FUNC const Derived& derived() const { return *static_cast<const Derived*>(this); }
 
-    EIGEN_DEVICE_FUNC
-    Derived& derived() { return *static_cast<Derived*>(this); }
-    EIGEN_DEVICE_FUNC
-    const Derived& derived() const { return *static_cast<const Derived*>(this); }
+  /** Copies the \a other transpositions into \c *this */
+  template <typename OtherDerived>
+  Derived& operator=(const TranspositionsBase<OtherDerived>& other) {
+    indices() = other.indices();
+    return derived();
+  }
 
-    /** Copies the \a other transpositions into \c *this */
-    template<typename OtherDerived>
-    Derived& operator=(const TranspositionsBase<OtherDerived>& other)
-    {
-      indices() = other.indices();
-      return derived();
-    }
+  /** \returns the number of transpositions */
+  EIGEN_DEVICE_FUNC Index size() const { return indices().size(); }
+  /** \returns the number of rows of the equivalent permutation matrix */
+  EIGEN_DEVICE_FUNC Index rows() const { return indices().size(); }
+  /** \returns the number of columns of the equivalent permutation matrix */
+  EIGEN_DEVICE_FUNC Index cols() const { return indices().size(); }
 
-    /** \returns the number of transpositions */
-    EIGEN_DEVICE_FUNC
-    Index size() const { return indices().size(); }
-    /** \returns the number of rows of the equivalent permutation matrix */
-    EIGEN_DEVICE_FUNC
-    Index rows() const { return indices().size(); }
-    /** \returns the number of columns of the equivalent permutation matrix */
-    EIGEN_DEVICE_FUNC
-    Index cols() const { return indices().size(); }
+  /** Direct access to the underlying index vector */
+  EIGEN_DEVICE_FUNC inline const StorageIndex& coeff(Index i) const { return indices().coeff(i); }
+  /** Direct access to the underlying index vector */
+  inline StorageIndex& coeffRef(Index i) { return indices().coeffRef(i); }
+  /** Direct access to the underlying index vector */
+  inline const StorageIndex& operator()(Index i) const { return indices()(i); }
+  /** Direct access to the underlying index vector */
+  inline StorageIndex& operator()(Index i) { return indices()(i); }
+  /** Direct access to the underlying index vector */
+  inline const StorageIndex& operator[](Index i) const { return indices()(i); }
+  /** Direct access to the underlying index vector */
+  inline StorageIndex& operator[](Index i) { return indices()(i); }
 
-    /** Direct access to the underlying index vector */
-    EIGEN_DEVICE_FUNC
-    inline const StorageIndex& coeff(Index i) const { return indices().coeff(i); }
-    /** Direct access to the underlying index vector */
-    inline StorageIndex& coeffRef(Index i) { return indices().coeffRef(i); }
-    /** Direct access to the underlying index vector */
-    inline const StorageIndex& operator()(Index i) const { return indices()(i); }
-    /** Direct access to the underlying index vector */
-    inline StorageIndex& operator()(Index i) { return indices()(i); }
-    /** Direct access to the underlying index vector */
-    inline const StorageIndex& operator[](Index i) const { return indices()(i); }
-    /** Direct access to the underlying index vector */
-    inline StorageIndex& operator[](Index i) { return indices()(i); }
+  /** const version of indices(). */
+  EIGEN_DEVICE_FUNC const IndicesType& indices() const { return derived().indices(); }
+  /** \returns a reference to the stored array representing the transpositions. */
+  EIGEN_DEVICE_FUNC IndicesType& indices() { return derived().indices(); }
 
-    /** const version of indices(). */
-    EIGEN_DEVICE_FUNC
-    const IndicesType& indices() const { return derived().indices(); }
-    /** \returns a reference to the stored array representing the transpositions. */
-    EIGEN_DEVICE_FUNC
-    IndicesType& indices() { return derived().indices(); }
+  /** Resizes to given size. */
+  inline void resize(Index newSize) { indices().resize(newSize); }
 
-    /** Resizes to given size. */
-    inline void resize(Index newSize)
-    {
-      indices().resize(newSize);
-    }
+  /** Sets \c *this to represents an identity transformation */
+  void setIdentity() {
+    for (StorageIndex i = 0; i < indices().size(); ++i) coeffRef(i) = i;
+  }
 
-    /** Sets \c *this to represents an identity transformation */
-    void setIdentity()
-    {
-      for(StorageIndex i = 0; i < indices().size(); ++i)
-        coeffRef(i) = i;
-    }
+  // FIXME: do we want such methods ?
+  // might be useful when the target matrix expression is complex, e.g.:
+  // object.matrix().block(..,..,..,..) = trans * object.matrix().block(..,..,..,..);
+  /*
+  template<typename MatrixType>
+  void applyForwardToRows(MatrixType& mat) const
+  {
+    for(Index k=0 ; k<size() ; ++k)
+      if(m_indices(k)!=k)
+        mat.row(k).swap(mat.row(m_indices(k)));
+  }
 
-    // FIXME: do we want such methods ?
-    // might be useful when the target matrix expression is complex, e.g.:
-    // object.matrix().block(..,..,..,..) = trans * object.matrix().block(..,..,..,..);
-    /*
-    template<typename MatrixType>
-    void applyForwardToRows(MatrixType& mat) const
-    {
-      for(Index k=0 ; k<size() ; ++k)
-        if(m_indices(k)!=k)
-          mat.row(k).swap(mat.row(m_indices(k)));
-    }
+  template<typename MatrixType>
+  void applyBackwardToRows(MatrixType& mat) const
+  {
+    for(Index k=size()-1 ; k>=0 ; --k)
+      if(m_indices(k)!=k)
+        mat.row(k).swap(mat.row(m_indices(k)));
+  }
+  */
 
-    template<typename MatrixType>
-    void applyBackwardToRows(MatrixType& mat) const
-    {
-      for(Index k=size()-1 ; k>=0 ; --k)
-        if(m_indices(k)!=k)
-          mat.row(k).swap(mat.row(m_indices(k)));
-    }
-    */
+  /** \returns the inverse transformation */
+  inline Transpose<TranspositionsBase> inverse() const { return Transpose<TranspositionsBase>(derived()); }
 
-    /** \returns the inverse transformation */
-    inline Transpose<TranspositionsBase> inverse() const
-    { return Transpose<TranspositionsBase>(derived()); }
+  /** \returns the tranpose transformation */
+  inline Transpose<TranspositionsBase> transpose() const { return Transpose<TranspositionsBase>(derived()); }
 
-    /** \returns the tranpose transformation */
-    inline Transpose<TranspositionsBase> transpose() const
-    { return Transpose<TranspositionsBase>(derived()); }
-
-  protected:
+ protected:
 };
 
 namespace internal {
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex>
-struct traits<Transpositions<SizeAtCompileTime,MaxSizeAtCompileTime,_StorageIndex> >
- : traits<PermutationMatrix<SizeAtCompileTime,MaxSizeAtCompileTime,_StorageIndex> >
-{
-  typedef Matrix<_StorageIndex, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1> IndicesType;
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_>
+struct traits<Transpositions<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_> >
+    : traits<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_> > {
+  typedef Matrix<StorageIndex_, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1> IndicesType;
   typedef TranspositionsStorage StorageKind;
 };
-}
+}  // namespace internal
 
 /** \class Transpositions
-  * \ingroup Core_Module
-  *
-  * \brief Represents a sequence of transpositions (row/column interchange)
-  *
-  * \tparam SizeAtCompileTime the number of transpositions, or Dynamic
-  * \tparam MaxSizeAtCompileTime the maximum number of transpositions, or Dynamic. This optional parameter defaults to SizeAtCompileTime. Most of the time, you should not have to specify it.
-  *
-  * This class represents a permutation transformation as a sequence of \em n transpositions
-  * \f$[T_{n-1} \ldots T_{i} \ldots T_{0}]\f$. It is internally stored as a vector of integers \c indices.
-  * Each transposition \f$ T_{i} \f$ applied on the left of a matrix (\f$ T_{i} M\f$) interchanges
-  * the rows \c i and \c indices[i] of the matrix \c M.
-  * A transposition applied on the right (e.g., \f$ M T_{i}\f$) yields a column interchange.
-  *
-  * Compared to the class PermutationMatrix, such a sequence of transpositions is what is
-  * computed during a decomposition with pivoting, and it is faster when applying the permutation in-place.
-  *
-  * To apply a sequence of transpositions to a matrix, simply use the operator * as in the following example:
-  * \code
-  * Transpositions tr;
-  * MatrixXf mat;
-  * mat = tr * mat;
-  * \endcode
-  * In this example, we detect that the matrix appears on both side, and so the transpositions
-  * are applied in-place without any temporary or extra copy.
-  *
-  * \sa class PermutationMatrix
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Represents a sequence of transpositions (row/column interchange)
+ *
+ * \tparam SizeAtCompileTime the number of transpositions, or Dynamic
+ * \tparam MaxSizeAtCompileTime the maximum number of transpositions, or Dynamic. This optional parameter defaults to
+ * SizeAtCompileTime. Most of the time, you should not have to specify it.
+ *
+ * This class represents a permutation transformation as a sequence of \em n transpositions
+ * \f$[T_{n-1} \ldots T_{i} \ldots T_{0}]\f$. It is internally stored as a vector of integers \c indices.
+ * Each transposition \f$ T_{i} \f$ applied on the left of a matrix (\f$ T_{i} M\f$) interchanges
+ * the rows \c i and \c indices[i] of the matrix \c M.
+ * A transposition applied on the right (e.g., \f$ M T_{i}\f$) yields a column interchange.
+ *
+ * Compared to the class PermutationMatrix, such a sequence of transpositions is what is
+ * computed during a decomposition with pivoting, and it is faster when applying the permutation in-place.
+ *
+ * To apply a sequence of transpositions to a matrix, simply use the operator * as in the following example:
+ * \code
+ * Transpositions tr;
+ * MatrixXf mat;
+ * mat = tr * mat;
+ * \endcode
+ * In this example, we detect that the matrix appears on both side, and so the transpositions
+ * are applied in-place without any temporary or extra copy.
+ *
+ * \sa class PermutationMatrix
+ */
 
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex>
-class Transpositions : public TranspositionsBase<Transpositions<SizeAtCompileTime,MaxSizeAtCompileTime,_StorageIndex> >
-{
-    typedef internal::traits<Transpositions> Traits;
-  public:
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_>
+class Transpositions
+    : public TranspositionsBase<Transpositions<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_> > {
+  typedef internal::traits<Transpositions> Traits;
 
-    typedef TranspositionsBase<Transpositions> Base;
-    typedef typename Traits::IndicesType IndicesType;
-    typedef typename IndicesType::Scalar StorageIndex;
+ public:
+  typedef TranspositionsBase<Transpositions> Base;
+  typedef typename Traits::IndicesType IndicesType;
+  typedef typename IndicesType::Scalar StorageIndex;
 
-    inline Transpositions() {}
+  inline Transpositions() {}
 
-    /** Copy constructor. */
-    template<typename OtherDerived>
-    inline Transpositions(const TranspositionsBase<OtherDerived>& other)
-      : m_indices(other.indices()) {}
+  /** Copy constructor. */
+  template <typename OtherDerived>
+  inline Transpositions(const TranspositionsBase<OtherDerived>& other) : m_indices(other.indices()) {}
 
-    /** Generic constructor from expression of the transposition indices. */
-    template<typename Other>
-    explicit inline Transpositions(const MatrixBase<Other>& indices) : m_indices(indices)
-    {}
+  /** Generic constructor from expression of the transposition indices. */
+  template <typename Other>
+  explicit inline Transpositions(const MatrixBase<Other>& indices) : m_indices(indices) {}
 
-    /** Copies the \a other transpositions into \c *this */
-    template<typename OtherDerived>
-    Transpositions& operator=(const TranspositionsBase<OtherDerived>& other)
-    {
-      return Base::operator=(other);
-    }
+  /** Copies the \a other transpositions into \c *this */
+  template <typename OtherDerived>
+  Transpositions& operator=(const TranspositionsBase<OtherDerived>& other) {
+    return Base::operator=(other);
+  }
 
-    /** Constructs an uninitialized permutation matrix of given size.
-      */
-    inline Transpositions(Index size) : m_indices(size)
-    {}
+  /** Constructs an uninitialized permutation matrix of given size.
+   */
+  inline Transpositions(Index size) : m_indices(size) {}
 
-    /** const version of indices(). */
-    EIGEN_DEVICE_FUNC
-    const IndicesType& indices() const { return m_indices; }
-    /** \returns a reference to the stored array representing the transpositions. */
-    EIGEN_DEVICE_FUNC
-    IndicesType& indices() { return m_indices; }
+  /** const version of indices(). */
+  EIGEN_DEVICE_FUNC const IndicesType& indices() const { return m_indices; }
+  /** \returns a reference to the stored array representing the transpositions. */
+  EIGEN_DEVICE_FUNC IndicesType& indices() { return m_indices; }
 
-  protected:
-
-    IndicesType m_indices;
-};
-
-
-namespace internal {
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex, int _PacketAccess>
-struct traits<Map<Transpositions<SizeAtCompileTime,MaxSizeAtCompileTime,_StorageIndex>,_PacketAccess> >
- : traits<PermutationMatrix<SizeAtCompileTime,MaxSizeAtCompileTime,_StorageIndex> >
-{
-  typedef Map<const Matrix<_StorageIndex,SizeAtCompileTime,1,0,MaxSizeAtCompileTime,1>, _PacketAccess> IndicesType;
-  typedef _StorageIndex StorageIndex;
-  typedef TranspositionsStorage StorageKind;
-};
-}
-
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime, typename _StorageIndex, int PacketAccess>
-class Map<Transpositions<SizeAtCompileTime,MaxSizeAtCompileTime,_StorageIndex>,PacketAccess>
- : public TranspositionsBase<Map<Transpositions<SizeAtCompileTime,MaxSizeAtCompileTime,_StorageIndex>,PacketAccess> >
-{
-    typedef internal::traits<Map> Traits;
-  public:
-
-    typedef TranspositionsBase<Map> Base;
-    typedef typename Traits::IndicesType IndicesType;
-    typedef typename IndicesType::Scalar StorageIndex;
-
-    explicit inline Map(const StorageIndex* indicesPtr)
-      : m_indices(indicesPtr)
-    {}
-
-    inline Map(const StorageIndex* indicesPtr, Index size)
-      : m_indices(indicesPtr,size)
-    {}
-
-    /** Copies the \a other transpositions into \c *this */
-    template<typename OtherDerived>
-    Map& operator=(const TranspositionsBase<OtherDerived>& other)
-    {
-      return Base::operator=(other);
-    }
-
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    Map& operator=(const Map& other)
-    {
-      m_indices = other.m_indices;
-      return *this;
-    }
-    #endif
-
-    /** const version of indices(). */
-    EIGEN_DEVICE_FUNC
-    const IndicesType& indices() const { return m_indices; }
-
-    /** \returns a reference to the stored array representing the transpositions. */
-    EIGEN_DEVICE_FUNC
-    IndicesType& indices() { return m_indices; }
-
-  protected:
-
-    IndicesType m_indices;
+ protected:
+  IndicesType m_indices;
 };
 
 namespace internal {
-template<typename _IndicesType>
-struct traits<TranspositionsWrapper<_IndicesType> >
- : traits<PermutationWrapper<_IndicesType> >
-{
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_, int PacketAccess_>
+struct traits<Map<Transpositions<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_>, PacketAccess_> >
+    : traits<PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_> > {
+  typedef Map<const Matrix<StorageIndex_, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1>, PacketAccess_> IndicesType;
+  typedef StorageIndex_ StorageIndex;
   typedef TranspositionsStorage StorageKind;
 };
-}
+}  // namespace internal
 
-template<typename _IndicesType>
-class TranspositionsWrapper
- : public TranspositionsBase<TranspositionsWrapper<_IndicesType> >
-{
-    typedef internal::traits<TranspositionsWrapper> Traits;
-  public:
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime, typename StorageIndex_, int PacketAccess>
+class Map<Transpositions<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_>, PacketAccess>
+    : public TranspositionsBase<
+          Map<Transpositions<SizeAtCompileTime, MaxSizeAtCompileTime, StorageIndex_>, PacketAccess> > {
+  typedef internal::traits<Map> Traits;
 
-    typedef TranspositionsBase<TranspositionsWrapper> Base;
-    typedef typename Traits::IndicesType IndicesType;
-    typedef typename IndicesType::Scalar StorageIndex;
+ public:
+  typedef TranspositionsBase<Map> Base;
+  typedef typename Traits::IndicesType IndicesType;
+  typedef typename IndicesType::Scalar StorageIndex;
 
-    explicit inline TranspositionsWrapper(IndicesType& indices)
-      : m_indices(indices)
-    {}
+  explicit inline Map(const StorageIndex* indicesPtr) : m_indices(indicesPtr) {}
 
-    /** Copies the \a other transpositions into \c *this */
-    template<typename OtherDerived>
-    TranspositionsWrapper& operator=(const TranspositionsBase<OtherDerived>& other)
-    {
-      return Base::operator=(other);
-    }
+  inline Map(const StorageIndex* indicesPtr, Index size) : m_indices(indicesPtr, size) {}
 
-    /** const version of indices(). */
-    EIGEN_DEVICE_FUNC
-    const IndicesType& indices() const { return m_indices; }
+  /** Copies the \a other transpositions into \c *this */
+  template <typename OtherDerived>
+  Map& operator=(const TranspositionsBase<OtherDerived>& other) {
+    return Base::operator=(other);
+  }
 
-    /** \returns a reference to the stored array representing the transpositions. */
-    EIGEN_DEVICE_FUNC
-    IndicesType& indices() { return m_indices; }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** This is a special case of the templated operator=. Its purpose is to
+   * prevent a default operator= from hiding the templated operator=.
+   */
+  Map& operator=(const Map& other) {
+    m_indices = other.m_indices;
+    return *this;
+  }
+#endif
 
-  protected:
+  /** const version of indices(). */
+  EIGEN_DEVICE_FUNC const IndicesType& indices() const { return m_indices; }
 
-    typename IndicesType::Nested m_indices;
+  /** \returns a reference to the stored array representing the transpositions. */
+  EIGEN_DEVICE_FUNC IndicesType& indices() { return m_indices; }
+
+ protected:
+  IndicesType m_indices;
 };
 
+namespace internal {
+template <typename IndicesType_>
+struct traits<TranspositionsWrapper<IndicesType_> > : traits<PermutationWrapper<IndicesType_> > {
+  typedef TranspositionsStorage StorageKind;
+};
+}  // namespace internal
 
+template <typename IndicesType_>
+class TranspositionsWrapper : public TranspositionsBase<TranspositionsWrapper<IndicesType_> > {
+  typedef internal::traits<TranspositionsWrapper> Traits;
+
+ public:
+  typedef TranspositionsBase<TranspositionsWrapper> Base;
+  typedef typename Traits::IndicesType IndicesType;
+  typedef typename IndicesType::Scalar StorageIndex;
+
+  explicit inline TranspositionsWrapper(IndicesType& indices) : m_indices(indices) {}
+
+  /** Copies the \a other transpositions into \c *this */
+  template <typename OtherDerived>
+  TranspositionsWrapper& operator=(const TranspositionsBase<OtherDerived>& other) {
+    return Base::operator=(other);
+  }
+
+  /** const version of indices(). */
+  EIGEN_DEVICE_FUNC const IndicesType& indices() const { return m_indices; }
+
+  /** \returns a reference to the stored array representing the transpositions. */
+  EIGEN_DEVICE_FUNC IndicesType& indices() { return m_indices; }
+
+ protected:
+  typename IndicesType::Nested m_indices;
+};
 
 /** \returns the \a matrix with the \a transpositions applied to the columns.
-  */
-template<typename MatrixDerived, typename TranspositionsDerived>
-EIGEN_DEVICE_FUNC
-const Product<MatrixDerived, TranspositionsDerived, AliasFreeProduct>
-operator*(const MatrixBase<MatrixDerived> &matrix,
-          const TranspositionsBase<TranspositionsDerived>& transpositions)
-{
-  return Product<MatrixDerived, TranspositionsDerived, AliasFreeProduct>
-            (matrix.derived(), transpositions.derived());
+ */
+template <typename MatrixDerived, typename TranspositionsDerived>
+EIGEN_DEVICE_FUNC const Product<MatrixDerived, TranspositionsDerived, AliasFreeProduct> operator*(
+    const MatrixBase<MatrixDerived>& matrix, const TranspositionsBase<TranspositionsDerived>& transpositions) {
+  return Product<MatrixDerived, TranspositionsDerived, AliasFreeProduct>(matrix.derived(), transpositions.derived());
 }
 
 /** \returns the \a matrix with the \a transpositions applied to the rows.
-  */
-template<typename TranspositionsDerived, typename MatrixDerived>
-EIGEN_DEVICE_FUNC
-const Product<TranspositionsDerived, MatrixDerived, AliasFreeProduct>
-operator*(const TranspositionsBase<TranspositionsDerived> &transpositions,
-          const MatrixBase<MatrixDerived>& matrix)
-{
-  return Product<TranspositionsDerived, MatrixDerived, AliasFreeProduct>
-            (transpositions.derived(), matrix.derived());
+ */
+template <typename TranspositionsDerived, typename MatrixDerived>
+EIGEN_DEVICE_FUNC const Product<TranspositionsDerived, MatrixDerived, AliasFreeProduct> operator*(
+    const TranspositionsBase<TranspositionsDerived>& transpositions, const MatrixBase<MatrixDerived>& matrix) {
+  return Product<TranspositionsDerived, MatrixDerived, AliasFreeProduct>(transpositions.derived(), matrix.derived());
 }
 
 // Template partial specialization for transposed/inverse transpositions
 
 namespace internal {
 
-template<typename Derived>
-struct traits<Transpose<TranspositionsBase<Derived> > >
- : traits<Derived>
-{};
+template <typename Derived>
+struct traits<Transpose<TranspositionsBase<Derived> > > : traits<Derived> {};
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename TranspositionsDerived>
-class Transpose<TranspositionsBase<TranspositionsDerived> >
-{
-    typedef TranspositionsDerived TranspositionType;
-    typedef typename TranspositionType::IndicesType IndicesType;
-  public:
+template <typename TranspositionsDerived>
+class Transpose<TranspositionsBase<TranspositionsDerived> > {
+  typedef TranspositionsDerived TranspositionType;
+  typedef typename TranspositionType::IndicesType IndicesType;
 
-    explicit Transpose(const TranspositionType& t) : m_transpositions(t) {}
+ public:
+  explicit Transpose(const TranspositionType& t) : m_transpositions(t) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index size() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
 
-    /** \returns the \a matrix with the inverse transpositions applied to the columns.
-      */
-    template<typename OtherDerived> friend
-    const Product<OtherDerived, Transpose, AliasFreeProduct>
-    operator*(const MatrixBase<OtherDerived>& matrix, const Transpose& trt)
-    {
-      return Product<OtherDerived, Transpose, AliasFreeProduct>(matrix.derived(), trt);
-    }
+  /** \returns the \a matrix with the inverse transpositions applied to the columns.
+   */
+  template <typename OtherDerived>
+  friend const Product<OtherDerived, Transpose, AliasFreeProduct> operator*(const MatrixBase<OtherDerived>& matrix,
+                                                                            const Transpose& trt) {
+    return Product<OtherDerived, Transpose, AliasFreeProduct>(matrix.derived(), trt);
+  }
 
-    /** \returns the \a matrix with the inverse transpositions applied to the rows.
-      */
-    template<typename OtherDerived>
-    const Product<Transpose, OtherDerived, AliasFreeProduct>
-    operator*(const MatrixBase<OtherDerived>& matrix) const
-    {
-      return Product<Transpose, OtherDerived, AliasFreeProduct>(*this, matrix.derived());
-    }
+  /** \returns the \a matrix with the inverse transpositions applied to the rows.
+   */
+  template <typename OtherDerived>
+  const Product<Transpose, OtherDerived, AliasFreeProduct> operator*(const MatrixBase<OtherDerived>& matrix) const {
+    return Product<Transpose, OtherDerived, AliasFreeProduct>(*this, matrix.derived());
+  }
 
-    EIGEN_DEVICE_FUNC
-    const TranspositionType& nestedExpression() const { return m_transpositions; }
+  EIGEN_DEVICE_FUNC const TranspositionType& nestedExpression() const { return m_transpositions; }
 
-  protected:
-    const TranspositionType& m_transpositions;
+ protected:
+  const TranspositionType& m_transpositions;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TRANSPOSITIONS_H
+#endif  // EIGEN_TRANSPOSITIONS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h
index fdb8bc1..afdb242 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h
@@ -11,702 +11,614 @@
 #ifndef EIGEN_TRIANGULARMATRIX_H
 #define EIGEN_TRIANGULARMATRIX_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<int Side, typename TriangularType, typename Rhs> struct triangular_solve_retval;
+template <int Side, typename TriangularType, typename Rhs>
+struct triangular_solve_retval;
 
 }
 
 /** \class TriangularBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for triangular part in a matrix
-  */
-template<typename Derived> class TriangularBase : public EigenBase<Derived>
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Base class for triangular part in a matrix
+ */
+template <typename Derived>
+class TriangularBase : public EigenBase<Derived> {
+ public:
+  enum {
+    Mode = internal::traits<Derived>::Mode,
+    RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+    ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+    MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
 
-    enum {
-      Mode = internal::traits<Derived>::Mode,
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
+    SizeAtCompileTime = (internal::size_of_xpr_at_compile_time<Derived>::ret),
+    /**< This is equal to the number of coefficients, i.e. the number of
+     * rows times the number of columns, or to \a Dynamic if this is not
+     * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
 
-      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
-                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
-      /**< This is equal to the number of coefficients, i.e. the number of
-          * rows times the number of columns, or to \a Dynamic if this is not
-          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+    MaxSizeAtCompileTime = internal::size_at_compile_time(internal::traits<Derived>::MaxRowsAtCompileTime,
+                                                          internal::traits<Derived>::MaxColsAtCompileTime)
 
-      MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
-                                                   internal::traits<Derived>::MaxColsAtCompileTime>::ret)
+  };
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+  typedef typename internal::traits<Derived>::FullMatrixType DenseMatrixType;
+  typedef DenseMatrixType DenseType;
+  typedef Derived const& Nested;
 
-    };
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
-    typedef typename internal::traits<Derived>::FullMatrixType DenseMatrixType;
-    typedef DenseMatrixType DenseType;
-    typedef Derived const& Nested;
+  EIGEN_DEVICE_FUNC inline TriangularBase() {
+    eigen_assert(!((int(Mode) & int(UnitDiag)) && (int(Mode) & int(ZeroDiag))));
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline TriangularBase() { eigen_assert(!((int(Mode) & int(UnitDiag)) && (int(Mode) & int(ZeroDiag)))); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return derived().outerStride(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return derived().innerStride(); }
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index outerStride() const EIGEN_NOEXCEPT { return derived().outerStride(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index innerStride() const EIGEN_NOEXCEPT { return derived().innerStride(); }
+  // dummy resize function
+  EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) {
+    EIGEN_UNUSED_VARIABLE(rows);
+    EIGEN_UNUSED_VARIABLE(cols);
+    eigen_assert(rows == this->rows() && cols == this->cols());
+  }
 
-    // dummy resize function
-    EIGEN_DEVICE_FUNC
-    void resize(Index rows, Index cols)
-    {
-      EIGEN_UNUSED_VARIABLE(rows);
-      EIGEN_UNUSED_VARIABLE(cols);
-      eigen_assert(rows==this->rows() && cols==this->cols());
-    }
+  EIGEN_DEVICE_FUNC inline Scalar coeff(Index row, Index col) const { return derived().coeff(row, col); }
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) { return derived().coeffRef(row, col); }
 
-    EIGEN_DEVICE_FUNC
-    inline Scalar coeff(Index row, Index col) const  { return derived().coeff(row,col); }
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index row, Index col) { return derived().coeffRef(row,col); }
+  /** \see MatrixBase::copyCoeff(row,col)
+   */
+  template <typename Other>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void copyCoeff(Index row, Index col, Other& other) {
+    derived().coeffRef(row, col) = other.coeff(row, col);
+  }
 
-    /** \see MatrixBase::copyCoeff(row,col)
-      */
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void copyCoeff(Index row, Index col, Other& other)
-    {
-      derived().coeffRef(row, col) = other.coeff(row, col);
-    }
+  EIGEN_DEVICE_FUNC inline Scalar operator()(Index row, Index col) const {
+    check_coordinates(row, col);
+    return coeff(row, col);
+  }
+  EIGEN_DEVICE_FUNC inline Scalar& operator()(Index row, Index col) {
+    check_coordinates(row, col);
+    return coeffRef(row, col);
+  }
 
-    EIGEN_DEVICE_FUNC
-    inline Scalar operator()(Index row, Index col) const
-    {
-      check_coordinates(row, col);
-      return coeff(row,col);
-    }
-    EIGEN_DEVICE_FUNC
-    inline Scalar& operator()(Index row, Index col)
-    {
-      check_coordinates(row, col);
-      return coeffRef(row,col);
-    }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+  EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast<Derived*>(this); }
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    EIGEN_DEVICE_FUNC
-    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
-    EIGEN_DEVICE_FUNC
-    inline Derived& derived() { return *static_cast<Derived*>(this); }
-    #endif // not EIGEN_PARSED_BY_DOXYGEN
+  template <typename DenseDerived>
+  EIGEN_DEVICE_FUNC void evalTo(MatrixBase<DenseDerived>& other) const;
+  template <typename DenseDerived>
+  EIGEN_DEVICE_FUNC void evalToLazy(MatrixBase<DenseDerived>& other) const;
 
-    template<typename DenseDerived>
-    EIGEN_DEVICE_FUNC
-    void evalTo(MatrixBase<DenseDerived> &other) const;
-    template<typename DenseDerived>
-    EIGEN_DEVICE_FUNC
-    void evalToLazy(MatrixBase<DenseDerived> &other) const;
+  EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const {
+    DenseMatrixType res(rows(), cols());
+    evalToLazy(res);
+    return res;
+  }
 
-    EIGEN_DEVICE_FUNC
-    DenseMatrixType toDenseMatrix() const
-    {
-      DenseMatrixType res(rows(), cols());
-      evalToLazy(res);
-      return res;
-    }
+ protected:
+  void check_coordinates(Index row, Index col) const {
+    EIGEN_ONLY_USED_FOR_DEBUG(row);
+    EIGEN_ONLY_USED_FOR_DEBUG(col);
+    eigen_assert(col >= 0 && col < cols() && row >= 0 && row < rows());
+    const int mode = int(Mode) & ~SelfAdjoint;
+    EIGEN_ONLY_USED_FOR_DEBUG(mode);
+    eigen_assert((mode == Upper && col >= row) || (mode == Lower && col <= row) ||
+                 ((mode == StrictlyUpper || mode == UnitUpper) && col > row) ||
+                 ((mode == StrictlyLower || mode == UnitLower) && col < row));
+  }
 
-  protected:
-
-    void check_coordinates(Index row, Index col) const
-    {
-      EIGEN_ONLY_USED_FOR_DEBUG(row);
-      EIGEN_ONLY_USED_FOR_DEBUG(col);
-      eigen_assert(col>=0 && col<cols() && row>=0 && row<rows());
-      const int mode = int(Mode) & ~SelfAdjoint;
-      EIGEN_ONLY_USED_FOR_DEBUG(mode);
-      eigen_assert((mode==Upper && col>=row)
-                || (mode==Lower && col<=row)
-                || ((mode==StrictlyUpper || mode==UnitUpper) && col>row)
-                || ((mode==StrictlyLower || mode==UnitLower) && col<row));
-    }
-
-    #ifdef EIGEN_INTERNAL_DEBUGGING
-    void check_coordinates_internal(Index row, Index col) const
-    {
-      check_coordinates(row, col);
-    }
-    #else
-    void check_coordinates_internal(Index , Index ) const {}
-    #endif
-
+#ifdef EIGEN_INTERNAL_DEBUGGING
+  void check_coordinates_internal(Index row, Index col) const { check_coordinates(row, col); }
+#else
+  void check_coordinates_internal(Index, Index) const {}
+#endif
 };
 
 /** \class TriangularView
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a triangular part in a matrix
-  *
-  * \param MatrixType the type of the object in which we are taking the triangular part
-  * \param Mode the kind of triangular matrix expression to construct. Can be #Upper,
-  *             #Lower, #UnitUpper, #UnitLower, #StrictlyUpper, or #StrictlyLower.
-  *             This is in fact a bit field; it must have either #Upper or #Lower,
-  *             and additionally it may have #UnitDiag or #ZeroDiag or neither.
-  *
-  * This class represents a triangular part of a matrix, not necessarily square. Strictly speaking, for rectangular
-  * matrices one should speak of "trapezoid" parts. This class is the return type
-  * of MatrixBase::triangularView() and SparseMatrixBase::triangularView(), and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::triangularView()
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a triangular part in a matrix
+ *
+ * \tparam MatrixType the type of the object in which we are taking the triangular part
+ * \tparam Mode the kind of triangular matrix expression to construct. Can be #Upper,
+ *             #Lower, #UnitUpper, #UnitLower, #StrictlyUpper, or #StrictlyLower.
+ *             This is in fact a bit field; it must have either #Upper or #Lower,
+ *             and additionally it may have #UnitDiag or #ZeroDiag or neither.
+ *
+ * This class represents a triangular part of a matrix, not necessarily square. Strictly speaking, for rectangular
+ * matrices one should speak of "trapezoid" parts. This class is the return type
+ * of MatrixBase::triangularView() and SparseMatrixBase::triangularView(), and most of the time this is the only way it
+ * is used.
+ *
+ * \sa MatrixBase::triangularView()
+ */
 namespace internal {
-template<typename MatrixType, unsigned int _Mode>
-struct traits<TriangularView<MatrixType, _Mode> > : traits<MatrixType>
-{
+template <typename MatrixType, unsigned int Mode_>
+struct traits<TriangularView<MatrixType, Mode_>> : traits<MatrixType> {
   typedef typename ref_selector<MatrixType>::non_const_type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type MatrixTypeNestedNonRef;
-  typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
+  typedef std::remove_reference_t<MatrixTypeNested> MatrixTypeNestedNonRef;
+  typedef remove_all_t<MatrixTypeNested> MatrixTypeNestedCleaned;
   typedef typename MatrixType::PlainObject FullMatrixType;
   typedef MatrixType ExpressionType;
   enum {
-    Mode = _Mode,
+    Mode = Mode_,
     FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
-    Flags = (MatrixTypeNestedCleaned::Flags & (HereditaryBits | FlagsLvalueBit) & (~(PacketAccessBit | DirectAccessBit | LinearAccessBit)))
+    Flags = (MatrixTypeNestedCleaned::Flags & (HereditaryBits | FlagsLvalueBit) &
+             (~(PacketAccessBit | DirectAccessBit | LinearAccessBit)))
   };
 };
-}
+}  // namespace internal
 
-template<typename _MatrixType, unsigned int _Mode, typename StorageKind> class TriangularViewImpl;
+template <typename MatrixType_, unsigned int Mode_, typename StorageKind>
+class TriangularViewImpl;
 
-template<typename _MatrixType, unsigned int _Mode> class TriangularView
-  : public TriangularViewImpl<_MatrixType, _Mode, typename internal::traits<_MatrixType>::StorageKind >
-{
-  public:
+template <typename MatrixType_, unsigned int Mode_>
+class TriangularView
+    : public TriangularViewImpl<MatrixType_, Mode_, typename internal::traits<MatrixType_>::StorageKind> {
+ public:
+  typedef TriangularViewImpl<MatrixType_, Mode_, typename internal::traits<MatrixType_>::StorageKind> Base;
+  typedef typename internal::traits<TriangularView>::Scalar Scalar;
+  typedef MatrixType_ MatrixType;
 
-    typedef TriangularViewImpl<_MatrixType, _Mode, typename internal::traits<_MatrixType>::StorageKind > Base;
-    typedef typename internal::traits<TriangularView>::Scalar Scalar;
-    typedef _MatrixType MatrixType;
+ protected:
+  typedef typename internal::traits<TriangularView>::MatrixTypeNested MatrixTypeNested;
+  typedef typename internal::traits<TriangularView>::MatrixTypeNestedNonRef MatrixTypeNestedNonRef;
 
-  protected:
-    typedef typename internal::traits<TriangularView>::MatrixTypeNested MatrixTypeNested;
-    typedef typename internal::traits<TriangularView>::MatrixTypeNestedNonRef MatrixTypeNestedNonRef;
+  typedef internal::remove_all_t<typename MatrixType::ConjugateReturnType> MatrixConjugateReturnType;
+  typedef TriangularView<std::add_const_t<MatrixType>, Mode_> ConstTriangularView;
 
-    typedef typename internal::remove_all<typename MatrixType::ConjugateReturnType>::type MatrixConjugateReturnType;
-    typedef TriangularView<typename internal::add_const<MatrixType>::type, _Mode> ConstTriangularView;
+ public:
+  typedef typename internal::traits<TriangularView>::StorageKind StorageKind;
+  typedef typename internal::traits<TriangularView>::MatrixTypeNestedCleaned NestedExpression;
 
-  public:
+  enum {
+    Mode = Mode_,
+    Flags = internal::traits<TriangularView>::Flags,
+    TransposeMode = (Mode & Upper ? Lower : 0) | (Mode & Lower ? Upper : 0) | (Mode & (UnitDiag)) | (Mode & (ZeroDiag)),
+    IsVectorAtCompileTime = false
+  };
 
-    typedef typename internal::traits<TriangularView>::StorageKind StorageKind;
-    typedef typename internal::traits<TriangularView>::MatrixTypeNestedCleaned NestedExpression;
+  EIGEN_DEVICE_FUNC explicit inline TriangularView(MatrixType& matrix) : m_matrix(matrix) {}
 
-    enum {
-      Mode = _Mode,
-      Flags = internal::traits<TriangularView>::Flags,
-      TransposeMode = (Mode & Upper ? Lower : 0)
-                    | (Mode & Lower ? Upper : 0)
-                    | (Mode & (UnitDiag))
-                    | (Mode & (ZeroDiag)),
-      IsVectorAtCompileTime = false
-    };
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TriangularView)
 
-    EIGEN_DEVICE_FUNC
-    explicit inline TriangularView(MatrixType& matrix) : m_matrix(matrix)
-    {}
+  /** \copydoc EigenBase::rows() */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  /** \copydoc EigenBase::cols() */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TriangularView)
+  /** \returns a const reference to the nested expression */
+  EIGEN_DEVICE_FUNC const NestedExpression& nestedExpression() const { return m_matrix; }
 
-    /** \copydoc EigenBase::rows() */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    /** \copydoc EigenBase::cols() */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  /** \returns a reference to the nested expression */
+  EIGEN_DEVICE_FUNC NestedExpression& nestedExpression() { return m_matrix; }
 
-    /** \returns a const reference to the nested expression */
-    EIGEN_DEVICE_FUNC
-    const NestedExpression& nestedExpression() const { return m_matrix; }
+  typedef TriangularView<const MatrixConjugateReturnType, Mode> ConjugateReturnType;
+  /** \sa MatrixBase::conjugate() const */
+  EIGEN_DEVICE_FUNC inline const ConjugateReturnType conjugate() const {
+    return ConjugateReturnType(m_matrix.conjugate());
+  }
 
-    /** \returns a reference to the nested expression */
-    EIGEN_DEVICE_FUNC
-    NestedExpression& nestedExpression() { return m_matrix; }
+  /** \returns an expression of the complex conjugate of \c *this if Cond==true,
+   *           returns \c *this otherwise.
+   */
+  template <bool Cond>
+  EIGEN_DEVICE_FUNC inline std::conditional_t<Cond, ConjugateReturnType, ConstTriangularView> conjugateIf() const {
+    typedef std::conditional_t<Cond, ConjugateReturnType, ConstTriangularView> ReturnType;
+    return ReturnType(m_matrix.template conjugateIf<Cond>());
+  }
 
-    typedef TriangularView<const MatrixConjugateReturnType,Mode> ConjugateReturnType;
-    /** \sa MatrixBase::conjugate() const */
-    EIGEN_DEVICE_FUNC
-    inline const ConjugateReturnType conjugate() const
-    { return ConjugateReturnType(m_matrix.conjugate()); }
+  typedef TriangularView<const typename MatrixType::AdjointReturnType, TransposeMode> AdjointReturnType;
+  /** \sa MatrixBase::adjoint() const */
+  EIGEN_DEVICE_FUNC inline const AdjointReturnType adjoint() const { return AdjointReturnType(m_matrix.adjoint()); }
 
-    /** \returns an expression of the complex conjugate of \c *this if Cond==true,
-     *           returns \c *this otherwise.
-     */
-    template<bool Cond>
-    EIGEN_DEVICE_FUNC
-    inline typename internal::conditional<Cond,ConjugateReturnType,ConstTriangularView>::type
-    conjugateIf() const
-    {
-      typedef typename internal::conditional<Cond,ConjugateReturnType,ConstTriangularView>::type ReturnType;
-      return ReturnType(m_matrix.template conjugateIf<Cond>());
-    }
+  typedef TriangularView<typename MatrixType::TransposeReturnType, TransposeMode> TransposeReturnType;
+  /** \sa MatrixBase::transpose() */
+  template <class Dummy = int>
+  EIGEN_DEVICE_FUNC inline TransposeReturnType transpose(
+      std::enable_if_t<Eigen::internal::is_lvalue<MatrixType>::value, Dummy*> = nullptr) {
+    typename MatrixType::TransposeReturnType tmp(m_matrix);
+    return TransposeReturnType(tmp);
+  }
 
-    typedef TriangularView<const typename MatrixType::AdjointReturnType,TransposeMode> AdjointReturnType;
-    /** \sa MatrixBase::adjoint() const */
-    EIGEN_DEVICE_FUNC
-    inline const AdjointReturnType adjoint() const
-    { return AdjointReturnType(m_matrix.adjoint()); }
+  typedef TriangularView<const typename MatrixType::ConstTransposeReturnType, TransposeMode> ConstTransposeReturnType;
+  /** \sa MatrixBase::transpose() const */
+  EIGEN_DEVICE_FUNC inline const ConstTransposeReturnType transpose() const {
+    return ConstTransposeReturnType(m_matrix.transpose());
+  }
 
-    typedef TriangularView<typename MatrixType::TransposeReturnType,TransposeMode> TransposeReturnType;
-     /** \sa MatrixBase::transpose() */
-    EIGEN_DEVICE_FUNC
-    inline TransposeReturnType transpose()
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
-      typename MatrixType::TransposeReturnType tmp(m_matrix);
-      return TransposeReturnType(tmp);
-    }
+  template <typename Other>
+  EIGEN_DEVICE_FUNC inline const Solve<TriangularView, Other> solve(const MatrixBase<Other>& other) const {
+    return Solve<TriangularView, Other>(*this, other.derived());
+  }
 
-    typedef TriangularView<const typename MatrixType::ConstTransposeReturnType,TransposeMode> ConstTransposeReturnType;
-    /** \sa MatrixBase::transpose() const */
-    EIGEN_DEVICE_FUNC
-    inline const ConstTransposeReturnType transpose() const
-    {
-      return ConstTransposeReturnType(m_matrix.transpose());
-    }
+// workaround MSVC ICE
+#if EIGEN_COMP_MSVC
+  template <int Side, typename Other>
+  EIGEN_DEVICE_FUNC inline const internal::triangular_solve_retval<Side, TriangularView, Other> solve(
+      const MatrixBase<Other>& other) const {
+    return Base::template solve<Side>(other);
+  }
+#else
+  using Base::solve;
+#endif
 
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    inline const Solve<TriangularView, Other>
-    solve(const MatrixBase<Other>& other) const
-    { return Solve<TriangularView, Other>(*this, other.derived()); }
+  /** \returns a selfadjoint view of the referenced triangular part which must be either \c #Upper or \c #Lower.
+   *
+   * This is a shortcut for \code this->nestedExpression().selfadjointView<(*this)::Mode>() \endcode
+   * \sa MatrixBase::selfadjointView() */
+  EIGEN_DEVICE_FUNC SelfAdjointView<MatrixTypeNestedNonRef, Mode> selfadjointView() {
+    EIGEN_STATIC_ASSERT((Mode & (UnitDiag | ZeroDiag)) == 0, PROGRAMMING_ERROR);
+    return SelfAdjointView<MatrixTypeNestedNonRef, Mode>(m_matrix);
+  }
 
-  // workaround MSVC ICE
-  #if EIGEN_COMP_MSVC
-    template<int Side, typename Other>
-    EIGEN_DEVICE_FUNC
-    inline const internal::triangular_solve_retval<Side,TriangularView, Other>
-    solve(const MatrixBase<Other>& other) const
-    { return Base::template solve<Side>(other); }
-  #else
-    using Base::solve;
-  #endif
+  /** This is the const version of selfadjointView() */
+  EIGEN_DEVICE_FUNC const SelfAdjointView<MatrixTypeNestedNonRef, Mode> selfadjointView() const {
+    EIGEN_STATIC_ASSERT((Mode & (UnitDiag | ZeroDiag)) == 0, PROGRAMMING_ERROR);
+    return SelfAdjointView<MatrixTypeNestedNonRef, Mode>(m_matrix);
+  }
 
-    /** \returns a selfadjoint view of the referenced triangular part which must be either \c #Upper or \c #Lower.
-      *
-      * This is a shortcut for \code this->nestedExpression().selfadjointView<(*this)::Mode>() \endcode
-      * \sa MatrixBase::selfadjointView() */
-    EIGEN_DEVICE_FUNC
-    SelfAdjointView<MatrixTypeNestedNonRef,Mode> selfadjointView()
-    {
-      EIGEN_STATIC_ASSERT((Mode&(UnitDiag|ZeroDiag))==0,PROGRAMMING_ERROR);
-      return SelfAdjointView<MatrixTypeNestedNonRef,Mode>(m_matrix);
-    }
+  /** \returns the determinant of the triangular matrix
+   * \sa MatrixBase::determinant() */
+  EIGEN_DEVICE_FUNC Scalar determinant() const {
+    if (Mode & UnitDiag)
+      return 1;
+    else if (Mode & ZeroDiag)
+      return 0;
+    else
+      return m_matrix.diagonal().prod();
+  }
 
-    /** This is the const version of selfadjointView() */
-    EIGEN_DEVICE_FUNC
-    const SelfAdjointView<MatrixTypeNestedNonRef,Mode> selfadjointView() const
-    {
-      EIGEN_STATIC_ASSERT((Mode&(UnitDiag|ZeroDiag))==0,PROGRAMMING_ERROR);
-      return SelfAdjointView<MatrixTypeNestedNonRef,Mode>(m_matrix);
-    }
-
-
-    /** \returns the determinant of the triangular matrix
-      * \sa MatrixBase::determinant() */
-    EIGEN_DEVICE_FUNC
-    Scalar determinant() const
-    {
-      if (Mode & UnitDiag)
-        return 1;
-      else if (Mode & ZeroDiag)
-        return 0;
-      else
-        return m_matrix.diagonal().prod();
-    }
-
-  protected:
-
-    MatrixTypeNested m_matrix;
+ protected:
+  MatrixTypeNested m_matrix;
 };
 
 /** \ingroup Core_Module
-  *
-  * \brief Base class for a triangular part in a \b dense matrix
-  *
-  * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be instantiated.
-  * It extends class TriangularView with additional methods which available for dense expressions only.
-  *
-  * \sa class TriangularView, MatrixBase::triangularView()
-  */
-template<typename _MatrixType, unsigned int _Mode> class TriangularViewImpl<_MatrixType,_Mode,Dense>
-  : public TriangularBase<TriangularView<_MatrixType, _Mode> >
-{
-  public:
+ *
+ * \brief Base class for a triangular part in a \b dense matrix
+ *
+ * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be
+ * instantiated. It extends class TriangularView with additional methods which available for dense expressions only.
+ *
+ * \sa class TriangularView, MatrixBase::triangularView()
+ */
+template <typename MatrixType_, unsigned int Mode_>
+class TriangularViewImpl<MatrixType_, Mode_, Dense> : public TriangularBase<TriangularView<MatrixType_, Mode_>> {
+ public:
+  typedef TriangularView<MatrixType_, Mode_> TriangularViewType;
 
-    typedef TriangularView<_MatrixType, _Mode> TriangularViewType;
-    typedef TriangularBase<TriangularViewType> Base;
-    typedef typename internal::traits<TriangularViewType>::Scalar Scalar;
+  typedef TriangularBase<TriangularViewType> Base;
+  typedef typename internal::traits<TriangularViewType>::Scalar Scalar;
 
-    typedef _MatrixType MatrixType;
-    typedef typename MatrixType::PlainObject DenseMatrixType;
-    typedef DenseMatrixType PlainObject;
+  typedef MatrixType_ MatrixType;
+  typedef typename MatrixType::PlainObject DenseMatrixType;
+  typedef DenseMatrixType PlainObject;
 
-  public:
-    using Base::evalToLazy;
-    using Base::derived;
+ public:
+  using Base::derived;
+  using Base::evalToLazy;
 
-    typedef typename internal::traits<TriangularViewType>::StorageKind StorageKind;
+  typedef typename internal::traits<TriangularViewType>::StorageKind StorageKind;
 
-    enum {
-      Mode = _Mode,
-      Flags = internal::traits<TriangularViewType>::Flags
-    };
+  enum { Mode = Mode_, Flags = internal::traits<TriangularViewType>::Flags };
 
-    /** \returns the outer-stride of the underlying dense matrix
-      * \sa DenseCoeffsBase::outerStride() */
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const { return derived().nestedExpression().outerStride(); }
-    /** \returns the inner-stride of the underlying dense matrix
-      * \sa DenseCoeffsBase::innerStride() */
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const { return derived().nestedExpression().innerStride(); }
+  /** \returns the outer-stride of the underlying dense matrix
+   * \sa DenseCoeffsBase::outerStride() */
+  EIGEN_DEVICE_FUNC inline Index outerStride() const { return derived().nestedExpression().outerStride(); }
+  /** \returns the inner-stride of the underlying dense matrix
+   * \sa DenseCoeffsBase::innerStride() */
+  EIGEN_DEVICE_FUNC inline Index innerStride() const { return derived().nestedExpression().innerStride(); }
 
-    /** \sa MatrixBase::operator+=() */
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator+=(const DenseBase<Other>& other) {
-      internal::call_assignment_no_alias(derived(), other.derived(), internal::add_assign_op<Scalar,typename Other::Scalar>());
-      return derived();
-    }
-    /** \sa MatrixBase::operator-=() */
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator-=(const DenseBase<Other>& other) {
-      internal::call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op<Scalar,typename Other::Scalar>());
-      return derived();
-    }
+  /** \sa MatrixBase::operator+=() */
+  template <typename Other>
+  EIGEN_DEVICE_FUNC TriangularViewType& operator+=(const DenseBase<Other>& other) {
+    internal::call_assignment_no_alias(derived(), other.derived(),
+                                       internal::add_assign_op<Scalar, typename Other::Scalar>());
+    return derived();
+  }
+  /** \sa MatrixBase::operator-=() */
+  template <typename Other>
+  EIGEN_DEVICE_FUNC TriangularViewType& operator-=(const DenseBase<Other>& other) {
+    internal::call_assignment_no_alias(derived(), other.derived(),
+                                       internal::sub_assign_op<Scalar, typename Other::Scalar>());
+    return derived();
+  }
 
-    /** \sa MatrixBase::operator*=() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator*=(const typename internal::traits<MatrixType>::Scalar& other) { return *this = derived().nestedExpression() * other; }
-    /** \sa DenseBase::operator/=() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator/=(const typename internal::traits<MatrixType>::Scalar& other) { return *this = derived().nestedExpression() / other; }
+  /** \sa MatrixBase::operator*=() */
+  EIGEN_DEVICE_FUNC TriangularViewType& operator*=(const typename internal::traits<MatrixType>::Scalar& other) {
+    return *this = derived().nestedExpression() * other;
+  }
+  /** \sa DenseBase::operator/=() */
+  EIGEN_DEVICE_FUNC TriangularViewType& operator/=(const typename internal::traits<MatrixType>::Scalar& other) {
+    return *this = derived().nestedExpression() / other;
+  }
 
-    /** \sa MatrixBase::fill() */
-    EIGEN_DEVICE_FUNC
-    void fill(const Scalar& value) { setConstant(value); }
-    /** \sa MatrixBase::setConstant() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& setConstant(const Scalar& value)
-    { return *this = MatrixType::Constant(derived().rows(), derived().cols(), value); }
-    /** \sa MatrixBase::setZero() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& setZero() { return setConstant(Scalar(0)); }
-    /** \sa MatrixBase::setOnes() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& setOnes() { return setConstant(Scalar(1)); }
+  /** \sa MatrixBase::fill() */
+  EIGEN_DEVICE_FUNC void fill(const Scalar& value) { setConstant(value); }
+  /** \sa MatrixBase::setConstant() */
+  EIGEN_DEVICE_FUNC TriangularViewType& setConstant(const Scalar& value) {
+    return *this = MatrixType::Constant(derived().rows(), derived().cols(), value);
+  }
+  /** \sa MatrixBase::setZero() */
+  EIGEN_DEVICE_FUNC TriangularViewType& setZero() { return setConstant(Scalar(0)); }
+  /** \sa MatrixBase::setOnes() */
+  EIGEN_DEVICE_FUNC TriangularViewType& setOnes() { return setConstant(Scalar(1)); }
 
-    /** \sa MatrixBase::coeff()
-      * \warning the coordinates must fit into the referenced triangular part
-      */
-    EIGEN_DEVICE_FUNC
-    inline Scalar coeff(Index row, Index col) const
-    {
-      Base::check_coordinates_internal(row, col);
-      return derived().nestedExpression().coeff(row, col);
-    }
+  /** \sa MatrixBase::coeff()
+   * \warning the coordinates must fit into the referenced triangular part
+   */
+  EIGEN_DEVICE_FUNC inline Scalar coeff(Index row, Index col) const {
+    Base::check_coordinates_internal(row, col);
+    return derived().nestedExpression().coeff(row, col);
+  }
 
-    /** \sa MatrixBase::coeffRef()
-      * \warning the coordinates must fit into the referenced triangular part
-      */
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(TriangularViewType);
-      Base::check_coordinates_internal(row, col);
-      return derived().nestedExpression().coeffRef(row, col);
-    }
+  /** \sa MatrixBase::coeffRef()
+   * \warning the coordinates must fit into the referenced triangular part
+   */
+  EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) {
+    EIGEN_STATIC_ASSERT_LVALUE(TriangularViewType);
+    Base::check_coordinates_internal(row, col);
+    return derived().nestedExpression().coeffRef(row, col);
+  }
 
-    /** Assigns a triangular matrix to a triangular part of a dense matrix */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& operator=(const TriangularBase<OtherDerived>& other);
+  /** Assigns a triangular matrix to a triangular part of a dense matrix */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC TriangularViewType& operator=(const TriangularBase<OtherDerived>& other);
 
-    /** Shortcut for\code *this = other.other.triangularView<(*this)::Mode>() \endcode */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& operator=(const MatrixBase<OtherDerived>& other);
+  /** Shortcut for\code *this = other.other.triangularView<(*this)::Mode>() \endcode */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC TriangularViewType& operator=(const MatrixBase<OtherDerived>& other);
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& operator=(const TriangularViewImpl& other)
-    { return *this = other.derived().nestedExpression(); }
+  EIGEN_DEVICE_FUNC TriangularViewType& operator=(const TriangularViewImpl& other) {
+    return *this = other.derived().nestedExpression();
+  }
 
-    template<typename OtherDerived>
-    /** \deprecated */
-    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
-    void lazyAssign(const TriangularBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  /** \deprecated */
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC void lazyAssign(const TriangularBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    /** \deprecated */
-    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
-    void lazyAssign(const MatrixBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  /** \deprecated */
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC void lazyAssign(const MatrixBase<OtherDerived>& other);
 #endif
 
-    /** Efficient triangular matrix times vector/matrix product */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<TriangularViewType,OtherDerived>
-    operator*(const MatrixBase<OtherDerived>& rhs) const
-    {
-      return Product<TriangularViewType,OtherDerived>(derived(), rhs.derived());
-    }
+  /** Efficient triangular matrix times vector/matrix product */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC const Product<TriangularViewType, OtherDerived> operator*(
+      const MatrixBase<OtherDerived>& rhs) const {
+    return Product<TriangularViewType, OtherDerived>(derived(), rhs.derived());
+  }
 
-    /** Efficient vector/matrix times triangular matrix product */
-    template<typename OtherDerived> friend
-    EIGEN_DEVICE_FUNC
-    const Product<OtherDerived,TriangularViewType>
-    operator*(const MatrixBase<OtherDerived>& lhs, const TriangularViewImpl& rhs)
-    {
-      return Product<OtherDerived,TriangularViewType>(lhs.derived(),rhs.derived());
-    }
+  /** Efficient vector/matrix times triangular matrix product */
+  template <typename OtherDerived>
+  friend EIGEN_DEVICE_FUNC const Product<OtherDerived, TriangularViewType> operator*(
+      const MatrixBase<OtherDerived>& lhs, const TriangularViewImpl& rhs) {
+    return Product<OtherDerived, TriangularViewType>(lhs.derived(), rhs.derived());
+  }
 
-    /** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
-      *
-      * This function computes the inverse-matrix matrix product inverse(\c *this) * \a other if
-      * \a Side==OnTheLeft (the default), or the right-inverse-multiply  \a other * inverse(\c *this) if
-      * \a Side==OnTheRight.
-      *
-      * Note that the template parameter \c Side can be omitted, in which case \c Side==OnTheLeft
-      *
-      * The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
-      * diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
-      * is an upper (resp. lower) triangular matrix.
-      *
-      * Example: \include Triangular_solve.cpp
-      * Output: \verbinclude Triangular_solve.out
-      *
-      * This function returns an expression of the inverse-multiply and can works in-place if it is assigned
-      * to the same matrix or vector \a other.
-      *
-      * For users coming from BLAS, this function (and more specifically solveInPlace()) offer
-      * all the operations supported by the \c *TRSV and \c *TRSM BLAS routines.
-      *
-      * \sa TriangularView::solveInPlace()
-      */
-    template<int Side, typename Other>
-    inline const internal::triangular_solve_retval<Side,TriangularViewType, Other>
-    solve(const MatrixBase<Other>& other) const;
+  /** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
+   *
+   * This function computes the inverse-matrix matrix product inverse(\c *this) * \a other if
+   * \a Side==OnTheLeft (the default), or the right-inverse-multiply  \a other * inverse(\c *this) if
+   * \a Side==OnTheRight.
+   *
+   * Note that the template parameter \c Side can be omitted, in which case \c Side==OnTheLeft
+   *
+   * The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
+   * diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
+   * is an upper (resp. lower) triangular matrix.
+   *
+   * Example: \include Triangular_solve.cpp
+   * Output: \verbinclude Triangular_solve.out
+   *
+   * This function returns an expression of the inverse-multiply and can works in-place if it is assigned
+   * to the same matrix or vector \a other.
+   *
+   * For users coming from BLAS, this function (and more specifically solveInPlace()) offer
+   * all the operations supported by the \c *TRSV and \c *TRSM BLAS routines.
+   *
+   * \sa TriangularView::solveInPlace()
+   */
+  template <int Side, typename Other>
+  inline const internal::triangular_solve_retval<Side, TriangularViewType, Other> solve(
+      const MatrixBase<Other>& other) const;
 
-    /** "in-place" version of TriangularView::solve() where the result is written in \a other
-      *
-      * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
-      * This function will const_cast it, so constness isn't honored here.
-      *
-      * Note that the template parameter \c Side can be omitted, in which case \c Side==OnTheLeft
-      *
-      * See TriangularView:solve() for the details.
-      */
-    template<int Side, typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void solveInPlace(const MatrixBase<OtherDerived>& other) const;
+  /** "in-place" version of TriangularView::solve() where the result is written in \a other
+   *
+   * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
+   * This function will const_cast it, so constness isn't honored here.
+   *
+   * Note that the template parameter \c Side can be omitted, in which case \c Side==OnTheLeft
+   *
+   * See TriangularView:solve() for the details.
+   */
+  template <int Side, typename OtherDerived>
+  EIGEN_DEVICE_FUNC void solveInPlace(const MatrixBase<OtherDerived>& other) const;
 
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void solveInPlace(const MatrixBase<OtherDerived>& other) const
-    { return solveInPlace<OnTheLeft>(other); }
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC void solveInPlace(const MatrixBase<OtherDerived>& other) const {
+    return solveInPlace<OnTheLeft>(other);
+  }
 
-    /** Swaps the coefficients of the common triangular parts of two matrices */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+  /** Swaps the coefficients of the common triangular parts of two matrices */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC
 #ifdef EIGEN_PARSED_BY_DOXYGEN
-    void swap(TriangularBase<OtherDerived> &other)
+      void
+      swap(TriangularBase<OtherDerived>& other)
 #else
-    void swap(TriangularBase<OtherDerived> const & other)
+      void
+      swap(TriangularBase<OtherDerived> const& other)
 #endif
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
-      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
-    }
+  {
+    EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
+    call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
+  }
 
-    /** Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */
-    template<typename OtherDerived>
-    /** \deprecated */
-    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
-    void swap(MatrixBase<OtherDerived> const & other)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
-      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
-    }
+  /** Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */
+  template <typename OtherDerived>
+  /** \deprecated */
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC void swap(MatrixBase<OtherDerived> const& other) {
+    EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
+    call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
+  }
 
-    template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _solve_impl(const RhsType &rhs, DstType &dst) const {
-      if(!internal::is_same_dense(dst,rhs))
-        dst = rhs;
-      this->solveInPlace(dst);
-    }
+  template <typename RhsType, typename DstType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _solve_impl(const RhsType& rhs, DstType& dst) const {
+    if (!internal::is_same_dense(dst, rhs)) dst = rhs;
+    this->solveInPlace(dst);
+  }
 
-    template<typename ProductType>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE TriangularViewType& _assignProduct(const ProductType& prod, const Scalar& alpha, bool beta);
-  protected:
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(TriangularViewImpl)
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TriangularViewImpl)
+  template <typename ProductType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TriangularViewType& _assignProduct(const ProductType& prod, const Scalar& alpha,
+                                                                           bool beta);
 
+ protected:
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(TriangularViewImpl)
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TriangularViewImpl)
 };
 
 /***************************************************************************
-* Implementation of triangular evaluation/assignment
-***************************************************************************/
+ * Implementation of triangular evaluation/assignment
+ ***************************************************************************/
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 // FIXME should we keep that possibility
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC inline TriangularView<MatrixType, Mode>&
-TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const MatrixBase<OtherDerived>& other)
-{
-  internal::call_assignment_no_alias(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+template <typename MatrixType, unsigned int Mode>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline TriangularView<MatrixType, Mode>& TriangularViewImpl<MatrixType, Mode, Dense>::operator=(
+    const MatrixBase<OtherDerived>& other) {
+  internal::call_assignment_no_alias(derived(), other.derived(),
+                                     internal::assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
 // FIXME should we keep that possibility
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const MatrixBase<OtherDerived>& other)
-{
+template <typename MatrixType, unsigned int Mode>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const MatrixBase<OtherDerived>& other) {
   internal::call_assignment_no_alias(derived(), other.template triangularView<Mode>());
 }
 
-
-
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC inline TriangularView<MatrixType, Mode>&
-TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const TriangularBase<OtherDerived>& other)
-{
+template <typename MatrixType, unsigned int Mode>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline TriangularView<MatrixType, Mode>& TriangularViewImpl<MatrixType, Mode, Dense>::operator=(
+    const TriangularBase<OtherDerived>& other) {
   eigen_assert(Mode == int(OtherDerived::Mode));
   internal::call_assignment(derived(), other.derived());
   return derived();
 }
 
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const TriangularBase<OtherDerived>& other)
-{
+template <typename MatrixType, unsigned int Mode>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(
+    const TriangularBase<OtherDerived>& other) {
   eigen_assert(Mode == int(OtherDerived::Mode));
   internal::call_assignment_no_alias(derived(), other.derived());
 }
 #endif
 
 /***************************************************************************
-* Implementation of TriangularBase methods
-***************************************************************************/
+ * Implementation of TriangularBase methods
+ ***************************************************************************/
 
 /** Assigns a triangular or selfadjoint matrix to a dense matrix.
-  * If the matrix is triangular, the opposite part is set to zero. */
-template<typename Derived>
-template<typename DenseDerived>
-EIGEN_DEVICE_FUNC void TriangularBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
-{
+ * If the matrix is triangular, the opposite part is set to zero. */
+template <typename Derived>
+template <typename DenseDerived>
+EIGEN_DEVICE_FUNC void TriangularBase<Derived>::evalTo(MatrixBase<DenseDerived>& other) const {
   evalToLazy(other.derived());
 }
 
 /***************************************************************************
-* Implementation of TriangularView methods
-***************************************************************************/
+ * Implementation of TriangularView methods
+ ***************************************************************************/
 
 /***************************************************************************
-* Implementation of MatrixBase methods
-***************************************************************************/
+ * Implementation of MatrixBase methods
+ ***************************************************************************/
 
 /**
-  * \returns an expression of a triangular view extracted from the current matrix
-  *
-  * The parameter \a Mode can have the following values: \c #Upper, \c #StrictlyUpper, \c #UnitUpper,
-  * \c #Lower, \c #StrictlyLower, \c #UnitLower.
-  *
-  * Example: \include MatrixBase_triangularView.cpp
-  * Output: \verbinclude MatrixBase_triangularView.out
-  *
-  * \sa class TriangularView
-  */
-template<typename Derived>
-template<unsigned int Mode>
-EIGEN_DEVICE_FUNC
-typename MatrixBase<Derived>::template TriangularViewReturnType<Mode>::Type
-MatrixBase<Derived>::triangularView()
-{
+ * \returns an expression of a triangular view extracted from the current matrix
+ *
+ * The parameter \a Mode can have the following values: \c #Upper, \c #StrictlyUpper, \c #UnitUpper,
+ * \c #Lower, \c #StrictlyLower, \c #UnitLower.
+ *
+ * Example: \include MatrixBase_triangularView.cpp
+ * Output: \verbinclude MatrixBase_triangularView.out
+ *
+ * \sa class TriangularView
+ */
+template <typename Derived>
+template <unsigned int Mode>
+EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::template TriangularViewReturnType<Mode>::Type
+MatrixBase<Derived>::triangularView() {
   return typename TriangularViewReturnType<Mode>::Type(derived());
 }
 
 /** This is the const version of MatrixBase::triangularView() */
-template<typename Derived>
-template<unsigned int Mode>
-EIGEN_DEVICE_FUNC
-typename MatrixBase<Derived>::template ConstTriangularViewReturnType<Mode>::Type
-MatrixBase<Derived>::triangularView() const
-{
+template <typename Derived>
+template <unsigned int Mode>
+EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::template ConstTriangularViewReturnType<Mode>::Type
+MatrixBase<Derived>::triangularView() const {
   return typename ConstTriangularViewReturnType<Mode>::Type(derived());
 }
 
 /** \returns true if *this is approximately equal to an upper triangular matrix,
-  *          within the precision given by \a prec.
-  *
-  * \sa isLowerTriangular()
-  */
-template<typename Derived>
-bool MatrixBase<Derived>::isUpperTriangular(const RealScalar& prec) const
-{
+ *          within the precision given by \a prec.
+ *
+ * \sa isLowerTriangular()
+ */
+template <typename Derived>
+bool MatrixBase<Derived>::isUpperTriangular(const RealScalar& prec) const {
   RealScalar maxAbsOnUpperPart = static_cast<RealScalar>(-1);
-  for(Index j = 0; j < cols(); ++j)
-  {
-    Index maxi = numext::mini(j, rows()-1);
-    for(Index i = 0; i <= maxi; ++i)
-    {
-      RealScalar absValue = numext::abs(coeff(i,j));
-      if(absValue > maxAbsOnUpperPart) maxAbsOnUpperPart = absValue;
+  for (Index j = 0; j < cols(); ++j) {
+    Index maxi = numext::mini(j, rows() - 1);
+    for (Index i = 0; i <= maxi; ++i) {
+      RealScalar absValue = numext::abs(coeff(i, j));
+      if (absValue > maxAbsOnUpperPart) maxAbsOnUpperPart = absValue;
     }
   }
   RealScalar threshold = maxAbsOnUpperPart * prec;
-  for(Index j = 0; j < cols(); ++j)
-    for(Index i = j+1; i < rows(); ++i)
-      if(numext::abs(coeff(i, j)) > threshold) return false;
+  for (Index j = 0; j < cols(); ++j)
+    for (Index i = j + 1; i < rows(); ++i)
+      if (numext::abs(coeff(i, j)) > threshold) return false;
   return true;
 }
 
 /** \returns true if *this is approximately equal to a lower triangular matrix,
-  *          within the precision given by \a prec.
-  *
-  * \sa isUpperTriangular()
-  */
-template<typename Derived>
-bool MatrixBase<Derived>::isLowerTriangular(const RealScalar& prec) const
-{
+ *          within the precision given by \a prec.
+ *
+ * \sa isUpperTriangular()
+ */
+template <typename Derived>
+bool MatrixBase<Derived>::isLowerTriangular(const RealScalar& prec) const {
   RealScalar maxAbsOnLowerPart = static_cast<RealScalar>(-1);
-  for(Index j = 0; j < cols(); ++j)
-    for(Index i = j; i < rows(); ++i)
-    {
-      RealScalar absValue = numext::abs(coeff(i,j));
-      if(absValue > maxAbsOnLowerPart) maxAbsOnLowerPart = absValue;
+  for (Index j = 0; j < cols(); ++j)
+    for (Index i = j; i < rows(); ++i) {
+      RealScalar absValue = numext::abs(coeff(i, j));
+      if (absValue > maxAbsOnLowerPart) maxAbsOnLowerPart = absValue;
     }
   RealScalar threshold = maxAbsOnLowerPart * prec;
-  for(Index j = 1; j < cols(); ++j)
-  {
-    Index maxi = numext::mini(j, rows()-1);
-    for(Index i = 0; i < maxi; ++i)
-      if(numext::abs(coeff(i, j)) > threshold) return false;
+  for (Index j = 1; j < cols(); ++j) {
+    Index maxi = numext::mini(j, rows() - 1);
+    for (Index i = 0; i < maxi; ++i)
+      if (numext::abs(coeff(i, j)) > threshold) return false;
   }
   return true;
 }
 
-
 /***************************************************************************
 ****************************************************************************
 * Evaluators and Assignment of triangular expressions
@@ -715,92 +627,85 @@
 
 namespace internal {
 
-
 // TODO currently a triangular expression has the form TriangularView<.,.>
 //      in the future triangular-ness should be defined by the expression traits
-//      such that Transpose<TriangularView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
-template<typename MatrixType, unsigned int Mode>
-struct evaluator_traits<TriangularView<MatrixType,Mode> >
-{
+//      such that Transpose<TriangularView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make
+//      it work)
+template <typename MatrixType, unsigned int Mode>
+struct evaluator_traits<TriangularView<MatrixType, Mode>> {
   typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
   typedef typename glue_shapes<typename evaluator_traits<MatrixType>::Shape, TriangularShape>::type Shape;
 };
 
-template<typename MatrixType, unsigned int Mode>
-struct unary_evaluator<TriangularView<MatrixType,Mode>, IndexBased>
- : evaluator<typename internal::remove_all<MatrixType>::type>
-{
-  typedef TriangularView<MatrixType,Mode> XprType;
-  typedef evaluator<typename internal::remove_all<MatrixType>::type> Base;
-  EIGEN_DEVICE_FUNC
-  unary_evaluator(const XprType &xpr) : Base(xpr.nestedExpression()) {}
+template <typename MatrixType, unsigned int Mode>
+struct unary_evaluator<TriangularView<MatrixType, Mode>, IndexBased> : evaluator<internal::remove_all_t<MatrixType>> {
+  typedef TriangularView<MatrixType, Mode> XprType;
+  typedef evaluator<internal::remove_all_t<MatrixType>> Base;
+  EIGEN_DEVICE_FUNC unary_evaluator(const XprType& xpr) : Base(xpr.nestedExpression()) {}
 };
 
 // Additional assignment kinds:
-struct Triangular2Triangular    {};
-struct Triangular2Dense         {};
-struct Dense2Triangular         {};
+struct Triangular2Triangular {};
+struct Triangular2Dense {};
+struct Dense2Triangular {};
 
-
-template<typename Kernel, unsigned int Mode, int UnrollCount, bool ClearOpposite> struct triangular_assignment_loop;
-
+template <typename Kernel, unsigned int Mode, int UnrollCount, bool ClearOpposite>
+struct triangular_assignment_loop;
 
 /** \internal Specialization of the dense assignment kernel for triangular matrices.
-  * The main difference is that the triangular, diagonal, and opposite parts are processed through three different functions.
-  * \tparam UpLo must be either Lower or Upper
-  * \tparam Mode must be either 0, UnitDiag, ZeroDiag, or SelfAdjoint
-  */
-template<int UpLo, int Mode, int SetOpposite, typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor, int Version = Specialized>
-class triangular_dense_assignment_kernel : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version>
-{
-protected:
+ * The main difference is that the triangular, diagonal, and opposite parts are processed through three different
+ * functions. \tparam UpLo must be either Lower or Upper \tparam Mode must be either 0, UnitDiag, ZeroDiag, or
+ * SelfAdjoint
+ */
+template <int UpLo, int Mode, int SetOpposite, typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor,
+          int Version = Specialized>
+class triangular_dense_assignment_kernel
+    : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version> {
+ protected:
   typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version> Base;
   typedef typename Base::DstXprType DstXprType;
   typedef typename Base::SrcXprType SrcXprType;
   using Base::m_dst;
-  using Base::m_src;
   using Base::m_functor;
-public:
+  using Base::m_src;
 
+ public:
   typedef typename Base::DstEvaluatorType DstEvaluatorType;
   typedef typename Base::SrcEvaluatorType SrcEvaluatorType;
   typedef typename Base::Scalar Scalar;
   typedef typename Base::AssignmentTraits AssignmentTraits;
 
-
-  EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
-    : Base(dst, src, func, dstExpr)
-  {}
+  EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src,
+                                                       const Functor& func, DstXprType& dstExpr)
+      : Base(dst, src, func, dstExpr) {}
 
 #ifdef EIGEN_INTERNAL_DEBUGGING
-  EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col)
-  {
-    eigen_internal_assert(row!=col);
-    Base::assignCoeff(row,col);
+  EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col) {
+    eigen_internal_assert(row != col);
+    Base::assignCoeff(row, col);
   }
 #else
   using Base::assignCoeff;
 #endif
 
-  EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id)
-  {
-         if(Mode==UnitDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(1));
-    else if(Mode==ZeroDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(0));
-    else if(Mode==0)                       Base::assignCoeff(id,id);
+  EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id) {
+    if (Mode == UnitDiag && SetOpposite)
+      m_functor.assignCoeff(m_dst.coeffRef(id, id), Scalar(1));
+    else if (Mode == ZeroDiag && SetOpposite)
+      m_functor.assignCoeff(m_dst.coeffRef(id, id), Scalar(0));
+    else if (Mode == 0)
+      Base::assignCoeff(id, id);
   }
 
-  EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index row, Index col)
-  {
-    eigen_internal_assert(row!=col);
-    if(SetOpposite)
-      m_functor.assignCoeff(m_dst.coeffRef(row,col), Scalar(0));
+  EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index row, Index col) {
+    eigen_internal_assert(row != col);
+    if (SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(row, col), Scalar(0));
   }
 };
 
-template<int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType, typename Functor>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src, const Functor &func)
-{
+template <int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType, typename Functor>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src,
+                                                                           const Functor& func) {
   typedef evaluator<DstXprType> DstEvaluatorType;
   typedef evaluator<SrcXprType> SrcEvaluatorType;
 
@@ -808,194 +713,187 @@
 
   Index dstRows = src.rows();
   Index dstCols = src.cols();
-  if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-    dst.resize(dstRows, dstCols);
+  if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
   DstEvaluatorType dstEvaluator(dst);
 
-  typedef triangular_dense_assignment_kernel< Mode&(Lower|Upper),Mode&(UnitDiag|ZeroDiag|SelfAdjoint),SetOpposite,
-                                              DstEvaluatorType,SrcEvaluatorType,Functor> Kernel;
+  typedef triangular_dense_assignment_kernel<Mode&(Lower | Upper), Mode&(UnitDiag | ZeroDiag | SelfAdjoint),
+                                             SetOpposite, DstEvaluatorType, SrcEvaluatorType, Functor>
+      Kernel;
   Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
 
   enum {
-      unroll = DstXprType::SizeAtCompileTime != Dynamic
-            && SrcEvaluatorType::CoeffReadCost < HugeCost
-            && DstXprType::SizeAtCompileTime * (int(DstEvaluatorType::CoeffReadCost) + int(SrcEvaluatorType::CoeffReadCost)) / 2 <= EIGEN_UNROLLING_LIMIT
-    };
+    unroll = DstXprType::SizeAtCompileTime != Dynamic && SrcEvaluatorType::CoeffReadCost < HugeCost &&
+             DstXprType::SizeAtCompileTime *
+                     (int(DstEvaluatorType::CoeffReadCost) + int(SrcEvaluatorType::CoeffReadCost)) / 2 <=
+                 EIGEN_UNROLLING_LIMIT
+  };
 
-  triangular_assignment_loop<Kernel, Mode, unroll ? int(DstXprType::SizeAtCompileTime) : Dynamic, SetOpposite>::run(kernel);
+  triangular_assignment_loop<Kernel, Mode, unroll ? int(DstXprType::SizeAtCompileTime) : Dynamic, SetOpposite>::run(
+      kernel);
 }
 
-template<int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src)
-{
-  call_triangular_assignment_loop<Mode,SetOpposite>(dst, src, internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>());
+template <int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src) {
+  call_triangular_assignment_loop<Mode, SetOpposite>(
+      dst, src, internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>());
 }
 
-template<> struct AssignmentKind<TriangularShape,TriangularShape> { typedef Triangular2Triangular Kind; };
-template<> struct AssignmentKind<DenseShape,TriangularShape>      { typedef Triangular2Dense      Kind; };
-template<> struct AssignmentKind<TriangularShape,DenseShape>      { typedef Dense2Triangular      Kind; };
+template <>
+struct AssignmentKind<TriangularShape, TriangularShape> {
+  typedef Triangular2Triangular Kind;
+};
+template <>
+struct AssignmentKind<DenseShape, TriangularShape> {
+  typedef Triangular2Dense Kind;
+};
+template <>
+struct AssignmentKind<TriangularShape, DenseShape> {
+  typedef Dense2Triangular Kind;
+};
 
-
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Triangular>
-{
-  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Triangular> {
+  EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
     eigen_assert(int(DstXprType::Mode) == int(SrcXprType::Mode));
 
     call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);
   }
 };
 
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Dense>
-{
-  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Dense> {
+  EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
     call_triangular_assignment_loop<SrcXprType::Mode, (int(SrcXprType::Mode) & int(SelfAdjoint)) == 0>(dst, src, func);
   }
 };
 
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Dense2Triangular>
-{
-  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Dense2Triangular> {
+  EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
     call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);
   }
 };
 
-
-template<typename Kernel, unsigned int Mode, int UnrollCount, bool SetOpposite>
-struct triangular_assignment_loop
-{
+template <typename Kernel, unsigned int Mode, int UnrollCount, bool SetOpposite>
+struct triangular_assignment_loop {
   // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
   typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
   typedef typename DstEvaluatorType::XprType DstXprType;
 
   enum {
-    col = (UnrollCount-1) / DstXprType::RowsAtCompileTime,
-    row = (UnrollCount-1) % DstXprType::RowsAtCompileTime
+    col = (UnrollCount - 1) / DstXprType::RowsAtCompileTime,
+    row = (UnrollCount - 1) % DstXprType::RowsAtCompileTime
   };
 
   typedef typename Kernel::Scalar Scalar;
 
-  EIGEN_DEVICE_FUNC
-  static inline void run(Kernel &kernel)
-  {
-    triangular_assignment_loop<Kernel, Mode, UnrollCount-1, SetOpposite>::run(kernel);
+  EIGEN_DEVICE_FUNC static inline void run(Kernel& kernel) {
+    triangular_assignment_loop<Kernel, Mode, UnrollCount - 1, SetOpposite>::run(kernel);
 
-    if(row==col)
+    if (row == col)
       kernel.assignDiagonalCoeff(row);
-    else if( ((Mode&Lower) && row>col) || ((Mode&Upper) && row<col) )
-      kernel.assignCoeff(row,col);
-    else if(SetOpposite)
-      kernel.assignOppositeCoeff(row,col);
+    else if (((Mode & Lower) && row > col) || ((Mode & Upper) && row < col))
+      kernel.assignCoeff(row, col);
+    else if (SetOpposite)
+      kernel.assignOppositeCoeff(row, col);
   }
 };
 
 // prevent buggy user code from causing an infinite recursion
-template<typename Kernel, unsigned int Mode, bool SetOpposite>
-struct triangular_assignment_loop<Kernel, Mode, 0, SetOpposite>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(Kernel &) {}
+template <typename Kernel, unsigned int Mode, bool SetOpposite>
+struct triangular_assignment_loop<Kernel, Mode, 0, SetOpposite> {
+  EIGEN_DEVICE_FUNC static inline void run(Kernel&) {}
 };
 
-
-
 // TODO: experiment with a recursive assignment procedure splitting the current
 //       triangular part into one rectangular and two triangular parts.
 
-
-template<typename Kernel, unsigned int Mode, bool SetOpposite>
-struct triangular_assignment_loop<Kernel, Mode, Dynamic, SetOpposite>
-{
+template <typename Kernel, unsigned int Mode, bool SetOpposite>
+struct triangular_assignment_loop<Kernel, Mode, Dynamic, SetOpposite> {
   typedef typename Kernel::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  static inline void run(Kernel &kernel)
-  {
-    for(Index j = 0; j < kernel.cols(); ++j)
-    {
+  EIGEN_DEVICE_FUNC static inline void run(Kernel& kernel) {
+    for (Index j = 0; j < kernel.cols(); ++j) {
       Index maxi = numext::mini(j, kernel.rows());
       Index i = 0;
-      if (((Mode&Lower) && SetOpposite) || (Mode&Upper))
-      {
-        for(; i < maxi; ++i)
-          if(Mode&Upper) kernel.assignCoeff(i, j);
-          else           kernel.assignOppositeCoeff(i, j);
-      }
-      else
+      if (((Mode & Lower) && SetOpposite) || (Mode & Upper)) {
+        for (; i < maxi; ++i)
+          if (Mode & Upper)
+            kernel.assignCoeff(i, j);
+          else
+            kernel.assignOppositeCoeff(i, j);
+      } else
         i = maxi;
 
-      if(i<kernel.rows()) // then i==j
+      if (i < kernel.rows())  // then i==j
         kernel.assignDiagonalCoeff(i++);
 
-      if (((Mode&Upper) && SetOpposite) || (Mode&Lower))
-      {
-        for(; i < kernel.rows(); ++i)
-          if(Mode&Lower) kernel.assignCoeff(i, j);
-          else           kernel.assignOppositeCoeff(i, j);
+      if (((Mode & Upper) && SetOpposite) || (Mode & Lower)) {
+        for (; i < kernel.rows(); ++i)
+          if (Mode & Lower)
+            kernel.assignCoeff(i, j);
+          else
+            kernel.assignOppositeCoeff(i, j);
       }
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** Assigns a triangular or selfadjoint matrix to a dense matrix.
-  * If the matrix is triangular, the opposite part is set to zero. */
-template<typename Derived>
-template<typename DenseDerived>
-EIGEN_DEVICE_FUNC void TriangularBase<Derived>::evalToLazy(MatrixBase<DenseDerived> &other) const
-{
+ * If the matrix is triangular, the opposite part is set to zero. */
+template <typename Derived>
+template <typename DenseDerived>
+EIGEN_DEVICE_FUNC void TriangularBase<Derived>::evalToLazy(MatrixBase<DenseDerived>& other) const {
   other.derived().resize(this->rows(), this->cols());
-  internal::call_triangular_assignment_loop<Derived::Mode, (int(Derived::Mode) & int(SelfAdjoint)) == 0 /* SetOpposite */>(other.derived(), derived().nestedExpression());
+  internal::call_triangular_assignment_loop<Derived::Mode,
+                                            (int(Derived::Mode) & int(SelfAdjoint)) == 0 /* SetOpposite */>(
+      other.derived(), derived().nestedExpression());
 }
 
 namespace internal {
 
 // Triangular = Product
-template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
-{
-  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename SrcXprType::Scalar> &)
-  {
+template <typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs, Rhs, DefaultProduct>,
+                  internal::assign_op<Scalar, typename Product<Lhs, Rhs, DefaultProduct>::Scalar>, Dense2Triangular> {
+  typedef Product<Lhs, Rhs, DefaultProduct> SrcXprType;
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<Scalar, typename SrcXprType::Scalar>&) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     dst._assignProduct(src, Scalar(1), false);
   }
 };
 
 // Triangular += Product
-template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::add_assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
-{
-  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<Scalar,typename SrcXprType::Scalar> &)
-  {
+template <typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs, Rhs, DefaultProduct>,
+                  internal::add_assign_op<Scalar, typename Product<Lhs, Rhs, DefaultProduct>::Scalar>,
+                  Dense2Triangular> {
+  typedef Product<Lhs, Rhs, DefaultProduct> SrcXprType;
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::add_assign_op<Scalar, typename SrcXprType::Scalar>&) {
     dst._assignProduct(src, Scalar(1), true);
   }
 };
 
 // Triangular -= Product
-template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::sub_assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
-{
-  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<Scalar,typename SrcXprType::Scalar> &)
-  {
+template <typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs, Rhs, DefaultProduct>,
+                  internal::sub_assign_op<Scalar, typename Product<Lhs, Rhs, DefaultProduct>::Scalar>,
+                  Dense2Triangular> {
+  typedef Product<Lhs, Rhs, DefaultProduct> SrcXprType;
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::sub_assign_op<Scalar, typename SrcXprType::Scalar>&) {
     dst._assignProduct(src, Scalar(-1), true);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TRIANGULARMATRIX_H
+#endif  // EIGEN_TRIANGULARMATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h
index 71c5b95..5ac13eb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h
@@ -11,86 +11,73 @@
 #ifndef EIGEN_VECTORBLOCK_H
 #define EIGEN_VECTORBLOCK_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-template<typename VectorType, int Size>
+template <typename VectorType, int Size>
 struct traits<VectorBlock<VectorType, Size> >
-  : public traits<Block<VectorType,
-                     traits<VectorType>::Flags & RowMajorBit ? 1 : Size,
-                     traits<VectorType>::Flags & RowMajorBit ? Size : 1> >
-{
-};
-}
+    : public traits<Block<VectorType, traits<VectorType>::Flags & RowMajorBit ? 1 : Size,
+                          traits<VectorType>::Flags & RowMajorBit ? Size : 1> > {};
+}  // namespace internal
 
 /** \class VectorBlock
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a fixed-size or dynamic-size sub-vector
-  *
-  * \tparam VectorType the type of the object in which we are taking a sub-vector
-  * \tparam Size size of the sub-vector we are taking at compile time (optional)
-  *
-  * This class represents an expression of either a fixed-size or dynamic-size sub-vector.
-  * It is the return type of DenseBase::segment(Index,Index) and DenseBase::segment<int>(Index) and
-  * most of the time this is the only way it is used.
-  *
-  * However, if you want to directly manipulate sub-vector expressions,
-  * for instance if you want to write a function returning such an expression, you
-  * will need to use this class.
-  *
-  * Here is an example illustrating the dynamic case:
-  * \include class_VectorBlock.cpp
-  * Output: \verbinclude class_VectorBlock.out
-  *
-  * \note Even though this expression has dynamic size, in the case where \a VectorType
-  * has fixed size, this expression inherits a fixed maximal size which means that evaluating
-  * it does not cause a dynamic memory allocation.
-  *
-  * Here is an example illustrating the fixed-size case:
-  * \include class_FixedVectorBlock.cpp
-  * Output: \verbinclude class_FixedVectorBlock.out
-  *
-  * \sa class Block, DenseBase::segment(Index,Index,Index,Index), DenseBase::segment(Index,Index)
-  */
-template<typename VectorType, int Size> class VectorBlock
-  : public Block<VectorType,
-                     internal::traits<VectorType>::Flags & RowMajorBit ? 1 : Size,
-                     internal::traits<VectorType>::Flags & RowMajorBit ? Size : 1>
-{
-    typedef Block<VectorType,
-                     internal::traits<VectorType>::Flags & RowMajorBit ? 1 : Size,
-                     internal::traits<VectorType>::Flags & RowMajorBit ? Size : 1> Base;
-    enum {
-      IsColVector = !(internal::traits<VectorType>::Flags & RowMajorBit)
-    };
-  public:
-    EIGEN_DENSE_PUBLIC_INTERFACE(VectorBlock)
+ * \ingroup Core_Module
+ *
+ * \brief Expression of a fixed-size or dynamic-size sub-vector
+ *
+ * \tparam VectorType the type of the object in which we are taking a sub-vector
+ * \tparam Size size of the sub-vector we are taking at compile time (optional)
+ *
+ * This class represents an expression of either a fixed-size or dynamic-size sub-vector.
+ * It is the return type of DenseBase::segment(Index,Index) and DenseBase::segment<int>(Index) and
+ * most of the time this is the only way it is used.
+ *
+ * However, if you want to directly manipulate sub-vector expressions,
+ * for instance if you want to write a function returning such an expression, you
+ * will need to use this class.
+ *
+ * Here is an example illustrating the dynamic case:
+ * \include class_VectorBlock.cpp
+ * Output: \verbinclude class_VectorBlock.out
+ *
+ * \note Even though this expression has dynamic size, in the case where \a VectorType
+ * has fixed size, this expression inherits a fixed maximal size which means that evaluating
+ * it does not cause a dynamic memory allocation.
+ *
+ * Here is an example illustrating the fixed-size case:
+ * \include class_FixedVectorBlock.cpp
+ * Output: \verbinclude class_FixedVectorBlock.out
+ *
+ * \sa class Block, DenseBase::segment(Index,Index,Index,Index), DenseBase::segment(Index,Index)
+ */
+template <typename VectorType, int Size>
+class VectorBlock : public Block<VectorType, internal::traits<VectorType>::Flags & RowMajorBit ? 1 : Size,
+                                 internal::traits<VectorType>::Flags & RowMajorBit ? Size : 1> {
+  typedef Block<VectorType, internal::traits<VectorType>::Flags & RowMajorBit ? 1 : Size,
+                internal::traits<VectorType>::Flags & RowMajorBit ? Size : 1>
+      Base;
+  enum { IsColVector = !(internal::traits<VectorType>::Flags & RowMajorBit) };
 
-    using Base::operator=;
+ public:
+  EIGEN_DENSE_PUBLIC_INTERFACE(VectorBlock)
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(VectorBlock)
 
-    /** Dynamic-size constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    VectorBlock(VectorType& vector, Index start, Index size)
-      : Base(vector,
-             IsColVector ? start : 0, IsColVector ? 0 : start,
-             IsColVector ? size  : 1, IsColVector ? 1 : size)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock);
-    }
+  /** Dynamic-size constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE VectorBlock(VectorType& vector, Index start, Index size)
+      : Base(vector, IsColVector ? start : 0, IsColVector ? 0 : start, IsColVector ? size : 1, IsColVector ? 1 : size) {
+  }
 
-    /** Fixed-size constructor
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    VectorBlock(VectorType& vector, Index start)
-      : Base(vector, IsColVector ? start : 0, IsColVector ? 0 : start)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock);
-    }
+  /** Fixed-size constructor
+   */
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE VectorBlock(VectorType& vector, Index start)
+      : Base(vector, IsColVector ? start : 0, IsColVector ? 0 : start) {}
 };
 
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_VECTORBLOCK_H
+#endif  // EIGEN_VECTORBLOCK_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h
index 870f4f1..9887db6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h
@@ -11,774 +11,703 @@
 #ifndef EIGEN_PARTIAL_REDUX_H
 #define EIGEN_PARTIAL_REDUX_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \class PartialReduxExpr
-  * \ingroup Core_Module
-  *
-  * \brief Generic expression of a partially reduxed matrix
-  *
-  * \tparam MatrixType the type of the matrix we are applying the redux operation
-  * \tparam MemberOp type of the member functor
-  * \tparam Direction indicates the direction of the redux (#Vertical or #Horizontal)
-  *
-  * This class represents an expression of a partial redux operator of a matrix.
-  * It is the return type of some VectorwiseOp functions,
-  * and most of the time this is the only way it is used.
-  *
-  * \sa class VectorwiseOp
-  */
+ * \ingroup Core_Module
+ *
+ * \brief Generic expression of a partially reduxed matrix
+ *
+ * \tparam MatrixType the type of the matrix we are applying the redux operation
+ * \tparam MemberOp type of the member functor
+ * \tparam Direction indicates the direction of the redux (#Vertical or #Horizontal)
+ *
+ * This class represents an expression of a partial redux operator of a matrix.
+ * It is the return type of some VectorwiseOp functions,
+ * and most of the time this is the only way it is used.
+ *
+ * \sa class VectorwiseOp
+ */
 
-template< typename MatrixType, typename MemberOp, int Direction>
+template <typename MatrixType, typename MemberOp, int Direction>
 class PartialReduxExpr;
 
 namespace internal {
-template<typename MatrixType, typename MemberOp, int Direction>
-struct traits<PartialReduxExpr<MatrixType, MemberOp, Direction> >
- : traits<MatrixType>
-{
+template <typename MatrixType, typename MemberOp, int Direction>
+struct traits<PartialReduxExpr<MatrixType, MemberOp, Direction> > : traits<MatrixType> {
   typedef typename MemberOp::result_type Scalar;
   typedef typename traits<MatrixType>::StorageKind StorageKind;
   typedef typename traits<MatrixType>::XprKind XprKind;
   typedef typename MatrixType::Scalar InputScalar;
   enum {
-    RowsAtCompileTime = Direction==Vertical   ? 1 : MatrixType::RowsAtCompileTime,
-    ColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::ColsAtCompileTime,
-    MaxRowsAtCompileTime = Direction==Vertical   ? 1 : MatrixType::MaxRowsAtCompileTime,
-    MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime,
+    RowsAtCompileTime = Direction == Vertical ? 1 : MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = Direction == Horizontal ? 1 : MatrixType::ColsAtCompileTime,
+    MaxRowsAtCompileTime = Direction == Vertical ? 1 : MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = Direction == Horizontal ? 1 : MatrixType::MaxColsAtCompileTime,
     Flags = RowsAtCompileTime == 1 ? RowMajorBit : 0,
-    TraversalSize = Direction==Vertical ? MatrixType::RowsAtCompileTime :  MatrixType::ColsAtCompileTime
+    TraversalSize = Direction == Vertical ? MatrixType::RowsAtCompileTime : MatrixType::ColsAtCompileTime
   };
 };
-}
+}  // namespace internal
 
-template< typename MatrixType, typename MemberOp, int Direction>
-class PartialReduxExpr : public internal::dense_xpr_base< PartialReduxExpr<MatrixType, MemberOp, Direction> >::type,
-                         internal::no_assignment_operator
-{
-  public:
+template <typename MatrixType, typename MemberOp, int Direction>
+class PartialReduxExpr : public internal::dense_xpr_base<PartialReduxExpr<MatrixType, MemberOp, Direction> >::type,
+                         internal::no_assignment_operator {
+ public:
+  typedef typename internal::dense_xpr_base<PartialReduxExpr>::type Base;
+  EIGEN_DENSE_PUBLIC_INTERFACE(PartialReduxExpr)
 
-    typedef typename internal::dense_xpr_base<PartialReduxExpr>::type Base;
-    EIGEN_DENSE_PUBLIC_INTERFACE(PartialReduxExpr)
-
-    EIGEN_DEVICE_FUNC
-    explicit PartialReduxExpr(const MatrixType& mat, const MemberOp& func = MemberOp())
+  EIGEN_DEVICE_FUNC explicit PartialReduxExpr(const MatrixType& mat, const MemberOp& func = MemberOp())
       : m_matrix(mat), m_functor(func) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return (Direction==Vertical   ? 1 : m_matrix.rows()); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return (Direction==Horizontal ? 1 : m_matrix.cols()); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT {
+    return (Direction == Vertical ? 1 : m_matrix.rows());
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT {
+    return (Direction == Horizontal ? 1 : m_matrix.cols());
+  }
 
-    EIGEN_DEVICE_FUNC
-    typename MatrixType::Nested nestedExpression() const { return m_matrix; }
+  EIGEN_DEVICE_FUNC typename MatrixType::Nested nestedExpression() const { return m_matrix; }
 
-    EIGEN_DEVICE_FUNC
-    const MemberOp& functor() const { return m_functor; }
+  EIGEN_DEVICE_FUNC const MemberOp& functor() const { return m_functor; }
 
-  protected:
-    typename MatrixType::Nested m_matrix;
-    const MemberOp m_functor;
+ protected:
+  typename MatrixType::Nested m_matrix;
+  const MemberOp m_functor;
 };
 
-template<typename A,typename B> struct partial_redux_dummy_func;
+template <typename A, typename B>
+struct partial_redux_dummy_func;
 
-#define EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(MEMBER,COST,VECTORIZABLE,BINARYOP)                \
-  template <typename ResultType,typename Scalar>                                                            \
+#define EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(MEMBER, COST, VECTORIZABLE, BINARYOP)              \
+  template <typename ResultType, typename Scalar>                                           \
   struct member_##MEMBER {                                                                  \
-    EIGEN_EMPTY_STRUCT_CTOR(member_##MEMBER)                                                \
     typedef ResultType result_type;                                                         \
-    typedef BINARYOP<Scalar,Scalar> BinaryOp;   \
-    template<int Size> struct Cost { enum { value = COST }; };             \
+    typedef BINARYOP<Scalar, Scalar> BinaryOp;                                              \
+    template <int Size>                                                                     \
+    struct Cost {                                                                           \
+      enum { value = COST };                                                                \
+    };                                                                                      \
     enum { Vectorizable = VECTORIZABLE };                                                   \
-    template<typename XprType>                                                              \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                                                   \
-    ResultType operator()(const XprType& mat) const                                         \
-    { return mat.MEMBER(); }                                                                \
+    template <typename XprType>                                                             \
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType operator()(const XprType& mat) const { \
+      return mat.MEMBER();                                                                  \
+    }                                                                                       \
     BinaryOp binaryFunc() const { return BinaryOp(); }                                      \
   }
 
-#define EIGEN_MEMBER_FUNCTOR(MEMBER,COST) \
-  EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(MEMBER,COST,0,partial_redux_dummy_func)
+#define EIGEN_MEMBER_FUNCTOR(MEMBER, COST) EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(MEMBER, COST, 0, partial_redux_dummy_func)
 
 namespace internal {
 
-EIGEN_MEMBER_FUNCTOR(norm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(stableNorm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(blueNorm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(hypotNorm, (Size-1) * functor_traits<scalar_hypot_op<Scalar> >::Cost );
-EIGEN_MEMBER_FUNCTOR(all, (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(any, (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(count, (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(norm, (Size + 5) * NumTraits<Scalar>::MulCost + (Size - 1) * NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(stableNorm, (Size + 5) * NumTraits<Scalar>::MulCost + (Size - 1) * NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(blueNorm, (Size + 5) * NumTraits<Scalar>::MulCost + (Size - 1) * NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(hypotNorm, (Size - 1) * functor_traits<scalar_hypot_op<Scalar> >::Cost);
+EIGEN_MEMBER_FUNCTOR(all, (Size - 1) * NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(any, (Size - 1) * NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(count, (Size - 1) * NumTraits<Scalar>::AddCost);
 
-EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(sum, (Size-1)*NumTraits<Scalar>::AddCost, 1, internal::scalar_sum_op);
-EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(minCoeff, (Size-1)*NumTraits<Scalar>::AddCost, 1, internal::scalar_min_op);
-EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(maxCoeff, (Size-1)*NumTraits<Scalar>::AddCost, 1, internal::scalar_max_op);
-EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(prod, (Size-1)*NumTraits<Scalar>::MulCost, 1, internal::scalar_product_op);
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(sum, (Size - 1) * NumTraits<Scalar>::AddCost, 1, internal::scalar_sum_op);
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(minCoeff, (Size - 1) * NumTraits<Scalar>::AddCost, 1, internal::scalar_min_op);
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(maxCoeff, (Size - 1) * NumTraits<Scalar>::AddCost, 1, internal::scalar_max_op);
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(prod, (Size - 1) * NumTraits<Scalar>::MulCost, 1, internal::scalar_product_op);
 
-template <int p, typename ResultType,typename Scalar>
+template <int p, typename ResultType, typename Scalar>
 struct member_lpnorm {
   typedef ResultType result_type;
   enum { Vectorizable = 0 };
-  template<int Size> struct Cost
-  { enum { value = (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost }; };
+  template <int Size>
+  struct Cost {
+    enum { value = (Size + 5) * NumTraits<Scalar>::MulCost + (Size - 1) * NumTraits<Scalar>::AddCost };
+  };
   EIGEN_DEVICE_FUNC member_lpnorm() {}
-  template<typename XprType>
-  EIGEN_DEVICE_FUNC inline ResultType operator()(const XprType& mat) const
-  { return mat.template lpNorm<p>(); }
+  template <typename XprType>
+  EIGEN_DEVICE_FUNC inline ResultType operator()(const XprType& mat) const {
+    return mat.template lpNorm<p>();
+  }
 };
 
 template <typename BinaryOpT, typename Scalar>
 struct member_redux {
   typedef BinaryOpT BinaryOp;
-  typedef typename result_of<
-                     BinaryOp(const Scalar&,const Scalar&)
-                   >::type  result_type;
+  typedef typename result_of<BinaryOp(const Scalar&, const Scalar&)>::type result_type;
 
   enum { Vectorizable = functor_traits<BinaryOp>::PacketAccess };
-  template<int Size> struct Cost { enum { value = (Size-1) * functor_traits<BinaryOp>::Cost }; };
+  template <int Size>
+  struct Cost {
+    enum { value = (Size - 1) * functor_traits<BinaryOp>::Cost };
+  };
   EIGEN_DEVICE_FUNC explicit member_redux(const BinaryOp func) : m_functor(func) {}
-  template<typename Derived>
-  EIGEN_DEVICE_FUNC inline result_type operator()(const DenseBase<Derived>& mat) const
-  { return mat.redux(m_functor); }
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline result_type operator()(const DenseBase<Derived>& mat) const {
+    return mat.redux(m_functor);
+  }
   const BinaryOp& binaryFunc() const { return m_functor; }
   const BinaryOp m_functor;
 };
-}
+}  // namespace internal
 
 /** \class VectorwiseOp
-  * \ingroup Core_Module
-  *
-  * \brief Pseudo expression providing broadcasting and partial reduction operations
-  *
-  * \tparam ExpressionType the type of the object on which to do partial reductions
-  * \tparam Direction indicates whether to operate on columns (#Vertical) or rows (#Horizontal)
-  *
-  * This class represents a pseudo expression with broadcasting and partial reduction features.
-  * It is the return type of DenseBase::colwise() and DenseBase::rowwise()
-  * and most of the time this is the only way it is explicitly used.
-  *
-  * To understand the logic of rowwise/colwise expression, let's consider a generic case `A.colwise().foo()`
-  * where `foo` is any method of `VectorwiseOp`. This expression is equivalent to applying `foo()` to each
-  * column of `A` and then re-assemble the outputs in a matrix expression:
-  * \code [A.col(0).foo(), A.col(1).foo(), ..., A.col(A.cols()-1).foo()] \endcode
-  *
-  * Example: \include MatrixBase_colwise.cpp
-  * Output: \verbinclude MatrixBase_colwise.out
-  *
-  * The begin() and end() methods are obviously exceptions to the previous rule as they
-  * return STL-compatible begin/end iterators to the rows or columns of the nested expression.
-  * Typical use cases include for-range-loop and calls to STL algorithms:
-  *
-  * Example: \include MatrixBase_colwise_iterator_cxx11.cpp
-  * Output: \verbinclude MatrixBase_colwise_iterator_cxx11.out
-  *
-  * For a partial reduction on an empty input, some rules apply.
-  * For the sake of clarity, let's consider a vertical reduction:
-  *   - If the number of columns is zero, then a 1x0 row-major vector expression is returned.
-  *   - Otherwise, if the number of rows is zero, then
-  *       - a row vector of zeros is returned for sum-like reductions (sum, squaredNorm, norm, etc.)
-  *       - a row vector of ones is returned for a product reduction (e.g., <code>MatrixXd(n,0).colwise().prod()</code>)
-  *       - an assert is triggered for all other reductions (minCoeff,maxCoeff,redux(bin_op))
-  *
-  * \sa DenseBase::colwise(), DenseBase::rowwise(), class PartialReduxExpr
-  */
-template<typename ExpressionType, int Direction> class VectorwiseOp
-{
-  public:
+ * \ingroup Core_Module
+ *
+ * \brief Pseudo expression providing broadcasting and partial reduction operations
+ *
+ * \tparam ExpressionType the type of the object on which to do partial reductions
+ * \tparam Direction indicates whether to operate on columns (#Vertical) or rows (#Horizontal)
+ *
+ * This class represents a pseudo expression with broadcasting and partial reduction features.
+ * It is the return type of DenseBase::colwise() and DenseBase::rowwise()
+ * and most of the time this is the only way it is explicitly used.
+ *
+ * To understand the logic of rowwise/colwise expression, let's consider a generic case `A.colwise().foo()`
+ * where `foo` is any method of `VectorwiseOp`. This expression is equivalent to applying `foo()` to each
+ * column of `A` and then re-assemble the outputs in a matrix expression:
+ * \code [A.col(0).foo(), A.col(1).foo(), ..., A.col(A.cols()-1).foo()] \endcode
+ *
+ * Example: \include MatrixBase_colwise.cpp
+ * Output: \verbinclude MatrixBase_colwise.out
+ *
+ * The begin() and end() methods are obviously exceptions to the previous rule as they
+ * return STL-compatible begin/end iterators to the rows or columns of the nested expression.
+ * Typical use cases include for-range-loop and calls to STL algorithms:
+ *
+ * Example: \include MatrixBase_colwise_iterator_cxx11.cpp
+ * Output: \verbinclude MatrixBase_colwise_iterator_cxx11.out
+ *
+ * For a partial reduction on an empty input, some rules apply.
+ * For the sake of clarity, let's consider a vertical reduction:
+ *   - If the number of columns is zero, then a 1x0 row-major vector expression is returned.
+ *   - Otherwise, if the number of rows is zero, then
+ *       - a row vector of zeros is returned for sum-like reductions (sum, squaredNorm, norm, etc.)
+ *       - a row vector of ones is returned for a product reduction (e.g., <code>MatrixXd(n,0).colwise().prod()</code>)
+ *       - an assert is triggered for all other reductions (minCoeff,maxCoeff,redux(bin_op))
+ *
+ * \sa DenseBase::colwise(), DenseBase::rowwise(), class PartialReduxExpr
+ */
+template <typename ExpressionType, int Direction>
+class VectorwiseOp {
+ public:
+  typedef typename ExpressionType::Scalar Scalar;
+  typedef typename ExpressionType::RealScalar RealScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
+  typedef typename internal::ref_selector<ExpressionType>::non_const_type ExpressionTypeNested;
+  typedef internal::remove_all_t<ExpressionTypeNested> ExpressionTypeNestedCleaned;
 
-    typedef typename ExpressionType::Scalar Scalar;
-    typedef typename ExpressionType::RealScalar RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    typedef typename internal::ref_selector<ExpressionType>::non_const_type ExpressionTypeNested;
-    typedef typename internal::remove_all<ExpressionTypeNested>::type ExpressionTypeNestedCleaned;
+  template <template <typename OutScalar, typename InputScalar> class Functor, typename ReturnScalar = Scalar>
+  struct ReturnType {
+    typedef PartialReduxExpr<ExpressionType, Functor<ReturnScalar, Scalar>, Direction> Type;
+  };
 
-    template<template<typename OutScalar,typename InputScalar> class Functor,
-                      typename ReturnScalar=Scalar> struct ReturnType
-    {
-      typedef PartialReduxExpr<ExpressionType,
-                               Functor<ReturnScalar,Scalar>,
-                               Direction
-                              > Type;
-    };
+  template <typename BinaryOp>
+  struct ReduxReturnType {
+    typedef PartialReduxExpr<ExpressionType, internal::member_redux<BinaryOp, Scalar>, Direction> Type;
+  };
 
-    template<typename BinaryOp> struct ReduxReturnType
-    {
-      typedef PartialReduxExpr<ExpressionType,
-                               internal::member_redux<BinaryOp,Scalar>,
-                               Direction
-                              > Type;
-    };
+  enum { isVertical = (Direction == Vertical) ? 1 : 0, isHorizontal = (Direction == Horizontal) ? 1 : 0 };
 
-    enum {
-      isVertical   = (Direction==Vertical) ? 1 : 0,
-      isHorizontal = (Direction==Horizontal) ? 1 : 0
-    };
+ protected:
+  template <typename OtherDerived>
+  struct ExtendedType {
+    typedef Replicate<OtherDerived, isVertical ? 1 : ExpressionType::RowsAtCompileTime,
+                      isHorizontal ? 1 : ExpressionType::ColsAtCompileTime>
+        Type;
+  };
 
-  protected:
+  /** \internal
+   * Replicates a vector to match the size of \c *this */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC typename ExtendedType<OtherDerived>::Type extendedTo(const DenseBase<OtherDerived>& other) const {
+    EIGEN_STATIC_ASSERT(internal::check_implication(isVertical, OtherDerived::MaxColsAtCompileTime == 1),
+                        YOU_PASSED_A_ROW_VECTOR_BUT_A_COLUMN_VECTOR_WAS_EXPECTED)
+    EIGEN_STATIC_ASSERT(internal::check_implication(isHorizontal, OtherDerived::MaxRowsAtCompileTime == 1),
+                        YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED)
+    return typename ExtendedType<OtherDerived>::Type(other.derived(), isVertical ? 1 : m_matrix.rows(),
+                                                     isHorizontal ? 1 : m_matrix.cols());
+  }
 
-    template<typename OtherDerived> struct ExtendedType {
-      typedef Replicate<OtherDerived,
-                        isVertical   ? 1 : ExpressionType::RowsAtCompileTime,
-                        isHorizontal ? 1 : ExpressionType::ColsAtCompileTime> Type;
-    };
+  template <typename OtherDerived>
+  struct OppositeExtendedType {
+    typedef Replicate<OtherDerived, isHorizontal ? 1 : ExpressionType::RowsAtCompileTime,
+                      isVertical ? 1 : ExpressionType::ColsAtCompileTime>
+        Type;
+  };
 
-    /** \internal
-      * Replicates a vector to match the size of \c *this */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    typename ExtendedType<OtherDerived>::Type
-    extendedTo(const DenseBase<OtherDerived>& other) const
-    {
-      EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(isVertical, OtherDerived::MaxColsAtCompileTime==1),
-                          YOU_PASSED_A_ROW_VECTOR_BUT_A_COLUMN_VECTOR_WAS_EXPECTED)
-      EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(isHorizontal, OtherDerived::MaxRowsAtCompileTime==1),
-                          YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED)
-      return typename ExtendedType<OtherDerived>::Type
-                      (other.derived(),
-                       isVertical   ? 1 : m_matrix.rows(),
-                       isHorizontal ? 1 : m_matrix.cols());
-    }
+  /** \internal
+   * Replicates a vector in the opposite direction to match the size of \c *this */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC typename OppositeExtendedType<OtherDerived>::Type extendedToOpposite(
+      const DenseBase<OtherDerived>& other) const {
+    EIGEN_STATIC_ASSERT(internal::check_implication(isHorizontal, OtherDerived::MaxColsAtCompileTime == 1),
+                        YOU_PASSED_A_ROW_VECTOR_BUT_A_COLUMN_VECTOR_WAS_EXPECTED)
+    EIGEN_STATIC_ASSERT(internal::check_implication(isVertical, OtherDerived::MaxRowsAtCompileTime == 1),
+                        YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED)
+    return typename OppositeExtendedType<OtherDerived>::Type(other.derived(), isHorizontal ? 1 : m_matrix.rows(),
+                                                             isVertical ? 1 : m_matrix.cols());
+  }
 
-    template<typename OtherDerived> struct OppositeExtendedType {
-      typedef Replicate<OtherDerived,
-                        isHorizontal ? 1 : ExpressionType::RowsAtCompileTime,
-                        isVertical   ? 1 : ExpressionType::ColsAtCompileTime> Type;
-    };
+ public:
+  EIGEN_DEVICE_FUNC explicit inline VectorwiseOp(ExpressionType& matrix) : m_matrix(matrix) {}
 
-    /** \internal
-      * Replicates a vector in the opposite direction to match the size of \c *this */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    typename OppositeExtendedType<OtherDerived>::Type
-    extendedToOpposite(const DenseBase<OtherDerived>& other) const
-    {
-      EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(isHorizontal, OtherDerived::MaxColsAtCompileTime==1),
-                          YOU_PASSED_A_ROW_VECTOR_BUT_A_COLUMN_VECTOR_WAS_EXPECTED)
-      EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(isVertical, OtherDerived::MaxRowsAtCompileTime==1),
-                          YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED)
-      return typename OppositeExtendedType<OtherDerived>::Type
-                      (other.derived(),
-                       isHorizontal  ? 1 : m_matrix.rows(),
-                       isVertical    ? 1 : m_matrix.cols());
-    }
+  /** \internal */
+  EIGEN_DEVICE_FUNC inline const ExpressionType& _expression() const { return m_matrix; }
 
-  public:
-    EIGEN_DEVICE_FUNC
-    explicit inline VectorwiseOp(ExpressionType& matrix) : m_matrix(matrix) {}
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** STL-like <a href="https://en.cppreference.com/w/cpp/named_req/RandomAccessIterator">RandomAccessIterator</a>
+   * iterator type over the columns or rows as returned by the begin() and end() methods.
+   */
+  random_access_iterator_type iterator;
+  /** This is the const version of iterator (aka read-only) */
+  random_access_iterator_type const_iterator;
+#else
+  typedef internal::subvector_stl_iterator<ExpressionType, DirectionType(Direction)> iterator;
+  typedef internal::subvector_stl_iterator<const ExpressionType, DirectionType(Direction)> const_iterator;
+  typedef internal::subvector_stl_reverse_iterator<ExpressionType, DirectionType(Direction)> reverse_iterator;
+  typedef internal::subvector_stl_reverse_iterator<const ExpressionType, DirectionType(Direction)>
+      const_reverse_iterator;
+#endif
 
-    /** \internal */
-    EIGEN_DEVICE_FUNC
-    inline const ExpressionType& _expression() const { return m_matrix; }
+  /** returns an iterator to the first row (rowwise) or column (colwise) of the nested expression.
+   * \sa end(), cbegin()
+   */
+  iterator begin() { return iterator(m_matrix, 0); }
+  /** const version of begin() */
+  const_iterator begin() const { return const_iterator(m_matrix, 0); }
+  /** const version of begin() */
+  const_iterator cbegin() const { return const_iterator(m_matrix, 0); }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** STL-like <a href="https://en.cppreference.com/w/cpp/named_req/RandomAccessIterator">RandomAccessIterator</a>
-      * iterator type over the columns or rows as returned by the begin() and end() methods.
-      */
-    random_access_iterator_type iterator;
-    /** This is the const version of iterator (aka read-only) */
-    random_access_iterator_type const_iterator;
-    #else
-    typedef internal::subvector_stl_iterator<ExpressionType,               DirectionType(Direction)> iterator;
-    typedef internal::subvector_stl_iterator<const ExpressionType,         DirectionType(Direction)> const_iterator;
-    typedef internal::subvector_stl_reverse_iterator<ExpressionType,       DirectionType(Direction)> reverse_iterator;
-    typedef internal::subvector_stl_reverse_iterator<const ExpressionType, DirectionType(Direction)> const_reverse_iterator;
-    #endif
+  /** returns a reverse iterator to the last row (rowwise) or column (colwise) of the nested expression.
+   * \sa rend(), crbegin()
+   */
+  reverse_iterator rbegin() {
+    return reverse_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>() - 1);
+  }
+  /** const version of rbegin() */
+  const_reverse_iterator rbegin() const {
+    return const_reverse_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>() - 1);
+  }
+  /** const version of rbegin() */
+  const_reverse_iterator crbegin() const {
+    return const_reverse_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>() - 1);
+  }
 
-    /** returns an iterator to the first row (rowwise) or column (colwise) of the nested expression.
-      * \sa end(), cbegin()
-      */
-    iterator                 begin()       { return iterator      (m_matrix, 0); }
-    /** const version of begin() */
-    const_iterator           begin() const { return const_iterator(m_matrix, 0); }
-    /** const version of begin() */
-    const_iterator          cbegin() const { return const_iterator(m_matrix, 0); }
+  /** returns an iterator to the row (resp. column) following the last row (resp. column) of the nested expression
+   * \sa begin(), cend()
+   */
+  iterator end() { return iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()); }
+  /** const version of end() */
+  const_iterator end() const {
+    return const_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>());
+  }
+  /** const version of end() */
+  const_iterator cend() const {
+    return const_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>());
+  }
 
-    /** returns a reverse iterator to the last row (rowwise) or column (colwise) of the nested expression.
-      * \sa rend(), crbegin()
-      */
-    reverse_iterator        rbegin()       { return reverse_iterator       (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()-1); }
-	/** const version of rbegin() */
-    const_reverse_iterator  rbegin() const { return const_reverse_iterator (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()-1); }
-	/** const version of rbegin() */
-	const_reverse_iterator crbegin() const { return const_reverse_iterator (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()-1); }
+  /** returns a reverse iterator to the row (resp. column) before the first row (resp. column) of the nested expression
+   * \sa begin(), cend()
+   */
+  reverse_iterator rend() { return reverse_iterator(m_matrix, -1); }
+  /** const version of rend() */
+  const_reverse_iterator rend() const { return const_reverse_iterator(m_matrix, -1); }
+  /** const version of rend() */
+  const_reverse_iterator crend() const { return const_reverse_iterator(m_matrix, -1); }
 
-    /** returns an iterator to the row (resp. column) following the last row (resp. column) of the nested expression
-      * \sa begin(), cend()
-      */
-    iterator                 end()         { return iterator      (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()); }
-    /** const version of end() */
-    const_iterator           end()  const  { return const_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()); }
-    /** const version of end() */
-    const_iterator          cend()  const  { return const_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()); }
+  /** \returns a row or column vector expression of \c *this reduxed by \a func
+   *
+   * The template parameter \a BinaryOp is the type of the functor
+   * of the custom redux operator. Note that func must be an associative operator.
+   *
+   * \warning the size along the reduction direction must be strictly positive,
+   *          otherwise an assertion is triggered.
+   *
+   * \sa class VectorwiseOp, DenseBase::colwise(), DenseBase::rowwise()
+   */
+  template <typename BinaryOp>
+  EIGEN_DEVICE_FUNC const typename ReduxReturnType<BinaryOp>::Type redux(const BinaryOp& func = BinaryOp()) const {
+    eigen_assert(redux_length() > 0 && "you are using an empty matrix");
+    return typename ReduxReturnType<BinaryOp>::Type(_expression(), internal::member_redux<BinaryOp, Scalar>(func));
+  }
 
-    /** returns a reverse iterator to the row (resp. column) before the first row (resp. column) of the nested expression
-      * \sa begin(), cend()
-      */
-    reverse_iterator        rend()         { return reverse_iterator       (m_matrix, -1); }
-    /** const version of rend() */
-    const_reverse_iterator  rend()  const  { return const_reverse_iterator (m_matrix, -1); }
-    /** const version of rend() */
-    const_reverse_iterator crend()  const  { return const_reverse_iterator (m_matrix, -1); }
+  typedef typename ReturnType<internal::member_minCoeff>::Type MinCoeffReturnType;
+  typedef typename ReturnType<internal::member_maxCoeff>::Type MaxCoeffReturnType;
+  typedef PartialReduxExpr<const CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const ExpressionTypeNestedCleaned>,
+                           internal::member_sum<RealScalar, RealScalar>, Direction>
+      SquaredNormReturnType;
+  typedef CwiseUnaryOp<internal::scalar_sqrt_op<RealScalar>, const SquaredNormReturnType> NormReturnType;
+  typedef typename ReturnType<internal::member_blueNorm, RealScalar>::Type BlueNormReturnType;
+  typedef typename ReturnType<internal::member_stableNorm, RealScalar>::Type StableNormReturnType;
+  typedef typename ReturnType<internal::member_hypotNorm, RealScalar>::Type HypotNormReturnType;
+  typedef typename ReturnType<internal::member_sum>::Type SumReturnType;
+  typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(SumReturnType, Scalar, quotient) MeanReturnType;
+  typedef typename ReturnType<internal::member_all, bool>::Type AllReturnType;
+  typedef typename ReturnType<internal::member_any, bool>::Type AnyReturnType;
+  typedef PartialReduxExpr<ExpressionType, internal::member_count<Index, Scalar>, Direction> CountReturnType;
+  typedef typename ReturnType<internal::member_prod>::Type ProdReturnType;
+  typedef Reverse<const ExpressionType, Direction> ConstReverseReturnType;
+  typedef Reverse<ExpressionType, Direction> ReverseReturnType;
 
-    /** \returns a row or column vector expression of \c *this reduxed by \a func
-      *
-      * The template parameter \a BinaryOp is the type of the functor
-      * of the custom redux operator. Note that func must be an associative operator.
-      *
-      * \warning the size along the reduction direction must be strictly positive,
-      *          otherwise an assertion is triggered.
-      *
-      * \sa class VectorwiseOp, DenseBase::colwise(), DenseBase::rowwise()
-      */
-    template<typename BinaryOp>
-    EIGEN_DEVICE_FUNC
-    const typename ReduxReturnType<BinaryOp>::Type
-    redux(const BinaryOp& func = BinaryOp()) const
-    {
-      eigen_assert(redux_length()>0 && "you are using an empty matrix");
-      return typename ReduxReturnType<BinaryOp>::Type(_expression(), internal::member_redux<BinaryOp,Scalar>(func));
-    }
+  template <int p>
+  struct LpNormReturnType {
+    typedef PartialReduxExpr<ExpressionType, internal::member_lpnorm<p, RealScalar, Scalar>, Direction> Type;
+  };
 
-    typedef typename ReturnType<internal::member_minCoeff>::Type MinCoeffReturnType;
-    typedef typename ReturnType<internal::member_maxCoeff>::Type MaxCoeffReturnType;
-    typedef PartialReduxExpr<const CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const ExpressionTypeNestedCleaned>,internal::member_sum<RealScalar,RealScalar>,Direction> SquaredNormReturnType;
-    typedef CwiseUnaryOp<internal::scalar_sqrt_op<RealScalar>, const SquaredNormReturnType> NormReturnType;
-    typedef typename ReturnType<internal::member_blueNorm,RealScalar>::Type BlueNormReturnType;
-    typedef typename ReturnType<internal::member_stableNorm,RealScalar>::Type StableNormReturnType;
-    typedef typename ReturnType<internal::member_hypotNorm,RealScalar>::Type HypotNormReturnType;
-    typedef typename ReturnType<internal::member_sum>::Type SumReturnType;
-    typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(SumReturnType,Scalar,quotient) MeanReturnType;
-    typedef typename ReturnType<internal::member_all>::Type AllReturnType;
-    typedef typename ReturnType<internal::member_any>::Type AnyReturnType;
-    typedef PartialReduxExpr<ExpressionType, internal::member_count<Index,Scalar>, Direction> CountReturnType;
-    typedef typename ReturnType<internal::member_prod>::Type ProdReturnType;
-    typedef Reverse<const ExpressionType, Direction> ConstReverseReturnType;
-    typedef Reverse<ExpressionType, Direction> ReverseReturnType;
+  /** \returns a row (or column) vector expression of the smallest coefficient
+   * of each column (or row) of the referenced expression.
+   *
+   * \warning the size along the reduction direction must be strictly positive,
+   *          otherwise an assertion is triggered.
+   *
+   * \warning the result is undefined if \c *this contains NaN.
+   *
+   * Example: \include PartialRedux_minCoeff.cpp
+   * Output: \verbinclude PartialRedux_minCoeff.out
+   *
+   * \sa DenseBase::minCoeff() */
+  EIGEN_DEVICE_FUNC const MinCoeffReturnType minCoeff() const {
+    eigen_assert(redux_length() > 0 && "you are using an empty matrix");
+    return MinCoeffReturnType(_expression());
+  }
 
-    template<int p> struct LpNormReturnType {
-      typedef PartialReduxExpr<ExpressionType, internal::member_lpnorm<p,RealScalar,Scalar>,Direction> Type;
-    };
+  /** \returns a row (or column) vector expression of the largest coefficient
+   * of each column (or row) of the referenced expression.
+   *
+   * \warning the size along the reduction direction must be strictly positive,
+   *          otherwise an assertion is triggered.
+   *
+   * \warning the result is undefined if \c *this contains NaN.
+   *
+   * Example: \include PartialRedux_maxCoeff.cpp
+   * Output: \verbinclude PartialRedux_maxCoeff.out
+   *
+   * \sa DenseBase::maxCoeff() */
+  EIGEN_DEVICE_FUNC const MaxCoeffReturnType maxCoeff() const {
+    eigen_assert(redux_length() > 0 && "you are using an empty matrix");
+    return MaxCoeffReturnType(_expression());
+  }
 
-    /** \returns a row (or column) vector expression of the smallest coefficient
-      * of each column (or row) of the referenced expression.
-      *
-      * \warning the size along the reduction direction must be strictly positive,
-      *          otherwise an assertion is triggered.
-      *
-      * \warning the result is undefined if \c *this contains NaN.
-      *
-      * Example: \include PartialRedux_minCoeff.cpp
-      * Output: \verbinclude PartialRedux_minCoeff.out
-      *
-      * \sa DenseBase::minCoeff() */
-    EIGEN_DEVICE_FUNC
-    const MinCoeffReturnType minCoeff() const
-    {
-      eigen_assert(redux_length()>0 && "you are using an empty matrix");
-      return MinCoeffReturnType(_expression());
-    }
+  /** \returns a row (or column) vector expression of the squared norm
+   * of each column (or row) of the referenced expression.
+   * This is a vector with real entries, even if the original matrix has complex entries.
+   *
+   * Example: \include PartialRedux_squaredNorm.cpp
+   * Output: \verbinclude PartialRedux_squaredNorm.out
+   *
+   * \sa DenseBase::squaredNorm() */
+  EIGEN_DEVICE_FUNC const SquaredNormReturnType squaredNorm() const {
+    return SquaredNormReturnType(m_matrix.cwiseAbs2());
+  }
 
-    /** \returns a row (or column) vector expression of the largest coefficient
-      * of each column (or row) of the referenced expression.
-      *
-      * \warning the size along the reduction direction must be strictly positive,
-      *          otherwise an assertion is triggered.
-      *
-      * \warning the result is undefined if \c *this contains NaN.
-      *
-      * Example: \include PartialRedux_maxCoeff.cpp
-      * Output: \verbinclude PartialRedux_maxCoeff.out
-      *
-      * \sa DenseBase::maxCoeff() */
-    EIGEN_DEVICE_FUNC
-    const MaxCoeffReturnType maxCoeff() const
-    {
-      eigen_assert(redux_length()>0 && "you are using an empty matrix");
-      return MaxCoeffReturnType(_expression());
-    }
+  /** \returns a row (or column) vector expression of the norm
+   * of each column (or row) of the referenced expression.
+   * This is a vector with real entries, even if the original matrix has complex entries.
+   *
+   * Example: \include PartialRedux_norm.cpp
+   * Output: \verbinclude PartialRedux_norm.out
+   *
+   * \sa DenseBase::norm() */
+  EIGEN_DEVICE_FUNC const NormReturnType norm() const { return NormReturnType(squaredNorm()); }
 
-    /** \returns a row (or column) vector expression of the squared norm
-      * of each column (or row) of the referenced expression.
-      * This is a vector with real entries, even if the original matrix has complex entries.
-      *
-      * Example: \include PartialRedux_squaredNorm.cpp
-      * Output: \verbinclude PartialRedux_squaredNorm.out
-      *
-      * \sa DenseBase::squaredNorm() */
-    EIGEN_DEVICE_FUNC
-    const SquaredNormReturnType squaredNorm() const
-    { return SquaredNormReturnType(m_matrix.cwiseAbs2()); }
+  /** \returns a row (or column) vector expression of the norm
+   * of each column (or row) of the referenced expression.
+   * This is a vector with real entries, even if the original matrix has complex entries.
+   *
+   * Example: \include PartialRedux_norm.cpp
+   * Output: \verbinclude PartialRedux_norm.out
+   *
+   * \sa DenseBase::norm() */
+  template <int p>
+  EIGEN_DEVICE_FUNC const typename LpNormReturnType<p>::Type lpNorm() const {
+    return typename LpNormReturnType<p>::Type(_expression());
+  }
 
-    /** \returns a row (or column) vector expression of the norm
-      * of each column (or row) of the referenced expression.
-      * This is a vector with real entries, even if the original matrix has complex entries.
-      *
-      * Example: \include PartialRedux_norm.cpp
-      * Output: \verbinclude PartialRedux_norm.out
-      *
-      * \sa DenseBase::norm() */
-    EIGEN_DEVICE_FUNC
-    const NormReturnType norm() const
-    { return NormReturnType(squaredNorm()); }
+  /** \returns a row (or column) vector expression of the norm
+   * of each column (or row) of the referenced expression, using
+   * Blue's algorithm.
+   * This is a vector with real entries, even if the original matrix has complex entries.
+   *
+   * \sa DenseBase::blueNorm() */
+  EIGEN_DEVICE_FUNC const BlueNormReturnType blueNorm() const { return BlueNormReturnType(_expression()); }
 
-    /** \returns a row (or column) vector expression of the norm
-      * of each column (or row) of the referenced expression.
-      * This is a vector with real entries, even if the original matrix has complex entries.
-      *
-      * Example: \include PartialRedux_norm.cpp
-      * Output: \verbinclude PartialRedux_norm.out
-      *
-      * \sa DenseBase::norm() */
-    template<int p>
-    EIGEN_DEVICE_FUNC
-    const typename LpNormReturnType<p>::Type lpNorm() const
-    { return typename LpNormReturnType<p>::Type(_expression()); }
+  /** \returns a row (or column) vector expression of the norm
+   * of each column (or row) of the referenced expression, avoiding
+   * underflow and overflow.
+   * This is a vector with real entries, even if the original matrix has complex entries.
+   *
+   * \sa DenseBase::stableNorm() */
+  EIGEN_DEVICE_FUNC const StableNormReturnType stableNorm() const { return StableNormReturnType(_expression()); }
 
+  /** \returns a row (or column) vector expression of the norm
+   * of each column (or row) of the referenced expression, avoiding
+   * underflow and overflow using a concatenation of hypot() calls.
+   * This is a vector with real entries, even if the original matrix has complex entries.
+   *
+   * \sa DenseBase::hypotNorm() */
+  EIGEN_DEVICE_FUNC const HypotNormReturnType hypotNorm() const { return HypotNormReturnType(_expression()); }
 
-    /** \returns a row (or column) vector expression of the norm
-      * of each column (or row) of the referenced expression, using
-      * Blue's algorithm.
-      * This is a vector with real entries, even if the original matrix has complex entries.
-      *
-      * \sa DenseBase::blueNorm() */
-    EIGEN_DEVICE_FUNC
-    const BlueNormReturnType blueNorm() const
-    { return BlueNormReturnType(_expression()); }
+  /** \returns a row (or column) vector expression of the sum
+   * of each column (or row) of the referenced expression.
+   *
+   * Example: \include PartialRedux_sum.cpp
+   * Output: \verbinclude PartialRedux_sum.out
+   *
+   * \sa DenseBase::sum() */
+  EIGEN_DEVICE_FUNC const SumReturnType sum() const { return SumReturnType(_expression()); }
 
+  /** \returns a row (or column) vector expression of the mean
+   * of each column (or row) of the referenced expression.
+   *
+   * \sa DenseBase::mean() */
+  EIGEN_DEVICE_FUNC const MeanReturnType mean() const {
+    return sum() / Scalar(Direction == Vertical ? m_matrix.rows() : m_matrix.cols());
+  }
 
-    /** \returns a row (or column) vector expression of the norm
-      * of each column (or row) of the referenced expression, avoiding
-      * underflow and overflow.
-      * This is a vector with real entries, even if the original matrix has complex entries.
-      *
-      * \sa DenseBase::stableNorm() */
-    EIGEN_DEVICE_FUNC
-    const StableNormReturnType stableNorm() const
-    { return StableNormReturnType(_expression()); }
+  /** \returns a row (or column) vector expression representing
+   * whether \b all coefficients of each respective column (or row) are \c true.
+   * This expression can be assigned to a vector with entries of type \c bool.
+   *
+   * \sa DenseBase::all() */
+  EIGEN_DEVICE_FUNC const AllReturnType all() const { return AllReturnType(_expression()); }
 
+  /** \returns a row (or column) vector expression representing
+   * whether \b at \b least one coefficient of each respective column (or row) is \c true.
+   * This expression can be assigned to a vector with entries of type \c bool.
+   *
+   * \sa DenseBase::any() */
+  EIGEN_DEVICE_FUNC const AnyReturnType any() const { return AnyReturnType(_expression()); }
 
-    /** \returns a row (or column) vector expression of the norm
-      * of each column (or row) of the referenced expression, avoiding
-      * underflow and overflow using a concatenation of hypot() calls.
-      * This is a vector with real entries, even if the original matrix has complex entries.
-      *
-      * \sa DenseBase::hypotNorm() */
-    EIGEN_DEVICE_FUNC
-    const HypotNormReturnType hypotNorm() const
-    { return HypotNormReturnType(_expression()); }
+  /** \returns a row (or column) vector expression representing
+   * the number of \c true coefficients of each respective column (or row).
+   * This expression can be assigned to a vector whose entries have the same type as is used to
+   * index entries of the original matrix; for dense matrices, this is \c std::ptrdiff_t .
+   *
+   * Example: \include PartialRedux_count.cpp
+   * Output: \verbinclude PartialRedux_count.out
+   *
+   * \sa DenseBase::count() */
+  EIGEN_DEVICE_FUNC const CountReturnType count() const { return CountReturnType(_expression()); }
 
-    /** \returns a row (or column) vector expression of the sum
-      * of each column (or row) of the referenced expression.
-      *
-      * Example: \include PartialRedux_sum.cpp
-      * Output: \verbinclude PartialRedux_sum.out
-      *
-      * \sa DenseBase::sum() */
-    EIGEN_DEVICE_FUNC
-    const SumReturnType sum() const
-    { return SumReturnType(_expression()); }
+  /** \returns a row (or column) vector expression of the product
+   * of each column (or row) of the referenced expression.
+   *
+   * Example: \include PartialRedux_prod.cpp
+   * Output: \verbinclude PartialRedux_prod.out
+   *
+   * \sa DenseBase::prod() */
+  EIGEN_DEVICE_FUNC const ProdReturnType prod() const { return ProdReturnType(_expression()); }
 
-    /** \returns a row (or column) vector expression of the mean
-    * of each column (or row) of the referenced expression.
-    *
-    * \sa DenseBase::mean() */
-    EIGEN_DEVICE_FUNC
-    const MeanReturnType mean() const
-    { return sum() / Scalar(Direction==Vertical?m_matrix.rows():m_matrix.cols()); }
+  /** \returns a matrix expression
+   * where each column (or row) are reversed.
+   *
+   * Example: \include Vectorwise_reverse.cpp
+   * Output: \verbinclude Vectorwise_reverse.out
+   *
+   * \sa DenseBase::reverse() */
+  EIGEN_DEVICE_FUNC const ConstReverseReturnType reverse() const { return ConstReverseReturnType(_expression()); }
 
-    /** \returns a row (or column) vector expression representing
-      * whether \b all coefficients of each respective column (or row) are \c true.
-      * This expression can be assigned to a vector with entries of type \c bool.
-      *
-      * \sa DenseBase::all() */
-    EIGEN_DEVICE_FUNC
-    const AllReturnType all() const
-    { return AllReturnType(_expression()); }
+  /** \returns a writable matrix expression
+   * where each column (or row) are reversed.
+   *
+   * \sa reverse() const */
+  EIGEN_DEVICE_FUNC ReverseReturnType reverse() { return ReverseReturnType(_expression()); }
 
-    /** \returns a row (or column) vector expression representing
-      * whether \b at \b least one coefficient of each respective column (or row) is \c true.
-      * This expression can be assigned to a vector with entries of type \c bool.
-      *
-      * \sa DenseBase::any() */
-    EIGEN_DEVICE_FUNC
-    const AnyReturnType any() const
-    { return AnyReturnType(_expression()); }
+  typedef Replicate<ExpressionType, (isVertical ? Dynamic : 1), (isHorizontal ? Dynamic : 1)> ReplicateReturnType;
+  EIGEN_DEVICE_FUNC const ReplicateReturnType replicate(Index factor) const;
 
-    /** \returns a row (or column) vector expression representing
-      * the number of \c true coefficients of each respective column (or row).
-      * This expression can be assigned to a vector whose entries have the same type as is used to
-      * index entries of the original matrix; for dense matrices, this is \c std::ptrdiff_t .
-      *
-      * Example: \include PartialRedux_count.cpp
-      * Output: \verbinclude PartialRedux_count.out
-      *
-      * \sa DenseBase::count() */
-    EIGEN_DEVICE_FUNC
-    const CountReturnType count() const
-    { return CountReturnType(_expression()); }
+  /**
+   * \return an expression of the replication of each column (or row) of \c *this
+   *
+   * Example: \include DirectionWise_replicate.cpp
+   * Output: \verbinclude DirectionWise_replicate.out
+   *
+   * \sa VectorwiseOp::replicate(Index), DenseBase::replicate(), class Replicate
+   */
+  // NOTE implemented here because of sunstudio's compilation errors
+  // isVertical*Factor+isHorizontal instead of (isVertical?Factor:1) to handle CUDA bug with ternary operator
+  template <int Factor>
+  const Replicate<ExpressionType, isVertical * Factor + isHorizontal,
+                  isHorizontal * Factor + isVertical> EIGEN_DEVICE_FUNC
+  replicate(Index factor = Factor) const {
+    return Replicate<ExpressionType, (isVertical ? Factor : 1), (isHorizontal ? Factor : 1)>(
+        _expression(), isVertical ? factor : 1, isHorizontal ? factor : 1);
+  }
 
-    /** \returns a row (or column) vector expression of the product
-      * of each column (or row) of the referenced expression.
-      *
-      * Example: \include PartialRedux_prod.cpp
-      * Output: \verbinclude PartialRedux_prod.out
-      *
-      * \sa DenseBase::prod() */
-    EIGEN_DEVICE_FUNC
-    const ProdReturnType prod() const
-    { return ProdReturnType(_expression()); }
+  /////////// Artithmetic operators ///////////
 
+  /** Copies the vector \a other to each subvector of \c *this */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC ExpressionType& operator=(const DenseBase<OtherDerived>& other) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    // eigen_assert((m_matrix.isNull()) == (other.isNull())); FIXME
+    return m_matrix = extendedTo(other.derived());
+  }
 
-    /** \returns a matrix expression
-      * where each column (or row) are reversed.
-      *
-      * Example: \include Vectorwise_reverse.cpp
-      * Output: \verbinclude Vectorwise_reverse.out
-      *
-      * \sa DenseBase::reverse() */
-    EIGEN_DEVICE_FUNC
-    const ConstReverseReturnType reverse() const
-    { return ConstReverseReturnType( _expression() ); }
+  /** Adds the vector \a other to each subvector of \c *this */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC ExpressionType& operator+=(const DenseBase<OtherDerived>& other) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    return m_matrix += extendedTo(other.derived());
+  }
 
-    /** \returns a writable matrix expression
-      * where each column (or row) are reversed.
-      *
-      * \sa reverse() const */
-    EIGEN_DEVICE_FUNC
-    ReverseReturnType reverse()
-    { return ReverseReturnType( _expression() ); }
+  /** Subtracts the vector \a other to each subvector of \c *this */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC ExpressionType& operator-=(const DenseBase<OtherDerived>& other) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    return m_matrix -= extendedTo(other.derived());
+  }
 
-    typedef Replicate<ExpressionType,(isVertical?Dynamic:1),(isHorizontal?Dynamic:1)> ReplicateReturnType;
-    EIGEN_DEVICE_FUNC
-    const ReplicateReturnType replicate(Index factor) const;
+  /** Multiplies each subvector of \c *this by the vector \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC ExpressionType& operator*=(const DenseBase<OtherDerived>& other) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    m_matrix *= extendedTo(other.derived());
+    return m_matrix;
+  }
 
-    /**
-      * \return an expression of the replication of each column (or row) of \c *this
-      *
-      * Example: \include DirectionWise_replicate.cpp
-      * Output: \verbinclude DirectionWise_replicate.out
-      *
-      * \sa VectorwiseOp::replicate(Index), DenseBase::replicate(), class Replicate
-      */
-    // NOTE implemented here because of sunstudio's compilation errors
-    // isVertical*Factor+isHorizontal instead of (isVertical?Factor:1) to handle CUDA bug with ternary operator
-    template<int Factor> const Replicate<ExpressionType,isVertical*Factor+isHorizontal,isHorizontal*Factor+isVertical>
-    EIGEN_DEVICE_FUNC
-    replicate(Index factor = Factor) const
-    {
-      return Replicate<ExpressionType,(isVertical?Factor:1),(isHorizontal?Factor:1)>
-          (_expression(),isVertical?factor:1,isHorizontal?factor:1);
-    }
+  /** Divides each subvector of \c *this by the vector \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC ExpressionType& operator/=(const DenseBase<OtherDerived>& other) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    m_matrix /= extendedTo(other.derived());
+    return m_matrix;
+  }
 
-/////////// Artithmetic operators ///////////
+  /** Returns the expression of the sum of the vector \a other to each subvector of \c *this */
+  template <typename OtherDerived>
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+      CwiseBinaryOp<internal::scalar_sum_op<Scalar, typename OtherDerived::Scalar>, const ExpressionTypeNestedCleaned,
+                    const typename ExtendedType<OtherDerived>::Type>
+      operator+(const DenseBase<OtherDerived>& other) const {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    return m_matrix + extendedTo(other.derived());
+  }
 
-    /** Copies the vector \a other to each subvector of \c *this */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    ExpressionType& operator=(const DenseBase<OtherDerived>& other)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      //eigen_assert((m_matrix.isNull()) == (other.isNull())); FIXME
-      return m_matrix = extendedTo(other.derived());
-    }
+  /** Returns the expression of the difference between each subvector of \c *this and the vector \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC CwiseBinaryOp<internal::scalar_difference_op<Scalar, typename OtherDerived::Scalar>,
+                                  const ExpressionTypeNestedCleaned, const typename ExtendedType<OtherDerived>::Type>
+  operator-(const DenseBase<OtherDerived>& other) const {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    return m_matrix - extendedTo(other.derived());
+  }
 
-    /** Adds the vector \a other to each subvector of \c *this */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    ExpressionType& operator+=(const DenseBase<OtherDerived>& other)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return m_matrix += extendedTo(other.derived());
-    }
+  /** Returns the expression where each subvector is the product of the vector \a other
+   * by the corresponding subvector of \c *this */
+  template <typename OtherDerived>
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+      CwiseBinaryOp<internal::scalar_product_op<Scalar>, const ExpressionTypeNestedCleaned,
+                    const typename ExtendedType<OtherDerived>::Type> EIGEN_DEVICE_FUNC
+      operator*(const DenseBase<OtherDerived>& other) const {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    return m_matrix * extendedTo(other.derived());
+  }
 
-    /** Substracts the vector \a other to each subvector of \c *this */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    ExpressionType& operator-=(const DenseBase<OtherDerived>& other)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return m_matrix -= extendedTo(other.derived());
-    }
+  /** Returns the expression where each subvector is the quotient of the corresponding
+   * subvector of \c *this by the vector \a other */
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC CwiseBinaryOp<internal::scalar_quotient_op<Scalar>, const ExpressionTypeNestedCleaned,
+                                  const typename ExtendedType<OtherDerived>::Type>
+  operator/(const DenseBase<OtherDerived>& other) const {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+    EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
+    EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
+    return m_matrix / extendedTo(other.derived());
+  }
 
-    /** Multiples each subvector of \c *this by the vector \a other */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    ExpressionType& operator*=(const DenseBase<OtherDerived>& other)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      m_matrix *= extendedTo(other.derived());
-      return m_matrix;
-    }
+  /** \returns an expression where each column (or row) of the referenced matrix are normalized.
+   * The referenced matrix is \b not modified.
+   * \sa MatrixBase::normalized(), normalize()
+   */
+  EIGEN_DEVICE_FUNC CwiseBinaryOp<internal::scalar_quotient_op<Scalar>, const ExpressionTypeNestedCleaned,
+                                  const typename OppositeExtendedType<NormReturnType>::Type>
+  normalized() const {
+    return m_matrix.cwiseQuotient(extendedToOpposite(this->norm()));
+  }
 
-    /** Divides each subvector of \c *this by the vector \a other */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    ExpressionType& operator/=(const DenseBase<OtherDerived>& other)
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      m_matrix /= extendedTo(other.derived());
-      return m_matrix;
-    }
+  /** Normalize in-place each row or columns of the referenced matrix.
+   * \sa MatrixBase::normalize(), normalized()
+   */
+  EIGEN_DEVICE_FUNC void normalize() { m_matrix = this->normalized(); }
 
-    /** Returns the expression of the sum of the vector \a other to each subvector of \c *this */
-    template<typename OtherDerived> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-    CwiseBinaryOp<internal::scalar_sum_op<Scalar,typename OtherDerived::Scalar>,
-                  const ExpressionTypeNestedCleaned,
-                  const typename ExtendedType<OtherDerived>::Type>
-    operator+(const DenseBase<OtherDerived>& other) const
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return m_matrix + extendedTo(other.derived());
-    }
+  EIGEN_DEVICE_FUNC inline void reverseInPlace();
 
-    /** Returns the expression of the difference between each subvector of \c *this and the vector \a other */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    CwiseBinaryOp<internal::scalar_difference_op<Scalar,typename OtherDerived::Scalar>,
-                  const ExpressionTypeNestedCleaned,
-                  const typename ExtendedType<OtherDerived>::Type>
-    operator-(const DenseBase<OtherDerived>& other) const
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return m_matrix - extendedTo(other.derived());
-    }
+  /////////// Geometry module ///////////
 
-    /** Returns the expression where each subvector is the product of the vector \a other
-      * by the corresponding subvector of \c *this */
-    template<typename OtherDerived> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-    CwiseBinaryOp<internal::scalar_product_op<Scalar>,
-                  const ExpressionTypeNestedCleaned,
-                  const typename ExtendedType<OtherDerived>::Type>
-    EIGEN_DEVICE_FUNC
-    operator*(const DenseBase<OtherDerived>& other) const
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return m_matrix * extendedTo(other.derived());
-    }
+  typedef Homogeneous<ExpressionType, Direction> HomogeneousReturnType;
+  EIGEN_DEVICE_FUNC HomogeneousReturnType homogeneous() const;
 
-    /** Returns the expression where each subvector is the quotient of the corresponding
-      * subvector of \c *this by the vector \a other */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    CwiseBinaryOp<internal::scalar_quotient_op<Scalar>,
-                  const ExpressionTypeNestedCleaned,
-                  const typename ExtendedType<OtherDerived>::Type>
-    operator/(const DenseBase<OtherDerived>& other) const
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-      EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
-      EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return m_matrix / extendedTo(other.derived());
-    }
+  typedef typename ExpressionType::PlainObject CrossReturnType;
+  template <typename OtherDerived>
+  EIGEN_DEVICE_FUNC const CrossReturnType cross(const MatrixBase<OtherDerived>& other) const;
 
-    /** \returns an expression where each column (or row) of the referenced matrix are normalized.
-      * The referenced matrix is \b not modified.
-      * \sa MatrixBase::normalized(), normalize()
-      */
-    EIGEN_DEVICE_FUNC
-    CwiseBinaryOp<internal::scalar_quotient_op<Scalar>,
-                  const ExpressionTypeNestedCleaned,
-                  const typename OppositeExtendedType<NormReturnType>::Type>
-    normalized() const { return m_matrix.cwiseQuotient(extendedToOpposite(this->norm())); }
-
-
-    /** Normalize in-place each row or columns of the referenced matrix.
-      * \sa MatrixBase::normalize(), normalized()
-      */
-    EIGEN_DEVICE_FUNC void normalize() {
-      m_matrix = this->normalized();
-    }
-
-    EIGEN_DEVICE_FUNC inline void reverseInPlace();
-
-/////////// Geometry module ///////////
-
-    typedef Homogeneous<ExpressionType,Direction> HomogeneousReturnType;
-    EIGEN_DEVICE_FUNC
-    HomogeneousReturnType homogeneous() const;
-
-    typedef typename ExpressionType::PlainObject CrossReturnType;
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const CrossReturnType cross(const MatrixBase<OtherDerived>& other) const;
-
-    enum {
-      HNormalized_Size = Direction==Vertical ? internal::traits<ExpressionType>::RowsAtCompileTime
+  enum {
+    HNormalized_Size = Direction == Vertical ? internal::traits<ExpressionType>::RowsAtCompileTime
                                              : internal::traits<ExpressionType>::ColsAtCompileTime,
-      HNormalized_SizeMinusOne = HNormalized_Size==Dynamic ? Dynamic : HNormalized_Size-1
-    };
-    typedef Block<const ExpressionType,
-                  Direction==Vertical   ? int(HNormalized_SizeMinusOne)
-                                        : int(internal::traits<ExpressionType>::RowsAtCompileTime),
-                  Direction==Horizontal ? int(HNormalized_SizeMinusOne)
+    HNormalized_SizeMinusOne = HNormalized_Size == Dynamic ? Dynamic : HNormalized_Size - 1
+  };
+  typedef Block<const ExpressionType,
+                Direction == Vertical ? int(HNormalized_SizeMinusOne)
+                                      : int(internal::traits<ExpressionType>::RowsAtCompileTime),
+                Direction == Horizontal ? int(HNormalized_SizeMinusOne)
                                         : int(internal::traits<ExpressionType>::ColsAtCompileTime)>
-            HNormalized_Block;
-    typedef Block<const ExpressionType,
-                  Direction==Vertical   ? 1 : int(internal::traits<ExpressionType>::RowsAtCompileTime),
-                  Direction==Horizontal ? 1 : int(internal::traits<ExpressionType>::ColsAtCompileTime)>
-            HNormalized_Factors;
-    typedef CwiseBinaryOp<internal::scalar_quotient_op<typename internal::traits<ExpressionType>::Scalar>,
-                const HNormalized_Block,
-                const Replicate<HNormalized_Factors,
-                  Direction==Vertical   ? HNormalized_SizeMinusOne : 1,
-                  Direction==Horizontal ? HNormalized_SizeMinusOne : 1> >
-            HNormalizedReturnType;
+      HNormalized_Block;
+  typedef Block<const ExpressionType,
+                Direction == Vertical ? 1 : int(internal::traits<ExpressionType>::RowsAtCompileTime),
+                Direction == Horizontal ? 1 : int(internal::traits<ExpressionType>::ColsAtCompileTime)>
+      HNormalized_Factors;
+  typedef CwiseBinaryOp<internal::scalar_quotient_op<typename internal::traits<ExpressionType>::Scalar>,
+                        const HNormalized_Block,
+                        const Replicate<HNormalized_Factors, Direction == Vertical ? HNormalized_SizeMinusOne : 1,
+                                        Direction == Horizontal ? HNormalized_SizeMinusOne : 1> >
+      HNormalizedReturnType;
 
-    EIGEN_DEVICE_FUNC
-    const HNormalizedReturnType hnormalized() const;
+  EIGEN_DEVICE_FUNC const HNormalizedReturnType hnormalized() const;
 
-#   ifdef EIGEN_VECTORWISEOP_PLUGIN
-#     include EIGEN_VECTORWISEOP_PLUGIN
-#   endif
+#ifdef EIGEN_VECTORWISEOP_PLUGIN
+#include EIGEN_VECTORWISEOP_PLUGIN
+#endif
 
-  protected:
-    Index redux_length() const
-    {
-      return Direction==Vertical ? m_matrix.rows() : m_matrix.cols();
-    }
-    ExpressionTypeNested m_matrix;
+ protected:
+  EIGEN_DEVICE_FUNC Index redux_length() const { return Direction == Vertical ? m_matrix.rows() : m_matrix.cols(); }
+  ExpressionTypeNested m_matrix;
 };
 
-//const colwise moved to DenseBase.h due to CUDA compiler bug
-
+// const colwise moved to DenseBase.h due to CUDA compiler bug
 
 /** \returns a writable VectorwiseOp wrapper of *this providing additional partial reduction operations
-  *
-  * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::ColwiseReturnType
-DenseBase<Derived>::colwise()
-{
+ *
+ * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::ColwiseReturnType DenseBase<Derived>::colwise() {
   return ColwiseReturnType(derived());
 }
 
-//const rowwise moved to DenseBase.h due to CUDA compiler bug
-
+// const rowwise moved to DenseBase.h due to CUDA compiler bug
 
 /** \returns a writable VectorwiseOp wrapper of *this providing additional partial reduction operations
-  *
-  * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::RowwiseReturnType
-DenseBase<Derived>::rowwise()
-{
+ *
+ * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::RowwiseReturnType DenseBase<Derived>::rowwise() {
   return RowwiseReturnType(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PARTIAL_REDUX_H
+#endif  // EIGEN_PARTIAL_REDUX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h
index 00bcca8..037a605 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h
@@ -10,290 +10,648 @@
 #ifndef EIGEN_VISITOR_H
 #define EIGEN_VISITOR_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename Visitor, typename Derived, int UnrollCount>
-struct visitor_impl
-{
-  enum {
-    col = (UnrollCount-1) / Derived::RowsAtCompileTime,
-    row = (UnrollCount-1) % Derived::RowsAtCompileTime
-  };
+template <typename Visitor, typename Derived, int UnrollCount,
+          bool Vectorize = (Derived::PacketAccess && functor_traits<Visitor>::PacketAccess), bool LinearAccess = false,
+          bool ShortCircuitEvaluation = false>
+struct visitor_impl;
 
-  EIGEN_DEVICE_FUNC
-  static inline void run(const Derived &mat, Visitor& visitor)
-  {
-    visitor_impl<Visitor, Derived, UnrollCount-1>::run(mat, visitor);
-    visitor(mat.coeff(row, col), row, col);
+template <typename Visitor, bool ShortCircuitEvaluation = false>
+struct short_circuit_eval_impl {
+  // if short circuit evaluation is not used, do nothing
+  static EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(const Visitor&) { return false; }
+};
+template <typename Visitor>
+struct short_circuit_eval_impl<Visitor, true> {
+  // if short circuit evaluation is used, check the visitor
+  static EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(const Visitor& visitor) {
+    return visitor.done();
   }
 };
 
-template<typename Visitor, typename Derived>
-struct visitor_impl<Visitor, Derived, 1>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(const Derived &mat, Visitor& visitor)
-  {
-    return visitor.init(mat.coeff(0, 0), 0, 0);
+// unrolled inner-outer traversal
+template <typename Visitor, typename Derived, int UnrollCount, bool Vectorize, bool ShortCircuitEvaluation>
+struct visitor_impl<Visitor, Derived, UnrollCount, Vectorize, false, ShortCircuitEvaluation> {
+  // don't use short circuit evaulation for unrolled version
+  using Scalar = typename Derived::Scalar;
+  using Packet = typename packet_traits<Scalar>::type;
+  static constexpr bool RowMajor = Derived::IsRowMajor;
+  static constexpr int RowsAtCompileTime = Derived::RowsAtCompileTime;
+  static constexpr int ColsAtCompileTime = Derived::ColsAtCompileTime;
+  static constexpr int PacketSize = packet_traits<Scalar>::size;
+
+  static constexpr bool CanVectorize(int K) {
+    constexpr int InnerSizeAtCompileTime = RowMajor ? ColsAtCompileTime : RowsAtCompileTime;
+    if (InnerSizeAtCompileTime < PacketSize) return false;
+    return Vectorize && (InnerSizeAtCompileTime - (K % InnerSizeAtCompileTime) >= PacketSize);
+  }
+
+  template <int K = 0, bool Empty = (K == UnrollCount), std::enable_if_t<Empty, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived&, Visitor&) {}
+
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && Initialize && !DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    visitor.init(mat.coeff(0, 0), 0, 0);
+    run<1>(mat, visitor);
+  }
+
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && !Initialize && !DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    static constexpr int R = RowMajor ? (K / ColsAtCompileTime) : (K % RowsAtCompileTime);
+    static constexpr int C = RowMajor ? (K % ColsAtCompileTime) : (K / RowsAtCompileTime);
+    visitor(mat.coeff(R, C), R, C);
+    run<K + 1>(mat, visitor);
+  }
+
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && Initialize && DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    Packet P = mat.template packet<Packet>(0, 0);
+    visitor.initpacket(P, 0, 0);
+    run<PacketSize>(mat, visitor);
+  }
+
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && !Initialize && DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    static constexpr int R = RowMajor ? (K / ColsAtCompileTime) : (K % RowsAtCompileTime);
+    static constexpr int C = RowMajor ? (K % ColsAtCompileTime) : (K / RowsAtCompileTime);
+    Packet P = mat.template packet<Packet>(R, C);
+    visitor.packet(P, R, C);
+    run<K + PacketSize>(mat, visitor);
   }
 };
 
-// This specialization enables visitors on empty matrices at compile-time
-template<typename Visitor, typename Derived>
-struct visitor_impl<Visitor, Derived, 0> {
-  EIGEN_DEVICE_FUNC
-  static inline void run(const Derived &/*mat*/, Visitor& /*visitor*/)
-  {}
+// unrolled linear traversal
+template <typename Visitor, typename Derived, int UnrollCount, bool Vectorize, bool ShortCircuitEvaluation>
+struct visitor_impl<Visitor, Derived, UnrollCount, Vectorize, true, ShortCircuitEvaluation> {
+  // don't use short circuit evaulation for unrolled version
+  using Scalar = typename Derived::Scalar;
+  using Packet = typename packet_traits<Scalar>::type;
+  static constexpr int PacketSize = packet_traits<Scalar>::size;
+
+  static constexpr bool CanVectorize(int K) { return Vectorize && ((UnrollCount - K) >= PacketSize); }
+
+  // empty
+  template <int K = 0, bool Empty = (K == UnrollCount), std::enable_if_t<Empty, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived&, Visitor&) {}
+
+  // scalar initialization
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && Initialize && !DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    visitor.init(mat.coeff(0), 0);
+    run<1>(mat, visitor);
+  }
+
+  // scalar iteration
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && !Initialize && !DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    visitor(mat.coeff(K), K);
+    run<K + 1>(mat, visitor);
+  }
+
+  // vector initialization
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && Initialize && DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    Packet P = mat.template packet<Packet>(0);
+    visitor.initpacket(P, 0);
+    run<PacketSize>(mat, visitor);
+  }
+
+  // vector iteration
+  template <int K = 0, bool Empty = (K == UnrollCount), bool Initialize = (K == 0), bool DoVectorOp = CanVectorize(K),
+            std::enable_if_t<!Empty && !Initialize && DoVectorOp, bool> = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    Packet P = mat.template packet<Packet>(K);
+    visitor.packet(P, K);
+    run<K + PacketSize>(mat, visitor);
+  }
 };
 
-template<typename Visitor, typename Derived>
-struct visitor_impl<Visitor, Derived, Dynamic>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(const Derived& mat, Visitor& visitor)
-  {
-    visitor.init(mat.coeff(0,0), 0, 0);
-    for(Index i = 1; i < mat.rows(); ++i)
-      visitor(mat.coeff(i, 0), i, 0);
-    for(Index j = 1; j < mat.cols(); ++j)
-      for(Index i = 0; i < mat.rows(); ++i)
-        visitor(mat.coeff(i, j), i, j);
+// dynamic scalar outer-inner traversal
+template <typename Visitor, typename Derived, bool ShortCircuitEvaluation>
+struct visitor_impl<Visitor, Derived, Dynamic, /*Vectorize=*/false, /*LinearAccess=*/false, ShortCircuitEvaluation> {
+  using short_circuit = short_circuit_eval_impl<Visitor, ShortCircuitEvaluation>;
+  static constexpr bool RowMajor = Derived::IsRowMajor;
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    const Index innerSize = RowMajor ? mat.cols() : mat.rows();
+    const Index outerSize = RowMajor ? mat.rows() : mat.cols();
+    if (innerSize == 0 || outerSize == 0) return;
+    {
+      visitor.init(mat.coeff(0, 0), 0, 0);
+      if (short_circuit::run(visitor)) return;
+      for (Index i = 1; i < innerSize; ++i) {
+        Index r = RowMajor ? 0 : i;
+        Index c = RowMajor ? i : 0;
+        visitor(mat.coeff(r, c), r, c);
+        if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+      }
+    }
+    for (Index j = 1; j < outerSize; j++) {
+      for (Index i = 0; i < innerSize; ++i) {
+        Index r = RowMajor ? j : i;
+        Index c = RowMajor ? i : j;
+        visitor(mat.coeff(r, c), r, c);
+        if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+      }
+    }
+  }
+};
+
+// dynamic vectorized outer-inner traversal
+template <typename Visitor, typename Derived, bool ShortCircuitEvaluation>
+struct visitor_impl<Visitor, Derived, Dynamic, /*Vectorize=*/true, /*LinearAccess=*/false, ShortCircuitEvaluation> {
+  using Scalar = typename Derived::Scalar;
+  using Packet = typename packet_traits<Scalar>::type;
+  static constexpr int PacketSize = packet_traits<Scalar>::size;
+  using short_circuit = short_circuit_eval_impl<Visitor, ShortCircuitEvaluation>;
+  static constexpr bool RowMajor = Derived::IsRowMajor;
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    const Index innerSize = RowMajor ? mat.cols() : mat.rows();
+    const Index outerSize = RowMajor ? mat.rows() : mat.cols();
+    if (innerSize == 0 || outerSize == 0) return;
+    {
+      Index i = 0;
+      if (innerSize < PacketSize) {
+        visitor.init(mat.coeff(0, 0), 0, 0);
+        i = 1;
+      } else {
+        Packet p = mat.template packet<Packet>(0, 0);
+        visitor.initpacket(p, 0, 0);
+        i = PacketSize;
+      }
+      if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+      for (; i + PacketSize - 1 < innerSize; i += PacketSize) {
+        Index r = RowMajor ? 0 : i;
+        Index c = RowMajor ? i : 0;
+        Packet p = mat.template packet<Packet>(r, c);
+        visitor.packet(p, r, c);
+        if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+      }
+      for (; i < innerSize; ++i) {
+        Index r = RowMajor ? 0 : i;
+        Index c = RowMajor ? i : 0;
+        visitor(mat.coeff(r, c), r, c);
+        if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+      }
+    }
+    for (Index j = 1; j < outerSize; j++) {
+      Index i = 0;
+      for (; i + PacketSize - 1 < innerSize; i += PacketSize) {
+        Index r = RowMajor ? j : i;
+        Index c = RowMajor ? i : j;
+        Packet p = mat.template packet<Packet>(r, c);
+        visitor.packet(p, r, c);
+        if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+      }
+      for (; i < innerSize; ++i) {
+        Index r = RowMajor ? j : i;
+        Index c = RowMajor ? i : j;
+        visitor(mat.coeff(r, c), r, c);
+        if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+      }
+    }
+  }
+};
+
+// dynamic scalar linear traversal
+template <typename Visitor, typename Derived, bool ShortCircuitEvaluation>
+struct visitor_impl<Visitor, Derived, Dynamic, /*Vectorize=*/false, /*LinearAccess=*/true, ShortCircuitEvaluation> {
+  using short_circuit = short_circuit_eval_impl<Visitor, ShortCircuitEvaluation>;
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    const Index size = mat.size();
+    if (size == 0) return;
+    visitor.init(mat.coeff(0), 0);
+    if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+    for (Index k = 1; k < size; k++) {
+      visitor(mat.coeff(k), k);
+      if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+    }
+  }
+};
+
+// dynamic vectorized linear traversal
+template <typename Visitor, typename Derived, bool ShortCircuitEvaluation>
+struct visitor_impl<Visitor, Derived, Dynamic, /*Vectorize=*/true, /*LinearAccess=*/true, ShortCircuitEvaluation> {
+  using Scalar = typename Derived::Scalar;
+  using Packet = typename packet_traits<Scalar>::type;
+  static constexpr int PacketSize = packet_traits<Scalar>::size;
+  using short_circuit = short_circuit_eval_impl<Visitor, ShortCircuitEvaluation>;
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& mat, Visitor& visitor) {
+    const Index size = mat.size();
+    if (size == 0) return;
+    Index k = 0;
+    if (size < PacketSize) {
+      visitor.init(mat.coeff(0), 0);
+      k = 1;
+    } else {
+      Packet p = mat.template packet<Packet>(k);
+      visitor.initpacket(p, k);
+      k = PacketSize;
+    }
+    if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+    for (; k + PacketSize - 1 < size; k += PacketSize) {
+      Packet p = mat.template packet<Packet>(k);
+      visitor.packet(p, k);
+      if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+    }
+    for (; k < size; k++) {
+      visitor(mat.coeff(k), k);
+      if EIGEN_PREDICT_FALSE (short_circuit::run(visitor)) return;
+    }
   }
 };
 
 // evaluator adaptor
-template<typename XprType>
-class visitor_evaluator
-{
-public:
-  EIGEN_DEVICE_FUNC
-  explicit visitor_evaluator(const XprType &xpr) : m_evaluator(xpr), m_xpr(xpr) {}
-
+template <typename XprType>
+class visitor_evaluator {
+ public:
+  typedef evaluator<XprType> Evaluator;
   typedef typename XprType::Scalar Scalar;
-  typedef typename XprType::CoeffReturnType CoeffReturnType;
+  using Packet = typename packet_traits<Scalar>::type;
+  typedef std::remove_const_t<typename XprType::CoeffReturnType> CoeffReturnType;
 
-  enum {
-    RowsAtCompileTime = XprType::RowsAtCompileTime,
-    CoeffReadCost = internal::evaluator<XprType>::CoeffReadCost
-  };
+  static constexpr bool PacketAccess = static_cast<bool>(Evaluator::Flags & PacketAccessBit);
+  static constexpr bool LinearAccess = static_cast<bool>(Evaluator::Flags & LinearAccessBit);
+  static constexpr bool IsRowMajor = static_cast<bool>(XprType::IsRowMajor);
+  static constexpr int RowsAtCompileTime = XprType::RowsAtCompileTime;
+  static constexpr int ColsAtCompileTime = XprType::ColsAtCompileTime;
+  static constexpr int XprAlignment = Evaluator::Alignment;
+  static constexpr int CoeffReadCost = Evaluator::CoeffReadCost;
+
+  EIGEN_DEVICE_FUNC explicit visitor_evaluator(const XprType& xpr) : m_evaluator(xpr), m_xpr(xpr) {}
 
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_xpr.size(); }
+  // outer-inner access
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
+    return m_evaluator.coeff(row, col);
+  }
+  template <typename Packet, int Alignment = Unaligned>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packet(Index row, Index col) const {
+    return m_evaluator.template packet<Alignment, Packet>(row, col);
+  }
+  // linear access
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { return m_evaluator.coeff(index); }
+  template <typename Packet, int Alignment = XprAlignment>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packet(Index index) const {
+    return m_evaluator.template packet<Alignment, Packet>(index);
+  }
 
-  EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index row, Index col) const
-  { return m_evaluator.coeff(row, col); }
-
-protected:
-  internal::evaluator<XprType> m_evaluator;
-  const XprType &m_xpr;
+ protected:
+  Evaluator m_evaluator;
+  const XprType& m_xpr;
 };
-} // end namespace internal
+
+template <typename Derived, typename Visitor, bool ShortCircuitEvaulation>
+struct visit_impl {
+  using Evaluator = visitor_evaluator<Derived>;
+  using Scalar = typename DenseBase<Derived>::Scalar;
+
+  static constexpr bool IsRowMajor = DenseBase<Derived>::IsRowMajor;
+  static constexpr int SizeAtCompileTime = DenseBase<Derived>::SizeAtCompileTime;
+  static constexpr int RowsAtCompileTime = DenseBase<Derived>::RowsAtCompileTime;
+  static constexpr int ColsAtCompileTime = DenseBase<Derived>::ColsAtCompileTime;
+  static constexpr int InnerSizeAtCompileTime = IsRowMajor ? ColsAtCompileTime : RowsAtCompileTime;
+  static constexpr int OuterSizeAtCompileTime = IsRowMajor ? RowsAtCompileTime : ColsAtCompileTime;
+
+  static constexpr bool LinearAccess =
+      Evaluator::LinearAccess && static_cast<bool>(functor_traits<Visitor>::LinearAccess);
+  static constexpr bool Vectorize = Evaluator::PacketAccess && static_cast<bool>(functor_traits<Visitor>::PacketAccess);
+
+  static constexpr int PacketSize = packet_traits<Scalar>::size;
+  static constexpr int VectorOps =
+      Vectorize ? (LinearAccess ? (SizeAtCompileTime / PacketSize)
+                                : (OuterSizeAtCompileTime * (InnerSizeAtCompileTime / PacketSize)))
+                : 0;
+  static constexpr int ScalarOps = SizeAtCompileTime - (VectorOps * PacketSize);
+  // treat vector op and scalar op as same cost for unroll logic
+  static constexpr int TotalOps = VectorOps + ScalarOps;
+
+  static constexpr int UnrollCost = int(Evaluator::CoeffReadCost) + int(functor_traits<Visitor>::Cost);
+  static constexpr bool Unroll = (SizeAtCompileTime != Dynamic) && ((TotalOps * UnrollCost) <= EIGEN_UNROLLING_LIMIT);
+  static constexpr int UnrollCount = Unroll ? int(SizeAtCompileTime) : Dynamic;
+
+  using impl = visitor_impl<Visitor, Evaluator, UnrollCount, Vectorize, LinearAccess, ShortCircuitEvaulation>;
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const DenseBase<Derived>& mat, Visitor& visitor) {
+    Evaluator evaluator(mat.derived());
+    impl::run(evaluator, visitor);
+  }
+};
+
+}  // end namespace internal
 
 /** Applies the visitor \a visitor to the whole coefficients of the matrix or vector.
-  *
-  * The template parameter \a Visitor is the type of the visitor and provides the following interface:
-  * \code
-  * struct MyVisitor {
-  *   // called for the first coefficient
-  *   void init(const Scalar& value, Index i, Index j);
-  *   // called for all other coefficients
-  *   void operator() (const Scalar& value, Index i, Index j);
-  * };
-  * \endcode
-  *
-  * \note compared to one or two \em for \em loops, visitors offer automatic
-  * unrolling for small fixed size matrix.
-  *
-  * \note if the matrix is empty, then the visitor is left unchanged.
-  *
-  * \sa minCoeff(Index*,Index*), maxCoeff(Index*,Index*), DenseBase::redux()
-  */
-template<typename Derived>
-template<typename Visitor>
-EIGEN_DEVICE_FUNC
-void DenseBase<Derived>::visit(Visitor& visitor) const
-{
-  if(size()==0)
-    return;
-
-  typedef typename internal::visitor_evaluator<Derived> ThisEvaluator;
-  ThisEvaluator thisEval(derived());
-
-  enum {
-    unroll =  SizeAtCompileTime != Dynamic
-           && SizeAtCompileTime * int(ThisEvaluator::CoeffReadCost) + (SizeAtCompileTime-1) * int(internal::functor_traits<Visitor>::Cost) <= EIGEN_UNROLLING_LIMIT
-  };
-  return internal::visitor_impl<Visitor, ThisEvaluator, unroll ? int(SizeAtCompileTime) : Dynamic>::run(thisEval, visitor);
+ *
+ * The template parameter \a Visitor is the type of the visitor and provides the following interface:
+ * \code
+ * struct MyVisitor {
+ *   // called for the first coefficient
+ *   void init(const Scalar& value, Index i, Index j);
+ *   // called for all other coefficients
+ *   void operator() (const Scalar& value, Index i, Index j);
+ * };
+ * \endcode
+ *
+ * \note compared to one or two \em for \em loops, visitors offer automatic
+ * unrolling for small fixed size matrix.
+ *
+ * \note if the matrix is empty, then the visitor is left unchanged.
+ *
+ * \sa minCoeff(Index*,Index*), maxCoeff(Index*,Index*), DenseBase::redux()
+ */
+template <typename Derived>
+template <typename Visitor>
+EIGEN_DEVICE_FUNC void DenseBase<Derived>::visit(Visitor& visitor) const {
+  using impl = internal::visit_impl<Derived, Visitor, /*ShortCircuitEvaulation*/ false>;
+  impl::run(derived(), visitor);
 }
 
 namespace internal {
 
 /** \internal
-  * \brief Base class to implement min and max visitors
-  */
+ * \brief Base class to implement min and max visitors
+ */
 template <typename Derived>
-struct coeff_visitor
-{
+struct coeff_visitor {
   // default initialization to avoid countless invalid maybe-uninitialized warnings by gcc
-  EIGEN_DEVICE_FUNC
-  coeff_visitor() : row(-1), col(-1), res(0) {}
+  EIGEN_DEVICE_FUNC coeff_visitor() : row(-1), col(-1), res(0) {}
   typedef typename Derived::Scalar Scalar;
   Index row, col;
   Scalar res;
-  EIGEN_DEVICE_FUNC
-  inline void init(const Scalar& value, Index i, Index j)
-  {
+  EIGEN_DEVICE_FUNC inline void init(const Scalar& value, Index i, Index j) {
     res = value;
     row = i;
     col = j;
   }
 };
 
-/** \internal
-  * \brief Visitor computing the min coefficient with its value and coordinates
-  *
-  * \sa DenseBase::minCoeff(Index*, Index*)
-  */
-template <typename Derived, int NaNPropagation>
-struct min_coeff_visitor : coeff_visitor<Derived>
-{
-  typedef typename Derived::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  void operator() (const Scalar& value, Index i, Index j)
-  {
-    if(value < this->res)
-    {
+template <typename Scalar, int NaNPropagation, bool is_min = true>
+struct minmax_compare {
+  typedef typename packet_traits<Scalar>::type Packet;
+  static EIGEN_DEVICE_FUNC inline bool compare(Scalar a, Scalar b) { return a < b; }
+  static EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& p) { return predux_min<NaNPropagation>(p); }
+};
+
+template <typename Scalar, int NaNPropagation>
+struct minmax_compare<Scalar, NaNPropagation, false> {
+  typedef typename packet_traits<Scalar>::type Packet;
+  static EIGEN_DEVICE_FUNC inline bool compare(Scalar a, Scalar b) { return a > b; }
+  static EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& p) { return predux_max<NaNPropagation>(p); }
+};
+
+// Default implementation used by non-floating types, where we do not
+// need special logic for NaN handling.
+template <typename Derived, bool is_min, int NaNPropagation,
+          bool isInt = NumTraits<typename Derived::Scalar>::IsInteger>
+struct minmax_coeff_visitor : coeff_visitor<Derived> {
+  using Scalar = typename Derived::Scalar;
+  using Packet = typename packet_traits<Scalar>::type;
+  using Comparator = minmax_compare<Scalar, NaNPropagation, is_min>;
+  static constexpr Index PacketSize = packet_traits<Scalar>::size;
+
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index i, Index j) {
+    if (Comparator::compare(value, this->res)) {
       this->res = value;
       this->row = i;
       this->col = j;
     }
   }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index i, Index j) {
+    Scalar value = Comparator::predux(p);
+    if (Comparator::compare(value, this->res)) {
+      const Packet range = preverse(plset<Packet>(Scalar(1)));
+      Packet mask = pcmp_eq(pset1<Packet>(value), p);
+      Index max_idx = PacketSize - static_cast<Index>(predux_max(pand(range, mask)));
+      this->res = value;
+      this->row = Derived::IsRowMajor ? i : i + max_idx;
+      this->col = Derived::IsRowMajor ? j + max_idx : j;
+    }
+  }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index i, Index j) {
+    Scalar value = Comparator::predux(p);
+    const Packet range = preverse(plset<Packet>(Scalar(1)));
+    Packet mask = pcmp_eq(pset1<Packet>(value), p);
+    Index max_idx = PacketSize - static_cast<Index>(predux_max(pand(range, mask)));
+    this->res = value;
+    this->row = Derived::IsRowMajor ? i : i + max_idx;
+    this->col = Derived::IsRowMajor ? j + max_idx : j;
+  }
 };
 
-template <typename Derived>
-struct min_coeff_visitor<Derived, PropagateNumbers> : coeff_visitor<Derived>
-{
+// Suppress NaN. The only case in which we return NaN is if the matrix is all NaN,
+// in which case, row=0, col=0 is returned for the location.
+template <typename Derived, bool is_min>
+struct minmax_coeff_visitor<Derived, is_min, PropagateNumbers, false> : coeff_visitor<Derived> {
   typedef typename Derived::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  void operator() (const Scalar& value, Index i, Index j)
-  {
-    if((numext::isnan)(this->res) || (!(numext::isnan)(value) && value < this->res))
-    {
+  using Packet = typename packet_traits<Scalar>::type;
+  using Comparator = minmax_compare<Scalar, PropagateNumbers, is_min>;
+
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index i, Index j) {
+    if ((!(numext::isnan)(value) && (numext::isnan)(this->res)) || Comparator::compare(value, this->res)) {
       this->res = value;
       this->row = i;
       this->col = j;
     }
   }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index i, Index j) {
+    const Index PacketSize = packet_traits<Scalar>::size;
+    Scalar value = Comparator::predux(p);
+    if ((!(numext::isnan)(value) && (numext::isnan)(this->res)) || Comparator::compare(value, this->res)) {
+      const Packet range = preverse(plset<Packet>(Scalar(1)));
+      /* mask will be zero for NaNs, so they will be ignored. */
+      Packet mask = pcmp_eq(pset1<Packet>(value), p);
+      Index max_idx = PacketSize - static_cast<Index>(predux_max(pand(range, mask)));
+      this->res = value;
+      this->row = Derived::IsRowMajor ? i : i + max_idx;
+      this->col = Derived::IsRowMajor ? j + max_idx : j;
+    }
+  }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index i, Index j) {
+    const Index PacketSize = packet_traits<Scalar>::size;
+    Scalar value = Comparator::predux(p);
+    if ((numext::isnan)(value)) {
+      this->res = value;
+      this->row = 0;
+      this->col = 0;
+      return;
+    }
+    const Packet range = preverse(plset<Packet>(Scalar(1)));
+    /* mask will be zero for NaNs, so they will be ignored. */
+    Packet mask = pcmp_eq(pset1<Packet>(value), p);
+    Index max_idx = PacketSize - static_cast<Index>(predux_max(pand(range, mask)));
+    this->res = value;
+    this->row = Derived::IsRowMajor ? i : i + max_idx;
+    this->col = Derived::IsRowMajor ? j + max_idx : j;
+  }
 };
 
-template <typename Derived>
-struct min_coeff_visitor<Derived, PropagateNaN> : coeff_visitor<Derived>
-{
+// Propagate NaNs. If the matrix contains NaN, the location of the first NaN
+// will be returned in row and col.
+template <typename Derived, bool is_min, int NaNPropagation>
+struct minmax_coeff_visitor<Derived, is_min, NaNPropagation, false> : coeff_visitor<Derived> {
   typedef typename Derived::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  void operator() (const Scalar& value, Index i, Index j)
-  {
-    if((numext::isnan)(value) || value < this->res)
-    {
+  using Packet = typename packet_traits<Scalar>::type;
+  using Comparator = minmax_compare<Scalar, PropagateNaN, is_min>;
+
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index i, Index j) {
+    const bool value_is_nan = (numext::isnan)(value);
+    if ((value_is_nan && !(numext::isnan)(this->res)) || Comparator::compare(value, this->res)) {
       this->res = value;
       this->row = i;
       this->col = j;
     }
   }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index i, Index j) {
+    const Index PacketSize = packet_traits<Scalar>::size;
+    Scalar value = Comparator::predux(p);
+    const bool value_is_nan = (numext::isnan)(value);
+    if ((value_is_nan && !(numext::isnan)(this->res)) || Comparator::compare(value, this->res)) {
+      const Packet range = preverse(plset<Packet>(Scalar(1)));
+      // If the value is NaN, pick the first position of a NaN, otherwise pick the first extremal value.
+      Packet mask = value_is_nan ? pnot(pcmp_eq(p, p)) : pcmp_eq(pset1<Packet>(value), p);
+      Index max_idx = PacketSize - static_cast<Index>(predux_max(pand(range, mask)));
+      this->res = value;
+      this->row = Derived::IsRowMajor ? i : i + max_idx;
+      this->col = Derived::IsRowMajor ? j + max_idx : j;
+    }
+  }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index i, Index j) {
+    const Index PacketSize = packet_traits<Scalar>::size;
+    Scalar value = Comparator::predux(p);
+    const bool value_is_nan = (numext::isnan)(value);
+    const Packet range = preverse(plset<Packet>(Scalar(1)));
+    // If the value is NaN, pick the first position of a NaN, otherwise pick the first extremal value.
+    Packet mask = value_is_nan ? pnot(pcmp_eq(p, p)) : pcmp_eq(pset1<Packet>(value), p);
+    Index max_idx = PacketSize - static_cast<Index>(predux_max(pand(range, mask)));
+    this->res = value;
+    this->row = Derived::IsRowMajor ? i : i + max_idx;
+    this->col = Derived::IsRowMajor ? j + max_idx : j;
+  }
 };
 
-template<typename Scalar, int NaNPropagation>
-    struct functor_traits<min_coeff_visitor<Scalar, NaNPropagation> > {
+template <typename Derived, bool is_min, int NaNPropagation>
+struct functor_traits<minmax_coeff_visitor<Derived, is_min, NaNPropagation>> {
+  using Scalar = typename Derived::Scalar;
+  enum { Cost = NumTraits<Scalar>::AddCost, LinearAccess = false, PacketAccess = packet_traits<Scalar>::HasCmp };
+};
+
+template <typename Scalar>
+struct all_visitor {
+  using result_type = bool;
+  using Packet = typename packet_traits<Scalar>::type;
+  EIGEN_DEVICE_FUNC inline void init(const Scalar& value, Index, Index) { res = (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline void init(const Scalar& value, Index) { res = (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline bool all_predux(const Packet& p) const { return !predux_any(pcmp_eq(p, pzero(p))); }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index, Index) { res = all_predux(p); }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index) { res = all_predux(p); }
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index, Index) { res = res && (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index) { res = res && (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index, Index) { res = res && all_predux(p); }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index) { res = res && all_predux(p); }
+  EIGEN_DEVICE_FUNC inline bool done() const { return !res; }
+  bool res = true;
+};
+template <typename Scalar>
+struct functor_traits<all_visitor<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::ReadCost, LinearAccess = true, PacketAccess = packet_traits<Scalar>::HasCmp };
+};
+
+template <typename Scalar>
+struct any_visitor {
+  using result_type = bool;
+  using Packet = typename packet_traits<Scalar>::type;
+  EIGEN_DEVICE_FUNC inline void init(const Scalar& value, Index, Index) { res = (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline void init(const Scalar& value, Index) { res = (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline bool any_predux(const Packet& p) const {
+    return predux_any(pandnot(ptrue(p), pcmp_eq(p, pzero(p))));
+  }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index, Index) { res = any_predux(p); }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index) { res = any_predux(p); }
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index, Index) { res = res || (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index) { res = res || (value != Scalar(0)); }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index, Index) { res = res || any_predux(p); }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index) { res = res || any_predux(p); }
+  EIGEN_DEVICE_FUNC inline bool done() const { return res; }
+  bool res = false;
+};
+template <typename Scalar>
+struct functor_traits<any_visitor<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::ReadCost, LinearAccess = true, PacketAccess = packet_traits<Scalar>::HasCmp };
+};
+
+template <typename Scalar>
+struct count_visitor {
+  using result_type = Index;
+  using Packet = typename packet_traits<Scalar>::type;
+  EIGEN_DEVICE_FUNC inline void init(const Scalar& value, Index, Index) { res = value != Scalar(0) ? 1 : 0; }
+  EIGEN_DEVICE_FUNC inline void init(const Scalar& value, Index) { res = value != Scalar(0) ? 1 : 0; }
+  EIGEN_DEVICE_FUNC inline Index count_redux(const Packet& p) const {
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    Packet true_vals = pandnot(cst_one, pcmp_eq(p, pzero(p)));
+    Scalar num_true = predux(true_vals);
+    return static_cast<Index>(num_true);
+  }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index, Index) { res = count_redux(p); }
+  EIGEN_DEVICE_FUNC inline void initpacket(const Packet& p, Index) { res = count_redux(p); }
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index, Index) {
+    if (value != Scalar(0)) res++;
+  }
+  EIGEN_DEVICE_FUNC inline void operator()(const Scalar& value, Index) {
+    if (value != Scalar(0)) res++;
+  }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index, Index) { res += count_redux(p); }
+  EIGEN_DEVICE_FUNC inline void packet(const Packet& p, Index) { res += count_redux(p); }
+  Index res = 0;
+};
+
+template <typename Scalar>
+struct functor_traits<count_visitor<Scalar>> {
   enum {
-    Cost = NumTraits<Scalar>::AddCost
+    Cost = NumTraits<Scalar>::AddCost,
+    LinearAccess = true,
+    // predux is problematic for bool
+    PacketAccess = packet_traits<Scalar>::HasCmp && packet_traits<Scalar>::HasAdd && !is_same<Scalar, bool>::value
   };
 };
 
-/** \internal
-  * \brief Visitor computing the max coefficient with its value and coordinates
-  *
-  * \sa DenseBase::maxCoeff(Index*, Index*)
-  */
-template <typename Derived, int NaNPropagation>
-struct max_coeff_visitor : coeff_visitor<Derived>
-{
-  typedef typename Derived::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  void operator() (const Scalar& value, Index i, Index j)
-  {
-    if(value > this->res)
-    {
-      this->res = value;
-      this->row = i;
-      this->col = j;
-    }
-  }
-};
-
-template <typename Derived>
-struct max_coeff_visitor<Derived, PropagateNumbers> : coeff_visitor<Derived>
-{
-  typedef typename Derived::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  void operator() (const Scalar& value, Index i, Index j)
-  {
-    if((numext::isnan)(this->res) || (!(numext::isnan)(value) && value > this->res))
-    {
-      this->res = value;
-      this->row = i;
-      this->col = j;
-    }
-  }
-};
-
-template <typename Derived>
-struct max_coeff_visitor<Derived, PropagateNaN> : coeff_visitor<Derived>
-{
-  typedef typename Derived::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  void operator() (const Scalar& value, Index i, Index j)
-  {
-    if((numext::isnan)(value) || value > this->res)
-    {
-      this->res = value;
-      this->row = i;
-      this->col = j;
-    }
-  }
-};
-
-template<typename Scalar, int NaNPropagation>
-struct functor_traits<max_coeff_visitor<Scalar, NaNPropagation> > {
-  enum {
-    Cost = NumTraits<Scalar>::AddCost
-  };
-};
-
-} // end namespace internal
+}  // end namespace internal
 
 /** \fn DenseBase<Derived>::minCoeff(IndexType* rowId, IndexType* colId) const
-  * \returns the minimum of all coefficients of *this and puts in *row and *col its location.
-  *
-  * In case \c *this contains NaN, NaNPropagation determines the behavior:
-  *   NaNPropagation == PropagateFast : undefined
-  *   NaNPropagation == PropagateNaN : result is NaN
-  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
-  * \warning the matrix must be not empty, otherwise an assertion is triggered.
-  *
-  * \sa DenseBase::minCoeff(Index*), DenseBase::maxCoeff(Index*,Index*), DenseBase::visit(), DenseBase::minCoeff()
-  */
-template<typename Derived>
-template<int NaNPropagation, typename IndexType>
-EIGEN_DEVICE_FUNC
-typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::minCoeff(IndexType* rowId, IndexType* colId) const
-{
-  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+ * \returns the minimum of all coefficients of *this and puts in *row and *col its location.
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::minCoeff(Index*), DenseBase::maxCoeff(Index*,Index*), DenseBase::visit(), DenseBase::minCoeff()
+ */
+template <typename Derived>
+template <int NaNPropagation, typename IndexType>
+EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar DenseBase<Derived>::minCoeff(IndexType* rowId,
+                                                                                          IndexType* colId) const {
+  eigen_assert(this->rows() > 0 && this->cols() > 0 && "you are using an empty matrix");
 
-  internal::min_coeff_visitor<Derived, NaNPropagation> minVisitor;
+  internal::minmax_coeff_visitor<Derived, true, NaNPropagation> minVisitor;
   this->visit(minVisitor);
   *rowId = minVisitor.row;
   if (colId) *colId = minVisitor.col;
@@ -301,50 +659,46 @@
 }
 
 /** \returns the minimum of all coefficients of *this and puts in *index its location.
-  *
-  * In case \c *this contains NaN, NaNPropagation determines the behavior:
-  *   NaNPropagation == PropagateFast : undefined
-  *   NaNPropagation == PropagateNaN : result is NaN
-  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
-  * \warning the matrix must be not empty, otherwise an assertion is triggered.
-  *
-  * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::visit(), DenseBase::minCoeff()
-  */
-template<typename Derived>
-template<int NaNPropagation, typename IndexType>
-EIGEN_DEVICE_FUNC
-typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::minCoeff(IndexType* index) const
-{
-  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
-
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::visit(),
+ * DenseBase::minCoeff()
+ */
+template <typename Derived>
+template <int NaNPropagation, typename IndexType>
+EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar DenseBase<Derived>::minCoeff(IndexType* index) const {
+  eigen_assert(this->rows() > 0 && this->cols() > 0 && "you are using an empty matrix");
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-      internal::min_coeff_visitor<Derived, NaNPropagation> minVisitor;
+
+  internal::minmax_coeff_visitor<Derived, true, NaNPropagation> minVisitor;
   this->visit(minVisitor);
-  *index = IndexType((RowsAtCompileTime==1) ? minVisitor.col : minVisitor.row);
+  *index = IndexType((RowsAtCompileTime == 1) ? minVisitor.col : minVisitor.row);
   return minVisitor.res;
 }
 
 /** \fn DenseBase<Derived>::maxCoeff(IndexType* rowId, IndexType* colId) const
-  * \returns the maximum of all coefficients of *this and puts in *row and *col its location.
-  *
-  * In case \c *this contains NaN, NaNPropagation determines the behavior:
-  *   NaNPropagation == PropagateFast : undefined
-  *   NaNPropagation == PropagateNaN : result is NaN
-  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
-  * \warning the matrix must be not empty, otherwise an assertion is triggered.
-  *
-  * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visit(), DenseBase::maxCoeff()
-  */
-template<typename Derived>
-template<int NaNPropagation, typename IndexType>
-EIGEN_DEVICE_FUNC
-typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::maxCoeff(IndexType* rowPtr, IndexType* colPtr) const
-{
-  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+ * \returns the maximum of all coefficients of *this and puts in *row and *col its location.
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visit(), DenseBase::maxCoeff()
+ */
+template <typename Derived>
+template <int NaNPropagation, typename IndexType>
+EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar DenseBase<Derived>::maxCoeff(IndexType* rowPtr,
+                                                                                          IndexType* colPtr) const {
+  eigen_assert(this->rows() > 0 && this->cols() > 0 && "you are using an empty matrix");
 
-  internal::max_coeff_visitor<Derived, NaNPropagation> maxVisitor;
+  internal::minmax_coeff_visitor<Derived, false, NaNPropagation> maxVisitor;
   this->visit(maxVisitor);
   *rowPtr = maxVisitor.row;
   if (colPtr) *colPtr = maxVisitor.col;
@@ -352,30 +706,84 @@
 }
 
 /** \returns the maximum of all coefficients of *this and puts in *index its location.
-  *
-  * In case \c *this contains NaN, NaNPropagation determines the behavior:
-  *   NaNPropagation == PropagateFast : undefined
-  *   NaNPropagation == PropagateNaN : result is NaN
-  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
-  * \warning the matrix must be not empty, otherwise an assertion is triggered.
-  *
-  * \sa DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visitor(), DenseBase::maxCoeff()
-  */
-template<typename Derived>
-template<int NaNPropagation, typename IndexType>
-EIGEN_DEVICE_FUNC
-typename internal::traits<Derived>::Scalar
-DenseBase<Derived>::maxCoeff(IndexType* index) const
-{
-  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visitor(),
+ * DenseBase::maxCoeff()
+ */
+template <typename Derived>
+template <int NaNPropagation, typename IndexType>
+EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar DenseBase<Derived>::maxCoeff(IndexType* index) const {
+  eigen_assert(this->rows() > 0 && this->cols() > 0 && "you are using an empty matrix");
 
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-      internal::max_coeff_visitor<Derived, NaNPropagation> maxVisitor;
+  internal::minmax_coeff_visitor<Derived, false, NaNPropagation> maxVisitor;
   this->visit(maxVisitor);
-  *index = (RowsAtCompileTime==1) ? maxVisitor.col : maxVisitor.row;
+  *index = (RowsAtCompileTime == 1) ? maxVisitor.col : maxVisitor.row;
   return maxVisitor.res;
 }
 
-} // end namespace Eigen
+/** \returns true if all coefficients are true
+ *
+ * Example: \include MatrixBase_all.cpp
+ * Output: \verbinclude MatrixBase_all.out
+ *
+ * \sa any(), Cwise::operator<()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::all() const {
+  using Visitor = internal::all_visitor<Scalar>;
+  using impl = internal::visit_impl<Derived, Visitor, /*ShortCircuitEvaulation*/ true>;
+  Visitor visitor;
+  impl::run(derived(), visitor);
+  return visitor.res;
+}
 
-#endif // EIGEN_VISITOR_H
+/** \returns true if at least one coefficient is true
+ *
+ * \sa all()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::any() const {
+  using Visitor = internal::any_visitor<Scalar>;
+  using impl = internal::visit_impl<Derived, Visitor, /*ShortCircuitEvaulation*/ true>;
+  Visitor visitor;
+  impl::run(derived(), visitor);
+  return visitor.res;
+}
+
+/** \returns the number of coefficients which evaluate to true
+ *
+ * \sa all(), any()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC Index DenseBase<Derived>::count() const {
+  using Visitor = internal::count_visitor<Scalar>;
+  using impl = internal::visit_impl<Derived, Visitor, /*ShortCircuitEvaulation*/ false>;
+  Visitor visitor;
+  impl::run(derived(), visitor);
+  return visitor.res;
+}
+
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::hasNaN() const {
+  return derived().cwiseTypedNotEqual(derived()).any();
+}
+
+/** \returns true if \c *this contains only finite numbers, i.e., no NaN and no +/-INF values.
+ *
+ * \sa hasNaN()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::allFinite() const {
+  return derived().array().isFinite().all();
+}
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_VISITOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h
index ab7bd6c..a5e6499 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h
@@ -10,73 +10,82 @@
 #ifndef EIGEN_COMPLEX_AVX_H
 #define EIGEN_COMPLEX_AVX_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 //---------- float ----------
-struct Packet4cf
-{
+struct Packet4cf {
   EIGEN_STRONG_INLINE Packet4cf() {}
   EIGEN_STRONG_INLINE explicit Packet4cf(const __m256& a) : v(a) {}
-  __m256  v;
+  __m256 v;
 };
 
 #ifndef EIGEN_VECTORIZE_AVX512
-template<> struct packet_traits<std::complex<float> >  : default_packet_traits
-{
+template <>
+struct packet_traits<std::complex<float> > : default_packet_traits {
   typedef Packet4cf type;
   typedef Packet2cf half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket = 1,
 
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasDiv    = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
     HasNegate = 1,
-    HasSqrt   = 1,
-    HasAbs    = 0,
-    HasAbs2   = 0,
-    HasMin    = 0,
-    HasMax    = 0,
+    HasSqrt = 1,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
     HasSetLinear = 0
   };
 };
 #endif
 
-template<> struct unpacket_traits<Packet4cf> {
+template <>
+struct unpacket_traits<Packet4cf> {
   typedef std::complex<float> type;
   typedef Packet2cf half;
   typedef Packet8f as_real;
   enum {
-    size=4,
-    alignment=Aligned32,
-    vectorizable=true,
-    masked_load_available=false,
-    masked_store_available=false
+    size = 4,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet4cf padd<Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_add_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet4cf psub<Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_sub_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet4cf pnegate(const Packet4cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4cf padd<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
+  return Packet4cf(_mm256_add_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4cf psub<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
+  return Packet4cf(_mm256_sub_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4cf pnegate(const Packet4cf& a) {
   return Packet4cf(pnegate(a.v));
 }
-template<> EIGEN_STRONG_INLINE Packet4cf pconj(const Packet4cf& a)
-{
-  const __m256 mask = _mm256_castsi256_ps(_mm256_setr_epi32(0x00000000,0x80000000,0x00000000,0x80000000,0x00000000,0x80000000,0x00000000,0x80000000));
-  return Packet4cf(_mm256_xor_ps(a.v,mask));
+template <>
+EIGEN_STRONG_INLINE Packet4cf pconj(const Packet4cf& a) {
+  const __m256 mask = _mm256_castsi256_ps(_mm256_setr_epi32(0x00000000, 0x80000000, 0x00000000, 0x80000000, 0x00000000,
+                                                            0x80000000, 0x00000000, 0x80000000));
+  return Packet4cf(_mm256_xor_ps(a.v, mask));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf pmul<Packet4cf>(const Packet4cf& a, const Packet4cf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4cf pmul<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
   __m256 tmp1 = _mm256_mul_ps(_mm256_moveldup_ps(a.v), b.v);
-  __m256 tmp2 = _mm256_mul_ps(_mm256_movehdup_ps(a.v), _mm256_permute_ps(b.v, _MM_SHUFFLE(2,3,0,1)));
+  __m256 tmp2 = _mm256_mul_ps(_mm256_movehdup_ps(a.v), _mm256_permute_ps(b.v, _MM_SHUFFLE(2, 3, 0, 1)));
   __m256 result = _mm256_addsub_ps(tmp1, tmp2);
   return Packet4cf(result);
 }
@@ -87,165 +96,196 @@
   return Packet4cf(_mm256_and_ps(eq, _mm256_permute_ps(eq, 0xb1)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf ptrue<Packet4cf>(const Packet4cf& a) { return Packet4cf(ptrue(Packet8f(a.v))); }
-template<> EIGEN_STRONG_INLINE Packet4cf pand   <Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_and_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet4cf por    <Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_or_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet4cf pxor   <Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_xor_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet4cf pandnot<Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_andnot_ps(b.v,a.v)); }
-
-template<> EIGEN_STRONG_INLINE Packet4cf pload <Packet4cf>(const std::complex<float>* from) { EIGEN_DEBUG_ALIGNED_LOAD return Packet4cf(pload<Packet8f>(&numext::real_ref(*from))); }
-template<> EIGEN_STRONG_INLINE Packet4cf ploadu<Packet4cf>(const std::complex<float>* from) { EIGEN_DEBUG_UNALIGNED_LOAD return Packet4cf(ploadu<Packet8f>(&numext::real_ref(*from))); }
-
-
-template<> EIGEN_STRONG_INLINE Packet4cf pset1<Packet4cf>(const std::complex<float>& from)
-{
-  return Packet4cf(_mm256_castpd_ps(_mm256_broadcast_sd((const double*)(const void*)&from)));
+template <>
+EIGEN_STRONG_INLINE Packet4cf ptrue<Packet4cf>(const Packet4cf& a) {
+  return Packet4cf(ptrue(Packet8f(a.v)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4cf pand<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
+  return Packet4cf(_mm256_and_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4cf por<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
+  return Packet4cf(_mm256_or_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4cf pxor<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
+  return Packet4cf(_mm256_xor_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4cf pandnot<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
+  return Packet4cf(_mm256_andnot_ps(b.v, a.v));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf ploaddup<Packet4cf>(const std::complex<float>* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4cf pload<Packet4cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return Packet4cf(pload<Packet8f>(&numext::real_ref(*from)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4cf ploadu<Packet4cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return Packet4cf(ploadu<Packet8f>(&numext::real_ref(*from)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4cf pset1<Packet4cf>(const std::complex<float>& from) {
+  const float re = std::real(from);
+  const float im = std::imag(from);
+  return Packet4cf(_mm256_set_ps(im, re, im, re, im, re, im, re));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4cf ploaddup<Packet4cf>(const std::complex<float>* from) {
   // FIXME The following might be optimized using _mm256_movedup_pd
   Packet2cf a = ploaddup<Packet2cf>(from);
-  Packet2cf b = ploaddup<Packet2cf>(from+1);
-  return  Packet4cf(_mm256_insertf128_ps(_mm256_castps128_ps256(a.v), b.v, 1));
+  Packet2cf b = ploaddup<Packet2cf>(from + 1);
+  return Packet4cf(_mm256_insertf128_ps(_mm256_castps128_ps256(a.v), b.v, 1));
 }
 
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float>* to, const Packet4cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore(&numext::real_ref(*to), from.v); }
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float>* to, const Packet4cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu(&numext::real_ref(*to), from.v); }
-
-template<> EIGEN_DEVICE_FUNC inline Packet4cf pgather<std::complex<float>, Packet4cf>(const std::complex<float>* from, Index stride)
-{
-  return Packet4cf(_mm256_set_ps(std::imag(from[3*stride]), std::real(from[3*stride]),
-                                 std::imag(from[2*stride]), std::real(from[2*stride]),
-                                 std::imag(from[1*stride]), std::real(from[1*stride]),
-                                 std::imag(from[0*stride]), std::real(from[0*stride])));
+template <>
+EIGEN_STRONG_INLINE void pstore<std::complex<float> >(std::complex<float>* to, const Packet4cf& from) {
+  EIGEN_DEBUG_ALIGNED_STORE pstore(&numext::real_ref(*to), from.v);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float>* to, const Packet4cf& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE pstoreu(&numext::real_ref(*to), from.v);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet4cf>(std::complex<float>* to, const Packet4cf& from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC inline Packet4cf pgather<std::complex<float>, Packet4cf>(const std::complex<float>* from,
+                                                                           Index stride) {
+  return Packet4cf(_mm256_set_ps(std::imag(from[3 * stride]), std::real(from[3 * stride]), std::imag(from[2 * stride]),
+                                 std::real(from[2 * stride]), std::imag(from[1 * stride]), std::real(from[1 * stride]),
+                                 std::imag(from[0 * stride]), std::real(from[0 * stride])));
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet4cf>(std::complex<float>* to, const Packet4cf& from,
+                                                                       Index stride) {
   __m128 low = _mm256_extractf128_ps(from.v, 0);
-  to[stride*0] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(low, low, 0)),
-                                     _mm_cvtss_f32(_mm_shuffle_ps(low, low, 1)));
-  to[stride*1] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(low, low, 2)),
-                                     _mm_cvtss_f32(_mm_shuffle_ps(low, low, 3)));
+  to[stride * 0] =
+      std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(low, low, 0)), _mm_cvtss_f32(_mm_shuffle_ps(low, low, 1)));
+  to[stride * 1] =
+      std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(low, low, 2)), _mm_cvtss_f32(_mm_shuffle_ps(low, low, 3)));
 
   __m128 high = _mm256_extractf128_ps(from.v, 1);
-  to[stride*2] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(high, high, 0)),
-                                     _mm_cvtss_f32(_mm_shuffle_ps(high, high, 1)));
-  to[stride*3] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(high, high, 2)),
-                                     _mm_cvtss_f32(_mm_shuffle_ps(high, high, 3)));
-
+  to[stride * 2] =
+      std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(high, high, 0)), _mm_cvtss_f32(_mm_shuffle_ps(high, high, 1)));
+  to[stride * 3] =
+      std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(high, high, 2)), _mm_cvtss_f32(_mm_shuffle_ps(high, high, 3)));
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<float>  pfirst<Packet4cf>(const Packet4cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet4cf>(const Packet4cf& a) {
   return pfirst(Packet2cf(_mm256_castps256_ps128(a.v)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf preverse(const Packet4cf& a) {
-  __m128 low  = _mm256_extractf128_ps(a.v, 0);
+template <>
+EIGEN_STRONG_INLINE Packet4cf preverse(const Packet4cf& a) {
+  __m128 low = _mm256_extractf128_ps(a.v, 0);
   __m128 high = _mm256_extractf128_ps(a.v, 1);
-  __m128d lowd  = _mm_castps_pd(low);
+  __m128d lowd = _mm_castps_pd(low);
   __m128d highd = _mm_castps_pd(high);
-  low  = _mm_castpd_ps(_mm_shuffle_pd(lowd,lowd,0x1));
-  high = _mm_castpd_ps(_mm_shuffle_pd(highd,highd,0x1));
+  low = _mm_castpd_ps(_mm_shuffle_pd(lowd, lowd, 0x1));
+  high = _mm_castpd_ps(_mm_shuffle_pd(highd, highd, 0x1));
   __m256 result = _mm256_setzero_ps();
   result = _mm256_insertf128_ps(result, low, 1);
   result = _mm256_insertf128_ps(result, high, 0);
   return Packet4cf(result);
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<float> predux<Packet4cf>(const Packet4cf& a)
-{
-  return predux(padd(Packet2cf(_mm256_extractf128_ps(a.v,0)),
-                     Packet2cf(_mm256_extractf128_ps(a.v,1))));
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux<Packet4cf>(const Packet4cf& a) {
+  return predux(padd(Packet2cf(_mm256_extractf128_ps(a.v, 0)), Packet2cf(_mm256_extractf128_ps(a.v, 1))));
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet4cf>(const Packet4cf& a)
-{
-  return predux_mul(pmul(Packet2cf(_mm256_extractf128_ps(a.v, 0)),
-                         Packet2cf(_mm256_extractf128_ps(a.v, 1))));
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet4cf>(const Packet4cf& a) {
+  return predux_mul(pmul(Packet2cf(_mm256_extractf128_ps(a.v, 0)), Packet2cf(_mm256_extractf128_ps(a.v, 1))));
 }
 
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet4cf,Packet8f)
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet4cf, Packet8f)
 
-template<> EIGEN_STRONG_INLINE Packet4cf pdiv<Packet4cf>(const Packet4cf& a, const Packet4cf& b)
-{
-  Packet4cf num = pmul(a, pconj(b));
-  __m256 tmp = _mm256_mul_ps(b.v, b.v);
-  __m256 tmp2    = _mm256_shuffle_ps(tmp,tmp,0xB1);
-  __m256 denom = _mm256_add_ps(tmp, tmp2);
-  return Packet4cf(_mm256_div_ps(num.v, denom));
+template <>
+EIGEN_STRONG_INLINE Packet4cf pdiv<Packet4cf>(const Packet4cf& a, const Packet4cf& b) {
+  return pdiv_complex(a, b);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf pcplxflip<Packet4cf>(const Packet4cf& x)
-{
-  return Packet4cf(_mm256_shuffle_ps(x.v, x.v, _MM_SHUFFLE(2, 3, 0 ,1)));
+template <>
+EIGEN_STRONG_INLINE Packet4cf pcplxflip<Packet4cf>(const Packet4cf& x) {
+  return Packet4cf(_mm256_shuffle_ps(x.v, x.v, _MM_SHUFFLE(2, 3, 0, 1)));
 }
 
 //---------- double ----------
-struct Packet2cd
-{
+struct Packet2cd {
   EIGEN_STRONG_INLINE Packet2cd() {}
   EIGEN_STRONG_INLINE explicit Packet2cd(const __m256d& a) : v(a) {}
-  __m256d  v;
+  __m256d v;
 };
 
 #ifndef EIGEN_VECTORIZE_AVX512
-template<> struct packet_traits<std::complex<double> >  : default_packet_traits
-{
+template <>
+struct packet_traits<std::complex<double> > : default_packet_traits {
   typedef Packet2cd type;
   typedef Packet1cd half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 0,
     size = 2,
-    HasHalfPacket = 1,
 
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasDiv    = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
     HasNegate = 1,
-    HasSqrt   = 1,
-    HasAbs    = 0,
-    HasAbs2   = 0,
-    HasMin    = 0,
-    HasMax    = 0,
+    HasSqrt = 1,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
     HasSetLinear = 0
   };
 };
 #endif
 
-template<> struct unpacket_traits<Packet2cd> {
+template <>
+struct unpacket_traits<Packet2cd> {
   typedef std::complex<double> type;
   typedef Packet1cd half;
   typedef Packet4d as_real;
   enum {
-    size=2,
-    alignment=Aligned32,
-    vectorizable=true,
-    masked_load_available=false,
-    masked_store_available=false
+    size = 2,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet2cd padd<Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_add_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cd psub<Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_sub_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cd pnegate(const Packet2cd& a) { return Packet2cd(pnegate(a.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cd pconj(const Packet2cd& a)
-{
-  const __m256d mask = _mm256_castsi256_pd(_mm256_set_epi32(0x80000000,0x0,0x0,0x0,0x80000000,0x0,0x0,0x0));
-  return Packet2cd(_mm256_xor_pd(a.v,mask));
+template <>
+EIGEN_STRONG_INLINE Packet2cd padd<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  return Packet2cd(_mm256_add_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd psub<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  return Packet2cd(_mm256_sub_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd pnegate(const Packet2cd& a) {
+  return Packet2cd(pnegate(a.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd pconj(const Packet2cd& a) {
+  const __m256d mask = _mm256_castsi256_pd(_mm256_set_epi32(0x80000000, 0x0, 0x0, 0x0, 0x80000000, 0x0, 0x0, 0x0));
+  return Packet2cd(_mm256_xor_pd(a.v, mask));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd pmul<Packet2cd>(const Packet2cd& a, const Packet2cd& b)
-{
-  __m256d tmp1 = _mm256_shuffle_pd(a.v,a.v,0x0);
+template <>
+EIGEN_STRONG_INLINE Packet2cd pmul<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  __m256d tmp1 = _mm256_shuffle_pd(a.v, a.v, 0x0);
   __m256d even = _mm256_mul_pd(tmp1, b.v);
-  __m256d tmp2 = _mm256_shuffle_pd(a.v,a.v,0xF);
-  __m256d tmp3 = _mm256_shuffle_pd(b.v,b.v,0x5);
-  __m256d odd  = _mm256_mul_pd(tmp2, tmp3);
+  __m256d tmp2 = _mm256_shuffle_pd(a.v, a.v, 0xF);
+  __m256d tmp3 = _mm256_shuffle_pd(b.v, b.v, 0x5);
+  __m256d odd = _mm256_mul_pd(tmp2, tmp3);
   return Packet2cd(_mm256_addsub_pd(even, odd));
 }
 
@@ -255,85 +295,110 @@
   return Packet2cd(pand(eq, _mm256_permute_pd(eq, 0x5)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd ptrue<Packet2cd>(const Packet2cd& a) { return Packet2cd(ptrue(Packet4d(a.v))); }
-template<> EIGEN_STRONG_INLINE Packet2cd pand   <Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_and_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cd por    <Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_or_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cd pxor   <Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_xor_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cd pandnot<Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_andnot_pd(b.v,a.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet2cd ptrue<Packet2cd>(const Packet2cd& a) {
+  return Packet2cd(ptrue(Packet4d(a.v)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd pand<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  return Packet2cd(_mm256_and_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd por<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  return Packet2cd(_mm256_or_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd pxor<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  return Packet2cd(_mm256_xor_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd pandnot<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  return Packet2cd(_mm256_andnot_pd(b.v, a.v));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2cd pload <Packet2cd>(const std::complex<double>* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return Packet2cd(pload<Packet4d>((const double*)from)); }
-template<> EIGEN_STRONG_INLINE Packet2cd ploadu<Packet2cd>(const std::complex<double>* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cd(ploadu<Packet4d>((const double*)from)); }
+template <>
+EIGEN_STRONG_INLINE Packet2cd pload<Packet2cd>(const std::complex<double>* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return Packet2cd(pload<Packet4d>((const double*)from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cd ploadu<Packet2cd>(const std::complex<double>* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cd(ploadu<Packet4d>((const double*)from));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2cd pset1<Packet2cd>(const std::complex<double>& from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2cd pset1<Packet2cd>(const std::complex<double>& from) {
   // in case casting to a __m128d* is really not safe, then we can still fallback to this version: (much slower though)
-//   return Packet2cd(_mm256_loadu2_m128d((const double*)&from,(const double*)&from));
-    return Packet2cd(_mm256_broadcast_pd((const __m128d*)(const void*)&from));
+  //   return Packet2cd(_mm256_loadu2_m128d((const double*)&from,(const double*)&from));
+  return Packet2cd(_mm256_broadcast_pd((const __m128d*)(const void*)&from));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd ploaddup<Packet2cd>(const std::complex<double>* from) { return pset1<Packet2cd>(*from); }
-
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<double> >(std::complex<double> *   to, const Packet2cd& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((double*)to, from.v); }
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double> *   to, const Packet2cd& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((double*)to, from.v); }
-
-template<> EIGEN_DEVICE_FUNC inline Packet2cd pgather<std::complex<double>, Packet2cd>(const std::complex<double>* from, Index stride)
-{
-  return Packet2cd(_mm256_set_pd(std::imag(from[1*stride]), std::real(from[1*stride]),
-				 std::imag(from[0*stride]), std::real(from[0*stride])));
+template <>
+EIGEN_STRONG_INLINE Packet2cd ploaddup<Packet2cd>(const std::complex<double>* from) {
+  return pset1<Packet2cd>(*from);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<double>, Packet2cd>(std::complex<double>* to, const Packet2cd& from, Index stride)
-{
+template <>
+EIGEN_STRONG_INLINE void pstore<std::complex<double> >(std::complex<double>* to, const Packet2cd& from) {
+  EIGEN_DEBUG_ALIGNED_STORE pstore((double*)to, from.v);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double>* to, const Packet2cd& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE pstoreu((double*)to, from.v);
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline Packet2cd pgather<std::complex<double>, Packet2cd>(const std::complex<double>* from,
+                                                                            Index stride) {
+  return Packet2cd(_mm256_set_pd(std::imag(from[1 * stride]), std::real(from[1 * stride]), std::imag(from[0 * stride]),
+                                 std::real(from[0 * stride])));
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<std::complex<double>, Packet2cd>(std::complex<double>* to, const Packet2cd& from,
+                                                                        Index stride) {
   __m128d low = _mm256_extractf128_pd(from.v, 0);
-  to[stride*0] = std::complex<double>(_mm_cvtsd_f64(low), _mm_cvtsd_f64(_mm_shuffle_pd(low, low, 1)));
+  to[stride * 0] = std::complex<double>(_mm_cvtsd_f64(low), _mm_cvtsd_f64(_mm_shuffle_pd(low, low, 1)));
   __m128d high = _mm256_extractf128_pd(from.v, 1);
-  to[stride*1] = std::complex<double>(_mm_cvtsd_f64(high), _mm_cvtsd_f64(_mm_shuffle_pd(high, high, 1)));
+  to[stride * 1] = std::complex<double>(_mm_cvtsd_f64(high), _mm_cvtsd_f64(_mm_shuffle_pd(high, high, 1)));
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<double> pfirst<Packet2cd>(const Packet2cd& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<double> pfirst<Packet2cd>(const Packet2cd& a) {
   __m128d low = _mm256_extractf128_pd(a.v, 0);
   EIGEN_ALIGN16 double res[2];
   _mm_store_pd(res, low);
-  return std::complex<double>(res[0],res[1]);
+  return std::complex<double>(res[0], res[1]);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd preverse(const Packet2cd& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2cd preverse(const Packet2cd& a) {
   __m256d result = _mm256_permute2f128_pd(a.v, a.v, 1);
   return Packet2cd(result);
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<double> predux<Packet2cd>(const Packet2cd& a)
-{
-  return predux(padd(Packet1cd(_mm256_extractf128_pd(a.v,0)),
-                     Packet1cd(_mm256_extractf128_pd(a.v,1))));
+template <>
+EIGEN_STRONG_INLINE std::complex<double> predux<Packet2cd>(const Packet2cd& a) {
+  return predux(padd(Packet1cd(_mm256_extractf128_pd(a.v, 0)), Packet1cd(_mm256_extractf128_pd(a.v, 1))));
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet2cd>(const Packet2cd& a)
-{
-  return predux(pmul(Packet1cd(_mm256_extractf128_pd(a.v,0)),
-                     Packet1cd(_mm256_extractf128_pd(a.v,1))));
+template <>
+EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet2cd>(const Packet2cd& a) {
+  return predux(pmul(Packet1cd(_mm256_extractf128_pd(a.v, 0)), Packet1cd(_mm256_extractf128_pd(a.v, 1))));
 }
 
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cd,Packet4d)
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cd, Packet4d)
 
-template<> EIGEN_STRONG_INLINE Packet2cd pdiv<Packet2cd>(const Packet2cd& a, const Packet2cd& b)
-{
-  Packet2cd num = pmul(a, pconj(b));
-  __m256d tmp = _mm256_mul_pd(b.v, b.v);
-  __m256d denom = _mm256_hadd_pd(tmp, tmp);
-  return Packet2cd(_mm256_div_pd(num.v, denom));
+template <>
+EIGEN_STRONG_INLINE Packet2cd pdiv<Packet2cd>(const Packet2cd& a, const Packet2cd& b) {
+  return pdiv_complex(a, b);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd pcplxflip<Packet2cd>(const Packet2cd& x)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2cd pcplxflip<Packet2cd>(const Packet2cd& x) {
   return Packet2cd(_mm256_shuffle_pd(x.v, x.v, 0x5));
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet4cf,4>& kernel) {
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet4cf, 4>& kernel) {
   __m256d P0 = _mm256_castps_pd(kernel.packet[0].v);
   __m256d P1 = _mm256_castps_pd(kernel.packet[1].v);
   __m256d P2 = _mm256_castps_pd(kernel.packet[2].v);
@@ -350,23 +415,24 @@
   kernel.packet[2].v = _mm256_castpd_ps(_mm256_permute2f128_pd(T1, T3, 49));
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet2cd,2>& kernel) {
-  __m256d tmp = _mm256_permute2f128_pd(kernel.packet[0].v, kernel.packet[1].v, 0+(2<<4));
-  kernel.packet[1].v = _mm256_permute2f128_pd(kernel.packet[0].v, kernel.packet[1].v, 1+(3<<4));
- kernel.packet[0].v = tmp;
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet2cd, 2>& kernel) {
+  __m256d tmp = _mm256_permute2f128_pd(kernel.packet[0].v, kernel.packet[1].v, 0 + (2 << 4));
+  kernel.packet[1].v = _mm256_permute2f128_pd(kernel.packet[0].v, kernel.packet[1].v, 1 + (3 << 4));
+  kernel.packet[0].v = tmp;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd psqrt<Packet2cd>(const Packet2cd& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2cd psqrt<Packet2cd>(const Packet2cd& a) {
   return psqrt_complex<Packet2cd>(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf psqrt<Packet4cf>(const Packet4cf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4cf psqrt<Packet4cf>(const Packet4cf& a) {
   return psqrt_complex<Packet4cf>(a);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COMPLEX_AVX_H
+#endif  // EIGEN_COMPLEX_AVX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h
index 67041c8..b125d59 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h
@@ -14,176 +14,49 @@
  * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/
  */
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
+EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_FLOAT(Packet8f)
+EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_DOUBLE(Packet4d)
+
+// Notice that for newer processors, it is counterproductive to use Newton
+// iteration for square root. In particular, Skylake and Zen2 processors
+// have approximately doubled throughput of the _mm_sqrt_ps instruction
+// compared to their predecessors.
 template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-psin<Packet8f>(const Packet8f& _x) {
-  return psin_float(_x);
-}
-
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-pcos<Packet8f>(const Packet8f& _x) {
-  return pcos_float(_x);
-}
-
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-plog<Packet8f>(const Packet8f& _x) {
-  return plog_float(_x);
-}
-
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4d
-plog<Packet4d>(const Packet4d& _x) {
-  return plog_double(_x);
-}
-
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-plog2<Packet8f>(const Packet8f& _x) {
-  return plog2_float(_x);
-}
-
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4d
-plog2<Packet4d>(const Packet4d& _x) {
-  return plog2_double(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f plog1p<Packet8f>(const Packet8f& _x) {
-  return generic_plog1p(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f pexpm1<Packet8f>(const Packet8f& _x) {
-  return generic_expm1(_x);
-}
-
-// Exponential function. Works by writing "x = m*log(2) + r" where
-// "m = floor(x/log(2)+1/2)" and "r" is the remainder. The result is then
-// "exp(x) = 2^m*exp(r)" where exp(r) is in the range [-1,1).
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-pexp<Packet8f>(const Packet8f& _x) {
-  return pexp_float(_x);
-}
-
-// Hyperbolic Tangent function.
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-ptanh<Packet8f>(const Packet8f& _x) {
-  return internal::generic_fast_tanh_float(_x);
-}
-
-// Exponential function for doubles.
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4d
-pexp<Packet4d>(const Packet4d& _x) {
-  return pexp_double(_x);
-}
-
-// Functions for sqrt.
-// The EIGEN_FAST_MATH version uses the _mm_rsqrt_ps approximation and one step
-// of Newton's method, at a cost of 1-2 bits of precision as opposed to the
-// exact solution. It does not handle +inf, or denormalized numbers correctly.
-// The main advantage of this approach is not just speed, but also the fact that
-// it can be inlined and pipelined with other computations, further reducing its
-// effective latency. This is similar to Quake3's fast inverse square root.
-// For detail see here: http://www.beyond3d.com/content/articles/8/
-#if EIGEN_FAST_MATH
-template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f psqrt<Packet8f>(const Packet8f& _x) {
-  Packet8f minus_half_x = pmul(_x, pset1<Packet8f>(-0.5f));
-  Packet8f denormal_mask = pandnot(
-      pcmp_lt(_x, pset1<Packet8f>((std::numeric_limits<float>::min)())),
-      pcmp_lt(_x, pzero(_x)));
-
-  // Compute approximate reciprocal sqrt.
-  Packet8f x = _mm256_rsqrt_ps(_x);
-  // Do a single step of Newton's iteration.
-  x = pmul(x, pmadd(minus_half_x, pmul(x,x), pset1<Packet8f>(1.5f)));
-  // Flush results for denormals to zero.
-  return pandnot(pmul(_x,x), denormal_mask);
-}
-
-#else
-
-template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f psqrt<Packet8f>(const Packet8f& _x) {
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet8f psqrt<Packet8f>(const Packet8f& _x) {
   return _mm256_sqrt_ps(_x);
 }
-
-#endif
-
-template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4d psqrt<Packet4d>(const Packet4d& _x) {
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet4d psqrt<Packet4d>(const Packet4d& _x) {
   return _mm256_sqrt_pd(_x);
 }
 
+// Even on Skylake, using Newton iteration is a win for reciprocal square root.
 #if EIGEN_FAST_MATH
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f prsqrt<Packet8f>(const Packet8f& _x) {
-  _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(inf, 0x7f800000);
-  _EIGEN_DECLARE_CONST_Packet8f(one_point_five, 1.5f);
-  _EIGEN_DECLARE_CONST_Packet8f(minus_half, -0.5f);
-  _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(flt_min, 0x00800000);
-
-  Packet8f neg_half = pmul(_x, p8f_minus_half);
-
-  // select only the inverse sqrt of positive normal inputs (denormals are
-  // flushed to zero and cause infs as well).
-  Packet8f lt_min_mask = _mm256_cmp_ps(_x, p8f_flt_min, _CMP_LT_OQ);
-  Packet8f inf_mask =  _mm256_cmp_ps(_x, p8f_inf, _CMP_EQ_OQ);
-  Packet8f not_normal_finite_mask = _mm256_or_ps(lt_min_mask, inf_mask);
-
-  // Compute an approximate result using the rsqrt intrinsic.
-  Packet8f y_approx = _mm256_rsqrt_ps(_x);
-
-  // Do a single step of Newton-Raphson iteration to improve the approximation.
-  // This uses the formula y_{n+1} = y_n * (1.5 - y_n * (0.5 * x) * y_n).
-  // It is essential to evaluate the inner term like this because forming
-  // y_n^2 may over- or underflow.
-  Packet8f y_newton = pmul(y_approx, pmadd(y_approx, pmul(neg_half, y_approx), p8f_one_point_five));
-
-  // Select the result of the Newton-Raphson step for positive normal arguments.
-  // For other arguments, choose the output of the intrinsic. This will
-  // return rsqrt(+inf) = 0, rsqrt(x) = NaN if x < 0, and rsqrt(x) = +inf if
-  // x is zero or a positive denormalized float (equivalent to flushing positive
-  // denormalized inputs to zero).
-  return pselect<Packet8f>(not_normal_finite_mask, y_approx, y_newton);
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet8f prsqrt<Packet8f>(const Packet8f& a) {
+  // _mm256_rsqrt_ps returns -inf for negative denormals.
+  // _mm512_rsqrt**_ps returns -NaN for negative denormals.  We may want
+  // consistency here.
+  // const Packet8f rsqrt = pselect(pcmp_lt(a, pzero(a)),
+  //                                pset1<Packet8f>(-NumTraits<float>::quiet_NaN()),
+  //                                _mm256_rsqrt_ps(a));
+  return generic_rsqrt_newton_step<Packet8f, /*Steps=*/1>::run(a, _mm256_rsqrt_ps(a));
 }
 
-#else
-template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f prsqrt<Packet8f>(const Packet8f& _x) {
-  _EIGEN_DECLARE_CONST_Packet8f(one, 1.0f);
-  return _mm256_div_ps(p8f_one, _mm256_sqrt_ps(_x));
+template <>
+EIGEN_STRONG_INLINE Packet8f preciprocal<Packet8f>(const Packet8f& a) {
+  return generic_reciprocal_newton_step<Packet8f, /*Steps=*/1>::run(a, _mm256_rcp_ps(a));
 }
+
 #endif
 
-template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4d prsqrt<Packet4d>(const Packet4d& _x) {
-  _EIGEN_DECLARE_CONST_Packet4d(one, 1.0);
-  return _mm256_div_pd(p4d_one, _mm256_sqrt_pd(_x));
-}
-
-F16_PACKET_FUNCTION(Packet8f, Packet8h, psin)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, pcos)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, plog)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, plog2)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, plog1p)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, pexpm1)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, pexp)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, ptanh)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, psqrt)
-F16_PACKET_FUNCTION(Packet8f, Packet8h, prsqrt)
-
 template <>
 EIGEN_STRONG_INLINE Packet8h pfrexp(const Packet8h& a, Packet8h& exponent) {
   Packet8f fexponent;
@@ -197,17 +70,6 @@
   return float2half(pldexp<Packet8f>(half2float(a), half2float(exponent)));
 }
 
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psin)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pcos)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog2)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog1p)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pexpm1)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pexp)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, ptanh)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psqrt)
-BF16_PACKET_FUNCTION(Packet8f, Packet8bf, prsqrt)
-
 template <>
 EIGEN_STRONG_INLINE Packet8bf pfrexp(const Packet8bf& a, Packet8bf& exponent) {
   Packet8f fexponent;
@@ -221,6 +83,29 @@
   return F32ToBf16(pldexp<Packet8f>(Bf16ToF32(a), Bf16ToF32(exponent)));
 }
 
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pcos)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pexp)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pexpm1)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog1p)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog2)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, preciprocal)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, prsqrt)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psin)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psqrt)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, ptanh)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, pcos)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, pexp)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, pexpm1)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, plog)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, plog1p)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, plog2)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, preciprocal)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, prsqrt)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, psin)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, psqrt)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, ptanh)
+
 }  // end namespace internal
 
 }  // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h
index 7fc32fd..d752f06 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h
@@ -10,6 +10,9 @@
 #ifndef EIGEN_PACKET_MATH_AVX_H
 #define EIGEN_PACKET_MATH_AVX_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
@@ -28,47 +31,89 @@
 #endif
 #endif
 
-typedef __m256  Packet8f;
-typedef __m256i Packet8i;
+typedef __m256 Packet8f;
+typedef eigen_packet_wrapper<__m256i, 0> Packet8i;
 typedef __m256d Packet4d;
+#ifndef EIGEN_VECTORIZE_AVX512FP16
 typedef eigen_packet_wrapper<__m128i, 2> Packet8h;
+#endif
 typedef eigen_packet_wrapper<__m128i, 3> Packet8bf;
+typedef eigen_packet_wrapper<__m256i, 4> Packet8ui;
 
-template<> struct is_arithmetic<__m256>  { enum { value = true }; };
-template<> struct is_arithmetic<__m256i> { enum { value = true }; };
-template<> struct is_arithmetic<__m256d> { enum { value = true }; };
-template<> struct is_arithmetic<Packet8h> { enum { value = true }; };
-template<> struct is_arithmetic<Packet8bf> { enum { value = true }; };
+#ifdef EIGEN_VECTORIZE_AVX2
+// Start from 3 to be compatible with AVX512
+typedef eigen_packet_wrapper<__m256i, 3> Packet4l;
+typedef eigen_packet_wrapper<__m256i, 5> Packet4ul;
+#endif
 
-#define _EIGEN_DECLARE_CONST_Packet8f(NAME,X) \
-  const Packet8f p8f_##NAME = pset1<Packet8f>(X)
-
-#define _EIGEN_DECLARE_CONST_Packet4d(NAME,X) \
-  const Packet4d p4d_##NAME = pset1<Packet4d>(X)
-
-#define _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(NAME,X) \
-  const Packet8f p8f_##NAME = _mm256_castsi256_ps(pset1<Packet8i>(X))
-
-#define _EIGEN_DECLARE_CONST_Packet8i(NAME,X) \
-  const Packet8i p8i_##NAME = pset1<Packet8i>(X)
+template <>
+struct is_arithmetic<__m256> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<__m256i> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<__m256d> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<Packet8i> {
+  enum { value = true };
+};
+// Note that `Packet8ui` uses the underlying type `__m256i`, which is
+// interpreted as a vector of _signed_ `int32`s, which breaks some arithmetic
+// operations used in `GenericPacketMath.h`.
+template <>
+struct is_arithmetic<Packet8ui> {
+  enum { value = false };
+};
+#ifndef EIGEN_VECTORIZE_AVX512FP16
+template <>
+struct is_arithmetic<Packet8h> {
+  enum { value = true };
+};
+#endif
+template <>
+struct is_arithmetic<Packet8bf> {
+  enum { value = true };
+};
+#ifdef EIGEN_VECTORIZE_AVX2
+template <>
+struct is_arithmetic<Packet4l> {
+  enum { value = true };
+};
+// Note that `Packet4ul` uses the underlying type `__m256i`, which is
+// interpreted as a vector of _signed_ `int32`s, which breaks some arithmetic
+// operations used in `GenericPacketMath.h`.
+template <>
+struct is_arithmetic<Packet4ul> {
+  enum { value = false };
+};
+#endif
 
 // Use the packet_traits defined in AVX512/PacketMath.h instead if we're going
 // to leverage AVX512 instructions.
 #ifndef EIGEN_VECTORIZE_AVX512
-template<> struct packet_traits<float>  : default_packet_traits
-{
+template <>
+struct packet_traits<float> : default_packet_traits {
   typedef Packet8f type;
   typedef Packet4f half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 8,
-    HasHalfPacket = 1,
 
-    HasCmp  = 1,
+    HasCmp = 1,
     HasDiv = 1,
+    HasReciprocal = EIGEN_FAST_MATH,
     HasSin = EIGEN_FAST_MATH,
     HasCos = EIGEN_FAST_MATH,
+    HasACos = 1,
+    HasASin = 1,
+    HasATan = 1,
+    HasATanh = 1,
     HasLog = 1,
     HasLog1p = 1,
     HasExpm1 = 1,
@@ -86,22 +131,22 @@
     HasRint = 1
   };
 };
-template<> struct packet_traits<double> : default_packet_traits
-{
+template <>
+struct packet_traits<double> : default_packet_traits {
   typedef Packet4d type;
   typedef Packet2d half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
-    size=4,
-    HasHalfPacket = 1,
+    size = 4,
 
-    HasCmp  = 1,
-    HasDiv  = 1,
-    HasLog  = 1,
-    HasExp  = 1,
+    HasCmp = 1,
+    HasDiv = 1,
+    HasLog = 1,
+    HasExp = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
+    HasATan = 1,
     HasBlend = 1,
     HasRound = 1,
     HasFloor = 1,
@@ -119,37 +164,36 @@
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 8,
-    HasHalfPacket = 0,
 
-    HasCmp    = 1,
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasDiv    = 1,
-    HasSin    = EIGEN_FAST_MATH,
-    HasCos    = EIGEN_FAST_MATH,
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
+    HasSin = EIGEN_FAST_MATH,
+    HasCos = EIGEN_FAST_MATH,
     HasNegate = 1,
-    HasAbs    = 1,
-    HasAbs2   = 0,
-    HasMin    = 1,
-    HasMax    = 1,
-    HasConj   = 1,
+    HasAbs = 1,
+    HasAbs2 = 0,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
     HasSetLinear = 0,
-    HasLog    = 1,
-    HasLog1p  = 1,
-    HasExpm1  = 1,
-    HasExp    = 1,
-    HasSqrt   = 1,
-    HasRsqrt  = 1,
-    HasTanh   = EIGEN_FAST_MATH,
-    HasErf    = EIGEN_FAST_MATH,
-    HasBlend  = 0,
-    HasRound  = 1,
-    HasFloor  = 1,
-    HasCeil   = 1,
-    HasRint   = 1,
+    HasLog = 1,
+    HasLog1p = 1,
+    HasExpm1 = 1,
+    HasExp = 1,
+    HasSqrt = 1,
+    HasRsqrt = 1,
+    HasTanh = EIGEN_FAST_MATH,
+    HasErf = EIGEN_FAST_MATH,
+    HasBlend = 0,
+    HasRound = 1,
+    HasFloor = 1,
+    HasCeil = 1,
+    HasRint = 1,
     HasBessel = 1,
-    HasNdtri  = 1
+    HasNdtri = 1
   };
 };
 
@@ -163,7 +207,6 @@
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 8,
-    HasHalfPacket = 0,
 
     HasCmp = 1,
     HasAdd = 1,
@@ -173,15 +216,15 @@
     HasSin = EIGEN_FAST_MATH,
     HasCos = EIGEN_FAST_MATH,
     HasNegate = 1,
-    HasAbs    = 1,
-    HasAbs2   = 0,
-    HasMin    = 1,
-    HasMax    = 1,
-    HasConj   = 1,
+    HasAbs = 1,
+    HasAbs2 = 0,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
     HasSetLinear = 0,
     HasLog = 1,
-    HasLog1p  = 1,
-    HasExpm1  = 1,
+    HasLog1p = 1,
+    HasExpm1 = 1,
     HasExp = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
@@ -193,41 +236,172 @@
     HasCeil = 1,
     HasRint = 1,
     HasBessel = 1,
-    HasNdtri  = 1
+    HasNdtri = 1
+  };
+};
+
+template <>
+struct packet_traits<int> : default_packet_traits {
+  typedef Packet8i type;
+  typedef Packet4i half;
+  enum { Vectorizable = 1, AlignedOnScalar = 1, HasCmp = 1, HasDiv = 1, size = 8 };
+};
+template <>
+struct packet_traits<uint32_t> : default_packet_traits {
+  typedef Packet8ui type;
+  typedef Packet4ui half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 8,
+
+    HasDiv = 0,
+    HasNegate = 0,
+    HasSqrt = 0,
+
+    HasCmp = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasShift = 1
+  };
+};
+
+#ifdef EIGEN_VECTORIZE_AVX2
+template <>
+struct packet_traits<int64_t> : default_packet_traits {
+  typedef Packet4l type;
+  // There is no half-size packet for current Packet4l.
+  // TODO: support as SSE path.
+  typedef Packet4l half;
+  enum { Vectorizable = 1, AlignedOnScalar = 1, HasCmp = 1, size = 4 };
+};
+template <>
+struct packet_traits<uint64_t> : default_packet_traits {
+  typedef Packet4ul type;
+  // There is no half-size packet for current Packet4ul.
+  // TODO: support as SSE path.
+  typedef Packet4ul half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 4,
+
+    // HasMin = 0,
+    // HasMax = 0,
+    HasDiv = 0,
+    HasBlend = 0,
+    HasTranspose = 0,
+    HasNegate = 0,
+    HasSqrt = 0,
+    HasCmp = 1,
+    HasShift = 1
   };
 };
 #endif
 
-template<> struct scalar_div_cost<float,true> { enum { value = 14 }; };
-template<> struct scalar_div_cost<double,true> { enum { value = 16 }; };
+#endif
 
-/* Proper support for integers is only provided by AVX2. In the meantime, we'll
-   use SSE instructions and packets to deal with integers.
-template<> struct packet_traits<int>    : default_packet_traits
-{
-  typedef Packet8i type;
+template <>
+struct scalar_div_cost<float, true> {
+  enum { value = 14 };
+};
+template <>
+struct scalar_div_cost<double, true> {
+  enum { value = 16 };
+};
+
+template <>
+struct unpacket_traits<Packet8f> {
+  typedef float type;
+  typedef Packet4f half;
+  typedef Packet8i integer_packet;
+  typedef uint8_t mask_t;
   enum {
-    Vectorizable = 1,
-    AlignedOnScalar = 1,
-    size=8
+    size = 8,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = true,
+    masked_store_available = true
+#ifdef EIGEN_VECTORIZE_AVX512
+    ,
+    masked_fpops_available = true
+#endif
   };
 };
-*/
-
-template<> struct unpacket_traits<Packet8f> {
-  typedef float     type;
-  typedef Packet4f  half;
-  typedef Packet8i  integer_packet;
-  typedef uint8_t   mask_t;
-  enum {size=8, alignment=Aligned32, vectorizable=true, masked_load_available=true, masked_store_available=true};
-};
-template<> struct unpacket_traits<Packet4d> {
+template <>
+struct unpacket_traits<Packet4d> {
   typedef double type;
   typedef Packet2d half;
-  enum {size=4, alignment=Aligned32, vectorizable=true, masked_load_available=false, masked_store_available=false};
+  enum {
+    size = 4,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
 };
-template<> struct unpacket_traits<Packet8i> { typedef int    type; typedef Packet4i half; enum {size=8, alignment=Aligned32, vectorizable=false, masked_load_available=false, masked_store_available=false}; };
-template<> struct unpacket_traits<Packet8bf> { typedef bfloat16 type; typedef Packet8bf half; enum {size=8, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false}; };
+template <>
+struct unpacket_traits<Packet8i> {
+  typedef int type;
+  typedef Packet4i half;
+  enum {
+    size = 8,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template <>
+struct unpacket_traits<Packet8ui> {
+  typedef uint32_t type;
+  typedef Packet4ui half;
+  enum {
+    size = 8,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+#ifdef EIGEN_VECTORIZE_AVX2
+template <>
+struct unpacket_traits<Packet4l> {
+  typedef int64_t type;
+  typedef Packet4l half;
+  enum {
+    size = 4,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template <>
+struct unpacket_traits<Packet4ul> {
+  typedef uint64_t type;
+  typedef Packet4ul half;
+  enum {
+    size = 4,
+    alignment = Aligned32,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+#endif
+template <>
+struct unpacket_traits<Packet8bf> {
+  typedef bfloat16 type;
+  typedef Packet8bf half;
+  enum {
+    size = 8,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
 
 // Helper function for bit packing snippet of low precision comparison.
 // It packs the flags from 16x16 to 8x16.
@@ -236,34 +410,434 @@
                          _mm256_extractf128_si256(_mm256_castps_si256(rf), 1));
 }
 
-
-template<> EIGEN_STRONG_INLINE Packet8f pset1<Packet8f>(const float&  from) { return _mm256_set1_ps(from); }
-template<> EIGEN_STRONG_INLINE Packet4d pset1<Packet4d>(const double& from) { return _mm256_set1_pd(from); }
-template<> EIGEN_STRONG_INLINE Packet8i pset1<Packet8i>(const int&    from) { return _mm256_set1_epi32(from); }
-
-template<> EIGEN_STRONG_INLINE Packet8f pset1frombits<Packet8f>(unsigned int from) { return _mm256_castsi256_ps(pset1<Packet8i>(from)); }
-template<> EIGEN_STRONG_INLINE Packet4d pset1frombits<Packet4d>(uint64_t from) { return _mm256_castsi256_pd(_mm256_set1_epi64x(from)); }
-
-template<> EIGEN_STRONG_INLINE Packet8f pzero(const Packet8f& /*a*/) { return _mm256_setzero_ps(); }
-template<> EIGEN_STRONG_INLINE Packet4d pzero(const Packet4d& /*a*/) { return _mm256_setzero_pd(); }
-template<> EIGEN_STRONG_INLINE Packet8i pzero(const Packet8i& /*a*/) { return _mm256_setzero_si256(); }
-
-
-template<> EIGEN_STRONG_INLINE Packet8f peven_mask(const Packet8f& /*a*/) { return _mm256_castsi256_ps(_mm256_set_epi32(0, -1, 0, -1, 0, -1, 0, -1)); }
-template<> EIGEN_STRONG_INLINE Packet8i peven_mask(const Packet8i& /*a*/) { return _mm256_set_epi32(0, -1, 0, -1, 0, -1, 0, -1); }
-template<> EIGEN_STRONG_INLINE Packet4d peven_mask(const Packet4d& /*a*/) { return _mm256_castsi256_pd(_mm256_set_epi32(0, 0, -1, -1, 0, 0, -1, -1)); }
-
-template<> EIGEN_STRONG_INLINE Packet8f pload1<Packet8f>(const float*  from) { return _mm256_broadcast_ss(from); }
-template<> EIGEN_STRONG_INLINE Packet4d pload1<Packet4d>(const double* from) { return _mm256_broadcast_sd(from); }
-
-template<> EIGEN_STRONG_INLINE Packet8f plset<Packet8f>(const float& a) { return _mm256_add_ps(_mm256_set1_ps(a), _mm256_set_ps(7.0,6.0,5.0,4.0,3.0,2.0,1.0,0.0)); }
-template<> EIGEN_STRONG_INLINE Packet4d plset<Packet4d>(const double& a) { return _mm256_add_pd(_mm256_set1_pd(a), _mm256_set_pd(3.0,2.0,1.0,0.0)); }
-
-template<> EIGEN_STRONG_INLINE Packet8f padd<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_add_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d padd<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_add_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8i padd<Packet8i>(const Packet8i& a, const Packet8i& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_add_epi32(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet4l pset1<Packet4l>(const int64_t& from) {
+  return _mm256_set1_epi64x(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pset1<Packet4ul>(const uint64_t& from) {
+  return _mm256_set1_epi64x(numext::bit_cast<uint64_t>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pzero(const Packet4l& /*a*/) {
+  return _mm256_setzero_si256();
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pzero(const Packet4ul& /*a*/) {
+  return _mm256_setzero_si256();
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l peven_mask(const Packet4l& /*a*/) {
+  return _mm256_set_epi64x(0ll, -1ll, 0ll, -1ll);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul peven_mask(const Packet4ul& /*a*/) {
+  return _mm256_set_epi64x(0ll, -1ll, 0ll, -1ll);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pload1<Packet4l>(const int64_t* from) {
+  return _mm256_set1_epi64x(*from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pload1<Packet4ul>(const uint64_t* from) {
+  return _mm256_set1_epi64x(*from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l padd<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  return _mm256_add_epi64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul padd<Packet4ul>(const Packet4ul& a, const Packet4ul& b) {
+  return _mm256_add_epi64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l plset<Packet4l>(const int64_t& a) {
+  return padd(pset1<Packet4l>(a), Packet4l(_mm256_set_epi64x(3ll, 2ll, 1ll, 0ll)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul plset<Packet4ul>(const uint64_t& a) {
+  return padd(pset1<Packet4ul>(a), Packet4ul(_mm256_set_epi64x(3ll, 2ll, 1ll, 0ll)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l psub<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  return _mm256_sub_epi64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul psub<Packet4ul>(const Packet4ul& a, const Packet4ul& b) {
+  return _mm256_sub_epi64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pnegate(const Packet4l& a) {
+  return psub(pzero(a), a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pconj(const Packet4l& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pcmp_le(const Packet4l& a, const Packet4l& b) {
+  return _mm256_xor_si256(_mm256_cmpgt_epi64(a, b), _mm256_set1_epi32(-1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pcmp_le(const Packet4ul& a, const Packet4ul& b) {
+  return (Packet4ul)pcmp_le((Packet4l)psub(a, pset1<Packet4ul>(0x8000000000000000UL)),
+                            (Packet4l)psub(b, pset1<Packet4ul>(0x8000000000000000UL)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pcmp_lt(const Packet4l& a, const Packet4l& b) {
+  return _mm256_cmpgt_epi64(b, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pcmp_lt(const Packet4ul& a, const Packet4ul& b) {
+  return (Packet4ul)pcmp_lt((Packet4l)psub(a, pset1<Packet4ul>(0x8000000000000000UL)),
+                            (Packet4l)psub(b, pset1<Packet4ul>(0x8000000000000000UL)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pcmp_eq(const Packet4l& a, const Packet4l& b) {
+  return _mm256_cmpeq_epi64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pcmp_eq(const Packet4ul& a, const Packet4ul& b) {
+  return _mm256_cmpeq_epi64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l ptrue<Packet4l>(const Packet4l& a) {
+  return _mm256_cmpeq_epi64(a, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul ptrue<Packet4ul>(const Packet4ul& a) {
+  return _mm256_cmpeq_epi64(a, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pand<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  return _mm256_and_si256(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l por<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  return _mm256_or_si256(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pxor<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  return _mm256_xor_si256(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pxor<Packet4ul>(const Packet4ul& a, const Packet4ul& b) {
+  return _mm256_xor_si256(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pandnot<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  return _mm256_andnot_si256(b, a);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4l plogical_shift_right(Packet4l a) {
+  return _mm256_srli_epi64(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4l plogical_shift_left(Packet4l a) {
+  return _mm256_slli_epi64(a, N);
+}
+#ifdef EIGEN_VECTORIZE_AVX512FP16
+template <int N>
+EIGEN_STRONG_INLINE Packet4l parithmetic_shift_right(Packet4l a) {
+  return _mm256_srai_epi64(a, N);
+}
+#else
+template <int N>
+EIGEN_STRONG_INLINE std::enable_if_t<(N == 0), Packet4l> parithmetic_shift_right(Packet4l a) {
+  return a;
+}
+template <int N>
+EIGEN_STRONG_INLINE std::enable_if_t<(N > 0) && (N < 32), Packet4l> parithmetic_shift_right(Packet4l a) {
+  __m256i hi_word = _mm256_srai_epi32(a, N);
+  __m256i lo_word = _mm256_srli_epi64(a, N);
+  return _mm256_blend_epi32(hi_word, lo_word, 0b01010101);
+}
+template <int N>
+EIGEN_STRONG_INLINE std::enable_if_t<(N >= 32) && (N < 63), Packet4l> parithmetic_shift_right(Packet4l a) {
+  __m256i hi_word = _mm256_srai_epi32(a, 31);
+  __m256i lo_word = _mm256_shuffle_epi32(_mm256_srai_epi32(a, N - 32), (shuffle_mask<1, 1, 3, 3>::mask));
+  return _mm256_blend_epi32(hi_word, lo_word, 0b01010101);
+}
+template <int N>
+EIGEN_STRONG_INLINE std::enable_if_t<(N == 63), Packet4l> parithmetic_shift_right(Packet4l a) {
+  return _mm256_shuffle_epi32(_mm256_srai_epi32(a, 31), (shuffle_mask<1, 1, 3, 3>::mask));
+}
+template <int N>
+EIGEN_STRONG_INLINE std::enable_if_t<(N < 0) || (N > 63), Packet4l> parithmetic_shift_right(Packet4l a) {
+  return parithmetic_shift_right<int(N & 63)>(a);
+}
+#endif
+template <>
+EIGEN_STRONG_INLINE Packet4l pload<Packet4l>(const int64_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_si256(reinterpret_cast<const __m256i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pload<Packet4ul>(const uint64_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_si256(reinterpret_cast<const __m256i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l ploadu<Packet4l>(const int64_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_si256(reinterpret_cast<const __m256i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul ploadu<Packet4ul>(const uint64_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_si256(reinterpret_cast<const __m256i*>(from));
+}
+// Loads 2 int64_ts from memory a returns the packet {a0, a0, a1, a1}
+template <>
+EIGEN_STRONG_INLINE Packet4l ploaddup<Packet4l>(const int64_t* from) {
+  const Packet4l a = _mm256_castsi128_si256(_mm_loadu_si128(reinterpret_cast<const __m128i*>(from)));
+  return _mm256_permutevar8x32_epi32(a, _mm256_setr_epi32(0, 1, 0, 1, 2, 3, 2, 3));
+}
+// Loads 2 uint64_ts from memory a returns the packet {a0, a0, a1, a1}
+template <>
+EIGEN_STRONG_INLINE Packet4ul ploaddup<Packet4ul>(const uint64_t* from) {
+  const Packet4ul a = _mm256_castsi128_si256(_mm_loadu_si128(reinterpret_cast<const __m128i*>(from)));
+  return _mm256_permutevar8x32_epi32(a, _mm256_setr_epi32(0, 1, 0, 1, 2, 3, 2, 3));
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int64_t>(int64_t* to, const Packet4l& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm256_store_si256(reinterpret_cast<__m256i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint64_t>(uint64_t* to, const Packet4ul& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm256_store_si256(reinterpret_cast<__m256i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int64_t>(int64_t* to, const Packet4l& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_si256(reinterpret_cast<__m256i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint64_t>(uint64_t* to, const Packet4ul& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_si256(reinterpret_cast<__m256i*>(to), from);
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet4l pgather<int64_t, Packet4l>(const int64_t* from, Index stride) {
+  return _mm256_set_epi64x(from[3 * stride], from[2 * stride], from[1 * stride], from[0 * stride]);
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet4ul pgather<uint64_t, Packet4ul>(const uint64_t* from, Index stride) {
+  return _mm256_set_epi64x(from[3 * stride], from[2 * stride], from[1 * stride], from[0 * stride]);
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<int64_t, Packet4l>(int64_t* to, const Packet4l& from, Index stride) {
+  __m128i low = _mm256_extractf128_si256(from, 0);
+  to[stride * 0] = _mm_extract_epi64(low, 0);
+  to[stride * 1] = _mm_extract_epi64(low, 1);
+
+  __m128i high = _mm256_extractf128_si256(from, 1);
+  to[stride * 2] = _mm_extract_epi64(high, 0);
+  to[stride * 3] = _mm_extract_epi64(high, 1);
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<uint64_t, Packet4ul>(uint64_t* to, const Packet4ul& from, Index stride) {
+  __m128i low = _mm256_extractf128_si256(from, 0);
+  to[stride * 0] = _mm_extract_epi64(low, 0);
+  to[stride * 1] = _mm_extract_epi64(low, 1);
+
+  __m128i high = _mm256_extractf128_si256(from, 1);
+  to[stride * 2] = _mm_extract_epi64(high, 0);
+  to[stride * 3] = _mm_extract_epi64(high, 1);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore1<Packet4l>(int64_t* to, const int64_t& a) {
+  Packet4l pa = pset1<Packet4l>(a);
+  pstore(to, pa);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore1<Packet4ul>(uint64_t* to, const uint64_t& a) {
+  Packet4ul pa = pset1<Packet4ul>(a);
+  pstore(to, pa);
+}
+template <>
+EIGEN_STRONG_INLINE int64_t pfirst<Packet4l>(const Packet4l& a) {
+  return _mm_cvtsi128_si64(_mm256_castsi256_si128(a));
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t pfirst<Packet4ul>(const Packet4ul& a) {
+  return _mm_cvtsi128_si64(_mm256_castsi256_si128(a));
+}
+template <>
+EIGEN_STRONG_INLINE int64_t predux<Packet4l>(const Packet4l& a) {
+  __m128i r = _mm_add_epi64(_mm256_castsi256_si128(a), _mm256_extractf128_si256(a, 1));
+  return _mm_extract_epi64(r, 0) + _mm_extract_epi64(r, 1);
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t predux<Packet4ul>(const Packet4ul& a) {
+  __m128i r = _mm_add_epi64(_mm256_castsi256_si128(a), _mm256_extractf128_si256(a, 1));
+  return numext::bit_cast<uint64_t>(_mm_extract_epi64(r, 0) + _mm_extract_epi64(r, 1));
+}
+#define MM256_SHUFFLE_EPI64(A, B, M) _mm256_shuffle_pd(_mm256_castsi256_pd(A), _mm256_castsi256_pd(B), M)
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet4l, 4>& kernel) {
+  __m256d T0 = MM256_SHUFFLE_EPI64(kernel.packet[0], kernel.packet[1], 15);
+  __m256d T1 = MM256_SHUFFLE_EPI64(kernel.packet[0], kernel.packet[1], 0);
+  __m256d T2 = MM256_SHUFFLE_EPI64(kernel.packet[2], kernel.packet[3], 15);
+  __m256d T3 = MM256_SHUFFLE_EPI64(kernel.packet[2], kernel.packet[3], 0);
+
+  kernel.packet[1] = _mm256_castpd_si256(_mm256_permute2f128_pd(T0, T2, 32));
+  kernel.packet[3] = _mm256_castpd_si256(_mm256_permute2f128_pd(T0, T2, 49));
+  kernel.packet[0] = _mm256_castpd_si256(_mm256_permute2f128_pd(T1, T3, 32));
+  kernel.packet[2] = _mm256_castpd_si256(_mm256_permute2f128_pd(T1, T3, 49));
+}
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet4ul, 4>& kernel) {
+  ptranspose((PacketBlock<Packet4l, 4>&)kernel);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pmin<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  __m256i cmp = _mm256_cmpgt_epi64(a, b);
+  __m256i a_min = _mm256_andnot_si256(cmp, a);
+  __m256i b_min = _mm256_and_si256(cmp, b);
+  return Packet4l(_mm256_or_si256(a_min, b_min));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pmin<Packet4ul>(const Packet4ul& a, const Packet4ul& b) {
+  return padd((Packet4ul)pmin((Packet4l)psub(a, pset1<Packet4ul>(0x8000000000000000UL)),
+                              (Packet4l)psub(b, pset1<Packet4ul>(0x8000000000000000UL))),
+              pset1<Packet4ul>(0x8000000000000000UL));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pmax<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  __m256i cmp = _mm256_cmpgt_epi64(a, b);
+  __m256i a_min = _mm256_and_si256(cmp, a);
+  __m256i b_min = _mm256_andnot_si256(cmp, b);
+  return Packet4l(_mm256_or_si256(a_min, b_min));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pmax<Packet4ul>(const Packet4ul& a, const Packet4ul& b) {
+  return padd((Packet4ul)pmax((Packet4l)psub(a, pset1<Packet4ul>(0x8000000000000000UL)),
+                              (Packet4l)psub(b, pset1<Packet4ul>(0x8000000000000000UL))),
+              pset1<Packet4ul>(0x8000000000000000UL));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pabs<Packet4l>(const Packet4l& a) {
+  Packet4l pz = pzero<Packet4l>(a);
+  Packet4l cmp = _mm256_cmpgt_epi64(a, pz);
+  return psub(cmp, pxor(a, cmp));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pabs<Packet4ul>(const Packet4ul& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4l pmul<Packet4l>(const Packet4l& a, const Packet4l& b) {
+  // 64-bit mul requires avx512, so do this with 32-bit multiplication
+  __m256i upper32_a = _mm256_srli_epi64(a, 32);
+  __m256i upper32_b = _mm256_srli_epi64(b, 32);
+
+  // upper * lower
+  __m256i mul1 = _mm256_mul_epu32(upper32_a, b);
+  __m256i mul2 = _mm256_mul_epu32(upper32_b, a);
+  // Gives us both upper*upper and lower*lower
+  __m256i mul3 = _mm256_mul_epu32(a, b);
+
+  __m256i high = _mm256_slli_epi64(_mm256_add_epi64(mul1, mul2), 32);
+  return _mm256_add_epi64(high, mul3);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul pmul<Packet4ul>(const Packet4ul& a, const Packet4ul& b) {
+  return (Packet4ul)pmul<Packet4l>((Packet4l)a, (Packet4l)b);
+}
+#endif
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pset1<Packet8f>(const float& from) {
+  return _mm256_set1_ps(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pset1<Packet4d>(const double& from) {
+  return _mm256_set1_pd(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pset1<Packet8i>(const int& from) {
+  return _mm256_set1_epi32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pset1<Packet8ui>(const uint32_t& from) {
+  return _mm256_set1_epi32(from);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pset1frombits<Packet8f>(unsigned int from) {
+  return _mm256_castsi256_ps(pset1<Packet8i>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pset1frombits<Packet4d>(uint64_t from) {
+  return _mm256_castsi256_pd(_mm256_set1_epi64x(from));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pzero(const Packet8f& /*a*/) {
+  return _mm256_setzero_ps();
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pzero(const Packet4d& /*a*/) {
+  return _mm256_setzero_pd();
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pzero(const Packet8i& /*a*/) {
+  return _mm256_setzero_si256();
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pzero(const Packet8ui& /*a*/) {
+  return _mm256_setzero_si256();
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f peven_mask(const Packet8f& /*a*/) {
+  return _mm256_castsi256_ps(_mm256_set_epi32(0, -1, 0, -1, 0, -1, 0, -1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i peven_mask(const Packet8i& /*a*/) {
+  return _mm256_set_epi32(0, -1, 0, -1, 0, -1, 0, -1);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui peven_mask(const Packet8ui& /*a*/) {
+  return _mm256_set_epi32(0, -1, 0, -1, 0, -1, 0, -1);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d peven_mask(const Packet4d& /*a*/) {
+  return _mm256_castsi256_pd(_mm256_set_epi32(0, 0, -1, -1, 0, 0, -1, -1));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pload1<Packet8f>(const float* from) {
+  return _mm256_broadcast_ss(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pload1<Packet4d>(const double* from) {
+  return _mm256_broadcast_sd(from);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f padd<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_add_ps(a, b);
+}
+#ifdef EIGEN_VECTORIZE_AVX512
+template <>
+EIGEN_STRONG_INLINE Packet8f padd<Packet8f>(const Packet8f& a, const Packet8f& b, uint8_t umask) {
+  __mmask16 mask = static_cast<__mmask16>(umask & 0x00FF);
+  return _mm512_castps512_ps256(_mm512_maskz_add_ps(mask, _mm512_castps256_ps512(a), _mm512_castps256_ps512(b)));
+}
+#endif
+template <>
+EIGEN_STRONG_INLINE Packet4d padd<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_add_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i padd<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_add_epi32(a, b);
+#else
+  __m128i lo = _mm_add_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_add_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui padd<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_add_epi32(a, b);
 #else
   __m128i lo = _mm_add_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
   __m128i hi = _mm_add_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
@@ -271,11 +845,45 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f psub<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_sub_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d psub<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_sub_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8i psub<Packet8i>(const Packet8i& a, const Packet8i& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8f plset<Packet8f>(const float& a) {
+  return padd(pset1<Packet8f>(a), _mm256_set_ps(7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d plset<Packet4d>(const double& a) {
+  return padd(pset1<Packet4d>(a), _mm256_set_pd(3.0, 2.0, 1.0, 0.0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i plset<Packet8i>(const int& a) {
+  return padd(pset1<Packet8i>(a), (Packet8i)_mm256_set_epi32(7, 6, 5, 4, 3, 2, 1, 0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui plset<Packet8ui>(const uint32_t& a) {
+  return padd(pset1<Packet8ui>(a), (Packet8ui)_mm256_set_epi32(7, 6, 5, 4, 3, 2, 1, 0));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f psub<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_sub_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d psub<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_sub_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i psub<Packet8i>(const Packet8i& a, const Packet8i& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_sub_epi32(a,b);
+  return _mm256_sub_epi32(a, b);
+#else
+  __m128i lo = _mm_sub_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_sub_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui psub<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_sub_epi32(a, b);
 #else
   __m128i lo = _mm_sub_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
   __m128i hi = _mm_sub_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
@@ -283,24 +891,56 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pnegate(const Packet8f& a)
-{
-  return _mm256_sub_ps(_mm256_set1_ps(0.0),a);
+template <>
+EIGEN_STRONG_INLINE Packet8f pnegate(const Packet8f& a) {
+  const Packet8f mask = _mm256_castsi256_ps(_mm256_set1_epi32(0x80000000));
+  return _mm256_xor_ps(a, mask);
 }
-template<> EIGEN_STRONG_INLINE Packet4d pnegate(const Packet4d& a)
-{
-  return _mm256_sub_pd(_mm256_set1_pd(0.0),a);
+template <>
+EIGEN_STRONG_INLINE Packet4d pnegate(const Packet4d& a) {
+  const Packet4d mask = _mm256_castsi256_pd(_mm256_set1_epi64x(0x8000000000000000ULL));
+  return _mm256_xor_pd(a, mask);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pnegate(const Packet8i& a) {
+  return psub(pzero(a), a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pconj(const Packet8f& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4d pconj(const Packet4d& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet8i pconj(const Packet8i& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet8f pconj(const Packet8f& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pconj(const Packet4d& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pconj(const Packet8i& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE Packet8f pmul<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_mul_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d pmul<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_mul_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8i pmul<Packet8i>(const Packet8i& a, const Packet8i& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8f pmul<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_mul_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pmul<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_mul_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pmul<Packet8i>(const Packet8i& a, const Packet8i& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_mullo_epi32(a,b);
+  return _mm256_mullo_epi32(a, b);
+#else
+  const __m128i lo = _mm_mullo_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  const __m128i hi = _mm_mullo_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pmul<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_mullo_epi32(a, b);
 #else
   const __m128i lo = _mm_mullo_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
   const __m128i hi = _mm_mullo_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
@@ -308,54 +948,142 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pdiv<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_div_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d pdiv<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_div_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8i pdiv<Packet8i>(const Packet8i& /*a*/, const Packet8i& /*b*/)
-{ eigen_assert(false && "packet integer division are not supported by AVX");
-  return pset1<Packet8i>(0);
+template <>
+EIGEN_STRONG_INLINE Packet8f pdiv<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_div_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pdiv<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_div_pd(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8i pdiv<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX512
+  return _mm512_cvttpd_epi32(_mm512_div_pd(_mm512_cvtepi32_pd(a), _mm512_cvtepi32_pd(b)));
+#else
+  Packet4i lo = pdiv<Packet4i>(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  Packet4i hi = pdiv<Packet4i>(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), hi, 1);
+#endif
 }
 
 #ifdef EIGEN_VECTORIZE_FMA
-template<> EIGEN_STRONG_INLINE Packet8f pmadd(const Packet8f& a, const Packet8f& b, const Packet8f& c) {
-#if ( (EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC<80) || (EIGEN_COMP_CLANG) )
-  // Clang stupidly generates a vfmadd213ps instruction plus some vmovaps on registers,
-  //  and even register spilling with clang>=6.0 (bug 1637).
-  // Gcc stupidly generates a vfmadd132ps instruction.
-  // So let's enforce it to generate a vfmadd231ps instruction since the most common use
-  //  case is to accumulate the result of the product.
-  Packet8f res = c;
-  __asm__("vfmadd231ps %[a], %[b], %[c]" : [c] "+x" (res) : [a] "x" (a), [b] "x" (b));
-  return res;
-#else
-  return _mm256_fmadd_ps(a,b,c);
-#endif
+template <>
+EIGEN_STRONG_INLINE Packet8f pmadd(const Packet8f& a, const Packet8f& b, const Packet8f& c) {
+  return _mm256_fmadd_ps(a, b, c);
 }
-template<> EIGEN_STRONG_INLINE Packet4d pmadd(const Packet4d& a, const Packet4d& b, const Packet4d& c) {
-#if ( (EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC<80) || (EIGEN_COMP_CLANG) )
-  // see above
-  Packet4d res = c;
-  __asm__("vfmadd231pd %[a], %[b], %[c]" : [c] "+x" (res) : [a] "x" (a), [b] "x" (b));
-  return res;
-#else
-  return _mm256_fmadd_pd(a,b,c);
-#endif
+template <>
+EIGEN_STRONG_INLINE Packet4d pmadd(const Packet4d& a, const Packet4d& b, const Packet4d& c) {
+  return _mm256_fmadd_pd(a, b, c);
 }
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pmsub(const Packet8f& a, const Packet8f& b, const Packet8f& c) {
+  return _mm256_fmsub_ps(a, b, c);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pmsub(const Packet4d& a, const Packet4d& b, const Packet4d& c) {
+  return _mm256_fmsub_pd(a, b, c);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pnmadd(const Packet8f& a, const Packet8f& b, const Packet8f& c) {
+  return _mm256_fnmadd_ps(a, b, c);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pnmadd(const Packet4d& a, const Packet4d& b, const Packet4d& c) {
+  return _mm256_fnmadd_pd(a, b, c);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pnmsub(const Packet8f& a, const Packet8f& b, const Packet8f& c) {
+  return _mm256_fnmsub_ps(a, b, c);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pnmsub(const Packet4d& a, const Packet4d& b, const Packet4d& c) {
+  return _mm256_fnmsub_pd(a, b, c);
+}
+
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet8f pcmp_le(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a,b,_CMP_LE_OQ); }
-template<> EIGEN_STRONG_INLINE Packet8f pcmp_lt(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a,b,_CMP_LT_OQ); }
-template<> EIGEN_STRONG_INLINE Packet8f pcmp_lt_or_nan(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a, b, _CMP_NGE_UQ); }
-template<> EIGEN_STRONG_INLINE Packet8f pcmp_eq(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a,b,_CMP_EQ_OQ); }
+template <>
+EIGEN_STRONG_INLINE Packet8f pcmp_le(const Packet8f& a, const Packet8f& b) {
+  return _mm256_cmp_ps(a, b, _CMP_LE_OQ);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8f pcmp_lt(const Packet8f& a, const Packet8f& b) {
+  return _mm256_cmp_ps(a, b, _CMP_LT_OQ);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8f pcmp_lt_or_nan(const Packet8f& a, const Packet8f& b) {
+  return _mm256_cmp_ps(a, b, _CMP_NGE_UQ);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8f pcmp_eq(const Packet8f& a, const Packet8f& b) {
+  return _mm256_cmp_ps(a, b, _CMP_EQ_OQ);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8f pisnan(const Packet8f& a) {
+  return _mm256_cmp_ps(a, a, _CMP_UNORD_Q);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4d pcmp_le(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a,b,_CMP_LE_OQ); }
-template<> EIGEN_STRONG_INLINE Packet4d pcmp_lt(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a,b,_CMP_LT_OQ); }
-template<> EIGEN_STRONG_INLINE Packet4d pcmp_lt_or_nan(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a, b, _CMP_NGE_UQ); }
-template<> EIGEN_STRONG_INLINE Packet4d pcmp_eq(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a,b,_CMP_EQ_OQ); }
+template <>
+EIGEN_STRONG_INLINE Packet4d pcmp_le(const Packet4d& a, const Packet4d& b) {
+  return _mm256_cmp_pd(a, b, _CMP_LE_OQ);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pcmp_lt(const Packet4d& a, const Packet4d& b) {
+  return _mm256_cmp_pd(a, b, _CMP_LT_OQ);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pcmp_lt_or_nan(const Packet4d& a, const Packet4d& b) {
+  return _mm256_cmp_pd(a, b, _CMP_NGE_UQ);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pcmp_eq(const Packet4d& a, const Packet4d& b) {
+  return _mm256_cmp_pd(a, b, _CMP_EQ_OQ);
+}
 
-
-template<> EIGEN_STRONG_INLINE Packet8i pcmp_eq(const Packet8i& a, const Packet8i& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8i pcmp_le(const Packet8i& a, const Packet8i& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_cmpeq_epi32(a,b);
+  return _mm256_xor_si256(_mm256_cmpgt_epi32(a, b), _mm256_set1_epi32(-1));
+#else
+  __m128i lo = _mm_cmpgt_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  lo = _mm_xor_si128(lo, _mm_set1_epi32(-1));
+  __m128i hi = _mm_cmpgt_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  hi = _mm_xor_si128(hi, _mm_set1_epi32(-1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pcmp_lt(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_cmpgt_epi32(b, a);
+#else
+  __m128i lo = _mm_cmpgt_epi32(_mm256_extractf128_si256(b, 0), _mm256_extractf128_si256(a, 0));
+  __m128i hi = _mm_cmpgt_epi32(_mm256_extractf128_si256(b, 1), _mm256_extractf128_si256(a, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pcmp_eq(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_cmpeq_epi32(a, b);
+#else
+  __m128i lo = _mm_cmpeq_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_cmpeq_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pcmp_eq(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_cmpeq_epi32(a, b);
 #else
   __m128i lo = _mm_cmpeq_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
   __m128i hi = _mm_cmpeq_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
@@ -363,188 +1091,344 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pmin<Packet8f>(const Packet8f& a, const Packet8f& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+template <>
+EIGEN_STRONG_INLINE Packet8f pmin<Packet8f>(const Packet8f& a, const Packet8f& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
   // There appears to be a bug in GCC, by which the optimizer may flip
   // the argument order in calls to _mm_min_ps/_mm_max_ps, so we have to
   // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
   // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
   Packet8f res;
-  asm("vminps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  asm("vminps %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
   return res;
 #else
   // Arguments are swapped to match NaN propagation behavior of std::min.
-  return _mm256_min_ps(b,a);
+  return _mm256_min_ps(b, a);
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet4d pmin<Packet4d>(const Packet4d& a, const Packet4d& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+template <>
+EIGEN_STRONG_INLINE Packet4d pmin<Packet4d>(const Packet4d& a, const Packet4d& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
   // See pmin above
   Packet4d res;
-  asm("vminpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  asm("vminpd %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
   return res;
 #else
   // Arguments are swapped to match NaN propagation behavior of std::min.
-  return _mm256_min_pd(b,a);
+  return _mm256_min_pd(b, a);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pmin<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_min_epi32(a, b);
+#else
+  __m128i lo = _mm_min_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_min_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pmin<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_min_epu32(a, b);
+#else
+  __m128i lo = _mm_min_epu32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_min_epu32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pmax<Packet8f>(const Packet8f& a, const Packet8f& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+template <>
+EIGEN_STRONG_INLINE Packet8f pmax<Packet8f>(const Packet8f& a, const Packet8f& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
   // See pmin above
   Packet8f res;
-  asm("vmaxps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  asm("vmaxps %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
   return res;
 #else
   // Arguments are swapped to match NaN propagation behavior of std::max.
-  return _mm256_max_ps(b,a);
+  return _mm256_max_ps(b, a);
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet4d pmax<Packet4d>(const Packet4d& a, const Packet4d& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+template <>
+EIGEN_STRONG_INLINE Packet4d pmax<Packet4d>(const Packet4d& a, const Packet4d& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
   // See pmin above
   Packet4d res;
-  asm("vmaxpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  asm("vmaxpd %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
   return res;
 #else
   // Arguments are swapped to match NaN propagation behavior of std::max.
-  return _mm256_max_pd(b,a);
+  return _mm256_max_pd(b, a);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pmax<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_max_epi32(a, b);
+#else
+  __m128i lo = _mm_max_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_max_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pmax<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_max_epu32(a, b);
+#else
+  __m128i lo = _mm_max_epu32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_max_epu32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
 #endif
 }
 
+#ifdef EIGEN_VECTORIZE_AVX2
+template <>
+EIGEN_STRONG_INLINE Packet8i psign(const Packet8i& a) {
+  return _mm256_sign_epi32(_mm256_set1_epi32(1), a);
+}
+#endif
+
 // Add specializations for min/max with prescribed NaN progation.
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet8f pmin<PropagateNumbers, Packet8f>(const Packet8f& a, const Packet8f& b) {
   return pminmax_propagate_numbers(a, b, pmin<Packet8f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4d pmin<PropagateNumbers, Packet4d>(const Packet4d& a, const Packet4d& b) {
   return pminmax_propagate_numbers(a, b, pmin<Packet4d>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet8f pmax<PropagateNumbers, Packet8f>(const Packet8f& a, const Packet8f& b) {
   return pminmax_propagate_numbers(a, b, pmax<Packet8f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4d pmax<PropagateNumbers, Packet4d>(const Packet4d& a, const Packet4d& b) {
   return pminmax_propagate_numbers(a, b, pmax<Packet4d>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet8f pmin<PropagateNaN, Packet8f>(const Packet8f& a, const Packet8f& b) {
   return pminmax_propagate_nan(a, b, pmin<Packet8f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4d pmin<PropagateNaN, Packet4d>(const Packet4d& a, const Packet4d& b) {
   return pminmax_propagate_nan(a, b, pmin<Packet4d>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet8f pmax<PropagateNaN, Packet8f>(const Packet8f& a, const Packet8f& b) {
   return pminmax_propagate_nan(a, b, pmax<Packet8f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4d pmax<PropagateNaN, Packet4d>(const Packet4d& a, const Packet4d& b) {
   return pminmax_propagate_nan(a, b, pmax<Packet4d>);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f print<Packet8f>(const Packet8f& a) { return _mm256_round_ps(a, _MM_FROUND_CUR_DIRECTION); }
-template<> EIGEN_STRONG_INLINE Packet4d print<Packet4d>(const Packet4d& a) { return _mm256_round_pd(a, _MM_FROUND_CUR_DIRECTION); }
+template <>
+EIGEN_STRONG_INLINE Packet8f print<Packet8f>(const Packet8f& a) {
+  return _mm256_round_ps(a, _MM_FROUND_CUR_DIRECTION);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d print<Packet4d>(const Packet4d& a) {
+  return _mm256_round_pd(a, _MM_FROUND_CUR_DIRECTION);
+}
 
-template<> EIGEN_STRONG_INLINE Packet8f pceil<Packet8f>(const Packet8f& a) { return _mm256_ceil_ps(a); }
-template<> EIGEN_STRONG_INLINE Packet4d pceil<Packet4d>(const Packet4d& a) { return _mm256_ceil_pd(a); }
+template <>
+EIGEN_STRONG_INLINE Packet8f pceil<Packet8f>(const Packet8f& a) {
+  return _mm256_ceil_ps(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pceil<Packet4d>(const Packet4d& a) {
+  return _mm256_ceil_pd(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet8f pfloor<Packet8f>(const Packet8f& a) { return _mm256_floor_ps(a); }
-template<> EIGEN_STRONG_INLINE Packet4d pfloor<Packet4d>(const Packet4d& a) { return _mm256_floor_pd(a); }
+template <>
+EIGEN_STRONG_INLINE Packet8f pfloor<Packet8f>(const Packet8f& a) {
+  return _mm256_floor_ps(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pfloor<Packet4d>(const Packet4d& a) {
+  return _mm256_floor_pd(a);
+}
 
-
-template<> EIGEN_STRONG_INLINE Packet8i ptrue<Packet8i>(const Packet8i& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8i ptrue<Packet8i>(const Packet8i& a) {
 #ifdef EIGEN_VECTORIZE_AVX2
   // vpcmpeqd has lower latency than the more general vcmpps
-  return _mm256_cmpeq_epi32(a,a);
+  return _mm256_cmpeq_epi32(a, a);
 #else
   const __m256 b = _mm256_castsi256_ps(a);
-  return _mm256_castps_si256(_mm256_cmp_ps(b,b,_CMP_TRUE_UQ));
+  return _mm256_castps_si256(_mm256_cmp_ps(b, b, _CMP_TRUE_UQ));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f ptrue<Packet8f>(const Packet8f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8f ptrue<Packet8f>(const Packet8f& a) {
 #ifdef EIGEN_VECTORIZE_AVX2
   // vpcmpeqd has lower latency than the more general vcmpps
   const __m256i b = _mm256_castps_si256(a);
-  return _mm256_castsi256_ps(_mm256_cmpeq_epi32(b,b));
+  return _mm256_castsi256_ps(_mm256_cmpeq_epi32(b, b));
 #else
-  return _mm256_cmp_ps(a,a,_CMP_TRUE_UQ);
+  return _mm256_cmp_ps(a, a, _CMP_TRUE_UQ);
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet4d ptrue<Packet4d>(const Packet4d& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4d ptrue<Packet4d>(const Packet4d& a) {
 #ifdef EIGEN_VECTORIZE_AVX2
   // vpcmpeqq has lower latency than the more general vcmppd
   const __m256i b = _mm256_castpd_si256(a);
-  return _mm256_castsi256_pd(_mm256_cmpeq_epi64(b,b));
+  return _mm256_castsi256_pd(_mm256_cmpeq_epi64(b, b));
 #else
-  return _mm256_cmp_pd(a,a,_CMP_TRUE_UQ);
+  return _mm256_cmp_pd(a, a, _CMP_TRUE_UQ);
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pand<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_and_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d pand<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_and_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8i pand<Packet8i>(const Packet8i& a, const Packet8i& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8f pand<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_and_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pand<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_and_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pand<Packet8i>(const Packet8i& a, const Packet8i& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_and_si256(a,b);
+  return _mm256_and_si256(a, b);
 #else
-  return _mm256_castps_si256(_mm256_and_ps(_mm256_castsi256_ps(a),_mm256_castsi256_ps(b)));
+  return _mm256_castps_si256(_mm256_and_ps(_mm256_castsi256_ps(a), _mm256_castsi256_ps(b)));
 #endif
 }
-
-template<> EIGEN_STRONG_INLINE Packet8f por<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_or_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d por<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_or_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8i por<Packet8i>(const Packet8i& a, const Packet8i& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8ui pand<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_or_si256(a,b);
+  return _mm256_and_si256(a, b);
 #else
-  return _mm256_castps_si256(_mm256_or_ps(_mm256_castsi256_ps(a),_mm256_castsi256_ps(b)));
+  return _mm256_castps_si256(_mm256_and_ps(_mm256_castsi256_ps(a), _mm256_castsi256_ps(b)));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pxor<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_xor_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d pxor<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_xor_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8i pxor<Packet8i>(const Packet8i& a, const Packet8i& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8f por<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_or_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d por<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_or_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i por<Packet8i>(const Packet8i& a, const Packet8i& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_xor_si256(a,b);
+  return _mm256_or_si256(a, b);
 #else
-  return _mm256_castps_si256(_mm256_xor_ps(_mm256_castsi256_ps(a),_mm256_castsi256_ps(b)));
+  return _mm256_castps_si256(_mm256_or_ps(_mm256_castsi256_ps(a), _mm256_castsi256_ps(b)));
 #endif
 }
-
-template<> EIGEN_STRONG_INLINE Packet8f pandnot<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_andnot_ps(b,a); }
-template<> EIGEN_STRONG_INLINE Packet4d pandnot<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_andnot_pd(b,a); }
-template<> EIGEN_STRONG_INLINE Packet8i pandnot<Packet8i>(const Packet8i& a, const Packet8i& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8ui por<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
 #ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_andnot_si256(b,a);
+  return _mm256_or_si256(a, b);
 #else
-  return _mm256_castps_si256(_mm256_andnot_ps(_mm256_castsi256_ps(b),_mm256_castsi256_ps(a)));
+  return _mm256_castps_si256(_mm256_or_ps(_mm256_castsi256_ps(a), _mm256_castsi256_ps(b)));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pround<Packet8f>(const Packet8f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8f pxor<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_xor_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pxor<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_xor_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pxor<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_xor_si256(a, b);
+#else
+  return _mm256_castps_si256(_mm256_xor_ps(_mm256_castsi256_ps(a), _mm256_castsi256_ps(b)));
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pxor<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_xor_si256(a, b);
+#else
+  return _mm256_castps_si256(_mm256_xor_ps(_mm256_castsi256_ps(a), _mm256_castsi256_ps(b)));
+#endif
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pandnot<Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return _mm256_andnot_ps(b, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pandnot<Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_andnot_pd(b, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pandnot<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_andnot_si256(b, a);
+#else
+  return _mm256_castps_si256(_mm256_andnot_ps(_mm256_castsi256_ps(b), _mm256_castsi256_ps(a)));
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pandnot<Packet8ui>(const Packet8ui& a, const Packet8ui& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_andnot_si256(b, a);
+#else
+  return _mm256_castps_si256(_mm256_andnot_ps(_mm256_castsi256_ps(b), _mm256_castsi256_ps(a)));
+#endif
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8ui pcmp_lt(const Packet8ui& a, const Packet8ui& b) {
+  return pxor(pcmp_eq(a, pmax(a, b)), ptrue(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pcmp_le(const Packet8ui& a, const Packet8ui& b) {
+  return pcmp_eq(a, pmin(a, b));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pround<Packet8f>(const Packet8f& a) {
   const Packet8f mask = pset1frombits<Packet8f>(static_cast<numext::uint32_t>(0x80000000u));
   const Packet8f prev0dot5 = pset1frombits<Packet8f>(static_cast<numext::uint32_t>(0x3EFFFFFFu));
   return _mm256_round_ps(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
 }
-template<> EIGEN_STRONG_INLINE Packet4d pround<Packet4d>(const Packet4d& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4d pround<Packet4d>(const Packet4d& a) {
   const Packet4d mask = pset1frombits<Packet4d>(static_cast<numext::uint64_t>(0x8000000000000000ull));
   const Packet4d prev0dot5 = pset1frombits<Packet4d>(static_cast<numext::uint64_t>(0x3FDFFFFFFFFFFFFFull));
   return _mm256_round_pd(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pselect<Packet8f>(const Packet8f& mask, const Packet8f& a, const Packet8f& b)
-{ return _mm256_blendv_ps(b,a,mask); }
-template<> EIGEN_STRONG_INLINE Packet4d pselect<Packet4d>(const Packet4d& mask, const Packet4d& a, const Packet4d& b)
-{ return _mm256_blendv_pd(b,a,mask); }
+template <>
+EIGEN_STRONG_INLINE Packet8f pselect<Packet8f>(const Packet8f& mask, const Packet8f& a, const Packet8f& b) {
+  return _mm256_blendv_ps(b, a, mask);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pselect<Packet8i>(const Packet8i& mask, const Packet8i& a, const Packet8i& b) {
+  return _mm256_castps_si256(
+      _mm256_blendv_ps(_mm256_castsi256_ps(b), _mm256_castsi256_ps(a), _mm256_castsi256_ps(mask)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pselect<Packet8ui>(const Packet8ui& mask, const Packet8ui& a, const Packet8ui& b) {
+  return _mm256_castps_si256(
+      _mm256_blendv_ps(_mm256_castsi256_ps(b), _mm256_castsi256_ps(a), _mm256_castsi256_ps(mask)));
+}
 
-template<int N> EIGEN_STRONG_INLINE Packet8i parithmetic_shift_right(Packet8i a) {
+template <>
+EIGEN_STRONG_INLINE Packet4d pselect<Packet4d>(const Packet4d& mask, const Packet4d& a, const Packet4d& b) {
+  return _mm256_blendv_pd(b, a, mask);
+}
+
+template <int N>
+EIGEN_STRONG_INLINE Packet8i parithmetic_shift_right(Packet8i a) {
 #ifdef EIGEN_VECTORIZE_AVX2
   return _mm256_srai_epi32(a, N);
 #else
@@ -554,7 +1438,8 @@
 #endif
 }
 
-template<int N> EIGEN_STRONG_INLINE Packet8i plogical_shift_right(Packet8i a) {
+template <int N>
+EIGEN_STRONG_INLINE Packet8i plogical_shift_right(Packet8i a) {
 #ifdef EIGEN_VECTORIZE_AVX2
   return _mm256_srli_epi32(a, N);
 #else
@@ -564,7 +1449,8 @@
 #endif
 }
 
-template<int N> EIGEN_STRONG_INLINE Packet8i plogical_shift_left(Packet8i a) {
+template <int N>
+EIGEN_STRONG_INLINE Packet8i plogical_shift_left(Packet8i a) {
 #ifdef EIGEN_VECTORIZE_AVX2
   return _mm256_slli_epi32(a, N);
 #else
@@ -574,174 +1460,410 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pload<Packet8f>(const float*   from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_ps(from); }
-template<> EIGEN_STRONG_INLINE Packet4d pload<Packet4d>(const double*  from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_pd(from); }
-template<> EIGEN_STRONG_INLINE Packet8i pload<Packet8i>(const int*     from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_si256(reinterpret_cast<const __m256i*>(from)); }
+template <int N>
+EIGEN_STRONG_INLINE Packet8ui parithmetic_shift_right(Packet8ui a) {
+  return (Packet8ui)plogical_shift_right<N>((Packet8i)a);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8ui plogical_shift_right(Packet8ui a) {
+  return (Packet8ui)plogical_shift_right<N>((Packet8i)a);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8ui plogical_shift_left(Packet8ui a) {
+  return (Packet8ui)plogical_shift_left<N>((Packet8i)a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet8f ploadu<Packet8f>(const float* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_ps(from); }
-template<> EIGEN_STRONG_INLINE Packet4d ploadu<Packet4d>(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_pd(from); }
-template<> EIGEN_STRONG_INLINE Packet8i ploadu<Packet8i>(const int* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_si256(reinterpret_cast<const __m256i*>(from)); }
+template <>
+EIGEN_STRONG_INLINE Packet8f pload<Packet8f>(const float* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_ps(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d pload<Packet4d>(const double* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_pd(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pload<Packet8i>(const int* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_si256(reinterpret_cast<const __m256i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pload<Packet8ui>(const uint32_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_si256(reinterpret_cast<const __m256i*>(from));
+}
 
-template<> EIGEN_STRONG_INLINE Packet8f ploadu<Packet8f>(const float* from, uint8_t umask) {
+template <>
+EIGEN_STRONG_INLINE Packet8f ploadu<Packet8f>(const float* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_ps(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4d ploadu<Packet4d>(const double* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_pd(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i ploadu<Packet8i>(const int* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_si256(reinterpret_cast<const __m256i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui ploadu<Packet8ui>(const uint32_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_si256(reinterpret_cast<const __m256i*>(from));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f ploadu<Packet8f>(const float* from, uint8_t umask) {
+#ifdef EIGEN_VECTORIZE_AVX512
+  __mmask16 mask = static_cast<__mmask16>(umask & 0x00FF);
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm512_castps512_ps256(_mm512_maskz_loadu_ps(mask, from));
+#else
   Packet8i mask = _mm256_set1_epi8(static_cast<char>(umask));
-  const Packet8i bit_mask = _mm256_set_epi32(0xffffff7f, 0xffffffbf, 0xffffffdf, 0xffffffef, 0xfffffff7, 0xfffffffb, 0xfffffffd, 0xfffffffe);
+  const Packet8i bit_mask =
+      _mm256_set_epi32(0xffffff7f, 0xffffffbf, 0xffffffdf, 0xffffffef, 0xfffffff7, 0xfffffffb, 0xfffffffd, 0xfffffffe);
   mask = por<Packet8i>(mask, bit_mask);
   mask = pcmp_eq<Packet8i>(mask, _mm256_set1_epi32(0xffffffff));
   EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_maskload_ps(from, mask);
+#endif
 }
 
 // Loads 4 floats from memory a returns the packet {a0, a0  a1, a1, a2, a2, a3, a3}
-template<> EIGEN_STRONG_INLINE Packet8f ploaddup<Packet8f>(const float* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8f ploaddup<Packet8f>(const float* from) {
   // TODO try to find a way to avoid the need of a temporary register
-//   Packet8f tmp  = _mm256_castps128_ps256(_mm_loadu_ps(from));
-//   tmp = _mm256_insertf128_ps(tmp, _mm_movehl_ps(_mm256_castps256_ps128(tmp),_mm256_castps256_ps128(tmp)), 1);
-//   return _mm256_unpacklo_ps(tmp,tmp);
+  //   Packet8f tmp  = _mm256_castps128_ps256(_mm_loadu_ps(from));
+  //   tmp = _mm256_insertf128_ps(tmp, _mm_movehl_ps(_mm256_castps256_ps128(tmp),_mm256_castps256_ps128(tmp)), 1);
+  //   return _mm256_unpacklo_ps(tmp,tmp);
 
   // _mm256_insertf128_ps is very slow on Haswell, thus:
   Packet8f tmp = _mm256_broadcast_ps((const __m128*)(const void*)from);
   // mimic an "inplace" permutation of the lower 128bits using a blend
-  tmp = _mm256_blend_ps(tmp,_mm256_castps128_ps256(_mm_permute_ps( _mm256_castps256_ps128(tmp), _MM_SHUFFLE(1,0,1,0))), 15);
+  tmp = _mm256_blend_ps(
+      tmp, _mm256_castps128_ps256(_mm_permute_ps(_mm256_castps256_ps128(tmp), _MM_SHUFFLE(1, 0, 1, 0))), 15);
   // then we can perform a consistent permutation on the global register to get everything in shape:
-  return  _mm256_permute_ps(tmp, _MM_SHUFFLE(3,3,2,2));
+  return _mm256_permute_ps(tmp, _MM_SHUFFLE(3, 3, 2, 2));
 }
-// Loads 2 doubles from memory a returns the packet {a0, a0  a1, a1}
-template<> EIGEN_STRONG_INLINE Packet4d ploaddup<Packet4d>(const double* from)
-{
+// Loads 2 doubles from memory a returns the packet {a0, a0, a1, a1}
+template <>
+EIGEN_STRONG_INLINE Packet4d ploaddup<Packet4d>(const double* from) {
   Packet4d tmp = _mm256_broadcast_pd((const __m128d*)(const void*)from);
-  return  _mm256_permute_pd(tmp, 3<<2);
+  return _mm256_permute_pd(tmp, 3 << 2);
+}
+// Loads 4 integers from memory a returns the packet {a0, a0, a1, a1, a2, a2, a3, a3}
+template <>
+EIGEN_STRONG_INLINE Packet8i ploaddup<Packet8i>(const int* from) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  const Packet8i a = _mm256_castsi128_si256(ploadu<Packet4i>(from));
+  return _mm256_permutevar8x32_epi32(a, _mm256_setr_epi32(0, 0, 1, 1, 2, 2, 3, 3));
+#else
+  __m256 tmp = _mm256_broadcast_ps((const __m128*)(const void*)from);
+  // mimic an "inplace" permutation of the lower 128bits using a blend
+  tmp = _mm256_blend_ps(
+      tmp, _mm256_castps128_ps256(_mm_permute_ps(_mm256_castps256_ps128(tmp), _MM_SHUFFLE(1, 0, 1, 0))), 15);
+  // then we can perform a consistent permutation on the global register to get everything in shape:
+  return _mm256_castps_si256(_mm256_permute_ps(tmp, _MM_SHUFFLE(3, 3, 2, 2)));
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui ploaddup<Packet8ui>(const uint32_t* from) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  const Packet8ui a = _mm256_castsi128_si256(ploadu<Packet4ui>(from));
+  return _mm256_permutevar8x32_epi32(a, _mm256_setr_epi32(0, 0, 1, 1, 2, 2, 3, 3));
+#else
+  __m256 tmp = _mm256_broadcast_ps((const __m128*)(const void*)from);
+  // mimic an "inplace" permutation of the lower 128bits using a blend
+  tmp = _mm256_blend_ps(
+      tmp, _mm256_castps128_ps256(_mm_permute_ps(_mm256_castps256_ps128(tmp), _MM_SHUFFLE(1, 0, 1, 0))), 15);
+  // then we can perform a consistent permutation on the global register to get
+  // everything in shape:
+  return _mm256_castps_si256(_mm256_permute_ps(tmp, _MM_SHUFFLE(3, 3, 2, 2)));
+#endif
 }
 
 // Loads 2 floats from memory a returns the packet {a0, a0  a0, a0, a1, a1, a1, a1}
-template<> EIGEN_STRONG_INLINE Packet8f ploadquad<Packet8f>(const float* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8f ploadquad<Packet8f>(const float* from) {
   Packet8f tmp = _mm256_castps128_ps256(_mm_broadcast_ss(from));
-  return _mm256_insertf128_ps(tmp, _mm_broadcast_ss(from+1), 1);
+  return _mm256_insertf128_ps(tmp, _mm_broadcast_ss(from + 1), 1);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i ploadquad<Packet8i>(const int* from) {
+  return _mm256_insertf128_si256(_mm256_set1_epi32(*from), _mm_set1_epi32(*(from + 1)), 1);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui ploadquad<Packet8ui>(const uint32_t* from) {
+  return _mm256_insertf128_si256(_mm256_set1_epi32(*from), _mm_set1_epi32(*(from + 1)), 1);
 }
 
-template<> EIGEN_STRONG_INLINE void pstore<float>(float*   to, const Packet8f& from) { EIGEN_DEBUG_ALIGNED_STORE _mm256_store_ps(to, from); }
-template<> EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet4d& from) { EIGEN_DEBUG_ALIGNED_STORE _mm256_store_pd(to, from); }
-template<> EIGEN_STRONG_INLINE void pstore<int>(int*       to, const Packet8i& from) { EIGEN_DEBUG_ALIGNED_STORE _mm256_storeu_si256(reinterpret_cast<__m256i*>(to), from); }
+template <>
+EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet8f& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm256_store_ps(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet4d& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm256_store_pd(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int>(int* to, const Packet8i& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm256_store_si256(reinterpret_cast<__m256i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet8ui& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm256_store_si256(reinterpret_cast<__m256i*>(to), from);
+}
 
-template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*   to, const Packet8f& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_ps(to, from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet4d& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_pd(to, from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int>(int*       to, const Packet8i& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_si256(reinterpret_cast<__m256i*>(to), from); }
+template <>
+EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet8f& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_ps(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet4d& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_pd(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int>(int* to, const Packet8i& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_si256(reinterpret_cast<__m256i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet8ui& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_si256(reinterpret_cast<__m256i*>(to), from);
+}
 
-template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*   to, const Packet8f& from, uint8_t umask) {
+template <>
+EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet8f& from, uint8_t umask) {
+#ifdef EIGEN_VECTORIZE_AVX512
+  __mmask16 mask = static_cast<__mmask16>(umask & 0x00FF);
+  EIGEN_DEBUG_UNALIGNED_STORE _mm512_mask_storeu_ps(to, mask, _mm512_castps256_ps512(from));
+#else
   Packet8i mask = _mm256_set1_epi8(static_cast<char>(umask));
-  const Packet8i bit_mask = _mm256_set_epi32(0xffffff7f, 0xffffffbf, 0xffffffdf, 0xffffffef, 0xfffffff7, 0xfffffffb, 0xfffffffd, 0xfffffffe);
+  const Packet8i bit_mask =
+      _mm256_set_epi32(0x7f7f7f7f, 0xbfbfbfbf, 0xdfdfdfdf, 0xefefefef, 0xf7f7f7f7, 0xfbfbfbfb, 0xfdfdfdfd, 0xfefefefe);
   mask = por<Packet8i>(mask, bit_mask);
   mask = pcmp_eq<Packet8i>(mask, _mm256_set1_epi32(0xffffffff));
-  EIGEN_DEBUG_UNALIGNED_STORE return _mm256_maskstore_ps(to, mask, from);
+#if EIGEN_COMP_MSVC
+  // MSVC sometimes seems to use a bogus mask with maskstore.
+  const __m256i ifrom = _mm256_castps_si256(from);
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_maskmoveu_si128(_mm256_extractf128_si256(ifrom, 0), _mm256_extractf128_si256(mask, 0),
+                                                  reinterpret_cast<char*>(to));
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_maskmoveu_si128(_mm256_extractf128_si256(ifrom, 1), _mm256_extractf128_si256(mask, 1),
+                                                  reinterpret_cast<char*>(to + 4));
+#else
+  EIGEN_DEBUG_UNALIGNED_STORE _mm256_maskstore_ps(to, mask, from);
+#endif
+#endif
 }
 
 // NOTE: leverage _mm256_i32gather_ps and _mm256_i32gather_pd if AVX2 instructions are available
-// NOTE: for the record the following seems to be slower: return _mm256_i32gather_ps(from, _mm256_set1_epi32(stride), 4);
-template<> EIGEN_DEVICE_FUNC inline Packet8f pgather<float, Packet8f>(const float* from, Index stride)
-{
-  return _mm256_set_ps(from[7*stride], from[6*stride], from[5*stride], from[4*stride],
-                       from[3*stride], from[2*stride], from[1*stride], from[0*stride]);
+// NOTE: for the record the following seems to be slower: return _mm256_i32gather_ps(from, _mm256_set1_epi32(stride),
+// 4);
+template <>
+EIGEN_DEVICE_FUNC inline Packet8f pgather<float, Packet8f>(const float* from, Index stride) {
+  return _mm256_set_ps(from[7 * stride], from[6 * stride], from[5 * stride], from[4 * stride], from[3 * stride],
+                       from[2 * stride], from[1 * stride], from[0 * stride]);
 }
-template<> EIGEN_DEVICE_FUNC inline Packet4d pgather<double, Packet4d>(const double* from, Index stride)
-{
-  return _mm256_set_pd(from[3*stride], from[2*stride], from[1*stride], from[0*stride]);
+template <>
+EIGEN_DEVICE_FUNC inline Packet4d pgather<double, Packet4d>(const double* from, Index stride) {
+  return _mm256_set_pd(from[3 * stride], from[2 * stride], from[1 * stride], from[0 * stride]);
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet8i pgather<int, Packet8i>(const int* from, Index stride) {
+  return _mm256_set_epi32(from[7 * stride], from[6 * stride], from[5 * stride], from[4 * stride], from[3 * stride],
+                          from[2 * stride], from[1 * stride], from[0 * stride]);
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet8ui pgather<uint32_t, Packet8ui>(const uint32_t* from, Index stride) {
+  return (Packet8ui)pgather<int, Packet8i>((int*)from, stride);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<float, Packet8f>(float* to, const Packet8f& from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<float, Packet8f>(float* to, const Packet8f& from, Index stride) {
   __m128 low = _mm256_extractf128_ps(from, 0);
-  to[stride*0] = _mm_cvtss_f32(low);
-  to[stride*1] = _mm_cvtss_f32(_mm_shuffle_ps(low, low, 1));
-  to[stride*2] = _mm_cvtss_f32(_mm_shuffle_ps(low, low, 2));
-  to[stride*3] = _mm_cvtss_f32(_mm_shuffle_ps(low, low, 3));
+  to[stride * 0] = _mm_cvtss_f32(low);
+  to[stride * 1] = _mm_cvtss_f32(_mm_shuffle_ps(low, low, 1));
+  to[stride * 2] = _mm_cvtss_f32(_mm_shuffle_ps(low, low, 2));
+  to[stride * 3] = _mm_cvtss_f32(_mm_shuffle_ps(low, low, 3));
 
   __m128 high = _mm256_extractf128_ps(from, 1);
-  to[stride*4] = _mm_cvtss_f32(high);
-  to[stride*5] = _mm_cvtss_f32(_mm_shuffle_ps(high, high, 1));
-  to[stride*6] = _mm_cvtss_f32(_mm_shuffle_ps(high, high, 2));
-  to[stride*7] = _mm_cvtss_f32(_mm_shuffle_ps(high, high, 3));
+  to[stride * 4] = _mm_cvtss_f32(high);
+  to[stride * 5] = _mm_cvtss_f32(_mm_shuffle_ps(high, high, 1));
+  to[stride * 6] = _mm_cvtss_f32(_mm_shuffle_ps(high, high, 2));
+  to[stride * 7] = _mm_cvtss_f32(_mm_shuffle_ps(high, high, 3));
 }
-template<> EIGEN_DEVICE_FUNC inline void pscatter<double, Packet4d>(double* to, const Packet4d& from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<double, Packet4d>(double* to, const Packet4d& from, Index stride) {
   __m128d low = _mm256_extractf128_pd(from, 0);
-  to[stride*0] = _mm_cvtsd_f64(low);
-  to[stride*1] = _mm_cvtsd_f64(_mm_shuffle_pd(low, low, 1));
+  to[stride * 0] = _mm_cvtsd_f64(low);
+  to[stride * 1] = _mm_cvtsd_f64(_mm_shuffle_pd(low, low, 1));
   __m128d high = _mm256_extractf128_pd(from, 1);
-  to[stride*2] = _mm_cvtsd_f64(high);
-  to[stride*3] = _mm_cvtsd_f64(_mm_shuffle_pd(high, high, 1));
+  to[stride * 2] = _mm_cvtsd_f64(high);
+  to[stride * 3] = _mm_cvtsd_f64(_mm_shuffle_pd(high, high, 1));
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<int, Packet8i>(int* to, const Packet8i& from, Index stride) {
+  __m128i low = _mm256_extractf128_si256(from, 0);
+  to[stride * 0] = _mm_extract_epi32(low, 0);
+  to[stride * 1] = _mm_extract_epi32(low, 1);
+  to[stride * 2] = _mm_extract_epi32(low, 2);
+  to[stride * 3] = _mm_extract_epi32(low, 3);
+
+  __m128i high = _mm256_extractf128_si256(from, 1);
+  to[stride * 4] = _mm_extract_epi32(high, 0);
+  to[stride * 5] = _mm_extract_epi32(high, 1);
+  to[stride * 6] = _mm_extract_epi32(high, 2);
+  to[stride * 7] = _mm_extract_epi32(high, 3);
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<uint32_t, Packet8ui>(uint32_t* to, const Packet8ui& from, Index stride) {
+  pscatter<int, Packet8i>((int*)to, (Packet8i)from, stride);
 }
 
-template<> EIGEN_STRONG_INLINE void pstore1<Packet8f>(float* to, const float& a)
-{
+template <>
+EIGEN_STRONG_INLINE void pstore1<Packet8f>(float* to, const float& a) {
   Packet8f pa = pset1<Packet8f>(a);
   pstore(to, pa);
 }
-template<> EIGEN_STRONG_INLINE void pstore1<Packet4d>(double* to, const double& a)
-{
+template <>
+EIGEN_STRONG_INLINE void pstore1<Packet4d>(double* to, const double& a) {
   Packet4d pa = pset1<Packet4d>(a);
   pstore(to, pa);
 }
-template<> EIGEN_STRONG_INLINE void pstore1<Packet8i>(int* to, const int& a)
-{
+template <>
+EIGEN_STRONG_INLINE void pstore1<Packet8i>(int* to, const int& a) {
   Packet8i pa = pset1<Packet8i>(a);
   pstore(to, pa);
 }
 
 #ifndef EIGEN_VECTORIZE_AVX512
-template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template <>
+EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<int>(const int* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<uint32_t>(const uint32_t* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE float  pfirst<Packet8f>(const Packet8f& a) {
+template <>
+EIGEN_STRONG_INLINE float pfirst<Packet8f>(const Packet8f& a) {
   return _mm_cvtss_f32(_mm256_castps256_ps128(a));
 }
-template<> EIGEN_STRONG_INLINE double pfirst<Packet4d>(const Packet4d& a) {
+template <>
+EIGEN_STRONG_INLINE double pfirst<Packet4d>(const Packet4d& a) {
   return _mm_cvtsd_f64(_mm256_castpd256_pd128(a));
 }
-template<> EIGEN_STRONG_INLINE int    pfirst<Packet8i>(const Packet8i& a) {
+template <>
+EIGEN_STRONG_INLINE int pfirst<Packet8i>(const Packet8i& a) {
   return _mm_cvtsi128_si32(_mm256_castsi256_si128(a));
 }
+template <>
+EIGEN_STRONG_INLINE uint32_t pfirst<Packet8ui>(const Packet8ui& a) {
+  return numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(_mm256_castsi256_si128(a)));
+}
 
-
-template<> EIGEN_STRONG_INLINE Packet8f preverse(const Packet8f& a)
-{
-  __m256 tmp = _mm256_shuffle_ps(a,a,0x1b);
+template <>
+EIGEN_STRONG_INLINE Packet8f preverse(const Packet8f& a) {
+  __m256 tmp = _mm256_shuffle_ps(a, a, 0x1b);
   return _mm256_permute2f128_ps(tmp, tmp, 1);
 }
-template<> EIGEN_STRONG_INLINE Packet4d preverse(const Packet4d& a)
-{
-   __m256d tmp = _mm256_shuffle_pd(a,a,5);
+template <>
+EIGEN_STRONG_INLINE Packet4d preverse(const Packet4d& a) {
+  __m256d tmp = _mm256_shuffle_pd(a, a, 5);
   return _mm256_permute2f128_pd(tmp, tmp, 1);
-  #if 0
+#if 0
   // This version is unlikely to be faster as _mm256_shuffle_ps and _mm256_permute_pd
   // exhibit the same latency/throughput, but it is here for future reference/benchmarking...
   __m256d swap_halves = _mm256_permute2f128_pd(a,a,1);
     return _mm256_permute_pd(swap_halves,5);
-  #endif
+#endif
 }
+template <>
+EIGEN_STRONG_INLINE Packet8i preverse(const Packet8i& a) {
+  return _mm256_castps_si256(preverse(_mm256_castsi256_ps(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui preverse(const Packet8ui& a) {
+  return _mm256_castps_si256(preverse(_mm256_castsi256_ps(a)));
+}
+
+#ifdef EIGEN_VECTORIZE_AVX2
+template <>
+EIGEN_STRONG_INLINE Packet4l preverse(const Packet4l& a) {
+  return _mm256_castpd_si256(preverse(_mm256_castsi256_pd(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul preverse(const Packet4ul& a) {
+  return _mm256_castpd_si256(preverse(_mm256_castsi256_pd(a)));
+}
+#endif
 
 // pabs should be ok
-template<> EIGEN_STRONG_INLINE Packet8f pabs(const Packet8f& a)
-{
-  const Packet8f mask = _mm256_castsi256_ps(_mm256_setr_epi32(0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF));
-  return _mm256_and_ps(a,mask);
+template <>
+EIGEN_STRONG_INLINE Packet8f pabs(const Packet8f& a) {
+  const Packet8f mask = _mm256_castsi256_ps(_mm256_setr_epi32(0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF,
+                                                              0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF));
+  return _mm256_and_ps(a, mask);
 }
-template<> EIGEN_STRONG_INLINE Packet4d pabs(const Packet4d& a)
-{
-  const Packet4d mask = _mm256_castsi256_pd(_mm256_setr_epi32(0xFFFFFFFF,0x7FFFFFFF,0xFFFFFFFF,0x7FFFFFFF,0xFFFFFFFF,0x7FFFFFFF,0xFFFFFFFF,0x7FFFFFFF));
-  return _mm256_and_pd(a,mask);
+template <>
+EIGEN_STRONG_INLINE Packet4d pabs(const Packet4d& a) {
+  const Packet4d mask = _mm256_castsi256_pd(_mm256_setr_epi32(0xFFFFFFFF, 0x7FFFFFFF, 0xFFFFFFFF, 0x7FFFFFFF,
+                                                              0xFFFFFFFF, 0x7FFFFFFF, 0xFFFFFFFF, 0x7FFFFFFF));
+  return _mm256_and_pd(a, mask);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8i pabs(const Packet8i& a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_abs_epi32(a);
+#else
+  __m128i lo = _mm_abs_epi32(_mm256_extractf128_si256(a, 0));
+  __m128i hi = _mm_abs_epi32(_mm256_extractf128_si256(a, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui pabs(const Packet8ui& a) {
+  return a;
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pfrexp<Packet8f>(const Packet8f& a, Packet8f& exponent) {
-  return pfrexp_generic(a,exponent);
+template <>
+EIGEN_STRONG_INLINE Packet8h psignbit(const Packet8h& a) {
+  return _mm_srai_epi16(a, 15);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8bf psignbit(const Packet8bf& a) {
+  return _mm_srai_epi16(a, 15);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8f psignbit(const Packet8f& a) {
+  return _mm256_castsi256_ps(parithmetic_shift_right<31>((Packet8i)_mm256_castps_si256(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8ui psignbit(const Packet8ui& a) {
+  return pzero(a);
+}
+#ifdef EIGEN_VECTORIZE_AVX2
+template <>
+EIGEN_STRONG_INLINE Packet4d psignbit(const Packet4d& a) {
+  return _mm256_castsi256_pd(parithmetic_shift_right<63>((Packet4l)_mm256_castpd_si256(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ul psignbit(const Packet4ul& a) {
+  return pzero(a);
+}
+#endif
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pfrexp<Packet8f>(const Packet8f& a, Packet8f& exponent) {
+  return pfrexp_generic(a, exponent);
 }
 
 // Extract exponent without existence of Packet4l.
-template<>
-EIGEN_STRONG_INLINE  
-Packet4d pfrexp_generic_get_biased_exponent(const Packet4d& a) {
-  const Packet4d cst_exp_mask  = pset1frombits<Packet4d>(static_cast<uint64_t>(0x7ff0000000000000ull));
+template <>
+EIGEN_STRONG_INLINE Packet4d pfrexp_generic_get_biased_exponent(const Packet4d& a) {
+  const Packet4d cst_exp_mask = pset1frombits<Packet4d>(static_cast<uint64_t>(0x7ff0000000000000ull));
   __m256i a_expo = _mm256_castpd_si256(pand(a, cst_exp_mask));
 #ifdef EIGEN_VECTORIZE_AVX2
   a_expo = _mm256_srli_epi64(a_expo, 52);
@@ -760,91 +1882,109 @@
   return exponent;
 }
 
-
-template<> EIGEN_STRONG_INLINE Packet4d pfrexp<Packet4d>(const Packet4d& a, Packet4d& exponent) {
+template <>
+EIGEN_STRONG_INLINE Packet4d pfrexp<Packet4d>(const Packet4d& a, Packet4d& exponent) {
   return pfrexp_generic(a, exponent);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pldexp<Packet8f>(const Packet8f& a, const Packet8f& exponent) {
+template <>
+EIGEN_STRONG_INLINE Packet8f pldexp<Packet8f>(const Packet8f& a, const Packet8f& exponent) {
   return pldexp_generic(a, exponent);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4d pldexp<Packet4d>(const Packet4d& a, const Packet4d& exponent) {
+template <>
+EIGEN_STRONG_INLINE Packet4d pldexp<Packet4d>(const Packet4d& a, const Packet4d& exponent) {
   // Clamp exponent to [-2099, 2099]
   const Packet4d max_exponent = pset1<Packet4d>(2099.0);
   const Packet4i e = _mm256_cvtpd_epi32(pmin(pmax(exponent, pnegate(max_exponent)), max_exponent));
-  
+
   // Split 2^e into four factors and multiply.
   const Packet4i bias = pset1<Packet4i>(1023);
   Packet4i b = parithmetic_shift_right<2>(e);  // floor(e/4)
-  
+
   // 2^b
   Packet4i hi = vec4i_swizzle1(padd(b, bias), 0, 2, 1, 3);
   Packet4i lo = _mm_slli_epi64(hi, 52);
   hi = _mm_slli_epi64(_mm_srli_epi64(hi, 32), 52);
   Packet4d c = _mm256_castsi256_pd(_mm256_insertf128_si256(_mm256_castsi128_si256(lo), hi, 1));
   Packet4d out = pmul(pmul(pmul(a, c), c), c);  // a * 2^(3b)
-  
+
   // 2^(e - 3b)
   b = psub(psub(psub(e, b), b), b);  // e - 3b
   hi = vec4i_swizzle1(padd(b, bias), 0, 2, 1, 3);
   lo = _mm_slli_epi64(hi, 52);
   hi = _mm_slli_epi64(_mm_srli_epi64(hi, 32), 52);
   c = _mm256_castsi256_pd(_mm256_insertf128_si256(_mm256_castsi128_si256(lo), hi, 1));
-  out = pmul(out, c); // a * 2^e
+  out = pmul(out, c);  // a * 2^e
   return out;
 }
 
-template<> EIGEN_STRONG_INLINE float predux<Packet8f>(const Packet8f& a)
-{
-  return predux(Packet4f(_mm_add_ps(_mm256_castps256_ps128(a),_mm256_extractf128_ps(a,1))));
+template <>
+EIGEN_STRONG_INLINE float predux<Packet8f>(const Packet8f& a) {
+  return predux(Packet4f(_mm_add_ps(_mm256_castps256_ps128(a), _mm256_extractf128_ps(a, 1))));
 }
-template<> EIGEN_STRONG_INLINE double predux<Packet4d>(const Packet4d& a)
-{
-  return predux(Packet2d(_mm_add_pd(_mm256_castpd256_pd128(a),_mm256_extractf128_pd(a,1))));
+template <>
+EIGEN_STRONG_INLINE double predux<Packet4d>(const Packet4d& a) {
+  return predux(Packet2d(_mm_add_pd(_mm256_castpd256_pd128(a), _mm256_extractf128_pd(a, 1))));
+}
+template <>
+EIGEN_STRONG_INLINE int predux<Packet8i>(const Packet8i& a) {
+  return predux(Packet4i(_mm_add_epi32(_mm256_castsi256_si128(a), _mm256_extractf128_si256(a, 1))));
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux<Packet8ui>(const Packet8ui& a) {
+  return predux(Packet4ui(_mm_add_epi32(_mm256_castsi256_si128(a), _mm256_extractf128_si256(a, 1))));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f predux_half_dowto4<Packet8f>(const Packet8f& a)
-{
-  return _mm_add_ps(_mm256_castps256_ps128(a),_mm256_extractf128_ps(a,1));
+template <>
+EIGEN_STRONG_INLINE Packet4f predux_half_dowto4<Packet8f>(const Packet8f& a) {
+  return _mm_add_ps(_mm256_castps256_ps128(a), _mm256_extractf128_ps(a, 1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i predux_half_dowto4<Packet8i>(const Packet8i& a) {
+  return _mm_add_epi32(_mm256_castsi256_si128(a), _mm256_extractf128_si256(a, 1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui predux_half_dowto4<Packet8ui>(const Packet8ui& a) {
+  return _mm_add_epi32(_mm256_castsi256_si128(a), _mm256_extractf128_si256(a, 1));
 }
 
-template<> EIGEN_STRONG_INLINE float predux_mul<Packet8f>(const Packet8f& a)
-{
+template <>
+EIGEN_STRONG_INLINE float predux_mul<Packet8f>(const Packet8f& a) {
   Packet8f tmp;
-  tmp = _mm256_mul_ps(a, _mm256_permute2f128_ps(a,a,1));
-  tmp = _mm256_mul_ps(tmp, _mm256_shuffle_ps(tmp,tmp,_MM_SHUFFLE(1,0,3,2)));
-  return pfirst(_mm256_mul_ps(tmp, _mm256_shuffle_ps(tmp,tmp,1)));
+  tmp = _mm256_mul_ps(a, _mm256_permute2f128_ps(a, a, 1));
+  tmp = _mm256_mul_ps(tmp, _mm256_shuffle_ps(tmp, tmp, _MM_SHUFFLE(1, 0, 3, 2)));
+  return pfirst(_mm256_mul_ps(tmp, _mm256_shuffle_ps(tmp, tmp, 1)));
 }
-template<> EIGEN_STRONG_INLINE double predux_mul<Packet4d>(const Packet4d& a)
-{
+template <>
+EIGEN_STRONG_INLINE double predux_mul<Packet4d>(const Packet4d& a) {
   Packet4d tmp;
-  tmp = _mm256_mul_pd(a, _mm256_permute2f128_pd(a,a,1));
-  return pfirst(_mm256_mul_pd(tmp, _mm256_shuffle_pd(tmp,tmp,1)));
+  tmp = _mm256_mul_pd(a, _mm256_permute2f128_pd(a, a, 1));
+  return pfirst(_mm256_mul_pd(tmp, _mm256_shuffle_pd(tmp, tmp, 1)));
 }
 
-template<> EIGEN_STRONG_INLINE float predux_min<Packet8f>(const Packet8f& a)
-{
-  Packet8f tmp = _mm256_min_ps(a, _mm256_permute2f128_ps(a,a,1));
-  tmp = _mm256_min_ps(tmp, _mm256_shuffle_ps(tmp,tmp,_MM_SHUFFLE(1,0,3,2)));
-  return pfirst(_mm256_min_ps(tmp, _mm256_shuffle_ps(tmp,tmp,1)));
+template <>
+EIGEN_STRONG_INLINE float predux_min<Packet8f>(const Packet8f& a) {
+  Packet8f tmp = _mm256_min_ps(a, _mm256_permute2f128_ps(a, a, 1));
+  tmp = _mm256_min_ps(tmp, _mm256_shuffle_ps(tmp, tmp, _MM_SHUFFLE(1, 0, 3, 2)));
+  return pfirst(_mm256_min_ps(tmp, _mm256_shuffle_ps(tmp, tmp, 1)));
 }
-template<> EIGEN_STRONG_INLINE double predux_min<Packet4d>(const Packet4d& a)
-{
-  Packet4d tmp = _mm256_min_pd(a, _mm256_permute2f128_pd(a,a,1));
+template <>
+EIGEN_STRONG_INLINE double predux_min<Packet4d>(const Packet4d& a) {
+  Packet4d tmp = _mm256_min_pd(a, _mm256_permute2f128_pd(a, a, 1));
   return pfirst(_mm256_min_pd(tmp, _mm256_shuffle_pd(tmp, tmp, 1)));
 }
 
-template<> EIGEN_STRONG_INLINE float predux_max<Packet8f>(const Packet8f& a)
-{
-  Packet8f tmp = _mm256_max_ps(a, _mm256_permute2f128_ps(a,a,1));
-  tmp = _mm256_max_ps(tmp, _mm256_shuffle_ps(tmp,tmp,_MM_SHUFFLE(1,0,3,2)));
-  return pfirst(_mm256_max_ps(tmp, _mm256_shuffle_ps(tmp,tmp,1)));
+template <>
+EIGEN_STRONG_INLINE float predux_max<Packet8f>(const Packet8f& a) {
+  Packet8f tmp = _mm256_max_ps(a, _mm256_permute2f128_ps(a, a, 1));
+  tmp = _mm256_max_ps(tmp, _mm256_shuffle_ps(tmp, tmp, _MM_SHUFFLE(1, 0, 3, 2)));
+  return pfirst(_mm256_max_ps(tmp, _mm256_shuffle_ps(tmp, tmp, 1)));
 }
 
-template<> EIGEN_STRONG_INLINE double predux_max<Packet4d>(const Packet4d& a)
-{
-  Packet4d tmp = _mm256_max_pd(a, _mm256_permute2f128_pd(a,a,1));
+template <>
+EIGEN_STRONG_INLINE double predux_max<Packet4d>(const Packet4d& a) {
+  Packet4d tmp = _mm256_max_pd(a, _mm256_permute2f128_pd(a, a, 1));
   return pfirst(_mm256_max_pd(tmp, _mm256_shuffle_pd(tmp, tmp, 1)));
 }
 
@@ -854,13 +1994,21 @@
 //   return _mm256_movemask_ps(x)==0xFF;
 // }
 
-template<> EIGEN_STRONG_INLINE bool predux_any(const Packet8f& x)
-{
-  return _mm256_movemask_ps(x)!=0;
+template <>
+EIGEN_STRONG_INLINE bool predux_any(const Packet8f& x) {
+  return _mm256_movemask_ps(x) != 0;
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet8f,8>& kernel) {
+template <>
+EIGEN_STRONG_INLINE bool predux_any(const Packet8i& x) {
+  return _mm256_movemask_ps(_mm256_castsi256_ps(x)) != 0;
+}
+template <>
+EIGEN_STRONG_INLINE bool predux_any(const Packet8ui& x) {
+  return _mm256_movemask_ps(_mm256_castsi256_ps(x)) != 0;
+}
+
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet8f, 8>& kernel) {
   __m256 T0 = _mm256_unpacklo_ps(kernel.packet[0], kernel.packet[1]);
   __m256 T1 = _mm256_unpackhi_ps(kernel.packet[0], kernel.packet[1]);
   __m256 T2 = _mm256_unpacklo_ps(kernel.packet[2], kernel.packet[3]);
@@ -869,14 +2017,14 @@
   __m256 T5 = _mm256_unpackhi_ps(kernel.packet[4], kernel.packet[5]);
   __m256 T6 = _mm256_unpacklo_ps(kernel.packet[6], kernel.packet[7]);
   __m256 T7 = _mm256_unpackhi_ps(kernel.packet[6], kernel.packet[7]);
-  __m256 S0 = _mm256_shuffle_ps(T0,T2,_MM_SHUFFLE(1,0,1,0));
-  __m256 S1 = _mm256_shuffle_ps(T0,T2,_MM_SHUFFLE(3,2,3,2));
-  __m256 S2 = _mm256_shuffle_ps(T1,T3,_MM_SHUFFLE(1,0,1,0));
-  __m256 S3 = _mm256_shuffle_ps(T1,T3,_MM_SHUFFLE(3,2,3,2));
-  __m256 S4 = _mm256_shuffle_ps(T4,T6,_MM_SHUFFLE(1,0,1,0));
-  __m256 S5 = _mm256_shuffle_ps(T4,T6,_MM_SHUFFLE(3,2,3,2));
-  __m256 S6 = _mm256_shuffle_ps(T5,T7,_MM_SHUFFLE(1,0,1,0));
-  __m256 S7 = _mm256_shuffle_ps(T5,T7,_MM_SHUFFLE(3,2,3,2));
+  __m256 S0 = _mm256_shuffle_ps(T0, T2, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256 S1 = _mm256_shuffle_ps(T0, T2, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256 S2 = _mm256_shuffle_ps(T1, T3, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256 S3 = _mm256_shuffle_ps(T1, T3, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256 S4 = _mm256_shuffle_ps(T4, T6, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256 S5 = _mm256_shuffle_ps(T4, T6, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256 S6 = _mm256_shuffle_ps(T5, T7, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256 S7 = _mm256_shuffle_ps(T5, T7, _MM_SHUFFLE(3, 2, 3, 2));
   kernel.packet[0] = _mm256_permute2f128_ps(S0, S4, 0x20);
   kernel.packet[1] = _mm256_permute2f128_ps(S1, S5, 0x20);
   kernel.packet[2] = _mm256_permute2f128_ps(S2, S6, 0x20);
@@ -887,17 +2035,16 @@
   kernel.packet[7] = _mm256_permute2f128_ps(S3, S7, 0x31);
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet8f,4>& kernel) {
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet8f, 4>& kernel) {
   __m256 T0 = _mm256_unpacklo_ps(kernel.packet[0], kernel.packet[1]);
   __m256 T1 = _mm256_unpackhi_ps(kernel.packet[0], kernel.packet[1]);
   __m256 T2 = _mm256_unpacklo_ps(kernel.packet[2], kernel.packet[3]);
   __m256 T3 = _mm256_unpackhi_ps(kernel.packet[2], kernel.packet[3]);
 
-  __m256 S0 = _mm256_shuffle_ps(T0,T2,_MM_SHUFFLE(1,0,1,0));
-  __m256 S1 = _mm256_shuffle_ps(T0,T2,_MM_SHUFFLE(3,2,3,2));
-  __m256 S2 = _mm256_shuffle_ps(T1,T3,_MM_SHUFFLE(1,0,1,0));
-  __m256 S3 = _mm256_shuffle_ps(T1,T3,_MM_SHUFFLE(3,2,3,2));
+  __m256 S0 = _mm256_shuffle_ps(T0, T2, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256 S1 = _mm256_shuffle_ps(T0, T2, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256 S2 = _mm256_shuffle_ps(T1, T3, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256 S3 = _mm256_shuffle_ps(T1, T3, _MM_SHUFFLE(3, 2, 3, 2));
 
   kernel.packet[0] = _mm256_permute2f128_ps(S0, S1, 0x20);
   kernel.packet[1] = _mm256_permute2f128_ps(S2, S3, 0x20);
@@ -905,8 +2052,70 @@
   kernel.packet[3] = _mm256_permute2f128_ps(S2, S3, 0x31);
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet4d,4>& kernel) {
+#define MM256_SHUFFLE_EPI32(A, B, M) \
+  _mm256_castps_si256(_mm256_shuffle_ps(_mm256_castsi256_ps(A), _mm256_castsi256_ps(B), M))
+
+#ifndef EIGEN_VECTORIZE_AVX2
+#define MM256_UNPACKLO_EPI32(A, B) \
+  _mm256_castps_si256(_mm256_unpacklo_ps(_mm256_castsi256_ps(A), _mm256_castsi256_ps(B)))
+#define MM256_UNPACKHI_EPI32(A, B) \
+  _mm256_castps_si256(_mm256_unpackhi_ps(_mm256_castsi256_ps(A), _mm256_castsi256_ps(B)))
+#else
+#define MM256_UNPACKLO_EPI32(A, B) _mm256_unpacklo_epi32(A, B)
+#define MM256_UNPACKHI_EPI32(A, B) _mm256_unpackhi_epi32(A, B)
+#endif
+
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet8i, 8>& kernel) {
+  __m256i T0 = MM256_UNPACKLO_EPI32(kernel.packet[0], kernel.packet[1]);
+  __m256i T1 = MM256_UNPACKHI_EPI32(kernel.packet[0], kernel.packet[1]);
+  __m256i T2 = MM256_UNPACKLO_EPI32(kernel.packet[2], kernel.packet[3]);
+  __m256i T3 = MM256_UNPACKHI_EPI32(kernel.packet[2], kernel.packet[3]);
+  __m256i T4 = MM256_UNPACKLO_EPI32(kernel.packet[4], kernel.packet[5]);
+  __m256i T5 = MM256_UNPACKHI_EPI32(kernel.packet[4], kernel.packet[5]);
+  __m256i T6 = MM256_UNPACKLO_EPI32(kernel.packet[6], kernel.packet[7]);
+  __m256i T7 = MM256_UNPACKHI_EPI32(kernel.packet[6], kernel.packet[7]);
+  __m256i S0 = MM256_SHUFFLE_EPI32(T0, T2, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256i S1 = MM256_SHUFFLE_EPI32(T0, T2, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256i S2 = MM256_SHUFFLE_EPI32(T1, T3, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256i S3 = MM256_SHUFFLE_EPI32(T1, T3, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256i S4 = MM256_SHUFFLE_EPI32(T4, T6, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256i S5 = MM256_SHUFFLE_EPI32(T4, T6, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256i S6 = MM256_SHUFFLE_EPI32(T5, T7, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256i S7 = MM256_SHUFFLE_EPI32(T5, T7, _MM_SHUFFLE(3, 2, 3, 2));
+  kernel.packet[0] = _mm256_permute2f128_si256(S0, S4, 0x20);
+  kernel.packet[1] = _mm256_permute2f128_si256(S1, S5, 0x20);
+  kernel.packet[2] = _mm256_permute2f128_si256(S2, S6, 0x20);
+  kernel.packet[3] = _mm256_permute2f128_si256(S3, S7, 0x20);
+  kernel.packet[4] = _mm256_permute2f128_si256(S0, S4, 0x31);
+  kernel.packet[5] = _mm256_permute2f128_si256(S1, S5, 0x31);
+  kernel.packet[6] = _mm256_permute2f128_si256(S2, S6, 0x31);
+  kernel.packet[7] = _mm256_permute2f128_si256(S3, S7, 0x31);
+}
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet8ui, 8>& kernel) {
+  ptranspose((PacketBlock<Packet8i, 8>&)kernel);
+}
+
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet8i, 4>& kernel) {
+  __m256i T0 = MM256_UNPACKLO_EPI32(kernel.packet[0], kernel.packet[1]);
+  __m256i T1 = MM256_UNPACKHI_EPI32(kernel.packet[0], kernel.packet[1]);
+  __m256i T2 = MM256_UNPACKLO_EPI32(kernel.packet[2], kernel.packet[3]);
+  __m256i T3 = MM256_UNPACKHI_EPI32(kernel.packet[2], kernel.packet[3]);
+
+  __m256i S0 = MM256_SHUFFLE_EPI32(T0, T2, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256i S1 = MM256_SHUFFLE_EPI32(T0, T2, _MM_SHUFFLE(3, 2, 3, 2));
+  __m256i S2 = MM256_SHUFFLE_EPI32(T1, T3, _MM_SHUFFLE(1, 0, 1, 0));
+  __m256i S3 = MM256_SHUFFLE_EPI32(T1, T3, _MM_SHUFFLE(3, 2, 3, 2));
+
+  kernel.packet[0] = _mm256_permute2f128_si256(S0, S1, 0x20);
+  kernel.packet[1] = _mm256_permute2f128_si256(S2, S3, 0x20);
+  kernel.packet[2] = _mm256_permute2f128_si256(S0, S1, 0x31);
+  kernel.packet[3] = _mm256_permute2f128_si256(S2, S3, 0x31);
+}
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet8ui, 4>& kernel) {
+  ptranspose((PacketBlock<Packet8i, 4>&)kernel);
+}
+
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet4d, 4>& kernel) {
   __m256d T0 = _mm256_shuffle_pd(kernel.packet[0], kernel.packet[1], 15);
   __m256d T1 = _mm256_shuffle_pd(kernel.packet[0], kernel.packet[1], 0);
   __m256d T2 = _mm256_shuffle_pd(kernel.packet[2], kernel.packet[3], 15);
@@ -918,49 +2127,90 @@
   kernel.packet[2] = _mm256_permute2f128_pd(T1, T3, 49);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pblend(const Selector<8>& ifPacket, const Packet8f& thenPacket, const Packet8f& elsePacket) {
+template <>
+EIGEN_STRONG_INLINE Packet8f pblend(const Selector<8>& ifPacket, const Packet8f& thenPacket,
+                                    const Packet8f& elsePacket) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  const __m256i zero = _mm256_setzero_si256();
+  const __m256i select =
+      _mm256_set_epi32(ifPacket.select[7], ifPacket.select[6], ifPacket.select[5], ifPacket.select[4],
+                       ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
+  __m256i false_mask = _mm256_cmpeq_epi32(zero, select);
+  return _mm256_blendv_ps(thenPacket, elsePacket, _mm256_castsi256_ps(false_mask));
+#else
   const __m256 zero = _mm256_setzero_ps();
-  const __m256 select = _mm256_set_ps(ifPacket.select[7], ifPacket.select[6], ifPacket.select[5], ifPacket.select[4], ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
+  const __m256 select = _mm256_set_ps(ifPacket.select[7], ifPacket.select[6], ifPacket.select[5], ifPacket.select[4],
+                                      ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
   __m256 false_mask = _mm256_cmp_ps(select, zero, _CMP_EQ_UQ);
   return _mm256_blendv_ps(thenPacket, elsePacket, false_mask);
+#endif
 }
-template<> EIGEN_STRONG_INLINE Packet4d pblend(const Selector<4>& ifPacket, const Packet4d& thenPacket, const Packet4d& elsePacket) {
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pblend(const Selector<4>& ifPacket, const Packet4d& thenPacket,
+                                    const Packet4d& elsePacket) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  const __m256i zero = _mm256_setzero_si256();
+  const __m256i select =
+      _mm256_set_epi64x(ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
+  __m256i false_mask = _mm256_cmpeq_epi64(select, zero);
+  return _mm256_blendv_pd(thenPacket, elsePacket, _mm256_castsi256_pd(false_mask));
+#else
   const __m256d zero = _mm256_setzero_pd();
   const __m256d select = _mm256_set_pd(ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
   __m256d false_mask = _mm256_cmp_pd(select, zero, _CMP_EQ_UQ);
   return _mm256_blendv_pd(thenPacket, elsePacket, false_mask);
+#endif
 }
 
 // Packet math for Eigen::half
+#ifndef EIGEN_VECTORIZE_AVX512FP16
+template <>
+struct unpacket_traits<Packet8h> {
+  typedef Eigen::half type;
+  enum {
+    size = 8,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+  typedef Packet8h half;
+};
+#endif
 
-template<> struct unpacket_traits<Packet8h> { typedef Eigen::half type; enum {size=8, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false}; typedef Packet8h half; };
-
-template<> EIGEN_STRONG_INLINE Packet8h pset1<Packet8h>(const Eigen::half& from) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pset1<Packet8h>(const Eigen::half& from) {
   return _mm_set1_epi16(numext::bit_cast<numext::uint16_t>(from));
 }
 
-template<> EIGEN_STRONG_INLINE Eigen::half pfirst<Packet8h>(const Packet8h& from) {
+template <>
+EIGEN_STRONG_INLINE Eigen::half pfirst<Packet8h>(const Packet8h& from) {
   return numext::bit_cast<Eigen::half>(static_cast<numext::uint16_t>(_mm_extract_epi16(from, 0)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pload<Packet8h>(const Eigen::half* from) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pload<Packet8h>(const Eigen::half* from) {
   return _mm_load_si128(reinterpret_cast<const __m128i*>(from));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h ploadu<Packet8h>(const Eigen::half* from) {
+template <>
+EIGEN_STRONG_INLINE Packet8h ploadu<Packet8h>(const Eigen::half* from) {
   return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
 }
 
-template<> EIGEN_STRONG_INLINE void pstore<Eigen::half>(Eigen::half* to, const Packet8h& from) {
+template <>
+EIGEN_STRONG_INLINE void pstore<Eigen::half>(Eigen::half* to, const Packet8h& from) {
   _mm_store_si128(reinterpret_cast<__m128i*>(to), from);
 }
 
-template<> EIGEN_STRONG_INLINE void pstoreu<Eigen::half>(Eigen::half* to, const Packet8h& from) {
+template <>
+EIGEN_STRONG_INLINE void pstoreu<Eigen::half>(Eigen::half* to, const Packet8h& from) {
   _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h
-ploaddup<Packet8h>(const Eigen::half*  from) {
+template <>
+EIGEN_STRONG_INLINE Packet8h ploaddup<Packet8h>(const Eigen::half* from) {
   const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
   const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
   const numext::uint16_t c = numext::bit_cast<numext::uint16_t>(from[2]);
@@ -968,15 +2218,16 @@
   return _mm_set_epi16(d, d, c, c, b, b, a, a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h
-ploadquad<Packet8h>(const Eigen::half* from) {
+template <>
+EIGEN_STRONG_INLINE Packet8h ploadquad<Packet8h>(const Eigen::half* from) {
   const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
   const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
   return _mm_set_epi16(b, b, b, b, a, a, a, a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h ptrue(const Packet8h& a) {
- return _mm_cmpeq_epi32(a, a);
+template <>
+EIGEN_STRONG_INLINE Packet8h ptrue(const Packet8h& a) {
+  return _mm_cmpeq_epi32(a, a);
 }
 
 template <>
@@ -989,48 +2240,29 @@
 #ifdef EIGEN_HAS_FP16_C
   return _mm256_cvtph_ps(a);
 #else
-  EIGEN_ALIGN32 Eigen::half aux[8];
-  pstore(aux, a);
-  float f0(aux[0]);
-  float f1(aux[1]);
-  float f2(aux[2]);
-  float f3(aux[3]);
-  float f4(aux[4]);
-  float f5(aux[5]);
-  float f6(aux[6]);
-  float f7(aux[7]);
-
-  return _mm256_set_ps(f7, f6, f5, f4, f3, f2, f1, f0);
+  Eigen::internal::Packet8f pp = _mm256_castsi256_ps(
+      _mm256_insertf128_si256(_mm256_castsi128_si256(half2floatsse(a)), half2floatsse(_mm_srli_si128(a, 8)), 1));
+  return pp;
 #endif
 }
 
 EIGEN_STRONG_INLINE Packet8h float2half(const Packet8f& a) {
 #ifdef EIGEN_HAS_FP16_C
-  return _mm256_cvtps_ph(a, _MM_FROUND_TO_NEAREST_INT|_MM_FROUND_NO_EXC);
+  return _mm256_cvtps_ph(a, _MM_FROUND_TO_NEAREST_INT);
 #else
-  EIGEN_ALIGN32 float aux[8];
-  pstore(aux, a);
-  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[0]));
-  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[1]));
-  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[2]));
-  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[3]));
-  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[4]));
-  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[5]));
-  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[6]));
-  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[7]));
-  return _mm_set_epi16(s7, s6, s5, s4, s3, s2, s1, s0);
+  __m128i lo = float2half(_mm256_extractf128_ps(a, 0));
+  __m128i hi = float2half(_mm256_extractf128_ps(a, 1));
+  return _mm_packus_epi32(lo, hi);
 #endif
 }
 
 template <>
-EIGEN_STRONG_INLINE Packet8h pmin<Packet8h>(const Packet8h& a,
-                                            const Packet8h& b) {
+EIGEN_STRONG_INLINE Packet8h pmin<Packet8h>(const Packet8h& a, const Packet8h& b) {
   return float2half(pmin<Packet8f>(half2float(a), half2float(b)));
 }
 
 template <>
-EIGEN_STRONG_INLINE Packet8h pmax<Packet8h>(const Packet8h& a,
-                                            const Packet8h& b) {
+EIGEN_STRONG_INLINE Packet8h pmax<Packet8h>(const Packet8h& a, const Packet8h& b) {
   return float2half(pmax<Packet8f>(half2float(a), half2float(b)));
 }
 
@@ -1039,151 +2271,179 @@
   return float2half(plset<Packet8f>(static_cast<float>(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h por(const Packet8h& a,const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h por(const Packet8h& a, const Packet8h& b) {
   // in some cases Packet4i is a wrapper around __m128i, so we either need to
   // cast to Packet4i to directly call the intrinsics as below:
-  return _mm_or_si128(a,b);
+  return _mm_or_si128(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8h pxor(const Packet8h& a,const Packet8h& b) {
-  return _mm_xor_si128(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet8h pxor(const Packet8h& a, const Packet8h& b) {
+  return _mm_xor_si128(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8h pand(const Packet8h& a,const Packet8h& b) {
-  return _mm_and_si128(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet8h pand(const Packet8h& a, const Packet8h& b) {
+  return _mm_and_si128(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8h pandnot(const Packet8h& a,const Packet8h& b) {
-  return _mm_andnot_si128(b,a);
+template <>
+EIGEN_STRONG_INLINE Packet8h pandnot(const Packet8h& a, const Packet8h& b) {
+  return _mm_andnot_si128(b, a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pselect(const Packet8h& mask, const Packet8h& a, const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pselect(const Packet8h& mask, const Packet8h& a, const Packet8h& b) {
   return _mm_blendv_epi8(b, a, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pround<Packet8h>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pround<Packet8h>(const Packet8h& a) {
   return float2half(pround<Packet8f>(half2float(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h print<Packet8h>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8h print<Packet8h>(const Packet8h& a) {
   return float2half(print<Packet8f>(half2float(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pceil<Packet8h>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pceil<Packet8h>(const Packet8h& a) {
   return float2half(pceil<Packet8f>(half2float(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pfloor<Packet8h>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pfloor<Packet8h>(const Packet8h& a) {
   return float2half(pfloor<Packet8f>(half2float(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pcmp_eq(const Packet8h& a,const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pcmp_eq(const Packet8h& a, const Packet8h& b) {
   return Pack16To8(pcmp_eq(half2float(a), half2float(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pcmp_le(const Packet8h& a,const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pcmp_le(const Packet8h& a, const Packet8h& b) {
   return Pack16To8(pcmp_le(half2float(a), half2float(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pcmp_lt(const Packet8h& a,const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pcmp_lt(const Packet8h& a, const Packet8h& b) {
   return Pack16To8(pcmp_lt(half2float(a), half2float(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pcmp_lt_or_nan(const Packet8h& a,const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pcmp_lt_or_nan(const Packet8h& a, const Packet8h& b) {
   return Pack16To8(pcmp_lt_or_nan(half2float(a), half2float(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pconj(const Packet8h& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet8h pconj(const Packet8h& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE Packet8h pnegate(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pnegate(const Packet8h& a) {
   Packet8h sign_mask = _mm_set1_epi16(static_cast<numext::uint16_t>(0x8000));
   return _mm_xor_si128(a, sign_mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h padd<Packet8h>(const Packet8h& a, const Packet8h& b) {
+#ifndef EIGEN_VECTORIZE_AVX512FP16
+template <>
+EIGEN_STRONG_INLINE Packet8h padd<Packet8h>(const Packet8h& a, const Packet8h& b) {
   Packet8f af = half2float(a);
   Packet8f bf = half2float(b);
   Packet8f rf = padd(af, bf);
   return float2half(rf);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h psub<Packet8h>(const Packet8h& a, const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h psub<Packet8h>(const Packet8h& a, const Packet8h& b) {
   Packet8f af = half2float(a);
   Packet8f bf = half2float(b);
   Packet8f rf = psub(af, bf);
   return float2half(rf);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pmul<Packet8h>(const Packet8h& a, const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pmul<Packet8h>(const Packet8h& a, const Packet8h& b) {
   Packet8f af = half2float(a);
   Packet8f bf = half2float(b);
   Packet8f rf = pmul(af, bf);
   return float2half(rf);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pdiv<Packet8h>(const Packet8h& a, const Packet8h& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pdiv<Packet8h>(const Packet8h& a, const Packet8h& b) {
   Packet8f af = half2float(a);
   Packet8f bf = half2float(b);
   Packet8f rf = pdiv(af, bf);
   return float2half(rf);
 }
+#endif
 
-template<> EIGEN_STRONG_INLINE Packet8h pgather<Eigen::half, Packet8h>(const Eigen::half* from, Index stride)
-{
-  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(from[0*stride]);
-  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(from[1*stride]);
-  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(from[2*stride]);
-  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(from[3*stride]);
-  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(from[4*stride]);
-  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(from[5*stride]);
-  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(from[6*stride]);
-  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(from[7*stride]);
+template <>
+EIGEN_STRONG_INLINE Packet8h pgather<Eigen::half, Packet8h>(const Eigen::half* from, Index stride) {
+  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(from[0 * stride]);
+  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(from[1 * stride]);
+  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(from[2 * stride]);
+  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(from[3 * stride]);
+  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(from[4 * stride]);
+  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(from[5 * stride]);
+  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(from[6 * stride]);
+  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(from[7 * stride]);
   return _mm_set_epi16(s7, s6, s5, s4, s3, s2, s1, s0);
 }
 
-template<> EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet8h>(Eigen::half* to, const Packet8h& from, Index stride)
-{
+template <>
+EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet8h>(Eigen::half* to, const Packet8h& from, Index stride) {
   EIGEN_ALIGN32 Eigen::half aux[8];
   pstore(aux, from);
-  to[stride*0] = aux[0];
-  to[stride*1] = aux[1];
-  to[stride*2] = aux[2];
-  to[stride*3] = aux[3];
-  to[stride*4] = aux[4];
-  to[stride*5] = aux[5];
-  to[stride*6] = aux[6];
-  to[stride*7] = aux[7];
+  to[stride * 0] = aux[0];
+  to[stride * 1] = aux[1];
+  to[stride * 2] = aux[2];
+  to[stride * 3] = aux[3];
+  to[stride * 4] = aux[4];
+  to[stride * 5] = aux[5];
+  to[stride * 6] = aux[6];
+  to[stride * 7] = aux[7];
 }
 
-template<> EIGEN_STRONG_INLINE Eigen::half predux<Packet8h>(const Packet8h& a) {
+#ifndef EIGEN_VECTORIZE_AVX512FP16
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux<Packet8h>(const Packet8h& a) {
   Packet8f af = half2float(a);
   float reduced = predux<Packet8f>(af);
   return Eigen::half(reduced);
 }
+#endif
 
-template<> EIGEN_STRONG_INLINE Eigen::half predux_max<Packet8h>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_max<Packet8h>(const Packet8h& a) {
   Packet8f af = half2float(a);
   float reduced = predux_max<Packet8f>(af);
   return Eigen::half(reduced);
 }
 
-template<> EIGEN_STRONG_INLINE Eigen::half predux_min<Packet8h>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_min<Packet8h>(const Packet8h& a) {
   Packet8f af = half2float(a);
   float reduced = predux_min<Packet8f>(af);
   return Eigen::half(reduced);
 }
 
-template<> EIGEN_STRONG_INLINE Eigen::half predux_mul<Packet8h>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_mul<Packet8h>(const Packet8h& a) {
   Packet8f af = half2float(a);
   float reduced = predux_mul<Packet8f>(af);
   return Eigen::half(reduced);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h preverse(const Packet8h& a)
-{
-  __m128i m = _mm_setr_epi8(14,15,12,13,10,11,8,9,6,7,4,5,2,3,0,1);
-  return _mm_shuffle_epi8(a,m);
+template <>
+EIGEN_STRONG_INLINE Packet8h preverse(const Packet8h& a) {
+  __m128i m = _mm_setr_epi8(14, 15, 12, 13, 10, 11, 8, 9, 6, 7, 4, 5, 2, 3, 0, 1);
+  return _mm_shuffle_epi8(a, m);
 }
 
-EIGEN_STRONG_INLINE void
-ptranspose(PacketBlock<Packet8h,8>& kernel) {
+EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8h, 8>& kernel) {
   __m128i a = kernel.packet[0];
   __m128i b = kernel.packet[1];
   __m128i c = kernel.packet[2];
@@ -1230,8 +2490,7 @@
   kernel.packet[7] = a7b7c7d7e7f7g7h7;
 }
 
-EIGEN_STRONG_INLINE void
-ptranspose(PacketBlock<Packet8h,4>& kernel) {
+EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8h, 4>& kernel) {
   EIGEN_ALIGN32 Eigen::half in[4][8];
   pstore<Eigen::half>(in[0], kernel.packet[0]);
   pstore<Eigen::half>(in[1], kernel.packet[1]);
@@ -1242,10 +2501,10 @@
 
   for (int i = 0; i < 4; ++i) {
     for (int j = 0; j < 4; ++j) {
-      out[i][j] = in[j][2*i];
+      out[i][j] = in[j][2 * i];
     }
     for (int j = 0; j < 4; ++j) {
-      out[i][j+4] = in[j][2*i+1];
+      out[i][j + 4] = in[j][2 * i + 1];
     }
   }
 
@@ -1272,8 +2531,6 @@
 
 // Convert float to bfloat16 according to round-to-nearest-even/denormals algorithm.
 EIGEN_STRONG_INLINE Packet8bf F32ToBf16(const Packet8f& a) {
-  Packet8bf r;
-
   __m256i input = _mm256_castps_si256(a);
 
 #ifdef EIGEN_VECTORIZE_AVX2
@@ -1292,8 +2549,7 @@
   __m256i nan = _mm256_set1_epi32(0x7fc0);
   t = _mm256_blendv_epi8(nan, t, _mm256_castps_si256(mask));
   // output = numext::bit_cast<uint16_t>(input);
-  return _mm_packus_epi32(_mm256_extractf128_si256(t, 0),
-                         _mm256_extractf128_si256(t, 1));
+  return _mm_packus_epi32(_mm256_extractf128_si256(t, 0), _mm256_extractf128_si256(t, 1));
 #else
   // uint32_t lsb = (input >> 16);
   __m128i lo = _mm_srli_epi32(_mm256_extractf128_si256(input, 0), 16);
@@ -1320,32 +2576,38 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pset1<Packet8bf>(const bfloat16& from) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pset1<Packet8bf>(const bfloat16& from) {
   return _mm_set1_epi16(numext::bit_cast<numext::uint16_t>(from));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 pfirst<Packet8bf>(const Packet8bf& from) {
+template <>
+EIGEN_STRONG_INLINE bfloat16 pfirst<Packet8bf>(const Packet8bf& from) {
   return numext::bit_cast<bfloat16>(static_cast<numext::uint16_t>(_mm_extract_epi16(from, 0)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pload<Packet8bf>(const bfloat16* from) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pload<Packet8bf>(const bfloat16* from) {
   return _mm_load_si128(reinterpret_cast<const __m128i*>(from));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf ploadu<Packet8bf>(const bfloat16* from) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf ploadu<Packet8bf>(const bfloat16* from) {
   return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
 }
 
-template<> EIGEN_STRONG_INLINE void pstore<bfloat16>(bfloat16* to, const Packet8bf& from) {
+template <>
+EIGEN_STRONG_INLINE void pstore<bfloat16>(bfloat16* to, const Packet8bf& from) {
   _mm_store_si128(reinterpret_cast<__m128i*>(to), from);
 }
 
-template<> EIGEN_STRONG_INLINE void pstoreu<bfloat16>(bfloat16* to, const Packet8bf& from) {
+template <>
+EIGEN_STRONG_INLINE void pstoreu<bfloat16>(bfloat16* to, const Packet8bf& from) {
   _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf
-ploaddup<Packet8bf>(const bfloat16* from) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf ploaddup<Packet8bf>(const bfloat16* from) {
   const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
   const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
   const numext::uint16_t c = numext::bit_cast<numext::uint16_t>(from[2]);
@@ -1353,15 +2615,16 @@
   return _mm_set_epi16(d, d, c, c, b, b, a, a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf
-ploadquad<Packet8bf>(const bfloat16* from) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf ploadquad<Packet8bf>(const bfloat16* from) {
   const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
   const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
   return _mm_set_epi16(b, b, b, b, a, a, a, a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf ptrue(const Packet8bf& a) {
- return _mm_cmpeq_epi32(a, a);
+template <>
+EIGEN_STRONG_INLINE Packet8bf ptrue(const Packet8bf& a) {
+  return _mm_cmpeq_epi32(a, a);
 }
 
 template <>
@@ -1371,14 +2634,12 @@
 }
 
 template <>
-EIGEN_STRONG_INLINE Packet8bf pmin<Packet8bf>(const Packet8bf& a,
-                                                const Packet8bf& b) {
+EIGEN_STRONG_INLINE Packet8bf pmin<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
   return F32ToBf16(pmin<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
 template <>
-EIGEN_STRONG_INLINE Packet8bf pmax<Packet8bf>(const Packet8bf& a,
-                                                const Packet8bf& b) {
+EIGEN_STRONG_INLINE Packet8bf pmax<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
   return F32ToBf16(pmax<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
@@ -1387,131 +2648,153 @@
   return F32ToBf16(plset<Packet8f>(static_cast<float>(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf por(const Packet8bf& a,const Packet8bf& b) {
-  return _mm_or_si128(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet8bf por(const Packet8bf& a, const Packet8bf& b) {
+  return _mm_or_si128(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8bf pxor(const Packet8bf& a,const Packet8bf& b) {
-  return _mm_xor_si128(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet8bf pxor(const Packet8bf& a, const Packet8bf& b) {
+  return _mm_xor_si128(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8bf pand(const Packet8bf& a,const Packet8bf& b) {
-  return _mm_and_si128(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet8bf pand(const Packet8bf& a, const Packet8bf& b) {
+  return _mm_and_si128(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8bf pandnot(const Packet8bf& a,const Packet8bf& b) {
-  return _mm_andnot_si128(b,a);
+template <>
+EIGEN_STRONG_INLINE Packet8bf pandnot(const Packet8bf& a, const Packet8bf& b) {
+  return _mm_andnot_si128(b, a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pselect(const Packet8bf& mask, const Packet8bf& a, const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pselect(const Packet8bf& mask, const Packet8bf& a, const Packet8bf& b) {
   return _mm_blendv_epi8(b, a, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pround<Packet8bf>(const Packet8bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8bf pround<Packet8bf>(const Packet8bf& a) {
   return F32ToBf16(pround<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf print<Packet8bf>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf print<Packet8bf>(const Packet8bf& a) {
   return F32ToBf16(print<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pceil<Packet8bf>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pceil<Packet8bf>(const Packet8bf& a) {
   return F32ToBf16(pceil<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pfloor<Packet8bf>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pfloor<Packet8bf>(const Packet8bf& a) {
   return F32ToBf16(pfloor<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pcmp_eq(const Packet8bf& a,const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pcmp_eq(const Packet8bf& a, const Packet8bf& b) {
   return Pack16To8(pcmp_eq(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pcmp_le(const Packet8bf& a,const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pcmp_le(const Packet8bf& a, const Packet8bf& b) {
   return Pack16To8(pcmp_le(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pcmp_lt(const Packet8bf& a,const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pcmp_lt(const Packet8bf& a, const Packet8bf& b) {
   return Pack16To8(pcmp_lt(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pcmp_lt_or_nan(const Packet8bf& a,const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pcmp_lt_or_nan(const Packet8bf& a, const Packet8bf& b) {
   return Pack16To8(pcmp_lt_or_nan(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pconj(const Packet8bf& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet8bf pconj(const Packet8bf& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE Packet8bf pnegate(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pnegate(const Packet8bf& a) {
   Packet8bf sign_mask = _mm_set1_epi16(static_cast<numext::uint16_t>(0x8000));
   return _mm_xor_si128(a, sign_mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf padd<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf padd<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
   return F32ToBf16(padd<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf psub<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf psub<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
   return F32ToBf16(psub<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pmul<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pmul<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
   return F32ToBf16(pmul<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pdiv<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pdiv<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
   return F32ToBf16(pdiv<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-
-template<> EIGEN_STRONG_INLINE Packet8bf pgather<bfloat16, Packet8bf>(const bfloat16* from, Index stride)
-{
-  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(from[0*stride]);
-  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(from[1*stride]);
-  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(from[2*stride]);
-  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(from[3*stride]);
-  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(from[4*stride]);
-  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(from[5*stride]);
-  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(from[6*stride]);
-  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(from[7*stride]);
+template <>
+EIGEN_STRONG_INLINE Packet8bf pgather<bfloat16, Packet8bf>(const bfloat16* from, Index stride) {
+  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(from[0 * stride]);
+  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(from[1 * stride]);
+  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(from[2 * stride]);
+  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(from[3 * stride]);
+  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(from[4 * stride]);
+  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(from[5 * stride]);
+  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(from[6 * stride]);
+  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(from[7 * stride]);
   return _mm_set_epi16(s7, s6, s5, s4, s3, s2, s1, s0);
 }
 
-template<> EIGEN_STRONG_INLINE void pscatter<bfloat16, Packet8bf>(bfloat16* to, const Packet8bf& from, Index stride)
-{
+template <>
+EIGEN_STRONG_INLINE void pscatter<bfloat16, Packet8bf>(bfloat16* to, const Packet8bf& from, Index stride) {
   EIGEN_ALIGN32 bfloat16 aux[8];
   pstore(aux, from);
-  to[stride*0] = aux[0];
-  to[stride*1] = aux[1];
-  to[stride*2] = aux[2];
-  to[stride*3] = aux[3];
-  to[stride*4] = aux[4];
-  to[stride*5] = aux[5];
-  to[stride*6] = aux[6];
-  to[stride*7] = aux[7];
+  to[stride * 0] = aux[0];
+  to[stride * 1] = aux[1];
+  to[stride * 2] = aux[2];
+  to[stride * 3] = aux[3];
+  to[stride * 4] = aux[4];
+  to[stride * 5] = aux[5];
+  to[stride * 6] = aux[6];
+  to[stride * 7] = aux[7];
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux<Packet8bf>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux<Packet8bf>(const Packet8bf& a) {
   return static_cast<bfloat16>(predux<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux_max<Packet8bf>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux_max<Packet8bf>(const Packet8bf& a) {
   return static_cast<bfloat16>(predux_max<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux_min<Packet8bf>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux_min<Packet8bf>(const Packet8bf& a) {
   return static_cast<bfloat16>(predux_min<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux_mul<Packet8bf>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux_mul<Packet8bf>(const Packet8bf& a) {
   return static_cast<bfloat16>(predux_mul<Packet8f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf preverse(const Packet8bf& a)
-{
-  __m128i m = _mm_setr_epi8(14,15,12,13,10,11,8,9,6,7,4,5,2,3,0,1);
-  return _mm_shuffle_epi8(a,m);
+template <>
+EIGEN_STRONG_INLINE Packet8bf preverse(const Packet8bf& a) {
+  __m128i m = _mm_setr_epi8(14, 15, 12, 13, 10, 11, 8, 9, 6, 7, 4, 5, 2, 3, 0, 1);
+  return _mm_shuffle_epi8(a, m);
 }
 
-EIGEN_STRONG_INLINE void
-ptranspose(PacketBlock<Packet8bf,8>& kernel) {
+EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8bf, 8>& kernel) {
   __m128i a = kernel.packet[0];
   __m128i b = kernel.packet[1];
   __m128i c = kernel.packet[2];
@@ -1549,8 +2832,7 @@
   kernel.packet[7] = _mm_unpackhi_epi64(a67b67c67d67, e67f67g67h67);
 }
 
-EIGEN_STRONG_INLINE void
-ptranspose(PacketBlock<Packet8bf,4>& kernel) {
+EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8bf, 4>& kernel) {
   __m128i a = kernel.packet[0];
   __m128i b = kernel.packet[1];
   __m128i c = kernel.packet[2];
@@ -1567,8 +2849,8 @@
   kernel.packet[3] = _mm_unpackhi_epi32(ab_47, cd_47);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PACKET_MATH_AVX_H
+#endif  // EIGEN_PACKET_MATH_AVX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h
index d507fb6..3688f8d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h
@@ -10,106 +10,218 @@
 #ifndef EIGEN_TYPE_CASTING_AVX_H
 #define EIGEN_TYPE_CASTING_AVX_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-// For now we use SSE to handle integers, so we can't use AVX instructions to cast
-// from int to float
-template <>
-struct type_casting_traits<float, int> {
-  enum {
-    VectorizedCast = 0,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
-
-template <>
-struct type_casting_traits<int, float> {
-  enum {
-    VectorizedCast = 0,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
-
-
 #ifndef EIGEN_VECTORIZE_AVX512
+template <>
+struct type_casting_traits<float, bool> : vectorized_type_casting_traits<float, bool> {};
+template <>
+struct type_casting_traits<bool, float> : vectorized_type_casting_traits<bool, float> {};
 
 template <>
-struct type_casting_traits<Eigen::half, float> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
-
+struct type_casting_traits<float, int> : vectorized_type_casting_traits<float, int> {};
+template <>
+struct type_casting_traits<int, float> : vectorized_type_casting_traits<int, float> {};
 
 template <>
-struct type_casting_traits<float, Eigen::half> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
+struct type_casting_traits<float, double> : vectorized_type_casting_traits<float, double> {};
+template <>
+struct type_casting_traits<double, float> : vectorized_type_casting_traits<double, float> {};
 
 template <>
-struct type_casting_traits<bfloat16, float> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
+struct type_casting_traits<double, int> : vectorized_type_casting_traits<double, int> {};
+template <>
+struct type_casting_traits<int, double> : vectorized_type_casting_traits<int, double> {};
 
 template <>
-struct type_casting_traits<float, bfloat16> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
+struct type_casting_traits<half, float> : vectorized_type_casting_traits<half, float> {};
+template <>
+struct type_casting_traits<float, half> : vectorized_type_casting_traits<float, half> {};
 
-#endif  // EIGEN_VECTORIZE_AVX512
+template <>
+struct type_casting_traits<bfloat16, float> : vectorized_type_casting_traits<bfloat16, float> {};
+template <>
+struct type_casting_traits<float, bfloat16> : vectorized_type_casting_traits<float, bfloat16> {};
+#endif
 
-template<> EIGEN_STRONG_INLINE Packet8i pcast<Packet8f, Packet8i>(const Packet8f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet16b pcast<Packet8f, Packet16b>(const Packet8f& a, const Packet8f& b) {
+  __m256 nonzero_a = _mm256_cmp_ps(a, pzero(a), _CMP_NEQ_UQ);
+  __m256 nonzero_b = _mm256_cmp_ps(b, pzero(b), _CMP_NEQ_UQ);
+  constexpr char kFF = '\255';
+#ifndef EIGEN_VECTORIZE_AVX2
+  __m128i shuffle_mask128_a_lo = _mm_set_epi8(kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, 12, 8, 4, 0);
+  __m128i shuffle_mask128_a_hi = _mm_set_epi8(kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, 12, 8, 4, 0, kFF, kFF, kFF, kFF);
+  __m128i shuffle_mask128_b_lo = _mm_set_epi8(kFF, kFF, kFF, kFF, 12, 8, 4, 0, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF);
+  __m128i shuffle_mask128_b_hi = _mm_set_epi8(12, 8, 4, 0, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF);
+  __m128i a_hi = _mm_shuffle_epi8(_mm256_extractf128_si256(_mm256_castps_si256(nonzero_a), 1), shuffle_mask128_a_hi);
+  __m128i a_lo = _mm_shuffle_epi8(_mm256_extractf128_si256(_mm256_castps_si256(nonzero_a), 0), shuffle_mask128_a_lo);
+  __m128i b_hi = _mm_shuffle_epi8(_mm256_extractf128_si256(_mm256_castps_si256(nonzero_b), 1), shuffle_mask128_b_hi);
+  __m128i b_lo = _mm_shuffle_epi8(_mm256_extractf128_si256(_mm256_castps_si256(nonzero_b), 0), shuffle_mask128_b_lo);
+  __m128i merged = _mm_or_si128(_mm_or_si128(b_lo, b_hi), _mm_or_si128(a_lo, a_hi));
+  return _mm_and_si128(merged, _mm_set1_epi8(1));
+#else
+  __m256i a_shuffle_mask = _mm256_set_epi8(kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, 12, 8, 4, 0, kFF, kFF, kFF, kFF, kFF,
+                                           kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, 12, 8, 4, 0);
+  __m256i b_shuffle_mask = _mm256_set_epi8(12, 8, 4, 0, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF,
+                                           kFF, kFF, kFF, 12, 8, 4, 0, kFF, kFF, kFF, kFF, kFF, kFF, kFF, kFF);
+  __m256i a_shuff = _mm256_shuffle_epi8(_mm256_castps_si256(nonzero_a), a_shuffle_mask);
+  __m256i b_shuff = _mm256_shuffle_epi8(_mm256_castps_si256(nonzero_b), b_shuffle_mask);
+  __m256i a_or_b = _mm256_or_si256(a_shuff, b_shuff);
+  __m256i merged = _mm256_or_si256(a_or_b, _mm256_castsi128_si256(_mm256_extractf128_si256(a_or_b, 1)));
+  return _mm256_castsi256_si128(_mm256_and_si256(merged, _mm256_set1_epi8(1)));
+#endif
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pcast<Packet16b, Packet8f>(const Packet16b& a) {
+  const __m256 cst_one = _mm256_set1_ps(1.0f);
+#ifdef EIGEN_VECTORIZE_AVX2
+  __m256i a_extended = _mm256_cvtepi8_epi32(a);
+  __m256i abcd_efgh = _mm256_cmpeq_epi32(a_extended, _mm256_setzero_si256());
+#else
+  __m128i abcd_efhg_ijkl_mnop = _mm_cmpeq_epi8(a, _mm_setzero_si128());
+  __m128i aabb_ccdd_eeff_gghh = _mm_unpacklo_epi8(abcd_efhg_ijkl_mnop, abcd_efhg_ijkl_mnop);
+  __m128i aaaa_bbbb_cccc_dddd = _mm_unpacklo_epi8(aabb_ccdd_eeff_gghh, aabb_ccdd_eeff_gghh);
+  __m128i eeee_ffff_gggg_hhhh = _mm_unpackhi_epi8(aabb_ccdd_eeff_gghh, aabb_ccdd_eeff_gghh);
+  __m256i abcd_efgh = _mm256_setr_m128i(aaaa_bbbb_cccc_dddd, eeee_ffff_gggg_hhhh);
+#endif
+  __m256 result = _mm256_andnot_ps(_mm256_castsi256_ps(abcd_efgh), cst_one);
+  return result;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8i pcast<Packet8f, Packet8i>(const Packet8f& a) {
   return _mm256_cvttps_epi32(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pcast<Packet8i, Packet8f>(const Packet8i& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8i pcast<Packet4d, Packet8i>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_set_m128i(_mm256_cvttpd_epi32(b), _mm256_cvttpd_epi32(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4d, Packet4i>(const Packet4d& a) {
+  return _mm256_cvttpd_epi32(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pcast<Packet8i, Packet8f>(const Packet8i& a) {
   return _mm256_cvtepi32_ps(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8i preinterpret<Packet8i,Packet8f>(const Packet8f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8f pcast<Packet4d, Packet8f>(const Packet4d& a, const Packet4d& b) {
+  return _mm256_set_m128(_mm256_cvtpd_ps(b), _mm256_cvtpd_ps(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4d, Packet4f>(const Packet4d& a) {
+  return _mm256_cvtpd_ps(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pcast<Packet8i, Packet4d>(const Packet8i& a) {
+  return _mm256_cvtepi32_pd(_mm256_castsi256_si128(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pcast<Packet4i, Packet4d>(const Packet4i& a) {
+  return _mm256_cvtepi32_pd(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pcast<Packet8f, Packet4d>(const Packet8f& a) {
+  return _mm256_cvtps_pd(_mm256_castps256_ps128(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4d pcast<Packet4f, Packet4d>(const Packet4f& a) {
+  return _mm256_cvtps_pd(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8i preinterpret<Packet8i, Packet8f>(const Packet8f& a) {
   return _mm256_castps_si256(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f preinterpret<Packet8f,Packet8i>(const Packet8i& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8f preinterpret<Packet8f, Packet8i>(const Packet8i& a) {
   return _mm256_castsi256_ps(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pcast<Packet8h, Packet8f>(const Packet8h& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8ui preinterpret<Packet8ui, Packet8i>(const Packet8i& a) {
+  return Packet8ui(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8i preinterpret<Packet8i, Packet8ui>(const Packet8ui& a) {
+  return Packet8i(a);
+}
+
+// truncation operations
+
+template <>
+EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet8f>(const Packet8f& a) {
+  return _mm256_castps256_ps128(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet4d>(const Packet4d& a) {
+  return _mm256_castpd256_pd128(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet8i>(const Packet8i& a) {
+  return _mm256_castsi256_si128(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet8ui>(const Packet8ui& a) {
+  return _mm256_castsi256_si128(a);
+}
+
+#ifdef EIGEN_VECTORIZE_AVX2
+template <>
+EIGEN_STRONG_INLINE Packet4ul preinterpret<Packet4ul, Packet4l>(const Packet4l& a) {
+  return Packet4ul(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4l preinterpret<Packet4l, Packet4ul>(const Packet4ul& a) {
+  return Packet4l(a);
+}
+
+#endif
+
+template <>
+EIGEN_STRONG_INLINE Packet8f pcast<Packet8h, Packet8f>(const Packet8h& a) {
   return half2float(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pcast<Packet8bf, Packet8f>(const Packet8bf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8f pcast<Packet8bf, Packet8f>(const Packet8bf& a) {
   return Bf16ToF32(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8h pcast<Packet8f, Packet8h>(const Packet8f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8h pcast<Packet8f, Packet8h>(const Packet8f& a) {
   return float2half(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8bf pcast<Packet8f, Packet8bf>(const Packet8f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8bf pcast<Packet8f, Packet8bf>(const Packet8f& a) {
   return F32ToBf16(a);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TYPE_CASTING_AVX_H
+#endif  // EIGEN_TYPE_CASTING_AVX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h
index 1c28f4f..68b48f9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h
@@ -16,26 +16,69 @@
 #ifndef EIGEN_BFLOAT16_H
 #define EIGEN_BFLOAT16_H
 
-#define BF16_PACKET_FUNCTION(PACKET_F, PACKET_BF16, METHOD)         \
-  template <>                                                       \
-  EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED  \
-  PACKET_BF16 METHOD<PACKET_BF16>(const PACKET_BF16& _x) {          \
-    return F32ToBf16(METHOD<PACKET_F>(Bf16ToF32(_x)));              \
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
+#if defined(EIGEN_HAS_HIP_BF16)
+// When compiling with GPU support, the "hip_bfloat16" base class as well as
+// some other routines are defined in the GPU compiler header files
+// (hip_bfloat16.h), and they are not tagged constexpr
+// As a consequence, we get compile failures when compiling Eigen with
+// GPU support. Hence the need to disable EIGEN_CONSTEXPR when building
+// Eigen with GPU support
+#pragma push_macro("EIGEN_CONSTEXPR")
+#undef EIGEN_CONSTEXPR
+#define EIGEN_CONSTEXPR
+#endif
+
+#define BF16_PACKET_FUNCTION(PACKET_F, PACKET_BF16, METHOD)                                         \
+  template <>                                                                                       \
+  EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED PACKET_BF16 METHOD<PACKET_BF16>( \
+      const PACKET_BF16& _x) {                                                                      \
+    return F32ToBf16(METHOD<PACKET_F>(Bf16ToF32(_x)));                                              \
   }
 
+// Only use HIP GPU bf16 in kernels
+#if defined(EIGEN_HAS_HIP_BF16) && defined(EIGEN_GPU_COMPILE_PHASE)
+#define EIGEN_USE_HIP_BF16
+#endif
+
 namespace Eigen {
 
 struct bfloat16;
 
+namespace numext {
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::bfloat16 bit_cast<Eigen::bfloat16, uint16_t>(const uint16_t& src);
+
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC uint16_t bit_cast<uint16_t, Eigen::bfloat16>(const Eigen::bfloat16& src);
+}  // namespace numext
 namespace bfloat16_impl {
 
+#if defined(EIGEN_USE_HIP_BF16)
+
+struct __bfloat16_raw : public hip_bfloat16 {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw() {}
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw(hip_bfloat16 hb) : hip_bfloat16(hb) {}
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw(unsigned short raw) : hip_bfloat16(raw) {}
+};
+
+#else
+
 // Make our own __bfloat16_raw definition.
 struct __bfloat16_raw {
+#if defined(EIGEN_HAS_HIP_BF16) && !defined(EIGEN_GPU_COMPILE_PHASE)
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw() {}
+#else
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw() : value(0) {}
+#endif
   explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw(unsigned short raw) : value(raw) {}
   unsigned short value;
 };
 
+#endif  // defined(EIGEN_USE_HIP_BF16)
+
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw raw_uint16_to_bfloat16(unsigned short value);
 template <bool AssumeArgumentIsNormalOrInfinityOrZero>
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne(float ff);
@@ -52,11 +95,10 @@
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16_base(const __bfloat16_raw& h) : __bfloat16_raw(h) {}
 };
 
-} // namespace bfloat16_impl
+}  // namespace bfloat16_impl
 
 // Class definition.
 struct bfloat16 : public bfloat16_impl::bfloat16_base {
-
   typedef bfloat16_impl::__bfloat16_raw __bfloat16_raw;
 
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16() {}
@@ -66,16 +108,17 @@
   explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16(bool b)
       : bfloat16_impl::bfloat16_base(bfloat16_impl::raw_uint16_to_bfloat16(b ? 0x3f80 : 0)) {}
 
-  template<class T>
+  template <class T>
   explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16(T val)
-      : bfloat16_impl::bfloat16_base(bfloat16_impl::float_to_bfloat16_rtne<internal::is_integral<T>::value>(static_cast<float>(val))) {}
+      : bfloat16_impl::bfloat16_base(
+            bfloat16_impl::float_to_bfloat16_rtne<internal::is_integral<T>::value>(static_cast<float>(val))) {}
 
   explicit EIGEN_DEVICE_FUNC bfloat16(float f)
       : bfloat16_impl::bfloat16_base(bfloat16_impl::float_to_bfloat16_rtne<false>(f)) {}
 
   // Following the convention of numpy, converting between complex and
   // float will lead to loss of imag value.
-  template<typename RealScalar>
+  template <typename RealScalar>
   explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16(const std::complex<RealScalar>& val)
       : bfloat16_impl::bfloat16_base(bfloat16_impl::float_to_bfloat16_rtne<false>(static_cast<float>(val.real()))) {}
 
@@ -83,57 +126,116 @@
     return bfloat16_impl::bfloat16_to_float(*this);
   }
 };
-} // namespace Eigen
 
-namespace std {
-template<>
-struct numeric_limits<Eigen::bfloat16> {
-  static const bool is_specialized = true;
-  static const bool is_signed = true;
-  static const bool is_integer = false;
-  static const bool is_exact = false;
-  static const bool has_infinity = true;
-  static const bool has_quiet_NaN = true;
-  static const bool has_signaling_NaN = true;
-  static const float_denorm_style has_denorm = std::denorm_absent;
-  static const bool has_denorm_loss = false;
-  static const std::float_round_style round_style = numeric_limits<float>::round_style;
-  static const bool is_iec559 = false;
-  static const bool is_bounded = true;
-  static const bool is_modulo = false;
-  static const int digits = 8;
-  static const int digits10 = 2;
-  static const int max_digits10 = 4;
-  static const int radix = 2;
-  static const int min_exponent = numeric_limits<float>::min_exponent;
-  static const int min_exponent10 = numeric_limits<float>::min_exponent10;
-  static const int max_exponent = numeric_limits<float>::max_exponent;
-  static const int max_exponent10 = numeric_limits<float>::max_exponent10;
-  static const bool traps = numeric_limits<float>::traps;
-  static const bool tinyness_before = numeric_limits<float>::tinyness_before;
+// TODO(majnemer): Get rid of this once we can rely on C++17 inline variables do
+// solve the ODR issue.
+namespace bfloat16_impl {
+template <typename = void>
+struct numeric_limits_bfloat16_impl {
+  static EIGEN_CONSTEXPR const bool is_specialized = true;
+  static EIGEN_CONSTEXPR const bool is_signed = true;
+  static EIGEN_CONSTEXPR const bool is_integer = false;
+  static EIGEN_CONSTEXPR const bool is_exact = false;
+  static EIGEN_CONSTEXPR const bool has_infinity = true;
+  static EIGEN_CONSTEXPR const bool has_quiet_NaN = true;
+  static EIGEN_CONSTEXPR const bool has_signaling_NaN = true;
+  static EIGEN_CONSTEXPR const std::float_denorm_style has_denorm = std::denorm_present;
+  static EIGEN_CONSTEXPR const bool has_denorm_loss = false;
+  static EIGEN_CONSTEXPR const std::float_round_style round_style = std::numeric_limits<float>::round_style;
+  static EIGEN_CONSTEXPR const bool is_iec559 = true;
+  // The C++ standard defines this as "true if the set of values representable
+  // by the type is finite." BFloat16 has finite precision.
+  static EIGEN_CONSTEXPR const bool is_bounded = true;
+  static EIGEN_CONSTEXPR const bool is_modulo = false;
+  static EIGEN_CONSTEXPR const int digits = 8;
+  static EIGEN_CONSTEXPR const int digits10 = 2;
+  static EIGEN_CONSTEXPR const int max_digits10 = 4;
+  static EIGEN_CONSTEXPR const int radix = std::numeric_limits<float>::radix;
+  static EIGEN_CONSTEXPR const int min_exponent = std::numeric_limits<float>::min_exponent;
+  static EIGEN_CONSTEXPR const int min_exponent10 = std::numeric_limits<float>::min_exponent10;
+  static EIGEN_CONSTEXPR const int max_exponent = std::numeric_limits<float>::max_exponent;
+  static EIGEN_CONSTEXPR const int max_exponent10 = std::numeric_limits<float>::max_exponent10;
+  static EIGEN_CONSTEXPR const bool traps = std::numeric_limits<float>::traps;
+  // IEEE754: "The implementer shall choose how tininess is detected, but shall
+  // detect tininess in the same way for all operations in radix two"
+  static EIGEN_CONSTEXPR const bool tinyness_before = std::numeric_limits<float>::tinyness_before;
 
-  static Eigen::bfloat16 (min)() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x0080); }
-  static Eigen::bfloat16 lowest() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0xff7f); }
-  static Eigen::bfloat16 (max)() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f7f); }
-  static Eigen::bfloat16 epsilon() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x3c00); }
-  static Eigen::bfloat16 round_error() { return Eigen::bfloat16(0x3f00); }
-  static Eigen::bfloat16 infinity() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f80); }
-  static Eigen::bfloat16 quiet_NaN() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7fc0); }
-  static Eigen::bfloat16 signaling_NaN() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f81); }
-  static Eigen::bfloat16 denorm_min() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x0001); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16(min)() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x0080); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16 lowest() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0xff7f); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16(max)() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f7f); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16 epsilon() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x3c00); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16 round_error() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x3f00); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16 infinity() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f80); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16 quiet_NaN() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7fc0); }
+  static EIGEN_CONSTEXPR Eigen::bfloat16 signaling_NaN() {
+    return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7fa0);
+  }
+  static EIGEN_CONSTEXPR Eigen::bfloat16 denorm_min() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x0001); }
 };
 
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::is_specialized;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::is_signed;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::is_integer;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::is_exact;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::has_infinity;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::has_quiet_NaN;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::has_signaling_NaN;
+template <typename T>
+EIGEN_CONSTEXPR const std::float_denorm_style numeric_limits_bfloat16_impl<T>::has_denorm;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::has_denorm_loss;
+template <typename T>
+EIGEN_CONSTEXPR const std::float_round_style numeric_limits_bfloat16_impl<T>::round_style;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::is_iec559;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::is_bounded;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::is_modulo;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::digits;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::digits10;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::max_digits10;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::radix;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::min_exponent;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::min_exponent10;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::max_exponent;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_bfloat16_impl<T>::max_exponent10;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::traps;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_bfloat16_impl<T>::tinyness_before;
+}  // end namespace bfloat16_impl
+}  // end namespace Eigen
+
+namespace std {
 // If std::numeric_limits<T> is specialized, should also specialize
 // std::numeric_limits<const T>, std::numeric_limits<volatile T>, and
 // std::numeric_limits<const volatile T>
 // https://stackoverflow.com/a/16519653/
-template<>
-struct numeric_limits<const Eigen::bfloat16> : numeric_limits<Eigen::bfloat16> {};
-template<>
-struct numeric_limits<volatile Eigen::bfloat16> : numeric_limits<Eigen::bfloat16> {};
-template<>
-struct numeric_limits<const volatile Eigen::bfloat16> : numeric_limits<Eigen::bfloat16> {};
-} // namespace std
+template <>
+class numeric_limits<Eigen::bfloat16> : public Eigen::bfloat16_impl::numeric_limits_bfloat16_impl<> {};
+template <>
+class numeric_limits<const Eigen::bfloat16> : public numeric_limits<Eigen::bfloat16> {};
+template <>
+class numeric_limits<volatile Eigen::bfloat16> : public numeric_limits<Eigen::bfloat16> {};
+template <>
+class numeric_limits<const volatile Eigen::bfloat16> : public numeric_limits<Eigen::bfloat16> {};
+}  // end namespace std
 
 namespace Eigen {
 
@@ -142,15 +244,15 @@
 // We need to distinguish ‘clang as the CUDA compiler’ from ‘clang as the host compiler,
 // invoked by NVCC’ (e.g. on MacOS). The former needs to see both host and device implementation
 // of the functions, while the latter can only deal with one of them.
-#if !defined(EIGEN_HAS_NATIVE_BF16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC) // Emulate support for bfloat16 floats
+#if !defined(EIGEN_HAS_NATIVE_BF16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC)  // Emulate support for bfloat16 floats
 
 #if EIGEN_COMP_CLANG && defined(EIGEN_CUDACC)
 // We need to provide emulated *host-side* BF16 operators for clang.
 #pragma push_macro("EIGEN_DEVICE_FUNC")
 #undef EIGEN_DEVICE_FUNC
-#if defined(EIGEN_HAS_CUDA_BF16) && defined(EIGEN_HAS_NATIVE_BF16)
+#if (defined(EIGEN_HAS_GPU_BF16) && defined(EIGEN_HAS_NATIVE_BF16))
 #define EIGEN_DEVICE_FUNC __host__
-#else // both host and device need emulated ops.
+#else  // both host and device need emulated ops.
 #define EIGEN_DEVICE_FUNC __host__ __device__
 #endif
 #endif
@@ -158,42 +260,41 @@
 // Definitions for CPUs, mostly working through conversion
 // to/from fp32.
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator + (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator+(const bfloat16& a, const bfloat16& b) {
   return bfloat16(float(a) + float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator + (const bfloat16& a, const int& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator+(const bfloat16& a, const int& b) {
   return bfloat16(float(a) + static_cast<float>(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator + (const int& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator+(const int& a, const bfloat16& b) {
   return bfloat16(static_cast<float>(a) + float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator * (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator*(const bfloat16& a, const bfloat16& b) {
   return bfloat16(float(a) * float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator - (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator-(const bfloat16& a, const bfloat16& b) {
   return bfloat16(float(a) - float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator / (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator/(const bfloat16& a, const bfloat16& b) {
   return bfloat16(float(a) / float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator - (const bfloat16& a) {
-  bfloat16 result;
-  result.value = a.value ^ 0x8000;
-  return result;
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator-(const bfloat16& a) {
+  numext::uint16_t x = numext::bit_cast<uint16_t>(a) ^ 0x8000;
+  return numext::bit_cast<bfloat16>(x);
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator += (bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator+=(bfloat16& a, const bfloat16& b) {
   a = bfloat16(float(a) + float(b));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator *= (bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator*=(bfloat16& a, const bfloat16& b) {
   a = bfloat16(float(a) * float(b));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator -= (bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator-=(bfloat16& a, const bfloat16& b) {
   a = bfloat16(float(a) - float(b));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator /= (bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator/=(bfloat16& a, const bfloat16& b) {
   a = bfloat16(float(a) / float(b));
   return a;
 }
@@ -215,22 +316,22 @@
   --a;
   return original_value;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator == (const bfloat16& a, const bfloat16& b) {
-  return numext::equal_strict(float(a),float(b));
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator==(const bfloat16& a, const bfloat16& b) {
+  return numext::equal_strict(float(a), float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator != (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator!=(const bfloat16& a, const bfloat16& b) {
   return numext::not_equal_strict(float(a), float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator < (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<(const bfloat16& a, const bfloat16& b) {
   return float(a) < float(b);
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator <= (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<=(const bfloat16& a, const bfloat16& b) {
   return float(a) <= float(b);
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator > (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>(const bfloat16& a, const bfloat16& b) {
   return float(a) > float(b);
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator >= (const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>=(const bfloat16& a, const bfloat16& b) {
   return float(a) >= float(b);
 }
 
@@ -241,49 +342,59 @@
 
 // Division by an index. Do it in full float precision to avoid accuracy
 // issues in converting the denominator to bfloat16.
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator / (const bfloat16& a, Index b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator/(const bfloat16& a, Index b) {
   return bfloat16(static_cast<float>(a) / static_cast<float>(b));
 }
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw truncate_to_bfloat16(const float v) {
+#if defined(EIGEN_USE_HIP_BF16)
+  return __bfloat16_raw(__bfloat16_raw::round_to_bfloat16(v, __bfloat16_raw::truncate));
+#else
   __bfloat16_raw output;
-  if (Eigen::numext::isnan EIGEN_NOT_A_MACRO(v)) {
-    output.value = std::signbit(v) ? 0xFFC0: 0x7FC0;
+  if (numext::isnan EIGEN_NOT_A_MACRO(v)) {
+    output.value = std::signbit(v) ? 0xFFC0 : 0x7FC0;
     return output;
   }
-  const uint16_t* p = reinterpret_cast<const uint16_t*>(&v);
-#if defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
-  output.value = p[0];
-#else
-  output.value = p[1];
-#endif
+  output.value = static_cast<numext::uint16_t>(numext::bit_cast<numext::uint32_t>(v) >> 16);
   return output;
+#endif
 }
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw raw_uint16_to_bfloat16(numext::uint16_t value) {
+#if defined(EIGEN_USE_HIP_BF16)
+  __bfloat16_raw bf;
+  bf.data = value;
+  return bf;
+#else
   return __bfloat16_raw(value);
+#endif
 }
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR numext::uint16_t raw_bfloat16_as_uint16(const __bfloat16_raw& bf) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR numext::uint16_t raw_bfloat16_as_uint16(
+    const __bfloat16_raw& bf) {
+#if defined(EIGEN_USE_HIP_BF16)
+  return bf.data;
+#else
   return bf.value;
+#endif
 }
 
 // float_to_bfloat16_rtne template specialization that does not make any
 // assumption about the value of its function argument (ff).
 template <>
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne<false>(float ff) {
-#if (defined(EIGEN_HAS_CUDA_BF16) && defined(EIGEN_HAS_HIP_BF16))
-  // Nothing to do here
+#if defined(EIGEN_USE_HIP_BF16)
+  return __bfloat16_raw(__bfloat16_raw::round_to_bfloat16(ff));
 #else
   __bfloat16_raw output;
 
-  if (Eigen::numext::isnan EIGEN_NOT_A_MACRO(ff)) {
+  if (numext::isnan EIGEN_NOT_A_MACRO(ff)) {
     // If the value is a NaN, squash it to a qNaN with msb of fraction set,
     // this makes sure after truncation we don't end up with an inf.
     //
     // qNaN magic: All exponent bits set + most significant bit of fraction
     // set.
-    output.value = std::signbit(ff) ? 0xFFC0: 0x7FC0;
+    output.value = std::signbit(ff) ? 0xFFC0 : 0x7FC0;
   } else {
     // Fast rounding algorithm that rounds a half value to nearest even. This
     // reduces expected error when we convert a large number of floats. Here
@@ -446,134 +557,97 @@
 // type to bfloat16.
 template <>
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne<true>(float ff) {
-#if (defined(EIGEN_HAS_CUDA_BF16) && defined(EIGEN_HAS_HIP_BF16))
-    // Nothing to do here
+#if defined(EIGEN_USE_HIP_BF16)
+  return __bfloat16_raw(__bfloat16_raw::round_to_bfloat16(ff));
 #else
-    numext::uint32_t input = numext::bit_cast<numext::uint32_t>(ff);
-    __bfloat16_raw output;
+  numext::uint32_t input = numext::bit_cast<numext::uint32_t>(ff);
+  __bfloat16_raw output;
 
-    // Least significant bit of resulting bfloat.
-    numext::uint32_t lsb = (input >> 16) & 1;
-    numext::uint32_t rounding_bias = 0x7fff + lsb;
-    input += rounding_bias;
-    output.value = static_cast<numext::uint16_t>(input >> 16);
-    return output;
+  // Least significant bit of resulting bfloat.
+  numext::uint32_t lsb = (input >> 16) & 1;
+  numext::uint32_t rounding_bias = 0x7fff + lsb;
+  input += rounding_bias;
+  output.value = static_cast<numext::uint16_t>(input >> 16);
+  return output;
 #endif
 }
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float bfloat16_to_float(__bfloat16_raw h) {
-    float result = 0;
-    unsigned short* q = reinterpret_cast<unsigned short*>(&result);
-#if defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
-    q[0] = h.value;
+#if defined(EIGEN_USE_HIP_BF16)
+  return static_cast<float>(h);
 #else
-    q[1] = h.value;
+  return numext::bit_cast<float>(static_cast<numext::uint32_t>(h.value) << 16);
 #endif
-    return result;
 }
+
 // --- standard functions ---
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isinf)(const bfloat16& a) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isinf)(const bfloat16& a) {
   EIGEN_USING_STD(isinf);
+#if defined(EIGEN_USE_HIP_BF16)
+  return (isinf)(a);  // Uses HIP hip_bfloat16 isinf operator
+#else
   return (isinf)(float(a));
+#endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isnan)(const bfloat16& a) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isnan)(const bfloat16& a) {
   EIGEN_USING_STD(isnan);
+#if defined(EIGEN_USE_HIP_BF16)
+  return (isnan)(a);  // Uses HIP hip_bfloat16 isnan operator
+#else
   return (isnan)(float(a));
+#endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isfinite)(const bfloat16& a) {
-  return !(isinf EIGEN_NOT_A_MACRO (a)) && !(isnan EIGEN_NOT_A_MACRO (a));
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isfinite)(const bfloat16& a) {
+  return !(isinf EIGEN_NOT_A_MACRO(a)) && !(isnan EIGEN_NOT_A_MACRO(a));
 }
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 abs(const bfloat16& a) {
-  bfloat16 result;
-  result.value = a.value & 0x7FFF;
-  return result;
+  numext::uint16_t x = numext::bit_cast<numext::uint16_t>(a) & 0x7FFF;
+  return numext::bit_cast<bfloat16>(x);
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 exp(const bfloat16& a) {
-   return bfloat16(::expf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 expm1(const bfloat16& a) {
-  return bfloat16(numext::expm1(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log(const bfloat16& a) {
-  return bfloat16(::logf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log1p(const bfloat16& a) {
-  return bfloat16(numext::log1p(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log10(const bfloat16& a) {
-  return bfloat16(::log10f(float(a)));
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 exp(const bfloat16& a) { return bfloat16(::expf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 expm1(const bfloat16& a) { return bfloat16(numext::expm1(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log(const bfloat16& a) { return bfloat16(::logf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log1p(const bfloat16& a) { return bfloat16(numext::log1p(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log10(const bfloat16& a) { return bfloat16(::log10f(float(a))); }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log2(const bfloat16& a) {
   return bfloat16(static_cast<float>(EIGEN_LOG2E) * ::logf(float(a)));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sqrt(const bfloat16& a) {
-    return bfloat16(::sqrtf(float(a)));
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sqrt(const bfloat16& a) { return bfloat16(::sqrtf(float(a))); }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 pow(const bfloat16& a, const bfloat16& b) {
   return bfloat16(::powf(float(a), float(b)));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sin(const bfloat16& a) {
-  return bfloat16(::sinf(float(a)));
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 atan2(const bfloat16& a, const bfloat16& b) {
+  return bfloat16(::atan2f(float(a), float(b)));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 cos(const bfloat16& a) {
-  return bfloat16(::cosf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 tan(const bfloat16& a) {
-  return bfloat16(::tanf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 asin(const bfloat16& a) {
-  return bfloat16(::asinf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 acos(const bfloat16& a) {
-  return bfloat16(::acosf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 atan(const bfloat16& a) {
-  return bfloat16(::atanf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sinh(const bfloat16& a) {
-  return bfloat16(::sinhf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 cosh(const bfloat16& a) {
-  return bfloat16(::coshf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 tanh(const bfloat16& a) {
-  return bfloat16(::tanhf(float(a)));
-}
-#if EIGEN_HAS_CXX11_MATH
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 asinh(const bfloat16& a) {
-  return bfloat16(::asinhf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 acosh(const bfloat16& a) {
-  return bfloat16(::acoshf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 atanh(const bfloat16& a) {
-  return bfloat16(::atanhf(float(a)));
-}
-#endif
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 floor(const bfloat16& a) {
-  return bfloat16(::floorf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 ceil(const bfloat16& a) {
-  return bfloat16(::ceilf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 rint(const bfloat16& a) {
-  return bfloat16(::rintf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 round(const bfloat16& a) {
-  return bfloat16(::roundf(float(a)));
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sin(const bfloat16& a) { return bfloat16(::sinf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 cos(const bfloat16& a) { return bfloat16(::cosf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 tan(const bfloat16& a) { return bfloat16(::tanf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 asin(const bfloat16& a) { return bfloat16(::asinf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 acos(const bfloat16& a) { return bfloat16(::acosf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 atan(const bfloat16& a) { return bfloat16(::atanf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sinh(const bfloat16& a) { return bfloat16(::sinhf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 cosh(const bfloat16& a) { return bfloat16(::coshf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 tanh(const bfloat16& a) { return bfloat16(::tanhf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 asinh(const bfloat16& a) { return bfloat16(::asinhf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 acosh(const bfloat16& a) { return bfloat16(::acoshf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 atanh(const bfloat16& a) { return bfloat16(::atanhf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 floor(const bfloat16& a) { return bfloat16(::floorf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 ceil(const bfloat16& a) { return bfloat16(::ceilf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 rint(const bfloat16& a) { return bfloat16(::rintf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 round(const bfloat16& a) { return bfloat16(::roundf(float(a))); }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmod(const bfloat16& a, const bfloat16& b) {
   return bfloat16(::fmodf(float(a), float(b)));
 }
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 (min)(const bfloat16& a, const bfloat16& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16(min)(const bfloat16& a, const bfloat16& b) {
   const float f1 = static_cast<float>(a);
   const float f2 = static_cast<float>(b);
   return f2 < f1 ? b : a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 (max)(const bfloat16& a, const bfloat16& b) {
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16(max)(const bfloat16& a, const bfloat16& b) {
   const float f1 = static_cast<float>(a);
   const float f2 = static_cast<float>(b);
   return f1 < f2 ? b : a;
@@ -584,6 +658,7 @@
   const float f2 = static_cast<float>(b);
   return bfloat16(::fminf(f1, f2));
 }
+
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmax(const bfloat16& a, const bfloat16& b) {
   const float f1 = static_cast<float>(a);
   const float f2 = static_cast<float>(b);
@@ -591,49 +666,40 @@
 }
 
 #ifndef EIGEN_NO_IO
-EIGEN_ALWAYS_INLINE std::ostream& operator << (std::ostream& os, const bfloat16& v) {
+EIGEN_ALWAYS_INLINE std::ostream& operator<<(std::ostream& os, const bfloat16& v) {
   os << static_cast<float>(v);
   return os;
 }
 #endif
 
-} // namespace bfloat16_impl
+}  // namespace bfloat16_impl
 
 namespace internal {
 
-template<>
-struct random_default_impl<bfloat16, false, false>
-{
-  static inline bfloat16 run(const bfloat16& x, const bfloat16& y)
-  {
-    return x + (y-x) * bfloat16(float(std::rand()) / float(RAND_MAX));
+template <>
+struct random_default_impl<bfloat16, false, false> {
+  static inline bfloat16 run(const bfloat16& x, const bfloat16& y) {
+    return x + (y - x) * bfloat16(float(std::rand()) / float(RAND_MAX));
   }
-  static inline bfloat16 run()
-  {
-    return run(bfloat16(-1.f), bfloat16(1.f));
-  }
+  static inline bfloat16 run() { return run(bfloat16(-1.f), bfloat16(1.f)); }
 };
 
-template<> struct is_arithmetic<bfloat16> { enum { value = true }; };
+template <>
+struct is_arithmetic<bfloat16> {
+  enum { value = true };
+};
 
-} // namespace internal
+}  // namespace internal
 
-template<> struct NumTraits<Eigen::bfloat16>
-    : GenericNumTraits<Eigen::bfloat16>
-{
-  enum {
-    IsSigned = true,
-    IsInteger = false,
-    IsComplex = false,
-    RequireInitialization = false
-  };
+template <>
+struct NumTraits<Eigen::bfloat16> : GenericNumTraits<Eigen::bfloat16> {
+  enum { IsSigned = true, IsInteger = false, IsComplex = false, RequireInitialization = false };
 
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 epsilon() {
     return bfloat16_impl::raw_uint16_to_bfloat16(0x3c00);
   }
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 dummy_precision() {
     return bfloat16_impl::raw_uint16_to_bfloat16(0x3D4D);  // bfloat16(5e-2f);
-
   }
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 highest() {
     return bfloat16_impl::raw_uint16_to_bfloat16(0x7F7F);
@@ -649,32 +715,33 @@
   }
 };
 
-} // namespace Eigen
+}  // namespace Eigen
+
+#if defined(EIGEN_HAS_HIP_BF16)
+#pragma pop_macro("EIGEN_CONSTEXPR")
+#endif
 
 namespace Eigen {
 namespace numext {
 
-template<>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-bool (isnan)(const Eigen::bfloat16& h) {
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool(isnan)(const Eigen::bfloat16& h) {
   return (bfloat16_impl::isnan)(h);
 }
 
-template<>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-bool (isinf)(const Eigen::bfloat16& h) {
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool(isinf)(const Eigen::bfloat16& h) {
   return (bfloat16_impl::isinf)(h);
 }
 
-template<>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-bool (isfinite)(const Eigen::bfloat16& h) {
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool(isfinite)(const Eigen::bfloat16& h) {
   return (bfloat16_impl::isfinite)(h);
 }
 
 template <>
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::bfloat16 bit_cast<Eigen::bfloat16, uint16_t>(const uint16_t& src) {
-  return Eigen::bfloat16(Eigen::bfloat16_impl::raw_uint16_to_bfloat16(src));
+  return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(src);
 }
 
 template <>
@@ -693,8 +760,57 @@
     return static_cast<std::size_t>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(a));
   }
 };
-} // namespace std
+}  // namespace std
 #endif
 
+// Add the missing shfl* intrinsics.
+// The __shfl* functions are only valid on HIP or _CUDA_ARCH_ >= 300.
+//   CUDA defines them for (__CUDA_ARCH__ >= 300 || !defined(__CUDA_ARCH__))
+//
+// HIP and CUDA prior to SDK 9.0 define
+//    __shfl, __shfl_up, __shfl_down, __shfl_xor for int and float
+// CUDA since 9.0 deprecates those and instead defines
+//    __shfl_sync, __shfl_up_sync, __shfl_down_sync, __shfl_xor_sync,
+//    with native support for __half and __nv_bfloat16
+//
+// Note that the following are __device__ - only functions.
+#if defined(EIGEN_HIPCC)
 
-#endif // EIGEN_BFLOAT16_H
+#if defined(EIGEN_HAS_HIP_BF16)
+
+__device__ EIGEN_STRONG_INLINE Eigen::bfloat16 __shfl(Eigen::bfloat16 var, int srcLane, int width = warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::bfloat16>(static_cast<Eigen::numext::uint16_t>(__shfl(ivar, srcLane, width)));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::bfloat16 __shfl_up(Eigen::bfloat16 var, unsigned int delta,
+                                                         int width = warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::bfloat16>(static_cast<Eigen::numext::uint16_t>(__shfl_up(ivar, delta, width)));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::bfloat16 __shfl_down(Eigen::bfloat16 var, unsigned int delta,
+                                                           int width = warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::bfloat16>(
+      static_cast<Eigen::numext::uint16_t>(__shfl_down(ivar, delta, width)));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::bfloat16 __shfl_xor(Eigen::bfloat16 var, int laneMask, int width = warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::bfloat16>(
+      static_cast<Eigen::numext::uint16_t>(__shfl_xor(ivar, laneMask, width)));
+}
+
+#endif  // HIP
+
+#endif  // __shfl*
+
+#if defined(EIGEN_HIPCC)
+EIGEN_STRONG_INLINE __device__ Eigen::bfloat16 __ldg(const Eigen::bfloat16* ptr) {
+  return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(
+      __ldg(Eigen::numext::bit_cast<const Eigen::numext::uint16_t*>(ptr)));
+}
+#endif  // __ldg
+
+#endif  // EIGEN_BFLOAT16_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h
index 53830b5..fd7923e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h
@@ -11,104 +11,115 @@
 #ifndef EIGEN_ARCH_CONJ_HELPER_H
 #define EIGEN_ARCH_CONJ_HELPER_H
 
-#define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL)      \
-  template <>                                                           \
-  struct conj_helper<PACKET_REAL, PACKET_CPLX, false, false> {          \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x,         \
-                                          const PACKET_CPLX& y,         \
-                                          const PACKET_CPLX& c) const { \
-      return padd(c, this->pmul(x, y));                                 \
-    }                                                                   \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x,          \
-                                         const PACKET_CPLX& y) const {  \
-      return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x, y.v));   \
-    }                                                                   \
-  };                                                                    \
-                                                                        \
-  template <>                                                           \
-  struct conj_helper<PACKET_CPLX, PACKET_REAL, false, false> {          \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x,         \
-                                          const PACKET_REAL& y,         \
-                                          const PACKET_CPLX& c) const { \
-      return padd(c, this->pmul(x, y));                                 \
-    }                                                                   \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x,          \
-                                         const PACKET_REAL& y) const {  \
-      return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x.v, y));   \
-    }                                                                   \
+#define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL)                                                  \
+  template <>                                                                                                       \
+  struct conj_helper<PACKET_REAL, PACKET_CPLX, false, false> {                                                      \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x, const PACKET_CPLX& y, const PACKET_CPLX& c) const { \
+      return padd(c, this->pmul(x, y));                                                                             \
+    }                                                                                                               \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x, const PACKET_CPLX& y) const {                        \
+      return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x, y.v));                                               \
+    }                                                                                                               \
+  };                                                                                                                \
+                                                                                                                    \
+  template <>                                                                                                       \
+  struct conj_helper<PACKET_CPLX, PACKET_REAL, false, false> {                                                      \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x, const PACKET_REAL& y, const PACKET_CPLX& c) const { \
+      return padd(c, this->pmul(x, y));                                                                             \
+    }                                                                                                               \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x, const PACKET_REAL& y) const {                        \
+      return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x.v, y));                                               \
+    }                                                                                                               \
   };
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
-template<bool Conjugate> struct conj_if;
+template <bool Conjugate>
+struct conj_if;
 
-template<> struct conj_if<true> {
-  template<typename T>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator()(const T& x) const { return numext::conj(x); }
-  template<typename T>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T pconj(const T& x) const { return internal::pconj(x); }
+template <>
+struct conj_if<true> {
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator()(const T& x) const {
+    return numext::conj(x);
+  }
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T pconj(const T& x) const {
+    return internal::pconj(x);
+  }
 };
 
-template<> struct conj_if<false> {
-  template<typename T>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& operator()(const T& x) const { return x; }
-  template<typename T>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& pconj(const T& x) const { return x; }
+template <>
+struct conj_if<false> {
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& operator()(const T& x) const {
+    return x;
+  }
+  template <typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& pconj(const T& x) const {
+    return x;
+  }
 };
 
 // Generic Implementation, assume scalars since the packet-version is
 // specialized below.
-template<typename LhsType, typename RhsType, bool ConjLhs, bool ConjRhs>
+template <typename LhsType, typename RhsType, bool ConjLhs, bool ConjRhs>
 struct conj_helper {
   typedef typename ScalarBinaryOpTraits<LhsType, RhsType>::ReturnType ResultType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
-  pmadd(const LhsType& x, const RhsType& y, const ResultType& c) const
-  { return this->pmul(x, y) + c; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType pmadd(const LhsType& x, const RhsType& y,
+                                                         const ResultType& c) const {
+    return this->pmul(x, y) + c;
+  }
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
-  pmul(const LhsType& x, const RhsType& y) const
-  { return conj_if<ConjLhs>()(x) * conj_if<ConjRhs>()(y); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType pmul(const LhsType& x, const RhsType& y) const {
+    return conj_if<ConjLhs>()(x) * conj_if<ConjRhs>()(y);
+  }
 };
 
-template<typename LhsScalar, typename RhsScalar>
+template <typename LhsScalar, typename RhsScalar>
 struct conj_helper<LhsScalar, RhsScalar, true, true> {
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar>::ReturnType ResultType;
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResultType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
-  pmadd(const LhsScalar& x, const RhsScalar& y, const ResultType& c) const
-  { return this->pmul(x, y) + c; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType pmadd(const LhsScalar& x, const RhsScalar& y,
+                                                         const ResultType& c) const {
+    return this->pmul(x, y) + c;
+  }
 
   // We save a conjuation by using the identity conj(a)*conj(b) = conj(a*b).
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
-  pmul(const LhsScalar& x, const RhsScalar& y) const
-  { return numext::conj(x * y); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType pmul(const LhsScalar& x, const RhsScalar& y) const {
+    return numext::conj(x * y);
+  }
 };
 
 // Implementation with equal type, use packet operations.
-template<typename Packet, bool ConjLhs, bool ConjRhs>
-struct conj_helper<Packet, Packet, ConjLhs, ConjRhs>
-{
+template <typename Packet, bool ConjLhs, bool ConjRhs>
+struct conj_helper<Packet, Packet, ConjLhs, ConjRhs> {
   typedef Packet ResultType;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmadd(const Packet& x, const Packet& y, const Packet& c) const
-  { return Eigen::internal::pmadd(conj_if<ConjLhs>().pconj(x), conj_if<ConjRhs>().pconj(y), c); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmadd(const Packet& x, const Packet& y, const Packet& c) const {
+    return Eigen::internal::pmadd(conj_if<ConjLhs>().pconj(x), conj_if<ConjRhs>().pconj(y), c);
+  }
 
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmul(const Packet& x, const Packet& y) const
-  { return Eigen::internal::pmul(conj_if<ConjLhs>().pconj(x), conj_if<ConjRhs>().pconj(y)); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmul(const Packet& x, const Packet& y) const {
+    return Eigen::internal::pmul(conj_if<ConjLhs>().pconj(x), conj_if<ConjRhs>().pconj(y));
+  }
 };
 
-template<typename Packet>
-struct conj_helper<Packet, Packet, true, true>
-{
+template <typename Packet>
+struct conj_helper<Packet, Packet, true, true> {
   typedef Packet ResultType;
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmadd(const Packet& x, const Packet& y, const Packet& c) const
-  { return Eigen::internal::pmadd(pconj(x), pconj(y), c); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmadd(const Packet& x, const Packet& y, const Packet& c) const {
+    return Eigen::internal::pmadd(pconj(x), pconj(y), c);
+  }
   // We save a conjuation by using the identity conj(a)*conj(b) = conj(a*b).
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmul(const Packet& x, const Packet& y) const
-  { return pconj(Eigen::internal::pmul(x, y)); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmul(const Packet& x, const Packet& y) const {
+    return pconj(Eigen::internal::pmul(x, y));
+  }
 };
 
 }  // namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
index c9fbaf6..8fb5b68 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
@@ -16,73 +16,86 @@
 #ifndef EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_H
 #define EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
 // Creates a Scalar integer type with same bit-width.
-template<typename T> struct make_integer;
-template<> struct make_integer<float>    { typedef numext::int32_t type; };
-template<> struct make_integer<double>   { typedef numext::int64_t type; };
-template<> struct make_integer<half>     { typedef numext::int16_t type; };
-template<> struct make_integer<bfloat16> { typedef numext::int16_t type; };
+template <typename T>
+struct make_integer;
+template <>
+struct make_integer<float> {
+  typedef numext::int32_t type;
+};
+template <>
+struct make_integer<double> {
+  typedef numext::int64_t type;
+};
+template <>
+struct make_integer<half> {
+  typedef numext::int16_t type;
+};
+template <>
+struct make_integer<bfloat16> {
+  typedef numext::int16_t type;
+};
 
-template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC  
-Packet pfrexp_generic_get_biased_exponent(const Packet& a) {
+template <typename Packet>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pfrexp_generic_get_biased_exponent(const Packet& a) {
   typedef typename unpacket_traits<Packet>::type Scalar;
   typedef typename unpacket_traits<Packet>::integer_packet PacketI;
-  enum { mantissa_bits = numext::numeric_limits<Scalar>::digits - 1};
+  static constexpr int mantissa_bits = numext::numeric_limits<Scalar>::digits - 1;
   return pcast<PacketI, Packet>(plogical_shift_right<mantissa_bits>(preinterpret<PacketI>(pabs(a))));
 }
 
 // Safely applies frexp, correctly handles denormals.
 // Assumes IEEE floating point format.
-template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-Packet pfrexp_generic(const Packet& a, Packet& exponent) {
+template <typename Packet>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pfrexp_generic(const Packet& a, Packet& exponent) {
   typedef typename unpacket_traits<Packet>::type Scalar;
   typedef typename make_unsigned<typename make_integer<Scalar>::type>::type ScalarUI;
-  enum {
-    TotalBits = sizeof(Scalar) * CHAR_BIT,
-    MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
-    ExponentBits = int(TotalBits) - int(MantissaBits) - 1
-  };
+  static constexpr int TotalBits = sizeof(Scalar) * CHAR_BIT, MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
+                       ExponentBits = TotalBits - MantissaBits - 1;
 
-  EIGEN_CONSTEXPR ScalarUI scalar_sign_mantissa_mask = 
-      ~(((ScalarUI(1) << int(ExponentBits)) - ScalarUI(1)) << int(MantissaBits)); // ~0x7f800000
-  const Packet sign_mantissa_mask = pset1frombits<Packet>(static_cast<ScalarUI>(scalar_sign_mantissa_mask)); 
+  EIGEN_CONSTEXPR ScalarUI scalar_sign_mantissa_mask =
+      ~(((ScalarUI(1) << ExponentBits) - ScalarUI(1)) << MantissaBits);  // ~0x7f800000
+  const Packet sign_mantissa_mask = pset1frombits<Packet>(static_cast<ScalarUI>(scalar_sign_mantissa_mask));
   const Packet half = pset1<Packet>(Scalar(0.5));
   const Packet zero = pzero(a);
-  const Packet normal_min = pset1<Packet>((numext::numeric_limits<Scalar>::min)()); // Minimum normal value, 2^-126
-  
+  const Packet normal_min = pset1<Packet>((numext::numeric_limits<Scalar>::min)());  // Minimum normal value, 2^-126
+
   // To handle denormals, normalize by multiplying by 2^(int(MantissaBits)+1).
   const Packet is_denormal = pcmp_lt(pabs(a), normal_min);
-  EIGEN_CONSTEXPR ScalarUI scalar_normalization_offset = ScalarUI(int(MantissaBits) + 1); // 24
+  EIGEN_CONSTEXPR ScalarUI scalar_normalization_offset = ScalarUI(MantissaBits + 1);  // 24
   // The following cannot be constexpr because bfloat16(uint16_t) is not constexpr.
-  const Scalar scalar_normalization_factor = Scalar(ScalarUI(1) << int(scalar_normalization_offset)); // 2^24
-  const Packet normalization_factor = pset1<Packet>(scalar_normalization_factor);  
+  const Scalar scalar_normalization_factor = Scalar(ScalarUI(1) << int(scalar_normalization_offset));  // 2^24
+  const Packet normalization_factor = pset1<Packet>(scalar_normalization_factor);
   const Packet normalized_a = pselect(is_denormal, pmul(a, normalization_factor), a);
-  
+
   // Determine exponent offset: -126 if normal, -126-24 if denormal
-  const Scalar scalar_exponent_offset = -Scalar((ScalarUI(1)<<(int(ExponentBits)-1)) - ScalarUI(2)); // -126
+  const Scalar scalar_exponent_offset = -Scalar((ScalarUI(1) << (ExponentBits - 1)) - ScalarUI(2));  // -126
   Packet exponent_offset = pset1<Packet>(scalar_exponent_offset);
-  const Packet normalization_offset = pset1<Packet>(-Scalar(scalar_normalization_offset)); // -24
+  const Packet normalization_offset = pset1<Packet>(-Scalar(scalar_normalization_offset));  // -24
   exponent_offset = pselect(is_denormal, padd(exponent_offset, normalization_offset), exponent_offset);
-  
+
   // Determine exponent and mantissa from normalized_a.
   exponent = pfrexp_generic_get_biased_exponent(normalized_a);
   // Zero, Inf and NaN return 'a' unmodified, exponent is zero
   // (technically the exponent is unspecified for inf/NaN, but GCC/Clang set it to zero)
-  const Scalar scalar_non_finite_exponent = Scalar((ScalarUI(1) << int(ExponentBits)) - ScalarUI(1));  // 255
+  const Scalar scalar_non_finite_exponent = Scalar((ScalarUI(1) << ExponentBits) - ScalarUI(1));  // 255
   const Packet non_finite_exponent = pset1<Packet>(scalar_non_finite_exponent);
   const Packet is_zero_or_not_finite = por(pcmp_eq(a, zero), pcmp_eq(exponent, non_finite_exponent));
   const Packet m = pselect(is_zero_or_not_finite, a, por(pand(normalized_a, sign_mantissa_mask), half));
-  exponent = pselect(is_zero_or_not_finite, zero, padd(exponent, exponent_offset));  
+  exponent = pselect(is_zero_or_not_finite, zero, padd(exponent, exponent_offset));
   return m;
 }
 
 // Safely applies ldexp, correctly handles overflows, underflows and denormals.
 // Assumes IEEE floating point format.
-template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-Packet pldexp_generic(const Packet& a, const Packet& exponent) {
+template <typename Packet>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pldexp_generic(const Packet& a, const Packet& exponent) {
   // We want to return a * 2^exponent, allowing for all possible integer
   // exponents without overflowing or underflowing in intermediate
   // computations.
@@ -91,7 +104,7 @@
   // to consider for a float is:
   //   -255-23 -> 255+23
   // Below -278 any finite float 'a' will become zero, and above +278 any
-  // finite float will become inf, including when 'a' is the smallest possible 
+  // finite float will become inf, including when 'a' is the smallest possible
   // denormal.
   //
   // Unfortunately, 2^(278) cannot be represented using either one or two
@@ -108,25 +121,22 @@
   typedef typename unpacket_traits<Packet>::integer_packet PacketI;
   typedef typename unpacket_traits<Packet>::type Scalar;
   typedef typename unpacket_traits<PacketI>::type ScalarI;
-  enum {
-    TotalBits = sizeof(Scalar) * CHAR_BIT,
-    MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
-    ExponentBits = int(TotalBits) - int(MantissaBits) - 1
-  };
+  static constexpr int TotalBits = sizeof(Scalar) * CHAR_BIT, MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
+                       ExponentBits = TotalBits - MantissaBits - 1;
 
-  const Packet max_exponent = pset1<Packet>(Scalar((ScalarI(1)<<int(ExponentBits)) + ScalarI(int(MantissaBits) - 1)));  // 278
-  const PacketI bias = pset1<PacketI>((ScalarI(1)<<(int(ExponentBits)-1)) - ScalarI(1));  // 127
+  const Packet max_exponent = pset1<Packet>(Scalar((ScalarI(1) << ExponentBits) + ScalarI(MantissaBits - 1)));  // 278
+  const PacketI bias = pset1<PacketI>((ScalarI(1) << (ExponentBits - 1)) - ScalarI(1));                         // 127
   const PacketI e = pcast<Packet, PacketI>(pmin(pmax(exponent, pnegate(max_exponent)), max_exponent));
-  PacketI b = parithmetic_shift_right<2>(e); // floor(e/4);
-  Packet c = preinterpret<Packet>(plogical_shift_left<int(MantissaBits)>(padd(b, bias)));  // 2^b
-  Packet out = pmul(pmul(pmul(a, c), c), c);  // a * 2^(3b)
-  b = psub(psub(psub(e, b), b), b); // e - 3b
-  c = preinterpret<Packet>(plogical_shift_left<int(MantissaBits)>(padd(b, bias)));  // 2^(e-3*b)
+  PacketI b = parithmetic_shift_right<2>(e);                                          // floor(e/4);
+  Packet c = preinterpret<Packet>(plogical_shift_left<MantissaBits>(padd(b, bias)));  // 2^b
+  Packet out = pmul(pmul(pmul(a, c), c), c);                                          // a * 2^(3b)
+  b = psub(psub(psub(e, b), b), b);                                                   // e - 3b
+  c = preinterpret<Packet>(plogical_shift_left<MantissaBits>(padd(b, bias)));         // 2^(e-3*b)
   out = pmul(out, c);
   return out;
 }
 
-// Explicitly multiplies 
+// Explicitly multiplies
 //    a * (2^e)
 // clamping e to the range
 // [NumTraits<Scalar>::min_exponent()-2, NumTraits<Scalar>::max_exponent()]
@@ -135,25 +145,21 @@
 // if 2^e doesn't fit into a normal floating-point Scalar.
 //
 // Assumes IEEE floating point format
-template<typename Packet>
+template <typename Packet>
 struct pldexp_fast_impl {
   typedef typename unpacket_traits<Packet>::integer_packet PacketI;
   typedef typename unpacket_traits<Packet>::type Scalar;
   typedef typename unpacket_traits<PacketI>::type ScalarI;
-  enum {
-    TotalBits = sizeof(Scalar) * CHAR_BIT,
-    MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
-    ExponentBits = int(TotalBits) - int(MantissaBits) - 1
-  };
-  
-  static EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-  Packet run(const Packet& a, const Packet& exponent) {
-    const Packet bias = pset1<Packet>(Scalar((ScalarI(1)<<(int(ExponentBits)-1)) - ScalarI(1)));  // 127
-    const Packet limit = pset1<Packet>(Scalar((ScalarI(1)<<int(ExponentBits)) - ScalarI(1)));     // 255
+  static constexpr int TotalBits = sizeof(Scalar) * CHAR_BIT, MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
+                       ExponentBits = TotalBits - MantissaBits - 1;
+
+  static EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet run(const Packet& a, const Packet& exponent) {
+    const Packet bias = pset1<Packet>(Scalar((ScalarI(1) << (ExponentBits - 1)) - ScalarI(1)));  // 127
+    const Packet limit = pset1<Packet>(Scalar((ScalarI(1) << ExponentBits) - ScalarI(1)));       // 255
     // restrict biased exponent between 0 and 255 for float.
-    const PacketI e = pcast<Packet, PacketI>(pmin(pmax(padd(exponent, bias), pzero(limit)), limit)); // exponent + 127
+    const PacketI e = pcast<Packet, PacketI>(pmin(pmax(padd(exponent, bias), pzero(limit)), limit));  // exponent + 127
     // return a * (2^e)
-    return pmul(a, preinterpret<Packet>(plogical_shift_left<int(MantissaBits)>(e)));
+    return pmul(a, preinterpret<Packet>(plogical_shift_left<MantissaBits>(e)));
   }
 };
 
@@ -164,37 +170,15 @@
 // TODO(gonnet): Further reduce the interval allowing for lower-degree
 //               polynomial interpolants -> ... -> profit!
 template <typename Packet, bool base2>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog_impl_float(const Packet _x)
-{
-  Packet x = _x;
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog_impl_float(const Packet _x) {
+  const Packet cst_1 = pset1<Packet>(1.0f);
+  const Packet cst_minus_inf = pset1frombits<Packet>(static_cast<Eigen::numext::uint32_t>(0xff800000u));
+  const Packet cst_pos_inf = pset1frombits<Packet>(static_cast<Eigen::numext::uint32_t>(0x7f800000u));
 
-  const Packet cst_1              = pset1<Packet>(1.0f);
-  const Packet cst_neg_half       = pset1<Packet>(-0.5f);
-  // The smallest non denormalized float number.
-  const Packet cst_min_norm_pos   = pset1frombits<Packet>( 0x00800000u);
-  const Packet cst_minus_inf      = pset1frombits<Packet>( 0xff800000u);
-  const Packet cst_pos_inf        = pset1frombits<Packet>( 0x7f800000u);
-
-  // Polynomial coefficients.
   const Packet cst_cephes_SQRTHF = pset1<Packet>(0.707106781186547524f);
-  const Packet cst_cephes_log_p0 = pset1<Packet>(7.0376836292E-2f);
-  const Packet cst_cephes_log_p1 = pset1<Packet>(-1.1514610310E-1f);
-  const Packet cst_cephes_log_p2 = pset1<Packet>(1.1676998740E-1f);
-  const Packet cst_cephes_log_p3 = pset1<Packet>(-1.2420140846E-1f);
-  const Packet cst_cephes_log_p4 = pset1<Packet>(+1.4249322787E-1f);
-  const Packet cst_cephes_log_p5 = pset1<Packet>(-1.6668057665E-1f);
-  const Packet cst_cephes_log_p6 = pset1<Packet>(+2.0000714765E-1f);
-  const Packet cst_cephes_log_p7 = pset1<Packet>(-2.4999993993E-1f);
-  const Packet cst_cephes_log_p8 = pset1<Packet>(+3.3333331174E-1f);
-
-  // Truncate input values to the minimum positive normal.
-  x = pmax(x, cst_min_norm_pos);
-
-  Packet e;
+  Packet e, x;
   // extract significant in the range [0.5,1) and exponent
-  x = pfrexp(x,e);
+  x = pfrexp(_x, e);
 
   // part2: Shift the inputs from the range [0.5,1) to [sqrt(1/2),sqrt(2))
   // and shift by -1. The values are then centered around 0, which improves
@@ -209,24 +193,22 @@
   e = psub(e, pand(cst_1, mask));
   x = padd(x, tmp);
 
-  Packet x2 = pmul(x, x);
-  Packet x3 = pmul(x2, x);
+  // Polynomial coefficients for rational (3,3) r(x) = p(x)/q(x)
+  // approximating log(1+x) on [sqrt(0.5)-1;sqrt(2)-1].
+  const Packet cst_p1 = pset1<Packet>(1.0000000190281136f);
+  const Packet cst_p2 = pset1<Packet>(1.0000000190281063f);
+  const Packet cst_p3 = pset1<Packet>(0.18256296349849254f);
+  const Packet cst_q1 = pset1<Packet>(1.4999999999999927f);
+  const Packet cst_q2 = pset1<Packet>(0.59923249590823520f);
+  const Packet cst_q3 = pset1<Packet>(0.049616247954120038f);
 
-  // Evaluate the polynomial approximant of degree 8 in three parts, probably
-  // to improve instruction-level parallelism.
-  Packet y, y1, y2;
-  y  = pmadd(cst_cephes_log_p0, x, cst_cephes_log_p1);
-  y1 = pmadd(cst_cephes_log_p3, x, cst_cephes_log_p4);
-  y2 = pmadd(cst_cephes_log_p6, x, cst_cephes_log_p7);
-  y  = pmadd(y, x, cst_cephes_log_p2);
-  y1 = pmadd(y1, x, cst_cephes_log_p5);
-  y2 = pmadd(y2, x, cst_cephes_log_p8);
-  y  = pmadd(y, x3, y1);
-  y  = pmadd(y, x3, y2);
-  y  = pmul(y, x3);
-
-  y = pmadd(cst_neg_half, x2, y);
-  x = padd(x, y);
+  Packet p = pmadd(x, cst_p3, cst_p2);
+  p = pmadd(x, p, cst_p1);
+  p = pmul(x, p);
+  Packet q = pmadd(x, cst_q3, cst_q2);
+  q = pmadd(x, q, cst_q1);
+  q = pmadd(x, q, cst_1);
+  x = pdiv(p, q);
 
   // Add the logarithm of the exponent back to the result of the interpolation.
   if (base2) {
@@ -238,29 +220,22 @@
   }
 
   Packet invalid_mask = pcmp_lt_or_nan(_x, pzero(_x));
-  Packet iszero_mask  = pcmp_eq(_x,pzero(_x));
-  Packet pos_inf_mask = pcmp_eq(_x,cst_pos_inf);
+  Packet iszero_mask = pcmp_eq(_x, pzero(_x));
+  Packet pos_inf_mask = pcmp_eq(_x, cst_pos_inf);
   // Filter out invalid inputs, i.e.:
   //  - negative arg will be NAN
   //  - 0 will be -INF
   //  - +INF will be +INF
-  return pselect(iszero_mask, cst_minus_inf,
-                              por(pselect(pos_inf_mask,cst_pos_inf,x), invalid_mask));
+  return pselect(iszero_mask, cst_minus_inf, por(pselect(pos_inf_mask, cst_pos_inf, x), invalid_mask));
 }
 
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog_float(const Packet _x)
-{
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog_float(const Packet _x) {
   return plog_impl_float<Packet, /* base2 */ false>(_x);
 }
 
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog2_float(const Packet _x)
-{
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog2_float(const Packet _x) {
   return plog_impl_float<Packet, /* base2 */ true>(_x);
 }
 
@@ -274,22 +249,16 @@
  * for more detail see: http://www.netlib.org/cephes/
  */
 template <typename Packet, bool base2>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog_impl_double(const Packet _x)
-{
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog_impl_double(const Packet _x) {
   Packet x = _x;
 
-  const Packet cst_1              = pset1<Packet>(1.0);
-  const Packet cst_neg_half       = pset1<Packet>(-0.5);
-  // The smallest non denormalized double.
-  const Packet cst_min_norm_pos   = pset1frombits<Packet>( static_cast<uint64_t>(0x0010000000000000ull));
-  const Packet cst_minus_inf      = pset1frombits<Packet>( static_cast<uint64_t>(0xfff0000000000000ull));
-  const Packet cst_pos_inf        = pset1frombits<Packet>( static_cast<uint64_t>(0x7ff0000000000000ull));
+  const Packet cst_1 = pset1<Packet>(1.0);
+  const Packet cst_neg_half = pset1<Packet>(-0.5);
+  const Packet cst_minus_inf = pset1frombits<Packet>(static_cast<uint64_t>(0xfff0000000000000ull));
+  const Packet cst_pos_inf = pset1frombits<Packet>(static_cast<uint64_t>(0x7ff0000000000000ull));
 
-
- // Polynomial Coefficients for log(1+x) = x - x**2/2 + x**3 P(x)/Q(x)
- //                             1/sqrt(2) <= x < sqrt(2)
+  // Polynomial Coefficients for log(1+x) = x - x**2/2 + x**3 P(x)/Q(x)
+  //                             1/sqrt(2) <= x < sqrt(2)
   const Packet cst_cephes_SQRTHF = pset1<Packet>(0.70710678118654752440E0);
   const Packet cst_cephes_log_p0 = pset1<Packet>(1.01875663804580931796E-4);
   const Packet cst_cephes_log_p1 = pset1<Packet>(4.97494994976747001425E-1);
@@ -305,13 +274,10 @@
   const Packet cst_cephes_log_q4 = pset1<Packet>(7.11544750618563894466E1);
   const Packet cst_cephes_log_q5 = pset1<Packet>(2.31251620126765340583E1);
 
-  // Truncate input values to the minimum positive normal.
-  x = pmax(x, cst_min_norm_pos);
-
   Packet e;
   // extract significant in the range [0.5,1) and exponent
-  x = pfrexp(x,e);
-  
+  x = pfrexp(x, e);
+
   // Shift the inputs from the range [0.5,1) to [sqrt(1/2),sqrt(2))
   // and shift by -1. The values are then centered around 0, which improves
   // the stability of the polynomial evaluation.
@@ -331,20 +297,20 @@
   // Evaluate the polynomial approximant , probably to improve instruction-level parallelism.
   // y = x - 0.5*x^2 + x^3 * polevl( x, P, 5 ) / p1evl( x, Q, 5 ) );
   Packet y, y1, y_;
-  y  = pmadd(cst_cephes_log_p0, x, cst_cephes_log_p1);
+  y = pmadd(cst_cephes_log_p0, x, cst_cephes_log_p1);
   y1 = pmadd(cst_cephes_log_p3, x, cst_cephes_log_p4);
-  y  = pmadd(y, x, cst_cephes_log_p2);
+  y = pmadd(y, x, cst_cephes_log_p2);
   y1 = pmadd(y1, x, cst_cephes_log_p5);
   y_ = pmadd(y, x3, y1);
 
-  y  = pmadd(cst_cephes_log_q0, x, cst_cephes_log_q1);
+  y = pmadd(cst_cephes_log_q0, x, cst_cephes_log_q1);
   y1 = pmadd(cst_cephes_log_q3, x, cst_cephes_log_q4);
-  y  = pmadd(y, x, cst_cephes_log_q2);
+  y = pmadd(y, x, cst_cephes_log_q2);
   y1 = pmadd(y1, x, cst_cephes_log_q5);
-  y  = pmadd(y, x3, y1);
+  y = pmadd(y, x3, y1);
 
   y_ = pmul(y_, x3);
-  y  = pdiv(y_, y);
+  y = pdiv(y_, y);
 
   y = pmadd(cst_neg_half, x2, y);
   x = padd(x, y);
@@ -359,38 +325,30 @@
   }
 
   Packet invalid_mask = pcmp_lt_or_nan(_x, pzero(_x));
-  Packet iszero_mask  = pcmp_eq(_x,pzero(_x));
-  Packet pos_inf_mask = pcmp_eq(_x,cst_pos_inf);
+  Packet iszero_mask = pcmp_eq(_x, pzero(_x));
+  Packet pos_inf_mask = pcmp_eq(_x, cst_pos_inf);
   // Filter out invalid inputs, i.e.:
   //  - negative arg will be NAN
   //  - 0 will be -INF
   //  - +INF will be +INF
-  return pselect(iszero_mask, cst_minus_inf,
-                              por(pselect(pos_inf_mask,cst_pos_inf,x), invalid_mask));
+  return pselect(iszero_mask, cst_minus_inf, por(pselect(pos_inf_mask, cst_pos_inf, x), invalid_mask));
 }
 
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog_double(const Packet _x)
-{
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog_double(const Packet _x) {
   return plog_impl_double<Packet, /* base2 */ false>(_x);
 }
 
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog2_double(const Packet _x)
-{
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog2_double(const Packet _x) {
   return plog_impl_double<Packet, /* base2 */ true>(_x);
 }
 
 /** \internal \returns log(1 + x) computed using W. Kahan's formula.
     See: http://www.plunk.org/~hatch/rightway.php
  */
-template<typename Packet>
-Packet generic_plog1p(const Packet& x)
-{
+template <typename Packet>
+Packet generic_plog1p(const Packet& x) {
   typedef typename unpacket_traits<Packet>::type ScalarType;
   const Packet one = pset1<Packet>(ScalarType(1));
   Packet xp1 = padd(x, one);
@@ -404,9 +362,8 @@
 /** \internal \returns exp(x)-1 computed using W. Kahan's formula.
     See: http://www.plunk.org/~hatch/rightway.php
  */
-template<typename Packet>
-Packet generic_expm1(const Packet& x)
-{
+template <typename Packet>
+Packet generic_expm1(const Packet& x) {
   typedef typename unpacket_traits<Packet>::type ScalarType;
   const Packet one = pset1<Packet>(ScalarType(1));
   const Packet neg_one = pset1<Packet>(ScalarType(-1));
@@ -422,37 +379,31 @@
   Packet pos_inf_mask = pcmp_eq(logu, u);
   Packet expm1 = pmul(u_minus_one, pdiv(x, logu));
   expm1 = pselect(pos_inf_mask, u, expm1);
-  return pselect(one_mask,
-                 x,
-                 pselect(neg_one_mask,
-                         neg_one,
-                         expm1));
+  return pselect(one_mask, x, pselect(neg_one_mask, neg_one, expm1));
 }
 
-
 // Exponential function. Works by writing "x = m*log(2) + r" where
 // "m = floor(x/log(2)+1/2)" and "r" is the remainder. The result is then
 // "exp(x) = 2^m*exp(r)" where exp(r) is in the range [-1,1).
+// exp(r) is computed using a 6th order minimax polynomial approximation.
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet pexp_float(const Packet _x)
-{
-  const Packet cst_1      = pset1<Packet>(1.0f);
-  const Packet cst_half   = pset1<Packet>(0.5f);
-  const Packet cst_exp_hi = pset1<Packet>( 88.723f);
-  const Packet cst_exp_lo = pset1<Packet>(-88.723f);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_float(const Packet _x) {
+  const Packet cst_zero = pset1<Packet>(0.0f);
+  const Packet cst_one = pset1<Packet>(1.0f);
+  const Packet cst_half = pset1<Packet>(0.5f);
+  const Packet cst_exp_hi = pset1<Packet>(88.723f);
+  const Packet cst_exp_lo = pset1<Packet>(-104.f);
 
   const Packet cst_cephes_LOG2EF = pset1<Packet>(1.44269504088896341f);
-  const Packet cst_cephes_exp_p0 = pset1<Packet>(1.9875691500E-4f);
-  const Packet cst_cephes_exp_p1 = pset1<Packet>(1.3981999507E-3f);
-  const Packet cst_cephes_exp_p2 = pset1<Packet>(8.3334519073E-3f);
-  const Packet cst_cephes_exp_p3 = pset1<Packet>(4.1665795894E-2f);
-  const Packet cst_cephes_exp_p4 = pset1<Packet>(1.6666665459E-1f);
-  const Packet cst_cephes_exp_p5 = pset1<Packet>(5.0000001201E-1f);
+  const Packet cst_p2 = pset1<Packet>(0.49999988079071044921875f);
+  const Packet cst_p3 = pset1<Packet>(0.16666518151760101318359375f);
+  const Packet cst_p4 = pset1<Packet>(4.166965186595916748046875e-2f);
+  const Packet cst_p5 = pset1<Packet>(8.36894474923610687255859375e-3f);
+  const Packet cst_p6 = pset1<Packet>(1.37449637986719608306884765625e-3f);
 
   // Clamp x.
-  Packet x = pmax(pmin(_x, cst_exp_hi), cst_exp_lo);
+  Packet zero_mask = pcmp_lt(_x, cst_exp_lo);
+  Packet x = pmin(_x, cst_exp_hi);
 
   // Express exp(x) as exp(m*ln(2) + r), start by extracting
   // m = floor(x/ln(2) + 0.5).
@@ -466,31 +417,25 @@
   Packet r = pmadd(m, cst_cephes_exp_C1, x);
   r = pmadd(m, cst_cephes_exp_C2, r);
 
-  Packet r2 = pmul(r, r);
-  Packet r3 = pmul(r2, r);
-
-  // Evaluate the polynomial approximant,improved by instruction-level parallelism.
-  Packet y, y1, y2;
-  y  = pmadd(cst_cephes_exp_p0, r, cst_cephes_exp_p1);
-  y1 = pmadd(cst_cephes_exp_p3, r, cst_cephes_exp_p4);
-  y2 = padd(r, cst_1);
-  y  = pmadd(y, r, cst_cephes_exp_p2);
-  y1 = pmadd(y1, r, cst_cephes_exp_p5);
-  y  = pmadd(y, r3, y1);
-  y  = pmadd(y, r2, y2);
+  // Evaluate the 6th order polynomial approximation to exp(r)
+  // with r in the interval [-ln(2)/2;ln(2)/2].
+  const Packet r2 = pmul(r, r);
+  Packet p_even = pmadd(r2, cst_p6, cst_p4);
+  const Packet p_odd = pmadd(r2, cst_p5, cst_p3);
+  p_even = pmadd(r2, p_even, cst_p2);
+  const Packet p_low = padd(r, cst_one);
+  Packet y = pmadd(r, p_odd, p_even);
+  y = pmadd(r2, y, p_low);
 
   // Return 2^m * exp(r).
   // TODO: replace pldexp with faster implementation since y in [-1, 1).
-  return pmax(pldexp(y,m), _x);
+  return pselect(zero_mask, cst_zero, pmax(pldexp(y, m), _x));
 }
 
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet pexp_double(const Packet _x)
-{
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_double(const Packet _x) {
   Packet x = _x;
-
+  const Packet cst_zero = pset1<Packet>(0.0);
   const Packet cst_1 = pset1<Packet>(1.0);
   const Packet cst_2 = pset1<Packet>(2.0);
   const Packet cst_half = pset1<Packet>(0.5);
@@ -512,7 +457,8 @@
   Packet tmp, fx;
 
   // clamp x
-  x = pmax(pmin(x, cst_exp_hi), cst_exp_lo);
+  Packet zero_mask = pcmp_lt(_x, cst_exp_lo);
+  x = pmin(x, cst_exp_hi);
   // Express exp(x) as exp(g + n*log(2)).
   fx = pmadd(cst_cephes_LOG2EF, x, cst_half);
 
@@ -550,7 +496,7 @@
   // Construct the result 2^n * exp(g) = e * x. The max is used to catch
   // non-finite values in the input.
   // TODO: replace pldexp with faster implementation since x in [-1, 1).
-  return pmax(pldexp(x,fx), _x);
+  return pselect(zero_mask, cst_zero, pmax(pldexp(x, fx), _x));
 }
 
 // The following code is inspired by the following stack-overflow answer:
@@ -562,29 +508,22 @@
 //    aligned on 8-bits, and (2) replicating the storage of the bits of 2/pi.
 //  - Avoid a branch in rounding and extraction of the remaining fractional part.
 // Overall, I measured a speed up higher than x2 on x86-64.
-inline float trig_reduce_huge (float xf, int *quadrant)
-{
+inline float trig_reduce_huge(float xf, Eigen::numext::int32_t* quadrant) {
   using Eigen::numext::int32_t;
-  using Eigen::numext::uint32_t;
   using Eigen::numext::int64_t;
+  using Eigen::numext::uint32_t;
   using Eigen::numext::uint64_t;
 
-  const double pio2_62 = 3.4061215800865545e-19;    // pi/2 * 2^-62
-  const uint64_t zero_dot_five = uint64_t(1) << 61; // 0.5 in 2.62-bit fixed-point foramt
+  const double pio2_62 = 3.4061215800865545e-19;     // pi/2 * 2^-62
+  const uint64_t zero_dot_five = uint64_t(1) << 61;  // 0.5 in 2.62-bit fixed-point format
 
   // 192 bits of 2/pi for Payne-Hanek reduction
   // Bits are introduced by packet of 8 to enable aligned reads.
-  static const uint32_t two_over_pi [] = 
-  {
-    0x00000028, 0x000028be, 0x0028be60, 0x28be60db,
-    0xbe60db93, 0x60db9391, 0xdb939105, 0x9391054a,
-    0x91054a7f, 0x054a7f09, 0x4a7f09d5, 0x7f09d5f4,
-    0x09d5f47d, 0xd5f47d4d, 0xf47d4d37, 0x7d4d3770,
-    0x4d377036, 0x377036d8, 0x7036d8a5, 0x36d8a566,
-    0xd8a5664f, 0xa5664f10, 0x664f10e4, 0x4f10e410,
-    0x10e41000, 0xe4100000
-  };
-  
+  static const uint32_t two_over_pi[] = {
+      0x00000028, 0x000028be, 0x0028be60, 0x28be60db, 0xbe60db93, 0x60db9391, 0xdb939105, 0x9391054a, 0x91054a7f,
+      0x054a7f09, 0x4a7f09d5, 0x7f09d5f4, 0x09d5f47d, 0xd5f47d4d, 0xf47d4d37, 0x7d4d3770, 0x4d377036, 0x377036d8,
+      0x7036d8a5, 0x36d8a566, 0xd8a5664f, 0xa5664f10, 0x664f10e4, 0x4f10e410, 0x10e41000, 0xe4100000};
+
   uint32_t xi = numext::bit_cast<uint32_t>(xf);
   // Below, -118 = -126 + 8.
   //   -126 is to get the exponent,
@@ -592,12 +531,12 @@
   // This is possible because the fractional part of x as only 24 meaningful bits.
   uint32_t e = (xi >> 23) - 118;
   // Extract the mantissa and shift it to align it wrt the exponent
-  xi = ((xi & 0x007fffffu)| 0x00800000u) << (e & 0x7);
+  xi = ((xi & 0x007fffffu) | 0x00800000u) << (e & 0x7);
 
   uint32_t i = e >> 3;
-  uint32_t twoopi_1  = two_over_pi[i-1];
-  uint32_t twoopi_2  = two_over_pi[i+3];
-  uint32_t twoopi_3  = two_over_pi[i+7];
+  uint32_t twoopi_1 = two_over_pi[i - 1];
+  uint32_t twoopi_2 = two_over_pi[i + 3];
+  uint32_t twoopi_3 = two_over_pi[i + 7];
 
   // Compute x * 2/pi in 2.62-bit fixed-point format.
   uint64_t p;
@@ -612,46 +551,45 @@
   // since we have p=x/(pi/2) with high accuracy, we can more efficiently compute r as:
   //   r = (p-q)*pi/2,
   // where the product can be be carried out with sufficient accuracy using double precision.
-  p -= q<<62;
+  p -= q << 62;
   return float(double(int64_t(p)) * pio2_62);
 }
 
-template<bool ComputeSine,typename Packet>
+template <bool ComputeSine, typename Packet>
 EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-#if EIGEN_GNUC_AT_LEAST(4,4) && EIGEN_COMP_GNUC_STRICT
-__attribute__((optimize("-fno-unsafe-math-optimizations")))
+#if EIGEN_COMP_GNUC_STRICT
+    __attribute__((optimize("-fno-unsafe-math-optimizations")))
 #endif
-Packet psincos_float(const Packet& _x)
-{
+    Packet
+    psincos_float(const Packet& _x) {
   typedef typename unpacket_traits<Packet>::integer_packet PacketI;
 
-  const Packet  cst_2oPI            = pset1<Packet>(0.636619746685028076171875f); // 2/PI
-  const Packet  cst_rounding_magic  = pset1<Packet>(12582912); // 2^23 for rounding
-  const PacketI csti_1              = pset1<PacketI>(1);
-  const Packet  cst_sign_mask       = pset1frombits<Packet>(0x80000000u);
+  const Packet cst_2oPI = pset1<Packet>(0.636619746685028076171875f);  // 2/PI
+  const Packet cst_rounding_magic = pset1<Packet>(12582912);           // 2^23 for rounding
+  const PacketI csti_1 = pset1<PacketI>(1);
+  const Packet cst_sign_mask = pset1frombits<Packet>(static_cast<Eigen::numext::uint32_t>(0x80000000u));
 
   Packet x = pabs(_x);
 
   // Scale x by 2/Pi to find x's octant.
   Packet y = pmul(x, cst_2oPI);
 
-  // Rounding trick:
+  // Rounding trick to find nearest integer:
   Packet y_round = padd(y, cst_rounding_magic);
   EIGEN_OPTIMIZATION_BARRIER(y_round)
-  PacketI y_int = preinterpret<PacketI>(y_round); // last 23 digits represent integer (if abs(x)<2^24)
-  y = psub(y_round, cst_rounding_magic); // nearest integer to x*4/pi
+  PacketI y_int = preinterpret<PacketI>(y_round);  // last 23 digits represent integer (if abs(x)<2^24)
+  y = psub(y_round, cst_rounding_magic);           // nearest integer to x * (2/pi)
 
-  // Reduce x by y octants to get: -Pi/4 <= x <= +Pi/4
-  // using "Extended precision modular arithmetic"
-  #if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD)
+// Subtract y * Pi/2 to reduce x to the interval -Pi/4 <= x <= +Pi/4
+// using "Extended precision modular arithmetic"
+#if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD)
   // This version requires true FMA for high accuracy
   // It provides a max error of 1ULP up to (with absolute_error < 5.9605e-08):
   const float huge_th = ComputeSine ? 117435.992f : 71476.0625f;
   x = pmadd(y, pset1<Packet>(-1.57079601287841796875f), x);
   x = pmadd(y, pset1<Packet>(-3.1391647326017846353352069854736328125e-07f), x);
   x = pmadd(y, pset1<Packet>(-5.390302529957764765544681040410068817436695098876953125e-15f), x);
-  #else
+#else
   // Without true FMA, the previous set of coefficients maintain 1ULP accuracy
   // up to x<15.7 (for sin), but accuracy is immediately lost for x>15.7.
   // We thus use one more iteration to maintain 2ULPs up to reasonably large inputs.
@@ -659,41 +597,38 @@
   // The following set of coefficients maintain 1ULP up to 9.43 and 14.16 for sin and cos respectively.
   // and 2 ULP up to:
   const float huge_th = ComputeSine ? 25966.f : 18838.f;
-  x = pmadd(y, pset1<Packet>(-1.5703125), x); // = 0xbfc90000
+  x = pmadd(y, pset1<Packet>(-1.5703125), x);  // = 0xbfc90000
   EIGEN_OPTIMIZATION_BARRIER(x)
-  x = pmadd(y, pset1<Packet>(-0.000483989715576171875), x); // = 0xb9fdc000
+  x = pmadd(y, pset1<Packet>(-0.000483989715576171875), x);  // = 0xb9fdc000
   EIGEN_OPTIMIZATION_BARRIER(x)
-  x = pmadd(y, pset1<Packet>(1.62865035235881805419921875e-07), x); // = 0x342ee000
-  x = pmadd(y, pset1<Packet>(5.5644315544167710640977020375430583953857421875e-11), x); // = 0x2e74b9ee
+  x = pmadd(y, pset1<Packet>(1.62865035235881805419921875e-07), x);                      // = 0x342ee000
+  x = pmadd(y, pset1<Packet>(5.5644315544167710640977020375430583953857421875e-11), x);  // = 0x2e74b9ee
 
-  // For the record, the following set of coefficients maintain 2ULP up
-  // to a slightly larger range:
-  // const float huge_th = ComputeSine ? 51981.f : 39086.125f;
-  // but it slightly fails to maintain 1ULP for two values of sin below pi.
-  // x = pmadd(y, pset1<Packet>(-3.140625/2.), x);
-  // x = pmadd(y, pset1<Packet>(-0.00048351287841796875), x);
-  // x = pmadd(y, pset1<Packet>(-3.13855707645416259765625e-07), x);
-  // x = pmadd(y, pset1<Packet>(-6.0771006282767103812147979624569416046142578125e-11), x);
+// For the record, the following set of coefficients maintain 2ULP up
+// to a slightly larger range:
+// const float huge_th = ComputeSine ? 51981.f : 39086.125f;
+// but it slightly fails to maintain 1ULP for two values of sin below pi.
+// x = pmadd(y, pset1<Packet>(-3.140625/2.), x);
+// x = pmadd(y, pset1<Packet>(-0.00048351287841796875), x);
+// x = pmadd(y, pset1<Packet>(-3.13855707645416259765625e-07), x);
+// x = pmadd(y, pset1<Packet>(-6.0771006282767103812147979624569416046142578125e-11), x);
 
-  // For the record, with only 3 iterations it is possible to maintain
-  // 1 ULP up to 3PI (maybe more) and 2ULP up to 255.
-  // The coefficients are: 0xbfc90f80, 0xb7354480, 0x2e74b9ee
-  #endif
+// For the record, with only 3 iterations it is possible to maintain
+// 1 ULP up to 3PI (maybe more) and 2ULP up to 255.
+// The coefficients are: 0xbfc90f80, 0xb7354480, 0x2e74b9ee
+#endif
 
-  if(predux_any(pcmp_le(pset1<Packet>(huge_th),pabs(_x))))
-  {
+  if (predux_any(pcmp_le(pset1<Packet>(huge_th), pabs(_x)))) {
     const int PacketSize = unpacket_traits<Packet>::size;
     EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) float vals[PacketSize];
     EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) float x_cpy[PacketSize];
-    EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) int y_int2[PacketSize];
+    EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Eigen::numext::int32_t y_int2[PacketSize];
     pstoreu(vals, pabs(_x));
     pstoreu(x_cpy, x);
     pstoreu(y_int2, y_int);
-    for(int k=0; k<PacketSize;++k)
-    {
+    for (int k = 0; k < PacketSize; ++k) {
       float val = vals[k];
-      if(val>=huge_th && (numext::isfinite)(val))
-        x_cpy[k] = trig_reduce_huge(val,&y_int2[k]);
+      if (val >= huge_th && (numext::isfinite)(val)) x_cpy[k] = trig_reduce_huge(val, &y_int2[k]);
     }
     x = ploadu<Packet>(x_cpy);
     y_int = ploadu<PacketI>(y_int2);
@@ -703,19 +638,19 @@
   // sin: sign = second_bit(y_int) xor signbit(_x)
   // cos: sign = second_bit(y_int+1)
   Packet sign_bit = ComputeSine ? pxor(_x, preinterpret<Packet>(plogical_shift_left<30>(y_int)))
-                                : preinterpret<Packet>(plogical_shift_left<30>(padd(y_int,csti_1)));
-  sign_bit = pand(sign_bit, cst_sign_mask); // clear all but left most bit
+                                : preinterpret<Packet>(plogical_shift_left<30>(padd(y_int, csti_1)));
+  sign_bit = pand(sign_bit, cst_sign_mask);  // clear all but left most bit
 
   // Get the polynomial selection mask from the second bit of y_int
   // We'll calculate both (sin and cos) polynomials and then select from the two.
   Packet poly_mask = preinterpret<Packet>(pcmp_eq(pand(y_int, csti_1), pzero(y_int)));
 
-  Packet x2 = pmul(x,x);
+  Packet x2 = pmul(x, x);
 
   // Evaluate the cos(x) polynomial. (-Pi/4 <= x <= Pi/4)
-  Packet y1 =        pset1<Packet>(2.4372266125283204019069671630859375e-05f);
-  y1 = pmadd(y1, x2, pset1<Packet>(-0.00138865201734006404876708984375f     ));
-  y1 = pmadd(y1, x2, pset1<Packet>(0.041666619479656219482421875f           ));
+  Packet y1 = pset1<Packet>(2.4372266125283204019069671630859375e-05f);
+  y1 = pmadd(y1, x2, pset1<Packet>(-0.00138865201734006404876708984375f));
+  y1 = pmadd(y1, x2, pset1<Packet>(0.041666619479656219482421875f));
   y1 = pmadd(y1, x2, pset1<Packet>(-0.5f));
   y1 = pmadd(y1, x2, pset1<Packet>(1.f));
 
@@ -727,41 +662,310 @@
   //    c = (A'*diag(w)*A)\(A'*diag(w)*(sin(x)-x)); # weighted LS, linear coeff forced to 1
   //    printf('%.64f\n %.64f\n%.64f\n', c(3), c(2), c(1))
   //
-  Packet y2 =        pset1<Packet>(-0.0001959234114083702898469196984621021329076029360294342041015625f);
-  y2 = pmadd(y2, x2, pset1<Packet>( 0.0083326873655616851693794799871284340042620897293090820312500000f));
+  Packet y2 = pset1<Packet>(-0.0001959234114083702898469196984621021329076029360294342041015625f);
+  y2 = pmadd(y2, x2, pset1<Packet>(0.0083326873655616851693794799871284340042620897293090820312500000f));
   y2 = pmadd(y2, x2, pset1<Packet>(-0.1666666203982298255503735617821803316473960876464843750000000000f));
   y2 = pmul(y2, x2);
   y2 = pmadd(y2, x, x);
 
   // Select the correct result from the two polynomials.
-  y = ComputeSine ? pselect(poly_mask,y2,y1)
-                  : pselect(poly_mask,y1,y2);
+  y = ComputeSine ? pselect(poly_mask, y2, y1) : pselect(poly_mask, y1, y2);
 
   // Update the sign and filter huge inputs
   return pxor(y, sign_bit);
 }
 
-template<typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet psin_float(const Packet& x)
-{
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psin_float(const Packet& x) {
   return psincos_float<true>(x);
 }
 
-template<typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet pcos_float(const Packet& x)
-{
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcos_float(const Packet& x) {
   return psincos_float<false>(x);
 }
 
+// Generic implementation of acos(x).
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pacos_float(const Packet& x_in) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static_assert(std::is_same<Scalar, float>::value, "Scalar type must be float");
 
-template<typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet psqrt_complex(const Packet& a) {
+  const Packet cst_one = pset1<Packet>(Scalar(1));
+  const Packet cst_pi = pset1<Packet>(Scalar(EIGEN_PI));
+  const Packet p6 = pset1<Packet>(Scalar(2.36423197202384471893310546875e-3));
+  const Packet p5 = pset1<Packet>(Scalar(-1.1368644423782825469970703125e-2));
+  const Packet p4 = pset1<Packet>(Scalar(2.717843465507030487060546875e-2));
+  const Packet p3 = pset1<Packet>(Scalar(-4.8969544470310211181640625e-2));
+  const Packet p2 = pset1<Packet>(Scalar(8.8804088532924652099609375e-2));
+  const Packet p1 = pset1<Packet>(Scalar(-0.214591205120086669921875));
+  const Packet p0 = pset1<Packet>(Scalar(1.57079637050628662109375));
+
+  // For x in [0:1], we approximate acos(x)/sqrt(1-x), which is a smooth
+  // function, by a 6'th order polynomial.
+  // For x in [-1:0) we use that acos(-x) = pi - acos(x).
+  const Packet neg_mask = psignbit(x_in);
+  const Packet abs_x = pabs(x_in);
+
+  // Evaluate the polynomial using Horner's rule:
+  //   P(x) = p0 + x * (p1 +  x * (p2 + ... (p5 + x * p6)) ... ) .
+  // We evaluate even and odd terms independently to increase
+  // instruction level parallelism.
+  Packet x2 = pmul(x_in, x_in);
+  Packet p_even = pmadd(p6, x2, p4);
+  Packet p_odd = pmadd(p5, x2, p3);
+  p_even = pmadd(p_even, x2, p2);
+  p_odd = pmadd(p_odd, x2, p1);
+  p_even = pmadd(p_even, x2, p0);
+  Packet p = pmadd(p_odd, abs_x, p_even);
+
+  // The polynomial approximates acos(x)/sqrt(1-x), so
+  // multiply by sqrt(1-x) to get acos(x).
+  // Conveniently returns NaN for arguments outside [-1:1].
+  Packet denom = psqrt(psub(cst_one, abs_x));
+  Packet result = pmul(denom, p);
+  // Undo mapping for negative arguments.
+  return pselect(neg_mask, psub(cst_pi, result), result);
+}
+
+// Generic implementation of asin(x).
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pasin_float(const Packet& x_in) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static_assert(std::is_same<Scalar, float>::value, "Scalar type must be float");
+
+  constexpr float kPiOverTwo = static_cast<float>(EIGEN_PI / 2);
+
+  const Packet cst_half = pset1<Packet>(0.5f);
+  const Packet cst_one = pset1<Packet>(1.0f);
+  const Packet cst_two = pset1<Packet>(2.0f);
+  const Packet cst_pi_over_two = pset1<Packet>(kPiOverTwo);
+  // For |x| < 0.5 approximate asin(x)/x by an 8th order polynomial with
+  // even terms only.
+  const Packet p9 = pset1<Packet>(5.08838854730129241943359375e-2f);
+  const Packet p7 = pset1<Packet>(3.95139865577220916748046875e-2f);
+  const Packet p5 = pset1<Packet>(7.550220191478729248046875e-2f);
+  const Packet p3 = pset1<Packet>(0.16664917767047882080078125f);
+  const Packet p1 = pset1<Packet>(1.00000011920928955078125f);
+
+  const Packet abs_x = pabs(x_in);
+  const Packet sign_mask = pandnot(x_in, abs_x);
+  const Packet invalid_mask = pcmp_lt(cst_one, abs_x);
+
+  // For arguments |x| > 0.5, we map x back to [0:0.5] using
+  // the transformation x_large = sqrt(0.5*(1-x)), and use the
+  // identity
+  //   asin(x) = pi/2 - 2 * asin( sqrt( 0.5 * (1 - x)))
+
+  const Packet x_large = psqrt(pnmadd(cst_half, abs_x, cst_half));
+  const Packet large_mask = pcmp_lt(cst_half, abs_x);
+  const Packet x = pselect(large_mask, x_large, abs_x);
+  const Packet x2 = pmul(x, x);
+
+  // Compute polynomial.
+  // x * (p1 + x^2*(p3 + x^2*(p5 + x^2*(p7 + x^2*p9))))
+
+  Packet p = pmadd(p9, x2, p7);
+  p = pmadd(p, x2, p5);
+  p = pmadd(p, x2, p3);
+  p = pmadd(p, x2, p1);
+  p = pmul(p, x);
+
+  const Packet p_large = pnmadd(cst_two, p, cst_pi_over_two);
+  p = pselect(large_mask, p_large, p);
+  // Flip the sign for negative arguments.
+  p = pxor(p, sign_mask);
+  // Return NaN for arguments outside [-1:1].
+  return por(invalid_mask, p);
+}
+
+// Computes elementwise atan(x) for x in [-1:1] with 2 ulp accuracy.
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan_reduced_float(const Packet& x) {
+  const Packet q0 = pset1<Packet>(-0.3333314359188079833984375f);
+  const Packet q2 = pset1<Packet>(0.19993579387664794921875f);
+  const Packet q4 = pset1<Packet>(-0.14209578931331634521484375f);
+  const Packet q6 = pset1<Packet>(0.1066047251224517822265625f);
+  const Packet q8 = pset1<Packet>(-7.5408883392810821533203125e-2f);
+  const Packet q10 = pset1<Packet>(4.3082617223262786865234375e-2f);
+  const Packet q12 = pset1<Packet>(-1.62907354533672332763671875e-2f);
+  const Packet q14 = pset1<Packet>(2.90188402868807315826416015625e-3f);
+
+  // Approximate atan(x) by a polynomial of the form
+  //   P(x) = x + x^3 * Q(x^2),
+  // where Q(x^2) is a 7th order polynomial in x^2.
+  // We evaluate even and odd terms in x^2 in parallel
+  // to take advantage of instruction level parallelism
+  // and hardware with multiple FMA units.
+
+  // note: if x == -0, this returns +0
+  const Packet x2 = pmul(x, x);
+  const Packet x4 = pmul(x2, x2);
+  Packet q_odd = pmadd(q14, x4, q10);
+  Packet q_even = pmadd(q12, x4, q8);
+  q_odd = pmadd(q_odd, x4, q6);
+  q_even = pmadd(q_even, x4, q4);
+  q_odd = pmadd(q_odd, x4, q2);
+  q_even = pmadd(q_even, x4, q0);
+  const Packet q = pmadd(q_odd, x2, q_even);
+  return pmadd(q, pmul(x, x2), x);
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan_float(const Packet& x_in) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static_assert(std::is_same<Scalar, float>::value, "Scalar type must be float");
+
+  constexpr float kPiOverTwo = static_cast<float>(EIGEN_PI / 2);
+
+  const Packet cst_signmask = pset1<Packet>(-0.0f);
+  const Packet cst_one = pset1<Packet>(1.0f);
+  const Packet cst_pi_over_two = pset1<Packet>(kPiOverTwo);
+
+  //   "Large": For |x| > 1, use atan(1/x) = sign(x)*pi/2 - atan(x).
+  //   "Small": For |x| <= 1, approximate atan(x) directly by a polynomial
+  //            calculated using Sollya.
+
+  const Packet abs_x = pabs(x_in);
+  const Packet x_signmask = pand(x_in, cst_signmask);
+  const Packet large_mask = pcmp_lt(cst_one, abs_x);
+  const Packet x = pselect(large_mask, preciprocal(abs_x), abs_x);
+  const Packet p = patan_reduced_float(x);
+  // Apply transformations according to the range reduction masks.
+  Packet result = pselect(large_mask, psub(cst_pi_over_two, p), p);
+  // Return correct sign
+  return pxor(result, x_signmask);
+}
+
+// Computes elementwise atan(x) for x in [-tan(pi/8):tan(pi/8)]
+// with 2 ulp accuracy.
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan_reduced_double(const Packet& x) {
+  const Packet q0 = pset1<Packet>(-0.33333333333330028569463365784031338989734649658203);
+  const Packet q2 = pset1<Packet>(0.199999999990664090177006073645316064357757568359375);
+  const Packet q4 = pset1<Packet>(-0.142857141937123677255527809393242932856082916259766);
+  const Packet q6 = pset1<Packet>(0.111111065991039953404495577160560060292482376098633);
+  const Packet q8 = pset1<Packet>(-9.0907812986129224452902519715280504897236824035645e-2);
+  const Packet q10 = pset1<Packet>(7.6900542950704739442180368769186316058039665222168e-2);
+  const Packet q12 = pset1<Packet>(-6.6410112986494976294871150912513257935643196105957e-2);
+  const Packet q14 = pset1<Packet>(5.6920144995467943094258345126945641823112964630127e-2);
+  const Packet q16 = pset1<Packet>(-4.3577020814990513608577771265117917209863662719727e-2);
+  const Packet q18 = pset1<Packet>(2.1244050233624342527427586446719942614436149597168e-2);
+
+  // Approximate atan(x) on [0:tan(pi/8)] by a polynomial of the form
+  //   P(x) = x + x^3 * Q(x^2),
+  // where Q(x^2) is a 9th order polynomial in x^2.
+  // We evaluate even and odd terms in x^2 in parallel
+  // to take advantage of instruction level parallelism
+  // and hardware with multiple FMA units.
+  const Packet x2 = pmul(x, x);
+  const Packet x4 = pmul(x2, x2);
+  Packet q_odd = pmadd(q18, x4, q14);
+  Packet q_even = pmadd(q16, x4, q12);
+  q_odd = pmadd(q_odd, x4, q10);
+  q_even = pmadd(q_even, x4, q8);
+  q_odd = pmadd(q_odd, x4, q6);
+  q_even = pmadd(q_even, x4, q4);
+  q_odd = pmadd(q_odd, x4, q2);
+  q_even = pmadd(q_even, x4, q0);
+  const Packet p = pmadd(q_odd, x2, q_even);
+  return pmadd(p, pmul(x, x2), x);
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan_double(const Packet& x_in) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static_assert(std::is_same<Scalar, double>::value, "Scalar type must be double");
+
+  constexpr double kPiOverTwo = static_cast<double>(EIGEN_PI / 2);
+  constexpr double kPiOverFour = static_cast<double>(EIGEN_PI / 4);
+  constexpr double kTanPiOverEight = 0.4142135623730950488016887;
+  constexpr double kTan3PiOverEight = 2.4142135623730950488016887;
+
+  const Packet cst_signmask = pset1<Packet>(-0.0);
+  const Packet cst_one = pset1<Packet>(1.0);
+  const Packet cst_pi_over_two = pset1<Packet>(kPiOverTwo);
+  const Packet cst_pi_over_four = pset1<Packet>(kPiOverFour);
+  const Packet cst_large = pset1<Packet>(kTan3PiOverEight);
+  const Packet cst_medium = pset1<Packet>(kTanPiOverEight);
+
+  // Use the same range reduction strategy (to [0:tan(pi/8)]) as the
+  // Cephes library:
+  //   "Large": For x >= tan(3*pi/8), use atan(1/x) = pi/2 - atan(x).
+  //   "Medium": For x in [tan(pi/8) : tan(3*pi/8)),
+  //             use atan(x) = pi/4 + atan((x-1)/(x+1)).
+  //   "Small": For x < tan(pi/8), approximate atan(x) directly by a polynomial
+  //            calculated using Sollya.
+
+  const Packet abs_x = pabs(x_in);
+  const Packet x_signmask = pand(x_in, cst_signmask);
+  const Packet large_mask = pcmp_lt(cst_large, abs_x);
+  const Packet medium_mask = pandnot(pcmp_lt(cst_medium, abs_x), large_mask);
+
+  Packet x = abs_x;
+  x = pselect(large_mask, preciprocal(abs_x), x);
+  x = pselect(medium_mask, pdiv(psub(abs_x, cst_one), padd(abs_x, cst_one)), x);
+
+  // Compute approximation of p ~= atan(x') where x' is the argument reduced to
+  // [0:tan(pi/8)].
+  Packet p = patan_reduced_double(x);
+
+  // Apply transformations according to the range reduction masks.
+  p = pselect(large_mask, psub(cst_pi_over_two, p), p);
+  p = pselect(medium_mask, padd(cst_pi_over_four, p), p);
+  // Return the correct sign
+  return pxor(p, x_signmask);
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patanh_float(const Packet& x) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static_assert(std::is_same<Scalar, float>::value, "Scalar type must be float");
+  const Packet half = pset1<Packet>(0.5f);
+  const Packet x_gt_half = pcmp_le(half, pabs(x));
+  // For |x| in [0:0.5] we use a polynomial approximation of the form
+  // P(x) = x + x^3*(c3 + x^2 * (c5 + x^2 * (... x^2 * c11) ... )).
+  const Packet C3 = pset1<Packet>(0.3333373963832855224609375f);
+  const Packet C5 = pset1<Packet>(0.1997792422771453857421875f);
+  const Packet C7 = pset1<Packet>(0.14672131836414337158203125f);
+  const Packet C9 = pset1<Packet>(8.2311116158962249755859375e-2f);
+  const Packet C11 = pset1<Packet>(0.1819281280040740966796875f);
+  const Packet x2 = pmul(x, x);
+  Packet p = pmadd(C11, x2, C9);
+  p = pmadd(x2, p, C7);
+  p = pmadd(x2, p, C5);
+  p = pmadd(x2, p, C3);
+  p = pmadd(pmul(x, x2), p, x);
+
+  // For |x| in ]0.5:1.0] we use atanh = 0.5*ln((1+x)/(1-x));
+  const Packet one = pset1<Packet>(1.0f);
+  Packet r = pdiv(padd(one, x), psub(one, x));
+  r = pmul(half, plog(r));
+  return pselect(x_gt_half, r, p);
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pdiv_complex(const Packet& x, const Packet& y) {
+  typedef typename unpacket_traits<Packet>::as_real RealPacket;
+  // In the following we annotate the code for the case where the inputs
+  // are a pair length-2 SIMD vectors representing a single pair of complex
+  // numbers x = a + i*b, y = c + i*d.
+  const RealPacket y_abs = pabs(y.v);                        // |c|, |d|
+  const RealPacket y_abs_flip = pcplxflip(Packet(y_abs)).v;  // |d|, |c|
+  const RealPacket y_max = pmax(y_abs, y_abs_flip);          // max(|c|, |d|), max(|c|, |d|)
+  const RealPacket y_scaled = pdiv(y.v, y_max);              // c / max(|c|, |d|), d / max(|c|, |d|)
+  // Compute scaled denominator.
+  const RealPacket y_scaled_sq = pmul(y_scaled, y_scaled);  // c'**2, d'**2
+  const RealPacket denom = padd(y_scaled_sq, pcplxflip(Packet(y_scaled_sq)).v);
+  Packet result_scaled = pmul(x, pconj(Packet(y_scaled)));  // a * c' + b * d', -a * d + b * c
+  // Divide elementwise by denom.
+  result_scaled = Packet(pdiv(result_scaled.v, denom));
+  // Rescale result
+  return Packet(pdiv(result_scaled.v, y_max));
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psqrt_complex(const Packet& a) {
   typedef typename unpacket_traits<Packet>::type Scalar;
   typedef typename Scalar::value_type RealScalar;
   typedef typename unpacket_traits<Packet>::as_real RealPacket;
@@ -802,14 +1006,14 @@
   //    l0 = (min0 == 0 ? max0 : max0 * sqrt(1 + (min0/max0)**2)),
   // where max0 = max(|x0|, |y0|), min0 = min(|x0|, |y0|), and similarly for l1.
 
-  RealPacket a_abs = pabs(a.v);           // [|x0|, |y0|, |x1|, |y1|]
-  RealPacket a_abs_flip = pcplxflip(Packet(a_abs)).v; // [|y0|, |x0|, |y1|, |x1|]
+  RealPacket a_abs = pabs(a.v);                        // [|x0|, |y0|, |x1|, |y1|]
+  RealPacket a_abs_flip = pcplxflip(Packet(a_abs)).v;  // [|y0|, |x0|, |y1|, |x1|]
   RealPacket a_max = pmax(a_abs, a_abs_flip);
   RealPacket a_min = pmin(a_abs, a_abs_flip);
   RealPacket a_min_zero_mask = pcmp_eq(a_min, pzero(a_min));
   RealPacket a_max_zero_mask = pcmp_eq(a_max, pzero(a_max));
   RealPacket r = pdiv(a_min, a_max);
-  const RealPacket cst_one  = pset1<RealPacket>(RealScalar(1));
+  const RealPacket cst_one = pset1<RealPacket>(RealScalar(1));
   RealPacket l = pmul(a_max, psqrt(padd(cst_one, pmul(r, r))));  // [l0, l0, l1, l1]
   // Set l to a_max if a_min is zero.
   l = pselect(a_min_zero_mask, a_max, l);
@@ -832,8 +1036,7 @@
 
   // Step 4. Compute solution for inputs with negative real part:
   //         [|eta0|, sign(y0)*rho0, |eta1|, sign(y1)*rho1]
-  const RealScalar neg_zero = RealScalar(numext::bit_cast<float>(0x80000000u));
-  const RealPacket cst_imag_sign_mask = pset1<Packet>(Scalar(RealScalar(0.0), neg_zero)).v;
+  const RealPacket cst_imag_sign_mask = pset1<Packet>(Scalar(RealScalar(0.0), RealScalar(-0.0))).v;
   RealPacket imag_signs = pand(a.v, cst_imag_sign_mask);
   Packet negative_real_result;
   // Notice that rho is positive, so taking it's absolute value is a noop.
@@ -866,11 +1069,96 @@
   is_imag_inf = por(is_imag_inf, pcplxflip(is_imag_inf));
   Packet imag_inf_result;
   imag_inf_result.v = por(pand(cst_pos_inf, real_mask), pandnot(a.v, real_mask));
+  // unless otherwise specified, if either the real or imaginary component is nan, the entire result is nan
+  Packet result_is_nan = pisnan(result);
+  result = por(result_is_nan, result);
 
-  return  pselect(is_imag_inf, imag_inf_result,
-                  pselect(is_real_inf, real_inf_result,result));
+  return pselect(is_imag_inf, imag_inf_result, pselect(is_real_inf, real_inf_result, result));
 }
 
+template <typename Packet>
+struct psign_impl<Packet, std::enable_if_t<!NumTraits<typename unpacket_traits<Packet>::type>::IsComplex &&
+                                           !NumTraits<typename unpacket_traits<Packet>::type>::IsInteger>> {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a) {
+    using Scalar = typename unpacket_traits<Packet>::type;
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    const Packet cst_zero = pzero(a);
+
+    const Packet abs_a = pabs(a);
+    const Packet sign_mask = pandnot(a, abs_a);
+    const Packet nonzero_mask = pcmp_lt(cst_zero, abs_a);
+
+    return pselect(nonzero_mask, por(sign_mask, cst_one), abs_a);
+  }
+};
+
+template <typename Packet>
+struct psign_impl<Packet, std::enable_if_t<!NumTraits<typename unpacket_traits<Packet>::type>::IsComplex &&
+                                           NumTraits<typename unpacket_traits<Packet>::type>::IsSigned &&
+                                           NumTraits<typename unpacket_traits<Packet>::type>::IsInteger>> {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a) {
+    using Scalar = typename unpacket_traits<Packet>::type;
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    const Packet cst_minus_one = pset1<Packet>(Scalar(-1));
+    const Packet cst_zero = pzero(a);
+
+    const Packet positive_mask = pcmp_lt(cst_zero, a);
+    const Packet positive = pand(positive_mask, cst_one);
+    const Packet negative_mask = pcmp_lt(a, cst_zero);
+    const Packet negative = pand(negative_mask, cst_minus_one);
+
+    return por(positive, negative);
+  }
+};
+
+template <typename Packet>
+struct psign_impl<Packet, std::enable_if_t<!NumTraits<typename unpacket_traits<Packet>::type>::IsComplex &&
+                                           !NumTraits<typename unpacket_traits<Packet>::type>::IsSigned &&
+                                           NumTraits<typename unpacket_traits<Packet>::type>::IsInteger>> {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a) {
+    using Scalar = typename unpacket_traits<Packet>::type;
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    const Packet cst_zero = pzero(a);
+
+    const Packet zero_mask = pcmp_eq(cst_zero, a);
+    return pandnot(cst_one, zero_mask);
+  }
+};
+
+// \internal \returns the the sign of a complex number z, defined as z / abs(z).
+template <typename Packet>
+struct psign_impl<Packet, std::enable_if_t<NumTraits<typename unpacket_traits<Packet>::type>::IsComplex &&
+                                           unpacket_traits<Packet>::vectorizable>> {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a) {
+    typedef typename unpacket_traits<Packet>::type Scalar;
+    typedef typename Scalar::value_type RealScalar;
+    typedef typename unpacket_traits<Packet>::as_real RealPacket;
+
+    // Step 1. Compute (for each element z = x + i*y in a)
+    //     l = abs(z) = sqrt(x^2 + y^2).
+    // To avoid over- and underflow, we use the stable formula for each hypotenuse
+    //    l = (zmin == 0 ? zmax : zmax * sqrt(1 + (zmin/zmax)**2)),
+    // where zmax = max(|x|, |y|), zmin = min(|x|, |y|),
+    RealPacket a_abs = pabs(a.v);
+    RealPacket a_abs_flip = pcplxflip(Packet(a_abs)).v;
+    RealPacket a_max = pmax(a_abs, a_abs_flip);
+    RealPacket a_min = pmin(a_abs, a_abs_flip);
+    RealPacket a_min_zero_mask = pcmp_eq(a_min, pzero(a_min));
+    RealPacket a_max_zero_mask = pcmp_eq(a_max, pzero(a_max));
+    RealPacket r = pdiv(a_min, a_max);
+    const RealPacket cst_one = pset1<RealPacket>(RealScalar(1));
+    RealPacket l = pmul(a_max, psqrt(padd(cst_one, pmul(r, r))));  // [l0, l0, l1, l1]
+    // Set l to a_max if a_min is zero, since the roundtrip sqrt(a_max^2) may be
+    // lossy.
+    l = pselect(a_min_zero_mask, a_max, l);
+    // Step 2 compute a / abs(a).
+    RealPacket sign_as_real = pandnot(pdiv(a.v, l), a_max_zero_mask);
+    Packet sign;
+    sign.v = sign_as_real;
+    return sign;
+  }
+};
+
 // TODO(rmlarsen): The following set of utilities for double word arithmetic
 // should perhaps be refactored as a separate file, since it would be generally
 // useful for special function implementation etc. Writing the algorithms in
@@ -878,18 +1166,16 @@
 
 // This function splits x into the nearest integer n and fractional part r,
 // such that x = n + r holds exactly.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void absolute_split(const Packet& x, Packet& n, Packet& r) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void absolute_split(const Packet& x, Packet& n, Packet& r) {
   n = pround(x);
   r = psub(x, n);
 }
 
 // This function computes the sum {s, r}, such that x + y = s_hi + s_lo
 // holds exactly, and s_hi = fl(x+y), if |x| >= |y|.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void fast_twosum(const Packet& x, const Packet& y, Packet& s_hi, Packet& s_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void fast_twosum(const Packet& x, const Packet& y, Packet& s_hi, Packet& s_lo) {
   s_hi = padd(x, y);
   const Packet t = psub(s_hi, x);
   s_lo = psub(y, t);
@@ -900,12 +1186,10 @@
 // a pair of floating point numbers. Given {x, y}, it computes the pair
 // {p_hi, p_lo} such that x * y = p_hi + p_lo holds exactly and
 // p_hi = fl(x * y).
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void twoprod(const Packet& x, const Packet& y,
-             Packet& p_hi, Packet& p_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void twoprod(const Packet& x, const Packet& y, Packet& p_hi, Packet& p_lo) {
   p_hi = pmul(x, y);
-  p_lo = pmadd(x, y, pnegate(p_hi));
+  p_lo = pmsub(x, y, p_hi);
 }
 
 #else
@@ -915,9 +1199,8 @@
 // exactly and that half of the significant of x fits in x_hi.
 // This is Algorithm 3 from Jean-Michel Muller, "Elementary Functions",
 // 3rd edition, Birkh\"auser, 2016.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void veltkamp_splitting(const Packet& x, Packet& x_hi, Packet& x_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void veltkamp_splitting(const Packet& x, Packet& x_hi, Packet& x_lo) {
   typedef typename unpacket_traits<Packet>::type Scalar;
   EIGEN_CONSTEXPR int shift = (NumTraits<Scalar>::digits() + 1) / 2;
   const Scalar shift_scale = Scalar(uint64_t(1) << shift);  // Scalar constructor not necessarily constexpr.
@@ -931,10 +1214,8 @@
 // Given floating point numbers {x, y} computes the pair
 // {p_hi, p_lo} such that x * y = p_hi + p_lo holds exactly and
 // p_hi = fl(x * y).
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void twoprod(const Packet& x, const Packet& y,
-             Packet& p_hi, Packet& p_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void twoprod(const Packet& x, const Packet& y, Packet& p_hi, Packet& p_lo) {
   Packet x_hi, x_lo, y_hi, y_lo;
   veltkamp_splitting(x, x_hi, x_lo);
   veltkamp_splitting(y, y_hi, y_lo);
@@ -948,23 +1229,20 @@
 
 #endif  // EIGEN_HAS_SINGLE_INSTRUCTION_MADD
 
-
 // This function implements Dekker's algorithm for the addition
 // of two double word numbers represented by {x_hi, x_lo} and {y_hi, y_lo}.
 // It returns the result as a pair {s_hi, s_lo} such that
 // x_hi + x_lo + y_hi + y_lo = s_hi + s_lo holds exactly.
 // This is Algorithm 5 from Jean-Michel Muller, "Elementary Functions",
 // 3rd edition, Birkh\"auser, 2016.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-  void twosum(const Packet& x_hi, const Packet& x_lo,
-              const Packet& y_hi, const Packet& y_lo,
-              Packet& s_hi, Packet& s_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void twosum(const Packet& x_hi, const Packet& x_lo, const Packet& y_hi, const Packet& y_lo,
+                                Packet& s_hi, Packet& s_lo) {
   const Packet x_greater_mask = pcmp_lt(pabs(y_hi), pabs(x_hi));
   Packet r_hi_1, r_lo_1;
-  fast_twosum(x_hi, y_hi,r_hi_1, r_lo_1);
+  fast_twosum(x_hi, y_hi, r_hi_1, r_lo_1);
   Packet r_hi_2, r_lo_2;
-  fast_twosum(y_hi, x_hi,r_hi_2, r_lo_2);
+  fast_twosum(y_hi, x_hi, r_hi_2, r_lo_2);
   const Packet r_hi = pselect(x_greater_mask, r_hi_1, r_hi_2);
 
   const Packet s1 = padd(padd(y_lo, r_lo_1), x_lo);
@@ -976,11 +1254,9 @@
 
 // This is a version of twosum for double word numbers,
 // which assumes that |x_hi| >= |y_hi|.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-  void fast_twosum(const Packet& x_hi, const Packet& x_lo,
-              const Packet& y_hi, const Packet& y_lo,
-              Packet& s_hi, Packet& s_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void fast_twosum(const Packet& x_hi, const Packet& x_lo, const Packet& y_hi, const Packet& y_lo,
+                                     Packet& s_hi, Packet& s_lo) {
   Packet r_hi, r_lo;
   fast_twosum(x_hi, y_hi, r_hi, r_lo);
   const Packet s = padd(padd(y_lo, r_lo), x_lo);
@@ -990,11 +1266,9 @@
 // This is a version of twosum for adding a floating point number x to
 // double word number {y_hi, y_lo} number, with the assumption
 // that |x| >= |y_hi|.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void fast_twosum(const Packet& x,
-                 const Packet& y_hi, const Packet& y_lo,
-                 Packet& s_hi, Packet& s_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void fast_twosum(const Packet& x, const Packet& y_hi, const Packet& y_lo, Packet& s_hi,
+                                     Packet& s_lo) {
   Packet r_hi, r_lo;
   fast_twosum(x, y_hi, r_hi, r_lo);
   const Packet s = padd(y_lo, r_lo);
@@ -1009,10 +1283,8 @@
 // in the floating point type.
 // This is Algorithm 7 from Jean-Michel Muller, "Elementary Functions",
 // 3rd edition, Birkh\"auser, 2016.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void twoprod(const Packet& x_hi, const Packet& x_lo, const Packet& y,
-             Packet& p_hi, Packet& p_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void twoprod(const Packet& x_hi, const Packet& x_lo, const Packet& y, Packet& p_hi, Packet& p_lo) {
   Packet c_hi, c_lo1;
   twoprod(x_hi, y, c_hi, c_lo1);
   const Packet c_lo2 = pmul(x_lo, y);
@@ -1028,11 +1300,9 @@
 // (x_hi + x_lo) * (y_hi + y_lo) = p_hi + p_lo holds with a relative error
 // of less than 2*2^{-2p}, where p is the number of significand bit
 // in the floating point type.
-template<typename Packet>
-EIGEN_STRONG_INLINE
-void twoprod(const Packet& x_hi, const Packet& x_lo,
-             const Packet& y_hi, const Packet& y_lo,
-             Packet& p_hi, Packet& p_lo) {
+template <typename Packet>
+EIGEN_STRONG_INLINE void twoprod(const Packet& x_hi, const Packet& x_lo, const Packet& y_hi, const Packet& y_lo,
+                                 Packet& p_hi, Packet& p_lo) {
   Packet p_hi_hi, p_hi_lo;
   twoprod(x_hi, x_lo, y_hi, p_hi_hi, p_hi_lo);
   Packet p_lo_hi, p_lo_lo;
@@ -1040,38 +1310,27 @@
   fast_twosum(p_hi_hi, p_hi_lo, p_lo_hi, p_lo_lo, p_hi, p_lo);
 }
 
-// This function computes the reciprocal of a floating point number
-// with extra precision and returns the result as a double word.
+// This function implements the division of double word {x_hi, x_lo}
+// by float y. This is Algorithm 15 from "Tight and rigourous error bounds
+// for basic building blocks of double-word arithmetic", Joldes, Muller, & Popescu,
+// 2017. https://hal.archives-ouvertes.fr/hal-01351529
 template <typename Packet>
-void doubleword_reciprocal(const Packet& x, Packet& recip_hi, Packet& recip_lo) {
-  typedef typename unpacket_traits<Packet>::type Scalar;
-  // 1. Approximate the reciprocal as the reciprocal of the high order element.
-  Packet approx_recip = prsqrt(x);
-  approx_recip = pmul(approx_recip, approx_recip);
-
-  // 2. Run one step of Newton-Raphson iteration in double word arithmetic
-  // to get the bottom half. The NR iteration for reciprocal of 'a' is
-  //    x_{i+1} = x_i * (2 - a * x_i)
-
-  // -a*x_i
-  Packet t1_hi, t1_lo;
-  twoprod(pnegate(x), approx_recip, t1_hi, t1_lo);
-  // 2 - a*x_i
-  Packet t2_hi, t2_lo;
-  fast_twosum(pset1<Packet>(Scalar(2)), t1_hi, t2_hi, t2_lo);
-  Packet t3_hi, t3_lo;
-  fast_twosum(t2_hi, padd(t2_lo, t1_lo), t3_hi, t3_lo);
-  // x_i * (2 - a * x_i)
-  twoprod(t3_hi, t3_lo, approx_recip, recip_hi, recip_lo);
+void doubleword_div_fp(const Packet& x_hi, const Packet& x_lo, const Packet& y, Packet& z_hi, Packet& z_lo) {
+  const Packet t_hi = pdiv(x_hi, y);
+  Packet pi_hi, pi_lo;
+  twoprod(t_hi, y, pi_hi, pi_lo);
+  const Packet delta_hi = psub(x_hi, pi_hi);
+  const Packet delta_t = psub(delta_hi, pi_lo);
+  const Packet delta = padd(delta_t, x_lo);
+  const Packet t_lo = pdiv(delta, y);
+  fast_twosum(t_hi, t_lo, z_hi, z_lo);
 }
 
-
 // This function computes log2(x) and returns the result as a double word.
 template <typename Scalar>
 struct accurate_log2 {
   template <typename Packet>
-  EIGEN_STRONG_INLINE
-  void operator()(const Packet& x, Packet& log2_x_hi, Packet& log2_x_lo) {
+  EIGEN_STRONG_INLINE void operator()(const Packet& x, Packet& log2_x_hi, Packet& log2_x_lo) {
     log2_x_hi = plog2(x);
     log2_x_lo = pzero(x);
   }
@@ -1086,8 +1345,7 @@
 template <>
 struct accurate_log2<float> {
   template <typename Packet>
-  EIGEN_STRONG_INLINE
-  void operator()(const Packet& z, Packet& log2_x_hi, Packet& log2_x_lo) {
+  EIGEN_STRONG_INLINE void operator()(const Packet& z, Packet& log2_x_hi, Packet& log2_x_lo) {
     // The function log(1+x)/x is approximated in the interval
     // [1/sqrt(2)-1;sqrt(2)-1] by a degree 10 polynomial of the form
     //  Q(x) = (C0 + x * (C1 + x * (C2 + x * (C3 + x * P(x))))),
@@ -1102,14 +1360,14 @@
     // > f = log2(1+x)/x;
     // > interval = [sqrt(0.5)-1;sqrt(2)-1];
     // > p = fpminimax(f,n,[|double,double,double,double,single...|],interval,relative,floating);
-    
-    const Packet p6 = pset1<Packet>( 9.703654795885e-2f);
+
+    const Packet p6 = pset1<Packet>(9.703654795885e-2f);
     const Packet p5 = pset1<Packet>(-0.1690667718648f);
-    const Packet p4 = pset1<Packet>( 0.1720575392246f);
+    const Packet p4 = pset1<Packet>(0.1720575392246f);
     const Packet p3 = pset1<Packet>(-0.1789081543684f);
-    const Packet p2 = pset1<Packet>( 0.2050433009862f);
+    const Packet p2 = pset1<Packet>(0.2050433009862f);
     const Packet p1 = pset1<Packet>(-0.2404672354459f);
-    const Packet p0 = pset1<Packet>( 0.2885761857032f);
+    const Packet p0 = pset1<Packet>(0.2885761857032f);
 
     const Packet C3_hi = pset1<Packet>(-0.360674142838f);
     const Packet C3_lo = pset1<Packet>(-6.13283912543e-09f);
@@ -1125,7 +1383,7 @@
     // Evaluate P(x) in working precision.
     // We evaluate it in multiple parts to improve instruction level
     // parallelism.
-    Packet x2 = pmul(x,x);
+    Packet x2 = pmul(x, x);
     Packet p_even = pmadd(p6, x2, p4);
     p_even = pmadd(p_even, x2, p2);
     p_even = pmadd(p_even, x2, p0);
@@ -1167,8 +1425,7 @@
 template <>
 struct accurate_log2<double> {
   template <typename Packet>
-  EIGEN_STRONG_INLINE
-  void operator()(const Packet& x, Packet& log2_x_hi, Packet& log2_x_lo) {
+  EIGEN_STRONG_INLINE void operator()(const Packet& x, Packet& log2_x_hi, Packet& log2_x_lo) {
     // We use a transformation of variables:
     //    r = c * (x-1) / (x+1),
     // such that
@@ -1204,16 +1461,13 @@
     const Packet cst_2_log2e_hi = pset1<Packet>(2.88539008177792677);
     const Packet cst_2_log2e_lo = pset1<Packet>(4.07660016854549667e-17);
     // c * (x - 1)
-    Packet num_hi, num_lo;
-    twoprod(cst_2_log2e_hi, cst_2_log2e_lo, psub(x, one), num_hi, num_lo);
-    // TODO(rmlarsen): Investigate if using the division algorithm by
-    // Muller et al. is faster/more accurate.
-    // 1 / (x + 1)
-    Packet denom_hi, denom_lo;
-    doubleword_reciprocal(padd(x, one), denom_hi, denom_lo);
-    // r =  c * (x-1) / (x+1),
+    Packet t_hi, t_lo;
+    // t = c * (x-1)
+    twoprod(cst_2_log2e_hi, cst_2_log2e_lo, psub(x, one), t_hi, t_lo);
+    // r = c * (x-1) / (x+1),
     Packet r_hi, r_lo;
-    twoprod(num_hi, num_lo, denom_hi, denom_lo, r_hi, r_lo);
+    doubleword_div_fp(t_hi, t_lo, padd(x, one), r_hi, r_lo);
+
     // r2 = r * r
     Packet r2_hi, r2_lo;
     twoprod(r_hi, r_lo, r_hi, r_lo, r2_hi, r2_lo);
@@ -1256,8 +1510,7 @@
 template <typename Scalar>
 struct fast_accurate_exp2 {
   template <typename Packet>
-  EIGEN_STRONG_INLINE
-  Packet operator()(const Packet& x) {
+  EIGEN_STRONG_INLINE Packet operator()(const Packet& x) {
     // TODO(rmlarsen): Add a pexp2 packetop.
     return pexp(pmul(pset1<Packet>(Scalar(EIGEN_LN2)), x));
   }
@@ -1270,8 +1523,7 @@
 template <>
 struct fast_accurate_exp2<float> {
   template <typename Packet>
-  EIGEN_STRONG_INLINE
-  Packet operator()(const Packet& x) {
+  EIGEN_STRONG_INLINE Packet operator()(const Packet& x) {
     // This function approximates exp2(x) by a degree 6 polynomial of the form
     // Q(x) = 1 + x * (C + x * P(x)), where the degree 4 polynomial P(x) is evaluated in
     // single precision, and the remaining steps are evaluated with extra precision using
@@ -1296,7 +1548,7 @@
     // Evaluate P(x) in working precision.
     // We evaluate even and odd parts of the polynomial separately
     // to gain some instruction level parallelism.
-    Packet x2 = pmul(x,x);
+    Packet x2 = pmul(x, x);
     Packet p_even = pmadd(p4, x2, p2);
     Packet p_odd = pmadd(p3, x2, p1);
     p_even = pmadd(p_even, x2, p0);
@@ -1328,8 +1580,7 @@
 template <>
 struct fast_accurate_exp2<double> {
   template <typename Packet>
-  EIGEN_STRONG_INLINE
-  Packet operator()(const Packet& x) {
+  EIGEN_STRONG_INLINE Packet operator()(const Packet& x) {
     // This function approximates exp2(x) by a degree 10 polynomial of the form
     // Q(x) = 1 + x * (C + x * P(x)), where the degree 8 polynomial P(x) is evaluated in
     // single precision, and the remaining steps are evaluated with extra precision using
@@ -1351,14 +1602,14 @@
     const Packet p2 = pset1<Packet>(9.618129107593478832e-3);
     const Packet p1 = pset1<Packet>(5.550410866481961247e-2);
     const Packet p0 = pset1<Packet>(0.240226506959101332);
-    const Packet C_hi = pset1<Packet>(0.693147180559945286); 
+    const Packet C_hi = pset1<Packet>(0.693147180559945286);
     const Packet C_lo = pset1<Packet>(4.81927865669806721e-17);
     const Packet one = pset1<Packet>(1.0);
 
     // Evaluate P(x) in working precision.
     // We evaluate even and odd parts of the polynomial separately
     // to gain some instruction level parallelism.
-    Packet x2 = pmul(x,x);
+    Packet x2 = pmul(x, x);
     Packet p_even = pmadd(p8, x2, p6);
     Packet p_odd = pmadd(p9, x2, p7);
     p_even = pmadd(p_even, x2, p4);
@@ -1443,39 +1694,40 @@
 }
 
 // Generic implementation of pow(x,y).
-template<typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet generic_pow(const Packet& x, const Packet& y) {
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet generic_pow(const Packet& x, const Packet& y) {
   typedef typename unpacket_traits<Packet>::type Scalar;
 
   const Packet cst_pos_inf = pset1<Packet>(NumTraits<Scalar>::infinity());
+  const Packet cst_neg_inf = pset1<Packet>(-NumTraits<Scalar>::infinity());
   const Packet cst_zero = pset1<Packet>(Scalar(0));
   const Packet cst_one = pset1<Packet>(Scalar(1));
   const Packet cst_nan = pset1<Packet>(NumTraits<Scalar>::quiet_NaN());
 
   const Packet abs_x = pabs(x);
   // Predicates for sign and magnitude of x.
-  const Packet x_is_zero = pcmp_eq(x, cst_zero);
-  const Packet x_is_neg = pcmp_lt(x, cst_zero);
+  const Packet abs_x_is_zero = pcmp_eq(abs_x, cst_zero);
+  const Packet x_has_signbit = psignbit(x);
+  const Packet x_is_neg = pandnot(x_has_signbit, abs_x_is_zero);
+  const Packet x_is_neg_zero = pand(x_has_signbit, abs_x_is_zero);
   const Packet abs_x_is_inf = pcmp_eq(abs_x, cst_pos_inf);
-  const Packet abs_x_is_one =  pcmp_eq(abs_x, cst_one);
+  const Packet abs_x_is_one = pcmp_eq(abs_x, cst_one);
   const Packet abs_x_is_gt_one = pcmp_lt(cst_one, abs_x);
   const Packet abs_x_is_lt_one = pcmp_lt(abs_x, cst_one);
-  const Packet x_is_one =  pandnot(abs_x_is_one, x_is_neg);
-  const Packet x_is_neg_one =  pand(abs_x_is_one, x_is_neg);
-  const Packet x_is_nan = pandnot(ptrue(x), pcmp_eq(x, x));
+  const Packet x_is_one = pandnot(abs_x_is_one, x_is_neg);
+  const Packet x_is_neg_one = pand(abs_x_is_one, x_is_neg);
+  const Packet x_is_nan = pisnan(x);
 
   // Predicates for sign and magnitude of y.
+  const Packet abs_y = pabs(y);
   const Packet y_is_one = pcmp_eq(y, cst_one);
-  const Packet y_is_zero = pcmp_eq(y, cst_zero);
+  const Packet abs_y_is_zero = pcmp_eq(abs_y, cst_zero);
   const Packet y_is_neg = pcmp_lt(y, cst_zero);
-  const Packet y_is_pos = pandnot(ptrue(y), por(y_is_zero, y_is_neg));
-  const Packet y_is_nan = pandnot(ptrue(y), pcmp_eq(y, y));
-  const Packet abs_y_is_inf = pcmp_eq(pabs(y), cst_pos_inf);
+  const Packet y_is_pos = pandnot(ptrue(y), por(abs_y_is_zero, y_is_neg));
+  const Packet y_is_nan = pisnan(y);
+  const Packet abs_y_is_inf = pcmp_eq(abs_y, cst_pos_inf);
   EIGEN_CONSTEXPR Scalar huge_exponent =
-      (NumTraits<Scalar>::max_exponent() * Scalar(EIGEN_LN2)) /
-       NumTraits<Scalar>::epsilon();
+      (NumTraits<Scalar>::max_exponent() * Scalar(EIGEN_LN2)) / NumTraits<Scalar>::epsilon();
   const Packet abs_y_is_huge = pcmp_le(pset1<Packet>(huge_exponent), pabs(y));
 
   // Predicates for whether y is integer and/or even.
@@ -1484,39 +1736,33 @@
   const Packet y_is_even = pcmp_eq(pround(y_div_2), y_div_2);
 
   // Predicates encoding special cases for the value of pow(x,y)
-  const Packet invalid_negative_x = pandnot(pandnot(pandnot(x_is_neg, abs_x_is_inf),
-                                                    y_is_int),
-                                            abs_y_is_inf);
-  const Packet pow_is_one = por(por(x_is_one, y_is_zero),
-                                pand(x_is_neg_one,
-                                     por(abs_y_is_inf, pandnot(y_is_even, invalid_negative_x))));
+  const Packet invalid_negative_x = pandnot(pandnot(pandnot(x_is_neg, abs_x_is_inf), y_is_int), abs_y_is_inf);
   const Packet pow_is_nan = por(invalid_negative_x, por(x_is_nan, y_is_nan));
-  const Packet pow_is_zero = por(por(por(pand(x_is_zero, y_is_pos),
-                                         pand(abs_x_is_inf, y_is_neg)),
-                                     pand(pand(abs_x_is_lt_one, abs_y_is_huge),
-                                          y_is_pos)),
-                                 pand(pand(abs_x_is_gt_one, abs_y_is_huge),
-                                      y_is_neg));
-  const Packet pow_is_inf = por(por(por(pand(x_is_zero, y_is_neg),
-                                        pand(abs_x_is_inf, y_is_pos)),
-                                    pand(pand(abs_x_is_lt_one, abs_y_is_huge),
-                                         y_is_neg)),
-                                pand(pand(abs_x_is_gt_one, abs_y_is_huge),
-                                     y_is_pos));
-
+  const Packet pow_is_one =
+      por(por(x_is_one, abs_y_is_zero), pand(x_is_neg_one, por(abs_y_is_inf, pandnot(y_is_even, invalid_negative_x))));
+  const Packet pow_is_zero = por(por(por(pand(abs_x_is_zero, y_is_pos), pand(abs_x_is_inf, y_is_neg)),
+                                     pand(pand(abs_x_is_lt_one, abs_y_is_huge), y_is_pos)),
+                                 pand(pand(abs_x_is_gt_one, abs_y_is_huge), y_is_neg));
+  const Packet pow_is_inf = por(por(por(pand(abs_x_is_zero, y_is_neg), pand(abs_x_is_inf, y_is_pos)),
+                                    pand(pand(abs_x_is_lt_one, abs_y_is_huge), y_is_neg)),
+                                pand(pand(abs_x_is_gt_one, abs_y_is_huge), y_is_pos));
+  const Packet pow_is_neg_zero = pand(pandnot(y_is_int, y_is_even),
+                                      por(pand(y_is_neg, pand(abs_x_is_inf, x_is_neg)), pand(y_is_pos, x_is_neg_zero)));
+  const Packet inf_val =
+      pselect(pandnot(pand(por(pand(abs_x_is_inf, x_is_neg), pand(x_is_neg_zero, y_is_neg)), y_is_int), y_is_even),
+              cst_neg_inf, cst_pos_inf);
   // General computation of pow(x,y) for positive x or negative x and integer y.
   const Packet negate_pow_abs = pandnot(x_is_neg, y_is_even);
   const Packet pow_abs = generic_pow_impl(abs_x, y);
   return pselect(y_is_one, x,
                  pselect(pow_is_one, cst_one,
                          pselect(pow_is_nan, cst_nan,
-                                 pselect(pow_is_inf, cst_pos_inf,
-                                         pselect(pow_is_zero, cst_zero,
-                                                 pselect(negate_pow_abs, pnegate(pow_abs), pow_abs))))));
+                                 pselect(pow_is_inf, inf_val,
+                                         pselect(pow_is_neg_zero, pnegate(cst_zero),
+                                                 pselect(pow_is_zero, cst_zero,
+                                                         pselect(negate_pow_abs, pnegate(pow_abs), pow_abs)))))));
 }
 
-
-
 /* polevl (modified for Eigen)
  *
  *      Evaluate polynomial
@@ -1558,15 +1804,17 @@
  */
 template <typename Packet, int N>
 struct ppolevl {
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const typename unpacket_traits<Packet>::type coeff[]) {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x,
+                                                          const typename unpacket_traits<Packet>::type coeff[]) {
     EIGEN_STATIC_ASSERT((N > 0), YOU_MADE_A_PROGRAMMING_MISTAKE);
-    return pmadd(ppolevl<Packet, N-1>::run(x, coeff), x, pset1<Packet>(coeff[N]));
+    return pmadd(ppolevl<Packet, N - 1>::run(x, coeff), x, pset1<Packet>(coeff[N]));
   }
 };
 
 template <typename Packet>
 struct ppolevl<Packet, 0> {
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const typename unpacket_traits<Packet>::type coeff[]) {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x,
+                                                          const typename unpacket_traits<Packet>::type coeff[]) {
     EIGEN_UNUSED_VARIABLE(x);
     return pset1<Packet>(coeff[0]);
   }
@@ -1626,8 +1874,8 @@
 
 template <typename Packet, int N>
 struct pchebevl {
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Packet run(Packet x, const typename unpacket_traits<Packet>::type coef[]) {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(Packet x,
+                                                          const typename unpacket_traits<Packet>::type coef[]) {
     typedef typename unpacket_traits<Packet>::type Scalar;
     Packet b0 = pset1<Packet>(coef[0]);
     Packet b1 = pset1<Packet>(static_cast<Scalar>(0.f));
@@ -1643,7 +1891,241 @@
   }
 };
 
-} // end namespace internal
-} // end namespace Eigen
+namespace unary_pow {
 
-#endif // EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_H
+template <typename ScalarExponent, bool IsInteger = NumTraits<ScalarExponent>::IsInteger>
+struct exponent_helper {
+  using safe_abs_type = ScalarExponent;
+  static constexpr ScalarExponent one_half = ScalarExponent(0.5);
+  // these routines assume that exp is an integer stored as a floating point type
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ScalarExponent safe_abs(const ScalarExponent& exp) {
+    return numext::abs(exp);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool is_odd(const ScalarExponent& exp) {
+    eigen_assert(((numext::isfinite)(exp) && exp == numext::floor(exp)) && "exp must be an integer");
+    ScalarExponent exp_div_2 = exp * one_half;
+    ScalarExponent floor_exp_div_2 = numext::floor(exp_div_2);
+    return exp_div_2 != floor_exp_div_2;
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ScalarExponent floor_div_two(const ScalarExponent& exp) {
+    ScalarExponent exp_div_2 = exp * one_half;
+    return numext::floor(exp_div_2);
+  }
+};
+
+template <typename ScalarExponent>
+struct exponent_helper<ScalarExponent, true> {
+  // if `exp` is a signed integer type, cast it to its unsigned counterpart to safely store its absolute value
+  // consider the (rare) case where `exp` is an int32_t: abs(-2147483648) != 2147483648
+  using safe_abs_type = typename numext::get_integer_by_size<sizeof(ScalarExponent)>::unsigned_type;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE safe_abs_type safe_abs(const ScalarExponent& exp) {
+    ScalarExponent mask = numext::signbit(exp);
+    safe_abs_type result = safe_abs_type(exp ^ mask);
+    return result + safe_abs_type(ScalarExponent(1) & mask);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool is_odd(const safe_abs_type& exp) {
+    return exp % safe_abs_type(2) != safe_abs_type(0);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE safe_abs_type floor_div_two(const safe_abs_type& exp) {
+    return exp >> safe_abs_type(1);
+  }
+};
+
+template <typename Packet, typename ScalarExponent,
+          bool ReciprocateIfExponentIsNegative =
+              !NumTraits<typename unpacket_traits<Packet>::type>::IsInteger && NumTraits<ScalarExponent>::IsSigned>
+struct reciprocate {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const ScalarExponent& exponent) {
+    using Scalar = typename unpacket_traits<Packet>::type;
+    const Packet cst_pos_one = pset1<Packet>(Scalar(1));
+    return exponent < 0 ? pdiv(cst_pos_one, x) : x;
+  }
+};
+
+template <typename Packet, typename ScalarExponent>
+struct reciprocate<Packet, ScalarExponent, false> {
+  // pdiv not defined, nor necessary for integer base types
+  // if the exponent is unsigned, then the exponent cannot be negative
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const ScalarExponent&) { return x; }
+};
+
+template <typename Packet, typename ScalarExponent>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet int_pow(const Packet& x, const ScalarExponent& exponent) {
+  using Scalar = typename unpacket_traits<Packet>::type;
+  using ExponentHelper = exponent_helper<ScalarExponent>;
+  using AbsExponentType = typename ExponentHelper::safe_abs_type;
+  const Packet cst_pos_one = pset1<Packet>(Scalar(1));
+  if (exponent == ScalarExponent(0)) return cst_pos_one;
+
+  Packet result = reciprocate<Packet, ScalarExponent>::run(x, exponent);
+  Packet y = cst_pos_one;
+  AbsExponentType m = ExponentHelper::safe_abs(exponent);
+
+  while (m > 1) {
+    bool odd = ExponentHelper::is_odd(m);
+    if (odd) y = pmul(y, result);
+    result = pmul(result, result);
+    m = ExponentHelper::floor_div_two(m);
+  }
+
+  return pmul(y, result);
+}
+
+template <typename Packet>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet gen_pow(const Packet& x,
+                                                     const typename unpacket_traits<Packet>::type& exponent) {
+  const Packet exponent_packet = pset1<Packet>(exponent);
+  return generic_pow_impl(x, exponent_packet);
+}
+
+template <typename Packet, typename ScalarExponent>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet handle_nonint_nonint_errors(const Packet& x, const Packet& powx,
+                                                                         const ScalarExponent& exponent) {
+  using Scalar = typename unpacket_traits<Packet>::type;
+
+  // non-integer base and exponent case
+
+  const Scalar pos_zero = Scalar(0);
+  const Scalar all_ones = ptrue<Scalar>(Scalar());
+  const Scalar pos_one = Scalar(1);
+  const Scalar pos_inf = NumTraits<Scalar>::infinity();
+
+  const Packet cst_pos_zero = pzero(x);
+  const Packet cst_pos_one = pset1<Packet>(pos_one);
+  const Packet cst_pos_inf = pset1<Packet>(pos_inf);
+
+  const bool exponent_is_not_fin = !(numext::isfinite)(exponent);
+  const bool exponent_is_neg = exponent < ScalarExponent(0);
+  const bool exponent_is_pos = exponent > ScalarExponent(0);
+
+  const Packet exp_is_not_fin = pset1<Packet>(exponent_is_not_fin ? all_ones : pos_zero);
+  const Packet exp_is_neg = pset1<Packet>(exponent_is_neg ? all_ones : pos_zero);
+  const Packet exp_is_pos = pset1<Packet>(exponent_is_pos ? all_ones : pos_zero);
+  const Packet exp_is_inf = pand(exp_is_not_fin, por(exp_is_neg, exp_is_pos));
+  const Packet exp_is_nan = pandnot(exp_is_not_fin, por(exp_is_neg, exp_is_pos));
+
+  const Packet x_is_le_zero = pcmp_le(x, cst_pos_zero);
+  const Packet x_is_ge_zero = pcmp_le(cst_pos_zero, x);
+  const Packet x_is_zero = pand(x_is_le_zero, x_is_ge_zero);
+
+  const Packet abs_x = pabs(x);
+  const Packet abs_x_is_le_one = pcmp_le(abs_x, cst_pos_one);
+  const Packet abs_x_is_ge_one = pcmp_le(cst_pos_one, abs_x);
+  const Packet abs_x_is_inf = pcmp_eq(abs_x, cst_pos_inf);
+  const Packet abs_x_is_one = pand(abs_x_is_le_one, abs_x_is_ge_one);
+
+  Packet pow_is_inf_if_exp_is_neg = por(x_is_zero, pand(abs_x_is_le_one, exp_is_inf));
+  Packet pow_is_inf_if_exp_is_pos = por(abs_x_is_inf, pand(abs_x_is_ge_one, exp_is_inf));
+  Packet pow_is_one = pand(abs_x_is_one, por(exp_is_inf, x_is_ge_zero));
+
+  Packet result = powx;
+  result = por(x_is_le_zero, result);
+  result = pselect(pow_is_inf_if_exp_is_neg, pand(cst_pos_inf, exp_is_neg), result);
+  result = pselect(pow_is_inf_if_exp_is_pos, pand(cst_pos_inf, exp_is_pos), result);
+  result = por(exp_is_nan, result);
+  result = pselect(pow_is_one, cst_pos_one, result);
+  return result;
+}
+
+template <typename Packet, typename ScalarExponent,
+          std::enable_if_t<NumTraits<typename unpacket_traits<Packet>::type>::IsSigned, bool> = true>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet handle_negative_exponent(const Packet& x, const ScalarExponent& exponent) {
+  using Scalar = typename unpacket_traits<Packet>::type;
+
+  // singed integer base, signed integer exponent case
+
+  // This routine handles negative exponents.
+  // The return value is either 0, 1, or -1.
+
+  const Scalar pos_zero = Scalar(0);
+  const Scalar all_ones = ptrue<Scalar>(Scalar());
+  const Scalar pos_one = Scalar(1);
+
+  const Packet cst_pos_one = pset1<Packet>(pos_one);
+
+  const bool exponent_is_odd = exponent % ScalarExponent(2) != ScalarExponent(0);
+
+  const Packet exp_is_odd = pset1<Packet>(exponent_is_odd ? all_ones : pos_zero);
+
+  const Packet abs_x = pabs(x);
+  const Packet abs_x_is_one = pcmp_eq(abs_x, cst_pos_one);
+
+  Packet result = pselect(exp_is_odd, x, abs_x);
+  result = pand(abs_x_is_one, result);
+  return result;
+}
+
+template <typename Packet, typename ScalarExponent,
+          std::enable_if_t<!NumTraits<typename unpacket_traits<Packet>::type>::IsSigned, bool> = true>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet handle_negative_exponent(const Packet& x, const ScalarExponent&) {
+  using Scalar = typename unpacket_traits<Packet>::type;
+
+  // unsigned integer base, signed integer exponent case
+
+  // This routine handles negative exponents.
+  // The return value is either 0 or 1
+
+  const Scalar pos_one = Scalar(1);
+
+  const Packet cst_pos_one = pset1<Packet>(pos_one);
+
+  const Packet x_is_one = pcmp_eq(x, cst_pos_one);
+
+  return pand(x_is_one, x);
+}
+
+}  // end namespace unary_pow
+
+template <typename Packet, typename ScalarExponent,
+          bool BaseIsIntegerType = NumTraits<typename unpacket_traits<Packet>::type>::IsInteger,
+          bool ExponentIsIntegerType = NumTraits<ScalarExponent>::IsInteger,
+          bool ExponentIsSigned = NumTraits<ScalarExponent>::IsSigned>
+struct unary_pow_impl;
+
+template <typename Packet, typename ScalarExponent, bool ExponentIsSigned>
+struct unary_pow_impl<Packet, ScalarExponent, false, false, ExponentIsSigned> {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const ScalarExponent& exponent) {
+    const bool exponent_is_integer = (numext::isfinite)(exponent) && numext::round(exponent) == exponent;
+    if (exponent_is_integer) {
+      return unary_pow::int_pow(x, exponent);
+    } else {
+      Packet result = unary_pow::gen_pow(x, exponent);
+      result = unary_pow::handle_nonint_nonint_errors(x, result, exponent);
+      return result;
+    }
+  }
+};
+
+template <typename Packet, typename ScalarExponent, bool ExponentIsSigned>
+struct unary_pow_impl<Packet, ScalarExponent, false, true, ExponentIsSigned> {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const ScalarExponent& exponent) {
+    return unary_pow::int_pow(x, exponent);
+  }
+};
+
+template <typename Packet, typename ScalarExponent>
+struct unary_pow_impl<Packet, ScalarExponent, true, true, true> {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const ScalarExponent& exponent) {
+    if (exponent < ScalarExponent(0)) {
+      return unary_pow::handle_negative_exponent(x, exponent);
+    } else {
+      return unary_pow::int_pow(x, exponent);
+    }
+  }
+};
+
+template <typename Packet, typename ScalarExponent>
+struct unary_pow_impl<Packet, ScalarExponent, true, true, false> {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const ScalarExponent& exponent) {
+    return unary_pow::int_pow(x, exponent);
+  }
+};
+
+}  // end namespace internal
+}  // end namespace Eigen
+
+#endif  // EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
index 177a04e..ade9f3f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
@@ -10,6 +10,9 @@
 #ifndef EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H
 #define EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
@@ -19,92 +22,137 @@
 
 /***************************************************************************
  * Some generic implementations to be used by implementors
-***************************************************************************/
+ ***************************************************************************/
 
 /** Default implementation of pfrexp.
-  * It is expected to be called by implementers of template<> pfrexp.
-  */
-template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-Packet pfrexp_generic(const Packet& a, Packet& exponent);
+ * It is expected to be called by implementers of template<> pfrexp.
+ */
+template <typename Packet>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pfrexp_generic(const Packet& a, Packet& exponent);
 
 // Extracts the biased exponent value from Packet p, and casts the results to
 // a floating-point Packet type. Used by pfrexp_generic. Override this if
 // there is no unpacket_traits<Packet>::integer_packet.
-template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-Packet pfrexp_generic_get_biased_exponent(const Packet& p);
+template <typename Packet>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pfrexp_generic_get_biased_exponent(const Packet& p);
 
 /** Default implementation of pldexp.
-  * It is expected to be called by implementers of template<> pldexp.
-  */
-template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-Packet pldexp_generic(const Packet& a, const Packet& exponent);
+ * It is expected to be called by implementers of template<> pldexp.
+ */
+template <typename Packet>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pldexp_generic(const Packet& a, const Packet& exponent);
 
 /** \internal \returns log(x) for single precision float */
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog_float(const Packet _x);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog_float(const Packet _x);
 
 /** \internal \returns log2(x) for single precision float */
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog2_float(const Packet _x);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog2_float(const Packet _x);
 
 /** \internal \returns log(x) for single precision float */
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog_double(const Packet _x);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog_double(const Packet _x);
 
 /** \internal \returns log2(x) for single precision float */
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet plog2_double(const Packet _x);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog2_double(const Packet _x);
 
 /** \internal \returns log(1 + x) */
-template<typename Packet>
+template <typename Packet>
 Packet generic_plog1p(const Packet& x);
 
 /** \internal \returns exp(x)-1 */
-template<typename Packet>
+template <typename Packet>
 Packet generic_expm1(const Packet& x);
 
 /** \internal \returns exp(x) for single precision float */
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet pexp_float(const Packet _x);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_float(const Packet _x);
 
 /** \internal \returns exp(x) for double precision real numbers */
 template <typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet pexp_double(const Packet _x);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_double(const Packet _x);
 
 /** \internal \returns sin(x) for single precision float */
-template<typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet psin_float(const Packet& x);
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psin_float(const Packet& x);
 
 /** \internal \returns cos(x) for single precision float */
-template<typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet pcos_float(const Packet& x);
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcos_float(const Packet& x);
+
+/** \internal \returns asin(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pasin_float(const Packet& x);
+
+/** \internal \returns acos(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pacos_float(const Packet& x);
+
+/** \internal \returns atan(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan_float(const Packet& x);
+
+/** \internal \returns atan(x) for double precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan_double(const Packet& x);
+
+/** \internal \returns atanh(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patanh_float(const Packet& x);
 
 /** \internal \returns sqrt(x) for complex types */
-template<typename Packet>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-EIGEN_UNUSED
-Packet psqrt_complex(const Packet& a);
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psqrt_complex(const Packet& a);
 
-template <typename Packet, int N> struct ppolevl;
+/** \internal \returns x / y for complex types */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pdiv_complex(const Packet& x, const Packet& y);
 
+template <typename Packet, int N>
+struct ppolevl;
 
-} // end namespace internal
-} // end namespace Eigen
+// Macros for instantiating these generic functions for different backends.
+#define EIGEN_PACKET_FUNCTION(METHOD, SCALAR, PACKET)                                             \
+  template <>                                                                                     \
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED PACKET p##METHOD<PACKET>(const PACKET& _x) { \
+    return p##METHOD##_##SCALAR(_x);                                                              \
+  }
 
-#endif // EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H
+#define EIGEN_FLOAT_PACKET_FUNCTION(METHOD, PACKET) EIGEN_PACKET_FUNCTION(METHOD, float, PACKET)
+#define EIGEN_DOUBLE_PACKET_FUNCTION(METHOD, PACKET) EIGEN_PACKET_FUNCTION(METHOD, double, PACKET)
+
+#define EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_FLOAT(PACKET)                                     \
+  EIGEN_FLOAT_PACKET_FUNCTION(sin, PACKET)                                                     \
+  EIGEN_FLOAT_PACKET_FUNCTION(cos, PACKET)                                                     \
+  EIGEN_FLOAT_PACKET_FUNCTION(asin, PACKET)                                                    \
+  EIGEN_FLOAT_PACKET_FUNCTION(acos, PACKET)                                                    \
+  EIGEN_FLOAT_PACKET_FUNCTION(atan, PACKET)                                                    \
+  EIGEN_FLOAT_PACKET_FUNCTION(atanh, PACKET)                                                   \
+  EIGEN_FLOAT_PACKET_FUNCTION(log, PACKET)                                                     \
+  EIGEN_FLOAT_PACKET_FUNCTION(log2, PACKET)                                                    \
+  EIGEN_FLOAT_PACKET_FUNCTION(exp, PACKET)                                                     \
+  template <>                                                                                  \
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED PACKET pexpm1<PACKET>(const PACKET& _x) { \
+    return internal::generic_expm1(_x);                                                        \
+  }                                                                                            \
+  template <>                                                                                  \
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED PACKET plog1p<PACKET>(const PACKET& _x) { \
+    return internal::generic_plog1p(_x);                                                       \
+  }                                                                                            \
+  template <>                                                                                  \
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED PACKET ptanh<PACKET>(const PACKET& _x) {  \
+    return internal::generic_fast_tanh_float(_x);                                              \
+  }
+
+#define EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_DOUBLE(PACKET) \
+  EIGEN_DOUBLE_PACKET_FUNCTION(atan, PACKET)                \
+  EIGEN_DOUBLE_PACKET_FUNCTION(log, PACKET)                 \
+  EIGEN_DOUBLE_PACKET_FUNCTION(log2, PACKET)                \
+  EIGEN_DOUBLE_PACKET_FUNCTION(exp, PACKET)
+
+}  // end namespace internal
+}  // end namespace Eigen
+
+#endif  // EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h
index 9f8e8cc..92516c7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h
@@ -24,7 +24,6 @@
 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
-
 // Standard 16-bit float type, mostly useful for GPUs. Defines a new
 // type Eigen::half (inheriting either from CUDA's or HIP's __half struct) with
 // operator overloads such that it behaves basically as an arithmetic
@@ -32,11 +31,11 @@
 // in fp32 for CPUs, except for simple parameter conversions, I/O
 // to disk and the likes), but fast on GPUs.
 
-
 #ifndef EIGEN_HALF_H
 #define EIGEN_HALF_H
 
-#include <sstream>
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
 
 #if defined(EIGEN_HAS_GPU_FP16) || defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
 // When compiling with GPU support, the "__half_raw" base class as well as
@@ -45,16 +44,15 @@
 // As a consequence, we get compile failures when compiling Eigen with
 // GPU support. Hence the need to disable EIGEN_CONSTEXPR when building
 // Eigen with GPU support
-  #pragma push_macro("EIGEN_CONSTEXPR")
-  #undef EIGEN_CONSTEXPR
-  #define EIGEN_CONSTEXPR
+#pragma push_macro("EIGEN_CONSTEXPR")
+#undef EIGEN_CONSTEXPR
+#define EIGEN_CONSTEXPR
 #endif
 
-#define F16_PACKET_FUNCTION(PACKET_F, PACKET_F16, METHOD)           \
-  template <>                                                       \
-  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED                \
-  PACKET_F16 METHOD<PACKET_F16>(const PACKET_F16& _x) {             \
-    return float2half(METHOD<PACKET_F>(half2float(_x)));            \
+#define F16_PACKET_FUNCTION(PACKET_F, PACKET_F16, METHOD)                                                  \
+  template <>                                                                                              \
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED PACKET_F16 METHOD<PACKET_F16>(const PACKET_F16& _x) { \
+    return float2half(METHOD<PACKET_F>(half2float(_x)));                                                   \
   }
 
 namespace Eigen {
@@ -96,8 +94,7 @@
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw() : x(0) {}
 #endif
 #if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
-  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw(numext::uint16_t raw) : x(numext::bit_cast<__fp16>(raw)) {
-  }
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw(numext::uint16_t raw) : x(numext::bit_cast<__fp16>(raw)) {}
   __fp16 x;
 #else
   explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw(numext::uint16_t raw) : x(raw) {}
@@ -106,15 +103,15 @@
 };
 
 #elif defined(EIGEN_HAS_HIP_FP16)
-  // Nothing to do here
-  // HIP fp16 header file has a definition for __half_raw
+// Nothing to do here
+// HIP fp16 header file has a definition for __half_raw
 #elif defined(EIGEN_HAS_CUDA_FP16)
-  #if EIGEN_CUDA_SDK_VER < 90000
-    // In CUDA < 9.0, __half is the equivalent of CUDA 9's __half_raw
-    typedef __half __half_raw;
-  #endif // defined(EIGEN_HAS_CUDA_FP16)
+#if EIGEN_CUDA_SDK_VER < 90000
+// In CUDA < 9.0, __half is the equivalent of CUDA 9's __half_raw
+typedef __half __half_raw;
+#endif  // defined(EIGEN_HAS_CUDA_FP16)
 #elif defined(SYCL_DEVICE_ONLY)
-  typedef cl::sycl::half __half_raw;
+typedef cl::sycl::half __half_raw;
 #endif
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw raw_uint16_to_half(numext::uint16_t x);
@@ -126,21 +123,20 @@
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half_raw& h) : __half_raw(h) {}
 
 #if defined(EIGEN_HAS_GPU_FP16)
- #if defined(EIGEN_HAS_HIP_FP16)
+#if defined(EIGEN_HAS_HIP_FP16)
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half& h) { x = __half_as_ushort(h); }
- #elif defined(EIGEN_HAS_CUDA_FP16)
-  #if EIGEN_CUDA_SDK_VER >= 90000
+#elif defined(EIGEN_HAS_CUDA_FP16)
+#if EIGEN_CUDA_SDK_VER >= 90000
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half& h) : __half_raw(*(__half_raw*)&h) {}
-  #endif
- #endif
+#endif
+#endif
 #endif
 };
 
-} // namespace half_impl
+}  // namespace half_impl
 
 // Class definition.
 struct half : public half_impl::half_base {
-
   // Writing this out as separate #if-else blocks to make the code easier to follow
   // The same applies to most #if-else blocks in this file
 #if !defined(EIGEN_HAS_GPU_FP16) || !defined(EIGEN_GPU_COMPILE_PHASE)
@@ -152,12 +148,12 @@
   // Nothing to do here
   // HIP fp16 header file has a definition for __half_raw
 #elif defined(EIGEN_HAS_CUDA_FP16)
-  // Note that EIGEN_CUDA_SDK_VER is set to 0 even when compiling with HIP, so
-  // (EIGEN_CUDA_SDK_VER < 90000) is true even for HIP!  So keeping this within
-  // #if defined(EIGEN_HAS_CUDA_FP16) is needed
-  #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER < 90000
-    typedef half_impl::__half_raw __half_raw;
-  #endif
+// Note that EIGEN_CUDA_SDK_VER is set to 0 even when compiling with HIP, so
+// (EIGEN_CUDA_SDK_VER < 90000) is true even for HIP!  So keeping this within
+// #if defined(EIGEN_HAS_CUDA_FP16) is needed
+#if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER < 90000
+  typedef half_impl::__half_raw __half_raw;
+#endif
 #endif
 
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half() {}
@@ -165,31 +161,29 @@
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half_raw& h) : half_impl::half_base(h) {}
 
 #if defined(EIGEN_HAS_GPU_FP16)
- #if defined(EIGEN_HAS_HIP_FP16)
+#if defined(EIGEN_HAS_HIP_FP16)
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {}
- #elif defined(EIGEN_HAS_CUDA_FP16)
-  #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000
+#elif defined(EIGEN_HAS_CUDA_FP16)
+#if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {}
-  #endif
- #endif
 #endif
-
+#endif
+#endif
 
   explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(bool b)
       : half_impl::half_base(half_impl::raw_uint16_to_half(b ? 0x3c00 : 0)) {}
-  template<class T>
+  template <class T>
   explicit EIGEN_DEVICE_FUNC half(T val)
       : half_impl::half_base(half_impl::float_to_half_rtne(static_cast<float>(val))) {}
-  explicit EIGEN_DEVICE_FUNC half(float f)
-      : half_impl::half_base(half_impl::float_to_half_rtne(f)) {}
+  explicit EIGEN_DEVICE_FUNC half(float f) : half_impl::half_base(half_impl::float_to_half_rtne(f)) {}
 
   // Following the convention of numpy, converting between complex and
   // float will lead to loss of imag value.
-  template<typename RealScalar>
+  template <typename RealScalar>
   explicit EIGEN_DEVICE_FUNC half(std::complex<RealScalar> c)
       : half_impl::half_base(half_impl::float_to_half_rtne(static_cast<float>(c.real()))) {}
 
-   EIGEN_DEVICE_FUNC operator float() const {  // NOLINT: Allow implicit conversion to float, because it is lossless.
+  EIGEN_DEVICE_FUNC operator float() const {  // NOLINT: Allow implicit conversion to float, because it is lossless.
     return half_impl::half_to_float(*this);
   }
 
@@ -202,66 +196,123 @@
 #endif
 };
 
-} // end namespace Eigen
+// TODO(majnemer): Get rid of this once we can rely on C++17 inline variables do
+// solve the ODR issue.
+namespace half_impl {
+template <typename = void>
+struct numeric_limits_half_impl {
+  static EIGEN_CONSTEXPR const bool is_specialized = true;
+  static EIGEN_CONSTEXPR const bool is_signed = true;
+  static EIGEN_CONSTEXPR const bool is_integer = false;
+  static EIGEN_CONSTEXPR const bool is_exact = false;
+  static EIGEN_CONSTEXPR const bool has_infinity = true;
+  static EIGEN_CONSTEXPR const bool has_quiet_NaN = true;
+  static EIGEN_CONSTEXPR const bool has_signaling_NaN = true;
+  static EIGEN_CONSTEXPR const std::float_denorm_style has_denorm = std::denorm_present;
+  static EIGEN_CONSTEXPR const bool has_denorm_loss = false;
+  static EIGEN_CONSTEXPR const std::float_round_style round_style = std::round_to_nearest;
+  static EIGEN_CONSTEXPR const bool is_iec559 = true;
+  // The C++ standard defines this as "true if the set of values representable
+  // by the type is finite." Half has finite precision.
+  static EIGEN_CONSTEXPR const bool is_bounded = true;
+  static EIGEN_CONSTEXPR const bool is_modulo = false;
+  static EIGEN_CONSTEXPR const int digits = 11;
+  static EIGEN_CONSTEXPR const int digits10 =
+      3;  // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
+  static EIGEN_CONSTEXPR const int max_digits10 =
+      5;  // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
+  static EIGEN_CONSTEXPR const int radix = std::numeric_limits<float>::radix;
+  static EIGEN_CONSTEXPR const int min_exponent = -13;
+  static EIGEN_CONSTEXPR const int min_exponent10 = -4;
+  static EIGEN_CONSTEXPR const int max_exponent = 16;
+  static EIGEN_CONSTEXPR const int max_exponent10 = 4;
+  static EIGEN_CONSTEXPR const bool traps = std::numeric_limits<float>::traps;
+  // IEEE754: "The implementer shall choose how tininess is detected, but shall
+  // detect tininess in the same way for all operations in radix two"
+  static EIGEN_CONSTEXPR const bool tinyness_before = std::numeric_limits<float>::tinyness_before;
 
-namespace std {
-template<>
-struct numeric_limits<Eigen::half> {
-  static const bool is_specialized = true;
-  static const bool is_signed = true;
-  static const bool is_integer = false;
-  static const bool is_exact = false;
-  static const bool has_infinity = true;
-  static const bool has_quiet_NaN = true;
-  static const bool has_signaling_NaN = true;
-  static const float_denorm_style has_denorm = denorm_present;
-  static const bool has_denorm_loss = false;
-  static const std::float_round_style round_style = std::round_to_nearest;
-  static const bool is_iec559 = false;
-  static const bool is_bounded = false;
-  static const bool is_modulo = false;
-  static const int digits = 11;
-  static const int digits10 = 3;      // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
-  static const int max_digits10 = 5;  // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
-  static const int radix = 2;
-  static const int min_exponent = -13;
-  static const int min_exponent10 = -4;
-  static const int max_exponent = 16;
-  static const int max_exponent10 = 4;
-  static const bool traps = true;
-  static const bool tinyness_before = false;
-
-  static Eigen::half (min)() { return Eigen::half_impl::raw_uint16_to_half(0x400); }
-  static Eigen::half lowest() { return Eigen::half_impl::raw_uint16_to_half(0xfbff); }
-  static Eigen::half (max)() { return Eigen::half_impl::raw_uint16_to_half(0x7bff); }
-  static Eigen::half epsilon() { return Eigen::half_impl::raw_uint16_to_half(0x0800); }
-  static Eigen::half round_error() { return Eigen::half(0.5); }
-  static Eigen::half infinity() { return Eigen::half_impl::raw_uint16_to_half(0x7c00); }
-  static Eigen::half quiet_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); }
-  static Eigen::half signaling_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7d00); }
-  static Eigen::half denorm_min() { return Eigen::half_impl::raw_uint16_to_half(0x1); }
+  static EIGEN_CONSTEXPR Eigen::half(min)() { return Eigen::half_impl::raw_uint16_to_half(0x0400); }
+  static EIGEN_CONSTEXPR Eigen::half lowest() { return Eigen::half_impl::raw_uint16_to_half(0xfbff); }
+  static EIGEN_CONSTEXPR Eigen::half(max)() { return Eigen::half_impl::raw_uint16_to_half(0x7bff); }
+  static EIGEN_CONSTEXPR Eigen::half epsilon() { return Eigen::half_impl::raw_uint16_to_half(0x1400); }
+  static EIGEN_CONSTEXPR Eigen::half round_error() { return Eigen::half_impl::raw_uint16_to_half(0x3800); }
+  static EIGEN_CONSTEXPR Eigen::half infinity() { return Eigen::half_impl::raw_uint16_to_half(0x7c00); }
+  static EIGEN_CONSTEXPR Eigen::half quiet_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); }
+  static EIGEN_CONSTEXPR Eigen::half signaling_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7d00); }
+  static EIGEN_CONSTEXPR Eigen::half denorm_min() { return Eigen::half_impl::raw_uint16_to_half(0x0001); }
 };
 
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::is_specialized;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::is_signed;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::is_integer;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::is_exact;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::has_infinity;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::has_quiet_NaN;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::has_signaling_NaN;
+template <typename T>
+EIGEN_CONSTEXPR const std::float_denorm_style numeric_limits_half_impl<T>::has_denorm;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::has_denorm_loss;
+template <typename T>
+EIGEN_CONSTEXPR const std::float_round_style numeric_limits_half_impl<T>::round_style;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::is_iec559;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::is_bounded;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::is_modulo;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::digits;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::digits10;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::max_digits10;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::radix;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::min_exponent;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::min_exponent10;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::max_exponent;
+template <typename T>
+EIGEN_CONSTEXPR const int numeric_limits_half_impl<T>::max_exponent10;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::traps;
+template <typename T>
+EIGEN_CONSTEXPR const bool numeric_limits_half_impl<T>::tinyness_before;
+}  // end namespace half_impl
+}  // end namespace Eigen
+
+namespace std {
 // If std::numeric_limits<T> is specialized, should also specialize
 // std::numeric_limits<const T>, std::numeric_limits<volatile T>, and
 // std::numeric_limits<const volatile T>
 // https://stackoverflow.com/a/16519653/
-template<>
-struct numeric_limits<const Eigen::half> : numeric_limits<Eigen::half> {};
-template<>
-struct numeric_limits<volatile Eigen::half> : numeric_limits<Eigen::half> {};
-template<>
-struct numeric_limits<const volatile Eigen::half> : numeric_limits<Eigen::half> {};
-} // end namespace std
+template <>
+class numeric_limits<Eigen::half> : public Eigen::half_impl::numeric_limits_half_impl<> {};
+template <>
+class numeric_limits<const Eigen::half> : public numeric_limits<Eigen::half> {};
+template <>
+class numeric_limits<volatile Eigen::half> : public numeric_limits<Eigen::half> {};
+template <>
+class numeric_limits<const volatile Eigen::half> : public numeric_limits<Eigen::half> {};
+}  // end namespace std
 
 namespace Eigen {
 
 namespace half_impl {
 
-#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && \
-     EIGEN_CUDA_ARCH >= 530) ||                                  \
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
     (defined(EIGEN_HAS_HIP_FP16) && defined(HIP_DEVICE_COMPILE))
-// Note: We deliberatly do *not* define this to 1 even if we have Arm's native
+// Note: We deliberately do *not* define this to 1 even if we have Arm's native
 // fp16 type since GPU halfs are rather different from native CPU halfs.
 // TODO: Rename to something like EIGEN_HAS_NATIVE_GPU_FP16
 #define EIGEN_HAS_NATIVE_FP16
@@ -273,20 +324,16 @@
 // conversion steps back and forth.
 
 #if defined(EIGEN_HAS_NATIVE_FP16)
-EIGEN_STRONG_INLINE __device__ half operator + (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half operator+(const half& a, const half& b) {
 #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000
   return __hadd(::__half(a), ::__half(b));
 #else
   return __hadd(a, b);
 #endif
 }
-EIGEN_STRONG_INLINE __device__ half operator * (const half& a, const half& b) {
-  return __hmul(a, b);
-}
-EIGEN_STRONG_INLINE __device__ half operator - (const half& a, const half& b) {
-  return __hsub(a, b);
-}
-EIGEN_STRONG_INLINE __device__ half operator / (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half operator*(const half& a, const half& b) { return __hmul(a, b); }
+EIGEN_STRONG_INLINE __device__ half operator-(const half& a, const half& b) { return __hsub(a, b); }
+EIGEN_STRONG_INLINE __device__ half operator/(const half& a, const half& b) {
 #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000
   return __hdiv(a, b);
 #else
@@ -295,173 +342,121 @@
   return __float2half(num / denom);
 #endif
 }
-EIGEN_STRONG_INLINE __device__ half operator - (const half& a) {
-  return __hneg(a);
-}
-EIGEN_STRONG_INLINE __device__ half& operator += (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half operator-(const half& a) { return __hneg(a); }
+EIGEN_STRONG_INLINE __device__ half& operator+=(half& a, const half& b) {
   a = a + b;
   return a;
 }
-EIGEN_STRONG_INLINE __device__ half& operator *= (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half& operator*=(half& a, const half& b) {
   a = a * b;
   return a;
 }
-EIGEN_STRONG_INLINE __device__ half& operator -= (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half& operator-=(half& a, const half& b) {
   a = a - b;
   return a;
 }
-EIGEN_STRONG_INLINE __device__ half& operator /= (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half& operator/=(half& a, const half& b) {
   a = a / b;
   return a;
 }
-EIGEN_STRONG_INLINE __device__ bool operator == (const half& a, const half& b) {
-  return __heq(a, b);
-}
-EIGEN_STRONG_INLINE __device__ bool operator != (const half& a, const half& b) {
-  return __hne(a, b);
-}
-EIGEN_STRONG_INLINE __device__ bool operator < (const half& a, const half& b) {
-  return __hlt(a, b);
-}
-EIGEN_STRONG_INLINE __device__ bool operator <= (const half& a, const half& b) {
-  return __hle(a, b);
-}
-EIGEN_STRONG_INLINE __device__ bool operator > (const half& a, const half& b) {
-  return __hgt(a, b);
-}
-EIGEN_STRONG_INLINE __device__ bool operator >= (const half& a, const half& b) {
-  return __hge(a, b);
-}
+EIGEN_STRONG_INLINE __device__ bool operator==(const half& a, const half& b) { return __heq(a, b); }
+EIGEN_STRONG_INLINE __device__ bool operator!=(const half& a, const half& b) { return __hne(a, b); }
+EIGEN_STRONG_INLINE __device__ bool operator<(const half& a, const half& b) { return __hlt(a, b); }
+EIGEN_STRONG_INLINE __device__ bool operator<=(const half& a, const half& b) { return __hle(a, b); }
+EIGEN_STRONG_INLINE __device__ bool operator>(const half& a, const half& b) { return __hgt(a, b); }
+EIGEN_STRONG_INLINE __device__ bool operator>=(const half& a, const half& b) { return __hge(a, b); }
 #endif
 
-#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator + (const half& a, const half& b) {
-  return half(vaddh_f16(a.x, b.x));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator * (const half& a, const half& b) {
-  return half(vmulh_f16(a.x, b.x));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a, const half& b) {
-  return half(vsubh_f16(a.x, b.x));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator / (const half& a, const half& b) {
-  return half(vdivh_f16(a.x, b.x));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a) {
-  return half(vnegh_f16(a.x));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator += (half& a, const half& b) {
+#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) && !defined(EIGEN_GPU_COMPILE_PHASE)
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator+(const half& a, const half& b) { return half(vaddh_f16(a.x, b.x)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator*(const half& a, const half& b) { return half(vmulh_f16(a.x, b.x)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator-(const half& a, const half& b) { return half(vsubh_f16(a.x, b.x)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator/(const half& a, const half& b) { return half(vdivh_f16(a.x, b.x)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator-(const half& a) { return half(vnegh_f16(a.x)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator+=(half& a, const half& b) {
   a = half(vaddh_f16(a.x, b.x));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator *= (half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator*=(half& a, const half& b) {
   a = half(vmulh_f16(a.x, b.x));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator -= (half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator-=(half& a, const half& b) {
   a = half(vsubh_f16(a.x, b.x));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator /= (half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator/=(half& a, const half& b) {
   a = half(vdivh_f16(a.x, b.x));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator == (const half& a, const half& b) {
-  return vceqh_f16(a.x, b.x);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator != (const half& a, const half& b) {
-  return !vceqh_f16(a.x, b.x);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator < (const half& a, const half& b) {
-  return vclth_f16(a.x, b.x);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator <= (const half& a, const half& b) {
-  return vcleh_f16(a.x, b.x);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator > (const half& a, const half& b) {
-  return vcgth_f16(a.x, b.x);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator >= (const half& a, const half& b) {
-  return vcgeh_f16(a.x, b.x);
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator==(const half& a, const half& b) { return vceqh_f16(a.x, b.x); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator!=(const half& a, const half& b) { return !vceqh_f16(a.x, b.x); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<(const half& a, const half& b) { return vclth_f16(a.x, b.x); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<=(const half& a, const half& b) { return vcleh_f16(a.x, b.x); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>(const half& a, const half& b) { return vcgth_f16(a.x, b.x); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>=(const half& a, const half& b) { return vcgeh_f16(a.x, b.x); }
 // We need to distinguish ‘clang as the CUDA compiler’ from ‘clang as the host compiler,
 // invoked by NVCC’ (e.g. on MacOS). The former needs to see both host and device implementation
 // of the functions, while the latter can only deal with one of them.
-#elif !defined(EIGEN_HAS_NATIVE_FP16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC) // Emulate support for half floats
+#elif !defined(EIGEN_HAS_NATIVE_FP16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC)  // Emulate support for half floats
 
-#if EIGEN_COMP_CLANG && defined(EIGEN_CUDACC)
+#if EIGEN_COMP_CLANG && defined(EIGEN_GPUCC)
 // We need to provide emulated *host-side* FP16 operators for clang.
 #pragma push_macro("EIGEN_DEVICE_FUNC")
 #undef EIGEN_DEVICE_FUNC
 #if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_HAS_NATIVE_FP16)
 #define EIGEN_DEVICE_FUNC __host__
-#else // both host and device need emulated ops.
+#else  // both host and device need emulated ops.
 #define EIGEN_DEVICE_FUNC __host__ __device__
 #endif
 #endif
 
 // Definitions for CPUs and older HIP+CUDA, mostly working through conversion
 // to/from fp32.
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator + (const half& a, const half& b) {
-  return half(float(a) + float(b));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator * (const half& a, const half& b) {
-  return half(float(a) * float(b));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a, const half& b) {
-  return half(float(a) - float(b));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator / (const half& a, const half& b) {
-  return half(float(a) / float(b));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator+(const half& a, const half& b) { return half(float(a) + float(b)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator*(const half& a, const half& b) { return half(float(a) * float(b)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator-(const half& a, const half& b) { return half(float(a) - float(b)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator/(const half& a, const half& b) { return half(float(a) / float(b)); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator-(const half& a) {
   half result;
   result.x = a.x ^ 0x8000;
   return result;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator += (half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator+=(half& a, const half& b) {
   a = half(float(a) + float(b));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator *= (half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator*=(half& a, const half& b) {
   a = half(float(a) * float(b));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator -= (half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator-=(half& a, const half& b) {
   a = half(float(a) - float(b));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator /= (half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator/=(half& a, const half& b) {
   a = half(float(a) / float(b));
   return a;
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator == (const half& a, const half& b) {
-  return numext::equal_strict(float(a),float(b));
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator==(const half& a, const half& b) {
+  return numext::equal_strict(float(a), float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator != (const half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator!=(const half& a, const half& b) {
   return numext::not_equal_strict(float(a), float(b));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator < (const half& a, const half& b) {
-  return float(a) < float(b);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator <= (const half& a, const half& b) {
-  return float(a) <= float(b);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator > (const half& a, const half& b) {
-  return float(a) > float(b);
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator >= (const half& a, const half& b) {
-  return float(a) >= float(b);
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<(const half& a, const half& b) { return float(a) < float(b); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<=(const half& a, const half& b) { return float(a) <= float(b); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>(const half& a, const half& b) { return float(a) > float(b); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>=(const half& a, const half& b) { return float(a) >= float(b); }
 
-#if defined(__clang__) && defined(__CUDA__)
+#if EIGEN_COMP_CLANG && defined(EIGEN_GPUCC)
 #pragma pop_macro("EIGEN_DEVICE_FUNC")
 #endif
 #endif  // Emulate support for half floats
 
 // Division by an index. Do it in full float precision to avoid accuracy
 // issues in converting the denominator to half.
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator / (const half& a, Index b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator/(const half& a, Index b) {
   return half(static_cast<float>(a) / static_cast<float>(b));
 }
 
@@ -500,8 +495,8 @@
   // Fortunately, since we need to disable EIGEN_CONSTEXPR for GPU anyway, we can get out
   // of this catch22 by having separate bodies for GPU / non GPU
 #if defined(EIGEN_HAS_GPU_FP16)
-   __half_raw h;
-   h.x = x;
+  __half_raw h;
+  h.x = x;
   return h;
 #else
   return __half_raw(x);
@@ -528,13 +523,18 @@
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff) {
 #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
-  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
   __half tmp_ff = __float2half(ff);
   return *(__half_raw*)&tmp_ff;
 
 #elif defined(EIGEN_HAS_FP16_C)
   __half_raw h;
+#if EIGEN_COMP_MSVC
+  // MSVC does not have scalar instructions.
+  h.x = _mm_extract_epi16(_mm_cvtps_ph(_mm_set_ss(ff), 0), 0);
+#else
   h.x = _cvtss_sh(ff, 0);
+#endif
   return h;
 
 #elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
@@ -543,11 +543,12 @@
   return h;
 
 #else
-  float32_bits f; f.f = ff;
+  float32_bits f;
+  f.f = ff;
 
-  const float32_bits f32infty = { 255 << 23 };
-  const float32_bits f16max = { (127 + 16) << 23 };
-  const float32_bits denorm_magic = { ((127 - 15) + (23 - 10) + 1) << 23 };
+  const float32_bits f32infty = {255 << 23};
+  const float32_bits f16max = {(127 + 16) << 23};
+  const float32_bits denorm_magic = {((127 - 15) + (23 - 10) + 1) << 23};
   unsigned int sign_mask = 0x80000000u;
   __half_raw o;
   o.x = static_cast<numext::uint16_t>(0x0u);
@@ -560,10 +561,10 @@
   // 0x80000000. Important if you want fast straight SSE2 code
   // (since there's no unsigned PCMPGTD).
 
-  if (f.u >= f16max.u) {  // result is Inf or NaN (all exponent bits set)
-    o.x = (f.u > f32infty.u) ? 0x7e00 : 0x7c00; // NaN->qNaN and Inf->Inf
-  } else {  // (De)normalized number or zero
-    if (f.u < (113 << 23)) {  // resulting FP16 is subnormal or zero
+  if (f.u >= f16max.u) {                         // result is Inf or NaN (all exponent bits set)
+    o.x = (f.u > f32infty.u) ? 0x7e00 : 0x7c00;  // NaN->qNaN and Inf->Inf
+  } else {                                       // (De)normalized number or zero
+    if (f.u < (113 << 23)) {                     // resulting FP16 is subnormal or zero
       // use a magic value to align our 10 mantissa bits at the bottom of
       // the float. as long as FP addition is round-to-nearest-even this
       // just works.
@@ -572,7 +573,7 @@
       // and one integer subtract of the bias later, we have our final float!
       o.x = static_cast<numext::uint16_t>(f.u - denorm_magic.u);
     } else {
-      unsigned int mant_odd = (f.u >> 13) & 1; // resulting mantissa is odd
+      unsigned int mant_odd = (f.u >> 13) & 1;  // resulting mantissa is odd
 
       // update exponent, rounding bias part 1
       // Equivalent to `f.u += ((unsigned int)(15 - 127) << 23) + 0xfff`, but
@@ -592,46 +593,51 @@
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h) {
 #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
-  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
   return __half2float(h);
 #elif defined(EIGEN_HAS_FP16_C)
+#if EIGEN_COMP_MSVC
+  // MSVC does not have scalar instructions.
+  return _mm_cvtss_f32(_mm_cvtph_ps(_mm_set1_epi16(h.x)));
+#else
   return _cvtsh_ss(h.x);
+#endif
 #elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
   return static_cast<float>(h.x);
 #else
-  const float32_bits magic = { 113 << 23 };
-  const unsigned int shifted_exp = 0x7c00 << 13; // exponent mask after shift
+  const float32_bits magic = {113 << 23};
+  const unsigned int shifted_exp = 0x7c00 << 13;  // exponent mask after shift
   float32_bits o;
 
-  o.u = (h.x & 0x7fff) << 13;             // exponent/mantissa bits
-  unsigned int exp = shifted_exp & o.u;   // just the exponent
-  o.u += (127 - 15) << 23;                // exponent adjust
+  o.u = (h.x & 0x7fff) << 13;            // exponent/mantissa bits
+  unsigned int exp = shifted_exp & o.u;  // just the exponent
+  o.u += (127 - 15) << 23;               // exponent adjust
 
   // handle exponent special cases
-  if (exp == shifted_exp) {     // Inf/NaN?
-    o.u += (128 - 16) << 23;    // extra exp adjust
-  } else if (exp == 0) {        // Zero/Denormal?
-    o.u += 1 << 23;             // extra exp adjust
-    o.f -= magic.f;             // renormalize
+  if (exp == shifted_exp) {   // Inf/NaN?
+    o.u += (128 - 16) << 23;  // extra exp adjust
+  } else if (exp == 0) {      // Zero/Denormal?
+    o.u += 1 << 23;           // extra exp adjust
+    o.f -= magic.f;           // renormalize
   }
 
-  o.u |= (h.x & 0x8000) << 16;    // sign bit
+  o.u |= (h.x & 0x8000) << 16;  // sign bit
   return o.f;
 #endif
 }
 
 // --- standard functions ---
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isinf)(const half& a) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isinf)(const half& a) {
 #ifdef EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC
   return (numext::bit_cast<numext::uint16_t>(a.x) & 0x7fff) == 0x7c00;
 #else
   return (a.x & 0x7fff) == 0x7c00;
 #endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isnan)(const half& a) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isnan)(const half& a) {
 #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
-  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
   return __hisnan(a);
 #elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
   return (numext::bit_cast<numext::uint16_t>(a.x) & 0x7fff) > 0x7c00;
@@ -639,8 +645,8 @@
   return (a.x & 0x7fff) > 0x7c00;
 #endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isfinite)(const half& a) {
-  return !(isinf EIGEN_NOT_A_MACRO (a)) && !(isnan EIGEN_NOT_A_MACRO (a));
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isfinite)(const half& a) {
+  return !(isinf EIGEN_NOT_A_MACRO(a)) && !(isnan EIGEN_NOT_A_MACRO(a));
 }
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half abs(const half& a) {
@@ -654,65 +660,53 @@
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half exp(const half& a) {
 #if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 530) || \
-  defined(EIGEN_HIP_DEVICE_COMPILE)
+    defined(EIGEN_HIP_DEVICE_COMPILE)
   return half(hexp(a));
 #else
-   return half(::expf(float(a)));
+  return half(::expf(float(a)));
 #endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half expm1(const half& a) {
-  return half(numext::expm1(float(a)));
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half expm1(const half& a) { return half(numext::expm1(float(a))); }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log(const half& a) {
-#if (defined(EIGEN_HAS_CUDA_FP16) && EIGEN_CUDA_SDK_VER >= 80000 && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
-  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+#if (defined(EIGEN_HAS_CUDA_FP16) && EIGEN_CUDA_SDK_VER >= 80000 && defined(EIGEN_CUDA_ARCH) && \
+     EIGEN_CUDA_ARCH >= 530) ||                                                                 \
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
   return half(::hlog(a));
 #else
   return half(::logf(float(a)));
 #endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log1p(const half& a) {
-  return half(numext::log1p(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log10(const half& a) {
-  return half(::log10f(float(a)));
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log1p(const half& a) { return half(numext::log1p(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log10(const half& a) { return half(::log10f(float(a))); }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log2(const half& a) {
   return half(static_cast<float>(EIGEN_LOG2E) * ::logf(float(a)));
 }
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half sqrt(const half& a) {
 #if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 530) || \
-  defined(EIGEN_HIP_DEVICE_COMPILE)
+    defined(EIGEN_HIP_DEVICE_COMPILE)
   return half(hsqrt(a));
 #else
-    return half(::sqrtf(float(a)));
+  return half(::sqrtf(float(a)));
 #endif
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half pow(const half& a, const half& b) {
   return half(::powf(float(a), float(b)));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half sin(const half& a) {
-  return half(::sinf(float(a)));
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half atan2(const half& a, const half& b) {
+  return half(::atan2f(float(a), float(b)));
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half cos(const half& a) {
-  return half(::cosf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half tan(const half& a) {
-  return half(::tanf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half tanh(const half& a) {
-  return half(::tanhf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half asin(const half& a) {
-  return half(::asinf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half acos(const half& a) {
-  return half(::acosf(float(a)));
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half sin(const half& a) { return half(::sinf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half cos(const half& a) { return half(::cosf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half tan(const half& a) { return half(::tanf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half tanh(const half& a) { return half(::tanhf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half asin(const half& a) { return half(::asinf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half acos(const half& a) { return half(::acosf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half atan(const half& a) { return half(::atanf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half atanh(const half& a) { return half(::atanhf(float(a))); }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half floor(const half& a) {
 #if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 300) || \
-  defined(EIGEN_HIP_DEVICE_COMPILE)
+    defined(EIGEN_HIP_DEVICE_COMPILE)
   return half(hfloor(a));
 #else
   return half(::floorf(float(a)));
@@ -720,25 +714,21 @@
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half ceil(const half& a) {
 #if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 300) || \
-  defined(EIGEN_HIP_DEVICE_COMPILE)
+    defined(EIGEN_HIP_DEVICE_COMPILE)
   return half(hceil(a));
 #else
   return half(::ceilf(float(a)));
 #endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half rint(const half& a) {
-  return half(::rintf(float(a)));
-}
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half round(const half& a) {
-  return half(::roundf(float(a)));
-}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half rint(const half& a) { return half(::rintf(float(a))); }
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half round(const half& a) { return half(::roundf(float(a))); }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half fmod(const half& a, const half& b) {
   return half(::fmodf(float(a), float(b)));
 }
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half (min)(const half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half(min)(const half& a, const half& b) {
 #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
-  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
   return __hlt(b, a) ? b : a;
 #else
   const float f1 = static_cast<float>(a);
@@ -746,9 +736,9 @@
   return f2 < f1 ? b : a;
 #endif
 }
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half (max)(const half& a, const half& b) {
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half(max)(const half& a, const half& b) {
 #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
-  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
   return __hlt(a, b) ? b : a;
 #else
   const float f1 = static_cast<float>(a);
@@ -758,51 +748,43 @@
 }
 
 #ifndef EIGEN_NO_IO
-EIGEN_ALWAYS_INLINE std::ostream& operator << (std::ostream& os, const half& v) {
+EIGEN_ALWAYS_INLINE std::ostream& operator<<(std::ostream& os, const half& v) {
   os << static_cast<float>(v);
   return os;
 }
 #endif
 
-} // end namespace half_impl
+}  // end namespace half_impl
 
 // import Eigen::half_impl::half into Eigen namespace
 // using half_impl::half;
 
 namespace internal {
 
-template<>
-struct random_default_impl<half, false, false>
-{
-  static inline half run(const half& x, const half& y)
-  {
-    return x + (y-x) * half(float(std::rand()) / float(RAND_MAX));
+template <>
+struct random_default_impl<half, false, false> {
+  static inline half run(const half& x, const half& y) {
+    return x + (y - x) * half(float(std::rand()) / float(RAND_MAX));
   }
-  static inline half run()
-  {
-    return run(half(-1.f), half(1.f));
-  }
+  static inline half run() { return run(half(-1.f), half(1.f)); }
 };
 
-template<> struct is_arithmetic<half> { enum { value = true }; };
+template <>
+struct is_arithmetic<half> {
+  enum { value = true };
+};
 
-} // end namespace internal
+}  // end namespace internal
 
-template<> struct NumTraits<Eigen::half>
-    : GenericNumTraits<Eigen::half>
-{
-  enum {
-    IsSigned = true,
-    IsInteger = false,
-    IsComplex = false,
-    RequireInitialization = false
-  };
+template <>
+struct NumTraits<Eigen::half> : GenericNumTraits<Eigen::half> {
+  enum { IsSigned = true, IsInteger = false, IsComplex = false, RequireInitialization = false };
 
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half epsilon() {
     return half_impl::raw_uint16_to_half(0x0800);
   }
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half dummy_precision() {
-    return half_impl::raw_uint16_to_half(0x211f); //  Eigen::half(1e-2f);
+    return half_impl::raw_uint16_to_half(0x211f);  //  Eigen::half(1e-2f);
   }
   EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half highest() {
     return half_impl::raw_uint16_to_half(0x7bff);
@@ -818,10 +800,10 @@
   }
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #if defined(EIGEN_HAS_GPU_FP16) || defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
-  #pragma pop_macro("EIGEN_CONSTEXPR")
+#pragma pop_macro("EIGEN_CONSTEXPR")
 #endif
 
 namespace Eigen {
@@ -870,63 +852,65 @@
 //    with native support for __half and __nv_bfloat16
 //
 // Note that the following are __device__ - only functions.
-#if (defined(EIGEN_CUDACC) && (!defined(EIGEN_CUDA_ARCH) || EIGEN_CUDA_ARCH >= 300)) \
-    || defined(EIGEN_HIPCC)
+#if (defined(EIGEN_CUDACC) && (!defined(EIGEN_CUDA_ARCH) || EIGEN_CUDA_ARCH >= 300)) || defined(EIGEN_HIPCC)
 
 #if defined(EIGEN_HAS_CUDA_FP16) && EIGEN_CUDA_SDK_VER >= 90000
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_sync(unsigned mask, Eigen::half var, int srcLane, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_sync(unsigned mask, Eigen::half var, int srcLane,
+                                                       int width = warpSize) {
   const __half h = var;
   return static_cast<Eigen::half>(__shfl_sync(mask, h, srcLane, width));
 }
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_up_sync(unsigned mask, Eigen::half var, unsigned int delta, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_up_sync(unsigned mask, Eigen::half var, unsigned int delta,
+                                                          int width = warpSize) {
   const __half h = var;
   return static_cast<Eigen::half>(__shfl_up_sync(mask, h, delta, width));
 }
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_down_sync(unsigned mask, Eigen::half var, unsigned int delta, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_down_sync(unsigned mask, Eigen::half var, unsigned int delta,
+                                                            int width = warpSize) {
   const __half h = var;
   return static_cast<Eigen::half>(__shfl_down_sync(mask, h, delta, width));
 }
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_xor_sync(unsigned mask, Eigen::half var, int laneMask, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_xor_sync(unsigned mask, Eigen::half var, int laneMask,
+                                                           int width = warpSize) {
   const __half h = var;
   return static_cast<Eigen::half>(__shfl_xor_sync(mask, h, laneMask, width));
 }
 
-#else // HIP or CUDA SDK < 9.0
+#else  // HIP or CUDA SDK < 9.0
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl(Eigen::half var, int srcLane, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl(Eigen::half var, int srcLane, int width = warpSize) {
   const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
   return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl(ivar, srcLane, width)));
 }
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_up(Eigen::half var, unsigned int delta, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_up(Eigen::half var, unsigned int delta, int width = warpSize) {
   const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
   return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl_up(ivar, delta, width)));
 }
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_down(Eigen::half var, unsigned int delta, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_down(Eigen::half var, unsigned int delta, int width = warpSize) {
   const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
   return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl_down(ivar, delta, width)));
 }
 
-__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_xor(Eigen::half var, int laneMask, int width=warpSize) {
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_xor(Eigen::half var, int laneMask, int width = warpSize) {
   const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
   return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl_xor(ivar, laneMask, width)));
 }
 
-#endif // HIP vs CUDA
-#endif // __shfl*
+#endif  // HIP vs CUDA
+#endif  // __shfl*
 
 // ldg() has an overload for __half_raw, but we also need one for Eigen::half.
-#if (defined(EIGEN_CUDACC) && (!defined(EIGEN_CUDA_ARCH) || EIGEN_CUDA_ARCH >= 350)) \
-    || defined(EIGEN_HIPCC)
+#if (defined(EIGEN_CUDACC) && (!defined(EIGEN_CUDA_ARCH) || EIGEN_CUDA_ARCH >= 350)) || defined(EIGEN_HIPCC)
 EIGEN_STRONG_INLINE __device__ Eigen::half __ldg(const Eigen::half* ptr) {
   return Eigen::half_impl::raw_uint16_to_half(__ldg(reinterpret_cast<const Eigen::numext::uint16_t*>(ptr)));
 }
-#endif // __ldg
+#endif  // __ldg
 
 #if EIGEN_HAS_STD_HASH
 namespace std {
@@ -936,7 +920,49 @@
     return static_cast<std::size_t>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(a));
   }
 };
-} // end namespace std
+}  // end namespace std
 #endif
 
-#endif // EIGEN_HALF_H
+namespace Eigen {
+namespace internal {
+
+template <>
+struct cast_impl<float, half> {
+  EIGEN_DEVICE_FUNC static inline half run(const float& a) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    return __float2half(a);
+#else
+    return half(a);
+#endif
+  }
+};
+
+template <>
+struct cast_impl<int, half> {
+  EIGEN_DEVICE_FUNC static inline half run(const int& a) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    return __float2half(static_cast<float>(a));
+#else
+    return half(static_cast<float>(a));
+#endif
+  }
+};
+
+template <>
+struct cast_impl<half, float> {
+  EIGEN_DEVICE_FUNC static inline float run(const half& a) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+    (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+    return __half2float(a);
+#else
+    return static_cast<float>(a);
+#endif
+  }
+};
+
+}  // namespace internal
+}  // namespace Eigen
+
+#endif  // EIGEN_HALF_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h
index a5c3ada..7e3a970 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h
@@ -8,7 +8,6 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 /* All the parameters defined in this file can be specialized in the
  * architecture specific files, and/or by the user.
  * More to come... */
@@ -17,33 +16,32 @@
 #define EIGEN_DEFAULT_SETTINGS_H
 
 /** Defines the maximal loop size to enable meta unrolling of loops.
-  * Note that the value here is expressed in Eigen's own notion of "number of FLOPS",
-  * it does not correspond to the number of iterations or the number of instructions
-  */
+ * Note that the value here is expressed in Eigen's own notion of "number of FLOPS",
+ * it does not correspond to the number of iterations or the number of instructions
+ */
 #ifndef EIGEN_UNROLLING_LIMIT
 #define EIGEN_UNROLLING_LIMIT 110
 #endif
 
 /** Defines the threshold between a "small" and a "large" matrix.
-  * This threshold is mainly used to select the proper product implementation.
-  */
+ * This threshold is mainly used to select the proper product implementation.
+ */
 #ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
 #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8
 #endif
 
 /** Defines the maximal width of the blocks used in the triangular product and solver
-  * for vectors (level 2 blas xTRMV and xTRSV). The default is 8.
-  */
+ * for vectors (level 2 blas xTRMV and xTRSV). The default is 8.
+ */
 #ifndef EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH
 #define EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 8
 #endif
 
-
 /** Defines the default number of registers available for that architecture.
-  * Currently it must be 8 or 16. Other values will fail.
-  */
+ * Currently it must be 8 or 16. Other values will fail.
+ */
 #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS
 #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 8
 #endif
 
-#endif // EIGEN_DEFAULT_SETTINGS_H
+#endif  // EIGEN_DEFAULT_SETTINGS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/TypeCasting.h
deleted file mode 100644
index fb8183b..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/TypeCasting.h
+++ /dev/null
@@ -1,120 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2016 Benoit Steiner <benoit.steiner.goog@gmail.com>
-// Copyright (C) 2019 Rasmus Munk Larsen <rmlarsen@google.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_GENERIC_TYPE_CASTING_H
-#define EIGEN_GENERIC_TYPE_CASTING_H
-
-namespace Eigen {
-
-namespace internal {
-
-template<>
-struct scalar_cast_op<float, Eigen::half> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
-  typedef Eigen::half result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half operator() (const float& a) const {
-    #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
-      (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
-      return __float2half(a);
-    #else
-      return Eigen::half(a);
-    #endif
-  }
-};
-
-template<>
-struct functor_traits<scalar_cast_op<float, Eigen::half> >
-{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
-
-
-template<>
-struct scalar_cast_op<int, Eigen::half> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
-  typedef Eigen::half result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half operator() (const int& a) const {
-    #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
-      (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
-      return __float2half(static_cast<float>(a));
-    #else
-      return Eigen::half(static_cast<float>(a));
-    #endif
-  }
-};
-
-template<>
-struct functor_traits<scalar_cast_op<int, Eigen::half> >
-{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
-
-
-template<>
-struct scalar_cast_op<Eigen::half, float> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
-  typedef float result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator() (const Eigen::half& a) const {
-    #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
-      (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
-      return __half2float(a);
-    #else
-      return static_cast<float>(a);
-    #endif
-  }
-};
-
-template<>
-struct functor_traits<scalar_cast_op<Eigen::half, float> >
-{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
-
-
-template<>
-struct scalar_cast_op<float, Eigen::bfloat16> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
-  typedef Eigen::bfloat16 result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::bfloat16 operator() (const float& a) const {
-    return Eigen::bfloat16(a);
-  }
-};
-
-template<>
-struct functor_traits<scalar_cast_op<float, Eigen::bfloat16> >
-{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
-
-
-template<>
-struct scalar_cast_op<int, Eigen::bfloat16> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
-  typedef Eigen::bfloat16 result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::bfloat16 operator() (const int& a) const {
-    return Eigen::bfloat16(static_cast<float>(a));
-  }
-};
-
-template<>
-struct functor_traits<scalar_cast_op<int, Eigen::bfloat16> >
-{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
-
-
-template<>
-struct scalar_cast_op<Eigen::bfloat16, float> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
-  typedef float result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator() (const Eigen::bfloat16& a) const {
-    return static_cast<float>(a);
-  }
-};
-
-template<>
-struct functor_traits<scalar_cast_op<Eigen::bfloat16, float> >
-{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
-
-
-}
-}
-
-#endif  // EIGEN_GENERIC_TYPE_CASTING_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h
index f40af7f..8240847 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h
@@ -11,74 +11,71 @@
 #ifndef EIGEN_COMPLEX_NEON_H
 #define EIGEN_COMPLEX_NEON_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-inline uint32x4_t p4ui_CONJ_XOR()
-{
+inline uint32x4_t p4ui_CONJ_XOR() {
 // See bug 1325, clang fails to call vld1q_u64.
 #if EIGEN_COMP_CLANG || EIGEN_COMP_CASTXML
-  uint32x4_t ret = { 0x00000000, 0x80000000, 0x00000000, 0x80000000 };
+  uint32x4_t ret = {0x00000000, 0x80000000, 0x00000000, 0x80000000};
   return ret;
 #else
-  static const uint32_t conj_XOR_DATA[] = { 0x00000000, 0x80000000, 0x00000000, 0x80000000 };
-  return vld1q_u32( conj_XOR_DATA );
+  static const uint32_t conj_XOR_DATA[] = {0x00000000, 0x80000000, 0x00000000, 0x80000000};
+  return vld1q_u32(conj_XOR_DATA);
 #endif
 }
 
-inline uint32x2_t p2ui_CONJ_XOR()
-{
-  static const uint32_t conj_XOR_DATA[] = { 0x00000000, 0x80000000 };
-  return vld1_u32( conj_XOR_DATA );
+inline uint32x2_t p2ui_CONJ_XOR() {
+  static const uint32_t conj_XOR_DATA[] = {0x00000000, 0x80000000};
+  return vld1_u32(conj_XOR_DATA);
 }
 
 //---------- float ----------
 
-struct Packet1cf
-{
+struct Packet1cf {
   EIGEN_STRONG_INLINE Packet1cf() {}
   EIGEN_STRONG_INLINE explicit Packet1cf(const Packet2f& a) : v(a) {}
   Packet2f v;
 };
-struct Packet2cf
-{
+struct Packet2cf {
   EIGEN_STRONG_INLINE Packet2cf() {}
   EIGEN_STRONG_INLINE explicit Packet2cf(const Packet4f& a) : v(a) {}
   Packet4f v;
 };
 
-template<> struct packet_traits<std::complex<float> > : default_packet_traits
-{
+template <>
+struct packet_traits<std::complex<float> > : default_packet_traits {
   typedef Packet2cf type;
   typedef Packet1cf half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 2,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasMul       = 1,
-    HasDiv       = 1,
-    HasNegate    = 1,
-    HasAbs       = 0,
-    HasAbs2      = 0,
-    HasMin       = 0,
-    HasMax       = 0,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
+    HasNegate = 1,
+    HasSqrt = 1,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
     HasSetLinear = 0
   };
 };
 
-template<> struct unpacket_traits<Packet1cf>
-{
+template <>
+struct unpacket_traits<Packet1cf> {
   typedef std::complex<float> type;
   typedef Packet1cf half;
   typedef Packet2f as_real;
-  enum
-  {
+  enum {
     size = 1,
     alignment = Aligned16,
     vectorizable = true,
@@ -86,13 +83,12 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet2cf>
-{
+template <>
+struct unpacket_traits<Packet2cf> {
   typedef std::complex<float> type;
   typedef Packet1cf half;
   typedef Packet4f as_real;
-  enum
-  {
+  enum {
     size = 2,
     alignment = Aligned16,
     vectorizable = true,
@@ -101,45 +97,65 @@
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet1cf pcast<float,Packet1cf>(const float& a)
-{ return Packet1cf(vset_lane_f32(a, vdup_n_f32(0.f), 0)); }
-template<> EIGEN_STRONG_INLINE Packet2cf pcast<Packet2f,Packet2cf>(const Packet2f& a)
-{ return Packet2cf(vreinterpretq_f32_u64(vmovl_u32(vreinterpret_u32_f32(a)))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cf pcast<float, Packet1cf>(const float& a) {
+  return Packet1cf(vset_lane_f32(a, vdup_n_f32(0.f), 0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pcast<Packet2f, Packet2cf>(const Packet2f& a) {
+  return Packet2cf(vreinterpretq_f32_u64(vmovl_u32(vreinterpret_u32_f32(a))));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cf pset1<Packet1cf>(const std::complex<float>& from)
-{ return Packet1cf(vld1_f32(reinterpret_cast<const float*>(&from))); }
-template<> EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>& from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet1cf pset1<Packet1cf>(const std::complex<float>& from) {
+  return Packet1cf(vld1_f32(reinterpret_cast<const float*>(&from)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>& from) {
   const float32x2_t r64 = vld1_f32(reinterpret_cast<const float*>(&from));
   return Packet2cf(vcombine_f32(r64, r64));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cf padd<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{ return Packet1cf(padd<Packet2f>(a.v, b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf padd<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{ return Packet2cf(padd<Packet4f>(a.v, b.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cf padd<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
+  return Packet1cf(padd<Packet2f>(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf padd<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(padd<Packet4f>(a.v, b.v));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cf psub<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{ return Packet1cf(psub<Packet2f>(a.v, b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf psub<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{ return Packet2cf(psub<Packet4f>(a.v, b.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cf psub<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
+  return Packet1cf(psub<Packet2f>(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf psub<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(psub<Packet4f>(a.v, b.v));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cf pnegate(const Packet1cf& a) { return Packet1cf(pnegate<Packet2f>(a.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf pnegate(const Packet2cf& a) { return Packet2cf(pnegate<Packet4f>(a.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cf pnegate(const Packet1cf& a) {
+  return Packet1cf(pnegate<Packet2f>(a.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pnegate(const Packet2cf& a) {
+  return Packet2cf(pnegate<Packet4f>(a.v));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cf pconj(const Packet1cf& a)
-{
-  const Packet2ui b = vreinterpret_u32_f32(a.v);
+template <>
+EIGEN_STRONG_INLINE Packet1cf pconj(const Packet1cf& a) {
+  const Packet2ui b = Packet2ui(vreinterpret_u32_f32(a.v));
   return Packet1cf(vreinterpret_f32_u32(veor_u32(b, p2ui_CONJ_XOR())));
 }
-template<> EIGEN_STRONG_INLINE Packet2cf pconj(const Packet2cf& a)
-{
-  const Packet4ui b = vreinterpretq_u32_f32(a.v);
+template <>
+EIGEN_STRONG_INLINE Packet2cf pconj(const Packet2cf& a) {
+  const Packet4ui b = Packet4ui(vreinterpretq_u32_f32(a.v));
   return Packet2cf(vreinterpretq_f32_u32(veorq_u32(b, p4ui_CONJ_XOR())));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cf pmul<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet1cf pmul<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
   Packet2f v1, v2;
 
   // Get the real values of a | a1_re | a1_re |
@@ -157,8 +173,8 @@
   // Add and return the result
   return Packet1cf(vadd_f32(v1, v2));
 }
-template<> EIGEN_STRONG_INLINE Packet2cf pmul<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2cf pmul<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
   Packet4f v1, v2;
 
   // Get the real values of a | a1_re | a1_re | a2_re | a2_re |
@@ -177,8 +193,8 @@
   return Packet2cf(vaddq_f32(v1, v2));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cf pcmp_eq(const Packet1cf& a, const Packet1cf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet1cf pcmp_eq(const Packet1cf& a, const Packet1cf& b) {
   // Compare real and imaginary parts of a and b to get the mask vector:
   // [re(a[0])==re(b[0]), im(a[0])==im(b[0])]
   Packet2f eq = pcmp_eq<Packet2f>(a.v, b.v);
@@ -188,8 +204,8 @@
   // Return re(a)==re(b) && im(a)==im(b) by computing bitwise AND of eq and eq_swapped
   return Packet1cf(pand<Packet2f>(eq, eq_swapped));
 }
-template<> EIGEN_STRONG_INLINE Packet2cf pcmp_eq(const Packet2cf& a, const Packet2cf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2cf pcmp_eq(const Packet2cf& a, const Packet2cf& b) {
   // Compare real and imaginary parts of a and b to get the mask vector:
   // [re(a[0])==re(b[0]), im(a[0])==im(b[0]), re(a[1])==re(b[1]), im(a[1])==im(b[1])]
   Packet4f eq = pcmp_eq<Packet4f>(a.v, b.v);
@@ -200,129 +216,178 @@
   return Packet2cf(pand<Packet4f>(eq, eq_swapped));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cf pand<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{ return Packet1cf(vreinterpret_f32_u32(vand_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
-template<> EIGEN_STRONG_INLINE Packet2cf pand<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{ return Packet2cf(vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
-
-template<> EIGEN_STRONG_INLINE Packet1cf por<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{ return Packet1cf(vreinterpret_f32_u32(vorr_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
-template<> EIGEN_STRONG_INLINE Packet2cf por<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{ return Packet2cf(vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
-
-template<> EIGEN_STRONG_INLINE Packet1cf pxor<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{ return Packet1cf(vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
-template<> EIGEN_STRONG_INLINE Packet2cf pxor<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{ return Packet2cf(vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
-
-template<> EIGEN_STRONG_INLINE Packet1cf pandnot<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{ return Packet1cf(vreinterpret_f32_u32(vbic_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
-template<> EIGEN_STRONG_INLINE Packet2cf pandnot<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{ return Packet2cf(vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
-
-template<> EIGEN_STRONG_INLINE Packet1cf pload<Packet1cf>(const std::complex<float>* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return Packet1cf(pload<Packet2f>((const float*)from)); }
-template<> EIGEN_STRONG_INLINE Packet2cf pload<Packet2cf>(const std::complex<float>* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return Packet2cf(pload<Packet4f>(reinterpret_cast<const float*>(from))); }
-
-template<> EIGEN_STRONG_INLINE Packet1cf ploadu<Packet1cf>(const std::complex<float>* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cf(ploadu<Packet2f>((const float*)from)); }
-template<> EIGEN_STRONG_INLINE Packet2cf ploadu<Packet2cf>(const std::complex<float>* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cf(ploadu<Packet4f>(reinterpret_cast<const float*>(from))); }
-
-template<> EIGEN_STRONG_INLINE Packet1cf ploaddup<Packet1cf>(const std::complex<float>* from)
-{ return pset1<Packet1cf>(*from); }
-template<> EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<float>* from)
-{ return pset1<Packet2cf>(*from); }
-
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> *to, const Packet1cf& from)
-{ EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); }
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> *to, const Packet2cf& from)
-{ EIGEN_DEBUG_ALIGNED_STORE pstore(reinterpret_cast<float*>(to), from.v); }
-
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> *to, const Packet1cf& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); }
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> *to, const Packet2cf& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<float*>(to), from.v); }
-
-template<> EIGEN_DEVICE_FUNC inline Packet1cf pgather<std::complex<float>, Packet1cf>(
-    const std::complex<float>* from, Index stride)
-{
-  const Packet2f tmp = vdup_n_f32(std::real(from[0*stride]));
-  return Packet1cf(vset_lane_f32(std::imag(from[0*stride]), tmp, 1));
+template <>
+EIGEN_STRONG_INLINE Packet1cf pand<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
+  return Packet1cf(vreinterpret_f32_u32(vand_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v))));
 }
-template<> EIGEN_DEVICE_FUNC inline Packet2cf pgather<std::complex<float>, Packet2cf>(
-    const std::complex<float>* from, Index stride)
-{
-  Packet4f res = vdupq_n_f32(std::real(from[0*stride]));
-  res = vsetq_lane_f32(std::imag(from[0*stride]), res, 1);
-  res = vsetq_lane_f32(std::real(from[1*stride]), res, 2);
-  res = vsetq_lane_f32(std::imag(from[1*stride]), res, 3);
+template <>
+EIGEN_STRONG_INLINE Packet2cf pand<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v))));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet1cf por<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
+  return Packet1cf(vreinterpret_f32_u32(vorr_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf por<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v))));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet1cf pxor<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
+  return Packet1cf(vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pxor<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v))));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet1cf pandnot<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
+  return Packet1cf(vreinterpret_f32_u32(vbic_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pandnot<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v))));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet1cf pload<Packet1cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return Packet1cf(pload<Packet2f>((const float*)from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pload<Packet2cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return Packet2cf(pload<Packet4f>(reinterpret_cast<const float*>(from)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet1cf ploadu<Packet1cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cf(ploadu<Packet2f>((const float*)from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf ploadu<Packet2cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cf(ploadu<Packet4f>(reinterpret_cast<const float*>(from)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet1cf ploaddup<Packet1cf>(const std::complex<float>* from) {
+  return pset1<Packet1cf>(*from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<float>* from) {
+  return pset1<Packet2cf>(*from);
+}
+
+template <>
+EIGEN_STRONG_INLINE void pstore<std::complex<float> >(std::complex<float>* to, const Packet1cf& from) {
+  EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<std::complex<float> >(std::complex<float>* to, const Packet2cf& from) {
+  EIGEN_DEBUG_ALIGNED_STORE pstore(reinterpret_cast<float*>(to), from.v);
+}
+
+template <>
+EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float>* to, const Packet1cf& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float>* to, const Packet2cf& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<float*>(to), from.v);
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline Packet1cf pgather<std::complex<float>, Packet1cf>(const std::complex<float>* from,
+                                                                           Index stride) {
+  const Packet2f tmp = vdup_n_f32(std::real(from[0 * stride]));
+  return Packet1cf(vset_lane_f32(std::imag(from[0 * stride]), tmp, 1));
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet2cf pgather<std::complex<float>, Packet2cf>(const std::complex<float>* from,
+                                                                           Index stride) {
+  Packet4f res = vdupq_n_f32(std::real(from[0 * stride]));
+  res = vsetq_lane_f32(std::imag(from[0 * stride]), res, 1);
+  res = vsetq_lane_f32(std::real(from[1 * stride]), res, 2);
+  res = vsetq_lane_f32(std::imag(from[1 * stride]), res, 3);
   return Packet2cf(res);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet1cf>(
-    std::complex<float>* to, const Packet1cf& from, Index stride)
-{ to[stride*0] = std::complex<float>(vget_lane_f32(from.v, 0), vget_lane_f32(from.v, 1)); }
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf>(
-    std::complex<float>* to, const Packet2cf& from, Index stride)
-{
-  to[stride*0] = std::complex<float>(vgetq_lane_f32(from.v, 0), vgetq_lane_f32(from.v, 1));
-  to[stride*1] = std::complex<float>(vgetq_lane_f32(from.v, 2), vgetq_lane_f32(from.v, 3));
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet1cf>(std::complex<float>* to, const Packet1cf& from,
+                                                                       Index stride) {
+  to[stride * 0] = std::complex<float>(vget_lane_f32(from.v, 0), vget_lane_f32(from.v, 1));
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf>(std::complex<float>* to, const Packet2cf& from,
+                                                                       Index stride) {
+  to[stride * 0] = std::complex<float>(vgetq_lane_f32(from.v, 0), vgetq_lane_f32(from.v, 1));
+  to[stride * 1] = std::complex<float>(vgetq_lane_f32(from.v, 2), vgetq_lane_f32(from.v, 3));
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *addr)
-{ EIGEN_ARM_PREFETCH(reinterpret_cast<const float*>(addr)); }
+template <>
+EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float>* addr) {
+  EIGEN_ARM_PREFETCH(reinterpret_cast<const float*>(addr));
+}
 
-template<> EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet1cf>(const Packet1cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet1cf>(const Packet1cf& a) {
   EIGEN_ALIGN16 std::complex<float> x;
   vst1_f32(reinterpret_cast<float*>(&x), a.v);
   return x;
 }
-template<> EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet2cf>(const Packet2cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet2cf>(const Packet2cf& a) {
   EIGEN_ALIGN16 std::complex<float> x[2];
   vst1q_f32(reinterpret_cast<float*>(x), a.v);
   return x[0];
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cf preverse(const Packet1cf& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2cf preverse(const Packet2cf& a)
-{ return Packet2cf(vcombine_f32(vget_high_f32(a.v), vget_low_f32(a.v))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cf preverse(const Packet1cf& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf preverse(const Packet2cf& a) {
+  return Packet2cf(vcombine_f32(vget_high_f32(a.v), vget_low_f32(a.v)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cf pcplxflip<Packet1cf>(const Packet1cf& a)
-{ return Packet1cf(vrev64_f32(a.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf pcplxflip<Packet2cf>(const Packet2cf& a)
-{ return Packet2cf(vrev64q_f32(a.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cf pcplxflip<Packet1cf>(const Packet1cf& a) {
+  return Packet1cf(vrev64_f32(a.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pcplxflip<Packet2cf>(const Packet2cf& a) {
+  return Packet2cf(vrev64q_f32(a.v));
+}
 
-template<> EIGEN_STRONG_INLINE std::complex<float> predux<Packet1cf>(const Packet1cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux<Packet1cf>(const Packet1cf& a) {
   std::complex<float> s;
-  vst1_f32((float *)&s, a.v);
+  vst1_f32((float*)&s, a.v);
   return s;
 }
-template<> EIGEN_STRONG_INLINE std::complex<float> predux<Packet2cf>(const Packet2cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux<Packet2cf>(const Packet2cf& a) {
   std::complex<float> s;
   vst1_f32(reinterpret_cast<float*>(&s), vadd_f32(vget_low_f32(a.v), vget_high_f32(a.v)));
   return s;
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet1cf>(const Packet1cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet1cf>(const Packet1cf& a) {
   std::complex<float> s;
-  vst1_f32((float *)&s, a.v);
+  vst1_f32((float*)&s, a.v);
   return s;
 }
-template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet2cf>(const Packet2cf& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet2cf>(const Packet2cf& a) {
   float32x2_t a1, a2, v1, v2, prod;
   std::complex<float> s;
 
   a1 = vget_low_f32(a.v);
   a2 = vget_high_f32(a.v);
-   // Get the real values of a | a1_re | a1_re | a2_re | a2_re |
+  // Get the real values of a | a1_re | a1_re | a2_re | a2_re |
   v1 = vdup_lane_f32(a1, 0);
   // Get the real values of a | a1_im | a1_im | a2_im | a2_im |
   v2 = vdup_lane_f32(a1, 1);
@@ -342,47 +407,32 @@
   return s;
 }
 
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cf,Packet2f)
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cf, Packet2f)
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf, Packet4f)
 
-template<> EIGEN_STRONG_INLINE Packet1cf pdiv<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
-{
-  // TODO optimize it for NEON
-  Packet1cf res = pmul(a, pconj(b));
-  Packet2f s, rev_s;
-
-  // this computes the norm
-  s = vmul_f32(b.v, b.v);
-  rev_s = vrev64_f32(s);
-
-  return Packet1cf(pdiv<Packet2f>(res.v, vadd_f32(s, rev_s)));
+template <>
+EIGEN_STRONG_INLINE Packet1cf pdiv<Packet1cf>(const Packet1cf& a, const Packet1cf& b) {
+  return pdiv_complex(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{
-  // TODO optimize it for NEON
-  Packet2cf res = pmul(a,pconj(b));
-  Packet4f s, rev_s;
-
-  // this computes the norm
-  s = vmulq_f32(b.v, b.v);
-  rev_s = vrev64q_f32(s);
-
-  return Packet2cf(pdiv<Packet4f>(res.v, vaddq_f32(s, rev_s)));
+template <>
+EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return pdiv_complex(a, b);
 }
 
 EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet1cf, 1>& /*kernel*/) {}
-EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet2cf, 2>& kernel)
-{
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet2cf, 2>& kernel) {
   Packet4f tmp = vcombine_f32(vget_high_f32(kernel.packet[0].v), vget_high_f32(kernel.packet[1].v));
   kernel.packet[0].v = vcombine_f32(vget_low_f32(kernel.packet[0].v), vget_low_f32(kernel.packet[1].v));
   kernel.packet[1].v = tmp;
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cf psqrt<Packet1cf>(const Packet1cf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet1cf psqrt<Packet1cf>(const Packet1cf& a) {
   return psqrt_complex<Packet1cf>(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf psqrt<Packet2cf>(const Packet2cf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2cf psqrt<Packet2cf>(const Packet2cf& a) {
   return psqrt_complex<Packet2cf>(a);
 }
 
@@ -390,85 +440,94 @@
 #if EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG
 
 // See bug 1325, clang fails to call vld1q_u64.
-#if EIGEN_COMP_CLANG || EIGEN_COMP_CASTXML
-  static uint64x2_t p2ul_CONJ_XOR = {0x0, 0x8000000000000000};
+#if EIGEN_COMP_CLANG || EIGEN_COMP_CASTXML || EIGEN_COMP_CPE
+static uint64x2_t p2ul_CONJ_XOR = {0x0, 0x8000000000000000};
 #else
-  const uint64_t  p2ul_conj_XOR_DATA[] = { 0x0, 0x8000000000000000 };
-  static uint64x2_t p2ul_CONJ_XOR = vld1q_u64( p2ul_conj_XOR_DATA );
+const uint64_t p2ul_conj_XOR_DATA[] = {0x0, 0x8000000000000000};
+static uint64x2_t p2ul_CONJ_XOR = vld1q_u64(p2ul_conj_XOR_DATA);
 #endif
 
-struct Packet1cd
-{
+struct Packet1cd {
   EIGEN_STRONG_INLINE Packet1cd() {}
   EIGEN_STRONG_INLINE explicit Packet1cd(const Packet2d& a) : v(a) {}
   Packet2d v;
 };
 
-template<> struct packet_traits<std::complex<double> >  : default_packet_traits
-{
+template <>
+struct packet_traits<std::complex<double> > : default_packet_traits {
   typedef Packet1cd type;
   typedef Packet1cd half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 0,
     size = 1,
-    HasHalfPacket = 0,
 
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasDiv    = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
     HasNegate = 1,
-    HasAbs    = 0,
-    HasAbs2   = 0,
-    HasMin    = 0,
-    HasMax    = 0,
+    HasSqrt = 1,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
     HasSetLinear = 0
   };
 };
 
-template<> struct unpacket_traits<Packet1cd>
-{
+template <>
+struct unpacket_traits<Packet1cd> {
   typedef std::complex<double> type;
   typedef Packet1cd half;
   typedef Packet2d as_real;
-  enum
-  {
-    size=1,
-    alignment=Aligned16,
-    vectorizable=true,
-    masked_load_available=false,
-    masked_store_available=false
+  enum {
+    size = 1,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet1cd pload<Packet1cd>(const std::complex<double>* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return Packet1cd(pload<Packet2d>(reinterpret_cast<const double*>(from))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd pload<Packet1cd>(const std::complex<double>* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return Packet1cd(pload<Packet2d>(reinterpret_cast<const double*>(from)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd ploadu<Packet1cd>(const std::complex<double>* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cd(ploadu<Packet2d>(reinterpret_cast<const double*>(from))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd ploadu<Packet1cd>(const std::complex<double>* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cd(ploadu<Packet2d>(reinterpret_cast<const double*>(from)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd pset1<Packet1cd>(const std::complex<double>& from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet1cd pset1<Packet1cd>(const std::complex<double>& from) {
   /* here we really have to use unaligned loads :( */
   return ploadu<Packet1cd>(&from);
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd padd<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{ return Packet1cd(padd<Packet2d>(a.v, b.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd padd<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(padd<Packet2d>(a.v, b.v));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd psub<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{ return Packet1cd(psub<Packet2d>(a.v, b.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd psub<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(psub<Packet2d>(a.v, b.v));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd pnegate(const Packet1cd& a)
-{ return Packet1cd(pnegate<Packet2d>(a.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd pnegate(const Packet1cd& a) {
+  return Packet1cd(pnegate<Packet2d>(a.v));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd pconj(const Packet1cd& a)
-{ return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v), p2ul_CONJ_XOR))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd pconj(const Packet1cd& a) {
+  return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v), p2ul_CONJ_XOR)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd pmul<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet1cd pmul<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
   Packet2d v1, v2;
 
   // Get the real values of a
@@ -487,8 +546,8 @@
   return Packet1cd(vaddq_f64(v1, v2));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd pcmp_eq(const Packet1cd& a, const Packet1cd& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet1cd pcmp_eq(const Packet1cd& a, const Packet1cd& b) {
   // Compare real and imaginary parts of a and b to get the mask vector:
   // [re(a)==re(b), im(a)==im(b)]
   Packet2d eq = pcmp_eq<Packet2d>(a.v, b.v);
@@ -499,86 +558,109 @@
   return Packet1cd(pand<Packet2d>(eq, eq_swapped));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd pand<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{ return Packet1cd(vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd pand<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a.v), vreinterpretq_u64_f64(b.v))));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd por<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{ return Packet1cd(vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd por<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a.v), vreinterpretq_u64_f64(b.v))));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd pxor<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{ return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd pxor<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v), vreinterpretq_u64_f64(b.v))));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd pandnot<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{ return Packet1cd(vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd pandnot<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a.v), vreinterpretq_u64_f64(b.v))));
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<double>* from)
-{ return pset1<Packet1cd>(*from); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<double>* from) {
+  return pset1<Packet1cd>(*from);
+}
 
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<double> >(std::complex<double> *to, const Packet1cd& from)
-{ EIGEN_DEBUG_ALIGNED_STORE pstore(reinterpret_cast<double*>(to), from.v); }
+template <>
+EIGEN_STRONG_INLINE void pstore<std::complex<double> >(std::complex<double>* to, const Packet1cd& from) {
+  EIGEN_DEBUG_ALIGNED_STORE pstore(reinterpret_cast<double*>(to), from.v);
+}
 
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double> *to, const Packet1cd& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<double*>(to), from.v); }
+template <>
+EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double>* to, const Packet1cd& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<double*>(to), from.v);
+}
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *addr)
-{ EIGEN_ARM_PREFETCH(reinterpret_cast<const double*>(addr)); }
+template <>
+EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double>* addr) {
+  EIGEN_ARM_PREFETCH(reinterpret_cast<const double*>(addr));
+}
 
-template<> EIGEN_DEVICE_FUNC inline Packet1cd pgather<std::complex<double>, Packet1cd>(
-    const std::complex<double>* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC inline Packet1cd pgather<std::complex<double>, Packet1cd>(const std::complex<double>* from,
+                                                                            Index stride) {
   Packet2d res = pset1<Packet2d>(0.0);
-  res = vsetq_lane_f64(std::real(from[0*stride]), res, 0);
-  res = vsetq_lane_f64(std::imag(from[0*stride]), res, 1);
+  res = vsetq_lane_f64(std::real(from[0 * stride]), res, 0);
+  res = vsetq_lane_f64(std::imag(from[0 * stride]), res, 1);
   return Packet1cd(res);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<double>, Packet1cd>(
-    std::complex<double>* to, const Packet1cd& from, Index stride)
-{ to[stride*0] = std::complex<double>(vgetq_lane_f64(from.v, 0), vgetq_lane_f64(from.v, 1)); }
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<std::complex<double>, Packet1cd>(std::complex<double>* to, const Packet1cd& from,
+                                                                        Index stride) {
+  to[stride * 0] = std::complex<double>(vgetq_lane_f64(from.v, 0), vgetq_lane_f64(from.v, 1));
+}
 
-template<> EIGEN_STRONG_INLINE std::complex<double> pfirst<Packet1cd>(const Packet1cd& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<double> pfirst<Packet1cd>(const Packet1cd& a) {
   EIGEN_ALIGN16 std::complex<double> res;
   pstore<std::complex<double> >(&res, a);
   return res;
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd preverse(const Packet1cd& a) { return a; }
-
-template<> EIGEN_STRONG_INLINE std::complex<double> predux<Packet1cd>(const Packet1cd& a) { return pfirst(a); }
-
-template<> EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet1cd>(const Packet1cd& a) { return pfirst(a); }
-
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
-
-template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{
-  // TODO optimize it for NEON
-  Packet1cd res = pmul(a,pconj(b));
-  Packet2d s = pmul<Packet2d>(b.v, b.v);
-  Packet2d rev_s = preverse<Packet2d>(s);
-
-  return Packet1cd(pdiv(res.v, padd<Packet2d>(s,rev_s)));
+template <>
+EIGEN_STRONG_INLINE Packet1cd preverse(const Packet1cd& a) {
+  return a;
 }
 
-EIGEN_STRONG_INLINE Packet1cd pcplxflip/*<Packet1cd>*/(const Packet1cd& x)
-{ return Packet1cd(preverse(Packet2d(x.v))); }
+template <>
+EIGEN_STRONG_INLINE std::complex<double> predux<Packet1cd>(const Packet1cd& a) {
+  return pfirst(a);
+}
 
-EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet1cd,2>& kernel)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet1cd>(const Packet1cd& a) {
+  return pfirst(a);
+}
+
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd, Packet2d)
+
+template <>
+EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return pdiv_complex(a, b);
+}
+
+EIGEN_STRONG_INLINE Packet1cd pcplxflip /*<Packet1cd>*/ (const Packet1cd& x) {
+  return Packet1cd(preverse(Packet2d(x.v)));
+}
+
+EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet1cd, 2>& kernel) {
   Packet2d tmp = vcombine_f64(vget_high_f64(kernel.packet[0].v), vget_high_f64(kernel.packet[1].v));
   kernel.packet[0].v = vcombine_f64(vget_low_f64(kernel.packet[0].v), vget_low_f64(kernel.packet[1].v));
   kernel.packet[1].v = tmp;
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd psqrt<Packet1cd>(const Packet1cd& a) {
+template <>
+EIGEN_STRONG_INLINE Packet1cd psqrt<Packet1cd>(const Packet1cd& a) {
   return psqrt_complex<Packet1cd>(a);
 }
 
-#endif // EIGEN_ARCH_ARM64
+#endif  // EIGEN_ARCH_ARM64
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COMPLEX_NEON_H
+#endif  // EIGEN_COMPLEX_NEON_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
index 3481f33..4ecf7d1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
@@ -1,183 +1,243 @@
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
-  
+
 #if EIGEN_ARCH_ARM && EIGEN_COMP_CLANG
 
 // Clang seems to excessively spill registers in the GEBP kernel on 32-bit arm.
 // Here we specialize gebp_traits to eliminate these register spills.
 // See #2138.
-template<>
-struct gebp_traits <float,float,false,false,Architecture::NEON,GEBPPacketFull>
- : gebp_traits<float,float,false,false,Architecture::Generic,GEBPPacketFull>
-{
-  EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const
-  { 
+template <>
+struct gebp_traits<float, float, false, false, Architecture::NEON, GEBPPacketFull>
+    : gebp_traits<float, float, false, false, Architecture::Generic, GEBPPacketFull> {
+  EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const {
     // This volatile inline ASM both acts as a barrier to prevent reordering,
     // as well as enforces strict register use.
-    asm volatile(
-      "vmla.f32 %q[r], %q[c], %q[alpha]"
-      : [r] "+w" (r)
-      : [c] "w" (c),
-        [alpha] "w" (alpha)
-      : );
+    asm volatile("vmla.f32 %q[r], %q[c], %q[alpha]" : [r] "+w"(r) : [c] "w"(c), [alpha] "w"(alpha) :);
   }
 
   template <typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const Packet4f& a, const Packet4f& b,
-                                Packet4f& c, Packet4f& tmp,
-                                const LaneIdType&) const {
+  EIGEN_STRONG_INLINE void madd(const Packet4f& a, const Packet4f& b, Packet4f& c, Packet4f&, const LaneIdType&) const {
     acc(a, b, c);
   }
-  
+
   template <typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const Packet4f& a, const QuadPacket<Packet4f>& b,
-                                Packet4f& c, Packet4f& tmp,
+  EIGEN_STRONG_INLINE void madd(const Packet4f& a, const QuadPacket<Packet4f>& b, Packet4f& c, Packet4f& tmp,
                                 const LaneIdType& lane) const {
     madd(a, b.get(lane), c, tmp, lane);
   }
 };
 
-#endif // EIGEN_ARCH_ARM && EIGEN_COMP_CLANG
+#endif  // EIGEN_ARCH_ARM && EIGEN_COMP_CLANG
 
 #if EIGEN_ARCH_ARM64
 
-template<>
-struct gebp_traits <float,float,false,false,Architecture::NEON,GEBPPacketFull>
- : gebp_traits<float,float,false,false,Architecture::Generic,GEBPPacketFull>
-{
+#ifndef EIGEN_NEON_GEBP_NR
+#define EIGEN_NEON_GEBP_NR 8
+#endif
+
+template <>
+struct gebp_traits<float, float, false, false, Architecture::NEON, GEBPPacketFull>
+    : gebp_traits<float, float, false, false, Architecture::Generic, GEBPPacketFull> {
   typedef float RhsPacket;
   typedef float32x4_t RhsPacketx4;
+  enum { nr = EIGEN_NEON_GEBP_NR };
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const { dest = *b; }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const
-  {
-    dest = *b;
-  }
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const { dest = vld1q_f32(b); }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
-  {
-    dest = vld1q_f32(b);
-  }
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacket& dest) const { dest = *b; }
 
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacket& dest) const
-  {
-    dest = *b;
-  }
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
 
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
-  {}
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const { loadRhs(b, dest); }
 
-  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
-  {
-    loadRhs(b,dest);
-  }
-
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
-  {
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<0>&) const {
     c = vfmaq_n_f32(c, a, b);
   }
-
   // NOTE: Template parameter inference failed when compiled with Android NDK:
   // "candidate template ignored: could not match 'FixedInt<N>' against 'Eigen::internal::FixedInt<0>".
 
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
-  { madd_helper<0>(a, b, c); }
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<1>&) const
-  { madd_helper<1>(a, b, c); }
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<2>&) const
-  { madd_helper<2>(a, b, c); }
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<3>&) const
-  { madd_helper<3>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<0>&) const {
+    madd_helper<0>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<1>&) const {
+    madd_helper<1>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<2>&) const {
+    madd_helper<2>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<3>&) const {
+    madd_helper<3>(a, b, c);
+  }
 
  private:
-  template<int LaneID>
-  EIGEN_STRONG_INLINE void madd_helper(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c) const
-  {
-    #if EIGEN_COMP_GNUC_STRICT && !(EIGEN_GNUC_AT_LEAST(9,0))
-    // workaround gcc issue https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89101
-    // vfmaq_laneq_f32 is implemented through a costly dup
-         if(LaneID==0)  asm("fmla %0.4s, %1.4s, %2.s[0]\n" : "+w" (c) : "w" (a), "w" (b) :  );
-    else if(LaneID==1)  asm("fmla %0.4s, %1.4s, %2.s[1]\n" : "+w" (c) : "w" (a), "w" (b) :  );
-    else if(LaneID==2)  asm("fmla %0.4s, %1.4s, %2.s[2]\n" : "+w" (c) : "w" (a), "w" (b) :  );
-    else if(LaneID==3)  asm("fmla %0.4s, %1.4s, %2.s[3]\n" : "+w" (c) : "w" (a), "w" (b) :  );
-    #else
+  template <int LaneID>
+  EIGEN_STRONG_INLINE void madd_helper(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c) const {
+#if EIGEN_GNUC_STRICT_LESS_THAN(9, 0, 0)
+    // 1. workaround gcc issue https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89101
+    //    vfmaq_laneq_f32 is implemented through a costly dup, which was fixed in gcc9
+    // 2. workaround the gcc register split problem on arm64-neon
+    if (LaneID == 0)
+      asm("fmla %0.4s, %1.4s, %2.s[0]\n" : "+w"(c) : "w"(a), "w"(b) :);
+    else if (LaneID == 1)
+      asm("fmla %0.4s, %1.4s, %2.s[1]\n" : "+w"(c) : "w"(a), "w"(b) :);
+    else if (LaneID == 2)
+      asm("fmla %0.4s, %1.4s, %2.s[2]\n" : "+w"(c) : "w"(a), "w"(b) :);
+    else if (LaneID == 3)
+      asm("fmla %0.4s, %1.4s, %2.s[3]\n" : "+w"(c) : "w"(a), "w"(b) :);
+#else
     c = vfmaq_laneq_f32(c, a, b, LaneID);
-    #endif
+#endif
   }
 };
 
-
-template<>
-struct gebp_traits <double,double,false,false,Architecture::NEON>
- : gebp_traits<double,double,false,false,Architecture::Generic>
-{
+template <>
+struct gebp_traits<double, double, false, false, Architecture::NEON>
+    : gebp_traits<double, double, false, false, Architecture::Generic> {
   typedef double RhsPacket;
-
+  enum { nr = EIGEN_NEON_GEBP_NR };
   struct RhsPacketx4 {
     float64x2_t B_0, B_1;
   };
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const
-  {
-    dest = *b;
-  }
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const { dest = *b; }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
-  {
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const {
     dest.B_0 = vld1q_f64(b);
-    dest.B_1 = vld1q_f64(b+2);
+    dest.B_1 = vld1q_f64(b + 2);
   }
 
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacket& dest) const
-  {
-    loadRhs(b,dest);
-  }
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacket& dest) const { loadRhs(b, dest); }
 
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
-  {}
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
 
-  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
-  {
-    loadRhs(b,dest);
-  }
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const { loadRhs(b, dest); }
 
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
-  {
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<0>&) const {
     c = vfmaq_n_f64(c, a, b);
   }
 
   // NOTE: Template parameter inference failed when compiled with Android NDK:
   // "candidate template ignored: could not match 'FixedInt<N>' against 'Eigen::internal::FixedInt<0>".
 
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
-  { madd_helper<0>(a, b, c); }
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<1>&) const
-  { madd_helper<1>(a, b, c); }
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<2>&) const
-  { madd_helper<2>(a, b, c); }
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<3>&) const
-  { madd_helper<3>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<0>&) const {
+    madd_helper<0>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<1>&) const {
+    madd_helper<1>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<2>&) const {
+    madd_helper<2>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<3>&) const {
+    madd_helper<3>(a, b, c);
+  }
 
  private:
   template <int LaneID>
-  EIGEN_STRONG_INLINE void madd_helper(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c) const
-  {
-    #if EIGEN_COMP_GNUC_STRICT && !(EIGEN_GNUC_AT_LEAST(9,0))
-    // workaround gcc issue https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89101
-    // vfmaq_laneq_f64 is implemented through a costly dup
-         if(LaneID==0)  asm("fmla %0.2d, %1.2d, %2.d[0]\n" : "+w" (c) : "w" (a), "w" (b.B_0) :  );
-    else if(LaneID==1)  asm("fmla %0.2d, %1.2d, %2.d[1]\n" : "+w" (c) : "w" (a), "w" (b.B_0) :  );
-    else if(LaneID==2)  asm("fmla %0.2d, %1.2d, %2.d[0]\n" : "+w" (c) : "w" (a), "w" (b.B_1) :  );
-    else if(LaneID==3)  asm("fmla %0.2d, %1.2d, %2.d[1]\n" : "+w" (c) : "w" (a), "w" (b.B_1) :  );
-    #else
-         if(LaneID==0) c = vfmaq_laneq_f64(c, a, b.B_0, 0);
-    else if(LaneID==1) c = vfmaq_laneq_f64(c, a, b.B_0, 1);
-    else if(LaneID==2) c = vfmaq_laneq_f64(c, a, b.B_1, 0);
-    else if(LaneID==3) c = vfmaq_laneq_f64(c, a, b.B_1, 1);
-    #endif
+  EIGEN_STRONG_INLINE void madd_helper(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c) const {
+#if EIGEN_GNUC_STRICT_LESS_THAN(9, 0, 0)
+    // 1. workaround gcc issue https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89101
+    //    vfmaq_laneq_f64 is implemented through a costly dup, which was fixed in gcc9
+    // 2. workaround the gcc register split problem on arm64-neon
+    if (LaneID == 0)
+      asm("fmla %0.2d, %1.2d, %2.d[0]\n" : "+w"(c) : "w"(a), "w"(b.B_0) :);
+    else if (LaneID == 1)
+      asm("fmla %0.2d, %1.2d, %2.d[1]\n" : "+w"(c) : "w"(a), "w"(b.B_0) :);
+    else if (LaneID == 2)
+      asm("fmla %0.2d, %1.2d, %2.d[0]\n" : "+w"(c) : "w"(a), "w"(b.B_1) :);
+    else if (LaneID == 3)
+      asm("fmla %0.2d, %1.2d, %2.d[1]\n" : "+w"(c) : "w"(a), "w"(b.B_1) :);
+#else
+    if (LaneID == 0)
+      c = vfmaq_laneq_f64(c, a, b.B_0, 0);
+    else if (LaneID == 1)
+      c = vfmaq_laneq_f64(c, a, b.B_0, 1);
+    else if (LaneID == 2)
+      c = vfmaq_laneq_f64(c, a, b.B_1, 0);
+    else if (LaneID == 3)
+      c = vfmaq_laneq_f64(c, a, b.B_1, 1);
+#endif
   }
 };
 
-#endif // EIGEN_ARCH_ARM64
+// The register at operand 3 of fmla for data type half must be v0~v15, the compiler may not
+// allocate a required register for the '%2' of inline asm 'fmla %0.8h, %1.8h, %2.h[id]',
+// so inline assembly can't be used here to advoid the bug that vfmaq_lane_f16 is implemented
+// through a costly dup in gcc compiler.
+#if EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC && EIGEN_COMP_CLANG
+
+template <>
+struct gebp_traits<half, half, false, false, Architecture::NEON>
+    : gebp_traits<half, half, false, false, Architecture::Generic> {
+  typedef half RhsPacket;
+  typedef float16x4_t RhsPacketx4;
+  typedef float16x4_t PacketHalf;
+  enum { nr = EIGEN_NEON_GEBP_NR };
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const { dest = *b; }
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const { dest = vld1_f16((const __fp16*)b); }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacket& dest) const { dest = *b; }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
+
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar*, RhsPacket&) const {
+    // If LHS is a Packet8h, we cannot correctly mimic a ploadquad of the RHS
+    // using a single scalar value.
+    eigen_assert(false && "Cannot loadRhsQuad for a scalar RHS.");
+  }
+
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<0>&) const {
+    c = vfmaq_n_f16(c, a, b);
+  }
+  EIGEN_STRONG_INLINE void madd(const PacketHalf& a, const RhsPacket& b, PacketHalf& c, RhsPacket& /*tmp*/,
+                                const FixedInt<0>&) const {
+    c = vfma_n_f16(c, a, b);
+  }
+
+  // NOTE: Template parameter inference failed when compiled with Android NDK:
+  // "candidate template ignored: could not match 'FixedInt<N>' against 'Eigen::internal::FixedInt<0>".
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<0>&) const {
+    madd_helper<0>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<1>&) const {
+    madd_helper<1>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<2>&) const {
+    madd_helper<2>(a, b, c);
+  }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/,
+                                const FixedInt<3>&) const {
+    madd_helper<3>(a, b, c);
+  }
+
+ private:
+  template <int LaneID>
+  EIGEN_STRONG_INLINE void madd_helper(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c) const {
+    c = vfmaq_lane_f16(c, a, b, LaneID);
+  }
+};
+#endif  // EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC && EIGEN_COMP_CLANG
+#endif  // EIGEN_ARCH_ARM64
 
 }  // namespace internal
 }  // namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h
index fa6615a..3d2e7bd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h
@@ -8,35 +8,30 @@
 #ifndef EIGEN_MATH_FUNCTIONS_NEON_H
 #define EIGEN_MATH_FUNCTIONS_NEON_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f pexp<Packet2f>(const Packet2f& x)
-{ return pexp_float(x); }
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f pexp<Packet4f>(const Packet4f& x)
-{ return pexp_float(x); }
+EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_FLOAT(Packet2f)
+EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_FLOAT(Packet4f)
 
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f plog<Packet2f>(const Packet2f& x)
-{ return plog_float(x); }
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f plog<Packet4f>(const Packet4f& x)
-{ return plog_float(x); }
+#if EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet4hf ptanh<Packet4hf>(const Packet4hf& x) {
+  // Convert to float, call the float ptanh, and then convert back.
+  return vcvt_f16_f32(ptanh<Packet4f>(vcvt_f32_f16(x)));
+}
 
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f psin<Packet2f>(const Packet2f& x)
-{ return psin_float(x); }
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f psin<Packet4f>(const Packet4f& x)
-{ return psin_float(x); }
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f pcos<Packet2f>(const Packet2f& x)
-{ return pcos_float(x); }
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f pcos<Packet4f>(const Packet4f& x)
-{ return pcos_float(x); }
-
-// Hyperbolic Tangent function.
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f ptanh<Packet2f>(const Packet2f& x)
-{ return internal::generic_fast_tanh_float(x); }
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f ptanh<Packet4f>(const Packet4f& x)
-{ return internal::generic_fast_tanh_float(x); }
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet8hf ptanh<Packet8hf>(const Packet8hf& x) {
+  // Convert each 4 halfs to float, call the float ptanh, and then convert back.
+  return vcombine_f16(vcvt_f16_f32(ptanh<Packet4f>(vcvt_f32_f16(vget_low_f16(x)))),
+                      vcvt_f16_f32(ptanh<Packet4f>(vcvt_high_f32_f16(x))));
+}
+#endif  // EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
 
 BF16_PACKET_FUNCTION(Packet4f, Packet4bf, psin)
 BF16_PACKET_FUNCTION(Packet4f, Packet4bf, pcos)
@@ -60,16 +55,13 @@
 //---------- double ----------
 
 #if EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d pexp<Packet2d>(const Packet2d& x)
-{ return pexp_double(x); }
 
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d plog<Packet2d>(const Packet2d& x)
-{ return plog_double(x); }
+EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_DOUBLE(Packet2d)
 
 #endif
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATH_FUNCTIONS_NEON_H
+#endif  // EIGEN_MATH_FUNCTIONS_NEON_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h
index d2aeef4..4e3a14d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h
@@ -12,6 +12,9 @@
 #ifndef EIGEN_PACKET_MATH_NEON_H
 #define EIGEN_PACKET_MATH_NEON_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
@@ -38,430 +41,411 @@
 // are aliases to the same underlying type __n128.
 // We thus have to wrap them to make them different C++ types.
 // (See also bug 1428)
-typedef eigen_packet_wrapper<float32x2_t,0>  Packet2f;
-typedef eigen_packet_wrapper<float32x4_t,1>  Packet4f;
-typedef eigen_packet_wrapper<int32_t    ,2>  Packet4c;
-typedef eigen_packet_wrapper<int8x8_t   ,3>  Packet8c;
-typedef eigen_packet_wrapper<int8x16_t  ,4>  Packet16c;
-typedef eigen_packet_wrapper<uint32_t   ,5>  Packet4uc;
-typedef eigen_packet_wrapper<uint8x8_t  ,6>  Packet8uc;
-typedef eigen_packet_wrapper<uint8x16_t ,7>  Packet16uc;
-typedef eigen_packet_wrapper<int16x4_t  ,8>  Packet4s;
-typedef eigen_packet_wrapper<int16x8_t  ,9>  Packet8s;
-typedef eigen_packet_wrapper<uint16x4_t ,10> Packet4us;
-typedef eigen_packet_wrapper<uint16x8_t ,11> Packet8us;
-typedef eigen_packet_wrapper<int32x2_t  ,12> Packet2i;
-typedef eigen_packet_wrapper<int32x4_t  ,13> Packet4i;
-typedef eigen_packet_wrapper<uint32x2_t ,14> Packet2ui;
-typedef eigen_packet_wrapper<uint32x4_t ,15> Packet4ui;
-typedef eigen_packet_wrapper<int64x2_t  ,16> Packet2l;
-typedef eigen_packet_wrapper<uint64x2_t ,17> Packet2ul;
+typedef eigen_packet_wrapper<float32x2_t, 0> Packet2f;
+typedef eigen_packet_wrapper<float32x4_t, 1> Packet4f;
+typedef eigen_packet_wrapper<int32_t, 2> Packet4c;
+typedef eigen_packet_wrapper<int8x8_t, 3> Packet8c;
+typedef eigen_packet_wrapper<int8x16_t, 4> Packet16c;
+typedef eigen_packet_wrapper<uint32_t, 5> Packet4uc;
+typedef eigen_packet_wrapper<uint8x8_t, 6> Packet8uc;
+typedef eigen_packet_wrapper<uint8x16_t, 7> Packet16uc;
+typedef eigen_packet_wrapper<int16x4_t, 8> Packet4s;
+typedef eigen_packet_wrapper<int16x8_t, 9> Packet8s;
+typedef eigen_packet_wrapper<uint16x4_t, 10> Packet4us;
+typedef eigen_packet_wrapper<uint16x8_t, 11> Packet8us;
+typedef eigen_packet_wrapper<int32x2_t, 12> Packet2i;
+typedef eigen_packet_wrapper<int32x4_t, 13> Packet4i;
+typedef eigen_packet_wrapper<uint32x2_t, 14> Packet2ui;
+typedef eigen_packet_wrapper<uint32x4_t, 15> Packet4ui;
+typedef eigen_packet_wrapper<int64x2_t, 16> Packet2l;
+typedef eigen_packet_wrapper<uint64x2_t, 17> Packet2ul;
+
+EIGEN_ALWAYS_INLINE Packet4f make_packet4f(float a, float b, float c, float d) {
+  float from[4] = {a, b, c, d};
+  return vld1q_f32(from);
+}
+
+EIGEN_ALWAYS_INLINE Packet2f make_packet2f(float a, float b) {
+  float from[2] = {a, b};
+  return vld1_f32(from);
+}
 
 #else
 
-typedef float32x2_t                          Packet2f;
-typedef float32x4_t                          Packet4f;
-typedef eigen_packet_wrapper<int32_t    ,2>  Packet4c;
-typedef int8x8_t                             Packet8c;
-typedef int8x16_t                            Packet16c;
-typedef eigen_packet_wrapper<uint32_t   ,5>  Packet4uc;
-typedef uint8x8_t                            Packet8uc;
-typedef uint8x16_t                           Packet16uc;
-typedef int16x4_t                            Packet4s;
-typedef int16x8_t                            Packet8s;
-typedef uint16x4_t                           Packet4us;
-typedef uint16x8_t                           Packet8us;
-typedef int32x2_t                            Packet2i;
-typedef int32x4_t                            Packet4i;
-typedef uint32x2_t                           Packet2ui;
-typedef uint32x4_t                           Packet4ui;
-typedef int64x2_t                            Packet2l;
-typedef uint64x2_t                           Packet2ul;
+typedef float32x2_t Packet2f;
+typedef float32x4_t Packet4f;
+typedef eigen_packet_wrapper<int32_t, 2> Packet4c;
+typedef int8x8_t Packet8c;
+typedef int8x16_t Packet16c;
+typedef eigen_packet_wrapper<uint32_t, 5> Packet4uc;
+typedef uint8x8_t Packet8uc;
+typedef uint8x16_t Packet16uc;
+typedef int16x4_t Packet4s;
+typedef int16x8_t Packet8s;
+typedef uint16x4_t Packet4us;
+typedef uint16x8_t Packet8us;
+typedef int32x2_t Packet2i;
+typedef int32x4_t Packet4i;
+typedef uint32x2_t Packet2ui;
+typedef uint32x4_t Packet4ui;
+typedef int64x2_t Packet2l;
+typedef uint64x2_t Packet2ul;
 
-#endif // EIGEN_COMP_MSVC_STRICT
+EIGEN_ALWAYS_INLINE Packet4f make_packet4f(float a, float b, float c, float d) { return Packet4f{a, b, c, d}; }
+EIGEN_ALWAYS_INLINE Packet2f make_packet2f(float a, float b) { return Packet2f{a, b}; }
 
-EIGEN_STRONG_INLINE Packet4f shuffle1(const Packet4f& m, int mask){
+#endif  // EIGEN_COMP_MSVC_STRICT
+
+EIGEN_STRONG_INLINE Packet4f shuffle1(const Packet4f& m, int mask) {
   const float* a = reinterpret_cast<const float*>(&m);
-  Packet4f res = {*(a + (mask & 3)), *(a + ((mask >> 2) & 3)), *(a + ((mask >> 4) & 3 )), *(a + ((mask >> 6) & 3))};
+  Packet4f res =
+      make_packet4f(*(a + (mask & 3)), *(a + ((mask >> 2) & 3)), *(a + ((mask >> 4) & 3)), *(a + ((mask >> 6) & 3)));
   return res;
 }
 
 // fuctionally equivalent to _mm_shuffle_ps in SSE when interleave
 // == false (i.e. shuffle<false>(m, n, mask) equals _mm_shuffle_ps(m, n, mask)),
 // interleave m and n when interleave == true. Currently used in LU/arch/InverseSize4.h
-// to enable a shared implementation for fast inversion of matrices of size 4. 
-template<bool interleave> 
-EIGEN_STRONG_INLINE Packet4f shuffle2(const Packet4f &m, const Packet4f &n, int mask)
-{
+// to enable a shared implementation for fast inversion of matrices of size 4.
+template <bool interleave>
+EIGEN_STRONG_INLINE Packet4f shuffle2(const Packet4f& m, const Packet4f& n, int mask) {
   const float* a = reinterpret_cast<const float*>(&m);
   const float* b = reinterpret_cast<const float*>(&n);
-  Packet4f res = {*(a + (mask & 3)), *(a + ((mask >> 2) & 3)), *(b + ((mask >> 4) & 3)), *(b + ((mask >> 6) & 3))};
+  Packet4f res =
+      make_packet4f(*(a + (mask & 3)), *(a + ((mask >> 2) & 3)), *(b + ((mask >> 4) & 3)), *(b + ((mask >> 6) & 3)));
   return res;
 }
 
-template<> 
-EIGEN_STRONG_INLINE Packet4f shuffle2<true>(const Packet4f &m, const Packet4f &n, int mask) 
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f shuffle2<true>(const Packet4f& m, const Packet4f& n, int mask) {
   const float* a = reinterpret_cast<const float*>(&m);
   const float* b = reinterpret_cast<const float*>(&n);
-  Packet4f res = {*(a + (mask & 3)), *(b + ((mask >> 2) & 3)), *(a + ((mask >> 4) & 3)), *(b + ((mask >> 6) & 3))};
+  Packet4f res =
+      make_packet4f(*(a + (mask & 3)), *(b + ((mask >> 2) & 3)), *(a + ((mask >> 4) & 3)), *(b + ((mask >> 6) & 3)));
   return res;
 }
 
-EIGEN_STRONG_INLINE static int eigen_neon_shuffle_mask(int p, int q, int r, int s) {return ((s)<<6|(r)<<4|(q)<<2|(p));}
+EIGEN_STRONG_INLINE static int eigen_neon_shuffle_mask(int p, int q, int r, int s) {
+  return ((s) << 6 | (r) << 4 | (q) << 2 | (p));
+}
 
-EIGEN_STRONG_INLINE Packet4f vec4f_swizzle1(const Packet4f& a, int p, int q, int r, int s)
-{ 
+EIGEN_STRONG_INLINE Packet4f vec4f_swizzle1(const Packet4f& a, int p, int q, int r, int s) {
   return shuffle1(a, eigen_neon_shuffle_mask(p, q, r, s));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_swizzle2(const Packet4f& a, const Packet4f& b, int p, int q, int r, int s)
-{ 
-  return shuffle2<false>(a,b,eigen_neon_shuffle_mask(p, q, r, s));
+EIGEN_STRONG_INLINE Packet4f vec4f_swizzle2(const Packet4f& a, const Packet4f& b, int p, int q, int r, int s) {
+  return shuffle2<false>(a, b, eigen_neon_shuffle_mask(p, q, r, s));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_movelh(const Packet4f& a, const Packet4f& b)
-{
-  return shuffle2<false>(a,b,eigen_neon_shuffle_mask(0, 1, 0, 1));
+EIGEN_STRONG_INLINE Packet4f vec4f_movelh(const Packet4f& a, const Packet4f& b) {
+  return shuffle2<false>(a, b, eigen_neon_shuffle_mask(0, 1, 0, 1));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_movehl(const Packet4f& a, const Packet4f& b)
-{
-  return shuffle2<false>(b,a,eigen_neon_shuffle_mask(2, 3, 2, 3));
+EIGEN_STRONG_INLINE Packet4f vec4f_movehl(const Packet4f& a, const Packet4f& b) {
+  return shuffle2<false>(b, a, eigen_neon_shuffle_mask(2, 3, 2, 3));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_unpacklo(const Packet4f& a, const Packet4f& b)
-{
-  return shuffle2<true>(a,b,eigen_neon_shuffle_mask(0, 0, 1, 1));
+EIGEN_STRONG_INLINE Packet4f vec4f_unpacklo(const Packet4f& a, const Packet4f& b) {
+  return shuffle2<true>(a, b, eigen_neon_shuffle_mask(0, 0, 1, 1));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_unpackhi(const Packet4f& a, const Packet4f& b)
-{
-  return shuffle2<true>(a,b,eigen_neon_shuffle_mask(2, 2, 3, 3));
+EIGEN_STRONG_INLINE Packet4f vec4f_unpackhi(const Packet4f& a, const Packet4f& b) {
+  return shuffle2<true>(a, b, eigen_neon_shuffle_mask(2, 2, 3, 3));
 }
-#define vec4f_duplane(a, p) \
-  vdupq_lane_f32(vget_low_f32(a), p)
+#define vec4f_duplane(a, p) Packet4f(vdupq_lane_f32(vget_low_f32(a), p))
 
-#define _EIGEN_DECLARE_CONST_Packet4f(NAME,X) \
-  const Packet4f p4f_##NAME = pset1<Packet4f>(X)
+#define EIGEN_DECLARE_CONST_Packet4f(NAME, X) const Packet4f p4f_##NAME = pset1<Packet4f>(X)
 
-#define _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(NAME,X) \
+#define EIGEN_DECLARE_CONST_Packet4f_FROM_INT(NAME, X) \
   const Packet4f p4f_##NAME = vreinterpretq_f32_u32(pset1<int32_t>(X))
 
-#define _EIGEN_DECLARE_CONST_Packet4i(NAME,X) \
-  const Packet4i p4i_##NAME = pset1<Packet4i>(X)
+#define EIGEN_DECLARE_CONST_Packet4i(NAME, X) const Packet4i p4i_##NAME = pset1<Packet4i>(X)
 
-#if EIGEN_ARCH_ARM64
-  // __builtin_prefetch tends to do nothing on ARM64 compilers because the
-  // prefetch instructions there are too detailed for __builtin_prefetch to map
-  // meaningfully to them.
-  #define EIGEN_ARM_PREFETCH(ADDR)  __asm__ __volatile__("prfm pldl1keep, [%[addr]]\n" ::[addr] "r"(ADDR) : );
+#if EIGEN_ARCH_ARM64 && EIGEN_COMP_GNUC
+// __builtin_prefetch tends to do nothing on ARM64 compilers because the
+// prefetch instructions there are too detailed for __builtin_prefetch to map
+// meaningfully to them.
+#define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__("prfm pldl1keep, [%[addr]]\n" ::[addr] "r"(ADDR) :);
 #elif EIGEN_HAS_BUILTIN(__builtin_prefetch) || EIGEN_COMP_GNUC
-  #define EIGEN_ARM_PREFETCH(ADDR) __builtin_prefetch(ADDR);
+#define EIGEN_ARM_PREFETCH(ADDR) __builtin_prefetch(ADDR);
 #elif defined __pld
-  #define EIGEN_ARM_PREFETCH(ADDR) __pld(ADDR)
-#elif EIGEN_ARCH_ARM32
-  #define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__ ("pld [%[addr]]\n" :: [addr] "r" (ADDR) : );
+#define EIGEN_ARM_PREFETCH(ADDR) __pld(ADDR)
+#elif EIGEN_ARCH_ARM
+#define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__("pld [%[addr]]\n" ::[addr] "r"(ADDR) :);
 #else
-  // by default no explicit prefetching
-  #define EIGEN_ARM_PREFETCH(ADDR)
+// by default no explicit prefetching
+#define EIGEN_ARM_PREFETCH(ADDR)
 #endif
 
 template <>
-struct packet_traits<float> : default_packet_traits
-{
+struct packet_traits<float> : default_packet_traits {
   typedef Packet4f type;
   typedef Packet2f half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0,
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0,
 
-    HasDiv   = 1,
+    HasDiv = 1,
     HasFloor = 1,
     HasCeil = 1,
     HasRint = 1,
 
-    HasSin  = EIGEN_FAST_MATH,
-    HasCos  = EIGEN_FAST_MATH,
-    HasLog  = 1,
-    HasExp  = 1,
+    HasSin = EIGEN_FAST_MATH,
+    HasCos = EIGEN_FAST_MATH,
+    HasACos = 1,
+    HasASin = 1,
+    HasATan = 1,
+    HasATanh = 1,
+    HasLog = 1,
+    HasExp = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
     HasTanh = EIGEN_FAST_MATH,
-    HasErf  = EIGEN_FAST_MATH,
+    HasErf = EIGEN_FAST_MATH,
     HasBessel = 0,  // Issues with accuracy.
     HasNdtri = 0
   };
 };
 
 template <>
-struct packet_traits<int8_t> : default_packet_traits
-{
+struct packet_traits<int8_t> : default_packet_traits {
   typedef Packet16c type;
   typedef Packet8c half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 16,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasAbsDiff   = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasAbsDiff = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0
   };
 };
 
 template <>
-struct packet_traits<uint8_t> : default_packet_traits
-{
+struct packet_traits<uint8_t> : default_packet_traits {
   typedef Packet16uc type;
   typedef Packet8uc half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 16,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 0,
-    HasAbs       = 1,
-    HasAbsDiff   = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0,
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 0,
+    HasAbs = 1,
+    HasAbsDiff = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0,
 
     HasSqrt = 1
   };
 };
 
 template <>
-struct packet_traits<int16_t> : default_packet_traits
-{
+struct packet_traits<int16_t> : default_packet_traits {
   typedef Packet8s type;
   typedef Packet4s half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 8,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasAbsDiff   = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasAbsDiff = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0
   };
 };
 
 template <>
-struct packet_traits<uint16_t> : default_packet_traits
-{
+struct packet_traits<uint16_t> : default_packet_traits {
   typedef Packet8us type;
   typedef Packet4us half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 8,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 0,
-    HasAbs       = 0,
-    HasAbsDiff   = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0,
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 0,
+    HasAbs = 1,
+    HasAbsDiff = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0,
     HasSqrt = 1
   };
 };
 
 template <>
-struct packet_traits<int32_t> : default_packet_traits
-{
+struct packet_traits<int32_t> : default_packet_traits {
   typedef Packet4i type;
   typedef Packet2i half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0
   };
 };
 
 template <>
-struct packet_traits<uint32_t> : default_packet_traits
-{
+struct packet_traits<uint32_t> : default_packet_traits {
   typedef Packet4ui type;
   typedef Packet2ui half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket = 1,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 0,
-    HasAbs       = 0,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0,
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 0,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0,
 
     HasSqrt = 1
   };
 };
 
 template <>
-struct packet_traits<int64_t> : default_packet_traits
-{
+struct packet_traits<int64_t> : default_packet_traits {
   typedef Packet2l type;
   typedef Packet2l half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 2,
-    HasHalfPacket = 0,
 
-    HasCmp       = 1,
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0
   };
 };
 
 template <>
-struct packet_traits<uint64_t> : default_packet_traits
-{
+struct packet_traits<uint64_t> : default_packet_traits {
   typedef Packet2ul type;
   typedef Packet2ul half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 2,
-    HasHalfPacket = 0,
 
-    HasCmp       = 1,
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 0,
-    HasAbs       = 0,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 0,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0
   };
 };
 
-#if EIGEN_GNUC_AT_MOST(4, 4) && !EIGEN_COMP_LLVM
-// workaround gcc 4.2, 4.3 and 4.4 compilation issue
-EIGEN_STRONG_INLINE float32x4_t vld1q_f32(const float* x) { return ::vld1q_f32((const float32_t*)x); }
-EIGEN_STRONG_INLINE float32x2_t vld1_f32(const float* x) { return ::vld1_f32 ((const float32_t*)x); }
-EIGEN_STRONG_INLINE float32x2_t vld1_dup_f32(const float* x) { return ::vld1_dup_f32 ((const float32_t*)x); }
-EIGEN_STRONG_INLINE void vst1q_f32(float* to, float32x4_t from) { ::vst1q_f32((float32_t*)to,from); }
-EIGEN_STRONG_INLINE void vst1_f32 (float* to, float32x2_t from) { ::vst1_f32 ((float32_t*)to,from); }
-#endif
-
-template<> struct unpacket_traits<Packet2f>
-{
+template <>
+struct unpacket_traits<Packet2f> {
   typedef float type;
   typedef Packet2f half;
   typedef Packet2i integer_packet;
-  enum
-  {
+  enum {
     size = 2,
     alignment = Aligned16,
     vectorizable = true,
@@ -469,13 +453,12 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet4f>
-{
+template <>
+struct unpacket_traits<Packet4f> {
   typedef float type;
   typedef Packet2f half;
   typedef Packet4i integer_packet;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Aligned16,
     vectorizable = true,
@@ -483,12 +466,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet4c>
-{
+template <>
+struct unpacket_traits<Packet4c> {
   typedef int8_t type;
   typedef Packet4c half;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Unaligned,
     vectorizable = true,
@@ -496,12 +478,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet8c>
-{
+template <>
+struct unpacket_traits<Packet8c> {
   typedef int8_t type;
   typedef Packet4c half;
-  enum
-  {
+  enum {
     size = 8,
     alignment = Aligned16,
     vectorizable = true,
@@ -509,12 +490,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet16c>
-{
+template <>
+struct unpacket_traits<Packet16c> {
   typedef int8_t type;
   typedef Packet8c half;
-  enum
-  {
+  enum {
     size = 16,
     alignment = Aligned16,
     vectorizable = true,
@@ -522,12 +502,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet4uc>
-{
+template <>
+struct unpacket_traits<Packet4uc> {
   typedef uint8_t type;
   typedef Packet4uc half;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Unaligned,
     vectorizable = true,
@@ -535,12 +514,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet8uc>
-{
+template <>
+struct unpacket_traits<Packet8uc> {
   typedef uint8_t type;
   typedef Packet4uc half;
-  enum
-  {
+  enum {
     size = 8,
     alignment = Aligned16,
     vectorizable = true,
@@ -548,24 +526,23 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet16uc>
-{
+template <>
+struct unpacket_traits<Packet16uc> {
   typedef uint8_t type;
   typedef Packet8uc half;
-  enum
-  {
+  enum {
     size = 16,
     alignment = Aligned16,
     vectorizable = true,
     masked_load_available = false,
-    masked_store_available = false};
+    masked_store_available = false
+  };
 };
-template<> struct unpacket_traits<Packet4s>
-{
+template <>
+struct unpacket_traits<Packet4s> {
   typedef int16_t type;
   typedef Packet4s half;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Aligned16,
     vectorizable = true,
@@ -573,12 +550,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet8s>
-{
+template <>
+struct unpacket_traits<Packet8s> {
   typedef int16_t type;
   typedef Packet4s half;
-  enum
-  {
+  enum {
     size = 8,
     alignment = Aligned16,
     vectorizable = true,
@@ -586,12 +562,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet4us>
-{
+template <>
+struct unpacket_traits<Packet4us> {
   typedef uint16_t type;
   typedef Packet4us half;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Aligned16,
     vectorizable = true,
@@ -599,12 +574,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet8us>
-{
+template <>
+struct unpacket_traits<Packet8us> {
   typedef uint16_t type;
   typedef Packet4us half;
-  enum
-  {
+  enum {
     size = 8,
     alignment = Aligned16,
     vectorizable = true,
@@ -612,12 +586,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet2i>
-{
+template <>
+struct unpacket_traits<Packet2i> {
   typedef int32_t type;
   typedef Packet2i half;
-  enum
-  {
+  enum {
     size = 2,
     alignment = Aligned16,
     vectorizable = true,
@@ -625,12 +598,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet4i>
-{
+template <>
+struct unpacket_traits<Packet4i> {
   typedef int32_t type;
   typedef Packet2i half;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Aligned16,
     vectorizable = true,
@@ -638,12 +610,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet2ui>
-{
+template <>
+struct unpacket_traits<Packet2ui> {
   typedef uint32_t type;
   typedef Packet2ui half;
-  enum
-  {
+  enum {
     size = 2,
     alignment = Aligned16,
     vectorizable = true,
@@ -651,12 +622,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet4ui>
-{
+template <>
+struct unpacket_traits<Packet4ui> {
   typedef uint32_t type;
   typedef Packet2ui half;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Aligned16,
     vectorizable = true,
@@ -664,12 +634,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet2l>
-{
+template <>
+struct unpacket_traits<Packet2l> {
   typedef int64_t type;
   typedef Packet2l half;
-  enum
-  {
+  enum {
     size = 2,
     alignment = Aligned16,
     vectorizable = true,
@@ -677,12 +646,11 @@
     masked_store_available = false
   };
 };
-template<> struct unpacket_traits<Packet2ul>
-{
+template <>
+struct unpacket_traits<Packet2ul> {
   typedef uint64_t type;
   typedef Packet2ul half;
-  enum
-  {
+  enum {
     size = 2,
     alignment = Aligned16,
     vectorizable = true,
@@ -691,1850 +659,3046 @@
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet2f pset1<Packet2f>(const float& from) { return vdup_n_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float& from) { return vdupq_n_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4c pset1<Packet4c>(const int8_t& from)
-{ return vget_lane_s32(vreinterpret_s32_s8(vdup_n_s8(from)), 0); }
-template<> EIGEN_STRONG_INLINE Packet8c pset1<Packet8c>(const int8_t& from) { return vdup_n_s8(from); }
-template<> EIGEN_STRONG_INLINE Packet16c pset1<Packet16c>(const int8_t& from) { return vdupq_n_s8(from); }
-template<> EIGEN_STRONG_INLINE Packet4uc pset1<Packet4uc>(const uint8_t& from)
-{ return vget_lane_u32(vreinterpret_u32_u8(vdup_n_u8(from)), 0); }
-template<> EIGEN_STRONG_INLINE Packet8uc pset1<Packet8uc>(const uint8_t& from) { return vdup_n_u8(from); }
-template<> EIGEN_STRONG_INLINE Packet16uc pset1<Packet16uc>(const uint8_t& from) { return vdupq_n_u8(from); }
-template<> EIGEN_STRONG_INLINE Packet4s pset1<Packet4s>(const int16_t& from) { return vdup_n_s16(from); }
-template<> EIGEN_STRONG_INLINE Packet8s pset1<Packet8s>(const int16_t& from) { return vdupq_n_s16(from); }
-template<> EIGEN_STRONG_INLINE Packet4us pset1<Packet4us>(const uint16_t& from) { return vdup_n_u16(from); }
-template<> EIGEN_STRONG_INLINE Packet8us pset1<Packet8us>(const uint16_t& from) { return vdupq_n_u16(from); }
-template<> EIGEN_STRONG_INLINE Packet2i pset1<Packet2i>(const int32_t& from) { return vdup_n_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int32_t& from) { return vdupq_n_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet2ui pset1<Packet2ui>(const uint32_t& from) { return vdup_n_u32(from); }
-template<> EIGEN_STRONG_INLINE Packet4ui pset1<Packet4ui>(const uint32_t& from) { return vdupq_n_u32(from); }
-template<> EIGEN_STRONG_INLINE Packet2l pset1<Packet2l>(const int64_t& from) { return vdupq_n_s64(from); }
-template<> EIGEN_STRONG_INLINE Packet2ul pset1<Packet2ul>(const uint64_t& from) { return vdupq_n_u64(from); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pset1<Packet2f>(const float& from) {
+  return vdup_n_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float& from) {
+  return vdupq_n_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pset1<Packet4c>(const int8_t& from) {
+  return vget_lane_s32(vreinterpret_s32_s8(vdup_n_s8(from)), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pset1<Packet8c>(const int8_t& from) {
+  return vdup_n_s8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pset1<Packet16c>(const int8_t& from) {
+  return vdupq_n_s8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pset1<Packet4uc>(const uint8_t& from) {
+  return vget_lane_u32(vreinterpret_u32_u8(vdup_n_u8(from)), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pset1<Packet8uc>(const uint8_t& from) {
+  return vdup_n_u8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pset1<Packet16uc>(const uint8_t& from) {
+  return vdupq_n_u8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pset1<Packet4s>(const int16_t& from) {
+  return vdup_n_s16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pset1<Packet8s>(const int16_t& from) {
+  return vdupq_n_s16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pset1<Packet4us>(const uint16_t& from) {
+  return vdup_n_u16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pset1<Packet8us>(const uint16_t& from) {
+  return vdupq_n_u16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pset1<Packet2i>(const int32_t& from) {
+  return vdup_n_s32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int32_t& from) {
+  return vdupq_n_s32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pset1<Packet2ui>(const uint32_t& from) {
+  return vdup_n_u32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pset1<Packet4ui>(const uint32_t& from) {
+  return vdupq_n_u32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pset1<Packet2l>(const int64_t& from) {
+  return vdupq_n_s64(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pset1<Packet2ul>(const uint64_t& from) {
+  return vdupq_n_u64(from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pset1frombits<Packet2f>(unsigned int from)
-{ return vreinterpret_f32_u32(vdup_n_u32(from)); }
-template<> EIGEN_STRONG_INLINE Packet4f pset1frombits<Packet4f>(unsigned int from)
-{ return vreinterpretq_f32_u32(vdupq_n_u32(from)); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pset1frombits<Packet2f>(uint32_t from) {
+  return vreinterpret_f32_u32(vdup_n_u32(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pset1frombits<Packet4f>(uint32_t from) {
+  return vreinterpretq_f32_u32(vdupq_n_u32(from));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f plset<Packet2f>(const float& a)
-{
-  const float c[] = {0.0f,1.0f};
+template <>
+EIGEN_STRONG_INLINE Packet2f plset<Packet2f>(const float& a) {
+  const float c[] = {0.0f, 1.0f};
   return vadd_f32(pset1<Packet2f>(a), vld1_f32(c));
 }
-template<> EIGEN_STRONG_INLINE Packet4f plset<Packet4f>(const float& a)
-{
-  const float c[] = {0.0f,1.0f,2.0f,3.0f};
+template <>
+EIGEN_STRONG_INLINE Packet4f plset<Packet4f>(const float& a) {
+  const float c[] = {0.0f, 1.0f, 2.0f, 3.0f};
   return vaddq_f32(pset1<Packet4f>(a), vld1q_f32(c));
 }
-template<> EIGEN_STRONG_INLINE Packet4c plset<Packet4c>(const int8_t& a)
-{ return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(vreinterpret_s8_u32(vdup_n_u32(0x03020100)), vdup_n_s8(a))), 0); }
-template<> EIGEN_STRONG_INLINE Packet8c plset<Packet8c>(const int8_t& a)
-{
-  const int8_t c[] = {0,1,2,3,4,5,6,7};
+template <>
+EIGEN_STRONG_INLINE Packet4c plset<Packet4c>(const int8_t& a) {
+  return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(vreinterpret_s8_u32(vdup_n_u32(0x03020100)), vdup_n_s8(a))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c plset<Packet8c>(const int8_t& a) {
+  const int8_t c[] = {0, 1, 2, 3, 4, 5, 6, 7};
   return vadd_s8(pset1<Packet8c>(a), vld1_s8(c));
 }
-template<> EIGEN_STRONG_INLINE Packet16c plset<Packet16c>(const int8_t& a)
-{
-  const int8_t c[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
+template <>
+EIGEN_STRONG_INLINE Packet16c plset<Packet16c>(const int8_t& a) {
+  const int8_t c[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
   return vaddq_s8(pset1<Packet16c>(a), vld1q_s8(c));
 }
-template<> EIGEN_STRONG_INLINE Packet4uc plset<Packet4uc>(const uint8_t& a)
-{ return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(vreinterpret_u8_u32(vdup_n_u32(0x03020100)), vdup_n_u8(a))), 0); }
-template<> EIGEN_STRONG_INLINE Packet8uc plset<Packet8uc>(const uint8_t& a)
-{
-  const uint8_t c[] = {0,1,2,3,4,5,6,7};
+template <>
+EIGEN_STRONG_INLINE Packet4uc plset<Packet4uc>(const uint8_t& a) {
+  return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(vreinterpret_u8_u32(vdup_n_u32(0x03020100)), vdup_n_u8(a))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc plset<Packet8uc>(const uint8_t& a) {
+  const uint8_t c[] = {0, 1, 2, 3, 4, 5, 6, 7};
   return vadd_u8(pset1<Packet8uc>(a), vld1_u8(c));
 }
-template<> EIGEN_STRONG_INLINE Packet16uc plset<Packet16uc>(const uint8_t& a)
-{
-  const uint8_t c[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
+template <>
+EIGEN_STRONG_INLINE Packet16uc plset<Packet16uc>(const uint8_t& a) {
+  const uint8_t c[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
   return vaddq_u8(pset1<Packet16uc>(a), vld1q_u8(c));
 }
-template<> EIGEN_STRONG_INLINE Packet4s plset<Packet4s>(const int16_t& a)
-{
-  const int16_t c[] = {0,1,2,3};
+template <>
+EIGEN_STRONG_INLINE Packet4s plset<Packet4s>(const int16_t& a) {
+  const int16_t c[] = {0, 1, 2, 3};
   return vadd_s16(pset1<Packet4s>(a), vld1_s16(c));
 }
-template<> EIGEN_STRONG_INLINE Packet4us plset<Packet4us>(const uint16_t& a)
-{
-  const uint16_t c[] = {0,1,2,3};
+template <>
+EIGEN_STRONG_INLINE Packet4us plset<Packet4us>(const uint16_t& a) {
+  const uint16_t c[] = {0, 1, 2, 3};
   return vadd_u16(pset1<Packet4us>(a), vld1_u16(c));
 }
-template<> EIGEN_STRONG_INLINE Packet8s plset<Packet8s>(const int16_t& a)
-{
-  const int16_t c[] = {0,1,2,3,4,5,6,7};
+template <>
+EIGEN_STRONG_INLINE Packet8s plset<Packet8s>(const int16_t& a) {
+  const int16_t c[] = {0, 1, 2, 3, 4, 5, 6, 7};
   return vaddq_s16(pset1<Packet8s>(a), vld1q_s16(c));
 }
-template<> EIGEN_STRONG_INLINE Packet8us plset<Packet8us>(const uint16_t& a)
-{
-  const uint16_t c[] = {0,1,2,3,4,5,6,7};
+template <>
+EIGEN_STRONG_INLINE Packet8us plset<Packet8us>(const uint16_t& a) {
+  const uint16_t c[] = {0, 1, 2, 3, 4, 5, 6, 7};
   return vaddq_u16(pset1<Packet8us>(a), vld1q_u16(c));
 }
-template<> EIGEN_STRONG_INLINE Packet2i plset<Packet2i>(const int32_t& a)
-{
-  const int32_t c[] = {0,1};
+template <>
+EIGEN_STRONG_INLINE Packet2i plset<Packet2i>(const int32_t& a) {
+  const int32_t c[] = {0, 1};
   return vadd_s32(pset1<Packet2i>(a), vld1_s32(c));
 }
-template<> EIGEN_STRONG_INLINE Packet4i plset<Packet4i>(const int32_t& a)
-{
-  const int32_t c[] = {0,1,2,3};
+template <>
+EIGEN_STRONG_INLINE Packet4i plset<Packet4i>(const int32_t& a) {
+  const int32_t c[] = {0, 1, 2, 3};
   return vaddq_s32(pset1<Packet4i>(a), vld1q_s32(c));
 }
-template<> EIGEN_STRONG_INLINE Packet2ui plset<Packet2ui>(const uint32_t& a)
-{
-  const uint32_t c[] = {0,1};
+template <>
+EIGEN_STRONG_INLINE Packet2ui plset<Packet2ui>(const uint32_t& a) {
+  const uint32_t c[] = {0, 1};
   return vadd_u32(pset1<Packet2ui>(a), vld1_u32(c));
 }
-template<> EIGEN_STRONG_INLINE Packet4ui plset<Packet4ui>(const uint32_t& a)
-{
-  const uint32_t c[] = {0,1,2,3};
+template <>
+EIGEN_STRONG_INLINE Packet4ui plset<Packet4ui>(const uint32_t& a) {
+  const uint32_t c[] = {0, 1, 2, 3};
   return vaddq_u32(pset1<Packet4ui>(a), vld1q_u32(c));
 }
-template<> EIGEN_STRONG_INLINE Packet2l plset<Packet2l>(const int64_t& a)
-{
-  const int64_t c[] = {0,1};
+template <>
+EIGEN_STRONG_INLINE Packet2l plset<Packet2l>(const int64_t& a) {
+  const int64_t c[] = {0, 1};
   return vaddq_s64(pset1<Packet2l>(a), vld1q_s64(c));
 }
-template<> EIGEN_STRONG_INLINE Packet2ul plset<Packet2ul>(const uint64_t& a)
-{
-  const uint64_t c[] = {0,1};
+template <>
+EIGEN_STRONG_INLINE Packet2ul plset<Packet2ul>(const uint64_t& a) {
+  const uint64_t c[] = {0, 1};
   return vaddq_u64(pset1<Packet2ul>(a), vld1q_u64(c));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f padd<Packet2f>(const Packet2f& a, const Packet2f& b) { return vadd_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f padd<Packet4f>(const Packet4f& a, const Packet4f& b) { return vaddq_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4c padd<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet2f padd<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vadd_f32(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8c padd<Packet8c>(const Packet8c& a, const Packet8c& b) { return vadd_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c padd<Packet16c>(const Packet16c& a, const Packet16c& b) { return vaddq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc padd<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet4f padd<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vaddq_f32(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8uc padd<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vadd_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc padd<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vaddq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s padd<Packet4s>(const Packet4s& a, const Packet4s& b) { return vadd_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s padd<Packet8s>(const Packet8s& a, const Packet8s& b) { return vaddq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us padd<Packet4us>(const Packet4us& a, const Packet4us& b) { return vadd_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us padd<Packet8us>(const Packet8us& a, const Packet8us& b) { return vaddq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i padd<Packet2i>(const Packet2i& a, const Packet2i& b) { return vadd_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) { return vaddq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui padd<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vadd_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui padd<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vaddq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l padd<Packet2l>(const Packet2l& a, const Packet2l& b) { return vaddq_s64(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ul padd<Packet2ul>(const Packet2ul& a, const Packet2ul& b) { return vaddq_u64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4c padd<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_s8(vadd_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c padd<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vadd_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c padd<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vaddq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc padd<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vadd_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc padd<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vadd_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc padd<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vaddq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s padd<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vadd_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s padd<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vaddq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us padd<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vadd_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us padd<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vaddq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i padd<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vadd_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vaddq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui padd<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vadd_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui padd<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vaddq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l padd<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vaddq_s64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul padd<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vaddq_u64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f psub<Packet2f>(const Packet2f& a, const Packet2f& b) { return vsub_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) { return vsubq_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4c psub<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vsub_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet2f psub<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vsub_f32(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8c psub<Packet8c>(const Packet8c& a, const Packet8c& b) { return vsub_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c psub<Packet16c>(const Packet16c& a, const Packet16c& b) { return vsubq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc psub<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vsub_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vsubq_f32(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8uc psub<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vsub_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc psub<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vsubq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s psub<Packet4s>(const Packet4s& a, const Packet4s& b) { return vsub_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s psub<Packet8s>(const Packet8s& a, const Packet8s& b) { return vsubq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us psub<Packet4us>(const Packet4us& a, const Packet4us& b) { return vsub_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us psub<Packet8us>(const Packet8us& a, const Packet8us& b) { return vsubq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i psub<Packet2i>(const Packet2i& a, const Packet2i& b) { return vsub_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) { return vsubq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui psub<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vsub_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui psub<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vsubq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l psub<Packet2l>(const Packet2l& a, const Packet2l& b) { return vsubq_s64(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ul psub<Packet2ul>(const Packet2ul& a, const Packet2ul& b) { return vsubq_u64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4c psub<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_s8(vsub_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c psub<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vsub_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c psub<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vsubq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc psub<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vsub_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc psub<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vsub_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc psub<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vsubq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s psub<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vsub_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s psub<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vsubq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us psub<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vsub_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us psub<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vsubq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i psub<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vsub_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vsubq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui psub<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vsub_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui psub<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vsubq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l psub<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vsubq_s64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul psub<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vsubq_u64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pxor<Packet2f>(const Packet2f& a, const Packet2f& b);
-template<> EIGEN_STRONG_INLINE Packet2f paddsub<Packet2f>(const Packet2f& a, const Packet2f & b) {
-  Packet2f mask = {numext::bit_cast<float>(0x80000000u), 0.0f};
+template <>
+EIGEN_STRONG_INLINE Packet2f pxor<Packet2f>(const Packet2f& a, const Packet2f& b);
+template <>
+EIGEN_STRONG_INLINE Packet2f paddsub<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  Packet2f mask = make_packet2f(numext::bit_cast<float>(0x80000000u), 0.0f);
   return padd(a, pxor(mask, b));
 }
-template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b);
-template<> EIGEN_STRONG_INLINE Packet4f paddsub<Packet4f>(const Packet4f& a, const Packet4f& b) {
-  Packet4f mask = {numext::bit_cast<float>(0x80000000u), 0.0f, numext::bit_cast<float>(0x80000000u), 0.0f};
+template <>
+EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b);
+template <>
+EIGEN_STRONG_INLINE Packet4f paddsub<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  Packet4f mask = make_packet4f(numext::bit_cast<float>(0x80000000u), 0.0f, numext::bit_cast<float>(0x80000000u), 0.0f);
   return padd(a, pxor(mask, b));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pnegate(const Packet2f& a) { return vneg_f32(a); }
-template<> EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a) { return vnegq_f32(a); }
-template<> EIGEN_STRONG_INLINE Packet4c pnegate(const Packet4c& a)
-{ return vget_lane_s32(vreinterpret_s32_s8(vneg_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0); }
-template<> EIGEN_STRONG_INLINE Packet8c pnegate(const Packet8c& a) { return vneg_s8(a); }
-template<> EIGEN_STRONG_INLINE Packet16c pnegate(const Packet16c& a) { return vnegq_s8(a); }
-template<> EIGEN_STRONG_INLINE Packet4s pnegate(const Packet4s& a) { return vneg_s16(a); }
-template<> EIGEN_STRONG_INLINE Packet8s pnegate(const Packet8s& a) { return vnegq_s16(a); }
-template<> EIGEN_STRONG_INLINE Packet2i pnegate(const Packet2i& a) { return vneg_s32(a); }
-template<> EIGEN_STRONG_INLINE Packet4i pnegate(const Packet4i& a) { return vnegq_s32(a); }
-template<> EIGEN_STRONG_INLINE Packet2l pnegate(const Packet2l& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2f pnegate(const Packet2f& a) {
+  return vneg_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a) {
+  return vnegq_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pnegate(const Packet4c& a) {
+  return vget_lane_s32(vreinterpret_s32_s8(vneg_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pnegate(const Packet8c& a) {
+  return vneg_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pnegate(const Packet16c& a) {
+  return vnegq_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pnegate(const Packet4s& a) {
+  return vneg_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pnegate(const Packet8s& a) {
+  return vnegq_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pnegate(const Packet2i& a) {
+  return vneg_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pnegate(const Packet4i& a) {
+  return vnegq_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pnegate(const Packet2l& a) {
 #if EIGEN_ARCH_ARM64
   return vnegq_s64(a);
 #else
-  return vcombine_s64(
-      vdup_n_s64(-vgetq_lane_s64(a, 0)),
-      vdup_n_s64(-vgetq_lane_s64(a, 1)));
+  return vcombine_s64(vdup_n_s64(-vgetq_lane_s64(a, 0)), vdup_n_s64(-vgetq_lane_s64(a, 1)));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pconj(const Packet2f& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4f pconj(const Packet4f& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4c pconj(const Packet4c& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet8c pconj(const Packet8c& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet16c pconj(const Packet16c& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4uc pconj(const Packet4uc& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet8uc pconj(const Packet8uc& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet16uc pconj(const Packet16uc& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4s pconj(const Packet4s& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet8s pconj(const Packet8s& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4us pconj(const Packet4us& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet8us pconj(const Packet8us& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2i pconj(const Packet2i& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4i pconj(const Packet4i& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2ui pconj(const Packet2ui& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4ui pconj(const Packet4ui& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2l pconj(const Packet2l& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2ul pconj(const Packet2ul& a) { return a; }
-
-template<> EIGEN_STRONG_INLINE Packet2f pmul<Packet2f>(const Packet2f& a, const Packet2f& b) { return vmul_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f pmul<Packet4f>(const Packet4f& a, const Packet4f& b) { return vmulq_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4c pmul<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vmul_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet2f pconj(const Packet2f& a) {
+  return a;
 }
-template<> EIGEN_STRONG_INLINE Packet8c pmul<Packet8c>(const Packet8c& a, const Packet8c& b) { return vmul_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pmul<Packet16c>(const Packet16c& a, const Packet16c& b) { return vmulq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pmul<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vmul_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet4f pconj(const Packet4f& a) {
+  return a;
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pmul<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vmul_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pmul<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vmulq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pmul<Packet4s>(const Packet4s& a, const Packet4s& b) { return vmul_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pmul<Packet8s>(const Packet8s& a, const Packet8s& b) { return vmulq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pmul<Packet4us>(const Packet4us& a, const Packet4us& b) { return vmul_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pmul<Packet8us>(const Packet8us& a, const Packet8us& b) { return vmulq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pmul<Packet2i>(const Packet2i& a, const Packet2i& b) { return vmul_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pmul<Packet4i>(const Packet4i& a, const Packet4i& b) { return vmulq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pmul<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vmul_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pmul<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vmulq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pmul<Packet2l>(const Packet2l& a, const Packet2l& b) {
-  return vcombine_s64(
-    vdup_n_s64(vgetq_lane_s64(a, 0)*vgetq_lane_s64(b, 0)),
-    vdup_n_s64(vgetq_lane_s64(a, 1)*vgetq_lane_s64(b, 1)));
+template <>
+EIGEN_STRONG_INLINE Packet4c pconj(const Packet4c& a) {
+  return a;
 }
-template<> EIGEN_STRONG_INLINE Packet2ul pmul<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
-  return vcombine_u64(
-    vdup_n_u64(vgetq_lane_u64(a, 0)*vgetq_lane_u64(b, 0)),
-    vdup_n_u64(vgetq_lane_u64(a, 1)*vgetq_lane_u64(b, 1)));
+template <>
+EIGEN_STRONG_INLINE Packet8c pconj(const Packet8c& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pconj(const Packet16c& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pconj(const Packet4uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pconj(const Packet8uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pconj(const Packet16uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pconj(const Packet4s& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pconj(const Packet8s& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pconj(const Packet4us& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pconj(const Packet8us& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pconj(const Packet2i& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pconj(const Packet4i& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pconj(const Packet2ui& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pconj(const Packet4ui& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pconj(const Packet2l& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pconj(const Packet2ul& a) {
+  return a;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pdiv<Packet2f>(const Packet2f& a, const Packet2f& b)
-{
-#if EIGEN_ARCH_ARM64
-  return vdiv_f32(a,b);
-#else
-  Packet2f inv, restep, div;
-
-  // NEON does not offer a divide instruction, we have to do a reciprocal approximation
-  // However NEON in contrast to other SIMD engines (AltiVec/SSE), offers
-  // a reciprocal estimate AND a reciprocal step -which saves a few instructions
-  // vrecpeq_f32() returns an estimate to 1/b, which we will finetune with
-  // Newton-Raphson and vrecpsq_f32()
-  inv = vrecpe_f32(b);
-
-  // This returns a differential, by which we will have to multiply inv to get a better
-  // approximation of 1/b.
-  restep = vrecps_f32(b, inv);
-  inv = vmul_f32(restep, inv);
-
-  // Finally, multiply a by 1/b and get the wanted result of the division.
-  div = vmul_f32(a, inv);
-
-  return div;
-#endif
+template <>
+EIGEN_STRONG_INLINE Packet2f pmul<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vmul_f32(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b)
-{
-#if EIGEN_ARCH_ARM64
-  return vdivq_f32(a,b);
-#else
-  Packet4f inv, restep, div;
-
-  // NEON does not offer a divide instruction, we have to do a reciprocal approximation
-  // However NEON in contrast to other SIMD engines (AltiVec/SSE), offers
-  // a reciprocal estimate AND a reciprocal step -which saves a few instructions
-  // vrecpeq_f32() returns an estimate to 1/b, which we will finetune with
-  // Newton-Raphson and vrecpsq_f32()
-  inv = vrecpeq_f32(b);
-
-  // This returns a differential, by which we will have to multiply inv to get a better
-  // approximation of 1/b.
-  restep = vrecpsq_f32(b, inv);
-  inv = vmulq_f32(restep, inv);
-
-  // Finally, multiply a by 1/b and get the wanted result of the division.
-  div = vmulq_f32(a, inv);
-
-  return div;
-#endif
+template <>
+EIGEN_STRONG_INLINE Packet4f pmul<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vmulq_f32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pmul<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_s8(vmul_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pmul<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vmul_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pmul<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vmulq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pmul<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vmul_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pmul<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vmul_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pmul<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vmulq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pmul<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vmul_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pmul<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vmulq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pmul<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vmul_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pmul<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vmulq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pmul<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vmul_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pmul<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vmulq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pmul<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vmul_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmul<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vmulq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pmul<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_s64(vdup_n_s64(vgetq_lane_s64(a, 0) * vgetq_lane_s64(b, 0)),
+                      vdup_n_s64(vgetq_lane_s64(a, 1) * vgetq_lane_s64(b, 1)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pmul<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vcombine_u64(vdup_n_u64(vgetq_lane_u64(a, 0) * vgetq_lane_u64(b, 0)),
+                      vdup_n_u64(vgetq_lane_u64(a, 1) * vgetq_lane_u64(b, 1)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4c pdiv<Packet4c>(const Packet4c& /*a*/, const Packet4c& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4c pdiv<Packet4c>(const Packet4c& /*a*/, const Packet4c& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet4c>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet8c pdiv<Packet8c>(const Packet8c& /*a*/, const Packet8c& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8c pdiv<Packet8c>(const Packet8c& /*a*/, const Packet8c& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet8c>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet16c pdiv<Packet16c>(const Packet16c& /*a*/, const Packet16c& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet16c pdiv<Packet16c>(const Packet16c& /*a*/, const Packet16c& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet16c>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet4uc pdiv<Packet4uc>(const Packet4uc& /*a*/, const Packet4uc& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4uc pdiv<Packet4uc>(const Packet4uc& /*a*/, const Packet4uc& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet4uc>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pdiv<Packet8uc>(const Packet8uc& /*a*/, const Packet8uc& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8uc pdiv<Packet8uc>(const Packet8uc& /*a*/, const Packet8uc& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet8uc>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet16uc pdiv<Packet16uc>(const Packet16uc& /*a*/, const Packet16uc& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet16uc pdiv<Packet16uc>(const Packet16uc& /*a*/, const Packet16uc& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet16uc>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet4s pdiv<Packet4s>(const Packet4s& /*a*/, const Packet4s& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4s pdiv<Packet4s>(const Packet4s& /*a*/, const Packet4s& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet4s>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet8s pdiv<Packet8s>(const Packet8s& /*a*/, const Packet8s& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8s pdiv<Packet8s>(const Packet8s& /*a*/, const Packet8s& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet8s>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet4us pdiv<Packet4us>(const Packet4us& /*a*/, const Packet4us& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4us pdiv<Packet4us>(const Packet4us& /*a*/, const Packet4us& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet4us>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet8us pdiv<Packet8us>(const Packet8us& /*a*/, const Packet8us& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8us pdiv<Packet8us>(const Packet8us& /*a*/, const Packet8us& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet8us>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet2i pdiv<Packet2i>(const Packet2i& /*a*/, const Packet2i& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2i pdiv<Packet2i>(const Packet2i& /*a*/, const Packet2i& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet2i>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet4i pdiv<Packet4i>(const Packet4i& /*a*/, const Packet4i& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4i pdiv<Packet4i>(const Packet4i& /*a*/, const Packet4i& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet4i>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet2ui pdiv<Packet2ui>(const Packet2ui& /*a*/, const Packet2ui& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2ui pdiv<Packet2ui>(const Packet2ui& /*a*/, const Packet2ui& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet2ui>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet4ui pdiv<Packet4ui>(const Packet4ui& /*a*/, const Packet4ui& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4ui pdiv<Packet4ui>(const Packet4ui& /*a*/, const Packet4ui& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet4ui>(0);
 }
-template<> EIGEN_STRONG_INLINE Packet2l pdiv<Packet2l>(const Packet2l& /*a*/, const Packet2l& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2l pdiv<Packet2l>(const Packet2l& /*a*/, const Packet2l& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet2l>(0LL);
 }
-template<> EIGEN_STRONG_INLINE Packet2ul pdiv<Packet2ul>(const Packet2ul& /*a*/, const Packet2ul& /*b*/)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2ul pdiv<Packet2ul>(const Packet2ul& /*a*/, const Packet2ul& /*b*/) {
   eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet2ul>(0ULL);
 }
 
-
 #ifdef __ARM_FEATURE_FMA
-template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c)
-{ return vfmaq_f32(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet2f pmadd(const Packet2f& a, const Packet2f& b, const Packet2f& c)
-{ return vfma_f32(c,a,b); }
-#else
-template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c)
-{
-  return vmlaq_f32(c,a,b);
+template <>
+EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
+  return vfmaq_f32(c, a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet2f pmadd(const Packet2f& a, const Packet2f& b, const Packet2f& c)
-{
-  return vmla_f32(c,a,b);
+template <>
+EIGEN_STRONG_INLINE Packet2f pmadd(const Packet2f& a, const Packet2f& b, const Packet2f& c) {
+  return vfma_f32(c, a, b);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
+  return vmlaq_f32(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pmadd(const Packet2f& a, const Packet2f& b, const Packet2f& c) {
+  return vmla_f32(c, a, b);
 }
 #endif
 
 // No FMA instruction for int, so use MLA unconditionally.
-template<> EIGEN_STRONG_INLINE Packet4c pmadd(const Packet4c& a, const Packet4c& b, const Packet4c& c)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vmla_s8(
-      vreinterpret_s8_s32(vdup_n_s32(c)),
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet4c pmadd(const Packet4c& a, const Packet4c& b, const Packet4c& c) {
+  return vget_lane_s32(
+      vreinterpret_s32_s8(vmla_s8(vreinterpret_s8_s32(vdup_n_s32(c)), vreinterpret_s8_s32(vdup_n_s32(a)),
+                                  vreinterpret_s8_s32(vdup_n_s32(b)))),
+      0);
 }
-template<> EIGEN_STRONG_INLINE Packet8c pmadd(const Packet8c& a, const Packet8c& b, const Packet8c& c)
-{ return vmla_s8(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pmadd(const Packet16c& a, const Packet16c& b, const Packet16c& c)
-{ return vmlaq_s8(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pmadd(const Packet4uc& a, const Packet4uc& b, const Packet4uc& c)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vmla_u8(
-      vreinterpret_u8_u32(vdup_n_u32(c)),
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet8c pmadd(const Packet8c& a, const Packet8c& b, const Packet8c& c) {
+  return vmla_s8(c, a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pmadd(const Packet8uc& a, const Packet8uc& b, const Packet8uc& c)
-{ return vmla_u8(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pmadd(const Packet16uc& a, const Packet16uc& b, const Packet16uc& c)
-{ return vmlaq_u8(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pmadd(const Packet4s& a, const Packet4s& b, const Packet4s& c)
-{ return vmla_s16(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pmadd(const Packet8s& a, const Packet8s& b, const Packet8s& c)
-{ return vmlaq_s16(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pmadd(const Packet4us& a, const Packet4us& b, const Packet4us& c)
-{ return vmla_u16(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pmadd(const Packet8us& a, const Packet8us& b, const Packet8us& c)
-{ return vmlaq_u16(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pmadd(const Packet2i& a, const Packet2i& b, const Packet2i& c)
-{ return vmla_s32(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c)
-{ return vmlaq_s32(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pmadd(const Packet2ui& a, const Packet2ui& b, const Packet2ui& c)
-{ return vmla_u32(c,a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pmadd(const Packet4ui& a, const Packet4ui& b, const Packet4ui& c)
-{ return vmlaq_u32(c,a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet16c pmadd(const Packet16c& a, const Packet16c& b, const Packet16c& c) {
+  return vmlaq_s8(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pmadd(const Packet4uc& a, const Packet4uc& b, const Packet4uc& c) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vmla_u8(vreinterpret_u8_u32(vdup_n_u32(c)), vreinterpret_u8_u32(vdup_n_u32(a)),
+                                  vreinterpret_u8_u32(vdup_n_u32(b)))),
+      0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pmadd(const Packet8uc& a, const Packet8uc& b, const Packet8uc& c) {
+  return vmla_u8(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pmadd(const Packet16uc& a, const Packet16uc& b, const Packet16uc& c) {
+  return vmlaq_u8(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pmadd(const Packet4s& a, const Packet4s& b, const Packet4s& c) {
+  return vmla_s16(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pmadd(const Packet8s& a, const Packet8s& b, const Packet8s& c) {
+  return vmlaq_s16(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pmadd(const Packet4us& a, const Packet4us& b, const Packet4us& c) {
+  return vmla_u16(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pmadd(const Packet8us& a, const Packet8us& b, const Packet8us& c) {
+  return vmlaq_u16(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pmadd(const Packet2i& a, const Packet2i& b, const Packet2i& c) {
+  return vmla_s32(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) {
+  return vmlaq_s32(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pmadd(const Packet2ui& a, const Packet2ui& b, const Packet2ui& c) {
+  return vmla_u32(c, a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmadd(const Packet4ui& a, const Packet4ui& b, const Packet4ui& c) {
+  return vmlaq_u32(c, a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pabsdiff<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vabd_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f pabsdiff<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vabdq_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4c pabsdiff<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vabd_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet2f pabsdiff<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vabd_f32(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8c pabsdiff<Packet8c>(const Packet8c& a, const Packet8c& b)
-{ return vabd_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pabsdiff<Packet16c>(const Packet16c& a, const Packet16c& b)
-{ return vabdq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pabsdiff<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vabd_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet4f pabsdiff<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vabdq_f32(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pabsdiff<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return vabd_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pabsdiff<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return vabdq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pabsdiff<Packet4s>(const Packet4s& a, const Packet4s& b)
-{ return vabd_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pabsdiff<Packet8s>(const Packet8s& a, const Packet8s& b)
-{ return vabdq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pabsdiff<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return vabd_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pabsdiff<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return vabdq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pabsdiff<Packet2i>(const Packet2i& a, const Packet2i& b)
-{ return vabd_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pabsdiff<Packet4i>(const Packet4i& a, const Packet4i& b)
-{ return vabdq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pabsdiff<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return vabd_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pabsdiff<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return vabdq_u32(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4c pabsdiff<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_s8(vabd_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pabsdiff<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vabd_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pabsdiff<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vabdq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pabsdiff<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vabd_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pabsdiff<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vabd_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pabsdiff<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vabdq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pabsdiff<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vabd_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pabsdiff<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vabdq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pabsdiff<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vabd_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pabsdiff<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vabdq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pabsdiff<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vabd_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pabsdiff<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vabdq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pabsdiff<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vabd_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pabsdiff<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vabdq_u32(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pmin<Packet2f>(const Packet2f& a, const Packet2f& b) { return vmin_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) { return vminq_f32(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pmin<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vmin_f32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vminq_f32(a, b);
+}
 
 #ifdef __ARM_FEATURE_NUMERIC_MAXMIN
-// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
-template<> EIGEN_STRONG_INLINE Packet4f pmin<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) { return vminnmq_f32(a, b); }
-template<> EIGEN_STRONG_INLINE Packet2f pmin<PropagateNumbers, Packet2f>(const Packet2f& a, const Packet2f& b) { return vminnm_f32(a, b); }
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8
+// systems).
+template <>
+EIGEN_STRONG_INLINE Packet4f pmin<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vminnmq_f32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pmin<PropagateNumbers, Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vminnm_f32(a, b);
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4f pmin<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) { return pmin<Packet4f>(a, b); }
-
-template<> EIGEN_STRONG_INLINE Packet2f pmin<PropagateNaN, Packet2f>(const Packet2f& a, const Packet2f& b) { return pmin<Packet2f>(a, b); }
-
-template<> EIGEN_STRONG_INLINE Packet4c pmin<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vmin_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
-}
-template<> EIGEN_STRONG_INLINE Packet8c pmin<Packet8c>(const Packet8c& a, const Packet8c& b) { return vmin_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pmin<Packet16c>(const Packet16c& a, const Packet16c& b) { return vminq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pmin<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vmin_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
-}
-template<> EIGEN_STRONG_INLINE Packet8uc pmin<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vmin_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pmin<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vminq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pmin<Packet4s>(const Packet4s& a, const Packet4s& b) { return vmin_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pmin<Packet8s>(const Packet8s& a, const Packet8s& b) { return vminq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pmin<Packet4us>(const Packet4us& a, const Packet4us& b) { return vmin_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pmin<Packet8us>(const Packet8us& a, const Packet8us& b) { return vminq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pmin<Packet2i>(const Packet2i& a, const Packet2i& b) { return vmin_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b) { return vminq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pmin<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vmin_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pmin<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vminq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pmin<Packet2l>(const Packet2l& a, const Packet2l& b) {
-  return vcombine_s64(
-      vdup_n_s64((std::min)(vgetq_lane_s64(a, 0), vgetq_lane_s64(b, 0))),
-      vdup_n_s64((std::min)(vgetq_lane_s64(a, 1), vgetq_lane_s64(b, 1))));
-}
-template<> EIGEN_STRONG_INLINE Packet2ul pmin<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
-  return vcombine_u64(
-      vdup_n_u64((std::min)(vgetq_lane_u64(a, 0), vgetq_lane_u64(b, 0))),
-      vdup_n_u64((std::min)(vgetq_lane_u64(a, 1), vgetq_lane_u64(b, 1))));
+template <>
+EIGEN_STRONG_INLINE Packet4f pmin<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return pmin<Packet4f>(a, b);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pmax<Packet2f>(const Packet2f& a, const Packet2f& b) { return vmax_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) { return vmaxq_f32(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pmin<PropagateNaN, Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return pmin<Packet2f>(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4c pmin<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_s8(vmin_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pmin<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vmin_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pmin<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vminq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pmin<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vmin_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pmin<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vmin_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pmin<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vminq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pmin<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vmin_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pmin<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vminq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pmin<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vmin_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pmin<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vminq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pmin<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vmin_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vminq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pmin<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vmin_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmin<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vminq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pmin<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_s64(vdup_n_s64((std::min)(vgetq_lane_s64(a, 0), vgetq_lane_s64(b, 0))),
+                      vdup_n_s64((std::min)(vgetq_lane_s64(a, 1), vgetq_lane_s64(b, 1))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pmin<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vcombine_u64(vdup_n_u64((std::min)(vgetq_lane_u64(a, 0), vgetq_lane_u64(b, 0))),
+                      vdup_n_u64((std::min)(vgetq_lane_u64(a, 1), vgetq_lane_u64(b, 1))));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f pmax<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vmax_f32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vmaxq_f32(a, b);
+}
 
 #ifdef __ARM_FEATURE_NUMERIC_MAXMIN
-// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
-template<> EIGEN_STRONG_INLINE Packet4f pmax<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) { return vmaxnmq_f32(a, b); }
-template<> EIGEN_STRONG_INLINE Packet2f pmax<PropagateNumbers, Packet2f>(const Packet2f& a, const Packet2f& b) { return vmaxnm_f32(a, b); }
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8
+// systems).
+template <>
+EIGEN_STRONG_INLINE Packet4f pmax<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vmaxnmq_f32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pmax<PropagateNumbers, Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vmaxnm_f32(a, b);
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4f pmax<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) { return pmax<Packet4f>(a, b); }
-
-template<> EIGEN_STRONG_INLINE Packet2f pmax<PropagateNaN, Packet2f>(const Packet2f& a, const Packet2f& b) { return pmax<Packet2f>(a, b); }
-
-template<> EIGEN_STRONG_INLINE Packet4c pmax<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vmax_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
-}
-template<> EIGEN_STRONG_INLINE Packet8c pmax<Packet8c>(const Packet8c& a, const Packet8c& b) { return vmax_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pmax<Packet16c>(const Packet16c& a, const Packet16c& b) { return vmaxq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pmax<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vmax_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
-}
-template<> EIGEN_STRONG_INLINE Packet8uc pmax<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vmax_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pmax<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vmaxq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pmax<Packet4s>(const Packet4s& a, const Packet4s& b) { return vmax_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pmax<Packet8s>(const Packet8s& a, const Packet8s& b) { return vmaxq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pmax<Packet4us>(const Packet4us& a, const Packet4us& b) { return vmax_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pmax<Packet8us>(const Packet8us& a, const Packet8us& b) { return vmaxq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pmax<Packet2i>(const Packet2i& a, const Packet2i& b) { return vmax_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b) { return vmaxq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pmax<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vmax_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pmax<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vmaxq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pmax<Packet2l>(const Packet2l& a, const Packet2l& b) {
-  return vcombine_s64(
-      vdup_n_s64((std::max)(vgetq_lane_s64(a, 0), vgetq_lane_s64(b, 0))),
-      vdup_n_s64((std::max)(vgetq_lane_s64(a, 1), vgetq_lane_s64(b, 1))));
-}
-template<> EIGEN_STRONG_INLINE Packet2ul pmax<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
-  return vcombine_u64(
-      vdup_n_u64((std::max)(vgetq_lane_u64(a, 0), vgetq_lane_u64(b, 0))),
-      vdup_n_u64((std::max)(vgetq_lane_u64(a, 1), vgetq_lane_u64(b, 1))));
+template <>
+EIGEN_STRONG_INLINE Packet4f pmax<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return pmax<Packet4f>(a, b);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pcmp_le<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(vcle_f32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_le<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(vcleq_f32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4c pcmp_le<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_u8(vcle_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet2f pmax<PropagateNaN, Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return pmax<Packet2f>(a, b);
 }
-template<> EIGEN_STRONG_INLINE Packet8c pcmp_le<Packet8c>(const Packet8c& a, const Packet8c& b)
-{ return vreinterpret_s8_u8(vcle_s8(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet16c pcmp_le<Packet16c>(const Packet16c& a, const Packet16c& b)
-{ return vreinterpretq_s8_u8(vcleq_s8(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4uc pcmp_le<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vcle_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+
+template <>
+EIGEN_STRONG_INLINE Packet4c pmax<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_s8(vmax_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pcmp_le<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return vcle_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pcmp_le<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return vcleq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pcmp_le<Packet4s>(const Packet4s& a, const Packet4s& b)
-{ return vreinterpret_s16_u16(vcle_s16(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet8s pcmp_le<Packet8s>(const Packet8s& a, const Packet8s& b)
-{ return vreinterpretq_s16_u16(vcleq_s16(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4us pcmp_le<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return vcle_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pcmp_le<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return vcleq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pcmp_le<Packet2i>(const Packet2i& a, const Packet2i& b)
-{ return vreinterpret_s32_u32(vcle_s32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4i pcmp_le<Packet4i>(const Packet4i& a, const Packet4i& b)
-{ return vreinterpretq_s32_u32(vcleq_s32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet2ui pcmp_le<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return vcle_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pcmp_le<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return vcleq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pcmp_le<Packet2l>(const Packet2l& a, const Packet2l& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8c pmax<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vmax_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pmax<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vmaxq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pmax<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vmax_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pmax<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vmax_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pmax<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vmaxq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pmax<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vmax_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pmax<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vmaxq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pmax<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vmax_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pmax<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vmaxq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pmax<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vmax_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vmaxq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pmax<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vmax_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmax<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vmaxq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pmax<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_s64(vdup_n_s64((std::max)(vgetq_lane_s64(a, 0), vgetq_lane_s64(b, 0))),
+                      vdup_n_s64((std::max)(vgetq_lane_s64(a, 1), vgetq_lane_s64(b, 1))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pmax<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vcombine_u64(vdup_n_u64((std::max)(vgetq_lane_u64(a, 0), vgetq_lane_u64(b, 0))),
+                      vdup_n_u64((std::max)(vgetq_lane_u64(a, 1), vgetq_lane_u64(b, 1))));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f pcmp_le<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(vcle_f32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_le<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(vcleq_f32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pcmp_le<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_u8(vcle_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcmp_le<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vreinterpret_s8_u8(vcle_s8(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pcmp_le<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vreinterpretq_s8_u8(vcleq_s8(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcmp_le<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vcle_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcmp_le<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vcle_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcmp_le<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vcleq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcmp_le<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vreinterpret_s16_u16(vcle_s16(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pcmp_le<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vreinterpretq_s16_u16(vcleq_s16(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcmp_le<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vcle_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pcmp_le<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vcleq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcmp_le<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vreinterpret_s32_u32(vcle_s32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcmp_le<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vreinterpretq_s32_u32(vcleq_s32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcmp_le<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vcle_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcmp_le<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vcleq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pcmp_le<Packet2l>(const Packet2l& a, const Packet2l& b) {
 #if EIGEN_ARCH_ARM64
-  return vreinterpretq_s64_u64(vcleq_s64(a,b));
+  return vreinterpretq_s64_u64(vcleq_s64(a, b));
 #else
-  return vcombine_s64(
-      vdup_n_s64(vgetq_lane_s64(a, 0) <= vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
-      vdup_n_s64(vgetq_lane_s64(a, 1) <= vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
+  return vcombine_s64(vdup_n_s64(vgetq_lane_s64(a, 0) <= vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
+                      vdup_n_s64(vgetq_lane_s64(a, 1) <= vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet2ul pcmp_le<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcmp_le<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
 #if EIGEN_ARCH_ARM64
-  return vcleq_u64(a,b);
+  return vcleq_u64(a, b);
 #else
-  return vcombine_u64(
-      vdup_n_u64(vgetq_lane_u64(a, 0) <= vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
-      vdup_n_u64(vgetq_lane_u64(a, 1) <= vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
+  return vcombine_u64(vdup_n_u64(vgetq_lane_u64(a, 0) <= vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
+                      vdup_n_u64(vgetq_lane_u64(a, 1) <= vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pcmp_lt<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(vclt_f32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(vcltq_f32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4c pcmp_lt<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_u8(vclt_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet2f pcmp_lt<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(vclt_f32(a, b));
 }
-template<> EIGEN_STRONG_INLINE Packet8c pcmp_lt<Packet8c>(const Packet8c& a, const Packet8c& b)
-{ return vreinterpret_s8_u8(vclt_s8(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet16c pcmp_lt<Packet16c>(const Packet16c& a, const Packet16c& b)
-{ return vreinterpretq_s8_u8(vcltq_s8(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4uc pcmp_lt<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vclt_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_lt<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(vcltq_f32(a, b));
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pcmp_lt<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return vclt_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pcmp_lt<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return vcltq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pcmp_lt<Packet4s>(const Packet4s& a, const Packet4s& b)
-{ return vreinterpret_s16_u16(vclt_s16(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet8s pcmp_lt<Packet8s>(const Packet8s& a, const Packet8s& b)
-{ return vreinterpretq_s16_u16(vcltq_s16(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4us pcmp_lt<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return vclt_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pcmp_lt<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return vcltq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pcmp_lt<Packet2i>(const Packet2i& a, const Packet2i& b)
-{ return vreinterpret_s32_u32(vclt_s32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4i pcmp_lt<Packet4i>(const Packet4i& a, const Packet4i& b)
-{ return vreinterpretq_s32_u32(vcltq_s32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet2ui pcmp_lt<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return vclt_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pcmp_lt<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return vcltq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pcmp_lt<Packet2l>(const Packet2l& a, const Packet2l& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4c pcmp_lt<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_u8(vclt_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcmp_lt<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vreinterpret_s8_u8(vclt_s8(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pcmp_lt<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vreinterpretq_s8_u8(vcltq_s8(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcmp_lt<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vclt_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcmp_lt<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vclt_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcmp_lt<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vcltq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcmp_lt<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vreinterpret_s16_u16(vclt_s16(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pcmp_lt<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vreinterpretq_s16_u16(vcltq_s16(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcmp_lt<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vclt_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pcmp_lt<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vcltq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcmp_lt<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vreinterpret_s32_u32(vclt_s32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcmp_lt<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vreinterpretq_s32_u32(vcltq_s32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcmp_lt<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vclt_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcmp_lt<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vcltq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pcmp_lt<Packet2l>(const Packet2l& a, const Packet2l& b) {
 #if EIGEN_ARCH_ARM64
-  return vreinterpretq_s64_u64(vcltq_s64(a,b));
+  return vreinterpretq_s64_u64(vcltq_s64(a, b));
 #else
-  return vcombine_s64(
-      vdup_n_s64(vgetq_lane_s64(a, 0) < vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
-      vdup_n_s64(vgetq_lane_s64(a, 1) < vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
+  return vcombine_s64(vdup_n_s64(vgetq_lane_s64(a, 0) < vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
+                      vdup_n_s64(vgetq_lane_s64(a, 1) < vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet2ul pcmp_lt<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcmp_lt<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
 #if EIGEN_ARCH_ARM64
-  return vcltq_u64(a,b);
+  return vcltq_u64(a, b);
 #else
-  return vcombine_u64(
-      vdup_n_u64(vgetq_lane_u64(a, 0) < vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
-      vdup_n_u64(vgetq_lane_u64(a, 1) < vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
+  return vcombine_u64(vdup_n_u64(vgetq_lane_u64(a, 0) < vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
+                      vdup_n_u64(vgetq_lane_u64(a, 1) < vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pcmp_eq<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(vceq_f32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_eq<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(vceqq_f32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4c pcmp_eq<Packet4c>(const Packet4c& a, const Packet4c& b)
-{
-  return vget_lane_s32(vreinterpret_s32_u8(vceq_s8(
-      vreinterpret_s8_s32(vdup_n_s32(a)),
-      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet2f pcmp_eq<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(vceq_f32(a, b));
 }
-template<> EIGEN_STRONG_INLINE Packet8c pcmp_eq<Packet8c>(const Packet8c& a, const Packet8c& b)
-{ return vreinterpret_s8_u8(vceq_s8(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet16c pcmp_eq<Packet16c>(const Packet16c& a, const Packet16c& b)
-{ return vreinterpretq_s8_u8(vceqq_s8(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4uc pcmp_eq<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vceq_u8(
-      vreinterpret_u8_u32(vdup_n_u32(a)),
-      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_eq<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(vceqq_f32(a, b));
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pcmp_eq<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return vceq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pcmp_eq<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return vceqq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pcmp_eq<Packet4s>(const Packet4s& a, const Packet4s& b)
-{ return vreinterpret_s16_u16(vceq_s16(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet8s pcmp_eq<Packet8s>(const Packet8s& a, const Packet8s& b)
-{ return vreinterpretq_s16_u16(vceqq_s16(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4us pcmp_eq<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return vceq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pcmp_eq<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return vceqq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pcmp_eq<Packet2i>(const Packet2i& a, const Packet2i& b)
-{ return vreinterpret_s32_u32(vceq_s32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet4i pcmp_eq<Packet4i>(const Packet4i& a, const Packet4i& b)
-{ return vreinterpretq_s32_u32(vceqq_s32(a,b)); }
-template<> EIGEN_STRONG_INLINE Packet2ui pcmp_eq<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return vceq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pcmp_eq<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return vceqq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pcmp_eq<Packet2l>(const Packet2l& a, const Packet2l& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4c pcmp_eq<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return vget_lane_s32(
+      vreinterpret_s32_u8(vceq_s8(vreinterpret_s8_s32(vdup_n_s32(a)), vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcmp_eq<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vreinterpret_s8_u8(vceq_s8(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pcmp_eq<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vreinterpretq_s8_u8(vceqq_s8(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcmp_eq<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return vget_lane_u32(
+      vreinterpret_u32_u8(vceq_u8(vreinterpret_u8_u32(vdup_n_u32(a)), vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcmp_eq<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vceq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcmp_eq<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vceqq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcmp_eq<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vreinterpret_s16_u16(vceq_s16(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pcmp_eq<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vreinterpretq_s16_u16(vceqq_s16(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcmp_eq<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vceq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pcmp_eq<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vceqq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcmp_eq<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vreinterpret_s32_u32(vceq_s32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcmp_eq<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vreinterpretq_s32_u32(vceqq_s32(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcmp_eq<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vceq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcmp_eq<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vceqq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pcmp_eq<Packet2l>(const Packet2l& a, const Packet2l& b) {
 #if EIGEN_ARCH_ARM64
-  return vreinterpretq_s64_u64(vceqq_s64(a,b));
+  return vreinterpretq_s64_u64(vceqq_s64(a, b));
 #else
-  return vcombine_s64(
-      vdup_n_s64(vgetq_lane_s64(a, 0) == vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
-      vdup_n_s64(vgetq_lane_s64(a, 1) == vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
+  return vcombine_s64(vdup_n_s64(vgetq_lane_s64(a, 0) == vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
+                      vdup_n_s64(vgetq_lane_s64(a, 1) == vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet2ul pcmp_eq<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcmp_eq<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
 #if EIGEN_ARCH_ARM64
-  return vceqq_u64(a,b);
+  return vceqq_u64(a, b);
 #else
-  return vcombine_u64(
-      vdup_n_u64(vgetq_lane_u64(a, 0) == vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
-      vdup_n_u64(vgetq_lane_u64(a, 1) == vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
+  return vcombine_u64(vdup_n_u64(vgetq_lane_u64(a, 0) == vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
+                      vdup_n_u64(vgetq_lane_u64(a, 1) == vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pcmp_lt_or_nan<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(vmvn_u32(vcge_f32(a,b))); }
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt_or_nan<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(vmvnq_u32(vcgeq_f32(a,b))); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pcmp_lt_or_nan<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(vmvn_u32(vcge_f32(a, b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_lt_or_nan<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(vmvnq_u32(vcgeq_f32(a, b)));
+}
 
 // Logical Operations are not supported for float, so we have to reinterpret casts using NEON intrinsics
-template<> EIGEN_STRONG_INLINE Packet2f pand<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(vand_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4c pand<Packet4c>(const Packet4c& a, const Packet4c& b)
-{ return a & b; }
-template<> EIGEN_STRONG_INLINE Packet8c pand<Packet8c>(const Packet8c& a, const Packet8c& b)
-{ return vand_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pand<Packet16c>(const Packet16c& a, const Packet16c& b)
-{ return vandq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pand<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{ return a & b; }
-template<> EIGEN_STRONG_INLINE Packet8uc pand<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return vand_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pand<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return vandq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pand<Packet4s>(const Packet4s& a, const Packet4s& b) { return vand_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pand<Packet8s>(const Packet8s& a, const Packet8s& b) { return vandq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pand<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return vand_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pand<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return vandq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pand<Packet2i>(const Packet2i& a, const Packet2i& b) { return vand_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) { return vandq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pand<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return vand_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pand<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return vandq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pand<Packet2l>(const Packet2l& a, const Packet2l& b) { return vandq_s64(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ul pand<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
-{ return vandq_u64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pand<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(vand_u32(vreinterpret_u32_f32(a), vreinterpret_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a), vreinterpretq_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pand<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return a & b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pand<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vand_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pand<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vandq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pand<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return a & b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pand<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vand_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pand<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vandq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pand<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vand_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pand<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vandq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pand<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vand_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pand<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vandq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pand<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vand_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vandq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pand<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vand_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pand<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vandq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pand<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vandq_s64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pand<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vandq_u64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f por<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(vorr_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4c por<Packet4c>(const Packet4c& a, const Packet4c& b)
-{ return a | b; }
-template<> EIGEN_STRONG_INLINE Packet8c por<Packet8c>(const Packet8c& a, const Packet8c& b) { return vorr_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c por<Packet16c>(const Packet16c& a, const Packet16c& b)
-{ return vorrq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc por<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{ return a | b; }
-template<> EIGEN_STRONG_INLINE Packet8uc por<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return vorr_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc por<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return vorrq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s por<Packet4s>(const Packet4s& a, const Packet4s& b)
-{ return vorr_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s por<Packet8s>(const Packet8s& a, const Packet8s& b)
-{ return vorrq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us por<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return vorr_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us por<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return vorrq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i por<Packet2i>(const Packet2i& a, const Packet2i& b) { return vorr_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) { return vorrq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui por<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return vorr_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui por<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return vorrq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l por<Packet2l>(const Packet2l& a, const Packet2l& b)
-{ return vorrq_s64(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ul por<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
-{ return vorrq_u64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2f por<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(vorr_u32(vreinterpret_u32_f32(a), vreinterpret_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a), vreinterpretq_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c por<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return a | b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c por<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vorr_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c por<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vorrq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc por<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return a | b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc por<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vorr_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc por<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vorrq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s por<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vorr_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s por<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vorrq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us por<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vorr_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us por<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vorrq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i por<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vorr_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vorrq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui por<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vorr_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui por<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vorrq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l por<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vorrq_s64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul por<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vorrq_u64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pxor<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4c pxor<Packet4c>(const Packet4c& a, const Packet4c& b)
-{ return a ^ b; }
-template<> EIGEN_STRONG_INLINE Packet8c pxor<Packet8c>(const Packet8c& a, const Packet8c& b)
-{ return veor_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pxor<Packet16c>(const Packet16c& a, const Packet16c& b)
-{ return veorq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pxor<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{ return a ^ b; }
-template<> EIGEN_STRONG_INLINE Packet8uc pxor<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return veor_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pxor<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return veorq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pxor<Packet4s>(const Packet4s& a, const Packet4s& b) { return veor_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pxor<Packet8s>(const Packet8s& a, const Packet8s& b) { return veorq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pxor<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return veor_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pxor<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return veorq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pxor<Packet2i>(const Packet2i& a, const Packet2i& b) { return veor_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) { return veorq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pxor<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return veor_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pxor<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return veorq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pxor<Packet2l>(const Packet2l& a, const Packet2l& b)
-{ return veorq_s64(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ul pxor<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
-{ return veorq_u64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pxor<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(a), vreinterpret_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a), vreinterpretq_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pxor<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return a ^ b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pxor<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return veor_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pxor<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return veorq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pxor<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return a ^ b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pxor<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return veor_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pxor<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return veorq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pxor<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return veor_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pxor<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return veorq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pxor<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return veor_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pxor<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return veorq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pxor<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return veor_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return veorq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pxor<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return veor_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pxor<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return veorq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pxor<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return veorq_s64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pxor<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return veorq_u64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pandnot<Packet2f>(const Packet2f& a, const Packet2f& b)
-{ return vreinterpret_f32_u32(vbic_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b)
-{ return vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
-template<> EIGEN_STRONG_INLINE Packet4c pandnot<Packet4c>(const Packet4c& a, const Packet4c& b)
-{ return a & ~b; }
-template<> EIGEN_STRONG_INLINE Packet8c pandnot<Packet8c>(const Packet8c& a, const Packet8c& b) { return vbic_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16c pandnot<Packet16c>(const Packet16c& a, const Packet16c& b) { return vbicq_s8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4uc pandnot<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
-{ return a & ~b; }
-template<> EIGEN_STRONG_INLINE Packet8uc pandnot<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
-{ return vbic_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16uc pandnot<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
-{ return vbicq_u8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4s pandnot<Packet4s>(const Packet4s& a, const Packet4s& b)
-{ return vbic_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8s pandnot<Packet8s>(const Packet8s& a, const Packet8s& b)
-{ return vbicq_s16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4us pandnot<Packet4us>(const Packet4us& a, const Packet4us& b)
-{ return vbic_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet8us pandnot<Packet8us>(const Packet8us& a, const Packet8us& b)
-{ return vbicq_u16(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2i pandnot<Packet2i>(const Packet2i& a, const Packet2i& b)
-{ return vbic_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b)
-{ return vbicq_s32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ui pandnot<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
-{ return vbic_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4ui pandnot<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
-{ return vbicq_u32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2l pandnot<Packet2l>(const Packet2l& a, const Packet2l& b)
-{ return vbicq_s64(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2ul pandnot<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
-{ return vbicq_u64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pandnot<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return vreinterpret_f32_u32(vbic_u32(vreinterpret_u32_f32(a), vreinterpret_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a), vreinterpretq_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pandnot<Packet4c>(const Packet4c& a, const Packet4c& b) {
+  return a & ~b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pandnot<Packet8c>(const Packet8c& a, const Packet8c& b) {
+  return vbic_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pandnot<Packet16c>(const Packet16c& a, const Packet16c& b) {
+  return vbicq_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pandnot<Packet4uc>(const Packet4uc& a, const Packet4uc& b) {
+  return a & ~b;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pandnot<Packet8uc>(const Packet8uc& a, const Packet8uc& b) {
+  return vbic_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pandnot<Packet16uc>(const Packet16uc& a, const Packet16uc& b) {
+  return vbicq_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pandnot<Packet4s>(const Packet4s& a, const Packet4s& b) {
+  return vbic_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pandnot<Packet8s>(const Packet8s& a, const Packet8s& b) {
+  return vbicq_s16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pandnot<Packet4us>(const Packet4us& a, const Packet4us& b) {
+  return vbic_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pandnot<Packet8us>(const Packet8us& a, const Packet8us& b) {
+  return vbicq_u16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pandnot<Packet2i>(const Packet2i& a, const Packet2i& b) {
+  return vbic_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return vbicq_s32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pandnot<Packet2ui>(const Packet2ui& a, const Packet2ui& b) {
+  return vbic_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pandnot<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return vbicq_u32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pandnot<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vbicq_s64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pandnot<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vbicq_u64(a, b);
+}
 
+template <int N>
+EIGEN_STRONG_INLINE Packet4c parithmetic_shift_right(Packet4c& a) {
+  return vget_lane_s32(vreinterpret_s32_s8(vshr_n_s8(vreinterpret_s8_s32(vdup_n_s32(a)), N)), 0);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8c parithmetic_shift_right(Packet8c a) {
+  return vshr_n_s8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet16c parithmetic_shift_right(Packet16c a) {
+  return vshrq_n_s8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4uc parithmetic_shift_right(Packet4uc& a) {
+  return vget_lane_u32(vreinterpret_u32_u8(vshr_n_u8(vreinterpret_u8_u32(vdup_n_u32(a)), N)), 0);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8uc parithmetic_shift_right(Packet8uc a) {
+  return vshr_n_u8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet16uc parithmetic_shift_right(Packet16uc a) {
+  return vshrq_n_u8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4s parithmetic_shift_right(Packet4s a) {
+  return vshr_n_s16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8s parithmetic_shift_right(Packet8s a) {
+  return vshrq_n_s16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4us parithmetic_shift_right(Packet4us a) {
+  return vshr_n_u16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8us parithmetic_shift_right(Packet8us a) {
+  return vshrq_n_u16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2i parithmetic_shift_right(Packet2i a) {
+  return vshr_n_s32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4i parithmetic_shift_right(Packet4i a) {
+  return vshrq_n_s32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2ui parithmetic_shift_right(Packet2ui a) {
+  return vshr_n_u32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4ui parithmetic_shift_right(Packet4ui a) {
+  return vshrq_n_u32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2l parithmetic_shift_right(Packet2l a) {
+  return vshrq_n_s64(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2ul parithmetic_shift_right(Packet2ul a) {
+  return vshrq_n_u64(a, N);
+}
 
-template<int N> EIGEN_STRONG_INLINE Packet4c parithmetic_shift_right(Packet4c& a)
-{ return vget_lane_s32(vreinterpret_s32_s8(vshr_n_s8(vreinterpret_s8_s32(vdup_n_s32(a)), N)), 0); }
-template<int N> EIGEN_STRONG_INLINE Packet8c parithmetic_shift_right(Packet8c a) { return vshr_n_s8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet16c parithmetic_shift_right(Packet16c a) { return vshrq_n_s8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4uc parithmetic_shift_right(Packet4uc& a)
-{ return vget_lane_u32(vreinterpret_u32_u8(vshr_n_u8(vreinterpret_u8_u32(vdup_n_u32(a)), N)), 0); }
-template<int N> EIGEN_STRONG_INLINE Packet8uc parithmetic_shift_right(Packet8uc a) { return vshr_n_u8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet16uc parithmetic_shift_right(Packet16uc a) { return vshrq_n_u8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4s parithmetic_shift_right(Packet4s a) { return vshr_n_s16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet8s parithmetic_shift_right(Packet8s a) { return vshrq_n_s16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4us parithmetic_shift_right(Packet4us a) { return vshr_n_u16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet8us parithmetic_shift_right(Packet8us a) { return vshrq_n_u16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2i parithmetic_shift_right(Packet2i a) { return vshr_n_s32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4i parithmetic_shift_right(Packet4i a) { return vshrq_n_s32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2ui parithmetic_shift_right(Packet2ui a) { return vshr_n_u32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4ui parithmetic_shift_right(Packet4ui a) { return vshrq_n_u32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2l parithmetic_shift_right(Packet2l a) { return vshrq_n_s64(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2ul parithmetic_shift_right(Packet2ul a) { return vshrq_n_u64(a,N); }
+template <int N>
+EIGEN_STRONG_INLINE Packet4c plogical_shift_right(Packet4c& a) {
+  return vget_lane_s32(vreinterpret_s32_u8(vshr_n_u8(vreinterpret_u8_s32(vdup_n_s32(a)), N)), 0);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8c plogical_shift_right(Packet8c a) {
+  return vreinterpret_s8_u8(vshr_n_u8(vreinterpret_u8_s8(a), N));
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet16c plogical_shift_right(Packet16c a) {
+  return vreinterpretq_s8_u8(vshrq_n_u8(vreinterpretq_u8_s8(a), N));
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4uc plogical_shift_right(Packet4uc& a) {
+  return vget_lane_u32(vreinterpret_u32_s8(vshr_n_s8(vreinterpret_s8_u32(vdup_n_u32(a)), N)), 0);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8uc plogical_shift_right(Packet8uc a) {
+  return vshr_n_u8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet16uc plogical_shift_right(Packet16uc a) {
+  return vshrq_n_u8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4s plogical_shift_right(Packet4s a) {
+  return vreinterpret_s16_u16(vshr_n_u16(vreinterpret_u16_s16(a), N));
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8s plogical_shift_right(Packet8s a) {
+  return vreinterpretq_s16_u16(vshrq_n_u16(vreinterpretq_u16_s16(a), N));
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4us plogical_shift_right(Packet4us a) {
+  return vshr_n_u16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8us plogical_shift_right(Packet8us a) {
+  return vshrq_n_u16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2i plogical_shift_right(Packet2i a) {
+  return vreinterpret_s32_u32(vshr_n_u32(vreinterpret_u32_s32(a), N));
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4i plogical_shift_right(Packet4i a) {
+  return vreinterpretq_s32_u32(vshrq_n_u32(vreinterpretq_u32_s32(a), N));
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2ui plogical_shift_right(Packet2ui a) {
+  return vshr_n_u32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4ui plogical_shift_right(Packet4ui a) {
+  return vshrq_n_u32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2l plogical_shift_right(Packet2l a) {
+  return vreinterpretq_s64_u64(vshrq_n_u64(vreinterpretq_u64_s64(a), N));
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2ul plogical_shift_right(Packet2ul a) {
+  return vshrq_n_u64(a, N);
+}
 
-template<int N> EIGEN_STRONG_INLINE Packet4c plogical_shift_right(Packet4c& a)
-{ return vget_lane_s32(vreinterpret_s32_u8(vshr_n_u8(vreinterpret_u8_s32(vdup_n_s32(a)), N)), 0); }
-template<int N> EIGEN_STRONG_INLINE Packet8c plogical_shift_right(Packet8c a)
-{ return vreinterpret_s8_u8(vshr_n_u8(vreinterpret_u8_s8(a),N)); }
-template<int N> EIGEN_STRONG_INLINE Packet16c plogical_shift_right(Packet16c a)
-{ return vreinterpretq_s8_u8(vshrq_n_u8(vreinterpretq_u8_s8(a),N)); }
-template<int N> EIGEN_STRONG_INLINE Packet4uc plogical_shift_right(Packet4uc& a)
-{ return vget_lane_u32(vreinterpret_u32_s8(vshr_n_s8(vreinterpret_s8_u32(vdup_n_u32(a)), N)), 0); }
-template<int N> EIGEN_STRONG_INLINE Packet8uc plogical_shift_right(Packet8uc a) { return vshr_n_u8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet16uc plogical_shift_right(Packet16uc a) { return vshrq_n_u8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4s plogical_shift_right(Packet4s a)
-{ return vreinterpret_s16_u16(vshr_n_u16(vreinterpret_u16_s16(a),N)); }
-template<int N> EIGEN_STRONG_INLINE Packet8s plogical_shift_right(Packet8s a)
-{ return vreinterpretq_s16_u16(vshrq_n_u16(vreinterpretq_u16_s16(a),N)); }
-template<int N> EIGEN_STRONG_INLINE Packet4us plogical_shift_right(Packet4us a) { return vshr_n_u16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet8us plogical_shift_right(Packet8us a) { return vshrq_n_u16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2i plogical_shift_right(Packet2i a)
-{ return vreinterpret_s32_u32(vshr_n_u32(vreinterpret_u32_s32(a),N)); }
-template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_right(Packet4i a)
-{ return vreinterpretq_s32_u32(vshrq_n_u32(vreinterpretq_u32_s32(a),N)); }
-template<int N> EIGEN_STRONG_INLINE Packet2ui plogical_shift_right(Packet2ui a) { return vshr_n_u32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4ui plogical_shift_right(Packet4ui a) { return vshrq_n_u32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2l plogical_shift_right(Packet2l a)
-{ return vreinterpretq_s64_u64(vshrq_n_u64(vreinterpretq_u64_s64(a),N)); }
-template<int N> EIGEN_STRONG_INLINE Packet2ul plogical_shift_right(Packet2ul a) { return vshrq_n_u64(a,N); }
+template <int N>
+EIGEN_STRONG_INLINE Packet4c plogical_shift_left(Packet4c& a) {
+  return vget_lane_s32(vreinterpret_s32_s8(vshl_n_s8(vreinterpret_s8_s32(vdup_n_s32(a)), N)), 0);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8c plogical_shift_left(Packet8c a) {
+  return vshl_n_s8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet16c plogical_shift_left(Packet16c a) {
+  return vshlq_n_s8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4uc plogical_shift_left(Packet4uc& a) {
+  return vget_lane_u32(vreinterpret_u32_u8(vshl_n_u8(vreinterpret_u8_u32(vdup_n_u32(a)), N)), 0);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8uc plogical_shift_left(Packet8uc a) {
+  return vshl_n_u8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet16uc plogical_shift_left(Packet16uc a) {
+  return vshlq_n_u8(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4s plogical_shift_left(Packet4s a) {
+  return vshl_n_s16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8s plogical_shift_left(Packet8s a) {
+  return vshlq_n_s16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4us plogical_shift_left(Packet4us a) {
+  return vshl_n_u16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet8us plogical_shift_left(Packet8us a) {
+  return vshlq_n_u16(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2i plogical_shift_left(Packet2i a) {
+  return vshl_n_s32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4i plogical_shift_left(Packet4i a) {
+  return vshlq_n_s32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2ui plogical_shift_left(Packet2ui a) {
+  return vshl_n_u32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4ui plogical_shift_left(Packet4ui a) {
+  return vshlq_n_u32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2l plogical_shift_left(Packet2l a) {
+  return vshlq_n_s64(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet2ul plogical_shift_left(Packet2ul a) {
+  return vshlq_n_u64(a, N);
+}
 
-template<int N> EIGEN_STRONG_INLINE Packet4c plogical_shift_left(Packet4c& a)
-{ return vget_lane_s32(vreinterpret_s32_s8(vshl_n_s8(vreinterpret_s8_s32(vdup_n_s32(a)), N)), 0); }
-template<int N> EIGEN_STRONG_INLINE Packet8c plogical_shift_left(Packet8c a) { return vshl_n_s8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet16c plogical_shift_left(Packet16c a) { return vshlq_n_s8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4uc plogical_shift_left(Packet4uc& a)
-{ return vget_lane_u32(vreinterpret_u32_u8(vshl_n_u8(vreinterpret_u8_u32(vdup_n_u32(a)), N)), 0); }
-template<int N> EIGEN_STRONG_INLINE Packet8uc plogical_shift_left(Packet8uc a) { return vshl_n_u8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet16uc plogical_shift_left(Packet16uc a) { return vshlq_n_u8(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4s plogical_shift_left(Packet4s a) { return vshl_n_s16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet8s plogical_shift_left(Packet8s a) { return vshlq_n_s16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4us plogical_shift_left(Packet4us a) { return vshl_n_u16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet8us plogical_shift_left(Packet8us a) { return vshlq_n_u16(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2i plogical_shift_left(Packet2i a) { return vshl_n_s32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_left(Packet4i a) { return vshlq_n_s32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2ui plogical_shift_left(Packet2ui a) { return vshl_n_u32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4ui plogical_shift_left(Packet4ui a) { return vshlq_n_u32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2l plogical_shift_left(Packet2l a) { return vshlq_n_s64(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet2ul plogical_shift_left(Packet2ul a) { return vshlq_n_u64(a,N); }
-
-template<> EIGEN_STRONG_INLINE Packet2f pload<Packet2f>(const float* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4c pload<Packet4c>(const int8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2f pload<Packet2f>(const float* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pload<Packet4c>(const int8_t* from) {
   Packet4c res;
   memcpy(&res, from, sizeof(Packet4c));
   return res;
 }
-template<> EIGEN_STRONG_INLINE Packet8c pload<Packet8c>(const int8_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_s8(from); }
-template<> EIGEN_STRONG_INLINE Packet16c pload<Packet16c>(const int8_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s8(from); }
-template<> EIGEN_STRONG_INLINE Packet4uc pload<Packet4uc>(const uint8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8c pload<Packet8c>(const int8_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_s8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pload<Packet16c>(const int8_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pload<Packet4uc>(const uint8_t* from) {
   Packet4uc res;
   memcpy(&res, from, sizeof(Packet4uc));
   return res;
 }
-template<> EIGEN_STRONG_INLINE Packet8uc pload<Packet8uc>(const uint8_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_u8(from); }
-template<> EIGEN_STRONG_INLINE Packet16uc pload<Packet16uc>(const uint8_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u8(from); }
-template<> EIGEN_STRONG_INLINE Packet4s pload<Packet4s>(const int16_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_s16(from); }
-template<> EIGEN_STRONG_INLINE Packet8s pload<Packet8s>(const int16_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s16(from); }
-template<> EIGEN_STRONG_INLINE Packet4us pload<Packet4us>(const uint16_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_u16(from); }
-template<> EIGEN_STRONG_INLINE Packet8us pload<Packet8us>(const uint16_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u16(from); }
-template<> EIGEN_STRONG_INLINE Packet2i pload<Packet2i>(const int32_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int32_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet2ui pload<Packet2ui>(const uint32_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_u32(from); }
-template<> EIGEN_STRONG_INLINE Packet4ui pload<Packet4ui>(const uint32_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u32(from); }
-template<> EIGEN_STRONG_INLINE Packet2l pload<Packet2l>(const int64_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s64(from); }
-template<> EIGEN_STRONG_INLINE Packet2ul pload<Packet2ul>(const uint64_t* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u64(from); }
+template <>
+EIGEN_STRONG_INLINE Packet8uc pload<Packet8uc>(const uint8_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_u8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pload<Packet16uc>(const uint8_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pload<Packet4s>(const int16_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_s16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pload<Packet8s>(const int16_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pload<Packet4us>(const uint16_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_u16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pload<Packet8us>(const uint16_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pload<Packet2i>(const int32_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_s32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int32_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pload<Packet2ui>(const uint32_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_u32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pload<Packet4ui>(const uint32_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pload<Packet2l>(const int64_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s64(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pload<Packet2ul>(const uint64_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u64(from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f ploadu<Packet2f>(const float* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4c ploadu<Packet4c>(const int8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2f ploadu<Packet2f>(const float* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c ploadu<Packet4c>(const int8_t* from) {
   Packet4c res;
   memcpy(&res, from, sizeof(Packet4c));
   return res;
 }
-template<> EIGEN_STRONG_INLINE Packet8c ploadu<Packet8c>(const int8_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s8(from); }
-template<> EIGEN_STRONG_INLINE Packet16c ploadu<Packet16c>(const int8_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s8(from); }
-template<> EIGEN_STRONG_INLINE Packet4uc ploadu<Packet4uc>(const uint8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8c ploadu<Packet8c>(const int8_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c ploadu<Packet16c>(const int8_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc ploadu<Packet4uc>(const uint8_t* from) {
   Packet4uc res;
   memcpy(&res, from, sizeof(Packet4uc));
   return res;
 }
-template<> EIGEN_STRONG_INLINE Packet8uc ploadu<Packet8uc>(const uint8_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u8(from); }
-template<> EIGEN_STRONG_INLINE Packet16uc ploadu<Packet16uc>(const uint8_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u8(from); }
-template<> EIGEN_STRONG_INLINE Packet4s ploadu<Packet4s>(const int16_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s16(from); }
-template<> EIGEN_STRONG_INLINE Packet8s ploadu<Packet8s>(const int16_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s16(from); }
-template<> EIGEN_STRONG_INLINE Packet4us ploadu<Packet4us>(const uint16_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u16(from); }
-template<> EIGEN_STRONG_INLINE Packet8us ploadu<Packet8us>(const uint16_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u16(from); }
-template<> EIGEN_STRONG_INLINE Packet2i ploadu<Packet2i>(const int32_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int32_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet2ui ploadu<Packet2ui>(const uint32_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u32(from); }
-template<> EIGEN_STRONG_INLINE Packet4ui ploadu<Packet4ui>(const uint32_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u32(from); }
-template<> EIGEN_STRONG_INLINE Packet2l ploadu<Packet2l>(const int64_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s64(from); }
-template<> EIGEN_STRONG_INLINE Packet2ul ploadu<Packet2ul>(const uint64_t* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u64(from); }
+template <>
+EIGEN_STRONG_INLINE Packet8uc ploadu<Packet8uc>(const uint8_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc ploadu<Packet16uc>(const uint8_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u8(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s ploadu<Packet4s>(const int16_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s ploadu<Packet8s>(const int16_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us ploadu<Packet4us>(const uint16_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us ploadu<Packet8us>(const uint16_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u16(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i ploadu<Packet2i>(const int32_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int32_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui ploadu<Packet2ui>(const uint32_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui ploadu<Packet4ui>(const uint32_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l ploadu<Packet2l>(const int64_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s64(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul ploadu<Packet2ul>(const uint64_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u64(from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f ploaddup<Packet2f>(const float* from)
-{ return vld1_dup_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float* from)
-{ return vcombine_f32(vld1_dup_f32(from), vld1_dup_f32(from+1)); }
-template<> EIGEN_STRONG_INLINE Packet4c ploaddup<Packet4c>(const int8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2f ploaddup<Packet2f>(const float* from) {
+  return vld1_dup_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float* from) {
+  return vcombine_f32(vld1_dup_f32(from), vld1_dup_f32(from + 1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c ploaddup<Packet4c>(const int8_t* from) {
   const int8x8_t a = vreinterpret_s8_s32(vdup_n_s32(pload<Packet4c>(from)));
-  return vget_lane_s32(vreinterpret_s32_s8(vzip_s8(a,a).val[0]), 0);
+  return vget_lane_s32(vreinterpret_s32_s8(vzip_s8(a, a).val[0]), 0);
 }
-template<> EIGEN_STRONG_INLINE Packet8c ploaddup<Packet8c>(const int8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8c ploaddup<Packet8c>(const int8_t* from) {
   const int8x8_t a = vld1_s8(from);
-  return vzip_s8(a,a).val[0];
+  return vzip_s8(a, a).val[0];
 }
-template<> EIGEN_STRONG_INLINE Packet16c ploaddup<Packet16c>(const int8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet16c ploaddup<Packet16c>(const int8_t* from) {
   const int8x8_t a = vld1_s8(from);
-  const int8x8x2_t b = vzip_s8(a,a);
+  const int8x8x2_t b = vzip_s8(a, a);
   return vcombine_s8(b.val[0], b.val[1]);
 }
-template<> EIGEN_STRONG_INLINE Packet4uc ploaddup<Packet4uc>(const uint8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4uc ploaddup<Packet4uc>(const uint8_t* from) {
   const uint8x8_t a = vreinterpret_u8_u32(vdup_n_u32(pload<Packet4uc>(from)));
-  return vget_lane_u32(vreinterpret_u32_u8(vzip_u8(a,a).val[0]), 0);
+  return vget_lane_u32(vreinterpret_u32_u8(vzip_u8(a, a).val[0]), 0);
 }
-template<> EIGEN_STRONG_INLINE Packet8uc ploaddup<Packet8uc>(const uint8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8uc ploaddup<Packet8uc>(const uint8_t* from) {
   const uint8x8_t a = vld1_u8(from);
-  return vzip_u8(a,a).val[0];
+  return vzip_u8(a, a).val[0];
 }
-template<> EIGEN_STRONG_INLINE Packet16uc ploaddup<Packet16uc>(const uint8_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet16uc ploaddup<Packet16uc>(const uint8_t* from) {
   const uint8x8_t a = vld1_u8(from);
-  const uint8x8x2_t b = vzip_u8(a,a);
+  const uint8x8x2_t b = vzip_u8(a, a);
   return vcombine_u8(b.val[0], b.val[1]);
 }
-template<> EIGEN_STRONG_INLINE Packet4s ploaddup<Packet4s>(const int16_t* from)
-{
-  return vreinterpret_s16_u32(vzip_u32(vreinterpret_u32_s16(vld1_dup_s16(from)),
-      vreinterpret_u32_s16(vld1_dup_s16(from+1))).val[0]);
+template <>
+EIGEN_STRONG_INLINE Packet4s ploaddup<Packet4s>(const int16_t* from) {
+  return vreinterpret_s16_u32(
+      vzip_u32(vreinterpret_u32_s16(vld1_dup_s16(from)), vreinterpret_u32_s16(vld1_dup_s16(from + 1))).val[0]);
 }
-template<> EIGEN_STRONG_INLINE Packet8s ploaddup<Packet8s>(const int16_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8s ploaddup<Packet8s>(const int16_t* from) {
   const int16x4_t a = vld1_s16(from);
-  const int16x4x2_t b = vzip_s16(a,a);
+  const int16x4x2_t b = vzip_s16(a, a);
   return vcombine_s16(b.val[0], b.val[1]);
 }
-template<> EIGEN_STRONG_INLINE Packet4us ploaddup<Packet4us>(const uint16_t* from)
-{
-  return vreinterpret_u16_u32(vzip_u32(vreinterpret_u32_u16(vld1_dup_u16(from)),
-      vreinterpret_u32_u16(vld1_dup_u16(from+1))).val[0]);
+template <>
+EIGEN_STRONG_INLINE Packet4us ploaddup<Packet4us>(const uint16_t* from) {
+  return vreinterpret_u16_u32(
+      vzip_u32(vreinterpret_u32_u16(vld1_dup_u16(from)), vreinterpret_u32_u16(vld1_dup_u16(from + 1))).val[0]);
 }
-template<> EIGEN_STRONG_INLINE Packet8us ploaddup<Packet8us>(const uint16_t* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet8us ploaddup<Packet8us>(const uint16_t* from) {
   const uint16x4_t a = vld1_u16(from);
-  const uint16x4x2_t b = vzip_u16(a,a);
+  const uint16x4x2_t b = vzip_u16(a, a);
   return vcombine_u16(b.val[0], b.val[1]);
 }
-template<> EIGEN_STRONG_INLINE Packet2i ploaddup<Packet2i>(const int32_t* from)
-{ return vld1_dup_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet4i ploaddup<Packet4i>(const int32_t* from)
-{ return vcombine_s32(vld1_dup_s32(from), vld1_dup_s32(from+1)); }
-template<> EIGEN_STRONG_INLINE Packet2ui ploaddup<Packet2ui>(const uint32_t* from)
-{ return vld1_dup_u32(from); }
-template<> EIGEN_STRONG_INLINE Packet4ui ploaddup<Packet4ui>(const uint32_t* from)
-{ return vcombine_u32(vld1_dup_u32(from), vld1_dup_u32(from+1)); }
-template<> EIGEN_STRONG_INLINE Packet2l ploaddup<Packet2l>(const int64_t* from)
-{ return vld1q_dup_s64(from); }
-template<> EIGEN_STRONG_INLINE Packet2ul ploaddup<Packet2ul>(const uint64_t* from)
-{ return vld1q_dup_u64(from); }
-
-template<> EIGEN_STRONG_INLINE Packet4f ploadquad<Packet4f>(const float* from) { return vld1q_dup_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4c ploadquad<Packet4c>(const int8_t* from)
-{ return vget_lane_s32(vreinterpret_s32_s8(vld1_dup_s8(from)), 0); }
-template<> EIGEN_STRONG_INLINE Packet8c ploadquad<Packet8c>(const int8_t* from)
-{
-  return vreinterpret_s8_u32(vzip_u32(
-      vreinterpret_u32_s8(vld1_dup_s8(from)),
-      vreinterpret_u32_s8(vld1_dup_s8(from+1))).val[0]);
+template <>
+EIGEN_STRONG_INLINE Packet2i ploaddup<Packet2i>(const int32_t* from) {
+  return vld1_dup_s32(from);
 }
-template<> EIGEN_STRONG_INLINE Packet16c ploadquad<Packet16c>(const int8_t* from)
-{
-  const int8x8_t a = vreinterpret_s8_u32(vzip_u32(
-      vreinterpret_u32_s8(vld1_dup_s8(from)),
-      vreinterpret_u32_s8(vld1_dup_s8(from+1))).val[0]);
-  const int8x8_t b = vreinterpret_s8_u32(vzip_u32(
-      vreinterpret_u32_s8(vld1_dup_s8(from+2)),
-      vreinterpret_u32_s8(vld1_dup_s8(from+3))).val[0]);
-  return vcombine_s8(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet4i ploaddup<Packet4i>(const int32_t* from) {
+  return vcombine_s32(vld1_dup_s32(from), vld1_dup_s32(from + 1));
 }
-template<> EIGEN_STRONG_INLINE Packet4uc ploadquad<Packet4uc>(const uint8_t* from)
-{ return vget_lane_u32(vreinterpret_u32_u8(vld1_dup_u8(from)), 0); }
-template<> EIGEN_STRONG_INLINE Packet8uc ploadquad<Packet8uc>(const uint8_t* from)
-{
-  return vreinterpret_u8_u32(vzip_u32(
-      vreinterpret_u32_u8(vld1_dup_u8(from)),
-      vreinterpret_u32_u8(vld1_dup_u8(from+1))).val[0]);
+template <>
+EIGEN_STRONG_INLINE Packet2ui ploaddup<Packet2ui>(const uint32_t* from) {
+  return vld1_dup_u32(from);
 }
-template<> EIGEN_STRONG_INLINE Packet16uc ploadquad<Packet16uc>(const uint8_t* from)
-{
-  const uint8x8_t a = vreinterpret_u8_u32(vzip_u32(
-      vreinterpret_u32_u8(vld1_dup_u8(from)),
-      vreinterpret_u32_u8(vld1_dup_u8(from+1))).val[0]);
-  const uint8x8_t b = vreinterpret_u8_u32(vzip_u32(
-      vreinterpret_u32_u8(vld1_dup_u8(from+2)),
-      vreinterpret_u32_u8(vld1_dup_u8(from+3))).val[0]);
-  return vcombine_u8(a,b);
+template <>
+EIGEN_STRONG_INLINE Packet4ui ploaddup<Packet4ui>(const uint32_t* from) {
+  return vcombine_u32(vld1_dup_u32(from), vld1_dup_u32(from + 1));
 }
-template<> EIGEN_STRONG_INLINE Packet8s ploadquad<Packet8s>(const int16_t* from)
-{ return vcombine_s16(vld1_dup_s16(from), vld1_dup_s16(from+1)); }
-template<> EIGEN_STRONG_INLINE Packet8us ploadquad<Packet8us>(const uint16_t* from)
-{ return vcombine_u16(vld1_dup_u16(from), vld1_dup_u16(from+1)); }
-template<> EIGEN_STRONG_INLINE Packet4i ploadquad<Packet4i>(const int32_t* from) { return vld1q_dup_s32(from); }
-template<> EIGEN_STRONG_INLINE Packet4ui ploadquad<Packet4ui>(const uint32_t* from) { return vld1q_dup_u32(from); }
+template <>
+EIGEN_STRONG_INLINE Packet2l ploaddup<Packet2l>(const int64_t* from) {
+  return vld1q_dup_s64(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul ploaddup<Packet2ul>(const uint64_t* from) {
+  return vld1q_dup_u64(from);
+}
 
-template<> EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet2f& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1_f32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet4f& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_f32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet4c& from)
-{ memcpy(to, &from, sizeof(from)); }
-template<> EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet8c& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1_s8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet16c& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet4uc& from)
-{ memcpy(to, &from, sizeof(from)); }
-template<> EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet8uc& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1_u8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet16uc& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<int16_t>(int16_t* to, const Packet4s& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1_s16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<int16_t>(int16_t* to, const Packet8s& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<uint16_t>(uint16_t* to, const Packet4us& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1_u16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<uint16_t>(uint16_t* to, const Packet8us& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<int32_t>(int32_t* to, const Packet2i& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1_s32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<int32_t>(int32_t* to, const Packet4i& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet2ui& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1_u32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet4ui& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<int64_t>(int64_t* to, const Packet2l& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s64(to,from); }
-template<> EIGEN_STRONG_INLINE void pstore<uint64_t>(uint64_t* to, const Packet2ul& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u64(to,from); }
+template <>
+EIGEN_STRONG_INLINE Packet4f ploadquad<Packet4f>(const float* from) {
+  return vld1q_dup_f32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c ploadquad<Packet4c>(const int8_t* from) {
+  return vget_lane_s32(vreinterpret_s32_s8(vld1_dup_s8(from)), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c ploadquad<Packet8c>(const int8_t* from) {
+  return vreinterpret_s8_u32(
+      vzip_u32(vreinterpret_u32_s8(vld1_dup_s8(from)), vreinterpret_u32_s8(vld1_dup_s8(from + 1))).val[0]);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c ploadquad<Packet16c>(const int8_t* from) {
+  const int8x8_t a = vreinterpret_s8_u32(
+      vzip_u32(vreinterpret_u32_s8(vld1_dup_s8(from)), vreinterpret_u32_s8(vld1_dup_s8(from + 1))).val[0]);
+  const int8x8_t b = vreinterpret_s8_u32(
+      vzip_u32(vreinterpret_u32_s8(vld1_dup_s8(from + 2)), vreinterpret_u32_s8(vld1_dup_s8(from + 3))).val[0]);
+  return vcombine_s8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc ploadquad<Packet4uc>(const uint8_t* from) {
+  return vget_lane_u32(vreinterpret_u32_u8(vld1_dup_u8(from)), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc ploadquad<Packet8uc>(const uint8_t* from) {
+  return vreinterpret_u8_u32(
+      vzip_u32(vreinterpret_u32_u8(vld1_dup_u8(from)), vreinterpret_u32_u8(vld1_dup_u8(from + 1))).val[0]);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc ploadquad<Packet16uc>(const uint8_t* from) {
+  const uint8x8_t a = vreinterpret_u8_u32(
+      vzip_u32(vreinterpret_u32_u8(vld1_dup_u8(from)), vreinterpret_u32_u8(vld1_dup_u8(from + 1))).val[0]);
+  const uint8x8_t b = vreinterpret_u8_u32(
+      vzip_u32(vreinterpret_u32_u8(vld1_dup_u8(from + 2)), vreinterpret_u32_u8(vld1_dup_u8(from + 3))).val[0]);
+  return vcombine_u8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s ploadquad<Packet8s>(const int16_t* from) {
+  return vcombine_s16(vld1_dup_s16(from), vld1_dup_s16(from + 1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us ploadquad<Packet8us>(const uint16_t* from) {
+  return vcombine_u16(vld1_dup_u16(from), vld1_dup_u16(from + 1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i ploadquad<Packet4i>(const int32_t* from) {
+  return vld1q_dup_s32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui ploadquad<Packet4ui>(const uint32_t* from) {
+  return vld1q_dup_u32(from);
+}
 
-template<> EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet2f& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1_f32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet4f& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet4c& from)
-{ memcpy(to, &from, sizeof(from)); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet8c& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1_s8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet16c& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet4uc& from)
-{ memcpy(to, &from, sizeof(from)); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet8uc& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1_u8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet16uc& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u8(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int16_t>(int16_t* to, const Packet4s& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1_s16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int16_t>(int16_t* to, const Packet8s& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint16_t>(uint16_t* to, const Packet4us& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1_u16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint16_t>(uint16_t* to, const Packet8us& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u16(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int32_t>(int32_t* to, const Packet2i& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1_s32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int32_t>(int32_t* to, const Packet4i& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet2ui& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1_u32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet4ui& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u32(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int64_t>(int64_t* to, const Packet2l& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s64(to,from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<uint64_t>(uint64_t* to, const Packet2ul& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u64(to,from); }
+template <>
+EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet2f& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_f32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet4f& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_f32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet4c& from) {
+  memcpy(to, &from, sizeof(from));
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet8c& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_s8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet16c& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_s8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet4uc& from) {
+  memcpy(to, &from, sizeof(from));
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet8uc& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_u8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet16uc& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_u8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int16_t>(int16_t* to, const Packet4s& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_s16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int16_t>(int16_t* to, const Packet8s& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_s16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint16_t>(uint16_t* to, const Packet4us& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_u16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint16_t>(uint16_t* to, const Packet8us& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_u16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int32_t>(int32_t* to, const Packet2i& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_s32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int32_t>(int32_t* to, const Packet4i& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_s32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet2ui& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_u32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet4ui& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_u32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<int64_t>(int64_t* to, const Packet2l& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_s64(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint64_t>(uint64_t* to, const Packet2ul& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_u64(to, from);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2f pgather<float, Packet2f>(const float* from, Index stride)
-{
+template <>
+EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet2f& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_f32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet4f& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet4c& from) {
+  memcpy(to, &from, sizeof(from));
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet8c& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_s8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet16c& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_s8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet4uc& from) {
+  memcpy(to, &from, sizeof(from));
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet8uc& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_u8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet16uc& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_u8(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int16_t>(int16_t* to, const Packet4s& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_s16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int16_t>(int16_t* to, const Packet8s& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_s16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint16_t>(uint16_t* to, const Packet4us& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_u16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint16_t>(uint16_t* to, const Packet8us& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_u16(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int32_t>(int32_t* to, const Packet2i& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_s32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int32_t>(int32_t* to, const Packet4i& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet2ui& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_u32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet4ui& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_u32(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int64_t>(int64_t* to, const Packet2l& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_s64(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint64_t>(uint64_t* to, const Packet2ul& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_u64(to, from);
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2f pgather<float, Packet2f>(const float* from, Index stride) {
   Packet2f res = vld1_dup_f32(from);
-  res = vld1_lane_f32(from + 1*stride, res, 1);
+  res = vld1_lane_f32(from + 1 * stride, res, 1);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4f pgather<float, Packet4f>(const float* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4f pgather<float, Packet4f>(const float* from, Index stride) {
   Packet4f res = vld1q_dup_f32(from);
-  res = vld1q_lane_f32(from + 1*stride, res, 1);
-  res = vld1q_lane_f32(from + 2*stride, res, 2);
-  res = vld1q_lane_f32(from + 3*stride, res, 3);
+  res = vld1q_lane_f32(from + 1 * stride, res, 1);
+  res = vld1q_lane_f32(from + 2 * stride, res, 2);
+  res = vld1q_lane_f32(from + 3 * stride, res, 3);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4c pgather<int8_t, Packet4c>(const int8_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4c pgather<int8_t, Packet4c>(const int8_t* from, Index stride) {
   Packet4c res;
-  for (int i = 0; i != 4; i++)
-    reinterpret_cast<int8_t*>(&res)[i] = *(from + i * stride);
+  for (int i = 0; i != 4; i++) reinterpret_cast<int8_t*>(&res)[i] = *(from + i * stride);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c pgather<int8_t, Packet8c>(const int8_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c pgather<int8_t, Packet8c>(const int8_t* from, Index stride) {
   Packet8c res = vld1_dup_s8(from);
-  res = vld1_lane_s8(from + 1*stride, res, 1);
-  res = vld1_lane_s8(from + 2*stride, res, 2);
-  res = vld1_lane_s8(from + 3*stride, res, 3);
-  res = vld1_lane_s8(from + 4*stride, res, 4);
-  res = vld1_lane_s8(from + 5*stride, res, 5);
-  res = vld1_lane_s8(from + 6*stride, res, 6);
-  res = vld1_lane_s8(from + 7*stride, res, 7);
+  res = vld1_lane_s8(from + 1 * stride, res, 1);
+  res = vld1_lane_s8(from + 2 * stride, res, 2);
+  res = vld1_lane_s8(from + 3 * stride, res, 3);
+  res = vld1_lane_s8(from + 4 * stride, res, 4);
+  res = vld1_lane_s8(from + 5 * stride, res, 5);
+  res = vld1_lane_s8(from + 6 * stride, res, 6);
+  res = vld1_lane_s8(from + 7 * stride, res, 7);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16c pgather<int8_t, Packet16c>(const int8_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16c pgather<int8_t, Packet16c>(const int8_t* from, Index stride) {
   Packet16c res = vld1q_dup_s8(from);
-  res = vld1q_lane_s8(from + 1*stride, res, 1);
-  res = vld1q_lane_s8(from + 2*stride, res, 2);
-  res = vld1q_lane_s8(from + 3*stride, res, 3);
-  res = vld1q_lane_s8(from + 4*stride, res, 4);
-  res = vld1q_lane_s8(from + 5*stride, res, 5);
-  res = vld1q_lane_s8(from + 6*stride, res, 6);
-  res = vld1q_lane_s8(from + 7*stride, res, 7);
-  res = vld1q_lane_s8(from + 8*stride, res, 8);
-  res = vld1q_lane_s8(from + 9*stride, res, 9);
-  res = vld1q_lane_s8(from + 10*stride, res, 10);
-  res = vld1q_lane_s8(from + 11*stride, res, 11);
-  res = vld1q_lane_s8(from + 12*stride, res, 12);
-  res = vld1q_lane_s8(from + 13*stride, res, 13);
-  res = vld1q_lane_s8(from + 14*stride, res, 14);
-  res = vld1q_lane_s8(from + 15*stride, res, 15);
+  res = vld1q_lane_s8(from + 1 * stride, res, 1);
+  res = vld1q_lane_s8(from + 2 * stride, res, 2);
+  res = vld1q_lane_s8(from + 3 * stride, res, 3);
+  res = vld1q_lane_s8(from + 4 * stride, res, 4);
+  res = vld1q_lane_s8(from + 5 * stride, res, 5);
+  res = vld1q_lane_s8(from + 6 * stride, res, 6);
+  res = vld1q_lane_s8(from + 7 * stride, res, 7);
+  res = vld1q_lane_s8(from + 8 * stride, res, 8);
+  res = vld1q_lane_s8(from + 9 * stride, res, 9);
+  res = vld1q_lane_s8(from + 10 * stride, res, 10);
+  res = vld1q_lane_s8(from + 11 * stride, res, 11);
+  res = vld1q_lane_s8(from + 12 * stride, res, 12);
+  res = vld1q_lane_s8(from + 13 * stride, res, 13);
+  res = vld1q_lane_s8(from + 14 * stride, res, 14);
+  res = vld1q_lane_s8(from + 15 * stride, res, 15);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4uc pgather<uint8_t, Packet4uc>(const uint8_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4uc pgather<uint8_t, Packet4uc>(const uint8_t* from, Index stride) {
   Packet4uc res;
-  for (int i = 0; i != 4; i++)
-    reinterpret_cast<uint8_t*>(&res)[i] = *(from + i * stride);
+  for (int i = 0; i != 4; i++) reinterpret_cast<uint8_t*>(&res)[i] = *(from + i * stride);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc pgather<uint8_t, Packet8uc>(const uint8_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc pgather<uint8_t, Packet8uc>(const uint8_t* from, Index stride) {
   Packet8uc res = vld1_dup_u8(from);
-  res = vld1_lane_u8(from + 1*stride, res, 1);
-  res = vld1_lane_u8(from + 2*stride, res, 2);
-  res = vld1_lane_u8(from + 3*stride, res, 3);
-  res = vld1_lane_u8(from + 4*stride, res, 4);
-  res = vld1_lane_u8(from + 5*stride, res, 5);
-  res = vld1_lane_u8(from + 6*stride, res, 6);
-  res = vld1_lane_u8(from + 7*stride, res, 7);
+  res = vld1_lane_u8(from + 1 * stride, res, 1);
+  res = vld1_lane_u8(from + 2 * stride, res, 2);
+  res = vld1_lane_u8(from + 3 * stride, res, 3);
+  res = vld1_lane_u8(from + 4 * stride, res, 4);
+  res = vld1_lane_u8(from + 5 * stride, res, 5);
+  res = vld1_lane_u8(from + 6 * stride, res, 6);
+  res = vld1_lane_u8(from + 7 * stride, res, 7);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16uc pgather<uint8_t, Packet16uc>(const uint8_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16uc pgather<uint8_t, Packet16uc>(const uint8_t* from, Index stride) {
   Packet16uc res = vld1q_dup_u8(from);
-  res = vld1q_lane_u8(from + 1*stride, res, 1);
-  res = vld1q_lane_u8(from + 2*stride, res, 2);
-  res = vld1q_lane_u8(from + 3*stride, res, 3);
-  res = vld1q_lane_u8(from + 4*stride, res, 4);
-  res = vld1q_lane_u8(from + 5*stride, res, 5);
-  res = vld1q_lane_u8(from + 6*stride, res, 6);
-  res = vld1q_lane_u8(from + 7*stride, res, 7);
-  res = vld1q_lane_u8(from + 8*stride, res, 8);
-  res = vld1q_lane_u8(from + 9*stride, res, 9);
-  res = vld1q_lane_u8(from + 10*stride, res, 10);
-  res = vld1q_lane_u8(from + 11*stride, res, 11);
-  res = vld1q_lane_u8(from + 12*stride, res, 12);
-  res = vld1q_lane_u8(from + 13*stride, res, 13);
-  res = vld1q_lane_u8(from + 14*stride, res, 14);
-  res = vld1q_lane_u8(from + 15*stride, res, 15);
+  res = vld1q_lane_u8(from + 1 * stride, res, 1);
+  res = vld1q_lane_u8(from + 2 * stride, res, 2);
+  res = vld1q_lane_u8(from + 3 * stride, res, 3);
+  res = vld1q_lane_u8(from + 4 * stride, res, 4);
+  res = vld1q_lane_u8(from + 5 * stride, res, 5);
+  res = vld1q_lane_u8(from + 6 * stride, res, 6);
+  res = vld1q_lane_u8(from + 7 * stride, res, 7);
+  res = vld1q_lane_u8(from + 8 * stride, res, 8);
+  res = vld1q_lane_u8(from + 9 * stride, res, 9);
+  res = vld1q_lane_u8(from + 10 * stride, res, 10);
+  res = vld1q_lane_u8(from + 11 * stride, res, 11);
+  res = vld1q_lane_u8(from + 12 * stride, res, 12);
+  res = vld1q_lane_u8(from + 13 * stride, res, 13);
+  res = vld1q_lane_u8(from + 14 * stride, res, 14);
+  res = vld1q_lane_u8(from + 15 * stride, res, 15);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s pgather<int16_t, Packet4s>(const int16_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s pgather<int16_t, Packet4s>(const int16_t* from, Index stride) {
   Packet4s res = vld1_dup_s16(from);
-  res = vld1_lane_s16(from + 1*stride, res, 1);
-  res = vld1_lane_s16(from + 2*stride, res, 2);
-  res = vld1_lane_s16(from + 3*stride, res, 3);
+  res = vld1_lane_s16(from + 1 * stride, res, 1);
+  res = vld1_lane_s16(from + 2 * stride, res, 2);
+  res = vld1_lane_s16(from + 3 * stride, res, 3);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8s pgather<int16_t, Packet8s>(const int16_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8s pgather<int16_t, Packet8s>(const int16_t* from, Index stride) {
   Packet8s res = vld1q_dup_s16(from);
-  res = vld1q_lane_s16(from + 1*stride, res, 1);
-  res = vld1q_lane_s16(from + 2*stride, res, 2);
-  res = vld1q_lane_s16(from + 3*stride, res, 3);
-  res = vld1q_lane_s16(from + 4*stride, res, 4);
-  res = vld1q_lane_s16(from + 5*stride, res, 5);
-  res = vld1q_lane_s16(from + 6*stride, res, 6);
-  res = vld1q_lane_s16(from + 7*stride, res, 7);
+  res = vld1q_lane_s16(from + 1 * stride, res, 1);
+  res = vld1q_lane_s16(from + 2 * stride, res, 2);
+  res = vld1q_lane_s16(from + 3 * stride, res, 3);
+  res = vld1q_lane_s16(from + 4 * stride, res, 4);
+  res = vld1q_lane_s16(from + 5 * stride, res, 5);
+  res = vld1q_lane_s16(from + 6 * stride, res, 6);
+  res = vld1q_lane_s16(from + 7 * stride, res, 7);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us pgather<uint16_t, Packet4us>(const uint16_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us pgather<uint16_t, Packet4us>(const uint16_t* from, Index stride) {
   Packet4us res = vld1_dup_u16(from);
-  res = vld1_lane_u16(from + 1*stride, res, 1);
-  res = vld1_lane_u16(from + 2*stride, res, 2);
-  res = vld1_lane_u16(from + 3*stride, res, 3);
+  res = vld1_lane_u16(from + 1 * stride, res, 1);
+  res = vld1_lane_u16(from + 2 * stride, res, 2);
+  res = vld1_lane_u16(from + 3 * stride, res, 3);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8us pgather<uint16_t, Packet8us>(const uint16_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8us pgather<uint16_t, Packet8us>(const uint16_t* from, Index stride) {
   Packet8us res = vld1q_dup_u16(from);
-  res = vld1q_lane_u16(from + 1*stride, res, 1);
-  res = vld1q_lane_u16(from + 2*stride, res, 2);
-  res = vld1q_lane_u16(from + 3*stride, res, 3);
-  res = vld1q_lane_u16(from + 4*stride, res, 4);
-  res = vld1q_lane_u16(from + 5*stride, res, 5);
-  res = vld1q_lane_u16(from + 6*stride, res, 6);
-  res = vld1q_lane_u16(from + 7*stride, res, 7);
+  res = vld1q_lane_u16(from + 1 * stride, res, 1);
+  res = vld1q_lane_u16(from + 2 * stride, res, 2);
+  res = vld1q_lane_u16(from + 3 * stride, res, 3);
+  res = vld1q_lane_u16(from + 4 * stride, res, 4);
+  res = vld1q_lane_u16(from + 5 * stride, res, 5);
+  res = vld1q_lane_u16(from + 6 * stride, res, 6);
+  res = vld1q_lane_u16(from + 7 * stride, res, 7);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2i pgather<int32_t, Packet2i>(const int32_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2i pgather<int32_t, Packet2i>(const int32_t* from, Index stride) {
   Packet2i res = vld1_dup_s32(from);
-  res = vld1_lane_s32(from + 1*stride, res, 1);
+  res = vld1_lane_s32(from + 1 * stride, res, 1);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4i pgather<int32_t, Packet4i>(const int32_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4i pgather<int32_t, Packet4i>(const int32_t* from, Index stride) {
   Packet4i res = vld1q_dup_s32(from);
-  res = vld1q_lane_s32(from + 1*stride, res, 1);
-  res = vld1q_lane_s32(from + 2*stride, res, 2);
-  res = vld1q_lane_s32(from + 3*stride, res, 3);
+  res = vld1q_lane_s32(from + 1 * stride, res, 1);
+  res = vld1q_lane_s32(from + 2 * stride, res, 2);
+  res = vld1q_lane_s32(from + 3 * stride, res, 3);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ui pgather<uint32_t, Packet2ui>(const uint32_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ui pgather<uint32_t, Packet2ui>(const uint32_t* from, Index stride) {
   Packet2ui res = vld1_dup_u32(from);
-  res = vld1_lane_u32(from + 1*stride, res, 1);
+  res = vld1_lane_u32(from + 1 * stride, res, 1);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4ui pgather<uint32_t, Packet4ui>(const uint32_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4ui pgather<uint32_t, Packet4ui>(const uint32_t* from, Index stride) {
   Packet4ui res = vld1q_dup_u32(from);
-  res = vld1q_lane_u32(from + 1*stride, res, 1);
-  res = vld1q_lane_u32(from + 2*stride, res, 2);
-  res = vld1q_lane_u32(from + 3*stride, res, 3);
+  res = vld1q_lane_u32(from + 1 * stride, res, 1);
+  res = vld1q_lane_u32(from + 2 * stride, res, 2);
+  res = vld1q_lane_u32(from + 3 * stride, res, 3);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2l pgather<int64_t, Packet2l>(const int64_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2l pgather<int64_t, Packet2l>(const int64_t* from, Index stride) {
   Packet2l res = vld1q_dup_s64(from);
-  res = vld1q_lane_s64(from + 1*stride, res, 1);
+  res = vld1q_lane_s64(from + 1 * stride, res, 1);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ul pgather<uint64_t, Packet2ul>(const uint64_t* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ul pgather<uint64_t, Packet2ul>(const uint64_t* from, Index stride) {
   Packet2ul res = vld1q_dup_u64(from);
-  res = vld1q_lane_u64(from + 1*stride, res, 1);
+  res = vld1q_lane_u64(from + 1 * stride, res, 1);
   return res;
 }
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<float, Packet2f>(float* to, const Packet2f& from, Index stride)
-{
-  vst1_lane_f32(to + stride*0, from, 0);
-  vst1_lane_f32(to + stride*1, from, 1);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<float, Packet2f>(float* to, const Packet2f& from, Index stride) {
+  vst1_lane_f32(to + stride * 0, from, 0);
+  vst1_lane_f32(to + stride * 1, from, 1);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<float, Packet4f>(float* to, const Packet4f& from, Index stride)
-{
-  vst1q_lane_f32(to + stride*0, from, 0);
-  vst1q_lane_f32(to + stride*1, from, 1);
-  vst1q_lane_f32(to + stride*2, from, 2);
-  vst1q_lane_f32(to + stride*3, from, 3);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<float, Packet4f>(float* to, const Packet4f& from, Index stride) {
+  vst1q_lane_f32(to + stride * 0, from, 0);
+  vst1q_lane_f32(to + stride * 1, from, 1);
+  vst1q_lane_f32(to + stride * 2, from, 2);
+  vst1q_lane_f32(to + stride * 3, from, 3);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet4c>(int8_t* to, const Packet4c& from, Index stride)
-{
-  for (int i = 0; i != 4; i++)
-    *(to + i * stride) = reinterpret_cast<const int8_t*>(&from)[i];
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet4c>(int8_t* to, const Packet4c& from, Index stride) {
+  for (int i = 0; i != 4; i++) *(to + i * stride) = reinterpret_cast<const int8_t*>(&from)[i];
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet8c>(int8_t* to, const Packet8c& from, Index stride)
-{
-  vst1_lane_s8(to + stride*0, from, 0);
-  vst1_lane_s8(to + stride*1, from, 1);
-  vst1_lane_s8(to + stride*2, from, 2);
-  vst1_lane_s8(to + stride*3, from, 3);
-  vst1_lane_s8(to + stride*4, from, 4);
-  vst1_lane_s8(to + stride*5, from, 5);
-  vst1_lane_s8(to + stride*6, from, 6);
-  vst1_lane_s8(to + stride*7, from, 7);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet8c>(int8_t* to, const Packet8c& from, Index stride) {
+  vst1_lane_s8(to + stride * 0, from, 0);
+  vst1_lane_s8(to + stride * 1, from, 1);
+  vst1_lane_s8(to + stride * 2, from, 2);
+  vst1_lane_s8(to + stride * 3, from, 3);
+  vst1_lane_s8(to + stride * 4, from, 4);
+  vst1_lane_s8(to + stride * 5, from, 5);
+  vst1_lane_s8(to + stride * 6, from, 6);
+  vst1_lane_s8(to + stride * 7, from, 7);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet16c>(int8_t* to, const Packet16c& from, Index stride)
-{
-  vst1q_lane_s8(to + stride*0, from, 0);
-  vst1q_lane_s8(to + stride*1, from, 1);
-  vst1q_lane_s8(to + stride*2, from, 2);
-  vst1q_lane_s8(to + stride*3, from, 3);
-  vst1q_lane_s8(to + stride*4, from, 4);
-  vst1q_lane_s8(to + stride*5, from, 5);
-  vst1q_lane_s8(to + stride*6, from, 6);
-  vst1q_lane_s8(to + stride*7, from, 7);
-  vst1q_lane_s8(to + stride*8, from, 8);
-  vst1q_lane_s8(to + stride*9, from, 9);
-  vst1q_lane_s8(to + stride*10, from, 10);
-  vst1q_lane_s8(to + stride*11, from, 11);
-  vst1q_lane_s8(to + stride*12, from, 12);
-  vst1q_lane_s8(to + stride*13, from, 13);
-  vst1q_lane_s8(to + stride*14, from, 14);
-  vst1q_lane_s8(to + stride*15, from, 15);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet16c>(int8_t* to, const Packet16c& from,
+                                                                       Index stride) {
+  vst1q_lane_s8(to + stride * 0, from, 0);
+  vst1q_lane_s8(to + stride * 1, from, 1);
+  vst1q_lane_s8(to + stride * 2, from, 2);
+  vst1q_lane_s8(to + stride * 3, from, 3);
+  vst1q_lane_s8(to + stride * 4, from, 4);
+  vst1q_lane_s8(to + stride * 5, from, 5);
+  vst1q_lane_s8(to + stride * 6, from, 6);
+  vst1q_lane_s8(to + stride * 7, from, 7);
+  vst1q_lane_s8(to + stride * 8, from, 8);
+  vst1q_lane_s8(to + stride * 9, from, 9);
+  vst1q_lane_s8(to + stride * 10, from, 10);
+  vst1q_lane_s8(to + stride * 11, from, 11);
+  vst1q_lane_s8(to + stride * 12, from, 12);
+  vst1q_lane_s8(to + stride * 13, from, 13);
+  vst1q_lane_s8(to + stride * 14, from, 14);
+  vst1q_lane_s8(to + stride * 15, from, 15);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet4uc>(uint8_t* to, const Packet4uc& from, Index stride)
-{
-  for (int i = 0; i != 4; i++)
-    *(to + i * stride) = reinterpret_cast<const uint8_t*>(&from)[i];
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet4uc>(uint8_t* to, const Packet4uc& from,
+                                                                        Index stride) {
+  for (int i = 0; i != 4; i++) *(to + i * stride) = reinterpret_cast<const uint8_t*>(&from)[i];
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet8uc>(uint8_t* to, const Packet8uc& from, Index stride)
-{
-  vst1_lane_u8(to + stride*0, from, 0);
-  vst1_lane_u8(to + stride*1, from, 1);
-  vst1_lane_u8(to + stride*2, from, 2);
-  vst1_lane_u8(to + stride*3, from, 3);
-  vst1_lane_u8(to + stride*4, from, 4);
-  vst1_lane_u8(to + stride*5, from, 5);
-  vst1_lane_u8(to + stride*6, from, 6);
-  vst1_lane_u8(to + stride*7, from, 7);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet8uc>(uint8_t* to, const Packet8uc& from,
+                                                                        Index stride) {
+  vst1_lane_u8(to + stride * 0, from, 0);
+  vst1_lane_u8(to + stride * 1, from, 1);
+  vst1_lane_u8(to + stride * 2, from, 2);
+  vst1_lane_u8(to + stride * 3, from, 3);
+  vst1_lane_u8(to + stride * 4, from, 4);
+  vst1_lane_u8(to + stride * 5, from, 5);
+  vst1_lane_u8(to + stride * 6, from, 6);
+  vst1_lane_u8(to + stride * 7, from, 7);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet16uc>(uint8_t* to, const Packet16uc& from, Index stride)
-{
-  vst1q_lane_u8(to + stride*0, from, 0);
-  vst1q_lane_u8(to + stride*1, from, 1);
-  vst1q_lane_u8(to + stride*2, from, 2);
-  vst1q_lane_u8(to + stride*3, from, 3);
-  vst1q_lane_u8(to + stride*4, from, 4);
-  vst1q_lane_u8(to + stride*5, from, 5);
-  vst1q_lane_u8(to + stride*6, from, 6);
-  vst1q_lane_u8(to + stride*7, from, 7);
-  vst1q_lane_u8(to + stride*8, from, 8);
-  vst1q_lane_u8(to + stride*9, from, 9);
-  vst1q_lane_u8(to + stride*10, from, 10);
-  vst1q_lane_u8(to + stride*11, from, 11);
-  vst1q_lane_u8(to + stride*12, from, 12);
-  vst1q_lane_u8(to + stride*13, from, 13);
-  vst1q_lane_u8(to + stride*14, from, 14);
-  vst1q_lane_u8(to + stride*15, from, 15);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet16uc>(uint8_t* to, const Packet16uc& from,
+                                                                         Index stride) {
+  vst1q_lane_u8(to + stride * 0, from, 0);
+  vst1q_lane_u8(to + stride * 1, from, 1);
+  vst1q_lane_u8(to + stride * 2, from, 2);
+  vst1q_lane_u8(to + stride * 3, from, 3);
+  vst1q_lane_u8(to + stride * 4, from, 4);
+  vst1q_lane_u8(to + stride * 5, from, 5);
+  vst1q_lane_u8(to + stride * 6, from, 6);
+  vst1q_lane_u8(to + stride * 7, from, 7);
+  vst1q_lane_u8(to + stride * 8, from, 8);
+  vst1q_lane_u8(to + stride * 9, from, 9);
+  vst1q_lane_u8(to + stride * 10, from, 10);
+  vst1q_lane_u8(to + stride * 11, from, 11);
+  vst1q_lane_u8(to + stride * 12, from, 12);
+  vst1q_lane_u8(to + stride * 13, from, 13);
+  vst1q_lane_u8(to + stride * 14, from, 14);
+  vst1q_lane_u8(to + stride * 15, from, 15);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int16_t, Packet4s>(int16_t* to, const Packet4s& from, Index stride)
-{
-  vst1_lane_s16(to + stride*0, from, 0);
-  vst1_lane_s16(to + stride*1, from, 1);
-  vst1_lane_s16(to + stride*2, from, 2);
-  vst1_lane_s16(to + stride*3, from, 3);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int16_t, Packet4s>(int16_t* to, const Packet4s& from,
+                                                                       Index stride) {
+  vst1_lane_s16(to + stride * 0, from, 0);
+  vst1_lane_s16(to + stride * 1, from, 1);
+  vst1_lane_s16(to + stride * 2, from, 2);
+  vst1_lane_s16(to + stride * 3, from, 3);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int16_t, Packet8s>(int16_t* to, const Packet8s& from, Index stride)
-{
-  vst1q_lane_s16(to + stride*0, from, 0);
-  vst1q_lane_s16(to + stride*1, from, 1);
-  vst1q_lane_s16(to + stride*2, from, 2);
-  vst1q_lane_s16(to + stride*3, from, 3);
-  vst1q_lane_s16(to + stride*4, from, 4);
-  vst1q_lane_s16(to + stride*5, from, 5);
-  vst1q_lane_s16(to + stride*6, from, 6);
-  vst1q_lane_s16(to + stride*7, from, 7);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int16_t, Packet8s>(int16_t* to, const Packet8s& from,
+                                                                       Index stride) {
+  vst1q_lane_s16(to + stride * 0, from, 0);
+  vst1q_lane_s16(to + stride * 1, from, 1);
+  vst1q_lane_s16(to + stride * 2, from, 2);
+  vst1q_lane_s16(to + stride * 3, from, 3);
+  vst1q_lane_s16(to + stride * 4, from, 4);
+  vst1q_lane_s16(to + stride * 5, from, 5);
+  vst1q_lane_s16(to + stride * 6, from, 6);
+  vst1q_lane_s16(to + stride * 7, from, 7);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint16_t, Packet4us>(uint16_t* to, const Packet4us& from, Index stride)
-{
-  vst1_lane_u16(to + stride*0, from, 0);
-  vst1_lane_u16(to + stride*1, from, 1);
-  vst1_lane_u16(to + stride*2, from, 2);
-  vst1_lane_u16(to + stride*3, from, 3);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint16_t, Packet4us>(uint16_t* to, const Packet4us& from,
+                                                                         Index stride) {
+  vst1_lane_u16(to + stride * 0, from, 0);
+  vst1_lane_u16(to + stride * 1, from, 1);
+  vst1_lane_u16(to + stride * 2, from, 2);
+  vst1_lane_u16(to + stride * 3, from, 3);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint16_t, Packet8us>(uint16_t* to, const Packet8us& from, Index stride)
-{
-  vst1q_lane_u16(to + stride*0, from, 0);
-  vst1q_lane_u16(to + stride*1, from, 1);
-  vst1q_lane_u16(to + stride*2, from, 2);
-  vst1q_lane_u16(to + stride*3, from, 3);
-  vst1q_lane_u16(to + stride*4, from, 4);
-  vst1q_lane_u16(to + stride*5, from, 5);
-  vst1q_lane_u16(to + stride*6, from, 6);
-  vst1q_lane_u16(to + stride*7, from, 7);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint16_t, Packet8us>(uint16_t* to, const Packet8us& from,
+                                                                         Index stride) {
+  vst1q_lane_u16(to + stride * 0, from, 0);
+  vst1q_lane_u16(to + stride * 1, from, 1);
+  vst1q_lane_u16(to + stride * 2, from, 2);
+  vst1q_lane_u16(to + stride * 3, from, 3);
+  vst1q_lane_u16(to + stride * 4, from, 4);
+  vst1q_lane_u16(to + stride * 5, from, 5);
+  vst1q_lane_u16(to + stride * 6, from, 6);
+  vst1q_lane_u16(to + stride * 7, from, 7);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int32_t, Packet2i>(int32_t* to, const Packet2i& from, Index stride)
-{
-  vst1_lane_s32(to + stride*0, from, 0);
-  vst1_lane_s32(to + stride*1, from, 1);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int32_t, Packet2i>(int32_t* to, const Packet2i& from,
+                                                                       Index stride) {
+  vst1_lane_s32(to + stride * 0, from, 0);
+  vst1_lane_s32(to + stride * 1, from, 1);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int32_t, Packet4i>(int32_t* to, const Packet4i& from, Index stride)
-{
-  vst1q_lane_s32(to + stride*0, from, 0);
-  vst1q_lane_s32(to + stride*1, from, 1);
-  vst1q_lane_s32(to + stride*2, from, 2);
-  vst1q_lane_s32(to + stride*3, from, 3);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int32_t, Packet4i>(int32_t* to, const Packet4i& from,
+                                                                       Index stride) {
+  vst1q_lane_s32(to + stride * 0, from, 0);
+  vst1q_lane_s32(to + stride * 1, from, 1);
+  vst1q_lane_s32(to + stride * 2, from, 2);
+  vst1q_lane_s32(to + stride * 3, from, 3);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint32_t, Packet2ui>(uint32_t* to, const Packet2ui& from, Index stride)
-{
-  vst1_lane_u32(to + stride*0, from, 0);
-  vst1_lane_u32(to + stride*1, from, 1);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint32_t, Packet2ui>(uint32_t* to, const Packet2ui& from,
+                                                                         Index stride) {
+  vst1_lane_u32(to + stride * 0, from, 0);
+  vst1_lane_u32(to + stride * 1, from, 1);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint32_t, Packet4ui>(uint32_t* to, const Packet4ui& from, Index stride)
-{
-  vst1q_lane_u32(to + stride*0, from, 0);
-  vst1q_lane_u32(to + stride*1, from, 1);
-  vst1q_lane_u32(to + stride*2, from, 2);
-  vst1q_lane_u32(to + stride*3, from, 3);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint32_t, Packet4ui>(uint32_t* to, const Packet4ui& from,
+                                                                         Index stride) {
+  vst1q_lane_u32(to + stride * 0, from, 0);
+  vst1q_lane_u32(to + stride * 1, from, 1);
+  vst1q_lane_u32(to + stride * 2, from, 2);
+  vst1q_lane_u32(to + stride * 3, from, 3);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int64_t, Packet2l>(int64_t* to, const Packet2l& from, Index stride)
-{
-  vst1q_lane_s64(to + stride*0, from, 0);
-  vst1q_lane_s64(to + stride*1, from, 1);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int64_t, Packet2l>(int64_t* to, const Packet2l& from,
+                                                                       Index stride) {
+  vst1q_lane_s64(to + stride * 0, from, 0);
+  vst1q_lane_s64(to + stride * 1, from, 1);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint64_t, Packet2ul>(uint64_t* to, const Packet2ul& from, Index stride)
-{
-  vst1q_lane_u64(to + stride*0, from, 0);
-  vst1q_lane_u64(to + stride*1, from, 1);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint64_t, Packet2ul>(uint64_t* to, const Packet2ul& from,
+                                                                         Index stride) {
+  vst1q_lane_u64(to + stride * 0, from, 0);
+  vst1q_lane_u64(to + stride * 1, from, 1);
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<int8_t>(const int8_t* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<uint8_t>(const uint8_t* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<int16_t>(const int16_t* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<uint16_t>(const uint16_t* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<int32_t>(const int32_t* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<uint32_t>(const uint32_t* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<int64_t>(const int64_t* addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<uint64_t>(const uint64_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template <>
+EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<int8_t>(const int8_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<uint8_t>(const uint8_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<int16_t>(const int16_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<uint16_t>(const uint16_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<int32_t>(const int32_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<uint32_t>(const uint32_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<int64_t>(const int64_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<uint64_t>(const uint64_t* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
 
-template<> EIGEN_STRONG_INLINE float pfirst<Packet2f>(const Packet2f& a) { return vget_lane_f32(a,0); }
-template<> EIGEN_STRONG_INLINE float pfirst<Packet4f>(const Packet4f& a) { return vgetq_lane_f32(a,0); }
-template<> EIGEN_STRONG_INLINE int8_t pfirst<Packet4c>(const Packet4c& a) { return static_cast<int8_t>(a & 0xff); }
-template<> EIGEN_STRONG_INLINE int8_t pfirst<Packet8c>(const Packet8c& a) { return vget_lane_s8(a,0); }
-template<> EIGEN_STRONG_INLINE int8_t pfirst<Packet16c>(const Packet16c& a) { return vgetq_lane_s8(a,0); }
-template<> EIGEN_STRONG_INLINE uint8_t pfirst<Packet4uc>(const Packet4uc& a) { return static_cast<uint8_t>(a & 0xff); }
-template<> EIGEN_STRONG_INLINE uint8_t pfirst<Packet8uc>(const Packet8uc& a) { return vget_lane_u8(a,0); }
-template<> EIGEN_STRONG_INLINE uint8_t pfirst<Packet16uc>(const Packet16uc& a) { return vgetq_lane_u8(a,0); }
-template<> EIGEN_STRONG_INLINE int16_t pfirst<Packet4s>(const Packet4s& a) { return vget_lane_s16(a,0); }
-template<> EIGEN_STRONG_INLINE int16_t pfirst<Packet8s>(const Packet8s& a) { return vgetq_lane_s16(a,0); }
-template<> EIGEN_STRONG_INLINE uint16_t pfirst<Packet4us>(const Packet4us& a) { return vget_lane_u16(a,0); }
-template<> EIGEN_STRONG_INLINE uint16_t pfirst<Packet8us>(const Packet8us& a) { return vgetq_lane_u16(a,0); }
-template<> EIGEN_STRONG_INLINE int32_t pfirst<Packet2i>(const Packet2i& a) { return vget_lane_s32(a,0); }
-template<> EIGEN_STRONG_INLINE int32_t pfirst<Packet4i>(const Packet4i& a) { return vgetq_lane_s32(a,0); }
-template<> EIGEN_STRONG_INLINE uint32_t pfirst<Packet2ui>(const Packet2ui& a) { return vget_lane_u32(a,0); }
-template<> EIGEN_STRONG_INLINE uint32_t pfirst<Packet4ui>(const Packet4ui& a) { return vgetq_lane_u32(a,0); }
-template<> EIGEN_STRONG_INLINE int64_t pfirst<Packet2l>(const Packet2l& a) { return vgetq_lane_s64(a,0); }
-template<> EIGEN_STRONG_INLINE uint64_t pfirst<Packet2ul>(const Packet2ul& a) { return vgetq_lane_u64(a,0); }
+template <>
+EIGEN_STRONG_INLINE float pfirst<Packet2f>(const Packet2f& a) {
+  return vget_lane_f32(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE float pfirst<Packet4f>(const Packet4f& a) {
+  return vgetq_lane_f32(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE int8_t pfirst<Packet4c>(const Packet4c& a) {
+  return static_cast<int8_t>(a & 0xff);
+}
+template <>
+EIGEN_STRONG_INLINE int8_t pfirst<Packet8c>(const Packet8c& a) {
+  return vget_lane_s8(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE int8_t pfirst<Packet16c>(const Packet16c& a) {
+  return vgetq_lane_s8(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint8_t pfirst<Packet4uc>(const Packet4uc& a) {
+  return static_cast<uint8_t>(a & 0xff);
+}
+template <>
+EIGEN_STRONG_INLINE uint8_t pfirst<Packet8uc>(const Packet8uc& a) {
+  return vget_lane_u8(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint8_t pfirst<Packet16uc>(const Packet16uc& a) {
+  return vgetq_lane_u8(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t pfirst<Packet4s>(const Packet4s& a) {
+  return vget_lane_s16(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t pfirst<Packet8s>(const Packet8s& a) {
+  return vgetq_lane_s16(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t pfirst<Packet4us>(const Packet4us& a) {
+  return vget_lane_u16(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t pfirst<Packet8us>(const Packet8us& a) {
+  return vgetq_lane_u16(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t pfirst<Packet2i>(const Packet2i& a) {
+  return vget_lane_s32(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t pfirst<Packet4i>(const Packet4i& a) {
+  return vgetq_lane_s32(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t pfirst<Packet2ui>(const Packet2ui& a) {
+  return vget_lane_u32(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t pfirst<Packet4ui>(const Packet4ui& a) {
+  return vgetq_lane_u32(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE int64_t pfirst<Packet2l>(const Packet2l& a) {
+  return vgetq_lane_s64(a, 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t pfirst<Packet2ul>(const Packet2ul& a) {
+  return vgetq_lane_u64(a, 0);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f preverse(const Packet2f& a) { return vrev64_f32(a); }
-template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2f preverse(const Packet2f& a) {
+  return vrev64_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a) {
   const float32x4_t a_r64 = vrev64q_f32(a);
   return vcombine_f32(vget_high_f32(a_r64), vget_low_f32(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet4c preverse(const Packet4c& a)
-{ return vget_lane_s32(vreinterpret_s32_s8(vrev64_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0); }
-template<> EIGEN_STRONG_INLINE Packet8c preverse(const Packet8c& a) { return vrev64_s8(a); }
-template<> EIGEN_STRONG_INLINE Packet16c preverse(const Packet16c& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4c preverse(const Packet4c& a) {
+  return vget_lane_s32(vreinterpret_s32_s8(vrev64_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c preverse(const Packet8c& a) {
+  return vrev64_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c preverse(const Packet16c& a) {
   const int8x16_t a_r64 = vrev64q_s8(a);
   return vcombine_s8(vget_high_s8(a_r64), vget_low_s8(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet4uc preverse(const Packet4uc& a)
-{ return vget_lane_u32(vreinterpret_u32_u8(vrev64_u8(vreinterpret_u8_u32(vdup_n_u32(a)))), 0); }
-template<> EIGEN_STRONG_INLINE Packet8uc preverse(const Packet8uc& a) { return vrev64_u8(a); }
-template<> EIGEN_STRONG_INLINE Packet16uc preverse(const Packet16uc& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4uc preverse(const Packet4uc& a) {
+  return vget_lane_u32(vreinterpret_u32_u8(vrev64_u8(vreinterpret_u8_u32(vdup_n_u32(a)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc preverse(const Packet8uc& a) {
+  return vrev64_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc preverse(const Packet16uc& a) {
   const uint8x16_t a_r64 = vrev64q_u8(a);
   return vcombine_u8(vget_high_u8(a_r64), vget_low_u8(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet4s preverse(const Packet4s& a) { return vrev64_s16(a); }
-template<> EIGEN_STRONG_INLINE Packet8s preverse(const Packet8s& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4s preverse(const Packet4s& a) {
+  return vrev64_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s preverse(const Packet8s& a) {
   const int16x8_t a_r64 = vrev64q_s16(a);
   return vcombine_s16(vget_high_s16(a_r64), vget_low_s16(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet4us preverse(const Packet4us& a) { return vrev64_u16(a); }
-template<> EIGEN_STRONG_INLINE Packet8us preverse(const Packet8us& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4us preverse(const Packet4us& a) {
+  return vrev64_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us preverse(const Packet8us& a) {
   const uint16x8_t a_r64 = vrev64q_u16(a);
   return vcombine_u16(vget_high_u16(a_r64), vget_low_u16(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet2i preverse(const Packet2i& a) { return vrev64_s32(a); }
-template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2i preverse(const Packet2i& a) {
+  return vrev64_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a) {
   const int32x4_t a_r64 = vrev64q_s32(a);
   return vcombine_s32(vget_high_s32(a_r64), vget_low_s32(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet2ui preverse(const Packet2ui& a) { return vrev64_u32(a); }
-template<> EIGEN_STRONG_INLINE Packet4ui preverse(const Packet4ui& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2ui preverse(const Packet2ui& a) {
+  return vrev64_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui preverse(const Packet4ui& a) {
   const uint32x4_t a_r64 = vrev64q_u32(a);
   return vcombine_u32(vget_high_u32(a_r64), vget_low_u32(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet2l preverse(const Packet2l& a)
-{ return vcombine_s64(vget_high_s64(a), vget_low_s64(a)); }
-template<> EIGEN_STRONG_INLINE Packet2ul preverse(const Packet2ul& a)
-{ return vcombine_u64(vget_high_u64(a), vget_low_u64(a)); }
+template <>
+EIGEN_STRONG_INLINE Packet2l preverse(const Packet2l& a) {
+  return vcombine_s64(vget_high_s64(a), vget_low_s64(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul preverse(const Packet2ul& a) {
+  return vcombine_u64(vget_high_u64(a), vget_low_u64(a));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pabs(const Packet2f& a) { return vabs_f32(a); }
-template<> EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a) { return vabsq_f32(a); }
-template<> EIGEN_STRONG_INLINE Packet4c pabs<Packet4c>(const Packet4c& a)
-{ return vget_lane_s32(vreinterpret_s32_s8(vabs_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0); }
-template<> EIGEN_STRONG_INLINE Packet8c pabs(const Packet8c& a) { return vabs_s8(a); }
-template<> EIGEN_STRONG_INLINE Packet16c pabs(const Packet16c& a) { return vabsq_s8(a); }
-template<> EIGEN_STRONG_INLINE Packet4uc pabs(const Packet4uc& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet8uc pabs(const Packet8uc& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet16uc pabs(const Packet16uc& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4s pabs(const Packet4s& a) { return vabs_s16(a); }
-template<> EIGEN_STRONG_INLINE Packet8s pabs(const Packet8s& a) { return vabsq_s16(a); }
-template<> EIGEN_STRONG_INLINE Packet4us pabs(const Packet4us& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet8us pabs(const Packet8us& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2i pabs(const Packet2i& a) { return vabs_s32(a); }
-template<> EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a) { return vabsq_s32(a); }
-template<> EIGEN_STRONG_INLINE Packet2ui pabs(const Packet2ui& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4ui pabs(const Packet4ui& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2l pabs(const Packet2l& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2f pabs(const Packet2f& a) {
+  return vabs_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a) {
+  return vabsq_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pabs<Packet4c>(const Packet4c& a) {
+  return vget_lane_s32(vreinterpret_s32_s8(vabs_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pabs(const Packet8c& a) {
+  return vabs_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c pabs(const Packet16c& a) {
+  return vabsq_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pabs(const Packet4uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pabs(const Packet8uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc pabs(const Packet16uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pabs(const Packet4s& a) {
+  return vabs_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s pabs(const Packet8s& a) {
+  return vabsq_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pabs(const Packet4us& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pabs(const Packet8us& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pabs(const Packet2i& a) {
+  return vabs_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a) {
+  return vabsq_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pabs(const Packet2ui& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pabs(const Packet4ui& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l pabs(const Packet2l& a) {
 #if EIGEN_ARCH_ARM64
   return vabsq_s64(a);
 #else
-  return vcombine_s64(
-      vdup_n_s64((std::abs)(vgetq_lane_s64(a, 0))),
-      vdup_n_s64((std::abs)(vgetq_lane_s64(a, 1))));
+  return vcombine_s64(vdup_n_s64((std::abs)(vgetq_lane_s64(a, 0))), vdup_n_s64((std::abs)(vgetq_lane_s64(a, 1))));
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet2ul pabs(const Packet2ul& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet2ul pabs(const Packet2ul& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pfrexp<Packet2f>(const Packet2f& a, Packet2f& exponent)
-{ return pfrexp_generic(a,exponent); }
-template<> EIGEN_STRONG_INLINE Packet4f pfrexp<Packet4f>(const Packet4f& a, Packet4f& exponent)
-{ return pfrexp_generic(a,exponent); }
+template <>
+EIGEN_STRONG_INLINE Packet2f psignbit(const Packet2f& a) {
+  return vreinterpret_f32_s32(vshr_n_s32(vreinterpret_s32_f32(a), 31));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f psignbit(const Packet4f& a) {
+  return vreinterpretq_f32_s32(vshrq_n_s32(vreinterpretq_s32_f32(a), 31));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pldexp<Packet2f>(const Packet2f& a, const Packet2f& exponent)
-{ return pldexp_generic(a,exponent); }
-template<> EIGEN_STRONG_INLINE Packet4f pldexp<Packet4f>(const Packet4f& a, const Packet4f& exponent)
-{ return pldexp_generic(a,exponent); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pfrexp<Packet2f>(const Packet2f& a, Packet2f& exponent) {
+  return pfrexp_generic(a, exponent);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pfrexp<Packet4f>(const Packet4f& a, Packet4f& exponent) {
+  return pfrexp_generic(a, exponent);
+}
 
-template<> EIGEN_STRONG_INLINE float predux<Packet2f>(const Packet2f& a) { return vget_lane_f32(vpadd_f32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2f pldexp<Packet2f>(const Packet2f& a, const Packet2f& exponent) {
+  return pldexp_generic(a, exponent);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pldexp<Packet4f>(const Packet4f& a, const Packet4f& exponent) {
+  return pldexp_generic(a, exponent);
+}
+
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE float predux<Packet2f>(const Packet2f& a) {
+  return vaddv_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a) {
+  return vaddvq_f32(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE float predux<Packet2f>(const Packet2f& a) {
+  return vget_lane_f32(vpadd_f32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a) {
   const float32x2_t sum = vadd_f32(vget_low_f32(a), vget_high_f32(a));
   return vget_lane_f32(vpadd_f32(sum, sum), 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux<Packet4c>(const Packet4c& a)
-{
+#endif
+template <>
+EIGEN_STRONG_INLINE int8_t predux<Packet4c>(const Packet4c& a) {
   const int8x8_t a_dup = vreinterpret_s8_s32(vdup_n_s32(a));
   int8x8_t sum = vpadd_s8(a_dup, a_dup);
   sum = vpadd_s8(sum, sum);
   return vget_lane_s8(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux<Packet8c>(const Packet8c& a)
-{
-  int8x8_t sum = vpadd_s8(a,a);
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE int8_t predux<Packet8c>(const Packet8c& a) {
+  return vaddv_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE int8_t predux<Packet16c>(const Packet16c& a) {
+  return vaddvq_s8(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE int8_t predux<Packet8c>(const Packet8c& a) {
+  int8x8_t sum = vpadd_s8(a, a);
   sum = vpadd_s8(sum, sum);
   sum = vpadd_s8(sum, sum);
   return vget_lane_s8(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux<Packet16c>(const Packet16c& a)
-{
+template <>
+EIGEN_STRONG_INLINE int8_t predux<Packet16c>(const Packet16c& a) {
   int8x8_t sum = vadd_s8(vget_low_s8(a), vget_high_s8(a));
   sum = vpadd_s8(sum, sum);
   sum = vpadd_s8(sum, sum);
   sum = vpadd_s8(sum, sum);
   return vget_lane_s8(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux<Packet4uc>(const Packet4uc& a)
-{
+#endif
+template <>
+EIGEN_STRONG_INLINE uint8_t predux<Packet4uc>(const Packet4uc& a) {
   const uint8x8_t a_dup = vreinterpret_u8_u32(vdup_n_u32(a));
   uint8x8_t sum = vpadd_u8(a_dup, a_dup);
   sum = vpadd_u8(sum, sum);
   return vget_lane_u8(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux<Packet8uc>(const Packet8uc& a)
-{
-  uint8x8_t sum = vpadd_u8(a,a);
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE uint8_t predux<Packet8uc>(const Packet8uc& a) {
+  return vaddv_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint8_t predux<Packet16uc>(const Packet16uc& a) {
+  return vaddvq_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t predux<Packet4s>(const Packet4s& a) {
+  return vaddv_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t predux<Packet8s>(const Packet8s& a) {
+  return vaddvq_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t predux<Packet4us>(const Packet4us& a) {
+  return vaddv_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t predux<Packet8us>(const Packet8us& a) {
+  return vaddvq_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux<Packet2i>(const Packet2i& a) {
+  return vaddv_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux<Packet4i>(const Packet4i& a) {
+  return vaddvq_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux<Packet2ui>(const Packet2ui& a) {
+  return vaddv_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux<Packet4ui>(const Packet4ui& a) {
+  return vaddvq_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE int64_t predux<Packet2l>(const Packet2l& a) {
+  return vaddvq_s64(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t predux<Packet2ul>(const Packet2ul& a) {
+  return vaddvq_u64(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE uint8_t predux<Packet8uc>(const Packet8uc& a) {
+  uint8x8_t sum = vpadd_u8(a, a);
   sum = vpadd_u8(sum, sum);
   sum = vpadd_u8(sum, sum);
   return vget_lane_u8(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux<Packet16uc>(const Packet16uc& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint8_t predux<Packet16uc>(const Packet16uc& a) {
   uint8x8_t sum = vadd_u8(vget_low_u8(a), vget_high_u8(a));
   sum = vpadd_u8(sum, sum);
   sum = vpadd_u8(sum, sum);
   sum = vpadd_u8(sum, sum);
   return vget_lane_u8(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE int16_t predux<Packet4s>(const Packet4s& a)
-{
-  const int16x4_t sum = vpadd_s16(a,a);
+template <>
+EIGEN_STRONG_INLINE int16_t predux<Packet4s>(const Packet4s& a) {
+  const int16x4_t sum = vpadd_s16(a, a);
   return vget_lane_s16(vpadd_s16(sum, sum), 0);
 }
-template<> EIGEN_STRONG_INLINE int16_t predux<Packet8s>(const Packet8s& a)
-{
+template <>
+EIGEN_STRONG_INLINE int16_t predux<Packet8s>(const Packet8s& a) {
   int16x4_t sum = vadd_s16(vget_low_s16(a), vget_high_s16(a));
   sum = vpadd_s16(sum, sum);
   sum = vpadd_s16(sum, sum);
   return vget_lane_s16(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux<Packet4us>(const Packet4us& a)
-{
-  const uint16x4_t sum = vpadd_u16(a,a);
+template <>
+EIGEN_STRONG_INLINE uint16_t predux<Packet4us>(const Packet4us& a) {
+  const uint16x4_t sum = vpadd_u16(a, a);
   return vget_lane_u16(vpadd_u16(sum, sum), 0);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux<Packet8us>(const Packet8us& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint16_t predux<Packet8us>(const Packet8us& a) {
   uint16x4_t sum = vadd_u16(vget_low_u16(a), vget_high_u16(a));
   sum = vpadd_u16(sum, sum);
   sum = vpadd_u16(sum, sum);
   return vget_lane_u16(sum, 0);
 }
-template<> EIGEN_STRONG_INLINE int32_t predux<Packet2i>(const Packet2i& a) { return vget_lane_s32(vpadd_s32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE int32_t predux<Packet4i>(const Packet4i& a)
-{
+template <>
+EIGEN_STRONG_INLINE int32_t predux<Packet2i>(const Packet2i& a) {
+  return vget_lane_s32(vpadd_s32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux<Packet4i>(const Packet4i& a) {
   const int32x2_t sum = vadd_s32(vget_low_s32(a), vget_high_s32(a));
   return vget_lane_s32(vpadd_s32(sum, sum), 0);
 }
-template<> EIGEN_STRONG_INLINE uint32_t predux<Packet2ui>(const Packet2ui& a) { return vget_lane_u32(vpadd_u32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE uint32_t predux<Packet4ui>(const Packet4ui& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint32_t predux<Packet2ui>(const Packet2ui& a) {
+  return vget_lane_u32(vpadd_u32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux<Packet4ui>(const Packet4ui& a) {
   const uint32x2_t sum = vadd_u32(vget_low_u32(a), vget_high_u32(a));
   return vget_lane_u32(vpadd_u32(sum, sum), 0);
 }
-template<> EIGEN_STRONG_INLINE int64_t predux<Packet2l>(const Packet2l& a)
-{ return vgetq_lane_s64(a, 0) + vgetq_lane_s64(a, 1); }
-template<> EIGEN_STRONG_INLINE uint64_t predux<Packet2ul>(const Packet2ul& a)
-{ return vgetq_lane_u64(a, 0) + vgetq_lane_u64(a, 1); }
+template <>
+EIGEN_STRONG_INLINE int64_t predux<Packet2l>(const Packet2l& a) {
+  return vgetq_lane_s64(a, 0) + vgetq_lane_s64(a, 1);
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t predux<Packet2ul>(const Packet2ul& a) {
+  return vgetq_lane_u64(a, 0) + vgetq_lane_u64(a, 1);
+}
+#endif
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4c predux_half_dowto4(const Packet8c& a)
-{
-  return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(a,
-      vreinterpret_s8_s32(vrev64_s32(vreinterpret_s32_s8(a))))), 0);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4c predux_half_dowto4(const Packet8c& a) {
+  return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(a, vreinterpret_s8_s32(vrev64_s32(vreinterpret_s32_s8(a))))), 0);
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c predux_half_dowto4(const Packet16c& a)
-{ return vadd_s8(vget_high_s8(a), vget_low_s8(a)); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4uc predux_half_dowto4(const Packet8uc& a)
-{
-  return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(a,
-      vreinterpret_u8_u32(vrev64_u32(vreinterpret_u32_u8(a))))), 0);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c predux_half_dowto4(const Packet16c& a) {
+  return vadd_s8(vget_high_s8(a), vget_low_s8(a));
 }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc predux_half_dowto4(const Packet16uc& a)
-{ return vadd_u8(vget_high_u8(a), vget_low_u8(a)); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s predux_half_dowto4(const Packet8s& a)
-{ return vadd_s16(vget_high_s16(a), vget_low_s16(a)); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us predux_half_dowto4(const Packet8us& a)
-{ return vadd_u16(vget_high_u16(a), vget_low_u16(a)); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4uc predux_half_dowto4(const Packet8uc& a) {
+  return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(a, vreinterpret_u8_u32(vrev64_u32(vreinterpret_u32_u8(a))))), 0);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc predux_half_dowto4(const Packet16uc& a) {
+  return vadd_u8(vget_high_u8(a), vget_low_u8(a));
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s predux_half_dowto4(const Packet8s& a) {
+  return vadd_s16(vget_high_s16(a), vget_low_s16(a));
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us predux_half_dowto4(const Packet8us& a) {
+  return vadd_u16(vget_high_u16(a), vget_low_u16(a));
+}
 
 // Other reduction functions:
 // mul
-template<> EIGEN_STRONG_INLINE float predux_mul<Packet2f>(const Packet2f& a)
-{ return vget_lane_f32(a, 0) * vget_lane_f32(a, 1); }
-template<> EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a)
-{ return predux_mul(vmul_f32(vget_low_f32(a), vget_high_f32(a))); }
-template<> EIGEN_STRONG_INLINE int8_t predux_mul<Packet4c>(const Packet4c& a)
-{
+template <>
+EIGEN_STRONG_INLINE float predux_mul<Packet2f>(const Packet2f& a) {
+  return vget_lane_f32(a, 0) * vget_lane_f32(a, 1);
+}
+template <>
+EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a) {
+  return predux_mul<Packet2f>(vmul_f32(vget_low_f32(a), vget_high_f32(a)));
+}
+template <>
+EIGEN_STRONG_INLINE int8_t predux_mul<Packet4c>(const Packet4c& a) {
   int8x8_t prod = vreinterpret_s8_s32(vdup_n_s32(a));
   prod = vmul_s8(prod, vrev16_s8(prod));
   return vget_lane_s8(prod, 0) * vget_lane_s8(prod, 2);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_mul<Packet8c>(const Packet8c& a)
-{
+template <>
+EIGEN_STRONG_INLINE int8_t predux_mul<Packet8c>(const Packet8c& a) {
   int8x8_t prod = vmul_s8(a, vrev16_s8(a));
   prod = vmul_s8(prod, vrev32_s8(prod));
   return vget_lane_s8(prod, 0) * vget_lane_s8(prod, 4);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_mul<Packet16c>(const Packet16c& a)
-{ return predux_mul(vmul_s8(vget_low_s8(a), vget_high_s8(a))); }
-template<> EIGEN_STRONG_INLINE uint8_t predux_mul<Packet4uc>(const Packet4uc& a)
-{
+template <>
+EIGEN_STRONG_INLINE int8_t predux_mul<Packet16c>(const Packet16c& a) {
+  return predux_mul<Packet8c>(vmul_s8(vget_low_s8(a), vget_high_s8(a)));
+}
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_mul<Packet4uc>(const Packet4uc& a) {
   uint8x8_t prod = vreinterpret_u8_u32(vdup_n_u32(a));
   prod = vmul_u8(prod, vrev16_u8(prod));
   return vget_lane_u8(prod, 0) * vget_lane_u8(prod, 2);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_mul<Packet8uc>(const Packet8uc& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_mul<Packet8uc>(const Packet8uc& a) {
   uint8x8_t prod = vmul_u8(a, vrev16_u8(a));
   prod = vmul_u8(prod, vrev32_u8(prod));
   return vget_lane_u8(prod, 0) * vget_lane_u8(prod, 4);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_mul<Packet16uc>(const Packet16uc& a)
-{ return predux_mul(vmul_u8(vget_low_u8(a), vget_high_u8(a))); }
-template<> EIGEN_STRONG_INLINE int16_t predux_mul<Packet4s>(const Packet4s& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_mul<Packet16uc>(const Packet16uc& a) {
+  return predux_mul<Packet8uc>(vmul_u8(vget_low_u8(a), vget_high_u8(a)));
+}
+template <>
+EIGEN_STRONG_INLINE int16_t predux_mul<Packet4s>(const Packet4s& a) {
   const int16x4_t prod = vmul_s16(a, vrev32_s16(a));
   return vget_lane_s16(prod, 0) * vget_lane_s16(prod, 2);
 }
-template<> EIGEN_STRONG_INLINE int16_t predux_mul<Packet8s>(const Packet8s& a)
-{
+template <>
+EIGEN_STRONG_INLINE int16_t predux_mul<Packet8s>(const Packet8s& a) {
   int16x4_t prod;
 
   // Get the product of a_lo * a_hi -> |a1*a5|a2*a6|a3*a7|a4*a8|
@@ -2544,13 +3708,13 @@
   // Multiply |a1*a5*a2*a6*a3*a7*a4*a8|
   return vget_lane_s16(prod, 0) * vget_lane_s16(prod, 2);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux_mul<Packet4us>(const Packet4us& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_mul<Packet4us>(const Packet4us& a) {
   const uint16x4_t prod = vmul_u16(a, vrev32_u16(a));
   return vget_lane_u16(prod, 0) * vget_lane_u16(prod, 2);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux_mul<Packet8us>(const Packet8us& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_mul<Packet8us>(const Packet8us& a) {
   uint16x4_t prod;
 
   // Get the product of a_lo * a_hi -> |a1*a5|a2*a6|a3*a7|a4*a8|
@@ -2560,326 +3724,487 @@
   // Multiply |a1*a5*a2*a6*a3*a7*a4*a8|
   return vget_lane_u16(prod, 0) * vget_lane_u16(prod, 2);
 }
-template<> EIGEN_STRONG_INLINE int32_t predux_mul<Packet2i>(const Packet2i& a)
-{ return vget_lane_s32(a, 0) * vget_lane_s32(a, 1); }
-template<> EIGEN_STRONG_INLINE int32_t predux_mul<Packet4i>(const Packet4i& a)
-{ return predux_mul(vmul_s32(vget_low_s32(a), vget_high_s32(a))); }
-template<> EIGEN_STRONG_INLINE uint32_t predux_mul<Packet2ui>(const Packet2ui& a)
-{ return vget_lane_u32(a, 0) * vget_lane_u32(a, 1); }
-template<> EIGEN_STRONG_INLINE uint32_t predux_mul<Packet4ui>(const Packet4ui& a)
-{ return predux_mul(vmul_u32(vget_low_u32(a), vget_high_u32(a))); }
-template<> EIGEN_STRONG_INLINE int64_t predux_mul<Packet2l>(const Packet2l& a)
-{ return vgetq_lane_s64(a, 0) * vgetq_lane_s64(a, 1); }
-template<> EIGEN_STRONG_INLINE uint64_t predux_mul<Packet2ul>(const Packet2ul& a)
-{ return vgetq_lane_u64(a, 0) * vgetq_lane_u64(a, 1); }
+template <>
+EIGEN_STRONG_INLINE int32_t predux_mul<Packet2i>(const Packet2i& a) {
+  return vget_lane_s32(a, 0) * vget_lane_s32(a, 1);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux_mul<Packet4i>(const Packet4i& a) {
+  return predux_mul<Packet2i>(vmul_s32(vget_low_s32(a), vget_high_s32(a)));
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_mul<Packet2ui>(const Packet2ui& a) {
+  return vget_lane_u32(a, 0) * vget_lane_u32(a, 1);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_mul<Packet4ui>(const Packet4ui& a) {
+  return predux_mul<Packet2ui>(vmul_u32(vget_low_u32(a), vget_high_u32(a)));
+}
+template <>
+EIGEN_STRONG_INLINE int64_t predux_mul<Packet2l>(const Packet2l& a) {
+  return vgetq_lane_s64(a, 0) * vgetq_lane_s64(a, 1);
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t predux_mul<Packet2ul>(const Packet2ul& a) {
+  return vgetq_lane_u64(a, 0) * vgetq_lane_u64(a, 1);
+}
 
 // min
-template<> EIGEN_STRONG_INLINE float predux_min<Packet2f>(const Packet2f& a)
-{ return vget_lane_f32(vpmin_f32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a)
-{
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE float predux_min<Packet2f>(const Packet2f& a) {
+  return vminv_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a) {
+  return vminvq_f32(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE float predux_min<Packet2f>(const Packet2f& a) {
+  return vget_lane_f32(vpmin_f32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a) {
   const float32x2_t min = vmin_f32(vget_low_f32(a), vget_high_f32(a));
   return vget_lane_f32(vpmin_f32(min, min), 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_min<Packet4c>(const Packet4c& a)
-{
+#endif
+template <>
+EIGEN_STRONG_INLINE int8_t predux_min<Packet4c>(const Packet4c& a) {
   const int8x8_t a_dup = vreinterpret_s8_s32(vdup_n_s32(a));
   int8x8_t min = vpmin_s8(a_dup, a_dup);
   min = vpmin_s8(min, min);
   return vget_lane_s8(min, 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_min<Packet8c>(const Packet8c& a)
-{
-  int8x8_t min = vpmin_s8(a,a);
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE int8_t predux_min<Packet8c>(const Packet8c& a) {
+  return vminv_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE int8_t predux_min<Packet16c>(const Packet16c& a) {
+  return vminvq_s8(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE int8_t predux_min<Packet8c>(const Packet8c& a) {
+  int8x8_t min = vpmin_s8(a, a);
   min = vpmin_s8(min, min);
   min = vpmin_s8(min, min);
   return vget_lane_s8(min, 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_min<Packet16c>(const Packet16c& a)
-{
+template <>
+EIGEN_STRONG_INLINE int8_t predux_min<Packet16c>(const Packet16c& a) {
   int8x8_t min = vmin_s8(vget_low_s8(a), vget_high_s8(a));
   min = vpmin_s8(min, min);
   min = vpmin_s8(min, min);
   min = vpmin_s8(min, min);
   return vget_lane_s8(min, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_min<Packet4uc>(const Packet4uc& a)
-{
+#endif
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_min<Packet4uc>(const Packet4uc& a) {
   const uint8x8_t a_dup = vreinterpret_u8_u32(vdup_n_u32(a));
   uint8x8_t min = vpmin_u8(a_dup, a_dup);
   min = vpmin_u8(min, min);
   return vget_lane_u8(min, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_min<Packet8uc>(const Packet8uc& a)
-{
-  uint8x8_t min = vpmin_u8(a,a);
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_min<Packet8uc>(const Packet8uc& a) {
+  return vminv_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_min<Packet16uc>(const Packet16uc& a) {
+  return vminvq_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t predux_min<Packet4s>(const Packet4s& a) {
+  return vminv_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t predux_min<Packet8s>(const Packet8s& a) {
+  return vminvq_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_min<Packet4us>(const Packet4us& a) {
+  return vminv_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_min<Packet8us>(const Packet8us& a) {
+  return vminvq_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux_min<Packet2i>(const Packet2i& a) {
+  return vminv_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux_min<Packet4i>(const Packet4i& a) {
+  return vminvq_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_min<Packet2ui>(const Packet2ui& a) {
+  return vminv_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_min<Packet4ui>(const Packet4ui& a) {
+  return vminvq_u32(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_min<Packet8uc>(const Packet8uc& a) {
+  uint8x8_t min = vpmin_u8(a, a);
   min = vpmin_u8(min, min);
   min = vpmin_u8(min, min);
   return vget_lane_u8(min, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_min<Packet16uc>(const Packet16uc& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_min<Packet16uc>(const Packet16uc& a) {
   uint8x8_t min = vmin_u8(vget_low_u8(a), vget_high_u8(a));
   min = vpmin_u8(min, min);
   min = vpmin_u8(min, min);
   min = vpmin_u8(min, min);
   return vget_lane_u8(min, 0);
 }
-template<> EIGEN_STRONG_INLINE int16_t predux_min<Packet4s>(const Packet4s& a)
-{
-  const int16x4_t min = vpmin_s16(a,a);
+template <>
+EIGEN_STRONG_INLINE int16_t predux_min<Packet4s>(const Packet4s& a) {
+  const int16x4_t min = vpmin_s16(a, a);
   return vget_lane_s16(vpmin_s16(min, min), 0);
 }
-template<> EIGEN_STRONG_INLINE int16_t predux_min<Packet8s>(const Packet8s& a)
-{
+template <>
+EIGEN_STRONG_INLINE int16_t predux_min<Packet8s>(const Packet8s& a) {
   int16x4_t min = vmin_s16(vget_low_s16(a), vget_high_s16(a));
   min = vpmin_s16(min, min);
   min = vpmin_s16(min, min);
   return vget_lane_s16(min, 0);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux_min<Packet4us>(const Packet4us& a)
-{
-  const uint16x4_t min = vpmin_u16(a,a);
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_min<Packet4us>(const Packet4us& a) {
+  const uint16x4_t min = vpmin_u16(a, a);
   return vget_lane_u16(vpmin_u16(min, min), 0);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux_min<Packet8us>(const Packet8us& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_min<Packet8us>(const Packet8us& a) {
   uint16x4_t min = vmin_u16(vget_low_u16(a), vget_high_u16(a));
   min = vpmin_u16(min, min);
   min = vpmin_u16(min, min);
   return vget_lane_u16(min, 0);
 }
-template<> EIGEN_STRONG_INLINE int32_t predux_min<Packet2i>(const Packet2i& a)
-{ return vget_lane_s32(vpmin_s32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE int32_t predux_min<Packet4i>(const Packet4i& a)
-{
+template <>
+EIGEN_STRONG_INLINE int32_t predux_min<Packet2i>(const Packet2i& a) {
+  return vget_lane_s32(vpmin_s32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux_min<Packet4i>(const Packet4i& a) {
   const int32x2_t min = vmin_s32(vget_low_s32(a), vget_high_s32(a));
   return vget_lane_s32(vpmin_s32(min, min), 0);
 }
-template<> EIGEN_STRONG_INLINE uint32_t predux_min<Packet2ui>(const Packet2ui& a)
-{ return vget_lane_u32(vpmin_u32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE uint32_t predux_min<Packet4ui>(const Packet4ui& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_min<Packet2ui>(const Packet2ui& a) {
+  return vget_lane_u32(vpmin_u32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_min<Packet4ui>(const Packet4ui& a) {
   const uint32x2_t min = vmin_u32(vget_low_u32(a), vget_high_u32(a));
   return vget_lane_u32(vpmin_u32(min, min), 0);
 }
-template<> EIGEN_STRONG_INLINE int64_t predux_min<Packet2l>(const Packet2l& a)
-{ return (std::min)(vgetq_lane_s64(a, 0), vgetq_lane_s64(a, 1)); }
-template<> EIGEN_STRONG_INLINE uint64_t predux_min<Packet2ul>(const Packet2ul& a)
-{ return (std::min)(vgetq_lane_u64(a, 0), vgetq_lane_u64(a, 1)); }
+#endif
+template <>
+EIGEN_STRONG_INLINE int64_t predux_min<Packet2l>(const Packet2l& a) {
+  return (std::min)(vgetq_lane_s64(a, 0), vgetq_lane_s64(a, 1));
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t predux_min<Packet2ul>(const Packet2ul& a) {
+  return (std::min)(vgetq_lane_u64(a, 0), vgetq_lane_u64(a, 1));
+}
 
 // max
-template<> EIGEN_STRONG_INLINE float predux_max<Packet2f>(const Packet2f& a)
-{ return vget_lane_f32(vpmax_f32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a)
-{
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE float predux_max<Packet2f>(const Packet2f& a) {
+  return vmaxv_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a) {
+  return vmaxvq_f32(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE float predux_max<Packet2f>(const Packet2f& a) {
+  return vget_lane_f32(vpmax_f32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a) {
   const float32x2_t max = vmax_f32(vget_low_f32(a), vget_high_f32(a));
   return vget_lane_f32(vpmax_f32(max, max), 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_max<Packet4c>(const Packet4c& a)
-{
+#endif
+template <>
+EIGEN_STRONG_INLINE int8_t predux_max<Packet4c>(const Packet4c& a) {
   const int8x8_t a_dup = vreinterpret_s8_s32(vdup_n_s32(a));
   int8x8_t max = vpmax_s8(a_dup, a_dup);
   max = vpmax_s8(max, max);
   return vget_lane_s8(max, 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_max<Packet8c>(const Packet8c& a)
-{
-  int8x8_t max = vpmax_s8(a,a);
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE int8_t predux_max<Packet8c>(const Packet8c& a) {
+  return vmaxv_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE int8_t predux_max<Packet16c>(const Packet16c& a) {
+  return vmaxvq_s8(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE int8_t predux_max<Packet8c>(const Packet8c& a) {
+  int8x8_t max = vpmax_s8(a, a);
   max = vpmax_s8(max, max);
   max = vpmax_s8(max, max);
   return vget_lane_s8(max, 0);
 }
-template<> EIGEN_STRONG_INLINE int8_t predux_max<Packet16c>(const Packet16c& a)
-{
+template <>
+EIGEN_STRONG_INLINE int8_t predux_max<Packet16c>(const Packet16c& a) {
   int8x8_t max = vmax_s8(vget_low_s8(a), vget_high_s8(a));
   max = vpmax_s8(max, max);
   max = vpmax_s8(max, max);
   max = vpmax_s8(max, max);
   return vget_lane_s8(max, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_max<Packet4uc>(const Packet4uc& a)
-{
+#endif
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_max<Packet4uc>(const Packet4uc& a) {
   const uint8x8_t a_dup = vreinterpret_u8_u32(vdup_n_u32(a));
   uint8x8_t max = vpmax_u8(a_dup, a_dup);
   max = vpmax_u8(max, max);
   return vget_lane_u8(max, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_max<Packet8uc>(const Packet8uc& a)
-{
-  uint8x8_t max = vpmax_u8(a,a);
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_max<Packet8uc>(const Packet8uc& a) {
+  return vmaxv_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_max<Packet16uc>(const Packet16uc& a) {
+  return vmaxvq_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t predux_max<Packet4s>(const Packet4s& a) {
+  return vmaxv_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE int16_t predux_max<Packet8s>(const Packet8s& a) {
+  return vmaxvq_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_max<Packet4us>(const Packet4us& a) {
+  return vmaxv_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_max<Packet8us>(const Packet8us& a) {
+  return vmaxvq_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux_max<Packet2i>(const Packet2i& a) {
+  return vmaxv_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux_max<Packet4i>(const Packet4i& a) {
+  return vmaxvq_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_max<Packet2ui>(const Packet2ui& a) {
+  return vmaxv_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_max<Packet4ui>(const Packet4ui& a) {
+  return vmaxvq_u32(a);
+}
+#else
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_max<Packet8uc>(const Packet8uc& a) {
+  uint8x8_t max = vpmax_u8(a, a);
   max = vpmax_u8(max, max);
   max = vpmax_u8(max, max);
   return vget_lane_u8(max, 0);
 }
-template<> EIGEN_STRONG_INLINE uint8_t predux_max<Packet16uc>(const Packet16uc& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint8_t predux_max<Packet16uc>(const Packet16uc& a) {
   uint8x8_t max = vmax_u8(vget_low_u8(a), vget_high_u8(a));
   max = vpmax_u8(max, max);
   max = vpmax_u8(max, max);
   max = vpmax_u8(max, max);
   return vget_lane_u8(max, 0);
 }
-template<> EIGEN_STRONG_INLINE int16_t predux_max<Packet4s>(const Packet4s& a)
-{
-  const int16x4_t max = vpmax_s16(a,a);
+template <>
+EIGEN_STRONG_INLINE int16_t predux_max<Packet4s>(const Packet4s& a) {
+  const int16x4_t max = vpmax_s16(a, a);
   return vget_lane_s16(vpmax_s16(max, max), 0);
 }
-template<> EIGEN_STRONG_INLINE int16_t predux_max<Packet8s>(const Packet8s& a)
-{
+template <>
+EIGEN_STRONG_INLINE int16_t predux_max<Packet8s>(const Packet8s& a) {
   int16x4_t max = vmax_s16(vget_low_s16(a), vget_high_s16(a));
   max = vpmax_s16(max, max);
   max = vpmax_s16(max, max);
   return vget_lane_s16(max, 0);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux_max<Packet4us>(const Packet4us& a)
-{
-  const uint16x4_t max = vpmax_u16(a,a);
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_max<Packet4us>(const Packet4us& a) {
+  const uint16x4_t max = vpmax_u16(a, a);
   return vget_lane_u16(vpmax_u16(max, max), 0);
 }
-template<> EIGEN_STRONG_INLINE uint16_t predux_max<Packet8us>(const Packet8us& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint16_t predux_max<Packet8us>(const Packet8us& a) {
   uint16x4_t max = vmax_u16(vget_low_u16(a), vget_high_u16(a));
   max = vpmax_u16(max, max);
   max = vpmax_u16(max, max);
   return vget_lane_u16(max, 0);
 }
-template<> EIGEN_STRONG_INLINE int32_t predux_max<Packet2i>(const Packet2i& a)
-{ return vget_lane_s32(vpmax_s32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE int32_t predux_max<Packet4i>(const Packet4i& a)
-{
+template <>
+EIGEN_STRONG_INLINE int32_t predux_max<Packet2i>(const Packet2i& a) {
+  return vget_lane_s32(vpmax_s32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE int32_t predux_max<Packet4i>(const Packet4i& a) {
   const int32x2_t max = vmax_s32(vget_low_s32(a), vget_high_s32(a));
   return vget_lane_s32(vpmax_s32(max, max), 0);
 }
-template<> EIGEN_STRONG_INLINE uint32_t predux_max<Packet2ui>(const Packet2ui& a)
-{ return vget_lane_u32(vpmax_u32(a,a), 0); }
-template<> EIGEN_STRONG_INLINE uint32_t predux_max<Packet4ui>(const Packet4ui& a)
-{
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_max<Packet2ui>(const Packet2ui& a) {
+  return vget_lane_u32(vpmax_u32(a, a), 0);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_max<Packet4ui>(const Packet4ui& a) {
   const uint32x2_t max = vmax_u32(vget_low_u32(a), vget_high_u32(a));
   return vget_lane_u32(vpmax_u32(max, max), 0);
 }
-template<> EIGEN_STRONG_INLINE int64_t predux_max<Packet2l>(const Packet2l& a)
-{ return (std::max)(vgetq_lane_s64(a, 0), vgetq_lane_s64(a, 1)); }
-template<> EIGEN_STRONG_INLINE uint64_t predux_max<Packet2ul>(const Packet2ul& a)
-{ return (std::max)(vgetq_lane_u64(a, 0), vgetq_lane_u64(a, 1)); }
+#endif
+template <>
+EIGEN_STRONG_INLINE int64_t predux_max<Packet2l>(const Packet2l& a) {
+  return (std::max)(vgetq_lane_s64(a, 0), vgetq_lane_s64(a, 1));
+}
+template <>
+EIGEN_STRONG_INLINE uint64_t predux_max<Packet2ul>(const Packet2ul& a) {
+  return (std::max)(vgetq_lane_u64(a, 0), vgetq_lane_u64(a, 1));
+}
 
-template<> EIGEN_STRONG_INLINE bool predux_any(const Packet4f& x)
-{
-  uint32x2_t tmp = vorr_u32(vget_low_u32( vreinterpretq_u32_f32(x)),
-                            vget_high_u32(vreinterpretq_u32_f32(x)));
+template <>
+EIGEN_STRONG_INLINE bool predux_any(const Packet4f& x) {
+  uint32x2_t tmp = vorr_u32(vget_low_u32(vreinterpretq_u32_f32(x)), vget_high_u32(vreinterpretq_u32_f32(x)));
   return vget_lane_u32(vpmax_u32(tmp, tmp), 0);
 }
 
 // Helpers for ptranspose.
 namespace detail {
-  
-template<typename Packet>
+
+template <typename Packet>
 void zip_in_place(Packet& p1, Packet& p2);
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet2f>(Packet2f& p1, Packet2f& p2) {
   const float32x2x2_t tmp = vzip_f32(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet4f>(Packet4f& p1, Packet4f& p2) {
   const float32x4x2_t tmp = vzipq_f32(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet8c>(Packet8c& p1, Packet8c& p2) {
   const int8x8x2_t tmp = vzip_s8(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet16c>(Packet16c& p1, Packet16c& p2) {
   const int8x16x2_t tmp = vzipq_s8(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet8uc>(Packet8uc& p1, Packet8uc& p2) {
   const uint8x8x2_t tmp = vzip_u8(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet16uc>(Packet16uc& p1, Packet16uc& p2) {
   const uint8x16x2_t tmp = vzipq_u8(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet2i>(Packet2i& p1, Packet2i& p2) {
   const int32x2x2_t tmp = vzip_s32(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet4i>(Packet4i& p1, Packet4i& p2) {
   const int32x4x2_t tmp = vzipq_s32(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet2ui>(Packet2ui& p1, Packet2ui& p2) {
   const uint32x2x2_t tmp = vzip_u32(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet4ui>(Packet4ui& p1, Packet4ui& p2) {
   const uint32x4x2_t tmp = vzipq_u32(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet4s>(Packet4s& p1, Packet4s& p2) {
   const int16x4x2_t tmp = vzip_s16(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet8s>(Packet8s& p1, Packet8s& p2) {
   const int16x8x2_t tmp = vzipq_s16(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet4us>(Packet4us& p1, Packet4us& p2) {
   const uint16x4x2_t tmp = vzip_u16(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<>
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet8us>(Packet8us& p1, Packet8us& p2) {
   const uint16x8x2_t tmp = vzipq_u16(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
 
-template<typename Packet>
+template <typename Packet>
 EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 2>& kernel) {
   zip_in_place(kernel.packet[0], kernel.packet[1]);
 }
 
-template<typename Packet>
+template <typename Packet>
 EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 4>& kernel) {
   zip_in_place(kernel.packet[0], kernel.packet[2]);
   zip_in_place(kernel.packet[1], kernel.packet[3]);
@@ -2887,7 +4212,7 @@
   zip_in_place(kernel.packet[2], kernel.packet[3]);
 }
 
-template<typename Packet>
+template <typename Packet>
 EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 8>& kernel) {
   zip_in_place(kernel.packet[0], kernel.packet[4]);
   zip_in_place(kernel.packet[1], kernel.packet[5]);
@@ -2898,31 +4223,31 @@
   zip_in_place(kernel.packet[1], kernel.packet[3]);
   zip_in_place(kernel.packet[4], kernel.packet[6]);
   zip_in_place(kernel.packet[5], kernel.packet[7]);
-  
+
   zip_in_place(kernel.packet[0], kernel.packet[1]);
   zip_in_place(kernel.packet[2], kernel.packet[3]);
   zip_in_place(kernel.packet[4], kernel.packet[5]);
   zip_in_place(kernel.packet[6], kernel.packet[7]);
 }
 
-template<typename Packet>
+template <typename Packet>
 EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 16>& kernel) {
   EIGEN_UNROLL_LOOP
-  for (int i=0; i<4; ++i) {
+  for (int i = 0; i < 4; ++i) {
     const int m = (1 << i);
     EIGEN_UNROLL_LOOP
-    for (int j=0; j<m; ++j) {
-      const int n = (1 << (3-i));
+    for (int j = 0; j < m; ++j) {
+      const int n = (1 << (3 - i));
       EIGEN_UNROLL_LOOP
-      for (int k=0; k<n; ++k) {
-        const int idx = 2*j*n+k;
+      for (int k = 0; k < n; ++k) {
+        const int idx = 2 * j * n + k;
         zip_in_place(kernel.packet[idx], kernel.packet[idx + n]);
       }
     }
   }
 }
 
-} // namespace detail
+}  // namespace detail
 
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2f, 2>& kernel) {
   detail::ptranspose_impl(kernel);
@@ -2931,12 +4256,11 @@
   detail::ptranspose_impl(kernel);
 }
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4c, 4>& kernel)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4c, 4>& kernel) {
   const int8x8_t a = vreinterpret_s8_s32(vset_lane_s32(kernel.packet[2], vdup_n_s32(kernel.packet[0]), 1));
   const int8x8_t b = vreinterpret_s8_s32(vset_lane_s32(kernel.packet[3], vdup_n_s32(kernel.packet[1]), 1));
 
-  const int8x8x2_t zip8 = vzip_s8(a,b);
+  const int8x8x2_t zip8 = vzip_s8(a, b);
   const int16x4x2_t zip16 = vzip_s16(vreinterpret_s16_s8(zip8.val[0]), vreinterpret_s16_s8(zip8.val[1]));
 
   kernel.packet[0] = vget_lane_s32(vreinterpret_s32_s16(zip16.val[0]), 0);
@@ -2960,12 +4284,11 @@
   detail::ptranspose_impl(kernel);
 }
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4uc, 4>& kernel)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4uc, 4>& kernel) {
   const uint8x8_t a = vreinterpret_u8_u32(vset_lane_u32(kernel.packet[2], vdup_n_u32(kernel.packet[0]), 1));
   const uint8x8_t b = vreinterpret_u8_u32(vset_lane_u32(kernel.packet[3], vdup_n_u32(kernel.packet[1]), 1));
 
-  const uint8x8x2_t zip8 = vzip_u8(a,b);
+  const uint8x8x2_t zip8 = vzip_u8(a, b);
   const uint16x4x2_t zip16 = vzip_u16(vreinterpret_u16_u8(zip8.val[0]), vreinterpret_u16_u8(zip8.val[1]));
 
   kernel.packet[0] = vget_lane_u32(vreinterpret_u32_u16(zip16.val[0]), 0);
@@ -3013,7 +4336,7 @@
   detail::ptranspose_impl(kernel);
 }
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4i, 4>& kernel) {
-    detail::ptranspose_impl(kernel);
+  detail::ptranspose_impl(kernel);
 }
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2ui, 2>& kernel) {
   detail::zip_in_place(kernel.packet[0], kernel.packet[1]);
@@ -3022,158 +4345,195 @@
   detail::ptranspose_impl(kernel);
 }
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void
-ptranspose(PacketBlock<Packet2l, 2>& kernel)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2l, 2>& kernel) {
 #if EIGEN_ARCH_ARM64
   const int64x2_t tmp1 = vzip1q_s64(kernel.packet[0], kernel.packet[1]);
   kernel.packet[1] = vzip2q_s64(kernel.packet[0], kernel.packet[1]);
   kernel.packet[0] = tmp1;
 #else
-  const int64x1_t tmp[2][2] = {
-    { vget_low_s64(kernel.packet[0]), vget_high_s64(kernel.packet[0]) },
-    { vget_low_s64(kernel.packet[1]), vget_high_s64(kernel.packet[1]) }
-  };
+  const int64x1_t tmp[2][2] = {{vget_low_s64(kernel.packet[0]), vget_high_s64(kernel.packet[0])},
+                               {vget_low_s64(kernel.packet[1]), vget_high_s64(kernel.packet[1])}};
 
   kernel.packet[0] = vcombine_s64(tmp[0][0], tmp[1][0]);
   kernel.packet[1] = vcombine_s64(tmp[0][1], tmp[1][1]);
 #endif
 }
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void
-ptranspose(PacketBlock<Packet2ul, 2>& kernel)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2ul, 2>& kernel) {
 #if EIGEN_ARCH_ARM64
   const uint64x2_t tmp1 = vzip1q_u64(kernel.packet[0], kernel.packet[1]);
   kernel.packet[1] = vzip2q_u64(kernel.packet[0], kernel.packet[1]);
   kernel.packet[0] = tmp1;
 #else
-  const uint64x1_t tmp[2][2] = {
-    { vget_low_u64(kernel.packet[0]), vget_high_u64(kernel.packet[0]) },
-    { vget_low_u64(kernel.packet[1]), vget_high_u64(kernel.packet[1]) }
-  };
+  const uint64x1_t tmp[2][2] = {{vget_low_u64(kernel.packet[0]), vget_high_u64(kernel.packet[0])},
+                                {vget_low_u64(kernel.packet[1]), vget_high_u64(kernel.packet[1])}};
 
   kernel.packet[0] = vcombine_u64(tmp[0][0], tmp[1][0]);
   kernel.packet[1] = vcombine_u64(tmp[0][1], tmp[1][1]);
 #endif
 }
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2f pselect( const Packet2f& mask, const Packet2f& a, const Packet2f& b)
-{ return vbsl_f32(vreinterpret_u32_f32(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4f pselect(const Packet4f& mask, const Packet4f& a, const Packet4f& b)
-{ return vbslq_f32(vreinterpretq_u32_f32(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c pselect(const Packet8c& mask, const Packet8c& a, const Packet8c& b)
-{ return vbsl_s8(vreinterpret_u8_s8(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16c pselect(const Packet16c& mask, const Packet16c& a, const Packet16c& b)
-{ return vbslq_s8(vreinterpretq_u8_s8(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc pselect(const Packet8uc& mask, const Packet8uc& a, const Packet8uc& b)
-{ return vbsl_u8(mask, a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16uc pselect(const Packet16uc& mask, const Packet16uc& a, const Packet16uc& b)
-{ return vbslq_u8(mask, a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s pselect(const Packet4s& mask, const Packet4s& a, const Packet4s& b)
-{ return vbsl_s16(vreinterpret_u16_s16(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8s pselect(const Packet8s& mask, const Packet8s& a, const Packet8s& b)
-{ return vbslq_s16(vreinterpretq_u16_s16(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us pselect(const Packet4us& mask, const Packet4us& a, const Packet4us& b)
-{ return vbsl_u16(mask, a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8us pselect(const Packet8us& mask, const Packet8us& a, const Packet8us& b)
-{ return vbslq_u16(mask, a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2i pselect(const Packet2i& mask, const Packet2i& a, const Packet2i& b)
-{ return vbsl_s32(vreinterpret_u32_s32(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4i pselect(const Packet4i& mask, const Packet4i& a, const Packet4i& b)
-{ return vbslq_s32(vreinterpretq_u32_s32(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ui pselect(const Packet2ui& mask, const Packet2ui& a, const Packet2ui& b)
-{ return vbsl_u32(mask, a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4ui pselect(const Packet4ui& mask, const Packet4ui& a, const Packet4ui& b)
-{ return vbslq_u32(mask, a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2l pselect(const Packet2l& mask, const Packet2l& a, const Packet2l& b)
-{ return vbslq_s64(vreinterpretq_u64_s64(mask), a, b); }
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ul pselect(const Packet2ul& mask, const Packet2ul& a, const Packet2ul& b)
-{ return vbslq_u64(mask, a, b); }
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2f pselect(const Packet2f& mask, const Packet2f& a, const Packet2f& b) {
+  return vbsl_f32(vreinterpret_u32_f32(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4f pselect(const Packet4f& mask, const Packet4f& a, const Packet4f& b) {
+  return vbslq_f32(vreinterpretq_u32_f32(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c pselect(const Packet8c& mask, const Packet8c& a, const Packet8c& b) {
+  return vbsl_s8(vreinterpret_u8_s8(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16c pselect(const Packet16c& mask, const Packet16c& a, const Packet16c& b) {
+  return vbslq_s8(vreinterpretq_u8_s8(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc pselect(const Packet8uc& mask, const Packet8uc& a, const Packet8uc& b) {
+  return vbsl_u8(mask, a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16uc pselect(const Packet16uc& mask, const Packet16uc& a,
+                                                         const Packet16uc& b) {
+  return vbslq_u8(mask, a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s pselect(const Packet4s& mask, const Packet4s& a, const Packet4s& b) {
+  return vbsl_s16(vreinterpret_u16_s16(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8s pselect(const Packet8s& mask, const Packet8s& a, const Packet8s& b) {
+  return vbslq_s16(vreinterpretq_u16_s16(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us pselect(const Packet4us& mask, const Packet4us& a, const Packet4us& b) {
+  return vbsl_u16(mask, a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8us pselect(const Packet8us& mask, const Packet8us& a, const Packet8us& b) {
+  return vbslq_u16(mask, a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2i pselect(const Packet2i& mask, const Packet2i& a, const Packet2i& b) {
+  return vbsl_s32(vreinterpret_u32_s32(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4i pselect(const Packet4i& mask, const Packet4i& a, const Packet4i& b) {
+  return vbslq_s32(vreinterpretq_u32_s32(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ui pselect(const Packet2ui& mask, const Packet2ui& a, const Packet2ui& b) {
+  return vbsl_u32(mask, a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4ui pselect(const Packet4ui& mask, const Packet4ui& a, const Packet4ui& b) {
+  return vbslq_u32(mask, a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2l pselect(const Packet2l& mask, const Packet2l& a, const Packet2l& b) {
+  return vbslq_s64(vreinterpretq_u64_s64(mask), a, b);
+}
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ul pselect(const Packet2ul& mask, const Packet2ul& a, const Packet2ul& b) {
+  return vbslq_u64(mask, a, b);
+}
 
 // Use armv8 rounding intinsics if available.
 #if EIGEN_ARCH_ARMV8
-template<> EIGEN_STRONG_INLINE Packet2f print<Packet2f>(const Packet2f& a)
-{ return vrndn_f32(a); }
+template <>
+EIGEN_STRONG_INLINE Packet2f print<Packet2f>(const Packet2f& a) {
+  return vrndn_f32(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f print<Packet4f>(const Packet4f& a)
-{ return vrndnq_f32(a); }
+template <>
+EIGEN_STRONG_INLINE Packet4f print<Packet4f>(const Packet4f& a) {
+  return vrndnq_f32(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pfloor<Packet2f>(const Packet2f& a)
-{ return vrndm_f32(a); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pfloor<Packet2f>(const Packet2f& a) {
+  return vrndm_f32(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a)
-{ return vrndmq_f32(a); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a) {
+  return vrndmq_f32(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2f pceil<Packet2f>(const Packet2f& a)
-{ return vrndp_f32(a); }
+template <>
+EIGEN_STRONG_INLINE Packet2f pceil<Packet2f>(const Packet2f& a) {
+  return vrndp_f32(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a)
-{ return vrndpq_f32(a); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a) {
+  return vrndpq_f32(a);
+}
 
 #else
 
-template<> EIGEN_STRONG_INLINE Packet4f print(const Packet4f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4f print(const Packet4f& a) {
   // Adds and subtracts signum(a) * 2^23 to force rounding.
-  const Packet4f limit = pset1<Packet4f>(static_cast<float>(1<<23));
+  const Packet4f limit = pset1<Packet4f>(static_cast<float>(1 << 23));
   const Packet4f abs_a = pabs(a);
   Packet4f r = padd(abs_a, limit);
   // Don't compile-away addition and subtraction.
   EIGEN_OPTIMIZATION_BARRIER(r);
   r = psub(r, limit);
   // If greater than limit, simply return a.  Otherwise, account for sign.
-  r = pselect(pcmp_lt(abs_a, limit),
-              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  r = pselect(pcmp_lt(abs_a, limit), pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
   return r;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f print(const Packet2f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2f print(const Packet2f& a) {
   // Adds and subtracts signum(a) * 2^23 to force rounding.
-  const Packet2f limit = pset1<Packet2f>(static_cast<float>(1<<23));
+  const Packet2f limit = pset1<Packet2f>(static_cast<float>(1 << 23));
   const Packet2f abs_a = pabs(a);
   Packet2f r = padd(abs_a, limit);
   // Don't compile-away addition and subtraction.
   EIGEN_OPTIMIZATION_BARRIER(r);
   r = psub(r, limit);
   // If greater than limit, simply return a.  Otherwise, account for sign.
-  r = pselect(pcmp_lt(abs_a, limit),
-              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  r = pselect(pcmp_lt(abs_a, limit), pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
   return r;
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a) {
   const Packet4f cst_1 = pset1<Packet4f>(1.0f);
-  Packet4f tmp  = print<Packet4f>(a);
+  Packet4f tmp = print<Packet4f>(a);
   // If greater, subtract one.
   Packet4f mask = pcmp_lt(a, tmp);
   mask = pand(mask, cst_1);
   return psub(tmp, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pfloor<Packet2f>(const Packet2f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2f pfloor<Packet2f>(const Packet2f& a) {
   const Packet2f cst_1 = pset1<Packet2f>(1.0f);
-  Packet2f tmp  = print<Packet2f>(a);
+  Packet2f tmp = print<Packet2f>(a);
   // If greater, subtract one.
   Packet2f mask = pcmp_lt(a, tmp);
   mask = pand(mask, cst_1);
   return psub(tmp, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a) {
   const Packet4f cst_1 = pset1<Packet4f>(1.0f);
-  Packet4f tmp  = print<Packet4f>(a);
+  Packet4f tmp = print<Packet4f>(a);
   // If smaller, add one.
   Packet4f mask = pcmp_lt(tmp, a);
   mask = pand(mask, cst_1);
   return padd(tmp, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f pceil<Packet2f>(const Packet2f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2f pceil<Packet2f>(const Packet2f& a) {
   const Packet2f cst_1 = pset1<Packet2f>(1.0);
-  Packet2f tmp  = print<Packet2f>(a);
+  Packet2f tmp = print<Packet2f>(a);
   // If smaller, add one.
   Packet2f mask = pcmp_lt(tmp, a);
   mask = pand(mask, cst_1);
@@ -3188,12 +4548,12 @@
  *   and tests whether setting that digit to 1 would cause the square of the value to be greater than the argument
  *   value. The algorithm is described in detail here: http://ww1.microchip.com/downloads/en/AppNotes/91040a.pdf .
  */
-template<> EIGEN_STRONG_INLINE Packet4uc psqrt(const Packet4uc& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4uc psqrt(const Packet4uc& a) {
   uint8x8_t x = vreinterpret_u8_u32(vdup_n_u32(a));
   uint8x8_t res = vdup_n_u8(0);
   uint8x8_t add = vdup_n_u8(0x8);
-  for (int i = 0; i < 4; i++)
-  {
+  for (int i = 0; i < 4; i++) {
     const uint8x8_t temp = vorr_u8(res, add);
     res = vbsl_u8(vcge_u8(x, vmul_u8(temp, temp)), temp, res);
     add = vshr_n_u8(add, 1);
@@ -3201,11 +4561,11 @@
   return vget_lane_u32(vreinterpret_u32_u8(res), 0);
 }
 /// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
-template<> EIGEN_STRONG_INLINE Packet8uc psqrt(const Packet8uc& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8uc psqrt(const Packet8uc& a) {
   uint8x8_t res = vdup_n_u8(0);
   uint8x8_t add = vdup_n_u8(0x8);
-  for (int i = 0; i < 4; i++)
-  {
+  for (int i = 0; i < 4; i++) {
     const uint8x8_t temp = vorr_u8(res, add);
     res = vbsl_u8(vcge_u8(a, vmul_u8(temp, temp)), temp, res);
     add = vshr_n_u8(add, 1);
@@ -3213,11 +4573,11 @@
   return res;
 }
 /// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
-template<> EIGEN_STRONG_INLINE Packet16uc psqrt(const Packet16uc& a) {
+template <>
+EIGEN_STRONG_INLINE Packet16uc psqrt(const Packet16uc& a) {
   uint8x16_t res = vdupq_n_u8(0);
   uint8x16_t add = vdupq_n_u8(0x8);
-  for (int i = 0; i < 4; i++)
-  {
+  for (int i = 0; i < 4; i++) {
     const uint8x16_t temp = vorrq_u8(res, add);
     res = vbslq_u8(vcgeq_u8(a, vmulq_u8(temp, temp)), temp, res);
     add = vshrq_n_u8(add, 1);
@@ -3225,11 +4585,11 @@
   return res;
 }
 /// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
-template<> EIGEN_STRONG_INLINE Packet4us psqrt(const Packet4us& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4us psqrt(const Packet4us& a) {
   uint16x4_t res = vdup_n_u16(0);
   uint16x4_t add = vdup_n_u16(0x80);
-  for (int i = 0; i < 8; i++)
-  {
+  for (int i = 0; i < 8; i++) {
     const uint16x4_t temp = vorr_u16(res, add);
     res = vbsl_u16(vcge_u16(a, vmul_u16(temp, temp)), temp, res);
     add = vshr_n_u16(add, 1);
@@ -3237,11 +4597,11 @@
   return res;
 }
 /// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
-template<> EIGEN_STRONG_INLINE Packet8us psqrt(const Packet8us& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8us psqrt(const Packet8us& a) {
   uint16x8_t res = vdupq_n_u16(0);
   uint16x8_t add = vdupq_n_u16(0x80);
-  for (int i = 0; i < 8; i++)
-  {
+  for (int i = 0; i < 8; i++) {
     const uint16x8_t temp = vorrq_u16(res, add);
     res = vbslq_u16(vcgeq_u16(a, vmulq_u16(temp, temp)), temp, res);
     add = vshrq_n_u16(add, 1);
@@ -3249,11 +4609,11 @@
   return res;
 }
 /// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
-template<> EIGEN_STRONG_INLINE Packet2ui psqrt(const Packet2ui& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2ui psqrt(const Packet2ui& a) {
   uint32x2_t res = vdup_n_u32(0);
   uint32x2_t add = vdup_n_u32(0x8000);
-  for (int i = 0; i < 16; i++)
-  {
+  for (int i = 0; i < 16; i++) {
     const uint32x2_t temp = vorr_u32(res, add);
     res = vbsl_u32(vcge_u32(a, vmul_u32(temp, temp)), temp, res);
     add = vshr_n_u32(add, 1);
@@ -3261,11 +4621,11 @@
   return res;
 }
 /// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
-template<> EIGEN_STRONG_INLINE Packet4ui psqrt(const Packet4ui& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4ui psqrt(const Packet4ui& a) {
   uint32x4_t res = vdupq_n_u32(0);
   uint32x4_t add = vdupq_n_u32(0x8000);
-  for (int i = 0; i < 16; i++)
-  {
+  for (int i = 0; i < 16; i++) {
     const uint32x4_t temp = vorrq_u32(res, add);
     res = vbslq_u32(vcgeq_u32(a, vmulq_u32(temp, temp)), temp, res);
     add = vshrq_n_u32(add, 1);
@@ -3273,40 +4633,134 @@
   return res;
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f prsqrt(const Packet4f& a) {
+EIGEN_STRONG_INLINE Packet4f prsqrt_float_unsafe(const Packet4f& a) {
   // Compute approximate reciprocal sqrt.
-  Packet4f x = vrsqrteq_f32(a);
-  // Do Newton iterations for 1/sqrt(x).
-  x = vmulq_f32(vrsqrtsq_f32(vmulq_f32(a, x), x), x);
-  x = vmulq_f32(vrsqrtsq_f32(vmulq_f32(a, x), x), x);
-  const Packet4f infinity = pset1<Packet4f>(NumTraits<float>::infinity());
-  return pselect(pcmp_eq(a, pzero(a)), infinity, x);
+  // Does not correctly handle +/- 0 or +inf
+  float32x4_t result = vrsqrteq_f32(a);
+  result = vmulq_f32(vrsqrtsq_f32(vmulq_f32(a, result), result), result);
+  result = vmulq_f32(vrsqrtsq_f32(vmulq_f32(a, result), result), result);
+  return result;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2f prsqrt(const Packet2f& a) {
+EIGEN_STRONG_INLINE Packet2f prsqrt_float_unsafe(const Packet2f& a) {
   // Compute approximate reciprocal sqrt.
-  Packet2f x = vrsqrte_f32(a);
-  // Do Newton iterations for 1/sqrt(x).
-  x = vmul_f32(vrsqrts_f32(vmul_f32(a, x), x), x);
-  x = vmul_f32(vrsqrts_f32(vmul_f32(a, x), x), x);
-  const Packet2f infinity = pset1<Packet2f>(NumTraits<float>::infinity());
-  return pselect(pcmp_eq(a, pzero(a)), infinity, x);
+  // Does not correctly handle +/- 0 or +inf
+  float32x2_t result = vrsqrte_f32(a);
+  result = vmul_f32(vrsqrts_f32(vmul_f32(a, result), result), result);
+  result = vmul_f32(vrsqrts_f32(vmul_f32(a, result), result), result);
+  return result;
+}
+
+template <typename Packet>
+Packet prsqrt_float_common(const Packet& a) {
+  const Packet cst_zero = pzero(a);
+  const Packet cst_inf = pset1<Packet>(NumTraits<float>::infinity());
+  Packet return_zero = pcmp_eq(a, cst_inf);
+  Packet return_inf = pcmp_eq(a, cst_zero);
+  Packet result = prsqrt_float_unsafe(a);
+  result = pselect(return_inf, por(cst_inf, a), result);
+  result = pandnot(result, return_zero);
+  return result;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f prsqrt(const Packet4f& a) {
+  return prsqrt_float_common(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f prsqrt(const Packet2f& a) {
+  return prsqrt_float_common(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f preciprocal<Packet4f>(const Packet4f& a) {
+  // Compute approximate reciprocal.
+  float32x4_t result = vrecpeq_f32(a);
+  result = vmulq_f32(vrecpsq_f32(a, result), result);
+  result = vmulq_f32(vrecpsq_f32(a, result), result);
+  return result;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f preciprocal<Packet2f>(const Packet2f& a) {
+  // Compute approximate reciprocal.
+  float32x2_t result = vrecpe_f32(a);
+  result = vmul_f32(vrecps_f32(a, result), result);
+  result = vmul_f32(vrecps_f32(a, result), result);
+  return result;
 }
 
 // Unfortunately vsqrt_f32 is only available for A64.
 #if EIGEN_ARCH_ARM64
-template<> EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& _x){return vsqrtq_f32(_x);}
-template<> EIGEN_STRONG_INLINE Packet2f psqrt(const Packet2f& _x){return vsqrt_f32(_x); }
-#else
-template<> EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& a) {
-  const Packet4f infinity = pset1<Packet4f>(NumTraits<float>::infinity());
-  const Packet4f is_zero_or_inf = por(pcmp_eq(a, pzero(a)), pcmp_eq(a, infinity));
-  return pselect(is_zero_or_inf, a, pmul(a, prsqrt(a)));
+template <>
+EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& a) {
+  return vsqrtq_f32(a);
 }
-template<> EIGEN_STRONG_INLINE Packet2f psqrt(const Packet2f& a) {
-  const Packet2f infinity = pset1<Packet2f>(NumTraits<float>::infinity());
-  const Packet2f is_zero_or_inf = por(pcmp_eq(a, pzero(a)), pcmp_eq(a, infinity));
-  return pselect(is_zero_or_inf, a, pmul(a, prsqrt(a)));
+
+template <>
+EIGEN_STRONG_INLINE Packet2f psqrt(const Packet2f& a) {
+  return vsqrt_f32(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f pdiv(const Packet4f& a, const Packet4f& b) {
+  return vdivq_f32(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f pdiv(const Packet2f& a, const Packet2f& b) {
+  return vdiv_f32(a, b);
+}
+#else
+template <typename Packet>
+EIGEN_STRONG_INLINE Packet psqrt_float_common(const Packet& a) {
+  const Packet cst_zero = pzero(a);
+  const Packet cst_inf = pset1<Packet>(NumTraits<float>::infinity());
+
+  Packet result = pmul(a, prsqrt_float_unsafe(a));
+  Packet a_is_zero = pcmp_eq(a, cst_zero);
+  Packet a_is_inf = pcmp_eq(a, cst_inf);
+  Packet return_a = por(a_is_zero, a_is_inf);
+
+  result = pselect(return_a, a, result);
+  return result;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& a) {
+  return psqrt_float_common(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f psqrt(const Packet2f& a) {
+  return psqrt_float_common(a);
+}
+
+template <typename Packet>
+EIGEN_STRONG_INLINE Packet pdiv_float_common(const Packet& a, const Packet& b) {
+  // if b is large, NEON intrinsics will flush preciprocal(b) to zero
+  // avoid underflow with the following manipulation:
+  // a / b = f * (a * reciprocal(f * b))
+
+  const Packet cst_one = pset1<Packet>(1.0f);
+  const Packet cst_quarter = pset1<Packet>(0.25f);
+  const Packet cst_thresh = pset1<Packet>(NumTraits<float>::highest() / 4.0f);
+
+  Packet b_will_underflow = pcmp_le(cst_thresh, pabs(b));
+  Packet f = pselect(b_will_underflow, cst_quarter, cst_one);
+  Packet result = pmul(f, pmul(a, preciprocal(pmul(b, f))));
+  return result;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return pdiv_float_common(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f pdiv<Packet2f>(const Packet2f& a, const Packet2f& b) {
+  return pdiv_float_common(a, b);
 }
 #endif
 
@@ -3316,57 +4770,57 @@
 // TODO: Guard if we have native bfloat16 support
 typedef eigen_packet_wrapper<uint16x4_t, 19> Packet4bf;
 
-template<> struct is_arithmetic<Packet4bf> { enum { value = true }; };
+template <>
+struct is_arithmetic<Packet4bf> {
+  enum { value = true };
+};
 
-template<> struct packet_traits<bfloat16> : default_packet_traits
-{
+template <>
+struct packet_traits<bfloat16> : default_packet_traits {
   typedef Packet4bf type;
   typedef Packet4bf half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket = 0,
 
-    HasCmp       = 1,
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0,
-    HasDiv       = 1,
-    HasFloor     = 1,
-    HasCeil      = 1,
-    HasRint      = 1,
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0,
+    HasDiv = 1,
+    HasFloor = 1,
+    HasCeil = 1,
+    HasRint = 1,
 
-    HasSin  = EIGEN_FAST_MATH,
-    HasCos  = EIGEN_FAST_MATH,
-    HasLog  = 1,
-    HasExp  = 1,
+    HasSin = EIGEN_FAST_MATH,
+    HasCos = EIGEN_FAST_MATH,
+    HasLog = 1,
+    HasExp = 1,
     HasSqrt = 0,
     HasTanh = EIGEN_FAST_MATH,
-    HasErf  = EIGEN_FAST_MATH,
+    HasErf = EIGEN_FAST_MATH,
     HasBessel = 0,  // Issues with accuracy.
     HasNdtri = 0
   };
 };
 
-template<> struct unpacket_traits<Packet4bf>
-{
+template <>
+struct unpacket_traits<Packet4bf> {
   typedef bfloat16 type;
   typedef Packet4bf half;
-  enum
-  {
+  enum {
     size = 4,
     alignment = Aligned16,
     vectorizable = true,
@@ -3375,23 +4829,22 @@
   };
 };
 
-namespace detail {  
-template<>
+namespace detail {
+template <>
 EIGEN_ALWAYS_INLINE void zip_in_place<Packet4bf>(Packet4bf& p1, Packet4bf& p2) {
   const uint16x4x2_t tmp = vzip_u16(p1, p2);
   p1 = tmp.val[0];
   p2 = tmp.val[1];
 }
-} // namespace detail
+}  // namespace detail
 
-EIGEN_STRONG_INLINE Packet4bf F32ToBf16(const Packet4f& p)
-{
-  // See the scalar implemention in BFloat16.h for a comprehensible explanation
+EIGEN_STRONG_INLINE Packet4bf F32ToBf16(const Packet4f& p) {
+  // See the scalar implementation in BFloat16.h for a comprehensible explanation
   // of this fast rounding algorithm
-  Packet4ui input = reinterpret_cast<Packet4ui>(p);
+  Packet4ui input = Packet4ui(vreinterpretq_u32_f32(p));
 
   // lsb = (input >> 16) & 1
-  Packet4ui lsb =  vandq_u32(vshrq_n_u32(input, 16), vdupq_n_u32(1));
+  Packet4ui lsb = vandq_u32(vshrq_n_u32(input, 16), vdupq_n_u32(1));
 
   // rounding_bias = 0x7fff + lsb
   Packet4ui rounding_bias = vaddq_u32(lsb, vdupq_n_u32(0x7fff));
@@ -3411,320 +4864,334 @@
   return vmovn_u32(input);
 }
 
-EIGEN_STRONG_INLINE Packet4f Bf16ToF32(const Packet4bf& p)
-{
-  return reinterpret_cast<Packet4f>(vshlq_n_u32(vmovl_u16(p), 16));
+EIGEN_STRONG_INLINE Packet4f Bf16ToF32(const Packet4bf& p) {
+  return Packet4f(vreinterpretq_f32_u32(vshlq_n_u32(vmovl_u16(p), 16)));
 }
 
-EIGEN_STRONG_INLINE Packet4bf F32MaskToBf16Mask(const Packet4f& p) {
-  return vmovn_u32(vreinterpretq_u32_f32(p));
+EIGEN_STRONG_INLINE Packet4bf F32MaskToBf16Mask(const Packet4f& p) { return vmovn_u32(vreinterpretq_u32_f32(p)); }
+
+template <>
+EIGEN_STRONG_INLINE Packet4bf pset1<Packet4bf>(const bfloat16& from) {
+  return Packet4bf(pset1<Packet4us>(from.value));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pset1<Packet4bf>(const bfloat16& from) {
-  return pset1<Packet4us>(from.value);
+template <>
+EIGEN_STRONG_INLINE bfloat16 pfirst<Packet4bf>(const Packet4bf& from) {
+  return bfloat16_impl::raw_uint16_to_bfloat16(static_cast<uint16_t>(pfirst<Packet4us>(Packet4us(from))));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 pfirst<Packet4bf>(const Packet4bf& from) {
-  return bfloat16_impl::raw_uint16_to_bfloat16(static_cast<uint16_t>(pfirst<Packet4us>(from)));
+template <>
+EIGEN_STRONG_INLINE Packet4bf pload<Packet4bf>(const bfloat16* from) {
+  return Packet4bf(pload<Packet4us>(reinterpret_cast<const uint16_t*>(from)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pload<Packet4bf>(const bfloat16* from)
-{
-  return pload<Packet4us>(reinterpret_cast<const uint16_t*>(from));
+template <>
+EIGEN_STRONG_INLINE Packet4bf ploadu<Packet4bf>(const bfloat16* from) {
+  return Packet4bf(ploadu<Packet4us>(reinterpret_cast<const uint16_t*>(from)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf ploadu<Packet4bf>(const bfloat16* from)
-{
-  return ploadu<Packet4us>(reinterpret_cast<const uint16_t*>(from));
-}
-
-template<> EIGEN_STRONG_INLINE void pstore<bfloat16>(bfloat16* to, const Packet4bf& from)
-{
+template <>
+EIGEN_STRONG_INLINE void pstore<bfloat16>(bfloat16* to, const Packet4bf& from) {
   EIGEN_DEBUG_ALIGNED_STORE vst1_u16(reinterpret_cast<uint16_t*>(to), from);
 }
 
-template<> EIGEN_STRONG_INLINE void pstoreu<bfloat16>(bfloat16* to, const Packet4bf& from)
-{
+template <>
+EIGEN_STRONG_INLINE void pstoreu<bfloat16>(bfloat16* to, const Packet4bf& from) {
   EIGEN_DEBUG_UNALIGNED_STORE vst1_u16(reinterpret_cast<uint16_t*>(to), from);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf ploaddup<Packet4bf>(const bfloat16* from)
-{
-  return ploaddup<Packet4us>(reinterpret_cast<const uint16_t*>(from));
+template <>
+EIGEN_STRONG_INLINE Packet4bf ploaddup<Packet4bf>(const bfloat16* from) {
+  return Packet4bf(ploaddup<Packet4us>(reinterpret_cast<const uint16_t*>(from)));
 }
 
-template <> EIGEN_STRONG_INLINE Packet4bf pabs(const Packet4bf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4bf pabs(const Packet4bf& a) {
   return F32ToBf16(pabs<Packet4f>(Bf16ToF32(a)));
 }
 
-template <> EIGEN_STRONG_INLINE Packet4bf pmin<PropagateNumbers, Packet4bf>(const Packet4bf &a,
-                                                                            const Packet4bf &b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pmin<PropagateNumbers, Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pmin<PropagateNumbers, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
-template <> EIGEN_STRONG_INLINE Packet4bf pmin<PropagateNaN, Packet4bf>(const Packet4bf &a,
-                                                                        const Packet4bf &b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pmin<PropagateNaN, Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pmin<PropagateNaN, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template <> EIGEN_STRONG_INLINE Packet4bf pmin<Packet4bf>(const Packet4bf &a,
-                                                          const Packet4bf &b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pmin<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pmin<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template <> EIGEN_STRONG_INLINE Packet4bf pmax<PropagateNumbers, Packet4bf>(const Packet4bf &a,
-                                                                            const Packet4bf &b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pmax<PropagateNumbers, Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pmax<PropagateNumbers, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
-template <> EIGEN_STRONG_INLINE Packet4bf pmax<PropagateNaN, Packet4bf>(const Packet4bf &a,
-                                                                        const Packet4bf &b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pmax<PropagateNaN, Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pmax<PropagateNaN, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template <> EIGEN_STRONG_INLINE Packet4bf pmax<Packet4bf>(const Packet4bf &a,
-                                                          const Packet4bf &b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pmax<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pmax<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf plset<Packet4bf>(const bfloat16& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf plset<Packet4bf>(const bfloat16& a) {
   return F32ToBf16(plset<Packet4f>(static_cast<float>(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf por(const Packet4bf& a,const Packet4bf& b) {
-  return por<Packet4us>(a, b);
+template <>
+EIGEN_STRONG_INLINE Packet4bf por(const Packet4bf& a, const Packet4bf& b) {
+  return Packet4bf(por<Packet4us>(Packet4us(a), Packet4us(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pxor(const Packet4bf& a,const Packet4bf& b) {
-  return pxor<Packet4us>(a, b);
+template <>
+EIGEN_STRONG_INLINE Packet4bf pxor(const Packet4bf& a, const Packet4bf& b) {
+  return Packet4bf(pxor<Packet4us>(Packet4us(a), Packet4us(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pand(const Packet4bf& a,const Packet4bf& b) {
-  return pand<Packet4us>(a, b);
+template <>
+EIGEN_STRONG_INLINE Packet4bf pand(const Packet4bf& a, const Packet4bf& b) {
+  return Packet4bf(pand<Packet4us>(Packet4us(a), Packet4us(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pandnot(const Packet4bf& a,const Packet4bf& b) {
-  return pandnot<Packet4us>(a, b);
+template <>
+EIGEN_STRONG_INLINE Packet4bf pandnot(const Packet4bf& a, const Packet4bf& b) {
+  return Packet4bf(pandnot<Packet4us>(Packet4us(a), Packet4us(b)));
 }
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4bf pselect(const Packet4bf& mask, const Packet4bf& a,
-                                                      const Packet4bf& b)
-{
-  return pselect<Packet4us>(mask, a, b);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4bf pselect(const Packet4bf& mask, const Packet4bf& a, const Packet4bf& b) {
+  return Packet4bf(pselect<Packet4us>(Packet4us(mask), Packet4us(a), Packet4us(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf print<Packet4bf>(const Packet4bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf print<Packet4bf>(const Packet4bf& a) {
   return F32ToBf16(print<Packet4f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pfloor<Packet4bf>(const Packet4bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pfloor<Packet4bf>(const Packet4bf& a) {
   return F32ToBf16(pfloor<Packet4f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pceil<Packet4bf>(const Packet4bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pceil<Packet4bf>(const Packet4bf& a) {
   return F32ToBf16(pceil<Packet4f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pconj(const Packet4bf& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet4bf pconj(const Packet4bf& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE Packet4bf padd<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet4bf padd<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(padd<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf psub<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet4bf psub<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(psub<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pmul<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet4bf pmul<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pmul<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pdiv<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+template <>
+EIGEN_STRONG_INLINE Packet4bf pdiv<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pdiv<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<>
-EIGEN_STRONG_INLINE Packet4bf pgather<bfloat16, Packet4bf>(const bfloat16* from, Index stride)
-{
-  return pgather<uint16_t, Packet4us>(reinterpret_cast<const uint16_t*>(from), stride);
+template <>
+EIGEN_STRONG_INLINE Packet4bf pgather<bfloat16, Packet4bf>(const bfloat16* from, Index stride) {
+  return Packet4bf(pgather<uint16_t, Packet4us>(reinterpret_cast<const uint16_t*>(from), stride));
 }
 
-template<>
-EIGEN_STRONG_INLINE void pscatter<bfloat16, Packet4bf>(bfloat16* to, const Packet4bf& from, Index stride)
-{
-  pscatter<uint16_t, Packet4us>(reinterpret_cast<uint16_t*>(to), from, stride);
+template <>
+EIGEN_STRONG_INLINE void pscatter<bfloat16, Packet4bf>(bfloat16* to, const Packet4bf& from, Index stride) {
+  pscatter<uint16_t, Packet4us>(reinterpret_cast<uint16_t*>(to), Packet4us(from), stride);
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux<Packet4bf>(const Packet4bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux<Packet4bf>(const Packet4bf& a) {
   return static_cast<bfloat16>(predux<Packet4f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux_max<Packet4bf>(const Packet4bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux_max<Packet4bf>(const Packet4bf& a) {
   return static_cast<bfloat16>(predux_max<Packet4f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux_min<Packet4bf>(const Packet4bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux_min<Packet4bf>(const Packet4bf& a) {
   return static_cast<bfloat16>(predux_min<Packet4f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE bfloat16 predux_mul<Packet4bf>(const Packet4bf& a)
-{
+template <>
+EIGEN_STRONG_INLINE bfloat16 predux_mul<Packet4bf>(const Packet4bf& a) {
   return static_cast<bfloat16>(predux_mul<Packet4f>(Bf16ToF32(a)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf preverse<Packet4bf>(const Packet4bf& a)
-{
-  return preverse<Packet4us>(a);
+template <>
+EIGEN_STRONG_INLINE Packet4bf preverse<Packet4bf>(const Packet4bf& a) {
+  return Packet4bf(preverse<Packet4us>(Packet4us(a)));
 }
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4bf, 4>& kernel)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4bf, 4>& kernel) {
   detail::ptranspose_impl(kernel);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pabsdiff<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pabsdiff<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32ToBf16(pabsdiff<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pcmp_eq<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pcmp_eq<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32MaskToBf16Mask(pcmp_eq<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pcmp_lt<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pcmp_lt<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32MaskToBf16Mask(pcmp_lt<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pcmp_lt_or_nan<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pcmp_lt_or_nan<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32MaskToBf16Mask(pcmp_lt_or_nan<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pcmp_le<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4bf pcmp_le<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
   return F32MaskToBf16Mask(pcmp_le<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4bf pnegate<Packet4bf>(const Packet4bf& a)
-{
-  return pxor<Packet4us>(a, pset1<Packet4us>(static_cast<uint16_t>(0x8000)));
+template <>
+EIGEN_STRONG_INLINE Packet4bf pnegate<Packet4bf>(const Packet4bf& a) {
+  return Packet4bf(pxor<Packet4us>(Packet4us(a), pset1<Packet4us>(static_cast<uint16_t>(0x8000))));
 }
 
 //---------- double ----------
 
 // Clang 3.5 in the iOS toolchain has an ICE triggered by NEON intrisics for double.
 // Confirmed at least with __apple_build_version__ = 6000054.
-#ifdef __apple_build_version__
+#if EIGEN_COMP_CLANGAPPLE
 // Let's hope that by the time __apple_build_version__ hits the 601* range, the bug will be fixed.
 // https://gist.github.com/yamaya/2924292 suggests that the 3 first digits are only updated with
 // major toolchain updates.
-#define EIGEN_APPLE_DOUBLE_NEON_BUG (__apple_build_version__ < 6010000)
+#define EIGEN_APPLE_DOUBLE_NEON_BUG (EIGEN_COMP_CLANGAPPLE < 6010000)
 #else
 #define EIGEN_APPLE_DOUBLE_NEON_BUG 0
 #endif
 
 #if EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG
 
+#if EIGEN_COMP_GNUC
 // Bug 907: workaround missing declarations of the following two functions in the ADK
 // Defining these functions as templates ensures that if these intrinsics are
 // already defined in arm_neon.h, then our workaround doesn't cause a conflict
 // and has lower priority in overload resolution.
-template <typename T> uint64x2_t vreinterpretq_u64_f64(T a) { return (uint64x2_t) a; }
+// This doesn't work with MSVC though, since the function names are macros.
+template <typename T>
+uint64x2_t vreinterpretq_u64_f64(T a) {
+  return (uint64x2_t)a;
+}
 
-template <typename T> float64x2_t vreinterpretq_f64_u64(T a) { return (float64x2_t) a; }
+template <typename T>
+float64x2_t vreinterpretq_f64_u64(T a) {
+  return (float64x2_t)a;
+}
+#endif
 
+#if EIGEN_COMP_MSVC_STRICT
+typedef eigen_packet_wrapper<float64x2_t, 18> Packet2d;
+typedef eigen_packet_wrapper<float64x1_t, 19> Packet1d;
+
+EIGEN_ALWAYS_INLINE Packet2d make_packet2d(double a, double b) {
+  double from[2] = {a, b};
+  return vld1q_f64(from);
+}
+
+#else
 typedef float64x2_t Packet2d;
 typedef float64x1_t Packet1d;
 
+EIGEN_ALWAYS_INLINE Packet2d make_packet2d(double a, double b) { return Packet2d{a, b}; }
+#endif
+
 // fuctionally equivalent to _mm_shuffle_pd in SSE (i.e. shuffle(m, n, mask) equals _mm_shuffle_pd(m,n,mask))
 // Currently used in LU/arch/InverseSize4.h to enable a shared implementation
 // for fast inversion of matrices of size 4.
-EIGEN_STRONG_INLINE Packet2d shuffle(const Packet2d& m, const Packet2d& n, int mask)
-{
+EIGEN_STRONG_INLINE Packet2d shuffle(const Packet2d& m, const Packet2d& n, int mask) {
   const double* a = reinterpret_cast<const double*>(&m);
   const double* b = reinterpret_cast<const double*>(&n);
-  Packet2d res = {*(a + (mask & 1)), *(b + ((mask >> 1) & 1))};
+  Packet2d res = make_packet2d(*(a + (mask & 1)), *(b + ((mask >> 1) & 1)));
   return res;
 }
 
-EIGEN_STRONG_INLINE Packet2d vec2d_swizzle2(const Packet2d& a, const Packet2d& b, int mask)
-{
+EIGEN_STRONG_INLINE Packet2d vec2d_swizzle2(const Packet2d& a, const Packet2d& b, int mask) {
   return shuffle(a, b, mask);
 }
-EIGEN_STRONG_INLINE Packet2d vec2d_unpacklo(const Packet2d& a,const Packet2d& b)
-{
-  return shuffle(a, b, 0);
-}
-EIGEN_STRONG_INLINE Packet2d vec2d_unpackhi(const Packet2d& a,const Packet2d& b)
-{
-  return shuffle(a, b, 3);
-}
-#define vec2d_duplane(a, p) \
-  vdupq_laneq_f64(a, p)
+EIGEN_STRONG_INLINE Packet2d vec2d_unpacklo(const Packet2d& a, const Packet2d& b) { return shuffle(a, b, 0); }
+EIGEN_STRONG_INLINE Packet2d vec2d_unpackhi(const Packet2d& a, const Packet2d& b) { return shuffle(a, b, 3); }
+#define vec2d_duplane(a, p) Packet2d(vdupq_laneq_f64(a, p))
 
-template<> struct packet_traits<double>  : default_packet_traits
-{
+template <>
+struct packet_traits<double> : default_packet_traits {
   typedef Packet2d type;
   typedef Packet2d half;
-  enum
-  {
+  enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 2,
-    HasHalfPacket = 0,
 
-    HasCmp       = 1,
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 1,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 1,
-    HasArg       = 0,
-    HasAbs2      = 1,
-    HasAbsDiff   = 1,
-    HasMin       = 1,
-    HasMax       = 1,
-    HasConj      = 1,
-    HasSetLinear = 0,
-    HasBlend     = 0,
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 1,
+    HasBlend = 0,
 
-    HasDiv   = 1,
+    HasDiv = 1,
     HasFloor = 1,
     HasCeil = 1,
     HasRint = 1,
 
-    HasSin  = 0,
-    HasCos  = 0,
-    HasLog  = 1,
-    HasExp  = 1,
+#if EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG
+    HasExp = 1,
+    HasLog = 1,
+    HasATan = 1,
+#endif
+    HasSin = 0,
+    HasCos = 0,
     HasSqrt = 1,
     HasRsqrt = 1,
     HasTanh = 0,
-    HasErf  = 0
+    HasErf = 0
   };
 };
 
-template<> struct unpacket_traits<Packet2d>
-{
+template <>
+struct unpacket_traits<Packet2d> {
   typedef double type;
   typedef Packet2d half;
   typedef Packet2l integer_packet;
-  enum
-  {
+  enum {
     size = 2,
     alignment = Aligned16,
     vectorizable = true,
@@ -3733,150 +5200,239 @@
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double&  from) { return vdupq_n_f64(from); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double& from) {
+  return vdupq_n_f64(from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d plset<Packet2d>(const double& a)
-{
-  const double c[] = {0.0,1.0};
+template <>
+EIGEN_STRONG_INLINE Packet2d plset<Packet2d>(const double& a) {
+  const double c[] = {0.0, 1.0};
   return vaddq_f64(pset1<Packet2d>(a), vld1q_f64(c));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d padd<Packet2d>(const Packet2d& a, const Packet2d& b) { return vaddq_f64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d padd<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vaddq_f64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d psub<Packet2d>(const Packet2d& a, const Packet2d& b) { return vsubq_f64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d psub<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vsubq_f64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& , const Packet2d& );
-template<> EIGEN_STRONG_INLINE Packet2d paddsub<Packet2d>(const Packet2d& a, const Packet2d& b){
-  const Packet2d mask = {numext::bit_cast<double>(0x8000000000000000ull),0.0};
+template <>
+EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d&, const Packet2d&);
+template <>
+EIGEN_STRONG_INLINE Packet2d paddsub<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  const Packet2d mask = make_packet2d(numext::bit_cast<double>(0x8000000000000000ull), 0.0);
   return padd(a, pxor(mask, b));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d pnegate(const Packet2d& a) { return vnegq_f64(a); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pnegate(const Packet2d& a) {
+  return vnegq_f64(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pconj(const Packet2d& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet2d pconj(const Packet2d& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pmul<Packet2d>(const Packet2d& a, const Packet2d& b) { return vmulq_f64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pmul<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vmulq_f64(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pdiv<Packet2d>(const Packet2d& a, const Packet2d& b) { return vdivq_f64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pdiv<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vdivq_f64(a, b);
+}
 
 #ifdef __ARM_FEATURE_FMA
 // See bug 936. See above comment about FMA for float.
-template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c)
-{ return vfmaq_f64(c,a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) {
+  return vfmaq_f64(c, a, b);
+}
 #else
-template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c)
-{ return vmlaq_f64(c,a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) {
+  return vmlaq_f64(c, a, b);
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) { return vminq_f64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vminq_f64(a, b);
+}
 
 #ifdef __ARM_FEATURE_NUMERIC_MAXMIN
-// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
-template<> EIGEN_STRONG_INLINE Packet2d pmin<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) { return vminnmq_f64(a, b); }
-template<> EIGEN_STRONG_INLINE Packet2d pmax<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) { return vmaxnmq_f64(a, b); }
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8
+// systems).
+template <>
+EIGEN_STRONG_INLINE Packet2d pmin<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vminnmq_f64(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pmax<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vmaxnmq_f64(a, b);
+}
 
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet2d pmin<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) { return pmin<Packet2d>(a, b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pmin<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return pmin<Packet2d>(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) { return vmaxq_f64(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vmaxq_f64(a, b);
+}
 
-
-template<> EIGEN_STRONG_INLINE Packet2d pmax<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) { return pmax<Packet2d>(a, b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pmax<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return pmax<Packet2d>(a, b);
+}
 
 // Logical Operations are not supported for float, so we have to reinterpret casts using NEON intrinsics
-template<> EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a), vreinterpretq_u64_f64(b)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
+template <>
+EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a), vreinterpretq_u64_f64(b)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a), vreinterpretq_u64_f64(b)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a), vreinterpretq_u64_f64(b)));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_le(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u64(vcleq_f64(a,b)); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_le(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u64(vcleq_f64(a, b));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u64(vcltq_f64(a,b)); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_lt(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u64(vcltq_f64(a, b));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt_or_nan(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u32(vmvnq_u32(vreinterpretq_u32_u64(vcgeq_f64(a,b)))); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_lt_or_nan(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u32(vmvnq_u32(vreinterpretq_u32_u64(vcgeq_f64(a, b))));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_eq(const Packet2d& a, const Packet2d& b)
-{ return vreinterpretq_f64_u64(vceqq_f64(a,b)); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_eq(const Packet2d& a, const Packet2d& b) {
+  return vreinterpretq_f64_u64(vceqq_f64(a, b));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f64(from); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f64(from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f64(from); }
+template <>
+EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f64(from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d ploaddup<Packet2d>(const double* from) { return vld1q_dup_f64(from); }
-template<> EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet2d& from)
-{ EIGEN_DEBUG_ALIGNED_STORE vst1q_f64(to,from); }
+template <>
+EIGEN_STRONG_INLINE Packet2d ploaddup<Packet2d>(const double* from) {
+  return vld1q_dup_f64(from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet2d& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_f64(to, from);
+}
 
-template<> EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet2d& from)
-{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_f64(to,from); }
+template <>
+EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet2d& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_f64(to, from);
+}
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2d pgather<double, Packet2d>(const double* from, Index stride)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2d pgather<double, Packet2d>(const double* from, Index stride) {
   Packet2d res = pset1<Packet2d>(0.0);
-  res = vld1q_lane_f64(from + 0*stride, res, 0);
-  res = vld1q_lane_f64(from + 1*stride, res, 1);
+  res = vld1q_lane_f64(from + 0 * stride, res, 0);
+  res = vld1q_lane_f64(from + 1 * stride, res, 1);
   return res;
 }
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<double, Packet2d>(double* to, const Packet2d& from, Index stride)
-{
-  vst1q_lane_f64(to + stride*0, from, 0);
-  vst1q_lane_f64(to + stride*1, from, 1);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<double, Packet2d>(double* to, const Packet2d& from, Index stride) {
+  vst1q_lane_f64(to + stride * 0, from, 0);
+  vst1q_lane_f64(to + stride * 1, from, 1);
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { EIGEN_ARM_PREFETCH(addr); }
+template <>
+EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
 
 // FIXME only store the 2 first elements ?
-template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return vgetq_lane_f64(a,0); }
+template <>
+EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) {
+  return vgetq_lane_f64(a, 0);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a)
-{ return vcombine_f64(vget_high_f64(a), vget_low_f64(a)); }
+template <>
+EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a) {
+  return vcombine_f64(vget_high_f64(a), vget_low_f64(a));
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a) { return vabsq_f64(a); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a) {
+  return vabsq_f64(a);
+}
 
-#if EIGEN_COMP_CLANG && defined(__apple_build_version__)
-// workaround ICE, see bug 907
-template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a)
-{ return (vget_low_f64(a) + vget_high_f64(a))[0]; }
-#else
-template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a)
-{ return vget_lane_f64(vget_low_f64(a) + vget_high_f64(a), 0); }
-#endif
+template <>
+EIGEN_STRONG_INLINE Packet2d psignbit(const Packet2d& a) {
+  return vreinterpretq_f64_s64(vshrq_n_s64(vreinterpretq_s64_f64(a), 63));
+}
+
+template <>
+EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a) {
+  return vaddvq_f64(a);
+}
 
 // Other reduction functions:
 // mul
-#if EIGEN_COMP_CLANG && defined(__apple_build_version__)
-template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a)
-{ return (vget_low_f64(a) * vget_high_f64(a))[0]; }
+#if EIGEN_COMP_CLANGAPPLE
+template <>
+EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a) {
+  return (vget_low_f64(a) * vget_high_f64(a))[0];
+}
 #else
-template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a)
-{ return vget_lane_f64(vget_low_f64(a) * vget_high_f64(a), 0); }
+template <>
+EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a) {
+  return vget_lane_f64(vmul_f64(vget_low_f64(a), vget_high_f64(a)), 0);
+}
 #endif
 
 // min
-template<> EIGEN_STRONG_INLINE double predux_min<Packet2d>(const Packet2d& a)
-{ return vgetq_lane_f64(vpminq_f64(a,a), 0); }
+template <>
+EIGEN_STRONG_INLINE double predux_min<Packet2d>(const Packet2d& a) {
+  return vminvq_f64(a);
+}
 
 // max
-template<> EIGEN_STRONG_INLINE double predux_max<Packet2d>(const Packet2d& a)
-{ return vgetq_lane_f64(vpmaxq_f64(a,a), 0); }
+template <>
+EIGEN_STRONG_INLINE double predux_max<Packet2d>(const Packet2d& a) {
+  return vmaxvq_f64(a);
+}
 
-
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void
-ptranspose(PacketBlock<Packet2d, 2>& kernel)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2d, 2>& kernel) {
   const float64x2_t tmp1 = vzip1q_f64(kernel.packet[0], kernel.packet[1]);
   const float64x2_t tmp2 = vzip2q_f64(kernel.packet[0], kernel.packet[1]);
 
@@ -3884,41 +5440,53 @@
   kernel.packet[1] = tmp2;
 }
 
-template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2d pselect( const Packet2d& mask, const Packet2d& a, const Packet2d& b)
-{ return vbslq_f64(vreinterpretq_u64_f64(mask), a, b); }
-
-template<> EIGEN_STRONG_INLINE Packet2d print<Packet2d>(const Packet2d& a)
-{ return vrndnq_f64(a); }
-
-template<> EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a)
-{ return vrndmq_f64(a); }
-
-template<> EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a)
-{ return vrndpq_f64(a); }
-
-template<> EIGEN_STRONG_INLINE Packet2d pldexp<Packet2d>(const Packet2d& a, const Packet2d& exponent)
-{ return pldexp_generic(a, exponent); }
-
-template<> EIGEN_STRONG_INLINE Packet2d pfrexp<Packet2d>(const Packet2d& a, Packet2d& exponent)
-{ return pfrexp_generic(a,exponent); }
-
-template<> EIGEN_STRONG_INLINE Packet2d pset1frombits<Packet2d>(uint64_t from)
-{ return vreinterpretq_f64_u64(vdupq_n_u64(from)); }
-
-template<> EIGEN_STRONG_INLINE Packet2d prsqrt(const Packet2d& a) {
-  // Compute approximate reciprocal sqrt.
-  Packet2d x = vrsqrteq_f64(a);
-  // Do Newton iterations for 1/sqrt(x).
-  x = vmulq_f64(vrsqrtsq_f64(vmulq_f64(a, x), x), x);
-  x = vmulq_f64(vrsqrtsq_f64(vmulq_f64(a, x), x), x);
-  x = vmulq_f64(vrsqrtsq_f64(vmulq_f64(a, x), x), x);
-  const Packet2d infinity = pset1<Packet2d>(NumTraits<double>::infinity());
-  return pselect(pcmp_eq(a, pzero(a)), infinity, x);
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2d pselect(const Packet2d& mask, const Packet2d& a, const Packet2d& b) {
+  return vbslq_f64(vreinterpretq_u64_f64(mask), a, b);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d psqrt(const Packet2d& _x){ return vsqrtq_f64(_x); }
+template <>
+EIGEN_STRONG_INLINE Packet2d print<Packet2d>(const Packet2d& a) {
+  return vrndnq_f64(a);
+}
 
-#endif // EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a) {
+  return vrndmq_f64(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a) {
+  return vrndpq_f64(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d pldexp<Packet2d>(const Packet2d& a, const Packet2d& exponent) {
+  return pldexp_generic(a, exponent);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d pfrexp<Packet2d>(const Packet2d& a, Packet2d& exponent) {
+  return pfrexp_generic(a, exponent);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d pset1frombits<Packet2d>(uint64_t from) {
+  return vreinterpretq_f64_u64(vdupq_n_u64(from));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d prsqrt(const Packet2d& a) {
+  // Do Newton iterations for 1/sqrt(x).
+  return generic_rsqrt_newton_step<Packet2d, /*Steps=*/3>::run(a, vrsqrteq_f64(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d psqrt(const Packet2d& _x) {
+  return vsqrtq_f64(_x);
+}
+
+#endif  // EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG
 
 // Do we have an fp16 types and supporting Neon intrinsics?
 #if EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
@@ -3933,7 +5501,6 @@
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 8,
-    HasHalfPacket = 1,
 
     HasCmp = 1,
     HasCast = 1,
@@ -3949,7 +5516,7 @@
     HasMin = 1,
     HasMax = 1,
     HasConj = 1,
-    HasSetLinear = 0,
+    HasSetLinear = 1,
     HasBlend = 0,
     HasInsert = 1,
     HasReduxp = 1,
@@ -3961,6 +5528,7 @@
     HasCos = 0,
     HasLog = 0,
     HasExp = 0,
+    HasTanh = packet_traits<float>::HasTanh,  // tanh<half> calls tanh<float>
     HasSqrt = 1,
     HasRsqrt = 1,
     HasErf = EIGEN_FAST_MATH,
@@ -3995,7 +5563,7 @@
   };
 };
 
-template<>
+template <>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf predux_half_dowto4<Packet8hf>(const Packet8hf& a) {
   return vadd_f16(vget_low_f16(a), vget_high_f16(a));
 }
@@ -4105,14 +5673,27 @@
 }
 
 #ifdef __ARM_FEATURE_NUMERIC_MAXMIN
-// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
-template<> EIGEN_STRONG_INLINE Packet4hf pmin<PropagateNumbers, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return vminnm_f16(a, b); }
-template<> EIGEN_STRONG_INLINE Packet8hf pmin<PropagateNumbers, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return vminnmq_f16(a, b); }
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8
+// systems).
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmin<PropagateNumbers, Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vminnm_f16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmin<PropagateNumbers, Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vminnmq_f16(a, b);
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4hf pmin<PropagateNaN, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return pmin<Packet4hf>(a, b); }
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmin<PropagateNaN, Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return pmin<Packet4hf>(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet8hf pmin<PropagateNaN, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return pmin<Packet8hf>(a, b); }
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmin<PropagateNaN, Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return pmin<Packet8hf>(a, b);
+}
 
 template <>
 EIGEN_STRONG_INLINE Packet8hf pmax<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
@@ -4125,14 +5706,27 @@
 }
 
 #ifdef __ARM_FEATURE_NUMERIC_MAXMIN
-// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
-template<> EIGEN_STRONG_INLINE Packet4hf pmax<PropagateNumbers, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return vmaxnm_f16(a, b); }
-template<> EIGEN_STRONG_INLINE Packet8hf pmax<PropagateNumbers, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return vmaxnmq_f16(a, b); }
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8
+// systems).
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmax<PropagateNumbers, Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vmaxnm_f16(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmax<PropagateNumbers, Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vmaxnmq_f16(a, b);
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4hf pmax<PropagateNaN, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return pmax<Packet4hf>(a, b); }
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmax<PropagateNaN, Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return pmax<Packet4hf>(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet8hf pmax<PropagateNaN, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return pmax<Packet8hf>(a, b); }
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmax<PropagateNaN, Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return pmax<Packet8hf>(a, b);
+}
 
 #define EIGEN_MAKE_ARM_FP16_CMP_8(name)                                               \
   template <>                                                                         \
@@ -4168,28 +5762,34 @@
 }
 
 template <>
-EIGEN_STRONG_INLINE Packet8hf print<Packet8hf>(const Packet8hf& a)
-{ return vrndnq_f16(a); }
+EIGEN_STRONG_INLINE Packet8hf print<Packet8hf>(const Packet8hf& a) {
+  return vrndnq_f16(a);
+}
 
 template <>
-EIGEN_STRONG_INLINE Packet4hf print<Packet4hf>(const Packet4hf& a)
-{ return vrndn_f16(a); }
+EIGEN_STRONG_INLINE Packet4hf print<Packet4hf>(const Packet4hf& a) {
+  return vrndn_f16(a);
+}
 
 template <>
-EIGEN_STRONG_INLINE Packet8hf pfloor<Packet8hf>(const Packet8hf& a)
-{ return vrndmq_f16(a); }
+EIGEN_STRONG_INLINE Packet8hf pfloor<Packet8hf>(const Packet8hf& a) {
+  return vrndmq_f16(a);
+}
 
 template <>
-EIGEN_STRONG_INLINE Packet4hf pfloor<Packet4hf>(const Packet4hf& a)
-{ return vrndm_f16(a); }
+EIGEN_STRONG_INLINE Packet4hf pfloor<Packet4hf>(const Packet4hf& a) {
+  return vrndm_f16(a);
+}
 
 template <>
-EIGEN_STRONG_INLINE Packet8hf pceil<Packet8hf>(const Packet8hf& a)
-{ return vrndpq_f16(a); }
+EIGEN_STRONG_INLINE Packet8hf pceil<Packet8hf>(const Packet8hf& a) {
+  return vrndpq_f16(a);
+}
 
 template <>
-EIGEN_STRONG_INLINE Packet4hf pceil<Packet4hf>(const Packet4hf& a)
-{ return vrndp_f16(a); }
+EIGEN_STRONG_INLINE Packet4hf pceil<Packet4hf>(const Packet4hf& a) {
+  return vrndp_f16(a);
+}
 
 template <>
 EIGEN_STRONG_INLINE Packet8hf psqrt<Packet8hf>(const Packet8hf& a) {
@@ -4291,13 +5891,17 @@
 EIGEN_STRONG_INLINE Packet8hf ploadquad<Packet8hf>(const Eigen::half* from) {
   Packet4hf lo, hi;
   lo = vld1_dup_f16(reinterpret_cast<const float16_t*>(from));
-  hi = vld1_dup_f16(reinterpret_cast<const float16_t*>(from+1));
+  hi = vld1_dup_f16(reinterpret_cast<const float16_t*>(from + 1));
   return vcombine_f16(lo, hi);
 }
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pinsertfirst(const Packet8hf& a, Eigen::half b) { return vsetq_lane_f16(b.x, a, 0); }
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pinsertfirst(const Packet8hf& a, Eigen::half b) {
+  return vsetq_lane_f16(b.x, a, 0);
+}
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pinsertfirst(const Packet4hf& a, Eigen::half b) { return vset_lane_f16(b.x, a, 0); }
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pinsertfirst(const Packet4hf& a, Eigen::half b) {
+  return vset_lane_f16(b.x, a, 0);
+}
 
 template <>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pselect(const Packet8hf& mask, const Packet8hf& a, const Packet8hf& b) {
@@ -4309,9 +5913,13 @@
   return vbsl_f16(vreinterpret_u16_f16(mask), a, b);
 }
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pinsertlast(const Packet8hf& a, Eigen::half b) { return vsetq_lane_f16(b.x, a, 7); }
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pinsertlast(const Packet8hf& a, Eigen::half b) {
+  return vsetq_lane_f16(b.x, a, 7);
+}
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pinsertlast(const Packet4hf& a, Eigen::half b) { return vset_lane_f16(b.x, a, 3); }
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pinsertlast(const Packet4hf& a, Eigen::half b) {
+  return vset_lane_f16(b.x, a, 3);
+}
 
 template <>
 EIGEN_STRONG_INLINE void pstore<Eigen::half>(Eigen::half* to, const Packet8hf& from) {
@@ -4358,7 +5966,8 @@
 }
 
 template <>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet8hf>(Eigen::half* to, const Packet8hf& from, Index stride) {
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet8hf>(Eigen::half* to, const Packet8hf& from,
+                                                                            Index stride) {
   to[stride * 0].x = vgetq_lane_f16(from, 0);
   to[stride * 1].x = vgetq_lane_f16(from, 1);
   to[stride * 2].x = vgetq_lane_f16(from, 2);
@@ -4370,7 +5979,8 @@
 }
 
 template <>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet4hf>(Eigen::half* to, const Packet4hf& from, Index stride) {
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet4hf>(Eigen::half* to, const Packet4hf& from,
+                                                                            Index stride) {
   to[stride * 0].x = vget_lane_f16(from, 0);
   to[stride * 1].x = vget_lane_f16(from, 1);
   to[stride * 2].x = vget_lane_f16(from, 2);
@@ -4400,7 +6010,8 @@
   return h;
 }
 
-template<> EIGEN_STRONG_INLINE Packet8hf preverse(const Packet8hf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet8hf preverse(const Packet8hf& a) {
   float16x4_t a_lo, a_hi;
   Packet8hf a_r64;
 
@@ -4421,11 +6032,21 @@
 }
 
 template <>
+EIGEN_STRONG_INLINE Packet8hf psignbit(const Packet8hf& a) {
+  return vreinterpretq_f16_s16(vshrq_n_s16(vreinterpretq_s16_f16(a), 15));
+}
+
+template <>
 EIGEN_STRONG_INLINE Packet4hf pabs<Packet4hf>(const Packet4hf& a) {
   return vabs_f16(a);
 }
 
 template <>
+EIGEN_STRONG_INLINE Packet4hf psignbit(const Packet4hf& a) {
+  return vreinterpret_f16_s16(vshr_n_s16(vreinterpret_s16_f16(a), 15));
+}
+
+template <>
 EIGEN_STRONG_INLINE Eigen::half predux<Packet8hf>(const Packet8hf& a) {
   float16x4_t a_lo, a_hi, sum;
 
@@ -4476,56 +6097,33 @@
 
 template <>
 EIGEN_STRONG_INLINE Eigen::half predux_min<Packet8hf>(const Packet8hf& a) {
-  float16x4_t a_lo, a_hi, min;
-
-  a_lo = vget_low_f16(a);
-  a_hi = vget_high_f16(a);
-  min = vpmin_f16(a_lo, a_hi);
-  min = vpmin_f16(min, min);
-  min = vpmin_f16(min, min);
-
   Eigen::half h;
-  h.x = vget_lane_f16(min, 0);
+  h.x = vminvq_f16(a);
   return h;
 }
 
 template <>
 EIGEN_STRONG_INLINE Eigen::half predux_min<Packet4hf>(const Packet4hf& a) {
-  Packet4hf tmp;
-  tmp = vpmin_f16(a, a);
-  tmp = vpmin_f16(tmp, tmp);
   Eigen::half h;
-  h.x = vget_lane_f16(tmp, 0);
+  h.x = vminv_f16(a);
   return h;
 }
 
 template <>
 EIGEN_STRONG_INLINE Eigen::half predux_max<Packet8hf>(const Packet8hf& a) {
-  float16x4_t a_lo, a_hi, max;
-
-  a_lo = vget_low_f16(a);
-  a_hi = vget_high_f16(a);
-  max = vpmax_f16(a_lo, a_hi);
-  max = vpmax_f16(max, max);
-  max = vpmax_f16(max, max);
-
   Eigen::half h;
-  h.x = vget_lane_f16(max, 0);
+  h.x = vmaxvq_f16(a);
   return h;
 }
 
 template <>
 EIGEN_STRONG_INLINE Eigen::half predux_max<Packet4hf>(const Packet4hf& a) {
-  Packet4hf tmp;
-  tmp = vpmax_f16(a, a);
-  tmp = vpmax_f16(tmp, tmp);
   Eigen::half h;
-  h.x = vget_lane_f16(tmp, 0);
+  h.x = vmaxv_f16(a);
   return h;
 }
 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8hf, 4>& kernel)
-{
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8hf, 4>& kernel) {
   const float16x8x2_t zip16_1 = vzipq_f16(kernel.packet[0], kernel.packet[1]);
   const float16x8x2_t zip16_2 = vzipq_f16(kernel.packet[2], kernel.packet[3]);
 
@@ -4578,10 +6176,10 @@
   kernel.packet[6] = T_3[1].val[1];
   kernel.packet[7] = T_3[3].val[1];
 }
-#endif // end EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
+#endif  // end EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PACKET_MATH_NEON_H
+#endif  // EIGEN_PACKET_MATH_NEON_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h
index 54f9733..58d7b8c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h
@@ -11,25 +11,176 @@
 #ifndef EIGEN_TYPE_CASTING_NEON_H
 #define EIGEN_TYPE_CASTING_NEON_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 //==============================================================================
-// pcast, SrcType = float
+// preinterpret (truncation operations)
+//==============================================================================
+
+template <>
+EIGEN_STRONG_INLINE Packet8c preinterpret<Packet8c, Packet16c>(const Packet16c& a) {
+  return Packet8c(vget_low_s8(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c preinterpret<Packet4c, Packet8c>(const Packet8c& a) {
+  return Packet4c(vget_lane_s32(vreinterpret_s32_s8(a), 0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c preinterpret<Packet4c, Packet16c>(const Packet16c& a) {
+  return preinterpret<Packet4c>(preinterpret<Packet8c>(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8uc preinterpret<Packet8uc, Packet16uc>(const Packet16uc& a) {
+  return Packet8uc(vget_low_u8(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc preinterpret<Packet4uc, Packet8uc>(const Packet8uc& a) {
+  return Packet4uc(vget_lane_u32(vreinterpret_u32_u8(a), 0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc preinterpret<Packet4uc, Packet16uc>(const Packet16uc& a) {
+  return preinterpret<Packet4uc>(preinterpret<Packet8uc>(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4s preinterpret<Packet4s, Packet8s>(const Packet8s& a) {
+  return Packet4s(vget_low_s16(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4us preinterpret<Packet4us, Packet8us>(const Packet8us& a) {
+  return Packet4us(vget_low_u16(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2i preinterpret<Packet2i, Packet4i>(const Packet4i& a) {
+  return Packet2i(vget_low_s32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui preinterpret<Packet2ui, Packet4ui>(const Packet4ui& a) {
+  return Packet2ui(vget_low_u32(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2f preinterpret<Packet2f, Packet4f>(const Packet4f& a) {
+  return Packet2f(vget_low_f32(a));
+}
+
+//==============================================================================
+// preinterpret
 //==============================================================================
 template <>
-struct type_casting_traits<float, float> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet4f pcast<Packet4f, Packet4f>(const Packet4f& a) {
-  return a;
+EIGEN_STRONG_INLINE Packet2f preinterpret<Packet2f, Packet2i>(const Packet2i& a) {
+  return Packet2f(vreinterpret_f32_s32(a));
 }
 template <>
-EIGEN_STRONG_INLINE Packet2f pcast<Packet2f, Packet2f>(const Packet2f& a) {
-  return a;
+EIGEN_STRONG_INLINE Packet2f preinterpret<Packet2f, Packet2ui>(const Packet2ui& a) {
+  return Packet2f(vreinterpret_f32_u32(a));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet4i>(const Packet4i& a) {
+  return Packet4f(vreinterpretq_f32_s32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet4ui>(const Packet4ui& a) {
+  return Packet4f(vreinterpretq_f32_u32(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4c preinterpret<Packet4c, Packet4uc>(const Packet4uc& a) {
+  return static_cast<Packet4c>(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c preinterpret<Packet8c, Packet8uc>(const Packet8uc& a) {
+  return Packet8c(vreinterpret_s8_u8(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c preinterpret<Packet16c, Packet16uc>(const Packet16uc& a) {
+  return Packet16c(vreinterpretq_s8_u8(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4uc preinterpret<Packet4uc, Packet4c>(const Packet4c& a) {
+  return static_cast<Packet4uc>(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc preinterpret<Packet8uc, Packet8c>(const Packet8c& a) {
+  return Packet8uc(vreinterpret_u8_s8(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc preinterpret<Packet16uc, Packet16c>(const Packet16c& a) {
+  return Packet16uc(vreinterpretq_u8_s8(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4s preinterpret<Packet4s, Packet4us>(const Packet4us& a) {
+  return Packet4s(vreinterpret_s16_u16(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s preinterpret<Packet8s, Packet8us>(const Packet8us& a) {
+  return Packet8s(vreinterpretq_s16_u16(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us preinterpret<Packet4us, Packet4s>(const Packet4s& a) {
+  return Packet4us(vreinterpret_u16_s16(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us preinterpret<Packet8us, Packet8s>(const Packet8s& a) {
+  return Packet8us(vreinterpretq_u16_s16(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2i preinterpret<Packet2i, Packet2f>(const Packet2f& a) {
+  return Packet2i(vreinterpret_s32_f32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i preinterpret<Packet2i, Packet2ui>(const Packet2ui& a) {
+  return Packet2i(vreinterpret_s32_u32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4f>(const Packet4f& a) {
+  return Packet4i(vreinterpretq_s32_f32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4ui>(const Packet4ui& a) {
+  return Packet4i(vreinterpretq_s32_u32(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2ui preinterpret<Packet2ui, Packet2f>(const Packet2f& a) {
+  return Packet2ui(vreinterpret_u32_f32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui preinterpret<Packet2ui, Packet2i>(const Packet2i& a) {
+  return Packet2ui(vreinterpret_u32_s32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet4f>(const Packet4f& a) {
+  return Packet4ui(vreinterpretq_u32_f32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet4i>(const Packet4i& a) {
+  return Packet4ui(vreinterpretq_u32_s32(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2l preinterpret<Packet2l, Packet2ul>(const Packet2ul& a) {
+  return Packet2l(vreinterpretq_s64_u64(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul preinterpret<Packet2ul, Packet2l>(const Packet2l& a) {
+  return Packet2ul(vreinterpretq_u64_s64(a));
+}
+
+//==============================================================================
+// pcast, SrcType = float
+//==============================================================================
 
 template <>
 struct type_casting_traits<float, numext::int64_t> {
@@ -47,10 +198,18 @@
   return vcvtq_s64_f64(vcvt_f64_f32(vget_low_f32(a)));
 }
 template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet2f, Packet2l>(const Packet2f& a) {
+  return vcvtq_s64_f64(vcvt_f64_f32(a));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2ul pcast<Packet4f, Packet2ul>(const Packet4f& a) {
   // Discard second half of input.
   return vcvtq_u64_f64(vcvt_f64_f32(vget_low_f32(a)));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet2f, Packet2ul>(const Packet2f& a) {
+  return vcvtq_u64_f64(vcvt_f64_f32(a));
+}
 #else
 template <>
 EIGEN_STRONG_INLINE Packet2l pcast<Packet4f, Packet2l>(const Packet4f& a) {
@@ -58,10 +217,19 @@
   return vmovl_s32(vget_low_s32(vcvtq_s32_f32(a)));
 }
 template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet2f, Packet2l>(const Packet2f& a) {
+  return vmovl_s32(vcvt_s32_f32(a));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2ul pcast<Packet4f, Packet2ul>(const Packet4f& a) {
   // Discard second half of input.
   return vmovl_u32(vget_low_u32(vcvtq_u32_f32(a)));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet2f, Packet2ul>(const Packet2f& a) {
+  // Discard second half of input.
+  return vmovl_u32(vcvt_u32_f32(a));
+}
 #endif  // EIGEN_ARCH_ARM64
 
 template <>
@@ -99,6 +267,10 @@
   return vcombine_s16(vmovn_s32(vcvtq_s32_f32(a)), vmovn_s32(vcvtq_s32_f32(b)));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet4f, Packet4s>(const Packet4f& a) {
+  return vmovn_s32(vcvtq_s32_f32(a));
+}
+template <>
 EIGEN_STRONG_INLINE Packet4s pcast<Packet2f, Packet4s>(const Packet2f& a, const Packet2f& b) {
   return vmovn_s32(vcombine_s32(vcvt_s32_f32(a), vcvt_s32_f32(b)));
 }
@@ -112,6 +284,10 @@
   return vcombine_u16(vmovn_u32(vcvtq_u32_f32(a)), vmovn_u32(vcvtq_u32_f32(b)));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet4f, Packet4us>(const Packet4f& a) {
+  return vmovn_u32(vcvtq_u32_f32(a));
+}
+template <>
 EIGEN_STRONG_INLINE Packet4us pcast<Packet2f, Packet4us>(const Packet2f& a, const Packet2f& b) {
   return vmovn_u32(vcombine_u32(vcvt_u32_f32(a), vcvt_u32_f32(b)));
 }
@@ -128,12 +304,25 @@
   return vcombine_s8(vmovn_s16(ab_s16), vmovn_s16(cd_s16));
 }
 template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet4f, Packet8c>(const Packet4f& a, const Packet4f& b) {
+  const int16x8_t ab_s16 = pcast<Packet4f, Packet8s>(a, b);
+  return vmovn_s16(ab_s16);
+}
+template <>
 EIGEN_STRONG_INLINE Packet8c pcast<Packet2f, Packet8c>(const Packet2f& a, const Packet2f& b, const Packet2f& c,
                                                        const Packet2f& d) {
   const int16x4_t ab_s16 = pcast<Packet2f, Packet4s>(a, b);
   const int16x4_t cd_s16 = pcast<Packet2f, Packet4s>(c, d);
   return vmovn_s16(vcombine_s16(ab_s16, cd_s16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet4f, Packet4c>(const Packet4f& a) {
+  const int32x4_t a_s32x4 = vcvtq_s32_f32(a);
+  const int16x4_t a_s16x4 = vmovn_s32(a_s32x4);
+  const int16x8_t aa_s16x8 = vcombine_s16(a_s16x4, a_s16x4);
+  const int8x8_t aa_s8x8 = vmovn_s16(aa_s16x8);
+  return vget_lane_s32(vreinterpret_s32_s8(aa_s8x8), 0);
+}
 
 template <>
 struct type_casting_traits<float, numext::uint8_t> {
@@ -142,16 +331,20 @@
 template <>
 EIGEN_STRONG_INLINE Packet16uc pcast<Packet4f, Packet16uc>(const Packet4f& a, const Packet4f& b, const Packet4f& c,
                                                            const Packet4f& d) {
-  const uint16x8_t ab_u16 = pcast<Packet4f, Packet8us>(a, b);
-  const uint16x8_t cd_u16 = pcast<Packet4f, Packet8us>(c, d);
-  return vcombine_u8(vmovn_u16(ab_u16), vmovn_u16(cd_u16));
+  return preinterpret<Packet16uc>(pcast<Packet4f, Packet16c>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet4f, Packet8uc>(const Packet4f& a, const Packet4f& b) {
+  return preinterpret<Packet8uc>(pcast<Packet4f, Packet8c>(a, b));
 }
 template <>
 EIGEN_STRONG_INLINE Packet8uc pcast<Packet2f, Packet8uc>(const Packet2f& a, const Packet2f& b, const Packet2f& c,
                                                          const Packet2f& d) {
-  const uint16x4_t ab_u16 = pcast<Packet2f, Packet4us>(a, b);
-  const uint16x4_t cd_u16 = pcast<Packet2f, Packet4us>(c, d);
-  return vmovn_u16(vcombine_u16(ab_u16, cd_u16));
+  return preinterpret<Packet8uc>(pcast<Packet2f, Packet8c>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet4f, Packet4uc>(const Packet4f& a) {
+  return static_cast<Packet4uc>(pcast<Packet4f, Packet4c>(a));
 }
 
 //==============================================================================
@@ -167,6 +360,10 @@
   return vcvtq_f32_s32(vmovl_s16(vget_low_s16(vmovl_s8(vget_low_s8(a)))));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4c, Packet4f>(const Packet4c& a) {
+  return vcvtq_f32_s32(vmovl_s16(vget_low_s16(vmovl_s8(vreinterpret_s8_s32(vdup_n_s32(a))))));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2f pcast<Packet8c, Packet2f>(const Packet8c& a) {
   // Discard all but first 2 bytes.
   return vcvt_f32_s32(vget_low_s32(vmovl_s16(vget_low_s16(vmovl_s8(a)))));
@@ -188,7 +385,7 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet2ul pcast<Packet16c, Packet2ul>(const Packet16c& a) {
-  return vreinterpretq_u64_s64(pcast<Packet16c, Packet2l>(a));
+  return preinterpret<Packet2ul>(pcast<Packet16c, Packet2l>(a));
 }
 
 template <>
@@ -201,6 +398,14 @@
   return vmovl_s16(vget_low_s16(vmovl_s8(vget_low_s8(a))));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet8c, Packet4i>(const Packet8c& a) {
+  return vmovl_s16(vget_low_s16(vmovl_s8(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4c, Packet4i>(const Packet4c& a) {
+  return pcast<Packet8c, Packet4i>(vreinterpret_s8_s32(vdup_n_s32(a)));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2i pcast<Packet8c, Packet2i>(const Packet8c& a) {
   // Discard all but first 2 bytes.
   return vget_low_s32(vmovl_s16(vget_low_s16(vmovl_s8(a))));
@@ -212,11 +417,15 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet4ui pcast<Packet16c, Packet4ui>(const Packet16c& a) {
-  return vreinterpretq_u32_s32(pcast<Packet16c, Packet4i>(a));
+  return preinterpret<Packet4ui>(pcast<Packet16c, Packet4i>(a));
 }
 template <>
 EIGEN_STRONG_INLINE Packet2ui pcast<Packet8c, Packet2ui>(const Packet8c& a) {
-  return vreinterpret_u32_s32(pcast<Packet8c, Packet2i>(a));
+  return preinterpret<Packet2ui>(pcast<Packet8c, Packet2i>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet4c, Packet4ui>(const Packet4c& a) {
+  return preinterpret<Packet4ui>(pcast<Packet4c, Packet4i>(a));
 }
 
 template <>
@@ -229,10 +438,18 @@
   return vmovl_s8(vget_low_s8(a));
 }
 template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet8c, Packet8s>(const Packet8c& a) {
+  return vmovl_s8(a);
+}
+template <>
 EIGEN_STRONG_INLINE Packet4s pcast<Packet8c, Packet4s>(const Packet8c& a) {
   // Discard second half of input.
   return vget_low_s16(vmovl_s8(a));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet4c, Packet4s>(const Packet4c& a) {
+  return pcast<Packet8c, Packet4s>(vreinterpret_s8_s32(vdup_n_s32(a)));
+}
 
 template <>
 struct type_casting_traits<numext::int8_t, numext::uint16_t> {
@@ -240,45 +457,19 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet8us pcast<Packet16c, Packet8us>(const Packet16c& a) {
-  return vreinterpretq_u16_s16(pcast<Packet16c, Packet8s>(a));
+  return preinterpret<Packet8us>(pcast<Packet16c, Packet8s>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet8c, Packet8us>(const Packet8c& a) {
+  return preinterpret<Packet8us>(pcast<Packet8c, Packet8s>(a));
 }
 template <>
 EIGEN_STRONG_INLINE Packet4us pcast<Packet8c, Packet4us>(const Packet8c& a) {
-  return vreinterpret_u16_s16(pcast<Packet8c, Packet4s>(a));
-}
-
-template <>
-struct type_casting_traits<numext::int8_t, numext::int8_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet16c pcast<Packet16c, Packet16c>(const Packet16c& a) {
-  return a;
+  return preinterpret<Packet4us>(pcast<Packet8c, Packet4s>(a));
 }
 template <>
-EIGEN_STRONG_INLINE Packet8c pcast<Packet8c, Packet8c>(const Packet8c& a) {
-  return a;
-}
-template <>
-EIGEN_STRONG_INLINE Packet4c pcast<Packet4c, Packet4c>(const Packet4c& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::int8_t, numext::uint8_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet16uc pcast<Packet16c, Packet16uc>(const Packet16c& a) {
-  return vreinterpretq_u8_s8(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet8uc pcast<Packet8c, Packet8uc>(const Packet8c& a) {
-  return vreinterpret_u8_s8(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4uc pcast<Packet4c, Packet4uc>(const Packet4c& a) {
-  return static_cast<Packet4uc>(a);
+EIGEN_STRONG_INLINE Packet4us pcast<Packet4c, Packet4us>(const Packet4c& a) {
+  return preinterpret<Packet4us>(pcast<Packet4c, Packet4s>(a));
 }
 
 //==============================================================================
@@ -294,6 +485,10 @@
   return vcvtq_f32_u32(vmovl_u16(vget_low_u16(vmovl_u8(vget_low_u8(a)))));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4uc, Packet4f>(const Packet4uc& a) {
+  return vcvtq_f32_u32(vmovl_u16(vget_low_u16(vmovl_u8(vreinterpret_u8_u32(vdup_n_u32(a))))));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2f pcast<Packet8uc, Packet2f>(const Packet8uc& a) {
   // Discard all but first 2 bytes.
   return vcvt_f32_u32(vget_low_u32(vmovl_u16(vget_low_u16(vmovl_u8(a)))));
@@ -315,7 +510,7 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet2l pcast<Packet16uc, Packet2l>(const Packet16uc& a) {
-  return vreinterpretq_s64_u64(pcast<Packet16uc, Packet2ul>(a));
+  return preinterpret<Packet2l>(pcast<Packet16uc, Packet2ul>(a));
 }
 
 template <>
@@ -328,10 +523,18 @@
   return vmovl_u16(vget_low_u16(vmovl_u8(vget_low_u8(a))));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet8uc, Packet4ui>(const Packet8uc& a) {
+  return vmovl_u16(vget_low_u16(vmovl_u8(a)));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2ui pcast<Packet8uc, Packet2ui>(const Packet8uc& a) {
   // Discard all but first 2 bytes.
   return vget_low_u32(vmovl_u16(vget_low_u16(vmovl_u8(a))));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet4uc, Packet4ui>(const Packet4uc& a) {
+  return pcast<Packet8uc, Packet4ui>(vreinterpret_u8_u32(vdup_n_u32(a)));
+}
 
 template <>
 struct type_casting_traits<numext::uint8_t, numext::int32_t> {
@@ -339,11 +542,15 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet4i pcast<Packet16uc, Packet4i>(const Packet16uc& a) {
-  return vreinterpretq_s32_u32(pcast<Packet16uc, Packet4ui>(a));
+  return preinterpret<Packet4i>(pcast<Packet16uc, Packet4ui>(a));
 }
 template <>
 EIGEN_STRONG_INLINE Packet2i pcast<Packet8uc, Packet2i>(const Packet8uc& a) {
-  return vreinterpret_s32_u32(pcast<Packet8uc, Packet2ui>(a));
+  return preinterpret<Packet2i>(pcast<Packet8uc, Packet2ui>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4uc, Packet4i>(const Packet4uc& a) {
+  return preinterpret<Packet4i>(pcast<Packet4uc, Packet4ui>(a));
 }
 
 template <>
@@ -356,9 +563,12 @@
   return vmovl_u8(vget_low_u8(a));
 }
 template <>
-EIGEN_STRONG_INLINE Packet4us pcast<Packet8uc, Packet4us>(const Packet8uc& a) {
-  // Discard second half of input.
-  return vget_low_u16(vmovl_u8(a));
+EIGEN_STRONG_INLINE Packet8us pcast<Packet8uc, Packet8us>(const Packet8uc& a) {
+  return vmovl_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet4uc, Packet4us>(const Packet4uc& a) {
+  return vget_low_u16(vmovl_u8(vreinterpret_u8_u32(vdup_n_u32(a))));
 }
 
 template <>
@@ -367,45 +577,15 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet8s pcast<Packet16uc, Packet8s>(const Packet16uc& a) {
-  return vreinterpretq_s16_u16(pcast<Packet16uc, Packet8us>(a));
+  return preinterpret<Packet8s>(pcast<Packet16uc, Packet8us>(a));
 }
 template <>
-EIGEN_STRONG_INLINE Packet4s pcast<Packet8uc, Packet4s>(const Packet8uc& a) {
-  return vreinterpret_s16_u16(pcast<Packet8uc, Packet4us>(a));
-}
-
-template <>
-struct type_casting_traits<numext::uint8_t, numext::uint8_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet16uc pcast<Packet16uc, Packet16uc>(const Packet16uc& a) {
-  return a;
+EIGEN_STRONG_INLINE Packet8s pcast<Packet8uc, Packet8s>(const Packet8uc& a) {
+  return preinterpret<Packet8s>(pcast<Packet8uc, Packet8us>(a));
 }
 template <>
-EIGEN_STRONG_INLINE Packet8uc pcast<Packet8uc, Packet8uc>(const Packet8uc& a) {
-  return a;
-}
-template <>
-EIGEN_STRONG_INLINE Packet4uc pcast<Packet4uc, Packet4uc>(const Packet4uc& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::uint8_t, numext::int8_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet16c pcast<Packet16uc, Packet16c>(const Packet16uc& a) {
-  return vreinterpretq_s8_u8(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet8c pcast<Packet8uc, Packet8c>(const Packet8uc& a) {
-  return vreinterpret_s8_u8(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4c pcast<Packet4uc, Packet4c>(const Packet4uc& a) {
-  return static_cast<Packet4c>(a);
+EIGEN_STRONG_INLINE Packet4s pcast<Packet4uc, Packet4s>(const Packet4uc& a) {
+  return preinterpret<Packet4s>(pcast<Packet4uc, Packet4us>(a));
 }
 
 //==============================================================================
@@ -421,6 +601,10 @@
   return vcvtq_f32_s32(vmovl_s16(vget_low_s16(a)));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4s, Packet4f>(const Packet4s& a) {
+  return vcvtq_f32_s32(vmovl_s16(a));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2f pcast<Packet4s, Packet2f>(const Packet4s& a) {
   // Discard second half of input.
   return vcvt_f32_s32(vget_low_s32(vmovl_s16(a)));
@@ -442,7 +626,7 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet2ul pcast<Packet8s, Packet2ul>(const Packet8s& a) {
-  return vreinterpretq_u64_s64(pcast<Packet8s, Packet2l>(a));
+  return preinterpret<Packet2ul>(pcast<Packet8s, Packet2l>(a));
 }
 
 template <>
@@ -455,6 +639,10 @@
   return vmovl_s16(vget_low_s16(a));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4s, Packet4i>(const Packet4s& a) {
+  return vmovl_s16(a);
+}
+template <>
 EIGEN_STRONG_INLINE Packet2i pcast<Packet4s, Packet2i>(const Packet4s& a) {
   // Discard second half of input.
   return vget_low_s32(vmovl_s16(a));
@@ -466,37 +654,15 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet4ui pcast<Packet8s, Packet4ui>(const Packet8s& a) {
-  return vreinterpretq_u32_s32(pcast<Packet8s, Packet4i>(a));
+  return preinterpret<Packet4ui>(pcast<Packet8s, Packet4i>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet4s, Packet4ui>(const Packet4s& a) {
+  return preinterpret<Packet4ui>(pcast<Packet4s, Packet4i>(a));
 }
 template <>
 EIGEN_STRONG_INLINE Packet2ui pcast<Packet4s, Packet2ui>(const Packet4s& a) {
-  return vreinterpret_u32_s32(pcast<Packet4s, Packet2i>(a));
-}
-
-template <>
-struct type_casting_traits<numext::int16_t, numext::int16_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet8s pcast<Packet8s, Packet8s>(const Packet8s& a) {
-  return a;
-}
-template <>
-EIGEN_STRONG_INLINE Packet4s pcast<Packet4s, Packet4s>(const Packet4s& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::int16_t, numext::uint16_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet8us pcast<Packet8s, Packet8us>(const Packet8s& a) {
-  return vreinterpretq_u16_s16(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4us pcast<Packet4s, Packet4us>(const Packet4s& a) {
-  return vreinterpret_u16_s16(a);
+  return preinterpret<Packet2ui>(pcast<Packet4s, Packet2i>(a));
 }
 
 template <>
@@ -508,9 +674,18 @@
   return vcombine_s8(vmovn_s16(a), vmovn_s16(b));
 }
 template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet8s, Packet8c>(const Packet8s& a) {
+  return vmovn_s16(a);
+}
+template <>
 EIGEN_STRONG_INLINE Packet8c pcast<Packet4s, Packet8c>(const Packet4s& a, const Packet4s& b) {
   return vmovn_s16(vcombine_s16(a, b));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet4s, Packet4c>(const Packet4s& a) {
+  const int8x8_t aa_s8x8 = pcast<Packet4s, Packet8c>(a, a);
+  return vget_lane_s32(vreinterpret_s32_s8(aa_s8x8), 0);
+}
 
 template <>
 struct type_casting_traits<numext::int16_t, numext::uint8_t> {
@@ -518,11 +693,19 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet16uc pcast<Packet8s, Packet16uc>(const Packet8s& a, const Packet8s& b) {
-  return vcombine_u8(vmovn_u16(vreinterpretq_u16_s16(a)), vmovn_u16(vreinterpretq_u16_s16(b)));
+  return preinterpret<Packet16uc>(pcast<Packet8s, Packet16c>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet8s, Packet8uc>(const Packet8s& a) {
+  return preinterpret<Packet8uc>(pcast<Packet8s, Packet8c>(a));
 }
 template <>
 EIGEN_STRONG_INLINE Packet8uc pcast<Packet4s, Packet8uc>(const Packet4s& a, const Packet4s& b) {
-  return vmovn_u16(vcombine_u16(vreinterpret_u16_s16(a), vreinterpret_u16_s16(b)));
+  return preinterpret<Packet8uc>(pcast<Packet4s, Packet8c>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet4s, Packet4uc>(const Packet4s& a) {
+  return static_cast<Packet4uc>(pcast<Packet4s, Packet4c>(a));
 }
 
 //==============================================================================
@@ -538,6 +721,10 @@
   return vcvtq_f32_u32(vmovl_u16(vget_low_u16(a)));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4us, Packet4f>(const Packet4us& a) {
+  return vcvtq_f32_u32(vmovl_u16(a));
+}
+template <>
 EIGEN_STRONG_INLINE Packet2f pcast<Packet4us, Packet2f>(const Packet4us& a) {
   // Discard second half of input.
   return vcvt_f32_u32(vget_low_u32(vmovl_u16(a)));
@@ -559,7 +746,7 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet2l pcast<Packet8us, Packet2l>(const Packet8us& a) {
-  return vreinterpretq_s64_u64(pcast<Packet8us, Packet2ul>(a));
+  return preinterpret<Packet2l>(pcast<Packet8us, Packet2ul>(a));
 }
 
 template <>
@@ -572,6 +759,10 @@
   return vmovl_u16(vget_low_u16(a));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet4us, Packet4ui>(const Packet4us& a) {
+  return vmovl_u16(a);
+}
+template <>
 EIGEN_STRONG_INLINE Packet2ui pcast<Packet4us, Packet2ui>(const Packet4us& a) {
   // Discard second half of input.
   return vget_low_u32(vmovl_u16(a));
@@ -583,37 +774,15 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet4i pcast<Packet8us, Packet4i>(const Packet8us& a) {
-  return vreinterpretq_s32_u32(pcast<Packet8us, Packet4ui>(a));
+  return preinterpret<Packet4i>(pcast<Packet8us, Packet4ui>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4us, Packet4i>(const Packet4us& a) {
+  return preinterpret<Packet4i>(pcast<Packet4us, Packet4ui>(a));
 }
 template <>
 EIGEN_STRONG_INLINE Packet2i pcast<Packet4us, Packet2i>(const Packet4us& a) {
-  return vreinterpret_s32_u32(pcast<Packet4us, Packet2ui>(a));
-}
-
-template <>
-struct type_casting_traits<numext::uint16_t, numext::uint16_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet8us pcast<Packet8us, Packet8us>(const Packet8us& a) {
-  return a;
-}
-template <>
-EIGEN_STRONG_INLINE Packet4us pcast<Packet4us, Packet4us>(const Packet4us& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::uint16_t, numext::int16_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet8s pcast<Packet8us, Packet8s>(const Packet8us& a) {
-  return vreinterpretq_s16_u16(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4s pcast<Packet4us, Packet4s>(const Packet4us& a) {
-  return vreinterpret_s16_u16(a);
+  return preinterpret<Packet2i>(pcast<Packet4us, Packet2ui>(a));
 }
 
 template <>
@@ -625,9 +794,18 @@
   return vcombine_u8(vmovn_u16(a), vmovn_u16(b));
 }
 template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet8us, Packet8uc>(const Packet8us& a) {
+  return vmovn_u16(a);
+}
+template <>
 EIGEN_STRONG_INLINE Packet8uc pcast<Packet4us, Packet8uc>(const Packet4us& a, const Packet4us& b) {
   return vmovn_u16(vcombine_u16(a, b));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet4us, Packet4uc>(const Packet4us& a) {
+  uint8x8_t aa_u8x8 = pcast<Packet4us, Packet8uc>(a, a);
+  return vget_lane_u32(vreinterpret_u32_u8(aa_u8x8), 0);
+}
 
 template <>
 struct type_casting_traits<numext::uint16_t, numext::int8_t> {
@@ -635,11 +813,19 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet16c pcast<Packet8us, Packet16c>(const Packet8us& a, const Packet8us& b) {
-  return vreinterpretq_s8_u8(pcast<Packet8us, Packet16uc>(a, b));
+  return preinterpret<Packet16c>(pcast<Packet8us, Packet16uc>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet8us, Packet8c>(const Packet8us& a) {
+  return preinterpret<Packet8c>(pcast<Packet8us, Packet8uc>(a));
 }
 template <>
 EIGEN_STRONG_INLINE Packet8c pcast<Packet4us, Packet8c>(const Packet4us& a, const Packet4us& b) {
-  return vreinterpret_s8_u8(pcast<Packet4us, Packet8uc>(a, b));
+  return preinterpret<Packet8c>(pcast<Packet4us, Packet8uc>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet4us, Packet4c>(const Packet4us& a) {
+  return static_cast<Packet4c>(pcast<Packet4us, Packet4uc>(a));
 }
 
 //==============================================================================
@@ -667,6 +853,10 @@
   // Discard second half of input.
   return vmovl_s32(vget_low_s32(a));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet2i, Packet2l>(const Packet2i& a) {
+  return vmovl_s32(a);
+}
 
 template <>
 struct type_casting_traits<numext::int32_t, numext::uint64_t> {
@@ -674,33 +864,11 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet2ul pcast<Packet4i, Packet2ul>(const Packet4i& a) {
-  return vreinterpretq_u64_s64(pcast<Packet4i, Packet2l>(a));
-}
-
-template <>
-struct type_casting_traits<numext::int32_t, numext::int32_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet4i pcast<Packet4i, Packet4i>(const Packet4i& a) {
-  return a;
+  return preinterpret<Packet2ul>(pcast<Packet4i, Packet2l>(a));
 }
 template <>
-EIGEN_STRONG_INLINE Packet2i pcast<Packet2i, Packet2i>(const Packet2i& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::int32_t, numext::uint32_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet4ui pcast<Packet4i, Packet4ui>(const Packet4i& a) {
-  return vreinterpretq_u32_s32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2ui pcast<Packet2i, Packet2ui>(const Packet2i& a) {
-  return vreinterpret_u32_s32(a);
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet2i, Packet2ul>(const Packet2i& a) {
+  return preinterpret<Packet2ul>(pcast<Packet2i, Packet2l>(a));
 }
 
 template <>
@@ -712,6 +880,10 @@
   return vcombine_s16(vmovn_s32(a), vmovn_s32(b));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet4i, Packet4s>(const Packet4i& a) {
+  return vmovn_s32(a);
+}
+template <>
 EIGEN_STRONG_INLINE Packet4s pcast<Packet2i, Packet4s>(const Packet2i& a, const Packet2i& b) {
   return vmovn_s32(vcombine_s32(a, b));
 }
@@ -725,6 +897,10 @@
   return vcombine_u16(vmovn_u32(vreinterpretq_u32_s32(a)), vmovn_u32(vreinterpretq_u32_s32(b)));
 }
 template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet4i, Packet4us>(const Packet4i& a) {
+  return vmovn_u32(vreinterpretq_u32_s32(a));
+}
+template <>
 EIGEN_STRONG_INLINE Packet4us pcast<Packet2i, Packet4us>(const Packet2i& a, const Packet2i& b) {
   return vmovn_u32(vreinterpretq_u32_s32(vcombine_s32(a, b)));
 }
@@ -741,12 +917,24 @@
   return vcombine_s8(vmovn_s16(ab_s16), vmovn_s16(cd_s16));
 }
 template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet4i, Packet8c>(const Packet4i& a, const Packet4i& b) {
+  const int16x8_t ab_s16 = pcast<Packet4i, Packet8s>(a, b);
+  return vmovn_s16(ab_s16);
+}
+template <>
 EIGEN_STRONG_INLINE Packet8c pcast<Packet2i, Packet8c>(const Packet2i& a, const Packet2i& b, const Packet2i& c,
                                                        const Packet2i& d) {
   const int16x4_t ab_s16 = vmovn_s32(vcombine_s32(a, b));
   const int16x4_t cd_s16 = vmovn_s32(vcombine_s32(c, d));
   return vmovn_s16(vcombine_s16(ab_s16, cd_s16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet4i, Packet4c>(const Packet4i& a) {
+  const int16x4_t a_s16x4 = vmovn_s32(a);
+  const int16x8_t aa_s16x8 = vcombine_s16(a_s16x4, a_s16x4);
+  const int8x8_t aa_s8x8 = vmovn_s16(aa_s16x8);
+  return vget_lane_s32(vreinterpret_s32_s8(aa_s8x8), 0);
+}
 
 template <>
 struct type_casting_traits<numext::int32_t, numext::uint8_t> {
@@ -755,16 +943,20 @@
 template <>
 EIGEN_STRONG_INLINE Packet16uc pcast<Packet4i, Packet16uc>(const Packet4i& a, const Packet4i& b, const Packet4i& c,
                                                            const Packet4i& d) {
-  const uint16x8_t ab_u16 = pcast<Packet4i, Packet8us>(a, b);
-  const uint16x8_t cd_u16 = pcast<Packet4i, Packet8us>(c, d);
-  return vcombine_u8(vmovn_u16(ab_u16), vmovn_u16(cd_u16));
+  return preinterpret<Packet16uc>(pcast<Packet4i, Packet16c>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet4i, Packet8uc>(const Packet4i& a, const Packet4i& b) {
+  return preinterpret<Packet8uc>(pcast<Packet4i, Packet8c>(a, b));
 }
 template <>
 EIGEN_STRONG_INLINE Packet8uc pcast<Packet2i, Packet8uc>(const Packet2i& a, const Packet2i& b, const Packet2i& c,
                                                          const Packet2i& d) {
-  const uint16x4_t ab_u16 = pcast<Packet2i, Packet4us>(a, b);
-  const uint16x4_t cd_u16 = pcast<Packet2i, Packet4us>(c, d);
-  return vmovn_u16(vcombine_u16(ab_u16, cd_u16));
+  return preinterpret<Packet8uc>(pcast<Packet2i, Packet8c>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet4i, Packet4uc>(const Packet4i& a) {
+  return static_cast<Packet4uc>(pcast<Packet4i, Packet4c>(a));
 }
 
 //==============================================================================
@@ -792,6 +984,10 @@
   // Discard second half of input.
   return vmovl_u32(vget_low_u32(a));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet2ui, Packet2ul>(const Packet2ui& a) {
+  return vmovl_u32(a);
+}
 
 template <>
 struct type_casting_traits<numext::uint32_t, numext::int64_t> {
@@ -799,33 +995,11 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet2l pcast<Packet4ui, Packet2l>(const Packet4ui& a) {
-  return vreinterpretq_s64_u64(pcast<Packet4ui, Packet2ul>(a));
-}
-
-template <>
-struct type_casting_traits<numext::uint32_t, numext::uint32_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet4ui pcast<Packet4ui, Packet4ui>(const Packet4ui& a) {
-  return a;
+  return preinterpret<Packet2l>(pcast<Packet4ui, Packet2ul>(a));
 }
 template <>
-EIGEN_STRONG_INLINE Packet2ui pcast<Packet2ui, Packet2ui>(const Packet2ui& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::uint32_t, numext::int32_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet4i pcast<Packet4ui, Packet4i>(const Packet4ui& a) {
-  return vreinterpretq_s32_u32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2i pcast<Packet2ui, Packet2i>(const Packet2ui& a) {
-  return vreinterpret_s32_u32(a);
+EIGEN_STRONG_INLINE Packet2l pcast<Packet2ui, Packet2l>(const Packet2ui& a) {
+  return preinterpret<Packet2l>(pcast<Packet2ui, Packet2ul>(a));
 }
 
 template <>
@@ -840,6 +1014,10 @@
 EIGEN_STRONG_INLINE Packet4us pcast<Packet2ui, Packet4us>(const Packet2ui& a, const Packet2ui& b) {
   return vmovn_u32(vcombine_u32(a, b));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet4ui, Packet4us>(const Packet4ui& a) {
+  return vmovn_u32(a);
+}
 
 template <>
 struct type_casting_traits<numext::uint32_t, numext::int16_t> {
@@ -847,11 +1025,15 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet8s pcast<Packet4ui, Packet8s>(const Packet4ui& a, const Packet4ui& b) {
-  return vreinterpretq_s16_u16(pcast<Packet4ui, Packet8us>(a, b));
+  return preinterpret<Packet8s>(pcast<Packet4ui, Packet8us>(a, b));
 }
 template <>
 EIGEN_STRONG_INLINE Packet4s pcast<Packet2ui, Packet4s>(const Packet2ui& a, const Packet2ui& b) {
-  return vreinterpret_s16_u16(pcast<Packet2ui, Packet4us>(a, b));
+  return preinterpret<Packet4s>(pcast<Packet2ui, Packet4us>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet4ui, Packet4s>(const Packet4ui& a) {
+  return preinterpret<Packet4s>(pcast<Packet4ui, Packet4us>(a));
 }
 
 template <>
@@ -866,12 +1048,24 @@
   return vcombine_u8(vmovn_u16(ab_u16), vmovn_u16(cd_u16));
 }
 template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet4ui, Packet8uc>(const Packet4ui& a, const Packet4ui& b) {
+  const uint16x8_t ab_u16 = vcombine_u16(vmovn_u32(a), vmovn_u32(b));
+  return vmovn_u16(ab_u16);
+}
+template <>
 EIGEN_STRONG_INLINE Packet8uc pcast<Packet2ui, Packet8uc>(const Packet2ui& a, const Packet2ui& b, const Packet2ui& c,
                                                           const Packet2ui& d) {
   const uint16x4_t ab_u16 = vmovn_u32(vcombine_u32(a, b));
   const uint16x4_t cd_u16 = vmovn_u32(vcombine_u32(c, d));
   return vmovn_u16(vcombine_u16(ab_u16, cd_u16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet4ui, Packet4uc>(const Packet4ui& a) {
+  const uint16x4_t a_u16x4 = vmovn_u32(a);
+  const uint16x8_t aa_u16x8 = vcombine_u16(a_u16x4, a_u16x4);
+  const uint8x8_t aa_u8x8 = vmovn_u16(aa_u16x8);
+  return vget_lane_u32(vreinterpret_u32_u8(aa_u8x8), 0);
+}
 
 template <>
 struct type_casting_traits<numext::uint32_t, numext::int8_t> {
@@ -880,12 +1074,20 @@
 template <>
 EIGEN_STRONG_INLINE Packet16c pcast<Packet4ui, Packet16c>(const Packet4ui& a, const Packet4ui& b, const Packet4ui& c,
                                                           const Packet4ui& d) {
-  return vreinterpretq_s8_u8(pcast<Packet4ui, Packet16uc>(a, b, c, d));
+  return preinterpret<Packet16c>(pcast<Packet4ui, Packet16uc>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet4ui, Packet8c>(const Packet4ui& a, const Packet4ui& b) {
+  return preinterpret<Packet8c>(pcast<Packet4ui, Packet8uc>(a, b));
 }
 template <>
 EIGEN_STRONG_INLINE Packet8c pcast<Packet2ui, Packet8c>(const Packet2ui& a, const Packet2ui& b, const Packet2ui& c,
                                                         const Packet2ui& d) {
-  return vreinterpret_s8_u8(pcast<Packet2ui, Packet8uc>(a, b, c, d));
+  return preinterpret<Packet8c>(pcast<Packet2ui, Packet8uc>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet4ui, Packet4c>(const Packet4ui& a) {
+  return static_cast<Packet4c>(pcast<Packet4ui, Packet4uc>(a));
 }
 
 //==============================================================================
@@ -895,27 +1097,31 @@
 struct type_casting_traits<numext::int64_t, float> {
   enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
 };
+
 template <>
 EIGEN_STRONG_INLINE Packet4f pcast<Packet2l, Packet4f>(const Packet2l& a, const Packet2l& b) {
-  return vcvtq_f32_s32(vcombine_s32(vmovn_s64(a), vmovn_s64(b)));
+#if EIGEN_ARCH_ARM64
+  return vcombine_f32(vcvt_f32_f64(vcvtq_f64_s64(a)), vcvt_f32_f64(vcvtq_f64_s64(b)));
+#else
+  EIGEN_ALIGN_MAX int64_t lvals[4];
+  pstore(lvals, a);
+  pstore(lvals + 2, b);
+  EIGEN_ALIGN_MAX float fvals[4] = {static_cast<float>(lvals[0]), static_cast<float>(lvals[1]),
+                                    static_cast<float>(lvals[2]), static_cast<float>(lvals[3])};
+  return pload<Packet4f>(fvals);
+#endif
 }
 
 template <>
-struct type_casting_traits<numext::int64_t, numext::int64_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet2l pcast<Packet2l, Packet2l>(const Packet2l& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::int64_t, numext::uint64_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet2ul pcast<Packet2l, Packet2ul>(const Packet2l& a) {
-  return vreinterpretq_u64_s64(a);
+EIGEN_STRONG_INLINE Packet2f pcast<Packet2l, Packet2f>(const Packet2l& a) {
+#if EIGEN_ARCH_ARM64
+  return vcvt_f32_f64(vcvtq_f64_s64(a));
+#else
+  EIGEN_ALIGN_MAX int64_t lvals[2];
+  pstore(lvals, a);
+  EIGEN_ALIGN_MAX float fvals[2] = {static_cast<float>(lvals[0]), static_cast<float>(lvals[1])};
+  return pload<Packet2f>(fvals);
+#endif
 }
 
 template <>
@@ -926,6 +1132,10 @@
 EIGEN_STRONG_INLINE Packet4i pcast<Packet2l, Packet4i>(const Packet2l& a, const Packet2l& b) {
   return vcombine_s32(vmovn_s64(a), vmovn_s64(b));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet2l, Packet2i>(const Packet2l& a) {
+  return vmovn_s64(a);
+}
 
 template <>
 struct type_casting_traits<numext::int64_t, numext::uint32_t> {
@@ -935,6 +1145,10 @@
 EIGEN_STRONG_INLINE Packet4ui pcast<Packet2l, Packet4ui>(const Packet2l& a, const Packet2l& b) {
   return vcombine_u32(vmovn_u64(vreinterpretq_u64_s64(a)), vmovn_u64(vreinterpretq_u64_s64(b)));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet2l, Packet2ui>(const Packet2l& a) {
+  return vmovn_u64(vreinterpretq_u64_s64(a));
+}
 
 template <>
 struct type_casting_traits<numext::int64_t, numext::int16_t> {
@@ -947,6 +1161,11 @@
   const int32x4_t cd_s32 = pcast<Packet2l, Packet4i>(c, d);
   return vcombine_s16(vmovn_s32(ab_s32), vmovn_s32(cd_s32));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet2l, Packet4s>(const Packet2l& a, const Packet2l& b) {
+  const int32x4_t ab_s32 = pcast<Packet2l, Packet4i>(a, b);
+  return vmovn_s32(ab_s32);
+}
 
 template <>
 struct type_casting_traits<numext::int64_t, numext::uint16_t> {
@@ -955,9 +1174,11 @@
 template <>
 EIGEN_STRONG_INLINE Packet8us pcast<Packet2l, Packet8us>(const Packet2l& a, const Packet2l& b, const Packet2l& c,
                                                          const Packet2l& d) {
-  const uint32x4_t ab_u32 = pcast<Packet2l, Packet4ui>(a, b);
-  const uint32x4_t cd_u32 = pcast<Packet2l, Packet4ui>(c, d);
-  return vcombine_u16(vmovn_u32(ab_u32), vmovn_u32(cd_u32));
+  return preinterpret<Packet8us>(pcast<Packet2l, Packet8s>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet2l, Packet4us>(const Packet2l& a, const Packet2l& b) {
+  return preinterpret<Packet4us>(pcast<Packet2l, Packet4s>(a, b));
 }
 
 template <>
@@ -972,6 +1193,19 @@
   const int16x8_t efgh_s16 = pcast<Packet2l, Packet8s>(e, f, g, h);
   return vcombine_s8(vmovn_s16(abcd_s16), vmovn_s16(efgh_s16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet2l, Packet8c>(const Packet2l& a, const Packet2l& b, const Packet2l& c,
+                                                       const Packet2l& d) {
+  const int16x8_t abcd_s16 = pcast<Packet2l, Packet8s>(a, b, c, d);
+  return vmovn_s16(abcd_s16);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet2l, Packet4c>(const Packet2l& a, const Packet2l& b) {
+  const int16x4_t ab_s16 = pcast<Packet2l, Packet4s>(a, b);
+  const int16x8_t abab_s16 = vcombine_s16(ab_s16, ab_s16);
+  const int8x8_t abab_s8 = vmovn_s16(abab_s16);
+  return vget_lane_s32(vreinterpret_s32_s8(abab_s8), 0);
+}
 
 template <>
 struct type_casting_traits<numext::int64_t, numext::uint8_t> {
@@ -985,6 +1219,15 @@
   const uint16x8_t efgh_u16 = pcast<Packet2l, Packet8us>(e, f, g, h);
   return vcombine_u8(vmovn_u16(abcd_u16), vmovn_u16(efgh_u16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet2l, Packet8uc>(const Packet2l& a, const Packet2l& b, const Packet2l& c,
+                                                         const Packet2l& d) {
+  return preinterpret<Packet8uc>(pcast<Packet2l, Packet8c>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet2l, Packet4uc>(const Packet2l& a, const Packet2l& b) {
+  return static_cast<Packet4uc>(pcast<Packet2l, Packet4c>(a, b));
+}
 
 //==============================================================================
 // pcast, SrcType = uint64_t
@@ -995,25 +1238,27 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet4f pcast<Packet2ul, Packet4f>(const Packet2ul& a, const Packet2ul& b) {
-  return vcvtq_f32_u32(vcombine_u32(vmovn_u64(a), vmovn_u64(b)));
+#if EIGEN_ARCH_ARM64
+  return vcombine_f32(vcvt_f32_f64(vcvtq_f64_u64(a)), vcvt_f32_f64(vcvtq_f64_u64(b)));
+#else
+  EIGEN_ALIGN_MAX uint64_t uvals[4];
+  pstore(uvals, a);
+  pstore(uvals + 2, b);
+  EIGEN_ALIGN_MAX float fvals[4] = {static_cast<float>(uvals[0]), static_cast<float>(uvals[1]),
+                                    static_cast<float>(uvals[2]), static_cast<float>(uvals[3])};
+  return pload<Packet4f>(fvals);
+#endif
 }
-
 template <>
-struct type_casting_traits<numext::uint64_t, numext::uint64_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet2ul pcast<Packet2ul, Packet2ul>(const Packet2ul& a) {
-  return a;
-}
-
-template <>
-struct type_casting_traits<numext::uint64_t, numext::int64_t> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
-template <>
-EIGEN_STRONG_INLINE Packet2l pcast<Packet2ul, Packet2l>(const Packet2ul& a) {
-  return vreinterpretq_s64_u64(a);
+EIGEN_STRONG_INLINE Packet2f pcast<Packet2ul, Packet2f>(const Packet2ul& a) {
+#if EIGEN_ARCH_ARM64
+  return vcvt_f32_f64(vcvtq_f64_u64(a));
+#else
+  EIGEN_ALIGN_MAX uint64_t uvals[2];
+  pstore(uvals, a);
+  EIGEN_ALIGN_MAX float fvals[2] = {static_cast<float>(uvals[0]), static_cast<float>(uvals[1])};
+  return pload<Packet2f>(fvals);
+#endif
 }
 
 template <>
@@ -1024,6 +1269,10 @@
 EIGEN_STRONG_INLINE Packet4ui pcast<Packet2ul, Packet4ui>(const Packet2ul& a, const Packet2ul& b) {
   return vcombine_u32(vmovn_u64(a), vmovn_u64(b));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet2ul, Packet2ui>(const Packet2ul& a) {
+  return vmovn_u64(a);
+}
 
 template <>
 struct type_casting_traits<numext::uint64_t, numext::int32_t> {
@@ -1031,7 +1280,11 @@
 };
 template <>
 EIGEN_STRONG_INLINE Packet4i pcast<Packet2ul, Packet4i>(const Packet2ul& a, const Packet2ul& b) {
-  return vreinterpretq_s32_u32(pcast<Packet2ul, Packet4ui>(a, b));
+  return preinterpret<Packet4i>(pcast<Packet2ul, Packet4ui>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet2ul, Packet2i>(const Packet2ul& a) {
+  return preinterpret<Packet2i>(pcast<Packet2ul, Packet2ui>(a));
 }
 
 template <>
@@ -1045,6 +1298,10 @@
   const uint16x4_t cd_u16 = vmovn_u32(vcombine_u32(vmovn_u64(c), vmovn_u64(d)));
   return vcombine_u16(ab_u16, cd_u16);
 }
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet2ul, Packet4us>(const Packet2ul& a, const Packet2ul& b) {
+  return vmovn_u32(vcombine_u32(vmovn_u64(a), vmovn_u64(b)));
+}
 
 template <>
 struct type_casting_traits<numext::uint64_t, numext::int16_t> {
@@ -1053,7 +1310,11 @@
 template <>
 EIGEN_STRONG_INLINE Packet8s pcast<Packet2ul, Packet8s>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
                                                         const Packet2ul& d) {
-  return vreinterpretq_s16_u16(pcast<Packet2ul, Packet8us>(a, b, c, d));
+  return preinterpret<Packet8s>(pcast<Packet2ul, Packet8us>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet2ul, Packet4s>(const Packet2ul& a, const Packet2ul& b) {
+  return preinterpret<Packet4s>(pcast<Packet2ul, Packet4us>(a, b));
 }
 
 template <>
@@ -1068,6 +1329,19 @@
   const uint16x8_t efgh_u16 = pcast<Packet2ul, Packet8us>(e, f, g, h);
   return vcombine_u8(vmovn_u16(abcd_u16), vmovn_u16(efgh_u16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet2ul, Packet8uc>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
+                                                          const Packet2ul& d) {
+  const uint16x8_t abcd_u16 = pcast<Packet2ul, Packet8us>(a, b, c, d);
+  return vmovn_u16(abcd_u16);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet2ul, Packet4uc>(const Packet2ul& a, const Packet2ul& b) {
+  const uint16x4_t ab_u16 = pcast<Packet2ul, Packet4us>(a, b);
+  const uint16x8_t abab_u16 = vcombine_u16(ab_u16, ab_u16);
+  const uint8x8_t abab_u8 = vmovn_u16(abab_u16);
+  return vget_lane_u32(vreinterpret_u32_u8(abab_u8), 0);
+}
 
 template <>
 struct type_casting_traits<numext::uint64_t, numext::int8_t> {
@@ -1077,114 +1351,16 @@
 EIGEN_STRONG_INLINE Packet16c pcast<Packet2ul, Packet16c>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
                                                           const Packet2ul& d, const Packet2ul& e, const Packet2ul& f,
                                                           const Packet2ul& g, const Packet2ul& h) {
-  return vreinterpretq_s8_u8(pcast<Packet2ul, Packet16uc>(a, b, c, d, e, f, g, h));
-}
-
-//==============================================================================
-// preinterpret
-//==============================================================================
-template <>
-EIGEN_STRONG_INLINE Packet2f preinterpret<Packet2f, Packet2i>(const Packet2i& a) {
-  return vreinterpret_f32_s32(a);
+  return preinterpret<Packet16c>(pcast<Packet2ul, Packet16uc>(a, b, c, d, e, f, g, h));
 }
 template <>
-EIGEN_STRONG_INLINE Packet2f preinterpret<Packet2f, Packet2ui>(const Packet2ui& a) {
-  return vreinterpret_f32_u32(a);
+EIGEN_STRONG_INLINE Packet8c pcast<Packet2ul, Packet8c>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
+                                                        const Packet2ul& d) {
+  return preinterpret<Packet8c>(pcast<Packet2ul, Packet8uc>(a, b, c, d));
 }
 template <>
-EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet4i>(const Packet4i& a) {
-  return vreinterpretq_f32_s32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet4ui>(const Packet4ui& a) {
-  return vreinterpretq_f32_u32(a);
-}
-
-template <>
-EIGEN_STRONG_INLINE Packet4c preinterpret<Packet4c, Packet4uc>(const Packet4uc& a) {
-  return static_cast<Packet4c>(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet8c preinterpret<Packet8c, Packet8uc>(const Packet8uc& a) {
-  return vreinterpret_s8_u8(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet16c preinterpret<Packet16c, Packet16uc>(const Packet16uc& a) {
-  return vreinterpretq_s8_u8(a);
-}
-
-template <>
-EIGEN_STRONG_INLINE Packet4uc preinterpret<Packet4uc, Packet4c>(const Packet4c& a) {
-  return static_cast<Packet4uc>(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet8uc preinterpret<Packet8uc, Packet8c>(const Packet8c& a) {
-  return vreinterpret_u8_s8(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet16uc preinterpret<Packet16uc, Packet16c>(const Packet16c& a) {
-  return vreinterpretq_u8_s8(a);
-}
-
-template <>
-EIGEN_STRONG_INLINE Packet4s preinterpret<Packet4s, Packet4us>(const Packet4us& a) {
-  return vreinterpret_s16_u16(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet8s preinterpret<Packet8s, Packet8us>(const Packet8us& a) {
-  return vreinterpretq_s16_u16(a);
-}
-
-template <>
-EIGEN_STRONG_INLINE Packet4us preinterpret<Packet4us, Packet4s>(const Packet4s& a) {
-  return vreinterpret_u16_s16(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet8us preinterpret<Packet8us, Packet8s>(const Packet8s& a) {
-  return vreinterpretq_u16_s16(a);
-}
-
-template <>
-EIGEN_STRONG_INLINE Packet2i preinterpret<Packet2i, Packet2f>(const Packet2f& a) {
-  return vreinterpret_s32_f32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2i preinterpret<Packet2i, Packet2ui>(const Packet2ui& a) {
-  return vreinterpret_s32_u32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4f>(const Packet4f& a) {
-  return vreinterpretq_s32_f32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4ui>(const Packet4ui& a) {
-  return vreinterpretq_s32_u32(a);
-}
-
-template <>
-EIGEN_STRONG_INLINE Packet2ui preinterpret<Packet2ui, Packet2f>(const Packet2f& a) {
-  return vreinterpret_u32_f32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2ui preinterpret<Packet2ui, Packet2i>(const Packet2i& a) {
-  return vreinterpret_u32_s32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet4f>(const Packet4f& a) {
-  return vreinterpretq_u32_f32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet4i>(const Packet4i& a) {
-  return vreinterpretq_u32_s32(a);
-}
-
-template <>
-EIGEN_STRONG_INLINE Packet2l preinterpret<Packet2l, Packet2ul>(const Packet2ul& a) {
-  return vreinterpretq_s64_u64(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2ul preinterpret<Packet2ul, Packet2l>(const Packet2l& a) {
-  return vreinterpretq_u64_s64(a);
+EIGEN_STRONG_INLINE Packet4c pcast<Packet2ul, Packet4c>(const Packet2ul& a, const Packet2ul& b) {
+  return static_cast<Packet4c>(pcast<Packet2ul, Packet4uc>(a, b));
 }
 
 #if EIGEN_ARCH_ARM64
@@ -1194,12 +1370,28 @@
 //==============================================================================
 
 template <>
-struct type_casting_traits<double, double> {
-  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
-};
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet2l>(const Packet2l& a) {
+  return Packet2d(vreinterpretq_f64_s64(a));
+}
 template <>
-EIGEN_STRONG_INLINE Packet2d pcast<Packet2d, Packet2d>(const Packet2d& a) {
-  return a;
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet2ul>(const Packet2ul& a) {
+  return Packet2d(vreinterpretq_f64_u64(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l preinterpret<Packet2l, Packet2d>(const Packet2d& a) {
+  return Packet2l(vreinterpretq_s64_f64(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul preinterpret<Packet2ul, Packet2d>(const Packet2d& a) {
+  return Packet2ul(vreinterpretq_u64_f64(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet4i>(const Packet4i& a) {
+  return Packet2d(vreinterpretq_f64_s32(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet2d>(const Packet2d& a) {
+  return Packet4i(vreinterpretq_s32_f64(a));
 }
 
 template <>
@@ -1210,6 +1402,10 @@
 EIGEN_STRONG_INLINE Packet4f pcast<Packet2d, Packet4f>(const Packet2d& a, const Packet2d& b) {
   return vcombine_f32(vcvt_f32_f64(a), vcvt_f32_f64(b));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet2d, Packet2f>(const Packet2d& a) {
+  return vcvt_f32_f64(a);
+}
 
 template <>
 struct type_casting_traits<double, numext::int64_t> {
@@ -1237,6 +1433,10 @@
 EIGEN_STRONG_INLINE Packet4i pcast<Packet2d, Packet4i>(const Packet2d& a, const Packet2d& b) {
   return vcombine_s32(vmovn_s64(vcvtq_s64_f64(a)), vmovn_s64(vcvtq_s64_f64(b)));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet2d, Packet2i>(const Packet2d& a) {
+  return vmovn_s64(vcvtq_s64_f64(a));
+}
 
 template <>
 struct type_casting_traits<double, numext::uint32_t> {
@@ -1246,6 +1446,10 @@
 EIGEN_STRONG_INLINE Packet4ui pcast<Packet2d, Packet4ui>(const Packet2d& a, const Packet2d& b) {
   return vcombine_u32(vmovn_u64(vcvtq_u64_f64(a)), vmovn_u64(vcvtq_u64_f64(b)));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet2d, Packet2ui>(const Packet2d& a) {
+  return vmovn_u64(vcvtq_u64_f64(a));
+}
 
 template <>
 struct type_casting_traits<double, numext::int16_t> {
@@ -1258,6 +1462,11 @@
   const int32x4_t cd_s32 = pcast<Packet2d, Packet4i>(c, d);
   return vcombine_s16(vmovn_s32(ab_s32), vmovn_s32(cd_s32));
 }
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet2d, Packet4s>(const Packet2d& a, const Packet2d& b) {
+  const int32x4_t ab_s32 = pcast<Packet2d, Packet4i>(a, b);
+  return vmovn_s32(ab_s32);
+}
 
 template <>
 struct type_casting_traits<double, numext::uint16_t> {
@@ -1266,9 +1475,11 @@
 template <>
 EIGEN_STRONG_INLINE Packet8us pcast<Packet2d, Packet8us>(const Packet2d& a, const Packet2d& b, const Packet2d& c,
                                                          const Packet2d& d) {
-  const uint32x4_t ab_u32 = pcast<Packet2d, Packet4ui>(a, b);
-  const uint32x4_t cd_u32 = pcast<Packet2d, Packet4ui>(c, d);
-  return vcombine_u16(vmovn_u32(ab_u32), vmovn_u32(cd_u32));
+  return preinterpret<Packet8us>(pcast<Packet2d, Packet8s>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet2d, Packet4us>(const Packet2d& a, const Packet2d& b) {
+  return preinterpret<Packet4us>(pcast<Packet2d, Packet4s>(a, b));
 }
 
 template <>
@@ -1283,6 +1494,17 @@
   const int16x8_t efgh_s16 = pcast<Packet2d, Packet8s>(e, f, g, h);
   return vcombine_s8(vmovn_s16(abcd_s16), vmovn_s16(efgh_s16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet2d, Packet8c>(const Packet2d& a, const Packet2d& b, const Packet2d& c,
+                                                       const Packet2d& d) {
+  const int16x8_t abcd_s16 = pcast<Packet2d, Packet8s>(a, b, c, d);
+  return vmovn_s16(abcd_s16);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet2d, Packet4c>(const Packet2d& a, const Packet2d& b) {
+  const int32x4_t ab_s32 = pcast<Packet2d, Packet4i>(a, b);
+  return pcast<Packet4i, Packet4c>(ab_s32);
+}
 
 template <>
 struct type_casting_traits<double, numext::uint8_t> {
@@ -1296,6 +1518,15 @@
   const uint16x8_t efgh_u16 = pcast<Packet2d, Packet8us>(e, f, g, h);
   return vcombine_u8(vmovn_u16(abcd_u16), vmovn_u16(efgh_u16));
 }
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet2d, Packet8uc>(const Packet2d& a, const Packet2d& b, const Packet2d& c,
+                                                         const Packet2d& d) {
+  return preinterpret<Packet8uc>(pcast<Packet2d, Packet8c>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet2d, Packet4uc>(const Packet2d& a, const Packet2d& b) {
+  return static_cast<Packet4uc>(pcast<Packet2d, Packet4c>(a, b));
+}
 
 template <>
 struct type_casting_traits<float, double> {
@@ -1306,6 +1537,10 @@
   // Discard second-half of input.
   return vcvt_f64_f32(vget_low_f32(a));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet2f, Packet2d>(const Packet2f& a) {
+  return vcvt_f64_f32(a);
+}
 
 template <>
 struct type_casting_traits<numext::int8_t, double> {
@@ -1314,7 +1549,9 @@
 template <>
 EIGEN_STRONG_INLINE Packet2d pcast<Packet16c, Packet2d>(const Packet16c& a) {
   // Discard all but first two values.
-  return vcvt_f64_f32(pcast<Packet8c, Packet2f>(vget_low_s8(a)));
+  // MSVC defines most intrinsics as macros, so we need to do this in two lines for portability.
+  Packet2f tmp = pcast<Packet8c, Packet2f>(vget_low_s8(a));
+  return vcvt_f64_f32(tmp);
 }
 
 template <>
@@ -1324,7 +1561,8 @@
 template <>
 EIGEN_STRONG_INLINE Packet2d pcast<Packet16uc, Packet2d>(const Packet16uc& a) {
   // Discard all but first two values.
-  return vcvt_f64_f32(pcast<Packet8uc, Packet2f>(vget_low_u8(a)));
+  Packet2f tmp = pcast<Packet8uc, Packet2f>(vget_low_u8(a));
+  return vcvt_f64_f32(tmp);
 }
 
 template <>
@@ -1334,7 +1572,8 @@
 template <>
 EIGEN_STRONG_INLINE Packet2d pcast<Packet8s, Packet2d>(const Packet8s& a) {
   // Discard all but first two values.
-  return vcvt_f64_f32(pcast<Packet4s, Packet2f>(vget_low_s16(a)));
+  Packet2f tmp = pcast<Packet4s, Packet2f>(vget_low_s16(a));
+  return vcvt_f64_f32(tmp);
 }
 
 template <>
@@ -1344,7 +1583,8 @@
 template <>
 EIGEN_STRONG_INLINE Packet2d pcast<Packet8us, Packet2d>(const Packet8us& a) {
   // Discard all but first two values.
-  return vcvt_f64_f32(pcast<Packet4us, Packet2f>(vget_low_u16(a)));
+  Packet2f tmp = pcast<Packet4us, Packet2f>(vget_low_u16(a));
+  return vcvt_f64_f32(tmp);
 }
 
 template <>
@@ -1356,6 +1596,10 @@
   // Discard second half of input.
   return vcvtq_f64_s64(vmovl_s32(vget_low_s32(a)));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet2i, Packet2d>(const Packet2i& a) {
+  return vcvtq_f64_s64(vmovl_s32(a));
+}
 
 template <>
 struct type_casting_traits<numext::uint32_t, double> {
@@ -1366,6 +1610,10 @@
   // Discard second half of input.
   return vcvtq_f64_u64(vmovl_u32(vget_low_u32(a)));
 }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet2ui, Packet2d>(const Packet2ui& a) {
+  return vcvtq_f64_u64(vmovl_u32(a));
+}
 
 template <>
 struct type_casting_traits<numext::int64_t, double> {
@@ -1385,31 +1633,6 @@
   return vcvtq_f64_u64(a);
 }
 
-template <>
-EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet2l>(const Packet2l& a) {
-  return vreinterpretq_f64_s64(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet2ul>(const Packet2ul& a) {
-  return vreinterpretq_f64_u64(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2l preinterpret<Packet2l, Packet2d>(const Packet2d& a) {
-  return vreinterpretq_s64_f64(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2ul preinterpret<Packet2ul, Packet2d>(const Packet2d& a) {
-  return vreinterpretq_u64_f64(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet4i>(const Packet4i& a) {
-  return vreinterpretq_f64_s32(a);
-}
-template <>
-EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet2d>(const Packet2d& a) {
-  return vreinterpretq_s32_f64(a);
-}
-
 #endif  // EIGEN_ARCH_ARM64
 
 }  // end namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/UnaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/UnaryFunctors.h
new file mode 100644
index 0000000..8be5bb0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/UnaryFunctors.h
@@ -0,0 +1,57 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_NEON_UNARY_FUNCTORS_H
+#define EIGEN_NEON_UNARY_FUNCTORS_H
+
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
+namespace Eigen {
+
+namespace internal {
+
+#if EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
+/** \internal
+ * \brief Template specialization of the logistic function for Eigen::half.
+ */
+template <>
+struct scalar_logistic_op<Eigen::half> {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half operator()(const Eigen::half& x) const {
+    // Convert to float and call scalar_logistic_op<float>.
+    const scalar_logistic_op<float> float_op;
+    return Eigen::half(float_op(float(x)));
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half packetOp(const Eigen::half& x) const { return this->operator()(x); }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf packetOp(const Packet4hf& x) const {
+    const scalar_logistic_op<float> float_op;
+    return vcvt_f16_f32(float_op.packetOp(vcvt_f32_f16(x)));
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf packetOp(const Packet8hf& x) const {
+    const scalar_logistic_op<float> float_op;
+    return vcombine_f16(vcvt_f16_f32(float_op.packetOp(vcvt_f32_f16(vget_low_f16(x)))),
+                        vcvt_f16_f32(float_op.packetOp(vcvt_high_f32_f16(x))));
+  }
+};
+
+template <>
+struct functor_traits<scalar_logistic_op<Eigen::half>> {
+  enum {
+    Cost = functor_traits<scalar_logistic_op<float>>::Cost,
+    PacketAccess = functor_traits<scalar_logistic_op<float>>::PacketAccess,
+  };
+};
+#endif  // EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_NEON_UNARY_FUNCTORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h
index 8fe22da..4c5c499 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h
@@ -10,13 +10,15 @@
 #ifndef EIGEN_COMPLEX_SSE_H
 #define EIGEN_COMPLEX_SSE_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 //---------- float ----------
-struct Packet2cf
-{
+struct Packet2cf {
   EIGEN_STRONG_INLINE Packet2cf() {}
   EIGEN_STRONG_INLINE explicit Packet2cf(const __m128& a) : v(a) {}
   Packet4f v;
@@ -25,166 +27,188 @@
 // Use the packet_traits defined in AVX/PacketMath.h instead if we're going
 // to leverage AVX instructions.
 #ifndef EIGEN_VECTORIZE_AVX
-template<> struct packet_traits<std::complex<float> >  : default_packet_traits
-{
+template <>
+struct packet_traits<std::complex<float> > : default_packet_traits {
   typedef Packet2cf type;
   typedef Packet2cf half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 2,
-    HasHalfPacket = 0,
 
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasDiv    = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
     HasNegate = 1,
-    HasSqrt   = 1,
-    HasAbs    = 0,
-    HasAbs2   = 0,
-    HasMin    = 0,
-    HasMax    = 0,
+    HasSqrt = 1,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
     HasSetLinear = 0,
-    HasBlend  = 1
+    HasBlend = 1
   };
 };
 #endif
 
-template<> struct unpacket_traits<Packet2cf> {
+template <>
+struct unpacket_traits<Packet2cf> {
   typedef std::complex<float> type;
   typedef Packet2cf half;
   typedef Packet4f as_real;
   enum {
-    size=2,
-    alignment=Aligned16,
-    vectorizable=true,
-    masked_load_available=false,
-    masked_store_available=false
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet2cf padd<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_add_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf psub<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_sub_ps(a.v,b.v)); }
-
-template<> EIGEN_STRONG_INLINE Packet2cf pnegate(const Packet2cf& a)
-{
-  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000,0x80000000,0x80000000,0x80000000));
-  return Packet2cf(_mm_xor_ps(a.v,mask));
+template <>
+EIGEN_STRONG_INLINE Packet2cf padd<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(_mm_add_ps(a.v, b.v));
 }
-template<> EIGEN_STRONG_INLINE Packet2cf pconj(const Packet2cf& a)
-{
-  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x00000000,0x80000000,0x00000000,0x80000000));
-  return Packet2cf(_mm_xor_ps(a.v,mask));
+template <>
+EIGEN_STRONG_INLINE Packet2cf psub<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(_mm_sub_ps(a.v, b.v));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf pmul<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{
-  #ifdef EIGEN_VECTORIZE_SSE3
-  return Packet2cf(_mm_addsub_ps(_mm_mul_ps(_mm_moveldup_ps(a.v), b.v),
-                                 _mm_mul_ps(_mm_movehdup_ps(a.v),
-                                            vec4f_swizzle1(b.v, 1, 0, 3, 2))));
-//   return Packet2cf(_mm_addsub_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 0, 0, 2, 2), b.v),
-//                                  _mm_mul_ps(vec4f_swizzle1(a.v, 1, 1, 3, 3),
-//                                             vec4f_swizzle1(b.v, 1, 0, 3, 2))));
-  #else
-  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000,0x00000000,0x80000000,0x00000000));
-  return Packet2cf(_mm_add_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 0, 0, 2, 2), b.v),
-                              _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 1, 1, 3, 3),
-                                                    vec4f_swizzle1(b.v, 1, 0, 3, 2)), mask)));
-  #endif
+template <>
+EIGEN_STRONG_INLINE Packet2cf pnegate(const Packet2cf& a) {
+  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000, 0x80000000, 0x80000000, 0x80000000));
+  return Packet2cf(_mm_xor_ps(a.v, mask));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pconj(const Packet2cf& a) {
+  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x00000000, 0x80000000, 0x00000000, 0x80000000));
+  return Packet2cf(_mm_xor_ps(a.v, mask));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf ptrue  <Packet2cf>(const Packet2cf& a) { return Packet2cf(ptrue(Packet4f(a.v))); }
-template<> EIGEN_STRONG_INLINE Packet2cf pand   <Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_and_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf por    <Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_or_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf pxor   <Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_xor_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf pandnot<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_andnot_ps(b.v,a.v)); }
-
-template<> EIGEN_STRONG_INLINE Packet2cf pload <Packet2cf>(const std::complex<float>* from) { EIGEN_DEBUG_ALIGNED_LOAD return Packet2cf(pload<Packet4f>(&numext::real_ref(*from))); }
-template<> EIGEN_STRONG_INLINE Packet2cf ploadu<Packet2cf>(const std::complex<float>* from) { EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cf(ploadu<Packet4f>(&numext::real_ref(*from))); }
-
-template<> EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>&  from)
-{
-  Packet2cf res;
+template <>
+EIGEN_STRONG_INLINE Packet2cf pmul<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
 #ifdef EIGEN_VECTORIZE_SSE3
-  res.v = _mm_castpd_ps(_mm_loaddup_pd(reinterpret_cast<double const*>(&from)));
+  return Packet2cf(_mm_addsub_ps(_mm_mul_ps(_mm_moveldup_ps(a.v), b.v),
+                                 _mm_mul_ps(_mm_movehdup_ps(a.v), vec4f_swizzle1(b.v, 1, 0, 3, 2))));
+  //   return Packet2cf(_mm_addsub_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 0, 0, 2, 2), b.v),
+  //                                  _mm_mul_ps(vec4f_swizzle1(a.v, 1, 1, 3, 3),
+  //                                             vec4f_swizzle1(b.v, 1, 0, 3, 2))));
 #else
-  res.v = _mm_castpd_ps(_mm_load_sd(reinterpret_cast<double const*>(&from)));
-  res.v = _mm_movelh_ps(res.v, res.v);
+  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000, 0x00000000, 0x80000000, 0x00000000));
+  return Packet2cf(
+      _mm_add_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 0, 0, 2, 2), b.v),
+                 _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 1, 1, 3, 3), vec4f_swizzle1(b.v, 1, 0, 3, 2)), mask)));
 #endif
-  return res;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<float>* from) { return pset1<Packet2cf>(*from); }
-
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> *   to, const Packet2cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore(&numext::real_ref(*to), Packet4f(from.v)); }
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> *   to, const Packet2cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu(&numext::real_ref(*to), Packet4f(from.v)); }
-
-
-template<> EIGEN_DEVICE_FUNC inline Packet2cf pgather<std::complex<float>, Packet2cf>(const std::complex<float>* from, Index stride)
-{
-  return Packet2cf(_mm_set_ps(std::imag(from[1*stride]), std::real(from[1*stride]),
-                              std::imag(from[0*stride]), std::real(from[0*stride])));
+template <>
+EIGEN_STRONG_INLINE Packet2cf ptrue<Packet2cf>(const Packet2cf& a) {
+  return Packet2cf(ptrue(Packet4f(a.v)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pand<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(_mm_and_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf por<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(_mm_or_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pxor<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(_mm_xor_ps(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf pandnot<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return Packet2cf(_mm_andnot_ps(b.v, a.v));
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf>(std::complex<float>* to, const Packet2cf& from, Index stride)
-{
-  to[stride*0] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 0)),
-                                     _mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 1)));
-  to[stride*1] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 2)),
-                                     _mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 3)));
+template <>
+EIGEN_STRONG_INLINE Packet2cf pload<Packet2cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return Packet2cf(pload<Packet4f>(&numext::real_ref(*from)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2cf ploadu<Packet2cf>(const std::complex<float>* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cf(ploadu<Packet4f>(&numext::real_ref(*from)));
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template <>
+EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>& from) {
+  const float re = std::real(from);
+  const float im = std::imag(from);
+  return Packet2cf(_mm_set_ps(im, re, im, re));
+}
 
-template<> EIGEN_STRONG_INLINE std::complex<float>  pfirst<Packet2cf>(const Packet2cf& a)
-{
-  #if EIGEN_GNUC_AT_MOST(4,3)
-  // Workaround gcc 4.2 ICE - this is not performance wise ideal, but who cares...
-  // This workaround also fix invalid code generation with gcc 4.3
-  EIGEN_ALIGN16 std::complex<float> res[2];
-  _mm_store_ps((float*)res, a.v);
-  return res[0];
-  #else
-  std::complex<float> res;
+template <>
+EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<float>* from) {
+  return pset1<Packet2cf>(*from);
+}
+
+template <>
+EIGEN_STRONG_INLINE void pstore<std::complex<float> >(std::complex<float>* to, const Packet2cf& from) {
+  EIGEN_DEBUG_ALIGNED_STORE pstore(&numext::real_ref(*to), Packet4f(from.v));
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float>* to, const Packet2cf& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE pstoreu(&numext::real_ref(*to), Packet4f(from.v));
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline Packet2cf pgather<std::complex<float>, Packet2cf>(const std::complex<float>* from,
+                                                                           Index stride) {
+  return Packet2cf(_mm_set_ps(std::imag(from[1 * stride]), std::real(from[1 * stride]), std::imag(from[0 * stride]),
+                              std::real(from[0 * stride])));
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf>(std::complex<float>* to, const Packet2cf& from,
+                                                                       Index stride) {
+  to[stride * 0] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 0)),
+                                       _mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 1)));
+  to[stride * 1] = std::complex<float>(_mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 2)),
+                                       _mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 3)));
+}
+
+template <>
+EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float>* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
+
+template <>
+EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet2cf>(const Packet2cf& a) {
+  alignas(alignof(__m64)) std::complex<float> res;
   _mm_storel_pi((__m64*)&res, a.v);
   return res;
-  #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf preverse(const Packet2cf& a) { return Packet2cf(_mm_castpd_ps(preverse(Packet2d(_mm_castps_pd(a.v))))); }
-
-template<> EIGEN_STRONG_INLINE std::complex<float> predux<Packet2cf>(const Packet2cf& a)
-{
-  return pfirst(Packet2cf(_mm_add_ps(a.v, _mm_movehl_ps(a.v,a.v))));
+template <>
+EIGEN_STRONG_INLINE Packet2cf preverse(const Packet2cf& a) {
+  return Packet2cf(_mm_castpd_ps(preverse(Packet2d(_mm_castps_pd(a.v)))));
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet2cf>(const Packet2cf& a)
-{
-  return pfirst(pmul(a, Packet2cf(_mm_movehl_ps(a.v,a.v))));
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux<Packet2cf>(const Packet2cf& a) {
+  return pfirst(Packet2cf(_mm_add_ps(a.v, _mm_movehl_ps(a.v, a.v))));
 }
 
-EIGEN_STRONG_INLINE Packet2cf pcplxflip/* <Packet2cf> */(const Packet2cf& x)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet2cf>(const Packet2cf& a) {
+  return pfirst(pmul(a, Packet2cf(_mm_movehl_ps(a.v, a.v))));
+}
+
+EIGEN_STRONG_INLINE Packet2cf pcplxflip /* <Packet2cf> */ (const Packet2cf& x) {
   return Packet2cf(vec4f_swizzle1(x.v, 1, 0, 3, 2));
 }
 
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf, Packet4f)
 
-template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{
-  // TODO optimize it for SSE3 and 4
-  Packet2cf res = pmul(a, pconj(b));
-  __m128 s = _mm_mul_ps(b.v,b.v);
-  return Packet2cf(_mm_div_ps(res.v,_mm_add_ps(s,vec4f_swizzle1(s, 1, 0, 3, 2))));
+template <>
+EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b) {
+  return pdiv_complex(a, b);
 }
 
-
-
 //---------- double ----------
-struct Packet1cd
-{
+struct Packet1cd {
   EIGEN_STRONG_INLINE Packet1cd() {}
   EIGEN_STRONG_INLINE explicit Packet1cd(const __m128d& a) : v(a) {}
   Packet2d v;
@@ -193,125 +217,164 @@
 // Use the packet_traits defined in AVX/PacketMath.h instead if we're going
 // to leverage AVX instructions.
 #ifndef EIGEN_VECTORIZE_AVX
-template<> struct packet_traits<std::complex<double> >  : default_packet_traits
-{
+template <>
+struct packet_traits<std::complex<double> > : default_packet_traits {
   typedef Packet1cd type;
   typedef Packet1cd half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 0,
     size = 1,
-    HasHalfPacket = 0,
 
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasDiv    = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
     HasNegate = 1,
-    HasSqrt   = 1,
-    HasAbs    = 0,
-    HasAbs2   = 0,
-    HasMin    = 0,
-    HasMax    = 0,
+    HasSqrt = 1,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
     HasSetLinear = 0
   };
 };
 #endif
 
-template<> struct unpacket_traits<Packet1cd> {
+template <>
+struct unpacket_traits<Packet1cd> {
   typedef std::complex<double> type;
   typedef Packet1cd half;
   typedef Packet2d as_real;
   enum {
-    size=1,
-    alignment=Aligned16,
-    vectorizable=true,
-    masked_load_available=false,
-    masked_store_available=false
+    size = 1,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet1cd padd<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_add_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd psub<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_sub_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd pnegate(const Packet1cd& a) { return Packet1cd(pnegate(Packet2d(a.v))); }
-template<> EIGEN_STRONG_INLINE Packet1cd pconj(const Packet1cd& a)
-{
-  const __m128d mask = _mm_castsi128_pd(_mm_set_epi32(0x80000000,0x0,0x0,0x0));
-  return Packet1cd(_mm_xor_pd(a.v,mask));
+template <>
+EIGEN_STRONG_INLINE Packet1cd padd<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(_mm_add_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd psub<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(_mm_sub_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd pnegate(const Packet1cd& a) {
+  return Packet1cd(pnegate(Packet2d(a.v)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd pconj(const Packet1cd& a) {
+  const __m128d mask = _mm_castsi128_pd(_mm_set_epi32(0x80000000, 0x0, 0x0, 0x0));
+  return Packet1cd(_mm_xor_pd(a.v, mask));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd pmul<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{
-  #ifdef EIGEN_VECTORIZE_SSE3
+template <>
+EIGEN_STRONG_INLINE Packet1cd pmul<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+#ifdef EIGEN_VECTORIZE_SSE3
   return Packet1cd(_mm_addsub_pd(_mm_mul_pd(_mm_movedup_pd(a.v), b.v),
-                                 _mm_mul_pd(vec2d_swizzle1(a.v, 1, 1),
-                                            vec2d_swizzle1(b.v, 1, 0))));
-  #else
-  const __m128d mask = _mm_castsi128_pd(_mm_set_epi32(0x0,0x0,0x80000000,0x0));
+                                 _mm_mul_pd(vec2d_swizzle1(a.v, 1, 1), vec2d_swizzle1(b.v, 1, 0))));
+#else
+  const __m128d mask = _mm_castsi128_pd(_mm_set_epi32(0x0, 0x0, 0x80000000, 0x0));
   return Packet1cd(_mm_add_pd(_mm_mul_pd(vec2d_swizzle1(a.v, 0, 0), b.v),
-                              _mm_xor_pd(_mm_mul_pd(vec2d_swizzle1(a.v, 1, 1),
-                                                    vec2d_swizzle1(b.v, 1, 0)), mask)));
-  #endif
+                              _mm_xor_pd(_mm_mul_pd(vec2d_swizzle1(a.v, 1, 1), vec2d_swizzle1(b.v, 1, 0)), mask)));
+#endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd ptrue  <Packet1cd>(const Packet1cd& a) { return Packet1cd(ptrue(Packet2d(a.v))); }
-template<> EIGEN_STRONG_INLINE Packet1cd pand   <Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_and_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd por    <Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_or_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd pxor   <Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_xor_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd pandnot<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_andnot_pd(b.v,a.v)); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd ptrue<Packet1cd>(const Packet1cd& a) {
+  return Packet1cd(ptrue(Packet2d(a.v)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd pand<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(_mm_and_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd por<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(_mm_or_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd pxor<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(_mm_xor_pd(a.v, b.v));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd pandnot<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return Packet1cd(_mm_andnot_pd(b.v, a.v));
+}
 
 // FIXME force unaligned load, this is a temporary fix
-template<> EIGEN_STRONG_INLINE Packet1cd pload <Packet1cd>(const std::complex<double>* from)
-{ EIGEN_DEBUG_ALIGNED_LOAD return Packet1cd(pload<Packet2d>((const double*)from)); }
-template<> EIGEN_STRONG_INLINE Packet1cd ploadu<Packet1cd>(const std::complex<double>* from)
-{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cd(ploadu<Packet2d>((const double*)from)); }
-template<> EIGEN_STRONG_INLINE Packet1cd pset1<Packet1cd>(const std::complex<double>&  from)
-{ /* here we really have to use unaligned loads :( */ return ploadu<Packet1cd>(&from); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd pload<Packet1cd>(const std::complex<double>* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return Packet1cd(pload<Packet2d>((const double*)from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd ploadu<Packet1cd>(const std::complex<double>* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cd(ploadu<Packet2d>((const double*)from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet1cd
+pset1<Packet1cd>(const std::complex<double>& from) { /* here we really have to use unaligned loads :( */
+  return ploadu<Packet1cd>(&from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<double>* from) { return pset1<Packet1cd>(*from); }
+template <>
+EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<double>* from) {
+  return pset1<Packet1cd>(*from);
+}
 
 // FIXME force unaligned store, this is a temporary fix
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((double*)to, Packet2d(from.v)); }
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((double*)to, Packet2d(from.v)); }
+template <>
+EIGEN_STRONG_INLINE void pstore<std::complex<double> >(std::complex<double>* to, const Packet1cd& from) {
+  EIGEN_DEBUG_ALIGNED_STORE pstore((double*)to, Packet2d(from.v));
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double>* to, const Packet1cd& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE pstoreu((double*)to, Packet2d(from.v));
+}
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template <>
+EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double>* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
 
-template<> EIGEN_STRONG_INLINE std::complex<double>  pfirst<Packet1cd>(const Packet1cd& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<double> pfirst<Packet1cd>(const Packet1cd& a) {
   EIGEN_ALIGN16 double res[2];
   _mm_store_pd(res, a.v);
-  return std::complex<double>(res[0],res[1]);
+  return std::complex<double>(res[0], res[1]);
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd preverse(const Packet1cd& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet1cd preverse(const Packet1cd& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE std::complex<double> predux<Packet1cd>(const Packet1cd& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<double> predux<Packet1cd>(const Packet1cd& a) {
   return pfirst(a);
 }
 
-template<> EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet1cd>(const Packet1cd& a)
-{
+template <>
+EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet1cd>(const Packet1cd& a) {
   return pfirst(a);
 }
 
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd, Packet2d)
 
-template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{
-  // TODO optimize it for SSE3 and 4
-  Packet1cd res = pmul(a,pconj(b));
-  __m128d s = _mm_mul_pd(b.v,b.v);
-  return Packet1cd(_mm_div_pd(res.v, _mm_add_pd(s,_mm_shuffle_pd(s, s, 0x1))));
+template <>
+EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b) {
+  return pdiv_complex(a, b);
 }
 
-EIGEN_STRONG_INLINE Packet1cd pcplxflip/* <Packet1cd> */(const Packet1cd& x)
-{
+EIGEN_STRONG_INLINE Packet1cd pcplxflip /* <Packet1cd> */ (const Packet1cd& x) {
   return Packet1cd(preverse(Packet2d(x.v)));
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet2cf,2>& kernel) {
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet2cf, 2>& kernel) {
   __m128d w1 = _mm_castps_pd(kernel.packet[0].v);
   __m128d w2 = _mm_castps_pd(kernel.packet[1].v);
 
@@ -320,32 +383,36 @@
   kernel.packet[1].v = tmp;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf pcmp_eq(const Packet2cf& a, const Packet2cf& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2cf pcmp_eq(const Packet2cf& a, const Packet2cf& b) {
   __m128 eq = _mm_cmpeq_ps(a.v, b.v);
   return Packet2cf(pand<Packet4f>(eq, vec4f_swizzle1(eq, 1, 0, 3, 2)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd pcmp_eq(const Packet1cd& a, const Packet1cd& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet1cd pcmp_eq(const Packet1cd& a, const Packet1cd& b) {
   __m128d eq = _mm_cmpeq_pd(a.v, b.v);
   return Packet1cd(pand<Packet2d>(eq, vec2d_swizzle1(eq, 1, 0)));
 }
 
-template<>  EIGEN_STRONG_INLINE Packet2cf pblend(const Selector<2>& ifPacket, const Packet2cf& thenPacket, const Packet2cf& elsePacket) {
+template <>
+EIGEN_STRONG_INLINE Packet2cf pblend(const Selector<2>& ifPacket, const Packet2cf& thenPacket,
+                                     const Packet2cf& elsePacket) {
   __m128d result = pblend<Packet2d>(ifPacket, _mm_castps_pd(thenPacket.v), _mm_castps_pd(elsePacket.v));
   return Packet2cf(_mm_castpd_ps(result));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd psqrt<Packet1cd>(const Packet1cd& a) {
+template <>
+EIGEN_STRONG_INLINE Packet1cd psqrt<Packet1cd>(const Packet1cd& a) {
   return psqrt_complex<Packet1cd>(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf psqrt<Packet2cf>(const Packet2cf& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2cf psqrt<Packet2cf>(const Packet2cf& a) {
   return psqrt_complex<Packet2cf>(a);
 }
 
-} // end namespace internal
-} // end namespace Eigen
+}  // end namespace internal
+}  // end namespace Eigen
 
-#endif // EIGEN_COMPLEX_SSE_H
+#endif  // EIGEN_COMPLEX_SSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h
index 8736d0d..30c1f07 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h
@@ -15,174 +15,63 @@
 #ifndef EIGEN_MATH_FUNCTIONS_SSE_H
 #define EIGEN_MATH_FUNCTIONS_SSE_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f plog<Packet4f>(const Packet4f& _x) {
-  return plog_float(_x);
-}
+EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_FLOAT(Packet4f)
+EIGEN_INSTANTIATE_GENERIC_MATH_FUNCS_DOUBLE(Packet2d)
 
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet2d plog<Packet2d>(const Packet2d& _x) {
-  return plog_double(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f plog2<Packet4f>(const Packet4f& _x) {
-  return plog2_float(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet2d plog2<Packet2d>(const Packet2d& _x) {
-  return plog2_double(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f plog1p<Packet4f>(const Packet4f& _x) {
-  return generic_plog1p(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f pexpm1<Packet4f>(const Packet4f& _x) {
-  return generic_expm1(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f pexp<Packet4f>(const Packet4f& _x)
-{
-  return pexp_float(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet2d pexp<Packet2d>(const Packet2d& x)
-{
-  return pexp_double(x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f psin<Packet4f>(const Packet4f& _x)
-{
-  return psin_float(_x);
-}
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f pcos<Packet4f>(const Packet4f& _x)
-{
-  return pcos_float(_x);
-}
-
-#if EIGEN_FAST_MATH
-
-// Functions for sqrt.
-// The EIGEN_FAST_MATH version uses the _mm_rsqrt_ps approximation and one step
-// of Newton's method, at a cost of 1-2 bits of precision as opposed to the
-// exact solution. It does not handle +inf, or denormalized numbers correctly.
-// The main advantage of this approach is not just speed, but also the fact that
-// it can be inlined and pipelined with other computations, further reducing its
-// effective latency. This is similar to Quake3's fast inverse square root.
-// For detail see here: http://www.beyond3d.com/content/articles/8/
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f psqrt<Packet4f>(const Packet4f& _x)
-{
-  Packet4f minus_half_x = pmul(_x, pset1<Packet4f>(-0.5f));
-  Packet4f denormal_mask = pandnot(
-      pcmp_lt(_x, pset1<Packet4f>((std::numeric_limits<float>::min)())),
-      pcmp_lt(_x, pzero(_x)));
-
-  // Compute approximate reciprocal sqrt.
-  Packet4f x = _mm_rsqrt_ps(_x);
-  // Do a single step of Newton's iteration.
-  x = pmul(x, pmadd(minus_half_x, pmul(x,x), pset1<Packet4f>(1.5f)));
-  // Flush results for denormals to zero.
-  return pandnot(pmul(_x,x), denormal_mask);
-}
-
-#else
-
-template<>EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f psqrt<Packet4f>(const Packet4f& x) { return _mm_sqrt_ps(x); }
-
-#endif
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet2d psqrt<Packet2d>(const Packet2d& x) { return _mm_sqrt_pd(x); }
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet16b psqrt<Packet16b>(const Packet16b& x) { return x; }
-
-#if EIGEN_FAST_MATH
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f prsqrt<Packet4f>(const Packet4f& _x) {
-  _EIGEN_DECLARE_CONST_Packet4f(one_point_five, 1.5f);
-  _EIGEN_DECLARE_CONST_Packet4f(minus_half, -0.5f);
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(inf, 0x7f800000u);
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(flt_min, 0x00800000u);
-
-  Packet4f neg_half = pmul(_x, p4f_minus_half);
-
-  // Identity infinite, zero, negative and denormal arguments.
-  Packet4f lt_min_mask = _mm_cmplt_ps(_x, p4f_flt_min);
-  Packet4f inf_mask = _mm_cmpeq_ps(_x, p4f_inf);
-  Packet4f not_normal_finite_mask = _mm_or_ps(lt_min_mask, inf_mask);
-
-  // Compute an approximate result using the rsqrt intrinsic.
-  Packet4f y_approx = _mm_rsqrt_ps(_x);
-
-  // Do a single step of Newton-Raphson iteration to improve the approximation.
-  // This uses the formula y_{n+1} = y_n * (1.5 - y_n * (0.5 * x) * y_n).
-  // It is essential to evaluate the inner term like this because forming
-  // y_n^2 may over- or underflow.
-  Packet4f y_newton = pmul(
-      y_approx, pmadd(y_approx, pmul(neg_half, y_approx), p4f_one_point_five));
-
-  // Select the result of the Newton-Raphson step for positive normal arguments.
-  // For other arguments, choose the output of the intrinsic. This will
-  // return rsqrt(+inf) = 0, rsqrt(x) = NaN if x < 0, and rsqrt(x) = +inf if
-  // x is zero or a positive denormalized float (equivalent to flushing positive
-  // denormalized inputs to zero).
-  return pselect<Packet4f>(not_normal_finite_mask, y_approx, y_newton);
-}
-
-#else
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f prsqrt<Packet4f>(const Packet4f& x) {
-  // Unfortunately we can't use the much faster mm_rsqrt_ps since it only provides an approximation.
-  return _mm_div_ps(pset1<Packet4f>(1.0f), _mm_sqrt_ps(x));
-}
-
-#endif
-
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet2d prsqrt<Packet2d>(const Packet2d& x) {
-  return _mm_div_pd(pset1<Packet2d>(1.0), _mm_sqrt_pd(x));
-}
-
-// Hyperbolic Tangent function.
+// Notice that for newer processors, it is counterproductive to use Newton
+// iteration for square root. In particular, Skylake and Zen2 processors
+// have approximately doubled throughput of the _mm_sqrt_ps instruction
+// compared to their predecessors.
 template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f
-ptanh<Packet4f>(const Packet4f& x) {
-  return internal::generic_fast_tanh_float(x);
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet4f psqrt<Packet4f>(const Packet4f& x) {
+  return _mm_sqrt_ps(x);
+}
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet2d psqrt<Packet2d>(const Packet2d& x) {
+  return _mm_sqrt_pd(x);
+}
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet16b psqrt<Packet16b>(const Packet16b& x) {
+  return x;
 }
 
-} // end namespace internal
+#if EIGEN_FAST_MATH
+// Even on Skylake, using Newton iteration is a win for reciprocal square root.
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f prsqrt<Packet4f>(const Packet4f& x) {
+  return generic_rsqrt_newton_step<Packet4f, /*Steps=*/1>::run(x, _mm_rsqrt_ps(x));
+}
+
+#ifdef EIGEN_VECTORIZE_FMA
+// Trying to speed up reciprocal using Newton-Raphson is counterproductive
+// unless FMA is available. Without FMA pdiv(pset1<Packet>(Scalar(1),a)) is
+// 30% faster.
+template <>
+EIGEN_STRONG_INLINE Packet4f preciprocal<Packet4f>(const Packet4f& x) {
+  return generic_reciprocal_newton_step<Packet4f, /*Steps=*/1>::run(x, _mm_rcp_ps(x));
+}
+#endif
+
+#endif
+
+}  // end namespace internal
 
 namespace numext {
 
-template<>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-float sqrt(const float &x)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float sqrt(const float& x) {
   return internal::pfirst(internal::Packet4f(_mm_sqrt_ss(_mm_set_ss(x))));
 }
 
-template<>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-double sqrt(const double &x)
-{
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double sqrt(const double& x) {
 #if EIGEN_COMP_GNUC_STRICT
   // This works around a GCC bug generating poor code for _mm_sqrt_pd
   // See https://gitlab.com/libeigen/eigen/commit/8dca9f97e38970
@@ -192,8 +81,8 @@
 #endif
 }
 
-} // end namespace numex
+}  // namespace numext
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATH_FUNCTIONS_SSE_H
+#endif  // EIGEN_MATH_FUNCTIONS_SSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h
index db102c7..be8183c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h
@@ -10,6 +10,10 @@
 #ifndef EIGEN_PACKET_MATH_SSE_H
 #define EIGEN_PACKET_MATH_SSE_H
 
+#include <cstdint>
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
@@ -21,7 +25,7 @@
 #if !defined(EIGEN_VECTORIZE_AVX) && !defined(EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS)
 // 32 bits =>  8 registers
 // 64 bits => 16 registers
-#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS (2*sizeof(void*))
+#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS (2 * sizeof(void*))
 #endif
 
 #ifdef EIGEN_VECTORIZE_FMA
@@ -30,94 +34,111 @@
 #endif
 #endif
 
-#if ((defined EIGEN_VECTORIZE_AVX) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_MINGW) && (__GXX_ABI_VERSION < 1004)) || EIGEN_OS_QNX
+#if ((defined EIGEN_VECTORIZE_AVX) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_MINGW || EIGEN_COMP_LCC) && \
+     (__GXX_ABI_VERSION < 1004)) ||                                                                     \
+    EIGEN_OS_QNX
 // With GCC's default ABI version, a __m128 or __m256 are the same types and therefore we cannot
 // have overloads for both types without linking error.
 // One solution is to increase ABI version using -fabi-version=4 (or greater).
 // Otherwise, we workaround this inconvenience by wrapping 128bit types into the following helper
 // structure:
-typedef eigen_packet_wrapper<__m128>  Packet4f;
+typedef eigen_packet_wrapper<__m128> Packet4f;
 typedef eigen_packet_wrapper<__m128d> Packet2d;
 #else
-typedef __m128  Packet4f;
+typedef __m128 Packet4f;
 typedef __m128d Packet2d;
 #endif
 
 typedef eigen_packet_wrapper<__m128i, 0> Packet4i;
 typedef eigen_packet_wrapper<__m128i, 1> Packet16b;
+typedef eigen_packet_wrapper<__m128i, 4> Packet4ui;
 
-template<> struct is_arithmetic<__m128>  { enum { value = true }; };
-template<> struct is_arithmetic<__m128i> { enum { value = true }; };
-template<> struct is_arithmetic<__m128d> { enum { value = true }; };
-template<> struct is_arithmetic<Packet4i>  { enum { value = true }; };
-template<> struct is_arithmetic<Packet16b>  { enum { value = true }; };
+template <>
+struct is_arithmetic<__m128> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<__m128i> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<__m128d> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<Packet4i> {
+  enum { value = true };
+};
+// Note that `Packet4ui` uses the underlying type `__m128i`, which is
+// interpreted as a vector of _signed_ `int32`s, which breaks some arithmetic
+// operations used in `GenericPacketMath.h`.
+template <>
+struct is_arithmetic<Packet4ui> {
+  enum { value = false };
+};
+template <>
+struct is_arithmetic<Packet16b> {
+  enum { value = true };
+};
 
-template<int p, int q, int r, int s>
-struct shuffle_mask{
- enum { mask = (s)<<6|(r)<<4|(q)<<2|(p) };
+template <int p, int q, int r, int s>
+struct shuffle_mask {
+  enum { mask = (s) << 6 | (r) << 4 | (q) << 2 | (p) };
 };
 
 // TODO: change the implementation of all swizzle* ops from macro to template,
-#define vec4f_swizzle1(v,p,q,r,s) \
-  Packet4f(_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), (shuffle_mask<p,q,r,s>::mask))))
+#define vec4f_swizzle1(v, p, q, r, s) \
+  Packet4f(_mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(v), (shuffle_mask<p, q, r, s>::mask))))
 
-#define vec4i_swizzle1(v,p,q,r,s) \
-  Packet4i(_mm_shuffle_epi32( v, (shuffle_mask<p,q,r,s>::mask)))
+#define vec4i_swizzle1(v, p, q, r, s) Packet4i(_mm_shuffle_epi32(v, (shuffle_mask<p, q, r, s>::mask)))
 
-#define vec2d_swizzle1(v,p,q) \
-  Packet2d(_mm_castsi128_pd(_mm_shuffle_epi32( _mm_castpd_si128(v), (shuffle_mask<2*p,2*p+1,2*q,2*q+1>::mask))))
+#define vec4ui_swizzle1(v, p, q, r, s) Packet4ui(vec4i_swizzle1(v, p, q, r, s))
 
-#define vec4f_swizzle2(a,b,p,q,r,s) \
-  Packet4f(_mm_shuffle_ps( (a), (b), (shuffle_mask<p,q,r,s>::mask)))
+#define vec2d_swizzle1(v, p, q) \
+  Packet2d(_mm_castsi128_pd(    \
+      _mm_shuffle_epi32(_mm_castpd_si128(v), (shuffle_mask<2 * p, 2 * p + 1, 2 * q, 2 * q + 1>::mask))))
 
-#define vec4i_swizzle2(a,b,p,q,r,s) \
-  Packet4i(_mm_castps_si128( (_mm_shuffle_ps( _mm_castsi128_ps(a), _mm_castsi128_ps(b), (shuffle_mask<p,q,r,s>::mask)))))
+#define vec4f_swizzle2(a, b, p, q, r, s) Packet4f(_mm_shuffle_ps((a), (b), (shuffle_mask<p, q, r, s>::mask)))
 
-EIGEN_STRONG_INLINE Packet4f vec4f_movelh(const Packet4f& a, const Packet4f& b)
-{
-  return Packet4f(_mm_movelh_ps(a,b));
+#define vec4i_swizzle2(a, b, p, q, r, s) \
+  Packet4i(                              \
+      _mm_castps_si128((_mm_shuffle_ps(_mm_castsi128_ps(a), _mm_castsi128_ps(b), (shuffle_mask<p, q, r, s>::mask)))))
+
+#define vec4ui_swizzle2(a, b, p, q, r, s) Packet4i(vec4i_swizzle2(a, b, p, q, r, s))
+
+EIGEN_STRONG_INLINE Packet4f vec4f_movelh(const Packet4f& a, const Packet4f& b) {
+  return Packet4f(_mm_movelh_ps(a, b));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_movehl(const Packet4f& a, const Packet4f& b)
-{
-  return Packet4f(_mm_movehl_ps(a,b));
+EIGEN_STRONG_INLINE Packet4f vec4f_movehl(const Packet4f& a, const Packet4f& b) {
+  return Packet4f(_mm_movehl_ps(a, b));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_unpacklo(const Packet4f& a, const Packet4f& b)
-{
-  return Packet4f(_mm_unpacklo_ps(a,b));
+EIGEN_STRONG_INLINE Packet4f vec4f_unpacklo(const Packet4f& a, const Packet4f& b) {
+  return Packet4f(_mm_unpacklo_ps(a, b));
 }
-EIGEN_STRONG_INLINE Packet4f vec4f_unpackhi(const Packet4f& a, const Packet4f& b)
-{
-  return Packet4f(_mm_unpackhi_ps(a,b));
+EIGEN_STRONG_INLINE Packet4f vec4f_unpackhi(const Packet4f& a, const Packet4f& b) {
+  return Packet4f(_mm_unpackhi_ps(a, b));
 }
-#define vec4f_duplane(a,p) \
-  vec4f_swizzle2(a,a,p,p,p,p)
+#define vec4f_duplane(a, p) vec4f_swizzle2(a, a, p, p, p, p)
 
-#define vec2d_swizzle2(a,b,mask) \
-  Packet2d(_mm_shuffle_pd(a,b,mask))
+#define vec2d_swizzle2(a, b, mask) Packet2d(_mm_shuffle_pd(a, b, mask))
 
-EIGEN_STRONG_INLINE Packet2d vec2d_unpacklo(const Packet2d& a, const Packet2d& b)
-{
-  return Packet2d(_mm_unpacklo_pd(a,b));
+EIGEN_STRONG_INLINE Packet2d vec2d_unpacklo(const Packet2d& a, const Packet2d& b) {
+  return Packet2d(_mm_unpacklo_pd(a, b));
 }
-EIGEN_STRONG_INLINE Packet2d vec2d_unpackhi(const Packet2d& a, const Packet2d& b)
-{
-  return Packet2d(_mm_unpackhi_pd(a,b));
+EIGEN_STRONG_INLINE Packet2d vec2d_unpackhi(const Packet2d& a, const Packet2d& b) {
+  return Packet2d(_mm_unpackhi_pd(a, b));
 }
-#define vec2d_duplane(a,p) \
-  vec2d_swizzle2(a,a,(p<<1)|p)
+#define vec2d_duplane(a, p) vec2d_swizzle2(a, a, (p << 1) | p)
 
-#define _EIGEN_DECLARE_CONST_Packet4f(NAME,X) \
-  const Packet4f p4f_##NAME = pset1<Packet4f>(X)
+#define EIGEN_DECLARE_CONST_Packet4f(NAME, X) const Packet4f p4f_##NAME = pset1<Packet4f>(X)
 
-#define _EIGEN_DECLARE_CONST_Packet2d(NAME,X) \
-  const Packet2d p2d_##NAME = pset1<Packet2d>(X)
+#define EIGEN_DECLARE_CONST_Packet2d(NAME, X) const Packet2d p2d_##NAME = pset1<Packet2d>(X)
 
-#define _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(NAME,X) \
-  const Packet4f p4f_##NAME = pset1frombits<Packet4f>(X)
+#define EIGEN_DECLARE_CONST_Packet4f_FROM_INT(NAME, X) const Packet4f p4f_##NAME = pset1frombits<Packet4f>(X)
 
-#define _EIGEN_DECLARE_CONST_Packet4i(NAME,X) \
-  const Packet4i p4i_##NAME = pset1<Packet4i>(X)
+#define EIGEN_DECLARE_CONST_Packet4i(NAME, X) const Packet4i p4i_##NAME = pset1<Packet4i>(X)
 
+#define EIGEN_DECLARE_CONST_Packet4ui(NAME, X) const Packet4ui p4ui_##NAME = pset1<Packet4ui>(X)
 
 // Use the packet_traits defined in AVX/PacketMath.h instead if we're going
 // to leverage AVX instructions.
@@ -130,12 +151,16 @@
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket = 0,
 
-    HasCmp  = 1,
+    HasCmp = 1,
     HasDiv = 1,
+    HasReciprocal = EIGEN_FAST_MATH,
     HasSin = EIGEN_FAST_MATH,
     HasCos = EIGEN_FAST_MATH,
+    HasACos = 1,
+    HasASin = 1,
+    HasATan = 1,
+    HasATanh = 1,
     HasLog = 1,
     HasLog1p = 1,
     HasExpm1 = 1,
@@ -152,7 +177,8 @@
 #ifdef EIGEN_VECTORIZE_SSE4_1
     HasRound = 1,
 #endif
-    HasRint = 1
+    HasRint = 1,
+    HasSign = 0  // The manually vectorized version is slightly slower for SSE.
   };
 };
 template <>
@@ -162,15 +188,15 @@
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
-    size=2,
-    HasHalfPacket = 0,
+    size = 2,
 
-    HasCmp  = 1,
-    HasDiv  = 1,
-    HasLog  = 1,
-    HasExp  = 1,
+    HasCmp = 1,
+    HasDiv = 1,
+    HasLog = 1,
+    HasExp = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
+    HasATan = 1,
     HasBlend = 1,
     HasFloor = 1,
     HasCeil = 1,
@@ -180,96 +206,202 @@
     HasRint = 1
   };
 };
-#endif
-template<> struct packet_traits<int>    : default_packet_traits
-{
+template <>
+struct packet_traits<int> : default_packet_traits {
   typedef Packet4i type;
   typedef Packet4i half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
-    size=4,
+    HasCmp = 1,
+    HasDiv = 1,
+    size = 4,
 
     HasShift = 1,
     HasBlend = 1
   };
 };
+template <>
+struct packet_traits<uint32_t> : default_packet_traits {
+  typedef Packet4ui type;
+  typedef Packet4ui half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 4,
 
-template<> struct packet_traits<bool> : default_packet_traits
-{
+    HasDiv = 0,
+    HasNegate = 0,
+    HasSqrt = 0,
+    HasCmp = 1,
+    HasMin = 1,
+    HasMax = 1,
+    HasShift = 1,
+    HasBlend = 1
+  };
+};
+#endif
+template <>
+struct packet_traits<bool> : default_packet_traits {
   typedef Packet16b type;
   typedef Packet16b half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
-    HasHalfPacket = 0,
-    size=16,
+    size = 16,
 
-    HasAdd       = 1,
-    HasSub       = 1,
-    HasShift     = 0,
-    HasMul       = 1,
-    HasNegate    = 1,
-    HasAbs       = 0,
-    HasAbs2      = 0,
-    HasMin       = 0,
-    HasMax       = 0,
-    HasConj      = 0,
-    HasSqrt      = 1
+    HasAdd = 1,
+    HasSub = 1,
+    HasCmp = 1,  // note -- only pcmp_eq is defined
+    HasShift = 0,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 0,
+    HasAbs2 = 0,
+    HasMin = 0,
+    HasMax = 0,
+    HasConj = 0,
+    HasSqrt = 1,
+    HasSign = 0  // Don't try to vectorize psign<bool> = identity.
   };
 };
 
-template<> struct unpacket_traits<Packet4f> {
-  typedef float     type;
-  typedef Packet4f  half;
-  typedef Packet4i  integer_packet;
-  enum {size=4, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false};
+template <>
+struct unpacket_traits<Packet4f> {
+  typedef float type;
+  typedef Packet4f half;
+  typedef Packet4i integer_packet;
+  enum {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
 };
-template<> struct unpacket_traits<Packet2d> {
-  typedef double    type;
-  typedef Packet2d  half;
-  enum {size=2, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false};
+template <>
+struct unpacket_traits<Packet2d> {
+  typedef double type;
+  typedef Packet2d half;
+  enum {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
 };
-template<> struct unpacket_traits<Packet4i> {
-  typedef int       type;
-  typedef Packet4i  half;
-  enum {size=4, alignment=Aligned16, vectorizable=false, masked_load_available=false, masked_store_available=false};
+template <>
+struct unpacket_traits<Packet4i> {
+  typedef int type;
+  typedef Packet4i half;
+  enum {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
 };
-template<> struct unpacket_traits<Packet16b> {
-  typedef bool       type;
-  typedef Packet16b  half;
-  enum {size=16, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false};
+template <>
+struct unpacket_traits<Packet4ui> {
+  typedef uint32_t type;
+  typedef Packet4ui half;
+  enum {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template <>
+struct unpacket_traits<Packet16b> {
+  typedef bool type;
+  typedef Packet16b half;
+  enum {
+    size = 16,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
 };
 
 #ifndef EIGEN_VECTORIZE_AVX
-template<> struct scalar_div_cost<float,true> { enum { value = 7 }; };
-template<> struct scalar_div_cost<double,true> { enum { value = 8 }; };
+template <>
+struct scalar_div_cost<float, true> {
+  enum { value = 7 };
+};
+template <>
+struct scalar_div_cost<double, true> {
+  enum { value = 8 };
+};
 #endif
 
-#if EIGEN_COMP_MSVC==1500
-// Workaround MSVC 9 internal compiler error.
-// TODO: It has been detected with win64 builds (amd64), so let's check whether it also happens in 32bits+SSE mode
-// TODO: let's check whether there does not exist a better fix, like adding a pset0() function. (it crashed on pset1(0)).
-template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float&  from) { return _mm_set_ps(from,from,from,from); }
-template<> EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double& from) { return _mm_set_pd(from,from); }
-template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int&    from) { return _mm_set_epi32(from,from,from,from); }
-#else
-template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float&  from) { return _mm_set_ps1(from); }
-template<> EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double& from) { return _mm_set1_pd(from); }
-template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int&    from) { return _mm_set1_epi32(from); }
-#endif
-template<> EIGEN_STRONG_INLINE Packet16b pset1<Packet16b>(const bool&    from) { return _mm_set1_epi8(static_cast<char>(from)); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float& from) {
+  return _mm_set_ps1(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double& from) {
+  return _mm_set1_pd(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int& from) {
+  return _mm_set1_epi32(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pset1<Packet4ui>(const uint32_t& from) {
+  return _mm_set1_epi32(numext::bit_cast<int32_t>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b pset1<Packet16b>(const bool& from) {
+  return _mm_set1_epi8(static_cast<char>(from));
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pset1frombits<Packet4f>(unsigned int from) { return _mm_castsi128_ps(pset1<Packet4i>(from)); }
-template<> EIGEN_STRONG_INLINE Packet2d pset1frombits<Packet2d>(uint64_t from) { return _mm_castsi128_pd(_mm_set1_epi64x(from)); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pset1frombits<Packet4f>(unsigned int from) {
+  return _mm_castsi128_ps(pset1<Packet4i>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pset1frombits<Packet2d>(uint64_t from) {
+  return _mm_castsi128_pd(_mm_set1_epi64x(from));
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f peven_mask(const Packet4f& /*a*/) { return _mm_castsi128_ps(_mm_set_epi32(0, -1, 0, -1)); }
-template<> EIGEN_STRONG_INLINE Packet4i peven_mask(const Packet4i& /*a*/) { return _mm_set_epi32(0, -1, 0, -1); }
-template<> EIGEN_STRONG_INLINE Packet2d peven_mask(const Packet2d& /*a*/) { return _mm_castsi128_pd(_mm_set_epi32(0, 0, -1, -1)); }
+template <>
+EIGEN_STRONG_INLINE Packet4f peven_mask(const Packet4f& /*a*/) {
+  return _mm_castsi128_ps(_mm_set_epi32(0, -1, 0, -1));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i peven_mask(const Packet4i& /*a*/) {
+  return _mm_set_epi32(0, -1, 0, -1);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui peven_mask(const Packet4ui& /*a*/) {
+  return _mm_set_epi32(0, -1, 0, -1);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d peven_mask(const Packet2d& /*a*/) {
+  return _mm_castsi128_pd(_mm_set_epi32(0, 0, -1, -1));
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pzero(const Packet4f& /*a*/) { return _mm_setzero_ps(); }
-template<> EIGEN_STRONG_INLINE Packet2d pzero(const Packet2d& /*a*/) { return _mm_setzero_pd(); }
-template<> EIGEN_STRONG_INLINE Packet4i pzero(const Packet4i& /*a*/) { return _mm_setzero_si128(); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pzero(const Packet4f& /*a*/) {
+  return _mm_setzero_ps();
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pzero(const Packet2d& /*a*/) {
+  return _mm_setzero_pd();
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pzero(const Packet4i& /*a*/) {
+  return _mm_setzero_si128();
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pzero(const Packet4ui& /*a*/) {
+  return _mm_setzero_si128();
+}
 
 // GCC generates a shufps instruction for _mm_set1_ps/_mm_load1_ps instead of the more efficient pshufd instruction.
 // However, using inrinsics for pset1 makes gcc to generate crappy code in some cases (see bug 203)
@@ -277,268 +409,580 @@
 // Therefore, we introduced the pload1 functions to be used in product kernels for which bug 203 does not apply.
 // Also note that with AVX, we want it to generate a vbroadcastss.
 #if EIGEN_COMP_GNUC_STRICT && (!defined __AVX__)
-template<> EIGEN_STRONG_INLINE Packet4f pload1<Packet4f>(const float *from) {
-  return vec4f_swizzle1(_mm_load_ss(from),0,0,0,0);
+template <>
+EIGEN_STRONG_INLINE Packet4f pload1<Packet4f>(const float* from) {
+  return vec4f_swizzle1(_mm_load_ss(from), 0, 0, 0, 0);
 }
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4f plset<Packet4f>(const float& a) { return _mm_add_ps(pset1<Packet4f>(a), _mm_set_ps(3,2,1,0)); }
-template<> EIGEN_STRONG_INLINE Packet2d plset<Packet2d>(const double& a) { return _mm_add_pd(pset1<Packet2d>(a),_mm_set_pd(1,0)); }
-template<> EIGEN_STRONG_INLINE Packet4i plset<Packet4i>(const int& a) { return _mm_add_epi32(pset1<Packet4i>(a),_mm_set_epi32(3,2,1,0)); }
+template <>
+EIGEN_STRONG_INLINE Packet4f plset<Packet4f>(const float& a) {
+  return _mm_add_ps(pset1<Packet4f>(a), _mm_set_ps(3, 2, 1, 0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d plset<Packet2d>(const double& a) {
+  return _mm_add_pd(pset1<Packet2d>(a), _mm_set_pd(1, 0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i plset<Packet4i>(const int& a) {
+  return _mm_add_epi32(pset1<Packet4i>(a), _mm_set_epi32(3, 2, 1, 0));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui plset<Packet4ui>(const uint32_t& a) {
+  return _mm_add_epi32(pset1<Packet4ui>(a), _mm_set_epi32(3, 2, 1, 0));
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f padd<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_add_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d padd<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_add_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_add_epi32(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4f padd<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_add_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d padd<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_add_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return _mm_add_epi32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui padd<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return _mm_add_epi32(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet16b padd<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_or_si128(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet16b padd<Packet16b>(const Packet16b& a, const Packet16b& b) {
+  return _mm_or_si128(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_sub_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d psub<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_sub_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_sub_epi32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16b psub<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_xor_si128(a,b); }
+template <typename Packet>
+EIGEN_STRONG_INLINE Packet padds(const Packet& a, const Packet& b);
+template <>
+EIGEN_STRONG_INLINE Packet4f padds<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_add_ss(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d padds<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_add_sd(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b);
-template<> EIGEN_STRONG_INLINE Packet4f paddsub<Packet4f>(const Packet4f& a, const Packet4f& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_sub_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d psub<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_sub_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return _mm_sub_epi32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui psub<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return _mm_sub_epi32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b psub<Packet16b>(const Packet16b& a, const Packet16b& b) {
+  return _mm_xor_si128(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b);
+template <>
+EIGEN_STRONG_INLINE Packet4f paddsub<Packet4f>(const Packet4f& a, const Packet4f& b) {
 #ifdef EIGEN_VECTORIZE_SSE3
-  return _mm_addsub_ps(a,b);
+  return _mm_addsub_ps(a, b);
 #else
-  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000,0x0,0x80000000,0x0));
+  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000, 0x0, 0x80000000, 0x0));
   return padd(a, pxor(mask, b));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& , const Packet2d& );
-template<> EIGEN_STRONG_INLINE Packet2d paddsub<Packet2d>(const Packet2d& a, const Packet2d& b) 
-{
-#ifdef EIGEN_VECTORIZE_SSE3  
-  return _mm_addsub_pd(a,b); 
+template <>
+EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d&, const Packet2d&);
+template <>
+EIGEN_STRONG_INLINE Packet2d paddsub<Packet2d>(const Packet2d& a, const Packet2d& b) {
+#ifdef EIGEN_VECTORIZE_SSE3
+  return _mm_addsub_pd(a, b);
 #else
-  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0x0,0x80000000,0x0,0x0)); 
+  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0x0, 0x80000000, 0x0, 0x0));
   return padd(a, pxor(mask, b));
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a)
-{
-  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000,0x80000000,0x80000000,0x80000000));
-  return _mm_xor_ps(a,mask);
+template <>
+EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a) {
+  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000, 0x80000000, 0x80000000, 0x80000000));
+  return _mm_xor_ps(a, mask);
 }
-template<> EIGEN_STRONG_INLINE Packet2d pnegate(const Packet2d& a)
-{
-  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0x0,0x80000000,0x0,0x80000000));
-  return _mm_xor_pd(a,mask);
+template <>
+EIGEN_STRONG_INLINE Packet2d pnegate(const Packet2d& a) {
+  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0x0, 0x80000000, 0x0, 0x80000000));
+  return _mm_xor_pd(a, mask);
 }
-template<> EIGEN_STRONG_INLINE Packet4i pnegate(const Packet4i& a)
-{
-  return psub(Packet4i(_mm_setr_epi32(0,0,0,0)), a);
+template <>
+EIGEN_STRONG_INLINE Packet4i pnegate(const Packet4i& a) {
+  return psub(Packet4i(_mm_setr_epi32(0, 0, 0, 0)), a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet16b pnegate(const Packet16b& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet16b pnegate(const Packet16b& a) {
   return psub(pset1<Packet16b>(false), a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pconj(const Packet4f& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet2d pconj(const Packet2d& a) { return a; }
-template<> EIGEN_STRONG_INLINE Packet4i pconj(const Packet4i& a) { return a; }
+template <>
+EIGEN_STRONG_INLINE Packet4f pconj(const Packet4f& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pconj(const Packet2d& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pconj(const Packet4i& a) {
+  return a;
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pmul<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_mul_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pmul<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_mul_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pmul<Packet4i>(const Packet4i& a, const Packet4i& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f pmul<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_mul_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pmul<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_mul_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pmul<Packet4i>(const Packet4i& a, const Packet4i& b) {
 #ifdef EIGEN_VECTORIZE_SSE4_1
-  return _mm_mullo_epi32(a,b);
+  return _mm_mullo_epi32(a, b);
 #else
   // this version is slightly faster than 4 scalar products
   return vec4i_swizzle1(
-            vec4i_swizzle2(
-              _mm_mul_epu32(a,b),
-              _mm_mul_epu32(vec4i_swizzle1(a,1,0,3,2),
-                            vec4i_swizzle1(b,1,0,3,2)),
-              0,2,0,2),
-            0,2,1,3);
+      vec4i_swizzle2(_mm_mul_epu32(a, b), _mm_mul_epu32(vec4i_swizzle1(a, 1, 0, 3, 2), vec4i_swizzle1(b, 1, 0, 3, 2)),
+                     0, 2, 0, 2),
+      0, 2, 1, 3);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmul<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  return _mm_mullo_epi32(a, b);
+#else
+  // this version is slightly faster than 4 scalar products
+  return vec4ui_swizzle1(
+      vec4ui_swizzle2(_mm_mul_epu32(a, b),
+                      _mm_mul_epu32(vec4ui_swizzle1(a, 1, 0, 3, 2), vec4ui_swizzle1(b, 1, 0, 3, 2)), 0, 2, 0, 2),
+      0, 2, 1, 3);
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet16b pmul<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_and_si128(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet16b pmul<Packet16b>(const Packet16b& a, const Packet16b& b) {
+  return _mm_and_si128(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_div_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pdiv<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_div_pd(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_div_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pdiv<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_div_pd(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4i pdiv<Packet4i>(const Packet4i& a, const Packet4i& b) {
+#ifdef EIGEN_VECTORIZE_AVX
+  return _mm256_cvttpd_epi32(_mm256_div_pd(_mm256_cvtepi32_pd(a), _mm256_cvtepi32_pd(b)));
+#else
+  __m128i q_lo = _mm_cvttpd_epi32(_mm_div_pd(_mm_cvtepi32_pd(a), _mm_cvtepi32_pd(b)));
+  __m128i q_hi = _mm_cvttpd_epi32(
+      _mm_div_pd(_mm_cvtepi32_pd(vec4i_swizzle1(a, 2, 3, 0, 1)), _mm_cvtepi32_pd(vec4i_swizzle1(b, 2, 3, 0, 1))));
+  return vec4i_swizzle1(_mm_unpacklo_epi32(q_lo, q_hi), 0, 2, 1, 3);
+#endif
+}
 
 // for some weird raisons, it has to be overloaded for packet of integers
-template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) { return padd(pmul(a,b), c); }
+template <>
+EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) {
+  return padd(pmul(a, b), c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmadd(const Packet4ui& a, const Packet4ui& b, const Packet4ui& c) {
+  return padd(pmul(a, b), c);
+}
 #ifdef EIGEN_VECTORIZE_FMA
-template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) { return _mm_fmadd_ps(a,b,c); }
-template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) { return _mm_fmadd_pd(a,b,c); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
+  return _mm_fmadd_ps(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) {
+  return _mm_fmadd_pd(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pmsub(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
+  return _mm_fmsub_ps(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pmsub(const Packet2d& a, const Packet2d& b, const Packet2d& c) {
+  return _mm_fmsub_pd(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pnmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
+  return _mm_fnmadd_ps(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pnmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) {
+  return _mm_fnmadd_pd(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pnmsub(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
+  return _mm_fnmsub_ps(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pnmsub(const Packet2d& a, const Packet2d& b, const Packet2d& c) {
+  return _mm_fnmsub_pd(a, b, c);
+}
+
+template <typename Packet>
+EIGEN_STRONG_INLINE Packet pmadds(const Packet& a, const Packet& b, const Packet& c);
+template <>
+EIGEN_STRONG_INLINE Packet4f pmadds<Packet4f>(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
+  return _mm_fmadd_ss(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pmadds<Packet2d>(const Packet2d& a, const Packet2d& b, const Packet2d& c) {
+  return _mm_fmadd_sd(a, b, c);
+}
 #endif
 
 #ifdef EIGEN_VECTORIZE_SSE4_1
-template<> EIGEN_DEVICE_FUNC inline Packet4f pselect(const Packet4f& mask, const Packet4f& a, const Packet4f& b) {
-  return _mm_blendv_ps(b,a,mask);
+template <>
+EIGEN_DEVICE_FUNC inline Packet4f pselect(const Packet4f& mask, const Packet4f& a, const Packet4f& b) {
+  return _mm_blendv_ps(b, a, mask);
 }
 
-template<> EIGEN_DEVICE_FUNC inline Packet4i pselect(const Packet4i& mask, const Packet4i& a, const Packet4i& b) {
-  return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(b),_mm_castsi128_ps(a),_mm_castsi128_ps(mask)));
+template <>
+EIGEN_DEVICE_FUNC inline Packet4i pselect(const Packet4i& mask, const Packet4i& a, const Packet4i& b) {
+  return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(b), _mm_castsi128_ps(a), _mm_castsi128_ps(mask)));
 }
 
-template<> EIGEN_DEVICE_FUNC inline Packet2d pselect(const Packet2d& mask, const Packet2d& a, const Packet2d& b) {  return _mm_blendv_pd(b,a,mask); }
+template <>
+EIGEN_DEVICE_FUNC inline Packet4ui pselect(const Packet4ui& mask, const Packet4ui& a, const Packet4ui& b) {
+  return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(b), _mm_castsi128_ps(a), _mm_castsi128_ps(mask)));
+}
 
-template<> EIGEN_DEVICE_FUNC inline Packet16b pselect(const Packet16b& mask, const Packet16b& a, const Packet16b& b) {
-  return _mm_blendv_epi8(b,a,mask);
+template <>
+EIGEN_DEVICE_FUNC inline Packet2d pselect(const Packet2d& mask, const Packet2d& a, const Packet2d& b) {
+  return _mm_blendv_pd(b, a, mask);
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline Packet16b pselect(const Packet16b& mask, const Packet16b& a, const Packet16b& b) {
+  return _mm_blendv_epi8(b, a, mask);
 }
 #else
-template<> EIGEN_DEVICE_FUNC inline Packet16b pselect(const Packet16b& mask, const Packet16b& a, const Packet16b& b) {
+template <>
+EIGEN_DEVICE_FUNC inline Packet16b pselect(const Packet16b& mask, const Packet16b& a, const Packet16b& b) {
   Packet16b a_part = _mm_and_si128(mask, a);
   Packet16b b_part = _mm_andnot_si128(mask, b);
   return _mm_or_si128(a_part, b_part);
 }
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4i ptrue<Packet4i>(const Packet4i& a) { return _mm_cmpeq_epi32(a, a); }
-template<> EIGEN_STRONG_INLINE Packet16b ptrue<Packet16b>(const Packet16b& a) { return _mm_cmpeq_epi8(a, a); }
-template<> EIGEN_STRONG_INLINE Packet4f
-ptrue<Packet4f>(const Packet4f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4i ptrue<Packet4i>(const Packet4i& a) {
+  return _mm_cmpeq_epi32(a, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b ptrue<Packet16b>(const Packet16b& a) {
+  return _mm_cmpeq_epi8(a, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f ptrue<Packet4f>(const Packet4f& a) {
   Packet4i b = _mm_castps_si128(a);
   return _mm_castsi128_ps(_mm_cmpeq_epi32(b, b));
 }
-template<> EIGEN_STRONG_INLINE Packet2d
-ptrue<Packet2d>(const Packet2d& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2d ptrue<Packet2d>(const Packet2d& a) {
   Packet4i b = _mm_castpd_si128(a);
   return _mm_castsi128_pd(_mm_cmpeq_epi32(b, b));
 }
 
+template <>
+EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_and_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_and_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return _mm_and_si128(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pand<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return _mm_and_si128(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b pand<Packet16b>(const Packet16b& a, const Packet16b& b) {
+  return _mm_and_si128(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_and_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_and_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_and_si128(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16b pand<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_and_si128(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_or_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_or_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return _mm_or_si128(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui por<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return _mm_or_si128(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b por<Packet16b>(const Packet16b& a, const Packet16b& b) {
+  return _mm_or_si128(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_or_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_or_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_or_si128(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16b por<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_or_si128(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_xor_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_xor_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return _mm_xor_si128(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pxor<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return _mm_xor_si128(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b pxor<Packet16b>(const Packet16b& a, const Packet16b& b) {
+  return _mm_xor_si128(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_xor_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_xor_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_xor_si128(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16b pxor<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_xor_si128(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return _mm_andnot_ps(b, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return _mm_andnot_pd(b, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) {
+  return _mm_andnot_si128(b, a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pandnot<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+  return _mm_andnot_si128(b, a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_andnot_ps(b,a); }
-template<> EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_andnot_pd(b,a); }
-template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_andnot_si128(b,a); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_le(const Packet4f& a, const Packet4f& b) {
+  return _mm_cmple_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_lt(const Packet4f& a, const Packet4f& b) {
+  return _mm_cmplt_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_lt_or_nan(const Packet4f& a, const Packet4f& b) {
+  return _mm_cmpnge_ps(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pcmp_eq(const Packet4f& a, const Packet4f& b) {
+  return _mm_cmpeq_ps(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_le(const Packet4f& a, const Packet4f& b) { return _mm_cmple_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt(const Packet4f& a, const Packet4f& b) { return _mm_cmplt_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt_or_nan(const Packet4f& a, const Packet4f& b) { return _mm_cmpnge_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4f pcmp_eq(const Packet4f& a, const Packet4f& b) { return _mm_cmpeq_ps(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_le(const Packet2d& a, const Packet2d& b) {
+  return _mm_cmple_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_lt(const Packet2d& a, const Packet2d& b) {
+  return _mm_cmplt_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_lt_or_nan(const Packet2d& a, const Packet2d& b) {
+  return _mm_cmpnge_pd(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pcmp_eq(const Packet2d& a, const Packet2d& b) {
+  return _mm_cmpeq_pd(a, b);
+}
 
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_le(const Packet2d& a, const Packet2d& b) { return _mm_cmple_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt(const Packet2d& a, const Packet2d& b) { return _mm_cmplt_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt_or_nan(const Packet2d& a, const Packet2d& b) { return _mm_cmpnge_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pcmp_eq(const Packet2d& a, const Packet2d& b) { return _mm_cmpeq_pd(a,b); }
+template <>
+EIGEN_STRONG_INLINE Packet4i pcmp_lt(const Packet4i& a, const Packet4i& b) {
+  return _mm_cmplt_epi32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcmp_eq(const Packet4i& a, const Packet4i& b) {
+  return _mm_cmpeq_epi32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcmp_eq(const Packet4ui& a, const Packet4ui& b) {
+  return _mm_cmpeq_epi32(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b pcmp_eq(const Packet16b& a, const Packet16b& b) {
+  return _mm_cmpeq_epi8(a, b);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pcmp_le(const Packet4i& a, const Packet4i& b) {
+  return por(pcmp_lt(a, b), pcmp_eq(a, b));
+}
 
-template<> EIGEN_STRONG_INLINE Packet4i pcmp_lt(const Packet4i& a, const Packet4i& b) { return _mm_cmplt_epi32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pcmp_eq(const Packet4i& a, const Packet4i& b) { return _mm_cmpeq_epi32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet16b pcmp_eq(const Packet16b& a, const Packet16b& b) { return _mm_cmpeq_epi8(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pcmp_le(const Packet4i& a, const Packet4i& b) { return por(pcmp_lt(a,b), pcmp_eq(a,b)); }
-
-template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
-  // There appears to be a bug in GCC, by which the optimizer may
-  // flip the argument order in calls to _mm_min_ps, so we have to
-  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
-  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
-  #ifdef EIGEN_VECTORIZE_AVX
+template <>
+EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
+// There appears to be a bug in GCC, by which the optimizer may
+// flip the argument order in calls to _mm_min_ps, so we have to
+// resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+// see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+#ifdef EIGEN_VECTORIZE_AVX
   Packet4f res;
-  asm("vminps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
-  #else
+  asm("vminps %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
+#else
   Packet4f res = b;
-  asm("minps %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
-  #endif
+  asm("minps %[a], %[res]" : [res] "+x"(res) : [a] "x"(a));
+#endif
   return res;
 #else
   // Arguments are reversed to match NaN propagation behavior of std::min.
   return _mm_min_ps(b, a);
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
-  // There appears to be a bug in GCC, by which the optimizer may
-  // flip the argument order in calls to _mm_min_pd, so we have to
-  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
-  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
-  #ifdef EIGEN_VECTORIZE_AVX
+template <>
+EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
+// There appears to be a bug in GCC, by which the optimizer may
+// flip the argument order in calls to _mm_min_pd, so we have to
+// resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+// see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+#ifdef EIGEN_VECTORIZE_AVX
   Packet2d res;
-  asm("vminpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
-  #else
+  asm("vminpd %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
+#else
   Packet2d res = b;
-  asm("minpd %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
-  #endif
+  asm("minpd %[a], %[res]" : [res] "+x"(res) : [a] "x"(a));
+#endif
   return res;
 #else
   // Arguments are reversed to match NaN propagation behavior of std::min.
   return _mm_min_pd(b, a);
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b) {
 #ifdef EIGEN_VECTORIZE_SSE4_1
-  return _mm_min_epi32(a,b);
+  return _mm_min_epi32(a, b);
 #else
   // after some bench, this version *is* faster than a scalar implementation
-  Packet4i mask = _mm_cmplt_epi32(a,b);
-  return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
+  Packet4i mask = _mm_cmplt_epi32(a, b);
+  return _mm_or_si128(_mm_and_si128(mask, a), _mm_andnot_si128(mask, b));
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmin<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  return _mm_min_epu32(a, b);
+#else
+  return padd((Packet4ui)pmin((Packet4i)psub(a, pset1<Packet4ui>(0x80000000UL)),
+                              (Packet4i)psub(b, pset1<Packet4ui>(0x80000000UL))),
+              pset1<Packet4ui>(0x80000000UL));
 #endif
 }
 
-
-template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
-  // There appears to be a bug in GCC, by which the optimizer may
-  // flip the argument order in calls to _mm_max_ps, so we have to
-  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
-  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
-  #ifdef EIGEN_VECTORIZE_AVX
+template <>
+EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
+// There appears to be a bug in GCC, by which the optimizer may
+// flip the argument order in calls to _mm_max_ps, so we have to
+// resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+// see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+#ifdef EIGEN_VECTORIZE_AVX
   Packet4f res;
-  asm("vmaxps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
-  #else
+  asm("vmaxps %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
+#else
   Packet4f res = b;
-  asm("maxps %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
-  #endif
+  asm("maxps %[a], %[res]" : [res] "+x"(res) : [a] "x"(a));
+#endif
   return res;
 #else
   // Arguments are reversed to match NaN propagation behavior of std::max.
   return _mm_max_ps(b, a);
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) {
-#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
-  // There appears to be a bug in GCC, by which the optimizer may
-  // flip the argument order in calls to _mm_max_pd, so we have to
-  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
-  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
-  #ifdef EIGEN_VECTORIZE_AVX
+template <>
+EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) {
+#if EIGEN_GNUC_STRICT_LESS_THAN(6, 3, 0)
+// There appears to be a bug in GCC, by which the optimizer may
+// flip the argument order in calls to _mm_max_pd, so we have to
+// resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+// see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+#ifdef EIGEN_VECTORIZE_AVX
   Packet2d res;
-  asm("vmaxpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
-  #else
+  asm("vmaxpd %[a], %[b], %[res]" : [res] "=x"(res) : [a] "x"(a), [b] "x"(b));
+#else
   Packet2d res = b;
-  asm("maxpd %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
-  #endif
+  asm("maxpd %[a], %[res]" : [res] "+x"(res) : [a] "x"(a));
+#endif
   return res;
 #else
   // Arguments are reversed to match NaN propagation behavior of std::max.
   return _mm_max_pd(b, a);
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b) {
 #ifdef EIGEN_VECTORIZE_SSE4_1
-  return _mm_max_epi32(a,b);
+  return _mm_max_epi32(a, b);
 #else
   // after some bench, this version *is* faster than a scalar implementation
-  Packet4i mask = _mm_cmpgt_epi32(a,b);
-  return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
+  Packet4i mask = _mm_cmpgt_epi32(a, b);
+  return _mm_or_si128(_mm_and_si128(mask, a), _mm_andnot_si128(mask, b));
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pmax<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  return _mm_max_epu32(a, b);
+#else
+  return padd((Packet4ui)pmax((Packet4i)psub(a, pset1<Packet4ui>(0x80000000UL)),
+                              (Packet4i)psub(b, pset1<Packet4ui>(0x80000000UL))),
+              pset1<Packet4ui>(0x80000000UL));
+#endif
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcmp_lt(const Packet4ui& a, const Packet4ui& b) {
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  return pxor(pcmp_eq(a, pmax(a, b)), ptrue(a));
+#else
+  return (Packet4ui)pcmp_lt((Packet4i)psub(a, pset1<Packet4ui>(0x80000000UL)),
+                            (Packet4i)psub(b, pset1<Packet4ui>(0x80000000UL)));
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcmp_le(const Packet4ui& a, const Packet4ui& b) {
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  return pcmp_eq(a, pmin(a, b));
+#else
+  return (Packet4ui)pcmp_le((Packet4i)psub(a, pset1<Packet4ui>(0x80000000UL)),
+                            (Packet4i)psub(b, pset1<Packet4ui>(0x80000000UL)));
 #endif
 }
 
@@ -561,150 +1005,212 @@
 }
 
 // Add specializations for min/max with prescribed NaN progation.
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4f pmin<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) {
   return pminmax_propagate_numbers(a, b, pmin<Packet4f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet2d pmin<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) {
   return pminmax_propagate_numbers(a, b, pmin<Packet2d>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4f pmax<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) {
   return pminmax_propagate_numbers(a, b, pmax<Packet4f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet2d pmax<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) {
   return pminmax_propagate_numbers(a, b, pmax<Packet2d>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4f pmin<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) {
   return pminmax_propagate_nan(a, b, pmin<Packet4f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet2d pmin<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) {
   return pminmax_propagate_nan(a, b, pmin<Packet2d>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet4f pmax<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) {
   return pminmax_propagate_nan(a, b, pmax<Packet4f>);
 }
-template<>
+template <>
 EIGEN_STRONG_INLINE Packet2d pmax<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) {
   return pminmax_propagate_nan(a, b, pmax<Packet2d>);
 }
 
-template<int N> EIGEN_STRONG_INLINE Packet4i parithmetic_shift_right(const Packet4i& a) { return _mm_srai_epi32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_right   (const Packet4i& a) { return _mm_srli_epi32(a,N); }
-template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_left    (const Packet4i& a) { return _mm_slli_epi32(a,N); }
+template <int N>
+EIGEN_STRONG_INLINE Packet4i parithmetic_shift_right(const Packet4i& a) {
+  return _mm_srai_epi32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4i plogical_shift_right(const Packet4i& a) {
+  return _mm_srli_epi32(a, N);
+}
+template <int N>
+EIGEN_STRONG_INLINE Packet4i plogical_shift_left(const Packet4i& a) {
+  return _mm_slli_epi32(a, N);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a)
-{
-  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF));
-  return _mm_and_ps(a,mask);
+template <int N>
+EIGEN_STRONG_INLINE Packet4ui parithmetic_shift_right(const Packet4ui& a) {
+  return _mm_srli_epi32(a, N);
 }
-template<> EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a)
-{
-  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0xFFFFFFFF,0x7FFFFFFF,0xFFFFFFFF,0x7FFFFFFF));
-  return _mm_and_pd(a,mask);
+template <int N>
+EIGEN_STRONG_INLINE Packet4ui plogical_shift_right(const Packet4ui& a) {
+  return _mm_srli_epi32(a, N);
 }
-template<> EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a)
-{
-  #ifdef EIGEN_VECTORIZE_SSSE3
+template <int N>
+EIGEN_STRONG_INLINE Packet4ui plogical_shift_left(const Packet4ui& a) {
+  return _mm_slli_epi32(a, N);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a) {
+  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF));
+  return _mm_and_ps(a, mask);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a) {
+  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0xFFFFFFFF, 0x7FFFFFFF, 0xFFFFFFFF, 0x7FFFFFFF));
+  return _mm_and_pd(a, mask);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a) {
+#ifdef EIGEN_VECTORIZE_SSSE3
   return _mm_abs_epi32(a);
-  #else
-  Packet4i aux = _mm_srai_epi32(a,31);
-  return _mm_sub_epi32(_mm_xor_si128(a,aux),aux);
-  #endif
+#else
+  Packet4i aux = _mm_srai_epi32(a, 31);
+  return _mm_sub_epi32(_mm_xor_si128(a, aux), aux);
+#endif
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pabs(const Packet4ui& a) {
+  return a;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f psignbit(const Packet4f& a) {
+  return _mm_castsi128_ps(_mm_srai_epi32(_mm_castps_si128(a), 31));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d psignbit(const Packet2d& a) {
+  Packet4f tmp = psignbit<Packet4f>(_mm_castpd_ps(a));
+#ifdef EIGEN_VECTORIZE_AVX
+  return _mm_castps_pd(_mm_permute_ps(tmp, (shuffle_mask<1, 1, 3, 3>::mask)));
+#else
+  return _mm_castps_pd(_mm_shuffle_ps(tmp, tmp, (shuffle_mask<1, 1, 3, 3>::mask)));
+#endif  // EIGEN_VECTORIZE_AVX
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui psignbit(const Packet4ui& a) {
+  return pzero(a);
 }
 
 #ifdef EIGEN_VECTORIZE_SSE4_1
-template<> EIGEN_STRONG_INLINE Packet4f pround<Packet4f>(const Packet4f& a)
-{
-  // Unfortunatly _mm_round_ps doesn't have a rounding mode to implement numext::round.
+template <>
+EIGEN_STRONG_INLINE Packet4f pround<Packet4f>(const Packet4f& a) {
+  // Unfortunately _mm_round_ps doesn't have a rounding mode to implement numext::round.
   const Packet4f mask = pset1frombits<Packet4f>(0x80000000u);
   const Packet4f prev0dot5 = pset1frombits<Packet4f>(0x3EFFFFFFu);
   return _mm_round_ps(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d pround<Packet2d>(const Packet2d& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2d pround<Packet2d>(const Packet2d& a) {
   const Packet2d mask = _mm_castsi128_pd(_mm_set_epi64x(0x8000000000000000ull, 0x8000000000000000ull));
   const Packet2d prev0dot5 = _mm_castsi128_pd(_mm_set_epi64x(0x3FDFFFFFFFFFFFFFull, 0x3FDFFFFFFFFFFFFFull));
   return _mm_round_pd(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f print<Packet4f>(const Packet4f& a) { return _mm_round_ps(a, _MM_FROUND_CUR_DIRECTION); }
-template<> EIGEN_STRONG_INLINE Packet2d print<Packet2d>(const Packet2d& a) { return _mm_round_pd(a, _MM_FROUND_CUR_DIRECTION); }
+template <>
+EIGEN_STRONG_INLINE Packet4f print<Packet4f>(const Packet4f& a) {
+  return _mm_round_ps(a, _MM_FROUND_CUR_DIRECTION);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d print<Packet2d>(const Packet2d& a) {
+  return _mm_round_pd(a, _MM_FROUND_CUR_DIRECTION);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a) { return _mm_ceil_ps(a); }
-template<> EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a) { return _mm_ceil_pd(a); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a) {
+  return _mm_ceil_ps(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a) {
+  return _mm_ceil_pd(a);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a) { return _mm_floor_ps(a); }
-template<> EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a) { return _mm_floor_pd(a); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a) {
+  return _mm_floor_ps(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a) {
+  return _mm_floor_pd(a);
+}
 #else
-template<> EIGEN_STRONG_INLINE Packet4f print(const Packet4f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4f print(const Packet4f& a) {
   // Adds and subtracts signum(a) * 2^23 to force rounding.
-  const Packet4f limit = pset1<Packet4f>(static_cast<float>(1<<23));
+  const Packet4f limit = pset1<Packet4f>(static_cast<float>(1 << 23));
   const Packet4f abs_a = pabs(a);
   Packet4f r = padd(abs_a, limit);
   // Don't compile-away addition and subtraction.
   EIGEN_OPTIMIZATION_BARRIER(r);
   r = psub(r, limit);
   // If greater than limit, simply return a.  Otherwise, account for sign.
-  r = pselect(pcmp_lt(abs_a, limit),
-              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  r = pselect(pcmp_lt(abs_a, limit), pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
   return r;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d print(const Packet2d& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2d print(const Packet2d& a) {
   // Adds and subtracts signum(a) * 2^52 to force rounding.
-  const Packet2d limit = pset1<Packet2d>(static_cast<double>(1ull<<52));
+  const Packet2d limit = pset1<Packet2d>(static_cast<double>(1ull << 52));
   const Packet2d abs_a = pabs(a);
   Packet2d r = padd(abs_a, limit);
   // Don't compile-away addition and subtraction.
   EIGEN_OPTIMIZATION_BARRIER(r);
   r = psub(r, limit);
   // If greater than limit, simply return a.  Otherwise, account for sign.
-  r = pselect(pcmp_lt(abs_a, limit),
-              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  r = pselect(pcmp_lt(abs_a, limit), pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
   return r;
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a) {
   const Packet4f cst_1 = pset1<Packet4f>(1.0f);
-  Packet4f tmp  = print<Packet4f>(a);
+  Packet4f tmp = print<Packet4f>(a);
   // If greater, subtract one.
   Packet4f mask = _mm_cmpgt_ps(tmp, a);
   mask = pand(mask, cst_1);
   return psub(tmp, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a) {
   const Packet2d cst_1 = pset1<Packet2d>(1.0);
-  Packet2d tmp  = print<Packet2d>(a);
+  Packet2d tmp = print<Packet2d>(a);
   // If greater, subtract one.
   Packet2d mask = _mm_cmpgt_pd(tmp, a);
   mask = pand(mask, cst_1);
   return psub(tmp, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a) {
   const Packet4f cst_1 = pset1<Packet4f>(1.0f);
-  Packet4f tmp  = print<Packet4f>(a);
+  Packet4f tmp = print<Packet4f>(a);
   // If smaller, add one.
   Packet4f mask = _mm_cmplt_ps(tmp, a);
   mask = pand(mask, cst_1);
   return padd(tmp, mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a) {
   const Packet2d cst_1 = pset1<Packet2d>(1.0);
-  Packet2d tmp  = print<Packet2d>(a);
+  Packet2d tmp = print<Packet2d>(a);
   // If smaller, add one.
   Packet2d mask = _mm_cmplt_pd(tmp, a);
   mask = pand(mask, cst_1);
@@ -712,186 +1218,373 @@
 }
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float*   from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_ps(from); }
-template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double*  from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from); }
-template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int*     from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_si128(reinterpret_cast<const __m128i*>(from)); }
-template<> EIGEN_STRONG_INLINE Packet16b pload<Packet16b>(const bool*     from) { EIGEN_DEBUG_ALIGNED_LOAD return  _mm_load_si128(reinterpret_cast<const __m128i*>(from)); }
+template <>
+EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_ps(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_si128(reinterpret_cast<const __m128i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui pload<Packet4ui>(const uint32_t* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_si128(reinterpret_cast<const __m128i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b pload<Packet16b>(const bool* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_si128(reinterpret_cast<const __m128i*>(from));
+}
 
 #if EIGEN_COMP_MSVC
-  template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float*  from) {
-    EIGEN_DEBUG_UNALIGNED_LOAD
-    #if (EIGEN_COMP_MSVC==1600)
-    // NOTE Some version of MSVC10 generates bad code when using _mm_loadu_ps
-    // (i.e., it does not generate an unaligned load!!
-    __m128 res = _mm_loadl_pi(_mm_set1_ps(0.0f), (const __m64*)(from));
-    res = _mm_loadh_pi(res, (const __m64*)(from+2));
-    return res;
-    #else
-    return _mm_loadu_ps(from);
-    #endif
-  }
+template <>
+EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD
+  return _mm_loadu_ps(from);
+}
 #else
 // NOTE: with the code below, MSVC's compiler crashes!
 
-template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from) {
   EIGEN_DEBUG_UNALIGNED_LOAD
   return _mm_loadu_ps(from);
 }
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from) {
   EIGEN_DEBUG_UNALIGNED_LOAD
   return _mm_loadu_pd(from);
 }
-template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from) {
   EIGEN_DEBUG_UNALIGNED_LOAD
   return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
 }
-template<> EIGEN_STRONG_INLINE Packet16b ploadu<Packet16b>(const bool*     from) {
+template <>
+EIGEN_STRONG_INLINE Packet4ui ploadu<Packet4ui>(const uint32_t* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD
+  return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b ploadu<Packet16b>(const bool* from) {
   EIGEN_DEBUG_UNALIGNED_LOAD
   return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
 }
 
+// Load lower part of packet zero extending.
+template <typename Packet>
+EIGEN_STRONG_INLINE Packet ploadl(const typename unpacket_traits<Packet>::type* from);
+template <>
+EIGEN_STRONG_INLINE Packet4f ploadl<Packet4f>(const float* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm_castpd_ps(_mm_load_sd(reinterpret_cast<const double*>(from)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d ploadl<Packet2d>(const double* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm_load_sd(from);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float*   from)
-{
+// Load scalar
+template <typename Packet>
+EIGEN_STRONG_INLINE Packet ploads(const typename unpacket_traits<Packet>::type* from);
+template <>
+EIGEN_STRONG_INLINE Packet4f ploads<Packet4f>(const float* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm_load_ss(from);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d ploads<Packet2d>(const double* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm_load_sd(from);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float* from) {
   return vec4f_swizzle1(_mm_castpd_ps(_mm_load_sd(reinterpret_cast<const double*>(from))), 0, 0, 1, 1);
 }
-template<> EIGEN_STRONG_INLINE Packet2d ploaddup<Packet2d>(const double*  from)
-{ return pset1<Packet2d>(from[0]); }
-template<> EIGEN_STRONG_INLINE Packet4i ploaddup<Packet4i>(const int*     from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet2d ploaddup<Packet2d>(const double* from) {
+  return pset1<Packet2d>(from[0]);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i ploaddup<Packet4i>(const int* from) {
   Packet4i tmp;
   tmp = _mm_loadl_epi64(reinterpret_cast<const __m128i*>(from));
   return vec4i_swizzle1(tmp, 0, 0, 1, 1);
 }
+template <>
+EIGEN_STRONG_INLINE Packet4ui ploaddup<Packet4ui>(const uint32_t* from) {
+  Packet4ui tmp;
+  tmp = _mm_loadl_epi64(reinterpret_cast<const __m128i*>(from));
+  return vec4ui_swizzle1(tmp, 0, 0, 1, 1);
+}
 
 // Loads 8 bools from memory and returns the packet
 // {b0, b0, b1, b1, b2, b2, b3, b3, b4, b4, b5, b5, b6, b6, b7, b7}
-template<> EIGEN_STRONG_INLINE Packet16b ploaddup<Packet16b>(const bool*     from)
-{
+template <>
+EIGEN_STRONG_INLINE Packet16b ploaddup<Packet16b>(const bool* from) {
   __m128i tmp = _mm_castpd_si128(pload1<Packet2d>(reinterpret_cast<const double*>(from)));
-  return  _mm_unpacklo_epi8(tmp, tmp);
+  return _mm_unpacklo_epi8(tmp, tmp);
 }
 
 // Loads 4 bools from memory and returns the packet
 // {b0, b0  b0, b0, b1, b1, b1, b1, b2, b2, b2, b2, b3, b3, b3, b3}
-template<> EIGEN_STRONG_INLINE Packet16b
-ploadquad<Packet16b>(const bool* from) {
+template <>
+EIGEN_STRONG_INLINE Packet16b ploadquad<Packet16b>(const bool* from) {
   __m128i tmp = _mm_castps_si128(pload1<Packet4f>(reinterpret_cast<const float*>(from)));
   tmp = _mm_unpacklo_epi8(tmp, tmp);
-  return  _mm_unpacklo_epi16(tmp, tmp);
+  return _mm_unpacklo_epi16(tmp, tmp);
 }
 
-template<> EIGEN_STRONG_INLINE void pstore<float>(float*   to, const Packet4f& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_ps(to, from); }
-template<> EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet2d& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_pd(to, from); }
-template<> EIGEN_STRONG_INLINE void pstore<int>(int*       to, const Packet4i& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<__m128i*>(to), from); }
-template<> EIGEN_STRONG_INLINE void pstore<bool>(bool*     to, const Packet16b& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<__m128i*>(to), from); }
-
-template<> EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet2d& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_pd(to, from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*   to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_ps(to, from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int>(int*       to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<bool>(bool*     to, const Packet16b& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from); }
-
-template<> EIGEN_DEVICE_FUNC inline Packet4f pgather<float, Packet4f>(const float* from, Index stride)
-{
- return _mm_set_ps(from[3*stride], from[2*stride], from[1*stride], from[0*stride]);
+template <>
+EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet4f& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm_store_ps(to, from);
 }
-template<> EIGEN_DEVICE_FUNC inline Packet2d pgather<double, Packet2d>(const double* from, Index stride)
-{
- return _mm_set_pd(from[1*stride], from[0*stride]);
+template <>
+EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet2d& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm_store_pd(to, from);
 }
-template<> EIGEN_DEVICE_FUNC inline Packet4i pgather<int, Packet4i>(const int* from, Index stride)
-{
- return _mm_set_epi32(from[3*stride], from[2*stride], from[1*stride], from[0*stride]);
+template <>
+EIGEN_STRONG_INLINE void pstore<int>(int* to, const Packet4i& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<__m128i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet4ui& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<__m128i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstore<bool>(bool* to, const Packet16b& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<__m128i*>(to), from);
 }
 
-template<> EIGEN_DEVICE_FUNC inline Packet16b pgather<bool, Packet16b>(const bool* from, Index stride)
-{
-  return _mm_set_epi8(from[15*stride], from[14*stride], from[13*stride], from[12*stride],
-                      from[11*stride], from[10*stride], from[9*stride], from[8*stride],
-                      from[7*stride], from[6*stride], from[5*stride], from[4*stride],
-                      from[3*stride], from[2*stride], from[1*stride], from[0*stride]);
+template <>
+EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet2d& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_pd(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet4f& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_ps(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<int>(int* to, const Packet4i& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet4ui& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstoreu<bool>(bool* to, const Packet16b& from) {
+  EIGEN_DEBUG_ALIGNED_STORE _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<float, Packet4f>(float* to, const Packet4f& from, Index stride)
-{
-  to[stride*0] = _mm_cvtss_f32(from);
-  to[stride*1] = _mm_cvtss_f32(_mm_shuffle_ps(from, from, 1));
-  to[stride*2] = _mm_cvtss_f32(_mm_shuffle_ps(from, from, 2));
-  to[stride*3] = _mm_cvtss_f32(_mm_shuffle_ps(from, from, 3));
+template <typename Scalar, typename Packet>
+EIGEN_STRONG_INLINE void pstorel(Scalar* to, const Packet& from);
+template <>
+EIGEN_STRONG_INLINE void pstorel(float* to, const Packet4f& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_storel_pi(reinterpret_cast<__m64*>(to), from);
 }
-template<> EIGEN_DEVICE_FUNC inline void pscatter<double, Packet2d>(double* to, const Packet2d& from, Index stride)
-{
-  to[stride*0] = _mm_cvtsd_f64(from);
-  to[stride*1] = _mm_cvtsd_f64(_mm_shuffle_pd(from, from, 1));
-}
-template<> EIGEN_DEVICE_FUNC inline void pscatter<int, Packet4i>(int* to, const Packet4i& from, Index stride)
-{
-  to[stride*0] = _mm_cvtsi128_si32(from);
-  to[stride*1] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 1));
-  to[stride*2] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2));
-  to[stride*3] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3));
-}
-template<> EIGEN_DEVICE_FUNC inline void pscatter<bool, Packet16b>(bool* to, const Packet16b& from, Index stride)
-{
-  to[4*stride*0] = _mm_cvtsi128_si32(from);
-  to[4*stride*1] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 1));
-  to[4*stride*2] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2));
-  to[4*stride*3] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3));
+template <>
+EIGEN_STRONG_INLINE void pstorel(double* to, const Packet2d& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_storel_pd(to, from);
 }
 
+template <typename Scalar, typename Packet>
+EIGEN_STRONG_INLINE void pstores(Scalar* to, const Packet& from);
+template <>
+EIGEN_STRONG_INLINE void pstores(float* to, const Packet4f& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_store_ss(to, from);
+}
+template <>
+EIGEN_STRONG_INLINE void pstores(double* to, const Packet2d& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE _mm_store_sd(to, from);
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline Packet4f pgather<float, Packet4f>(const float* from, Index stride) {
+  return _mm_set_ps(from[3 * stride], from[2 * stride], from[1 * stride], from[0 * stride]);
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet2d pgather<double, Packet2d>(const double* from, Index stride) {
+  return _mm_set_pd(from[1 * stride], from[0 * stride]);
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet4i pgather<int, Packet4i>(const int* from, Index stride) {
+  return _mm_set_epi32(from[3 * stride], from[2 * stride], from[1 * stride], from[0 * stride]);
+}
+template <>
+EIGEN_DEVICE_FUNC inline Packet4ui pgather<uint32_t, Packet4ui>(const uint32_t* from, Index stride) {
+  return _mm_set_epi32(numext::bit_cast<int32_t>(from[3 * stride]), numext::bit_cast<int32_t>(from[2 * stride]),
+                       numext::bit_cast<int32_t>(from[1 * stride]), numext::bit_cast<int32_t>(from[0 * stride]));
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline Packet16b pgather<bool, Packet16b>(const bool* from, Index stride) {
+  return _mm_set_epi8(from[15 * stride], from[14 * stride], from[13 * stride], from[12 * stride], from[11 * stride],
+                      from[10 * stride], from[9 * stride], from[8 * stride], from[7 * stride], from[6 * stride],
+                      from[5 * stride], from[4 * stride], from[3 * stride], from[2 * stride], from[1 * stride],
+                      from[0 * stride]);
+}
+
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<float, Packet4f>(float* to, const Packet4f& from, Index stride) {
+  to[stride * 0] = _mm_cvtss_f32(from);
+  to[stride * 1] = _mm_cvtss_f32(_mm_shuffle_ps(from, from, 1));
+  to[stride * 2] = _mm_cvtss_f32(_mm_shuffle_ps(from, from, 2));
+  to[stride * 3] = _mm_cvtss_f32(_mm_shuffle_ps(from, from, 3));
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<double, Packet2d>(double* to, const Packet2d& from, Index stride) {
+  to[stride * 0] = _mm_cvtsd_f64(from);
+  to[stride * 1] = _mm_cvtsd_f64(_mm_shuffle_pd(from, from, 1));
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<int, Packet4i>(int* to, const Packet4i& from, Index stride) {
+  to[stride * 0] = _mm_cvtsi128_si32(from);
+  to[stride * 1] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 1));
+  to[stride * 2] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2));
+  to[stride * 3] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3));
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<uint32_t, Packet4ui>(uint32_t* to, const Packet4ui& from, Index stride) {
+  to[stride * 0] = numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(from));
+  to[stride * 1] = numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(_mm_shuffle_epi32(from, 1)));
+  to[stride * 2] = numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2)));
+  to[stride * 3] = numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3)));
+}
+template <>
+EIGEN_DEVICE_FUNC inline void pscatter<bool, Packet16b>(bool* to, const Packet16b& from, Index stride) {
+  to[4 * stride * 0] = _mm_cvtsi128_si32(from);
+  to[4 * stride * 1] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 1));
+  to[4 * stride * 2] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2));
+  to[4 * stride * 3] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3));
+}
 
 // some compilers might be tempted to perform multiple moves instead of using a vector path.
-template<> EIGEN_STRONG_INLINE void pstore1<Packet4f>(float* to, const float& a)
-{
+template <>
+EIGEN_STRONG_INLINE void pstore1<Packet4f>(float* to, const float& a) {
   Packet4f pa = _mm_set_ss(a);
-  pstore(to, Packet4f(vec4f_swizzle1(pa,0,0,0,0)));
+  pstore(to, Packet4f(vec4f_swizzle1(pa, 0, 0, 0, 0)));
 }
 // some compilers might be tempted to perform multiple moves instead of using a vector path.
-template<> EIGEN_STRONG_INLINE void pstore1<Packet2d>(double* to, const double& a)
-{
+template <>
+EIGEN_STRONG_INLINE void pstore1<Packet2d>(double* to, const double& a) {
   Packet2d pa = _mm_set_sd(a);
-  pstore(to, Packet2d(vec2d_swizzle1(pa,0,0)));
+  pstore(to, Packet2d(vec2d_swizzle1(pa, 0, 0)));
 }
 
 #if EIGEN_COMP_PGI && EIGEN_COMP_PGI < 1900
-typedef const void * SsePrefetchPtrType;
+typedef const void* SsePrefetchPtrType;
 #else
-typedef const char * SsePrefetchPtrType;
+typedef const char* SsePrefetchPtrType;
 #endif
 
 #ifndef EIGEN_VECTORIZE_AVX
-template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template <>
+EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<int>(const int* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
+template <>
+EIGEN_STRONG_INLINE void prefetch<uint32_t>(const uint32_t* addr) {
+  _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0);
+}
 #endif
 
 #if EIGEN_COMP_MSVC_STRICT && EIGEN_OS_WIN64
 // The temporary variable fixes an internal compilation error in vs <= 2008 and a wrong-result bug in vs 2010
 // Direct of the struct members fixed bug #62.
-template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { return a.m128_f32[0]; }
-template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return a.m128d_f64[0]; }
-template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { int x = _mm_cvtsi128_si32(a); return x; }
+template <>
+EIGEN_STRONG_INLINE float pfirst<Packet4f>(const Packet4f& a) {
+  return a.m128_f32[0];
+}
+template <>
+EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) {
+  return a.m128d_f64[0];
+}
+template <>
+EIGEN_STRONG_INLINE int pfirst<Packet4i>(const Packet4i& a) {
+  int x = _mm_cvtsi128_si32(a);
+  return x;
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t pfirst<Packet4ui>(const Packet4ui& a) {
+  uint32_t x = numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(a));
+  return x;
+}
 #elif EIGEN_COMP_MSVC_STRICT
 // The temporary variable fixes an internal compilation error in vs <= 2008 and a wrong-result bug in vs 2010
-template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { float x = _mm_cvtss_f32(a); return x; }
-template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { double x = _mm_cvtsd_f64(a); return x; }
-template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { int x = _mm_cvtsi128_si32(a); return x; }
+template <>
+EIGEN_STRONG_INLINE float pfirst<Packet4f>(const Packet4f& a) {
+  float x = _mm_cvtss_f32(a);
+  return x;
+}
+template <>
+EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) {
+  double x = _mm_cvtsd_f64(a);
+  return x;
+}
+template <>
+EIGEN_STRONG_INLINE int pfirst<Packet4i>(const Packet4i& a) {
+  int x = _mm_cvtsi128_si32(a);
+  return x;
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t pfirst<Packet4ui>(const Packet4ui& a) {
+  uint32_t x = numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(a));
+  return x;
+}
 #else
-template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { return _mm_cvtss_f32(a); }
-template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return _mm_cvtsd_f64(a); }
-template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { return _mm_cvtsi128_si32(a); }
+template <>
+EIGEN_STRONG_INLINE float pfirst<Packet4f>(const Packet4f& a) {
+  return _mm_cvtss_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) {
+  return _mm_cvtsd_f64(a);
+}
+template <>
+EIGEN_STRONG_INLINE int pfirst<Packet4i>(const Packet4i& a) {
+  return _mm_cvtsi128_si32(a);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t pfirst<Packet4ui>(const Packet4ui& a) {
+  return numext::bit_cast<uint32_t>(_mm_cvtsi128_si32(a));
+}
 #endif
-template<> EIGEN_STRONG_INLINE bool   pfirst<Packet16b>(const Packet16b& a) { int x = _mm_cvtsi128_si32(a); return static_cast<bool>(x & 1); }
+template <>
+EIGEN_STRONG_INLINE bool pfirst<Packet16b>(const Packet16b& a) {
+  int x = _mm_cvtsi128_si32(a);
+  return static_cast<bool>(x & 1);
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a) { return _mm_shuffle_ps(a,a,0x1B); }
-template<> EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a) { return _mm_shuffle_pd(a,a,0x1); }
-template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a) { return _mm_shuffle_epi32(a,0x1B); }
-template<> EIGEN_STRONG_INLINE Packet16b preverse(const Packet16b& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a) {
+  return _mm_shuffle_ps(a, a, 0x1B);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a) {
+  return _mm_shuffle_pd(a, a, 0x1);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a) {
+  return _mm_shuffle_epi32(a, 0x1B);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui preverse(const Packet4ui& a) {
+  return _mm_shuffle_epi32(a, 0x1B);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16b preverse(const Packet16b& a) {
 #ifdef EIGEN_VECTORIZE_SSSE3
   __m128i mask = _mm_set_epi8(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15);
   return _mm_shuffle_epi8(a, mask);
@@ -902,211 +1595,259 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pfrexp<Packet4f>(const Packet4f& a, Packet4f& exponent) {
-  return pfrexp_generic(a,exponent);
+template <>
+EIGEN_STRONG_INLINE Packet4f pfrexp<Packet4f>(const Packet4f& a, Packet4f& exponent) {
+  return pfrexp_generic(a, exponent);
 }
 
 // Extract exponent without existence of Packet2l.
-template<>
-EIGEN_STRONG_INLINE  
-Packet2d pfrexp_generic_get_biased_exponent(const Packet2d& a) {
-  const Packet2d cst_exp_mask  = pset1frombits<Packet2d>(static_cast<uint64_t>(0x7ff0000000000000ull));
+template <>
+EIGEN_STRONG_INLINE Packet2d pfrexp_generic_get_biased_exponent(const Packet2d& a) {
+  const Packet2d cst_exp_mask = pset1frombits<Packet2d>(static_cast<uint64_t>(0x7ff0000000000000ull));
   __m128i a_expo = _mm_srli_epi64(_mm_castpd_si128(pand(a, cst_exp_mask)), 52);
   return _mm_cvtepi32_pd(vec4i_swizzle1(a_expo, 0, 2, 1, 3));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d pfrexp<Packet2d>(const Packet2d& a, Packet2d& exponent) {
+template <>
+EIGEN_STRONG_INLINE Packet2d pfrexp<Packet2d>(const Packet2d& a, Packet2d& exponent) {
   return pfrexp_generic(a, exponent);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pldexp<Packet4f>(const Packet4f& a, const Packet4f& exponent) {
-  return pldexp_generic(a,exponent);
+template <>
+EIGEN_STRONG_INLINE Packet4f pldexp<Packet4f>(const Packet4f& a, const Packet4f& exponent) {
+  return pldexp_generic(a, exponent);
 }
 
 // We specialize pldexp here, since the generic implementation uses Packet2l, which is not well
 // supported by SSE, and has more range than is needed for exponents.
-template<> EIGEN_STRONG_INLINE Packet2d pldexp<Packet2d>(const Packet2d& a, const Packet2d& exponent) {
+template <>
+EIGEN_STRONG_INLINE Packet2d pldexp<Packet2d>(const Packet2d& a, const Packet2d& exponent) {
   // Clamp exponent to [-2099, 2099]
   const Packet2d max_exponent = pset1<Packet2d>(2099.0);
   const Packet2d e = pmin(pmax(exponent, pnegate(max_exponent)), max_exponent);
-  
+
   // Convert e to integer and swizzle to low-order bits.
   const Packet4i ei = vec4i_swizzle1(_mm_cvtpd_epi32(e), 0, 3, 1, 3);
-  
+
   // Split 2^e into four factors and multiply:
   const Packet4i bias = _mm_set_epi32(0, 1023, 0, 1023);
-  Packet4i b = parithmetic_shift_right<2>(ei);  // floor(e/4)
+  Packet4i b = parithmetic_shift_right<2>(ei);                       // floor(e/4)
   Packet2d c = _mm_castsi128_pd(_mm_slli_epi64(padd(b, bias), 52));  // 2^b
-  Packet2d out = pmul(pmul(pmul(a, c), c), c); // a * 2^(3b)
-  b = psub(psub(psub(ei, b), b), b);  // e - 3b
-  c = _mm_castsi128_pd(_mm_slli_epi64(padd(b, bias), 52));  // 2^(e - 3b)
-  out = pmul(out, c);  // a * 2^e
+  Packet2d out = pmul(pmul(pmul(a, c), c), c);                       // a * 2^(3b)
+  b = psub(psub(psub(ei, b), b), b);                                 // e - 3b
+  c = _mm_castsi128_pd(_mm_slli_epi64(padd(b, bias), 52));           // 2^(e - 3b)
+  out = pmul(out, c);                                                // a * 2^e
   return out;
 }
 
 // with AVX, the default implementations based on pload1 are faster
 #ifndef __AVX__
-template<> EIGEN_STRONG_INLINE void
-pbroadcast4<Packet4f>(const float *a,
-                      Packet4f& a0, Packet4f& a1, Packet4f& a2, Packet4f& a3)
-{
+template <>
+EIGEN_STRONG_INLINE void pbroadcast4<Packet4f>(const float* a, Packet4f& a0, Packet4f& a1, Packet4f& a2, Packet4f& a3) {
   a3 = pload<Packet4f>(a);
-  a0 = vec4f_swizzle1(a3, 0,0,0,0);
-  a1 = vec4f_swizzle1(a3, 1,1,1,1);
-  a2 = vec4f_swizzle1(a3, 2,2,2,2);
-  a3 = vec4f_swizzle1(a3, 3,3,3,3);
+  a0 = vec4f_swizzle1(a3, 0, 0, 0, 0);
+  a1 = vec4f_swizzle1(a3, 1, 1, 1, 1);
+  a2 = vec4f_swizzle1(a3, 2, 2, 2, 2);
+  a3 = vec4f_swizzle1(a3, 3, 3, 3, 3);
 }
-template<> EIGEN_STRONG_INLINE void
-pbroadcast4<Packet2d>(const double *a,
-                      Packet2d& a0, Packet2d& a1, Packet2d& a2, Packet2d& a3)
-{
+template <>
+EIGEN_STRONG_INLINE void pbroadcast4<Packet2d>(const double* a, Packet2d& a0, Packet2d& a1, Packet2d& a2,
+                                               Packet2d& a3) {
 #ifdef EIGEN_VECTORIZE_SSE3
-  a0 = _mm_loaddup_pd(a+0);
-  a1 = _mm_loaddup_pd(a+1);
-  a2 = _mm_loaddup_pd(a+2);
-  a3 = _mm_loaddup_pd(a+3);
+  a0 = _mm_loaddup_pd(a + 0);
+  a1 = _mm_loaddup_pd(a + 1);
+  a2 = _mm_loaddup_pd(a + 2);
+  a3 = _mm_loaddup_pd(a + 3);
 #else
   a1 = pload<Packet2d>(a);
-  a0 = vec2d_swizzle1(a1, 0,0);
-  a1 = vec2d_swizzle1(a1, 1,1);
-  a3 = pload<Packet2d>(a+2);
-  a2 = vec2d_swizzle1(a3, 0,0);
-  a3 = vec2d_swizzle1(a3, 1,1);
+  a0 = vec2d_swizzle1(a1, 0, 0);
+  a1 = vec2d_swizzle1(a1, 1, 1);
+  a3 = pload<Packet2d>(a + 2);
+  a2 = vec2d_swizzle1(a3, 0, 0);
+  a3 = vec2d_swizzle1(a3, 1, 1);
 #endif
 }
 #endif
 
-EIGEN_STRONG_INLINE void punpackp(Packet4f* vecs)
-{
+EIGEN_STRONG_INLINE void punpackp(Packet4f* vecs) {
   vecs[1] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0x55));
   vecs[2] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0xAA));
   vecs[3] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0xFF));
   vecs[0] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0x00));
 }
 
-template<> EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a)
-{
+template <>
+EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a) {
   // Disable SSE3 _mm_hadd_pd that is extremely slow on all existing Intel's architectures
   // (from Nehalem to Haswell)
-// #ifdef EIGEN_VECTORIZE_SSE3
-//   Packet4f tmp = _mm_add_ps(a, vec4f_swizzle1(a,2,3,2,3));
-//   return pfirst<Packet4f>(_mm_hadd_ps(tmp, tmp));
-// #else
-  Packet4f tmp = _mm_add_ps(a, _mm_movehl_ps(a,a));
-  return pfirst<Packet4f>(_mm_add_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
-// #endif
+  // #ifdef EIGEN_VECTORIZE_SSE3
+  //   Packet4f tmp = _mm_add_ps(a, vec4f_swizzle1(a,2,3,2,3));
+  //   return pfirst<Packet4f>(_mm_hadd_ps(tmp, tmp));
+  // #else
+  Packet4f tmp = _mm_add_ps(a, _mm_movehl_ps(a, a));
+  return pfirst<Packet4f>(_mm_add_ss(tmp, _mm_shuffle_ps(tmp, tmp, 1)));
+  // #endif
 }
 
-template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a)
-{
+template <>
+EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a) {
   // Disable SSE3 _mm_hadd_pd that is extremely slow on all existing Intel's architectures
   // (from Nehalem to Haswell)
-// #ifdef EIGEN_VECTORIZE_SSE3
-//   return pfirst<Packet2d>(_mm_hadd_pd(a, a));
-// #else
-  return pfirst<Packet2d>(_mm_add_sd(a, _mm_unpackhi_pd(a,a)));
-// #endif
+  // #ifdef EIGEN_VECTORIZE_SSE3
+  //   return pfirst<Packet2d>(_mm_hadd_pd(a, a));
+  // #else
+  return pfirst<Packet2d>(_mm_add_sd(a, _mm_unpackhi_pd(a, a)));
+  // #endif
 }
 
 #ifdef EIGEN_VECTORIZE_SSSE3
-template<> EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a)
-{
-  Packet4i tmp0 = _mm_hadd_epi32(a,a);
-  return pfirst<Packet4i>(_mm_hadd_epi32(tmp0,tmp0));
+template <>
+EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a) {
+  Packet4i tmp0 = _mm_hadd_epi32(a, a);
+  return pfirst<Packet4i>(_mm_hadd_epi32(tmp0, tmp0));
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux<Packet4ui>(const Packet4ui& a) {
+  Packet4ui tmp0 = _mm_hadd_epi32(a, a);
+  return pfirst<Packet4ui>(_mm_hadd_epi32(tmp0, tmp0));
 }
 
 #else
-template<> EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a)
-{
-  Packet4i tmp = _mm_add_epi32(a, _mm_unpackhi_epi64(a,a));
+template <>
+EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a) {
+  Packet4i tmp = _mm_add_epi32(a, _mm_unpackhi_epi64(a, a));
   return pfirst(tmp) + pfirst<Packet4i>(_mm_shuffle_epi32(tmp, 1));
 }
+template <>
+EIGEN_STRONG_INLINE uint32_t predux<Packet4ui>(const Packet4ui& a) {
+  Packet4ui tmp = _mm_add_epi32(a, _mm_unpackhi_epi64(a, a));
+  return pfirst(tmp) + pfirst<Packet4ui>(_mm_shuffle_epi32(tmp, 1));
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE bool predux<Packet16b>(const Packet16b& a) {
-  Packet4i tmp = _mm_or_si128(a, _mm_unpackhi_epi64(a,a));
+template <>
+EIGEN_STRONG_INLINE bool predux<Packet16b>(const Packet16b& a) {
+  Packet4i tmp = _mm_or_si128(a, _mm_unpackhi_epi64(a, a));
   return (pfirst(tmp) != 0) || (pfirst<Packet4i>(_mm_shuffle_epi32(tmp, 1)) != 0);
 }
 
 // Other reduction functions:
 
-
 // mul
-template<> EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a)
-{
-  Packet4f tmp = _mm_mul_ps(a, _mm_movehl_ps(a,a));
-  return pfirst<Packet4f>(_mm_mul_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+template <>
+EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a) {
+  Packet4f tmp = _mm_mul_ps(a, _mm_movehl_ps(a, a));
+  return pfirst<Packet4f>(_mm_mul_ss(tmp, _mm_shuffle_ps(tmp, tmp, 1)));
 }
-template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a)
-{
-  return pfirst<Packet2d>(_mm_mul_sd(a, _mm_unpackhi_pd(a,a)));
+template <>
+EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a) {
+  return pfirst<Packet2d>(_mm_mul_sd(a, _mm_unpackhi_pd(a, a)));
 }
-template<> EIGEN_STRONG_INLINE int predux_mul<Packet4i>(const Packet4i& a)
-{
+template <>
+EIGEN_STRONG_INLINE int predux_mul<Packet4i>(const Packet4i& a) {
   // after some experiments, it is seems this is the fastest way to implement it
   // for GCC (eg., reusing pmul is very slow !)
   // TODO try to call _mm_mul_epu32 directly
   EIGEN_ALIGN16 int aux[4];
   pstore(aux, a);
-  return  (aux[0] * aux[1]) * (aux[2] * aux[3]);
+  return (aux[0] * aux[1]) * (aux[2] * aux[3]);
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_mul<Packet4ui>(const Packet4ui& a) {
+  // after some experiments, it is seems this is the fastest way to implement it
+  // for GCC (eg., reusing pmul is very slow !)
+  // TODO try to call _mm_mul_epu32 directly
+  EIGEN_ALIGN16 uint32_t aux[4];
+  pstore(aux, a);
+  return (aux[0] * aux[1]) * (aux[2] * aux[3]);
 }
 
-template<> EIGEN_STRONG_INLINE bool predux_mul<Packet16b>(const Packet16b& a) {
-  Packet4i tmp = _mm_and_si128(a, _mm_unpackhi_epi64(a,a));
-  return ((pfirst<Packet4i>(tmp) == 0x01010101) &&
-          (pfirst<Packet4i>(_mm_shuffle_epi32(tmp, 1)) == 0x01010101));
+template <>
+EIGEN_STRONG_INLINE bool predux_mul<Packet16b>(const Packet16b& a) {
+  Packet4i tmp = _mm_and_si128(a, _mm_unpackhi_epi64(a, a));
+  return ((pfirst<Packet4i>(tmp) == 0x01010101) && (pfirst<Packet4i>(_mm_shuffle_epi32(tmp, 1)) == 0x01010101));
 }
 
 // min
-template<> EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a)
-{
-  Packet4f tmp = _mm_min_ps(a, _mm_movehl_ps(a,a));
-  return pfirst<Packet4f>(_mm_min_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+template <>
+EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a) {
+  Packet4f tmp = _mm_min_ps(a, _mm_movehl_ps(a, a));
+  return pfirst<Packet4f>(_mm_min_ss(tmp, _mm_shuffle_ps(tmp, tmp, 1)));
 }
-template<> EIGEN_STRONG_INLINE double predux_min<Packet2d>(const Packet2d& a)
-{
-  return pfirst<Packet2d>(_mm_min_sd(a, _mm_unpackhi_pd(a,a)));
+template <>
+EIGEN_STRONG_INLINE double predux_min<Packet2d>(const Packet2d& a) {
+  return pfirst<Packet2d>(_mm_min_sd(a, _mm_unpackhi_pd(a, a)));
 }
-template<> EIGEN_STRONG_INLINE int predux_min<Packet4i>(const Packet4i& a)
-{
+template <>
+EIGEN_STRONG_INLINE int predux_min<Packet4i>(const Packet4i& a) {
 #ifdef EIGEN_VECTORIZE_SSE4_1
-  Packet4i tmp = _mm_min_epi32(a, _mm_shuffle_epi32(a, _MM_SHUFFLE(0,0,3,2)));
-  return pfirst<Packet4i>(_mm_min_epi32(tmp,_mm_shuffle_epi32(tmp, 1)));
+  Packet4i tmp = _mm_min_epi32(a, _mm_shuffle_epi32(a, _MM_SHUFFLE(0, 0, 3, 2)));
+  return pfirst<Packet4i>(_mm_min_epi32(tmp, _mm_shuffle_epi32(tmp, 1)));
 #else
   // after some experiments, it is seems this is the fastest way to implement it
   // for GCC (eg., it does not like using std::min after the pstore !!)
   EIGEN_ALIGN16 int aux[4];
   pstore(aux, a);
-  int aux0 = aux[0]<aux[1] ? aux[0] : aux[1];
-  int aux2 = aux[2]<aux[3] ? aux[2] : aux[3];
-  return aux0<aux2 ? aux0 : aux2;
-#endif // EIGEN_VECTORIZE_SSE4_1
+  int aux0 = aux[0] < aux[1] ? aux[0] : aux[1];
+  int aux2 = aux[2] < aux[3] ? aux[2] : aux[3];
+  return aux0 < aux2 ? aux0 : aux2;
+#endif  // EIGEN_VECTORIZE_SSE4_1
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_min<Packet4ui>(const Packet4ui& a) {
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  Packet4ui tmp = _mm_min_epu32(a, _mm_shuffle_epi32(a, _MM_SHUFFLE(0, 0, 3, 2)));
+  return pfirst<Packet4ui>(_mm_min_epu32(tmp, _mm_shuffle_epi32(tmp, 1)));
+#else
+  // after some experiments, it is seems this is the fastest way to implement it
+  // for GCC (eg., it does not like using std::min after the pstore !!)
+  EIGEN_ALIGN16 uint32_t aux[4];
+  pstore(aux, a);
+  uint32_t aux0 = aux[0] < aux[1] ? aux[0] : aux[1];
+  uint32_t aux2 = aux[2] < aux[3] ? aux[2] : aux[3];
+  return aux0 < aux2 ? aux0 : aux2;
+#endif  // EIGEN_VECTORIZE_SSE4_1
 }
 
 // max
-template<> EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a)
-{
-  Packet4f tmp = _mm_max_ps(a, _mm_movehl_ps(a,a));
-  return pfirst<Packet4f>(_mm_max_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+template <>
+EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a) {
+  Packet4f tmp = _mm_max_ps(a, _mm_movehl_ps(a, a));
+  return pfirst<Packet4f>(_mm_max_ss(tmp, _mm_shuffle_ps(tmp, tmp, 1)));
 }
-template<> EIGEN_STRONG_INLINE double predux_max<Packet2d>(const Packet2d& a)
-{
-  return pfirst<Packet2d>(_mm_max_sd(a, _mm_unpackhi_pd(a,a)));
+template <>
+EIGEN_STRONG_INLINE double predux_max<Packet2d>(const Packet2d& a) {
+  return pfirst<Packet2d>(_mm_max_sd(a, _mm_unpackhi_pd(a, a)));
 }
-template<> EIGEN_STRONG_INLINE int predux_max<Packet4i>(const Packet4i& a)
-{
+template <>
+EIGEN_STRONG_INLINE int predux_max<Packet4i>(const Packet4i& a) {
 #ifdef EIGEN_VECTORIZE_SSE4_1
-  Packet4i tmp = _mm_max_epi32(a, _mm_shuffle_epi32(a, _MM_SHUFFLE(0,0,3,2)));
-  return pfirst<Packet4i>(_mm_max_epi32(tmp,_mm_shuffle_epi32(tmp, 1)));
+  Packet4i tmp = _mm_max_epi32(a, _mm_shuffle_epi32(a, _MM_SHUFFLE(0, 0, 3, 2)));
+  return pfirst<Packet4i>(_mm_max_epi32(tmp, _mm_shuffle_epi32(tmp, 1)));
 #else
   // after some experiments, it is seems this is the fastest way to implement it
   // for GCC (eg., it does not like using std::min after the pstore !!)
   EIGEN_ALIGN16 int aux[4];
   pstore(aux, a);
-  int aux0 = aux[0]>aux[1] ? aux[0] : aux[1];
-  int aux2 = aux[2]>aux[3] ? aux[2] : aux[3];
-  return aux0>aux2 ? aux0 : aux2;
-#endif // EIGEN_VECTORIZE_SSE4_1
+  int aux0 = aux[0] > aux[1] ? aux[0] : aux[1];
+  int aux2 = aux[2] > aux[3] ? aux[2] : aux[3];
+  return aux0 > aux2 ? aux0 : aux2;
+#endif  // EIGEN_VECTORIZE_SSE4_1
+}
+template <>
+EIGEN_STRONG_INLINE uint32_t predux_max<Packet4ui>(const Packet4ui& a) {
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  Packet4ui tmp = _mm_max_epu32(a, _mm_shuffle_epi32(a, _MM_SHUFFLE(0, 0, 3, 2)));
+  return pfirst<Packet4ui>(_mm_max_epu32(tmp, _mm_shuffle_epi32(tmp, 1)));
+#else
+  // after some experiments, it is seems this is the fastest way to implement it
+  // for GCC (eg., it does not like using std::min after the pstore !!)
+  EIGEN_ALIGN16 uint32_t aux[4];
+  pstore(aux, a);
+  uint32_t aux0 = aux[0] > aux[1] ? aux[0] : aux[1];
+  uint32_t aux2 = aux[2] > aux[3] ? aux[2] : aux[3];
+  return aux0 > aux2 ? aux0 : aux2;
+#endif  // EIGEN_VECTORIZE_SSE4_1
 }
 
 // not needed yet
@@ -1115,25 +1856,31 @@
 //   return _mm_movemask_ps(x) == 0xF;
 // }
 
-template<> EIGEN_STRONG_INLINE bool predux_any(const Packet4f& x)
-{
+template <>
+EIGEN_STRONG_INLINE bool predux_any(const Packet4f& x) {
   return _mm_movemask_ps(x) != 0x0;
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet4f,4>& kernel) {
+template <>
+EIGEN_STRONG_INLINE bool predux_any(const Packet4i& x) {
+  return _mm_movemask_ps(_mm_castsi128_ps(x)) != 0x0;
+}
+template <>
+EIGEN_STRONG_INLINE bool predux_any(const Packet4ui& x) {
+  return _mm_movemask_ps(_mm_castsi128_ps(x)) != 0x0;
+}
+
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet4f, 4>& kernel) {
   _MM_TRANSPOSE4_PS(kernel.packet[0], kernel.packet[1], kernel.packet[2], kernel.packet[3]);
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet2d,2>& kernel) {
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet2d, 2>& kernel) {
   __m128d tmp = _mm_unpackhi_pd(kernel.packet[0], kernel.packet[1]);
   kernel.packet[0] = _mm_unpacklo_pd(kernel.packet[0], kernel.packet[1]);
   kernel.packet[1] = tmp;
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet4i,4>& kernel) {
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet4i, 4>& kernel) {
   __m128i T0 = _mm_unpacklo_epi32(kernel.packet[0], kernel.packet[1]);
   __m128i T1 = _mm_unpacklo_epi32(kernel.packet[2], kernel.packet[3]);
   __m128i T2 = _mm_unpackhi_epi32(kernel.packet[0], kernel.packet[1]);
@@ -1144,21 +1891,22 @@
   kernel.packet[2] = _mm_unpacklo_epi64(T2, T3);
   kernel.packet[3] = _mm_unpackhi_epi64(T2, T3);
 }
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet4ui, 4>& kernel) {
+  ptranspose((PacketBlock<Packet4i, 4>&)kernel);
+}
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet16b,4>& kernel) {
-  __m128i T0 =  _mm_unpacklo_epi8(kernel.packet[0], kernel.packet[1]);
-  __m128i T1 =  _mm_unpackhi_epi8(kernel.packet[0], kernel.packet[1]);
-  __m128i T2 =  _mm_unpacklo_epi8(kernel.packet[2], kernel.packet[3]);
-  __m128i T3 =  _mm_unpackhi_epi8(kernel.packet[2], kernel.packet[3]);
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet16b, 4>& kernel) {
+  __m128i T0 = _mm_unpacklo_epi8(kernel.packet[0], kernel.packet[1]);
+  __m128i T1 = _mm_unpackhi_epi8(kernel.packet[0], kernel.packet[1]);
+  __m128i T2 = _mm_unpacklo_epi8(kernel.packet[2], kernel.packet[3]);
+  __m128i T3 = _mm_unpackhi_epi8(kernel.packet[2], kernel.packet[3]);
   kernel.packet[0] = _mm_unpacklo_epi16(T0, T2);
   kernel.packet[1] = _mm_unpackhi_epi16(T0, T2);
   kernel.packet[2] = _mm_unpacklo_epi16(T1, T3);
   kernel.packet[3] = _mm_unpackhi_epi16(T1, T3);
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet16b,16>& kernel) {
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet16b, 16>& kernel) {
   // If we number the elements in the input thus:
   // kernel.packet[ 0] = {00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 0a, 0b, 0c, 0d, 0e, 0f}
   // kernel.packet[ 1] = {10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 1a, 1b, 1c, 1d, 1e, 1f}
@@ -1170,67 +1918,72 @@
   // kernel.packet[ 1] = {01, 11, 21, 31, 41, 51, 61, 71, 81, 91, a1, b1, c1, d1, e1, f1}
   // ...
   // kernel.packet[15] = {0f, 1f, 2f, 3f, 4f, 5f, 6f, 7f, 8f, 9f, af, bf, cf, df, ef, ff},
-  __m128i t0 =  _mm_unpacklo_epi8(kernel.packet[0], kernel.packet[1]); // 00 10 01 11 02 12 03 13 04 14 05 15 06 16 07 17
-  __m128i t1 =  _mm_unpackhi_epi8(kernel.packet[0], kernel.packet[1]); // 08 18 09 19 0a 1a 0b 1b 0c 1c 0d 1d 0e 1e 0f 1f
-  __m128i t2 =  _mm_unpacklo_epi8(kernel.packet[2], kernel.packet[3]); // 20 30 21 31 22 32 ...                     27 37
-  __m128i t3 =  _mm_unpackhi_epi8(kernel.packet[2], kernel.packet[3]); // 28 38 29 39 2a 3a ...                     2f 3f
-  __m128i t4 =  _mm_unpacklo_epi8(kernel.packet[4], kernel.packet[5]); // 40 50 41 51 42 52                         47 57
-  __m128i t5 =  _mm_unpackhi_epi8(kernel.packet[4], kernel.packet[5]); // 48 58 49 59 4a 5a
-  __m128i t6 =  _mm_unpacklo_epi8(kernel.packet[6], kernel.packet[7]);
-  __m128i t7 =  _mm_unpackhi_epi8(kernel.packet[6], kernel.packet[7]);
-  __m128i t8 =  _mm_unpacklo_epi8(kernel.packet[8], kernel.packet[9]);
-  __m128i t9 =  _mm_unpackhi_epi8(kernel.packet[8], kernel.packet[9]);
-  __m128i ta =  _mm_unpacklo_epi8(kernel.packet[10], kernel.packet[11]);
-  __m128i tb =  _mm_unpackhi_epi8(kernel.packet[10], kernel.packet[11]);
-  __m128i tc =  _mm_unpacklo_epi8(kernel.packet[12], kernel.packet[13]);
-  __m128i td =  _mm_unpackhi_epi8(kernel.packet[12], kernel.packet[13]);
-  __m128i te =  _mm_unpacklo_epi8(kernel.packet[14], kernel.packet[15]);
-  __m128i tf =  _mm_unpackhi_epi8(kernel.packet[14], kernel.packet[15]);
+  __m128i t0 =
+      _mm_unpacklo_epi8(kernel.packet[0], kernel.packet[1]);  // 00 10 01 11 02 12 03 13 04 14 05 15 06 16 07 17
+  __m128i t1 =
+      _mm_unpackhi_epi8(kernel.packet[0], kernel.packet[1]);  // 08 18 09 19 0a 1a 0b 1b 0c 1c 0d 1d 0e 1e 0f 1f
+  __m128i t2 =
+      _mm_unpacklo_epi8(kernel.packet[2], kernel.packet[3]);  // 20 30 21 31 22 32 ...                     27 37
+  __m128i t3 =
+      _mm_unpackhi_epi8(kernel.packet[2], kernel.packet[3]);  // 28 38 29 39 2a 3a ...                     2f 3f
+  __m128i t4 =
+      _mm_unpacklo_epi8(kernel.packet[4], kernel.packet[5]);  // 40 50 41 51 42 52                         47 57
+  __m128i t5 = _mm_unpackhi_epi8(kernel.packet[4], kernel.packet[5]);  // 48 58 49 59 4a 5a
+  __m128i t6 = _mm_unpacklo_epi8(kernel.packet[6], kernel.packet[7]);
+  __m128i t7 = _mm_unpackhi_epi8(kernel.packet[6], kernel.packet[7]);
+  __m128i t8 = _mm_unpacklo_epi8(kernel.packet[8], kernel.packet[9]);
+  __m128i t9 = _mm_unpackhi_epi8(kernel.packet[8], kernel.packet[9]);
+  __m128i ta = _mm_unpacklo_epi8(kernel.packet[10], kernel.packet[11]);
+  __m128i tb = _mm_unpackhi_epi8(kernel.packet[10], kernel.packet[11]);
+  __m128i tc = _mm_unpacklo_epi8(kernel.packet[12], kernel.packet[13]);
+  __m128i td = _mm_unpackhi_epi8(kernel.packet[12], kernel.packet[13]);
+  __m128i te = _mm_unpacklo_epi8(kernel.packet[14], kernel.packet[15]);
+  __m128i tf = _mm_unpackhi_epi8(kernel.packet[14], kernel.packet[15]);
 
-  __m128i s0 =  _mm_unpacklo_epi16(t0, t2); // 00 10 20 30 01 11 21 31 02 12 22 32 03 13 23 33
-  __m128i s1 =  _mm_unpackhi_epi16(t0, t2); // 04 14 24 34
-  __m128i s2 =  _mm_unpacklo_epi16(t1, t3); // 08 18 28 38 ...
-  __m128i s3 =  _mm_unpackhi_epi16(t1, t3); // 0c 1c 2c 3c ...
-  __m128i s4 =  _mm_unpacklo_epi16(t4, t6); // 40 50 60 70 41 51 61 71 42 52 62 72 43 53 63 73
-  __m128i s5 =  _mm_unpackhi_epi16(t4, t6); // 44 54 64 74 ...
-  __m128i s6 =  _mm_unpacklo_epi16(t5, t7);
-  __m128i s7 =  _mm_unpackhi_epi16(t5, t7);
-  __m128i s8 =  _mm_unpacklo_epi16(t8, ta);
-  __m128i s9 =  _mm_unpackhi_epi16(t8, ta);
-  __m128i sa =  _mm_unpacklo_epi16(t9, tb);
-  __m128i sb =  _mm_unpackhi_epi16(t9, tb);
-  __m128i sc =  _mm_unpacklo_epi16(tc, te);
-  __m128i sd =  _mm_unpackhi_epi16(tc, te);
-  __m128i se =  _mm_unpacklo_epi16(td, tf);
-  __m128i sf =  _mm_unpackhi_epi16(td, tf);
+  __m128i s0 = _mm_unpacklo_epi16(t0, t2);  // 00 10 20 30 01 11 21 31 02 12 22 32 03 13 23 33
+  __m128i s1 = _mm_unpackhi_epi16(t0, t2);  // 04 14 24 34
+  __m128i s2 = _mm_unpacklo_epi16(t1, t3);  // 08 18 28 38 ...
+  __m128i s3 = _mm_unpackhi_epi16(t1, t3);  // 0c 1c 2c 3c ...
+  __m128i s4 = _mm_unpacklo_epi16(t4, t6);  // 40 50 60 70 41 51 61 71 42 52 62 72 43 53 63 73
+  __m128i s5 = _mm_unpackhi_epi16(t4, t6);  // 44 54 64 74 ...
+  __m128i s6 = _mm_unpacklo_epi16(t5, t7);
+  __m128i s7 = _mm_unpackhi_epi16(t5, t7);
+  __m128i s8 = _mm_unpacklo_epi16(t8, ta);
+  __m128i s9 = _mm_unpackhi_epi16(t8, ta);
+  __m128i sa = _mm_unpacklo_epi16(t9, tb);
+  __m128i sb = _mm_unpackhi_epi16(t9, tb);
+  __m128i sc = _mm_unpacklo_epi16(tc, te);
+  __m128i sd = _mm_unpackhi_epi16(tc, te);
+  __m128i se = _mm_unpacklo_epi16(td, tf);
+  __m128i sf = _mm_unpackhi_epi16(td, tf);
 
-  __m128i u0 =  _mm_unpacklo_epi32(s0, s4); // 00 10 20 30 40 50 60 70 01 11 21 31 41 51 61 71
-  __m128i u1 =  _mm_unpackhi_epi32(s0, s4); // 02 12 22 32 42 52 62 72 03 13 23 33 43 53 63 73
-  __m128i u2 =  _mm_unpacklo_epi32(s1, s5);
-  __m128i u3 =  _mm_unpackhi_epi32(s1, s5);
-  __m128i u4 =  _mm_unpacklo_epi32(s2, s6);
-  __m128i u5 =  _mm_unpackhi_epi32(s2, s6);
-  __m128i u6 =  _mm_unpacklo_epi32(s3, s7);
-  __m128i u7 =  _mm_unpackhi_epi32(s3, s7);
-  __m128i u8 =  _mm_unpacklo_epi32(s8, sc);
-  __m128i u9 =  _mm_unpackhi_epi32(s8, sc);
-  __m128i ua =  _mm_unpacklo_epi32(s9, sd);
-  __m128i ub =  _mm_unpackhi_epi32(s9, sd);
-  __m128i uc =  _mm_unpacklo_epi32(sa, se);
-  __m128i ud =  _mm_unpackhi_epi32(sa, se);
-  __m128i ue =  _mm_unpacklo_epi32(sb, sf);
-  __m128i uf =  _mm_unpackhi_epi32(sb, sf);
+  __m128i u0 = _mm_unpacklo_epi32(s0, s4);  // 00 10 20 30 40 50 60 70 01 11 21 31 41 51 61 71
+  __m128i u1 = _mm_unpackhi_epi32(s0, s4);  // 02 12 22 32 42 52 62 72 03 13 23 33 43 53 63 73
+  __m128i u2 = _mm_unpacklo_epi32(s1, s5);
+  __m128i u3 = _mm_unpackhi_epi32(s1, s5);
+  __m128i u4 = _mm_unpacklo_epi32(s2, s6);
+  __m128i u5 = _mm_unpackhi_epi32(s2, s6);
+  __m128i u6 = _mm_unpacklo_epi32(s3, s7);
+  __m128i u7 = _mm_unpackhi_epi32(s3, s7);
+  __m128i u8 = _mm_unpacklo_epi32(s8, sc);
+  __m128i u9 = _mm_unpackhi_epi32(s8, sc);
+  __m128i ua = _mm_unpacklo_epi32(s9, sd);
+  __m128i ub = _mm_unpackhi_epi32(s9, sd);
+  __m128i uc = _mm_unpacklo_epi32(sa, se);
+  __m128i ud = _mm_unpackhi_epi32(sa, se);
+  __m128i ue = _mm_unpacklo_epi32(sb, sf);
+  __m128i uf = _mm_unpackhi_epi32(sb, sf);
 
-  kernel.packet[0]  = _mm_unpacklo_epi64(u0, u8);
-  kernel.packet[1]  = _mm_unpackhi_epi64(u0, u8);
-  kernel.packet[2]  = _mm_unpacklo_epi64(u1, u9);
-  kernel.packet[3]  = _mm_unpackhi_epi64(u1, u9);
-  kernel.packet[4]  = _mm_unpacklo_epi64(u2, ua);
-  kernel.packet[5]  = _mm_unpackhi_epi64(u2, ua);
-  kernel.packet[6]  = _mm_unpacklo_epi64(u3, ub);
-  kernel.packet[7]  = _mm_unpackhi_epi64(u3, ub);
-  kernel.packet[8]  = _mm_unpacklo_epi64(u4, uc);
-  kernel.packet[9]  = _mm_unpackhi_epi64(u4, uc);
+  kernel.packet[0] = _mm_unpacklo_epi64(u0, u8);
+  kernel.packet[1] = _mm_unpackhi_epi64(u0, u8);
+  kernel.packet[2] = _mm_unpacklo_epi64(u1, u9);
+  kernel.packet[3] = _mm_unpackhi_epi64(u1, u9);
+  kernel.packet[4] = _mm_unpacklo_epi64(u2, ua);
+  kernel.packet[5] = _mm_unpackhi_epi64(u2, ua);
+  kernel.packet[6] = _mm_unpacklo_epi64(u3, ub);
+  kernel.packet[7] = _mm_unpackhi_epi64(u3, ub);
+  kernel.packet[8] = _mm_unpacklo_epi64(u4, uc);
+  kernel.packet[9] = _mm_unpackhi_epi64(u4, uc);
   kernel.packet[10] = _mm_unpacklo_epi64(u5, ud);
   kernel.packet[11] = _mm_unpackhi_epi64(u5, ud);
   kernel.packet[12] = _mm_unpacklo_epi64(u6, ue);
@@ -1239,7 +1992,9 @@
   kernel.packet[15] = _mm_unpackhi_epi64(u7, uf);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4i pblend(const Selector<4>& ifPacket, const Packet4i& thenPacket, const Packet4i& elsePacket) {
+template <>
+EIGEN_STRONG_INLINE Packet4i pblend(const Selector<4>& ifPacket, const Packet4i& thenPacket,
+                                    const Packet4i& elsePacket) {
   const __m128i zero = _mm_setzero_si128();
   const __m128i select = _mm_set_epi32(ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
   __m128i false_mask = _mm_cmpeq_epi32(select, zero);
@@ -1249,7 +2004,14 @@
   return _mm_or_si128(_mm_andnot_si128(false_mask, thenPacket), _mm_and_si128(false_mask, elsePacket));
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet4f pblend(const Selector<4>& ifPacket, const Packet4f& thenPacket, const Packet4f& elsePacket) {
+template <>
+EIGEN_STRONG_INLINE Packet4ui pblend(const Selector<4>& ifPacket, const Packet4ui& thenPacket,
+                                     const Packet4ui& elsePacket) {
+  return (Packet4ui)pblend(ifPacket, (Packet4i)thenPacket, (Packet4i)elsePacket);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f pblend(const Selector<4>& ifPacket, const Packet4f& thenPacket,
+                                    const Packet4f& elsePacket) {
   const __m128 zero = _mm_setzero_ps();
   const __m128 select = _mm_set_ps(ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
   __m128 false_mask = _mm_cmpeq_ps(select, zero);
@@ -1259,7 +2021,9 @@
   return _mm_or_ps(_mm_andnot_ps(false_mask, thenPacket), _mm_and_ps(false_mask, elsePacket));
 #endif
 }
-template<> EIGEN_STRONG_INLINE Packet2d pblend(const Selector<2>& ifPacket, const Packet2d& thenPacket, const Packet2d& elsePacket) {
+template <>
+EIGEN_STRONG_INLINE Packet2d pblend(const Selector<2>& ifPacket, const Packet2d& thenPacket,
+                                    const Packet2d& elsePacket) {
   const __m128d zero = _mm_setzero_pd();
   const __m128d select = _mm_set_pd(ifPacket.select[1], ifPacket.select[0]);
   __m128d false_mask = _mm_cmpeq_pd(select, zero);
@@ -1272,18 +2036,140 @@
 
 // Scalar path for pmadd with FMA to ensure consistency with vectorized path.
 #ifdef EIGEN_VECTORIZE_FMA
-template<> EIGEN_STRONG_INLINE float pmadd(const float& a, const float& b, const float& c) {
-  return ::fmaf(a,b,c);
+template <>
+EIGEN_STRONG_INLINE float pmadd(const float& a, const float& b, const float& c) {
+  return ::fmaf(a, b, c);
 }
-template<> EIGEN_STRONG_INLINE double pmadd(const double& a, const double& b, const double& c) {
-  return ::fma(a,b,c);
+template <>
+EIGEN_STRONG_INLINE double pmadd(const double& a, const double& b, const double& c) {
+  return ::fma(a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE float pmsub(const float& a, const float& b, const float& c) {
+  return ::fmaf(a, b, -c);
+}
+template <>
+EIGEN_STRONG_INLINE double pmsub(const double& a, const double& b, const double& c) {
+  return ::fma(a, b, -c);
+}
+template <>
+EIGEN_STRONG_INLINE float pnmadd(const float& a, const float& b, const float& c) {
+  return ::fmaf(-a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE double pnmadd(const double& a, const double& b, const double& c) {
+  return ::fma(-a, b, c);
+}
+template <>
+EIGEN_STRONG_INLINE float pnmsub(const float& a, const float& b, const float& c) {
+  return ::fmaf(-a, b, -c);
+}
+template <>
+EIGEN_STRONG_INLINE double pnmsub(const double& a, const double& b, const double& c) {
+  return ::fma(-a, b, -c);
 }
 #endif
 
+#ifdef EIGEN_VECTORIZE_SSE4_1
+// Helpers for half->float and float->half conversions.
+// Currently only used by the AVX code.
+EIGEN_STRONG_INLINE __m128i half2floatsse(__m128i h) {
+  __m128i input = _mm_cvtepu16_epi32(h);
+
+  // Direct vectorization of half_to_float, C parts in the comments.
+  __m128i shifted_exp = _mm_set1_epi32(0x7c00 << 13);
+  // o.u = (h.x & 0x7fff) << 13; // exponent/mantissa bits
+  __m128i ou = _mm_slli_epi32(_mm_and_si128(input, _mm_set1_epi32(0x7fff)), 13);
+  // exp = shifted_exp & o.u;   // just the exponent
+  __m128i exp = _mm_and_si128(ou, shifted_exp);
+  // o.u += (127 - 15) << 23;
+  ou = _mm_add_epi32(ou, _mm_set1_epi32((127 - 15) << 23));
+
+  // Inf/NaN?
+  __m128i naninf_mask = _mm_cmpeq_epi32(exp, shifted_exp);
+  // Inf/NaN adjust
+  __m128i naninf_adj = _mm_and_si128(_mm_set1_epi32((128 - 16) << 23), naninf_mask);
+  // extra exp adjust for  Inf/NaN
+  ou = _mm_add_epi32(ou, naninf_adj);
+
+  // Zero/Denormal?
+  __m128i zeroden_mask = _mm_cmpeq_epi32(exp, _mm_setzero_si128());
+  __m128i zeroden_adj = _mm_and_si128(zeroden_mask, _mm_set1_epi32(1 << 23));
+  // o.u += 1 << 23;
+  ou = _mm_add_epi32(ou, zeroden_adj);
+  // magic.u = 113 << 23
+  __m128i magic = _mm_and_si128(zeroden_mask, _mm_set1_epi32(113 << 23));
+  // o.f -= magic.f
+  ou = _mm_castps_si128(_mm_sub_ps(_mm_castsi128_ps(ou), _mm_castsi128_ps(magic)));
+
+  __m128i sign = _mm_slli_epi32(_mm_and_si128(input, _mm_set1_epi32(0x8000)), 16);
+  // o.u |= (h.x & 0x8000) << 16;    // sign bit
+  ou = _mm_or_si128(ou, sign);
+  // return o.f;
+  // We are actually returning uint version, to make
+  // _mm256_insertf128_si256 work.
+  return ou;
+}
+
+EIGEN_STRONG_INLINE __m128i float2half(__m128 f) {
+  __m128i o = _mm_setzero_si128();
+
+  // unsigned int sign_mask = 0x80000000u;
+  __m128i sign = _mm_set1_epi32(0x80000000u);
+  // unsigned int sign = f.u & sign_mask;
+  sign = _mm_and_si128(sign, _mm_castps_si128(f));
+  // f.u ^= sign;
+  f = _mm_xor_ps(f, _mm_castsi128_ps(sign));
+
+  __m128i fu = _mm_castps_si128(f);
+
+  __m128i f16max = _mm_set1_epi32((127 + 16) << 23);
+  __m128i f32infty = _mm_set1_epi32(255 << 23);
+  // if (f.u >= f16max.u) // result is Inf or NaN (all exponent bits set)
+  // there is no _mm_cmpge_epi32, so use lt and swap operands
+  __m128i infnan_mask = _mm_cmplt_epi32(f16max, _mm_castps_si128(f));
+  __m128i inf_mask = _mm_cmpgt_epi32(_mm_castps_si128(f), f32infty);
+  __m128i nan_mask = _mm_andnot_si128(inf_mask, infnan_mask);
+  __m128i inf_value = _mm_and_si128(inf_mask, _mm_set1_epi32(0x7e00));
+  __m128i nan_value = _mm_and_si128(nan_mask, _mm_set1_epi32(0x7c00));
+  // o.x = (f.u > f32infty.u) ? 0x7e00 : 0x7c00; // NaN->qNaN and Inf->Inf
+  __m128i naninf_value = _mm_or_si128(inf_value, nan_value);
+
+  __m128i denorm_magic = _mm_set1_epi32(((127 - 15) + (23 - 10) + 1) << 23);
+  __m128i subnorm_mask = _mm_cmplt_epi32(_mm_castps_si128(f), _mm_set1_epi32(113 << 23));
+  //  f.f += denorm_magic.f;
+  f = _mm_add_ps(f, _mm_castsi128_ps(denorm_magic));
+  // f.u - denorm_magic.u
+  o = _mm_sub_epi32(_mm_castps_si128(f), denorm_magic);
+  o = _mm_and_si128(o, subnorm_mask);
+  // Correct result for inf/nan/zero/subnormal, 0 otherwise
+  o = _mm_or_si128(o, naninf_value);
+
+  __m128i mask = _mm_or_si128(infnan_mask, subnorm_mask);
+  o = _mm_and_si128(o, mask);
+
+  // mant_odd = (f.u >> 13) & 1;
+  __m128i mand_odd = _mm_and_si128(_mm_srli_epi32(fu, 13), _mm_set1_epi32(0x1));
+  // f.u += 0xc8000fffU;
+  fu = _mm_add_epi32(fu, _mm_set1_epi32(0xc8000fffU));
+  // f.u += mant_odd;
+  fu = _mm_add_epi32(fu, mand_odd);
+  fu = _mm_andnot_si128(mask, fu);
+  // f.u >> 13
+  fu = _mm_srli_epi32(fu, 13);
+  o = _mm_or_si128(fu, o);
+
+  // o.x |= static_cast<numext::uint16_t>(sign >> 16);
+  o = _mm_or_si128(o, _mm_srli_epi32(sign, 16));
+
+  // 16 bit values
+  return _mm_and_si128(o, _mm_set1_epi32(0xffff));
+}
+#endif
 
 // Packet math for Eigen::half
 // Disable the following code since it's broken on too many platforms / compilers.
-//#elif defined(EIGEN_VECTORIZE_SSE) && (!EIGEN_ARCH_x86_64) && (!EIGEN_COMP_MSVC)
+// #elif defined(EIGEN_VECTORIZE_SSE) && (!EIGEN_ARCH_x86_64) && (!EIGEN_COMP_MSVC)
 #if 0
 
 typedef struct {
@@ -1302,7 +2188,6 @@
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket = 0,
     HasAdd    = 1,
     HasSub    = 1,
     HasMul    = 1,
@@ -1487,19 +2372,18 @@
 
 #endif
 
+}  // end namespace internal
 
-} // end namespace internal
-
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #if EIGEN_COMP_PGI && EIGEN_COMP_PGI < 1900
 // PGI++ does not define the following intrinsics in C++ mode.
-static inline __m128  _mm_castpd_ps   (__m128d x) { return reinterpret_cast<__m128&>(x);  }
+static inline __m128 _mm_castpd_ps(__m128d x) { return reinterpret_cast<__m128&>(x); }
 static inline __m128i _mm_castpd_si128(__m128d x) { return reinterpret_cast<__m128i&>(x); }
-static inline __m128d _mm_castps_pd   (__m128  x) { return reinterpret_cast<__m128d&>(x); }
-static inline __m128i _mm_castps_si128(__m128  x) { return reinterpret_cast<__m128i&>(x); }
-static inline __m128  _mm_castsi128_ps(__m128i x) { return reinterpret_cast<__m128&>(x);  }
+static inline __m128d _mm_castps_pd(__m128 x) { return reinterpret_cast<__m128d&>(x); }
+static inline __m128i _mm_castps_si128(__m128 x) { return reinterpret_cast<__m128i&>(x); }
+static inline __m128 _mm_castsi128_ps(__m128i x) { return reinterpret_cast<__m128&>(x); }
 static inline __m128d _mm_castsi128_pd(__m128i x) { return reinterpret_cast<__m128d&>(x); }
 #endif
 
-#endif // EIGEN_PACKET_MATH_SSE_H
+#endif  // EIGEN_PACKET_MATH_SSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h
index d2a0037..cbc6d47 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h
@@ -10,83 +10,138 @@
 #ifndef EIGEN_TYPE_CASTING_SSE_H
 #define EIGEN_TYPE_CASTING_SSE_H
 
+// IWYU pragma: private
+#include "../../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 #ifndef EIGEN_VECTORIZE_AVX
 template <>
-struct type_casting_traits<float, int> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
+struct type_casting_traits<float, bool> : vectorized_type_casting_traits<float, bool> {};
+template <>
+struct type_casting_traits<bool, float> : vectorized_type_casting_traits<bool, float> {};
 
 template <>
-struct type_casting_traits<int, float> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 1
-  };
-};
+struct type_casting_traits<float, int> : vectorized_type_casting_traits<float, int> {};
+template <>
+struct type_casting_traits<int, float> : vectorized_type_casting_traits<int, float> {};
 
 template <>
-struct type_casting_traits<double, float> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 2,
-    TgtCoeffRatio = 1
-  };
-};
+struct type_casting_traits<float, double> : vectorized_type_casting_traits<float, double> {};
+template <>
+struct type_casting_traits<double, float> : vectorized_type_casting_traits<double, float> {};
 
 template <>
-struct type_casting_traits<float, double> {
-  enum {
-    VectorizedCast = 1,
-    SrcCoeffRatio = 1,
-    TgtCoeffRatio = 2
-  };
-};
+struct type_casting_traits<double, int> : vectorized_type_casting_traits<double, int> {};
+template <>
+struct type_casting_traits<int, double> : vectorized_type_casting_traits<int, double> {};
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4i pcast<Packet4f, Packet4i>(const Packet4f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet16b pcast<Packet4f, Packet16b>(const Packet4f& a, const Packet4f& b, const Packet4f& c,
+                                                         const Packet4f& d) {
+  __m128 zero = pzero(a);
+  __m128 nonzero_a = _mm_cmpneq_ps(a, zero);
+  __m128 nonzero_b = _mm_cmpneq_ps(b, zero);
+  __m128 nonzero_c = _mm_cmpneq_ps(c, zero);
+  __m128 nonzero_d = _mm_cmpneq_ps(d, zero);
+  __m128i ab_bytes = _mm_packs_epi32(_mm_castps_si128(nonzero_a), _mm_castps_si128(nonzero_b));
+  __m128i cd_bytes = _mm_packs_epi32(_mm_castps_si128(nonzero_c), _mm_castps_si128(nonzero_d));
+  __m128i merged = _mm_packs_epi16(ab_bytes, cd_bytes);
+  return _mm_and_si128(merged, _mm_set1_epi8(1));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet16b, Packet4f>(const Packet16b& a) {
+  const __m128 cst_one = _mm_set_ps1(1.0f);
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  __m128i a_extended = _mm_cvtepi8_epi32(a);
+  __m128i abcd = _mm_cmpeq_epi32(a_extended, _mm_setzero_si128());
+#else
+  __m128i abcd_efhg_ijkl_mnop = _mm_cmpeq_epi8(a, _mm_setzero_si128());
+  __m128i aabb_ccdd_eeff_gghh = _mm_unpacklo_epi8(abcd_efhg_ijkl_mnop, abcd_efhg_ijkl_mnop);
+  __m128i abcd = _mm_unpacklo_epi8(aabb_ccdd_eeff_gghh, aabb_ccdd_eeff_gghh);
+#endif
+  __m128 result = _mm_andnot_ps(_mm_castsi128_ps(abcd), cst_one);
+  return result;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4f, Packet4i>(const Packet4f& a) {
   return _mm_cvttps_epi32(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pcast<Packet4i, Packet4f>(const Packet4i& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet2d, Packet4i>(const Packet2d& a, const Packet2d& b) {
+  return _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(_mm_cvttpd_epi32(a)), _mm_castsi128_ps(_mm_cvttpd_epi32(b)),
+                                         (1 << 2) | (1 << 6)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4i, Packet4f>(const Packet4i& a) {
   return _mm_cvtepi32_ps(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pcast<Packet2d, Packet4f>(const Packet2d& a, const Packet2d& b) {
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet2d, Packet4f>(const Packet2d& a, const Packet2d& b) {
   return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d pcast<Packet4f, Packet2d>(const Packet4f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet4i, Packet2d>(const Packet4i& a) {
+  // Simply discard the second half of the input
+  return _mm_cvtepi32_pd(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet4f, Packet2d>(const Packet4f& a) {
   // Simply discard the second half of the input
   return _mm_cvtps_pd(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i,Packet4f>(const Packet4f& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet4f>(const Packet4f& a) {
+  return _mm_castps_pd(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet2d>(const Packet2d& a) {
+  return _mm_castpd_ps(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4f>(const Packet4f& a) {
   return _mm_castps_si128(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f,Packet4i>(const Packet4i& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet4i>(const Packet4i& a) {
   return _mm_castsi128_ps(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d,Packet4i>(const Packet4i& a) {
+template <>
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet4i>(const Packet4i& a) {
   return _mm_castsi128_pd(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i,Packet2d>(const Packet2d& a) {
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet2d>(const Packet2d& a) {
   return _mm_castpd_si128(a);
 }
 
+template <>
+EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet4i>(const Packet4i& a) {
+  return Packet4ui(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4ui>(const Packet4ui& a) {
+  return Packet4i(a);
+}
 // Disable the following code since it's broken on too many platforms / compilers.
-//#elif defined(EIGEN_VECTORIZE_SSE) && (!EIGEN_ARCH_x86_64) && (!EIGEN_COMP_MSVC)
+// #elif defined(EIGEN_VECTORIZE_SSE) && (!EIGEN_ARCH_x86_64) && (!EIGEN_COMP_MSVC)
 #if 0
 
 template <>
@@ -135,8 +190,8 @@
 
 #endif
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TYPE_CASTING_SSE_H
+#endif  // EIGEN_TYPE_CASTING_SSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h
index bf64ef4..09d1da8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h
@@ -10,168 +10,172 @@
 #ifndef EIGEN_ASSIGNMENT_FUNCTORS_H
 #define EIGEN_ASSIGNMENT_FUNCTORS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-  
-/** \internal
-  * \brief Template functor for scalar/packet assignment
-  *
-  */
-template<typename DstScalar,typename SrcScalar> struct assign_op {
 
-  EIGEN_EMPTY_STRUCT_CTOR(assign_op)
+/** \internal
+ * \brief Template functor for scalar/packet assignment
+ *
+ */
+template <typename DstScalar, typename SrcScalar>
+struct assign_op {
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
-  
-  template<int Alignment, typename Packet>
-  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const
-  { internal::pstoret<DstScalar,Packet,Alignment>(a,b); }
+
+  template <int Alignment, typename Packet>
+  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {
+    internal::pstoret<DstScalar, Packet, Alignment>(a, b);
+  }
 };
 
 // Empty overload for void type (used by PermutationMatrix)
-template<typename DstScalar> struct assign_op<DstScalar,void> {};
+template <typename DstScalar>
+struct assign_op<DstScalar, void> {};
 
-template<typename DstScalar,typename SrcScalar>
-struct functor_traits<assign_op<DstScalar,SrcScalar> > {
+template <typename DstScalar, typename SrcScalar>
+struct functor_traits<assign_op<DstScalar, SrcScalar> > {
   enum {
     Cost = NumTraits<DstScalar>::ReadCost,
-    PacketAccess = is_same<DstScalar,SrcScalar>::value && packet_traits<DstScalar>::Vectorizable && packet_traits<SrcScalar>::Vectorizable
+    PacketAccess = is_same<DstScalar, SrcScalar>::value && packet_traits<DstScalar>::Vectorizable &&
+                   packet_traits<SrcScalar>::Vectorizable
   };
 };
 
 /** \internal
-  * \brief Template functor for scalar/packet assignment with addition
-  *
-  */
-template<typename DstScalar,typename SrcScalar> struct add_assign_op {
-
-  EIGEN_EMPTY_STRUCT_CTOR(add_assign_op)
+ * \brief Template functor for scalar/packet assignment with addition
+ *
+ */
+template <typename DstScalar, typename SrcScalar>
+struct add_assign_op {
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a += b; }
-  
-  template<int Alignment, typename Packet>
-  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const
-  { internal::pstoret<DstScalar,Packet,Alignment>(a,internal::padd(internal::ploadt<Packet,Alignment>(a),b)); }
+
+  template <int Alignment, typename Packet>
+  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {
+    internal::pstoret<DstScalar, Packet, Alignment>(a, internal::padd(internal::ploadt<Packet, Alignment>(a), b));
+  }
 };
-template<typename DstScalar,typename SrcScalar>
-struct functor_traits<add_assign_op<DstScalar,SrcScalar> > {
+template <typename DstScalar, typename SrcScalar>
+struct functor_traits<add_assign_op<DstScalar, SrcScalar> > {
   enum {
     Cost = NumTraits<DstScalar>::ReadCost + NumTraits<DstScalar>::AddCost,
-    PacketAccess = is_same<DstScalar,SrcScalar>::value && packet_traits<DstScalar>::HasAdd
+    PacketAccess = is_same<DstScalar, SrcScalar>::value && packet_traits<DstScalar>::HasAdd
   };
 };
 
 /** \internal
-  * \brief Template functor for scalar/packet assignment with subtraction
-  *
-  */
-template<typename DstScalar,typename SrcScalar> struct sub_assign_op {
-
-  EIGEN_EMPTY_STRUCT_CTOR(sub_assign_op)
+ * \brief Template functor for scalar/packet assignment with subtraction
+ *
+ */
+template <typename DstScalar, typename SrcScalar>
+struct sub_assign_op {
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a -= b; }
-  
-  template<int Alignment, typename Packet>
-  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const
-  { internal::pstoret<DstScalar,Packet,Alignment>(a,internal::psub(internal::ploadt<Packet,Alignment>(a),b)); }
+
+  template <int Alignment, typename Packet>
+  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {
+    internal::pstoret<DstScalar, Packet, Alignment>(a, internal::psub(internal::ploadt<Packet, Alignment>(a), b));
+  }
 };
-template<typename DstScalar,typename SrcScalar>
-struct functor_traits<sub_assign_op<DstScalar,SrcScalar> > {
+template <typename DstScalar, typename SrcScalar>
+struct functor_traits<sub_assign_op<DstScalar, SrcScalar> > {
   enum {
     Cost = NumTraits<DstScalar>::ReadCost + NumTraits<DstScalar>::AddCost,
-    PacketAccess = is_same<DstScalar,SrcScalar>::value && packet_traits<DstScalar>::HasSub
+    PacketAccess = is_same<DstScalar, SrcScalar>::value && packet_traits<DstScalar>::HasSub
   };
 };
 
 /** \internal
-  * \brief Template functor for scalar/packet assignment with multiplication
-  *
-  */
-template<typename DstScalar, typename SrcScalar=DstScalar>
+ * \brief Template functor for scalar/packet assignment with multiplication
+ *
+ */
+template <typename DstScalar, typename SrcScalar = DstScalar>
 struct mul_assign_op {
-
-  EIGEN_EMPTY_STRUCT_CTOR(mul_assign_op)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a *= b; }
-  
-  template<int Alignment, typename Packet>
-  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const
-  { internal::pstoret<DstScalar,Packet,Alignment>(a,internal::pmul(internal::ploadt<Packet,Alignment>(a),b)); }
+
+  template <int Alignment, typename Packet>
+  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {
+    internal::pstoret<DstScalar, Packet, Alignment>(a, internal::pmul(internal::ploadt<Packet, Alignment>(a), b));
+  }
 };
-template<typename DstScalar, typename SrcScalar>
-struct functor_traits<mul_assign_op<DstScalar,SrcScalar> > {
+template <typename DstScalar, typename SrcScalar>
+struct functor_traits<mul_assign_op<DstScalar, SrcScalar> > {
   enum {
     Cost = NumTraits<DstScalar>::ReadCost + NumTraits<DstScalar>::MulCost,
-    PacketAccess = is_same<DstScalar,SrcScalar>::value && packet_traits<DstScalar>::HasMul
+    PacketAccess = is_same<DstScalar, SrcScalar>::value && packet_traits<DstScalar>::HasMul
   };
 };
 
 /** \internal
-  * \brief Template functor for scalar/packet assignment with diviving
-  *
-  */
-template<typename DstScalar, typename SrcScalar=DstScalar> struct div_assign_op {
-
-  EIGEN_EMPTY_STRUCT_CTOR(div_assign_op)
+ * \brief Template functor for scalar/packet assignment with diviving
+ *
+ */
+template <typename DstScalar, typename SrcScalar = DstScalar>
+struct div_assign_op {
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a /= b; }
-  
-  template<int Alignment, typename Packet>
-  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const
-  { internal::pstoret<DstScalar,Packet,Alignment>(a,internal::pdiv(internal::ploadt<Packet,Alignment>(a),b)); }
+
+  template <int Alignment, typename Packet>
+  EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {
+    internal::pstoret<DstScalar, Packet, Alignment>(a, internal::pdiv(internal::ploadt<Packet, Alignment>(a), b));
+  }
 };
-template<typename DstScalar, typename SrcScalar>
-struct functor_traits<div_assign_op<DstScalar,SrcScalar> > {
+template <typename DstScalar, typename SrcScalar>
+struct functor_traits<div_assign_op<DstScalar, SrcScalar> > {
   enum {
     Cost = NumTraits<DstScalar>::ReadCost + NumTraits<DstScalar>::MulCost,
-    PacketAccess = is_same<DstScalar,SrcScalar>::value && packet_traits<DstScalar>::HasDiv
+    PacketAccess = is_same<DstScalar, SrcScalar>::value && packet_traits<DstScalar>::HasDiv
   };
 };
 
 /** \internal
-  * \brief Template functor for scalar/packet assignment with swapping
-  *
-  * It works as follow. For a non-vectorized evaluation loop, we have:
-  *   for(i) func(A.coeffRef(i), B.coeff(i));
-  * where B is a SwapWrapper expression. The trick is to make SwapWrapper::coeff behaves like a non-const coeffRef.
-  * Actually, SwapWrapper might not even be needed since even if B is a plain expression, since it has to be writable
-  * B.coeff already returns a const reference to the underlying scalar value.
-  * 
-  * The case of a vectorized loop is more tricky:
-  *   for(i,j) func.assignPacket<A_Align>(&A.coeffRef(i,j), B.packet<B_Align>(i,j));
-  * Here, B must be a SwapWrapper whose packet function actually returns a proxy object holding a Scalar*,
-  * the actual alignment and Packet type.
-  *
-  */
-template<typename Scalar> struct swap_assign_op {
-
-  EIGEN_EMPTY_STRUCT_CTOR(swap_assign_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Scalar& a, const Scalar& b) const
-  {
+ * \brief Template functor for scalar/packet assignment with swapping
+ *
+ * It works as follow. For a non-vectorized evaluation loop, we have:
+ *   for(i) func(A.coeffRef(i), B.coeff(i));
+ * where B is a SwapWrapper expression. The trick is to make SwapWrapper::coeff behaves like a non-const coeffRef.
+ * Actually, SwapWrapper might not even be needed since even if B is a plain expression, since it has to be writable
+ * B.coeff already returns a const reference to the underlying scalar value.
+ *
+ * The case of a vectorized loop is more tricky:
+ *   for(i,j) func.assignPacket<A_Align>(&A.coeffRef(i,j), B.packet<B_Align>(i,j));
+ * Here, B must be a SwapWrapper whose packet function actually returns a proxy object holding a Scalar*,
+ * the actual alignment and Packet type.
+ *
+ */
+template <typename Scalar>
+struct swap_assign_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Scalar& a, const Scalar& b) const {
 #ifdef EIGEN_GPUCC
     // FIXME is there some kind of cuda::swap?
-    Scalar t=b; const_cast<Scalar&>(b)=a; a=t;
+    Scalar t = b;
+    const_cast<Scalar&>(b) = a;
+    a = t;
 #else
     using std::swap;
-    swap(a,const_cast<Scalar&>(b));
+    swap(a, const_cast<Scalar&>(b));
 #endif
   }
 };
-template<typename Scalar>
+template <typename Scalar>
 struct functor_traits<swap_assign_op<Scalar> > {
   enum {
     Cost = 3 * NumTraits<Scalar>::ReadCost,
-    PacketAccess = 
-    #if defined(EIGEN_VECTORIZE_AVX) && EIGEN_COMP_CLANG && (EIGEN_COMP_CLANG<800 || defined(__apple_build_version__))
-    // This is a partial workaround for a bug in clang generating bad code
-    // when mixing 256/512 bits loads and 128 bits moves.
-    // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1684
-    //     https://bugs.llvm.org/show_bug.cgi?id=40815
+    PacketAccess =
+#if defined(EIGEN_VECTORIZE_AVX) && (EIGEN_CLANG_STRICT_LESS_THAN(8, 0, 0) || EIGEN_COMP_CLANGAPPLE)
+        // This is a partial workaround for a bug in clang generating bad code
+        // when mixing 256/512 bits loads and 128 bits moves.
+        // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1684
+        //     https://bugs.llvm.org/show_bug.cgi?id=40815
     0
-    #else
-    packet_traits<Scalar>::Vectorizable
-    #endif
+#else
+        packet_traits<Scalar>::Vectorizable
+#endif
   };
 };
 
-} // namespace internal
+}  // namespace internal
 
-} // namespace Eigen
+}  // namespace Eigen
 
-#endif // EIGEN_ASSIGNMENT_FUNCTORS_H
+#endif  // EIGEN_ASSIGNMENT_FUNCTORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h
index 63f09ab..c91e6bb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h
@@ -10,119 +10,123 @@
 #ifndef EIGEN_BINARY_FUNCTORS_H
 #define EIGEN_BINARY_FUNCTORS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 //---------- associative binary functors ----------
 
-template<typename Arg1, typename Arg2>
-struct binary_op_base
-{
+template <typename Arg1, typename Arg2>
+struct binary_op_base {
   typedef Arg1 first_argument_type;
   typedef Arg2 second_argument_type;
 };
 
 /** \internal
-  * \brief Template functor to compute the sum of two scalars
-  *
-  * \sa class CwiseBinaryOp, MatrixBase::operator+, class VectorwiseOp, DenseBase::sum()
-  */
-template<typename LhsScalar,typename RhsScalar>
-struct scalar_sum_op : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_sum_op>::ReturnType result_type;
-#ifndef EIGEN_SCALAR_BINARY_OP_PLUGIN
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sum_op)
-#else
-  scalar_sum_op() {
-    EIGEN_SCALAR_BINARY_OP_PLUGIN
-  }
+ * \brief Template functor to compute the sum of two scalars
+ *
+ * \sa class CwiseBinaryOp, MatrixBase::operator+, class VectorwiseOp, DenseBase::sum()
+ */
+template <typename LhsScalar, typename RhsScalar>
+struct scalar_sum_op : binary_op_base<LhsScalar, RhsScalar> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_sum_op>::ReturnType result_type;
+#ifdef EIGEN_SCALAR_BINARY_OP_PLUGIN
+  scalar_sum_op(){EIGEN_SCALAR_BINARY_OP_PLUGIN}
 #endif
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a + b; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::padd(a,b); }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
-  { return internal::predux(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type
+  operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a + b;
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return internal::padd(a, b);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const {
+    return internal::predux(a);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_sum_op<LhsScalar,RhsScalar> > {
+template <typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_sum_op<LhsScalar, RhsScalar>> {
   enum {
-    Cost = (int(NumTraits<LhsScalar>::AddCost) + int(NumTraits<RhsScalar>::AddCost)) / 2, // rough estimate!
-    PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasAdd && packet_traits<RhsScalar>::HasAdd
+    Cost = (int(NumTraits<LhsScalar>::AddCost) + int(NumTraits<RhsScalar>::AddCost)) / 2,  // rough estimate!
+    PacketAccess =
+        is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasAdd && packet_traits<RhsScalar>::HasAdd
     // TODO vectorize mixed sum
   };
 };
 
-
-template<>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool scalar_sum_op<bool,bool>::operator() (const bool& a, const bool& b) const { return a || b; }
-
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool scalar_sum_op<bool, bool>::operator()(const bool& a, const bool& b) const {
+  return a || b;
+}
 
 /** \internal
-  * \brief Template functor to compute the product of two scalars
-  *
-  * \sa class CwiseBinaryOp, Cwise::operator*(), class VectorwiseOp, MatrixBase::redux()
-  */
-template<typename LhsScalar,typename RhsScalar>
-struct scalar_product_op  : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_product_op>::ReturnType result_type;
-#ifndef EIGEN_SCALAR_BINARY_OP_PLUGIN
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_product_op)
-#else
-  scalar_product_op() {
-    EIGEN_SCALAR_BINARY_OP_PLUGIN
-  }
+ * \brief Template functor to compute the product of two scalars
+ *
+ * \sa class CwiseBinaryOp, Cwise::operator*(), class VectorwiseOp, MatrixBase::redux()
+ */
+template <typename LhsScalar, typename RhsScalar>
+struct scalar_product_op : binary_op_base<LhsScalar, RhsScalar> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_product_op>::ReturnType result_type;
+#ifdef EIGEN_SCALAR_BINARY_OP_PLUGIN
+  scalar_product_op(){EIGEN_SCALAR_BINARY_OP_PLUGIN}
 #endif
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a * b; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::pmul(a,b); }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
-  { return internal::predux_mul(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type
+  operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a * b;
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return internal::pmul(a, b);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const {
+    return internal::predux_mul(a);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_product_op<LhsScalar,RhsScalar> > {
+template <typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_product_op<LhsScalar, RhsScalar>> {
   enum {
-    Cost = (int(NumTraits<LhsScalar>::MulCost) + int(NumTraits<RhsScalar>::MulCost))/2, // rough estimate!
-    PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasMul && packet_traits<RhsScalar>::HasMul
+    Cost = (int(NumTraits<LhsScalar>::MulCost) + int(NumTraits<RhsScalar>::MulCost)) / 2,  // rough estimate!
+    PacketAccess =
+        is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasMul && packet_traits<RhsScalar>::HasMul
     // TODO vectorize mixed product
   };
 };
 
-template<>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool scalar_product_op<bool,bool>::operator() (const bool& a, const bool& b) const { return a && b; }
-
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool scalar_product_op<bool, bool>::operator()(const bool& a,
+                                                                                     const bool& b) const {
+  return a && b;
+}
 
 /** \internal
-  * \brief Template functor to compute the conjugate product of two scalars
-  *
-  * This is a short cut for conj(x) * y which is needed for optimization purpose; in Eigen2 support mode, this becomes x * conj(y)
-  */
-template<typename LhsScalar,typename RhsScalar>
-struct scalar_conj_product_op  : binary_op_base<LhsScalar,RhsScalar>
-{
+ * \brief Template functor to compute the conjugate product of two scalars
+ *
+ * This is a short cut for conj(x) * y which is needed for optimization purpose; in Eigen2 support mode, this becomes x
+ * * conj(y)
+ */
+template <typename LhsScalar, typename RhsScalar>
+struct scalar_conj_product_op : binary_op_base<LhsScalar, RhsScalar> {
+  enum { Conj = NumTraits<LhsScalar>::IsComplex };
 
-  enum {
-    Conj = NumTraits<LhsScalar>::IsComplex
-  };
-  
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_conj_product_op>::ReturnType result_type;
-  
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_conj_product_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const
-  { return conj_helper<LhsScalar,RhsScalar,Conj,false>().pmul(a,b); }
-  
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
-  { return conj_helper<Packet,Packet,Conj,false>().pmul(a,b); }
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_conj_product_op>::ReturnType result_type;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return conj_helper<LhsScalar, RhsScalar, Conj, false>().pmul(a, b);
+  }
+
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return conj_helper<Packet, Packet, Conj, false>().pmul(a, b);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_conj_product_op<LhsScalar,RhsScalar> > {
+template <typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_conj_product_op<LhsScalar, RhsScalar>> {
   enum {
     Cost = NumTraits<LhsScalar>::MulCost,
     PacketAccess = internal::is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasMul
@@ -130,183 +134,213 @@
 };
 
 /** \internal
-  * \brief Template functor to compute the min of two scalars
-  *
-  * \sa class CwiseBinaryOp, MatrixBase::cwiseMin, class VectorwiseOp, MatrixBase::minCoeff()
-  */
-template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
-struct scalar_min_op : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_min_op>::ReturnType result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_min_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const {
+ * \brief Template functor to compute the min of two scalars
+ *
+ * \sa class CwiseBinaryOp, MatrixBase::cwiseMin, class VectorwiseOp, MatrixBase::minCoeff()
+ */
+template <typename LhsScalar, typename RhsScalar, int NaNPropagation>
+struct scalar_min_op : binary_op_base<LhsScalar, RhsScalar> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_min_op>::ReturnType result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
     return internal::pmin<NaNPropagation>(a, b);
   }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
-  {
-    return internal::pmin<NaNPropagation>(a,b);
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return internal::pmin<NaNPropagation>(a, b);
   }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
-  {
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const {
     return internal::predux_min<NaNPropagation>(a);
   }
 };
 
-template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
-struct functor_traits<scalar_min_op<LhsScalar,RhsScalar, NaNPropagation> > {
+template <typename LhsScalar, typename RhsScalar, int NaNPropagation>
+struct functor_traits<scalar_min_op<LhsScalar, RhsScalar, NaNPropagation>> {
   enum {
-    Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
+    Cost = (NumTraits<LhsScalar>::AddCost + NumTraits<RhsScalar>::AddCost) / 2,
     PacketAccess = internal::is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasMin
   };
 };
 
 /** \internal
-  * \brief Template functor to compute the max of two scalars
-  *
-  * \sa class CwiseBinaryOp, MatrixBase::cwiseMax, class VectorwiseOp, MatrixBase::maxCoeff()
-  */
-template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
-struct scalar_max_op : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_max_op>::ReturnType result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const {
-    return internal::pmax<NaNPropagation>(a,b);
+ * \brief Template functor to compute the max of two scalars
+ *
+ * \sa class CwiseBinaryOp, MatrixBase::cwiseMax, class VectorwiseOp, MatrixBase::maxCoeff()
+ */
+template <typename LhsScalar, typename RhsScalar, int NaNPropagation>
+struct scalar_max_op : binary_op_base<LhsScalar, RhsScalar> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_max_op>::ReturnType result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return internal::pmax<NaNPropagation>(a, b);
   }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
-  {
-    return internal::pmax<NaNPropagation>(a,b);
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return internal::pmax<NaNPropagation>(a, b);
   }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
-  {
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const {
     return internal::predux_max<NaNPropagation>(a);
   }
 };
 
-template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
-struct functor_traits<scalar_max_op<LhsScalar,RhsScalar, NaNPropagation> > {
+template <typename LhsScalar, typename RhsScalar, int NaNPropagation>
+struct functor_traits<scalar_max_op<LhsScalar, RhsScalar, NaNPropagation>> {
   enum {
-    Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
+    Cost = (NumTraits<LhsScalar>::AddCost + NumTraits<RhsScalar>::AddCost) / 2,
     PacketAccess = internal::is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasMax
   };
 };
 
 /** \internal
-  * \brief Template functors for comparison of two scalars
-  * \todo Implement packet-comparisons
-  */
-template<typename LhsScalar, typename RhsScalar, ComparisonName cmp> struct scalar_cmp_op;
+ * \brief Template functors for comparison of two scalars
+ * \todo Implement packet-comparisons
+ */
+template <typename LhsScalar, typename RhsScalar, ComparisonName cmp, bool UseTypedComparators = false>
+struct scalar_cmp_op;
 
-template<typename LhsScalar, typename RhsScalar, ComparisonName cmp>
-struct functor_traits<scalar_cmp_op<LhsScalar,RhsScalar, cmp> > {
+template <typename LhsScalar, typename RhsScalar, ComparisonName cmp, bool UseTypedComparators>
+struct functor_traits<scalar_cmp_op<LhsScalar, RhsScalar, cmp, UseTypedComparators>> {
   enum {
-    Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
-    PacketAccess = false
+    Cost = (NumTraits<LhsScalar>::AddCost + NumTraits<RhsScalar>::AddCost) / 2,
+    PacketAccess = (UseTypedComparators || is_same<LhsScalar, bool>::value) && is_same<LhsScalar, RhsScalar>::value &&
+                   packet_traits<LhsScalar>::HasCmp
   };
 };
 
-template<ComparisonName Cmp, typename LhsScalar, typename RhsScalar>
-struct result_of<scalar_cmp_op<LhsScalar, RhsScalar, Cmp>(LhsScalar,RhsScalar)> {
-  typedef bool type;
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct typed_cmp_helper {
+  static constexpr bool SameType = is_same<LhsScalar, RhsScalar>::value;
+  static constexpr bool IsNumeric = is_arithmetic<typename NumTraits<LhsScalar>::Real>::value;
+  static constexpr bool UseTyped = UseTypedComparators && SameType && IsNumeric;
+  using type = typename conditional<UseTyped, LhsScalar, bool>::type;
 };
 
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+using cmp_return_t = typename typed_cmp_helper<LhsScalar, RhsScalar, UseTypedComparators>::type;
 
-template<typename LhsScalar, typename RhsScalar>
-struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_EQ> : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef bool result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return a==b;}
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_EQ, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
+  using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a == b ? result_type(1) : result_type(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(result_type(1));
+    return pand(pcmp_eq(a, b), cst_one);
+  }
 };
-template<typename LhsScalar, typename RhsScalar>
-struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_LT> : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef bool result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return a<b;}
+
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_LT, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
+  using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a < b ? result_type(1) : result_type(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(result_type(1));
+    return pand(pcmp_lt(a, b), cst_one);
+  }
 };
-template<typename LhsScalar, typename RhsScalar>
-struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_LE> : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef bool result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return a<=b;}
+
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_LE, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
+  using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a <= b ? result_type(1) : result_type(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(result_type(1));
+    return pand(cst_one, pcmp_le(a, b));
+  }
 };
-template<typename LhsScalar, typename RhsScalar>
-struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_GT> : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef bool result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return a>b;}
+
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_GT, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
+  using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a > b ? result_type(1) : result_type(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(result_type(1));
+    return pand(cst_one, pcmp_lt(b, a));
+  }
 };
-template<typename LhsScalar, typename RhsScalar>
-struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_GE> : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef bool result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return a>=b;}
+
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_GE, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
+  using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a >= b ? result_type(1) : result_type(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(result_type(1));
+    return pand(cst_one, pcmp_le(b, a));
+  }
 };
-template<typename LhsScalar, typename RhsScalar>
-struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_UNORD> : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef bool result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return !(a<=b || b<=a);}
+
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_UNORD, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
+  using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return !(a <= b || b <= a) ? result_type(1) : result_type(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(result_type(1));
+    return pandnot(cst_one, por(pcmp_le(a, b), pcmp_le(b, a)));
+  }
 };
-template<typename LhsScalar, typename RhsScalar>
-struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_NEQ> : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef bool result_type;
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return a!=b;}
+
+template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
+struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_NEQ, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
+  using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a != b ? result_type(1) : result_type(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(result_type(1));
+    return pandnot(cst_one, pcmp_eq(a, b));
+  }
 };
 
 /** \internal
-  * \brief Template functor to compute the hypot of two \b positive \b and \b real scalars
-  *
-  * \sa MatrixBase::stableNorm(), class Redux
-  */
-template<typename Scalar>
-struct scalar_hypot_op<Scalar,Scalar> : binary_op_base<Scalar,Scalar>
-{
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_hypot_op)
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar &x, const Scalar &y) const
-  {
+ * \brief Template functor to compute the hypot of two \b positive \b and \b real scalars
+ *
+ * \sa MatrixBase::stableNorm(), class Redux
+ */
+template <typename Scalar>
+struct scalar_hypot_op<Scalar, Scalar> : binary_op_base<Scalar, Scalar> {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& x, const Scalar& y) const {
     // This functor is used by hypotNorm only for which it is faster to first apply abs
     // on all coefficients prior to reduction through hypot.
     // This way we avoid calling abs on positive and real entries, and this also permits
     // to seamlessly handle complexes. Otherwise we would have to handle both real and complexes
     // through the same functor...
-    return internal::positive_real_hypot(x,y);
+    return internal::positive_real_hypot(x, y);
   }
 };
-template<typename Scalar>
-struct functor_traits<scalar_hypot_op<Scalar,Scalar> > {
-  enum
-  {
-    Cost = 3 * NumTraits<Scalar>::AddCost +
-           2 * NumTraits<Scalar>::MulCost +
-           2 * scalar_div_cost<Scalar,false>::value,
+template <typename Scalar>
+struct functor_traits<scalar_hypot_op<Scalar, Scalar>> {
+  enum {
+    Cost = 3 * NumTraits<Scalar>::AddCost + 2 * NumTraits<Scalar>::MulCost + 2 * scalar_div_cost<Scalar, false>::value,
     PacketAccess = false
   };
 };
 
 /** \internal
-  * \brief Template functor to compute the pow of two scalars
-  * See the specification of pow in https://en.cppreference.com/w/cpp/numeric/math/pow
-  */
-template<typename Scalar, typename Exponent>
-struct scalar_pow_op  : binary_op_base<Scalar,Exponent>
-{
-  typedef typename ScalarBinaryOpTraits<Scalar,Exponent,scalar_pow_op>::ReturnType result_type;
-#ifndef EIGEN_SCALAR_BINARY_OP_PLUGIN
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_pow_op)
-#else
+ * \brief Template functor to compute the pow of two scalars
+ * See the specification of pow in https://en.cppreference.com/w/cpp/numeric/math/pow
+ */
+template <typename Scalar, typename Exponent>
+struct scalar_pow_op : binary_op_base<Scalar, Exponent> {
+  typedef typename ScalarBinaryOpTraits<Scalar, Exponent, scalar_pow_op>::ReturnType result_type;
+#ifdef EIGEN_SCALAR_BINARY_OP_PLUGIN
   scalar_pow_op() {
     typedef Scalar LhsScalar;
     typedef Exponent RhsScalar;
@@ -314,228 +348,416 @@
   }
 #endif
 
-  EIGEN_DEVICE_FUNC
-  inline result_type operator() (const Scalar& a, const Exponent& b) const { return numext::pow(a, b); }
+  EIGEN_DEVICE_FUNC inline result_type operator()(const Scalar& a, const Exponent& b) const {
+    return numext::pow(a, b);
+  }
 
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  {
-    return generic_pow(a,b);
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const {
+    return generic_pow(a, b);
   }
 };
 
-template<typename Scalar, typename Exponent>
-struct functor_traits<scalar_pow_op<Scalar,Exponent> > {
+template <typename Scalar, typename Exponent>
+struct functor_traits<scalar_pow_op<Scalar, Exponent>> {
   enum {
     Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = (!NumTraits<Scalar>::IsComplex && !NumTraits<Scalar>::IsInteger &&
-                    packet_traits<Scalar>::HasExp && packet_traits<Scalar>::HasLog &&
-                    packet_traits<Scalar>::HasRound && packet_traits<Scalar>::HasCmp &&
-                    // Temporarly disable packet access for half/bfloat16 until
+    PacketAccess = (!NumTraits<Scalar>::IsComplex && !NumTraits<Scalar>::IsInteger && packet_traits<Scalar>::HasExp &&
+                    packet_traits<Scalar>::HasLog && packet_traits<Scalar>::HasRound && packet_traits<Scalar>::HasCmp &&
+                    // Temporarily disable packet access for half/bfloat16 until
                     // accuracy is improved.
-                    !is_same<Scalar, half>::value && !is_same<Scalar, bfloat16>::value
-                    )
+                    !is_same<Scalar, half>::value && !is_same<Scalar, bfloat16>::value)
   };
 };
 
 //---------- non associative binary functors ----------
 
 /** \internal
-  * \brief Template functor to compute the difference of two scalars
-  *
-  * \sa class CwiseBinaryOp, MatrixBase::operator-
-  */
-template<typename LhsScalar,typename RhsScalar>
-struct scalar_difference_op : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_difference_op>::ReturnType result_type;
-#ifndef EIGEN_SCALAR_BINARY_OP_PLUGIN
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_difference_op)
-#else
-  scalar_difference_op() {
-    EIGEN_SCALAR_BINARY_OP_PLUGIN
-  }
+ * \brief Template functor to compute the difference of two scalars
+ *
+ * \sa class CwiseBinaryOp, MatrixBase::operator-
+ */
+template <typename LhsScalar, typename RhsScalar>
+struct scalar_difference_op : binary_op_base<LhsScalar, RhsScalar> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_difference_op>::ReturnType result_type;
+#ifdef EIGEN_SCALAR_BINARY_OP_PLUGIN
+  scalar_difference_op(){EIGEN_SCALAR_BINARY_OP_PLUGIN}
 #endif
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a - b; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::psub(a,b); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
+  operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a - b;
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const {
+    return internal::psub(a, b);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_difference_op<LhsScalar,RhsScalar> > {
+template <typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_difference_op<LhsScalar, RhsScalar>> {
   enum {
     Cost = (int(NumTraits<LhsScalar>::AddCost) + int(NumTraits<RhsScalar>::AddCost)) / 2,
-    PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasSub && packet_traits<RhsScalar>::HasSub
+    PacketAccess =
+        is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasSub && packet_traits<RhsScalar>::HasSub
   };
 };
 
-/** \internal
-  * \brief Template functor to compute the quotient of two scalars
-  *
-  * \sa class CwiseBinaryOp, Cwise::operator/()
-  */
-template<typename LhsScalar,typename RhsScalar>
-struct scalar_quotient_op  : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_quotient_op>::ReturnType result_type;
-#ifndef EIGEN_SCALAR_BINARY_OP_PLUGIN
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_quotient_op)
-#else
-  scalar_quotient_op() {
-    EIGEN_SCALAR_BINARY_OP_PLUGIN
+template <typename Packet, bool IsInteger = NumTraits<typename unpacket_traits<Packet>::type>::IsInteger>
+struct maybe_raise_div_by_zero {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Packet x) { EIGEN_UNUSED_VARIABLE(x); }
+};
+
+#ifndef EIGEN_GPU_COMPILE_PHASE
+template <typename Packet>
+struct maybe_raise_div_by_zero<Packet, true> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Packet x) {
+    if (EIGEN_PREDICT_FALSE(predux_any(pcmp_eq(x, pzero(x))))) {
+      // Use volatile variables to force a division by zero, which will
+      // result in the default platform behaviour (usually SIGFPE).
+      volatile typename unpacket_traits<Packet>::type zero = 0;
+      volatile typename unpacket_traits<Packet>::type val = 1;
+      val = val / zero;
+    }
   }
+};
 #endif
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a / b; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::pdiv(a,b); }
+
+/** \internal
+ * \brief Template functor to compute the quotient of two scalars
+ *
+ * \sa class CwiseBinaryOp, Cwise::operator/()
+ */
+template <typename LhsScalar, typename RhsScalar>
+struct scalar_quotient_op : binary_op_base<LhsScalar, RhsScalar> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_quotient_op>::ReturnType result_type;
+#ifdef EIGEN_SCALAR_BINARY_OP_PLUGIN
+  scalar_quotient_op(){EIGEN_SCALAR_BINARY_OP_PLUGIN}
+#endif
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
+  operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return a / b;
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const {
+    maybe_raise_div_by_zero<Packet>::run(b);
+    return internal::pdiv(a, b);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_quotient_op<LhsScalar,RhsScalar> > {
-  typedef typename scalar_quotient_op<LhsScalar,RhsScalar>::result_type result_type;
+template <typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_quotient_op<LhsScalar, RhsScalar>> {
+  typedef typename scalar_quotient_op<LhsScalar, RhsScalar>::result_type result_type;
   enum {
-    PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasDiv && packet_traits<RhsScalar>::HasDiv,
-    Cost = scalar_div_cost<result_type,PacketAccess>::value
+    PacketAccess =
+        is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasDiv && packet_traits<RhsScalar>::HasDiv,
+    Cost = scalar_div_cost<result_type, PacketAccess>::value
   };
 };
 
-
-
 /** \internal
-  * \brief Template functor to compute the and of two booleans
-  *
-  * \sa class CwiseBinaryOp, ArrayBase::operator&&
-  */
+ * \brief Template functor to compute the and of two scalars as if they were booleans
+ *
+ * \sa class CwiseBinaryOp, ArrayBase::operator&&
+ */
+template <typename Scalar>
 struct scalar_boolean_and_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_and_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a && b; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::pand(a,b); }
+  using result_type = Scalar;
+  // `false` any value `a` that satisfies `a == Scalar(0)`
+  // `true` is the complement of `false`
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a, const Scalar& b) const {
+    return (a != Scalar(0)) && (b != Scalar(0)) ? Scalar(1) : Scalar(0);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    // and(a,b) == !or(!a,!b)
+    Packet not_a = pcmp_eq(a, pzero(a));
+    Packet not_b = pcmp_eq(b, pzero(b));
+    Packet a_nand_b = por(not_a, not_b);
+    return pandnot(cst_one, a_nand_b);
+  }
 };
-template<> struct functor_traits<scalar_boolean_and_op> {
-  enum {
-    Cost = NumTraits<bool>::AddCost,
-    PacketAccess = true
-  };
+template <typename Scalar>
+struct functor_traits<scalar_boolean_and_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasCmp };
 };
 
 /** \internal
-  * \brief Template functor to compute the or of two booleans
-  *
-  * \sa class CwiseBinaryOp, ArrayBase::operator||
-  */
+ * \brief Template functor to compute the or of two scalars as if they were booleans
+ *
+ * \sa class CwiseBinaryOp, ArrayBase::operator||
+ */
+template <typename Scalar>
 struct scalar_boolean_or_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_or_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a || b; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::por(a,b); }
+  using result_type = Scalar;
+  // `false` any value `a` that satisfies `a == Scalar(0)`
+  // `true` is the complement of `false`
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a, const Scalar& b) const {
+    return (a != Scalar(0)) || (b != Scalar(0)) ? Scalar(1) : Scalar(0);
+  }
+  template <typename Packet>
+  EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    // if or(a,b) == 0, then a == 0 and b == 0
+    // or(a,b) == !nor(a,b)
+    Packet a_nor_b = pcmp_eq(por(a, b), pzero(a));
+    return pandnot(cst_one, a_nor_b);
+  }
 };
-template<> struct functor_traits<scalar_boolean_or_op> {
-  enum {
-    Cost = NumTraits<bool>::AddCost,
-    PacketAccess = true
-  };
+template <typename Scalar>
+struct functor_traits<scalar_boolean_or_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasCmp };
 };
 
 /** \internal
- * \brief Template functor to compute the xor of two booleans
+ * \brief Template functor to compute the xor of two scalars as if they were booleans
  *
  * \sa class CwiseBinaryOp, ArrayBase::operator^
  */
+template <typename Scalar>
 struct scalar_boolean_xor_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_xor_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a ^ b; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::pxor(a,b); }
+  using result_type = Scalar;
+  // `false` any value `a` that satisfies `a == Scalar(0)`
+  // `true` is the complement of `false`
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a, const Scalar& b) const {
+    return (a != Scalar(0)) != (b != Scalar(0)) ? Scalar(1) : Scalar(0);
+  }
+  template <typename Packet>
+  EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    // xor(a,b) == xor(!a,!b)
+    Packet not_a = pcmp_eq(a, pzero(a));
+    Packet not_b = pcmp_eq(b, pzero(b));
+    Packet a_xor_b = pxor(not_a, not_b);
+    return pand(cst_one, a_xor_b);
+  }
 };
-template<> struct functor_traits<scalar_boolean_xor_op> {
-  enum {
-    Cost = NumTraits<bool>::AddCost,
-    PacketAccess = true
-  };
+template <typename Scalar>
+struct functor_traits<scalar_boolean_xor_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasCmp };
+};
+
+template <typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct bitwise_binary_impl {
+  static constexpr size_t Size = sizeof(Scalar);
+  using uint_t = typename numext::get_integer_by_size<Size>::unsigned_type;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_and(const Scalar& a, const Scalar& b) {
+    uint_t a_as_uint = numext::bit_cast<uint_t, Scalar>(a);
+    uint_t b_as_uint = numext::bit_cast<uint_t, Scalar>(b);
+    uint_t result = a_as_uint & b_as_uint;
+    return numext::bit_cast<Scalar, uint_t>(result);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_or(const Scalar& a, const Scalar& b) {
+    uint_t a_as_uint = numext::bit_cast<uint_t, Scalar>(a);
+    uint_t b_as_uint = numext::bit_cast<uint_t, Scalar>(b);
+    uint_t result = a_as_uint | b_as_uint;
+    return numext::bit_cast<Scalar, uint_t>(result);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_xor(const Scalar& a, const Scalar& b) {
+    uint_t a_as_uint = numext::bit_cast<uint_t, Scalar>(a);
+    uint_t b_as_uint = numext::bit_cast<uint_t, Scalar>(b);
+    uint_t result = a_as_uint ^ b_as_uint;
+    return numext::bit_cast<Scalar, uint_t>(result);
+  }
+};
+
+template <typename Scalar>
+struct bitwise_binary_impl<Scalar, true> {
+  using Real = typename NumTraits<Scalar>::Real;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_and(const Scalar& a, const Scalar& b) {
+    Real real_result = bitwise_binary_impl<Real>::run_and(numext::real(a), numext::real(b));
+    Real imag_result = bitwise_binary_impl<Real>::run_and(numext::imag(a), numext::imag(b));
+    return Scalar(real_result, imag_result);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_or(const Scalar& a, const Scalar& b) {
+    Real real_result = bitwise_binary_impl<Real>::run_or(numext::real(a), numext::real(b));
+    Real imag_result = bitwise_binary_impl<Real>::run_or(numext::imag(a), numext::imag(b));
+    return Scalar(real_result, imag_result);
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_xor(const Scalar& a, const Scalar& b) {
+    Real real_result = bitwise_binary_impl<Real>::run_xor(numext::real(a), numext::real(b));
+    Real imag_result = bitwise_binary_impl<Real>::run_xor(numext::imag(a), numext::imag(b));
+    return Scalar(real_result, imag_result);
+  }
 };
 
 /** \internal
-  * \brief Template functor to compute the absolute difference of two scalars
-  *
-  * \sa class CwiseBinaryOp, MatrixBase::absolute_difference
-  */
-template<typename LhsScalar,typename RhsScalar>
-struct scalar_absolute_difference_op : binary_op_base<LhsScalar,RhsScalar>
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_absolute_difference_op>::ReturnType result_type;
-#ifndef EIGEN_SCALAR_BINARY_OP_PLUGIN
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_absolute_difference_op)
-#else
-  scalar_absolute_difference_op() {
-    EIGEN_SCALAR_BINARY_OP_PLUGIN
+ * \brief Template functor to compute the bitwise and of two scalars
+ *
+ * \sa class CwiseBinaryOp, ArrayBase::operator&
+ */
+template <typename Scalar>
+struct scalar_bitwise_and_op {
+  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::RequireInitialization,
+                      BITWISE OPERATIONS MAY ONLY BE PERFORMED ON PLAIN DATA TYPES)
+  EIGEN_STATIC_ASSERT((!internal::is_same<Scalar, bool>::value), DONT USE BITWISE OPS ON BOOLEAN TYPES)
+  using result_type = Scalar;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a, const Scalar& b) const {
+    return bitwise_binary_impl<Scalar>::run_and(a, b);
   }
-#endif
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const
-  { return numext::absdiff(a,b); }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::pabsdiff(a,b); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return pand(a, b);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_absolute_difference_op<LhsScalar,RhsScalar> > {
+template <typename Scalar>
+struct functor_traits<scalar_bitwise_and_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = true };
+};
+
+/** \internal
+ * \brief Template functor to compute the bitwise or of two scalars
+ *
+ * \sa class CwiseBinaryOp, ArrayBase::operator|
+ */
+template <typename Scalar>
+struct scalar_bitwise_or_op {
+  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::RequireInitialization,
+                      BITWISE OPERATIONS MAY ONLY BE PERFORMED ON PLAIN DATA TYPES)
+  EIGEN_STATIC_ASSERT((!internal::is_same<Scalar, bool>::value), DONT USE BITWISE OPS ON BOOLEAN TYPES)
+  using result_type = Scalar;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a, const Scalar& b) const {
+    return bitwise_binary_impl<Scalar>::run_or(a, b);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return por(a, b);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_bitwise_or_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = true };
+};
+
+/** \internal
+ * \brief Template functor to compute the bitwise xor of two scalars
+ *
+ * \sa class CwiseBinaryOp, ArrayBase::operator^
+ */
+template <typename Scalar>
+struct scalar_bitwise_xor_op {
+  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::RequireInitialization,
+                      BITWISE OPERATIONS MAY ONLY BE PERFORMED ON PLAIN DATA TYPES)
+  EIGEN_STATIC_ASSERT((!internal::is_same<Scalar, bool>::value), DONT USE BITWISE OPS ON BOOLEAN TYPES)
+  using result_type = Scalar;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a, const Scalar& b) const {
+    return bitwise_binary_impl<Scalar>::run_xor(a, b);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const {
+    return pxor(a, b);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_bitwise_xor_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = true };
+};
+
+/** \internal
+ * \brief Template functor to compute the absolute difference of two scalars
+ *
+ * \sa class CwiseBinaryOp, MatrixBase::absolute_difference
+ */
+template <typename LhsScalar, typename RhsScalar>
+struct scalar_absolute_difference_op : binary_op_base<LhsScalar, RhsScalar> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_absolute_difference_op>::ReturnType result_type;
+#ifdef EIGEN_SCALAR_BINARY_OP_PLUGIN
+  scalar_absolute_difference_op(){EIGEN_SCALAR_BINARY_OP_PLUGIN}
+#endif
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
+  operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return numext::absdiff(a, b);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const {
+    return internal::pabsdiff(a, b);
+  }
+};
+template <typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_absolute_difference_op<LhsScalar, RhsScalar>> {
   enum {
-    Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
-    PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasAbsDiff
+    Cost = (NumTraits<LhsScalar>::AddCost + NumTraits<RhsScalar>::AddCost) / 2,
+    PacketAccess = is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasAbsDiff
   };
 };
 
+template <typename LhsScalar, typename RhsScalar>
+struct scalar_atan2_op {
+  using Scalar = LhsScalar;
 
+  static constexpr bool Enable =
+      is_same<LhsScalar, RhsScalar>::value && !NumTraits<Scalar>::IsInteger && !NumTraits<Scalar>::IsComplex;
+  EIGEN_STATIC_ASSERT(Enable, "LhsScalar and RhsScalar must be the same non-integer, non-complex type")
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& y, const Scalar& x) const {
+    return numext::atan2(y, x);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& y, const Packet& x) const {
+    return internal::patan2(y, x);
+  }
+};
+
+template <typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_atan2_op<LhsScalar, RhsScalar>> {
+  using Scalar = LhsScalar;
+  enum {
+    PacketAccess = is_same<LhsScalar, RhsScalar>::value && packet_traits<Scalar>::HasATan &&
+                   packet_traits<Scalar>::HasDiv && !NumTraits<Scalar>::IsInteger && !NumTraits<Scalar>::IsComplex,
+    Cost = int(scalar_div_cost<Scalar, PacketAccess>::value) + int(functor_traits<scalar_atan_op<Scalar>>::Cost)
+  };
+};
 
 //---------- binary functors bound to a constant, thus appearing as a unary functor ----------
 
-// The following two classes permits to turn any binary functor into a unary one with one argument bound to a constant value.
-// They are analogues to std::binder1st/binder2nd but with the following differences:
+// The following two classes permits to turn any binary functor into a unary one with one argument bound to a constant
+// value. They are analogues to std::binder1st/binder2nd but with the following differences:
 //  - they are compatible with packetOp
 //  - they are portable across C++ versions (the std::binder* are deprecated in C++11)
-template<typename BinaryOp> struct bind1st_op : BinaryOp {
-
-  typedef typename BinaryOp::first_argument_type  first_argument_type;
+template <typename BinaryOp>
+struct bind1st_op : BinaryOp {
+  typedef typename BinaryOp::first_argument_type first_argument_type;
   typedef typename BinaryOp::second_argument_type second_argument_type;
-  typedef typename BinaryOp::result_type          result_type;
+  typedef typename BinaryOp::result_type result_type;
 
-  EIGEN_DEVICE_FUNC explicit bind1st_op(const first_argument_type &val) : m_value(val) {}
+  EIGEN_DEVICE_FUNC explicit bind1st_op(const first_argument_type& val) : m_value(val) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const second_argument_type& b) const { return BinaryOp::operator()(m_value,b); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator()(const second_argument_type& b) const {
+    return BinaryOp::operator()(m_value, b);
+  }
 
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& b) const
-  { return BinaryOp::packetOp(internal::pset1<Packet>(m_value), b); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& b) const {
+    return BinaryOp::packetOp(internal::pset1<Packet>(m_value), b);
+  }
 
   first_argument_type m_value;
 };
-template<typename BinaryOp> struct functor_traits<bind1st_op<BinaryOp> > : functor_traits<BinaryOp> {};
+template <typename BinaryOp>
+struct functor_traits<bind1st_op<BinaryOp>> : functor_traits<BinaryOp> {};
 
-
-template<typename BinaryOp> struct bind2nd_op : BinaryOp {
-
-  typedef typename BinaryOp::first_argument_type  first_argument_type;
+template <typename BinaryOp>
+struct bind2nd_op : BinaryOp {
+  typedef typename BinaryOp::first_argument_type first_argument_type;
   typedef typename BinaryOp::second_argument_type second_argument_type;
-  typedef typename BinaryOp::result_type          result_type;
+  typedef typename BinaryOp::result_type result_type;
 
-  EIGEN_DEVICE_FUNC explicit bind2nd_op(const second_argument_type &val) : m_value(val) {}
+  EIGEN_DEVICE_FUNC explicit bind2nd_op(const second_argument_type& val) : m_value(val) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const first_argument_type& a) const { return BinaryOp::operator()(a,m_value); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator()(const first_argument_type& a) const {
+    return BinaryOp::operator()(a, m_value);
+  }
 
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return BinaryOp::packetOp(a,internal::pset1<Packet>(m_value)); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return BinaryOp::packetOp(a, internal::pset1<Packet>(m_value));
+  }
 
   second_argument_type m_value;
 };
-template<typename BinaryOp> struct functor_traits<bind2nd_op<BinaryOp> > : functor_traits<BinaryOp> {};
+template <typename BinaryOp>
+struct functor_traits<bind2nd_op<BinaryOp>> : functor_traits<BinaryOp> {};
 
+}  // end namespace internal
 
-} // end namespace internal
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_BINARY_FUNCTORS_H
+#endif  // EIGEN_BINARY_FUNCTORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h
index 192f225..c53bb90 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h
@@ -10,72 +10,82 @@
 #ifndef EIGEN_NULLARY_FUNCTORS_H
 #define EIGEN_NULLARY_FUNCTORS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename Scalar>
+template <typename Scalar>
 struct scalar_constant_op {
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_constant_op(const scalar_constant_op& other) : m_other(other.m_other) { }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_constant_op(const Scalar& other) : m_other(other) { }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() () const { return m_other; }
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packetOp() const { return internal::pset1<PacketType>(m_other); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_constant_op(const scalar_constant_op& other) : m_other(other.m_other) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_constant_op(const Scalar& other) : m_other(other) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()() const { return m_other; }
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packetOp() const {
+    return internal::pset1<PacketType>(m_other);
+  }
   const Scalar m_other;
 };
-template<typename Scalar>
-struct functor_traits<scalar_constant_op<Scalar> >
-{ enum { Cost = 0 /* as the constant value should be loaded in register only once for the whole expression */,
-         PacketAccess = packet_traits<Scalar>::Vectorizable, IsRepeatable = true }; };
-
-template<typename Scalar> struct scalar_identity_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_identity_op)
-  template<typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (IndexType row, IndexType col) const { return row==col ? Scalar(1) : Scalar(0); }
+template <typename Scalar>
+struct functor_traits<scalar_constant_op<Scalar> > {
+  enum {
+    Cost = 0 /* as the constant value should be loaded in register only once for the whole expression */,
+    PacketAccess = packet_traits<Scalar>::Vectorizable,
+    IsRepeatable = true
+  };
 };
-template<typename Scalar>
-struct functor_traits<scalar_identity_op<Scalar> >
-{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false, IsRepeatable = true }; };
-
-template <typename Scalar, bool IsInteger> struct linspaced_op_impl;
 
 template <typename Scalar>
-struct linspaced_op_impl<Scalar,/*IsInteger*/false>
-{
+struct scalar_identity_op {
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(IndexType row, IndexType col) const {
+    return row == col ? Scalar(1) : Scalar(0);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_identity_op<Scalar> > {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false, IsRepeatable = true };
+};
+
+template <typename Scalar, bool IsInteger>
+struct linspaced_op_impl;
+
+template <typename Scalar>
+struct linspaced_op_impl<Scalar, /*IsInteger*/ false> {
   typedef typename NumTraits<Scalar>::Real RealScalar;
 
-  EIGEN_DEVICE_FUNC linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps) :
-    m_low(low), m_high(high), m_size1(num_steps==1 ? 1 : num_steps-1), m_step(num_steps==1 ? Scalar() : Scalar((high-low)/RealScalar(num_steps-1))),
-    m_flip(numext::abs(high)<numext::abs(low))
-  {}
+  EIGEN_DEVICE_FUNC linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps)
+      : m_low(low),
+        m_high(high),
+        m_size1(num_steps == 1 ? 1 : num_steps - 1),
+        m_step(num_steps == 1 ? Scalar() : Scalar((high - low) / RealScalar(num_steps - 1))),
+        m_flip(numext::abs(high) < numext::abs(low)) {}
 
-  template<typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (IndexType i) const {
-    if(m_flip)
-      return (i==0)? m_low : Scalar(m_high - RealScalar(m_size1-i)*m_step);
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(IndexType i) const {
+    if (m_flip)
+      return (i == 0) ? m_low : Scalar(m_high - RealScalar(m_size1 - i) * m_step);
     else
-      return (i==m_size1)? m_high : Scalar(m_low + RealScalar(i)*m_step);
+      return (i == m_size1) ? m_high : Scalar(m_low + RealScalar(i) * m_step);
   }
 
-  template<typename Packet, typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const
-  {
+  template <typename Packet, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const {
     // Principle:
     // [low, ..., low] + ( [step, ..., step] * ( [i, ..., i] + [0, ..., size] ) )
-    if(m_flip)
-    {
-      Packet pi = plset<Packet>(Scalar(i-m_size1));
+    if (m_flip) {
+      Packet pi = plset<Packet>(Scalar(i - m_size1));
       Packet res = padd(pset1<Packet>(m_high), pmul(pset1<Packet>(m_step), pi));
       if (EIGEN_PREDICT_TRUE(i != 0)) return res;
       Packet mask = pcmp_lt(pset1<Packet>(0), plset<Packet>(0));
       return pselect<Packet>(mask, res, pset1<Packet>(m_low));
-    }
-    else
-    {
+    } else {
       Packet pi = plset<Packet>(Scalar(i));
       Packet res = padd(pset1<Packet>(m_low), pmul(pset1<Packet>(m_step), pi));
-      if(EIGEN_PREDICT_TRUE(i != m_size1-unpacket_traits<Packet>::size+1)) return res;
-      Packet mask = pcmp_lt(plset<Packet>(0), pset1<Packet>(unpacket_traits<Packet>::size-1));
+      if (EIGEN_PREDICT_TRUE(i != m_size1 - unpacket_traits<Packet>::size + 1)) return res;
+      Packet mask = pcmp_lt(plset<Packet>(0), pset1<Packet>(unpacket_traits<Packet>::size - 1));
       return pselect<Packet>(mask, res, pset1<Packet>(m_high));
     }
   }
@@ -88,21 +98,20 @@
 };
 
 template <typename Scalar>
-struct linspaced_op_impl<Scalar,/*IsInteger*/true>
-{
-  EIGEN_DEVICE_FUNC linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps) :
-    m_low(low),
-    m_multiplier((high-low)/convert_index<Scalar>(num_steps<=1 ? 1 : num_steps-1)),
-    m_divisor(convert_index<Scalar>((high>=low?num_steps:-num_steps)+(high-low))/((numext::abs(high-low)+1)==0?1:(numext::abs(high-low)+1))),
-    m_use_divisor(num_steps>1 && (numext::abs(high-low)+1)<num_steps)
-  {}
+struct linspaced_op_impl<Scalar, /*IsInteger*/ true> {
+  EIGEN_DEVICE_FUNC linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps)
+      : m_low(low),
+        m_multiplier((high - low) / convert_index<Scalar>(num_steps <= 1 ? 1 : num_steps - 1)),
+        m_divisor(convert_index<Scalar>((high >= low ? num_steps : -num_steps) + (high - low)) /
+                  ((numext::abs(high - low) + 1) == 0 ? 1 : (numext::abs(high - low) + 1))),
+        m_use_divisor(num_steps > 1 && (numext::abs(high - low) + 1) < num_steps) {}
 
-  template<typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const Scalar operator() (IndexType i) const
-  {
-    if(m_use_divisor) return m_low + convert_index<Scalar>(i)/m_divisor;
-    else              return m_low + convert_index<Scalar>(i)*m_multiplier;
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(IndexType i) const {
+    if (m_use_divisor)
+      return m_low + convert_index<Scalar>(i) / m_divisor;
+    else
+      return m_low + convert_index<Scalar>(i) * m_multiplier;
   }
 
   const Scalar m_low;
@@ -116,74 +125,139 @@
 // Forward declaration (we default to random access which does not really give
 // us a speed gain when using packet access but it allows to use the functor in
 // nested expressions).
-template <typename Scalar> struct linspaced_op;
-template <typename Scalar> struct functor_traits< linspaced_op<Scalar> >
-{
-  enum
-  {
+template <typename Scalar>
+struct linspaced_op;
+template <typename Scalar>
+struct functor_traits<linspaced_op<Scalar> > {
+  enum {
     Cost = 1,
-    PacketAccess =   (!NumTraits<Scalar>::IsInteger) && packet_traits<Scalar>::HasSetLinear && packet_traits<Scalar>::HasBlend,
-                  /*&& ((!NumTraits<Scalar>::IsInteger) || packet_traits<Scalar>::HasDiv),*/ // <- vectorization for integer is currently disabled
+    PacketAccess =
+        (!NumTraits<Scalar>::IsInteger) && packet_traits<Scalar>::HasSetLinear && packet_traits<Scalar>::HasBlend,
+    /*&& ((!NumTraits<Scalar>::IsInteger) || packet_traits<Scalar>::HasDiv),*/  // <- vectorization for integer is
+                                                                                // currently disabled
     IsRepeatable = true
   };
 };
-template <typename Scalar> struct linspaced_op
-{
+template <typename Scalar>
+struct linspaced_op {
   EIGEN_DEVICE_FUNC linspaced_op(const Scalar& low, const Scalar& high, Index num_steps)
-    : impl((num_steps==1 ? high : low),high,num_steps)
-  {}
+      : impl((num_steps == 1 ? high : low), high, num_steps) {}
 
-  template<typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (IndexType i) const { return impl(i); }
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(IndexType i) const {
+    return impl(i);
+  }
 
-  template<typename Packet,typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const { return impl.template packetOp<Packet>(i); }
+  template <typename Packet, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const {
+    return impl.template packetOp<Packet>(i);
+  }
 
   // This proxy object handles the actual required temporaries and the different
   // implementations (integer vs. floating point).
-  const linspaced_op_impl<Scalar,NumTraits<Scalar>::IsInteger> impl;
+  const linspaced_op_impl<Scalar, NumTraits<Scalar>::IsInteger> impl;
+};
+
+template <typename Scalar>
+struct equalspaced_op {
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+
+  EIGEN_DEVICE_FUNC equalspaced_op(const Scalar& start, const Scalar& step) : m_start(start), m_step(step) {}
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(IndexType i) const {
+    return m_start + m_step * static_cast<Scalar>(i);
+  }
+  template <typename Packet, typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(IndexType i) const {
+    const Packet cst_start = pset1<Packet>(m_start);
+    const Packet cst_step = pset1<Packet>(m_step);
+    const Packet cst_lin0 = plset<Packet>(Scalar(0));
+    const Packet cst_offset = pmadd(cst_lin0, cst_step, cst_start);
+
+    Packet i_packet = pset1<Packet>(static_cast<Scalar>(i));
+    return pmadd(i_packet, cst_step, cst_offset);
+  }
+  const Scalar m_start;
+  const Scalar m_step;
+};
+
+template <typename Scalar>
+struct functor_traits<equalspaced_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost + NumTraits<Scalar>::MulCost,
+    PacketAccess =
+        packet_traits<Scalar>::HasSetLinear && packet_traits<Scalar>::HasMul && packet_traits<Scalar>::HasAdd,
+    IsRepeatable = true
+  };
 };
 
 // Linear access is automatically determined from the operator() prototypes available for the given functor.
 // If it exposes an operator()(i,j), then we assume the i and j coefficients are required independently
 // and linear access is not possible. In all other cases, linear access is enabled.
 // Users should not have to deal with this structure.
-template<typename Functor> struct functor_has_linear_access { enum { ret = !has_binary_operator<Functor>::value }; };
+template <typename Functor>
+struct functor_has_linear_access {
+  enum { ret = !has_binary_operator<Functor>::value };
+};
 
 // For unreliable compilers, let's specialize the has_*ary_operator
 // helpers so that at least built-in nullary functors work fine.
-#if !( (EIGEN_COMP_MSVC>1600) || (EIGEN_GNUC_AT_LEAST(4,8)) || (EIGEN_COMP_ICC>=1600))
-template<typename Scalar,typename IndexType>
-struct has_nullary_operator<scalar_constant_op<Scalar>,IndexType> { enum { value = 1}; };
-template<typename Scalar,typename IndexType>
-struct has_unary_operator<scalar_constant_op<Scalar>,IndexType> { enum { value = 0}; };
-template<typename Scalar,typename IndexType>
-struct has_binary_operator<scalar_constant_op<Scalar>,IndexType> { enum { value = 0}; };
+#if !(EIGEN_COMP_MSVC || EIGEN_COMP_GNUC || (EIGEN_COMP_ICC >= 1600))
+template <typename Scalar, typename IndexType>
+struct has_nullary_operator<scalar_constant_op<Scalar>, IndexType> {
+  enum { value = 1 };
+};
+template <typename Scalar, typename IndexType>
+struct has_unary_operator<scalar_constant_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
+template <typename Scalar, typename IndexType>
+struct has_binary_operator<scalar_constant_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
 
-template<typename Scalar,typename IndexType>
-struct has_nullary_operator<scalar_identity_op<Scalar>,IndexType> { enum { value = 0}; };
-template<typename Scalar,typename IndexType>
-struct has_unary_operator<scalar_identity_op<Scalar>,IndexType> { enum { value = 0}; };
-template<typename Scalar,typename IndexType>
-struct has_binary_operator<scalar_identity_op<Scalar>,IndexType> { enum { value = 1}; };
+template <typename Scalar, typename IndexType>
+struct has_nullary_operator<scalar_identity_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
+template <typename Scalar, typename IndexType>
+struct has_unary_operator<scalar_identity_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
+template <typename Scalar, typename IndexType>
+struct has_binary_operator<scalar_identity_op<Scalar>, IndexType> {
+  enum { value = 1 };
+};
 
-template<typename Scalar,typename IndexType>
-struct has_nullary_operator<linspaced_op<Scalar>,IndexType> { enum { value = 0}; };
-template<typename Scalar,typename IndexType>
-struct has_unary_operator<linspaced_op<Scalar>,IndexType> { enum { value = 1}; };
-template<typename Scalar,typename IndexType>
-struct has_binary_operator<linspaced_op<Scalar>,IndexType> { enum { value = 0}; };
+template <typename Scalar, typename IndexType>
+struct has_nullary_operator<linspaced_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
+template <typename Scalar, typename IndexType>
+struct has_unary_operator<linspaced_op<Scalar>, IndexType> {
+  enum { value = 1 };
+};
+template <typename Scalar, typename IndexType>
+struct has_binary_operator<linspaced_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
 
-template<typename Scalar,typename IndexType>
-struct has_nullary_operator<scalar_random_op<Scalar>,IndexType> { enum { value = 1}; };
-template<typename Scalar,typename IndexType>
-struct has_unary_operator<scalar_random_op<Scalar>,IndexType> { enum { value = 0}; };
-template<typename Scalar,typename IndexType>
-struct has_binary_operator<scalar_random_op<Scalar>,IndexType> { enum { value = 0}; };
+template <typename Scalar, typename IndexType>
+struct has_nullary_operator<scalar_random_op<Scalar>, IndexType> {
+  enum { value = 1 };
+};
+template <typename Scalar, typename IndexType>
+struct has_unary_operator<scalar_random_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
+template <typename Scalar, typename IndexType>
+struct has_binary_operator<scalar_random_op<Scalar>, IndexType> {
+  enum { value = 0 };
+};
 #endif
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_NULLARY_FUNCTORS_H
+#endif  // EIGEN_NULLARY_FUNCTORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h
index 4570c9b..0599ce3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h
@@ -10,148 +10,131 @@
 #ifndef EIGEN_STL_FUNCTORS_H
 #define EIGEN_STL_FUNCTORS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
-
-// Portable replacements for certain functors.
-namespace numext {
-
-template<typename T = void>
-struct equal_to {
-  typedef bool result_type;
-  EIGEN_DEVICE_FUNC bool operator()(const T& lhs, const T& rhs) const {
-    return lhs == rhs;
-  }
-};
-
-template<typename T = void>
-struct not_equal_to {
-  typedef bool result_type;
-  EIGEN_DEVICE_FUNC bool operator()(const T& lhs, const T& rhs) const {
-    return lhs != rhs;
-  }
-};
-
-}
-
-
 namespace internal {
 
 // default functor traits for STL functors:
 
-template<typename T>
-struct functor_traits<std::multiplies<T> >
-{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::multiplies<T> > {
+  enum { Cost = NumTraits<T>::MulCost, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::divides<T> >
-{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::divides<T> > {
+  enum { Cost = NumTraits<T>::MulCost, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::plus<T> >
-{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::plus<T> > {
+  enum { Cost = NumTraits<T>::AddCost, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::minus<T> >
-{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::minus<T> > {
+  enum { Cost = NumTraits<T>::AddCost, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::negate<T> >
-{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::negate<T> > {
+  enum { Cost = NumTraits<T>::AddCost, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::logical_or<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::logical_or<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::logical_and<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::logical_and<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::logical_not<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::logical_not<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::greater<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::greater<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::less<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::less<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::greater_equal<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::greater_equal<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::less_equal<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::less_equal<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<std::equal_to<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::equal_to<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
-template<typename T>
-struct functor_traits<numext::equal_to<T> >
-  : functor_traits<std::equal_to<T> > {};
-
-template<typename T>
-struct functor_traits<std::not_equal_to<T> >
-{ enum { Cost = 1, PacketAccess = false }; };
-
-template<typename T>
-struct functor_traits<numext::not_equal_to<T> >
-  : functor_traits<std::not_equal_to<T> > {};
-
-#if (EIGEN_COMP_CXXVER < 11)
-// std::binder* are deprecated since c++11 and will be removed in c++17
-template<typename T>
-struct functor_traits<std::binder2nd<T> >
-{ enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
-
-template<typename T>
-struct functor_traits<std::binder1st<T> >
-{ enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
-#endif
+template <typename T>
+struct functor_traits<std::not_equal_to<T> > {
+  enum { Cost = 1, PacketAccess = false };
+};
 
 #if (EIGEN_COMP_CXXVER < 17)
 // std::unary_negate is deprecated since c++17 and will be removed in c++20
-template<typename T>
-struct functor_traits<std::unary_negate<T> >
-{ enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::unary_negate<T> > {
+  enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false };
+};
 
 // std::binary_negate is deprecated since c++17 and will be removed in c++20
-template<typename T>
-struct functor_traits<std::binary_negate<T> >
-{ enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false }; };
+template <typename T>
+struct functor_traits<std::binary_negate<T> > {
+  enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false };
+};
 #endif
 
 #ifdef EIGEN_STDEXT_SUPPORT
 
-template<typename T0,typename T1>
-struct functor_traits<std::project1st<T0,T1> >
-{ enum { Cost = 0, PacketAccess = false }; };
+template <typename T0, typename T1>
+struct functor_traits<std::project1st<T0, T1> > {
+  enum { Cost = 0, PacketAccess = false };
+};
 
-template<typename T0,typename T1>
-struct functor_traits<std::project2nd<T0,T1> >
-{ enum { Cost = 0, PacketAccess = false }; };
+template <typename T0, typename T1>
+struct functor_traits<std::project2nd<T0, T1> > {
+  enum { Cost = 0, PacketAccess = false };
+};
 
-template<typename T0,typename T1>
-struct functor_traits<std::select2nd<std::pair<T0,T1> > >
-{ enum { Cost = 0, PacketAccess = false }; };
+template <typename T0, typename T1>
+struct functor_traits<std::select2nd<std::pair<T0, T1> > > {
+  enum { Cost = 0, PacketAccess = false };
+};
 
-template<typename T0,typename T1>
-struct functor_traits<std::select1st<std::pair<T0,T1> > >
-{ enum { Cost = 0, PacketAccess = false }; };
+template <typename T0, typename T1>
+struct functor_traits<std::select1st<std::pair<T0, T1> > > {
+  enum { Cost = 0, PacketAccess = false };
+};
 
-template<typename T0,typename T1>
-struct functor_traits<std::unary_compose<T0,T1> >
-{ enum { Cost = functor_traits<T0>::Cost + functor_traits<T1>::Cost, PacketAccess = false }; };
+template <typename T0, typename T1>
+struct functor_traits<std::unary_compose<T0, T1> > {
+  enum { Cost = functor_traits<T0>::Cost + functor_traits<T1>::Cost, PacketAccess = false };
+};
 
-template<typename T0,typename T1,typename T2>
-struct functor_traits<std::binary_compose<T0,T1,T2> >
-{ enum { Cost = functor_traits<T0>::Cost + functor_traits<T1>::Cost + functor_traits<T2>::Cost, PacketAccess = false }; };
+template <typename T0, typename T1, typename T2>
+struct functor_traits<std::binary_compose<T0, T1, T2> > {
+  enum { Cost = functor_traits<T0>::Cost + functor_traits<T1>::Cost + functor_traits<T2>::Cost, PacketAccess = false };
+};
 
-#endif // EIGEN_STDEXT_SUPPORT
+#endif  // EIGEN_STDEXT_SUPPORT
 
 // allow to add new functors and specializations of functor_traits from outside Eigen.
 // this macro is really needed because functor_traits must be specialized after it is declared but before it is used...
@@ -159,8 +142,8 @@
 #include EIGEN_FUNCTORS_PLUGIN
 #endif
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_STL_FUNCTORS_H
+#endif  // EIGEN_STL_FUNCTORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h
index b254e96..745779a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h
@@ -10,16 +10,43 @@
 #ifndef EIGEN_TERNARY_FUNCTORS_H
 #define EIGEN_TERNARY_FUNCTORS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 //---------- associative ternary functors ----------
 
+template <typename ThenScalar, typename ElseScalar, typename ConditionScalar>
+struct scalar_boolean_select_op {
+  static constexpr bool ThenElseAreSame = is_same<ThenScalar, ElseScalar>::value;
+  EIGEN_STATIC_ASSERT(ThenElseAreSame, THEN AND ELSE MUST BE SAME TYPE)
+  using Scalar = ThenScalar;
+  using result_type = Scalar;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const ThenScalar& a, const ElseScalar& b,
+                                                          const ConditionScalar& cond) const {
+    return cond == ConditionScalar(0) ? b : a;
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b, const Packet& cond) const {
+    return pselect(pcmp_eq(cond, pzero(cond)), b, a);
+  }
+};
 
+template <typename ThenScalar, typename ElseScalar, typename ConditionScalar>
+struct functor_traits<scalar_boolean_select_op<ThenScalar, ElseScalar, ConditionScalar>> {
+  using Scalar = ThenScalar;
+  enum {
+    Cost = 1,
+    PacketAccess = is_same<ThenScalar, ElseScalar>::value && is_same<ConditionScalar, Scalar>::value &&
+                   packet_traits<Scalar>::HasCmp
+  };
+};
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TERNARY_FUNCTORS_H
+#endif  // EIGEN_TERNARY_FUNCTORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h
index 16136d1..3c7dfb7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h
@@ -10,112 +10,114 @@
 #ifndef EIGEN_UNARY_FUNCTORS_H
 #define EIGEN_UNARY_FUNCTORS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 /** \internal
-  * \brief Template functor to compute the opposite of a scalar
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::operator-
-  */
-template<typename Scalar> struct scalar_opposite_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_opposite_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return -a; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return internal::pnegate(a); }
+ * \brief Template functor to compute the opposite of a scalar
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::operator-
+ */
+template <typename Scalar>
+struct scalar_opposite_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return -a; }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return internal::pnegate(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_opposite_op<Scalar> >
-{ enum {
-    Cost = NumTraits<Scalar>::AddCost,
-    PacketAccess = packet_traits<Scalar>::HasNegate };
+template <typename Scalar>
+struct functor_traits<scalar_opposite_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasNegate };
 };
 
 /** \internal
-  * \brief Template functor to compute the absolute value of a scalar
-  *
-  * \sa class CwiseUnaryOp, Cwise::abs
-  */
-template<typename Scalar> struct scalar_abs_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_abs_op)
+ * \brief Template functor to compute the absolute value of a scalar
+ *
+ * \sa class CwiseUnaryOp, Cwise::abs
+ */
+template <typename Scalar>
+struct scalar_abs_op {
   typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return numext::abs(a); }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return internal::pabs(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator()(const Scalar& a) const { return numext::abs(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return internal::pabs(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_abs_op<Scalar> >
-{
-  enum {
-    Cost = NumTraits<Scalar>::AddCost,
-    PacketAccess = packet_traits<Scalar>::HasAbs
-  };
+template <typename Scalar>
+struct functor_traits<scalar_abs_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasAbs };
 };
 
 /** \internal
-  * \brief Template functor to compute the score of a scalar, to chose a pivot
-  *
-  * \sa class CwiseUnaryOp
-  */
-template<typename Scalar> struct scalar_score_coeff_op : scalar_abs_op<Scalar>
-{
+ * \brief Template functor to compute the score of a scalar, to chose a pivot
+ *
+ * \sa class CwiseUnaryOp
+ */
+template <typename Scalar>
+struct scalar_score_coeff_op : scalar_abs_op<Scalar> {
   typedef void Score_is_abs;
 };
-template<typename Scalar>
-struct functor_traits<scalar_score_coeff_op<Scalar> > : functor_traits<scalar_abs_op<Scalar> > {};
+template <typename Scalar>
+struct functor_traits<scalar_score_coeff_op<Scalar>> : functor_traits<scalar_abs_op<Scalar>> {};
 
 /* Avoid recomputing abs when we know the score and they are the same. Not a true Eigen functor.  */
-template<typename Scalar, typename=void> struct abs_knowing_score
-{
-  EIGEN_EMPTY_STRUCT_CTOR(abs_knowing_score)
+template <typename Scalar, typename = void>
+struct abs_knowing_score {
   typedef typename NumTraits<Scalar>::Real result_type;
-  template<typename Score>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a, const Score&) const { return numext::abs(a); }
+  template <typename Score>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator()(const Scalar& a, const Score&) const {
+    return numext::abs(a);
+  }
 };
-template<typename Scalar> struct abs_knowing_score<Scalar, typename scalar_score_coeff_op<Scalar>::Score_is_abs>
-{
-  EIGEN_EMPTY_STRUCT_CTOR(abs_knowing_score)
+template <typename Scalar>
+struct abs_knowing_score<Scalar, typename scalar_score_coeff_op<Scalar>::Score_is_abs> {
   typedef typename NumTraits<Scalar>::Real result_type;
-  template<typename Scal>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const Scal&, const result_type& a) const { return a; }
+  template <typename Scal>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator()(const Scal&, const result_type& a) const {
+    return a;
+  }
 };
 
 /** \internal
-  * \brief Template functor to compute the squared absolute value of a scalar
-  *
-  * \sa class CwiseUnaryOp, Cwise::abs2
-  */
-template<typename Scalar> struct scalar_abs2_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_abs2_op)
+ * \brief Template functor to compute the squared absolute value of a scalar
+ *
+ * \sa class CwiseUnaryOp, Cwise::abs2
+ */
+template <typename Scalar>
+struct scalar_abs2_op {
   typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return numext::abs2(a); }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return internal::pmul(a,a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator()(const Scalar& a) const { return numext::abs2(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return internal::pmul(a, a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_abs2_op<Scalar> >
-{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasAbs2 }; };
+template <typename Scalar>
+struct functor_traits<scalar_abs2_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasAbs2 };
+};
 
 /** \internal
-  * \brief Template functor to compute the conjugate of a complex value
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::conjugate()
-  */
-template<typename Scalar> struct scalar_conjugate_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_conjugate_op)
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return numext::conj(a); }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const { return internal::pconj(a); }
+ * \brief Template functor to compute the conjugate of a complex value
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::conjugate()
+ */
+template <typename Scalar>
+struct scalar_conjugate_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return numext::conj(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return internal::pconj(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_conjugate_op<Scalar> >
-{
+template <typename Scalar>
+struct functor_traits<scalar_conjugate_op<Scalar>> {
   enum {
     Cost = 0,
     // Yes the cost is zero even for complexes because in most cases for which
@@ -131,305 +133,359 @@
 };
 
 /** \internal
-  * \brief Template functor to compute the phase angle of a complex
-  *
-  * \sa class CwiseUnaryOp, Cwise::arg
-  */
-template<typename Scalar> struct scalar_arg_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_arg_op)
+ * \brief Template functor to compute the phase angle of a complex
+ *
+ * \sa class CwiseUnaryOp, Cwise::arg
+ */
+template <typename Scalar>
+struct scalar_arg_op {
   typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return numext::arg(a); }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return internal::parg(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator()(const Scalar& a) const { return numext::arg(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return internal::parg(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_arg_op<Scalar> >
-{
+template <typename Scalar>
+struct functor_traits<scalar_arg_op<Scalar>> {
   enum {
     Cost = NumTraits<Scalar>::IsComplex ? 5 * NumTraits<Scalar>::MulCost : NumTraits<Scalar>::AddCost,
     PacketAccess = packet_traits<Scalar>::HasArg
   };
 };
-/** \internal
-  * \brief Template functor to cast a scalar to another type
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::cast()
-  */
-template<typename Scalar, typename NewType>
-struct scalar_cast_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
-  typedef NewType result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const NewType operator() (const Scalar& a) const { return cast<Scalar, NewType>(a); }
-};
-template<typename Scalar, typename NewType>
-struct functor_traits<scalar_cast_op<Scalar,NewType> >
-{ enum { Cost = is_same<Scalar, NewType>::value ? 0 : NumTraits<NewType>::AddCost, PacketAccess = false }; };
 
 /** \internal
-  * \brief Template functor to arithmetically shift a scalar right by a number of bits
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::shift_right()
-  */
-template<typename Scalar, int N>
-struct scalar_shift_right_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_shift_right_op)
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const
-  { return a >> N; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return internal::parithmetic_shift_right<N>(a); }
-};
-template<typename Scalar, int N>
-struct functor_traits<scalar_shift_right_op<Scalar,N> >
-{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasShift }; };
-
-/** \internal
-  * \brief Template functor to logically shift a scalar left by a number of bits
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::shift_left()
-  */
-template<typename Scalar, int N>
-struct scalar_shift_left_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_shift_left_op)
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const
-  { return a << N; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return internal::plogical_shift_left<N>(a); }
-};
-template<typename Scalar, int N>
-struct functor_traits<scalar_shift_left_op<Scalar,N> >
-{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasShift }; };
-
-/** \internal
-  * \brief Template functor to extract the real part of a complex
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::real()
-  */
-template<typename Scalar>
-struct scalar_real_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_real_op)
-  typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return numext::real(a); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_real_op<Scalar> >
-{ enum { Cost = 0, PacketAccess = false }; };
-
-/** \internal
-  * \brief Template functor to extract the imaginary part of a complex
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::imag()
-  */
-template<typename Scalar>
-struct scalar_imag_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_op)
-  typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return numext::imag(a); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_imag_op<Scalar> >
-{ enum { Cost = 0, PacketAccess = false }; };
-
-/** \internal
-  * \brief Template functor to extract the real part of a complex as a reference
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::real()
-  */
-template<typename Scalar>
-struct scalar_real_ref_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_real_ref_op)
-  typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return numext::real_ref(*const_cast<Scalar*>(&a)); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_real_ref_op<Scalar> >
-{ enum { Cost = 0, PacketAccess = false }; };
-
-/** \internal
-  * \brief Template functor to extract the imaginary part of a complex as a reference
-  *
-  * \sa class CwiseUnaryOp, MatrixBase::imag()
-  */
-template<typename Scalar>
-struct scalar_imag_ref_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_ref_op)
-  typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return numext::imag_ref(*const_cast<Scalar*>(&a)); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_imag_ref_op<Scalar> >
-{ enum { Cost = 0, PacketAccess = false }; };
-
-/** \internal
-  *
-  * \brief Template functor to compute the exponential of a scalar
-  *
-  * \sa class CwiseUnaryOp, Cwise::exp()
-  */
-template<typename Scalar> struct scalar_exp_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_exp_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::exp(a); }
+ * \brief Template functor to compute the complex argument, returned as a complex type
+ *
+ * \sa class CwiseUnaryOp, Cwise::carg
+ */
+template <typename Scalar>
+struct scalar_carg_op {
+  using result_type = Scalar;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const {
+    return Scalar(numext::arg(a));
+  }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pexp(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return pcarg(a);
+  }
 };
 template <typename Scalar>
-struct functor_traits<scalar_exp_op<Scalar> > {
+struct functor_traits<scalar_carg_op<Scalar>> {
+  using RealScalar = typename NumTraits<Scalar>::Real;
+  enum { Cost = functor_traits<scalar_atan2_op<RealScalar>>::Cost, PacketAccess = packet_traits<RealScalar>::HasATan };
+};
+
+/** \internal
+ * \brief Template functor to cast a scalar to another type
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::cast()
+ */
+template <typename Scalar, typename NewType>
+struct scalar_cast_op {
+  typedef NewType result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const NewType operator()(const Scalar& a) const {
+    return cast<Scalar, NewType>(a);
+  }
+};
+
+template <typename Scalar, typename NewType>
+struct functor_traits<scalar_cast_op<Scalar, NewType>> {
+  enum { Cost = is_same<Scalar, NewType>::value ? 0 : NumTraits<NewType>::AddCost, PacketAccess = false };
+};
+
+/** \internal
+ * `core_cast_op` serves to distinguish the vectorized implementation from that of the legacy `scalar_cast_op` for
+ * backwards compatibility. The manner in which packet ops are handled is defined by the specialized unary_evaluator:
+ * `unary_evaluator<CwiseUnaryOp<core_cast_op<SrcType, DstType>, ArgType>, IndexBased>` in CoreEvaluators.h
+ * Otherwise, the non-vectorized behavior is identical to that of `scalar_cast_op`
+ */
+template <typename SrcType, typename DstType>
+struct core_cast_op : scalar_cast_op<SrcType, DstType> {};
+
+template <typename SrcType, typename DstType>
+struct functor_traits<core_cast_op<SrcType, DstType>> {
+  using CastingTraits = type_casting_traits<SrcType, DstType>;
+  enum {
+    Cost = is_same<SrcType, DstType>::value ? 0 : NumTraits<DstType>::AddCost,
+    PacketAccess = CastingTraits::VectorizedCast && (CastingTraits::SrcCoeffRatio <= 8)
+  };
+};
+
+/** \internal
+ * \brief Template functor to arithmetically shift a scalar right by a number of bits
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::shift_right()
+ */
+template <typename Scalar, int N>
+struct scalar_shift_right_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return a >> N; }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return internal::parithmetic_shift_right<N>(a);
+  }
+};
+template <typename Scalar, int N>
+struct functor_traits<scalar_shift_right_op<Scalar, N>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasShift };
+};
+
+/** \internal
+ * \brief Template functor to logically shift a scalar left by a number of bits
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::shift_left()
+ */
+template <typename Scalar, int N>
+struct scalar_shift_left_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return a << N; }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const {
+    return internal::plogical_shift_left<N>(a);
+  }
+};
+template <typename Scalar, int N>
+struct functor_traits<scalar_shift_left_op<Scalar, N>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasShift };
+};
+
+/** \internal
+ * \brief Template functor to extract the real part of a complex
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::real()
+ */
+template <typename Scalar>
+struct scalar_real_op {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const Scalar& a) const { return numext::real(a); }
+};
+template <typename Scalar>
+struct functor_traits<scalar_real_op<Scalar>> {
+  enum { Cost = 0, PacketAccess = false };
+};
+
+/** \internal
+ * \brief Template functor to extract the imaginary part of a complex
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::imag()
+ */
+template <typename Scalar>
+struct scalar_imag_op {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const Scalar& a) const { return numext::imag(a); }
+};
+template <typename Scalar>
+struct functor_traits<scalar_imag_op<Scalar>> {
+  enum { Cost = 0, PacketAccess = false };
+};
+
+/** \internal
+ * \brief Template functor to extract the real part of a complex as a reference
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::real()
+ */
+template <typename Scalar>
+struct scalar_real_ref_op {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type& operator()(const Scalar& a) const {
+    return numext::real_ref(*const_cast<Scalar*>(&a));
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_real_ref_op<Scalar>> {
+  enum { Cost = 0, PacketAccess = false };
+};
+
+/** \internal
+ * \brief Template functor to extract the imaginary part of a complex as a reference
+ *
+ * \sa class CwiseUnaryOp, MatrixBase::imag()
+ */
+template <typename Scalar>
+struct scalar_imag_ref_op {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type& operator()(const Scalar& a) const {
+    return numext::imag_ref(*const_cast<Scalar*>(&a));
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_imag_ref_op<Scalar>> {
+  enum { Cost = 0, PacketAccess = false };
+};
+
+/** \internal
+ *
+ * \brief Template functor to compute the exponential of a scalar
+ *
+ * \sa class CwiseUnaryOp, Cwise::exp()
+ */
+template <typename Scalar>
+struct scalar_exp_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return internal::pexp(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pexp(a);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_exp_op<Scalar>> {
   enum {
     PacketAccess = packet_traits<Scalar>::HasExp,
-    // The following numbers are based on the AVX implementation.
+  // The following numbers are based on the AVX implementation.
 #ifdef EIGEN_VECTORIZE_FMA
     // Haswell can issue 2 add/mul/madd per cycle.
-    Cost =
-    (sizeof(Scalar) == 4
-     // float: 8 pmadd, 4 pmul, 2 padd/psub, 6 other
-     ? (8 * NumTraits<Scalar>::AddCost + 6 * NumTraits<Scalar>::MulCost)
-     // double: 7 pmadd, 5 pmul, 3 padd/psub, 1 div,  13 other
-     : (14 * NumTraits<Scalar>::AddCost +
-        6 * NumTraits<Scalar>::MulCost +
-        scalar_div_cost<Scalar,packet_traits<Scalar>::HasDiv>::value))
+    Cost = (sizeof(Scalar) == 4
+                // float: 8 pmadd, 4 pmul, 2 padd/psub, 6 other
+                ? (8 * NumTraits<Scalar>::AddCost + 6 * NumTraits<Scalar>::MulCost)
+                // double: 7 pmadd, 5 pmul, 3 padd/psub, 1 div,  13 other
+                : (14 * NumTraits<Scalar>::AddCost + 6 * NumTraits<Scalar>::MulCost +
+                   scalar_div_cost<Scalar, packet_traits<Scalar>::HasDiv>::value))
 #else
-    Cost =
-    (sizeof(Scalar) == 4
-     // float: 7 pmadd, 6 pmul, 4 padd/psub, 10 other
-     ? (21 * NumTraits<Scalar>::AddCost + 13 * NumTraits<Scalar>::MulCost)
-     // double: 7 pmadd, 5 pmul, 3 padd/psub, 1 div,  13 other
-     : (23 * NumTraits<Scalar>::AddCost +
-        12 * NumTraits<Scalar>::MulCost +
-        scalar_div_cost<Scalar,packet_traits<Scalar>::HasDiv>::value))
+    Cost = (sizeof(Scalar) == 4
+                // float: 7 pmadd, 6 pmul, 4 padd/psub, 10 other
+                ? (21 * NumTraits<Scalar>::AddCost + 13 * NumTraits<Scalar>::MulCost)
+                // double: 7 pmadd, 5 pmul, 3 padd/psub, 1 div,  13 other
+                : (23 * NumTraits<Scalar>::AddCost + 12 * NumTraits<Scalar>::MulCost +
+                   scalar_div_cost<Scalar, packet_traits<Scalar>::HasDiv>::value))
 #endif
   };
 };
 
 /** \internal
-  *
-  * \brief Template functor to compute the exponential of a scalar - 1.
-  *
-  * \sa class CwiseUnaryOp, ArrayBase::expm1()
-  */
-template<typename Scalar> struct scalar_expm1_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_expm1_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::expm1(a); }
+ *
+ * \brief Template functor to compute the exponential of a scalar - 1.
+ *
+ * \sa class CwiseUnaryOp, ArrayBase::expm1()
+ */
+template <typename Scalar>
+struct scalar_expm1_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::expm1(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pexpm1(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pexpm1(a);
+  }
 };
 template <typename Scalar>
-struct functor_traits<scalar_expm1_op<Scalar> > {
+struct functor_traits<scalar_expm1_op<Scalar>> {
   enum {
     PacketAccess = packet_traits<Scalar>::HasExpm1,
-    Cost = functor_traits<scalar_exp_op<Scalar> >::Cost // TODO measure cost of expm1
+    Cost = functor_traits<scalar_exp_op<Scalar>>::Cost  // TODO measure cost of expm1
   };
 };
 
 /** \internal
-  *
-  * \brief Template functor to compute the logarithm of a scalar
-  *
-  * \sa class CwiseUnaryOp, ArrayBase::log()
-  */
-template<typename Scalar> struct scalar_log_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_log_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::log(a); }
+ *
+ * \brief Template functor to compute the logarithm of a scalar
+ *
+ * \sa class CwiseUnaryOp, ArrayBase::log()
+ */
+template <typename Scalar>
+struct scalar_log_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::log(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::plog(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::plog(a);
+  }
 };
 template <typename Scalar>
-struct functor_traits<scalar_log_op<Scalar> > {
+struct functor_traits<scalar_log_op<Scalar>> {
   enum {
     PacketAccess = packet_traits<Scalar>::HasLog,
-    Cost =
-    (PacketAccess
-     // The following numbers are based on the AVX implementation.
+    Cost = (PacketAccess
+  // The following numbers are based on the AVX implementation.
 #ifdef EIGEN_VECTORIZE_FMA
-     // 8 pmadd, 6 pmul, 8 padd/psub, 16 other, can issue 2 add/mul/madd per cycle.
-     ? (20 * NumTraits<Scalar>::AddCost + 7 * NumTraits<Scalar>::MulCost)
+                // 8 pmadd, 6 pmul, 8 padd/psub, 16 other, can issue 2 add/mul/madd per cycle.
+                ? (20 * NumTraits<Scalar>::AddCost + 7 * NumTraits<Scalar>::MulCost)
 #else
-     // 8 pmadd, 6 pmul, 8 padd/psub, 20 other
-     ? (36 * NumTraits<Scalar>::AddCost + 14 * NumTraits<Scalar>::MulCost)
+                // 8 pmadd, 6 pmul, 8 padd/psub, 20 other
+                ? (36 * NumTraits<Scalar>::AddCost + 14 * NumTraits<Scalar>::MulCost)
 #endif
-     // Measured cost of std::log.
-     : sizeof(Scalar)==4 ? 40 : 85)
+                // Measured cost of std::log.
+                : sizeof(Scalar) == 4 ? 40 : 85)
   };
 };
 
 /** \internal
-  *
-  * \brief Template functor to compute the logarithm of 1 plus a scalar value
-  *
-  * \sa class CwiseUnaryOp, ArrayBase::log1p()
-  */
-template<typename Scalar> struct scalar_log1p_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_log1p_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::log1p(a); }
+ *
+ * \brief Template functor to compute the logarithm of 1 plus a scalar value
+ *
+ * \sa class CwiseUnaryOp, ArrayBase::log1p()
+ */
+template <typename Scalar>
+struct scalar_log1p_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::log1p(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::plog1p(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::plog1p(a);
+  }
 };
 template <typename Scalar>
-struct functor_traits<scalar_log1p_op<Scalar> > {
+struct functor_traits<scalar_log1p_op<Scalar>> {
   enum {
     PacketAccess = packet_traits<Scalar>::HasLog1p,
-    Cost = functor_traits<scalar_log_op<Scalar> >::Cost // TODO measure cost of log1p
+    Cost = functor_traits<scalar_log_op<Scalar>>::Cost  // TODO measure cost of log1p
   };
 };
 
 /** \internal
-  *
-  * \brief Template functor to compute the base-10 logarithm of a scalar
-  *
-  * \sa class CwiseUnaryOp, Cwise::log10()
-  */
-template<typename Scalar> struct scalar_log10_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_log10_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { EIGEN_USING_STD(log10) return log10(a); }
+ *
+ * \brief Template functor to compute the base-10 logarithm of a scalar
+ *
+ * \sa class CwiseUnaryOp, Cwise::log10()
+ */
+template <typename Scalar>
+struct scalar_log10_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { EIGEN_USING_STD(log10) return log10(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::plog10(a); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_log10_op<Scalar> >
-{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasLog10 }; };
-
-/** \internal
-  *
-  * \brief Template functor to compute the base-2 logarithm of a scalar
-  *
-  * \sa class CwiseUnaryOp, Cwise::log2()
-  */
-template<typename Scalar> struct scalar_log2_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_log2_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return Scalar(EIGEN_LOG2E) * numext::log(a); }
-  template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::plog2(a); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_log2_op<Scalar> >
-{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasLog }; };
-
-/** \internal
-  * \brief Template functor to compute the square root of a scalar
-  * \sa class CwiseUnaryOp, Cwise::sqrt()
-  */
-template<typename Scalar> struct scalar_sqrt_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sqrt_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::sqrt(a); }
-  template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psqrt(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::plog10(a);
+  }
 };
 template <typename Scalar>
-struct functor_traits<scalar_sqrt_op<Scalar> > {
+struct functor_traits<scalar_log10_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasLog10 };
+};
+
+/** \internal
+ *
+ * \brief Template functor to compute the base-2 logarithm of a scalar
+ *
+ * \sa class CwiseUnaryOp, Cwise::log2()
+ */
+template <typename Scalar>
+struct scalar_log2_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const {
+    return Scalar(EIGEN_LOG2E) * numext::log(a);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::plog2(a);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_log2_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasLog };
+};
+
+/** \internal
+ * \brief Template functor to compute the square root of a scalar
+ * \sa class CwiseUnaryOp, Cwise::sqrt()
+ */
+template <typename Scalar>
+struct scalar_sqrt_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::sqrt(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::psqrt(a);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_sqrt_op<Scalar>> {
   enum {
 #if EIGEN_FAST_MATH
     // The following numbers are based on the AVX implementation.
     Cost = (sizeof(Scalar) == 8 ? 28
                                 // 4 pmul, 1 pmadd, 3 other
-                                : (3 * NumTraits<Scalar>::AddCost +
-                                   5 * NumTraits<Scalar>::MulCost)),
+                                : (3 * NumTraits<Scalar>::AddCost + 5 * NumTraits<Scalar>::MulCost)),
 #else
     // The following numbers are based on min VSQRT throughput on Haswell.
     Cost = (sizeof(Scalar) == 8 ? 28 : 14),
@@ -439,464 +495,477 @@
 };
 
 // Boolean specialization to eliminate -Wimplicit-conversion-floating-point-to-bool warnings.
-template<> struct scalar_sqrt_op<bool> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sqrt_op)
-  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator() (const bool& a) const { return a; }
+template <>
+struct scalar_sqrt_op<bool> {
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator()(const bool& a) const { return a; }
   template <typename Packet>
-  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return a; }
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return a;
+  }
 };
 template <>
-struct functor_traits<scalar_sqrt_op<bool> > {
+struct functor_traits<scalar_sqrt_op<bool>> {
   enum { Cost = 1, PacketAccess = packet_traits<bool>::Vectorizable };
 };
 
 /** \internal
-  * \brief Template functor to compute the reciprocal square root of a scalar
-  * \sa class CwiseUnaryOp, Cwise::rsqrt()
-  */
-template<typename Scalar> struct scalar_rsqrt_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_rsqrt_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::rsqrt(a); }
-  template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::prsqrt(a); }
+ * \brief Template functor to compute the cube root of a scalar
+ * \sa class CwiseUnaryOp, Cwise::sqrt()
+ */
+template <typename Scalar>
+struct scalar_cbrt_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::cbrt(a); }
 };
 
-template<typename Scalar>
-struct functor_traits<scalar_rsqrt_op<Scalar> >
-{ enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasRsqrt
-  };
+template <typename Scalar>
+struct functor_traits<scalar_cbrt_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
 };
 
 /** \internal
-  * \brief Template functor to compute the cosine of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::cos()
-  */
-template<typename Scalar> struct scalar_cos_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cos_op)
-  EIGEN_DEVICE_FUNC inline Scalar operator() (const Scalar& a) const { return numext::cos(a); }
+ * \brief Template functor to compute the reciprocal square root of a scalar
+ * \sa class CwiseUnaryOp, Cwise::rsqrt()
+ */
+template <typename Scalar>
+struct scalar_rsqrt_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::rsqrt(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pcos(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::prsqrt(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_cos_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasCos
-  };
+
+template <typename Scalar>
+struct functor_traits<scalar_rsqrt_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasRsqrt };
 };
 
 /** \internal
-  * \brief Template functor to compute the sine of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::sin()
-  */
-template<typename Scalar> struct scalar_sin_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sin_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::sin(a); }
+ * \brief Template functor to compute the cosine of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::cos()
+ */
+template <typename Scalar>
+struct scalar_cos_op {
+  EIGEN_DEVICE_FUNC inline Scalar operator()(const Scalar& a) const { return numext::cos(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psin(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pcos(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_sin_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasSin
-  };
-};
-
-
-/** \internal
-  * \brief Template functor to compute the tan of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::tan()
-  */
-template<typename Scalar> struct scalar_tan_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_tan_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::tan(a); }
-  template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::ptan(a); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_tan_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasTan
-  };
+template <typename Scalar>
+struct functor_traits<scalar_cos_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasCos };
 };
 
 /** \internal
-  * \brief Template functor to compute the arc cosine of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::acos()
-  */
-template<typename Scalar> struct scalar_acos_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_acos_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::acos(a); }
+ * \brief Template functor to compute the sine of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::sin()
+ */
+template <typename Scalar>
+struct scalar_sin_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::sin(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pacos(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::psin(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_acos_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasACos
-  };
+template <typename Scalar>
+struct functor_traits<scalar_sin_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasSin };
 };
 
 /** \internal
-  * \brief Template functor to compute the arc sine of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::asin()
-  */
-template<typename Scalar> struct scalar_asin_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_asin_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::asin(a); }
+ * \brief Template functor to compute the tan of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::tan()
+ */
+template <typename Scalar>
+struct scalar_tan_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::tan(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pasin(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::ptan(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_asin_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasASin
-  };
-};
-
-
-/** \internal
-  * \brief Template functor to compute the atan of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::atan()
-  */
-template<typename Scalar> struct scalar_atan_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_atan_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::atan(a); }
-  template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::patan(a); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_atan_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasATan
-  };
+template <typename Scalar>
+struct functor_traits<scalar_tan_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasTan };
 };
 
 /** \internal
-  * \brief Template functor to compute the tanh of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::tanh()
-  */
+ * \brief Template functor to compute the arc cosine of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::acos()
+ */
+template <typename Scalar>
+struct scalar_acos_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::acos(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pacos(a);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_acos_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasACos };
+};
+
+/** \internal
+ * \brief Template functor to compute the arc sine of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::asin()
+ */
+template <typename Scalar>
+struct scalar_asin_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::asin(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pasin(a);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_asin_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasASin };
+};
+
+/** \internal
+ * \brief Template functor to compute the atan of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::atan()
+ */
+template <typename Scalar>
+struct scalar_atan_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::atan(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::patan(a);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_atan_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasATan };
+};
+
+/** \internal
+ * \brief Template functor to compute the tanh of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::tanh()
+ */
 template <typename Scalar>
 struct scalar_tanh_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_tanh_op)
   EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::tanh(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& x) const { return ptanh(x); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& x) const {
+    return ptanh(x);
+  }
 };
 
 template <typename Scalar>
-struct functor_traits<scalar_tanh_op<Scalar> > {
+struct functor_traits<scalar_tanh_op<Scalar>> {
   enum {
     PacketAccess = packet_traits<Scalar>::HasTanh,
-    Cost = ( (EIGEN_FAST_MATH && is_same<Scalar,float>::value)
+    Cost = ((EIGEN_FAST_MATH && is_same<Scalar, float>::value)
 // The following numbers are based on the AVX implementation,
 #ifdef EIGEN_VECTORIZE_FMA
                 // Haswell can issue 2 add/mul/madd per cycle.
                 // 9 pmadd, 2 pmul, 1 div, 2 other
-                ? (2 * NumTraits<Scalar>::AddCost +
-                   6 * NumTraits<Scalar>::MulCost +
-                   scalar_div_cost<Scalar,packet_traits<Scalar>::HasDiv>::value)
+                ? (2 * NumTraits<Scalar>::AddCost + 6 * NumTraits<Scalar>::MulCost +
+                   scalar_div_cost<Scalar, packet_traits<Scalar>::HasDiv>::value)
 #else
-                ? (11 * NumTraits<Scalar>::AddCost +
-                   11 * NumTraits<Scalar>::MulCost +
-                   scalar_div_cost<Scalar,packet_traits<Scalar>::HasDiv>::value)
+                ? (11 * NumTraits<Scalar>::AddCost + 11 * NumTraits<Scalar>::MulCost +
+                   scalar_div_cost<Scalar, packet_traits<Scalar>::HasDiv>::value)
 #endif
                 // This number assumes a naive implementation of tanh
-                : (6 * NumTraits<Scalar>::AddCost +
-                   3 * NumTraits<Scalar>::MulCost +
-                   2 * scalar_div_cost<Scalar,packet_traits<Scalar>::HasDiv>::value +
-                   functor_traits<scalar_exp_op<Scalar> >::Cost))
+                : (6 * NumTraits<Scalar>::AddCost + 3 * NumTraits<Scalar>::MulCost +
+                   2 * scalar_div_cost<Scalar, packet_traits<Scalar>::HasDiv>::value +
+                   functor_traits<scalar_exp_op<Scalar>>::Cost))
   };
 };
 
-#if EIGEN_HAS_CXX11_MATH
 /** \internal
-  * \brief Template functor to compute the atanh of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::atanh()
-  */
+ * \brief Template functor to compute the atanh of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::atanh()
+ */
 template <typename Scalar>
 struct scalar_atanh_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_atanh_op)
   EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::atanh(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& x) const {
+    return patanh(x);
+  }
 };
 
 template <typename Scalar>
-struct functor_traits<scalar_atanh_op<Scalar> > {
-  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
+struct functor_traits<scalar_atanh_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasATanh };
 };
-#endif
 
 /** \internal
-  * \brief Template functor to compute the sinh of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::sinh()
-  */
-template<typename Scalar> struct scalar_sinh_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sinh_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::sinh(a); }
+ * \brief Template functor to compute the sinh of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::sinh()
+ */
+template <typename Scalar>
+struct scalar_sinh_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::sinh(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psinh(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::psinh(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_sinh_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasSinh
-  };
+template <typename Scalar>
+struct functor_traits<scalar_sinh_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasSinh };
 };
 
-#if EIGEN_HAS_CXX11_MATH
 /** \internal
-  * \brief Template functor to compute the asinh of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::asinh()
-  */
+ * \brief Template functor to compute the asinh of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::asinh()
+ */
 template <typename Scalar>
 struct scalar_asinh_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_asinh_op)
   EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::asinh(a); }
 };
 
 template <typename Scalar>
-struct functor_traits<scalar_asinh_op<Scalar> > {
+struct functor_traits<scalar_asinh_op<Scalar>> {
   enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
 };
-#endif
 
 /** \internal
-  * \brief Template functor to compute the cosh of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::cosh()
-  */
-template<typename Scalar> struct scalar_cosh_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cosh_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::cosh(a); }
+ * \brief Template functor to compute the cosh of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::cosh()
+ */
+template <typename Scalar>
+struct scalar_cosh_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::cosh(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pcosh(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pcosh(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_cosh_op<Scalar> >
-{
-  enum {
-    Cost = 5 * NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasCosh
-  };
+template <typename Scalar>
+struct functor_traits<scalar_cosh_op<Scalar>> {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasCosh };
 };
 
-#if EIGEN_HAS_CXX11_MATH
 /** \internal
-  * \brief Template functor to compute the acosh of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::acosh()
-  */
+ * \brief Template functor to compute the acosh of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::acosh()
+ */
 template <typename Scalar>
 struct scalar_acosh_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_acosh_op)
   EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::acosh(a); }
 };
 
 template <typename Scalar>
-struct functor_traits<scalar_acosh_op<Scalar> > {
+struct functor_traits<scalar_acosh_op<Scalar>> {
   enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
 };
-#endif
 
 /** \internal
-  * \brief Template functor to compute the inverse of a scalar
-  * \sa class CwiseUnaryOp, Cwise::inverse()
-  */
-template<typename Scalar>
+ * \brief Template functor to compute the inverse of a scalar
+ * \sa class CwiseUnaryOp, Cwise::inverse()
+ */
+template <typename Scalar>
 struct scalar_inverse_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_inverse_op)
-  EIGEN_DEVICE_FUNC inline Scalar operator() (const Scalar& a) const { return Scalar(1)/a; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
-  { return internal::pdiv(pset1<Packet>(Scalar(1)),a); }
+  EIGEN_DEVICE_FUNC inline Scalar operator()(const Scalar& a) const { return Scalar(1) / a; }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const {
+    return internal::preciprocal(a);
+  }
 };
 template <typename Scalar>
-struct functor_traits<scalar_inverse_op<Scalar> > {
+struct functor_traits<scalar_inverse_op<Scalar>> {
   enum {
     PacketAccess = packet_traits<Scalar>::HasDiv,
-    Cost = scalar_div_cost<Scalar, PacketAccess>::value
+    // If packet_traits<Scalar>::HasReciprocal then the Estimated cost is that
+    // of computing an approximation plus a single Newton-Raphson step, which
+    // consists of 1 pmul + 1 pmadd.
+    Cost = (packet_traits<Scalar>::HasReciprocal ? 4 * NumTraits<Scalar>::MulCost
+                                                 : scalar_div_cost<Scalar, PacketAccess>::value)
   };
 };
 
 /** \internal
-  * \brief Template functor to compute the square of a scalar
-  * \sa class CwiseUnaryOp, Cwise::square()
-  */
-template<typename Scalar>
+ * \brief Template functor to compute the square of a scalar
+ * \sa class CwiseUnaryOp, Cwise::square()
+ */
+template <typename Scalar>
 struct scalar_square_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_square_op)
-  EIGEN_DEVICE_FUNC inline Scalar operator() (const Scalar& a) const { return a*a; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
-  { return internal::pmul(a,a); }
+  EIGEN_DEVICE_FUNC inline Scalar operator()(const Scalar& a) const { return a * a; }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const {
+    return internal::pmul(a, a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_square_op<Scalar> >
-{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
+template <typename Scalar>
+struct functor_traits<scalar_square_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul };
+};
 
 // Boolean specialization to avoid -Wint-in-bool-context warnings on GCC.
-template<>
+template <>
 struct scalar_square_op<bool> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_square_op)
-  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator() (const bool& a) const { return a; }
-  template<typename Packet>
-  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
-  { return a; }
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator()(const bool& a) const { return a; }
+  template <typename Packet>
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const {
+    return a;
+  }
 };
-template<>
-struct functor_traits<scalar_square_op<bool> >
-{ enum { Cost = 0, PacketAccess = packet_traits<bool>::Vectorizable }; };
+template <>
+struct functor_traits<scalar_square_op<bool>> {
+  enum { Cost = 0, PacketAccess = packet_traits<bool>::Vectorizable };
+};
 
 /** \internal
-  * \brief Template functor to compute the cube of a scalar
-  * \sa class CwiseUnaryOp, Cwise::cube()
-  */
-template<typename Scalar>
+ * \brief Template functor to compute the cube of a scalar
+ * \sa class CwiseUnaryOp, Cwise::cube()
+ */
+template <typename Scalar>
 struct scalar_cube_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cube_op)
-  EIGEN_DEVICE_FUNC inline Scalar operator() (const Scalar& a) const { return a*a*a; }
-  template<typename Packet>
-  EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
-  { return internal::pmul(a,pmul(a,a)); }
+  EIGEN_DEVICE_FUNC inline Scalar operator()(const Scalar& a) const { return a * a * a; }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const {
+    return internal::pmul(a, pmul(a, a));
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_cube_op<Scalar> >
-{ enum { Cost = 2*NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
+template <typename Scalar>
+struct functor_traits<scalar_cube_op<Scalar>> {
+  enum { Cost = 2 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul };
+};
 
 // Boolean specialization to avoid -Wint-in-bool-context warnings on GCC.
-template<>
+template <>
 struct scalar_cube_op<bool> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_cube_op)
-  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator() (const bool& a) const { return a; }
-  template<typename Packet>
-  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
-  { return a; }
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator()(const bool& a) const { return a; }
+  template <typename Packet>
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const {
+    return a;
+  }
 };
-template<>
-struct functor_traits<scalar_cube_op<bool> >
-{ enum { Cost = 0, PacketAccess = packet_traits<bool>::Vectorizable }; };
+template <>
+struct functor_traits<scalar_cube_op<bool>> {
+  enum { Cost = 0, PacketAccess = packet_traits<bool>::Vectorizable };
+};
 
 /** \internal
-  * \brief Template functor to compute the rounded value of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::round()
-  */
-template<typename Scalar> struct scalar_round_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_round_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return numext::round(a); }
+ * \brief Template functor to compute the rounded value of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::round()
+ */
+template <typename Scalar>
+struct scalar_round_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return numext::round(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pround(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pround(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_round_op<Scalar> >
-{
+template <typename Scalar>
+struct functor_traits<scalar_round_op<Scalar>> {
   enum {
     Cost = NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasRound
+    PacketAccess = packet_traits<Scalar>::HasRound || NumTraits<Scalar>::IsInteger
   };
 };
 
 /** \internal
-  * \brief Template functor to compute the floor of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::floor()
-  */
-template<typename Scalar> struct scalar_floor_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_floor_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return numext::floor(a); }
+ * \brief Template functor to compute the floor of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::floor()
+ */
+template <typename Scalar>
+struct scalar_floor_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return numext::floor(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pfloor(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pfloor(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_floor_op<Scalar> >
-{
+template <typename Scalar>
+struct functor_traits<scalar_floor_op<Scalar>> {
   enum {
     Cost = NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasFloor
+    PacketAccess = packet_traits<Scalar>::HasFloor || NumTraits<Scalar>::IsInteger
   };
 };
 
 /** \internal
-  * \brief Template functor to compute the rounded (with current rounding mode)  value of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::rint()
-  */
-template<typename Scalar> struct scalar_rint_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_rint_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return numext::rint(a); }
+ * \brief Template functor to compute the rounded (with current rounding mode)  value of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::rint()
+ */
+template <typename Scalar>
+struct scalar_rint_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return numext::rint(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::print(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::print(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_rint_op<Scalar> >
-{
+template <typename Scalar>
+struct functor_traits<scalar_rint_op<Scalar>> {
   enum {
     Cost = NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasRint
+    PacketAccess = packet_traits<Scalar>::HasRint || NumTraits<Scalar>::IsInteger
   };
 };
 
 /** \internal
-  * \brief Template functor to compute the ceil of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::ceil()
-  */
-template<typename Scalar> struct scalar_ceil_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_ceil_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return numext::ceil(a); }
+ * \brief Template functor to compute the ceil of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::ceil()
+ */
+template <typename Scalar>
+struct scalar_ceil_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()(const Scalar& a) const { return numext::ceil(a); }
   template <typename Packet>
-  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pceil(a); }
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::pceil(a);
+  }
 };
-template<typename Scalar>
-struct functor_traits<scalar_ceil_op<Scalar> >
-{
+template <typename Scalar>
+struct functor_traits<scalar_ceil_op<Scalar>> {
   enum {
     Cost = NumTraits<Scalar>::MulCost,
-    PacketAccess = packet_traits<Scalar>::HasCeil
+    PacketAccess = packet_traits<Scalar>::HasCeil || NumTraits<Scalar>::IsInteger
   };
 };
 
 /** \internal
-  * \brief Template functor to compute whether a scalar is NaN
-  * \sa class CwiseUnaryOp, ArrayBase::isnan()
-  */
-template<typename Scalar> struct scalar_isnan_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_isnan_op)
-  typedef bool result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const {
+ * \brief Template functor to compute whether a scalar is NaN
+ * \sa class CwiseUnaryOp, ArrayBase::isnan()
+ */
+template <typename Scalar, bool UseTypedPredicate = false>
+struct scalar_isnan_op {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const Scalar& a) const {
 #if defined(SYCL_DEVICE_ONLY)
     return numext::isnan(a);
 #else
-    return (numext::isnan)(a);
+    return numext::isnan EIGEN_NOT_A_MACRO(a);
 #endif
   }
 };
-template<typename Scalar>
-struct functor_traits<scalar_isnan_op<Scalar> >
-{
-  enum {
-    Cost = NumTraits<Scalar>::MulCost,
-    PacketAccess = false
-  };
+
+template <typename Scalar>
+struct scalar_isnan_op<Scalar, true> {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a) const {
+#if defined(SYCL_DEVICE_ONLY)
+    return (numext::isnan(a) ? ptrue(a) : pzero(a));
+#else
+    return (numext::isnan EIGEN_NOT_A_MACRO(a) ? ptrue(a) : pzero(a));
+#endif
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return pisnan(a);
+  }
+};
+
+template <typename Scalar, bool UseTypedPredicate>
+struct functor_traits<scalar_isnan_op<Scalar, UseTypedPredicate>> {
+  enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasCmp && UseTypedPredicate };
 };
 
 /** \internal
-  * \brief Template functor to check whether a scalar is +/-inf
-  * \sa class CwiseUnaryOp, ArrayBase::isinf()
-  */
-template<typename Scalar> struct scalar_isinf_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_isinf_op)
+ * \brief Template functor to check whether a scalar is +/-inf
+ * \sa class CwiseUnaryOp, ArrayBase::isinf()
+ */
+template <typename Scalar>
+struct scalar_isinf_op {
   typedef bool result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const Scalar& a) const {
 #if defined(SYCL_DEVICE_ONLY)
     return numext::isinf(a);
 #else
@@ -904,23 +973,19 @@
 #endif
   }
 };
-template<typename Scalar>
-struct functor_traits<scalar_isinf_op<Scalar> >
-{
-  enum {
-    Cost = NumTraits<Scalar>::MulCost,
-    PacketAccess = false
-  };
+template <typename Scalar>
+struct functor_traits<scalar_isinf_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = false };
 };
 
 /** \internal
-  * \brief Template functor to check whether a scalar has a finite value
-  * \sa class CwiseUnaryOp, ArrayBase::isfinite()
-  */
-template<typename Scalar> struct scalar_isfinite_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_isfinite_op)
+ * \brief Template functor to check whether a scalar has a finite value
+ * \sa class CwiseUnaryOp, ArrayBase::isfinite()
+ */
+template <typename Scalar>
+struct scalar_isfinite_op {
   typedef bool result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const Scalar& a) const {
 #if defined(SYCL_DEVICE_ONLY)
     return numext::isfinite(a);
 #else
@@ -928,204 +993,319 @@
 #endif
   }
 };
-template<typename Scalar>
-struct functor_traits<scalar_isfinite_op<Scalar> >
-{
+template <typename Scalar>
+struct functor_traits<scalar_isfinite_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = false };
+};
+
+/** \internal
+ * \brief Template functor to compute the logical not of a scalar as if it were a boolean
+ *
+ * \sa class CwiseUnaryOp, ArrayBase::operator!
+ */
+template <typename Scalar>
+struct scalar_boolean_not_op {
+  using result_type = Scalar;
+  // `false` any value `a` that satisfies `a == Scalar(0)`
+  // `true` is the complement of `false`
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a) const {
+    return a == Scalar(0) ? Scalar(1) : Scalar(0);
+  }
+  template <typename Packet>
+  EIGEN_STRONG_INLINE Packet packetOp(const Packet& a) const {
+    const Packet cst_one = pset1<Packet>(Scalar(1));
+    Packet not_a = pcmp_eq(a, pzero(a));
+    return pand(not_a, cst_one);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_boolean_not_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasCmp };
+};
+
+template <typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct bitwise_unary_impl {
+  static constexpr size_t Size = sizeof(Scalar);
+  using uint_t = typename numext::get_integer_by_size<Size>::unsigned_type;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_not(const Scalar& a) {
+    uint_t a_as_uint = numext::bit_cast<uint_t, Scalar>(a);
+    uint_t result = ~a_as_uint;
+    return numext::bit_cast<Scalar, uint_t>(result);
+  }
+};
+
+template <typename Scalar>
+struct bitwise_unary_impl<Scalar, true> {
+  using Real = typename NumTraits<Scalar>::Real;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_not(const Scalar& a) {
+    Real real_result = bitwise_unary_impl<Real>::run_not(numext::real(a));
+    Real imag_result = bitwise_unary_impl<Real>::run_not(numext::imag(a));
+    return Scalar(real_result, imag_result);
+  }
+};
+
+/** \internal
+ * \brief Template functor to compute the bitwise not of a scalar
+ *
+ * \sa class CwiseUnaryOp, ArrayBase::operator~
+ */
+template <typename Scalar>
+struct scalar_bitwise_not_op {
+  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::RequireInitialization,
+                      BITWISE OPERATIONS MAY ONLY BE PERFORMED ON PLAIN DATA TYPES)
+  EIGEN_STATIC_ASSERT((!internal::is_same<Scalar, bool>::value), DONT USE BITWISE OPS ON BOOLEAN TYPES)
+  using result_type = Scalar;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a) const {
+    return bitwise_unary_impl<Scalar>::run_not(a);
+  }
+  template <typename Packet>
+  EIGEN_STRONG_INLINE Packet packetOp(const Packet& a) const {
+    return pandnot(ptrue(a), a);
+  }
+};
+template <typename Scalar>
+struct functor_traits<scalar_bitwise_not_op<Scalar>> {
+  enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = true };
+};
+
+/** \internal
+ * \brief Template functor to compute the signum of a scalar
+ * \sa class CwiseUnaryOp, Cwise::sign()
+ */
+template <typename Scalar>
+struct scalar_sign_op {
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::sign(a); }
+
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const {
+    return internal::psign(a);
+  }
+};
+
+template <typename Scalar>
+struct functor_traits<scalar_sign_op<Scalar>> {
   enum {
-    Cost = NumTraits<Scalar>::MulCost,
-    PacketAccess = false
+    Cost = NumTraits<Scalar>::IsComplex ? (8 * NumTraits<Scalar>::MulCost)  // roughly
+                                        : (3 * NumTraits<Scalar>::AddCost),
+    PacketAccess = packet_traits<Scalar>::HasSign && packet_traits<Scalar>::Vectorizable
   };
 };
 
 /** \internal
-  * \brief Template functor to compute the logical not of a boolean
-  *
-  * \sa class CwiseUnaryOp, ArrayBase::operator!
-  */
-template<typename Scalar> struct scalar_boolean_not_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_not_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator() (const bool& a) const { return !a; }
-};
-template<typename Scalar>
-struct functor_traits<scalar_boolean_not_op<Scalar> > {
-  enum {
-    Cost = NumTraits<bool>::AddCost,
-    PacketAccess = false
-  };
-};
-
-/** \internal
-  * \brief Template functor to compute the signum of a scalar
-  * \sa class CwiseUnaryOp, Cwise::sign()
-  */
-template<typename Scalar,bool is_complex=(NumTraits<Scalar>::IsComplex!=0), bool is_integer=(NumTraits<Scalar>::IsInteger!=0) > struct scalar_sign_op;
-template<typename Scalar>
-struct scalar_sign_op<Scalar, false, true> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sign_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const
-  {
-      return Scalar( (a>Scalar(0)) - (a<Scalar(0)) );
-  }
-  //TODO
-  //template <typename Packet>
-  //EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psign(a); }
-};
-
-template<typename Scalar>
-struct scalar_sign_op<Scalar, false, false> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sign_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const
-  {
-    return (numext::isnan)(a) ? a : Scalar( (a>Scalar(0)) - (a<Scalar(0)) );
-  }
-  //TODO
-  //template <typename Packet>
-  //EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psign(a); }
-};
-
-template<typename Scalar, bool is_integer>
-struct scalar_sign_op<Scalar,true, is_integer> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_sign_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const
-  {
-    typedef typename NumTraits<Scalar>::Real real_type;
-    real_type aa = numext::abs(a);
-    if (aa==real_type(0))
-      return Scalar(0);
-    aa = real_type(1)/aa;
-    return Scalar(a.real()*aa, a.imag()*aa );
-  }
-  //TODO
-  //template <typename Packet>
-  //EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psign(a); }
-};
-template<typename Scalar>
-struct functor_traits<scalar_sign_op<Scalar> >
-{ enum {
-    Cost =
-        NumTraits<Scalar>::IsComplex
-        ? ( 8*NumTraits<Scalar>::MulCost  ) // roughly
-        : ( 3*NumTraits<Scalar>::AddCost),
-    PacketAccess = packet_traits<Scalar>::HasSign
-  };
-};
-
-/** \internal
-  * \brief Template functor to compute the logistic function of a scalar
-  * \sa class CwiseUnaryOp, ArrayBase::logistic()
-  */
+ * \brief Template functor to compute the logistic function of a scalar
+ * \sa class CwiseUnaryOp, ArrayBase::logistic()
+ */
 template <typename T>
 struct scalar_logistic_op {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_logistic_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator()(const T& x) const {
-    return packetOp(x);
-  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator()(const T& x) const { return packetOp(x); }
 
-  template <typename Packet> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Packet packetOp(const Packet& x) const {
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& x) const {
     const Packet one = pset1<Packet>(T(1));
-    return pdiv(one, padd(one, pexp(pnegate(x))));
+    const Packet inf = pset1<Packet>(NumTraits<T>::infinity());
+    const Packet e = pexp(x);
+    const Packet inf_mask = pcmp_eq(e, inf);
+    return pselect(inf_mask, one, pdiv(e, padd(one, e)));
   }
 };
 
-#ifndef EIGEN_GPU_COMPILE_PHASE
+// TODO(rmlarsen): Enable the following on host when integer_packet is defined
+// for the relevant packet types.
+#ifdef EIGEN_GPU_CC
+
 /** \internal
-  * \brief Template specialization of the logistic function for float.
-  *
-  *  Uses just a 9/10-degree rational interpolant which
-  *  interpolates 1/(1+exp(-x)) - 0.5 up to a couple of ulps in the range
-  *  [-9, 18]. Below -9 we use the more accurate approximation
-  *  1/(1+exp(-x)) ~= exp(x), and above 18 the logistic function is 1 withing
-  *  one ulp. The shifted logistic is interpolated because it was easier to
-  *  make the fit converge.
-  *
-  */
+ * \brief Template specialization of the logistic function for float.
+ * Computes S(x) = exp(x) / (1 + exp(x)), where exp(x) is implemented
+ * using an algorithm partly adopted from the implementation of
+ * pexp_float. See the individual steps described in the code below.
+ * Note that compared to pexp, we use an additional outer multiplicative
+ * range reduction step using the identity exp(x) = exp(x/2)^2.
+ * This prevert us from having to call ldexp on values that could produce
+ * a denormal result, which allows us to call the faster implementation in
+ * pldexp_fast_impl<Packet>::run(p, m).
+ * The final squaring, however, doubles the error bound on the final
+ * approximation. Exhaustive testing shows that we have a worst case error
+ * of 4.5 ulps (compared to computing S(x) in double precision), which is
+ * acceptable.
+ */
 template <>
 struct scalar_logistic_op<float> {
-  EIGEN_EMPTY_STRUCT_CTOR(scalar_logistic_op)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator()(const float& x) const {
-    return packetOp(x);
+    // Truncate at the first point where the interpolant is exactly one.
+    const float cst_exp_hi = 16.6355324f;
+    const float e = numext::exp(numext::mini(x, cst_exp_hi));
+    return e / (1.0f + e);
   }
 
-  template <typename Packet> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  Packet packetOp(const Packet& _x) const {
-    const Packet cutoff_lower = pset1<Packet>(-9.f);
-    const Packet lt_mask = pcmp_lt<Packet>(_x, cutoff_lower);
-    const bool any_small = predux_any(lt_mask);
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& _x) const {
+    const Packet cst_zero = pset1<Packet>(0.0f);
+    const Packet cst_one = pset1<Packet>(1.0f);
+    const Packet cst_half = pset1<Packet>(0.5f);
+    // Truncate at the first point where the interpolant is exactly one.
+    const Packet cst_exp_hi = pset1<Packet>(16.6355324f);
+    const Packet cst_exp_lo = pset1<Packet>(-104.f);
 
-    // The upper cut-off is the smallest x for which the rational approximation evaluates to 1.
-    // Choosing this value saves us a few instructions clamping the results at the end.
-#ifdef EIGEN_VECTORIZE_FMA
-    const Packet cutoff_upper = pset1<Packet>(15.7243833541870117f);
-#else
-    const Packet cutoff_upper = pset1<Packet>(15.6437711715698242f);
-#endif
-    const Packet x = pmin(_x, cutoff_upper);
+    // Clamp x to the non-trivial range where S(x). Outside this
+    // interval the correctly rounded value of S(x) is either zero
+    // or one.
+    Packet zero_mask = pcmp_lt(_x, cst_exp_lo);
+    Packet x = pmin(_x, cst_exp_hi);
 
-    // The monomial coefficients of the numerator polynomial (odd).
-    const Packet alpha_1 = pset1<Packet>(2.48287947061529e-01f);
-    const Packet alpha_3 = pset1<Packet>(8.51377133304701e-03f);
-    const Packet alpha_5 = pset1<Packet>(6.08574864600143e-05f);
-    const Packet alpha_7 = pset1<Packet>(1.15627324459942e-07f);
-    const Packet alpha_9 = pset1<Packet>(4.37031012579801e-11f);
+    // 1. Multiplicative range reduction:
+    // Reduce the range of x by a factor of 2. This avoids having
+    // to compute exp(x) accurately where the result is a denormalized
+    // value.
+    x = pmul(x, cst_half);
 
-    // The monomial coefficients of the denominator polynomial (even).
-    const Packet beta_0 = pset1<Packet>(9.93151921023180e-01f);
-    const Packet beta_2 = pset1<Packet>(1.16817656904453e-01f);
-    const Packet beta_4 = pset1<Packet>(1.70198817374094e-03f);
-    const Packet beta_6 = pset1<Packet>(6.29106785017040e-06f);
-    const Packet beta_8 = pset1<Packet>(5.76102136993427e-09f);
-    const Packet beta_10 = pset1<Packet>(6.10247389755681e-13f);
+    // 2. Subtractive range reduction:
+    // Express exp(x) as exp(m*ln(2) + r) = 2^m*exp(r), start by extracting
+    // m = floor(x/ln(2) + 0.5), such that x = m*ln(2) + r.
+    const Packet cst_cephes_LOG2EF = pset1<Packet>(1.44269504088896341f);
+    Packet m = pfloor(pmadd(x, cst_cephes_LOG2EF, cst_half));
+    // Get r = x - m*ln(2). We use a trick from Cephes where the term
+    // m*ln(2) is subtracted out in two parts, m*C1+m*C2 = m*ln(2),
+    // to avoid accumulating truncation errors.
+    const Packet cst_cephes_exp_C1 = pset1<Packet>(-0.693359375f);
+    const Packet cst_cephes_exp_C2 = pset1<Packet>(2.12194440e-4f);
+    Packet r = pmadd(m, cst_cephes_exp_C1, x);
+    r = pmadd(m, cst_cephes_exp_C2, r);
 
-    // Since the polynomials are odd/even, we need x^2.
-    const Packet x2 = pmul(x, x);
+    // 3. Compute an approximation to exp(r) using a degree 5 minimax polynomial.
+    // We compute even and odd terms separately to increase instruction level
+    // parallelism.
+    Packet r2 = pmul(r, r);
+    const Packet cst_p2 = pset1<Packet>(0.49999141693115234375f);
+    const Packet cst_p3 = pset1<Packet>(0.16666877269744873046875f);
+    const Packet cst_p4 = pset1<Packet>(4.1898667812347412109375e-2f);
+    const Packet cst_p5 = pset1<Packet>(8.33471305668354034423828125e-3f);
 
-    // Evaluate the numerator polynomial p.
-    Packet p = pmadd(x2, alpha_9, alpha_7);
-    p = pmadd(x2, p, alpha_5);
-    p = pmadd(x2, p, alpha_3);
-    p = pmadd(x2, p, alpha_1);
-    p = pmul(x, p);
+    const Packet p_even = pmadd(r2, cst_p4, cst_p2);
+    const Packet p_odd = pmadd(r2, cst_p5, cst_p3);
+    const Packet p_low = padd(r, cst_one);
+    Packet p = pmadd(r, p_odd, p_even);
+    p = pmadd(r2, p, p_low);
 
-    // Evaluate the denominator polynomial q.
-    Packet q = pmadd(x2, beta_10, beta_8);
-    q = pmadd(x2, q, beta_6);
-    q = pmadd(x2, q, beta_4);
-    q = pmadd(x2, q, beta_2);
-    q = pmadd(x2, q, beta_0);
-    // Divide the numerator by the denominator and shift it up.
-    const Packet logistic = padd(pdiv(p, q), pset1<Packet>(0.5f));
-    if (EIGEN_PREDICT_FALSE(any_small)) {
-      const Packet exponential = pexp(_x);
-      return pselect(lt_mask, exponential, logistic);
-    } else {
-      return logistic;
-    }
+    // 4. Undo subtractive range reduction exp(m*ln(2) + r) = 2^m * exp(r).
+    Packet e = pldexp_fast_impl<Packet>::run(p, m);
+
+    // 5. Undo multiplicative range reduction by using exp(r) = exp(r/2)^2.
+    e = pmul(e, e);
+
+    // Return exp(x) / (1 + exp(x))
+    return pselect(zero_mask, cst_zero, pdiv(e, padd(cst_one, e)));
   }
 };
 #endif  // #ifndef EIGEN_GPU_COMPILE_PHASE
 
 template <typename T>
-struct functor_traits<scalar_logistic_op<T> > {
+struct functor_traits<scalar_logistic_op<T>> {
   enum {
     // The cost estimate for float here here is for the common(?) case where
     // all arguments are greater than -9.
     Cost = scalar_div_cost<T, packet_traits<T>::HasDiv>::value +
-           (internal::is_same<T, float>::value
-                ? NumTraits<T>::AddCost * 15 + NumTraits<T>::MulCost * 11
-                : NumTraits<T>::AddCost * 2 +
-                      functor_traits<scalar_exp_op<T> >::Cost),
-    PacketAccess =
-        packet_traits<T>::HasAdd && packet_traits<T>::HasDiv &&
-        (internal::is_same<T, float>::value
-             ? packet_traits<T>::HasMul && packet_traits<T>::HasMax &&
-                   packet_traits<T>::HasMin
-             : packet_traits<T>::HasNegate && packet_traits<T>::HasExp)
+           (internal::is_same<T, float>::value ? NumTraits<T>::AddCost * 15 + NumTraits<T>::MulCost * 11
+                                               : NumTraits<T>::AddCost * 2 + functor_traits<scalar_exp_op<T>>::Cost),
+    PacketAccess = packet_traits<T>::HasAdd && packet_traits<T>::HasDiv &&
+                   (internal::is_same<T, float>::value
+                        ? packet_traits<T>::HasMul && packet_traits<T>::HasMax && packet_traits<T>::HasMin
+                        : packet_traits<T>::HasNegate && packet_traits<T>::HasExp)
   };
 };
 
-} // end namespace internal
+template <typename Scalar, typename ExponentScalar, bool IsBaseInteger = NumTraits<Scalar>::IsInteger,
+          bool IsExponentInteger = NumTraits<ExponentScalar>::IsInteger,
+          bool IsBaseComplex = NumTraits<Scalar>::IsComplex,
+          bool IsExponentComplex = NumTraits<ExponentScalar>::IsComplex>
+struct scalar_unary_pow_op {
+  typedef typename internal::promote_scalar_arg<
+      Scalar, ExponentScalar,
+      internal::has_ReturnType<ScalarBinaryOpTraits<Scalar, ExponentScalar, scalar_unary_pow_op>>::value>::type
+      PromotedExponent;
+  typedef typename ScalarBinaryOpTraits<Scalar, PromotedExponent, scalar_unary_pow_op>::ReturnType result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_unary_pow_op(const ExponentScalar& exponent) : m_exponent(exponent) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const Scalar& a) const {
+    EIGEN_USING_STD(pow);
+    return static_cast<result_type>(pow(a, m_exponent));
+  }
 
-} // end namespace Eigen
+ private:
+  const ExponentScalar m_exponent;
+  scalar_unary_pow_op() {}
+};
 
-#endif // EIGEN_FUNCTORS_H
+template <typename T>
+constexpr int exponent_digits() {
+  return CHAR_BIT * sizeof(T) - NumTraits<T>::digits() - NumTraits<T>::IsSigned;
+}
+
+template <typename From, typename To>
+struct is_floating_exactly_representable {
+  // TODO(rmlarsen): Add radix to NumTraits and enable this check.
+  // (NumTraits<To>::radix == NumTraits<From>::radix) &&
+  static constexpr bool value =
+      (exponent_digits<To>() >= exponent_digits<From>() && NumTraits<To>::digits() >= NumTraits<From>::digits());
+};
+
+// Specialization for real, non-integer types, non-complex types.
+template <typename Scalar, typename ExponentScalar>
+struct scalar_unary_pow_op<Scalar, ExponentScalar, false, false, false, false> {
+  template <bool IsExactlyRepresentable = is_floating_exactly_representable<ExponentScalar, Scalar>::value>
+  std::enable_if_t<IsExactlyRepresentable, void> check_is_representable() const {}
+
+  // Issue a deprecation warning if we do a narrowing conversion on the exponent.
+  template <bool IsExactlyRepresentable = is_floating_exactly_representable<ExponentScalar, Scalar>::value>
+  EIGEN_DEPRECATED std::enable_if_t<!IsExactlyRepresentable, void> check_is_representable() const {}
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_unary_pow_op(const ExponentScalar& exponent)
+      : m_exponent(static_cast<Scalar>(exponent)) {
+    check_is_representable();
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a) const {
+    EIGEN_USING_STD(pow);
+    return static_cast<Scalar>(pow(a, m_exponent));
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a) const {
+    return unary_pow_impl<Packet, Scalar>::run(a, m_exponent);
+  }
+
+ private:
+  const Scalar m_exponent;
+  scalar_unary_pow_op() {}
+};
+
+template <typename Scalar, typename ExponentScalar, bool BaseIsInteger>
+struct scalar_unary_pow_op<Scalar, ExponentScalar, BaseIsInteger, true, false, false> {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_unary_pow_op(const ExponentScalar& exponent) : m_exponent(exponent) {}
+  // TODO: error handling logic for complex^real_integer
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const Scalar& a) const {
+    return unary_pow_impl<Scalar, ExponentScalar>::run(a, m_exponent);
+  }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a) const {
+    return unary_pow_impl<Packet, ExponentScalar>::run(a, m_exponent);
+  }
+
+ private:
+  const ExponentScalar m_exponent;
+  scalar_unary_pow_op() {}
+};
+
+template <typename Scalar, typename ExponentScalar>
+struct functor_traits<scalar_unary_pow_op<Scalar, ExponentScalar>> {
+  enum {
+    GenPacketAccess = functor_traits<scalar_pow_op<Scalar, ExponentScalar>>::PacketAccess,
+    IntPacketAccess = !NumTraits<Scalar>::IsComplex && packet_traits<Scalar>::HasMul &&
+                      (packet_traits<Scalar>::HasDiv || NumTraits<Scalar>::IsInteger) && packet_traits<Scalar>::HasCmp,
+    PacketAccess = NumTraits<ExponentScalar>::IsInteger ? IntPacketAccess : (IntPacketAccess && GenPacketAccess),
+    Cost = functor_traits<scalar_pow_op<Scalar, ExponentScalar>>::Cost
+  };
+};
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_FUNCTORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h
index f35b760..647a7dd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h
@@ -10,57 +10,57 @@
 #ifndef EIGEN_GENERAL_BLOCK_PANEL_H
 #define EIGEN_GENERAL_BLOCK_PANEL_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
 
 namespace Eigen {
 
 namespace internal {
 
-enum GEBPPacketSizeType {
-  GEBPPacketFull = 0,
-  GEBPPacketHalf,
-  GEBPPacketQuarter
-};
+enum GEBPPacketSizeType { GEBPPacketFull = 0, GEBPPacketHalf, GEBPPacketQuarter };
 
-template<typename _LhsScalar, typename _RhsScalar, bool _ConjLhs=false, bool _ConjRhs=false, int Arch=Architecture::Target, int _PacketSize=GEBPPacketFull>
+template <typename LhsScalar_, typename RhsScalar_, bool ConjLhs_ = false, bool ConjRhs_ = false,
+          int Arch = Architecture::Target, int PacketSize_ = GEBPPacketFull>
 class gebp_traits;
 
-
 /** \internal \returns b if a<=0, and returns a otherwise. */
-inline std::ptrdiff_t manage_caching_sizes_helper(std::ptrdiff_t a, std::ptrdiff_t b)
-{
-  return a<=0 ? b : a;
-}
+inline std::ptrdiff_t manage_caching_sizes_helper(std::ptrdiff_t a, std::ptrdiff_t b) { return a <= 0 ? b : a; }
 
 #if defined(EIGEN_DEFAULT_L1_CACHE_SIZE)
 #define EIGEN_SET_DEFAULT_L1_CACHE_SIZE(val) EIGEN_DEFAULT_L1_CACHE_SIZE
 #else
 #define EIGEN_SET_DEFAULT_L1_CACHE_SIZE(val) val
-#endif // defined(EIGEN_DEFAULT_L1_CACHE_SIZE)
+#endif  // defined(EIGEN_DEFAULT_L1_CACHE_SIZE)
 
 #if defined(EIGEN_DEFAULT_L2_CACHE_SIZE)
 #define EIGEN_SET_DEFAULT_L2_CACHE_SIZE(val) EIGEN_DEFAULT_L2_CACHE_SIZE
 #else
 #define EIGEN_SET_DEFAULT_L2_CACHE_SIZE(val) val
-#endif // defined(EIGEN_DEFAULT_L2_CACHE_SIZE)
+#endif  // defined(EIGEN_DEFAULT_L2_CACHE_SIZE)
 
 #if defined(EIGEN_DEFAULT_L3_CACHE_SIZE)
 #define EIGEN_SET_DEFAULT_L3_CACHE_SIZE(val) EIGEN_DEFAULT_L3_CACHE_SIZE
 #else
 #define EIGEN_SET_DEFAULT_L3_CACHE_SIZE(val) val
-#endif // defined(EIGEN_DEFAULT_L3_CACHE_SIZE)
-  
+#endif  // defined(EIGEN_DEFAULT_L3_CACHE_SIZE)
+
 #if EIGEN_ARCH_i386_OR_x86_64
-const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(32*1024);
-const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(256*1024);
-const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(2*1024*1024);
+const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(32 * 1024);
+const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(256 * 1024);
+const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(2 * 1024 * 1024);
 #elif EIGEN_ARCH_PPC
-const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(64*1024);
-const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(512*1024);
-const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(4*1024*1024);
+const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(64 * 1024);
+#ifdef _ARCH_PWR10
+const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(2 * 1024 * 1024);
+const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(8 * 1024 * 1024);
 #else
-const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(16*1024);
-const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(512*1024);
-const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(512*1024);
+const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(512 * 1024);
+const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(4 * 1024 * 1024);
+#endif
+#else
+const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(16 * 1024);
+const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(512 * 1024);
+const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(512 * 1024);
 #endif
 
 #undef EIGEN_SET_DEFAULT_L1_CACHE_SIZE
@@ -69,7 +69,7 @@
 
 /** \internal */
 struct CacheSizes {
-  CacheSizes(): m_l1(-1),m_l2(-1),m_l3(-1) {
+  CacheSizes() : m_l1(-1), m_l2(-1), m_l3(-1) {
     int l1CacheSize, l2CacheSize, l3CacheSize;
     queryCacheSizes(l1CacheSize, l2CacheSize, l3CacheSize);
     m_l1 = manage_caching_sizes_helper(l1CacheSize, defaultL1CacheSize);
@@ -83,27 +83,21 @@
 };
 
 /** \internal */
-inline void manage_caching_sizes(Action action, std::ptrdiff_t* l1, std::ptrdiff_t* l2, std::ptrdiff_t* l3)
-{
+inline void manage_caching_sizes(Action action, std::ptrdiff_t* l1, std::ptrdiff_t* l2, std::ptrdiff_t* l3) {
   static CacheSizes m_cacheSizes;
 
-  if(action==SetAction)
-  {
+  if (action == SetAction) {
     // set the cpu cache size and cache all block sizes from a global cache size in byte
-    eigen_internal_assert(l1!=0 && l2!=0);
+    eigen_internal_assert(l1 != 0 && l2 != 0);
     m_cacheSizes.m_l1 = *l1;
     m_cacheSizes.m_l2 = *l2;
     m_cacheSizes.m_l3 = *l3;
-  }
-  else if(action==GetAction)
-  {
-    eigen_internal_assert(l1!=0 && l2!=0);
+  } else if (action == GetAction) {
+    eigen_internal_assert(l1 != 0 && l2 != 0);
     *l1 = m_cacheSizes.m_l1;
     *l2 = m_cacheSizes.m_l2;
     *l3 = m_cacheSizes.m_l3;
-  }
-  else
-  {
+  } else {
     eigen_internal_assert(false);
   }
 }
@@ -120,10 +114,9 @@
  *
  * \sa setCpuCacheSizes */
 
-template<typename LhsScalar, typename RhsScalar, int KcFactor, typename Index>
-void evaluateProductBlockingSizesHeuristic(Index& k, Index& m, Index& n, Index num_threads = 1)
-{
-  typedef gebp_traits<LhsScalar,RhsScalar> Traits;
+template <typename LhsScalar, typename RhsScalar, int KcFactor, typename Index>
+void evaluateProductBlockingSizesHeuristic(Index& k, Index& m, Index& n, Index num_threads = 1) {
+  typedef gebp_traits<LhsScalar, RhsScalar> Traits;
 
   // Explanations:
   // Let's recall that the product algorithms form mc x kc vertical panels A' on the lhs and
@@ -132,7 +125,7 @@
   // at the register level. This small horizontal panel has to stay within L1 cache.
   std::ptrdiff_t l1, l2, l3;
   manage_caching_sizes(GetAction, &l1, &l2, &l3);
-  #ifdef EIGEN_VECTORIZE_AVX512
+#ifdef EIGEN_VECTORIZE_AVX512
   // We need to find a rationale for that, but without this adjustment,
   // performance with AVX512 is pretty bad, like -20% slower.
   // One reason is that with increasing packet-size, the blocking size k
@@ -141,7 +134,7 @@
   //   k*(3*64 + 4*8) Bytes, with l1=32kBytes, and k%8=0, we have k=144.
   // This is quite small for a good reuse of the accumulation registers.
   l1 *= 4;
-  #endif
+#endif
 
   if (num_threads > 1) {
     typedef typename Traits::ResScalar ResScalar;
@@ -157,13 +150,13 @@
     // increasing the value of k, so we'll cap it at 320 (value determined
     // experimentally).
     // To avoid that k vanishes, we make k_cache at least as big as kr
-    const Index k_cache = numext::maxi<Index>(kr, (numext::mini<Index>)((l1-ksub)/kdiv, 320));
+    const Index k_cache = numext::maxi<Index>(kr, (numext::mini<Index>)((l1 - ksub) / kdiv, 320));
     if (k_cache < k) {
       k = k_cache - (k_cache % kr);
       eigen_internal_assert(k > 0);
     }
 
-    const Index n_cache = (l2-l1) / (nr * sizeof(RhsScalar) * k);
+    const Index n_cache = (l2 - l1) / (nr * sizeof(RhsScalar) * k);
     const Index n_per_thread = numext::div_ceil(n, num_threads);
     if (n_cache <= n_per_thread) {
       // Don't exceed the capacity of the l2 cache.
@@ -176,31 +169,29 @@
 
     if (l3 > l2) {
       // l3 is shared between all cores, so we'll give each thread its own chunk of l3.
-      const Index m_cache = (l3-l2) / (sizeof(LhsScalar) * k * num_threads);
+      const Index m_cache = (l3 - l2) / (sizeof(LhsScalar) * k * num_threads);
       const Index m_per_thread = numext::div_ceil(m, num_threads);
-      if(m_cache < m_per_thread && m_cache >= static_cast<Index>(mr)) {
+      if (m_cache < m_per_thread && m_cache >= static_cast<Index>(mr)) {
         m = m_cache - (m_cache % mr);
         eigen_internal_assert(m > 0);
       } else {
         m = (numext::mini<Index>)(m, (m_per_thread + mr - 1) - ((m_per_thread + mr - 1) % mr));
       }
     }
-  }
-  else {
+  } else {
     // In unit tests we do not want to use extra large matrices,
     // so we reduce the cache size to check the blocking strategy is not flawed
 #ifdef EIGEN_DEBUG_SMALL_PRODUCT_BLOCKS
-    l1 = 9*1024;
-    l2 = 32*1024;
-    l3 = 512*1024;
+    l1 = 9 * 1024;
+    l2 = 32 * 1024;
+    l3 = 512 * 1024;
 #endif
 
     // Early return for small problems because the computation below are time consuming for small problems.
     // Perhaps it would make more sense to consider k*n*m??
     // Note that for very tiny problem, this function should be bypassed anyway
     // because we use the coefficient-based implementation for them.
-    if((numext::maxi)(k,(numext::maxi)(m,n))<48)
-      return;
+    if ((numext::maxi)(k, (numext::maxi)(m, n)) < 48) return;
 
     typedef typename Traits::ResScalar ResScalar;
     enum {
@@ -216,30 +207,29 @@
     // We also include a register-level block of the result (mx x nr).
     // (In an ideal world only the lhs panel would stay in L1)
     // Moreover, kc has to be a multiple of 8 to be compatible with loop peeling, leading to a maximum blocking size of:
-    const Index max_kc = numext::maxi<Index>(((l1-k_sub)/k_div) & (~(k_peeling-1)),1);
+    const Index max_kc = numext::maxi<Index>(((l1 - k_sub) / k_div) & (~(k_peeling - 1)), 1);
     const Index old_k = k;
-    if(k>max_kc)
-    {
+    if (k > max_kc) {
       // We are really blocking on the third dimension:
       // -> reduce blocking size to make sure the last block is as large as possible
       //    while keeping the same number of sweeps over the result.
-      k = (k%max_kc)==0 ? max_kc
-                        : max_kc - k_peeling * ((max_kc-1-(k%max_kc))/(k_peeling*(k/max_kc+1)));
+      k = (k % max_kc) == 0 ? max_kc
+                            : max_kc - k_peeling * ((max_kc - 1 - (k % max_kc)) / (k_peeling * (k / max_kc + 1)));
 
-      eigen_internal_assert(((old_k/k) == (old_k/max_kc)) && "the number of sweeps has to remain the same");
+      eigen_internal_assert(((old_k / k) == (old_k / max_kc)) && "the number of sweeps has to remain the same");
     }
 
-    // ---- 2nd level of blocking on max(L2,L3), yields nc ----
+// ---- 2nd level of blocking on max(L2,L3), yields nc ----
 
-    // TODO find a reliable way to get the actual amount of cache per core to use for 2nd level blocking, that is:
-    //      actual_l2 = max(l2, l3/nb_core_sharing_l3)
-    // The number below is quite conservative: it is better to underestimate the cache size rather than overestimating it)
-    // For instance, it corresponds to 6MB of L3 shared among 4 cores.
-    #ifdef EIGEN_DEBUG_SMALL_PRODUCT_BLOCKS
+// TODO find a reliable way to get the actual amount of cache per core to use for 2nd level blocking, that is:
+//      actual_l2 = max(l2, l3/nb_core_sharing_l3)
+// The number below is quite conservative: it is better to underestimate the cache size rather than overestimating it)
+// For instance, it corresponds to 6MB of L3 shared among 4 cores.
+#ifdef EIGEN_DEBUG_SMALL_PRODUCT_BLOCKS
     const Index actual_l2 = l3;
-    #else
-    const Index actual_l2 = 1572864; // == 1.5 MB
-    #endif
+#else
+    const Index actual_l2 = 1572864;  // == 1.5 MB
+#endif
 
     // Here, nc is chosen such that a block of kc x nc of the rhs fit within half of L2.
     // The second half is implicitly reserved to access the result and lhs coefficients.
@@ -249,61 +239,52 @@
     // and it becomes fruitful to keep the packed rhs blocks in L1 if there is enough remaining space.
     Index max_nc;
     const Index lhs_bytes = m * k * sizeof(LhsScalar);
-    const Index remaining_l1 = l1- k_sub - lhs_bytes;
-    if(remaining_l1 >= Index(Traits::nr*sizeof(RhsScalar))*k)
-    {
+    const Index remaining_l1 = l1 - k_sub - lhs_bytes;
+    if (remaining_l1 >= Index(Traits::nr * sizeof(RhsScalar)) * k) {
       // L1 blocking
-      max_nc = remaining_l1 / (k*sizeof(RhsScalar));
-    }
-    else
-    {
+      max_nc = remaining_l1 / (k * sizeof(RhsScalar));
+    } else {
       // L2 blocking
-      max_nc = (3*actual_l2)/(2*2*max_kc*sizeof(RhsScalar));
+      max_nc = (3 * actual_l2) / (2 * 2 * max_kc * sizeof(RhsScalar));
     }
     // WARNING Below, we assume that Traits::nr is a power of two.
-    Index nc = numext::mini<Index>(actual_l2/(2*k*sizeof(RhsScalar)), max_nc) & (~(Traits::nr-1));
-    if(n>nc)
-    {
+    Index nc = numext::mini<Index>(actual_l2 / (2 * k * sizeof(RhsScalar)), max_nc) & (~(Traits::nr - 1));
+    if (n > nc) {
       // We are really blocking over the columns:
       // -> reduce blocking size to make sure the last block is as large as possible
       //    while keeping the same number of sweeps over the packed lhs.
       //    Here we allow one more sweep if this gives us a perfect match, thus the commented "-1"
-      n = (n%nc)==0 ? nc
-                    : (nc - Traits::nr * ((nc/*-1*/-(n%nc))/(Traits::nr*(n/nc+1))));
-    }
-    else if(old_k==k)
-    {
+      n = (n % nc) == 0 ? nc : (nc - Traits::nr * ((nc /*-1*/ - (n % nc)) / (Traits::nr * (n / nc + 1))));
+    } else if (old_k == k) {
       // So far, no blocking at all, i.e., kc==k, and nc==n.
       // In this case, let's perform a blocking over the rows such that the packed lhs data is kept in cache L1/L2
-      // TODO: part of this blocking strategy is now implemented within the kernel itself, so the L1-based heuristic here should be obsolete.
-      Index problem_size = k*n*sizeof(LhsScalar);
+      // TODO: part of this blocking strategy is now implemented within the kernel itself, so the L1-based heuristic
+      // here should be obsolete.
+      Index problem_size = k * n * sizeof(LhsScalar);
       Index actual_lm = actual_l2;
       Index max_mc = m;
-      if(problem_size<=1024)
-      {
+      if (problem_size <= 1024) {
         // problem is small enough to keep in L1
         // Let's choose m such that lhs's block fit in 1/3 of L1
         actual_lm = l1;
-      }
-      else if(l3!=0 && problem_size<=32768)
-      {
+      } else if (l3 != 0 && problem_size <= 32768) {
         // we have both L2 and L3, and problem is small enough to be kept in L2
         // Let's choose m such that lhs's block fit in 1/3 of L2
         actual_lm = l2;
-        max_mc = (numext::mini<Index>)(576,max_mc);
+        max_mc = (numext::mini<Index>)(576, max_mc);
       }
-      Index mc = (numext::mini<Index>)(actual_lm/(3*k*sizeof(LhsScalar)), max_mc);
-      if (mc > Traits::mr) mc -= mc % Traits::mr;
-      else if (mc==0) return;
-      m = (m%mc)==0 ? mc
-                    : (mc - Traits::mr * ((mc/*-1*/-(m%mc))/(Traits::mr*(m/mc+1))));
+      Index mc = (numext::mini<Index>)(actual_lm / (3 * k * sizeof(LhsScalar)), max_mc);
+      if (mc > Traits::mr)
+        mc -= mc % Traits::mr;
+      else if (mc == 0)
+        return;
+      m = (m % mc) == 0 ? mc : (mc - Traits::mr * ((mc /*-1*/ - (m % mc)) / (Traits::mr * (m / mc + 1))));
     }
   }
 }
 
 template <typename Index>
-inline bool useSpecificBlockingSizes(Index& k, Index& m, Index& n)
-{
+inline bool useSpecificBlockingSizes(Index& k, Index& m, Index& n) {
 #ifdef EIGEN_TEST_SPECIFIC_BLOCKING_SIZES
   if (EIGEN_TEST_SPECIFIC_BLOCKING_SIZES) {
     k = numext::mini<Index>(k, EIGEN_TEST_SPECIFIC_BLOCKING_SIZE_K);
@@ -320,46 +301,46 @@
 }
 
 /** \brief Computes the blocking parameters for a m x k times k x n matrix product
-  *
-  * \param[in,out] k Input: the third dimension of the product. Output: the blocking size along the same dimension.
-  * \param[in,out] m Input: the number of rows of the left hand side. Output: the blocking size along the same dimension.
-  * \param[in,out] n Input: the number of columns of the right hand side. Output: the blocking size along the same dimension.
-  *
-  * Given a m x k times k x n matrix product of scalar types \c LhsScalar and \c RhsScalar,
-  * this function computes the blocking size parameters along the respective dimensions
-  * for matrix products and related algorithms.
-  *
-  * The blocking size parameters may be evaluated:
-  *   - either by a heuristic based on cache sizes;
-  *   - or using fixed prescribed values (for testing purposes).
-  *
-  * \sa setCpuCacheSizes */
+ *
+ * \param[in,out] k Input: the third dimension of the product. Output: the blocking size along the same dimension.
+ * \param[in,out] m Input: the number of rows of the left hand side. Output: the blocking size along the same dimension.
+ * \param[in,out] n Input: the number of columns of the right hand side. Output: the blocking size along the same
+ * dimension.
+ *
+ * Given a m x k times k x n matrix product of scalar types \c LhsScalar and \c RhsScalar,
+ * this function computes the blocking size parameters along the respective dimensions
+ * for matrix products and related algorithms.
+ *
+ * The blocking size parameters may be evaluated:
+ *   - either by a heuristic based on cache sizes;
+ *   - or using fixed prescribed values (for testing purposes).
+ *
+ * \sa setCpuCacheSizes */
 
-template<typename LhsScalar, typename RhsScalar, int KcFactor, typename Index>
-void computeProductBlockingSizes(Index& k, Index& m, Index& n, Index num_threads = 1)
-{
+template <typename LhsScalar, typename RhsScalar, int KcFactor, typename Index>
+void computeProductBlockingSizes(Index& k, Index& m, Index& n, Index num_threads = 1) {
   if (!useSpecificBlockingSizes(k, m, n)) {
     evaluateProductBlockingSizesHeuristic<LhsScalar, RhsScalar, KcFactor, Index>(k, m, n, num_threads);
   }
 }
 
-template<typename LhsScalar, typename RhsScalar, typename Index>
-inline void computeProductBlockingSizes(Index& k, Index& m, Index& n, Index num_threads = 1)
-{
-  computeProductBlockingSizes<LhsScalar,RhsScalar,1,Index>(k, m, n, num_threads);
+template <typename LhsScalar, typename RhsScalar, typename Index>
+inline void computeProductBlockingSizes(Index& k, Index& m, Index& n, Index num_threads = 1) {
+  computeProductBlockingSizes<LhsScalar, RhsScalar, 1, Index>(k, m, n, num_threads);
 }
 
 template <typename RhsPacket, typename RhsPacketx4, int registers_taken>
 struct RhsPanelHelper {
  private:
-  static const int remaining_registers = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS - registers_taken;
+  static constexpr int remaining_registers =
+      (std::max)(int(EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS) - registers_taken, 0);
+
  public:
-  typedef typename conditional<remaining_registers>=4, RhsPacketx4, RhsPacket>::type type;
+  typedef std::conditional_t<remaining_registers >= 4, RhsPacketx4, RhsPacket> type;
 };
 
 template <typename Packet>
-struct QuadPacket
-{
+struct QuadPacket {
   Packet B_0, B1, B2, B3;
   const Packet& get(const FixedInt<0>&) const { return B_0; }
   const Packet& get(const FixedInt<1>&) const { return B1; }
@@ -368,329 +349,295 @@
 };
 
 template <int N, typename T1, typename T2, typename T3>
-struct packet_conditional { typedef T3 type; };
+struct packet_conditional {
+  typedef T3 type;
+};
 
 template <typename T1, typename T2, typename T3>
-struct packet_conditional<GEBPPacketFull, T1, T2, T3> { typedef T1 type; };
+struct packet_conditional<GEBPPacketFull, T1, T2, T3> {
+  typedef T1 type;
+};
 
 template <typename T1, typename T2, typename T3>
-struct packet_conditional<GEBPPacketHalf, T1, T2, T3> { typedef T2 type; };
+struct packet_conditional<GEBPPacketHalf, T1, T2, T3> {
+  typedef T2 type;
+};
 
-#define PACKET_DECL_COND_PREFIX(prefix, name, packet_size)         \
-  typedef typename packet_conditional<packet_size,                 \
-                                      typename packet_traits<name ## Scalar>::type, \
-                                      typename packet_traits<name ## Scalar>::half, \
-                                      typename unpacket_traits<typename packet_traits<name ## Scalar>::half>::half>::type \
-  prefix ## name ## Packet
+#define PACKET_DECL_COND_POSTFIX(postfix, name, packet_size)                                               \
+  typedef typename packet_conditional<                                                                     \
+      packet_size, typename packet_traits<name##Scalar>::type, typename packet_traits<name##Scalar>::half, \
+      typename unpacket_traits<typename packet_traits<name##Scalar>::half>::half>::type name##Packet##postfix
 
-#define PACKET_DECL_COND(name, packet_size)                        \
-  typedef typename packet_conditional<packet_size,                 \
-                                      typename packet_traits<name ## Scalar>::type, \
-                                      typename packet_traits<name ## Scalar>::half, \
-                                      typename unpacket_traits<typename packet_traits<name ## Scalar>::half>::half>::type \
-  name ## Packet
+#define PACKET_DECL_COND(name, packet_size)                                                                \
+  typedef typename packet_conditional<                                                                     \
+      packet_size, typename packet_traits<name##Scalar>::type, typename packet_traits<name##Scalar>::half, \
+      typename unpacket_traits<typename packet_traits<name##Scalar>::half>::half>::type name##Packet
 
-#define PACKET_DECL_COND_SCALAR_PREFIX(prefix, packet_size)        \
-  typedef typename packet_conditional<packet_size,                 \
-                                      typename packet_traits<Scalar>::type, \
-                                      typename packet_traits<Scalar>::half, \
-                                      typename unpacket_traits<typename packet_traits<Scalar>::half>::half>::type \
-  prefix ## ScalarPacket
+#define PACKET_DECL_COND_SCALAR_POSTFIX(postfix, packet_size)                                  \
+  typedef typename packet_conditional<                                                         \
+      packet_size, typename packet_traits<Scalar>::type, typename packet_traits<Scalar>::half, \
+      typename unpacket_traits<typename packet_traits<Scalar>::half>::half>::type ScalarPacket##postfix
 
-#define PACKET_DECL_COND_SCALAR(packet_size)                       \
-  typedef typename packet_conditional<packet_size,                 \
-                                      typename packet_traits<Scalar>::type, \
-                                      typename packet_traits<Scalar>::half, \
-                                      typename unpacket_traits<typename packet_traits<Scalar>::half>::half>::type \
-  ScalarPacket
+#define PACKET_DECL_COND_SCALAR(packet_size)                                                   \
+  typedef typename packet_conditional<                                                         \
+      packet_size, typename packet_traits<Scalar>::type, typename packet_traits<Scalar>::half, \
+      typename unpacket_traits<typename packet_traits<Scalar>::half>::half>::type ScalarPacket
 
 /* Vectorization logic
  *  real*real: unpack rhs to constant packets, ...
- * 
+ *
  *  cd*cd : unpack rhs to (b_r,b_r), (b_i,b_i), mul to get (a_r b_r,a_i b_r) (a_r b_i,a_i b_i),
  *          storing each res packet into two packets (2x2),
- *          at the end combine them: swap the second and addsub them 
+ *          at the end combine them: swap the second and addsub them
  *  cf*cf : same but with 2x4 blocks
  *  cplx*real : unpack rhs to constant packets, ...
  *  real*cplx : load lhs as (a0,a0,a1,a1), and mul as usual
  */
-template<typename _LhsScalar, typename _RhsScalar, bool _ConjLhs, bool _ConjRhs, int Arch, int _PacketSize>
-class gebp_traits
-{
-public:
-  typedef _LhsScalar LhsScalar;
-  typedef _RhsScalar RhsScalar;
+template <typename LhsScalar_, typename RhsScalar_, bool ConjLhs_, bool ConjRhs_, int Arch, int PacketSize_>
+class gebp_traits {
+ public:
+  typedef LhsScalar_ LhsScalar;
+  typedef RhsScalar_ RhsScalar;
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
-  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
+  PACKET_DECL_COND_POSTFIX(_, Lhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Rhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Res, PacketSize_);
 
   enum {
-    ConjLhs = _ConjLhs,
-    ConjRhs = _ConjRhs,
-    Vectorizable = unpacket_traits<_LhsPacket>::vectorizable && unpacket_traits<_RhsPacket>::vectorizable,
-    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
-    RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
-    ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
-    
+    ConjLhs = ConjLhs_,
+    ConjRhs = ConjRhs_,
+    Vectorizable = unpacket_traits<LhsPacket_>::vectorizable && unpacket_traits<RhsPacket_>::vectorizable,
+    LhsPacketSize = Vectorizable ? unpacket_traits<LhsPacket_>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<RhsPacket_>::size : 1,
+    ResPacketSize = Vectorizable ? unpacket_traits<ResPacket_>::size : 1,
+
     NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
 
     // register block size along the N direction must be 1 or 4
     nr = 4,
 
     // register block size along the M direction (currently, this one cannot be modified)
-    default_mr = (EIGEN_PLAIN_ENUM_MIN(16,NumberOfRegisters)/2/nr)*LhsPacketSize,
-#if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD) && !defined(EIGEN_VECTORIZE_ALTIVEC) && !defined(EIGEN_VECTORIZE_VSX) \
-    && ((!EIGEN_COMP_MSVC) || (EIGEN_COMP_MSVC>=1914))
+    default_mr = (plain_enum_min(16, NumberOfRegisters) / 2 / nr) * LhsPacketSize,
+#if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD) && !defined(EIGEN_VECTORIZE_ALTIVEC) && \
+    !defined(EIGEN_VECTORIZE_VSX) && ((!EIGEN_COMP_MSVC) || (EIGEN_COMP_MSVC >= 1914))
     // we assume 16 registers or more
     // See bug 992, if the scalar type is not vectorizable but that EIGEN_HAS_SINGLE_INSTRUCTION_MADD is defined,
     // then using 3*LhsPacketSize triggers non-implemented paths in syrk.
     // Bug 1515: MSVC prior to v19.14 yields to register spilling.
-    mr = Vectorizable ? 3*LhsPacketSize : default_mr,
+    mr = Vectorizable ? 3 * LhsPacketSize : default_mr,
 #else
     mr = default_mr,
 #endif
-    
+
     LhsProgress = LhsPacketSize,
     RhsProgress = 1
   };
 
-
-  typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
-  typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
-  typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+  typedef std::conditional_t<Vectorizable, LhsPacket_, LhsScalar> LhsPacket;
+  typedef std::conditional_t<Vectorizable, RhsPacket_, RhsScalar> RhsPacket;
+  typedef std::conditional_t<Vectorizable, ResPacket_, ResScalar> ResPacket;
   typedef LhsPacket LhsPacket4Packing;
 
   typedef QuadPacket<RhsPacket> RhsPacketx4;
   typedef ResPacket AccPacket;
-  
-  EIGEN_STRONG_INLINE void initAcc(AccPacket& p)
-  {
-    p = pset1<ResPacket>(ResScalar(0));
-  }
 
-  template<typename RhsPacketType>
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const
-  {
+  EIGEN_STRONG_INLINE void initAcc(AccPacket& p) { p = pset1<ResPacket>(ResScalar(0)); }
+
+  template <typename RhsPacketType>
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const {
     dest = pset1<RhsPacketType>(*b);
   }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
-  {
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const {
     pbroadcast4(b, dest.B_0, dest.B1, dest.B2, dest.B3);
   }
 
-  template<typename RhsPacketType>
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const
-  {
+  template <typename RhsPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const {
     loadRhs(b, dest);
   }
 
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
-  {
-  }
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
 
-  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
-  {
-    dest = ploadquad<RhsPacket>(b);
-  }
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const { dest = ploadquad<RhsPacket>(b); }
 
-  template<typename LhsPacketType>
-  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacketType& dest) const
-  {
+  template <typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacketType& dest) const {
     dest = pload<LhsPacketType>(a);
   }
 
-  template<typename LhsPacketType>
-  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const
-  {
+  template <typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const {
     dest = ploadu<LhsPacketType>(a);
   }
 
-  template<typename LhsPacketType, typename RhsPacketType, typename AccPacketType, typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const LaneIdType&) const
-  {
-    conj_helper<LhsPacketType,RhsPacketType,ConjLhs,ConjRhs> cj;
+  template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp,
+                                const LaneIdType&) const {
+    conj_helper<LhsPacketType, RhsPacketType, ConjLhs, ConjRhs> cj;
     // It would be a lot cleaner to call pmadd all the time. Unfortunately if we
     // let gcc allocate the register in which to store the result of the pmul
     // (in the case where there is no FMA) gcc fails to figure out how to avoid
     // spilling register.
 #ifdef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
     EIGEN_UNUSED_VARIABLE(tmp);
-    c = cj.pmadd(a,b,c);
+    c = cj.pmadd(a, b, c);
 #else
-    tmp = b; tmp = cj.pmul(a,tmp); c = padd(c,tmp);
+    tmp = b;
+    tmp = cj.pmul(a, tmp);
+    c = padd(c, tmp);
 #endif
   }
 
-  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
-  {
+  template <typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp,
+                                const LaneIdType& lane) const {
     madd(a, b.get(lane), c, tmp, lane);
   }
 
-  EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const
-  {
-    r = pmadd(c,alpha,r);
-  }
-  
-  template<typename ResPacketHalf>
-  EIGEN_STRONG_INLINE void acc(const ResPacketHalf& c, const ResPacketHalf& alpha, ResPacketHalf& r) const
-  {
-    r = pmadd(c,alpha,r);
+  EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const {
+    r = pmadd(c, alpha, r);
   }
 
+  template <typename ResPacketHalf>
+  EIGEN_STRONG_INLINE void acc(const ResPacketHalf& c, const ResPacketHalf& alpha, ResPacketHalf& r) const {
+    r = pmadd(c, alpha, r);
+  }
 };
 
-template<typename RealScalar, bool _ConjLhs, int Arch, int _PacketSize>
-class gebp_traits<std::complex<RealScalar>, RealScalar, _ConjLhs, false, Arch, _PacketSize>
-{
-public:
+template <typename RealScalar, bool ConjLhs_, int Arch, int PacketSize_>
+class gebp_traits<std::complex<RealScalar>, RealScalar, ConjLhs_, false, Arch, PacketSize_> {
+ public:
   typedef std::complex<RealScalar> LhsScalar;
   typedef RealScalar RhsScalar;
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
-  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
+  PACKET_DECL_COND_POSTFIX(_, Lhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Rhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Res, PacketSize_);
 
   enum {
-    ConjLhs = _ConjLhs,
+    ConjLhs = ConjLhs_,
     ConjRhs = false,
-    Vectorizable = unpacket_traits<_LhsPacket>::vectorizable && unpacket_traits<_RhsPacket>::vectorizable,
-    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
-    RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
-    ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
-    
+    Vectorizable = unpacket_traits<LhsPacket_>::vectorizable && unpacket_traits<RhsPacket_>::vectorizable,
+    LhsPacketSize = Vectorizable ? unpacket_traits<LhsPacket_>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<RhsPacket_>::size : 1,
+    ResPacketSize = Vectorizable ? unpacket_traits<ResPacket_>::size : 1,
+
     NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
     nr = 4,
 #if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD) && !defined(EIGEN_VECTORIZE_ALTIVEC) && !defined(EIGEN_VECTORIZE_VSX)
     // we assume 16 registers
-    mr = 3*LhsPacketSize,
+    mr = 3 * LhsPacketSize,
 #else
-    mr = (EIGEN_PLAIN_ENUM_MIN(16,NumberOfRegisters)/2/nr)*LhsPacketSize,
+    mr = (plain_enum_min(16, NumberOfRegisters) / 2 / nr) * LhsPacketSize,
 #endif
 
     LhsProgress = LhsPacketSize,
     RhsProgress = 1
   };
 
-  typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
-  typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
-  typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+  typedef std::conditional_t<Vectorizable, LhsPacket_, LhsScalar> LhsPacket;
+  typedef std::conditional_t<Vectorizable, RhsPacket_, RhsScalar> RhsPacket;
+  typedef std::conditional_t<Vectorizable, ResPacket_, ResScalar> ResPacket;
   typedef LhsPacket LhsPacket4Packing;
 
   typedef QuadPacket<RhsPacket> RhsPacketx4;
 
   typedef ResPacket AccPacket;
 
-  EIGEN_STRONG_INLINE void initAcc(AccPacket& p)
-  {
-    p = pset1<ResPacket>(ResScalar(0));
-  }
+  EIGEN_STRONG_INLINE void initAcc(AccPacket& p) { p = pset1<ResPacket>(ResScalar(0)); }
 
-  template<typename RhsPacketType>
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const
-  {
+  template <typename RhsPacketType>
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const {
     dest = pset1<RhsPacketType>(*b);
   }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
-  {
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const {
     pbroadcast4(b, dest.B_0, dest.B1, dest.B2, dest.B3);
   }
 
-  template<typename RhsPacketType>
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const
-  {
+  template <typename RhsPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const {
     loadRhs(b, dest);
   }
 
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
-  {}
-  
-  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
-  {
-    loadRhsQuad_impl(b,dest, typename conditional<RhsPacketSize==16,true_type,false_type>::type());
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
+
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const {
+    loadRhsQuad_impl(b, dest, std::conditional_t<RhsPacketSize == 16, true_type, false_type>());
   }
 
-  EIGEN_STRONG_INLINE void loadRhsQuad_impl(const RhsScalar* b, RhsPacket& dest, const true_type&) const
-  {
+  EIGEN_STRONG_INLINE void loadRhsQuad_impl(const RhsScalar* b, RhsPacket& dest, const true_type&) const {
     // FIXME we can do better!
     // what we want here is a ploadheight
-    RhsScalar tmp[4] = {b[0],b[0],b[1],b[1]};
+    RhsScalar tmp[4] = {b[0], b[0], b[1], b[1]};
     dest = ploadquad<RhsPacket>(tmp);
   }
 
-  EIGEN_STRONG_INLINE void loadRhsQuad_impl(const RhsScalar* b, RhsPacket& dest, const false_type&) const
-  {
-    eigen_internal_assert(RhsPacketSize<=8);
+  EIGEN_STRONG_INLINE void loadRhsQuad_impl(const RhsScalar* b, RhsPacket& dest, const false_type&) const {
+    eigen_internal_assert(RhsPacketSize <= 8);
     dest = pset1<RhsPacket>(*b);
   }
 
-  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacket& dest) const
-  {
-    dest = pload<LhsPacket>(a);
-  }
+  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacket& dest) const { dest = pload<LhsPacket>(a); }
 
-  template<typename LhsPacketType>
-  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const
-  {
+  template <typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const {
     dest = ploadu<LhsPacketType>(a);
   }
 
   template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType, typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const LaneIdType&) const
-  {
-    madd_impl(a, b, c, tmp, typename conditional<Vectorizable,true_type,false_type>::type());
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp,
+                                const LaneIdType&) const {
+    madd_impl(a, b, c, tmp, std::conditional_t<Vectorizable, true_type, false_type>());
   }
 
   template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType>
-  EIGEN_STRONG_INLINE void madd_impl(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const true_type&) const
-  {
+  EIGEN_STRONG_INLINE void madd_impl(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c,
+                                     RhsPacketType& tmp, const true_type&) const {
 #ifdef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
     EIGEN_UNUSED_VARIABLE(tmp);
-    c.v = pmadd(a.v,b,c.v);
+    c.v = pmadd(a.v, b, c.v);
 #else
-    tmp = b; tmp = pmul(a.v,tmp); c.v = padd(c.v,tmp);
+    tmp = b;
+    tmp = pmul(a.v, tmp);
+    c.v = padd(c.v, tmp);
 #endif
   }
 
-  EIGEN_STRONG_INLINE void madd_impl(const LhsScalar& a, const RhsScalar& b, ResScalar& c, RhsScalar& /*tmp*/, const false_type&) const
-  {
+  EIGEN_STRONG_INLINE void madd_impl(const LhsScalar& a, const RhsScalar& b, ResScalar& c, RhsScalar& /*tmp*/,
+                                     const false_type&) const {
     c += a * b;
   }
 
-  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
-  {
+  template <typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp,
+                                const LaneIdType& lane) const {
     madd(a, b.get(lane), c, tmp, lane);
   }
 
   template <typename ResPacketType, typename AccPacketType>
-  EIGEN_STRONG_INLINE void acc(const AccPacketType& c, const ResPacketType& alpha, ResPacketType& r) const
-  {
-    conj_helper<ResPacketType,ResPacketType,ConjLhs,false> cj;
-    r = cj.pmadd(c,alpha,r);
+  EIGEN_STRONG_INLINE void acc(const AccPacketType& c, const ResPacketType& alpha, ResPacketType& r) const {
+    conj_helper<ResPacketType, ResPacketType, ConjLhs, false> cj;
+    r = cj.pmadd(c, alpha, r);
   }
 
-protected:
+ protected:
 };
 
-template<typename Packet>
-struct DoublePacket
-{
+template <typename Packet>
+struct DoublePacket {
   Packet first;
   Packet second;
 };
 
-template<typename Packet>
-DoublePacket<Packet> padd(const DoublePacket<Packet> &a, const DoublePacket<Packet> &b)
-{
+template <typename Packet>
+DoublePacket<Packet> padd(const DoublePacket<Packet>& a, const DoublePacket<Packet>& b) {
   DoublePacket<Packet> res;
-  res.first  = padd(a.first, b.first);
-  res.second = padd(a.second,b.second);
+  res.first = padd(a.first, b.first);
+  res.second = padd(a.second, b.second);
   return res;
 }
 
@@ -698,52 +645,47 @@
 // corresponds to the number of complexes, so it means "8"
 // it terms of real coefficients.
 
-template<typename Packet>
-const DoublePacket<Packet>&
-predux_half_dowto4(const DoublePacket<Packet> &a,
-                   typename enable_if<unpacket_traits<Packet>::size<=8>::type* = 0)
-{
+template <typename Packet>
+const DoublePacket<Packet>& predux_half_dowto4(const DoublePacket<Packet>& a,
+                                               std::enable_if_t<unpacket_traits<Packet>::size <= 8>* = 0) {
   return a;
 }
 
-template<typename Packet>
-DoublePacket<typename unpacket_traits<Packet>::half>
-predux_half_dowto4(const DoublePacket<Packet> &a,
-                   typename enable_if<unpacket_traits<Packet>::size==16>::type* = 0)
-{
+template <typename Packet>
+DoublePacket<typename unpacket_traits<Packet>::half> predux_half_dowto4(
+    const DoublePacket<Packet>& a, std::enable_if_t<unpacket_traits<Packet>::size == 16>* = 0) {
   // yes, that's pretty hackish :(
   DoublePacket<typename unpacket_traits<Packet>::half> res;
   typedef std::complex<typename unpacket_traits<Packet>::type> Cplx;
   typedef typename packet_traits<Cplx>::type CplxPacket;
-  res.first  = predux_half_dowto4(CplxPacket(a.first)).v;
+  res.first = predux_half_dowto4(CplxPacket(a.first)).v;
   res.second = predux_half_dowto4(CplxPacket(a.second)).v;
   return res;
 }
 
 // same here, "quad" actually means "8" in terms of real coefficients
-template<typename Scalar, typename RealPacket>
+template <typename Scalar, typename RealPacket>
 void loadQuadToDoublePacket(const Scalar* b, DoublePacket<RealPacket>& dest,
-                            typename enable_if<unpacket_traits<RealPacket>::size<=8>::type* = 0)
-{
-  dest.first  = pset1<RealPacket>(numext::real(*b));
+                            std::enable_if_t<unpacket_traits<RealPacket>::size <= 8>* = 0) {
+  dest.first = pset1<RealPacket>(numext::real(*b));
   dest.second = pset1<RealPacket>(numext::imag(*b));
 }
 
-template<typename Scalar, typename RealPacket>
+template <typename Scalar, typename RealPacket>
 void loadQuadToDoublePacket(const Scalar* b, DoublePacket<RealPacket>& dest,
-                            typename enable_if<unpacket_traits<RealPacket>::size==16>::type* = 0)
-{
+                            std::enable_if_t<unpacket_traits<RealPacket>::size == 16>* = 0) {
   // yes, that's pretty hackish too :(
   typedef typename NumTraits<Scalar>::Real RealScalar;
   RealScalar r[4] = {numext::real(b[0]), numext::real(b[0]), numext::real(b[1]), numext::real(b[1])};
   RealScalar i[4] = {numext::imag(b[0]), numext::imag(b[0]), numext::imag(b[1]), numext::imag(b[1])};
-  dest.first  = ploadquad<RealPacket>(r);
+  dest.first = ploadquad<RealPacket>(r);
   dest.second = ploadquad<RealPacket>(i);
 }
 
-
-template<typename Packet> struct unpacket_traits<DoublePacket<Packet> > {
+template <typename Packet>
+struct unpacket_traits<DoublePacket<Packet> > {
   typedef DoublePacket<typename unpacket_traits<Packet>::half> half;
+  enum { size = 2 * unpacket_traits<Packet>::size };
 };
 // template<typename Packet>
 // DoublePacket<Packet> pmadd(const DoublePacket<Packet> &a, const DoublePacket<Packet> &b)
@@ -754,30 +696,28 @@
 //   return res;
 // }
 
-template<typename RealScalar, bool _ConjLhs, bool _ConjRhs, int Arch, int _PacketSize>
-class gebp_traits<std::complex<RealScalar>, std::complex<RealScalar>, _ConjLhs, _ConjRhs, Arch, _PacketSize >
-{
-public:
-  typedef std::complex<RealScalar>  Scalar;
-  typedef std::complex<RealScalar>  LhsScalar;
-  typedef std::complex<RealScalar>  RhsScalar;
-  typedef std::complex<RealScalar>  ResScalar;
-  
-  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
-  PACKET_DECL_COND(Real, _PacketSize);
-  PACKET_DECL_COND_SCALAR(_PacketSize);
+template <typename RealScalar, bool ConjLhs_, bool ConjRhs_, int Arch, int PacketSize_>
+class gebp_traits<std::complex<RealScalar>, std::complex<RealScalar>, ConjLhs_, ConjRhs_, Arch, PacketSize_> {
+ public:
+  typedef std::complex<RealScalar> Scalar;
+  typedef std::complex<RealScalar> LhsScalar;
+  typedef std::complex<RealScalar> RhsScalar;
+  typedef std::complex<RealScalar> ResScalar;
+
+  PACKET_DECL_COND_POSTFIX(_, Lhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Rhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Res, PacketSize_);
+  PACKET_DECL_COND(Real, PacketSize_);
+  PACKET_DECL_COND_SCALAR(PacketSize_);
 
   enum {
-    ConjLhs = _ConjLhs,
-    ConjRhs = _ConjRhs,
-    Vectorizable = unpacket_traits<RealPacket>::vectorizable
-                && unpacket_traits<ScalarPacket>::vectorizable,
-    ResPacketSize   = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
-    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
+    ConjLhs = ConjLhs_,
+    ConjRhs = ConjRhs_,
+    Vectorizable = unpacket_traits<RealPacket>::vectorizable && unpacket_traits<ScalarPacket>::vectorizable,
+    ResPacketSize = Vectorizable ? unpacket_traits<ResPacket_>::size : 1,
+    LhsPacketSize = Vectorizable ? unpacket_traits<LhsPacket_>::size : 1,
     RhsPacketSize = Vectorizable ? unpacket_traits<RhsScalar>::size : 1,
-    RealPacketSize  = Vectorizable ? unpacket_traits<RealPacket>::size : 1,
+    RealPacketSize = Vectorizable ? unpacket_traits<RealPacket>::size : 1,
 
     // FIXME: should depend on NumberOfRegisters
     nr = 4,
@@ -786,42 +726,36 @@
     LhsProgress = ResPacketSize,
     RhsProgress = 1
   };
-  
-  typedef DoublePacket<RealPacket>                 DoublePacketType;
 
-  typedef typename conditional<Vectorizable,ScalarPacket,Scalar>::type LhsPacket4Packing;
-  typedef typename conditional<Vectorizable,RealPacket,  Scalar>::type LhsPacket;
-  typedef typename conditional<Vectorizable,DoublePacketType,Scalar>::type RhsPacket;
-  typedef typename conditional<Vectorizable,ScalarPacket,Scalar>::type ResPacket;
-  typedef typename conditional<Vectorizable,DoublePacketType,Scalar>::type AccPacket;
+  typedef DoublePacket<RealPacket> DoublePacketType;
 
-  // this actualy holds 8 packets!
+  typedef std::conditional_t<Vectorizable, ScalarPacket, Scalar> LhsPacket4Packing;
+  typedef std::conditional_t<Vectorizable, RealPacket, Scalar> LhsPacket;
+  typedef std::conditional_t<Vectorizable, DoublePacketType, Scalar> RhsPacket;
+  typedef std::conditional_t<Vectorizable, ScalarPacket, Scalar> ResPacket;
+  typedef std::conditional_t<Vectorizable, DoublePacketType, Scalar> AccPacket;
+
+  // this actually holds 8 packets!
   typedef QuadPacket<RhsPacket> RhsPacketx4;
-  
+
   EIGEN_STRONG_INLINE void initAcc(Scalar& p) { p = Scalar(0); }
 
-  EIGEN_STRONG_INLINE void initAcc(DoublePacketType& p)
-  {
-    p.first   = pset1<RealPacket>(RealScalar(0));
-    p.second  = pset1<RealPacket>(RealScalar(0));
+  EIGEN_STRONG_INLINE void initAcc(DoublePacketType& p) {
+    p.first = pset1<RealPacket>(RealScalar(0));
+    p.second = pset1<RealPacket>(RealScalar(0));
   }
 
   // Scalar path
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, ScalarPacket& dest) const
-  {
-    dest = pset1<ScalarPacket>(*b);
-  }
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, ScalarPacket& dest) const { dest = pset1<ScalarPacket>(*b); }
 
   // Vectorized path
-  template<typename RealPacketType>
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, DoublePacket<RealPacketType>& dest) const
-  {
-    dest.first  = pset1<RealPacketType>(numext::real(*b));
+  template <typename RealPacketType>
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, DoublePacket<RealPacketType>& dest) const {
+    dest.first = pset1<RealPacketType>(numext::real(*b));
     dest.second = pset1<RealPacketType>(numext::imag(*b));
   }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
-  {
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const {
     loadRhs(b, dest.B_0);
     loadRhs(b + 1, dest.B1);
     loadRhs(b + 2, dest.B2);
@@ -829,221 +763,189 @@
   }
 
   // Scalar path
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, ScalarPacket& dest) const
-  {
-    loadRhs(b, dest);
-  }
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, ScalarPacket& dest) const { loadRhs(b, dest); }
 
   // Vectorized path
-  template<typename RealPacketType>
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, DoublePacket<RealPacketType>& dest) const
-  {
+  template <typename RealPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, DoublePacket<RealPacketType>& dest) const {
     loadRhs(b, dest);
   }
 
   EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
-  
-  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, ResPacket& dest) const
-  {
-    loadRhs(b,dest);
-  }
-  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, DoublePacketType& dest) const
-  {
-    loadQuadToDoublePacket(b,dest);
+
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, ResPacket& dest) const { loadRhs(b, dest); }
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, DoublePacketType& dest) const {
+    loadQuadToDoublePacket(b, dest);
   }
 
   // nothing special here
-  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacket& dest) const
-  {
+  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacket& dest) const {
     dest = pload<LhsPacket>((const typename unpacket_traits<LhsPacket>::type*)(a));
   }
 
-  template<typename LhsPacketType>
-  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const
-  {
+  template <typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const {
     dest = ploadu<LhsPacketType>((const typename unpacket_traits<LhsPacketType>::type*)(a));
   }
 
-  template<typename LhsPacketType, typename RhsPacketType, typename ResPacketType, typename TmpType, typename LaneIdType>
-  EIGEN_STRONG_INLINE
-  typename enable_if<!is_same<RhsPacketType,RhsPacketx4>::value>::type
-  madd(const LhsPacketType& a, const RhsPacketType& b, DoublePacket<ResPacketType>& c, TmpType& /*tmp*/, const LaneIdType&) const
-  {
-    c.first   = padd(pmul(a,b.first), c.first);
-    c.second  = padd(pmul(a,b.second),c.second);
+  template <typename LhsPacketType, typename RhsPacketType, typename ResPacketType, typename TmpType,
+            typename LaneIdType>
+  EIGEN_STRONG_INLINE std::enable_if_t<!is_same<RhsPacketType, RhsPacketx4>::value> madd(const LhsPacketType& a,
+                                                                                         const RhsPacketType& b,
+                                                                                         DoublePacket<ResPacketType>& c,
+                                                                                         TmpType& /*tmp*/,
+                                                                                         const LaneIdType&) const {
+    c.first = padd(pmul(a, b.first), c.first);
+    c.second = padd(pmul(a, b.second), c.second);
   }
 
-  template<typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, ResPacket& c, RhsPacket& /*tmp*/, const LaneIdType&) const
-  {
-    c = cj.pmadd(a,b,c);
+  template <typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, ResPacket& c, RhsPacket& /*tmp*/,
+                                const LaneIdType&) const {
+    c = cj.pmadd(a, b, c);
   }
 
-  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
-  {
+  template <typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp,
+                                const LaneIdType& lane) const {
     madd(a, b.get(lane), c, tmp, lane);
   }
-  
+
   EIGEN_STRONG_INLINE void acc(const Scalar& c, const Scalar& alpha, Scalar& r) const { r += alpha * c; }
-  
-  template<typename RealPacketType, typename ResPacketType>
-  EIGEN_STRONG_INLINE void acc(const DoublePacket<RealPacketType>& c, const ResPacketType& alpha, ResPacketType& r) const
-  {
+
+  template <typename RealPacketType, typename ResPacketType>
+  EIGEN_STRONG_INLINE void acc(const DoublePacket<RealPacketType>& c, const ResPacketType& alpha,
+                               ResPacketType& r) const {
     // assemble c
     ResPacketType tmp;
-    if((!ConjLhs)&&(!ConjRhs))
-    {
+    if ((!ConjLhs) && (!ConjRhs)) {
       tmp = pcplxflip(pconj(ResPacketType(c.second)));
-      tmp = padd(ResPacketType(c.first),tmp);
-    }
-    else if((!ConjLhs)&&(ConjRhs))
-    {
+      tmp = padd(ResPacketType(c.first), tmp);
+    } else if ((!ConjLhs) && (ConjRhs)) {
       tmp = pconj(pcplxflip(ResPacketType(c.second)));
-      tmp = padd(ResPacketType(c.first),tmp);
-    }
-    else if((ConjLhs)&&(!ConjRhs))
-    {
+      tmp = padd(ResPacketType(c.first), tmp);
+    } else if ((ConjLhs) && (!ConjRhs)) {
       tmp = pcplxflip(ResPacketType(c.second));
-      tmp = padd(pconj(ResPacketType(c.first)),tmp);
-    }
-    else if((ConjLhs)&&(ConjRhs))
-    {
+      tmp = padd(pconj(ResPacketType(c.first)), tmp);
+    } else if ((ConjLhs) && (ConjRhs)) {
       tmp = pcplxflip(ResPacketType(c.second));
-      tmp = psub(pconj(ResPacketType(c.first)),tmp);
+      tmp = psub(pconj(ResPacketType(c.first)), tmp);
     }
-    
-    r = pmadd(tmp,alpha,r);
+
+    r = pmadd(tmp, alpha, r);
   }
 
-protected:
-  conj_helper<LhsScalar,RhsScalar,ConjLhs,ConjRhs> cj;
+ protected:
+  conj_helper<LhsScalar, RhsScalar, ConjLhs, ConjRhs> cj;
 };
 
-template<typename RealScalar, bool _ConjRhs, int Arch, int _PacketSize>
-class gebp_traits<RealScalar, std::complex<RealScalar>, false, _ConjRhs, Arch, _PacketSize >
-{
-public:
-  typedef std::complex<RealScalar>  Scalar;
-  typedef RealScalar  LhsScalar;
-  typedef Scalar      RhsScalar;
-  typedef Scalar      ResScalar;
+template <typename RealScalar, bool ConjRhs_, int Arch, int PacketSize_>
+class gebp_traits<RealScalar, std::complex<RealScalar>, false, ConjRhs_, Arch, PacketSize_> {
+ public:
+  typedef std::complex<RealScalar> Scalar;
+  typedef RealScalar LhsScalar;
+  typedef Scalar RhsScalar;
+  typedef Scalar ResScalar;
 
-  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Real, _PacketSize);
-  PACKET_DECL_COND_SCALAR_PREFIX(_, _PacketSize);
+  PACKET_DECL_COND_POSTFIX(_, Lhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Rhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Res, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Real, PacketSize_);
+  PACKET_DECL_COND_SCALAR_POSTFIX(_, PacketSize_);
 
-#undef PACKET_DECL_COND_SCALAR_PREFIX
-#undef PACKET_DECL_COND_PREFIX
+#undef PACKET_DECL_COND_SCALAR_POSTFIX
+#undef PACKET_DECL_COND_POSTFIX
 #undef PACKET_DECL_COND_SCALAR
 #undef PACKET_DECL_COND
 
   enum {
     ConjLhs = false,
-    ConjRhs = _ConjRhs,
-    Vectorizable = unpacket_traits<_RealPacket>::vectorizable
-                && unpacket_traits<_ScalarPacket>::vectorizable,
-    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
-    RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
-    ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
-    
+    ConjRhs = ConjRhs_,
+    Vectorizable = unpacket_traits<RealPacket_>::vectorizable && unpacket_traits<ScalarPacket_>::vectorizable,
+    LhsPacketSize = Vectorizable ? unpacket_traits<LhsPacket_>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<RhsPacket_>::size : 1,
+    ResPacketSize = Vectorizable ? unpacket_traits<ResPacket_>::size : 1,
+
     NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
     // FIXME: should depend on NumberOfRegisters
     nr = 4,
-    mr = (EIGEN_PLAIN_ENUM_MIN(16,NumberOfRegisters)/2/nr)*ResPacketSize,
+    mr = (plain_enum_min(16, NumberOfRegisters) / 2 / nr) * ResPacketSize,
 
     LhsProgress = ResPacketSize,
     RhsProgress = 1
   };
 
-  typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
-  typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
-  typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+  typedef std::conditional_t<Vectorizable, LhsPacket_, LhsScalar> LhsPacket;
+  typedef std::conditional_t<Vectorizable, RhsPacket_, RhsScalar> RhsPacket;
+  typedef std::conditional_t<Vectorizable, ResPacket_, ResScalar> ResPacket;
   typedef LhsPacket LhsPacket4Packing;
   typedef QuadPacket<RhsPacket> RhsPacketx4;
   typedef ResPacket AccPacket;
 
-  EIGEN_STRONG_INLINE void initAcc(AccPacket& p)
-  {
-    p = pset1<ResPacket>(ResScalar(0));
-  }
+  EIGEN_STRONG_INLINE void initAcc(AccPacket& p) { p = pset1<ResPacket>(ResScalar(0)); }
 
-  template<typename RhsPacketType>
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const
-  {
+  template <typename RhsPacketType>
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const {
     dest = pset1<RhsPacketType>(*b);
   }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
-  {
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const {
     pbroadcast4(b, dest.B_0, dest.B1, dest.B2, dest.B3);
   }
 
-  template<typename RhsPacketType>
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const
-  {
+  template <typename RhsPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const {
     loadRhs(b, dest);
   }
 
-  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
-  {}
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
 
-  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacket& dest) const
-  {
-    dest = ploaddup<LhsPacket>(a);
-  }
-  
-  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
-  {
-    dest = ploadquad<RhsPacket>(b);
-  }
+  EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacket& dest) const { dest = ploaddup<LhsPacket>(a); }
 
-  template<typename LhsPacketType>
-  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const
-  {
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const { dest = ploadquad<RhsPacket>(b); }
+
+  template <typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const {
     dest = ploaddup<LhsPacketType>(a);
   }
 
   template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType, typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const LaneIdType&) const
-  {
-    madd_impl(a, b, c, tmp, typename conditional<Vectorizable,true_type,false_type>::type());
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp,
+                                const LaneIdType&) const {
+    madd_impl(a, b, c, tmp, std::conditional_t<Vectorizable, true_type, false_type>());
   }
 
   template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType>
-  EIGEN_STRONG_INLINE void madd_impl(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const true_type&) const
-  {
+  EIGEN_STRONG_INLINE void madd_impl(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c,
+                                     RhsPacketType& tmp, const true_type&) const {
 #ifdef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
     EIGEN_UNUSED_VARIABLE(tmp);
-    c.v = pmadd(a,b.v,c.v);
+    c.v = pmadd(a, b.v, c.v);
 #else
-    tmp = b; tmp.v = pmul(a,tmp.v); c = padd(c,tmp);
+    tmp = b;
+    tmp.v = pmul(a, tmp.v);
+    c = padd(c, tmp);
 #endif
-    
   }
 
-  EIGEN_STRONG_INLINE void madd_impl(const LhsScalar& a, const RhsScalar& b, ResScalar& c, RhsScalar& /*tmp*/, const false_type&) const
-  {
+  EIGEN_STRONG_INLINE void madd_impl(const LhsScalar& a, const RhsScalar& b, ResScalar& c, RhsScalar& /*tmp*/,
+                                     const false_type&) const {
     c += a * b;
   }
 
-  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
-  {
+  template <typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp,
+                                const LaneIdType& lane) const {
     madd(a, b.get(lane), c, tmp, lane);
   }
 
   template <typename ResPacketType, typename AccPacketType>
-  EIGEN_STRONG_INLINE void acc(const AccPacketType& c, const ResPacketType& alpha, ResPacketType& r) const
-  {
-    conj_helper<ResPacketType,ResPacketType,false,ConjRhs> cj;
-    r = cj.pmadd(alpha,c,r);
+  EIGEN_STRONG_INLINE void acc(const AccPacketType& c, const ResPacketType& alpha, ResPacketType& r) const {
+    conj_helper<ResPacketType, ResPacketType, false, ConjRhs> cj;
+    r = cj.pmadd(alpha, c, r);
   }
 
-protected:
-
+ protected:
 };
 
 /* optimized General packed Block * packed Panel product kernel
@@ -1053,13 +955,15 @@
  *  |real |cplx | no vectorization yet, would require to pack A with duplication
  *  |cplx |real | easy vectorization
  */
-template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs>
-struct gebp_kernel
-{
-  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target> Traits;
-  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target,GEBPPacketHalf> HalfTraits;
-  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target,GEBPPacketQuarter> QuarterTraits;
-  
+template <typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr,
+          bool ConjugateLhs, bool ConjugateRhs>
+struct gebp_kernel {
+  typedef gebp_traits<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs, Architecture::Target> Traits;
+  typedef gebp_traits<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs, Architecture::Target, GEBPPacketHalf>
+      HalfTraits;
+  typedef gebp_traits<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs, Architecture::Target, GEBPPacketQuarter>
+      QuarterTraits;
+
   typedef typename Traits::ResScalar ResScalar;
   typedef typename Traits::LhsPacket LhsPacket;
   typedef typename Traits::RhsPacket RhsPacket;
@@ -1068,8 +972,9 @@
   typedef typename Traits::RhsPacketx4 RhsPacketx4;
 
   typedef typename RhsPanelHelper<RhsPacket, RhsPacketx4, 15>::type RhsPanel15;
+  typedef typename RhsPanelHelper<RhsPacket, RhsPacketx4, 27>::type RhsPanel27;
 
-  typedef gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target> SwappedTraits;
+  typedef gebp_traits<RhsScalar, LhsScalar, ConjugateRhs, ConjugateLhs, Architecture::Target> SwappedTraits;
 
   typedef typename SwappedTraits::ResScalar SResScalar;
   typedef typename SwappedTraits::LhsPacket SLhsPacket;
@@ -1090,28 +995,28 @@
   typedef typename DataMapper::LinearMapper LinearMapper;
 
   enum {
-    Vectorizable  = Traits::Vectorizable,
-    LhsProgress   = Traits::LhsProgress,
-    LhsProgressHalf      = HalfTraits::LhsProgress,
-    LhsProgressQuarter   = QuarterTraits::LhsProgress,
-    RhsProgress   = Traits::RhsProgress,
-    RhsProgressHalf      = HalfTraits::RhsProgress,
-    RhsProgressQuarter   = QuarterTraits::RhsProgress,
+    Vectorizable = Traits::Vectorizable,
+    LhsProgress = Traits::LhsProgress,
+    LhsProgressHalf = HalfTraits::LhsProgress,
+    LhsProgressQuarter = QuarterTraits::LhsProgress,
+    RhsProgress = Traits::RhsProgress,
+    RhsProgressHalf = HalfTraits::RhsProgress,
+    RhsProgressQuarter = QuarterTraits::RhsProgress,
     ResPacketSize = Traits::ResPacketSize
   };
 
-  EIGEN_DONT_INLINE
-  void operator()(const DataMapper& res, const LhsScalar* blockA, const RhsScalar* blockB,
-                  Index rows, Index depth, Index cols, ResScalar alpha,
-                  Index strideA=-1, Index strideB=-1, Index offsetA=0, Index offsetB=0);
+  EIGEN_DONT_INLINE void operator()(const DataMapper& res, const LhsScalar* blockA, const RhsScalar* blockB, Index rows,
+                                    Index depth, Index cols, ResScalar alpha, Index strideA = -1, Index strideB = -1,
+                                    Index offsetA = 0, Index offsetB = 0);
 };
 
-template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs,
-int SwappedLhsProgress = gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target>::LhsProgress>
-struct last_row_process_16_packets
-{
-  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target> Traits;
-  typedef gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target> SwappedTraits;
+template <typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr,
+          bool ConjugateLhs, bool ConjugateRhs,
+          int SwappedLhsProgress =
+              gebp_traits<RhsScalar, LhsScalar, ConjugateRhs, ConjugateLhs, Architecture::Target>::LhsProgress>
+struct last_row_process_16_packets {
+  typedef gebp_traits<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs, Architecture::Target> Traits;
+  typedef gebp_traits<RhsScalar, LhsScalar, ConjugateRhs, ConjugateLhs, Architecture::Target> SwappedTraits;
 
   typedef typename Traits::ResScalar ResScalar;
   typedef typename SwappedTraits::LhsPacket SLhsPacket;
@@ -1119,28 +1024,27 @@
   typedef typename SwappedTraits::ResPacket SResPacket;
   typedef typename SwappedTraits::AccPacket SAccPacket;
 
-  EIGEN_STRONG_INLINE void operator()(const DataMapper& res, SwappedTraits &straits, const LhsScalar* blA,
-                  const RhsScalar* blB, Index depth, const Index endk, Index i, Index j2,
-                  ResScalar alpha, SAccPacket &C0)
-    {
-      EIGEN_UNUSED_VARIABLE(res);
-      EIGEN_UNUSED_VARIABLE(straits);
-      EIGEN_UNUSED_VARIABLE(blA);
-      EIGEN_UNUSED_VARIABLE(blB);
-      EIGEN_UNUSED_VARIABLE(depth);
-      EIGEN_UNUSED_VARIABLE(endk);
-      EIGEN_UNUSED_VARIABLE(i);
-      EIGEN_UNUSED_VARIABLE(j2);
-      EIGEN_UNUSED_VARIABLE(alpha);
-      EIGEN_UNUSED_VARIABLE(C0);
-    }
+  EIGEN_STRONG_INLINE void operator()(const DataMapper& res, SwappedTraits& straits, const LhsScalar* blA,
+                                      const RhsScalar* blB, Index depth, const Index endk, Index i, Index j2,
+                                      ResScalar alpha, SAccPacket& C0) {
+    EIGEN_UNUSED_VARIABLE(res);
+    EIGEN_UNUSED_VARIABLE(straits);
+    EIGEN_UNUSED_VARIABLE(blA);
+    EIGEN_UNUSED_VARIABLE(blB);
+    EIGEN_UNUSED_VARIABLE(depth);
+    EIGEN_UNUSED_VARIABLE(endk);
+    EIGEN_UNUSED_VARIABLE(i);
+    EIGEN_UNUSED_VARIABLE(j2);
+    EIGEN_UNUSED_VARIABLE(alpha);
+    EIGEN_UNUSED_VARIABLE(C0);
+  }
 };
 
-
-template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs>
-struct last_row_process_16_packets<LhsScalar, RhsScalar, Index, DataMapper,  mr,  nr, ConjugateLhs,  ConjugateRhs, 16> {
-  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target> Traits;
-  typedef gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target> SwappedTraits;
+template <typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr,
+          bool ConjugateLhs, bool ConjugateRhs>
+struct last_row_process_16_packets<LhsScalar, RhsScalar, Index, DataMapper, mr, nr, ConjugateLhs, ConjugateRhs, 16> {
+  typedef gebp_traits<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs, Architecture::Target> Traits;
+  typedef gebp_traits<RhsScalar, LhsScalar, ConjugateRhs, ConjugateLhs, Architecture::Target> SwappedTraits;
 
   typedef typename Traits::ResScalar ResScalar;
   typedef typename SwappedTraits::LhsPacket SLhsPacket;
@@ -1148,10 +1052,9 @@
   typedef typename SwappedTraits::ResPacket SResPacket;
   typedef typename SwappedTraits::AccPacket SAccPacket;
 
-  EIGEN_STRONG_INLINE void operator()(const DataMapper& res, SwappedTraits &straits, const LhsScalar* blA,
-                  const RhsScalar* blB, Index depth, const Index endk, Index i, Index j2,
-                  ResScalar alpha, SAccPacket &C0)
-  {
+  EIGEN_STRONG_INLINE void operator()(const DataMapper& res, SwappedTraits& straits, const LhsScalar* blA,
+                                      const RhsScalar* blB, Index depth, const Index endk, Index i, Index j2,
+                                      ResScalar alpha, SAccPacket& C0) {
     typedef typename unpacket_traits<typename unpacket_traits<SResPacket>::half>::half SResPacketQuarter;
     typedef typename unpacket_traits<typename unpacket_traits<SLhsPacket>::half>::half SLhsPacketQuarter;
     typedef typename unpacket_traits<typename unpacket_traits<SRhsPacket>::half>::half SRhsPacketQuarter;
@@ -1160,71 +1063,190 @@
     SResPacketQuarter R = res.template gatherPacket<SResPacketQuarter>(i, j2);
     SResPacketQuarter alphav = pset1<SResPacketQuarter>(alpha);
 
-    if (depth - endk > 0)
-      {
-	// We have to handle the last row(s) of the rhs, which
-	// correspond to a half-packet
-	SAccPacketQuarter c0 = predux_half_dowto4(predux_half_dowto4(C0));
+    if (depth - endk > 0) {
+      // We have to handle the last row(s) of the rhs, which
+      // correspond to a half-packet
+      SAccPacketQuarter c0 = predux_half_dowto4(predux_half_dowto4(C0));
 
-	for (Index kk = endk; kk < depth; kk++)
-	  {
-	    SLhsPacketQuarter a0;
-	    SRhsPacketQuarter b0;
-	    straits.loadLhsUnaligned(blB, a0);
-	    straits.loadRhs(blA, b0);
-	    straits.madd(a0,b0,c0,b0, fix<0>);
-	    blB += SwappedTraits::LhsProgress/4;
-	    blA += 1;
-	  }
-	straits.acc(c0, alphav, R);
+      for (Index kk = endk; kk < depth; kk++) {
+        SLhsPacketQuarter a0;
+        SRhsPacketQuarter b0;
+        straits.loadLhsUnaligned(blB, a0);
+        straits.loadRhs(blA, b0);
+        straits.madd(a0, b0, c0, b0, fix<0>);
+        blB += SwappedTraits::LhsProgress / 4;
+        blA += 1;
       }
-    else
-      {
-	straits.acc(predux_half_dowto4(predux_half_dowto4(C0)), alphav, R);
-      }
+      straits.acc(c0, alphav, R);
+    } else {
+      straits.acc(predux_half_dowto4(predux_half_dowto4(C0)), alphav, R);
+    }
     res.scatterPacket(i, j2, R);
   }
 };
 
-template<int nr, Index LhsProgress, Index RhsProgress, typename LhsScalar, typename RhsScalar, typename ResScalar, typename AccPacket, typename LhsPacket, typename RhsPacket, typename ResPacket, typename GEBPTraits, typename LinearMapper, typename DataMapper>
-struct lhs_process_one_packet
-{
+template <int nr, Index LhsProgress, Index RhsProgress, typename LhsScalar, typename RhsScalar, typename ResScalar,
+          typename AccPacket, typename LhsPacket, typename RhsPacket, typename ResPacket, typename GEBPTraits,
+          typename LinearMapper, typename DataMapper>
+struct lhs_process_one_packet {
   typedef typename GEBPTraits::RhsPacketx4 RhsPacketx4;
 
-  EIGEN_STRONG_INLINE void peeled_kc_onestep(Index K, const LhsScalar* blA, const RhsScalar* blB, GEBPTraits traits, LhsPacket *A0, RhsPacketx4 *rhs_panel, RhsPacket *T0, AccPacket *C0, AccPacket *C1, AccPacket *C2, AccPacket *C3)
-  {
+  EIGEN_STRONG_INLINE void peeled_kc_onestep(Index K, const LhsScalar* blA, const RhsScalar* blB, GEBPTraits traits,
+                                             LhsPacket* A0, RhsPacketx4* rhs_panel, RhsPacket* T0, AccPacket* C0,
+                                             AccPacket* C1, AccPacket* C2, AccPacket* C3) {
     EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1X4");
     EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!");
-    traits.loadLhs(&blA[(0+1*K)*LhsProgress], *A0);
-    traits.loadRhs(&blB[(0+4*K)*RhsProgress], *rhs_panel);
+    traits.loadLhs(&blA[(0 + 1 * K) * LhsProgress], *A0);
+    traits.loadRhs(&blB[(0 + 4 * K) * RhsProgress], *rhs_panel);
     traits.madd(*A0, *rhs_panel, *C0, *T0, fix<0>);
     traits.madd(*A0, *rhs_panel, *C1, *T0, fix<1>);
     traits.madd(*A0, *rhs_panel, *C2, *T0, fix<2>);
     traits.madd(*A0, *rhs_panel, *C3, *T0, fix<3>);
-    #if EIGEN_GNUC_AT_LEAST(6,0) && defined(EIGEN_VECTORIZE_SSE)
-    __asm__  ("" : "+x,m" (*A0));
-    #endif
+#if EIGEN_GNUC_STRICT_AT_LEAST(6, 0, 0) && defined(EIGEN_VECTORIZE_SSE) && !(EIGEN_COMP_LCC)
+    __asm__("" : "+x,m"(*A0));
+#endif
     EIGEN_ASM_COMMENT("end step of gebp micro kernel 1X4");
   }
 
-  EIGEN_STRONG_INLINE void operator()(
-    const DataMapper& res, const LhsScalar* blockA, const RhsScalar* blockB, ResScalar alpha,
-    Index peelStart, Index peelEnd, Index strideA, Index strideB, Index offsetA, Index offsetB,
-    int prefetch_res_offset, Index peeled_kc, Index pk, Index cols, Index depth, Index packet_cols4)
-  {
+  EIGEN_STRONG_INLINE void operator()(const DataMapper& res, const LhsScalar* blockA, const RhsScalar* blockB,
+                                      ResScalar alpha, Index peelStart, Index peelEnd, Index strideA, Index strideB,
+                                      Index offsetA, Index offsetB, int prefetch_res_offset, Index peeled_kc, Index pk,
+                                      Index cols, Index depth, Index packet_cols4) {
     GEBPTraits traits;
-
+    Index packet_cols8 = nr >= 8 ? (cols / 8) * 8 : 0;
     // loops on each largest micro horizontal panel of lhs
     // (LhsProgress x depth)
-    for(Index i=peelStart; i<peelEnd; i+=LhsProgress)
-    {
+    for (Index i = peelStart; i < peelEnd; i += LhsProgress) {
+#if EIGEN_ARCH_ARM64
+      EIGEN_IF_CONSTEXPR(nr >= 8) {
+        for (Index j2 = 0; j2 < packet_cols8; j2 += 8) {
+          const LhsScalar* blA = &blockA[i * strideA + offsetA * (LhsProgress)];
+          prefetch(&blA[0]);
+
+          // gets res block as register
+          AccPacket C0, C1, C2, C3, C4, C5, C6, C7;
+          traits.initAcc(C0);
+          traits.initAcc(C1);
+          traits.initAcc(C2);
+          traits.initAcc(C3);
+          traits.initAcc(C4);
+          traits.initAcc(C5);
+          traits.initAcc(C6);
+          traits.initAcc(C7);
+
+          LinearMapper r0 = res.getLinearMapper(i, j2 + 0);
+          LinearMapper r1 = res.getLinearMapper(i, j2 + 1);
+          LinearMapper r2 = res.getLinearMapper(i, j2 + 2);
+          LinearMapper r3 = res.getLinearMapper(i, j2 + 3);
+          LinearMapper r4 = res.getLinearMapper(i, j2 + 4);
+          LinearMapper r5 = res.getLinearMapper(i, j2 + 5);
+          LinearMapper r6 = res.getLinearMapper(i, j2 + 6);
+          LinearMapper r7 = res.getLinearMapper(i, j2 + 7);
+          r0.prefetch(prefetch_res_offset);
+          r1.prefetch(prefetch_res_offset);
+          r2.prefetch(prefetch_res_offset);
+          r3.prefetch(prefetch_res_offset);
+          r4.prefetch(prefetch_res_offset);
+          r5.prefetch(prefetch_res_offset);
+          r6.prefetch(prefetch_res_offset);
+          r7.prefetch(prefetch_res_offset);
+          const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 8];
+          prefetch(&blB[0]);
+
+          LhsPacket A0;
+          for (Index k = 0; k < peeled_kc; k += pk) {
+            RhsPacketx4 rhs_panel;
+            RhsPacket T0;
+#define EIGEN_GEBGP_ONESTEP(K)                                    \
+  do {                                                            \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1pX8");    \
+    traits.loadLhs(&blA[(0 + 1 * K) * LhsProgress], A0);          \
+    traits.loadRhs(&blB[(0 + 8 * K) * RhsProgress], rhs_panel);   \
+    traits.madd(A0, rhs_panel, C0, T0, fix<0>);                   \
+    traits.updateRhs(&blB[(1 + 8 * K) * RhsProgress], rhs_panel); \
+    traits.madd(A0, rhs_panel, C1, T0, fix<1>);                   \
+    traits.updateRhs(&blB[(2 + 8 * K) * RhsProgress], rhs_panel); \
+    traits.madd(A0, rhs_panel, C2, T0, fix<2>);                   \
+    traits.updateRhs(&blB[(3 + 8 * K) * RhsProgress], rhs_panel); \
+    traits.madd(A0, rhs_panel, C3, T0, fix<3>);                   \
+    traits.loadRhs(&blB[(4 + 8 * K) * RhsProgress], rhs_panel);   \
+    traits.madd(A0, rhs_panel, C4, T0, fix<0>);                   \
+    traits.updateRhs(&blB[(5 + 8 * K) * RhsProgress], rhs_panel); \
+    traits.madd(A0, rhs_panel, C5, T0, fix<1>);                   \
+    traits.updateRhs(&blB[(6 + 8 * K) * RhsProgress], rhs_panel); \
+    traits.madd(A0, rhs_panel, C6, T0, fix<2>);                   \
+    traits.updateRhs(&blB[(7 + 8 * K) * RhsProgress], rhs_panel); \
+    traits.madd(A0, rhs_panel, C7, T0, fix<3>);                   \
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 1pX8");      \
+  } while (false)
+
+            EIGEN_ASM_COMMENT("begin gebp micro kernel 1pX8");
+
+            EIGEN_GEBGP_ONESTEP(0);
+            EIGEN_GEBGP_ONESTEP(1);
+            EIGEN_GEBGP_ONESTEP(2);
+            EIGEN_GEBGP_ONESTEP(3);
+            EIGEN_GEBGP_ONESTEP(4);
+            EIGEN_GEBGP_ONESTEP(5);
+            EIGEN_GEBGP_ONESTEP(6);
+            EIGEN_GEBGP_ONESTEP(7);
+
+            blB += pk * 8 * RhsProgress;
+            blA += pk * (1 * LhsProgress);
+
+            EIGEN_ASM_COMMENT("end gebp micro kernel 1pX8");
+          }
+          // process remaining peeled loop
+          for (Index k = peeled_kc; k < depth; k++) {
+            RhsPacketx4 rhs_panel;
+            RhsPacket T0;
+            EIGEN_GEBGP_ONESTEP(0);
+            blB += 8 * RhsProgress;
+            blA += 1 * LhsProgress;
+          }
+
+#undef EIGEN_GEBGP_ONESTEP
+
+          ResPacket R0, R1;
+          ResPacket alphav = pset1<ResPacket>(alpha);
+
+          R0 = r0.template loadPacket<ResPacket>(0);
+          R1 = r1.template loadPacket<ResPacket>(0);
+          traits.acc(C0, alphav, R0);
+          traits.acc(C1, alphav, R1);
+          r0.storePacket(0, R0);
+          r1.storePacket(0, R1);
+
+          R0 = r2.template loadPacket<ResPacket>(0);
+          R1 = r3.template loadPacket<ResPacket>(0);
+          traits.acc(C2, alphav, R0);
+          traits.acc(C3, alphav, R1);
+          r2.storePacket(0, R0);
+          r3.storePacket(0, R1);
+
+          R0 = r4.template loadPacket<ResPacket>(0);
+          R1 = r5.template loadPacket<ResPacket>(0);
+          traits.acc(C4, alphav, R0);
+          traits.acc(C5, alphav, R1);
+          r4.storePacket(0, R0);
+          r5.storePacket(0, R1);
+
+          R0 = r6.template loadPacket<ResPacket>(0);
+          R1 = r7.template loadPacket<ResPacket>(0);
+          traits.acc(C6, alphav, R0);
+          traits.acc(C7, alphav, R1);
+          r6.storePacket(0, R0);
+          r7.storePacket(0, R1);
+        }
+      }
+#endif
+
       // loops on each largest micro vertical panel of rhs (depth * nr)
-      for(Index j2=0; j2<packet_cols4; j2+=nr)
-      {
+      for (Index j2 = packet_cols8; j2 < packet_cols4; j2 += 4) {
         // We select a LhsProgress x nr micro block of res
         // which is entirely stored into 1 x nr registers.
 
-        const LhsScalar* blA = &blockA[i*strideA+offsetA*(LhsProgress)];
+        const LhsScalar* blA = &blockA[i * strideA + offsetA * (LhsProgress)];
         prefetch(&blA[0]);
 
         // gets res block as register
@@ -1235,7 +1257,7 @@
         traits.initAcc(C3);
         // To improve instruction pipelining, let's double the accumulation registers:
         //  even k will accumulate in C*, while odd k will accumulate in D*.
-        // This trick is crutial to get good performance with FMA, otherwise it is 
+        // This trick is crutial to get good performance with FMA, otherwise it is
         // actually faster to perform separated MUL+ADD because of a naturally
         // better instruction-level parallelism.
         AccPacket D0, D1, D2, D3;
@@ -1255,44 +1277,42 @@
         r3.prefetch(prefetch_res_offset);
 
         // performs "inner" products
-        const RhsScalar* blB = &blockB[j2*strideB+offsetB*nr];
+        const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 4];
         prefetch(&blB[0]);
         LhsPacket A0, A1;
 
-        for(Index k=0; k<peeled_kc; k+=pk)
-        {
+        for (Index k = 0; k < peeled_kc; k += pk) {
           EIGEN_ASM_COMMENT("begin gebp micro kernel 1/half/quarterX4");
           RhsPacketx4 rhs_panel;
           RhsPacket T0;
 
-          internal::prefetch(blB+(48+0));
+          internal::prefetch(blB + (48 + 0));
           peeled_kc_onestep(0, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
           peeled_kc_onestep(1, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
           peeled_kc_onestep(2, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
           peeled_kc_onestep(3, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
-          internal::prefetch(blB+(48+16));
+          internal::prefetch(blB + (48 + 16));
           peeled_kc_onestep(4, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
           peeled_kc_onestep(5, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
           peeled_kc_onestep(6, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
           peeled_kc_onestep(7, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
 
-          blB += pk*4*RhsProgress;
-          blA += pk*LhsProgress;
+          blB += pk * 4 * RhsProgress;
+          blA += pk * LhsProgress;
 
           EIGEN_ASM_COMMENT("end gebp micro kernel 1/half/quarterX4");
         }
-        C0 = padd(C0,D0);
-        C1 = padd(C1,D1);
-        C2 = padd(C2,D2);
-        C3 = padd(C3,D3);
+        C0 = padd(C0, D0);
+        C1 = padd(C1, D1);
+        C2 = padd(C2, D2);
+        C3 = padd(C3, D3);
 
         // process remaining peeled loop
-        for(Index k=peeled_kc; k<depth; k++)
-        {
+        for (Index k = peeled_kc; k < depth; k++) {
           RhsPacketx4 rhs_panel;
           RhsPacket T0;
           peeled_kc_onestep(0, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
-          blB += 4*RhsProgress;
+          blB += 4 * RhsProgress;
           blA += LhsProgress;
         }
 
@@ -1302,23 +1322,22 @@
         R0 = r0.template loadPacket<ResPacket>(0);
         R1 = r1.template loadPacket<ResPacket>(0);
         traits.acc(C0, alphav, R0);
-        traits.acc(C1,  alphav, R1);
+        traits.acc(C1, alphav, R1);
         r0.storePacket(0, R0);
         r1.storePacket(0, R1);
 
         R0 = r2.template loadPacket<ResPacket>(0);
         R1 = r3.template loadPacket<ResPacket>(0);
-        traits.acc(C2,  alphav, R0);
-        traits.acc(C3,  alphav, R1);
+        traits.acc(C2, alphav, R0);
+        traits.acc(C3, alphav, R1);
         r2.storePacket(0, R0);
         r3.storePacket(0, R1);
       }
 
       // Deal with remaining columns of the rhs
-      for(Index j2=packet_cols4; j2<cols; j2++)
-      {
+      for (Index j2 = packet_cols4; j2 < cols; j2++) {
         // One column at a time
-        const LhsScalar* blA = &blockA[i*strideA+offsetA*(LhsProgress)];
+        const LhsScalar* blA = &blockA[i * strideA + offsetA * (LhsProgress)];
         prefetch(&blA[0]);
 
         // gets res block as register
@@ -1328,24 +1347,23 @@
         LinearMapper r0 = res.getLinearMapper(i, j2);
 
         // performs "inner" products
-        const RhsScalar* blB = &blockB[j2*strideB+offsetB];
+        const RhsScalar* blB = &blockB[j2 * strideB + offsetB];
         LhsPacket A0;
 
-        for(Index k= 0; k<peeled_kc; k+=pk)
-        {
+        for (Index k = 0; k < peeled_kc; k += pk) {
           EIGEN_ASM_COMMENT("begin gebp micro kernel 1/half/quarterX1");
           RhsPacket B_0;
 
-#define EIGEN_GEBGP_ONESTEP(K)                                          \
-	      do {                                                      \
-		EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1/half/quarterX1"); \
-		EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-    /* FIXME: why unaligned???? */ \
-		traits.loadLhsUnaligned(&blA[(0+1*K)*LhsProgress], A0); \
-		traits.loadRhs(&blB[(0+K)*RhsProgress], B_0);		\
-		traits.madd(A0, B_0, C0, B_0, fix<0>);				\
-		EIGEN_ASM_COMMENT("end step of gebp micro kernel 1/half/quarterX1"); \
-	      } while(false);
+#define EIGEN_GEBGP_ONESTEP(K)                                             \
+  do {                                                                     \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1/half/quarterX1"); \
+    EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!");    \
+    /* FIXME: why unaligned???? */                                         \
+    traits.loadLhsUnaligned(&blA[(0 + 1 * K) * LhsProgress], A0);          \
+    traits.loadRhs(&blB[(0 + K) * RhsProgress], B_0);                      \
+    traits.madd(A0, B_0, C0, B_0, fix<0>);                                 \
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 1/half/quarterX1");   \
+  } while (false);
 
           EIGEN_GEBGP_ONESTEP(0);
           EIGEN_GEBGP_ONESTEP(1);
@@ -1356,15 +1374,14 @@
           EIGEN_GEBGP_ONESTEP(6);
           EIGEN_GEBGP_ONESTEP(7);
 
-          blB += pk*RhsProgress;
-          blA += pk*LhsProgress;
+          blB += pk * RhsProgress;
+          blA += pk * LhsProgress;
 
           EIGEN_ASM_COMMENT("end gebp micro kernel 1/half/quarterX1");
         }
 
         // process remaining peeled loop
-        for(Index k=peeled_kc; k<depth; k++)
-        {
+        for (Index k = peeled_kc; k < depth; k++) {
           RhsPacket B_0;
           EIGEN_GEBGP_ONESTEP(0);
           blB += RhsProgress;
@@ -1381,84 +1398,321 @@
   }
 };
 
-template<int nr, Index LhsProgress, Index RhsProgress, typename LhsScalar, typename RhsScalar, typename ResScalar, typename AccPacket, typename LhsPacket, typename RhsPacket, typename ResPacket, typename GEBPTraits, typename LinearMapper, typename DataMapper>
-struct lhs_process_fraction_of_packet : lhs_process_one_packet<nr, LhsProgress, RhsProgress, LhsScalar, RhsScalar, ResScalar, AccPacket, LhsPacket, RhsPacket, ResPacket, GEBPTraits, LinearMapper, DataMapper>
-{
-
-EIGEN_STRONG_INLINE void peeled_kc_onestep(Index K, const LhsScalar* blA, const RhsScalar* blB, GEBPTraits traits, LhsPacket *A0, RhsPacket *B_0, RhsPacket *B1, RhsPacket *B2, RhsPacket *B3, AccPacket *C0, AccPacket *C1, AccPacket *C2, AccPacket *C3)
-  {
-        EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1X4");
-        EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!");
-        traits.loadLhsUnaligned(&blA[(0+1*K)*(LhsProgress)], *A0);
-        traits.broadcastRhs(&blB[(0+4*K)*RhsProgress], *B_0, *B1, *B2, *B3);
-        traits.madd(*A0, *B_0, *C0, *B_0);
-        traits.madd(*A0, *B1,  *C1, *B1);
-        traits.madd(*A0, *B2,  *C2, *B2);
-        traits.madd(*A0, *B3,  *C3, *B3);
-        EIGEN_ASM_COMMENT("end step of gebp micro kernel 1X4");
+template <int nr, Index LhsProgress, Index RhsProgress, typename LhsScalar, typename RhsScalar, typename ResScalar,
+          typename AccPacket, typename LhsPacket, typename RhsPacket, typename ResPacket, typename GEBPTraits,
+          typename LinearMapper, typename DataMapper>
+struct lhs_process_fraction_of_packet
+    : lhs_process_one_packet<nr, LhsProgress, RhsProgress, LhsScalar, RhsScalar, ResScalar, AccPacket, LhsPacket,
+                             RhsPacket, ResPacket, GEBPTraits, LinearMapper, DataMapper> {
+  EIGEN_STRONG_INLINE void peeled_kc_onestep(Index K, const LhsScalar* blA, const RhsScalar* blB, GEBPTraits traits,
+                                             LhsPacket* A0, RhsPacket* B_0, RhsPacket* B1, RhsPacket* B2, RhsPacket* B3,
+                                             AccPacket* C0, AccPacket* C1, AccPacket* C2, AccPacket* C3) {
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1X4");
+    EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!");
+    traits.loadLhsUnaligned(&blA[(0 + 1 * K) * (LhsProgress)], *A0);
+    traits.broadcastRhs(&blB[(0 + 4 * K) * RhsProgress], *B_0, *B1, *B2, *B3);
+    traits.madd(*A0, *B_0, *C0, *B_0);
+    traits.madd(*A0, *B1, *C1, *B1);
+    traits.madd(*A0, *B2, *C2, *B2);
+    traits.madd(*A0, *B3, *C3, *B3);
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 1X4");
   }
 };
 
-template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs>
-EIGEN_DONT_INLINE
-void gebp_kernel<LhsScalar,RhsScalar,Index,DataMapper,mr,nr,ConjugateLhs,ConjugateRhs>
-  ::operator()(const DataMapper& res, const LhsScalar* blockA, const RhsScalar* blockB,
-               Index rows, Index depth, Index cols, ResScalar alpha,
-               Index strideA, Index strideB, Index offsetA, Index offsetB)
-  {
-    Traits traits;
-    SwappedTraits straits;
-    
-    if(strideA==-1) strideA = depth;
-    if(strideB==-1) strideB = depth;
-    conj_helper<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs> cj;
-    Index packet_cols4 = nr>=4 ? (cols/4) * 4 : 0;
-    const Index peeled_mc3 = mr>=3*Traits::LhsProgress ? (rows/(3*LhsProgress))*(3*LhsProgress) : 0;
-    const Index peeled_mc2 = mr>=2*Traits::LhsProgress ? peeled_mc3+((rows-peeled_mc3)/(2*LhsProgress))*(2*LhsProgress) : 0;
-    const Index peeled_mc1 = mr>=1*Traits::LhsProgress ? peeled_mc2+((rows-peeled_mc2)/(1*LhsProgress))*(1*LhsProgress) : 0;
-    const Index peeled_mc_half = mr>=LhsProgressHalf ? peeled_mc1+((rows-peeled_mc1)/(LhsProgressHalf))*(LhsProgressHalf) : 0;
-    const Index peeled_mc_quarter = mr>=LhsProgressQuarter ? peeled_mc_half+((rows-peeled_mc_half)/(LhsProgressQuarter))*(LhsProgressQuarter) : 0;
-    enum { pk = 8 }; // NOTE Such a large peeling factor is important for large matrices (~ +5% when >1000 on Haswell)
-    const Index peeled_kc  = depth & ~(pk-1);
-    const int prefetch_res_offset = 32/sizeof(ResScalar);    
-//     const Index depth2     = depth & ~1;
+template <typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr,
+          bool ConjugateLhs, bool ConjugateRhs>
+EIGEN_DONT_INLINE void gebp_kernel<LhsScalar, RhsScalar, Index, DataMapper, mr, nr, ConjugateLhs,
+                                   ConjugateRhs>::operator()(const DataMapper& res, const LhsScalar* blockA,
+                                                             const RhsScalar* blockB, Index rows, Index depth,
+                                                             Index cols, ResScalar alpha, Index strideA, Index strideB,
+                                                             Index offsetA, Index offsetB) {
+  Traits traits;
+  SwappedTraits straits;
 
-    //---------- Process 3 * LhsProgress rows at once ----------
-    // This corresponds to 3*LhsProgress x nr register blocks.
-    // Usually, make sense only with FMA
-    if(mr>=3*Traits::LhsProgress)
-    {
-      // Here, the general idea is to loop on each largest micro horizontal panel of the lhs (3*Traits::LhsProgress x depth)
-      // and on each largest micro vertical panel of the rhs (depth * nr).
-      // Blocking sizes, i.e., 'depth' has been computed so that the micro horizontal panel of the lhs fit in L1.
-      // However, if depth is too small, we can extend the number of rows of these horizontal panels.
-      // This actual number of rows is computed as follow:
-      const Index l1 = defaultL1CacheSize; // in Bytes, TODO, l1 should be passed to this function.
-      // The max(1, ...) here is needed because we may be using blocking params larger than what our known l1 cache size
-      // suggests we should be using: either because our known l1 cache size is inaccurate (e.g. on Android, we can only guess),
-      // or because we are testing specific blocking sizes.
-      const Index actual_panel_rows = (3*LhsProgress) * std::max<Index>(1,( (l1 - sizeof(ResScalar)*mr*nr - depth*nr*sizeof(RhsScalar)) / (depth * sizeof(LhsScalar) * 3*LhsProgress) ));
-      for(Index i1=0; i1<peeled_mc3; i1+=actual_panel_rows)
-      {
-        const Index actual_panel_end = (std::min)(i1+actual_panel_rows, peeled_mc3);
-        for(Index j2=0; j2<packet_cols4; j2+=nr)
-        {
-          for(Index i=i1; i<actual_panel_end; i+=3*LhsProgress)
-          {
-          
+  if (strideA == -1) strideA = depth;
+  if (strideB == -1) strideB = depth;
+  conj_helper<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs> cj;
+  Index packet_cols4 = nr >= 4 ? (cols / 4) * 4 : 0;
+  Index packet_cols8 = nr >= 8 ? (cols / 8) * 8 : 0;
+  const Index peeled_mc3 = mr >= 3 * Traits::LhsProgress ? (rows / (3 * LhsProgress)) * (3 * LhsProgress) : 0;
+  const Index peeled_mc2 =
+      mr >= 2 * Traits::LhsProgress ? peeled_mc3 + ((rows - peeled_mc3) / (2 * LhsProgress)) * (2 * LhsProgress) : 0;
+  const Index peeled_mc1 =
+      mr >= 1 * Traits::LhsProgress ? peeled_mc2 + ((rows - peeled_mc2) / (1 * LhsProgress)) * (1 * LhsProgress) : 0;
+  const Index peeled_mc_half =
+      mr >= LhsProgressHalf ? peeled_mc1 + ((rows - peeled_mc1) / (LhsProgressHalf)) * (LhsProgressHalf) : 0;
+  const Index peeled_mc_quarter =
+      mr >= LhsProgressQuarter
+          ? peeled_mc_half + ((rows - peeled_mc_half) / (LhsProgressQuarter)) * (LhsProgressQuarter)
+          : 0;
+  enum { pk = 8 };  // NOTE Such a large peeling factor is important for large matrices (~ +5% when >1000 on Haswell)
+  const Index peeled_kc = depth & ~(pk - 1);
+  const int prefetch_res_offset = 32 / sizeof(ResScalar);
+  //     const Index depth2     = depth & ~1;
+
+  //---------- Process 3 * LhsProgress rows at once ----------
+  // This corresponds to 3*LhsProgress x nr register blocks.
+  // Usually, make sense only with FMA
+  if (mr >= 3 * Traits::LhsProgress) {
+    // Here, the general idea is to loop on each largest micro horizontal panel of the lhs (3*Traits::LhsProgress x
+    // depth) and on each largest micro vertical panel of the rhs (depth * nr). Blocking sizes, i.e., 'depth' has been
+    // computed so that the micro horizontal panel of the lhs fit in L1. However, if depth is too small, we can extend
+    // the number of rows of these horizontal panels. This actual number of rows is computed as follow:
+    const Index l1 = defaultL1CacheSize;  // in Bytes, TODO, l1 should be passed to this function.
+    // The max(1, ...) here is needed because we may be using blocking params larger than what our known l1 cache size
+    // suggests we should be using: either because our known l1 cache size is inaccurate (e.g. on Android, we can only
+    // guess), or because we are testing specific blocking sizes.
+    const Index actual_panel_rows =
+        (3 * LhsProgress) * std::max<Index>(1, ((l1 - sizeof(ResScalar) * mr * nr - depth * nr * sizeof(RhsScalar)) /
+                                                (depth * sizeof(LhsScalar) * 3 * LhsProgress)));
+    for (Index i1 = 0; i1 < peeled_mc3; i1 += actual_panel_rows) {
+      const Index actual_panel_end = (std::min)(i1 + actual_panel_rows, peeled_mc3);
+#if EIGEN_ARCH_ARM64
+      EIGEN_IF_CONSTEXPR(nr >= 8) {
+        for (Index j2 = 0; j2 < packet_cols8; j2 += 8) {
+          for (Index i = i1; i < actual_panel_end; i += 3 * LhsProgress) {
+            const LhsScalar* blA = &blockA[i * strideA + offsetA * (3 * LhsProgress)];
+            prefetch(&blA[0]);
+            // gets res block as register
+            AccPacket C0, C1, C2, C3, C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20,
+                C21, C22, C23;
+            traits.initAcc(C0);
+            traits.initAcc(C1);
+            traits.initAcc(C2);
+            traits.initAcc(C3);
+            traits.initAcc(C4);
+            traits.initAcc(C5);
+            traits.initAcc(C6);
+            traits.initAcc(C7);
+            traits.initAcc(C8);
+            traits.initAcc(C9);
+            traits.initAcc(C10);
+            traits.initAcc(C11);
+            traits.initAcc(C12);
+            traits.initAcc(C13);
+            traits.initAcc(C14);
+            traits.initAcc(C15);
+            traits.initAcc(C16);
+            traits.initAcc(C17);
+            traits.initAcc(C18);
+            traits.initAcc(C19);
+            traits.initAcc(C20);
+            traits.initAcc(C21);
+            traits.initAcc(C22);
+            traits.initAcc(C23);
+
+            LinearMapper r0 = res.getLinearMapper(i, j2 + 0);
+            LinearMapper r1 = res.getLinearMapper(i, j2 + 1);
+            LinearMapper r2 = res.getLinearMapper(i, j2 + 2);
+            LinearMapper r3 = res.getLinearMapper(i, j2 + 3);
+            LinearMapper r4 = res.getLinearMapper(i, j2 + 4);
+            LinearMapper r5 = res.getLinearMapper(i, j2 + 5);
+            LinearMapper r6 = res.getLinearMapper(i, j2 + 6);
+            LinearMapper r7 = res.getLinearMapper(i, j2 + 7);
+
+            r0.prefetch(0);
+            r1.prefetch(0);
+            r2.prefetch(0);
+            r3.prefetch(0);
+            r4.prefetch(0);
+            r5.prefetch(0);
+            r6.prefetch(0);
+            r7.prefetch(0);
+
+            // performs "inner" products
+            const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 8];
+            prefetch(&blB[0]);
+            LhsPacket A0, A1;
+            for (Index k = 0; k < peeled_kc; k += pk) {
+              EIGEN_ASM_COMMENT("begin gebp micro kernel 3pX8");
+              // 27 registers are taken (24 for acc, 3 for lhs).
+              RhsPanel27 rhs_panel;
+              RhsPacket T0;
+              LhsPacket A2;
+#if EIGEN_ARCH_ARM64 && defined(EIGEN_VECTORIZE_NEON) && EIGEN_GNUC_STRICT_LESS_THAN(9, 0, 0)
+// see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1633
+// without this workaround A0, A1, and A2 are loaded in the same register,
+// which is not good for pipelining
+#define EIGEN_GEBP_3Px8_REGISTER_ALLOC_WORKAROUND __asm__("" : "+w,m"(A0), "+w,m"(A1), "+w,m"(A2));
+#else
+#define EIGEN_GEBP_3Px8_REGISTER_ALLOC_WORKAROUND
+#endif
+
+#define EIGEN_GEBP_ONESTEP(K)                                                                                     \
+  do {                                                                                                            \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX8");                                                    \
+    traits.loadLhs(&blA[(0 + 3 * K) * LhsProgress], A0);                                                          \
+    traits.loadLhs(&blA[(1 + 3 * K) * LhsProgress], A1);                                                          \
+    traits.loadLhs(&blA[(2 + 3 * K) * LhsProgress], A2);                                                          \
+    EIGEN_GEBP_3Px8_REGISTER_ALLOC_WORKAROUND traits.loadRhs(blB + (0 + 8 * K) * Traits::RhsProgress, rhs_panel); \
+    traits.madd(A0, rhs_panel, C0, T0, fix<0>);                                                                   \
+    traits.madd(A1, rhs_panel, C8, T0, fix<0>);                                                                   \
+    traits.madd(A2, rhs_panel, C16, T0, fix<0>);                                                                  \
+    traits.updateRhs(blB + (1 + 8 * K) * Traits::RhsProgress, rhs_panel);                                         \
+    traits.madd(A0, rhs_panel, C1, T0, fix<1>);                                                                   \
+    traits.madd(A1, rhs_panel, C9, T0, fix<1>);                                                                   \
+    traits.madd(A2, rhs_panel, C17, T0, fix<1>);                                                                  \
+    traits.updateRhs(blB + (2 + 8 * K) * Traits::RhsProgress, rhs_panel);                                         \
+    traits.madd(A0, rhs_panel, C2, T0, fix<2>);                                                                   \
+    traits.madd(A1, rhs_panel, C10, T0, fix<2>);                                                                  \
+    traits.madd(A2, rhs_panel, C18, T0, fix<2>);                                                                  \
+    traits.updateRhs(blB + (3 + 8 * K) * Traits::RhsProgress, rhs_panel);                                         \
+    traits.madd(A0, rhs_panel, C3, T0, fix<3>);                                                                   \
+    traits.madd(A1, rhs_panel, C11, T0, fix<3>);                                                                  \
+    traits.madd(A2, rhs_panel, C19, T0, fix<3>);                                                                  \
+    traits.loadRhs(blB + (4 + 8 * K) * Traits::RhsProgress, rhs_panel);                                           \
+    traits.madd(A0, rhs_panel, C4, T0, fix<0>);                                                                   \
+    traits.madd(A1, rhs_panel, C12, T0, fix<0>);                                                                  \
+    traits.madd(A2, rhs_panel, C20, T0, fix<0>);                                                                  \
+    traits.updateRhs(blB + (5 + 8 * K) * Traits::RhsProgress, rhs_panel);                                         \
+    traits.madd(A0, rhs_panel, C5, T0, fix<1>);                                                                   \
+    traits.madd(A1, rhs_panel, C13, T0, fix<1>);                                                                  \
+    traits.madd(A2, rhs_panel, C21, T0, fix<1>);                                                                  \
+    traits.updateRhs(blB + (6 + 8 * K) * Traits::RhsProgress, rhs_panel);                                         \
+    traits.madd(A0, rhs_panel, C6, T0, fix<2>);                                                                   \
+    traits.madd(A1, rhs_panel, C14, T0, fix<2>);                                                                  \
+    traits.madd(A2, rhs_panel, C22, T0, fix<2>);                                                                  \
+    traits.updateRhs(blB + (7 + 8 * K) * Traits::RhsProgress, rhs_panel);                                         \
+    traits.madd(A0, rhs_panel, C7, T0, fix<3>);                                                                   \
+    traits.madd(A1, rhs_panel, C15, T0, fix<3>);                                                                  \
+    traits.madd(A2, rhs_panel, C23, T0, fix<3>);                                                                  \
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX8");                                                      \
+  } while (false)
+
+              EIGEN_GEBP_ONESTEP(0);
+              EIGEN_GEBP_ONESTEP(1);
+              EIGEN_GEBP_ONESTEP(2);
+              EIGEN_GEBP_ONESTEP(3);
+              EIGEN_GEBP_ONESTEP(4);
+              EIGEN_GEBP_ONESTEP(5);
+              EIGEN_GEBP_ONESTEP(6);
+              EIGEN_GEBP_ONESTEP(7);
+
+              blB += pk * 8 * RhsProgress;
+              blA += pk * 3 * Traits::LhsProgress;
+              EIGEN_ASM_COMMENT("end gebp micro kernel 3pX8");
+            }
+
+            // process remaining peeled loop
+            for (Index k = peeled_kc; k < depth; k++) {
+              RhsPanel27 rhs_panel;
+              RhsPacket T0;
+              LhsPacket A2;
+              EIGEN_GEBP_ONESTEP(0);
+              blB += 8 * RhsProgress;
+              blA += 3 * Traits::LhsProgress;
+            }
+
+#undef EIGEN_GEBP_ONESTEP
+
+            ResPacket R0, R1, R2;
+            ResPacket alphav = pset1<ResPacket>(alpha);
+
+            R0 = r0.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r0.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r0.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C0, alphav, R0);
+            traits.acc(C8, alphav, R1);
+            traits.acc(C16, alphav, R2);
+            r0.storePacket(0 * Traits::ResPacketSize, R0);
+            r0.storePacket(1 * Traits::ResPacketSize, R1);
+            r0.storePacket(2 * Traits::ResPacketSize, R2);
+
+            R0 = r1.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r1.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r1.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C1, alphav, R0);
+            traits.acc(C9, alphav, R1);
+            traits.acc(C17, alphav, R2);
+            r1.storePacket(0 * Traits::ResPacketSize, R0);
+            r1.storePacket(1 * Traits::ResPacketSize, R1);
+            r1.storePacket(2 * Traits::ResPacketSize, R2);
+
+            R0 = r2.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r2.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r2.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C2, alphav, R0);
+            traits.acc(C10, alphav, R1);
+            traits.acc(C18, alphav, R2);
+            r2.storePacket(0 * Traits::ResPacketSize, R0);
+            r2.storePacket(1 * Traits::ResPacketSize, R1);
+            r2.storePacket(2 * Traits::ResPacketSize, R2);
+
+            R0 = r3.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r3.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r3.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C3, alphav, R0);
+            traits.acc(C11, alphav, R1);
+            traits.acc(C19, alphav, R2);
+            r3.storePacket(0 * Traits::ResPacketSize, R0);
+            r3.storePacket(1 * Traits::ResPacketSize, R1);
+            r3.storePacket(2 * Traits::ResPacketSize, R2);
+
+            R0 = r4.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r4.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r4.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C4, alphav, R0);
+            traits.acc(C12, alphav, R1);
+            traits.acc(C20, alphav, R2);
+            r4.storePacket(0 * Traits::ResPacketSize, R0);
+            r4.storePacket(1 * Traits::ResPacketSize, R1);
+            r4.storePacket(2 * Traits::ResPacketSize, R2);
+
+            R0 = r5.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r5.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r5.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C5, alphav, R0);
+            traits.acc(C13, alphav, R1);
+            traits.acc(C21, alphav, R2);
+            r5.storePacket(0 * Traits::ResPacketSize, R0);
+            r5.storePacket(1 * Traits::ResPacketSize, R1);
+            r5.storePacket(2 * Traits::ResPacketSize, R2);
+
+            R0 = r6.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r6.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r6.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C6, alphav, R0);
+            traits.acc(C14, alphav, R1);
+            traits.acc(C22, alphav, R2);
+            r6.storePacket(0 * Traits::ResPacketSize, R0);
+            r6.storePacket(1 * Traits::ResPacketSize, R1);
+            r6.storePacket(2 * Traits::ResPacketSize, R2);
+
+            R0 = r7.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r7.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r7.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
+            traits.acc(C7, alphav, R0);
+            traits.acc(C15, alphav, R1);
+            traits.acc(C23, alphav, R2);
+            r7.storePacket(0 * Traits::ResPacketSize, R0);
+            r7.storePacket(1 * Traits::ResPacketSize, R1);
+            r7.storePacket(2 * Traits::ResPacketSize, R2);
+          }
+        }
+      }
+#endif
+      for (Index j2 = packet_cols8; j2 < packet_cols4; j2 += 4) {
+        for (Index i = i1; i < actual_panel_end; i += 3 * LhsProgress) {
           // We selected a 3*Traits::LhsProgress x nr micro block of res which is entirely
           // stored into 3 x nr registers.
-          
-          const LhsScalar* blA = &blockA[i*strideA+offsetA*(3*LhsProgress)];
+
+          const LhsScalar* blA = &blockA[i * strideA + offsetA * (3 * LhsProgress)];
           prefetch(&blA[0]);
 
           // gets res block as register
-          AccPacket C0, C1, C2,  C3,
-                    C4, C5, C6,  C7,
-                    C8, C9, C10, C11;
-          traits.initAcc(C0);  traits.initAcc(C1);  traits.initAcc(C2);  traits.initAcc(C3);
-          traits.initAcc(C4);  traits.initAcc(C5);  traits.initAcc(C6);  traits.initAcc(C7);
-          traits.initAcc(C8);  traits.initAcc(C9);  traits.initAcc(C10); traits.initAcc(C11);
+          AccPacket C0, C1, C2, C3, C4, C5, C6, C7, C8, C9, C10, C11;
+          traits.initAcc(C0);
+          traits.initAcc(C1);
+          traits.initAcc(C2);
+          traits.initAcc(C3);
+          traits.initAcc(C4);
+          traits.initAcc(C5);
+          traits.initAcc(C6);
+          traits.initAcc(C7);
+          traits.initAcc(C8);
+          traits.initAcc(C9);
+          traits.initAcc(C10);
+          traits.initAcc(C11);
 
           LinearMapper r0 = res.getLinearMapper(i, j2 + 0);
           LinearMapper r1 = res.getLinearMapper(i, j2 + 1);
@@ -1471,55 +1725,54 @@
           r3.prefetch(0);
 
           // performs "inner" products
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB*nr];
+          const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 4];
           prefetch(&blB[0]);
           LhsPacket A0, A1;
 
-          for(Index k=0; k<peeled_kc; k+=pk)
-          {
+          for (Index k = 0; k < peeled_kc; k += pk) {
             EIGEN_ASM_COMMENT("begin gebp micro kernel 3pX4");
-            // 15 registers are taken (12 for acc, 2 for lhs).
+            // 15 registers are taken (12 for acc, 3 for lhs).
             RhsPanel15 rhs_panel;
             RhsPacket T0;
             LhsPacket A2;
-            #if EIGEN_COMP_GNUC_STRICT && EIGEN_ARCH_ARM64 && defined(EIGEN_VECTORIZE_NEON) && !(EIGEN_GNUC_AT_LEAST(9,0))
-            // see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1633
-            // without this workaround A0, A1, and A2 are loaded in the same register,
-            // which is not good for pipelining
-            #define EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND __asm__  ("" : "+w,m" (A0), "+w,m" (A1), "+w,m" (A2));
-            #else
-            #define EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND
-            #endif
-#define EIGEN_GEBP_ONESTEP(K)                                                     \
-            do {                                                                  \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX4");          \
-              EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-              internal::prefetch(blA + (3 * K + 16) * LhsProgress);               \
-              if (EIGEN_ARCH_ARM || EIGEN_ARCH_MIPS) {                            \
-                internal::prefetch(blB + (4 * K + 16) * RhsProgress);             \
-              } /* Bug 953 */                                                     \
-              traits.loadLhs(&blA[(0 + 3 * K) * LhsProgress], A0);                \
-              traits.loadLhs(&blA[(1 + 3 * K) * LhsProgress], A1);                \
-              traits.loadLhs(&blA[(2 + 3 * K) * LhsProgress], A2);                \
-              EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND \
-              traits.loadRhs(blB + (0+4*K) * Traits::RhsProgress, rhs_panel);     \
-              traits.madd(A0, rhs_panel, C0, T0, fix<0>);                         \
-              traits.madd(A1, rhs_panel, C4, T0, fix<0>);                         \
-              traits.madd(A2, rhs_panel, C8, T0, fix<0>);                         \
-              traits.updateRhs(blB + (1+4*K) * Traits::RhsProgress, rhs_panel);   \
-              traits.madd(A0, rhs_panel, C1, T0, fix<1>);                         \
-              traits.madd(A1, rhs_panel, C5, T0, fix<1>);                         \
-              traits.madd(A2, rhs_panel, C9, T0, fix<1>);                         \
-              traits.updateRhs(blB + (2+4*K) * Traits::RhsProgress, rhs_panel);   \
-              traits.madd(A0, rhs_panel, C2, T0, fix<2>);                         \
-              traits.madd(A1, rhs_panel, C6, T0, fix<2>);                         \
-              traits.madd(A2, rhs_panel, C10, T0, fix<2>);                        \
-              traits.updateRhs(blB + (3+4*K) * Traits::RhsProgress, rhs_panel);   \
-              traits.madd(A0, rhs_panel, C3, T0, fix<3>);                         \
-              traits.madd(A1, rhs_panel, C7, T0, fix<3>);                         \
-              traits.madd(A2, rhs_panel, C11, T0, fix<3>);                        \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX4");            \
-            } while (false)
+#if EIGEN_ARCH_ARM64 && defined(EIGEN_VECTORIZE_NEON) && EIGEN_GNUC_STRICT_LESS_THAN(9, 0, 0)
+// see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1633
+// without this workaround A0, A1, and A2 are loaded in the same register,
+// which is not good for pipelining
+#define EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND __asm__("" : "+w,m"(A0), "+w,m"(A1), "+w,m"(A2));
+#else
+#define EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND
+#endif
+#define EIGEN_GEBP_ONESTEP(K)                                             \
+  do {                                                                    \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX4");            \
+    EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!");   \
+    internal::prefetch(blA + (3 * K + 16) * LhsProgress);                 \
+    if (EIGEN_ARCH_ARM || EIGEN_ARCH_MIPS) {                              \
+      internal::prefetch(blB + (4 * K + 16) * RhsProgress);               \
+    } /* Bug 953 */                                                       \
+    traits.loadLhs(&blA[(0 + 3 * K) * LhsProgress], A0);                  \
+    traits.loadLhs(&blA[(1 + 3 * K) * LhsProgress], A1);                  \
+    traits.loadLhs(&blA[(2 + 3 * K) * LhsProgress], A2);                  \
+    EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND                             \
+    traits.loadRhs(blB + (0 + 4 * K) * Traits::RhsProgress, rhs_panel);   \
+    traits.madd(A0, rhs_panel, C0, T0, fix<0>);                           \
+    traits.madd(A1, rhs_panel, C4, T0, fix<0>);                           \
+    traits.madd(A2, rhs_panel, C8, T0, fix<0>);                           \
+    traits.updateRhs(blB + (1 + 4 * K) * Traits::RhsProgress, rhs_panel); \
+    traits.madd(A0, rhs_panel, C1, T0, fix<1>);                           \
+    traits.madd(A1, rhs_panel, C5, T0, fix<1>);                           \
+    traits.madd(A2, rhs_panel, C9, T0, fix<1>);                           \
+    traits.updateRhs(blB + (2 + 4 * K) * Traits::RhsProgress, rhs_panel); \
+    traits.madd(A0, rhs_panel, C2, T0, fix<2>);                           \
+    traits.madd(A1, rhs_panel, C6, T0, fix<2>);                           \
+    traits.madd(A2, rhs_panel, C10, T0, fix<2>);                          \
+    traits.updateRhs(blB + (3 + 4 * K) * Traits::RhsProgress, rhs_panel); \
+    traits.madd(A0, rhs_panel, C3, T0, fix<3>);                           \
+    traits.madd(A1, rhs_panel, C7, T0, fix<3>);                           \
+    traits.madd(A2, rhs_panel, C11, T0, fix<3>);                          \
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX4");              \
+  } while (false)
 
             internal::prefetch(blB);
             EIGEN_GEBP_ONESTEP(0);
@@ -1531,20 +1784,19 @@
             EIGEN_GEBP_ONESTEP(6);
             EIGEN_GEBP_ONESTEP(7);
 
-            blB += pk*4*RhsProgress;
-            blA += pk*3*Traits::LhsProgress;
+            blB += pk * 4 * RhsProgress;
+            blA += pk * 3 * Traits::LhsProgress;
 
             EIGEN_ASM_COMMENT("end gebp micro kernel 3pX4");
           }
           // process remaining peeled loop
-          for(Index k=peeled_kc; k<depth; k++)
-          {
+          for (Index k = peeled_kc; k < depth; k++) {
             RhsPanel15 rhs_panel;
             RhsPacket T0;
             LhsPacket A2;
             EIGEN_GEBP_ONESTEP(0);
-            blB += 4*RhsProgress;
-            blA += 3*Traits::LhsProgress;
+            blB += 4 * RhsProgress;
+            blA += 3 * Traits::LhsProgress;
           }
 
 #undef EIGEN_GEBP_ONESTEP
@@ -1590,17 +1842,15 @@
           traits.acc(C11, alphav, R2);
           r3.storePacket(0 * Traits::ResPacketSize, R0);
           r3.storePacket(1 * Traits::ResPacketSize, R1);
-          r3.storePacket(2 * Traits::ResPacketSize, R2);          
-          }
+          r3.storePacket(2 * Traits::ResPacketSize, R2);
         }
+      }
 
-        // Deal with remaining columns of the rhs
-        for(Index j2=packet_cols4; j2<cols; j2++)
-        {
-          for(Index i=i1; i<actual_panel_end; i+=3*LhsProgress)
-          {
+      // Deal with remaining columns of the rhs
+      for (Index j2 = packet_cols4; j2 < cols; j2++) {
+        for (Index i = i1; i < actual_panel_end; i += 3 * LhsProgress) {
           // One column at a time
-          const LhsScalar* blA = &blockA[i*strideA+offsetA*(3*Traits::LhsProgress)];
+          const LhsScalar* blA = &blockA[i * strideA + offsetA * (3 * Traits::LhsProgress)];
           prefetch(&blA[0]);
 
           // gets res block as register
@@ -1613,26 +1863,25 @@
           r0.prefetch(0);
 
           // performs "inner" products
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB];
+          const RhsScalar* blB = &blockB[j2 * strideB + offsetB];
           LhsPacket A0, A1, A2;
-          
-          for(Index k=0; k<peeled_kc; k+=pk)
-          {
+
+          for (Index k = 0; k < peeled_kc; k += pk) {
             EIGEN_ASM_COMMENT("begin gebp micro kernel 3pX1");
             RhsPacket B_0;
-#define EIGEN_GEBGP_ONESTEP(K)                                                    \
-            do {                                                                  \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX1");          \
-              EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-              traits.loadLhs(&blA[(0 + 3 * K) * LhsProgress], A0);                \
-              traits.loadLhs(&blA[(1 + 3 * K) * LhsProgress], A1);                \
-              traits.loadLhs(&blA[(2 + 3 * K) * LhsProgress], A2);                \
-              traits.loadRhs(&blB[(0 + K) * RhsProgress], B_0);                   \
-              traits.madd(A0, B_0, C0, B_0, fix<0>);                              \
-              traits.madd(A1, B_0, C4, B_0, fix<0>);                              \
-              traits.madd(A2, B_0, C8, B_0, fix<0>);                              \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX1");            \
-            } while (false)
+#define EIGEN_GEBGP_ONESTEP(K)                                          \
+  do {                                                                  \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX1");          \
+    EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
+    traits.loadLhs(&blA[(0 + 3 * K) * LhsProgress], A0);                \
+    traits.loadLhs(&blA[(1 + 3 * K) * LhsProgress], A1);                \
+    traits.loadLhs(&blA[(2 + 3 * K) * LhsProgress], A2);                \
+    traits.loadRhs(&blB[(0 + K) * RhsProgress], B_0);                   \
+    traits.madd(A0, B_0, C0, B_0, fix<0>);                              \
+    traits.madd(A1, B_0, C4, B_0, fix<0>);                              \
+    traits.madd(A2, B_0, C8, B_0, fix<0>);                              \
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX1");            \
+  } while (false)
 
             EIGEN_GEBGP_ONESTEP(0);
             EIGEN_GEBGP_ONESTEP(1);
@@ -1650,12 +1899,11 @@
           }
 
           // process remaining peeled loop
-          for(Index k=peeled_kc; k<depth; k++)
-          {
+          for (Index k = peeled_kc; k < depth; k++) {
             RhsPacket B_0;
             EIGEN_GEBGP_ONESTEP(0);
             blB += RhsProgress;
-            blA += 3*Traits::LhsProgress;
+            blA += 3 * Traits::LhsProgress;
           }
 #undef EIGEN_GEBGP_ONESTEP
           ResPacket R0, R1, R2;
@@ -1669,40 +1917,214 @@
           traits.acc(C8, alphav, R2);
           r0.storePacket(0 * Traits::ResPacketSize, R0);
           r0.storePacket(1 * Traits::ResPacketSize, R1);
-          r0.storePacket(2 * Traits::ResPacketSize, R2);          
-          }
+          r0.storePacket(2 * Traits::ResPacketSize, R2);
         }
       }
     }
+  }
 
-    //---------- Process 2 * LhsProgress rows at once ----------
-    if(mr>=2*Traits::LhsProgress)
-    {
-      const Index l1 = defaultL1CacheSize; // in Bytes, TODO, l1 should be passed to this function.
-      // The max(1, ...) here is needed because we may be using blocking params larger than what our known l1 cache size
-      // suggests we should be using: either because our known l1 cache size is inaccurate (e.g. on Android, we can only guess),
-      // or because we are testing specific blocking sizes.
-      Index actual_panel_rows = (2*LhsProgress) * std::max<Index>(1,( (l1 - sizeof(ResScalar)*mr*nr - depth*nr*sizeof(RhsScalar)) / (depth * sizeof(LhsScalar) * 2*LhsProgress) ));
+  //---------- Process 2 * LhsProgress rows at once ----------
+  if (mr >= 2 * Traits::LhsProgress) {
+    const Index l1 = defaultL1CacheSize;  // in Bytes, TODO, l1 should be passed to this function.
+    // The max(1, ...) here is needed because we may be using blocking params larger than what our known l1 cache size
+    // suggests we should be using: either because our known l1 cache size is inaccurate (e.g. on Android, we can only
+    // guess), or because we are testing specific blocking sizes.
+    Index actual_panel_rows =
+        (2 * LhsProgress) * std::max<Index>(1, ((l1 - sizeof(ResScalar) * mr * nr - depth * nr * sizeof(RhsScalar)) /
+                                                (depth * sizeof(LhsScalar) * 2 * LhsProgress)));
 
-      for(Index i1=peeled_mc3; i1<peeled_mc2; i1+=actual_panel_rows)
-      {
-        Index actual_panel_end = (std::min)(i1+actual_panel_rows, peeled_mc2);
-        for(Index j2=0; j2<packet_cols4; j2+=nr)
-        {
-          for(Index i=i1; i<actual_panel_end; i+=2*LhsProgress)
-          {
-          
+    for (Index i1 = peeled_mc3; i1 < peeled_mc2; i1 += actual_panel_rows) {
+      Index actual_panel_end = (std::min)(i1 + actual_panel_rows, peeled_mc2);
+#if EIGEN_ARCH_ARM64
+      EIGEN_IF_CONSTEXPR(nr >= 8) {
+        for (Index j2 = 0; j2 < packet_cols8; j2 += 8) {
+          for (Index i = i1; i < actual_panel_end; i += 2 * LhsProgress) {
+            const LhsScalar* blA = &blockA[i * strideA + offsetA * (2 * Traits::LhsProgress)];
+            prefetch(&blA[0]);
+
+            AccPacket C0, C1, C2, C3, C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15;
+            traits.initAcc(C0);
+            traits.initAcc(C1);
+            traits.initAcc(C2);
+            traits.initAcc(C3);
+            traits.initAcc(C4);
+            traits.initAcc(C5);
+            traits.initAcc(C6);
+            traits.initAcc(C7);
+            traits.initAcc(C8);
+            traits.initAcc(C9);
+            traits.initAcc(C10);
+            traits.initAcc(C11);
+            traits.initAcc(C12);
+            traits.initAcc(C13);
+            traits.initAcc(C14);
+            traits.initAcc(C15);
+
+            LinearMapper r0 = res.getLinearMapper(i, j2 + 0);
+            LinearMapper r1 = res.getLinearMapper(i, j2 + 1);
+            LinearMapper r2 = res.getLinearMapper(i, j2 + 2);
+            LinearMapper r3 = res.getLinearMapper(i, j2 + 3);
+            LinearMapper r4 = res.getLinearMapper(i, j2 + 4);
+            LinearMapper r5 = res.getLinearMapper(i, j2 + 5);
+            LinearMapper r6 = res.getLinearMapper(i, j2 + 6);
+            LinearMapper r7 = res.getLinearMapper(i, j2 + 7);
+            r0.prefetch(prefetch_res_offset);
+            r1.prefetch(prefetch_res_offset);
+            r2.prefetch(prefetch_res_offset);
+            r3.prefetch(prefetch_res_offset);
+            r4.prefetch(prefetch_res_offset);
+            r5.prefetch(prefetch_res_offset);
+            r6.prefetch(prefetch_res_offset);
+            r7.prefetch(prefetch_res_offset);
+
+            const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 8];
+            prefetch(&blB[0]);
+            LhsPacket A0, A1;
+            for (Index k = 0; k < peeled_kc; k += pk) {
+              RhsPacketx4 rhs_panel;
+              RhsPacket T0;
+// NOTE: the begin/end asm comments below work around bug 935!
+// but they are not enough for gcc>=6 without FMA (bug 1637)
+#if EIGEN_GNUC_STRICT_AT_LEAST(6, 0, 0) && defined(EIGEN_VECTORIZE_SSE)
+#define EIGEN_GEBP_2Px8_SPILLING_WORKAROUND __asm__("" : [a0] "+x,m"(A0), [a1] "+x,m"(A1));
+#else
+#define EIGEN_GEBP_2Px8_SPILLING_WORKAROUND
+#endif
+#define EIGEN_GEBGP_ONESTEP(K)                                                                   \
+  do {                                                                                           \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX8");                                   \
+    traits.loadLhs(&blA[(0 + 2 * K) * LhsProgress], A0);                                         \
+    traits.loadLhs(&blA[(1 + 2 * K) * LhsProgress], A1);                                         \
+    traits.loadRhs(&blB[(0 + 8 * K) * RhsProgress], rhs_panel);                                  \
+    traits.madd(A0, rhs_panel, C0, T0, fix<0>);                                                  \
+    traits.madd(A1, rhs_panel, C8, T0, fix<0>);                                                  \
+    traits.updateRhs(&blB[(1 + 8 * K) * RhsProgress], rhs_panel);                                \
+    traits.madd(A0, rhs_panel, C1, T0, fix<1>);                                                  \
+    traits.madd(A1, rhs_panel, C9, T0, fix<1>);                                                  \
+    traits.updateRhs(&blB[(2 + 8 * K) * RhsProgress], rhs_panel);                                \
+    traits.madd(A0, rhs_panel, C2, T0, fix<2>);                                                  \
+    traits.madd(A1, rhs_panel, C10, T0, fix<2>);                                                 \
+    traits.updateRhs(&blB[(3 + 8 * K) * RhsProgress], rhs_panel);                                \
+    traits.madd(A0, rhs_panel, C3, T0, fix<3>);                                                  \
+    traits.madd(A1, rhs_panel, C11, T0, fix<3>);                                                 \
+    traits.loadRhs(&blB[(4 + 8 * K) * RhsProgress], rhs_panel);                                  \
+    traits.madd(A0, rhs_panel, C4, T0, fix<0>);                                                  \
+    traits.madd(A1, rhs_panel, C12, T0, fix<0>);                                                 \
+    traits.updateRhs(&blB[(5 + 8 * K) * RhsProgress], rhs_panel);                                \
+    traits.madd(A0, rhs_panel, C5, T0, fix<1>);                                                  \
+    traits.madd(A1, rhs_panel, C13, T0, fix<1>);                                                 \
+    traits.updateRhs(&blB[(6 + 8 * K) * RhsProgress], rhs_panel);                                \
+    traits.madd(A0, rhs_panel, C6, T0, fix<2>);                                                  \
+    traits.madd(A1, rhs_panel, C14, T0, fix<2>);                                                 \
+    traits.updateRhs(&blB[(7 + 8 * K) * RhsProgress], rhs_panel);                                \
+    traits.madd(A0, rhs_panel, C7, T0, fix<3>);                                                  \
+    traits.madd(A1, rhs_panel, C15, T0, fix<3>);                                                 \
+    EIGEN_GEBP_2Px8_SPILLING_WORKAROUND EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX8"); \
+  } while (false)
+
+              EIGEN_ASM_COMMENT("begin gebp micro kernel 2pX8");
+
+              EIGEN_GEBGP_ONESTEP(0);
+              EIGEN_GEBGP_ONESTEP(1);
+              EIGEN_GEBGP_ONESTEP(2);
+              EIGEN_GEBGP_ONESTEP(3);
+              EIGEN_GEBGP_ONESTEP(4);
+              EIGEN_GEBGP_ONESTEP(5);
+              EIGEN_GEBGP_ONESTEP(6);
+              EIGEN_GEBGP_ONESTEP(7);
+
+              blB += pk * 8 * RhsProgress;
+              blA += pk * (2 * Traits::LhsProgress);
+
+              EIGEN_ASM_COMMENT("end gebp micro kernel 2pX8");
+            }
+            // process remaining peeled loop
+            for (Index k = peeled_kc; k < depth; k++) {
+              RhsPacketx4 rhs_panel;
+              RhsPacket T0;
+              EIGEN_GEBGP_ONESTEP(0);
+              blB += 8 * RhsProgress;
+              blA += 2 * Traits::LhsProgress;
+            }
+
+#undef EIGEN_GEBGP_ONESTEP
+
+            ResPacket R0, R1, R2, R3;
+            ResPacket alphav = pset1<ResPacket>(alpha);
+
+            R0 = r0.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r0.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r1.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R3 = r1.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            traits.acc(C0, alphav, R0);
+            traits.acc(C8, alphav, R1);
+            traits.acc(C1, alphav, R2);
+            traits.acc(C9, alphav, R3);
+            r0.storePacket(0 * Traits::ResPacketSize, R0);
+            r0.storePacket(1 * Traits::ResPacketSize, R1);
+            r1.storePacket(0 * Traits::ResPacketSize, R2);
+            r1.storePacket(1 * Traits::ResPacketSize, R3);
+
+            R0 = r2.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r2.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r3.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R3 = r3.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            traits.acc(C2, alphav, R0);
+            traits.acc(C10, alphav, R1);
+            traits.acc(C3, alphav, R2);
+            traits.acc(C11, alphav, R3);
+            r2.storePacket(0 * Traits::ResPacketSize, R0);
+            r2.storePacket(1 * Traits::ResPacketSize, R1);
+            r3.storePacket(0 * Traits::ResPacketSize, R2);
+            r3.storePacket(1 * Traits::ResPacketSize, R3);
+
+            R0 = r4.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r4.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r5.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R3 = r5.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            traits.acc(C4, alphav, R0);
+            traits.acc(C12, alphav, R1);
+            traits.acc(C5, alphav, R2);
+            traits.acc(C13, alphav, R3);
+            r4.storePacket(0 * Traits::ResPacketSize, R0);
+            r4.storePacket(1 * Traits::ResPacketSize, R1);
+            r5.storePacket(0 * Traits::ResPacketSize, R2);
+            r5.storePacket(1 * Traits::ResPacketSize, R3);
+
+            R0 = r6.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R1 = r6.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            R2 = r7.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+            R3 = r7.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+            traits.acc(C6, alphav, R0);
+            traits.acc(C14, alphav, R1);
+            traits.acc(C7, alphav, R2);
+            traits.acc(C15, alphav, R3);
+            r6.storePacket(0 * Traits::ResPacketSize, R0);
+            r6.storePacket(1 * Traits::ResPacketSize, R1);
+            r7.storePacket(0 * Traits::ResPacketSize, R2);
+            r7.storePacket(1 * Traits::ResPacketSize, R3);
+          }
+        }
+      }
+#endif
+      for (Index j2 = packet_cols8; j2 < packet_cols4; j2 += 4) {
+        for (Index i = i1; i < actual_panel_end; i += 2 * LhsProgress) {
           // We selected a 2*Traits::LhsProgress x nr micro block of res which is entirely
           // stored into 2 x nr registers.
-          
-          const LhsScalar* blA = &blockA[i*strideA+offsetA*(2*Traits::LhsProgress)];
+
+          const LhsScalar* blA = &blockA[i * strideA + offsetA * (2 * Traits::LhsProgress)];
           prefetch(&blA[0]);
 
           // gets res block as register
-          AccPacket C0, C1, C2, C3,
-                    C4, C5, C6, C7;
-          traits.initAcc(C0); traits.initAcc(C1); traits.initAcc(C2); traits.initAcc(C3);
-          traits.initAcc(C4); traits.initAcc(C5); traits.initAcc(C6); traits.initAcc(C7);
+          AccPacket C0, C1, C2, C3, C4, C5, C6, C7;
+          traits.initAcc(C0);
+          traits.initAcc(C1);
+          traits.initAcc(C2);
+          traits.initAcc(C3);
+          traits.initAcc(C4);
+          traits.initAcc(C5);
+          traits.initAcc(C6);
+          traits.initAcc(C7);
 
           LinearMapper r0 = res.getLinearMapper(i, j2 + 0);
           LinearMapper r1 = res.getLinearMapper(i, j2 + 1);
@@ -1715,65 +2137,63 @@
           r3.prefetch(prefetch_res_offset);
 
           // performs "inner" products
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB*nr];
+          const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 4];
           prefetch(&blB[0]);
           LhsPacket A0, A1;
 
-          for(Index k=0; k<peeled_kc; k+=pk)
-          {
+          for (Index k = 0; k < peeled_kc; k += pk) {
             EIGEN_ASM_COMMENT("begin gebp micro kernel 2pX4");
             RhsPacketx4 rhs_panel;
             RhsPacket T0;
 
-          // NOTE: the begin/end asm comments below work around bug 935!
-          // but they are not enough for gcc>=6 without FMA (bug 1637)
-          #if EIGEN_GNUC_AT_LEAST(6,0) && defined(EIGEN_VECTORIZE_SSE)
-            #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND __asm__  ("" : [a0] "+x,m" (A0),[a1] "+x,m" (A1));
-          #else
-            #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND
-          #endif
-#define EIGEN_GEBGP_ONESTEP(K)                                            \
-            do {                                                          \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX4");  \
-              traits.loadLhs(&blA[(0 + 2 * K) * LhsProgress], A0);        \
-              traits.loadLhs(&blA[(1 + 2 * K) * LhsProgress], A1);        \
-              traits.loadRhs(&blB[(0 + 4 * K) * RhsProgress], rhs_panel); \
-              traits.madd(A0, rhs_panel, C0, T0, fix<0>);                 \
-              traits.madd(A1, rhs_panel, C4, T0, fix<0>);                 \
-              traits.madd(A0, rhs_panel, C1, T0, fix<1>);                 \
-              traits.madd(A1, rhs_panel, C5, T0, fix<1>);                 \
-              traits.madd(A0, rhs_panel, C2, T0, fix<2>);                 \
-              traits.madd(A1, rhs_panel, C6, T0, fix<2>);                 \
-              traits.madd(A0, rhs_panel, C3, T0, fix<3>);                 \
-              traits.madd(A1, rhs_panel, C7, T0, fix<3>);                 \
-              EIGEN_GEBP_2PX4_SPILLING_WORKAROUND                         \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX4");    \
-            } while (false)
+// NOTE: the begin/end asm comments below work around bug 935!
+// but they are not enough for gcc>=6 without FMA (bug 1637)
+#if EIGEN_GNUC_STRICT_AT_LEAST(6, 0, 0) && defined(EIGEN_VECTORIZE_SSE) && !(EIGEN_COMP_LCC)
+#define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND __asm__("" : [a0] "+x,m"(A0), [a1] "+x,m"(A1));
+#else
+#define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND
+#endif
+#define EIGEN_GEBGP_ONESTEP(K)                                  \
+  do {                                                          \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX4");  \
+    traits.loadLhs(&blA[(0 + 2 * K) * LhsProgress], A0);        \
+    traits.loadLhs(&blA[(1 + 2 * K) * LhsProgress], A1);        \
+    traits.loadRhs(&blB[(0 + 4 * K) * RhsProgress], rhs_panel); \
+    traits.madd(A0, rhs_panel, C0, T0, fix<0>);                 \
+    traits.madd(A1, rhs_panel, C4, T0, fix<0>);                 \
+    traits.madd(A0, rhs_panel, C1, T0, fix<1>);                 \
+    traits.madd(A1, rhs_panel, C5, T0, fix<1>);                 \
+    traits.madd(A0, rhs_panel, C2, T0, fix<2>);                 \
+    traits.madd(A1, rhs_panel, C6, T0, fix<2>);                 \
+    traits.madd(A0, rhs_panel, C3, T0, fix<3>);                 \
+    traits.madd(A1, rhs_panel, C7, T0, fix<3>);                 \
+    EIGEN_GEBP_2PX4_SPILLING_WORKAROUND                         \
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX4");    \
+  } while (false)
 
-            internal::prefetch(blB+(48+0));
+            internal::prefetch(blB + (48 + 0));
             EIGEN_GEBGP_ONESTEP(0);
             EIGEN_GEBGP_ONESTEP(1);
             EIGEN_GEBGP_ONESTEP(2);
             EIGEN_GEBGP_ONESTEP(3);
-            internal::prefetch(blB+(48+16));
+            internal::prefetch(blB + (48 + 16));
             EIGEN_GEBGP_ONESTEP(4);
             EIGEN_GEBGP_ONESTEP(5);
             EIGEN_GEBGP_ONESTEP(6);
             EIGEN_GEBGP_ONESTEP(7);
 
-            blB += pk*4*RhsProgress;
-            blA += pk*(2*Traits::LhsProgress);
+            blB += pk * 4 * RhsProgress;
+            blA += pk * (2 * Traits::LhsProgress);
 
             EIGEN_ASM_COMMENT("end gebp micro kernel 2pX4");
           }
           // process remaining peeled loop
-          for(Index k=peeled_kc; k<depth; k++)
-          {
+          for (Index k = peeled_kc; k < depth; k++) {
             RhsPacketx4 rhs_panel;
             RhsPacket T0;
             EIGEN_GEBGP_ONESTEP(0);
-            blB += 4*RhsProgress;
-            blA += 2*Traits::LhsProgress;
+            blB += 4 * RhsProgress;
+            blA += 2 * Traits::LhsProgress;
           }
 #undef EIGEN_GEBGP_ONESTEP
 
@@ -1797,24 +2217,22 @@
           R1 = r2.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
           R2 = r3.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
           R3 = r3.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
-          traits.acc(C2,  alphav, R0);
-          traits.acc(C6,  alphav, R1);
-          traits.acc(C3,  alphav, R2);
-          traits.acc(C7,  alphav, R3);
+          traits.acc(C2, alphav, R0);
+          traits.acc(C6, alphav, R1);
+          traits.acc(C3, alphav, R2);
+          traits.acc(C7, alphav, R3);
           r2.storePacket(0 * Traits::ResPacketSize, R0);
           r2.storePacket(1 * Traits::ResPacketSize, R1);
           r3.storePacket(0 * Traits::ResPacketSize, R2);
           r3.storePacket(1 * Traits::ResPacketSize, R3);
-          }
         }
-      
-        // Deal with remaining columns of the rhs
-        for(Index j2=packet_cols4; j2<cols; j2++)
-        {
-          for(Index i=i1; i<actual_panel_end; i+=2*LhsProgress)
-          {
+      }
+
+      // Deal with remaining columns of the rhs
+      for (Index j2 = packet_cols4; j2 < cols; j2++) {
+        for (Index i = i1; i < actual_panel_end; i += 2 * LhsProgress) {
           // One column at a time
-          const LhsScalar* blA = &blockA[i*strideA+offsetA*(2*Traits::LhsProgress)];
+          const LhsScalar* blA = &blockA[i * strideA + offsetA * (2 * Traits::LhsProgress)];
           prefetch(&blA[0]);
 
           // gets res block as register
@@ -1826,26 +2244,25 @@
           r0.prefetch(prefetch_res_offset);
 
           // performs "inner" products
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB];
+          const RhsScalar* blB = &blockB[j2 * strideB + offsetB];
           LhsPacket A0, A1;
 
-          for(Index k=0; k<peeled_kc; k+=pk)
-          {
+          for (Index k = 0; k < peeled_kc; k += pk) {
             EIGEN_ASM_COMMENT("begin gebp micro kernel 2pX1");
             RhsPacket B_0, B1;
-        
-#define EIGEN_GEBGP_ONESTEP(K) \
-            do {                                                                  \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX1");          \
-              EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-              traits.loadLhs(&blA[(0+2*K)*LhsProgress], A0);                      \
-              traits.loadLhs(&blA[(1+2*K)*LhsProgress], A1);                      \
-              traits.loadRhs(&blB[(0+K)*RhsProgress], B_0);                       \
-              traits.madd(A0, B_0, C0, B1, fix<0>);                               \
-              traits.madd(A1, B_0, C4, B_0, fix<0>);                              \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX1");            \
-            } while(false)
-        
+
+#define EIGEN_GEBGP_ONESTEP(K)                                          \
+  do {                                                                  \
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX1");          \
+    EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
+    traits.loadLhs(&blA[(0 + 2 * K) * LhsProgress], A0);                \
+    traits.loadLhs(&blA[(1 + 2 * K) * LhsProgress], A1);                \
+    traits.loadRhs(&blB[(0 + K) * RhsProgress], B_0);                   \
+    traits.madd(A0, B_0, C0, B1, fix<0>);                               \
+    traits.madd(A1, B_0, C4, B_0, fix<0>);                              \
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX1");            \
+  } while (false)
+
             EIGEN_GEBGP_ONESTEP(0);
             EIGEN_GEBGP_ONESTEP(1);
             EIGEN_GEBGP_ONESTEP(2);
@@ -1862,12 +2279,11 @@
           }
 
           // process remaining peeled loop
-          for(Index k=peeled_kc; k<depth; k++)
-          {
+          for (Index k = peeled_kc; k < depth; k++) {
             RhsPacket B_0, B1;
             EIGEN_GEBGP_ONESTEP(0);
             blB += RhsProgress;
-            blA += 2*Traits::LhsProgress;
+            blA += 2 * Traits::LhsProgress;
           }
 #undef EIGEN_GEBGP_ONESTEP
           ResPacket R0, R1;
@@ -1879,197 +2295,252 @@
           traits.acc(C4, alphav, R1);
           r0.storePacket(0 * Traits::ResPacketSize, R0);
           r0.storePacket(1 * Traits::ResPacketSize, R1);
-          }
-        }
-      }
-    }
-    //---------- Process 1 * LhsProgress rows at once ----------
-    if(mr>=1*Traits::LhsProgress)
-    {
-      lhs_process_one_packet<nr, LhsProgress, RhsProgress, LhsScalar, RhsScalar, ResScalar, AccPacket, LhsPacket, RhsPacket, ResPacket, Traits, LinearMapper, DataMapper> p;
-      p(res, blockA, blockB, alpha, peeled_mc2, peeled_mc1, strideA, strideB, offsetA, offsetB, prefetch_res_offset, peeled_kc, pk, cols, depth, packet_cols4);
-    }
-    //---------- Process LhsProgressHalf rows at once ----------
-    if((LhsProgressHalf < LhsProgress) && mr>=LhsProgressHalf)
-    {
-      lhs_process_fraction_of_packet<nr, LhsProgressHalf, RhsProgressHalf, LhsScalar, RhsScalar, ResScalar, AccPacketHalf, LhsPacketHalf, RhsPacketHalf, ResPacketHalf, HalfTraits, LinearMapper, DataMapper> p;
-      p(res, blockA, blockB, alpha, peeled_mc1, peeled_mc_half, strideA, strideB, offsetA, offsetB, prefetch_res_offset, peeled_kc, pk, cols, depth, packet_cols4);
-    }
-    //---------- Process LhsProgressQuarter rows at once ----------
-    if((LhsProgressQuarter < LhsProgressHalf) && mr>=LhsProgressQuarter)
-    {
-      lhs_process_fraction_of_packet<nr, LhsProgressQuarter, RhsProgressQuarter, LhsScalar, RhsScalar, ResScalar, AccPacketQuarter, LhsPacketQuarter, RhsPacketQuarter, ResPacketQuarter, QuarterTraits, LinearMapper, DataMapper> p;
-      p(res, blockA, blockB, alpha, peeled_mc_half, peeled_mc_quarter, strideA, strideB, offsetA, offsetB, prefetch_res_offset, peeled_kc, pk, cols, depth, packet_cols4);
-    }
-    //---------- Process remaining rows, 1 at once ----------
-    if(peeled_mc_quarter<rows)
-    {
-      // loop on each panel of the rhs
-      for(Index j2=0; j2<packet_cols4; j2+=nr)
-      {
-        // loop on each row of the lhs (1*LhsProgress x depth)
-        for(Index i=peeled_mc_quarter; i<rows; i+=1)
-        {
-          const LhsScalar* blA = &blockA[i*strideA+offsetA];
-          prefetch(&blA[0]);
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB*nr];
-
-          // If LhsProgress is 8 or 16, it assumes that there is a
-          // half or quarter packet, respectively, of the same size as
-          // nr (which is currently 4) for the return type.
-          const int SResPacketHalfSize = unpacket_traits<typename unpacket_traits<SResPacket>::half>::size;
-          const int SResPacketQuarterSize = unpacket_traits<typename unpacket_traits<typename unpacket_traits<SResPacket>::half>::half>::size;
-          if ((SwappedTraits::LhsProgress % 4) == 0 &&
-              (SwappedTraits::LhsProgress<=16) &&
-              (SwappedTraits::LhsProgress!=8  || SResPacketHalfSize==nr) &&
-              (SwappedTraits::LhsProgress!=16 || SResPacketQuarterSize==nr))
-          {
-            SAccPacket C0, C1, C2, C3;
-            straits.initAcc(C0);
-            straits.initAcc(C1);
-            straits.initAcc(C2);
-            straits.initAcc(C3);
-
-            const Index spk   = (std::max)(1,SwappedTraits::LhsProgress/4);
-            const Index endk  = (depth/spk)*spk;
-            const Index endk4 = (depth/(spk*4))*(spk*4);
-
-            Index k=0;
-            for(; k<endk4; k+=4*spk)
-            {
-              SLhsPacket A0,A1;
-              SRhsPacket B_0,B_1;
-
-              straits.loadLhsUnaligned(blB+0*SwappedTraits::LhsProgress, A0);
-              straits.loadLhsUnaligned(blB+1*SwappedTraits::LhsProgress, A1);
-
-              straits.loadRhsQuad(blA+0*spk, B_0);
-              straits.loadRhsQuad(blA+1*spk, B_1);
-              straits.madd(A0,B_0,C0,B_0, fix<0>);
-              straits.madd(A1,B_1,C1,B_1, fix<0>);
-
-              straits.loadLhsUnaligned(blB+2*SwappedTraits::LhsProgress, A0);
-              straits.loadLhsUnaligned(blB+3*SwappedTraits::LhsProgress, A1);
-              straits.loadRhsQuad(blA+2*spk, B_0);
-              straits.loadRhsQuad(blA+3*spk, B_1);
-              straits.madd(A0,B_0,C2,B_0, fix<0>);
-              straits.madd(A1,B_1,C3,B_1, fix<0>);
-
-              blB += 4*SwappedTraits::LhsProgress;
-              blA += 4*spk;
-            }
-            C0 = padd(padd(C0,C1),padd(C2,C3));
-            for(; k<endk; k+=spk)
-            {
-              SLhsPacket A0;
-              SRhsPacket B_0;
-
-              straits.loadLhsUnaligned(blB, A0);
-              straits.loadRhsQuad(blA, B_0);
-              straits.madd(A0,B_0,C0,B_0, fix<0>);
-
-              blB += SwappedTraits::LhsProgress;
-              blA += spk;
-            }
-            if(SwappedTraits::LhsProgress==8)
-            {
-              // Special case where we have to first reduce the accumulation register C0
-              typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SResPacket>::half,SResPacket>::type SResPacketHalf;
-              typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SLhsPacket>::half,SLhsPacket>::type SLhsPacketHalf;
-              typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SRhsPacket>::half,SRhsPacket>::type SRhsPacketHalf;
-              typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SAccPacket>::half,SAccPacket>::type SAccPacketHalf;
-
-              SResPacketHalf R = res.template gatherPacket<SResPacketHalf>(i, j2);
-              SResPacketHalf alphav = pset1<SResPacketHalf>(alpha);
-
-              if(depth-endk>0)
-              {
-                // We have to handle the last row of the rhs which corresponds to a half-packet
-                SLhsPacketHalf a0;
-                SRhsPacketHalf b0;
-                straits.loadLhsUnaligned(blB, a0);
-                straits.loadRhs(blA, b0);
-                SAccPacketHalf c0 = predux_half_dowto4(C0);
-                straits.madd(a0,b0,c0,b0, fix<0>);
-                straits.acc(c0, alphav, R);
-              }
-              else
-              {
-                straits.acc(predux_half_dowto4(C0), alphav, R);
-              }
-              res.scatterPacket(i, j2, R);
-            }
-            else if (SwappedTraits::LhsProgress==16)
-            {
-              // Special case where we have to first reduce the
-              // accumulation register C0. We specialize the block in
-              // template form, so that LhsProgress < 16 paths don't
-              // fail to compile
-              last_row_process_16_packets<LhsScalar, RhsScalar, Index, DataMapper, mr, nr, ConjugateLhs, ConjugateRhs> p;
-	            p(res, straits, blA, blB, depth, endk, i, j2,alpha, C0);
-            }
-            else
-            {
-              SResPacket R = res.template gatherPacket<SResPacket>(i, j2);
-              SResPacket alphav = pset1<SResPacket>(alpha);
-              straits.acc(C0, alphav, R);
-              res.scatterPacket(i, j2, R);
-            }
-          }
-          else // scalar path
-          {
-            // get a 1 x 4 res block as registers
-            ResScalar C0(0), C1(0), C2(0), C3(0);
-
-            for(Index k=0; k<depth; k++)
-            {
-              LhsScalar A0;
-              RhsScalar B_0, B_1;
-
-              A0 = blA[k];
-
-              B_0 = blB[0];
-              B_1 = blB[1];
-              C0 = cj.pmadd(A0,B_0,C0);
-              C1 = cj.pmadd(A0,B_1,C1);
-
-              B_0 = blB[2];
-              B_1 = blB[3];
-              C2 = cj.pmadd(A0,B_0,C2);
-              C3 = cj.pmadd(A0,B_1,C3);
-
-              blB += 4;
-            }
-            res(i, j2 + 0) += alpha * C0;
-            res(i, j2 + 1) += alpha * C1;
-            res(i, j2 + 2) += alpha * C2;
-            res(i, j2 + 3) += alpha * C3;
-          }
-        }
-      }
-      // remaining columns
-      for(Index j2=packet_cols4; j2<cols; j2++)
-      {
-        // loop on each row of the lhs (1*LhsProgress x depth)
-        for(Index i=peeled_mc_quarter; i<rows; i+=1)
-        {
-          const LhsScalar* blA = &blockA[i*strideA+offsetA];
-          prefetch(&blA[0]);
-          // gets a 1 x 1 res block as registers
-          ResScalar C0(0);
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB];
-          for(Index k=0; k<depth; k++)
-          {
-            LhsScalar A0 = blA[k];
-            RhsScalar B_0 = blB[k];
-            C0 = cj.pmadd(A0, B_0, C0);
-          }
-          res(i, j2) += alpha * C0;
         }
       }
     }
   }
+  //---------- Process 1 * LhsProgress rows at once ----------
+  if (mr >= 1 * Traits::LhsProgress) {
+    lhs_process_one_packet<nr, LhsProgress, RhsProgress, LhsScalar, RhsScalar, ResScalar, AccPacket, LhsPacket,
+                           RhsPacket, ResPacket, Traits, LinearMapper, DataMapper>
+        p;
+    p(res, blockA, blockB, alpha, peeled_mc2, peeled_mc1, strideA, strideB, offsetA, offsetB, prefetch_res_offset,
+      peeled_kc, pk, cols, depth, packet_cols4);
+  }
+  //---------- Process LhsProgressHalf rows at once ----------
+  if ((LhsProgressHalf < LhsProgress) && mr >= LhsProgressHalf) {
+    lhs_process_fraction_of_packet<nr, LhsProgressHalf, RhsProgressHalf, LhsScalar, RhsScalar, ResScalar, AccPacketHalf,
+                                   LhsPacketHalf, RhsPacketHalf, ResPacketHalf, HalfTraits, LinearMapper, DataMapper>
+        p;
+    p(res, blockA, blockB, alpha, peeled_mc1, peeled_mc_half, strideA, strideB, offsetA, offsetB, prefetch_res_offset,
+      peeled_kc, pk, cols, depth, packet_cols4);
+  }
+  //---------- Process LhsProgressQuarter rows at once ----------
+  if ((LhsProgressQuarter < LhsProgressHalf) && mr >= LhsProgressQuarter) {
+    lhs_process_fraction_of_packet<nr, LhsProgressQuarter, RhsProgressQuarter, LhsScalar, RhsScalar, ResScalar,
+                                   AccPacketQuarter, LhsPacketQuarter, RhsPacketQuarter, ResPacketQuarter,
+                                   QuarterTraits, LinearMapper, DataMapper>
+        p;
+    p(res, blockA, blockB, alpha, peeled_mc_half, peeled_mc_quarter, strideA, strideB, offsetA, offsetB,
+      prefetch_res_offset, peeled_kc, pk, cols, depth, packet_cols4);
+  }
+  //---------- Process remaining rows, 1 at once ----------
+  if (peeled_mc_quarter < rows) {
+#if EIGEN_ARCH_ARM64
+    EIGEN_IF_CONSTEXPR(nr >= 8) {
+      // loop on each panel of the rhs
+      for (Index j2 = 0; j2 < packet_cols8; j2 += 8) {
+        // loop on each row of the lhs (1*LhsProgress x depth)
+        for (Index i = peeled_mc_quarter; i < rows; i += 1) {
+          const LhsScalar* blA = &blockA[i * strideA + offsetA];
+          prefetch(&blA[0]);
+          // gets a 1 x 1 res block as registers
+          ResScalar C0(0), C1(0), C2(0), C3(0), C4(0), C5(0), C6(0), C7(0);
+          const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 8];
+          for (Index k = 0; k < depth; k++) {
+            LhsScalar A0 = blA[k];
+            RhsScalar B_0;
 
+            B_0 = blB[0];
+            C0 = cj.pmadd(A0, B_0, C0);
+
+            B_0 = blB[1];
+            C1 = cj.pmadd(A0, B_0, C1);
+
+            B_0 = blB[2];
+            C2 = cj.pmadd(A0, B_0, C2);
+
+            B_0 = blB[3];
+            C3 = cj.pmadd(A0, B_0, C3);
+
+            B_0 = blB[4];
+            C4 = cj.pmadd(A0, B_0, C4);
+
+            B_0 = blB[5];
+            C5 = cj.pmadd(A0, B_0, C5);
+
+            B_0 = blB[6];
+            C6 = cj.pmadd(A0, B_0, C6);
+
+            B_0 = blB[7];
+            C7 = cj.pmadd(A0, B_0, C7);
+
+            blB += 8;
+          }
+          res(i, j2 + 0) += alpha * C0;
+          res(i, j2 + 1) += alpha * C1;
+          res(i, j2 + 2) += alpha * C2;
+          res(i, j2 + 3) += alpha * C3;
+          res(i, j2 + 4) += alpha * C4;
+          res(i, j2 + 5) += alpha * C5;
+          res(i, j2 + 6) += alpha * C6;
+          res(i, j2 + 7) += alpha * C7;
+        }
+      }
+    }
+#endif
+
+    for (Index j2 = packet_cols8; j2 < packet_cols4; j2 += 4) {
+      // loop on each row of the lhs (1*LhsProgress x depth)
+      for (Index i = peeled_mc_quarter; i < rows; i += 1) {
+        const LhsScalar* blA = &blockA[i * strideA + offsetA];
+        prefetch(&blA[0]);
+        const RhsScalar* blB = &blockB[j2 * strideB + offsetB * 4];
+
+        // If LhsProgress is 8 or 16, it assumes that there is a
+        // half or quarter packet, respectively, of the same size as
+        // nr (which is currently 4) for the return type.
+        const int SResPacketHalfSize = unpacket_traits<typename unpacket_traits<SResPacket>::half>::size;
+        const int SResPacketQuarterSize =
+            unpacket_traits<typename unpacket_traits<typename unpacket_traits<SResPacket>::half>::half>::size;
+        // The following code assumes we can load SRhsPacket in such a way that
+        // it multiplies blocks of 4 elements in SLhsPacket.  This is not the
+        // case for some customized kernels (i.e. NEON fp16).  If the assumption
+        // fails, drop down to the scalar path.
+        constexpr bool kCanLoadSRhsQuad =
+            (unpacket_traits<SLhsPacket>::size < 4) ||
+            (unpacket_traits<SRhsPacket>::size % (unpacket_traits<SLhsPacket>::size / 4)) == 0;
+        if (kCanLoadSRhsQuad && (SwappedTraits::LhsProgress % 4) == 0 && (SwappedTraits::LhsProgress <= 16) &&
+            (SwappedTraits::LhsProgress != 8 || SResPacketHalfSize == nr) &&
+            (SwappedTraits::LhsProgress != 16 || SResPacketQuarterSize == nr)) {
+          SAccPacket C0, C1, C2, C3;
+          straits.initAcc(C0);
+          straits.initAcc(C1);
+          straits.initAcc(C2);
+          straits.initAcc(C3);
+
+          const Index spk = (std::max)(1, SwappedTraits::LhsProgress / 4);
+          const Index endk = (depth / spk) * spk;
+          const Index endk4 = (depth / (spk * 4)) * (spk * 4);
+
+          Index k = 0;
+          for (; k < endk4; k += 4 * spk) {
+            SLhsPacket A0, A1;
+            SRhsPacket B_0, B_1;
+
+            straits.loadLhsUnaligned(blB + 0 * SwappedTraits::LhsProgress, A0);
+            straits.loadLhsUnaligned(blB + 1 * SwappedTraits::LhsProgress, A1);
+
+            straits.loadRhsQuad(blA + 0 * spk, B_0);
+            straits.loadRhsQuad(blA + 1 * spk, B_1);
+            straits.madd(A0, B_0, C0, B_0, fix<0>);
+            straits.madd(A1, B_1, C1, B_1, fix<0>);
+
+            straits.loadLhsUnaligned(blB + 2 * SwappedTraits::LhsProgress, A0);
+            straits.loadLhsUnaligned(blB + 3 * SwappedTraits::LhsProgress, A1);
+            straits.loadRhsQuad(blA + 2 * spk, B_0);
+            straits.loadRhsQuad(blA + 3 * spk, B_1);
+            straits.madd(A0, B_0, C2, B_0, fix<0>);
+            straits.madd(A1, B_1, C3, B_1, fix<0>);
+
+            blB += 4 * SwappedTraits::LhsProgress;
+            blA += 4 * spk;
+          }
+          C0 = padd(padd(C0, C1), padd(C2, C3));
+          for (; k < endk; k += spk) {
+            SLhsPacket A0;
+            SRhsPacket B_0;
+
+            straits.loadLhsUnaligned(blB, A0);
+            straits.loadRhsQuad(blA, B_0);
+            straits.madd(A0, B_0, C0, B_0, fix<0>);
+
+            blB += SwappedTraits::LhsProgress;
+            blA += spk;
+          }
+          if (SwappedTraits::LhsProgress == 8) {
+            // Special case where we have to first reduce the accumulation register C0
+            typedef std::conditional_t<SwappedTraits::LhsProgress >= 8, typename unpacket_traits<SResPacket>::half,
+                                       SResPacket>
+                SResPacketHalf;
+            typedef std::conditional_t<SwappedTraits::LhsProgress >= 8, typename unpacket_traits<SLhsPacket>::half,
+                                       SLhsPacket>
+                SLhsPacketHalf;
+            typedef std::conditional_t<SwappedTraits::LhsProgress >= 8, typename unpacket_traits<SRhsPacket>::half,
+                                       SRhsPacket>
+                SRhsPacketHalf;
+            typedef std::conditional_t<SwappedTraits::LhsProgress >= 8, typename unpacket_traits<SAccPacket>::half,
+                                       SAccPacket>
+                SAccPacketHalf;
+
+            SResPacketHalf R = res.template gatherPacket<SResPacketHalf>(i, j2);
+            SResPacketHalf alphav = pset1<SResPacketHalf>(alpha);
+
+            if (depth - endk > 0) {
+              // We have to handle the last row of the rhs which corresponds to a half-packet
+              SLhsPacketHalf a0;
+              SRhsPacketHalf b0;
+              straits.loadLhsUnaligned(blB, a0);
+              straits.loadRhs(blA, b0);
+              SAccPacketHalf c0 = predux_half_dowto4(C0);
+              straits.madd(a0, b0, c0, b0, fix<0>);
+              straits.acc(c0, alphav, R);
+            } else {
+              straits.acc(predux_half_dowto4(C0), alphav, R);
+            }
+            res.scatterPacket(i, j2, R);
+          } else if (SwappedTraits::LhsProgress == 16) {
+            // Special case where we have to first reduce the
+            // accumulation register C0. We specialize the block in
+            // template form, so that LhsProgress < 16 paths don't
+            // fail to compile
+            last_row_process_16_packets<LhsScalar, RhsScalar, Index, DataMapper, mr, nr, ConjugateLhs, ConjugateRhs> p;
+            p(res, straits, blA, blB, depth, endk, i, j2, alpha, C0);
+          } else {
+            SResPacket R = res.template gatherPacket<SResPacket>(i, j2);
+            SResPacket alphav = pset1<SResPacket>(alpha);
+            straits.acc(C0, alphav, R);
+            res.scatterPacket(i, j2, R);
+          }
+        } else  // scalar path
+        {
+          // get a 1 x 4 res block as registers
+          ResScalar C0(0), C1(0), C2(0), C3(0);
+
+          for (Index k = 0; k < depth; k++) {
+            LhsScalar A0;
+            RhsScalar B_0, B_1;
+
+            A0 = blA[k];
+
+            B_0 = blB[0];
+            B_1 = blB[1];
+            C0 = cj.pmadd(A0, B_0, C0);
+            C1 = cj.pmadd(A0, B_1, C1);
+
+            B_0 = blB[2];
+            B_1 = blB[3];
+            C2 = cj.pmadd(A0, B_0, C2);
+            C3 = cj.pmadd(A0, B_1, C3);
+
+            blB += 4;
+          }
+          res(i, j2 + 0) += alpha * C0;
+          res(i, j2 + 1) += alpha * C1;
+          res(i, j2 + 2) += alpha * C2;
+          res(i, j2 + 3) += alpha * C3;
+        }
+      }
+    }
+    // remaining columns
+    for (Index j2 = packet_cols4; j2 < cols; j2++) {
+      // loop on each row of the lhs (1*LhsProgress x depth)
+      for (Index i = peeled_mc_quarter; i < rows; i += 1) {
+        const LhsScalar* blA = &blockA[i * strideA + offsetA];
+        prefetch(&blA[0]);
+        // gets a 1 x 1 res block as registers
+        ResScalar C0(0);
+        const RhsScalar* blB = &blockB[j2 * strideB + offsetB];
+        for (Index k = 0; k < depth; k++) {
+          LhsScalar A0 = blA[k];
+          RhsScalar B_0 = blB[k];
+          C0 = cj.pmadd(A0, B_0, C0);
+        }
+        res(i, j2) += alpha * C0;
+      }
+    }
+  }
+}
 
 // pack a block of the lhs
 // The traversal is as follow (mr==4):
@@ -2085,131 +2556,129 @@
 //
 //  32 33 34 35 ...
 //  36 36 38 39 ...
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
-struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, ColMajor, Conjugate, PanelMode>
-{
+template <typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate,
+          bool PanelMode>
+struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, ColMajor, Conjugate, PanelMode> {
   typedef typename DataMapper::LinearMapper LinearMapper;
-  EIGEN_DONT_INLINE void operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride=0, Index offset=0);
+  EIGEN_DONT_INLINE void operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride = 0,
+                                    Index offset = 0);
 };
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
-EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, ColMajor, Conjugate, PanelMode>
-  ::operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride, Index offset)
-{
+template <typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate,
+          bool PanelMode>
+EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, ColMajor, Conjugate,
+                                     PanelMode>::operator()(Scalar* blockA, const DataMapper& lhs, Index depth,
+                                                            Index rows, Index stride, Index offset) {
   typedef typename unpacket_traits<Packet>::half HalfPacket;
   typedef typename unpacket_traits<typename unpacket_traits<Packet>::half>::half QuarterPacket;
-  enum { PacketSize = unpacket_traits<Packet>::size,
-         HalfPacketSize = unpacket_traits<HalfPacket>::size,
-         QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
-         HasHalf = (int)HalfPacketSize < (int)PacketSize,
-         HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize};
+  enum {
+    PacketSize = unpacket_traits<Packet>::size,
+    HalfPacketSize = unpacket_traits<HalfPacket>::size,
+    QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
+    HasHalf = (int)HalfPacketSize < (int)PacketSize,
+    HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize
+  };
 
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS");
   EIGEN_UNUSED_VARIABLE(stride);
   EIGEN_UNUSED_VARIABLE(offset);
-  eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
-  eigen_assert( ((Pack1%PacketSize)==0 && Pack1<=4*PacketSize) || (Pack1<=4) );
+  eigen_assert(((!PanelMode) && stride == 0 && offset == 0) || (PanelMode && stride >= depth && offset <= stride));
+  eigen_assert(((Pack1 % PacketSize) == 0 && Pack1 <= 4 * PacketSize) || (Pack1 <= 4));
   conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
   Index count = 0;
 
-  const Index peeled_mc3 = Pack1>=3*PacketSize ? (rows/(3*PacketSize))*(3*PacketSize) : 0;
-  const Index peeled_mc2 = Pack1>=2*PacketSize ? peeled_mc3+((rows-peeled_mc3)/(2*PacketSize))*(2*PacketSize) : 0;
-  const Index peeled_mc1 = Pack1>=1*PacketSize ? peeled_mc2+((rows-peeled_mc2)/(1*PacketSize))*(1*PacketSize) : 0;
-  const Index peeled_mc_half = Pack1>=HalfPacketSize ? peeled_mc1+((rows-peeled_mc1)/(HalfPacketSize))*(HalfPacketSize) : 0;
-  const Index peeled_mc_quarter = Pack1>=QuarterPacketSize ? (rows/(QuarterPacketSize))*(QuarterPacketSize) : 0;
+  const Index peeled_mc3 = Pack1 >= 3 * PacketSize ? (rows / (3 * PacketSize)) * (3 * PacketSize) : 0;
+  const Index peeled_mc2 =
+      Pack1 >= 2 * PacketSize ? peeled_mc3 + ((rows - peeled_mc3) / (2 * PacketSize)) * (2 * PacketSize) : 0;
+  const Index peeled_mc1 =
+      Pack1 >= 1 * PacketSize ? peeled_mc2 + ((rows - peeled_mc2) / (1 * PacketSize)) * (1 * PacketSize) : 0;
+  const Index peeled_mc_half =
+      Pack1 >= HalfPacketSize ? peeled_mc1 + ((rows - peeled_mc1) / (HalfPacketSize)) * (HalfPacketSize) : 0;
+  const Index peeled_mc_quarter = Pack1 >= QuarterPacketSize ? (rows / (QuarterPacketSize)) * (QuarterPacketSize) : 0;
   const Index last_lhs_progress = rows > peeled_mc_quarter ? (rows - peeled_mc_quarter) & ~1 : 0;
-  const Index peeled_mc0 = Pack2>=PacketSize ? peeled_mc_quarter
-                         : Pack2>1 && last_lhs_progress ? (rows/last_lhs_progress)*last_lhs_progress : 0;
+  const Index peeled_mc0 = Pack2 >= PacketSize              ? peeled_mc_quarter
+                           : Pack2 > 1 && last_lhs_progress ? (rows / last_lhs_progress) * last_lhs_progress
+                                                            : 0;
 
-  Index i=0;
+  Index i = 0;
 
   // Pack 3 packets
-  if(Pack1>=3*PacketSize)
-  {
-    for(; i<peeled_mc3; i+=3*PacketSize)
-    {
-      if(PanelMode) count += (3*PacketSize) * offset;
+  if (Pack1 >= 3 * PacketSize) {
+    for (; i < peeled_mc3; i += 3 * PacketSize) {
+      if (PanelMode) count += (3 * PacketSize) * offset;
 
-      for(Index k=0; k<depth; k++)
-      {
+      for (Index k = 0; k < depth; k++) {
         Packet A, B, C;
-        A = lhs.template loadPacket<Packet>(i+0*PacketSize, k);
-        B = lhs.template loadPacket<Packet>(i+1*PacketSize, k);
-        C = lhs.template loadPacket<Packet>(i+2*PacketSize, k);
-        pstore(blockA+count, cj.pconj(A)); count+=PacketSize;
-        pstore(blockA+count, cj.pconj(B)); count+=PacketSize;
-        pstore(blockA+count, cj.pconj(C)); count+=PacketSize;
+        A = lhs.template loadPacket<Packet>(i + 0 * PacketSize, k);
+        B = lhs.template loadPacket<Packet>(i + 1 * PacketSize, k);
+        C = lhs.template loadPacket<Packet>(i + 2 * PacketSize, k);
+        pstore(blockA + count, cj.pconj(A));
+        count += PacketSize;
+        pstore(blockA + count, cj.pconj(B));
+        count += PacketSize;
+        pstore(blockA + count, cj.pconj(C));
+        count += PacketSize;
       }
-      if(PanelMode) count += (3*PacketSize) * (stride-offset-depth);
+      if (PanelMode) count += (3 * PacketSize) * (stride - offset - depth);
     }
   }
   // Pack 2 packets
-  if(Pack1>=2*PacketSize)
-  {
-    for(; i<peeled_mc2; i+=2*PacketSize)
-    {
-      if(PanelMode) count += (2*PacketSize) * offset;
+  if (Pack1 >= 2 * PacketSize) {
+    for (; i < peeled_mc2; i += 2 * PacketSize) {
+      if (PanelMode) count += (2 * PacketSize) * offset;
 
-      for(Index k=0; k<depth; k++)
-      {
+      for (Index k = 0; k < depth; k++) {
         Packet A, B;
-        A = lhs.template loadPacket<Packet>(i+0*PacketSize, k);
-        B = lhs.template loadPacket<Packet>(i+1*PacketSize, k);
-        pstore(blockA+count, cj.pconj(A)); count+=PacketSize;
-        pstore(blockA+count, cj.pconj(B)); count+=PacketSize;
+        A = lhs.template loadPacket<Packet>(i + 0 * PacketSize, k);
+        B = lhs.template loadPacket<Packet>(i + 1 * PacketSize, k);
+        pstore(blockA + count, cj.pconj(A));
+        count += PacketSize;
+        pstore(blockA + count, cj.pconj(B));
+        count += PacketSize;
       }
-      if(PanelMode) count += (2*PacketSize) * (stride-offset-depth);
+      if (PanelMode) count += (2 * PacketSize) * (stride - offset - depth);
     }
   }
   // Pack 1 packets
-  if(Pack1>=1*PacketSize)
-  {
-    for(; i<peeled_mc1; i+=1*PacketSize)
-    {
-      if(PanelMode) count += (1*PacketSize) * offset;
+  if (Pack1 >= 1 * PacketSize) {
+    for (; i < peeled_mc1; i += 1 * PacketSize) {
+      if (PanelMode) count += (1 * PacketSize) * offset;
 
-      for(Index k=0; k<depth; k++)
-      {
+      for (Index k = 0; k < depth; k++) {
         Packet A;
-        A = lhs.template loadPacket<Packet>(i+0*PacketSize, k);
-        pstore(blockA+count, cj.pconj(A));
-        count+=PacketSize;
+        A = lhs.template loadPacket<Packet>(i + 0 * PacketSize, k);
+        pstore(blockA + count, cj.pconj(A));
+        count += PacketSize;
       }
-      if(PanelMode) count += (1*PacketSize) * (stride-offset-depth);
+      if (PanelMode) count += (1 * PacketSize) * (stride - offset - depth);
     }
   }
   // Pack half packets
-  if(HasHalf && Pack1>=HalfPacketSize)
-  {
-    for(; i<peeled_mc_half; i+=HalfPacketSize)
-    {
-      if(PanelMode) count += (HalfPacketSize) * offset;
+  if (HasHalf && Pack1 >= HalfPacketSize) {
+    for (; i < peeled_mc_half; i += HalfPacketSize) {
+      if (PanelMode) count += (HalfPacketSize)*offset;
 
-      for(Index k=0; k<depth; k++)
-      {
+      for (Index k = 0; k < depth; k++) {
         HalfPacket A;
-        A = lhs.template loadPacket<HalfPacket>(i+0*(HalfPacketSize), k);
-        pstoreu(blockA+count, cj.pconj(A));
-        count+=HalfPacketSize;
+        A = lhs.template loadPacket<HalfPacket>(i + 0 * (HalfPacketSize), k);
+        pstoreu(blockA + count, cj.pconj(A));
+        count += HalfPacketSize;
       }
-      if(PanelMode) count += (HalfPacketSize) * (stride-offset-depth);
+      if (PanelMode) count += (HalfPacketSize) * (stride - offset - depth);
     }
   }
   // Pack quarter packets
-  if(HasQuarter && Pack1>=QuarterPacketSize)
-  {
-    for(; i<peeled_mc_quarter; i+=QuarterPacketSize)
-    {
-      if(PanelMode) count += (QuarterPacketSize) * offset;
+  if (HasQuarter && Pack1 >= QuarterPacketSize) {
+    for (; i < peeled_mc_quarter; i += QuarterPacketSize) {
+      if (PanelMode) count += (QuarterPacketSize)*offset;
 
-      for(Index k=0; k<depth; k++)
-      {
+      for (Index k = 0; k < depth; k++) {
         QuarterPacket A;
-        A = lhs.template loadPacket<QuarterPacket>(i+0*(QuarterPacketSize), k);
-        pstoreu(blockA+count, cj.pconj(A));
-        count+=QuarterPacketSize;
+        A = lhs.template loadPacket<QuarterPacket>(i + 0 * (QuarterPacketSize), k);
+        pstoreu(blockA + count, cj.pconj(A));
+        count += QuarterPacketSize;
       }
-      if(PanelMode) count += (QuarterPacketSize) * (stride-offset-depth);
+      if (PanelMode) count += (QuarterPacketSize) * (stride - offset - depth);
     }
   }
   // Pack2 may be *smaller* than PacketSize—that happens for
@@ -2218,128 +2687,118 @@
   // address both real & imaginary parts on the rhs. This portion will
   // pack those half ones until they match the number expected on the
   // last peeling loop at this point (for the rhs).
-  if(Pack2<PacketSize && Pack2>1)
-  {
-    for(; i<peeled_mc0; i+=last_lhs_progress)
-    {
-      if(PanelMode) count += last_lhs_progress * offset;
+  if (Pack2 < PacketSize && Pack2 > 1) {
+    for (; i < peeled_mc0; i += last_lhs_progress) {
+      if (PanelMode) count += last_lhs_progress * offset;
 
-      for(Index k=0; k<depth; k++)
-        for(Index w=0; w<last_lhs_progress; w++)
-          blockA[count++] = cj(lhs(i+w, k));
+      for (Index k = 0; k < depth; k++)
+        for (Index w = 0; w < last_lhs_progress; w++) blockA[count++] = cj(lhs(i + w, k));
 
-      if(PanelMode) count += last_lhs_progress * (stride-offset-depth);
+      if (PanelMode) count += last_lhs_progress * (stride - offset - depth);
     }
   }
   // Pack scalars
-  for(; i<rows; i++)
-  {
-    if(PanelMode) count += offset;
-    for(Index k=0; k<depth; k++)
-      blockA[count++] = cj(lhs(i, k));
-    if(PanelMode) count += (stride-offset-depth);
+  for (; i < rows; i++) {
+    if (PanelMode) count += offset;
+    for (Index k = 0; k < depth; k++) blockA[count++] = cj(lhs(i, k));
+    if (PanelMode) count += (stride - offset - depth);
   }
 }
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
-struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, RowMajor, Conjugate, PanelMode>
-{
+template <typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate,
+          bool PanelMode>
+struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, RowMajor, Conjugate, PanelMode> {
   typedef typename DataMapper::LinearMapper LinearMapper;
-  EIGEN_DONT_INLINE void operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride=0, Index offset=0);
+  EIGEN_DONT_INLINE void operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride = 0,
+                                    Index offset = 0);
 };
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
-EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, RowMajor, Conjugate, PanelMode>
-  ::operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride, Index offset)
-{
+template <typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate,
+          bool PanelMode>
+EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, RowMajor, Conjugate,
+                                     PanelMode>::operator()(Scalar* blockA, const DataMapper& lhs, Index depth,
+                                                            Index rows, Index stride, Index offset) {
   typedef typename unpacket_traits<Packet>::half HalfPacket;
   typedef typename unpacket_traits<typename unpacket_traits<Packet>::half>::half QuarterPacket;
-  enum { PacketSize = unpacket_traits<Packet>::size,
-         HalfPacketSize = unpacket_traits<HalfPacket>::size,
-         QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
-         HasHalf = (int)HalfPacketSize < (int)PacketSize,
-         HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize};
+  enum {
+    PacketSize = unpacket_traits<Packet>::size,
+    HalfPacketSize = unpacket_traits<HalfPacket>::size,
+    QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
+    HasHalf = (int)HalfPacketSize < (int)PacketSize,
+    HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize
+  };
 
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS");
   EIGEN_UNUSED_VARIABLE(stride);
   EIGEN_UNUSED_VARIABLE(offset);
-  eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
+  eigen_assert(((!PanelMode) && stride == 0 && offset == 0) || (PanelMode && stride >= depth && offset <= stride));
   conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
   Index count = 0;
   bool gone_half = false, gone_quarter = false, gone_last = false;
 
   Index i = 0;
-  int pack = Pack1;
-  int psize = PacketSize;
-  while(pack>0)
-  {
-    Index remaining_rows = rows-i;
-    Index peeled_mc = gone_last ? Pack2>1 ? (rows/pack)*pack : 0 : i+(remaining_rows/pack)*pack;
+  Index pack = Pack1;
+  Index psize = PacketSize;
+  while (pack > 0) {
+    Index remaining_rows = rows - i;
+    Index peeled_mc = gone_last ? Pack2 > 1 ? (rows / pack) * pack : 0 : i + (remaining_rows / pack) * pack;
     Index starting_pos = i;
-    for(; i<peeled_mc; i+=pack)
-    {
-      if(PanelMode) count += pack * offset;
+    for (; i < peeled_mc; i += pack) {
+      if (PanelMode) count += pack * offset;
 
-      Index k=0;
-      if(pack>=psize && psize >= QuarterPacketSize)
-      {
-        const Index peeled_k = (depth/psize)*psize;
-        for(; k<peeled_k; k+=psize)
-        {
-          for (Index m = 0; m < pack; m += psize)
-          {
+      Index k = 0;
+      if (pack >= psize && psize >= QuarterPacketSize) {
+        const Index peeled_k = (depth / psize) * psize;
+        for (; k < peeled_k; k += psize) {
+          for (Index m = 0; m < pack; m += psize) {
             if (psize == PacketSize) {
               PacketBlock<Packet> kernel;
-              for (int p = 0; p < psize; ++p) kernel.packet[p] = lhs.template loadPacket<Packet>(i+p+m, k);
+              for (Index p = 0; p < psize; ++p) kernel.packet[p] = lhs.template loadPacket<Packet>(i + p + m, k);
               ptranspose(kernel);
-              for (int p = 0; p < psize; ++p) pstore(blockA+count+m+(pack)*p, cj.pconj(kernel.packet[p]));
+              for (Index p = 0; p < psize; ++p) pstore(blockA + count + m + (pack)*p, cj.pconj(kernel.packet[p]));
             } else if (HasHalf && psize == HalfPacketSize) {
               gone_half = true;
               PacketBlock<HalfPacket> kernel_half;
-              for (int p = 0; p < psize; ++p) kernel_half.packet[p] = lhs.template loadPacket<HalfPacket>(i+p+m, k);
+              for (Index p = 0; p < psize; ++p)
+                kernel_half.packet[p] = lhs.template loadPacket<HalfPacket>(i + p + m, k);
               ptranspose(kernel_half);
-              for (int p = 0; p < psize; ++p) pstore(blockA+count+m+(pack)*p, cj.pconj(kernel_half.packet[p]));
+              for (Index p = 0; p < psize; ++p) pstore(blockA + count + m + (pack)*p, cj.pconj(kernel_half.packet[p]));
             } else if (HasQuarter && psize == QuarterPacketSize) {
               gone_quarter = true;
               PacketBlock<QuarterPacket> kernel_quarter;
-              for (int p = 0; p < psize; ++p) kernel_quarter.packet[p] = lhs.template loadPacket<QuarterPacket>(i+p+m, k);
+              for (Index p = 0; p < psize; ++p)
+                kernel_quarter.packet[p] = lhs.template loadPacket<QuarterPacket>(i + p + m, k);
               ptranspose(kernel_quarter);
-              for (int p = 0; p < psize; ++p) pstore(blockA+count+m+(pack)*p, cj.pconj(kernel_quarter.packet[p]));
-	    }
+              for (Index p = 0; p < psize; ++p)
+                pstore(blockA + count + m + (pack)*p, cj.pconj(kernel_quarter.packet[p]));
+            }
           }
-          count += psize*pack;
+          count += psize * pack;
         }
       }
 
-      for(; k<depth; k++)
-      {
-        Index w=0;
-        for(; w<pack-3; w+=4)
-        {
-          Scalar a(cj(lhs(i+w+0, k))),
-                 b(cj(lhs(i+w+1, k))),
-                 c(cj(lhs(i+w+2, k))),
-                 d(cj(lhs(i+w+3, k)));
+      for (; k < depth; k++) {
+        Index w = 0;
+        for (; w < pack - 3; w += 4) {
+          Scalar a(cj(lhs(i + w + 0, k))), b(cj(lhs(i + w + 1, k))), c(cj(lhs(i + w + 2, k))), d(cj(lhs(i + w + 3, k)));
           blockA[count++] = a;
           blockA[count++] = b;
           blockA[count++] = c;
           blockA[count++] = d;
         }
-        if(pack%4)
-          for(;w<pack;++w)
-            blockA[count++] = cj(lhs(i+w, k));
+        if (pack % 4)
+          for (; w < pack; ++w) blockA[count++] = cj(lhs(i + w, k));
       }
 
-      if(PanelMode) count += pack * (stride-offset-depth);
+      if (PanelMode) count += pack * (stride - offset - depth);
     }
 
     pack -= psize;
     Index left = rows - i;
     if (pack <= 0) {
-      if (!gone_last &&
-          (starting_pos == i || left >= psize/2 || left >= psize/4) &&
-          ((psize/2 == HalfPacketSize && HasHalf && !gone_half) ||
-           (psize/2 == QuarterPacketSize && HasQuarter && !gone_quarter))) {
+      if (!gone_last && (starting_pos == i || left >= psize / 2 || left >= psize / 4) &&
+          ((psize / 2 == HalfPacketSize && HasHalf && !gone_half) ||
+           (psize / 2 == QuarterPacketSize && HasQuarter && !gone_quarter))) {
         psize /= 2;
         pack = psize;
         continue;
@@ -2357,12 +2816,10 @@
     }
   }
 
-  for(; i<rows; i++)
-  {
-    if(PanelMode) count += offset;
-    for(Index k=0; k<depth; k++)
-      blockA[count++] = cj(lhs(i, k));
-    if(PanelMode) count += (stride-offset-depth);
+  for (; i < rows; i++) {
+    if (PanelMode) count += offset;
+    for (Index k = 0; k < depth; k++) blockA[count++] = cj(lhs(i, k));
+    if (PanelMode) count += (stride - offset - depth);
   }
 }
 
@@ -2373,273 +2830,324 @@
 //  4  5  6  7   16 17 18 19   25 28
 //  8  9 10 11   20 21 22 23   26 29
 //  .  .  .  .    .  .  .  .    .  .
-template<typename Scalar, typename Index, typename DataMapper, int nr, bool Conjugate, bool PanelMode>
-struct gemm_pack_rhs<Scalar, Index, DataMapper, nr, ColMajor, Conjugate, PanelMode>
-{
+template <typename Scalar, typename Index, typename DataMapper, int nr, bool Conjugate, bool PanelMode>
+struct gemm_pack_rhs<Scalar, Index, DataMapper, nr, ColMajor, Conjugate, PanelMode> {
   typedef typename packet_traits<Scalar>::type Packet;
   typedef typename DataMapper::LinearMapper LinearMapper;
   enum { PacketSize = packet_traits<Scalar>::size };
-  EIGEN_DONT_INLINE void operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride=0, Index offset=0);
+  EIGEN_DONT_INLINE void operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride = 0,
+                                    Index offset = 0);
 };
 
-template<typename Scalar, typename Index, typename DataMapper, int nr, bool Conjugate, bool PanelMode>
-EIGEN_DONT_INLINE void gemm_pack_rhs<Scalar, Index, DataMapper, nr, ColMajor, Conjugate, PanelMode>
-  ::operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride, Index offset)
-{
+template <typename Scalar, typename Index, typename DataMapper, int nr, bool Conjugate, bool PanelMode>
+EIGEN_DONT_INLINE void gemm_pack_rhs<Scalar, Index, DataMapper, nr, ColMajor, Conjugate, PanelMode>::operator()(
+    Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride, Index offset) {
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS COLMAJOR");
   EIGEN_UNUSED_VARIABLE(stride);
   EIGEN_UNUSED_VARIABLE(offset);
-  eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
+  eigen_assert(((!PanelMode) && stride == 0 && offset == 0) || (PanelMode && stride >= depth && offset <= stride));
   conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
-  Index packet_cols8 = nr>=8 ? (cols/8) * 8 : 0;
-  Index packet_cols4 = nr>=4 ? (cols/4) * 4 : 0;
+  Index packet_cols8 = nr >= 8 ? (cols / 8) * 8 : 0;
+  Index packet_cols4 = nr >= 4 ? (cols / 4) * 4 : 0;
   Index count = 0;
-  const Index peeled_k = (depth/PacketSize)*PacketSize;
-//   if(nr>=8)
-//   {
-//     for(Index j2=0; j2<packet_cols8; j2+=8)
-//     {
-//       // skip what we have before
-//       if(PanelMode) count += 8 * offset;
-//       const Scalar* b0 = &rhs[(j2+0)*rhsStride];
-//       const Scalar* b1 = &rhs[(j2+1)*rhsStride];
-//       const Scalar* b2 = &rhs[(j2+2)*rhsStride];
-//       const Scalar* b3 = &rhs[(j2+3)*rhsStride];
-//       const Scalar* b4 = &rhs[(j2+4)*rhsStride];
-//       const Scalar* b5 = &rhs[(j2+5)*rhsStride];
-//       const Scalar* b6 = &rhs[(j2+6)*rhsStride];
-//       const Scalar* b7 = &rhs[(j2+7)*rhsStride];
-//       Index k=0;
-//       if(PacketSize==8) // TODO enable vectorized transposition for PacketSize==4
-//       {
-//         for(; k<peeled_k; k+=PacketSize) {
-//           PacketBlock<Packet> kernel;
-//           for (int p = 0; p < PacketSize; ++p) {
-//             kernel.packet[p] = ploadu<Packet>(&rhs[(j2+p)*rhsStride+k]);
-//           }
-//           ptranspose(kernel);
-//           for (int p = 0; p < PacketSize; ++p) {
-//             pstoreu(blockB+count, cj.pconj(kernel.packet[p]));
-//             count+=PacketSize;
-//           }
-//         }
-//       }
-//       for(; k<depth; k++)
-//       {
-//         blockB[count+0] = cj(b0[k]);
-//         blockB[count+1] = cj(b1[k]);
-//         blockB[count+2] = cj(b2[k]);
-//         blockB[count+3] = cj(b3[k]);
-//         blockB[count+4] = cj(b4[k]);
-//         blockB[count+5] = cj(b5[k]);
-//         blockB[count+6] = cj(b6[k]);
-//         blockB[count+7] = cj(b7[k]);
-//         count += 8;
-//       }
-//       // skip what we have after
-//       if(PanelMode) count += 8 * (stride-offset-depth);
-//     }
-//   }
+  const Index peeled_k = (depth / PacketSize) * PacketSize;
 
-  if(nr>=4)
-  {
-    for(Index j2=packet_cols8; j2<packet_cols4; j2+=4)
-    {
+#if EIGEN_ARCH_ARM64
+  EIGEN_IF_CONSTEXPR(nr >= 8) {
+    for (Index j2 = 0; j2 < packet_cols8; j2 += 8) {
       // skip what we have before
-      if(PanelMode) count += 4 * offset;
+      if (PanelMode) count += 8 * offset;
+      const LinearMapper dm0 = rhs.getLinearMapper(0, j2 + 0);
+      const LinearMapper dm1 = rhs.getLinearMapper(0, j2 + 1);
+      const LinearMapper dm2 = rhs.getLinearMapper(0, j2 + 2);
+      const LinearMapper dm3 = rhs.getLinearMapper(0, j2 + 3);
+      const LinearMapper dm4 = rhs.getLinearMapper(0, j2 + 4);
+      const LinearMapper dm5 = rhs.getLinearMapper(0, j2 + 5);
+      const LinearMapper dm6 = rhs.getLinearMapper(0, j2 + 6);
+      const LinearMapper dm7 = rhs.getLinearMapper(0, j2 + 7);
+      Index k = 0;
+      if (PacketSize % 2 == 0 && PacketSize <= 8)  // 2 4 8
+      {
+        for (; k < peeled_k; k += PacketSize) {
+          if (PacketSize == 2) {
+            PacketBlock<Packet, PacketSize == 2 ? 2 : PacketSize> kernel0, kernel1, kernel2, kernel3;
+            kernel0.packet[0 % PacketSize] = dm0.template loadPacket<Packet>(k);
+            kernel0.packet[1 % PacketSize] = dm1.template loadPacket<Packet>(k);
+            kernel1.packet[0 % PacketSize] = dm2.template loadPacket<Packet>(k);
+            kernel1.packet[1 % PacketSize] = dm3.template loadPacket<Packet>(k);
+            kernel2.packet[0 % PacketSize] = dm4.template loadPacket<Packet>(k);
+            kernel2.packet[1 % PacketSize] = dm5.template loadPacket<Packet>(k);
+            kernel3.packet[0 % PacketSize] = dm6.template loadPacket<Packet>(k);
+            kernel3.packet[1 % PacketSize] = dm7.template loadPacket<Packet>(k);
+            ptranspose(kernel0);
+            ptranspose(kernel1);
+            ptranspose(kernel2);
+            ptranspose(kernel3);
+
+            pstoreu(blockB + count + 0 * PacketSize, cj.pconj(kernel0.packet[0 % PacketSize]));
+            pstoreu(blockB + count + 1 * PacketSize, cj.pconj(kernel1.packet[0 % PacketSize]));
+            pstoreu(blockB + count + 2 * PacketSize, cj.pconj(kernel2.packet[0 % PacketSize]));
+            pstoreu(blockB + count + 3 * PacketSize, cj.pconj(kernel3.packet[0 % PacketSize]));
+
+            pstoreu(blockB + count + 4 * PacketSize, cj.pconj(kernel0.packet[1 % PacketSize]));
+            pstoreu(blockB + count + 5 * PacketSize, cj.pconj(kernel1.packet[1 % PacketSize]));
+            pstoreu(blockB + count + 6 * PacketSize, cj.pconj(kernel2.packet[1 % PacketSize]));
+            pstoreu(blockB + count + 7 * PacketSize, cj.pconj(kernel3.packet[1 % PacketSize]));
+            count += 8 * PacketSize;
+          } else if (PacketSize == 4) {
+            PacketBlock<Packet, PacketSize == 4 ? 4 : PacketSize> kernel0, kernel1;
+
+            kernel0.packet[0 % PacketSize] = dm0.template loadPacket<Packet>(k);
+            kernel0.packet[1 % PacketSize] = dm1.template loadPacket<Packet>(k);
+            kernel0.packet[2 % PacketSize] = dm2.template loadPacket<Packet>(k);
+            kernel0.packet[3 % PacketSize] = dm3.template loadPacket<Packet>(k);
+            kernel1.packet[0 % PacketSize] = dm4.template loadPacket<Packet>(k);
+            kernel1.packet[1 % PacketSize] = dm5.template loadPacket<Packet>(k);
+            kernel1.packet[2 % PacketSize] = dm6.template loadPacket<Packet>(k);
+            kernel1.packet[3 % PacketSize] = dm7.template loadPacket<Packet>(k);
+            ptranspose(kernel0);
+            ptranspose(kernel1);
+
+            pstoreu(blockB + count + 0 * PacketSize, cj.pconj(kernel0.packet[0 % PacketSize]));
+            pstoreu(blockB + count + 1 * PacketSize, cj.pconj(kernel1.packet[0 % PacketSize]));
+            pstoreu(blockB + count + 2 * PacketSize, cj.pconj(kernel0.packet[1 % PacketSize]));
+            pstoreu(blockB + count + 3 * PacketSize, cj.pconj(kernel1.packet[1 % PacketSize]));
+            pstoreu(blockB + count + 4 * PacketSize, cj.pconj(kernel0.packet[2 % PacketSize]));
+            pstoreu(blockB + count + 5 * PacketSize, cj.pconj(kernel1.packet[2 % PacketSize]));
+            pstoreu(blockB + count + 6 * PacketSize, cj.pconj(kernel0.packet[3 % PacketSize]));
+            pstoreu(blockB + count + 7 * PacketSize, cj.pconj(kernel1.packet[3 % PacketSize]));
+            count += 8 * PacketSize;
+          } else if (PacketSize == 8) {
+            PacketBlock<Packet, PacketSize == 8 ? 8 : PacketSize> kernel0;
+
+            kernel0.packet[0 % PacketSize] = dm0.template loadPacket<Packet>(k);
+            kernel0.packet[1 % PacketSize] = dm1.template loadPacket<Packet>(k);
+            kernel0.packet[2 % PacketSize] = dm2.template loadPacket<Packet>(k);
+            kernel0.packet[3 % PacketSize] = dm3.template loadPacket<Packet>(k);
+            kernel0.packet[4 % PacketSize] = dm4.template loadPacket<Packet>(k);
+            kernel0.packet[5 % PacketSize] = dm5.template loadPacket<Packet>(k);
+            kernel0.packet[6 % PacketSize] = dm6.template loadPacket<Packet>(k);
+            kernel0.packet[7 % PacketSize] = dm7.template loadPacket<Packet>(k);
+            ptranspose(kernel0);
+
+            pstoreu(blockB + count + 0 * PacketSize, cj.pconj(kernel0.packet[0 % PacketSize]));
+            pstoreu(blockB + count + 1 * PacketSize, cj.pconj(kernel0.packet[1 % PacketSize]));
+            pstoreu(blockB + count + 2 * PacketSize, cj.pconj(kernel0.packet[2 % PacketSize]));
+            pstoreu(blockB + count + 3 * PacketSize, cj.pconj(kernel0.packet[3 % PacketSize]));
+            pstoreu(blockB + count + 4 * PacketSize, cj.pconj(kernel0.packet[4 % PacketSize]));
+            pstoreu(blockB + count + 5 * PacketSize, cj.pconj(kernel0.packet[5 % PacketSize]));
+            pstoreu(blockB + count + 6 * PacketSize, cj.pconj(kernel0.packet[6 % PacketSize]));
+            pstoreu(blockB + count + 7 * PacketSize, cj.pconj(kernel0.packet[7 % PacketSize]));
+            count += 8 * PacketSize;
+          }
+        }
+      }
+
+      for (; k < depth; k++) {
+        blockB[count + 0] = cj(dm0(k));
+        blockB[count + 1] = cj(dm1(k));
+        blockB[count + 2] = cj(dm2(k));
+        blockB[count + 3] = cj(dm3(k));
+        blockB[count + 4] = cj(dm4(k));
+        blockB[count + 5] = cj(dm5(k));
+        blockB[count + 6] = cj(dm6(k));
+        blockB[count + 7] = cj(dm7(k));
+        count += 8;
+      }
+      // skip what we have after
+      if (PanelMode) count += 8 * (stride - offset - depth);
+    }
+  }
+#endif
+
+  EIGEN_IF_CONSTEXPR(nr >= 4) {
+    for (Index j2 = packet_cols8; j2 < packet_cols4; j2 += 4) {
+      // skip what we have before
+      if (PanelMode) count += 4 * offset;
       const LinearMapper dm0 = rhs.getLinearMapper(0, j2 + 0);
       const LinearMapper dm1 = rhs.getLinearMapper(0, j2 + 1);
       const LinearMapper dm2 = rhs.getLinearMapper(0, j2 + 2);
       const LinearMapper dm3 = rhs.getLinearMapper(0, j2 + 3);
 
-      Index k=0;
-      if((PacketSize%4)==0) // TODO enable vectorized transposition for PacketSize==2 ??
+      Index k = 0;
+      if ((PacketSize % 4) == 0)  // TODO enable vectorized transposition for PacketSize==2 ??
       {
-        for(; k<peeled_k; k+=PacketSize) {
-          PacketBlock<Packet,(PacketSize%4)==0?4:PacketSize> kernel;
-          kernel.packet[0           ] = dm0.template loadPacket<Packet>(k);
-          kernel.packet[1%PacketSize] = dm1.template loadPacket<Packet>(k);
-          kernel.packet[2%PacketSize] = dm2.template loadPacket<Packet>(k);
-          kernel.packet[3%PacketSize] = dm3.template loadPacket<Packet>(k);
+        for (; k < peeled_k; k += PacketSize) {
+          PacketBlock<Packet, (PacketSize % 4) == 0 ? 4 : PacketSize> kernel;
+          kernel.packet[0] = dm0.template loadPacket<Packet>(k);
+          kernel.packet[1 % PacketSize] = dm1.template loadPacket<Packet>(k);
+          kernel.packet[2 % PacketSize] = dm2.template loadPacket<Packet>(k);
+          kernel.packet[3 % PacketSize] = dm3.template loadPacket<Packet>(k);
           ptranspose(kernel);
-          pstoreu(blockB+count+0*PacketSize, cj.pconj(kernel.packet[0]));
-          pstoreu(blockB+count+1*PacketSize, cj.pconj(kernel.packet[1%PacketSize]));
-          pstoreu(blockB+count+2*PacketSize, cj.pconj(kernel.packet[2%PacketSize]));
-          pstoreu(blockB+count+3*PacketSize, cj.pconj(kernel.packet[3%PacketSize]));
-          count+=4*PacketSize;
+          pstoreu(blockB + count + 0 * PacketSize, cj.pconj(kernel.packet[0]));
+          pstoreu(blockB + count + 1 * PacketSize, cj.pconj(kernel.packet[1 % PacketSize]));
+          pstoreu(blockB + count + 2 * PacketSize, cj.pconj(kernel.packet[2 % PacketSize]));
+          pstoreu(blockB + count + 3 * PacketSize, cj.pconj(kernel.packet[3 % PacketSize]));
+          count += 4 * PacketSize;
         }
       }
-      for(; k<depth; k++)
-      {
-        blockB[count+0] = cj(dm0(k));
-        blockB[count+1] = cj(dm1(k));
-        blockB[count+2] = cj(dm2(k));
-        blockB[count+3] = cj(dm3(k));
+      for (; k < depth; k++) {
+        blockB[count + 0] = cj(dm0(k));
+        blockB[count + 1] = cj(dm1(k));
+        blockB[count + 2] = cj(dm2(k));
+        blockB[count + 3] = cj(dm3(k));
         count += 4;
       }
       // skip what we have after
-      if(PanelMode) count += 4 * (stride-offset-depth);
+      if (PanelMode) count += 4 * (stride - offset - depth);
     }
   }
 
   // copy the remaining columns one at a time (nr==1)
-  for(Index j2=packet_cols4; j2<cols; ++j2)
-  {
-    if(PanelMode) count += offset;
+  for (Index j2 = packet_cols4; j2 < cols; ++j2) {
+    if (PanelMode) count += offset;
     const LinearMapper dm0 = rhs.getLinearMapper(0, j2);
-    for(Index k=0; k<depth; k++)
-    {
+    for (Index k = 0; k < depth; k++) {
       blockB[count] = cj(dm0(k));
       count += 1;
     }
-    if(PanelMode) count += (stride-offset-depth);
+    if (PanelMode) count += (stride - offset - depth);
   }
 }
 
 // this version is optimized for row major matrices
-template<typename Scalar, typename Index, typename DataMapper, int nr, bool Conjugate, bool PanelMode>
-struct gemm_pack_rhs<Scalar, Index, DataMapper, nr, RowMajor, Conjugate, PanelMode>
-{
+template <typename Scalar, typename Index, typename DataMapper, int nr, bool Conjugate, bool PanelMode>
+struct gemm_pack_rhs<Scalar, Index, DataMapper, nr, RowMajor, Conjugate, PanelMode> {
   typedef typename packet_traits<Scalar>::type Packet;
   typedef typename unpacket_traits<Packet>::half HalfPacket;
   typedef typename unpacket_traits<typename unpacket_traits<Packet>::half>::half QuarterPacket;
   typedef typename DataMapper::LinearMapper LinearMapper;
-  enum { PacketSize = packet_traits<Scalar>::size,
-         HalfPacketSize = unpacket_traits<HalfPacket>::size,
-		 QuarterPacketSize = unpacket_traits<QuarterPacket>::size};
-  EIGEN_DONT_INLINE void operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride=0, Index offset=0)
-  {
+  enum {
+    PacketSize = packet_traits<Scalar>::size,
+    HalfPacketSize = unpacket_traits<HalfPacket>::size,
+    QuarterPacketSize = unpacket_traits<QuarterPacket>::size
+  };
+  EIGEN_DONT_INLINE void operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride = 0,
+                                    Index offset = 0) {
     EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS ROWMAJOR");
     EIGEN_UNUSED_VARIABLE(stride);
     EIGEN_UNUSED_VARIABLE(offset);
-    eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
+    eigen_assert(((!PanelMode) && stride == 0 && offset == 0) || (PanelMode && stride >= depth && offset <= stride));
     const bool HasHalf = (int)HalfPacketSize < (int)PacketSize;
     const bool HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize;
     conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
-    Index packet_cols8 = nr>=8 ? (cols/8) * 8 : 0;
-    Index packet_cols4 = nr>=4 ? (cols/4) * 4 : 0;
+    Index packet_cols8 = nr >= 8 ? (cols / 8) * 8 : 0;
+    Index packet_cols4 = nr >= 4 ? (cols / 4) * 4 : 0;
     Index count = 0;
 
-  //   if(nr>=8)
-  //   {
-  //     for(Index j2=0; j2<packet_cols8; j2+=8)
-  //     {
-  //       // skip what we have before
-  //       if(PanelMode) count += 8 * offset;
-  //       for(Index k=0; k<depth; k++)
-  //       {
-  //         if (PacketSize==8) {
-  //           Packet A = ploadu<Packet>(&rhs[k*rhsStride + j2]);
-  //           pstoreu(blockB+count, cj.pconj(A));
-  //         } else if (PacketSize==4) {
-  //           Packet A = ploadu<Packet>(&rhs[k*rhsStride + j2]);
-  //           Packet B = ploadu<Packet>(&rhs[k*rhsStride + j2 + PacketSize]);
-  //           pstoreu(blockB+count, cj.pconj(A));
-  //           pstoreu(blockB+count+PacketSize, cj.pconj(B));
-  //         } else {
-  //           const Scalar* b0 = &rhs[k*rhsStride + j2];
-  //           blockB[count+0] = cj(b0[0]);
-  //           blockB[count+1] = cj(b0[1]);
-  //           blockB[count+2] = cj(b0[2]);
-  //           blockB[count+3] = cj(b0[3]);
-  //           blockB[count+4] = cj(b0[4]);
-  //           blockB[count+5] = cj(b0[5]);
-  //           blockB[count+6] = cj(b0[6]);
-  //           blockB[count+7] = cj(b0[7]);
-  //         }
-  //         count += 8;
-  //       }
-  //       // skip what we have after
-  //       if(PanelMode) count += 8 * (stride-offset-depth);
-  //     }
-  //   }
-    if(nr>=4)
-    {
-      for(Index j2=packet_cols8; j2<packet_cols4; j2+=4)
-      {
+#if EIGEN_ARCH_ARM64
+    EIGEN_IF_CONSTEXPR(nr >= 8) {
+      for (Index j2 = 0; j2 < packet_cols8; j2 += 8) {
         // skip what we have before
-        if(PanelMode) count += 4 * offset;
-        for(Index k=0; k<depth; k++)
-        {
-          if (PacketSize==4) {
+        if (PanelMode) count += 8 * offset;
+        for (Index k = 0; k < depth; k++) {
+          if (PacketSize == 8) {
             Packet A = rhs.template loadPacket<Packet>(k, j2);
-            pstoreu(blockB+count, cj.pconj(A));
+            pstoreu(blockB + count, cj.pconj(A));
             count += PacketSize;
-          } else if (HasHalf && HalfPacketSize==4) {
+          } else if (PacketSize == 4) {
+            Packet A = rhs.template loadPacket<Packet>(k, j2);
+            Packet B = rhs.template loadPacket<Packet>(k, j2 + 4);
+            pstoreu(blockB + count, cj.pconj(A));
+            pstoreu(blockB + count + PacketSize, cj.pconj(B));
+            count += 2 * PacketSize;
+          } else {
+            const LinearMapper dm0 = rhs.getLinearMapper(k, j2);
+            blockB[count + 0] = cj(dm0(0));
+            blockB[count + 1] = cj(dm0(1));
+            blockB[count + 2] = cj(dm0(2));
+            blockB[count + 3] = cj(dm0(3));
+            blockB[count + 4] = cj(dm0(4));
+            blockB[count + 5] = cj(dm0(5));
+            blockB[count + 6] = cj(dm0(6));
+            blockB[count + 7] = cj(dm0(7));
+            count += 8;
+          }
+        }
+        // skip what we have after
+        if (PanelMode) count += 8 * (stride - offset - depth);
+      }
+    }
+#endif
+
+    if (nr >= 4) {
+      for (Index j2 = packet_cols8; j2 < packet_cols4; j2 += 4) {
+        // skip what we have before
+        if (PanelMode) count += 4 * offset;
+        for (Index k = 0; k < depth; k++) {
+          if (PacketSize == 4) {
+            Packet A = rhs.template loadPacket<Packet>(k, j2);
+            pstoreu(blockB + count, cj.pconj(A));
+            count += PacketSize;
+          } else if (HasHalf && HalfPacketSize == 4) {
             HalfPacket A = rhs.template loadPacket<HalfPacket>(k, j2);
-            pstoreu(blockB+count, cj.pconj(A));
+            pstoreu(blockB + count, cj.pconj(A));
             count += HalfPacketSize;
-          } else if (HasQuarter && QuarterPacketSize==4) {
+          } else if (HasQuarter && QuarterPacketSize == 4) {
             QuarterPacket A = rhs.template loadPacket<QuarterPacket>(k, j2);
-            pstoreu(blockB+count, cj.pconj(A));
+            pstoreu(blockB + count, cj.pconj(A));
             count += QuarterPacketSize;
           } else {
             const LinearMapper dm0 = rhs.getLinearMapper(k, j2);
-            blockB[count+0] = cj(dm0(0));
-            blockB[count+1] = cj(dm0(1));
-            blockB[count+2] = cj(dm0(2));
-            blockB[count+3] = cj(dm0(3));
+            blockB[count + 0] = cj(dm0(0));
+            blockB[count + 1] = cj(dm0(1));
+            blockB[count + 2] = cj(dm0(2));
+            blockB[count + 3] = cj(dm0(3));
             count += 4;
           }
         }
         // skip what we have after
-        if(PanelMode) count += 4 * (stride-offset-depth);
+        if (PanelMode) count += 4 * (stride - offset - depth);
       }
     }
     // copy the remaining columns one at a time (nr==1)
-    for(Index j2=packet_cols4; j2<cols; ++j2)
-    {
-      if(PanelMode) count += offset;
-      for(Index k=0; k<depth; k++)
-      {
+    for (Index j2 = packet_cols4; j2 < cols; ++j2) {
+      if (PanelMode) count += offset;
+      for (Index k = 0; k < depth; k++) {
         blockB[count] = cj(rhs(k, j2));
         count += 1;
       }
-      if(PanelMode) count += stride-offset-depth;
+      if (PanelMode) count += stride - offset - depth;
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \returns the currently set level 1 cpu cache size (in bytes) used to estimate the ideal blocking size parameters.
-  * \sa setCpuCacheSize */
-inline std::ptrdiff_t l1CacheSize()
-{
+ * \sa setCpuCacheSize */
+inline std::ptrdiff_t l1CacheSize() {
   std::ptrdiff_t l1, l2, l3;
   internal::manage_caching_sizes(GetAction, &l1, &l2, &l3);
   return l1;
 }
 
 /** \returns the currently set level 2 cpu cache size (in bytes) used to estimate the ideal blocking size parameters.
-  * \sa setCpuCacheSize */
-inline std::ptrdiff_t l2CacheSize()
-{
+ * \sa setCpuCacheSize */
+inline std::ptrdiff_t l2CacheSize() {
   std::ptrdiff_t l1, l2, l3;
   internal::manage_caching_sizes(GetAction, &l1, &l2, &l3);
   return l2;
 }
 
 /** \returns the currently set level 3 cpu cache size (in bytes) used to estimate the ideal blocking size paramete\
-rs.                                                                                                                
+rs.
 * \sa setCpuCacheSize */
-inline std::ptrdiff_t l3CacheSize()
-{
+inline std::ptrdiff_t l3CacheSize() {
   std::ptrdiff_t l1, l2, l3;
   internal::manage_caching_sizes(GetAction, &l1, &l2, &l3);
   return l3;
 }
 
 /** Set the cpu L1 and L2 cache sizes (in bytes).
-  * These values are use to adjust the size of the blocks
-  * for the algorithms working per blocks.
-  *
-  * \sa computeProductBlockingSizes */
-inline void setCpuCacheSizes(std::ptrdiff_t l1, std::ptrdiff_t l2, std::ptrdiff_t l3)
-{
+ * These values are use to adjust the size of the blocks
+ * for the algorithms working per blocks.
+ *
+ * \sa computeProductBlockingSizes */
+inline void setCpuCacheSizes(std::ptrdiff_t l1, std::ptrdiff_t l2, std::ptrdiff_t l3) {
   internal::manage_caching_sizes(SetAction, &l1, &l2, &l3);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_GENERAL_BLOCK_PANEL_H
+#endif  // EIGEN_GENERAL_BLOCK_PANEL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h
index caa65fc..55fa5ff 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h
@@ -10,508 +10,448 @@
 #ifndef EIGEN_GENERAL_MATRIX_MATRIX_H
 #define EIGEN_GENERAL_MATRIX_MATRIX_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename _LhsScalar, typename _RhsScalar> class level3_blocking;
+template <typename LhsScalar_, typename RhsScalar_>
+class level3_blocking;
 
 /* Specialization for a row-major destination matrix => simple transposition of the product */
-template<
-  typename Index,
-  typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-  typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-  int ResInnerStride>
-struct general_matrix_matrix_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,RowMajor,ResInnerStride>
-{
-  typedef gebp_traits<RhsScalar,LhsScalar> Traits;
+template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar,
+          int RhsStorageOrder, bool ConjugateRhs, int ResInnerStride>
+struct general_matrix_matrix_product<Index, LhsScalar, LhsStorageOrder, ConjugateLhs, RhsScalar, RhsStorageOrder,
+                                     ConjugateRhs, RowMajor, ResInnerStride> {
+  typedef gebp_traits<RhsScalar, LhsScalar> Traits;
 
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
-  static EIGEN_STRONG_INLINE void run(
-    Index rows, Index cols, Index depth,
-    const LhsScalar* lhs, Index lhsStride,
-    const RhsScalar* rhs, Index rhsStride,
-    ResScalar* res, Index resIncr, Index resStride,
-    ResScalar alpha,
-    level3_blocking<RhsScalar,LhsScalar>& blocking,
-    GemmParallelInfo<Index>* info = 0)
-  {
+  static EIGEN_STRONG_INLINE void run(Index rows, Index cols, Index depth, const LhsScalar* lhs, Index lhsStride,
+                                      const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resIncr,
+                                      Index resStride, ResScalar alpha, level3_blocking<RhsScalar, LhsScalar>& blocking,
+                                      GemmParallelInfo<Index>* info = 0) {
     // transpose the product such that the result is column major
-    general_matrix_matrix_product<Index,
-      RhsScalar, RhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateRhs,
-      LhsScalar, LhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateLhs,
-      ColMajor,ResInnerStride>
-    ::run(cols,rows,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking,info);
+    general_matrix_matrix_product<Index, RhsScalar, RhsStorageOrder == RowMajor ? ColMajor : RowMajor, ConjugateRhs,
+                                  LhsScalar, LhsStorageOrder == RowMajor ? ColMajor : RowMajor, ConjugateLhs, ColMajor,
+                                  ResInnerStride>::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resIncr,
+                                                       resStride, alpha, blocking, info);
   }
 };
 
 /*  Specialization for a col-major destination matrix
  *    => Blocking algorithm following Goto's paper */
-template<
-  typename Index,
-  typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-  typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-  int ResInnerStride>
-struct general_matrix_matrix_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride>
-{
+template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar,
+          int RhsStorageOrder, bool ConjugateRhs, int ResInnerStride>
+struct general_matrix_matrix_product<Index, LhsScalar, LhsStorageOrder, ConjugateLhs, RhsScalar, RhsStorageOrder,
+                                     ConjugateRhs, ColMajor, ResInnerStride> {
+  typedef gebp_traits<LhsScalar, RhsScalar> Traits;
 
-typedef gebp_traits<LhsScalar,RhsScalar> Traits;
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
+  static void run(Index rows, Index cols, Index depth, const LhsScalar* lhs_, Index lhsStride, const RhsScalar* rhs_,
+                  Index rhsStride, ResScalar* res_, Index resIncr, Index resStride, ResScalar alpha,
+                  level3_blocking<LhsScalar, RhsScalar>& blocking, GemmParallelInfo<Index>* info = 0) {
+    typedef const_blas_data_mapper<LhsScalar, Index, LhsStorageOrder> LhsMapper;
+    typedef const_blas_data_mapper<RhsScalar, Index, RhsStorageOrder> RhsMapper;
+    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
+    LhsMapper lhs(lhs_, lhsStride);
+    RhsMapper rhs(rhs_, rhsStride);
+    ResMapper res(res_, resStride, resIncr);
 
-typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
-static void run(Index rows, Index cols, Index depth,
-  const LhsScalar* _lhs, Index lhsStride,
-  const RhsScalar* _rhs, Index rhsStride,
-  ResScalar* _res, Index resIncr, Index resStride,
-  ResScalar alpha,
-  level3_blocking<LhsScalar,RhsScalar>& blocking,
-  GemmParallelInfo<Index>* info = 0)
-{
-  typedef const_blas_data_mapper<LhsScalar, Index, LhsStorageOrder> LhsMapper;
-  typedef const_blas_data_mapper<RhsScalar, Index, RhsStorageOrder> RhsMapper;
-  typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor,Unaligned,ResInnerStride> ResMapper;
-  LhsMapper lhs(_lhs, lhsStride);
-  RhsMapper rhs(_rhs, rhsStride);
-  ResMapper res(_res, resStride, resIncr);
+    Index kc = blocking.kc();                    // cache block size along the K direction
+    Index mc = (std::min)(rows, blocking.mc());  // cache block size along the M direction
+    Index nc = (std::min)(cols, blocking.nc());  // cache block size along the N direction
 
-  Index kc = blocking.kc();                   // cache block size along the K direction
-  Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
-  Index nc = (std::min)(cols,blocking.nc());  // cache block size along the N direction
+    gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                  LhsStorageOrder>
+        pack_lhs;
+    gemm_pack_rhs<RhsScalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
+    gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp;
 
-  gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
-  gemm_pack_rhs<RhsScalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
-  gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp;
+#if defined(EIGEN_HAS_OPENMP) || defined(EIGEN_GEMM_THREADPOOL)
+    if (info) {
+      // this is the parallel version!
+      int tid = info->logical_thread_id;
+      int threads = info->num_threads;
 
-#ifdef EIGEN_HAS_OPENMP
-  if(info)
-  {
-    // this is the parallel version!
-    int tid = omp_get_thread_num();
-    int threads = omp_get_num_threads();
+      LhsScalar* blockA = blocking.blockA();
+      eigen_internal_assert(blockA != 0);
 
-    LhsScalar* blockA = blocking.blockA();
-    eigen_internal_assert(blockA!=0);
+      std::size_t sizeB = kc * nc;
+      ei_declare_aligned_stack_constructed_variable(RhsScalar, blockB, sizeB, 0);
 
-    std::size_t sizeB = kc*nc;
-    ei_declare_aligned_stack_constructed_variable(RhsScalar, blockB, sizeB, 0);
+      // For each horizontal panel of the rhs, and corresponding vertical panel of the lhs...
+      for (Index k = 0; k < depth; k += kc) {
+        const Index actual_kc = (std::min)(k + kc, depth) - k;  // => rows of B', and cols of the A'
 
-    // For each horizontal panel of the rhs, and corresponding vertical panel of the lhs...
-    for(Index k=0; k<depth; k+=kc)
+        // In order to reduce the chance that a thread has to wait for the other,
+        // let's start by packing B'.
+        pack_rhs(blockB, rhs.getSubMapper(k, 0), actual_kc, nc);
+
+        // Pack A_k to A' in a parallel fashion:
+        // each thread packs the sub block A_k,i to A'_i where i is the thread id.
+
+        // However, before copying to A'_i, we have to make sure that no other thread is still using it,
+        // i.e., we test that info->task_info[tid].users equals 0.
+        // Then, we set info->task_info[tid].users to the number of threads to mark that all other threads are going to
+        // use it.
+        while (info->task_info[tid].users != 0) {
+        }
+        info->task_info[tid].users = threads;
+
+        pack_lhs(blockA + info->task_info[tid].lhs_start * actual_kc,
+                 lhs.getSubMapper(info->task_info[tid].lhs_start, k), actual_kc, info->task_info[tid].lhs_length);
+
+        // Notify the other threads that the part A'_i is ready to go.
+        info->task_info[tid].sync = k;
+
+        // Computes C_i += A' * B' per A'_i
+        for (int shift = 0; shift < threads; ++shift) {
+          int i = (tid + shift) % threads;
+
+          // At this point we have to make sure that A'_i has been updated by the thread i,
+          // we use testAndSetOrdered to mimic a volatile access.
+          // However, no need to wait for the B' part which has been updated by the current thread!
+          if (shift > 0) {
+            while (info->task_info[i].sync != k) {
+            }
+          }
+
+          gebp(res.getSubMapper(info->task_info[i].lhs_start, 0), blockA + info->task_info[i].lhs_start * actual_kc,
+               blockB, info->task_info[i].lhs_length, actual_kc, nc, alpha);
+        }
+
+        // Then keep going as usual with the remaining B'
+        for (Index j = nc; j < cols; j += nc) {
+          const Index actual_nc = (std::min)(j + nc, cols) - j;
+
+          // pack B_k,j to B'
+          pack_rhs(blockB, rhs.getSubMapper(k, j), actual_kc, actual_nc);
+
+          // C_j += A' * B'
+          gebp(res.getSubMapper(0, j), blockA, blockB, rows, actual_kc, actual_nc, alpha);
+        }
+
+        // Release all the sub blocks A'_i of A' for the current thread,
+        // i.e., we simply decrement the number of users by 1
+        for (Index i = 0; i < threads; ++i) info->task_info[i].users -= 1;
+      }
+    } else
+#endif  // defined(EIGEN_HAS_OPENMP) || defined(EIGEN_GEMM_THREADPOOL)
     {
-      const Index actual_kc = (std::min)(k+kc,depth)-k; // => rows of B', and cols of the A'
+      EIGEN_UNUSED_VARIABLE(info);
 
-      // In order to reduce the chance that a thread has to wait for the other,
-      // let's start by packing B'.
-      pack_rhs(blockB, rhs.getSubMapper(k,0), actual_kc, nc);
+      // this is the sequential version!
+      std::size_t sizeA = kc * mc;
+      std::size_t sizeB = kc * nc;
 
-      // Pack A_k to A' in a parallel fashion:
-      // each thread packs the sub block A_k,i to A'_i where i is the thread id.
+      ei_declare_aligned_stack_constructed_variable(LhsScalar, blockA, sizeA, blocking.blockA());
+      ei_declare_aligned_stack_constructed_variable(RhsScalar, blockB, sizeB, blocking.blockB());
 
-      // However, before copying to A'_i, we have to make sure that no other thread is still using it,
-      // i.e., we test that info[tid].users equals 0.
-      // Then, we set info[tid].users to the number of threads to mark that all other threads are going to use it.
-      while(info[tid].users!=0) {}
-      info[tid].users = threads;
+      const bool pack_rhs_once = mc != rows && kc == depth && nc == cols;
 
-      pack_lhs(blockA+info[tid].lhs_start*actual_kc, lhs.getSubMapper(info[tid].lhs_start,k), actual_kc, info[tid].lhs_length);
+      // For each horizontal panel of the rhs, and corresponding panel of the lhs...
+      for (Index i2 = 0; i2 < rows; i2 += mc) {
+        const Index actual_mc = (std::min)(i2 + mc, rows) - i2;
 
-      // Notify the other threads that the part A'_i is ready to go.
-      info[tid].sync = k;
+        for (Index k2 = 0; k2 < depth; k2 += kc) {
+          const Index actual_kc = (std::min)(k2 + kc, depth) - k2;
 
-      // Computes C_i += A' * B' per A'_i
-      for(int shift=0; shift<threads; ++shift)
-      {
-        int i = (tid+shift)%threads;
+          // OK, here we have selected one horizontal panel of rhs and one vertical panel of lhs.
+          // => Pack lhs's panel into a sequential chunk of memory (L2/L3 caching)
+          // Note that this panel will be read as many times as the number of blocks in the rhs's
+          // horizontal panel which is, in practice, a very low number.
+          pack_lhs(blockA, lhs.getSubMapper(i2, k2), actual_kc, actual_mc);
 
-        // At this point we have to make sure that A'_i has been updated by the thread i,
-        // we use testAndSetOrdered to mimic a volatile access.
-        // However, no need to wait for the B' part which has been updated by the current thread!
-        if (shift>0) {
-          while(info[i].sync!=k) {
+          // For each kc x nc block of the rhs's horizontal panel...
+          for (Index j2 = 0; j2 < cols; j2 += nc) {
+            const Index actual_nc = (std::min)(j2 + nc, cols) - j2;
+
+            // We pack the rhs's block into a sequential chunk of memory (L2 caching)
+            // Note that this block will be read a very high number of times, which is equal to the number of
+            // micro horizontal panel of the large rhs's panel (e.g., rows/12 times).
+            if ((!pack_rhs_once) || i2 == 0) pack_rhs(blockB, rhs.getSubMapper(k2, j2), actual_kc, actual_nc);
+
+            // Everything is packed, we can now call the panel * block kernel:
+            gebp(res.getSubMapper(i2, j2), blockA, blockB, actual_mc, actual_kc, actual_nc, alpha);
           }
         }
-
-        gebp(res.getSubMapper(info[i].lhs_start, 0), blockA+info[i].lhs_start*actual_kc, blockB, info[i].lhs_length, actual_kc, nc, alpha);
-      }
-
-      // Then keep going as usual with the remaining B'
-      for(Index j=nc; j<cols; j+=nc)
-      {
-        const Index actual_nc = (std::min)(j+nc,cols)-j;
-
-        // pack B_k,j to B'
-        pack_rhs(blockB, rhs.getSubMapper(k,j), actual_kc, actual_nc);
-
-        // C_j += A' * B'
-        gebp(res.getSubMapper(0, j), blockA, blockB, rows, actual_kc, actual_nc, alpha);
-      }
-
-      // Release all the sub blocks A'_i of A' for the current thread,
-      // i.e., we simply decrement the number of users by 1
-      for(Index i=0; i<threads; ++i)
-#if !EIGEN_HAS_CXX11_ATOMIC
-        #pragma omp atomic
-#endif
-        info[i].users -= 1;
-    }
-  }
-  else
-#endif // EIGEN_HAS_OPENMP
-  {
-    EIGEN_UNUSED_VARIABLE(info);
-
-    // this is the sequential version!
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*nc;
-
-    ei_declare_aligned_stack_constructed_variable(LhsScalar, blockA, sizeA, blocking.blockA());
-    ei_declare_aligned_stack_constructed_variable(RhsScalar, blockB, sizeB, blocking.blockB());
-
-    const bool pack_rhs_once = mc!=rows && kc==depth && nc==cols;
-
-    // For each horizontal panel of the rhs, and corresponding panel of the lhs...
-    for(Index i2=0; i2<rows; i2+=mc)
-    {
-      const Index actual_mc = (std::min)(i2+mc,rows)-i2;
-
-      for(Index k2=0; k2<depth; k2+=kc)
-      {
-        const Index actual_kc = (std::min)(k2+kc,depth)-k2;
-
-        // OK, here we have selected one horizontal panel of rhs and one vertical panel of lhs.
-        // => Pack lhs's panel into a sequential chunk of memory (L2/L3 caching)
-        // Note that this panel will be read as many times as the number of blocks in the rhs's
-        // horizontal panel which is, in practice, a very low number.
-        pack_lhs(blockA, lhs.getSubMapper(i2,k2), actual_kc, actual_mc);
-
-        // For each kc x nc block of the rhs's horizontal panel...
-        for(Index j2=0; j2<cols; j2+=nc)
-        {
-          const Index actual_nc = (std::min)(j2+nc,cols)-j2;
-
-          // We pack the rhs's block into a sequential chunk of memory (L2 caching)
-          // Note that this block will be read a very high number of times, which is equal to the number of
-          // micro horizontal panel of the large rhs's panel (e.g., rows/12 times).
-          if((!pack_rhs_once) || i2==0)
-            pack_rhs(blockB, rhs.getSubMapper(k2,j2), actual_kc, actual_nc);
-
-          // Everything is packed, we can now call the panel * block kernel:
-          gebp(res.getSubMapper(i2, j2), blockA, blockB, actual_mc, actual_kc, actual_nc, alpha);
-        }
       }
     }
   }
-}
-
 };
 
 /*********************************************************************************
-*  Specialization of generic_product_impl for "large" GEMM, i.e.,
-*  implementation of the high level wrapper to general_matrix_matrix_product
-**********************************************************************************/
+ *  Specialization of generic_product_impl for "large" GEMM, i.e.,
+ *  implementation of the high level wrapper to general_matrix_matrix_product
+ **********************************************************************************/
 
-template<typename Scalar, typename Index, typename Gemm, typename Lhs, typename Rhs, typename Dest, typename BlockingType>
-struct gemm_functor
-{
+template <typename Scalar, typename Index, typename Gemm, typename Lhs, typename Rhs, typename Dest,
+          typename BlockingType>
+struct gemm_functor {
   gemm_functor(const Lhs& lhs, const Rhs& rhs, Dest& dest, const Scalar& actualAlpha, BlockingType& blocking)
-    : m_lhs(lhs), m_rhs(rhs), m_dest(dest), m_actualAlpha(actualAlpha), m_blocking(blocking)
-  {}
+      : m_lhs(lhs), m_rhs(rhs), m_dest(dest), m_actualAlpha(actualAlpha), m_blocking(blocking) {}
 
-  void initParallelSession(Index num_threads) const
-  {
+  void initParallelSession(Index num_threads) const {
     m_blocking.initParallel(m_lhs.rows(), m_rhs.cols(), m_lhs.cols(), num_threads);
     m_blocking.allocateA();
   }
 
-  void operator() (Index row, Index rows, Index col=0, Index cols=-1, GemmParallelInfo<Index>* info=0) const
-  {
-    if(cols==-1)
-      cols = m_rhs.cols();
+  void operator()(Index row, Index rows, Index col = 0, Index cols = -1, GemmParallelInfo<Index>* info = 0) const {
+    if (cols == -1) cols = m_rhs.cols();
 
-    Gemm::run(rows, cols, m_lhs.cols(),
-              &m_lhs.coeffRef(row,0), m_lhs.outerStride(),
-              &m_rhs.coeffRef(0,col), m_rhs.outerStride(),
-              (Scalar*)&(m_dest.coeffRef(row,col)), m_dest.innerStride(), m_dest.outerStride(),
+    Gemm::run(rows, cols, m_lhs.cols(), &m_lhs.coeffRef(row, 0), m_lhs.outerStride(), &m_rhs.coeffRef(0, col),
+              m_rhs.outerStride(), (Scalar*)&(m_dest.coeffRef(row, col)), m_dest.innerStride(), m_dest.outerStride(),
               m_actualAlpha, m_blocking, info);
   }
 
   typedef typename Gemm::Traits Traits;
 
-  protected:
-    const Lhs& m_lhs;
-    const Rhs& m_rhs;
-    Dest& m_dest;
-    Scalar m_actualAlpha;
-    BlockingType& m_blocking;
+ protected:
+  const Lhs& m_lhs;
+  const Rhs& m_rhs;
+  Dest& m_dest;
+  Scalar m_actualAlpha;
+  BlockingType& m_blocking;
 };
 
-template<int StorageOrder, typename LhsScalar, typename RhsScalar, int MaxRows, int MaxCols, int MaxDepth, int KcFactor=1,
-bool FiniteAtCompileTime = MaxRows!=Dynamic && MaxCols!=Dynamic && MaxDepth != Dynamic> class gemm_blocking_space;
+template <int StorageOrder, typename LhsScalar, typename RhsScalar, int MaxRows, int MaxCols, int MaxDepth,
+          int KcFactor = 1, bool FiniteAtCompileTime = MaxRows != Dynamic && MaxCols != Dynamic && MaxDepth != Dynamic>
+class gemm_blocking_space;
 
-template<typename _LhsScalar, typename _RhsScalar>
-class level3_blocking
-{
-    typedef _LhsScalar LhsScalar;
-    typedef _RhsScalar RhsScalar;
+template <typename LhsScalar_, typename RhsScalar_>
+class level3_blocking {
+  typedef LhsScalar_ LhsScalar;
+  typedef RhsScalar_ RhsScalar;
 
-  protected:
-    LhsScalar* m_blockA;
-    RhsScalar* m_blockB;
+ protected:
+  LhsScalar* m_blockA;
+  RhsScalar* m_blockB;
 
-    Index m_mc;
-    Index m_nc;
-    Index m_kc;
+  Index m_mc;
+  Index m_nc;
+  Index m_kc;
 
-  public:
+ public:
+  level3_blocking() : m_blockA(0), m_blockB(0), m_mc(0), m_nc(0), m_kc(0) {}
 
-    level3_blocking()
-      : m_blockA(0), m_blockB(0), m_mc(0), m_nc(0), m_kc(0)
-    {}
+  inline Index mc() const { return m_mc; }
+  inline Index nc() const { return m_nc; }
+  inline Index kc() const { return m_kc; }
 
-    inline Index mc() const { return m_mc; }
-    inline Index nc() const { return m_nc; }
-    inline Index kc() const { return m_kc; }
-
-    inline LhsScalar* blockA() { return m_blockA; }
-    inline RhsScalar* blockB() { return m_blockB; }
+  inline LhsScalar* blockA() { return m_blockA; }
+  inline RhsScalar* blockB() { return m_blockB; }
 };
 
-template<int StorageOrder, typename _LhsScalar, typename _RhsScalar, int MaxRows, int MaxCols, int MaxDepth, int KcFactor>
-class gemm_blocking_space<StorageOrder,_LhsScalar,_RhsScalar,MaxRows, MaxCols, MaxDepth, KcFactor, true /* == FiniteAtCompileTime */>
-  : public level3_blocking<
-      typename conditional<StorageOrder==RowMajor,_RhsScalar,_LhsScalar>::type,
-      typename conditional<StorageOrder==RowMajor,_LhsScalar,_RhsScalar>::type>
-{
-    enum {
-      Transpose = StorageOrder==RowMajor,
-      ActualRows = Transpose ? MaxCols : MaxRows,
-      ActualCols = Transpose ? MaxRows : MaxCols
-    };
-    typedef typename conditional<Transpose,_RhsScalar,_LhsScalar>::type LhsScalar;
-    typedef typename conditional<Transpose,_LhsScalar,_RhsScalar>::type RhsScalar;
-    typedef gebp_traits<LhsScalar,RhsScalar> Traits;
-    enum {
-      SizeA = ActualRows * MaxDepth,
-      SizeB = ActualCols * MaxDepth
-    };
+template <int StorageOrder, typename LhsScalar_, typename RhsScalar_, int MaxRows, int MaxCols, int MaxDepth,
+          int KcFactor>
+class gemm_blocking_space<StorageOrder, LhsScalar_, RhsScalar_, MaxRows, MaxCols, MaxDepth, KcFactor,
+                          true /* == FiniteAtCompileTime */>
+    : public level3_blocking<std::conditional_t<StorageOrder == RowMajor, RhsScalar_, LhsScalar_>,
+                             std::conditional_t<StorageOrder == RowMajor, LhsScalar_, RhsScalar_>> {
+  enum {
+    Transpose = StorageOrder == RowMajor,
+    ActualRows = Transpose ? MaxCols : MaxRows,
+    ActualCols = Transpose ? MaxRows : MaxCols
+  };
+  typedef std::conditional_t<Transpose, RhsScalar_, LhsScalar_> LhsScalar;
+  typedef std::conditional_t<Transpose, LhsScalar_, RhsScalar_> RhsScalar;
+  enum { SizeA = ActualRows * MaxDepth, SizeB = ActualCols * MaxDepth };
 
 #if EIGEN_MAX_STATIC_ALIGN_BYTES >= EIGEN_DEFAULT_ALIGN_BYTES
-    EIGEN_ALIGN_MAX LhsScalar m_staticA[SizeA];
-    EIGEN_ALIGN_MAX RhsScalar m_staticB[SizeB];
+  EIGEN_ALIGN_MAX LhsScalar m_staticA[SizeA];
+  EIGEN_ALIGN_MAX RhsScalar m_staticB[SizeB];
 #else
-    EIGEN_ALIGN_MAX char m_staticA[SizeA * sizeof(LhsScalar) + EIGEN_DEFAULT_ALIGN_BYTES-1];
-    EIGEN_ALIGN_MAX char m_staticB[SizeB * sizeof(RhsScalar) + EIGEN_DEFAULT_ALIGN_BYTES-1];
+  EIGEN_ALIGN_MAX char m_staticA[SizeA * sizeof(LhsScalar) + EIGEN_DEFAULT_ALIGN_BYTES - 1];
+  EIGEN_ALIGN_MAX char m_staticB[SizeB * sizeof(RhsScalar) + EIGEN_DEFAULT_ALIGN_BYTES - 1];
 #endif
 
-  public:
-
-    gemm_blocking_space(Index /*rows*/, Index /*cols*/, Index /*depth*/, Index /*num_threads*/, bool /*full_rows = false*/)
-    {
-      this->m_mc = ActualRows;
-      this->m_nc = ActualCols;
-      this->m_kc = MaxDepth;
+ public:
+  gemm_blocking_space(Index /*rows*/, Index /*cols*/, Index /*depth*/, Index /*num_threads*/,
+                      bool /*full_rows = false*/) {
+    this->m_mc = ActualRows;
+    this->m_nc = ActualCols;
+    this->m_kc = MaxDepth;
 #if EIGEN_MAX_STATIC_ALIGN_BYTES >= EIGEN_DEFAULT_ALIGN_BYTES
-      this->m_blockA = m_staticA;
-      this->m_blockB = m_staticB;
+    this->m_blockA = m_staticA;
+    this->m_blockB = m_staticB;
 #else
-      this->m_blockA = reinterpret_cast<LhsScalar*>((internal::UIntPtr(m_staticA) + (EIGEN_DEFAULT_ALIGN_BYTES-1)) & ~std::size_t(EIGEN_DEFAULT_ALIGN_BYTES-1));
-      this->m_blockB = reinterpret_cast<RhsScalar*>((internal::UIntPtr(m_staticB) + (EIGEN_DEFAULT_ALIGN_BYTES-1)) & ~std::size_t(EIGEN_DEFAULT_ALIGN_BYTES-1));
+    this->m_blockA = reinterpret_cast<LhsScalar*>((std::uintptr_t(m_staticA) + (EIGEN_DEFAULT_ALIGN_BYTES - 1)) &
+                                                  ~std::size_t(EIGEN_DEFAULT_ALIGN_BYTES - 1));
+    this->m_blockB = reinterpret_cast<RhsScalar*>((std::uintptr_t(m_staticB) + (EIGEN_DEFAULT_ALIGN_BYTES - 1)) &
+                                                  ~std::size_t(EIGEN_DEFAULT_ALIGN_BYTES - 1));
 #endif
-    }
+  }
 
-    void initParallel(Index, Index, Index, Index)
-    {}
+  void initParallel(Index, Index, Index, Index) {}
 
-    inline void allocateA() {}
-    inline void allocateB() {}
-    inline void allocateAll() {}
+  inline void allocateA() {}
+  inline void allocateB() {}
+  inline void allocateAll() {}
 };
 
-template<int StorageOrder, typename _LhsScalar, typename _RhsScalar, int MaxRows, int MaxCols, int MaxDepth, int KcFactor>
-class gemm_blocking_space<StorageOrder,_LhsScalar,_RhsScalar,MaxRows, MaxCols, MaxDepth, KcFactor, false>
-  : public level3_blocking<
-      typename conditional<StorageOrder==RowMajor,_RhsScalar,_LhsScalar>::type,
-      typename conditional<StorageOrder==RowMajor,_LhsScalar,_RhsScalar>::type>
-{
-    enum {
-      Transpose = StorageOrder==RowMajor
-    };
-    typedef typename conditional<Transpose,_RhsScalar,_LhsScalar>::type LhsScalar;
-    typedef typename conditional<Transpose,_LhsScalar,_RhsScalar>::type RhsScalar;
-    typedef gebp_traits<LhsScalar,RhsScalar> Traits;
+template <int StorageOrder, typename LhsScalar_, typename RhsScalar_, int MaxRows, int MaxCols, int MaxDepth,
+          int KcFactor>
+class gemm_blocking_space<StorageOrder, LhsScalar_, RhsScalar_, MaxRows, MaxCols, MaxDepth, KcFactor, false>
+    : public level3_blocking<std::conditional_t<StorageOrder == RowMajor, RhsScalar_, LhsScalar_>,
+                             std::conditional_t<StorageOrder == RowMajor, LhsScalar_, RhsScalar_>> {
+  enum { Transpose = StorageOrder == RowMajor };
+  typedef std::conditional_t<Transpose, RhsScalar_, LhsScalar_> LhsScalar;
+  typedef std::conditional_t<Transpose, LhsScalar_, RhsScalar_> RhsScalar;
 
-    Index m_sizeA;
-    Index m_sizeB;
+  Index m_sizeA;
+  Index m_sizeB;
 
-  public:
+ public:
+  gemm_blocking_space(Index rows, Index cols, Index depth, Index num_threads, bool l3_blocking) {
+    this->m_mc = Transpose ? cols : rows;
+    this->m_nc = Transpose ? rows : cols;
+    this->m_kc = depth;
 
-    gemm_blocking_space(Index rows, Index cols, Index depth, Index num_threads, bool l3_blocking)
+    if (l3_blocking) {
+      computeProductBlockingSizes<LhsScalar, RhsScalar, KcFactor>(this->m_kc, this->m_mc, this->m_nc, num_threads);
+    } else  // no l3 blocking
     {
-      this->m_mc = Transpose ? cols : rows;
-      this->m_nc = Transpose ? rows : cols;
-      this->m_kc = depth;
-
-      if(l3_blocking)
-      {
-        computeProductBlockingSizes<LhsScalar,RhsScalar,KcFactor>(this->m_kc, this->m_mc, this->m_nc, num_threads);
-      }
-      else  // no l3 blocking
-      {
-        Index n = this->m_nc;
-        computeProductBlockingSizes<LhsScalar,RhsScalar,KcFactor>(this->m_kc, this->m_mc, n, num_threads);
-      }
-
-      m_sizeA = this->m_mc * this->m_kc;
-      m_sizeB = this->m_kc * this->m_nc;
+      Index n = this->m_nc;
+      computeProductBlockingSizes<LhsScalar, RhsScalar, KcFactor>(this->m_kc, this->m_mc, n, num_threads);
     }
 
-    void initParallel(Index rows, Index cols, Index depth, Index num_threads)
-    {
-      this->m_mc = Transpose ? cols : rows;
-      this->m_nc = Transpose ? rows : cols;
-      this->m_kc = depth;
+    m_sizeA = this->m_mc * this->m_kc;
+    m_sizeB = this->m_kc * this->m_nc;
+  }
 
-      eigen_internal_assert(this->m_blockA==0 && this->m_blockB==0);
-      Index m = this->m_mc;
-      computeProductBlockingSizes<LhsScalar,RhsScalar,KcFactor>(this->m_kc, m, this->m_nc, num_threads);
-      m_sizeA = this->m_mc * this->m_kc;
-      m_sizeB = this->m_kc * this->m_nc;
-    }
+  void initParallel(Index rows, Index cols, Index depth, Index num_threads) {
+    this->m_mc = Transpose ? cols : rows;
+    this->m_nc = Transpose ? rows : cols;
+    this->m_kc = depth;
 
-    void allocateA()
-    {
-      if(this->m_blockA==0)
-        this->m_blockA = aligned_new<LhsScalar>(m_sizeA);
-    }
+    eigen_internal_assert(this->m_blockA == 0 && this->m_blockB == 0);
+    Index m = this->m_mc;
+    computeProductBlockingSizes<LhsScalar, RhsScalar, KcFactor>(this->m_kc, m, this->m_nc, num_threads);
+    m_sizeA = this->m_mc * this->m_kc;
+    m_sizeB = this->m_kc * this->m_nc;
+  }
 
-    void allocateB()
-    {
-      if(this->m_blockB==0)
-        this->m_blockB = aligned_new<RhsScalar>(m_sizeB);
-    }
+  void allocateA() {
+    if (this->m_blockA == 0) this->m_blockA = aligned_new<LhsScalar>(m_sizeA);
+  }
 
-    void allocateAll()
-    {
-      allocateA();
-      allocateB();
-    }
+  void allocateB() {
+    if (this->m_blockB == 0) this->m_blockB = aligned_new<RhsScalar>(m_sizeB);
+  }
 
-    ~gemm_blocking_space()
-    {
-      aligned_delete(this->m_blockA, m_sizeA);
-      aligned_delete(this->m_blockB, m_sizeB);
-    }
+  void allocateAll() {
+    allocateA();
+    allocateB();
+  }
+
+  ~gemm_blocking_space() {
+    aligned_delete(this->m_blockA, m_sizeA);
+    aligned_delete(this->m_blockB, m_sizeB);
+  }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
-template<typename Lhs, typename Rhs>
-struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,GemmProduct>
-  : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,GemmProduct> >
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+template <typename Lhs, typename Rhs>
+struct generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, GemmProduct>
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, GemmProduct>> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
   typedef typename Lhs::Scalar LhsScalar;
   typedef typename Rhs::Scalar RhsScalar;
 
   typedef internal::blas_traits<Lhs> LhsBlasTraits;
   typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
-  typedef typename internal::remove_all<ActualLhsType>::type ActualLhsTypeCleaned;
+  typedef internal::remove_all_t<ActualLhsType> ActualLhsTypeCleaned;
 
   typedef internal::blas_traits<Rhs> RhsBlasTraits;
   typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-  typedef typename internal::remove_all<ActualRhsType>::type ActualRhsTypeCleaned;
+  typedef internal::remove_all_t<ActualRhsType> ActualRhsTypeCleaned;
 
-  enum {
-    MaxDepthAtCompileTime = EIGEN_SIZE_MIN_PREFER_FIXED(Lhs::MaxColsAtCompileTime,Rhs::MaxRowsAtCompileTime)
-  };
+  enum { MaxDepthAtCompileTime = min_size_prefer_fixed(Lhs::MaxColsAtCompileTime, Rhs::MaxRowsAtCompileTime) };
 
-  typedef generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,CoeffBasedProductMode> lazyproduct;
+  typedef generic_product_impl<Lhs, Rhs, DenseShape, DenseShape, CoeffBasedProductMode> lazyproduct;
 
-  template<typename Dst>
-  static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+  template <typename Dst>
+  static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
     // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=404 for a discussion and helper program
     // to determine the following heuristic.
     // EIGEN_GEMM_TO_COEFFBASED_THRESHOLD is typically defined to 20 in GeneralProduct.h,
     // unless it has been specialized by the user or for a given architecture.
     // Note that the condition rhs.rows()>0 was required because lazy product is (was?) not happy with empty inputs.
     // I'm not sure it is still required.
-    if((rhs.rows()+dst.rows()+dst.cols())<EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows()>0)
-      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::assign_op<typename Dst::Scalar,Scalar>());
-    else
-    {
+    if ((rhs.rows() + dst.rows() + dst.cols()) < EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows() > 0)
+      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::assign_op<typename Dst::Scalar, Scalar>());
+    else {
       dst.setZero();
       scaleAndAddTo(dst, lhs, rhs, Scalar(1));
     }
   }
 
-  template<typename Dst>
-  static void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
-    if((rhs.rows()+dst.rows()+dst.cols())<EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows()>0)
-      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::add_assign_op<typename Dst::Scalar,Scalar>());
+  template <typename Dst>
+  static void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    if ((rhs.rows() + dst.rows() + dst.cols()) < EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows() > 0)
+      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::add_assign_op<typename Dst::Scalar, Scalar>());
     else
-      scaleAndAddTo(dst,lhs, rhs, Scalar(1));
+      scaleAndAddTo(dst, lhs, rhs, Scalar(1));
   }
 
-  template<typename Dst>
-  static void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
-  {
-    if((rhs.rows()+dst.rows()+dst.cols())<EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows()>0)
-      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::sub_assign_op<typename Dst::Scalar,Scalar>());
+  template <typename Dst>
+  static void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) {
+    if ((rhs.rows() + dst.rows() + dst.cols()) < EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows() > 0)
+      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::sub_assign_op<typename Dst::Scalar, Scalar>());
     else
       scaleAndAddTo(dst, lhs, rhs, Scalar(-1));
   }
 
-  template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const Lhs& a_lhs, const Rhs& a_rhs, const Scalar& alpha)
-  {
-    eigen_assert(dst.rows()==a_lhs.rows() && dst.cols()==a_rhs.cols());
-    if(a_lhs.cols()==0 || a_lhs.rows()==0 || a_rhs.cols()==0)
-      return;
+  template <typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& a_lhs, const Rhs& a_rhs, const Scalar& alpha) {
+    eigen_assert(dst.rows() == a_lhs.rows() && dst.cols() == a_rhs.cols());
+    if (a_lhs.cols() == 0 || a_lhs.rows() == 0 || a_rhs.cols() == 0) return;
 
-    if (dst.cols() == 1)
-    {
+    if (dst.cols() == 1) {
       // Fallback to GEMV if either the lhs or rhs is a runtime vector
       typename Dest::ColXpr dst_vec(dst.col(0));
-      return internal::generic_product_impl<Lhs,typename Rhs::ConstColXpr,DenseShape,DenseShape,GemvProduct>
-        ::scaleAndAddTo(dst_vec, a_lhs, a_rhs.col(0), alpha);
-    }
-    else if (dst.rows() == 1)
-    {
+      return internal::generic_product_impl<Lhs, typename Rhs::ConstColXpr, DenseShape, DenseShape,
+                                            GemvProduct>::scaleAndAddTo(dst_vec, a_lhs, a_rhs.col(0), alpha);
+    } else if (dst.rows() == 1) {
       // Fallback to GEMV if either the lhs or rhs is a runtime vector
       typename Dest::RowXpr dst_vec(dst.row(0));
-      return internal::generic_product_impl<typename Lhs::ConstRowXpr,Rhs,DenseShape,DenseShape,GemvProduct>
-        ::scaleAndAddTo(dst_vec, a_lhs.row(0), a_rhs, alpha);
+      return internal::generic_product_impl<typename Lhs::ConstRowXpr, Rhs, DenseShape, DenseShape,
+                                            GemvProduct>::scaleAndAddTo(dst_vec, a_lhs.row(0), a_rhs, alpha);
     }
 
-    typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(a_lhs);
-    typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(a_rhs);
+    add_const_on_value_type_t<ActualLhsType> lhs = LhsBlasTraits::extract(a_lhs);
+    add_const_on_value_type_t<ActualRhsType> rhs = RhsBlasTraits::extract(a_rhs);
 
     Scalar actualAlpha = combine_scalar_factors(alpha, a_lhs, a_rhs);
 
-    typedef internal::gemm_blocking_space<(Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,LhsScalar,RhsScalar,
-            Dest::MaxRowsAtCompileTime,Dest::MaxColsAtCompileTime,MaxDepthAtCompileTime> BlockingType;
+    typedef internal::gemm_blocking_space<(Dest::Flags & RowMajorBit) ? RowMajor : ColMajor, LhsScalar, RhsScalar,
+                                          Dest::MaxRowsAtCompileTime, Dest::MaxColsAtCompileTime, MaxDepthAtCompileTime>
+        BlockingType;
 
     typedef internal::gemm_functor<
-      Scalar, Index,
-      internal::general_matrix_matrix_product<
-        Index,
-        LhsScalar, (ActualLhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(LhsBlasTraits::NeedToConjugate),
-        RhsScalar, (ActualRhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(RhsBlasTraits::NeedToConjugate),
-        (Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,
-        Dest::InnerStrideAtCompileTime>,
-      ActualLhsTypeCleaned, ActualRhsTypeCleaned, Dest, BlockingType> GemmFunctor;
+        Scalar, Index,
+        internal::general_matrix_matrix_product<
+            Index, LhsScalar, (ActualLhsTypeCleaned::Flags & RowMajorBit) ? RowMajor : ColMajor,
+            bool(LhsBlasTraits::NeedToConjugate), RhsScalar,
+            (ActualRhsTypeCleaned::Flags & RowMajorBit) ? RowMajor : ColMajor, bool(RhsBlasTraits::NeedToConjugate),
+            (Dest::Flags & RowMajorBit) ? RowMajor : ColMajor, Dest::InnerStrideAtCompileTime>,
+        ActualLhsTypeCleaned, ActualRhsTypeCleaned, Dest, BlockingType>
+        GemmFunctor;
 
     BlockingType blocking(dst.rows(), dst.cols(), lhs.cols(), 1, true);
-    internal::parallelize_gemm<(Dest::MaxRowsAtCompileTime>32 || Dest::MaxRowsAtCompileTime==Dynamic)>
-        (GemmFunctor(lhs, rhs, dst, actualAlpha, blocking), a_lhs.rows(), a_rhs.cols(), a_lhs.cols(), Dest::Flags&RowMajorBit);
+    internal::parallelize_gemm<(Dest::MaxRowsAtCompileTime > 32 || Dest::MaxRowsAtCompileTime == Dynamic)>(
+        GemmFunctor(lhs, rhs, dst, actualAlpha, blocking), a_lhs.rows(), a_rhs.cols(), a_lhs.cols(),
+        Dest::Flags & RowMajorBit);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_GENERAL_MATRIX_MATRIX_H
+#endif  // EIGEN_GENERAL_MATRIX_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
index 6ba0d9b..ac94b3f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
@@ -10,98 +10,105 @@
 #ifndef EIGEN_GENERAL_MATRIX_MATRIX_TRIANGULAR_H
 #define EIGEN_GENERAL_MATRIX_MATRIX_TRIANGULAR_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
 
-template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjLhs, bool ConjRhs>
+namespace Eigen {
+
+template <typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjLhs, bool ConjRhs>
 struct selfadjoint_rank1_update;
 
 namespace internal {
 
 /**********************************************************************
-* This file implements a general A * B product while
-* evaluating only one triangular part of the product.
-* This is a more general version of self adjoint product (C += A A^T)
-* as the level 3 SYRK Blas routine.
-**********************************************************************/
+ * This file implements a general A * B product while
+ * evaluating only one triangular part of the product.
+ * This is a more general version of self adjoint product (C += A A^T)
+ * as the level 3 SYRK Blas routine.
+ **********************************************************************/
 
 // forward declarations (defined at the end of this file)
-template<typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs, int ResInnerStride, int UpLo>
+template <typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs,
+          int ResInnerStride, int UpLo>
 struct tribb_kernel;
-  
+
 /* Optimized matrix-matrix product evaluating only one triangular half */
-template <typename Index,
-          typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-          typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-                              int ResStorageOrder, int ResInnerStride, int  UpLo, int Version = Specialized>
+template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar,
+          int RhsStorageOrder, bool ConjugateRhs, int ResStorageOrder, int ResInnerStride, int UpLo,
+          int Version = Specialized>
 struct general_matrix_matrix_triangular_product;
 
 // as usual if the result is row major => we transpose the product
-template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-                          typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-                          int ResInnerStride, int  UpLo, int Version>
-struct general_matrix_matrix_triangular_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,RowMajor,ResInnerStride,UpLo,Version>
-{
+template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar,
+          int RhsStorageOrder, bool ConjugateRhs, int ResInnerStride, int UpLo, int Version>
+struct general_matrix_matrix_triangular_product<Index, LhsScalar, LhsStorageOrder, ConjugateLhs, RhsScalar,
+                                                RhsStorageOrder, ConjugateRhs, RowMajor, ResInnerStride, UpLo,
+                                                Version> {
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
-  static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* lhs, Index lhsStride,
-                                      const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resIncr, Index resStride,
-                                      const ResScalar& alpha, level3_blocking<RhsScalar,LhsScalar>& blocking)
-  {
-    general_matrix_matrix_triangular_product<Index,
-        RhsScalar, RhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateRhs,
-        LhsScalar, LhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateLhs,
-        ColMajor, ResInnerStride, UpLo==Lower?Upper:Lower>
-      ::run(size,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking);
+  static EIGEN_STRONG_INLINE void run(Index size, Index depth, const LhsScalar* lhs, Index lhsStride,
+                                      const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resIncr,
+                                      Index resStride, const ResScalar& alpha,
+                                      level3_blocking<RhsScalar, LhsScalar>& blocking) {
+    general_matrix_matrix_triangular_product<Index, RhsScalar, RhsStorageOrder == RowMajor ? ColMajor : RowMajor,
+                                             ConjugateRhs, LhsScalar, LhsStorageOrder == RowMajor ? ColMajor : RowMajor,
+                                             ConjugateLhs, ColMajor, ResInnerStride,
+                                             UpLo == Lower ? Upper : Lower>::run(size, depth, rhs, rhsStride, lhs,
+                                                                                 lhsStride, res, resIncr, resStride,
+                                                                                 alpha, blocking);
   }
 };
 
-template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-                          typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-                          int ResInnerStride, int  UpLo, int Version>
-struct general_matrix_matrix_triangular_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,UpLo,Version>
-{
+template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar,
+          int RhsStorageOrder, bool ConjugateRhs, int ResInnerStride, int UpLo, int Version>
+struct general_matrix_matrix_triangular_product<Index, LhsScalar, LhsStorageOrder, ConjugateLhs, RhsScalar,
+                                                RhsStorageOrder, ConjugateRhs, ColMajor, ResInnerStride, UpLo,
+                                                Version> {
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
-  static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* _lhs, Index lhsStride,
-                                      const RhsScalar* _rhs, Index rhsStride,
-                                      ResScalar* _res, Index resIncr, Index resStride,
-                                      const ResScalar& alpha, level3_blocking<LhsScalar,RhsScalar>& blocking)
-  {
-    typedef gebp_traits<LhsScalar,RhsScalar> Traits;
+  static EIGEN_STRONG_INLINE void run(Index size, Index depth, const LhsScalar* lhs_, Index lhsStride,
+                                      const RhsScalar* rhs_, Index rhsStride, ResScalar* res_, Index resIncr,
+                                      Index resStride, const ResScalar& alpha,
+                                      level3_blocking<LhsScalar, RhsScalar>& blocking) {
+    typedef gebp_traits<LhsScalar, RhsScalar> Traits;
 
     typedef const_blas_data_mapper<LhsScalar, Index, LhsStorageOrder> LhsMapper;
     typedef const_blas_data_mapper<RhsScalar, Index, RhsStorageOrder> RhsMapper;
     typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
-    LhsMapper lhs(_lhs,lhsStride);
-    RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride, resIncr);
+    LhsMapper lhs(lhs_, lhsStride);
+    RhsMapper rhs(rhs_, rhsStride);
+    ResMapper res(res_, resStride, resIncr);
 
     Index kc = blocking.kc();
-    Index mc = (std::min)(size,blocking.mc());
+    // Ensure that mc >= nr and <= size
+    Index mc = (std::min)(size, (std::max)(static_cast<decltype(blocking.mc())>(Traits::nr), blocking.mc()));
 
-    // !!! mc must be a multiple of nr:
-    if(mc > Traits::nr)
-      mc = (mc/Traits::nr)*Traits::nr;
+    // !!! mc must be a multiple of nr
+    if (mc > Traits::nr) {
+      using UnsignedIndex = typename make_unsigned<Index>::type;
+      mc = (UnsignedIndex(mc) / Traits::nr) * Traits::nr;
+    }
 
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*size;
+    std::size_t sizeA = kc * mc;
+    std::size_t sizeB = kc * size;
 
     ei_declare_aligned_stack_constructed_variable(LhsScalar, blockA, sizeA, blocking.blockA());
     ei_declare_aligned_stack_constructed_variable(RhsScalar, blockB, sizeB, blocking.blockB());
 
-    gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
+    gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                  LhsStorageOrder>
+        pack_lhs;
     gemm_pack_rhs<RhsScalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
     gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp;
-    tribb_kernel<LhsScalar, RhsScalar, Index, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs, ResInnerStride, UpLo> sybb;
+    tribb_kernel<LhsScalar, RhsScalar, Index, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs, ResInnerStride, UpLo>
+        sybb;
 
-    for(Index k2=0; k2<depth; k2+=kc)
-    {
-      const Index actual_kc = (std::min)(k2+kc,depth)-k2;
+    for (Index k2 = 0; k2 < depth; k2 += kc) {
+      const Index actual_kc = (std::min)(k2 + kc, depth) - k2;
 
       // note that the actual rhs is the transpose/adjoint of mat
-      pack_rhs(blockB, rhs.getSubMapper(k2,0), actual_kc, size);
+      pack_rhs(blockB, rhs.getSubMapper(k2, 0), actual_kc, size);
 
-      for(Index i2=0; i2<size; i2+=mc)
-      {
-        const Index actual_mc = (std::min)(i2+mc,size)-i2;
+      for (Index i2 = 0; i2 < size; i2 += mc) {
+        const Index actual_mc = (std::min)(i2 + mc, size) - i2;
 
         pack_lhs(blockA, lhs.getSubMapper(i2, k2), actual_kc, actual_mc);
 
@@ -109,17 +116,17 @@
         //  1 - before the diagonal => processed with gebp or skipped
         //  2 - the actual_mc x actual_mc symmetric block => processed with a special kernel
         //  3 - after the diagonal => processed with gebp or skipped
-        if (UpLo==Lower)
-          gebp(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc,
-               (std::min)(size,i2), alpha, -1, -1, 0, 0);
+        if (UpLo == Lower)
+          gebp(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, (std::min)(size, i2), alpha, -1, -1, 0,
+               0);
 
-        sybb(_res+resStride*i2 + resIncr*i2, resIncr, resStride, blockA, blockB + actual_kc*i2, actual_mc, actual_kc, alpha);
+        sybb(res_ + resStride * i2 + resIncr * i2, resIncr, resStride, blockA, blockB + actual_kc * i2, actual_mc,
+             actual_kc, alpha);
 
-        if (UpLo==Upper)
-        {
-          Index j2 = i2+actual_mc;
-          gebp(res.getSubMapper(i2, j2), blockA, blockB+actual_kc*j2, actual_mc,
-               actual_kc, (std::max)(Index(0), size-j2), alpha, -1, -1, 0, 0);
+        if (UpLo == Upper) {
+          Index j2 = i2 + actual_mc;
+          gebp(res.getSubMapper(i2, j2), blockA, blockB + actual_kc * j2, actual_mc, actual_kc,
+               (std::max)(Index(0), size - j2), alpha, -1, -1, 0, 0);
         }
       }
     }
@@ -135,183 +142,181 @@
 //   while the triangular block overlapping the diagonal is evaluated into a
 //   small temporary buffer which is then accumulated into the result using a
 //   triangular traversal.
-template<typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs, int ResInnerStride, int UpLo>
-struct tribb_kernel
-{
-  typedef gebp_traits<LhsScalar,RhsScalar,ConjLhs,ConjRhs> Traits;
+template <typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs,
+          int ResInnerStride, int UpLo>
+struct tribb_kernel {
+  typedef gebp_traits<LhsScalar, RhsScalar, ConjLhs, ConjRhs> Traits;
   typedef typename Traits::ResScalar ResScalar;
 
-  enum {
-    BlockSize  = meta_least_common_multiple<EIGEN_PLAIN_ENUM_MAX(mr,nr),EIGEN_PLAIN_ENUM_MIN(mr,nr)>::ret
-  };
-  void operator()(ResScalar* _res, Index resIncr, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB, Index size, Index depth, const ResScalar& alpha)
-  {
+  enum { BlockSize = meta_least_common_multiple<plain_enum_max(mr, nr), plain_enum_min(mr, nr)>::ret };
+  void operator()(ResScalar* res_, Index resIncr, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB,
+                  Index size, Index depth, const ResScalar& alpha) {
     typedef blas_data_mapper<ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
     typedef blas_data_mapper<ResScalar, Index, ColMajor, Unaligned> BufferMapper;
-    ResMapper res(_res, resStride, resIncr);
+    ResMapper res(res_, resStride, resIncr);
     gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, mr, nr, ConjLhs, ConjRhs> gebp_kernel1;
     gebp_kernel<LhsScalar, RhsScalar, Index, BufferMapper, mr, nr, ConjLhs, ConjRhs> gebp_kernel2;
 
-    Matrix<ResScalar,BlockSize,BlockSize,ColMajor> buffer((internal::constructor_without_unaligned_array_assert()));
+    Matrix<ResScalar, BlockSize, BlockSize, ColMajor> buffer((internal::constructor_without_unaligned_array_assert()));
 
     // let's process the block per panel of actual_mc x BlockSize,
     // again, each is split into three parts, etc.
-    for (Index j=0; j<size; j+=BlockSize)
-    {
-      Index actualBlockSize = std::min<Index>(BlockSize,size - j);
-      const RhsScalar* actual_b = blockB+j*depth;
+    for (Index j = 0; j < size; j += BlockSize) {
+      Index actualBlockSize = std::min<Index>(BlockSize, size - j);
+      const RhsScalar* actual_b = blockB + j * depth;
 
-      if(UpLo==Upper)
-        gebp_kernel1(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha,
-                     -1, -1, 0, 0);
-      
+      if (UpLo == Upper)
+        gebp_kernel1(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha, -1, -1, 0, 0);
+
       // selfadjoint micro block
       {
         Index i = j;
         buffer.setZero();
         // 1 - apply the kernel on the temporary buffer
-        gebp_kernel2(BufferMapper(buffer.data(), BlockSize), blockA+depth*i, actual_b, actualBlockSize, depth, actualBlockSize, alpha,
-                     -1, -1, 0, 0);
+        gebp_kernel2(BufferMapper(buffer.data(), BlockSize), blockA + depth * i, actual_b, actualBlockSize, depth,
+                     actualBlockSize, alpha, -1, -1, 0, 0);
 
         // 2 - triangular accumulation
-        for(Index j1=0; j1<actualBlockSize; ++j1)
-        {
-          typename ResMapper::LinearMapper r = res.getLinearMapper(i,j+j1);
-          for(Index i1=UpLo==Lower ? j1 : 0;
-              UpLo==Lower ? i1<actualBlockSize : i1<=j1; ++i1)
-            r(i1) += buffer(i1,j1);
+        for (Index j1 = 0; j1 < actualBlockSize; ++j1) {
+          typename ResMapper::LinearMapper r = res.getLinearMapper(i, j + j1);
+          for (Index i1 = UpLo == Lower ? j1 : 0; UpLo == Lower ? i1 < actualBlockSize : i1 <= j1; ++i1)
+            r(i1) += buffer(i1, j1);
         }
       }
 
-      if(UpLo==Lower)
-      {
-        Index i = j+actualBlockSize;
-        gebp_kernel1(res.getSubMapper(i, j), blockA+depth*i, actual_b, size-i, 
-                     depth, actualBlockSize, alpha, -1, -1, 0, 0);
+      if (UpLo == Lower) {
+        Index i = j + actualBlockSize;
+        gebp_kernel1(res.getSubMapper(i, j), blockA + depth * i, actual_b, size - i, depth, actualBlockSize, alpha, -1,
+                     -1, 0, 0);
       }
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 // high level API
 
-template<typename MatrixType, typename ProductType, int UpLo, bool IsOuterProduct>
+template <typename MatrixType, typename ProductType, int UpLo, bool IsOuterProduct>
 struct general_product_to_triangular_selector;
 
-
-template<typename MatrixType, typename ProductType, int UpLo>
-struct general_product_to_triangular_selector<MatrixType,ProductType,UpLo,true>
-{
-  static void run(MatrixType& mat, const ProductType& prod, const typename MatrixType::Scalar& alpha, bool beta)
-  {
+template <typename MatrixType, typename ProductType, int UpLo>
+struct general_product_to_triangular_selector<MatrixType, ProductType, UpLo, true> {
+  static void run(MatrixType& mat, const ProductType& prod, const typename MatrixType::Scalar& alpha, bool beta) {
     typedef typename MatrixType::Scalar Scalar;
-    
-    typedef typename internal::remove_all<typename ProductType::LhsNested>::type Lhs;
+
+    typedef internal::remove_all_t<typename ProductType::LhsNested> Lhs;
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhs;
-    typedef typename internal::remove_all<ActualLhs>::type _ActualLhs;
-    typename internal::add_const_on_value_type<ActualLhs>::type actualLhs = LhsBlasTraits::extract(prod.lhs());
-    
-    typedef typename internal::remove_all<typename ProductType::RhsNested>::type Rhs;
+    typedef internal::remove_all_t<ActualLhs> ActualLhs_;
+    internal::add_const_on_value_type_t<ActualLhs> actualLhs = LhsBlasTraits::extract(prod.lhs());
+
+    typedef internal::remove_all_t<typename ProductType::RhsNested> Rhs;
     typedef internal::blas_traits<Rhs> RhsBlasTraits;
     typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhs;
-    typedef typename internal::remove_all<ActualRhs>::type _ActualRhs;
-    typename internal::add_const_on_value_type<ActualRhs>::type actualRhs = RhsBlasTraits::extract(prod.rhs());
+    typedef internal::remove_all_t<ActualRhs> ActualRhs_;
+    internal::add_const_on_value_type_t<ActualRhs> actualRhs = RhsBlasTraits::extract(prod.rhs());
 
-    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs().derived()) * RhsBlasTraits::extractScalarFactor(prod.rhs().derived());
+    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs().derived()) *
+                         RhsBlasTraits::extractScalarFactor(prod.rhs().derived());
 
-    if(!beta)
-      mat.template triangularView<UpLo>().setZero();
+    if (!beta) mat.template triangularView<UpLo>().setZero();
 
     enum {
-      StorageOrder = (internal::traits<MatrixType>::Flags&RowMajorBit) ? RowMajor : ColMajor,
-      UseLhsDirectly = _ActualLhs::InnerStrideAtCompileTime==1,
-      UseRhsDirectly = _ActualRhs::InnerStrideAtCompileTime==1
+      StorageOrder = (internal::traits<MatrixType>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+      UseLhsDirectly = ActualLhs_::InnerStrideAtCompileTime == 1,
+      UseRhsDirectly = ActualRhs_::InnerStrideAtCompileTime == 1
     };
-    
-    internal::gemv_static_vector_if<Scalar,Lhs::SizeAtCompileTime,Lhs::MaxSizeAtCompileTime,!UseLhsDirectly> static_lhs;
-    ei_declare_aligned_stack_constructed_variable(Scalar, actualLhsPtr, actualLhs.size(),
-      (UseLhsDirectly ? const_cast<Scalar*>(actualLhs.data()) : static_lhs.data()));
-    if(!UseLhsDirectly) Map<typename _ActualLhs::PlainObject>(actualLhsPtr, actualLhs.size()) = actualLhs;
-    
-    internal::gemv_static_vector_if<Scalar,Rhs::SizeAtCompileTime,Rhs::MaxSizeAtCompileTime,!UseRhsDirectly> static_rhs;
-    ei_declare_aligned_stack_constructed_variable(Scalar, actualRhsPtr, actualRhs.size(),
-      (UseRhsDirectly ? const_cast<Scalar*>(actualRhs.data()) : static_rhs.data()));
-    if(!UseRhsDirectly) Map<typename _ActualRhs::PlainObject>(actualRhsPtr, actualRhs.size()) = actualRhs;
-    
-    
-    selfadjoint_rank1_update<Scalar,Index,StorageOrder,UpLo,
-                              LhsBlasTraits::NeedToConjugate && NumTraits<Scalar>::IsComplex,
-                              RhsBlasTraits::NeedToConjugate && NumTraits<Scalar>::IsComplex>
-          ::run(actualLhs.size(), mat.data(), mat.outerStride(), actualLhsPtr, actualRhsPtr, actualAlpha);
+
+    internal::gemv_static_vector_if<Scalar, Lhs::SizeAtCompileTime, Lhs::MaxSizeAtCompileTime, !UseLhsDirectly>
+        static_lhs;
+    ei_declare_aligned_stack_constructed_variable(
+        Scalar, actualLhsPtr, actualLhs.size(),
+        (UseLhsDirectly ? const_cast<Scalar*>(actualLhs.data()) : static_lhs.data()));
+    if (!UseLhsDirectly) Map<typename ActualLhs_::PlainObject>(actualLhsPtr, actualLhs.size()) = actualLhs;
+
+    internal::gemv_static_vector_if<Scalar, Rhs::SizeAtCompileTime, Rhs::MaxSizeAtCompileTime, !UseRhsDirectly>
+        static_rhs;
+    ei_declare_aligned_stack_constructed_variable(
+        Scalar, actualRhsPtr, actualRhs.size(),
+        (UseRhsDirectly ? const_cast<Scalar*>(actualRhs.data()) : static_rhs.data()));
+    if (!UseRhsDirectly) Map<typename ActualRhs_::PlainObject>(actualRhsPtr, actualRhs.size()) = actualRhs;
+
+    selfadjoint_rank1_update<
+        Scalar, Index, StorageOrder, UpLo, LhsBlasTraits::NeedToConjugate && NumTraits<Scalar>::IsComplex,
+        RhsBlasTraits::NeedToConjugate && NumTraits<Scalar>::IsComplex>::run(actualLhs.size(), mat.data(),
+                                                                             mat.outerStride(), actualLhsPtr,
+                                                                             actualRhsPtr, actualAlpha);
   }
 };
 
-template<typename MatrixType, typename ProductType, int UpLo>
-struct general_product_to_triangular_selector<MatrixType,ProductType,UpLo,false>
-{
-  static void run(MatrixType& mat, const ProductType& prod, const typename MatrixType::Scalar& alpha, bool beta)
-  {
-    typedef typename internal::remove_all<typename ProductType::LhsNested>::type Lhs;
+template <typename MatrixType, typename ProductType, int UpLo>
+struct general_product_to_triangular_selector<MatrixType, ProductType, UpLo, false> {
+  static void run(MatrixType& mat, const ProductType& prod, const typename MatrixType::Scalar& alpha, bool beta) {
+    typedef internal::remove_all_t<typename ProductType::LhsNested> Lhs;
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhs;
-    typedef typename internal::remove_all<ActualLhs>::type _ActualLhs;
-    typename internal::add_const_on_value_type<ActualLhs>::type actualLhs = LhsBlasTraits::extract(prod.lhs());
-    
-    typedef typename internal::remove_all<typename ProductType::RhsNested>::type Rhs;
+    typedef internal::remove_all_t<ActualLhs> ActualLhs_;
+    internal::add_const_on_value_type_t<ActualLhs> actualLhs = LhsBlasTraits::extract(prod.lhs());
+
+    typedef internal::remove_all_t<typename ProductType::RhsNested> Rhs;
     typedef internal::blas_traits<Rhs> RhsBlasTraits;
     typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhs;
-    typedef typename internal::remove_all<ActualRhs>::type _ActualRhs;
-    typename internal::add_const_on_value_type<ActualRhs>::type actualRhs = RhsBlasTraits::extract(prod.rhs());
+    typedef internal::remove_all_t<ActualRhs> ActualRhs_;
+    internal::add_const_on_value_type_t<ActualRhs> actualRhs = RhsBlasTraits::extract(prod.rhs());
 
-    typename ProductType::Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs().derived()) * RhsBlasTraits::extractScalarFactor(prod.rhs().derived());
+    typename ProductType::Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs().derived()) *
+                                               RhsBlasTraits::extractScalarFactor(prod.rhs().derived());
 
-    if(!beta)
-      mat.template triangularView<UpLo>().setZero();
+    if (!beta) mat.template triangularView<UpLo>().setZero();
 
     enum {
-      IsRowMajor = (internal::traits<MatrixType>::Flags&RowMajorBit) ? 1 : 0,
-      LhsIsRowMajor = _ActualLhs::Flags&RowMajorBit ? 1 : 0,
-      RhsIsRowMajor = _ActualRhs::Flags&RowMajorBit ? 1 : 0,
-      SkipDiag = (UpLo&(UnitDiag|ZeroDiag))!=0
+      IsRowMajor = (internal::traits<MatrixType>::Flags & RowMajorBit) ? 1 : 0,
+      LhsIsRowMajor = ActualLhs_::Flags & RowMajorBit ? 1 : 0,
+      RhsIsRowMajor = ActualRhs_::Flags & RowMajorBit ? 1 : 0,
+      SkipDiag = (UpLo & (UnitDiag | ZeroDiag)) != 0
     };
 
     Index size = mat.cols();
-    if(SkipDiag)
-      size--;
+    if (SkipDiag) size--;
     Index depth = actualLhs.cols();
 
-    typedef internal::gemm_blocking_space<IsRowMajor ? RowMajor : ColMajor,typename Lhs::Scalar,typename Rhs::Scalar,
-          MatrixType::MaxColsAtCompileTime, MatrixType::MaxColsAtCompileTime, _ActualRhs::MaxColsAtCompileTime> BlockingType;
+    typedef internal::gemm_blocking_space<IsRowMajor ? RowMajor : ColMajor, typename Lhs::Scalar, typename Rhs::Scalar,
+                                          MatrixType::MaxColsAtCompileTime, MatrixType::MaxColsAtCompileTime,
+                                          ActualRhs_::MaxColsAtCompileTime>
+        BlockingType;
 
     BlockingType blocking(size, size, depth, 1, false);
 
-    internal::general_matrix_matrix_triangular_product<Index,
-      typename Lhs::Scalar, LhsIsRowMajor ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate,
-      typename Rhs::Scalar, RhsIsRowMajor ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate,
-      IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime, UpLo&(Lower|Upper)>
-      ::run(size, depth,
-            &actualLhs.coeffRef(SkipDiag&&(UpLo&Lower)==Lower ? 1 : 0,0), actualLhs.outerStride(),
-            &actualRhs.coeffRef(0,SkipDiag&&(UpLo&Upper)==Upper ? 1 : 0), actualRhs.outerStride(),
-            mat.data() + (SkipDiag ? (bool(IsRowMajor) != ((UpLo&Lower)==Lower) ? mat.innerStride() : mat.outerStride() ) : 0),
-            mat.innerStride(), mat.outerStride(), actualAlpha, blocking);
+    internal::general_matrix_matrix_triangular_product<
+        Index, typename Lhs::Scalar, LhsIsRowMajor ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate,
+        typename Rhs::Scalar, RhsIsRowMajor ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate,
+        IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime,
+        UpLo&(Lower | Upper)>::run(size, depth, &actualLhs.coeffRef(SkipDiag && (UpLo & Lower) == Lower ? 1 : 0, 0),
+                                   actualLhs.outerStride(),
+                                   &actualRhs.coeffRef(0, SkipDiag && (UpLo & Upper) == Upper ? 1 : 0),
+                                   actualRhs.outerStride(),
+                                   mat.data() +
+                                       (SkipDiag ? (bool(IsRowMajor) != ((UpLo & Lower) == Lower) ? mat.innerStride()
+                                                                                                  : mat.outerStride())
+                                                 : 0),
+                                   mat.innerStride(), mat.outerStride(), actualAlpha, blocking);
   }
 };
 
-template<typename MatrixType, unsigned int UpLo>
-template<typename ProductType>
-EIGEN_DEVICE_FUNC TriangularView<MatrixType,UpLo>& TriangularViewImpl<MatrixType,UpLo,Dense>::_assignProduct(const ProductType& prod, const Scalar& alpha, bool beta)
-{
-  EIGEN_STATIC_ASSERT((UpLo&UnitDiag)==0, WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED);
+template <typename MatrixType, unsigned int UpLo>
+template <typename ProductType>
+EIGEN_DEVICE_FUNC TriangularView<MatrixType, UpLo>& TriangularViewImpl<MatrixType, UpLo, Dense>::_assignProduct(
+    const ProductType& prod, const Scalar& alpha, bool beta) {
+  EIGEN_STATIC_ASSERT((UpLo & UnitDiag) == 0, WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED);
   eigen_assert(derived().nestedExpression().rows() == prod.rows() && derived().cols() == prod.cols());
 
-  general_product_to_triangular_selector<MatrixType, ProductType, UpLo, internal::traits<ProductType>::InnerSize==1>::run(derived().nestedExpression().const_cast_derived(), prod, alpha, beta);
+  general_product_to_triangular_selector<MatrixType, ProductType, UpLo, internal::traits<ProductType>::InnerSize == 1>::
+      run(derived().nestedExpression().const_cast_derived(), prod, alpha, beta);
 
   return derived();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_GENERAL_MATRIX_MATRIX_TRIANGULAR_H
+#endif  // EIGEN_GENERAL_MATRIX_MATRIX_TRIANGULAR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h
index dfb6aeb..afd8155 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h
@@ -10,61 +10,61 @@
 #ifndef EIGEN_GENERAL_MATRIX_VECTOR_H
 #define EIGEN_GENERAL_MATRIX_VECTOR_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-enum GEMVPacketSizeType {
-  GEMVPacketFull = 0,
-  GEMVPacketHalf,
-  GEMVPacketQuarter
-};
+enum GEMVPacketSizeType { GEMVPacketFull = 0, GEMVPacketHalf, GEMVPacketQuarter };
 
 template <int N, typename T1, typename T2, typename T3>
-struct gemv_packet_cond { typedef T3 type; };
-
-template <typename T1, typename T2, typename T3>
-struct gemv_packet_cond<GEMVPacketFull, T1, T2, T3> { typedef T1 type; };
-
-template <typename T1, typename T2, typename T3>
-struct gemv_packet_cond<GEMVPacketHalf, T1, T2, T3> { typedef T2 type; };
-
-template<typename LhsScalar, typename RhsScalar, int _PacketSize=GEMVPacketFull>
-class gemv_traits
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
-
-#define PACKET_DECL_COND_PREFIX(prefix, name, packet_size)                        \
-  typedef typename gemv_packet_cond<packet_size,                                  \
-                                    typename packet_traits<name ## Scalar>::type, \
-                                    typename packet_traits<name ## Scalar>::half, \
-                                    typename unpacket_traits<typename packet_traits<name ## Scalar>::half>::half>::type \
-  prefix ## name ## Packet
-
-  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
-  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
-#undef PACKET_DECL_COND_PREFIX
-
-public:
-  enum {
-        Vectorizable = unpacket_traits<_LhsPacket>::vectorizable &&
-        unpacket_traits<_RhsPacket>::vectorizable &&
-        int(unpacket_traits<_LhsPacket>::size)==int(unpacket_traits<_RhsPacket>::size),
-        LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
-        RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
-        ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1
-  };
-
-  typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
-  typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
-  typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+struct gemv_packet_cond {
+  typedef T3 type;
 };
 
+template <typename T1, typename T2, typename T3>
+struct gemv_packet_cond<GEMVPacketFull, T1, T2, T3> {
+  typedef T1 type;
+};
+
+template <typename T1, typename T2, typename T3>
+struct gemv_packet_cond<GEMVPacketHalf, T1, T2, T3> {
+  typedef T2 type;
+};
+
+template <typename LhsScalar, typename RhsScalar, int PacketSize_ = GEMVPacketFull>
+class gemv_traits {
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
+
+#define PACKET_DECL_COND_POSTFIX(postfix, name, packet_size)                                               \
+  typedef typename gemv_packet_cond<                                                                       \
+      packet_size, typename packet_traits<name##Scalar>::type, typename packet_traits<name##Scalar>::half, \
+      typename unpacket_traits<typename packet_traits<name##Scalar>::half>::half>::type name##Packet##postfix
+
+  PACKET_DECL_COND_POSTFIX(_, Lhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Rhs, PacketSize_);
+  PACKET_DECL_COND_POSTFIX(_, Res, PacketSize_);
+#undef PACKET_DECL_COND_POSTFIX
+
+ public:
+  enum {
+    Vectorizable = unpacket_traits<LhsPacket_>::vectorizable && unpacket_traits<RhsPacket_>::vectorizable &&
+                   int(unpacket_traits<LhsPacket_>::size) == int(unpacket_traits<RhsPacket_>::size),
+    LhsPacketSize = Vectorizable ? unpacket_traits<LhsPacket_>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<RhsPacket_>::size : 1,
+    ResPacketSize = Vectorizable ? unpacket_traits<ResPacket_>::size : 1
+  };
+
+  typedef std::conditional_t<Vectorizable, LhsPacket_, LhsScalar> LhsPacket;
+  typedef std::conditional_t<Vectorizable, RhsPacket_, RhsScalar> RhsPacket;
+  typedef std::conditional_t<Vectorizable, ResPacket_, ResScalar> ResPacket;
+};
 
 /* Optimized col-major matrix * vector product:
  * This algorithm processes the matrix per vertical panels,
- * which are then processed horizontaly per chunck of 8*PacketSize x 1 vertical segments.
+ * which are then processed horizontally per chunck of 8*PacketSize x 1 vertical segments.
  *
  * Mixing type logic: C += alpha * A * B
  *  |  A  |  B  |alpha| comments
@@ -75,12 +75,13 @@
  *
  * The same reasoning apply for the transposed case.
  */
-template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
-struct general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>
-{
-  typedef gemv_traits<LhsScalar,RhsScalar> Traits;
-  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketHalf> HalfTraits;
-  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketQuarter> QuarterTraits;
+template <typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar,
+          typename RhsMapper, bool ConjugateRhs, int Version>
+struct general_matrix_vector_product<Index, LhsScalar, LhsMapper, ColMajor, ConjugateLhs, RhsScalar, RhsMapper,
+                                     ConjugateRhs, Version> {
+  typedef gemv_traits<LhsScalar, RhsScalar> Traits;
+  typedef gemv_traits<LhsScalar, RhsScalar, GEMVPacketHalf> HalfTraits;
+  typedef gemv_traits<LhsScalar, RhsScalar, GEMVPacketQuarter> QuarterTraits;
 
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
@@ -96,190 +97,163 @@
   typedef typename QuarterTraits::RhsPacket RhsPacketQuarter;
   typedef typename QuarterTraits::ResPacket ResPacketQuarter;
 
-EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static void run(
-  Index rows, Index cols,
-  const LhsMapper& lhs,
-  const RhsMapper& rhs,
-        ResScalar* res, Index resIncr,
-  RhsScalar alpha);
+  EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static void run(Index rows, Index cols, const LhsMapper& lhs,
+                                                      const RhsMapper& rhs, ResScalar* res, Index resIncr,
+                                                      RhsScalar alpha);
 };
 
-template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
-EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>::run(
-  Index rows, Index cols,
-  const LhsMapper& alhs,
-  const RhsMapper& rhs,
-        ResScalar* res, Index resIncr,
-  RhsScalar alpha)
-{
+template <typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar,
+          typename RhsMapper, bool ConjugateRhs, int Version>
+EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE void
+general_matrix_vector_product<Index, LhsScalar, LhsMapper, ColMajor, ConjugateLhs, RhsScalar, RhsMapper, ConjugateRhs,
+                              Version>::run(Index rows, Index cols, const LhsMapper& alhs, const RhsMapper& rhs,
+                                            ResScalar* res, Index resIncr, RhsScalar alpha) {
   EIGEN_UNUSED_VARIABLE(resIncr);
-  eigen_internal_assert(resIncr==1);
+  eigen_internal_assert(resIncr == 1);
 
   // The following copy tells the compiler that lhs's attributes are not modified outside this function
   // This helps GCC to generate propoer code.
   LhsMapper lhs(alhs);
 
-  conj_helper<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs> cj;
-  conj_helper<LhsPacket,RhsPacket,ConjugateLhs,ConjugateRhs> pcj;
-  conj_helper<LhsPacketHalf,RhsPacketHalf,ConjugateLhs,ConjugateRhs> pcj_half;
-  conj_helper<LhsPacketQuarter,RhsPacketQuarter,ConjugateLhs,ConjugateRhs> pcj_quarter;
+  conj_helper<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs> cj;
+  conj_helper<LhsPacket, RhsPacket, ConjugateLhs, ConjugateRhs> pcj;
+  conj_helper<LhsPacketHalf, RhsPacketHalf, ConjugateLhs, ConjugateRhs> pcj_half;
+  conj_helper<LhsPacketQuarter, RhsPacketQuarter, ConjugateLhs, ConjugateRhs> pcj_quarter;
 
   const Index lhsStride = lhs.stride();
   // TODO: for padded aligned inputs, we could enable aligned reads
-  enum { LhsAlignment = Unaligned,
-         ResPacketSize = Traits::ResPacketSize,
-         ResPacketSizeHalf = HalfTraits::ResPacketSize,
-         ResPacketSizeQuarter = QuarterTraits::ResPacketSize,
-         LhsPacketSize = Traits::LhsPacketSize,
-         HasHalf = (int)ResPacketSizeHalf < (int)ResPacketSize,
-         HasQuarter = (int)ResPacketSizeQuarter < (int)ResPacketSizeHalf
+  enum {
+    LhsAlignment = Unaligned,
+    ResPacketSize = Traits::ResPacketSize,
+    ResPacketSizeHalf = HalfTraits::ResPacketSize,
+    ResPacketSizeQuarter = QuarterTraits::ResPacketSize,
+    LhsPacketSize = Traits::LhsPacketSize,
+    HasHalf = (int)ResPacketSizeHalf < (int)ResPacketSize,
+    HasQuarter = (int)ResPacketSizeQuarter < (int)ResPacketSizeHalf
   };
 
-  const Index n8 = rows-8*ResPacketSize+1;
-  const Index n4 = rows-4*ResPacketSize+1;
-  const Index n3 = rows-3*ResPacketSize+1;
-  const Index n2 = rows-2*ResPacketSize+1;
-  const Index n1 = rows-1*ResPacketSize+1;
-  const Index n_half = rows-1*ResPacketSizeHalf+1;
-  const Index n_quarter = rows-1*ResPacketSizeQuarter+1;
+  const Index n8 = rows - 8 * ResPacketSize + 1;
+  const Index n4 = rows - 4 * ResPacketSize + 1;
+  const Index n3 = rows - 3 * ResPacketSize + 1;
+  const Index n2 = rows - 2 * ResPacketSize + 1;
+  const Index n1 = rows - 1 * ResPacketSize + 1;
+  const Index n_half = rows - 1 * ResPacketSizeHalf + 1;
+  const Index n_quarter = rows - 1 * ResPacketSizeQuarter + 1;
 
   // TODO: improve the following heuristic:
-  const Index block_cols = cols<128 ? cols : (lhsStride*sizeof(LhsScalar)<32000?16:4);
+  const Index block_cols = cols < 128 ? cols : (lhsStride * sizeof(LhsScalar) < 32000 ? 16 : 4);
   ResPacket palpha = pset1<ResPacket>(alpha);
   ResPacketHalf palpha_half = pset1<ResPacketHalf>(alpha);
   ResPacketQuarter palpha_quarter = pset1<ResPacketQuarter>(alpha);
 
-  for(Index j2=0; j2<cols; j2+=block_cols)
-  {
-    Index jend = numext::mini(j2+block_cols,cols);
-    Index i=0;
-    for(; i<n8; i+=ResPacketSize*8)
-    {
-      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
-                c1 = pset1<ResPacket>(ResScalar(0)),
-                c2 = pset1<ResPacket>(ResScalar(0)),
-                c3 = pset1<ResPacket>(ResScalar(0)),
-                c4 = pset1<ResPacket>(ResScalar(0)),
-                c5 = pset1<ResPacket>(ResScalar(0)),
-                c6 = pset1<ResPacket>(ResScalar(0)),
-                c7 = pset1<ResPacket>(ResScalar(0));
+  for (Index j2 = 0; j2 < cols; j2 += block_cols) {
+    Index jend = numext::mini(j2 + block_cols, cols);
+    Index i = 0;
+    for (; i < n8; i += ResPacketSize * 8) {
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)), c1 = pset1<ResPacket>(ResScalar(0)),
+                c2 = pset1<ResPacket>(ResScalar(0)), c3 = pset1<ResPacket>(ResScalar(0)),
+                c4 = pset1<ResPacket>(ResScalar(0)), c5 = pset1<ResPacket>(ResScalar(0)),
+                c6 = pset1<ResPacket>(ResScalar(0)), c7 = pset1<ResPacket>(ResScalar(0));
 
-      for(Index j=j2; j<jend; j+=1)
-      {
-        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
-        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
-        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
-        c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*2,j),b0,c2);
-        c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*3,j),b0,c3);
-        c4 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*4,j),b0,c4);
-        c5 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*5,j),b0,c5);
-        c6 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*6,j),b0,c6);
-        c7 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*7,j),b0,c7);
+      for (Index j = j2; j < jend; j += 1) {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j, 0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 0, j), b0, c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 1, j), b0, c1);
+        c2 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 2, j), b0, c2);
+        c3 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 3, j), b0, c3);
+        c4 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 4, j), b0, c4);
+        c5 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 5, j), b0, c5);
+        c6 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 6, j), b0, c6);
+        c7 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 7, j), b0, c7);
       }
-      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
-      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
-      pstoreu(res+i+ResPacketSize*2, pmadd(c2,palpha,ploadu<ResPacket>(res+i+ResPacketSize*2)));
-      pstoreu(res+i+ResPacketSize*3, pmadd(c3,palpha,ploadu<ResPacket>(res+i+ResPacketSize*3)));
-      pstoreu(res+i+ResPacketSize*4, pmadd(c4,palpha,ploadu<ResPacket>(res+i+ResPacketSize*4)));
-      pstoreu(res+i+ResPacketSize*5, pmadd(c5,palpha,ploadu<ResPacket>(res+i+ResPacketSize*5)));
-      pstoreu(res+i+ResPacketSize*6, pmadd(c6,palpha,ploadu<ResPacket>(res+i+ResPacketSize*6)));
-      pstoreu(res+i+ResPacketSize*7, pmadd(c7,palpha,ploadu<ResPacket>(res+i+ResPacketSize*7)));
+      pstoreu(res + i + ResPacketSize * 0, pmadd(c0, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 0)));
+      pstoreu(res + i + ResPacketSize * 1, pmadd(c1, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 1)));
+      pstoreu(res + i + ResPacketSize * 2, pmadd(c2, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 2)));
+      pstoreu(res + i + ResPacketSize * 3, pmadd(c3, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 3)));
+      pstoreu(res + i + ResPacketSize * 4, pmadd(c4, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 4)));
+      pstoreu(res + i + ResPacketSize * 5, pmadd(c5, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 5)));
+      pstoreu(res + i + ResPacketSize * 6, pmadd(c6, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 6)));
+      pstoreu(res + i + ResPacketSize * 7, pmadd(c7, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 7)));
     }
-    if(i<n4)
-    {
-      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
-                c1 = pset1<ResPacket>(ResScalar(0)),
-                c2 = pset1<ResPacket>(ResScalar(0)),
-                c3 = pset1<ResPacket>(ResScalar(0));
+    if (i < n4) {
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)), c1 = pset1<ResPacket>(ResScalar(0)),
+                c2 = pset1<ResPacket>(ResScalar(0)), c3 = pset1<ResPacket>(ResScalar(0));
 
-      for(Index j=j2; j<jend; j+=1)
-      {
-        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
-        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
-        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
-        c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*2,j),b0,c2);
-        c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*3,j),b0,c3);
+      for (Index j = j2; j < jend; j += 1) {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j, 0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 0, j), b0, c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 1, j), b0, c1);
+        c2 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 2, j), b0, c2);
+        c3 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 3, j), b0, c3);
       }
-      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
-      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
-      pstoreu(res+i+ResPacketSize*2, pmadd(c2,palpha,ploadu<ResPacket>(res+i+ResPacketSize*2)));
-      pstoreu(res+i+ResPacketSize*3, pmadd(c3,palpha,ploadu<ResPacket>(res+i+ResPacketSize*3)));
+      pstoreu(res + i + ResPacketSize * 0, pmadd(c0, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 0)));
+      pstoreu(res + i + ResPacketSize * 1, pmadd(c1, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 1)));
+      pstoreu(res + i + ResPacketSize * 2, pmadd(c2, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 2)));
+      pstoreu(res + i + ResPacketSize * 3, pmadd(c3, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 3)));
 
-      i+=ResPacketSize*4;
+      i += ResPacketSize * 4;
     }
-    if(i<n3)
-    {
-      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
-                c1 = pset1<ResPacket>(ResScalar(0)),
+    if (i < n3) {
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)), c1 = pset1<ResPacket>(ResScalar(0)),
                 c2 = pset1<ResPacket>(ResScalar(0));
 
-      for(Index j=j2; j<jend; j+=1)
-      {
-        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
-        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
-        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
-        c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*2,j),b0,c2);
+      for (Index j = j2; j < jend; j += 1) {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j, 0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 0, j), b0, c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 1, j), b0, c1);
+        c2 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 2, j), b0, c2);
       }
-      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
-      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
-      pstoreu(res+i+ResPacketSize*2, pmadd(c2,palpha,ploadu<ResPacket>(res+i+ResPacketSize*2)));
+      pstoreu(res + i + ResPacketSize * 0, pmadd(c0, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 0)));
+      pstoreu(res + i + ResPacketSize * 1, pmadd(c1, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 1)));
+      pstoreu(res + i + ResPacketSize * 2, pmadd(c2, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 2)));
 
-      i+=ResPacketSize*3;
+      i += ResPacketSize * 3;
     }
-    if(i<n2)
-    {
-      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
-                c1 = pset1<ResPacket>(ResScalar(0));
+    if (i < n2) {
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)), c1 = pset1<ResPacket>(ResScalar(0));
 
-      for(Index j=j2; j<jend; j+=1)
-      {
-        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
-        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
-        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
+      for (Index j = j2; j < jend; j += 1) {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j, 0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 0, j), b0, c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + LhsPacketSize * 1, j), b0, c1);
       }
-      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
-      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
-      i+=ResPacketSize*2;
+      pstoreu(res + i + ResPacketSize * 0, pmadd(c0, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 0)));
+      pstoreu(res + i + ResPacketSize * 1, pmadd(c1, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 1)));
+      i += ResPacketSize * 2;
     }
-    if(i<n1)
-    {
+    if (i < n1) {
       ResPacket c0 = pset1<ResPacket>(ResScalar(0));
-      for(Index j=j2; j<jend; j+=1)
-      {
-        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
-        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
+      for (Index j = j2; j < jend; j += 1) {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j, 0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 0, j), b0, c0);
       }
-      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
-      i+=ResPacketSize;
+      pstoreu(res + i + ResPacketSize * 0, pmadd(c0, palpha, ploadu<ResPacket>(res + i + ResPacketSize * 0)));
+      i += ResPacketSize;
     }
-    if(HasHalf && i<n_half)
-    {
+    if (HasHalf && i < n_half) {
       ResPacketHalf c0 = pset1<ResPacketHalf>(ResScalar(0));
-      for(Index j=j2; j<jend; j+=1)
-      {
-        RhsPacketHalf b0 = pset1<RhsPacketHalf>(rhs(j,0));
-        c0 = pcj_half.pmadd(lhs.template load<LhsPacketHalf,LhsAlignment>(i+0,j),b0,c0);
+      for (Index j = j2; j < jend; j += 1) {
+        RhsPacketHalf b0 = pset1<RhsPacketHalf>(rhs(j, 0));
+        c0 = pcj_half.pmadd(lhs.template load<LhsPacketHalf, LhsAlignment>(i + 0, j), b0, c0);
       }
-      pstoreu(res+i+ResPacketSizeHalf*0, pmadd(c0,palpha_half,ploadu<ResPacketHalf>(res+i+ResPacketSizeHalf*0)));
-      i+=ResPacketSizeHalf;
+      pstoreu(res + i + ResPacketSizeHalf * 0,
+              pmadd(c0, palpha_half, ploadu<ResPacketHalf>(res + i + ResPacketSizeHalf * 0)));
+      i += ResPacketSizeHalf;
     }
-    if(HasQuarter && i<n_quarter)
-    {
+    if (HasQuarter && i < n_quarter) {
       ResPacketQuarter c0 = pset1<ResPacketQuarter>(ResScalar(0));
-      for(Index j=j2; j<jend; j+=1)
-      {
-        RhsPacketQuarter b0 = pset1<RhsPacketQuarter>(rhs(j,0));
-        c0 = pcj_quarter.pmadd(lhs.template load<LhsPacketQuarter,LhsAlignment>(i+0,j),b0,c0);
+      for (Index j = j2; j < jend; j += 1) {
+        RhsPacketQuarter b0 = pset1<RhsPacketQuarter>(rhs(j, 0));
+        c0 = pcj_quarter.pmadd(lhs.template load<LhsPacketQuarter, LhsAlignment>(i + 0, j), b0, c0);
       }
-      pstoreu(res+i+ResPacketSizeQuarter*0, pmadd(c0,palpha_quarter,ploadu<ResPacketQuarter>(res+i+ResPacketSizeQuarter*0)));
-      i+=ResPacketSizeQuarter;
+      pstoreu(res + i + ResPacketSizeQuarter * 0,
+              pmadd(c0, palpha_quarter, ploadu<ResPacketQuarter>(res + i + ResPacketSizeQuarter * 0)));
+      i += ResPacketSizeQuarter;
     }
-    for(;i<rows;++i)
-    {
+    for (; i < rows; ++i) {
       ResScalar c0(0);
-      for(Index j=j2; j<jend; j+=1)
-        c0 += cj.pmul(lhs(i,j), rhs(j,0));
-      res[i] += alpha*c0;
+      for (Index j = j2; j < jend; j += 1) c0 += cj.pmul(lhs(i, j), rhs(j, 0));
+      res[i] += alpha * c0;
     }
   }
 }
@@ -294,12 +268,13 @@
  *  - alpha is always a complex (or converted to a complex)
  *  - no vectorization
  */
-template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
-struct general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>
-{
-  typedef gemv_traits<LhsScalar,RhsScalar> Traits;
-  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketHalf> HalfTraits;
-  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketQuarter> QuarterTraits;
+template <typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar,
+          typename RhsMapper, bool ConjugateRhs, int Version>
+struct general_matrix_vector_product<Index, LhsScalar, LhsMapper, RowMajor, ConjugateLhs, RhsScalar, RhsMapper,
+                                     ConjugateRhs, Version> {
+  typedef gemv_traits<LhsScalar, RhsScalar> Traits;
+  typedef gemv_traits<LhsScalar, RhsScalar, GEMVPacketHalf> HalfTraits;
+  typedef gemv_traits<LhsScalar, RhsScalar, GEMVPacketQuarter> QuarterTraits;
 
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
@@ -315,75 +290,69 @@
   typedef typename QuarterTraits::RhsPacket RhsPacketQuarter;
   typedef typename QuarterTraits::ResPacket ResPacketQuarter;
 
-EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static void run(
-  Index rows, Index cols,
-  const LhsMapper& lhs,
-  const RhsMapper& rhs,
-        ResScalar* res, Index resIncr,
-  ResScalar alpha);
+  EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static void run(Index rows, Index cols, const LhsMapper& lhs,
+                                                      const RhsMapper& rhs, ResScalar* res, Index resIncr,
+                                                      ResScalar alpha);
 };
 
-template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
-EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>::run(
-  Index rows, Index cols,
-  const LhsMapper& alhs,
-  const RhsMapper& rhs,
-  ResScalar* res, Index resIncr,
-  ResScalar alpha)
-{
+template <typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar,
+          typename RhsMapper, bool ConjugateRhs, int Version>
+EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE void
+general_matrix_vector_product<Index, LhsScalar, LhsMapper, RowMajor, ConjugateLhs, RhsScalar, RhsMapper, ConjugateRhs,
+                              Version>::run(Index rows, Index cols, const LhsMapper& alhs, const RhsMapper& rhs,
+                                            ResScalar* res, Index resIncr, ResScalar alpha) {
   // The following copy tells the compiler that lhs's attributes are not modified outside this function
   // This helps GCC to generate propoer code.
   LhsMapper lhs(alhs);
 
-  eigen_internal_assert(rhs.stride()==1);
-  conj_helper<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs> cj;
-  conj_helper<LhsPacket,RhsPacket,ConjugateLhs,ConjugateRhs> pcj;
-  conj_helper<LhsPacketHalf,RhsPacketHalf,ConjugateLhs,ConjugateRhs> pcj_half;
-  conj_helper<LhsPacketQuarter,RhsPacketQuarter,ConjugateLhs,ConjugateRhs> pcj_quarter;
+  eigen_internal_assert(rhs.stride() == 1);
+  conj_helper<LhsScalar, RhsScalar, ConjugateLhs, ConjugateRhs> cj;
+  conj_helper<LhsPacket, RhsPacket, ConjugateLhs, ConjugateRhs> pcj;
+  conj_helper<LhsPacketHalf, RhsPacketHalf, ConjugateLhs, ConjugateRhs> pcj_half;
+  conj_helper<LhsPacketQuarter, RhsPacketQuarter, ConjugateLhs, ConjugateRhs> pcj_quarter;
 
   // TODO: fine tune the following heuristic. The rationale is that if the matrix is very large,
   //       processing 8 rows at once might be counter productive wrt cache.
-  const Index n8 = lhs.stride()*sizeof(LhsScalar)>32000 ? 0 : rows-7;
-  const Index n4 = rows-3;
-  const Index n2 = rows-1;
+  const Index n8 = lhs.stride() * sizeof(LhsScalar) > 32000 ? 0 : rows - 7;
+  const Index n4 = rows - 3;
+  const Index n2 = rows - 1;
 
   // TODO: for padded aligned inputs, we could enable aligned reads
-  enum { LhsAlignment = Unaligned,
-         ResPacketSize = Traits::ResPacketSize,
-         ResPacketSizeHalf = HalfTraits::ResPacketSize,
-         ResPacketSizeQuarter = QuarterTraits::ResPacketSize,
-         LhsPacketSize = Traits::LhsPacketSize,
-         LhsPacketSizeHalf = HalfTraits::LhsPacketSize,
-         LhsPacketSizeQuarter = QuarterTraits::LhsPacketSize,
-         HasHalf = (int)ResPacketSizeHalf < (int)ResPacketSize,
-         HasQuarter = (int)ResPacketSizeQuarter < (int)ResPacketSizeHalf
+  enum {
+    LhsAlignment = Unaligned,
+    ResPacketSize = Traits::ResPacketSize,
+    ResPacketSizeHalf = HalfTraits::ResPacketSize,
+    ResPacketSizeQuarter = QuarterTraits::ResPacketSize,
+    LhsPacketSize = Traits::LhsPacketSize,
+    LhsPacketSizeHalf = HalfTraits::LhsPacketSize,
+    LhsPacketSizeQuarter = QuarterTraits::LhsPacketSize,
+    HasHalf = (int)ResPacketSizeHalf < (int)ResPacketSize,
+    HasQuarter = (int)ResPacketSizeQuarter < (int)ResPacketSizeHalf
   };
 
-  Index i=0;
-  for(; i<n8; i+=8)
-  {
-    ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
-              c1 = pset1<ResPacket>(ResScalar(0)),
-              c2 = pset1<ResPacket>(ResScalar(0)),
-              c3 = pset1<ResPacket>(ResScalar(0)),
-              c4 = pset1<ResPacket>(ResScalar(0)),
-              c5 = pset1<ResPacket>(ResScalar(0)),
-              c6 = pset1<ResPacket>(ResScalar(0)),
-              c7 = pset1<ResPacket>(ResScalar(0));
+  using UnsignedIndex = typename make_unsigned<Index>::type;
+  const Index fullColBlockEnd = LhsPacketSize * (UnsignedIndex(cols) / LhsPacketSize);
+  const Index halfColBlockEnd = LhsPacketSizeHalf * (UnsignedIndex(cols) / LhsPacketSizeHalf);
+  const Index quarterColBlockEnd = LhsPacketSizeQuarter * (UnsignedIndex(cols) / LhsPacketSizeQuarter);
 
-    Index j=0;
-    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
-    {
-      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j,0);
+  Index i = 0;
+  for (; i < n8; i += 8) {
+    ResPacket c0 = pset1<ResPacket>(ResScalar(0)), c1 = pset1<ResPacket>(ResScalar(0)),
+              c2 = pset1<ResPacket>(ResScalar(0)), c3 = pset1<ResPacket>(ResScalar(0)),
+              c4 = pset1<ResPacket>(ResScalar(0)), c5 = pset1<ResPacket>(ResScalar(0)),
+              c6 = pset1<ResPacket>(ResScalar(0)), c7 = pset1<ResPacket>(ResScalar(0));
 
-      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
-      c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+1,j),b0,c1);
-      c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+2,j),b0,c2);
-      c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+3,j),b0,c3);
-      c4 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+4,j),b0,c4);
-      c5 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+5,j),b0,c5);
-      c6 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+6,j),b0,c6);
-      c7 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+7,j),b0,c7);
+    for (Index j = 0; j < fullColBlockEnd; j += LhsPacketSize) {
+      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j, 0);
+
+      c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 0, j), b0, c0);
+      c1 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 1, j), b0, c1);
+      c2 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 2, j), b0, c2);
+      c3 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 3, j), b0, c3);
+      c4 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 4, j), b0, c4);
+      c5 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 5, j), b0, c5);
+      c6 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 6, j), b0, c6);
+      c7 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 7, j), b0, c7);
     }
     ResScalar cc0 = predux(c0);
     ResScalar cc1 = predux(c1);
@@ -393,126 +362,112 @@
     ResScalar cc5 = predux(c5);
     ResScalar cc6 = predux(c6);
     ResScalar cc7 = predux(c7);
-    for(; j<cols; ++j)
-    {
-      RhsScalar b0 = rhs(j,0);
 
-      cc0 += cj.pmul(lhs(i+0,j), b0);
-      cc1 += cj.pmul(lhs(i+1,j), b0);
-      cc2 += cj.pmul(lhs(i+2,j), b0);
-      cc3 += cj.pmul(lhs(i+3,j), b0);
-      cc4 += cj.pmul(lhs(i+4,j), b0);
-      cc5 += cj.pmul(lhs(i+5,j), b0);
-      cc6 += cj.pmul(lhs(i+6,j), b0);
-      cc7 += cj.pmul(lhs(i+7,j), b0);
+    for (Index j = fullColBlockEnd; j < cols; ++j) {
+      RhsScalar b0 = rhs(j, 0);
+
+      cc0 += cj.pmul(lhs(i + 0, j), b0);
+      cc1 += cj.pmul(lhs(i + 1, j), b0);
+      cc2 += cj.pmul(lhs(i + 2, j), b0);
+      cc3 += cj.pmul(lhs(i + 3, j), b0);
+      cc4 += cj.pmul(lhs(i + 4, j), b0);
+      cc5 += cj.pmul(lhs(i + 5, j), b0);
+      cc6 += cj.pmul(lhs(i + 6, j), b0);
+      cc7 += cj.pmul(lhs(i + 7, j), b0);
     }
-    res[(i+0)*resIncr] += alpha*cc0;
-    res[(i+1)*resIncr] += alpha*cc1;
-    res[(i+2)*resIncr] += alpha*cc2;
-    res[(i+3)*resIncr] += alpha*cc3;
-    res[(i+4)*resIncr] += alpha*cc4;
-    res[(i+5)*resIncr] += alpha*cc5;
-    res[(i+6)*resIncr] += alpha*cc6;
-    res[(i+7)*resIncr] += alpha*cc7;
+    res[(i + 0) * resIncr] += alpha * cc0;
+    res[(i + 1) * resIncr] += alpha * cc1;
+    res[(i + 2) * resIncr] += alpha * cc2;
+    res[(i + 3) * resIncr] += alpha * cc3;
+    res[(i + 4) * resIncr] += alpha * cc4;
+    res[(i + 5) * resIncr] += alpha * cc5;
+    res[(i + 6) * resIncr] += alpha * cc6;
+    res[(i + 7) * resIncr] += alpha * cc7;
   }
-  for(; i<n4; i+=4)
-  {
-    ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
-              c1 = pset1<ResPacket>(ResScalar(0)),
-              c2 = pset1<ResPacket>(ResScalar(0)),
-              c3 = pset1<ResPacket>(ResScalar(0));
+  for (; i < n4; i += 4) {
+    ResPacket c0 = pset1<ResPacket>(ResScalar(0)), c1 = pset1<ResPacket>(ResScalar(0)),
+              c2 = pset1<ResPacket>(ResScalar(0)), c3 = pset1<ResPacket>(ResScalar(0));
 
-    Index j=0;
-    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
-    {
-      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j,0);
+    for (Index j = 0; j < fullColBlockEnd; j += LhsPacketSize) {
+      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j, 0);
 
-      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
-      c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+1,j),b0,c1);
-      c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+2,j),b0,c2);
-      c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+3,j),b0,c3);
+      c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 0, j), b0, c0);
+      c1 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 1, j), b0, c1);
+      c2 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 2, j), b0, c2);
+      c3 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 3, j), b0, c3);
     }
     ResScalar cc0 = predux(c0);
     ResScalar cc1 = predux(c1);
     ResScalar cc2 = predux(c2);
     ResScalar cc3 = predux(c3);
-    for(; j<cols; ++j)
-    {
-      RhsScalar b0 = rhs(j,0);
 
-      cc0 += cj.pmul(lhs(i+0,j), b0);
-      cc1 += cj.pmul(lhs(i+1,j), b0);
-      cc2 += cj.pmul(lhs(i+2,j), b0);
-      cc3 += cj.pmul(lhs(i+3,j), b0);
+    for (Index j = fullColBlockEnd; j < cols; ++j) {
+      RhsScalar b0 = rhs(j, 0);
+
+      cc0 += cj.pmul(lhs(i + 0, j), b0);
+      cc1 += cj.pmul(lhs(i + 1, j), b0);
+      cc2 += cj.pmul(lhs(i + 2, j), b0);
+      cc3 += cj.pmul(lhs(i + 3, j), b0);
     }
-    res[(i+0)*resIncr] += alpha*cc0;
-    res[(i+1)*resIncr] += alpha*cc1;
-    res[(i+2)*resIncr] += alpha*cc2;
-    res[(i+3)*resIncr] += alpha*cc3;
+    res[(i + 0) * resIncr] += alpha * cc0;
+    res[(i + 1) * resIncr] += alpha * cc1;
+    res[(i + 2) * resIncr] += alpha * cc2;
+    res[(i + 3) * resIncr] += alpha * cc3;
   }
-  for(; i<n2; i+=2)
-  {
-    ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
-              c1 = pset1<ResPacket>(ResScalar(0));
+  for (; i < n2; i += 2) {
+    ResPacket c0 = pset1<ResPacket>(ResScalar(0)), c1 = pset1<ResPacket>(ResScalar(0));
 
-    Index j=0;
-    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
-    {
-      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j,0);
+    for (Index j = 0; j < fullColBlockEnd; j += LhsPacketSize) {
+      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j, 0);
 
-      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
-      c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+1,j),b0,c1);
+      c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 0, j), b0, c0);
+      c1 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i + 1, j), b0, c1);
     }
     ResScalar cc0 = predux(c0);
     ResScalar cc1 = predux(c1);
-    for(; j<cols; ++j)
-    {
-      RhsScalar b0 = rhs(j,0);
 
-      cc0 += cj.pmul(lhs(i+0,j), b0);
-      cc1 += cj.pmul(lhs(i+1,j), b0);
+    for (Index j = fullColBlockEnd; j < cols; ++j) {
+      RhsScalar b0 = rhs(j, 0);
+
+      cc0 += cj.pmul(lhs(i + 0, j), b0);
+      cc1 += cj.pmul(lhs(i + 1, j), b0);
     }
-    res[(i+0)*resIncr] += alpha*cc0;
-    res[(i+1)*resIncr] += alpha*cc1;
+    res[(i + 0) * resIncr] += alpha * cc0;
+    res[(i + 1) * resIncr] += alpha * cc1;
   }
-  for(; i<rows; ++i)
-  {
+  for (; i < rows; ++i) {
     ResPacket c0 = pset1<ResPacket>(ResScalar(0));
     ResPacketHalf c0_h = pset1<ResPacketHalf>(ResScalar(0));
     ResPacketQuarter c0_q = pset1<ResPacketQuarter>(ResScalar(0));
-    Index j=0;
-    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
-    {
-      RhsPacket b0 = rhs.template load<RhsPacket,Unaligned>(j,0);
-      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i,j),b0,c0);
+
+    for (Index j = 0; j < fullColBlockEnd; j += LhsPacketSize) {
+      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j, 0);
+      c0 = pcj.pmadd(lhs.template load<LhsPacket, LhsAlignment>(i, j), b0, c0);
     }
     ResScalar cc0 = predux(c0);
     if (HasHalf) {
-      for(; j+LhsPacketSizeHalf<=cols; j+=LhsPacketSizeHalf)
-        {
-          RhsPacketHalf b0 = rhs.template load<RhsPacketHalf,Unaligned>(j,0);
-          c0_h = pcj_half.pmadd(lhs.template load<LhsPacketHalf,LhsAlignment>(i,j),b0,c0_h);
-        }
+      for (Index j = fullColBlockEnd; j < halfColBlockEnd; j += LhsPacketSizeHalf) {
+        RhsPacketHalf b0 = rhs.template load<RhsPacketHalf, Unaligned>(j, 0);
+        c0_h = pcj_half.pmadd(lhs.template load<LhsPacketHalf, LhsAlignment>(i, j), b0, c0_h);
+      }
       cc0 += predux(c0_h);
     }
     if (HasQuarter) {
-      for(; j+LhsPacketSizeQuarter<=cols; j+=LhsPacketSizeQuarter)
-        {
-          RhsPacketQuarter b0 = rhs.template load<RhsPacketQuarter,Unaligned>(j,0);
-          c0_q = pcj_quarter.pmadd(lhs.template load<LhsPacketQuarter,LhsAlignment>(i,j),b0,c0_q);
-        }
+      for (Index j = halfColBlockEnd; j < quarterColBlockEnd; j += LhsPacketSizeQuarter) {
+        RhsPacketQuarter b0 = rhs.template load<RhsPacketQuarter, Unaligned>(j, 0);
+        c0_q = pcj_quarter.pmadd(lhs.template load<LhsPacketQuarter, LhsAlignment>(i, j), b0, c0_q);
+      }
       cc0 += predux(c0_q);
     }
-    for(; j<cols; ++j)
-    {
-      cc0 += cj.pmul(lhs(i,j), rhs(j,0));
+    for (Index j = quarterColBlockEnd; j < cols; ++j) {
+      cc0 += cj.pmul(lhs(i, j), rhs(j, 0));
     }
-    res[i*resIncr] += alpha*cc0;
+    res[i * resIncr] += alpha * cc0;
   }
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_GENERAL_MATRIX_VECTOR_H
+#endif  // EIGEN_GENERAL_MATRIX_VECTOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h
index 8f91879..667fea2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h
@@ -10,171 +10,267 @@
 #ifndef EIGEN_PARALLELIZER_H
 #define EIGEN_PARALLELIZER_H
 
-#if EIGEN_HAS_CXX11_ATOMIC
-#include <atomic>
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+// Note that in the following, there are 3 different uses of the concept
+// "number of threads":
+//  1. Max number of threads used by OpenMP or ThreadPool.
+//     * For OpenMP this is typically the value set by the OMP_NUM_THREADS
+//       environment variable, or by a call to omp_set_num_threads() prior to
+//       calling Eigen.
+//     * For ThreadPool, this is the number of threads in the ThreadPool.
+//  2. Max number of threads currently allowed to be used by parallel Eigen
+//     operations. This is set by setNbThreads(), and cannot exceed the value
+//     in 1.
+//  3. The actual number of threads used for a given parallel Eigen operation.
+//     This is typically computed on the fly using a cost model and cannot exceed
+//     the value in 2.
+//     * For OpenMP, this is typically the number of threads specified in individual
+//       "omp parallel" pragmas associated with an Eigen operation.
+//     * For ThreadPool, it is the number of concurrent tasks scheduled in the
+//       threadpool for a given Eigen operation. Notice that since the threadpool
+//       uses task stealing, there is no way to limit the number of concurrently
+//       executing tasks to below the number in 1. except by limiting the total
+//       number of tasks in flight.
+
+#if defined(EIGEN_HAS_OPENMP) && defined(EIGEN_GEMM_THREADPOOL)
+#error "EIGEN_HAS_OPENMP and EIGEN_GEMM_THREADPOOL may not both be defined."
 #endif
 
 namespace Eigen {
 
 namespace internal {
-
-/** \internal */
-inline void manage_multi_threading(Action action, int* v)
-{
-  static int m_maxThreads = -1;
-  EIGEN_UNUSED_VARIABLE(m_maxThreads)
-
-  if(action==SetAction)
-  {
-    eigen_internal_assert(v!=0);
-    m_maxThreads = *v;
-  }
-  else if(action==GetAction)
-  {
-    eigen_internal_assert(v!=0);
-    #ifdef EIGEN_HAS_OPENMP
-    if(m_maxThreads>0)
-      *v = m_maxThreads;
-    else
-      *v = omp_get_max_threads();
-    #else
-    *v = 1;
-    #endif
-  }
-  else
-  {
-    eigen_internal_assert(false);
-  }
+inline void manage_multi_threading(Action action, int* v);
 }
 
-}
+// Public APIs.
 
 /** Must be call first when calling Eigen from multiple threads */
-inline void initParallel()
-{
-  int nbt;
-  internal::manage_multi_threading(GetAction, &nbt);
-  std::ptrdiff_t l1, l2, l3;
-  internal::manage_caching_sizes(GetAction, &l1, &l2, &l3);
-}
+EIGEN_DEPRECATED inline void initParallel() {}
 
 /** \returns the max number of threads reserved for Eigen
-  * \sa setNbThreads */
-inline int nbThreads()
-{
+ * \sa setNbThreads */
+inline int nbThreads() {
   int ret;
   internal::manage_multi_threading(GetAction, &ret);
   return ret;
 }
 
 /** Sets the max number of threads reserved for Eigen
-  * \sa nbThreads */
-inline void setNbThreads(int v)
-{
-  internal::manage_multi_threading(SetAction, &v);
+ * \sa nbThreads */
+inline void setNbThreads(int v) { internal::manage_multi_threading(SetAction, &v); }
+
+#ifdef EIGEN_GEMM_THREADPOOL
+// Sets the ThreadPool used by Eigen parallel Gemm.
+//
+// NOTICE: This function has a known race condition with
+// parallelize_gemm below, and should not be called while
+// an instance of that function is running.
+//
+// TODO(rmlarsen): Make the device API available instead of
+// storing a local static pointer variable to avoid this issue.
+inline ThreadPool* setGemmThreadPool(ThreadPool* new_pool) {
+  static ThreadPool* pool;
+  if (new_pool != nullptr) {
+    // This will wait for work in all threads in *pool to finish,
+    // then destroy the old ThreadPool, and then replace it with new_pool.
+    pool = new_pool;
+    // Reset the number of threads to the number of threads on the new pool.
+    setNbThreads(pool->NumThreads());
+  }
+  return pool;
 }
 
+// Gets the ThreadPool used by Eigen parallel Gemm.
+inline ThreadPool* getGemmThreadPool() { return setGemmThreadPool(nullptr); }
+#endif
+
 namespace internal {
 
-template<typename Index> struct GemmParallelInfo
-{
-  GemmParallelInfo() : sync(-1), users(0), lhs_start(0), lhs_length(0) {}
+// Implementation.
 
-  // volatile is not enough on all architectures (see bug 1572)
-  // to guarantee that when thread A says to thread B that it is
-  // done with packing a block, then all writes have been really
-  // carried out... C++11 memory model+atomic guarantees this.
-#if EIGEN_HAS_CXX11_ATOMIC
+#if defined(EIGEN_USE_BLAS) || (!defined(EIGEN_HAS_OPENMP) && !defined(EIGEN_GEMM_THREADPOOL))
+
+inline void manage_multi_threading(Action action, int* v) {
+  if (action == SetAction) {
+    eigen_internal_assert(v != nullptr);
+  } else if (action == GetAction) {
+    eigen_internal_assert(v != nullptr);
+    *v = 1;
+  } else {
+    eigen_internal_assert(false);
+  }
+}
+template <typename Index>
+struct GemmParallelInfo {};
+template <bool Condition, typename Functor, typename Index>
+EIGEN_STRONG_INLINE void parallelize_gemm(const Functor& func, Index rows, Index cols, Index /*unused*/,
+                                          bool /*unused*/) {
+  func(0, rows, 0, cols);
+}
+
+#else
+
+template <typename Index>
+struct GemmParallelTaskInfo {
+  GemmParallelTaskInfo() : sync(-1), users(0), lhs_start(0), lhs_length(0) {}
   std::atomic<Index> sync;
   std::atomic<int> users;
-#else
-  Index volatile sync;
-  int volatile users;
-#endif
-
   Index lhs_start;
   Index lhs_length;
 };
 
-template<bool Condition, typename Functor, typename Index>
-void parallelize_gemm(const Functor& func, Index rows, Index cols, Index depth, bool transpose)
-{
-  // TODO when EIGEN_USE_BLAS is defined,
-  // we should still enable OMP for other scalar types
-  // Without C++11, we have to disable GEMM's parallelization on
-  // non x86 architectures because there volatile is not enough for our purpose.
-  // See bug 1572.
-#if (! defined(EIGEN_HAS_OPENMP)) || defined(EIGEN_USE_BLAS) || ((!EIGEN_HAS_CXX11_ATOMIC) && !(EIGEN_ARCH_i386_OR_x86_64))
-  // FIXME the transpose variable is only needed to properly split
-  // the matrix product when multithreading is enabled. This is a temporary
-  // fix to support row-major destination matrices. This whole
-  // parallelizer mechanism has to be redesigned anyway.
-  EIGEN_UNUSED_VARIABLE(depth);
-  EIGEN_UNUSED_VARIABLE(transpose);
-  func(0,rows, 0,cols);
-#else
+template <typename Index>
+struct GemmParallelInfo {
+  const int logical_thread_id;
+  const int num_threads;
+  GemmParallelTaskInfo<Index>* task_info;
 
-  // Dynamically check whether we should enable or disable OpenMP.
+  GemmParallelInfo(int logical_thread_id_, int num_threads_, GemmParallelTaskInfo<Index>* task_info_)
+      : logical_thread_id(logical_thread_id_), num_threads(num_threads_), task_info(task_info_) {}
+};
+
+inline void manage_multi_threading(Action action, int* v) {
+  static int m_maxThreads = -1;
+  if (action == SetAction) {
+    eigen_internal_assert(v != nullptr);
+#if defined(EIGEN_HAS_OPENMP)
+    // Calling action == SetAction and *v = 0 means
+    // restoring m_maxThreads to the maximum number of threads specified
+    // for OpenMP.
+    eigen_internal_assert(*v >= 0);
+    int omp_threads = omp_get_max_threads();
+    m_maxThreads = (*v == 0 ? omp_threads : std::min(*v, omp_threads));
+#elif defined(EIGEN_GEMM_THREADPOOL)
+    // Calling action == SetAction and *v = 0 means
+    // restoring m_maxThreads to the number of threads in the ThreadPool,
+    // which defaults to 1 if no pool was provided.
+    eigen_internal_assert(*v >= 0);
+    ThreadPool* pool = getGemmThreadPool();
+    int pool_threads = pool != nullptr ? pool->NumThreads() : 1;
+    m_maxThreads = (*v == 0 ? pool_threads : numext::mini(pool_threads, *v));
+#endif
+  } else if (action == GetAction) {
+    eigen_internal_assert(v != nullptr);
+    *v = m_maxThreads;
+  } else {
+    eigen_internal_assert(false);
+  }
+}
+
+template <bool Condition, typename Functor, typename Index>
+EIGEN_STRONG_INLINE void parallelize_gemm(const Functor& func, Index rows, Index cols, Index depth, bool transpose) {
+  // Dynamically check whether we should even try to execute in parallel.
   // The conditions are:
   // - the max number of threads we can create is greater than 1
   // - we are not already in a parallel code
   // - the sizes are large enough
 
   // compute the maximal number of threads from the size of the product:
-  // This first heuristic takes into account that the product kernel is fully optimized when working with nr columns at once.
+  // This first heuristic takes into account that the product kernel is fully optimized when working with nr columns at
+  // once.
   Index size = transpose ? rows : cols;
-  Index pb_max_threads = std::max<Index>(1,size / Functor::Traits::nr);
+  Index pb_max_threads = std::max<Index>(1, size / Functor::Traits::nr);
 
   // compute the maximal number of threads from the total amount of work:
-  double work = static_cast<double>(rows) * static_cast<double>(cols) *
-      static_cast<double>(depth);
+  double work = static_cast<double>(rows) * static_cast<double>(cols) * static_cast<double>(depth);
   double kMinTaskSize = 50000;  // FIXME improve this heuristic.
-  pb_max_threads = std::max<Index>(1, std::min<Index>(pb_max_threads, static_cast<Index>( work / kMinTaskSize ) ));
+  pb_max_threads = std::max<Index>(1, std::min<Index>(pb_max_threads, static_cast<Index>(work / kMinTaskSize)));
 
   // compute the number of threads we are going to use
-  Index threads = std::min<Index>(nbThreads(), pb_max_threads);
+  int threads = std::min<int>(nbThreads(), static_cast<int>(pb_max_threads));
 
-  // if multi-threading is explicitly disabled, not useful, or if we already are in a parallel session,
-  // then abort multi-threading
-  // FIXME omp_get_num_threads()>1 only works for openmp, what if the user does not use openmp?
-  if((!Condition) || (threads==1) || (omp_get_num_threads()>1))
-    return func(0,rows, 0,cols);
+  // if multi-threading is explicitly disabled, not useful, or if we already are
+  // inside a parallel session, then abort multi-threading
+  bool dont_parallelize = (!Condition) || (threads <= 1);
+#if defined(EIGEN_HAS_OPENMP)
+  // don't parallelize if we are executing in a parallel context already.
+  dont_parallelize |= omp_get_num_threads() > 1;
+#elif defined(EIGEN_GEMM_THREADPOOL)
+  // don't parallelize if we have a trivial threadpool or the current thread id
+  // is != -1, indicating that we are already executing on a thread inside the pool.
+  // In other words, we do not allow nested parallelism, since this would lead to
+  // deadlocks due to the workstealing nature of the threadpool.
+  ThreadPool* pool = getGemmThreadPool();
+  dont_parallelize |= (pool == nullptr || pool->CurrentThreadId() != -1);
+#endif
+  if (dont_parallelize) return func(0, rows, 0, cols);
 
-  Eigen::initParallel();
   func.initParallelSession(threads);
 
-  if(transpose)
-    std::swap(rows,cols);
+  if (transpose) std::swap(rows, cols);
 
-  ei_declare_aligned_stack_constructed_variable(GemmParallelInfo<Index>,info,threads,0);
+  ei_declare_aligned_stack_constructed_variable(GemmParallelTaskInfo<Index>, task_info, threads, 0);
 
-  #pragma omp parallel num_threads(threads)
+#if defined(EIGEN_HAS_OPENMP)
+#pragma omp parallel num_threads(threads)
   {
     Index i = omp_get_thread_num();
-    // Note that the actual number of threads might be lower than the number of request ones.
+    // Note that the actual number of threads might be lower than the number of
+    // requested ones
     Index actual_threads = omp_get_num_threads();
+    GemmParallelInfo<Index> info(i, static_cast<int>(actual_threads), task_info);
 
     Index blockCols = (cols / actual_threads) & ~Index(0x3);
     Index blockRows = (rows / actual_threads);
-    blockRows = (blockRows/Functor::Traits::mr)*Functor::Traits::mr;
+    blockRows = (blockRows / Functor::Traits::mr) * Functor::Traits::mr;
 
-    Index r0 = i*blockRows;
-    Index actualBlockRows = (i+1==actual_threads) ? rows-r0 : blockRows;
+    Index r0 = i * blockRows;
+    Index actualBlockRows = (i + 1 == actual_threads) ? rows - r0 : blockRows;
 
-    Index c0 = i*blockCols;
-    Index actualBlockCols = (i+1==actual_threads) ? cols-c0 : blockCols;
+    Index c0 = i * blockCols;
+    Index actualBlockCols = (i + 1 == actual_threads) ? cols - c0 : blockCols;
 
-    info[i].lhs_start = r0;
-    info[i].lhs_length = actualBlockRows;
+    info.task_info[i].lhs_start = r0;
+    info.task_info[i].lhs_length = actualBlockRows;
 
-    if(transpose) func(c0, actualBlockCols, 0, rows, info);
-    else          func(0, rows, c0, actualBlockCols, info);
+    if (transpose)
+      func(c0, actualBlockCols, 0, rows, &info);
+    else
+      func(0, rows, c0, actualBlockCols, &info);
   }
+
+#elif defined(EIGEN_GEMM_THREADPOOL)
+  ei_declare_aligned_stack_constructed_variable(GemmParallelTaskInfo<Index>, meta_info, threads, 0);
+  Barrier barrier(threads);
+  auto task = [=, &func, &barrier, &task_info](int i) {
+    Index actual_threads = threads;
+    GemmParallelInfo<Index> info(i, static_cast<int>(actual_threads), task_info);
+    Index blockCols = (cols / actual_threads) & ~Index(0x3);
+    Index blockRows = (rows / actual_threads);
+    blockRows = (blockRows / Functor::Traits::mr) * Functor::Traits::mr;
+
+    Index r0 = i * blockRows;
+    Index actualBlockRows = (i + 1 == actual_threads) ? rows - r0 : blockRows;
+
+    Index c0 = i * blockCols;
+    Index actualBlockCols = (i + 1 == actual_threads) ? cols - c0 : blockCols;
+
+    info.task_info[i].lhs_start = r0;
+    info.task_info[i].lhs_length = actualBlockRows;
+
+    if (transpose)
+      func(c0, actualBlockCols, 0, rows, &info);
+    else
+      func(0, rows, c0, actualBlockCols, &info);
+
+    barrier.Notify();
+  };
+  // Notice that we do not schedule more than "threads" tasks, which allows us to
+  // limit number of running threads, even if the threadpool itself was constructed
+  // with a larger number of threads.
+  for (int i = 0; i < threads - 1; ++i) {
+    pool->Schedule([=, task = std::move(task)] { task(i); });
+  }
+  task(threads - 1);
+  barrier.Wait();
 #endif
 }
 
-} // end namespace internal
+#endif
 
-} // end namespace Eigen
+}  // end namespace internal
+}  // end namespace Eigen
 
-#endif // EIGEN_PARALLELIZER_H
+#endif  // EIGEN_PARALLELIZER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
index 33ecf10..899283d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
@@ -10,278 +10,246 @@
 #ifndef EIGEN_SELFADJOINT_MATRIX_MATRIX_H
 #define EIGEN_SELFADJOINT_MATRIX_MATRIX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 // pack a selfadjoint block diagonal for use with the gebp_kernel
-template<typename Scalar, typename Index, int Pack1, int Pack2_dummy, int StorageOrder>
-struct symm_pack_lhs
-{
-  template<int BlockRows> inline
-  void pack(Scalar* blockA, const const_blas_data_mapper<Scalar,Index,StorageOrder>& lhs, Index cols, Index i, Index& count)
-  {
+template <typename Scalar, typename Index, int Pack1, int Pack2_dummy, int StorageOrder>
+struct symm_pack_lhs {
+  template <int BlockRows>
+  inline void pack(Scalar* blockA, const const_blas_data_mapper<Scalar, Index, StorageOrder>& lhs, Index cols, Index i,
+                   Index& count) {
     // normal copy
-    for(Index k=0; k<i; k++)
-      for(Index w=0; w<BlockRows; w++)
-        blockA[count++] = lhs(i+w,k);           // normal
+    for (Index k = 0; k < i; k++)
+      for (Index w = 0; w < BlockRows; w++) blockA[count++] = lhs(i + w, k);  // normal
     // symmetric copy
     Index h = 0;
-    for(Index k=i; k<i+BlockRows; k++)
-    {
-      for(Index w=0; w<h; w++)
-        blockA[count++] = numext::conj(lhs(k, i+w)); // transposed
+    for (Index k = i; k < i + BlockRows; k++) {
+      for (Index w = 0; w < h; w++) blockA[count++] = numext::conj(lhs(k, i + w));  // transposed
 
-      blockA[count++] = numext::real(lhs(k,k));   // real (diagonal)
+      blockA[count++] = numext::real(lhs(k, k));  // real (diagonal)
 
-      for(Index w=h+1; w<BlockRows; w++)
-        blockA[count++] = lhs(i+w, k);          // normal
+      for (Index w = h + 1; w < BlockRows; w++) blockA[count++] = lhs(i + w, k);  // normal
       ++h;
     }
     // transposed copy
-    for(Index k=i+BlockRows; k<cols; k++)
-      for(Index w=0; w<BlockRows; w++)
-        blockA[count++] = numext::conj(lhs(k, i+w)); // transposed
+    for (Index k = i + BlockRows; k < cols; k++)
+      for (Index w = 0; w < BlockRows; w++) blockA[count++] = numext::conj(lhs(k, i + w));  // transposed
   }
-  void operator()(Scalar* blockA, const Scalar* _lhs, Index lhsStride, Index cols, Index rows)
-  {
+  void operator()(Scalar* blockA, const Scalar* lhs_, Index lhsStride, Index cols, Index rows) {
     typedef typename unpacket_traits<typename packet_traits<Scalar>::type>::half HalfPacket;
-    typedef typename unpacket_traits<typename unpacket_traits<typename packet_traits<Scalar>::type>::half>::half QuarterPacket;
-    enum { PacketSize = packet_traits<Scalar>::size,
-           HalfPacketSize = unpacket_traits<HalfPacket>::size,
-           QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
-           HasHalf = (int)HalfPacketSize < (int)PacketSize,
-           HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize};
+    typedef typename unpacket_traits<typename unpacket_traits<typename packet_traits<Scalar>::type>::half>::half
+        QuarterPacket;
+    enum {
+      PacketSize = packet_traits<Scalar>::size,
+      HalfPacketSize = unpacket_traits<HalfPacket>::size,
+      QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
+      HasHalf = (int)HalfPacketSize < (int)PacketSize,
+      HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize
+    };
 
-    const_blas_data_mapper<Scalar,Index,StorageOrder> lhs(_lhs,lhsStride);
+    const_blas_data_mapper<Scalar, Index, StorageOrder> lhs(lhs_, lhsStride);
     Index count = 0;
-    //Index peeled_mc3 = (rows/Pack1)*Pack1;
-    
-    const Index peeled_mc3 = Pack1>=3*PacketSize ? (rows/(3*PacketSize))*(3*PacketSize) : 0;
-    const Index peeled_mc2 = Pack1>=2*PacketSize ? peeled_mc3+((rows-peeled_mc3)/(2*PacketSize))*(2*PacketSize) : 0;
-    const Index peeled_mc1 = Pack1>=1*PacketSize ? peeled_mc2+((rows-peeled_mc2)/(1*PacketSize))*(1*PacketSize) : 0;
-    const Index peeled_mc_half = Pack1>=HalfPacketSize ? peeled_mc1+((rows-peeled_mc1)/(HalfPacketSize))*(HalfPacketSize) : 0;
-    const Index peeled_mc_quarter = Pack1>=QuarterPacketSize ? peeled_mc_half+((rows-peeled_mc_half)/(QuarterPacketSize))*(QuarterPacketSize) : 0;
-    
-    if(Pack1>=3*PacketSize)
-      for(Index i=0; i<peeled_mc3; i+=3*PacketSize)
-        pack<3*PacketSize>(blockA, lhs, cols, i, count);
-    
-    if(Pack1>=2*PacketSize)
-      for(Index i=peeled_mc3; i<peeled_mc2; i+=2*PacketSize)
-        pack<2*PacketSize>(blockA, lhs, cols, i, count);
-    
-    if(Pack1>=1*PacketSize)
-      for(Index i=peeled_mc2; i<peeled_mc1; i+=1*PacketSize)
-        pack<1*PacketSize>(blockA, lhs, cols, i, count);
+    // Index peeled_mc3 = (rows/Pack1)*Pack1;
 
-    if(HasHalf && Pack1>=HalfPacketSize)
-      for(Index i=peeled_mc1; i<peeled_mc_half; i+=HalfPacketSize)
+    const Index peeled_mc3 = Pack1 >= 3 * PacketSize ? (rows / (3 * PacketSize)) * (3 * PacketSize) : 0;
+    const Index peeled_mc2 =
+        Pack1 >= 2 * PacketSize ? peeled_mc3 + ((rows - peeled_mc3) / (2 * PacketSize)) * (2 * PacketSize) : 0;
+    const Index peeled_mc1 =
+        Pack1 >= 1 * PacketSize ? peeled_mc2 + ((rows - peeled_mc2) / (1 * PacketSize)) * (1 * PacketSize) : 0;
+    const Index peeled_mc_half =
+        Pack1 >= HalfPacketSize ? peeled_mc1 + ((rows - peeled_mc1) / (HalfPacketSize)) * (HalfPacketSize) : 0;
+    const Index peeled_mc_quarter =
+        Pack1 >= QuarterPacketSize
+            ? peeled_mc_half + ((rows - peeled_mc_half) / (QuarterPacketSize)) * (QuarterPacketSize)
+            : 0;
+
+    if (Pack1 >= 3 * PacketSize)
+      for (Index i = 0; i < peeled_mc3; i += 3 * PacketSize) pack<3 * PacketSize>(blockA, lhs, cols, i, count);
+
+    if (Pack1 >= 2 * PacketSize)
+      for (Index i = peeled_mc3; i < peeled_mc2; i += 2 * PacketSize) pack<2 * PacketSize>(blockA, lhs, cols, i, count);
+
+    if (Pack1 >= 1 * PacketSize)
+      for (Index i = peeled_mc2; i < peeled_mc1; i += 1 * PacketSize) pack<1 * PacketSize>(blockA, lhs, cols, i, count);
+
+    if (HasHalf && Pack1 >= HalfPacketSize)
+      for (Index i = peeled_mc1; i < peeled_mc_half; i += HalfPacketSize)
         pack<HalfPacketSize>(blockA, lhs, cols, i, count);
 
-    if(HasQuarter && Pack1>=QuarterPacketSize)
-      for(Index i=peeled_mc_half; i<peeled_mc_quarter; i+=QuarterPacketSize)
+    if (HasQuarter && Pack1 >= QuarterPacketSize)
+      for (Index i = peeled_mc_half; i < peeled_mc_quarter; i += QuarterPacketSize)
         pack<QuarterPacketSize>(blockA, lhs, cols, i, count);
 
     // do the same with mr==1
-    for(Index i=peeled_mc_quarter; i<rows; i++)
-    {
-      for(Index k=0; k<i; k++)
-        blockA[count++] = lhs(i, k);                   // normal
+    for (Index i = peeled_mc_quarter; i < rows; i++) {
+      for (Index k = 0; k < i; k++) blockA[count++] = lhs(i, k);  // normal
 
-      blockA[count++] = numext::real(lhs(i, i));       // real (diagonal)
+      blockA[count++] = numext::real(lhs(i, i));  // real (diagonal)
 
-      for(Index k=i+1; k<cols; k++)
-        blockA[count++] = numext::conj(lhs(k, i));     // transposed
+      for (Index k = i + 1; k < cols; k++) blockA[count++] = numext::conj(lhs(k, i));  // transposed
     }
   }
 };
 
-template<typename Scalar, typename Index, int nr, int StorageOrder>
-struct symm_pack_rhs
-{
+template <typename Scalar, typename Index, int nr, int StorageOrder>
+struct symm_pack_rhs {
   enum { PacketSize = packet_traits<Scalar>::size };
-  void operator()(Scalar* blockB, const Scalar* _rhs, Index rhsStride, Index rows, Index cols, Index k2)
-  {
+  void operator()(Scalar* blockB, const Scalar* rhs_, Index rhsStride, Index rows, Index cols, Index k2) {
     Index end_k = k2 + rows;
     Index count = 0;
-    const_blas_data_mapper<Scalar,Index,StorageOrder> rhs(_rhs,rhsStride);
-    Index packet_cols8 = nr>=8 ? (cols/8) * 8 : 0;
-    Index packet_cols4 = nr>=4 ? (cols/4) * 4 : 0;
+    const_blas_data_mapper<Scalar, Index, StorageOrder> rhs(rhs_, rhsStride);
+    Index packet_cols8 = nr >= 8 ? (cols / 8) * 8 : 0;
+    Index packet_cols4 = nr >= 4 ? (cols / 4) * 4 : 0;
 
     // first part: normal case
-    for(Index j2=0; j2<k2; j2+=nr)
-    {
-      for(Index k=k2; k<end_k; k++)
-      {
-        blockB[count+0] = rhs(k,j2+0);
-        blockB[count+1] = rhs(k,j2+1);
-        if (nr>=4)
-        {
-          blockB[count+2] = rhs(k,j2+2);
-          blockB[count+3] = rhs(k,j2+3);
+    for (Index j2 = 0; j2 < k2; j2 += nr) {
+      for (Index k = k2; k < end_k; k++) {
+        blockB[count + 0] = rhs(k, j2 + 0);
+        blockB[count + 1] = rhs(k, j2 + 1);
+        if (nr >= 4) {
+          blockB[count + 2] = rhs(k, j2 + 2);
+          blockB[count + 3] = rhs(k, j2 + 3);
         }
-        if (nr>=8)
-        {
-          blockB[count+4] = rhs(k,j2+4);
-          blockB[count+5] = rhs(k,j2+5);
-          blockB[count+6] = rhs(k,j2+6);
-          blockB[count+7] = rhs(k,j2+7);
+        if (nr >= 8) {
+          blockB[count + 4] = rhs(k, j2 + 4);
+          blockB[count + 5] = rhs(k, j2 + 5);
+          blockB[count + 6] = rhs(k, j2 + 6);
+          blockB[count + 7] = rhs(k, j2 + 7);
         }
         count += nr;
       }
     }
 
     // second part: diagonal block
-    Index end8 = nr>=8 ? (std::min)(k2+rows,packet_cols8) : k2;
-    if(nr>=8)
-    {
-      for(Index j2=k2; j2<end8; j2+=8)
-      {
+    Index end8 = nr >= 8 ? (std::min)(k2 + rows, packet_cols8) : k2;
+    if (nr >= 8) {
+      for (Index j2 = k2; j2 < end8; j2 += 8) {
         // again we can split vertically in three different parts (transpose, symmetric, normal)
         // transpose
-        for(Index k=k2; k<j2; k++)
-        {
-          blockB[count+0] = numext::conj(rhs(j2+0,k));
-          blockB[count+1] = numext::conj(rhs(j2+1,k));
-          blockB[count+2] = numext::conj(rhs(j2+2,k));
-          blockB[count+3] = numext::conj(rhs(j2+3,k));
-          blockB[count+4] = numext::conj(rhs(j2+4,k));
-          blockB[count+5] = numext::conj(rhs(j2+5,k));
-          blockB[count+6] = numext::conj(rhs(j2+6,k));
-          blockB[count+7] = numext::conj(rhs(j2+7,k));
+        for (Index k = k2; k < j2; k++) {
+          blockB[count + 0] = numext::conj(rhs(j2 + 0, k));
+          blockB[count + 1] = numext::conj(rhs(j2 + 1, k));
+          blockB[count + 2] = numext::conj(rhs(j2 + 2, k));
+          blockB[count + 3] = numext::conj(rhs(j2 + 3, k));
+          blockB[count + 4] = numext::conj(rhs(j2 + 4, k));
+          blockB[count + 5] = numext::conj(rhs(j2 + 5, k));
+          blockB[count + 6] = numext::conj(rhs(j2 + 6, k));
+          blockB[count + 7] = numext::conj(rhs(j2 + 7, k));
           count += 8;
         }
         // symmetric
         Index h = 0;
-        for(Index k=j2; k<j2+8; k++)
-        {
+        for (Index k = j2; k < j2 + 8; k++) {
           // normal
-          for (Index w=0 ; w<h; ++w)
-            blockB[count+w] = rhs(k,j2+w);
+          for (Index w = 0; w < h; ++w) blockB[count + w] = rhs(k, j2 + w);
 
-          blockB[count+h] = numext::real(rhs(k,k));
+          blockB[count + h] = numext::real(rhs(k, k));
 
           // transpose
-          for (Index w=h+1 ; w<8; ++w)
-            blockB[count+w] = numext::conj(rhs(j2+w,k));
+          for (Index w = h + 1; w < 8; ++w) blockB[count + w] = numext::conj(rhs(j2 + w, k));
           count += 8;
           ++h;
         }
         // normal
-        for(Index k=j2+8; k<end_k; k++)
-        {
-          blockB[count+0] = rhs(k,j2+0);
-          blockB[count+1] = rhs(k,j2+1);
-          blockB[count+2] = rhs(k,j2+2);
-          blockB[count+3] = rhs(k,j2+3);
-          blockB[count+4] = rhs(k,j2+4);
-          blockB[count+5] = rhs(k,j2+5);
-          blockB[count+6] = rhs(k,j2+6);
-          blockB[count+7] = rhs(k,j2+7);
+        for (Index k = j2 + 8; k < end_k; k++) {
+          blockB[count + 0] = rhs(k, j2 + 0);
+          blockB[count + 1] = rhs(k, j2 + 1);
+          blockB[count + 2] = rhs(k, j2 + 2);
+          blockB[count + 3] = rhs(k, j2 + 3);
+          blockB[count + 4] = rhs(k, j2 + 4);
+          blockB[count + 5] = rhs(k, j2 + 5);
+          blockB[count + 6] = rhs(k, j2 + 6);
+          blockB[count + 7] = rhs(k, j2 + 7);
           count += 8;
         }
       }
     }
-    if(nr>=4)
-    {
-      for(Index j2=end8; j2<(std::min)(k2+rows,packet_cols4); j2+=4)
-      {
+    if (nr >= 4) {
+      for (Index j2 = end8; j2 < (std::min)(k2 + rows, packet_cols4); j2 += 4) {
         // again we can split vertically in three different parts (transpose, symmetric, normal)
         // transpose
-        for(Index k=k2; k<j2; k++)
-        {
-          blockB[count+0] = numext::conj(rhs(j2+0,k));
-          blockB[count+1] = numext::conj(rhs(j2+1,k));
-          blockB[count+2] = numext::conj(rhs(j2+2,k));
-          blockB[count+3] = numext::conj(rhs(j2+3,k));
+        for (Index k = k2; k < j2; k++) {
+          blockB[count + 0] = numext::conj(rhs(j2 + 0, k));
+          blockB[count + 1] = numext::conj(rhs(j2 + 1, k));
+          blockB[count + 2] = numext::conj(rhs(j2 + 2, k));
+          blockB[count + 3] = numext::conj(rhs(j2 + 3, k));
           count += 4;
         }
         // symmetric
         Index h = 0;
-        for(Index k=j2; k<j2+4; k++)
-        {
+        for (Index k = j2; k < j2 + 4; k++) {
           // normal
-          for (Index w=0 ; w<h; ++w)
-            blockB[count+w] = rhs(k,j2+w);
+          for (Index w = 0; w < h; ++w) blockB[count + w] = rhs(k, j2 + w);
 
-          blockB[count+h] = numext::real(rhs(k,k));
+          blockB[count + h] = numext::real(rhs(k, k));
 
           // transpose
-          for (Index w=h+1 ; w<4; ++w)
-            blockB[count+w] = numext::conj(rhs(j2+w,k));
+          for (Index w = h + 1; w < 4; ++w) blockB[count + w] = numext::conj(rhs(j2 + w, k));
           count += 4;
           ++h;
         }
         // normal
-        for(Index k=j2+4; k<end_k; k++)
-        {
-          blockB[count+0] = rhs(k,j2+0);
-          blockB[count+1] = rhs(k,j2+1);
-          blockB[count+2] = rhs(k,j2+2);
-          blockB[count+3] = rhs(k,j2+3);
+        for (Index k = j2 + 4; k < end_k; k++) {
+          blockB[count + 0] = rhs(k, j2 + 0);
+          blockB[count + 1] = rhs(k, j2 + 1);
+          blockB[count + 2] = rhs(k, j2 + 2);
+          blockB[count + 3] = rhs(k, j2 + 3);
           count += 4;
         }
       }
     }
 
     // third part: transposed
-    if(nr>=8)
-    {
-      for(Index j2=k2+rows; j2<packet_cols8; j2+=8)
-      {
-        for(Index k=k2; k<end_k; k++)
-        {
-          blockB[count+0] = numext::conj(rhs(j2+0,k));
-          blockB[count+1] = numext::conj(rhs(j2+1,k));
-          blockB[count+2] = numext::conj(rhs(j2+2,k));
-          blockB[count+3] = numext::conj(rhs(j2+3,k));
-          blockB[count+4] = numext::conj(rhs(j2+4,k));
-          blockB[count+5] = numext::conj(rhs(j2+5,k));
-          blockB[count+6] = numext::conj(rhs(j2+6,k));
-          blockB[count+7] = numext::conj(rhs(j2+7,k));
+    if (nr >= 8) {
+      for (Index j2 = k2 + rows; j2 < packet_cols8; j2 += 8) {
+        for (Index k = k2; k < end_k; k++) {
+          blockB[count + 0] = numext::conj(rhs(j2 + 0, k));
+          blockB[count + 1] = numext::conj(rhs(j2 + 1, k));
+          blockB[count + 2] = numext::conj(rhs(j2 + 2, k));
+          blockB[count + 3] = numext::conj(rhs(j2 + 3, k));
+          blockB[count + 4] = numext::conj(rhs(j2 + 4, k));
+          blockB[count + 5] = numext::conj(rhs(j2 + 5, k));
+          blockB[count + 6] = numext::conj(rhs(j2 + 6, k));
+          blockB[count + 7] = numext::conj(rhs(j2 + 7, k));
           count += 8;
         }
       }
     }
-    if(nr>=4)
-    {
-      for(Index j2=(std::max)(packet_cols8,k2+rows); j2<packet_cols4; j2+=4)
-      {
-        for(Index k=k2; k<end_k; k++)
-        {
-          blockB[count+0] = numext::conj(rhs(j2+0,k));
-          blockB[count+1] = numext::conj(rhs(j2+1,k));
-          blockB[count+2] = numext::conj(rhs(j2+2,k));
-          blockB[count+3] = numext::conj(rhs(j2+3,k));
+    if (nr >= 4) {
+      for (Index j2 = (std::max)(packet_cols8, k2 + rows); j2 < packet_cols4; j2 += 4) {
+        for (Index k = k2; k < end_k; k++) {
+          blockB[count + 0] = numext::conj(rhs(j2 + 0, k));
+          blockB[count + 1] = numext::conj(rhs(j2 + 1, k));
+          blockB[count + 2] = numext::conj(rhs(j2 + 2, k));
+          blockB[count + 3] = numext::conj(rhs(j2 + 3, k));
           count += 4;
         }
       }
     }
 
     // copy the remaining columns one at a time (=> the same with nr==1)
-    for(Index j2=packet_cols4; j2<cols; ++j2)
-    {
+    for (Index j2 = packet_cols4; j2 < cols; ++j2) {
       // transpose
-      Index half = (std::min)(end_k,j2);
-      for(Index k=k2; k<half; k++)
-      {
-        blockB[count] = numext::conj(rhs(j2,k));
+      Index half = (std::min)(end_k, j2);
+      for (Index k = k2; k < half; k++) {
+        blockB[count] = numext::conj(rhs(j2, k));
         count += 1;
       }
 
-      if(half==j2 && half<k2+rows)
-      {
-        blockB[count] = numext::real(rhs(j2,j2));
+      if (half == j2 && half < k2 + rows) {
+        blockB[count] = numext::real(rhs(j2, j2));
         count += 1;
-      }
-      else
+      } else
         half--;
 
       // normal
-      for(Index k=half+1; k<k2+rows; k++)
-      {
-        blockB[count] = rhs(k,j2);
+      for (Index k = half + 1; k < k2 + rows; k++) {
+        blockB[count] = rhs(k, j2);
         count += 1;
       }
     }
@@ -291,254 +259,225 @@
 /* Optimized selfadjoint matrix * matrix (_SYMM) product built on top of
  * the general matrix matrix product.
  */
-template <typename Scalar, typename Index,
-          int LhsStorageOrder, bool LhsSelfAdjoint, bool ConjugateLhs,
-          int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs,
-          int ResStorageOrder, int ResInnerStride>
+template <typename Scalar, typename Index, int LhsStorageOrder, bool LhsSelfAdjoint, bool ConjugateLhs,
+          int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs, int ResStorageOrder, int ResInnerStride>
 struct product_selfadjoint_matrix;
 
-template <typename Scalar, typename Index,
-          int LhsStorageOrder, bool LhsSelfAdjoint, bool ConjugateLhs,
-          int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs,
-          int ResInnerStride>
-struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,LhsSelfAdjoint,ConjugateLhs, RhsStorageOrder,RhsSelfAdjoint,ConjugateRhs,RowMajor,ResInnerStride>
-{
-
-  static EIGEN_STRONG_INLINE void run(
-    Index rows, Index cols,
-    const Scalar* lhs, Index lhsStride,
-    const Scalar* rhs, Index rhsStride,
-    Scalar* res,       Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
-  {
-    product_selfadjoint_matrix<Scalar, Index,
-      EIGEN_LOGICAL_XOR(RhsSelfAdjoint,RhsStorageOrder==RowMajor) ? ColMajor : RowMajor,
-      RhsSelfAdjoint, NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(RhsSelfAdjoint,ConjugateRhs),
-      EIGEN_LOGICAL_XOR(LhsSelfAdjoint,LhsStorageOrder==RowMajor) ? ColMajor : RowMajor,
-      LhsSelfAdjoint, NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(LhsSelfAdjoint,ConjugateLhs),
-      ColMajor,ResInnerStride>
-      ::run(cols, rows,  rhs, rhsStride,  lhs, lhsStride,  res, resIncr, resStride,  alpha, blocking);
+template <typename Scalar, typename Index, int LhsStorageOrder, bool LhsSelfAdjoint, bool ConjugateLhs,
+          int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs, int ResInnerStride>
+struct product_selfadjoint_matrix<Scalar, Index, LhsStorageOrder, LhsSelfAdjoint, ConjugateLhs, RhsStorageOrder,
+                                  RhsSelfAdjoint, ConjugateRhs, RowMajor, ResInnerStride> {
+  static EIGEN_STRONG_INLINE void run(Index rows, Index cols, const Scalar* lhs, Index lhsStride, const Scalar* rhs,
+                                      Index rhsStride, Scalar* res, Index resIncr, Index resStride, const Scalar& alpha,
+                                      level3_blocking<Scalar, Scalar>& blocking) {
+    product_selfadjoint_matrix<
+        Scalar, Index, logical_xor(RhsSelfAdjoint, RhsStorageOrder == RowMajor) ? ColMajor : RowMajor, RhsSelfAdjoint,
+        NumTraits<Scalar>::IsComplex && logical_xor(RhsSelfAdjoint, ConjugateRhs),
+        logical_xor(LhsSelfAdjoint, LhsStorageOrder == RowMajor) ? ColMajor : RowMajor, LhsSelfAdjoint,
+        NumTraits<Scalar>::IsComplex && logical_xor(LhsSelfAdjoint, ConjugateLhs), ColMajor,
+        ResInnerStride>::run(cols, rows, rhs, rhsStride, lhs, lhsStride, res, resIncr, resStride, alpha, blocking);
   }
 };
 
-template <typename Scalar, typename Index,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride>
-struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,true,ConjugateLhs, RhsStorageOrder,false,ConjugateRhs,ColMajor,ResInnerStride>
-{
-
-  static EIGEN_DONT_INLINE void run(
-    Index rows, Index cols,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
+template <typename Scalar, typename Index, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride>
+struct product_selfadjoint_matrix<Scalar, Index, LhsStorageOrder, true, ConjugateLhs, RhsStorageOrder, false,
+                                  ConjugateRhs, ColMajor, ResInnerStride> {
+  static EIGEN_DONT_INLINE void run(Index rows, Index cols, const Scalar* lhs_, Index lhsStride, const Scalar* rhs_,
+                                    Index rhsStride, Scalar* res, Index resIncr, Index resStride, const Scalar& alpha,
+                                    level3_blocking<Scalar, Scalar>& blocking);
 };
 
-template <typename Scalar, typename Index,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride>
-EIGEN_DONT_INLINE void product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,true,ConjugateLhs, RhsStorageOrder,false,ConjugateRhs,ColMajor,ResInnerStride>::run(
-    Index rows, Index cols,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,       Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
-  {
-    Index size = rows;
+template <typename Scalar, typename Index, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride>
+EIGEN_DONT_INLINE void
+product_selfadjoint_matrix<Scalar, Index, LhsStorageOrder, true, ConjugateLhs, RhsStorageOrder, false, ConjugateRhs,
+                           ColMajor, ResInnerStride>::run(Index rows, Index cols, const Scalar* lhs_, Index lhsStride,
+                                                          const Scalar* rhs_, Index rhsStride, Scalar* res_,
+                                                          Index resIncr, Index resStride, const Scalar& alpha,
+                                                          level3_blocking<Scalar, Scalar>& blocking) {
+  Index size = rows;
 
-    typedef gebp_traits<Scalar,Scalar> Traits;
+  typedef gebp_traits<Scalar, Scalar> Traits;
 
-    typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
-    typedef const_blas_data_mapper<Scalar, Index, (LhsStorageOrder == RowMajor) ? ColMajor : RowMajor> LhsTransposeMapper;
-    typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
-    LhsMapper lhs(_lhs,lhsStride);
-    LhsTransposeMapper lhs_transpose(_lhs,lhsStride);
-    RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride, resIncr);
+  typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
+  typedef const_blas_data_mapper<Scalar, Index, (LhsStorageOrder == RowMajor) ? ColMajor : RowMajor> LhsTransposeMapper;
+  typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
+  typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
+  LhsMapper lhs(lhs_, lhsStride);
+  LhsTransposeMapper lhs_transpose(lhs_, lhsStride);
+  RhsMapper rhs(rhs_, rhsStride);
+  ResMapper res(res_, resStride, resIncr);
 
-    Index kc = blocking.kc();                   // cache block size along the K direction
-    Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
-    // kc must be smaller than mc
-    kc = (std::min)(kc,mc);
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*cols;
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+  Index kc = blocking.kc();                    // cache block size along the K direction
+  Index mc = (std::min)(rows, blocking.mc());  // cache block size along the M direction
+  // kc must be smaller than mc
+  kc = (std::min)(kc, mc);
+  std::size_t sizeA = kc * mc;
+  std::size_t sizeB = kc * cols;
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
-    gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
-    symm_pack_lhs<Scalar, Index, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
-    gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder> pack_rhs;
-    gemm_pack_lhs<Scalar, Index, LhsTransposeMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder==RowMajor?ColMajor:RowMajor, true> pack_lhs_transposed;
+  gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
+  symm_pack_lhs<Scalar, Index, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
+  gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
+  gemm_pack_lhs<Scalar, Index, LhsTransposeMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                LhsStorageOrder == RowMajor ? ColMajor : RowMajor, true>
+      pack_lhs_transposed;
 
-    for(Index k2=0; k2<size; k2+=kc)
+  for (Index k2 = 0; k2 < size; k2 += kc) {
+    const Index actual_kc = (std::min)(k2 + kc, size) - k2;
+
+    // we have selected one row panel of rhs and one column panel of lhs
+    // pack rhs's panel into a sequential chunk of memory
+    // and expand each coeff to a constant packet for further reuse
+    pack_rhs(blockB, rhs.getSubMapper(k2, 0), actual_kc, cols);
+
+    // the select lhs's panel has to be split in three different parts:
+    //  1 - the transposed panel above the diagonal block => transposed packed copy
+    //  2 - the diagonal block => special packed copy
+    //  3 - the panel below the diagonal block => generic packed copy
+    for (Index i2 = 0; i2 < k2; i2 += mc) {
+      const Index actual_mc = (std::min)(i2 + mc, k2) - i2;
+      // transposed packed copy
+      pack_lhs_transposed(blockA, lhs_transpose.getSubMapper(i2, k2), actual_kc, actual_mc);
+
+      gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
+    }
+    // the block diagonal
     {
-      const Index actual_kc = (std::min)(k2+kc,size)-k2;
+      const Index actual_mc = (std::min)(k2 + kc, size) - k2;
+      // symmetric packed copy
+      pack_lhs(blockA, &lhs(k2, k2), lhsStride, actual_kc, actual_mc);
 
-      // we have selected one row panel of rhs and one column panel of lhs
-      // pack rhs's panel into a sequential chunk of memory
-      // and expand each coeff to a constant packet for further reuse
-      pack_rhs(blockB, rhs.getSubMapper(k2,0), actual_kc, cols);
+      gebp_kernel(res.getSubMapper(k2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
+    }
 
-      // the select lhs's panel has to be split in three different parts:
-      //  1 - the transposed panel above the diagonal block => transposed packed copy
-      //  2 - the diagonal block => special packed copy
-      //  3 - the panel below the diagonal block => generic packed copy
-      for(Index i2=0; i2<k2; i2+=mc)
-      {
-        const Index actual_mc = (std::min)(i2+mc,k2)-i2;
-        // transposed packed copy
-        pack_lhs_transposed(blockA, lhs_transpose.getSubMapper(i2, k2), actual_kc, actual_mc);
+    for (Index i2 = k2 + kc; i2 < size; i2 += mc) {
+      const Index actual_mc = (std::min)(i2 + mc, size) - i2;
+      gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                    LhsStorageOrder, false>()(blockA, lhs.getSubMapper(i2, k2), actual_kc, actual_mc);
 
-        gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
-      }
-      // the block diagonal
-      {
-        const Index actual_mc = (std::min)(k2+kc,size)-k2;
-        // symmetric packed copy
-        pack_lhs(blockA, &lhs(k2,k2), lhsStride, actual_kc, actual_mc);
-
-        gebp_kernel(res.getSubMapper(k2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
-      }
-
-      for(Index i2=k2+kc; i2<size; i2+=mc)
-      {
-        const Index actual_mc = (std::min)(i2+mc,size)-i2;
-        gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder,false>()
-          (blockA, lhs.getSubMapper(i2, k2), actual_kc, actual_mc);
-
-        gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
-      }
+      gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
     }
   }
+}
 
 // matrix * selfadjoint product
-template <typename Scalar, typename Index,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride>
-struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,false,ConjugateLhs, RhsStorageOrder,true,ConjugateRhs,ColMajor,ResInnerStride>
-{
-
-  static EIGEN_DONT_INLINE void run(
-    Index rows, Index cols,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
+template <typename Scalar, typename Index, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride>
+struct product_selfadjoint_matrix<Scalar, Index, LhsStorageOrder, false, ConjugateLhs, RhsStorageOrder, true,
+                                  ConjugateRhs, ColMajor, ResInnerStride> {
+  static EIGEN_DONT_INLINE void run(Index rows, Index cols, const Scalar* lhs_, Index lhsStride, const Scalar* rhs_,
+                                    Index rhsStride, Scalar* res, Index resIncr, Index resStride, const Scalar& alpha,
+                                    level3_blocking<Scalar, Scalar>& blocking);
 };
 
-template <typename Scalar, typename Index,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride>
-EIGEN_DONT_INLINE void product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,false,ConjugateLhs, RhsStorageOrder,true,ConjugateRhs,ColMajor,ResInnerStride>::run(
-    Index rows, Index cols,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,       Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
-  {
-    Index size = cols;
+template <typename Scalar, typename Index, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride>
+EIGEN_DONT_INLINE void
+product_selfadjoint_matrix<Scalar, Index, LhsStorageOrder, false, ConjugateLhs, RhsStorageOrder, true, ConjugateRhs,
+                           ColMajor, ResInnerStride>::run(Index rows, Index cols, const Scalar* lhs_, Index lhsStride,
+                                                          const Scalar* rhs_, Index rhsStride, Scalar* res_,
+                                                          Index resIncr, Index resStride, const Scalar& alpha,
+                                                          level3_blocking<Scalar, Scalar>& blocking) {
+  Index size = cols;
 
-    typedef gebp_traits<Scalar,Scalar> Traits;
+  typedef gebp_traits<Scalar, Scalar> Traits;
 
-    typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
-    LhsMapper lhs(_lhs,lhsStride);
-    ResMapper res(_res,resStride, resIncr);
+  typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
+  typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
+  LhsMapper lhs(lhs_, lhsStride);
+  ResMapper res(res_, resStride, resIncr);
 
-    Index kc = blocking.kc();                   // cache block size along the K direction
-    Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*cols;
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+  Index kc = blocking.kc();                    // cache block size along the K direction
+  Index mc = (std::min)(rows, blocking.mc());  // cache block size along the M direction
+  std::size_t sizeA = kc * mc;
+  std::size_t sizeB = kc * cols;
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
-    gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
-    symm_pack_rhs<Scalar, Index, Traits::nr,RhsStorageOrder> pack_rhs;
+  gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
+  gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                LhsStorageOrder>
+      pack_lhs;
+  symm_pack_rhs<Scalar, Index, Traits::nr, RhsStorageOrder> pack_rhs;
 
-    for(Index k2=0; k2<size; k2+=kc)
-    {
-      const Index actual_kc = (std::min)(k2+kc,size)-k2;
+  for (Index k2 = 0; k2 < size; k2 += kc) {
+    const Index actual_kc = (std::min)(k2 + kc, size) - k2;
 
-      pack_rhs(blockB, _rhs, rhsStride, actual_kc, cols, k2);
+    pack_rhs(blockB, rhs_, rhsStride, actual_kc, cols, k2);
 
-      // => GEPP
-      for(Index i2=0; i2<rows; i2+=mc)
-      {
-        const Index actual_mc = (std::min)(i2+mc,rows)-i2;
-        pack_lhs(blockA, lhs.getSubMapper(i2, k2), actual_kc, actual_mc);
+    // => GEPP
+    for (Index i2 = 0; i2 < rows; i2 += mc) {
+      const Index actual_mc = (std::min)(i2 + mc, rows) - i2;
+      pack_lhs(blockA, lhs.getSubMapper(i2, k2), actual_kc, actual_mc);
 
-        gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
-      }
+      gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
     }
   }
+}
 
-} // end namespace internal
+}  // end namespace internal
 
 /***************************************************************************
-* Wrapper to product_selfadjoint_matrix
-***************************************************************************/
+ * Wrapper to product_selfadjoint_matrix
+ ***************************************************************************/
 
 namespace internal {
-  
-template<typename Lhs, int LhsMode, typename Rhs, int RhsMode>
-struct selfadjoint_product_impl<Lhs,LhsMode,false,Rhs,RhsMode,false>
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
+template <typename Lhs, int LhsMode, typename Rhs, int RhsMode>
+struct selfadjoint_product_impl<Lhs, LhsMode, false, Rhs, RhsMode, false> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
+
   typedef internal::blas_traits<Lhs> LhsBlasTraits;
   typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
   typedef internal::blas_traits<Rhs> RhsBlasTraits;
   typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-  
+
   enum {
-    LhsIsUpper = (LhsMode&(Upper|Lower))==Upper,
-    LhsIsSelfAdjoint = (LhsMode&SelfAdjoint)==SelfAdjoint,
-    RhsIsUpper = (RhsMode&(Upper|Lower))==Upper,
-    RhsIsSelfAdjoint = (RhsMode&SelfAdjoint)==SelfAdjoint
+    LhsIsUpper = (LhsMode & (Upper | Lower)) == Upper,
+    LhsIsSelfAdjoint = (LhsMode & SelfAdjoint) == SelfAdjoint,
+    RhsIsUpper = (RhsMode & (Upper | Lower)) == Upper,
+    RhsIsSelfAdjoint = (RhsMode & SelfAdjoint) == SelfAdjoint
   };
-  
-  template<typename Dest>
-  static void run(Dest &dst, const Lhs &a_lhs, const Rhs &a_rhs, const Scalar& alpha)
-  {
-    eigen_assert(dst.rows()==a_lhs.rows() && dst.cols()==a_rhs.cols());
 
-    typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(a_lhs);
-    typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(a_rhs);
+  template <typename Dest>
+  static void run(Dest& dst, const Lhs& a_lhs, const Rhs& a_rhs, const Scalar& alpha) {
+    eigen_assert(dst.rows() == a_lhs.rows() && dst.cols() == a_rhs.cols());
 
-    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(a_lhs)
-                               * RhsBlasTraits::extractScalarFactor(a_rhs);
+    add_const_on_value_type_t<ActualLhsType> lhs = LhsBlasTraits::extract(a_lhs);
+    add_const_on_value_type_t<ActualRhsType> rhs = RhsBlasTraits::extract(a_rhs);
 
-    typedef internal::gemm_blocking_space<(Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,Scalar,Scalar,
-              Lhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime, Lhs::MaxColsAtCompileTime,1> BlockingType;
+    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(a_lhs) * RhsBlasTraits::extractScalarFactor(a_rhs);
+
+    typedef internal::gemm_blocking_space<(Dest::Flags & RowMajorBit) ? RowMajor : ColMajor, Scalar, Scalar,
+                                          Lhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime,
+                                          Lhs::MaxColsAtCompileTime, 1>
+        BlockingType;
 
     BlockingType blocking(lhs.rows(), rhs.cols(), lhs.cols(), 1, false);
 
-    internal::product_selfadjoint_matrix<Scalar, Index,
-      EIGEN_LOGICAL_XOR(LhsIsUpper,internal::traits<Lhs>::Flags &RowMajorBit) ? RowMajor : ColMajor, LhsIsSelfAdjoint,
-      NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(LhsIsUpper,bool(LhsBlasTraits::NeedToConjugate)),
-      EIGEN_LOGICAL_XOR(RhsIsUpper,internal::traits<Rhs>::Flags &RowMajorBit) ? RowMajor : ColMajor, RhsIsSelfAdjoint,
-      NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(RhsIsUpper,bool(RhsBlasTraits::NeedToConjugate)),
-      internal::traits<Dest>::Flags&RowMajorBit  ? RowMajor : ColMajor,
-      Dest::InnerStrideAtCompileTime>
-      ::run(
-        lhs.rows(), rhs.cols(),                 // sizes
-        &lhs.coeffRef(0,0), lhs.outerStride(),  // lhs info
-        &rhs.coeffRef(0,0), rhs.outerStride(),  // rhs info
-        &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(),  // result info
-        actualAlpha, blocking                   // alpha
-      );
+    internal::product_selfadjoint_matrix<
+        Scalar, Index,
+        internal::logical_xor(LhsIsUpper, internal::traits<Lhs>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+        LhsIsSelfAdjoint,
+        NumTraits<Scalar>::IsComplex && internal::logical_xor(LhsIsUpper, bool(LhsBlasTraits::NeedToConjugate)),
+        internal::logical_xor(RhsIsUpper, internal::traits<Rhs>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+        RhsIsSelfAdjoint,
+        NumTraits<Scalar>::IsComplex && internal::logical_xor(RhsIsUpper, bool(RhsBlasTraits::NeedToConjugate)),
+        internal::traits<Dest>::Flags & RowMajorBit ? RowMajor : ColMajor,
+        Dest::InnerStrideAtCompileTime>::run(lhs.rows(), rhs.cols(),                                     // sizes
+                                             &lhs.coeffRef(0, 0), lhs.outerStride(),                     // lhs info
+                                             &rhs.coeffRef(0, 0), rhs.outerStride(),                     // rhs info
+                                             &dst.coeffRef(0, 0), dst.innerStride(), dst.outerStride(),  // result info
+                                             actualAlpha, blocking                                       // alpha
+    );
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELFADJOINT_MATRIX_MATRIX_H
+#endif  // EIGEN_SELFADJOINT_MATRIX_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h
index d38fd72..9333d16 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h
@@ -10,7 +10,10 @@
 #ifndef EIGEN_SELFADJOINT_MATRIX_VECTOR_H
 #define EIGEN_SELFADJOINT_MATRIX_VECTOR_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
@@ -20,63 +23,54 @@
  * the instruction dependency.
  */
 
-template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version=Specialized>
+template <typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs,
+          int Version = Specialized>
 struct selfadjoint_matrix_vector_product;
 
-template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version>
+template <typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs,
+          int Version>
 struct selfadjoint_matrix_vector_product
 
 {
-static EIGEN_DONT_INLINE EIGEN_DEVICE_FUNC
-void run(
-  Index size,
-  const Scalar*  lhs, Index lhsStride,
-  const Scalar*  rhs,
-  Scalar* res,
-  Scalar alpha);
+  static EIGEN_DONT_INLINE EIGEN_DEVICE_FUNC void run(Index size, const Scalar* lhs, Index lhsStride, const Scalar* rhs,
+                                                      Scalar* res, Scalar alpha);
 };
 
-template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version>
-EIGEN_DONT_INLINE EIGEN_DEVICE_FUNC
-void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrder,UpLo,ConjugateLhs,ConjugateRhs,Version>::run(
-  Index size,
-  const Scalar*  lhs, Index lhsStride,
-  const Scalar*  rhs,
-  Scalar* res,
-  Scalar alpha)
-{
+template <typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs,
+          int Version>
+EIGEN_DONT_INLINE EIGEN_DEVICE_FUNC void
+selfadjoint_matrix_vector_product<Scalar, Index, StorageOrder, UpLo, ConjugateLhs, ConjugateRhs, Version>::run(
+    Index size, const Scalar* lhs, Index lhsStride, const Scalar* rhs, Scalar* res, Scalar alpha) {
   typedef typename packet_traits<Scalar>::type Packet;
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  const Index PacketSize = sizeof(Packet)/sizeof(Scalar);
+  const Index PacketSize = sizeof(Packet) / sizeof(Scalar);
 
   enum {
-    IsRowMajor = StorageOrder==RowMajor ? 1 : 0,
+    IsRowMajor = StorageOrder == RowMajor ? 1 : 0,
     IsLower = UpLo == Lower ? 1 : 0,
     FirstTriangular = IsRowMajor == IsLower
   };
 
-  conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs,  IsRowMajor), ConjugateRhs> cj0;
-  conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> cj1;
-  conj_helper<RealScalar,Scalar,false, ConjugateRhs> cjd;
+  conj_helper<Scalar, Scalar, NumTraits<Scalar>::IsComplex && logical_xor(ConjugateLhs, IsRowMajor), ConjugateRhs> cj0;
+  conj_helper<Scalar, Scalar, NumTraits<Scalar>::IsComplex && logical_xor(ConjugateLhs, !IsRowMajor), ConjugateRhs> cj1;
+  conj_helper<RealScalar, Scalar, false, ConjugateRhs> cjd;
 
-  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs,  IsRowMajor), ConjugateRhs> pcj0;
-  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> pcj1;
+  conj_helper<Packet, Packet, NumTraits<Scalar>::IsComplex && logical_xor(ConjugateLhs, IsRowMajor), ConjugateRhs> pcj0;
+  conj_helper<Packet, Packet, NumTraits<Scalar>::IsComplex && logical_xor(ConjugateLhs, !IsRowMajor), ConjugateRhs>
+      pcj1;
 
   Scalar cjAlpha = ConjugateRhs ? numext::conj(alpha) : alpha;
 
-  Index bound = numext::maxi(Index(0), size-8) & 0xfffffffe;
-  if (FirstTriangular)
-    bound = size - bound;
+  Index bound = numext::maxi(Index(0), size - 8) & 0xfffffffe;
+  if (FirstTriangular) bound = size - bound;
 
-  for (Index j=FirstTriangular ? bound : 0;
-       j<(FirstTriangular ? size : bound);j+=2)
-  {
-    const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
-    const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride;
+  for (Index j = FirstTriangular ? bound : 0; j < (FirstTriangular ? size : bound); j += 2) {
+    const Scalar* EIGEN_RESTRICT A0 = lhs + j * lhsStride;
+    const Scalar* EIGEN_RESTRICT A1 = lhs + (j + 1) * lhsStride;
 
     Scalar t0 = cjAlpha * rhs[j];
     Packet ptmp0 = pset1<Packet>(t0);
-    Scalar t1 = cjAlpha * rhs[j+1];
+    Scalar t1 = cjAlpha * rhs[j + 1];
     Packet ptmp1 = pset1<Packet>(t1);
 
     Scalar t2(0);
@@ -84,67 +78,63 @@
     Scalar t3(0);
     Packet ptmp3 = pset1<Packet>(t3);
 
-    Index starti = FirstTriangular ? 0 : j+2;
-    Index endi   = FirstTriangular ? j : size;
-    Index alignedStart = (starti) + internal::first_default_aligned(&res[starti], endi-starti);
-    Index alignedEnd = alignedStart + ((endi-alignedStart)/(PacketSize))*(PacketSize);
+    Index starti = FirstTriangular ? 0 : j + 2;
+    Index endi = FirstTriangular ? j : size;
+    Index alignedStart = (starti) + internal::first_default_aligned(&res[starti], endi - starti);
+    Index alignedEnd = alignedStart + ((endi - alignedStart) / (PacketSize)) * (PacketSize);
 
-    res[j]   += cjd.pmul(numext::real(A0[j]), t0);
-    res[j+1] += cjd.pmul(numext::real(A1[j+1]), t1);
-    if(FirstTriangular)
-    {
-      res[j]   += cj0.pmul(A1[j],   t1);
-      t3       += cj1.pmul(A1[j],   rhs[j]);
-    }
-    else
-    {
-      res[j+1] += cj0.pmul(A0[j+1],t0);
-      t2 += cj1.pmul(A0[j+1], rhs[j+1]);
+    res[j] += cjd.pmul(numext::real(A0[j]), t0);
+    res[j + 1] += cjd.pmul(numext::real(A1[j + 1]), t1);
+    if (FirstTriangular) {
+      res[j] += cj0.pmul(A1[j], t1);
+      t3 += cj1.pmul(A1[j], rhs[j]);
+    } else {
+      res[j + 1] += cj0.pmul(A0[j + 1], t0);
+      t2 += cj1.pmul(A0[j + 1], rhs[j + 1]);
     }
 
-    for (Index i=starti; i<alignedStart; ++i)
-    {
-      res[i] += cj0.pmul(A0[i], t0) + cj0.pmul(A1[i],t1);
+    for (Index i = starti; i < alignedStart; ++i) {
+      res[i] += cj0.pmul(A0[i], t0) + cj0.pmul(A1[i], t1);
       t2 += cj1.pmul(A0[i], rhs[i]);
       t3 += cj1.pmul(A1[i], rhs[i]);
     }
     // Yes this an optimization for gcc 4.3 and 4.4 (=> huge speed up)
     // gcc 4.2 does this optimization automatically.
-    const Scalar* EIGEN_RESTRICT a0It  = A0  + alignedStart;
-    const Scalar* EIGEN_RESTRICT a1It  = A1  + alignedStart;
+    const Scalar* EIGEN_RESTRICT a0It = A0 + alignedStart;
+    const Scalar* EIGEN_RESTRICT a1It = A1 + alignedStart;
     const Scalar* EIGEN_RESTRICT rhsIt = rhs + alignedStart;
-          Scalar* EIGEN_RESTRICT resIt = res + alignedStart;
-    for (Index i=alignedStart; i<alignedEnd; i+=PacketSize)
-    {
-      Packet A0i = ploadu<Packet>(a0It);  a0It  += PacketSize;
-      Packet A1i = ploadu<Packet>(a1It);  a1It  += PacketSize;
-      Packet Bi  = ploadu<Packet>(rhsIt); rhsIt += PacketSize; // FIXME should be aligned in most cases
-      Packet Xi  = pload <Packet>(resIt);
+    Scalar* EIGEN_RESTRICT resIt = res + alignedStart;
+    for (Index i = alignedStart; i < alignedEnd; i += PacketSize) {
+      Packet A0i = ploadu<Packet>(a0It);
+      a0It += PacketSize;
+      Packet A1i = ploadu<Packet>(a1It);
+      a1It += PacketSize;
+      Packet Bi = ploadu<Packet>(rhsIt);
+      rhsIt += PacketSize;  // FIXME should be aligned in most cases
+      Packet Xi = pload<Packet>(resIt);
 
-      Xi    = pcj0.pmadd(A0i,ptmp0, pcj0.pmadd(A1i,ptmp1,Xi));
-      ptmp2 = pcj1.pmadd(A0i,  Bi, ptmp2);
-      ptmp3 = pcj1.pmadd(A1i,  Bi, ptmp3);
-      pstore(resIt,Xi); resIt += PacketSize;
+      Xi = pcj0.pmadd(A0i, ptmp0, pcj0.pmadd(A1i, ptmp1, Xi));
+      ptmp2 = pcj1.pmadd(A0i, Bi, ptmp2);
+      ptmp3 = pcj1.pmadd(A1i, Bi, ptmp3);
+      pstore(resIt, Xi);
+      resIt += PacketSize;
     }
-    for (Index i=alignedEnd; i<endi; i++)
-    {
-      res[i] += cj0.pmul(A0[i], t0) + cj0.pmul(A1[i],t1);
+    for (Index i = alignedEnd; i < endi; i++) {
+      res[i] += cj0.pmul(A0[i], t0) + cj0.pmul(A1[i], t1);
       t2 += cj1.pmul(A0[i], rhs[i]);
       t3 += cj1.pmul(A1[i], rhs[i]);
     }
 
-    res[j]   += alpha * (t2 + predux(ptmp2));
-    res[j+1] += alpha * (t3 + predux(ptmp3));
+    res[j] += alpha * (t2 + predux(ptmp2));
+    res[j + 1] += alpha * (t3 + predux(ptmp3));
   }
-  for (Index j=FirstTriangular ? 0 : bound;j<(FirstTriangular ? bound : size);j++)
-  {
-    const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
+  for (Index j = FirstTriangular ? 0 : bound; j < (FirstTriangular ? bound : size); j++) {
+    const Scalar* EIGEN_RESTRICT A0 = lhs + j * lhsStride;
 
     Scalar t1 = cjAlpha * rhs[j];
     Scalar t2(0);
     res[j] += cjd.pmul(numext::real(A0[j]), t1);
-    for (Index i=FirstTriangular ? 0 : j+1; i<(FirstTriangular ? j : size); i++)
-    {
+    for (Index i = FirstTriangular ? 0 : j + 1; i < (FirstTriangular ? j : size); i++) {
       res[i] += cj0.pmul(A0[i], t1);
       t2 += cj1.pmul(A0[i], rhs[i]);
     }
@@ -152,111 +142,105 @@
   }
 }
 
-} // end namespace internal 
+}  // end namespace internal
 
 /***************************************************************************
-* Wrapper to product_selfadjoint_vector
-***************************************************************************/
+ * Wrapper to product_selfadjoint_vector
+ ***************************************************************************/
 
 namespace internal {
 
-template<typename Lhs, int LhsMode, typename Rhs>
-struct selfadjoint_product_impl<Lhs,LhsMode,false,Rhs,0,true>
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+template <typename Lhs, int LhsMode, typename Rhs>
+struct selfadjoint_product_impl<Lhs, LhsMode, false, Rhs, 0, true> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
+
   typedef internal::blas_traits<Lhs> LhsBlasTraits;
   typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
-  typedef typename internal::remove_all<ActualLhsType>::type ActualLhsTypeCleaned;
-  
+  typedef internal::remove_all_t<ActualLhsType> ActualLhsTypeCleaned;
+
   typedef internal::blas_traits<Rhs> RhsBlasTraits;
   typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-  typedef typename internal::remove_all<ActualRhsType>::type ActualRhsTypeCleaned;
+  typedef internal::remove_all_t<ActualRhsType> ActualRhsTypeCleaned;
 
-  enum { LhsUpLo = LhsMode&(Upper|Lower) };
+  enum { LhsUpLo = LhsMode & (Upper | Lower) };
 
-  template<typename Dest>
-  static EIGEN_DEVICE_FUNC
-  void run(Dest& dest, const Lhs &a_lhs, const Rhs &a_rhs, const Scalar& alpha)
-  {
+  template <typename Dest>
+  static EIGEN_DEVICE_FUNC void run(Dest& dest, const Lhs& a_lhs, const Rhs& a_rhs, const Scalar& alpha) {
     typedef typename Dest::Scalar ResScalar;
     typedef typename Rhs::Scalar RhsScalar;
-    typedef Map<Matrix<ResScalar,Dynamic,1>, EIGEN_PLAIN_ENUM_MIN(AlignedMax,internal::packet_traits<ResScalar>::size)> MappedDest;
-    
-    eigen_assert(dest.rows()==a_lhs.rows() && dest.cols()==a_rhs.cols());
+    typedef Map<Matrix<ResScalar, Dynamic, 1>, plain_enum_min(AlignedMax, internal::packet_traits<ResScalar>::size)>
+        MappedDest;
 
-    typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(a_lhs);
-    typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(a_rhs);
+    eigen_assert(dest.rows() == a_lhs.rows() && dest.cols() == a_rhs.cols());
 
-    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(a_lhs)
-                               * RhsBlasTraits::extractScalarFactor(a_rhs);
+    add_const_on_value_type_t<ActualLhsType> lhs = LhsBlasTraits::extract(a_lhs);
+    add_const_on_value_type_t<ActualRhsType> rhs = RhsBlasTraits::extract(a_rhs);
+
+    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(a_lhs) * RhsBlasTraits::extractScalarFactor(a_rhs);
 
     enum {
-      EvalToDest = (Dest::InnerStrideAtCompileTime==1),
-      UseRhs = (ActualRhsTypeCleaned::InnerStrideAtCompileTime==1)
+      EvalToDest = (Dest::InnerStrideAtCompileTime == 1),
+      UseRhs = (ActualRhsTypeCleaned::InnerStrideAtCompileTime == 1)
     };
-    
-    internal::gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,!EvalToDest> static_dest;
-    internal::gemv_static_vector_if<RhsScalar,ActualRhsTypeCleaned::SizeAtCompileTime,ActualRhsTypeCleaned::MaxSizeAtCompileTime,!UseRhs> static_rhs;
 
-    ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(),
+    internal::gemv_static_vector_if<ResScalar, Dest::SizeAtCompileTime, Dest::MaxSizeAtCompileTime, !EvalToDest>
+        static_dest;
+    internal::gemv_static_vector_if<RhsScalar, ActualRhsTypeCleaned::SizeAtCompileTime,
+                                    ActualRhsTypeCleaned::MaxSizeAtCompileTime, !UseRhs>
+        static_rhs;
+
+    ei_declare_aligned_stack_constructed_variable(ResScalar, actualDestPtr, dest.size(),
                                                   EvalToDest ? dest.data() : static_dest.data());
-                                                  
-    ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,rhs.size(),
-        UseRhs ? const_cast<RhsScalar*>(rhs.data()) : static_rhs.data());
-    
-    if(!EvalToDest)
-    {
-      #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+
+    ei_declare_aligned_stack_constructed_variable(RhsScalar, actualRhsPtr, rhs.size(),
+                                                  UseRhs ? const_cast<RhsScalar*>(rhs.data()) : static_rhs.data());
+
+    if (!EvalToDest) {
+#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
       Index size = dest.size();
       EIGEN_DENSE_STORAGE_CTOR_PLUGIN
-      #endif
+#endif
       MappedDest(actualDestPtr, dest.size()) = dest;
     }
-      
-    if(!UseRhs)
-    {
-      #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+
+    if (!UseRhs) {
+#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
       Index size = rhs.size();
       EIGEN_DENSE_STORAGE_CTOR_PLUGIN
-      #endif
+#endif
       Map<typename ActualRhsTypeCleaned::PlainObject>(actualRhsPtr, rhs.size()) = rhs;
     }
-      
-      
-    internal::selfadjoint_matrix_vector_product<Scalar, Index, (internal::traits<ActualLhsTypeCleaned>::Flags&RowMajorBit) ? RowMajor : ColMajor,
-                                                int(LhsUpLo), bool(LhsBlasTraits::NeedToConjugate), bool(RhsBlasTraits::NeedToConjugate)>::run
-      (
-        lhs.rows(),                             // size
-        &lhs.coeffRef(0,0),  lhs.outerStride(), // lhs info
-        actualRhsPtr,                           // rhs info
-        actualDestPtr,                          // result info
-        actualAlpha                             // scale factor
-      );
-    
-    if(!EvalToDest)
-      dest = MappedDest(actualDestPtr, dest.size());
+
+    internal::selfadjoint_matrix_vector_product<
+        Scalar, Index, (internal::traits<ActualLhsTypeCleaned>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+        int(LhsUpLo), bool(LhsBlasTraits::NeedToConjugate),
+        bool(RhsBlasTraits::NeedToConjugate)>::run(lhs.rows(),                              // size
+                                                   &lhs.coeffRef(0, 0), lhs.outerStride(),  // lhs info
+                                                   actualRhsPtr,                            // rhs info
+                                                   actualDestPtr,                           // result info
+                                                   actualAlpha                              // scale factor
+    );
+
+    if (!EvalToDest) dest = MappedDest(actualDestPtr, dest.size());
   }
 };
 
-template<typename Lhs, typename Rhs, int RhsMode>
-struct selfadjoint_product_impl<Lhs,0,true,Rhs,RhsMode,false>
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  enum { RhsUpLo = RhsMode&(Upper|Lower)  };
+template <typename Lhs, typename Rhs, int RhsMode>
+struct selfadjoint_product_impl<Lhs, 0, true, Rhs, RhsMode, false> {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
+  enum { RhsUpLo = RhsMode & (Upper | Lower) };
 
-  template<typename Dest>
-  static void run(Dest& dest, const Lhs &a_lhs, const Rhs &a_rhs, const Scalar& alpha)
-  {
+  template <typename Dest>
+  static void run(Dest& dest, const Lhs& a_lhs, const Rhs& a_rhs, const Scalar& alpha) {
     // let's simply transpose the product
     Transpose<Dest> destT(dest);
-    selfadjoint_product_impl<Transpose<const Rhs>, int(RhsUpLo)==Upper ? Lower : Upper, false,
-                             Transpose<const Lhs>, 0, true>::run(destT, a_rhs.transpose(), a_lhs.transpose(), alpha);
+    selfadjoint_product_impl<Transpose<const Rhs>, int(RhsUpLo) == Upper ? Lower : Upper, false, Transpose<const Lhs>,
+                             0, true>::run(destT, a_rhs.transpose(), a_lhs.transpose(), alpha);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELFADJOINT_MATRIX_VECTOR_H
+#endif  // EIGEN_SELFADJOINT_MATRIX_VECTOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h
index a21be80..f103465 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h
@@ -11,123 +11,123 @@
 #define EIGEN_SELFADJOINT_PRODUCT_H
 
 /**********************************************************************
-* This file implements a self adjoint product: C += A A^T updating only
-* half of the selfadjoint matrix C.
-* It corresponds to the level 3 SYRK and level 2 SYR Blas routines.
-**********************************************************************/
+ * This file implements a self adjoint product: C += A A^T updating only
+ * half of the selfadjoint matrix C.
+ * It corresponds to the level 3 SYRK and level 2 SYR Blas routines.
+ **********************************************************************/
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
 
+namespace Eigen {
 
-template<typename Scalar, typename Index, int UpLo, bool ConjLhs, bool ConjRhs>
-struct selfadjoint_rank1_update<Scalar,Index,ColMajor,UpLo,ConjLhs,ConjRhs>
-{
-  static void run(Index size, Scalar* mat, Index stride, const Scalar* vecX, const Scalar* vecY, const Scalar& alpha)
-  {
+template <typename Scalar, typename Index, int UpLo, bool ConjLhs, bool ConjRhs>
+struct selfadjoint_rank1_update<Scalar, Index, ColMajor, UpLo, ConjLhs, ConjRhs> {
+  static void run(Index size, Scalar* mat, Index stride, const Scalar* vecX, const Scalar* vecY, const Scalar& alpha) {
     internal::conj_if<ConjRhs> cj;
-    typedef Map<const Matrix<Scalar,Dynamic,1> > OtherMap;
-    typedef typename internal::conditional<ConjLhs,typename OtherMap::ConjugateReturnType,const OtherMap&>::type ConjLhsType;
-    for (Index i=0; i<size; ++i)
-    {
-      Map<Matrix<Scalar,Dynamic,1> >(mat+stride*i+(UpLo==Lower ? i : 0), (UpLo==Lower ? size-i : (i+1)))
-          += (alpha * cj(vecY[i])) * ConjLhsType(OtherMap(vecX+(UpLo==Lower ? i : 0),UpLo==Lower ? size-i : (i+1)));
+    typedef Map<const Matrix<Scalar, Dynamic, 1> > OtherMap;
+    typedef std::conditional_t<ConjLhs, typename OtherMap::ConjugateReturnType, const OtherMap&> ConjLhsType;
+    for (Index i = 0; i < size; ++i) {
+      Map<Matrix<Scalar, Dynamic, 1> >(mat + stride * i + (UpLo == Lower ? i : 0),
+                                       (UpLo == Lower ? size - i : (i + 1))) +=
+          (alpha * cj(vecY[i])) *
+          ConjLhsType(OtherMap(vecX + (UpLo == Lower ? i : 0), UpLo == Lower ? size - i : (i + 1)));
     }
   }
 };
 
-template<typename Scalar, typename Index, int UpLo, bool ConjLhs, bool ConjRhs>
-struct selfadjoint_rank1_update<Scalar,Index,RowMajor,UpLo,ConjLhs,ConjRhs>
-{
-  static void run(Index size, Scalar* mat, Index stride, const Scalar* vecX, const Scalar* vecY, const Scalar& alpha)
-  {
-    selfadjoint_rank1_update<Scalar,Index,ColMajor,UpLo==Lower?Upper:Lower,ConjRhs,ConjLhs>::run(size,mat,stride,vecY,vecX,alpha);
+template <typename Scalar, typename Index, int UpLo, bool ConjLhs, bool ConjRhs>
+struct selfadjoint_rank1_update<Scalar, Index, RowMajor, UpLo, ConjLhs, ConjRhs> {
+  static void run(Index size, Scalar* mat, Index stride, const Scalar* vecX, const Scalar* vecY, const Scalar& alpha) {
+    selfadjoint_rank1_update<Scalar, Index, ColMajor, UpLo == Lower ? Upper : Lower, ConjRhs, ConjLhs>::run(
+        size, mat, stride, vecY, vecX, alpha);
   }
 };
 
-template<typename MatrixType, typename OtherType, int UpLo, bool OtherIsVector = OtherType::IsVectorAtCompileTime>
+template <typename MatrixType, typename OtherType, int UpLo, bool OtherIsVector = OtherType::IsVectorAtCompileTime>
 struct selfadjoint_product_selector;
 
-template<typename MatrixType, typename OtherType, int UpLo>
-struct selfadjoint_product_selector<MatrixType,OtherType,UpLo,true>
-{
-  static void run(MatrixType& mat, const OtherType& other, const typename MatrixType::Scalar& alpha)
-  {
+template <typename MatrixType, typename OtherType, int UpLo>
+struct selfadjoint_product_selector<MatrixType, OtherType, UpLo, true> {
+  static void run(MatrixType& mat, const OtherType& other, const typename MatrixType::Scalar& alpha) {
     typedef typename MatrixType::Scalar Scalar;
     typedef internal::blas_traits<OtherType> OtherBlasTraits;
     typedef typename OtherBlasTraits::DirectLinearAccessType ActualOtherType;
-    typedef typename internal::remove_all<ActualOtherType>::type _ActualOtherType;
-    typename internal::add_const_on_value_type<ActualOtherType>::type actualOther = OtherBlasTraits::extract(other.derived());
+    typedef internal::remove_all_t<ActualOtherType> ActualOtherType_;
+    internal::add_const_on_value_type_t<ActualOtherType> actualOther = OtherBlasTraits::extract(other.derived());
 
     Scalar actualAlpha = alpha * OtherBlasTraits::extractScalarFactor(other.derived());
 
     enum {
-      StorageOrder = (internal::traits<MatrixType>::Flags&RowMajorBit) ? RowMajor : ColMajor,
-      UseOtherDirectly = _ActualOtherType::InnerStrideAtCompileTime==1
+      StorageOrder = (internal::traits<MatrixType>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+      UseOtherDirectly = ActualOtherType_::InnerStrideAtCompileTime == 1
     };
-    internal::gemv_static_vector_if<Scalar,OtherType::SizeAtCompileTime,OtherType::MaxSizeAtCompileTime,!UseOtherDirectly> static_other;
+    internal::gemv_static_vector_if<Scalar, OtherType::SizeAtCompileTime, OtherType::MaxSizeAtCompileTime,
+                                    !UseOtherDirectly>
+        static_other;
 
-    ei_declare_aligned_stack_constructed_variable(Scalar, actualOtherPtr, other.size(),
-      (UseOtherDirectly ? const_cast<Scalar*>(actualOther.data()) : static_other.data()));
-      
-    if(!UseOtherDirectly)
-      Map<typename _ActualOtherType::PlainObject>(actualOtherPtr, actualOther.size()) = actualOther;
-    
-    selfadjoint_rank1_update<Scalar,Index,StorageOrder,UpLo,
-                              OtherBlasTraits::NeedToConjugate  && NumTraits<Scalar>::IsComplex,
-                            (!OtherBlasTraits::NeedToConjugate) && NumTraits<Scalar>::IsComplex>
-          ::run(other.size(), mat.data(), mat.outerStride(), actualOtherPtr, actualOtherPtr, actualAlpha);
+    ei_declare_aligned_stack_constructed_variable(
+        Scalar, actualOtherPtr, other.size(),
+        (UseOtherDirectly ? const_cast<Scalar*>(actualOther.data()) : static_other.data()));
+
+    if (!UseOtherDirectly)
+      Map<typename ActualOtherType_::PlainObject>(actualOtherPtr, actualOther.size()) = actualOther;
+
+    selfadjoint_rank1_update<
+        Scalar, Index, StorageOrder, UpLo, OtherBlasTraits::NeedToConjugate && NumTraits<Scalar>::IsComplex,
+        (!OtherBlasTraits::NeedToConjugate) && NumTraits<Scalar>::IsComplex>::run(other.size(), mat.data(),
+                                                                                  mat.outerStride(), actualOtherPtr,
+                                                                                  actualOtherPtr, actualAlpha);
   }
 };
 
-template<typename MatrixType, typename OtherType, int UpLo>
-struct selfadjoint_product_selector<MatrixType,OtherType,UpLo,false>
-{
-  static void run(MatrixType& mat, const OtherType& other, const typename MatrixType::Scalar& alpha)
-  {
+template <typename MatrixType, typename OtherType, int UpLo>
+struct selfadjoint_product_selector<MatrixType, OtherType, UpLo, false> {
+  static void run(MatrixType& mat, const OtherType& other, const typename MatrixType::Scalar& alpha) {
     typedef typename MatrixType::Scalar Scalar;
     typedef internal::blas_traits<OtherType> OtherBlasTraits;
     typedef typename OtherBlasTraits::DirectLinearAccessType ActualOtherType;
-    typedef typename internal::remove_all<ActualOtherType>::type _ActualOtherType;
-    typename internal::add_const_on_value_type<ActualOtherType>::type actualOther = OtherBlasTraits::extract(other.derived());
+    typedef internal::remove_all_t<ActualOtherType> ActualOtherType_;
+    internal::add_const_on_value_type_t<ActualOtherType> actualOther = OtherBlasTraits::extract(other.derived());
 
     Scalar actualAlpha = alpha * OtherBlasTraits::extractScalarFactor(other.derived());
 
     enum {
-      IsRowMajor = (internal::traits<MatrixType>::Flags&RowMajorBit) ? 1 : 0,
-      OtherIsRowMajor = _ActualOtherType::Flags&RowMajorBit ? 1 : 0
+      IsRowMajor = (internal::traits<MatrixType>::Flags & RowMajorBit) ? 1 : 0,
+      OtherIsRowMajor = ActualOtherType_::Flags & RowMajorBit ? 1 : 0
     };
 
     Index size = mat.cols();
     Index depth = actualOther.cols();
 
-    typedef internal::gemm_blocking_space<IsRowMajor ? RowMajor : ColMajor,Scalar,Scalar,
-              MatrixType::MaxColsAtCompileTime, MatrixType::MaxColsAtCompileTime, _ActualOtherType::MaxColsAtCompileTime> BlockingType;
+    typedef internal::gemm_blocking_space<IsRowMajor ? RowMajor : ColMajor, Scalar, Scalar,
+                                          MatrixType::MaxColsAtCompileTime, MatrixType::MaxColsAtCompileTime,
+                                          ActualOtherType_::MaxColsAtCompileTime>
+        BlockingType;
 
     BlockingType blocking(size, size, depth, 1, false);
 
-
-    internal::general_matrix_matrix_triangular_product<Index,
-      Scalar, OtherIsRowMajor ? RowMajor : ColMajor,   OtherBlasTraits::NeedToConjugate  && NumTraits<Scalar>::IsComplex,
-      Scalar, OtherIsRowMajor ? ColMajor : RowMajor, (!OtherBlasTraits::NeedToConjugate) && NumTraits<Scalar>::IsComplex,
-      IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime, UpLo>
-      ::run(size, depth,
-            actualOther.data(), actualOther.outerStride(), actualOther.data(), actualOther.outerStride(),
-            mat.data(), mat.innerStride(), mat.outerStride(), actualAlpha, blocking);
+    internal::general_matrix_matrix_triangular_product<
+        Index, Scalar, OtherIsRowMajor ? RowMajor : ColMajor,
+        OtherBlasTraits::NeedToConjugate && NumTraits<Scalar>::IsComplex, Scalar, OtherIsRowMajor ? ColMajor : RowMajor,
+        (!OtherBlasTraits::NeedToConjugate) && NumTraits<Scalar>::IsComplex, IsRowMajor ? RowMajor : ColMajor,
+        MatrixType::InnerStrideAtCompileTime, UpLo>::run(size, depth, actualOther.data(), actualOther.outerStride(),
+                                                         actualOther.data(), actualOther.outerStride(), mat.data(),
+                                                         mat.innerStride(), mat.outerStride(), actualAlpha, blocking);
   }
 };
 
 // high level API
 
-template<typename MatrixType, unsigned int UpLo>
-template<typename DerivedU>
-EIGEN_DEVICE_FUNC SelfAdjointView<MatrixType,UpLo>& SelfAdjointView<MatrixType,UpLo>
-::rankUpdate(const MatrixBase<DerivedU>& u, const Scalar& alpha)
-{
-  selfadjoint_product_selector<MatrixType,DerivedU,UpLo>::run(_expression().const_cast_derived(), u.derived(), alpha);
+template <typename MatrixType, unsigned int UpLo>
+template <typename DerivedU>
+EIGEN_DEVICE_FUNC SelfAdjointView<MatrixType, UpLo>& SelfAdjointView<MatrixType, UpLo>::rankUpdate(
+    const MatrixBase<DerivedU>& u, const Scalar& alpha) {
+  selfadjoint_product_selector<MatrixType, DerivedU, UpLo>::run(_expression().const_cast_derived(), u.derived(), alpha);
 
   return *this;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELFADJOINT_PRODUCT_H
+#endif  // EIGEN_SELFADJOINT_PRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h
index f752a0b..9c234ec 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h
@@ -10,7 +10,10 @@
 #ifndef EIGEN_SELFADJOINTRANK2UPTADE_H
 #define EIGEN_SELFADJOINTRANK2UPTADE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
@@ -18,77 +21,75 @@
  * It corresponds to the Level2 syr2 BLAS routine
  */
 
-template<typename Scalar, typename Index, typename UType, typename VType, int UpLo>
+template <typename Scalar, typename Index, typename UType, typename VType, int UpLo>
 struct selfadjoint_rank2_update_selector;
 
-template<typename Scalar, typename Index, typename UType, typename VType>
-struct selfadjoint_rank2_update_selector<Scalar,Index,UType,VType,Lower>
-{
-  static EIGEN_DEVICE_FUNC
-  void run(Scalar* mat, Index stride, const UType& u, const VType& v, const Scalar& alpha)
-  {
+template <typename Scalar, typename Index, typename UType, typename VType>
+struct selfadjoint_rank2_update_selector<Scalar, Index, UType, VType, Lower> {
+  static EIGEN_DEVICE_FUNC void run(Scalar* mat, Index stride, const UType& u, const VType& v, const Scalar& alpha) {
     const Index size = u.size();
-    for (Index i=0; i<size; ++i)
-    {
-      Map<Matrix<Scalar,Dynamic,1> >(mat+stride*i+i, size-i) +=
-                        (numext::conj(alpha) * numext::conj(u.coeff(i))) * v.tail(size-i)
-                      + (alpha * numext::conj(v.coeff(i))) * u.tail(size-i);
+    for (Index i = 0; i < size; ++i) {
+      Map<Matrix<Scalar, Dynamic, 1>>(mat + stride * i + i, size - i) +=
+          (numext::conj(alpha) * numext::conj(u.coeff(i))) * v.tail(size - i) +
+          (alpha * numext::conj(v.coeff(i))) * u.tail(size - i);
     }
   }
 };
 
-template<typename Scalar, typename Index, typename UType, typename VType>
-struct selfadjoint_rank2_update_selector<Scalar,Index,UType,VType,Upper>
-{
-  static void run(Scalar* mat, Index stride, const UType& u, const VType& v, const Scalar& alpha)
-  {
+template <typename Scalar, typename Index, typename UType, typename VType>
+struct selfadjoint_rank2_update_selector<Scalar, Index, UType, VType, Upper> {
+  static void run(Scalar* mat, Index stride, const UType& u, const VType& v, const Scalar& alpha) {
     const Index size = u.size();
-    for (Index i=0; i<size; ++i)
-      Map<Matrix<Scalar,Dynamic,1> >(mat+stride*i, i+1) +=
-                        (numext::conj(alpha)  * numext::conj(u.coeff(i))) * v.head(i+1)
-                      + (alpha * numext::conj(v.coeff(i))) * u.head(i+1);
+    for (Index i = 0; i < size; ++i)
+      Map<Matrix<Scalar, Dynamic, 1>>(mat + stride * i, i + 1) +=
+          (numext::conj(alpha) * numext::conj(u.coeff(i))) * v.head(i + 1) +
+          (alpha * numext::conj(v.coeff(i))) * u.head(i + 1);
   }
 };
 
-template<bool Cond, typename T> struct conj_expr_if
-  : conditional<!Cond, const T&,
-      CwiseUnaryOp<scalar_conjugate_op<typename traits<T>::Scalar>,T> > {};
+template <bool Cond, typename T>
+using conj_expr_if =
+    std::conditional<!Cond, const T&, CwiseUnaryOp<scalar_conjugate_op<typename traits<T>::Scalar>, T>>;
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename MatrixType, unsigned int UpLo>
-template<typename DerivedU, typename DerivedV>
-EIGEN_DEVICE_FUNC SelfAdjointView<MatrixType,UpLo>& SelfAdjointView<MatrixType,UpLo>
-::rankUpdate(const MatrixBase<DerivedU>& u, const MatrixBase<DerivedV>& v, const Scalar& alpha)
-{
+template <typename MatrixType, unsigned int UpLo>
+template <typename DerivedU, typename DerivedV>
+EIGEN_DEVICE_FUNC SelfAdjointView<MatrixType, UpLo>& SelfAdjointView<MatrixType, UpLo>::rankUpdate(
+    const MatrixBase<DerivedU>& u, const MatrixBase<DerivedV>& v, const Scalar& alpha) {
   typedef internal::blas_traits<DerivedU> UBlasTraits;
   typedef typename UBlasTraits::DirectLinearAccessType ActualUType;
-  typedef typename internal::remove_all<ActualUType>::type _ActualUType;
-  typename internal::add_const_on_value_type<ActualUType>::type actualU = UBlasTraits::extract(u.derived());
+  typedef internal::remove_all_t<ActualUType> ActualUType_;
+  internal::add_const_on_value_type_t<ActualUType> actualU = UBlasTraits::extract(u.derived());
 
   typedef internal::blas_traits<DerivedV> VBlasTraits;
   typedef typename VBlasTraits::DirectLinearAccessType ActualVType;
-  typedef typename internal::remove_all<ActualVType>::type _ActualVType;
-  typename internal::add_const_on_value_type<ActualVType>::type actualV = VBlasTraits::extract(v.derived());
+  typedef internal::remove_all_t<ActualVType> ActualVType_;
+  internal::add_const_on_value_type_t<ActualVType> actualV = VBlasTraits::extract(v.derived());
 
   // If MatrixType is row major, then we use the routine for lower triangular in the upper triangular case and
   // vice versa, and take the complex conjugate of all coefficients and vector entries.
 
-  enum { IsRowMajor = (internal::traits<MatrixType>::Flags&RowMajorBit) ? 1 : 0 };
-  Scalar actualAlpha = alpha * UBlasTraits::extractScalarFactor(u.derived())
-                             * numext::conj(VBlasTraits::extractScalarFactor(v.derived()));
-  if (IsRowMajor)
-    actualAlpha = numext::conj(actualAlpha);
+  enum { IsRowMajor = (internal::traits<MatrixType>::Flags & RowMajorBit) ? 1 : 0 };
+  Scalar actualAlpha = alpha * UBlasTraits::extractScalarFactor(u.derived()) *
+                       numext::conj(VBlasTraits::extractScalarFactor(v.derived()));
+  if (IsRowMajor) actualAlpha = numext::conj(actualAlpha);
 
-  typedef typename internal::remove_all<typename internal::conj_expr_if<int(IsRowMajor) ^ int(UBlasTraits::NeedToConjugate), _ActualUType>::type>::type UType;
-  typedef typename internal::remove_all<typename internal::conj_expr_if<int(IsRowMajor) ^ int(VBlasTraits::NeedToConjugate), _ActualVType>::type>::type VType;
+  typedef internal::remove_all_t<
+      typename internal::conj_expr_if<int(IsRowMajor) ^ int(UBlasTraits::NeedToConjugate), ActualUType_>::type>
+      UType;
+  typedef internal::remove_all_t<
+      typename internal::conj_expr_if<int(IsRowMajor) ^ int(VBlasTraits::NeedToConjugate), ActualVType_>::type>
+      VType;
   internal::selfadjoint_rank2_update_selector<Scalar, Index, UType, VType,
-    (IsRowMajor ? int(UpLo==Upper ? Lower : Upper) : UpLo)>
-    ::run(_expression().const_cast_derived().data(),_expression().outerStride(),UType(actualU),VType(actualV),actualAlpha);
+                                              (IsRowMajor ? int(UpLo == Upper ? Lower : Upper)
+                                                          : UpLo)>::run(_expression().const_cast_derived().data(),
+                                                                        _expression().outerStride(), UType(actualU),
+                                                                        VType(actualV), actualAlpha);
 
   return *this;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELFADJOINTRANK2UPTADE_H
+#endif  // EIGEN_SELFADJOINTRANK2UPTADE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h
index f0c6050..c541909 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h
@@ -10,7 +10,10 @@
 #ifndef EIGEN_TRIANGULAR_MATRIX_MATRIX_H
 #define EIGEN_TRIANGULAR_MATRIX_MATRIX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
@@ -18,10 +21,10 @@
 // struct gemm_pack_lhs_triangular
 // {
 //   Matrix<Scalar,mr,mr,
-//   void operator()(Scalar* blockA, const EIGEN_RESTRICT Scalar* _lhs, int lhsStride, int depth, int rows)
+//   void operator()(Scalar* blockA, const EIGEN_RESTRICT Scalar* lhs_, int lhsStride, int depth, int rows)
 //   {
 //     conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
-//     const_blas_data_mapper<Scalar, StorageOrder> lhs(_lhs,lhsStride);
+//     const_blas_data_mapper<Scalar, StorageOrder> lhs(lhs_,lhsStride);
 //     int count = 0;
 //     const int peeled_mc = (rows/mr)*mr;
 //     for(int i=0; i<peeled_mc; i+=mr)
@@ -41,432 +44,361 @@
 /* Optimized triangular matrix * matrix (_TRMM++) product built on top of
  * the general matrix matrix product.
  */
-template <typename Scalar, typename Index,
-          int Mode, bool LhsIsTriangular,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResStorageOrder, int ResInnerStride,
-          int Version = Specialized>
+template <typename Scalar, typename Index, int Mode, bool LhsIsTriangular, int LhsStorageOrder, bool ConjugateLhs,
+          int RhsStorageOrder, bool ConjugateRhs, int ResStorageOrder, int ResInnerStride, int Version = Specialized>
 struct product_triangular_matrix_matrix;
 
-template <typename Scalar, typename Index,
-          int Mode, bool LhsIsTriangular,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride, int Version>
-struct product_triangular_matrix_matrix<Scalar,Index,Mode,LhsIsTriangular,
-                                           LhsStorageOrder,ConjugateLhs,
-                                           RhsStorageOrder,ConjugateRhs,RowMajor,ResInnerStride,Version>
-{
-  static EIGEN_STRONG_INLINE void run(
-    Index rows, Index cols, Index depth,
-    const Scalar* lhs, Index lhsStride,
-    const Scalar* rhs, Index rhsStride,
-    Scalar* res,       Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
-  {
-    product_triangular_matrix_matrix<Scalar, Index,
-      (Mode&(UnitDiag|ZeroDiag)) | ((Mode&Upper) ? Lower : Upper),
-      (!LhsIsTriangular),
-      RhsStorageOrder==RowMajor ? ColMajor : RowMajor,
-      ConjugateRhs,
-      LhsStorageOrder==RowMajor ? ColMajor : RowMajor,
-      ConjugateLhs,
-      ColMajor, ResInnerStride>
-      ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resIncr, resStride, alpha, blocking);
+template <typename Scalar, typename Index, int Mode, bool LhsIsTriangular, int LhsStorageOrder, bool ConjugateLhs,
+          int RhsStorageOrder, bool ConjugateRhs, int ResInnerStride, int Version>
+struct product_triangular_matrix_matrix<Scalar, Index, Mode, LhsIsTriangular, LhsStorageOrder, ConjugateLhs,
+                                        RhsStorageOrder, ConjugateRhs, RowMajor, ResInnerStride, Version> {
+  static EIGEN_STRONG_INLINE void run(Index rows, Index cols, Index depth, const Scalar* lhs, Index lhsStride,
+                                      const Scalar* rhs, Index rhsStride, Scalar* res, Index resIncr, Index resStride,
+                                      const Scalar& alpha, level3_blocking<Scalar, Scalar>& blocking) {
+    product_triangular_matrix_matrix<Scalar, Index, (Mode & (UnitDiag | ZeroDiag)) | ((Mode & Upper) ? Lower : Upper),
+                                     (!LhsIsTriangular), RhsStorageOrder == RowMajor ? ColMajor : RowMajor,
+                                     ConjugateRhs, LhsStorageOrder == RowMajor ? ColMajor : RowMajor, ConjugateLhs,
+                                     ColMajor, ResInnerStride>::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride,
+                                                                    res, resIncr, resStride, alpha, blocking);
   }
 };
 
 // implements col-major += alpha * op(triangular) * op(general)
-template <typename Scalar, typename Index, int Mode,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride, int Version>
-struct product_triangular_matrix_matrix<Scalar,Index,Mode,true,
-                                           LhsStorageOrder,ConjugateLhs,
-                                           RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>
-{
-  
-  typedef gebp_traits<Scalar,Scalar> Traits;
+template <typename Scalar, typename Index, int Mode, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride, int Version>
+struct product_triangular_matrix_matrix<Scalar, Index, Mode, true, LhsStorageOrder, ConjugateLhs, RhsStorageOrder,
+                                        ConjugateRhs, ColMajor, ResInnerStride, Version> {
+  typedef gebp_traits<Scalar, Scalar> Traits;
   enum {
-    SmallPanelWidth   = 2 * EIGEN_PLAIN_ENUM_MAX(Traits::mr,Traits::nr),
-    IsLower = (Mode&Lower) == Lower,
-    SetDiag = (Mode&(ZeroDiag|UnitDiag)) ? 0 : 1
+    SmallPanelWidth = 2 * plain_enum_max(Traits::mr, Traits::nr),
+    IsLower = (Mode & Lower) == Lower,
+    SetDiag = (Mode & (ZeroDiag | UnitDiag)) ? 0 : 1
   };
 
-  static EIGEN_DONT_INLINE void run(
-    Index _rows, Index _cols, Index _depth,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
+  static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, Index _depth, const Scalar* lhs_, Index lhsStride,
+                                    const Scalar* rhs_, Index rhsStride, Scalar* res, Index resIncr, Index resStride,
+                                    const Scalar& alpha, level3_blocking<Scalar, Scalar>& blocking);
 };
 
-template <typename Scalar, typename Index, int Mode,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride, int Version>
-EIGEN_DONT_INLINE void product_triangular_matrix_matrix<Scalar,Index,Mode,true,
-                                                        LhsStorageOrder,ConjugateLhs,
-                                                        RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>::run(
-    Index _rows, Index _cols, Index _depth,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,       Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
-  {
-    // strip zeros
-    Index diagSize  = (std::min)(_rows,_depth);
-    Index rows      = IsLower ? _rows : diagSize;
-    Index depth     = IsLower ? diagSize : _depth;
-    Index cols      = _cols;
-    
-    typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
-    typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
-    LhsMapper lhs(_lhs,lhsStride);
-    RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride, resIncr);
+template <typename Scalar, typename Index, int Mode, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride, int Version>
+EIGEN_DONT_INLINE void product_triangular_matrix_matrix<
+    Scalar, Index, Mode, true, LhsStorageOrder, ConjugateLhs, RhsStorageOrder, ConjugateRhs, ColMajor, ResInnerStride,
+    Version>::run(Index _rows, Index _cols, Index _depth, const Scalar* lhs_, Index lhsStride, const Scalar* rhs_,
+                  Index rhsStride, Scalar* res_, Index resIncr, Index resStride, const Scalar& alpha,
+                  level3_blocking<Scalar, Scalar>& blocking) {
+  // strip zeros
+  Index diagSize = (std::min)(_rows, _depth);
+  Index rows = IsLower ? _rows : diagSize;
+  Index depth = IsLower ? diagSize : _depth;
+  Index cols = _cols;
 
-    Index kc = blocking.kc();                   // cache block size along the K direction
-    Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
-    // The small panel size must not be larger than blocking size.
-    // Usually this should never be the case because SmallPanelWidth^2 is very small
-    // compared to L2 cache size, but let's be safe:
-    Index panelWidth = (std::min)(Index(SmallPanelWidth),(std::min)(kc,mc));
+  typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
+  typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
+  typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
+  LhsMapper lhs(lhs_, lhsStride);
+  RhsMapper rhs(rhs_, rhsStride);
+  ResMapper res(res_, resStride, resIncr);
 
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*cols;
+  Index kc = blocking.kc();                    // cache block size along the K direction
+  Index mc = (std::min)(rows, blocking.mc());  // cache block size along the M direction
+  // The small panel size must not be larger than blocking size.
+  // Usually this should never be the case because SmallPanelWidth^2 is very small
+  // compared to L2 cache size, but let's be safe:
+  Index panelWidth = (std::min)(Index(SmallPanelWidth), (std::min)(kc, mc));
 
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+  std::size_t sizeA = kc * mc;
+  std::size_t sizeB = kc * cols;
 
-    // To work around an "error: member reference base type 'Matrix<...>
-    // (Eigen::internal::constructor_without_unaligned_array_assert (*)())' is
-    // not a structure or union" compilation error in nvcc (tested V8.0.61),
-    // create a dummy internal::constructor_without_unaligned_array_assert
-    // object to pass to the Matrix constructor.
-    internal::constructor_without_unaligned_array_assert a;
-    Matrix<Scalar,SmallPanelWidth,SmallPanelWidth,LhsStorageOrder> triangularBuffer(a);
-    triangularBuffer.setZero();
-    if((Mode&ZeroDiag)==ZeroDiag)
-      triangularBuffer.diagonal().setZero();
-    else
-      triangularBuffer.diagonal().setOnes();
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
-    gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
-    gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder> pack_rhs;
+  // To work around an "error: member reference base type 'Matrix<...>
+  // (Eigen::internal::constructor_without_unaligned_array_assert (*)())' is
+  // not a structure or union" compilation error in nvcc (tested V8.0.61),
+  // create a dummy internal::constructor_without_unaligned_array_assert
+  // object to pass to the Matrix constructor.
+  internal::constructor_without_unaligned_array_assert a;
+  Matrix<Scalar, SmallPanelWidth, SmallPanelWidth, LhsStorageOrder> triangularBuffer(a);
+  triangularBuffer.setZero();
+  if ((Mode & ZeroDiag) == ZeroDiag)
+    triangularBuffer.diagonal().setZero();
+  else
+    triangularBuffer.diagonal().setOnes();
 
-    for(Index k2=IsLower ? depth : 0;
-        IsLower ? k2>0 : k2<depth;
-        IsLower ? k2-=kc : k2+=kc)
-    {
-      Index actual_kc = (std::min)(IsLower ? k2 : depth-k2, kc);
-      Index actual_k2 = IsLower ? k2-actual_kc : k2;
+  gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
+  gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                LhsStorageOrder>
+      pack_lhs;
+  gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
 
-      // align blocks with the end of the triangular part for trapezoidal lhs
-      if((!IsLower)&&(k2<rows)&&(k2+actual_kc>rows))
-      {
-        actual_kc = rows-k2;
-        k2 = k2+actual_kc-kc;
-      }
+  for (Index k2 = IsLower ? depth : 0; IsLower ? k2 > 0 : k2 < depth; IsLower ? k2 -= kc : k2 += kc) {
+    Index actual_kc = (std::min)(IsLower ? k2 : depth - k2, kc);
+    Index actual_k2 = IsLower ? k2 - actual_kc : k2;
 
-      pack_rhs(blockB, rhs.getSubMapper(actual_k2,0), actual_kc, cols);
+    // align blocks with the end of the triangular part for trapezoidal lhs
+    if ((!IsLower) && (k2 < rows) && (k2 + actual_kc > rows)) {
+      actual_kc = rows - k2;
+      k2 = k2 + actual_kc - kc;
+    }
 
-      // the selected lhs's panel has to be split in three different parts:
-      //  1 - the part which is zero => skip it
-      //  2 - the diagonal block => special kernel
-      //  3 - the dense panel below (lower case) or above (upper case) the diagonal block => GEPP
+    pack_rhs(blockB, rhs.getSubMapper(actual_k2, 0), actual_kc, cols);
 
-      // the block diagonal, if any:
-      if(IsLower || actual_k2<rows)
-      {
-        // for each small vertical panels of lhs
-        for (Index k1=0; k1<actual_kc; k1+=panelWidth)
-        {
-          Index actualPanelWidth = std::min<Index>(actual_kc-k1, panelWidth);
-          Index lengthTarget = IsLower ? actual_kc-k1-actualPanelWidth : k1;
-          Index startBlock   = actual_k2+k1;
-          Index blockBOffset = k1;
+    // the selected lhs's panel has to be split in three different parts:
+    //  1 - the part which is zero => skip it
+    //  2 - the diagonal block => special kernel
+    //  3 - the dense panel below (lower case) or above (upper case) the diagonal block => GEPP
 
-          // => GEBP with the micro triangular block
-          // The trick is to pack this micro block while filling the opposite triangular part with zeros.
-          // To this end we do an extra triangular copy to a small temporary buffer
-          for (Index k=0;k<actualPanelWidth;++k)
-          {
-            if (SetDiag)
-              triangularBuffer.coeffRef(k,k) = lhs(startBlock+k,startBlock+k);
-            for (Index i=IsLower ? k+1 : 0; IsLower ? i<actualPanelWidth : i<k; ++i)
-              triangularBuffer.coeffRef(i,k) = lhs(startBlock+i,startBlock+k);
-          }
-          pack_lhs(blockA, LhsMapper(triangularBuffer.data(), triangularBuffer.outerStride()), actualPanelWidth, actualPanelWidth);
+    // the block diagonal, if any:
+    if (IsLower || actual_k2 < rows) {
+      // for each small vertical panels of lhs
+      for (Index k1 = 0; k1 < actual_kc; k1 += panelWidth) {
+        Index actualPanelWidth = std::min<Index>(actual_kc - k1, panelWidth);
+        Index lengthTarget = IsLower ? actual_kc - k1 - actualPanelWidth : k1;
+        Index startBlock = actual_k2 + k1;
+        Index blockBOffset = k1;
 
-          gebp_kernel(res.getSubMapper(startBlock, 0), blockA, blockB,
-                      actualPanelWidth, actualPanelWidth, cols, alpha,
-                      actualPanelWidth, actual_kc, 0, blockBOffset);
-
-          // GEBP with remaining micro panel
-          if (lengthTarget>0)
-          {
-            Index startTarget  = IsLower ? actual_k2+k1+actualPanelWidth : actual_k2;
-
-            pack_lhs(blockA, lhs.getSubMapper(startTarget,startBlock), actualPanelWidth, lengthTarget);
-
-            gebp_kernel(res.getSubMapper(startTarget, 0), blockA, blockB,
-                        lengthTarget, actualPanelWidth, cols, alpha,
-                        actualPanelWidth, actual_kc, 0, blockBOffset);
-          }
+        // => GEBP with the micro triangular block
+        // The trick is to pack this micro block while filling the opposite triangular part with zeros.
+        // To this end we do an extra triangular copy to a small temporary buffer
+        for (Index k = 0; k < actualPanelWidth; ++k) {
+          if (SetDiag) triangularBuffer.coeffRef(k, k) = lhs(startBlock + k, startBlock + k);
+          for (Index i = IsLower ? k + 1 : 0; IsLower ? i < actualPanelWidth : i < k; ++i)
+            triangularBuffer.coeffRef(i, k) = lhs(startBlock + i, startBlock + k);
         }
-      }
-      // the part below (lower case) or above (upper case) the diagonal => GEPP
-      {
-        Index start = IsLower ? k2 : 0;
-        Index end   = IsLower ? rows : (std::min)(actual_k2,rows);
-        for(Index i2=start; i2<end; i2+=mc)
-        {
-          const Index actual_mc = (std::min)(i2+mc,end)-i2;
-          gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr,Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder,false>()
-            (blockA, lhs.getSubMapper(i2, actual_k2), actual_kc, actual_mc);
+        pack_lhs(blockA, LhsMapper(triangularBuffer.data(), triangularBuffer.outerStride()), actualPanelWidth,
+                 actualPanelWidth);
 
-          gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc,
-                      actual_kc, cols, alpha, -1, -1, 0, 0);
+        gebp_kernel(res.getSubMapper(startBlock, 0), blockA, blockB, actualPanelWidth, actualPanelWidth, cols, alpha,
+                    actualPanelWidth, actual_kc, 0, blockBOffset);
+
+        // GEBP with remaining micro panel
+        if (lengthTarget > 0) {
+          Index startTarget = IsLower ? actual_k2 + k1 + actualPanelWidth : actual_k2;
+
+          pack_lhs(blockA, lhs.getSubMapper(startTarget, startBlock), actualPanelWidth, lengthTarget);
+
+          gebp_kernel(res.getSubMapper(startTarget, 0), blockA, blockB, lengthTarget, actualPanelWidth, cols, alpha,
+                      actualPanelWidth, actual_kc, 0, blockBOffset);
         }
       }
     }
+    // the part below (lower case) or above (upper case) the diagonal => GEPP
+    {
+      Index start = IsLower ? k2 : 0;
+      Index end = IsLower ? rows : (std::min)(actual_k2, rows);
+      for (Index i2 = start; i2 < end; i2 += mc) {
+        const Index actual_mc = (std::min)(i2 + mc, end) - i2;
+        gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                      LhsStorageOrder, false>()(blockA, lhs.getSubMapper(i2, actual_k2), actual_kc, actual_mc);
+
+        gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha, -1, -1, 0, 0);
+      }
+    }
   }
+}
 
 // implements col-major += alpha * op(general) * op(triangular)
-template <typename Scalar, typename Index, int Mode,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride, int Version>
-struct product_triangular_matrix_matrix<Scalar,Index,Mode,false,
-                                        LhsStorageOrder,ConjugateLhs,
-                                        RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>
-{
-  typedef gebp_traits<Scalar,Scalar> Traits;
+template <typename Scalar, typename Index, int Mode, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride, int Version>
+struct product_triangular_matrix_matrix<Scalar, Index, Mode, false, LhsStorageOrder, ConjugateLhs, RhsStorageOrder,
+                                        ConjugateRhs, ColMajor, ResInnerStride, Version> {
+  typedef gebp_traits<Scalar, Scalar> Traits;
   enum {
-    SmallPanelWidth   = EIGEN_PLAIN_ENUM_MAX(Traits::mr,Traits::nr),
-    IsLower = (Mode&Lower) == Lower,
-    SetDiag = (Mode&(ZeroDiag|UnitDiag)) ? 0 : 1
+    SmallPanelWidth = plain_enum_max(Traits::mr, Traits::nr),
+    IsLower = (Mode & Lower) == Lower,
+    SetDiag = (Mode & (ZeroDiag | UnitDiag)) ? 0 : 1
   };
 
-  static EIGEN_DONT_INLINE void run(
-    Index _rows, Index _cols, Index _depth,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
+  static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, Index _depth, const Scalar* lhs_, Index lhsStride,
+                                    const Scalar* rhs_, Index rhsStride, Scalar* res, Index resIncr, Index resStride,
+                                    const Scalar& alpha, level3_blocking<Scalar, Scalar>& blocking);
 };
 
-template <typename Scalar, typename Index, int Mode,
-          int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs,
-          int ResInnerStride, int Version>
-EIGEN_DONT_INLINE void product_triangular_matrix_matrix<Scalar,Index,Mode,false,
-                                                        LhsStorageOrder,ConjugateLhs,
-                                                        RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>::run(
-    Index _rows, Index _cols, Index _depth,
-    const Scalar* _lhs, Index lhsStride,
-    const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,       Index resIncr, Index resStride,
-    const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
-  {
-    const Index PacketBytes = packet_traits<Scalar>::size*sizeof(Scalar);
-    // strip zeros
-    Index diagSize  = (std::min)(_cols,_depth);
-    Index rows      = _rows;
-    Index depth     = IsLower ? _depth : diagSize;
-    Index cols      = IsLower ? diagSize : _cols;
-    
-    typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
-    typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
-    LhsMapper lhs(_lhs,lhsStride);
-    RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride, resIncr);
+template <typename Scalar, typename Index, int Mode, int LhsStorageOrder, bool ConjugateLhs, int RhsStorageOrder,
+          bool ConjugateRhs, int ResInnerStride, int Version>
+EIGEN_DONT_INLINE void product_triangular_matrix_matrix<
+    Scalar, Index, Mode, false, LhsStorageOrder, ConjugateLhs, RhsStorageOrder, ConjugateRhs, ColMajor, ResInnerStride,
+    Version>::run(Index _rows, Index _cols, Index _depth, const Scalar* lhs_, Index lhsStride, const Scalar* rhs_,
+                  Index rhsStride, Scalar* res_, Index resIncr, Index resStride, const Scalar& alpha,
+                  level3_blocking<Scalar, Scalar>& blocking) {
+  const Index PacketBytes = packet_traits<Scalar>::size * sizeof(Scalar);
+  // strip zeros
+  Index diagSize = (std::min)(_cols, _depth);
+  Index rows = _rows;
+  Index depth = IsLower ? _depth : diagSize;
+  Index cols = IsLower ? diagSize : _cols;
 
-    Index kc = blocking.kc();                   // cache block size along the K direction
-    Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
+  typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
+  typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
+  typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
+  LhsMapper lhs(lhs_, lhsStride);
+  RhsMapper rhs(rhs_, rhsStride);
+  ResMapper res(res_, resStride, resIncr);
 
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*cols+EIGEN_MAX_ALIGN_BYTES/sizeof(Scalar);
+  Index kc = blocking.kc();                    // cache block size along the K direction
+  Index mc = (std::min)(rows, blocking.mc());  // cache block size along the M direction
 
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+  std::size_t sizeA = kc * mc;
+  std::size_t sizeB = kc * cols + EIGEN_MAX_ALIGN_BYTES / sizeof(Scalar);
 
-    internal::constructor_without_unaligned_array_assert a;
-    Matrix<Scalar,SmallPanelWidth,SmallPanelWidth,RhsStorageOrder> triangularBuffer(a);
-    triangularBuffer.setZero();
-    if((Mode&ZeroDiag)==ZeroDiag)
-      triangularBuffer.diagonal().setZero();
-    else
-      triangularBuffer.diagonal().setOnes();
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
-    gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
-    gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder> pack_rhs;
-    gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder,false,true> pack_rhs_panel;
+  internal::constructor_without_unaligned_array_assert a;
+  Matrix<Scalar, SmallPanelWidth, SmallPanelWidth, RhsStorageOrder> triangularBuffer(a);
+  triangularBuffer.setZero();
+  if ((Mode & ZeroDiag) == ZeroDiag)
+    triangularBuffer.diagonal().setZero();
+  else
+    triangularBuffer.diagonal().setOnes();
 
-    for(Index k2=IsLower ? 0 : depth;
-        IsLower ? k2<depth  : k2>0;
-        IsLower ? k2+=kc   : k2-=kc)
-    {
-      Index actual_kc = (std::min)(IsLower ? depth-k2 : k2, kc);
-      Index actual_k2 = IsLower ? k2 : k2-actual_kc;
+  gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
+  gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                LhsStorageOrder>
+      pack_lhs;
+  gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
+  gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder, false, true> pack_rhs_panel;
 
-      // align blocks with the end of the triangular part for trapezoidal rhs
-      if(IsLower && (k2<cols) && (actual_k2+actual_kc>cols))
-      {
-        actual_kc = cols-k2;
-        k2 = actual_k2 + actual_kc - kc;
-      }
+  for (Index k2 = IsLower ? 0 : depth; IsLower ? k2 < depth : k2 > 0; IsLower ? k2 += kc : k2 -= kc) {
+    Index actual_kc = (std::min)(IsLower ? depth - k2 : k2, kc);
+    Index actual_k2 = IsLower ? k2 : k2 - actual_kc;
 
-      // remaining size
-      Index rs = IsLower ? (std::min)(cols,actual_k2) : cols - k2;
-      // size of the triangular part
-      Index ts = (IsLower && actual_k2>=cols) ? 0 : actual_kc;
+    // align blocks with the end of the triangular part for trapezoidal rhs
+    if (IsLower && (k2 < cols) && (actual_k2 + actual_kc > cols)) {
+      actual_kc = cols - k2;
+      k2 = actual_k2 + actual_kc - kc;
+    }
 
-      Scalar* geb = blockB+ts*ts;
-      geb = geb + internal::first_aligned<PacketBytes>(geb,PacketBytes/sizeof(Scalar));
+    // remaining size
+    Index rs = IsLower ? (std::min)(cols, actual_k2) : cols - k2;
+    // size of the triangular part
+    Index ts = (IsLower && actual_k2 >= cols) ? 0 : actual_kc;
 
-      pack_rhs(geb, rhs.getSubMapper(actual_k2,IsLower ? 0 : k2), actual_kc, rs);
+    Scalar* geb = blockB + ts * ts;
+    geb = geb + internal::first_aligned<PacketBytes>(geb, PacketBytes / sizeof(Scalar));
 
-      // pack the triangular part of the rhs padding the unrolled blocks with zeros
-      if(ts>0)
-      {
-        for (Index j2=0; j2<actual_kc; j2+=SmallPanelWidth)
-        {
-          Index actualPanelWidth = std::min<Index>(actual_kc-j2, SmallPanelWidth);
-          Index actual_j2 = actual_k2 + j2;
-          Index panelOffset = IsLower ? j2+actualPanelWidth : 0;
-          Index panelLength = IsLower ? actual_kc-j2-actualPanelWidth : j2;
-          // general part
-          pack_rhs_panel(blockB+j2*actual_kc,
-                         rhs.getSubMapper(actual_k2+panelOffset, actual_j2),
-                         panelLength, actualPanelWidth,
-                         actual_kc, panelOffset);
+    pack_rhs(geb, rhs.getSubMapper(actual_k2, IsLower ? 0 : k2), actual_kc, rs);
 
-          // append the triangular part via a temporary buffer
-          for (Index j=0;j<actualPanelWidth;++j)
-          {
-            if (SetDiag)
-              triangularBuffer.coeffRef(j,j) = rhs(actual_j2+j,actual_j2+j);
-            for (Index k=IsLower ? j+1 : 0; IsLower ? k<actualPanelWidth : k<j; ++k)
-              triangularBuffer.coeffRef(k,j) = rhs(actual_j2+k,actual_j2+j);
-          }
+    // pack the triangular part of the rhs padding the unrolled blocks with zeros
+    if (ts > 0) {
+      for (Index j2 = 0; j2 < actual_kc; j2 += SmallPanelWidth) {
+        Index actualPanelWidth = std::min<Index>(actual_kc - j2, SmallPanelWidth);
+        Index actual_j2 = actual_k2 + j2;
+        Index panelOffset = IsLower ? j2 + actualPanelWidth : 0;
+        Index panelLength = IsLower ? actual_kc - j2 - actualPanelWidth : j2;
+        // general part
+        pack_rhs_panel(blockB + j2 * actual_kc, rhs.getSubMapper(actual_k2 + panelOffset, actual_j2), panelLength,
+                       actualPanelWidth, actual_kc, panelOffset);
 
-          pack_rhs_panel(blockB+j2*actual_kc,
-                         RhsMapper(triangularBuffer.data(), triangularBuffer.outerStride()),
-                         actualPanelWidth, actualPanelWidth,
-                         actual_kc, j2);
+        // append the triangular part via a temporary buffer
+        for (Index j = 0; j < actualPanelWidth; ++j) {
+          if (SetDiag) triangularBuffer.coeffRef(j, j) = rhs(actual_j2 + j, actual_j2 + j);
+          for (Index k = IsLower ? j + 1 : 0; IsLower ? k < actualPanelWidth : k < j; ++k)
+            triangularBuffer.coeffRef(k, j) = rhs(actual_j2 + k, actual_j2 + j);
         }
-      }
 
-      for (Index i2=0; i2<rows; i2+=mc)
-      {
-        const Index actual_mc = (std::min)(mc,rows-i2);
-        pack_lhs(blockA, lhs.getSubMapper(i2, actual_k2), actual_kc, actual_mc);
-
-        // triangular kernel
-        if(ts>0)
-        {
-          for (Index j2=0; j2<actual_kc; j2+=SmallPanelWidth)
-          {
-            Index actualPanelWidth = std::min<Index>(actual_kc-j2, SmallPanelWidth);
-            Index panelLength = IsLower ? actual_kc-j2 : j2+actualPanelWidth;
-            Index blockOffset = IsLower ? j2 : 0;
-
-            gebp_kernel(res.getSubMapper(i2, actual_k2 + j2),
-                        blockA, blockB+j2*actual_kc,
-                        actual_mc, panelLength, actualPanelWidth,
-                        alpha,
-                        actual_kc, actual_kc,  // strides
-                        blockOffset, blockOffset);// offsets
-          }
-        }
-        gebp_kernel(res.getSubMapper(i2, IsLower ? 0 : k2),
-                    blockA, geb, actual_mc, actual_kc, rs,
-                    alpha,
-                    -1, -1, 0, 0);
+        pack_rhs_panel(blockB + j2 * actual_kc, RhsMapper(triangularBuffer.data(), triangularBuffer.outerStride()),
+                       actualPanelWidth, actualPanelWidth, actual_kc, j2);
       }
     }
+
+    for (Index i2 = 0; i2 < rows; i2 += mc) {
+      const Index actual_mc = (std::min)(mc, rows - i2);
+      pack_lhs(blockA, lhs.getSubMapper(i2, actual_k2), actual_kc, actual_mc);
+
+      // triangular kernel
+      if (ts > 0) {
+        for (Index j2 = 0; j2 < actual_kc; j2 += SmallPanelWidth) {
+          Index actualPanelWidth = std::min<Index>(actual_kc - j2, SmallPanelWidth);
+          Index panelLength = IsLower ? actual_kc - j2 : j2 + actualPanelWidth;
+          Index blockOffset = IsLower ? j2 : 0;
+
+          gebp_kernel(res.getSubMapper(i2, actual_k2 + j2), blockA, blockB + j2 * actual_kc, actual_mc, panelLength,
+                      actualPanelWidth, alpha, actual_kc, actual_kc,  // strides
+                      blockOffset, blockOffset);                      // offsets
+        }
+      }
+      gebp_kernel(res.getSubMapper(i2, IsLower ? 0 : k2), blockA, geb, actual_mc, actual_kc, rs, alpha, -1, -1, 0, 0);
+    }
   }
+}
 
 /***************************************************************************
-* Wrapper to product_triangular_matrix_matrix
-***************************************************************************/
+ * Wrapper to product_triangular_matrix_matrix
+ ***************************************************************************/
 
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
-template<int Mode, bool LhsIsTriangular, typename Lhs, typename Rhs>
-struct triangular_product_impl<Mode,LhsIsTriangular,Lhs,false,Rhs,false>
-{
-  template<typename Dest> static void run(Dest& dst, const Lhs &a_lhs, const Rhs &a_rhs, const typename Dest::Scalar& alpha)
-  {
-    typedef typename Lhs::Scalar  LhsScalar;
-    typedef typename Rhs::Scalar  RhsScalar;
+template <int Mode, bool LhsIsTriangular, typename Lhs, typename Rhs>
+struct triangular_product_impl<Mode, LhsIsTriangular, Lhs, false, Rhs, false> {
+  template <typename Dest>
+  static void run(Dest& dst, const Lhs& a_lhs, const Rhs& a_rhs, const typename Dest::Scalar& alpha) {
+    typedef typename Lhs::Scalar LhsScalar;
+    typedef typename Rhs::Scalar RhsScalar;
     typedef typename Dest::Scalar Scalar;
-    
+
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
-    typedef typename internal::remove_all<ActualLhsType>::type ActualLhsTypeCleaned;
+    typedef internal::remove_all_t<ActualLhsType> ActualLhsTypeCleaned;
     typedef internal::blas_traits<Rhs> RhsBlasTraits;
     typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-    typedef typename internal::remove_all<ActualRhsType>::type ActualRhsTypeCleaned;
-    
-    typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(a_lhs);
-    typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(a_rhs);
+    typedef internal::remove_all_t<ActualRhsType> ActualRhsTypeCleaned;
+
+    internal::add_const_on_value_type_t<ActualLhsType> lhs = LhsBlasTraits::extract(a_lhs);
+    internal::add_const_on_value_type_t<ActualRhsType> rhs = RhsBlasTraits::extract(a_rhs);
+
+    // Empty product, return early.  Otherwise, we get `nullptr` use errors below when we try to access
+    // coeffRef(0,0).
+    if (lhs.size() == 0 || rhs.size() == 0) {
+      return;
+    }
 
     LhsScalar lhs_alpha = LhsBlasTraits::extractScalarFactor(a_lhs);
     RhsScalar rhs_alpha = RhsBlasTraits::extractScalarFactor(a_rhs);
     Scalar actualAlpha = alpha * lhs_alpha * rhs_alpha;
 
-    typedef internal::gemm_blocking_space<(Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,Scalar,Scalar,
-              Lhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime, Lhs::MaxColsAtCompileTime,4> BlockingType;
+    typedef internal::gemm_blocking_space<(Dest::Flags & RowMajorBit) ? RowMajor : ColMajor, Scalar, Scalar,
+                                          Lhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime,
+                                          Lhs::MaxColsAtCompileTime, 4>
+        BlockingType;
 
-    enum { IsLower = (Mode&Lower) == Lower };
-    Index stripedRows  = ((!LhsIsTriangular) || (IsLower))  ? lhs.rows() : (std::min)(lhs.rows(),lhs.cols());
-    Index stripedCols  = ((LhsIsTriangular)  || (!IsLower)) ? rhs.cols() : (std::min)(rhs.cols(),rhs.rows());
-    Index stripedDepth = LhsIsTriangular ? ((!IsLower) ? lhs.cols() : (std::min)(lhs.cols(),lhs.rows()))
-                                         : ((IsLower)  ? rhs.rows() : (std::min)(rhs.rows(),rhs.cols()));
+    enum { IsLower = (Mode & Lower) == Lower };
+    Index stripedRows = ((!LhsIsTriangular) || (IsLower)) ? lhs.rows() : (std::min)(lhs.rows(), lhs.cols());
+    Index stripedCols = ((LhsIsTriangular) || (!IsLower)) ? rhs.cols() : (std::min)(rhs.cols(), rhs.rows());
+    Index stripedDepth = LhsIsTriangular ? ((!IsLower) ? lhs.cols() : (std::min)(lhs.cols(), lhs.rows()))
+                                         : ((IsLower) ? rhs.rows() : (std::min)(rhs.rows(), rhs.cols()));
 
     BlockingType blocking(stripedRows, stripedCols, stripedDepth, 1, false);
 
-    internal::product_triangular_matrix_matrix<Scalar, Index,
-      Mode, LhsIsTriangular,
-      (internal::traits<ActualLhsTypeCleaned>::Flags&RowMajorBit) ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate,
-      (internal::traits<ActualRhsTypeCleaned>::Flags&RowMajorBit) ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate,
-      (internal::traits<Dest          >::Flags&RowMajorBit) ? RowMajor : ColMajor, Dest::InnerStrideAtCompileTime>
-      ::run(
-        stripedRows, stripedCols, stripedDepth,   // sizes
-        &lhs.coeffRef(0,0), lhs.outerStride(),    // lhs info
-        &rhs.coeffRef(0,0), rhs.outerStride(),    // rhs info
-        &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(),    // result info
-        actualAlpha, blocking
-      );
+    internal::product_triangular_matrix_matrix<
+        Scalar, Index, Mode, LhsIsTriangular,
+        (internal::traits<ActualLhsTypeCleaned>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+        LhsBlasTraits::NeedToConjugate,
+        (internal::traits<ActualRhsTypeCleaned>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+        RhsBlasTraits::NeedToConjugate, (internal::traits<Dest>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+        Dest::InnerStrideAtCompileTime>::run(stripedRows, stripedCols, stripedDepth,                     // sizes
+                                             &lhs.coeffRef(0, 0), lhs.outerStride(),                     // lhs info
+                                             &rhs.coeffRef(0, 0), rhs.outerStride(),                     // rhs info
+                                             &dst.coeffRef(0, 0), dst.innerStride(), dst.outerStride(),  // result info
+                                             actualAlpha, blocking);
 
     // Apply correction if the diagonal is unit and a scalar factor was nested:
-    if ((Mode&UnitDiag)==UnitDiag)
-    {
-      if (LhsIsTriangular && lhs_alpha!=LhsScalar(1))
-      {
-        Index diagSize = (std::min)(lhs.rows(),lhs.cols());
-        dst.topRows(diagSize) -= ((lhs_alpha-LhsScalar(1))*a_rhs).topRows(diagSize);
-      }
-      else if ((!LhsIsTriangular) && rhs_alpha!=RhsScalar(1))
-      {
-        Index diagSize = (std::min)(rhs.rows(),rhs.cols());
-        dst.leftCols(diagSize) -= (rhs_alpha-RhsScalar(1))*a_lhs.leftCols(diagSize);
+    if ((Mode & UnitDiag) == UnitDiag) {
+      if (LhsIsTriangular && !numext::is_exactly_one(lhs_alpha)) {
+        Index diagSize = (std::min)(lhs.rows(), lhs.cols());
+        dst.topRows(diagSize) -= ((lhs_alpha - LhsScalar(1)) * a_rhs).topRows(diagSize);
+      } else if ((!LhsIsTriangular) && !numext::is_exactly_one(rhs_alpha)) {
+        Index diagSize = (std::min)(rhs.rows(), rhs.cols());
+        dst.leftCols(diagSize) -= (rhs_alpha - RhsScalar(1)) * a_lhs.leftCols(diagSize);
       }
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TRIANGULAR_MATRIX_MATRIX_H
+#endif  // EIGEN_TRIANGULAR_MATRIX_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h
index 76bfa15..413f0ee 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h
@@ -10,341 +10,318 @@
 #ifndef EIGEN_TRIANGULARMATRIXVECTOR_H
 #define EIGEN_TRIANGULARMATRIXVECTOR_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int StorageOrder, int Version=Specialized>
+template <typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs,
+          int StorageOrder, int Version = Specialized>
 struct triangular_matrix_vector_product;
 
-template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
-struct triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,ColMajor,Version>
-{
+template <typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
+struct triangular_matrix_vector_product<Index, Mode, LhsScalar, ConjLhs, RhsScalar, ConjRhs, ColMajor, Version> {
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
-  enum {
-    IsLower = ((Mode&Lower)==Lower),
-    HasUnitDiag = (Mode & UnitDiag)==UnitDiag,
-    HasZeroDiag = (Mode & ZeroDiag)==ZeroDiag
-  };
-  static EIGEN_DONT_INLINE  void run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
-                                     const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const RhsScalar& alpha);
+  static constexpr bool IsLower = ((Mode & Lower) == Lower);
+  static constexpr bool HasUnitDiag = (Mode & UnitDiag) == UnitDiag;
+  static constexpr bool HasZeroDiag = (Mode & ZeroDiag) == ZeroDiag;
+  static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, const LhsScalar* lhs_, Index lhsStride,
+                                    const RhsScalar* rhs_, Index rhsIncr, ResScalar* res_, Index resIncr,
+                                    const RhsScalar& alpha);
 };
 
-template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
-EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,ColMajor,Version>
-  ::run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
-        const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const RhsScalar& alpha)
-  {
-    static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
-    Index size = (std::min)(_rows,_cols);
-    Index rows = IsLower ? _rows : (std::min)(_rows,_cols);
-    Index cols = IsLower ? (std::min)(_rows,_cols) : _cols;
+template <typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
+EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index, Mode, LhsScalar, ConjLhs, RhsScalar, ConjRhs, ColMajor,
+                                                        Version>::run(Index _rows, Index _cols, const LhsScalar* lhs_,
+                                                                      Index lhsStride, const RhsScalar* rhs_,
+                                                                      Index rhsIncr, ResScalar* res_, Index resIncr,
+                                                                      const RhsScalar& alpha) {
+  static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
+  Index size = (std::min)(_rows, _cols);
+  Index rows = IsLower ? _rows : (std::min)(_rows, _cols);
+  Index cols = IsLower ? (std::min)(_rows, _cols) : _cols;
 
-    typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > LhsMap;
-    const LhsMap lhs(_lhs,rows,cols,OuterStride<>(lhsStride));
-    typename conj_expr_if<ConjLhs,LhsMap>::type cjLhs(lhs);
+  typedef Map<const Matrix<LhsScalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<> > LhsMap;
+  const LhsMap lhs(lhs_, rows, cols, OuterStride<>(lhsStride));
+  typename conj_expr_if<ConjLhs, LhsMap>::type cjLhs(lhs);
 
-    typedef Map<const Matrix<RhsScalar,Dynamic,1>, 0, InnerStride<> > RhsMap;
-    const RhsMap rhs(_rhs,cols,InnerStride<>(rhsIncr));
-    typename conj_expr_if<ConjRhs,RhsMap>::type cjRhs(rhs);
+  typedef Map<const Matrix<RhsScalar, Dynamic, 1>, 0, InnerStride<> > RhsMap;
+  const RhsMap rhs(rhs_, cols, InnerStride<>(rhsIncr));
+  typename conj_expr_if<ConjRhs, RhsMap>::type cjRhs(rhs);
 
-    typedef Map<Matrix<ResScalar,Dynamic,1> > ResMap;
-    ResMap res(_res,rows);
+  typedef Map<Matrix<ResScalar, Dynamic, 1> > ResMap;
+  ResMap res(res_, rows);
 
-    typedef const_blas_data_mapper<LhsScalar,Index,ColMajor> LhsMapper;
-    typedef const_blas_data_mapper<RhsScalar,Index,RowMajor> RhsMapper;
+  typedef const_blas_data_mapper<LhsScalar, Index, ColMajor> LhsMapper;
+  typedef const_blas_data_mapper<RhsScalar, Index, RowMajor> RhsMapper;
 
-    for (Index pi=0; pi<size; pi+=PanelWidth)
-    {
-      Index actualPanelWidth = (std::min)(PanelWidth, size-pi);
-      for (Index k=0; k<actualPanelWidth; ++k)
-      {
-        Index i = pi + k;
-        Index s = IsLower ? ((HasUnitDiag||HasZeroDiag) ? i+1 : i ) : pi;
-        Index r = IsLower ? actualPanelWidth-k : k+1;
-        if ((!(HasUnitDiag||HasZeroDiag)) || (--r)>0)
-          res.segment(s,r) += (alpha * cjRhs.coeff(i)) * cjLhs.col(i).segment(s,r);
-        if (HasUnitDiag)
-          res.coeffRef(i) += alpha * cjRhs.coeff(i);
-      }
-      Index r = IsLower ? rows - pi - actualPanelWidth : pi;
-      if (r>0)
-      {
-        Index s = IsLower ? pi+actualPanelWidth : 0;
-        general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,ConjLhs,RhsScalar,RhsMapper,ConjRhs,BuiltIn>::run(
-            r, actualPanelWidth,
-            LhsMapper(&lhs.coeffRef(s,pi), lhsStride),
-            RhsMapper(&rhs.coeffRef(pi), rhsIncr),
-            &res.coeffRef(s), resIncr, alpha);
-      }
+  for (Index pi = 0; pi < size; pi += PanelWidth) {
+    Index actualPanelWidth = (std::min)(PanelWidth, size - pi);
+    for (Index k = 0; k < actualPanelWidth; ++k) {
+      Index i = pi + k;
+      Index s = IsLower ? ((HasUnitDiag || HasZeroDiag) ? i + 1 : i) : pi;
+      Index r = IsLower ? actualPanelWidth - k : k + 1;
+      if ((!(HasUnitDiag || HasZeroDiag)) || (--r) > 0)
+        res.segment(s, r) += (alpha * cjRhs.coeff(i)) * cjLhs.col(i).segment(s, r);
+      if (HasUnitDiag) res.coeffRef(i) += alpha * cjRhs.coeff(i);
     }
-    if((!IsLower) && cols>size)
-    {
-      general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,ConjLhs,RhsScalar,RhsMapper,ConjRhs>::run(
-          rows, cols-size,
-          LhsMapper(&lhs.coeffRef(0,size), lhsStride),
-          RhsMapper(&rhs.coeffRef(size), rhsIncr),
-          _res, resIncr, alpha);
+    Index r = IsLower ? rows - pi - actualPanelWidth : pi;
+    if (r > 0) {
+      Index s = IsLower ? pi + actualPanelWidth : 0;
+      general_matrix_vector_product<Index, LhsScalar, LhsMapper, ColMajor, ConjLhs, RhsScalar, RhsMapper, ConjRhs,
+                                    BuiltIn>::run(r, actualPanelWidth, LhsMapper(&lhs.coeffRef(s, pi), lhsStride),
+                                                  RhsMapper(&rhs.coeffRef(pi), rhsIncr), &res.coeffRef(s), resIncr,
+                                                  alpha);
     }
   }
+  if ((!IsLower) && cols > size) {
+    general_matrix_vector_product<Index, LhsScalar, LhsMapper, ColMajor, ConjLhs, RhsScalar, RhsMapper, ConjRhs>::run(
+        rows, cols - size, LhsMapper(&lhs.coeffRef(0, size), lhsStride), RhsMapper(&rhs.coeffRef(size), rhsIncr), res_,
+        resIncr, alpha);
+  }
+}
 
-template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs,int Version>
-struct triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,RowMajor,Version>
-{
+template <typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
+struct triangular_matrix_vector_product<Index, Mode, LhsScalar, ConjLhs, RhsScalar, ConjRhs, RowMajor, Version> {
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
-  enum {
-    IsLower = ((Mode&Lower)==Lower),
-    HasUnitDiag = (Mode & UnitDiag)==UnitDiag,
-    HasZeroDiag = (Mode & ZeroDiag)==ZeroDiag
-  };
-  static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
-                                    const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha);
+  static constexpr bool IsLower = ((Mode & Lower) == Lower);
+  static constexpr bool HasUnitDiag = (Mode & UnitDiag) == UnitDiag;
+  static constexpr bool HasZeroDiag = (Mode & ZeroDiag) == ZeroDiag;
+  static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, const LhsScalar* lhs_, Index lhsStride,
+                                    const RhsScalar* rhs_, Index rhsIncr, ResScalar* res_, Index resIncr,
+                                    const ResScalar& alpha);
 };
 
-template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs,int Version>
-EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,RowMajor,Version>
-  ::run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
-        const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha)
-  {
-    static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
-    Index diagSize = (std::min)(_rows,_cols);
-    Index rows = IsLower ? _rows : diagSize;
-    Index cols = IsLower ? diagSize : _cols;
+template <typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
+EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index, Mode, LhsScalar, ConjLhs, RhsScalar, ConjRhs, RowMajor,
+                                                        Version>::run(Index _rows, Index _cols, const LhsScalar* lhs_,
+                                                                      Index lhsStride, const RhsScalar* rhs_,
+                                                                      Index rhsIncr, ResScalar* res_, Index resIncr,
+                                                                      const ResScalar& alpha) {
+  static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
+  Index diagSize = (std::min)(_rows, _cols);
+  Index rows = IsLower ? _rows : diagSize;
+  Index cols = IsLower ? diagSize : _cols;
 
-    typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,RowMajor>, 0, OuterStride<> > LhsMap;
-    const LhsMap lhs(_lhs,rows,cols,OuterStride<>(lhsStride));
-    typename conj_expr_if<ConjLhs,LhsMap>::type cjLhs(lhs);
+  typedef Map<const Matrix<LhsScalar, Dynamic, Dynamic, RowMajor>, 0, OuterStride<> > LhsMap;
+  const LhsMap lhs(lhs_, rows, cols, OuterStride<>(lhsStride));
+  typename conj_expr_if<ConjLhs, LhsMap>::type cjLhs(lhs);
 
-    typedef Map<const Matrix<RhsScalar,Dynamic,1> > RhsMap;
-    const RhsMap rhs(_rhs,cols);
-    typename conj_expr_if<ConjRhs,RhsMap>::type cjRhs(rhs);
+  typedef Map<const Matrix<RhsScalar, Dynamic, 1> > RhsMap;
+  const RhsMap rhs(rhs_, cols);
+  typename conj_expr_if<ConjRhs, RhsMap>::type cjRhs(rhs);
 
-    typedef Map<Matrix<ResScalar,Dynamic,1>, 0, InnerStride<> > ResMap;
-    ResMap res(_res,rows,InnerStride<>(resIncr));
+  typedef Map<Matrix<ResScalar, Dynamic, 1>, 0, InnerStride<> > ResMap;
+  ResMap res(res_, rows, InnerStride<>(resIncr));
 
-    typedef const_blas_data_mapper<LhsScalar,Index,RowMajor> LhsMapper;
-    typedef const_blas_data_mapper<RhsScalar,Index,RowMajor> RhsMapper;
+  typedef const_blas_data_mapper<LhsScalar, Index, RowMajor> LhsMapper;
+  typedef const_blas_data_mapper<RhsScalar, Index, RowMajor> RhsMapper;
 
-    for (Index pi=0; pi<diagSize; pi+=PanelWidth)
-    {
-      Index actualPanelWidth = (std::min)(PanelWidth, diagSize-pi);
-      for (Index k=0; k<actualPanelWidth; ++k)
-      {
-        Index i = pi + k;
-        Index s = IsLower ? pi  : ((HasUnitDiag||HasZeroDiag) ? i+1 : i);
-        Index r = IsLower ? k+1 : actualPanelWidth-k;
-        if ((!(HasUnitDiag||HasZeroDiag)) || (--r)>0)
-          res.coeffRef(i) += alpha * (cjLhs.row(i).segment(s,r).cwiseProduct(cjRhs.segment(s,r).transpose())).sum();
-        if (HasUnitDiag)
-          res.coeffRef(i) += alpha * cjRhs.coeff(i);
-      }
-      Index r = IsLower ? pi : cols - pi - actualPanelWidth;
-      if (r>0)
-      {
-        Index s = IsLower ? 0 : pi + actualPanelWidth;
-        general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,ConjLhs,RhsScalar,RhsMapper,ConjRhs,BuiltIn>::run(
-            actualPanelWidth, r,
-            LhsMapper(&lhs.coeffRef(pi,s), lhsStride),
-            RhsMapper(&rhs.coeffRef(s), rhsIncr),
-            &res.coeffRef(pi), resIncr, alpha);
-      }
+  for (Index pi = 0; pi < diagSize; pi += PanelWidth) {
+    Index actualPanelWidth = (std::min)(PanelWidth, diagSize - pi);
+    for (Index k = 0; k < actualPanelWidth; ++k) {
+      Index i = pi + k;
+      Index s = IsLower ? pi : ((HasUnitDiag || HasZeroDiag) ? i + 1 : i);
+      Index r = IsLower ? k + 1 : actualPanelWidth - k;
+      if ((!(HasUnitDiag || HasZeroDiag)) || (--r) > 0)
+        res.coeffRef(i) += alpha * (cjLhs.row(i).segment(s, r).cwiseProduct(cjRhs.segment(s, r).transpose())).sum();
+      if (HasUnitDiag) res.coeffRef(i) += alpha * cjRhs.coeff(i);
     }
-    if(IsLower && rows>diagSize)
-    {
-      general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,ConjLhs,RhsScalar,RhsMapper,ConjRhs>::run(
-            rows-diagSize, cols,
-            LhsMapper(&lhs.coeffRef(diagSize,0), lhsStride),
-            RhsMapper(&rhs.coeffRef(0), rhsIncr),
-            &res.coeffRef(diagSize), resIncr, alpha);
+    Index r = IsLower ? pi : cols - pi - actualPanelWidth;
+    if (r > 0) {
+      Index s = IsLower ? 0 : pi + actualPanelWidth;
+      general_matrix_vector_product<Index, LhsScalar, LhsMapper, RowMajor, ConjLhs, RhsScalar, RhsMapper, ConjRhs,
+                                    BuiltIn>::run(actualPanelWidth, r, LhsMapper(&lhs.coeffRef(pi, s), lhsStride),
+                                                  RhsMapper(&rhs.coeffRef(s), rhsIncr), &res.coeffRef(pi), resIncr,
+                                                  alpha);
     }
   }
+  if (IsLower && rows > diagSize) {
+    general_matrix_vector_product<Index, LhsScalar, LhsMapper, RowMajor, ConjLhs, RhsScalar, RhsMapper, ConjRhs>::run(
+        rows - diagSize, cols, LhsMapper(&lhs.coeffRef(diagSize, 0), lhsStride), RhsMapper(&rhs.coeffRef(0), rhsIncr),
+        &res.coeffRef(diagSize), resIncr, alpha);
+  }
+}
 
 /***************************************************************************
-* Wrapper to product_triangular_vector
-***************************************************************************/
+ * Wrapper to product_triangular_vector
+ ***************************************************************************/
 
-template<int Mode,int StorageOrder>
+template <int Mode, int StorageOrder>
 struct trmv_selector;
 
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
-template<int Mode, typename Lhs, typename Rhs>
-struct triangular_product_impl<Mode,true,Lhs,false,Rhs,true>
-{
-  template<typename Dest> static void run(Dest& dst, const Lhs &lhs, const Rhs &rhs, const typename Dest::Scalar& alpha)
-  {
-    eigen_assert(dst.rows()==lhs.rows() && dst.cols()==rhs.cols());
-  
-    internal::trmv_selector<Mode,(int(internal::traits<Lhs>::Flags)&RowMajorBit) ? RowMajor : ColMajor>::run(lhs, rhs, dst, alpha);
+template <int Mode, typename Lhs, typename Rhs>
+struct triangular_product_impl<Mode, true, Lhs, false, Rhs, true> {
+  template <typename Dest>
+  static void run(Dest& dst, const Lhs& lhs, const Rhs& rhs, const typename Dest::Scalar& alpha) {
+    eigen_assert(dst.rows() == lhs.rows() && dst.cols() == rhs.cols());
+
+    internal::trmv_selector<Mode, (int(internal::traits<Lhs>::Flags) & RowMajorBit) ? RowMajor : ColMajor>::run(
+        lhs, rhs, dst, alpha);
   }
 };
 
-template<int Mode, typename Lhs, typename Rhs>
-struct triangular_product_impl<Mode,false,Lhs,true,Rhs,false>
-{
-  template<typename Dest> static void run(Dest& dst, const Lhs &lhs, const Rhs &rhs, const typename Dest::Scalar& alpha)
-  {
-    eigen_assert(dst.rows()==lhs.rows() && dst.cols()==rhs.cols());
+template <int Mode, typename Lhs, typename Rhs>
+struct triangular_product_impl<Mode, false, Lhs, true, Rhs, false> {
+  template <typename Dest>
+  static void run(Dest& dst, const Lhs& lhs, const Rhs& rhs, const typename Dest::Scalar& alpha) {
+    eigen_assert(dst.rows() == lhs.rows() && dst.cols() == rhs.cols());
 
     Transpose<Dest> dstT(dst);
-    internal::trmv_selector<(Mode & (UnitDiag|ZeroDiag)) | ((Mode & Lower) ? Upper : Lower),
-                            (int(internal::traits<Rhs>::Flags)&RowMajorBit) ? ColMajor : RowMajor>
-            ::run(rhs.transpose(),lhs.transpose(), dstT, alpha);
+    internal::trmv_selector<(Mode & (UnitDiag | ZeroDiag)) | ((Mode & Lower) ? Upper : Lower),
+                            (int(internal::traits<Rhs>::Flags) & RowMajorBit) ? ColMajor
+                                                                              : RowMajor>::run(rhs.transpose(),
+                                                                                               lhs.transpose(), dstT,
+                                                                                               alpha);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
 // TODO: find a way to factorize this piece of code with gemv_selector since the logic is exactly the same.
-  
-template<int Mode> struct trmv_selector<Mode,ColMajor>
-{
-  template<typename Lhs, typename Rhs, typename Dest>
-  static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha)
-  {
-    typedef typename Lhs::Scalar      LhsScalar;
-    typedef typename Rhs::Scalar      RhsScalar;
-    typedef typename Dest::Scalar     ResScalar;
-    typedef typename Dest::RealScalar RealScalar;
-    
+
+template <int Mode>
+struct trmv_selector<Mode, ColMajor> {
+  template <typename Lhs, typename Rhs, typename Dest>
+  static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) {
+    typedef typename Lhs::Scalar LhsScalar;
+    typedef typename Rhs::Scalar RhsScalar;
+    typedef typename Dest::Scalar ResScalar;
+
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
     typedef internal::blas_traits<Rhs> RhsBlasTraits;
     typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-    
-    typedef Map<Matrix<ResScalar,Dynamic,1>, EIGEN_PLAIN_ENUM_MIN(AlignedMax,internal::packet_traits<ResScalar>::size)> MappedDest;
+    constexpr int Alignment = (std::min)(int(AlignedMax), int(internal::packet_traits<ResScalar>::size));
 
-    typename internal::add_const_on_value_type<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(lhs);
-    typename internal::add_const_on_value_type<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(rhs);
+    typedef Map<Matrix<ResScalar, Dynamic, 1>, Alignment> MappedDest;
+
+    add_const_on_value_type_t<ActualLhsType> actualLhs = LhsBlasTraits::extract(lhs);
+    add_const_on_value_type_t<ActualRhsType> actualRhs = RhsBlasTraits::extract(rhs);
 
     LhsScalar lhs_alpha = LhsBlasTraits::extractScalarFactor(lhs);
     RhsScalar rhs_alpha = RhsBlasTraits::extractScalarFactor(rhs);
     ResScalar actualAlpha = alpha * lhs_alpha * rhs_alpha;
 
-    enum {
-      // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
-      // on, the other hand it is good for the cache to pack the vector anyways...
-      EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime==1,
-      ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex),
-      MightCannotUseDest = (Dest::InnerStrideAtCompileTime!=1) || ComplexByReal
-    };
+    // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
+    // on, the other hand it is good for the cache to pack the vector anyways...
+    constexpr bool EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime == 1;
+    constexpr bool ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex);
+    constexpr bool MightCannotUseDest = (Dest::InnerStrideAtCompileTime != 1) || ComplexByReal;
 
-    gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
+    gemv_static_vector_if<ResScalar, Dest::SizeAtCompileTime, Dest::MaxSizeAtCompileTime, MightCannotUseDest>
+        static_dest;
 
-    bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0));
+    bool alphaIsCompatible = (!ComplexByReal) || numext::is_exactly_zero(numext::imag(actualAlpha));
     bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible;
 
-    RhsScalar compatibleAlpha = get_factor<ResScalar,RhsScalar>::run(actualAlpha);
+    RhsScalar compatibleAlpha = get_factor<ResScalar, RhsScalar>::run(actualAlpha);
 
-    ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(),
+    ei_declare_aligned_stack_constructed_variable(ResScalar, actualDestPtr, dest.size(),
                                                   evalToDest ? dest.data() : static_dest.data());
 
-    if(!evalToDest)
-    {
-      #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+    if (!evalToDest) {
+#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
       Index size = dest.size();
       EIGEN_DENSE_STORAGE_CTOR_PLUGIN
-      #endif
-      if(!alphaIsCompatible)
-      {
+#endif
+      if (!alphaIsCompatible) {
         MappedDest(actualDestPtr, dest.size()).setZero();
         compatibleAlpha = RhsScalar(1);
-      }
-      else
+      } else
         MappedDest(actualDestPtr, dest.size()) = dest;
     }
 
-    internal::triangular_matrix_vector_product
-      <Index,Mode,
-       LhsScalar, LhsBlasTraits::NeedToConjugate,
-       RhsScalar, RhsBlasTraits::NeedToConjugate,
-       ColMajor>
-      ::run(actualLhs.rows(),actualLhs.cols(),
-            actualLhs.data(),actualLhs.outerStride(),
-            actualRhs.data(),actualRhs.innerStride(),
-            actualDestPtr,1,compatibleAlpha);
+    internal::triangular_matrix_vector_product<Index, Mode, LhsScalar, LhsBlasTraits::NeedToConjugate, RhsScalar,
+                                               RhsBlasTraits::NeedToConjugate, ColMajor>::run(actualLhs.rows(),
+                                                                                              actualLhs.cols(),
+                                                                                              actualLhs.data(),
+                                                                                              actualLhs.outerStride(),
+                                                                                              actualRhs.data(),
+                                                                                              actualRhs.innerStride(),
+                                                                                              actualDestPtr, 1,
+                                                                                              compatibleAlpha);
 
-    if (!evalToDest)
-    {
-      if(!alphaIsCompatible)
+    if (!evalToDest) {
+      if (!alphaIsCompatible)
         dest += actualAlpha * MappedDest(actualDestPtr, dest.size());
       else
         dest = MappedDest(actualDestPtr, dest.size());
     }
 
-    if ( ((Mode&UnitDiag)==UnitDiag) && (lhs_alpha!=LhsScalar(1)) )
-    {
-      Index diagSize = (std::min)(lhs.rows(),lhs.cols());
-      dest.head(diagSize) -= (lhs_alpha-LhsScalar(1))*rhs.head(diagSize);
+    if (((Mode & UnitDiag) == UnitDiag) && !numext::is_exactly_one(lhs_alpha)) {
+      Index diagSize = (std::min)(lhs.rows(), lhs.cols());
+      dest.head(diagSize) -= (lhs_alpha - LhsScalar(1)) * rhs.head(diagSize);
     }
   }
 };
 
-template<int Mode> struct trmv_selector<Mode,RowMajor>
-{
-  template<typename Lhs, typename Rhs, typename Dest>
-  static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha)
-  {
-    typedef typename Lhs::Scalar      LhsScalar;
-    typedef typename Rhs::Scalar      RhsScalar;
-    typedef typename Dest::Scalar     ResScalar;
-    
+template <int Mode>
+struct trmv_selector<Mode, RowMajor> {
+  template <typename Lhs, typename Rhs, typename Dest>
+  static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) {
+    typedef typename Lhs::Scalar LhsScalar;
+    typedef typename Rhs::Scalar RhsScalar;
+    typedef typename Dest::Scalar ResScalar;
+
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
     typedef internal::blas_traits<Rhs> RhsBlasTraits;
     typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType;
-    typedef typename internal::remove_all<ActualRhsType>::type ActualRhsTypeCleaned;
+    typedef internal::remove_all_t<ActualRhsType> ActualRhsTypeCleaned;
 
-    typename add_const<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(lhs);
-    typename add_const<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(rhs);
+    std::add_const_t<ActualLhsType> actualLhs = LhsBlasTraits::extract(lhs);
+    std::add_const_t<ActualRhsType> actualRhs = RhsBlasTraits::extract(rhs);
 
     LhsScalar lhs_alpha = LhsBlasTraits::extractScalarFactor(lhs);
     RhsScalar rhs_alpha = RhsBlasTraits::extractScalarFactor(rhs);
     ResScalar actualAlpha = alpha * lhs_alpha * rhs_alpha;
 
-    enum {
-      DirectlyUseRhs = ActualRhsTypeCleaned::InnerStrideAtCompileTime==1
-    };
+    constexpr bool DirectlyUseRhs = ActualRhsTypeCleaned::InnerStrideAtCompileTime == 1;
 
-    gemv_static_vector_if<RhsScalar,ActualRhsTypeCleaned::SizeAtCompileTime,ActualRhsTypeCleaned::MaxSizeAtCompileTime,!DirectlyUseRhs> static_rhs;
+    gemv_static_vector_if<RhsScalar, ActualRhsTypeCleaned::SizeAtCompileTime,
+                          ActualRhsTypeCleaned::MaxSizeAtCompileTime, !DirectlyUseRhs>
+        static_rhs;
 
-    ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,actualRhs.size(),
+    ei_declare_aligned_stack_constructed_variable(
+        RhsScalar, actualRhsPtr, actualRhs.size(),
         DirectlyUseRhs ? const_cast<RhsScalar*>(actualRhs.data()) : static_rhs.data());
 
-    if(!DirectlyUseRhs)
-    {
-      #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+    if (!DirectlyUseRhs) {
+#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
       Index size = actualRhs.size();
       EIGEN_DENSE_STORAGE_CTOR_PLUGIN
-      #endif
+#endif
       Map<typename ActualRhsTypeCleaned::PlainObject>(actualRhsPtr, actualRhs.size()) = actualRhs;
     }
 
-    internal::triangular_matrix_vector_product
-      <Index,Mode,
-       LhsScalar, LhsBlasTraits::NeedToConjugate,
-       RhsScalar, RhsBlasTraits::NeedToConjugate,
-       RowMajor>
-      ::run(actualLhs.rows(),actualLhs.cols(),
-            actualLhs.data(),actualLhs.outerStride(),
-            actualRhsPtr,1,
-            dest.data(),dest.innerStride(),
-            actualAlpha);
+    internal::triangular_matrix_vector_product<Index, Mode, LhsScalar, LhsBlasTraits::NeedToConjugate, RhsScalar,
+                                               RhsBlasTraits::NeedToConjugate, RowMajor>::run(actualLhs.rows(),
+                                                                                              actualLhs.cols(),
+                                                                                              actualLhs.data(),
+                                                                                              actualLhs.outerStride(),
+                                                                                              actualRhsPtr, 1,
+                                                                                              dest.data(),
+                                                                                              dest.innerStride(),
+                                                                                              actualAlpha);
 
-    if ( ((Mode&UnitDiag)==UnitDiag) && (lhs_alpha!=LhsScalar(1)) )
-    {
-      Index diagSize = (std::min)(lhs.rows(),lhs.cols());
-      dest.head(diagSize) -= (lhs_alpha-LhsScalar(1))*rhs.head(diagSize);
+    if (((Mode & UnitDiag) == UnitDiag) && !numext::is_exactly_one(lhs_alpha)) {
+      Index diagSize = (std::min)(lhs.rows(), lhs.cols());
+      dest.head(diagSize) -= (lhs_alpha - LhsScalar(1)) * rhs.head(diagSize);
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TRIANGULARMATRIXVECTOR_H
+#endif  // EIGEN_TRIANGULARMATRIXVECTOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h
index 6d879ba..f9b2ad0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h
@@ -2,6 +2,7 @@
 // for linear algebra.
 //
 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Modifications Copyright (C) 2022 Intel Corporation
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -10,328 +11,378 @@
 #ifndef EIGEN_TRIANGULAR_SOLVER_MATRIX_H
 #define EIGEN_TRIANGULAR_SOLVER_MATRIX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride,
+          bool Specialized>
+struct trsmKernelL {
+  // Generic Implementation of triangular solve for triangular matrix on left and multiple rhs.
+  // Handles non-packed matrices.
+  static void kernel(Index size, Index otherSize, const Scalar* _tri, Index triStride, Scalar* _other, Index otherIncr,
+                     Index otherStride);
+};
+
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride,
+          bool Specialized>
+struct trsmKernelR {
+  // Generic Implementation of triangular solve for triangular matrix on right and multiple lhs.
+  // Handles non-packed matrices.
+  static void kernel(Index size, Index otherSize, const Scalar* _tri, Index triStride, Scalar* _other, Index otherIncr,
+                     Index otherStride);
+};
+
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride,
+          bool Specialized>
+EIGEN_STRONG_INLINE void trsmKernelL<Scalar, Index, Mode, Conjugate, TriStorageOrder, OtherInnerStride,
+                                     Specialized>::kernel(Index size, Index otherSize, const Scalar* _tri,
+                                                          Index triStride, Scalar* _other, Index otherIncr,
+                                                          Index otherStride) {
+  typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> TriMapper;
+  typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> OtherMapper;
+  TriMapper tri(_tri, triStride);
+  OtherMapper other(_other, otherStride, otherIncr);
+
+  enum { IsLower = (Mode & Lower) == Lower };
+  conj_if<Conjugate> conj;
+
+  // tr solve
+  for (Index k = 0; k < size; ++k) {
+    // TODO write a small kernel handling this (can be shared with trsv)
+    Index i = IsLower ? k : -k - 1;
+    Index rs = size - k - 1;  // remaining size
+    Index s = TriStorageOrder == RowMajor ? (IsLower ? 0 : i + 1) : IsLower ? i + 1 : i - rs;
+
+    Scalar a = (Mode & UnitDiag) ? Scalar(1) : Scalar(1) / conj(tri(i, i));
+    for (Index j = 0; j < otherSize; ++j) {
+      if (TriStorageOrder == RowMajor) {
+        Scalar b(0);
+        const Scalar* l = &tri(i, s);
+        typename OtherMapper::LinearMapper r = other.getLinearMapper(s, j);
+        for (Index i3 = 0; i3 < k; ++i3) b += conj(l[i3]) * r(i3);
+
+        other(i, j) = (other(i, j) - b) * a;
+      } else {
+        Scalar& otherij = other(i, j);
+        otherij *= a;
+        Scalar b = otherij;
+        typename OtherMapper::LinearMapper r = other.getLinearMapper(s, j);
+        typename TriMapper::LinearMapper l = tri.getLinearMapper(s, i);
+        for (Index i3 = 0; i3 < rs; ++i3) r(i3) -= b * conj(l(i3));
+      }
+    }
+  }
+}
+
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride,
+          bool Specialized>
+EIGEN_STRONG_INLINE void trsmKernelR<Scalar, Index, Mode, Conjugate, TriStorageOrder, OtherInnerStride,
+                                     Specialized>::kernel(Index size, Index otherSize, const Scalar* _tri,
+                                                          Index triStride, Scalar* _other, Index otherIncr,
+                                                          Index otherStride) {
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> LhsMapper;
+  typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> RhsMapper;
+  LhsMapper lhs(_other, otherStride, otherIncr);
+  RhsMapper rhs(_tri, triStride);
+
+  enum { RhsStorageOrder = TriStorageOrder, IsLower = (Mode & Lower) == Lower };
+  conj_if<Conjugate> conj;
+
+  for (Index k = 0; k < size; ++k) {
+    Index j = IsLower ? size - k - 1 : k;
+
+    typename LhsMapper::LinearMapper r = lhs.getLinearMapper(0, j);
+    for (Index k3 = 0; k3 < k; ++k3) {
+      Scalar b = conj(rhs(IsLower ? j + 1 + k3 : k3, j));
+      typename LhsMapper::LinearMapper a = lhs.getLinearMapper(0, IsLower ? j + 1 + k3 : k3);
+      for (Index i = 0; i < otherSize; ++i) r(i) -= a(i) * b;
+    }
+    if ((Mode & UnitDiag) == 0) {
+      Scalar inv_rjj = RealScalar(1) / conj(rhs(j, j));
+      for (Index i = 0; i < otherSize; ++i) r(i) *= inv_rjj;
+    }
+  }
+}
+
 // if the rhs is row major, let's transpose the product
-template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
-struct triangular_solve_matrix<Scalar,Index,Side,Mode,Conjugate,TriStorageOrder,RowMajor,OtherInnerStride>
-{
-  static void run(
-    Index size, Index cols,
-    const Scalar*  tri, Index triStride,
-    Scalar* _other, Index otherIncr, Index otherStride,
-    level3_blocking<Scalar,Scalar>& blocking)
-  {
+template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder,
+          int OtherInnerStride>
+struct triangular_solve_matrix<Scalar, Index, Side, Mode, Conjugate, TriStorageOrder, RowMajor, OtherInnerStride> {
+  static void run(Index size, Index cols, const Scalar* tri, Index triStride, Scalar* _other, Index otherIncr,
+                  Index otherStride, level3_blocking<Scalar, Scalar>& blocking) {
     triangular_solve_matrix<
-      Scalar, Index, Side==OnTheLeft?OnTheRight:OnTheLeft,
-      (Mode&UnitDiag) | ((Mode&Upper) ? Lower : Upper),
-      NumTraits<Scalar>::IsComplex && Conjugate,
-      TriStorageOrder==RowMajor ? ColMajor : RowMajor, ColMajor, OtherInnerStride>
-      ::run(size, cols, tri, triStride, _other, otherIncr, otherStride, blocking);
+        Scalar, Index, Side == OnTheLeft ? OnTheRight : OnTheLeft, (Mode & UnitDiag) | ((Mode & Upper) ? Lower : Upper),
+        NumTraits<Scalar>::IsComplex && Conjugate, TriStorageOrder == RowMajor ? ColMajor : RowMajor, ColMajor,
+        OtherInnerStride>::run(size, cols, tri, triStride, _other, otherIncr, otherStride, blocking);
   }
 };
 
 /* Optimized triangular solver with multiple right hand side and the triangular matrix on the left
  */
-template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder,int OtherInnerStride>
-struct triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>
-{
-  static EIGEN_DONT_INLINE void run(
-    Index size, Index otherSize,
-    const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherIncr, Index otherStride,
-    level3_blocking<Scalar,Scalar>& blocking);
-};
 template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
-EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>::run(
-    Index size, Index otherSize,
-    const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherIncr, Index otherStride,
-    level3_blocking<Scalar,Scalar>& blocking)
-  {
-    Index cols = otherSize;
+struct triangular_solve_matrix<Scalar, Index, OnTheLeft, Mode, Conjugate, TriStorageOrder, ColMajor, OtherInnerStride> {
+  static EIGEN_DONT_INLINE void run(Index size, Index otherSize, const Scalar* _tri, Index triStride, Scalar* _other,
+                                    Index otherIncr, Index otherStride, level3_blocking<Scalar, Scalar>& blocking);
+};
 
-    typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> TriMapper;
-    typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> OtherMapper;
-    TriMapper tri(_tri, triStride);
-    OtherMapper other(_other, otherStride, otherIncr);
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
+EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar, Index, OnTheLeft, Mode, Conjugate, TriStorageOrder, ColMajor,
+                                               OtherInnerStride>::run(Index size, Index otherSize, const Scalar* _tri,
+                                                                      Index triStride, Scalar* _other, Index otherIncr,
+                                                                      Index otherStride,
+                                                                      level3_blocking<Scalar, Scalar>& blocking) {
+  Index cols = otherSize;
 
-    typedef gebp_traits<Scalar,Scalar> Traits;
+  std::ptrdiff_t l1, l2, l3;
+  manage_caching_sizes(GetAction, &l1, &l2, &l3);
 
-    enum {
-      SmallPanelWidth   = EIGEN_PLAIN_ENUM_MAX(Traits::mr,Traits::nr),
-      IsLower = (Mode&Lower) == Lower
-    };
+#if defined(EIGEN_VECTORIZE_AVX512) && EIGEN_USE_AVX512_TRSM_L_KERNELS && EIGEN_ENABLE_AVX512_NOCOPY_TRSM_L_CUTOFFS
+  EIGEN_IF_CONSTEXPR(
+      (OtherInnerStride == 1 && (std::is_same<Scalar, float>::value || std::is_same<Scalar, double>::value))) {
+    // Very rough cutoffs to determine when to call trsm w/o packing
+    // For small problem sizes trsmKernel compiled with clang is generally faster.
+    // TODO: Investigate better heuristics for cutoffs.
+    double L2Cap = 0.5;  // 50% of L2 size
+    if (size < avx512_trsm_cutoff<Scalar>(l2, cols, L2Cap)) {
+      trsmKernelL<Scalar, Index, Mode, Conjugate, TriStorageOrder, 1, /*Specialized=*/true>::kernel(
+          size, cols, _tri, triStride, _other, 1, otherStride);
+      return;
+    }
+  }
+#endif
 
-    Index kc = blocking.kc();                   // cache block size along the K direction
-    Index mc = (std::min)(size,blocking.mc());  // cache block size along the M direction
+  typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> TriMapper;
+  typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> OtherMapper;
+  TriMapper tri(_tri, triStride);
+  OtherMapper other(_other, otherStride, otherIncr);
 
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*cols;
+  typedef gebp_traits<Scalar, Scalar> Traits;
 
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+  enum { SmallPanelWidth = plain_enum_max(Traits::mr, Traits::nr), IsLower = (Mode & Lower) == Lower };
 
-    conj_if<Conjugate> conj;
-    gebp_kernel<Scalar, Scalar, Index, OtherMapper, Traits::mr, Traits::nr, Conjugate, false> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, TriMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, TriStorageOrder> pack_lhs;
-    gemm_pack_rhs<Scalar, Index, OtherMapper, Traits::nr, ColMajor, false, true> pack_rhs;
+  Index kc = blocking.kc();                    // cache block size along the K direction
+  Index mc = (std::min)(size, blocking.mc());  // cache block size along the M direction
 
-    // the goal here is to subdivise the Rhs panels such that we keep some cache
-    // coherence when accessing the rhs elements
-    std::ptrdiff_t l1, l2, l3;
-    manage_caching_sizes(GetAction, &l1, &l2, &l3);
-    Index subcols = cols>0 ? l2/(4 * sizeof(Scalar) * std::max<Index>(otherStride,size)) : 0;
-    subcols = std::max<Index>((subcols/Traits::nr)*Traits::nr, Traits::nr);
+  std::size_t sizeA = kc * mc;
+  std::size_t sizeB = kc * cols;
 
-    for(Index k2=IsLower ? 0 : size;
-        IsLower ? k2<size : k2>0;
-        IsLower ? k2+=kc : k2-=kc)
-    {
-      const Index actual_kc = (std::min)(IsLower ? size-k2 : k2, kc);
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
-      // We have selected and packed a big horizontal panel R1 of rhs. Let B be the packed copy of this panel,
-      // and R2 the remaining part of rhs. The corresponding vertical panel of lhs is split into
-      // A11 (the triangular part) and A21 the remaining rectangular part.
-      // Then the high level algorithm is:
-      //  - B = R1                    => general block copy (done during the next step)
-      //  - R1 = A11^-1 B             => tricky part
-      //  - update B from the new R1  => actually this has to be performed continuously during the above step
-      //  - R2 -= A21 * B             => GEPP
+  gebp_kernel<Scalar, Scalar, Index, OtherMapper, Traits::mr, Traits::nr, Conjugate, false> gebp_kernel;
+  gemm_pack_lhs<Scalar, Index, TriMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing,
+                TriStorageOrder>
+      pack_lhs;
+  gemm_pack_rhs<Scalar, Index, OtherMapper, Traits::nr, ColMajor, false, true> pack_rhs;
 
-      // The tricky part: compute R1 = A11^-1 B while updating B from R1
-      // The idea is to split A11 into multiple small vertical panels.
-      // Each panel can be split into a small triangular part T1k which is processed without optimization,
-      // and the remaining small part T2k which is processed using gebp with appropriate block strides
-      for(Index j2=0; j2<cols; j2+=subcols)
-      {
-        Index actual_cols = (std::min)(cols-j2,subcols);
-        // for each small vertical panels [T1k^T, T2k^T]^T of lhs
-        for (Index k1=0; k1<actual_kc; k1+=SmallPanelWidth)
+  // the goal here is to subdivise the Rhs panels such that we keep some cache
+  // coherence when accessing the rhs elements
+  Index subcols = cols > 0 ? l2 / (4 * sizeof(Scalar) * std::max<Index>(otherStride, size)) : 0;
+  subcols = std::max<Index>((subcols / Traits::nr) * Traits::nr, Traits::nr);
+
+  for (Index k2 = IsLower ? 0 : size; IsLower ? k2 < size : k2 > 0; IsLower ? k2 += kc : k2 -= kc) {
+    const Index actual_kc = (std::min)(IsLower ? size - k2 : k2, kc);
+
+    // We have selected and packed a big horizontal panel R1 of rhs. Let B be the packed copy of this panel,
+    // and R2 the remaining part of rhs. The corresponding vertical panel of lhs is split into
+    // A11 (the triangular part) and A21 the remaining rectangular part.
+    // Then the high level algorithm is:
+    //  - B = R1                    => general block copy (done during the next step)
+    //  - R1 = A11^-1 B             => tricky part
+    //  - update B from the new R1  => actually this has to be performed continuously during the above step
+    //  - R2 -= A21 * B             => GEPP
+
+    // The tricky part: compute R1 = A11^-1 B while updating B from R1
+    // The idea is to split A11 into multiple small vertical panels.
+    // Each panel can be split into a small triangular part T1k which is processed without optimization,
+    // and the remaining small part T2k which is processed using gebp with appropriate block strides
+    for (Index j2 = 0; j2 < cols; j2 += subcols) {
+      Index actual_cols = (std::min)(cols - j2, subcols);
+      // for each small vertical panels [T1k^T, T2k^T]^T of lhs
+      for (Index k1 = 0; k1 < actual_kc; k1 += SmallPanelWidth) {
+        Index actualPanelWidth = std::min<Index>(actual_kc - k1, SmallPanelWidth);
+        // tr solve
         {
-          Index actualPanelWidth = std::min<Index>(actual_kc-k1, SmallPanelWidth);
-          // tr solve
-          for (Index k=0; k<actualPanelWidth; ++k)
-          {
-            // TODO write a small kernel handling this (can be shared with trsv)
-            Index i  = IsLower ? k2+k1+k : k2-k1-k-1;
-            Index rs = actualPanelWidth - k - 1; // remaining size
-            Index s  = TriStorageOrder==RowMajor ? (IsLower ? k2+k1 : i+1)
-                                                 :  IsLower ? i+1 : i-rs;
-
-            Scalar a = (Mode & UnitDiag) ? Scalar(1) : Scalar(1)/conj(tri(i,i));
-            for (Index j=j2; j<j2+actual_cols; ++j)
-            {
-              if (TriStorageOrder==RowMajor)
-              {
-                Scalar b(0);
-                const Scalar* l = &tri(i,s);
-                typename OtherMapper::LinearMapper r = other.getLinearMapper(s,j);
-                for (Index i3=0; i3<k; ++i3)
-                  b += conj(l[i3]) * r(i3);
-
-                other(i,j) = (other(i,j) - b)*a;
-              }
-              else
-              {
-                Scalar& otherij = other(i,j);
-                otherij *= a;
-                Scalar b = otherij;
-                typename OtherMapper::LinearMapper r = other.getLinearMapper(s,j);
-                typename TriMapper::LinearMapper l = tri.getLinearMapper(s,i);
-                for (Index i3=0;i3<rs;++i3)
-                  r(i3) -= b * conj(l(i3));
-              }
-            }
+          Index i = IsLower ? k2 + k1 : k2 - k1;
+#if defined(EIGEN_VECTORIZE_AVX512) && EIGEN_USE_AVX512_TRSM_L_KERNELS
+          EIGEN_IF_CONSTEXPR(
+              (OtherInnerStride == 1 && (std::is_same<Scalar, float>::value || std::is_same<Scalar, double>::value))) {
+            i = IsLower ? k2 + k1 : k2 - k1 - actualPanelWidth;
           }
+#endif
+          trsmKernelL<Scalar, Index, Mode, Conjugate, TriStorageOrder, OtherInnerStride, /*Specialized=*/true>::kernel(
+              actualPanelWidth, actual_cols, _tri + i + (i)*triStride, triStride,
+              _other + i * OtherInnerStride + j2 * otherStride, otherIncr, otherStride);
+        }
 
-          Index lengthTarget = actual_kc-k1-actualPanelWidth;
-          Index startBlock   = IsLower ? k2+k1 : k2-k1-actualPanelWidth;
-          Index blockBOffset = IsLower ? k1 : lengthTarget;
+        Index lengthTarget = actual_kc - k1 - actualPanelWidth;
+        Index startBlock = IsLower ? k2 + k1 : k2 - k1 - actualPanelWidth;
+        Index blockBOffset = IsLower ? k1 : lengthTarget;
 
-          // update the respective rows of B from other
-          pack_rhs(blockB+actual_kc*j2, other.getSubMapper(startBlock,j2), actualPanelWidth, actual_cols, actual_kc, blockBOffset);
+        // update the respective rows of B from other
+        pack_rhs(blockB + actual_kc * j2, other.getSubMapper(startBlock, j2), actualPanelWidth, actual_cols, actual_kc,
+                 blockBOffset);
 
-          // GEBP
-          if (lengthTarget>0)
-          {
-            Index startTarget  = IsLower ? k2+k1+actualPanelWidth : k2-actual_kc;
+        // GEBP
+        if (lengthTarget > 0) {
+          Index startTarget = IsLower ? k2 + k1 + actualPanelWidth : k2 - actual_kc;
 
-            pack_lhs(blockA, tri.getSubMapper(startTarget,startBlock), actualPanelWidth, lengthTarget);
+          pack_lhs(blockA, tri.getSubMapper(startTarget, startBlock), actualPanelWidth, lengthTarget);
 
-            gebp_kernel(other.getSubMapper(startTarget,j2), blockA, blockB+actual_kc*j2, lengthTarget, actualPanelWidth, actual_cols, Scalar(-1),
-                        actualPanelWidth, actual_kc, 0, blockBOffset);
-          }
+          gebp_kernel(other.getSubMapper(startTarget, j2), blockA, blockB + actual_kc * j2, lengthTarget,
+                      actualPanelWidth, actual_cols, Scalar(-1), actualPanelWidth, actual_kc, 0, blockBOffset);
         }
       }
-      
-      // R2 -= A21 * B => GEPP
-      {
-        Index start = IsLower ? k2+kc : 0;
-        Index end   = IsLower ? size : k2-kc;
-        for(Index i2=start; i2<end; i2+=mc)
-        {
-          const Index actual_mc = (std::min)(mc,end-i2);
-          if (actual_mc>0)
-          {
-            pack_lhs(blockA, tri.getSubMapper(i2, IsLower ? k2 : k2-kc), actual_kc, actual_mc);
+    }
 
-            gebp_kernel(other.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, Scalar(-1), -1, -1, 0, 0);
-          }
+    // R2 -= A21 * B => GEPP
+    {
+      Index start = IsLower ? k2 + kc : 0;
+      Index end = IsLower ? size : k2 - kc;
+      for (Index i2 = start; i2 < end; i2 += mc) {
+        const Index actual_mc = (std::min)(mc, end - i2);
+        if (actual_mc > 0) {
+          pack_lhs(blockA, tri.getSubMapper(i2, IsLower ? k2 : k2 - kc), actual_kc, actual_mc);
+
+          gebp_kernel(other.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, Scalar(-1), -1, -1, 0, 0);
         }
       }
     }
   }
+}
 
 /* Optimized triangular solver with multiple left hand sides and the triangular matrix on the right
  */
 template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
-struct triangular_solve_matrix<Scalar,Index,OnTheRight,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>
-{
-  static EIGEN_DONT_INLINE void run(
-    Index size, Index otherSize,
-    const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherIncr, Index otherStride,
-    level3_blocking<Scalar,Scalar>& blocking);
+struct triangular_solve_matrix<Scalar, Index, OnTheRight, Mode, Conjugate, TriStorageOrder, ColMajor,
+                               OtherInnerStride> {
+  static EIGEN_DONT_INLINE void run(Index size, Index otherSize, const Scalar* _tri, Index triStride, Scalar* _other,
+                                    Index otherIncr, Index otherStride, level3_blocking<Scalar, Scalar>& blocking);
 };
+
 template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
-EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheRight,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>::run(
-    Index size, Index otherSize,
-    const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherIncr, Index otherStride,
-    level3_blocking<Scalar,Scalar>& blocking)
-  {
-    Index rows = otherSize;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar, Index, OnTheRight, Mode, Conjugate, TriStorageOrder, ColMajor,
+                                               OtherInnerStride>::run(Index size, Index otherSize, const Scalar* _tri,
+                                                                      Index triStride, Scalar* _other, Index otherIncr,
+                                                                      Index otherStride,
+                                                                      level3_blocking<Scalar, Scalar>& blocking) {
+  Index rows = otherSize;
 
-    typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> LhsMapper;
-    typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> RhsMapper;
-    LhsMapper lhs(_other, otherStride, otherIncr);
-    RhsMapper rhs(_tri, triStride);
-
-    typedef gebp_traits<Scalar,Scalar> Traits;
-    enum {
-      RhsStorageOrder   = TriStorageOrder,
-      SmallPanelWidth   = EIGEN_PLAIN_ENUM_MAX(Traits::mr,Traits::nr),
-      IsLower = (Mode&Lower) == Lower
-    };
-
-    Index kc = blocking.kc();                   // cache block size along the K direction
-    Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
-
-    std::size_t sizeA = kc*mc;
-    std::size_t sizeB = kc*size;
-
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
-
-    conj_if<Conjugate> conj;
-    gebp_kernel<Scalar, Scalar, Index, LhsMapper, Traits::mr, Traits::nr, false, Conjugate> gebp_kernel;
-    gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
-    gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder,false,true> pack_rhs_panel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, ColMajor, false, true> pack_lhs_panel;
-
-    for(Index k2=IsLower ? size : 0;
-        IsLower ? k2>0 : k2<size;
-        IsLower ? k2-=kc : k2+=kc)
-    {
-      const Index actual_kc = (std::min)(IsLower ? k2 : size-k2, kc);
-      Index actual_k2 = IsLower ? k2-actual_kc : k2 ;
-
-      Index startPanel = IsLower ? 0 : k2+actual_kc;
-      Index rs = IsLower ? actual_k2 : size - actual_k2 - actual_kc;
-      Scalar* geb = blockB+actual_kc*actual_kc;
-
-      if (rs>0) pack_rhs(geb, rhs.getSubMapper(actual_k2,startPanel), actual_kc, rs);
-
-      // triangular packing (we only pack the panels off the diagonal,
-      // neglecting the blocks overlapping the diagonal
-      {
-        for (Index j2=0; j2<actual_kc; j2+=SmallPanelWidth)
-        {
-          Index actualPanelWidth = std::min<Index>(actual_kc-j2, SmallPanelWidth);
-          Index actual_j2 = actual_k2 + j2;
-          Index panelOffset = IsLower ? j2+actualPanelWidth : 0;
-          Index panelLength = IsLower ? actual_kc-j2-actualPanelWidth : j2;
-
-          if (panelLength>0)
-          pack_rhs_panel(blockB+j2*actual_kc,
-                         rhs.getSubMapper(actual_k2+panelOffset, actual_j2),
-                         panelLength, actualPanelWidth,
-                         actual_kc, panelOffset);
-        }
-      }
-
-      for(Index i2=0; i2<rows; i2+=mc)
-      {
-        const Index actual_mc = (std::min)(mc,rows-i2);
-
-        // triangular solver kernel
-        {
-          // for each small block of the diagonal (=> vertical panels of rhs)
-          for (Index j2 = IsLower
-                      ? (actual_kc - ((actual_kc%SmallPanelWidth) ? Index(actual_kc%SmallPanelWidth)
-                                                                  : Index(SmallPanelWidth)))
-                      : 0;
-               IsLower ? j2>=0 : j2<actual_kc;
-               IsLower ? j2-=SmallPanelWidth : j2+=SmallPanelWidth)
-          {
-            Index actualPanelWidth = std::min<Index>(actual_kc-j2, SmallPanelWidth);
-            Index absolute_j2 = actual_k2 + j2;
-            Index panelOffset = IsLower ? j2+actualPanelWidth : 0;
-            Index panelLength = IsLower ? actual_kc - j2 - actualPanelWidth : j2;
-
-            // GEBP
-            if(panelLength>0)
-            {
-              gebp_kernel(lhs.getSubMapper(i2,absolute_j2),
-                          blockA, blockB+j2*actual_kc,
-                          actual_mc, panelLength, actualPanelWidth,
-                          Scalar(-1),
-                          actual_kc, actual_kc, // strides
-                          panelOffset, panelOffset); // offsets
-            }
-
-            // unblocked triangular solve
-            for (Index k=0; k<actualPanelWidth; ++k)
-            {
-              Index j = IsLower ? absolute_j2+actualPanelWidth-k-1 : absolute_j2+k;
-
-              typename LhsMapper::LinearMapper r = lhs.getLinearMapper(i2,j);
-              for (Index k3=0; k3<k; ++k3)
-              {
-                Scalar b = conj(rhs(IsLower ? j+1+k3 : absolute_j2+k3,j));
-                typename LhsMapper::LinearMapper a = lhs.getLinearMapper(i2,IsLower ? j+1+k3 : absolute_j2+k3);
-                for (Index i=0; i<actual_mc; ++i)
-                  r(i) -= a(i) * b;
-              }
-              if((Mode & UnitDiag)==0)
-              {
-                Scalar inv_rjj = RealScalar(1)/conj(rhs(j,j));
-                for (Index i=0; i<actual_mc; ++i)
-                  r(i) *= inv_rjj;
-              }
-            }
-
-            // pack the just computed part of lhs to A
-            pack_lhs_panel(blockA, lhs.getSubMapper(i2,absolute_j2),
-                           actualPanelWidth, actual_mc,
-                           actual_kc, j2);
-          }
-        }
-
-        if (rs>0)
-          gebp_kernel(lhs.getSubMapper(i2, startPanel), blockA, geb,
-                      actual_mc, actual_kc, rs, Scalar(-1),
-                      -1, -1, 0, 0);
-      }
+#if defined(EIGEN_VECTORIZE_AVX512) && EIGEN_USE_AVX512_TRSM_R_KERNELS && EIGEN_ENABLE_AVX512_NOCOPY_TRSM_R_CUTOFFS
+  EIGEN_IF_CONSTEXPR(
+      (OtherInnerStride == 1 && (std::is_same<Scalar, float>::value || std::is_same<Scalar, double>::value))) {
+    // TODO: Investigate better heuristics for cutoffs.
+    std::ptrdiff_t l1, l2, l3;
+    manage_caching_sizes(GetAction, &l1, &l2, &l3);
+    double L2Cap = 0.5;  // 50% of L2 size
+    if (size < avx512_trsm_cutoff<Scalar>(l2, rows, L2Cap)) {
+      trsmKernelR<Scalar, Index, Mode, Conjugate, TriStorageOrder, OtherInnerStride, /*Specialized=*/true>::kernel(
+          size, rows, _tri, triStride, _other, 1, otherStride);
+      return;
     }
   }
+#endif
 
-} // end namespace internal
+  typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> LhsMapper;
+  typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> RhsMapper;
+  LhsMapper lhs(_other, otherStride, otherIncr);
+  RhsMapper rhs(_tri, triStride);
 
-} // end namespace Eigen
+  typedef gebp_traits<Scalar, Scalar> Traits;
+  enum {
+    RhsStorageOrder = TriStorageOrder,
+    SmallPanelWidth = plain_enum_max(Traits::mr, Traits::nr),
+    IsLower = (Mode & Lower) == Lower
+  };
 
-#endif // EIGEN_TRIANGULAR_SOLVER_MATRIX_H
+  Index kc = blocking.kc();                    // cache block size along the K direction
+  Index mc = (std::min)(rows, blocking.mc());  // cache block size along the M direction
+
+  std::size_t sizeA = kc * mc;
+  std::size_t sizeB = kc * size;
+
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+  ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+
+  gebp_kernel<Scalar, Scalar, Index, LhsMapper, Traits::mr, Traits::nr, false, Conjugate> gebp_kernel;
+  gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
+  gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder, false, true> pack_rhs_panel;
+  gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, ColMajor,
+                false, true>
+      pack_lhs_panel;
+
+  for (Index k2 = IsLower ? size : 0; IsLower ? k2 > 0 : k2 < size; IsLower ? k2 -= kc : k2 += kc) {
+    const Index actual_kc = (std::min)(IsLower ? k2 : size - k2, kc);
+    Index actual_k2 = IsLower ? k2 - actual_kc : k2;
+
+    Index startPanel = IsLower ? 0 : k2 + actual_kc;
+    Index rs = IsLower ? actual_k2 : size - actual_k2 - actual_kc;
+    Scalar* geb = blockB + actual_kc * actual_kc;
+
+    if (rs > 0) pack_rhs(geb, rhs.getSubMapper(actual_k2, startPanel), actual_kc, rs);
+
+    // triangular packing (we only pack the panels off the diagonal,
+    // neglecting the blocks overlapping the diagonal
+    {
+      for (Index j2 = 0; j2 < actual_kc; j2 += SmallPanelWidth) {
+        Index actualPanelWidth = std::min<Index>(actual_kc - j2, SmallPanelWidth);
+        Index actual_j2 = actual_k2 + j2;
+        Index panelOffset = IsLower ? j2 + actualPanelWidth : 0;
+        Index panelLength = IsLower ? actual_kc - j2 - actualPanelWidth : j2;
+
+        if (panelLength > 0)
+          pack_rhs_panel(blockB + j2 * actual_kc, rhs.getSubMapper(actual_k2 + panelOffset, actual_j2), panelLength,
+                         actualPanelWidth, actual_kc, panelOffset);
+      }
+    }
+
+    for (Index i2 = 0; i2 < rows; i2 += mc) {
+      const Index actual_mc = (std::min)(mc, rows - i2);
+
+      // triangular solver kernel
+      {
+        // for each small block of the diagonal (=> vertical panels of rhs)
+        for (Index j2 = IsLower ? (actual_kc - ((actual_kc % SmallPanelWidth) ? Index(actual_kc % SmallPanelWidth)
+                                                                              : Index(SmallPanelWidth)))
+                                : 0;
+             IsLower ? j2 >= 0 : j2 < actual_kc; IsLower ? j2 -= SmallPanelWidth : j2 += SmallPanelWidth) {
+          Index actualPanelWidth = std::min<Index>(actual_kc - j2, SmallPanelWidth);
+          Index absolute_j2 = actual_k2 + j2;
+          Index panelOffset = IsLower ? j2 + actualPanelWidth : 0;
+          Index panelLength = IsLower ? actual_kc - j2 - actualPanelWidth : j2;
+
+          // GEBP
+          if (panelLength > 0) {
+            gebp_kernel(lhs.getSubMapper(i2, absolute_j2), blockA, blockB + j2 * actual_kc, actual_mc, panelLength,
+                        actualPanelWidth, Scalar(-1), actual_kc, actual_kc,  // strides
+                        panelOffset, panelOffset);                           // offsets
+          }
+
+          {
+            // unblocked triangular solve
+            trsmKernelR<Scalar, Index, Mode, Conjugate, TriStorageOrder, OtherInnerStride,
+                        /*Specialized=*/true>::kernel(actualPanelWidth, actual_mc,
+                                                      _tri + absolute_j2 + absolute_j2 * triStride, triStride,
+                                                      _other + i2 * OtherInnerStride + absolute_j2 * otherStride,
+                                                      otherIncr, otherStride);
+          }
+          // pack the just computed part of lhs to A
+          pack_lhs_panel(blockA, lhs.getSubMapper(i2, absolute_j2), actualPanelWidth, actual_mc, actual_kc, j2);
+        }
+      }
+
+      if (rs > 0)
+        gebp_kernel(lhs.getSubMapper(i2, startPanel), blockA, geb, actual_mc, actual_kc, rs, Scalar(-1), -1, -1, 0, 0);
+    }
+  }
+}
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_TRIANGULAR_SOLVER_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h
index 6473170..ff7c43f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h
@@ -10,139 +10,113 @@
 #ifndef EIGEN_TRIANGULAR_SOLVER_VECTOR_H
 #define EIGEN_TRIANGULAR_SOLVER_VECTOR_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate, int StorageOrder>
-struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheRight, Mode, Conjugate, StorageOrder>
-{
-  static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs)
-  {
-    triangular_solve_vector<LhsScalar,RhsScalar,Index,OnTheLeft,
-        ((Mode&Upper)==Upper ? Lower : Upper) | (Mode&UnitDiag),
-        Conjugate,StorageOrder==RowMajor?ColMajor:RowMajor
-      >::run(size, _lhs, lhsStride, rhs);
+template <typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate, int StorageOrder>
+struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheRight, Mode, Conjugate, StorageOrder> {
+  static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs) {
+    triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheLeft,
+                            ((Mode & Upper) == Upper ? Lower : Upper) | (Mode & UnitDiag), Conjugate,
+                            StorageOrder == RowMajor ? ColMajor : RowMajor>::run(size, _lhs, lhsStride, rhs);
   }
 };
 
 // forward and backward substitution, row-major, rhs is a vector
-template<typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate>
-struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheLeft, Mode, Conjugate, RowMajor>
-{
-  enum {
-    IsLower = ((Mode&Lower)==Lower)
-  };
-  static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs)
-  {
-    typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,RowMajor>, 0, OuterStride<> > LhsMap;
-    const LhsMap lhs(_lhs,size,size,OuterStride<>(lhsStride));
+template <typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate>
+struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheLeft, Mode, Conjugate, RowMajor> {
+  enum { IsLower = ((Mode & Lower) == Lower) };
+  static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs) {
+    typedef Map<const Matrix<LhsScalar, Dynamic, Dynamic, RowMajor>, 0, OuterStride<> > LhsMap;
+    const LhsMap lhs(_lhs, size, size, OuterStride<>(lhsStride));
 
-    typedef const_blas_data_mapper<LhsScalar,Index,RowMajor> LhsMapper;
-    typedef const_blas_data_mapper<RhsScalar,Index,ColMajor> RhsMapper;
+    typedef const_blas_data_mapper<LhsScalar, Index, RowMajor> LhsMapper;
+    typedef const_blas_data_mapper<RhsScalar, Index, ColMajor> RhsMapper;
 
-    typename internal::conditional<
-                          Conjugate,
-                          const CwiseUnaryOp<typename internal::scalar_conjugate_op<LhsScalar>,LhsMap>,
-                          const LhsMap&>
-                        ::type cjLhs(lhs);
+    std::conditional_t<Conjugate, const CwiseUnaryOp<typename internal::scalar_conjugate_op<LhsScalar>, LhsMap>,
+                       const LhsMap&>
+        cjLhs(lhs);
     static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
-    for(Index pi=IsLower ? 0 : size;
-        IsLower ? pi<size : pi>0;
-        IsLower ? pi+=PanelWidth : pi-=PanelWidth)
-    {
+    for (Index pi = IsLower ? 0 : size; IsLower ? pi < size : pi > 0; IsLower ? pi += PanelWidth : pi -= PanelWidth) {
       Index actualPanelWidth = (std::min)(IsLower ? size - pi : pi, PanelWidth);
 
-      Index r = IsLower ? pi : size - pi; // remaining size
-      if (r > 0)
-      {
+      Index r = IsLower ? pi : size - pi;  // remaining size
+      if (r > 0) {
         // let's directly call the low level product function because:
         // 1 - it is faster to compile
         // 2 - it is slightly faster at runtime
-        Index startRow = IsLower ? pi : pi-actualPanelWidth;
+        Index startRow = IsLower ? pi : pi - actualPanelWidth;
         Index startCol = IsLower ? 0 : pi;
 
-        general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,Conjugate,RhsScalar,RhsMapper,false>::run(
-          actualPanelWidth, r,
-          LhsMapper(&lhs.coeffRef(startRow,startCol), lhsStride),
-          RhsMapper(rhs + startCol, 1),
-          rhs + startRow, 1,
-          RhsScalar(-1));
+        general_matrix_vector_product<Index, LhsScalar, LhsMapper, RowMajor, Conjugate, RhsScalar, RhsMapper,
+                                      false>::run(actualPanelWidth, r,
+                                                  LhsMapper(&lhs.coeffRef(startRow, startCol), lhsStride),
+                                                  RhsMapper(rhs + startCol, 1), rhs + startRow, 1, RhsScalar(-1));
       }
 
-      for(Index k=0; k<actualPanelWidth; ++k)
-      {
-        Index i = IsLower ? pi+k : pi-k-1;
-        Index s = IsLower ? pi   : i+1;
-        if (k>0)
-          rhs[i] -= (cjLhs.row(i).segment(s,k).transpose().cwiseProduct(Map<const Matrix<RhsScalar,Dynamic,1> >(rhs+s,k))).sum();
+      for (Index k = 0; k < actualPanelWidth; ++k) {
+        Index i = IsLower ? pi + k : pi - k - 1;
+        Index s = IsLower ? pi : i + 1;
+        if (k > 0)
+          rhs[i] -= (cjLhs.row(i).segment(s, k).transpose().cwiseProduct(
+                         Map<const Matrix<RhsScalar, Dynamic, 1> >(rhs + s, k)))
+                        .sum();
 
-        if((!(Mode & UnitDiag)) && numext::not_equal_strict(rhs[i],RhsScalar(0)))
-          rhs[i] /= cjLhs(i,i);
+        if ((!(Mode & UnitDiag)) && !is_identically_zero(rhs[i])) rhs[i] /= cjLhs(i, i);
       }
     }
   }
 };
 
 // forward and backward substitution, column-major, rhs is a vector
-template<typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate>
-struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheLeft, Mode, Conjugate, ColMajor>
-{
-  enum {
-    IsLower = ((Mode&Lower)==Lower)
-  };
-  static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs)
-  {
-    typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > LhsMap;
-    const LhsMap lhs(_lhs,size,size,OuterStride<>(lhsStride));
-    typedef const_blas_data_mapper<LhsScalar,Index,ColMajor> LhsMapper;
-    typedef const_blas_data_mapper<RhsScalar,Index,ColMajor> RhsMapper;
-    typename internal::conditional<Conjugate,
-                                   const CwiseUnaryOp<typename internal::scalar_conjugate_op<LhsScalar>,LhsMap>,
-                                   const LhsMap&
-                                  >::type cjLhs(lhs);
+template <typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate>
+struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheLeft, Mode, Conjugate, ColMajor> {
+  enum { IsLower = ((Mode & Lower) == Lower) };
+  static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs) {
+    typedef Map<const Matrix<LhsScalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<> > LhsMap;
+    const LhsMap lhs(_lhs, size, size, OuterStride<>(lhsStride));
+    typedef const_blas_data_mapper<LhsScalar, Index, ColMajor> LhsMapper;
+    typedef const_blas_data_mapper<RhsScalar, Index, ColMajor> RhsMapper;
+    std::conditional_t<Conjugate, const CwiseUnaryOp<typename internal::scalar_conjugate_op<LhsScalar>, LhsMap>,
+                       const LhsMap&>
+        cjLhs(lhs);
     static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
 
-    for(Index pi=IsLower ? 0 : size;
-        IsLower ? pi<size : pi>0;
-        IsLower ? pi+=PanelWidth : pi-=PanelWidth)
-    {
+    for (Index pi = IsLower ? 0 : size; IsLower ? pi < size : pi > 0; IsLower ? pi += PanelWidth : pi -= PanelWidth) {
       Index actualPanelWidth = (std::min)(IsLower ? size - pi : pi, PanelWidth);
-      Index startBlock = IsLower ? pi : pi-actualPanelWidth;
+      Index startBlock = IsLower ? pi : pi - actualPanelWidth;
       Index endBlock = IsLower ? pi + actualPanelWidth : 0;
 
-      for(Index k=0; k<actualPanelWidth; ++k)
-      {
-        Index i = IsLower ? pi+k : pi-k-1;
-        if(numext::not_equal_strict(rhs[i],RhsScalar(0)))
-        {
-          if(!(Mode & UnitDiag))
-            rhs[i] /= cjLhs.coeff(i,i);
+      for (Index k = 0; k < actualPanelWidth; ++k) {
+        Index i = IsLower ? pi + k : pi - k - 1;
+        if (!is_identically_zero(rhs[i])) {
+          if (!(Mode & UnitDiag)) rhs[i] /= cjLhs.coeff(i, i);
 
-          Index r = actualPanelWidth - k - 1; // remaining size
-          Index s = IsLower ? i+1 : i-r;
-          if (r>0)
-            Map<Matrix<RhsScalar,Dynamic,1> >(rhs+s,r) -= rhs[i] * cjLhs.col(i).segment(s,r);
+          Index r = actualPanelWidth - k - 1;  // remaining size
+          Index s = IsLower ? i + 1 : i - r;
+          if (r > 0) Map<Matrix<RhsScalar, Dynamic, 1> >(rhs + s, r) -= rhs[i] * cjLhs.col(i).segment(s, r);
         }
       }
-      Index r = IsLower ? size - endBlock : startBlock; // remaining size
-      if (r > 0)
-      {
+      Index r = IsLower ? size - endBlock : startBlock;  // remaining size
+      if (r > 0) {
         // let's directly call the low level product function because:
         // 1 - it is faster to compile
         // 2 - it is slightly faster at runtime
-        general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,Conjugate,RhsScalar,RhsMapper,false>::run(
-            r, actualPanelWidth,
-            LhsMapper(&lhs.coeffRef(endBlock,startBlock), lhsStride),
-            RhsMapper(rhs+startBlock, 1),
-            rhs+endBlock, 1, RhsScalar(-1));
+        general_matrix_vector_product<Index, LhsScalar, LhsMapper, ColMajor, Conjugate, RhsScalar, RhsMapper,
+                                      false>::run(r, actualPanelWidth,
+                                                  LhsMapper(&lhs.coeffRef(endBlock, startBlock), lhsStride),
+                                                  RhsMapper(rhs + startBlock, 1), rhs + endBlock, 1, RhsScalar(-1));
       }
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TRIANGULAR_SOLVER_VECTOR_H
+#endif  // EIGEN_TRIANGULAR_SOLVER_VECTOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Assert.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Assert.h
new file mode 100644
index 0000000..09a411a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Assert.h
@@ -0,0 +1,158 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2022, The Eigen authors.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CORE_UTIL_ASSERT_H
+#define EIGEN_CORE_UTIL_ASSERT_H
+
+// Eigen custom assert function.
+//
+// The combination of Eigen's relative includes and cassert's `assert` function
+// (or any usage of the __FILE__ macro) can lead to ODR issues:
+// a header included using different relative paths in two different TUs will
+// have two different token-for-token definitions, since __FILE__ is expanded
+// as an in-line string with different values.  Normally this would be
+// harmless - the linker would just choose one definition. However, it breaks
+// with C++20 modules when functions in different modules have different
+// definitions.
+//
+// To get around this, we need to use __builtin_FILE() when available, which is
+// considered a single token, and thus satisfies the ODR.
+
+// Only define eigen_plain_assert if we are debugging, and either
+//  - we are not compiling for GPU, or
+//  - gpu debugging is enabled.
+#if !defined(EIGEN_NO_DEBUG) && (!defined(EIGEN_GPU_COMPILE_PHASE) || !defined(EIGEN_NO_DEBUG_GPU))
+
+#include <cassert>
+
+#ifndef EIGEN_USE_CUSTOM_PLAIN_ASSERT
+// Disable new custom asserts by default for now.
+#define EIGEN_USE_CUSTOM_PLAIN_ASSERT 0
+#endif
+
+#if EIGEN_USE_CUSTOM_PLAIN_ASSERT
+
+#ifndef EIGEN_HAS_BUILTIN_FILE
+// Clang can check if __builtin_FILE() is supported.
+// GCC > 5, MSVC 2019 14.26 (1926) all have __builtin_FILE().
+//
+// For NVCC, it's more complicated.  Through trial-and-error:
+//   - nvcc+gcc supports __builtin_FILE() on host, and on device after CUDA 11.
+//   - nvcc+msvc supports __builtin_FILE() only after CUDA 11.
+#if (EIGEN_HAS_BUILTIN(__builtin_FILE) && (EIGEN_COMP_CLANG || !defined(EIGEN_CUDA_ARCH))) ||            \
+    (EIGEN_GNUC_STRICT_AT_LEAST(5, 0, 0) && (EIGEN_COMP_NVCC >= 110000 || !defined(EIGEN_CUDA_ARCH))) || \
+    (EIGEN_COMP_MSVC >= 1926 && (!EIGEN_COMP_NVCC || EIGEN_COMP_NVCC >= 110000))
+#define EIGEN_HAS_BUILTIN_FILE 1
+#else
+#define EIGEN_HAS_BUILTIN_FILE 0
+#endif
+#endif  // EIGEN_HAS_BUILTIN_FILE
+
+#if EIGEN_HAS_BUILTIN_FILE
+#define EIGEN_BUILTIN_FILE __builtin_FILE()
+#define EIGEN_BUILTIN_LINE __builtin_LINE()
+#else
+// Default (potentially unsafe) values.
+#define EIGEN_BUILTIN_FILE __FILE__
+#define EIGEN_BUILTIN_LINE __LINE__
+#endif
+
+// Use __PRETTY_FUNCTION__ when available, since it is more descriptive, as
+// __builtin_FUNCTION() only returns the undecorated function name.
+// This should still be okay ODR-wise since it is a compiler-specific fixed
+// value.  Mixing compilers will likely lead to ODR violations anyways.
+#if EIGEN_COMP_MSVC
+#define EIGEN_BUILTIN_FUNCTION __FUNCSIG__
+#elif EIGEN_COMP_GNUC
+#define EIGEN_BUILTIN_FUNCTION __PRETTY_FUNCTION__
+#else
+#define EIGEN_BUILTIN_FUNCTION __func__
+#endif
+
+namespace Eigen {
+namespace internal {
+
+// Generic default assert handler.
+template <typename EnableIf = void, typename... EmptyArgs>
+struct assert_handler_impl {
+  EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static inline void run(const char* expression, const char* file, unsigned line,
+                                                             const char* function) {
+#ifdef EIGEN_GPU_COMPILE_PHASE
+    // GPU device code doesn't allow stderr or abort, so use printf and raise an
+    // illegal instruction exception to trigger a kernel failure.
+#ifndef EIGEN_NO_IO
+    printf("Assertion failed at %s:%u in %s: %s\n", file == nullptr ? "<file>" : file, line,
+           function == nullptr ? "<function>" : function, expression);
+#endif
+    __trap();
+
+#else  // EIGEN_GPU_COMPILE_PHASE
+
+    // Print to stderr and abort, as specified in <cassert>.
+#ifndef EIGEN_NO_IO
+    fprintf(stderr, "Assertion failed at %s:%u in %s: %s\n", file == nullptr ? "<file>" : file, line,
+            function == nullptr ? "<function>" : function, expression);
+#endif
+    std::abort();
+
+#endif  // EIGEN_GPU_COMPILE_PHASE
+  }
+};
+
+// Use POSIX __assert_fail handler when available.
+//
+// This allows us to integrate with systems that have custom handlers.
+//
+// NOTE: this handler is not always available on all POSIX systems (otherwise
+// we could simply test for __unix__ or similar).  The handler function name
+// seems to depend on the specific toolchain implementation, and differs between
+// compilers, platforms, OSes, etc.  Hence, we detect support via SFINAE.
+template <typename... EmptyArgs>
+struct assert_handler_impl<void_t<decltype(__assert_fail((const char*)nullptr,         // expression
+                                                         (const char*)nullptr,         // file
+                                                         0,                            // line
+                                                         (const char*)nullptr,         // function
+                                                         std::declval<EmptyArgs>()...  // Empty substitution required
+                                                                                       // for SFINAE.
+                                                         ))>,
+                           EmptyArgs...> {
+  EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static inline void run(const char* expression, const char* file, unsigned line,
+                                                             const char* function) {
+    // GCC requires this call to be dependent on the template parameters.
+    __assert_fail(expression, file, line, function, std::declval<EmptyArgs>()...);
+  }
+};
+
+EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE inline void __assert_handler(const char* expression, const char* file,
+                                                                 unsigned line, const char* function) {
+  assert_handler_impl<>::run(expression, file, line, function);
+}
+
+}  // namespace internal
+}  // namespace Eigen
+
+#define eigen_plain_assert(expression)                                                                                \
+  (EIGEN_PREDICT_FALSE(!(expression)) ? Eigen::internal::__assert_handler(#expression, EIGEN_BUILTIN_FILE,            \
+                                                                          EIGEN_BUILTIN_LINE, EIGEN_BUILTIN_FUNCTION) \
+                                      : (void)0)
+
+#else  // EIGEN_USE_CUSTOM_PLAIN_ASSERT
+
+// Use regular assert.
+#define eigen_plain_assert(condition) assert(condition)
+
+#endif  // EIGEN_USE_CUSTOM_PLAIN_ASSERT
+
+#else  // EIGEN_NO_DEBUG
+
+#define eigen_plain_assert(condition) ((void)0)
+
+#endif  // EIGEN_NO_DEBUG
+
+#endif  // EIGEN_CORE_UTIL_ASSERT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h
index e16a564..c2994b2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h
@@ -13,50 +13,52 @@
 // This file contains many lightweight helper classes used to
 // implement and control fast level 2 and level 3 BLAS-like routines.
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
 // forward declarations
-template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs=false, bool ConjugateRhs=false>
+template <typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr,
+          bool ConjugateLhs = false, bool ConjugateRhs = false>
 struct gebp_kernel;
 
-template<typename Scalar, typename Index, typename DataMapper, int nr, int StorageOrder, bool Conjugate = false, bool PanelMode=false>
+template <typename Scalar, typename Index, typename DataMapper, int nr, int StorageOrder, bool Conjugate = false,
+          bool PanelMode = false>
 struct gemm_pack_rhs;
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, int StorageOrder, bool Conjugate = false, bool PanelMode = false>
+template <typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, int StorageOrder,
+          bool Conjugate = false, bool PanelMode = false>
 struct gemm_pack_lhs;
 
-template<
-  typename Index,
-  typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-  typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-  int ResStorageOrder, int ResInnerStride>
+template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar,
+          int RhsStorageOrder, bool ConjugateRhs, int ResStorageOrder, int ResInnerStride>
 struct general_matrix_matrix_product;
 
-template<typename Index,
-         typename LhsScalar, typename LhsMapper, int LhsStorageOrder, bool ConjugateLhs,
-         typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version=Specialized>
+template <typename Index, typename LhsScalar, typename LhsMapper, int LhsStorageOrder, bool ConjugateLhs,
+          typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version = Specialized>
 struct general_matrix_vector_product;
 
-template<typename From,typename To> struct get_factor {
+template <typename From, typename To>
+struct get_factor {
   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE To run(const From& x) { return To(x); }
 };
 
-template<typename Scalar> struct get_factor<Scalar,typename NumTraits<Scalar>::Real> {
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE typename NumTraits<Scalar>::Real run(const Scalar& x) { return numext::real(x); }
+template <typename Scalar>
+struct get_factor<Scalar, typename NumTraits<Scalar>::Real> {
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE typename NumTraits<Scalar>::Real run(const Scalar& x) {
+    return numext::real(x);
+  }
 };
 
-
-template<typename Scalar, typename Index>
+template <typename Scalar, typename Index>
 class BlasVectorMapper {
-  public:
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasVectorMapper(Scalar *data) : m_data(data) {}
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasVectorMapper(Scalar* data) : m_data(data) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar operator()(Index i) const {
-    return m_data[i];
-  }
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar operator()(Index i) const { return m_data[i]; }
   template <typename Packet, int AlignmentType>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet load(Index i) const {
     return ploadt<Packet, AlignmentType>(m_data + i);
@@ -64,82 +66,91 @@
 
   template <typename Packet>
   EIGEN_DEVICE_FUNC bool aligned(Index i) const {
-    return (UIntPtr(m_data+i)%sizeof(Packet))==0;
+    return (std::uintptr_t(m_data + i) % sizeof(Packet)) == 0;
   }
 
-  protected:
+ protected:
   Scalar* m_data;
 };
 
-template<typename Scalar, typename Index, int AlignmentType, int Incr=1>
+template <typename Scalar, typename Index, int AlignmentType, int Incr = 1>
 class BlasLinearMapper;
 
-template<typename Scalar, typename Index, int AlignmentType>
-class BlasLinearMapper<Scalar,Index,AlignmentType>
-{
-public:
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data, Index incr=1)
-    : m_data(data)
-  {
+template <typename Scalar, typename Index, int AlignmentType>
+class BlasLinearMapper<Scalar, Index, AlignmentType> {
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar* data, Index incr = 1) : m_data(data) {
     EIGEN_ONLY_USED_FOR_DEBUG(incr);
-    eigen_assert(incr==1);
+    eigen_assert(incr == 1);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const {
-    internal::prefetch(&operator()(i));
-  }
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(Index i) const { internal::prefetch(&operator()(i)); }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i) const {
-    return m_data[i];
-  }
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i) const { return m_data[i]; }
 
-  template<typename PacketType>
+  template <typename PacketType>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i) const {
     return ploadt<PacketType, AlignmentType>(m_data + i);
   }
 
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType &p) const {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacketPartial(Index i, Index n, Index offset = 0) const {
+    return ploadt_partial<PacketType, AlignmentType>(m_data + i, n, offset);
+  }
+
+  template <typename PacketType, int AlignmentT>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType load(Index i) const {
+    return ploadt<PacketType, AlignmentT>(m_data + i);
+  }
+
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType& p) const {
     pstoret<Scalar, PacketType, AlignmentType>(m_data + i, p);
   }
 
-protected:
-  Scalar *m_data;
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketPartial(Index i, const PacketType& p, Index n,
+                                                                Index offset = 0) const {
+    pstoret_partial<Scalar, PacketType, AlignmentType>(m_data + i, p, n, offset);
+  }
+
+ protected:
+  Scalar* m_data;
 };
 
 // Lightweight helper class to access matrix coefficients.
-template<typename Scalar, typename Index, int StorageOrder, int AlignmentType = Unaligned, int Incr = 1>
+template <typename Scalar, typename Index, int StorageOrder, int AlignmentType = Unaligned, int Incr = 1>
 class blas_data_mapper;
 
 // TMP to help PacketBlock store implementation.
 // There's currently no known use case for PacketBlock load.
 // The default implementation assumes ColMajor order.
 // It always store each packet sequentially one `stride` apart.
-template<typename Index, typename Scalar, typename Packet, int n, int idx, int StorageOrder>
-struct PacketBlockManagement
-{
+template <typename Index, typename Scalar, typename Packet, int n, int idx, int StorageOrder>
+struct PacketBlockManagement {
   PacketBlockManagement<Index, Scalar, Packet, n, idx - 1, StorageOrder> pbm;
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar* to, const Index stride, Index i, Index j,
+                                                   const PacketBlock<Packet, n>& block) const {
     pbm.store(to, stride, i, j, block);
-    pstoreu<Scalar>(to + i + (j + idx)*stride, block.packet[idx]);
+    pstoreu<Scalar>(to + i + (j + idx) * stride, block.packet[idx]);
   }
 };
 
 // PacketBlockManagement specialization to take care of RowMajor order without ifs.
-template<typename Index, typename Scalar, typename Packet, int n, int idx>
-struct PacketBlockManagement<Index, Scalar, Packet, n, idx, RowMajor>
-{
+template <typename Index, typename Scalar, typename Packet, int n, int idx>
+struct PacketBlockManagement<Index, Scalar, Packet, n, idx, RowMajor> {
   PacketBlockManagement<Index, Scalar, Packet, n, idx - 1, RowMajor> pbm;
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar* to, const Index stride, Index i, Index j,
+                                                   const PacketBlock<Packet, n>& block) const {
     pbm.store(to, stride, i, j, block);
-    pstoreu<Scalar>(to + j + (i + idx)*stride, block.packet[idx]);
+    pstoreu<Scalar>(to + j + (i + idx) * stride, block.packet[idx]);
   }
 };
 
-template<typename Index, typename Scalar, typename Packet, int n, int StorageOrder>
-struct PacketBlockManagement<Index, Scalar, Packet, n, -1, StorageOrder>
-{
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+template <typename Index, typename Scalar, typename Packet, int n, int StorageOrder>
+struct PacketBlockManagement<Index, Scalar, Packet, n, -1, StorageOrder> {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar* to, const Index stride, Index i, Index j,
+                                                   const PacketBlock<Packet, n>& block) const {
     EIGEN_UNUSED_VARIABLE(to);
     EIGEN_UNUSED_VARIABLE(stride);
     EIGEN_UNUSED_VARIABLE(i);
@@ -148,10 +159,10 @@
   }
 };
 
-template<typename Index, typename Scalar, typename Packet, int n>
-struct PacketBlockManagement<Index, Scalar, Packet, n, -1, RowMajor>
-{
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+template <typename Index, typename Scalar, typename Packet, int n>
+struct PacketBlockManagement<Index, Scalar, Packet, n, -1, RowMajor> {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar* to, const Index stride, Index i, Index j,
+                                                   const PacketBlock<Packet, n>& block) const {
     EIGEN_UNUSED_VARIABLE(to);
     EIGEN_UNUSED_VARIABLE(stride);
     EIGEN_UNUSED_VARIABLE(i);
@@ -160,75 +171,93 @@
   }
 };
 
-template<typename Scalar, typename Index, int StorageOrder, int AlignmentType>
-class blas_data_mapper<Scalar,Index,StorageOrder,AlignmentType,1>
-{
-public:
+template <typename Scalar, typename Index, int StorageOrder, int AlignmentType>
+class blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, 1> {
+ public:
   typedef BlasLinearMapper<Scalar, Index, AlignmentType> LinearMapper;
+  typedef blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType> SubMapper;
   typedef BlasVectorMapper<Scalar, Index> VectorMapper;
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr=1)
-   : m_data(data), m_stride(stride)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr = 1)
+      : m_data(data), m_stride(stride) {
     EIGEN_ONLY_USED_FOR_DEBUG(incr);
-    eigen_assert(incr==1);
+    eigen_assert(incr == 1);
   }
 
-  EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType>
-  getSubMapper(Index i, Index j) const {
-    return blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType>(&operator()(i, j), m_stride);
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE SubMapper getSubMapper(Index i, Index j) const {
+    return SubMapper(&operator()(i, j), m_stride);
   }
 
-  EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const {
     return LinearMapper(&operator()(i, j));
   }
 
-  EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE VectorMapper getVectorMapper(Index i, Index j) const {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE VectorMapper getVectorMapper(Index i, Index j) const {
     return VectorMapper(&operator()(i, j));
   }
 
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(Index i, Index j) const { internal::prefetch(&operator()(i, j)); }
 
-  EIGEN_DEVICE_FUNC
-  EIGEN_ALWAYS_INLINE Scalar& operator()(Index i, Index j) const {
-    return m_data[StorageOrder==RowMajor ? j + i*m_stride : i + j*m_stride];
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i, Index j) const {
+    return m_data[StorageOrder == RowMajor ? j + i * m_stride : i + j * m_stride];
   }
 
-  template<typename PacketType>
+  template <typename PacketType>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i, Index j) const {
     return ploadt<PacketType, AlignmentType>(&operator()(i, j));
   }
 
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacketPartial(Index i, Index j, Index n,
+                                                                     Index offset = 0) const {
+    return ploadt_partial<PacketType, AlignmentType>(&operator()(i, j), n, offset);
+  }
+
   template <typename PacketT, int AlignmentT>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketT load(Index i, Index j) const {
     return ploadt<PacketT, AlignmentT>(&operator()(i, j));
   }
 
-  template<typename SubPacket>
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void scatterPacket(Index i, Index j, const SubPacket &p) const {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, Index j, const PacketType& p) const {
+    pstoret<Scalar, PacketType, AlignmentType>(&operator()(i, j), p);
+  }
+
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketPartial(Index i, Index j, const PacketType& p, Index n,
+                                                                Index offset = 0) const {
+    pstoret_partial<Scalar, PacketType, AlignmentType>(&operator()(i, j), p, n, offset);
+  }
+
+  template <typename SubPacket>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void scatterPacket(Index i, Index j, const SubPacket& p) const {
     pscatter<Scalar, SubPacket>(&operator()(i, j), p, m_stride);
   }
 
-  template<typename SubPacket>
+  template <typename SubPacket>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE SubPacket gatherPacket(Index i, Index j) const {
     return pgather<Scalar, SubPacket>(&operator()(i, j), m_stride);
   }
 
   EIGEN_DEVICE_FUNC const Index stride() const { return m_stride; }
+  EIGEN_DEVICE_FUNC const Index incr() const { return 1; }
   EIGEN_DEVICE_FUNC const Scalar* data() const { return m_data; }
 
   EIGEN_DEVICE_FUNC Index firstAligned(Index size) const {
-    if (UIntPtr(m_data)%sizeof(Scalar)) {
+    if (std::uintptr_t(m_data) % sizeof(Scalar)) {
       return -1;
     }
     return internal::first_default_aligned(m_data, size);
   }
 
-  template<typename SubPacket, int n>
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketBlock(Index i, Index j, const PacketBlock<SubPacket, n> &block) const {
-    PacketBlockManagement<Index, Scalar, SubPacket, n, n-1, StorageOrder> pbm;
+  template <typename SubPacket, int n>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketBlock(Index i, Index j,
+                                                              const PacketBlock<SubPacket, n>& block) const {
+    PacketBlockManagement<Index, Scalar, SubPacket, n, n - 1, StorageOrder> pbm;
     pbm.store(m_data, m_stride, i, j, block);
   }
-protected:
+
+ protected:
   Scalar* EIGEN_RESTRICT m_data;
   const Index m_stride;
 };
@@ -236,302 +265,320 @@
 // Implementation of non-natural increment (i.e. inner-stride != 1)
 // The exposed API is not complete yet compared to the Incr==1 case
 // because some features makes less sense in this case.
-template<typename Scalar, typename Index, int AlignmentType, int Incr>
-class BlasLinearMapper
-{
-public:
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data,Index incr) : m_data(data), m_incr(incr) {}
+template <typename Scalar, typename Index, int AlignmentType, int Incr>
+class BlasLinearMapper {
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar* data, Index incr) : m_data(data), m_incr(incr) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const {
-    internal::prefetch(&operator()(i));
-  }
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const { internal::prefetch(&operator()(i)); }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i) const {
-    return m_data[i*m_incr.value()];
-  }
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i) const { return m_data[i * m_incr.value()]; }
 
-  template<typename PacketType>
+  template <typename PacketType>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i) const {
-    return pgather<Scalar,PacketType>(m_data + i*m_incr.value(), m_incr.value());
+    return pgather<Scalar, PacketType>(m_data + i * m_incr.value(), m_incr.value());
   }
 
-  template<typename PacketType>
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType &p) const {
-    pscatter<Scalar, PacketType>(m_data + i*m_incr.value(), p, m_incr.value());
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacketPartial(Index i, Index n, Index /*offset*/ = 0) const {
+    return pgather_partial<Scalar, PacketType>(m_data + i * m_incr.value(), m_incr.value(), n);
   }
 
-protected:
-  Scalar *m_data;
-  const internal::variable_if_dynamic<Index,Incr> m_incr;
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType& p) const {
+    pscatter<Scalar, PacketType>(m_data + i * m_incr.value(), p, m_incr.value());
+  }
+
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketPartial(Index i, const PacketType& p, Index n,
+                                                                Index /*offset*/ = 0) const {
+    pscatter_partial<Scalar, PacketType>(m_data + i * m_incr.value(), p, m_incr.value(), n);
+  }
+
+ protected:
+  Scalar* m_data;
+  const internal::variable_if_dynamic<Index, Incr> m_incr;
 };
 
-template<typename Scalar, typename Index, int StorageOrder, int AlignmentType,int Incr>
-class blas_data_mapper
-{
-public:
-  typedef BlasLinearMapper<Scalar, Index, AlignmentType,Incr> LinearMapper;
+template <typename Scalar, typename Index, int StorageOrder, int AlignmentType, int Incr>
+class blas_data_mapper {
+ public:
+  typedef BlasLinearMapper<Scalar, Index, AlignmentType, Incr> LinearMapper;
+  typedef blas_data_mapper SubMapper;
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr) : m_data(data), m_stride(stride), m_incr(incr) {}
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr)
+      : m_data(data), m_stride(stride), m_incr(incr) {}
 
-  EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE blas_data_mapper
-  getSubMapper(Index i, Index j) const {
-    return blas_data_mapper(&operator()(i, j), m_stride, m_incr.value());
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE SubMapper getSubMapper(Index i, Index j) const {
+    return SubMapper(&operator()(i, j), m_stride, m_incr.value());
   }
 
-  EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const {
     return LinearMapper(&operator()(i, j), m_incr.value());
   }
 
-  EIGEN_DEVICE_FUNC
-  EIGEN_ALWAYS_INLINE Scalar& operator()(Index i, Index j) const {
-    return m_data[StorageOrder==RowMajor ? j*m_incr.value() + i*m_stride : i*m_incr.value() + j*m_stride];
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(Index i, Index j) const { internal::prefetch(&operator()(i, j)); }
+
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i, Index j) const {
+    return m_data[StorageOrder == RowMajor ? j * m_incr.value() + i * m_stride : i * m_incr.value() + j * m_stride];
   }
 
-  template<typename PacketType>
+  template <typename PacketType>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i, Index j) const {
-    return pgather<Scalar,PacketType>(&operator()(i, j),m_incr.value());
+    return pgather<Scalar, PacketType>(&operator()(i, j), m_incr.value());
+  }
+
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacketPartial(Index i, Index j, Index n,
+                                                                     Index /*offset*/ = 0) const {
+    return pgather_partial<Scalar, PacketType>(&operator()(i, j), m_incr.value(), n);
   }
 
   template <typename PacketT, int AlignmentT>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketT load(Index i, Index j) const {
-    return pgather<Scalar,PacketT>(&operator()(i, j),m_incr.value());
+    return pgather<Scalar, PacketT>(&operator()(i, j), m_incr.value());
   }
 
-  template<typename SubPacket>
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void scatterPacket(Index i, Index j, const SubPacket &p) const {
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, Index j, const PacketType& p) const {
+    pscatter<Scalar, PacketType>(&operator()(i, j), p, m_incr.value());
+  }
+
+  template <typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketPartial(Index i, Index j, const PacketType& p, Index n,
+                                                                Index /*offset*/ = 0) const {
+    pscatter_partial<Scalar, PacketType>(&operator()(i, j), p, m_incr.value(), n);
+  }
+
+  template <typename SubPacket>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void scatterPacket(Index i, Index j, const SubPacket& p) const {
     pscatter<Scalar, SubPacket>(&operator()(i, j), p, m_stride);
   }
 
-  template<typename SubPacket>
+  template <typename SubPacket>
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE SubPacket gatherPacket(Index i, Index j) const {
     return pgather<Scalar, SubPacket>(&operator()(i, j), m_stride);
   }
 
-  // storePacketBlock_helper defines a way to access values inside the PacketBlock, this is essentially required by the Complex types.
-  template<typename SubPacket, typename ScalarT, int n, int idx>
-  struct storePacketBlock_helper
-  {
-    storePacketBlock_helper<SubPacket, ScalarT, n, idx-1> spbh;
-    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j, const PacketBlock<SubPacket, n>& block) const {
-      spbh.store(sup, i,j,block);
-      for(int l = 0; l < unpacket_traits<SubPacket>::size; l++)
-      {
-        ScalarT *v = &sup->operator()(i+l, j+idx);
-        *v = block.packet[idx][l];
+  // storePacketBlock_helper defines a way to access values inside the PacketBlock, this is essentially required by the
+  // Complex types.
+  template <typename SubPacket, typename Scalar_, int n, int idx>
+  struct storePacketBlock_helper {
+    storePacketBlock_helper<SubPacket, Scalar_, n, idx - 1> spbh;
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(
+        const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j,
+        const PacketBlock<SubPacket, n>& block) const {
+      spbh.store(sup, i, j, block);
+      sup->template storePacket<SubPacket>(i, j + idx, block.packet[idx]);
+    }
+  };
+
+  template <typename SubPacket, int n, int idx>
+  struct storePacketBlock_helper<SubPacket, std::complex<float>, n, idx> {
+    storePacketBlock_helper<SubPacket, std::complex<float>, n, idx - 1> spbh;
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(
+        const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j,
+        const PacketBlock<SubPacket, n>& block) const {
+      spbh.store(sup, i, j, block);
+      sup->template storePacket<SubPacket>(i, j + idx, block.packet[idx]);
+    }
+  };
+
+  template <typename SubPacket, int n, int idx>
+  struct storePacketBlock_helper<SubPacket, std::complex<double>, n, idx> {
+    storePacketBlock_helper<SubPacket, std::complex<double>, n, idx - 1> spbh;
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(
+        const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j,
+        const PacketBlock<SubPacket, n>& block) const {
+      spbh.store(sup, i, j, block);
+      for (int l = 0; l < unpacket_traits<SubPacket>::size; l++) {
+        std::complex<double>* v = &sup->operator()(i + l, j + idx);
+        v->real(block.packet[idx].v[2 * l + 0]);
+        v->imag(block.packet[idx].v[2 * l + 1]);
       }
     }
   };
 
-  template<typename SubPacket, int n, int idx>
-  struct storePacketBlock_helper<SubPacket, std::complex<float>, n, idx>
-  {
-    storePacketBlock_helper<SubPacket, std::complex<float>, n, idx-1> spbh;
-    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j, const PacketBlock<SubPacket, n>& block) const {
-      spbh.store(sup,i,j,block);
-      for(int l = 0; l < unpacket_traits<SubPacket>::size; l++)
-      {
-        std::complex<float> *v = &sup->operator()(i+l, j+idx);
-        v->real(block.packet[idx].v[2*l+0]);
-        v->imag(block.packet[idx].v[2*l+1]);
-      }
-    }
+  template <typename SubPacket, typename Scalar_, int n>
+  struct storePacketBlock_helper<SubPacket, Scalar_, n, -1> {
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(
+        const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index,
+        const PacketBlock<SubPacket, n>&) const {}
   };
 
-  template<typename SubPacket, int n, int idx>
-  struct storePacketBlock_helper<SubPacket, std::complex<double>, n, idx>
-  {
-    storePacketBlock_helper<SubPacket, std::complex<double>, n, idx-1> spbh;
-    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j, const PacketBlock<SubPacket, n>& block) const {
-      spbh.store(sup,i,j,block);
-      for(int l = 0; l < unpacket_traits<SubPacket>::size; l++)
-      {
-        std::complex<double> *v = &sup->operator()(i+l, j+idx);
-        v->real(block.packet[idx].v[2*l+0]);
-        v->imag(block.packet[idx].v[2*l+1]);
-      }
-    }
+  template <typename SubPacket, int n>
+  struct storePacketBlock_helper<SubPacket, std::complex<float>, n, -1> {
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(
+        const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index,
+        const PacketBlock<SubPacket, n>&) const {}
   };
 
-  template<typename SubPacket, typename ScalarT, int n>
-  struct storePacketBlock_helper<SubPacket, ScalarT, n, -1>
-  {
-    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index, const PacketBlock<SubPacket, n>& ) const {
-    }
+  template <typename SubPacket, int n>
+  struct storePacketBlock_helper<SubPacket, std::complex<double>, n, -1> {
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(
+        const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index,
+        const PacketBlock<SubPacket, n>&) const {}
   };
-
-  template<typename SubPacket, int n>
-  struct storePacketBlock_helper<SubPacket, std::complex<float>, n, -1>
-  {
-    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index, const PacketBlock<SubPacket, n>& ) const {
-    }
-  };
-
-  template<typename SubPacket, int n>
-  struct storePacketBlock_helper<SubPacket, std::complex<double>, n, -1>
-  {
-    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index, const PacketBlock<SubPacket, n>& ) const {
-    }
-  };
-  // This function stores a PacketBlock on m_data, this approach is really quite slow compare to Incr=1 and should be avoided when possible.
-  template<typename SubPacket, int n>
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketBlock(Index i, Index j, const PacketBlock<SubPacket, n>&block) const {
-    storePacketBlock_helper<SubPacket, Scalar, n, n-1> spb;
-    spb.store(this, i,j,block);
+  // This function stores a PacketBlock on m_data, this approach is really quite slow compare to Incr=1 and should be
+  // avoided when possible.
+  template <typename SubPacket, int n>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketBlock(Index i, Index j,
+                                                              const PacketBlock<SubPacket, n>& block) const {
+    storePacketBlock_helper<SubPacket, Scalar, n, n - 1> spb;
+    spb.store(this, i, j, block);
   }
-protected:
+
+  EIGEN_DEVICE_FUNC const Index stride() const { return m_stride; }
+  EIGEN_DEVICE_FUNC const Index incr() const { return m_incr.value(); }
+  EIGEN_DEVICE_FUNC Scalar* data() const { return m_data; }
+
+ protected:
   Scalar* EIGEN_RESTRICT m_data;
   const Index m_stride;
-  const internal::variable_if_dynamic<Index,Incr> m_incr;
+  const internal::variable_if_dynamic<Index, Incr> m_incr;
 };
 
 // lightweight helper class to access matrix coefficients (const version)
-template<typename Scalar, typename Index, int StorageOrder>
+template <typename Scalar, typename Index, int StorageOrder>
 class const_blas_data_mapper : public blas_data_mapper<const Scalar, Index, StorageOrder> {
-  public:
-  EIGEN_ALWAYS_INLINE const_blas_data_mapper(const Scalar *data, Index stride) : blas_data_mapper<const Scalar, Index, StorageOrder>(data, stride) {}
+ public:
+  typedef const_blas_data_mapper<Scalar, Index, StorageOrder> SubMapper;
 
-  EIGEN_ALWAYS_INLINE const_blas_data_mapper<Scalar, Index, StorageOrder> getSubMapper(Index i, Index j) const {
-    return const_blas_data_mapper<Scalar, Index, StorageOrder>(&(this->operator()(i, j)), this->m_stride);
+  EIGEN_ALWAYS_INLINE const_blas_data_mapper(const Scalar* data, Index stride)
+      : blas_data_mapper<const Scalar, Index, StorageOrder>(data, stride) {}
+
+  EIGEN_ALWAYS_INLINE SubMapper getSubMapper(Index i, Index j) const {
+    return SubMapper(&(this->operator()(i, j)), this->m_stride);
   }
 };
 
-
 /* Helper class to analyze the factors of a Product expression.
  * In particular it allows to pop out operator-, scalar multiples,
  * and conjugate */
-template<typename XprType> struct blas_traits
-{
+template <typename XprType>
+struct blas_traits {
   typedef typename traits<XprType>::Scalar Scalar;
   typedef const XprType& ExtractType;
-  typedef XprType _ExtractType;
+  typedef XprType ExtractType_;
   enum {
     IsComplex = NumTraits<Scalar>::IsComplex,
     IsTransposed = false,
     NeedToConjugate = false,
-    HasUsableDirectAccess = (    (int(XprType::Flags)&DirectAccessBit)
-                              && (   bool(XprType::IsVectorAtCompileTime)
-                                  || int(inner_stride_at_compile_time<XprType>::ret) == 1)
-                             ) ?  1 : 0,
+    HasUsableDirectAccess =
+        ((int(XprType::Flags) & DirectAccessBit) &&
+         (bool(XprType::IsVectorAtCompileTime) || int(inner_stride_at_compile_time<XprType>::ret) == 1))
+            ? 1
+            : 0,
     HasScalarFactor = false
   };
-  typedef typename conditional<bool(HasUsableDirectAccess),
-    ExtractType,
-    typename _ExtractType::PlainObject
-    >::type DirectLinearAccessType;
-  static inline EIGEN_DEVICE_FUNC ExtractType extract(const XprType& x) { return x; }
-  static inline EIGEN_DEVICE_FUNC const Scalar extractScalarFactor(const XprType&) { return Scalar(1); }
+  typedef std::conditional_t<bool(HasUsableDirectAccess), ExtractType, typename ExtractType_::PlainObject>
+      DirectLinearAccessType;
+  EIGEN_DEVICE_FUNC static inline EIGEN_DEVICE_FUNC ExtractType extract(const XprType& x) { return x; }
+  EIGEN_DEVICE_FUNC static inline EIGEN_DEVICE_FUNC const Scalar extractScalarFactor(const XprType&) {
+    return Scalar(1);
+  }
 };
 
 // pop conjugate
-template<typename Scalar, typename NestedXpr>
-struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> >
- : blas_traits<NestedXpr>
-{
+template <typename Scalar, typename NestedXpr>
+struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> > : blas_traits<NestedXpr> {
   typedef blas_traits<NestedXpr> Base;
   typedef CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> XprType;
   typedef typename Base::ExtractType ExtractType;
 
-  enum {
-    IsComplex = NumTraits<Scalar>::IsComplex,
-    NeedToConjugate = Base::NeedToConjugate ? 0 : IsComplex
-  };
-  static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
-  static inline Scalar extractScalarFactor(const XprType& x) { return conj(Base::extractScalarFactor(x.nestedExpression())); }
+  enum { IsComplex = NumTraits<Scalar>::IsComplex, NeedToConjugate = Base::NeedToConjugate ? 0 : IsComplex };
+  EIGEN_DEVICE_FUNC static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
+  EIGEN_DEVICE_FUNC static inline Scalar extractScalarFactor(const XprType& x) {
+    return conj(Base::extractScalarFactor(x.nestedExpression()));
+  }
 };
 
 // pop scalar multiple
-template<typename Scalar, typename NestedXpr, typename Plain>
-struct blas_traits<CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain>, NestedXpr> >
- : blas_traits<NestedXpr>
-{
-  enum {
-    HasScalarFactor = true
-  };
+template <typename Scalar, typename NestedXpr, typename Plain>
+struct blas_traits<
+    CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>, Plain>, NestedXpr> >
+    : blas_traits<NestedXpr> {
+  enum { HasScalarFactor = true };
   typedef blas_traits<NestedXpr> Base;
-  typedef CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain>, NestedXpr> XprType;
+  typedef CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>, Plain>, NestedXpr>
+      XprType;
   typedef typename Base::ExtractType ExtractType;
-  static inline EIGEN_DEVICE_FUNC ExtractType extract(const XprType& x) { return Base::extract(x.rhs()); }
-  static inline EIGEN_DEVICE_FUNC Scalar extractScalarFactor(const XprType& x)
-  { return x.lhs().functor().m_other * Base::extractScalarFactor(x.rhs()); }
+  EIGEN_DEVICE_FUNC static inline EIGEN_DEVICE_FUNC ExtractType extract(const XprType& x) {
+    return Base::extract(x.rhs());
+  }
+  EIGEN_DEVICE_FUNC static inline EIGEN_DEVICE_FUNC Scalar extractScalarFactor(const XprType& x) {
+    return x.lhs().functor().m_other * Base::extractScalarFactor(x.rhs());
+  }
 };
-template<typename Scalar, typename NestedXpr, typename Plain>
-struct blas_traits<CwiseBinaryOp<scalar_product_op<Scalar>, NestedXpr, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain> > >
- : blas_traits<NestedXpr>
-{
-  enum {
-    HasScalarFactor = true
-  };
+template <typename Scalar, typename NestedXpr, typename Plain>
+struct blas_traits<
+    CwiseBinaryOp<scalar_product_op<Scalar>, NestedXpr, const CwiseNullaryOp<scalar_constant_op<Scalar>, Plain> > >
+    : blas_traits<NestedXpr> {
+  enum { HasScalarFactor = true };
   typedef blas_traits<NestedXpr> Base;
-  typedef CwiseBinaryOp<scalar_product_op<Scalar>, NestedXpr, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain> > XprType;
+  typedef CwiseBinaryOp<scalar_product_op<Scalar>, NestedXpr, const CwiseNullaryOp<scalar_constant_op<Scalar>, Plain> >
+      XprType;
   typedef typename Base::ExtractType ExtractType;
-  static inline ExtractType extract(const XprType& x) { return Base::extract(x.lhs()); }
-  static inline Scalar extractScalarFactor(const XprType& x)
-  { return Base::extractScalarFactor(x.lhs()) * x.rhs().functor().m_other; }
+  EIGEN_DEVICE_FUNC static inline ExtractType extract(const XprType& x) { return Base::extract(x.lhs()); }
+  EIGEN_DEVICE_FUNC static inline Scalar extractScalarFactor(const XprType& x) {
+    return Base::extractScalarFactor(x.lhs()) * x.rhs().functor().m_other;
+  }
 };
-template<typename Scalar, typename Plain1, typename Plain2>
-struct blas_traits<CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain1>,
-                                                            const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain2> > >
- : blas_traits<CwiseNullaryOp<scalar_constant_op<Scalar>,Plain1> >
-{};
+template <typename Scalar, typename Plain1, typename Plain2>
+struct blas_traits<CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>, Plain1>,
+                                 const CwiseNullaryOp<scalar_constant_op<Scalar>, Plain2> > >
+    : blas_traits<CwiseNullaryOp<scalar_constant_op<Scalar>, Plain1> > {};
 
 // pop opposite
-template<typename Scalar, typename NestedXpr>
-struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> >
- : blas_traits<NestedXpr>
-{
-  enum {
-    HasScalarFactor = true
-  };
+template <typename Scalar, typename NestedXpr>
+struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> > : blas_traits<NestedXpr> {
+  enum { HasScalarFactor = true };
   typedef blas_traits<NestedXpr> Base;
   typedef CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> XprType;
   typedef typename Base::ExtractType ExtractType;
-  static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
-  static inline Scalar extractScalarFactor(const XprType& x)
-  { return - Base::extractScalarFactor(x.nestedExpression()); }
+  EIGEN_DEVICE_FUNC static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
+  EIGEN_DEVICE_FUNC static inline Scalar extractScalarFactor(const XprType& x) {
+    return -Base::extractScalarFactor(x.nestedExpression());
+  }
 };
 
 // pop/push transpose
-template<typename NestedXpr>
-struct blas_traits<Transpose<NestedXpr> >
- : blas_traits<NestedXpr>
-{
+template <typename NestedXpr>
+struct blas_traits<Transpose<NestedXpr> > : blas_traits<NestedXpr> {
   typedef typename NestedXpr::Scalar Scalar;
   typedef blas_traits<NestedXpr> Base;
   typedef Transpose<NestedXpr> XprType;
-  typedef Transpose<const typename Base::_ExtractType>  ExtractType; // const to get rid of a compile error; anyway blas traits are only used on the RHS
-  typedef Transpose<const typename Base::_ExtractType> _ExtractType;
-  typedef typename conditional<bool(Base::HasUsableDirectAccess),
-    ExtractType,
-    typename ExtractType::PlainObject
-    >::type DirectLinearAccessType;
-  enum {
-    IsTransposed = Base::IsTransposed ? 0 : 1
-  };
-  static inline ExtractType extract(const XprType& x) { return ExtractType(Base::extract(x.nestedExpression())); }
-  static inline Scalar extractScalarFactor(const XprType& x) { return Base::extractScalarFactor(x.nestedExpression()); }
+  typedef Transpose<const typename Base::ExtractType_>
+      ExtractType;  // const to get rid of a compile error; anyway blas traits are only used on the RHS
+  typedef Transpose<const typename Base::ExtractType_> ExtractType_;
+  typedef std::conditional_t<bool(Base::HasUsableDirectAccess), ExtractType, typename ExtractType::PlainObject>
+      DirectLinearAccessType;
+  enum { IsTransposed = Base::IsTransposed ? 0 : 1 };
+  EIGEN_DEVICE_FUNC static inline ExtractType extract(const XprType& x) {
+    return ExtractType(Base::extract(x.nestedExpression()));
+  }
+  EIGEN_DEVICE_FUNC static inline Scalar extractScalarFactor(const XprType& x) {
+    return Base::extractScalarFactor(x.nestedExpression());
+  }
 };
 
-template<typename T>
-struct blas_traits<const T>
-     : blas_traits<T>
-{};
+template <typename T>
+struct blas_traits<const T> : blas_traits<T> {};
 
-template<typename T, bool HasUsableDirectAccess=blas_traits<T>::HasUsableDirectAccess>
+template <typename T, bool HasUsableDirectAccess = blas_traits<T>::HasUsableDirectAccess>
 struct extract_data_selector {
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static const typename T::Scalar* run(const T& m)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static const typename T::Scalar* run(const T& m) {
     return blas_traits<T>::extract(m).data();
   }
 };
 
-template<typename T>
-struct extract_data_selector<T,false> {
-  static typename T::Scalar* run(const T&) { return 0; }
+template <typename T>
+struct extract_data_selector<T, false> {
+  EIGEN_DEVICE_FUNC static typename T::Scalar* run(const T&) { return 0; }
 };
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE const typename T::Scalar* extract_data(const T& m)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE const typename T::Scalar* extract_data(const T& m) {
   return extract_data_selector<T>::run(m);
 }
 
@@ -539,45 +586,37 @@
  * \c combine_scalar_factors extracts and multiplies factors from GEMM and GEMV products.
  * There is a specialization for booleans
  */
-template<typename ResScalar, typename Lhs, typename Rhs>
-struct combine_scalar_factors_impl
-{
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static ResScalar run(const Lhs& lhs, const Rhs& rhs)
-  {
+template <typename ResScalar, typename Lhs, typename Rhs>
+struct combine_scalar_factors_impl {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static ResScalar run(const Lhs& lhs, const Rhs& rhs) {
     return blas_traits<Lhs>::extractScalarFactor(lhs) * blas_traits<Rhs>::extractScalarFactor(rhs);
   }
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static ResScalar run(const ResScalar& alpha, const Lhs& lhs, const Rhs& rhs)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static ResScalar run(const ResScalar& alpha, const Lhs& lhs, const Rhs& rhs) {
     return alpha * blas_traits<Lhs>::extractScalarFactor(lhs) * blas_traits<Rhs>::extractScalarFactor(rhs);
   }
 };
-template<typename Lhs, typename Rhs>
-struct combine_scalar_factors_impl<bool, Lhs, Rhs>
-{
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static bool run(const Lhs& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs>
+struct combine_scalar_factors_impl<bool, Lhs, Rhs> {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static bool run(const Lhs& lhs, const Rhs& rhs) {
     return blas_traits<Lhs>::extractScalarFactor(lhs) && blas_traits<Rhs>::extractScalarFactor(rhs);
   }
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static bool run(const bool& alpha, const Lhs& lhs, const Rhs& rhs)
-  {
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static bool run(const bool& alpha, const Lhs& lhs, const Rhs& rhs) {
     return alpha && blas_traits<Lhs>::extractScalarFactor(lhs) && blas_traits<Rhs>::extractScalarFactor(rhs);
   }
 };
 
-template<typename ResScalar, typename Lhs, typename Rhs>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE ResScalar combine_scalar_factors(const ResScalar& alpha, const Lhs& lhs, const Rhs& rhs)
-{
-  return combine_scalar_factors_impl<ResScalar,Lhs,Rhs>::run(alpha, lhs, rhs);
+template <typename ResScalar, typename Lhs, typename Rhs>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE ResScalar combine_scalar_factors(const ResScalar& alpha, const Lhs& lhs,
+                                                                       const Rhs& rhs) {
+  return combine_scalar_factors_impl<ResScalar, Lhs, Rhs>::run(alpha, lhs, rhs);
 }
-template<typename ResScalar, typename Lhs, typename Rhs>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE ResScalar combine_scalar_factors(const Lhs& lhs, const Rhs& rhs)
-{
-  return combine_scalar_factors_impl<ResScalar,Lhs,Rhs>::run(lhs, rhs);
+template <typename ResScalar, typename Lhs, typename Rhs>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE ResScalar combine_scalar_factors(const Lhs& lhs, const Rhs& rhs) {
+  return combine_scalar_factors_impl<ResScalar, Lhs, Rhs>::run(lhs, rhs);
 }
 
+}  // end namespace internal
 
-} // end namespace internal
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_BLASUTIL_H
+#endif  // EIGEN_BLASUTIL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h
index af4e696..b16952a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h
@@ -23,55 +23,41 @@
 // to be used to declare statically aligned buffers.
 //------------------------------------------------------------------------------------------
 
-
 /* EIGEN_ALIGN_TO_BOUNDARY(n) forces data to be n-byte aligned. This is used to satisfy SIMD requirements.
  * However, we do that EVEN if vectorization (EIGEN_VECTORIZE) is disabled,
  * so that vectorization doesn't affect binary compatibility.
  *
  * If we made alignment depend on whether or not EIGEN_VECTORIZE is defined, it would be impossible to link
  * vectorized and non-vectorized code.
- * 
- * FIXME: this code can be cleaned up once we switch to proper C++11 only.
  */
 #if (defined EIGEN_CUDACC)
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __align__(n)
-  #define EIGEN_ALIGNOF(x) __alignof(x)
-#elif EIGEN_HAS_ALIGNAS
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) alignas(n)
-  #define EIGEN_ALIGNOF(x) alignof(x)
-#elif EIGEN_COMP_GNUC || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
-  #define EIGEN_ALIGNOF(x) __alignof(x)
-#elif EIGEN_COMP_MSVC
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __declspec(align(n))
-  #define EIGEN_ALIGNOF(x) __alignof(x)
-#elif EIGEN_COMP_SUNCC
-  // FIXME not sure about this one:
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
-  #define EIGEN_ALIGNOF(x) __alignof(x)
+#define EIGEN_ALIGN_TO_BOUNDARY(n) __align__(n)
+#define EIGEN_ALIGNOF(x) __alignof(x)
 #else
-  #error Please tell me what is the equivalent of alignas(n) and alignof(x) for your compiler
+#define EIGEN_ALIGN_TO_BOUNDARY(n) alignas(n)
+#define EIGEN_ALIGNOF(x) alignof(x)
 #endif
 
 // If the user explicitly disable vectorization, then we also disable alignment
 #if defined(EIGEN_DONT_VECTORIZE)
-  #if defined(EIGEN_GPUCC)
-    // GPU code is always vectorized and requires memory alignment for
-    // statically allocated buffers.
-    #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
-  #else
-    #define EIGEN_IDEAL_MAX_ALIGN_BYTES 0
-  #endif
-#elif defined(__AVX512F__)
-  // 64 bytes static alignment is preferred only if really required
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 64
-#elif defined(__AVX__)
-  // 32 bytes static alignment is preferred only if really required
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 32
+#if defined(EIGEN_GPUCC)
+// GPU code is always vectorized and requires memory alignment for
+// statically allocated buffers.
+#define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
 #else
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
+#define EIGEN_IDEAL_MAX_ALIGN_BYTES 0
 #endif
-
+#elif defined(__AVX512F__)
+// 64 bytes static alignment is preferred only if really required
+#define EIGEN_IDEAL_MAX_ALIGN_BYTES 64
+#elif defined(__AVX__)
+// 32 bytes static alignment is preferred only if really required
+#define EIGEN_IDEAL_MAX_ALIGN_BYTES 32
+#elif defined __HVX__ && (__HVX_LENGTH__ == 128)
+#define EIGEN_IDEAL_MAX_ALIGN_BYTES 128
+#else
+#define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
+#endif
 
 // EIGEN_MIN_ALIGN_BYTES defines the minimal value for which the notion of explicit alignment makes sense
 #define EIGEN_MIN_ALIGN_BYTES 16
@@ -80,99 +66,91 @@
 // that unless EIGEN_ALIGN is defined and not equal to 0, the data may not be
 // aligned at all regardless of the value of this #define.
 
-#if (defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN))  && defined(EIGEN_MAX_STATIC_ALIGN_BYTES) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
+#if (defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN)) && defined(EIGEN_MAX_STATIC_ALIGN_BYTES) && \
+    EIGEN_MAX_STATIC_ALIGN_BYTES > 0
 #error EIGEN_MAX_STATIC_ALIGN_BYTES and EIGEN_DONT_ALIGN[_STATICALLY] are both defined with EIGEN_MAX_STATIC_ALIGN_BYTES!=0. Use EIGEN_MAX_STATIC_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN_STATICALLY.
 #endif
 
 // EIGEN_DONT_ALIGN_STATICALLY and EIGEN_DONT_ALIGN are deprecated
 // They imply EIGEN_MAX_STATIC_ALIGN_BYTES=0
 #if defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN)
-  #ifdef EIGEN_MAX_STATIC_ALIGN_BYTES
-    #undef EIGEN_MAX_STATIC_ALIGN_BYTES
-  #endif
-  #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
+#ifdef EIGEN_MAX_STATIC_ALIGN_BYTES
+#undef EIGEN_MAX_STATIC_ALIGN_BYTES
+#endif
+#define EIGEN_MAX_STATIC_ALIGN_BYTES 0
 #endif
 
 #ifndef EIGEN_MAX_STATIC_ALIGN_BYTES
 
-  // Try to automatically guess what is the best default value for EIGEN_MAX_STATIC_ALIGN_BYTES
+// Try to automatically guess what is the best default value for EIGEN_MAX_STATIC_ALIGN_BYTES
 
-  // 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
-  // 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
-  // enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
-  // certain common platform (compiler+architecture combinations) to avoid these problems.
-  // Only static alignment is really problematic (relies on nonstandard compiler extensions),
-  // try to keep heap alignment even when we have to disable static alignment.
-  #if EIGEN_COMP_GNUC && !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64 || EIGEN_ARCH_MIPS)
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
-  #elif EIGEN_ARCH_ARM_OR_ARM64 && EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_MOST(4, 6)
-  // Old versions of GCC on ARM, at least 4.4, were once seen to have buggy static alignment support.
-  // Not sure which version fixed it, hopefully it doesn't affect 4.7, which is still somewhat in use.
-  // 4.8 and newer seem definitely unaffected.
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
-  #else
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 0
-  #endif
+// 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
+// 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
+// enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
+// certain common platform (compiler+architecture combinations) to avoid these problems.
+// Only static alignment is really problematic (relies on nonstandard compiler extensions),
+// try to keep heap alignment even when we have to disable static alignment.
+#if EIGEN_COMP_GNUC && \
+    !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64 || EIGEN_ARCH_MIPS)
+#define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
+#else
+#define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 0
+#endif
 
-  // static alignment is completely disabled with GCC 3, Sun Studio, and QCC/QNX
-  #if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT \
-  && !EIGEN_GCC3_OR_OLDER \
-  && !EIGEN_COMP_SUNCC \
-  && !EIGEN_OS_QNX
-    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 1
-  #else
-    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 0
-  #endif
+// static alignment is completely disabled with GCC 3, Sun Studio, and QCC/QNX
+#if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT && !EIGEN_COMP_SUNCC && !EIGEN_OS_QNX
+#define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 1
+#else
+#define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 0
+#endif
 
-  #if EIGEN_ARCH_WANTS_STACK_ALIGNMENT
-    #define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
-  #else
-    #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
-  #endif
+#if EIGEN_ARCH_WANTS_STACK_ALIGNMENT
+#define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+#else
+#define EIGEN_MAX_STATIC_ALIGN_BYTES 0
+#endif
 
 #endif
 
 // If EIGEN_MAX_ALIGN_BYTES is defined, then it is considered as an upper bound for EIGEN_MAX_STATIC_ALIGN_BYTES
-#if defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES<EIGEN_MAX_STATIC_ALIGN_BYTES
+#if defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES < EIGEN_MAX_STATIC_ALIGN_BYTES
 #undef EIGEN_MAX_STATIC_ALIGN_BYTES
 #define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
 #endif
 
-#if EIGEN_MAX_STATIC_ALIGN_BYTES==0 && !defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
-  #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+#if EIGEN_MAX_STATIC_ALIGN_BYTES == 0 && !defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
+#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
 #endif
 
 // At this stage, EIGEN_MAX_STATIC_ALIGN_BYTES>0 is the true test whether we want to align arrays on the stack or not.
-// It takes into account both the user choice to explicitly enable/disable alignment (by setting EIGEN_MAX_STATIC_ALIGN_BYTES)
-// and the architecture config (EIGEN_ARCH_WANTS_STACK_ALIGNMENT).
-// Henceforth, only EIGEN_MAX_STATIC_ALIGN_BYTES should be used.
-
+// It takes into account both the user choice to explicitly enable/disable alignment (by setting
+// EIGEN_MAX_STATIC_ALIGN_BYTES) and the architecture config (EIGEN_ARCH_WANTS_STACK_ALIGNMENT). Henceforth, only
+// EIGEN_MAX_STATIC_ALIGN_BYTES should be used.
 
 // Shortcuts to EIGEN_ALIGN_TO_BOUNDARY
-#define EIGEN_ALIGN8  EIGEN_ALIGN_TO_BOUNDARY(8)
+#define EIGEN_ALIGN8 EIGEN_ALIGN_TO_BOUNDARY(8)
 #define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16)
 #define EIGEN_ALIGN32 EIGEN_ALIGN_TO_BOUNDARY(32)
 #define EIGEN_ALIGN64 EIGEN_ALIGN_TO_BOUNDARY(64)
-#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
+#if EIGEN_MAX_STATIC_ALIGN_BYTES > 0
 #define EIGEN_ALIGN_MAX EIGEN_ALIGN_TO_BOUNDARY(EIGEN_MAX_STATIC_ALIGN_BYTES)
 #else
 #define EIGEN_ALIGN_MAX
 #endif
 
-
 // Dynamic alignment control
 
-#if defined(EIGEN_DONT_ALIGN) && defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES>0
+#if defined(EIGEN_DONT_ALIGN) && defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES > 0
 #error EIGEN_MAX_ALIGN_BYTES and EIGEN_DONT_ALIGN are both defined with EIGEN_MAX_ALIGN_BYTES!=0. Use EIGEN_MAX_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN.
 #endif
 
 #ifdef EIGEN_DONT_ALIGN
-  #ifdef EIGEN_MAX_ALIGN_BYTES
-    #undef EIGEN_MAX_ALIGN_BYTES
-  #endif
-  #define EIGEN_MAX_ALIGN_BYTES 0
+#ifdef EIGEN_MAX_ALIGN_BYTES
+#undef EIGEN_MAX_ALIGN_BYTES
+#endif
+#define EIGEN_MAX_ALIGN_BYTES 0
 #elif !defined(EIGEN_MAX_ALIGN_BYTES)
-  #define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+#define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
 #endif
 
 #if EIGEN_IDEAL_MAX_ALIGN_BYTES > EIGEN_MAX_ALIGN_BYTES
@@ -181,7 +159,6 @@
 #define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
 #endif
 
-
 #ifndef EIGEN_UNALIGNED_VECTORIZE
 #define EIGEN_UNALIGNED_VECTORIZE 1
 #endif
@@ -190,220 +167,230 @@
 
 // if alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into
 // account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks
-#if EIGEN_MAX_ALIGN_BYTES==0
-  #ifndef EIGEN_DONT_VECTORIZE
-    #define EIGEN_DONT_VECTORIZE
-  #endif
+#if EIGEN_MAX_ALIGN_BYTES == 0
+#ifndef EIGEN_DONT_VECTORIZE
+#define EIGEN_DONT_VECTORIZE
 #endif
-
+#endif
 
 // The following (except #include <malloc.h> and _M_IX86_FP ??) can likely be
 // removed as gcc 4.1 and msvc 2008 are not supported anyways.
 #if EIGEN_COMP_MSVC
-  #include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
-  #if (EIGEN_COMP_MSVC >= 1500) // 2008 or later
-    // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
-    #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64
-      #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
-    #endif
-  #endif
+#include <malloc.h>  // for _aligned_malloc -- need it regardless of whether vectorization is enabled
+// a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
+#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64
+#define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
+#endif
 #else
-  #if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) )
-    #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
-  #endif
+#if defined(__SSE2__)
+#define EIGEN_SSE2_ON_NON_MSVC
+#endif
 #endif
 
 #if !(defined(EIGEN_DONT_VECTORIZE) || defined(EIGEN_GPUCC))
 
-  #if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
+#if defined(EIGEN_SSE2_ON_NON_MSVC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
 
-    // Defines symbols for compile-time detection of which instructions are
-    // used.
-    // EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_SSE
-    #define EIGEN_VECTORIZE_SSE2
+// Defines symbols for compile-time detection of which instructions are
+// used.
+// EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_SSE
+#define EIGEN_VECTORIZE_SSE2
 
-    // Detect sse3/ssse3/sse4:
-    // gcc and icc defines __SSE3__, ...
-    // there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
-    // want to force the use of those instructions with msvc.
-    #ifdef __SSE3__
-      #define EIGEN_VECTORIZE_SSE3
-    #endif
-    #ifdef __SSSE3__
-      #define EIGEN_VECTORIZE_SSSE3
-    #endif
-    #ifdef __SSE4_1__
-      #define EIGEN_VECTORIZE_SSE4_1
-    #endif
-    #ifdef __SSE4_2__
-      #define EIGEN_VECTORIZE_SSE4_2
-    #endif
-    #ifdef __AVX__
-      #ifndef EIGEN_USE_SYCL 
-        #define EIGEN_VECTORIZE_AVX
-      #endif
-      #define EIGEN_VECTORIZE_SSE3
-      #define EIGEN_VECTORIZE_SSSE3
-      #define EIGEN_VECTORIZE_SSE4_1
-      #define EIGEN_VECTORIZE_SSE4_2
-    #endif
-    #ifdef __AVX2__
-      #ifndef EIGEN_USE_SYCL 
-        #define EIGEN_VECTORIZE_AVX2
-        #define EIGEN_VECTORIZE_AVX
-      #endif
-      #define EIGEN_VECTORIZE_SSE3
-      #define EIGEN_VECTORIZE_SSSE3
-      #define EIGEN_VECTORIZE_SSE4_1
-      #define EIGEN_VECTORIZE_SSE4_2
-    #endif
-    #if defined(__FMA__) || (EIGEN_COMP_MSVC && defined(__AVX2__))
-      // MSVC does not expose a switch dedicated for FMA
-      // For MSVC, AVX2 => FMA
-      #define EIGEN_VECTORIZE_FMA
-    #endif
-    #if defined(__AVX512F__)
-      #ifndef EIGEN_VECTORIZE_FMA
-      #if EIGEN_COMP_GNUC
-      #error Please add -mfma to your compiler flags: compiling with -mavx512f alone without SSE/AVX FMA is not supported (bug 1638).
-      #else
-      #error Please enable FMA in your compiler flags (e.g. -mfma): compiling with AVX512 alone without SSE/AVX FMA is not supported (bug 1638).
-      #endif
-      #endif
-      #ifndef EIGEN_USE_SYCL
-        #define EIGEN_VECTORIZE_AVX512
-        #define EIGEN_VECTORIZE_AVX2
-        #define EIGEN_VECTORIZE_AVX
-      #endif
-      #define EIGEN_VECTORIZE_FMA
-      #define EIGEN_VECTORIZE_SSE3
-      #define EIGEN_VECTORIZE_SSSE3
-      #define EIGEN_VECTORIZE_SSE4_1
-      #define EIGEN_VECTORIZE_SSE4_2
-      #ifndef EIGEN_USE_SYCL
-        #ifdef __AVX512DQ__
-          #define EIGEN_VECTORIZE_AVX512DQ
-        #endif
-        #ifdef __AVX512ER__
-          #define EIGEN_VECTORIZE_AVX512ER
-        #endif
-        #ifdef __AVX512BF16__
-          #define EIGEN_VECTORIZE_AVX512BF16
-        #endif
-      #endif
-    #endif
+// Detect sse3/ssse3/sse4:
+// gcc and icc defines __SSE3__, ...
+// there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
+// want to force the use of those instructions with msvc.
+#ifdef __SSE3__
+#define EIGEN_VECTORIZE_SSE3
+#endif
+#ifdef __SSSE3__
+#define EIGEN_VECTORIZE_SSSE3
+#endif
+#ifdef __SSE4_1__
+#define EIGEN_VECTORIZE_SSE4_1
+#endif
+#ifdef __SSE4_2__
+#define EIGEN_VECTORIZE_SSE4_2
+#endif
+#ifdef __AVX__
+#ifndef EIGEN_USE_SYCL
+#define EIGEN_VECTORIZE_AVX
+#endif
+#define EIGEN_VECTORIZE_SSE3
+#define EIGEN_VECTORIZE_SSSE3
+#define EIGEN_VECTORIZE_SSE4_1
+#define EIGEN_VECTORIZE_SSE4_2
+#endif
+#ifdef __AVX2__
+#ifndef EIGEN_USE_SYCL
+#define EIGEN_VECTORIZE_AVX2
+#define EIGEN_VECTORIZE_AVX
+#endif
+#define EIGEN_VECTORIZE_SSE3
+#define EIGEN_VECTORIZE_SSSE3
+#define EIGEN_VECTORIZE_SSE4_1
+#define EIGEN_VECTORIZE_SSE4_2
+#endif
+#if defined(__FMA__) || (EIGEN_COMP_MSVC && defined(__AVX2__))
+// MSVC does not expose a switch dedicated for FMA
+// For MSVC, AVX2 => FMA
+#define EIGEN_VECTORIZE_FMA
+#endif
+#if defined(__AVX512F__)
+#ifndef EIGEN_VECTORIZE_FMA
+#if EIGEN_COMP_GNUC
+#error Please add -mfma to your compiler flags: compiling with -mavx512f alone without SSE/AVX FMA is not supported (bug 1638).
+#else
+#error Please enable FMA in your compiler flags (e.g. -mfma): compiling with AVX512 alone without SSE/AVX FMA is not supported (bug 1638).
+#endif
+#endif
+#ifndef EIGEN_USE_SYCL
+#define EIGEN_VECTORIZE_AVX512
+#define EIGEN_VECTORIZE_AVX2
+#define EIGEN_VECTORIZE_AVX
+#endif
+#define EIGEN_VECTORIZE_FMA
+#define EIGEN_VECTORIZE_SSE3
+#define EIGEN_VECTORIZE_SSSE3
+#define EIGEN_VECTORIZE_SSE4_1
+#define EIGEN_VECTORIZE_SSE4_2
+#ifndef EIGEN_USE_SYCL
+#ifdef __AVX512DQ__
+#define EIGEN_VECTORIZE_AVX512DQ
+#endif
+#ifdef __AVX512ER__
+#define EIGEN_VECTORIZE_AVX512ER
+#endif
+#ifdef __AVX512BF16__
+#define EIGEN_VECTORIZE_AVX512BF16
+#endif
+#ifdef __AVX512FP16__
+#ifdef __AVX512VL__
+#define EIGEN_VECTORIZE_AVX512FP16
+#else
+#if EIGEN_COMP_GNUC
+#error Please add -mavx512vl to your compiler flags: compiling with -mavx512fp16 alone without AVX512-VL is not supported.
+#else
+#error Please enable AVX512-VL in your compiler flags (e.g. -mavx512vl): compiling with AVX512-FP16 alone without AVX512-VL is not supported.
+#endif
+#endif
+#endif
+#endif
+#endif
 
-    // Disable AVX support on broken xcode versions
-    #if defined(__apple_build_version__) && (__apple_build_version__ == 11000033 ) && ( __MAC_OS_X_VERSION_MIN_REQUIRED == 101500 )
-      // A nasty bug in the clang compiler shipped with xcode in a common compilation situation
-      // when XCode 11.0 and Mac deployment target macOS 10.15 is https://trac.macports.org/ticket/58776#no1
-      #ifdef EIGEN_VECTORIZE_AVX
-        #undef EIGEN_VECTORIZE_AVX
-        #warning "Disabling AVX support: clang compiler shipped with XCode 11.[012] generates broken assembly with -macosx-version-min=10.15 and AVX enabled. "
-        #ifdef EIGEN_VECTORIZE_AVX2
-          #undef EIGEN_VECTORIZE_AVX2
-        #endif
-        #ifdef EIGEN_VECTORIZE_FMA
-          #undef EIGEN_VECTORIZE_FMA
-        #endif
-        #ifdef EIGEN_VECTORIZE_AVX512
-          #undef EIGEN_VECTORIZE_AVX512
-        #endif
-        #ifdef EIGEN_VECTORIZE_AVX512DQ
-          #undef EIGEN_VECTORIZE_AVX512DQ
-        #endif
-        #ifdef EIGEN_VECTORIZE_AVX512ER
-          #undef EIGEN_VECTORIZE_AVX512ER
-        #endif
-      #endif
-      // NOTE: Confirmed test failures in XCode 11.0, and XCode 11.2 with  -macosx-version-min=10.15 and AVX
-      // NOTE using -macosx-version-min=10.15 with Xcode 11.0 results in runtime segmentation faults in many tests, 11.2 produce core dumps in 3 tests
-      // NOTE using -macosx-version-min=10.14 produces functioning and passing tests in all cases
-      // NOTE __clang_version__ "11.0.0 (clang-1100.0.33.8)"  XCode 11.0 <- Produces many segfault and core dumping tests
-      //                                                                    with  -macosx-version-min=10.15 and AVX
-      // NOTE __clang_version__ "11.0.0 (clang-1100.0.33.12)" XCode 11.2 <- Produces 3 core dumping tests with  
-      //                                                                    -macosx-version-min=10.15 and AVX
-    #endif
+// Disable AVX support on broken xcode versions
+#if (EIGEN_COMP_CLANGAPPLE == 11000033) && (__MAC_OS_X_VERSION_MIN_REQUIRED == 101500)
+// A nasty bug in the clang compiler shipped with xcode in a common compilation situation
+// when XCode 11.0 and Mac deployment target macOS 10.15 is https://trac.macports.org/ticket/58776#no1
+#ifdef EIGEN_VECTORIZE_AVX
+#undef EIGEN_VECTORIZE_AVX
+#warning \
+    "Disabling AVX support: clang compiler shipped with XCode 11.[012] generates broken assembly with -macosx-version-min=10.15 and AVX enabled. "
+#ifdef EIGEN_VECTORIZE_AVX2
+#undef EIGEN_VECTORIZE_AVX2
+#endif
+#ifdef EIGEN_VECTORIZE_FMA
+#undef EIGEN_VECTORIZE_FMA
+#endif
+#ifdef EIGEN_VECTORIZE_AVX512
+#undef EIGEN_VECTORIZE_AVX512
+#endif
+#ifdef EIGEN_VECTORIZE_AVX512DQ
+#undef EIGEN_VECTORIZE_AVX512DQ
+#endif
+#ifdef EIGEN_VECTORIZE_AVX512ER
+#undef EIGEN_VECTORIZE_AVX512ER
+#endif
+#endif
+// NOTE: Confirmed test failures in XCode 11.0, and XCode 11.2 with  -macosx-version-min=10.15 and AVX
+// NOTE using -macosx-version-min=10.15 with Xcode 11.0 results in runtime segmentation faults in many tests, 11.2
+// produce core dumps in 3 tests NOTE using -macosx-version-min=10.14 produces functioning and passing tests in all
+// cases NOTE __clang_version__ "11.0.0 (clang-1100.0.33.8)"  XCode 11.0 <- Produces many segfault and core dumping
+// tests
+//                                                                    with  -macosx-version-min=10.15 and AVX
+// NOTE __clang_version__ "11.0.0 (clang-1100.0.33.12)" XCode 11.2 <- Produces 3 core dumping tests with
+//                                                                    -macosx-version-min=10.15 and AVX
+#endif
 
-    // include files
+// include files
 
-    // This extern "C" works around a MINGW-w64 compilation issue
-    // https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
-    // In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
-    // However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
-    // with conflicting linkage.  The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
-    // so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
-    // notice that since these are C headers, the extern "C" is theoretically needed anyways.
-    extern "C" {
-      // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
-      // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
-      #if EIGEN_COMP_ICC >= 1110
-        #include <immintrin.h>
-      #else
-        #include <mmintrin.h>
-        #include <emmintrin.h>
-        #include <xmmintrin.h>
-        #ifdef  EIGEN_VECTORIZE_SSE3
-        #include <pmmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSSE3
-        #include <tmmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSE4_1
-        #include <smmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSE4_2
-        #include <nmmintrin.h>
-        #endif
-        #if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512)
-        #include <immintrin.h>
-        #endif
-      #endif
-    } // end extern "C"
+// This extern "C" works around a MINGW-w64 compilation issue
+// https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
+// In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
+// However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
+// with conflicting linkage.  The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
+// so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
+// notice that since these are C headers, the extern "C" is theoretically needed anyways.
+extern "C" {
+// In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
+// Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
+#if EIGEN_COMP_ICC >= 1110 || EIGEN_COMP_EMSCRIPTEN
+#include <immintrin.h>
+#else
+#include <mmintrin.h>
+#include <emmintrin.h>
+#include <xmmintrin.h>
+#ifdef EIGEN_VECTORIZE_SSE3
+#include <pmmintrin.h>
+#endif
+#ifdef EIGEN_VECTORIZE_SSSE3
+#include <tmmintrin.h>
+#endif
+#ifdef EIGEN_VECTORIZE_SSE4_1
+#include <smmintrin.h>
+#endif
+#ifdef EIGEN_VECTORIZE_SSE4_2
+#include <nmmintrin.h>
+#endif
+#if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512)
+#include <immintrin.h>
+#endif
+#endif
+}  // end extern "C"
 
-  #elif defined __VSX__
+#elif defined(__VSX__) && !defined(__APPLE__)
 
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_VSX
-    #include <altivec.h>
-    // We need to #undef all these ugly tokens defined in <altivec.h>
-    // => use __vector instead of vector
-    #undef bool
-    #undef vector
-    #undef pixel
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_VSX 1
+#include <altivec.h>
+// We need to #undef all these ugly tokens defined in <altivec.h>
+// => use __vector instead of vector
+#undef bool
+#undef vector
+#undef pixel
 
-  #elif defined __ALTIVEC__
+#elif defined __ALTIVEC__
 
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_ALTIVEC
-    #include <altivec.h>
-    // We need to #undef all these ugly tokens defined in <altivec.h>
-    // => use __vector instead of vector
-    #undef bool
-    #undef vector
-    #undef pixel
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_ALTIVEC
+#include <altivec.h>
+// We need to #undef all these ugly tokens defined in <altivec.h>
+// => use __vector instead of vector
+#undef bool
+#undef vector
+#undef pixel
 
-  #elif ((defined  __ARM_NEON) || (defined __ARM_NEON__)) && !(defined EIGEN_ARM64_USE_SVE)
+#elif ((defined __ARM_NEON) || (defined __ARM_NEON__)) && !(defined EIGEN_ARM64_USE_SVE)
 
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_NEON
-    #include <arm_neon.h>
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_NEON
+#include <arm_neon.h>
 
-  // We currently require SVE to be enabled explicitly via EIGEN_ARM64_USE_SVE and
-  // will not select the backend automatically
-  #elif (defined __ARM_FEATURE_SVE) && (defined EIGEN_ARM64_USE_SVE)
+// We currently require SVE to be enabled explicitly via EIGEN_ARM64_USE_SVE and
+// will not select the backend automatically
+#elif (defined __ARM_FEATURE_SVE) && (defined EIGEN_ARM64_USE_SVE)
 
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_SVE
-    #include <arm_sve.h>
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_SVE
+#include <arm_sve.h>
 
-    // Since we depend on knowing SVE vector lengths at compile-time, we need
-    // to ensure a fixed lengths is set
-    #if defined __ARM_FEATURE_SVE_BITS
-      #define EIGEN_ARM64_SVE_VL __ARM_FEATURE_SVE_BITS
-    #else
+// Since we depend on knowing SVE vector lengths at compile-time, we need
+// to ensure a fixed lengths is set
+#if defined __ARM_FEATURE_SVE_BITS
+#define EIGEN_ARM64_SVE_VL __ARM_FEATURE_SVE_BITS
+#else
 #error "Eigen requires a fixed SVE lector length but EIGEN_ARM64_SVE_VL is not set."
 #endif
 
@@ -428,6 +415,12 @@
 #include <msa.h>
 #endif
 
+#elif defined __HVX__ && (__HVX_LENGTH__ == 128)
+
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_HVX
+#include <hexagon_types.h>
+
 #endif
 #endif
 
@@ -435,43 +428,49 @@
 // compilers seem to follow this. We therefore include it explicitly.
 // See also: https://bugs.llvm.org/show_bug.cgi?id=47955
 #if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
-  #include <arm_fp16.h>
+#include <arm_fp16.h>
 #endif
 
-#if defined(__F16C__) && (!defined(EIGEN_GPUCC) && (!defined(EIGEN_COMP_CLANG) || EIGEN_COMP_CLANG>=380))
-  // We can use the optimized fp16 to float and float to fp16 conversion routines
-  #define EIGEN_HAS_FP16_C
+#if defined(__F16C__) && !defined(EIGEN_GPUCC) && (!EIGEN_COMP_CLANG_STRICT || EIGEN_CLANG_STRICT_AT_LEAST(3, 8, 0))
+// We can use the optimized fp16 to float and float to fp16 conversion routines
+#define EIGEN_HAS_FP16_C
 
-  #if defined(EIGEN_COMP_CLANG)
-    // Workaround for clang: The FP16C intrinsics for clang are included by
-    // immintrin.h, as opposed to emmintrin.h as suggested by Intel:
-    // https://software.intel.com/sites/landingpage/IntrinsicsGuide/#othertechs=FP16C&expand=1711
-    #include <immintrin.h>
-  #endif
+#if EIGEN_COMP_GNUC
+// Make sure immintrin.h is included, even if e.g. vectorization is
+// explicitly disabled (see also issue #2395).
+// Note that FP16C intrinsics for gcc and clang are included by immintrin.h,
+// as opposed to emmintrin.h as suggested by Intel:
+// https://software.intel.com/sites/landingpage/IntrinsicsGuide/#othertechs=FP16C&expand=1711
+#include <immintrin.h>
+#endif
 #endif
 
 #if defined EIGEN_CUDACC
-  #define EIGEN_VECTORIZE_GPU
-  #include <vector_types.h>
-  #if EIGEN_CUDA_SDK_VER >= 70500
-    #define EIGEN_HAS_CUDA_FP16
-  #endif
+#define EIGEN_VECTORIZE_GPU
+#include <vector_types.h>
+#if EIGEN_CUDA_SDK_VER >= 70500
+#define EIGEN_HAS_CUDA_FP16
+#endif
 #endif
 
 #if defined(EIGEN_HAS_CUDA_FP16)
-  #include <cuda_runtime_api.h>
-  #include <cuda_fp16.h>
+#include <cuda_runtime_api.h>
+#include <cuda_fp16.h>
 #endif
 
 #if defined(EIGEN_HIPCC)
-  #define EIGEN_VECTORIZE_GPU
-  #include <hip/hip_vector_types.h>
-  #define EIGEN_HAS_HIP_FP16
-  #include <hip/hip_fp16.h>
+#define EIGEN_VECTORIZE_GPU
+#include <hip/hip_vector_types.h>
+#define EIGEN_HAS_HIP_FP16
+#include <hip/hip_fp16.h>
+#define EIGEN_HAS_HIP_BF16
+#include <hip/hip_bfloat16.h>
 #endif
 
-
 /** \brief Namespace containing all symbols from the %Eigen library. */
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 inline static const char *SimdInstructionSetsInUse(void) {
@@ -506,7 +505,6 @@
 #endif
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-
-#endif // EIGEN_CONFIGURE_VECTORIZATION_H
+#endif  // EIGEN_CONFIGURE_VECTORIZATION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h
index 35dcaa7..8b06c67 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h
@@ -12,169 +12,173 @@
 #ifndef EIGEN_CONSTANTS_H
 #define EIGEN_CONSTANTS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** This value means that a positive quantity (e.g., a size) is not known at compile-time, and that instead the value is
-  * stored in some runtime variable.
-  *
-  * Changing the value of Dynamic breaks the ABI, as Dynamic is often used as a template parameter for Matrix.
-  */
+ * stored in some runtime variable.
+ *
+ * Changing the value of Dynamic breaks the ABI, as Dynamic is often used as a template parameter for Matrix.
+ */
 const int Dynamic = -1;
 
-/** This value means that a signed quantity (e.g., a signed index) is not known at compile-time, and that instead its value
-  * has to be specified at runtime.
-  */
+/** This value means that a signed quantity (e.g., a signed index) is not known at compile-time, and that instead its
+ * value has to be specified at runtime.
+ */
 const int DynamicIndex = 0xffffff;
 
 /** This value means that the increment to go from one value to another in a sequence is not constant for each step.
-  */
+ */
 const int UndefinedIncr = 0xfffffe;
 
 /** This value means +Infinity; it is currently used only as the p parameter to MatrixBase::lpNorm<int>().
-  * The value Infinity there means the L-infinity norm.
-  */
+ * The value Infinity there means the L-infinity norm.
+ */
 const int Infinity = -1;
 
 /** This value means that the cost to evaluate an expression coefficient is either very expensive or
-  * cannot be known at compile time.
-  *
-  * This value has to be positive to (1) simplify cost computation, and (2) allow to distinguish between a very expensive and very very expensive expressions.
-  * It thus must also be large enough to make sure unrolling won't happen and that sub expressions will be evaluated, but not too large to avoid overflow.
-  */
+ * cannot be known at compile time.
+ *
+ * This value has to be positive to (1) simplify cost computation, and (2) allow to distinguish between a very expensive
+ * and very very expensive expressions. It thus must also be large enough to make sure unrolling won't happen and that
+ * sub expressions will be evaluated, but not too large to avoid overflow.
+ */
 const int HugeCost = 10000;
 
 /** \defgroup flags Flags
-  * \ingroup Core_Module
-  *
-  * These are the possible bits which can be OR'ed to constitute the flags of a matrix or
-  * expression.
-  *
-  * It is important to note that these flags are a purely compile-time notion. They are a compile-time property of
-  * an expression type, implemented as enum's. They are not stored in memory at runtime, and they do not incur any
-  * runtime overhead.
-  *
-  * \sa MatrixBase::Flags
-  */
+ * \ingroup Core_Module
+ *
+ * These are the possible bits which can be OR'ed to constitute the flags of a matrix or
+ * expression.
+ *
+ * It is important to note that these flags are a purely compile-time notion. They are a compile-time property of
+ * an expression type, implemented as enum's. They are not stored in memory at runtime, and they do not incur any
+ * runtime overhead.
+ *
+ * \sa MatrixBase::Flags
+ */
 
 /** \ingroup flags
-  *
-  * for a matrix, this means that the storage order is row-major.
-  * If this bit is not set, the storage order is column-major.
-  * For an expression, this determines the storage order of
-  * the matrix created by evaluation of that expression.
-  * \sa \blank  \ref TopicStorageOrders */
+ *
+ * for a matrix, this means that the storage order is row-major.
+ * If this bit is not set, the storage order is column-major.
+ * For an expression, this determines the storage order of
+ * the matrix created by evaluation of that expression.
+ * \sa \blank  \ref TopicStorageOrders */
 const unsigned int RowMajorBit = 0x1;
 
 /** \ingroup flags
-  * means the expression should be evaluated by the calling expression */
+ * means the expression should be evaluated by the calling expression */
 const unsigned int EvalBeforeNestingBit = 0x2;
 
 /** \ingroup flags
-  * \deprecated
-  * means the expression should be evaluated before any assignment */
-EIGEN_DEPRECATED
-const unsigned int EvalBeforeAssigningBit = 0x4; // FIXME deprecated
+ * \deprecated
+ * means the expression should be evaluated before any assignment */
+EIGEN_DEPRECATED const unsigned int EvalBeforeAssigningBit = 0x4;  // FIXME deprecated
 
 /** \ingroup flags
-  *
-  * Short version: means the expression might be vectorized
-  *
-  * Long version: means that the coefficients can be handled by packets
-  * and start at a memory location whose alignment meets the requirements
-  * of the present CPU architecture for optimized packet access. In the fixed-size
-  * case, there is the additional condition that it be possible to access all the
-  * coefficients by packets (this implies the requirement that the size be a multiple of 16 bytes,
-  * and that any nontrivial strides don't break the alignment). In the dynamic-size case,
-  * there is no such condition on the total size and strides, so it might not be possible to access
-  * all coeffs by packets.
-  *
-  * \note This bit can be set regardless of whether vectorization is actually enabled.
-  *       To check for actual vectorizability, see \a ActualPacketAccessBit.
-  */
+ *
+ * Short version: means the expression might be vectorized
+ *
+ * Long version: means that the coefficients can be handled by packets
+ * and start at a memory location whose alignment meets the requirements
+ * of the present CPU architecture for optimized packet access. In the fixed-size
+ * case, there is the additional condition that it be possible to access all the
+ * coefficients by packets (this implies the requirement that the size be a multiple of 16 bytes,
+ * and that any nontrivial strides don't break the alignment). In the dynamic-size case,
+ * there is no such condition on the total size and strides, so it might not be possible to access
+ * all coeffs by packets.
+ *
+ * \note This bit can be set regardless of whether vectorization is actually enabled.
+ *       To check for actual vectorizability, see \a ActualPacketAccessBit.
+ */
 const unsigned int PacketAccessBit = 0x8;
 
 #ifdef EIGEN_VECTORIZE
 /** \ingroup flags
-  *
-  * If vectorization is enabled (EIGEN_VECTORIZE is defined) this constant
-  * is set to the value \a PacketAccessBit.
-  *
-  * If vectorization is not enabled (EIGEN_VECTORIZE is not defined) this constant
-  * is set to the value 0.
-  */
+ *
+ * If vectorization is enabled (EIGEN_VECTORIZE is defined) this constant
+ * is set to the value \a PacketAccessBit.
+ *
+ * If vectorization is not enabled (EIGEN_VECTORIZE is not defined) this constant
+ * is set to the value 0.
+ */
 const unsigned int ActualPacketAccessBit = PacketAccessBit;
 #else
 const unsigned int ActualPacketAccessBit = 0x0;
 #endif
 
 /** \ingroup flags
-  *
-  * Short version: means the expression can be seen as 1D vector.
-  *
-  * Long version: means that one can access the coefficients
-  * of this expression by coeff(int), and coeffRef(int) in the case of a lvalue expression. These
-  * index-based access methods are guaranteed
-  * to not have to do any runtime computation of a (row, col)-pair from the index, so that it
-  * is guaranteed that whenever it is available, index-based access is at least as fast as
-  * (row,col)-based access. Expressions for which that isn't possible don't have the LinearAccessBit.
-  *
-  * If both PacketAccessBit and LinearAccessBit are set, then the
-  * packets of this expression can be accessed by packet(int), and writePacket(int) in the case of a
-  * lvalue expression.
-  *
-  * Typically, all vector expressions have the LinearAccessBit, but there is one exception:
-  * Product expressions don't have it, because it would be troublesome for vectorization, even when the
-  * Product is a vector expression. Thus, vector Product expressions allow index-based coefficient access but
-  * not index-based packet access, so they don't have the LinearAccessBit.
-  */
+ *
+ * Short version: means the expression can be seen as 1D vector.
+ *
+ * Long version: means that one can access the coefficients
+ * of this expression by coeff(int), and coeffRef(int) in the case of a lvalue expression. These
+ * index-based access methods are guaranteed
+ * to not have to do any runtime computation of a (row, col)-pair from the index, so that it
+ * is guaranteed that whenever it is available, index-based access is at least as fast as
+ * (row,col)-based access. Expressions for which that isn't possible don't have the LinearAccessBit.
+ *
+ * If both PacketAccessBit and LinearAccessBit are set, then the
+ * packets of this expression can be accessed by packet(int), and writePacket(int) in the case of a
+ * lvalue expression.
+ *
+ * Typically, all vector expressions have the LinearAccessBit, but there is one exception:
+ * Product expressions don't have it, because it would be troublesome for vectorization, even when the
+ * Product is a vector expression. Thus, vector Product expressions allow index-based coefficient access but
+ * not index-based packet access, so they don't have the LinearAccessBit.
+ */
 const unsigned int LinearAccessBit = 0x10;
 
 /** \ingroup flags
-  *
-  * Means the expression has a coeffRef() method, i.e. is writable as its individual coefficients are directly addressable.
-  * This rules out read-only expressions.
-  *
-  * Note that DirectAccessBit and LvalueBit are mutually orthogonal, as there are examples of expression having one but note
-  * the other:
-  *   \li writable expressions that don't have a very simple memory layout as a strided array, have LvalueBit but not DirectAccessBit
-  *   \li Map-to-const expressions, for example Map<const Matrix>, have DirectAccessBit but not LvalueBit
-  *
-  * Expressions having LvalueBit also have their coeff() method returning a const reference instead of returning a new value.
-  */
+ *
+ * Means the expression has a coeffRef() method, i.e. is writable as its individual coefficients are directly
+ * addressable. This rules out read-only expressions.
+ *
+ * Note that DirectAccessBit and LvalueBit are mutually orthogonal, as there are examples of expression having one but
+ * not the other: \li writable expressions that don't have a very simple memory layout as a strided array, have
+ * LvalueBit but not DirectAccessBit \li Map-to-const expressions, for example Map<const Matrix>, have DirectAccessBit
+ * but not LvalueBit
+ *
+ * Expressions having LvalueBit also have their coeff() method returning a const reference instead of returning a new
+ * value.
+ */
 const unsigned int LvalueBit = 0x20;
 
 /** \ingroup flags
-  *
-  * Means that the underlying array of coefficients can be directly accessed as a plain strided array. The memory layout
-  * of the array of coefficients must be exactly the natural one suggested by rows(), cols(),
-  * outerStride(), innerStride(), and the RowMajorBit. This rules out expressions such as Diagonal, whose coefficients,
-  * though referencable, do not have such a regular memory layout.
-  *
-  * See the comment on LvalueBit for an explanation of how LvalueBit and DirectAccessBit are mutually orthogonal.
-  */
+ *
+ * Means that the underlying array of coefficients can be directly accessed as a plain strided array. The memory layout
+ * of the array of coefficients must be exactly the natural one suggested by rows(), cols(),
+ * outerStride(), innerStride(), and the RowMajorBit. This rules out expressions such as Diagonal, whose coefficients,
+ * though referencable, do not have such a regular memory layout.
+ *
+ * See the comment on LvalueBit for an explanation of how LvalueBit and DirectAccessBit are mutually orthogonal.
+ */
 const unsigned int DirectAccessBit = 0x40;
 
 /** \deprecated \ingroup flags
-  *
-  * means the first coefficient packet is guaranteed to be aligned.
-  * An expression cannot have the AlignedBit without the PacketAccessBit flag.
-  * In other words, this means we are allow to perform an aligned packet access to the first element regardless
-  * of the expression kind:
-  * \code
-  * expression.packet<Aligned>(0);
-  * \endcode
-  */
+ *
+ * means the first coefficient packet is guaranteed to be aligned.
+ * An expression cannot have the AlignedBit without the PacketAccessBit flag.
+ * In other words, this means we are allow to perform an aligned packet access to the first element regardless
+ * of the expression kind:
+ * \code
+ * expression.packet<Aligned>(0);
+ * \endcode
+ */
 EIGEN_DEPRECATED const unsigned int AlignedBit = 0x80;
 
 const unsigned int NestByRefBit = 0x100;
 
 /** \ingroup flags
-  *
-  * for an expression, this means that the storage order
-  * can be either row-major or column-major.
-  * The precise choice will be decided at evaluation time or when
-  * combined with other expressions.
-  * \sa \blank  \ref RowMajorBit, \ref TopicStorageOrders */
+ *
+ * for an expression, this means that the storage order
+ * can be either row-major or column-major.
+ * The precise choice will be decided at evaluation time or when
+ * combined with other expressions.
+ * \sa \blank  \ref RowMajorBit, \ref TopicStorageOrders */
 const unsigned int NoPreferredStorageOrderBit = 0x200;
 
 /** \ingroup flags
@@ -190,65 +194,63 @@
   */
 const unsigned int CompressedAccessBit = 0x400;
 
-
 // list of flags that are inherited by default
-const unsigned int HereditaryBits = RowMajorBit
-                                  | EvalBeforeNestingBit;
+const unsigned int HereditaryBits = RowMajorBit | EvalBeforeNestingBit;
 
 /** \defgroup enums Enumerations
-  * \ingroup Core_Module
-  *
-  * Various enumerations used in %Eigen. Many of these are used as template parameters.
-  */
+ * \ingroup Core_Module
+ *
+ * Various enumerations used in %Eigen. Many of these are used as template parameters.
+ */
 
 /** \ingroup enums
-  * Enum containing possible values for the \c Mode or \c UpLo parameter of
-  * MatrixBase::selfadjointView() and MatrixBase::triangularView(), and selfadjoint solvers. */
+ * Enum containing possible values for the \c Mode or \c UpLo parameter of
+ * MatrixBase::selfadjointView() and MatrixBase::triangularView(), and selfadjoint solvers. */
 enum UpLoType {
   /** View matrix as a lower triangular matrix. */
-  Lower=0x1,                      
+  Lower = 0x1,
   /** View matrix as an upper triangular matrix. */
-  Upper=0x2,                      
+  Upper = 0x2,
   /** %Matrix has ones on the diagonal; to be used in combination with #Lower or #Upper. */
-  UnitDiag=0x4, 
+  UnitDiag = 0x4,
   /** %Matrix has zeros on the diagonal; to be used in combination with #Lower or #Upper. */
-  ZeroDiag=0x8,
+  ZeroDiag = 0x8,
   /** View matrix as a lower triangular matrix with ones on the diagonal. */
-  UnitLower=UnitDiag|Lower, 
+  UnitLower = UnitDiag | Lower,
   /** View matrix as an upper triangular matrix with ones on the diagonal. */
-  UnitUpper=UnitDiag|Upper,
+  UnitUpper = UnitDiag | Upper,
   /** View matrix as a lower triangular matrix with zeros on the diagonal. */
-  StrictlyLower=ZeroDiag|Lower, 
+  StrictlyLower = ZeroDiag | Lower,
   /** View matrix as an upper triangular matrix with zeros on the diagonal. */
-  StrictlyUpper=ZeroDiag|Upper,
+  StrictlyUpper = ZeroDiag | Upper,
   /** Used in BandMatrix and SelfAdjointView to indicate that the matrix is self-adjoint. */
-  SelfAdjoint=0x10,
+  SelfAdjoint = 0x10,
   /** Used to support symmetric, non-selfadjoint, complex matrices. */
-  Symmetric=0x20
+  Symmetric = 0x20
 };
 
 /** \ingroup enums
-  * Enum for indicating whether a buffer is aligned or not. */
+ * Enum for indicating whether a buffer is aligned or not. */
 enum AlignmentType {
-  Unaligned=0,        /**< Data pointer has no specific alignment. */
-  Aligned8=8,         /**< Data pointer is aligned on a 8 bytes boundary. */
-  Aligned16=16,       /**< Data pointer is aligned on a 16 bytes boundary. */
-  Aligned32=32,       /**< Data pointer is aligned on a 32 bytes boundary. */
-  Aligned64=64,       /**< Data pointer is aligned on a 64 bytes boundary. */
-  Aligned128=128,     /**< Data pointer is aligned on a 128 bytes boundary. */
-  AlignedMask=255,
-  Aligned=16,         /**< \deprecated Synonym for Aligned16. */
-#if EIGEN_MAX_ALIGN_BYTES==128
+  Unaligned = 0,    /**< Data pointer has no specific alignment. */
+  Aligned8 = 8,     /**< Data pointer is aligned on a 8 bytes boundary. */
+  Aligned16 = 16,   /**< Data pointer is aligned on a 16 bytes boundary. */
+  Aligned32 = 32,   /**< Data pointer is aligned on a 32 bytes boundary. */
+  Aligned64 = 64,   /**< Data pointer is aligned on a 64 bytes boundary. */
+  Aligned128 = 128, /**< Data pointer is aligned on a 128 bytes boundary. */
+  AlignedMask = 255,
+  Aligned = 16, /**< \deprecated Synonym for Aligned16. */
+#if EIGEN_MAX_ALIGN_BYTES == 128
   AlignedMax = Aligned128
-#elif EIGEN_MAX_ALIGN_BYTES==64
+#elif EIGEN_MAX_ALIGN_BYTES == 64
   AlignedMax = Aligned64
-#elif EIGEN_MAX_ALIGN_BYTES==32
+#elif EIGEN_MAX_ALIGN_BYTES == 32
   AlignedMax = Aligned32
-#elif EIGEN_MAX_ALIGN_BYTES==16
+#elif EIGEN_MAX_ALIGN_BYTES == 16
   AlignedMax = Aligned16
-#elif EIGEN_MAX_ALIGN_BYTES==8
+#elif EIGEN_MAX_ALIGN_BYTES == 8
   AlignedMax = Aligned8
-#elif EIGEN_MAX_ALIGN_BYTES==0
+#elif EIGEN_MAX_ALIGN_BYTES == 0
   AlignedMax = Unaligned
 #else
 #error Invalid value for EIGEN_MAX_ALIGN_BYTES
@@ -256,35 +258,35 @@
 };
 
 /** \ingroup enums
-  * Enum containing possible values for the \p Direction parameter of
-  * Reverse, PartialReduxExpr and VectorwiseOp. */
-enum DirectionType { 
-  /** For Reverse, all columns are reversed; 
-    * for PartialReduxExpr and VectorwiseOp, act on columns. */
-  Vertical, 
-  /** For Reverse, all rows are reversed; 
-    * for PartialReduxExpr and VectorwiseOp, act on rows. */
-  Horizontal, 
-  /** For Reverse, both rows and columns are reversed; 
-    * not used for PartialReduxExpr and VectorwiseOp. */
-  BothDirections 
+ * Enum containing possible values for the \p Direction parameter of
+ * Reverse, PartialReduxExpr and VectorwiseOp. */
+enum DirectionType {
+  /** For Reverse, all columns are reversed;
+   * for PartialReduxExpr and VectorwiseOp, act on columns. */
+  Vertical,
+  /** For Reverse, all rows are reversed;
+   * for PartialReduxExpr and VectorwiseOp, act on rows. */
+  Horizontal,
+  /** For Reverse, both rows and columns are reversed;
+   * not used for PartialReduxExpr and VectorwiseOp. */
+  BothDirections
 };
 
 /** \internal \ingroup enums
-  * Enum to specify how to traverse the entries of a matrix. */
+ * Enum to specify how to traverse the entries of a matrix. */
 enum TraversalType {
   /** \internal Default traversal, no vectorization, no index-based access */
   DefaultTraversal,
   /** \internal No vectorization, use index-based access to have only one for loop instead of 2 nested loops */
   LinearTraversal,
   /** \internal Equivalent to a slice vectorization for fixed-size matrices having good alignment
-    * and good size */
+   * and good size */
   InnerVectorizedTraversal,
   /** \internal Vectorization path using a single loop plus scalar loops for the
-    * unaligned boundaries */
+   * unaligned boundaries */
   LinearVectorizedTraversal,
   /** \internal Generic vectorization path using one vectorized loop per row/column with some
-    * scalar loops to handle the unaligned boundaries */
+   * scalar loops to handle the unaligned boundaries */
   SliceVectorizedTraversal,
   /** \internal Special case to properly handle incompatible scalar types or other defecting cases*/
   InvalidTraversal,
@@ -293,27 +295,24 @@
 };
 
 /** \internal \ingroup enums
-  * Enum to specify whether to unroll loops when traversing over the entries of a matrix. */
+ * Enum to specify whether to unroll loops when traversing over the entries of a matrix. */
 enum UnrollingType {
   /** \internal Do not unroll loops. */
   NoUnrolling,
   /** \internal Unroll only the inner loop, but not the outer loop. */
   InnerUnrolling,
-  /** \internal Unroll both the inner and the outer loop. If there is only one loop, 
-    * because linear traversal is used, then unroll that loop. */
+  /** \internal Unroll both the inner and the outer loop. If there is only one loop,
+   * because linear traversal is used, then unroll that loop. */
   CompleteUnrolling
 };
 
 /** \internal \ingroup enums
-  * Enum to specify whether to use the default (built-in) implementation or the specialization. */
-enum SpecializedType {
-  Specialized,
-  BuiltIn
-};
+ * Enum to specify whether to use the default (built-in) implementation or the specialization. */
+enum SpecializedType { Specialized, BuiltIn };
 
 /** \ingroup enums
-  * Enum containing possible values for the \p _Options template parameter of
-  * Matrix, Array and BandMatrix. */
+ * Enum containing possible values for the \p Options_ template parameter of
+ * Matrix, Array and BandMatrix. */
 enum StorageOptions {
   /** Storage order is column major (see \ref TopicStorageOrders). */
   ColMajor = 0,
@@ -326,7 +325,7 @@
 };
 
 /** \ingroup enums
-  * Enum for specifying whether to apply or solve on the left or right. */
+ * Enum for specifying whether to apply or solve on the left or right. */
 enum SideType {
   /** Apply transformation on the left. */
   OnTheLeft = 1,
@@ -352,83 +351,82 @@
  *     EIGEN_UNUSED NoChange_t NoChange;
  *   }
  *
- * on the ground that it feels dangerous to disambiguate overloaded functions on enum/integer types.  
+ * on the ground that it feels dangerous to disambiguate overloaded functions on enum/integer types.
  * However, this leads to "variable declared but never referenced" warnings on Intel Composer XE,
  * and we do not know how to get rid of them (bug 450).
  */
 
-enum NoChange_t   { NoChange };
+enum NoChange_t { NoChange };
 enum Sequential_t { Sequential };
-enum Default_t    { Default };
+enum Default_t { Default };
 
 /** \internal \ingroup enums
-  * Used in AmbiVector. */
-enum AmbiVectorMode {
-  IsDense         = 0,
-  IsSparse
-};
+ * Used in AmbiVector. */
+enum AmbiVectorMode { IsDense = 0, IsSparse };
 
 /** \ingroup enums
-  * Used as template parameter in DenseCoeffBase and MapBase to indicate 
-  * which accessors should be provided. */
+ * Used as template parameter in DenseCoeffBase and MapBase to indicate
+ * which accessors should be provided. */
 enum AccessorLevels {
   /** Read-only access via a member function. */
-  ReadOnlyAccessors, 
+  ReadOnlyAccessors,
   /** Read/write access via member functions. */
-  WriteAccessors, 
+  WriteAccessors,
   /** Direct read-only access to the coefficients. */
-  DirectAccessors, 
+  DirectAccessors,
   /** Direct read/write access to the coefficients. */
   DirectWriteAccessors
 };
 
 /** \ingroup enums
-  * Enum with options to give to various decompositions. */
+ * Enum with options to give to various decompositions. */
 enum DecompositionOptions {
   /** \internal Not used (meant for LDLT?). */
-  Pivoting            = 0x01, 
+  Pivoting = 0x01,
   /** \internal Not used (meant for LDLT?). */
-  NoPivoting          = 0x02, 
+  NoPivoting = 0x02,
   /** Used in JacobiSVD to indicate that the square matrix U is to be computed. */
-  ComputeFullU        = 0x04,
+  ComputeFullU = 0x04,
   /** Used in JacobiSVD to indicate that the thin matrix U is to be computed. */
-  ComputeThinU        = 0x08,
+  ComputeThinU = 0x08,
   /** Used in JacobiSVD to indicate that the square matrix V is to be computed. */
-  ComputeFullV        = 0x10,
+  ComputeFullV = 0x10,
   /** Used in JacobiSVD to indicate that the thin matrix V is to be computed. */
-  ComputeThinV        = 0x20,
+  ComputeThinV = 0x20,
   /** Used in SelfAdjointEigenSolver and GeneralizedSelfAdjointEigenSolver to specify
-    * that only the eigenvalues are to be computed and not the eigenvectors. */
-  EigenvaluesOnly     = 0x40,
+   * that only the eigenvalues are to be computed and not the eigenvectors. */
+  EigenvaluesOnly = 0x40,
   /** Used in SelfAdjointEigenSolver and GeneralizedSelfAdjointEigenSolver to specify
-    * that both the eigenvalues and the eigenvectors are to be computed. */
+   * that both the eigenvalues and the eigenvectors are to be computed. */
   ComputeEigenvectors = 0x80,
   /** \internal */
   EigVecMask = EigenvaluesOnly | ComputeEigenvectors,
   /** Used in GeneralizedSelfAdjointEigenSolver to indicate that it should
-    * solve the generalized eigenproblem \f$ Ax = \lambda B x \f$. */
-  Ax_lBx              = 0x100,
+   * solve the generalized eigenproblem \f$ Ax = \lambda B x \f$. */
+  Ax_lBx = 0x100,
   /** Used in GeneralizedSelfAdjointEigenSolver to indicate that it should
-    * solve the generalized eigenproblem \f$ ABx = \lambda x \f$. */
-  ABx_lx              = 0x200,
+   * solve the generalized eigenproblem \f$ ABx = \lambda x \f$. */
+  ABx_lx = 0x200,
   /** Used in GeneralizedSelfAdjointEigenSolver to indicate that it should
-    * solve the generalized eigenproblem \f$ BAx = \lambda x \f$. */
-  BAx_lx              = 0x400,
+   * solve the generalized eigenproblem \f$ BAx = \lambda x \f$. */
+  BAx_lx = 0x400,
   /** \internal */
   GenEigMask = Ax_lBx | ABx_lx | BAx_lx
 };
 
 /** \ingroup enums
-  * Possible values for the \p QRPreconditioner template parameter of JacobiSVD. */
+ * Possible values for the \p QRPreconditioner template parameter of JacobiSVD. */
 enum QRPreconditioners {
-  /** Do not specify what is to be done if the SVD of a non-square matrix is asked for. */
-  NoQRPreconditioner,
-  /** Use a QR decomposition without pivoting as the first step. */
-  HouseholderQRPreconditioner,
   /** Use a QR decomposition with column pivoting as the first step. */
-  ColPivHouseholderQRPreconditioner,
+  ColPivHouseholderQRPreconditioner = 0x0,
+  /** Do not specify what is to be done if the SVD of a non-square matrix is asked for. */
+  NoQRPreconditioner = 0x40,
+  /** Use a QR decomposition without pivoting as the first step. */
+  HouseholderQRPreconditioner = 0x80,
   /** Use a QR decomposition with full pivoting as the first step. */
-  FullPivHouseholderQRPreconditioner
+  FullPivHouseholderQRPreconditioner = 0xC0,
+  /** Used to disable the QR Preconditioner in BDCSVD. */
+  DisableQRDecomposition = NoQRPreconditioner
 };
 
 #ifdef Success
@@ -436,72 +434,83 @@
 #endif
 
 /** \ingroup enums
-  * Enum for reporting the status of a computation. */
+ * Enum for reporting the status of a computation. */
 enum ComputationInfo {
   /** Computation was successful. */
-  Success = 0,        
+  Success = 0,
   /** The provided data did not satisfy the prerequisites. */
-  NumericalIssue = 1, 
+  NumericalIssue = 1,
   /** Iterative procedure did not converge. */
   NoConvergence = 2,
   /** The inputs are invalid, or the algorithm has been improperly called.
-    * When assertions are enabled, such errors trigger an assert. */
+   * When assertions are enabled, such errors trigger an assert. */
   InvalidInput = 3
 };
 
 /** \ingroup enums
-  * Enum used to specify how a particular transformation is stored in a matrix.
-  * \sa Transform, Hyperplane::transform(). */
+ * Enum used to specify how a particular transformation is stored in a matrix.
+ * \sa Transform, Hyperplane::transform(). */
 enum TransformTraits {
   /** Transformation is an isometry. */
-  Isometry      = 0x1,
-  /** Transformation is an affine transformation stored as a (Dim+1)^2 matrix whose last row is 
-    * assumed to be [0 ... 0 1]. */
-  Affine        = 0x2,
+  Isometry = 0x1,
+  /** Transformation is an affine transformation stored as a (Dim+1)^2 matrix whose last row is
+   * assumed to be [0 ... 0 1]. */
+  Affine = 0x2,
   /** Transformation is an affine transformation stored as a (Dim) x (Dim+1) matrix. */
   AffineCompact = 0x10 | Affine,
   /** Transformation is a general projective transformation stored as a (Dim+1)^2 matrix. */
-  Projective    = 0x20
+  Projective = 0x20
 };
 
 /** \internal \ingroup enums
-  * Enum used to choose between implementation depending on the computer architecture. */
-namespace Architecture
-{
-  enum Type {
-    Generic = 0x0,
-    SSE = 0x1,
-    AltiVec = 0x2,
-    VSX = 0x3,
-    NEON = 0x4,
-    MSA = 0x5,
-    SVE = 0x6,
+ * Enum used to choose between implementation depending on the computer architecture. */
+namespace Architecture {
+enum Type {
+  Generic = 0x0,
+  SSE = 0x1,
+  AltiVec = 0x2,
+  VSX = 0x3,
+  NEON = 0x4,
+  MSA = 0x5,
+  SVE = 0x6,
+  HVX = 0x7,
 #if defined EIGEN_VECTORIZE_SSE
-    Target = SSE
+  Target = SSE
 #elif defined EIGEN_VECTORIZE_ALTIVEC
-    Target = AltiVec
+  Target = AltiVec
 #elif defined EIGEN_VECTORIZE_VSX
-    Target = VSX
+  Target = VSX
 #elif defined EIGEN_VECTORIZE_NEON
-    Target = NEON
+  Target = NEON
 #elif defined EIGEN_VECTORIZE_SVE
-    Target = SVE
+  Target = SVE
 #elif defined EIGEN_VECTORIZE_MSA
-    Target = MSA
+  Target = MSA
+#elif defined EIGEN_VECTORIZE_HVX
+  Target = HVX
 #else
-    Target = Generic
+  Target = Generic
 #endif
-  };
-}
+};
+}  // namespace Architecture
 
 /** \internal \ingroup enums
-  * Enum used as template parameter in Product and product evaluators. */
-enum ProductImplType
-{ DefaultProduct=0, LazyProduct, AliasFreeProduct, CoeffBasedProductMode, LazyCoeffBasedProductMode, OuterProduct, InnerProduct, GemvProduct, GemmProduct };
+ * Enum used as template parameter in Product and product evaluators. */
+enum ProductImplType {
+  DefaultProduct = 0,
+  LazyProduct,
+  AliasFreeProduct,
+  CoeffBasedProductMode,
+  LazyCoeffBasedProductMode,
+  OuterProduct,
+  InnerProduct,
+  GemvProduct,
+  GemmProduct
+};
 
 /** \internal \ingroup enums
-  * Enum used in experimental parallel implementation. */
-enum Action {GetAction, SetAction};
+ * Enum used in experimental parallel implementation. */
+enum Action { GetAction, SetAction };
 
 /** The type used to identify a dense storage. */
 struct Dense {};
@@ -525,29 +534,52 @@
 struct ArrayXpr {};
 
 // An evaluator must define its shape. By default, it can be one of the following:
-struct DenseShape             { static std::string debugName() { return "DenseShape"; } };
-struct SolverShape            { static std::string debugName() { return "SolverShape"; } };
-struct HomogeneousShape       { static std::string debugName() { return "HomogeneousShape"; } };
-struct DiagonalShape          { static std::string debugName() { return "DiagonalShape"; } };
-struct BandShape              { static std::string debugName() { return "BandShape"; } };
-struct TriangularShape        { static std::string debugName() { return "TriangularShape"; } };
-struct SelfAdjointShape       { static std::string debugName() { return "SelfAdjointShape"; } };
-struct PermutationShape       { static std::string debugName() { return "PermutationShape"; } };
-struct TranspositionsShape    { static std::string debugName() { return "TranspositionsShape"; } };
-struct SparseShape            { static std::string debugName() { return "SparseShape"; } };
+struct DenseShape {
+  static std::string debugName() { return "DenseShape"; }
+};
+struct SolverShape {
+  static std::string debugName() { return "SolverShape"; }
+};
+struct HomogeneousShape {
+  static std::string debugName() { return "HomogeneousShape"; }
+};
+struct DiagonalShape {
+  static std::string debugName() { return "DiagonalShape"; }
+};
+struct SkewSymmetricShape {
+  static std::string debugName() { return "SkewSymmetricShape"; }
+};
+struct BandShape {
+  static std::string debugName() { return "BandShape"; }
+};
+struct TriangularShape {
+  static std::string debugName() { return "TriangularShape"; }
+};
+struct SelfAdjointShape {
+  static std::string debugName() { return "SelfAdjointShape"; }
+};
+struct PermutationShape {
+  static std::string debugName() { return "PermutationShape"; }
+};
+struct TranspositionsShape {
+  static std::string debugName() { return "TranspositionsShape"; }
+};
+struct SparseShape {
+  static std::string debugName() { return "SparseShape"; }
+};
 
 namespace internal {
 
-  // random access iterators based on coeff*() accessors.
+// random access iterators based on coeff*() accessors.
 struct IndexBased {};
 
-// evaluator based on iterators to access coefficients. 
+// evaluator based on iterators to access coefficients.
 struct IteratorBased {};
 
 /** \internal
  * Constants for comparison functors
  */
-enum ComparisonName {
+enum ComparisonName : unsigned int {
   cmp_EQ = 0,
   cmp_LT = 1,
   cmp_LE = 2,
@@ -556,8 +588,8 @@
   cmp_GT = 5,
   cmp_GE = 6
 };
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_CONSTANTS_H
+#endif  // EIGEN_CONSTANTS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
index 9a630e4..eb25943 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
@@ -1,117 +1,155 @@
 #ifndef EIGEN_WARNINGS_DISABLED
 #define EIGEN_WARNINGS_DISABLED
 
-#ifdef _MSC_VER
-  // 4100 - unreferenced formal parameter (occurred e.g. in aligned_allocator::destroy(pointer p))
-  // 4101 - unreferenced local variable
-  // 4181 - qualifier applied to reference type ignored
-  // 4211 - nonstandard extension used : redefined extern to static
-  // 4244 - 'argument' : conversion from 'type1' to 'type2', possible loss of data
-  // 4273 - QtAlignedMalloc, inconsistent DLL linkage
-  // 4324 - structure was padded due to declspec(align())
-  // 4503 - decorated name length exceeded, name was truncated
-  // 4512 - assignment operator could not be generated
-  // 4522 - 'class' : multiple assignment operators specified
-  // 4700 - uninitialized local variable 'xyz' used
-  // 4714 - function marked as __forceinline not inlined
-  // 4717 - 'function' : recursive on all control paths, function will cause runtime stack overflow
-  // 4800 - 'type' : forcing value to bool 'true' or 'false' (performance warning)
-  #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
-    #pragma warning( push )
-  #endif
-  #pragma warning( disable : 4100 4101 4181 4211 4244 4273 4324 4503 4512 4522 4700 4714 4717 4800)
+#if defined(_MSC_VER)
+// 4100 - unreferenced formal parameter (occurred e.g. in aligned_allocator::destroy(pointer p))
+// 4101 - unreferenced local variable
+// 4127 - conditional expression is constant
+// 4181 - qualifier applied to reference type ignored
+// 4211 - nonstandard extension used : redefined extern to static
+// 4244 - 'argument' : conversion from 'type1' to 'type2', possible loss of data
+// 4273 - QtAlignedMalloc, inconsistent DLL linkage
+// 4324 - structure was padded due to declspec(align())
+// 4503 - decorated name length exceeded, name was truncated
+// 4512 - assignment operator could not be generated
+// 4522 - 'class' : multiple assignment operators specified
+// 4700 - uninitialized local variable 'xyz' used
+// 4714 - function marked as __forceinline not inlined
+// 4717 - 'function' : recursive on all control paths, function will cause runtime stack overflow
+// 4800 - 'type' : forcing value to bool 'true' or 'false' (performance warning)
+#ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
+#pragma warning(push)
+#endif
+#pragma warning(disable : 4100 4101 4127 4181 4211 4244 4273 4324 4503 4512 4522 4700 4714 4717 4800)
+// We currently rely on has_denorm in tests, and need it defined correctly for half/bfloat16.
+#ifndef _SILENCE_CXX23_DENORM_DEPRECATION_WARNING
+#define EIGEN_REENABLE_CXX23_DENORM_DEPRECATION_WARNING 1
+#define _SILENCE_CXX23_DENORM_DEPRECATION_WARNING
+#endif
 
 #elif defined __INTEL_COMPILER
-  // 2196 - routine is both "inline" and "noinline" ("noinline" assumed)
-  //        ICC 12 generates this warning even without any inline keyword, when defining class methods 'inline' i.e. inside of class body
-  //        typedef that may be a reference type.
-  // 279  - controlling expression is constant
-  //        ICC 12 generates this warning on assert(constant_expression_depending_on_template_params) and frankly this is a legitimate use case.
-  // 1684 - conversion from pointer to same-sized integral type (potential portability problem)
-  // 2259 - non-pointer conversion from "Eigen::Index={ptrdiff_t={long}}" to "int" may lose significant bits
-  #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
-    #pragma warning push
-  #endif
-  #pragma warning disable 2196 279 1684 2259
+// 2196 - routine is both "inline" and "noinline" ("noinline" assumed)
+//        ICC 12 generates this warning even without any inline keyword, when defining class methods 'inline' i.e.
+//        inside of class body typedef that may be a reference type.
+// 279  - controlling expression is constant
+//        ICC 12 generates this warning on assert(constant_expression_depending_on_template_params) and frankly this is
+//        a legitimate use case.
+// 1684 - conversion from pointer to same-sized integral type (potential portability problem)
+// 2259 - non-pointer conversion from "Eigen::Index={ptrdiff_t={long}}" to "int" may lose significant bits
+#ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
+#pragma warning push
+#endif
+#pragma warning disable 2196 279 1684 2259
 
 #elif defined __clang__
-  // -Wconstant-logical-operand - warning: use of logical && with constant operand; switch to bitwise & or remove constant
-  //     this is really a stupid warning as it warns on compile-time expressions involving enums
-  #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
-    #pragma clang diagnostic push
-  #endif
-  #pragma clang diagnostic ignored "-Wconstant-logical-operand"
-  #if __clang_major__ >= 3 && __clang_minor__ >= 5
-    #pragma clang diagnostic ignored "-Wabsolute-value"
-  #endif
-  #if __clang_major__ >= 10
-    #pragma clang diagnostic ignored "-Wimplicit-int-float-conversion"
-  #endif
-  #if ( defined(__ALTIVEC__) || defined(__VSX__) ) && __cplusplus < 201103L
-    // warning: generic selections are a C11-specific feature
-    // ignoring warnings thrown at vec_ctf in Altivec/PacketMath.h
-    #pragma clang diagnostic ignored "-Wc11-extensions"
-  #endif
+#ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
+#pragma clang diagnostic push
+#endif
+#if defined(__has_warning)
+// -Wconstant-logical-operand - warning: use of logical && with constant operand; switch to bitwise & or remove constant
+//     this is really a stupid warning as it warns on compile-time expressions involving enums
+#if __has_warning("-Wconstant-logical-operand")
+#pragma clang diagnostic ignored "-Wconstant-logical-operand"
+#endif
+#if __has_warning("-Wimplicit-int-float-conversion")
+#pragma clang diagnostic ignored "-Wimplicit-int-float-conversion"
+#endif
+#if (defined(__ALTIVEC__) || defined(__VSX__)) && (!defined(__STDC_VERSION__) || (__STDC_VERSION__ < 201112L))
+// warning: generic selections are a C11-specific feature
+// ignoring warnings thrown at vec_ctf in Altivec/PacketMath.h
+#if __has_warning("-Wc11-extensions")
+#pragma clang diagnostic ignored "-Wc11-extensions"
+#endif
+#endif
+#endif
 
-#elif defined __GNUC__
+#elif defined __GNUC__ && !defined(__FUJITSU)
 
-  #if (!defined(EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS)) &&  (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
-    #pragma GCC diagnostic push
-  #endif
-  // g++ warns about local variables shadowing member functions, which is too strict
-  #pragma GCC diagnostic ignored "-Wshadow"
-  #if __GNUC__ == 4 && __GNUC_MINOR__ < 8
-    // Until g++-4.7 there are warnings when comparing unsigned int vs 0, even in templated functions:
-    #pragma GCC diagnostic ignored "-Wtype-limits"
-  #endif
-  #if __GNUC__>=6
-    #pragma GCC diagnostic ignored "-Wignored-attributes"
-  #endif
-  #if __GNUC__==7
-    // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
-    #pragma GCC diagnostic ignored "-Wattributes"
-  #endif
-  #if __GNUC__>=8
-    #pragma GCC diagnostic ignored "-Wclass-memaccess"
-  #endif
-  #if __GNUC__>=11
-    // This warning is a false positive
-    #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
-  #endif
-  #if __GNUC__>=12
-    // This warning is a false positive
-    #pragma GCC diagnostic ignored "-Warray-bounds"
-  #endif
+#if (!defined(EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS)) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
+#pragma GCC diagnostic push
+#endif
+// g++ warns about local variables shadowing member functions, which is too strict
+#pragma GCC diagnostic ignored "-Wshadow"
+#if __GNUC__ == 4 && __GNUC_MINOR__ < 8
+// Until g++-4.7 there are warnings when comparing unsigned int vs 0, even in templated functions:
+#pragma GCC diagnostic ignored "-Wtype-limits"
+#endif
+#if __GNUC__ >= 6
+#pragma GCC diagnostic ignored "-Wignored-attributes"
+#endif
+#if __GNUC__ == 7
+// See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
+#pragma GCC diagnostic ignored "-Wattributes"
+#endif
+#if __GNUC__>=8
+#pragma GCC diagnostic ignored "-Wclass-memaccess"
+#endif
+#if __GNUC__>=11
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif
+#if __GNUC__>=12
+#pragma GCC diagnostic ignored "-Warray-bounds"
+#endif
 #endif
 
 #if defined __NVCC__
-  #pragma diag_suppress boolean_controlling_expr_is_constant
-  // Disable the "statement is unreachable" message
-  #pragma diag_suppress code_is_unreachable
-  // Disable the "dynamic initialization in unreachable code" message
-  #pragma diag_suppress initialization_not_reachable
-  // Disable the "invalid error number" message that we get with older versions of nvcc
-  #pragma diag_suppress 1222
-  // Disable the "calling a __host__ function from a __host__ __device__ function is not allowed" messages (yes, there are many of them and they seem to change with every version of the compiler)
-  #pragma diag_suppress 2527
-  #pragma diag_suppress 2529
-  #pragma diag_suppress 2651
-  #pragma diag_suppress 2653
-  #pragma diag_suppress 2668
-  #pragma diag_suppress 2669
-  #pragma diag_suppress 2670
-  #pragma diag_suppress 2671
-  #pragma diag_suppress 2735
-  #pragma diag_suppress 2737
-  #pragma diag_suppress 2739
+// MSVC 14.16 (required by CUDA 9.*) does not support the _Pragma keyword, so
+// we instead use Microsoft's __pragma extension.
+#if defined _MSC_VER
+#define EIGEN_MAKE_PRAGMA(X) __pragma(#X)
+#else
+#define EIGEN_MAKE_PRAGMA(X) _Pragma(#X)
+#endif
+#if defined __NVCC_DIAG_PRAGMA_SUPPORT__
+#define EIGEN_NV_DIAG_SUPPRESS(X) EIGEN_MAKE_PRAGMA(nv_diag_suppress X)
+#else
+#define EIGEN_NV_DIAG_SUPPRESS(X) EIGEN_MAKE_PRAGMA(diag_suppress X)
+#endif
+
+EIGEN_NV_DIAG_SUPPRESS(boolean_controlling_expr_is_constant)
+// Disable the "statement is unreachable" message
+EIGEN_NV_DIAG_SUPPRESS(code_is_unreachable)
+// Disable the "dynamic initialization in unreachable code" message
+EIGEN_NV_DIAG_SUPPRESS(initialization_not_reachable)
+// Disable the "invalid error number" message that we get with older versions of nvcc
+EIGEN_NV_DIAG_SUPPRESS(1222)
+// Disable the "calling a __host__ function from a __host__ __device__ function is not allowed" messages (yes, there are
+// many of them and they seem to change with every version of the compiler)
+EIGEN_NV_DIAG_SUPPRESS(2527)
+EIGEN_NV_DIAG_SUPPRESS(2529)
+EIGEN_NV_DIAG_SUPPRESS(2651)
+EIGEN_NV_DIAG_SUPPRESS(2653)
+EIGEN_NV_DIAG_SUPPRESS(2668)
+EIGEN_NV_DIAG_SUPPRESS(2669)
+EIGEN_NV_DIAG_SUPPRESS(2670)
+EIGEN_NV_DIAG_SUPPRESS(2671)
+EIGEN_NV_DIAG_SUPPRESS(2735)
+EIGEN_NV_DIAG_SUPPRESS(2737)
+EIGEN_NV_DIAG_SUPPRESS(2739)
+EIGEN_NV_DIAG_SUPPRESS(2885)
+EIGEN_NV_DIAG_SUPPRESS(2888)
+EIGEN_NV_DIAG_SUPPRESS(2976)
+EIGEN_NV_DIAG_SUPPRESS(2979)
+EIGEN_NV_DIAG_SUPPRESS(20011)
+EIGEN_NV_DIAG_SUPPRESS(20014)
+// Disable the "// __device__ annotation is ignored on a function(...) that is
+//              explicitly defaulted on its first declaration" message.
+// The __device__ annotation seems to actually be needed in some cases,
+// otherwise resulting in kernel runtime errors.
+EIGEN_NV_DIAG_SUPPRESS(2886)
+EIGEN_NV_DIAG_SUPPRESS(2929)
+EIGEN_NV_DIAG_SUPPRESS(2977)
+EIGEN_NV_DIAG_SUPPRESS(20012)
+#undef EIGEN_NV_DIAG_SUPPRESS
+#undef EIGEN_MAKE_PRAGMA
 #endif
 
 #else
 // warnings already disabled:
-# ifndef EIGEN_WARNINGS_DISABLED_2
-#  define EIGEN_WARNINGS_DISABLED_2
-# elif defined(EIGEN_INTERNAL_DEBUGGING)
-#  error "Do not include \"DisableStupidWarnings.h\" recursively more than twice!"
-# endif
+#ifndef EIGEN_WARNINGS_DISABLED_2
+#define EIGEN_WARNINGS_DISABLED_2
+#elif defined(EIGEN_INTERNAL_DEBUGGING)
+#error "Do not include \"DisableStupidWarnings.h\" recursively more than twice!"
+#endif
 
-#endif // not EIGEN_WARNINGS_DISABLED
+#endif  // not EIGEN_WARNINGS_DISABLED
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h
new file mode 100644
index 0000000..2b11552
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h
@@ -0,0 +1,272 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_EMULATE_ARRAY_H
+#define EIGEN_EMULATE_ARRAY_H
+
+// CUDA doesn't support the STL containers, so we use our own instead.
+#if defined(EIGEN_GPUCC) || defined(EIGEN_AVOID_STL_ARRAY)
+
+namespace Eigen {
+template <typename T, size_t n>
+class array {
+ public:
+  typedef T value_type;
+  typedef T* iterator;
+  typedef const T* const_iterator;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE iterator begin() { return values; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const_iterator begin() const { return values; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE iterator end() { return values + n; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const_iterator end() const { return values + n; }
+
+#if !defined(EIGEN_GPUCC)
+  typedef std::reverse_iterator<iterator> reverse_iterator;
+  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE reverse_iterator rbegin() { return reverse_iterator(end()); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE reverse_iterator rend() { return reverse_iterator(begin()); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }
+#endif
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& operator[](size_t index) {
+    eigen_internal_assert(index < size());
+    return values[index];
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& operator[](size_t index) const {
+    eigen_internal_assert(index < size());
+    return values[index];
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& at(size_t index) {
+    eigen_assert(index < size());
+    return values[index];
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& at(size_t index) const {
+    eigen_assert(index < size());
+    return values[index];
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& front() { return values[0]; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& front() const { return values[0]; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& back() { return values[n - 1]; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& back() const { return values[n - 1]; }
+
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static std::size_t size() { return n; }
+
+  T values[n];
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array() {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v) {
+    EIGEN_STATIC_ASSERT(n == 1, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v1, const T& v2) {
+    EIGEN_STATIC_ASSERT(n == 2, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v1;
+    values[1] = v2;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v1, const T& v2, const T& v3) {
+    EIGEN_STATIC_ASSERT(n == 3, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v1;
+    values[1] = v2;
+    values[2] = v3;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v1, const T& v2, const T& v3, const T& v4) {
+    EIGEN_STATIC_ASSERT(n == 4, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v1;
+    values[1] = v2;
+    values[2] = v3;
+    values[3] = v4;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v1, const T& v2, const T& v3, const T& v4, const T& v5) {
+    EIGEN_STATIC_ASSERT(n == 5, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v1;
+    values[1] = v2;
+    values[2] = v3;
+    values[3] = v4;
+    values[4] = v5;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v1, const T& v2, const T& v3, const T& v4, const T& v5,
+                                              const T& v6) {
+    EIGEN_STATIC_ASSERT(n == 6, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v1;
+    values[1] = v2;
+    values[2] = v3;
+    values[3] = v4;
+    values[4] = v5;
+    values[5] = v6;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v1, const T& v2, const T& v3, const T& v4, const T& v5,
+                                              const T& v6, const T& v7) {
+    EIGEN_STATIC_ASSERT(n == 7, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v1;
+    values[1] = v2;
+    values[2] = v3;
+    values[3] = v4;
+    values[4] = v5;
+    values[5] = v6;
+    values[6] = v7;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(const T& v1, const T& v2, const T& v3, const T& v4, const T& v5,
+                                              const T& v6, const T& v7, const T& v8) {
+    EIGEN_STATIC_ASSERT(n == 8, YOU_MADE_A_PROGRAMMING_MISTAKE)
+    values[0] = v1;
+    values[1] = v2;
+    values[2] = v3;
+    values[3] = v4;
+    values[4] = v5;
+    values[5] = v6;
+    values[6] = v7;
+    values[7] = v8;
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array(std::initializer_list<T> l) {
+    eigen_assert(l.size() == n);
+    internal::smart_copy(l.begin(), l.end(), values);
+  }
+};
+
+// Specialize array for zero size
+template <typename T>
+class array<T, 0> {
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& operator[](size_t) {
+    eigen_assert(false && "Can't index a zero size array");
+    return dummy;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& operator[](size_t) const {
+    eigen_assert(false && "Can't index a zero size array");
+    return dummy;
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& front() {
+    eigen_assert(false && "Can't index a zero size array");
+    return dummy;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& front() const {
+    eigen_assert(false && "Can't index a zero size array");
+    return dummy;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& back() {
+    eigen_assert(false && "Can't index a zero size array");
+    return dummy;
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& back() const {
+    eigen_assert(false && "Can't index a zero size array");
+    return dummy;
+  }
+
+  static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE std::size_t size() { return 0; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE array() : dummy() {}
+
+  EIGEN_DEVICE_FUNC array(std::initializer_list<T> l) : dummy() {
+    EIGEN_UNUSED_VARIABLE(l);
+    eigen_assert(l.size() == 0);
+  }
+
+ private:
+  T dummy;
+};
+
+// Comparison operator
+// Todo: implement !=, <, <=, >,  and >=
+template <class T, std::size_t N>
+EIGEN_DEVICE_FUNC bool operator==(const array<T, N>& lhs, const array<T, N>& rhs) {
+  for (std::size_t i = 0; i < N; ++i) {
+    if (lhs[i] != rhs[i]) {
+      return false;
+    }
+  }
+  return true;
+}
+
+namespace internal {
+template <std::size_t I_, class T, std::size_t N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& array_get(array<T, N>& a) {
+  return a[I_];
+}
+template <std::size_t I_, class T, std::size_t N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& array_get(const array<T, N>& a) {
+  return a[I_];
+}
+
+template <class T, std::size_t N>
+struct array_size<array<T, N> > {
+  enum { value = N };
+};
+template <class T, std::size_t N>
+struct array_size<array<T, N>&> {
+  enum { value = N };
+};
+template <class T, std::size_t N>
+struct array_size<const array<T, N> > {
+  enum { value = N };
+};
+template <class T, std::size_t N>
+struct array_size<const array<T, N>&> {
+  enum { value = N };
+};
+
+}  // end namespace internal
+}  // end namespace Eigen
+
+#else
+
+// The compiler supports c++11, and we're not targeting cuda: use std::array as Eigen::array
+#include <array>
+
+namespace Eigen {
+
+template <typename T, std::size_t N>
+using array = std::array<T, N>;
+
+namespace internal {
+/* std::get is only constexpr in C++14, not yet in C++11
+ *     - libstdc++ from version 4.7 onwards has it nevertheless,
+ *                                          so use that
+ *     - libstdc++ older versions: use _M_instance directly
+ *     - libc++ all versions so far: use __elems_ directly
+ *     - all other libs: use std::get to be portable, but
+ *                       this may not be constexpr
+ */
+#if defined(__GLIBCXX__) && __GLIBCXX__ < 20120322
+#define STD_GET_ARR_HACK a._M_instance[I_]
+#elif defined(_LIBCPP_VERSION)
+#define STD_GET_ARR_HACK a.__elems_[I_]
+#else
+#define STD_GET_ARR_HACK std::template get<I_, T, N>(a)
+#endif
+
+template <std::size_t I_, class T, std::size_t N>
+constexpr inline T& array_get(std::array<T, N>& a) {
+  return (T&)STD_GET_ARR_HACK;
+}
+template <std::size_t I_, class T, std::size_t N>
+constexpr inline T&& array_get(std::array<T, N>&& a) {
+  return (T&&)STD_GET_ARR_HACK;
+}
+template <std::size_t I_, class T, std::size_t N>
+constexpr inline T const& array_get(std::array<T, N> const& a) {
+  return (T const&)STD_GET_ARR_HACK;
+}
+
+#undef STD_GET_ARR_HACK
+
+}  // end namespace internal
+}  // end namespace Eigen
+
+#endif
+
+#endif  // EIGEN_EMULATE_ARRAY_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h
index 2f9cc44..c312939 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h
@@ -11,312 +11,495 @@
 #ifndef EIGEN_FORWARDDECLARATIONS_H
 #define EIGEN_FORWARDDECLARATIONS_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
-template<typename T> struct traits;
+template <typename T>
+struct traits;
 
 // here we say once and for all that traits<const T> == traits<T>
 // When constness must affect traits, it has to be constness on template parameters on which T itself depends.
 // For example, traits<Map<const T> > != traits<Map<T> >, but
 //              traits<const Map<T> > == traits<Map<T> >
-template<typename T> struct traits<const T> : traits<T> {};
+template <typename T>
+struct traits<const T> : traits<T> {};
 
-template<typename Derived> struct has_direct_access
-{
+template <typename Derived>
+struct has_direct_access {
   enum { ret = (traits<Derived>::Flags & DirectAccessBit) ? 1 : 0 };
 };
 
-template<typename Derived> struct accessors_level
-{
-  enum { has_direct_access = (traits<Derived>::Flags & DirectAccessBit) ? 1 : 0,
-         has_write_access = (traits<Derived>::Flags & LvalueBit) ? 1 : 0,
-         value = has_direct_access ? (has_write_access ? DirectWriteAccessors : DirectAccessors)
-                                   : (has_write_access ? WriteAccessors       : ReadOnlyAccessors)
+template <typename Derived>
+struct accessors_level {
+  enum {
+    has_direct_access = (traits<Derived>::Flags & DirectAccessBit) ? 1 : 0,
+    has_write_access = (traits<Derived>::Flags & LvalueBit) ? 1 : 0,
+    value = has_direct_access ? (has_write_access ? DirectWriteAccessors : DirectAccessors)
+                              : (has_write_access ? WriteAccessors : ReadOnlyAccessors)
   };
 };
 
-template<typename T> struct evaluator_traits;
+template <typename T>
+struct evaluator_traits;
 
-template< typename T> struct evaluator;
+template <typename T>
+struct evaluator;
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename T> struct NumTraits;
+template <typename T>
+struct NumTraits;
 
-template<typename Derived> struct EigenBase;
-template<typename Derived> class DenseBase;
-template<typename Derived> class PlainObjectBase;
-template<typename Derived, int Level> class DenseCoeffsBase;
+template <typename Derived>
+struct EigenBase;
+template <typename Derived>
+class DenseBase;
+template <typename Derived>
+class PlainObjectBase;
+template <typename Derived, int Level>
+class DenseCoeffsBase;
 
-template<typename _Scalar, int _Rows, int _Cols,
-         int _Options = AutoAlign |
-#if EIGEN_GNUC_AT(3,4)
-    // workaround a bug in at least gcc 3.4.6
-    // the innermost ?: ternary operator is misparsed. We write it slightly
-    // differently and this makes gcc 3.4.6 happy, but it's ugly.
-    // The error would only show up with EIGEN_DEFAULT_TO_ROW_MAJOR is defined
-    // (when EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION is RowMajor)
-                          ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
-                          : !(_Cols==1 && _Rows!=1) ?  EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
-                          : Eigen::ColMajor ),
-#else
-                          ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
-                          : (_Cols==1 && _Rows!=1) ? Eigen::ColMajor
-                          : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
-#endif
-         int _MaxRows = _Rows,
-         int _MaxCols = _Cols
-> class Matrix;
+template <typename Scalar_, int Rows_, int Cols_,
+          int Options_ = AutoAlign | ((Rows_ == 1 && Cols_ != 1)   ? Eigen::RowMajor
+                                      : (Cols_ == 1 && Rows_ != 1) ? Eigen::ColMajor
+                                                                   : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
+          int MaxRows_ = Rows_, int MaxCols_ = Cols_>
+class Matrix;
 
-template<typename Derived> class MatrixBase;
-template<typename Derived> class ArrayBase;
+template <typename Derived>
+class MatrixBase;
+template <typename Derived>
+class ArrayBase;
 
-template<typename ExpressionType, unsigned int Added, unsigned int Removed> class Flagged;
-template<typename ExpressionType, template <typename> class StorageBase > class NoAlias;
-template<typename ExpressionType> class NestByValue;
-template<typename ExpressionType> class ForceAlignedAccess;
-template<typename ExpressionType> class SwapWrapper;
+template <typename ExpressionType, unsigned int Added, unsigned int Removed>
+class Flagged;
+template <typename ExpressionType, template <typename> class StorageBase>
+class NoAlias;
+template <typename ExpressionType>
+class NestByValue;
+template <typename ExpressionType>
+class ForceAlignedAccess;
+template <typename ExpressionType>
+class SwapWrapper;
 
-template<typename XprType, int BlockRows=Dynamic, int BlockCols=Dynamic, bool InnerPanel = false> class Block;
-template<typename XprType, typename RowIndices, typename ColIndices> class IndexedView;
-template<typename XprType, int Rows=Dynamic, int Cols=Dynamic, int Order=0> class Reshaped;
+template <typename XprType, int BlockRows = Dynamic, int BlockCols = Dynamic, bool InnerPanel = false>
+class Block;
+template <typename XprType, typename RowIndices, typename ColIndices>
+class IndexedView;
+template <typename XprType, int Rows = Dynamic, int Cols = Dynamic, int Order = 0>
+class Reshaped;
 
-template<typename MatrixType, int Size=Dynamic> class VectorBlock;
-template<typename MatrixType> class Transpose;
-template<typename MatrixType> class Conjugate;
-template<typename NullaryOp, typename MatrixType>         class CwiseNullaryOp;
-template<typename UnaryOp,   typename MatrixType>         class CwiseUnaryOp;
-template<typename ViewOp,    typename MatrixType>         class CwiseUnaryView;
-template<typename BinaryOp,  typename Lhs, typename Rhs>  class CwiseBinaryOp;
-template<typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>  class CwiseTernaryOp;
-template<typename Decomposition, typename Rhstype>        class Solve;
-template<typename XprType>                                class Inverse;
+template <typename MatrixType, int Size = Dynamic>
+class VectorBlock;
+template <typename MatrixType>
+class Transpose;
+template <typename MatrixType>
+class Conjugate;
+template <typename NullaryOp, typename MatrixType>
+class CwiseNullaryOp;
+template <typename UnaryOp, typename MatrixType>
+class CwiseUnaryOp;
+template <typename BinaryOp, typename Lhs, typename Rhs>
+class CwiseBinaryOp;
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>
+class CwiseTernaryOp;
+template <typename Decomposition, typename Rhstype>
+class Solve;
+template <typename XprType>
+class Inverse;
 
-template<typename Lhs, typename Rhs, int Option = DefaultProduct> class Product;
+template <typename Lhs, typename Rhs, int Option = DefaultProduct>
+class Product;
 
-template<typename Derived> class DiagonalBase;
-template<typename _DiagonalVectorType> class DiagonalWrapper;
-template<typename _Scalar, int SizeAtCompileTime, int MaxSizeAtCompileTime=SizeAtCompileTime> class DiagonalMatrix;
-template<typename MatrixType, typename DiagonalType, int ProductOrder> class DiagonalProduct;
-template<typename MatrixType, int Index = 0> class Diagonal;
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime = SizeAtCompileTime, typename IndexType=int> class PermutationMatrix;
-template<int SizeAtCompileTime, int MaxSizeAtCompileTime = SizeAtCompileTime, typename IndexType=int> class Transpositions;
-template<typename Derived> class PermutationBase;
-template<typename Derived> class TranspositionsBase;
-template<typename _IndicesType> class PermutationWrapper;
-template<typename _IndicesType> class TranspositionsWrapper;
+template <typename Derived>
+class DiagonalBase;
+template <typename DiagonalVectorType_>
+class DiagonalWrapper;
+template <typename Scalar_, int SizeAtCompileTime, int MaxSizeAtCompileTime = SizeAtCompileTime>
+class DiagonalMatrix;
+template <typename MatrixType, typename DiagonalType, int ProductOrder>
+class DiagonalProduct;
+template <typename MatrixType, int Index = 0>
+class Diagonal;
+template <typename Derived>
+class SkewSymmetricBase;
+template <typename VectorType_>
+class SkewSymmetricWrapper;
+template <typename Scalar_>
+class SkewSymmetricMatrix3;
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime = SizeAtCompileTime, typename IndexType = int>
+class PermutationMatrix;
+template <int SizeAtCompileTime, int MaxSizeAtCompileTime = SizeAtCompileTime, typename IndexType = int>
+class Transpositions;
+template <typename Derived>
+class PermutationBase;
+template <typename Derived>
+class TranspositionsBase;
+template <typename IndicesType_>
+class PermutationWrapper;
+template <typename IndicesType_>
+class TranspositionsWrapper;
 
-template<typename Derived,
-         int Level = internal::accessors_level<Derived>::has_write_access ? WriteAccessors : ReadOnlyAccessors
-> class MapBase;
-template<int OuterStrideAtCompileTime, int InnerStrideAtCompileTime> class Stride;
-template<int Value = Dynamic> class InnerStride;
-template<int Value = Dynamic> class OuterStride;
-template<typename MatrixType, int MapOptions=Unaligned, typename StrideType = Stride<0,0> > class Map;
-template<typename Derived> class RefBase;
-template<typename PlainObjectType, int Options = 0,
-         typename StrideType = typename internal::conditional<PlainObjectType::IsVectorAtCompileTime,InnerStride<1>,OuterStride<> >::type > class Ref;
+template <typename Derived,
+          int Level = internal::accessors_level<Derived>::has_write_access ? WriteAccessors : ReadOnlyAccessors>
+class MapBase;
+template <int OuterStrideAtCompileTime, int InnerStrideAtCompileTime>
+class Stride;
+template <int Value = Dynamic>
+class InnerStride;
+template <int Value = Dynamic>
+class OuterStride;
+template <typename MatrixType, int MapOptions = Unaligned, typename StrideType = Stride<0, 0>>
+class Map;
+template <typename Derived>
+class RefBase;
+template <typename PlainObjectType, int Options = 0,
+          typename StrideType =
+              typename std::conditional_t<PlainObjectType::IsVectorAtCompileTime, InnerStride<1>, OuterStride<>>>
+class Ref;
+template <typename ViewOp, typename MatrixType, typename StrideType = Stride<0, 0>>
+class CwiseUnaryView;
 
-template<typename Derived> class TriangularBase;
-template<typename MatrixType, unsigned int Mode> class TriangularView;
-template<typename MatrixType, unsigned int Mode> class SelfAdjointView;
-template<typename MatrixType> class SparseView;
-template<typename ExpressionType> class WithFormat;
-template<typename MatrixType> struct CommaInitializer;
-template<typename Derived> class ReturnByValue;
-template<typename ExpressionType> class ArrayWrapper;
-template<typename ExpressionType> class MatrixWrapper;
-template<typename Derived> class SolverBase;
-template<typename XprType> class InnerIterator;
+template <typename Derived>
+class TriangularBase;
+template <typename MatrixType, unsigned int Mode>
+class TriangularView;
+template <typename MatrixType, unsigned int Mode>
+class SelfAdjointView;
+template <typename MatrixType>
+class SparseView;
+template <typename ExpressionType>
+class WithFormat;
+template <typename MatrixType>
+struct CommaInitializer;
+template <typename Derived>
+class ReturnByValue;
+template <typename ExpressionType>
+class ArrayWrapper;
+template <typename ExpressionType>
+class MatrixWrapper;
+template <typename Derived>
+class SolverBase;
+template <typename XprType>
+class InnerIterator;
 
 namespace internal {
-template<typename XprType> class generic_randaccess_stl_iterator;
-template<typename XprType> class pointer_based_stl_iterator;
-template<typename XprType, DirectionType Direction> class subvector_stl_iterator;
-template<typename XprType, DirectionType Direction> class subvector_stl_reverse_iterator;
-template<typename DecompositionType> struct kernel_retval_base;
-template<typename DecompositionType> struct kernel_retval;
-template<typename DecompositionType> struct image_retval_base;
-template<typename DecompositionType> struct image_retval;
-} // end namespace internal
+template <typename XprType>
+class generic_randaccess_stl_iterator;
+template <typename XprType>
+class pointer_based_stl_iterator;
+template <typename XprType, DirectionType Direction>
+class subvector_stl_iterator;
+template <typename XprType, DirectionType Direction>
+class subvector_stl_reverse_iterator;
+template <typename DecompositionType>
+struct kernel_retval_base;
+template <typename DecompositionType>
+struct kernel_retval;
+template <typename DecompositionType>
+struct image_retval_base;
+template <typename DecompositionType>
+struct image_retval;
+}  // end namespace internal
 
 namespace internal {
-template<typename _Scalar, int Rows=Dynamic, int Cols=Dynamic, int Supers=Dynamic, int Subs=Dynamic, int Options=0> class BandMatrix;
+template <typename Scalar_, int Rows = Dynamic, int Cols = Dynamic, int Supers = Dynamic, int Subs = Dynamic,
+          int Options = 0>
+class BandMatrix;
 }
 
 namespace internal {
-template<typename Lhs, typename Rhs> struct product_type;
+template <typename Lhs, typename Rhs>
+struct product_type;
 
-template<bool> struct EnableIf;
+template <bool>
+struct EnableIf;
 
 /** \internal
-  * \class product_evaluator
-  * Products need their own evaluator with more template arguments allowing for
-  * easier partial template specializations.
-  */
-template< typename T,
-          int ProductTag = internal::product_type<typename T::Lhs,typename T::Rhs>::ret,
+ * \class product_evaluator
+ * Products need their own evaluator with more template arguments allowing for
+ * easier partial template specializations.
+ */
+template <typename T, int ProductTag = internal::product_type<typename T::Lhs, typename T::Rhs>::ret,
           typename LhsShape = typename evaluator_traits<typename T::Lhs>::Shape,
           typename RhsShape = typename evaluator_traits<typename T::Rhs>::Shape,
           typename LhsScalar = typename traits<typename T::Lhs>::Scalar,
-          typename RhsScalar = typename traits<typename T::Rhs>::Scalar
-        > struct product_evaluator;
-}
+          typename RhsScalar = typename traits<typename T::Rhs>::Scalar>
+struct product_evaluator;
+}  // namespace internal
 
-template<typename Lhs, typename Rhs,
-         int ProductType = internal::product_type<Lhs,Rhs>::value>
+template <typename Lhs, typename Rhs, int ProductType = internal::product_type<Lhs, Rhs>::value>
 struct ProductReturnType;
 
 // this is a workaround for sun CC
-template<typename Lhs, typename Rhs> struct LazyProductReturnType;
+template <typename Lhs, typename Rhs>
+struct LazyProductReturnType;
 
 namespace internal {
 
 // Provides scalar/packet-wise product and product with accumulation
 // with optional conjugation of the arguments.
-template<typename LhsScalar, typename RhsScalar, bool ConjLhs=false, bool ConjRhs=false> struct conj_helper;
+template <typename LhsScalar, typename RhsScalar, bool ConjLhs = false, bool ConjRhs = false>
+struct conj_helper;
 
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_sum_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_difference_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_conj_product_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar, int NaNPropagation=PropagateFast> struct scalar_min_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar, int NaNPropagation=PropagateFast> struct scalar_max_op;
-template<typename Scalar> struct scalar_opposite_op;
-template<typename Scalar> struct scalar_conjugate_op;
-template<typename Scalar> struct scalar_real_op;
-template<typename Scalar> struct scalar_imag_op;
-template<typename Scalar> struct scalar_abs_op;
-template<typename Scalar> struct scalar_abs2_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_absolute_difference_op;
-template<typename Scalar> struct scalar_sqrt_op;
-template<typename Scalar> struct scalar_rsqrt_op;
-template<typename Scalar> struct scalar_exp_op;
-template<typename Scalar> struct scalar_log_op;
-template<typename Scalar> struct scalar_cos_op;
-template<typename Scalar> struct scalar_sin_op;
-template<typename Scalar> struct scalar_acos_op;
-template<typename Scalar> struct scalar_asin_op;
-template<typename Scalar> struct scalar_tan_op;
-template<typename Scalar> struct scalar_inverse_op;
-template<typename Scalar> struct scalar_square_op;
-template<typename Scalar> struct scalar_cube_op;
-template<typename Scalar, typename NewType> struct scalar_cast_op;
-template<typename Scalar> struct scalar_random_op;
-template<typename Scalar> struct scalar_constant_op;
-template<typename Scalar> struct scalar_identity_op;
-template<typename Scalar,bool is_complex, bool is_integer> struct scalar_sign_op;
-template<typename Scalar,typename ScalarExponent> struct scalar_pow_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_hypot_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_product_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_quotient_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_sum_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_difference_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_conj_product_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar, int NaNPropagation = PropagateFast>
+struct scalar_min_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar, int NaNPropagation = PropagateFast>
+struct scalar_max_op;
+template <typename Scalar>
+struct scalar_opposite_op;
+template <typename Scalar>
+struct scalar_conjugate_op;
+template <typename Scalar>
+struct scalar_real_op;
+template <typename Scalar>
+struct scalar_imag_op;
+template <typename Scalar>
+struct scalar_abs_op;
+template <typename Scalar>
+struct scalar_abs2_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_absolute_difference_op;
+template <typename Scalar>
+struct scalar_sqrt_op;
+template <typename Scalar>
+struct scalar_cbrt_op;
+template <typename Scalar>
+struct scalar_rsqrt_op;
+template <typename Scalar>
+struct scalar_exp_op;
+template <typename Scalar>
+struct scalar_log_op;
+template <typename Scalar>
+struct scalar_cos_op;
+template <typename Scalar>
+struct scalar_sin_op;
+template <typename Scalar>
+struct scalar_acos_op;
+template <typename Scalar>
+struct scalar_asin_op;
+template <typename Scalar>
+struct scalar_tan_op;
+template <typename Scalar>
+struct scalar_atan_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_atan2_op;
+template <typename Scalar>
+struct scalar_inverse_op;
+template <typename Scalar>
+struct scalar_square_op;
+template <typename Scalar>
+struct scalar_cube_op;
+template <typename Scalar, typename NewType>
+struct scalar_cast_op;
+template <typename Scalar>
+struct scalar_random_op;
+template <typename Scalar>
+struct scalar_constant_op;
+template <typename Scalar>
+struct scalar_identity_op;
+template <typename Scalar>
+struct scalar_sign_op;
+template <typename Scalar, typename ScalarExponent>
+struct scalar_pow_op;
+template <typename Scalar, typename ScalarExponent, bool BaseIsInteger, bool ExponentIsInteger, bool BaseIsComplex,
+          bool ExponentIsComplex>
+struct scalar_unary_pow_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_hypot_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_product_op;
+template <typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_quotient_op;
+// logical and bitwise operations
+template <typename Scalar>
+struct scalar_boolean_and_op;
+template <typename Scalar>
+struct scalar_boolean_or_op;
+template <typename Scalar>
+struct scalar_boolean_xor_op;
+template <typename Scalar>
+struct scalar_boolean_not_op;
+template <typename Scalar>
+struct scalar_bitwise_and_op;
+template <typename Scalar>
+struct scalar_bitwise_or_op;
+template <typename Scalar>
+struct scalar_bitwise_xor_op;
+template <typename Scalar>
+struct scalar_bitwise_not_op;
 
 // SpecialFunctions module
-template<typename Scalar> struct scalar_lgamma_op;
-template<typename Scalar> struct scalar_digamma_op;
-template<typename Scalar> struct scalar_erf_op;
-template<typename Scalar> struct scalar_erfc_op;
-template<typename Scalar> struct scalar_ndtri_op;
-template<typename Scalar> struct scalar_igamma_op;
-template<typename Scalar> struct scalar_igammac_op;
-template<typename Scalar> struct scalar_zeta_op;
-template<typename Scalar> struct scalar_betainc_op;
+template <typename Scalar>
+struct scalar_lgamma_op;
+template <typename Scalar>
+struct scalar_digamma_op;
+template <typename Scalar>
+struct scalar_erf_op;
+template <typename Scalar>
+struct scalar_erfc_op;
+template <typename Scalar>
+struct scalar_ndtri_op;
+template <typename Scalar>
+struct scalar_igamma_op;
+template <typename Scalar>
+struct scalar_igammac_op;
+template <typename Scalar>
+struct scalar_zeta_op;
+template <typename Scalar>
+struct scalar_betainc_op;
 
 // Bessel functions in SpecialFunctions module
-template<typename Scalar> struct scalar_bessel_i0_op;
-template<typename Scalar> struct scalar_bessel_i0e_op;
-template<typename Scalar> struct scalar_bessel_i1_op;
-template<typename Scalar> struct scalar_bessel_i1e_op;
-template<typename Scalar> struct scalar_bessel_j0_op;
-template<typename Scalar> struct scalar_bessel_y0_op;
-template<typename Scalar> struct scalar_bessel_j1_op;
-template<typename Scalar> struct scalar_bessel_y1_op;
-template<typename Scalar> struct scalar_bessel_k0_op;
-template<typename Scalar> struct scalar_bessel_k0e_op;
-template<typename Scalar> struct scalar_bessel_k1_op;
-template<typename Scalar> struct scalar_bessel_k1e_op;
+template <typename Scalar>
+struct scalar_bessel_i0_op;
+template <typename Scalar>
+struct scalar_bessel_i0e_op;
+template <typename Scalar>
+struct scalar_bessel_i1_op;
+template <typename Scalar>
+struct scalar_bessel_i1e_op;
+template <typename Scalar>
+struct scalar_bessel_j0_op;
+template <typename Scalar>
+struct scalar_bessel_y0_op;
+template <typename Scalar>
+struct scalar_bessel_j1_op;
+template <typename Scalar>
+struct scalar_bessel_y1_op;
+template <typename Scalar>
+struct scalar_bessel_k0_op;
+template <typename Scalar>
+struct scalar_bessel_k0e_op;
+template <typename Scalar>
+struct scalar_bessel_k1_op;
+template <typename Scalar>
+struct scalar_bessel_k1e_op;
 
-
-} // end namespace internal
+}  // end namespace internal
 
 struct IOFormat;
 
 // Array module
-template<typename _Scalar, int _Rows, int _Cols,
-         int _Options = AutoAlign |
-#if EIGEN_GNUC_AT(3,4)
-    // workaround a bug in at least gcc 3.4.6
-    // the innermost ?: ternary operator is misparsed. We write it slightly
-    // differently and this makes gcc 3.4.6 happy, but it's ugly.
-    // The error would only show up with EIGEN_DEFAULT_TO_ROW_MAJOR is defined
-    // (when EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION is RowMajor)
-                          ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
-                          : !(_Cols==1 && _Rows!=1) ?  EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
-                          : Eigen::ColMajor ),
-#else
-                          ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
-                          : (_Cols==1 && _Rows!=1) ? Eigen::ColMajor
-                          : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
-#endif
-         int _MaxRows = _Rows, int _MaxCols = _Cols> class Array;
-template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType> class Select;
-template<typename MatrixType, typename BinaryOp, int Direction> class PartialReduxExpr;
-template<typename ExpressionType, int Direction> class VectorwiseOp;
-template<typename MatrixType,int RowFactor,int ColFactor> class Replicate;
-template<typename MatrixType, int Direction = BothDirections> class Reverse;
+template <typename Scalar_, int Rows_, int Cols_,
+          int Options_ = AutoAlign | ((Rows_ == 1 && Cols_ != 1)   ? Eigen::RowMajor
+                                      : (Cols_ == 1 && Rows_ != 1) ? Eigen::ColMajor
+                                                                   : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
+          int MaxRows_ = Rows_, int MaxCols_ = Cols_>
+class Array;
+template <typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
+class Select;
+template <typename MatrixType, typename BinaryOp, int Direction>
+class PartialReduxExpr;
+template <typename ExpressionType, int Direction>
+class VectorwiseOp;
+template <typename MatrixType, int RowFactor, int ColFactor>
+class Replicate;
+template <typename MatrixType, int Direction = BothDirections>
+class Reverse;
 
-template<typename MatrixType> class FullPivLU;
-template<typename MatrixType> class PartialPivLU;
+#if defined(EIGEN_USE_LAPACKE) && defined(lapack_int)
+// Lapacke interface requires StorageIndex to be lapack_int
+typedef lapack_int DefaultPermutationIndex;
+#else
+typedef int DefaultPermutationIndex;
+#endif
+
+template <typename MatrixType, typename PermutationIndex = DefaultPermutationIndex>
+class FullPivLU;
+template <typename MatrixType, typename PermutationIndex = DefaultPermutationIndex>
+class PartialPivLU;
 namespace internal {
-template<typename MatrixType> struct inverse_impl;
+template <typename MatrixType>
+struct inverse_impl;
 }
-template<typename MatrixType> class HouseholderQR;
-template<typename MatrixType> class ColPivHouseholderQR;
-template<typename MatrixType> class FullPivHouseholderQR;
-template<typename MatrixType> class CompleteOrthogonalDecomposition;
-template<typename MatrixType> class SVDBase;
-template<typename MatrixType, int QRPreconditioner = ColPivHouseholderQRPreconditioner> class JacobiSVD;
-template<typename MatrixType> class BDCSVD;
-template<typename MatrixType, int UpLo = Lower> class LLT;
-template<typename MatrixType, int UpLo = Lower> class LDLT;
-template<typename VectorsType, typename CoeffsType, int Side=OnTheLeft> class HouseholderSequence;
-template<typename Scalar>     class JacobiRotation;
+template <typename MatrixType>
+class HouseholderQR;
+template <typename MatrixType, typename PermutationIndex = DefaultPermutationIndex>
+class ColPivHouseholderQR;
+template <typename MatrixType, typename PermutationIndex = DefaultPermutationIndex>
+class FullPivHouseholderQR;
+template <typename MatrixType, typename PermutationIndex = DefaultPermutationIndex>
+class CompleteOrthogonalDecomposition;
+template <typename MatrixType>
+class SVDBase;
+template <typename MatrixType, int Options = 0>
+class JacobiSVD;
+template <typename MatrixType, int Options = 0>
+class BDCSVD;
+template <typename MatrixType, int UpLo = Lower>
+class LLT;
+template <typename MatrixType, int UpLo = Lower>
+class LDLT;
+template <typename VectorsType, typename CoeffsType, int Side = OnTheLeft>
+class HouseholderSequence;
+template <typename Scalar>
+class JacobiRotation;
 
 // Geometry module:
-template<typename Derived, int _Dim> class RotationBase;
-template<typename Lhs, typename Rhs> class Cross;
-template<typename Derived> class QuaternionBase;
-template<typename Scalar> class Rotation2D;
-template<typename Scalar> class AngleAxis;
-template<typename Scalar,int Dim> class Translation;
-template<typename Scalar,int Dim> class AlignedBox;
-template<typename Scalar, int Options = AutoAlign> class Quaternion;
-template<typename Scalar,int Dim,int Mode,int _Options=AutoAlign> class Transform;
-template <typename _Scalar, int _AmbientDim, int Options=AutoAlign> class ParametrizedLine;
-template <typename _Scalar, int _AmbientDim, int Options=AutoAlign> class Hyperplane;
-template<typename Scalar> class UniformScaling;
-template<typename MatrixType,int Direction> class Homogeneous;
+namespace internal {
+template <typename Derived, typename OtherDerived, int Size = MatrixBase<Derived>::SizeAtCompileTime>
+struct cross_impl;
+}
+template <typename Derived, int Dim_>
+class RotationBase;
+template <typename Derived>
+class QuaternionBase;
+template <typename Scalar>
+class Rotation2D;
+template <typename Scalar>
+class AngleAxis;
+template <typename Scalar, int Dim>
+class Translation;
+template <typename Scalar, int Dim>
+class AlignedBox;
+template <typename Scalar, int Options = AutoAlign>
+class Quaternion;
+template <typename Scalar, int Dim, int Mode, int Options_ = AutoAlign>
+class Transform;
+template <typename Scalar_, int AmbientDim_, int Options = AutoAlign>
+class ParametrizedLine;
+template <typename Scalar_, int AmbientDim_, int Options = AutoAlign>
+class Hyperplane;
+template <typename Scalar>
+class UniformScaling;
+template <typename MatrixType, int Direction>
+class Homogeneous;
 
 // Sparse module:
-template<typename Derived> class SparseMatrixBase;
+template <typename Derived>
+class SparseMatrixBase;
 
 // MatrixFunctions module
-template<typename Derived> struct MatrixExponentialReturnValue;
-template<typename Derived> class MatrixFunctionReturnValue;
-template<typename Derived> class MatrixSquareRootReturnValue;
-template<typename Derived> class MatrixLogarithmReturnValue;
-template<typename Derived> class MatrixPowerReturnValue;
-template<typename Derived> class MatrixComplexPowerReturnValue;
+template <typename Derived>
+struct MatrixExponentialReturnValue;
+template <typename Derived>
+class MatrixFunctionReturnValue;
+template <typename Derived>
+class MatrixSquareRootReturnValue;
+template <typename Derived>
+class MatrixLogarithmReturnValue;
+template <typename Derived>
+class MatrixPowerReturnValue;
+template <typename Derived>
+class MatrixComplexPowerReturnValue;
 
 namespace internal {
 template <typename Scalar>
-struct stem_function
-{
+struct stem_function {
   typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
   typedef ComplexScalar type(ComplexScalar, int);
 };
-}
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_FORWARDDECLARATIONS_H
+#endif  // EIGEN_FORWARDDECLARATIONS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h
index f85de30..3b45108 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h
@@ -7,97 +7,90 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 #ifndef EIGEN_INDEXED_VIEW_HELPER_H
 #define EIGEN_INDEXED_VIEW_HELPER_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 struct symbolic_last_tag {};
-}
+}  // namespace internal
+
+namespace placeholders {
+
+typedef symbolic::SymbolExpr<internal::symbolic_last_tag> last_t;
 
 /** \var last
-  * \ingroup Core_Module
-  *
-  * Can be used as a parameter to Eigen::seq and Eigen::seqN functions to symbolically reference the last element/row/columns
-  * of the underlying vector or matrix once passed to DenseBase::operator()(const RowIndices&, const ColIndices&).
-  *
-  * This symbolic placeholder supports standard arithmetic operations.
-  *
-  * A typical usage example would be:
-  * \code
-  * using namespace Eigen;
-  * using Eigen::last;
-  * VectorXd v(n);
-  * v(seq(2,last-2)).setOnes();
-  * \endcode
-  *
-  * \sa end
-  */
-static const symbolic::SymbolExpr<internal::symbolic_last_tag> last; // PLEASE use Eigen::last   instead of Eigen::placeholders::last
+ * \ingroup Core_Module
+ *
+ * Can be used as a parameter to Eigen::seq and Eigen::seqN functions to symbolically reference the last
+ * element/row/columns of the underlying vector or matrix once passed to DenseBase::operator()(const RowIndices&, const
+ * ColIndices&).
+ *
+ * This symbolic placeholder supports standard arithmetic operations.
+ *
+ * A typical usage example would be:
+ * \code
+ * using namespace Eigen;
+ * using Eigen::placeholders::last;
+ * VectorXd v(n);
+ * v(seq(2,last-2)).setOnes();
+ * \endcode
+ *
+ * \sa end
+ */
+static const last_t last;
 
-/** \var lastp1
-  * \ingroup Core_Module
-  *
-  * Can be used as a parameter to Eigen::seq and Eigen::seqN functions to symbolically
-  * reference the last+1 element/row/columns of the underlying vector or matrix once
-  * passed to DenseBase::operator()(const RowIndices&, const ColIndices&).
-  *
-  * This symbolic placeholder supports standard arithmetic operations.
-  * It is essentially an alias to last+fix<1>.
-  *
-  * \sa last
-  */
-#ifdef EIGEN_PARSED_BY_DOXYGEN
-static const auto lastp1 = last+fix<1>;
-#else
-// Using a FixedExpr<1> expression is important here to make sure the compiler
-// can fully optimize the computation starting indices with zero overhead.
-static const symbolic::AddExpr<symbolic::SymbolExpr<internal::symbolic_last_tag>,symbolic::ValueExpr<Eigen::internal::FixedInt<1> > > lastp1(last+fix<1>());
-#endif
+}  // namespace placeholders
 
 namespace internal {
 
- // Replace symbolic last/end "keywords" by their true runtime value
-inline Index eval_expr_given_size(Index x, Index /* size */)   { return x; }
+// Replace symbolic last/end "keywords" by their true runtime value
+inline Index eval_expr_given_size(Index x, Index /* size */) { return x; }
 
-template<int N>
-FixedInt<N> eval_expr_given_size(FixedInt<N> x, Index /*size*/)   { return x; }
+template <int N>
+FixedInt<N> eval_expr_given_size(FixedInt<N> x, Index /*size*/) {
+  return x;
+}
 
-template<typename Derived>
-Index eval_expr_given_size(const symbolic::BaseExpr<Derived> &x, Index size)
-{
-  return x.derived().eval(last=size-1);
+template <typename Derived>
+Index eval_expr_given_size(const symbolic::BaseExpr<Derived>& x, Index size) {
+  return x.derived().eval(Eigen::placeholders::last = size - 1);
 }
 
 // Extract increment/step at compile time
-template<typename T, typename EnableIf = void> struct get_compile_time_incr {
+template <typename T, typename EnableIf = void>
+struct get_compile_time_incr {
   enum { value = UndefinedIncr };
 };
 
 // Analogue of std::get<0>(x), but tailored for our needs.
-template<typename T>
-EIGEN_CONSTEXPR Index first(const T& x) EIGEN_NOEXCEPT { return x.first(); }
+template <typename T>
+EIGEN_CONSTEXPR Index first(const T& x) EIGEN_NOEXCEPT {
+  return x.first();
+}
 
-// IndexedViewCompatibleType/makeIndexedViewCompatible turn an arbitrary object of type T into something usable by MatrixSlice
-// The generic implementation is a no-op
-template<typename T,int XprSize,typename EnableIf=void>
+// IndexedViewCompatibleType/makeIndexedViewCompatible turn an arbitrary object of type T into something usable by
+// MatrixSlice The generic implementation is a no-op
+template <typename T, int XprSize, typename EnableIf = void>
 struct IndexedViewCompatibleType {
   typedef T type;
 };
 
-template<typename T,typename Q>
-const T& makeIndexedViewCompatible(const T& x, Index /*size*/, Q) { return x; }
+template <typename T, typename Q>
+const T& makeIndexedViewCompatible(const T& x, Index /*size*/, Q) {
+  return x;
+}
 
 //--------------------------------------------------------------------------------
 // Handling of a single Index
 //--------------------------------------------------------------------------------
 
 struct SingleRange {
-  enum {
-    SizeAtCompileTime = 1
-  };
+  enum { SizeAtCompileTime = 1 };
   SingleRange(Index val) : m_value(val) {}
   Index operator[](Index) const { return m_value; }
   static EIGEN_CONSTEXPR Index size() EIGEN_NOEXCEPT { return 1; }
@@ -105,82 +98,111 @@
   Index m_value;
 };
 
-template<> struct get_compile_time_incr<SingleRange> {
-  enum { value = 1 }; // 1 or 0 ??
+template <>
+struct get_compile_time_incr<SingleRange> {
+  enum { value = 1 };  // 1 or 0 ??
 };
 
-// Turn a single index into something that looks like an array (i.e., that exposes a .size(), and operator[](int) methods)
-template<typename T, int XprSize>
-struct IndexedViewCompatibleType<T,XprSize,typename internal::enable_if<internal::is_integral<T>::value>::type> {
+// Turn a single index into something that looks like an array (i.e., that exposes a .size(), and operator[](int)
+// methods)
+template <typename T, int XprSize>
+struct IndexedViewCompatibleType<T, XprSize, std::enable_if_t<internal::is_integral<T>::value>> {
   // Here we could simply use Array, but maybe it's less work for the compiler to use
   // a simpler wrapper as SingleRange
-  //typedef Eigen::Array<Index,1,1> type;
+  // typedef Eigen::Array<Index,1,1> type;
   typedef SingleRange type;
 };
 
-template<typename T, int XprSize>
-struct IndexedViewCompatibleType<T, XprSize, typename enable_if<symbolic::is_symbolic<T>::value>::type> {
+template <typename T, int XprSize>
+struct IndexedViewCompatibleType<T, XprSize, std::enable_if_t<symbolic::is_symbolic<T>::value>> {
   typedef SingleRange type;
 };
 
-
-template<typename T>
-typename enable_if<symbolic::is_symbolic<T>::value,SingleRange>::type
-makeIndexedViewCompatible(const T& id, Index size, SpecializedType) {
-  return eval_expr_given_size(id,size);
+template <typename T>
+std::enable_if_t<symbolic::is_symbolic<T>::value, SingleRange> makeIndexedViewCompatible(const T& id, Index size,
+                                                                                         SpecializedType) {
+  return eval_expr_given_size(id, size);
 }
 
 //--------------------------------------------------------------------------------
 // Handling of all
 //--------------------------------------------------------------------------------
 
-struct all_t { all_t() {} };
+struct all_t {
+  all_t() {}
+};
 
 // Convert a symbolic 'all' into a usable range type
-template<int XprSize>
+template <int XprSize>
 struct AllRange {
   enum { SizeAtCompileTime = XprSize };
   AllRange(Index size = XprSize) : m_size(size) {}
   EIGEN_CONSTEXPR Index operator[](Index i) const EIGEN_NOEXCEPT { return i; }
   EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_size.value(); }
   EIGEN_CONSTEXPR Index first() const EIGEN_NOEXCEPT { return 0; }
-  variable_if_dynamic<Index,XprSize> m_size;
+  variable_if_dynamic<Index, XprSize> m_size;
 };
 
-template<int XprSize>
-struct IndexedViewCompatibleType<all_t,XprSize> {
+template <int XprSize>
+struct IndexedViewCompatibleType<all_t, XprSize> {
   typedef AllRange<XprSize> type;
 };
 
-template<typename XprSizeType>
-inline AllRange<get_fixed_value<XprSizeType>::value> makeIndexedViewCompatible(all_t , XprSizeType size, SpecializedType) {
+template <typename XprSizeType>
+inline AllRange<get_fixed_value<XprSizeType>::value> makeIndexedViewCompatible(all_t, XprSizeType size,
+                                                                               SpecializedType) {
   return AllRange<get_fixed_value<XprSizeType>::value>(size);
 }
 
-template<int Size> struct get_compile_time_incr<AllRange<Size> > {
+template <int Size>
+struct get_compile_time_incr<AllRange<Size>> {
   enum { value = 1 };
 };
 
-} // end namespace internal
-
-
-/** \var all
-  * \ingroup Core_Module
-  * Can be used as a parameter to DenseBase::operator()(const RowIndices&, const ColIndices&) to index all rows or columns
-  */
-static const Eigen::internal::all_t all; // PLEASE use Eigen::all instead of Eigen::placeholders::all
-
+}  // end namespace internal
 
 namespace placeholders {
-  typedef symbolic::SymbolExpr<internal::symbolic_last_tag> last_t;
-  typedef symbolic::AddExpr<symbolic::SymbolExpr<internal::symbolic_last_tag>,symbolic::ValueExpr<Eigen::internal::FixedInt<1> > > end_t;
-  typedef Eigen::internal::all_t all_t;
 
-  EIGEN_DEPRECATED static const all_t  all  = Eigen::all;    // PLEASE use Eigen::all    instead of Eigen::placeholders::all
-  EIGEN_DEPRECATED static const last_t last = Eigen::last;   // PLEASE use Eigen::last   instead of Eigen::placeholders::last
-  EIGEN_DEPRECATED static const end_t  end  = Eigen::lastp1; // PLEASE use Eigen::lastp1 instead of Eigen::placeholders::end
-}
+typedef symbolic::AddExpr<symbolic::SymbolExpr<internal::symbolic_last_tag>,
+                          symbolic::ValueExpr<Eigen::internal::FixedInt<1>>>
+    lastp1_t;
+typedef Eigen::internal::all_t all_t;
 
-} // end namespace Eigen
+/** \var lastp1
+ * \ingroup Core_Module
+ *
+ * Can be used as a parameter to Eigen::seq and Eigen::seqN functions to symbolically
+ * reference the last+1 element/row/columns of the underlying vector or matrix once
+ * passed to DenseBase::operator()(const RowIndices&, const ColIndices&).
+ *
+ * This symbolic placeholder supports standard arithmetic operations.
+ * It is essentially an alias to last+fix<1>.
+ *
+ * \sa last
+ */
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+static const auto lastp1 = last + fix<1>;
+#else
+// Using a FixedExpr<1> expression is important here to make sure the compiler
+// can fully optimize the computation starting indices with zero overhead.
+static const lastp1_t lastp1(last + fix<1>());
+#endif
 
-#endif // EIGEN_INDEXED_VIEW_HELPER_H
+/** \var end
+ * \ingroup Core_Module
+ * \sa lastp1
+ */
+static const lastp1_t end = lastp1;
+
+/** \var all
+ * \ingroup Core_Module
+ * Can be used as a parameter to DenseBase::operator()(const RowIndices&, const ColIndices&) to index all rows or
+ * columns
+ */
+static const Eigen::internal::all_t all;
+
+}  // namespace placeholders
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_INDEXED_VIEW_HELPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h
index 945d426..279d553 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h
@@ -7,266 +7,278 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 #ifndef EIGEN_INTEGRAL_CONSTANT_H
 #define EIGEN_INTEGRAL_CONSTANT_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<int N> class FixedInt;
-template<int N> class VariableAndFixedInt;
+template <int N>
+class FixedInt;
+template <int N>
+class VariableAndFixedInt;
 
 /** \internal
-  * \class FixedInt
-  *
-  * This class embeds a compile-time integer \c N.
-  *
-  * It is similar to c++11 std::integral_constant<int,N> but with some additional features
-  * such as:
-  *  - implicit conversion to int
-  *  - arithmetic and some bitwise operators: -, +, *, /, %, &, |
-  *  - c++98/14 compatibility with fix<N> and fix<N>() syntax to define integral constants.
-  *
-  * It is strongly discouraged to directly deal with this class FixedInt. Instances are expcected to
-  * be created by the user using Eigen::fix<N> or Eigen::fix<N>(). In C++98-11, the former syntax does
-  * not create a FixedInt<N> instance but rather a point to function that needs to be \em cleaned-up
-  * using the generic helper:
-  * \code
-  * internal::cleanup_index_type<T>::type
-  * internal::cleanup_index_type<T,DynamicKey>::type
-  * \endcode
-  * where T can a FixedInt<N>, a pointer to function FixedInt<N> (*)(), or numerous other integer-like representations.
-  * \c DynamicKey is either Dynamic (default) or DynamicIndex and used to identify true compile-time values.
-  *
-  * For convenience, you can extract the compile-time value \c N in a generic way using the following helper:
-  * \code
-  * internal::get_fixed_value<T,DefaultVal>::value
-  * \endcode
-  * that will give you \c N if T equals FixedInt<N> or FixedInt<N> (*)(), and \c DefaultVal if T does not embed any compile-time value (e.g., T==int).
-  *
-  * \sa fix<N>, class VariableAndFixedInt
-  */
-template<int N> class FixedInt
-{
-public:
+ * \class FixedInt
+ *
+ * This class embeds a compile-time integer \c N.
+ *
+ * It is similar to c++11 std::integral_constant<int,N> but with some additional features
+ * such as:
+ *  - implicit conversion to int
+ *  - arithmetic and some bitwise operators: -, +, *, /, %, &, |
+ *  - c++98/14 compatibility with fix<N> and fix<N>() syntax to define integral constants.
+ *
+ * It is strongly discouraged to directly deal with this class FixedInt. Instances are expected to
+ * be created by the user using Eigen::fix<N> or Eigen::fix<N>().
+ * \code
+ * internal::cleanup_index_type<T>::type
+ * internal::cleanup_index_type<T,DynamicKey>::type
+ * \endcode
+ * where T can a FixedInt<N>, a pointer to function FixedInt<N> (*)(), or numerous other integer-like representations.
+ * \c DynamicKey is either Dynamic (default) or DynamicIndex and used to identify true compile-time values.
+ *
+ * For convenience, you can extract the compile-time value \c N in a generic way using the following helper:
+ * \code
+ * internal::get_fixed_value<T,DefaultVal>::value
+ * \endcode
+ * that will give you \c N if T equals FixedInt<N> or FixedInt<N> (*)(), and \c DefaultVal if T does not embed any
+ * compile-time value (e.g., T==int).
+ *
+ * \sa fix<N>, class VariableAndFixedInt
+ */
+template <int N>
+class FixedInt {
+ public:
   static const int value = N;
   EIGEN_CONSTEXPR operator int() const { return value; }
-  FixedInt() {}
-  FixedInt( VariableAndFixedInt<N> other) {
-    #ifndef EIGEN_INTERNAL_DEBUGGING
+
+  EIGEN_CONSTEXPR
+  FixedInt() = default;
+
+  EIGEN_CONSTEXPR
+  FixedInt(std::integral_constant<int, N>) {}
+
+  EIGEN_CONSTEXPR
+  FixedInt(VariableAndFixedInt<N> other) {
+#ifndef EIGEN_INTERNAL_DEBUGGING
     EIGEN_UNUSED_VARIABLE(other);
-    #endif
-    eigen_internal_assert(int(other)==N);
+#endif
+    eigen_internal_assert(int(other) == N);
   }
 
+  EIGEN_CONSTEXPR
   FixedInt<-N> operator-() const { return FixedInt<-N>(); }
-  template<int M>
-  FixedInt<N+M> operator+( FixedInt<M>) const { return FixedInt<N+M>(); }
-  template<int M>
-  FixedInt<N-M> operator-( FixedInt<M>) const { return FixedInt<N-M>(); }
-  template<int M>
-  FixedInt<N*M> operator*( FixedInt<M>) const { return FixedInt<N*M>(); }
-  template<int M>
-  FixedInt<N/M> operator/( FixedInt<M>) const { return FixedInt<N/M>(); }
-  template<int M>
-  FixedInt<N%M> operator%( FixedInt<M>) const { return FixedInt<N%M>(); }
-  template<int M>
-  FixedInt<N|M> operator|( FixedInt<M>) const { return FixedInt<N|M>(); }
-  template<int M>
-  FixedInt<N&M> operator&( FixedInt<M>) const { return FixedInt<N&M>(); }
 
-#if EIGEN_HAS_CXX14_VARIABLE_TEMPLATES
+  template <int M>
+  EIGEN_CONSTEXPR FixedInt<N + M> operator+(FixedInt<M>) const {
+    return FixedInt<N + M>();
+  }
+
+  template <int M>
+  EIGEN_CONSTEXPR FixedInt<N - M> operator-(FixedInt<M>) const {
+    return FixedInt<N - M>();
+  }
+
+  template <int M>
+  EIGEN_CONSTEXPR FixedInt<N * M> operator*(FixedInt<M>) const {
+    return FixedInt<N * M>();
+  }
+
+  template <int M>
+  EIGEN_CONSTEXPR FixedInt<N / M> operator/(FixedInt<M>) const {
+    return FixedInt<N / M>();
+  }
+
+  template <int M>
+  EIGEN_CONSTEXPR FixedInt<N % M> operator%(FixedInt<M>) const {
+    return FixedInt<N % M>();
+  }
+
+  template <int M>
+  EIGEN_CONSTEXPR FixedInt<N | M> operator|(FixedInt<M>) const {
+    return FixedInt<N | M>();
+  }
+
+  template <int M>
+  EIGEN_CONSTEXPR FixedInt<N & M> operator&(FixedInt<M>) const {
+    return FixedInt<N & M>();
+  }
+
   // Needed in C++14 to allow fix<N>():
-  FixedInt operator() () const { return *this; }
+  EIGEN_CONSTEXPR FixedInt operator()() const { return *this; }
 
-  VariableAndFixedInt<N> operator() (int val) const { return VariableAndFixedInt<N>(val); }
-#else
-  FixedInt ( FixedInt<N> (*)() ) {}
-#endif
-
-#if EIGEN_HAS_CXX11
-  FixedInt(std::integral_constant<int,N>) {}
-#endif
+  VariableAndFixedInt<N> operator()(int val) const { return VariableAndFixedInt<N>(val); }
 };
 
 /** \internal
-  * \class VariableAndFixedInt
-  *
-  * This class embeds both a compile-time integer \c N and a runtime integer.
-  * Both values are supposed to be equal unless the compile-time value \c N has a special
-  * value meaning that the runtime-value should be used. Depending on the context, this special
-  * value can be either Eigen::Dynamic (for positive quantities) or Eigen::DynamicIndex (for
-  * quantities that can be negative).
-  *
-  * It is the return-type of the function Eigen::fix<N>(int), and most of the time this is the only
-  * way it is used. It is strongly discouraged to directly deal with instances of VariableAndFixedInt.
-  * Indeed, in order to write generic code, it is the responsibility of the callee to properly convert
-  * it to either a true compile-time quantity (i.e. a FixedInt<N>), or to a runtime quantity (e.g., an Index)
-  * using the following generic helper:
-  * \code
-  * internal::cleanup_index_type<T>::type
-  * internal::cleanup_index_type<T,DynamicKey>::type
-  * \endcode
-  * where T can be a template instantiation of VariableAndFixedInt or numerous other integer-like representations.
-  * \c DynamicKey is either Dynamic (default) or DynamicIndex and used to identify true compile-time values.
-  *
-  * For convenience, you can also extract the compile-time value \c N using the following helper:
-  * \code
-  * internal::get_fixed_value<T,DefaultVal>::value
-  * \endcode
-  * that will give you \c N if T equals VariableAndFixedInt<N>, and \c DefaultVal if T does not embed any compile-time value (e.g., T==int).
-  *
-  * \sa fix<N>(int), class FixedInt
-  */
-template<int N> class VariableAndFixedInt
-{
-public:
+ * \class VariableAndFixedInt
+ *
+ * This class embeds both a compile-time integer \c N and a runtime integer.
+ * Both values are supposed to be equal unless the compile-time value \c N has a special
+ * value meaning that the runtime-value should be used. Depending on the context, this special
+ * value can be either Eigen::Dynamic (for positive quantities) or Eigen::DynamicIndex (for
+ * quantities that can be negative).
+ *
+ * It is the return-type of the function Eigen::fix<N>(int), and most of the time this is the only
+ * way it is used. It is strongly discouraged to directly deal with instances of VariableAndFixedInt.
+ * Indeed, in order to write generic code, it is the responsibility of the callee to properly convert
+ * it to either a true compile-time quantity (i.e. a FixedInt<N>), or to a runtime quantity (e.g., an Index)
+ * using the following generic helper:
+ * \code
+ * internal::cleanup_index_type<T>::type
+ * internal::cleanup_index_type<T,DynamicKey>::type
+ * \endcode
+ * where T can be a template instantiation of VariableAndFixedInt or numerous other integer-like representations.
+ * \c DynamicKey is either Dynamic (default) or DynamicIndex and used to identify true compile-time values.
+ *
+ * For convenience, you can also extract the compile-time value \c N using the following helper:
+ * \code
+ * internal::get_fixed_value<T,DefaultVal>::value
+ * \endcode
+ * that will give you \c N if T equals VariableAndFixedInt<N>, and \c DefaultVal if T does not embed any compile-time
+ * value (e.g., T==int).
+ *
+ * \sa fix<N>(int), class FixedInt
+ */
+template <int N>
+class VariableAndFixedInt {
+ public:
   static const int value = N;
   operator int() const { return m_value; }
   VariableAndFixedInt(int val) { m_value = val; }
-protected:
+
+ protected:
   int m_value;
 };
 
-template<typename T, int Default=Dynamic> struct get_fixed_value {
+template <typename T, int Default = Dynamic>
+struct get_fixed_value {
   static const int value = Default;
 };
 
-template<int N,int Default> struct get_fixed_value<FixedInt<N>,Default> {
+template <int N, int Default>
+struct get_fixed_value<FixedInt<N>, Default> {
   static const int value = N;
 };
 
-#if !EIGEN_HAS_CXX14
-template<int N,int Default> struct get_fixed_value<FixedInt<N> (*)(),Default> {
-  static const int value = N;
-};
-#endif
-
-template<int N,int Default> struct get_fixed_value<VariableAndFixedInt<N>,Default> {
-  static const int value = N ;
-};
-
-template<typename T, int N, int Default>
-struct get_fixed_value<variable_if_dynamic<T,N>,Default> {
+template <int N, int Default>
+struct get_fixed_value<VariableAndFixedInt<N>, Default> {
   static const int value = N;
 };
 
-template<typename T> EIGEN_DEVICE_FUNC Index get_runtime_value(const T &x) { return x; }
-#if !EIGEN_HAS_CXX14
-template<int N> EIGEN_DEVICE_FUNC Index get_runtime_value(FixedInt<N> (*)()) { return N; }
-#endif
+template <typename T, int N, int Default>
+struct get_fixed_value<variable_if_dynamic<T, N>, Default> {
+  static const int value = N;
+};
+
+template <typename T>
+EIGEN_DEVICE_FUNC Index get_runtime_value(const T &x) {
+  return x;
+}
 
 // Cleanup integer/FixedInt/VariableAndFixedInt/etc types:
 
 // By default, no cleanup:
-template<typename T, int DynamicKey=Dynamic, typename EnableIf=void> struct cleanup_index_type { typedef T type; };
+template <typename T, int DynamicKey = Dynamic, typename EnableIf = void>
+struct cleanup_index_type {
+  typedef T type;
+};
 
 // Convert any integral type (e.g., short, int, unsigned int, etc.) to Eigen::Index
-template<typename T, int DynamicKey> struct cleanup_index_type<T,DynamicKey,typename internal::enable_if<internal::is_integral<T>::value>::type> { typedef Index type; };
-
-#if !EIGEN_HAS_CXX14
-// In c++98/c++11, fix<N> is a pointer to function that we better cleanup to a true FixedInt<N>:
-template<int N, int DynamicKey> struct cleanup_index_type<FixedInt<N> (*)(), DynamicKey> { typedef FixedInt<N> type; };
-#endif
+template <typename T, int DynamicKey>
+struct cleanup_index_type<T, DynamicKey, std::enable_if_t<internal::is_integral<T>::value>> {
+  typedef Index type;
+};
 
 // If VariableAndFixedInt does not match DynamicKey, then we turn it to a pure compile-time value:
-template<int N, int DynamicKey> struct cleanup_index_type<VariableAndFixedInt<N>, DynamicKey> { typedef FixedInt<N> type; };
+template <int N, int DynamicKey>
+struct cleanup_index_type<VariableAndFixedInt<N>, DynamicKey> {
+  typedef FixedInt<N> type;
+};
 // If VariableAndFixedInt matches DynamicKey, then we turn it to a pure runtime-value (aka Index):
-template<int DynamicKey> struct cleanup_index_type<VariableAndFixedInt<DynamicKey>, DynamicKey> { typedef Index type; };
+template <int DynamicKey>
+struct cleanup_index_type<VariableAndFixedInt<DynamicKey>, DynamicKey> {
+  typedef Index type;
+};
 
-#if EIGEN_HAS_CXX11
-template<int N, int DynamicKey> struct cleanup_index_type<std::integral_constant<int,N>, DynamicKey> { typedef FixedInt<N> type; };
-#endif
+template <int N, int DynamicKey>
+struct cleanup_index_type<std::integral_constant<int, N>, DynamicKey> {
+  typedef FixedInt<N> type;
+};
 
-} // end namespace internal
+}  // end namespace internal
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 
-#if EIGEN_HAS_CXX14_VARIABLE_TEMPLATES
-template<int N>
-static const internal::FixedInt<N> fix{};
-#else
-template<int N>
-inline internal::FixedInt<N> fix() { return internal::FixedInt<N>(); }
+template <int N>
+constexpr internal::FixedInt<N> fix{};
 
-// The generic typename T is mandatory. Otherwise, a code like fix<N> could refer to either the function above or this next overload.
-// This way a code like fix<N> can only refer to the previous function.
-template<int N,typename T>
-inline internal::VariableAndFixedInt<N> fix(T val) { return internal::VariableAndFixedInt<N>(internal::convert_index<int>(val)); }
-#endif
-
-#else // EIGEN_PARSED_BY_DOXYGEN
+#else  // EIGEN_PARSED_BY_DOXYGEN
 
 /** \var fix<N>()
-  * \ingroup Core_Module
-  *
-  * This \em identifier permits to construct an object embedding a compile-time integer \c N.
-  *
-  * \tparam N the compile-time integer value
-  *
-  * It is typically used in conjunction with the Eigen::seq and Eigen::seqN functions to pass compile-time values to them:
-  * \code
-  * seqN(10,fix<4>,fix<-3>)   // <=> [10 7 4 1]
-  * \endcode
-  *
-  * See also the function fix(int) to pass both a compile-time and runtime value.
-  *
-  * In c++14, it is implemented as:
-  * \code
-  * template<int N> static const internal::FixedInt<N> fix{};
-  * \endcode
-  * where internal::FixedInt<N> is an internal template class similar to
-  * <a href="http://en.cppreference.com/w/cpp/types/integral_constant">\c std::integral_constant </a><tt> <int,N> </tt>
-  * Here, \c fix<N> is thus an object of type \c internal::FixedInt<N>.
-  *
-  * In c++98/11, it is implemented as a function:
-  * \code
-  * template<int N> inline internal::FixedInt<N> fix();
-  * \endcode
-  * Here internal::FixedInt<N> is thus a pointer to function.
-  *
-  * If for some reason you want a true object in c++98 then you can write: \code fix<N>() \endcode which is also valid in c++14.
-  *
-  * \sa fix<N>(int), seq, seqN
-  */
-template<int N>
+ * \ingroup Core_Module
+ *
+ * This \em identifier permits to construct an object embedding a compile-time integer \c N.
+ *
+ * \tparam N the compile-time integer value
+ *
+ * It is typically used in conjunction with the Eigen::seq and Eigen::seqN functions to pass compile-time values to
+ * them: \code seqN(10,fix<4>,fix<-3>)   // <=> [10 7 4 1] \endcode
+ *
+ * See also the function fix(int) to pass both a compile-time and runtime value.
+ *
+ * In c++14, it is implemented as:
+ * \code
+ * template<int N> static const internal::FixedInt<N> fix{};
+ * \endcode
+ * where internal::FixedInt<N> is an internal template class similar to
+ * <a href="http://en.cppreference.com/w/cpp/types/integral_constant">\c std::integral_constant </a><tt> <int,N> </tt>
+ * Here, \c fix<N> is thus an object of type \c internal::FixedInt<N>.
+ *
+ * \sa fix<N>(int), seq, seqN
+ */
+template <int N>
 static const auto fix();
 
 /** \fn fix<N>(int)
-  * \ingroup Core_Module
-  *
-  * This function returns an object embedding both a compile-time integer \c N, and a fallback runtime value \a val.
-  *
-  * \tparam N the compile-time integer value
-  * \param  val the fallback runtime integer value
-  *
-  * This function is a more general version of the \ref fix identifier/function that can be used in template code
-  * where the compile-time value could turn out to actually mean "undefined at compile-time". For positive integers
-  * such as a size or a dimension, this case is identified by Eigen::Dynamic, whereas runtime signed integers
-  * (e.g., an increment/stride) are identified as Eigen::DynamicIndex. In such a case, the runtime value \a val
-  * will be used as a fallback.
-  *
-  * A typical use case would be:
-  * \code
-  * template<typename Derived> void foo(const MatrixBase<Derived> &mat) {
-  *   const int N = Derived::RowsAtCompileTime==Dynamic ? Dynamic : Derived::RowsAtCompileTime/2;
-  *   const int n = mat.rows()/2;
-  *   ... mat( seqN(0,fix<N>(n) ) ...;
-  * }
-  * \endcode
-  * In this example, the function Eigen::seqN knows that the second argument is expected to be a size.
-  * If the passed compile-time value N equals Eigen::Dynamic, then the proxy object returned by fix will be dissmissed, and converted to an Eigen::Index of value \c n.
-  * Otherwise, the runtime-value \c n will be dissmissed, and the returned ArithmeticSequence will be of the exact same type as <tt> seqN(0,fix<N>) </tt>.
-  *
-  * \sa fix, seqN, class ArithmeticSequence
-  */
-template<int N>
+ * \ingroup Core_Module
+ *
+ * This function returns an object embedding both a compile-time integer \c N, and a fallback runtime value \a val.
+ *
+ * \tparam N the compile-time integer value
+ * \param  val the fallback runtime integer value
+ *
+ * This function is a more general version of the \ref fix identifier/function that can be used in template code
+ * where the compile-time value could turn out to actually mean "undefined at compile-time". For positive integers
+ * such as a size or a dimension, this case is identified by Eigen::Dynamic, whereas runtime signed integers
+ * (e.g., an increment/stride) are identified as Eigen::DynamicIndex. In such a case, the runtime value \a val
+ * will be used as a fallback.
+ *
+ * A typical use case would be:
+ * \code
+ * template<typename Derived> void foo(const MatrixBase<Derived> &mat) {
+ *   const int N = Derived::RowsAtCompileTime==Dynamic ? Dynamic : Derived::RowsAtCompileTime/2;
+ *   const int n = mat.rows()/2;
+ *   ... mat( seqN(0,fix<N>(n) ) ...;
+ * }
+ * \endcode
+ * In this example, the function Eigen::seqN knows that the second argument is expected to be a size.
+ * If the passed compile-time value N equals Eigen::Dynamic, then the proxy object returned by fix will be dissmissed,
+ * and converted to an Eigen::Index of value \c n. Otherwise, the runtime-value \c n will be dissmissed, and the
+ * returned ArithmeticSequence will be of the exact same type as <tt> seqN(0,fix<N>) </tt>.
+ *
+ * \sa fix, seqN, class ArithmeticSequence
+ */
+template <int N>
 static const auto fix(int val);
 
-#endif // EIGEN_PARSED_BY_DOXYGEN
+#endif  // EIGEN_PARSED_BY_DOXYGEN
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_INTEGRAL_CONSTANT_H
+#endif  // EIGEN_INTEGRAL_CONSTANT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
index 81986b9..fda6ad9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
@@ -10,6 +10,8 @@
 
 #ifndef EIGEN_MACROS_H
 #define EIGEN_MACROS_H
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
 
 //------------------------------------------------------------------------------------------
 // Eigen version and basic defaults
@@ -17,11 +19,11 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 4
-#define EIGEN_MINOR_VERSION 0
+#define EIGEN_MINOR_VERSION 90
 
-#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
-                                      (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
-                                                                 EIGEN_MINOR_VERSION>=z))))
+#define EIGEN_VERSION_AT_LEAST(x, y, z) \
+  (EIGEN_WORLD_VERSION > x ||           \
+   (EIGEN_WORLD_VERSION >= x && (EIGEN_MAJOR_VERSION > y || (EIGEN_MAJOR_VERSION >= y && EIGEN_MINOR_VERSION >= z))))
 
 #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
 #define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::RowMajor
@@ -41,10 +43,10 @@
 #endif
 
 /** Allows to disable some optimizations which might affect the accuracy of the result.
-  * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
-  * They currently include:
-  *   - single precision ArrayBase::sin() and ArrayBase::cos() for SSE and AVX vectorization.
-  */
+ * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
+ * They currently include:
+ *   - single precision ArrayBase::sin() and ArrayBase::cos() for SSE and AVX vectorization.
+ */
 #ifndef EIGEN_FAST_MATH
 #define EIGEN_FAST_MATH 1
 #endif
@@ -68,80 +70,91 @@
 #endif
 #endif
 
-/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
+/// \internal EIGEN_COMP_GNUC set to version (e.g., 951 for GCC 9.5.1) for all compilers compatible with GCC
 #ifdef __GNUC__
-  #define EIGEN_COMP_GNUC (__GNUC__*10+__GNUC_MINOR__)
+#define EIGEN_COMP_GNUC (__GNUC__ * 100 + __GNUC_MINOR__ * 10 + __GNUC_PATCHLEVEL__)
 #else
-  #define EIGEN_COMP_GNUC 0
+#define EIGEN_COMP_GNUC 0
 #endif
 
-/// \internal EIGEN_COMP_CLANG set to major+minor version (e.g., 307 for clang 3.7) if the compiler is clang
+/// \internal EIGEN_COMP_CLANG set to version (e.g., 372 for clang 3.7.2) if the compiler is clang
 #if defined(__clang__)
-  #define EIGEN_COMP_CLANG (__clang_major__*100+__clang_minor__)
+#define EIGEN_COMP_CLANG (__clang_major__ * 100 + __clang_minor__ * 10 + __clang_patchlevel__)
 #else
-  #define EIGEN_COMP_CLANG 0
+#define EIGEN_COMP_CLANG 0
+#endif
+
+/// \internal EIGEN_COMP_CLANGAPPLE set to the version number (e.g. 9000000 for AppleClang 9.0) if the compiler is
+/// AppleClang
+#if defined(__clang__) && defined(__apple_build_version__)
+#define EIGEN_COMP_CLANGAPPLE __apple_build_version__
+#else
+#define EIGEN_COMP_CLANGAPPLE 0
 #endif
 
 /// \internal EIGEN_COMP_CASTXML set to 1 if being preprocessed by CastXML
 #if defined(__castxml__)
-  #define EIGEN_COMP_CASTXML 1
+#define EIGEN_COMP_CASTXML 1
 #else
-  #define EIGEN_COMP_CASTXML 0
+#define EIGEN_COMP_CASTXML 0
 #endif
 
 /// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm
 #if defined(__llvm__)
-  #define EIGEN_COMP_LLVM 1
+#define EIGEN_COMP_LLVM 1
 #else
-  #define EIGEN_COMP_LLVM 0
+#define EIGEN_COMP_LLVM 0
 #endif
 
-/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel compiler, 0 otherwise
+/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel icc compiler, 0 otherwise
 #if defined(__INTEL_COMPILER)
-  #define EIGEN_COMP_ICC __INTEL_COMPILER
+#define EIGEN_COMP_ICC __INTEL_COMPILER
 #else
-  #define EIGEN_COMP_ICC 0
+#define EIGEN_COMP_ICC 0
+#endif
+
+/// \internal EIGEN_COMP_CLANGICC set to __INTEL_CLANG_COMPILER if the compiler is Intel icx compiler, 0 otherwise
+#if defined(__INTEL_CLANG_COMPILER)
+#define EIGEN_COMP_CLANGICC __INTEL_CLANG_COMPILER
+#else
+#define EIGEN_COMP_CLANGICC 0
 #endif
 
 /// \internal EIGEN_COMP_MINGW set to 1 if the compiler is mingw
 #if defined(__MINGW32__)
-  #define EIGEN_COMP_MINGW 1
+#define EIGEN_COMP_MINGW 1
 #else
-  #define EIGEN_COMP_MINGW 0
+#define EIGEN_COMP_MINGW 0
 #endif
 
 /// \internal EIGEN_COMP_SUNCC set to 1 if the compiler is Solaris Studio
 #if defined(__SUNPRO_CC)
-  #define EIGEN_COMP_SUNCC 1
+#define EIGEN_COMP_SUNCC 1
 #else
-  #define EIGEN_COMP_SUNCC 0
+#define EIGEN_COMP_SUNCC 0
 #endif
 
 /// \internal EIGEN_COMP_MSVC set to _MSC_VER if the compiler is Microsoft Visual C++, 0 otherwise.
 #if defined(_MSC_VER)
-  #define EIGEN_COMP_MSVC _MSC_VER
+#define EIGEN_COMP_MSVC _MSC_VER
 #else
-  #define EIGEN_COMP_MSVC 0
+#define EIGEN_COMP_MSVC 0
 #endif
 
 #if defined(__NVCC__)
 #if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
-  #define EIGEN_COMP_NVCC  ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
+#define EIGEN_COMP_NVCC ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
 #elif defined(__CUDACC_VER__)
-  #define EIGEN_COMP_NVCC __CUDACC_VER__
+#define EIGEN_COMP_NVCC __CUDACC_VER__
 #else
-  #error "NVCC did not define compiler version."
+#error "NVCC did not define compiler version."
 #endif
 #else
-  #define EIGEN_COMP_NVCC 0
+#define EIGEN_COMP_NVCC 0
 #endif
 
 // For the record, here is a table summarizing the possible values for EIGEN_COMP_MSVC:
 //  name        ver   MSC_VER
-//  2008         9      1500
-//  2010        10      1600
-//  2012        11      1700
-//  2013        12      1800
 //  2015        14      1900
 //  "15"        15      1900
 //  2017-14.1   15.0    1910
@@ -149,12 +162,15 @@
 //  2017-14.12  15.5    1912
 //  2017-14.13  15.6    1913
 //  2017-14.14  15.7    1914
+//  2017        15.8    1915
+//  2017        15.9    1916
+//  2019 RTW    16.0    1920
 
 /// \internal EIGEN_COMP_MSVC_LANG set to _MSVC_LANG if the compiler is Microsoft Visual C++, 0 otherwise.
 #if defined(_MSVC_LANG)
-  #define EIGEN_COMP_MSVC_LANG _MSVC_LANG
+#define EIGEN_COMP_MSVC_LANG _MSVC_LANG
 #else
-  #define EIGEN_COMP_MSVC_LANG 0
+#define EIGEN_COMP_MSVC_LANG 0
 #endif
 
 // For the record, here is a table summarizing the possible values for EIGEN_COMP_MSVC_LANG:
@@ -163,11 +179,12 @@
 // /std:c++17                           C++17     201703L
 // /std:c++latest                       >C++17    >201703L
 
-/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC or clang-cl
+/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC or
+/// clang-cl
 #if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC || EIGEN_COMP_LLVM || EIGEN_COMP_CLANG)
-  #define EIGEN_COMP_MSVC_STRICT _MSC_VER
+#define EIGEN_COMP_MSVC_STRICT _MSC_VER
 #else
-  #define EIGEN_COMP_MSVC_STRICT 0
+#define EIGEN_COMP_MSVC_STRICT 0
 #endif
 
 /// \internal EIGEN_COMP_IBM set to xlc version if the compiler is IBM XL C++
@@ -177,102 +194,163 @@
 // 5.0   0x0500
 // 12.1  0x0C01
 #if defined(__IBMCPP__) || defined(__xlc__) || defined(__ibmxl__)
-  #define EIGEN_COMP_IBM __xlC__
+#define EIGEN_COMP_IBM __xlC__
 #else
-  #define EIGEN_COMP_IBM 0
+#define EIGEN_COMP_IBM 0
 #endif
 
 /// \internal EIGEN_COMP_PGI set to PGI version if the compiler is Portland Group Compiler
 #if defined(__PGI)
-  #define EIGEN_COMP_PGI (__PGIC__*100+__PGIC_MINOR__)
+#define EIGEN_COMP_PGI (__PGIC__ * 100 + __PGIC_MINOR__)
 #else
-  #define EIGEN_COMP_PGI 0
+#define EIGEN_COMP_PGI 0
 #endif
 
 /// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
 #if defined(__CC_ARM) || defined(__ARMCC_VERSION)
-  #define EIGEN_COMP_ARM 1
+#define EIGEN_COMP_ARM 1
 #else
-  #define EIGEN_COMP_ARM 0
+#define EIGEN_COMP_ARM 0
 #endif
 
 /// \internal EIGEN_COMP_EMSCRIPTEN set to 1 if the compiler is Emscripten Compiler
 #if defined(__EMSCRIPTEN__)
-  #define EIGEN_COMP_EMSCRIPTEN 1
+#define EIGEN_COMP_EMSCRIPTEN 1
 #else
-  #define EIGEN_COMP_EMSCRIPTEN 0
+#define EIGEN_COMP_EMSCRIPTEN 0
 #endif
 
-
-/// \internal EIGEN_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC, clang, mingw, etc.)
-#if EIGEN_COMP_GNUC && !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM || EIGEN_COMP_EMSCRIPTEN)
-  #define EIGEN_COMP_GNUC_STRICT 1
+/// \internal EIGEN_COMP_FCC set to FCC version if the compiler is Fujitsu Compiler (traditional mode)
+/// \note The Fujitsu C/C++ compiler uses the traditional mode based
+/// on EDG g++ 6.1 by default or if envoked with the -Nnoclang flag
+#if defined(__FUJITSU)
+#define EIGEN_COMP_FCC (__FCC_major__ * 100 + __FCC_minor__ * 10 + __FCC_patchlevel__)
 #else
-  #define EIGEN_COMP_GNUC_STRICT 0
+#define EIGEN_COMP_FCC 0
 #endif
 
-
-#if EIGEN_COMP_GNUC
-  #define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__==x && __GNUC_MINOR__>=y) || __GNUC__>x)
-  #define EIGEN_GNUC_AT_MOST(x,y)  ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
-  #define EIGEN_GNUC_AT(x,y)       ( __GNUC__==x && __GNUC_MINOR__==y )
+/// \internal EIGEN_COMP_CLANGFCC set to FCC version if the compiler is Fujitsu Compiler (Clang mode)
+/// \note The Fujitsu C/C++ compiler uses the non-traditional mode
+/// based on Clang 7.1.0 if envoked with the -Nclang flag
+#if defined(__CLANG_FUJITSU)
+#define EIGEN_COMP_CLANGFCC (__FCC_major__ * 100 + __FCC_minor__ * 10 + __FCC_patchlevel__)
 #else
-  #define EIGEN_GNUC_AT_LEAST(x,y) 0
-  #define EIGEN_GNUC_AT_MOST(x,y)  0
-  #define EIGEN_GNUC_AT(x,y)       0
+#define EIGEN_COMP_CLANGFCC 0
 #endif
 
-// FIXME: could probably be removed as we do not support gcc 3.x anymore
-#if EIGEN_COMP_GNUC && (__GNUC__ <= 3)
-#define EIGEN_GCC3_OR_OLDER 1
+/// \internal EIGEN_COMP_CPE set to CPE version if the compiler is HPE Cray Compiler (GCC based)
+/// \note This is the SVE-enabled C/C++ compiler from the HPE Cray
+/// Programming Environment (CPE) based on Cray GCC 8.1
+#if defined(_CRAYC) && !defined(__clang__)
+#define EIGEN_COMP_CPE (_RELEASE_MAJOR * 100 + _RELEASE_MINOR * 10 + _RELEASE_PATCHLEVEL)
 #else
-#define EIGEN_GCC3_OR_OLDER 0
+#define EIGEN_COMP_CPE 0
 #endif
 
+/// \internal EIGEN_COMP_CLANGCPE set to CPE version if the compiler is HPE Cray Compiler (Clang based)
+/// \note This is the C/C++ compiler from the HPE Cray Programming
+/// Environment (CPE) based on Cray Clang 11.0 without SVE-support
+#if defined(_CRAYC) && defined(__clang__)
+#define EIGEN_COMP_CLANGCPE (_RELEASE_MAJOR * 100 + _RELEASE_MINOR * 10 + _RELEASE_PATCHLEVEL)
+#else
+#define EIGEN_COMP_CLANGCPE 0
+#endif
 
+/// \internal EIGEN_COMP_LCC set to 1 if the compiler is MCST-LCC (MCST eLbrus Compiler Collection)
+#if defined(__LCC__) && defined(__MCST__)
+#define EIGEN_COMP_LCC (__LCC__ * 100 + __LCC_MINOR__)
+#else
+#define EIGEN_COMP_LCC 0
+#endif
+
+/// \internal EIGEN_COMP_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC,
+/// clang, mingw, etc.)
+#if EIGEN_COMP_GNUC &&                                                                                      \
+    !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_CLANGICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI ||    \
+      EIGEN_COMP_IBM || EIGEN_COMP_ARM || EIGEN_COMP_EMSCRIPTEN || EIGEN_COMP_FCC || EIGEN_COMP_CLANGFCC || \
+      EIGEN_COMP_CPE || EIGEN_COMP_CLANGCPE || EIGEN_COMP_LCC)
+#define EIGEN_COMP_GNUC_STRICT 1
+#else
+#define EIGEN_COMP_GNUC_STRICT 0
+#endif
+
+// GCC, and compilers that pretend to be it, have different version schemes, so this only makes sense to use with the
+// real GCC.
+#if EIGEN_COMP_GNUC_STRICT
+#define EIGEN_GNUC_STRICT_AT_LEAST(x, y, z)                   \
+  ((__GNUC__ > x) || (__GNUC__ == x && __GNUC_MINOR__ > y) || \
+   (__GNUC__ == x && __GNUC_MINOR__ == y && __GNUC_PATCHLEVEL__ >= z))
+#define EIGEN_GNUC_STRICT_LESS_THAN(x, y, z)                  \
+  ((__GNUC__ < x) || (__GNUC__ == x && __GNUC_MINOR__ < y) || \
+   (__GNUC__ == x && __GNUC_MINOR__ == y && __GNUC_PATCHLEVEL__ < z))
+#else
+#define EIGEN_GNUC_STRICT_AT_LEAST(x, y, z) 0
+#define EIGEN_GNUC_STRICT_LESS_THAN(x, y, z) 0
+#endif
+
+/// \internal EIGEN_COMP_CLANG_STRICT set to 1 if the compiler is really Clang and not a compatible compiler (e.g.,
+/// AppleClang, etc.)
+#if EIGEN_COMP_CLANG && !(EIGEN_COMP_CLANGAPPLE || EIGEN_COMP_CLANGICC || EIGEN_COMP_CLANGFCC || EIGEN_COMP_CLANGCPE)
+#define EIGEN_COMP_CLANG_STRICT 1
+#else
+#define EIGEN_COMP_CLANG_STRICT 0
+#endif
+
+// Clang, and compilers forked from it, have different version schemes, so this only makes sense to use with the real
+// Clang.
+#if EIGEN_COMP_CLANG_STRICT
+#define EIGEN_CLANG_STRICT_AT_LEAST(x, y, z)                                 \
+  ((__clang_major__ > x) || (__clang_major__ == x && __clang_minor__ > y) || \
+   (__clang_major__ == x && __clang_minor__ == y && __clang_patchlevel__ >= z))
+#define EIGEN_CLANG_STRICT_LESS_THAN(x, y, z)                                \
+  ((__clang_major__ < x) || (__clang_major__ == x && __clang_minor__ < y) || \
+   (__clang_major__ == x && __clang_minor__ == y && __clang_patchlevel__ < z))
+#else
+#define EIGEN_CLANG_STRICT_AT_LEAST(x, y, z) 0
+#define EIGEN_CLANG_STRICT_LESS_THAN(x, y, z) 0
+#endif
 
 //------------------------------------------------------------------------------------------
 // Architecture identification, EIGEN_ARCH_*
 //------------------------------------------------------------------------------------------
 
-
 #if defined(__x86_64__) || (defined(_M_X64) && !defined(_M_ARM64EC)) || defined(__amd64)
-  #define EIGEN_ARCH_x86_64 1
+#define EIGEN_ARCH_x86_64 1
 #else
-  #define EIGEN_ARCH_x86_64 0
+#define EIGEN_ARCH_x86_64 0
 #endif
 
 #if defined(__i386__) || defined(_M_IX86) || defined(_X86_) || defined(__i386)
-  #define EIGEN_ARCH_i386 1
+#define EIGEN_ARCH_i386 1
 #else
-  #define EIGEN_ARCH_i386 0
+#define EIGEN_ARCH_i386 0
 #endif
 
 #if EIGEN_ARCH_x86_64 || EIGEN_ARCH_i386
-  #define EIGEN_ARCH_i386_OR_x86_64 1
+#define EIGEN_ARCH_i386_OR_x86_64 1
 #else
-  #define EIGEN_ARCH_i386_OR_x86_64 0
+#define EIGEN_ARCH_i386_OR_x86_64 0
 #endif
 
 /// \internal EIGEN_ARCH_ARM set to 1 if the architecture is ARM
 #if defined(__arm__)
-  #define EIGEN_ARCH_ARM 1
+#define EIGEN_ARCH_ARM 1
 #else
-  #define EIGEN_ARCH_ARM 0
+#define EIGEN_ARCH_ARM 0
 #endif
 
 /// \internal EIGEN_ARCH_ARM64 set to 1 if the architecture is ARM64
 #if defined(__aarch64__) || defined(_M_ARM64) || defined(_M_ARM64EC)
-  #define EIGEN_ARCH_ARM64 1
+#define EIGEN_ARCH_ARM64 1
 #else
-  #define EIGEN_ARCH_ARM64 0
+#define EIGEN_ARCH_ARM64 0
 #endif
 
 /// \internal EIGEN_ARCH_ARM_OR_ARM64 set to 1 if the architecture is ARM or ARM64
 #if EIGEN_ARCH_ARM || EIGEN_ARCH_ARM64
-  #define EIGEN_ARCH_ARM_OR_ARM64 1
+#define EIGEN_ARCH_ARM_OR_ARM64 1
 #else
-  #define EIGEN_ARCH_ARM_OR_ARM64 0
+#define EIGEN_ARCH_ARM_OR_ARM64 0
 #endif
 
 /// \internal EIGEN_ARCH_ARMV8 set to 1 if the architecture is armv8 or greater.
@@ -282,158 +360,133 @@
 #define EIGEN_ARCH_ARMV8 0
 #endif
 
-
 /// \internal EIGEN_HAS_ARM64_FP16 set to 1 if the architecture provides an IEEE
 /// compliant Arm fp16 type
-#if EIGEN_ARCH_ARM64
-  #ifndef EIGEN_HAS_ARM64_FP16
-    #if defined(__ARM_FP16_FORMAT_IEEE)
-      #define EIGEN_HAS_ARM64_FP16 1
-    #else
-      #define EIGEN_HAS_ARM64_FP16 0
-    #endif
-  #endif
+#if EIGEN_ARCH_ARM_OR_ARM64
+#ifndef EIGEN_HAS_ARM64_FP16
+#if defined(__ARM_FP16_FORMAT_IEEE)
+#define EIGEN_HAS_ARM64_FP16 1
+#else
+#define EIGEN_HAS_ARM64_FP16 0
 #endif
-
-/// \internal EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC set to 1 if the architecture
-/// supports Neon vector intrinsics for fp16.
-#if EIGEN_ARCH_ARM64
-  #ifndef EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
-    #if defined(__ARM_FEATURE_FP16_VECTOR_ARITHMETIC)
-      #define EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC 1
-    #else
-      #define EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC 0
-    #endif
-  #endif
 #endif
-
-/// \internal EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC set to 1 if the architecture
-/// supports Neon scalar intrinsics for fp16.
-#if EIGEN_ARCH_ARM64
-  #ifndef EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC
-    #if defined(__ARM_FEATURE_FP16_SCALAR_ARITHMETIC)
-      #define EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC 1
-    #endif
-  #endif
 #endif
 
 /// \internal EIGEN_ARCH_MIPS set to 1 if the architecture is MIPS
 #if defined(__mips__) || defined(__mips)
-  #define EIGEN_ARCH_MIPS 1
+#define EIGEN_ARCH_MIPS 1
 #else
-  #define EIGEN_ARCH_MIPS 0
+#define EIGEN_ARCH_MIPS 0
 #endif
 
 /// \internal EIGEN_ARCH_SPARC set to 1 if the architecture is SPARC
 #if defined(__sparc__) || defined(__sparc)
-  #define EIGEN_ARCH_SPARC 1
+#define EIGEN_ARCH_SPARC 1
 #else
-  #define EIGEN_ARCH_SPARC 0
+#define EIGEN_ARCH_SPARC 0
 #endif
 
 /// \internal EIGEN_ARCH_IA64 set to 1 if the architecture is Intel Itanium
 #if defined(__ia64__)
-  #define EIGEN_ARCH_IA64 1
+#define EIGEN_ARCH_IA64 1
 #else
-  #define EIGEN_ARCH_IA64 0
+#define EIGEN_ARCH_IA64 0
 #endif
 
 /// \internal EIGEN_ARCH_PPC set to 1 if the architecture is PowerPC
-#if defined(__powerpc__) || defined(__ppc__) || defined(_M_PPC)
-  #define EIGEN_ARCH_PPC 1
+#if defined(__powerpc__) || defined(__ppc__) || defined(_M_PPC) || defined(__POWERPC__)
+#define EIGEN_ARCH_PPC 1
 #else
-  #define EIGEN_ARCH_PPC 0
+#define EIGEN_ARCH_PPC 0
 #endif
 
-
-
 //------------------------------------------------------------------------------------------
 // Operating system identification, EIGEN_OS_*
 //------------------------------------------------------------------------------------------
 
 /// \internal EIGEN_OS_UNIX set to 1 if the OS is a unix variant
 #if defined(__unix__) || defined(__unix)
-  #define EIGEN_OS_UNIX 1
+#define EIGEN_OS_UNIX 1
 #else
-  #define EIGEN_OS_UNIX 0
+#define EIGEN_OS_UNIX 0
 #endif
 
 /// \internal EIGEN_OS_LINUX set to 1 if the OS is based on Linux kernel
 #if defined(__linux__)
-  #define EIGEN_OS_LINUX 1
+#define EIGEN_OS_LINUX 1
 #else
-  #define EIGEN_OS_LINUX 0
+#define EIGEN_OS_LINUX 0
 #endif
 
 /// \internal EIGEN_OS_ANDROID set to 1 if the OS is Android
 // note: ANDROID is defined when using ndk_build, __ANDROID__ is defined when using a standalone toolchain.
 #if defined(__ANDROID__) || defined(ANDROID)
-  #define EIGEN_OS_ANDROID 1
+#define EIGEN_OS_ANDROID 1
 #else
-  #define EIGEN_OS_ANDROID 0
+#define EIGEN_OS_ANDROID 0
 #endif
 
 /// \internal EIGEN_OS_GNULINUX set to 1 if the OS is GNU Linux and not Linux-based OS (e.g., not android)
 #if defined(__gnu_linux__) && !(EIGEN_OS_ANDROID)
-  #define EIGEN_OS_GNULINUX 1
+#define EIGEN_OS_GNULINUX 1
 #else
-  #define EIGEN_OS_GNULINUX 0
+#define EIGEN_OS_GNULINUX 0
 #endif
 
 /// \internal EIGEN_OS_BSD set to 1 if the OS is a BSD variant
 #if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__OpenBSD__) || defined(__bsdi__) || defined(__DragonFly__)
-  #define EIGEN_OS_BSD 1
+#define EIGEN_OS_BSD 1
 #else
-  #define EIGEN_OS_BSD 0
+#define EIGEN_OS_BSD 0
 #endif
 
 /// \internal EIGEN_OS_MAC set to 1 if the OS is MacOS
 #if defined(__APPLE__)
-  #define EIGEN_OS_MAC 1
+#define EIGEN_OS_MAC 1
 #else
-  #define EIGEN_OS_MAC 0
+#define EIGEN_OS_MAC 0
 #endif
 
 /// \internal EIGEN_OS_QNX set to 1 if the OS is QNX
 #if defined(__QNX__)
-  #define EIGEN_OS_QNX 1
+#define EIGEN_OS_QNX 1
 #else
-  #define EIGEN_OS_QNX 0
+#define EIGEN_OS_QNX 0
 #endif
 
 /// \internal EIGEN_OS_WIN set to 1 if the OS is Windows based
 #if defined(_WIN32)
-  #define EIGEN_OS_WIN 1
+#define EIGEN_OS_WIN 1
 #else
-  #define EIGEN_OS_WIN 0
+#define EIGEN_OS_WIN 0
 #endif
 
 /// \internal EIGEN_OS_WIN64 set to 1 if the OS is Windows 64bits
 #if defined(_WIN64)
-  #define EIGEN_OS_WIN64 1
+#define EIGEN_OS_WIN64 1
 #else
-  #define EIGEN_OS_WIN64 0
+#define EIGEN_OS_WIN64 0
 #endif
 
 /// \internal EIGEN_OS_WINCE set to 1 if the OS is Windows CE
 #if defined(_WIN32_WCE)
-  #define EIGEN_OS_WINCE 1
+#define EIGEN_OS_WINCE 1
 #else
-  #define EIGEN_OS_WINCE 0
+#define EIGEN_OS_WINCE 0
 #endif
 
 /// \internal EIGEN_OS_CYGWIN set to 1 if the OS is Windows/Cygwin
 #if defined(__CYGWIN__)
-  #define EIGEN_OS_CYGWIN 1
+#define EIGEN_OS_CYGWIN 1
 #else
-  #define EIGEN_OS_CYGWIN 0
+#define EIGEN_OS_CYGWIN 0
 #endif
 
 /// \internal EIGEN_OS_WIN_STRICT set to 1 if the OS is really Windows and not some variants
-#if EIGEN_OS_WIN && !( EIGEN_OS_WINCE || EIGEN_OS_CYGWIN )
-  #define EIGEN_OS_WIN_STRICT 1
+#if EIGEN_OS_WIN && !(EIGEN_OS_WINCE || EIGEN_OS_CYGWIN)
+#define EIGEN_OS_WIN_STRICT 1
 #else
-  #define EIGEN_OS_WIN_STRICT 0
+#define EIGEN_OS_WIN_STRICT 0
 #endif
 
 /// \internal EIGEN_OS_SUN set to __SUNPRO_C if the OS is SUN
@@ -446,19 +499,18 @@
 // 5.11	     12.2      0x5110
 // 5.12	     12.3      0x5120
 #if (defined(sun) || defined(__sun)) && !(defined(__SVR4) || defined(__svr4__))
-  #define EIGEN_OS_SUN __SUNPRO_C
+#define EIGEN_OS_SUN __SUNPRO_C
 #else
-  #define EIGEN_OS_SUN 0
+#define EIGEN_OS_SUN 0
 #endif
 
 /// \internal EIGEN_OS_SOLARIS set to 1 if the OS is Solaris
 #if (defined(sun) || defined(__sun)) && (defined(__SVR4) || defined(__svr4__))
-  #define EIGEN_OS_SOLARIS 1
+#define EIGEN_OS_SOLARIS 1
 #else
-  #define EIGEN_OS_SOLARIS 0
+#define EIGEN_OS_SOLARIS 0
 #endif
 
-
 //------------------------------------------------------------------------------------------
 // Detect GPU compilers and architectures
 //------------------------------------------------------------------------------------------
@@ -466,59 +518,59 @@
 // NVCC is not supported as the target platform for HIPCC
 // Note that this also makes EIGEN_CUDACC and EIGEN_HIPCC mutually exclusive
 #if defined(__NVCC__) && defined(__HIPCC__)
-  #error "NVCC as the target platform for HIPCC is currently not supported."
+#error "NVCC as the target platform for HIPCC is currently not supported."
 #endif
 
-#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
-  // Means the compiler is either nvcc or clang with CUDA enabled
-  #define EIGEN_CUDACC __CUDACC__
+#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA) && !defined(__SYCL_DEVICE_ONLY__)
+// Means the compiler is either nvcc or clang with CUDA enabled
+#define EIGEN_CUDACC __CUDACC__
 #endif
 
-#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
-  // Means we are generating code for the device
-  #define EIGEN_CUDA_ARCH __CUDA_ARCH__
+#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA) && !defined(__SYCL_DEVICE_ONLY__)
+// Means we are generating code for the device
+#define EIGEN_CUDA_ARCH __CUDA_ARCH__
 #endif
 
 #if defined(EIGEN_CUDACC)
 #include <cuda.h>
-  #define EIGEN_CUDA_SDK_VER (CUDA_VERSION * 10)
+#define EIGEN_CUDA_SDK_VER (CUDA_VERSION * 10)
 #else
-  #define EIGEN_CUDA_SDK_VER 0
+#define EIGEN_CUDA_SDK_VER 0
 #endif
 
-#if defined(__HIPCC__) && !defined(EIGEN_NO_HIP)
-  // Means the compiler is HIPCC (analogous to EIGEN_CUDACC, but for HIP)
-  #define EIGEN_HIPCC __HIPCC__
+#if defined(__HIPCC__) && !defined(EIGEN_NO_HIP) && !defined(__SYCL_DEVICE_ONLY__)
+// Means the compiler is HIPCC (analogous to EIGEN_CUDACC, but for HIP)
+#define EIGEN_HIPCC __HIPCC__
 
-  // We need to include hip_runtime.h here because it pulls in
-  // ++ hip_common.h which contains the define for  __HIP_DEVICE_COMPILE__
-  // ++ host_defines.h which contains the defines for the __host__ and __device__ macros
-  #include <hip/hip_runtime.h>
+// We need to include hip_runtime.h here because it pulls in
+// ++ hip_common.h which contains the define for  __HIP_DEVICE_COMPILE__
+// ++ host_defines.h which contains the defines for the __host__ and __device__ macros
+#include <hip/hip_runtime.h>
 
-  #if defined(__HIP_DEVICE_COMPILE__)
-    // analogous to EIGEN_CUDA_ARCH, but for HIP
-    #define EIGEN_HIP_DEVICE_COMPILE __HIP_DEVICE_COMPILE__
-  #endif
+#if defined(__HIP_DEVICE_COMPILE__) && !defined(__SYCL_DEVICE_ONLY__)
+// analogous to EIGEN_CUDA_ARCH, but for HIP
+#define EIGEN_HIP_DEVICE_COMPILE __HIP_DEVICE_COMPILE__
+#endif
 
-  // For HIP (ROCm 3.5 and higher), we need to explicitly set the launch_bounds attribute
-  // value to 1024. The compiler assigns a default value of 256 when the attribute is not
-  // specified. This results in failures on the HIP platform, for cases when a GPU kernel
-  // without an explicit launch_bounds attribute is called with a threads_per_block value
-  // greater than 256.
-  //
-  // This is a regression in functioanlity and is expected to be fixed within the next
-  // couple of ROCm releases (compiler will go back to using 1024 value as the default)
-  //
-  // In the meantime, we will use a "only enabled for HIP" macro to set the launch_bounds
-  // attribute.
+// For HIP (ROCm 3.5 and higher), we need to explicitly set the launch_bounds attribute
+// value to 1024. The compiler assigns a default value of 256 when the attribute is not
+// specified. This results in failures on the HIP platform, for cases when a GPU kernel
+// without an explicit launch_bounds attribute is called with a threads_per_block value
+// greater than 256.
+//
+// This is a regression in functioanlity and is expected to be fixed within the next
+// couple of ROCm releases (compiler will go back to using 1024 value as the default)
+//
+// In the meantime, we will use a "only enabled for HIP" macro to set the launch_bounds
+// attribute.
 
-  #define EIGEN_HIP_LAUNCH_BOUNDS_1024 __launch_bounds__(1024)
+#define EIGEN_HIP_LAUNCH_BOUNDS_1024 __launch_bounds__(1024)
 
 #endif
 
 #if !defined(EIGEN_HIP_LAUNCH_BOUNDS_1024)
 #define EIGEN_HIP_LAUNCH_BOUNDS_1024
-#endif // !defined(EIGEN_HIP_LAUNCH_BOUNDS_1024)
+#endif  // !defined(EIGEN_HIP_LAUNCH_BOUNDS_1024)
 
 // Unify CUDA/HIPCC
 
@@ -575,6 +627,32 @@
 //
 #endif
 
+/// \internal EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC set to 1 if the architecture
+/// supports Neon vector intrinsics for fp16.
+#if EIGEN_ARCH_ARM_OR_ARM64
+#ifndef EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
+// Clang only supports FP16 on aarch64, and not all intrinsics are available
+// on A32 anyways even in GCC (e.g. vdiv_f16, vsqrt_f16).
+#if EIGEN_ARCH_ARM64 && defined(__ARM_FEATURE_FP16_VECTOR_ARITHMETIC) && !defined(EIGEN_GPU_COMPILE_PHASE)
+#define EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC 1
+#else
+#define EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC 0
+#endif
+#endif
+#endif
+
+/// \internal EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC set to 1 if the architecture
+/// supports Neon scalar intrinsics for fp16.
+#if EIGEN_ARCH_ARM_OR_ARM64
+#ifndef EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC
+// Clang only supports FP16 on aarch64, and not all intrinsics are available
+// on A32 anyways, even in GCC (e.g. vceqh_f16).
+#if EIGEN_ARCH_ARM64 && defined(__ARM_FEATURE_FP16_SCALAR_ARITHMETIC) && !defined(EIGEN_GPU_COMPILE_PHASE)
+#define EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC 1
+#endif
+#endif
+#endif
+
 #if defined(EIGEN_USE_SYCL) && defined(__SYCL_DEVICE_ONLY__)
 // EIGEN_USE_SYCL is a user-defined macro while __SYCL_DEVICE_ONLY__ is a compiler-defined macro.
 // In most cases we want to check if both macros are defined which can be done using the define below.
@@ -585,34 +663,17 @@
 // Detect Compiler/Architecture/OS specific features
 //------------------------------------------------------------------------------------------
 
-#if EIGEN_GNUC_AT_MOST(4,3) && !EIGEN_COMP_CLANG
-  // see bug 89
-  #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0
-#else
-  #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 1
-#endif
-
 // Cross compiler wrapper around LLVM's __has_builtin
 #ifdef __has_builtin
-#  define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
+#define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
 #else
-#  define EIGEN_HAS_BUILTIN(x) 0
+#define EIGEN_HAS_BUILTIN(x) 0
 #endif
 
 // A Clang feature extension to determine compiler features.
 // We use it to determine 'cxx_rvalue_references'
 #ifndef __has_feature
-# define __has_feature(x) 0
-#endif
-
-// Some old compilers do not support template specializations like:
-// template<typename T,int N> void foo(const T x[N]);
-#if !(   EIGEN_COMP_CLANG && (   (EIGEN_COMP_CLANG<309)                                                       \
-                              || (defined(__apple_build_version__) && (__apple_build_version__ < 9000000)))  \
-      || EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC<49)
-#define EIGEN_HAS_STATIC_ARRAY_TEMPLATE 1
-#else
-#define EIGEN_HAS_STATIC_ARRAY_TEMPLATE 0
+#define __has_feature(x) 0
 #endif
 
 // The macro EIGEN_CPLUSPLUS is a replacement for __cplusplus/_MSVC_LANG that
@@ -632,80 +693,42 @@
 #define EIGEN_CPLUSPLUS 0
 #endif
 
-// The macro EIGEN_COMP_CXXVER defines the c++ verson expected by the compiler.
+// The macro EIGEN_COMP_CXXVER defines the c++ version expected by the compiler.
 // For instance, if compiling with gcc and -std=c++17, then EIGEN_COMP_CXXVER
 // is defined to 17.
-#if EIGEN_CPLUSPLUS > 201703L
-  #define EIGEN_COMP_CXXVER 20
-#elif EIGEN_CPLUSPLUS > 201402L
-  #define EIGEN_COMP_CXXVER 17
-#elif EIGEN_CPLUSPLUS > 201103L
-  #define EIGEN_COMP_CXXVER 14
+#if EIGEN_CPLUSPLUS >= 202002L
+#define EIGEN_COMP_CXXVER 20
+#elif EIGEN_CPLUSPLUS >= 201703L
+#define EIGEN_COMP_CXXVER 17
+#elif EIGEN_CPLUSPLUS >= 201402L
+#define EIGEN_COMP_CXXVER 14
 #elif EIGEN_CPLUSPLUS >= 201103L
-  #define EIGEN_COMP_CXXVER 11
+#define EIGEN_COMP_CXXVER 11
 #else
-  #define EIGEN_COMP_CXXVER 03
+#define EIGEN_COMP_CXXVER 03
 #endif
 
-#ifndef EIGEN_HAS_CXX14_VARIABLE_TEMPLATES
-  #if defined(__cpp_variable_templates) && __cpp_variable_templates >= 201304 && EIGEN_MAX_CPP_VER>=14
-    #define EIGEN_HAS_CXX14_VARIABLE_TEMPLATES 1
-  #else
-    #define EIGEN_HAS_CXX14_VARIABLE_TEMPLATES 0
-  #endif
-#endif
-
-
 // The macros EIGEN_HAS_CXX?? defines a rough estimate of available c++ features
-// but in practice we should not rely on them but rather on the availabilty of
+// but in practice we should not rely on them but rather on the availability of
 // individual features as defined later.
 // This is why there is no EIGEN_HAS_CXX17.
-// FIXME: get rid of EIGEN_HAS_CXX14 and maybe even EIGEN_HAS_CXX11.
-#if EIGEN_MAX_CPP_VER>=11 && EIGEN_COMP_CXXVER>=11
-#define EIGEN_HAS_CXX11 1
-#else
-#define EIGEN_HAS_CXX11 0
-#endif
-
-#if EIGEN_MAX_CPP_VER>=14 && EIGEN_COMP_CXXVER>=14
-#define EIGEN_HAS_CXX14 1
-#else
-#define EIGEN_HAS_CXX14 0
-#endif
-
-// Do we support r-value references?
-#ifndef EIGEN_HAS_RVALUE_REFERENCES
-#if EIGEN_MAX_CPP_VER>=11 && \
-    (__has_feature(cxx_rvalue_references) || \
-     (EIGEN_COMP_CXXVER >= 11) || (EIGEN_COMP_MSVC >= 1600))
-  #define EIGEN_HAS_RVALUE_REFERENCES 1
-#else
-  #define EIGEN_HAS_RVALUE_REFERENCES 0
-#endif
+#if EIGEN_MAX_CPP_VER < 14 || EIGEN_COMP_CXXVER < 14 || (EIGEN_COMP_MSVC && EIGEN_COMP_MSVC < 1900) || \
+    (EIGEN_COMP_ICC && EIGEN_COMP_ICC < 1500) || (EIGEN_COMP_NVCC && EIGEN_COMP_NVCC < 80000) ||       \
+    (EIGEN_COMP_CLANG_STRICT && EIGEN_COMP_CLANG < 390) ||                                             \
+    (EIGEN_COMP_CLANGAPPLE && EIGEN_COMP_CLANGAPPLE < 9000000) || (EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC < 510)
+#error This compiler appears to be too old to be supported by Eigen
 #endif
 
 // Does the compiler support C99?
 // Need to include <cmath> to make sure _GLIBCXX_USE_C99 gets defined
 #include <cmath>
 #ifndef EIGEN_HAS_C99_MATH
-#if EIGEN_MAX_CPP_VER>=11 && \
-    ((defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901))       \
-  || (defined(__GNUC__) && defined(_GLIBCXX_USE_C99)) \
-  || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)) \
-  || (EIGEN_COMP_MSVC >= 1900) || defined(SYCL_DEVICE_ONLY))
-  #define EIGEN_HAS_C99_MATH 1
+#if ((defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)) ||                                          \
+     (defined(__GNUC__) && defined(_GLIBCXX_USE_C99)) || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)) || \
+     (EIGEN_COMP_MSVC) || defined(SYCL_DEVICE_ONLY))
+#define EIGEN_HAS_C99_MATH 1
 #else
-  #define EIGEN_HAS_C99_MATH 0
-#endif
-#endif
-
-// Does the compiler support result_of?
-// result_of was deprecated in c++17 and removed in c++ 20
-#ifndef EIGEN_HAS_STD_RESULT_OF
-#if EIGEN_HAS_CXX11 && EIGEN_COMP_CXXVER < 17
-#define EIGEN_HAS_STD_RESULT_OF 1
-#else
-#define EIGEN_HAS_STD_RESULT_OF 0
+#define EIGEN_HAS_C99_MATH 0
 #endif
 #endif
 
@@ -713,7 +736,7 @@
 #ifndef EIGEN_HAS_STD_HASH
 // The std::hash struct is defined in C++11 but is not labelled as a __device__
 // function and is not constexpr, so cannot be used on device.
-#if EIGEN_HAS_CXX11 && !defined(EIGEN_GPU_COMPILE_PHASE)
+#if !defined(EIGEN_GPU_COMPILE_PHASE)
 #define EIGEN_HAS_STD_HASH 1
 #else
 #define EIGEN_HAS_STD_HASH 0
@@ -728,157 +751,37 @@
 #endif
 #endif
 
-#ifndef EIGEN_HAS_ALIGNAS
-#if EIGEN_MAX_CPP_VER>=11 && EIGEN_HAS_CXX11 &&   \
-      (     __has_feature(cxx_alignas)            \
-        ||  EIGEN_HAS_CXX14                       \
-        || (EIGEN_COMP_MSVC >= 1800)              \
-        || (EIGEN_GNUC_AT_LEAST(4,8))             \
-        || (EIGEN_COMP_CLANG>=305)                \
-        || (EIGEN_COMP_ICC>=1500)                 \
-        || (EIGEN_COMP_PGI>=1500)                 \
-        || (EIGEN_COMP_SUNCC>=0x5130))
-#define EIGEN_HAS_ALIGNAS 1
-#else
-#define EIGEN_HAS_ALIGNAS 0
-#endif
-#endif
-
-// Does the compiler support type_traits?
-// - full support of type traits was added only to GCC 5.1.0.
-// - 20150626 corresponds to the last release of 4.x libstdc++
-#ifndef EIGEN_HAS_TYPE_TRAITS
-#if EIGEN_MAX_CPP_VER>=11 && (EIGEN_HAS_CXX11 || EIGEN_COMP_MSVC >= 1700) \
-  && ((!EIGEN_COMP_GNUC_STRICT) || EIGEN_GNUC_AT_LEAST(5, 1)) \
-  && ((!defined(__GLIBCXX__))   || __GLIBCXX__ > 20150626)
-#define EIGEN_HAS_TYPE_TRAITS 1
-#define EIGEN_INCLUDE_TYPE_TRAITS
-#else
-#define EIGEN_HAS_TYPE_TRAITS 0
-#endif
-#endif
-
-// Does the compiler support variadic templates?
-#ifndef EIGEN_HAS_VARIADIC_TEMPLATES
-#if EIGEN_MAX_CPP_VER>=11 && (EIGEN_COMP_CXXVER >= 11) \
-  && (!defined(__NVCC__) || !EIGEN_ARCH_ARM_OR_ARM64 || (EIGEN_COMP_NVCC >= 80000) )
-    // ^^ Disable the use of variadic templates when compiling with versions of nvcc older than 8.0 on ARM devices:
-    //    this prevents nvcc from crashing when compiling Eigen on Tegra X1
-#define EIGEN_HAS_VARIADIC_TEMPLATES 1
-#elif  EIGEN_MAX_CPP_VER>=11 && (EIGEN_COMP_CXXVER >= 11) && defined(SYCL_DEVICE_ONLY)
-#define EIGEN_HAS_VARIADIC_TEMPLATES 1
-#else
-#define EIGEN_HAS_VARIADIC_TEMPLATES 0
-#endif
-#endif
-
-// Does the compiler fully support const expressions? (as in c++14)
-#ifndef EIGEN_HAS_CONSTEXPR
-  #if defined(EIGEN_CUDACC)
-  // Const expressions are supported provided that c++11 is enabled and we're using either clang or nvcc 7.5 or above
-    #if EIGEN_MAX_CPP_VER>=14 && (EIGEN_COMP_CXXVER >= 11 && (EIGEN_COMP_CLANG || EIGEN_COMP_NVCC >= 70500))
-      #define EIGEN_HAS_CONSTEXPR 1
-    #endif
-  #elif EIGEN_MAX_CPP_VER>=14 && (__has_feature(cxx_relaxed_constexpr) || (EIGEN_COMP_CXXVER >= 14) || \
-    (EIGEN_GNUC_AT_LEAST(4,8) && (EIGEN_COMP_CXXVER >= 11)) || \
-    (EIGEN_COMP_CLANG >= 306 && (EIGEN_COMP_CXXVER >= 11)))
-    #define EIGEN_HAS_CONSTEXPR 1
-  #endif
-
-  #ifndef EIGEN_HAS_CONSTEXPR
-    #define EIGEN_HAS_CONSTEXPR 0
-  #endif
-
-#endif // EIGEN_HAS_CONSTEXPR
-
-#if EIGEN_HAS_CONSTEXPR
 #define EIGEN_CONSTEXPR constexpr
-#else
-#define EIGEN_CONSTEXPR
-#endif
-
-// Does the compiler support C++11 math?
-// Let's be conservative and enable the default C++11 implementation only if we are sure it exists
-#ifndef EIGEN_HAS_CXX11_MATH
-  #if EIGEN_MAX_CPP_VER>=11 && ((EIGEN_COMP_CXXVER > 11) || (EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC)  \
-      && (EIGEN_ARCH_i386_OR_x86_64) && (EIGEN_OS_GNULINUX || EIGEN_OS_WIN_STRICT || EIGEN_OS_MAC))
-    #define EIGEN_HAS_CXX11_MATH 1
-  #else
-    #define EIGEN_HAS_CXX11_MATH 0
-  #endif
-#endif
-
-// Does the compiler support proper C++11 containers?
-#ifndef EIGEN_HAS_CXX11_CONTAINERS
-  #if    EIGEN_MAX_CPP_VER>=11 && \
-         ((EIGEN_COMP_CXXVER > 11) \
-      || ((EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC>=1400)))
-    #define EIGEN_HAS_CXX11_CONTAINERS 1
-  #else
-    #define EIGEN_HAS_CXX11_CONTAINERS 0
-  #endif
-#endif
-
-// Does the compiler support C++11 noexcept?
-#ifndef EIGEN_HAS_CXX11_NOEXCEPT
-  #if    EIGEN_MAX_CPP_VER>=11 && \
-         (__has_feature(cxx_noexcept) \
-      || (EIGEN_COMP_CXXVER > 11) \
-      || ((EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC>=1400)))
-    #define EIGEN_HAS_CXX11_NOEXCEPT 1
-  #else
-    #define EIGEN_HAS_CXX11_NOEXCEPT 0
-  #endif
-#endif
-
-#ifndef EIGEN_HAS_CXX11_ATOMIC
-  #if    EIGEN_MAX_CPP_VER>=11 && \
-         (__has_feature(cxx_atomic) \
-      || (EIGEN_COMP_CXXVER > 11) \
-      || ((EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_MSVC==0 || EIGEN_COMP_MSVC >= 1700)))
-    #define EIGEN_HAS_CXX11_ATOMIC 1
-  #else
-    #define EIGEN_HAS_CXX11_ATOMIC 0
-  #endif
-#endif
-
-#ifndef EIGEN_HAS_CXX11_OVERRIDE_FINAL
-  #if    EIGEN_MAX_CPP_VER>=11 && \
-       (EIGEN_COMP_CXXVER >= 11 || EIGEN_COMP_MSVC >= 1700)
-    #define EIGEN_HAS_CXX11_OVERRIDE_FINAL 1
-  #else
-    #define EIGEN_HAS_CXX11_OVERRIDE_FINAL 0
-  #endif
-#endif
 
 // NOTE: the required Apple's clang version is very conservative
 //       and it could be that XCode 9 works just fine.
 // NOTE: the MSVC version is based on https://en.cppreference.com/w/cpp/compiler_support
 //       and not tested.
+// NOTE: Intel C++ Compiler Classic (icc) Version 19.0 and later supports dynamic allocation
+//       for over-aligned data, but not in a manner that is compatible with Eigen.
+//       See https://gitlab.com/libeigen/eigen/-/issues/2575
 #ifndef EIGEN_HAS_CXX17_OVERALIGN
-#if EIGEN_MAX_CPP_VER>=17 && EIGEN_COMP_CXXVER>=17 && (                                 \
-           (EIGEN_COMP_MSVC >= 1912)                                                    \
-        || (EIGEN_GNUC_AT_LEAST(7,0))                                                   \
-        || ((!defined(__apple_build_version__)) && (EIGEN_COMP_CLANG>=500))             \
-        || (( defined(__apple_build_version__)) && (__apple_build_version__>=10000000)) \
-      )
+#if EIGEN_MAX_CPP_VER >= 17 && EIGEN_COMP_CXXVER >= 17 &&                                                            \
+    ((EIGEN_COMP_MSVC >= 1912) || (EIGEN_GNUC_STRICT_AT_LEAST(7, 0, 0)) || (EIGEN_CLANG_STRICT_AT_LEAST(5, 0, 0)) || \
+     (EIGEN_COMP_CLANGAPPLE && EIGEN_COMP_CLANGAPPLE >= 10000000)) &&                                                \
+    !EIGEN_COMP_ICC
 #define EIGEN_HAS_CXX17_OVERALIGN 1
 #else
 #define EIGEN_HAS_CXX17_OVERALIGN 0
 #endif
 #endif
 
-#if defined(EIGEN_CUDACC) && EIGEN_HAS_CONSTEXPR
-  // While available already with c++11, this is useful mostly starting with c++14 and relaxed constexpr rules
-  #if defined(__NVCC__)
-    // nvcc considers constexpr functions as __host__ __device__ with the option --expt-relaxed-constexpr
-    #ifdef __CUDACC_RELAXED_CONSTEXPR__
-      #define EIGEN_CONSTEXPR_ARE_DEVICE_FUNC
-    #endif
-  #elif defined(__clang__) && defined(__CUDA__) && __has_feature(cxx_relaxed_constexpr)
-    // clang++ always considers constexpr functions as implicitly __host__ __device__
-    #define EIGEN_CONSTEXPR_ARE_DEVICE_FUNC
-  #endif
+#if defined(EIGEN_CUDACC)
+// While available already with c++11, this is useful mostly starting with c++14 and relaxed constexpr rules
+#if defined(__NVCC__)
+// nvcc considers constexpr functions as __host__ __device__ with the option --expt-relaxed-constexpr
+#ifdef __CUDACC_RELAXED_CONSTEXPR__
+#define EIGEN_CONSTEXPR_ARE_DEVICE_FUNC
+#endif
+#elif defined(__clang__) && defined(__CUDA__) && __has_feature(cxx_relaxed_constexpr)
+// clang++ always considers constexpr functions as implicitly __host__ __device__
+#define EIGEN_CONSTEXPR_ARE_DEVICE_FUNC
+#endif
 #endif
 
 // Does the compiler support the __int128 and __uint128_t extensions for 128-bit
@@ -908,8 +811,8 @@
 #define EIGEN_DEBUG_VAR(x) std::cerr << #x << " = " << x << std::endl;
 
 // concatenate two tokens
-#define EIGEN_CAT2(a,b) a ## b
-#define EIGEN_CAT(a,b) EIGEN_CAT2(a,b)
+#define EIGEN_CAT2(a, b) a##b
+#define EIGEN_CAT(a, b) EIGEN_CAT2(a, b)
 
 #define EIGEN_COMMA ,
 
@@ -928,15 +831,11 @@
 #endif
 #endif
 
-// EIGEN_ALWAYS_INLINE is the stronget, it has the effect of making the function inline and adding every possible
+// EIGEN_ALWAYS_INLINE is the strongest, it has the effect of making the function inline and adding every possible
 // attribute to maximize inlining. This should only be used when really necessary: in particular,
 // it uses __attribute__((always_inline)) on GCC, which most of the time is useless and can severely harm compile times.
 // FIXME with the always_inline attribute,
-// gcc 3.4.x and 4.1 reports the following compilation error:
-//   Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval<Derived> Eigen::MatrixBase<Scalar, Derived>::eval() const'
-//    : function body not available
-//   See also bug 1367
-#if EIGEN_GNUC_AT_LEAST(4,2) && !defined(SYCL_DEVICE_ONLY)
+#if EIGEN_COMP_GNUC && !defined(SYCL_DEVICE_ONLY)
 #define EIGEN_ALWAYS_INLINE __attribute__((always_inline)) inline
 #else
 #define EIGEN_ALWAYS_INLINE EIGEN_STRONG_INLINE
@@ -958,35 +857,37 @@
 
 // GPU stuff
 
-// Disable some features when compiling with GPU compilers (NVCC/clang-cuda/SYCL/HIPCC)
-#if defined(EIGEN_CUDACC) || defined(SYCL_DEVICE_ONLY) || defined(EIGEN_HIPCC)
-  // Do not try asserts on device code
-  #ifndef EIGEN_NO_DEBUG
-  #define EIGEN_NO_DEBUG
-  #endif
+// Disable some features when compiling with GPU compilers (SYCL/HIPCC)
+#if defined(SYCL_DEVICE_ONLY) || defined(EIGEN_HIP_DEVICE_COMPILE)
+// Do not try asserts on device code
+#ifndef EIGEN_NO_DEBUG
+#define EIGEN_NO_DEBUG
+#endif
 
-  #ifdef EIGEN_INTERNAL_DEBUGGING
-  #undef EIGEN_INTERNAL_DEBUGGING
-  #endif
+#ifdef EIGEN_INTERNAL_DEBUGGING
+#undef EIGEN_INTERNAL_DEBUGGING
+#endif
+#endif
 
-  #ifdef EIGEN_EXCEPTIONS
-  #undef EIGEN_EXCEPTIONS
-  #endif
+// No exceptions on device.
+#if defined(SYCL_DEVICE_ONLY) || defined(EIGEN_GPU_COMPILE_PHASE)
+#ifdef EIGEN_EXCEPTIONS
+#undef EIGEN_EXCEPTIONS
+#endif
 #endif
 
 #if defined(SYCL_DEVICE_ONLY)
-  #ifndef EIGEN_DONT_VECTORIZE
-    #define EIGEN_DONT_VECTORIZE
-  #endif
-  #define EIGEN_DEVICE_FUNC __attribute__((flatten)) __attribute__((always_inline))
+#ifndef EIGEN_DONT_VECTORIZE
+#define EIGEN_DONT_VECTORIZE
+#endif
+#define EIGEN_DEVICE_FUNC __attribute__((flatten)) __attribute__((always_inline))
 // All functions callable from CUDA/HIP code must be qualified with __device__
 #elif defined(EIGEN_GPUCC)
-    #define EIGEN_DEVICE_FUNC __host__ __device__
+#define EIGEN_DEVICE_FUNC __host__ __device__
 #else
-  #define EIGEN_DEVICE_FUNC
+#define EIGEN_DEVICE_FUNC
 #endif
 
-
 // this macro allows to get rid of linking errors about multiply defined functions.
 //  - static is not very good because it prevents definitions from different object files to be merged.
 //           So static causes the resulting linked executable to be bloated with multiple copies of the same function.
@@ -995,51 +896,9 @@
 #define EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_DEVICE_FUNC inline
 
 #ifdef NDEBUG
-# ifndef EIGEN_NO_DEBUG
-#  define EIGEN_NO_DEBUG
-# endif
+#ifndef EIGEN_NO_DEBUG
+#define EIGEN_NO_DEBUG
 #endif
-
-// eigen_plain_assert is where we implement the workaround for the assert() bug in GCC <= 4.3, see bug 89
-#ifdef EIGEN_NO_DEBUG
-  #ifdef SYCL_DEVICE_ONLY // used to silence the warning on SYCL device
-    #define eigen_plain_assert(x) EIGEN_UNUSED_VARIABLE(x)
-  #else
-    #define eigen_plain_assert(x)
-  #endif
-#else
-  #if EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO
-    namespace Eigen {
-    namespace internal {
-    inline bool copy_bool(bool b) { return b; }
-    }
-    }
-    #define eigen_plain_assert(x) assert(x)
-  #else
-    // work around bug 89
-    #include <cstdlib>   // for abort
-    #include <iostream>  // for std::cerr
-
-    namespace Eigen {
-    namespace internal {
-    // trivial function copying a bool. Must be EIGEN_DONT_INLINE, so we implement it after including Eigen headers.
-    // see bug 89.
-    namespace {
-    EIGEN_DONT_INLINE bool copy_bool(bool b) { return b; }
-    }
-    inline void assert_fail(const char *condition, const char *function, const char *file, int line)
-    {
-      std::cerr << "assertion failed: " << condition << " in function " << function << " at " << file << ":" << line << std::endl;
-      abort();
-    }
-    }
-    }
-    #define eigen_plain_assert(x) \
-      do { \
-        if(!Eigen::internal::copy_bool(x)) \
-          Eigen::internal::assert_fail(EIGEN_MAKESTRING(x), __PRETTY_FUNCTION__, __FILE__, __LINE__); \
-      } while(false)
-  #endif
 #endif
 
 // eigen_assert can be overridden
@@ -1050,25 +909,25 @@
 #ifdef EIGEN_INTERNAL_DEBUGGING
 #define eigen_internal_assert(x) eigen_assert(x)
 #else
-#define eigen_internal_assert(x)
+#define eigen_internal_assert(x) ((void)0)
 #endif
 
-#ifdef EIGEN_NO_DEBUG
+#if defined(EIGEN_NO_DEBUG) || (defined(EIGEN_GPU_COMPILE_PHASE) && defined(EIGEN_NO_DEBUG_GPU))
 #define EIGEN_ONLY_USED_FOR_DEBUG(x) EIGEN_UNUSED_VARIABLE(x)
 #else
 #define EIGEN_ONLY_USED_FOR_DEBUG(x)
 #endif
 
 #ifndef EIGEN_NO_DEPRECATED_WARNING
-  #if EIGEN_COMP_GNUC
-    #define EIGEN_DEPRECATED __attribute__((deprecated))
-  #elif EIGEN_COMP_MSVC
-    #define EIGEN_DEPRECATED __declspec(deprecated)
-  #else
-    #define EIGEN_DEPRECATED
-  #endif
+#if EIGEN_COMP_GNUC
+#define EIGEN_DEPRECATED __attribute__((deprecated))
+#elif EIGEN_COMP_MSVC
+#define EIGEN_DEPRECATED __declspec(deprecated)
 #else
-  #define EIGEN_DEPRECATED
+#define EIGEN_DEPRECATED
+#endif
+#else
+#define EIGEN_DEPRECATED
 #endif
 
 #if EIGEN_COMP_GNUC
@@ -1077,22 +936,38 @@
 #define EIGEN_UNUSED
 #endif
 
+#if EIGEN_COMP_GNUC
+#define EIGEN_PRAGMA(tokens) _Pragma(#tokens)
+#define EIGEN_DIAGNOSTICS(tokens) EIGEN_PRAGMA(GCC diagnostic tokens)
+#define EIGEN_DIAGNOSTICS_OFF(msc, gcc) EIGEN_DIAGNOSTICS(gcc)
+#elif EIGEN_COMP_MSVC
+#define EIGEN_PRAGMA(tokens) __pragma(tokens)
+#define EIGEN_DIAGNOSTICS(tokens) EIGEN_PRAGMA(warning(tokens))
+#define EIGEN_DIAGNOSTICS_OFF(msc, gcc) EIGEN_DIAGNOSTICS(msc)
+#else
+#define EIGEN_PRAGMA(tokens)
+#define EIGEN_DIAGNOSTICS(tokens)
+#define EIGEN_DIAGNOSTICS_OFF(msc, gcc)
+#endif
+
+#define EIGEN_DISABLE_DEPRECATED_WARNING EIGEN_DIAGNOSTICS_OFF(disable : 4996, ignored "-Wdeprecated-declarations")
+
 // Suppresses 'unused variable' warnings.
 namespace Eigen {
-  namespace internal {
-    template<typename T> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ignore_unused_variable(const T&) {}
-  }
-}
+namespace internal {
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void ignore_unused_variable(const T&) {}
+}  // namespace internal
+}  // namespace Eigen
 #define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var);
 
 #if !defined(EIGEN_ASM_COMMENT)
-  #if EIGEN_COMP_GNUC && (EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64)
-    #define EIGEN_ASM_COMMENT(X)  __asm__("#" X)
-  #else
-    #define EIGEN_ASM_COMMENT(X)
-  #endif
+#if EIGEN_COMP_GNUC && (EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64)
+#define EIGEN_ASM_COMMENT(X) __asm__("#" X)
+#else
+#define EIGEN_ASM_COMMENT(X)
 #endif
-
+#endif
 
 // Acts as a barrier preventing operations involving `X` from crossing. This
 // occurs, for example, in the fast rounding trick where a magic constant is
@@ -1100,77 +975,90 @@
 //
 // See bug 1674
 #if !defined(EIGEN_OPTIMIZATION_BARRIER)
-  #if EIGEN_COMP_GNUC
-    // According to https://gcc.gnu.org/onlinedocs/gcc/Constraints.html:
-    //   X: Any operand whatsoever.
-    //   r: A register operand is allowed provided that it is in a general
-    //      register.
-    //   g: Any register, memory or immediate integer operand is allowed, except
-    //      for registers that are not general registers.
-    //   w: (AArch32/AArch64) Floating point register, Advanced SIMD vector
-    //      register or SVE vector register.
-    //   x: (SSE) Any SSE register.
-    //      (AArch64) Like w, but restricted to registers 0 to 15 inclusive.
-    //   v: (PowerPC) An Altivec vector register.
-    //   wa:(PowerPC) A VSX register.
-    //
-    // "X" (uppercase) should work for all cases, though this seems to fail for
-    // some versions of GCC for arm/aarch64 with
-    //   "error: inconsistent operand constraints in an 'asm'"
-    // Clang x86_64/arm/aarch64 seems to require "g" to support both scalars and
-    // vectors, otherwise
-    //   "error: non-trivial scalar-to-vector conversion, possible invalid
-    //    constraint for vector type"
-    //
-    // GCC for ppc64le generates an internal compiler error with x/X/g.
-    // GCC for AVX generates an internal compiler error with X.
-    //
-    // Tested on icc/gcc/clang for sse, avx, avx2, avx512dq
-    //           gcc for arm, aarch64,
-    //           gcc for ppc64le,
-    // both vectors and scalars.
-    //
-    // Note that this is restricted to plain types - this will not work
-    // directly for std::complex<T>, Eigen::half, Eigen::bfloat16. For these,
-    // you will need to apply to the underlying POD type.
-    #if EIGEN_ARCH_PPC && EIGEN_COMP_GNUC_STRICT
-      // This seems to be broken on clang.  Packet4f is loaded into a single
-      //   register rather than a vector, zeroing out some entries.  Integer
-      //   types also generate a compile error.
-      // General, Altivec, VSX.
-      #define EIGEN_OPTIMIZATION_BARRIER(X)  __asm__  ("" : "+r,v,wa" (X));
-    #elif EIGEN_ARCH_ARM_OR_ARM64
-      // General, NEON.
-      #define EIGEN_OPTIMIZATION_BARRIER(X)  __asm__  ("" : "+g,w" (X));
-    #elif EIGEN_ARCH_i386_OR_x86_64
-      // General, SSE.
-      #define EIGEN_OPTIMIZATION_BARRIER(X)  __asm__  ("" : "+g,x" (X));
-    #else
-      // Not implemented for other architectures.
-      #define EIGEN_OPTIMIZATION_BARRIER(X)
-    #endif
-  #else
-    // Not implemented for other compilers.
-    #define EIGEN_OPTIMIZATION_BARRIER(X)
-  #endif
+#if EIGEN_COMP_GNUC
+   // According to https://gcc.gnu.org/onlinedocs/gcc/Constraints.html:
+//   X: Any operand whatsoever.
+//   r: A register operand is allowed provided that it is in a general
+//      register.
+//   g: Any register, memory or immediate integer operand is allowed, except
+//      for registers that are not general registers.
+//   w: (AArch32/AArch64) Floating point register, Advanced SIMD vector
+//      register or SVE vector register.
+//   x: (SSE) Any SSE register.
+//      (AArch64) Like w, but restricted to registers 0 to 15 inclusive.
+//   v: (PowerPC) An Altivec vector register.
+//   wa:(PowerPC) A VSX register.
+//
+// "X" (uppercase) should work for all cases, though this seems to fail for
+// some versions of GCC for arm/aarch64 with
+//   "error: inconsistent operand constraints in an 'asm'"
+// Clang x86_64/arm/aarch64 seems to require "g" to support both scalars and
+// vectors, otherwise
+//   "error: non-trivial scalar-to-vector conversion, possible invalid
+//    constraint for vector type"
+//
+// GCC for ppc64le generates an internal compiler error with x/X/g.
+// GCC for AVX generates an internal compiler error with X.
+//
+// Tested on icc/gcc/clang for sse, avx, avx2, avx512dq
+//           gcc for arm, aarch64,
+//           gcc for ppc64le,
+// both vectors and scalars.
+//
+// Note that this is restricted to plain types - this will not work
+// directly for std::complex<T>, Eigen::half, Eigen::bfloat16. For these,
+// you will need to apply to the underlying POD type.
+#if EIGEN_ARCH_PPC && EIGEN_COMP_GNUC_STRICT
+   // This seems to be broken on clang. Packet4f is loaded into a single
+//   register rather than a vector, zeroing out some entries. Integer
+//   types also generate a compile error.
+#if EIGEN_OS_MAC
+   // General, Altivec for Apple (VSX were added in ISA v2.06):
+#define EIGEN_OPTIMIZATION_BARRIER(X) __asm__("" : "+r,v"(X));
+#else
+   // General, Altivec, VSX otherwise:
+#define EIGEN_OPTIMIZATION_BARRIER(X) __asm__("" : "+r,v,wa"(X));
+#endif
+#elif EIGEN_ARCH_ARM_OR_ARM64
+#ifdef __ARM_FP
+   // General, VFP or NEON.
+// Clang doesn't like "r",
+//    error: non-trivial scalar-to-vector conversion, possible invalid
+//           constraint for vector typ
+#define EIGEN_OPTIMIZATION_BARRIER(X) __asm__("" : "+g,w"(X));
+#else
+   // Arm without VFP or NEON.
+// "w" constraint will not compile.
+#define EIGEN_OPTIMIZATION_BARRIER(X) __asm__("" : "+g"(X));
+#endif
+#elif EIGEN_ARCH_i386_OR_x86_64
+   // General, SSE.
+#define EIGEN_OPTIMIZATION_BARRIER(X) __asm__("" : "+g,x"(X));
+#else
+   // Not implemented for other architectures.
+#define EIGEN_OPTIMIZATION_BARRIER(X)
+#endif
+#else
+   // Not implemented for other compilers.
+#define EIGEN_OPTIMIZATION_BARRIER(X)
+#endif
 #endif
 
 #if EIGEN_COMP_MSVC
-  // NOTE MSVC often gives C4127 warnings with compiletime if statements. See bug 1362.
-  // This workaround is ugly, but it does the job.
-#  define EIGEN_CONST_CONDITIONAL(cond)  (void)0, cond
+// NOTE MSVC often gives C4127 warnings with compiletime if statements. See bug 1362.
+// This workaround is ugly, but it does the job.
+#define EIGEN_CONST_CONDITIONAL(cond) (void)0, cond
 #else
-#  define EIGEN_CONST_CONDITIONAL(cond)  cond
+#define EIGEN_CONST_CONDITIONAL(cond) cond
 #endif
 
 #ifdef EIGEN_DONT_USE_RESTRICT_KEYWORD
-  #define EIGEN_RESTRICT
+#define EIGEN_RESTRICT
 #endif
 #ifndef EIGEN_RESTRICT
-  #define EIGEN_RESTRICT __restrict
+#define EIGEN_RESTRICT __restrict
 #endif
 
-
 #ifndef EIGEN_DEFAULT_IO_FORMAT
 #ifdef EIGEN_MAKING_DOCS
 // format used in Eigen's documentation
@@ -1184,63 +1072,59 @@
 // just an empty macro !
 #define EIGEN_EMPTY
 
-
 // When compiling CUDA/HIP device code with NVCC or HIPCC
 // pull in math functions from the global namespace.
 // In host mode, and when device code is compiled with clang,
 // use the std versions.
 #if (defined(EIGEN_CUDA_ARCH) && defined(__NVCC__)) || defined(EIGEN_HIP_DEVICE_COMPILE)
-  #define EIGEN_USING_STD(FUNC) using ::FUNC;
+#define EIGEN_USING_STD(FUNC) using ::FUNC;
 #else
-  #define EIGEN_USING_STD(FUNC) using std::FUNC;
+#define EIGEN_USING_STD(FUNC) using std::FUNC;
 #endif
 
-#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC < 1900 || (EIGEN_COMP_MSVC == 1900 && EIGEN_COMP_NVCC))
-  // For older MSVC versions, as well as 1900 && CUDA 8, using the base operator is necessary,
-  //   otherwise we get duplicate definition errors
-  // For later MSVC versions, we require explicit operator= definition, otherwise we get
-  //   use of implicitly deleted operator errors.
-  // (cf Bugs 920, 1000, 1324, 2291)
-  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
-    using Base::operator =;
-#elif EIGEN_COMP_CLANG // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)
-  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
-    using Base::operator =; \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) { Base::operator=(other); return *this; } \
-    template <typename OtherDerived> \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase<OtherDerived>& other) { Base::operator=(other.derived()); return *this; }
+#if EIGEN_COMP_MSVC_STRICT && EIGEN_COMP_NVCC
+// Wwhen compiling with NVCC, using the base operator is necessary,
+//   otherwise we get duplicate definition errors
+// For later MSVC versions, we require explicit operator= definition, otherwise we get
+//   use of implicitly deleted operator errors.
+// (cf Bugs 920, 1000, 1324, 2291)
+#define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) using Base::operator=;
+#elif EIGEN_COMP_CLANG  // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)
+#define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)                                           \
+  using Base::operator=;                                                                           \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) {                 \
+    Base::operator=(other);                                                                        \
+    return *this;                                                                                  \
+  }                                                                                                \
+  template <typename OtherDerived>                                                                 \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase<OtherDerived>& other) { \
+    Base::operator=(other.derived());                                                              \
+    return *this;                                                                                  \
+  }
 #else
-  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
-    using Base::operator =; \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) \
-    { \
-      Base::operator=(other); \
-      return *this; \
-    }
+#define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)                           \
+  using Base::operator=;                                                           \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) { \
+    Base::operator=(other);                                                        \
+    return *this;                                                                  \
+  }
 #endif
 
-
 /**
  * \internal
  * \brief Macro to explicitly define the default copy constructor.
  * This is necessary, because the implicit definition is deprecated if the copy-assignment is overridden.
  */
-#if EIGEN_HAS_CXX11
-#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) CLASS(const CLASS&) = default;
-#else
-#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS)
-#endif
-
-
+#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default;
 
 /** \internal
  * \brief Macro to manually inherit assignment operators.
- * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined.
- * With C++11 or later this also default-implements the copy-constructor
+ * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is
+ * defined. With C++11 or later this also default-implements the copy-constructor
  */
-#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived)  \
-    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived)
+#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
+  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)  \
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived)
 
 /** \internal
  * \brief Macro to manually define default constructors and destructors.
@@ -1249,81 +1133,47 @@
  *
  * Hiding the default destructor lead to problems in C++03 mode together with boost::multiprecision
  */
-#if EIGEN_HAS_CXX11
-#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived)  \
-    Derived() = default; \
-    ~Derived() = default;
-#else
-#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived)  \
-    Derived() {}; \
-    /* ~Derived() {}; */
-#endif
-
-
-
-
+#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived) \
+  EIGEN_DEVICE_FUNC Derived() = default;                        \
+  EIGEN_DEVICE_FUNC ~Derived() = default;
 
 /**
-* Just a side note. Commenting within defines works only by documenting
-* behind the object (via '!<'). Comments cannot be multi-line and thus
-* we have these extra long lines. What is confusing doxygen over here is
-* that we use '\' and basically have a bunch of typedefs with their
-* documentation in a single line.
-**/
+ * Just a side note. Commenting within defines works only by documenting
+ * behind the object (via '!<'). Comments cannot be multi-line and thus
+ * we have these extra long lines. What is confusing doxygen over here is
+ * that we use '\' and basically have a bunch of typedefs with their
+ * documentation in a single line.
+ **/
 
-#define EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) \
-  typedef typename Eigen::internal::traits<Derived>::Scalar Scalar; /*!< \brief Numeric type, e.g. float, double, int or std::complex<float>. */ \
-  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; /*!< \brief The underlying numeric type for composed scalar types. \details In cases where Scalar is e.g. std::complex<T>, T were corresponding to RealScalar. */ \
-  typedef typename Base::CoeffReturnType CoeffReturnType; /*!< \brief The return type for coefficient access. \details Depending on whether the object allows direct coefficient access (e.g. for a MatrixXd), this type is either 'const Scalar&' or simply 'Scalar' for objects that do not allow direct coefficient access. */ \
-  typedef typename Eigen::internal::ref_selector<Derived>::type Nested; \
-  typedef typename Eigen::internal::traits<Derived>::StorageKind StorageKind; \
-  typedef typename Eigen::internal::traits<Derived>::StorageIndex StorageIndex; \
-  enum CompileTimeTraits \
-      { RowsAtCompileTime = Eigen::internal::traits<Derived>::RowsAtCompileTime, \
-        ColsAtCompileTime = Eigen::internal::traits<Derived>::ColsAtCompileTime, \
-        Flags = Eigen::internal::traits<Derived>::Flags, \
-        SizeAtCompileTime = Base::SizeAtCompileTime, \
-        MaxSizeAtCompileTime = Base::MaxSizeAtCompileTime, \
-        IsVectorAtCompileTime = Base::IsVectorAtCompileTime }; \
-  using Base::derived; \
+#define EIGEN_GENERIC_PUBLIC_INTERFACE(Derived)                                                                        \
+  typedef typename Eigen::internal::traits<Derived>::Scalar                                                            \
+      Scalar; /*!< \brief Numeric type, e.g. float, double, int or std::complex<float>. */                             \
+  typedef typename Eigen::NumTraits<Scalar>::Real                                                                      \
+      RealScalar; /*!< \brief The underlying numeric type for composed scalar types. \details In cases where Scalar is \
+                     e.g. std::complex<T>, T were corresponding to RealScalar. */                                      \
+  typedef typename Base::CoeffReturnType                                                                               \
+      CoeffReturnType; /*!< \brief The return type for coefficient access. \details Depending on whether the object    \
+                          allows direct coefficient access (e.g. for a MatrixXd), this type is either 'const Scalar&'  \
+                          or simply 'Scalar' for objects that do not allow direct coefficient access. */               \
+  typedef typename Eigen::internal::ref_selector<Derived>::type Nested;                                                \
+  typedef typename Eigen::internal::traits<Derived>::StorageKind StorageKind;                                          \
+  typedef typename Eigen::internal::traits<Derived>::StorageIndex StorageIndex;                                        \
+  enum CompileTimeTraits {                                                                                             \
+    RowsAtCompileTime = Eigen::internal::traits<Derived>::RowsAtCompileTime,                                           \
+    ColsAtCompileTime = Eigen::internal::traits<Derived>::ColsAtCompileTime,                                           \
+    Flags = Eigen::internal::traits<Derived>::Flags,                                                                   \
+    SizeAtCompileTime = Base::SizeAtCompileTime,                                                                       \
+    MaxSizeAtCompileTime = Base::MaxSizeAtCompileTime,                                                                 \
+    IsVectorAtCompileTime = Base::IsVectorAtCompileTime                                                                \
+  };                                                                                                                   \
+  using Base::derived;                                                                                                 \
   using Base::const_cast_derived;
 
-
 // FIXME Maybe the EIGEN_DENSE_PUBLIC_INTERFACE could be removed as importing PacketScalar is rarely needed
 #define EIGEN_DENSE_PUBLIC_INTERFACE(Derived) \
-  EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) \
+  EIGEN_GENERIC_PUBLIC_INTERFACE(Derived)     \
   typedef typename Base::PacketScalar PacketScalar;
 
-
-#define EIGEN_PLAIN_ENUM_MIN(a,b) (((int)a <= (int)b) ? (int)a : (int)b)
-#define EIGEN_PLAIN_ENUM_MAX(a,b) (((int)a >= (int)b) ? (int)a : (int)b)
-
-// EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
-// followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
-// finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
-#define EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) (((int)a == 0 || (int)b == 0) ? 0 \
-                           : ((int)a == 1 || (int)b == 1) ? 1 \
-                           : ((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \
-                           : ((int)a <= (int)b) ? (int)a : (int)b)
-
-// EIGEN_SIZE_MIN_PREFER_FIXED is a variant of EIGEN_SIZE_MIN_PREFER_DYNAMIC comparing MaxSizes. The difference is that finite values
-// now have priority over Dynamic, so that min(3, Dynamic) gives 3. Indeed, whatever the actual value is
-// (between 0 and 3), it is not more than 3.
-#define EIGEN_SIZE_MIN_PREFER_FIXED(a,b)  (((int)a == 0 || (int)b == 0) ? 0 \
-                           : ((int)a == 1 || (int)b == 1) ? 1 \
-                           : ((int)a == Dynamic && (int)b == Dynamic) ? Dynamic \
-                           : ((int)a == Dynamic) ? (int)b \
-                           : ((int)b == Dynamic) ? (int)a \
-                           : ((int)a <= (int)b) ? (int)a : (int)b)
-
-// see EIGEN_SIZE_MIN_PREFER_DYNAMIC. No need for a separate variant for MaxSizes here.
-#define EIGEN_SIZE_MAX(a,b) (((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \
-                           : ((int)a >= (int)b) ? (int)a : (int)b)
-
-#define EIGEN_LOGICAL_XOR(a,b) (((a) || (b)) && !((a) && (b)))
-
-#define EIGEN_IMPLIES(a,b) (!(a) || (b))
-
 #if EIGEN_HAS_BUILTIN(__builtin_expect) || EIGEN_COMP_GNUC
 #define EIGEN_PREDICT_FALSE(x) (__builtin_expect(x, false))
 #define EIGEN_PREDICT_TRUE(x) (__builtin_expect(false || (x), true))
@@ -1333,142 +1183,129 @@
 #endif
 
 // the expression type of a standard coefficient wise binary operation
-#define EIGEN_CWISE_BINARY_RETURN_TYPE(LHS,RHS,OPNAME) \
-    CwiseBinaryOp< \
-      EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)< \
-          typename internal::traits<LHS>::Scalar, \
-          typename internal::traits<RHS>::Scalar \
-      >, \
-      const LHS, \
-      const RHS \
-    >
+#define EIGEN_CWISE_BINARY_RETURN_TYPE(LHS, RHS, OPNAME)                                                       \
+  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_, OPNAME), _op) < typename internal::traits<LHS>::Scalar, \
+                typename internal::traits<RHS>::Scalar>,                                                       \
+      const LHS, const RHS >
 
-#define EIGEN_MAKE_CWISE_BINARY_OP(METHOD,OPNAME) \
-  template<typename OtherDerived> \
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,OPNAME) \
-  (METHOD)(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const \
-  { \
-    return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,OPNAME)(derived(), other.derived()); \
+#define EIGEN_MAKE_CWISE_BINARY_OP(METHOD, OPNAME)                                                                \
+  template <typename OtherDerived>                                                                                \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(                                     \
+      Derived, OtherDerived, OPNAME)(METHOD)(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const { \
+    return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived, OtherDerived, OPNAME)(derived(), other.derived());             \
   }
 
-#define EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,TYPEA,TYPEB) \
-  (Eigen::internal::has_ReturnType<Eigen::ScalarBinaryOpTraits<TYPEA,TYPEB,EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,OPNAME),_op)<TYPEA,TYPEB>  > >::value)
+#define EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME, TYPEA, TYPEB)     \
+  (Eigen::internal::has_ReturnType<Eigen::ScalarBinaryOpTraits< \
+       TYPEA, TYPEB, EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, OPNAME), _op) < TYPEA, TYPEB> > > ::value)
 
-#define EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(EXPR,SCALAR,OPNAME) \
-  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)<typename internal::traits<EXPR>::Scalar,SCALAR>, const EXPR, \
-                const typename internal::plain_constant_type<EXPR,SCALAR>::type>
+#define EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(EXPR, SCALAR, OPNAME)                                            \
+  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_, OPNAME), _op) < typename internal::traits<EXPR>::Scalar, \
+                SCALAR>,                                                                                        \
+      const EXPR, const typename internal::plain_constant_type<EXPR, SCALAR>::type >
 
-#define EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(SCALAR,EXPR,OPNAME) \
-  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)<SCALAR,typename internal::traits<EXPR>::Scalar>, \
-                const typename internal::plain_constant_type<EXPR,SCALAR>::type, const EXPR>
+#define EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(SCALAR, EXPR, OPNAME)           \
+  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_, OPNAME), _op) < SCALAR, \
+                typename internal::traits<EXPR>::Scalar>,                      \
+      const typename internal::plain_constant_type<EXPR, SCALAR>::type, const EXPR >
 
-// Workaround for MSVC 2010 (see ML thread "patch with compile for for MSVC 2010")
-#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC_STRICT<=1600)
-#define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) typename internal::enable_if<true,X>::type
-#else
-#define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) X
+#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD, OPNAME)                                                       \
+  template <typename T>                                                                                              \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(                                \
+      Derived,                                                                                                       \
+      typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(          \
+          OPNAME, Scalar, T)>::type,                                                                                 \
+      OPNAME)(METHOD)(const T& scalar) const {                                                                       \
+    typedef typename internal::promote_scalar_arg<Scalar, T, EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME, Scalar, T)>::type \
+        PromotedT;                                                                                                   \
+    return EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived, PromotedT, OPNAME)(                                       \
+        derived(), typename internal::plain_constant_type<Derived, PromotedT>::type(                                 \
+                       derived().rows(), derived().cols(), internal::scalar_constant_op<PromotedT>(scalar)));        \
+  }
+
+#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD, OPNAME)                                                        \
+  template <typename T>                                                                                              \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE friend const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(                         \
+      typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(          \
+          OPNAME, T, Scalar)>::type,                                                                                 \
+      Derived, OPNAME)(METHOD)(const T& scalar, const StorageBaseType& matrix) {                                     \
+    typedef typename internal::promote_scalar_arg<Scalar, T, EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME, T, Scalar)>::type \
+        PromotedT;                                                                                                   \
+    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedT, Derived, OPNAME)(                                       \
+        typename internal::plain_constant_type<Derived, PromotedT>::type(                                            \
+            matrix.derived().rows(), matrix.derived().cols(), internal::scalar_constant_op<PromotedT>(scalar)),      \
+        matrix.derived());                                                                                           \
+  }
+
+#define EIGEN_MAKE_SCALAR_BINARY_OP(METHOD, OPNAME)     \
+  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD, OPNAME) \
+  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD, OPNAME)
+
+#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_CUDA_ARCH) && !defined(EIGEN_EXCEPTIONS) && \
+    !defined(EIGEN_USE_SYCL) && !defined(EIGEN_HIP_DEVICE_COMPILE)
+#define EIGEN_EXCEPTIONS
 #endif
 
-#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME) \
-  template <typename T> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE \
-  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type,OPNAME))\
-  (METHOD)(const T& scalar) const { \
-    typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type PromotedT; \
-    return EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,PromotedT,OPNAME)(derived(), \
-           typename internal::plain_constant_type<Derived,PromotedT>::type(derived().rows(), derived().cols(), internal::scalar_constant_op<PromotedT>(scalar))); \
-  }
-
-#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD,OPNAME) \
-  template <typename T> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE friend \
-  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type,Derived,OPNAME)) \
-  (METHOD)(const T& scalar, const StorageBaseType& matrix) { \
-    typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type PromotedT; \
-    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedT,Derived,OPNAME)( \
-           typename internal::plain_constant_type<Derived,PromotedT>::type(matrix.derived().rows(), matrix.derived().cols(), internal::scalar_constant_op<PromotedT>(scalar)), matrix.derived()); \
-  }
-
-#define EIGEN_MAKE_SCALAR_BINARY_OP(METHOD,OPNAME) \
-  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD,OPNAME) \
-  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME)
-
-
-#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_CUDA_ARCH) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL) && !defined(EIGEN_HIP_DEVICE_COMPILE)
-  #define EIGEN_EXCEPTIONS
-#endif
-
-
 #ifdef EIGEN_EXCEPTIONS
-#  define EIGEN_THROW_X(X) throw X
-#  define EIGEN_THROW throw
-#  define EIGEN_TRY try
-#  define EIGEN_CATCH(X) catch (X)
+#define EIGEN_THROW_X(X) throw X
+#define EIGEN_THROW throw
+#define EIGEN_TRY try
+#define EIGEN_CATCH(X) catch (X)
 #else
-#  if defined(EIGEN_CUDA_ARCH)
-#    define EIGEN_THROW_X(X) asm("trap;")
-#    define EIGEN_THROW asm("trap;")
-#  elif defined(EIGEN_HIP_DEVICE_COMPILE)
-#    define EIGEN_THROW_X(X) asm("s_trap 0")
-#    define EIGEN_THROW asm("s_trap 0")
-#  else
-#    define EIGEN_THROW_X(X) std::abort()
-#    define EIGEN_THROW std::abort()
-#  endif
-#  define EIGEN_TRY if (true)
-#  define EIGEN_CATCH(X) else
+#if defined(EIGEN_CUDA_ARCH)
+#define EIGEN_THROW_X(X) asm("trap;")
+#define EIGEN_THROW asm("trap;")
+#elif defined(EIGEN_HIP_DEVICE_COMPILE)
+#define EIGEN_THROW_X(X) asm("s_trap 0")
+#define EIGEN_THROW asm("s_trap 0")
+#else
+#define EIGEN_THROW_X(X) std::abort()
+#define EIGEN_THROW std::abort()
+#endif
+#define EIGEN_TRY if (true)
+#define EIGEN_CATCH(X) else
 #endif
 
+#define EIGEN_NOEXCEPT noexcept
+#define EIGEN_NOEXCEPT_IF(x) noexcept(x)
+#define EIGEN_NO_THROW noexcept(true)
+#define EIGEN_EXCEPTION_SPEC(X) noexcept(false)
 
-#if EIGEN_HAS_CXX11_NOEXCEPT
-#   define EIGEN_INCLUDE_TYPE_TRAITS
-#   define EIGEN_NOEXCEPT noexcept
-#   define EIGEN_NOEXCEPT_IF(x) noexcept(x)
-#   define EIGEN_NO_THROW noexcept(true)
-#   define EIGEN_EXCEPTION_SPEC(X) noexcept(false)
-#else
-#   define EIGEN_NOEXCEPT
-#   define EIGEN_NOEXCEPT_IF(x)
-#   define EIGEN_NO_THROW throw()
-#   if EIGEN_COMP_MSVC || EIGEN_COMP_CXXVER>=17
-      // MSVC does not support exception specifications (warning C4290),
-      // and they are deprecated in c++11 anyway. This is even an error in c++17.
-#     define EIGEN_EXCEPTION_SPEC(X) throw()
-#   else
-#     define EIGEN_EXCEPTION_SPEC(X) throw(X)
-#   endif
-#endif
-
-#if EIGEN_HAS_VARIADIC_TEMPLATES
 // The all function is used to enable a variadic version of eigen_assert which can take a parameter pack as its input.
 namespace Eigen {
 namespace internal {
 
-inline bool all(){ return true; }
+EIGEN_DEVICE_FUNC inline bool all() { return true; }
 
-template<typename T, typename ...Ts>
-bool all(T t, Ts ... ts){ return t && all(ts...); }
-
+template <typename T, typename... Ts>
+EIGEN_DEVICE_FUNC bool all(T t, Ts... ts) {
+  return t && all(ts...);
 }
-}
-#endif
 
-#if EIGEN_HAS_CXX11_OVERRIDE_FINAL
+}  // namespace internal
+}  // namespace Eigen
+
 // provide override and final specifiers if they are available:
-#   define EIGEN_OVERRIDE override
-#   define EIGEN_FINAL final
-#else
-#   define EIGEN_OVERRIDE
-#   define EIGEN_FINAL
-#endif
+#define EIGEN_OVERRIDE override
+#define EIGEN_FINAL final
 
 // Wrapping #pragma unroll in a macro since it is required for SYCL
 #if defined(SYCL_DEVICE_ONLY)
-  #if defined(_MSC_VER)
-    #define EIGEN_UNROLL_LOOP __pragma(unroll)
-  #else
-    #define EIGEN_UNROLL_LOOP _Pragma("unroll")
-  #endif
+#if defined(_MSC_VER)
+#define EIGEN_UNROLL_LOOP __pragma(unroll)
 #else
-  #define EIGEN_UNROLL_LOOP
+#define EIGEN_UNROLL_LOOP _Pragma("unroll")
+#endif
+#else
+#define EIGEN_UNROLL_LOOP
 #endif
 
-#endif // EIGEN_MACROS_H
+// Notice: Use this macro with caution. The code in the if body should still
+// compile with C++14.
+#if defined(EIGEN_HAS_CXX17_IFCONSTEXPR)
+#define EIGEN_IF_CONSTEXPR(X) if constexpr (X)
+#else
+#define EIGEN_IF_CONSTEXPR(X) if (X)
+#endif
+
+#endif  // EIGEN_MACROS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h
new file mode 100644
index 0000000..2f1e3d3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h
@@ -0,0 +1,139 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_FIXEDSIZEVECTOR_H
+#define EIGEN_FIXEDSIZEVECTOR_H
+
+namespace Eigen {
+
+/** \class MaxSizeVector
+ * \ingroup Core
+ *
+ * \brief The MaxSizeVector class.
+ *
+ * The %MaxSizeVector provides a subset of std::vector functionality.
+ *
+ * The goal is to provide basic std::vector operations when using
+ * std::vector is not an option (e.g. on GPU or when compiling using
+ * FMA/AVX, as this can cause either compilation failures or illegal
+ * instruction failures).
+ *
+ * Beware: The constructors are not API compatible with these of
+ * std::vector.
+ */
+template <typename T>
+class MaxSizeVector {
+  static const size_t alignment = internal::plain_enum_max(EIGEN_ALIGNOF(T), sizeof(void*));
+
+ public:
+  // Construct a new MaxSizeVector, reserve n elements.
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit MaxSizeVector(size_t n)
+      : reserve_(n), size_(0), data_(static_cast<T*>(internal::handmade_aligned_malloc(n * sizeof(T), alignment))) {}
+
+  // Construct a new MaxSizeVector, reserve and resize to n.
+  // Copy the init value to all elements.
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE MaxSizeVector(size_t n, const T& init)
+      : reserve_(n), size_(n), data_(static_cast<T*>(internal::handmade_aligned_malloc(n * sizeof(T), alignment))) {
+    size_t i = 0;
+    EIGEN_TRY {
+      for (; i < size_; ++i) {
+        new (&data_[i]) T(init);
+      }
+    }
+    EIGEN_CATCH(...) {
+      // Construction failed, destruct in reverse order:
+      for (; (i + 1) > 0; --i) {
+        data_[i - 1].~T();
+      }
+      internal::handmade_aligned_free(data_);
+      EIGEN_THROW;
+    }
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ~MaxSizeVector() {
+    for (size_t i = size_; i > 0; --i) {
+      data_[i - 1].~T();
+    }
+    internal::handmade_aligned_free(data_);
+  }
+
+  void resize(size_t n) {
+    eigen_assert(n <= reserve_);
+    for (; size_ < n; ++size_) {
+      new (&data_[size_]) T;
+    }
+    for (; size_ > n; --size_) {
+      data_[size_ - 1].~T();
+    }
+    eigen_assert(size_ == n);
+  }
+
+  // Append new elements (up to reserved size).
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void push_back(const T& t) {
+    eigen_assert(size_ < reserve_);
+    new (&data_[size_++]) T(t);
+  }
+
+  // For C++03 compatibility this only takes one argument
+  template <class X>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void emplace_back(const X& x) {
+    eigen_assert(size_ < reserve_);
+    new (&data_[size_++]) T(x);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& operator[](size_t i) const {
+    eigen_assert(i < size_);
+    return data_[i];
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& operator[](size_t i) {
+    eigen_assert(i < size_);
+    return data_[i];
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T& back() {
+    eigen_assert(size_ > 0);
+    return data_[size_ - 1];
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& back() const {
+    eigen_assert(size_ > 0);
+    return data_[size_ - 1];
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pop_back() {
+    eigen_assert(size_ > 0);
+    data_[--size_].~T();
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t size() const { return size_; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool empty() const { return size_ == 0; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T* data() { return data_; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T* data() const { return data_; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T* begin() { return data_; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T* end() { return data_ + size_; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T* begin() const { return data_; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T* end() const { return data_ + size_; }
+
+ private:
+  size_t reserve_;
+  size_t size_;
+  T* data_;
+};
+
+}  // namespace Eigen
+
+#endif  // EIGEN_FIXEDSIZEVECTOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h
index 875318c..31f1057 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h
@@ -12,7 +12,6 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 /*****************************************************************************
 *** Platform checks for aligned malloc functions                           ***
 *****************************************************************************/
@@ -31,11 +30,11 @@
 //   http://gcc.fyxm.net/summit/2003/Porting%20to%2064%20bit.pdf
 // page 114, "[The] LP64 model [...] is used by all 64-bit UNIX ports" so it's indeed
 // quite safe, at least within the context of glibc, to equate 64-bit with LP64.
-#if defined(__GLIBC__) && ((__GLIBC__>=2 && __GLIBC_MINOR__ >= 8) || __GLIBC__>2) \
- && defined(__LP64__) && ! defined( __SANITIZE_ADDRESS__ ) && (EIGEN_DEFAULT_ALIGN_BYTES == 16)
-  #define EIGEN_GLIBC_MALLOC_ALREADY_ALIGNED 1
+#if defined(__GLIBC__) && ((__GLIBC__ >= 2 && __GLIBC_MINOR__ >= 8) || __GLIBC__ > 2) && defined(__LP64__) && \
+    !defined(__SANITIZE_ADDRESS__) && (EIGEN_DEFAULT_ALIGN_BYTES == 16)
+#define EIGEN_GLIBC_MALLOC_ALREADY_ALIGNED 1
 #else
-  #define EIGEN_GLIBC_MALLOC_ALREADY_ALIGNED 0
+#define EIGEN_GLIBC_MALLOC_ALREADY_ALIGNED 0
 #endif
 
 // FreeBSD 6 seems to have 16-byte aligned malloc
@@ -43,49 +42,92 @@
 // FreeBSD 7 seems to have 16-byte aligned malloc except on ARM and MIPS architectures
 //   See http://svn.freebsd.org/viewvc/base/stable/7/lib/libc/stdlib/malloc.c?view=markup
 #if defined(__FreeBSD__) && !(EIGEN_ARCH_ARM || EIGEN_ARCH_MIPS) && (EIGEN_DEFAULT_ALIGN_BYTES == 16)
-  #define EIGEN_FREEBSD_MALLOC_ALREADY_ALIGNED 1
+#define EIGEN_FREEBSD_MALLOC_ALREADY_ALIGNED 1
 #else
-  #define EIGEN_FREEBSD_MALLOC_ALREADY_ALIGNED 0
+#define EIGEN_FREEBSD_MALLOC_ALREADY_ALIGNED 0
 #endif
 
-#if (EIGEN_OS_MAC && (EIGEN_DEFAULT_ALIGN_BYTES == 16))     \
- || (EIGEN_OS_WIN64 && (EIGEN_DEFAULT_ALIGN_BYTES == 16))   \
- || EIGEN_GLIBC_MALLOC_ALREADY_ALIGNED              \
- || EIGEN_FREEBSD_MALLOC_ALREADY_ALIGNED
-  #define EIGEN_MALLOC_ALREADY_ALIGNED 1
+#if (EIGEN_OS_MAC && (EIGEN_DEFAULT_ALIGN_BYTES == 16)) || (EIGEN_OS_WIN64 && (EIGEN_DEFAULT_ALIGN_BYTES == 16)) || \
+    EIGEN_GLIBC_MALLOC_ALREADY_ALIGNED || EIGEN_FREEBSD_MALLOC_ALREADY_ALIGNED
+#define EIGEN_MALLOC_ALREADY_ALIGNED 1
 #else
-  #define EIGEN_MALLOC_ALREADY_ALIGNED 0
+#define EIGEN_MALLOC_ALREADY_ALIGNED 0
 #endif
 
 #endif
 
+#ifndef EIGEN_MALLOC_CHECK_THREAD_LOCAL
+
+// Check whether we can use the thread_local keyword to allow or disallow
+// allocating memory with per-thread granularity, by means of the
+// set_is_malloc_allowed() function.
+#ifndef EIGEN_AVOID_THREAD_LOCAL
+
+#if ((EIGEN_COMP_GNUC) || __has_feature(cxx_thread_local) || EIGEN_COMP_MSVC >= 1900) && \
+    !defined(EIGEN_GPU_COMPILE_PHASE)
+#define EIGEN_MALLOC_CHECK_THREAD_LOCAL thread_local
+#else
+#define EIGEN_MALLOC_CHECK_THREAD_LOCAL
+#endif
+
+#else  // EIGEN_AVOID_THREAD_LOCAL
+#define EIGEN_MALLOC_CHECK_THREAD_LOCAL
+#endif  // EIGEN_AVOID_THREAD_LOCAL
+
+#endif
+
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-EIGEN_DEVICE_FUNC
-inline void throw_std_bad_alloc()
-{
-  #ifdef EIGEN_EXCEPTIONS
-    throw std::bad_alloc();
-  #else
-    std::size_t huge = static_cast<std::size_t>(-1);
-    #if defined(EIGEN_HIPCC)
-    //
-    // calls to "::operator new" are to be treated as opaque function calls (i.e no inlining),
-    // and as a consequence the code in the #else block triggers the hipcc warning :
-    // "no overloaded function has restriction specifiers that are compatible with the ambient context"
-    //
-    // "throw_std_bad_alloc" has the EIGEN_DEVICE_FUNC attribute, so it seems that hipcc expects
-    // the same on "operator new"
-    // Reverting code back to the old version in this #if block for the hipcc compiler
-    //
-    new int[huge];
-    #else
-    void* unused = ::operator new(huge);
-    EIGEN_UNUSED_VARIABLE(unused);
-    #endif
-  #endif
+/*****************************************************************************
+*** Implementation of portable aligned versions of malloc/free/realloc     ***
+*****************************************************************************/
+
+#ifdef EIGEN_NO_MALLOC
+EIGEN_DEVICE_FUNC inline void check_that_malloc_is_allowed() {
+  eigen_assert(false && "heap allocation is forbidden (EIGEN_NO_MALLOC is defined)");
+}
+#elif defined EIGEN_RUNTIME_NO_MALLOC
+EIGEN_DEVICE_FUNC inline bool is_malloc_allowed_impl(bool update, bool new_value = false) {
+  EIGEN_MALLOC_CHECK_THREAD_LOCAL static bool value = true;
+  if (update == 1) value = new_value;
+  return value;
+}
+EIGEN_DEVICE_FUNC inline bool is_malloc_allowed() { return is_malloc_allowed_impl(false); }
+EIGEN_DEVICE_FUNC inline bool set_is_malloc_allowed(bool new_value) { return is_malloc_allowed_impl(true, new_value); }
+EIGEN_DEVICE_FUNC inline void check_that_malloc_is_allowed() {
+  eigen_assert(is_malloc_allowed() &&
+               "heap allocation is forbidden (EIGEN_RUNTIME_NO_MALLOC is defined and g_is_malloc_allowed is false)");
+}
+#else
+EIGEN_DEVICE_FUNC inline void check_that_malloc_is_allowed() {}
+#endif
+
+EIGEN_DEVICE_FUNC inline void throw_std_bad_alloc() {
+#ifdef EIGEN_EXCEPTIONS
+  throw std::bad_alloc();
+#else
+  std::size_t huge = static_cast<std::size_t>(-1);
+#if defined(EIGEN_HIPCC)
+  //
+  // calls to "::operator new" are to be treated as opaque function calls (i.e no inlining),
+  // and as a consequence the code in the #else block triggers the hipcc warning :
+  // "no overloaded function has restriction specifiers that are compatible with the ambient context"
+  //
+  // "throw_std_bad_alloc" has the EIGEN_DEVICE_FUNC attribute, so it seems that hipcc expects
+  // the same on "operator new"
+  // Reverting code back to the old version in this #if block for the hipcc compiler
+  //
+  new int[huge];
+#else
+  void* unused = ::operator new(huge);
+  EIGEN_UNUSED_VARIABLE(unused);
+#endif
+#endif
 }
 
 /*****************************************************************************
@@ -94,137 +136,128 @@
 
 /* ----- Hand made implementations of aligned malloc/free and realloc ----- */
 
-/** \internal Like malloc, but the returned pointer is guaranteed to be 16-byte aligned.
-  * Fast, but wastes 16 additional bytes of memory. Does not throw any exception.
-  */
-EIGEN_DEVICE_FUNC inline void* handmade_aligned_malloc(std::size_t size, std::size_t alignment = EIGEN_DEFAULT_ALIGN_BYTES)
-{
-  eigen_assert(alignment >= sizeof(void*) && (alignment & (alignment-1)) == 0 && "Alignment must be at least sizeof(void*) and a power of 2");
+/** \internal Like malloc, but the returned pointer is guaranteed to be aligned to `alignment`.
+ * Fast, but wastes `alignment` additional bytes of memory. Does not throw any exception.
+ */
+EIGEN_DEVICE_FUNC inline void* handmade_aligned_malloc(std::size_t size,
+                                                       std::size_t alignment = EIGEN_DEFAULT_ALIGN_BYTES) {
+  eigen_assert(alignment >= sizeof(void*) && alignment <= 128 && (alignment & (alignment - 1)) == 0 &&
+               "Alignment must be at least sizeof(void*), less than or equal to 128, and a power of 2");
 
+  check_that_malloc_is_allowed();
   EIGEN_USING_STD(malloc)
-  void *original = malloc(size+alignment);
-  
+  void* original = malloc(size + alignment);
   if (original == 0) return 0;
-  void *aligned = reinterpret_cast<void*>((reinterpret_cast<std::size_t>(original) & ~(std::size_t(alignment-1))) + alignment);
-  *(reinterpret_cast<void**>(aligned) - 1) = original;
+  uint8_t offset = static_cast<uint8_t>(alignment - (reinterpret_cast<std::size_t>(original) & (alignment - 1)));
+  void* aligned = static_cast<void*>(static_cast<uint8_t*>(original) + offset);
+  *(static_cast<uint8_t*>(aligned) - 1) = offset;
   return aligned;
 }
 
 /** \internal Frees memory allocated with handmade_aligned_malloc */
-EIGEN_DEVICE_FUNC inline void handmade_aligned_free(void *ptr)
-{
+EIGEN_DEVICE_FUNC inline void handmade_aligned_free(void* ptr) {
   if (ptr) {
+    uint8_t offset = static_cast<uint8_t>(*(static_cast<uint8_t*>(ptr) - 1));
+    void* original = static_cast<void*>(static_cast<uint8_t*>(ptr) - offset);
+
+    check_that_malloc_is_allowed();
     EIGEN_USING_STD(free)
-    free(*(reinterpret_cast<void**>(ptr) - 1));
+    free(original);
   }
 }
 
 /** \internal
-  * \brief Reallocates aligned memory.
-  * Since we know that our handmade version is based on std::malloc
-  * we can use std::realloc to implement efficient reallocation.
-  */
-inline void* handmade_aligned_realloc(void* ptr, std::size_t size, std::size_t = 0)
-{
-  if (ptr == 0) return handmade_aligned_malloc(size);
-  void *original = *(reinterpret_cast<void**>(ptr) - 1);
-  std::ptrdiff_t previous_offset = static_cast<char *>(ptr)-static_cast<char *>(original);
-  original = std::realloc(original,size+EIGEN_DEFAULT_ALIGN_BYTES);
-  if (original == 0) return 0;
-  void *aligned = reinterpret_cast<void*>((reinterpret_cast<std::size_t>(original) & ~(std::size_t(EIGEN_DEFAULT_ALIGN_BYTES-1))) + EIGEN_DEFAULT_ALIGN_BYTES);
-  void *previous_aligned = static_cast<char *>(original)+previous_offset;
-  if(aligned!=previous_aligned)
-    std::memmove(aligned, previous_aligned, size);
+ * \brief Reallocates aligned memory.
+ * Since we know that our handmade version is based on std::malloc
+ * we can use std::realloc to implement efficient reallocation.
+ */
+EIGEN_DEVICE_FUNC inline void* handmade_aligned_realloc(void* ptr, std::size_t new_size, std::size_t old_size,
+                                                        std::size_t alignment = EIGEN_DEFAULT_ALIGN_BYTES) {
+  if (ptr == nullptr) return handmade_aligned_malloc(new_size, alignment);
+  uint8_t old_offset = *(static_cast<uint8_t*>(ptr) - 1);
+  void* old_original = static_cast<uint8_t*>(ptr) - old_offset;
 
-  *(reinterpret_cast<void**>(aligned) - 1) = original;
+  check_that_malloc_is_allowed();
+  EIGEN_USING_STD(realloc)
+  void* original = realloc(old_original, new_size + alignment);
+  if (original == nullptr) return nullptr;
+  if (original == old_original) return ptr;
+  uint8_t offset = static_cast<uint8_t>(alignment - (reinterpret_cast<std::size_t>(original) & (alignment - 1)));
+  void* aligned = static_cast<void*>(static_cast<uint8_t*>(original) + offset);
+  if (offset != old_offset) {
+    const void* src = static_cast<const void*>(static_cast<uint8_t*>(original) + old_offset);
+    std::size_t count = (std::min)(new_size, old_size);
+    std::memmove(aligned, src, count);
+  }
+  *(static_cast<uint8_t*>(aligned) - 1) = offset;
   return aligned;
 }
 
-/*****************************************************************************
-*** Implementation of portable aligned versions of malloc/free/realloc     ***
-*****************************************************************************/
+/** \internal Allocates \a size bytes. The returned pointer is guaranteed to have 16 or 32 bytes alignment depending on
+ * the requirements. On allocation error, the returned pointer is null, and std::bad_alloc is thrown.
+ */
+EIGEN_DEVICE_FUNC inline void* aligned_malloc(std::size_t size) {
+  if (size == 0) return nullptr;
 
-#ifdef EIGEN_NO_MALLOC
-EIGEN_DEVICE_FUNC inline void check_that_malloc_is_allowed()
-{
-  eigen_assert(false && "heap allocation is forbidden (EIGEN_NO_MALLOC is defined)");
-}
-#elif defined EIGEN_RUNTIME_NO_MALLOC
-EIGEN_DEVICE_FUNC inline bool is_malloc_allowed_impl(bool update, bool new_value = false)
-{
-  static bool value = true;
-  if (update == 1)
-    value = new_value;
-  return value;
-}
-EIGEN_DEVICE_FUNC inline bool is_malloc_allowed() { return is_malloc_allowed_impl(false); }
-EIGEN_DEVICE_FUNC inline bool set_is_malloc_allowed(bool new_value) { return is_malloc_allowed_impl(true, new_value); }
-EIGEN_DEVICE_FUNC inline void check_that_malloc_is_allowed()
-{
-  eigen_assert(is_malloc_allowed() && "heap allocation is forbidden (EIGEN_RUNTIME_NO_MALLOC is defined and g_is_malloc_allowed is false)");
-}
+  void* result;
+#if (EIGEN_DEFAULT_ALIGN_BYTES == 0) || EIGEN_MALLOC_ALREADY_ALIGNED
+
+  check_that_malloc_is_allowed();
+  EIGEN_USING_STD(malloc)
+  result = malloc(size);
+
+#if EIGEN_DEFAULT_ALIGN_BYTES == 16
+  eigen_assert((size < 16 || (std::size_t(result) % 16) == 0) &&
+               "System's malloc returned an unaligned pointer. Compile with EIGEN_MALLOC_ALREADY_ALIGNED=0 to fallback "
+               "to handmade aligned memory allocator.");
+#endif
 #else
-EIGEN_DEVICE_FUNC inline void check_that_malloc_is_allowed()
-{}
+  result = handmade_aligned_malloc(size);
 #endif
 
-/** \internal Allocates \a size bytes. The returned pointer is guaranteed to have 16 or 32 bytes alignment depending on the requirements.
-  * On allocation error, the returned pointer is null, and std::bad_alloc is thrown.
-  */
-EIGEN_DEVICE_FUNC inline void* aligned_malloc(std::size_t size)
-{
-  check_that_malloc_is_allowed();
-
-  void *result;
-  #if (EIGEN_DEFAULT_ALIGN_BYTES==0) || EIGEN_MALLOC_ALREADY_ALIGNED
-
-    EIGEN_USING_STD(malloc)
-    result = malloc(size);
-
-    #if EIGEN_DEFAULT_ALIGN_BYTES==16
-    eigen_assert((size<16 || (std::size_t(result)%16)==0) && "System's malloc returned an unaligned pointer. Compile with EIGEN_MALLOC_ALREADY_ALIGNED=0 to fallback to handmade aligned memory allocator.");
-    #endif
-  #else
-    result = handmade_aligned_malloc(size);
-  #endif
-
-  if(!result && size)
-    throw_std_bad_alloc();
+  if (!result && size) throw_std_bad_alloc();
 
   return result;
 }
 
 /** \internal Frees memory allocated with aligned_malloc. */
-EIGEN_DEVICE_FUNC inline void aligned_free(void *ptr)
-{
-  #if (EIGEN_DEFAULT_ALIGN_BYTES==0) || EIGEN_MALLOC_ALREADY_ALIGNED
+EIGEN_DEVICE_FUNC inline void aligned_free(void* ptr) {
+#if (EIGEN_DEFAULT_ALIGN_BYTES == 0) || EIGEN_MALLOC_ALREADY_ALIGNED
 
-    EIGEN_USING_STD(free)
-    free(ptr);
+  if (ptr) check_that_malloc_is_allowed();
+  EIGEN_USING_STD(free)
+  free(ptr);
 
-  #else
-    handmade_aligned_free(ptr);
-  #endif
+#else
+  handmade_aligned_free(ptr);
+#endif
 }
 
 /**
-  * \internal
-  * \brief Reallocates an aligned block of memory.
-  * \throws std::bad_alloc on allocation failure
-  */
-inline void* aligned_realloc(void *ptr, std::size_t new_size, std::size_t old_size)
-{
+ * \internal
+ * \brief Reallocates an aligned block of memory.
+ * \throws std::bad_alloc on allocation failure
+ */
+EIGEN_DEVICE_FUNC inline void* aligned_realloc(void* ptr, std::size_t new_size, std::size_t old_size) {
+  if (ptr == nullptr) return aligned_malloc(new_size);
+  if (old_size == new_size) return ptr;
+  if (new_size == 0) {
+    aligned_free(ptr);
+    return nullptr;
+  }
+
+  void* result;
+#if (EIGEN_DEFAULT_ALIGN_BYTES == 0) || EIGEN_MALLOC_ALREADY_ALIGNED
   EIGEN_UNUSED_VARIABLE(old_size)
 
-  void *result;
-#if (EIGEN_DEFAULT_ALIGN_BYTES==0) || EIGEN_MALLOC_ALREADY_ALIGNED
-  result = std::realloc(ptr,new_size);
+  check_that_malloc_is_allowed();
+  EIGEN_USING_STD(realloc)
+  result = realloc(ptr, new_size);
 #else
-  result = handmade_aligned_realloc(ptr,new_size,old_size);
+  result = handmade_aligned_realloc(ptr, new_size, old_size);
 #endif
 
-  if (!result && new_size)
-    throw_std_bad_alloc();
+  if (!result && new_size) throw_std_bad_alloc();
 
   return result;
 }
@@ -234,45 +267,56 @@
 *****************************************************************************/
 
 /** \internal Allocates \a size bytes. If Align is true, then the returned ptr is 16-byte-aligned.
-  * On allocation error, the returned pointer is null, and a std::bad_alloc is thrown.
-  */
-template<bool Align> EIGEN_DEVICE_FUNC inline void* conditional_aligned_malloc(std::size_t size)
-{
+ * On allocation error, the returned pointer is null, and a std::bad_alloc is thrown.
+ */
+template <bool Align>
+EIGEN_DEVICE_FUNC inline void* conditional_aligned_malloc(std::size_t size) {
   return aligned_malloc(size);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void* conditional_aligned_malloc<false>(std::size_t size)
-{
+template <>
+EIGEN_DEVICE_FUNC inline void* conditional_aligned_malloc<false>(std::size_t size) {
+  if (size == 0) return nullptr;
+
   check_that_malloc_is_allowed();
-
   EIGEN_USING_STD(malloc)
-  void *result = malloc(size);
+  void* result = malloc(size);
 
-  if(!result && size)
-    throw_std_bad_alloc();
+  if (!result && size) throw_std_bad_alloc();
   return result;
 }
 
 /** \internal Frees memory allocated with conditional_aligned_malloc */
-template<bool Align> EIGEN_DEVICE_FUNC inline void conditional_aligned_free(void *ptr)
-{
+template <bool Align>
+EIGEN_DEVICE_FUNC inline void conditional_aligned_free(void* ptr) {
   aligned_free(ptr);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void conditional_aligned_free<false>(void *ptr)
-{
+template <>
+EIGEN_DEVICE_FUNC inline void conditional_aligned_free<false>(void* ptr) {
+  if (ptr) check_that_malloc_is_allowed();
   EIGEN_USING_STD(free)
   free(ptr);
 }
 
-template<bool Align> inline void* conditional_aligned_realloc(void* ptr, std::size_t new_size, std::size_t old_size)
-{
+template <bool Align>
+EIGEN_DEVICE_FUNC inline void* conditional_aligned_realloc(void* ptr, std::size_t new_size, std::size_t old_size) {
   return aligned_realloc(ptr, new_size, old_size);
 }
 
-template<> inline void* conditional_aligned_realloc<false>(void* ptr, std::size_t new_size, std::size_t)
-{
-  return std::realloc(ptr, new_size);
+template <>
+EIGEN_DEVICE_FUNC inline void* conditional_aligned_realloc<false>(void* ptr, std::size_t new_size,
+                                                                  std::size_t old_size) {
+  if (ptr == nullptr) return conditional_aligned_malloc<false>(new_size);
+  if (old_size == new_size) return ptr;
+  if (new_size == 0) {
+    conditional_aligned_free<false>(ptr);
+    return nullptr;
+  }
+
+  check_that_malloc_is_allowed();
+  EIGEN_USING_STD(realloc)
+  return realloc(ptr, new_size);
 }
 
 /*****************************************************************************
@@ -280,75 +324,94 @@
 *****************************************************************************/
 
 /** \internal Destructs the elements of an array.
-  * The \a size parameters tells on how many objects to call the destructor of T.
-  */
-template<typename T> EIGEN_DEVICE_FUNC inline void destruct_elements_of_array(T *ptr, std::size_t size)
-{
+ * The \a size parameters tells on how many objects to call the destructor of T.
+ */
+template <typename T>
+EIGEN_DEVICE_FUNC inline void destruct_elements_of_array(T* ptr, std::size_t size) {
   // always destruct an array starting from the end.
-  if(ptr)
-    while(size) ptr[--size].~T();
+  if (ptr)
+    while (size) ptr[--size].~T();
 }
 
 /** \internal Constructs the elements of an array.
-  * The \a size parameter tells on how many objects to call the constructor of T.
-  */
-template<typename T> EIGEN_DEVICE_FUNC inline T* construct_elements_of_array(T *ptr, std::size_t size)
-{
-  std::size_t i;
-  EIGEN_TRY
-  {
-      for (i = 0; i < size; ++i) ::new (ptr + i) T;
-      return ptr;
+ * The \a size parameter tells on how many objects to call the constructor of T.
+ */
+template <typename T>
+EIGEN_DEVICE_FUNC inline T* default_construct_elements_of_array(T* ptr, std::size_t size) {
+  std::size_t i = 0;
+  EIGEN_TRY {
+    for (i = 0; i < size; ++i) ::new (ptr + i) T;
   }
-  EIGEN_CATCH(...)
-  {
+  EIGEN_CATCH(...) {
     destruct_elements_of_array(ptr, i);
     EIGEN_THROW;
   }
-  return NULL;
+  return ptr;
+}
+
+/** \internal Copy-constructs the elements of an array.
+ * The \a size parameter tells on how many objects to copy.
+ */
+template <typename T>
+EIGEN_DEVICE_FUNC inline T* copy_construct_elements_of_array(T* ptr, const T* src, std::size_t size) {
+  std::size_t i = 0;
+  EIGEN_TRY {
+    for (i = 0; i < size; ++i) ::new (ptr + i) T(*(src + i));
+  }
+  EIGEN_CATCH(...) {
+    destruct_elements_of_array(ptr, i);
+    EIGEN_THROW;
+  }
+  return ptr;
+}
+
+/** \internal Move-constructs the elements of an array.
+ * The \a size parameter tells on how many objects to move.
+ */
+template <typename T>
+EIGEN_DEVICE_FUNC inline T* move_construct_elements_of_array(T* ptr, T* src, std::size_t size) {
+  std::size_t i = 0;
+  EIGEN_TRY {
+    for (i = 0; i < size; ++i) ::new (ptr + i) T(std::move(*(src + i)));
+  }
+  EIGEN_CATCH(...) {
+    destruct_elements_of_array(ptr, i);
+    EIGEN_THROW;
+  }
+  return ptr;
 }
 
 /*****************************************************************************
 *** Implementation of aligned new/delete-like functions                    ***
 *****************************************************************************/
 
-template<typename T>
-EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void check_size_for_overflow(std::size_t size)
-{
-  if(size > std::size_t(-1) / sizeof(T))
-    throw_std_bad_alloc();
+template <typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void check_size_for_overflow(std::size_t size) {
+  if (size > std::size_t(-1) / sizeof(T)) throw_std_bad_alloc();
 }
 
 /** \internal Allocates \a size objects of type T. The returned pointer is guaranteed to have 16 bytes alignment.
-  * On allocation error, the returned pointer is undefined, but a std::bad_alloc is thrown.
-  * The default constructor of T is called.
-  */
-template<typename T> EIGEN_DEVICE_FUNC inline T* aligned_new(std::size_t size)
-{
+ * On allocation error, the returned pointer is undefined, but a std::bad_alloc is thrown.
+ * The default constructor of T is called.
+ */
+template <typename T>
+EIGEN_DEVICE_FUNC inline T* aligned_new(std::size_t size) {
   check_size_for_overflow<T>(size);
-  T *result = reinterpret_cast<T*>(aligned_malloc(sizeof(T)*size));
-  EIGEN_TRY
-  {
-    return construct_elements_of_array(result, size);
-  }
-  EIGEN_CATCH(...)
-  {
+  T* result = static_cast<T*>(aligned_malloc(sizeof(T) * size));
+  EIGEN_TRY { return default_construct_elements_of_array(result, size); }
+  EIGEN_CATCH(...) {
     aligned_free(result);
     EIGEN_THROW;
   }
   return result;
 }
 
-template<typename T, bool Align> EIGEN_DEVICE_FUNC inline T* conditional_aligned_new(std::size_t size)
-{
+template <typename T, bool Align>
+EIGEN_DEVICE_FUNC inline T* conditional_aligned_new(std::size_t size) {
   check_size_for_overflow<T>(size);
-  T *result = reinterpret_cast<T*>(conditional_aligned_malloc<Align>(sizeof(T)*size));
-  EIGEN_TRY
-  {
-    return construct_elements_of_array(result, size);
-  }
-  EIGEN_CATCH(...)
-  {
+  T* result = static_cast<T*>(conditional_aligned_malloc<Align>(sizeof(T) * size));
+  EIGEN_TRY { return default_construct_elements_of_array(result, size); }
+  EIGEN_CATCH(...) {
     conditional_aligned_free<Align>(result);
     EIGEN_THROW;
   }
@@ -356,60 +419,62 @@
 }
 
 /** \internal Deletes objects constructed with aligned_new
-  * The \a size parameters tells on how many objects to call the destructor of T.
-  */
-template<typename T> EIGEN_DEVICE_FUNC inline void aligned_delete(T *ptr, std::size_t size)
-{
+ * The \a size parameters tells on how many objects to call the destructor of T.
+ */
+template <typename T>
+EIGEN_DEVICE_FUNC inline void aligned_delete(T* ptr, std::size_t size) {
   destruct_elements_of_array<T>(ptr, size);
-  Eigen::internal::aligned_free(ptr);
+  aligned_free(ptr);
 }
 
 /** \internal Deletes objects constructed with conditional_aligned_new
-  * The \a size parameters tells on how many objects to call the destructor of T.
-  */
-template<typename T, bool Align> EIGEN_DEVICE_FUNC inline void conditional_aligned_delete(T *ptr, std::size_t size)
-{
+ * The \a size parameters tells on how many objects to call the destructor of T.
+ */
+template <typename T, bool Align>
+EIGEN_DEVICE_FUNC inline void conditional_aligned_delete(T* ptr, std::size_t size) {
   destruct_elements_of_array<T>(ptr, size);
   conditional_aligned_free<Align>(ptr);
 }
 
-template<typename T, bool Align> EIGEN_DEVICE_FUNC inline T* conditional_aligned_realloc_new(T* pts, std::size_t new_size, std::size_t old_size)
-{
+template <typename T, bool Align>
+EIGEN_DEVICE_FUNC inline T* conditional_aligned_realloc_new(T* pts, std::size_t new_size, std::size_t old_size) {
   check_size_for_overflow<T>(new_size);
   check_size_for_overflow<T>(old_size);
-  if(new_size < old_size)
-    destruct_elements_of_array(pts+new_size, old_size-new_size);
-  T *result = reinterpret_cast<T*>(conditional_aligned_realloc<Align>(reinterpret_cast<void*>(pts), sizeof(T)*new_size, sizeof(T)*old_size));
-  if(new_size > old_size)
-  {
-    EIGEN_TRY
-    {
-      construct_elements_of_array(result+old_size, new_size-old_size);
+
+  // If elements need to be explicitly initialized, we cannot simply realloc
+  // (or memcpy) the memory block - each element needs to be reconstructed.
+  // Otherwise, objects that contain internal pointers like mpfr or
+  // AnnoyingScalar can be pointing to the wrong thing.
+  T* result = static_cast<T*>(conditional_aligned_malloc<Align>(sizeof(T) * new_size));
+  EIGEN_TRY {
+    // Move-construct initial elements.
+    std::size_t copy_size = (std::min)(old_size, new_size);
+    move_construct_elements_of_array(result, pts, copy_size);
+
+    // Default-construct remaining elements.
+    if (new_size > old_size) {
+      default_construct_elements_of_array(result + copy_size, new_size - old_size);
     }
-    EIGEN_CATCH(...)
-    {
-      conditional_aligned_free<Align>(result);
-      EIGEN_THROW;
-    }
+
+    // Delete old elements.
+    conditional_aligned_delete<T, Align>(pts, old_size);
   }
+  EIGEN_CATCH(...) {
+    conditional_aligned_free<Align>(result);
+    EIGEN_THROW;
+  }
+
   return result;
 }
 
-
-template<typename T, bool Align> EIGEN_DEVICE_FUNC inline T* conditional_aligned_new_auto(std::size_t size)
-{
-  if(size==0)
-    return 0; // short-cut. Also fixes Bug 884
+template <typename T, bool Align>
+EIGEN_DEVICE_FUNC inline T* conditional_aligned_new_auto(std::size_t size) {
+  if (size == 0) return 0;  // short-cut. Also fixes Bug 884
   check_size_for_overflow<T>(size);
-  T *result = reinterpret_cast<T*>(conditional_aligned_malloc<Align>(sizeof(T)*size));
-  if(NumTraits<T>::RequireInitialization)
-  {
-    EIGEN_TRY
-    {
-      construct_elements_of_array(result, size);
-    }
-    EIGEN_CATCH(...)
-    {
+  T* result = static_cast<T*>(conditional_aligned_malloc<Align>(sizeof(T) * size));
+  if (NumTraits<T>::RequireInitialization) {
+    EIGEN_TRY { default_construct_elements_of_array(result, size); }
+    EIGEN_CATCH(...) {
       conditional_aligned_free<Align>(result);
       EIGEN_THROW;
     }
@@ -417,166 +482,140 @@
   return result;
 }
 
-template<typename T, bool Align> inline T* conditional_aligned_realloc_new_auto(T* pts, std::size_t new_size, std::size_t old_size)
-{
+template <typename T, bool Align>
+EIGEN_DEVICE_FUNC inline T* conditional_aligned_realloc_new_auto(T* pts, std::size_t new_size, std::size_t old_size) {
+  if (NumTraits<T>::RequireInitialization) {
+    return conditional_aligned_realloc_new<T, Align>(pts, new_size, old_size);
+  }
+
   check_size_for_overflow<T>(new_size);
   check_size_for_overflow<T>(old_size);
-  if(NumTraits<T>::RequireInitialization && (new_size < old_size))
-    destruct_elements_of_array(pts+new_size, old_size-new_size);
-  T *result = reinterpret_cast<T*>(conditional_aligned_realloc<Align>(reinterpret_cast<void*>(pts), sizeof(T)*new_size, sizeof(T)*old_size));
-  if(NumTraits<T>::RequireInitialization && (new_size > old_size))
-  {
-    EIGEN_TRY
-    {
-      construct_elements_of_array(result+old_size, new_size-old_size);
-    }
-    EIGEN_CATCH(...)
-    {
-      conditional_aligned_free<Align>(result);
-      EIGEN_THROW;
-    }
-  }
-  return result;
+  return static_cast<T*>(
+      conditional_aligned_realloc<Align>(static_cast<void*>(pts), sizeof(T) * new_size, sizeof(T) * old_size));
 }
 
-template<typename T, bool Align> EIGEN_DEVICE_FUNC inline void conditional_aligned_delete_auto(T *ptr, std::size_t size)
-{
-  if(NumTraits<T>::RequireInitialization)
-    destruct_elements_of_array<T>(ptr, size);
+template <typename T, bool Align>
+EIGEN_DEVICE_FUNC inline void conditional_aligned_delete_auto(T* ptr, std::size_t size) {
+  if (NumTraits<T>::RequireInitialization) destruct_elements_of_array<T>(ptr, size);
   conditional_aligned_free<Align>(ptr);
 }
 
 /****************************************************************************/
 
-/** \internal Returns the index of the first element of the array that is well aligned with respect to the requested \a Alignment.
-  *
-  * \tparam Alignment requested alignment in Bytes.
-  * \param array the address of the start of the array
-  * \param size the size of the array
-  *
-  * \note If no element of the array is well aligned or the requested alignment is not a multiple of a scalar,
-  * the size of the array is returned. For example with SSE, the requested alignment is typically 16-bytes. If
-  * packet size for the given scalar type is 1, then everything is considered well-aligned.
-  *
-  * \note Otherwise, if the Alignment is larger that the scalar size, we rely on the assumptions that sizeof(Scalar) is a
-  * power of 2. On the other hand, we do not assume that the array address is a multiple of sizeof(Scalar), as that fails for
-  * example with Scalar=double on certain 32-bit platforms, see bug #79.
-  *
-  * There is also the variant first_aligned(const MatrixBase&) defined in DenseCoeffsBase.h.
-  * \sa first_default_aligned()
-  */
-template<int Alignment, typename Scalar, typename Index>
-EIGEN_DEVICE_FUNC inline Index first_aligned(const Scalar* array, Index size)
-{
+/** \internal Returns the index of the first element of the array that is well aligned with respect to the requested \a
+ * Alignment.
+ *
+ * \tparam Alignment requested alignment in Bytes.
+ * \param array the address of the start of the array
+ * \param size the size of the array
+ *
+ * \note If no element of the array is well aligned or the requested alignment is not a multiple of a scalar,
+ * the size of the array is returned. For example with SSE, the requested alignment is typically 16-bytes. If
+ * packet size for the given scalar type is 1, then everything is considered well-aligned.
+ *
+ * \note Otherwise, if the Alignment is larger that the scalar size, we rely on the assumptions that sizeof(Scalar) is a
+ * power of 2. On the other hand, we do not assume that the array address is a multiple of sizeof(Scalar), as that fails
+ * for example with Scalar=double on certain 32-bit platforms, see bug #79.
+ *
+ * There is also the variant first_aligned(const MatrixBase&) defined in DenseCoeffsBase.h.
+ * \sa first_default_aligned()
+ */
+template <int Alignment, typename Scalar, typename Index>
+EIGEN_DEVICE_FUNC inline Index first_aligned(const Scalar* array, Index size) {
   const Index ScalarSize = sizeof(Scalar);
   const Index AlignmentSize = Alignment / ScalarSize;
-  const Index AlignmentMask = AlignmentSize-1;
+  const Index AlignmentMask = AlignmentSize - 1;
 
-  if(AlignmentSize<=1)
-  {
+  if (AlignmentSize <= 1) {
     // Either the requested alignment if smaller than a scalar, or it exactly match a 1 scalar
     // so that all elements of the array have the same alignment.
     return 0;
-  }
-  else if( (UIntPtr(array) & (sizeof(Scalar)-1)) || (Alignment%ScalarSize)!=0)
-  {
-    // The array is not aligned to the size of a single scalar, or the requested alignment is not a multiple of the scalar size.
-    // Consequently, no element of the array is well aligned.
+  } else if ((std::uintptr_t(array) & (sizeof(Scalar) - 1)) || (Alignment % ScalarSize) != 0) {
+    // The array is not aligned to the size of a single scalar, or the requested alignment is not a multiple of the
+    // scalar size. Consequently, no element of the array is well aligned.
     return size;
-  }
-  else
-  {
-    Index first = (AlignmentSize - (Index((UIntPtr(array)/sizeof(Scalar))) & AlignmentMask)) & AlignmentMask;
+  } else {
+    Index first = (AlignmentSize - (Index((std::uintptr_t(array) / sizeof(Scalar))) & AlignmentMask)) & AlignmentMask;
     return (first < size) ? first : size;
   }
 }
 
-/** \internal Returns the index of the first element of the array that is well aligned with respect the largest packet requirement.
-   * \sa first_aligned(Scalar*,Index) and first_default_aligned(DenseBase<Derived>) */
-template<typename Scalar, typename Index>
-EIGEN_DEVICE_FUNC inline Index first_default_aligned(const Scalar* array, Index size)
-{
+/** \internal Returns the index of the first element of the array that is well aligned with respect the largest packet
+ * requirement. \sa first_aligned(Scalar*,Index) and first_default_aligned(DenseBase<Derived>) */
+template <typename Scalar, typename Index>
+EIGEN_DEVICE_FUNC inline Index first_default_aligned(const Scalar* array, Index size) {
   typedef typename packet_traits<Scalar>::type DefaultPacketType;
   return first_aligned<unpacket_traits<DefaultPacketType>::alignment>(array, size);
 }
 
 /** \internal Returns the smallest integer multiple of \a base and greater or equal to \a size
-  */
-template<typename Index>
-inline Index first_multiple(Index size, Index base)
-{
-  return ((size+base-1)/base)*base;
+ */
+template <typename Index>
+inline Index first_multiple(Index size, Index base) {
+  return ((size + base - 1) / base) * base;
 }
 
 // std::copy is much slower than memcpy, so let's introduce a smart_copy which
 // use memcpy on trivial types, i.e., on types that does not require an initialization ctor.
-template<typename T, bool UseMemcpy> struct smart_copy_helper;
+template <typename T, bool UseMemcpy>
+struct smart_copy_helper;
 
-template<typename T> EIGEN_DEVICE_FUNC void smart_copy(const T* start, const T* end, T* target)
-{
-  smart_copy_helper<T,!NumTraits<T>::RequireInitialization>::run(start, end, target);
+template <typename T>
+EIGEN_DEVICE_FUNC void smart_copy(const T* start, const T* end, T* target) {
+  smart_copy_helper<T, !NumTraits<T>::RequireInitialization>::run(start, end, target);
 }
 
-template<typename T> struct smart_copy_helper<T,true> {
-  EIGEN_DEVICE_FUNC static inline void run(const T* start, const T* end, T* target)
-  {
-    IntPtr size = IntPtr(end)-IntPtr(start);
-    if(size==0) return;
-    eigen_internal_assert(start!=0 && end!=0 && target!=0);
+template <typename T>
+struct smart_copy_helper<T, true> {
+  EIGEN_DEVICE_FUNC static inline void run(const T* start, const T* end, T* target) {
+    std::intptr_t size = std::intptr_t(end) - std::intptr_t(start);
+    if (size == 0) return;
+    eigen_internal_assert(start != 0 && end != 0 && target != 0);
     EIGEN_USING_STD(memcpy)
     memcpy(target, start, size);
   }
 };
 
-template<typename T> struct smart_copy_helper<T,false> {
-  EIGEN_DEVICE_FUNC static inline void run(const T* start, const T* end, T* target)
-  { std::copy(start, end, target); }
+template <typename T>
+struct smart_copy_helper<T, false> {
+  EIGEN_DEVICE_FUNC static inline void run(const T* start, const T* end, T* target) { std::copy(start, end, target); }
 };
 
 // intelligent memmove. falls back to std::memmove for POD types, uses std::copy otherwise.
-template<typename T, bool UseMemmove> struct smart_memmove_helper;
+template <typename T, bool UseMemmove>
+struct smart_memmove_helper;
 
-template<typename T> void smart_memmove(const T* start, const T* end, T* target)
-{
-  smart_memmove_helper<T,!NumTraits<T>::RequireInitialization>::run(start, end, target);
+template <typename T>
+void smart_memmove(const T* start, const T* end, T* target) {
+  smart_memmove_helper<T, !NumTraits<T>::RequireInitialization>::run(start, end, target);
 }
 
-template<typename T> struct smart_memmove_helper<T,true> {
-  static inline void run(const T* start, const T* end, T* target)
-  {
-    IntPtr size = IntPtr(end)-IntPtr(start);
-    if(size==0) return;
-    eigen_internal_assert(start!=0 && end!=0 && target!=0);
+template <typename T>
+struct smart_memmove_helper<T, true> {
+  static inline void run(const T* start, const T* end, T* target) {
+    std::intptr_t size = std::intptr_t(end) - std::intptr_t(start);
+    if (size == 0) return;
+    eigen_internal_assert(start != 0 && end != 0 && target != 0);
     std::memmove(target, start, size);
   }
 };
 
-template<typename T> struct smart_memmove_helper<T,false> {
-  static inline void run(const T* start, const T* end, T* target)
-  {
-    if (UIntPtr(target) < UIntPtr(start))
-    {
+template <typename T>
+struct smart_memmove_helper<T, false> {
+  static inline void run(const T* start, const T* end, T* target) {
+    if (std::uintptr_t(target) < std::uintptr_t(start)) {
       std::copy(start, end, target);
-    }
-    else
-    {
-      std::ptrdiff_t count = (std::ptrdiff_t(end)-std::ptrdiff_t(start)) / sizeof(T);
+    } else {
+      std::ptrdiff_t count = (std::ptrdiff_t(end) - std::ptrdiff_t(start)) / sizeof(T);
       std::copy_backward(start, end, target + count);
     }
   }
 };
 
-#if EIGEN_HAS_RVALUE_REFERENCES
-template<typename T> EIGEN_DEVICE_FUNC T* smart_move(T* start, T* end, T* target)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC T* smart_move(T* start, T* end, T* target) {
   return std::move(start, end, target);
 }
-#else
-template<typename T> EIGEN_DEVICE_FUNC T* smart_move(T* start, T* end, T* target)
-{
-  return std::copy(start, end, target);
-}
-#endif
 
 /*****************************************************************************
 *** Implementation of runtime stack allocation (falling back to malloc)    ***
@@ -584,12 +623,12 @@
 
 // you can overwrite Eigen's default behavior regarding alloca by defining EIGEN_ALLOCA
 // to the appropriate stack allocation function
-#if ! defined EIGEN_ALLOCA && ! defined EIGEN_GPU_COMPILE_PHASE
-  #if EIGEN_OS_LINUX || EIGEN_OS_MAC || (defined alloca)
-    #define EIGEN_ALLOCA alloca
-  #elif EIGEN_COMP_MSVC
-    #define EIGEN_ALLOCA _alloca
-  #endif
+#if !defined EIGEN_ALLOCA && !defined EIGEN_GPU_COMPILE_PHASE
+#if EIGEN_OS_LINUX || EIGEN_OS_MAC || (defined alloca)
+#define EIGEN_ALLOCA alloca
+#elif EIGEN_COMP_MSVC
+#define EIGEN_ALLOCA _alloca
+#endif
 #endif
 
 // With clang -Oz -mthumb, alloca changes the stack pointer in a way that is
@@ -598,184 +637,168 @@
 // TODO: Eliminate after https://bugs.llvm.org/show_bug.cgi?id=23772
 // is fixed.
 #if defined(__clang__) && defined(__thumb__)
-  #undef EIGEN_ALLOCA
+#undef EIGEN_ALLOCA
 #endif
 
 // This helper class construct the allocated memory, and takes care of destructing and freeing the handled data
 // at destruction time. In practice this helper class is mainly useful to avoid memory leak in case of exceptions.
-template<typename T> class aligned_stack_memory_handler : noncopyable
-{
-  public:
-    /* Creates a stack_memory_handler responsible for the buffer \a ptr of size \a size.
-     * Note that \a ptr can be 0 regardless of the other parameters.
-     * This constructor takes care of constructing/initializing the elements of the buffer if required by the scalar type T (see NumTraits<T>::RequireInitialization).
-     * In this case, the buffer elements will also be destructed when this handler will be destructed.
-     * Finally, if \a dealloc is true, then the pointer \a ptr is freed.
-     **/
-    EIGEN_DEVICE_FUNC
-    aligned_stack_memory_handler(T* ptr, std::size_t size, bool dealloc)
-      : m_ptr(ptr), m_size(size), m_deallocate(dealloc)
-    {
-      if(NumTraits<T>::RequireInitialization && m_ptr)
-        Eigen::internal::construct_elements_of_array(m_ptr, size);
-    }
-    EIGEN_DEVICE_FUNC
-    ~aligned_stack_memory_handler()
-    {
-      if(NumTraits<T>::RequireInitialization && m_ptr)
-        Eigen::internal::destruct_elements_of_array<T>(m_ptr, m_size);
-      if(m_deallocate)
-        Eigen::internal::aligned_free(m_ptr);
-    }
-  protected:
-    T* m_ptr;
-    std::size_t m_size;
-    bool m_deallocate;
+template <typename T>
+class aligned_stack_memory_handler : noncopyable {
+ public:
+  /* Creates a stack_memory_handler responsible for the buffer \a ptr of size \a size.
+   * Note that \a ptr can be 0 regardless of the other parameters.
+   * This constructor takes care of constructing/initializing the elements of the buffer if required by the scalar type
+   *T (see NumTraits<T>::RequireInitialization). In this case, the buffer elements will also be destructed when this
+   *handler will be destructed. Finally, if \a dealloc is true, then the pointer \a ptr is freed.
+   **/
+  EIGEN_DEVICE_FUNC aligned_stack_memory_handler(T* ptr, std::size_t size, bool dealloc)
+      : m_ptr(ptr), m_size(size), m_deallocate(dealloc) {
+    if (NumTraits<T>::RequireInitialization && m_ptr) Eigen::internal::default_construct_elements_of_array(m_ptr, size);
+  }
+  EIGEN_DEVICE_FUNC ~aligned_stack_memory_handler() {
+    if (NumTraits<T>::RequireInitialization && m_ptr) Eigen::internal::destruct_elements_of_array<T>(m_ptr, m_size);
+    if (m_deallocate) Eigen::internal::aligned_free(m_ptr);
+  }
+
+ protected:
+  T* m_ptr;
+  std::size_t m_size;
+  bool m_deallocate;
 };
 
 #ifdef EIGEN_ALLOCA
 
-template<typename Xpr, int NbEvaluations,
-         bool MapExternalBuffer = nested_eval<Xpr,NbEvaluations>::Evaluate && Xpr::MaxSizeAtCompileTime==Dynamic
-         >
-struct local_nested_eval_wrapper
-{
-  static const bool NeedExternalBuffer = false;
+template <typename Xpr, int NbEvaluations,
+          bool MapExternalBuffer = nested_eval<Xpr, NbEvaluations>::Evaluate && Xpr::MaxSizeAtCompileTime == Dynamic>
+struct local_nested_eval_wrapper {
+  static constexpr bool NeedExternalBuffer = false;
   typedef typename Xpr::Scalar Scalar;
-  typedef typename nested_eval<Xpr,NbEvaluations>::type ObjectType;
+  typedef typename nested_eval<Xpr, NbEvaluations>::type ObjectType;
   ObjectType object;
 
-  EIGEN_DEVICE_FUNC
-  local_nested_eval_wrapper(const Xpr& xpr, Scalar* ptr) : object(xpr)
-  {
+  EIGEN_DEVICE_FUNC local_nested_eval_wrapper(const Xpr& xpr, Scalar* ptr) : object(xpr) {
     EIGEN_UNUSED_VARIABLE(ptr);
-    eigen_internal_assert(ptr==0);
+    eigen_internal_assert(ptr == 0);
   }
 };
 
-template<typename Xpr, int NbEvaluations>
-struct local_nested_eval_wrapper<Xpr,NbEvaluations,true>
-{
-  static const bool NeedExternalBuffer = true;
+template <typename Xpr, int NbEvaluations>
+struct local_nested_eval_wrapper<Xpr, NbEvaluations, true> {
+  static constexpr bool NeedExternalBuffer = true;
   typedef typename Xpr::Scalar Scalar;
   typedef typename plain_object_eval<Xpr>::type PlainObject;
-  typedef Map<PlainObject,EIGEN_DEFAULT_ALIGN_BYTES> ObjectType;
+  typedef Map<PlainObject, EIGEN_DEFAULT_ALIGN_BYTES> ObjectType;
   ObjectType object;
 
-  EIGEN_DEVICE_FUNC
-  local_nested_eval_wrapper(const Xpr& xpr, Scalar* ptr)
-    : object(ptr==0 ? reinterpret_cast<Scalar*>(Eigen::internal::aligned_malloc(sizeof(Scalar)*xpr.size())) : ptr, xpr.rows(), xpr.cols()),
-      m_deallocate(ptr==0)
-  {
-    if(NumTraits<Scalar>::RequireInitialization && object.data())
-      Eigen::internal::construct_elements_of_array(object.data(), object.size());
+  EIGEN_DEVICE_FUNC local_nested_eval_wrapper(const Xpr& xpr, Scalar* ptr)
+      : object(ptr == 0 ? reinterpret_cast<Scalar*>(Eigen::internal::aligned_malloc(sizeof(Scalar) * xpr.size())) : ptr,
+               xpr.rows(), xpr.cols()),
+        m_deallocate(ptr == 0) {
+    if (NumTraits<Scalar>::RequireInitialization && object.data())
+      Eigen::internal::default_construct_elements_of_array(object.data(), object.size());
     object = xpr;
   }
 
-  EIGEN_DEVICE_FUNC
-  ~local_nested_eval_wrapper()
-  {
-    if(NumTraits<Scalar>::RequireInitialization && object.data())
+  EIGEN_DEVICE_FUNC ~local_nested_eval_wrapper() {
+    if (NumTraits<Scalar>::RequireInitialization && object.data())
       Eigen::internal::destruct_elements_of_array(object.data(), object.size());
-    if(m_deallocate)
-      Eigen::internal::aligned_free(object.data());
+    if (m_deallocate) Eigen::internal::aligned_free(object.data());
   }
 
-private:
+ private:
   bool m_deallocate;
 };
 
-#endif // EIGEN_ALLOCA
+#endif  // EIGEN_ALLOCA
 
-template<typename T> class scoped_array : noncopyable
-{
+template <typename T>
+class scoped_array : noncopyable {
   T* m_ptr;
-public:
-  explicit scoped_array(std::ptrdiff_t size)
-  {
-    m_ptr = new T[size];
-  }
-  ~scoped_array()
-  {
-    delete[] m_ptr;
-  }
+
+ public:
+  explicit scoped_array(std::ptrdiff_t size) { m_ptr = new T[size]; }
+  ~scoped_array() { delete[] m_ptr; }
   T& operator[](std::ptrdiff_t i) { return m_ptr[i]; }
   const T& operator[](std::ptrdiff_t i) const { return m_ptr[i]; }
-  T* &ptr() { return m_ptr; }
+  T*& ptr() { return m_ptr; }
   const T* ptr() const { return m_ptr; }
   operator const T*() const { return m_ptr; }
 };
 
-template<typename T> void swap(scoped_array<T> &a,scoped_array<T> &b)
-{
-  std::swap(a.ptr(),b.ptr());
+template <typename T>
+void swap(scoped_array<T>& a, scoped_array<T>& b) {
+  std::swap(a.ptr(), b.ptr());
 }
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \internal
-  *
-  * The macro ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) declares, allocates,
-  * and construct an aligned buffer named NAME of SIZE elements of type TYPE on the stack
-  * if the size in bytes is smaller than EIGEN_STACK_ALLOCATION_LIMIT, and if stack allocation is supported by the platform
-  * (currently, this is Linux, OSX and Visual Studio only). Otherwise the memory is allocated on the heap.
-  * The allocated buffer is automatically deleted when exiting the scope of this declaration.
-  * If BUFFER is non null, then the declared variable is simply an alias for BUFFER, and no allocation/deletion occurs.
-  * Here is an example:
-  * \code
-  * {
-  *   ei_declare_aligned_stack_constructed_variable(float,data,size,0);
-  *   // use data[0] to data[size-1]
-  * }
-  * \endcode
-  * The underlying stack allocation function can controlled with the EIGEN_ALLOCA preprocessor token.
-  *
-  * The macro ei_declare_local_nested_eval(XPR_T,XPR,N,NAME) is analogue to
-  * \code
-  *   typename internal::nested_eval<XPRT_T,N>::type NAME(XPR);
-  * \endcode
-  * with the advantage of using aligned stack allocation even if the maximal size of XPR at compile time is unknown.
-  * This is accomplished through alloca if this later is supported and if the required number of bytes
-  * is below EIGEN_STACK_ALLOCATION_LIMIT.
-  */
+ *
+ * The macro ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) declares, allocates,
+ * and construct an aligned buffer named NAME of SIZE elements of type TYPE on the stack
+ * if the size in bytes is smaller than EIGEN_STACK_ALLOCATION_LIMIT, and if stack allocation is supported by the
+ * platform (currently, this is Linux, OSX and Visual Studio only). Otherwise the memory is allocated on the heap. The
+ * allocated buffer is automatically deleted when exiting the scope of this declaration. If BUFFER is non null, then the
+ * declared variable is simply an alias for BUFFER, and no allocation/deletion occurs. Here is an example: \code
+ * {
+ *   ei_declare_aligned_stack_constructed_variable(float,data,size,0);
+ *   // use data[0] to data[size-1]
+ * }
+ * \endcode
+ * The underlying stack allocation function can controlled with the EIGEN_ALLOCA preprocessor token.
+ *
+ * The macro ei_declare_local_nested_eval(XPR_T,XPR,N,NAME) is analogue to
+ * \code
+ *   typename internal::nested_eval<XPRT_T,N>::type NAME(XPR);
+ * \endcode
+ * with the advantage of using aligned stack allocation even if the maximal size of XPR at compile time is unknown.
+ * This is accomplished through alloca if this later is supported and if the required number of bytes
+ * is below EIGEN_STACK_ALLOCATION_LIMIT.
+ */
 #ifdef EIGEN_ALLOCA
 
-  #if EIGEN_DEFAULT_ALIGN_BYTES>0
-    // We always manually re-align the result of EIGEN_ALLOCA.
-    // If alloca is already aligned, the compiler should be smart enough to optimize away the re-alignment.
-    #define EIGEN_ALIGNED_ALLOCA(SIZE) reinterpret_cast<void*>((internal::UIntPtr(EIGEN_ALLOCA(SIZE+EIGEN_DEFAULT_ALIGN_BYTES-1)) + EIGEN_DEFAULT_ALIGN_BYTES-1) & ~(std::size_t(EIGEN_DEFAULT_ALIGN_BYTES-1)))
-  #else
-    #define EIGEN_ALIGNED_ALLOCA(SIZE) EIGEN_ALLOCA(SIZE)
-  #endif
+#if EIGEN_DEFAULT_ALIGN_BYTES > 0
+   // We always manually re-align the result of EIGEN_ALLOCA.
+// If alloca is already aligned, the compiler should be smart enough to optimize away the re-alignment.
+#define EIGEN_ALIGNED_ALLOCA(SIZE)                                                                           \
+  reinterpret_cast<void*>(                                                                                   \
+      (std::uintptr_t(EIGEN_ALLOCA(SIZE + EIGEN_DEFAULT_ALIGN_BYTES - 1)) + EIGEN_DEFAULT_ALIGN_BYTES - 1) & \
+      ~(std::size_t(EIGEN_DEFAULT_ALIGN_BYTES - 1)))
+#else
+#define EIGEN_ALIGNED_ALLOCA(SIZE) EIGEN_ALLOCA(SIZE)
+#endif
 
-  #define ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) \
-    Eigen::internal::check_size_for_overflow<TYPE>(SIZE); \
-    TYPE* NAME = (BUFFER)!=0 ? (BUFFER) \
-               : reinterpret_cast<TYPE*>( \
-                      (sizeof(TYPE)*SIZE<=EIGEN_STACK_ALLOCATION_LIMIT) ? EIGEN_ALIGNED_ALLOCA(sizeof(TYPE)*SIZE) \
-                    : Eigen::internal::aligned_malloc(sizeof(TYPE)*SIZE) );  \
-    Eigen::internal::aligned_stack_memory_handler<TYPE> EIGEN_CAT(NAME,_stack_memory_destructor)((BUFFER)==0 ? NAME : 0,SIZE,sizeof(TYPE)*SIZE>EIGEN_STACK_ALLOCATION_LIMIT)
+#define ei_declare_aligned_stack_constructed_variable(TYPE, NAME, SIZE, BUFFER)                                     \
+  Eigen::internal::check_size_for_overflow<TYPE>(SIZE);                                                             \
+  TYPE* NAME = (BUFFER) != 0 ? (BUFFER)                                                                             \
+                             : reinterpret_cast<TYPE*>((sizeof(TYPE) * SIZE <= EIGEN_STACK_ALLOCATION_LIMIT)        \
+                                                           ? EIGEN_ALIGNED_ALLOCA(sizeof(TYPE) * SIZE)              \
+                                                           : Eigen::internal::aligned_malloc(sizeof(TYPE) * SIZE)); \
+  Eigen::internal::aligned_stack_memory_handler<TYPE> EIGEN_CAT(NAME, _stack_memory_destructor)(                    \
+      (BUFFER) == 0 ? NAME : 0, SIZE, sizeof(TYPE) * SIZE > EIGEN_STACK_ALLOCATION_LIMIT)
 
-
-  #define ei_declare_local_nested_eval(XPR_T,XPR,N,NAME) \
-    Eigen::internal::local_nested_eval_wrapper<XPR_T,N> EIGEN_CAT(NAME,_wrapper)(XPR, reinterpret_cast<typename XPR_T::Scalar*>( \
-      ( (Eigen::internal::local_nested_eval_wrapper<XPR_T,N>::NeedExternalBuffer) && ((sizeof(typename XPR_T::Scalar)*XPR.size())<=EIGEN_STACK_ALLOCATION_LIMIT) ) \
-        ? EIGEN_ALIGNED_ALLOCA( sizeof(typename XPR_T::Scalar)*XPR.size() ) : 0 ) ) ; \
-    typename Eigen::internal::local_nested_eval_wrapper<XPR_T,N>::ObjectType NAME(EIGEN_CAT(NAME,_wrapper).object)
+#define ei_declare_local_nested_eval(XPR_T, XPR, N, NAME)                                        \
+  Eigen::internal::local_nested_eval_wrapper<XPR_T, N> EIGEN_CAT(NAME, _wrapper)(                \
+      XPR, reinterpret_cast<typename XPR_T::Scalar*>(                                            \
+               ((Eigen::internal::local_nested_eval_wrapper<XPR_T, N>::NeedExternalBuffer) &&    \
+                ((sizeof(typename XPR_T::Scalar) * XPR.size()) <= EIGEN_STACK_ALLOCATION_LIMIT)) \
+                   ? EIGEN_ALIGNED_ALLOCA(sizeof(typename XPR_T::Scalar) * XPR.size())           \
+                   : 0));                                                                        \
+  typename Eigen::internal::local_nested_eval_wrapper<XPR_T, N>::ObjectType NAME(EIGEN_CAT(NAME, _wrapper).object)
 
 #else
 
-  #define ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) \
-    Eigen::internal::check_size_for_overflow<TYPE>(SIZE); \
-    TYPE* NAME = (BUFFER)!=0 ? BUFFER : reinterpret_cast<TYPE*>(Eigen::internal::aligned_malloc(sizeof(TYPE)*SIZE));    \
-    Eigen::internal::aligned_stack_memory_handler<TYPE> EIGEN_CAT(NAME,_stack_memory_destructor)((BUFFER)==0 ? NAME : 0,SIZE,true)
+#define ei_declare_aligned_stack_constructed_variable(TYPE, NAME, SIZE, BUFFER)                                        \
+  Eigen::internal::check_size_for_overflow<TYPE>(SIZE);                                                                \
+  TYPE* NAME = (BUFFER) != 0 ? BUFFER : reinterpret_cast<TYPE*>(Eigen::internal::aligned_malloc(sizeof(TYPE) * SIZE)); \
+  Eigen::internal::aligned_stack_memory_handler<TYPE> EIGEN_CAT(NAME, _stack_memory_destructor)(                       \
+      (BUFFER) == 0 ? NAME : 0, SIZE, true)
 
-
-#define ei_declare_local_nested_eval(XPR_T,XPR,N,NAME) typename Eigen::internal::nested_eval<XPR_T,N>::type NAME(XPR)
+#define ei_declare_local_nested_eval(XPR_T, XPR, N, NAME) \
+  typename Eigen::internal::nested_eval<XPR_T, N>::type NAME(XPR)
 
 #endif
 
-
 /*****************************************************************************
 *** Implementation of EIGEN_MAKE_ALIGNED_OPERATOR_NEW [_IF]                ***
 *****************************************************************************/
@@ -787,108 +810,106 @@
 #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign)
 #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar,Size)
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)
 
 #else
 
 // HIP does not support new/delete on device.
-#if EIGEN_MAX_ALIGN_BYTES!=0 && !defined(EIGEN_HIP_DEVICE_COMPILE)
-  #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
-      EIGEN_DEVICE_FUNC \
-      void* operator new(std::size_t size, const std::nothrow_t&) EIGEN_NO_THROW { \
-        EIGEN_TRY { return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); } \
-        EIGEN_CATCH (...) { return 0; } \
-      }
-  #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
-      EIGEN_DEVICE_FUNC \
-      void *operator new(std::size_t size) { \
-        return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); \
-      } \
-      EIGEN_DEVICE_FUNC \
-      void *operator new[](std::size_t size) { \
-        return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); \
-      } \
-      EIGEN_DEVICE_FUNC \
-      void operator delete(void * ptr) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
-      EIGEN_DEVICE_FUNC \
-      void operator delete[](void * ptr) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
-      EIGEN_DEVICE_FUNC \
-      void operator delete(void * ptr, std::size_t /* sz */) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
-      EIGEN_DEVICE_FUNC \
-      void operator delete[](void * ptr, std::size_t /* sz */) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
-      /* in-place new and delete. since (at least afaik) there is no actual   */ \
-      /* memory allocated we can safely let the default implementation handle */ \
-      /* this particular case. */ \
-      EIGEN_DEVICE_FUNC \
-      static void *operator new(std::size_t size, void *ptr) { return ::operator new(size,ptr); } \
-      EIGEN_DEVICE_FUNC \
-      static void *operator new[](std::size_t size, void* ptr) { return ::operator new[](size,ptr); } \
-      EIGEN_DEVICE_FUNC \
-      void operator delete(void * memory, void *ptr) EIGEN_NO_THROW { return ::operator delete(memory,ptr); } \
-      EIGEN_DEVICE_FUNC \
-      void operator delete[](void * memory, void *ptr) EIGEN_NO_THROW { return ::operator delete[](memory,ptr); } \
-      /* nothrow-new (returns zero instead of std::bad_alloc) */ \
-      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
-      EIGEN_DEVICE_FUNC \
-      void operator delete(void *ptr, const std::nothrow_t&) EIGEN_NO_THROW { \
-        Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); \
-      } \
-      typedef void eigen_aligned_operator_new_marker_type;
+#if EIGEN_MAX_ALIGN_BYTES != 0 && !defined(EIGEN_HIP_DEVICE_COMPILE)
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign)                                    \
+  EIGEN_DEVICE_FUNC void* operator new(std::size_t size, const std::nothrow_t&) EIGEN_NO_THROW { \
+    EIGEN_TRY { return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); }        \
+    EIGEN_CATCH(...) { return 0; }                                                               \
+  }
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)                                                             \
+  EIGEN_DEVICE_FUNC void* operator new(std::size_t size) {                                                           \
+    return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size);                                          \
+  }                                                                                                                  \
+  EIGEN_DEVICE_FUNC void* operator new[](std::size_t size) {                                                         \
+    return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size);                                          \
+  }                                                                                                                  \
+  EIGEN_DEVICE_FUNC void operator delete(void* ptr) EIGEN_NO_THROW {                                                 \
+    Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr);                                                    \
+  }                                                                                                                  \
+  EIGEN_DEVICE_FUNC void operator delete[](void* ptr) EIGEN_NO_THROW {                                               \
+    Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr);                                                    \
+  }                                                                                                                  \
+  EIGEN_DEVICE_FUNC void operator delete(void* ptr, std::size_t /* sz */) EIGEN_NO_THROW {                           \
+    Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr);                                                    \
+  }                                                                                                                  \
+  EIGEN_DEVICE_FUNC void operator delete[](void* ptr, std::size_t /* sz */) EIGEN_NO_THROW {                         \
+    Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr);                                                    \
+  }                                                                                                                  \
+  /* in-place new and delete. since (at least afaik) there is no actual   */                                         \
+  /* memory allocated we can safely let the default implementation handle */                                         \
+  /* this particular case. */                                                                                        \
+  EIGEN_DEVICE_FUNC static void* operator new(std::size_t size, void* ptr) { return ::operator new(size, ptr); }     \
+  EIGEN_DEVICE_FUNC static void* operator new[](std::size_t size, void* ptr) { return ::operator new[](size, ptr); } \
+  EIGEN_DEVICE_FUNC void operator delete(void* memory, void* ptr) EIGEN_NO_THROW {                                   \
+    return ::operator delete(memory, ptr);                                                                           \
+  }                                                                                                                  \
+  EIGEN_DEVICE_FUNC void operator delete[](void* memory, void* ptr) EIGEN_NO_THROW {                                 \
+    return ::operator delete[](memory, ptr);                                                                         \
+  }                                                                                                                  \
+  /* nothrow-new (returns zero instead of std::bad_alloc) */                                                         \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign)                                                              \
+  EIGEN_DEVICE_FUNC void operator delete(void* ptr, const std::nothrow_t&) EIGEN_NO_THROW {                          \
+    Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr);                                                    \
+  }                                                                                                                  \
+  typedef void eigen_aligned_operator_new_marker_type;
 #else
-  #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 #endif
 
 #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
-#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar,Size)                        \
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(                                                             \
-        ((Size)!=Eigen::Dynamic) &&                                                                    \
-        (((EIGEN_MAX_ALIGN_BYTES>=16) && ((sizeof(Scalar)*(Size))%(EIGEN_MAX_ALIGN_BYTES  )==0)) ||    \
-         ((EIGEN_MAX_ALIGN_BYTES>=32) && ((sizeof(Scalar)*(Size))%(EIGEN_MAX_ALIGN_BYTES/2)==0)) ||    \
-         ((EIGEN_MAX_ALIGN_BYTES>=64) && ((sizeof(Scalar)*(Size))%(EIGEN_MAX_ALIGN_BYTES/4)==0))   )))
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)                                 \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(                                                                            \
+      bool(((Size) != Eigen::Dynamic) &&                                                                         \
+           (((EIGEN_MAX_ALIGN_BYTES >= 16) && ((sizeof(Scalar) * (Size)) % (EIGEN_MAX_ALIGN_BYTES) == 0)) ||     \
+            ((EIGEN_MAX_ALIGN_BYTES >= 32) && ((sizeof(Scalar) * (Size)) % (EIGEN_MAX_ALIGN_BYTES / 2) == 0)) || \
+            ((EIGEN_MAX_ALIGN_BYTES >= 64) && ((sizeof(Scalar) * (Size)) % (EIGEN_MAX_ALIGN_BYTES / 4) == 0)))))
 
 #endif
 
 /****************************************************************************/
 
 /** \class aligned_allocator
-* \ingroup Core_Module
-*
-* \brief STL compatible allocator to use with types requiring a non standrad alignment.
-*
-* The memory is aligned as for dynamically aligned matrix/array types such as MatrixXd.
-* By default, it will thus provide at least 16 bytes alignment and more in following cases:
-*  - 32 bytes alignment if AVX is enabled.
-*  - 64 bytes alignment if AVX512 is enabled.
-*
-* This can be controlled using the \c EIGEN_MAX_ALIGN_BYTES macro as documented
-* \link TopicPreprocessorDirectivesPerformance there \endlink.
-*
-* Example:
-* \code
-* // Matrix4f requires 16 bytes alignment:
-* std::map< int, Matrix4f, std::less<int>,
-*           aligned_allocator<std::pair<const int, Matrix4f> > > my_map_mat4;
-* // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
-* std::map< int, Vector3f > my_map_vec3;
-* \endcode
-*
-* \sa \blank \ref TopicStlContainers.
-*/
-template<class T>
-class aligned_allocator : public std::allocator<T>
-{
-public:
-  typedef std::size_t     size_type;
-  typedef std::ptrdiff_t  difference_type;
-  typedef T*              pointer;
-  typedef const T*        const_pointer;
-  typedef T&              reference;
-  typedef const T&        const_reference;
-  typedef T               value_type;
+ * \ingroup Core_Module
+ *
+ * \brief STL compatible allocator to use with types requiring a non-standard alignment.
+ *
+ * The memory is aligned as for dynamically aligned matrix/array types such as MatrixXd.
+ * By default, it will thus provide at least 16 bytes alignment and more in following cases:
+ *  - 32 bytes alignment if AVX is enabled.
+ *  - 64 bytes alignment if AVX512 is enabled.
+ *
+ * This can be controlled using the \c EIGEN_MAX_ALIGN_BYTES macro as documented
+ * \link TopicPreprocessorDirectivesPerformance there \endlink.
+ *
+ * Example:
+ * \code
+ * // Matrix4f requires 16 bytes alignment:
+ * std::map< int, Matrix4f, std::less<int>,
+ *           aligned_allocator<std::pair<const int, Matrix4f> > > my_map_mat4;
+ * // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
+ * std::map< int, Vector3f > my_map_vec3;
+ * \endcode
+ *
+ * \sa \blank \ref TopicStlContainers.
+ */
+template <class T>
+class aligned_allocator : public std::allocator<T> {
+ public:
+  typedef std::size_t size_type;
+  typedef std::ptrdiff_t difference_type;
+  typedef T* pointer;
+  typedef const T* const_pointer;
+  typedef T& reference;
+  typedef const T& const_reference;
+  typedef T value_type;
 
-  template<class U>
-  struct rebind
-  {
+  template <class U>
+  struct rebind {
     typedef aligned_allocator<U> other;
   };
 
@@ -896,206 +917,320 @@
 
   aligned_allocator(const aligned_allocator& other) : std::allocator<T>(other) {}
 
-  template<class U>
+  template <class U>
   aligned_allocator(const aligned_allocator<U>& other) : std::allocator<T>(other) {}
 
   ~aligned_allocator() {}
 
-  #if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(7,0)
+#if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_STRICT_AT_LEAST(7, 0, 0)
   // In gcc std::allocator::max_size() is bugged making gcc triggers a warning:
-  // eigen/Eigen/src/Core/util/Memory.h:189:12: warning: argument 1 value '18446744073709551612' exceeds maximum object size 9223372036854775807
-  // See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=87544
-  size_type max_size() const {
-    return (std::numeric_limits<std::ptrdiff_t>::max)()/sizeof(T);
-  }
-  #endif
+  // eigen/Eigen/src/Core/util/Memory.h:189:12: warning: argument 1 value '18446744073709551612' exceeds maximum object
+  // size 9223372036854775807 See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=87544
+  size_type max_size() const { return (std::numeric_limits<std::ptrdiff_t>::max)() / sizeof(T); }
+#endif
 
-  pointer allocate(size_type num, const void* /*hint*/ = 0)
-  {
+  pointer allocate(size_type num, const void* /*hint*/ = 0) {
     internal::check_size_for_overflow<T>(num);
-    return static_cast<pointer>( internal::aligned_malloc(num * sizeof(T)) );
+    return static_cast<pointer>(internal::aligned_malloc(num * sizeof(T)));
   }
 
-  void deallocate(pointer p, size_type /*num*/)
-  {
-    internal::aligned_free(p);
-  }
+  void deallocate(pointer p, size_type /*num*/) { internal::aligned_free(p); }
 };
 
 //---------- Cache sizes ----------
 
 #if !defined(EIGEN_NO_CPUID)
-#  if EIGEN_COMP_GNUC && EIGEN_ARCH_i386_OR_x86_64
-#    if defined(__PIC__) && EIGEN_ARCH_i386
-       // Case for x86 with PIC
-#      define EIGEN_CPUID(abcd,func,id) \
-         __asm__ __volatile__ ("xchgl %%ebx, %k1;cpuid; xchgl %%ebx,%k1": "=a" (abcd[0]), "=&r" (abcd[1]), "=c" (abcd[2]), "=d" (abcd[3]) : "a" (func), "c" (id));
-#    elif defined(__PIC__) && EIGEN_ARCH_x86_64
-       // Case for x64 with PIC. In theory this is only a problem with recent gcc and with medium or large code model, not with the default small code model.
-       // However, we cannot detect which code model is used, and the xchg overhead is negligible anyway.
-#      define EIGEN_CPUID(abcd,func,id) \
-        __asm__ __volatile__ ("xchg{q}\t{%%}rbx, %q1; cpuid; xchg{q}\t{%%}rbx, %q1": "=a" (abcd[0]), "=&r" (abcd[1]), "=c" (abcd[2]), "=d" (abcd[3]) : "0" (func), "2" (id));
-#    else
-       // Case for x86_64 or x86 w/o PIC
-#      define EIGEN_CPUID(abcd,func,id) \
-         __asm__ __volatile__ ("cpuid": "=a" (abcd[0]), "=b" (abcd[1]), "=c" (abcd[2]), "=d" (abcd[3]) : "0" (func), "2" (id) );
-#    endif
-#  elif EIGEN_COMP_MSVC
-#    if (EIGEN_COMP_MSVC > 1500) && EIGEN_ARCH_i386_OR_x86_64
-#      define EIGEN_CPUID(abcd,func,id) __cpuidex((int*)abcd,func,id)
-#    endif
-#  endif
+#if EIGEN_COMP_GNUC && EIGEN_ARCH_i386_OR_x86_64
+#if defined(__PIC__) && EIGEN_ARCH_i386
+// Case for x86 with PIC
+#define EIGEN_CPUID(abcd, func, id)                                                  \
+  __asm__ __volatile__("xchgl %%ebx, %k1;cpuid; xchgl %%ebx,%k1"                     \
+                       : "=a"(abcd[0]), "=&r"(abcd[1]), "=c"(abcd[2]), "=d"(abcd[3]) \
+                       : "a"(func), "c"(id));
+#elif defined(__PIC__) && EIGEN_ARCH_x86_64
+// Case for x64 with PIC. In theory this is only a problem with recent gcc and with medium or large code model, not with
+// the default small code model. However, we cannot detect which code model is used, and the xchg overhead is negligible
+// anyway.
+#define EIGEN_CPUID(abcd, func, id)                                                  \
+  __asm__ __volatile__("xchg{q}\t{%%}rbx, %q1; cpuid; xchg{q}\t{%%}rbx, %q1"         \
+                       : "=a"(abcd[0]), "=&r"(abcd[1]), "=c"(abcd[2]), "=d"(abcd[3]) \
+                       : "0"(func), "2"(id));
+#else
+// Case for x86_64 or x86 w/o PIC
+#define EIGEN_CPUID(abcd, func, id) \
+  __asm__ __volatile__("cpuid" : "=a"(abcd[0]), "=b"(abcd[1]), "=c"(abcd[2]), "=d"(abcd[3]) : "0"(func), "2"(id));
+#endif
+#elif EIGEN_COMP_MSVC
+#if EIGEN_ARCH_i386_OR_x86_64
+#define EIGEN_CPUID(abcd, func, id) __cpuidex((int*)abcd, func, id)
+#endif
+#endif
 #endif
 
 namespace internal {
 
 #ifdef EIGEN_CPUID
 
-inline bool cpuid_is_vendor(int abcd[4], const int vendor[3])
-{
-  return abcd[1]==vendor[0] && abcd[3]==vendor[1] && abcd[2]==vendor[2];
+inline bool cpuid_is_vendor(int abcd[4], const int vendor[3]) {
+  return abcd[1] == vendor[0] && abcd[3] == vendor[1] && abcd[2] == vendor[2];
 }
 
-inline void queryCacheSizes_intel_direct(int& l1, int& l2, int& l3)
-{
+inline void queryCacheSizes_intel_direct(int& l1, int& l2, int& l3) {
   int abcd[4];
   l1 = l2 = l3 = 0;
   int cache_id = 0;
   int cache_type = 0;
   do {
     abcd[0] = abcd[1] = abcd[2] = abcd[3] = 0;
-    EIGEN_CPUID(abcd,0x4,cache_id);
-    cache_type  = (abcd[0] & 0x0F) >> 0;
-    if(cache_type==1||cache_type==3) // data or unified cache
+    EIGEN_CPUID(abcd, 0x4, cache_id);
+    cache_type = (abcd[0] & 0x0F) >> 0;
+    if (cache_type == 1 || cache_type == 3)  // data or unified cache
     {
-      int cache_level = (abcd[0] & 0xE0) >> 5;  // A[7:5]
-      int ways        = (abcd[1] & 0xFFC00000) >> 22; // B[31:22]
-      int partitions  = (abcd[1] & 0x003FF000) >> 12; // B[21:12]
-      int line_size   = (abcd[1] & 0x00000FFF) >>  0; // B[11:0]
-      int sets        = (abcd[2]);                    // C[31:0]
+      int cache_level = (abcd[0] & 0xE0) >> 5;        // A[7:5]
+      int ways = (abcd[1] & 0xFFC00000) >> 22;        // B[31:22]
+      int partitions = (abcd[1] & 0x003FF000) >> 12;  // B[21:12]
+      int line_size = (abcd[1] & 0x00000FFF) >> 0;    // B[11:0]
+      int sets = (abcd[2]);                           // C[31:0]
 
-      int cache_size = (ways+1) * (partitions+1) * (line_size+1) * (sets+1);
+      int cache_size = (ways + 1) * (partitions + 1) * (line_size + 1) * (sets + 1);
 
-      switch(cache_level)
-      {
-        case 1: l1 = cache_size; break;
-        case 2: l2 = cache_size; break;
-        case 3: l3 = cache_size; break;
-        default: break;
+      switch (cache_level) {
+        case 1:
+          l1 = cache_size;
+          break;
+        case 2:
+          l2 = cache_size;
+          break;
+        case 3:
+          l3 = cache_size;
+          break;
+        default:
+          break;
       }
     }
     cache_id++;
-  } while(cache_type>0 && cache_id<16);
+  } while (cache_type > 0 && cache_id < 16);
 }
 
-inline void queryCacheSizes_intel_codes(int& l1, int& l2, int& l3)
-{
+inline void queryCacheSizes_intel_codes(int& l1, int& l2, int& l3) {
   int abcd[4];
   abcd[0] = abcd[1] = abcd[2] = abcd[3] = 0;
   l1 = l2 = l3 = 0;
-  EIGEN_CPUID(abcd,0x00000002,0);
-  unsigned char * bytes = reinterpret_cast<unsigned char *>(abcd)+2;
+  EIGEN_CPUID(abcd, 0x00000002, 0);
+  unsigned char* bytes = reinterpret_cast<unsigned char*>(abcd) + 2;
   bool check_for_p2_core2 = false;
-  for(int i=0; i<14; ++i)
-  {
-    switch(bytes[i])
-    {
-      case 0x0A: l1 = 8; break;   // 0Ah   data L1 cache, 8 KB, 2 ways, 32 byte lines
-      case 0x0C: l1 = 16; break;  // 0Ch   data L1 cache, 16 KB, 4 ways, 32 byte lines
-      case 0x0E: l1 = 24; break;  // 0Eh   data L1 cache, 24 KB, 6 ways, 64 byte lines
-      case 0x10: l1 = 16; break;  // 10h   data L1 cache, 16 KB, 4 ways, 32 byte lines (IA-64)
-      case 0x15: l1 = 16; break;  // 15h   code L1 cache, 16 KB, 4 ways, 32 byte lines (IA-64)
-      case 0x2C: l1 = 32; break;  // 2Ch   data L1 cache, 32 KB, 8 ways, 64 byte lines
-      case 0x30: l1 = 32; break;  // 30h   code L1 cache, 32 KB, 8 ways, 64 byte lines
-      case 0x60: l1 = 16; break;  // 60h   data L1 cache, 16 KB, 8 ways, 64 byte lines, sectored
-      case 0x66: l1 = 8; break;   // 66h   data L1 cache, 8 KB, 4 ways, 64 byte lines, sectored
-      case 0x67: l1 = 16; break;  // 67h   data L1 cache, 16 KB, 4 ways, 64 byte lines, sectored
-      case 0x68: l1 = 32; break;  // 68h   data L1 cache, 32 KB, 4 ways, 64 byte lines, sectored
-      case 0x1A: l2 = 96; break;   // code and data L2 cache, 96 KB, 6 ways, 64 byte lines (IA-64)
-      case 0x22: l3 = 512; break;   // code and data L3 cache, 512 KB, 4 ways (!), 64 byte lines, dual-sectored
-      case 0x23: l3 = 1024; break;   // code and data L3 cache, 1024 KB, 8 ways, 64 byte lines, dual-sectored
-      case 0x25: l3 = 2048; break;   // code and data L3 cache, 2048 KB, 8 ways, 64 byte lines, dual-sectored
-      case 0x29: l3 = 4096; break;   // code and data L3 cache, 4096 KB, 8 ways, 64 byte lines, dual-sectored
-      case 0x39: l2 = 128; break;   // code and data L2 cache, 128 KB, 4 ways, 64 byte lines, sectored
-      case 0x3A: l2 = 192; break;   // code and data L2 cache, 192 KB, 6 ways, 64 byte lines, sectored
-      case 0x3B: l2 = 128; break;   // code and data L2 cache, 128 KB, 2 ways, 64 byte lines, sectored
-      case 0x3C: l2 = 256; break;   // code and data L2 cache, 256 KB, 4 ways, 64 byte lines, sectored
-      case 0x3D: l2 = 384; break;   // code and data L2 cache, 384 KB, 6 ways, 64 byte lines, sectored
-      case 0x3E: l2 = 512; break;   // code and data L2 cache, 512 KB, 4 ways, 64 byte lines, sectored
-      case 0x40: l2 = 0; break;   // no integrated L2 cache (P6 core) or L3 cache (P4 core)
-      case 0x41: l2 = 128; break;   // code and data L2 cache, 128 KB, 4 ways, 32 byte lines
-      case 0x42: l2 = 256; break;   // code and data L2 cache, 256 KB, 4 ways, 32 byte lines
-      case 0x43: l2 = 512; break;   // code and data L2 cache, 512 KB, 4 ways, 32 byte lines
-      case 0x44: l2 = 1024; break;   // code and data L2 cache, 1024 KB, 4 ways, 32 byte lines
-      case 0x45: l2 = 2048; break;   // code and data L2 cache, 2048 KB, 4 ways, 32 byte lines
-      case 0x46: l3 = 4096; break;   // code and data L3 cache, 4096 KB, 4 ways, 64 byte lines
-      case 0x47: l3 = 8192; break;   // code and data L3 cache, 8192 KB, 8 ways, 64 byte lines
-      case 0x48: l2 = 3072; break;   // code and data L2 cache, 3072 KB, 12 ways, 64 byte lines
-      case 0x49: if(l2!=0) l3 = 4096; else {check_for_p2_core2=true; l3 = l2 = 4096;} break;// code and data L3 cache, 4096 KB, 16 ways, 64 byte lines (P4) or L2 for core2
-      case 0x4A: l3 = 6144; break;   // code and data L3 cache, 6144 KB, 12 ways, 64 byte lines
-      case 0x4B: l3 = 8192; break;   // code and data L3 cache, 8192 KB, 16 ways, 64 byte lines
-      case 0x4C: l3 = 12288; break;   // code and data L3 cache, 12288 KB, 12 ways, 64 byte lines
-      case 0x4D: l3 = 16384; break;   // code and data L3 cache, 16384 KB, 16 ways, 64 byte lines
-      case 0x4E: l2 = 6144; break;   // code and data L2 cache, 6144 KB, 24 ways, 64 byte lines
-      case 0x78: l2 = 1024; break;   // code and data L2 cache, 1024 KB, 4 ways, 64 byte lines
-      case 0x79: l2 = 128; break;   // code and data L2 cache, 128 KB, 8 ways, 64 byte lines, dual-sectored
-      case 0x7A: l2 = 256; break;   // code and data L2 cache, 256 KB, 8 ways, 64 byte lines, dual-sectored
-      case 0x7B: l2 = 512; break;   // code and data L2 cache, 512 KB, 8 ways, 64 byte lines, dual-sectored
-      case 0x7C: l2 = 1024; break;   // code and data L2 cache, 1024 KB, 8 ways, 64 byte lines, dual-sectored
-      case 0x7D: l2 = 2048; break;   // code and data L2 cache, 2048 KB, 8 ways, 64 byte lines
-      case 0x7E: l2 = 256; break;   // code and data L2 cache, 256 KB, 8 ways, 128 byte lines, sect. (IA-64)
-      case 0x7F: l2 = 512; break;   // code and data L2 cache, 512 KB, 2 ways, 64 byte lines
-      case 0x80: l2 = 512; break;   // code and data L2 cache, 512 KB, 8 ways, 64 byte lines
-      case 0x81: l2 = 128; break;   // code and data L2 cache, 128 KB, 8 ways, 32 byte lines
-      case 0x82: l2 = 256; break;   // code and data L2 cache, 256 KB, 8 ways, 32 byte lines
-      case 0x83: l2 = 512; break;   // code and data L2 cache, 512 KB, 8 ways, 32 byte lines
-      case 0x84: l2 = 1024; break;   // code and data L2 cache, 1024 KB, 8 ways, 32 byte lines
-      case 0x85: l2 = 2048; break;   // code and data L2 cache, 2048 KB, 8 ways, 32 byte lines
-      case 0x86: l2 = 512; break;   // code and data L2 cache, 512 KB, 4 ways, 64 byte lines
-      case 0x87: l2 = 1024; break;   // code and data L2 cache, 1024 KB, 8 ways, 64 byte lines
-      case 0x88: l3 = 2048; break;   // code and data L3 cache, 2048 KB, 4 ways, 64 byte lines (IA-64)
-      case 0x89: l3 = 4096; break;   // code and data L3 cache, 4096 KB, 4 ways, 64 byte lines (IA-64)
-      case 0x8A: l3 = 8192; break;   // code and data L3 cache, 8192 KB, 4 ways, 64 byte lines (IA-64)
-      case 0x8D: l3 = 3072; break;   // code and data L3 cache, 3072 KB, 12 ways, 128 byte lines (IA-64)
+  for (int i = 0; i < 14; ++i) {
+    switch (bytes[i]) {
+      case 0x0A:
+        l1 = 8;
+        break;  // 0Ah   data L1 cache, 8 KB, 2 ways, 32 byte lines
+      case 0x0C:
+        l1 = 16;
+        break;  // 0Ch   data L1 cache, 16 KB, 4 ways, 32 byte lines
+      case 0x0E:
+        l1 = 24;
+        break;  // 0Eh   data L1 cache, 24 KB, 6 ways, 64 byte lines
+      case 0x10:
+        l1 = 16;
+        break;  // 10h   data L1 cache, 16 KB, 4 ways, 32 byte lines (IA-64)
+      case 0x15:
+        l1 = 16;
+        break;  // 15h   code L1 cache, 16 KB, 4 ways, 32 byte lines (IA-64)
+      case 0x2C:
+        l1 = 32;
+        break;  // 2Ch   data L1 cache, 32 KB, 8 ways, 64 byte lines
+      case 0x30:
+        l1 = 32;
+        break;  // 30h   code L1 cache, 32 KB, 8 ways, 64 byte lines
+      case 0x60:
+        l1 = 16;
+        break;  // 60h   data L1 cache, 16 KB, 8 ways, 64 byte lines, sectored
+      case 0x66:
+        l1 = 8;
+        break;  // 66h   data L1 cache, 8 KB, 4 ways, 64 byte lines, sectored
+      case 0x67:
+        l1 = 16;
+        break;  // 67h   data L1 cache, 16 KB, 4 ways, 64 byte lines, sectored
+      case 0x68:
+        l1 = 32;
+        break;  // 68h   data L1 cache, 32 KB, 4 ways, 64 byte lines, sectored
+      case 0x1A:
+        l2 = 96;
+        break;  // code and data L2 cache, 96 KB, 6 ways, 64 byte lines (IA-64)
+      case 0x22:
+        l3 = 512;
+        break;  // code and data L3 cache, 512 KB, 4 ways (!), 64 byte lines, dual-sectored
+      case 0x23:
+        l3 = 1024;
+        break;  // code and data L3 cache, 1024 KB, 8 ways, 64 byte lines, dual-sectored
+      case 0x25:
+        l3 = 2048;
+        break;  // code and data L3 cache, 2048 KB, 8 ways, 64 byte lines, dual-sectored
+      case 0x29:
+        l3 = 4096;
+        break;  // code and data L3 cache, 4096 KB, 8 ways, 64 byte lines, dual-sectored
+      case 0x39:
+        l2 = 128;
+        break;  // code and data L2 cache, 128 KB, 4 ways, 64 byte lines, sectored
+      case 0x3A:
+        l2 = 192;
+        break;  // code and data L2 cache, 192 KB, 6 ways, 64 byte lines, sectored
+      case 0x3B:
+        l2 = 128;
+        break;  // code and data L2 cache, 128 KB, 2 ways, 64 byte lines, sectored
+      case 0x3C:
+        l2 = 256;
+        break;  // code and data L2 cache, 256 KB, 4 ways, 64 byte lines, sectored
+      case 0x3D:
+        l2 = 384;
+        break;  // code and data L2 cache, 384 KB, 6 ways, 64 byte lines, sectored
+      case 0x3E:
+        l2 = 512;
+        break;  // code and data L2 cache, 512 KB, 4 ways, 64 byte lines, sectored
+      case 0x40:
+        l2 = 0;
+        break;  // no integrated L2 cache (P6 core) or L3 cache (P4 core)
+      case 0x41:
+        l2 = 128;
+        break;  // code and data L2 cache, 128 KB, 4 ways, 32 byte lines
+      case 0x42:
+        l2 = 256;
+        break;  // code and data L2 cache, 256 KB, 4 ways, 32 byte lines
+      case 0x43:
+        l2 = 512;
+        break;  // code and data L2 cache, 512 KB, 4 ways, 32 byte lines
+      case 0x44:
+        l2 = 1024;
+        break;  // code and data L2 cache, 1024 KB, 4 ways, 32 byte lines
+      case 0x45:
+        l2 = 2048;
+        break;  // code and data L2 cache, 2048 KB, 4 ways, 32 byte lines
+      case 0x46:
+        l3 = 4096;
+        break;  // code and data L3 cache, 4096 KB, 4 ways, 64 byte lines
+      case 0x47:
+        l3 = 8192;
+        break;  // code and data L3 cache, 8192 KB, 8 ways, 64 byte lines
+      case 0x48:
+        l2 = 3072;
+        break;  // code and data L2 cache, 3072 KB, 12 ways, 64 byte lines
+      case 0x49:
+        if (l2 != 0)
+          l3 = 4096;
+        else {
+          check_for_p2_core2 = true;
+          l3 = l2 = 4096;
+        }
+        break;  // code and data L3 cache, 4096 KB, 16 ways, 64 byte lines (P4) or L2 for core2
+      case 0x4A:
+        l3 = 6144;
+        break;  // code and data L3 cache, 6144 KB, 12 ways, 64 byte lines
+      case 0x4B:
+        l3 = 8192;
+        break;  // code and data L3 cache, 8192 KB, 16 ways, 64 byte lines
+      case 0x4C:
+        l3 = 12288;
+        break;  // code and data L3 cache, 12288 KB, 12 ways, 64 byte lines
+      case 0x4D:
+        l3 = 16384;
+        break;  // code and data L3 cache, 16384 KB, 16 ways, 64 byte lines
+      case 0x4E:
+        l2 = 6144;
+        break;  // code and data L2 cache, 6144 KB, 24 ways, 64 byte lines
+      case 0x78:
+        l2 = 1024;
+        break;  // code and data L2 cache, 1024 KB, 4 ways, 64 byte lines
+      case 0x79:
+        l2 = 128;
+        break;  // code and data L2 cache, 128 KB, 8 ways, 64 byte lines, dual-sectored
+      case 0x7A:
+        l2 = 256;
+        break;  // code and data L2 cache, 256 KB, 8 ways, 64 byte lines, dual-sectored
+      case 0x7B:
+        l2 = 512;
+        break;  // code and data L2 cache, 512 KB, 8 ways, 64 byte lines, dual-sectored
+      case 0x7C:
+        l2 = 1024;
+        break;  // code and data L2 cache, 1024 KB, 8 ways, 64 byte lines, dual-sectored
+      case 0x7D:
+        l2 = 2048;
+        break;  // code and data L2 cache, 2048 KB, 8 ways, 64 byte lines
+      case 0x7E:
+        l2 = 256;
+        break;  // code and data L2 cache, 256 KB, 8 ways, 128 byte lines, sect. (IA-64)
+      case 0x7F:
+        l2 = 512;
+        break;  // code and data L2 cache, 512 KB, 2 ways, 64 byte lines
+      case 0x80:
+        l2 = 512;
+        break;  // code and data L2 cache, 512 KB, 8 ways, 64 byte lines
+      case 0x81:
+        l2 = 128;
+        break;  // code and data L2 cache, 128 KB, 8 ways, 32 byte lines
+      case 0x82:
+        l2 = 256;
+        break;  // code and data L2 cache, 256 KB, 8 ways, 32 byte lines
+      case 0x83:
+        l2 = 512;
+        break;  // code and data L2 cache, 512 KB, 8 ways, 32 byte lines
+      case 0x84:
+        l2 = 1024;
+        break;  // code and data L2 cache, 1024 KB, 8 ways, 32 byte lines
+      case 0x85:
+        l2 = 2048;
+        break;  // code and data L2 cache, 2048 KB, 8 ways, 32 byte lines
+      case 0x86:
+        l2 = 512;
+        break;  // code and data L2 cache, 512 KB, 4 ways, 64 byte lines
+      case 0x87:
+        l2 = 1024;
+        break;  // code and data L2 cache, 1024 KB, 8 ways, 64 byte lines
+      case 0x88:
+        l3 = 2048;
+        break;  // code and data L3 cache, 2048 KB, 4 ways, 64 byte lines (IA-64)
+      case 0x89:
+        l3 = 4096;
+        break;  // code and data L3 cache, 4096 KB, 4 ways, 64 byte lines (IA-64)
+      case 0x8A:
+        l3 = 8192;
+        break;  // code and data L3 cache, 8192 KB, 4 ways, 64 byte lines (IA-64)
+      case 0x8D:
+        l3 = 3072;
+        break;  // code and data L3 cache, 3072 KB, 12 ways, 128 byte lines (IA-64)
 
-      default: break;
+      default:
+        break;
     }
   }
-  if(check_for_p2_core2 && l2 == l3)
-    l3 = 0;
+  if (check_for_p2_core2 && l2 == l3) l3 = 0;
   l1 *= 1024;
   l2 *= 1024;
   l3 *= 1024;
 }
 
-inline void queryCacheSizes_intel(int& l1, int& l2, int& l3, int max_std_funcs)
-{
-  if(max_std_funcs>=4)
-    queryCacheSizes_intel_direct(l1,l2,l3);
-  else if(max_std_funcs>=2)
-    queryCacheSizes_intel_codes(l1,l2,l3);
+inline void queryCacheSizes_intel(int& l1, int& l2, int& l3, int max_std_funcs) {
+  if (max_std_funcs >= 4)
+    queryCacheSizes_intel_direct(l1, l2, l3);
+  else if (max_std_funcs >= 2)
+    queryCacheSizes_intel_codes(l1, l2, l3);
   else
     l1 = l2 = l3 = 0;
 }
 
-inline void queryCacheSizes_amd(int& l1, int& l2, int& l3)
-{
+inline void queryCacheSizes_amd(int& l1, int& l2, int& l3) {
   int abcd[4];
   abcd[0] = abcd[1] = abcd[2] = abcd[3] = 0;
-  
+
   // First query the max supported function.
-  EIGEN_CPUID(abcd,0x80000000,0);
-  if(static_cast<numext::uint32_t>(abcd[0]) >= static_cast<numext::uint32_t>(0x80000006))
-  {
-    EIGEN_CPUID(abcd,0x80000005,0);
-    l1 = (abcd[2] >> 24) * 1024; // C[31:24] = L1 size in KB
+  EIGEN_CPUID(abcd, 0x80000000, 0);
+  if (static_cast<numext::uint32_t>(abcd[0]) >= static_cast<numext::uint32_t>(0x80000006)) {
+    EIGEN_CPUID(abcd, 0x80000005, 0);
+    l1 = (abcd[2] >> 24) * 1024;  // C[31:24] = L1 size in KB
     abcd[0] = abcd[1] = abcd[2] = abcd[3] = 0;
-    EIGEN_CPUID(abcd,0x80000006,0);
-    l2 = (abcd[2] >> 16) * 1024; // C[31;16] = l2 cache size in KB
-    l3 = ((abcd[3] & 0xFFFC000) >> 18) * 512 * 1024; // D[31;18] = l3 cache size in 512KB
-  }
-  else
-  {
+    EIGEN_CPUID(abcd, 0x80000006, 0);
+    l2 = (abcd[2] >> 16) * 1024;                      // C[31;16] = l2 cache size in KB
+    l3 = ((abcd[3] & 0xFFFC000) >> 18) * 512 * 1024;  // D[31;18] = l3 cache size in 512KB
+  } else {
     l1 = l2 = l3 = 0;
   }
 }
@@ -1103,61 +1238,85 @@
 
 /** \internal
  * Queries and returns the cache sizes in Bytes of the L1, L2, and L3 data caches respectively */
-inline void queryCacheSizes(int& l1, int& l2, int& l3)
-{
-  #ifdef EIGEN_CPUID
+inline void queryCacheSizes(int& l1, int& l2, int& l3) {
+#ifdef EIGEN_CPUID
   int abcd[4];
   const int GenuineIntel[] = {0x756e6547, 0x49656e69, 0x6c65746e};
   const int AuthenticAMD[] = {0x68747541, 0x69746e65, 0x444d4163};
-  const int AMDisbetter_[] = {0x69444d41, 0x74656273, 0x21726574}; // "AMDisbetter!"
+  const int AMDisbetter_[] = {0x69444d41, 0x74656273, 0x21726574};  // "AMDisbetter!"
 
   // identify the CPU vendor
-  EIGEN_CPUID(abcd,0x0,0);
+  EIGEN_CPUID(abcd, 0x0, 0);
   int max_std_funcs = abcd[0];
-  if(cpuid_is_vendor(abcd,GenuineIntel))
-    queryCacheSizes_intel(l1,l2,l3,max_std_funcs);
-  else if(cpuid_is_vendor(abcd,AuthenticAMD) || cpuid_is_vendor(abcd,AMDisbetter_))
-    queryCacheSizes_amd(l1,l2,l3);
+  if (cpuid_is_vendor(abcd, GenuineIntel))
+    queryCacheSizes_intel(l1, l2, l3, max_std_funcs);
+  else if (cpuid_is_vendor(abcd, AuthenticAMD) || cpuid_is_vendor(abcd, AMDisbetter_))
+    queryCacheSizes_amd(l1, l2, l3);
   else
     // by default let's use Intel's API
-    queryCacheSizes_intel(l1,l2,l3,max_std_funcs);
+    queryCacheSizes_intel(l1, l2, l3, max_std_funcs);
 
-  // here is the list of other vendors:
-//   ||cpuid_is_vendor(abcd,"VIA VIA VIA ")
-//   ||cpuid_is_vendor(abcd,"CyrixInstead")
-//   ||cpuid_is_vendor(abcd,"CentaurHauls")
-//   ||cpuid_is_vendor(abcd,"GenuineTMx86")
-//   ||cpuid_is_vendor(abcd,"TransmetaCPU")
-//   ||cpuid_is_vendor(abcd,"RiseRiseRise")
-//   ||cpuid_is_vendor(abcd,"Geode by NSC")
-//   ||cpuid_is_vendor(abcd,"SiS SiS SiS ")
-//   ||cpuid_is_vendor(abcd,"UMC UMC UMC ")
-//   ||cpuid_is_vendor(abcd,"NexGenDriven")
-  #else
+    // here is the list of other vendors:
+    //   ||cpuid_is_vendor(abcd,"VIA VIA VIA ")
+    //   ||cpuid_is_vendor(abcd,"CyrixInstead")
+    //   ||cpuid_is_vendor(abcd,"CentaurHauls")
+    //   ||cpuid_is_vendor(abcd,"GenuineTMx86")
+    //   ||cpuid_is_vendor(abcd,"TransmetaCPU")
+    //   ||cpuid_is_vendor(abcd,"RiseRiseRise")
+    //   ||cpuid_is_vendor(abcd,"Geode by NSC")
+    //   ||cpuid_is_vendor(abcd,"SiS SiS SiS ")
+    //   ||cpuid_is_vendor(abcd,"UMC UMC UMC ")
+    //   ||cpuid_is_vendor(abcd,"NexGenDriven")
+#else
   l1 = l2 = l3 = -1;
-  #endif
+#endif
 }
 
 /** \internal
  * \returns the size in Bytes of the L1 data cache */
-inline int queryL1CacheSize()
-{
+inline int queryL1CacheSize() {
   int l1(-1), l2, l3;
-  queryCacheSizes(l1,l2,l3);
+  queryCacheSizes(l1, l2, l3);
   return l1;
 }
 
 /** \internal
  * \returns the size in Bytes of the L2 or L3 cache if this later is present */
-inline int queryTopLevelCacheSize()
-{
+inline int queryTopLevelCacheSize() {
   int l1, l2(-1), l3(-1);
-  queryCacheSizes(l1,l2,l3);
-  return (std::max)(l2,l3);
+  queryCacheSizes(l1, l2, l3);
+  return (std::max)(l2, l3);
 }
 
-} // end namespace internal
+/** \internal
+ * This wraps C++20's std::construct_at, using placement new instead if it is not available.
+ */
 
-} // end namespace Eigen
+#if EIGEN_COMP_CXXVER >= 20
+using std::construct_at;
+#else
+template <class T, class... Args>
+EIGEN_DEVICE_FUNC T* construct_at(T* p, Args&&... args) {
+  return ::new (const_cast<void*>(static_cast<const volatile void*>(p))) T(std::forward<Args>(args)...);
+}
+#endif
 
-#endif // EIGEN_MEMORY_H
+/** \internal
+ * This wraps C++17's std::destroy_at.  If it's not available it calls the destructor.
+ * The wrapper is not a full replacement for C++20's std::destroy_at as it cannot
+ * be applied to std::array.
+ */
+#if EIGEN_COMP_CXXVER >= 17
+using std::destroy_at;
+#else
+template <class T>
+EIGEN_DEVICE_FUNC void destroy_at(T* p) {
+  p->~T();
+}
+#endif
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_MEMORY_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h
index 7badfdc..99cbf5f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h
@@ -11,55 +11,64 @@
 #ifndef EIGEN_META_H
 #define EIGEN_META_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 #if defined(EIGEN_GPU_COMPILE_PHASE)
 
- #include <cfloat>
+#include <cfloat>
 
- #if defined(EIGEN_CUDA_ARCH)
-  #include <math_constants.h>
- #endif
-
- #if defined(EIGEN_HIP_DEVICE_COMPILE)
-  // #include "Eigen/src/Core/arch/HIP/hcc/math_constants.h"
-  #endif
-
+#if defined(EIGEN_CUDA_ARCH)
+#include <math_constants.h>
 #endif
 
-// Recent versions of ICC require <cstdint> for pointer types below.
-#define EIGEN_ICC_NEEDS_CSTDINT (EIGEN_COMP_ICC>=1600 && EIGEN_COMP_CXXVER >= 11)
+#if defined(EIGEN_HIP_DEVICE_COMPILE)
+// #include "Eigen/src/Core/arch/HIP/hcc/math_constants.h"
+#endif
+
+#endif
 
 // Define portable (u)int{32,64} types
-#if EIGEN_HAS_CXX11 || EIGEN_ICC_NEEDS_CSTDINT
 #include <cstdint>
+
 namespace Eigen {
 namespace numext {
-typedef std::uint8_t  uint8_t;
-typedef std::int8_t   int8_t;
+typedef std::uint8_t uint8_t;
+typedef std::int8_t int8_t;
 typedef std::uint16_t uint16_t;
-typedef std::int16_t  int16_t;
+typedef std::int16_t int16_t;
 typedef std::uint32_t uint32_t;
-typedef std::int32_t  int32_t;
+typedef std::int32_t int32_t;
 typedef std::uint64_t uint64_t;
-typedef std::int64_t  int64_t;
-}
-}
-#else
-// Without c++11, all compilers able to compile Eigen also
-// provide the C99 stdint.h header file.
-#include <stdint.h>
-namespace Eigen {
-namespace numext {
-typedef ::uint8_t  uint8_t;
-typedef ::int8_t   int8_t;
-typedef ::uint16_t uint16_t;
-typedef ::int16_t  int16_t;
-typedef ::uint32_t uint32_t;
-typedef ::int32_t  int32_t;
-typedef ::uint64_t uint64_t;
-typedef ::int64_t  int64_t;
-}
-}
-#endif
+typedef std::int64_t int64_t;
+
+template <size_t Size>
+struct get_integer_by_size {
+  typedef void signed_type;
+  typedef void unsigned_type;
+};
+template <>
+struct get_integer_by_size<1> {
+  typedef int8_t signed_type;
+  typedef uint8_t unsigned_type;
+};
+template <>
+struct get_integer_by_size<2> {
+  typedef int16_t signed_type;
+  typedef uint16_t unsigned_type;
+};
+template <>
+struct get_integer_by_size<4> {
+  typedef int32_t signed_type;
+  typedef uint32_t unsigned_type;
+};
+template <>
+struct get_integer_by_size<8> {
+  typedef int64_t signed_type;
+  typedef uint64_t unsigned_type;
+};
+}  // namespace numext
+}  // namespace Eigen
 
 namespace Eigen {
 
@@ -76,737 +85,633 @@
 namespace internal {
 
 /** \internal
-  * \file Meta.h
-  * This file contains generic metaprogramming classes which are not specifically related to Eigen.
-  * \note In case you wonder, yes we're aware that Boost already provides all these features,
-  * we however don't want to add a dependency to Boost.
-  */
+ * \file Meta.h
+ * This file contains generic metaprogramming classes which are not specifically related to Eigen.
+ * \note In case you wonder, yes we're aware that Boost already provides all these features,
+ * we however don't want to add a dependency to Boost.
+ */
 
-// Only recent versions of ICC complain about using ptrdiff_t to hold pointers,
-// and older versions do not provide *intptr_t types.
-#if EIGEN_ICC_NEEDS_CSTDINT
-typedef std::intptr_t  IntPtr;
-typedef std::uintptr_t UIntPtr;
-#else
-typedef std::ptrdiff_t IntPtr;
-typedef std::size_t UIntPtr;
-#endif
-#undef EIGEN_ICC_NEEDS_CSTDINT
+struct true_type {
+  enum { value = 1 };
+};
+struct false_type {
+  enum { value = 0 };
+};
 
-struct true_type {  enum { value = 1 }; };
-struct false_type { enum { value = 0 }; };
-
-template<bool Condition>
+template <bool Condition>
 struct bool_constant;
 
-template<>
+template <>
 struct bool_constant<true> : true_type {};
 
-template<>
+template <>
 struct bool_constant<false> : false_type {};
 
-template<bool Condition, typename Then, typename Else>
-struct conditional { typedef Then type; };
+// Third-party libraries rely on these.
+using std::conditional;
+using std::remove_const;
+using std::remove_pointer;
+using std::remove_reference;
 
-template<typename Then, typename Else>
-struct conditional <false, Then, Else> { typedef Else type; };
+template <typename T>
+struct remove_all {
+  typedef T type;
+};
+template <typename T>
+struct remove_all<const T> {
+  typedef typename remove_all<T>::type type;
+};
+template <typename T>
+struct remove_all<T const&> {
+  typedef typename remove_all<T>::type type;
+};
+template <typename T>
+struct remove_all<T&> {
+  typedef typename remove_all<T>::type type;
+};
+template <typename T>
+struct remove_all<T const*> {
+  typedef typename remove_all<T>::type type;
+};
+template <typename T>
+struct remove_all<T*> {
+  typedef typename remove_all<T>::type type;
+};
 
-template<typename T> struct remove_reference { typedef T type; };
-template<typename T> struct remove_reference<T&> { typedef T type; };
+template <typename T>
+using remove_all_t = typename remove_all<T>::type;
 
-template<typename T> struct remove_pointer { typedef T type; };
-template<typename T> struct remove_pointer<T*> { typedef T type; };
-template<typename T> struct remove_pointer<T*const> { typedef T type; };
+template <typename T>
+struct is_arithmetic {
+  enum { value = false };
+};
+template <>
+struct is_arithmetic<float> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<double> {
+  enum { value = true };
+};
+// GPU devices treat `long double` as `double`.
+#ifndef EIGEN_GPU_COMPILE_PHASE
+template <>
+struct is_arithmetic<long double> {
+  enum { value = true };
+};
+#endif
+template <>
+struct is_arithmetic<bool> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<char> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<signed char> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<unsigned char> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<signed short> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<unsigned short> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<signed int> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<unsigned int> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<signed long> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<unsigned long> {
+  enum { value = true };
+};
 
-template <class T> struct remove_const { typedef T type; };
-template <class T> struct remove_const<const T> { typedef T type; };
-template <class T> struct remove_const<const T[]> { typedef T type[]; };
-template <class T, unsigned int Size> struct remove_const<const T[Size]> { typedef T type[Size]; };
+template <typename T, typename U>
+struct is_same {
+  enum { value = 0 };
+};
+template <typename T>
+struct is_same<T, T> {
+  enum { value = 1 };
+};
 
-template<typename T> struct remove_all { typedef T type; };
-template<typename T> struct remove_all<const T>   { typedef typename remove_all<T>::type type; };
-template<typename T> struct remove_all<T const&>  { typedef typename remove_all<T>::type type; };
-template<typename T> struct remove_all<T&>        { typedef typename remove_all<T>::type type; };
-template<typename T> struct remove_all<T const*>  { typedef typename remove_all<T>::type type; };
-template<typename T> struct remove_all<T*>        { typedef typename remove_all<T>::type type; };
+template <class T>
+struct is_void : is_same<void, std::remove_const_t<T>> {};
 
-template<typename T> struct is_arithmetic      { enum { value = false }; };
-template<> struct is_arithmetic<float>         { enum { value = true }; };
-template<> struct is_arithmetic<double>        { enum { value = true }; };
-template<> struct is_arithmetic<long double>   { enum { value = true }; };
-template<> struct is_arithmetic<bool>          { enum { value = true }; };
-template<> struct is_arithmetic<char>          { enum { value = true }; };
-template<> struct is_arithmetic<signed char>   { enum { value = true }; };
-template<> struct is_arithmetic<unsigned char> { enum { value = true }; };
-template<> struct is_arithmetic<signed short>  { enum { value = true }; };
-template<> struct is_arithmetic<unsigned short>{ enum { value = true }; };
-template<> struct is_arithmetic<signed int>    { enum { value = true }; };
-template<> struct is_arithmetic<unsigned int>  { enum { value = true }; };
-template<> struct is_arithmetic<signed long>   { enum { value = true }; };
-template<> struct is_arithmetic<unsigned long> { enum { value = true }; };
+/** \internal
+ * Implementation of std::void_t for SFINAE.
+ *
+ * Pre C++17:
+ * Custom implementation.
+ *
+ * Post C++17: Uses std::void_t
+ */
+#if EIGEN_COMP_CXXVER >= 17
+using std::void_t;
+#else
+template <typename...>
+using void_t = void;
+#endif
 
-template<typename T, typename U> struct is_same { enum { value = 0 }; };
-template<typename T> struct is_same<T,T> { enum { value = 1 }; };
-
-template< class T >
-struct is_void : is_same<void, typename remove_const<T>::type> {};
-
-#if EIGEN_HAS_CXX11
-template<> struct is_arithmetic<signed long long>   { enum { value = true }; };
-template<> struct is_arithmetic<unsigned long long> { enum { value = true }; };
+template <>
+struct is_arithmetic<signed long long> {
+  enum { value = true };
+};
+template <>
+struct is_arithmetic<unsigned long long> {
+  enum { value = true };
+};
 using std::is_integral;
-#else
-template<typename T> struct is_integral               { enum { value = false }; };
-template<> struct is_integral<bool>                   { enum { value = true }; };
-template<> struct is_integral<char>                   { enum { value = true }; };
-template<> struct is_integral<signed char>            { enum { value = true }; };
-template<> struct is_integral<unsigned char>          { enum { value = true }; };
-template<> struct is_integral<signed short>           { enum { value = true }; };
-template<> struct is_integral<unsigned short>         { enum { value = true }; };
-template<> struct is_integral<signed int>             { enum { value = true }; };
-template<> struct is_integral<unsigned int>           { enum { value = true }; };
-template<> struct is_integral<signed long>            { enum { value = true }; };
-template<> struct is_integral<unsigned long>          { enum { value = true }; };
-#if EIGEN_COMP_MSVC
-template<> struct is_integral<signed __int64>         { enum { value = true }; };
-template<> struct is_integral<unsigned __int64>       { enum { value = true }; };
-#endif
-#endif
 
-#if EIGEN_HAS_CXX11
 using std::make_unsigned;
-#else
-// TODO: Possibly improve this implementation of make_unsigned.
-// It is currently used only by
-// template<typename Scalar> struct random_default_impl<Scalar, false, true>.
-template<typename> struct make_unsigned;
-template<> struct make_unsigned<char>             { typedef unsigned char type; };
-template<> struct make_unsigned<signed char>      { typedef unsigned char type; };
-template<> struct make_unsigned<unsigned char>    { typedef unsigned char type; };
-template<> struct make_unsigned<signed short>     { typedef unsigned short type; };
-template<> struct make_unsigned<unsigned short>   { typedef unsigned short type; };
-template<> struct make_unsigned<signed int>       { typedef unsigned int type; };
-template<> struct make_unsigned<unsigned int>     { typedef unsigned int type; };
-template<> struct make_unsigned<signed long>      { typedef unsigned long type; };
-template<> struct make_unsigned<unsigned long>    { typedef unsigned long type; };
-#if EIGEN_COMP_MSVC
-template<> struct make_unsigned<signed __int64>   { typedef unsigned __int64 type; };
-template<> struct make_unsigned<unsigned __int64> { typedef unsigned __int64 type; };
-#endif
 
-// Some platforms define int64_t as `long long` even for C++03, where
-// `long long` is not guaranteed by the standard. In this case we are missing
-// the definition for make_unsigned. If we just define it, we run into issues
-// where `long long` doesn't exist in some compilers for C++03. We therefore add
-// the specialization for these platforms only.
-#if EIGEN_OS_MAC || EIGEN_COMP_MINGW
-template<> struct make_unsigned<unsigned long long> { typedef unsigned long long type; };
-template<> struct make_unsigned<long long>          { typedef unsigned long long type; };
-#endif
-#endif
+template <typename T>
+struct is_const {
+  enum { value = 0 };
+};
+template <typename T>
+struct is_const<T const> {
+  enum { value = 1 };
+};
 
-template <typename T> struct add_const { typedef const T type; };
-template <typename T> struct add_const<T&> { typedef T& type; };
+template <typename T>
+struct add_const_on_value_type {
+  typedef const T type;
+};
+template <typename T>
+struct add_const_on_value_type<T&> {
+  typedef T const& type;
+};
+template <typename T>
+struct add_const_on_value_type<T*> {
+  typedef T const* type;
+};
+template <typename T>
+struct add_const_on_value_type<T* const> {
+  typedef T const* const type;
+};
+template <typename T>
+struct add_const_on_value_type<T const* const> {
+  typedef T const* const type;
+};
 
-template <typename T> struct is_const { enum { value = 0 }; };
-template <typename T> struct is_const<T const> { enum { value = 1 }; };
-
-template<typename T> struct add_const_on_value_type            { typedef const T type;  };
-template<typename T> struct add_const_on_value_type<T&>        { typedef T const& type; };
-template<typename T> struct add_const_on_value_type<T*>        { typedef T const* type; };
-template<typename T> struct add_const_on_value_type<T* const>  { typedef T const* const type; };
-template<typename T> struct add_const_on_value_type<T const* const>  { typedef T const* const type; };
-
-#if EIGEN_HAS_CXX11
+template <typename T>
+using add_const_on_value_type_t = typename add_const_on_value_type<T>::type;
 
 using std::is_convertible;
 
-#else
-
-template<typename From, typename To>
-struct is_convertible_impl
-{
-private:
-  struct any_conversion
-  {
-    template <typename T> any_conversion(const volatile T&);
-    template <typename T> any_conversion(T&);
-  };
-  struct yes {int a[1];};
-  struct no  {int a[2];};
-
-  template<typename T>
-  static yes test(T, int);
-
-  template<typename T>
-  static no  test(any_conversion, ...);
-
-public:
-  static typename internal::remove_reference<From>::type* ms_from;
-#ifdef __INTEL_COMPILER
-  #pragma warning push
-  #pragma warning ( disable : 2259 )
-#endif
-  enum { value = sizeof(test<To>(*ms_from, 0))==sizeof(yes) };
-#ifdef __INTEL_COMPILER
-  #pragma warning pop
-#endif
-};
-
-template<typename From, typename To>
-struct is_convertible
-{
-  enum { value = is_convertible_impl<From,To>::value };
-};
-
-template<typename T>
-struct is_convertible<T,T&> { enum { value = false }; };
-
-template<typename T>
-struct is_convertible<const T,const T&> { enum { value = true }; };
-
-#endif
-
-/** \internal Allows to enable/disable an overload
-  * according to a compile time condition.
-  */
-template<bool Condition, typename T=void> struct enable_if;
-
-template<typename T> struct enable_if<true,T>
-{ typedef T type; };
-
-#if defined(EIGEN_GPU_COMPILE_PHASE) && !EIGEN_HAS_CXX11
-#if !defined(__FLT_EPSILON__)
-#define __FLT_EPSILON__ FLT_EPSILON
-#define __DBL_EPSILON__ DBL_EPSILON
-#endif
-
-namespace device {
-
-template<typename T> struct numeric_limits
-{
-  EIGEN_DEVICE_FUNC
-  static EIGEN_CONSTEXPR T epsilon() { return 0; }
-  static T (max)() { assert(false && "Highest not supported for this type"); }
-  static T (min)() { assert(false && "Lowest not supported for this type"); }
-  static T infinity() { assert(false && "Infinity not supported for this type"); }
-  static T quiet_NaN() { assert(false && "quiet_NaN not supported for this type"); }
-};
-template<> struct numeric_limits<float>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static float epsilon() { return __FLT_EPSILON__; }
-  EIGEN_DEVICE_FUNC
-  static float (max)() {
-  #if defined(EIGEN_CUDA_ARCH)
-    return CUDART_MAX_NORMAL_F;
-  #else
-    return HIPRT_MAX_NORMAL_F;
-  #endif
-  }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static float (min)() { return FLT_MIN; }
-  EIGEN_DEVICE_FUNC
-  static float infinity() {
-  #if defined(EIGEN_CUDA_ARCH)
-    return CUDART_INF_F;
-  #else
-    return HIPRT_INF_F;
-  #endif
-  }
-  EIGEN_DEVICE_FUNC
-  static float quiet_NaN() {
-  #if defined(EIGEN_CUDA_ARCH)
-    return CUDART_NAN_F;
-  #else
-    return HIPRT_NAN_F;
-  #endif
-  }
-};
-template<> struct numeric_limits<double>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static double epsilon() { return __DBL_EPSILON__; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static double (max)() { return DBL_MAX; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static double (min)() { return DBL_MIN; }
-  EIGEN_DEVICE_FUNC
-  static double infinity() {
-  #if defined(EIGEN_CUDA_ARCH)
-    return CUDART_INF;
-  #else
-    return HIPRT_INF;
-  #endif
-  }
-  EIGEN_DEVICE_FUNC
-  static double quiet_NaN() {
-  #if defined(EIGEN_CUDA_ARCH)
-    return CUDART_NAN;
-  #else
-    return HIPRT_NAN;
-  #endif
-  }
-};
-template<> struct numeric_limits<int>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int (max)() { return INT_MAX; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static int (min)() { return INT_MIN; }
-};
-template<> struct numeric_limits<unsigned int>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned int epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned int (max)() { return UINT_MAX; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned int (min)() { return 0; }
-};
-template<> struct numeric_limits<long>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static long (max)() { return LONG_MAX; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static long (min)() { return LONG_MIN; }
-};
-template<> struct numeric_limits<unsigned long>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned long (max)() { return ULONG_MAX; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned long (min)() { return 0; }
-};
-template<> struct numeric_limits<long long>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static long long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static long long (max)() { return LLONG_MAX; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static long long (min)() { return LLONG_MIN; }
-};
-template<> struct numeric_limits<unsigned long long>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned long long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned long long (max)() { return ULLONG_MAX; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static unsigned long long (min)() { return 0; }
-};
-template<> struct numeric_limits<bool>
-{
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static bool epsilon() { return false; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  static bool (max)() { return true; }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR 
-  static bool (min)() { return false; }
-};
-
-}
-
-#endif // defined(EIGEN_GPU_COMPILE_PHASE) && !EIGEN_HAS_CXX11
-
 /** \internal
-  * A base class do disable default copy ctor and copy assignment operator.
-  */
-class noncopyable
-{
+ * A base class do disable default copy ctor and copy assignment operator.
+ */
+class noncopyable {
   EIGEN_DEVICE_FUNC noncopyable(const noncopyable&);
   EIGEN_DEVICE_FUNC const noncopyable& operator=(const noncopyable&);
-protected:
+
+ protected:
   EIGEN_DEVICE_FUNC noncopyable() {}
   EIGEN_DEVICE_FUNC ~noncopyable() {}
 };
 
 /** \internal
-  * Provides access to the number of elements in the object of as a compile-time constant expression.
-  * It "returns" Eigen::Dynamic if the size cannot be resolved at compile-time (default).
-  *
-  * Similar to std::tuple_size, but more general.
-  *
-  * It currently supports:
-  *  - any types T defining T::SizeAtCompileTime
-  *  - plain C arrays as T[N]
-  *  - std::array (c++11)
-  *  - some internal types such as SingleRange and AllRange
-  *
-  * The second template parameter eases SFINAE-based specializations.
-  */
-template<typename T, typename EnableIf = void> struct array_size {
+ * Provides access to the number of elements in the object of as a compile-time constant expression.
+ * It "returns" Eigen::Dynamic if the size cannot be resolved at compile-time (default).
+ *
+ * Similar to std::tuple_size, but more general.
+ *
+ * It currently supports:
+ *  - any types T defining T::SizeAtCompileTime
+ *  - plain C arrays as T[N]
+ *  - std::array (c++11)
+ *  - some internal types such as SingleRange and AllRange
+ *
+ * The second template parameter eases SFINAE-based specializations.
+ */
+template <typename T, typename EnableIf = void>
+struct array_size {
   enum { value = Dynamic };
 };
 
-template<typename T> struct array_size<T,typename internal::enable_if<((T::SizeAtCompileTime&0)==0)>::type> {
+template <typename T>
+struct array_size<T, std::enable_if_t<((T::SizeAtCompileTime & 0) == 0)>> {
   enum { value = T::SizeAtCompileTime };
 };
 
-template<typename T, int N> struct array_size<const T (&)[N]> {
+template <typename T, int N>
+struct array_size<const T (&)[N]> {
   enum { value = N };
 };
-template<typename T, int N> struct array_size<T (&)[N]> {
+template <typename T, int N>
+struct array_size<T (&)[N]> {
   enum { value = N };
 };
 
-#if EIGEN_HAS_CXX11
-template<typename T, std::size_t N> struct array_size<const std::array<T,N> > {
+template <typename T, std::size_t N>
+struct array_size<const std::array<T, N>> {
   enum { value = N };
 };
-template<typename T, std::size_t N> struct array_size<std::array<T,N> > {
+template <typename T, std::size_t N>
+struct array_size<std::array<T, N>> {
   enum { value = N };
 };
-#endif
 
 /** \internal
-  * Analogue of the std::size free function.
-  * It returns the size of the container or view \a x of type \c T
-  *
-  * It currently supports:
-  *  - any types T defining a member T::size() const
-  *  - plain C arrays as T[N]
-  *
-  */
-template<typename T>
-EIGEN_CONSTEXPR Index size(const T& x) { return x.size(); }
+ * Analogue of the std::ssize free function.
+ * It returns the signed size of the container or view \a x of type \c T
+ *
+ * It currently supports:
+ *  - any types T defining a member T::size() const
+ *  - plain C arrays as T[N]
+ *
+ * For C++20, this function just forwards to `std::ssize`, or any ADL discoverable `ssize` function.
+ */
+#if EIGEN_COMP_CXXVER < 20 || EIGEN_GNUC_STRICT_LESS_THAN(10, 0, 0)
+template <typename T>
+EIGEN_CONSTEXPR auto index_list_size(const T& x) {
+  using R = std::common_type_t<std::ptrdiff_t, std::make_signed_t<decltype(x.size())>>;
+  return static_cast<R>(x.size());
+}
 
-template<typename T,std::size_t N>
-EIGEN_CONSTEXPR Index size(const T (&) [N]) { return N; }
+template <typename T, std::ptrdiff_t N>
+EIGEN_CONSTEXPR std::ptrdiff_t index_list_size(const T (&)[N]) {
+  return N;
+}
+#else
+template <typename T>
+EIGEN_CONSTEXPR auto index_list_size(T&& x) {
+  using std::ssize;
+  return ssize(std::forward<T>(x));
+}
+#endif  // EIGEN_COMP_CXXVER
 
 /** \internal
-  * Convenient struct to get the result type of a nullary, unary, binary, or
-  * ternary functor.
-  * 
-  * Pre C++11:
-  * Supports both a Func::result_type member and templated
-  * Func::result<Func(ArgTypes...)>::type member.
-  * 
-  * If none of these members is provided, then the type of the first
-  * argument is returned.
-  * 
-  * Post C++11:
-  * This uses std::result_of. However, note the `type` member removes
-  * const and converts references/pointers to their corresponding value type.
-  */
+ * Convenient struct to get the result type of a nullary, unary, binary, or
+ * ternary functor.
+ *
+ * Pre C++17:
+ * This uses std::result_of. However, note the `type` member removes
+ * const and converts references/pointers to their corresponding value type.
+ *
+ * Post C++17: Uses std::invoke_result
+ */
 #if EIGEN_HAS_STD_INVOKE_RESULT
-template<typename T> struct result_of;
+template <typename T>
+struct result_of;
 
-template<typename F, typename... ArgTypes>
+template <typename F, typename... ArgTypes>
 struct result_of<F(ArgTypes...)> {
   typedef typename std::invoke_result<F, ArgTypes...>::type type1;
-  typedef typename remove_all<type1>::type type;
-};
-#elif EIGEN_HAS_STD_RESULT_OF
-template<typename T> struct result_of {
-  typedef typename std::result_of<T>::type type1;
-  typedef typename remove_all<type1>::type type;
-};
-#else
-template<typename T> struct result_of { };
-
-struct has_none {int a[1];};
-struct has_std_result_type {int a[2];};
-struct has_tr1_result {int a[3];};
-
-template<typename Func, int SizeOf>
-struct nullary_result_of_select {};
-
-template<typename Func>
-struct nullary_result_of_select<Func, sizeof(has_std_result_type)> {typedef typename Func::result_type type;};
-
-template<typename Func>
-struct nullary_result_of_select<Func, sizeof(has_tr1_result)> {typedef typename Func::template result<Func()>::type type;};
-
-template<typename Func>
-struct result_of<Func()> {
-    template<typename T>
-    static has_std_result_type    testFunctor(T const *, typename T::result_type const * = 0);
-    template<typename T>
-    static has_tr1_result         testFunctor(T const *, typename T::template result<T()>::type const * = 0);
-    static has_none               testFunctor(...);
-
-    // note that the following indirection is needed for gcc-3.3
-    enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
-    typedef typename nullary_result_of_select<Func, FunctorType>::type type;
+  typedef remove_all_t<type1> type;
 };
 
-template<typename Func, typename ArgType, int SizeOf=sizeof(has_none)>
-struct unary_result_of_select {typedef typename internal::remove_all<ArgType>::type type;};
-
-template<typename Func, typename ArgType>
-struct unary_result_of_select<Func, ArgType, sizeof(has_std_result_type)> {typedef typename Func::result_type type;};
-
-template<typename Func, typename ArgType>
-struct unary_result_of_select<Func, ArgType, sizeof(has_tr1_result)> {typedef typename Func::template result<Func(ArgType)>::type type;};
-
-template<typename Func, typename ArgType>
-struct result_of<Func(ArgType)> {
-    template<typename T>
-    static has_std_result_type    testFunctor(T const *, typename T::result_type const * = 0);
-    template<typename T>
-    static has_tr1_result         testFunctor(T const *, typename T::template result<T(ArgType)>::type const * = 0);
-    static has_none               testFunctor(...);
-
-    // note that the following indirection is needed for gcc-3.3
-    enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
-    typedef typename unary_result_of_select<Func, ArgType, FunctorType>::type type;
-};
-
-template<typename Func, typename ArgType0, typename ArgType1, int SizeOf=sizeof(has_none)>
-struct binary_result_of_select {typedef typename internal::remove_all<ArgType0>::type type;};
-
-template<typename Func, typename ArgType0, typename ArgType1>
-struct binary_result_of_select<Func, ArgType0, ArgType1, sizeof(has_std_result_type)>
-{typedef typename Func::result_type type;};
-
-template<typename Func, typename ArgType0, typename ArgType1>
-struct binary_result_of_select<Func, ArgType0, ArgType1, sizeof(has_tr1_result)>
-{typedef typename Func::template result<Func(ArgType0,ArgType1)>::type type;};
-
-template<typename Func, typename ArgType0, typename ArgType1>
-struct result_of<Func(ArgType0,ArgType1)> {
-    template<typename T>
-    static has_std_result_type    testFunctor(T const *, typename T::result_type const * = 0);
-    template<typename T>
-    static has_tr1_result         testFunctor(T const *, typename T::template result<T(ArgType0,ArgType1)>::type const * = 0);
-    static has_none               testFunctor(...);
-
-    // note that the following indirection is needed for gcc-3.3
-    enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
-    typedef typename binary_result_of_select<Func, ArgType0, ArgType1, FunctorType>::type type;
-};
-
-template<typename Func, typename ArgType0, typename ArgType1, typename ArgType2, int SizeOf=sizeof(has_none)>
-struct ternary_result_of_select {typedef typename internal::remove_all<ArgType0>::type type;};
-
-template<typename Func, typename ArgType0, typename ArgType1, typename ArgType2>
-struct ternary_result_of_select<Func, ArgType0, ArgType1, ArgType2, sizeof(has_std_result_type)>
-{typedef typename Func::result_type type;};
-
-template<typename Func, typename ArgType0, typename ArgType1, typename ArgType2>
-struct ternary_result_of_select<Func, ArgType0, ArgType1, ArgType2, sizeof(has_tr1_result)>
-{typedef typename Func::template result<Func(ArgType0,ArgType1,ArgType2)>::type type;};
-
-template<typename Func, typename ArgType0, typename ArgType1, typename ArgType2>
-struct result_of<Func(ArgType0,ArgType1,ArgType2)> {
-    template<typename T>
-    static has_std_result_type    testFunctor(T const *, typename T::result_type const * = 0);
-    template<typename T>
-    static has_tr1_result         testFunctor(T const *, typename T::template result<T(ArgType0,ArgType1,ArgType2)>::type const * = 0);
-    static has_none               testFunctor(...);
-
-    // note that the following indirection is needed for gcc-3.3
-    enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
-    typedef typename ternary_result_of_select<Func, ArgType0, ArgType1, ArgType2, FunctorType>::type type;
-};
-
-#endif
-
-#if EIGEN_HAS_STD_INVOKE_RESULT
-template<typename F, typename... ArgTypes>
+template <typename F, typename... ArgTypes>
 struct invoke_result {
   typedef typename std::invoke_result<F, ArgTypes...>::type type1;
-  typedef typename remove_all<type1>::type type;
-};
-#elif EIGEN_HAS_CXX11
-template<typename F, typename... ArgTypes>
-struct invoke_result {
-  typedef typename result_of<F(ArgTypes...)>::type type1;
-  typedef typename remove_all<type1>::type type;
+  typedef remove_all_t<type1> type;
 };
 #else
-template<typename F, typename ArgType0 = void, typename ArgType1 = void, typename ArgType2 = void>
+template <typename T>
+struct result_of {
+  typedef typename std::result_of<T>::type type1;
+  typedef remove_all_t<type1> type;
+};
+
+template <typename F, typename... ArgTypes>
 struct invoke_result {
-  typedef typename result_of<F(ArgType0, ArgType1, ArgType2)>::type type1;
-  typedef typename remove_all<type1>::type type;
-};
-
-template<typename F>
-struct invoke_result<F, void, void, void> {
-  typedef typename result_of<F()>::type type1;
-  typedef typename remove_all<type1>::type type;
-};
-
-template<typename F, typename ArgType0>
-struct invoke_result<F, ArgType0, void, void> {
-  typedef typename result_of<F(ArgType0)>::type type1;
-  typedef typename remove_all<type1>::type type;
-};
-
-template<typename F, typename ArgType0, typename ArgType1>
-struct invoke_result<F, ArgType0, ArgType1, void> {
-  typedef typename result_of<F(ArgType0, ArgType1)>::type type1;
-  typedef typename remove_all<type1>::type type;
+  typedef typename result_of<F(ArgTypes...)>::type type1;
+  typedef remove_all_t<type1> type;
 };
 #endif
 
-struct meta_yes { char a[1]; };
-struct meta_no  { char a[2]; };
+// Reduces a sequence of bools to true if all are true, false otherwise.
+template <bool... values>
+using reduce_all =
+    std::is_same<std::integer_sequence<bool, values..., true>, std::integer_sequence<bool, true, values...>>;
+
+// Reduces a sequence of bools to true if any are true, false if all false.
+template <bool... values>
+using reduce_any = std::integral_constant<bool, !std::is_same<std::integer_sequence<bool, values..., false>,
+                                                              std::integer_sequence<bool, false, values...>>::value>;
+
+struct meta_yes {
+  char a[1];
+};
+struct meta_no {
+  char a[2];
+};
 
 // Check whether T::ReturnType does exist
 template <typename T>
-struct has_ReturnType
-{
-  template <typename C> static meta_yes testFunctor(C const *, typename C::ReturnType const * = 0);
-  template <typename C> static meta_no  testFunctor(...);
+struct has_ReturnType {
+  template <typename C>
+  static meta_yes testFunctor(C const*, typename C::ReturnType const* = 0);
+  template <typename C>
+  static meta_no testFunctor(...);
 
   enum { value = sizeof(testFunctor<T>(static_cast<T*>(0))) == sizeof(meta_yes) };
 };
 
-template<typename T> const T* return_ptr();
+template <typename T>
+const T* return_ptr();
 
-template <typename T, typename IndexType=Index>
-struct has_nullary_operator
-{
-  template <typename C> static meta_yes testFunctor(C const *,typename enable_if<(sizeof(return_ptr<C>()->operator()())>0)>::type * = 0);
+template <typename T, typename IndexType = Index>
+struct has_nullary_operator {
+  template <typename C>
+  static meta_yes testFunctor(C const*, std::enable_if_t<(sizeof(return_ptr<C>()->operator()()) > 0)>* = 0);
   static meta_no testFunctor(...);
 
   enum { value = sizeof(testFunctor(static_cast<T*>(0))) == sizeof(meta_yes) };
 };
 
-template <typename T, typename IndexType=Index>
-struct has_unary_operator
-{
-  template <typename C> static meta_yes testFunctor(C const *,typename enable_if<(sizeof(return_ptr<C>()->operator()(IndexType(0)))>0)>::type * = 0);
+template <typename T, typename IndexType = Index>
+struct has_unary_operator {
+  template <typename C>
+  static meta_yes testFunctor(C const*, std::enable_if_t<(sizeof(return_ptr<C>()->operator()(IndexType(0))) > 0)>* = 0);
   static meta_no testFunctor(...);
 
   enum { value = sizeof(testFunctor(static_cast<T*>(0))) == sizeof(meta_yes) };
 };
 
-template <typename T, typename IndexType=Index>
-struct has_binary_operator
-{
-  template <typename C> static meta_yes testFunctor(C const *,typename enable_if<(sizeof(return_ptr<C>()->operator()(IndexType(0),IndexType(0)))>0)>::type * = 0);
+template <typename T, typename IndexType = Index>
+struct has_binary_operator {
+  template <typename C>
+  static meta_yes testFunctor(
+      C const*, std::enable_if_t<(sizeof(return_ptr<C>()->operator()(IndexType(0), IndexType(0))) > 0)>* = 0);
   static meta_no testFunctor(...);
 
   enum { value = sizeof(testFunctor(static_cast<T*>(0))) == sizeof(meta_yes) };
 };
 
 /** \internal In short, it computes int(sqrt(\a Y)) with \a Y an integer.
-  * Usage example: \code meta_sqrt<1023>::ret \endcode
-  */
-template<int Y,
-         int InfX = 0,
-         int SupX = ((Y==1) ? 1 : Y/2),
-         bool Done = ((SupX-InfX)<=1 ? true : ((SupX*SupX <= Y) && ((SupX+1)*(SupX+1) > Y))) >
-                                // use ?: instead of || just to shut up a stupid gcc 4.3 warning
-class meta_sqrt
-{
-    enum {
-      MidX = (InfX+SupX)/2,
-      TakeInf = MidX*MidX > Y ? 1 : 0,
-      NewInf = int(TakeInf) ? InfX : int(MidX),
-      NewSup = int(TakeInf) ? int(MidX) : SupX
-    };
-  public:
-    enum { ret = meta_sqrt<Y,NewInf,NewSup>::ret };
+ * Usage example: \code meta_sqrt<1023>::ret \endcode
+ */
+template <int Y, int InfX = 0, int SupX = ((Y == 1) ? 1 : Y / 2),
+          bool Done = ((SupX - InfX) <= 1 || ((SupX * SupX <= Y) && ((SupX + 1) * (SupX + 1) > Y)))>
+class meta_sqrt {
+  enum {
+    MidX = (InfX + SupX) / 2,
+    TakeInf = MidX * MidX > Y ? 1 : 0,
+    NewInf = int(TakeInf) ? InfX : int(MidX),
+    NewSup = int(TakeInf) ? int(MidX) : SupX
+  };
+
+ public:
+  enum { ret = meta_sqrt<Y, NewInf, NewSup>::ret };
 };
 
-template<int Y, int InfX, int SupX>
-class meta_sqrt<Y, InfX, SupX, true> { public:  enum { ret = (SupX*SupX <= Y) ? SupX : InfX }; };
-
+template <int Y, int InfX, int SupX>
+class meta_sqrt<Y, InfX, SupX, true> {
+ public:
+  enum { ret = (SupX * SupX <= Y) ? SupX : InfX };
+};
 
 /** \internal Computes the least common multiple of two positive integer A and B
-  * at compile-time. 
-  */
-template<int A, int B, int K=1, bool Done = ((A*K)%B)==0, bool Big=(A>=B)>
-struct meta_least_common_multiple
-{
-  enum { ret = meta_least_common_multiple<A,B,K+1>::ret };
+ * at compile-time.
+ */
+template <int A, int B, int K = 1, bool Done = ((A * K) % B) == 0, bool Big = (A >= B)>
+struct meta_least_common_multiple {
+  enum { ret = meta_least_common_multiple<A, B, K + 1>::ret };
 };
-template<int A, int B, int K, bool Done>
-struct meta_least_common_multiple<A,B,K,Done,false>
-{
-  enum { ret = meta_least_common_multiple<B,A,K>::ret };
+template <int A, int B, int K, bool Done>
+struct meta_least_common_multiple<A, B, K, Done, false> {
+  enum { ret = meta_least_common_multiple<B, A, K>::ret };
 };
-template<int A, int B, int K>
-struct meta_least_common_multiple<A,B,K,true,true>
-{
-  enum { ret = A*K };
+template <int A, int B, int K>
+struct meta_least_common_multiple<A, B, K, true, true> {
+  enum { ret = A * K };
 };
 
-
 /** \internal determines whether the product of two numeric types is allowed and what the return type is */
-template<typename T, typename U> struct scalar_product_traits
-{
+template <typename T, typename U>
+struct scalar_product_traits {
   enum { Defined = 0 };
 };
 
 // FIXME quick workaround around current limitation of result_of
 // template<typename Scalar, typename ArgType0, typename ArgType1>
 // struct result_of<scalar_product_op<Scalar>(ArgType0,ArgType1)> {
-// typedef typename scalar_product_traits<typename remove_all<ArgType0>::type, typename remove_all<ArgType1>::type>::ReturnType type;
+// typedef typename scalar_product_traits<remove_all_t<ArgType0>, remove_all_t<ArgType1>>::ReturnType type;
 // };
 
 /** \internal Obtains a POD type suitable to use as storage for an object of a size
-  * of at most Len bytes, aligned as specified by \c Align.
-  */
-template<unsigned Len, unsigned Align>
+ * of at most Len bytes, aligned as specified by \c Align.
+ */
+template <unsigned Len, unsigned Align>
 struct aligned_storage {
   struct type {
     EIGEN_ALIGN_TO_BOUNDARY(Align) unsigned char data[Len];
   };
 };
 
-} // end namespace internal
+}  // end namespace internal
+
+template <typename T>
+struct NumTraits;
 
 namespace numext {
 
 #if defined(EIGEN_GPU_COMPILE_PHASE)
-template<typename T> EIGEN_DEVICE_FUNC   void swap(T &a, T &b) { T tmp = b; b = a; a = tmp; }
-#else
-template<typename T> EIGEN_STRONG_INLINE void swap(T &a, T &b) { std::swap(a,b); }
-#endif
-
-#if defined(EIGEN_GPU_COMPILE_PHASE) && !EIGEN_HAS_CXX11
-using internal::device::numeric_limits;
-#else
-using std::numeric_limits;
-#endif
-
-// Integer division with rounding up.
-// T is assumed to be an integer type with a>=0, and b>0
-template<typename T>
-EIGEN_DEVICE_FUNC
-T div_ceil(const T &a, const T &b)
-{
-  return (a+b-1) / b;
+template <typename T>
+EIGEN_DEVICE_FUNC void swap(T& a, T& b) {
+  T tmp = b;
+  b = a;
+  a = tmp;
 }
+#else
+template <typename T>
+EIGEN_STRONG_INLINE void swap(T& a, T& b) {
+  std::swap(a, b);
+}
+#endif
+
+using std::numeric_limits;
+
+// Handle integer comparisons of different signedness.
+template <typename X, typename Y, bool XIsInteger = NumTraits<X>::IsInteger, bool XIsSigned = NumTraits<X>::IsSigned,
+          bool YIsInteger = NumTraits<Y>::IsInteger, bool YIsSigned = NumTraits<Y>::IsSigned>
+struct equal_strict_impl {
+  static EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool run(const X& x, const Y& y) { return x == y; }
+};
+template <typename X, typename Y>
+struct equal_strict_impl<X, Y, true, false, true, true> {
+  // X is an unsigned integer
+  // Y is a signed integer
+  // if Y is non-negative, it may be represented exactly as its unsigned counterpart.
+  using UnsignedY = typename internal::make_unsigned<Y>::type;
+  static EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool run(const X& x, const Y& y) {
+    return y < Y(0) ? false : (x == static_cast<UnsignedY>(y));
+  }
+};
+template <typename X, typename Y>
+struct equal_strict_impl<X, Y, true, true, true, false> {
+  // X is a signed integer
+  // Y is an unsigned integer
+  static EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool run(const X& x, const Y& y) {
+    return equal_strict_impl<Y, X>::run(y, x);
+  }
+};
 
 // The aim of the following functions is to bypass -Wfloat-equal warnings
 // when we really want a strict equality comparison on floating points.
-template<typename X, typename Y> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-bool equal_strict(const X& x,const Y& y) { return x == y; }
+template <typename X, typename Y>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool equal_strict(const X& x, const Y& y) {
+  return equal_strict_impl<X, Y>::run(x, y);
+}
 
 #if !defined(EIGEN_GPU_COMPILE_PHASE) || (!defined(EIGEN_CUDA_ARCH) && defined(EIGEN_CONSTEXPR_ARE_DEVICE_FUNC))
-template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-bool equal_strict(const float& x,const float& y) { return std::equal_to<float>()(x,y); }
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool equal_strict(const float& x, const float& y) {
+  return std::equal_to<float>()(x, y);
+}
 
-template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-bool equal_strict(const double& x,const double& y) { return std::equal_to<double>()(x,y); }
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool equal_strict(const double& x, const double& y) {
+  return std::equal_to<double>()(x, y);
+}
 #endif
 
-template<typename X, typename Y> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-bool not_equal_strict(const X& x,const Y& y) { return x != y; }
+/**
+ * \internal Performs an exact comparison of x to zero, e.g. to decide whether a term can be ignored.
+ * Use this to to bypass -Wfloat-equal warnings when exact zero is what needs to be tested.
+ */
+template <typename X>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool is_exactly_zero(const X& x) {
+  return equal_strict(x, typename NumTraits<X>::Literal{0});
+}
+
+/**
+ * \internal Performs an exact comparison of x to one, e.g. to decide whether a factor needs to be multiplied.
+ * Use this to to bypass -Wfloat-equal warnings when exact one is what needs to be tested.
+ */
+template <typename X>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool is_exactly_one(const X& x) {
+  return equal_strict(x, typename NumTraits<X>::Literal{1});
+}
+
+template <typename X, typename Y>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool not_equal_strict(const X& x, const Y& y) {
+  return !equal_strict_impl<X, Y>::run(x, y);
+}
 
 #if !defined(EIGEN_GPU_COMPILE_PHASE) || (!defined(EIGEN_CUDA_ARCH) && defined(EIGEN_CONSTEXPR_ARE_DEVICE_FUNC))
-template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-bool not_equal_strict(const float& x,const float& y) { return std::not_equal_to<float>()(x,y); }
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool not_equal_strict(const float& x, const float& y) {
+  return std::not_equal_to<float>()(x, y);
+}
 
-template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
-bool not_equal_strict(const double& x,const double& y) { return std::not_equal_to<double>()(x,y); }
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool not_equal_strict(const double& x, const double& y) {
+  return std::not_equal_to<double>()(x, y);
+}
 #endif
 
-} // end namespace numext
+}  // end namespace numext
 
-} // end namespace Eigen
+namespace internal {
 
-#endif // EIGEN_META_H
+template <typename Scalar>
+struct is_identically_zero_impl {
+  static inline bool run(const Scalar& s) { return numext::is_exactly_zero(s); }
+};
+
+template <typename Scalar>
+EIGEN_STRONG_INLINE bool is_identically_zero(const Scalar& s) {
+  return is_identically_zero_impl<Scalar>::run(s);
+}
+
+/// \internal Returns true if its argument is of integer or enum type.
+/// FIXME this has the same purpose as `is_valid_index_type` in XprHelper.h
+template <typename A>
+constexpr bool is_int_or_enum_v = std::is_enum<A>::value || std::is_integral<A>::value;
+
+/// \internal Gets the minimum of two values which may be integers or enums
+template <typename A, typename B>
+inline constexpr int plain_enum_min(A a, B b) {
+  static_assert(is_int_or_enum_v<A>, "Argument a must be an integer or enum");
+  static_assert(is_int_or_enum_v<B>, "Argument b must be an integer or enum");
+  return ((int)a <= (int)b) ? (int)a : (int)b;
+}
+
+/// \internal Gets the maximum of two values which may be integers or enums
+template <typename A, typename B>
+inline constexpr int plain_enum_max(A a, B b) {
+  static_assert(is_int_or_enum_v<A>, "Argument a must be an integer or enum");
+  static_assert(is_int_or_enum_v<B>, "Argument b must be an integer or enum");
+  return ((int)a >= (int)b) ? (int)a : (int)b;
+}
+
+/**
+ * \internal
+ *  `min_size_prefer_dynamic` gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
+ *  followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
+ *  finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
+ */
+template <typename A, typename B>
+inline constexpr int min_size_prefer_dynamic(A a, B b) {
+  static_assert(is_int_or_enum_v<A>, "Argument a must be an integer or enum");
+  static_assert(is_int_or_enum_v<B>, "Argument b must be an integer or enum");
+  if ((int)a == 0 || (int)b == 0) return 0;
+  if ((int)a == 1 || (int)b == 1) return 1;
+  if ((int)a == Dynamic || (int)b == Dynamic) return Dynamic;
+  return plain_enum_min(a, b);
+}
+
+/**
+ * \internal
+ *  min_size_prefer_fixed is a variant of `min_size_prefer_dynamic` comparing MaxSizes. The difference is that finite
+ * values now have priority over Dynamic, so that min(3, Dynamic) gives 3. Indeed, whatever the actual value is (between
+ * 0 and 3), it is not more than 3.
+ */
+template <typename A, typename B>
+inline constexpr int min_size_prefer_fixed(A a, B b) {
+  static_assert(is_int_or_enum_v<A>, "Argument a must be an integer or enum");
+  static_assert(is_int_or_enum_v<B>, "Argument b must be an integer or enum");
+  if ((int)a == 0 || (int)b == 0) return 0;
+  if ((int)a == 1 || (int)b == 1) return 1;
+  if ((int)a == Dynamic && (int)b == Dynamic) return Dynamic;
+  if ((int)a == Dynamic) return (int)b;
+  if ((int)b == Dynamic) return (int)a;
+  return plain_enum_min(a, b);
+}
+
+/// \internal see `min_size_prefer_fixed`. No need for a separate variant for MaxSizes here.
+template <typename A, typename B>
+inline constexpr int max_size_prefer_dynamic(A a, B b) {
+  static_assert(is_int_or_enum_v<A>, "Argument a must be an integer or enum");
+  static_assert(is_int_or_enum_v<B>, "Argument b must be an integer or enum");
+  if ((int)a == Dynamic || (int)b == Dynamic) return Dynamic;
+  return plain_enum_max(a, b);
+}
+
+/// \internal Calculate logical XOR at compile time
+inline constexpr bool logical_xor(bool a, bool b) { return a != b; }
+
+/// \internal Calculate logical IMPLIES at compile time
+inline constexpr bool check_implication(bool a, bool b) { return !a || b; }
+
+/// \internal Provide fallback for std::is_constant_evaluated for pre-C++20.
+#if EIGEN_COMP_CXXVER >= 20
+using std::is_constant_evaluated;
+#else
+constexpr bool is_constant_evaluated() { return false; }
+#endif
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_META_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h
new file mode 100644
index 0000000..2d4aeee
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h
@@ -0,0 +1,630 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MOREMETA_H
+#define EIGEN_MOREMETA_H
+
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+namespace Eigen {
+
+namespace internal {
+
+template <typename... tt>
+struct type_list {
+  constexpr static int count = sizeof...(tt);
+};
+
+template <typename t, typename... tt>
+struct type_list<t, tt...> {
+  constexpr static int count = sizeof...(tt) + 1;
+  typedef t first_type;
+};
+
+template <typename T, T... nn>
+struct numeric_list {
+  constexpr static std::size_t count = sizeof...(nn);
+};
+
+template <typename T, T n, T... nn>
+struct numeric_list<T, n, nn...> {
+  static constexpr std::size_t count = sizeof...(nn) + 1;
+  static constexpr T first_value = n;
+};
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+/* numeric list constructors
+ *
+ * equivalencies:
+ *     constructor                                              result
+ *     typename gen_numeric_list<int, 5>::type                  numeric_list<int, 0,1,2,3,4>
+ *     typename gen_numeric_list_reversed<int, 5>::type         numeric_list<int, 4,3,2,1,0>
+ *     typename gen_numeric_list_swapped_pair<int, 5,1,2>::type numeric_list<int, 0,2,1,3,4>
+ *     typename gen_numeric_list_repeated<int, 0, 5>::type      numeric_list<int, 0,0,0,0,0>
+ */
+
+template <typename T, std::size_t n, T start = 0, T... ii>
+struct gen_numeric_list : gen_numeric_list<T, n - 1, start, start + n - 1, ii...> {};
+template <typename T, T start, T... ii>
+struct gen_numeric_list<T, 0, start, ii...> {
+  typedef numeric_list<T, ii...> type;
+};
+
+template <typename T, std::size_t n, T start = 0, T... ii>
+struct gen_numeric_list_reversed : gen_numeric_list_reversed<T, n - 1, start, ii..., start + n - 1> {};
+template <typename T, T start, T... ii>
+struct gen_numeric_list_reversed<T, 0, start, ii...> {
+  typedef numeric_list<T, ii...> type;
+};
+
+template <typename T, std::size_t n, T a, T b, T start = 0, T... ii>
+struct gen_numeric_list_swapped_pair
+    : gen_numeric_list_swapped_pair<T, n - 1, a, b, start,
+                                    (start + n - 1) == a ? b : ((start + n - 1) == b ? a : (start + n - 1)), ii...> {};
+template <typename T, T a, T b, T start, T... ii>
+struct gen_numeric_list_swapped_pair<T, 0, a, b, start, ii...> {
+  typedef numeric_list<T, ii...> type;
+};
+
+template <typename T, std::size_t n, T V, T... nn>
+struct gen_numeric_list_repeated : gen_numeric_list_repeated<T, n - 1, V, V, nn...> {};
+template <typename T, T V, T... nn>
+struct gen_numeric_list_repeated<T, 0, V, nn...> {
+  typedef numeric_list<T, nn...> type;
+};
+
+/* list manipulation: concatenate */
+
+template <class a, class b>
+struct concat;
+
+template <typename... as, typename... bs>
+struct concat<type_list<as...>, type_list<bs...>> {
+  typedef type_list<as..., bs...> type;
+};
+template <typename T, T... as, T... bs>
+struct concat<numeric_list<T, as...>, numeric_list<T, bs...>> {
+  typedef numeric_list<T, as..., bs...> type;
+};
+
+template <typename... p>
+struct mconcat;
+template <typename a>
+struct mconcat<a> {
+  typedef a type;
+};
+template <typename a, typename b>
+struct mconcat<a, b> : concat<a, b> {};
+template <typename a, typename b, typename... cs>
+struct mconcat<a, b, cs...> : concat<a, typename mconcat<b, cs...>::type> {};
+
+/* list manipulation: extract slices */
+
+template <int n, typename x>
+struct take;
+template <int n, typename a, typename... as>
+struct take<n, type_list<a, as...>> : concat<type_list<a>, typename take<n - 1, type_list<as...>>::type> {};
+template <int n>
+struct take<n, type_list<>> {
+  typedef type_list<> type;
+};
+template <typename a, typename... as>
+struct take<0, type_list<a, as...>> {
+  typedef type_list<> type;
+};
+template <>
+struct take<0, type_list<>> {
+  typedef type_list<> type;
+};
+
+template <typename T, int n, T a, T... as>
+struct take<n, numeric_list<T, a, as...>>
+    : concat<numeric_list<T, a>, typename take<n - 1, numeric_list<T, as...>>::type> {};
+// XXX The following breaks in gcc-11, and is invalid anyways.
+// template<typename T, int n>               struct take<n, numeric_list<T>>           { typedef numeric_list<T> type;
+// };
+template <typename T, T a, T... as>
+struct take<0, numeric_list<T, a, as...>> {
+  typedef numeric_list<T> type;
+};
+template <typename T>
+struct take<0, numeric_list<T>> {
+  typedef numeric_list<T> type;
+};
+
+template <typename T, int n, T... ii>
+struct h_skip_helper_numeric;
+template <typename T, int n, T i, T... ii>
+struct h_skip_helper_numeric<T, n, i, ii...> : h_skip_helper_numeric<T, n - 1, ii...> {};
+template <typename T, T i, T... ii>
+struct h_skip_helper_numeric<T, 0, i, ii...> {
+  typedef numeric_list<T, i, ii...> type;
+};
+template <typename T, int n>
+struct h_skip_helper_numeric<T, n> {
+  typedef numeric_list<T> type;
+};
+template <typename T>
+struct h_skip_helper_numeric<T, 0> {
+  typedef numeric_list<T> type;
+};
+
+template <int n, typename... tt>
+struct h_skip_helper_type;
+template <int n, typename t, typename... tt>
+struct h_skip_helper_type<n, t, tt...> : h_skip_helper_type<n - 1, tt...> {};
+template <typename t, typename... tt>
+struct h_skip_helper_type<0, t, tt...> {
+  typedef type_list<t, tt...> type;
+};
+template <int n>
+struct h_skip_helper_type<n> {
+  typedef type_list<> type;
+};
+template <>
+struct h_skip_helper_type<0> {
+  typedef type_list<> type;
+};
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
+
+template <int n>
+struct h_skip {
+  template <typename T, T... ii>
+  constexpr static EIGEN_STRONG_INLINE typename h_skip_helper_numeric<T, n, ii...>::type helper(
+      numeric_list<T, ii...>) {
+    return typename h_skip_helper_numeric<T, n, ii...>::type();
+  }
+  template <typename... tt>
+  constexpr static EIGEN_STRONG_INLINE typename h_skip_helper_type<n, tt...>::type helper(type_list<tt...>) {
+    return typename h_skip_helper_type<n, tt...>::type();
+  }
+};
+
+template <int n, typename a>
+struct skip {
+  typedef decltype(h_skip<n>::helper(a())) type;
+};
+
+template <int start, int count, typename a>
+struct slice : take<count, typename skip<start, a>::type> {};
+
+/* list manipulation: retrieve single element from list */
+
+template <int n, typename x>
+struct get;
+
+template <int n, typename a, typename... as>
+struct get<n, type_list<a, as...>> : get<n - 1, type_list<as...>> {};
+template <typename a, typename... as>
+struct get<0, type_list<a, as...>> {
+  typedef a type;
+};
+
+template <typename T, int n, T a, T... as>
+struct get<n, numeric_list<T, a, as...>> : get<n - 1, numeric_list<T, as...>> {};
+template <typename T, T a, T... as>
+struct get<0, numeric_list<T, a, as...>> {
+  constexpr static T value = a;
+};
+
+template <std::size_t n, typename T, T a, T... as>
+constexpr T array_get(const numeric_list<T, a, as...>&) {
+  return get<(int)n, numeric_list<T, a, as...>>::value;
+}
+
+/* always get type, regardless of dummy; good for parameter pack expansion */
+
+template <typename T, T dummy, typename t>
+struct id_numeric {
+  typedef t type;
+};
+template <typename dummy, typename t>
+struct id_type {
+  typedef t type;
+};
+
+/* equality checking, flagged version */
+
+template <typename a, typename b>
+struct is_same_gf : is_same<a, b> {
+  constexpr static int global_flags = 0;
+};
+
+/* apply_op to list */
+
+template <bool from_left,  // false
+          template <typename, typename> class op, typename additional_param, typename... values>
+struct h_apply_op_helper {
+  typedef type_list<typename op<values, additional_param>::type...> type;
+};
+template <template <typename, typename> class op, typename additional_param, typename... values>
+struct h_apply_op_helper<true, op, additional_param, values...> {
+  typedef type_list<typename op<additional_param, values>::type...> type;
+};
+
+template <bool from_left, template <typename, typename> class op, typename additional_param>
+struct h_apply_op {
+  template <typename... values>
+  constexpr static typename h_apply_op_helper<from_left, op, additional_param, values...>::type helper(
+      type_list<values...>) {
+    return typename h_apply_op_helper<from_left, op, additional_param, values...>::type();
+  }
+};
+
+template <template <typename, typename> class op, typename additional_param, typename a>
+struct apply_op_from_left {
+  typedef decltype(h_apply_op<true, op, additional_param>::helper(a())) type;
+};
+
+template <template <typename, typename> class op, typename additional_param, typename a>
+struct apply_op_from_right {
+  typedef decltype(h_apply_op<false, op, additional_param>::helper(a())) type;
+};
+
+/* see if an element is in a list */
+
+template <template <typename, typename> class test, typename check_against, typename h_list,
+          bool last_check_positive = false>
+struct contained_in_list;
+
+template <template <typename, typename> class test, typename check_against, typename h_list>
+struct contained_in_list<test, check_against, h_list, true> {
+  constexpr static bool value = true;
+};
+
+template <template <typename, typename> class test, typename check_against, typename a, typename... as>
+struct contained_in_list<test, check_against, type_list<a, as...>, false>
+    : contained_in_list<test, check_against, type_list<as...>, test<check_against, a>::value> {};
+
+template <template <typename, typename> class test, typename check_against, typename... empty>
+struct contained_in_list<test, check_against, type_list<empty...>, false> {
+  constexpr static bool value = false;
+};
+
+/* see if an element is in a list and check for global flags */
+
+template <template <typename, typename> class test, typename check_against, typename h_list, int default_flags = 0,
+          bool last_check_positive = false, int last_check_flags = default_flags>
+struct contained_in_list_gf;
+
+template <template <typename, typename> class test, typename check_against, typename h_list, int default_flags,
+          int last_check_flags>
+struct contained_in_list_gf<test, check_against, h_list, default_flags, true, last_check_flags> {
+  constexpr static bool value = true;
+  constexpr static int global_flags = last_check_flags;
+};
+
+template <template <typename, typename> class test, typename check_against, typename a, typename... as,
+          int default_flags, int last_check_flags>
+struct contained_in_list_gf<test, check_against, type_list<a, as...>, default_flags, false, last_check_flags>
+    : contained_in_list_gf<test, check_against, type_list<as...>, default_flags, test<check_against, a>::value,
+                           test<check_against, a>::global_flags> {};
+
+template <template <typename, typename> class test, typename check_against, typename... empty, int default_flags,
+          int last_check_flags>
+struct contained_in_list_gf<test, check_against, type_list<empty...>, default_flags, false, last_check_flags> {
+  constexpr static bool value = false;
+  constexpr static int global_flags = default_flags;
+};
+
+/* generic reductions */
+
+template <typename Reducer, typename... Ts>
+struct reduce;
+
+template <typename Reducer>
+struct reduce<Reducer> {
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE int run() { return Reducer::Identity; }
+};
+
+template <typename Reducer, typename A>
+struct reduce<Reducer, A> {
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE A run(A a) { return a; }
+};
+
+template <typename Reducer, typename A, typename... Ts>
+struct reduce<Reducer, A, Ts...> {
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE auto run(A a, Ts... ts)
+      -> decltype(Reducer::run(a, reduce<Reducer, Ts...>::run(ts...))) {
+    return Reducer::run(a, reduce<Reducer, Ts...>::run(ts...));
+  }
+};
+
+/* generic binary operations */
+
+struct sum_op {
+  template <typename A, typename B>
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a + b) {
+    return a + b;
+  }
+  static constexpr int Identity = 0;
+};
+struct product_op {
+  template <typename A, typename B>
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a * b) {
+    return a * b;
+  }
+  static constexpr int Identity = 1;
+};
+
+struct logical_and_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a && b) {
+    return a && b;
+  }
+};
+struct logical_or_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a || b) {
+    return a || b;
+  }
+};
+
+struct equal_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a == b) {
+    return a == b;
+  }
+};
+struct not_equal_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a != b) {
+    return a != b;
+  }
+};
+struct lesser_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a < b) {
+    return a < b;
+  }
+};
+struct lesser_equal_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a <= b) {
+    return a <= b;
+  }
+};
+struct greater_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a > b) {
+    return a > b;
+  }
+};
+struct greater_equal_op {
+  template <typename A, typename B>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a, B b) -> decltype(a >= b) {
+    return a >= b;
+  }
+};
+
+/* generic unary operations */
+
+struct not_op {
+  template <typename A>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a) -> decltype(!a) {
+    return !a;
+  }
+};
+struct negation_op {
+  template <typename A>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a) -> decltype(-a) {
+    return -a;
+  }
+};
+struct greater_equal_zero_op {
+  template <typename A>
+  constexpr static EIGEN_STRONG_INLINE auto run(A a) -> decltype(a >= 0) {
+    return a >= 0;
+  }
+};
+
+/* reductions for lists */
+
+// using auto -> return value spec makes ICC 13.0 and 13.1 crash here, so we have to hack it
+// together in front... (13.0 doesn't work with array_prod/array_reduce/... anyway, but 13.1
+// does...
+template <typename... Ts>
+EIGEN_DEVICE_FUNC constexpr EIGEN_STRONG_INLINE decltype(reduce<product_op, Ts...>::run((*((Ts*)0))...)) arg_prod(
+    Ts... ts) {
+  return reduce<product_op, Ts...>::run(ts...);
+}
+
+template <typename... Ts>
+constexpr EIGEN_STRONG_INLINE decltype(reduce<sum_op, Ts...>::run((*((Ts*)0))...)) arg_sum(Ts... ts) {
+  return reduce<sum_op, Ts...>::run(ts...);
+}
+
+/* reverse arrays */
+
+template <typename Array, int... n>
+constexpr EIGEN_STRONG_INLINE Array h_array_reverse(Array arr, numeric_list<int, n...>) {
+  return {{array_get<sizeof...(n) - n - 1>(arr)...}};
+}
+
+template <typename T, std::size_t N>
+constexpr EIGEN_STRONG_INLINE array<T, N> array_reverse(array<T, N> arr) {
+  return h_array_reverse(arr, typename gen_numeric_list<int, N>::type());
+}
+
+/* generic array reductions */
+
+// can't reuse standard reduce() interface above because Intel's Compiler
+// *really* doesn't like it, so we just reimplement the stuff
+// (start from N - 1 and work down to 0 because specialization for
+// n == N - 1 also doesn't work in Intel's compiler, so it goes into
+// an infinite loop)
+template <typename Reducer, typename T, std::size_t N, std::size_t n = N - 1>
+struct h_array_reduce {
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE auto run(array<T, N> arr, T identity)
+      -> decltype(Reducer::run(h_array_reduce<Reducer, T, N, n - 1>::run(arr, identity), array_get<n>(arr))) {
+    return Reducer::run(h_array_reduce<Reducer, T, N, n - 1>::run(arr, identity), array_get<n>(arr));
+  }
+};
+
+template <typename Reducer, typename T, std::size_t N>
+struct h_array_reduce<Reducer, T, N, 0> {
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE T run(const array<T, N>& arr, T) { return array_get<0>(arr); }
+};
+
+template <typename Reducer, typename T>
+struct h_array_reduce<Reducer, T, 0> {
+  EIGEN_DEVICE_FUNC constexpr static EIGEN_STRONG_INLINE T run(const array<T, 0>&, T identity) { return identity; }
+};
+
+template <typename Reducer, typename T, std::size_t N>
+EIGEN_DEVICE_FUNC constexpr EIGEN_STRONG_INLINE auto array_reduce(const array<T, N>& arr, T identity)
+    -> decltype(h_array_reduce<Reducer, T, N>::run(arr, identity)) {
+  return h_array_reduce<Reducer, T, N>::run(arr, identity);
+}
+
+/* standard array reductions */
+
+template <typename T, std::size_t N>
+EIGEN_DEVICE_FUNC constexpr EIGEN_STRONG_INLINE auto array_sum(const array<T, N>& arr)
+    -> decltype(array_reduce<sum_op, T, N>(arr, static_cast<T>(0))) {
+  return array_reduce<sum_op, T, N>(arr, static_cast<T>(0));
+}
+
+template <typename T, std::size_t N>
+EIGEN_DEVICE_FUNC constexpr EIGEN_STRONG_INLINE auto array_prod(const array<T, N>& arr)
+    -> decltype(array_reduce<product_op, T, N>(arr, static_cast<T>(1))) {
+  return array_reduce<product_op, T, N>(arr, static_cast<T>(1));
+}
+
+template <typename t>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE t array_prod(const std::vector<t>& a) {
+  eigen_assert(a.size() > 0);
+  t prod = 1;
+  for (size_t i = 0; i < a.size(); ++i) {
+    prod *= a[i];
+  }
+  return prod;
+}
+
+/* zip an array */
+
+template <typename Op, typename A, typename B, std::size_t N, int... n>
+constexpr EIGEN_STRONG_INLINE array<decltype(Op::run(A(), B())), N> h_array_zip(array<A, N> a, array<B, N> b,
+                                                                                numeric_list<int, n...>) {
+  return array<decltype(Op::run(A(), B())), N>{{Op::run(array_get<n>(a), array_get<n>(b))...}};
+}
+
+template <typename Op, typename A, typename B, std::size_t N>
+constexpr EIGEN_STRONG_INLINE array<decltype(Op::run(A(), B())), N> array_zip(array<A, N> a, array<B, N> b) {
+  return h_array_zip<Op>(a, b, typename gen_numeric_list<int, N>::type());
+}
+
+/* zip an array and reduce the result */
+
+template <typename Reducer, typename Op, typename A, typename B, std::size_t N, int... n>
+constexpr EIGEN_STRONG_INLINE auto h_array_zip_and_reduce(array<A, N> a, array<B, N> b, numeric_list<int, n...>)
+    -> decltype(reduce<Reducer, typename id_numeric<int, n, decltype(Op::run(A(), B()))>::type...>::run(
+        Op::run(array_get<n>(a), array_get<n>(b))...)) {
+  return reduce<Reducer, typename id_numeric<int, n, decltype(Op::run(A(), B()))>::type...>::run(
+      Op::run(array_get<n>(a), array_get<n>(b))...);
+}
+
+template <typename Reducer, typename Op, typename A, typename B, std::size_t N>
+constexpr EIGEN_STRONG_INLINE auto array_zip_and_reduce(array<A, N> a, array<B, N> b)
+    -> decltype(h_array_zip_and_reduce<Reducer, Op, A, B, N>(a, b, typename gen_numeric_list<int, N>::type())) {
+  return h_array_zip_and_reduce<Reducer, Op, A, B, N>(a, b, typename gen_numeric_list<int, N>::type());
+}
+
+/* apply stuff to an array */
+
+template <typename Op, typename A, std::size_t N, int... n>
+constexpr EIGEN_STRONG_INLINE array<decltype(Op::run(A())), N> h_array_apply(array<A, N> a, numeric_list<int, n...>) {
+  return array<decltype(Op::run(A())), N>{{Op::run(array_get<n>(a))...}};
+}
+
+template <typename Op, typename A, std::size_t N>
+constexpr EIGEN_STRONG_INLINE array<decltype(Op::run(A())), N> array_apply(array<A, N> a) {
+  return h_array_apply<Op>(a, typename gen_numeric_list<int, N>::type());
+}
+
+/* apply stuff to an array and reduce */
+
+template <typename Reducer, typename Op, typename A, std::size_t N, int... n>
+constexpr EIGEN_STRONG_INLINE auto h_array_apply_and_reduce(array<A, N> arr, numeric_list<int, n...>)
+    -> decltype(reduce<Reducer, typename id_numeric<int, n, decltype(Op::run(A()))>::type...>::run(
+        Op::run(array_get<n>(arr))...)) {
+  return reduce<Reducer, typename id_numeric<int, n, decltype(Op::run(A()))>::type...>::run(
+      Op::run(array_get<n>(arr))...);
+}
+
+template <typename Reducer, typename Op, typename A, std::size_t N>
+constexpr EIGEN_STRONG_INLINE auto array_apply_and_reduce(array<A, N> a)
+    -> decltype(h_array_apply_and_reduce<Reducer, Op, A, N>(a, typename gen_numeric_list<int, N>::type())) {
+  return h_array_apply_and_reduce<Reducer, Op, A, N>(a, typename gen_numeric_list<int, N>::type());
+}
+
+/* repeat a value n times (and make an array out of it
+ * usage:
+ *   array<int, 16> = repeat<16>(42);
+ */
+
+template <int n>
+struct h_repeat {
+  template <typename t, int... ii>
+  constexpr static EIGEN_STRONG_INLINE array<t, n> run(t v, numeric_list<int, ii...>) {
+    return {{typename id_numeric<int, ii, t>::type(v)...}};
+  }
+};
+
+template <int n, typename t>
+constexpr array<t, n> repeat(t v) {
+  return h_repeat<n>::run(v, typename gen_numeric_list<int, n>::type());
+}
+
+/* instantiate a class by a C-style array */
+template <class InstType, typename ArrType, std::size_t N, bool Reverse, typename... Ps>
+struct h_instantiate_by_c_array;
+
+template <class InstType, typename ArrType, std::size_t N, typename... Ps>
+struct h_instantiate_by_c_array<InstType, ArrType, N, false, Ps...> {
+  static InstType run(ArrType* arr, Ps... args) {
+    return h_instantiate_by_c_array<InstType, ArrType, N - 1, false, Ps..., ArrType>::run(arr + 1, args..., arr[0]);
+  }
+};
+
+template <class InstType, typename ArrType, std::size_t N, typename... Ps>
+struct h_instantiate_by_c_array<InstType, ArrType, N, true, Ps...> {
+  static InstType run(ArrType* arr, Ps... args) {
+    return h_instantiate_by_c_array<InstType, ArrType, N - 1, false, ArrType, Ps...>::run(arr + 1, arr[0], args...);
+  }
+};
+
+template <class InstType, typename ArrType, typename... Ps>
+struct h_instantiate_by_c_array<InstType, ArrType, 0, false, Ps...> {
+  static InstType run(ArrType* arr, Ps... args) {
+    (void)arr;
+    return InstType(args...);
+  }
+};
+
+template <class InstType, typename ArrType, typename... Ps>
+struct h_instantiate_by_c_array<InstType, ArrType, 0, true, Ps...> {
+  static InstType run(ArrType* arr, Ps... args) {
+    (void)arr;
+    return InstType(args...);
+  }
+};
+
+template <class InstType, typename ArrType, std::size_t N, bool Reverse = false>
+InstType instantiate_by_c_array(ArrType* arr) {
+  return h_instantiate_by_c_array<InstType, ArrType, N, Reverse>::run(arr);
+}
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_MOREMETA_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h
index 1ce6fd1..0af5a43 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h
@@ -1,31 +1,44 @@
 #ifdef EIGEN_WARNINGS_DISABLED_2
-// "DisableStupidWarnings.h" was included twice recursively: Do not reenable warnings yet!
-#  undef EIGEN_WARNINGS_DISABLED_2
+// "DisableStupidWarnings.h" was included twice recursively: Do not re-enable warnings yet!
+#undef EIGEN_WARNINGS_DISABLED_2
 
 #elif defined(EIGEN_WARNINGS_DISABLED)
 #undef EIGEN_WARNINGS_DISABLED
 
 #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
-  #ifdef _MSC_VER
-    #pragma warning( pop )
-  #elif defined __INTEL_COMPILER
-    #pragma warning pop
-  #elif defined __clang__
-    #pragma clang diagnostic pop
-  #elif defined __GNUC__  &&  (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
-    #pragma GCC diagnostic pop
-  #endif
+#ifdef _MSC_VER
+#pragma warning(pop)
+#ifdef EIGEN_REENABLE_CXX23_DENORM_DEPRECATION_WARNING
+#undef EIGEN_REENABLE_CXX23_DENORM_DEPRECATION_WARNING
+#undef _SILENCE_CXX23_DENORM_DEPRECATION_WARNING
+#endif
 
-  #if defined __NVCC__
-//    Don't reenable the diagnostic messages, as it turns out these messages need
+#elif defined __INTEL_COMPILER
+#pragma warning pop
+#elif defined __clang__
+#pragma clang diagnostic pop
+#elif defined __GNUC__ && !defined(__FUJITSU) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
+#pragma GCC diagnostic pop
+#endif
+
+#if defined __NVCC__
+//    Don't re-enable the diagnostic messages, as it turns out these messages need
 //    to be disabled at the point of the template instantiation (i.e the user code)
 //    otherwise they'll be triggered by nvcc.
-//    #pragma diag_default code_is_unreachable
-//    #pragma diag_default initialization_not_reachable
-//    #pragma diag_default 2651
-//    #pragma diag_default 2653
-  #endif
+//    #define EIGEN_MAKE_PRAGMA(X) _Pragma(#X)
+//    #if __NVCC_DIAG_PRAGMA_SUPPORT__
+//      #define EIGEN_NV_DIAG_DEFAULT(X) EIGEN_MAKE_PRAGMA(nv_diag_default X)
+//    #else
+//      #define EIGEN_NV_DIAG_DEFAULT(X) EIGEN_MAKE_PRAGMA(diag_default X)
+//    #endif
+//    EIGEN_NV_DIAG_DEFAULT(code_is_unreachable)
+//    EIGEN_NV_DIAG_DEFAULT(initialization_not_reachable)
+//    EIGEN_NV_DIAG_DEFAULT(2651)
+//    EIGEN_NV_DIAG_DEFAULT(2653)
+//    #undef EIGEN_NV_DIAG_DEFAULT
+//    #undef EIGEN_MAKE_PRAGMA
+#endif
 
 #endif
 
-#endif // EIGEN_WARNINGS_DISABLED
+#endif  // EIGEN_WARNINGS_DISABLED
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h
index 4124321..e569408 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h
@@ -7,45 +7,45 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 #ifndef EIGEN_RESHAPED_HELPER_H
 #define EIGEN_RESHAPED_HELPER_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
-enum AutoSize_t   { AutoSize };
+enum AutoSize_t { AutoSize };
 const int AutoOrder = 2;
 
 namespace internal {
 
-template<typename SizeType,typename OtherSize, int TotalSize>
+template <typename SizeType, typename OtherSize, int TotalSize>
 struct get_compiletime_reshape_size {
   enum { value = get_fixed_value<SizeType>::value };
 };
 
-template<typename SizeType>
+template <typename SizeType>
 Index get_runtime_reshape_size(SizeType size, Index /*other*/, Index /*total*/) {
   return internal::get_runtime_value(size);
 }
 
-template<typename OtherSize, int TotalSize>
-struct get_compiletime_reshape_size<AutoSize_t,OtherSize,TotalSize> {
+template <typename OtherSize, int TotalSize>
+struct get_compiletime_reshape_size<AutoSize_t, OtherSize, TotalSize> {
   enum {
     other_size = get_fixed_value<OtherSize>::value,
-    value = (TotalSize==Dynamic || other_size==Dynamic) ? Dynamic : TotalSize / other_size };
+    value = (TotalSize == Dynamic || other_size == Dynamic) ? Dynamic : TotalSize / other_size
+  };
 };
 
-inline Index get_runtime_reshape_size(AutoSize_t /*size*/, Index other, Index total) {
-  return total/other;
+inline Index get_runtime_reshape_size(AutoSize_t /*size*/, Index other, Index total) { return total / other; }
+
+constexpr inline int get_compiletime_reshape_order(int flags, int order) {
+  return order == AutoOrder ? flags & RowMajorBit : order;
 }
 
-template<int Flags, int Order>
-struct get_compiletime_reshape_order {
-  enum { value = Order == AutoOrder ? Flags & RowMajorBit : Order };
-};
+}  // namespace internal
 
-}
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_RESHAPED_HELPER_H
+#endif  // EIGEN_RESHAPED_HELPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h
new file mode 100644
index 0000000..1e12820
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h
@@ -0,0 +1,208 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2021 The Eigen Team
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SERIALIZER_H
+#define EIGEN_SERIALIZER_H
+
+#include <type_traits>
+
+// The Serializer class encodes data into a memory buffer so it can be later
+// reconstructed. This is mainly used to send objects back-and-forth between
+// the CPU and GPU.
+
+namespace Eigen {
+
+/**
+ * Serializes an object to a memory buffer.
+ *
+ * Useful for transferring data (e.g. back-and-forth to a device).
+ */
+template <typename T, typename EnableIf = void>
+class Serializer;
+
+// Specialization for POD types.
+template <typename T>
+class Serializer<T, typename std::enable_if_t<std::is_trivial<T>::value && std::is_standard_layout<T>::value>> {
+ public:
+  /**
+   * Determines the required size of the serialization buffer for a value.
+   *
+   * \param value the value to serialize.
+   * \return the required size.
+   */
+  EIGEN_DEVICE_FUNC size_t size(const T& value) const { return sizeof(value); }
+
+  /**
+   * Serializes a value to a byte buffer.
+   * \param dest the destination buffer; if this is nullptr, does nothing.
+   * \param end the end of the destination buffer.
+   * \param value the value to serialize.
+   * \return the next memory address past the end of the serialized data.
+   */
+  EIGEN_DEVICE_FUNC uint8_t* serialize(uint8_t* dest, uint8_t* end, const T& value) {
+    if (EIGEN_PREDICT_FALSE(dest == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(dest + sizeof(value) > end)) return nullptr;
+    EIGEN_USING_STD(memcpy)
+    memcpy(dest, &value, sizeof(value));
+    return dest + sizeof(value);
+  }
+
+  /**
+   * Deserializes a value from a byte buffer.
+   * \param src the source buffer; if this is nullptr, does nothing.
+   * \param end the end of the source buffer.
+   * \param value the value to populate.
+   * \return the next unprocessed memory address; nullptr if parsing errors are detected.
+   */
+  EIGEN_DEVICE_FUNC const uint8_t* deserialize(const uint8_t* src, const uint8_t* end, T& value) const {
+    if (EIGEN_PREDICT_FALSE(src == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(src + sizeof(value) > end)) return nullptr;
+    EIGEN_USING_STD(memcpy)
+    memcpy(&value, src, sizeof(value));
+    return src + sizeof(value);
+  }
+};
+
+// Specialization for DenseBase.
+// Serializes [rows, cols, data...].
+template <typename Derived>
+class Serializer<DenseBase<Derived>, void> {
+ public:
+  typedef typename Derived::Scalar Scalar;
+
+  struct Header {
+    typename Derived::Index rows;
+    typename Derived::Index cols;
+  };
+
+  EIGEN_DEVICE_FUNC size_t size(const Derived& value) const { return sizeof(Header) + sizeof(Scalar) * value.size(); }
+
+  EIGEN_DEVICE_FUNC uint8_t* serialize(uint8_t* dest, uint8_t* end, const Derived& value) {
+    if (EIGEN_PREDICT_FALSE(dest == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(dest + size(value) > end)) return nullptr;
+    const size_t header_bytes = sizeof(Header);
+    const size_t data_bytes = sizeof(Scalar) * value.size();
+    Header header = {value.rows(), value.cols()};
+    EIGEN_USING_STD(memcpy)
+    memcpy(dest, &header, header_bytes);
+    dest += header_bytes;
+    memcpy(dest, value.data(), data_bytes);
+    return dest + data_bytes;
+  }
+
+  EIGEN_DEVICE_FUNC const uint8_t* deserialize(const uint8_t* src, const uint8_t* end, Derived& value) const {
+    if (EIGEN_PREDICT_FALSE(src == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(src + sizeof(Header) > end)) return nullptr;
+    const size_t header_bytes = sizeof(Header);
+    Header header;
+    EIGEN_USING_STD(memcpy)
+    memcpy(&header, src, header_bytes);
+    src += header_bytes;
+    const size_t data_bytes = sizeof(Scalar) * header.rows * header.cols;
+    if (EIGEN_PREDICT_FALSE(src + data_bytes > end)) return nullptr;
+    value.resize(header.rows, header.cols);
+    memcpy(value.data(), src, data_bytes);
+    return src + data_bytes;
+  }
+};
+
+template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+class Serializer<Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>>
+    : public Serializer<DenseBase<Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>>> {};
+
+template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+class Serializer<Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols>>
+    : public Serializer<DenseBase<Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols>>> {};
+
+namespace internal {
+
+// Recursive serialization implementation helper.
+template <size_t N, typename... Types>
+struct serialize_impl;
+
+template <size_t N, typename T1, typename... Ts>
+struct serialize_impl<N, T1, Ts...> {
+  using Serializer = Eigen::Serializer<typename std::decay<T1>::type>;
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t serialize_size(const T1& value, const Ts&... args) {
+    Serializer serializer;
+    size_t size = serializer.size(value);
+    return size + serialize_impl<N - 1, Ts...>::serialize_size(args...);
+  }
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE uint8_t* serialize(uint8_t* dest, uint8_t* end, const T1& value,
+                                                                  const Ts&... args) {
+    Serializer serializer;
+    dest = serializer.serialize(dest, end, value);
+    return serialize_impl<N - 1, Ts...>::serialize(dest, end, args...);
+  }
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const uint8_t* deserialize(const uint8_t* src, const uint8_t* end,
+                                                                          T1& value, Ts&... args) {
+    Serializer serializer;
+    src = serializer.deserialize(src, end, value);
+    return serialize_impl<N - 1, Ts...>::deserialize(src, end, args...);
+  }
+};
+
+// Base case.
+template <>
+struct serialize_impl<0> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t serialize_size() { return 0; }
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE uint8_t* serialize(uint8_t* dest, uint8_t* /*end*/) { return dest; }
+
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const uint8_t* deserialize(const uint8_t* src, const uint8_t* /*end*/) {
+    return src;
+  }
+};
+
+}  // namespace internal
+
+/**
+ * Determine the buffer size required to serialize a set of values.
+ *
+ * \param args ... arguments to serialize in sequence.
+ * \return the total size of the required buffer.
+ */
+template <typename... Args>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t serialize_size(const Args&... args) {
+  return internal::serialize_impl<sizeof...(args), Args...>::serialize_size(args...);
+}
+
+/**
+ * Serialize a set of values to the byte buffer.
+ *
+ * \param dest output byte buffer; if this is nullptr, does nothing.
+ * \param end the end of the output byte buffer.
+ * \param args ... arguments to serialize in sequence.
+ * \return the next address after all serialized values.
+ */
+template <typename... Args>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE uint8_t* serialize(uint8_t* dest, uint8_t* end, const Args&... args) {
+  return internal::serialize_impl<sizeof...(args), Args...>::serialize(dest, end, args...);
+}
+
+/**
+ * Deserialize a set of values from the byte buffer.
+ *
+ * \param src input byte buffer; if this is nullptr, does nothing.
+ * \param end the end of input byte buffer.
+ * \param args ... arguments to deserialize in sequence.
+ * \return the next address after all parsed values; nullptr if parsing errors are detected.
+ */
+template <typename... Args>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const uint8_t* deserialize(const uint8_t* src, const uint8_t* end,
+                                                                 Args&... args) {
+  return internal::serialize_impl<sizeof...(args), Args...>::deserialize(src, end, args...);
+}
+
+}  // namespace Eigen
+
+#endif  // EIGEN_SERIALIZER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h
index c45de59..f062354 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h
@@ -16,10 +16,6 @@
  *  - in EIGEN_STATIC_ASSERT(CONDITION,MSG) the parameter CONDITION must be a compile time boolean
  *    expression, and MSG an enum listed in struct internal::static_assertion<true>
  *
- *  - define EIGEN_NO_STATIC_ASSERT to disable them (and save compilation time)
- *    in that case, the static assertion is converted to the following runtime assert:
- *      eigen_assert(CONDITION && "MSG")
- *
  *  - currently EIGEN_STATIC_ASSERT can only be used in function scope
  *
  */
@@ -27,195 +23,83 @@
 #ifndef EIGEN_STATIC_ASSERT
 #ifndef EIGEN_NO_STATIC_ASSERT
 
-  #if EIGEN_MAX_CPP_VER>=11 && (__has_feature(cxx_static_assert) || (EIGEN_COMP_CXXVER >= 11) || (EIGEN_COMP_MSVC >= 1600))
+#define EIGEN_STATIC_ASSERT(X, MSG) static_assert(X, #MSG);
 
-    // if native static_assert is enabled, let's use it
-    #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
+#else  // EIGEN_NO_STATIC_ASSERT
 
-  #else // not CXX0X
+#define EIGEN_STATIC_ASSERT(CONDITION, MSG)
 
-    namespace Eigen {
-
-    namespace internal {
-
-    template<bool condition>
-    struct static_assertion {};
-
-    template<>
-    struct static_assertion<true>
-    {
-      enum {
-        YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX=1,
-        YOU_MIXED_VECTORS_OF_DIFFERENT_SIZES=1,
-        YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES=1,
-        THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE=1,
-        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE=1,
-        THIS_METHOD_IS_ONLY_FOR_OBJECTS_OF_A_SPECIFIC_SIZE=1,
-        OUT_OF_RANGE_ACCESS=1,
-        YOU_MADE_A_PROGRAMMING_MISTAKE=1,
-        EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT=1,
-        EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE=1,
-        YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR=1,
-        YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR=1,
-        UNALIGNED_LOAD_AND_STORE_OPERATIONS_UNIMPLEMENTED_ON_ALTIVEC=1,
-        THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES=1,
-        FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED=1,
-        NUMERIC_TYPE_MUST_BE_REAL=1,
-        COEFFICIENT_WRITE_ACCESS_TO_SELFADJOINT_NOT_SUPPORTED=1,
-        WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED=1,
-        THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE=1,
-        INVALID_MATRIX_PRODUCT=1,
-        INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS=1,
-        INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION=1,
-        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY=1,
-        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES=1,
-        THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES=1,
-        INVALID_MATRIX_TEMPLATE_PARAMETERS=1,
-        INVALID_MATRIXBASE_TEMPLATE_PARAMETERS=1,
-        BOTH_MATRICES_MUST_HAVE_THE_SAME_STORAGE_ORDER=1,
-        THIS_METHOD_IS_ONLY_FOR_DIAGONAL_MATRIX=1,
-        THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE=1,
-        THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_WITH_DIRECT_MEMORY_ACCESS_SUCH_AS_MAP_OR_PLAIN_MATRICES=1,
-        YOU_ALREADY_SPECIFIED_THIS_STRIDE=1,
-        INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION=1,
-        THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD=1,
-        PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1=1,
-        THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS=1,
-        YOU_CANNOT_MIX_ARRAYS_AND_MATRICES=1,
-        YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION=1,
-        THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY=1,
-        YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT=1,
-        THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS=1,
-        THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS=1,
-        THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL=1,
-        THIS_METHOD_IS_ONLY_FOR_ARRAYS_NOT_MATRICES=1,
-        YOU_PASSED_A_ROW_VECTOR_BUT_A_COLUMN_VECTOR_WAS_EXPECTED=1,
-        YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED=1,
-        THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE=1,
-        THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH=1,
-        OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG=1,
-        IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY=1,
-        STORAGE_LAYOUT_DOES_NOT_MATCH=1,
-        EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE=1,
-        THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS=1,
-        MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY=1,
-        THIS_TYPE_IS_NOT_SUPPORTED=1,
-        STORAGE_KIND_MUST_MATCH=1,
-        STORAGE_INDEX_MUST_MATCH=1,
-        CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY=1,
-        SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY=1,
-        INVALID_TEMPLATE_PARAMETER=1,
-        GPU_TENSOR_CONTRACTION_DOES_NOT_SUPPORT_OUTPUT_KERNELS=1,
-        THE_ARRAY_SIZE_SHOULD_EQUAL_WITH_PACKET_SIZE=1
-      };
-    };
-
-    } // end namespace internal
-
-    } // end namespace Eigen
-
-    // Specialized implementation for MSVC to avoid "conditional
-    // expression is constant" warnings.  This implementation doesn't
-    // appear to work under GCC, hence the multiple implementations.
-    #if EIGEN_COMP_MSVC
-
-      #define EIGEN_STATIC_ASSERT(CONDITION,MSG) \
-        {Eigen::internal::static_assertion<bool(CONDITION)>::MSG;}
-
-    #else
-      // In some cases clang interprets bool(CONDITION) as function declaration
-      #define EIGEN_STATIC_ASSERT(CONDITION,MSG) \
-        if (Eigen::internal::static_assertion<static_cast<bool>(CONDITION)>::MSG) {}
-
-    #endif
-
-  #endif // not CXX0X
-
-#else // EIGEN_NO_STATIC_ASSERT
-
-  #define EIGEN_STATIC_ASSERT(CONDITION,MSG) eigen_assert((CONDITION) && #MSG);
-
-#endif // EIGEN_NO_STATIC_ASSERT
-#endif // EIGEN_STATIC_ASSERT
+#endif  // EIGEN_NO_STATIC_ASSERT
+#endif  // EIGEN_STATIC_ASSERT
 
 // static assertion failing if the type \a TYPE is not a vector type
 #define EIGEN_STATIC_ASSERT_VECTOR_ONLY(TYPE) \
-  EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime, \
-                      YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX)
+  EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX)
 
 // static assertion failing if the type \a TYPE is not fixed-size
-#define EIGEN_STATIC_ASSERT_FIXED_SIZE(TYPE) \
-  EIGEN_STATIC_ASSERT(TYPE::SizeAtCompileTime!=Eigen::Dynamic, \
+#define EIGEN_STATIC_ASSERT_FIXED_SIZE(TYPE)                     \
+  EIGEN_STATIC_ASSERT(TYPE::SizeAtCompileTime != Eigen::Dynamic, \
                       YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
 
 // static assertion failing if the type \a TYPE is not dynamic-size
-#define EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(TYPE) \
-  EIGEN_STATIC_ASSERT(TYPE::SizeAtCompileTime==Eigen::Dynamic, \
+#define EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(TYPE)                   \
+  EIGEN_STATIC_ASSERT(TYPE::SizeAtCompileTime == Eigen::Dynamic, \
                       YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR)
 
 // static assertion failing if the type \a TYPE is not a vector type of the given size
-#define EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(TYPE, SIZE) \
-  EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime && TYPE::SizeAtCompileTime==SIZE, \
+#define EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(TYPE, SIZE)                         \
+  EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime&& TYPE::SizeAtCompileTime == SIZE, \
                       THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE)
 
 // static assertion failing if the type \a TYPE is not a vector type of the given size
-#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(TYPE, ROWS, COLS) \
-  EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime==ROWS && TYPE::ColsAtCompileTime==COLS, \
+#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(TYPE, ROWS, COLS)                        \
+  EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime == ROWS && TYPE::ColsAtCompileTime == COLS, \
                       THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
 
 // static assertion failing if the two vector expression types are not compatible (same fixed-size or dynamic size)
-#define EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TYPE0,TYPE1) \
-  EIGEN_STATIC_ASSERT( \
-      (int(TYPE0::SizeAtCompileTime)==Eigen::Dynamic \
-    || int(TYPE1::SizeAtCompileTime)==Eigen::Dynamic \
-    || int(TYPE0::SizeAtCompileTime)==int(TYPE1::SizeAtCompileTime)),\
-    YOU_MIXED_VECTORS_OF_DIFFERENT_SIZES)
+#define EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TYPE0, TYPE1)                                                   \
+  EIGEN_STATIC_ASSERT(                                                                                       \
+      (int(TYPE0::SizeAtCompileTime) == Eigen::Dynamic || int(TYPE1::SizeAtCompileTime) == Eigen::Dynamic || \
+       int(TYPE0::SizeAtCompileTime) == int(TYPE1::SizeAtCompileTime)),                                      \
+      YOU_MIXED_VECTORS_OF_DIFFERENT_SIZES)
 
-#define EIGEN_PREDICATE_SAME_MATRIX_SIZE(TYPE0,TYPE1) \
-     ( \
-        (int(Eigen::internal::size_of_xpr_at_compile_time<TYPE0>::ret)==0 && int(Eigen::internal::size_of_xpr_at_compile_time<TYPE1>::ret)==0) \
-    || (\
-          (int(TYPE0::RowsAtCompileTime)==Eigen::Dynamic \
-        || int(TYPE1::RowsAtCompileTime)==Eigen::Dynamic \
-        || int(TYPE0::RowsAtCompileTime)==int(TYPE1::RowsAtCompileTime)) \
-      &&  (int(TYPE0::ColsAtCompileTime)==Eigen::Dynamic \
-        || int(TYPE1::ColsAtCompileTime)==Eigen::Dynamic \
-        || int(TYPE0::ColsAtCompileTime)==int(TYPE1::ColsAtCompileTime))\
-       ) \
-     )
+#define EIGEN_PREDICATE_SAME_MATRIX_SIZE(TYPE0, TYPE1)                                                     \
+  ((int(Eigen::internal::size_of_xpr_at_compile_time<TYPE0>::ret) == 0 &&                                  \
+    int(Eigen::internal::size_of_xpr_at_compile_time<TYPE1>::ret) == 0) ||                                 \
+   ((int(TYPE0::RowsAtCompileTime) == Eigen::Dynamic || int(TYPE1::RowsAtCompileTime) == Eigen::Dynamic || \
+     int(TYPE0::RowsAtCompileTime) == int(TYPE1::RowsAtCompileTime)) &&                                    \
+    (int(TYPE0::ColsAtCompileTime) == Eigen::Dynamic || int(TYPE1::ColsAtCompileTime) == Eigen::Dynamic || \
+     int(TYPE0::ColsAtCompileTime) == int(TYPE1::ColsAtCompileTime))))
 
 #define EIGEN_STATIC_ASSERT_NON_INTEGER(TYPE) \
-    EIGEN_STATIC_ASSERT(!Eigen::NumTraits<TYPE>::IsInteger, THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
+  EIGEN_STATIC_ASSERT(!Eigen::NumTraits<TYPE>::IsInteger, THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
 
+// static assertion failing if it is guaranteed at compile-time that the two matrix expression types have different
+// sizes
+#define EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(TYPE0, TYPE1) \
+  EIGEN_STATIC_ASSERT(EIGEN_PREDICATE_SAME_MATRIX_SIZE(TYPE0, TYPE1), YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES)
 
-// static assertion failing if it is guaranteed at compile-time that the two matrix expression types have different sizes
-#define EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(TYPE0,TYPE1) \
-  EIGEN_STATIC_ASSERT( \
-     EIGEN_PREDICATE_SAME_MATRIX_SIZE(TYPE0,TYPE1),\
-    YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES)
-
-#define EIGEN_STATIC_ASSERT_SIZE_1x1(TYPE) \
-      EIGEN_STATIC_ASSERT((TYPE::RowsAtCompileTime == 1 || TYPE::RowsAtCompileTime == Eigen::Dynamic) && \
+#define EIGEN_STATIC_ASSERT_SIZE_1x1(TYPE)                                                             \
+  EIGEN_STATIC_ASSERT((TYPE::RowsAtCompileTime == 1 || TYPE::RowsAtCompileTime == Eigen::Dynamic) &&   \
                           (TYPE::ColsAtCompileTime == 1 || TYPE::ColsAtCompileTime == Eigen::Dynamic), \
-                          THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS)
+                      THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS)
 
 #define EIGEN_STATIC_ASSERT_LVALUE(Derived) \
-      EIGEN_STATIC_ASSERT(Eigen::internal::is_lvalue<Derived>::value, \
-                          THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY)
+  EIGEN_STATIC_ASSERT(Eigen::internal::is_lvalue<Derived>::value, THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY)
 
-#define EIGEN_STATIC_ASSERT_ARRAYXPR(Derived) \
-      EIGEN_STATIC_ASSERT((Eigen::internal::is_same<typename Eigen::internal::traits<Derived>::XprKind, ArrayXpr>::value), \
-                          THIS_METHOD_IS_ONLY_FOR_ARRAYS_NOT_MATRICES)
+#define EIGEN_STATIC_ASSERT_ARRAYXPR(Derived)                                                                          \
+  EIGEN_STATIC_ASSERT((Eigen::internal::is_same<typename Eigen::internal::traits<Derived>::XprKind, ArrayXpr>::value), \
+                      THIS_METHOD_IS_ONLY_FOR_ARRAYS_NOT_MATRICES)
 
-#define EIGEN_STATIC_ASSERT_SAME_XPR_KIND(Derived1, Derived2) \
-      EIGEN_STATIC_ASSERT((Eigen::internal::is_same<typename Eigen::internal::traits<Derived1>::XprKind, \
-                                             typename Eigen::internal::traits<Derived2>::XprKind \
-                                            >::value), \
-                          YOU_CANNOT_MIX_ARRAYS_AND_MATRICES)
+#define EIGEN_STATIC_ASSERT_SAME_XPR_KIND(Derived1, Derived2)                                                 \
+  EIGEN_STATIC_ASSERT((Eigen::internal::is_same<typename Eigen::internal::traits<Derived1>::XprKind,          \
+                                                typename Eigen::internal::traits<Derived2>::XprKind>::value), \
+                      YOU_CANNOT_MIX_ARRAYS_AND_MATRICES)
 
 // Check that a cost value is positive, and that is stay within a reasonable range
 // TODO this check could be enabled for internal debugging only
-#define EIGEN_INTERNAL_CHECK_COST_VALUE(C) \
-      EIGEN_STATIC_ASSERT((C)>=0 && (C)<=HugeCost*HugeCost, EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE);
+#define EIGEN_INTERNAL_CHECK_COST_VALUE(C)                    \
+  EIGEN_STATIC_ASSERT((C) >= 0 && (C) <= HugeCost * HugeCost, \
+                      EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE);
 
-#endif // EIGEN_STATIC_ASSERT_H
+#endif  // EIGEN_STATIC_ASSERT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h
index 354dd9a..136942c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h
@@ -10,284 +10,292 @@
 #ifndef EIGEN_SYMBOLIC_INDEX_H
 #define EIGEN_SYMBOLIC_INDEX_H
 
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \namespace Eigen::symbolic
-  * \ingroup Core_Module
-  *
-  * This namespace defines a set of classes and functions to build and evaluate symbolic expressions of scalar type Index.
-  * Here is a simple example:
-  *
-  * \code
-  * // First step, defines symbols:
-  * struct x_tag {};  static const symbolic::SymbolExpr<x_tag> x;
-  * struct y_tag {};  static const symbolic::SymbolExpr<y_tag> y;
-  * struct z_tag {};  static const symbolic::SymbolExpr<z_tag> z;
-  *
-  * // Defines an expression:
-  * auto expr = (x+3)/y+z;
-  *
-  * // And evaluate it: (c++14)
-  * std::cout << expr.eval(x=6,y=3,z=-13) << "\n";
-  *
-  * // In c++98/11, only one symbol per expression is supported for now:
-  * auto expr98 = (3-x)/2;
-  * std::cout << expr98.eval(x=6) << "\n";
-  * \endcode
-  *
-  * It is currently only used internally to define and manipulate the Eigen::last and Eigen::lastp1 symbols in Eigen::seq and Eigen::seqN.
-  *
-  */
+ * \ingroup Core_Module
+ *
+ * This namespace defines a set of classes and functions to build and evaluate symbolic expressions of scalar type
+ * Index. Here is a simple example:
+ *
+ * \code
+ * // First step, defines symbols:
+ * struct x_tag {};  static const symbolic::SymbolExpr<x_tag> x;
+ * struct y_tag {};  static const symbolic::SymbolExpr<y_tag> y;
+ * struct z_tag {};  static const symbolic::SymbolExpr<z_tag> z;
+ *
+ * // Defines an expression:
+ * auto expr = (x+3)/y+z;
+ *
+ * // And evaluate it: (c++14)
+ * std::cout << expr.eval(x=6,y=3,z=-13) << "\n";
+ *
+ * \endcode
+ *
+ * It is currently only used internally to define and manipulate the
+ * Eigen::placeholders::last and Eigen::placeholders::lastp1 symbols in
+ * Eigen::seq and Eigen::seqN.
+ *
+ */
 namespace symbolic {
 
-template<typename Tag> class Symbol;
-template<typename Arg0> class NegateExpr;
-template<typename Arg1,typename Arg2> class AddExpr;
-template<typename Arg1,typename Arg2> class ProductExpr;
-template<typename Arg1,typename Arg2> class QuotientExpr;
+template <typename Tag>
+class Symbol;
+template <typename Arg0>
+class NegateExpr;
+template <typename Arg1, typename Arg2>
+class AddExpr;
+template <typename Arg1, typename Arg2>
+class ProductExpr;
+template <typename Arg1, typename Arg2>
+class QuotientExpr;
 
 // A simple wrapper around an integral value to provide the eval method.
 // We could also use a free-function symbolic_eval...
-template<typename IndexType=Index>
+template <typename IndexType = Index>
 class ValueExpr {
-public:
+ public:
   ValueExpr(IndexType val) : m_value(val) {}
-  template<typename T>
-  IndexType eval_impl(const T&) const { return m_value; }
-protected:
+  template <typename T>
+  IndexType eval_impl(const T&) const {
+    return m_value;
+  }
+
+ protected:
   IndexType m_value;
 };
 
 // Specialization for compile-time value,
 // It is similar to ValueExpr(N) but this version helps the compiler to generate better code.
-template<int N>
+template <int N>
 class ValueExpr<internal::FixedInt<N> > {
-public:
+ public:
   ValueExpr() {}
-  template<typename T>
-  EIGEN_CONSTEXPR Index eval_impl(const T&) const { return N; }
+  template <typename T>
+  EIGEN_CONSTEXPR Index eval_impl(const T&) const {
+    return N;
+  }
 };
 
-
 /** \class BaseExpr
-  * \ingroup Core_Module
-  * Common base class of any symbolic expressions
-  */
-template<typename Derived>
-class BaseExpr
-{
-public:
+ * \ingroup Core_Module
+ * Common base class of any symbolic expressions
+ */
+template <typename Derived>
+class BaseExpr {
+ public:
   const Derived& derived() const { return *static_cast<const Derived*>(this); }
 
   /** Evaluate the expression given the \a values of the symbols.
-    *
-    * \param values defines the values of the symbols, it can either be a SymbolValue or a std::tuple of SymbolValue
-    *               as constructed by SymbolExpr::operator= operator.
-    *
-    */
-  template<typename T>
-  Index eval(const T& values) const { return derived().eval_impl(values); }
+   *
+   * \param values defines the values of the symbols, it can either be a SymbolValue or a std::tuple of SymbolValue
+   *               as constructed by SymbolExpr::operator= operator.
+   *
+   */
+  template <typename T>
+  Index eval(const T& values) const {
+    return derived().eval_impl(values);
+  }
 
-#if EIGEN_HAS_CXX14
-  template<typename... Types>
-  Index eval(Types&&... values) const { return derived().eval_impl(std::make_tuple(values...)); }
-#endif
+  template <typename... Types>
+  Index eval(Types&&... values) const {
+    return derived().eval_impl(std::make_tuple(values...));
+  }
 
   NegateExpr<Derived> operator-() const { return NegateExpr<Derived>(derived()); }
 
-  AddExpr<Derived,ValueExpr<> > operator+(Index b) const
-  { return AddExpr<Derived,ValueExpr<> >(derived(),  b); }
-  AddExpr<Derived,ValueExpr<> > operator-(Index a) const
-  { return AddExpr<Derived,ValueExpr<> >(derived(), -a); }
-  ProductExpr<Derived,ValueExpr<> > operator*(Index a) const
-  { return ProductExpr<Derived,ValueExpr<> >(derived(),a); }
-  QuotientExpr<Derived,ValueExpr<> > operator/(Index a) const
-  { return QuotientExpr<Derived,ValueExpr<> >(derived(),a); }
+  AddExpr<Derived, ValueExpr<> > operator+(Index b) const { return AddExpr<Derived, ValueExpr<> >(derived(), b); }
+  AddExpr<Derived, ValueExpr<> > operator-(Index a) const { return AddExpr<Derived, ValueExpr<> >(derived(), -a); }
+  ProductExpr<Derived, ValueExpr<> > operator*(Index a) const {
+    return ProductExpr<Derived, ValueExpr<> >(derived(), a);
+  }
+  QuotientExpr<Derived, ValueExpr<> > operator/(Index a) const {
+    return QuotientExpr<Derived, ValueExpr<> >(derived(), a);
+  }
 
-  friend AddExpr<Derived,ValueExpr<> > operator+(Index a, const BaseExpr& b)
-  { return AddExpr<Derived,ValueExpr<> >(b.derived(), a); }
-  friend AddExpr<NegateExpr<Derived>,ValueExpr<> > operator-(Index a, const BaseExpr& b)
-  { return AddExpr<NegateExpr<Derived>,ValueExpr<> >(-b.derived(), a); }
-  friend ProductExpr<ValueExpr<>,Derived> operator*(Index a, const BaseExpr& b)
-  { return ProductExpr<ValueExpr<>,Derived>(a,b.derived()); }
-  friend QuotientExpr<ValueExpr<>,Derived> operator/(Index a, const BaseExpr& b)
-  { return QuotientExpr<ValueExpr<>,Derived>(a,b.derived()); }
+  friend AddExpr<Derived, ValueExpr<> > operator+(Index a, const BaseExpr& b) {
+    return AddExpr<Derived, ValueExpr<> >(b.derived(), a);
+  }
+  friend AddExpr<NegateExpr<Derived>, ValueExpr<> > operator-(Index a, const BaseExpr& b) {
+    return AddExpr<NegateExpr<Derived>, ValueExpr<> >(-b.derived(), a);
+  }
+  friend ProductExpr<ValueExpr<>, Derived> operator*(Index a, const BaseExpr& b) {
+    return ProductExpr<ValueExpr<>, Derived>(a, b.derived());
+  }
+  friend QuotientExpr<ValueExpr<>, Derived> operator/(Index a, const BaseExpr& b) {
+    return QuotientExpr<ValueExpr<>, Derived>(a, b.derived());
+  }
 
-  template<int N>
-  AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N>) const
-  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(), ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > > operator-(internal::FixedInt<N>) const
-  { return AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > >(derived(), ValueExpr<internal::FixedInt<-N> >()); }
-  template<int N>
-  ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator*(internal::FixedInt<N>) const
-  { return ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator/(internal::FixedInt<N>) const
-  { return QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
+  template <int N>
+  AddExpr<Derived, ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N>) const {
+    return AddExpr<Derived, ValueExpr<internal::FixedInt<N> > >(derived(), ValueExpr<internal::FixedInt<N> >());
+  }
+  template <int N>
+  AddExpr<Derived, ValueExpr<internal::FixedInt<-N> > > operator-(internal::FixedInt<N>) const {
+    return AddExpr<Derived, ValueExpr<internal::FixedInt<-N> > >(derived(), ValueExpr<internal::FixedInt<-N> >());
+  }
+  template <int N>
+  ProductExpr<Derived, ValueExpr<internal::FixedInt<N> > > operator*(internal::FixedInt<N>) const {
+    return ProductExpr<Derived, ValueExpr<internal::FixedInt<N> > >(derived(), ValueExpr<internal::FixedInt<N> >());
+  }
+  template <int N>
+  QuotientExpr<Derived, ValueExpr<internal::FixedInt<N> > > operator/(internal::FixedInt<N>) const {
+    return QuotientExpr<Derived, ValueExpr<internal::FixedInt<N> > >(derived(), ValueExpr<internal::FixedInt<N> >());
+  }
 
-  template<int N>
-  friend AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N>, const BaseExpr& b)
-  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(b.derived(), ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  friend AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > > operator-(internal::FixedInt<N>, const BaseExpr& b)
-  { return AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > >(-b.derived(), ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  friend ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator*(internal::FixedInt<N>, const BaseExpr& b)
-  { return ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
-  template<int N>
-  friend QuotientExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator/(internal::FixedInt<N>, const BaseExpr& b)
-  { return QuotientExpr<ValueExpr<internal::FixedInt<N> > ,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
+  template <int N>
+  friend AddExpr<Derived, ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N>, const BaseExpr& b) {
+    return AddExpr<Derived, ValueExpr<internal::FixedInt<N> > >(b.derived(), ValueExpr<internal::FixedInt<N> >());
+  }
+  template <int N>
+  friend AddExpr<NegateExpr<Derived>, ValueExpr<internal::FixedInt<N> > > operator-(internal::FixedInt<N>,
+                                                                                    const BaseExpr& b) {
+    return AddExpr<NegateExpr<Derived>, ValueExpr<internal::FixedInt<N> > >(-b.derived(),
+                                                                            ValueExpr<internal::FixedInt<N> >());
+  }
+  template <int N>
+  friend ProductExpr<ValueExpr<internal::FixedInt<N> >, Derived> operator*(internal::FixedInt<N>, const BaseExpr& b) {
+    return ProductExpr<ValueExpr<internal::FixedInt<N> >, Derived>(ValueExpr<internal::FixedInt<N> >(), b.derived());
+  }
+  template <int N>
+  friend QuotientExpr<ValueExpr<internal::FixedInt<N> >, Derived> operator/(internal::FixedInt<N>, const BaseExpr& b) {
+    return QuotientExpr<ValueExpr<internal::FixedInt<N> >, Derived>(ValueExpr<internal::FixedInt<N> >(), b.derived());
+  }
 
-#if (!EIGEN_HAS_CXX14)
-  template<int N>
-  AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N> (*)()) const
-  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(), ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > > operator-(internal::FixedInt<N> (*)()) const
-  { return AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > >(derived(), ValueExpr<internal::FixedInt<-N> >()); }
-  template<int N>
-  ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator*(internal::FixedInt<N> (*)()) const
-  { return ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator/(internal::FixedInt<N> (*)()) const
-  { return QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
+  template <typename OtherDerived>
+  AddExpr<Derived, OtherDerived> operator+(const BaseExpr<OtherDerived>& b) const {
+    return AddExpr<Derived, OtherDerived>(derived(), b.derived());
+  }
 
-  template<int N>
-  friend AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N> (*)(), const BaseExpr& b)
-  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(b.derived(), ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  friend AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > > operator-(internal::FixedInt<N> (*)(), const BaseExpr& b)
-  { return AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > >(-b.derived(), ValueExpr<internal::FixedInt<N> >()); }
-  template<int N>
-  friend ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator*(internal::FixedInt<N> (*)(), const BaseExpr& b)
-  { return ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
-  template<int N>
-  friend QuotientExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator/(internal::FixedInt<N> (*)(), const BaseExpr& b)
-  { return QuotientExpr<ValueExpr<internal::FixedInt<N> > ,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
-#endif
+  template <typename OtherDerived>
+  AddExpr<Derived, NegateExpr<OtherDerived> > operator-(const BaseExpr<OtherDerived>& b) const {
+    return AddExpr<Derived, NegateExpr<OtherDerived> >(derived(), -b.derived());
+  }
 
+  template <typename OtherDerived>
+  ProductExpr<Derived, OtherDerived> operator*(const BaseExpr<OtherDerived>& b) const {
+    return ProductExpr<Derived, OtherDerived>(derived(), b.derived());
+  }
 
-  template<typename OtherDerived>
-  AddExpr<Derived,OtherDerived> operator+(const BaseExpr<OtherDerived> &b) const
-  { return AddExpr<Derived,OtherDerived>(derived(),  b.derived()); }
-
-  template<typename OtherDerived>
-  AddExpr<Derived,NegateExpr<OtherDerived> > operator-(const BaseExpr<OtherDerived> &b) const
-  { return AddExpr<Derived,NegateExpr<OtherDerived> >(derived(), -b.derived()); }
-
-  template<typename OtherDerived>
-  ProductExpr<Derived,OtherDerived> operator*(const BaseExpr<OtherDerived> &b) const
-  { return ProductExpr<Derived,OtherDerived>(derived(), b.derived()); }
-
-  template<typename OtherDerived>
-  QuotientExpr<Derived,OtherDerived> operator/(const BaseExpr<OtherDerived> &b) const
-  { return QuotientExpr<Derived,OtherDerived>(derived(), b.derived()); }
+  template <typename OtherDerived>
+  QuotientExpr<Derived, OtherDerived> operator/(const BaseExpr<OtherDerived>& b) const {
+    return QuotientExpr<Derived, OtherDerived>(derived(), b.derived());
+  }
 };
 
-template<typename T>
+template <typename T>
 struct is_symbolic {
-  // BaseExpr has no conversion ctor, so we only have to check whether T can be statically cast to its base class BaseExpr<T>.
-  enum { value = internal::is_convertible<T,BaseExpr<T> >::value };
+  // BaseExpr has no conversion ctor, so we only have to check whether T can be statically cast to its base class
+  // BaseExpr<T>.
+  enum { value = internal::is_convertible<T, BaseExpr<T> >::value };
 };
 
 /** Represents the actual value of a symbol identified by its tag
-  *
-  * It is the return type of SymbolValue::operator=, and most of the time this is only way it is used.
-  */
-template<typename Tag>
-class SymbolValue
-{
-public:
+ *
+ * It is the return type of SymbolValue::operator=, and most of the time this is only way it is used.
+ */
+template <typename Tag>
+class SymbolValue {
+ public:
   /** Default constructor from the value \a val */
   SymbolValue(Index val) : m_value(val) {}
 
   /** \returns the stored value of the symbol */
   Index value() const { return m_value; }
-protected:
+
+ protected:
   Index m_value;
 };
 
 /** Expression of a symbol uniquely identified by the template parameter type \c tag */
-template<typename tag>
-class SymbolExpr : public BaseExpr<SymbolExpr<tag> >
-{
-public:
+template <typename tag>
+class SymbolExpr : public BaseExpr<SymbolExpr<tag> > {
+ public:
   /** Alias to the template parameter \c tag */
   typedef tag Tag;
 
   SymbolExpr() {}
 
   /** Associate the value \a val to the given symbol \c *this, uniquely identified by its \c Tag.
-    *
-    * The returned object should be passed to ExprBase::eval() to evaluate a given expression with this specified runtime-time value.
-    */
-  SymbolValue<Tag> operator=(Index val) const {
-    return SymbolValue<Tag>(val);
-  }
+   *
+   * The returned object should be passed to ExprBase::eval() to evaluate a given expression with this specified
+   * runtime-time value.
+   */
+  SymbolValue<Tag> operator=(Index val) const { return SymbolValue<Tag>(val); }
 
-  Index eval_impl(const SymbolValue<Tag> &values) const { return values.value(); }
+  Index eval_impl(const SymbolValue<Tag>& values) const { return values.value(); }
 
-#if EIGEN_HAS_CXX14
   // C++14 versions suitable for multiple symbols
-  template<typename... Types>
-  Index eval_impl(const std::tuple<Types...>& values) const { return std::get<SymbolValue<Tag> >(values).value(); }
-#endif
+  template <typename... Types>
+  Index eval_impl(const std::tuple<Types...>& values) const {
+    return std::get<SymbolValue<Tag> >(values).value();
+  }
 };
 
-template<typename Arg0>
-class NegateExpr : public BaseExpr<NegateExpr<Arg0> >
-{
-public:
+template <typename Arg0>
+class NegateExpr : public BaseExpr<NegateExpr<Arg0> > {
+ public:
   NegateExpr(const Arg0& arg0) : m_arg0(arg0) {}
 
-  template<typename T>
-  Index eval_impl(const T& values) const { return -m_arg0.eval_impl(values); }
-protected:
+  template <typename T>
+  Index eval_impl(const T& values) const {
+    return -m_arg0.eval_impl(values);
+  }
+
+ protected:
   Arg0 m_arg0;
 };
 
-template<typename Arg0, typename Arg1>
-class AddExpr : public BaseExpr<AddExpr<Arg0,Arg1> >
-{
-public:
+template <typename Arg0, typename Arg1>
+class AddExpr : public BaseExpr<AddExpr<Arg0, Arg1> > {
+ public:
   AddExpr(const Arg0& arg0, const Arg1& arg1) : m_arg0(arg0), m_arg1(arg1) {}
 
-  template<typename T>
-  Index eval_impl(const T& values) const { return m_arg0.eval_impl(values) + m_arg1.eval_impl(values); }
-protected:
+  template <typename T>
+  Index eval_impl(const T& values) const {
+    return m_arg0.eval_impl(values) + m_arg1.eval_impl(values);
+  }
+
+ protected:
   Arg0 m_arg0;
   Arg1 m_arg1;
 };
 
-template<typename Arg0, typename Arg1>
-class ProductExpr : public BaseExpr<ProductExpr<Arg0,Arg1> >
-{
-public:
+template <typename Arg0, typename Arg1>
+class ProductExpr : public BaseExpr<ProductExpr<Arg0, Arg1> > {
+ public:
   ProductExpr(const Arg0& arg0, const Arg1& arg1) : m_arg0(arg0), m_arg1(arg1) {}
 
-  template<typename T>
-  Index eval_impl(const T& values) const { return m_arg0.eval_impl(values) * m_arg1.eval_impl(values); }
-protected:
+  template <typename T>
+  Index eval_impl(const T& values) const {
+    return m_arg0.eval_impl(values) * m_arg1.eval_impl(values);
+  }
+
+ protected:
   Arg0 m_arg0;
   Arg1 m_arg1;
 };
 
-template<typename Arg0, typename Arg1>
-class QuotientExpr : public BaseExpr<QuotientExpr<Arg0,Arg1> >
-{
-public:
+template <typename Arg0, typename Arg1>
+class QuotientExpr : public BaseExpr<QuotientExpr<Arg0, Arg1> > {
+ public:
   QuotientExpr(const Arg0& arg0, const Arg1& arg1) : m_arg0(arg0), m_arg1(arg1) {}
 
-  template<typename T>
-  Index eval_impl(const T& values) const { return m_arg0.eval_impl(values) / m_arg1.eval_impl(values); }
-protected:
+  template <typename T>
+  Index eval_impl(const T& values) const {
+    return m_arg0.eval_impl(values) / m_arg1.eval_impl(values);
+  }
+
+ protected:
   Arg0 m_arg0;
   Arg1 m_arg1;
 };
 
-} // end namespace symbolic
+}  // end namespace symbolic
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SYMBOLIC_INDEX_H
+#endif  // EIGEN_SYMBOLIC_INDEX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h
index 71c32b8..5b7bdc0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h
@@ -11,764 +11,936 @@
 #ifndef EIGEN_XPRHELPER_H
 #define EIGEN_XPRHELPER_H
 
-// just a workaround because GCC seems to not really like empty structs
-// FIXME: gcc 4.3 generates bad code when strict-aliasing is enabled
-// so currently we simply disable this optimization for gcc 4.3
-#if EIGEN_COMP_GNUC && !EIGEN_GNUC_AT(4,3)
-  #define EIGEN_EMPTY_STRUCT_CTOR(X) \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE X() {} \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE X(const X& ) {}
-#else
-  #define EIGEN_EMPTY_STRUCT_CTOR(X)
-#endif
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
 
 namespace Eigen {
 
 namespace internal {
 
-template<typename IndexDest, typename IndexSrc>
-EIGEN_DEVICE_FUNC
-inline IndexDest convert_index(const IndexSrc& idx) {
-  // for sizeof(IndexDest)>=sizeof(IndexSrc) compilers should be able to optimize this away:
-  eigen_internal_assert(idx <= NumTraits<IndexDest>::highest() && "Index value to big for target type");
-  return IndexDest(idx);
+// useful for unsigned / signed integer comparisons when idx is intended to be non-negative
+template <typename IndexType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename make_unsigned<IndexType>::type returnUnsignedIndexValue(
+    const IndexType& idx) {
+  EIGEN_STATIC_ASSERT((NumTraits<IndexType>::IsInteger), THIS FUNCTION IS FOR INTEGER TYPES)
+  eigen_internal_assert(idx >= 0 && "Index value is negative and target type is unsigned");
+  using UnsignedType = typename make_unsigned<IndexType>::type;
+  return static_cast<UnsignedType>(idx);
+}
+
+template <typename IndexDest, typename IndexSrc, bool IndexDestIsInteger = NumTraits<IndexDest>::IsInteger,
+          bool IndexDestIsSigned = NumTraits<IndexDest>::IsSigned,
+          bool IndexSrcIsInteger = NumTraits<IndexSrc>::IsInteger,
+          bool IndexSrcIsSigned = NumTraits<IndexSrc>::IsSigned>
+struct convert_index_impl {
+  static inline EIGEN_DEVICE_FUNC IndexDest run(const IndexSrc& idx) {
+    eigen_internal_assert(idx <= NumTraits<IndexDest>::highest() && "Index value is too big for target type");
+    return static_cast<IndexDest>(idx);
+  }
+};
+template <typename IndexDest, typename IndexSrc>
+struct convert_index_impl<IndexDest, IndexSrc, true, true, true, false> {
+  // IndexDest is a signed integer
+  // IndexSrc is an unsigned integer
+  static inline EIGEN_DEVICE_FUNC IndexDest run(const IndexSrc& idx) {
+    eigen_internal_assert(idx <= returnUnsignedIndexValue(NumTraits<IndexDest>::highest()) &&
+                          "Index value is too big for target type");
+    return static_cast<IndexDest>(idx);
+  }
+};
+template <typename IndexDest, typename IndexSrc>
+struct convert_index_impl<IndexDest, IndexSrc, true, false, true, true> {
+  // IndexDest is an unsigned integer
+  // IndexSrc is a signed integer
+  static inline EIGEN_DEVICE_FUNC IndexDest run(const IndexSrc& idx) {
+    eigen_internal_assert(returnUnsignedIndexValue(idx) <= NumTraits<IndexDest>::highest() &&
+                          "Index value is too big for target type");
+    return static_cast<IndexDest>(idx);
+  }
+};
+
+template <typename IndexDest, typename IndexSrc>
+EIGEN_DEVICE_FUNC inline IndexDest convert_index(const IndexSrc& idx) {
+  return convert_index_impl<IndexDest, IndexSrc>::run(idx);
 }
 
 // true if T can be considered as an integral index (i.e., and integral type or enum)
-template<typename T> struct is_valid_index_type
-{
-  enum { value =
-#if EIGEN_HAS_TYPE_TRAITS
-    internal::is_integral<T>::value || std::is_enum<T>::value
-#elif EIGEN_COMP_MSVC
-    internal::is_integral<T>::value || __is_enum(T)
-#else
-    // without C++11, we use is_convertible to Index instead of is_integral in order to treat enums as Index.
-    internal::is_convertible<T,Index>::value && !internal::is_same<T,float>::value && !is_same<T,double>::value
-#endif
-  };
+template <typename T>
+struct is_valid_index_type {
+  enum { value = internal::is_integral<T>::value || std::is_enum<T>::value };
 };
 
 // true if both types are not valid index types
-template<typename RowIndices, typename ColIndices>
+template <typename RowIndices, typename ColIndices>
 struct valid_indexed_view_overload {
-  enum { value = !(internal::is_valid_index_type<RowIndices>::value && internal::is_valid_index_type<ColIndices>::value) };
+  enum {
+    value = !(internal::is_valid_index_type<RowIndices>::value && internal::is_valid_index_type<ColIndices>::value)
+  };
 };
 
 // promote_scalar_arg is an helper used in operation between an expression and a scalar, like:
 //    expression * scalar
-// Its role is to determine how the type T of the scalar operand should be promoted given the scalar type ExprScalar of the given expression.
-// The IsSupported template parameter must be provided by the caller as: internal::has_ReturnType<ScalarBinaryOpTraits<ExprScalar,T,op> >::value using the proper order for ExprScalar and T.
+// Its role is to determine how the type T of the scalar operand should be promoted given the scalar type ExprScalar of
+// the given expression. The IsSupported template parameter must be provided by the caller as:
+// internal::has_ReturnType<ScalarBinaryOpTraits<ExprScalar,T,op> >::value using the proper order for ExprScalar and T.
 // Then the logic is as follows:
-//  - if the operation is natively supported as defined by IsSupported, then the scalar type is not promoted, and T is returned.
-//  - otherwise, NumTraits<ExprScalar>::Literal is returned if T is implicitly convertible to NumTraits<ExprScalar>::Literal AND that this does not imply a float to integer conversion.
-//  - otherwise, ExprScalar is returned if T is implicitly convertible to ExprScalar AND that this does not imply a float to integer conversion.
-//  - In all other cases, the promoted type is not defined, and the respective operation is thus invalid and not available (SFINAE).
-template<typename ExprScalar,typename T, bool IsSupported>
+//  - if the operation is natively supported as defined by IsSupported, then the scalar type is not promoted, and T is
+//  returned.
+//  - otherwise, NumTraits<ExprScalar>::Literal is returned if T is implicitly convertible to
+//  NumTraits<ExprScalar>::Literal AND that this does not imply a float to integer conversion.
+//  - otherwise, ExprScalar is returned if T is implicitly convertible to ExprScalar AND that this does not imply a
+//  float to integer conversion.
+//  - In all other cases, the promoted type is not defined, and the respective operation is thus invalid and not
+//  available (SFINAE).
+template <typename ExprScalar, typename T, bool IsSupported>
 struct promote_scalar_arg;
 
-template<typename S,typename T>
-struct promote_scalar_arg<S,T,true>
-{
+template <typename S, typename T>
+struct promote_scalar_arg<S, T, true> {
   typedef T type;
 };
 
 // Recursively check safe conversion to PromotedType, and then ExprScalar if they are different.
-template<typename ExprScalar,typename T,typename PromotedType,
-  bool ConvertibleToLiteral = internal::is_convertible<T,PromotedType>::value,
-  bool IsSafe = NumTraits<T>::IsInteger || !NumTraits<PromotedType>::IsInteger>
+template <typename ExprScalar, typename T, typename PromotedType,
+          bool ConvertibleToLiteral = internal::is_convertible<T, PromotedType>::value,
+          bool IsSafe = NumTraits<T>::IsInteger || !NumTraits<PromotedType>::IsInteger>
 struct promote_scalar_arg_unsupported;
 
 // Start recursion with NumTraits<ExprScalar>::Literal
-template<typename S,typename T>
-struct promote_scalar_arg<S,T,false> : promote_scalar_arg_unsupported<S,T,typename NumTraits<S>::Literal> {};
+template <typename S, typename T>
+struct promote_scalar_arg<S, T, false> : promote_scalar_arg_unsupported<S, T, typename NumTraits<S>::Literal> {};
 
 // We found a match!
-template<typename S,typename T, typename PromotedType>
-struct promote_scalar_arg_unsupported<S,T,PromotedType,true,true>
-{
+template <typename S, typename T, typename PromotedType>
+struct promote_scalar_arg_unsupported<S, T, PromotedType, true, true> {
   typedef PromotedType type;
 };
 
 // No match, but no real-to-integer issues, and ExprScalar and current PromotedType are different,
 // so let's try to promote to ExprScalar
-template<typename ExprScalar,typename T, typename PromotedType>
-struct promote_scalar_arg_unsupported<ExprScalar,T,PromotedType,false,true>
-   : promote_scalar_arg_unsupported<ExprScalar,T,ExprScalar>
-{};
+template <typename ExprScalar, typename T, typename PromotedType>
+struct promote_scalar_arg_unsupported<ExprScalar, T, PromotedType, false, true>
+    : promote_scalar_arg_unsupported<ExprScalar, T, ExprScalar> {};
 
 // Unsafe real-to-integer, let's stop.
-template<typename S,typename T, typename PromotedType, bool ConvertibleToLiteral>
-struct promote_scalar_arg_unsupported<S,T,PromotedType,ConvertibleToLiteral,false> {};
+template <typename S, typename T, typename PromotedType, bool ConvertibleToLiteral>
+struct promote_scalar_arg_unsupported<S, T, PromotedType, ConvertibleToLiteral, false> {};
 
 // T is not even convertible to ExprScalar, let's stop.
-template<typename S,typename T>
-struct promote_scalar_arg_unsupported<S,T,S,false,true> {};
+template <typename S, typename T>
+struct promote_scalar_arg_unsupported<S, T, S, false, true> {};
 
-//classes inheriting no_assignment_operator don't generate a default operator=.
-class no_assignment_operator
-{
-  private:
-    no_assignment_operator& operator=(const no_assignment_operator&);
-  protected:
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(no_assignment_operator)
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(no_assignment_operator)
+// classes inheriting no_assignment_operator don't generate a default operator=.
+class no_assignment_operator {
+ private:
+  no_assignment_operator& operator=(const no_assignment_operator&);
+
+ protected:
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(no_assignment_operator)
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(no_assignment_operator)
 };
 
 /** \internal return the index type with the largest number of bits */
-template<typename I1, typename I2>
-struct promote_index_type
-{
-  typedef typename conditional<(sizeof(I1)<sizeof(I2)), I2, I1>::type type;
+template <typename I1, typename I2>
+struct promote_index_type {
+  typedef std::conditional_t<(sizeof(I1) < sizeof(I2)), I2, I1> type;
 };
 
 /** \internal If the template parameter Value is Dynamic, this class is just a wrapper around a T variable that
-  * can be accessed using value() and setValue().
-  * Otherwise, this class is an empty structure and value() just returns the template parameter Value.
-  */
-template<typename T, int Value> class variable_if_dynamic
-{
-  public:
-    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(variable_if_dynamic)
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
-    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    T value() { return T(Value); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    operator T() const { return T(Value); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void setValue(T v) const { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
+ * can be accessed using value() and setValue().
+ * Otherwise, this class is an empty structure and value() just returns the template parameter Value.
+ */
+template <typename T, int Value>
+class variable_if_dynamic {
+ public:
+  EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(variable_if_dynamic)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T v) {
+    EIGEN_ONLY_USED_FOR_DEBUG(v);
+    eigen_assert(v == T(Value));
+  }
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR T value() { return T(Value); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR operator T() const { return T(Value); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T v) const {
+    EIGEN_ONLY_USED_FOR_DEBUG(v);
+    eigen_assert(v == T(Value));
+  }
 };
 
-template<typename T> class variable_if_dynamic<T, Dynamic>
-{
-    T m_value;
-  public:
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value = 0) EIGEN_NO_THROW : m_value(value) {}
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T value() const { return m_value; }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator T() const { return m_value; }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
+template <typename T>
+class variable_if_dynamic<T, Dynamic> {
+  T m_value;
+
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value = 0) EIGEN_NO_THROW : m_value(value) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T value() const { return m_value; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator T() const { return m_value; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
 };
 
 /** \internal like variable_if_dynamic but for DynamicIndex
-  */
-template<typename T, int Value> class variable_if_dynamicindex
-{
-  public:
-    EIGEN_EMPTY_STRUCT_CTOR(variable_if_dynamicindex)
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
-    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-    T value() { return T(Value); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    void setValue(T) {}
+ */
+template <typename T, int Value>
+class variable_if_dynamicindex {
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T v) {
+    EIGEN_ONLY_USED_FOR_DEBUG(v);
+    eigen_assert(v == T(Value));
+  }
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR T value() { return T(Value); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {}
 };
 
-template<typename T> class variable_if_dynamicindex<T, DynamicIndex>
-{
-    T m_value;
-    EIGEN_DEVICE_FUNC variable_if_dynamicindex() { eigen_assert(false); }
-  public:
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T value) : m_value(value) {}
-    EIGEN_DEVICE_FUNC T EIGEN_STRONG_INLINE value() const { return m_value; }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
+template <typename T>
+class variable_if_dynamicindex<T, DynamicIndex> {
+  T m_value;
+  EIGEN_DEVICE_FUNC variable_if_dynamicindex() { eigen_assert(false); }
+
+ public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T value) : m_value(value) {}
+  EIGEN_DEVICE_FUNC T EIGEN_STRONG_INLINE value() const { return m_value; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
 };
 
-template<typename T> struct functor_traits
-{
-  enum
-  {
-    Cost = 10,
-    PacketAccess = false,
-    IsRepeatable = false
-  };
+template <typename T>
+struct functor_traits {
+  enum { Cost = 10, PacketAccess = false, IsRepeatable = false };
 };
 
-template<typename T> struct packet_traits;
+template <typename T>
+struct packet_traits;
 
-template<typename T> struct unpacket_traits;
+template <typename T>
+struct unpacket_traits;
 
-template<int Size, typename PacketType,
-         bool Stop = Size==Dynamic || (Size%unpacket_traits<PacketType>::size)==0 || is_same<PacketType,typename unpacket_traits<PacketType>::half>::value>
+template <int Size, typename PacketType,
+          bool Stop = Size == Dynamic || (Size % unpacket_traits<PacketType>::size) == 0 ||
+                      is_same<PacketType, typename unpacket_traits<PacketType>::half>::value>
 struct find_best_packet_helper;
 
-template< int Size, typename PacketType>
-struct find_best_packet_helper<Size,PacketType,true>
-{
+template <int Size, typename PacketType>
+struct find_best_packet_helper<Size, PacketType, true> {
   typedef PacketType type;
 };
 
-template<int Size, typename PacketType>
-struct find_best_packet_helper<Size,PacketType,false>
-{
-  typedef typename find_best_packet_helper<Size,typename unpacket_traits<PacketType>::half>::type type;
+template <int Size, typename PacketType>
+struct find_best_packet_helper<Size, PacketType, false> {
+  typedef typename find_best_packet_helper<Size, typename unpacket_traits<PacketType>::half>::type type;
 };
 
-template<typename T, int Size>
-struct find_best_packet
-{
-  typedef typename find_best_packet_helper<Size,typename packet_traits<T>::type>::type type;
+template <typename T, int Size>
+struct find_best_packet {
+  typedef typename find_best_packet_helper<Size, typename packet_traits<T>::type>::type type;
 };
 
-#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
-template<int ArrayBytes, int AlignmentBytes,
-         bool Match     =  bool((ArrayBytes%AlignmentBytes)==0),
-         bool TryHalf   =  bool(EIGEN_MIN_ALIGN_BYTES<AlignmentBytes) >
-struct compute_default_alignment_helper
-{
-  enum { value = 0 };
+template <int Size, typename PacketType,
+          bool Stop = (Size == unpacket_traits<PacketType>::size) ||
+                      is_same<PacketType, typename unpacket_traits<PacketType>::half>::value>
+struct find_packet_by_size_helper;
+template <int Size, typename PacketType>
+struct find_packet_by_size_helper<Size, PacketType, true> {
+  using type = PacketType;
+};
+template <int Size, typename PacketType>
+struct find_packet_by_size_helper<Size, PacketType, false> {
+  using type = typename find_packet_by_size_helper<Size, typename unpacket_traits<PacketType>::half>::type;
 };
 
-template<int ArrayBytes, int AlignmentBytes, bool TryHalf>
-struct compute_default_alignment_helper<ArrayBytes, AlignmentBytes, true, TryHalf> // Match
-{
-  enum { value = AlignmentBytes };
+template <typename T, int Size>
+struct find_packet_by_size {
+  using type = typename find_packet_by_size_helper<Size, typename packet_traits<T>::type>::type;
+  static constexpr bool value = (Size == unpacket_traits<type>::size);
+};
+template <typename T>
+struct find_packet_by_size<T, 1> {
+  using type = typename unpacket_traits<T>::type;
+  static constexpr bool value = (unpacket_traits<type>::size == 1);
 };
 
-template<int ArrayBytes, int AlignmentBytes>
-struct compute_default_alignment_helper<ArrayBytes, AlignmentBytes, false, true> // Try-half
-{
-  // current packet too large, try with an half-packet
-  enum { value = compute_default_alignment_helper<ArrayBytes, AlignmentBytes/2>::value };
-};
+#if EIGEN_MAX_STATIC_ALIGN_BYTES > 0
+constexpr inline int compute_default_alignment_helper(int ArrayBytes, int AlignmentBytes) {
+  if ((ArrayBytes % AlignmentBytes) == 0) {
+    return AlignmentBytes;
+  } else if (EIGEN_MIN_ALIGN_BYTES < AlignmentBytes) {
+    return compute_default_alignment_helper(ArrayBytes, AlignmentBytes / 2);
+  } else {
+    return 0;
+  }
+}
 #else
 // If static alignment is disabled, no need to bother.
-// This also avoids a division by zero in "bool Match =  bool((ArrayBytes%AlignmentBytes)==0)"
-template<int ArrayBytes, int AlignmentBytes>
-struct compute_default_alignment_helper
-{
-  enum { value = 0 };
-};
+// This also avoids a division by zero
+constexpr inline int compute_default_alignment_helper(int ArrayBytes, int AlignmentBytes) {
+  EIGEN_UNUSED_VARIABLE(ArrayBytes);
+  EIGEN_UNUSED_VARIABLE(AlignmentBytes);
+  return 0;
+}
 #endif
 
-template<typename T, int Size> struct compute_default_alignment {
-  enum { value = compute_default_alignment_helper<Size*sizeof(T),EIGEN_MAX_STATIC_ALIGN_BYTES>::value };
+template <typename T, int Size>
+struct compute_default_alignment {
+  enum { value = compute_default_alignment_helper(Size * sizeof(T), EIGEN_MAX_STATIC_ALIGN_BYTES) };
 };
 
-template<typename T> struct compute_default_alignment<T,Dynamic> {
+template <typename T>
+struct compute_default_alignment<T, Dynamic> {
   enum { value = EIGEN_MAX_ALIGN_BYTES };
 };
 
-template<typename _Scalar, int _Rows, int _Cols,
-         int _Options = AutoAlign |
-                          ( (_Rows==1 && _Cols!=1) ? RowMajor
-                          : (_Cols==1 && _Rows!=1) ? ColMajor
-                          : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
-         int _MaxRows = _Rows,
-         int _MaxCols = _Cols
-> class make_proper_matrix_type
-{
-    enum {
-      IsColVector = _Cols==1 && _Rows!=1,
-      IsRowVector = _Rows==1 && _Cols!=1,
-      Options = IsColVector ? (_Options | ColMajor) & ~RowMajor
-              : IsRowVector ? (_Options | RowMajor) & ~ColMajor
-              : _Options
-    };
-  public:
-    typedef Matrix<_Scalar, _Rows, _Cols, Options, _MaxRows, _MaxCols> type;
+template <typename Scalar_, int Rows_, int Cols_,
+          int Options_ = AutoAlign | ((Rows_ == 1 && Cols_ != 1)   ? RowMajor
+                                      : (Cols_ == 1 && Rows_ != 1) ? ColMajor
+                                                                   : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
+          int MaxRows_ = Rows_, int MaxCols_ = Cols_>
+class make_proper_matrix_type {
+  enum {
+    IsColVector = Cols_ == 1 && Rows_ != 1,
+    IsRowVector = Rows_ == 1 && Cols_ != 1,
+    Options = IsColVector   ? (Options_ | ColMajor) & ~RowMajor
+              : IsRowVector ? (Options_ | RowMajor) & ~ColMajor
+                            : Options_
+  };
+
+ public:
+  typedef Matrix<Scalar_, Rows_, Cols_, Options, MaxRows_, MaxCols_> type;
 };
 
-template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-class compute_matrix_flags
-{
-    enum { row_major_bit = Options&RowMajor ? RowMajorBit : 0 };
-  public:
-    // FIXME currently we still have to handle DirectAccessBit at the expression level to handle DenseCoeffsBase<>
-    // and then propagate this information to the evaluator's flags.
-    // However, I (Gael) think that DirectAccessBit should only matter at the evaluation stage.
-    enum { ret = DirectAccessBit | LvalueBit | NestByRefBit | row_major_bit };
-};
+constexpr inline unsigned compute_matrix_flags(int Options) {
+  unsigned row_major_bit = Options & RowMajor ? RowMajorBit : 0;
+  // FIXME currently we still have to handle DirectAccessBit at the expression level to handle DenseCoeffsBase<>
+  // and then propagate this information to the evaluator's flags.
+  // However, I (Gael) think that DirectAccessBit should only matter at the evaluation stage.
+  return DirectAccessBit | LvalueBit | NestByRefBit | row_major_bit;
+}
 
-template<int _Rows, int _Cols> struct size_at_compile_time
-{
-  enum { ret = (_Rows==Dynamic || _Cols==Dynamic) ? Dynamic : _Rows * _Cols };
-};
+constexpr inline int size_at_compile_time(int rows, int cols) {
+  if (rows == 0 || cols == 0) return 0;
+  if (rows == Dynamic || cols == Dynamic) return Dynamic;
+  return rows * cols;
+}
 
-template<typename XprType> struct size_of_xpr_at_compile_time
-{
-  enum { ret = size_at_compile_time<traits<XprType>::RowsAtCompileTime,traits<XprType>::ColsAtCompileTime>::ret };
+template <typename XprType>
+struct size_of_xpr_at_compile_time {
+  enum { ret = size_at_compile_time(traits<XprType>::RowsAtCompileTime, traits<XprType>::ColsAtCompileTime) };
 };
 
 /* plain_matrix_type : the difference from eval is that plain_matrix_type is always a plain matrix type,
  * whereas eval is a const reference in the case of a matrix
  */
 
-template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct plain_matrix_type;
-template<typename T, typename BaseClassType, int Flags> struct plain_matrix_type_dense;
-template<typename T> struct plain_matrix_type<T,Dense>
-{
-  typedef typename plain_matrix_type_dense<T,typename traits<T>::XprKind, traits<T>::Flags>::type type;
+template <typename T, typename StorageKind = typename traits<T>::StorageKind>
+struct plain_matrix_type;
+template <typename T, typename BaseClassType, int Flags>
+struct plain_matrix_type_dense;
+template <typename T>
+struct plain_matrix_type<T, Dense> {
+  typedef typename plain_matrix_type_dense<T, typename traits<T>::XprKind, traits<T>::Flags>::type type;
 };
-template<typename T> struct plain_matrix_type<T,DiagonalShape>
-{
+template <typename T>
+struct plain_matrix_type<T, DiagonalShape> {
   typedef typename T::PlainObject type;
 };
 
-template<typename T, int Flags> struct plain_matrix_type_dense<T,MatrixXpr,Flags>
-{
-  typedef Matrix<typename traits<T>::Scalar,
-                traits<T>::RowsAtCompileTime,
-                traits<T>::ColsAtCompileTime,
-                AutoAlign | (Flags&RowMajorBit ? RowMajor : ColMajor),
-                traits<T>::MaxRowsAtCompileTime,
-                traits<T>::MaxColsAtCompileTime
-          > type;
+template <typename T>
+struct plain_matrix_type<T, SkewSymmetricShape> {
+  typedef typename T::PlainObject type;
 };
 
-template<typename T, int Flags> struct plain_matrix_type_dense<T,ArrayXpr,Flags>
-{
-  typedef Array<typename traits<T>::Scalar,
-                traits<T>::RowsAtCompileTime,
-                traits<T>::ColsAtCompileTime,
-                AutoAlign | (Flags&RowMajorBit ? RowMajor : ColMajor),
-                traits<T>::MaxRowsAtCompileTime,
-                traits<T>::MaxColsAtCompileTime
-          > type;
+template <typename T, int Flags>
+struct plain_matrix_type_dense<T, MatrixXpr, Flags> {
+  typedef Matrix<typename traits<T>::Scalar, traits<T>::RowsAtCompileTime, traits<T>::ColsAtCompileTime,
+                 AutoAlign | (Flags & RowMajorBit ? RowMajor : ColMajor), traits<T>::MaxRowsAtCompileTime,
+                 traits<T>::MaxColsAtCompileTime>
+      type;
+};
+
+template <typename T, int Flags>
+struct plain_matrix_type_dense<T, ArrayXpr, Flags> {
+  typedef Array<typename traits<T>::Scalar, traits<T>::RowsAtCompileTime, traits<T>::ColsAtCompileTime,
+                AutoAlign | (Flags & RowMajorBit ? RowMajor : ColMajor), traits<T>::MaxRowsAtCompileTime,
+                traits<T>::MaxColsAtCompileTime>
+      type;
 };
 
 /* eval : the return type of eval(). For matrices, this is just a const reference
  * in order to avoid a useless copy
  */
 
-template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct eval;
+template <typename T, typename StorageKind = typename traits<T>::StorageKind>
+struct eval;
 
-template<typename T> struct eval<T,Dense>
-{
+template <typename T>
+struct eval<T, Dense> {
   typedef typename plain_matrix_type<T>::type type;
-//   typedef typename T::PlainObject type;
-//   typedef T::Matrix<typename traits<T>::Scalar,
-//                 traits<T>::RowsAtCompileTime,
-//                 traits<T>::ColsAtCompileTime,
-//                 AutoAlign | (traits<T>::Flags&RowMajorBit ? RowMajor : ColMajor),
-//                 traits<T>::MaxRowsAtCompileTime,
-//                 traits<T>::MaxColsAtCompileTime
-//           > type;
+  //   typedef typename T::PlainObject type;
+  //   typedef T::Matrix<typename traits<T>::Scalar,
+  //                 traits<T>::RowsAtCompileTime,
+  //                 traits<T>::ColsAtCompileTime,
+  //                 AutoAlign | (traits<T>::Flags&RowMajorBit ? RowMajor : ColMajor),
+  //                 traits<T>::MaxRowsAtCompileTime,
+  //                 traits<T>::MaxColsAtCompileTime
+  //           > type;
 };
 
-template<typename T> struct eval<T,DiagonalShape>
-{
+template <typename T>
+struct eval<T, DiagonalShape> {
+  typedef typename plain_matrix_type<T>::type type;
+};
+
+template <typename T>
+struct eval<T, SkewSymmetricShape> {
   typedef typename plain_matrix_type<T>::type type;
 };
 
 // for matrices, no need to evaluate, just use a const reference to avoid a useless copy
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct eval<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, Dense>
-{
-  typedef const Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& type;
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+struct eval<Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>, Dense> {
+  typedef const Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>& type;
 };
 
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct eval<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, Dense>
-{
-  typedef const Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& type;
+template <typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
+struct eval<Array<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>, Dense> {
+  typedef const Array<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>& type;
 };
 
-
 /* similar to plain_matrix_type, but using the evaluator's Flags */
-template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct plain_object_eval;
+template <typename T, typename StorageKind = typename traits<T>::StorageKind>
+struct plain_object_eval;
 
-template<typename T>
-struct plain_object_eval<T,Dense>
-{
-  typedef typename plain_matrix_type_dense<T,typename traits<T>::XprKind, evaluator<T>::Flags>::type type;
+template <typename T>
+struct plain_object_eval<T, Dense> {
+  typedef typename plain_matrix_type_dense<T, typename traits<T>::XprKind, evaluator<T>::Flags>::type type;
 };
 
-
 /* plain_matrix_type_column_major : same as plain_matrix_type but guaranteed to be column-major
  */
-template<typename T> struct plain_matrix_type_column_major
-{
-  enum { Rows = traits<T>::RowsAtCompileTime,
-         Cols = traits<T>::ColsAtCompileTime,
-         MaxRows = traits<T>::MaxRowsAtCompileTime,
-         MaxCols = traits<T>::MaxColsAtCompileTime
+template <typename T>
+struct plain_matrix_type_column_major {
+  enum {
+    Rows = traits<T>::RowsAtCompileTime,
+    Cols = traits<T>::ColsAtCompileTime,
+    MaxRows = traits<T>::MaxRowsAtCompileTime,
+    MaxCols = traits<T>::MaxColsAtCompileTime
   };
-  typedef Matrix<typename traits<T>::Scalar,
-                Rows,
-                Cols,
-                (MaxRows==1&&MaxCols!=1) ? RowMajor : ColMajor,
-                MaxRows,
-                MaxCols
-          > type;
+  typedef Matrix<typename traits<T>::Scalar, Rows, Cols, (MaxRows == 1 && MaxCols != 1) ? RowMajor : ColMajor, MaxRows,
+                 MaxCols>
+      type;
 };
 
 /* plain_matrix_type_row_major : same as plain_matrix_type but guaranteed to be row-major
  */
-template<typename T> struct plain_matrix_type_row_major
-{
-  enum { Rows = traits<T>::RowsAtCompileTime,
-         Cols = traits<T>::ColsAtCompileTime,
-         MaxRows = traits<T>::MaxRowsAtCompileTime,
-         MaxCols = traits<T>::MaxColsAtCompileTime
+template <typename T>
+struct plain_matrix_type_row_major {
+  enum {
+    Rows = traits<T>::RowsAtCompileTime,
+    Cols = traits<T>::ColsAtCompileTime,
+    MaxRows = traits<T>::MaxRowsAtCompileTime,
+    MaxCols = traits<T>::MaxColsAtCompileTime
   };
-  typedef Matrix<typename traits<T>::Scalar,
-                Rows,
-                Cols,
-                (MaxCols==1&&MaxRows!=1) ? ColMajor : RowMajor,
-                MaxRows,
-                MaxCols
-          > type;
+  typedef Matrix<typename traits<T>::Scalar, Rows, Cols, (MaxCols == 1 && MaxRows != 1) ? ColMajor : RowMajor, MaxRows,
+                 MaxCols>
+      type;
 };
 
 /** \internal The reference selector for template expressions. The idea is that we don't
-  * need to use references for expressions since they are light weight proxy
-  * objects which should generate no copying overhead. */
+ * need to use references for expressions since they are light weight proxy
+ * objects which should generate no copying overhead. */
 template <typename T>
-struct ref_selector
-{
-  typedef typename conditional<
-    bool(traits<T>::Flags & NestByRefBit),
-    T const&,
-    const T
-  >::type type;
+struct ref_selector {
+  typedef std::conditional_t<bool(traits<T>::Flags& NestByRefBit), T const&, const T> type;
 
-  typedef typename conditional<
-    bool(traits<T>::Flags & NestByRefBit),
-    T &,
-    T
-  >::type non_const_type;
+  typedef std::conditional_t<bool(traits<T>::Flags& NestByRefBit), T&, T> non_const_type;
 };
 
 /** \internal Adds the const qualifier on the value-type of T2 if and only if T1 is a const type */
-template<typename T1, typename T2>
-struct transfer_constness
-{
-  typedef typename conditional<
-    bool(internal::is_const<T1>::value),
-    typename internal::add_const_on_value_type<T2>::type,
-    T2
-  >::type type;
+template <typename T1, typename T2>
+struct transfer_constness {
+  typedef std::conditional_t<bool(internal::is_const<T1>::value), add_const_on_value_type_t<T2>, T2> type;
 };
 
-
 // However, we still need a mechanism to detect whether an expression which is evaluated multiple time
 // has to be evaluated into a temporary.
 // That's the purpose of this new nested_eval helper:
 /** \internal Determines how a given expression should be nested when evaluated multiple times.
-  * For example, when you do a * (b+c), Eigen will determine how the expression b+c should be
-  * evaluated into the bigger product expression. The choice is between nesting the expression b+c as-is, or
-  * evaluating that expression b+c into a temporary variable d, and nest d so that the resulting expression is
-  * a*d. Evaluating can be beneficial for example if every coefficient access in the resulting expression causes
-  * many coefficient accesses in the nested expressions -- as is the case with matrix product for example.
-  *
-  * \tparam T the type of the expression being nested.
-  * \tparam n the number of coefficient accesses in the nested expression for each coefficient access in the bigger expression.
-  * \tparam PlainObject the type of the temporary if needed.
-  */
-template<typename T, int n, typename PlainObject = typename plain_object_eval<T>::type> struct nested_eval
-{
+ * For example, when you do a * (b+c), Eigen will determine how the expression b+c should be
+ * evaluated into the bigger product expression. The choice is between nesting the expression b+c as-is, or
+ * evaluating that expression b+c into a temporary variable d, and nest d so that the resulting expression is
+ * a*d. Evaluating can be beneficial for example if every coefficient access in the resulting expression causes
+ * many coefficient accesses in the nested expressions -- as is the case with matrix product for example.
+ *
+ * \tparam T the type of the expression being nested.
+ * \tparam n the number of coefficient accesses in the nested expression for each coefficient access in the bigger
+ * expression. \tparam PlainObject the type of the temporary if needed.
+ */
+template <typename T, int n, typename PlainObject = typename plain_object_eval<T>::type>
+struct nested_eval {
   enum {
     ScalarReadCost = NumTraits<typename traits<T>::Scalar>::ReadCost,
-    CoeffReadCost = evaluator<T>::CoeffReadCost,  // NOTE What if an evaluator evaluate itself into a temporary?
-                                                  //      Then CoeffReadCost will be small (e.g., 1) but we still have to evaluate, especially if n>1.
-                                                  //      This situation is already taken care by the EvalBeforeNestingBit flag, which is turned ON
-                                                  //      for all evaluator creating a temporary. This flag is then propagated by the parent evaluators.
-                                                  //      Another solution could be to count the number of temps?
+    CoeffReadCost =
+        evaluator<T>::CoeffReadCost,  // NOTE What if an evaluator evaluate itself into a temporary?
+                                      //      Then CoeffReadCost will be small (e.g., 1) but we still have to evaluate,
+                                      //      especially if n>1. This situation is already taken care by the
+                                      //      EvalBeforeNestingBit flag, which is turned ON for all evaluator creating a
+                                      //      temporary. This flag is then propagated by the parent evaluators. Another
+                                      //      solution could be to count the number of temps?
     NAsInteger = n == Dynamic ? HugeCost : n,
-    CostEval   = (NAsInteger+1) * ScalarReadCost + CoeffReadCost,
+    CostEval = (NAsInteger + 1) * ScalarReadCost + CoeffReadCost,
     CostNoEval = NAsInteger * CoeffReadCost,
     Evaluate = (int(evaluator<T>::Flags) & EvalBeforeNestingBit) || (int(CostEval) < int(CostNoEval))
   };
 
-  typedef typename conditional<Evaluate, PlainObject, typename ref_selector<T>::type>::type type;
+  typedef std::conditional_t<Evaluate, PlainObject, typename ref_selector<T>::type> type;
 };
 
-template<typename T>
-EIGEN_DEVICE_FUNC
-inline T* const_cast_ptr(const T* ptr)
-{
+template <typename T>
+EIGEN_DEVICE_FUNC inline T* const_cast_ptr(const T* ptr) {
   return const_cast<T*>(ptr);
 }
 
-template<typename Derived, typename XprKind = typename traits<Derived>::XprKind>
-struct dense_xpr_base
-{
-  /* dense_xpr_base should only ever be used on dense expressions, thus falling either into the MatrixXpr or into the ArrayXpr cases */
+template <typename Derived, typename XprKind = typename traits<Derived>::XprKind>
+struct dense_xpr_base {
+  /* dense_xpr_base should only ever be used on dense expressions, thus falling either into the MatrixXpr or into the
+   * ArrayXpr cases */
 };
 
-template<typename Derived>
-struct dense_xpr_base<Derived, MatrixXpr>
-{
+template <typename Derived>
+struct dense_xpr_base<Derived, MatrixXpr> {
   typedef MatrixBase<Derived> type;
 };
 
-template<typename Derived>
-struct dense_xpr_base<Derived, ArrayXpr>
-{
+template <typename Derived>
+struct dense_xpr_base<Derived, ArrayXpr> {
   typedef ArrayBase<Derived> type;
 };
 
-template<typename Derived, typename XprKind = typename traits<Derived>::XprKind, typename StorageKind = typename traits<Derived>::StorageKind>
+template <typename Derived, typename XprKind = typename traits<Derived>::XprKind,
+          typename StorageKind = typename traits<Derived>::StorageKind>
 struct generic_xpr_base;
 
-template<typename Derived, typename XprKind>
-struct generic_xpr_base<Derived, XprKind, Dense>
-{
-  typedef typename dense_xpr_base<Derived,XprKind>::type type;
+template <typename Derived, typename XprKind>
+struct generic_xpr_base<Derived, XprKind, Dense> {
+  typedef typename dense_xpr_base<Derived, XprKind>::type type;
 };
 
-template<typename XprType, typename CastType> struct cast_return_type
-{
+template <typename XprType, typename CastType>
+struct cast_return_type {
   typedef typename XprType::Scalar CurrentScalarType;
-  typedef typename remove_all<CastType>::type _CastType;
-  typedef typename _CastType::Scalar NewScalarType;
-  typedef typename conditional<is_same<CurrentScalarType,NewScalarType>::value,
-                              const XprType&,CastType>::type type;
+  typedef remove_all_t<CastType> CastType_;
+  typedef typename CastType_::Scalar NewScalarType;
+  typedef std::conditional_t<is_same<CurrentScalarType, NewScalarType>::value, const XprType&, CastType> type;
 };
 
-template <typename A, typename B> struct promote_storage_type;
+template <typename A, typename B>
+struct promote_storage_type;
 
-template <typename A> struct promote_storage_type<A,A>
-{
+template <typename A>
+struct promote_storage_type<A, A> {
   typedef A ret;
 };
-template <typename A> struct promote_storage_type<A, const A>
-{
+template <typename A>
+struct promote_storage_type<A, const A> {
   typedef A ret;
 };
-template <typename A> struct promote_storage_type<const A, A>
-{
+template <typename A>
+struct promote_storage_type<const A, A> {
   typedef A ret;
 };
 
 /** \internal Specify the "storage kind" of applying a coefficient-wise
-  * binary operations between two expressions of kinds A and B respectively.
-  * The template parameter Functor permits to specialize the resulting storage kind wrt to
-  * the functor.
-  * The default rules are as follows:
-  * \code
-  * A      op A      -> A
-  * A      op dense  -> dense
-  * dense  op B      -> dense
-  * sparse op dense  -> sparse
-  * dense  op sparse -> sparse
-  * \endcode
-  */
-template <typename A, typename B, typename Functor> struct cwise_promote_storage_type;
+ * binary operations between two expressions of kinds A and B respectively.
+ * The template parameter Functor permits to specialize the resulting storage kind wrt to
+ * the functor.
+ * The default rules are as follows:
+ * \code
+ * A      op A      -> A
+ * A      op dense  -> dense
+ * dense  op B      -> dense
+ * sparse op dense  -> sparse
+ * dense  op sparse -> sparse
+ * \endcode
+ */
+template <typename A, typename B, typename Functor>
+struct cwise_promote_storage_type;
 
-template <typename A, typename Functor>                   struct cwise_promote_storage_type<A,A,Functor>                                      { typedef A      ret; };
-template <typename Functor>                               struct cwise_promote_storage_type<Dense,Dense,Functor>                              { typedef Dense  ret; };
-template <typename A, typename Functor>                   struct cwise_promote_storage_type<A,Dense,Functor>                                  { typedef Dense  ret; };
-template <typename B, typename Functor>                   struct cwise_promote_storage_type<Dense,B,Functor>                                  { typedef Dense  ret; };
-template <typename Functor>                               struct cwise_promote_storage_type<Sparse,Dense,Functor>                             { typedef Sparse ret; };
-template <typename Functor>                               struct cwise_promote_storage_type<Dense,Sparse,Functor>                             { typedef Sparse ret; };
+template <typename A, typename Functor>
+struct cwise_promote_storage_type<A, A, Functor> {
+  typedef A ret;
+};
+template <typename Functor>
+struct cwise_promote_storage_type<Dense, Dense, Functor> {
+  typedef Dense ret;
+};
+template <typename A, typename Functor>
+struct cwise_promote_storage_type<A, Dense, Functor> {
+  typedef Dense ret;
+};
+template <typename B, typename Functor>
+struct cwise_promote_storage_type<Dense, B, Functor> {
+  typedef Dense ret;
+};
+template <typename Functor>
+struct cwise_promote_storage_type<Sparse, Dense, Functor> {
+  typedef Sparse ret;
+};
+template <typename Functor>
+struct cwise_promote_storage_type<Dense, Sparse, Functor> {
+  typedef Sparse ret;
+};
 
-template <typename LhsKind, typename RhsKind, int LhsOrder, int RhsOrder> struct cwise_promote_storage_order {
+template <typename LhsKind, typename RhsKind, int LhsOrder, int RhsOrder>
+struct cwise_promote_storage_order {
   enum { value = LhsOrder };
 };
 
-template <typename LhsKind, int LhsOrder, int RhsOrder>   struct cwise_promote_storage_order<LhsKind,Sparse,LhsOrder,RhsOrder>                { enum { value = RhsOrder }; };
-template <typename RhsKind, int LhsOrder, int RhsOrder>   struct cwise_promote_storage_order<Sparse,RhsKind,LhsOrder,RhsOrder>                { enum { value = LhsOrder }; };
-template <int Order>                                      struct cwise_promote_storage_order<Sparse,Sparse,Order,Order>                       { enum { value = Order }; };
-
+template <typename LhsKind, int LhsOrder, int RhsOrder>
+struct cwise_promote_storage_order<LhsKind, Sparse, LhsOrder, RhsOrder> {
+  enum { value = RhsOrder };
+};
+template <typename RhsKind, int LhsOrder, int RhsOrder>
+struct cwise_promote_storage_order<Sparse, RhsKind, LhsOrder, RhsOrder> {
+  enum { value = LhsOrder };
+};
+template <int Order>
+struct cwise_promote_storage_order<Sparse, Sparse, Order, Order> {
+  enum { value = Order };
+};
 
 /** \internal Specify the "storage kind" of multiplying an expression of kind A with kind B.
-  * The template parameter ProductTag permits to specialize the resulting storage kind wrt to
-  * some compile-time properties of the product: GemmProduct, GemvProduct, OuterProduct, InnerProduct.
-  * The default rules are as follows:
-  * \code
-  *  K * K            -> K
-  *  dense * K        -> dense
-  *  K * dense        -> dense
-  *  diag * K         -> K
-  *  K * diag         -> K
-  *  Perm * K         -> K
-  * K * Perm          -> K
-  * \endcode
-  */
-template <typename A, typename B, int ProductTag> struct product_promote_storage_type;
+ * The template parameter ProductTag permits to specialize the resulting storage kind wrt to
+ * some compile-time properties of the product: GemmProduct, GemvProduct, OuterProduct, InnerProduct.
+ * The default rules are as follows:
+ * \code
+ *  K * K            -> K
+ *  dense * K        -> dense
+ *  K * dense        -> dense
+ *  diag * K         -> K
+ *  K * diag         -> K
+ *  Perm * K         -> K
+ * K * Perm          -> K
+ * \endcode
+ */
+template <typename A, typename B, int ProductTag>
+struct product_promote_storage_type;
 
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  A,                  ProductTag> { typedef A     ret;};
-template <int ProductTag>             struct product_promote_storage_type<Dense,              Dense,              ProductTag> { typedef Dense ret;};
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  Dense,              ProductTag> { typedef Dense ret; };
-template <typename B, int ProductTag> struct product_promote_storage_type<Dense,              B,                  ProductTag> { typedef Dense ret; };
+template <typename A, int ProductTag>
+struct product_promote_storage_type<A, A, ProductTag> {
+  typedef A ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<Dense, Dense, ProductTag> {
+  typedef Dense ret;
+};
+template <typename A, int ProductTag>
+struct product_promote_storage_type<A, Dense, ProductTag> {
+  typedef Dense ret;
+};
+template <typename B, int ProductTag>
+struct product_promote_storage_type<Dense, B, ProductTag> {
+  typedef Dense ret;
+};
 
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  DiagonalShape,      ProductTag> { typedef A ret; };
-template <typename B, int ProductTag> struct product_promote_storage_type<DiagonalShape,      B,                  ProductTag> { typedef B ret; };
-template <int ProductTag>             struct product_promote_storage_type<Dense,              DiagonalShape,      ProductTag> { typedef Dense ret; };
-template <int ProductTag>             struct product_promote_storage_type<DiagonalShape,      Dense,              ProductTag> { typedef Dense ret; };
+template <typename A, int ProductTag>
+struct product_promote_storage_type<A, DiagonalShape, ProductTag> {
+  typedef A ret;
+};
+template <typename B, int ProductTag>
+struct product_promote_storage_type<DiagonalShape, B, ProductTag> {
+  typedef B ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<Dense, DiagonalShape, ProductTag> {
+  typedef Dense ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<DiagonalShape, Dense, ProductTag> {
+  typedef Dense ret;
+};
 
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  PermutationStorage, ProductTag> { typedef A ret; };
-template <typename B, int ProductTag> struct product_promote_storage_type<PermutationStorage, B,                  ProductTag> { typedef B ret; };
-template <int ProductTag>             struct product_promote_storage_type<Dense,              PermutationStorage, ProductTag> { typedef Dense ret; };
-template <int ProductTag>             struct product_promote_storage_type<PermutationStorage, Dense,              ProductTag> { typedef Dense ret; };
+template <typename A, int ProductTag>
+struct product_promote_storage_type<A, SkewSymmetricShape, ProductTag> {
+  typedef A ret;
+};
+template <typename B, int ProductTag>
+struct product_promote_storage_type<SkewSymmetricShape, B, ProductTag> {
+  typedef B ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<Dense, SkewSymmetricShape, ProductTag> {
+  typedef Dense ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<SkewSymmetricShape, Dense, ProductTag> {
+  typedef Dense ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<SkewSymmetricShape, SkewSymmetricShape, ProductTag> {
+  typedef Dense ret;
+};
+
+template <typename A, int ProductTag>
+struct product_promote_storage_type<A, PermutationStorage, ProductTag> {
+  typedef A ret;
+};
+template <typename B, int ProductTag>
+struct product_promote_storage_type<PermutationStorage, B, ProductTag> {
+  typedef B ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<Dense, PermutationStorage, ProductTag> {
+  typedef Dense ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<PermutationStorage, Dense, ProductTag> {
+  typedef Dense ret;
+};
 
 /** \internal gives the plain matrix or array type to store a row/column/diagonal of a matrix type.
-  * \tparam Scalar optional parameter allowing to pass a different scalar type than the one of the MatrixType.
-  */
-template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
-struct plain_row_type
-{
+ * \tparam Scalar optional parameter allowing to pass a different scalar type than the one of the MatrixType.
+ */
+template <typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
+struct plain_row_type {
   typedef Matrix<Scalar, 1, ExpressionType::ColsAtCompileTime,
-                 int(ExpressionType::PlainObject::Options) | int(RowMajor), 1, ExpressionType::MaxColsAtCompileTime> MatrixRowType;
-  typedef Array<Scalar, 1, ExpressionType::ColsAtCompileTime,
-                 int(ExpressionType::PlainObject::Options) | int(RowMajor), 1, ExpressionType::MaxColsAtCompileTime> ArrayRowType;
+                 int(ExpressionType::PlainObject::Options) | int(RowMajor), 1, ExpressionType::MaxColsAtCompileTime>
+      MatrixRowType;
+  typedef Array<Scalar, 1, ExpressionType::ColsAtCompileTime, int(ExpressionType::PlainObject::Options) | int(RowMajor),
+                1, ExpressionType::MaxColsAtCompileTime>
+      ArrayRowType;
 
-  typedef typename conditional<
-    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
-    MatrixRowType,
-    ArrayRowType
-  >::type type;
+  typedef std::conditional_t<is_same<typename traits<ExpressionType>::XprKind, MatrixXpr>::value, MatrixRowType,
+                             ArrayRowType>
+      type;
 };
 
-template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
-struct plain_col_type
-{
-  typedef Matrix<Scalar, ExpressionType::RowsAtCompileTime, 1,
-                 ExpressionType::PlainObject::Options & ~RowMajor, ExpressionType::MaxRowsAtCompileTime, 1> MatrixColType;
-  typedef Array<Scalar, ExpressionType::RowsAtCompileTime, 1,
-                 ExpressionType::PlainObject::Options & ~RowMajor, ExpressionType::MaxRowsAtCompileTime, 1> ArrayColType;
+template <typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
+struct plain_col_type {
+  typedef Matrix<Scalar, ExpressionType::RowsAtCompileTime, 1, ExpressionType::PlainObject::Options & ~RowMajor,
+                 ExpressionType::MaxRowsAtCompileTime, 1>
+      MatrixColType;
+  typedef Array<Scalar, ExpressionType::RowsAtCompileTime, 1, ExpressionType::PlainObject::Options & ~RowMajor,
+                ExpressionType::MaxRowsAtCompileTime, 1>
+      ArrayColType;
 
-  typedef typename conditional<
-    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
-    MatrixColType,
-    ArrayColType
-  >::type type;
+  typedef std::conditional_t<is_same<typename traits<ExpressionType>::XprKind, MatrixXpr>::value, MatrixColType,
+                             ArrayColType>
+      type;
 };
 
-template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
-struct plain_diag_type
-{
-  enum { diag_size = EIGEN_SIZE_MIN_PREFER_DYNAMIC(ExpressionType::RowsAtCompileTime, ExpressionType::ColsAtCompileTime),
-         max_diag_size = EIGEN_SIZE_MIN_PREFER_FIXED(ExpressionType::MaxRowsAtCompileTime, ExpressionType::MaxColsAtCompileTime)
+template <typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
+struct plain_diag_type {
+  enum {
+    diag_size = internal::min_size_prefer_dynamic(ExpressionType::RowsAtCompileTime, ExpressionType::ColsAtCompileTime),
+    max_diag_size = min_size_prefer_fixed(ExpressionType::MaxRowsAtCompileTime, ExpressionType::MaxColsAtCompileTime)
   };
-  typedef Matrix<Scalar, diag_size, 1, ExpressionType::PlainObject::Options & ~RowMajor, max_diag_size, 1> MatrixDiagType;
+  typedef Matrix<Scalar, diag_size, 1, ExpressionType::PlainObject::Options & ~RowMajor, max_diag_size, 1>
+      MatrixDiagType;
   typedef Array<Scalar, diag_size, 1, ExpressionType::PlainObject::Options & ~RowMajor, max_diag_size, 1> ArrayDiagType;
 
-  typedef typename conditional<
-    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
-    MatrixDiagType,
-    ArrayDiagType
-  >::type type;
+  typedef std::conditional_t<is_same<typename traits<ExpressionType>::XprKind, MatrixXpr>::value, MatrixDiagType,
+                             ArrayDiagType>
+      type;
 };
 
-template<typename Expr,typename Scalar = typename Expr::Scalar>
-struct plain_constant_type
-{
-  enum { Options = (traits<Expr>::Flags&RowMajorBit)?RowMajor:0 };
+template <typename Expr, typename Scalar = typename Expr::Scalar>
+struct plain_constant_type {
+  enum { Options = (traits<Expr>::Flags & RowMajorBit) ? RowMajor : 0 };
 
-  typedef Array<Scalar,  traits<Expr>::RowsAtCompileTime,   traits<Expr>::ColsAtCompileTime,
-                Options, traits<Expr>::MaxRowsAtCompileTime,traits<Expr>::MaxColsAtCompileTime> array_type;
+  typedef Array<Scalar, traits<Expr>::RowsAtCompileTime, traits<Expr>::ColsAtCompileTime, Options,
+                traits<Expr>::MaxRowsAtCompileTime, traits<Expr>::MaxColsAtCompileTime>
+      array_type;
 
-  typedef Matrix<Scalar,  traits<Expr>::RowsAtCompileTime,   traits<Expr>::ColsAtCompileTime,
-                 Options, traits<Expr>::MaxRowsAtCompileTime,traits<Expr>::MaxColsAtCompileTime> matrix_type;
+  typedef Matrix<Scalar, traits<Expr>::RowsAtCompileTime, traits<Expr>::ColsAtCompileTime, Options,
+                 traits<Expr>::MaxRowsAtCompileTime, traits<Expr>::MaxColsAtCompileTime>
+      matrix_type;
 
-  typedef CwiseNullaryOp<scalar_constant_op<Scalar>, const typename conditional<is_same< typename traits<Expr>::XprKind, MatrixXpr >::value, matrix_type, array_type>::type > type;
+  typedef CwiseNullaryOp<
+      scalar_constant_op<Scalar>,
+      const std::conditional_t<is_same<typename traits<Expr>::XprKind, MatrixXpr>::value, matrix_type, array_type>>
+      type;
 };
 
-template<typename ExpressionType>
-struct is_lvalue
-{
-  enum { value = (!bool(is_const<ExpressionType>::value)) &&
-                 bool(traits<ExpressionType>::Flags & LvalueBit) };
+template <typename ExpressionType>
+struct is_lvalue {
+  enum { value = (!bool(is_const<ExpressionType>::value)) && bool(traits<ExpressionType>::Flags & LvalueBit) };
 };
 
-template<typename T> struct is_diagonal
-{ enum { ret = false }; };
+template <typename T>
+struct is_diagonal {
+  enum { ret = false };
+};
 
-template<typename T> struct is_diagonal<DiagonalBase<T> >
-{ enum { ret = true }; };
+template <typename T>
+struct is_diagonal<DiagonalBase<T>> {
+  enum { ret = true };
+};
 
-template<typename T> struct is_diagonal<DiagonalWrapper<T> >
-{ enum { ret = true }; };
+template <typename T>
+struct is_diagonal<DiagonalWrapper<T>> {
+  enum { ret = true };
+};
 
-template<typename T, int S> struct is_diagonal<DiagonalMatrix<T,S> >
-{ enum { ret = true }; };
+template <typename T, int S>
+struct is_diagonal<DiagonalMatrix<T, S>> {
+  enum { ret = true };
+};
 
+template <typename T>
+struct is_identity {
+  enum { value = false };
+};
 
-template<typename T> struct is_identity
-{ enum { value = false }; };
+template <typename T>
+struct is_identity<CwiseNullaryOp<internal::scalar_identity_op<typename T::Scalar>, T>> {
+  enum { value = true };
+};
 
-template<typename T> struct is_identity<CwiseNullaryOp<internal::scalar_identity_op<typename T::Scalar>, T> >
-{ enum { value = true }; };
+template <typename S1, typename S2>
+struct glue_shapes;
+template <>
+struct glue_shapes<DenseShape, TriangularShape> {
+  typedef TriangularShape type;
+};
 
-
-template<typename S1, typename S2> struct glue_shapes;
-template<> struct glue_shapes<DenseShape,TriangularShape> { typedef TriangularShape type;  };
-
-template<typename T1, typename T2>
+template <typename T1, typename T2>
 struct possibly_same_dense {
-  enum { value = has_direct_access<T1>::ret && has_direct_access<T2>::ret && is_same<typename T1::Scalar,typename T2::Scalar>::value };
+  enum {
+    value = has_direct_access<T1>::ret && has_direct_access<T2>::ret &&
+            is_same<typename T1::Scalar, typename T2::Scalar>::value
+  };
 };
 
-template<typename T1, typename T2>
-EIGEN_DEVICE_FUNC
-bool is_same_dense(const T1 &mat1, const T2 &mat2, typename enable_if<possibly_same_dense<T1,T2>::value>::type * = 0)
-{
-  return (mat1.data()==mat2.data()) && (mat1.innerStride()==mat2.innerStride()) && (mat1.outerStride()==mat2.outerStride());
+template <typename T1, typename T2>
+EIGEN_DEVICE_FUNC bool is_same_dense(const T1& mat1, const T2& mat2,
+                                     std::enable_if_t<possibly_same_dense<T1, T2>::value>* = 0) {
+  return (mat1.data() == mat2.data()) && (mat1.innerStride() == mat2.innerStride()) &&
+         (mat1.outerStride() == mat2.outerStride());
 }
 
-template<typename T1, typename T2>
-EIGEN_DEVICE_FUNC
-bool is_same_dense(const T1 &, const T2 &, typename enable_if<!possibly_same_dense<T1,T2>::value>::type * = 0)
-{
+template <typename T1, typename T2>
+EIGEN_DEVICE_FUNC bool is_same_dense(const T1&, const T2&, std::enable_if_t<!possibly_same_dense<T1, T2>::value>* = 0) {
   return false;
 }
 
 // Internal helper defining the cost of a scalar division for the type T.
 // The default heuristic can be specialized for each scalar type and architecture.
-template<typename T,bool Vectorized=false,typename EnableIf = void>
+template <typename T, bool Vectorized = false, typename EnableIf = void>
 struct scalar_div_cost {
-  enum { value = 8*NumTraits<T>::MulCost };
+  enum { value = 8 * NumTraits<T>::MulCost };
 };
 
-template<typename T,bool Vectorized>
+template <typename T, bool Vectorized>
 struct scalar_div_cost<std::complex<T>, Vectorized> {
-  enum { value = 2*scalar_div_cost<T>::value
-               + 6*NumTraits<T>::MulCost
-               + 3*NumTraits<T>::AddCost
-  };
+  enum { value = 2 * scalar_div_cost<T>::value + 6 * NumTraits<T>::MulCost + 3 * NumTraits<T>::AddCost };
 };
 
-
-template<bool Vectorized>
-struct scalar_div_cost<signed long,Vectorized,typename conditional<sizeof(long)==8,void,false_type>::type> { enum { value = 24 }; };
-template<bool Vectorized>
-struct scalar_div_cost<unsigned long,Vectorized,typename conditional<sizeof(long)==8,void,false_type>::type> { enum { value = 21 }; };
-
+template <bool Vectorized>
+struct scalar_div_cost<signed long, Vectorized, std::conditional_t<sizeof(long) == 8, void, false_type>> {
+  enum { value = 24 };
+};
+template <bool Vectorized>
+struct scalar_div_cost<unsigned long, Vectorized, std::conditional_t<sizeof(long) == 8, void, false_type>> {
+  enum { value = 21 };
+};
 
 #ifdef EIGEN_DEBUG_ASSIGN
-std::string demangle_traversal(int t)
-{
-  if(t==DefaultTraversal) return "DefaultTraversal";
-  if(t==LinearTraversal) return "LinearTraversal";
-  if(t==InnerVectorizedTraversal) return "InnerVectorizedTraversal";
-  if(t==LinearVectorizedTraversal) return "LinearVectorizedTraversal";
-  if(t==SliceVectorizedTraversal) return "SliceVectorizedTraversal";
+std::string demangle_traversal(int t) {
+  if (t == DefaultTraversal) return "DefaultTraversal";
+  if (t == LinearTraversal) return "LinearTraversal";
+  if (t == InnerVectorizedTraversal) return "InnerVectorizedTraversal";
+  if (t == LinearVectorizedTraversal) return "LinearVectorizedTraversal";
+  if (t == SliceVectorizedTraversal) return "SliceVectorizedTraversal";
   return "?";
 }
-std::string demangle_unrolling(int t)
-{
-  if(t==NoUnrolling) return "NoUnrolling";
-  if(t==InnerUnrolling) return "InnerUnrolling";
-  if(t==CompleteUnrolling) return "CompleteUnrolling";
+std::string demangle_unrolling(int t) {
+  if (t == NoUnrolling) return "NoUnrolling";
+  if (t == InnerUnrolling) return "InnerUnrolling";
+  if (t == CompleteUnrolling) return "CompleteUnrolling";
   return "?";
 }
-std::string demangle_flags(int f)
-{
+std::string demangle_flags(int f) {
   std::string res;
-  if(f&RowMajorBit)                 res += " | RowMajor";
-  if(f&PacketAccessBit)             res += " | Packet";
-  if(f&LinearAccessBit)             res += " | Linear";
-  if(f&LvalueBit)                   res += " | Lvalue";
-  if(f&DirectAccessBit)             res += " | Direct";
-  if(f&NestByRefBit)                res += " | NestByRef";
-  if(f&NoPreferredStorageOrderBit)  res += " | NoPreferredStorageOrderBit";
+  if (f & RowMajorBit) res += " | RowMajor";
+  if (f & PacketAccessBit) res += " | Packet";
+  if (f & LinearAccessBit) res += " | Linear";
+  if (f & LvalueBit) res += " | Lvalue";
+  if (f & DirectAccessBit) res += " | Direct";
+  if (f & NestByRefBit) res += " | NestByRef";
+  if (f & NoPreferredStorageOrderBit) res += " | NoPreferredStorageOrderBit";
 
   return res;
 }
 #endif
 
-} // end namespace internal
+template <typename XprType>
+struct is_block_xpr : std::false_type {};
 
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+struct is_block_xpr<Block<XprType, BlockRows, BlockCols, InnerPanel>> : std::true_type {};
+
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+struct is_block_xpr<const Block<XprType, BlockRows, BlockCols, InnerPanel>> : std::true_type {};
+
+// Helper utility for constructing non-recursive block expressions.
+template <typename XprType>
+struct block_xpr_helper {
+  using BaseType = XprType;
+
+  // For regular block expressions, simply forward along the InnerPanel argument,
+  // which is set when calling row/column expressions.
+  static constexpr bool is_inner_panel(bool inner_panel) { return inner_panel; }
+
+  // Only enable non-const base function if XprType is not const (otherwise we get a duplicate definition).
+  template <typename T = XprType, typename EnableIf = std::enable_if_t<!std::is_const<T>::value>>
+  static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BaseType& base(XprType& xpr) {
+    return xpr;
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE const BaseType& base(const XprType& xpr) { return xpr; }
+  static constexpr EIGEN_ALWAYS_INLINE Index row(const XprType& /*xpr*/, Index r) { return r; }
+  static constexpr EIGEN_ALWAYS_INLINE Index col(const XprType& /*xpr*/, Index c) { return c; }
+};
+
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+struct block_xpr_helper<Block<XprType, BlockRows, BlockCols, InnerPanel>> {
+  using BlockXprType = Block<XprType, BlockRows, BlockCols, InnerPanel>;
+  // Recursive helper in case of explicit block-of-block expression.
+  using NestedXprHelper = block_xpr_helper<XprType>;
+  using BaseType = typename NestedXprHelper::BaseType;
+
+  // For block-of-block expressions, we need to combine the InnerPannel trait
+  // with that of the block subexpression.
+  static constexpr bool is_inner_panel(bool inner_panel) { return InnerPanel && inner_panel; }
+
+  // Only enable non-const base function if XprType is not const (otherwise we get a duplicates definition).
+  template <typename T = XprType, typename EnableIf = std::enable_if_t<!std::is_const<T>::value>>
+  static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BaseType& base(BlockXprType& xpr) {
+    return NestedXprHelper::base(xpr.nestedExpression());
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE const BaseType& base(const BlockXprType& xpr) {
+    return NestedXprHelper::base(xpr.nestedExpression());
+  }
+  static constexpr EIGEN_ALWAYS_INLINE Index row(const BlockXprType& xpr, Index r) {
+    return xpr.startRow() + NestedXprHelper::row(xpr.nestedExpression(), r);
+  }
+  static constexpr EIGEN_ALWAYS_INLINE Index col(const BlockXprType& xpr, Index c) {
+    return xpr.startCol() + NestedXprHelper::col(xpr.nestedExpression(), c);
+  }
+};
+
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+struct block_xpr_helper<const Block<XprType, BlockRows, BlockCols, InnerPanel>>
+    : block_xpr_helper<Block<XprType, BlockRows, BlockCols, InnerPanel>> {};
+
+}  // end namespace internal
 
 /** \class ScalarBinaryOpTraits
   * \ingroup Core_Module
   *
-  * \brief Determines whether the given binary operation of two numeric types is allowed and what the scalar return type is.
+  * \brief Determines whether the given binary operation of two numeric types is allowed and what the scalar return type
+  is.
   *
-  * This class permits to control the scalar return type of any binary operation performed on two different scalar types through (partial) template specializations.
+  * This class permits to control the scalar return type of any binary operation performed on two different scalar types
+  through (partial) template specializations.
   *
-  * For instance, let \c U1, \c U2 and \c U3 be three user defined scalar types for which most operations between instances of \c U1 and \c U2 returns an \c U3.
+  * For instance, let \c U1, \c U2 and \c U3 be three user defined scalar types for which most operations between
+  instances of \c U1 and \c U2 returns an \c U3.
   * You can let %Eigen knows that by defining:
     \code
     template<typename BinaryOp>
@@ -791,66 +963,63 @@
   <table class="manual">
   <tr><th>ScalarA</th><th>ScalarB</th><th>BinaryOp</th><th>ReturnType</th><th>Note</th></tr>
   <tr            ><td>\c T </td><td>\c T </td><td>\c * </td><td>\c T </td><td></td></tr>
-  <tr class="alt"><td>\c NumTraits<T>::Real </td><td>\c T </td><td>\c * </td><td>\c T </td><td>Only if \c NumTraits<T>::IsComplex </td></tr>
-  <tr            ><td>\c T </td><td>\c NumTraits<T>::Real </td><td>\c * </td><td>\c T </td><td>Only if \c NumTraits<T>::IsComplex </td></tr>
+  <tr class="alt"><td>\c NumTraits<T>::Real </td><td>\c T </td><td>\c * </td><td>\c T </td><td>Only if \c
+  NumTraits<T>::IsComplex </td></tr> <tr            ><td>\c T </td><td>\c NumTraits<T>::Real </td><td>\c * </td><td>\c T
+  </td><td>Only if \c NumTraits<T>::IsComplex </td></tr>
   </table>
   *
   * \sa CwiseBinaryOp
   */
-template<typename ScalarA, typename ScalarB, typename BinaryOp=internal::scalar_product_op<ScalarA,ScalarB> >
+template <typename ScalarA, typename ScalarB, typename BinaryOp = internal::scalar_product_op<ScalarA, ScalarB>>
 struct ScalarBinaryOpTraits
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-  // for backward compatibility, use the hints given by the (deprecated) internal::scalar_product_traits class.
-  : internal::scalar_product_traits<ScalarA,ScalarB>
-#endif // EIGEN_PARSED_BY_DOXYGEN
-{};
-
-template<typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<T,T,BinaryOp>
+    // for backward compatibility, use the hints given by the (deprecated) internal::scalar_product_traits class.
+    : internal::scalar_product_traits<ScalarA, ScalarB>
+#endif  // EIGEN_PARSED_BY_DOXYGEN
 {
+};
+
+template <typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<T, T, BinaryOp> {
   typedef T ReturnType;
 };
 
 template <typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<T, typename NumTraits<typename internal::enable_if<NumTraits<T>::IsComplex,T>::type>::Real, BinaryOp>
-{
+struct ScalarBinaryOpTraits<T, typename NumTraits<std::enable_if_t<NumTraits<T>::IsComplex, T>>::Real, BinaryOp> {
   typedef T ReturnType;
 };
 template <typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<typename NumTraits<typename internal::enable_if<NumTraits<T>::IsComplex,T>::type>::Real, T, BinaryOp>
-{
+struct ScalarBinaryOpTraits<typename NumTraits<std::enable_if_t<NumTraits<T>::IsComplex, T>>::Real, T, BinaryOp> {
   typedef T ReturnType;
 };
 
 // For Matrix * Permutation
-template<typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<T,void,BinaryOp>
-{
+template <typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<T, void, BinaryOp> {
   typedef T ReturnType;
 };
 
 // For Permutation * Matrix
-template<typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<void,T,BinaryOp>
-{
+template <typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<void, T, BinaryOp> {
   typedef T ReturnType;
 };
 
 // for Permutation*Permutation
-template<typename BinaryOp>
-struct ScalarBinaryOpTraits<void,void,BinaryOp>
-{
+template <typename BinaryOp>
+struct ScalarBinaryOpTraits<void, void, BinaryOp> {
   typedef void ReturnType;
 };
 
 // We require Lhs and Rhs to have "compatible" scalar types.
-// It is tempting to always allow mixing different types but remember that this is often impossible in the vectorized paths.
-// So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user tries to
-// add together a float matrix and a double matrix.
-#define EIGEN_CHECK_BINARY_COMPATIBILIY(BINOP,LHS,RHS) \
-  EIGEN_STATIC_ASSERT((Eigen::internal::has_ReturnType<ScalarBinaryOpTraits<LHS, RHS,BINOP> >::value), \
-    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+// It is tempting to always allow mixing different types but remember that this is often impossible in the vectorized
+// paths. So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user
+// tries to add together a float matrix and a double matrix.
+#define EIGEN_CHECK_BINARY_COMPATIBILIY(BINOP, LHS, RHS)                               \
+  EIGEN_STATIC_ASSERT(                                                                 \
+      (Eigen::internal::has_ReturnType<ScalarBinaryOpTraits<LHS, RHS, BINOP>>::value), \
+      YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_XPRHELPER_H
+#endif  // EIGEN_XPRHELPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h
index 081e918..6efd3c1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h
@@ -14,254 +14,235 @@
 
 #include "./ComplexSchur.h"
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class ComplexEigenSolver
-  *
-  * \brief Computes eigenvalues and eigenvectors of general complex matrices
-  *
-  * \tparam _MatrixType the type of the matrix of which we are
-  * computing the eigendecomposition; this is expected to be an
-  * instantiation of the Matrix class template.
-  *
-  * The eigenvalues and eigenvectors of a matrix \f$ A \f$ are scalars
-  * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda v
-  * \f$.  If \f$ D \f$ is a diagonal matrix with the eigenvalues on
-  * the diagonal, and \f$ V \f$ is a matrix with the eigenvectors as
-  * its columns, then \f$ A V = V D \f$. The matrix \f$ V \f$ is
-  * almost always invertible, in which case we have \f$ A = V D V^{-1}
-  * \f$. This is called the eigendecomposition.
-  *
-  * The main function in this class is compute(), which computes the
-  * eigenvalues and eigenvectors of a given function. The
-  * documentation for that function contains an example showing the
-  * main features of the class.
-  *
-  * \sa class EigenSolver, class SelfAdjointEigenSolver
-  */
-template<typename _MatrixType> class ComplexEigenSolver
-{
-  public:
+ *
+ *
+ * \class ComplexEigenSolver
+ *
+ * \brief Computes eigenvalues and eigenvectors of general complex matrices
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are
+ * computing the eigendecomposition; this is expected to be an
+ * instantiation of the Matrix class template.
+ *
+ * The eigenvalues and eigenvectors of a matrix \f$ A \f$ are scalars
+ * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda v
+ * \f$.  If \f$ D \f$ is a diagonal matrix with the eigenvalues on
+ * the diagonal, and \f$ V \f$ is a matrix with the eigenvectors as
+ * its columns, then \f$ A V = V D \f$. The matrix \f$ V \f$ is
+ * almost always invertible, in which case we have \f$ A = V D V^{-1}
+ * \f$. This is called the eigendecomposition.
+ *
+ * The main function in this class is compute(), which computes the
+ * eigenvalues and eigenvectors of a given function. The
+ * documentation for that function contains an example showing the
+ * main features of the class.
+ *
+ * \sa class EigenSolver, class SelfAdjointEigenSolver
+ */
+template <typename MatrixType_>
+class ComplexEigenSolver {
+ public:
+  /** \brief Synonym for the template parameter \p MatrixType_. */
+  typedef MatrixType_ MatrixType;
 
-    /** \brief Synonym for the template parameter \p _MatrixType. */
-    typedef _MatrixType MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
 
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      Options = MatrixType::Options,
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
+  /** \brief Scalar type for matrices of type #MatrixType. */
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    /** \brief Scalar type for matrices of type #MatrixType. */
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  /** \brief Complex scalar type for #MatrixType.
+   *
+   * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
+   * \c float or \c double) and just \c Scalar if #Scalar is
+   * complex.
+   */
+  typedef std::complex<RealScalar> ComplexScalar;
 
-    /** \brief Complex scalar type for #MatrixType.
-      *
-      * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
-      * \c float or \c double) and just \c Scalar if #Scalar is
-      * complex.
-      */
-    typedef std::complex<RealScalar> ComplexScalar;
+  /** \brief Type for vector of eigenvalues as returned by eigenvalues().
+   *
+   * This is a column vector with entries of type #ComplexScalar.
+   * The length of the vector is the size of #MatrixType.
+   */
+  typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & (~RowMajor), MaxColsAtCompileTime, 1> EigenvalueType;
 
-    /** \brief Type for vector of eigenvalues as returned by eigenvalues().
-      *
-      * This is a column vector with entries of type #ComplexScalar.
-      * The length of the vector is the size of #MatrixType.
-      */
-    typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options&(~RowMajor), MaxColsAtCompileTime, 1> EigenvalueType;
+  /** \brief Type for matrix of eigenvectors as returned by eigenvectors().
+   *
+   * This is a square matrix with entries of type #ComplexScalar.
+   * The size is the same as the size of #MatrixType.
+   */
+  typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime,
+                 MaxColsAtCompileTime>
+      EigenvectorType;
 
-    /** \brief Type for matrix of eigenvectors as returned by eigenvectors().
-      *
-      * This is a square matrix with entries of type #ComplexScalar.
-      * The size is the same as the size of #MatrixType.
-      */
-    typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> EigenvectorType;
+  /** \brief Default constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via compute().
+   */
+  ComplexEigenSolver()
+      : m_eivec(), m_eivalues(), m_schur(), m_isInitialized(false), m_eigenvectorsOk(false), m_matX() {}
 
-    /** \brief Default constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via compute().
-      */
-    ComplexEigenSolver()
-            : m_eivec(),
-              m_eivalues(),
-              m_schur(),
-              m_isInitialized(false),
-              m_eigenvectorsOk(false),
-              m_matX()
-    {}
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa ComplexEigenSolver()
+   */
+  explicit ComplexEigenSolver(Index size)
+      : m_eivec(size, size),
+        m_eivalues(size),
+        m_schur(size),
+        m_isInitialized(false),
+        m_eigenvectorsOk(false),
+        m_matX(size, size) {}
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa ComplexEigenSolver()
-      */
-    explicit ComplexEigenSolver(Index size)
-            : m_eivec(size, size),
-              m_eivalues(size),
-              m_schur(size),
-              m_isInitialized(false),
-              m_eigenvectorsOk(false),
-              m_matX(size, size)
-    {}
+  /** \brief Constructor; computes eigendecomposition of given matrix.
+   *
+   * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
+   *    eigenvalues are computed; if false, only the eigenvalues are
+   *    computed.
+   *
+   * This constructor calls compute() to compute the eigendecomposition.
+   */
+  template <typename InputType>
+  explicit ComplexEigenSolver(const EigenBase<InputType>& matrix, bool computeEigenvectors = true)
+      : m_eivec(matrix.rows(), matrix.cols()),
+        m_eivalues(matrix.cols()),
+        m_schur(matrix.rows()),
+        m_isInitialized(false),
+        m_eigenvectorsOk(false),
+        m_matX(matrix.rows(), matrix.cols()) {
+    compute(matrix.derived(), computeEigenvectors);
+  }
 
-    /** \brief Constructor; computes eigendecomposition of given matrix.
-      *
-      * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
-      *    eigenvalues are computed; if false, only the eigenvalues are
-      *    computed.
-      *
-      * This constructor calls compute() to compute the eigendecomposition.
-      */
-    template<typename InputType>
-    explicit ComplexEigenSolver(const EigenBase<InputType>& matrix, bool computeEigenvectors = true)
-            : m_eivec(matrix.rows(),matrix.cols()),
-              m_eivalues(matrix.cols()),
-              m_schur(matrix.rows()),
-              m_isInitialized(false),
-              m_eigenvectorsOk(false),
-              m_matX(matrix.rows(),matrix.cols())
-    {
-      compute(matrix.derived(), computeEigenvectors);
-    }
+  /** \brief Returns the eigenvectors of given matrix.
+   *
+   * \returns  A const reference to the matrix whose columns are the eigenvectors.
+   *
+   * \pre Either the constructor
+   * ComplexEigenSolver(const MatrixType& matrix, bool) or the member
+   * function compute(const MatrixType& matrix, bool) has been called before
+   * to compute the eigendecomposition of a matrix, and
+   * \p computeEigenvectors was set to true (the default).
+   *
+   * This function returns a matrix whose columns are the eigenvectors. Column
+   * \f$ k \f$ is an eigenvector corresponding to eigenvalue number \f$ k
+   * \f$ as returned by eigenvalues().  The eigenvectors are normalized to
+   * have (Euclidean) norm equal to one. The matrix returned by this
+   * function is the matrix \f$ V \f$ in the eigendecomposition \f$ A = V D
+   * V^{-1} \f$, if it exists.
+   *
+   * Example: \include ComplexEigenSolver_eigenvectors.cpp
+   * Output: \verbinclude ComplexEigenSolver_eigenvectors.out
+   */
+  const EigenvectorType& eigenvectors() const {
+    eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized.");
+    eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
+    return m_eivec;
+  }
 
-    /** \brief Returns the eigenvectors of given matrix.
-      *
-      * \returns  A const reference to the matrix whose columns are the eigenvectors.
-      *
-      * \pre Either the constructor
-      * ComplexEigenSolver(const MatrixType& matrix, bool) or the member
-      * function compute(const MatrixType& matrix, bool) has been called before
-      * to compute the eigendecomposition of a matrix, and
-      * \p computeEigenvectors was set to true (the default).
-      *
-      * This function returns a matrix whose columns are the eigenvectors. Column
-      * \f$ k \f$ is an eigenvector corresponding to eigenvalue number \f$ k
-      * \f$ as returned by eigenvalues().  The eigenvectors are normalized to
-      * have (Euclidean) norm equal to one. The matrix returned by this
-      * function is the matrix \f$ V \f$ in the eigendecomposition \f$ A = V D
-      * V^{-1} \f$, if it exists.
-      *
-      * Example: \include ComplexEigenSolver_eigenvectors.cpp
-      * Output: \verbinclude ComplexEigenSolver_eigenvectors.out
-      */
-    const EigenvectorType& eigenvectors() const
-    {
-      eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized.");
-      eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
-      return m_eivec;
-    }
+  /** \brief Returns the eigenvalues of given matrix.
+   *
+   * \returns A const reference to the column vector containing the eigenvalues.
+   *
+   * \pre Either the constructor
+   * ComplexEigenSolver(const MatrixType& matrix, bool) or the member
+   * function compute(const MatrixType& matrix, bool) has been called before
+   * to compute the eigendecomposition of a matrix.
+   *
+   * This function returns a column vector containing the
+   * eigenvalues. Eigenvalues are repeated according to their
+   * algebraic multiplicity, so there are as many eigenvalues as
+   * rows in the matrix. The eigenvalues are not sorted in any particular
+   * order.
+   *
+   * Example: \include ComplexEigenSolver_eigenvalues.cpp
+   * Output: \verbinclude ComplexEigenSolver_eigenvalues.out
+   */
+  const EigenvalueType& eigenvalues() const {
+    eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized.");
+    return m_eivalues;
+  }
 
-    /** \brief Returns the eigenvalues of given matrix.
-      *
-      * \returns A const reference to the column vector containing the eigenvalues.
-      *
-      * \pre Either the constructor
-      * ComplexEigenSolver(const MatrixType& matrix, bool) or the member
-      * function compute(const MatrixType& matrix, bool) has been called before
-      * to compute the eigendecomposition of a matrix.
-      *
-      * This function returns a column vector containing the
-      * eigenvalues. Eigenvalues are repeated according to their
-      * algebraic multiplicity, so there are as many eigenvalues as
-      * rows in the matrix. The eigenvalues are not sorted in any particular
-      * order.
-      *
-      * Example: \include ComplexEigenSolver_eigenvalues.cpp
-      * Output: \verbinclude ComplexEigenSolver_eigenvalues.out
-      */
-    const EigenvalueType& eigenvalues() const
-    {
-      eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized.");
-      return m_eivalues;
-    }
+  /** \brief Computes eigendecomposition of given matrix.
+   *
+   * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
+   *    eigenvalues are computed; if false, only the eigenvalues are
+   *    computed.
+   * \returns    Reference to \c *this
+   *
+   * This function computes the eigenvalues of the complex matrix \p matrix.
+   * The eigenvalues() function can be used to retrieve them.  If
+   * \p computeEigenvectors is true, then the eigenvectors are also computed
+   * and can be retrieved by calling eigenvectors().
+   *
+   * The matrix is first reduced to Schur form using the
+   * ComplexSchur class. The Schur decomposition is then used to
+   * compute the eigenvalues and eigenvectors.
+   *
+   * The cost of the computation is dominated by the cost of the
+   * Schur decomposition, which is \f$ O(n^3) \f$ where \f$ n \f$
+   * is the size of the matrix.
+   *
+   * Example: \include ComplexEigenSolver_compute.cpp
+   * Output: \verbinclude ComplexEigenSolver_compute.out
+   */
+  template <typename InputType>
+  ComplexEigenSolver& compute(const EigenBase<InputType>& matrix, bool computeEigenvectors = true);
 
-    /** \brief Computes eigendecomposition of given matrix.
-      *
-      * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
-      *    eigenvalues are computed; if false, only the eigenvalues are
-      *    computed.
-      * \returns    Reference to \c *this
-      *
-      * This function computes the eigenvalues of the complex matrix \p matrix.
-      * The eigenvalues() function can be used to retrieve them.  If
-      * \p computeEigenvectors is true, then the eigenvectors are also computed
-      * and can be retrieved by calling eigenvectors().
-      *
-      * The matrix is first reduced to Schur form using the
-      * ComplexSchur class. The Schur decomposition is then used to
-      * compute the eigenvalues and eigenvectors.
-      *
-      * The cost of the computation is dominated by the cost of the
-      * Schur decomposition, which is \f$ O(n^3) \f$ where \f$ n \f$
-      * is the size of the matrix.
-      *
-      * Example: \include ComplexEigenSolver_compute.cpp
-      * Output: \verbinclude ComplexEigenSolver_compute.out
-      */
-    template<typename InputType>
-    ComplexEigenSolver& compute(const EigenBase<InputType>& matrix, bool computeEigenvectors = true);
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful, \c NoConvergence otherwise.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized.");
+    return m_schur.info();
+  }
 
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized.");
-      return m_schur.info();
-    }
+  /** \brief Sets the maximum number of iterations allowed. */
+  ComplexEigenSolver& setMaxIterations(Index maxIters) {
+    m_schur.setMaxIterations(maxIters);
+    return *this;
+  }
 
-    /** \brief Sets the maximum number of iterations allowed. */
-    ComplexEigenSolver& setMaxIterations(Index maxIters)
-    {
-      m_schur.setMaxIterations(maxIters);
-      return *this;
-    }
+  /** \brief Returns the maximum number of iterations. */
+  Index getMaxIterations() { return m_schur.getMaxIterations(); }
 
-    /** \brief Returns the maximum number of iterations. */
-    Index getMaxIterations()
-    {
-      return m_schur.getMaxIterations();
-    }
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-  protected:
-    
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
-    
-    EigenvectorType m_eivec;
-    EigenvalueType m_eivalues;
-    ComplexSchur<MatrixType> m_schur;
-    bool m_isInitialized;
-    bool m_eigenvectorsOk;
-    EigenvectorType m_matX;
+  EigenvectorType m_eivec;
+  EigenvalueType m_eivalues;
+  ComplexSchur<MatrixType> m_schur;
+  bool m_isInitialized;
+  bool m_eigenvectorsOk;
+  EigenvectorType m_matX;
 
-  private:
-    void doComputeEigenvectors(RealScalar matrixnorm);
-    void sortEigenvalues(bool computeEigenvectors);
+ private:
+  void doComputeEigenvectors(RealScalar matrixnorm);
+  void sortEigenvalues(bool computeEigenvectors);
 };
 
-
-template<typename MatrixType>
-template<typename InputType>
-ComplexEigenSolver<MatrixType>& 
-ComplexEigenSolver<MatrixType>::compute(const EigenBase<InputType>& matrix, bool computeEigenvectors)
-{
-  check_template_parameters();
-  
+template <typename MatrixType>
+template <typename InputType>
+ComplexEigenSolver<MatrixType>& ComplexEigenSolver<MatrixType>::compute(const EigenBase<InputType>& matrix,
+                                                                        bool computeEigenvectors) {
   // this code is inspired from Jampack
   eigen_assert(matrix.cols() == matrix.rows());
 
@@ -269,11 +250,9 @@
   // The eigenvalues are on the diagonal of T.
   m_schur.compute(matrix.derived(), computeEigenvectors);
 
-  if(m_schur.info() == Success)
-  {
+  if (m_schur.info() == Success) {
     m_eivalues = m_schur.matrixT().diagonal();
-    if(computeEigenvectors)
-      doComputeEigenvectors(m_schur.matrixT().norm());
+    if (computeEigenvectors) doComputeEigenvectors(m_schur.matrixT().norm());
     sortEigenvalues(computeEigenvectors);
   }
 
@@ -282,65 +261,55 @@
   return *this;
 }
 
-
-template<typename MatrixType>
-void ComplexEigenSolver<MatrixType>::doComputeEigenvectors(RealScalar matrixnorm)
-{
+template <typename MatrixType>
+void ComplexEigenSolver<MatrixType>::doComputeEigenvectors(RealScalar matrixnorm) {
   const Index n = m_eivalues.size();
 
-  matrixnorm = numext::maxi(matrixnorm,(std::numeric_limits<RealScalar>::min)());
+  matrixnorm = numext::maxi(matrixnorm, (std::numeric_limits<RealScalar>::min)());
 
   // Compute X such that T = X D X^(-1), where D is the diagonal of T.
   // The matrix X is unit triangular.
   m_matX = EigenvectorType::Zero(n, n);
-  for(Index k=n-1 ; k>=0 ; k--)
-  {
-    m_matX.coeffRef(k,k) = ComplexScalar(1.0,0.0);
+  for (Index k = n - 1; k >= 0; k--) {
+    m_matX.coeffRef(k, k) = ComplexScalar(1.0, 0.0);
     // Compute X(i,k) using the (i,k) entry of the equation X T = D X
-    for(Index i=k-1 ; i>=0 ; i--)
-    {
-      m_matX.coeffRef(i,k) = -m_schur.matrixT().coeff(i,k);
-      if(k-i-1>0)
-        m_matX.coeffRef(i,k) -= (m_schur.matrixT().row(i).segment(i+1,k-i-1) * m_matX.col(k).segment(i+1,k-i-1)).value();
-      ComplexScalar z = m_schur.matrixT().coeff(i,i) - m_schur.matrixT().coeff(k,k);
-      if(z==ComplexScalar(0))
-      {
+    for (Index i = k - 1; i >= 0; i--) {
+      m_matX.coeffRef(i, k) = -m_schur.matrixT().coeff(i, k);
+      if (k - i - 1 > 0)
+        m_matX.coeffRef(i, k) -=
+            (m_schur.matrixT().row(i).segment(i + 1, k - i - 1) * m_matX.col(k).segment(i + 1, k - i - 1)).value();
+      ComplexScalar z = m_schur.matrixT().coeff(i, i) - m_schur.matrixT().coeff(k, k);
+      if (z == ComplexScalar(0)) {
         // If the i-th and k-th eigenvalue are equal, then z equals 0.
         // Use a small value instead, to prevent division by zero.
         numext::real_ref(z) = NumTraits<RealScalar>::epsilon() * matrixnorm;
       }
-      m_matX.coeffRef(i,k) = m_matX.coeff(i,k) / z;
+      m_matX.coeffRef(i, k) = m_matX.coeff(i, k) / z;
     }
   }
 
   // Compute V as V = U X; now A = U T U^* = U X D X^(-1) U^* = V D V^(-1)
   m_eivec.noalias() = m_schur.matrixU() * m_matX;
   // .. and normalize the eigenvectors
-  for(Index k=0 ; k<n ; k++)
-  {
+  for (Index k = 0; k < n; k++) {
     m_eivec.col(k).normalize();
   }
 }
 
-
-template<typename MatrixType>
-void ComplexEigenSolver<MatrixType>::sortEigenvalues(bool computeEigenvectors)
-{
-  const Index n =  m_eivalues.size();
-  for (Index i=0; i<n; i++)
-  {
+template <typename MatrixType>
+void ComplexEigenSolver<MatrixType>::sortEigenvalues(bool computeEigenvectors) {
+  const Index n = m_eivalues.size();
+  for (Index i = 0; i < n; i++) {
     Index k;
-    m_eivalues.cwiseAbs().tail(n-i).minCoeff(&k);
-    if (k != 0)
-    {
+    m_eivalues.cwiseAbs().tail(n - i).minCoeff(&k);
+    if (k != 0) {
       k += i;
-      std::swap(m_eivalues[k],m_eivalues[i]);
-      if(computeEigenvectors)
-	m_eivec.col(i).swap(m_eivec.col(k));
+      std::swap(m_eivalues[k], m_eivalues[i]);
+      if (computeEigenvectors) m_eivec.col(i).swap(m_eivec.col(k));
     }
   }
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COMPLEX_EIGEN_SOLVER_H
+#endif  // EIGEN_COMPLEX_EIGEN_SOLVER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h
index fc71468..126b442 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h
@@ -14,416 +14,397 @@
 
 #include "./HessenbergDecomposition.h"
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-template<typename MatrixType, bool IsComplex> struct complex_schur_reduce_to_hessenberg;
+template <typename MatrixType, bool IsComplex>
+struct complex_schur_reduce_to_hessenberg;
 }
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class ComplexSchur
-  *
-  * \brief Performs a complex Schur decomposition of a real or complex square matrix
-  *
-  * \tparam _MatrixType the type of the matrix of which we are
-  * computing the Schur decomposition; this is expected to be an
-  * instantiation of the Matrix class template.
-  *
-  * Given a real or complex square matrix A, this class computes the
-  * Schur decomposition: \f$ A = U T U^*\f$ where U is a unitary
-  * complex matrix, and T is a complex upper triangular matrix.  The
-  * diagonal of the matrix T corresponds to the eigenvalues of the
-  * matrix A.
-  *
-  * Call the function compute() to compute the Schur decomposition of
-  * a given matrix. Alternatively, you can use the 
-  * ComplexSchur(const MatrixType&, bool) constructor which computes
-  * the Schur decomposition at construction time. Once the
-  * decomposition is computed, you can use the matrixU() and matrixT()
-  * functions to retrieve the matrices U and V in the decomposition.
-  *
-  * \note This code is inspired from Jampack
-  *
-  * \sa class RealSchur, class EigenSolver, class ComplexEigenSolver
-  */
-template<typename _MatrixType> class ComplexSchur
-{
-  public:
-    typedef _MatrixType MatrixType;
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      Options = MatrixType::Options,
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
+ *
+ *
+ * \class ComplexSchur
+ *
+ * \brief Performs a complex Schur decomposition of a real or complex square matrix
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are
+ * computing the Schur decomposition; this is expected to be an
+ * instantiation of the Matrix class template.
+ *
+ * Given a real or complex square matrix A, this class computes the
+ * Schur decomposition: \f$ A = U T U^*\f$ where U is a unitary
+ * complex matrix, and T is a complex upper triangular matrix.  The
+ * diagonal of the matrix T corresponds to the eigenvalues of the
+ * matrix A.
+ *
+ * Call the function compute() to compute the Schur decomposition of
+ * a given matrix. Alternatively, you can use the
+ * ComplexSchur(const MatrixType&, bool) constructor which computes
+ * the Schur decomposition at construction time. Once the
+ * decomposition is computed, you can use the matrixU() and matrixT()
+ * functions to retrieve the matrices U and V in the decomposition.
+ *
+ * \note This code is inspired from Jampack
+ *
+ * \sa class RealSchur, class EigenSolver, class ComplexEigenSolver
+ */
+template <typename MatrixType_>
+class ComplexSchur {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
 
-    /** \brief Scalar type for matrices of type \p _MatrixType. */
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  /** \brief Scalar type for matrices of type \p MatrixType_. */
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    /** \brief Complex scalar type for \p _MatrixType. 
-      *
-      * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
-      * \c float or \c double) and just \c Scalar if #Scalar is
-      * complex.
-      */
-    typedef std::complex<RealScalar> ComplexScalar;
+  /** \brief Complex scalar type for \p MatrixType_.
+   *
+   * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
+   * \c float or \c double) and just \c Scalar if #Scalar is
+   * complex.
+   */
+  typedef std::complex<RealScalar> ComplexScalar;
 
-    /** \brief Type for the matrices in the Schur decomposition.
-      *
-      * This is a square matrix with entries of type #ComplexScalar. 
-      * The size is the same as the size of \p _MatrixType.
-      */
-    typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> ComplexMatrixType;
+  /** \brief Type for the matrices in the Schur decomposition.
+   *
+   * This is a square matrix with entries of type #ComplexScalar.
+   * The size is the same as the size of \p MatrixType_.
+   */
+  typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime,
+                 MaxColsAtCompileTime>
+      ComplexMatrixType;
 
-    /** \brief Default constructor.
-      *
-      * \param [in] size  Positive integer, size of the matrix whose Schur decomposition will be computed.
-      *
-      * The default constructor is useful in cases in which the user
-      * intends to perform decompositions via compute().  The \p size
-      * parameter is only used as a hint. It is not an error to give a
-      * wrong \p size, but it may impair performance.
-      *
-      * \sa compute() for an example.
-      */
-    explicit ComplexSchur(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime)
-      : m_matT(size,size),
-        m_matU(size,size),
+  /** \brief Default constructor.
+   *
+   * \param [in] size  Positive integer, size of the matrix whose Schur decomposition will be computed.
+   *
+   * The default constructor is useful in cases in which the user
+   * intends to perform decompositions via compute().  The \p size
+   * parameter is only used as a hint. It is not an error to give a
+   * wrong \p size, but it may impair performance.
+   *
+   * \sa compute() for an example.
+   */
+  explicit ComplexSchur(Index size = RowsAtCompileTime == Dynamic ? 1 : RowsAtCompileTime)
+      : m_matT(size, size),
+        m_matU(size, size),
         m_hess(size),
         m_isInitialized(false),
         m_matUisUptodate(false),
-        m_maxIters(-1)
-    {}
+        m_maxIters(-1) {}
 
-    /** \brief Constructor; computes Schur decomposition of given matrix. 
-      * 
-      * \param[in]  matrix    Square matrix whose Schur decomposition is to be computed.
-      * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
-      *
-      * This constructor calls compute() to compute the Schur decomposition.
-      *
-      * \sa matrixT() and matrixU() for examples.
-      */
-    template<typename InputType>
-    explicit ComplexSchur(const EigenBase<InputType>& matrix, bool computeU = true)
-      : m_matT(matrix.rows(),matrix.cols()),
-        m_matU(matrix.rows(),matrix.cols()),
+  /** \brief Constructor; computes Schur decomposition of given matrix.
+   *
+   * \param[in]  matrix    Square matrix whose Schur decomposition is to be computed.
+   * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
+   *
+   * This constructor calls compute() to compute the Schur decomposition.
+   *
+   * \sa matrixT() and matrixU() for examples.
+   */
+  template <typename InputType>
+  explicit ComplexSchur(const EigenBase<InputType>& matrix, bool computeU = true)
+      : m_matT(matrix.rows(), matrix.cols()),
+        m_matU(matrix.rows(), matrix.cols()),
         m_hess(matrix.rows()),
         m_isInitialized(false),
         m_matUisUptodate(false),
-        m_maxIters(-1)
-    {
-      compute(matrix.derived(), computeU);
-    }
+        m_maxIters(-1) {
+    compute(matrix.derived(), computeU);
+  }
 
-    /** \brief Returns the unitary matrix in the Schur decomposition. 
-      *
-      * \returns A const reference to the matrix U.
-      *
-      * It is assumed that either the constructor
-      * ComplexSchur(const MatrixType& matrix, bool computeU) or the
-      * member function compute(const MatrixType& matrix, bool computeU)
-      * has been called before to compute the Schur decomposition of a
-      * matrix, and that \p computeU was set to true (the default
-      * value).
-      *
-      * Example: \include ComplexSchur_matrixU.cpp
-      * Output: \verbinclude ComplexSchur_matrixU.out
-      */
-    const ComplexMatrixType& matrixU() const
-    {
-      eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
-      eigen_assert(m_matUisUptodate && "The matrix U has not been computed during the ComplexSchur decomposition.");
-      return m_matU;
-    }
+  /** \brief Returns the unitary matrix in the Schur decomposition.
+   *
+   * \returns A const reference to the matrix U.
+   *
+   * It is assumed that either the constructor
+   * ComplexSchur(const MatrixType& matrix, bool computeU) or the
+   * member function compute(const MatrixType& matrix, bool computeU)
+   * has been called before to compute the Schur decomposition of a
+   * matrix, and that \p computeU was set to true (the default
+   * value).
+   *
+   * Example: \include ComplexSchur_matrixU.cpp
+   * Output: \verbinclude ComplexSchur_matrixU.out
+   */
+  const ComplexMatrixType& matrixU() const {
+    eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
+    eigen_assert(m_matUisUptodate && "The matrix U has not been computed during the ComplexSchur decomposition.");
+    return m_matU;
+  }
 
-    /** \brief Returns the triangular matrix in the Schur decomposition. 
-      *
-      * \returns A const reference to the matrix T.
-      *
-      * It is assumed that either the constructor
-      * ComplexSchur(const MatrixType& matrix, bool computeU) or the
-      * member function compute(const MatrixType& matrix, bool computeU)
-      * has been called before to compute the Schur decomposition of a
-      * matrix.
-      *
-      * Note that this function returns a plain square matrix. If you want to reference
-      * only the upper triangular part, use:
-      * \code schur.matrixT().triangularView<Upper>() \endcode 
-      *
-      * Example: \include ComplexSchur_matrixT.cpp
-      * Output: \verbinclude ComplexSchur_matrixT.out
-      */
-    const ComplexMatrixType& matrixT() const
-    {
-      eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
-      return m_matT;
-    }
+  /** \brief Returns the triangular matrix in the Schur decomposition.
+   *
+   * \returns A const reference to the matrix T.
+   *
+   * It is assumed that either the constructor
+   * ComplexSchur(const MatrixType& matrix, bool computeU) or the
+   * member function compute(const MatrixType& matrix, bool computeU)
+   * has been called before to compute the Schur decomposition of a
+   * matrix.
+   *
+   * Note that this function returns a plain square matrix. If you want to reference
+   * only the upper triangular part, use:
+   * \code schur.matrixT().triangularView<Upper>() \endcode
+   *
+   * Example: \include ComplexSchur_matrixT.cpp
+   * Output: \verbinclude ComplexSchur_matrixT.out
+   */
+  const ComplexMatrixType& matrixT() const {
+    eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
+    return m_matT;
+  }
 
-    /** \brief Computes Schur decomposition of given matrix. 
-      * 
-      * \param[in]  matrix  Square matrix whose Schur decomposition is to be computed.
-      * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
+  /** \brief Computes Schur decomposition of given matrix.
+    *
+    * \param[in]  matrix  Square matrix whose Schur decomposition is to be computed.
+    * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
 
-      * \returns    Reference to \c *this
-      *
-      * The Schur decomposition is computed by first reducing the
-      * matrix to Hessenberg form using the class
-      * HessenbergDecomposition. The Hessenberg matrix is then reduced
-      * to triangular form by performing QR iterations with a single
-      * shift. The cost of computing the Schur decomposition depends
-      * on the number of iterations; as a rough guide, it may be taken
-      * on the number of iterations; as a rough guide, it may be taken
-      * to be \f$25n^3\f$ complex flops, or \f$10n^3\f$ complex flops
-      * if \a computeU is false.
-      *
-      * Example: \include ComplexSchur_compute.cpp
-      * Output: \verbinclude ComplexSchur_compute.out
-      *
-      * \sa compute(const MatrixType&, bool, Index)
-      */
-    template<typename InputType>
-    ComplexSchur& compute(const EigenBase<InputType>& matrix, bool computeU = true);
-    
-    /** \brief Compute Schur decomposition from a given Hessenberg matrix
-     *  \param[in] matrixH Matrix in Hessenberg form H
-     *  \param[in] matrixQ orthogonal matrix Q that transform a matrix A to H : A = Q H Q^T
-     *  \param computeU Computes the matriX U of the Schur vectors
-     * \return Reference to \c *this
-     * 
-     *  This routine assumes that the matrix is already reduced in Hessenberg form matrixH
-     *  using either the class HessenbergDecomposition or another mean. 
-     *  It computes the upper quasi-triangular matrix T of the Schur decomposition of H
-     *  When computeU is true, this routine computes the matrix U such that 
-     *  A = U T U^T =  (QZ) T (QZ)^T = Q H Q^T where A is the initial matrix
-     * 
-     * NOTE Q is referenced if computeU is true; so, if the initial orthogonal matrix
-     * is not available, the user should give an identity matrix (Q.setIdentity())
-     * 
-     * \sa compute(const MatrixType&, bool)
-     */
-    template<typename HessMatrixType, typename OrthMatrixType>
-    ComplexSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ,  bool computeU=true);
+    * \returns    Reference to \c *this
+    *
+    * The Schur decomposition is computed by first reducing the
+    * matrix to Hessenberg form using the class
+    * HessenbergDecomposition. The Hessenberg matrix is then reduced
+    * to triangular form by performing QR iterations with a single
+    * shift. The cost of computing the Schur decomposition depends
+    * on the number of iterations; as a rough guide, it may be taken
+    * on the number of iterations; as a rough guide, it may be taken
+    * to be \f$25n^3\f$ complex flops, or \f$10n^3\f$ complex flops
+    * if \a computeU is false.
+    *
+    * Example: \include ComplexSchur_compute.cpp
+    * Output: \verbinclude ComplexSchur_compute.out
+    *
+    * \sa compute(const MatrixType&, bool, Index)
+    */
+  template <typename InputType>
+  ComplexSchur& compute(const EigenBase<InputType>& matrix, bool computeU = true);
 
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
-      return m_info;
-    }
+  /** \brief Compute Schur decomposition from a given Hessenberg matrix
+   *  \param[in] matrixH Matrix in Hessenberg form H
+   *  \param[in] matrixQ orthogonal matrix Q that transform a matrix A to H : A = Q H Q^T
+   *  \param computeU Computes the matriX U of the Schur vectors
+   * \return Reference to \c *this
+   *
+   *  This routine assumes that the matrix is already reduced in Hessenberg form matrixH
+   *  using either the class HessenbergDecomposition or another mean.
+   *  It computes the upper quasi-triangular matrix T of the Schur decomposition of H
+   *  When computeU is true, this routine computes the matrix U such that
+   *  A = U T U^T =  (QZ) T (QZ)^T = Q H Q^T where A is the initial matrix
+   *
+   * NOTE Q is referenced if computeU is true; so, if the initial orthogonal matrix
+   * is not available, the user should give an identity matrix (Q.setIdentity())
+   *
+   * \sa compute(const MatrixType&, bool)
+   */
+  template <typename HessMatrixType, typename OrthMatrixType>
+  ComplexSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ,
+                                      bool computeU = true);
 
-    /** \brief Sets the maximum number of iterations allowed. 
-      *
-      * If not specified by the user, the maximum number of iterations is m_maxIterationsPerRow times the size
-      * of the matrix.
-      */
-    ComplexSchur& setMaxIterations(Index maxIters)
-    {
-      m_maxIters = maxIters;
-      return *this;
-    }
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful, \c NoConvergence otherwise.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
+    return m_info;
+  }
 
-    /** \brief Returns the maximum number of iterations. */
-    Index getMaxIterations()
-    {
-      return m_maxIters;
-    }
+  /** \brief Sets the maximum number of iterations allowed.
+   *
+   * If not specified by the user, the maximum number of iterations is m_maxIterationsPerRow times the size
+   * of the matrix.
+   */
+  ComplexSchur& setMaxIterations(Index maxIters) {
+    m_maxIters = maxIters;
+    return *this;
+  }
 
-    /** \brief Maximum number of iterations per row.
-      *
-      * If not otherwise specified, the maximum number of iterations is this number times the size of the
-      * matrix. It is currently set to 30.
-      */
-    static const int m_maxIterationsPerRow = 30;
+  /** \brief Returns the maximum number of iterations. */
+  Index getMaxIterations() { return m_maxIters; }
 
-  protected:
-    ComplexMatrixType m_matT, m_matU;
-    HessenbergDecomposition<MatrixType> m_hess;
-    ComputationInfo m_info;
-    bool m_isInitialized;
-    bool m_matUisUptodate;
-    Index m_maxIters;
+  /** \brief Maximum number of iterations per row.
+   *
+   * If not otherwise specified, the maximum number of iterations is this number times the size of the
+   * matrix. It is currently set to 30.
+   */
+  static const int m_maxIterationsPerRow = 30;
 
-  private:  
-    bool subdiagonalEntryIsNeglegible(Index i);
-    ComplexScalar computeShift(Index iu, Index iter);
-    void reduceToTriangularForm(bool computeU);
-    friend struct internal::complex_schur_reduce_to_hessenberg<MatrixType, NumTraits<Scalar>::IsComplex>;
+ protected:
+  ComplexMatrixType m_matT, m_matU;
+  HessenbergDecomposition<MatrixType> m_hess;
+  ComputationInfo m_info;
+  bool m_isInitialized;
+  bool m_matUisUptodate;
+  Index m_maxIters;
+
+ private:
+  bool subdiagonalEntryIsNeglegible(Index i);
+  ComplexScalar computeShift(Index iu, Index iter);
+  void reduceToTriangularForm(bool computeU);
+  friend struct internal::complex_schur_reduce_to_hessenberg<MatrixType, NumTraits<Scalar>::IsComplex>;
 };
 
-/** If m_matT(i+1,i) is neglegible in floating point arithmetic
-  * compared to m_matT(i,i) and m_matT(j,j), then set it to zero and
-  * return true, else return false. */
-template<typename MatrixType>
-inline bool ComplexSchur<MatrixType>::subdiagonalEntryIsNeglegible(Index i)
-{
-  RealScalar d = numext::norm1(m_matT.coeff(i,i)) + numext::norm1(m_matT.coeff(i+1,i+1));
-  RealScalar sd = numext::norm1(m_matT.coeff(i+1,i));
-  if (internal::isMuchSmallerThan(sd, d, NumTraits<RealScalar>::epsilon()))
-  {
-    m_matT.coeffRef(i+1,i) = ComplexScalar(0);
+/** If m_matT(i+1,i) is negligible in floating point arithmetic
+ * compared to m_matT(i,i) and m_matT(j,j), then set it to zero and
+ * return true, else return false. */
+template <typename MatrixType>
+inline bool ComplexSchur<MatrixType>::subdiagonalEntryIsNeglegible(Index i) {
+  RealScalar d = numext::norm1(m_matT.coeff(i, i)) + numext::norm1(m_matT.coeff(i + 1, i + 1));
+  RealScalar sd = numext::norm1(m_matT.coeff(i + 1, i));
+  if (internal::isMuchSmallerThan(sd, d, NumTraits<RealScalar>::epsilon())) {
+    m_matT.coeffRef(i + 1, i) = ComplexScalar(0);
     return true;
   }
   return false;
 }
 
-
 /** Compute the shift in the current QR iteration. */
-template<typename MatrixType>
-typename ComplexSchur<MatrixType>::ComplexScalar ComplexSchur<MatrixType>::computeShift(Index iu, Index iter)
-{
+template <typename MatrixType>
+typename ComplexSchur<MatrixType>::ComplexScalar ComplexSchur<MatrixType>::computeShift(Index iu, Index iter) {
   using std::abs;
-  if (iter == 10 || iter == 20) 
-  {
+  if (iter == 10 || iter == 20) {
     // exceptional shift, taken from http://www.netlib.org/eispack/comqr.f
-    return abs(numext::real(m_matT.coeff(iu,iu-1))) + abs(numext::real(m_matT.coeff(iu-1,iu-2)));
+    return abs(numext::real(m_matT.coeff(iu, iu - 1))) + abs(numext::real(m_matT.coeff(iu - 1, iu - 2)));
   }
 
   // compute the shift as one of the eigenvalues of t, the 2x2
   // diagonal block on the bottom of the active submatrix
-  Matrix<ComplexScalar,2,2> t = m_matT.template block<2,2>(iu-1,iu-1);
+  Matrix<ComplexScalar, 2, 2> t = m_matT.template block<2, 2>(iu - 1, iu - 1);
   RealScalar normt = t.cwiseAbs().sum();
-  t /= normt;     // the normalization by sf is to avoid under/overflow
+  t /= normt;  // the normalization by sf is to avoid under/overflow
 
-  ComplexScalar b = t.coeff(0,1) * t.coeff(1,0);
-  ComplexScalar c = t.coeff(0,0) - t.coeff(1,1);
-  ComplexScalar disc = sqrt(c*c + RealScalar(4)*b);
-  ComplexScalar det = t.coeff(0,0) * t.coeff(1,1) - b;
-  ComplexScalar trace = t.coeff(0,0) + t.coeff(1,1);
+  ComplexScalar b = t.coeff(0, 1) * t.coeff(1, 0);
+  ComplexScalar c = t.coeff(0, 0) - t.coeff(1, 1);
+  ComplexScalar disc = sqrt(c * c + RealScalar(4) * b);
+  ComplexScalar det = t.coeff(0, 0) * t.coeff(1, 1) - b;
+  ComplexScalar trace = t.coeff(0, 0) + t.coeff(1, 1);
   ComplexScalar eival1 = (trace + disc) / RealScalar(2);
   ComplexScalar eival2 = (trace - disc) / RealScalar(2);
   RealScalar eival1_norm = numext::norm1(eival1);
   RealScalar eival2_norm = numext::norm1(eival2);
   // A division by zero can only occur if eival1==eival2==0.
   // In this case, det==0, and all we have to do is checking that eival2_norm!=0
-  if(eival1_norm > eival2_norm)
+  if (eival1_norm > eival2_norm)
     eival2 = det / eival1;
-  else if(eival2_norm!=RealScalar(0))
+  else if (!numext::is_exactly_zero(eival2_norm))
     eival1 = det / eival2;
 
   // choose the eigenvalue closest to the bottom entry of the diagonal
-  if(numext::norm1(eival1-t.coeff(1,1)) < numext::norm1(eival2-t.coeff(1,1)))
+  if (numext::norm1(eival1 - t.coeff(1, 1)) < numext::norm1(eival2 - t.coeff(1, 1)))
     return normt * eival1;
   else
     return normt * eival2;
 }
 
-
-template<typename MatrixType>
-template<typename InputType>
-ComplexSchur<MatrixType>& ComplexSchur<MatrixType>::compute(const EigenBase<InputType>& matrix, bool computeU)
-{
+template <typename MatrixType>
+template <typename InputType>
+ComplexSchur<MatrixType>& ComplexSchur<MatrixType>::compute(const EigenBase<InputType>& matrix, bool computeU) {
   m_matUisUptodate = false;
   eigen_assert(matrix.cols() == matrix.rows());
 
-  if(matrix.cols() == 1)
-  {
+  if (matrix.cols() == 1) {
     m_matT = matrix.derived().template cast<ComplexScalar>();
-    if(computeU)  m_matU = ComplexMatrixType::Identity(1,1);
+    if (computeU) m_matU = ComplexMatrixType::Identity(1, 1);
     m_info = Success;
     m_isInitialized = true;
     m_matUisUptodate = computeU;
     return *this;
   }
 
-  internal::complex_schur_reduce_to_hessenberg<MatrixType, NumTraits<Scalar>::IsComplex>::run(*this, matrix.derived(), computeU);
+  internal::complex_schur_reduce_to_hessenberg<MatrixType, NumTraits<Scalar>::IsComplex>::run(*this, matrix.derived(),
+                                                                                              computeU);
   computeFromHessenberg(m_matT, m_matU, computeU);
   return *this;
 }
 
-template<typename MatrixType>
-template<typename HessMatrixType, typename OrthMatrixType>
-ComplexSchur<MatrixType>& ComplexSchur<MatrixType>::computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ, bool computeU)
-{
+template <typename MatrixType>
+template <typename HessMatrixType, typename OrthMatrixType>
+ComplexSchur<MatrixType>& ComplexSchur<MatrixType>::computeFromHessenberg(const HessMatrixType& matrixH,
+                                                                          const OrthMatrixType& matrixQ,
+                                                                          bool computeU) {
   m_matT = matrixH;
-  if(computeU)
-    m_matU = matrixQ;
+  if (computeU) m_matU = matrixQ;
   reduceToTriangularForm(computeU);
   return *this;
 }
 namespace internal {
 
 /* Reduce given matrix to Hessenberg form */
-template<typename MatrixType, bool IsComplex>
-struct complex_schur_reduce_to_hessenberg
-{
+template <typename MatrixType, bool IsComplex>
+struct complex_schur_reduce_to_hessenberg {
   // this is the implementation for the case IsComplex = true
-  static void run(ComplexSchur<MatrixType>& _this, const MatrixType& matrix, bool computeU)
-  {
+  static void run(ComplexSchur<MatrixType>& _this, const MatrixType& matrix, bool computeU) {
     _this.m_hess.compute(matrix);
     _this.m_matT = _this.m_hess.matrixH();
-    if(computeU)  _this.m_matU = _this.m_hess.matrixQ();
+    if (computeU) _this.m_matU = _this.m_hess.matrixQ();
   }
 };
 
-template<typename MatrixType>
-struct complex_schur_reduce_to_hessenberg<MatrixType, false>
-{
-  static void run(ComplexSchur<MatrixType>& _this, const MatrixType& matrix, bool computeU)
-  {
+template <typename MatrixType>
+struct complex_schur_reduce_to_hessenberg<MatrixType, false> {
+  static void run(ComplexSchur<MatrixType>& _this, const MatrixType& matrix, bool computeU) {
     typedef typename ComplexSchur<MatrixType>::ComplexScalar ComplexScalar;
 
     // Note: m_hess is over RealScalar; m_matT and m_matU is over ComplexScalar
     _this.m_hess.compute(matrix);
     _this.m_matT = _this.m_hess.matrixH().template cast<ComplexScalar>();
-    if(computeU)  
-    {
+    if (computeU) {
       // This may cause an allocation which seems to be avoidable
-      MatrixType Q = _this.m_hess.matrixQ(); 
+      MatrixType Q = _this.m_hess.matrixQ();
       _this.m_matU = Q.template cast<ComplexScalar>();
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 // Reduce the Hessenberg matrix m_matT to triangular form by QR iteration.
-template<typename MatrixType>
-void ComplexSchur<MatrixType>::reduceToTriangularForm(bool computeU)
-{  
+template <typename MatrixType>
+void ComplexSchur<MatrixType>::reduceToTriangularForm(bool computeU) {
   Index maxIters = m_maxIters;
-  if (maxIters == -1)
-    maxIters = m_maxIterationsPerRow * m_matT.rows();
+  if (maxIters == -1) maxIters = m_maxIterationsPerRow * m_matT.rows();
 
-  // The matrix m_matT is divided in three parts. 
-  // Rows 0,...,il-1 are decoupled from the rest because m_matT(il,il-1) is zero. 
+  // The matrix m_matT is divided in three parts.
+  // Rows 0,...,il-1 are decoupled from the rest because m_matT(il,il-1) is zero.
   // Rows il,...,iu is the part we are working on (the active submatrix).
   // Rows iu+1,...,end are already brought in triangular form.
   Index iu = m_matT.cols() - 1;
   Index il;
-  Index iter = 0; // number of iterations we are working on the (iu,iu) element
-  Index totalIter = 0; // number of iterations for whole matrix
+  Index iter = 0;       // number of iterations we are working on the (iu,iu) element
+  Index totalIter = 0;  // number of iterations for whole matrix
 
-  while(true)
-  {
+  while (true) {
     // find iu, the bottom row of the active submatrix
-    while(iu > 0)
-    {
-      if(!subdiagonalEntryIsNeglegible(iu-1)) break;
+    while (iu > 0) {
+      if (!subdiagonalEntryIsNeglegible(iu - 1)) break;
       iter = 0;
       --iu;
     }
 
     // if iu is zero then we are done; the whole matrix is triangularized
-    if(iu==0) break;
+    if (iu == 0) break;
 
     // if we spent too many iterations, we give up
     iter++;
     totalIter++;
-    if(totalIter > maxIters) break;
+    if (totalIter > maxIters) break;
 
     // find il, the top row of the active submatrix
-    il = iu-1;
-    while(il > 0 && !subdiagonalEntryIsNeglegible(il-1))
-    {
+    il = iu - 1;
+    while (il > 0 && !subdiagonalEntryIsNeglegible(il - 1)) {
       --il;
     }
 
@@ -433,22 +414,21 @@
 
     ComplexScalar shift = computeShift(iu, iter);
     JacobiRotation<ComplexScalar> rot;
-    rot.makeGivens(m_matT.coeff(il,il) - shift, m_matT.coeff(il+1,il));
-    m_matT.rightCols(m_matT.cols()-il).applyOnTheLeft(il, il+1, rot.adjoint());
-    m_matT.topRows((std::min)(il+2,iu)+1).applyOnTheRight(il, il+1, rot);
-    if(computeU) m_matU.applyOnTheRight(il, il+1, rot);
+    rot.makeGivens(m_matT.coeff(il, il) - shift, m_matT.coeff(il + 1, il));
+    m_matT.rightCols(m_matT.cols() - il).applyOnTheLeft(il, il + 1, rot.adjoint());
+    m_matT.topRows((std::min)(il + 2, iu) + 1).applyOnTheRight(il, il + 1, rot);
+    if (computeU) m_matU.applyOnTheRight(il, il + 1, rot);
 
-    for(Index i=il+1 ; i<iu ; i++)
-    {
-      rot.makeGivens(m_matT.coeffRef(i,i-1), m_matT.coeffRef(i+1,i-1), &m_matT.coeffRef(i,i-1));
-      m_matT.coeffRef(i+1,i-1) = ComplexScalar(0);
-      m_matT.rightCols(m_matT.cols()-i).applyOnTheLeft(i, i+1, rot.adjoint());
-      m_matT.topRows((std::min)(i+2,iu)+1).applyOnTheRight(i, i+1, rot);
-      if(computeU) m_matU.applyOnTheRight(i, i+1, rot);
+    for (Index i = il + 1; i < iu; i++) {
+      rot.makeGivens(m_matT.coeffRef(i, i - 1), m_matT.coeffRef(i + 1, i - 1), &m_matT.coeffRef(i, i - 1));
+      m_matT.coeffRef(i + 1, i - 1) = ComplexScalar(0);
+      m_matT.rightCols(m_matT.cols() - i).applyOnTheLeft(i, i + 1, rot.adjoint());
+      m_matT.topRows((std::min)(i + 2, iu) + 1).applyOnTheRight(i, i + 1, rot);
+      if (computeU) m_matU.applyOnTheRight(i, i + 1, rot);
     }
   }
 
-  if(totalIter <= maxIters)
+  if (totalIter <= maxIters)
     m_info = Success;
   else
     m_info = NoConvergence;
@@ -457,6 +437,6 @@
   m_matUisUptodate = computeU;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COMPLEX_SCHUR_H
+#endif  // EIGEN_COMPLEX_SCHUR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h
index 572b29e..bb6583a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h
@@ -13,426 +13,404 @@
 
 #include "./RealSchur.h"
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class EigenSolver
-  *
-  * \brief Computes eigenvalues and eigenvectors of general matrices
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the
-  * eigendecomposition; this is expected to be an instantiation of the Matrix
-  * class template. Currently, only real matrices are supported.
-  *
-  * The eigenvalues and eigenvectors of a matrix \f$ A \f$ are scalars
-  * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda v \f$.  If
-  * \f$ D \f$ is a diagonal matrix with the eigenvalues on the diagonal, and
-  * \f$ V \f$ is a matrix with the eigenvectors as its columns, then \f$ A V =
-  * V D \f$. The matrix \f$ V \f$ is almost always invertible, in which case we
-  * have \f$ A = V D V^{-1} \f$. This is called the eigendecomposition.
-  *
-  * The eigenvalues and eigenvectors of a matrix may be complex, even when the
-  * matrix is real. However, we can choose real matrices \f$ V \f$ and \f$ D
-  * \f$ satisfying \f$ A V = V D \f$, just like the eigendecomposition, if the
-  * matrix \f$ D \f$ is not required to be diagonal, but if it is allowed to
-  * have blocks of the form
-  * \f[ \begin{bmatrix} u & v \\ -v & u \end{bmatrix} \f]
-  * (where \f$ u \f$ and \f$ v \f$ are real numbers) on the diagonal.  These
-  * blocks correspond to complex eigenvalue pairs \f$ u \pm iv \f$. We call
-  * this variant of the eigendecomposition the pseudo-eigendecomposition.
-  *
-  * Call the function compute() to compute the eigenvalues and eigenvectors of
-  * a given matrix. Alternatively, you can use the 
-  * EigenSolver(const MatrixType&, bool) constructor which computes the
-  * eigenvalues and eigenvectors at construction time. Once the eigenvalue and
-  * eigenvectors are computed, they can be retrieved with the eigenvalues() and
-  * eigenvectors() functions. The pseudoEigenvalueMatrix() and
-  * pseudoEigenvectors() methods allow the construction of the
-  * pseudo-eigendecomposition.
-  *
-  * The documentation for EigenSolver(const MatrixType&, bool) contains an
-  * example of the typical use of this class.
-  *
-  * \note The implementation is adapted from
-  * <a href="http://math.nist.gov/javanumerics/jama/">JAMA</a> (public domain).
-  * Their code is based on EISPACK.
-  *
-  * \sa MatrixBase::eigenvalues(), class ComplexEigenSolver, class SelfAdjointEigenSolver
-  */
-template<typename _MatrixType> class EigenSolver
-{
-  public:
+ *
+ *
+ * \class EigenSolver
+ *
+ * \brief Computes eigenvalues and eigenvectors of general matrices
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the
+ * eigendecomposition; this is expected to be an instantiation of the Matrix
+ * class template. Currently, only real matrices are supported.
+ *
+ * The eigenvalues and eigenvectors of a matrix \f$ A \f$ are scalars
+ * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda v \f$.  If
+ * \f$ D \f$ is a diagonal matrix with the eigenvalues on the diagonal, and
+ * \f$ V \f$ is a matrix with the eigenvectors as its columns, then \f$ A V =
+ * V D \f$. The matrix \f$ V \f$ is almost always invertible, in which case we
+ * have \f$ A = V D V^{-1} \f$. This is called the eigendecomposition.
+ *
+ * The eigenvalues and eigenvectors of a matrix may be complex, even when the
+ * matrix is real. However, we can choose real matrices \f$ V \f$ and \f$ D
+ * \f$ satisfying \f$ A V = V D \f$, just like the eigendecomposition, if the
+ * matrix \f$ D \f$ is not required to be diagonal, but if it is allowed to
+ * have blocks of the form
+ * \f[ \begin{bmatrix} u & v \\ -v & u \end{bmatrix} \f]
+ * (where \f$ u \f$ and \f$ v \f$ are real numbers) on the diagonal.  These
+ * blocks correspond to complex eigenvalue pairs \f$ u \pm iv \f$. We call
+ * this variant of the eigendecomposition the pseudo-eigendecomposition.
+ *
+ * Call the function compute() to compute the eigenvalues and eigenvectors of
+ * a given matrix. Alternatively, you can use the
+ * EigenSolver(const MatrixType&, bool) constructor which computes the
+ * eigenvalues and eigenvectors at construction time. Once the eigenvalue and
+ * eigenvectors are computed, they can be retrieved with the eigenvalues() and
+ * eigenvectors() functions. The pseudoEigenvalueMatrix() and
+ * pseudoEigenvectors() methods allow the construction of the
+ * pseudo-eigendecomposition.
+ *
+ * The documentation for EigenSolver(const MatrixType&, bool) contains an
+ * example of the typical use of this class.
+ *
+ * \note The implementation is adapted from
+ * <a href="http://math.nist.gov/javanumerics/jama/">JAMA</a> (public domain).
+ * Their code is based on EISPACK.
+ *
+ * \sa MatrixBase::eigenvalues(), class ComplexEigenSolver, class SelfAdjointEigenSolver
+ */
+template <typename MatrixType_>
+class EigenSolver {
+ public:
+  /** \brief Synonym for the template parameter \p MatrixType_. */
+  typedef MatrixType_ MatrixType;
 
-    /** \brief Synonym for the template parameter \p _MatrixType. */
-    typedef _MatrixType MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
 
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      Options = MatrixType::Options,
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
+  /** \brief Scalar type for matrices of type #MatrixType. */
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    /** \brief Scalar type for matrices of type #MatrixType. */
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  /** \brief Complex scalar type for #MatrixType.
+   *
+   * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
+   * \c float or \c double) and just \c Scalar if #Scalar is
+   * complex.
+   */
+  typedef std::complex<RealScalar> ComplexScalar;
 
-    /** \brief Complex scalar type for #MatrixType. 
-      *
-      * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
-      * \c float or \c double) and just \c Scalar if #Scalar is
-      * complex.
-      */
-    typedef std::complex<RealScalar> ComplexScalar;
+  /** \brief Type for vector of eigenvalues as returned by eigenvalues().
+   *
+   * This is a column vector with entries of type #ComplexScalar.
+   * The length of the vector is the size of #MatrixType.
+   */
+  typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
 
-    /** \brief Type for vector of eigenvalues as returned by eigenvalues(). 
-      *
-      * This is a column vector with entries of type #ComplexScalar.
-      * The length of the vector is the size of #MatrixType.
-      */
-    typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
+  /** \brief Type for matrix of eigenvectors as returned by eigenvectors().
+   *
+   * This is a square matrix with entries of type #ComplexScalar.
+   * The size is the same as the size of #MatrixType.
+   */
+  typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime,
+                 MaxColsAtCompileTime>
+      EigenvectorsType;
 
-    /** \brief Type for matrix of eigenvectors as returned by eigenvectors(). 
-      *
-      * This is a square matrix with entries of type #ComplexScalar. 
-      * The size is the same as the size of #MatrixType.
-      */
-    typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> EigenvectorsType;
+  /** \brief Default constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via EigenSolver::compute(const MatrixType&, bool).
+   *
+   * \sa compute() for an example.
+   */
+  EigenSolver()
+      : m_eivec(), m_eivalues(), m_isInitialized(false), m_eigenvectorsOk(false), m_realSchur(), m_matT(), m_tmp() {}
 
-    /** \brief Default constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via EigenSolver::compute(const MatrixType&, bool).
-      *
-      * \sa compute() for an example.
-      */
-    EigenSolver() : m_eivec(), m_eivalues(), m_isInitialized(false), m_eigenvectorsOk(false), m_realSchur(), m_matT(), m_tmp() {}
-
-    /** \brief Default constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa EigenSolver()
-      */
-    explicit EigenSolver(Index size)
+  /** \brief Default constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa EigenSolver()
+   */
+  explicit EigenSolver(Index size)
       : m_eivec(size, size),
         m_eivalues(size),
         m_isInitialized(false),
         m_eigenvectorsOk(false),
         m_realSchur(size),
-        m_matT(size, size), 
-        m_tmp(size)
-    {}
+        m_matT(size, size),
+        m_tmp(size) {}
 
-    /** \brief Constructor; computes eigendecomposition of given matrix. 
-      * 
-      * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
-      *    eigenvalues are computed; if false, only the eigenvalues are
-      *    computed. 
-      *
-      * This constructor calls compute() to compute the eigenvalues
-      * and eigenvectors.
-      *
-      * Example: \include EigenSolver_EigenSolver_MatrixType.cpp
-      * Output: \verbinclude EigenSolver_EigenSolver_MatrixType.out
-      *
-      * \sa compute()
-      */
-    template<typename InputType>
-    explicit EigenSolver(const EigenBase<InputType>& matrix, bool computeEigenvectors = true)
+  /** \brief Constructor; computes eigendecomposition of given matrix.
+   *
+   * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
+   *    eigenvalues are computed; if false, only the eigenvalues are
+   *    computed.
+   *
+   * This constructor calls compute() to compute the eigenvalues
+   * and eigenvectors.
+   *
+   * Example: \include EigenSolver_EigenSolver_MatrixType.cpp
+   * Output: \verbinclude EigenSolver_EigenSolver_MatrixType.out
+   *
+   * \sa compute()
+   */
+  template <typename InputType>
+  explicit EigenSolver(const EigenBase<InputType>& matrix, bool computeEigenvectors = true)
       : m_eivec(matrix.rows(), matrix.cols()),
         m_eivalues(matrix.cols()),
         m_isInitialized(false),
         m_eigenvectorsOk(false),
         m_realSchur(matrix.cols()),
-        m_matT(matrix.rows(), matrix.cols()), 
-        m_tmp(matrix.cols())
-    {
-      compute(matrix.derived(), computeEigenvectors);
-    }
+        m_matT(matrix.rows(), matrix.cols()),
+        m_tmp(matrix.cols()) {
+    compute(matrix.derived(), computeEigenvectors);
+  }
 
-    /** \brief Returns the eigenvectors of given matrix. 
-      *
-      * \returns  %Matrix whose columns are the (possibly complex) eigenvectors.
-      *
-      * \pre Either the constructor 
-      * EigenSolver(const MatrixType&,bool) or the member function
-      * compute(const MatrixType&, bool) has been called before, and
-      * \p computeEigenvectors was set to true (the default).
-      *
-      * Column \f$ k \f$ of the returned matrix is an eigenvector corresponding
-      * to eigenvalue number \f$ k \f$ as returned by eigenvalues().  The
-      * eigenvectors are normalized to have (Euclidean) norm equal to one. The
-      * matrix returned by this function is the matrix \f$ V \f$ in the
-      * eigendecomposition \f$ A = V D V^{-1} \f$, if it exists.
-      *
-      * Example: \include EigenSolver_eigenvectors.cpp
-      * Output: \verbinclude EigenSolver_eigenvectors.out
-      *
-      * \sa eigenvalues(), pseudoEigenvectors()
-      */
-    EigenvectorsType eigenvectors() const;
+  /** \brief Returns the eigenvectors of given matrix.
+   *
+   * \returns  %Matrix whose columns are the (possibly complex) eigenvectors.
+   *
+   * \pre Either the constructor
+   * EigenSolver(const MatrixType&,bool) or the member function
+   * compute(const MatrixType&, bool) has been called before, and
+   * \p computeEigenvectors was set to true (the default).
+   *
+   * Column \f$ k \f$ of the returned matrix is an eigenvector corresponding
+   * to eigenvalue number \f$ k \f$ as returned by eigenvalues().  The
+   * eigenvectors are normalized to have (Euclidean) norm equal to one. The
+   * matrix returned by this function is the matrix \f$ V \f$ in the
+   * eigendecomposition \f$ A = V D V^{-1} \f$, if it exists.
+   *
+   * Example: \include EigenSolver_eigenvectors.cpp
+   * Output: \verbinclude EigenSolver_eigenvectors.out
+   *
+   * \sa eigenvalues(), pseudoEigenvectors()
+   */
+  EigenvectorsType eigenvectors() const;
 
-    /** \brief Returns the pseudo-eigenvectors of given matrix. 
-      *
-      * \returns  Const reference to matrix whose columns are the pseudo-eigenvectors.
-      *
-      * \pre Either the constructor 
-      * EigenSolver(const MatrixType&,bool) or the member function
-      * compute(const MatrixType&, bool) has been called before, and
-      * \p computeEigenvectors was set to true (the default).
-      *
-      * The real matrix \f$ V \f$ returned by this function and the
-      * block-diagonal matrix \f$ D \f$ returned by pseudoEigenvalueMatrix()
-      * satisfy \f$ AV = VD \f$.
-      *
-      * Example: \include EigenSolver_pseudoEigenvectors.cpp
-      * Output: \verbinclude EigenSolver_pseudoEigenvectors.out
-      *
-      * \sa pseudoEigenvalueMatrix(), eigenvectors()
-      */
-    const MatrixType& pseudoEigenvectors() const
-    {
-      eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
-      eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
-      return m_eivec;
-    }
+  /** \brief Returns the pseudo-eigenvectors of given matrix.
+   *
+   * \returns  Const reference to matrix whose columns are the pseudo-eigenvectors.
+   *
+   * \pre Either the constructor
+   * EigenSolver(const MatrixType&,bool) or the member function
+   * compute(const MatrixType&, bool) has been called before, and
+   * \p computeEigenvectors was set to true (the default).
+   *
+   * The real matrix \f$ V \f$ returned by this function and the
+   * block-diagonal matrix \f$ D \f$ returned by pseudoEigenvalueMatrix()
+   * satisfy \f$ AV = VD \f$.
+   *
+   * Example: \include EigenSolver_pseudoEigenvectors.cpp
+   * Output: \verbinclude EigenSolver_pseudoEigenvectors.out
+   *
+   * \sa pseudoEigenvalueMatrix(), eigenvectors()
+   */
+  const MatrixType& pseudoEigenvectors() const {
+    eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
+    eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
+    return m_eivec;
+  }
 
-    /** \brief Returns the block-diagonal matrix in the pseudo-eigendecomposition.
-      *
-      * \returns  A block-diagonal matrix.
-      *
-      * \pre Either the constructor 
-      * EigenSolver(const MatrixType&,bool) or the member function
-      * compute(const MatrixType&, bool) has been called before.
-      *
-      * The matrix \f$ D \f$ returned by this function is real and
-      * block-diagonal. The blocks on the diagonal are either 1-by-1 or 2-by-2
-      * blocks of the form
-      * \f$ \begin{bmatrix} u & v \\ -v & u \end{bmatrix} \f$.
-      * These blocks are not sorted in any particular order.
-      * The matrix \f$ D \f$ and the matrix \f$ V \f$ returned by
-      * pseudoEigenvectors() satisfy \f$ AV = VD \f$.
-      *
-      * \sa pseudoEigenvectors() for an example, eigenvalues()
-      */
-    MatrixType pseudoEigenvalueMatrix() const;
+  /** \brief Returns the block-diagonal matrix in the pseudo-eigendecomposition.
+   *
+   * \returns  A block-diagonal matrix.
+   *
+   * \pre Either the constructor
+   * EigenSolver(const MatrixType&,bool) or the member function
+   * compute(const MatrixType&, bool) has been called before.
+   *
+   * The matrix \f$ D \f$ returned by this function is real and
+   * block-diagonal. The blocks on the diagonal are either 1-by-1 or 2-by-2
+   * blocks of the form
+   * \f$ \begin{bmatrix} u & v \\ -v & u \end{bmatrix} \f$.
+   * These blocks are not sorted in any particular order.
+   * The matrix \f$ D \f$ and the matrix \f$ V \f$ returned by
+   * pseudoEigenvectors() satisfy \f$ AV = VD \f$.
+   *
+   * \sa pseudoEigenvectors() for an example, eigenvalues()
+   */
+  MatrixType pseudoEigenvalueMatrix() const;
 
-    /** \brief Returns the eigenvalues of given matrix. 
-      *
-      * \returns A const reference to the column vector containing the eigenvalues.
-      *
-      * \pre Either the constructor 
-      * EigenSolver(const MatrixType&,bool) or the member function
-      * compute(const MatrixType&, bool) has been called before.
-      *
-      * The eigenvalues are repeated according to their algebraic multiplicity,
-      * so there are as many eigenvalues as rows in the matrix. The eigenvalues 
-      * are not sorted in any particular order.
-      *
-      * Example: \include EigenSolver_eigenvalues.cpp
-      * Output: \verbinclude EigenSolver_eigenvalues.out
-      *
-      * \sa eigenvectors(), pseudoEigenvalueMatrix(),
-      *     MatrixBase::eigenvalues()
-      */
-    const EigenvalueType& eigenvalues() const
-    {
-      eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
-      return m_eivalues;
-    }
+  /** \brief Returns the eigenvalues of given matrix.
+   *
+   * \returns A const reference to the column vector containing the eigenvalues.
+   *
+   * \pre Either the constructor
+   * EigenSolver(const MatrixType&,bool) or the member function
+   * compute(const MatrixType&, bool) has been called before.
+   *
+   * The eigenvalues are repeated according to their algebraic multiplicity,
+   * so there are as many eigenvalues as rows in the matrix. The eigenvalues
+   * are not sorted in any particular order.
+   *
+   * Example: \include EigenSolver_eigenvalues.cpp
+   * Output: \verbinclude EigenSolver_eigenvalues.out
+   *
+   * \sa eigenvectors(), pseudoEigenvalueMatrix(),
+   *     MatrixBase::eigenvalues()
+   */
+  const EigenvalueType& eigenvalues() const {
+    eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
+    return m_eivalues;
+  }
 
-    /** \brief Computes eigendecomposition of given matrix. 
-      * 
-      * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
-      *    eigenvalues are computed; if false, only the eigenvalues are
-      *    computed. 
-      * \returns    Reference to \c *this
-      *
-      * This function computes the eigenvalues of the real matrix \p matrix.
-      * The eigenvalues() function can be used to retrieve them.  If 
-      * \p computeEigenvectors is true, then the eigenvectors are also computed
-      * and can be retrieved by calling eigenvectors().
-      *
-      * The matrix is first reduced to real Schur form using the RealSchur
-      * class. The Schur decomposition is then used to compute the eigenvalues
-      * and eigenvectors.
-      *
-      * The cost of the computation is dominated by the cost of the
-      * Schur decomposition, which is very approximately \f$ 25n^3 \f$
-      * (where \f$ n \f$ is the size of the matrix) if \p computeEigenvectors 
-      * is true, and \f$ 10n^3 \f$ if \p computeEigenvectors is false.
-      *
-      * This method reuses of the allocated data in the EigenSolver object.
-      *
-      * Example: \include EigenSolver_compute.cpp
-      * Output: \verbinclude EigenSolver_compute.out
-      */
-    template<typename InputType>
-    EigenSolver& compute(const EigenBase<InputType>& matrix, bool computeEigenvectors = true);
+  /** \brief Computes eigendecomposition of given matrix.
+   *
+   * \param[in]  matrix  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
+   *    eigenvalues are computed; if false, only the eigenvalues are
+   *    computed.
+   * \returns    Reference to \c *this
+   *
+   * This function computes the eigenvalues of the real matrix \p matrix.
+   * The eigenvalues() function can be used to retrieve them.  If
+   * \p computeEigenvectors is true, then the eigenvectors are also computed
+   * and can be retrieved by calling eigenvectors().
+   *
+   * The matrix is first reduced to real Schur form using the RealSchur
+   * class. The Schur decomposition is then used to compute the eigenvalues
+   * and eigenvectors.
+   *
+   * The cost of the computation is dominated by the cost of the
+   * Schur decomposition, which is very approximately \f$ 25n^3 \f$
+   * (where \f$ n \f$ is the size of the matrix) if \p computeEigenvectors
+   * is true, and \f$ 10n^3 \f$ if \p computeEigenvectors is false.
+   *
+   * This method reuses of the allocated data in the EigenSolver object.
+   *
+   * Example: \include EigenSolver_compute.cpp
+   * Output: \verbinclude EigenSolver_compute.out
+   */
+  template <typename InputType>
+  EigenSolver& compute(const EigenBase<InputType>& matrix, bool computeEigenvectors = true);
 
-    /** \returns NumericalIssue if the input contains INF or NaN values or overflow occurred. Returns Success otherwise. */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
-      return m_info;
-    }
+  /** \returns NumericalIssue if the input contains INF or NaN values or overflow occurred. Returns Success otherwise.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
+    return m_info;
+  }
 
-    /** \brief Sets the maximum number of iterations allowed. */
-    EigenSolver& setMaxIterations(Index maxIters)
-    {
-      m_realSchur.setMaxIterations(maxIters);
-      return *this;
-    }
+  /** \brief Sets the maximum number of iterations allowed. */
+  EigenSolver& setMaxIterations(Index maxIters) {
+    m_realSchur.setMaxIterations(maxIters);
+    return *this;
+  }
 
-    /** \brief Returns the maximum number of iterations. */
-    Index getMaxIterations()
-    {
-      return m_realSchur.getMaxIterations();
-    }
+  /** \brief Returns the maximum number of iterations. */
+  Index getMaxIterations() { return m_realSchur.getMaxIterations(); }
 
-  private:
-    void doComputeEigenvectors();
+ private:
+  void doComputeEigenvectors();
 
-  protected:
-    
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-      EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL);
-    }
-    
-    MatrixType m_eivec;
-    EigenvalueType m_eivalues;
-    bool m_isInitialized;
-    bool m_eigenvectorsOk;
-    ComputationInfo m_info;
-    RealSchur<MatrixType> m_realSchur;
-    MatrixType m_matT;
+ protected:
+  static void check_template_parameters() {
+    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL);
+  }
 
-    typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;
-    ColumnVectorType m_tmp;
+  MatrixType m_eivec;
+  EigenvalueType m_eivalues;
+  bool m_isInitialized;
+  bool m_eigenvectorsOk;
+  ComputationInfo m_info;
+  RealSchur<MatrixType> m_realSchur;
+  MatrixType m_matT;
+
+  typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;
+  ColumnVectorType m_tmp;
 };
 
-template<typename MatrixType>
-MatrixType EigenSolver<MatrixType>::pseudoEigenvalueMatrix() const
-{
+template <typename MatrixType>
+MatrixType EigenSolver<MatrixType>::pseudoEigenvalueMatrix() const {
   eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
-  const RealScalar precision = RealScalar(2)*NumTraits<RealScalar>::epsilon();
+  const RealScalar precision = RealScalar(2) * NumTraits<RealScalar>::epsilon();
   Index n = m_eivalues.rows();
-  MatrixType matD = MatrixType::Zero(n,n);
-  for (Index i=0; i<n; ++i)
-  {
+  MatrixType matD = MatrixType::Zero(n, n);
+  for (Index i = 0; i < n; ++i) {
     if (internal::isMuchSmallerThan(numext::imag(m_eivalues.coeff(i)), numext::real(m_eivalues.coeff(i)), precision))
-      matD.coeffRef(i,i) = numext::real(m_eivalues.coeff(i));
-    else
-    {
-      matD.template block<2,2>(i,i) <<  numext::real(m_eivalues.coeff(i)), numext::imag(m_eivalues.coeff(i)),
-                                       -numext::imag(m_eivalues.coeff(i)), numext::real(m_eivalues.coeff(i));
+      matD.coeffRef(i, i) = numext::real(m_eivalues.coeff(i));
+    else {
+      matD.template block<2, 2>(i, i) << numext::real(m_eivalues.coeff(i)), numext::imag(m_eivalues.coeff(i)),
+          -numext::imag(m_eivalues.coeff(i)), numext::real(m_eivalues.coeff(i));
       ++i;
     }
   }
   return matD;
 }
 
-template<typename MatrixType>
-typename EigenSolver<MatrixType>::EigenvectorsType EigenSolver<MatrixType>::eigenvectors() const
-{
+template <typename MatrixType>
+typename EigenSolver<MatrixType>::EigenvectorsType EigenSolver<MatrixType>::eigenvectors() const {
   eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
   eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
-  const RealScalar precision = RealScalar(2)*NumTraits<RealScalar>::epsilon();
+  const RealScalar precision = RealScalar(2) * NumTraits<RealScalar>::epsilon();
   Index n = m_eivec.cols();
-  EigenvectorsType matV(n,n);
-  for (Index j=0; j<n; ++j)
-  {
-    if (internal::isMuchSmallerThan(numext::imag(m_eivalues.coeff(j)), numext::real(m_eivalues.coeff(j)), precision) || j+1==n)
-    {
+  EigenvectorsType matV(n, n);
+  for (Index j = 0; j < n; ++j) {
+    if (internal::isMuchSmallerThan(numext::imag(m_eivalues.coeff(j)), numext::real(m_eivalues.coeff(j)), precision) ||
+        j + 1 == n) {
       // we have a real eigen value
       matV.col(j) = m_eivec.col(j).template cast<ComplexScalar>();
       matV.col(j).normalize();
-    }
-    else
-    {
+    } else {
       // we have a pair of complex eigen values
-      for (Index i=0; i<n; ++i)
-      {
-        matV.coeffRef(i,j)   = ComplexScalar(m_eivec.coeff(i,j),  m_eivec.coeff(i,j+1));
-        matV.coeffRef(i,j+1) = ComplexScalar(m_eivec.coeff(i,j), -m_eivec.coeff(i,j+1));
+      for (Index i = 0; i < n; ++i) {
+        matV.coeffRef(i, j) = ComplexScalar(m_eivec.coeff(i, j), m_eivec.coeff(i, j + 1));
+        matV.coeffRef(i, j + 1) = ComplexScalar(m_eivec.coeff(i, j), -m_eivec.coeff(i, j + 1));
       }
       matV.col(j).normalize();
-      matV.col(j+1).normalize();
+      matV.col(j + 1).normalize();
       ++j;
     }
   }
   return matV;
 }
 
-template<typename MatrixType>
-template<typename InputType>
-EigenSolver<MatrixType>& 
-EigenSolver<MatrixType>::compute(const EigenBase<InputType>& matrix, bool computeEigenvectors)
-{
+template <typename MatrixType>
+template <typename InputType>
+EigenSolver<MatrixType>& EigenSolver<MatrixType>::compute(const EigenBase<InputType>& matrix,
+                                                          bool computeEigenvectors) {
   check_template_parameters();
-  
-  using std::sqrt;
-  using std::abs;
+
   using numext::isfinite;
+  using std::abs;
+  using std::sqrt;
   eigen_assert(matrix.cols() == matrix.rows());
 
   // Reduce to real Schur form.
   m_realSchur.compute(matrix.derived(), computeEigenvectors);
-  
+
   m_info = m_realSchur.info();
 
-  if (m_info == Success)
-  {
+  if (m_info == Success) {
     m_matT = m_realSchur.matrixT();
-    if (computeEigenvectors)
-      m_eivec = m_realSchur.matrixU();
-  
+    if (computeEigenvectors) m_eivec = m_realSchur.matrixU();
+
     // Compute eigenvalues from matT
     m_eivalues.resize(matrix.cols());
     Index i = 0;
-    while (i < matrix.cols()) 
-    {
-      if (i == matrix.cols() - 1 || m_matT.coeff(i+1, i) == Scalar(0)) 
-      {
+    while (i < matrix.cols()) {
+      if (i == matrix.cols() - 1 || m_matT.coeff(i + 1, i) == Scalar(0)) {
         m_eivalues.coeffRef(i) = m_matT.coeff(i, i);
-        if(!(isfinite)(m_eivalues.coeffRef(i)))
-        {
+        if (!(isfinite)(m_eivalues.coeffRef(i))) {
           m_isInitialized = true;
           m_eigenvectorsOk = false;
           m_info = NumericalIssue;
           return *this;
         }
         ++i;
-      }
-      else
-      {
-        Scalar p = Scalar(0.5) * (m_matT.coeff(i, i) - m_matT.coeff(i+1, i+1));
+      } else {
+        Scalar p = Scalar(0.5) * (m_matT.coeff(i, i) - m_matT.coeff(i + 1, i + 1));
         Scalar z;
         // Compute z = sqrt(abs(p * p + m_matT.coeff(i+1, i) * m_matT.coeff(i, i+1)));
         // without overflow
         {
-          Scalar t0 = m_matT.coeff(i+1, i);
-          Scalar t1 = m_matT.coeff(i, i+1);
-          Scalar maxval = numext::maxi<Scalar>(abs(p),numext::maxi<Scalar>(abs(t0),abs(t1)));
+          Scalar t0 = m_matT.coeff(i + 1, i);
+          Scalar t1 = m_matT.coeff(i, i + 1);
+          Scalar maxval = numext::maxi<Scalar>(abs(p), numext::maxi<Scalar>(abs(t0), abs(t1)));
           t0 /= maxval;
           t1 /= maxval;
-          Scalar p0 = p/maxval;
+          Scalar p0 = p / maxval;
           z = maxval * sqrt(abs(p0 * p0 + t0 * t1));
         }
-        
-        m_eivalues.coeffRef(i)   = ComplexScalar(m_matT.coeff(i+1, i+1) + p, z);
-        m_eivalues.coeffRef(i+1) = ComplexScalar(m_matT.coeff(i+1, i+1) + p, -z);
-        if(!((isfinite)(m_eivalues.coeffRef(i)) && (isfinite)(m_eivalues.coeffRef(i+1))))
-        {
+
+        m_eivalues.coeffRef(i) = ComplexScalar(m_matT.coeff(i + 1, i + 1) + p, z);
+        m_eivalues.coeffRef(i + 1) = ComplexScalar(m_matT.coeff(i + 1, i + 1) + p, -z);
+        if (!((isfinite)(m_eivalues.coeffRef(i)) && (isfinite)(m_eivalues.coeffRef(i + 1)))) {
           m_isInitialized = true;
           m_eigenvectorsOk = false;
           m_info = NumericalIssue;
@@ -441,10 +419,9 @@
         i += 2;
       }
     }
-    
+
     // Compute eigenvectors.
-    if (computeEigenvectors)
-      doComputeEigenvectors();
+    if (computeEigenvectors) doComputeEigenvectors();
   }
 
   m_isInitialized = true;
@@ -453,170 +430,143 @@
   return *this;
 }
 
-
-template<typename MatrixType>
-void EigenSolver<MatrixType>::doComputeEigenvectors()
-{
+template <typename MatrixType>
+void EigenSolver<MatrixType>::doComputeEigenvectors() {
   using std::abs;
   const Index size = m_eivec.cols();
   const Scalar eps = NumTraits<Scalar>::epsilon();
 
   // inefficient! this is already computed in RealSchur
   Scalar norm(0);
-  for (Index j = 0; j < size; ++j)
-  {
-    norm += m_matT.row(j).segment((std::max)(j-1,Index(0)), size-(std::max)(j-1,Index(0))).cwiseAbs().sum();
+  for (Index j = 0; j < size; ++j) {
+    norm += m_matT.row(j).segment((std::max)(j - 1, Index(0)), size - (std::max)(j - 1, Index(0))).cwiseAbs().sum();
   }
-  
+
   // Backsubstitute to find vectors of upper triangular form
-  if (norm == Scalar(0))
-  {
+  if (norm == Scalar(0)) {
     return;
   }
 
-  for (Index n = size-1; n >= 0; n--)
-  {
+  for (Index n = size - 1; n >= 0; n--) {
     Scalar p = m_eivalues.coeff(n).real();
     Scalar q = m_eivalues.coeff(n).imag();
 
     // Scalar vector
-    if (q == Scalar(0))
-    {
+    if (q == Scalar(0)) {
       Scalar lastr(0), lastw(0);
       Index l = n;
 
-      m_matT.coeffRef(n,n) = Scalar(1);
-      for (Index i = n-1; i >= 0; i--)
-      {
-        Scalar w = m_matT.coeff(i,i) - p;
-        Scalar r = m_matT.row(i).segment(l,n-l+1).dot(m_matT.col(n).segment(l, n-l+1));
+      m_matT.coeffRef(n, n) = Scalar(1);
+      for (Index i = n - 1; i >= 0; i--) {
+        Scalar w = m_matT.coeff(i, i) - p;
+        Scalar r = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n).segment(l, n - l + 1));
 
-        if (m_eivalues.coeff(i).imag() < Scalar(0))
-        {
+        if (m_eivalues.coeff(i).imag() < Scalar(0)) {
           lastw = w;
           lastr = r;
-        }
-        else
-        {
+        } else {
           l = i;
-          if (m_eivalues.coeff(i).imag() == Scalar(0))
-          {
+          if (m_eivalues.coeff(i).imag() == Scalar(0)) {
             if (w != Scalar(0))
-              m_matT.coeffRef(i,n) = -r / w;
+              m_matT.coeffRef(i, n) = -r / w;
             else
-              m_matT.coeffRef(i,n) = -r / (eps * norm);
-          }
-          else // Solve real equations
+              m_matT.coeffRef(i, n) = -r / (eps * norm);
+          } else  // Solve real equations
           {
-            Scalar x = m_matT.coeff(i,i+1);
-            Scalar y = m_matT.coeff(i+1,i);
-            Scalar denom = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag();
+            Scalar x = m_matT.coeff(i, i + 1);
+            Scalar y = m_matT.coeff(i + 1, i);
+            Scalar denom = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) +
+                           m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag();
             Scalar t = (x * lastr - lastw * r) / denom;
-            m_matT.coeffRef(i,n) = t;
+            m_matT.coeffRef(i, n) = t;
             if (abs(x) > abs(lastw))
-              m_matT.coeffRef(i+1,n) = (-r - w * t) / x;
+              m_matT.coeffRef(i + 1, n) = (-r - w * t) / x;
             else
-              m_matT.coeffRef(i+1,n) = (-lastr - y * t) / lastw;
+              m_matT.coeffRef(i + 1, n) = (-lastr - y * t) / lastw;
           }
 
           // Overflow control
-          Scalar t = abs(m_matT.coeff(i,n));
-          if ((eps * t) * t > Scalar(1))
-            m_matT.col(n).tail(size-i) /= t;
+          Scalar t = abs(m_matT.coeff(i, n));
+          if ((eps * t) * t > Scalar(1)) m_matT.col(n).tail(size - i) /= t;
         }
       }
-    }
-    else if (q < Scalar(0) && n > 0) // Complex vector
+    } else if (q < Scalar(0) && n > 0)  // Complex vector
     {
       Scalar lastra(0), lastsa(0), lastw(0);
-      Index l = n-1;
+      Index l = n - 1;
 
       // Last vector component imaginary so matrix is triangular
-      if (abs(m_matT.coeff(n,n-1)) > abs(m_matT.coeff(n-1,n)))
-      {
-        m_matT.coeffRef(n-1,n-1) = q / m_matT.coeff(n,n-1);
-        m_matT.coeffRef(n-1,n) = -(m_matT.coeff(n,n) - p) / m_matT.coeff(n,n-1);
+      if (abs(m_matT.coeff(n, n - 1)) > abs(m_matT.coeff(n - 1, n))) {
+        m_matT.coeffRef(n - 1, n - 1) = q / m_matT.coeff(n, n - 1);
+        m_matT.coeffRef(n - 1, n) = -(m_matT.coeff(n, n) - p) / m_matT.coeff(n, n - 1);
+      } else {
+        ComplexScalar cc =
+            ComplexScalar(Scalar(0), -m_matT.coeff(n - 1, n)) / ComplexScalar(m_matT.coeff(n - 1, n - 1) - p, q);
+        m_matT.coeffRef(n - 1, n - 1) = numext::real(cc);
+        m_matT.coeffRef(n - 1, n) = numext::imag(cc);
       }
-      else
-      {
-        ComplexScalar cc = ComplexScalar(Scalar(0),-m_matT.coeff(n-1,n)) / ComplexScalar(m_matT.coeff(n-1,n-1)-p,q);
-        m_matT.coeffRef(n-1,n-1) = numext::real(cc);
-        m_matT.coeffRef(n-1,n) = numext::imag(cc);
-      }
-      m_matT.coeffRef(n,n-1) = Scalar(0);
-      m_matT.coeffRef(n,n) = Scalar(1);
-      for (Index i = n-2; i >= 0; i--)
-      {
-        Scalar ra = m_matT.row(i).segment(l, n-l+1).dot(m_matT.col(n-1).segment(l, n-l+1));
-        Scalar sa = m_matT.row(i).segment(l, n-l+1).dot(m_matT.col(n).segment(l, n-l+1));
-        Scalar w = m_matT.coeff(i,i) - p;
+      m_matT.coeffRef(n, n - 1) = Scalar(0);
+      m_matT.coeffRef(n, n) = Scalar(1);
+      for (Index i = n - 2; i >= 0; i--) {
+        Scalar ra = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n - 1).segment(l, n - l + 1));
+        Scalar sa = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n).segment(l, n - l + 1));
+        Scalar w = m_matT.coeff(i, i) - p;
 
-        if (m_eivalues.coeff(i).imag() < Scalar(0))
-        {
+        if (m_eivalues.coeff(i).imag() < Scalar(0)) {
           lastw = w;
           lastra = ra;
           lastsa = sa;
-        }
-        else
-        {
+        } else {
           l = i;
-          if (m_eivalues.coeff(i).imag() == RealScalar(0))
-          {
-            ComplexScalar cc = ComplexScalar(-ra,-sa) / ComplexScalar(w,q);
-            m_matT.coeffRef(i,n-1) = numext::real(cc);
-            m_matT.coeffRef(i,n) = numext::imag(cc);
-          }
-          else
-          {
+          if (m_eivalues.coeff(i).imag() == RealScalar(0)) {
+            ComplexScalar cc = ComplexScalar(-ra, -sa) / ComplexScalar(w, q);
+            m_matT.coeffRef(i, n - 1) = numext::real(cc);
+            m_matT.coeffRef(i, n) = numext::imag(cc);
+          } else {
             // Solve complex equations
-            Scalar x = m_matT.coeff(i,i+1);
-            Scalar y = m_matT.coeff(i+1,i);
-            Scalar vr = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag() - q * q;
+            Scalar x = m_matT.coeff(i, i + 1);
+            Scalar y = m_matT.coeff(i + 1, i);
+            Scalar vr = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) +
+                        m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag() - q * q;
             Scalar vi = (m_eivalues.coeff(i).real() - p) * Scalar(2) * q;
             if ((vr == Scalar(0)) && (vi == Scalar(0)))
               vr = eps * norm * (abs(w) + abs(q) + abs(x) + abs(y) + abs(lastw));
 
-            ComplexScalar cc = ComplexScalar(x*lastra-lastw*ra+q*sa,x*lastsa-lastw*sa-q*ra) / ComplexScalar(vr,vi);
-            m_matT.coeffRef(i,n-1) = numext::real(cc);
-            m_matT.coeffRef(i,n) = numext::imag(cc);
-            if (abs(x) > (abs(lastw) + abs(q)))
-            {
-              m_matT.coeffRef(i+1,n-1) = (-ra - w * m_matT.coeff(i,n-1) + q * m_matT.coeff(i,n)) / x;
-              m_matT.coeffRef(i+1,n) = (-sa - w * m_matT.coeff(i,n) - q * m_matT.coeff(i,n-1)) / x;
-            }
-            else
-            {
-              cc = ComplexScalar(-lastra-y*m_matT.coeff(i,n-1),-lastsa-y*m_matT.coeff(i,n)) / ComplexScalar(lastw,q);
-              m_matT.coeffRef(i+1,n-1) = numext::real(cc);
-              m_matT.coeffRef(i+1,n) = numext::imag(cc);
+            ComplexScalar cc = ComplexScalar(x * lastra - lastw * ra + q * sa, x * lastsa - lastw * sa - q * ra) /
+                               ComplexScalar(vr, vi);
+            m_matT.coeffRef(i, n - 1) = numext::real(cc);
+            m_matT.coeffRef(i, n) = numext::imag(cc);
+            if (abs(x) > (abs(lastw) + abs(q))) {
+              m_matT.coeffRef(i + 1, n - 1) = (-ra - w * m_matT.coeff(i, n - 1) + q * m_matT.coeff(i, n)) / x;
+              m_matT.coeffRef(i + 1, n) = (-sa - w * m_matT.coeff(i, n) - q * m_matT.coeff(i, n - 1)) / x;
+            } else {
+              cc = ComplexScalar(-lastra - y * m_matT.coeff(i, n - 1), -lastsa - y * m_matT.coeff(i, n)) /
+                   ComplexScalar(lastw, q);
+              m_matT.coeffRef(i + 1, n - 1) = numext::real(cc);
+              m_matT.coeffRef(i + 1, n) = numext::imag(cc);
             }
           }
 
           // Overflow control
-          Scalar t = numext::maxi<Scalar>(abs(m_matT.coeff(i,n-1)),abs(m_matT.coeff(i,n)));
-          if ((eps * t) * t > Scalar(1))
-            m_matT.block(i, n-1, size-i, 2) /= t;
-
+          Scalar t = numext::maxi<Scalar>(abs(m_matT.coeff(i, n - 1)), abs(m_matT.coeff(i, n)));
+          if ((eps * t) * t > Scalar(1)) m_matT.block(i, n - 1, size - i, 2) /= t;
         }
       }
-      
+
       // We handled a pair of complex conjugate eigenvalues, so need to skip them both
       n--;
-    }
-    else
-    {
-      eigen_assert(0 && "Internal bug in EigenSolver (INF or NaN has not been detected)"); // this should not happen
+    } else {
+      eigen_assert(0 && "Internal bug in EigenSolver (INF or NaN has not been detected)");  // this should not happen
     }
   }
 
   // Back transformation to get eigenvectors of original matrix
-  for (Index j = size-1; j >= 0; j--)
-  {
-    m_tmp.noalias() = m_eivec.leftCols(j+1) * m_matT.col(j).segment(0, j+1);
+  for (Index j = size - 1; j >= 0; j--) {
+    m_tmp.noalias() = m_eivec.leftCols(j + 1) * m_matT.col(j).segment(0, j + 1);
     m_eivec.col(j) = m_tmp;
   }
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_EIGENSOLVER_H
+#endif  // EIGEN_EIGENSOLVER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
index 87d789b..95954e7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
@@ -14,339 +14,322 @@
 
 #include "./RealQZ.h"
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class GeneralizedEigenSolver
-  *
-  * \brief Computes the generalized eigenvalues and eigenvectors of a pair of general matrices
-  *
-  * \tparam _MatrixType the type of the matrices of which we are computing the
-  * eigen-decomposition; this is expected to be an instantiation of the Matrix
-  * class template. Currently, only real matrices are supported.
-  *
-  * The generalized eigenvalues and eigenvectors of a matrix pair \f$ A \f$ and \f$ B \f$ are scalars
-  * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda Bv \f$.  If
-  * \f$ D \f$ is a diagonal matrix with the eigenvalues on the diagonal, and
-  * \f$ V \f$ is a matrix with the eigenvectors as its columns, then \f$ A V =
-  * B V D \f$. The matrix \f$ V \f$ is almost always invertible, in which case we
-  * have \f$ A = B V D V^{-1} \f$. This is called the generalized eigen-decomposition.
-  *
-  * The generalized eigenvalues and eigenvectors of a matrix pair may be complex, even when the
-  * matrices are real. Moreover, the generalized eigenvalue might be infinite if the matrix B is
-  * singular. To workaround this difficulty, the eigenvalues are provided as a pair of complex \f$ \alpha \f$
-  * and real \f$ \beta \f$ such that: \f$ \lambda_i = \alpha_i / \beta_i \f$. If \f$ \beta_i \f$ is (nearly) zero,
-  * then one can consider the well defined left eigenvalue \f$ \mu = \beta_i / \alpha_i\f$ such that:
-  * \f$ \mu_i A v_i = B v_i \f$, or even \f$ \mu_i u_i^T A  = u_i^T B \f$ where \f$ u_i \f$ is
-  * called the left eigenvector.
-  *
-  * Call the function compute() to compute the generalized eigenvalues and eigenvectors of
-  * a given matrix pair. Alternatively, you can use the
-  * GeneralizedEigenSolver(const MatrixType&, const MatrixType&, bool) constructor which computes the
-  * eigenvalues and eigenvectors at construction time. Once the eigenvalue and
-  * eigenvectors are computed, they can be retrieved with the eigenvalues() and
-  * eigenvectors() functions.
-  *
-  * Here is an usage example of this class:
-  * Example: \include GeneralizedEigenSolver.cpp
-  * Output: \verbinclude GeneralizedEigenSolver.out
-  *
-  * \sa MatrixBase::eigenvalues(), class ComplexEigenSolver, class SelfAdjointEigenSolver
-  */
-template<typename _MatrixType> class GeneralizedEigenSolver
-{
-  public:
+ *
+ *
+ * \class GeneralizedEigenSolver
+ *
+ * \brief Computes the generalized eigenvalues and eigenvectors of a pair of general matrices
+ *
+ * \tparam MatrixType_ the type of the matrices of which we are computing the
+ * eigen-decomposition; this is expected to be an instantiation of the Matrix
+ * class template. Currently, only real matrices are supported.
+ *
+ * The generalized eigenvalues and eigenvectors of a matrix pair \f$ A \f$ and \f$ B \f$ are scalars
+ * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda Bv \f$.  If
+ * \f$ D \f$ is a diagonal matrix with the eigenvalues on the diagonal, and
+ * \f$ V \f$ is a matrix with the eigenvectors as its columns, then \f$ A V =
+ * B V D \f$. The matrix \f$ V \f$ is almost always invertible, in which case we
+ * have \f$ A = B V D V^{-1} \f$. This is called the generalized eigen-decomposition.
+ *
+ * The generalized eigenvalues and eigenvectors of a matrix pair may be complex, even when the
+ * matrices are real. Moreover, the generalized eigenvalue might be infinite if the matrix B is
+ * singular. To workaround this difficulty, the eigenvalues are provided as a pair of complex \f$ \alpha \f$
+ * and real \f$ \beta \f$ such that: \f$ \lambda_i = \alpha_i / \beta_i \f$. If \f$ \beta_i \f$ is (nearly) zero,
+ * then one can consider the well defined left eigenvalue \f$ \mu = \beta_i / \alpha_i\f$ such that:
+ * \f$ \mu_i A v_i = B v_i \f$, or even \f$ \mu_i u_i^T A  = u_i^T B \f$ where \f$ u_i \f$ is
+ * called the left eigenvector.
+ *
+ * Call the function compute() to compute the generalized eigenvalues and eigenvectors of
+ * a given matrix pair. Alternatively, you can use the
+ * GeneralizedEigenSolver(const MatrixType&, const MatrixType&, bool) constructor which computes the
+ * eigenvalues and eigenvectors at construction time. Once the eigenvalue and
+ * eigenvectors are computed, they can be retrieved with the eigenvalues() and
+ * eigenvectors() functions.
+ *
+ * Here is an usage example of this class:
+ * Example: \include GeneralizedEigenSolver.cpp
+ * Output: \verbinclude GeneralizedEigenSolver.out
+ *
+ * \sa MatrixBase::eigenvalues(), class ComplexEigenSolver, class SelfAdjointEigenSolver
+ */
+template <typename MatrixType_>
+class GeneralizedEigenSolver {
+ public:
+  /** \brief Synonym for the template parameter \p MatrixType_. */
+  typedef MatrixType_ MatrixType;
 
-    /** \brief Synonym for the template parameter \p _MatrixType. */
-    typedef _MatrixType MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
 
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      Options = MatrixType::Options,
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
+  /** \brief Scalar type for matrices of type #MatrixType. */
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    /** \brief Scalar type for matrices of type #MatrixType. */
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  /** \brief Complex scalar type for #MatrixType.
+   *
+   * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
+   * \c float or \c double) and just \c Scalar if #Scalar is
+   * complex.
+   */
+  typedef std::complex<RealScalar> ComplexScalar;
 
-    /** \brief Complex scalar type for #MatrixType. 
-      *
-      * This is \c std::complex<Scalar> if #Scalar is real (e.g.,
-      * \c float or \c double) and just \c Scalar if #Scalar is
-      * complex.
-      */
-    typedef std::complex<RealScalar> ComplexScalar;
+  /** \brief Type for vector of real scalar values eigenvalues as returned by betas().
+   *
+   * This is a column vector with entries of type #Scalar.
+   * The length of the vector is the size of #MatrixType.
+   */
+  typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> VectorType;
 
-    /** \brief Type for vector of real scalar values eigenvalues as returned by betas().
-      *
-      * This is a column vector with entries of type #Scalar.
-      * The length of the vector is the size of #MatrixType.
-      */
-    typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> VectorType;
+  /** \brief Type for vector of complex scalar values eigenvalues as returned by alphas().
+   *
+   * This is a column vector with entries of type #ComplexScalar.
+   * The length of the vector is the size of #MatrixType.
+   */
+  typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ComplexVectorType;
 
-    /** \brief Type for vector of complex scalar values eigenvalues as returned by alphas().
-      *
-      * This is a column vector with entries of type #ComplexScalar.
-      * The length of the vector is the size of #MatrixType.
-      */
-    typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ComplexVectorType;
+  /** \brief Expression type for the eigenvalues as returned by eigenvalues().
+   */
+  typedef CwiseBinaryOp<internal::scalar_quotient_op<ComplexScalar, Scalar>, ComplexVectorType, VectorType>
+      EigenvalueType;
 
-    /** \brief Expression type for the eigenvalues as returned by eigenvalues().
-      */
-    typedef CwiseBinaryOp<internal::scalar_quotient_op<ComplexScalar,Scalar>,ComplexVectorType,VectorType> EigenvalueType;
+  /** \brief Type for matrix of eigenvectors as returned by eigenvectors().
+   *
+   * This is a square matrix with entries of type #ComplexScalar.
+   * The size is the same as the size of #MatrixType.
+   */
+  typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime,
+                 MaxColsAtCompileTime>
+      EigenvectorsType;
 
-    /** \brief Type for matrix of eigenvectors as returned by eigenvectors(). 
-      *
-      * This is a square matrix with entries of type #ComplexScalar. 
-      * The size is the same as the size of #MatrixType.
-      */
-    typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> EigenvectorsType;
+  /** \brief Default constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via EigenSolver::compute(const MatrixType&, bool).
+   *
+   * \sa compute() for an example.
+   */
+  GeneralizedEigenSolver()
+      : m_eivec(), m_alphas(), m_betas(), m_computeEigenvectors(false), m_isInitialized(false), m_realQZ() {}
 
-    /** \brief Default constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via EigenSolver::compute(const MatrixType&, bool).
-      *
-      * \sa compute() for an example.
-      */
-    GeneralizedEigenSolver()
-      : m_eivec(),
-        m_alphas(),
-        m_betas(),
-        m_valuesOkay(false),
-        m_vectorsOkay(false),
-        m_realQZ()
-    {}
-
-    /** \brief Default constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa GeneralizedEigenSolver()
-      */
-    explicit GeneralizedEigenSolver(Index size)
+  /** \brief Default constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa GeneralizedEigenSolver()
+   */
+  explicit GeneralizedEigenSolver(Index size)
       : m_eivec(size, size),
         m_alphas(size),
         m_betas(size),
-        m_valuesOkay(false),
-        m_vectorsOkay(false),
+        m_computeEigenvectors(false),
+        m_isInitialized(false),
         m_realQZ(size),
-        m_tmp(size)
-    {}
+        m_tmp(size) {}
 
-    /** \brief Constructor; computes the generalized eigendecomposition of given matrix pair.
-      * 
-      * \param[in]  A  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  B  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
-      *    eigenvalues are computed; if false, only the eigenvalues are computed.
-      *
-      * This constructor calls compute() to compute the generalized eigenvalues
-      * and eigenvectors.
-      *
-      * \sa compute()
-      */
-    GeneralizedEigenSolver(const MatrixType& A, const MatrixType& B, bool computeEigenvectors = true)
+  /** \brief Constructor; computes the generalized eigendecomposition of given matrix pair.
+   *
+   * \param[in]  A  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  B  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
+   *    eigenvalues are computed; if false, only the eigenvalues are computed.
+   *
+   * This constructor calls compute() to compute the generalized eigenvalues
+   * and eigenvectors.
+   *
+   * \sa compute()
+   */
+  GeneralizedEigenSolver(const MatrixType& A, const MatrixType& B, bool computeEigenvectors = true)
       : m_eivec(A.rows(), A.cols()),
         m_alphas(A.cols()),
         m_betas(A.cols()),
-        m_valuesOkay(false),
-        m_vectorsOkay(false),
+        m_computeEigenvectors(false),
+        m_isInitialized(false),
         m_realQZ(A.cols()),
-        m_tmp(A.cols())
-    {
-      compute(A, B, computeEigenvectors);
-    }
+        m_tmp(A.cols()) {
+    compute(A, B, computeEigenvectors);
+  }
 
-    /* \brief Returns the computed generalized eigenvectors.
-      *
-      * \returns  %Matrix whose columns are the (possibly complex) right eigenvectors.
-      * i.e. the eigenvectors that solve (A - l*B)x = 0. The ordering matches the eigenvalues.
-      *
-      * \pre Either the constructor 
-      * GeneralizedEigenSolver(const MatrixType&,const MatrixType&, bool) or the member function
-      * compute(const MatrixType&, const MatrixType& bool) has been called before, and
-      * \p computeEigenvectors was set to true (the default).
-      *
-      * \sa eigenvalues()
-      */
-    EigenvectorsType eigenvectors() const {
-      eigen_assert(m_vectorsOkay && "Eigenvectors for GeneralizedEigenSolver were not calculated.");
-      return m_eivec;
-    }
+  /* \brief Returns the computed generalized eigenvectors.
+   *
+   * \returns  %Matrix whose columns are the (possibly complex) right eigenvectors.
+   * i.e. the eigenvectors that solve (A - l*B)x = 0. The ordering matches the eigenvalues.
+   *
+   * \pre Either the constructor
+   * GeneralizedEigenSolver(const MatrixType&,const MatrixType&, bool) or the member function
+   * compute(const MatrixType&, const MatrixType& bool) has been called before, and
+   * \p computeEigenvectors was set to true (the default).
+   *
+   * \sa eigenvalues()
+   */
+  EigenvectorsType eigenvectors() const {
+    eigen_assert(info() == Success && "GeneralizedEigenSolver failed to compute eigenvectors");
+    eigen_assert(m_computeEigenvectors && "Eigenvectors for GeneralizedEigenSolver were not calculated");
+    return m_eivec;
+  }
 
-    /** \brief Returns an expression of the computed generalized eigenvalues.
-      *
-      * \returns An expression of the column vector containing the eigenvalues.
-      *
-      * It is a shortcut for \code this->alphas().cwiseQuotient(this->betas()); \endcode
-      * Not that betas might contain zeros. It is therefore not recommended to use this function,
-      * but rather directly deal with the alphas and betas vectors.
-      *
-      * \pre Either the constructor 
-      * GeneralizedEigenSolver(const MatrixType&,const MatrixType&,bool) or the member function
-      * compute(const MatrixType&,const MatrixType&,bool) has been called before.
-      *
-      * The eigenvalues are repeated according to their algebraic multiplicity,
-      * so there are as many eigenvalues as rows in the matrix. The eigenvalues 
-      * are not sorted in any particular order.
-      *
-      * \sa alphas(), betas(), eigenvectors()
-      */
-    EigenvalueType eigenvalues() const
-    {
-      eigen_assert(m_valuesOkay && "GeneralizedEigenSolver is not initialized.");
-      return EigenvalueType(m_alphas,m_betas);
-    }
+  /** \brief Returns an expression of the computed generalized eigenvalues.
+   *
+   * \returns An expression of the column vector containing the eigenvalues.
+   *
+   * It is a shortcut for \code this->alphas().cwiseQuotient(this->betas()); \endcode
+   * Not that betas might contain zeros. It is therefore not recommended to use this function,
+   * but rather directly deal with the alphas and betas vectors.
+   *
+   * \pre Either the constructor
+   * GeneralizedEigenSolver(const MatrixType&,const MatrixType&,bool) or the member function
+   * compute(const MatrixType&,const MatrixType&,bool) has been called before.
+   *
+   * The eigenvalues are repeated according to their algebraic multiplicity,
+   * so there are as many eigenvalues as rows in the matrix. The eigenvalues
+   * are not sorted in any particular order.
+   *
+   * \sa alphas(), betas(), eigenvectors()
+   */
+  EigenvalueType eigenvalues() const {
+    eigen_assert(info() == Success && "GeneralizedEigenSolver failed to compute eigenvalues.");
+    return EigenvalueType(m_alphas, m_betas);
+  }
 
-    /** \returns A const reference to the vectors containing the alpha values
-      *
-      * This vector permits to reconstruct the j-th eigenvalues as alphas(i)/betas(j).
-      *
-      * \sa betas(), eigenvalues() */
-    ComplexVectorType alphas() const
-    {
-      eigen_assert(m_valuesOkay && "GeneralizedEigenSolver is not initialized.");
-      return m_alphas;
-    }
+  /** \returns A const reference to the vectors containing the alpha values
+   *
+   * This vector permits to reconstruct the j-th eigenvalues as alphas(i)/betas(j).
+   *
+   * \sa betas(), eigenvalues() */
+  const ComplexVectorType& alphas() const {
+    eigen_assert(info() == Success && "GeneralizedEigenSolver failed to compute alphas.");
+    return m_alphas;
+  }
 
-    /** \returns A const reference to the vectors containing the beta values
-      *
-      * This vector permits to reconstruct the j-th eigenvalues as alphas(i)/betas(j).
-      *
-      * \sa alphas(), eigenvalues() */
-    VectorType betas() const
-    {
-      eigen_assert(m_valuesOkay && "GeneralizedEigenSolver is not initialized.");
-      return m_betas;
-    }
+  /** \returns A const reference to the vectors containing the beta values
+   *
+   * This vector permits to reconstruct the j-th eigenvalues as alphas(i)/betas(j).
+   *
+   * \sa alphas(), eigenvalues() */
+  const VectorType& betas() const {
+    eigen_assert(info() == Success && "GeneralizedEigenSolver failed to compute betas.");
+    return m_betas;
+  }
 
-    /** \brief Computes generalized eigendecomposition of given matrix.
-      * 
-      * \param[in]  A  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  B  Square matrix whose eigendecomposition is to be computed.
-      * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
-      *    eigenvalues are computed; if false, only the eigenvalues are
-      *    computed. 
-      * \returns    Reference to \c *this
-      *
-      * This function computes the eigenvalues of the real matrix \p matrix.
-      * The eigenvalues() function can be used to retrieve them.  If 
-      * \p computeEigenvectors is true, then the eigenvectors are also computed
-      * and can be retrieved by calling eigenvectors().
-      *
-      * The matrix is first reduced to real generalized Schur form using the RealQZ
-      * class. The generalized Schur decomposition is then used to compute the eigenvalues
-      * and eigenvectors.
-      *
-      * The cost of the computation is dominated by the cost of the
-      * generalized Schur decomposition.
-      *
-      * This method reuses of the allocated data in the GeneralizedEigenSolver object.
-      */
-    GeneralizedEigenSolver& compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors = true);
+  /** \brief Computes generalized eigendecomposition of given matrix.
+   *
+   * \param[in]  A  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  B  Square matrix whose eigendecomposition is to be computed.
+   * \param[in]  computeEigenvectors  If true, both the eigenvectors and the
+   *    eigenvalues are computed; if false, only the eigenvalues are
+   *    computed.
+   * \returns    Reference to \c *this
+   *
+   * This function computes the eigenvalues of the real matrix \p matrix.
+   * The eigenvalues() function can be used to retrieve them.  If
+   * \p computeEigenvectors is true, then the eigenvectors are also computed
+   * and can be retrieved by calling eigenvectors().
+   *
+   * The matrix is first reduced to real generalized Schur form using the RealQZ
+   * class. The generalized Schur decomposition is then used to compute the eigenvalues
+   * and eigenvectors.
+   *
+   * The cost of the computation is dominated by the cost of the
+   * generalized Schur decomposition.
+   *
+   * This method reuses of the allocated data in the GeneralizedEigenSolver object.
+   */
+  GeneralizedEigenSolver& compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors = true);
 
-    ComputationInfo info() const
-    {
-      eigen_assert(m_valuesOkay && "EigenSolver is not initialized.");
-      return m_realQZ.info();
-    }
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
+    return m_realQZ.info();
+  }
 
-    /** Sets the maximal number of iterations allowed.
-    */
-    GeneralizedEigenSolver& setMaxIterations(Index maxIters)
-    {
-      m_realQZ.setMaxIterations(maxIters);
-      return *this;
-    }
+  /** Sets the maximal number of iterations allowed.
+   */
+  GeneralizedEigenSolver& setMaxIterations(Index maxIters) {
+    m_realQZ.setMaxIterations(maxIters);
+    return *this;
+  }
 
-  protected:
-    
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-      EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL);
-    }
-    
-    EigenvectorsType m_eivec;
-    ComplexVectorType m_alphas;
-    VectorType m_betas;
-    bool m_valuesOkay, m_vectorsOkay;
-    RealQZ<MatrixType> m_realQZ;
-    ComplexVectorType m_tmp;
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
+  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
+
+  EigenvectorsType m_eivec;
+  ComplexVectorType m_alphas;
+  VectorType m_betas;
+  bool m_computeEigenvectors;
+  bool m_isInitialized;
+  RealQZ<MatrixType> m_realQZ;
+  ComplexVectorType m_tmp;
 };
 
-template<typename MatrixType>
-GeneralizedEigenSolver<MatrixType>&
-GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors)
-{
-  check_template_parameters();
-  
-  using std::sqrt;
+template <typename MatrixType>
+GeneralizedEigenSolver<MatrixType>& GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A,
+                                                                                const MatrixType& B,
+                                                                                bool computeEigenvectors) {
   using std::abs;
+  using std::sqrt;
   eigen_assert(A.cols() == A.rows() && B.cols() == A.rows() && B.cols() == B.rows());
   Index size = A.cols();
-  m_valuesOkay = false;
-  m_vectorsOkay = false;
   // Reduce to generalized real Schur form:
   // A = Q S Z and B = Q T Z
   m_realQZ.compute(A, B, computeEigenvectors);
-  if (m_realQZ.info() == Success)
-  {
+  if (m_realQZ.info() == Success) {
     // Resize storage
     m_alphas.resize(size);
     m_betas.resize(size);
-    if (computeEigenvectors)
-    {
-      m_eivec.resize(size,size);
+    if (computeEigenvectors) {
+      m_eivec.resize(size, size);
       m_tmp.resize(size);
     }
 
     // Aliases:
     Map<VectorType> v(reinterpret_cast<Scalar*>(m_tmp.data()), size);
-    ComplexVectorType &cv = m_tmp;
-    const MatrixType &mS = m_realQZ.matrixS();
-    const MatrixType &mT = m_realQZ.matrixT();
+    ComplexVectorType& cv = m_tmp;
+    const MatrixType& mS = m_realQZ.matrixS();
+    const MatrixType& mT = m_realQZ.matrixT();
 
     Index i = 0;
-    while (i < size)
-    {
-      if (i == size - 1 || mS.coeff(i+1, i) == Scalar(0))
-      {
+    while (i < size) {
+      if (i == size - 1 || mS.coeff(i + 1, i) == Scalar(0)) {
         // Real eigenvalue
         m_alphas.coeffRef(i) = mS.diagonal().coeff(i);
-        m_betas.coeffRef(i)  = mT.diagonal().coeff(i);
-        if (computeEigenvectors)
-        {
+        m_betas.coeffRef(i) = mT.diagonal().coeff(i);
+        if (computeEigenvectors) {
           v.setConstant(Scalar(0.0));
           v.coeffRef(i) = Scalar(1.0);
           // For singular eigenvalues do nothing more
-          if(abs(m_betas.coeffRef(i)) >= (std::numeric_limits<RealScalar>::min)())
-          {
+          if (abs(m_betas.coeffRef(i)) >= (std::numeric_limits<RealScalar>::min)()) {
             // Non-singular eigenvalue
             const Scalar alpha = real(m_alphas.coeffRef(i));
             const Scalar beta = m_betas.coeffRef(i);
-            for (Index j = i-1; j >= 0; j--)
-            {
-              const Index st = j+1;
-              const Index sz = i-j;
-              if (j > 0 && mS.coeff(j, j-1) != Scalar(0))
-              {
+            for (Index j = i - 1; j >= 0; j--) {
+              const Index st = j + 1;
+              const Index sz = i - j;
+              if (j > 0 && mS.coeff(j, j - 1) != Scalar(0)) {
                 // 2x2 block
-                Matrix<Scalar, 2, 1> rhs = (alpha*mT.template block<2,Dynamic>(j-1,st,2,sz) - beta*mS.template block<2,Dynamic>(j-1,st,2,sz)) .lazyProduct( v.segment(st,sz) );
-                Matrix<Scalar, 2, 2> lhs = beta * mS.template block<2,2>(j-1,j-1) - alpha * mT.template block<2,2>(j-1,j-1);
-                v.template segment<2>(j-1) = lhs.partialPivLu().solve(rhs);
+                Matrix<Scalar, 2, 1> rhs = (alpha * mT.template block<2, Dynamic>(j - 1, st, 2, sz) -
+                                            beta * mS.template block<2, Dynamic>(j - 1, st, 2, sz))
+                                               .lazyProduct(v.segment(st, sz));
+                Matrix<Scalar, 2, 2> lhs =
+                    beta * mS.template block<2, 2>(j - 1, j - 1) - alpha * mT.template block<2, 2>(j - 1, j - 1);
+                v.template segment<2>(j - 1) = lhs.partialPivLu().solve(rhs);
                 j--;
-              }
-              else
-              {
-                v.coeffRef(j) = -v.segment(st,sz).transpose().cwiseProduct(beta*mS.block(j,st,1,sz) - alpha*mT.block(j,st,1,sz)).sum() / (beta*mS.coeffRef(j,j) - alpha*mT.coeffRef(j,j));
+              } else {
+                v.coeffRef(j) = -v.segment(st, sz)
+                                     .transpose()
+                                     .cwiseProduct(beta * mS.block(j, st, 1, sz) - alpha * mT.block(j, st, 1, sz))
+                                     .sum() /
+                                (beta * mS.coeffRef(j, j) - alpha * mT.coeffRef(j, j));
               }
             }
           }
@@ -355,64 +338,65 @@
           m_eivec.col(i).imag().setConstant(0);
         }
         ++i;
-      }
-      else
-      {
-        // We need to extract the generalized eigenvalues of the pair of a general 2x2 block S and a positive diagonal 2x2 block T
-        // Then taking beta=T_00*T_11, we can avoid any division, and alpha is the eigenvalues of A = (U^-1 * S * U) * diag(T_11,T_00):
+      } else {
+        // We need to extract the generalized eigenvalues of the pair of a general 2x2 block S and a positive diagonal
+        // 2x2 block T Then taking beta=T_00*T_11, we can avoid any division, and alpha is the eigenvalues of A = (U^-1
+        // * S * U) * diag(T_11,T_00):
 
         // T =  [a 0]
         //      [0 b]
-        RealScalar a = mT.diagonal().coeff(i),
-                   b = mT.diagonal().coeff(i+1);
-        const RealScalar beta = m_betas.coeffRef(i) = m_betas.coeffRef(i+1) = a*b;
+        RealScalar a = mT.diagonal().coeff(i), b = mT.diagonal().coeff(i + 1);
+        const RealScalar beta = m_betas.coeffRef(i) = m_betas.coeffRef(i + 1) = a * b;
 
         // ^^ NOTE: using diagonal()(i) instead of coeff(i,i) workarounds a MSVC bug.
-        Matrix<RealScalar,2,2> S2 = mS.template block<2,2>(i,i) * Matrix<Scalar,2,1>(b,a).asDiagonal();
+        Matrix<RealScalar, 2, 2> S2 = mS.template block<2, 2>(i, i) * Matrix<Scalar, 2, 1>(b, a).asDiagonal();
 
-        Scalar p = Scalar(0.5) * (S2.coeff(0,0) - S2.coeff(1,1));
-        Scalar z = sqrt(abs(p * p + S2.coeff(1,0) * S2.coeff(0,1)));
-        const ComplexScalar alpha = ComplexScalar(S2.coeff(1,1) + p, (beta > 0) ? z : -z);
-        m_alphas.coeffRef(i)   = conj(alpha);
-        m_alphas.coeffRef(i+1) = alpha;
+        Scalar p = Scalar(0.5) * (S2.coeff(0, 0) - S2.coeff(1, 1));
+        Scalar z = sqrt(abs(p * p + S2.coeff(1, 0) * S2.coeff(0, 1)));
+        const ComplexScalar alpha = ComplexScalar(S2.coeff(1, 1) + p, (beta > 0) ? z : -z);
+        m_alphas.coeffRef(i) = conj(alpha);
+        m_alphas.coeffRef(i + 1) = alpha;
 
         if (computeEigenvectors) {
           // Compute eigenvector in position (i+1) and then position (i) is just the conjugate
           cv.setZero();
-          cv.coeffRef(i+1) = Scalar(1.0);
+          cv.coeffRef(i + 1) = Scalar(1.0);
           // here, the "static_cast" workaound expression template issues.
-          cv.coeffRef(i) = -(static_cast<Scalar>(beta*mS.coeffRef(i,i+1)) - alpha*mT.coeffRef(i,i+1))
-                          / (static_cast<Scalar>(beta*mS.coeffRef(i,i))   - alpha*mT.coeffRef(i,i));
-          for (Index j = i-1; j >= 0; j--)
-          {
-            const Index st = j+1;
-            const Index sz = i+1-j;
-            if (j > 0 && mS.coeff(j, j-1) != Scalar(0))
-            {
+          cv.coeffRef(i) = -(static_cast<Scalar>(beta * mS.coeffRef(i, i + 1)) - alpha * mT.coeffRef(i, i + 1)) /
+                           (static_cast<Scalar>(beta * mS.coeffRef(i, i)) - alpha * mT.coeffRef(i, i));
+          for (Index j = i - 1; j >= 0; j--) {
+            const Index st = j + 1;
+            const Index sz = i + 1 - j;
+            if (j > 0 && mS.coeff(j, j - 1) != Scalar(0)) {
               // 2x2 block
-              Matrix<ComplexScalar, 2, 1> rhs = (alpha*mT.template block<2,Dynamic>(j-1,st,2,sz) - beta*mS.template block<2,Dynamic>(j-1,st,2,sz)) .lazyProduct( cv.segment(st,sz) );
-              Matrix<ComplexScalar, 2, 2> lhs = beta * mS.template block<2,2>(j-1,j-1) - alpha * mT.template block<2,2>(j-1,j-1);
-              cv.template segment<2>(j-1) = lhs.partialPivLu().solve(rhs);
+              Matrix<ComplexScalar, 2, 1> rhs = (alpha * mT.template block<2, Dynamic>(j - 1, st, 2, sz) -
+                                                 beta * mS.template block<2, Dynamic>(j - 1, st, 2, sz))
+                                                    .lazyProduct(cv.segment(st, sz));
+              Matrix<ComplexScalar, 2, 2> lhs =
+                  beta * mS.template block<2, 2>(j - 1, j - 1) - alpha * mT.template block<2, 2>(j - 1, j - 1);
+              cv.template segment<2>(j - 1) = lhs.partialPivLu().solve(rhs);
               j--;
             } else {
-              cv.coeffRef(j) =  cv.segment(st,sz).transpose().cwiseProduct(beta*mS.block(j,st,1,sz) - alpha*mT.block(j,st,1,sz)).sum()
-                              / (alpha*mT.coeffRef(j,j) - static_cast<Scalar>(beta*mS.coeffRef(j,j)));
+              cv.coeffRef(j) = cv.segment(st, sz)
+                                   .transpose()
+                                   .cwiseProduct(beta * mS.block(j, st, 1, sz) - alpha * mT.block(j, st, 1, sz))
+                                   .sum() /
+                               (alpha * mT.coeffRef(j, j) - static_cast<Scalar>(beta * mS.coeffRef(j, j)));
             }
           }
-          m_eivec.col(i+1).noalias() = (m_realQZ.matrixZ().transpose() * cv);
-          m_eivec.col(i+1).normalize();
-          m_eivec.col(i) = m_eivec.col(i+1).conjugate();
+          m_eivec.col(i + 1).noalias() = (m_realQZ.matrixZ().transpose() * cv);
+          m_eivec.col(i + 1).normalize();
+          m_eivec.col(i) = m_eivec.col(i + 1).conjugate();
         }
         i += 2;
       }
     }
-
-    m_valuesOkay = true;
-    m_vectorsOkay = computeEigenvectors;
   }
+  m_computeEigenvectors = computeEigenvectors;
+  m_isInitialized = true;
   return *this;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_GENERALIZEDEIGENSOLVER_H
+#endif  // EIGEN_GENERALIZEDEIGENSOLVER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
index d0f9091..adff3a3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
@@ -13,186 +13,176 @@
 
 #include "./Tridiagonalization.h"
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class GeneralizedSelfAdjointEigenSolver
-  *
-  * \brief Computes eigenvalues and eigenvectors of the generalized selfadjoint eigen problem
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the
-  * eigendecomposition; this is expected to be an instantiation of the Matrix
-  * class template.
-  *
-  * This class solves the generalized eigenvalue problem
-  * \f$ Av = \lambda Bv \f$. In this case, the matrix \f$ A \f$ should be
-  * selfadjoint and the matrix \f$ B \f$ should be positive definite.
-  *
-  * Only the \b lower \b triangular \b part of the input matrix is referenced.
-  *
-  * Call the function compute() to compute the eigenvalues and eigenvectors of
-  * a given matrix. Alternatively, you can use the
-  * GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int)
-  * constructor which computes the eigenvalues and eigenvectors at construction time.
-  * Once the eigenvalue and eigenvectors are computed, they can be retrieved with the eigenvalues()
-  * and eigenvectors() functions.
-  *
-  * The documentation for GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int)
-  * contains an example of the typical use of this class.
-  *
-  * \sa class SelfAdjointEigenSolver, class EigenSolver, class ComplexEigenSolver
-  */
-template<typename _MatrixType>
-class GeneralizedSelfAdjointEigenSolver : public SelfAdjointEigenSolver<_MatrixType>
-{
-    typedef SelfAdjointEigenSolver<_MatrixType> Base;
-  public:
+ *
+ *
+ * \class GeneralizedSelfAdjointEigenSolver
+ *
+ * \brief Computes eigenvalues and eigenvectors of the generalized selfadjoint eigen problem
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the
+ * eigendecomposition; this is expected to be an instantiation of the Matrix
+ * class template.
+ *
+ * This class solves the generalized eigenvalue problem
+ * \f$ Av = \lambda Bv \f$. In this case, the matrix \f$ A \f$ should be
+ * selfadjoint and the matrix \f$ B \f$ should be positive definite.
+ *
+ * Only the \b lower \b triangular \b part of the input matrix is referenced.
+ *
+ * Call the function compute() to compute the eigenvalues and eigenvectors of
+ * a given matrix. Alternatively, you can use the
+ * GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int)
+ * constructor which computes the eigenvalues and eigenvectors at construction time.
+ * Once the eigenvalue and eigenvectors are computed, they can be retrieved with the eigenvalues()
+ * and eigenvectors() functions.
+ *
+ * The documentation for GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int)
+ * contains an example of the typical use of this class.
+ *
+ * \sa class SelfAdjointEigenSolver, class EigenSolver, class ComplexEigenSolver
+ */
+template <typename MatrixType_>
+class GeneralizedSelfAdjointEigenSolver : public SelfAdjointEigenSolver<MatrixType_> {
+  typedef SelfAdjointEigenSolver<MatrixType_> Base;
 
-    typedef _MatrixType MatrixType;
+ public:
+  typedef MatrixType_ MatrixType;
 
-    /** \brief Default constructor for fixed-size matrices.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via compute(). This constructor
-      * can only be used if \p _MatrixType is a fixed-size matrix; use
-      * GeneralizedSelfAdjointEigenSolver(Index) for dynamic-size matrices.
-      */
-    GeneralizedSelfAdjointEigenSolver() : Base() {}
+  /** \brief Default constructor for fixed-size matrices.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via compute(). This constructor
+   * can only be used if \p MatrixType_ is a fixed-size matrix; use
+   * GeneralizedSelfAdjointEigenSolver(Index) for dynamic-size matrices.
+   */
+  GeneralizedSelfAdjointEigenSolver() : Base() {}
 
-    /** \brief Constructor, pre-allocates memory for dynamic-size matrices.
-      *
-      * \param [in]  size  Positive integer, size of the matrix whose
-      * eigenvalues and eigenvectors will be computed.
-      *
-      * This constructor is useful for dynamic-size matrices, when the user
-      * intends to perform decompositions via compute(). The \p size
-      * parameter is only used as a hint. It is not an error to give a wrong
-      * \p size, but it may impair performance.
-      *
-      * \sa compute() for an example
-      */
-    explicit GeneralizedSelfAdjointEigenSolver(Index size)
-        : Base(size)
-    {}
+  /** \brief Constructor, pre-allocates memory for dynamic-size matrices.
+   *
+   * \param [in]  size  Positive integer, size of the matrix whose
+   * eigenvalues and eigenvectors will be computed.
+   *
+   * This constructor is useful for dynamic-size matrices, when the user
+   * intends to perform decompositions via compute(). The \p size
+   * parameter is only used as a hint. It is not an error to give a wrong
+   * \p size, but it may impair performance.
+   *
+   * \sa compute() for an example
+   */
+  explicit GeneralizedSelfAdjointEigenSolver(Index size) : Base(size) {}
 
-    /** \brief Constructor; computes generalized eigendecomposition of given matrix pencil.
-      *
-      * \param[in]  matA  Selfadjoint matrix in matrix pencil.
-      *                   Only the lower triangular part of the matrix is referenced.
-      * \param[in]  matB  Positive-definite matrix in matrix pencil.
-      *                   Only the lower triangular part of the matrix is referenced.
-      * \param[in]  options A or-ed set of flags {#ComputeEigenvectors,#EigenvaluesOnly} | {#Ax_lBx,#ABx_lx,#BAx_lx}.
-      *                     Default is #ComputeEigenvectors|#Ax_lBx.
-      *
-      * This constructor calls compute(const MatrixType&, const MatrixType&, int)
-      * to compute the eigenvalues and (if requested) the eigenvectors of the
-      * generalized eigenproblem \f$ Ax = \lambda B x \f$ with \a matA the
-      * selfadjoint matrix \f$ A \f$ and \a matB the positive definite matrix
-      * \f$ B \f$. Each eigenvector \f$ x \f$ satisfies the property
-      * \f$ x^* B x = 1 \f$. The eigenvectors are computed if
-      * \a options contains ComputeEigenvectors.
-      *
-      * In addition, the two following variants can be solved via \p options:
-      * - \c ABx_lx: \f$ ABx = \lambda x \f$
-      * - \c BAx_lx: \f$ BAx = \lambda x \f$
-      *
-      * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType2.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType2.out
-      *
-      * \sa compute(const MatrixType&, const MatrixType&, int)
-      */
-    GeneralizedSelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB,
-                                      int options = ComputeEigenvectors|Ax_lBx)
-      : Base(matA.cols())
-    {
-      compute(matA, matB, options);
-    }
+  /** \brief Constructor; computes generalized eigendecomposition of given matrix pencil.
+   *
+   * \param[in]  matA  Selfadjoint matrix in matrix pencil.
+   *                   Only the lower triangular part of the matrix is referenced.
+   * \param[in]  matB  Positive-definite matrix in matrix pencil.
+   *                   Only the lower triangular part of the matrix is referenced.
+   * \param[in]  options A or-ed set of flags {#ComputeEigenvectors,#EigenvaluesOnly} | {#Ax_lBx,#ABx_lx,#BAx_lx}.
+   *                     Default is #ComputeEigenvectors|#Ax_lBx.
+   *
+   * This constructor calls compute(const MatrixType&, const MatrixType&, int)
+   * to compute the eigenvalues and (if requested) the eigenvectors of the
+   * generalized eigenproblem \f$ Ax = \lambda B x \f$ with \a matA the
+   * selfadjoint matrix \f$ A \f$ and \a matB the positive definite matrix
+   * \f$ B \f$. Each eigenvector \f$ x \f$ satisfies the property
+   * \f$ x^* B x = 1 \f$. The eigenvectors are computed if
+   * \a options contains ComputeEigenvectors.
+   *
+   * In addition, the two following variants can be solved via \p options:
+   * - \c ABx_lx: \f$ ABx = \lambda x \f$
+   * - \c BAx_lx: \f$ BAx = \lambda x \f$
+   *
+   * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType2.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType2.out
+   *
+   * \sa compute(const MatrixType&, const MatrixType&, int)
+   */
+  GeneralizedSelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB,
+                                    int options = ComputeEigenvectors | Ax_lBx)
+      : Base(matA.cols()) {
+    compute(matA, matB, options);
+  }
 
-    /** \brief Computes generalized eigendecomposition of given matrix pencil.
-      *
-      * \param[in]  matA  Selfadjoint matrix in matrix pencil.
-      *                   Only the lower triangular part of the matrix is referenced.
-      * \param[in]  matB  Positive-definite matrix in matrix pencil.
-      *                   Only the lower triangular part of the matrix is referenced.
-      * \param[in]  options A or-ed set of flags {#ComputeEigenvectors,#EigenvaluesOnly} | {#Ax_lBx,#ABx_lx,#BAx_lx}.
-      *                     Default is #ComputeEigenvectors|#Ax_lBx.
-      *
-      * \returns    Reference to \c *this
-      *
-      * According to \p options, this function computes eigenvalues and (if requested)
-      * the eigenvectors of one of the following three generalized eigenproblems:
-      * - \c Ax_lBx: \f$ Ax = \lambda B x \f$
-      * - \c ABx_lx: \f$ ABx = \lambda x \f$
-      * - \c BAx_lx: \f$ BAx = \lambda x \f$
-      * with \a matA the selfadjoint matrix \f$ A \f$ and \a matB the positive definite
-      * matrix \f$ B \f$.
-      * In addition, each eigenvector \f$ x \f$ satisfies the property \f$ x^* B x = 1 \f$.
-      *
-      * The eigenvalues() function can be used to retrieve
-      * the eigenvalues. If \p options contains ComputeEigenvectors, then the
-      * eigenvectors are also computed and can be retrieved by calling
-      * eigenvectors().
-      *
-      * The implementation uses LLT to compute the Cholesky decomposition
-      * \f$ B = LL^* \f$ and computes the classical eigendecomposition
-      * of the selfadjoint matrix \f$ L^{-1} A (L^*)^{-1} \f$ if \p options contains Ax_lBx
-      * and of \f$ L^{*} A L \f$ otherwise. This solves the
-      * generalized eigenproblem, because any solution of the generalized
-      * eigenproblem \f$ Ax = \lambda B x \f$ corresponds to a solution
-      * \f$ L^{-1} A (L^*)^{-1} (L^* x) = \lambda (L^* x) \f$ of the
-      * eigenproblem for \f$ L^{-1} A (L^*)^{-1} \f$. Similar statements
-      * can be made for the two other variants.
-      *
-      * Example: \include SelfAdjointEigenSolver_compute_MatrixType2.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_compute_MatrixType2.out
-      *
-      * \sa GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int)
-      */
-    GeneralizedSelfAdjointEigenSolver& compute(const MatrixType& matA, const MatrixType& matB,
-                                               int options = ComputeEigenvectors|Ax_lBx);
+  /** \brief Computes generalized eigendecomposition of given matrix pencil.
+   *
+   * \param[in]  matA  Selfadjoint matrix in matrix pencil.
+   *                   Only the lower triangular part of the matrix is referenced.
+   * \param[in]  matB  Positive-definite matrix in matrix pencil.
+   *                   Only the lower triangular part of the matrix is referenced.
+   * \param[in]  options A or-ed set of flags {#ComputeEigenvectors,#EigenvaluesOnly} | {#Ax_lBx,#ABx_lx,#BAx_lx}.
+   *                     Default is #ComputeEigenvectors|#Ax_lBx.
+   *
+   * \returns    Reference to \c *this
+   *
+   * According to \p options, this function computes eigenvalues and (if requested)
+   * the eigenvectors of one of the following three generalized eigenproblems:
+   * - \c Ax_lBx: \f$ Ax = \lambda B x \f$
+   * - \c ABx_lx: \f$ ABx = \lambda x \f$
+   * - \c BAx_lx: \f$ BAx = \lambda x \f$
+   * with \a matA the selfadjoint matrix \f$ A \f$ and \a matB the positive definite
+   * matrix \f$ B \f$.
+   * In addition, each eigenvector \f$ x \f$ satisfies the property \f$ x^* B x = 1 \f$.
+   *
+   * The eigenvalues() function can be used to retrieve
+   * the eigenvalues. If \p options contains ComputeEigenvectors, then the
+   * eigenvectors are also computed and can be retrieved by calling
+   * eigenvectors().
+   *
+   * The implementation uses LLT to compute the Cholesky decomposition
+   * \f$ B = LL^* \f$ and computes the classical eigendecomposition
+   * of the selfadjoint matrix \f$ L^{-1} A (L^*)^{-1} \f$ if \p options contains Ax_lBx
+   * and of \f$ L^{*} A L \f$ otherwise. This solves the
+   * generalized eigenproblem, because any solution of the generalized
+   * eigenproblem \f$ Ax = \lambda B x \f$ corresponds to a solution
+   * \f$ L^{-1} A (L^*)^{-1} (L^* x) = \lambda (L^* x) \f$ of the
+   * eigenproblem for \f$ L^{-1} A (L^*)^{-1} \f$. Similar statements
+   * can be made for the two other variants.
+   *
+   * Example: \include SelfAdjointEigenSolver_compute_MatrixType2.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_compute_MatrixType2.out
+   *
+   * \sa GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int)
+   */
+  GeneralizedSelfAdjointEigenSolver& compute(const MatrixType& matA, const MatrixType& matB,
+                                             int options = ComputeEigenvectors | Ax_lBx);
 
-  protected:
-
+ protected:
 };
 
+template <typename MatrixType>
+GeneralizedSelfAdjointEigenSolver<MatrixType>& GeneralizedSelfAdjointEigenSolver<MatrixType>::compute(
+    const MatrixType& matA, const MatrixType& matB, int options) {
+  eigen_assert(matA.cols() == matA.rows() && matB.rows() == matA.rows() && matB.cols() == matB.rows());
+  eigen_assert((options & ~(EigVecMask | GenEigMask)) == 0 && (options & EigVecMask) != EigVecMask &&
+               ((options & GenEigMask) == 0 || (options & GenEigMask) == Ax_lBx || (options & GenEigMask) == ABx_lx ||
+                (options & GenEigMask) == BAx_lx) &&
+               "invalid option parameter");
 
-template<typename MatrixType>
-GeneralizedSelfAdjointEigenSolver<MatrixType>& GeneralizedSelfAdjointEigenSolver<MatrixType>::
-compute(const MatrixType& matA, const MatrixType& matB, int options)
-{
-  eigen_assert(matA.cols()==matA.rows() && matB.rows()==matA.rows() && matB.cols()==matB.rows());
-  eigen_assert((options&~(EigVecMask|GenEigMask))==0
-          && (options&EigVecMask)!=EigVecMask
-          && ((options&GenEigMask)==0 || (options&GenEigMask)==Ax_lBx
-           || (options&GenEigMask)==ABx_lx || (options&GenEigMask)==BAx_lx)
-          && "invalid option parameter");
-
-  bool computeEigVecs = ((options&EigVecMask)==0) || ((options&EigVecMask)==ComputeEigenvectors);
+  bool computeEigVecs = ((options & EigVecMask) == 0) || ((options & EigVecMask) == ComputeEigenvectors);
 
   // Compute the cholesky decomposition of matB = L L' = U'U
   LLT<MatrixType> cholB(matB);
 
-  int type = (options&GenEigMask);
-  if(type==0)
-    type = Ax_lBx;
+  int type = (options & GenEigMask);
+  if (type == 0) type = Ax_lBx;
 
-  if(type==Ax_lBx)
-  {
+  if (type == Ax_lBx) {
     // compute C = inv(L) A inv(L')
     MatrixType matC = matA.template selfadjointView<Lower>();
     cholB.matrixL().template solveInPlace<OnTheLeft>(matC);
     cholB.matrixU().template solveInPlace<OnTheRight>(matC);
 
-    Base::compute(matC, computeEigVecs ? ComputeEigenvectors : EigenvaluesOnly );
+    Base::compute(matC, computeEigVecs ? ComputeEigenvectors : EigenvaluesOnly);
 
     // transform back the eigen vectors: evecs = inv(U) * evecs
-    if(computeEigVecs)
-      cholB.matrixU().solveInPlace(Base::m_eivec);
-  }
-  else if(type==ABx_lx)
-  {
+    if (computeEigVecs) cholB.matrixU().solveInPlace(Base::m_eivec);
+  } else if (type == ABx_lx) {
     // compute C = L' A L
     MatrixType matC = matA.template selfadjointView<Lower>();
     matC = matC * cholB.matrixL();
@@ -201,11 +191,8 @@
     Base::compute(matC, computeEigVecs ? ComputeEigenvectors : EigenvaluesOnly);
 
     // transform back the eigen vectors: evecs = inv(U) * evecs
-    if(computeEigVecs)
-      cholB.matrixU().solveInPlace(Base::m_eivec);
-  }
-  else if(type==BAx_lx)
-  {
+    if (computeEigVecs) cholB.matrixU().solveInPlace(Base::m_eivec);
+  } else if (type == BAx_lx) {
     // compute C = L' A L
     MatrixType matC = matA.template selfadjointView<Lower>();
     matC = matC * cholB.matrixL();
@@ -214,13 +201,12 @@
     Base::compute(matC, computeEigVecs ? ComputeEigenvectors : EigenvaluesOnly);
 
     // transform back the eigen vectors: evecs = L * evecs
-    if(computeEigVecs)
-      Base::m_eivec = cholB.matrixL() * Base::m_eivec;
+    if (computeEigVecs) Base::m_eivec = cholB.matrixL() * Base::m_eivec;
   }
 
   return *this;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_GENERALIZEDSELFADJOINTEIGENSOLVER_H
+#endif  // EIGEN_GENERALIZEDSELFADJOINTEIGENSOLVER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h
index 1f21139..8f3c1b3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h
@@ -11,299 +11,283 @@
 #ifndef EIGEN_HESSENBERGDECOMPOSITION_H
 #define EIGEN_HESSENBERGDECOMPOSITION_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-  
-template<typename MatrixType> struct HessenbergDecompositionMatrixHReturnType;
-template<typename MatrixType>
-struct traits<HessenbergDecompositionMatrixHReturnType<MatrixType> >
-{
+
+template <typename MatrixType>
+struct HessenbergDecompositionMatrixHReturnType;
+template <typename MatrixType>
+struct traits<HessenbergDecompositionMatrixHReturnType<MatrixType>> {
   typedef MatrixType ReturnType;
 };
 
-}
+}  // namespace internal
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class HessenbergDecomposition
-  *
-  * \brief Reduces a square matrix to Hessenberg form by an orthogonal similarity transformation
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the Hessenberg decomposition
-  *
-  * This class performs an Hessenberg decomposition of a matrix \f$ A \f$. In
-  * the real case, the Hessenberg decomposition consists of an orthogonal
-  * matrix \f$ Q \f$ and a Hessenberg matrix \f$ H \f$ such that \f$ A = Q H
-  * Q^T \f$. An orthogonal matrix is a matrix whose inverse equals its
-  * transpose (\f$ Q^{-1} = Q^T \f$). A Hessenberg matrix has zeros below the
-  * subdiagonal, so it is almost upper triangular. The Hessenberg decomposition
-  * of a complex matrix is \f$ A = Q H Q^* \f$ with \f$ Q \f$ unitary (that is,
-  * \f$ Q^{-1} = Q^* \f$).
-  *
-  * Call the function compute() to compute the Hessenberg decomposition of a
-  * given matrix. Alternatively, you can use the
-  * HessenbergDecomposition(const MatrixType&) constructor which computes the
-  * Hessenberg decomposition at construction time. Once the decomposition is
-  * computed, you can use the matrixH() and matrixQ() functions to construct
-  * the matrices H and Q in the decomposition.
-  *
-  * The documentation for matrixH() contains an example of the typical use of
-  * this class.
-  *
-  * \sa class ComplexSchur, class Tridiagonalization, \ref QR_Module "QR Module"
-  */
-template<typename _MatrixType> class HessenbergDecomposition
-{
-  public:
+ *
+ *
+ * \class HessenbergDecomposition
+ *
+ * \brief Reduces a square matrix to Hessenberg form by an orthogonal similarity transformation
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the Hessenberg decomposition
+ *
+ * This class performs an Hessenberg decomposition of a matrix \f$ A \f$. In
+ * the real case, the Hessenberg decomposition consists of an orthogonal
+ * matrix \f$ Q \f$ and a Hessenberg matrix \f$ H \f$ such that \f$ A = Q H
+ * Q^T \f$. An orthogonal matrix is a matrix whose inverse equals its
+ * transpose (\f$ Q^{-1} = Q^T \f$). A Hessenberg matrix has zeros below the
+ * subdiagonal, so it is almost upper triangular. The Hessenberg decomposition
+ * of a complex matrix is \f$ A = Q H Q^* \f$ with \f$ Q \f$ unitary (that is,
+ * \f$ Q^{-1} = Q^* \f$).
+ *
+ * Call the function compute() to compute the Hessenberg decomposition of a
+ * given matrix. Alternatively, you can use the
+ * HessenbergDecomposition(const MatrixType&) constructor which computes the
+ * Hessenberg decomposition at construction time. Once the decomposition is
+ * computed, you can use the matrixH() and matrixQ() functions to construct
+ * the matrices H and Q in the decomposition.
+ *
+ * The documentation for matrixH() contains an example of the typical use of
+ * this class.
+ *
+ * \sa class ComplexSchur, class Tridiagonalization, \ref QR_Module "QR Module"
+ */
+template <typename MatrixType_>
+class HessenbergDecomposition {
+ public:
+  /** \brief Synonym for the template parameter \p MatrixType_. */
+  typedef MatrixType_ MatrixType;
 
-    /** \brief Synonym for the template parameter \p _MatrixType. */
-    typedef _MatrixType MatrixType;
+  enum {
+    Size = MatrixType::RowsAtCompileTime,
+    SizeMinusOne = Size == Dynamic ? Dynamic : Size - 1,
+    Options = MatrixType::Options,
+    MaxSize = MatrixType::MaxRowsAtCompileTime,
+    MaxSizeMinusOne = MaxSize == Dynamic ? Dynamic : MaxSize - 1
+  };
 
-    enum {
-      Size = MatrixType::RowsAtCompileTime,
-      SizeMinusOne = Size == Dynamic ? Dynamic : Size - 1,
-      Options = MatrixType::Options,
-      MaxSize = MatrixType::MaxRowsAtCompileTime,
-      MaxSizeMinusOne = MaxSize == Dynamic ? Dynamic : MaxSize - 1
-    };
+  /** \brief Scalar type for matrices of type #MatrixType. */
+  typedef typename MatrixType::Scalar Scalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    /** \brief Scalar type for matrices of type #MatrixType. */
-    typedef typename MatrixType::Scalar Scalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  /** \brief Type for vector of Householder coefficients.
+   *
+   * This is column vector with entries of type #Scalar. The length of the
+   * vector is one less than the size of #MatrixType, if it is a fixed-side
+   * type.
+   */
+  typedef Matrix<Scalar, SizeMinusOne, 1, Options & ~RowMajor, MaxSizeMinusOne, 1> CoeffVectorType;
 
-    /** \brief Type for vector of Householder coefficients.
-      *
-      * This is column vector with entries of type #Scalar. The length of the
-      * vector is one less than the size of #MatrixType, if it is a fixed-side
-      * type.
-      */
-    typedef Matrix<Scalar, SizeMinusOne, 1, Options & ~RowMajor, MaxSizeMinusOne, 1> CoeffVectorType;
+  /** \brief Return type of matrixQ() */
+  typedef HouseholderSequence<MatrixType, internal::remove_all_t<typename CoeffVectorType::ConjugateReturnType>>
+      HouseholderSequenceType;
 
-    /** \brief Return type of matrixQ() */
-    typedef HouseholderSequence<MatrixType,typename internal::remove_all<typename CoeffVectorType::ConjugateReturnType>::type> HouseholderSequenceType;
-    
-    typedef internal::HessenbergDecompositionMatrixHReturnType<MatrixType> MatrixHReturnType;
+  typedef internal::HessenbergDecompositionMatrixHReturnType<MatrixType> MatrixHReturnType;
 
-    /** \brief Default constructor; the decomposition will be computed later.
-      *
-      * \param [in] size  The size of the matrix whose Hessenberg decomposition will be computed.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via compute().  The \p size parameter is only
-      * used as a hint. It is not an error to give a wrong \p size, but it may
-      * impair performance.
-      *
-      * \sa compute() for an example.
-      */
-    explicit HessenbergDecomposition(Index size = Size==Dynamic ? 2 : Size)
-      : m_matrix(size,size),
-        m_temp(size),
-        m_isInitialized(false)
-    {
-      if(size>1)
-        m_hCoeffs.resize(size-1);
-    }
+  /** \brief Default constructor; the decomposition will be computed later.
+   *
+   * \param [in] size  The size of the matrix whose Hessenberg decomposition will be computed.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via compute().  The \p size parameter is only
+   * used as a hint. It is not an error to give a wrong \p size, but it may
+   * impair performance.
+   *
+   * \sa compute() for an example.
+   */
+  explicit HessenbergDecomposition(Index size = Size == Dynamic ? 2 : Size)
+      : m_matrix(size, size), m_temp(size), m_isInitialized(false) {
+    if (size > 1) m_hCoeffs.resize(size - 1);
+  }
 
-    /** \brief Constructor; computes Hessenberg decomposition of given matrix.
-      *
-      * \param[in]  matrix  Square matrix whose Hessenberg decomposition is to be computed.
-      *
-      * This constructor calls compute() to compute the Hessenberg
-      * decomposition.
-      *
-      * \sa matrixH() for an example.
-      */
-    template<typename InputType>
-    explicit HessenbergDecomposition(const EigenBase<InputType>& matrix)
-      : m_matrix(matrix.derived()),
-        m_temp(matrix.rows()),
-        m_isInitialized(false)
-    {
-      if(matrix.rows()<2)
-      {
-        m_isInitialized = true;
-        return;
-      }
-      m_hCoeffs.resize(matrix.rows()-1,1);
-      _compute(m_matrix, m_hCoeffs, m_temp);
+  /** \brief Constructor; computes Hessenberg decomposition of given matrix.
+   *
+   * \param[in]  matrix  Square matrix whose Hessenberg decomposition is to be computed.
+   *
+   * This constructor calls compute() to compute the Hessenberg
+   * decomposition.
+   *
+   * \sa matrixH() for an example.
+   */
+  template <typename InputType>
+  explicit HessenbergDecomposition(const EigenBase<InputType>& matrix)
+      : m_matrix(matrix.derived()), m_temp(matrix.rows()), m_isInitialized(false) {
+    if (matrix.rows() < 2) {
       m_isInitialized = true;
+      return;
     }
+    m_hCoeffs.resize(matrix.rows() - 1, 1);
+    _compute(m_matrix, m_hCoeffs, m_temp);
+    m_isInitialized = true;
+  }
 
-    /** \brief Computes Hessenberg decomposition of given matrix.
-      *
-      * \param[in]  matrix  Square matrix whose Hessenberg decomposition is to be computed.
-      * \returns    Reference to \c *this
-      *
-      * The Hessenberg decomposition is computed by bringing the columns of the
-      * matrix successively in the required form using Householder reflections
-      * (see, e.g., Algorithm 7.4.2 in Golub \& Van Loan, <i>%Matrix
-      * Computations</i>). The cost is \f$ 10n^3/3 \f$ flops, where \f$ n \f$
-      * denotes the size of the given matrix.
-      *
-      * This method reuses of the allocated data in the HessenbergDecomposition
-      * object.
-      *
-      * Example: \include HessenbergDecomposition_compute.cpp
-      * Output: \verbinclude HessenbergDecomposition_compute.out
-      */
-    template<typename InputType>
-    HessenbergDecomposition& compute(const EigenBase<InputType>& matrix)
-    {
-      m_matrix = matrix.derived();
-      if(matrix.rows()<2)
-      {
-        m_isInitialized = true;
-        return *this;
-      }
-      m_hCoeffs.resize(matrix.rows()-1,1);
-      _compute(m_matrix, m_hCoeffs, m_temp);
+  /** \brief Computes Hessenberg decomposition of given matrix.
+   *
+   * \param[in]  matrix  Square matrix whose Hessenberg decomposition is to be computed.
+   * \returns    Reference to \c *this
+   *
+   * The Hessenberg decomposition is computed by bringing the columns of the
+   * matrix successively in the required form using Householder reflections
+   * (see, e.g., Algorithm 7.4.2 in Golub \& Van Loan, <i>%Matrix
+   * Computations</i>). The cost is \f$ 10n^3/3 \f$ flops, where \f$ n \f$
+   * denotes the size of the given matrix.
+   *
+   * This method reuses of the allocated data in the HessenbergDecomposition
+   * object.
+   *
+   * Example: \include HessenbergDecomposition_compute.cpp
+   * Output: \verbinclude HessenbergDecomposition_compute.out
+   */
+  template <typename InputType>
+  HessenbergDecomposition& compute(const EigenBase<InputType>& matrix) {
+    m_matrix = matrix.derived();
+    if (matrix.rows() < 2) {
       m_isInitialized = true;
       return *this;
     }
+    m_hCoeffs.resize(matrix.rows() - 1, 1);
+    _compute(m_matrix, m_hCoeffs, m_temp);
+    m_isInitialized = true;
+    return *this;
+  }
 
-    /** \brief Returns the Householder coefficients.
-      *
-      * \returns a const reference to the vector of Householder coefficients
-      *
-      * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
-      * or the member function compute(const MatrixType&) has been called
-      * before to compute the Hessenberg decomposition of a matrix.
-      *
-      * The Householder coefficients allow the reconstruction of the matrix
-      * \f$ Q \f$ in the Hessenberg decomposition from the packed data.
-      *
-      * \sa packedMatrix(), \ref Householder_Module "Householder module"
-      */
-    const CoeffVectorType& householderCoefficients() const
-    {
-      eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
-      return m_hCoeffs;
-    }
+  /** \brief Returns the Householder coefficients.
+   *
+   * \returns a const reference to the vector of Householder coefficients
+   *
+   * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
+   * or the member function compute(const MatrixType&) has been called
+   * before to compute the Hessenberg decomposition of a matrix.
+   *
+   * The Householder coefficients allow the reconstruction of the matrix
+   * \f$ Q \f$ in the Hessenberg decomposition from the packed data.
+   *
+   * \sa packedMatrix(), \ref Householder_Module "Householder module"
+   */
+  const CoeffVectorType& householderCoefficients() const {
+    eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
+    return m_hCoeffs;
+  }
 
-    /** \brief Returns the internal representation of the decomposition
-      *
-      *	\returns a const reference to a matrix with the internal representation
-      *	         of the decomposition.
-      *
-      * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
-      * or the member function compute(const MatrixType&) has been called
-      * before to compute the Hessenberg decomposition of a matrix.
-      *
-      * The returned matrix contains the following information:
-      *  - the upper part and lower sub-diagonal represent the Hessenberg matrix H
-      *  - the rest of the lower part contains the Householder vectors that, combined with
-      *    Householder coefficients returned by householderCoefficients(),
-      *    allows to reconstruct the matrix Q as
-      *       \f$ Q = H_{N-1} \ldots H_1 H_0 \f$.
-      *    Here, the matrices \f$ H_i \f$ are the Householder transformations
-      *       \f$ H_i = (I - h_i v_i v_i^T) \f$
-      *    where \f$ h_i \f$ is the \f$ i \f$th Householder coefficient and
-      *    \f$ v_i \f$ is the Householder vector defined by
-      *       \f$ v_i = [ 0, \ldots, 0, 1, M(i+2,i), \ldots, M(N-1,i) ]^T \f$
-      *    with M the matrix returned by this function.
-      *
-      * See LAPACK for further details on this packed storage.
-      *
-      * Example: \include HessenbergDecomposition_packedMatrix.cpp
-      * Output: \verbinclude HessenbergDecomposition_packedMatrix.out
-      *
-      * \sa householderCoefficients()
-      */
-    const MatrixType& packedMatrix() const
-    {
-      eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
-      return m_matrix;
-    }
+  /** \brief Returns the internal representation of the decomposition
+   *
+   *	\returns a const reference to a matrix with the internal representation
+   *	         of the decomposition.
+   *
+   * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
+   * or the member function compute(const MatrixType&) has been called
+   * before to compute the Hessenberg decomposition of a matrix.
+   *
+   * The returned matrix contains the following information:
+   *  - the upper part and lower sub-diagonal represent the Hessenberg matrix H
+   *  - the rest of the lower part contains the Householder vectors that, combined with
+   *    Householder coefficients returned by householderCoefficients(),
+   *    allows to reconstruct the matrix Q as
+   *       \f$ Q = H_{N-1} \ldots H_1 H_0 \f$.
+   *    Here, the matrices \f$ H_i \f$ are the Householder transformations
+   *       \f$ H_i = (I - h_i v_i v_i^T) \f$
+   *    where \f$ h_i \f$ is the \f$ i \f$th Householder coefficient and
+   *    \f$ v_i \f$ is the Householder vector defined by
+   *       \f$ v_i = [ 0, \ldots, 0, 1, M(i+2,i), \ldots, M(N-1,i) ]^T \f$
+   *    with M the matrix returned by this function.
+   *
+   * See LAPACK for further details on this packed storage.
+   *
+   * Example: \include HessenbergDecomposition_packedMatrix.cpp
+   * Output: \verbinclude HessenbergDecomposition_packedMatrix.out
+   *
+   * \sa householderCoefficients()
+   */
+  const MatrixType& packedMatrix() const {
+    eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
+    return m_matrix;
+  }
 
-    /** \brief Reconstructs the orthogonal matrix Q in the decomposition
-      *
-      * \returns object representing the matrix Q
-      *
-      * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
-      * or the member function compute(const MatrixType&) has been called
-      * before to compute the Hessenberg decomposition of a matrix.
-      *
-      * This function returns a light-weight object of template class
-      * HouseholderSequence. You can either apply it directly to a matrix or
-      * you can convert it to a matrix of type #MatrixType.
-      *
-      * \sa matrixH() for an example, class HouseholderSequence
-      */
-    HouseholderSequenceType matrixQ() const
-    {
-      eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
-      return HouseholderSequenceType(m_matrix, m_hCoeffs.conjugate())
-             .setLength(m_matrix.rows() - 1)
-             .setShift(1);
-    }
+  /** \brief Reconstructs the orthogonal matrix Q in the decomposition
+   *
+   * \returns object representing the matrix Q
+   *
+   * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
+   * or the member function compute(const MatrixType&) has been called
+   * before to compute the Hessenberg decomposition of a matrix.
+   *
+   * This function returns a light-weight object of template class
+   * HouseholderSequence. You can either apply it directly to a matrix or
+   * you can convert it to a matrix of type #MatrixType.
+   *
+   * \sa matrixH() for an example, class HouseholderSequence
+   */
+  HouseholderSequenceType matrixQ() const {
+    eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
+    return HouseholderSequenceType(m_matrix, m_hCoeffs.conjugate()).setLength(m_matrix.rows() - 1).setShift(1);
+  }
 
-    /** \brief Constructs the Hessenberg matrix H in the decomposition
-      *
-      * \returns expression object representing the matrix H
-      *
-      * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
-      * or the member function compute(const MatrixType&) has been called
-      * before to compute the Hessenberg decomposition of a matrix.
-      *
-      * The object returned by this function constructs the Hessenberg matrix H
-      * when it is assigned to a matrix or otherwise evaluated. The matrix H is
-      * constructed from the packed matrix as returned by packedMatrix(): The
-      * upper part (including the subdiagonal) of the packed matrix contains
-      * the matrix H. It may sometimes be better to directly use the packed
-      * matrix instead of constructing the matrix H.
-      *
-      * Example: \include HessenbergDecomposition_matrixH.cpp
-      * Output: \verbinclude HessenbergDecomposition_matrixH.out
-      *
-      * \sa matrixQ(), packedMatrix()
-      */
-    MatrixHReturnType matrixH() const
-    {
-      eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
-      return MatrixHReturnType(*this);
-    }
+  /** \brief Constructs the Hessenberg matrix H in the decomposition
+   *
+   * \returns expression object representing the matrix H
+   *
+   * \pre Either the constructor HessenbergDecomposition(const MatrixType&)
+   * or the member function compute(const MatrixType&) has been called
+   * before to compute the Hessenberg decomposition of a matrix.
+   *
+   * The object returned by this function constructs the Hessenberg matrix H
+   * when it is assigned to a matrix or otherwise evaluated. The matrix H is
+   * constructed from the packed matrix as returned by packedMatrix(): The
+   * upper part (including the subdiagonal) of the packed matrix contains
+   * the matrix H. It may sometimes be better to directly use the packed
+   * matrix instead of constructing the matrix H.
+   *
+   * Example: \include HessenbergDecomposition_matrixH.cpp
+   * Output: \verbinclude HessenbergDecomposition_matrixH.out
+   *
+   * \sa matrixQ(), packedMatrix()
+   */
+  MatrixHReturnType matrixH() const {
+    eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized.");
+    return MatrixHReturnType(*this);
+  }
 
-  private:
+ private:
+  typedef Matrix<Scalar, 1, Size, int(Options) | int(RowMajor), 1, MaxSize> VectorType;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs, VectorType& temp);
 
-    typedef Matrix<Scalar, 1, Size, int(Options) | int(RowMajor), 1, MaxSize> VectorType;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs, VectorType& temp);
-
-  protected:
-    MatrixType m_matrix;
-    CoeffVectorType m_hCoeffs;
-    VectorType m_temp;
-    bool m_isInitialized;
+ protected:
+  MatrixType m_matrix;
+  CoeffVectorType m_hCoeffs;
+  VectorType m_temp;
+  bool m_isInitialized;
 };
 
 /** \internal
-  * Performs a tridiagonal decomposition of \a matA in place.
-  *
-  * \param matA the input selfadjoint matrix
-  * \param hCoeffs returned Householder coefficients
-  *
-  * The result is written in the lower triangular part of \a matA.
-  *
-  * Implemented from Golub's "%Matrix Computations", algorithm 8.3.1.
-  *
-  * \sa packedMatrix()
-  */
-template<typename MatrixType>
-void HessenbergDecomposition<MatrixType>::_compute(MatrixType& matA, CoeffVectorType& hCoeffs, VectorType& temp)
-{
-  eigen_assert(matA.rows()==matA.cols());
+ * Performs a tridiagonal decomposition of \a matA in place.
+ *
+ * \param matA the input selfadjoint matrix
+ * \param hCoeffs returned Householder coefficients
+ *
+ * The result is written in the lower triangular part of \a matA.
+ *
+ * Implemented from Golub's "%Matrix Computations", algorithm 8.3.1.
+ *
+ * \sa packedMatrix()
+ */
+template <typename MatrixType>
+void HessenbergDecomposition<MatrixType>::_compute(MatrixType& matA, CoeffVectorType& hCoeffs, VectorType& temp) {
+  eigen_assert(matA.rows() == matA.cols());
   Index n = matA.rows();
   temp.resize(n);
-  for (Index i = 0; i<n-1; ++i)
-  {
+  for (Index i = 0; i < n - 1; ++i) {
     // let's consider the vector v = i-th column starting at position i+1
-    Index remainingSize = n-i-1;
+    Index remainingSize = n - i - 1;
     RealScalar beta;
     Scalar h;
     matA.col(i).tail(remainingSize).makeHouseholderInPlace(h, beta);
-    matA.col(i).coeffRef(i+1) = beta;
+    matA.col(i).coeffRef(i + 1) = beta;
     hCoeffs.coeffRef(i) = h;
 
     // Apply similarity transformation to remaining columns,
@@ -311,64 +295,62 @@
 
     // A = H A
     matA.bottomRightCorner(remainingSize, remainingSize)
-        .applyHouseholderOnTheLeft(matA.col(i).tail(remainingSize-1), h, &temp.coeffRef(0));
+        .applyHouseholderOnTheLeft(matA.col(i).tail(remainingSize - 1), h, &temp.coeffRef(0));
 
     // A = A H'
     matA.rightCols(remainingSize)
-        .applyHouseholderOnTheRight(matA.col(i).tail(remainingSize-1), numext::conj(h), &temp.coeffRef(0));
+        .applyHouseholderOnTheRight(matA.col(i).tail(remainingSize - 1), numext::conj(h), &temp.coeffRef(0));
   }
 }
 
 namespace internal {
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \brief Expression type for return value of HessenbergDecomposition::matrixH()
-  *
-  * \tparam MatrixType type of matrix in the Hessenberg decomposition
-  *
-  * Objects of this type represent the Hessenberg matrix in the Hessenberg
-  * decomposition of some matrix. The object holds a reference to the
-  * HessenbergDecomposition class until the it is assigned or evaluated for
-  * some other reason (the reference should remain valid during the life time
-  * of this object). This class is the return type of
-  * HessenbergDecomposition::matrixH(); there is probably no other use for this
-  * class.
-  */
-template<typename MatrixType> struct HessenbergDecompositionMatrixHReturnType
-: public ReturnByValue<HessenbergDecompositionMatrixHReturnType<MatrixType> >
-{
-  public:
-    /** \brief Constructor.
-      *
-      * \param[in] hess  Hessenberg decomposition
-      */
-    HessenbergDecompositionMatrixHReturnType(const HessenbergDecomposition<MatrixType>& hess) : m_hess(hess) { }
+ *
+ *
+ * \brief Expression type for return value of HessenbergDecomposition::matrixH()
+ *
+ * \tparam MatrixType type of matrix in the Hessenberg decomposition
+ *
+ * Objects of this type represent the Hessenberg matrix in the Hessenberg
+ * decomposition of some matrix. The object holds a reference to the
+ * HessenbergDecomposition class until the it is assigned or evaluated for
+ * some other reason (the reference should remain valid during the life time
+ * of this object). This class is the return type of
+ * HessenbergDecomposition::matrixH(); there is probably no other use for this
+ * class.
+ */
+template <typename MatrixType>
+struct HessenbergDecompositionMatrixHReturnType
+    : public ReturnByValue<HessenbergDecompositionMatrixHReturnType<MatrixType>> {
+ public:
+  /** \brief Constructor.
+   *
+   * \param[in] hess  Hessenberg decomposition
+   */
+  HessenbergDecompositionMatrixHReturnType(const HessenbergDecomposition<MatrixType>& hess) : m_hess(hess) {}
 
-    /** \brief Hessenberg matrix in decomposition.
-      *
-      * \param[out] result  Hessenberg matrix in decomposition \p hess which
-      *                     was passed to the constructor
-      */
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      result = m_hess.packedMatrix();
-      Index n = result.rows();
-      if (n>2)
-        result.bottomLeftCorner(n-2, n-2).template triangularView<Lower>().setZero();
-    }
+  /** \brief Hessenberg matrix in decomposition.
+   *
+   * \param[out] result  Hessenberg matrix in decomposition \p hess which
+   *                     was passed to the constructor
+   */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    result = m_hess.packedMatrix();
+    Index n = result.rows();
+    if (n > 2) result.bottomLeftCorner(n - 2, n - 2).template triangularView<Lower>().setZero();
+  }
 
-    Index rows() const { return m_hess.packedMatrix().rows(); }
-    Index cols() const { return m_hess.packedMatrix().cols(); }
+  Index rows() const { return m_hess.packedMatrix().rows(); }
+  Index cols() const { return m_hess.packedMatrix().cols(); }
 
-  protected:
-    const HessenbergDecomposition<MatrixType>& m_hess;
+ protected:
+  const HessenbergDecomposition<MatrixType>& m_hess;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_HESSENBERGDECOMPOSITION_H
+#endif  // EIGEN_HESSENBERGDECOMPOSITION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/InternalHeaderCheck.h
new file mode 100644
index 0000000..374cbd4
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_EIGENVALUES_MODULE_H
+#error "Please include Eigen/Eigenvalues instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
index 66e5a3d..62227bd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
@@ -11,148 +11,132 @@
 #ifndef EIGEN_MATRIXBASEEIGENVALUES_H
 #define EIGEN_MATRIXBASEEIGENVALUES_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename Derived, bool IsComplex>
-struct eigenvalues_selector
-{
+template <typename Derived, bool IsComplex>
+struct eigenvalues_selector {
   // this is the implementation for the case IsComplex = true
-  static inline typename MatrixBase<Derived>::EigenvaluesReturnType const
-  run(const MatrixBase<Derived>& m)
-  {
+  static inline typename MatrixBase<Derived>::EigenvaluesReturnType const run(const MatrixBase<Derived>& m) {
     typedef typename Derived::PlainObject PlainObject;
     PlainObject m_eval(m);
     return ComplexEigenSolver<PlainObject>(m_eval, false).eigenvalues();
   }
 };
 
-template<typename Derived>
-struct eigenvalues_selector<Derived, false>
-{
-  static inline typename MatrixBase<Derived>::EigenvaluesReturnType const
-  run(const MatrixBase<Derived>& m)
-  {
+template <typename Derived>
+struct eigenvalues_selector<Derived, false> {
+  static inline typename MatrixBase<Derived>::EigenvaluesReturnType const run(const MatrixBase<Derived>& m) {
     typedef typename Derived::PlainObject PlainObject;
     PlainObject m_eval(m);
     return EigenSolver<PlainObject>(m_eval, false).eigenvalues();
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-/** \brief Computes the eigenvalues of a matrix 
-  * \returns Column vector containing the eigenvalues.
-  *
-  * \eigenvalues_module
-  * This function computes the eigenvalues with the help of the EigenSolver
-  * class (for real matrices) or the ComplexEigenSolver class (for complex
-  * matrices). 
-  *
-  * The eigenvalues are repeated according to their algebraic multiplicity,
-  * so there are as many eigenvalues as rows in the matrix.
-  *
-  * The SelfAdjointView class provides a better algorithm for selfadjoint
-  * matrices.
-  *
-  * Example: \include MatrixBase_eigenvalues.cpp
-  * Output: \verbinclude MatrixBase_eigenvalues.out
-  *
-  * \sa EigenSolver::eigenvalues(), ComplexEigenSolver::eigenvalues(),
-  *     SelfAdjointView::eigenvalues()
-  */
-template<typename Derived>
-inline typename MatrixBase<Derived>::EigenvaluesReturnType
-MatrixBase<Derived>::eigenvalues() const
-{
+/** \brief Computes the eigenvalues of a matrix
+ * \returns Column vector containing the eigenvalues.
+ *
+ * \eigenvalues_module
+ * This function computes the eigenvalues with the help of the EigenSolver
+ * class (for real matrices) or the ComplexEigenSolver class (for complex
+ * matrices).
+ *
+ * The eigenvalues are repeated according to their algebraic multiplicity,
+ * so there are as many eigenvalues as rows in the matrix.
+ *
+ * The SelfAdjointView class provides a better algorithm for selfadjoint
+ * matrices.
+ *
+ * Example: \include MatrixBase_eigenvalues.cpp
+ * Output: \verbinclude MatrixBase_eigenvalues.out
+ *
+ * \sa EigenSolver::eigenvalues(), ComplexEigenSolver::eigenvalues(),
+ *     SelfAdjointView::eigenvalues()
+ */
+template <typename Derived>
+inline typename MatrixBase<Derived>::EigenvaluesReturnType MatrixBase<Derived>::eigenvalues() const {
   return internal::eigenvalues_selector<Derived, NumTraits<Scalar>::IsComplex>::run(derived());
 }
 
 /** \brief Computes the eigenvalues of a matrix
-  * \returns Column vector containing the eigenvalues.
-  *
-  * \eigenvalues_module
-  * This function computes the eigenvalues with the help of the
-  * SelfAdjointEigenSolver class.  The eigenvalues are repeated according to
-  * their algebraic multiplicity, so there are as many eigenvalues as rows in
-  * the matrix.
-  *
-  * Example: \include SelfAdjointView_eigenvalues.cpp
-  * Output: \verbinclude SelfAdjointView_eigenvalues.out
-  *
-  * \sa SelfAdjointEigenSolver::eigenvalues(), MatrixBase::eigenvalues()
-  */
-template<typename MatrixType, unsigned int UpLo> 
+ * \returns Column vector containing the eigenvalues.
+ *
+ * \eigenvalues_module
+ * This function computes the eigenvalues with the help of the
+ * SelfAdjointEigenSolver class.  The eigenvalues are repeated according to
+ * their algebraic multiplicity, so there are as many eigenvalues as rows in
+ * the matrix.
+ *
+ * Example: \include SelfAdjointView_eigenvalues.cpp
+ * Output: \verbinclude SelfAdjointView_eigenvalues.out
+ *
+ * \sa SelfAdjointEigenSolver::eigenvalues(), MatrixBase::eigenvalues()
+ */
+template <typename MatrixType, unsigned int UpLo>
 EIGEN_DEVICE_FUNC inline typename SelfAdjointView<MatrixType, UpLo>::EigenvaluesReturnType
-SelfAdjointView<MatrixType, UpLo>::eigenvalues() const
-{
+SelfAdjointView<MatrixType, UpLo>::eigenvalues() const {
   PlainObject thisAsMatrix(*this);
   return SelfAdjointEigenSolver<PlainObject>(thisAsMatrix, false).eigenvalues();
 }
 
-
-
 /** \brief Computes the L2 operator norm
-  * \returns Operator norm of the matrix.
-  *
-  * \eigenvalues_module
-  * This function computes the L2 operator norm of a matrix, which is also
-  * known as the spectral norm. The norm of a matrix \f$ A \f$ is defined to be
-  * \f[ \|A\|_2 = \max_x \frac{\|Ax\|_2}{\|x\|_2} \f]
-  * where the maximum is over all vectors and the norm on the right is the
-  * Euclidean vector norm. The norm equals the largest singular value, which is
-  * the square root of the largest eigenvalue of the positive semi-definite
-  * matrix \f$ A^*A \f$.
-  *
-  * The current implementation uses the eigenvalues of \f$ A^*A \f$, as computed
-  * by SelfAdjointView::eigenvalues(), to compute the operator norm of a
-  * matrix.  The SelfAdjointView class provides a better algorithm for
-  * selfadjoint matrices.
-  *
-  * Example: \include MatrixBase_operatorNorm.cpp
-  * Output: \verbinclude MatrixBase_operatorNorm.out
-  *
-  * \sa SelfAdjointView::eigenvalues(), SelfAdjointView::operatorNorm()
-  */
-template<typename Derived>
-inline typename MatrixBase<Derived>::RealScalar
-MatrixBase<Derived>::operatorNorm() const
-{
+ * \returns Operator norm of the matrix.
+ *
+ * \eigenvalues_module
+ * This function computes the L2 operator norm of a matrix, which is also
+ * known as the spectral norm. The norm of a matrix \f$ A \f$ is defined to be
+ * \f[ \|A\|_2 = \max_x \frac{\|Ax\|_2}{\|x\|_2} \f]
+ * where the maximum is over all vectors and the norm on the right is the
+ * Euclidean vector norm. The norm equals the largest singular value, which is
+ * the square root of the largest eigenvalue of the positive semi-definite
+ * matrix \f$ A^*A \f$.
+ *
+ * The current implementation uses the eigenvalues of \f$ A^*A \f$, as computed
+ * by SelfAdjointView::eigenvalues(), to compute the operator norm of a
+ * matrix.  The SelfAdjointView class provides a better algorithm for
+ * selfadjoint matrices.
+ *
+ * Example: \include MatrixBase_operatorNorm.cpp
+ * Output: \verbinclude MatrixBase_operatorNorm.out
+ *
+ * \sa SelfAdjointView::eigenvalues(), SelfAdjointView::operatorNorm()
+ */
+template <typename Derived>
+inline typename MatrixBase<Derived>::RealScalar MatrixBase<Derived>::operatorNorm() const {
   using std::sqrt;
   typename Derived::PlainObject m_eval(derived());
   // FIXME if it is really guaranteed that the eigenvalues are already sorted,
   // then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough.
-  return sqrt((m_eval*m_eval.adjoint())
-                 .eval()
-		 .template selfadjointView<Lower>()
-		 .eigenvalues()
-		 .maxCoeff()
-		 );
+  return sqrt((m_eval * m_eval.adjoint()).eval().template selfadjointView<Lower>().eigenvalues().maxCoeff());
 }
 
 /** \brief Computes the L2 operator norm
-  * \returns Operator norm of the matrix.
-  *
-  * \eigenvalues_module
-  * This function computes the L2 operator norm of a self-adjoint matrix. For a
-  * self-adjoint matrix, the operator norm is the largest eigenvalue.
-  *
-  * The current implementation uses the eigenvalues of the matrix, as computed
-  * by eigenvalues(), to compute the operator norm of the matrix.
-  *
-  * Example: \include SelfAdjointView_operatorNorm.cpp
-  * Output: \verbinclude SelfAdjointView_operatorNorm.out
-  *
-  * \sa eigenvalues(), MatrixBase::operatorNorm()
-  */
-template<typename MatrixType, unsigned int UpLo>
+ * \returns Operator norm of the matrix.
+ *
+ * \eigenvalues_module
+ * This function computes the L2 operator norm of a self-adjoint matrix. For a
+ * self-adjoint matrix, the operator norm is the largest eigenvalue.
+ *
+ * The current implementation uses the eigenvalues of the matrix, as computed
+ * by eigenvalues(), to compute the operator norm of the matrix.
+ *
+ * Example: \include SelfAdjointView_operatorNorm.cpp
+ * Output: \verbinclude SelfAdjointView_operatorNorm.out
+ *
+ * \sa eigenvalues(), MatrixBase::operatorNorm()
+ */
+template <typename MatrixType, unsigned int UpLo>
 EIGEN_DEVICE_FUNC inline typename SelfAdjointView<MatrixType, UpLo>::RealScalar
-SelfAdjointView<MatrixType, UpLo>::operatorNorm() const
-{
+SelfAdjointView<MatrixType, UpLo>::operatorNorm() const {
   return eigenvalues().cwiseAbs().maxCoeff();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h
index 5091301..9fba7ad 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h
@@ -10,648 +10,578 @@
 #ifndef EIGEN_REAL_QZ_H
 #define EIGEN_REAL_QZ_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-  /** \eigenvalues_module \ingroup Eigenvalues_Module
+/** \eigenvalues_module \ingroup Eigenvalues_Module
+ *
+ *
+ * \class RealQZ
+ *
+ * \brief Performs a real QZ decomposition of a pair of square matrices
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the
+ * real QZ decomposition; this is expected to be an instantiation of the
+ * Matrix class template.
+ *
+ * Given a real square matrices A and B, this class computes the real QZ
+ * decomposition: \f$ A = Q S Z \f$, \f$ B = Q T Z \f$ where Q and Z are
+ * real orthogonal matrixes, T is upper-triangular matrix, and S is upper
+ * quasi-triangular matrix. An orthogonal matrix is a matrix whose
+ * inverse is equal to its transpose, \f$ U^{-1} = U^T \f$. A quasi-triangular
+ * matrix is a block-triangular matrix whose diagonal consists of 1-by-1
+ * blocks and 2-by-2 blocks where further reduction is impossible due to
+ * complex eigenvalues.
+ *
+ * The eigenvalues of the pencil \f$ A - z B \f$ can be obtained from
+ * 1x1 and 2x2 blocks on the diagonals of S and T.
+ *
+ * Call the function compute() to compute the real QZ decomposition of a
+ * given pair of matrices. Alternatively, you can use the
+ * RealQZ(const MatrixType& B, const MatrixType& B, bool computeQZ)
+ * constructor which computes the real QZ decomposition at construction
+ * time. Once the decomposition is computed, you can use the matrixS(),
+ * matrixT(), matrixQ() and matrixZ() functions to retrieve the matrices
+ * S, T, Q and Z in the decomposition. If computeQZ==false, some time
+ * is saved by not computing matrices Q and Z.
+ *
+ * Example: \include RealQZ_compute.cpp
+ * Output: \include RealQZ_compute.out
+ *
+ * \note The implementation is based on the algorithm in "Matrix Computations"
+ * by Gene H. Golub and Charles F. Van Loan, and a paper "An algorithm for
+ * generalized eigenvalue problems" by C.B.Moler and G.W.Stewart.
+ *
+ * \sa class RealSchur, class ComplexSchur, class EigenSolver, class ComplexEigenSolver
+ */
+
+template <typename MatrixType_>
+class RealQZ {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
+
+  typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
+  typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;
+
+  /** \brief Default constructor.
    *
+   * \param [in] size  Positive integer, size of the matrix whose QZ decomposition will be computed.
    *
-   * \class RealQZ
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via compute().  The \p size parameter is only
+   * used as a hint. It is not an error to give a wrong \p size, but it may
+   * impair performance.
    *
-   * \brief Performs a real QZ decomposition of a pair of square matrices
-   *
-   * \tparam _MatrixType the type of the matrix of which we are computing the
-   * real QZ decomposition; this is expected to be an instantiation of the
-   * Matrix class template.
-   *
-   * Given a real square matrices A and B, this class computes the real QZ
-   * decomposition: \f$ A = Q S Z \f$, \f$ B = Q T Z \f$ where Q and Z are
-   * real orthogonal matrixes, T is upper-triangular matrix, and S is upper
-   * quasi-triangular matrix. An orthogonal matrix is a matrix whose
-   * inverse is equal to its transpose, \f$ U^{-1} = U^T \f$. A quasi-triangular
-   * matrix is a block-triangular matrix whose diagonal consists of 1-by-1
-   * blocks and 2-by-2 blocks where further reduction is impossible due to
-   * complex eigenvalues. 
-   *
-   * The eigenvalues of the pencil \f$ A - z B \f$ can be obtained from
-   * 1x1 and 2x2 blocks on the diagonals of S and T.
-   *
-   * Call the function compute() to compute the real QZ decomposition of a
-   * given pair of matrices. Alternatively, you can use the 
-   * RealQZ(const MatrixType& B, const MatrixType& B, bool computeQZ)
-   * constructor which computes the real QZ decomposition at construction
-   * time. Once the decomposition is computed, you can use the matrixS(),
-   * matrixT(), matrixQ() and matrixZ() functions to retrieve the matrices
-   * S, T, Q and Z in the decomposition. If computeQZ==false, some time
-   * is saved by not computing matrices Q and Z.
-   *
-   * Example: \include RealQZ_compute.cpp
-   * Output: \include RealQZ_compute.out
-   *
-   * \note The implementation is based on the algorithm in "Matrix Computations"
-   * by Gene H. Golub and Charles F. Van Loan, and a paper "An algorithm for
-   * generalized eigenvalue problems" by C.B.Moler and G.W.Stewart.
-   *
-   * \sa class RealSchur, class ComplexSchur, class EigenSolver, class ComplexEigenSolver
+   * \sa compute() for an example.
    */
-
-  template<typename _MatrixType> class RealQZ
-  {
-    public:
-      typedef _MatrixType MatrixType;
-      enum {
-        RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-        ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-        Options = MatrixType::Options,
-        MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-        MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-      };
-      typedef typename MatrixType::Scalar Scalar;
-      typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
-      typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-
-      typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
-      typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;
-
-      /** \brief Default constructor.
-       *
-       * \param [in] size  Positive integer, size of the matrix whose QZ decomposition will be computed.
-       *
-       * The default constructor is useful in cases in which the user intends to
-       * perform decompositions via compute().  The \p size parameter is only
-       * used as a hint. It is not an error to give a wrong \p size, but it may
-       * impair performance.
-       *
-       * \sa compute() for an example.
-       */
-      explicit RealQZ(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime) :
-        m_S(size, size),
+  explicit RealQZ(Index size = RowsAtCompileTime == Dynamic ? 1 : RowsAtCompileTime)
+      : m_S(size, size),
         m_T(size, size),
         m_Q(size, size),
         m_Z(size, size),
-        m_workspace(size*2),
+        m_workspace(size * 2),
         m_maxIters(400),
         m_isInitialized(false),
-        m_computeQZ(true)
-      {}
+        m_computeQZ(true) {}
 
-      /** \brief Constructor; computes real QZ decomposition of given matrices
-       * 
-       * \param[in]  A          Matrix A.
-       * \param[in]  B          Matrix B.
-       * \param[in]  computeQZ  If false, A and Z are not computed.
-       *
-       * This constructor calls compute() to compute the QZ decomposition.
-       */
-      RealQZ(const MatrixType& A, const MatrixType& B, bool computeQZ = true) :
-        m_S(A.rows(),A.cols()),
-        m_T(A.rows(),A.cols()),
-        m_Q(A.rows(),A.cols()),
-        m_Z(A.rows(),A.cols()),
-        m_workspace(A.rows()*2),
+  /** \brief Constructor; computes real QZ decomposition of given matrices
+   *
+   * \param[in]  A          Matrix A.
+   * \param[in]  B          Matrix B.
+   * \param[in]  computeQZ  If false, A and Z are not computed.
+   *
+   * This constructor calls compute() to compute the QZ decomposition.
+   */
+  RealQZ(const MatrixType& A, const MatrixType& B, bool computeQZ = true)
+      : m_S(A.rows(), A.cols()),
+        m_T(A.rows(), A.cols()),
+        m_Q(A.rows(), A.cols()),
+        m_Z(A.rows(), A.cols()),
+        m_workspace(A.rows() * 2),
         m_maxIters(400),
         m_isInitialized(false),
-        m_computeQZ(true)
-      {
-        compute(A, B, computeQZ);
-      }
+        m_computeQZ(true) {
+    compute(A, B, computeQZ);
+  }
 
-      /** \brief Returns matrix Q in the QZ decomposition. 
-       *
-       * \returns A const reference to the matrix Q.
-       */
-      const MatrixType& matrixQ() const {
-        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
-        eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
-        return m_Q;
-      }
+  /** \brief Returns matrix Q in the QZ decomposition.
+   *
+   * \returns A const reference to the matrix Q.
+   */
+  const MatrixType& matrixQ() const {
+    eigen_assert(m_isInitialized && "RealQZ is not initialized.");
+    eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
+    return m_Q;
+  }
 
-      /** \brief Returns matrix Z in the QZ decomposition. 
-       *
-       * \returns A const reference to the matrix Z.
-       */
-      const MatrixType& matrixZ() const {
-        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
-        eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
-        return m_Z;
-      }
+  /** \brief Returns matrix Z in the QZ decomposition.
+   *
+   * \returns A const reference to the matrix Z.
+   */
+  const MatrixType& matrixZ() const {
+    eigen_assert(m_isInitialized && "RealQZ is not initialized.");
+    eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
+    return m_Z;
+  }
 
-      /** \brief Returns matrix S in the QZ decomposition. 
-       *
-       * \returns A const reference to the matrix S.
-       */
-      const MatrixType& matrixS() const {
-        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
-        return m_S;
-      }
+  /** \brief Returns matrix S in the QZ decomposition.
+   *
+   * \returns A const reference to the matrix S.
+   */
+  const MatrixType& matrixS() const {
+    eigen_assert(m_isInitialized && "RealQZ is not initialized.");
+    return m_S;
+  }
 
-      /** \brief Returns matrix S in the QZ decomposition. 
-       *
-       * \returns A const reference to the matrix S.
-       */
-      const MatrixType& matrixT() const {
-        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
-        return m_T;
-      }
+  /** \brief Returns matrix S in the QZ decomposition.
+   *
+   * \returns A const reference to the matrix S.
+   */
+  const MatrixType& matrixT() const {
+    eigen_assert(m_isInitialized && "RealQZ is not initialized.");
+    return m_T;
+  }
 
-      /** \brief Computes QZ decomposition of given matrix. 
-       * 
-       * \param[in]  A          Matrix A.
-       * \param[in]  B          Matrix B.
-       * \param[in]  computeQZ  If false, A and Z are not computed.
-       * \returns    Reference to \c *this
-       */
-      RealQZ& compute(const MatrixType& A, const MatrixType& B, bool computeQZ = true);
+  /** \brief Computes QZ decomposition of given matrix.
+   *
+   * \param[in]  A          Matrix A.
+   * \param[in]  B          Matrix B.
+   * \param[in]  computeQZ  If false, A and Z are not computed.
+   * \returns    Reference to \c *this
+   */
+  RealQZ& compute(const MatrixType& A, const MatrixType& B, bool computeQZ = true);
 
-      /** \brief Reports whether previous computation was successful.
-       *
-       * \returns \c Success if computation was successful, \c NoConvergence otherwise.
-       */
-      ComputationInfo info() const
-      {
-        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
-        return m_info;
-      }
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful, \c NoConvergence otherwise.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "RealQZ is not initialized.");
+    return m_info;
+  }
 
-      /** \brief Returns number of performed QR-like iterations.
-      */
-      Index iterations() const
-      {
-        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
-        return m_global_iter;
-      }
+  /** \brief Returns number of performed QR-like iterations.
+   */
+  Index iterations() const {
+    eigen_assert(m_isInitialized && "RealQZ is not initialized.");
+    return m_global_iter;
+  }
 
-      /** Sets the maximal number of iterations allowed to converge to one eigenvalue
-       * or decouple the problem.
-      */
-      RealQZ& setMaxIterations(Index maxIters)
-      {
-        m_maxIters = maxIters;
-        return *this;
-      }
+  /** Sets the maximal number of iterations allowed to converge to one eigenvalue
+   * or decouple the problem.
+   */
+  RealQZ& setMaxIterations(Index maxIters) {
+    m_maxIters = maxIters;
+    return *this;
+  }
 
-    private:
+ private:
+  MatrixType m_S, m_T, m_Q, m_Z;
+  Matrix<Scalar, Dynamic, 1> m_workspace;
+  ComputationInfo m_info;
+  Index m_maxIters;
+  bool m_isInitialized;
+  bool m_computeQZ;
+  Scalar m_normOfT, m_normOfS;
+  Index m_global_iter;
 
-      MatrixType m_S, m_T, m_Q, m_Z;
-      Matrix<Scalar,Dynamic,1> m_workspace;
-      ComputationInfo m_info;
-      Index m_maxIters;
-      bool m_isInitialized;
-      bool m_computeQZ;
-      Scalar m_normOfT, m_normOfS;
-      Index m_global_iter;
+  typedef Matrix<Scalar, 3, 1> Vector3s;
+  typedef Matrix<Scalar, 2, 1> Vector2s;
+  typedef Matrix<Scalar, 2, 2> Matrix2s;
+  typedef JacobiRotation<Scalar> JRs;
 
-      typedef Matrix<Scalar,3,1> Vector3s;
-      typedef Matrix<Scalar,2,1> Vector2s;
-      typedef Matrix<Scalar,2,2> Matrix2s;
-      typedef JacobiRotation<Scalar> JRs;
+  void hessenbergTriangular();
+  void computeNorms();
+  Index findSmallSubdiagEntry(Index iu);
+  Index findSmallDiagEntry(Index f, Index l);
+  void splitOffTwoRows(Index i);
+  void pushDownZero(Index z, Index f, Index l);
+  void step(Index f, Index l, Index iter);
 
-      void hessenbergTriangular();
-      void computeNorms();
-      Index findSmallSubdiagEntry(Index iu);
-      Index findSmallDiagEntry(Index f, Index l);
-      void splitOffTwoRows(Index i);
-      void pushDownZero(Index z, Index f, Index l);
-      void step(Index f, Index l, Index iter);
+};  // RealQZ
 
-  }; // RealQZ
+/** \internal Reduces S and T to upper Hessenberg - triangular form */
+template <typename MatrixType>
+void RealQZ<MatrixType>::hessenbergTriangular() {
+  const Index dim = m_S.cols();
 
-  /** \internal Reduces S and T to upper Hessenberg - triangular form */
-  template<typename MatrixType>
-    void RealQZ<MatrixType>::hessenbergTriangular()
-    {
-
-      const Index dim = m_S.cols();
-
-      // perform QR decomposition of T, overwrite T with R, save Q
-      HouseholderQR<MatrixType> qrT(m_T);
-      m_T = qrT.matrixQR();
-      m_T.template triangularView<StrictlyLower>().setZero();
-      m_Q = qrT.householderQ();
-      // overwrite S with Q* S
-      m_S.applyOnTheLeft(m_Q.adjoint());
-      // init Z as Identity
-      if (m_computeQZ)
-        m_Z = MatrixType::Identity(dim,dim);
-      // reduce S to upper Hessenberg with Givens rotations
-      for (Index j=0; j<=dim-3; j++) {
-        for (Index i=dim-1; i>=j+2; i--) {
-          JRs G;
-          // kill S(i,j)
-          if(m_S.coeff(i,j) != 0)
-          {
-            G.makeGivens(m_S.coeff(i-1,j), m_S.coeff(i,j), &m_S.coeffRef(i-1, j));
-            m_S.coeffRef(i,j) = Scalar(0.0);
-            m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint());
-            m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint());
-            // update Q
-            if (m_computeQZ)
-              m_Q.applyOnTheRight(i-1,i,G);
-          }
-          // kill T(i,i-1)
-          if(m_T.coeff(i,i-1)!=Scalar(0))
-          {
-            G.makeGivens(m_T.coeff(i,i), m_T.coeff(i,i-1), &m_T.coeffRef(i,i));
-            m_T.coeffRef(i,i-1) = Scalar(0.0);
-            m_S.applyOnTheRight(i,i-1,G);
-            m_T.topRows(i).applyOnTheRight(i,i-1,G);
-            // update Z
-            if (m_computeQZ)
-              m_Z.applyOnTheLeft(i,i-1,G.adjoint());
-          }
-        }
-      }
-    }
-
-  /** \internal Computes vector L1 norms of S and T when in Hessenberg-Triangular form already */
-  template<typename MatrixType>
-    inline void RealQZ<MatrixType>::computeNorms()
-    {
-      const Index size = m_S.cols();
-      m_normOfS = Scalar(0.0);
-      m_normOfT = Scalar(0.0);
-      for (Index j = 0; j < size; ++j)
-      {
-        m_normOfS += m_S.col(j).segment(0, (std::min)(size,j+2)).cwiseAbs().sum();
-        m_normOfT += m_T.row(j).segment(j, size - j).cwiseAbs().sum();
-      }
-    }
-
-
-  /** \internal Look for single small sub-diagonal element S(res, res-1) and return res (or 0) */
-  template<typename MatrixType>
-    inline Index RealQZ<MatrixType>::findSmallSubdiagEntry(Index iu)
-    {
-      using std::abs;
-      Index res = iu;
-      while (res > 0)
-      {
-        Scalar s = abs(m_S.coeff(res-1,res-1)) + abs(m_S.coeff(res,res));
-        if (s == Scalar(0.0))
-          s = m_normOfS;
-        if (abs(m_S.coeff(res,res-1)) < NumTraits<Scalar>::epsilon() * s)
-          break;
-        res--;
-      }
-      return res;
-    }
-
-  /** \internal Look for single small diagonal element T(res, res) for res between f and l, and return res (or f-1)  */
-  template<typename MatrixType>
-    inline Index RealQZ<MatrixType>::findSmallDiagEntry(Index f, Index l)
-    {
-      using std::abs;
-      Index res = l;
-      while (res >= f) {
-        if (abs(m_T.coeff(res,res)) <= NumTraits<Scalar>::epsilon() * m_normOfT)
-          break;
-        res--;
-      }
-      return res;
-    }
-
-  /** \internal decouple 2x2 diagonal block in rows i, i+1 if eigenvalues are real */
-  template<typename MatrixType>
-    inline void RealQZ<MatrixType>::splitOffTwoRows(Index i)
-    {
-      using std::abs;
-      using std::sqrt;
-      const Index dim=m_S.cols();
-      if (abs(m_S.coeff(i+1,i))==Scalar(0))
-        return;
-      Index j = findSmallDiagEntry(i,i+1);
-      if (j==i-1)
-      {
-        // block of (S T^{-1})
-        Matrix2s STi = m_T.template block<2,2>(i,i).template triangularView<Upper>().
-          template solve<OnTheRight>(m_S.template block<2,2>(i,i));
-        Scalar p = Scalar(0.5)*(STi(0,0)-STi(1,1));
-        Scalar q = p*p + STi(1,0)*STi(0,1);
-        if (q>=0) {
-          Scalar z = sqrt(q);
-          // one QR-like iteration for ABi - lambda I
-          // is enough - when we know exact eigenvalue in advance,
-          // convergence is immediate
-          JRs G;
-          if (p>=0)
-            G.makeGivens(p + z, STi(1,0));
-          else
-            G.makeGivens(p - z, STi(1,0));
-          m_S.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint());
-          m_T.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint());
-          // update Q
-          if (m_computeQZ)
-            m_Q.applyOnTheRight(i,i+1,G);
-
-          G.makeGivens(m_T.coeff(i+1,i+1), m_T.coeff(i+1,i));
-          m_S.topRows(i+2).applyOnTheRight(i+1,i,G);
-          m_T.topRows(i+2).applyOnTheRight(i+1,i,G);
-          // update Z
-          if (m_computeQZ)
-            m_Z.applyOnTheLeft(i+1,i,G.adjoint());
-
-          m_S.coeffRef(i+1,i) = Scalar(0.0);
-          m_T.coeffRef(i+1,i) = Scalar(0.0);
-        }
-      }
-      else
-      {
-        pushDownZero(j,i,i+1);
-      }
-    }
-
-  /** \internal use zero in T(z,z) to zero S(l,l-1), working in block f..l */
-  template<typename MatrixType>
-    inline void RealQZ<MatrixType>::pushDownZero(Index z, Index f, Index l)
-    {
+  // perform QR decomposition of T, overwrite T with R, save Q
+  HouseholderQR<MatrixType> qrT(m_T);
+  m_T = qrT.matrixQR();
+  m_T.template triangularView<StrictlyLower>().setZero();
+  m_Q = qrT.householderQ();
+  // overwrite S with Q* S
+  m_S.applyOnTheLeft(m_Q.adjoint());
+  // init Z as Identity
+  if (m_computeQZ) m_Z = MatrixType::Identity(dim, dim);
+  // reduce S to upper Hessenberg with Givens rotations
+  for (Index j = 0; j <= dim - 3; j++) {
+    for (Index i = dim - 1; i >= j + 2; i--) {
       JRs G;
-      const Index dim = m_S.cols();
-      for (Index zz=z; zz<l; zz++)
-      {
-        // push 0 down
-        Index firstColS = zz>f ? (zz-1) : zz;
-        G.makeGivens(m_T.coeff(zz, zz+1), m_T.coeff(zz+1, zz+1));
-        m_S.rightCols(dim-firstColS).applyOnTheLeft(zz,zz+1,G.adjoint());
-        m_T.rightCols(dim-zz).applyOnTheLeft(zz,zz+1,G.adjoint());
-        m_T.coeffRef(zz+1,zz+1) = Scalar(0.0);
+      // kill S(i,j)
+      if (!numext::is_exactly_zero(m_S.coeff(i, j))) {
+        G.makeGivens(m_S.coeff(i - 1, j), m_S.coeff(i, j), &m_S.coeffRef(i - 1, j));
+        m_S.coeffRef(i, j) = Scalar(0.0);
+        m_S.rightCols(dim - j - 1).applyOnTheLeft(i - 1, i, G.adjoint());
+        m_T.rightCols(dim - i + 1).applyOnTheLeft(i - 1, i, G.adjoint());
         // update Q
-        if (m_computeQZ)
-          m_Q.applyOnTheRight(zz,zz+1,G);
-        // kill S(zz+1, zz-1)
-        if (zz>f)
-        {
-          G.makeGivens(m_S.coeff(zz+1, zz), m_S.coeff(zz+1,zz-1));
-          m_S.topRows(zz+2).applyOnTheRight(zz, zz-1,G);
-          m_T.topRows(zz+1).applyOnTheRight(zz, zz-1,G);
-          m_S.coeffRef(zz+1,zz-1) = Scalar(0.0);
-          // update Z
-          if (m_computeQZ)
-            m_Z.applyOnTheLeft(zz,zz-1,G.adjoint());
-        }
+        if (m_computeQZ) m_Q.applyOnTheRight(i - 1, i, G);
       }
-      // finally kill S(l,l-1)
-      G.makeGivens(m_S.coeff(l,l), m_S.coeff(l,l-1));
-      m_S.applyOnTheRight(l,l-1,G);
-      m_T.applyOnTheRight(l,l-1,G);
-      m_S.coeffRef(l,l-1)=Scalar(0.0);
-      // update Z
-      if (m_computeQZ)
-        m_Z.applyOnTheLeft(l,l-1,G.adjoint());
-    }
-
-  /** \internal QR-like iterative step for block f..l */
-  template<typename MatrixType>
-    inline void RealQZ<MatrixType>::step(Index f, Index l, Index iter)
-    {
-      using std::abs;
-      const Index dim = m_S.cols();
-
-      // x, y, z
-      Scalar x, y, z;
-      if (iter==10)
-      {
-        // Wilkinson ad hoc shift
-        const Scalar
-          a11=m_S.coeff(f+0,f+0), a12=m_S.coeff(f+0,f+1),
-          a21=m_S.coeff(f+1,f+0), a22=m_S.coeff(f+1,f+1), a32=m_S.coeff(f+2,f+1),
-          b12=m_T.coeff(f+0,f+1),
-          b11i=Scalar(1.0)/m_T.coeff(f+0,f+0),
-          b22i=Scalar(1.0)/m_T.coeff(f+1,f+1),
-          a87=m_S.coeff(l-1,l-2),
-          a98=m_S.coeff(l-0,l-1),
-          b77i=Scalar(1.0)/m_T.coeff(l-2,l-2),
-          b88i=Scalar(1.0)/m_T.coeff(l-1,l-1);
-        Scalar ss = abs(a87*b77i) + abs(a98*b88i),
-               lpl = Scalar(1.5)*ss,
-               ll = ss*ss;
-        x = ll + a11*a11*b11i*b11i - lpl*a11*b11i + a12*a21*b11i*b22i
-          - a11*a21*b12*b11i*b11i*b22i;
-        y = a11*a21*b11i*b11i - lpl*a21*b11i + a21*a22*b11i*b22i 
-          - a21*a21*b12*b11i*b11i*b22i;
-        z = a21*a32*b11i*b22i;
-      }
-      else if (iter==16)
-      {
-        // another exceptional shift
-        x = m_S.coeff(f,f)/m_T.coeff(f,f)-m_S.coeff(l,l)/m_T.coeff(l,l) + m_S.coeff(l,l-1)*m_T.coeff(l-1,l) /
-          (m_T.coeff(l-1,l-1)*m_T.coeff(l,l));
-        y = m_S.coeff(f+1,f)/m_T.coeff(f,f);
-        z = 0;
-      }
-      else if (iter>23 && !(iter%8))
-      {
-        // extremely exceptional shift
-        x = internal::random<Scalar>(-1.0,1.0);
-        y = internal::random<Scalar>(-1.0,1.0);
-        z = internal::random<Scalar>(-1.0,1.0);
-      }
-      else
-      {
-        // Compute the shifts: (x,y,z,0...) = (AB^-1 - l1 I) (AB^-1 - l2 I) e1
-        // where l1 and l2 are the eigenvalues of the 2x2 matrix C = U V^-1 where
-        // U and V are 2x2 bottom right sub matrices of A and B. Thus:
-        //  = AB^-1AB^-1 + l1 l2 I - (l1+l2)(AB^-1)
-        //  = AB^-1AB^-1 + det(M) - tr(M)(AB^-1)
-        // Since we are only interested in having x, y, z with a correct ratio, we have:
-        const Scalar
-          a11 = m_S.coeff(f,f),     a12 = m_S.coeff(f,f+1),
-          a21 = m_S.coeff(f+1,f),   a22 = m_S.coeff(f+1,f+1),
-                                    a32 = m_S.coeff(f+2,f+1),
-
-          a88 = m_S.coeff(l-1,l-1), a89 = m_S.coeff(l-1,l),
-          a98 = m_S.coeff(l,l-1),   a99 = m_S.coeff(l,l),
-
-          b11 = m_T.coeff(f,f),     b12 = m_T.coeff(f,f+1),
-                                    b22 = m_T.coeff(f+1,f+1),
-
-          b88 = m_T.coeff(l-1,l-1), b89 = m_T.coeff(l-1,l),
-                                    b99 = m_T.coeff(l,l);
-
-        x = ( (a88/b88 - a11/b11)*(a99/b99 - a11/b11) - (a89/b99)*(a98/b88) + (a98/b88)*(b89/b99)*(a11/b11) ) * (b11/a21)
-          + a12/b22 - (a11/b11)*(b12/b22);
-        y = (a22/b22-a11/b11) - (a21/b11)*(b12/b22) - (a88/b88-a11/b11) - (a99/b99-a11/b11) + (a98/b88)*(b89/b99);
-        z = a32/b22;
-      }
-
-      JRs G;
-
-      for (Index k=f; k<=l-2; k++)
-      {
-        // variables for Householder reflections
-        Vector2s essential2;
-        Scalar tau, beta;
-
-        Vector3s hr(x,y,z);
-
-        // Q_k to annihilate S(k+1,k-1) and S(k+2,k-1)
-        hr.makeHouseholderInPlace(tau, beta);
-        essential2 = hr.template bottomRows<2>();
-        Index fc=(std::max)(k-1,Index(0));  // first col to update
-        m_S.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
-        m_T.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
-        if (m_computeQZ)
-          m_Q.template middleCols<3>(k).applyHouseholderOnTheRight(essential2, tau, m_workspace.data());
-        if (k>f)
-          m_S.coeffRef(k+2,k-1) = m_S.coeffRef(k+1,k-1) = Scalar(0.0);
-
-        // Z_{k1} to annihilate T(k+2,k+1) and T(k+2,k)
-        hr << m_T.coeff(k+2,k+2),m_T.coeff(k+2,k),m_T.coeff(k+2,k+1);
-        hr.makeHouseholderInPlace(tau, beta);
-        essential2 = hr.template bottomRows<2>();
-        {
-          Index lr = (std::min)(k+4,dim); // last row to update
-          Map<Matrix<Scalar,Dynamic,1> > tmp(m_workspace.data(),lr);
-          // S
-          tmp = m_S.template middleCols<2>(k).topRows(lr) * essential2;
-          tmp += m_S.col(k+2).head(lr);
-          m_S.col(k+2).head(lr) -= tau*tmp;
-          m_S.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint();
-          // T
-          tmp = m_T.template middleCols<2>(k).topRows(lr) * essential2;
-          tmp += m_T.col(k+2).head(lr);
-          m_T.col(k+2).head(lr) -= tau*tmp;
-          m_T.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint();
-        }
-        if (m_computeQZ)
-        {
-          // Z
-          Map<Matrix<Scalar,1,Dynamic> > tmp(m_workspace.data(),dim);
-          tmp = essential2.adjoint()*(m_Z.template middleRows<2>(k));
-          tmp += m_Z.row(k+2);
-          m_Z.row(k+2) -= tau*tmp;
-          m_Z.template middleRows<2>(k) -= essential2 * (tau*tmp);
-        }
-        m_T.coeffRef(k+2,k) = m_T.coeffRef(k+2,k+1) = Scalar(0.0);
-
-        // Z_{k2} to annihilate T(k+1,k)
-        G.makeGivens(m_T.coeff(k+1,k+1), m_T.coeff(k+1,k));
-        m_S.applyOnTheRight(k+1,k,G);
-        m_T.applyOnTheRight(k+1,k,G);
+      // kill T(i,i-1)
+      if (!numext::is_exactly_zero(m_T.coeff(i, i - 1))) {
+        G.makeGivens(m_T.coeff(i, i), m_T.coeff(i, i - 1), &m_T.coeffRef(i, i));
+        m_T.coeffRef(i, i - 1) = Scalar(0.0);
+        m_S.applyOnTheRight(i, i - 1, G);
+        m_T.topRows(i).applyOnTheRight(i, i - 1, G);
         // update Z
-        if (m_computeQZ)
-          m_Z.applyOnTheLeft(k+1,k,G.adjoint());
-        m_T.coeffRef(k+1,k) = Scalar(0.0);
-
-        // update x,y,z
-        x = m_S.coeff(k+1,k);
-        y = m_S.coeff(k+2,k);
-        if (k < l-2)
-          z = m_S.coeff(k+3,k);
-      } // loop over k
-
-      // Q_{n-1} to annihilate y = S(l,l-2)
-      G.makeGivens(x,y);
-      m_S.applyOnTheLeft(l-1,l,G.adjoint());
-      m_T.applyOnTheLeft(l-1,l,G.adjoint());
-      if (m_computeQZ)
-        m_Q.applyOnTheRight(l-1,l,G);
-      m_S.coeffRef(l,l-2) = Scalar(0.0);
-
-      // Z_{n-1} to annihilate T(l,l-1)
-      G.makeGivens(m_T.coeff(l,l),m_T.coeff(l,l-1));
-      m_S.applyOnTheRight(l,l-1,G);
-      m_T.applyOnTheRight(l,l-1,G);
-      if (m_computeQZ)
-        m_Z.applyOnTheLeft(l,l-1,G.adjoint());
-      m_T.coeffRef(l,l-1) = Scalar(0.0);
+        if (m_computeQZ) m_Z.applyOnTheLeft(i, i - 1, G.adjoint());
+      }
     }
+  }
+}
 
-  template<typename MatrixType>
-    RealQZ<MatrixType>& RealQZ<MatrixType>::compute(const MatrixType& A_in, const MatrixType& B_in, bool computeQZ)
+/** \internal Computes vector L1 norms of S and T when in Hessenberg-Triangular form already */
+template <typename MatrixType>
+inline void RealQZ<MatrixType>::computeNorms() {
+  const Index size = m_S.cols();
+  m_normOfS = Scalar(0.0);
+  m_normOfT = Scalar(0.0);
+  for (Index j = 0; j < size; ++j) {
+    m_normOfS += m_S.col(j).segment(0, (std::min)(size, j + 2)).cwiseAbs().sum();
+    m_normOfT += m_T.row(j).segment(j, size - j).cwiseAbs().sum();
+  }
+}
+
+/** \internal Look for single small sub-diagonal element S(res, res-1) and return res (or 0) */
+template <typename MatrixType>
+inline Index RealQZ<MatrixType>::findSmallSubdiagEntry(Index iu) {
+  using std::abs;
+  Index res = iu;
+  while (res > 0) {
+    Scalar s = abs(m_S.coeff(res - 1, res - 1)) + abs(m_S.coeff(res, res));
+    if (numext::is_exactly_zero(s)) s = m_normOfS;
+    if (abs(m_S.coeff(res, res - 1)) < NumTraits<Scalar>::epsilon() * s) break;
+    res--;
+  }
+  return res;
+}
+
+/** \internal Look for single small diagonal element T(res, res) for res between f and l, and return res (or f-1)  */
+template <typename MatrixType>
+inline Index RealQZ<MatrixType>::findSmallDiagEntry(Index f, Index l) {
+  using std::abs;
+  Index res = l;
+  while (res >= f) {
+    if (abs(m_T.coeff(res, res)) <= NumTraits<Scalar>::epsilon() * m_normOfT) break;
+    res--;
+  }
+  return res;
+}
+
+/** \internal decouple 2x2 diagonal block in rows i, i+1 if eigenvalues are real */
+template <typename MatrixType>
+inline void RealQZ<MatrixType>::splitOffTwoRows(Index i) {
+  using std::abs;
+  using std::sqrt;
+  const Index dim = m_S.cols();
+  if (numext::is_exactly_zero(abs(m_S.coeff(i + 1, i)))) return;
+  Index j = findSmallDiagEntry(i, i + 1);
+  if (j == i - 1) {
+    // block of (S T^{-1})
+    Matrix2s STi = m_T.template block<2, 2>(i, i).template triangularView<Upper>().template solve<OnTheRight>(
+        m_S.template block<2, 2>(i, i));
+    Scalar p = Scalar(0.5) * (STi(0, 0) - STi(1, 1));
+    Scalar q = p * p + STi(1, 0) * STi(0, 1);
+    if (q >= 0) {
+      Scalar z = sqrt(q);
+      // one QR-like iteration for ABi - lambda I
+      // is enough - when we know exact eigenvalue in advance,
+      // convergence is immediate
+      JRs G;
+      if (p >= 0)
+        G.makeGivens(p + z, STi(1, 0));
+      else
+        G.makeGivens(p - z, STi(1, 0));
+      m_S.rightCols(dim - i).applyOnTheLeft(i, i + 1, G.adjoint());
+      m_T.rightCols(dim - i).applyOnTheLeft(i, i + 1, G.adjoint());
+      // update Q
+      if (m_computeQZ) m_Q.applyOnTheRight(i, i + 1, G);
+
+      G.makeGivens(m_T.coeff(i + 1, i + 1), m_T.coeff(i + 1, i));
+      m_S.topRows(i + 2).applyOnTheRight(i + 1, i, G);
+      m_T.topRows(i + 2).applyOnTheRight(i + 1, i, G);
+      // update Z
+      if (m_computeQZ) m_Z.applyOnTheLeft(i + 1, i, G.adjoint());
+
+      m_S.coeffRef(i + 1, i) = Scalar(0.0);
+      m_T.coeffRef(i + 1, i) = Scalar(0.0);
+    }
+  } else {
+    pushDownZero(j, i, i + 1);
+  }
+}
+
+/** \internal use zero in T(z,z) to zero S(l,l-1), working in block f..l */
+template <typename MatrixType>
+inline void RealQZ<MatrixType>::pushDownZero(Index z, Index f, Index l) {
+  JRs G;
+  const Index dim = m_S.cols();
+  for (Index zz = z; zz < l; zz++) {
+    // push 0 down
+    Index firstColS = zz > f ? (zz - 1) : zz;
+    G.makeGivens(m_T.coeff(zz, zz + 1), m_T.coeff(zz + 1, zz + 1));
+    m_S.rightCols(dim - firstColS).applyOnTheLeft(zz, zz + 1, G.adjoint());
+    m_T.rightCols(dim - zz).applyOnTheLeft(zz, zz + 1, G.adjoint());
+    m_T.coeffRef(zz + 1, zz + 1) = Scalar(0.0);
+    // update Q
+    if (m_computeQZ) m_Q.applyOnTheRight(zz, zz + 1, G);
+    // kill S(zz+1, zz-1)
+    if (zz > f) {
+      G.makeGivens(m_S.coeff(zz + 1, zz), m_S.coeff(zz + 1, zz - 1));
+      m_S.topRows(zz + 2).applyOnTheRight(zz, zz - 1, G);
+      m_T.topRows(zz + 1).applyOnTheRight(zz, zz - 1, G);
+      m_S.coeffRef(zz + 1, zz - 1) = Scalar(0.0);
+      // update Z
+      if (m_computeQZ) m_Z.applyOnTheLeft(zz, zz - 1, G.adjoint());
+    }
+  }
+  // finally kill S(l,l-1)
+  G.makeGivens(m_S.coeff(l, l), m_S.coeff(l, l - 1));
+  m_S.applyOnTheRight(l, l - 1, G);
+  m_T.applyOnTheRight(l, l - 1, G);
+  m_S.coeffRef(l, l - 1) = Scalar(0.0);
+  // update Z
+  if (m_computeQZ) m_Z.applyOnTheLeft(l, l - 1, G.adjoint());
+}
+
+/** \internal QR-like iterative step for block f..l */
+template <typename MatrixType>
+inline void RealQZ<MatrixType>::step(Index f, Index l, Index iter) {
+  using std::abs;
+  const Index dim = m_S.cols();
+
+  // x, y, z
+  Scalar x, y, z;
+  if (iter == 10) {
+    // Wilkinson ad hoc shift
+    const Scalar a11 = m_S.coeff(f + 0, f + 0), a12 = m_S.coeff(f + 0, f + 1), a21 = m_S.coeff(f + 1, f + 0),
+                 a22 = m_S.coeff(f + 1, f + 1), a32 = m_S.coeff(f + 2, f + 1), b12 = m_T.coeff(f + 0, f + 1),
+                 b11i = Scalar(1.0) / m_T.coeff(f + 0, f + 0), b22i = Scalar(1.0) / m_T.coeff(f + 1, f + 1),
+                 a87 = m_S.coeff(l - 1, l - 2), a98 = m_S.coeff(l - 0, l - 1),
+                 b77i = Scalar(1.0) / m_T.coeff(l - 2, l - 2), b88i = Scalar(1.0) / m_T.coeff(l - 1, l - 1);
+    Scalar ss = abs(a87 * b77i) + abs(a98 * b88i), lpl = Scalar(1.5) * ss, ll = ss * ss;
+    x = ll + a11 * a11 * b11i * b11i - lpl * a11 * b11i + a12 * a21 * b11i * b22i -
+        a11 * a21 * b12 * b11i * b11i * b22i;
+    y = a11 * a21 * b11i * b11i - lpl * a21 * b11i + a21 * a22 * b11i * b22i - a21 * a21 * b12 * b11i * b11i * b22i;
+    z = a21 * a32 * b11i * b22i;
+  } else if (iter == 16) {
+    // another exceptional shift
+    x = m_S.coeff(f, f) / m_T.coeff(f, f) - m_S.coeff(l, l) / m_T.coeff(l, l) +
+        m_S.coeff(l, l - 1) * m_T.coeff(l - 1, l) / (m_T.coeff(l - 1, l - 1) * m_T.coeff(l, l));
+    y = m_S.coeff(f + 1, f) / m_T.coeff(f, f);
+    z = 0;
+  } else if (iter > 23 && !(iter % 8)) {
+    // extremely exceptional shift
+    x = internal::random<Scalar>(-1.0, 1.0);
+    y = internal::random<Scalar>(-1.0, 1.0);
+    z = internal::random<Scalar>(-1.0, 1.0);
+  } else {
+    // Compute the shifts: (x,y,z,0...) = (AB^-1 - l1 I) (AB^-1 - l2 I) e1
+    // where l1 and l2 are the eigenvalues of the 2x2 matrix C = U V^-1 where
+    // U and V are 2x2 bottom right sub matrices of A and B. Thus:
+    //  = AB^-1AB^-1 + l1 l2 I - (l1+l2)(AB^-1)
+    //  = AB^-1AB^-1 + det(M) - tr(M)(AB^-1)
+    // Since we are only interested in having x, y, z with a correct ratio, we have:
+    const Scalar a11 = m_S.coeff(f, f), a12 = m_S.coeff(f, f + 1), a21 = m_S.coeff(f + 1, f),
+                 a22 = m_S.coeff(f + 1, f + 1), a32 = m_S.coeff(f + 2, f + 1),
+
+                 a88 = m_S.coeff(l - 1, l - 1), a89 = m_S.coeff(l - 1, l), a98 = m_S.coeff(l, l - 1),
+                 a99 = m_S.coeff(l, l),
+
+                 b11 = m_T.coeff(f, f), b12 = m_T.coeff(f, f + 1), b22 = m_T.coeff(f + 1, f + 1),
+
+                 b88 = m_T.coeff(l - 1, l - 1), b89 = m_T.coeff(l - 1, l), b99 = m_T.coeff(l, l);
+
+    x = ((a88 / b88 - a11 / b11) * (a99 / b99 - a11 / b11) - (a89 / b99) * (a98 / b88) +
+         (a98 / b88) * (b89 / b99) * (a11 / b11)) *
+            (b11 / a21) +
+        a12 / b22 - (a11 / b11) * (b12 / b22);
+    y = (a22 / b22 - a11 / b11) - (a21 / b11) * (b12 / b22) - (a88 / b88 - a11 / b11) - (a99 / b99 - a11 / b11) +
+        (a98 / b88) * (b89 / b99);
+    z = a32 / b22;
+  }
+
+  JRs G;
+
+  for (Index k = f; k <= l - 2; k++) {
+    // variables for Householder reflections
+    Vector2s essential2;
+    Scalar tau, beta;
+
+    Vector3s hr(x, y, z);
+
+    // Q_k to annihilate S(k+1,k-1) and S(k+2,k-1)
+    hr.makeHouseholderInPlace(tau, beta);
+    essential2 = hr.template bottomRows<2>();
+    Index fc = (std::max)(k - 1, Index(0));  // first col to update
+    m_S.template middleRows<3>(k).rightCols(dim - fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
+    m_T.template middleRows<3>(k).rightCols(dim - fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
+    if (m_computeQZ) m_Q.template middleCols<3>(k).applyHouseholderOnTheRight(essential2, tau, m_workspace.data());
+    if (k > f) m_S.coeffRef(k + 2, k - 1) = m_S.coeffRef(k + 1, k - 1) = Scalar(0.0);
+
+    // Z_{k1} to annihilate T(k+2,k+1) and T(k+2,k)
+    hr << m_T.coeff(k + 2, k + 2), m_T.coeff(k + 2, k), m_T.coeff(k + 2, k + 1);
+    hr.makeHouseholderInPlace(tau, beta);
+    essential2 = hr.template bottomRows<2>();
     {
+      Index lr = (std::min)(k + 4, dim);  // last row to update
+      Map<Matrix<Scalar, Dynamic, 1> > tmp(m_workspace.data(), lr);
+      // S
+      tmp = m_S.template middleCols<2>(k).topRows(lr) * essential2;
+      tmp += m_S.col(k + 2).head(lr);
+      m_S.col(k + 2).head(lr) -= tau * tmp;
+      m_S.template middleCols<2>(k).topRows(lr) -= (tau * tmp) * essential2.adjoint();
+      // T
+      tmp = m_T.template middleCols<2>(k).topRows(lr) * essential2;
+      tmp += m_T.col(k + 2).head(lr);
+      m_T.col(k + 2).head(lr) -= tau * tmp;
+      m_T.template middleCols<2>(k).topRows(lr) -= (tau * tmp) * essential2.adjoint();
+    }
+    if (m_computeQZ) {
+      // Z
+      Map<Matrix<Scalar, 1, Dynamic> > tmp(m_workspace.data(), dim);
+      tmp = essential2.adjoint() * (m_Z.template middleRows<2>(k));
+      tmp += m_Z.row(k + 2);
+      m_Z.row(k + 2) -= tau * tmp;
+      m_Z.template middleRows<2>(k) -= essential2 * (tau * tmp);
+    }
+    m_T.coeffRef(k + 2, k) = m_T.coeffRef(k + 2, k + 1) = Scalar(0.0);
 
-      const Index dim = A_in.cols();
+    // Z_{k2} to annihilate T(k+1,k)
+    G.makeGivens(m_T.coeff(k + 1, k + 1), m_T.coeff(k + 1, k));
+    m_S.applyOnTheRight(k + 1, k, G);
+    m_T.applyOnTheRight(k + 1, k, G);
+    // update Z
+    if (m_computeQZ) m_Z.applyOnTheLeft(k + 1, k, G.adjoint());
+    m_T.coeffRef(k + 1, k) = Scalar(0.0);
 
-      eigen_assert (A_in.rows()==dim && A_in.cols()==dim 
-          && B_in.rows()==dim && B_in.cols()==dim 
-          && "Need square matrices of the same dimension");
+    // update x,y,z
+    x = m_S.coeff(k + 1, k);
+    y = m_S.coeff(k + 2, k);
+    if (k < l - 2) z = m_S.coeff(k + 3, k);
+  }  // loop over k
 
-      m_isInitialized = true;
-      m_computeQZ = computeQZ;
-      m_S = A_in; m_T = B_in;
-      m_workspace.resize(dim*2);
-      m_global_iter = 0;
+  // Q_{n-1} to annihilate y = S(l,l-2)
+  G.makeGivens(x, y);
+  m_S.applyOnTheLeft(l - 1, l, G.adjoint());
+  m_T.applyOnTheLeft(l - 1, l, G.adjoint());
+  if (m_computeQZ) m_Q.applyOnTheRight(l - 1, l, G);
+  m_S.coeffRef(l, l - 2) = Scalar(0.0);
 
-      // entrance point: hessenberg triangular decomposition
-      hessenbergTriangular();
-      // compute L1 vector norms of T, S into m_normOfS, m_normOfT
-      computeNorms();
+  // Z_{n-1} to annihilate T(l,l-1)
+  G.makeGivens(m_T.coeff(l, l), m_T.coeff(l, l - 1));
+  m_S.applyOnTheRight(l, l - 1, G);
+  m_T.applyOnTheRight(l, l - 1, G);
+  if (m_computeQZ) m_Z.applyOnTheLeft(l, l - 1, G.adjoint());
+  m_T.coeffRef(l, l - 1) = Scalar(0.0);
+}
 
-      Index l = dim-1, 
-            f, 
-            local_iter = 0;
+template <typename MatrixType>
+RealQZ<MatrixType>& RealQZ<MatrixType>::compute(const MatrixType& A_in, const MatrixType& B_in, bool computeQZ) {
+  const Index dim = A_in.cols();
 
-      while (l>0 && local_iter<m_maxIters)
-      {
-        f = findSmallSubdiagEntry(l);
-        // now rows and columns f..l (including) decouple from the rest of the problem
-        if (f>0) m_S.coeffRef(f,f-1) = Scalar(0.0);
-        if (f == l) // One root found
-        {
-          l--;
-          local_iter = 0;
-        }
-        else if (f == l-1) // Two roots found
-        {
-          splitOffTwoRows(f);
-          l -= 2;
-          local_iter = 0;
-        }
-        else // No convergence yet
-        {
-          // if there's zero on diagonal of T, we can isolate an eigenvalue with Givens rotations
-          Index z = findSmallDiagEntry(f,l);
-          if (z>=f)
-          {
-            // zero found
-            pushDownZero(z,f,l);
-          }
-          else
-          {
-            // We are sure now that S.block(f,f, l-f+1,l-f+1) is underuced upper-Hessenberg 
-            // and T.block(f,f, l-f+1,l-f+1) is invertible uper-triangular, which allows to
-            // apply a QR-like iteration to rows and columns f..l.
-            step(f,l, local_iter);
-            local_iter++;
-            m_global_iter++;
-          }
-        }
+  eigen_assert(A_in.rows() == dim && A_in.cols() == dim && B_in.rows() == dim && B_in.cols() == dim &&
+               "Need square matrices of the same dimension");
+
+  m_isInitialized = true;
+  m_computeQZ = computeQZ;
+  m_S = A_in;
+  m_T = B_in;
+  m_workspace.resize(dim * 2);
+  m_global_iter = 0;
+
+  // entrance point: hessenberg triangular decomposition
+  hessenbergTriangular();
+  // compute L1 vector norms of T, S into m_normOfS, m_normOfT
+  computeNorms();
+
+  Index l = dim - 1, f, local_iter = 0;
+
+  while (l > 0 && local_iter < m_maxIters) {
+    f = findSmallSubdiagEntry(l);
+    // now rows and columns f..l (including) decouple from the rest of the problem
+    if (f > 0) m_S.coeffRef(f, f - 1) = Scalar(0.0);
+    if (f == l)  // One root found
+    {
+      l--;
+      local_iter = 0;
+    } else if (f == l - 1)  // Two roots found
+    {
+      splitOffTwoRows(f);
+      l -= 2;
+      local_iter = 0;
+    } else  // No convergence yet
+    {
+      // if there's zero on diagonal of T, we can isolate an eigenvalue with Givens rotations
+      Index z = findSmallDiagEntry(f, l);
+      if (z >= f) {
+        // zero found
+        pushDownZero(z, f, l);
+      } else {
+        // We are sure now that S.block(f,f, l-f+1,l-f+1) is underuced upper-Hessenberg
+        // and T.block(f,f, l-f+1,l-f+1) is invertible uper-triangular, which allows to
+        // apply a QR-like iteration to rows and columns f..l.
+        step(f, l, local_iter);
+        local_iter++;
+        m_global_iter++;
       }
-      // check if we converged before reaching iterations limit
-      m_info = (local_iter<m_maxIters) ? Success : NoConvergence;
+    }
+  }
+  // check if we converged before reaching iterations limit
+  m_info = (local_iter < m_maxIters) ? Success : NoConvergence;
 
-      // For each non triangular 2x2 diagonal block of S,
-      //    reduce the respective 2x2 diagonal block of T to positive diagonal form using 2x2 SVD.
-      // This step is not mandatory for QZ, but it does help further extraction of eigenvalues/eigenvectors,
-      // and is in par with Lapack/Matlab QZ.
-      if(m_info==Success)
-      {
-        for(Index i=0; i<dim-1; ++i)
-        {
-          if(m_S.coeff(i+1, i) != Scalar(0))
-          {
-            JacobiRotation<Scalar> j_left, j_right;
-            internal::real_2x2_jacobi_svd(m_T, i, i+1, &j_left, &j_right);
+  // For each non triangular 2x2 diagonal block of S,
+  //    reduce the respective 2x2 diagonal block of T to positive diagonal form using 2x2 SVD.
+  // This step is not mandatory for QZ, but it does help further extraction of eigenvalues/eigenvectors,
+  // and is in par with Lapack/Matlab QZ.
+  if (m_info == Success) {
+    for (Index i = 0; i < dim - 1; ++i) {
+      if (!numext::is_exactly_zero(m_S.coeff(i + 1, i))) {
+        JacobiRotation<Scalar> j_left, j_right;
+        internal::real_2x2_jacobi_svd(m_T, i, i + 1, &j_left, &j_right);
 
-            // Apply resulting Jacobi rotations
-            m_S.applyOnTheLeft(i,i+1,j_left);
-            m_S.applyOnTheRight(i,i+1,j_right);
-            m_T.applyOnTheLeft(i,i+1,j_left);
-            m_T.applyOnTheRight(i,i+1,j_right);
-            m_T(i+1,i) = m_T(i,i+1) = Scalar(0);
+        // Apply resulting Jacobi rotations
+        m_S.applyOnTheLeft(i, i + 1, j_left);
+        m_S.applyOnTheRight(i, i + 1, j_right);
+        m_T.applyOnTheLeft(i, i + 1, j_left);
+        m_T.applyOnTheRight(i, i + 1, j_right);
+        m_T(i + 1, i) = m_T(i, i + 1) = Scalar(0);
 
-            if(m_computeQZ) {
-              m_Q.applyOnTheRight(i,i+1,j_left.transpose());
-              m_Z.applyOnTheLeft(i,i+1,j_right.transpose());
-            }
-
-            i++;
-          }
+        if (m_computeQZ) {
+          m_Q.applyOnTheRight(i, i + 1, j_left.transpose());
+          m_Z.applyOnTheLeft(i, i + 1, j_right.transpose());
         }
+
+        i++;
       }
+    }
+  }
 
-      return *this;
-    } // end compute
+  return *this;
+}  // end compute
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif //EIGEN_REAL_QZ
+#endif  // EIGEN_REAL_QZ
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h
index 7304ef3..1ac9af8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h
@@ -13,254 +13,243 @@
 
 #include "./HessenbergDecomposition.h"
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class RealSchur
-  *
-  * \brief Performs a real Schur decomposition of a square matrix
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the
-  * real Schur decomposition; this is expected to be an instantiation of the
-  * Matrix class template.
-  *
-  * Given a real square matrix A, this class computes the real Schur
-  * decomposition: \f$ A = U T U^T \f$ where U is a real orthogonal matrix and
-  * T is a real quasi-triangular matrix. An orthogonal matrix is a matrix whose
-  * inverse is equal to its transpose, \f$ U^{-1} = U^T \f$. A quasi-triangular
-  * matrix is a block-triangular matrix whose diagonal consists of 1-by-1
-  * blocks and 2-by-2 blocks with complex eigenvalues. The eigenvalues of the
-  * blocks on the diagonal of T are the same as the eigenvalues of the matrix
-  * A, and thus the real Schur decomposition is used in EigenSolver to compute
-  * the eigendecomposition of a matrix.
-  *
-  * Call the function compute() to compute the real Schur decomposition of a
-  * given matrix. Alternatively, you can use the RealSchur(const MatrixType&, bool)
-  * constructor which computes the real Schur decomposition at construction
-  * time. Once the decomposition is computed, you can use the matrixU() and
-  * matrixT() functions to retrieve the matrices U and T in the decomposition.
-  *
-  * The documentation of RealSchur(const MatrixType&, bool) contains an example
-  * of the typical use of this class.
-  *
-  * \note The implementation is adapted from
-  * <a href="http://math.nist.gov/javanumerics/jama/">JAMA</a> (public domain).
-  * Their code is based on EISPACK.
-  *
-  * \sa class ComplexSchur, class EigenSolver, class ComplexEigenSolver
-  */
-template<typename _MatrixType> class RealSchur
-{
-  public:
-    typedef _MatrixType MatrixType;
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      Options = MatrixType::Options,
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+ *
+ *
+ * \class RealSchur
+ *
+ * \brief Performs a real Schur decomposition of a square matrix
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the
+ * real Schur decomposition; this is expected to be an instantiation of the
+ * Matrix class template.
+ *
+ * Given a real square matrix A, this class computes the real Schur
+ * decomposition: \f$ A = U T U^T \f$ where U is a real orthogonal matrix and
+ * T is a real quasi-triangular matrix. An orthogonal matrix is a matrix whose
+ * inverse is equal to its transpose, \f$ U^{-1} = U^T \f$. A quasi-triangular
+ * matrix is a block-triangular matrix whose diagonal consists of 1-by-1
+ * blocks and 2-by-2 blocks with complex eigenvalues. The eigenvalues of the
+ * blocks on the diagonal of T are the same as the eigenvalues of the matrix
+ * A, and thus the real Schur decomposition is used in EigenSolver to compute
+ * the eigendecomposition of a matrix.
+ *
+ * Call the function compute() to compute the real Schur decomposition of a
+ * given matrix. Alternatively, you can use the RealSchur(const MatrixType&, bool)
+ * constructor which computes the real Schur decomposition at construction
+ * time. Once the decomposition is computed, you can use the matrixU() and
+ * matrixT() functions to retrieve the matrices U and T in the decomposition.
+ *
+ * The documentation of RealSchur(const MatrixType&, bool) contains an example
+ * of the typical use of this class.
+ *
+ * \note The implementation is adapted from
+ * <a href="http://math.nist.gov/javanumerics/jama/">JAMA</a> (public domain).
+ * Their code is based on EISPACK.
+ *
+ * \sa class ComplexSchur, class EigenSolver, class ComplexEigenSolver
+ */
+template <typename MatrixType_>
+class RealSchur {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
-    typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;
+  typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
+  typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;
 
-    /** \brief Default constructor.
-      *
-      * \param [in] size  Positive integer, size of the matrix whose Schur decomposition will be computed.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via compute().  The \p size parameter is only
-      * used as a hint. It is not an error to give a wrong \p size, but it may
-      * impair performance.
-      *
-      * \sa compute() for an example.
-      */
-    explicit RealSchur(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime)
-            : m_matT(size, size),
-              m_matU(size, size),
-              m_workspaceVector(size),
-              m_hess(size),
-              m_isInitialized(false),
-              m_matUisUptodate(false),
-              m_maxIters(-1)
-    { }
+  /** \brief Default constructor.
+   *
+   * \param [in] size  Positive integer, size of the matrix whose Schur decomposition will be computed.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via compute().  The \p size parameter is only
+   * used as a hint. It is not an error to give a wrong \p size, but it may
+   * impair performance.
+   *
+   * \sa compute() for an example.
+   */
+  explicit RealSchur(Index size = RowsAtCompileTime == Dynamic ? 1 : RowsAtCompileTime)
+      : m_matT(size, size),
+        m_matU(size, size),
+        m_workspaceVector(size),
+        m_hess(size),
+        m_isInitialized(false),
+        m_matUisUptodate(false),
+        m_maxIters(-1) {}
 
-    /** \brief Constructor; computes real Schur decomposition of given matrix. 
-      * 
-      * \param[in]  matrix    Square matrix whose Schur decomposition is to be computed.
-      * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
-      *
-      * This constructor calls compute() to compute the Schur decomposition.
-      *
-      * Example: \include RealSchur_RealSchur_MatrixType.cpp
-      * Output: \verbinclude RealSchur_RealSchur_MatrixType.out
-      */
-    template<typename InputType>
-    explicit RealSchur(const EigenBase<InputType>& matrix, bool computeU = true)
-            : m_matT(matrix.rows(),matrix.cols()),
-              m_matU(matrix.rows(),matrix.cols()),
-              m_workspaceVector(matrix.rows()),
-              m_hess(matrix.rows()),
-              m_isInitialized(false),
-              m_matUisUptodate(false),
-              m_maxIters(-1)
-    {
-      compute(matrix.derived(), computeU);
-    }
+  /** \brief Constructor; computes real Schur decomposition of given matrix.
+   *
+   * \param[in]  matrix    Square matrix whose Schur decomposition is to be computed.
+   * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
+   *
+   * This constructor calls compute() to compute the Schur decomposition.
+   *
+   * Example: \include RealSchur_RealSchur_MatrixType.cpp
+   * Output: \verbinclude RealSchur_RealSchur_MatrixType.out
+   */
+  template <typename InputType>
+  explicit RealSchur(const EigenBase<InputType>& matrix, bool computeU = true)
+      : m_matT(matrix.rows(), matrix.cols()),
+        m_matU(matrix.rows(), matrix.cols()),
+        m_workspaceVector(matrix.rows()),
+        m_hess(matrix.rows()),
+        m_isInitialized(false),
+        m_matUisUptodate(false),
+        m_maxIters(-1) {
+    compute(matrix.derived(), computeU);
+  }
 
-    /** \brief Returns the orthogonal matrix in the Schur decomposition. 
-      *
-      * \returns A const reference to the matrix U.
-      *
-      * \pre Either the constructor RealSchur(const MatrixType&, bool) or the
-      * member function compute(const MatrixType&, bool) has been called before
-      * to compute the Schur decomposition of a matrix, and \p computeU was set
-      * to true (the default value).
-      *
-      * \sa RealSchur(const MatrixType&, bool) for an example
-      */
-    const MatrixType& matrixU() const
-    {
-      eigen_assert(m_isInitialized && "RealSchur is not initialized.");
-      eigen_assert(m_matUisUptodate && "The matrix U has not been computed during the RealSchur decomposition.");
-      return m_matU;
-    }
+  /** \brief Returns the orthogonal matrix in the Schur decomposition.
+   *
+   * \returns A const reference to the matrix U.
+   *
+   * \pre Either the constructor RealSchur(const MatrixType&, bool) or the
+   * member function compute(const MatrixType&, bool) has been called before
+   * to compute the Schur decomposition of a matrix, and \p computeU was set
+   * to true (the default value).
+   *
+   * \sa RealSchur(const MatrixType&, bool) for an example
+   */
+  const MatrixType& matrixU() const {
+    eigen_assert(m_isInitialized && "RealSchur is not initialized.");
+    eigen_assert(m_matUisUptodate && "The matrix U has not been computed during the RealSchur decomposition.");
+    return m_matU;
+  }
 
-    /** \brief Returns the quasi-triangular matrix in the Schur decomposition. 
-      *
-      * \returns A const reference to the matrix T.
-      *
-      * \pre Either the constructor RealSchur(const MatrixType&, bool) or the
-      * member function compute(const MatrixType&, bool) has been called before
-      * to compute the Schur decomposition of a matrix.
-      *
-      * \sa RealSchur(const MatrixType&, bool) for an example
-      */
-    const MatrixType& matrixT() const
-    {
-      eigen_assert(m_isInitialized && "RealSchur is not initialized.");
-      return m_matT;
-    }
-  
-    /** \brief Computes Schur decomposition of given matrix. 
-      * 
-      * \param[in]  matrix    Square matrix whose Schur decomposition is to be computed.
-      * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
-      * \returns    Reference to \c *this
-      *
-      * The Schur decomposition is computed by first reducing the matrix to
-      * Hessenberg form using the class HessenbergDecomposition. The Hessenberg
-      * matrix is then reduced to triangular form by performing Francis QR
-      * iterations with implicit double shift. The cost of computing the Schur
-      * decomposition depends on the number of iterations; as a rough guide, it
-      * may be taken to be \f$25n^3\f$ flops if \a computeU is true and
-      * \f$10n^3\f$ flops if \a computeU is false.
-      *
-      * Example: \include RealSchur_compute.cpp
-      * Output: \verbinclude RealSchur_compute.out
-      *
-      * \sa compute(const MatrixType&, bool, Index)
-      */
-    template<typename InputType>
-    RealSchur& compute(const EigenBase<InputType>& matrix, bool computeU = true);
+  /** \brief Returns the quasi-triangular matrix in the Schur decomposition.
+   *
+   * \returns A const reference to the matrix T.
+   *
+   * \pre Either the constructor RealSchur(const MatrixType&, bool) or the
+   * member function compute(const MatrixType&, bool) has been called before
+   * to compute the Schur decomposition of a matrix.
+   *
+   * \sa RealSchur(const MatrixType&, bool) for an example
+   */
+  const MatrixType& matrixT() const {
+    eigen_assert(m_isInitialized && "RealSchur is not initialized.");
+    return m_matT;
+  }
 
-    /** \brief Computes Schur decomposition of a Hessenberg matrix H = Z T Z^T
-     *  \param[in] matrixH Matrix in Hessenberg form H
-     *  \param[in] matrixQ orthogonal matrix Q that transform a matrix A to H : A = Q H Q^T
-     *  \param computeU Computes the matriX U of the Schur vectors
-     * \return Reference to \c *this
-     * 
-     *  This routine assumes that the matrix is already reduced in Hessenberg form matrixH
-     *  using either the class HessenbergDecomposition or another mean. 
-     *  It computes the upper quasi-triangular matrix T of the Schur decomposition of H
-     *  When computeU is true, this routine computes the matrix U such that 
-     *  A = U T U^T =  (QZ) T (QZ)^T = Q H Q^T where A is the initial matrix
-     * 
-     * NOTE Q is referenced if computeU is true; so, if the initial orthogonal matrix
-     * is not available, the user should give an identity matrix (Q.setIdentity())
-     * 
-     * \sa compute(const MatrixType&, bool)
-     */
-    template<typename HessMatrixType, typename OrthMatrixType>
-    RealSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ,  bool computeU);
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "RealSchur is not initialized.");
-      return m_info;
-    }
+  /** \brief Computes Schur decomposition of given matrix.
+   *
+   * \param[in]  matrix    Square matrix whose Schur decomposition is to be computed.
+   * \param[in]  computeU  If true, both T and U are computed; if false, only T is computed.
+   * \returns    Reference to \c *this
+   *
+   * The Schur decomposition is computed by first reducing the matrix to
+   * Hessenberg form using the class HessenbergDecomposition. The Hessenberg
+   * matrix is then reduced to triangular form by performing Francis QR
+   * iterations with implicit double shift. The cost of computing the Schur
+   * decomposition depends on the number of iterations; as a rough guide, it
+   * may be taken to be \f$25n^3\f$ flops if \a computeU is true and
+   * \f$10n^3\f$ flops if \a computeU is false.
+   *
+   * Example: \include RealSchur_compute.cpp
+   * Output: \verbinclude RealSchur_compute.out
+   *
+   * \sa compute(const MatrixType&, bool, Index)
+   */
+  template <typename InputType>
+  RealSchur& compute(const EigenBase<InputType>& matrix, bool computeU = true);
 
-    /** \brief Sets the maximum number of iterations allowed. 
-      *
-      * If not specified by the user, the maximum number of iterations is m_maxIterationsPerRow times the size
-      * of the matrix.
-      */
-    RealSchur& setMaxIterations(Index maxIters)
-    {
-      m_maxIters = maxIters;
-      return *this;
-    }
+  /** \brief Computes Schur decomposition of a Hessenberg matrix H = Z T Z^T
+   *  \param[in] matrixH Matrix in Hessenberg form H
+   *  \param[in] matrixQ orthogonal matrix Q that transform a matrix A to H : A = Q H Q^T
+   *  \param computeU Computes the matriX U of the Schur vectors
+   * \return Reference to \c *this
+   *
+   *  This routine assumes that the matrix is already reduced in Hessenberg form matrixH
+   *  using either the class HessenbergDecomposition or another mean.
+   *  It computes the upper quasi-triangular matrix T of the Schur decomposition of H
+   *  When computeU is true, this routine computes the matrix U such that
+   *  A = U T U^T =  (QZ) T (QZ)^T = Q H Q^T where A is the initial matrix
+   *
+   * NOTE Q is referenced if computeU is true; so, if the initial orthogonal matrix
+   * is not available, the user should give an identity matrix (Q.setIdentity())
+   *
+   * \sa compute(const MatrixType&, bool)
+   */
+  template <typename HessMatrixType, typename OrthMatrixType>
+  RealSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ, bool computeU);
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful, \c NoConvergence otherwise.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "RealSchur is not initialized.");
+    return m_info;
+  }
 
-    /** \brief Returns the maximum number of iterations. */
-    Index getMaxIterations()
-    {
-      return m_maxIters;
-    }
+  /** \brief Sets the maximum number of iterations allowed.
+   *
+   * If not specified by the user, the maximum number of iterations is m_maxIterationsPerRow times the size
+   * of the matrix.
+   */
+  RealSchur& setMaxIterations(Index maxIters) {
+    m_maxIters = maxIters;
+    return *this;
+  }
 
-    /** \brief Maximum number of iterations per row.
-      *
-      * If not otherwise specified, the maximum number of iterations is this number times the size of the
-      * matrix. It is currently set to 40.
-      */
-    static const int m_maxIterationsPerRow = 40;
+  /** \brief Returns the maximum number of iterations. */
+  Index getMaxIterations() { return m_maxIters; }
 
-  private:
-    
-    MatrixType m_matT;
-    MatrixType m_matU;
-    ColumnVectorType m_workspaceVector;
-    HessenbergDecomposition<MatrixType> m_hess;
-    ComputationInfo m_info;
-    bool m_isInitialized;
-    bool m_matUisUptodate;
-    Index m_maxIters;
+  /** \brief Maximum number of iterations per row.
+   *
+   * If not otherwise specified, the maximum number of iterations is this number times the size of the
+   * matrix. It is currently set to 40.
+   */
+  static const int m_maxIterationsPerRow = 40;
 
-    typedef Matrix<Scalar,3,1> Vector3s;
+ private:
+  MatrixType m_matT;
+  MatrixType m_matU;
+  ColumnVectorType m_workspaceVector;
+  HessenbergDecomposition<MatrixType> m_hess;
+  ComputationInfo m_info;
+  bool m_isInitialized;
+  bool m_matUisUptodate;
+  Index m_maxIters;
 
-    Scalar computeNormOfT();
-    Index findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero);
-    void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift);
-    void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo);
-    void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector);
-    void performFrancisQRStep(Index il, Index im, Index iu, bool computeU, const Vector3s& firstHouseholderVector, Scalar* workspace);
+  typedef Matrix<Scalar, 3, 1> Vector3s;
+
+  Scalar computeNormOfT();
+  Index findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero);
+  void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift);
+  void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo);
+  void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector);
+  void performFrancisQRStep(Index il, Index im, Index iu, bool computeU, const Vector3s& firstHouseholderVector,
+                            Scalar* workspace);
 };
 
-
-template<typename MatrixType>
-template<typename InputType>
-RealSchur<MatrixType>& RealSchur<MatrixType>::compute(const EigenBase<InputType>& matrix, bool computeU)
-{
+template <typename MatrixType>
+template <typename InputType>
+RealSchur<MatrixType>& RealSchur<MatrixType>::compute(const EigenBase<InputType>& matrix, bool computeU) {
   const Scalar considerAsZero = (std::numeric_limits<Scalar>::min)();
 
   eigen_assert(matrix.cols() == matrix.rows());
   Index maxIters = m_maxIters;
-  if (maxIters == -1)
-    maxIters = m_maxIterationsPerRow * matrix.rows();
+  if (maxIters == -1) maxIters = m_maxIterationsPerRow * matrix.rows();
 
   Scalar scale = matrix.derived().cwiseAbs().maxCoeff();
-  if(scale<considerAsZero)
-  {
-    m_matT.setZero(matrix.rows(),matrix.cols());
-    if(computeU)
-      m_matU.setIdentity(matrix.rows(),matrix.cols());
+  if (scale < considerAsZero) {
+    m_matT.setZero(matrix.rows(), matrix.cols());
+    if (computeU) m_matU.setIdentity(matrix.rows(), matrix.cols());
     m_info = Success;
     m_isInitialized = true;
     m_matUisUptodate = computeU;
@@ -268,74 +257,67 @@
   }
 
   // Step 1. Reduce to Hessenberg form
-  m_hess.compute(matrix.derived()/scale);
+  m_hess.compute(matrix.derived() / scale);
 
   // Step 2. Reduce to real Schur form
   // Note: we copy m_hess.matrixQ() into m_matU here and not in computeFromHessenberg
   //       to be able to pass our working-space buffer for the Householder to Dense evaluation.
   m_workspaceVector.resize(matrix.cols());
-  if(computeU)
-    m_hess.matrixQ().evalTo(m_matU, m_workspaceVector);
+  if (computeU) m_hess.matrixQ().evalTo(m_matU, m_workspaceVector);
   computeFromHessenberg(m_hess.matrixH(), m_matU, computeU);
 
   m_matT *= scale;
-  
+
   return *this;
 }
-template<typename MatrixType>
-template<typename HessMatrixType, typename OrthMatrixType>
-RealSchur<MatrixType>& RealSchur<MatrixType>::computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ,  bool computeU)
-{
+template <typename MatrixType>
+template <typename HessMatrixType, typename OrthMatrixType>
+RealSchur<MatrixType>& RealSchur<MatrixType>::computeFromHessenberg(const HessMatrixType& matrixH,
+                                                                    const OrthMatrixType& matrixQ, bool computeU) {
   using std::abs;
 
   m_matT = matrixH;
   m_workspaceVector.resize(m_matT.cols());
-  if(computeU && !internal::is_same_dense(m_matU,matrixQ))
-    m_matU = matrixQ;
-  
+  if (computeU && !internal::is_same_dense(m_matU, matrixQ)) m_matU = matrixQ;
+
   Index maxIters = m_maxIters;
-  if (maxIters == -1)
-    maxIters = m_maxIterationsPerRow * matrixH.rows();
+  if (maxIters == -1) maxIters = m_maxIterationsPerRow * matrixH.rows();
   Scalar* workspace = &m_workspaceVector.coeffRef(0);
 
-  // The matrix m_matT is divided in three parts. 
-  // Rows 0,...,il-1 are decoupled from the rest because m_matT(il,il-1) is zero. 
+  // The matrix m_matT is divided in three parts.
+  // Rows 0,...,il-1 are decoupled from the rest because m_matT(il,il-1) is zero.
   // Rows il,...,iu is the part we are working on (the active window).
   // Rows iu+1,...,end are already brought in triangular form.
   Index iu = m_matT.cols() - 1;
-  Index iter = 0;      // iteration count for current eigenvalue
-  Index totalIter = 0; // iteration count for whole matrix
-  Scalar exshift(0);   // sum of exceptional shifts
+  Index iter = 0;       // iteration count for current eigenvalue
+  Index totalIter = 0;  // iteration count for whole matrix
+  Scalar exshift(0);    // sum of exceptional shifts
   Scalar norm = computeNormOfT();
   // sub-diagonal entries smaller than considerAsZero will be treated as zero.
   // We use eps^2 to enable more precision in small eigenvalues.
-  Scalar considerAsZero = numext::maxi<Scalar>( norm * numext::abs2(NumTraits<Scalar>::epsilon()),
-                                                (std::numeric_limits<Scalar>::min)() );
+  Scalar considerAsZero =
+      numext::maxi<Scalar>(norm * numext::abs2(NumTraits<Scalar>::epsilon()), (std::numeric_limits<Scalar>::min)());
 
-  if(norm!=Scalar(0))
-  {
-    while (iu >= 0)
-    {
-      Index il = findSmallSubdiagEntry(iu,considerAsZero);
+  if (!numext::is_exactly_zero(norm)) {
+    while (iu >= 0) {
+      Index il = findSmallSubdiagEntry(iu, considerAsZero);
 
       // Check for convergence
-      if (il == iu) // One root found
+      if (il == iu)  // One root found
       {
-        m_matT.coeffRef(iu,iu) = m_matT.coeff(iu,iu) + exshift;
-        if (iu > 0)
-          m_matT.coeffRef(iu, iu-1) = Scalar(0);
+        m_matT.coeffRef(iu, iu) = m_matT.coeff(iu, iu) + exshift;
+        if (iu > 0) m_matT.coeffRef(iu, iu - 1) = Scalar(0);
         iu--;
         iter = 0;
-      }
-      else if (il == iu-1) // Two roots found
+      } else if (il == iu - 1)  // Two roots found
       {
         splitOffTwoRows(iu, computeU, exshift);
         iu -= 2;
         iter = 0;
-      }
-      else // No convergence yet
+      } else  // No convergence yet
       {
-        // The firstHouseholderVector vector has to be initialized to something to get rid of a silly GCC warning (-O1 -Wall -DNDEBUG )
+        // The firstHouseholderVector vector has to be initialized to something to get rid of a silly GCC warning (-O1
+        // -Wall -DNDEBUG )
         Vector3s firstHouseholderVector = Vector3s::Zero(), shiftInfo;
         computeShift(iu, iter, exshift, shiftInfo);
         iter = iter + 1;
@@ -347,7 +329,7 @@
       }
     }
   }
-  if(totalIter <= maxIters)
+  if (totalIter <= maxIters)
     m_info = Success;
   else
     m_info = NoConvergence;
@@ -358,201 +340,179 @@
 }
 
 /** \internal Computes and returns vector L1 norm of T */
-template<typename MatrixType>
-inline typename MatrixType::Scalar RealSchur<MatrixType>::computeNormOfT()
-{
+template <typename MatrixType>
+inline typename MatrixType::Scalar RealSchur<MatrixType>::computeNormOfT() {
   const Index size = m_matT.cols();
   // FIXME to be efficient the following would requires a triangular reduxion code
-  // Scalar norm = m_matT.upper().cwiseAbs().sum() 
+  // Scalar norm = m_matT.upper().cwiseAbs().sum()
   //               + m_matT.bottomLeftCorner(size-1,size-1).diagonal().cwiseAbs().sum();
   Scalar norm(0);
-  for (Index j = 0; j < size; ++j)
-    norm += m_matT.col(j).segment(0, (std::min)(size,j+2)).cwiseAbs().sum();
+  for (Index j = 0; j < size; ++j) norm += m_matT.col(j).segment(0, (std::min)(size, j + 2)).cwiseAbs().sum();
   return norm;
 }
 
 /** \internal Look for single small sub-diagonal element and returns its index */
-template<typename MatrixType>
-inline Index RealSchur<MatrixType>::findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero)
-{
+template <typename MatrixType>
+inline Index RealSchur<MatrixType>::findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero) {
   using std::abs;
   Index res = iu;
-  while (res > 0)
-  {
-    Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res));
+  while (res > 0) {
+    Scalar s = abs(m_matT.coeff(res - 1, res - 1)) + abs(m_matT.coeff(res, res));
 
     s = numext::maxi<Scalar>(s * NumTraits<Scalar>::epsilon(), considerAsZero);
-    
-    if (abs(m_matT.coeff(res,res-1)) <= s)
-      break;
+
+    if (abs(m_matT.coeff(res, res - 1)) <= s) break;
     res--;
   }
   return res;
 }
 
 /** \internal Update T given that rows iu-1 and iu decouple from the rest. */
-template<typename MatrixType>
-inline void RealSchur<MatrixType>::splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift)
-{
-  using std::sqrt;
+template <typename MatrixType>
+inline void RealSchur<MatrixType>::splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift) {
   using std::abs;
+  using std::sqrt;
   const Index size = m_matT.cols();
 
-  // The eigenvalues of the 2x2 matrix [a b; c d] are 
+  // The eigenvalues of the 2x2 matrix [a b; c d] are
   // trace +/- sqrt(discr/4) where discr = tr^2 - 4*det, tr = a + d, det = ad - bc
-  Scalar p = Scalar(0.5) * (m_matT.coeff(iu-1,iu-1) - m_matT.coeff(iu,iu));
-  Scalar q = p * p + m_matT.coeff(iu,iu-1) * m_matT.coeff(iu-1,iu);   // q = tr^2 / 4 - det = discr/4
-  m_matT.coeffRef(iu,iu) += exshift;
-  m_matT.coeffRef(iu-1,iu-1) += exshift;
+  Scalar p = Scalar(0.5) * (m_matT.coeff(iu - 1, iu - 1) - m_matT.coeff(iu, iu));
+  Scalar q = p * p + m_matT.coeff(iu, iu - 1) * m_matT.coeff(iu - 1, iu);  // q = tr^2 / 4 - det = discr/4
+  m_matT.coeffRef(iu, iu) += exshift;
+  m_matT.coeffRef(iu - 1, iu - 1) += exshift;
 
-  if (q >= Scalar(0)) // Two real eigenvalues
+  if (q >= Scalar(0))  // Two real eigenvalues
   {
     Scalar z = sqrt(abs(q));
     JacobiRotation<Scalar> rot;
     if (p >= Scalar(0))
-      rot.makeGivens(p + z, m_matT.coeff(iu, iu-1));
+      rot.makeGivens(p + z, m_matT.coeff(iu, iu - 1));
     else
-      rot.makeGivens(p - z, m_matT.coeff(iu, iu-1));
+      rot.makeGivens(p - z, m_matT.coeff(iu, iu - 1));
 
-    m_matT.rightCols(size-iu+1).applyOnTheLeft(iu-1, iu, rot.adjoint());
-    m_matT.topRows(iu+1).applyOnTheRight(iu-1, iu, rot);
-    m_matT.coeffRef(iu, iu-1) = Scalar(0); 
-    if (computeU)
-      m_matU.applyOnTheRight(iu-1, iu, rot);
+    m_matT.rightCols(size - iu + 1).applyOnTheLeft(iu - 1, iu, rot.adjoint());
+    m_matT.topRows(iu + 1).applyOnTheRight(iu - 1, iu, rot);
+    m_matT.coeffRef(iu, iu - 1) = Scalar(0);
+    if (computeU) m_matU.applyOnTheRight(iu - 1, iu, rot);
   }
 
-  if (iu > 1) 
-    m_matT.coeffRef(iu-1, iu-2) = Scalar(0);
+  if (iu > 1) m_matT.coeffRef(iu - 1, iu - 2) = Scalar(0);
 }
 
 /** \internal Form shift in shiftInfo, and update exshift if an exceptional shift is performed. */
-template<typename MatrixType>
-inline void RealSchur<MatrixType>::computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo)
-{
-  using std::sqrt;
+template <typename MatrixType>
+inline void RealSchur<MatrixType>::computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo) {
   using std::abs;
-  shiftInfo.coeffRef(0) = m_matT.coeff(iu,iu);
-  shiftInfo.coeffRef(1) = m_matT.coeff(iu-1,iu-1);
-  shiftInfo.coeffRef(2) = m_matT.coeff(iu,iu-1) * m_matT.coeff(iu-1,iu);
+  using std::sqrt;
+  shiftInfo.coeffRef(0) = m_matT.coeff(iu, iu);
+  shiftInfo.coeffRef(1) = m_matT.coeff(iu - 1, iu - 1);
+  shiftInfo.coeffRef(2) = m_matT.coeff(iu, iu - 1) * m_matT.coeff(iu - 1, iu);
 
   // Wilkinson's original ad hoc shift
-  if (iter == 10)
-  {
+  if (iter == 10) {
     exshift += shiftInfo.coeff(0);
-    for (Index i = 0; i <= iu; ++i)
-      m_matT.coeffRef(i,i) -= shiftInfo.coeff(0);
-    Scalar s = abs(m_matT.coeff(iu,iu-1)) + abs(m_matT.coeff(iu-1,iu-2));
+    for (Index i = 0; i <= iu; ++i) m_matT.coeffRef(i, i) -= shiftInfo.coeff(0);
+    Scalar s = abs(m_matT.coeff(iu, iu - 1)) + abs(m_matT.coeff(iu - 1, iu - 2));
     shiftInfo.coeffRef(0) = Scalar(0.75) * s;
     shiftInfo.coeffRef(1) = Scalar(0.75) * s;
     shiftInfo.coeffRef(2) = Scalar(-0.4375) * s * s;
   }
 
   // MATLAB's new ad hoc shift
-  if (iter == 30)
-  {
+  if (iter == 30) {
     Scalar s = (shiftInfo.coeff(1) - shiftInfo.coeff(0)) / Scalar(2.0);
     s = s * s + shiftInfo.coeff(2);
-    if (s > Scalar(0))
-    {
+    if (s > Scalar(0)) {
       s = sqrt(s);
-      if (shiftInfo.coeff(1) < shiftInfo.coeff(0))
-        s = -s;
+      if (shiftInfo.coeff(1) < shiftInfo.coeff(0)) s = -s;
       s = s + (shiftInfo.coeff(1) - shiftInfo.coeff(0)) / Scalar(2.0);
       s = shiftInfo.coeff(0) - shiftInfo.coeff(2) / s;
       exshift += s;
-      for (Index i = 0; i <= iu; ++i)
-        m_matT.coeffRef(i,i) -= s;
+      for (Index i = 0; i <= iu; ++i) m_matT.coeffRef(i, i) -= s;
       shiftInfo.setConstant(Scalar(0.964));
     }
   }
 }
 
 /** \internal Compute index im at which Francis QR step starts and the first Householder vector. */
-template<typename MatrixType>
-inline void RealSchur<MatrixType>::initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector)
-{
+template <typename MatrixType>
+inline void RealSchur<MatrixType>::initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im,
+                                                     Vector3s& firstHouseholderVector) {
   using std::abs;
-  Vector3s& v = firstHouseholderVector; // alias to save typing
+  Vector3s& v = firstHouseholderVector;  // alias to save typing
 
-  for (im = iu-2; im >= il; --im)
-  {
-    const Scalar Tmm = m_matT.coeff(im,im);
+  for (im = iu - 2; im >= il; --im) {
+    const Scalar Tmm = m_matT.coeff(im, im);
     const Scalar r = shiftInfo.coeff(0) - Tmm;
     const Scalar s = shiftInfo.coeff(1) - Tmm;
-    v.coeffRef(0) = (r * s - shiftInfo.coeff(2)) / m_matT.coeff(im+1,im) + m_matT.coeff(im,im+1);
-    v.coeffRef(1) = m_matT.coeff(im+1,im+1) - Tmm - r - s;
-    v.coeffRef(2) = m_matT.coeff(im+2,im+1);
+    v.coeffRef(0) = (r * s - shiftInfo.coeff(2)) / m_matT.coeff(im + 1, im) + m_matT.coeff(im, im + 1);
+    v.coeffRef(1) = m_matT.coeff(im + 1, im + 1) - Tmm - r - s;
+    v.coeffRef(2) = m_matT.coeff(im + 2, im + 1);
     if (im == il) {
       break;
     }
-    const Scalar lhs = m_matT.coeff(im,im-1) * (abs(v.coeff(1)) + abs(v.coeff(2)));
-    const Scalar rhs = v.coeff(0) * (abs(m_matT.coeff(im-1,im-1)) + abs(Tmm) + abs(m_matT.coeff(im+1,im+1)));
-    if (abs(lhs) < NumTraits<Scalar>::epsilon() * rhs)
-      break;
+    const Scalar lhs = m_matT.coeff(im, im - 1) * (abs(v.coeff(1)) + abs(v.coeff(2)));
+    const Scalar rhs = v.coeff(0) * (abs(m_matT.coeff(im - 1, im - 1)) + abs(Tmm) + abs(m_matT.coeff(im + 1, im + 1)));
+    if (abs(lhs) < NumTraits<Scalar>::epsilon() * rhs) break;
   }
 }
 
 /** \internal Perform a Francis QR step involving rows il:iu and columns im:iu. */
-template<typename MatrixType>
-inline void RealSchur<MatrixType>::performFrancisQRStep(Index il, Index im, Index iu, bool computeU, const Vector3s& firstHouseholderVector, Scalar* workspace)
-{
+template <typename MatrixType>
+inline void RealSchur<MatrixType>::performFrancisQRStep(Index il, Index im, Index iu, bool computeU,
+                                                        const Vector3s& firstHouseholderVector, Scalar* workspace) {
   eigen_assert(im >= il);
-  eigen_assert(im <= iu-2);
+  eigen_assert(im <= iu - 2);
 
   const Index size = m_matT.cols();
 
-  for (Index k = im; k <= iu-2; ++k)
-  {
+  for (Index k = im; k <= iu - 2; ++k) {
     bool firstIteration = (k == im);
 
     Vector3s v;
     if (firstIteration)
       v = firstHouseholderVector;
     else
-      v = m_matT.template block<3,1>(k,k-1);
+      v = m_matT.template block<3, 1>(k, k - 1);
 
     Scalar tau, beta;
     Matrix<Scalar, 2, 1> ess;
     v.makeHouseholder(ess, tau, beta);
-    
-    if (beta != Scalar(0)) // if v is not zero
+
+    if (!numext::is_exactly_zero(beta))  // if v is not zero
     {
       if (firstIteration && k > il)
-        m_matT.coeffRef(k,k-1) = -m_matT.coeff(k,k-1);
+        m_matT.coeffRef(k, k - 1) = -m_matT.coeff(k, k - 1);
       else if (!firstIteration)
-        m_matT.coeffRef(k,k-1) = beta;
+        m_matT.coeffRef(k, k - 1) = beta;
 
       // These Householder transformations form the O(n^3) part of the algorithm
-      m_matT.block(k, k, 3, size-k).applyHouseholderOnTheLeft(ess, tau, workspace);
-      m_matT.block(0, k, (std::min)(iu,k+3) + 1, 3).applyHouseholderOnTheRight(ess, tau, workspace);
-      if (computeU)
-        m_matU.block(0, k, size, 3).applyHouseholderOnTheRight(ess, tau, workspace);
+      m_matT.block(k, k, 3, size - k).applyHouseholderOnTheLeft(ess, tau, workspace);
+      m_matT.block(0, k, (std::min)(iu, k + 3) + 1, 3).applyHouseholderOnTheRight(ess, tau, workspace);
+      if (computeU) m_matU.block(0, k, size, 3).applyHouseholderOnTheRight(ess, tau, workspace);
     }
   }
 
-  Matrix<Scalar, 2, 1> v = m_matT.template block<2,1>(iu-1, iu-2);
+  Matrix<Scalar, 2, 1> v = m_matT.template block<2, 1>(iu - 1, iu - 2);
   Scalar tau, beta;
   Matrix<Scalar, 1, 1> ess;
   v.makeHouseholder(ess, tau, beta);
 
-  if (beta != Scalar(0)) // if v is not zero
+  if (!numext::is_exactly_zero(beta))  // if v is not zero
   {
-    m_matT.coeffRef(iu-1, iu-2) = beta;
-    m_matT.block(iu-1, iu-1, 2, size-iu+1).applyHouseholderOnTheLeft(ess, tau, workspace);
-    m_matT.block(0, iu-1, iu+1, 2).applyHouseholderOnTheRight(ess, tau, workspace);
-    if (computeU)
-      m_matU.block(0, iu-1, size, 2).applyHouseholderOnTheRight(ess, tau, workspace);
+    m_matT.coeffRef(iu - 1, iu - 2) = beta;
+    m_matT.block(iu - 1, iu - 1, 2, size - iu + 1).applyHouseholderOnTheLeft(ess, tau, workspace);
+    m_matT.block(0, iu - 1, iu + 1, 2).applyHouseholderOnTheRight(ess, tau, workspace);
+    if (computeU) m_matU.block(0, iu - 1, size, 2).applyHouseholderOnTheRight(ess, tau, workspace);
   }
 
   // clean up pollution due to round-off errors
-  for (Index i = im+2; i <= iu; ++i)
-  {
-    m_matT.coeffRef(i,i-2) = Scalar(0);
-    if (i > im+2)
-      m_matT.coeffRef(i,i-3) = Scalar(0);
+  for (Index i = im + 2; i <= iu; ++i) {
+    m_matT.coeffRef(i, i - 2) = Scalar(0);
+    if (i > im + 2) m_matT.coeffRef(i, i - 3) = Scalar(0);
   }
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_REAL_SCHUR_H
+#endif  // EIGEN_REAL_SCHUR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
index 1469236..9511e68 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
@@ -13,431 +13,415 @@
 
 #include "./Tridiagonalization.h"
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-template<typename _MatrixType>
+namespace Eigen {
+
+template <typename MatrixType_>
 class GeneralizedSelfAdjointEigenSolver;
 
 namespace internal {
-template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues;
+template <typename SolverType, int Size, bool IsComplex>
+struct direct_selfadjoint_eigenvalues;
 
-template<typename MatrixType, typename DiagType, typename SubDiagType>
-EIGEN_DEVICE_FUNC
-ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec);
-}
+template <typename MatrixType, typename DiagType, typename SubDiagType>
+EIGEN_DEVICE_FUNC ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag,
+                                                              const Index maxIterations, bool computeEigenvectors,
+                                                              MatrixType& eivec);
+}  // namespace internal
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class SelfAdjointEigenSolver
-  *
-  * \brief Computes eigenvalues and eigenvectors of selfadjoint matrices
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the
-  * eigendecomposition; this is expected to be an instantiation of the Matrix
-  * class template.
-  *
-  * A matrix \f$ A \f$ is selfadjoint if it equals its adjoint. For real
-  * matrices, this means that the matrix is symmetric: it equals its
-  * transpose. This class computes the eigenvalues and eigenvectors of a
-  * selfadjoint matrix. These are the scalars \f$ \lambda \f$ and vectors
-  * \f$ v \f$ such that \f$ Av = \lambda v \f$.  The eigenvalues of a
-  * selfadjoint matrix are always real. If \f$ D \f$ is a diagonal matrix with
-  * the eigenvalues on the diagonal, and \f$ V \f$ is a matrix with the
-  * eigenvectors as its columns, then \f$ A = V D V^{-1} \f$. This is called the
-  * eigendecomposition.
-  *
-  * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal
-  * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then
-  * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is
-  * equal to its transpose, \f$ V^{-1} = V^T \f$.
-  *
-  * The algorithm exploits the fact that the matrix is selfadjoint, making it
-  * faster and more accurate than the general purpose eigenvalue algorithms
-  * implemented in EigenSolver and ComplexEigenSolver.
-  *
-  * Only the \b lower \b triangular \b part of the input matrix is referenced.
-  *
-  * Call the function compute() to compute the eigenvalues and eigenvectors of
-  * a given matrix. Alternatively, you can use the
-  * SelfAdjointEigenSolver(const MatrixType&, int) constructor which computes
-  * the eigenvalues and eigenvectors at construction time. Once the eigenvalue
-  * and eigenvectors are computed, they can be retrieved with the eigenvalues()
-  * and eigenvectors() functions.
-  *
-  * The documentation for SelfAdjointEigenSolver(const MatrixType&, int)
-  * contains an example of the typical use of this class.
-  *
-  * To solve the \em generalized eigenvalue problem \f$ Av = \lambda Bv \f$ and
-  * the likes, see the class GeneralizedSelfAdjointEigenSolver.
-  *
-  * \sa MatrixBase::eigenvalues(), class EigenSolver, class ComplexEigenSolver
-  */
-template<typename _MatrixType> class SelfAdjointEigenSolver
-{
-  public:
+ *
+ *
+ * \class SelfAdjointEigenSolver
+ *
+ * \brief Computes eigenvalues and eigenvectors of selfadjoint matrices
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the
+ * eigendecomposition; this is expected to be an instantiation of the Matrix
+ * class template.
+ *
+ * A matrix \f$ A \f$ is selfadjoint if it equals its adjoint. For real
+ * matrices, this means that the matrix is symmetric: it equals its
+ * transpose. This class computes the eigenvalues and eigenvectors of a
+ * selfadjoint matrix. These are the scalars \f$ \lambda \f$ and vectors
+ * \f$ v \f$ such that \f$ Av = \lambda v \f$.  The eigenvalues of a
+ * selfadjoint matrix are always real. If \f$ D \f$ is a diagonal matrix with
+ * the eigenvalues on the diagonal, and \f$ V \f$ is a matrix with the
+ * eigenvectors as its columns, then \f$ A = V D V^{-1} \f$. This is called the
+ * eigendecomposition.
+ *
+ * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal
+ * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then
+ * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is
+ * equal to its transpose, \f$ V^{-1} = V^T \f$.
+ *
+ * The algorithm exploits the fact that the matrix is selfadjoint, making it
+ * faster and more accurate than the general purpose eigenvalue algorithms
+ * implemented in EigenSolver and ComplexEigenSolver.
+ *
+ * Only the \b lower \b triangular \b part of the input matrix is referenced.
+ *
+ * Call the function compute() to compute the eigenvalues and eigenvectors of
+ * a given matrix. Alternatively, you can use the
+ * SelfAdjointEigenSolver(const MatrixType&, int) constructor which computes
+ * the eigenvalues and eigenvectors at construction time. Once the eigenvalue
+ * and eigenvectors are computed, they can be retrieved with the eigenvalues()
+ * and eigenvectors() functions.
+ *
+ * The documentation for SelfAdjointEigenSolver(const MatrixType&, int)
+ * contains an example of the typical use of this class.
+ *
+ * To solve the \em generalized eigenvalue problem \f$ Av = \lambda Bv \f$ and
+ * the likes, see the class GeneralizedSelfAdjointEigenSolver.
+ *
+ * \sa MatrixBase::eigenvalues(), class EigenSolver, class ComplexEigenSolver
+ */
+template <typename MatrixType_>
+class SelfAdjointEigenSolver {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum {
+    Size = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
 
-    typedef _MatrixType MatrixType;
-    enum {
-      Size = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      Options = MatrixType::Options,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    
-    /** \brief Scalar type for matrices of type \p _MatrixType. */
-    typedef typename MatrixType::Scalar Scalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    
-    typedef Matrix<Scalar,Size,Size,ColMajor,MaxColsAtCompileTime,MaxColsAtCompileTime> EigenvectorsType;
+  /** \brief Scalar type for matrices of type \p MatrixType_. */
+  typedef typename MatrixType::Scalar Scalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    /** \brief Real scalar type for \p _MatrixType.
-      *
-      * This is just \c Scalar if #Scalar is real (e.g., \c float or
-      * \c double), and the type of the real part of \c Scalar if #Scalar is
-      * complex.
-      */
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    
-    friend struct internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>;
+  typedef Matrix<Scalar, Size, Size, ColMajor, MaxColsAtCompileTime, MaxColsAtCompileTime> EigenvectorsType;
 
-    /** \brief Type for vector of eigenvalues as returned by eigenvalues().
-      *
-      * This is a column vector with entries of type #RealScalar.
-      * The length of the vector is the size of \p _MatrixType.
-      */
-    typedef typename internal::plain_col_type<MatrixType, RealScalar>::type RealVectorType;
-    typedef Tridiagonalization<MatrixType> TridiagonalizationType;
-    typedef typename TridiagonalizationType::SubDiagonalType SubDiagonalType;
+  /** \brief Real scalar type for \p MatrixType_.
+   *
+   * This is just \c Scalar if #Scalar is real (e.g., \c float or
+   * \c double), and the type of the real part of \c Scalar if #Scalar is
+   * complex.
+   */
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    /** \brief Default constructor for fixed-size matrices.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via compute(). This constructor
-      * can only be used if \p _MatrixType is a fixed-size matrix; use
-      * SelfAdjointEigenSolver(Index) for dynamic-size matrices.
-      *
-      * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver.out
-      */
-    EIGEN_DEVICE_FUNC
-    SelfAdjointEigenSolver()
-        : m_eivec(),
-          m_eivalues(),
-          m_subdiag(),
-          m_hcoeffs(),
-          m_info(InvalidInput),
-          m_isInitialized(false),
-          m_eigenvectorsOk(false)
-    { }
+  friend struct internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver, Size, NumTraits<Scalar>::IsComplex>;
 
-    /** \brief Constructor, pre-allocates memory for dynamic-size matrices.
-      *
-      * \param [in]  size  Positive integer, size of the matrix whose
-      * eigenvalues and eigenvectors will be computed.
-      *
-      * This constructor is useful for dynamic-size matrices, when the user
-      * intends to perform decompositions via compute(). The \p size
-      * parameter is only used as a hint. It is not an error to give a wrong
-      * \p size, but it may impair performance.
-      *
-      * \sa compute() for an example
-      */
-    EIGEN_DEVICE_FUNC
-    explicit SelfAdjointEigenSolver(Index size)
-        : m_eivec(size, size),
-          m_eivalues(size),
-          m_subdiag(size > 1 ? size - 1 : 1),
-          m_hcoeffs(size > 1 ? size - 1 : 1),
-          m_isInitialized(false),
-          m_eigenvectorsOk(false)
-    {}
+  /** \brief Type for vector of eigenvalues as returned by eigenvalues().
+   *
+   * This is a column vector with entries of type #RealScalar.
+   * The length of the vector is the size of \p MatrixType_.
+   */
+  typedef typename internal::plain_col_type<MatrixType, Scalar>::type VectorType;
+  typedef typename internal::plain_col_type<MatrixType, RealScalar>::type RealVectorType;
+  typedef Tridiagonalization<MatrixType> TridiagonalizationType;
+  typedef typename TridiagonalizationType::SubDiagonalType SubDiagonalType;
 
-    /** \brief Constructor; computes eigendecomposition of given matrix.
-      *
-      * \param[in]  matrix  Selfadjoint matrix whose eigendecomposition is to
-      *    be computed. Only the lower triangular part of the matrix is referenced.
-      * \param[in]  options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
-      *
-      * This constructor calls compute(const MatrixType&, int) to compute the
-      * eigenvalues of the matrix \p matrix. The eigenvectors are computed if
-      * \p options equals #ComputeEigenvectors.
-      *
-      * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.out
-      *
-      * \sa compute(const MatrixType&, int)
-      */
-    template<typename InputType>
-    EIGEN_DEVICE_FUNC
-    explicit SelfAdjointEigenSolver(const EigenBase<InputType>& matrix, int options = ComputeEigenvectors)
+  /** \brief Default constructor for fixed-size matrices.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via compute(). This constructor
+   * can only be used if \p MatrixType_ is a fixed-size matrix; use
+   * SelfAdjointEigenSolver(Index) for dynamic-size matrices.
+   *
+   * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver.out
+   */
+  EIGEN_DEVICE_FUNC SelfAdjointEigenSolver()
+      : m_eivec(),
+        m_workspace(),
+        m_eivalues(),
+        m_subdiag(),
+        m_hcoeffs(),
+        m_info(InvalidInput),
+        m_isInitialized(false),
+        m_eigenvectorsOk(false) {}
+
+  /** \brief Constructor, pre-allocates memory for dynamic-size matrices.
+   *
+   * \param [in]  size  Positive integer, size of the matrix whose
+   * eigenvalues and eigenvectors will be computed.
+   *
+   * This constructor is useful for dynamic-size matrices, when the user
+   * intends to perform decompositions via compute(). The \p size
+   * parameter is only used as a hint. It is not an error to give a wrong
+   * \p size, but it may impair performance.
+   *
+   * \sa compute() for an example
+   */
+  EIGEN_DEVICE_FUNC explicit SelfAdjointEigenSolver(Index size)
+      : m_eivec(size, size),
+        m_workspace(size),
+        m_eivalues(size),
+        m_subdiag(size > 1 ? size - 1 : 1),
+        m_hcoeffs(size > 1 ? size - 1 : 1),
+        m_isInitialized(false),
+        m_eigenvectorsOk(false) {}
+
+  /** \brief Constructor; computes eigendecomposition of given matrix.
+   *
+   * \param[in]  matrix  Selfadjoint matrix whose eigendecomposition is to
+   *    be computed. Only the lower triangular part of the matrix is referenced.
+   * \param[in]  options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
+   *
+   * This constructor calls compute(const MatrixType&, int) to compute the
+   * eigenvalues of the matrix \p matrix. The eigenvectors are computed if
+   * \p options equals #ComputeEigenvectors.
+   *
+   * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.out
+   *
+   * \sa compute(const MatrixType&, int)
+   */
+  template <typename InputType>
+  EIGEN_DEVICE_FUNC explicit SelfAdjointEigenSolver(const EigenBase<InputType>& matrix,
+                                                    int options = ComputeEigenvectors)
       : m_eivec(matrix.rows(), matrix.cols()),
+        m_workspace(matrix.cols()),
         m_eivalues(matrix.cols()),
         m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
         m_hcoeffs(matrix.cols() > 1 ? matrix.cols() - 1 : 1),
         m_isInitialized(false),
-        m_eigenvectorsOk(false)
-    {
-      compute(matrix.derived(), options);
-    }
+        m_eigenvectorsOk(false) {
+    compute(matrix.derived(), options);
+  }
 
-    /** \brief Computes eigendecomposition of given matrix.
-      *
-      * \param[in]  matrix  Selfadjoint matrix whose eigendecomposition is to
-      *    be computed. Only the lower triangular part of the matrix is referenced.
-      * \param[in]  options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
-      * \returns    Reference to \c *this
-      *
-      * This function computes the eigenvalues of \p matrix.  The eigenvalues()
-      * function can be used to retrieve them.  If \p options equals #ComputeEigenvectors,
-      * then the eigenvectors are also computed and can be retrieved by
-      * calling eigenvectors().
-      *
-      * This implementation uses a symmetric QR algorithm. The matrix is first
-      * reduced to tridiagonal form using the Tridiagonalization class. The
-      * tridiagonal matrix is then brought to diagonal form with implicit
-      * symmetric QR steps with Wilkinson shift. Details can be found in
-      * Section 8.3 of Golub \& Van Loan, <i>%Matrix Computations</i>.
-      *
-      * The cost of the computation is about \f$ 9n^3 \f$ if the eigenvectors
-      * are required and \f$ 4n^3/3 \f$ if they are not required.
-      *
-      * This method reuses the memory in the SelfAdjointEigenSolver object that
-      * was allocated when the object was constructed, if the size of the
-      * matrix does not change.
-      *
-      * Example: \include SelfAdjointEigenSolver_compute_MatrixType.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_compute_MatrixType.out
-      *
-      * \sa SelfAdjointEigenSolver(const MatrixType&, int)
-      */
-    template<typename InputType>
-    EIGEN_DEVICE_FUNC
-    SelfAdjointEigenSolver& compute(const EigenBase<InputType>& matrix, int options = ComputeEigenvectors);
-    
-    /** \brief Computes eigendecomposition of given matrix using a closed-form algorithm
-      *
-      * This is a variant of compute(const MatrixType&, int options) which
-      * directly solves the underlying polynomial equation.
-      * 
-      * Currently only 2x2 and 3x3 matrices for which the sizes are known at compile time are supported (e.g., Matrix3d).
-      * 
-      * This method is usually significantly faster than the QR iterative algorithm
-      * but it might also be less accurate. It is also worth noting that
-      * for 3x3 matrices it involves trigonometric operations which are
-      * not necessarily available for all scalar types.
-      * 
-      * For the 3x3 case, we observed the following worst case relative error regarding the eigenvalues:
-      *   - double: 1e-8
-      *   - float:  1e-3
-      *
-      * \sa compute(const MatrixType&, int options)
-      */
-    EIGEN_DEVICE_FUNC
-    SelfAdjointEigenSolver& computeDirect(const MatrixType& matrix, int options = ComputeEigenvectors);
+  /** \brief Computes eigendecomposition of given matrix.
+   *
+   * \param[in]  matrix  Selfadjoint matrix whose eigendecomposition is to
+   *    be computed. Only the lower triangular part of the matrix is referenced.
+   * \param[in]  options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
+   * \returns    Reference to \c *this
+   *
+   * This function computes the eigenvalues of \p matrix.  The eigenvalues()
+   * function can be used to retrieve them.  If \p options equals #ComputeEigenvectors,
+   * then the eigenvectors are also computed and can be retrieved by
+   * calling eigenvectors().
+   *
+   * This implementation uses a symmetric QR algorithm. The matrix is first
+   * reduced to tridiagonal form using the Tridiagonalization class. The
+   * tridiagonal matrix is then brought to diagonal form with implicit
+   * symmetric QR steps with Wilkinson shift. Details can be found in
+   * Section 8.3 of Golub \& Van Loan, <i>%Matrix Computations</i>.
+   *
+   * The cost of the computation is about \f$ 9n^3 \f$ if the eigenvectors
+   * are required and \f$ 4n^3/3 \f$ if they are not required.
+   *
+   * This method reuses the memory in the SelfAdjointEigenSolver object that
+   * was allocated when the object was constructed, if the size of the
+   * matrix does not change.
+   *
+   * Example: \include SelfAdjointEigenSolver_compute_MatrixType.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_compute_MatrixType.out
+   *
+   * \sa SelfAdjointEigenSolver(const MatrixType&, int)
+   */
+  template <typename InputType>
+  EIGEN_DEVICE_FUNC SelfAdjointEigenSolver& compute(const EigenBase<InputType>& matrix,
+                                                    int options = ComputeEigenvectors);
 
-    /**
-      *\brief Computes the eigen decomposition from a tridiagonal symmetric matrix
-      *
-      * \param[in] diag The vector containing the diagonal of the matrix.
-      * \param[in] subdiag The subdiagonal of the matrix.
-      * \param[in] options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
-      * \returns Reference to \c *this
-      *
-      * This function assumes that the matrix has been reduced to tridiagonal form.
-      *
-      * \sa compute(const MatrixType&, int) for more information
-      */
-    SelfAdjointEigenSolver& computeFromTridiagonal(const RealVectorType& diag, const SubDiagonalType& subdiag , int options=ComputeEigenvectors);
+  /** \brief Computes eigendecomposition of given matrix using a closed-form algorithm
+   *
+   * This is a variant of compute(const MatrixType&, int options) which
+   * directly solves the underlying polynomial equation.
+   *
+   * Currently only 2x2 and 3x3 matrices for which the sizes are known at compile time are supported (e.g., Matrix3d).
+   *
+   * This method is usually significantly faster than the QR iterative algorithm
+   * but it might also be less accurate. It is also worth noting that
+   * for 3x3 matrices it involves trigonometric operations which are
+   * not necessarily available for all scalar types.
+   *
+   * For the 3x3 case, we observed the following worst case relative error regarding the eigenvalues:
+   *   - double: 1e-8
+   *   - float:  1e-3
+   *
+   * \sa compute(const MatrixType&, int options)
+   */
+  EIGEN_DEVICE_FUNC SelfAdjointEigenSolver& computeDirect(const MatrixType& matrix, int options = ComputeEigenvectors);
 
-    /** \brief Returns the eigenvectors of given matrix.
-      *
-      * \returns  A const reference to the matrix whose columns are the eigenvectors.
-      *
-      * \pre The eigenvectors have been computed before.
-      *
-      * Column \f$ k \f$ of the returned matrix is an eigenvector corresponding
-      * to eigenvalue number \f$ k \f$ as returned by eigenvalues().  The
-      * eigenvectors are normalized to have (Euclidean) norm equal to one. If
-      * this object was used to solve the eigenproblem for the selfadjoint
-      * matrix \f$ A \f$, then the matrix returned by this function is the
-      * matrix \f$ V \f$ in the eigendecomposition \f$ A = V D V^{-1} \f$.
-      *
-      * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal
-      * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then
-      * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is
-      * equal to its transpose, \f$ V^{-1} = V^T \f$.
-      *
-      * Example: \include SelfAdjointEigenSolver_eigenvectors.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_eigenvectors.out
-      *
-      * \sa eigenvalues()
-      */
-    EIGEN_DEVICE_FUNC
-    const EigenvectorsType& eigenvectors() const
-    {
-      eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
-      return m_eivec;
-    }
+  /**
+   *\brief Computes the eigen decomposition from a tridiagonal symmetric matrix
+   *
+   * \param[in] diag The vector containing the diagonal of the matrix.
+   * \param[in] subdiag The subdiagonal of the matrix.
+   * \param[in] options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
+   * \returns Reference to \c *this
+   *
+   * This function assumes that the matrix has been reduced to tridiagonal form.
+   *
+   * \sa compute(const MatrixType&, int) for more information
+   */
+  SelfAdjointEigenSolver& computeFromTridiagonal(const RealVectorType& diag, const SubDiagonalType& subdiag,
+                                                 int options = ComputeEigenvectors);
 
-    /** \brief Returns the eigenvalues of given matrix.
-      *
-      * \returns A const reference to the column vector containing the eigenvalues.
-      *
-      * \pre The eigenvalues have been computed before.
-      *
-      * The eigenvalues are repeated according to their algebraic multiplicity,
-      * so there are as many eigenvalues as rows in the matrix. The eigenvalues
-      * are sorted in increasing order.
-      *
-      * Example: \include SelfAdjointEigenSolver_eigenvalues.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_eigenvalues.out
-      *
-      * \sa eigenvectors(), MatrixBase::eigenvalues()
-      */
-    EIGEN_DEVICE_FUNC
-    const RealVectorType& eigenvalues() const
-    {
-      eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      return m_eivalues;
-    }
+  /** \brief Returns the eigenvectors of given matrix.
+   *
+   * \returns  A const reference to the matrix whose columns are the eigenvectors.
+   *
+   * \pre The eigenvectors have been computed before.
+   *
+   * Column \f$ k \f$ of the returned matrix is an eigenvector corresponding
+   * to eigenvalue number \f$ k \f$ as returned by eigenvalues().  The
+   * eigenvectors are normalized to have (Euclidean) norm equal to one. If
+   * this object was used to solve the eigenproblem for the selfadjoint
+   * matrix \f$ A \f$, then the matrix returned by this function is the
+   * matrix \f$ V \f$ in the eigendecomposition \f$ A = V D V^{-1} \f$.
+   *
+   * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal
+   * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then
+   * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is
+   * equal to its transpose, \f$ V^{-1} = V^T \f$.
+   *
+   * Example: \include SelfAdjointEigenSolver_eigenvectors.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_eigenvectors.out
+   *
+   * \sa eigenvalues()
+   */
+  EIGEN_DEVICE_FUNC const EigenvectorsType& eigenvectors() const {
+    eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
+    eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
+    return m_eivec;
+  }
 
-    /** \brief Computes the positive-definite square root of the matrix.
-      *
-      * \returns the positive-definite square root of the matrix
-      *
-      * \pre The eigenvalues and eigenvectors of a positive-definite matrix
-      * have been computed before.
-      *
-      * The square root of a positive-definite matrix \f$ A \f$ is the
-      * positive-definite matrix whose square equals \f$ A \f$. This function
-      * uses the eigendecomposition \f$ A = V D V^{-1} \f$ to compute the
-      * square root as \f$ A^{1/2} = V D^{1/2} V^{-1} \f$.
-      *
-      * Example: \include SelfAdjointEigenSolver_operatorSqrt.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_operatorSqrt.out
-      *
-      * \sa operatorInverseSqrt(), <a href="unsupported/group__MatrixFunctions__Module.html">MatrixFunctions Module</a>
-      */
-    EIGEN_DEVICE_FUNC
-    MatrixType operatorSqrt() const
-    {
-      eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
-      return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint();
-    }
+  /** \brief Returns the eigenvalues of given matrix.
+   *
+   * \returns A const reference to the column vector containing the eigenvalues.
+   *
+   * \pre The eigenvalues have been computed before.
+   *
+   * The eigenvalues are repeated according to their algebraic multiplicity,
+   * so there are as many eigenvalues as rows in the matrix. The eigenvalues
+   * are sorted in increasing order.
+   *
+   * Example: \include SelfAdjointEigenSolver_eigenvalues.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_eigenvalues.out
+   *
+   * \sa eigenvectors(), MatrixBase::eigenvalues()
+   */
+  EIGEN_DEVICE_FUNC const RealVectorType& eigenvalues() const {
+    eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
+    return m_eivalues;
+  }
 
-    /** \brief Computes the inverse square root of the matrix.
-      *
-      * \returns the inverse positive-definite square root of the matrix
-      *
-      * \pre The eigenvalues and eigenvectors of a positive-definite matrix
-      * have been computed before.
-      *
-      * This function uses the eigendecomposition \f$ A = V D V^{-1} \f$ to
-      * compute the inverse square root as \f$ V D^{-1/2} V^{-1} \f$. This is
-      * cheaper than first computing the square root with operatorSqrt() and
-      * then its inverse with MatrixBase::inverse().
-      *
-      * Example: \include SelfAdjointEigenSolver_operatorInverseSqrt.cpp
-      * Output: \verbinclude SelfAdjointEigenSolver_operatorInverseSqrt.out
-      *
-      * \sa operatorSqrt(), MatrixBase::inverse(), <a href="unsupported/group__MatrixFunctions__Module.html">MatrixFunctions Module</a>
-      */
-    EIGEN_DEVICE_FUNC
-    MatrixType operatorInverseSqrt() const
-    {
-      eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
-      return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint();
-    }
+  /** \brief Computes the positive-definite square root of the matrix.
+   *
+   * \returns the positive-definite square root of the matrix
+   *
+   * \pre The eigenvalues and eigenvectors of a positive-definite matrix
+   * have been computed before.
+   *
+   * The square root of a positive-definite matrix \f$ A \f$ is the
+   * positive-definite matrix whose square equals \f$ A \f$. This function
+   * uses the eigendecomposition \f$ A = V D V^{-1} \f$ to compute the
+   * square root as \f$ A^{1/2} = V D^{1/2} V^{-1} \f$.
+   *
+   * Example: \include SelfAdjointEigenSolver_operatorSqrt.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_operatorSqrt.out
+   *
+   * \sa operatorInverseSqrt(), <a href="unsupported/group__MatrixFunctions__Module.html">MatrixFunctions Module</a>
+   */
+  EIGEN_DEVICE_FUNC MatrixType operatorSqrt() const {
+    eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
+    eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
+    return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint();
+  }
 
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
-      */
-    EIGEN_DEVICE_FUNC
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      return m_info;
-    }
+  /** \brief Computes the inverse square root of the matrix.
+   *
+   * \returns the inverse positive-definite square root of the matrix
+   *
+   * \pre The eigenvalues and eigenvectors of a positive-definite matrix
+   * have been computed before.
+   *
+   * This function uses the eigendecomposition \f$ A = V D V^{-1} \f$ to
+   * compute the inverse square root as \f$ V D^{-1/2} V^{-1} \f$. This is
+   * cheaper than first computing the square root with operatorSqrt() and
+   * then its inverse with MatrixBase::inverse().
+   *
+   * Example: \include SelfAdjointEigenSolver_operatorInverseSqrt.cpp
+   * Output: \verbinclude SelfAdjointEigenSolver_operatorInverseSqrt.out
+   *
+   * \sa operatorSqrt(), MatrixBase::inverse(), <a
+   * href="unsupported/group__MatrixFunctions__Module.html">MatrixFunctions Module</a>
+   */
+  EIGEN_DEVICE_FUNC MatrixType operatorInverseSqrt() const {
+    eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
+    eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
+    return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint();
+  }
 
-    /** \brief Maximum number of iterations.
-      *
-      * The algorithm terminates if it does not converge within m_maxIterations * n iterations, where n
-      * denotes the size of the matrix. This value is currently set to 30 (copied from LAPACK).
-      */
-    static const int m_maxIterations = 30;
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful, \c NoConvergence otherwise.
+   */
+  EIGEN_DEVICE_FUNC ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
+    return m_info;
+  }
 
-  protected:
-    static EIGEN_DEVICE_FUNC
-    void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
-    
-    EigenvectorsType m_eivec;
-    RealVectorType m_eivalues;
-    typename TridiagonalizationType::SubDiagonalType m_subdiag;
-    typename TridiagonalizationType::CoeffVectorType m_hcoeffs;
-    ComputationInfo m_info;
-    bool m_isInitialized;
-    bool m_eigenvectorsOk;
+  /** \brief Maximum number of iterations.
+   *
+   * The algorithm terminates if it does not converge within m_maxIterations * n iterations, where n
+   * denotes the size of the matrix. This value is currently set to 30 (copied from LAPACK).
+   */
+  static const int m_maxIterations = 30;
+
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
+
+  EigenvectorsType m_eivec;
+  VectorType m_workspace;
+  RealVectorType m_eivalues;
+  typename TridiagonalizationType::SubDiagonalType m_subdiag;
+  typename TridiagonalizationType::CoeffVectorType m_hcoeffs;
+  ComputationInfo m_info;
+  bool m_isInitialized;
+  bool m_eigenvectorsOk;
 };
 
 namespace internal {
 /** \internal
-  *
-  * \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  * Performs a QR step on a tridiagonal symmetric matrix represented as a
-  * pair of two vectors \a diag and \a subdiag.
-  *
-  * \param diag the diagonal part of the input selfadjoint tridiagonal matrix
-  * \param subdiag the sub-diagonal part of the input selfadjoint tridiagonal matrix
-  * \param start starting index of the submatrix to work on
-  * \param end last+1 index of the submatrix to work on
-  * \param matrixQ pointer to the column-major matrix holding the eigenvectors, can be 0
-  * \param n size of the input matrix
-  *
-  * For compilation efficiency reasons, this procedure does not use eigen expression
-  * for its arguments.
-  *
-  * Implemented from Golub's "Matrix Computations", algorithm 8.3.2:
-  * "implicit symmetric QR step with Wilkinson shift"
-  */
-template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
-EIGEN_DEVICE_FUNC
-static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n);
-}
+ *
+ * \eigenvalues_module \ingroup Eigenvalues_Module
+ *
+ * Performs a QR step on a tridiagonal symmetric matrix represented as a
+ * pair of two vectors \a diag and \a subdiag.
+ *
+ * \param diag the diagonal part of the input selfadjoint tridiagonal matrix
+ * \param subdiag the sub-diagonal part of the input selfadjoint tridiagonal matrix
+ * \param start starting index of the submatrix to work on
+ * \param end last+1 index of the submatrix to work on
+ * \param matrixQ pointer to the column-major matrix holding the eigenvectors, can be 0
+ * \param n size of the input matrix
+ *
+ * For compilation efficiency reasons, this procedure does not use eigen expression
+ * for its arguments.
+ *
+ * Implemented from Golub's "Matrix Computations", algorithm 8.3.2:
+ * "implicit symmetric QR step with Wilkinson shift"
+ */
+template <int StorageOrder, typename RealScalar, typename Scalar, typename Index>
+EIGEN_DEVICE_FUNC static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end,
+                                                  Scalar* matrixQ, Index n);
+}  // namespace internal
 
-template<typename MatrixType>
-template<typename InputType>
-EIGEN_DEVICE_FUNC
-SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
-::compute(const EigenBase<InputType>& a_matrix, int options)
-{
-  check_template_parameters();
-  
-  const InputType &matrix(a_matrix.derived());
-  
+template <typename MatrixType>
+template <typename InputType>
+EIGEN_DEVICE_FUNC SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>::compute(
+    const EigenBase<InputType>& a_matrix, int options) {
+  const InputType& matrix(a_matrix.derived());
+
   EIGEN_USING_STD(abs);
   eigen_assert(matrix.cols() == matrix.rows());
-  eigen_assert((options&~(EigVecMask|GenEigMask))==0
-          && (options&EigVecMask)!=EigVecMask
-          && "invalid option parameter");
-  bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
+  eigen_assert((options & ~(EigVecMask | GenEigMask)) == 0 && (options & EigVecMask) != EigVecMask &&
+               "invalid option parameter");
+  bool computeEigenvectors = (options & ComputeEigenvectors) == ComputeEigenvectors;
   Index n = matrix.cols();
-  m_eivalues.resize(n,1);
+  m_eivalues.resize(n, 1);
 
-  if(n==1)
-  {
+  if (n == 1) {
     m_eivec = matrix;
-    m_eivalues.coeffRef(0,0) = numext::real(m_eivec.coeff(0,0));
-    if(computeEigenvectors)
-      m_eivec.setOnes(n,n);
+    m_eivalues.coeffRef(0, 0) = numext::real(m_eivec.coeff(0, 0));
+    if (computeEigenvectors) m_eivec.setOnes(n, n);
     m_info = Success;
     m_isInitialized = true;
     m_eigenvectorsOk = computeEigenvectors;
@@ -451,14 +435,14 @@
   // map the matrix coefficients to [-1:1] to avoid over- and underflow.
   mat = matrix.template triangularView<Lower>();
   RealScalar scale = mat.cwiseAbs().maxCoeff();
-  if(scale==RealScalar(0)) scale = RealScalar(1);
+  if (numext::is_exactly_zero(scale)) scale = RealScalar(1);
   mat.template triangularView<Lower>() /= scale;
-  m_subdiag.resize(n-1);
-  m_hcoeffs.resize(n-1);
-  internal::tridiagonalization_inplace(mat, diag, m_subdiag, m_hcoeffs, computeEigenvectors);
+  m_subdiag.resize(n - 1);
+  m_hcoeffs.resize(n - 1);
+  internal::tridiagonalization_inplace(mat, diag, m_subdiag, m_hcoeffs, m_workspace, computeEigenvectors);
 
   m_info = internal::computeFromTridiagonal_impl(diag, m_subdiag, m_maxIterations, computeEigenvectors, m_eivec);
-  
+
   // scale back the eigen values
   m_eivalues *= scale;
 
@@ -467,17 +451,15 @@
   return *this;
 }
 
-template<typename MatrixType>
-SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
-::computeFromTridiagonal(const RealVectorType& diag, const SubDiagonalType& subdiag , int options)
-{
-  //TODO : Add an option to scale the values beforehand
-  bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
+template <typename MatrixType>
+SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>::computeFromTridiagonal(
+    const RealVectorType& diag, const SubDiagonalType& subdiag, int options) {
+  // TODO : Add an option to scale the values beforehand
+  bool computeEigenvectors = (options & ComputeEigenvectors) == ComputeEigenvectors;
 
   m_eivalues = diag;
   m_subdiag = subdiag;
-  if (computeEigenvectors)
-  {
+  if (computeEigenvectors) {
     m_eivec.setIdentity(diag.size(), diag.size());
   }
   m_info = internal::computeFromTridiagonal_impl(m_eivalues, m_subdiag, m_maxIterations, computeEigenvectors, m_eivec);
@@ -489,63 +471,60 @@
 
 namespace internal {
 /**
-  * \internal
-  * \brief Compute the eigendecomposition from a tridiagonal matrix
-  *
-  * \param[in,out] diag : On input, the diagonal of the matrix, on output the eigenvalues
-  * \param[in,out] subdiag : The subdiagonal part of the matrix (entries are modified during the decomposition)
-  * \param[in] maxIterations : the maximum number of iterations
-  * \param[in] computeEigenvectors : whether the eigenvectors have to be computed or not
-  * \param[out] eivec : The matrix to store the eigenvectors if computeEigenvectors==true. Must be allocated on input.
-  * \returns \c Success or \c NoConvergence
-  */
-template<typename MatrixType, typename DiagType, typename SubDiagType>
-EIGEN_DEVICE_FUNC
-ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec)
-{
+ * \internal
+ * \brief Compute the eigendecomposition from a tridiagonal matrix
+ *
+ * \param[in,out] diag : On input, the diagonal of the matrix, on output the eigenvalues
+ * \param[in,out] subdiag : The subdiagonal part of the matrix (entries are modified during the decomposition)
+ * \param[in] maxIterations : the maximum number of iterations
+ * \param[in] computeEigenvectors : whether the eigenvectors have to be computed or not
+ * \param[out] eivec : The matrix to store the eigenvectors if computeEigenvectors==true. Must be allocated on input.
+ * \returns \c Success or \c NoConvergence
+ */
+template <typename MatrixType, typename DiagType, typename SubDiagType>
+EIGEN_DEVICE_FUNC ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag,
+                                                              const Index maxIterations, bool computeEigenvectors,
+                                                              MatrixType& eivec) {
   ComputationInfo info;
   typedef typename MatrixType::Scalar Scalar;
 
   Index n = diag.size();
-  Index end = n-1;
+  Index end = n - 1;
   Index start = 0;
-  Index iter = 0; // total number of iterations
-  
+  Index iter = 0;  // total number of iterations
+
   typedef typename DiagType::RealScalar RealScalar;
   const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
-  const RealScalar precision_inv = RealScalar(1)/NumTraits<RealScalar>::epsilon();
-  while (end>0)
-  {
-    for (Index i = start; i<end; ++i) {
+  const RealScalar precision_inv = RealScalar(1) / NumTraits<RealScalar>::epsilon();
+  while (end > 0) {
+    for (Index i = start; i < end; ++i) {
       if (numext::abs(subdiag[i]) < considerAsZero) {
         subdiag[i] = RealScalar(0);
       } else {
         // abs(subdiag[i]) <= epsilon * sqrt(abs(diag[i]) + abs(diag[i+1]))
         // Scaled to prevent underflows.
         const RealScalar scaled_subdiag = precision_inv * subdiag[i];
-        if (scaled_subdiag * scaled_subdiag <= (numext::abs(diag[i])+numext::abs(diag[i+1]))) {
+        if (scaled_subdiag * scaled_subdiag <= (numext::abs(diag[i]) + numext::abs(diag[i + 1]))) {
           subdiag[i] = RealScalar(0);
         }
       }
     }
 
     // find the largest unreduced block at the end of the matrix.
-    while (end>0 && subdiag[end-1]==RealScalar(0))
-    {
+    while (end > 0 && numext::is_exactly_zero(subdiag[end - 1])) {
       end--;
     }
-    if (end<=0)
-      break;
+    if (end <= 0) break;
 
     // if we spent too many iterations, we give up
     iter++;
-    if(iter > maxIterations * n) break;
+    if (iter > maxIterations * n) break;
 
     start = end - 1;
-    while (start>0 && subdiag[start-1]!=0)
-      start--;
+    while (start > 0 && !numext::is_exactly_zero(subdiag[start - 1])) start--;
 
-    internal::tridiagonal_qr_step<MatrixType::Flags&RowMajorBit ? RowMajor : ColMajor>(diag.data(), subdiag.data(), start, end, computeEigenvectors ? eivec.data() : (Scalar*)0, n);
+    internal::tridiagonal_qr_step<MatrixType::Flags & RowMajorBit ? RowMajor : ColMajor>(
+        diag.data(), subdiag.data(), start, end, computeEigenvectors ? eivec.data() : (Scalar*)0, n);
   }
   if (iter <= maxIterations * n)
     info = Success;
@@ -555,84 +534,78 @@
   // Sort eigenvalues and corresponding vectors.
   // TODO make the sort optional ?
   // TODO use a better sort algorithm !!
-  if (info == Success)
-  {
-    for (Index i = 0; i < n-1; ++i)
-    {
+  if (info == Success) {
+    for (Index i = 0; i < n - 1; ++i) {
       Index k;
-      diag.segment(i,n-i).minCoeff(&k);
-      if (k > 0)
-      {
-        numext::swap(diag[i], diag[k+i]);
-        if(computeEigenvectors)
-          eivec.col(i).swap(eivec.col(k+i));
+      diag.segment(i, n - i).minCoeff(&k);
+      if (k > 0) {
+        numext::swap(diag[i], diag[k + i]);
+        if (computeEigenvectors) eivec.col(i).swap(eivec.col(k + i));
       }
     }
   }
   return info;
 }
-  
-template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(SolverType& eig, const typename SolverType::MatrixType& A, int options)
-  { eig.compute(A,options); }
+
+template <typename SolverType, int Size, bool IsComplex>
+struct direct_selfadjoint_eigenvalues {
+  EIGEN_DEVICE_FUNC static inline void run(SolverType& eig, const typename SolverType::MatrixType& A, int options) {
+    eig.compute(A, options);
+  }
 };
 
-template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3,false>
-{
+template <typename SolverType>
+struct direct_selfadjoint_eigenvalues<SolverType, 3, false> {
   typedef typename SolverType::MatrixType MatrixType;
   typedef typename SolverType::RealVectorType VectorType;
   typedef typename SolverType::Scalar Scalar;
   typedef typename SolverType::EigenvectorsType EigenvectorsType;
-  
 
   /** \internal
    * Computes the roots of the characteristic polynomial of \a m.
    * For numerical stability m.trace() should be near zero and to avoid over- or underflow m should be normalized.
    */
-  EIGEN_DEVICE_FUNC
-  static inline void computeRoots(const MatrixType& m, VectorType& roots)
-  {
+  EIGEN_DEVICE_FUNC static inline void computeRoots(const MatrixType& m, VectorType& roots) {
     EIGEN_USING_STD(sqrt)
     EIGEN_USING_STD(atan2)
     EIGEN_USING_STD(cos)
     EIGEN_USING_STD(sin)
-    const Scalar s_inv3 = Scalar(1)/Scalar(3);
+    const Scalar s_inv3 = Scalar(1) / Scalar(3);
     const Scalar s_sqrt3 = sqrt(Scalar(3));
 
     // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
     // eigenvalues are the roots to this equation, all guaranteed to be
     // real-valued, because the matrix is symmetric.
-    Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(1,0)*m(2,0)*m(2,1) - m(0,0)*m(2,1)*m(2,1) - m(1,1)*m(2,0)*m(2,0) - m(2,2)*m(1,0)*m(1,0);
-    Scalar c1 = m(0,0)*m(1,1) - m(1,0)*m(1,0) + m(0,0)*m(2,2) - m(2,0)*m(2,0) + m(1,1)*m(2,2) - m(2,1)*m(2,1);
-    Scalar c2 = m(0,0) + m(1,1) + m(2,2);
+    Scalar c0 = m(0, 0) * m(1, 1) * m(2, 2) + Scalar(2) * m(1, 0) * m(2, 0) * m(2, 1) - m(0, 0) * m(2, 1) * m(2, 1) -
+                m(1, 1) * m(2, 0) * m(2, 0) - m(2, 2) * m(1, 0) * m(1, 0);
+    Scalar c1 = m(0, 0) * m(1, 1) - m(1, 0) * m(1, 0) + m(0, 0) * m(2, 2) - m(2, 0) * m(2, 0) + m(1, 1) * m(2, 2) -
+                m(2, 1) * m(2, 1);
+    Scalar c2 = m(0, 0) + m(1, 1) + m(2, 2);
 
     // Construct the parameters used in classifying the roots of the equation
     // and in solving the equation for the roots in closed form.
-    Scalar c2_over_3 = c2*s_inv3;
-    Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3;
+    Scalar c2_over_3 = c2 * s_inv3;
+    Scalar a_over_3 = (c2 * c2_over_3 - c1) * s_inv3;
     a_over_3 = numext::maxi(a_over_3, Scalar(0));
 
-    Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
+    Scalar half_b = Scalar(0.5) * (c0 + c2_over_3 * (Scalar(2) * c2_over_3 * c2_over_3 - c1));
 
-    Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b;
+    Scalar q = a_over_3 * a_over_3 * a_over_3 - half_b * half_b;
     q = numext::maxi(q, Scalar(0));
 
     // Compute the eigenvalues by solving for the roots of the polynomial.
     Scalar rho = sqrt(a_over_3);
-    Scalar theta = atan2(sqrt(q),half_b)*s_inv3;  // since sqrt(q) > 0, atan2 is in [0, pi] and theta is in [0, pi/3]
+    Scalar theta = atan2(sqrt(q), half_b) * s_inv3;  // since sqrt(q) > 0, atan2 is in [0, pi] and theta is in [0, pi/3]
     Scalar cos_theta = cos(theta);
     Scalar sin_theta = sin(theta);
     // roots are already sorted, since cos is monotonically decreasing on [0, pi]
-    roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3)
-    roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); // == 2*rho*cos(theta+ pi/3)
-    roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta;
+    roots(0) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);  // == 2*rho*cos(theta+2pi/3)
+    roots(1) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);  // == 2*rho*cos(theta+ pi/3)
+    roots(2) = c2_over_3 + Scalar(2) * rho * cos_theta;
   }
 
-  EIGEN_DEVICE_FUNC
-  static inline bool extract_kernel(MatrixType& mat, Ref<VectorType> res, Ref<VectorType> representative)
-  {
+  EIGEN_DEVICE_FUNC static inline bool extract_kernel(MatrixType& mat, Ref<VectorType> res,
+                                                      Ref<VectorType> representative) {
     EIGEN_USING_STD(abs);
     EIGEN_USING_STD(sqrt);
     Index i0;
@@ -643,47 +616,43 @@
     representative = mat.col(i0);
     Scalar n0, n1;
     VectorType c0, c1;
-    n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm();
-    n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm();
-    if(n0>n1) res = c0/sqrt(n0);
-    else      res = c1/sqrt(n1);
+    n0 = (c0 = representative.cross(mat.col((i0 + 1) % 3))).squaredNorm();
+    n1 = (c1 = representative.cross(mat.col((i0 + 2) % 3))).squaredNorm();
+    if (n0 > n1)
+      res = c0 / sqrt(n0);
+    else
+      res = c1 / sqrt(n1);
 
     return true;
   }
 
-  EIGEN_DEVICE_FUNC
-  static inline void run(SolverType& solver, const MatrixType& mat, int options)
-  {
+  EIGEN_DEVICE_FUNC static inline void run(SolverType& solver, const MatrixType& mat, int options) {
     eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows());
-    eigen_assert((options&~(EigVecMask|GenEigMask))==0
-            && (options&EigVecMask)!=EigVecMask
-            && "invalid option parameter");
-    bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
-    
+    eigen_assert((options & ~(EigVecMask | GenEigMask)) == 0 && (options & EigVecMask) != EigVecMask &&
+                 "invalid option parameter");
+    bool computeEigenvectors = (options & ComputeEigenvectors) == ComputeEigenvectors;
+
     EigenvectorsType& eivecs = solver.m_eivec;
     VectorType& eivals = solver.m_eivalues;
-  
+
     // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow.
     Scalar shift = mat.trace() / Scalar(3);
-    // TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for computing the eigenvectors later
+    // TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for
+    // computing the eigenvectors later
     MatrixType scaledMat = mat.template selfadjointView<Lower>();
     scaledMat.diagonal().array() -= shift;
     Scalar scale = scaledMat.cwiseAbs().maxCoeff();
-    if(scale > 0) scaledMat /= scale;   // TODO for scale==0 we could save the remaining operations
+    if (scale > 0) scaledMat /= scale;  // TODO for scale==0 we could save the remaining operations
 
     // compute the eigenvalues
-    computeRoots(scaledMat,eivals);
+    computeRoots(scaledMat, eivals);
 
     // compute the eigenvectors
-    if(computeEigenvectors)
-    {
-      if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon())
-      {
+    if (computeEigenvectors) {
+      if ((eivals(2) - eivals(0)) <= Eigen::NumTraits<Scalar>::epsilon()) {
         // All three eigenvalues are numerically the same
         eivecs.setIdentity();
-      }
-      else
-      {
+      } else {
         MatrixType tmp;
         tmp = scaledMat;
 
@@ -691,31 +660,27 @@
         Scalar d0 = eivals(2) - eivals(1);
         Scalar d1 = eivals(1) - eivals(0);
         Index k(0), l(2);
-        if(d0 > d1)
-        {
-          numext::swap(k,l);
+        if (d0 > d1) {
+          numext::swap(k, l);
           d0 = d1;
         }
 
         // Compute the eigenvector of index k
         {
-          tmp.diagonal().array () -= eivals(k);
+          tmp.diagonal().array() -= eivals(k);
           // By construction, 'tmp' is of rank 2, and its kernel corresponds to the respective eigenvector.
           extract_kernel(tmp, eivecs.col(k), eivecs.col(l));
         }
 
         // Compute eigenvector of index l
-        if(d0<=2*Eigen::NumTraits<Scalar>::epsilon()*d1)
-        {
+        if (d0 <= 2 * Eigen::NumTraits<Scalar>::epsilon() * d1) {
           // If d0 is too small, then the two other eigenvalues are numerically the same,
           // and thus we only have to ortho-normalize the near orthogonal vector we saved above.
-          eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l))*eivecs.col(l);
+          eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l)) * eivecs.col(l);
           eivecs.col(l).normalize();
-        }
-        else
-        {
+        } else {
           tmp = scaledMat;
-          tmp.diagonal().array () -= eivals(l);
+          tmp.diagonal().array() -= eivals(l);
 
           VectorType dummy;
           extract_kernel(tmp, eivecs.col(l), dummy);
@@ -729,7 +694,7 @@
     // Rescale back to the original size.
     eivals *= scale;
     eivals.array() += shift;
-    
+
     solver.m_info = Success;
     solver.m_isInitialized = true;
     solver.m_eigenvectorsOk = computeEigenvectors;
@@ -737,73 +702,59 @@
 };
 
 // 2x2 direct eigenvalues decomposition, code from Hauke Heibel
-template<typename SolverType> 
-struct direct_selfadjoint_eigenvalues<SolverType,2,false>
-{
+template <typename SolverType>
+struct direct_selfadjoint_eigenvalues<SolverType, 2, false> {
   typedef typename SolverType::MatrixType MatrixType;
   typedef typename SolverType::RealVectorType VectorType;
   typedef typename SolverType::Scalar Scalar;
   typedef typename SolverType::EigenvectorsType EigenvectorsType;
-  
-  EIGEN_DEVICE_FUNC
-  static inline void computeRoots(const MatrixType& m, VectorType& roots)
-  {
+
+  EIGEN_DEVICE_FUNC static inline void computeRoots(const MatrixType& m, VectorType& roots) {
     EIGEN_USING_STD(sqrt);
-    const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*numext::abs2(m(1,0)));
-    const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1));
+    const Scalar t0 = Scalar(0.5) * sqrt(numext::abs2(m(0, 0) - m(1, 1)) + Scalar(4) * numext::abs2(m(1, 0)));
+    const Scalar t1 = Scalar(0.5) * (m(0, 0) + m(1, 1));
     roots(0) = t1 - t0;
     roots(1) = t1 + t0;
   }
-  
-  EIGEN_DEVICE_FUNC
-  static inline void run(SolverType& solver, const MatrixType& mat, int options)
-  {
+
+  EIGEN_DEVICE_FUNC static inline void run(SolverType& solver, const MatrixType& mat, int options) {
     EIGEN_USING_STD(sqrt);
     EIGEN_USING_STD(abs);
-    
+
     eigen_assert(mat.cols() == 2 && mat.cols() == mat.rows());
-    eigen_assert((options&~(EigVecMask|GenEigMask))==0
-            && (options&EigVecMask)!=EigVecMask
-            && "invalid option parameter");
-    bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
-    
+    eigen_assert((options & ~(EigVecMask | GenEigMask)) == 0 && (options & EigVecMask) != EigVecMask &&
+                 "invalid option parameter");
+    bool computeEigenvectors = (options & ComputeEigenvectors) == ComputeEigenvectors;
+
     EigenvectorsType& eivecs = solver.m_eivec;
     VectorType& eivals = solver.m_eivalues;
-  
+
     // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow.
     Scalar shift = mat.trace() / Scalar(2);
     MatrixType scaledMat = mat;
-    scaledMat.coeffRef(0,1) = mat.coeff(1,0);
+    scaledMat.coeffRef(0, 1) = mat.coeff(1, 0);
     scaledMat.diagonal().array() -= shift;
     Scalar scale = scaledMat.cwiseAbs().maxCoeff();
-    if(scale > Scalar(0))
-      scaledMat /= scale;
+    if (scale > Scalar(0)) scaledMat /= scale;
 
     // Compute the eigenvalues
-    computeRoots(scaledMat,eivals);
+    computeRoots(scaledMat, eivals);
 
     // compute the eigen vectors
-    if(computeEigenvectors)
-    {
-      if((eivals(1)-eivals(0))<=abs(eivals(1))*Eigen::NumTraits<Scalar>::epsilon())
-      {
+    if (computeEigenvectors) {
+      if ((eivals(1) - eivals(0)) <= abs(eivals(1)) * Eigen::NumTraits<Scalar>::epsilon()) {
         eivecs.setIdentity();
-      }
-      else
-      {
-        scaledMat.diagonal().array () -= eivals(1);
-        Scalar a2 = numext::abs2(scaledMat(0,0));
-        Scalar c2 = numext::abs2(scaledMat(1,1));
-        Scalar b2 = numext::abs2(scaledMat(1,0));
-        if(a2>c2)
-        {
-          eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0);
-          eivecs.col(1) /= sqrt(a2+b2);
-        }
-        else
-        {
-          eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0);
-          eivecs.col(1) /= sqrt(c2+b2);
+      } else {
+        scaledMat.diagonal().array() -= eivals(1);
+        Scalar a2 = numext::abs2(scaledMat(0, 0));
+        Scalar c2 = numext::abs2(scaledMat(1, 1));
+        Scalar b2 = numext::abs2(scaledMat(1, 0));
+        if (a2 > c2) {
+          eivecs.col(1) << -scaledMat(1, 0), scaledMat(0, 0);
+          eivecs.col(1) /= sqrt(a2 + b2);
+        } else {
+          eivecs.col(1) << -scaledMat(1, 1), scaledMat(1, 0);
+          eivecs.col(1) /= sqrt(c2 + b2);
         }
 
         eivecs.col(0) << eivecs.col(1).unitOrthogonal();
@@ -820,42 +771,40 @@
   }
 };
 
-}
+}  // namespace internal
 
-template<typename MatrixType>
-EIGEN_DEVICE_FUNC
-SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
-::computeDirect(const MatrixType& matrix, int options)
-{
-  internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>::run(*this,matrix,options);
+template <typename MatrixType>
+EIGEN_DEVICE_FUNC SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>::computeDirect(
+    const MatrixType& matrix, int options) {
+  internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver, Size, NumTraits<Scalar>::IsComplex>::run(
+      *this, matrix, options);
   return *this;
 }
 
 namespace internal {
 
 // Francis implicit QR step.
-template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
-EIGEN_DEVICE_FUNC
-static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n)
-{
+template <int StorageOrder, typename RealScalar, typename Scalar, typename Index>
+EIGEN_DEVICE_FUNC static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end,
+                                                  Scalar* matrixQ, Index n) {
   // Wilkinson Shift.
-  RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
-  RealScalar e = subdiag[end-1];
+  RealScalar td = (diag[end - 1] - diag[end]) * RealScalar(0.5);
+  RealScalar e = subdiag[end - 1];
   // Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still
   // underflow thus leading to inf/NaN values when using the following commented code:
   //   RealScalar e2 = numext::abs2(subdiag[end-1]);
   //   RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
   // This explain the following, somewhat more complicated, version:
   RealScalar mu = diag[end];
-  if(td==RealScalar(0)) {
+  if (numext::is_exactly_zero(td)) {
     mu -= numext::abs(e);
-  } else if (e != RealScalar(0)) {
+  } else if (!numext::is_exactly_zero(e)) {
     const RealScalar e2 = numext::abs2(e);
-    const RealScalar h = numext::hypot(td,e);
-    if(e2 == RealScalar(0)) {
-      mu -= e / ((td + (td>RealScalar(0) ? h : -h)) / e);
+    const RealScalar h = numext::hypot(td, e);
+    if (numext::is_exactly_zero(e2)) {
+      mu -= e / ((td + (td > RealScalar(0) ? h : -h)) / e);
     } else {
-      mu -= e2 / (td + (td>RealScalar(0) ? h : -h)); 
+      mu -= e2 / (td + (td > RealScalar(0) ? h : -h));
     }
   }
 
@@ -863,42 +812,39 @@
   RealScalar z = subdiag[start];
   // If z ever becomes zero, the Givens rotation will be the identity and
   // z will stay zero for all future iterations.
-  for (Index k = start; k < end && z != RealScalar(0); ++k)
-  {
+  for (Index k = start; k < end && !numext::is_exactly_zero(z); ++k) {
     JacobiRotation<RealScalar> rot;
     rot.makeGivens(x, z);
 
     // do T = G' T G
     RealScalar sdk = rot.s() * diag[k] + rot.c() * subdiag[k];
-    RealScalar dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k+1];
+    RealScalar dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k + 1];
 
-    diag[k] = rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k]) - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k+1]);
-    diag[k+1] = rot.s() * sdk + rot.c() * dkp1;
+    diag[k] =
+        rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k]) - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k + 1]);
+    diag[k + 1] = rot.s() * sdk + rot.c() * dkp1;
     subdiag[k] = rot.c() * sdk - rot.s() * dkp1;
-    
-    if (k > start)
-      subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z;
+
+    if (k > start) subdiag[k - 1] = rot.c() * subdiag[k - 1] - rot.s() * z;
 
     // "Chasing the bulge" to return to triangular form.
     x = subdiag[k];
-    if (k < end - 1)
-    {
-      z = -rot.s() * subdiag[k+1];
-      subdiag[k + 1] = rot.c() * subdiag[k+1];
+    if (k < end - 1) {
+      z = -rot.s() * subdiag[k + 1];
+      subdiag[k + 1] = rot.c() * subdiag[k + 1];
     }
-    
+
     // apply the givens rotation to the unit matrix Q = Q * G
-    if (matrixQ)
-    {
+    if (matrixQ) {
       // FIXME if StorageOrder == RowMajor this operation is not very efficient
-      Map<Matrix<Scalar,Dynamic,Dynamic,StorageOrder> > q(matrixQ,n,n);
-      q.applyOnTheRight(k,k+1,rot);
+      Map<Matrix<Scalar, Dynamic, Dynamic, StorageOrder> > q(matrixQ, n, n);
+      q.applyOnTheRight(k, k + 1, rot);
     }
   }
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SELFADJOINTEIGENSOLVER_H
+#endif  // EIGEN_SELFADJOINTEIGENSOLVER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h
index 674c92a..76158e9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h
@@ -11,310 +11,294 @@
 #ifndef EIGEN_TRIDIAGONALIZATION_H
 #define EIGEN_TRIDIAGONALIZATION_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename MatrixType> struct TridiagonalizationMatrixTReturnType;
-template<typename MatrixType>
-struct traits<TridiagonalizationMatrixTReturnType<MatrixType> >
-  : public traits<typename MatrixType::PlainObject>
-{
-  typedef typename MatrixType::PlainObject ReturnType; // FIXME shall it be a BandMatrix?
+template <typename MatrixType>
+struct TridiagonalizationMatrixTReturnType;
+template <typename MatrixType>
+struct traits<TridiagonalizationMatrixTReturnType<MatrixType>> : public traits<typename MatrixType::PlainObject> {
+  typedef typename MatrixType::PlainObject ReturnType;  // FIXME shall it be a BandMatrix?
   enum { Flags = 0 };
 };
 
-template<typename MatrixType, typename CoeffVectorType>
-EIGEN_DEVICE_FUNC
-void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs);
-}
+template <typename MatrixType, typename CoeffVectorType>
+EIGEN_DEVICE_FUNC void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs);
+}  // namespace internal
 
 /** \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  *
-  * \class Tridiagonalization
-  *
-  * \brief Tridiagonal decomposition of a selfadjoint matrix
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the
-  * tridiagonal decomposition; this is expected to be an instantiation of the
-  * Matrix class template.
-  *
-  * This class performs a tridiagonal decomposition of a selfadjoint matrix \f$ A \f$ such that:
-  * \f$ A = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real symmetric tridiagonal matrix.
-  *
-  * A tridiagonal matrix is a matrix which has nonzero elements only on the
-  * main diagonal and the first diagonal below and above it. The Hessenberg
-  * decomposition of a selfadjoint matrix is in fact a tridiagonal
-  * decomposition. This class is used in SelfAdjointEigenSolver to compute the
-  * eigenvalues and eigenvectors of a selfadjoint matrix.
-  *
-  * Call the function compute() to compute the tridiagonal decomposition of a
-  * given matrix. Alternatively, you can use the Tridiagonalization(const MatrixType&)
-  * constructor which computes the tridiagonal Schur decomposition at
-  * construction time. Once the decomposition is computed, you can use the
-  * matrixQ() and matrixT() functions to retrieve the matrices Q and T in the
-  * decomposition.
-  *
-  * The documentation of Tridiagonalization(const MatrixType&) contains an
-  * example of the typical use of this class.
-  *
-  * \sa class HessenbergDecomposition, class SelfAdjointEigenSolver
-  */
-template<typename _MatrixType> class Tridiagonalization
-{
-  public:
+ *
+ *
+ * \class Tridiagonalization
+ *
+ * \brief Tridiagonal decomposition of a selfadjoint matrix
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the
+ * tridiagonal decomposition; this is expected to be an instantiation of the
+ * Matrix class template.
+ *
+ * This class performs a tridiagonal decomposition of a selfadjoint matrix \f$ A \f$ such that:
+ * \f$ A = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real symmetric tridiagonal matrix.
+ *
+ * A tridiagonal matrix is a matrix which has nonzero elements only on the
+ * main diagonal and the first diagonal below and above it. The Hessenberg
+ * decomposition of a selfadjoint matrix is in fact a tridiagonal
+ * decomposition. This class is used in SelfAdjointEigenSolver to compute the
+ * eigenvalues and eigenvectors of a selfadjoint matrix.
+ *
+ * Call the function compute() to compute the tridiagonal decomposition of a
+ * given matrix. Alternatively, you can use the Tridiagonalization(const MatrixType&)
+ * constructor which computes the tridiagonal Schur decomposition at
+ * construction time. Once the decomposition is computed, you can use the
+ * matrixQ() and matrixT() functions to retrieve the matrices Q and T in the
+ * decomposition.
+ *
+ * The documentation of Tridiagonalization(const MatrixType&) contains an
+ * example of the typical use of this class.
+ *
+ * \sa class HessenbergDecomposition, class SelfAdjointEigenSolver
+ */
+template <typename MatrixType_>
+class Tridiagonalization {
+ public:
+  /** \brief Synonym for the template parameter \p MatrixType_. */
+  typedef MatrixType_ MatrixType;
 
-    /** \brief Synonym for the template parameter \p _MatrixType. */
-    typedef _MatrixType MatrixType;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
 
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  enum {
+    Size = MatrixType::RowsAtCompileTime,
+    SizeMinusOne = Size == Dynamic ? Dynamic : (Size > 1 ? Size - 1 : 1),
+    Options = MatrixType::Options,
+    MaxSize = MatrixType::MaxRowsAtCompileTime,
+    MaxSizeMinusOne = MaxSize == Dynamic ? Dynamic : (MaxSize > 1 ? MaxSize - 1 : 1)
+  };
 
-    enum {
-      Size = MatrixType::RowsAtCompileTime,
-      SizeMinusOne = Size == Dynamic ? Dynamic : (Size > 1 ? Size - 1 : 1),
-      Options = MatrixType::Options,
-      MaxSize = MatrixType::MaxRowsAtCompileTime,
-      MaxSizeMinusOne = MaxSize == Dynamic ? Dynamic : (MaxSize > 1 ? MaxSize - 1 : 1)
-    };
+  typedef Matrix<Scalar, SizeMinusOne, 1, Options & ~RowMajor, MaxSizeMinusOne, 1> CoeffVectorType;
+  typedef typename internal::plain_col_type<MatrixType, RealScalar>::type DiagonalType;
+  typedef Matrix<RealScalar, SizeMinusOne, 1, Options & ~RowMajor, MaxSizeMinusOne, 1> SubDiagonalType;
+  typedef internal::remove_all_t<typename MatrixType::RealReturnType> MatrixTypeRealView;
+  typedef internal::TridiagonalizationMatrixTReturnType<MatrixTypeRealView> MatrixTReturnType;
 
-    typedef Matrix<Scalar, SizeMinusOne, 1, Options & ~RowMajor, MaxSizeMinusOne, 1> CoeffVectorType;
-    typedef typename internal::plain_col_type<MatrixType, RealScalar>::type DiagonalType;
-    typedef Matrix<RealScalar, SizeMinusOne, 1, Options & ~RowMajor, MaxSizeMinusOne, 1> SubDiagonalType;
-    typedef typename internal::remove_all<typename MatrixType::RealReturnType>::type MatrixTypeRealView;
-    typedef internal::TridiagonalizationMatrixTReturnType<MatrixTypeRealView> MatrixTReturnType;
+  typedef std::conditional_t<NumTraits<Scalar>::IsComplex,
+                             internal::add_const_on_value_type_t<typename Diagonal<const MatrixType>::RealReturnType>,
+                             const Diagonal<const MatrixType>>
+      DiagonalReturnType;
 
-    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-              typename internal::add_const_on_value_type<typename Diagonal<const MatrixType>::RealReturnType>::type,
-              const Diagonal<const MatrixType>
-            >::type DiagonalReturnType;
+  typedef std::conditional_t<
+      NumTraits<Scalar>::IsComplex,
+      internal::add_const_on_value_type_t<typename Diagonal<const MatrixType, -1>::RealReturnType>,
+      const Diagonal<const MatrixType, -1>>
+      SubDiagonalReturnType;
 
-    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-              typename internal::add_const_on_value_type<typename Diagonal<const MatrixType, -1>::RealReturnType>::type,
-              const Diagonal<const MatrixType, -1>
-            >::type SubDiagonalReturnType;
+  /** \brief Return type of matrixQ() */
+  typedef HouseholderSequence<MatrixType, internal::remove_all_t<typename CoeffVectorType::ConjugateReturnType>>
+      HouseholderSequenceType;
 
-    /** \brief Return type of matrixQ() */
-    typedef HouseholderSequence<MatrixType,typename internal::remove_all<typename CoeffVectorType::ConjugateReturnType>::type> HouseholderSequenceType;
+  /** \brief Default constructor.
+   *
+   * \param [in]  size  Positive integer, size of the matrix whose tridiagonal
+   * decomposition will be computed.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via compute().  The \p size parameter is only
+   * used as a hint. It is not an error to give a wrong \p size, but it may
+   * impair performance.
+   *
+   * \sa compute() for an example.
+   */
+  explicit Tridiagonalization(Index size = Size == Dynamic ? 2 : Size)
+      : m_matrix(size, size), m_hCoeffs(size > 1 ? size - 1 : 1), m_isInitialized(false) {}
 
-    /** \brief Default constructor.
-      *
-      * \param [in]  size  Positive integer, size of the matrix whose tridiagonal
-      * decomposition will be computed.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via compute().  The \p size parameter is only
-      * used as a hint. It is not an error to give a wrong \p size, but it may
-      * impair performance.
-      *
-      * \sa compute() for an example.
-      */
-    explicit Tridiagonalization(Index size = Size==Dynamic ? 2 : Size)
-      : m_matrix(size,size),
-        m_hCoeffs(size > 1 ? size-1 : 1),
-        m_isInitialized(false)
-    {}
+  /** \brief Constructor; computes tridiagonal decomposition of given matrix.
+   *
+   * \param[in]  matrix  Selfadjoint matrix whose tridiagonal decomposition
+   * is to be computed.
+   *
+   * This constructor calls compute() to compute the tridiagonal decomposition.
+   *
+   * Example: \include Tridiagonalization_Tridiagonalization_MatrixType.cpp
+   * Output: \verbinclude Tridiagonalization_Tridiagonalization_MatrixType.out
+   */
+  template <typename InputType>
+  explicit Tridiagonalization(const EigenBase<InputType>& matrix)
+      : m_matrix(matrix.derived()), m_hCoeffs(matrix.cols() > 1 ? matrix.cols() - 1 : 1), m_isInitialized(false) {
+    internal::tridiagonalization_inplace(m_matrix, m_hCoeffs);
+    m_isInitialized = true;
+  }
 
-    /** \brief Constructor; computes tridiagonal decomposition of given matrix.
-      *
-      * \param[in]  matrix  Selfadjoint matrix whose tridiagonal decomposition
-      * is to be computed.
-      *
-      * This constructor calls compute() to compute the tridiagonal decomposition.
-      *
-      * Example: \include Tridiagonalization_Tridiagonalization_MatrixType.cpp
-      * Output: \verbinclude Tridiagonalization_Tridiagonalization_MatrixType.out
-      */
-    template<typename InputType>
-    explicit Tridiagonalization(const EigenBase<InputType>& matrix)
-      : m_matrix(matrix.derived()),
-        m_hCoeffs(matrix.cols() > 1 ? matrix.cols()-1 : 1),
-        m_isInitialized(false)
-    {
-      internal::tridiagonalization_inplace(m_matrix, m_hCoeffs);
-      m_isInitialized = true;
-    }
+  /** \brief Computes tridiagonal decomposition of given matrix.
+   *
+   * \param[in]  matrix  Selfadjoint matrix whose tridiagonal decomposition
+   * is to be computed.
+   * \returns    Reference to \c *this
+   *
+   * The tridiagonal decomposition is computed by bringing the columns of
+   * the matrix successively in the required form using Householder
+   * reflections. The cost is \f$ 4n^3/3 \f$ flops, where \f$ n \f$ denotes
+   * the size of the given matrix.
+   *
+   * This method reuses of the allocated data in the Tridiagonalization
+   * object, if the size of the matrix does not change.
+   *
+   * Example: \include Tridiagonalization_compute.cpp
+   * Output: \verbinclude Tridiagonalization_compute.out
+   */
+  template <typename InputType>
+  Tridiagonalization& compute(const EigenBase<InputType>& matrix) {
+    m_matrix = matrix.derived();
+    m_hCoeffs.resize(matrix.rows() - 1, 1);
+    internal::tridiagonalization_inplace(m_matrix, m_hCoeffs);
+    m_isInitialized = true;
+    return *this;
+  }
 
-    /** \brief Computes tridiagonal decomposition of given matrix.
-      *
-      * \param[in]  matrix  Selfadjoint matrix whose tridiagonal decomposition
-      * is to be computed.
-      * \returns    Reference to \c *this
-      *
-      * The tridiagonal decomposition is computed by bringing the columns of
-      * the matrix successively in the required form using Householder
-      * reflections. The cost is \f$ 4n^3/3 \f$ flops, where \f$ n \f$ denotes
-      * the size of the given matrix.
-      *
-      * This method reuses of the allocated data in the Tridiagonalization
-      * object, if the size of the matrix does not change.
-      *
-      * Example: \include Tridiagonalization_compute.cpp
-      * Output: \verbinclude Tridiagonalization_compute.out
-      */
-    template<typename InputType>
-    Tridiagonalization& compute(const EigenBase<InputType>& matrix)
-    {
-      m_matrix = matrix.derived();
-      m_hCoeffs.resize(matrix.rows()-1, 1);
-      internal::tridiagonalization_inplace(m_matrix, m_hCoeffs);
-      m_isInitialized = true;
-      return *this;
-    }
+  /** \brief Returns the Householder coefficients.
+   *
+   * \returns a const reference to the vector of Householder coefficients
+   *
+   * \pre Either the constructor Tridiagonalization(const MatrixType&) or
+   * the member function compute(const MatrixType&) has been called before
+   * to compute the tridiagonal decomposition of a matrix.
+   *
+   * The Householder coefficients allow the reconstruction of the matrix
+   * \f$ Q \f$ in the tridiagonal decomposition from the packed data.
+   *
+   * Example: \include Tridiagonalization_householderCoefficients.cpp
+   * Output: \verbinclude Tridiagonalization_householderCoefficients.out
+   *
+   * \sa packedMatrix(), \ref Householder_Module "Householder module"
+   */
+  inline CoeffVectorType householderCoefficients() const {
+    eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
+    return m_hCoeffs;
+  }
 
-    /** \brief Returns the Householder coefficients.
-      *
-      * \returns a const reference to the vector of Householder coefficients
-      *
-      * \pre Either the constructor Tridiagonalization(const MatrixType&) or
-      * the member function compute(const MatrixType&) has been called before
-      * to compute the tridiagonal decomposition of a matrix.
-      *
-      * The Householder coefficients allow the reconstruction of the matrix
-      * \f$ Q \f$ in the tridiagonal decomposition from the packed data.
-      *
-      * Example: \include Tridiagonalization_householderCoefficients.cpp
-      * Output: \verbinclude Tridiagonalization_householderCoefficients.out
-      *
-      * \sa packedMatrix(), \ref Householder_Module "Householder module"
-      */
-    inline CoeffVectorType householderCoefficients() const
-    {
-      eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
-      return m_hCoeffs;
-    }
+  /** \brief Returns the internal representation of the decomposition
+   *
+   *	\returns a const reference to a matrix with the internal representation
+   *	         of the decomposition.
+   *
+   * \pre Either the constructor Tridiagonalization(const MatrixType&) or
+   * the member function compute(const MatrixType&) has been called before
+   * to compute the tridiagonal decomposition of a matrix.
+   *
+   * The returned matrix contains the following information:
+   *  - the strict upper triangular part is equal to the input matrix A.
+   *  - the diagonal and lower sub-diagonal represent the real tridiagonal
+   *    symmetric matrix T.
+   *  - the rest of the lower part contains the Householder vectors that,
+   *    combined with Householder coefficients returned by
+   *    householderCoefficients(), allows to reconstruct the matrix Q as
+   *       \f$ Q = H_{N-1} \ldots H_1 H_0 \f$.
+   *    Here, the matrices \f$ H_i \f$ are the Householder transformations
+   *       \f$ H_i = (I - h_i v_i v_i^T) \f$
+   *    where \f$ h_i \f$ is the \f$ i \f$th Householder coefficient and
+   *    \f$ v_i \f$ is the Householder vector defined by
+   *       \f$ v_i = [ 0, \ldots, 0, 1, M(i+2,i), \ldots, M(N-1,i) ]^T \f$
+   *    with M the matrix returned by this function.
+   *
+   * See LAPACK for further details on this packed storage.
+   *
+   * Example: \include Tridiagonalization_packedMatrix.cpp
+   * Output: \verbinclude Tridiagonalization_packedMatrix.out
+   *
+   * \sa householderCoefficients()
+   */
+  inline const MatrixType& packedMatrix() const {
+    eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
+    return m_matrix;
+  }
 
-    /** \brief Returns the internal representation of the decomposition
-      *
-      *	\returns a const reference to a matrix with the internal representation
-      *	         of the decomposition.
-      *
-      * \pre Either the constructor Tridiagonalization(const MatrixType&) or
-      * the member function compute(const MatrixType&) has been called before
-      * to compute the tridiagonal decomposition of a matrix.
-      *
-      * The returned matrix contains the following information:
-      *  - the strict upper triangular part is equal to the input matrix A.
-      *  - the diagonal and lower sub-diagonal represent the real tridiagonal
-      *    symmetric matrix T.
-      *  - the rest of the lower part contains the Householder vectors that,
-      *    combined with Householder coefficients returned by
-      *    householderCoefficients(), allows to reconstruct the matrix Q as
-      *       \f$ Q = H_{N-1} \ldots H_1 H_0 \f$.
-      *    Here, the matrices \f$ H_i \f$ are the Householder transformations
-      *       \f$ H_i = (I - h_i v_i v_i^T) \f$
-      *    where \f$ h_i \f$ is the \f$ i \f$th Householder coefficient and
-      *    \f$ v_i \f$ is the Householder vector defined by
-      *       \f$ v_i = [ 0, \ldots, 0, 1, M(i+2,i), \ldots, M(N-1,i) ]^T \f$
-      *    with M the matrix returned by this function.
-      *
-      * See LAPACK for further details on this packed storage.
-      *
-      * Example: \include Tridiagonalization_packedMatrix.cpp
-      * Output: \verbinclude Tridiagonalization_packedMatrix.out
-      *
-      * \sa householderCoefficients()
-      */
-    inline const MatrixType& packedMatrix() const
-    {
-      eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
-      return m_matrix;
-    }
+  /** \brief Returns the unitary matrix Q in the decomposition
+   *
+   * \returns object representing the matrix Q
+   *
+   * \pre Either the constructor Tridiagonalization(const MatrixType&) or
+   * the member function compute(const MatrixType&) has been called before
+   * to compute the tridiagonal decomposition of a matrix.
+   *
+   * This function returns a light-weight object of template class
+   * HouseholderSequence. You can either apply it directly to a matrix or
+   * you can convert it to a matrix of type #MatrixType.
+   *
+   * \sa Tridiagonalization(const MatrixType&) for an example,
+   *     matrixT(), class HouseholderSequence
+   */
+  HouseholderSequenceType matrixQ() const {
+    eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
+    return HouseholderSequenceType(m_matrix, m_hCoeffs.conjugate()).setLength(m_matrix.rows() - 1).setShift(1);
+  }
 
-    /** \brief Returns the unitary matrix Q in the decomposition
-      *
-      * \returns object representing the matrix Q
-      *
-      * \pre Either the constructor Tridiagonalization(const MatrixType&) or
-      * the member function compute(const MatrixType&) has been called before
-      * to compute the tridiagonal decomposition of a matrix.
-      *
-      * This function returns a light-weight object of template class
-      * HouseholderSequence. You can either apply it directly to a matrix or
-      * you can convert it to a matrix of type #MatrixType.
-      *
-      * \sa Tridiagonalization(const MatrixType&) for an example,
-      *     matrixT(), class HouseholderSequence
-      */
-    HouseholderSequenceType matrixQ() const
-    {
-      eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
-      return HouseholderSequenceType(m_matrix, m_hCoeffs.conjugate())
-             .setLength(m_matrix.rows() - 1)
-             .setShift(1);
-    }
+  /** \brief Returns an expression of the tridiagonal matrix T in the decomposition
+   *
+   * \returns expression object representing the matrix T
+   *
+   * \pre Either the constructor Tridiagonalization(const MatrixType&) or
+   * the member function compute(const MatrixType&) has been called before
+   * to compute the tridiagonal decomposition of a matrix.
+   *
+   * Currently, this function can be used to extract the matrix T from internal
+   * data and copy it to a dense matrix object. In most cases, it may be
+   * sufficient to directly use the packed matrix or the vector expressions
+   * returned by diagonal() and subDiagonal() instead of creating a new
+   * dense copy matrix with this function.
+   *
+   * \sa Tridiagonalization(const MatrixType&) for an example,
+   * matrixQ(), packedMatrix(), diagonal(), subDiagonal()
+   */
+  MatrixTReturnType matrixT() const {
+    eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
+    return MatrixTReturnType(m_matrix.real());
+  }
 
-    /** \brief Returns an expression of the tridiagonal matrix T in the decomposition
-      *
-      * \returns expression object representing the matrix T
-      *
-      * \pre Either the constructor Tridiagonalization(const MatrixType&) or
-      * the member function compute(const MatrixType&) has been called before
-      * to compute the tridiagonal decomposition of a matrix.
-      *
-      * Currently, this function can be used to extract the matrix T from internal
-      * data and copy it to a dense matrix object. In most cases, it may be
-      * sufficient to directly use the packed matrix or the vector expressions
-      * returned by diagonal() and subDiagonal() instead of creating a new
-      * dense copy matrix with this function.
-      *
-      * \sa Tridiagonalization(const MatrixType&) for an example,
-      * matrixQ(), packedMatrix(), diagonal(), subDiagonal()
-      */
-    MatrixTReturnType matrixT() const
-    {
-      eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
-      return MatrixTReturnType(m_matrix.real());
-    }
+  /** \brief Returns the diagonal of the tridiagonal matrix T in the decomposition.
+   *
+   * \returns expression representing the diagonal of T
+   *
+   * \pre Either the constructor Tridiagonalization(const MatrixType&) or
+   * the member function compute(const MatrixType&) has been called before
+   * to compute the tridiagonal decomposition of a matrix.
+   *
+   * Example: \include Tridiagonalization_diagonal.cpp
+   * Output: \verbinclude Tridiagonalization_diagonal.out
+   *
+   * \sa matrixT(), subDiagonal()
+   */
+  DiagonalReturnType diagonal() const;
 
-    /** \brief Returns the diagonal of the tridiagonal matrix T in the decomposition.
-      *
-      * \returns expression representing the diagonal of T
-      *
-      * \pre Either the constructor Tridiagonalization(const MatrixType&) or
-      * the member function compute(const MatrixType&) has been called before
-      * to compute the tridiagonal decomposition of a matrix.
-      *
-      * Example: \include Tridiagonalization_diagonal.cpp
-      * Output: \verbinclude Tridiagonalization_diagonal.out
-      *
-      * \sa matrixT(), subDiagonal()
-      */
-    DiagonalReturnType diagonal() const;
+  /** \brief Returns the subdiagonal of the tridiagonal matrix T in the decomposition.
+   *
+   * \returns expression representing the subdiagonal of T
+   *
+   * \pre Either the constructor Tridiagonalization(const MatrixType&) or
+   * the member function compute(const MatrixType&) has been called before
+   * to compute the tridiagonal decomposition of a matrix.
+   *
+   * \sa diagonal() for an example, matrixT()
+   */
+  SubDiagonalReturnType subDiagonal() const;
 
-    /** \brief Returns the subdiagonal of the tridiagonal matrix T in the decomposition.
-      *
-      * \returns expression representing the subdiagonal of T
-      *
-      * \pre Either the constructor Tridiagonalization(const MatrixType&) or
-      * the member function compute(const MatrixType&) has been called before
-      * to compute the tridiagonal decomposition of a matrix.
-      *
-      * \sa diagonal() for an example, matrixT()
-      */
-    SubDiagonalReturnType subDiagonal() const;
-
-  protected:
-
-    MatrixType m_matrix;
-    CoeffVectorType m_hCoeffs;
-    bool m_isInitialized;
+ protected:
+  MatrixType m_matrix;
+  CoeffVectorType m_hCoeffs;
+  bool m_isInitialized;
 };
 
-template<typename MatrixType>
-typename Tridiagonalization<MatrixType>::DiagonalReturnType
-Tridiagonalization<MatrixType>::diagonal() const
-{
+template <typename MatrixType>
+typename Tridiagonalization<MatrixType>::DiagonalReturnType Tridiagonalization<MatrixType>::diagonal() const {
   eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
   return m_matrix.diagonal().real();
 }
 
-template<typename MatrixType>
-typename Tridiagonalization<MatrixType>::SubDiagonalReturnType
-Tridiagonalization<MatrixType>::subDiagonal() const
-{
+template <typename MatrixType>
+typename Tridiagonalization<MatrixType>::SubDiagonalReturnType Tridiagonalization<MatrixType>::subDiagonal() const {
   eigen_assert(m_isInitialized && "Tridiagonalization is not initialized.");
   return m_matrix.template diagonal<-1>().real();
 }
@@ -322,240 +306,222 @@
 namespace internal {
 
 /** \internal
-  * Performs a tridiagonal decomposition of the selfadjoint matrix \a matA in-place.
-  *
-  * \param[in,out] matA On input the selfadjoint matrix. Only the \b lower triangular part is referenced.
-  *                     On output, the strict upper part is left unchanged, and the lower triangular part
-  *                     represents the T and Q matrices in packed format has detailed below.
-  * \param[out]    hCoeffs returned Householder coefficients (see below)
-  *
-  * On output, the tridiagonal selfadjoint matrix T is stored in the diagonal
-  * and lower sub-diagonal of the matrix \a matA.
-  * The unitary matrix Q is represented in a compact way as a product of
-  * Householder reflectors \f$ H_i \f$ such that:
-  *       \f$ Q = H_{N-1} \ldots H_1 H_0 \f$.
-  * The Householder reflectors are defined as
-  *       \f$ H_i = (I - h_i v_i v_i^T) \f$
-  * where \f$ h_i = hCoeffs[i]\f$ is the \f$ i \f$th Householder coefficient and
-  * \f$ v_i \f$ is the Householder vector defined by
-  *       \f$ v_i = [ 0, \ldots, 0, 1, matA(i+2,i), \ldots, matA(N-1,i) ]^T \f$.
-  *
-  * Implemented from Golub's "Matrix Computations", algorithm 8.3.1.
-  *
-  * \sa Tridiagonalization::packedMatrix()
-  */
-template<typename MatrixType, typename CoeffVectorType>
-EIGEN_DEVICE_FUNC
-void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs)
-{
+ * Performs a tridiagonal decomposition of the selfadjoint matrix \a matA in-place.
+ *
+ * \param[in,out] matA On input the selfadjoint matrix. Only the \b lower triangular part is referenced.
+ *                     On output, the strict upper part is left unchanged, and the lower triangular part
+ *                     represents the T and Q matrices in packed format has detailed below.
+ * \param[out]    hCoeffs returned Householder coefficients (see below)
+ *
+ * On output, the tridiagonal selfadjoint matrix T is stored in the diagonal
+ * and lower sub-diagonal of the matrix \a matA.
+ * The unitary matrix Q is represented in a compact way as a product of
+ * Householder reflectors \f$ H_i \f$ such that:
+ *       \f$ Q = H_{N-1} \ldots H_1 H_0 \f$.
+ * The Householder reflectors are defined as
+ *       \f$ H_i = (I - h_i v_i v_i^T) \f$
+ * where \f$ h_i = hCoeffs[i]\f$ is the \f$ i \f$th Householder coefficient and
+ * \f$ v_i \f$ is the Householder vector defined by
+ *       \f$ v_i = [ 0, \ldots, 0, 1, matA(i+2,i), \ldots, matA(N-1,i) ]^T \f$.
+ *
+ * Implemented from Golub's "Matrix Computations", algorithm 8.3.1.
+ *
+ * \sa Tridiagonalization::packedMatrix()
+ */
+template <typename MatrixType, typename CoeffVectorType>
+EIGEN_DEVICE_FUNC void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs) {
   using numext::conj;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   Index n = matA.rows();
-  eigen_assert(n==matA.cols());
-  eigen_assert(n==hCoeffs.size()+1 || n==1);
+  eigen_assert(n == matA.cols());
+  eigen_assert(n == hCoeffs.size() + 1 || n == 1);
 
-  for (Index i = 0; i<n-1; ++i)
-  {
-    Index remainingSize = n-i-1;
+  for (Index i = 0; i < n - 1; ++i) {
+    Index remainingSize = n - i - 1;
     RealScalar beta;
     Scalar h;
     matA.col(i).tail(remainingSize).makeHouseholderInPlace(h, beta);
 
     // Apply similarity transformation to remaining columns,
     // i.e., A = H A H' where H = I - h v v' and v = matA.col(i).tail(n-i-1)
-    matA.col(i).coeffRef(i+1) = 1;
+    matA.col(i).coeffRef(i + 1) = 1;
 
-    hCoeffs.tail(n-i-1).noalias() = (matA.bottomRightCorner(remainingSize,remainingSize).template selfadjointView<Lower>()
-                                  * (conj(h) * matA.col(i).tail(remainingSize)));
+    hCoeffs.tail(n - i - 1).noalias() =
+        (matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView<Lower>() *
+         (conj(h) * matA.col(i).tail(remainingSize)));
 
-    hCoeffs.tail(n-i-1) += (conj(h)*RealScalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1);
+    hCoeffs.tail(n - i - 1) +=
+        (conj(h) * RealScalar(-0.5) * (hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) *
+        matA.col(i).tail(n - i - 1);
 
-    matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView<Lower>()
-      .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), Scalar(-1));
+    matA.bottomRightCorner(remainingSize, remainingSize)
+        .template selfadjointView<Lower>()
+        .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), Scalar(-1));
 
-    matA.col(i).coeffRef(i+1) = beta;
+    matA.col(i).coeffRef(i + 1) = beta;
     hCoeffs.coeffRef(i) = h;
   }
 }
 
 // forward declaration, implementation at the end of this file
-template<typename MatrixType,
-         int Size=MatrixType::ColsAtCompileTime,
-         bool IsComplex=NumTraits<typename MatrixType::Scalar>::IsComplex>
+template <typename MatrixType, int Size = MatrixType::ColsAtCompileTime,
+          bool IsComplex = NumTraits<typename MatrixType::Scalar>::IsComplex>
 struct tridiagonalization_inplace_selector;
 
 /** \brief Performs a full tridiagonalization in place
-  *
-  * \param[in,out]  mat  On input, the selfadjoint matrix whose tridiagonal
-  *    decomposition is to be computed. Only the lower triangular part referenced.
-  *    The rest is left unchanged. On output, the orthogonal matrix Q
-  *    in the decomposition if \p extractQ is true.
-  * \param[out]  diag  The diagonal of the tridiagonal matrix T in the
-  *    decomposition.
-  * \param[out]  subdiag  The subdiagonal of the tridiagonal matrix T in
-  *    the decomposition.
-  * \param[in]  extractQ  If true, the orthogonal matrix Q in the
-  *    decomposition is computed and stored in \p mat.
-  *
-  * Computes the tridiagonal decomposition of the selfadjoint matrix \p mat in place
-  * such that \f$ mat = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real
-  * symmetric tridiagonal matrix.
-  *
-  * The tridiagonal matrix T is passed to the output parameters \p diag and \p subdiag. If
-  * \p extractQ is true, then the orthogonal matrix Q is passed to \p mat. Otherwise the lower
-  * part of the matrix \p mat is destroyed.
-  *
-  * The vectors \p diag and \p subdiag are not resized. The function
-  * assumes that they are already of the correct size. The length of the
-  * vector \p diag should equal the number of rows in \p mat, and the
-  * length of the vector \p subdiag should be one left.
-  *
-  * This implementation contains an optimized path for 3-by-3 matrices
-  * which is especially useful for plane fitting.
-  *
-  * \note Currently, it requires two temporary vectors to hold the intermediate
-  * Householder coefficients, and to reconstruct the matrix Q from the Householder
-  * reflectors.
-  *
-  * Example (this uses the same matrix as the example in
-  *    Tridiagonalization::Tridiagonalization(const MatrixType&)):
-  *    \include Tridiagonalization_decomposeInPlace.cpp
-  * Output: \verbinclude Tridiagonalization_decomposeInPlace.out
-  *
-  * \sa class Tridiagonalization
-  */
-template<typename MatrixType, typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType>
-EIGEN_DEVICE_FUNC
-void tridiagonalization_inplace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag,
-                                CoeffVectorType& hcoeffs, bool extractQ)
-{
-  eigen_assert(mat.cols()==mat.rows() && diag.size()==mat.rows() && subdiag.size()==mat.rows()-1);
-  tridiagonalization_inplace_selector<MatrixType>::run(mat, diag, subdiag, hcoeffs, extractQ);
+ *
+ * \param[in,out]  mat  On input, the selfadjoint matrix whose tridiagonal
+ *    decomposition is to be computed. Only the lower triangular part referenced.
+ *    The rest is left unchanged. On output, the orthogonal matrix Q
+ *    in the decomposition if \p extractQ is true.
+ * \param[out]  diag  The diagonal of the tridiagonal matrix T in the
+ *    decomposition.
+ * \param[out]  subdiag  The subdiagonal of the tridiagonal matrix T in
+ *    the decomposition.
+ * \param[in]  extractQ  If true, the orthogonal matrix Q in the
+ *    decomposition is computed and stored in \p mat.
+ *
+ * Computes the tridiagonal decomposition of the selfadjoint matrix \p mat in place
+ * such that \f$ mat = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real
+ * symmetric tridiagonal matrix.
+ *
+ * The tridiagonal matrix T is passed to the output parameters \p diag and \p subdiag. If
+ * \p extractQ is true, then the orthogonal matrix Q is passed to \p mat. Otherwise the lower
+ * part of the matrix \p mat is destroyed.
+ *
+ * The vectors \p diag and \p subdiag are not resized. The function
+ * assumes that they are already of the correct size. The length of the
+ * vector \p diag should equal the number of rows in \p mat, and the
+ * length of the vector \p subdiag should be one left.
+ *
+ * This implementation contains an optimized path for 3-by-3 matrices
+ * which is especially useful for plane fitting.
+ *
+ * \note Currently, it requires two temporary vectors to hold the intermediate
+ * Householder coefficients, and to reconstruct the matrix Q from the Householder
+ * reflectors.
+ *
+ * Example (this uses the same matrix as the example in
+ *    Tridiagonalization::Tridiagonalization(const MatrixType&)):
+ *    \include Tridiagonalization_decomposeInPlace.cpp
+ * Output: \verbinclude Tridiagonalization_decomposeInPlace.out
+ *
+ * \sa class Tridiagonalization
+ */
+template <typename MatrixType, typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType,
+          typename WorkSpaceType>
+EIGEN_DEVICE_FUNC void tridiagonalization_inplace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag,
+                                                  CoeffVectorType& hcoeffs, WorkSpaceType& workspace, bool extractQ) {
+  eigen_assert(mat.cols() == mat.rows() && diag.size() == mat.rows() && subdiag.size() == mat.rows() - 1);
+  tridiagonalization_inplace_selector<MatrixType>::run(mat, diag, subdiag, hcoeffs, workspace, extractQ);
 }
 
 /** \internal
-  * General full tridiagonalization
-  */
-template<typename MatrixType, int Size, bool IsComplex>
-struct tridiagonalization_inplace_selector
-{
-  typedef typename Tridiagonalization<MatrixType>::CoeffVectorType CoeffVectorType;
+ * General full tridiagonalization
+ */
+template <typename MatrixType, int Size, bool IsComplex>
+struct tridiagonalization_inplace_selector {
   typedef typename Tridiagonalization<MatrixType>::HouseholderSequenceType HouseholderSequenceType;
-  template<typename DiagonalType, typename SubDiagonalType>
-  static EIGEN_DEVICE_FUNC
-      void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, CoeffVectorType& hCoeffs, bool extractQ)
-  {
+  template <typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType, typename WorkSpaceType>
+  static EIGEN_DEVICE_FUNC void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag,
+                                    CoeffVectorType& hCoeffs, WorkSpaceType& workspace, bool extractQ) {
     tridiagonalization_inplace(mat, hCoeffs);
     diag = mat.diagonal().real();
     subdiag = mat.template diagonal<-1>().real();
-    if(extractQ)
-      mat = HouseholderSequenceType(mat, hCoeffs.conjugate())
-            .setLength(mat.rows() - 1)
-            .setShift(1);
+    if (extractQ) {
+      HouseholderSequenceType(mat, hCoeffs.conjugate()).setLength(mat.rows() - 1).setShift(1).evalTo(mat, workspace);
+    }
   }
 };
 
 /** \internal
-  * Specialization for 3x3 real matrices.
-  * Especially useful for plane fitting.
-  */
-template<typename MatrixType>
-struct tridiagonalization_inplace_selector<MatrixType,3,false>
-{
+ * Specialization for 3x3 real matrices.
+ * Especially useful for plane fitting.
+ */
+template <typename MatrixType>
+struct tridiagonalization_inplace_selector<MatrixType, 3, false> {
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
 
-  template<typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType>
-  static void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, CoeffVectorType&, bool extractQ)
-  {
+  template <typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType, typename WorkSpaceType>
+  static void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, CoeffVectorType&, WorkSpaceType&,
+                  bool extractQ) {
     using std::sqrt;
     const RealScalar tol = (std::numeric_limits<RealScalar>::min)();
-    diag[0] = mat(0,0);
-    RealScalar v1norm2 = numext::abs2(mat(2,0));
-    if(v1norm2 <= tol)
-    {
-      diag[1] = mat(1,1);
-      diag[2] = mat(2,2);
-      subdiag[0] = mat(1,0);
-      subdiag[1] = mat(2,1);
-      if (extractQ)
-        mat.setIdentity();
-    }
-    else
-    {
-      RealScalar beta = sqrt(numext::abs2(mat(1,0)) + v1norm2);
-      RealScalar invBeta = RealScalar(1)/beta;
-      Scalar m01 = mat(1,0) * invBeta;
-      Scalar m02 = mat(2,0) * invBeta;
-      Scalar q = RealScalar(2)*m01*mat(2,1) + m02*(mat(2,2) - mat(1,1));
-      diag[1] = mat(1,1) + m02*q;
-      diag[2] = mat(2,2) - m02*q;
+    diag[0] = mat(0, 0);
+    RealScalar v1norm2 = numext::abs2(mat(2, 0));
+    if (v1norm2 <= tol) {
+      diag[1] = mat(1, 1);
+      diag[2] = mat(2, 2);
+      subdiag[0] = mat(1, 0);
+      subdiag[1] = mat(2, 1);
+      if (extractQ) mat.setIdentity();
+    } else {
+      RealScalar beta = sqrt(numext::abs2(mat(1, 0)) + v1norm2);
+      RealScalar invBeta = RealScalar(1) / beta;
+      Scalar m01 = mat(1, 0) * invBeta;
+      Scalar m02 = mat(2, 0) * invBeta;
+      Scalar q = RealScalar(2) * m01 * mat(2, 1) + m02 * (mat(2, 2) - mat(1, 1));
+      diag[1] = mat(1, 1) + m02 * q;
+      diag[2] = mat(2, 2) - m02 * q;
       subdiag[0] = beta;
-      subdiag[1] = mat(2,1) - m01 * q;
-      if (extractQ)
-      {
-        mat << 1,   0,    0,
-               0, m01,  m02,
-               0, m02, -m01;
+      subdiag[1] = mat(2, 1) - m01 * q;
+      if (extractQ) {
+        mat << 1, 0, 0, 0, m01, m02, 0, m02, -m01;
       }
     }
   }
 };
 
 /** \internal
-  * Trivial specialization for 1x1 matrices
-  */
-template<typename MatrixType, bool IsComplex>
-struct tridiagonalization_inplace_selector<MatrixType,1,IsComplex>
-{
+ * Trivial specialization for 1x1 matrices
+ */
+template <typename MatrixType, bool IsComplex>
+struct tridiagonalization_inplace_selector<MatrixType, 1, IsComplex> {
   typedef typename MatrixType::Scalar Scalar;
 
-  template<typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType>
-  static EIGEN_DEVICE_FUNC
-  void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType&, CoeffVectorType&, bool extractQ)
-  {
-    diag(0,0) = numext::real(mat(0,0));
-    if(extractQ)
-      mat(0,0) = Scalar(1);
+  template <typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType, typename WorkSpaceType>
+  static EIGEN_DEVICE_FUNC void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType&, CoeffVectorType&,
+                                    WorkSpaceType&, bool extractQ) {
+    diag(0, 0) = numext::real(mat(0, 0));
+    if (extractQ) mat(0, 0) = Scalar(1);
   }
 };
 
 /** \internal
-  * \eigenvalues_module \ingroup Eigenvalues_Module
-  *
-  * \brief Expression type for return value of Tridiagonalization::matrixT()
-  *
-  * \tparam MatrixType type of underlying dense matrix
-  */
-template<typename MatrixType> struct TridiagonalizationMatrixTReturnType
-: public ReturnByValue<TridiagonalizationMatrixTReturnType<MatrixType> >
-{
-  public:
-    /** \brief Constructor.
-      *
-      * \param[in] mat The underlying dense matrix
-      */
-    TridiagonalizationMatrixTReturnType(const MatrixType& mat) : m_matrix(mat) { }
+ * \eigenvalues_module \ingroup Eigenvalues_Module
+ *
+ * \brief Expression type for return value of Tridiagonalization::matrixT()
+ *
+ * \tparam MatrixType type of underlying dense matrix
+ */
+template <typename MatrixType>
+struct TridiagonalizationMatrixTReturnType : public ReturnByValue<TridiagonalizationMatrixTReturnType<MatrixType>> {
+ public:
+  /** \brief Constructor.
+   *
+   * \param[in] mat The underlying dense matrix
+   */
+  TridiagonalizationMatrixTReturnType(const MatrixType& mat) : m_matrix(mat) {}
 
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      result.setZero();
-      result.template diagonal<1>() = m_matrix.template diagonal<-1>().conjugate();
-      result.diagonal() = m_matrix.diagonal();
-      result.template diagonal<-1>() = m_matrix.template diagonal<-1>();
-    }
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    result.setZero();
+    result.template diagonal<1>() = m_matrix.template diagonal<-1>().conjugate();
+    result.diagonal() = m_matrix.diagonal();
+    result.template diagonal<-1>() = m_matrix.template diagonal<-1>();
+  }
 
-    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
-  protected:
-    typename MatrixType::Nested m_matrix;
+ protected:
+  typename MatrixType::Nested m_matrix;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_TRIDIAGONALIZATION_H
+#endif  // EIGEN_TRIDIAGONALIZATION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h
index 39ce1c2..1d6cc1c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h
@@ -13,23 +13,27 @@
 
 // This file contains some helper function to deal with block householder reflectors
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-  
+
 /** \internal */
 // template<typename TriangularFactorType,typename VectorsType,typename CoeffsType>
-// void make_block_householder_triangular_factor(TriangularFactorType& triFactor, const VectorsType& vectors, const CoeffsType& hCoeffs)
+// void make_block_householder_triangular_factor(TriangularFactorType& triFactor, const VectorsType& vectors, const
+// CoeffsType& hCoeffs)
 // {
 //   typedef typename VectorsType::Scalar Scalar;
 //   const Index nbVecs = vectors.cols();
 //   eigen_assert(triFactor.rows() == nbVecs && triFactor.cols() == nbVecs && vectors.rows()>=nbVecs);
-// 
+//
 //   for(Index i = 0; i < nbVecs; i++)
 //   {
 //     Index rs = vectors.rows() - i;
 //     // Warning, note that hCoeffs may alias with vectors.
-//     // It is then necessary to copy it before modifying vectors(i,i). 
+//     // It is then necessary to copy it before modifying vectors(i,i).
 //     typename CoeffsType::Scalar h = hCoeffs(i);
 //     // This hack permits to pass trough nested Block<> and Transpose<> expressions.
 //     Scalar *Vii_ptr = const_cast<Scalar*>(vectors.data() + vectors.outerStride()*i + vectors.innerStride()*i);
@@ -47,64 +51,65 @@
 
 /** \internal */
 // This variant avoid modifications in vectors
-template<typename TriangularFactorType,typename VectorsType,typename CoeffsType>
-void make_block_householder_triangular_factor(TriangularFactorType& triFactor, const VectorsType& vectors, const CoeffsType& hCoeffs)
-{
+template <typename TriangularFactorType, typename VectorsType, typename CoeffsType>
+void make_block_householder_triangular_factor(TriangularFactorType& triFactor, const VectorsType& vectors,
+                                              const CoeffsType& hCoeffs) {
   const Index nbVecs = vectors.cols();
-  eigen_assert(triFactor.rows() == nbVecs && triFactor.cols() == nbVecs && vectors.rows()>=nbVecs);
+  eigen_assert(triFactor.rows() == nbVecs && triFactor.cols() == nbVecs && vectors.rows() >= nbVecs);
 
-  for(Index i = nbVecs-1; i >=0 ; --i)
-  {
+  for (Index i = nbVecs - 1; i >= 0; --i) {
     Index rs = vectors.rows() - i - 1;
-    Index rt = nbVecs-i-1;
+    Index rt = nbVecs - i - 1;
 
-    if(rt>0)
-    {
-      triFactor.row(i).tail(rt).noalias() = -hCoeffs(i) * vectors.col(i).tail(rs).adjoint()
-                                                        * vectors.bottomRightCorner(rs, rt).template triangularView<UnitLower>();
-            
+    if (rt > 0) {
+      triFactor.row(i).tail(rt).noalias() = -hCoeffs(i) * vectors.col(i).tail(rs).adjoint() *
+                                            vectors.bottomRightCorner(rs, rt).template triangularView<UnitLower>();
+
       // FIXME use the following line with .noalias() once the triangular product can work inplace
-      // triFactor.row(i).tail(rt) = triFactor.row(i).tail(rt) * triFactor.bottomRightCorner(rt,rt).template triangularView<Upper>();
-      for(Index j=nbVecs-1; j>i; --j)
-      {
-        typename TriangularFactorType::Scalar z = triFactor(i,j);
-        triFactor(i,j) = z * triFactor(j,j);
-        if(nbVecs-j-1>0)
-          triFactor.row(i).tail(nbVecs-j-1) += z * triFactor.row(j).tail(nbVecs-j-1);
+      // triFactor.row(i).tail(rt) = triFactor.row(i).tail(rt) * triFactor.bottomRightCorner(rt,rt).template
+      // triangularView<Upper>();
+      for (Index j = nbVecs - 1; j > i; --j) {
+        typename TriangularFactorType::Scalar z = triFactor(i, j);
+        triFactor(i, j) = z * triFactor(j, j);
+        if (nbVecs - j - 1 > 0) triFactor.row(i).tail(nbVecs - j - 1) += z * triFactor.row(j).tail(nbVecs - j - 1);
       }
-      
     }
-    triFactor(i,i) = hCoeffs(i);
+    triFactor(i, i) = hCoeffs(i);
   }
 }
 
 /** \internal
-  * if forward then perform   mat = H0 * H1 * H2 * mat
-  * otherwise perform         mat = H2 * H1 * H0 * mat
-  */
-template<typename MatrixType,typename VectorsType,typename CoeffsType>
-void apply_block_householder_on_the_left(MatrixType& mat, const VectorsType& vectors, const CoeffsType& hCoeffs, bool forward)
-{
-  enum { TFactorSize = MatrixType::ColsAtCompileTime };
+ * if forward then perform   mat = H0 * H1 * H2 * mat
+ * otherwise perform         mat = H2 * H1 * H0 * mat
+ */
+template <typename MatrixType, typename VectorsType, typename CoeffsType>
+void apply_block_householder_on_the_left(MatrixType& mat, const VectorsType& vectors, const CoeffsType& hCoeffs,
+                                         bool forward) {
+  enum { TFactorSize = VectorsType::ColsAtCompileTime };
   Index nbVecs = vectors.cols();
-  Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize, RowMajor> T(nbVecs,nbVecs);
-  
-  if(forward) make_block_householder_triangular_factor(T, vectors, hCoeffs);
-  else        make_block_householder_triangular_factor(T, vectors, hCoeffs.conjugate());  
+  Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize, RowMajor> T(nbVecs, nbVecs);
+
+  if (forward)
+    make_block_householder_triangular_factor(T, vectors, hCoeffs);
+  else
+    make_block_householder_triangular_factor(T, vectors, hCoeffs.conjugate());
   const TriangularView<const VectorsType, UnitLower> V(vectors);
 
   // A -= V T V^* A
-  Matrix<typename MatrixType::Scalar,VectorsType::ColsAtCompileTime,MatrixType::ColsAtCompileTime,
-         (VectorsType::MaxColsAtCompileTime==1 && MatrixType::MaxColsAtCompileTime!=1)?RowMajor:ColMajor,
-         VectorsType::MaxColsAtCompileTime,MatrixType::MaxColsAtCompileTime> tmp = V.adjoint() * mat;
+  Matrix<typename MatrixType::Scalar, VectorsType::ColsAtCompileTime, MatrixType::ColsAtCompileTime,
+         (VectorsType::MaxColsAtCompileTime == 1 && MatrixType::MaxColsAtCompileTime != 1) ? RowMajor : ColMajor,
+         VectorsType::MaxColsAtCompileTime, MatrixType::MaxColsAtCompileTime>
+      tmp = V.adjoint() * mat;
   // FIXME add .noalias() once the triangular product can work inplace
-  if(forward) tmp = T.template triangularView<Upper>()           * tmp;
-  else        tmp = T.template triangularView<Upper>().adjoint() * tmp;
+  if (forward)
+    tmp = T.template triangularView<Upper>() * tmp;
+  else
+    tmp = T.template triangularView<Upper>().adjoint() * tmp;
   mat.noalias() -= V * tmp;
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_BLOCK_HOUSEHOLDER_H
+#endif  // EIGEN_BLOCK_HOUSEHOLDER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h
index 5bc037f..96b1daf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h
@@ -11,121 +11,106 @@
 #ifndef EIGEN_HOUSEHOLDER_H
 #define EIGEN_HOUSEHOLDER_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-template<int n> struct decrement_size
-{
-  enum {
-    ret = n==Dynamic ? n : n-1
-  };
+template <int n>
+struct decrement_size {
+  enum { ret = n == Dynamic ? n : n - 1 };
 };
-}
+}  // namespace internal
 
 /** Computes the elementary reflector H such that:
-  * \f$ H *this = [ beta 0 ... 0]^T \f$
-  * where the transformation H is:
-  * \f$ H = I - tau v v^*\f$
-  * and the vector v is:
-  * \f$ v^T = [1 essential^T] \f$
-  *
-  * The essential part of the vector \c v is stored in *this.
-  * 
-  * On output:
-  * \param tau the scaling factor of the Householder transformation
-  * \param beta the result of H * \c *this
-  *
-  * \sa MatrixBase::makeHouseholder(), MatrixBase::applyHouseholderOnTheLeft(),
-  *     MatrixBase::applyHouseholderOnTheRight()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC
-void MatrixBase<Derived>::makeHouseholderInPlace(Scalar& tau, RealScalar& beta)
-{
-  VectorBlock<Derived, internal::decrement_size<Base::SizeAtCompileTime>::ret> essentialPart(derived(), 1, size()-1);
+ * \f$ H *this = [ beta 0 ... 0]^T \f$
+ * where the transformation H is:
+ * \f$ H = I - tau v v^*\f$
+ * and the vector v is:
+ * \f$ v^T = [1 essential^T] \f$
+ *
+ * The essential part of the vector \c v is stored in *this.
+ *
+ * On output:
+ * \param tau the scaling factor of the Householder transformation
+ * \param beta the result of H * \c *this
+ *
+ * \sa MatrixBase::makeHouseholder(), MatrixBase::applyHouseholderOnTheLeft(),
+ *     MatrixBase::applyHouseholderOnTheRight()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC void MatrixBase<Derived>::makeHouseholderInPlace(Scalar& tau, RealScalar& beta) {
+  VectorBlock<Derived, internal::decrement_size<Base::SizeAtCompileTime>::ret> essentialPart(derived(), 1, size() - 1);
   makeHouseholder(essentialPart, tau, beta);
 }
 
 /** Computes the elementary reflector H such that:
-  * \f$ H *this = [ beta 0 ... 0]^T \f$
-  * where the transformation H is:
-  * \f$ H = I - tau v v^*\f$
-  * and the vector v is:
-  * \f$ v^T = [1 essential^T] \f$
-  *
-  * On output:
-  * \param essential the essential part of the vector \c v
-  * \param tau the scaling factor of the Householder transformation
-  * \param beta the result of H * \c *this
-  *
-  * \sa MatrixBase::makeHouseholderInPlace(), MatrixBase::applyHouseholderOnTheLeft(),
-  *     MatrixBase::applyHouseholderOnTheRight()
-  */
-template<typename Derived>
-template<typename EssentialPart>
-EIGEN_DEVICE_FUNC
-void MatrixBase<Derived>::makeHouseholder(
-  EssentialPart& essential,
-  Scalar& tau,
-  RealScalar& beta) const
-{
-  using std::sqrt;
+ * \f$ H *this = [ beta 0 ... 0]^T \f$
+ * where the transformation H is:
+ * \f$ H = I - tau v v^*\f$
+ * and the vector v is:
+ * \f$ v^T = [1 essential^T] \f$
+ *
+ * On output:
+ * \param essential the essential part of the vector \c v
+ * \param tau the scaling factor of the Householder transformation
+ * \param beta the result of H * \c *this
+ *
+ * \sa MatrixBase::makeHouseholderInPlace(), MatrixBase::applyHouseholderOnTheLeft(),
+ *     MatrixBase::applyHouseholderOnTheRight()
+ */
+template <typename Derived>
+template <typename EssentialPart>
+EIGEN_DEVICE_FUNC void MatrixBase<Derived>::makeHouseholder(EssentialPart& essential, Scalar& tau,
+                                                            RealScalar& beta) const {
   using numext::conj;
-  
+  using numext::sqrt;
+
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(EssentialPart)
-  VectorBlock<const Derived, EssentialPart::SizeAtCompileTime> tail(derived(), 1, size()-1);
-  
-  RealScalar tailSqNorm = size()==1 ? RealScalar(0) : tail.squaredNorm();
+  VectorBlock<const Derived, EssentialPart::SizeAtCompileTime> tail(derived(), 1, size() - 1);
+
+  RealScalar tailSqNorm = size() == 1 ? RealScalar(0) : tail.squaredNorm();
   Scalar c0 = coeff(0);
   const RealScalar tol = (std::numeric_limits<RealScalar>::min)();
 
-  if(tailSqNorm <= tol && numext::abs2(numext::imag(c0))<=tol)
-  {
+  if (tailSqNorm <= tol && numext::abs2(numext::imag(c0)) <= tol) {
     tau = RealScalar(0);
     beta = numext::real(c0);
     essential.setZero();
-  }
-  else
-  {
+  } else {
     beta = sqrt(numext::abs2(c0) + tailSqNorm);
-    if (numext::real(c0)>=RealScalar(0))
-      beta = -beta;
+    if (numext::real(c0) >= RealScalar(0)) beta = -beta;
     essential = tail / (c0 - beta);
     tau = conj((beta - c0) / beta);
   }
 }
 
 /** Apply the elementary reflector H given by
-  * \f$ H = I - tau v v^*\f$
-  * with
-  * \f$ v^T = [1 essential^T] \f$
-  * from the left to a vector or matrix.
-  *
-  * On input:
-  * \param essential the essential part of the vector \c v
-  * \param tau the scaling factor of the Householder transformation
-  * \param workspace a pointer to working space with at least
-  *                  this->cols() entries
-  *
-  * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(), 
-  *     MatrixBase::applyHouseholderOnTheRight()
-  */
-template<typename Derived>
-template<typename EssentialPart>
-EIGEN_DEVICE_FUNC
-void MatrixBase<Derived>::applyHouseholderOnTheLeft(
-  const EssentialPart& essential,
-  const Scalar& tau,
-  Scalar* workspace)
-{
-  if(rows() == 1)
-  {
-    *this *= Scalar(1)-tau;
-  }
-  else if(tau!=Scalar(0))
-  {
-    Map<typename internal::plain_row_type<PlainObject>::type> tmp(workspace,cols());
-    Block<Derived, EssentialPart::SizeAtCompileTime, Derived::ColsAtCompileTime> bottom(derived(), 1, 0, rows()-1, cols());
+ * \f$ H = I - tau v v^*\f$
+ * with
+ * \f$ v^T = [1 essential^T] \f$
+ * from the left to a vector or matrix.
+ *
+ * On input:
+ * \param essential the essential part of the vector \c v
+ * \param tau the scaling factor of the Householder transformation
+ * \param workspace a pointer to working space with at least
+ *                  this->cols() entries
+ *
+ * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(),
+ *     MatrixBase::applyHouseholderOnTheRight()
+ */
+template <typename Derived>
+template <typename EssentialPart>
+EIGEN_DEVICE_FUNC void MatrixBase<Derived>::applyHouseholderOnTheLeft(const EssentialPart& essential, const Scalar& tau,
+                                                                      Scalar* workspace) {
+  if (rows() == 1) {
+    *this *= Scalar(1) - tau;
+  } else if (!numext::is_exactly_zero(tau)) {
+    Map<typename internal::plain_row_type<PlainObject>::type> tmp(workspace, cols());
+    Block<Derived, EssentialPart::SizeAtCompileTime, Derived::ColsAtCompileTime> bottom(derived(), 1, 0, rows() - 1,
+                                                                                        cols());
     tmp.noalias() = essential.adjoint() * bottom;
     tmp += this->row(0);
     this->row(0) -= tau * tmp;
@@ -134,36 +119,30 @@
 }
 
 /** Apply the elementary reflector H given by
-  * \f$ H = I - tau v v^*\f$
-  * with
-  * \f$ v^T = [1 essential^T] \f$
-  * from the right to a vector or matrix.
-  *
-  * On input:
-  * \param essential the essential part of the vector \c v
-  * \param tau the scaling factor of the Householder transformation
-  * \param workspace a pointer to working space with at least
-  *                  this->rows() entries
-  *
-  * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(), 
-  *     MatrixBase::applyHouseholderOnTheLeft()
-  */
-template<typename Derived>
-template<typename EssentialPart>
-EIGEN_DEVICE_FUNC
-void MatrixBase<Derived>::applyHouseholderOnTheRight(
-  const EssentialPart& essential,
-  const Scalar& tau,
-  Scalar* workspace)
-{
-  if(cols() == 1)
-  {
-    *this *= Scalar(1)-tau;
-  }
-  else if(tau!=Scalar(0))
-  {
-    Map<typename internal::plain_col_type<PlainObject>::type> tmp(workspace,rows());
-    Block<Derived, Derived::RowsAtCompileTime, EssentialPart::SizeAtCompileTime> right(derived(), 0, 1, rows(), cols()-1);
+ * \f$ H = I - tau v v^*\f$
+ * with
+ * \f$ v^T = [1 essential^T] \f$
+ * from the right to a vector or matrix.
+ *
+ * On input:
+ * \param essential the essential part of the vector \c v
+ * \param tau the scaling factor of the Householder transformation
+ * \param workspace a pointer to working space with at least
+ *                  this->rows() entries
+ *
+ * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(),
+ *     MatrixBase::applyHouseholderOnTheLeft()
+ */
+template <typename Derived>
+template <typename EssentialPart>
+EIGEN_DEVICE_FUNC void MatrixBase<Derived>::applyHouseholderOnTheRight(const EssentialPart& essential,
+                                                                       const Scalar& tau, Scalar* workspace) {
+  if (cols() == 1) {
+    *this *= Scalar(1) - tau;
+  } else if (!numext::is_exactly_zero(tau)) {
+    Map<typename internal::plain_col_type<PlainObject>::type> tmp(workspace, rows());
+    Block<Derived, Derived::RowsAtCompileTime, EssentialPart::SizeAtCompileTime> right(derived(), 0, 1, rows(),
+                                                                                       cols() - 1);
     tmp.noalias() = right * essential;
     tmp += this->col(0);
     this->col(0) -= tau * tmp;
@@ -171,6 +150,6 @@
   }
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_HOUSEHOLDER_H
+#endif  // EIGEN_HOUSEHOLDER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h
index 022f6c3..024c4a4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h
@@ -11,63 +11,65 @@
 #ifndef EIGEN_HOUSEHOLDER_SEQUENCE_H
 #define EIGEN_HOUSEHOLDER_SEQUENCE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \ingroup Householder_Module
-  * \householder_module
-  * \class HouseholderSequence
-  * \brief Sequence of Householder reflections acting on subspaces with decreasing size
-  * \tparam VectorsType type of matrix containing the Householder vectors
-  * \tparam CoeffsType  type of vector containing the Householder coefficients
-  * \tparam Side        either OnTheLeft (the default) or OnTheRight
-  *
-  * This class represents a product sequence of Householder reflections where the first Householder reflection
-  * acts on the whole space, the second Householder reflection leaves the one-dimensional subspace spanned by
-  * the first unit vector invariant, the third Householder reflection leaves the two-dimensional subspace
-  * spanned by the first two unit vectors invariant, and so on up to the last reflection which leaves all but
-  * one dimensions invariant and acts only on the last dimension. Such sequences of Householder reflections
-  * are used in several algorithms to zero out certain parts of a matrix. Indeed, the methods
-  * HessenbergDecomposition::matrixQ(), Tridiagonalization::matrixQ(), HouseholderQR::householderQ(),
-  * and ColPivHouseholderQR::householderQ() all return a %HouseholderSequence.
-  *
-  * More precisely, the class %HouseholderSequence represents an \f$ n \times n \f$ matrix \f$ H \f$ of the
-  * form \f$ H = \prod_{i=0}^{n-1} H_i \f$ where the i-th Householder reflection is \f$ H_i = I - h_i v_i
-  * v_i^* \f$. The i-th Householder coefficient \f$ h_i \f$ is a scalar and the i-th Householder vector \f$
-  * v_i \f$ is a vector of the form
-  * \f[
-  * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ].
-  * \f]
-  * The last \f$ n-i \f$ entries of \f$ v_i \f$ are called the essential part of the Householder vector.
-  *
-  * Typical usages are listed below, where H is a HouseholderSequence:
-  * \code
-  * A.applyOnTheRight(H);             // A = A * H
-  * A.applyOnTheLeft(H);              // A = H * A
-  * A.applyOnTheRight(H.adjoint());   // A = A * H^*
-  * A.applyOnTheLeft(H.adjoint());    // A = H^* * A
-  * MatrixXd Q = H;                   // conversion to a dense matrix
-  * \endcode
-  * In addition to the adjoint, you can also apply the inverse (=adjoint), the transpose, and the conjugate operators.
-  *
-  * See the documentation for HouseholderSequence(const VectorsType&, const CoeffsType&) for an example.
-  *
-  * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
-  */
+ * \householder_module
+ * \class HouseholderSequence
+ * \brief Sequence of Householder reflections acting on subspaces with decreasing size
+ * \tparam VectorsType type of matrix containing the Householder vectors
+ * \tparam CoeffsType  type of vector containing the Householder coefficients
+ * \tparam Side        either OnTheLeft (the default) or OnTheRight
+ *
+ * This class represents a product sequence of Householder reflections where the first Householder reflection
+ * acts on the whole space, the second Householder reflection leaves the one-dimensional subspace spanned by
+ * the first unit vector invariant, the third Householder reflection leaves the two-dimensional subspace
+ * spanned by the first two unit vectors invariant, and so on up to the last reflection which leaves all but
+ * one dimensions invariant and acts only on the last dimension. Such sequences of Householder reflections
+ * are used in several algorithms to zero out certain parts of a matrix. Indeed, the methods
+ * HessenbergDecomposition::matrixQ(), Tridiagonalization::matrixQ(), HouseholderQR::householderQ(),
+ * and ColPivHouseholderQR::householderQ() all return a %HouseholderSequence.
+ *
+ * More precisely, the class %HouseholderSequence represents an \f$ n \times n \f$ matrix \f$ H \f$ of the
+ * form \f$ H = \prod_{i=0}^{n-1} H_i \f$ where the i-th Householder reflection is \f$ H_i = I - h_i v_i
+ * v_i^* \f$. The i-th Householder coefficient \f$ h_i \f$ is a scalar and the i-th Householder vector \f$
+ * v_i \f$ is a vector of the form
+ * \f[
+ * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ].
+ * \f]
+ * The last \f$ n-i \f$ entries of \f$ v_i \f$ are called the essential part of the Householder vector.
+ *
+ * Typical usages are listed below, where H is a HouseholderSequence:
+ * \code
+ * A.applyOnTheRight(H);             // A = A * H
+ * A.applyOnTheLeft(H);              // A = H * A
+ * A.applyOnTheRight(H.adjoint());   // A = A * H^*
+ * A.applyOnTheLeft(H.adjoint());    // A = H^* * A
+ * MatrixXd Q = H;                   // conversion to a dense matrix
+ * \endcode
+ * In addition to the adjoint, you can also apply the inverse (=adjoint), the transpose, and the conjugate operators.
+ *
+ * See the documentation for HouseholderSequence(const VectorsType&, const CoeffsType&) for an example.
+ *
+ * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
+ */
 
 namespace internal {
 
-template<typename VectorsType, typename CoeffsType, int Side>
-struct traits<HouseholderSequence<VectorsType,CoeffsType,Side> >
-{
+template <typename VectorsType, typename CoeffsType, int Side>
+struct traits<HouseholderSequence<VectorsType, CoeffsType, Side> > {
   typedef typename VectorsType::Scalar Scalar;
   typedef typename VectorsType::StorageIndex StorageIndex;
   typedef typename VectorsType::StorageKind StorageKind;
   enum {
-    RowsAtCompileTime = Side==OnTheLeft ? traits<VectorsType>::RowsAtCompileTime
-                                        : traits<VectorsType>::ColsAtCompileTime,
+    RowsAtCompileTime =
+        Side == OnTheLeft ? traits<VectorsType>::RowsAtCompileTime : traits<VectorsType>::ColsAtCompileTime,
     ColsAtCompileTime = RowsAtCompileTime,
-    MaxRowsAtCompileTime = Side==OnTheLeft ? traits<VectorsType>::MaxRowsAtCompileTime
-                                           : traits<VectorsType>::MaxColsAtCompileTime,
+    MaxRowsAtCompileTime =
+        Side == OnTheLeft ? traits<VectorsType>::MaxRowsAtCompileTime : traits<VectorsType>::MaxColsAtCompileTime,
     MaxColsAtCompileTime = MaxRowsAtCompileTime,
     Flags = 0
   };
@@ -75,471 +77,426 @@
 
 struct HouseholderSequenceShape {};
 
-template<typename VectorsType, typename CoeffsType, int Side>
-struct evaluator_traits<HouseholderSequence<VectorsType,CoeffsType,Side> >
-  : public evaluator_traits_base<HouseholderSequence<VectorsType,CoeffsType,Side> >
-{
+template <typename VectorsType, typename CoeffsType, int Side>
+struct evaluator_traits<HouseholderSequence<VectorsType, CoeffsType, Side> >
+    : public evaluator_traits_base<HouseholderSequence<VectorsType, CoeffsType, Side> > {
   typedef HouseholderSequenceShape Shape;
 };
 
-template<typename VectorsType, typename CoeffsType, int Side>
-struct hseq_side_dependent_impl
-{
+template <typename VectorsType, typename CoeffsType, int Side>
+struct hseq_side_dependent_impl {
   typedef Block<const VectorsType, Dynamic, 1> EssentialVectorType;
   typedef HouseholderSequence<VectorsType, CoeffsType, OnTheLeft> HouseholderSequenceType;
-  static EIGEN_DEVICE_FUNC inline const EssentialVectorType essentialVector(const HouseholderSequenceType& h, Index k)
-  {
-    Index start = k+1+h.m_shift;
-    return Block<const VectorsType,Dynamic,1>(h.m_vectors, start, k, h.rows()-start, 1);
+  static EIGEN_DEVICE_FUNC inline const EssentialVectorType essentialVector(const HouseholderSequenceType& h, Index k) {
+    Index start = k + 1 + h.m_shift;
+    return Block<const VectorsType, Dynamic, 1>(h.m_vectors, start, k, h.rows() - start, 1);
   }
 };
 
-template<typename VectorsType, typename CoeffsType>
-struct hseq_side_dependent_impl<VectorsType, CoeffsType, OnTheRight>
-{
+template <typename VectorsType, typename CoeffsType>
+struct hseq_side_dependent_impl<VectorsType, CoeffsType, OnTheRight> {
   typedef Transpose<Block<const VectorsType, 1, Dynamic> > EssentialVectorType;
   typedef HouseholderSequence<VectorsType, CoeffsType, OnTheRight> HouseholderSequenceType;
-  static inline const EssentialVectorType essentialVector(const HouseholderSequenceType& h, Index k)
-  {
-    Index start = k+1+h.m_shift;
-    return Block<const VectorsType,1,Dynamic>(h.m_vectors, k, start, 1, h.rows()-start).transpose();
+  static inline const EssentialVectorType essentialVector(const HouseholderSequenceType& h, Index k) {
+    Index start = k + 1 + h.m_shift;
+    return Block<const VectorsType, 1, Dynamic>(h.m_vectors, k, start, 1, h.rows() - start).transpose();
   }
 };
 
-template<typename OtherScalarType, typename MatrixType> struct matrix_type_times_scalar_type
-{
-  typedef typename ScalarBinaryOpTraits<OtherScalarType, typename MatrixType::Scalar>::ReturnType
-    ResultScalar;
-  typedef Matrix<ResultScalar, MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime,
-                 0, MatrixType::MaxRowsAtCompileTime, MatrixType::MaxColsAtCompileTime> Type;
+template <typename OtherScalarType, typename MatrixType>
+struct matrix_type_times_scalar_type {
+  typedef typename ScalarBinaryOpTraits<OtherScalarType, typename MatrixType::Scalar>::ReturnType ResultScalar;
+  typedef Matrix<ResultScalar, MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime, 0,
+                 MatrixType::MaxRowsAtCompileTime, MatrixType::MaxColsAtCompileTime>
+      Type;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename VectorsType, typename CoeffsType, int Side> class HouseholderSequence
-  : public EigenBase<HouseholderSequence<VectorsType,CoeffsType,Side> >
-{
-    typedef typename internal::hseq_side_dependent_impl<VectorsType,CoeffsType,Side>::EssentialVectorType EssentialVectorType;
+template <typename VectorsType, typename CoeffsType, int Side>
+class HouseholderSequence : public EigenBase<HouseholderSequence<VectorsType, CoeffsType, Side> > {
+  typedef typename internal::hseq_side_dependent_impl<VectorsType, CoeffsType, Side>::EssentialVectorType
+      EssentialVectorType;
 
-  public:
-    enum {
-      RowsAtCompileTime = internal::traits<HouseholderSequence>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<HouseholderSequence>::ColsAtCompileTime,
-      MaxRowsAtCompileTime = internal::traits<HouseholderSequence>::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = internal::traits<HouseholderSequence>::MaxColsAtCompileTime
-    };
-    typedef typename internal::traits<HouseholderSequence>::Scalar Scalar;
+ public:
+  enum {
+    RowsAtCompileTime = internal::traits<HouseholderSequence>::RowsAtCompileTime,
+    ColsAtCompileTime = internal::traits<HouseholderSequence>::ColsAtCompileTime,
+    MaxRowsAtCompileTime = internal::traits<HouseholderSequence>::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = internal::traits<HouseholderSequence>::MaxColsAtCompileTime
+  };
+  typedef typename internal::traits<HouseholderSequence>::Scalar Scalar;
 
-    typedef HouseholderSequence<
-      typename internal::conditional<NumTraits<Scalar>::IsComplex,
-        typename internal::remove_all<typename VectorsType::ConjugateReturnType>::type,
-        VectorsType>::type,
-      typename internal::conditional<NumTraits<Scalar>::IsComplex,
-        typename internal::remove_all<typename CoeffsType::ConjugateReturnType>::type,
-        CoeffsType>::type,
-      Side
-    > ConjugateReturnType;
+  typedef HouseholderSequence<
+      std::conditional_t<NumTraits<Scalar>::IsComplex,
+                         internal::remove_all_t<typename VectorsType::ConjugateReturnType>, VectorsType>,
+      std::conditional_t<NumTraits<Scalar>::IsComplex, internal::remove_all_t<typename CoeffsType::ConjugateReturnType>,
+                         CoeffsType>,
+      Side>
+      ConjugateReturnType;
 
-    typedef HouseholderSequence<
+  typedef HouseholderSequence<
       VectorsType,
-      typename internal::conditional<NumTraits<Scalar>::IsComplex,
-        typename internal::remove_all<typename CoeffsType::ConjugateReturnType>::type,
-        CoeffsType>::type,
-      Side
-    > AdjointReturnType;
+      std::conditional_t<NumTraits<Scalar>::IsComplex, internal::remove_all_t<typename CoeffsType::ConjugateReturnType>,
+                         CoeffsType>,
+      Side>
+      AdjointReturnType;
 
-    typedef HouseholderSequence<
-      typename internal::conditional<NumTraits<Scalar>::IsComplex,
-        typename internal::remove_all<typename VectorsType::ConjugateReturnType>::type,
-        VectorsType>::type,
-      CoeffsType,
-      Side
-    > TransposeReturnType;
+  typedef HouseholderSequence<
+      std::conditional_t<NumTraits<Scalar>::IsComplex,
+                         internal::remove_all_t<typename VectorsType::ConjugateReturnType>, VectorsType>,
+      CoeffsType, Side>
+      TransposeReturnType;
 
-    typedef HouseholderSequence<
-      typename internal::add_const<VectorsType>::type,
-      typename internal::add_const<CoeffsType>::type,
-      Side
-    > ConstHouseholderSequence;
+  typedef HouseholderSequence<std::add_const_t<VectorsType>, std::add_const_t<CoeffsType>, Side>
+      ConstHouseholderSequence;
 
-    /** \brief Constructor.
-      * \param[in]  v      %Matrix containing the essential parts of the Householder vectors
-      * \param[in]  h      Vector containing the Householder coefficients
-      *
-      * Constructs the Householder sequence with coefficients given by \p h and vectors given by \p v. The
-      * i-th Householder coefficient \f$ h_i \f$ is given by \p h(i) and the essential part of the i-th
-      * Householder vector \f$ v_i \f$ is given by \p v(k,i) with \p k > \p i (the subdiagonal part of the
-      * i-th column). If \p v has fewer columns than rows, then the Householder sequence contains as many
-      * Householder reflections as there are columns.
-      *
-      * \note The %HouseholderSequence object stores \p v and \p h by reference.
-      *
-      * Example: \include HouseholderSequence_HouseholderSequence.cpp
-      * Output: \verbinclude HouseholderSequence_HouseholderSequence.out
-      *
-      * \sa setLength(), setShift()
-      */
-    EIGEN_DEVICE_FUNC
-    HouseholderSequence(const VectorsType& v, const CoeffsType& h)
-      : m_vectors(v), m_coeffs(h), m_reverse(false), m_length(v.diagonalSize()),
-        m_shift(0)
-    {
-    }
+  /** \brief Constructor.
+   * \param[in]  v      %Matrix containing the essential parts of the Householder vectors
+   * \param[in]  h      Vector containing the Householder coefficients
+   *
+   * Constructs the Householder sequence with coefficients given by \p h and vectors given by \p v. The
+   * i-th Householder coefficient \f$ h_i \f$ is given by \p h(i) and the essential part of the i-th
+   * Householder vector \f$ v_i \f$ is given by \p v(k,i) with \p k > \p i (the subdiagonal part of the
+   * i-th column). If \p v has fewer columns than rows, then the Householder sequence contains as many
+   * Householder reflections as there are columns.
+   *
+   * \note The %HouseholderSequence object stores \p v and \p h by reference.
+   *
+   * Example: \include HouseholderSequence_HouseholderSequence.cpp
+   * Output: \verbinclude HouseholderSequence_HouseholderSequence.out
+   *
+   * \sa setLength(), setShift()
+   */
+  EIGEN_DEVICE_FUNC HouseholderSequence(const VectorsType& v, const CoeffsType& h)
+      : m_vectors(v), m_coeffs(h), m_reverse(false), m_length(v.diagonalSize()), m_shift(0) {}
 
-    /** \brief Copy constructor. */
-    EIGEN_DEVICE_FUNC
-    HouseholderSequence(const HouseholderSequence& other)
+  /** \brief Copy constructor. */
+  EIGEN_DEVICE_FUNC HouseholderSequence(const HouseholderSequence& other)
       : m_vectors(other.m_vectors),
         m_coeffs(other.m_coeffs),
         m_reverse(other.m_reverse),
         m_length(other.m_length),
-        m_shift(other.m_shift)
-    {
-    }
+        m_shift(other.m_shift) {}
 
-    /** \brief Number of rows of transformation viewed as a matrix.
-      * \returns Number of rows
-      * \details This equals the dimension of the space that the transformation acts on.
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index rows() const EIGEN_NOEXCEPT { return Side==OnTheLeft ? m_vectors.rows() : m_vectors.cols(); }
+  /** \brief Number of rows of transformation viewed as a matrix.
+   * \returns Number of rows
+   * \details This equals the dimension of the space that the transformation acts on.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT {
+    return Side == OnTheLeft ? m_vectors.rows() : m_vectors.cols();
+  }
 
-    /** \brief Number of columns of transformation viewed as a matrix.
-      * \returns Number of columns
-      * \details This equals the dimension of the space that the transformation acts on.
-      */
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    Index cols() const EIGEN_NOEXCEPT { return rows(); }
+  /** \brief Number of columns of transformation viewed as a matrix.
+   * \returns Number of columns
+   * \details This equals the dimension of the space that the transformation acts on.
+   */
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return rows(); }
 
-    /** \brief Essential part of a Householder vector.
-      * \param[in]  k  Index of Householder reflection
-      * \returns    Vector containing non-trivial entries of k-th Householder vector
-      *
-      * This function returns the essential part of the Householder vector \f$ v_i \f$. This is a vector of
-      * length \f$ n-i \f$ containing the last \f$ n-i \f$ entries of the vector
-      * \f[
-      * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ].
-      * \f]
-      * The index \f$ i \f$ equals \p k + shift(), corresponding to the k-th column of the matrix \p v
-      * passed to the constructor.
-      *
-      * \sa setShift(), shift()
-      */
-    EIGEN_DEVICE_FUNC
-    const EssentialVectorType essentialVector(Index k) const
-    {
-      eigen_assert(k >= 0 && k < m_length);
-      return internal::hseq_side_dependent_impl<VectorsType,CoeffsType,Side>::essentialVector(*this, k);
-    }
+  /** \brief Essential part of a Householder vector.
+   * \param[in]  k  Index of Householder reflection
+   * \returns    Vector containing non-trivial entries of k-th Householder vector
+   *
+   * This function returns the essential part of the Householder vector \f$ v_i \f$. This is a vector of
+   * length \f$ n-i \f$ containing the last \f$ n-i \f$ entries of the vector
+   * \f[
+   * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ].
+   * \f]
+   * The index \f$ i \f$ equals \p k + shift(), corresponding to the k-th column of the matrix \p v
+   * passed to the constructor.
+   *
+   * \sa setShift(), shift()
+   */
+  EIGEN_DEVICE_FUNC const EssentialVectorType essentialVector(Index k) const {
+    eigen_assert(k >= 0 && k < m_length);
+    return internal::hseq_side_dependent_impl<VectorsType, CoeffsType, Side>::essentialVector(*this, k);
+  }
 
-    /** \brief %Transpose of the Householder sequence. */
-    TransposeReturnType transpose() const
-    {
-      return TransposeReturnType(m_vectors.conjugate(), m_coeffs)
-              .setReverseFlag(!m_reverse)
-              .setLength(m_length)
-              .setShift(m_shift);
-    }
+  /** \brief %Transpose of the Householder sequence. */
+  TransposeReturnType transpose() const {
+    return TransposeReturnType(m_vectors.conjugate(), m_coeffs)
+        .setReverseFlag(!m_reverse)
+        .setLength(m_length)
+        .setShift(m_shift);
+  }
 
-    /** \brief Complex conjugate of the Householder sequence. */
-    ConjugateReturnType conjugate() const
-    {
-      return ConjugateReturnType(m_vectors.conjugate(), m_coeffs.conjugate())
-             .setReverseFlag(m_reverse)
-             .setLength(m_length)
-             .setShift(m_shift);
-    }
+  /** \brief Complex conjugate of the Householder sequence. */
+  ConjugateReturnType conjugate() const {
+    return ConjugateReturnType(m_vectors.conjugate(), m_coeffs.conjugate())
+        .setReverseFlag(m_reverse)
+        .setLength(m_length)
+        .setShift(m_shift);
+  }
 
-    /** \returns an expression of the complex conjugate of \c *this if Cond==true,
-     *           returns \c *this otherwise.
-     */
-    template<bool Cond>
-    EIGEN_DEVICE_FUNC
-    inline typename internal::conditional<Cond,ConjugateReturnType,ConstHouseholderSequence>::type
-    conjugateIf() const
-    {
-      typedef typename internal::conditional<Cond,ConjugateReturnType,ConstHouseholderSequence>::type ReturnType;
-      return ReturnType(m_vectors.template conjugateIf<Cond>(), m_coeffs.template conjugateIf<Cond>());
-    }
+  /** \returns an expression of the complex conjugate of \c *this if Cond==true,
+   *           returns \c *this otherwise.
+   */
+  template <bool Cond>
+  EIGEN_DEVICE_FUNC inline std::conditional_t<Cond, ConjugateReturnType, ConstHouseholderSequence> conjugateIf() const {
+    typedef std::conditional_t<Cond, ConjugateReturnType, ConstHouseholderSequence> ReturnType;
+    return ReturnType(m_vectors.template conjugateIf<Cond>(), m_coeffs.template conjugateIf<Cond>());
+  }
 
-    /** \brief Adjoint (conjugate transpose) of the Householder sequence. */
-    AdjointReturnType adjoint() const
-    {
-      return AdjointReturnType(m_vectors, m_coeffs.conjugate())
-              .setReverseFlag(!m_reverse)
-              .setLength(m_length)
-              .setShift(m_shift);
-    }
+  /** \brief Adjoint (conjugate transpose) of the Householder sequence. */
+  AdjointReturnType adjoint() const {
+    return AdjointReturnType(m_vectors, m_coeffs.conjugate())
+        .setReverseFlag(!m_reverse)
+        .setLength(m_length)
+        .setShift(m_shift);
+  }
 
-    /** \brief Inverse of the Householder sequence (equals the adjoint). */
-    AdjointReturnType inverse() const { return adjoint(); }
+  /** \brief Inverse of the Householder sequence (equals the adjoint). */
+  AdjointReturnType inverse() const { return adjoint(); }
 
-    /** \internal */
-    template<typename DestType>
-    inline EIGEN_DEVICE_FUNC
-    void evalTo(DestType& dst) const
-    {
-      Matrix<Scalar, DestType::RowsAtCompileTime, 1,
-             AutoAlign|ColMajor, DestType::MaxRowsAtCompileTime, 1> workspace(rows());
-      evalTo(dst, workspace);
-    }
+  /** \internal */
+  template <typename DestType>
+  inline EIGEN_DEVICE_FUNC void evalTo(DestType& dst) const {
+    Matrix<Scalar, DestType::RowsAtCompileTime, 1, AutoAlign | ColMajor, DestType::MaxRowsAtCompileTime, 1> workspace(
+        rows());
+    evalTo(dst, workspace);
+  }
 
-    /** \internal */
-    template<typename Dest, typename Workspace>
-    EIGEN_DEVICE_FUNC
-    void evalTo(Dest& dst, Workspace& workspace) const
-    {
-      workspace.resize(rows());
-      Index vecs = m_length;
-      if(internal::is_same_dense(dst,m_vectors))
-      {
-        // in-place
-        dst.diagonal().setOnes();
-        dst.template triangularView<StrictlyUpper>().setZero();
-        for(Index k = vecs-1; k >= 0; --k)
-        {
-          Index cornerSize = rows() - k - m_shift;
-          if(m_reverse)
-            dst.bottomRightCorner(cornerSize, cornerSize)
-               .applyHouseholderOnTheRight(essentialVector(k), m_coeffs.coeff(k), workspace.data());
-          else
-            dst.bottomRightCorner(cornerSize, cornerSize)
-               .applyHouseholderOnTheLeft(essentialVector(k), m_coeffs.coeff(k), workspace.data());
-
-          // clear the off diagonal vector
-          dst.col(k).tail(rows()-k-1).setZero();
-        }
-        // clear the remaining columns if needed
-        for(Index k = 0; k<cols()-vecs ; ++k)
-          dst.col(k).tail(rows()-k-1).setZero();
-      }
-      else if(m_length>BlockSize)
-      {
-        dst.setIdentity(rows(), rows());
-        if(m_reverse)
-          applyThisOnTheLeft(dst,workspace,true);
+  /** \internal */
+  template <typename Dest, typename Workspace>
+  EIGEN_DEVICE_FUNC void evalTo(Dest& dst, Workspace& workspace) const {
+    workspace.resize(rows());
+    Index vecs = m_length;
+    if (internal::is_same_dense(dst, m_vectors)) {
+      // in-place
+      dst.diagonal().setOnes();
+      dst.template triangularView<StrictlyUpper>().setZero();
+      for (Index k = vecs - 1; k >= 0; --k) {
+        Index cornerSize = rows() - k - m_shift;
+        if (m_reverse)
+          dst.bottomRightCorner(cornerSize, cornerSize)
+              .applyHouseholderOnTheRight(essentialVector(k), m_coeffs.coeff(k), workspace.data());
         else
-          applyThisOnTheLeft(dst,workspace,true);
+          dst.bottomRightCorner(cornerSize, cornerSize)
+              .applyHouseholderOnTheLeft(essentialVector(k), m_coeffs.coeff(k), workspace.data());
+
+        // clear the off diagonal vector
+        dst.col(k).tail(rows() - k - 1).setZero();
       }
+      // clear the remaining columns if needed
+      for (Index k = 0; k < cols() - vecs; ++k) dst.col(k).tail(rows() - k - 1).setZero();
+    } else if (m_length > BlockSize) {
+      dst.setIdentity(rows(), rows());
+      if (m_reverse)
+        applyThisOnTheLeft(dst, workspace, true);
       else
-      {
-        dst.setIdentity(rows(), rows());
-        for(Index k = vecs-1; k >= 0; --k)
-        {
-          Index cornerSize = rows() - k - m_shift;
-          if(m_reverse)
-            dst.bottomRightCorner(cornerSize, cornerSize)
-               .applyHouseholderOnTheRight(essentialVector(k), m_coeffs.coeff(k), workspace.data());
-          else
-            dst.bottomRightCorner(cornerSize, cornerSize)
-               .applyHouseholderOnTheLeft(essentialVector(k), m_coeffs.coeff(k), workspace.data());
-        }
+        applyThisOnTheLeft(dst, workspace, true);
+    } else {
+      dst.setIdentity(rows(), rows());
+      for (Index k = vecs - 1; k >= 0; --k) {
+        Index cornerSize = rows() - k - m_shift;
+        if (m_reverse)
+          dst.bottomRightCorner(cornerSize, cornerSize)
+              .applyHouseholderOnTheRight(essentialVector(k), m_coeffs.coeff(k), workspace.data());
+        else
+          dst.bottomRightCorner(cornerSize, cornerSize)
+              .applyHouseholderOnTheLeft(essentialVector(k), m_coeffs.coeff(k), workspace.data());
       }
     }
+  }
 
-    /** \internal */
-    template<typename Dest> inline void applyThisOnTheRight(Dest& dst) const
-    {
-      Matrix<Scalar,1,Dest::RowsAtCompileTime,RowMajor,1,Dest::MaxRowsAtCompileTime> workspace(dst.rows());
-      applyThisOnTheRight(dst, workspace);
+  /** \internal */
+  template <typename Dest>
+  inline void applyThisOnTheRight(Dest& dst) const {
+    Matrix<Scalar, 1, Dest::RowsAtCompileTime, RowMajor, 1, Dest::MaxRowsAtCompileTime> workspace(dst.rows());
+    applyThisOnTheRight(dst, workspace);
+  }
+
+  /** \internal */
+  template <typename Dest, typename Workspace>
+  inline void applyThisOnTheRight(Dest& dst, Workspace& workspace) const {
+    workspace.resize(dst.rows());
+    for (Index k = 0; k < m_length; ++k) {
+      Index actual_k = m_reverse ? m_length - k - 1 : k;
+      dst.rightCols(rows() - m_shift - actual_k)
+          .applyHouseholderOnTheRight(essentialVector(actual_k), m_coeffs.coeff(actual_k), workspace.data());
     }
+  }
 
-    /** \internal */
-    template<typename Dest, typename Workspace>
-    inline void applyThisOnTheRight(Dest& dst, Workspace& workspace) const
-    {
-      workspace.resize(dst.rows());
-      for(Index k = 0; k < m_length; ++k)
-      {
-        Index actual_k = m_reverse ? m_length-k-1 : k;
-        dst.rightCols(rows()-m_shift-actual_k)
-           .applyHouseholderOnTheRight(essentialVector(actual_k), m_coeffs.coeff(actual_k), workspace.data());
-      }
-    }
+  /** \internal */
+  template <typename Dest>
+  inline void applyThisOnTheLeft(Dest& dst, bool inputIsIdentity = false) const {
+    Matrix<Scalar, 1, Dest::ColsAtCompileTime, RowMajor, 1, Dest::MaxColsAtCompileTime> workspace;
+    applyThisOnTheLeft(dst, workspace, inputIsIdentity);
+  }
 
-    /** \internal */
-    template<typename Dest> inline void applyThisOnTheLeft(Dest& dst, bool inputIsIdentity = false) const
-    {
-      Matrix<Scalar,1,Dest::ColsAtCompileTime,RowMajor,1,Dest::MaxColsAtCompileTime> workspace;
-      applyThisOnTheLeft(dst, workspace, inputIsIdentity);
-    }
+  /** \internal */
+  template <typename Dest, typename Workspace>
+  inline void applyThisOnTheLeft(Dest& dst, Workspace& workspace, bool inputIsIdentity = false) const {
+    if (inputIsIdentity && m_reverse) inputIsIdentity = false;
+    // if the entries are large enough, then apply the reflectors by block
+    if (m_length >= BlockSize && dst.cols() > 1) {
+      // Make sure we have at least 2 useful blocks, otherwise it is point-less:
+      Index blockSize = m_length < Index(2 * BlockSize) ? (m_length + 1) / 2 : Index(BlockSize);
+      for (Index i = 0; i < m_length; i += blockSize) {
+        Index end = m_reverse ? (std::min)(m_length, i + blockSize) : m_length - i;
+        Index k = m_reverse ? i : (std::max)(Index(0), end - blockSize);
+        Index bs = end - k;
+        Index start = k + m_shift;
 
-    /** \internal */
-    template<typename Dest, typename Workspace>
-    inline void applyThisOnTheLeft(Dest& dst, Workspace& workspace, bool inputIsIdentity = false) const
-    {
-      if(inputIsIdentity && m_reverse)
-        inputIsIdentity = false;
-      // if the entries are large enough, then apply the reflectors by block
-      if(m_length>=BlockSize && dst.cols()>1)
-      {
-        // Make sure we have at least 2 useful blocks, otherwise it is point-less:
-        Index blockSize = m_length<Index(2*BlockSize) ? (m_length+1)/2 : Index(BlockSize);
-        for(Index i = 0; i < m_length; i+=blockSize)
-        {
-          Index end = m_reverse ? (std::min)(m_length,i+blockSize) : m_length-i;
-          Index k = m_reverse ? i : (std::max)(Index(0),end-blockSize);
-          Index bs = end-k;
-          Index start = k + m_shift;
+        typedef Block<internal::remove_all_t<VectorsType>, Dynamic, Dynamic> SubVectorsType;
+        SubVectorsType sub_vecs1(m_vectors.const_cast_derived(), Side == OnTheRight ? k : start,
+                                 Side == OnTheRight ? start : k, Side == OnTheRight ? bs : m_vectors.rows() - start,
+                                 Side == OnTheRight ? m_vectors.cols() - start : bs);
+        std::conditional_t<Side == OnTheRight, Transpose<SubVectorsType>, SubVectorsType&> sub_vecs(sub_vecs1);
 
-          typedef Block<typename internal::remove_all<VectorsType>::type,Dynamic,Dynamic> SubVectorsType;
-          SubVectorsType sub_vecs1(m_vectors.const_cast_derived(), Side==OnTheRight ? k : start,
-                                                                   Side==OnTheRight ? start : k,
-                                                                   Side==OnTheRight ? bs : m_vectors.rows()-start,
-                                                                   Side==OnTheRight ? m_vectors.cols()-start : bs);
-          typename internal::conditional<Side==OnTheRight, Transpose<SubVectorsType>, SubVectorsType&>::type sub_vecs(sub_vecs1);
+        Index dstRows = rows() - m_shift - k;
 
-          Index dstStart = dst.rows()-rows()+m_shift+k;
-          Index dstRows  = rows()-m_shift-k;
-          Block<Dest,Dynamic,Dynamic> sub_dst(dst,
-                                              dstStart,
-                                              inputIsIdentity ? dstStart : 0,
-                                              dstRows,
-                                              inputIsIdentity ? dstRows : dst.cols());
+        if (inputIsIdentity) {
+          Block<Dest, Dynamic, Dynamic> sub_dst = dst.bottomRightCorner(dstRows, dstRows);
+          apply_block_householder_on_the_left(sub_dst, sub_vecs, m_coeffs.segment(k, bs), !m_reverse);
+        } else {
+          auto sub_dst = dst.bottomRows(dstRows);
           apply_block_householder_on_the_left(sub_dst, sub_vecs, m_coeffs.segment(k, bs), !m_reverse);
         }
       }
-      else
-      {
-        workspace.resize(dst.cols());
-        for(Index k = 0; k < m_length; ++k)
-        {
-          Index actual_k = m_reverse ? k : m_length-k-1;
-          Index dstStart = rows()-m_shift-actual_k;
-          dst.bottomRightCorner(dstStart, inputIsIdentity ? dstStart : dst.cols())
-            .applyHouseholderOnTheLeft(essentialVector(actual_k), m_coeffs.coeff(actual_k), workspace.data());
+    } else {
+      workspace.resize(dst.cols());
+      for (Index k = 0; k < m_length; ++k) {
+        Index actual_k = m_reverse ? k : m_length - k - 1;
+        Index dstRows = rows() - m_shift - actual_k;
+
+        if (inputIsIdentity) {
+          Block<Dest, Dynamic, Dynamic> sub_dst = dst.bottomRightCorner(dstRows, dstRows);
+          sub_dst.applyHouseholderOnTheLeft(essentialVector(actual_k), m_coeffs.coeff(actual_k), workspace.data());
+        } else {
+          auto sub_dst = dst.bottomRows(dstRows);
+          sub_dst.applyHouseholderOnTheLeft(essentialVector(actual_k), m_coeffs.coeff(actual_k), workspace.data());
         }
       }
     }
+  }
 
-    /** \brief Computes the product of a Householder sequence with a matrix.
-      * \param[in]  other  %Matrix being multiplied.
-      * \returns    Expression object representing the product.
-      *
-      * This function computes \f$ HM \f$ where \f$ H \f$ is the Householder sequence represented by \p *this
-      * and \f$ M \f$ is the matrix \p other.
-      */
-    template<typename OtherDerived>
-    typename internal::matrix_type_times_scalar_type<Scalar, OtherDerived>::Type operator*(const MatrixBase<OtherDerived>& other) const
-    {
-      typename internal::matrix_type_times_scalar_type<Scalar, OtherDerived>::Type
-        res(other.template cast<typename internal::matrix_type_times_scalar_type<Scalar,OtherDerived>::ResultScalar>());
-      applyThisOnTheLeft(res, internal::is_identity<OtherDerived>::value && res.rows()==res.cols());
-      return res;
-    }
+  /** \brief Computes the product of a Householder sequence with a matrix.
+   * \param[in]  other  %Matrix being multiplied.
+   * \returns    Expression object representing the product.
+   *
+   * This function computes \f$ HM \f$ where \f$ H \f$ is the Householder sequence represented by \p *this
+   * and \f$ M \f$ is the matrix \p other.
+   */
+  template <typename OtherDerived>
+  typename internal::matrix_type_times_scalar_type<Scalar, OtherDerived>::Type operator*(
+      const MatrixBase<OtherDerived>& other) const {
+    typename internal::matrix_type_times_scalar_type<Scalar, OtherDerived>::Type res(
+        other.template cast<typename internal::matrix_type_times_scalar_type<Scalar, OtherDerived>::ResultScalar>());
+    applyThisOnTheLeft(res, internal::is_identity<OtherDerived>::value && res.rows() == res.cols());
+    return res;
+  }
 
-    template<typename _VectorsType, typename _CoeffsType, int _Side> friend struct internal::hseq_side_dependent_impl;
+  template <typename VectorsType_, typename CoeffsType_, int Side_>
+  friend struct internal::hseq_side_dependent_impl;
 
-    /** \brief Sets the length of the Householder sequence.
-      * \param [in]  length  New value for the length.
-      *
-      * By default, the length \f$ n \f$ of the Householder sequence \f$ H = H_0 H_1 \ldots H_{n-1} \f$ is set
-      * to the number of columns of the matrix \p v passed to the constructor, or the number of rows if that
-      * is smaller. After this function is called, the length equals \p length.
-      *
-      * \sa length()
-      */
-    EIGEN_DEVICE_FUNC
-    HouseholderSequence& setLength(Index length)
-    {
-      m_length = length;
-      return *this;
-    }
+  /** \brief Sets the length of the Householder sequence.
+   * \param [in]  length  New value for the length.
+   *
+   * By default, the length \f$ n \f$ of the Householder sequence \f$ H = H_0 H_1 \ldots H_{n-1} \f$ is set
+   * to the number of columns of the matrix \p v passed to the constructor, or the number of rows if that
+   * is smaller. After this function is called, the length equals \p length.
+   *
+   * \sa length()
+   */
+  EIGEN_DEVICE_FUNC HouseholderSequence& setLength(Index length) {
+    m_length = length;
+    return *this;
+  }
 
-    /** \brief Sets the shift of the Householder sequence.
-      * \param [in]  shift  New value for the shift.
-      *
-      * By default, a %HouseholderSequence object represents \f$ H = H_0 H_1 \ldots H_{n-1} \f$ and the i-th
-      * column of the matrix \p v passed to the constructor corresponds to the i-th Householder
-      * reflection. After this function is called, the object represents \f$ H = H_{\mathrm{shift}}
-      * H_{\mathrm{shift}+1} \ldots H_{n-1} \f$ and the i-th column of \p v corresponds to the (shift+i)-th
-      * Householder reflection.
-      *
-      * \sa shift()
-      */
-    EIGEN_DEVICE_FUNC
-    HouseholderSequence& setShift(Index shift)
-    {
-      m_shift = shift;
-      return *this;
-    }
+  /** \brief Sets the shift of the Householder sequence.
+   * \param [in]  shift  New value for the shift.
+   *
+   * By default, a %HouseholderSequence object represents \f$ H = H_0 H_1 \ldots H_{n-1} \f$ and the i-th
+   * column of the matrix \p v passed to the constructor corresponds to the i-th Householder
+   * reflection. After this function is called, the object represents \f$ H = H_{\mathrm{shift}}
+   * H_{\mathrm{shift}+1} \ldots H_{n-1} \f$ and the i-th column of \p v corresponds to the (shift+i)-th
+   * Householder reflection.
+   *
+   * \sa shift()
+   */
+  EIGEN_DEVICE_FUNC HouseholderSequence& setShift(Index shift) {
+    m_shift = shift;
+    return *this;
+  }
 
-    EIGEN_DEVICE_FUNC
-    Index length() const { return m_length; }  /**< \brief Returns the length of the Householder sequence. */
+  EIGEN_DEVICE_FUNC Index length() const {
+    return m_length;
+  } /**< \brief Returns the length of the Householder sequence. */
 
-    EIGEN_DEVICE_FUNC
-    Index shift() const { return m_shift; }    /**< \brief Returns the shift of the Householder sequence. */
+  EIGEN_DEVICE_FUNC Index shift() const {
+    return m_shift;
+  } /**< \brief Returns the shift of the Householder sequence. */
 
-    /* Necessary for .adjoint() and .conjugate() */
-    template <typename VectorsType2, typename CoeffsType2, int Side2> friend class HouseholderSequence;
+  /* Necessary for .adjoint() and .conjugate() */
+  template <typename VectorsType2, typename CoeffsType2, int Side2>
+  friend class HouseholderSequence;
 
-  protected:
+ protected:
+  /** \internal
+   * \brief Sets the reverse flag.
+   * \param [in]  reverse  New value of the reverse flag.
+   *
+   * By default, the reverse flag is not set. If the reverse flag is set, then this object represents
+   * \f$ H^r = H_{n-1} \ldots H_1 H_0 \f$ instead of \f$ H = H_0 H_1 \ldots H_{n-1} \f$.
+   * \note For real valued HouseholderSequence this is equivalent to transposing \f$ H \f$.
+   *
+   * \sa reverseFlag(), transpose(), adjoint()
+   */
+  HouseholderSequence& setReverseFlag(bool reverse) {
+    m_reverse = reverse;
+    return *this;
+  }
 
-    /** \internal
-      * \brief Sets the reverse flag.
-      * \param [in]  reverse  New value of the reverse flag.
-      *
-      * By default, the reverse flag is not set. If the reverse flag is set, then this object represents
-      * \f$ H^r = H_{n-1} \ldots H_1 H_0 \f$ instead of \f$ H = H_0 H_1 \ldots H_{n-1} \f$.
-      * \note For real valued HouseholderSequence this is equivalent to transposing \f$ H \f$.
-      *
-      * \sa reverseFlag(), transpose(), adjoint()
-      */
-    HouseholderSequence& setReverseFlag(bool reverse)
-    {
-      m_reverse = reverse;
-      return *this;
-    }
+  bool reverseFlag() const { return m_reverse; } /**< \internal \brief Returns the reverse flag. */
 
-    bool reverseFlag() const { return m_reverse; }     /**< \internal \brief Returns the reverse flag. */
-
-    typename VectorsType::Nested m_vectors;
-    typename CoeffsType::Nested m_coeffs;
-    bool m_reverse;
-    Index m_length;
-    Index m_shift;
-    enum { BlockSize = 48 };
+  typename VectorsType::Nested m_vectors;
+  typename CoeffsType::Nested m_coeffs;
+  bool m_reverse;
+  Index m_length;
+  Index m_shift;
+  enum { BlockSize = 48 };
 };
 
 /** \brief Computes the product of a matrix with a Householder sequence.
-  * \param[in]  other  %Matrix being multiplied.
-  * \param[in]  h      %HouseholderSequence being multiplied.
-  * \returns    Expression object representing the product.
-  *
-  * This function computes \f$ MH \f$ where \f$ M \f$ is the matrix \p other and \f$ H \f$ is the
-  * Householder sequence represented by \p h.
-  */
-template<typename OtherDerived, typename VectorsType, typename CoeffsType, int Side>
-typename internal::matrix_type_times_scalar_type<typename VectorsType::Scalar,OtherDerived>::Type operator*(const MatrixBase<OtherDerived>& other, const HouseholderSequence<VectorsType,CoeffsType,Side>& h)
-{
-  typename internal::matrix_type_times_scalar_type<typename VectorsType::Scalar,OtherDerived>::Type
-    res(other.template cast<typename internal::matrix_type_times_scalar_type<typename VectorsType::Scalar,OtherDerived>::ResultScalar>());
+ * \param[in]  other  %Matrix being multiplied.
+ * \param[in]  h      %HouseholderSequence being multiplied.
+ * \returns    Expression object representing the product.
+ *
+ * This function computes \f$ MH \f$ where \f$ M \f$ is the matrix \p other and \f$ H \f$ is the
+ * Householder sequence represented by \p h.
+ */
+template <typename OtherDerived, typename VectorsType, typename CoeffsType, int Side>
+typename internal::matrix_type_times_scalar_type<typename VectorsType::Scalar, OtherDerived>::Type operator*(
+    const MatrixBase<OtherDerived>& other, const HouseholderSequence<VectorsType, CoeffsType, Side>& h) {
+  typename internal::matrix_type_times_scalar_type<typename VectorsType::Scalar, OtherDerived>::Type res(
+      other.template cast<typename internal::matrix_type_times_scalar_type<typename VectorsType::Scalar,
+                                                                           OtherDerived>::ResultScalar>());
   h.applyThisOnTheRight(res);
   return res;
 }
 
 /** \ingroup Householder_Module \householder_module
-  * \brief Convenience function for constructing a Householder sequence.
-  * \returns A HouseholderSequence constructed from the specified arguments.
-  */
-template<typename VectorsType, typename CoeffsType>
-HouseholderSequence<VectorsType,CoeffsType> householderSequence(const VectorsType& v, const CoeffsType& h)
-{
-  return HouseholderSequence<VectorsType,CoeffsType,OnTheLeft>(v, h);
+ * \brief Convenience function for constructing a Householder sequence.
+ * \returns A HouseholderSequence constructed from the specified arguments.
+ */
+template <typename VectorsType, typename CoeffsType>
+HouseholderSequence<VectorsType, CoeffsType> householderSequence(const VectorsType& v, const CoeffsType& h) {
+  return HouseholderSequence<VectorsType, CoeffsType, OnTheLeft>(v, h);
 }
 
 /** \ingroup Householder_Module \householder_module
-  * \brief Convenience function for constructing a Householder sequence.
-  * \returns A HouseholderSequence constructed from the specified arguments.
-  * \details This function differs from householderSequence() in that the template argument \p OnTheSide of
-  * the constructed HouseholderSequence is set to OnTheRight, instead of the default OnTheLeft.
-  */
-template<typename VectorsType, typename CoeffsType>
-HouseholderSequence<VectorsType,CoeffsType,OnTheRight> rightHouseholderSequence(const VectorsType& v, const CoeffsType& h)
-{
-  return HouseholderSequence<VectorsType,CoeffsType,OnTheRight>(v, h);
+ * \brief Convenience function for constructing a Householder sequence.
+ * \returns A HouseholderSequence constructed from the specified arguments.
+ * \details This function differs from householderSequence() in that the template argument \p OnTheSide of
+ * the constructed HouseholderSequence is set to OnTheRight, instead of the default OnTheLeft.
+ */
+template <typename VectorsType, typename CoeffsType>
+HouseholderSequence<VectorsType, CoeffsType, OnTheRight> rightHouseholderSequence(const VectorsType& v,
+                                                                                  const CoeffsType& h) {
+  return HouseholderSequence<VectorsType, CoeffsType, OnTheRight>(v, h);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_HOUSEHOLDER_SEQUENCE_H
+#endif  // EIGEN_HOUSEHOLDER_SEQUENCE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/InternalHeaderCheck.h
new file mode 100644
index 0000000..70de89b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_HOUSEHOLDER_MODULE_H
+#error "Please include Eigen/Householder instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
index a117fc1..0beef60 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
@@ -10,6 +10,9 @@
 #ifndef EIGEN_BASIC_PRECONDITIONERS_H
 #define EIGEN_BASIC_PRECONDITIONERS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \ingroup IterativeLinearSolvers_Module
@@ -21,7 +24,7 @@
     A.diagonal().asDiagonal() . x = b
     \endcode
   *
-  * \tparam _Scalar the type of the scalar.
+  * \tparam Scalar_ the type of the scalar.
   *
   * \implsparsesolverconcept
   *
@@ -32,79 +35,69 @@
   *
   * \sa class LeastSquareDiagonalPreconditioner, class ConjugateGradient
   */
-template <typename _Scalar>
-class DiagonalPreconditioner
-{
-    typedef _Scalar Scalar;
-    typedef Matrix<Scalar,Dynamic,1> Vector;
-  public:
-    typedef typename Vector::StorageIndex StorageIndex;
-    enum {
-      ColsAtCompileTime = Dynamic,
-      MaxColsAtCompileTime = Dynamic
-    };
+template <typename Scalar_>
+class DiagonalPreconditioner {
+  typedef Scalar_ Scalar;
+  typedef Matrix<Scalar, Dynamic, 1> Vector;
 
-    DiagonalPreconditioner() : m_isInitialized(false) {}
+ public:
+  typedef typename Vector::StorageIndex StorageIndex;
+  enum { ColsAtCompileTime = Dynamic, MaxColsAtCompileTime = Dynamic };
 
-    template<typename MatType>
-    explicit DiagonalPreconditioner(const MatType& mat) : m_invdiag(mat.cols())
-    {
-      compute(mat);
+  DiagonalPreconditioner() : m_isInitialized(false) {}
+
+  template <typename MatType>
+  explicit DiagonalPreconditioner(const MatType& mat) : m_invdiag(mat.cols()) {
+    compute(mat);
+  }
+
+  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
+  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
+
+  template <typename MatType>
+  DiagonalPreconditioner& analyzePattern(const MatType&) {
+    return *this;
+  }
+
+  template <typename MatType>
+  DiagonalPreconditioner& factorize(const MatType& mat) {
+    m_invdiag.resize(mat.cols());
+    for (int j = 0; j < mat.outerSize(); ++j) {
+      typename MatType::InnerIterator it(mat, j);
+      while (it && it.index() != j) ++it;
+      if (it && it.index() == j && it.value() != Scalar(0))
+        m_invdiag(j) = Scalar(1) / it.value();
+      else
+        m_invdiag(j) = Scalar(1);
     }
+    m_isInitialized = true;
+    return *this;
+  }
 
-    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
-    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
+  template <typename MatType>
+  DiagonalPreconditioner& compute(const MatType& mat) {
+    return factorize(mat);
+  }
 
-    template<typename MatType>
-    DiagonalPreconditioner& analyzePattern(const MatType& )
-    {
-      return *this;
-    }
+  /** \internal */
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const Rhs& b, Dest& x) const {
+    x = m_invdiag.array() * b.array();
+  }
 
-    template<typename MatType>
-    DiagonalPreconditioner& factorize(const MatType& mat)
-    {
-      m_invdiag.resize(mat.cols());
-      for(int j=0; j<mat.outerSize(); ++j)
-      {
-        typename MatType::InnerIterator it(mat,j);
-        while(it && it.index()!=j) ++it;
-        if(it && it.index()==j && it.value()!=Scalar(0))
-          m_invdiag(j) = Scalar(1)/it.value();
-        else
-          m_invdiag(j) = Scalar(1);
-      }
-      m_isInitialized = true;
-      return *this;
-    }
+  template <typename Rhs>
+  inline const Solve<DiagonalPreconditioner, Rhs> solve(const MatrixBase<Rhs>& b) const {
+    eigen_assert(m_isInitialized && "DiagonalPreconditioner is not initialized.");
+    eigen_assert(m_invdiag.size() == b.rows() &&
+                 "DiagonalPreconditioner::solve(): invalid number of rows of the right hand side matrix b");
+    return Solve<DiagonalPreconditioner, Rhs>(*this, b.derived());
+  }
 
-    template<typename MatType>
-    DiagonalPreconditioner& compute(const MatType& mat)
-    {
-      return factorize(mat);
-    }
+  ComputationInfo info() { return Success; }
 
-    /** \internal */
-    template<typename Rhs, typename Dest>
-    void _solve_impl(const Rhs& b, Dest& x) const
-    {
-      x = m_invdiag.array() * b.array() ;
-    }
-
-    template<typename Rhs> inline const Solve<DiagonalPreconditioner, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "DiagonalPreconditioner is not initialized.");
-      eigen_assert(m_invdiag.size()==b.rows()
-                && "DiagonalPreconditioner::solve(): invalid number of rows of the right hand side matrix b");
-      return Solve<DiagonalPreconditioner, Rhs>(*this, b.derived());
-    }
-
-    ComputationInfo info() { return Success; }
-
-  protected:
-    Vector m_invdiag;
-    bool m_isInitialized;
+ protected:
+  Vector m_invdiag;
+  bool m_isInitialized;
 };
 
 /** \ingroup IterativeLinearSolvers_Module
@@ -116,7 +109,7 @@
     (A.adjoint() * A).diagonal().asDiagonal() * x = b
     \endcode
   *
-  * \tparam _Scalar the type of the scalar.
+  * \tparam Scalar_ the type of the scalar.
   *
   * \implsparsesolverconcept
   *
@@ -124,103 +117,97 @@
   *
   * \sa class LeastSquaresConjugateGradient, class DiagonalPreconditioner
   */
-template <typename _Scalar>
-class LeastSquareDiagonalPreconditioner : public DiagonalPreconditioner<_Scalar>
-{
-    typedef _Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef DiagonalPreconditioner<_Scalar> Base;
-    using Base::m_invdiag;
-  public:
+template <typename Scalar_>
+class LeastSquareDiagonalPreconditioner : public DiagonalPreconditioner<Scalar_> {
+  typedef Scalar_ Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef DiagonalPreconditioner<Scalar_> Base;
+  using Base::m_invdiag;
 
-    LeastSquareDiagonalPreconditioner() : Base() {}
+ public:
+  LeastSquareDiagonalPreconditioner() : Base() {}
 
-    template<typename MatType>
-    explicit LeastSquareDiagonalPreconditioner(const MatType& mat) : Base()
-    {
-      compute(mat);
-    }
+  template <typename MatType>
+  explicit LeastSquareDiagonalPreconditioner(const MatType& mat) : Base() {
+    compute(mat);
+  }
 
-    template<typename MatType>
-    LeastSquareDiagonalPreconditioner& analyzePattern(const MatType& )
-    {
-      return *this;
-    }
+  template <typename MatType>
+  LeastSquareDiagonalPreconditioner& analyzePattern(const MatType&) {
+    return *this;
+  }
 
-    template<typename MatType>
-    LeastSquareDiagonalPreconditioner& factorize(const MatType& mat)
-    {
-      // Compute the inverse squared-norm of each column of mat
-      m_invdiag.resize(mat.cols());
-      if(MatType::IsRowMajor)
-      {
-        m_invdiag.setZero();
-        for(Index j=0; j<mat.outerSize(); ++j)
-        {
-          for(typename MatType::InnerIterator it(mat,j); it; ++it)
-            m_invdiag(it.index()) += numext::abs2(it.value());
-        }
-        for(Index j=0; j<mat.cols(); ++j)
-          if(numext::real(m_invdiag(j))>RealScalar(0))
-            m_invdiag(j) = RealScalar(1)/numext::real(m_invdiag(j));
+  template <typename MatType>
+  LeastSquareDiagonalPreconditioner& factorize(const MatType& mat) {
+    // Compute the inverse squared-norm of each column of mat
+    m_invdiag.resize(mat.cols());
+    if (MatType::IsRowMajor) {
+      m_invdiag.setZero();
+      for (Index j = 0; j < mat.outerSize(); ++j) {
+        for (typename MatType::InnerIterator it(mat, j); it; ++it) m_invdiag(it.index()) += numext::abs2(it.value());
       }
-      else
-      {
-        for(Index j=0; j<mat.outerSize(); ++j)
-        {
-          RealScalar sum = mat.col(j).squaredNorm();
-          if(sum>RealScalar(0))
-            m_invdiag(j) = RealScalar(1)/sum;
-          else
-            m_invdiag(j) = RealScalar(1);
-        }
+      for (Index j = 0; j < mat.cols(); ++j)
+        if (numext::real(m_invdiag(j)) > RealScalar(0)) m_invdiag(j) = RealScalar(1) / numext::real(m_invdiag(j));
+    } else {
+      for (Index j = 0; j < mat.outerSize(); ++j) {
+        RealScalar sum = mat.col(j).squaredNorm();
+        if (sum > RealScalar(0))
+          m_invdiag(j) = RealScalar(1) / sum;
+        else
+          m_invdiag(j) = RealScalar(1);
       }
-      Base::m_isInitialized = true;
-      return *this;
     }
+    Base::m_isInitialized = true;
+    return *this;
+  }
 
-    template<typename MatType>
-    LeastSquareDiagonalPreconditioner& compute(const MatType& mat)
-    {
-      return factorize(mat);
-    }
+  template <typename MatType>
+  LeastSquareDiagonalPreconditioner& compute(const MatType& mat) {
+    return factorize(mat);
+  }
 
-    ComputationInfo info() { return Success; }
+  ComputationInfo info() { return Success; }
 
-  protected:
+ protected:
 };
 
 /** \ingroup IterativeLinearSolvers_Module
-  * \brief A naive preconditioner which approximates any matrix as the identity matrix
-  *
-  * \implsparsesolverconcept
-  *
-  * \sa class DiagonalPreconditioner
-  */
-class IdentityPreconditioner
-{
-  public:
+ * \brief A naive preconditioner which approximates any matrix as the identity matrix
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa class DiagonalPreconditioner
+ */
+class IdentityPreconditioner {
+ public:
+  IdentityPreconditioner() {}
 
-    IdentityPreconditioner() {}
+  template <typename MatrixType>
+  explicit IdentityPreconditioner(const MatrixType&) {}
 
-    template<typename MatrixType>
-    explicit IdentityPreconditioner(const MatrixType& ) {}
+  template <typename MatrixType>
+  IdentityPreconditioner& analyzePattern(const MatrixType&) {
+    return *this;
+  }
 
-    template<typename MatrixType>
-    IdentityPreconditioner& analyzePattern(const MatrixType& ) { return *this; }
+  template <typename MatrixType>
+  IdentityPreconditioner& factorize(const MatrixType&) {
+    return *this;
+  }
 
-    template<typename MatrixType>
-    IdentityPreconditioner& factorize(const MatrixType& ) { return *this; }
+  template <typename MatrixType>
+  IdentityPreconditioner& compute(const MatrixType&) {
+    return *this;
+  }
 
-    template<typename MatrixType>
-    IdentityPreconditioner& compute(const MatrixType& ) { return *this; }
+  template <typename Rhs>
+  inline const Rhs& solve(const Rhs& b) const {
+    return b;
+  }
 
-    template<typename Rhs>
-    inline const Rhs& solve(const Rhs& b) const { return b; }
-
-    ComputationInfo info() { return Success; }
+  ComputationInfo info() { return Success; }
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_BASIC_PRECONDITIONERS_H
+#endif  // EIGEN_BASIC_PRECONDITIONERS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
index 153acef..e3154b4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
@@ -11,79 +11,76 @@
 #ifndef EIGEN_BICGSTAB_H
 #define EIGEN_BICGSTAB_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \internal Low-level bi conjugate gradient stabilized algorithm
-  * \param mat The matrix A
-  * \param rhs The right hand side vector b
-  * \param x On input and initial solution, on output the computed solution.
-  * \param precond A preconditioner being able to efficiently solve for an
-  *                approximation of Ax=b (regardless of b)
-  * \param iters On input the max number of iteration, on output the number of performed iterations.
-  * \param tol_error On input the tolerance error, on output an estimation of the relative error.
-  * \return false in the case of numerical issue, for example a break down of BiCGSTAB. 
-  */
-template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
-bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
-              const Preconditioner& precond, Index& iters,
-              typename Dest::RealScalar& tol_error)
-{
-  using std::sqrt;
+ * \param mat The matrix A
+ * \param rhs The right hand side vector b
+ * \param x On input and initial solution, on output the computed solution.
+ * \param precond A preconditioner being able to efficiently solve for an
+ *                approximation of Ax=b (regardless of b)
+ * \param iters On input the max number of iteration, on output the number of performed iterations.
+ * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+ * \return false in the case of numerical issue, for example a break down of BiCGSTAB.
+ */
+template <typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, const Preconditioner& precond, Index& iters,
+              typename Dest::RealScalar& tol_error) {
   using std::abs;
+  using std::sqrt;
   typedef typename Dest::RealScalar RealScalar;
   typedef typename Dest::Scalar Scalar;
-  typedef Matrix<Scalar,Dynamic,1> VectorType;
+  typedef Matrix<Scalar, Dynamic, 1> VectorType;
   RealScalar tol = tol_error;
   Index maxIters = iters;
 
   Index n = mat.cols();
-  VectorType r  = rhs - mat * x;
+  VectorType r = rhs - mat * x;
   VectorType r0 = r;
-  
+
   RealScalar r0_sqnorm = r0.squaredNorm();
   RealScalar rhs_sqnorm = rhs.squaredNorm();
-  if(rhs_sqnorm == 0)
-  {
+  if (rhs_sqnorm == 0) {
     x.setZero();
     return true;
   }
-  Scalar rho    = 1;
-  Scalar alpha  = 1;
-  Scalar w      = 1;
-  
+  Scalar rho(1);
+  Scalar alpha(1);
+  Scalar w(1);
+
   VectorType v = VectorType::Zero(n), p = VectorType::Zero(n);
-  VectorType y(n),  z(n);
+  VectorType y(n), z(n);
   VectorType kt(n), ks(n);
 
   VectorType s(n), t(n);
 
-  RealScalar tol2 = tol*tol*rhs_sqnorm;
-  RealScalar eps2 = NumTraits<Scalar>::epsilon()*NumTraits<Scalar>::epsilon();
+  RealScalar tol2 = tol * tol * rhs_sqnorm;
+  RealScalar eps2 = NumTraits<Scalar>::epsilon() * NumTraits<Scalar>::epsilon();
   Index i = 0;
   Index restarts = 0;
 
-  while ( r.squaredNorm() > tol2 && i<maxIters )
-  {
+  while (r.squaredNorm() > tol2 && i < maxIters) {
     Scalar rho_old = rho;
 
     rho = r0.dot(r);
-    if (abs(rho) < eps2*r0_sqnorm)
-    {
+    if (abs(rho) < eps2 * r0_sqnorm) {
       // The new residual vector became too orthogonal to the arbitrarily chosen direction r0
       // Let's restart with a new r0:
-      r  = rhs - mat * x;
+      r = rhs - mat * x;
       r0 = r;
       rho = r0_sqnorm = r.squaredNorm();
-      if(restarts++ == 0)
-        i = 0;
+      if (restarts++ == 0) i = 0;
     }
-    Scalar beta = (rho/rho_old) * (alpha / w);
+    Scalar beta = (rho / rho_old) * (alpha / w);
     p = r + beta * (p - w * v);
-    
+
     y = precond.solve(p);
-    
+
     v.noalias() = mat * y;
 
     alpha = rho / r0.dot(v);
@@ -93,7 +90,7 @@
     t.noalias() = mat * z;
 
     RealScalar tmp = t.squaredNorm();
-    if(tmp>RealScalar(0))
+    if (tmp > RealScalar(0))
       w = t.dot(s) / tmp;
     else
       w = Scalar(0);
@@ -101,112 +98,105 @@
     r = s - w * t;
     ++i;
   }
-  tol_error = sqrt(r.squaredNorm()/rhs_sqnorm);
+  tol_error = sqrt(r.squaredNorm() / rhs_sqnorm);
   iters = i;
-  return true; 
+  return true;
 }
 
-}
+}  // namespace internal
 
-template< typename _MatrixType,
-          typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
+template <typename MatrixType_, typename Preconditioner_ = DiagonalPreconditioner<typename MatrixType_::Scalar> >
 class BiCGSTAB;
 
 namespace internal {
 
-template< typename _MatrixType, typename _Preconditioner>
-struct traits<BiCGSTAB<_MatrixType,_Preconditioner> >
-{
-  typedef _MatrixType MatrixType;
-  typedef _Preconditioner Preconditioner;
+template <typename MatrixType_, typename Preconditioner_>
+struct traits<BiCGSTAB<MatrixType_, Preconditioner_> > {
+  typedef MatrixType_ MatrixType;
+  typedef Preconditioner_ Preconditioner;
 };
 
-}
+}  // namespace internal
 
 /** \ingroup IterativeLinearSolvers_Module
-  * \brief A bi conjugate gradient stabilized solver for sparse square problems
-  *
-  * This class allows to solve for A.x = b sparse linear problems using a bi conjugate gradient
-  * stabilized algorithm. The vectors x and b can be either dense or sparse.
-  *
-  * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix.
-  * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
-  *
-  * \implsparsesolverconcept
-  *
-  * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
-  * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
-  * and NumTraits<Scalar>::epsilon() for the tolerance.
-  * 
-  * The tolerance corresponds to the relative residual error: |Ax-b|/|b|
-  * 
-  * \b Performance: when using sparse matrices, best performance is achied for a row-major sparse matrix format.
-  * Moreover, in this case multi-threading can be exploited if the user code is compiled with OpenMP enabled.
-  * See \ref TopicMultiThreading for details.
-  * 
-  * This class can be used as the direct solver classes. Here is a typical usage example:
-  * \include BiCGSTAB_simple.cpp
-  * 
-  * By default the iterations start with x=0 as an initial guess of the solution.
-  * One can control the start using the solveWithGuess() method.
-  * 
-  * BiCGSTAB can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
-  *
-  * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
-  */
-template< typename _MatrixType, typename _Preconditioner>
-class BiCGSTAB : public IterativeSolverBase<BiCGSTAB<_MatrixType,_Preconditioner> >
-{
+ * \brief A bi conjugate gradient stabilized solver for sparse square problems
+ *
+ * This class allows to solve for A.x = b sparse linear problems using a bi conjugate gradient
+ * stabilized algorithm. The vectors x and b can be either dense or sparse.
+ *
+ * \tparam MatrixType_ the type of the sparse matrix A, can be a dense or a sparse matrix.
+ * \tparam Preconditioner_ the type of the preconditioner. Default is DiagonalPreconditioner
+ *
+ * \implsparsesolverconcept
+ *
+ * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
+ * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
+ * and NumTraits<Scalar>::epsilon() for the tolerance.
+ *
+ * The tolerance corresponds to the relative residual error: |Ax-b|/|b|
+ *
+ * \b Performance: when using sparse matrices, best performance is achied for a row-major sparse matrix format.
+ * Moreover, in this case multi-threading can be exploited if the user code is compiled with OpenMP enabled.
+ * See \ref TopicMultiThreading for details.
+ *
+ * This class can be used as the direct solver classes. Here is a typical usage example:
+ * \include BiCGSTAB_simple.cpp
+ *
+ * By default the iterations start with x=0 as an initial guess of the solution.
+ * One can control the start using the solveWithGuess() method.
+ *
+ * BiCGSTAB can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
+ *
+ * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+ */
+template <typename MatrixType_, typename Preconditioner_>
+class BiCGSTAB : public IterativeSolverBase<BiCGSTAB<MatrixType_, Preconditioner_> > {
   typedef IterativeSolverBase<BiCGSTAB> Base;
-  using Base::matrix;
   using Base::m_error;
-  using Base::m_iterations;
   using Base::m_info;
   using Base::m_isInitialized;
-public:
-  typedef _MatrixType MatrixType;
+  using Base::m_iterations;
+  using Base::matrix;
+
+ public:
+  typedef MatrixType_ MatrixType;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
-  typedef _Preconditioner Preconditioner;
+  typedef Preconditioner_ Preconditioner;
 
-public:
-
+ public:
   /** Default constructor. */
   BiCGSTAB() : Base() {}
 
   /** Initialize the solver with matrix \a A for further \c Ax=b solving.
-    * 
-    * This constructor is a shortcut for the default constructor followed
-    * by a call to compute().
-    * 
-    * \warning this class stores a reference to the matrix A as well as some
-    * precomputed values that depend on it. Therefore, if \a A is changed
-    * this class becomes invalid. Call compute() to update it with the new
-    * matrix A, or modify a copy of A.
-    */
-  template<typename MatrixDerived>
+   *
+   * This constructor is a shortcut for the default constructor followed
+   * by a call to compute().
+   *
+   * \warning this class stores a reference to the matrix A as well as some
+   * precomputed values that depend on it. Therefore, if \a A is changed
+   * this class becomes invalid. Call compute() to update it with the new
+   * matrix A, or modify a copy of A.
+   */
+  template <typename MatrixDerived>
   explicit BiCGSTAB(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
 
   ~BiCGSTAB() {}
 
   /** \internal */
-  template<typename Rhs,typename Dest>
-  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
-  {    
+  template <typename Rhs, typename Dest>
+  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const {
     m_iterations = Base::maxIterations();
     m_error = Base::m_tolerance;
-    
+
     bool ret = internal::bicgstab(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error);
 
-    m_info = (!ret) ? NumericalIssue
-           : m_error <= Base::m_tolerance ? Success
-           : NoConvergence;
+    m_info = (!ret) ? NumericalIssue : m_error <= Base::m_tolerance ? Success : NoConvergence;
   }
 
-protected:
-
+ protected:
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_BICGSTAB_H
+#endif  // EIGEN_BICGSTAB_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
index 5d8c6b4..5bb0efe 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
@@ -10,102 +10,95 @@
 #ifndef EIGEN_CONJUGATE_GRADIENT_H
 #define EIGEN_CONJUGATE_GRADIENT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \internal Low-level conjugate gradient algorithm
-  * \param mat The matrix A
-  * \param rhs The right hand side vector b
-  * \param x On input and initial solution, on output the computed solution.
-  * \param precond A preconditioner being able to efficiently solve for an
-  *                approximation of Ax=b (regardless of b)
-  * \param iters On input the max number of iteration, on output the number of performed iterations.
-  * \param tol_error On input the tolerance error, on output an estimation of the relative error.
-  */
-template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
-EIGEN_DONT_INLINE
-void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
-                        const Preconditioner& precond, Index& iters,
-                        typename Dest::RealScalar& tol_error)
-{
-  using std::sqrt;
-  using std::abs;
+ * \param mat The matrix A
+ * \param rhs The right hand side vector b
+ * \param x On input and initial solution, on output the computed solution.
+ * \param precond A preconditioner being able to efficiently solve for an
+ *                approximation of Ax=b (regardless of b)
+ * \param iters On input the max number of iteration, on output the number of performed iterations.
+ * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+ */
+template <typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+EIGEN_DONT_INLINE void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x, const Preconditioner& precond,
+                                          Index& iters, typename Dest::RealScalar& tol_error) {
   typedef typename Dest::RealScalar RealScalar;
   typedef typename Dest::Scalar Scalar;
-  typedef Matrix<Scalar,Dynamic,1> VectorType;
-  
+  typedef Matrix<Scalar, Dynamic, 1> VectorType;
+
   RealScalar tol = tol_error;
   Index maxIters = iters;
-  
+
   Index n = mat.cols();
 
-  VectorType residual = rhs - mat * x; //initial residual
+  VectorType residual = rhs - mat * x;  // initial residual
 
   RealScalar rhsNorm2 = rhs.squaredNorm();
-  if(rhsNorm2 == 0) 
-  {
+  if (rhsNorm2 == 0) {
     x.setZero();
     iters = 0;
     tol_error = 0;
     return;
   }
   const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
-  RealScalar threshold = numext::maxi(RealScalar(tol*tol*rhsNorm2),considerAsZero);
+  RealScalar threshold = numext::maxi(RealScalar(tol * tol * rhsNorm2), considerAsZero);
   RealScalar residualNorm2 = residual.squaredNorm();
-  if (residualNorm2 < threshold)
-  {
+  if (residualNorm2 < threshold) {
     iters = 0;
-    tol_error = sqrt(residualNorm2 / rhsNorm2);
+    tol_error = numext::sqrt(residualNorm2 / rhsNorm2);
     return;
   }
 
   VectorType p(n);
-  p = precond.solve(residual);      // initial search direction
+  p = precond.solve(residual);  // initial search direction
 
   VectorType z(n), tmp(n);
   RealScalar absNew = numext::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
   Index i = 0;
-  while(i < maxIters)
-  {
-    tmp.noalias() = mat * p;                    // the bottleneck of the algorithm
+  while (i < maxIters) {
+    tmp.noalias() = mat * p;  // the bottleneck of the algorithm
 
-    Scalar alpha = absNew / p.dot(tmp);         // the amount we travel on dir
-    x += alpha * p;                             // update solution
-    residual -= alpha * tmp;                    // update residual
-    
+    Scalar alpha = absNew / p.dot(tmp);  // the amount we travel on dir
+    x += alpha * p;                      // update solution
+    residual -= alpha * tmp;             // update residual
+
     residualNorm2 = residual.squaredNorm();
-    if(residualNorm2 < threshold)
-      break;
-    
-    z = precond.solve(residual);                // approximately solve for "A z = residual"
+    if (residualNorm2 < threshold) break;
+
+    z = precond.solve(residual);  // approximately solve for "A z = residual"
 
     RealScalar absOld = absNew;
-    absNew = numext::real(residual.dot(z));     // update the absolute value of r
-    RealScalar beta = absNew / absOld;          // calculate the Gram-Schmidt value used to create the new search direction
-    p = z + beta * p;                           // update search direction
+    absNew = numext::real(residual.dot(z));  // update the absolute value of r
+    RealScalar beta = absNew / absOld;       // calculate the Gram-Schmidt value used to create the new search direction
+    p = z + beta * p;                        // update search direction
     i++;
   }
-  tol_error = sqrt(residualNorm2 / rhsNorm2);
+  tol_error = numext::sqrt(residualNorm2 / rhsNorm2);
   iters = i;
 }
 
-}
+}  // namespace internal
 
-template< typename _MatrixType, int _UpLo=Lower,
-          typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
+template <typename MatrixType_, int UpLo_ = Lower,
+          typename Preconditioner_ = DiagonalPreconditioner<typename MatrixType_::Scalar> >
 class ConjugateGradient;
 
 namespace internal {
 
-template< typename _MatrixType, int _UpLo, typename _Preconditioner>
-struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
-{
-  typedef _MatrixType MatrixType;
-  typedef _Preconditioner Preconditioner;
+template <typename MatrixType_, int UpLo_, typename Preconditioner_>
+struct traits<ConjugateGradient<MatrixType_, UpLo_, Preconditioner_> > {
+  typedef MatrixType_ MatrixType;
+  typedef Preconditioner_ Preconditioner;
 };
 
-}
+}  // namespace internal
 
 /** \ingroup IterativeLinearSolvers_Module
   * \brief A conjugate gradient solver for sparse (or dense) self-adjoint problems
@@ -113,25 +106,25 @@
   * This class allows to solve for A.x = b linear problems using an iterative conjugate gradient algorithm.
   * The matrix A must be selfadjoint. The matrix A and the vectors x and b can be either dense or sparse.
   *
-  * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix.
-  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower,
+  * \tparam MatrixType_ the type of the matrix A, can be a dense or a sparse matrix.
+  * \tparam UpLo_ the triangular part that will be used for the computations. It can be Lower,
   *               \c Upper, or \c Lower|Upper in which the full matrix entries will be considered.
   *               Default is \c Lower, best performance is \c Lower|Upper.
-  * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
+  * \tparam Preconditioner_ the type of the preconditioner. Default is DiagonalPreconditioner
   *
   * \implsparsesolverconcept
   *
   * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
   * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
   * and NumTraits<Scalar>::epsilon() for the tolerance.
-  * 
+  *
   * The tolerance corresponds to the relative residual error: |Ax-b|/|b|
-  * 
-  * \b Performance: Even though the default value of \c _UpLo is \c Lower, significantly higher performance is
-  * achieved when using a complete matrix and \b Lower|Upper as the \a _UpLo template parameter. Moreover, in this
+  *
+  * \b Performance: Even though the default value of \c UpLo_ is \c Lower, significantly higher performance is
+  * achieved when using a complete matrix and \b Lower|Upper as the \a UpLo_ template parameter. Moreover, in this
   * case multi-threading can be exploited if the user code is compiled with OpenMP enabled.
   * See \ref TopicMultiThreading for details.
-  * 
+  *
   * This class can be used as the direct solver classes. Here is a typical usage example:
     \code
     int n = 10000;
@@ -146,71 +139,67 @@
     // update b, and solve again
     x = cg.solve(b);
     \endcode
-  * 
+  *
   * By default the iterations start with x=0 as an initial guess of the solution.
   * One can control the start using the solveWithGuess() method.
-  * 
-  * ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
+  *
+  * ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example
+  \endlink.
   *
   * \sa class LeastSquaresConjugateGradient, class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
   */
-template< typename _MatrixType, int _UpLo, typename _Preconditioner>
-class ConjugateGradient : public IterativeSolverBase<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
-{
+template <typename MatrixType_, int UpLo_, typename Preconditioner_>
+class ConjugateGradient : public IterativeSolverBase<ConjugateGradient<MatrixType_, UpLo_, Preconditioner_> > {
   typedef IterativeSolverBase<ConjugateGradient> Base;
-  using Base::matrix;
   using Base::m_error;
-  using Base::m_iterations;
   using Base::m_info;
   using Base::m_isInitialized;
-public:
-  typedef _MatrixType MatrixType;
+  using Base::m_iterations;
+  using Base::matrix;
+
+ public:
+  typedef MatrixType_ MatrixType;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
-  typedef _Preconditioner Preconditioner;
+  typedef Preconditioner_ Preconditioner;
 
-  enum {
-    UpLo = _UpLo
-  };
+  enum { UpLo = UpLo_ };
 
-public:
-
+ public:
   /** Default constructor. */
   ConjugateGradient() : Base() {}
 
   /** Initialize the solver with matrix \a A for further \c Ax=b solving.
-    * 
-    * This constructor is a shortcut for the default constructor followed
-    * by a call to compute().
-    * 
-    * \warning this class stores a reference to the matrix A as well as some
-    * precomputed values that depend on it. Therefore, if \a A is changed
-    * this class becomes invalid. Call compute() to update it with the new
-    * matrix A, or modify a copy of A.
-    */
-  template<typename MatrixDerived>
+   *
+   * This constructor is a shortcut for the default constructor followed
+   * by a call to compute().
+   *
+   * \warning this class stores a reference to the matrix A as well as some
+   * precomputed values that depend on it. Therefore, if \a A is changed
+   * this class becomes invalid. Call compute() to update it with the new
+   * matrix A, or modify a copy of A.
+   */
+  template <typename MatrixDerived>
   explicit ConjugateGradient(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
 
   ~ConjugateGradient() {}
 
   /** \internal */
-  template<typename Rhs,typename Dest>
-  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
-  {
+  template <typename Rhs, typename Dest>
+  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const {
     typedef typename Base::MatrixWrapper MatrixWrapper;
     typedef typename Base::ActualMatrixType ActualMatrixType;
     enum {
-      TransposeInput  =   (!MatrixWrapper::MatrixFree)
-                      &&  (UpLo==(Lower|Upper))
-                      &&  (!MatrixType::IsRowMajor)
-                      &&  (!NumTraits<Scalar>::IsComplex)
+      TransposeInput = (!MatrixWrapper::MatrixFree) && (UpLo == (Lower | Upper)) && (!MatrixType::IsRowMajor) &&
+                       (!NumTraits<Scalar>::IsComplex)
     };
-    typedef typename internal::conditional<TransposeInput,Transpose<const ActualMatrixType>, ActualMatrixType const&>::type RowMajorWrapper;
-    EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(MatrixWrapper::MatrixFree,UpLo==(Lower|Upper)),MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY);
-    typedef typename internal::conditional<UpLo==(Lower|Upper),
-                                           RowMajorWrapper,
-                                           typename MatrixWrapper::template ConstSelfAdjointViewReturnType<UpLo>::Type
-                                          >::type SelfAdjointWrapper;
+    typedef std::conditional_t<TransposeInput, Transpose<const ActualMatrixType>, ActualMatrixType const&>
+        RowMajorWrapper;
+    EIGEN_STATIC_ASSERT(internal::check_implication(MatrixWrapper::MatrixFree, UpLo == (Lower | Upper)),
+                        MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY);
+    typedef std::conditional_t<UpLo == (Lower | Upper), RowMajorWrapper,
+                               typename MatrixWrapper::template ConstSelfAdjointViewReturnType<UpLo>::Type>
+        SelfAdjointWrapper;
 
     m_iterations = Base::maxIterations();
     m_error = Base::m_tolerance;
@@ -220,10 +209,9 @@
     m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
   }
 
-protected:
-
+ protected:
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_CONJUGATE_GRADIENT_H
+#endif  // EIGEN_CONJUGATE_GRADIENT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
index 7803fd8..14ae6ea 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
@@ -14,229 +14,233 @@
 #include <vector>
 #include <list>
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 /**
-  * \brief Modified Incomplete Cholesky with dual threshold
-  *
-  * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with
-  *              Limited memory, SIAM J. Sci. Comput.  21(1), pp. 24-45, 1999
-  *
-  * \tparam Scalar the scalar type of the input matrices
-  * \tparam _UpLo The triangular part that will be used for the computations. It can be Lower
-    *               or Upper. Default is Lower.
-  * \tparam _OrderingType The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<int>,
-  *                       unless EIGEN_MPL2_ONLY is defined, in which case the default is NaturalOrdering<int>.
-  *
-  * \implsparsesolverconcept
-  *
-  * It performs the following incomplete factorization: \f$ S P A P' S \approx L L' \f$
-  * where L is a lower triangular factor, S is a diagonal scaling matrix, and P is a
-  * fill-in reducing permutation as computed by the ordering method.
-  *
-  * \b Shifting \b strategy: Let \f$ B = S P A P' S \f$  be the scaled matrix on which the factorization is carried out,
-  * and \f$ \beta \f$ be the minimum value of the diagonal. If \f$ \beta > 0 \f$ then, the factorization is directly performed
-  * on the matrix B. Otherwise, the factorization is performed on the shifted matrix \f$ B + (\sigma+|\beta| I \f$ where
-  * \f$ \sigma \f$ is the initial shift value as returned and set by setInitialShift() method. The default value is \f$ \sigma = 10^{-3} \f$.
-  * If the factorization fails, then the shift in doubled until it succeed or a maximum of ten attempts. If it still fails, as returned by
-  * the info() method, then you can either increase the initial shift, or better use another preconditioning technique.
-  *
-  */
-template <typename Scalar, int _UpLo = Lower, typename _OrderingType = AMDOrdering<int> >
-class IncompleteCholesky : public SparseSolverBase<IncompleteCholesky<Scalar,_UpLo,_OrderingType> >
-{
-  protected:
-    typedef SparseSolverBase<IncompleteCholesky<Scalar,_UpLo,_OrderingType> > Base;
-    using Base::m_isInitialized;
-  public:
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef _OrderingType OrderingType;
-    typedef typename OrderingType::PermutationType PermutationType;
-    typedef typename PermutationType::StorageIndex StorageIndex;
-    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> FactorType;
-    typedef Matrix<Scalar,Dynamic,1> VectorSx;
-    typedef Matrix<RealScalar,Dynamic,1> VectorRx;
-    typedef Matrix<StorageIndex,Dynamic, 1> VectorIx;
-    typedef std::vector<std::list<StorageIndex> > VectorList;
-    enum { UpLo = _UpLo };
-    enum {
-      ColsAtCompileTime = Dynamic,
-      MaxColsAtCompileTime = Dynamic
-    };
-  public:
+ * \brief Modified Incomplete Cholesky with dual threshold
+ *
+ * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with
+ *              Limited memory, SIAM J. Sci. Comput.  21(1), pp. 24-45, 1999
+ *
+ * \tparam Scalar the scalar type of the input matrices
+ * \tparam UpLo_ The triangular part that will be used for the computations. It can be Lower
+ *               or Upper. Default is Lower.
+ * \tparam OrderingType_ The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is
+ * AMDOrdering<int>.
+ *
+ * \implsparsesolverconcept
+ *
+ * It performs the following incomplete factorization: \f$ S P A P' S \approx L L' \f$
+ * where L is a lower triangular factor, S is a diagonal scaling matrix, and P is a
+ * fill-in reducing permutation as computed by the ordering method.
+ *
+ * \b Shifting \b strategy: Let \f$ B = S P A P' S \f$  be the scaled matrix on which the factorization is carried out,
+ * and \f$ \beta \f$ be the minimum value of the diagonal. If \f$ \beta > 0 \f$ then, the factorization is directly
+ * performed on the matrix B. Otherwise, the factorization is performed on the shifted matrix \f$ B + (\sigma+|\beta| I
+ * \f$ where \f$ \sigma \f$ is the initial shift value as returned and set by setInitialShift() method. The default
+ * value is \f$ \sigma = 10^{-3} \f$. If the factorization fails, then the shift in doubled until it succeed or a
+ * maximum of ten attempts. If it still fails, as returned by the info() method, then you can either increase the
+ * initial shift, or better use another preconditioning technique.
+ *
+ */
+template <typename Scalar, int UpLo_ = Lower, typename OrderingType_ = AMDOrdering<int> >
+class IncompleteCholesky : public SparseSolverBase<IncompleteCholesky<Scalar, UpLo_, OrderingType_> > {
+ protected:
+  typedef SparseSolverBase<IncompleteCholesky<Scalar, UpLo_, OrderingType_> > Base;
+  using Base::m_isInitialized;
 
-    /** Default constructor leaving the object in a partly non-initialized stage.
-      *
-      * You must call compute() or the pair analyzePattern()/factorize() to make it valid.
-      *
-      * \sa IncompleteCholesky(const MatrixType&)
-      */
-    IncompleteCholesky() : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false) {}
+ public:
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef OrderingType_ OrderingType;
+  typedef typename OrderingType::PermutationType PermutationType;
+  typedef typename PermutationType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> FactorType;
+  typedef Matrix<Scalar, Dynamic, 1> VectorSx;
+  typedef Matrix<RealScalar, Dynamic, 1> VectorRx;
+  typedef Matrix<StorageIndex, Dynamic, 1> VectorIx;
+  typedef std::vector<std::list<StorageIndex> > VectorList;
+  enum { UpLo = UpLo_ };
+  enum { ColsAtCompileTime = Dynamic, MaxColsAtCompileTime = Dynamic };
 
-    /** Constructor computing the incomplete factorization for the given matrix \a matrix.
-      */
-    template<typename MatrixType>
-    IncompleteCholesky(const MatrixType& matrix) : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false)
-    {
-      compute(matrix);
-    }
+ public:
+  /** Default constructor leaving the object in a partly non-initialized stage.
+   *
+   * You must call compute() or the pair analyzePattern()/factorize() to make it valid.
+   *
+   * \sa IncompleteCholesky(const MatrixType&)
+   */
+  IncompleteCholesky() : m_initialShift(1e-3), m_analysisIsOk(false), m_factorizationIsOk(false) {}
 
-    /** \returns number of rows of the factored matrix */
-    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_L.rows(); }
+  /** Constructor computing the incomplete factorization for the given matrix \a matrix.
+   */
+  template <typename MatrixType>
+  IncompleteCholesky(const MatrixType& matrix)
+      : m_initialShift(1e-3), m_analysisIsOk(false), m_factorizationIsOk(false) {
+    compute(matrix);
+  }
 
-    /** \returns number of columns of the factored matrix */
-    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_L.cols(); }
+  /** \returns number of rows of the factored matrix */
+  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_L.rows(); }
 
+  /** \returns number of columns of the factored matrix */
+  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_L.cols(); }
 
-    /** \brief Reports whether previous computation was successful.
-      *
-      * It triggers an assertion if \c *this has not been initialized through the respective constructor,
-      * or a call to compute() or analyzePattern().
-      *
-      * \returns \c Success if computation was successful,
-      *          \c NumericalIssue if the matrix appears to be negative.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "IncompleteCholesky is not initialized.");
-      return m_info;
-    }
+  /** \brief Reports whether previous computation was successful.
+   *
+   * It triggers an assertion if \c *this has not been initialized through the respective constructor,
+   * or a call to compute() or analyzePattern().
+   *
+   * \returns \c Success if computation was successful,
+   *          \c NumericalIssue if the matrix appears to be negative.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "IncompleteCholesky is not initialized.");
+    return m_info;
+  }
 
-    /** \brief Set the initial shift parameter \f$ \sigma \f$.
-      */
-    void setInitialShift(RealScalar shift) { m_initialShift = shift; }
+  /** \brief Set the initial shift parameter \f$ \sigma \f$.
+   */
+  void setInitialShift(RealScalar shift) { m_initialShift = shift; }
 
-    /** \brief Computes the fill reducing permutation vector using the sparsity pattern of \a mat
-      */
-    template<typename MatrixType>
-    void analyzePattern(const MatrixType& mat)
-    {
-      OrderingType ord;
-      PermutationType pinv;
-      ord(mat.template selfadjointView<UpLo>(), pinv);
-      if(pinv.size()>0) m_perm = pinv.inverse();
-      else              m_perm.resize(0);
-      m_L.resize(mat.rows(), mat.cols());
-      m_analysisIsOk = true;
-      m_isInitialized = true;
-      m_info = Success;
-    }
+  /** \brief Computes the fill reducing permutation vector using the sparsity pattern of \a mat
+   */
+  template <typename MatrixType>
+  void analyzePattern(const MatrixType& mat) {
+    OrderingType ord;
+    PermutationType pinv;
+    ord(mat.template selfadjointView<UpLo>(), pinv);
+    if (pinv.size() > 0)
+      m_perm = pinv.inverse();
+    else
+      m_perm.resize(0);
+    m_L.resize(mat.rows(), mat.cols());
+    m_analysisIsOk = true;
+    m_isInitialized = true;
+    m_info = Success;
+  }
 
-    /** \brief Performs the numerical factorization of the input matrix \a mat
-      *
-      * The method analyzePattern() or compute() must have been called beforehand
-      * with a matrix having the same pattern.
-      *
-      * \sa compute(), analyzePattern()
-      */
-    template<typename MatrixType>
-    void factorize(const MatrixType& mat);
+  /** \brief Performs the numerical factorization of the input matrix \a mat
+   *
+   * The method analyzePattern() or compute() must have been called beforehand
+   * with a matrix having the same pattern.
+   *
+   * \sa compute(), analyzePattern()
+   */
+  template <typename MatrixType>
+  void factorize(const MatrixType& mat);
 
-    /** Computes or re-computes the incomplete Cholesky factorization of the input matrix \a mat
-      *
-      * It is a shortcut for a sequential call to the analyzePattern() and factorize() methods.
-      *
-      * \sa analyzePattern(), factorize()
-      */
-    template<typename MatrixType>
-    void compute(const MatrixType& mat)
-    {
-      analyzePattern(mat);
-      factorize(mat);
-    }
+  /** Computes or re-computes the incomplete Cholesky factorization of the input matrix \a mat
+   *
+   * It is a shortcut for a sequential call to the analyzePattern() and factorize() methods.
+   *
+   * \sa analyzePattern(), factorize()
+   */
+  template <typename MatrixType>
+  void compute(const MatrixType& mat) {
+    analyzePattern(mat);
+    factorize(mat);
+  }
 
-    // internal
-    template<typename Rhs, typename Dest>
-    void _solve_impl(const Rhs& b, Dest& x) const
-    {
-      eigen_assert(m_factorizationIsOk && "factorize() should be called first");
-      if (m_perm.rows() == b.rows())  x = m_perm * b;
-      else                            x = b;
-      x = m_scale.asDiagonal() * x;
-      x = m_L.template triangularView<Lower>().solve(x);
-      x = m_L.adjoint().template triangularView<Upper>().solve(x);
-      x = m_scale.asDiagonal() * x;
-      if (m_perm.rows() == b.rows())
-        x = m_perm.inverse() * x;
-    }
+  // internal
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const Rhs& b, Dest& x) const {
+    eigen_assert(m_factorizationIsOk && "factorize() should be called first");
+    if (m_perm.rows() == b.rows())
+      x = m_perm * b;
+    else
+      x = b;
+    x = m_scale.asDiagonal() * x;
+    x = m_L.template triangularView<Lower>().solve(x);
+    x = m_L.adjoint().template triangularView<Upper>().solve(x);
+    x = m_scale.asDiagonal() * x;
+    if (m_perm.rows() == b.rows()) x = m_perm.inverse() * x;
+  }
 
-    /** \returns the sparse lower triangular factor L */
-    const FactorType& matrixL() const { eigen_assert("m_factorizationIsOk"); return m_L; }
+  /** \returns the sparse lower triangular factor L */
+  const FactorType& matrixL() const {
+    eigen_assert(m_factorizationIsOk && "factorize() should be called first");
+    return m_L;
+  }
 
-    /** \returns a vector representing the scaling factor S */
-    const VectorRx& scalingS() const { eigen_assert("m_factorizationIsOk"); return m_scale; }
+  /** \returns a vector representing the scaling factor S */
+  const VectorRx& scalingS() const {
+    eigen_assert(m_factorizationIsOk && "factorize() should be called first");
+    return m_scale;
+  }
 
-    /** \returns the fill-in reducing permutation P (can be empty for a natural ordering) */
-    const PermutationType& permutationP() const { eigen_assert("m_analysisIsOk"); return m_perm; }
+  /** \returns the fill-in reducing permutation P (can be empty for a natural ordering) */
+  const PermutationType& permutationP() const {
+    eigen_assert(m_analysisIsOk && "analyzePattern() should be called first");
+    return m_perm;
+  }
 
-  protected:
-    FactorType m_L;              // The lower part stored in CSC
-    VectorRx m_scale;            // The vector for scaling the matrix
-    RealScalar m_initialShift;   // The initial shift parameter
-    bool m_analysisIsOk;
-    bool m_factorizationIsOk;
-    ComputationInfo m_info;
-    PermutationType m_perm;
+ protected:
+  FactorType m_L;             // The lower part stored in CSC
+  VectorRx m_scale;           // The vector for scaling the matrix
+  RealScalar m_initialShift;  // The initial shift parameter
+  bool m_analysisIsOk;
+  bool m_factorizationIsOk;
+  ComputationInfo m_info;
+  PermutationType m_perm;
 
-  private:
-    inline void updateList(Ref<const VectorIx> colPtr, Ref<VectorIx> rowIdx, Ref<VectorSx> vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol);
+ private:
+  inline void updateList(Ref<const VectorIx> colPtr, Ref<VectorIx> rowIdx, Ref<VectorSx> vals, const Index& col,
+                         const Index& jk, VectorIx& firstElt, VectorList& listCol);
 };
 
 // Based on the following paper:
 //   C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with
 //   Limited memory, SIAM J. Sci. Comput.  21(1), pp. 24-45, 1999
 //   http://ftp.mcs.anl.gov/pub/tech_reports/reports/P682.pdf
-template<typename Scalar, int _UpLo, typename OrderingType>
-template<typename _MatrixType>
-void IncompleteCholesky<Scalar,_UpLo, OrderingType>::factorize(const _MatrixType& mat)
-{
+template <typename Scalar, int UpLo_, typename OrderingType>
+template <typename MatrixType_>
+void IncompleteCholesky<Scalar, UpLo_, OrderingType>::factorize(const MatrixType_& mat) {
   using std::sqrt;
   eigen_assert(m_analysisIsOk && "analyzePattern() should be called first");
 
-  // Dropping strategy : Keep only the p largest elements per column, where p is the number of elements in the column of the original matrix. Other strategies will be added
+  // Dropping strategy : Keep only the p largest elements per column, where p is the number of elements in the column of
+  // the original matrix. Other strategies will be added
 
   // Apply the fill-reducing permutation computed in analyzePattern()
-  if (m_perm.rows() == mat.rows() ) // To detect the null permutation
+  if (m_perm.rows() == mat.rows())  // To detect the null permutation
   {
     // The temporary is needed to make sure that the diagonal entry is properly sorted
     FactorType tmp(mat.rows(), mat.cols());
-    tmp = mat.template selfadjointView<_UpLo>().twistedBy(m_perm);
+    tmp = mat.template selfadjointView<UpLo_>().twistedBy(m_perm);
     m_L.template selfadjointView<Lower>() = tmp.template selfadjointView<Lower>();
-  }
-  else
-  {
-    m_L.template selfadjointView<Lower>() = mat.template selfadjointView<_UpLo>();
+  } else {
+    m_L.template selfadjointView<Lower>() = mat.template selfadjointView<UpLo_>();
   }
 
   Index n = m_L.cols();
   Index nnz = m_L.nonZeros();
-  Map<VectorSx> vals(m_L.valuePtr(), nnz);         //values
-  Map<VectorIx> rowIdx(m_L.innerIndexPtr(), nnz);  //Row indices
-  Map<VectorIx> colPtr( m_L.outerIndexPtr(), n+1); // Pointer to the beginning of each row
-  VectorIx firstElt(n-1); // for each j, points to the next entry in vals that will be used in the factorization
-  VectorList listCol(n);  // listCol(j) is a linked list of columns to update column j
-  VectorSx col_vals(n);   // Store a  nonzero values in each column
-  VectorIx col_irow(n);   // Row indices of nonzero elements in each column
+  Map<VectorSx> vals(m_L.valuePtr(), nnz);           // values
+  Map<VectorIx> rowIdx(m_L.innerIndexPtr(), nnz);    // Row indices
+  Map<VectorIx> colPtr(m_L.outerIndexPtr(), n + 1);  // Pointer to the beginning of each row
+  VectorIx firstElt(n - 1);  // for each j, points to the next entry in vals that will be used in the factorization
+  VectorList listCol(n);     // listCol(j) is a linked list of columns to update column j
+  VectorSx col_vals(n);      // Store a  nonzero values in each column
+  VectorIx col_irow(n);      // Row indices of nonzero elements in each column
   VectorIx col_pattern(n);
   col_pattern.fill(-1);
   StorageIndex col_nnz;
 
-
   // Computes the scaling factors
   m_scale.resize(n);
   m_scale.setZero();
   for (Index j = 0; j < n; j++)
-    for (Index k = colPtr[j]; k < colPtr[j+1]; k++)
-    {
+    for (Index k = colPtr[j]; k < colPtr[j + 1]; k++) {
       m_scale(j) += numext::abs2(vals(k));
-      if(rowIdx[k]!=j)
-        m_scale(rowIdx[k]) += numext::abs2(vals(k));
+      if (rowIdx[k] != j) m_scale(rowIdx[k]) += numext::abs2(vals(k));
     }
 
   m_scale = m_scale.cwiseSqrt().cwiseSqrt();
 
   for (Index j = 0; j < n; ++j)
-    if(m_scale(j)>(std::numeric_limits<RealScalar>::min)())
-      m_scale(j) = RealScalar(1)/m_scale(j);
+    if (m_scale(j) > (std::numeric_limits<RealScalar>::min)())
+      m_scale(j) = RealScalar(1) / m_scale(j);
     else
       m_scale(j) = 1;
 
@@ -244,40 +248,34 @@
 
   // Scale and compute the shift for the matrix
   RealScalar mindiag = NumTraits<RealScalar>::highest();
-  for (Index j = 0; j < n; j++)
-  {
-    for (Index k = colPtr[j]; k < colPtr[j+1]; k++)
-      vals[k] *= (m_scale(j)*m_scale(rowIdx[k]));
-    eigen_internal_assert(rowIdx[colPtr[j]]==j && "IncompleteCholesky: only the lower triangular part must be stored");
+  for (Index j = 0; j < n; j++) {
+    for (Index k = colPtr[j]; k < colPtr[j + 1]; k++) vals[k] *= (m_scale(j) * m_scale(rowIdx[k]));
+    eigen_internal_assert(rowIdx[colPtr[j]] == j &&
+                          "IncompleteCholesky: only the lower triangular part must be stored");
     mindiag = numext::mini(numext::real(vals[colPtr[j]]), mindiag);
   }
 
   FactorType L_save = m_L;
 
   RealScalar shift = 0;
-  if(mindiag <= RealScalar(0.))
-    shift = m_initialShift - mindiag;
+  if (mindiag <= RealScalar(0.)) shift = m_initialShift - mindiag;
 
   m_info = NumericalIssue;
 
   // Try to perform the incomplete factorization using the current shift
   int iter = 0;
-  do
-  {
+  do {
     // Apply the shift to the diagonal elements of the matrix
-    for (Index j = 0; j < n; j++)
-      vals[colPtr[j]] += shift;
+    for (Index j = 0; j < n; j++) vals[colPtr[j]] += shift;
 
     // jki version of the Cholesky factorization
-    Index j=0;
-    for (; j < n; ++j)
-    {
+    Index j = 0;
+    for (; j < n; ++j) {
       // Left-looking factorization of the j-th column
       // First, load the j-th column into col_vals
       Scalar diag = vals[colPtr[j]];  // It is assumed that only the lower part is stored
       col_nnz = 0;
-      for (Index i = colPtr[j] + 1; i < colPtr[j+1]; i++)
-      {
+      for (Index i = colPtr[j] + 1; i < colPtr[j + 1]; i++) {
         StorageIndex l = rowIdx[i];
         col_vals(col_nnz) = vals[i];
         col_irow(col_nnz) = l;
@@ -287,69 +285,60 @@
       {
         typename std::list<StorageIndex>::iterator k;
         // Browse all previous columns that will update column j
-        for(k = listCol[j].begin(); k != listCol[j].end(); k++)
-        {
-          Index jk = firstElt(*k); // First element to use in the column
-          eigen_internal_assert(rowIdx[jk]==j);
+        for (k = listCol[j].begin(); k != listCol[j].end(); k++) {
+          Index jk = firstElt(*k);  // First element to use in the column
+          eigen_internal_assert(rowIdx[jk] == j);
           Scalar v_j_jk = numext::conj(vals[jk]);
 
           jk += 1;
-          for (Index i = jk; i < colPtr[*k+1]; i++)
-          {
+          for (Index i = jk; i < colPtr[*k + 1]; i++) {
             StorageIndex l = rowIdx[i];
-            if(col_pattern[l]<0)
-            {
+            if (col_pattern[l] < 0) {
               col_vals(col_nnz) = vals[i] * v_j_jk;
               col_irow[col_nnz] = l;
               col_pattern(l) = col_nnz;
               col_nnz++;
-            }
-            else
+            } else
               col_vals(col_pattern[l]) -= vals[i] * v_j_jk;
           }
-          updateList(colPtr,rowIdx,vals, *k, jk, firstElt, listCol);
+          updateList(colPtr, rowIdx, vals, *k, jk, firstElt, listCol);
         }
       }
 
       // Scale the current column
-      if(numext::real(diag) <= 0)
-      {
-        if(++iter>=10)
-          return;
+      if (numext::real(diag) <= 0) {
+        if (++iter >= 10) return;
 
         // increase shift
-        shift = numext::maxi(m_initialShift,RealScalar(2)*shift);
+        shift = numext::maxi(m_initialShift, RealScalar(2) * shift);
         // restore m_L, col_pattern, and listCol
         vals = Map<const VectorSx>(L_save.valuePtr(), nnz);
         rowIdx = Map<const VectorIx>(L_save.innerIndexPtr(), nnz);
-        colPtr = Map<const VectorIx>(L_save.outerIndexPtr(), n+1);
+        colPtr = Map<const VectorIx>(L_save.outerIndexPtr(), n + 1);
         col_pattern.fill(-1);
-        for(Index i=0; i<n; ++i)
-          listCol[i].clear();
+        for (Index i = 0; i < n; ++i) listCol[i].clear();
 
         break;
       }
 
       RealScalar rdiag = sqrt(numext::real(diag));
       vals[colPtr[j]] = rdiag;
-      for (Index k = 0; k<col_nnz; ++k)
-      {
+      for (Index k = 0; k < col_nnz; ++k) {
         Index i = col_irow[k];
-        //Scale
+        // Scale
         col_vals(k) /= rdiag;
-        //Update the remaining diagonals with col_vals
+        // Update the remaining diagonals with col_vals
         vals[colPtr[i]] -= numext::abs2(col_vals(k));
       }
       // Select the largest p elements
       // p is the original number of elements in the column (without the diagonal)
-      Index p = colPtr[j+1] - colPtr[j] - 1 ;
+      Index p = colPtr[j + 1] - colPtr[j] - 1;
       Ref<VectorSx> cvals = col_vals.head(col_nnz);
       Ref<VectorIx> cirow = col_irow.head(col_nnz);
-      internal::QuickSplit(cvals,cirow, p);
+      internal::QuickSplit(cvals, cirow, p);
       // Insert the largest p elements in the matrix
       Index cpt = 0;
-      for (Index i = colPtr[j]+1; i < colPtr[j+1]; i++)
-      {
+      for (Index i = colPtr[j] + 1; i < colPtr[j + 1]; i++) {
         vals[i] = col_vals(cpt);
         rowIdx[i] = col_irow(cpt);
         // restore col_pattern:
@@ -357,38 +346,37 @@
         cpt++;
       }
       // Get the first smallest row index and put it after the diagonal element
-      Index jk = colPtr(j)+1;
-      updateList(colPtr,rowIdx,vals,j,jk,firstElt,listCol);
+      Index jk = colPtr(j) + 1;
+      updateList(colPtr, rowIdx, vals, j, jk, firstElt, listCol);
     }
 
-    if(j==n)
-    {
+    if (j == n) {
       m_factorizationIsOk = true;
       m_info = Success;
     }
-  } while(m_info!=Success);
+  } while (m_info != Success);
 }
 
-template<typename Scalar, int _UpLo, typename OrderingType>
-inline void IncompleteCholesky<Scalar,_UpLo, OrderingType>::updateList(Ref<const VectorIx> colPtr, Ref<VectorIx> rowIdx, Ref<VectorSx> vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol)
-{
-  if (jk < colPtr(col+1) )
-  {
-    Index p = colPtr(col+1) - jk;
+template <typename Scalar, int UpLo_, typename OrderingType>
+inline void IncompleteCholesky<Scalar, UpLo_, OrderingType>::updateList(Ref<const VectorIx> colPtr,
+                                                                        Ref<VectorIx> rowIdx, Ref<VectorSx> vals,
+                                                                        const Index& col, const Index& jk,
+                                                                        VectorIx& firstElt, VectorList& listCol) {
+  if (jk < colPtr(col + 1)) {
+    Index p = colPtr(col + 1) - jk;
     Index minpos;
-    rowIdx.segment(jk,p).minCoeff(&minpos);
+    rowIdx.segment(jk, p).minCoeff(&minpos);
     minpos += jk;
-    if (rowIdx(minpos) != rowIdx(jk))
-    {
-      //Swap
-      std::swap(rowIdx(jk),rowIdx(minpos));
-      std::swap(vals(jk),vals(minpos));
+    if (rowIdx(minpos) != rowIdx(jk)) {
+      // Swap
+      std::swap(rowIdx(jk), rowIdx(minpos));
+      std::swap(vals(jk), vals(minpos));
     }
-    firstElt(col) = internal::convert_index<StorageIndex,Index>(jk);
-    listCol[rowIdx(jk)].push_back(internal::convert_index<StorageIndex,Index>(col));
+    firstElt(col) = internal::convert_index<StorageIndex, Index>(jk);
+    listCol[rowIdx(jk)].push_back(internal::convert_index<StorageIndex, Index>(col));
   }
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
index cdcf709..575a7b2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
@@ -11,40 +11,41 @@
 #ifndef EIGEN_INCOMPLETE_LUT_H
 #define EIGEN_INCOMPLETE_LUT_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
 namespace Eigen {
 
 namespace internal {
 
 /** \internal
-  * Compute a quick-sort split of a vector
-  * On output, the vector row is permuted such that its elements satisfy
-  * abs(row(i)) >= abs(row(ncut)) if i<ncut
-  * abs(row(i)) <= abs(row(ncut)) if i>ncut
-  * \param row The vector of values
-  * \param ind The array of index for the elements in @p row
-  * \param ncut  The number of largest elements to keep
-  **/
+ * Compute a quick-sort split of a vector
+ * On output, the vector row is permuted such that its elements satisfy
+ * abs(row(i)) >= abs(row(ncut)) if i<ncut
+ * abs(row(i)) <= abs(row(ncut)) if i>ncut
+ * \param row The vector of values
+ * \param ind The array of index for the elements in @p row
+ * \param ncut  The number of largest elements to keep
+ **/
 template <typename VectorV, typename VectorI>
-Index QuickSplit(VectorV &row, VectorI &ind, Index ncut)
-{
+Index QuickSplit(VectorV& row, VectorI& ind, Index ncut) {
   typedef typename VectorV::RealScalar RealScalar;
-  using std::swap;
   using std::abs;
+  using std::swap;
   Index mid;
   Index n = row.size(); /* length of the vector */
-  Index first, last ;
+  Index first, last;
 
   ncut--; /* to fit the zero-based indices */
   first = 0;
-  last = n-1;
-  if (ncut < first || ncut > last ) return 0;
+  last = n - 1;
+  if (ncut < first || ncut > last) return 0;
 
   do {
     mid = first;
     RealScalar abskey = abs(row(mid));
     for (Index j = first + 1; j <= last; j++) {
-      if ( abs(row(j)) > abskey) {
+      if (abs(row(j)) > abskey) {
         ++mid;
         swap(row(mid), row(j));
         swap(ind(mid), ind(j));
@@ -54,157 +55,146 @@
     swap(row(mid), row(first));
     swap(ind(mid), ind(first));
 
-    if (mid > ncut) last = mid - 1;
-    else if (mid < ncut ) first = mid + 1;
-  } while (mid != ncut );
+    if (mid > ncut)
+      last = mid - 1;
+    else if (mid < ncut)
+      first = mid + 1;
+  } while (mid != ncut);
 
   return 0; /* mid is equal to ncut */
 }
 
-}// end namespace internal
+}  // end namespace internal
 
 /** \ingroup IterativeLinearSolvers_Module
-  * \class IncompleteLUT
-  * \brief Incomplete LU factorization with dual-threshold strategy
-  *
-  * \implsparsesolverconcept
-  *
-  * During the numerical factorization, two dropping rules are used :
-  *  1) any element whose magnitude is less than some tolerance is dropped.
-  *    This tolerance is obtained by multiplying the input tolerance @p droptol
-  *    by the average magnitude of all the original elements in the current row.
-  *  2) After the elimination of the row, only the @p fill largest elements in
-  *    the L part and the @p fill largest elements in the U part are kept
-  *    (in addition to the diagonal element ). Note that @p fill is computed from
-  *    the input parameter @p fillfactor which is used the ratio to control the fill_in
-  *    relatively to the initial number of nonzero elements.
-  *
-  * The two extreme cases are when @p droptol=0 (to keep all the @p fill*2 largest elements)
-  * and when @p fill=n/2 with @p droptol being different to zero.
-  *
-  * References : Yousef Saad, ILUT: A dual threshold incomplete LU factorization,
-  *              Numerical Linear Algebra with Applications, 1(4), pp 387-402, 1994.
-  *
-  * NOTE : The following implementation is derived from the ILUT implementation
-  * in the SPARSKIT package, Copyright (C) 2005, the Regents of the University of Minnesota
-  *  released under the terms of the GNU LGPL:
-  *    http://www-users.cs.umn.edu/~saad/software/SPARSKIT/README
-  * However, Yousef Saad gave us permission to relicense his ILUT code to MPL2.
-  * See the Eigen mailing list archive, thread: ILUT, date: July 8, 2012:
-  *   http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2012/07/msg00064.html
-  * alternatively, on GMANE:
-  *   http://comments.gmane.org/gmane.comp.lib.eigen/3302
-  */
-template <typename _Scalar, typename _StorageIndex = int>
-class IncompleteLUT : public SparseSolverBase<IncompleteLUT<_Scalar, _StorageIndex> >
-{
-  protected:
-    typedef SparseSolverBase<IncompleteLUT> Base;
-    using Base::m_isInitialized;
-  public:
-    typedef _Scalar Scalar;
-    typedef _StorageIndex StorageIndex;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef Matrix<Scalar,Dynamic,1> Vector;
-    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
-    typedef SparseMatrix<Scalar,RowMajor,StorageIndex> FactorType;
+ * \class IncompleteLUT
+ * \brief Incomplete LU factorization with dual-threshold strategy
+ *
+ * \implsparsesolverconcept
+ *
+ * During the numerical factorization, two dropping rules are used :
+ *  1) any element whose magnitude is less than some tolerance is dropped.
+ *    This tolerance is obtained by multiplying the input tolerance @p droptol
+ *    by the average magnitude of all the original elements in the current row.
+ *  2) After the elimination of the row, only the @p fill largest elements in
+ *    the L part and the @p fill largest elements in the U part are kept
+ *    (in addition to the diagonal element ). Note that @p fill is computed from
+ *    the input parameter @p fillfactor which is used the ratio to control the fill_in
+ *    relatively to the initial number of nonzero elements.
+ *
+ * The two extreme cases are when @p droptol=0 (to keep all the @p fill*2 largest elements)
+ * and when @p fill=n/2 with @p droptol being different to zero.
+ *
+ * References : Yousef Saad, ILUT: A dual threshold incomplete LU factorization,
+ *              Numerical Linear Algebra with Applications, 1(4), pp 387-402, 1994.
+ *
+ * NOTE : The following implementation is derived from the ILUT implementation
+ * in the SPARSKIT package, Copyright (C) 2005, the Regents of the University of Minnesota
+ *  released under the terms of the GNU LGPL:
+ *    http://www-users.cs.umn.edu/~saad/software/SPARSKIT/README
+ * However, Yousef Saad gave us permission to relicense his ILUT code to MPL2.
+ * See the Eigen mailing list archive, thread: ILUT, date: July 8, 2012:
+ *   http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2012/07/msg00064.html
+ * alternatively, on GMANE:
+ *   http://comments.gmane.org/gmane.comp.lib.eigen/3302
+ */
+template <typename Scalar_, typename StorageIndex_ = int>
+class IncompleteLUT : public SparseSolverBase<IncompleteLUT<Scalar_, StorageIndex_> > {
+ protected:
+  typedef SparseSolverBase<IncompleteLUT> Base;
+  using Base::m_isInitialized;
 
-    enum {
-      ColsAtCompileTime = Dynamic,
-      MaxColsAtCompileTime = Dynamic
-    };
+ public:
+  typedef Scalar_ Scalar;
+  typedef StorageIndex_ StorageIndex;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Matrix<Scalar, Dynamic, 1> Vector;
+  typedef Matrix<StorageIndex, Dynamic, 1> VectorI;
+  typedef SparseMatrix<Scalar, RowMajor, StorageIndex> FactorType;
 
-  public:
+  enum { ColsAtCompileTime = Dynamic, MaxColsAtCompileTime = Dynamic };
 
-    IncompleteLUT()
-      : m_droptol(NumTraits<Scalar>::dummy_precision()), m_fillfactor(10),
-        m_analysisIsOk(false), m_factorizationIsOk(false)
-    {}
+ public:
+  IncompleteLUT()
+      : m_droptol(NumTraits<Scalar>::dummy_precision()),
+        m_fillfactor(10),
+        m_analysisIsOk(false),
+        m_factorizationIsOk(false) {}
 
-    template<typename MatrixType>
-    explicit IncompleteLUT(const MatrixType& mat, const RealScalar& droptol=NumTraits<Scalar>::dummy_precision(), int fillfactor = 10)
-      : m_droptol(droptol),m_fillfactor(fillfactor),
-        m_analysisIsOk(false),m_factorizationIsOk(false)
-    {
-      eigen_assert(fillfactor != 0);
-      compute(mat);
-    }
+  template <typename MatrixType>
+  explicit IncompleteLUT(const MatrixType& mat, const RealScalar& droptol = NumTraits<Scalar>::dummy_precision(),
+                         int fillfactor = 10)
+      : m_droptol(droptol), m_fillfactor(fillfactor), m_analysisIsOk(false), m_factorizationIsOk(false) {
+    eigen_assert(fillfactor != 0);
+    compute(mat);
+  }
 
-    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
+  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
 
-    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
+  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
 
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful,
-      *          \c NumericalIssue if the matrix.appears to be negative.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "IncompleteLUT is not initialized.");
-      return m_info;
-    }
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful,
+   *          \c NumericalIssue if the matrix.appears to be negative.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "IncompleteLUT is not initialized.");
+    return m_info;
+  }
 
-    template<typename MatrixType>
-    void analyzePattern(const MatrixType& amat);
+  template <typename MatrixType>
+  void analyzePattern(const MatrixType& amat);
 
-    template<typename MatrixType>
-    void factorize(const MatrixType& amat);
+  template <typename MatrixType>
+  void factorize(const MatrixType& amat);
 
-    /**
-      * Compute an incomplete LU factorization with dual threshold on the matrix mat
-      * No pivoting is done in this version
-      *
-      **/
-    template<typename MatrixType>
-    IncompleteLUT& compute(const MatrixType& amat)
-    {
-      analyzePattern(amat);
-      factorize(amat);
-      return *this;
-    }
+  /**
+   * Compute an incomplete LU factorization with dual threshold on the matrix mat
+   * No pivoting is done in this version
+   *
+   **/
+  template <typename MatrixType>
+  IncompleteLUT& compute(const MatrixType& amat) {
+    analyzePattern(amat);
+    factorize(amat);
+    return *this;
+  }
 
-    void setDroptol(const RealScalar& droptol);
-    void setFillfactor(int fillfactor);
+  void setDroptol(const RealScalar& droptol);
+  void setFillfactor(int fillfactor);
 
-    template<typename Rhs, typename Dest>
-    void _solve_impl(const Rhs& b, Dest& x) const
-    {
-      x = m_Pinv * b;
-      x = m_lu.template triangularView<UnitLower>().solve(x);
-      x = m_lu.template triangularView<Upper>().solve(x);
-      x = m_P * x;
-    }
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const Rhs& b, Dest& x) const {
+    x = m_Pinv * b;
+    x = m_lu.template triangularView<UnitLower>().solve(x);
+    x = m_lu.template triangularView<Upper>().solve(x);
+    x = m_P * x;
+  }
 
-protected:
+ protected:
+  /** keeps off-diagonal entries; drops diagonal entries */
+  struct keep_diag {
+    inline bool operator()(const Index& row, const Index& col, const Scalar&) const { return row != col; }
+  };
 
-    /** keeps off-diagonal entries; drops diagonal entries */
-    struct keep_diag {
-      inline bool operator() (const Index& row, const Index& col, const Scalar&) const
-      {
-        return row!=col;
-      }
-    };
-
-protected:
-
-    FactorType m_lu;
-    RealScalar m_droptol;
-    int m_fillfactor;
-    bool m_analysisIsOk;
-    bool m_factorizationIsOk;
-    ComputationInfo m_info;
-    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_P;     // Fill-reducing permutation
-    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_Pinv;  // Inverse permutation
+ protected:
+  FactorType m_lu;
+  RealScalar m_droptol;
+  int m_fillfactor;
+  bool m_analysisIsOk;
+  bool m_factorizationIsOk;
+  ComputationInfo m_info;
+  PermutationMatrix<Dynamic, Dynamic, StorageIndex> m_P;     // Fill-reducing permutation
+  PermutationMatrix<Dynamic, Dynamic, StorageIndex> m_Pinv;  // Inverse permutation
 };
 
 /**
  * Set control parameter droptol
  *  \param droptol   Drop any element whose magnitude is less than this tolerance
  **/
-template<typename Scalar, typename StorageIndex>
-void IncompleteLUT<Scalar,StorageIndex>::setDroptol(const RealScalar& droptol)
-{
+template <typename Scalar, typename StorageIndex>
+void IncompleteLUT<Scalar, StorageIndex>::setDroptol(const RealScalar& droptol) {
   this->m_droptol = droptol;
 }
 
@@ -212,53 +202,50 @@
  * Set control parameter fillfactor
  * \param fillfactor  This is used to compute the  number @p fill_in of largest elements to keep on each row.
  **/
-template<typename Scalar, typename StorageIndex>
-void IncompleteLUT<Scalar,StorageIndex>::setFillfactor(int fillfactor)
-{
+template <typename Scalar, typename StorageIndex>
+void IncompleteLUT<Scalar, StorageIndex>::setFillfactor(int fillfactor) {
   this->m_fillfactor = fillfactor;
 }
 
 template <typename Scalar, typename StorageIndex>
-template<typename _MatrixType>
-void IncompleteLUT<Scalar,StorageIndex>::analyzePattern(const _MatrixType& amat)
-{
+template <typename MatrixType_>
+void IncompleteLUT<Scalar, StorageIndex>::analyzePattern(const MatrixType_& amat) {
   // Compute the Fill-reducing permutation
   // Since ILUT does not perform any numerical pivoting,
   // it is highly preferable to keep the diagonal through symmetric permutations.
   // To this end, let's symmetrize the pattern and perform AMD on it.
-  SparseMatrix<Scalar,ColMajor, StorageIndex> mat1 = amat;
-  SparseMatrix<Scalar,ColMajor, StorageIndex> mat2 = amat.transpose();
+  SparseMatrix<Scalar, ColMajor, StorageIndex> mat1 = amat;
+  SparseMatrix<Scalar, ColMajor, StorageIndex> mat2 = amat.transpose();
   // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice.
   //       on the other hand for a really non-symmetric pattern, mat2*mat1 should be preferred...
-  SparseMatrix<Scalar,ColMajor, StorageIndex> AtA = mat2 + mat1;
+  SparseMatrix<Scalar, ColMajor, StorageIndex> AtA = mat2 + mat1;
   AMDOrdering<StorageIndex> ordering;
-  ordering(AtA,m_P);
-  m_Pinv  = m_P.inverse(); // cache the inverse permutation
+  ordering(AtA, m_P);
+  m_Pinv = m_P.inverse();  // cache the inverse permutation
   m_analysisIsOk = true;
   m_factorizationIsOk = false;
   m_isInitialized = true;
 }
 
 template <typename Scalar, typename StorageIndex>
-template<typename _MatrixType>
-void IncompleteLUT<Scalar,StorageIndex>::factorize(const _MatrixType& amat)
-{
+template <typename MatrixType_>
+void IncompleteLUT<Scalar, StorageIndex>::factorize(const MatrixType_& amat) {
+  using internal::convert_index;
+  using std::abs;
   using std::sqrt;
   using std::swap;
-  using std::abs;
-  using internal::convert_index;
 
   eigen_assert((amat.rows() == amat.cols()) && "The factorization should be done on a square matrix");
   Index n = amat.cols();  // Size of the matrix
-  m_lu.resize(n,n);
+  m_lu.resize(n, n);
   // Declare Working vectors and variables
-  Vector u(n) ;     // real values of the row -- maximum size is n --
-  VectorI ju(n);   // column position of the values in u -- maximum size  is n
-  VectorI jr(n);   // Indicate the position of the nonzero elements in the vector u -- A zero location is indicated by -1
+  Vector u(n);    // real values of the row -- maximum size is n --
+  VectorI ju(n);  // column position of the values in u -- maximum size  is n
+  VectorI jr(n);  // Indicate the position of the nonzero elements in the vector u -- A zero location is indicated by -1
 
   // Apply the fill-reducing permutation
   eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
-  SparseMatrix<Scalar,RowMajor, StorageIndex> mat;
+  SparseMatrix<Scalar, RowMajor, StorageIndex> mat;
   mat = amat.twistedBy(m_Pinv);
 
   // Initialization
@@ -267,44 +254,37 @@
   u.fill(0);
 
   // number of largest elements to keep in each row:
-  Index fill_in = (amat.nonZeros()*m_fillfactor)/n + 1;
+  Index fill_in = (amat.nonZeros() * m_fillfactor) / n + 1;
   if (fill_in > n) fill_in = n;
 
   // number of largest nonzero elements to keep in the L and the U part of the current row:
-  Index nnzL = fill_in/2;
+  Index nnzL = fill_in / 2;
   Index nnzU = nnzL;
   m_lu.reserve(n * (nnzL + nnzU + 1));
 
   // global loop over the rows of the sparse matrix
-  for (Index ii = 0; ii < n; ii++)
-  {
+  for (Index ii = 0; ii < n; ii++) {
     // 1 - copy the lower and the upper part of the row i of mat in the working vector u
 
-    Index sizeu = 1; // number of nonzero elements in the upper part of the current row
-    Index sizel = 0; // number of nonzero elements in the lower part of the current row
-    ju(ii)    = convert_index<StorageIndex>(ii);
-    u(ii)     = 0;
-    jr(ii)    = convert_index<StorageIndex>(ii);
+    Index sizeu = 1;  // number of nonzero elements in the upper part of the current row
+    Index sizel = 0;  // number of nonzero elements in the lower part of the current row
+    ju(ii) = convert_index<StorageIndex>(ii);
+    u(ii) = 0;
+    jr(ii) = convert_index<StorageIndex>(ii);
     RealScalar rownorm = 0;
 
-    typename FactorType::InnerIterator j_it(mat, ii); // Iterate through the current row ii
-    for (; j_it; ++j_it)
-    {
+    typename FactorType::InnerIterator j_it(mat, ii);  // Iterate through the current row ii
+    for (; j_it; ++j_it) {
       Index k = j_it.index();
-      if (k < ii)
-      {
+      if (k < ii) {
         // copy the lower part
         ju(sizel) = convert_index<StorageIndex>(k);
         u(sizel) = j_it.value();
         jr(k) = convert_index<StorageIndex>(sizel);
         ++sizel;
-      }
-      else if (k == ii)
-      {
+      } else if (k == ii) {
         u(ii) = j_it.value();
-      }
-      else
-      {
+      } else {
         // copy the upper part
         Index jpos = ii + sizeu;
         ju(jpos) = convert_index<StorageIndex>(k);
@@ -316,8 +296,7 @@
     }
 
     // 2 - detect possible zero row
-    if(rownorm==0)
-    {
+    if (rownorm == 0) {
       m_info = NumericalIssue;
       return;
     }
@@ -327,15 +306,13 @@
     // 3 - eliminate the previous nonzero rows
     Index jj = 0;
     Index len = 0;
-    while (jj < sizel)
-    {
+    while (jj < sizel) {
       // In order to eliminate in the correct order,
       // we must select first the smallest column index among  ju(jj:sizel)
       Index k;
-      Index minrow = ju.segment(jj,sizel-jj).minCoeff(&k); // k is relative to the segment
+      Index minrow = ju.segment(jj, sizel - jj).minCoeff(&k);  // k is relative to the segment
       k += jj;
-      if (minrow != ju(jj))
-      {
+      if (minrow != ju(jj)) {
         // swap the two locations
         Index j = ju(jj);
         swap(ju(jj), ju(k));
@@ -349,55 +326,51 @@
       // Start elimination
       typename FactorType::InnerIterator ki_it(m_lu, minrow);
       while (ki_it && ki_it.index() < minrow) ++ki_it;
-      eigen_internal_assert(ki_it && ki_it.col()==minrow);
+      eigen_internal_assert(ki_it && ki_it.col() == minrow);
       Scalar fact = u(jj) / ki_it.value();
 
       // drop too small elements
-      if(abs(fact) <= m_droptol)
-      {
+      if (abs(fact) <= m_droptol) {
         jj++;
         continue;
       }
 
       // linear combination of the current row ii and the row minrow
       ++ki_it;
-      for (; ki_it; ++ki_it)
-      {
+      for (; ki_it; ++ki_it) {
         Scalar prod = fact * ki_it.value();
-        Index j     = ki_it.index();
-        Index jpos  = jr(j);
-        if (jpos == -1) // fill-in element
+        Index j = ki_it.index();
+        Index jpos = jr(j);
+        if (jpos == -1)  // fill-in element
         {
           Index newpos;
-          if (j >= ii) // dealing with the upper part
+          if (j >= ii)  // dealing with the upper part
           {
             newpos = ii + sizeu;
             sizeu++;
-            eigen_internal_assert(sizeu<=n);
-          }
-          else // dealing with the lower part
+            eigen_internal_assert(sizeu <= n);
+          } else  // dealing with the lower part
           {
             newpos = sizel;
             sizel++;
-            eigen_internal_assert(sizel<=ii);
+            eigen_internal_assert(sizel <= ii);
           }
           ju(newpos) = convert_index<StorageIndex>(j);
           u(newpos) = -prod;
           jr(j) = convert_index<StorageIndex>(newpos);
-        }
-        else
+        } else
           u(jpos) -= prod;
       }
       // store the pivot element
-      u(len)  = fact;
+      u(len) = fact;
       ju(len) = convert_index<StorageIndex>(minrow);
       ++len;
 
       jj++;
-    } // end of the elimination on the row ii
+    }  // end of the elimination on the row ii
 
     // reset the upper part of the pointer jr to zero
-    for(Index k = 0; k <sizeu; k++) jr(ju(ii+k)) = -1;
+    for (Index k = 0; k < sizeu; k++) jr(ju(ii + k)) = -1;
 
     // 4 - partially sort and insert the elements in the m_lu matrix
 
@@ -410,36 +383,31 @@
 
     // store the largest m_fill elements of the L part
     m_lu.startVec(ii);
-    for(Index k = 0; k < len; k++)
-      m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
+    for (Index k = 0; k < len; k++) m_lu.insertBackByOuterInnerUnordered(ii, ju(k)) = u(k);
 
     // store the diagonal element
     // apply a shifting rule to avoid zero pivots (we are doing an incomplete factorization)
-    if (u(ii) == Scalar(0))
-      u(ii) = sqrt(m_droptol) * rownorm;
+    if (u(ii) == Scalar(0)) u(ii) = sqrt(m_droptol) * rownorm;
     m_lu.insertBackByOuterInnerUnordered(ii, ii) = u(ii);
 
     // sort the U-part of the row
     // apply the dropping rule first
     len = 0;
-    for(Index k = 1; k < sizeu; k++)
-    {
-      if(abs(u(ii+k)) > m_droptol * rownorm )
-      {
+    for (Index k = 1; k < sizeu; k++) {
+      if (abs(u(ii + k)) > m_droptol * rownorm) {
         ++len;
-        u(ii + len)  = u(ii + k);
+        u(ii + len) = u(ii + k);
         ju(ii + len) = ju(ii + k);
       }
     }
-    sizeu = len + 1; // +1 to take into account the diagonal element
+    sizeu = len + 1;  // +1 to take into account the diagonal element
     len = (std::min)(sizeu, nnzU);
-    typename Vector::SegmentReturnType uu(u.segment(ii+1, sizeu-1));
-    typename VectorI::SegmentReturnType juu(ju.segment(ii+1, sizeu-1));
+    typename Vector::SegmentReturnType uu(u.segment(ii + 1, sizeu - 1));
+    typename VectorI::SegmentReturnType juu(ju.segment(ii + 1, sizeu - 1));
     internal::QuickSplit(uu, juu, len);
 
     // store the largest elements of the U part
-    for(Index k = ii + 1; k < ii + len; k++)
-      m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
+    for (Index k = ii + 1; k < ii + len; k++) m_lu.insertBackByOuterInnerUnordered(ii, ju(k)) = u(k);
   }
   m_lu.finalize();
   m_lu.makeCompressed();
@@ -448,6 +416,6 @@
   m_info = Success;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_INCOMPLETE_LUT_H
+#endif  // EIGEN_INCOMPLETE_LUT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/InternalHeaderCheck.h
new file mode 100644
index 0000000..b657e84
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
+#error "Please include Eigen/IterativeLinearSolvers instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
index 28a0c51..cf85f2e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
@@ -10,192 +10,164 @@
 #ifndef EIGEN_ITERATIVE_SOLVER_BASE_H
 #define EIGEN_ITERATIVE_SOLVER_BASE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename MatrixType>
-struct is_ref_compatible_impl
-{
-private:
+template <typename MatrixType>
+struct is_ref_compatible_impl {
+ private:
   template <typename T0>
-  struct any_conversion
-  {
-    template <typename T> any_conversion(const volatile T&);
-    template <typename T> any_conversion(T&);
+  struct any_conversion {
+    template <typename T>
+    any_conversion(const volatile T&);
+    template <typename T>
+    any_conversion(T&);
   };
-  struct yes {int a[1];};
-  struct no  {int a[2];};
+  struct yes {
+    int a[1];
+  };
+  struct no {
+    int a[2];
+  };
 
-  template<typename T>
+  template <typename T>
   static yes test(const Ref<const T>&, int);
-  template<typename T>
-  static no  test(any_conversion<T>, ...);
+  template <typename T>
+  static no test(any_conversion<T>, ...);
 
-public:
+ public:
   static MatrixType ms_from;
-  enum { value = sizeof(test<MatrixType>(ms_from, 0))==sizeof(yes) };
+  enum { value = sizeof(test<MatrixType>(ms_from, 0)) == sizeof(yes) };
 };
 
-template<typename MatrixType>
-struct is_ref_compatible
-{
-  enum { value = is_ref_compatible_impl<typename remove_all<MatrixType>::type>::value };
+template <typename MatrixType>
+struct is_ref_compatible {
+  enum { value = is_ref_compatible_impl<remove_all_t<MatrixType>>::value };
 };
 
-template<typename MatrixType, bool MatrixFree = !internal::is_ref_compatible<MatrixType>::value>
+template <typename MatrixType, bool MatrixFree = !internal::is_ref_compatible<MatrixType>::value>
 class generic_matrix_wrapper;
 
 // We have an explicit matrix at hand, compatible with Ref<>
-template<typename MatrixType>
-class generic_matrix_wrapper<MatrixType,false>
-{
-public:
+template <typename MatrixType>
+class generic_matrix_wrapper<MatrixType, false> {
+ public:
   typedef Ref<const MatrixType> ActualMatrixType;
-  template<int UpLo> struct ConstSelfAdjointViewReturnType {
+  template <int UpLo>
+  struct ConstSelfAdjointViewReturnType {
     typedef typename ActualMatrixType::template ConstSelfAdjointViewReturnType<UpLo>::Type Type;
   };
 
-  enum {
-    MatrixFree = false
-  };
+  enum { MatrixFree = false };
 
-  generic_matrix_wrapper()
-    : m_dummy(0,0), m_matrix(m_dummy)
-  {}
+  generic_matrix_wrapper() : m_dummy(0, 0), m_matrix(m_dummy) {}
 
-  template<typename InputType>
-  generic_matrix_wrapper(const InputType &mat)
-    : m_matrix(mat)
-  {}
+  template <typename InputType>
+  generic_matrix_wrapper(const InputType& mat) : m_matrix(mat) {}
 
-  const ActualMatrixType& matrix() const
-  {
-    return m_matrix;
+  const ActualMatrixType& matrix() const { return m_matrix; }
+
+  template <typename MatrixDerived>
+  void grab(const EigenBase<MatrixDerived>& mat) {
+    internal::destroy_at(&m_matrix);
+    internal::construct_at(&m_matrix, mat.derived());
   }
 
-  template<typename MatrixDerived>
-  void grab(const EigenBase<MatrixDerived> &mat)
-  {
-    m_matrix.~Ref<const MatrixType>();
-    ::new (&m_matrix) Ref<const MatrixType>(mat.derived());
-  }
-
-  void grab(const Ref<const MatrixType> &mat)
-  {
-    if(&(mat.derived()) != &m_matrix)
-    {
-      m_matrix.~Ref<const MatrixType>();
-      ::new (&m_matrix) Ref<const MatrixType>(mat);
+  void grab(const Ref<const MatrixType>& mat) {
+    if (&(mat.derived()) != &m_matrix) {
+      internal::destroy_at(&m_matrix);
+      internal::construct_at(&m_matrix, mat);
     }
   }
 
-protected:
-  MatrixType m_dummy; // used to default initialize the Ref<> object
+ protected:
+  MatrixType m_dummy;  // used to default initialize the Ref<> object
   ActualMatrixType m_matrix;
 };
 
 // MatrixType is not compatible with Ref<> -> matrix-free wrapper
-template<typename MatrixType>
-class generic_matrix_wrapper<MatrixType,true>
-{
-public:
+template <typename MatrixType>
+class generic_matrix_wrapper<MatrixType, true> {
+ public:
   typedef MatrixType ActualMatrixType;
-  template<int UpLo> struct ConstSelfAdjointViewReturnType
-  {
+  template <int UpLo>
+  struct ConstSelfAdjointViewReturnType {
     typedef ActualMatrixType Type;
   };
 
-  enum {
-    MatrixFree = true
-  };
+  enum { MatrixFree = true };
 
-  generic_matrix_wrapper()
-    : mp_matrix(0)
-  {}
+  generic_matrix_wrapper() : mp_matrix(0) {}
 
-  generic_matrix_wrapper(const MatrixType &mat)
-    : mp_matrix(&mat)
-  {}
+  generic_matrix_wrapper(const MatrixType& mat) : mp_matrix(&mat) {}
 
-  const ActualMatrixType& matrix() const
-  {
-    return *mp_matrix;
-  }
+  const ActualMatrixType& matrix() const { return *mp_matrix; }
 
-  void grab(const MatrixType &mat)
-  {
-    mp_matrix = &mat;
-  }
+  void grab(const MatrixType& mat) { mp_matrix = &mat; }
 
-protected:
-  const ActualMatrixType *mp_matrix;
+ protected:
+  const ActualMatrixType* mp_matrix;
 };
 
-}
+}  // namespace internal
 
 /** \ingroup IterativeLinearSolvers_Module
-  * \brief Base class for linear iterative solvers
-  *
-  * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
-  */
-template< typename Derived>
-class IterativeSolverBase : public SparseSolverBase<Derived>
-{
-protected:
+ * \brief Base class for linear iterative solvers
+ *
+ * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+ */
+template <typename Derived>
+class IterativeSolverBase : public SparseSolverBase<Derived> {
+ protected:
   typedef SparseSolverBase<Derived> Base;
   using Base::m_isInitialized;
 
-public:
+ public:
   typedef typename internal::traits<Derived>::MatrixType MatrixType;
   typedef typename internal::traits<Derived>::Preconditioner Preconditioner;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::StorageIndex StorageIndex;
   typedef typename MatrixType::RealScalar RealScalar;
 
-  enum {
-    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-  };
+  enum { ColsAtCompileTime = MatrixType::ColsAtCompileTime, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime };
 
-public:
-
+ public:
   using Base::derived;
 
   /** Default constructor. */
-  IterativeSolverBase()
-  {
-    init();
-  }
+  IterativeSolverBase() { init(); }
 
   /** Initialize the solver with matrix \a A for further \c Ax=b solving.
-    *
-    * This constructor is a shortcut for the default constructor followed
-    * by a call to compute().
-    *
-    * \warning this class stores a reference to the matrix A as well as some
-    * precomputed values that depend on it. Therefore, if \a A is changed
-    * this class becomes invalid. Call compute() to update it with the new
-    * matrix A, or modify a copy of A.
-    */
-  template<typename MatrixDerived>
-  explicit IterativeSolverBase(const EigenBase<MatrixDerived>& A)
-    : m_matrixWrapper(A.derived())
-  {
+   *
+   * This constructor is a shortcut for the default constructor followed
+   * by a call to compute().
+   *
+   * \warning this class stores a reference to the matrix A as well as some
+   * precomputed values that depend on it. Therefore, if \a A is changed
+   * this class becomes invalid. Call compute() to update it with the new
+   * matrix A, or modify a copy of A.
+   */
+  template <typename MatrixDerived>
+  explicit IterativeSolverBase(const EigenBase<MatrixDerived>& A) : m_matrixWrapper(A.derived()) {
     init();
     compute(matrix());
   }
 
+  IterativeSolverBase(IterativeSolverBase&&) = default;
+
   ~IterativeSolverBase() {}
 
   /** Initializes the iterative solver for the sparsity pattern of the matrix \a A for further solving \c Ax=b problems.
-    *
-    * Currently, this function mostly calls analyzePattern on the preconditioner. In the future
-    * we might, for instance, implement column reordering for faster matrix vector products.
-    */
-  template<typename MatrixDerived>
-  Derived& analyzePattern(const EigenBase<MatrixDerived>& A)
-  {
+   *
+   * Currently, this function mostly calls analyzePattern on the preconditioner. In the future
+   * we might, for instance, implement column reordering for faster matrix vector products.
+   */
+  template <typename MatrixDerived>
+  Derived& analyzePattern(const EigenBase<MatrixDerived>& A) {
     grab(A.derived());
     m_preconditioner.analyzePattern(matrix());
     m_isInitialized = true;
@@ -204,18 +176,18 @@
     return derived();
   }
 
-  /** Initializes the iterative solver with the numerical values of the matrix \a A for further solving \c Ax=b problems.
-    *
-    * Currently, this function mostly calls factorize on the preconditioner.
-    *
-    * \warning this class stores a reference to the matrix A as well as some
-    * precomputed values that depend on it. Therefore, if \a A is changed
-    * this class becomes invalid. Call compute() to update it with the new
-    * matrix A, or modify a copy of A.
-    */
-  template<typename MatrixDerived>
-  Derived& factorize(const EigenBase<MatrixDerived>& A)
-  {
+  /** Initializes the iterative solver with the numerical values of the matrix \a A for further solving \c Ax=b
+   * problems.
+   *
+   * Currently, this function mostly calls factorize on the preconditioner.
+   *
+   * \warning this class stores a reference to the matrix A as well as some
+   * precomputed values that depend on it. Therefore, if \a A is changed
+   * this class becomes invalid. Call compute() to update it with the new
+   * matrix A, or modify a copy of A.
+   */
+  template <typename MatrixDerived>
+  Derived& factorize(const EigenBase<MatrixDerived>& A) {
     eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
     grab(A.derived());
     m_preconditioner.factorize(matrix());
@@ -225,18 +197,17 @@
   }
 
   /** Initializes the iterative solver with the matrix \a A for further solving \c Ax=b problems.
-    *
-    * Currently, this function mostly initializes/computes the preconditioner. In the future
-    * we might, for instance, implement column reordering for faster matrix vector products.
-    *
-    * \warning this class stores a reference to the matrix A as well as some
-    * precomputed values that depend on it. Therefore, if \a A is changed
-    * this class becomes invalid. Call compute() to update it with the new
-    * matrix A, or modify a copy of A.
-    */
-  template<typename MatrixDerived>
-  Derived& compute(const EigenBase<MatrixDerived>& A)
-  {
+   *
+   * Currently, this function mostly initializes/computes the preconditioner. In the future
+   * we might, for instance, implement column reordering for faster matrix vector products.
+   *
+   * \warning this class stores a reference to the matrix A as well as some
+   * precomputed values that depend on it. Therefore, if \a A is changed
+   * this class becomes invalid. Call compute() to update it with the new
+   * matrix A, or modify a copy of A.
+   */
+  template <typename MatrixDerived>
+  Derived& compute(const EigenBase<MatrixDerived>& A) {
     grab(A.derived());
     m_preconditioner.compute(matrix());
     m_isInitialized = true;
@@ -253,17 +224,16 @@
   EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return matrix().cols(); }
 
   /** \returns the tolerance threshold used by the stopping criteria.
-    * \sa setTolerance()
-    */
+   * \sa setTolerance()
+   */
   RealScalar tolerance() const { return m_tolerance; }
 
   /** Sets the tolerance threshold used by the stopping criteria.
-    *
-    * This value is used as an upper bound to the relative residual error: |Ax-b|/|b|.
-    * The default value is the machine precision given by NumTraits<Scalar>::epsilon()
-    */
-  Derived& setTolerance(const RealScalar& tolerance)
-  {
+   *
+   * This value is used as an upper bound to the relative residual error: |Ax-b|/|b|.
+   * The default value is the machine precision given by NumTraits<Scalar>::epsilon()
+   */
+  Derived& setTolerance(const RealScalar& tolerance) {
     m_tolerance = tolerance;
     return derived();
   }
@@ -275,137 +245,122 @@
   const Preconditioner& preconditioner() const { return m_preconditioner; }
 
   /** \returns the max number of iterations.
-    * It is either the value set by setMaxIterations or, by default,
-    * twice the number of columns of the matrix.
-    */
-  Index maxIterations() const
-  {
-    return (m_maxIterations<0) ? 2*matrix().cols() : m_maxIterations;
-  }
+   * It is either the value set by setMaxIterations or, by default,
+   * twice the number of columns of the matrix.
+   */
+  Index maxIterations() const { return (m_maxIterations < 0) ? 2 * matrix().cols() : m_maxIterations; }
 
   /** Sets the max number of iterations.
-    * Default is twice the number of columns of the matrix.
-    */
-  Derived& setMaxIterations(Index maxIters)
-  {
+   * Default is twice the number of columns of the matrix.
+   */
+  Derived& setMaxIterations(Index maxIters) {
     m_maxIterations = maxIters;
     return derived();
   }
 
   /** \returns the number of iterations performed during the last solve */
-  Index iterations() const
-  {
-    eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
+  Index iterations() const {
+    eigen_assert(m_isInitialized && "IterativeSolverBase is not initialized.");
     return m_iterations;
   }
 
   /** \returns the tolerance error reached during the last solve.
-    * It is a close approximation of the true relative residual error |Ax-b|/|b|.
-    */
-  RealScalar error() const
-  {
-    eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
+   * It is a close approximation of the true relative residual error |Ax-b|/|b|.
+   */
+  RealScalar error() const {
+    eigen_assert(m_isInitialized && "IterativeSolverBase is not initialized.");
     return m_error;
   }
 
   /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A
-    * and \a x0 as an initial solution.
-    *
-    * \sa solve(), compute()
-    */
-  template<typename Rhs,typename Guess>
-  inline const SolveWithGuess<Derived, Rhs, Guess>
-  solveWithGuess(const MatrixBase<Rhs>& b, const Guess& x0) const
-  {
+   * and \a x0 as an initial solution.
+   *
+   * \sa solve(), compute()
+   */
+  template <typename Rhs, typename Guess>
+  inline const SolveWithGuess<Derived, Rhs, Guess> solveWithGuess(const MatrixBase<Rhs>& b, const Guess& x0) const {
     eigen_assert(m_isInitialized && "Solver is not initialized.");
-    eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+    eigen_assert(derived().rows() == b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
     return SolveWithGuess<Derived, Rhs, Guess>(derived(), b.derived(), x0);
   }
 
   /** \returns Success if the iterations converged, and NoConvergence otherwise. */
-  ComputationInfo info() const
-  {
+  ComputationInfo info() const {
     eigen_assert(m_isInitialized && "IterativeSolverBase is not initialized.");
     return m_info;
   }
 
   /** \internal */
-  template<typename Rhs, typename DestDerived>
-  void _solve_with_guess_impl(const Rhs& b, SparseMatrixBase<DestDerived> &aDest) const
-  {
-    eigen_assert(rows()==b.rows());
+  template <typename Rhs, typename DestDerived>
+  void _solve_with_guess_impl(const Rhs& b, SparseMatrixBase<DestDerived>& aDest) const {
+    eigen_assert(rows() == b.rows());
 
     Index rhsCols = b.cols();
     Index size = b.rows();
     DestDerived& dest(aDest.derived());
     typedef typename DestDerived::Scalar DestScalar;
-    Eigen::Matrix<DestScalar,Dynamic,1> tb(size);
-    Eigen::Matrix<DestScalar,Dynamic,1> tx(cols());
+    Eigen::Matrix<DestScalar, Dynamic, 1> tb(size);
+    Eigen::Matrix<DestScalar, Dynamic, 1> tx(cols());
     // We do not directly fill dest because sparse expressions have to be free of aliasing issue.
-    // For non square least-square problems, b and dest might not have the same size whereas they might alias each-other.
-    typename DestDerived::PlainObject tmp(cols(),rhsCols);
+    // For non square least-square problems, b and dest might not have the same size whereas they might alias
+    // each-other.
+    typename DestDerived::PlainObject tmp(cols(), rhsCols);
     ComputationInfo global_info = Success;
-    for(Index k=0; k<rhsCols; ++k)
-    {
+    for (Index k = 0; k < rhsCols; ++k) {
       tb = b.col(k);
       tx = dest.col(k);
-      derived()._solve_vector_with_guess_impl(tb,tx);
+      derived()._solve_vector_with_guess_impl(tb, tx);
       tmp.col(k) = tx.sparseView(0);
 
       // The call to _solve_vector_with_guess_impl updates m_info, so if it failed for a previous column
       // we need to restore it to the worst value.
-      if(m_info==NumericalIssue)
+      if (m_info == NumericalIssue)
         global_info = NumericalIssue;
-      else if(m_info==NoConvergence)
+      else if (m_info == NoConvergence)
         global_info = NoConvergence;
     }
     m_info = global_info;
     dest.swap(tmp);
   }
 
-  template<typename Rhs, typename DestDerived>
-  typename internal::enable_if<Rhs::ColsAtCompileTime!=1 && DestDerived::ColsAtCompileTime!=1>::type
-  _solve_with_guess_impl(const Rhs& b, MatrixBase<DestDerived> &aDest) const
-  {
-    eigen_assert(rows()==b.rows());
+  template <typename Rhs, typename DestDerived>
+  std::enable_if_t<Rhs::ColsAtCompileTime != 1 && DestDerived::ColsAtCompileTime != 1> _solve_with_guess_impl(
+      const Rhs& b, MatrixBase<DestDerived>& aDest) const {
+    eigen_assert(rows() == b.rows());
 
     Index rhsCols = b.cols();
     DestDerived& dest(aDest.derived());
     ComputationInfo global_info = Success;
-    for(Index k=0; k<rhsCols; ++k)
-    {
-      typename DestDerived::ColXpr xk(dest,k);
-      typename Rhs::ConstColXpr bk(b,k);
-      derived()._solve_vector_with_guess_impl(bk,xk);
+    for (Index k = 0; k < rhsCols; ++k) {
+      typename DestDerived::ColXpr xk(dest, k);
+      typename Rhs::ConstColXpr bk(b, k);
+      derived()._solve_vector_with_guess_impl(bk, xk);
 
       // The call to _solve_vector_with_guess updates m_info, so if it failed for a previous column
       // we need to restore it to the worst value.
-      if(m_info==NumericalIssue)
+      if (m_info == NumericalIssue)
         global_info = NumericalIssue;
-      else if(m_info==NoConvergence)
+      else if (m_info == NoConvergence)
         global_info = NoConvergence;
     }
     m_info = global_info;
   }
 
-  template<typename Rhs, typename DestDerived>
-  typename internal::enable_if<Rhs::ColsAtCompileTime==1 || DestDerived::ColsAtCompileTime==1>::type
-  _solve_with_guess_impl(const Rhs& b, MatrixBase<DestDerived> &dest) const
-  {
-    derived()._solve_vector_with_guess_impl(b,dest.derived());
+  template <typename Rhs, typename DestDerived>
+  std::enable_if_t<Rhs::ColsAtCompileTime == 1 || DestDerived::ColsAtCompileTime == 1> _solve_with_guess_impl(
+      const Rhs& b, MatrixBase<DestDerived>& dest) const {
+    derived()._solve_vector_with_guess_impl(b, dest.derived());
   }
 
   /** \internal default initial guess = 0 */
-  template<typename Rhs,typename Dest>
-  void _solve_impl(const Rhs& b, Dest& x) const
-  {
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const Rhs& b, Dest& x) const {
     x.setZero();
-    derived()._solve_with_guess_impl(b,x);
+    derived()._solve_with_guess_impl(b, x);
   }
 
-protected:
-  void init()
-  {
+ protected:
+  void init() {
     m_isInitialized = false;
     m_analysisIsOk = false;
     m_factorizationIsOk = false;
@@ -416,14 +371,10 @@
   typedef internal::generic_matrix_wrapper<MatrixType> MatrixWrapper;
   typedef typename MatrixWrapper::ActualMatrixType ActualMatrixType;
 
-  const ActualMatrixType& matrix() const
-  {
-    return m_matrixWrapper.matrix();
-  }
+  const ActualMatrixType& matrix() const { return m_matrixWrapper.matrix(); }
 
-  template<typename InputType>
-  void grab(const InputType &A)
-  {
+  template <typename InputType>
+  void grab(const InputType& A) {
     m_matrixWrapper.grab(A);
   }
 
@@ -439,6 +390,6 @@
   mutable bool m_analysisIsOk, m_factorizationIsOk;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ITERATIVE_SOLVER_BASE_H
+#endif  // EIGEN_ITERATIVE_SOLVER_BASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
index 203fd0e..182f319 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
@@ -10,121 +10,118 @@
 #ifndef EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
 #define EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \internal Low-level conjugate gradient algorithm for least-square problems
-  * \param mat The matrix A
-  * \param rhs The right hand side vector b
-  * \param x On input and initial solution, on output the computed solution.
-  * \param precond A preconditioner being able to efficiently solve for an
-  *                approximation of A'Ax=b (regardless of b)
-  * \param iters On input the max number of iteration, on output the number of performed iterations.
-  * \param tol_error On input the tolerance error, on output an estimation of the relative error.
-  */
-template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
-EIGEN_DONT_INLINE
-void least_square_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
-                                     const Preconditioner& precond, Index& iters,
-                                     typename Dest::RealScalar& tol_error)
-{
-  using std::sqrt;
+ * \param mat The matrix A
+ * \param rhs The right hand side vector b
+ * \param x On input and initial solution, on output the computed solution.
+ * \param precond A preconditioner being able to efficiently solve for an
+ *                approximation of A'Ax=b (regardless of b)
+ * \param iters On input the max number of iteration, on output the number of performed iterations.
+ * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+ */
+template <typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+EIGEN_DONT_INLINE void least_square_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
+                                                       const Preconditioner& precond, Index& iters,
+                                                       typename Dest::RealScalar& tol_error) {
   using std::abs;
+  using std::sqrt;
   typedef typename Dest::RealScalar RealScalar;
   typedef typename Dest::Scalar Scalar;
-  typedef Matrix<Scalar,Dynamic,1> VectorType;
-  
+  typedef Matrix<Scalar, Dynamic, 1> VectorType;
+
   RealScalar tol = tol_error;
   Index maxIters = iters;
-  
+
   Index m = mat.rows(), n = mat.cols();
 
-  VectorType residual        = rhs - mat * x;
+  VectorType residual = rhs - mat * x;
   VectorType normal_residual = mat.adjoint() * residual;
 
-  RealScalar rhsNorm2 = (mat.adjoint()*rhs).squaredNorm();
-  if(rhsNorm2 == 0) 
-  {
+  RealScalar rhsNorm2 = (mat.adjoint() * rhs).squaredNorm();
+  if (rhsNorm2 == 0) {
     x.setZero();
     iters = 0;
     tol_error = 0;
     return;
   }
-  RealScalar threshold = tol*tol*rhsNorm2;
+  RealScalar threshold = tol * tol * rhsNorm2;
   RealScalar residualNorm2 = normal_residual.squaredNorm();
-  if (residualNorm2 < threshold)
-  {
+  if (residualNorm2 < threshold) {
     iters = 0;
     tol_error = sqrt(residualNorm2 / rhsNorm2);
     return;
   }
-  
+
   VectorType p(n);
-  p = precond.solve(normal_residual);                         // initial search direction
+  p = precond.solve(normal_residual);  // initial search direction
 
   VectorType z(n), tmp(m);
   RealScalar absNew = numext::real(normal_residual.dot(p));  // the square of the absolute value of r scaled by invM
   Index i = 0;
-  while(i < maxIters)
-  {
+  while (i < maxIters) {
     tmp.noalias() = mat * p;
 
-    Scalar alpha = absNew / tmp.squaredNorm();      // the amount we travel on dir
-    x += alpha * p;                                 // update solution
-    residual -= alpha * tmp;                        // update residual
-    normal_residual = mat.adjoint() * residual;     // update residual of the normal equation
-    
+    Scalar alpha = absNew / tmp.squaredNorm();             // the amount we travel on dir
+    x += alpha * p;                                        // update solution
+    residual -= alpha * tmp;                               // update residual
+    normal_residual.noalias() = mat.adjoint() * residual;  // update residual of the normal equation
+
     residualNorm2 = normal_residual.squaredNorm();
-    if(residualNorm2 < threshold)
-      break;
-    
-    z = precond.solve(normal_residual);             // approximately solve for "A'A z = normal_residual"
+    if (residualNorm2 < threshold) break;
+
+    z = precond.solve(normal_residual);  // approximately solve for "A'A z = normal_residual"
 
     RealScalar absOld = absNew;
     absNew = numext::real(normal_residual.dot(z));  // update the absolute value of r
-    RealScalar beta = absNew / absOld;              // calculate the Gram-Schmidt value used to create the new search direction
-    p = z + beta * p;                               // update search direction
+    RealScalar beta = absNew / absOld;  // calculate the Gram-Schmidt value used to create the new search direction
+    p = z + beta * p;                   // update search direction
     i++;
   }
   tol_error = sqrt(residualNorm2 / rhsNorm2);
   iters = i;
 }
 
-}
+}  // namespace internal
 
-template< typename _MatrixType,
-          typename _Preconditioner = LeastSquareDiagonalPreconditioner<typename _MatrixType::Scalar> >
+template <typename MatrixType_,
+          typename Preconditioner_ = LeastSquareDiagonalPreconditioner<typename MatrixType_::Scalar> >
 class LeastSquaresConjugateGradient;
 
 namespace internal {
 
-template< typename _MatrixType, typename _Preconditioner>
-struct traits<LeastSquaresConjugateGradient<_MatrixType,_Preconditioner> >
-{
-  typedef _MatrixType MatrixType;
-  typedef _Preconditioner Preconditioner;
+template <typename MatrixType_, typename Preconditioner_>
+struct traits<LeastSquaresConjugateGradient<MatrixType_, Preconditioner_> > {
+  typedef MatrixType_ MatrixType;
+  typedef Preconditioner_ Preconditioner;
 };
 
-}
+}  // namespace internal
 
 /** \ingroup IterativeLinearSolvers_Module
   * \brief A conjugate gradient solver for sparse (or dense) least-square problems
   *
-  * This class allows to solve for A x = b linear problems using an iterative conjugate gradient algorithm.
-  * The matrix A can be non symmetric and rectangular, but the matrix A' A should be positive-definite to guaranty stability.
+  * This class solves for the least-squares solution to A x = b using an iterative conjugate gradient algorithm.
+  * The matrix A can be non symmetric and rectangular, but the matrix A' A should be positive-definite to guaranty
+  stability.
   * Otherwise, the SparseLU or SparseQR classes might be preferable.
   * The matrix A and the vectors x and b can be either dense or sparse.
   *
-  * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix.
-  * \tparam _Preconditioner the type of the preconditioner. Default is LeastSquareDiagonalPreconditioner
+  * \tparam MatrixType_ the type of the matrix A, can be a dense or a sparse matrix.
+  * \tparam Preconditioner_ the type of the preconditioner. Default is LeastSquareDiagonalPreconditioner
   *
   * \implsparsesolverconcept
-  * 
+  *
   * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
   * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
   * and NumTraits<Scalar>::epsilon() for the tolerance.
-  * 
+  *
   * This class can be used as the direct solver classes. Here is a typical usage example:
     \code
     int m=1000000, n = 10000;
@@ -139,60 +136,58 @@
     // update b, and solve again
     x = lscg.solve(b);
     \endcode
-  * 
+  *
   * By default the iterations start with x=0 as an initial guess of the solution.
   * One can control the start using the solveWithGuess() method.
-  * 
+  *
   * \sa class ConjugateGradient, SparseLU, SparseQR
   */
-template< typename _MatrixType, typename _Preconditioner>
-class LeastSquaresConjugateGradient : public IterativeSolverBase<LeastSquaresConjugateGradient<_MatrixType,_Preconditioner> >
-{
+template <typename MatrixType_, typename Preconditioner_>
+class LeastSquaresConjugateGradient
+    : public IterativeSolverBase<LeastSquaresConjugateGradient<MatrixType_, Preconditioner_> > {
   typedef IterativeSolverBase<LeastSquaresConjugateGradient> Base;
-  using Base::matrix;
   using Base::m_error;
-  using Base::m_iterations;
   using Base::m_info;
   using Base::m_isInitialized;
-public:
-  typedef _MatrixType MatrixType;
+  using Base::m_iterations;
+  using Base::matrix;
+
+ public:
+  typedef MatrixType_ MatrixType;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
-  typedef _Preconditioner Preconditioner;
+  typedef Preconditioner_ Preconditioner;
 
-public:
-
+ public:
   /** Default constructor. */
   LeastSquaresConjugateGradient() : Base() {}
 
   /** Initialize the solver with matrix \a A for further \c Ax=b solving.
-    * 
-    * This constructor is a shortcut for the default constructor followed
-    * by a call to compute().
-    * 
-    * \warning this class stores a reference to the matrix A as well as some
-    * precomputed values that depend on it. Therefore, if \a A is changed
-    * this class becomes invalid. Call compute() to update it with the new
-    * matrix A, or modify a copy of A.
-    */
-  template<typename MatrixDerived>
+   *
+   * This constructor is a shortcut for the default constructor followed
+   * by a call to compute().
+   *
+   * \warning this class stores a reference to the matrix A as well as some
+   * precomputed values that depend on it. Therefore, if \a A is changed
+   * this class becomes invalid. Call compute() to update it with the new
+   * matrix A, or modify a copy of A.
+   */
+  template <typename MatrixDerived>
   explicit LeastSquaresConjugateGradient(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
 
   ~LeastSquaresConjugateGradient() {}
 
   /** \internal */
-  template<typename Rhs,typename Dest>
-  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
-  {
+  template <typename Rhs, typename Dest>
+  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const {
     m_iterations = Base::maxIterations();
     m_error = Base::m_tolerance;
 
     internal::least_square_conjugate_gradient(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error);
     m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
   }
-
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
+#endif  // EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
index 7b89657..020241b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
@@ -10,61 +10,59 @@
 #ifndef EIGEN_SOLVEWITHGUESS_H
 #define EIGEN_SOLVEWITHGUESS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename Decomposition, typename RhsType, typename GuessType> class SolveWithGuess;
+template <typename Decomposition, typename RhsType, typename GuessType>
+class SolveWithGuess;
 
 /** \class SolveWithGuess
-  * \ingroup IterativeLinearSolvers_Module
-  *
-  * \brief Pseudo expression representing a solving operation
-  *
-  * \tparam Decomposition the type of the matrix or decomposion object
-  * \tparam Rhstype the type of the right-hand side
-  *
-  * This class represents an expression of A.solve(B)
-  * and most of the time this is the only way it is used.
-  *
-  */
+ * \ingroup IterativeLinearSolvers_Module
+ *
+ * \brief Pseudo expression representing a solving operation
+ *
+ * \tparam Decomposition the type of the matrix or decomposion object
+ * \tparam Rhstype the type of the right-hand side
+ *
+ * This class represents an expression of A.solve(B)
+ * and most of the time this is the only way it is used.
+ *
+ */
 namespace internal {
 
+template <typename Decomposition, typename RhsType, typename GuessType>
+struct traits<SolveWithGuess<Decomposition, RhsType, GuessType> > : traits<Solve<Decomposition, RhsType> > {};
 
-template<typename Decomposition, typename RhsType, typename GuessType>
-struct traits<SolveWithGuess<Decomposition, RhsType, GuessType> >
-  : traits<Solve<Decomposition,RhsType> >
-{};
+}  // namespace internal
 
-}
-
-
-template<typename Decomposition, typename RhsType, typename GuessType>
-class SolveWithGuess : public internal::generic_xpr_base<SolveWithGuess<Decomposition,RhsType,GuessType>, MatrixXpr, typename internal::traits<RhsType>::StorageKind>::type
-{
-public:
+template <typename Decomposition, typename RhsType, typename GuessType>
+class SolveWithGuess : public internal::generic_xpr_base<SolveWithGuess<Decomposition, RhsType, GuessType>, MatrixXpr,
+                                                         typename internal::traits<RhsType>::StorageKind>::type {
+ public:
   typedef typename internal::traits<SolveWithGuess>::Scalar Scalar;
   typedef typename internal::traits<SolveWithGuess>::PlainObject PlainObject;
-  typedef typename internal::generic_xpr_base<SolveWithGuess<Decomposition,RhsType,GuessType>, MatrixXpr, typename internal::traits<RhsType>::StorageKind>::type Base;
+  typedef typename internal::generic_xpr_base<SolveWithGuess<Decomposition, RhsType, GuessType>, MatrixXpr,
+                                              typename internal::traits<RhsType>::StorageKind>::type Base;
   typedef typename internal::ref_selector<SolveWithGuess>::type Nested;
 
   SolveWithGuess(const Decomposition &dec, const RhsType &rhs, const GuessType &guess)
-    : m_dec(dec), m_rhs(rhs), m_guess(guess)
-  {}
+      : m_dec(dec), m_rhs(rhs), m_guess(guess) {}
 
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  Index rows() const EIGEN_NOEXCEPT { return m_dec.cols(); }
-  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-  Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_dec.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
 
-  EIGEN_DEVICE_FUNC const Decomposition& dec()   const { return m_dec; }
-  EIGEN_DEVICE_FUNC const RhsType&       rhs()   const { return m_rhs; }
-  EIGEN_DEVICE_FUNC const GuessType&     guess() const { return m_guess; }
+  EIGEN_DEVICE_FUNC const Decomposition &dec() const { return m_dec; }
+  EIGEN_DEVICE_FUNC const RhsType &rhs() const { return m_rhs; }
+  EIGEN_DEVICE_FUNC const GuessType &guess() const { return m_guess; }
 
-protected:
+ protected:
   const Decomposition &m_dec;
-  const RhsType       &m_rhs;
-  const GuessType     &m_guess;
+  const RhsType &m_rhs;
+  const GuessType &m_guess;
 
-private:
+ private:
   Scalar coeff(Index row, Index col) const;
   Scalar coeff(Index i) const;
 };
@@ -72,46 +70,42 @@
 namespace internal {
 
 // Evaluator of SolveWithGuess -> eval into a temporary
-template<typename Decomposition, typename RhsType, typename GuessType>
-struct evaluator<SolveWithGuess<Decomposition,RhsType, GuessType> >
-  : public evaluator<typename SolveWithGuess<Decomposition,RhsType,GuessType>::PlainObject>
-{
-  typedef SolveWithGuess<Decomposition,RhsType,GuessType> SolveType;
+template <typename Decomposition, typename RhsType, typename GuessType>
+struct evaluator<SolveWithGuess<Decomposition, RhsType, GuessType> >
+    : public evaluator<typename SolveWithGuess<Decomposition, RhsType, GuessType>::PlainObject> {
+  typedef SolveWithGuess<Decomposition, RhsType, GuessType> SolveType;
   typedef typename SolveType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  evaluator(const SolveType& solve)
-    : m_result(solve.rows(), solve.cols())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
+  evaluator(const SolveType &solve) : m_result(solve.rows(), solve.cols()) {
+    internal::construct_at<Base>(this, m_result);
     m_result = solve.guess();
     solve.dec()._solve_with_guess_impl(solve.rhs(), m_result);
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
 // Specialization for "dst = dec.solveWithGuess(rhs)"
-// NOTE we need to specialize it for Dense2Dense to avoid ambiguous specialization error and a Sparse2Sparse specialization must exist somewhere
-template<typename DstXprType, typename DecType, typename RhsType, typename GuessType, typename Scalar>
-struct Assignment<DstXprType, SolveWithGuess<DecType,RhsType,GuessType>, internal::assign_op<Scalar,Scalar>, Dense2Dense>
-{
-  typedef SolveWithGuess<DecType,RhsType,GuessType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
-  {
+// NOTE we need to specialize it for Dense2Dense to avoid ambiguous specialization error and a Sparse2Sparse
+// specialization must exist somewhere
+template <typename DstXprType, typename DecType, typename RhsType, typename GuessType, typename Scalar>
+struct Assignment<DstXprType, SolveWithGuess<DecType, RhsType, GuessType>, internal::assign_op<Scalar, Scalar>,
+                  Dense2Dense> {
+  typedef SolveWithGuess<DecType, RhsType, GuessType> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar, Scalar> &) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     dst = src.guess();
-    src.dec()._solve_with_guess_impl(src.rhs(), dst/*, src.guess()*/);
+    src.dec()._solve_with_guess_impl(src.rhs(), dst /*, src.guess()*/);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SOLVEWITHGUESS_H
+#endif  // EIGEN_SOLVEWITHGUESS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/InternalHeaderCheck.h
new file mode 100644
index 0000000..b17b1f2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_JACOBI_MODULE_H
+#error "Please include Eigen/Jacobi instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h
index 76668a5..f53b8ec 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h
@@ -11,305 +11,270 @@
 #ifndef EIGEN_JACOBI_H
 #define EIGEN_JACOBI_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \ingroup Jacobi_Module
-  * \jacobi_module
-  * \class JacobiRotation
-  * \brief Rotation given by a cosine-sine pair.
-  *
-  * This class represents a Jacobi or Givens rotation.
-  * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
-  * its cosine \c c and sine \c s as follow:
-  * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s  & \overline c \end{array} \right ) \f$
-  *
-  * You can apply the respective counter-clockwise rotation to a column vector \c v by
-  * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
-  * \code
-  * v.applyOnTheLeft(J.adjoint());
-  * \endcode
-  *
-  * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
-  */
-template<typename Scalar> class JacobiRotation
-{
-  public:
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+ * \jacobi_module
+ * \class JacobiRotation
+ * \brief Rotation given by a cosine-sine pair.
+ *
+ * This class represents a Jacobi or Givens rotation.
+ * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
+ * its cosine \c c and sine \c s as follow:
+ * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s  & \overline c \end{array} \right ) \f$
+ *
+ * You can apply the respective counter-clockwise rotation to a column vector \c v by
+ * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
+ * \code
+ * v.applyOnTheLeft(J.adjoint());
+ * \endcode
+ *
+ * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
+ */
+template <typename Scalar>
+class JacobiRotation {
+ public:
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    /** Default constructor without any initialization. */
-    EIGEN_DEVICE_FUNC
-    JacobiRotation() {}
+  /** Default constructor without any initialization. */
+  EIGEN_DEVICE_FUNC JacobiRotation() {}
 
-    /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
-    EIGEN_DEVICE_FUNC
-    JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
+  /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
+  EIGEN_DEVICE_FUNC JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
 
-    EIGEN_DEVICE_FUNC Scalar& c() { return m_c; }
-    EIGEN_DEVICE_FUNC Scalar c() const { return m_c; }
-    EIGEN_DEVICE_FUNC Scalar& s() { return m_s; }
-    EIGEN_DEVICE_FUNC Scalar s() const { return m_s; }
+  EIGEN_DEVICE_FUNC Scalar& c() { return m_c; }
+  EIGEN_DEVICE_FUNC Scalar c() const { return m_c; }
+  EIGEN_DEVICE_FUNC Scalar& s() { return m_s; }
+  EIGEN_DEVICE_FUNC Scalar s() const { return m_s; }
 
-    /** Concatenates two planar rotation */
-    EIGEN_DEVICE_FUNC
-    JacobiRotation operator*(const JacobiRotation& other)
-    {
-      using numext::conj;
-      return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s,
-                            conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c)));
-    }
+  /** Concatenates two planar rotation */
+  EIGEN_DEVICE_FUNC JacobiRotation operator*(const JacobiRotation& other) {
+    using numext::conj;
+    return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s,
+                          conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c)));
+  }
 
-    /** Returns the transposed transformation */
-    EIGEN_DEVICE_FUNC
-    JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
+  /** Returns the transposed transformation */
+  EIGEN_DEVICE_FUNC JacobiRotation transpose() const {
+    using numext::conj;
+    return JacobiRotation(m_c, -conj(m_s));
+  }
 
-    /** Returns the adjoint transformation */
-    EIGEN_DEVICE_FUNC
-    JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); }
+  /** Returns the adjoint transformation */
+  EIGEN_DEVICE_FUNC JacobiRotation adjoint() const {
+    using numext::conj;
+    return JacobiRotation(conj(m_c), -m_s);
+  }
 
-    template<typename Derived>
-    EIGEN_DEVICE_FUNC
-    bool makeJacobi(const MatrixBase<Derived>&, Index p, Index q);
-    EIGEN_DEVICE_FUNC
-    bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC bool makeJacobi(const MatrixBase<Derived>&, Index p, Index q);
+  EIGEN_DEVICE_FUNC bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
 
-    EIGEN_DEVICE_FUNC
-    void makeGivens(const Scalar& p, const Scalar& q, Scalar* r=0);
+  EIGEN_DEVICE_FUNC void makeGivens(const Scalar& p, const Scalar& q, Scalar* r = 0);
 
-  protected:
-    EIGEN_DEVICE_FUNC
-    void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
-    EIGEN_DEVICE_FUNC
-    void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type);
+ protected:
+  EIGEN_DEVICE_FUNC void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
+  EIGEN_DEVICE_FUNC void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type);
 
-    Scalar m_c, m_s;
+  Scalar m_c, m_s;
 };
 
-/** Makes \c *this as a Jacobi rotation \a J such that applying \a J on both the right and left sides of the selfadjoint 2x2 matrix
-  * \f$ B = \left ( \begin{array}{cc} x & y \\ \overline y & z \end{array} \right )\f$ yields a diagonal matrix \f$ A = J^* B J \f$
-  *
-  * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
-  */
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z)
-{
-  using std::sqrt;
+/** Makes \c *this as a Jacobi rotation \a J such that applying \a J on both the right and left sides of the selfadjoint
+ * 2x2 matrix \f$ B = \left ( \begin{array}{cc} x & y \\ \overline y & z \end{array} \right )\f$ yields a diagonal
+ * matrix \f$ A = J^* B J \f$
+ *
+ * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(),
+ * MatrixBase::applyOnTheRight()
+ */
+template <typename Scalar>
+EIGEN_DEVICE_FUNC bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z) {
   using std::abs;
+  using std::sqrt;
 
-  RealScalar deno = RealScalar(2)*abs(y);
-  if(deno < (std::numeric_limits<RealScalar>::min)())
-  {
+  RealScalar deno = RealScalar(2) * abs(y);
+  if (deno < (std::numeric_limits<RealScalar>::min)()) {
     m_c = Scalar(1);
     m_s = Scalar(0);
     return false;
-  }
-  else
-  {
-    RealScalar tau = (x-z)/deno;
+  } else {
+    RealScalar tau = (x - z) / deno;
     RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
     RealScalar t;
-    if(tau>RealScalar(0))
-    {
+    if (tau > RealScalar(0)) {
       t = RealScalar(1) / (tau + w);
-    }
-    else
-    {
+    } else {
       t = RealScalar(1) / (tau - w);
     }
     RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
-    RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
-    m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
+    RealScalar n = RealScalar(1) / sqrt(numext::abs2(t) + RealScalar(1));
+    m_s = -sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
     m_c = n;
     return true;
   }
 }
 
-/** Makes \c *this as a Jacobi rotation \c J such that applying \a J on both the right and left sides of the 2x2 selfadjoint matrix
-  * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
-  * a diagonal matrix \f$ A = J^* B J \f$
-  *
-  * Example: \include Jacobi_makeJacobi.cpp
-  * Output: \verbinclude Jacobi_makeJacobi.out
-  *
-  * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
-  */
-template<typename Scalar>
-template<typename Derived>
-EIGEN_DEVICE_FUNC
-inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, Index q)
-{
-  return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
+/** Makes \c *this as a Jacobi rotation \c J such that applying \a J on both the right and left sides of the 2x2
+ * selfadjoint matrix \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* &
+ * \text{this}_{qq} \end{array} \right )\f$ yields a diagonal matrix \f$ A = J^* B J \f$
+ *
+ * Example: \include Jacobi_makeJacobi.cpp
+ * Output: \verbinclude Jacobi_makeJacobi.out
+ *
+ * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(),
+ * MatrixBase::applyOnTheRight()
+ */
+template <typename Scalar>
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, Index q) {
+  return makeJacobi(numext::real(m.coeff(p, p)), m.coeff(p, q), numext::real(m.coeff(q, q)));
 }
 
 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
-  * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
-  * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
-  *
-  * The value of \a r is returned if \a r is not null (the default is null).
-  * Also note that G is built such that the cosine is always real.
-  *
-  * Example: \include Jacobi_makeGivens.cpp
-  * Output: \verbinclude Jacobi_makeGivens.out
-  *
-  * This function implements the continuous Givens rotation generation algorithm
-  * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
-  * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
-  *
-  * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
-  */
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r)
-{
-  makeGivens(p, q, r, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
+ * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
+ * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
+ *
+ * The value of \a r is returned if \a r is not null (the default is null).
+ * Also note that G is built such that the cosine is always real.
+ *
+ * Example: \include Jacobi_makeGivens.cpp
+ * Output: \verbinclude Jacobi_makeGivens.out
+ *
+ * This function implements the continuous Givens rotation generation algorithm
+ * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
+ * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
+ *
+ * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
+ */
+template <typename Scalar>
+EIGEN_DEVICE_FUNC void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r) {
+  makeGivens(p, q, r, std::conditional_t<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>());
 }
 
-
 // specialization for complexes
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
-{
-  using std::sqrt;
-  using std::abs;
+template <typename Scalar>
+EIGEN_DEVICE_FUNC void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r,
+                                                          internal::true_type) {
   using numext::conj;
+  using std::abs;
+  using std::sqrt;
 
-  if(q==Scalar(0))
-  {
-    m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
+  if (q == Scalar(0)) {
+    m_c = numext::real(p) < 0 ? Scalar(-1) : Scalar(1);
     m_s = 0;
-    if(r) *r = m_c * p;
-  }
-  else if(p==Scalar(0))
-  {
+    if (r) *r = m_c * p;
+  } else if (p == Scalar(0)) {
     m_c = 0;
-    m_s = -q/abs(q);
-    if(r) *r = abs(q);
-  }
-  else
-  {
+    m_s = -q / abs(q);
+    if (r) *r = abs(q);
+  } else {
     RealScalar p1 = numext::norm1(p);
     RealScalar q1 = numext::norm1(q);
-    if(p1>=q1)
-    {
+    if (p1 >= q1) {
       Scalar ps = p / p1;
       RealScalar p2 = numext::abs2(ps);
       Scalar qs = q / p1;
       RealScalar q2 = numext::abs2(qs);
 
-      RealScalar u = sqrt(RealScalar(1) + q2/p2);
-      if(numext::real(p)<RealScalar(0))
-        u = -u;
+      RealScalar u = sqrt(RealScalar(1) + q2 / p2);
+      if (numext::real(p) < RealScalar(0)) u = -u;
 
-      m_c = Scalar(1)/u;
-      m_s = -qs*conj(ps)*(m_c/p2);
-      if(r) *r = p * u;
-    }
-    else
-    {
+      m_c = Scalar(1) / u;
+      m_s = -qs * conj(ps) * (m_c / p2);
+      if (r) *r = p * u;
+    } else {
       Scalar ps = p / q1;
       RealScalar p2 = numext::abs2(ps);
       Scalar qs = q / q1;
       RealScalar q2 = numext::abs2(qs);
 
       RealScalar u = q1 * sqrt(p2 + q2);
-      if(numext::real(p)<RealScalar(0))
-        u = -u;
+      if (numext::real(p) < RealScalar(0)) u = -u;
 
       p1 = abs(p);
-      ps = p/p1;
-      m_c = p1/u;
-      m_s = -conj(ps) * (q/u);
-      if(r) *r = ps * u;
+      ps = p / p1;
+      m_c = p1 / u;
+      m_s = -conj(ps) * (q / u);
+      if (r) *r = ps * u;
     }
   }
 }
 
 // specialization for reals
-template<typename Scalar>
-EIGEN_DEVICE_FUNC
-void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
-{
-  using std::sqrt;
+template <typename Scalar>
+EIGEN_DEVICE_FUNC void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r,
+                                                          internal::false_type) {
   using std::abs;
-  if(q==Scalar(0))
-  {
-    m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
+  using std::sqrt;
+  if (numext::is_exactly_zero(q)) {
+    m_c = p < Scalar(0) ? Scalar(-1) : Scalar(1);
     m_s = Scalar(0);
-    if(r) *r = abs(p);
-  }
-  else if(p==Scalar(0))
-  {
+    if (r) *r = abs(p);
+  } else if (numext::is_exactly_zero(p)) {
     m_c = Scalar(0);
-    m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
-    if(r) *r = abs(q);
-  }
-  else if(abs(p) > abs(q))
-  {
-    Scalar t = q/p;
+    m_s = q < Scalar(0) ? Scalar(1) : Scalar(-1);
+    if (r) *r = abs(q);
+  } else if (abs(p) > abs(q)) {
+    Scalar t = q / p;
     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
-    if(p<Scalar(0))
-      u = -u;
-    m_c = Scalar(1)/u;
+    if (p < Scalar(0)) u = -u;
+    m_c = Scalar(1) / u;
     m_s = -t * m_c;
-    if(r) *r = p * u;
-  }
-  else
-  {
-    Scalar t = p/q;
+    if (r) *r = p * u;
+  } else {
+    Scalar t = p / q;
     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
-    if(q<Scalar(0))
-      u = -u;
-    m_s = -Scalar(1)/u;
+    if (q < Scalar(0)) u = -u;
+    m_s = -Scalar(1) / u;
     m_c = -t * m_s;
-    if(r) *r = q * u;
+    if (r) *r = q * u;
   }
-
 }
 
 /****************************************************************************************
-*   Implementation of MatrixBase methods
-****************************************************************************************/
+ *   Implementation of MatrixBase methods
+ ****************************************************************************************/
 
 namespace internal {
 /** \jacobi_module
-  * Applies the clock wise 2D rotation \a j to the set of 2D vectors of coordinates \a x and \a y:
-  * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right )  =  J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
-  *
-  * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
-  */
-template<typename VectorX, typename VectorY, typename OtherScalar>
-EIGEN_DEVICE_FUNC
-void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j);
-}
+ * Applies the clock wise 2D rotation \a j to the set of 2D vectors of coordinates \a x and \a y:
+ * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right )  =  J \left ( \begin{array}{cc} x \\ y \end{array} \right )
+ * \f$
+ *
+ * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
+ */
+template <typename VectorX, typename VectorY, typename OtherScalar>
+EIGEN_DEVICE_FUNC void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y,
+                                                   const JacobiRotation<OtherScalar>& j);
+}  // namespace internal
 
 /** \jacobi_module
-  * Applies the rotation in the plane \a j to the rows \a p and \a q of \c *this, i.e., it computes B = J * B,
-  * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
-  *
-  * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
-  */
-template<typename Derived>
-template<typename OtherScalar>
-EIGEN_DEVICE_FUNC
-inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
-{
+ * Applies the rotation in the plane \a j to the rows \a p and \a q of \c *this, i.e., it computes B = J * B,
+ * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
+ *
+ * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
+ */
+template <typename Derived>
+template <typename OtherScalar>
+EIGEN_DEVICE_FUNC inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q,
+                                                                  const JacobiRotation<OtherScalar>& j) {
   RowXpr x(this->row(p));
   RowXpr y(this->row(q));
   internal::apply_rotation_in_the_plane(x, y, j);
 }
 
 /** \ingroup Jacobi_Module
-  * Applies the rotation in the plane \a j to the columns \a p and \a q of \c *this, i.e., it computes B = B * J
-  * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
-  *
-  * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
-  */
-template<typename Derived>
-template<typename OtherScalar>
-EIGEN_DEVICE_FUNC
-inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
-{
+ * Applies the rotation in the plane \a j to the columns \a p and \a q of \c *this, i.e., it computes B = B * J
+ * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
+ *
+ * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
+ */
+template <typename Derived>
+template <typename OtherScalar>
+EIGEN_DEVICE_FUNC inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q,
+                                                                   const JacobiRotation<OtherScalar>& j) {
   ColXpr x(this->col(p));
   ColXpr y(this->col(q));
   internal::apply_rotation_in_the_plane(x, y, j.transpose());
@@ -317,18 +282,14 @@
 
 namespace internal {
 
-template<typename Scalar, typename OtherScalar,
-         int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
-struct apply_rotation_in_the_plane_selector
-{
-  static EIGEN_DEVICE_FUNC
-  inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
-  {
-    for(Index i=0; i<size; ++i)
-    {
+template <typename Scalar, typename OtherScalar, int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
+struct apply_rotation_in_the_plane_selector {
+  static EIGEN_DEVICE_FUNC inline void run(Scalar* x, Index incrx, Scalar* y, Index incry, Index size, OtherScalar c,
+                                           OtherScalar s) {
+    for (Index i = 0; i < size; ++i) {
       Scalar xi = *x;
       Scalar yi = *y;
-      *x =  c * xi + numext::conj(s) * yi;
+      *x = c * xi + numext::conj(s) * yi;
       *y = -s * xi + numext::conj(c) * yi;
       x += incrx;
       y += incry;
@@ -336,125 +297,111 @@
   }
 };
 
-template<typename Scalar, typename OtherScalar,
-         int SizeAtCompileTime, int MinAlignment>
-struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */>
-{
-  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
-  {
-    enum {
-      PacketSize = packet_traits<Scalar>::size,
-      OtherPacketSize = packet_traits<OtherScalar>::size
-    };
+template <typename Scalar, typename OtherScalar, int SizeAtCompileTime, int MinAlignment>
+struct apply_rotation_in_the_plane_selector<Scalar, OtherScalar, SizeAtCompileTime, MinAlignment,
+                                            true /* vectorizable */> {
+  static inline void run(Scalar* x, Index incrx, Scalar* y, Index incry, Index size, OtherScalar c, OtherScalar s) {
     typedef typename packet_traits<Scalar>::type Packet;
     typedef typename packet_traits<OtherScalar>::type OtherPacket;
 
+    constexpr int RequiredAlignment =
+        (std::max)(unpacket_traits<Packet>::alignment, unpacket_traits<OtherPacket>::alignment);
+    constexpr Index PacketSize = packet_traits<Scalar>::size;
+
     /*** dynamic-size vectorized paths ***/
-    if(SizeAtCompileTime == Dynamic && ((incrx==1 && incry==1) || PacketSize == 1))
-    {
+    if (size >= 2 * PacketSize && SizeAtCompileTime == Dynamic && ((incrx == 1 && incry == 1) || PacketSize == 1)) {
       // both vectors are sequentially stored in memory => vectorization
-      enum { Peeling = 2 };
+      constexpr Index Peeling = 2;
 
       Index alignedStart = internal::first_default_aligned(y, size);
-      Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
+      Index alignedEnd = alignedStart + ((size - alignedStart) / PacketSize) * PacketSize;
 
       const OtherPacket pc = pset1<OtherPacket>(c);
       const OtherPacket ps = pset1<OtherPacket>(s);
-      conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj;
-      conj_helper<OtherPacket,Packet,false,false> pm;
+      conj_helper<OtherPacket, Packet, NumTraits<OtherScalar>::IsComplex, false> pcj;
+      conj_helper<OtherPacket, Packet, false, false> pm;
 
-      for(Index i=0; i<alignedStart; ++i)
-      {
+      for (Index i = 0; i < alignedStart; ++i) {
         Scalar xi = x[i];
         Scalar yi = y[i];
-        x[i] =  c * xi + numext::conj(s) * yi;
+        x[i] = c * xi + numext::conj(s) * yi;
         y[i] = -s * xi + numext::conj(c) * yi;
       }
 
       Scalar* EIGEN_RESTRICT px = x + alignedStart;
       Scalar* EIGEN_RESTRICT py = y + alignedStart;
 
-      if(internal::first_default_aligned(x, size)==alignedStart)
-      {
-        for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
-        {
+      if (internal::first_default_aligned(x, size) == alignedStart) {
+        for (Index i = alignedStart; i < alignedEnd; i += PacketSize) {
           Packet xi = pload<Packet>(px);
           Packet yi = pload<Packet>(py);
-          pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-          pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
+          pstore(px, padd(pm.pmul(pc, xi), pcj.pmul(ps, yi)));
+          pstore(py, psub(pcj.pmul(pc, yi), pm.pmul(ps, xi)));
           px += PacketSize;
           py += PacketSize;
         }
-      }
-      else
-      {
-        Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
-        for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
-        {
-          Packet xi   = ploadu<Packet>(px);
-          Packet xi1  = ploadu<Packet>(px+PacketSize);
-          Packet yi   = pload <Packet>(py);
-          Packet yi1  = pload <Packet>(py+PacketSize);
-          pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-          pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
-          pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
-          pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
-          px += Peeling*PacketSize;
-          py += Peeling*PacketSize;
+      } else {
+        Index peelingEnd = alignedStart + ((size - alignedStart) / (Peeling * PacketSize)) * (Peeling * PacketSize);
+        for (Index i = alignedStart; i < peelingEnd; i += Peeling * PacketSize) {
+          Packet xi = ploadu<Packet>(px);
+          Packet xi1 = ploadu<Packet>(px + PacketSize);
+          Packet yi = pload<Packet>(py);
+          Packet yi1 = pload<Packet>(py + PacketSize);
+          pstoreu(px, padd(pm.pmul(pc, xi), pcj.pmul(ps, yi)));
+          pstoreu(px + PacketSize, padd(pm.pmul(pc, xi1), pcj.pmul(ps, yi1)));
+          pstore(py, psub(pcj.pmul(pc, yi), pm.pmul(ps, xi)));
+          pstore(py + PacketSize, psub(pcj.pmul(pc, yi1), pm.pmul(ps, xi1)));
+          px += Peeling * PacketSize;
+          py += Peeling * PacketSize;
         }
-        if(alignedEnd!=peelingEnd)
-        {
-          Packet xi = ploadu<Packet>(x+peelingEnd);
-          Packet yi = pload <Packet>(y+peelingEnd);
-          pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-          pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
+        if (alignedEnd != peelingEnd) {
+          Packet xi = ploadu<Packet>(x + peelingEnd);
+          Packet yi = pload<Packet>(y + peelingEnd);
+          pstoreu(x + peelingEnd, padd(pm.pmul(pc, xi), pcj.pmul(ps, yi)));
+          pstore(y + peelingEnd, psub(pcj.pmul(pc, yi), pm.pmul(ps, xi)));
         }
       }
 
-      for(Index i=alignedEnd; i<size; ++i)
-      {
+      for (Index i = alignedEnd; i < size; ++i) {
         Scalar xi = x[i];
         Scalar yi = y[i];
-        x[i] =  c * xi + numext::conj(s) * yi;
+        x[i] = c * xi + numext::conj(s) * yi;
         y[i] = -s * xi + numext::conj(c) * yi;
       }
     }
 
     /*** fixed-size vectorized path ***/
-    else if(SizeAtCompileTime != Dynamic && MinAlignment>0) // FIXME should be compared to the required alignment
-    {
+    else if (SizeAtCompileTime != Dynamic && MinAlignment >= RequiredAlignment) {
       const OtherPacket pc = pset1<OtherPacket>(c);
       const OtherPacket ps = pset1<OtherPacket>(s);
-      conj_helper<OtherPacket,Packet,NumTraits<OtherPacket>::IsComplex,false> pcj;
-      conj_helper<OtherPacket,Packet,false,false> pm;
+      conj_helper<OtherPacket, Packet, NumTraits<OtherScalar>::IsComplex, false> pcj;
+      conj_helper<OtherPacket, Packet, false, false> pm;
       Scalar* EIGEN_RESTRICT px = x;
       Scalar* EIGEN_RESTRICT py = y;
-      for(Index i=0; i<size; i+=PacketSize)
-      {
+      for (Index i = 0; i < size; i += PacketSize) {
         Packet xi = pload<Packet>(px);
         Packet yi = pload<Packet>(py);
-        pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-        pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
+        pstore(px, padd(pm.pmul(pc, xi), pcj.pmul(ps, yi)));
+        pstore(py, psub(pcj.pmul(pc, yi), pm.pmul(ps, xi)));
         px += PacketSize;
         py += PacketSize;
       }
     }
 
     /*** non-vectorized path ***/
-    else
-    {
-      apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,false>::run(x,incrx,y,incry,size,c,s);
+    else {
+      apply_rotation_in_the_plane_selector<Scalar, OtherScalar, SizeAtCompileTime, MinAlignment, false>::run(
+          x, incrx, y, incry, size, c, s);
     }
   }
 };
 
-template<typename VectorX, typename VectorY, typename OtherScalar>
-EIGEN_DEVICE_FUNC
-void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
-{
+template <typename VectorX, typename VectorY, typename OtherScalar>
+EIGEN_DEVICE_FUNC void inline apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y,
+                                                          const JacobiRotation<OtherScalar>& j) {
   typedef typename VectorX::Scalar Scalar;
-  const bool Vectorizable =    (int(VectorX::Flags) & int(VectorY::Flags) & PacketAccessBit)
-                            && (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
+  constexpr bool Vectorizable = (int(evaluator<VectorX>::Flags) & int(evaluator<VectorY>::Flags) & PacketAccessBit) &&
+                                (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
 
   eigen_assert(xpr_x.size() == xpr_y.size());
   Index size = xpr_x.size();
@@ -466,18 +413,15 @@
 
   OtherScalar c = j.c();
   OtherScalar s = j.s();
-  if (c==OtherScalar(1) && s==OtherScalar(0))
-    return;
+  if (numext::is_exactly_one(c) && numext::is_exactly_zero(s)) return;
 
-  apply_rotation_in_the_plane_selector<
-    Scalar,OtherScalar,
-    VectorX::SizeAtCompileTime,
-    EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment),
-    Vectorizable>::run(x,incrx,y,incry,size,c,s);
+  constexpr int Alignment = (std::min)(int(evaluator<VectorX>::Alignment), int(evaluator<VectorY>::Alignment));
+  apply_rotation_in_the_plane_selector<Scalar, OtherScalar, VectorX::SizeAtCompileTime, Alignment, Vectorizable>::run(
+      x, incrx, y, incry, size, c, s);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_JACOBI_H
+#endif  // EIGEN_JACOBI_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h
index 3a41e6f..ae4fee3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h
@@ -10,108 +10,89 @@
 #ifndef EIGEN_DETERMINANT_H
 #define EIGEN_DETERMINANT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename Derived>
-EIGEN_DEVICE_FUNC
-inline const typename Derived::Scalar bruteforce_det3_helper
-(const MatrixBase<Derived>& matrix, int a, int b, int c)
-{
-  return matrix.coeff(0,a)
-         * (matrix.coeff(1,b) * matrix.coeff(2,c) - matrix.coeff(1,c) * matrix.coeff(2,b));
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const typename Derived::Scalar bruteforce_det3_helper(const MatrixBase<Derived>& matrix, int a,
+                                                                               int b, int c) {
+  return matrix.coeff(0, a) * (matrix.coeff(1, b) * matrix.coeff(2, c) - matrix.coeff(1, c) * matrix.coeff(2, b));
 }
 
-template<typename Derived,
-         int DeterminantType = Derived::RowsAtCompileTime
-> struct determinant_impl
-{
-  static inline typename traits<Derived>::Scalar run(const Derived& m)
-  {
-    if(Derived::ColsAtCompileTime==Dynamic && m.rows()==0)
-      return typename traits<Derived>::Scalar(1);
+template <typename Derived, int DeterminantType = Derived::RowsAtCompileTime>
+struct determinant_impl {
+  static inline typename traits<Derived>::Scalar run(const Derived& m) {
+    if (Derived::ColsAtCompileTime == Dynamic && m.rows() == 0) return typename traits<Derived>::Scalar(1);
     return m.partialPivLu().determinant();
   }
 };
 
-template<typename Derived> struct determinant_impl<Derived, 1>
-{
-  static inline EIGEN_DEVICE_FUNC
-  typename traits<Derived>::Scalar run(const Derived& m)
-  {
-    return m.coeff(0,0);
+template <typename Derived>
+struct determinant_impl<Derived, 1> {
+  static inline EIGEN_DEVICE_FUNC typename traits<Derived>::Scalar run(const Derived& m) { return m.coeff(0, 0); }
+};
+
+template <typename Derived>
+struct determinant_impl<Derived, 2> {
+  static inline EIGEN_DEVICE_FUNC typename traits<Derived>::Scalar run(const Derived& m) {
+    return m.coeff(0, 0) * m.coeff(1, 1) - m.coeff(1, 0) * m.coeff(0, 1);
   }
 };
 
-template<typename Derived> struct determinant_impl<Derived, 2>
-{
-  static inline EIGEN_DEVICE_FUNC
-  typename traits<Derived>::Scalar run(const Derived& m)
-  {
-    return m.coeff(0,0) * m.coeff(1,1) - m.coeff(1,0) * m.coeff(0,1);
+template <typename Derived>
+struct determinant_impl<Derived, 3> {
+  static inline EIGEN_DEVICE_FUNC typename traits<Derived>::Scalar run(const Derived& m) {
+    return bruteforce_det3_helper(m, 0, 1, 2) - bruteforce_det3_helper(m, 1, 0, 2) + bruteforce_det3_helper(m, 2, 0, 1);
   }
 };
 
-template<typename Derived> struct determinant_impl<Derived, 3>
-{
-  static inline EIGEN_DEVICE_FUNC
-  typename traits<Derived>::Scalar run(const Derived& m)
-  {
-    return bruteforce_det3_helper(m,0,1,2)
-          - bruteforce_det3_helper(m,1,0,2)
-          + bruteforce_det3_helper(m,2,0,1);
-  }
-};
-
-template<typename Derived> struct determinant_impl<Derived, 4>
-{
+template <typename Derived>
+struct determinant_impl<Derived, 4> {
   typedef typename traits<Derived>::Scalar Scalar;
-  static EIGEN_DEVICE_FUNC
-  Scalar run(const Derived& m)
-  {
+  static EIGEN_DEVICE_FUNC Scalar run(const Derived& m) {
     Scalar d2_01 = det2(m, 0, 1);
     Scalar d2_02 = det2(m, 0, 2);
     Scalar d2_03 = det2(m, 0, 3);
     Scalar d2_12 = det2(m, 1, 2);
     Scalar d2_13 = det2(m, 1, 3);
     Scalar d2_23 = det2(m, 2, 3);
-    Scalar d3_0 = det3(m, 1,d2_23, 2,d2_13, 3,d2_12);
-    Scalar d3_1 = det3(m, 0,d2_23, 2,d2_03, 3,d2_02);
-    Scalar d3_2 = det3(m, 0,d2_13, 1,d2_03, 3,d2_01);
-    Scalar d3_3 = det3(m, 0,d2_12, 1,d2_02, 2,d2_01);
-    return internal::pmadd(-m(0,3),d3_0, m(1,3)*d3_1) +
-           internal::pmadd(-m(2,3),d3_2, m(3,3)*d3_3);
-  }
-protected:
-  static EIGEN_DEVICE_FUNC
-  Scalar det2(const Derived& m, Index i0, Index i1)
-  {
-    return m(i0,0) * m(i1,1) - m(i1,0) * m(i0,1);
+    Scalar d3_0 = det3(m, 1, d2_23, 2, d2_13, 3, d2_12);
+    Scalar d3_1 = det3(m, 0, d2_23, 2, d2_03, 3, d2_02);
+    Scalar d3_2 = det3(m, 0, d2_13, 1, d2_03, 3, d2_01);
+    Scalar d3_3 = det3(m, 0, d2_12, 1, d2_02, 2, d2_01);
+    return internal::pmadd(static_cast<Scalar>(-m(0, 3)), d3_0, static_cast<Scalar>(m(1, 3) * d3_1)) +
+           internal::pmadd(static_cast<Scalar>(-m(2, 3)), d3_2, static_cast<Scalar>(m(3, 3) * d3_3));
   }
 
-  static EIGEN_DEVICE_FUNC
-  Scalar det3(const Derived& m, Index i0, const Scalar& d0, Index i1, const Scalar& d1, Index i2, const Scalar& d2)
-  {
-    return internal::pmadd(m(i0,2), d0, internal::pmadd(-m(i1,2), d1, m(i2,2)*d2));
+ protected:
+  static EIGEN_DEVICE_FUNC Scalar det2(const Derived& m, Index i0, Index i1) {
+    return m(i0, 0) * m(i1, 1) - m(i1, 0) * m(i0, 1);
+  }
+
+  static EIGEN_DEVICE_FUNC Scalar det3(const Derived& m, Index i0, const Scalar& d0, Index i1, const Scalar& d1,
+                                       Index i2, const Scalar& d2) {
+    return internal::pmadd(m(i0, 2), d0,
+                           internal::pmadd(static_cast<Scalar>(-m(i1, 2)), d1, static_cast<Scalar>(m(i2, 2) * d2)));
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \lu_module
-  *
-  * \returns the determinant of this matrix
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC
-inline typename internal::traits<Derived>::Scalar MatrixBase<Derived>::determinant() const
-{
+ *
+ * \returns the determinant of this matrix
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar MatrixBase<Derived>::determinant() const {
   eigen_assert(rows() == cols());
-  typedef typename internal::nested_eval<Derived,Base::RowsAtCompileTime>::type Nested;
-  return internal::determinant_impl<typename internal::remove_all<Nested>::type>::run(derived());
+  typedef typename internal::nested_eval<Derived, Base::RowsAtCompileTime>::type Nested;
+  return internal::determinant_impl<internal::remove_all_t<Nested>>::run(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_DETERMINANT_H
+#endif  // EIGEN_DETERMINANT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h
index ba1749f..57d049c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h
@@ -10,487 +10,455 @@
 #ifndef EIGEN_LU_H
 #define EIGEN_LU_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename _MatrixType> struct traits<FullPivLU<_MatrixType> >
- : traits<_MatrixType>
-{
+template <typename MatrixType_, typename PermutationIndex_>
+struct traits<FullPivLU<MatrixType_, PermutationIndex_> > : traits<MatrixType_> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
-  typedef int StorageIndex;
+  typedef PermutationIndex_ StorageIndex;
   enum { Flags = 0 };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup LU_Module
-  *
-  * \class FullPivLU
-  *
-  * \brief LU decomposition of a matrix with complete pivoting, and related features
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the LU decomposition
-  *
-  * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A is
-  * decomposed as \f$ A = P^{-1} L U Q^{-1} \f$ where L is unit-lower-triangular, U is
-  * upper-triangular, and P and Q are permutation matrices. This is a rank-revealing LU
-  * decomposition. The eigenvalues (diagonal coefficients) of U are sorted in such a way that any
-  * zeros are at the end.
-  *
-  * This decomposition provides the generic approach to solving systems of linear equations, computing
-  * the rank, invertibility, inverse, kernel, and determinant.
-  *
-  * This LU decomposition is very stable and well tested with large matrices. However there are use cases where the SVD
-  * decomposition is inherently more stable and/or flexible. For example, when computing the kernel of a matrix,
-  * working with the SVD allows to select the smallest singular values of the matrix, something that
-  * the LU decomposition doesn't see.
-  *
-  * The data of the LU decomposition can be directly accessed through the methods matrixLU(),
-  * permutationP(), permutationQ().
-  *
-  * As an example, here is how the original matrix can be retrieved:
-  * \include class_FullPivLU.cpp
-  * Output: \verbinclude class_FullPivLU.out
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  *
-  * \sa MatrixBase::fullPivLu(), MatrixBase::determinant(), MatrixBase::inverse()
-  */
-template<typename _MatrixType> class FullPivLU
-  : public SolverBase<FullPivLU<_MatrixType> >
-{
-  public:
-    typedef _MatrixType MatrixType;
-    typedef SolverBase<FullPivLU> Base;
-    friend class SolverBase<FullPivLU>;
+ *
+ * \class FullPivLU
+ *
+ * \brief LU decomposition of a matrix with complete pivoting, and related features
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the LU decomposition
+ *
+ * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A is
+ * decomposed as \f$ A = P^{-1} L U Q^{-1} \f$ where L is unit-lower-triangular, U is
+ * upper-triangular, and P and Q are permutation matrices. This is a rank-revealing LU
+ * decomposition. The eigenvalues (diagonal coefficients) of U are sorted in such a way that any
+ * zeros are at the end.
+ *
+ * This decomposition provides the generic approach to solving systems of linear equations, computing
+ * the rank, invertibility, inverse, kernel, and determinant.
+ *
+ * This LU decomposition is very stable and well tested with large matrices. However there are use cases where the SVD
+ * decomposition is inherently more stable and/or flexible. For example, when computing the kernel of a matrix,
+ * working with the SVD allows to select the smallest singular values of the matrix, something that
+ * the LU decomposition doesn't see.
+ *
+ * The data of the LU decomposition can be directly accessed through the methods matrixLU(),
+ * permutationP(), permutationQ().
+ *
+ * As an example, here is how the original matrix can be retrieved:
+ * \include class_FullPivLU.cpp
+ * Output: \verbinclude class_FullPivLU.out
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * \sa MatrixBase::fullPivLu(), MatrixBase::determinant(), MatrixBase::inverse()
+ */
+template <typename MatrixType_, typename PermutationIndex_>
+class FullPivLU : public SolverBase<FullPivLU<MatrixType_, PermutationIndex_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef SolverBase<FullPivLU> Base;
+  friend class SolverBase<FullPivLU>;
 
-    EIGEN_GENERIC_PUBLIC_INTERFACE(FullPivLU)
-    enum {
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    typedef typename internal::plain_row_type<MatrixType, StorageIndex>::type IntRowVectorType;
-    typedef typename internal::plain_col_type<MatrixType, StorageIndex>::type IntColVectorType;
-    typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationQType;
-    typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationPType;
-    typedef typename MatrixType::PlainObject PlainObject;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(FullPivLU)
+  enum {
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  using PermutationIndex = PermutationIndex_;
+  typedef typename internal::plain_row_type<MatrixType, PermutationIndex>::type IntRowVectorType;
+  typedef typename internal::plain_col_type<MatrixType, PermutationIndex>::type IntColVectorType;
+  typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime, PermutationIndex> PermutationQType;
+  typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime, PermutationIndex> PermutationPType;
+  typedef typename MatrixType::PlainObject PlainObject;
 
-    /**
-      * \brief Default Constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via LU::compute(const MatrixType&).
-      */
-    FullPivLU();
+  /**
+   * \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via LU::compute(const MatrixType&).
+   */
+  FullPivLU();
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa FullPivLU()
-      */
-    FullPivLU(Index rows, Index cols);
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa FullPivLU()
+   */
+  FullPivLU(Index rows, Index cols);
 
-    /** Constructor.
-      *
-      * \param matrix the matrix of which to compute the LU decomposition.
-      *               It is required to be nonzero.
-      */
-    template<typename InputType>
-    explicit FullPivLU(const EigenBase<InputType>& matrix);
+  /** Constructor.
+   *
+   * \param matrix the matrix of which to compute the LU decomposition.
+   *               It is required to be nonzero.
+   */
+  template <typename InputType>
+  explicit FullPivLU(const EigenBase<InputType>& matrix);
 
-    /** \brief Constructs a LU factorization from a given matrix
-      *
-      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref.
-      *
-      * \sa FullPivLU(const EigenBase&)
-      */
-    template<typename InputType>
-    explicit FullPivLU(EigenBase<InputType>& matrix);
+  /** \brief Constructs a LU factorization from a given matrix
+   *
+   * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c
+   * MatrixType is a Eigen::Ref.
+   *
+   * \sa FullPivLU(const EigenBase&)
+   */
+  template <typename InputType>
+  explicit FullPivLU(EigenBase<InputType>& matrix);
 
-    /** Computes the LU decomposition of the given matrix.
-      *
-      * \param matrix the matrix of which to compute the LU decomposition.
-      *               It is required to be nonzero.
-      *
-      * \returns a reference to *this
-      */
-    template<typename InputType>
-    FullPivLU& compute(const EigenBase<InputType>& matrix) {
-      m_lu = matrix.derived();
-      computeInPlace();
-      return *this;
-    }
+  /** Computes the LU decomposition of the given matrix.
+   *
+   * \param matrix the matrix of which to compute the LU decomposition.
+   *               It is required to be nonzero.
+   *
+   * \returns a reference to *this
+   */
+  template <typename InputType>
+  FullPivLU& compute(const EigenBase<InputType>& matrix) {
+    m_lu = matrix.derived();
+    computeInPlace();
+    return *this;
+  }
 
-    /** \returns the LU decomposition matrix: the upper-triangular part is U, the
-      * unit-lower-triangular part is L (at least for square matrices; in the non-square
-      * case, special care is needed, see the documentation of class FullPivLU).
-      *
-      * \sa matrixL(), matrixU()
-      */
-    inline const MatrixType& matrixLU() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return m_lu;
-    }
+  /** \returns the LU decomposition matrix: the upper-triangular part is U, the
+   * unit-lower-triangular part is L (at least for square matrices; in the non-square
+   * case, special care is needed, see the documentation of class FullPivLU).
+   *
+   * \sa matrixL(), matrixU()
+   */
+  inline const MatrixType& matrixLU() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return m_lu;
+  }
 
-    /** \returns the number of nonzero pivots in the LU decomposition.
-      * Here nonzero is meant in the exact sense, not in a fuzzy sense.
-      * So that notion isn't really intrinsically interesting, but it is
-      * still useful when implementing algorithms.
-      *
-      * \sa rank()
-      */
-    inline Index nonzeroPivots() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return m_nonzero_pivots;
-    }
+  /** \returns the number of nonzero pivots in the LU decomposition.
+   * Here nonzero is meant in the exact sense, not in a fuzzy sense.
+   * So that notion isn't really intrinsically interesting, but it is
+   * still useful when implementing algorithms.
+   *
+   * \sa rank()
+   */
+  inline Index nonzeroPivots() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return m_nonzero_pivots;
+  }
 
-    /** \returns the absolute value of the biggest pivot, i.e. the biggest
-      *          diagonal coefficient of U.
-      */
-    RealScalar maxPivot() const { return m_maxpivot; }
+  /** \returns the absolute value of the biggest pivot, i.e. the biggest
+   *          diagonal coefficient of U.
+   */
+  RealScalar maxPivot() const { return m_maxpivot; }
 
-    /** \returns the permutation matrix P
-      *
-      * \sa permutationQ()
-      */
-    EIGEN_DEVICE_FUNC inline const PermutationPType& permutationP() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return m_p;
-    }
+  /** \returns the permutation matrix P
+   *
+   * \sa permutationQ()
+   */
+  EIGEN_DEVICE_FUNC inline const PermutationPType& permutationP() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return m_p;
+  }
 
-    /** \returns the permutation matrix Q
-      *
-      * \sa permutationP()
-      */
-    inline const PermutationQType& permutationQ() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return m_q;
-    }
+  /** \returns the permutation matrix Q
+   *
+   * \sa permutationP()
+   */
+  inline const PermutationQType& permutationQ() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return m_q;
+  }
 
-    /** \returns the kernel of the matrix, also called its null-space. The columns of the returned matrix
-      * will form a basis of the kernel.
-      *
-      * \note If the kernel has dimension zero, then the returned matrix is a column-vector filled with zeros.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      *
-      * Example: \include FullPivLU_kernel.cpp
-      * Output: \verbinclude FullPivLU_kernel.out
-      *
-      * \sa image()
-      */
-    inline const internal::kernel_retval<FullPivLU> kernel() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return internal::kernel_retval<FullPivLU>(*this);
-    }
+  /** \returns the kernel of the matrix, also called its null-space. The columns of the returned matrix
+   * will form a basis of the kernel.
+   *
+   * \note If the kernel has dimension zero, then the returned matrix is a column-vector filled with zeros.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   *
+   * Example: \include FullPivLU_kernel.cpp
+   * Output: \verbinclude FullPivLU_kernel.out
+   *
+   * \sa image()
+   */
+  inline const internal::kernel_retval<FullPivLU> kernel() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return internal::kernel_retval<FullPivLU>(*this);
+  }
 
-    /** \returns the image of the matrix, also called its column-space. The columns of the returned matrix
-      * will form a basis of the image (column-space).
-      *
-      * \param originalMatrix the original matrix, of which *this is the LU decomposition.
-      *                       The reason why it is needed to pass it here, is that this allows
-      *                       a large optimization, as otherwise this method would need to reconstruct it
-      *                       from the LU decomposition.
-      *
-      * \note If the image has dimension zero, then the returned matrix is a column-vector filled with zeros.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      *
-      * Example: \include FullPivLU_image.cpp
-      * Output: \verbinclude FullPivLU_image.out
-      *
-      * \sa kernel()
-      */
-    inline const internal::image_retval<FullPivLU>
-      image(const MatrixType& originalMatrix) const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return internal::image_retval<FullPivLU>(*this, originalMatrix);
-    }
+  /** \returns the image of the matrix, also called its column-space. The columns of the returned matrix
+   * will form a basis of the image (column-space).
+   *
+   * \param originalMatrix the original matrix, of which *this is the LU decomposition.
+   *                       The reason why it is needed to pass it here, is that this allows
+   *                       a large optimization, as otherwise this method would need to reconstruct it
+   *                       from the LU decomposition.
+   *
+   * \note If the image has dimension zero, then the returned matrix is a column-vector filled with zeros.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   *
+   * Example: \include FullPivLU_image.cpp
+   * Output: \verbinclude FullPivLU_image.out
+   *
+   * \sa kernel()
+   */
+  inline const internal::image_retval<FullPivLU> image(const MatrixType& originalMatrix) const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return internal::image_retval<FullPivLU>(*this, originalMatrix);
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** \return a solution x to the equation Ax=b, where A is the matrix of which
-      * *this is the LU decomposition.
-      *
-      * \param b the right-hand-side of the equation to solve. Can be a vector or a matrix,
-      *          the only requirement in order for the equation to make sense is that
-      *          b.rows()==A.rows(), where A is the matrix of which *this is the LU decomposition.
-      *
-      * \returns a solution.
-      *
-      * \note_about_checking_solutions
-      *
-      * \note_about_arbitrary_choice_of_solution
-      * \note_about_using_kernel_to_study_multiple_solutions
-      *
-      * Example: \include FullPivLU_solve.cpp
-      * Output: \verbinclude FullPivLU_solve.out
-      *
-      * \sa TriangularView::solve(), kernel(), inverse()
-      */
-    template<typename Rhs>
-    inline const Solve<FullPivLU, Rhs>
-    solve(const MatrixBase<Rhs>& b) const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** \return a solution x to the equation Ax=b, where A is the matrix of which
+   * *this is the LU decomposition.
+   *
+   * \param b the right-hand-side of the equation to solve. Can be a vector or a matrix,
+   *          the only requirement in order for the equation to make sense is that
+   *          b.rows()==A.rows(), where A is the matrix of which *this is the LU decomposition.
+   *
+   * \returns a solution.
+   *
+   * \note_about_checking_solutions
+   *
+   * \note_about_arbitrary_choice_of_solution
+   * \note_about_using_kernel_to_study_multiple_solutions
+   *
+   * Example: \include FullPivLU_solve.cpp
+   * Output: \verbinclude FullPivLU_solve.out
+   *
+   * \sa TriangularView::solve(), kernel(), inverse()
+   */
+  template <typename Rhs>
+  inline const Solve<FullPivLU, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
-    /** \returns an estimate of the reciprocal condition number of the matrix of which \c *this is
-        the LU decomposition.
-      */
-    inline RealScalar rcond() const
-    {
-      eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
-      return internal::rcond_estimate_helper(m_l1_norm, *this);
-    }
+  /** \returns an estimate of the reciprocal condition number of the matrix of which \c *this is
+      the LU decomposition.
+    */
+  inline RealScalar rcond() const {
+    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
+    return internal::rcond_estimate_helper(m_l1_norm, *this);
+  }
 
-    /** \returns the determinant of the matrix of which
-      * *this is the LU decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the LU decomposition has already been computed.
-      *
-      * \note This is only for square matrices.
-      *
-      * \note For fixed-size matrices of size up to 4, MatrixBase::determinant() offers
-      *       optimized paths.
-      *
-      * \warning a determinant can be very big or small, so for matrices
-      * of large enough dimension, there is a risk of overflow/underflow.
-      *
-      * \sa MatrixBase::determinant()
-      */
-    typename internal::traits<MatrixType>::Scalar determinant() const;
+  /** \returns the determinant of the matrix of which
+   * *this is the LU decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the LU decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \note For fixed-size matrices of size up to 4, MatrixBase::determinant() offers
+   *       optimized paths.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   *
+   * \sa MatrixBase::determinant()
+   */
+  typename internal::traits<MatrixType>::Scalar determinant() const;
 
-    /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
-      * who need to determine when pivots are to be considered nonzero. This is not used for the
-      * LU decomposition itself.
-      *
-      * When it needs to get the threshold value, Eigen calls threshold(). By default, this
-      * uses a formula to automatically determine a reasonable threshold.
-      * Once you have called the present method setThreshold(const RealScalar&),
-      * your value is used instead.
-      *
-      * \param threshold The new value to use as the threshold.
-      *
-      * A pivot will be considered nonzero if its absolute value is strictly greater than
-      *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
-      * where maxpivot is the biggest pivot.
-      *
-      * If you want to come back to the default behavior, call setThreshold(Default_t)
-      */
-    FullPivLU& setThreshold(const RealScalar& threshold)
-    {
-      m_usePrescribedThreshold = true;
-      m_prescribedThreshold = threshold;
-      return *this;
-    }
+  /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
+   * who need to determine when pivots are to be considered nonzero. This is not used for the
+   * LU decomposition itself.
+   *
+   * When it needs to get the threshold value, Eigen calls threshold(). By default, this
+   * uses a formula to automatically determine a reasonable threshold.
+   * Once you have called the present method setThreshold(const RealScalar&),
+   * your value is used instead.
+   *
+   * \param threshold The new value to use as the threshold.
+   *
+   * A pivot will be considered nonzero if its absolute value is strictly greater than
+   *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
+   * where maxpivot is the biggest pivot.
+   *
+   * If you want to come back to the default behavior, call setThreshold(Default_t)
+   */
+  FullPivLU& setThreshold(const RealScalar& threshold) {
+    m_usePrescribedThreshold = true;
+    m_prescribedThreshold = threshold;
+    return *this;
+  }
 
-    /** Allows to come back to the default behavior, letting Eigen use its default formula for
-      * determining the threshold.
-      *
-      * You should pass the special object Eigen::Default as parameter here.
-      * \code lu.setThreshold(Eigen::Default); \endcode
-      *
-      * See the documentation of setThreshold(const RealScalar&).
-      */
-    FullPivLU& setThreshold(Default_t)
-    {
-      m_usePrescribedThreshold = false;
-      return *this;
-    }
+  /** Allows to come back to the default behavior, letting Eigen use its default formula for
+   * determining the threshold.
+   *
+   * You should pass the special object Eigen::Default as parameter here.
+   * \code lu.setThreshold(Eigen::Default); \endcode
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  FullPivLU& setThreshold(Default_t) {
+    m_usePrescribedThreshold = false;
+    return *this;
+  }
 
-    /** Returns the threshold that will be used by certain methods such as rank().
-      *
-      * See the documentation of setThreshold(const RealScalar&).
-      */
-    RealScalar threshold() const
-    {
-      eigen_assert(m_isInitialized || m_usePrescribedThreshold);
-      return m_usePrescribedThreshold ? m_prescribedThreshold
-      // this formula comes from experimenting (see "LU precision tuning" thread on the list)
-      // and turns out to be identical to Higham's formula used already in LDLt.
-          : NumTraits<Scalar>::epsilon() * RealScalar(m_lu.diagonalSize());
-    }
+  /** Returns the threshold that will be used by certain methods such as rank().
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  RealScalar threshold() const {
+    eigen_assert(m_isInitialized || m_usePrescribedThreshold);
+    return m_usePrescribedThreshold ? m_prescribedThreshold
+                                    // this formula comes from experimenting (see "LU precision tuning" thread on the
+                                    // list) and turns out to be identical to Higham's formula used already in LDLt.
+                                    : NumTraits<Scalar>::epsilon() * RealScalar(m_lu.diagonalSize());
+  }
 
-    /** \returns the rank of the matrix of which *this is the LU decomposition.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline Index rank() const
-    {
-      using std::abs;
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      RealScalar premultiplied_threshold = abs(m_maxpivot) * threshold();
-      Index result = 0;
-      for(Index i = 0; i < m_nonzero_pivots; ++i)
-        result += (abs(m_lu.coeff(i,i)) > premultiplied_threshold);
-      return result;
-    }
+  /** \returns the rank of the matrix of which *this is the LU decomposition.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline Index rank() const {
+    using std::abs;
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    RealScalar premultiplied_threshold = abs(m_maxpivot) * threshold();
+    Index result = 0;
+    for (Index i = 0; i < m_nonzero_pivots; ++i) result += (abs(m_lu.coeff(i, i)) > premultiplied_threshold);
+    return result;
+  }
 
-    /** \returns the dimension of the kernel of the matrix of which *this is the LU decomposition.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline Index dimensionOfKernel() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return cols() - rank();
-    }
+  /** \returns the dimension of the kernel of the matrix of which *this is the LU decomposition.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline Index dimensionOfKernel() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return cols() - rank();
+  }
 
-    /** \returns true if the matrix of which *this is the LU decomposition represents an injective
-      *          linear map, i.e. has trivial kernel; false otherwise.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isInjective() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return rank() == cols();
-    }
+  /** \returns true if the matrix of which *this is the LU decomposition represents an injective
+   *          linear map, i.e. has trivial kernel; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isInjective() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return rank() == cols();
+  }
 
-    /** \returns true if the matrix of which *this is the LU decomposition represents a surjective
-      *          linear map; false otherwise.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isSurjective() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return rank() == rows();
-    }
+  /** \returns true if the matrix of which *this is the LU decomposition represents a surjective
+   *          linear map; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isSurjective() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return rank() == rows();
+  }
 
-    /** \returns true if the matrix of which *this is the LU decomposition is invertible.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isInvertible() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return isInjective() && (m_lu.rows() == m_lu.cols());
-    }
+  /** \returns true if the matrix of which *this is the LU decomposition is invertible.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isInvertible() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return isInjective() && (m_lu.rows() == m_lu.cols());
+  }
 
-    /** \returns the inverse of the matrix of which *this is the LU decomposition.
-      *
-      * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
-      *       Use isInvertible() to first determine whether this matrix is invertible.
-      *
-      * \sa MatrixBase::inverse()
-      */
-    inline const Inverse<FullPivLU> inverse() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      eigen_assert(m_lu.rows() == m_lu.cols() && "You can't take the inverse of a non-square matrix!");
-      return Inverse<FullPivLU>(*this);
-    }
+  /** \returns the inverse of the matrix of which *this is the LU decomposition.
+   *
+   * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
+   *       Use isInvertible() to first determine whether this matrix is invertible.
+   *
+   * \sa MatrixBase::inverse()
+   */
+  inline const Inverse<FullPivLU> inverse() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    eigen_assert(m_lu.rows() == m_lu.cols() && "You can't take the inverse of a non-square matrix!");
+    return Inverse<FullPivLU>(*this);
+  }
 
-    MatrixType reconstructedMatrix() const;
+  MatrixType reconstructedMatrix() const;
 
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
-    inline Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename RhsType, typename DstType>
-    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-    template<bool Conjugate, typename RhsType, typename DstType>
-    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
-    #endif
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
+#endif
 
-  protected:
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
+  void computeInPlace();
 
-    void computeInPlace();
-
-    MatrixType m_lu;
-    PermutationPType m_p;
-    PermutationQType m_q;
-    IntColVectorType m_rowsTranspositions;
-    IntRowVectorType m_colsTranspositions;
-    Index m_nonzero_pivots;
-    RealScalar m_l1_norm;
-    RealScalar m_maxpivot, m_prescribedThreshold;
-    signed char m_det_pq;
-    bool m_isInitialized, m_usePrescribedThreshold;
+  MatrixType m_lu;
+  PermutationPType m_p;
+  PermutationQType m_q;
+  IntColVectorType m_rowsTranspositions;
+  IntRowVectorType m_colsTranspositions;
+  Index m_nonzero_pivots;
+  RealScalar m_l1_norm;
+  RealScalar m_maxpivot, m_prescribedThreshold;
+  signed char m_det_pq;
+  bool m_isInitialized, m_usePrescribedThreshold;
 };
 
-template<typename MatrixType>
-FullPivLU<MatrixType>::FullPivLU()
-  : m_isInitialized(false), m_usePrescribedThreshold(false)
-{
-}
+template <typename MatrixType, typename PermutationIndex>
+FullPivLU<MatrixType, PermutationIndex>::FullPivLU() : m_isInitialized(false), m_usePrescribedThreshold(false) {}
 
-template<typename MatrixType>
-FullPivLU<MatrixType>::FullPivLU(Index rows, Index cols)
-  : m_lu(rows, cols),
-    m_p(rows),
-    m_q(cols),
-    m_rowsTranspositions(rows),
-    m_colsTranspositions(cols),
-    m_isInitialized(false),
-    m_usePrescribedThreshold(false)
-{
-}
+template <typename MatrixType, typename PermutationIndex>
+FullPivLU<MatrixType, PermutationIndex>::FullPivLU(Index rows, Index cols)
+    : m_lu(rows, cols),
+      m_p(rows),
+      m_q(cols),
+      m_rowsTranspositions(rows),
+      m_colsTranspositions(cols),
+      m_isInitialized(false),
+      m_usePrescribedThreshold(false) {}
 
-template<typename MatrixType>
-template<typename InputType>
-FullPivLU<MatrixType>::FullPivLU(const EigenBase<InputType>& matrix)
-  : m_lu(matrix.rows(), matrix.cols()),
-    m_p(matrix.rows()),
-    m_q(matrix.cols()),
-    m_rowsTranspositions(matrix.rows()),
-    m_colsTranspositions(matrix.cols()),
-    m_isInitialized(false),
-    m_usePrescribedThreshold(false)
-{
+template <typename MatrixType, typename PermutationIndex>
+template <typename InputType>
+FullPivLU<MatrixType, PermutationIndex>::FullPivLU(const EigenBase<InputType>& matrix)
+    : m_lu(matrix.rows(), matrix.cols()),
+      m_p(matrix.rows()),
+      m_q(matrix.cols()),
+      m_rowsTranspositions(matrix.rows()),
+      m_colsTranspositions(matrix.cols()),
+      m_isInitialized(false),
+      m_usePrescribedThreshold(false) {
   compute(matrix.derived());
 }
 
-template<typename MatrixType>
-template<typename InputType>
-FullPivLU<MatrixType>::FullPivLU(EigenBase<InputType>& matrix)
-  : m_lu(matrix.derived()),
-    m_p(matrix.rows()),
-    m_q(matrix.cols()),
-    m_rowsTranspositions(matrix.rows()),
-    m_colsTranspositions(matrix.cols()),
-    m_isInitialized(false),
-    m_usePrescribedThreshold(false)
-{
+template <typename MatrixType, typename PermutationIndex>
+template <typename InputType>
+FullPivLU<MatrixType, PermutationIndex>::FullPivLU(EigenBase<InputType>& matrix)
+    : m_lu(matrix.derived()),
+      m_p(matrix.rows()),
+      m_q(matrix.cols()),
+      m_rowsTranspositions(matrix.rows()),
+      m_colsTranspositions(matrix.cols()),
+      m_isInitialized(false),
+      m_usePrescribedThreshold(false) {
   computeInPlace();
 }
 
-template<typename MatrixType>
-void FullPivLU<MatrixType>::computeInPlace()
-{
-  check_template_parameters();
-
-  // the permutations are stored as int indices, so just to be sure:
-  eigen_assert(m_lu.rows()<=NumTraits<int>::highest() && m_lu.cols()<=NumTraits<int>::highest());
+template <typename MatrixType, typename PermutationIndex>
+void FullPivLU<MatrixType, PermutationIndex>::computeInPlace() {
+  eigen_assert(m_lu.rows() <= NumTraits<PermutationIndex>::highest() &&
+               m_lu.cols() <= NumTraits<PermutationIndex>::highest());
 
   m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff();
 
@@ -502,13 +470,12 @@
   // can't accumulate on-the-fly because that will be done in reverse order for the rows.
   m_rowsTranspositions.resize(m_lu.rows());
   m_colsTranspositions.resize(m_lu.cols());
-  Index number_of_transpositions = 0; // number of NONTRIVIAL transpositions, i.e. m_rowsTranspositions[i]!=i
+  Index number_of_transpositions = 0;  // number of NONTRIVIAL transpositions, i.e. m_rowsTranspositions[i]!=i
 
-  m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
+  m_nonzero_pivots = size;  // the generic case is that in which all pivots are nonzero (invertible case)
   m_maxpivot = RealScalar(0);
 
-  for(Index k = 0; k < size; ++k)
-  {
+  for (Index k = 0; k < size; ++k) {
     // First, we need to find the pivot.
 
     // biggest coefficient in the remaining bottom-right corner (starting at row k, col k)
@@ -516,38 +483,37 @@
     typedef internal::scalar_score_coeff_op<Scalar> Scoring;
     typedef typename Scoring::result_type Score;
     Score biggest_in_corner;
-    biggest_in_corner = m_lu.bottomRightCorner(rows-k, cols-k)
-                        .unaryExpr(Scoring())
-                        .maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner);
-    row_of_biggest_in_corner += k; // correct the values! since they were computed in the corner,
-    col_of_biggest_in_corner += k; // need to add k to them.
+    biggest_in_corner = m_lu.bottomRightCorner(rows - k, cols - k)
+                            .unaryExpr(Scoring())
+                            .maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner);
+    row_of_biggest_in_corner += k;  // correct the values! since they were computed in the corner,
+    col_of_biggest_in_corner += k;  // need to add k to them.
 
-    if(biggest_in_corner==Score(0))
-    {
+    if (numext::is_exactly_zero(biggest_in_corner)) {
       // before exiting, make sure to initialize the still uninitialized transpositions
       // in a sane state without destroying what we already have.
       m_nonzero_pivots = k;
-      for(Index i = k; i < size; ++i)
-      {
+      for (Index i = k; i < size; ++i) {
         m_rowsTranspositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
         m_colsTranspositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
       }
       break;
     }
 
-    RealScalar abs_pivot = internal::abs_knowing_score<Scalar>()(m_lu(row_of_biggest_in_corner, col_of_biggest_in_corner), biggest_in_corner);
-    if(abs_pivot > m_maxpivot) m_maxpivot = abs_pivot;
+    RealScalar abs_pivot = internal::abs_knowing_score<Scalar>()(
+        m_lu(row_of_biggest_in_corner, col_of_biggest_in_corner), biggest_in_corner);
+    if (abs_pivot > m_maxpivot) m_maxpivot = abs_pivot;
 
     // Now that we've found the pivot, we need to apply the row/col swaps to
     // bring it to the location (k,k).
 
     m_rowsTranspositions.coeffRef(k) = internal::convert_index<StorageIndex>(row_of_biggest_in_corner);
     m_colsTranspositions.coeffRef(k) = internal::convert_index<StorageIndex>(col_of_biggest_in_corner);
-    if(k != row_of_biggest_in_corner) {
+    if (k != row_of_biggest_in_corner) {
       m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner));
       ++number_of_transpositions;
     }
-    if(k != col_of_biggest_in_corner) {
+    if (k != col_of_biggest_in_corner) {
       m_lu.col(k).swap(m_lu.col(col_of_biggest_in_corner));
       ++number_of_transpositions;
     }
@@ -555,31 +521,28 @@
     // Now that the pivot is at the right location, we update the remaining
     // bottom-right corner by Gaussian elimination.
 
-    if(k<rows-1)
-      m_lu.col(k).tail(rows-k-1) /= m_lu.coeff(k,k);
-    if(k<size-1)
-      m_lu.block(k+1,k+1,rows-k-1,cols-k-1).noalias() -= m_lu.col(k).tail(rows-k-1) * m_lu.row(k).tail(cols-k-1);
+    if (k < rows - 1) m_lu.col(k).tail(rows - k - 1) /= m_lu.coeff(k, k);
+    if (k < size - 1)
+      m_lu.block(k + 1, k + 1, rows - k - 1, cols - k - 1).noalias() -=
+          m_lu.col(k).tail(rows - k - 1) * m_lu.row(k).tail(cols - k - 1);
   }
 
   // the main loop is over, we still have to accumulate the transpositions to find the
   // permutations P and Q
 
   m_p.setIdentity(rows);
-  for(Index k = size-1; k >= 0; --k)
-    m_p.applyTranspositionOnTheRight(k, m_rowsTranspositions.coeff(k));
+  for (Index k = size - 1; k >= 0; --k) m_p.applyTranspositionOnTheRight(k, m_rowsTranspositions.coeff(k));
 
   m_q.setIdentity(cols);
-  for(Index k = 0; k < size; ++k)
-    m_q.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k));
+  for (Index k = 0; k < size; ++k) m_q.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k));
 
-  m_det_pq = (number_of_transpositions%2) ? -1 : 1;
+  m_det_pq = (number_of_transpositions % 2) ? -1 : 1;
 
   m_isInitialized = true;
 }
 
-template<typename MatrixType>
-typename internal::traits<MatrixType>::Scalar FullPivLU<MatrixType>::determinant() const
-{
+template <typename MatrixType, typename PermutationIndex>
+typename internal::traits<MatrixType>::Scalar FullPivLU<MatrixType, PermutationIndex>::determinant() const {
   eigen_assert(m_isInitialized && "LU is not initialized.");
   eigen_assert(m_lu.rows() == m_lu.cols() && "You can't take the determinant of a non-square matrix!");
   return Scalar(m_det_pq) * Scalar(m_lu.diagonal().prod());
@@ -588,18 +551,15 @@
 /** \returns the matrix represented by the decomposition,
  * i.e., it returns the product: \f$ P^{-1} L U Q^{-1} \f$.
  * This function is provided for debug purposes. */
-template<typename MatrixType>
-MatrixType FullPivLU<MatrixType>::reconstructedMatrix() const
-{
+template <typename MatrixType, typename PermutationIndex>
+MatrixType FullPivLU<MatrixType, PermutationIndex>::reconstructedMatrix() const {
   eigen_assert(m_isInitialized && "LU is not initialized.");
   const Index smalldim = (std::min)(m_lu.rows(), m_lu.cols());
   // LU
-  MatrixType res(m_lu.rows(),m_lu.cols());
+  MatrixType res(m_lu.rows(), m_lu.cols());
   // FIXME the .toDenseMatrix() should not be needed...
-  res = m_lu.leftCols(smalldim)
-            .template triangularView<UnitLower>().toDenseMatrix()
-      * m_lu.topRows(smalldim)
-            .template triangularView<Upper>().toDenseMatrix();
+  res = m_lu.leftCols(smalldim).template triangularView<UnitLower>().toDenseMatrix() *
+        m_lu.topRows(smalldim).template triangularView<Upper>().toDenseMatrix();
 
   // P^{-1}(LU)
   res = m_p.inverse() * res;
@@ -613,23 +573,21 @@
 /********* Implementation of kernel() **************************************************/
 
 namespace internal {
-template<typename _MatrixType>
-struct kernel_retval<FullPivLU<_MatrixType> >
-  : kernel_retval_base<FullPivLU<_MatrixType> >
-{
-  EIGEN_MAKE_KERNEL_HELPERS(FullPivLU<_MatrixType>)
+template <typename MatrixType_, typename PermutationIndex_>
+struct kernel_retval<FullPivLU<MatrixType_, PermutationIndex_> >
+    : kernel_retval_base<FullPivLU<MatrixType_, PermutationIndex_> > {
+  using DecompositionType = FullPivLU<MatrixType_, PermutationIndex_>;
+  EIGEN_MAKE_KERNEL_HELPERS(DecompositionType)
 
-  enum { MaxSmallDimAtCompileTime = EIGEN_SIZE_MIN_PREFER_FIXED(
-            MatrixType::MaxColsAtCompileTime,
-            MatrixType::MaxRowsAtCompileTime)
+  enum {
+    MaxSmallDimAtCompileTime = min_size_prefer_fixed(MatrixType::MaxColsAtCompileTime, MatrixType::MaxRowsAtCompileTime)
   };
 
-  template<typename Dest> void evalTo(Dest& dst) const
-  {
+  template <typename Dest>
+  void evalTo(Dest& dst) const {
     using std::abs;
     const Index cols = dec().matrixLU().cols(), dimker = cols - rank();
-    if(dimker == 0)
-    {
+    if (dimker == 0) {
       // The Kernel is just {0}, so it doesn't have a basis properly speaking, but let's
       // avoid crashing/asserting as that depends on floating point calculations. Let's
       // just return a single column vector filled with zeros.
@@ -638,83 +596,74 @@
     }
 
     /* Let us use the following lemma:
-      *
-      * Lemma: If the matrix A has the LU decomposition PAQ = LU,
-      * then Ker A = Q(Ker U).
-      *
-      * Proof: trivial: just keep in mind that P, Q, L are invertible.
-      */
+     *
+     * Lemma: If the matrix A has the LU decomposition PAQ = LU,
+     * then Ker A = Q(Ker U).
+     *
+     * Proof: trivial: just keep in mind that P, Q, L are invertible.
+     */
 
     /* Thus, all we need to do is to compute Ker U, and then apply Q.
-      *
-      * U is upper triangular, with eigenvalues sorted so that any zeros appear at the end.
-      * Thus, the diagonal of U ends with exactly
-      * dimKer zero's. Let us use that to construct dimKer linearly
-      * independent vectors in Ker U.
-      */
+     *
+     * U is upper triangular, with eigenvalues sorted so that any zeros appear at the end.
+     * Thus, the diagonal of U ends with exactly
+     * dimKer zero's. Let us use that to construct dimKer linearly
+     * independent vectors in Ker U.
+     */
 
     Matrix<Index, Dynamic, 1, 0, MaxSmallDimAtCompileTime, 1> pivots(rank());
     RealScalar premultiplied_threshold = dec().maxPivot() * dec().threshold();
     Index p = 0;
-    for(Index i = 0; i < dec().nonzeroPivots(); ++i)
-      if(abs(dec().matrixLU().coeff(i,i)) > premultiplied_threshold)
-        pivots.coeffRef(p++) = i;
+    for (Index i = 0; i < dec().nonzeroPivots(); ++i)
+      if (abs(dec().matrixLU().coeff(i, i)) > premultiplied_threshold) pivots.coeffRef(p++) = i;
     eigen_internal_assert(p == rank());
 
     // we construct a temporaty trapezoid matrix m, by taking the U matrix and
     // permuting the rows and cols to bring the nonnegligible pivots to the top of
     // the main diagonal. We need that to be able to apply our triangular solvers.
     // FIXME when we get triangularView-for-rectangular-matrices, this can be simplified
-    Matrix<typename MatrixType::Scalar, Dynamic, Dynamic, MatrixType::Options,
-           MaxSmallDimAtCompileTime, MatrixType::MaxColsAtCompileTime>
-      m(dec().matrixLU().block(0, 0, rank(), cols));
-    for(Index i = 0; i < rank(); ++i)
-    {
-      if(i) m.row(i).head(i).setZero();
-      m.row(i).tail(cols-i) = dec().matrixLU().row(pivots.coeff(i)).tail(cols-i);
+    Matrix<typename MatrixType::Scalar, Dynamic, Dynamic, MatrixType::Options, MaxSmallDimAtCompileTime,
+           MatrixType::MaxColsAtCompileTime>
+        m(dec().matrixLU().block(0, 0, rank(), cols));
+    for (Index i = 0; i < rank(); ++i) {
+      if (i) m.row(i).head(i).setZero();
+      m.row(i).tail(cols - i) = dec().matrixLU().row(pivots.coeff(i)).tail(cols - i);
     }
     m.block(0, 0, rank(), rank());
     m.block(0, 0, rank(), rank()).template triangularView<StrictlyLower>().setZero();
-    for(Index i = 0; i < rank(); ++i)
-      m.col(i).swap(m.col(pivots.coeff(i)));
+    for (Index i = 0; i < rank(); ++i) m.col(i).swap(m.col(pivots.coeff(i)));
 
     // ok, we have our trapezoid matrix, we can apply the triangular solver.
     // notice that the math behind this suggests that we should apply this to the
     // negative of the RHS, but for performance we just put the negative sign elsewhere, see below.
-    m.topLeftCorner(rank(), rank())
-     .template triangularView<Upper>().solveInPlace(
-        m.topRightCorner(rank(), dimker)
-      );
+    m.topLeftCorner(rank(), rank()).template triangularView<Upper>().solveInPlace(m.topRightCorner(rank(), dimker));
 
     // now we must undo the column permutation that we had applied!
-    for(Index i = rank()-1; i >= 0; --i)
-      m.col(i).swap(m.col(pivots.coeff(i)));
+    for (Index i = rank() - 1; i >= 0; --i) m.col(i).swap(m.col(pivots.coeff(i)));
 
     // see the negative sign in the next line, that's what we were talking about above.
-    for(Index i = 0; i < rank(); ++i) dst.row(dec().permutationQ().indices().coeff(i)) = -m.row(i).tail(dimker);
-    for(Index i = rank(); i < cols; ++i) dst.row(dec().permutationQ().indices().coeff(i)).setZero();
-    for(Index k = 0; k < dimker; ++k) dst.coeffRef(dec().permutationQ().indices().coeff(rank()+k), k) = Scalar(1);
+    for (Index i = 0; i < rank(); ++i) dst.row(dec().permutationQ().indices().coeff(i)) = -m.row(i).tail(dimker);
+    for (Index i = rank(); i < cols; ++i) dst.row(dec().permutationQ().indices().coeff(i)).setZero();
+    for (Index k = 0; k < dimker; ++k) dst.coeffRef(dec().permutationQ().indices().coeff(rank() + k), k) = Scalar(1);
   }
 };
 
 /***** Implementation of image() *****************************************************/
 
-template<typename _MatrixType>
-struct image_retval<FullPivLU<_MatrixType> >
-  : image_retval_base<FullPivLU<_MatrixType> >
-{
-  EIGEN_MAKE_IMAGE_HELPERS(FullPivLU<_MatrixType>)
+template <typename MatrixType_, typename PermutationIndex_>
+struct image_retval<FullPivLU<MatrixType_, PermutationIndex_> >
+    : image_retval_base<FullPivLU<MatrixType_, PermutationIndex_> > {
+  using DecompositionType = FullPivLU<MatrixType_, PermutationIndex_>;
+  EIGEN_MAKE_IMAGE_HELPERS(DecompositionType)
 
-  enum { MaxSmallDimAtCompileTime = EIGEN_SIZE_MIN_PREFER_FIXED(
-            MatrixType::MaxColsAtCompileTime,
-            MatrixType::MaxRowsAtCompileTime)
+  enum {
+    MaxSmallDimAtCompileTime = min_size_prefer_fixed(MatrixType::MaxColsAtCompileTime, MatrixType::MaxRowsAtCompileTime)
   };
 
-  template<typename Dest> void evalTo(Dest& dst) const
-  {
+  template <typename Dest>
+  void evalTo(Dest& dst) const {
     using std::abs;
-    if(rank() == 0)
-    {
+    if (rank() == 0) {
       // The Image is just {0}, so it doesn't have a basis properly speaking, but let's
       // avoid crashing/asserting as that depends on floating point calculations. Let's
       // just return a single column vector filled with zeros.
@@ -725,40 +674,35 @@
     Matrix<Index, Dynamic, 1, 0, MaxSmallDimAtCompileTime, 1> pivots(rank());
     RealScalar premultiplied_threshold = dec().maxPivot() * dec().threshold();
     Index p = 0;
-    for(Index i = 0; i < dec().nonzeroPivots(); ++i)
-      if(abs(dec().matrixLU().coeff(i,i)) > premultiplied_threshold)
-        pivots.coeffRef(p++) = i;
+    for (Index i = 0; i < dec().nonzeroPivots(); ++i)
+      if (abs(dec().matrixLU().coeff(i, i)) > premultiplied_threshold) pivots.coeffRef(p++) = i;
     eigen_internal_assert(p == rank());
 
-    for(Index i = 0; i < rank(); ++i)
+    for (Index i = 0; i < rank(); ++i)
       dst.col(i) = originalMatrix().col(dec().permutationQ().indices().coeff(pivots.coeff(i)));
   }
 };
 
 /***** Implementation of solve() *****************************************************/
 
-} // end namespace internal
+}  // end namespace internal
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename _MatrixType>
-template<typename RhsType, typename DstType>
-void FullPivLU<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, typename PermutationIndex_>
+template <typename RhsType, typename DstType>
+void FullPivLU<MatrixType_, PermutationIndex_>::_solve_impl(const RhsType& rhs, DstType& dst) const {
   /* The decomposition PAQ = LU can be rewritten as A = P^{-1} L U Q^{-1}.
-  * So we proceed as follows:
-  * Step 1: compute c = P * rhs.
-  * Step 2: replace c by the solution x to Lx = c. Exists because L is invertible.
-  * Step 3: replace c by the solution x to Ux = c. May or may not exist.
-  * Step 4: result = Q * c;
-  */
+   * So we proceed as follows:
+   * Step 1: compute c = P * rhs.
+   * Step 2: replace c by the solution x to Lx = c. Exists because L is invertible.
+   * Step 3: replace c by the solution x to Ux = c. May or may not exist.
+   * Step 4: result = Q * c;
+   */
 
-  const Index rows = this->rows(),
-              cols = this->cols(),
-              nonzero_pivots = this->rank();
+  const Index rows = this->rows(), cols = this->cols(), nonzero_pivots = this->rank();
   const Index smalldim = (std::min)(rows, cols);
 
-  if(nonzero_pivots == 0)
-  {
+  if (nonzero_pivots == 0) {
     dst.setZero();
     return;
   }
@@ -769,11 +713,8 @@
   c = permutationP() * rhs;
 
   // Step 2
-  m_lu.topLeftCorner(smalldim,smalldim)
-      .template triangularView<UnitLower>()
-      .solveInPlace(c.topRows(smalldim));
-  if(rows>cols)
-    c.bottomRows(rows-cols) -= m_lu.bottomRows(rows-cols) * c.topRows(cols);
+  m_lu.topLeftCorner(smalldim, smalldim).template triangularView<UnitLower>().solveInPlace(c.topRows(smalldim));
+  if (rows > cols) c.bottomRows(rows - cols) -= m_lu.bottomRows(rows - cols) * c.topRows(cols);
 
   // Step 3
   m_lu.topLeftCorner(nonzero_pivots, nonzero_pivots)
@@ -781,16 +722,13 @@
       .solveInPlace(c.topRows(nonzero_pivots));
 
   // Step 4
-  for(Index i = 0; i < nonzero_pivots; ++i)
-    dst.row(permutationQ().indices().coeff(i)) = c.row(i);
-  for(Index i = nonzero_pivots; i < m_lu.cols(); ++i)
-    dst.row(permutationQ().indices().coeff(i)).setZero();
+  for (Index i = 0; i < nonzero_pivots; ++i) dst.row(permutationQ().indices().coeff(i)) = c.row(i);
+  for (Index i = nonzero_pivots; i < m_lu.cols(); ++i) dst.row(permutationQ().indices().coeff(i)).setZero();
 }
 
-template<typename _MatrixType>
-template<bool Conjugate, typename RhsType, typename DstType>
-void FullPivLU<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, typename PermutationIndex_>
+template <bool Conjugate, typename RhsType, typename DstType>
+void FullPivLU<MatrixType_, PermutationIndex_>::_solve_impl_transposed(const RhsType& rhs, DstType& dst) const {
   /* The decomposition PAQ = LU can be rewritten as A = P^{-1} L U Q^{-1},
    * and since permutations are real and unitary, we can write this
    * as   A^T = Q U^T L^T P,
@@ -802,12 +740,10 @@
    * If Conjugate is true, replace "^T" by "^*" above.
    */
 
-  const Index rows = this->rows(), cols = this->cols(),
-    nonzero_pivots = this->rank();
+  const Index rows = this->rows(), cols = this->cols(), nonzero_pivots = this->rank();
   const Index smalldim = (std::min)(rows, cols);
 
-  if(nonzero_pivots == 0)
-  {
+  if (nonzero_pivots == 0) {
     dst.setZero();
     return;
   }
@@ -833,45 +769,44 @@
 
   // Step 4
   PermutationPType invp = permutationP().inverse().eval();
-  for(Index i = 0; i < smalldim; ++i)
-    dst.row(invp.indices().coeff(i)) = c.row(i);
-  for(Index i = smalldim; i < rows; ++i)
-    dst.row(invp.indices().coeff(i)).setZero();
+  for (Index i = 0; i < smalldim; ++i) dst.row(invp.indices().coeff(i)) = c.row(i);
+  for (Index i = smalldim; i < rows; ++i) dst.row(invp.indices().coeff(i)).setZero();
 }
 
 #endif
 
 namespace internal {
 
-
 /***** Implementation of inverse() *****************************************************/
-template<typename DstXprType, typename MatrixType>
-struct Assignment<DstXprType, Inverse<FullPivLU<MatrixType> >, internal::assign_op<typename DstXprType::Scalar,typename FullPivLU<MatrixType>::Scalar>, Dense2Dense>
-{
-  typedef FullPivLU<MatrixType> LuType;
+template <typename DstXprType, typename MatrixType, typename PermutationIndex>
+struct Assignment<
+    DstXprType, Inverse<FullPivLU<MatrixType, PermutationIndex> >,
+    internal::assign_op<typename DstXprType::Scalar, typename FullPivLU<MatrixType, PermutationIndex>::Scalar>,
+    Dense2Dense> {
+  typedef FullPivLU<MatrixType, PermutationIndex> LuType;
   typedef Inverse<LuType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename MatrixType::Scalar> &)
-  {
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename MatrixType::Scalar>&) {
     dst = src.nestedExpression().solve(MatrixType::Identity(src.rows(), src.cols()));
   }
 };
-} // end namespace internal
+}  // end namespace internal
 
 /******* MatrixBase methods *****************************************************************/
 
 /** \lu_module
-  *
-  * \return the full-pivoting LU decomposition of \c *this.
-  *
-  * \sa class FullPivLU
-  */
-template<typename Derived>
-inline const FullPivLU<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::fullPivLu() const
-{
-  return FullPivLU<PlainObject>(eval());
+ *
+ * \return the full-pivoting LU decomposition of \c *this.
+ *
+ * \sa class FullPivLU
+ */
+template <typename Derived>
+template <typename PermutationIndex>
+inline const FullPivLU<typename MatrixBase<Derived>::PlainObject, PermutationIndex> MatrixBase<Derived>::fullPivLu()
+    const {
+  return FullPivLU<PlainObject, PermutationIndex>(eval());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_LU_H
+#endif  // EIGEN_LU_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InternalHeaderCheck.h
new file mode 100644
index 0000000..f346b17
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_LU_MODULE_H
+#error "Please include Eigen/LU instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h
index a40cefa..57fd677 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h
@@ -11,7 +11,10 @@
 #ifndef EIGEN_INVERSE_IMPL_H
 #define EIGEN_INVERSE_IMPL_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
@@ -19,51 +22,40 @@
 *** General case implementation ***
 **********************************/
 
-template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
-struct compute_inverse
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(const MatrixType& matrix, ResultType& result)
-  {
+template <typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
+struct compute_inverse {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix, ResultType& result) {
     result = matrix.partialPivLu().inverse();
   }
 };
 
-template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
-struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
+template <typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
+struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */
+};
 
 /****************************
 *** Size 1 implementation ***
 ****************************/
 
-template<typename MatrixType, typename ResultType>
-struct compute_inverse<MatrixType, ResultType, 1>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(const MatrixType& matrix, ResultType& result)
-  {
+template <typename MatrixType, typename ResultType>
+struct compute_inverse<MatrixType, ResultType, 1> {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix, ResultType& result) {
     typedef typename MatrixType::Scalar Scalar;
     internal::evaluator<MatrixType> matrixEval(matrix);
-    result.coeffRef(0,0) = Scalar(1) / matrixEval.coeff(0,0);
+    result.coeffRef(0, 0) = Scalar(1) / matrixEval.coeff(0, 0);
   }
 };
 
-template<typename MatrixType, typename ResultType>
-struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(
-    const MatrixType& matrix,
-    const typename MatrixType::RealScalar& absDeterminantThreshold,
-    ResultType& result,
-    typename ResultType::Scalar& determinant,
-    bool& invertible
-  )
-  {
+template <typename MatrixType, typename ResultType>
+struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1> {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix,
+                                           const typename MatrixType::RealScalar& absDeterminantThreshold,
+                                           ResultType& result, typename ResultType::Scalar& determinant,
+                                           bool& invertible) {
     using std::abs;
-    determinant = matrix.coeff(0,0);
+    determinant = matrix.coeff(0, 0);
     invertible = abs(determinant) > absDeterminantThreshold;
-    if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
+    if (invertible) result.coeffRef(0, 0) = typename ResultType::Scalar(1) / determinant;
   }
 };
 
@@ -71,48 +63,37 @@
 *** Size 2 implementation ***
 ****************************/
 
-template<typename MatrixType, typename ResultType>
-EIGEN_DEVICE_FUNC 
-inline void compute_inverse_size2_helper(
-    const MatrixType& matrix, const typename ResultType::Scalar& invdet,
-    ResultType& result)
-{
-  typename ResultType::Scalar temp = matrix.coeff(0,0);
-  result.coeffRef(0,0) =  matrix.coeff(1,1) * invdet;
-  result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
-  result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
-  result.coeffRef(1,1) =  temp * invdet;
+template <typename MatrixType, typename ResultType>
+EIGEN_DEVICE_FUNC inline void compute_inverse_size2_helper(const MatrixType& matrix,
+                                                           const typename ResultType::Scalar& invdet,
+                                                           ResultType& result) {
+  typename ResultType::Scalar temp = matrix.coeff(0, 0);
+  result.coeffRef(0, 0) = matrix.coeff(1, 1) * invdet;
+  result.coeffRef(1, 0) = -matrix.coeff(1, 0) * invdet;
+  result.coeffRef(0, 1) = -matrix.coeff(0, 1) * invdet;
+  result.coeffRef(1, 1) = temp * invdet;
 }
 
-template<typename MatrixType, typename ResultType>
-struct compute_inverse<MatrixType, ResultType, 2>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(const MatrixType& matrix, ResultType& result)
-  {
+template <typename MatrixType, typename ResultType>
+struct compute_inverse<MatrixType, ResultType, 2> {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix, ResultType& result) {
     typedef typename ResultType::Scalar Scalar;
     const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
     compute_inverse_size2_helper(matrix, invdet, result);
   }
 };
 
-template<typename MatrixType, typename ResultType>
-struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(
-    const MatrixType& matrix,
-    const typename MatrixType::RealScalar& absDeterminantThreshold,
-    ResultType& inverse,
-    typename ResultType::Scalar& determinant,
-    bool& invertible
-  )
-  {
+template <typename MatrixType, typename ResultType>
+struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2> {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix,
+                                           const typename MatrixType::RealScalar& absDeterminantThreshold,
+                                           ResultType& inverse, typename ResultType::Scalar& determinant,
+                                           bool& invertible) {
     using std::abs;
     typedef typename ResultType::Scalar Scalar;
     determinant = matrix.determinant();
     invertible = abs(determinant) > absDeterminantThreshold;
-    if(!invertible) return;
+    if (!invertible) return;
     const Scalar invdet = Scalar(1) / determinant;
     compute_inverse_size2_helper(matrix, invdet, inverse);
   }
@@ -122,79 +103,58 @@
 *** Size 3 implementation ***
 ****************************/
 
-template<typename MatrixType, int i, int j>
-EIGEN_DEVICE_FUNC 
-inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
-{
-  enum {
-    i1 = (i+1) % 3,
-    i2 = (i+2) % 3,
-    j1 = (j+1) % 3,
-    j2 = (j+2) % 3
-  };
-  return m.coeff(i1, j1) * m.coeff(i2, j2)
-       - m.coeff(i1, j2) * m.coeff(i2, j1);
+template <typename MatrixType, int i, int j>
+EIGEN_DEVICE_FUNC inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m) {
+  enum { i1 = (i + 1) % 3, i2 = (i + 2) % 3, j1 = (j + 1) % 3, j2 = (j + 2) % 3 };
+  return m.coeff(i1, j1) * m.coeff(i2, j2) - m.coeff(i1, j2) * m.coeff(i2, j1);
 }
 
-template<typename MatrixType, typename ResultType>
-EIGEN_DEVICE_FUNC
-inline void compute_inverse_size3_helper(
-    const MatrixType& matrix,
-    const typename ResultType::Scalar& invdet,
-    const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
-    ResultType& result)
-{
+template <typename MatrixType, typename ResultType>
+EIGEN_DEVICE_FUNC inline void compute_inverse_size3_helper(
+    const MatrixType& matrix, const typename ResultType::Scalar& invdet,
+    const Matrix<typename ResultType::Scalar, 3, 1>& cofactors_col0, ResultType& result) {
   // Compute cofactors in a way that avoids aliasing issues.
   typedef typename ResultType::Scalar Scalar;
-  const Scalar c01 = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
-  const Scalar c11 = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
-  const Scalar c02 = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
-  result.coeffRef(1,2) =  cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
-  result.coeffRef(2,1) =  cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
-  result.coeffRef(2,2) =  cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
-  result.coeffRef(1,0) =  c01;
-  result.coeffRef(1,1) =  c11;
-  result.coeffRef(2,0) =  c02;  
+  const Scalar c01 = cofactor_3x3<MatrixType, 0, 1>(matrix) * invdet;
+  const Scalar c11 = cofactor_3x3<MatrixType, 1, 1>(matrix) * invdet;
+  const Scalar c02 = cofactor_3x3<MatrixType, 0, 2>(matrix) * invdet;
+  result.coeffRef(1, 2) = cofactor_3x3<MatrixType, 2, 1>(matrix) * invdet;
+  result.coeffRef(2, 1) = cofactor_3x3<MatrixType, 1, 2>(matrix) * invdet;
+  result.coeffRef(2, 2) = cofactor_3x3<MatrixType, 2, 2>(matrix) * invdet;
+  result.coeffRef(1, 0) = c01;
+  result.coeffRef(1, 1) = c11;
+  result.coeffRef(2, 0) = c02;
   result.row(0) = cofactors_col0 * invdet;
 }
 
-template<typename MatrixType, typename ResultType>
-struct compute_inverse<MatrixType, ResultType, 3>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(const MatrixType& matrix, ResultType& result)
-  {
+template <typename MatrixType, typename ResultType>
+struct compute_inverse<MatrixType, ResultType, 3> {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix, ResultType& result) {
     typedef typename ResultType::Scalar Scalar;
-    Matrix<typename MatrixType::Scalar,3,1> cofactors_col0;
-    cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
-    cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
-    cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
+    Matrix<typename MatrixType::Scalar, 3, 1> cofactors_col0;
+    cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType, 0, 0>(matrix);
+    cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType, 1, 0>(matrix);
+    cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType, 2, 0>(matrix);
     const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
     const Scalar invdet = Scalar(1) / det;
     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
   }
 };
 
-template<typename MatrixType, typename ResultType>
-struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(
-    const MatrixType& matrix,
-    const typename MatrixType::RealScalar& absDeterminantThreshold,
-    ResultType& inverse,
-    typename ResultType::Scalar& determinant,
-    bool& invertible
-  )
-  {
+template <typename MatrixType, typename ResultType>
+struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3> {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix,
+                                           const typename MatrixType::RealScalar& absDeterminantThreshold,
+                                           ResultType& inverse, typename ResultType::Scalar& determinant,
+                                           bool& invertible) {
     typedef typename ResultType::Scalar Scalar;
-    Matrix<Scalar,3,1> cofactors_col0;
-    cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
-    cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
-    cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
+    Matrix<Scalar, 3, 1> cofactors_col0;
+    cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType, 0, 0>(matrix);
+    cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType, 1, 0>(matrix);
+    cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType, 2, 0>(matrix);
     determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
     invertible = Eigen::numext::abs(determinant) > absDeterminantThreshold;
-    if(!invertible) return;
+    if (!invertible) return;
     const Scalar invdet = Scalar(1) / determinant;
     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
   }
@@ -204,84 +164,59 @@
 *** Size 4 implementation ***
 ****************************/
 
-template<typename Derived>
-EIGEN_DEVICE_FUNC 
-inline const typename Derived::Scalar general_det3_helper
-(const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
-{
-  return matrix.coeff(i1,j1)
-         * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const typename Derived::Scalar general_det3_helper(const MatrixBase<Derived>& matrix, int i1,
+                                                                            int i2, int i3, int j1, int j2, int j3) {
+  return matrix.coeff(i1, j1) *
+         (matrix.coeff(i2, j2) * matrix.coeff(i3, j3) - matrix.coeff(i2, j3) * matrix.coeff(i3, j2));
 }
 
-template<typename MatrixType, int i, int j>
-EIGEN_DEVICE_FUNC 
-inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
-{
-  enum {
-    i1 = (i+1) % 4,
-    i2 = (i+2) % 4,
-    i3 = (i+3) % 4,
-    j1 = (j+1) % 4,
-    j2 = (j+2) % 4,
-    j3 = (j+3) % 4
-  };
-  return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
-       + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
-       + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
+template <typename MatrixType, int i, int j>
+EIGEN_DEVICE_FUNC inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix) {
+  enum { i1 = (i + 1) % 4, i2 = (i + 2) % 4, i3 = (i + 3) % 4, j1 = (j + 1) % 4, j2 = (j + 2) % 4, j3 = (j + 3) % 4 };
+  return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3) + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3) +
+         general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
 }
 
-template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
-struct compute_inverse_size4
-{
-  EIGEN_DEVICE_FUNC
-  static void run(const MatrixType& matrix, ResultType& result)
-  {
-    result.coeffRef(0,0) =  cofactor_4x4<MatrixType,0,0>(matrix);
-    result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
-    result.coeffRef(2,0) =  cofactor_4x4<MatrixType,0,2>(matrix);
-    result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
-    result.coeffRef(0,2) =  cofactor_4x4<MatrixType,2,0>(matrix);
-    result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
-    result.coeffRef(2,2) =  cofactor_4x4<MatrixType,2,2>(matrix);
-    result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
-    result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
-    result.coeffRef(1,1) =  cofactor_4x4<MatrixType,1,1>(matrix);
-    result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
-    result.coeffRef(3,1) =  cofactor_4x4<MatrixType,1,3>(matrix);
-    result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
-    result.coeffRef(1,3) =  cofactor_4x4<MatrixType,3,1>(matrix);
-    result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
-    result.coeffRef(3,3) =  cofactor_4x4<MatrixType,3,3>(matrix);
+template <int Arch, typename Scalar, typename MatrixType, typename ResultType>
+struct compute_inverse_size4 {
+  EIGEN_DEVICE_FUNC static void run(const MatrixType& matrix, ResultType& result) {
+    result.coeffRef(0, 0) = cofactor_4x4<MatrixType, 0, 0>(matrix);
+    result.coeffRef(1, 0) = -cofactor_4x4<MatrixType, 0, 1>(matrix);
+    result.coeffRef(2, 0) = cofactor_4x4<MatrixType, 0, 2>(matrix);
+    result.coeffRef(3, 0) = -cofactor_4x4<MatrixType, 0, 3>(matrix);
+    result.coeffRef(0, 2) = cofactor_4x4<MatrixType, 2, 0>(matrix);
+    result.coeffRef(1, 2) = -cofactor_4x4<MatrixType, 2, 1>(matrix);
+    result.coeffRef(2, 2) = cofactor_4x4<MatrixType, 2, 2>(matrix);
+    result.coeffRef(3, 2) = -cofactor_4x4<MatrixType, 2, 3>(matrix);
+    result.coeffRef(0, 1) = -cofactor_4x4<MatrixType, 1, 0>(matrix);
+    result.coeffRef(1, 1) = cofactor_4x4<MatrixType, 1, 1>(matrix);
+    result.coeffRef(2, 1) = -cofactor_4x4<MatrixType, 1, 2>(matrix);
+    result.coeffRef(3, 1) = cofactor_4x4<MatrixType, 1, 3>(matrix);
+    result.coeffRef(0, 3) = -cofactor_4x4<MatrixType, 3, 0>(matrix);
+    result.coeffRef(1, 3) = cofactor_4x4<MatrixType, 3, 1>(matrix);
+    result.coeffRef(2, 3) = -cofactor_4x4<MatrixType, 3, 2>(matrix);
+    result.coeffRef(3, 3) = cofactor_4x4<MatrixType, 3, 3>(matrix);
     result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
   }
 };
 
-template<typename MatrixType, typename ResultType>
+template <typename MatrixType, typename ResultType>
 struct compute_inverse<MatrixType, ResultType, 4>
- : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
-                            MatrixType, ResultType>
-{
-};
+    : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar, MatrixType, ResultType> {};
 
-template<typename MatrixType, typename ResultType>
-struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(
-    const MatrixType& matrix,
-    const typename MatrixType::RealScalar& absDeterminantThreshold,
-    ResultType& inverse,
-    typename ResultType::Scalar& determinant,
-    bool& invertible
-  )
-  {
+template <typename MatrixType, typename ResultType>
+struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4> {
+  EIGEN_DEVICE_FUNC static inline void run(const MatrixType& matrix,
+                                           const typename MatrixType::RealScalar& absDeterminantThreshold,
+                                           ResultType& inverse, typename ResultType::Scalar& determinant,
+                                           bool& invertible) {
     using std::abs;
     determinant = matrix.determinant();
     invertible = abs(determinant) > absDeterminantThreshold;
-    if(invertible && extract_data(matrix) != extract_data(inverse)) {
+    if (invertible && extract_data(matrix) != extract_data(inverse)) {
       compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
-    }
-    else if(invertible) {
+    } else if (invertible) {
       MatrixType matrix_t = matrix;
       compute_inverse<MatrixType, ResultType>::run(matrix_t, inverse);
     }
@@ -292,141 +227,127 @@
 *** MatrixBase methods ***
 *************************/
 
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
 // Specialization for "dense = dense_xpr.inverse()"
-template<typename DstXprType, typename XprType>
-struct Assignment<DstXprType, Inverse<XprType>, internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar>, Dense2Dense>
-{
+template <typename DstXprType, typename XprType>
+struct Assignment<DstXprType, Inverse<XprType>,
+                  internal::assign_op<typename DstXprType::Scalar, typename XprType::Scalar>, Dense2Dense> {
   typedef Inverse<XprType> SrcXprType;
-  EIGEN_DEVICE_FUNC
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar> &)
-  {
+  EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src,
+                                    const internal::assign_op<typename DstXprType::Scalar, typename XprType::Scalar>&) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
-    
-    const int Size = EIGEN_PLAIN_ENUM_MIN(XprType::ColsAtCompileTime,DstXprType::ColsAtCompileTime);
-    EIGEN_ONLY_USED_FOR_DEBUG(Size);
-    eigen_assert(( (Size<=1) || (Size>4) || (extract_data(src.nestedExpression())!=extract_data(dst)))
-              && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
-    typedef typename internal::nested_eval<XprType,XprType::ColsAtCompileTime>::type  ActualXprType;
-    typedef typename internal::remove_all<ActualXprType>::type                        ActualXprTypeCleanded;
-    
+    const int Size = plain_enum_min(XprType::ColsAtCompileTime, DstXprType::ColsAtCompileTime);
+    EIGEN_ONLY_USED_FOR_DEBUG(Size);
+    eigen_assert(((Size <= 1) || (Size > 4) || (extract_data(src.nestedExpression()) != extract_data(dst))) &&
+                 "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
+
+    typedef typename internal::nested_eval<XprType, XprType::ColsAtCompileTime>::type ActualXprType;
+    typedef internal::remove_all_t<ActualXprType> ActualXprTypeCleanded;
+
     ActualXprType actual_xpr(src.nestedExpression());
-    
+
     compute_inverse<ActualXprTypeCleanded, DstXprType>::run(actual_xpr, dst);
   }
 };
 
-  
-} // end namespace internal
+}  // end namespace internal
 
 /** \lu_module
-  *
-  * \returns the matrix inverse of this matrix.
-  *
-  * For small fixed sizes up to 4x4, this method uses cofactors.
-  * In the general case, this method uses class PartialPivLU.
-  *
-  * \note This matrix must be invertible, otherwise the result is undefined. If you need an
-  * invertibility check, do the following:
-  * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck().
-  * \li for the general case, use class FullPivLU.
-  *
-  * Example: \include MatrixBase_inverse.cpp
-  * Output: \verbinclude MatrixBase_inverse.out
-  *
-  * \sa computeInverseAndDetWithCheck()
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC
-inline const Inverse<Derived> MatrixBase<Derived>::inverse() const
-{
-  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
+ *
+ * \returns the matrix inverse of this matrix.
+ *
+ * For small fixed sizes up to 4x4, this method uses cofactors.
+ * In the general case, this method uses class PartialPivLU.
+ *
+ * \note This matrix must be invertible, otherwise the result is undefined. If you need an
+ * invertibility check, do the following:
+ * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck().
+ * \li for the general case, use class FullPivLU.
+ *
+ * Example: \include MatrixBase_inverse.cpp
+ * Output: \verbinclude MatrixBase_inverse.out
+ *
+ * \sa computeInverseAndDetWithCheck()
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline const Inverse<Derived> MatrixBase<Derived>::inverse() const {
+  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger, THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
   eigen_assert(rows() == cols());
   return Inverse<Derived>(derived());
 }
 
 /** \lu_module
-  *
-  * Computation of matrix inverse and determinant, with invertibility check.
-  *
-  * This is only for fixed-size square matrices of size up to 4x4.
-  *
-  * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
-  *
-  * \param inverse Reference to the matrix in which to store the inverse.
-  * \param determinant Reference to the variable in which to store the determinant.
-  * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
-  * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
-  *                                The matrix will be declared invertible if the absolute value of its
-  *                                determinant is greater than this threshold.
-  *
-  * Example: \include MatrixBase_computeInverseAndDetWithCheck.cpp
-  * Output: \verbinclude MatrixBase_computeInverseAndDetWithCheck.out
-  *
-  * \sa inverse(), computeInverseWithCheck()
-  */
-template<typename Derived>
-template<typename ResultType>
-inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(
-    ResultType& inverse,
-    typename ResultType::Scalar& determinant,
-    bool& invertible,
-    const RealScalar& absDeterminantThreshold
-  ) const
-{
+ *
+ * Computation of matrix inverse and determinant, with invertibility check.
+ *
+ * This is only for fixed-size square matrices of size up to 4x4.
+ *
+ * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
+ *
+ * \param inverse Reference to the matrix in which to store the inverse.
+ * \param determinant Reference to the variable in which to store the determinant.
+ * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
+ * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
+ *                                The matrix will be declared invertible if the absolute value of its
+ *                                determinant is greater than this threshold.
+ *
+ * Example: \include MatrixBase_computeInverseAndDetWithCheck.cpp
+ * Output: \verbinclude MatrixBase_computeInverseAndDetWithCheck.out
+ *
+ * \sa inverse(), computeInverseWithCheck()
+ */
+template <typename Derived>
+template <typename ResultType>
+inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(ResultType& inverse,
+                                                               typename ResultType::Scalar& determinant,
+                                                               bool& invertible,
+                                                               const RealScalar& absDeterminantThreshold) const {
   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
   eigen_assert(rows() == cols());
   // for 2x2, it's worth giving a chance to avoid evaluating.
   // for larger sizes, evaluating has negligible cost and limits code size.
-  typedef typename internal::conditional<
-    RowsAtCompileTime == 2,
-    typename internal::remove_all<typename internal::nested_eval<Derived, 2>::type>::type,
-    PlainObject
-  >::type MatrixType;
-  internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
-    (derived(), absDeterminantThreshold, inverse, determinant, invertible);
+  typedef std::conditional_t<RowsAtCompileTime == 2,
+                             internal::remove_all_t<typename internal::nested_eval<Derived, 2>::type>, PlainObject>
+      MatrixType;
+  internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run(derived(), absDeterminantThreshold, inverse,
+                                                                            determinant, invertible);
 }
 
 /** \lu_module
-  *
-  * Computation of matrix inverse, with invertibility check.
-  *
-  * This is only for fixed-size square matrices of size up to 4x4.
-  *
-  * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
-  *
-  * \param inverse Reference to the matrix in which to store the inverse.
-  * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
-  * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
-  *                                The matrix will be declared invertible if the absolute value of its
-  *                                determinant is greater than this threshold.
-  *
-  * Example: \include MatrixBase_computeInverseWithCheck.cpp
-  * Output: \verbinclude MatrixBase_computeInverseWithCheck.out
-  *
-  * \sa inverse(), computeInverseAndDetWithCheck()
-  */
-template<typename Derived>
-template<typename ResultType>
-inline void MatrixBase<Derived>::computeInverseWithCheck(
-    ResultType& inverse,
-    bool& invertible,
-    const RealScalar& absDeterminantThreshold
-  ) const
-{
+ *
+ * Computation of matrix inverse, with invertibility check.
+ *
+ * This is only for fixed-size square matrices of size up to 4x4.
+ *
+ * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
+ *
+ * \param inverse Reference to the matrix in which to store the inverse.
+ * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
+ * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
+ *                                The matrix will be declared invertible if the absolute value of its
+ *                                determinant is greater than this threshold.
+ *
+ * Example: \include MatrixBase_computeInverseWithCheck.cpp
+ * Output: \verbinclude MatrixBase_computeInverseWithCheck.out
+ *
+ * \sa inverse(), computeInverseAndDetWithCheck()
+ */
+template <typename Derived>
+template <typename ResultType>
+inline void MatrixBase<Derived>::computeInverseWithCheck(ResultType& inverse, bool& invertible,
+                                                         const RealScalar& absDeterminantThreshold) const {
   Scalar determinant;
   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
   eigen_assert(rows() == cols());
-  computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
+  computeInverseAndDetWithCheck(inverse, determinant, invertible, absDeterminantThreshold);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_INVERSE_IMPL_H
+#endif  // EIGEN_INVERSE_IMPL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h
index 34aed72..1edd6b8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h
@@ -11,431 +11,391 @@
 #ifndef EIGEN_PARTIALLU_H
 #define EIGEN_PARTIALLU_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename _MatrixType> struct traits<PartialPivLU<_MatrixType> >
- : traits<_MatrixType>
-{
+template <typename MatrixType_, typename PermutationIndex_>
+struct traits<PartialPivLU<MatrixType_, PermutationIndex_> > : traits<MatrixType_> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
-  typedef int StorageIndex;
-  typedef traits<_MatrixType> BaseTraits;
-  enum {
-    Flags = BaseTraits::Flags & RowMajorBit,
-    CoeffReadCost = Dynamic
-  };
+  typedef PermutationIndex_ StorageIndex;
+  typedef traits<MatrixType_> BaseTraits;
+  enum { Flags = BaseTraits::Flags & RowMajorBit, CoeffReadCost = Dynamic };
 };
 
-template<typename T,typename Derived>
+template <typename T, typename Derived>
 struct enable_if_ref;
 // {
 //   typedef Derived type;
 // };
 
-template<typename T,typename Derived>
-struct enable_if_ref<Ref<T>,Derived> {
+template <typename T, typename Derived>
+struct enable_if_ref<Ref<T>, Derived> {
   typedef Derived type;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup LU_Module
-  *
-  * \class PartialPivLU
-  *
-  * \brief LU decomposition of a matrix with partial pivoting, and related features
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the LU decomposition
-  *
-  * This class represents a LU decomposition of a \b square \b invertible matrix, with partial pivoting: the matrix A
-  * is decomposed as A = PLU where L is unit-lower-triangular, U is upper-triangular, and P
-  * is a permutation matrix.
-  *
-  * Typically, partial pivoting LU decomposition is only considered numerically stable for square invertible
-  * matrices. Thus LAPACK's dgesv and dgesvx require the matrix to be square and invertible. The present class
-  * does the same. It will assert that the matrix is square, but it won't (actually it can't) check that the
-  * matrix is invertible: it is your task to check that you only use this decomposition on invertible matrices.
-  *
-  * The guaranteed safe alternative, working for all matrices, is the full pivoting LU decomposition, provided
-  * by class FullPivLU.
-  *
-  * This is \b not a rank-revealing LU decomposition. Many features are intentionally absent from this class,
-  * such as rank computation. If you need these features, use class FullPivLU.
-  *
-  * This LU decomposition is suitable to invert invertible matrices. It is what MatrixBase::inverse() uses
-  * in the general case.
-  * On the other hand, it is \b not suitable to determine whether a given matrix is invertible.
-  *
-  * The data of the LU decomposition can be directly accessed through the methods matrixLU(), permutationP().
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  *
-  * \sa MatrixBase::partialPivLu(), MatrixBase::determinant(), MatrixBase::inverse(), MatrixBase::computeInverse(), class FullPivLU
-  */
-template<typename _MatrixType> class PartialPivLU
-  : public SolverBase<PartialPivLU<_MatrixType> >
-{
-  public:
+ *
+ * \class PartialPivLU
+ *
+ * \brief LU decomposition of a matrix with partial pivoting, and related features
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the LU decomposition
+ *
+ * This class represents a LU decomposition of a \b square \b invertible matrix, with partial pivoting: the matrix A
+ * is decomposed as A = PLU where L is unit-lower-triangular, U is upper-triangular, and P
+ * is a permutation matrix.
+ *
+ * Typically, partial pivoting LU decomposition is only considered numerically stable for square invertible
+ * matrices. Thus LAPACK's dgesv and dgesvx require the matrix to be square and invertible. The present class
+ * does the same. It will assert that the matrix is square, but it won't (actually it can't) check that the
+ * matrix is invertible: it is your task to check that you only use this decomposition on invertible matrices.
+ *
+ * The guaranteed safe alternative, working for all matrices, is the full pivoting LU decomposition, provided
+ * by class FullPivLU.
+ *
+ * This is \b not a rank-revealing LU decomposition. Many features are intentionally absent from this class,
+ * such as rank computation. If you need these features, use class FullPivLU.
+ *
+ * This LU decomposition is suitable to invert invertible matrices. It is what MatrixBase::inverse() uses
+ * in the general case.
+ * On the other hand, it is \b not suitable to determine whether a given matrix is invertible.
+ *
+ * The data of the LU decomposition can be directly accessed through the methods matrixLU(), permutationP().
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * \sa MatrixBase::partialPivLu(), MatrixBase::determinant(), MatrixBase::inverse(), MatrixBase::computeInverse(), class
+ * FullPivLU
+ */
+template <typename MatrixType_, typename PermutationIndex_>
+class PartialPivLU : public SolverBase<PartialPivLU<MatrixType_, PermutationIndex_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef SolverBase<PartialPivLU> Base;
+  friend class SolverBase<PartialPivLU>;
 
-    typedef _MatrixType MatrixType;
-    typedef SolverBase<PartialPivLU> Base;
-    friend class SolverBase<PartialPivLU>;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(PartialPivLU)
+  enum {
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  using PermutationIndex = PermutationIndex_;
+  typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime, PermutationIndex> PermutationType;
+  typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime, PermutationIndex> TranspositionType;
+  typedef typename MatrixType::PlainObject PlainObject;
 
-    EIGEN_GENERIC_PUBLIC_INTERFACE(PartialPivLU)
-    enum {
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
-    typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
-    typedef typename MatrixType::PlainObject PlainObject;
+  /**
+   * \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via PartialPivLU::compute(const MatrixType&).
+   */
+  PartialPivLU();
 
-    /**
-      * \brief Default Constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via PartialPivLU::compute(const MatrixType&).
-      */
-    PartialPivLU();
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa PartialPivLU()
+   */
+  explicit PartialPivLU(Index size);
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa PartialPivLU()
-      */
-    explicit PartialPivLU(Index size);
+  /** Constructor.
+   *
+   * \param matrix the matrix of which to compute the LU decomposition.
+   *
+   * \warning The matrix should have full rank (e.g. if it's square, it should be invertible).
+   * If you need to deal with non-full rank, use class FullPivLU instead.
+   */
+  template <typename InputType>
+  explicit PartialPivLU(const EigenBase<InputType>& matrix);
 
-    /** Constructor.
-      *
-      * \param matrix the matrix of which to compute the LU decomposition.
-      *
-      * \warning The matrix should have full rank (e.g. if it's square, it should be invertible).
-      * If you need to deal with non-full rank, use class FullPivLU instead.
-      */
-    template<typename InputType>
-    explicit PartialPivLU(const EigenBase<InputType>& matrix);
+  /** Constructor for \link InplaceDecomposition inplace decomposition \endlink
+   *
+   * \param matrix the matrix of which to compute the LU decomposition.
+   *
+   * \warning The matrix should have full rank (e.g. if it's square, it should be invertible).
+   * If you need to deal with non-full rank, use class FullPivLU instead.
+   */
+  template <typename InputType>
+  explicit PartialPivLU(EigenBase<InputType>& matrix);
 
-    /** Constructor for \link InplaceDecomposition inplace decomposition \endlink
-      *
-      * \param matrix the matrix of which to compute the LU decomposition.
-      *
-      * \warning The matrix should have full rank (e.g. if it's square, it should be invertible).
-      * If you need to deal with non-full rank, use class FullPivLU instead.
-      */
-    template<typename InputType>
-    explicit PartialPivLU(EigenBase<InputType>& matrix);
+  template <typename InputType>
+  PartialPivLU& compute(const EigenBase<InputType>& matrix) {
+    m_lu = matrix.derived();
+    compute();
+    return *this;
+  }
 
-    template<typename InputType>
-    PartialPivLU& compute(const EigenBase<InputType>& matrix) {
-      m_lu = matrix.derived();
-      compute();
-      return *this;
-    }
+  /** \returns the LU decomposition matrix: the upper-triangular part is U, the
+   * unit-lower-triangular part is L (at least for square matrices; in the non-square
+   * case, special care is needed, see the documentation of class FullPivLU).
+   *
+   * \sa matrixL(), matrixU()
+   */
+  inline const MatrixType& matrixLU() const {
+    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
+    return m_lu;
+  }
 
-    /** \returns the LU decomposition matrix: the upper-triangular part is U, the
-      * unit-lower-triangular part is L (at least for square matrices; in the non-square
-      * case, special care is needed, see the documentation of class FullPivLU).
-      *
-      * \sa matrixL(), matrixU()
-      */
-    inline const MatrixType& matrixLU() const
-    {
-      eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
-      return m_lu;
-    }
+  /** \returns the permutation matrix P.
+   */
+  inline const PermutationType& permutationP() const {
+    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
+    return m_p;
+  }
 
-    /** \returns the permutation matrix P.
-      */
-    inline const PermutationType& permutationP() const
-    {
-      eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
-      return m_p;
-    }
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** This method returns the solution x to the equation Ax=b, where A is the matrix of which
+   * *this is the LU decomposition.
+   *
+   * \param b the right-hand-side of the equation to solve. Can be a vector or a matrix,
+   *          the only requirement in order for the equation to make sense is that
+   *          b.rows()==A.rows(), where A is the matrix of which *this is the LU decomposition.
+   *
+   * \returns the solution.
+   *
+   * Example: \include PartialPivLU_solve.cpp
+   * Output: \verbinclude PartialPivLU_solve.out
+   *
+   * Since this PartialPivLU class assumes anyway that the matrix A is invertible, the solution
+   * theoretically exists and is unique regardless of b.
+   *
+   * \sa TriangularView::solve(), inverse(), computeInverse()
+   */
+  template <typename Rhs>
+  inline const Solve<PartialPivLU, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** This method returns the solution x to the equation Ax=b, where A is the matrix of which
-      * *this is the LU decomposition.
-      *
-      * \param b the right-hand-side of the equation to solve. Can be a vector or a matrix,
-      *          the only requirement in order for the equation to make sense is that
-      *          b.rows()==A.rows(), where A is the matrix of which *this is the LU decomposition.
-      *
-      * \returns the solution.
-      *
-      * Example: \include PartialPivLU_solve.cpp
-      * Output: \verbinclude PartialPivLU_solve.out
-      *
-      * Since this PartialPivLU class assumes anyway that the matrix A is invertible, the solution
-      * theoretically exists and is unique regardless of b.
-      *
-      * \sa TriangularView::solve(), inverse(), computeInverse()
-      */
-    template<typename Rhs>
-    inline const Solve<PartialPivLU, Rhs>
-    solve(const MatrixBase<Rhs>& b) const;
-    #endif
+  /** \returns an estimate of the reciprocal condition number of the matrix of which \c *this is
+      the LU decomposition.
+    */
+  inline RealScalar rcond() const {
+    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
+    return internal::rcond_estimate_helper(m_l1_norm, *this);
+  }
 
-    /** \returns an estimate of the reciprocal condition number of the matrix of which \c *this is
-        the LU decomposition.
-      */
-    inline RealScalar rcond() const
-    {
-      eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
-      return internal::rcond_estimate_helper(m_l1_norm, *this);
-    }
+  /** \returns the inverse of the matrix of which *this is the LU decomposition.
+   *
+   * \warning The matrix being decomposed here is assumed to be invertible. If you need to check for
+   *          invertibility, use class FullPivLU instead.
+   *
+   * \sa MatrixBase::inverse(), LU::inverse()
+   */
+  inline const Inverse<PartialPivLU> inverse() const {
+    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
+    return Inverse<PartialPivLU>(*this);
+  }
 
-    /** \returns the inverse of the matrix of which *this is the LU decomposition.
-      *
-      * \warning The matrix being decomposed here is assumed to be invertible. If you need to check for
-      *          invertibility, use class FullPivLU instead.
-      *
-      * \sa MatrixBase::inverse(), LU::inverse()
-      */
-    inline const Inverse<PartialPivLU> inverse() const
-    {
-      eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
-      return Inverse<PartialPivLU>(*this);
-    }
+  /** \returns the determinant of the matrix of which
+   * *this is the LU decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the LU decomposition has already been computed.
+   *
+   * \note For fixed-size matrices of size up to 4, MatrixBase::determinant() offers
+   *       optimized paths.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   *
+   * \sa MatrixBase::determinant()
+   */
+  Scalar determinant() const;
 
-    /** \returns the determinant of the matrix of which
-      * *this is the LU decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the LU decomposition has already been computed.
-      *
-      * \note For fixed-size matrices of size up to 4, MatrixBase::determinant() offers
-      *       optimized paths.
-      *
-      * \warning a determinant can be very big or small, so for matrices
-      * of large enough dimension, there is a risk of overflow/underflow.
-      *
-      * \sa MatrixBase::determinant()
-      */
-    Scalar determinant() const;
+  MatrixType reconstructedMatrix() const;
 
-    MatrixType reconstructedMatrix() const;
+  EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
+  EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
 
-    EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
-    EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  EIGEN_DEVICE_FUNC void _solve_impl(const RhsType& rhs, DstType& dst) const {
+    /* The decomposition PA = LU can be rewritten as A = P^{-1} L U.
+     * So we proceed as follows:
+     * Step 1: compute c = Pb.
+     * Step 2: replace c by the solution x to Lx = c.
+     * Step 3: replace c by the solution x to Ux = c.
+     */
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
-    void _solve_impl(const RhsType &rhs, DstType &dst) const {
-     /* The decomposition PA = LU can be rewritten as A = P^{-1} L U.
-      * So we proceed as follows:
-      * Step 1: compute c = Pb.
-      * Step 2: replace c by the solution x to Lx = c.
-      * Step 3: replace c by the solution x to Ux = c.
-      */
+    // Step 1
+    dst = permutationP() * rhs;
 
-      // Step 1
-      dst = permutationP() * rhs;
+    // Step 2
+    m_lu.template triangularView<UnitLower>().solveInPlace(dst);
 
-      // Step 2
-      m_lu.template triangularView<UnitLower>().solveInPlace(dst);
+    // Step 3
+    m_lu.template triangularView<Upper>().solveInPlace(dst);
+  }
 
-      // Step 3
-      m_lu.template triangularView<Upper>().solveInPlace(dst);
-    }
+  template <bool Conjugate, typename RhsType, typename DstType>
+  EIGEN_DEVICE_FUNC void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const {
+    /* The decomposition PA = LU can be rewritten as A^T = U^T L^T P.
+     * So we proceed as follows:
+     * Step 1: compute c as the solution to L^T c = b
+     * Step 2: replace c by the solution x to U^T x = c.
+     * Step 3: update  c = P^-1 c.
+     */
 
-    template<bool Conjugate, typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
-    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const {
-     /* The decomposition PA = LU can be rewritten as A^T = U^T L^T P.
-      * So we proceed as follows:
-      * Step 1: compute c as the solution to L^T c = b
-      * Step 2: replace c by the solution x to U^T x = c.
-      * Step 3: update  c = P^-1 c.
-      */
+    eigen_assert(rhs.rows() == m_lu.cols());
 
-      eigen_assert(rhs.rows() == m_lu.cols());
+    // Step 1
+    dst = m_lu.template triangularView<Upper>().transpose().template conjugateIf<Conjugate>().solve(rhs);
+    // Step 2
+    m_lu.template triangularView<UnitLower>().transpose().template conjugateIf<Conjugate>().solveInPlace(dst);
+    // Step 3
+    dst = permutationP().transpose() * dst;
+  }
+#endif
 
-      // Step 1
-      dst = m_lu.template triangularView<Upper>().transpose()
-                .template conjugateIf<Conjugate>().solve(rhs);
-      // Step 2
-      m_lu.template triangularView<UnitLower>().transpose()
-          .template conjugateIf<Conjugate>().solveInPlace(dst);
-      // Step 3
-      dst = permutationP().transpose() * dst;
-    }
-    #endif
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-  protected:
+  void compute();
 
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
-
-    void compute();
-
-    MatrixType m_lu;
-    PermutationType m_p;
-    TranspositionType m_rowsTranspositions;
-    RealScalar m_l1_norm;
-    signed char m_det_p;
-    bool m_isInitialized;
+  MatrixType m_lu;
+  PermutationType m_p;
+  TranspositionType m_rowsTranspositions;
+  RealScalar m_l1_norm;
+  signed char m_det_p;
+  bool m_isInitialized;
 };
 
-template<typename MatrixType>
-PartialPivLU<MatrixType>::PartialPivLU()
-  : m_lu(),
-    m_p(),
-    m_rowsTranspositions(),
-    m_l1_norm(0),
-    m_det_p(0),
-    m_isInitialized(false)
-{
-}
+template <typename MatrixType, typename PermutationIndex>
+PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU()
+    : m_lu(), m_p(), m_rowsTranspositions(), m_l1_norm(0), m_det_p(0), m_isInitialized(false) {}
 
-template<typename MatrixType>
-PartialPivLU<MatrixType>::PartialPivLU(Index size)
-  : m_lu(size, size),
-    m_p(size),
-    m_rowsTranspositions(size),
-    m_l1_norm(0),
-    m_det_p(0),
-    m_isInitialized(false)
-{
-}
+template <typename MatrixType, typename PermutationIndex>
+PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU(Index size)
+    : m_lu(size, size), m_p(size), m_rowsTranspositions(size), m_l1_norm(0), m_det_p(0), m_isInitialized(false) {}
 
-template<typename MatrixType>
-template<typename InputType>
-PartialPivLU<MatrixType>::PartialPivLU(const EigenBase<InputType>& matrix)
-  : m_lu(matrix.rows(),matrix.cols()),
-    m_p(matrix.rows()),
-    m_rowsTranspositions(matrix.rows()),
-    m_l1_norm(0),
-    m_det_p(0),
-    m_isInitialized(false)
-{
+template <typename MatrixType, typename PermutationIndex>
+template <typename InputType>
+PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU(const EigenBase<InputType>& matrix)
+    : m_lu(matrix.rows(), matrix.cols()),
+      m_p(matrix.rows()),
+      m_rowsTranspositions(matrix.rows()),
+      m_l1_norm(0),
+      m_det_p(0),
+      m_isInitialized(false) {
   compute(matrix.derived());
 }
 
-template<typename MatrixType>
-template<typename InputType>
-PartialPivLU<MatrixType>::PartialPivLU(EigenBase<InputType>& matrix)
-  : m_lu(matrix.derived()),
-    m_p(matrix.rows()),
-    m_rowsTranspositions(matrix.rows()),
-    m_l1_norm(0),
-    m_det_p(0),
-    m_isInitialized(false)
-{
+template <typename MatrixType, typename PermutationIndex>
+template <typename InputType>
+PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU(EigenBase<InputType>& matrix)
+    : m_lu(matrix.derived()),
+      m_p(matrix.rows()),
+      m_rowsTranspositions(matrix.rows()),
+      m_l1_norm(0),
+      m_det_p(0),
+      m_isInitialized(false) {
   compute();
 }
 
 namespace internal {
 
 /** \internal This is the blocked version of fullpivlu_unblocked() */
-template<typename Scalar, int StorageOrder, typename PivIndex, int SizeAtCompileTime=Dynamic>
-struct partial_lu_impl
-{
-  static const int UnBlockedBound = 16;
-  static const bool UnBlockedAtCompileTime = SizeAtCompileTime!=Dynamic && SizeAtCompileTime<=UnBlockedBound;
-  static const int ActualSizeAtCompileTime = UnBlockedAtCompileTime ? SizeAtCompileTime : Dynamic;
+template <typename Scalar, int StorageOrder, typename PivIndex, int SizeAtCompileTime = Dynamic>
+struct partial_lu_impl {
+  static constexpr int UnBlockedBound = 16;
+  static constexpr bool UnBlockedAtCompileTime = SizeAtCompileTime != Dynamic && SizeAtCompileTime <= UnBlockedBound;
+  static constexpr int ActualSizeAtCompileTime = UnBlockedAtCompileTime ? SizeAtCompileTime : Dynamic;
   // Remaining rows and columns at compile-time:
-  static const int RRows = SizeAtCompileTime==2 ? 1 : Dynamic;
-  static const int RCols = SizeAtCompileTime==2 ? 1 : Dynamic;
+  static constexpr int RRows = SizeAtCompileTime == 2 ? 1 : Dynamic;
+  static constexpr int RCols = SizeAtCompileTime == 2 ? 1 : Dynamic;
   typedef Matrix<Scalar, ActualSizeAtCompileTime, ActualSizeAtCompileTime, StorageOrder> MatrixType;
   typedef Ref<MatrixType> MatrixTypeRef;
   typedef Ref<Matrix<Scalar, Dynamic, Dynamic, StorageOrder> > BlockType;
   typedef typename MatrixType::RealScalar RealScalar;
 
   /** \internal performs the LU decomposition in-place of the matrix \a lu
-    * using an unblocked algorithm.
-    *
-    * In addition, this function returns the row transpositions in the
-    * vector \a row_transpositions which must have a size equal to the number
-    * of columns of the matrix \a lu, and an integer \a nb_transpositions
-    * which returns the actual number of transpositions.
-    *
-    * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise.
-    */
-  static Index unblocked_lu(MatrixTypeRef& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions)
-  {
+   * using an unblocked algorithm.
+   *
+   * In addition, this function returns the row transpositions in the
+   * vector \a row_transpositions which must have a size equal to the number
+   * of columns of the matrix \a lu, and an integer \a nb_transpositions
+   * which returns the actual number of transpositions.
+   *
+   * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise.
+   */
+  static Index unblocked_lu(MatrixTypeRef& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions) {
     typedef scalar_score_coeff_op<Scalar> Scoring;
     typedef typename Scoring::result_type Score;
     const Index rows = lu.rows();
     const Index cols = lu.cols();
-    const Index size = (std::min)(rows,cols);
+    const Index size = (std::min)(rows, cols);
     // For small compile-time matrices it is worth processing the last row separately:
     //  speedup: +100% for 2x2, +10% for others.
-    const Index endk = UnBlockedAtCompileTime ? size-1 : size;
+    const Index endk = UnBlockedAtCompileTime ? size - 1 : size;
     nb_transpositions = 0;
     Index first_zero_pivot = -1;
-    for(Index k = 0; k < endk; ++k)
-    {
-      int rrows = internal::convert_index<int>(rows-k-1);
-      int rcols = internal::convert_index<int>(cols-k-1);
+    for (Index k = 0; k < endk; ++k) {
+      int rrows = internal::convert_index<int>(rows - k - 1);
+      int rcols = internal::convert_index<int>(cols - k - 1);
 
       Index row_of_biggest_in_col;
-      Score biggest_in_corner
-        = lu.col(k).tail(rows-k).unaryExpr(Scoring()).maxCoeff(&row_of_biggest_in_col);
+      Score biggest_in_corner = lu.col(k).tail(rows - k).unaryExpr(Scoring()).maxCoeff(&row_of_biggest_in_col);
       row_of_biggest_in_col += k;
 
       row_transpositions[k] = PivIndex(row_of_biggest_in_col);
 
-      if(biggest_in_corner != Score(0))
-      {
-        if(k != row_of_biggest_in_col)
-        {
+      if (!numext::is_exactly_zero(biggest_in_corner)) {
+        if (k != row_of_biggest_in_col) {
           lu.row(k).swap(lu.row(row_of_biggest_in_col));
           ++nb_transpositions;
         }
 
-        lu.col(k).tail(fix<RRows>(rrows)) /= lu.coeff(k,k);
-      }
-      else if(first_zero_pivot==-1)
-      {
+        lu.col(k).tail(fix<RRows>(rrows)) /= lu.coeff(k, k);
+      } else if (first_zero_pivot == -1) {
         // the pivot is exactly zero, we record the index of the first pivot which is exactly 0,
         // and continue the factorization such we still have A = PLU
         first_zero_pivot = k;
       }
 
-      if(k<rows-1)
-        lu.bottomRightCorner(fix<RRows>(rrows),fix<RCols>(rcols)).noalias() -= lu.col(k).tail(fix<RRows>(rrows)) * lu.row(k).tail(fix<RCols>(rcols));
+      if (k < rows - 1)
+        lu.bottomRightCorner(fix<RRows>(rrows), fix<RCols>(rcols)).noalias() -=
+            lu.col(k).tail(fix<RRows>(rrows)) * lu.row(k).tail(fix<RCols>(rcols));
     }
 
     // special handling of the last entry
-    if(UnBlockedAtCompileTime)
-    {
+    if (UnBlockedAtCompileTime) {
       Index k = endk;
       row_transpositions[k] = PivIndex(k);
-      if (Scoring()(lu(k, k)) == Score(0) && first_zero_pivot == -1)
-        first_zero_pivot = k;
+      if (numext::is_exactly_zero(Scoring()(lu(k, k))) && first_zero_pivot == -1) first_zero_pivot = k;
     }
 
     return first_zero_pivot;
   }
 
   /** \internal performs the LU decomposition in-place of the matrix represented
-    * by the variables \a rows, \a cols, \a lu_data, and \a lu_stride using a
-    * recursive, blocked algorithm.
-    *
-    * In addition, this function returns the row transpositions in the
-    * vector \a row_transpositions which must have a size equal to the number
-    * of columns of the matrix \a lu, and an integer \a nb_transpositions
-    * which returns the actual number of transpositions.
-    *
-    * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise.
-    *
-    * \note This very low level interface using pointers, etc. is to:
-    *   1 - reduce the number of instantiations to the strict minimum
-    *   2 - avoid infinite recursion of the instantiations with Block<Block<Block<...> > >
-    */
-  static Index blocked_lu(Index rows, Index cols, Scalar* lu_data, Index luStride, PivIndex* row_transpositions, PivIndex& nb_transpositions, Index maxBlockSize=256)
-  {
-    MatrixTypeRef lu = MatrixType::Map(lu_data,rows, cols, OuterStride<>(luStride));
+   * by the variables \a rows, \a cols, \a lu_data, and \a lu_stride using a
+   * recursive, blocked algorithm.
+   *
+   * In addition, this function returns the row transpositions in the
+   * vector \a row_transpositions which must have a size equal to the number
+   * of columns of the matrix \a lu, and an integer \a nb_transpositions
+   * which returns the actual number of transpositions.
+   *
+   * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise.
+   *
+   * \note This very low level interface using pointers, etc. is to:
+   *   1 - reduce the number of instantiations to the strict minimum
+   *   2 - avoid infinite recursion of the instantiations with Block<Block<Block<...> > >
+   */
+  static Index blocked_lu(Index rows, Index cols, Scalar* lu_data, Index luStride, PivIndex* row_transpositions,
+                          PivIndex& nb_transpositions, Index maxBlockSize = 256) {
+    MatrixTypeRef lu = MatrixType::Map(lu_data, rows, cols, OuterStride<>(luStride));
 
-    const Index size = (std::min)(rows,cols);
+    const Index size = (std::min)(rows, cols);
 
     // if the matrix is too small, no blocking:
-    if(UnBlockedAtCompileTime || size<=UnBlockedBound)
-    {
+    if (UnBlockedAtCompileTime || size <= UnBlockedBound) {
       return unblocked_lu(lu, row_transpositions, nb_transpositions);
     }
 
@@ -443,51 +403,46 @@
     // of the matrix so that there is enough sub blocks:
     Index blockSize;
     {
-      blockSize = size/8;
-      blockSize = (blockSize/16)*16;
-      blockSize = (std::min)((std::max)(blockSize,Index(8)), maxBlockSize);
+      blockSize = size / 8;
+      blockSize = (blockSize / 16) * 16;
+      blockSize = (std::min)((std::max)(blockSize, Index(8)), maxBlockSize);
     }
 
     nb_transpositions = 0;
     Index first_zero_pivot = -1;
-    for(Index k = 0; k < size; k+=blockSize)
-    {
-      Index bs = (std::min)(size-k,blockSize); // actual size of the block
-      Index trows = rows - k - bs; // trailing rows
-      Index tsize = size - k - bs; // trailing size
+    for (Index k = 0; k < size; k += blockSize) {
+      Index bs = (std::min)(size - k, blockSize);  // actual size of the block
+      Index trows = rows - k - bs;                 // trailing rows
+      Index tsize = size - k - bs;                 // trailing size
 
       // partition the matrix:
       //                          A00 | A01 | A02
       // lu  = A_0 | A_1 | A_2 =  A10 | A11 | A12
       //                          A20 | A21 | A22
-      BlockType A_0 = lu.block(0,0,rows,k);
-      BlockType A_2 = lu.block(0,k+bs,rows,tsize);
-      BlockType A11 = lu.block(k,k,bs,bs);
-      BlockType A12 = lu.block(k,k+bs,bs,tsize);
-      BlockType A21 = lu.block(k+bs,k,trows,bs);
-      BlockType A22 = lu.block(k+bs,k+bs,trows,tsize);
+      BlockType A_0 = lu.block(0, 0, rows, k);
+      BlockType A_2 = lu.block(0, k + bs, rows, tsize);
+      BlockType A11 = lu.block(k, k, bs, bs);
+      BlockType A12 = lu.block(k, k + bs, bs, tsize);
+      BlockType A21 = lu.block(k + bs, k, trows, bs);
+      BlockType A22 = lu.block(k + bs, k + bs, trows, tsize);
 
       PivIndex nb_transpositions_in_panel;
       // recursively call the blocked LU algorithm on [A11^T A21^T]^T
       // with a very small blocking size:
-      Index ret = blocked_lu(trows+bs, bs, &lu.coeffRef(k,k), luStride,
-                   row_transpositions+k, nb_transpositions_in_panel, 16);
-      if(ret>=0 && first_zero_pivot==-1)
-        first_zero_pivot = k+ret;
+      Index ret = blocked_lu(trows + bs, bs, &lu.coeffRef(k, k), luStride, row_transpositions + k,
+                             nb_transpositions_in_panel, 16);
+      if (ret >= 0 && first_zero_pivot == -1) first_zero_pivot = k + ret;
 
       nb_transpositions += nb_transpositions_in_panel;
       // update permutations and apply them to A_0
-      for(Index i=k; i<k+bs; ++i)
-      {
+      for (Index i = k; i < k + bs; ++i) {
         Index piv = (row_transpositions[i] += internal::convert_index<PivIndex>(k));
         A_0.row(i).swap(A_0.row(piv));
       }
 
-      if(trows)
-      {
+      if (trows) {
         // apply permutations to A_2
-        for(Index i=k;i<k+bs; ++i)
-          A_2.row(i).swap(A_2.row(row_transpositions[i]));
+        for (Index i = k; i < k + bs; ++i) A_2.row(i).swap(A_2.row(row_transpositions[i]));
 
         // A12 = A11^-1 A12
         A11.template triangularView<UnitLower>().solveInPlace(A12);
@@ -500,36 +455,33 @@
 };
 
 /** \internal performs the LU decomposition with partial pivoting in-place.
-  */
-template<typename MatrixType, typename TranspositionType>
-void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions, typename TranspositionType::StorageIndex& nb_transpositions)
-{
+ */
+template <typename MatrixType, typename TranspositionType>
+void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions,
+                        typename TranspositionType::StorageIndex& nb_transpositions) {
   // Special-case of zero matrix.
   if (lu.rows() == 0 || lu.cols() == 0) {
     nb_transpositions = 0;
     return;
   }
   eigen_assert(lu.cols() == row_transpositions.size());
-  eigen_assert(row_transpositions.size() < 2 || (&row_transpositions.coeffRef(1)-&row_transpositions.coeffRef(0)) == 1);
+  eigen_assert(row_transpositions.size() < 2 ||
+               (&row_transpositions.coeffRef(1) - &row_transpositions.coeffRef(0)) == 1);
 
-  partial_lu_impl
-    < typename MatrixType::Scalar, MatrixType::Flags&RowMajorBit?RowMajor:ColMajor,
-      typename TranspositionType::StorageIndex,
-      EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::RowsAtCompileTime,MatrixType::ColsAtCompileTime)>
-    ::blocked_lu(lu.rows(), lu.cols(), &lu.coeffRef(0,0), lu.outerStride(), &row_transpositions.coeffRef(0), nb_transpositions);
+  partial_lu_impl<typename MatrixType::Scalar, MatrixType::Flags & RowMajorBit ? RowMajor : ColMajor,
+                  typename TranspositionType::StorageIndex,
+                  internal::min_size_prefer_fixed(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime)>::
+      blocked_lu(lu.rows(), lu.cols(), &lu.coeffRef(0, 0), lu.outerStride(), &row_transpositions.coeffRef(0),
+                 nb_transpositions);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename MatrixType>
-void PartialPivLU<MatrixType>::compute()
-{
-  check_template_parameters();
+template <typename MatrixType, typename PermutationIndex>
+void PartialPivLU<MatrixType, PermutationIndex>::compute() {
+  eigen_assert(m_lu.rows() < NumTraits<PermutationIndex>::highest());
 
-  // the row permutation is stored as int indices, so just to be sure:
-  eigen_assert(m_lu.rows()<NumTraits<int>::highest());
-
-  if(m_lu.cols()>0)
+  if (m_lu.cols() > 0)
     m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff();
   else
     m_l1_norm = RealScalar(0);
@@ -541,16 +493,16 @@
 
   typename TranspositionType::StorageIndex nb_transpositions;
   internal::partial_lu_inplace(m_lu, m_rowsTranspositions, nb_transpositions);
-  m_det_p = (nb_transpositions%2) ? -1 : 1;
+  m_det_p = (nb_transpositions % 2) ? -1 : 1;
 
   m_p = m_rowsTranspositions;
 
   m_isInitialized = true;
 }
 
-template<typename MatrixType>
-typename PartialPivLU<MatrixType>::Scalar PartialPivLU<MatrixType>::determinant() const
-{
+template <typename MatrixType, typename PermutationIndex>
+typename PartialPivLU<MatrixType, PermutationIndex>::Scalar PartialPivLU<MatrixType, PermutationIndex>::determinant()
+    const {
   eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
   return Scalar(m_det_p) * m_lu.diagonal().prod();
 }
@@ -558,13 +510,11 @@
 /** \returns the matrix represented by the decomposition,
  * i.e., it returns the product: P^{-1} L U.
  * This function is provided for debug purpose. */
-template<typename MatrixType>
-MatrixType PartialPivLU<MatrixType>::reconstructedMatrix() const
-{
+template <typename MatrixType, typename PermutationIndex>
+MatrixType PartialPivLU<MatrixType, PermutationIndex>::reconstructedMatrix() const {
   eigen_assert(m_isInitialized && "LU is not initialized.");
   // LU
-  MatrixType res = m_lu.template triangularView<UnitLower>().toDenseMatrix()
-                 * m_lu.template triangularView<Upper>();
+  MatrixType res = m_lu.template triangularView<UnitLower>().toDenseMatrix() * m_lu.template triangularView<Upper>();
 
   // P^{-1}(LU)
   res = m_p.inverse() * res;
@@ -577,48 +527,49 @@
 namespace internal {
 
 /***** Implementation of inverse() *****************************************************/
-template<typename DstXprType, typename MatrixType>
-struct Assignment<DstXprType, Inverse<PartialPivLU<MatrixType> >, internal::assign_op<typename DstXprType::Scalar,typename PartialPivLU<MatrixType>::Scalar>, Dense2Dense>
-{
-  typedef PartialPivLU<MatrixType> LuType;
+template <typename DstXprType, typename MatrixType, typename PermutationIndex>
+struct Assignment<
+    DstXprType, Inverse<PartialPivLU<MatrixType, PermutationIndex> >,
+    internal::assign_op<typename DstXprType::Scalar, typename PartialPivLU<MatrixType, PermutationIndex>::Scalar>,
+    Dense2Dense> {
+  typedef PartialPivLU<MatrixType, PermutationIndex> LuType;
   typedef Inverse<LuType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename LuType::Scalar> &)
-  {
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename LuType::Scalar>&) {
     dst = src.nestedExpression().solve(MatrixType::Identity(src.rows(), src.cols()));
   }
 };
-} // end namespace internal
+}  // end namespace internal
 
 /******** MatrixBase methods *******/
 
 /** \lu_module
-  *
-  * \return the partial-pivoting LU decomposition of \c *this.
-  *
-  * \sa class PartialPivLU
-  */
-template<typename Derived>
-inline const PartialPivLU<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::partialPivLu() const
-{
-  return PartialPivLU<PlainObject>(eval());
+ *
+ * \return the partial-pivoting LU decomposition of \c *this.
+ *
+ * \sa class PartialPivLU
+ */
+template <typename Derived>
+template <typename PermutationIndex>
+inline const PartialPivLU<typename MatrixBase<Derived>::PlainObject, PermutationIndex>
+MatrixBase<Derived>::partialPivLu() const {
+  return PartialPivLU<PlainObject, PermutationIndex>(eval());
 }
 
 /** \lu_module
-  *
-  * Synonym of partialPivLu().
-  *
-  * \return the partial-pivoting LU decomposition of \c *this.
-  *
-  * \sa class PartialPivLU
-  */
-template<typename Derived>
-inline const PartialPivLU<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::lu() const
-{
-  return PartialPivLU<PlainObject>(eval());
+ *
+ * Synonym of partialPivLu().
+ *
+ * \return the partial-pivoting LU decomposition of \c *this.
+ *
+ * \sa class PartialPivLU
+ */
+template <typename Derived>
+template <typename PermutationIndex>
+inline const PartialPivLU<typename MatrixBase<Derived>::PlainObject, PermutationIndex> MatrixBase<Derived>::lu() const {
+  return PartialPivLU<PlainObject, PermutationIndex>(eval());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_PARTIALLU_H
+#endif  // EIGEN_PARTIALLU_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h
index a232ffc..f0ddb2f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h
@@ -35,50 +35,54 @@
 #ifndef EIGEN_INVERSE_SIZE_4_H
 #define EIGEN_INVERSE_SIZE_4_H
 
-namespace Eigen
-{
-namespace internal
-{
+// IWYU pragma: private
+#include "../InternalHeaderCheck.h"
+
+#if EIGEN_COMP_GNUC_STRICT
+// These routines requires bit manipulation of the sign, which is not compatible
+// with fastmath.
+#pragma GCC push_options
+#pragma GCC optimize("no-fast-math")
+#endif
+
+namespace Eigen {
+namespace internal {
 template <typename MatrixType, typename ResultType>
-struct compute_inverse_size4<Architecture::Target, float, MatrixType, ResultType>
-{
-  enum
-  {
+struct compute_inverse_size4<Architecture::Target, float, MatrixType, ResultType> {
+  enum {
     MatrixAlignment = traits<MatrixType>::Alignment,
     ResultAlignment = traits<ResultType>::Alignment,
     StorageOrdersMatch = (MatrixType::Flags & RowMajorBit) == (ResultType::Flags & RowMajorBit)
   };
-  typedef typename conditional<(MatrixType::Flags & LinearAccessBit), MatrixType const &, typename MatrixType::PlainObject>::type ActualMatrixType;
+  typedef std::conditional_t<(MatrixType::Flags & LinearAccessBit), MatrixType const &,
+                             typename MatrixType::PlainObject>
+      ActualMatrixType;
 
-  static void run(const MatrixType &mat, ResultType &result)
-  {
+  static void run(const MatrixType &mat, ResultType &result) {
     ActualMatrixType matrix(mat);
 
-    const float* data = matrix.data();
+    const float *data = matrix.data();
     const Index stride = matrix.innerStride();
-    Packet4f _L1 = ploadt<Packet4f,MatrixAlignment>(data);
-    Packet4f _L2 = ploadt<Packet4f,MatrixAlignment>(data + stride*4);
-    Packet4f _L3 = ploadt<Packet4f,MatrixAlignment>(data + stride*8);
-    Packet4f _L4 = ploadt<Packet4f,MatrixAlignment>(data + stride*12);
+    Packet4f L1 = ploadt<Packet4f, MatrixAlignment>(data);
+    Packet4f L2 = ploadt<Packet4f, MatrixAlignment>(data + stride * 4);
+    Packet4f L3 = ploadt<Packet4f, MatrixAlignment>(data + stride * 8);
+    Packet4f L4 = ploadt<Packet4f, MatrixAlignment>(data + stride * 12);
 
     // Four 2x2 sub-matrices of the input matrix
     // input = [[A, B],
     //          [C, D]]
     Packet4f A, B, C, D;
 
-    if (!StorageOrdersMatch)
-    {
-      A = vec4f_unpacklo(_L1, _L2);
-      B = vec4f_unpacklo(_L3, _L4);
-      C = vec4f_unpackhi(_L1, _L2);
-      D = vec4f_unpackhi(_L3, _L4);
-    }
-    else
-    {
-      A = vec4f_movelh(_L1, _L2);
-      B = vec4f_movehl(_L2, _L1);
-      C = vec4f_movelh(_L3, _L4);
-      D = vec4f_movehl(_L4, _L3);
+    if (!StorageOrdersMatch) {
+      A = vec4f_unpacklo(L1, L2);
+      B = vec4f_unpacklo(L3, L4);
+      C = vec4f_unpackhi(L1, L2);
+      D = vec4f_unpackhi(L3, L4);
+    } else {
+      A = vec4f_movelh(L1, L2);
+      B = vec4f_movehl(L2, L1);
+      C = vec4f_movelh(L3, L4);
+      D = vec4f_movehl(L4, L3);
     }
 
     Packet4f AB, DC;
@@ -118,7 +122,7 @@
     Packet4f det = vec4f_duplane(psub(padd(d1, d2), d), 0);
 
     // reciprocal of the determinant of the input matrix, rd = 1/det
-    Packet4f rd = pdiv(pset1<Packet4f>(1.0f), det);
+    Packet4f rd = preciprocal(det);
 
     // Four sub-matrices of the inverse
     Packet4f iA, iB, iC, iD;
@@ -143,8 +147,8 @@
     iC = psub(iC, pmul(vec4f_swizzle2(A, A, 1, 0, 3, 2), vec4f_swizzle2(DC, DC, 2, 1, 2, 1)));
     iC = psub(pmul(B, vec4f_duplane(dC, 0)), iC);
 
-    const float sign_mask[4] = {0.0f, numext::bit_cast<float>(0x80000000u), numext::bit_cast<float>(0x80000000u), 0.0f};
-    const Packet4f p4f_sign_PNNP = ploadu<Packet4f>(sign_mask);
+    EIGEN_ALIGN_MAX const float sign_mask[4] = {0.0f, -0.0f, -0.0f, 0.0f};
+    const Packet4f p4f_sign_PNNP = pload<Packet4f>(sign_mask);
     rd = pxor(rd, p4f_sign_PNNP);
     iA = pmul(iA, rd);
     iB = pmul(iB, rd);
@@ -165,21 +169,17 @@
 // same algorithm as above, except that each operand is split into
 // halves for two registers to hold.
 template <typename MatrixType, typename ResultType>
-struct compute_inverse_size4<Architecture::Target, double, MatrixType, ResultType>
-{
-  enum
-  {
+struct compute_inverse_size4<Architecture::Target, double, MatrixType, ResultType> {
+  enum {
     MatrixAlignment = traits<MatrixType>::Alignment,
     ResultAlignment = traits<ResultType>::Alignment,
     StorageOrdersMatch = (MatrixType::Flags & RowMajorBit) == (ResultType::Flags & RowMajorBit)
   };
-  typedef typename conditional<(MatrixType::Flags & LinearAccessBit),
-                               MatrixType const &,
-                               typename MatrixType::PlainObject>::type
+  typedef std::conditional_t<(MatrixType::Flags & LinearAccessBit), MatrixType const &,
+                             typename MatrixType::PlainObject>
       ActualMatrixType;
 
-  static void run(const MatrixType &mat, ResultType &result)
-  {
+  static void run(const MatrixType &mat, ResultType &result) {
     ActualMatrixType matrix(mat);
 
     // Four 2x2 sub-matrices of the input matrix, each is further divided into upper and lower
@@ -191,26 +191,23 @@
 
     Packet2d A1, A2, B1, B2, C1, C2, D1, D2;
 
-    const double* data = matrix.data();
+    const double *data = matrix.data();
     const Index stride = matrix.innerStride();
-    if (StorageOrdersMatch)
-    {
-      A1 = ploadt<Packet2d,MatrixAlignment>(data + stride*0);
-      B1 = ploadt<Packet2d,MatrixAlignment>(data + stride*2);
-      A2 = ploadt<Packet2d,MatrixAlignment>(data + stride*4);
-      B2 = ploadt<Packet2d,MatrixAlignment>(data + stride*6);
-      C1 = ploadt<Packet2d,MatrixAlignment>(data + stride*8);
-      D1 = ploadt<Packet2d,MatrixAlignment>(data + stride*10);
-      C2 = ploadt<Packet2d,MatrixAlignment>(data + stride*12);
-      D2 = ploadt<Packet2d,MatrixAlignment>(data + stride*14);
-    }
-    else
-    {
+    if (StorageOrdersMatch) {
+      A1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 0);
+      B1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 2);
+      A2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 4);
+      B2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 6);
+      C1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 8);
+      D1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 10);
+      C2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 12);
+      D2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 14);
+    } else {
       Packet2d temp;
-      A1 = ploadt<Packet2d,MatrixAlignment>(data + stride*0);
-      C1 = ploadt<Packet2d,MatrixAlignment>(data + stride*2);
-      A2 = ploadt<Packet2d,MatrixAlignment>(data + stride*4);
-      C2 = ploadt<Packet2d,MatrixAlignment>(data + stride*6);
+      A1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 0);
+      C1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 2);
+      A2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 4);
+      C2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 6);
       temp = A1;
       A1 = vec2d_unpacklo(A1, A2);
       A2 = vec2d_unpackhi(temp, A2);
@@ -219,10 +216,10 @@
       C1 = vec2d_unpacklo(C1, C2);
       C2 = vec2d_unpackhi(temp, C2);
 
-      B1 = ploadt<Packet2d,MatrixAlignment>(data + stride*8);
-      D1 = ploadt<Packet2d,MatrixAlignment>(data + stride*10);
-      B2 = ploadt<Packet2d,MatrixAlignment>(data + stride*12);
-      D2 = ploadt<Packet2d,MatrixAlignment>(data + stride*14);
+      B1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 8);
+      D1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 10);
+      B2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 12);
+      D2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 14);
 
       temp = B1;
       B1 = vec2d_unpacklo(B1, B2);
@@ -326,10 +323,10 @@
     iC1 = psub(pmul(B1, dC), iC1);
     iC2 = psub(pmul(B2, dC), iC2);
 
-    const double sign_mask1[2] = {0.0, numext::bit_cast<double>(0x8000000000000000ull)};
-    const double sign_mask2[2] = {numext::bit_cast<double>(0x8000000000000000ull), 0.0};
-    const Packet2d sign_PN = ploadu<Packet2d>(sign_mask1);
-    const Packet2d sign_NP = ploadu<Packet2d>(sign_mask2);
+    EIGEN_ALIGN_MAX const double sign_mask1[2] = {0.0, -0.0};
+    EIGEN_ALIGN_MAX const double sign_mask2[2] = {-0.0, 0.0};
+    const Packet2d sign_PN = pload<Packet2d>(sign_mask1);
+    const Packet2d sign_NP = pload<Packet2d>(sign_mask2);
     d1 = pxor(rd, sign_PN);
     d2 = pxor(rd, sign_NP);
 
@@ -346,6 +343,11 @@
   }
 };
 #endif
-} // namespace internal
-} // namespace Eigen
+}  // namespace internal
+}  // namespace Eigen
+
+#if EIGEN_COMP_GNUC_STRICT
+#pragma GCC pop_options
+#endif
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h
index 7ca3f33..0b0bf02 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h
@@ -21,415 +21,393 @@
 #ifndef EIGEN_SPARSE_AMD_H
 #define EIGEN_SPARSE_AMD_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-  
-template<typename T> inline T amd_flip(const T& i) { return -i-2; }
-template<typename T> inline T amd_unflip(const T& i) { return i<0 ? amd_flip(i) : i; }
-template<typename T0, typename T1> inline bool amd_marked(const T0* w, const T1& j) { return w[j]<0; }
-template<typename T0, typename T1> inline void amd_mark(const T0* w, const T1& j) { return w[j] = amd_flip(w[j]); }
+
+template <typename T>
+inline T amd_flip(const T& i) {
+  return -i - 2;
+}
+template <typename T>
+inline T amd_unflip(const T& i) {
+  return i < 0 ? amd_flip(i) : i;
+}
+template <typename T0, typename T1>
+inline bool amd_marked(const T0* w, const T1& j) {
+  return w[j] < 0;
+}
+template <typename T0, typename T1>
+inline void amd_mark(const T0* w, const T1& j) {
+  return w[j] = amd_flip(w[j]);
+}
 
 /* clear w */
-template<typename StorageIndex>
-static StorageIndex cs_wclear (StorageIndex mark, StorageIndex lemax, StorageIndex *w, StorageIndex n)
-{
+template <typename StorageIndex>
+static StorageIndex cs_wclear(StorageIndex mark, StorageIndex lemax, StorageIndex* w, StorageIndex n) {
   StorageIndex k;
-  if(mark < 2 || (mark + lemax < 0))
-  {
-    for(k = 0; k < n; k++)
-      if(w[k] != 0)
-        w[k] = 1;
+  if (mark < 2 || (mark + lemax < 0)) {
+    for (k = 0; k < n; k++)
+      if (w[k] != 0) w[k] = 1;
     mark = 2;
   }
-  return (mark);     /* at this point, w[0..n-1] < mark holds */
+  return (mark); /* at this point, w[0..n-1] < mark holds */
 }
 
 /* depth-first search and postorder of a tree rooted at node j */
-template<typename StorageIndex>
-StorageIndex cs_tdfs(StorageIndex j, StorageIndex k, StorageIndex *head, const StorageIndex *next, StorageIndex *post, StorageIndex *stack)
-{
+template <typename StorageIndex>
+StorageIndex cs_tdfs(StorageIndex j, StorageIndex k, StorageIndex* head, const StorageIndex* next, StorageIndex* post,
+                     StorageIndex* stack) {
   StorageIndex i, p, top = 0;
-  if(!head || !next || !post || !stack) return (-1);    /* check inputs */
-  stack[0] = j;                 /* place j on the stack */
-  while (top >= 0)                /* while (stack is not empty) */
+  if (!head || !next || !post || !stack) return (-1); /* check inputs */
+  stack[0] = j;                                       /* place j on the stack */
+  while (top >= 0)                                    /* while (stack is not empty) */
   {
-    p = stack[top];           /* p = top of stack */
-    i = head[p];              /* i = youngest child of p */
-    if(i == -1)
-    {
-      top--;                 /* p has no unordered children left */
-      post[k++] = p;        /* node p is the kth postordered node */
-    }
-    else
-    {
-      head[p] = next[i];   /* remove i from children of p */
-      stack[++top] = i;     /* start dfs on child node i */
+    p = stack[top]; /* p = top of stack */
+    i = head[p];    /* i = youngest child of p */
+    if (i == -1) {
+      top--;         /* p has no unordered children left */
+      post[k++] = p; /* node p is the kth postordered node */
+    } else {
+      head[p] = next[i]; /* remove i from children of p */
+      stack[++top] = i;  /* start dfs on child node i */
     }
   }
   return k;
 }
 
-
 /** \internal
-  * \ingroup OrderingMethods_Module 
-  * Approximate minimum degree ordering algorithm.
-  *
-  * \param[in] C the input selfadjoint matrix stored in compressed column major format.
-  * \param[out] perm the permutation P reducing the fill-in of the input matrix \a C
-  *
-  * Note that the input matrix \a C must be complete, that is both the upper and lower parts have to be stored, as well as the diagonal entries.
-  * On exit the values of C are destroyed */
-template<typename Scalar, typename StorageIndex>
-void minimum_degree_ordering(SparseMatrix<Scalar,ColMajor,StorageIndex>& C, PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm)
-{
+ * \ingroup OrderingMethods_Module
+ * Approximate minimum degree ordering algorithm.
+ *
+ * \param[in] C the input selfadjoint matrix stored in compressed column major format.
+ * \param[out] perm the permutation P reducing the fill-in of the input matrix \a C
+ *
+ * Note that the input matrix \a C must be complete, that is both the upper and lower parts have to be stored, as well
+ * as the diagonal entries. On exit the values of C are destroyed */
+template <typename Scalar, typename StorageIndex>
+void minimum_degree_ordering(SparseMatrix<Scalar, ColMajor, StorageIndex>& C,
+                             PermutationMatrix<Dynamic, Dynamic, StorageIndex>& perm) {
   using std::sqrt;
-  
-  StorageIndex d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
-                k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
-                ok, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t, h;
-  
+
+  StorageIndex d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1, k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi,
+                            nvj, nvk, mark, wnvi, ok, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t, h;
+
   StorageIndex n = StorageIndex(C.cols());
-  dense = std::max<StorageIndex> (16, StorageIndex(10 * sqrt(double(n))));   /* find dense threshold */
-  dense = (std::min)(n-2, dense);
-  
+  dense = std::max<StorageIndex>(16, StorageIndex(10 * sqrt(double(n)))); /* find dense threshold */
+  dense = (std::min)(n - 2, dense);
+
   StorageIndex cnz = StorageIndex(C.nonZeros());
-  perm.resize(n+1);
-  t = cnz + cnz/5 + 2*n;                 /* add elbow room to C */
+  perm.resize(n + 1);
+  t = cnz + cnz / 5 + 2 * n; /* add elbow room to C */
   C.resizeNonZeros(t);
-  
+
   // get workspace
-  ei_declare_aligned_stack_constructed_variable(StorageIndex,W,8*(n+1),0);
-  StorageIndex* len     = W;
-  StorageIndex* nv      = W +   (n+1);
-  StorageIndex* next    = W + 2*(n+1);
-  StorageIndex* head    = W + 3*(n+1);
-  StorageIndex* elen    = W + 4*(n+1);
-  StorageIndex* degree  = W + 5*(n+1);
-  StorageIndex* w       = W + 6*(n+1);
-  StorageIndex* hhead   = W + 7*(n+1);
-  StorageIndex* last    = perm.indices().data();                              /* use P as workspace for last */
-  
+  ei_declare_aligned_stack_constructed_variable(StorageIndex, W, 8 * (n + 1), 0);
+  StorageIndex* len = W;
+  StorageIndex* nv = W + (n + 1);
+  StorageIndex* next = W + 2 * (n + 1);
+  StorageIndex* head = W + 3 * (n + 1);
+  StorageIndex* elen = W + 4 * (n + 1);
+  StorageIndex* degree = W + 5 * (n + 1);
+  StorageIndex* w = W + 6 * (n + 1);
+  StorageIndex* hhead = W + 7 * (n + 1);
+  StorageIndex* last = perm.indices().data(); /* use P as workspace for last */
+
   /* --- Initialize quotient graph ---------------------------------------- */
   StorageIndex* Cp = C.outerIndexPtr();
   StorageIndex* Ci = C.innerIndexPtr();
-  for(k = 0; k < n; k++)
-    len[k] = Cp[k+1] - Cp[k];
+  for (k = 0; k < n; k++) len[k] = Cp[k + 1] - Cp[k];
   len[n] = 0;
   nzmax = t;
-  
-  for(i = 0; i <= n; i++)
-  {
-    head[i]   = -1;                     // degree list i is empty
-    last[i]   = -1;
-    next[i]   = -1;
-    hhead[i]  = -1;                     // hash list i is empty 
-    nv[i]     = 1;                      // node i is just one node
-    w[i]      = 1;                      // node i is alive
-    elen[i]   = 0;                      // Ek of node i is empty
-    degree[i] = len[i];                 // degree of node i
+
+  for (i = 0; i <= n; i++) {
+    head[i] = -1;  // degree list i is empty
+    last[i] = -1;
+    next[i] = -1;
+    hhead[i] = -1;       // hash list i is empty
+    nv[i] = 1;           // node i is just one node
+    w[i] = 1;            // node i is alive
+    elen[i] = 0;         // Ek of node i is empty
+    degree[i] = len[i];  // degree of node i
   }
-  mark = internal::cs_wclear<StorageIndex>(0, 0, w, n);         /* clear w */
-  
+  mark = internal::cs_wclear<StorageIndex>(0, 0, w, n); /* clear w */
+
   /* --- Initialize degree lists ------------------------------------------ */
-  for(i = 0; i < n; i++)
-  {
+  for (i = 0; i < n; i++) {
     bool has_diag = false;
-    for(p = Cp[i]; p<Cp[i+1]; ++p)
-      if(Ci[p]==i)
-      {
+    for (p = Cp[i]; p < Cp[i + 1]; ++p)
+      if (Ci[p] == i) {
         has_diag = true;
         break;
       }
-   
+
     d = degree[i];
-    if(d == 1 && has_diag)           /* node i is empty */
+    if (d == 1 && has_diag) /* node i is empty */
     {
-      elen[i] = -2;                 /* element i is dead */
+      elen[i] = -2; /* element i is dead */
       nel++;
-      Cp[i] = -1;                   /* i is a root of assembly tree */
+      Cp[i] = -1; /* i is a root of assembly tree */
       w[i] = 0;
-    }
-    else if(d > dense || !has_diag)  /* node i is dense or has no structural diagonal element */
+    } else if (d > dense || !has_diag) /* node i is dense or has no structural diagonal element */
     {
-      nv[i] = 0;                    /* absorb i into element n */
-      elen[i] = -1;                 /* node i is dead */
+      nv[i] = 0;    /* absorb i into element n */
+      elen[i] = -1; /* node i is dead */
       nel++;
-      Cp[i] = amd_flip (n);
+      Cp[i] = amd_flip(n);
       nv[n]++;
-    }
-    else
-    {
-      if(head[d] != -1) last[head[d]] = i;
-      next[i] = head[d];           /* put node i in degree list d */
+    } else {
+      if (head[d] != -1) last[head[d]] = i;
+      next[i] = head[d]; /* put node i in degree list d */
       head[d] = i;
     }
   }
-  
-  elen[n] = -2;                         /* n is a dead element */
-  Cp[n] = -1;                           /* n is a root of assembly tree */
-  w[n] = 0;                             /* n is a dead element */
-  
-  while (nel < n)                         /* while (selecting pivots) do */
+
+  elen[n] = -2; /* n is a dead element */
+  Cp[n] = -1;   /* n is a root of assembly tree */
+  w[n] = 0;     /* n is a dead element */
+
+  while (nel < n) /* while (selecting pivots) do */
   {
     /* --- Select node of minimum approximate degree -------------------- */
-    for(k = -1; mindeg < n && (k = head[mindeg]) == -1; mindeg++) {}
-    if(next[k] != -1) last[next[k]] = -1;
-    head[mindeg] = next[k];          /* remove k from degree list */
-    elenk = elen[k];                  /* elenk = |Ek| */
-    nvk = nv[k];                      /* # of nodes k represents */
-    nel += nvk;                        /* nv[k] nodes of A eliminated */
-    
-    /* --- Garbage collection ------------------------------------------- */
-    if(elenk > 0 && cnz + mindeg >= nzmax)
-    {
-      for(j = 0; j < n; j++)
-      {
-        if((p = Cp[j]) >= 0)      /* j is a live node or element */
-        {
-          Cp[j] = Ci[p];          /* save first entry of object */
-          Ci[p] = amd_flip (j);    /* first entry is now amd_flip(j) */
-        }
-      }
-      for(q = 0, p = 0; p < cnz; ) /* scan all of memory */
-      {
-        if((j = amd_flip (Ci[p++])) >= 0)  /* found object j */
-        {
-          Ci[q] = Cp[j];       /* restore first entry of object */
-          Cp[j] = q++;          /* new pointer to object j */
-          for(k3 = 0; k3 < len[j]-1; k3++) Ci[q++] = Ci[p++];
-        }
-      }
-      cnz = q;                       /* Ci[cnz...nzmax-1] now free */
+    for (k = -1; mindeg < n && (k = head[mindeg]) == -1; mindeg++) {
     }
-    
+    if (next[k] != -1) last[next[k]] = -1;
+    head[mindeg] = next[k]; /* remove k from degree list */
+    elenk = elen[k];        /* elenk = |Ek| */
+    nvk = nv[k];            /* # of nodes k represents */
+    nel += nvk;             /* nv[k] nodes of A eliminated */
+
+    /* --- Garbage collection ------------------------------------------- */
+    if (elenk > 0 && cnz + mindeg >= nzmax) {
+      for (j = 0; j < n; j++) {
+        if ((p = Cp[j]) >= 0) /* j is a live node or element */
+        {
+          Cp[j] = Ci[p];       /* save first entry of object */
+          Ci[p] = amd_flip(j); /* first entry is now amd_flip(j) */
+        }
+      }
+      for (q = 0, p = 0; p < cnz;) /* scan all of memory */
+      {
+        if ((j = amd_flip(Ci[p++])) >= 0) /* found object j */
+        {
+          Ci[q] = Cp[j]; /* restore first entry of object */
+          Cp[j] = q++;   /* new pointer to object j */
+          for (k3 = 0; k3 < len[j] - 1; k3++) Ci[q++] = Ci[p++];
+        }
+      }
+      cnz = q; /* Ci[cnz...nzmax-1] now free */
+    }
+
     /* --- Construct new element ---------------------------------------- */
     dk = 0;
-    nv[k] = -nvk;                     /* flag k as in Lk */
+    nv[k] = -nvk; /* flag k as in Lk */
     p = Cp[k];
-    pk1 = (elenk == 0) ? p : cnz;      /* do in place if elen[k] == 0 */
+    pk1 = (elenk == 0) ? p : cnz; /* do in place if elen[k] == 0 */
     pk2 = pk1;
-    for(k1 = 1; k1 <= elenk + 1; k1++)
-    {
-      if(k1 > elenk)
-      {
-        e = k;                     /* search the nodes in k */
-        pj = p;                    /* list of nodes starts at Ci[pj]*/
-        ln = len[k] - elenk;      /* length of list of nodes in k */
-      }
-      else
-      {
-        e = Ci[p++];              /* search the nodes in e */
+    for (k1 = 1; k1 <= elenk + 1; k1++) {
+      if (k1 > elenk) {
+        e = k;               /* search the nodes in k */
+        pj = p;              /* list of nodes starts at Ci[pj]*/
+        ln = len[k] - elenk; /* length of list of nodes in k */
+      } else {
+        e = Ci[p++]; /* search the nodes in e */
         pj = Cp[e];
-        ln = len[e];              /* length of list of nodes in e */
+        ln = len[e]; /* length of list of nodes in e */
       }
-      for(k2 = 1; k2 <= ln; k2++)
-      {
+      for (k2 = 1; k2 <= ln; k2++) {
         i = Ci[pj++];
-        if((nvi = nv[i]) <= 0) continue; /* node i dead, or seen */
-        dk += nvi;                 /* degree[Lk] += size of node i */
-        nv[i] = -nvi;             /* negate nv[i] to denote i in Lk*/
-        Ci[pk2++] = i;            /* place i in Lk */
-        if(next[i] != -1) last[next[i]] = last[i];
-        if(last[i] != -1)         /* remove i from degree list */
+        if ((nvi = nv[i]) <= 0) continue; /* node i dead, or seen */
+        dk += nvi;                        /* degree[Lk] += size of node i */
+        nv[i] = -nvi;                     /* negate nv[i] to denote i in Lk*/
+        Ci[pk2++] = i;                    /* place i in Lk */
+        if (next[i] != -1) last[next[i]] = last[i];
+        if (last[i] != -1) /* remove i from degree list */
         {
           next[last[i]] = next[i];
-        }
-        else
-        {
+        } else {
           head[degree[i]] = next[i];
         }
       }
-      if(e != k)
-      {
-        Cp[e] = amd_flip (k);      /* absorb e into k */
-        w[e] = 0;                 /* e is now a dead element */
+      if (e != k) {
+        Cp[e] = amd_flip(k); /* absorb e into k */
+        w[e] = 0;            /* e is now a dead element */
       }
     }
-    if(elenk != 0) cnz = pk2;         /* Ci[cnz...nzmax] is free */
-    degree[k] = dk;                   /* external degree of k - |Lk\i| */
-    Cp[k] = pk1;                      /* element k is in Ci[pk1..pk2-1] */
+    if (elenk != 0) cnz = pk2; /* Ci[cnz...nzmax] is free */
+    degree[k] = dk;            /* external degree of k - |Lk\i| */
+    Cp[k] = pk1;               /* element k is in Ci[pk1..pk2-1] */
     len[k] = pk2 - pk1;
-    elen[k] = -2;                     /* k is now an element */
-    
+    elen[k] = -2; /* k is now an element */
+
     /* --- Find set differences ----------------------------------------- */
-    mark = internal::cs_wclear<StorageIndex>(mark, lemax, w, n);  /* clear w if necessary */
-    for(pk = pk1; pk < pk2; pk++)    /* scan 1: find |Le\Lk| */
+    mark = internal::cs_wclear<StorageIndex>(mark, lemax, w, n); /* clear w if necessary */
+    for (pk = pk1; pk < pk2; pk++)                               /* scan 1: find |Le\Lk| */
     {
       i = Ci[pk];
-      if((eln = elen[i]) <= 0) continue;/* skip if elen[i] empty */
-      nvi = -nv[i];                      /* nv[i] was negated */
+      if ((eln = elen[i]) <= 0) continue; /* skip if elen[i] empty */
+      nvi = -nv[i];                       /* nv[i] was negated */
       wnvi = mark - nvi;
-      for(p = Cp[i]; p <= Cp[i] + eln - 1; p++)  /* scan Ei */
+      for (p = Cp[i]; p <= Cp[i] + eln - 1; p++) /* scan Ei */
       {
         e = Ci[p];
-        if(w[e] >= mark)
-        {
-          w[e] -= nvi;          /* decrement |Le\Lk| */
-        }
-        else if(w[e] != 0)        /* ensure e is a live element */
+        if (w[e] >= mark) {
+          w[e] -= nvi;        /* decrement |Le\Lk| */
+        } else if (w[e] != 0) /* ensure e is a live element */
         {
           w[e] = degree[e] + wnvi; /* 1st time e seen in scan 1 */
         }
       }
     }
-    
+
     /* --- Degree update ------------------------------------------------ */
-    for(pk = pk1; pk < pk2; pk++)    /* scan2: degree update */
+    for (pk = pk1; pk < pk2; pk++) /* scan2: degree update */
     {
-      i = Ci[pk];                   /* consider node i in Lk */
+      i = Ci[pk]; /* consider node i in Lk */
       p1 = Cp[i];
       p2 = p1 + elen[i] - 1;
       pn = p1;
-      for(h = 0, d = 0, p = p1; p <= p2; p++)    /* scan Ei */
+      for (h = 0, d = 0, p = p1; p <= p2; p++) /* scan Ei */
       {
         e = Ci[p];
-        if(w[e] != 0)             /* e is an unabsorbed element */
+        if (w[e] != 0) /* e is an unabsorbed element */
         {
-          dext = w[e] - mark;   /* dext = |Le\Lk| */
-          if(dext > 0)
-          {
-            d += dext;         /* sum up the set differences */
-            Ci[pn++] = e;     /* keep e in Ei */
-            h += e;            /* compute the hash of node i */
-          }
-          else
-          {
-            Cp[e] = amd_flip (k);  /* aggressive absorb. e->k */
-            w[e] = 0;             /* e is a dead element */
+          dext = w[e] - mark; /* dext = |Le\Lk| */
+          if (dext > 0) {
+            d += dext;    /* sum up the set differences */
+            Ci[pn++] = e; /* keep e in Ei */
+            h += e;       /* compute the hash of node i */
+          } else {
+            Cp[e] = amd_flip(k); /* aggressive absorb. e->k */
+            w[e] = 0;            /* e is a dead element */
           }
         }
       }
-      elen[i] = pn - p1 + 1;        /* elen[i] = |Ei| */
+      elen[i] = pn - p1 + 1; /* elen[i] = |Ei| */
       p3 = pn;
       p4 = p1 + len[i];
-      for(p = p2 + 1; p < p4; p++) /* prune edges in Ai */
+      for (p = p2 + 1; p < p4; p++) /* prune edges in Ai */
       {
         j = Ci[p];
-        if((nvj = nv[j]) <= 0) continue; /* node j dead or in Lk */
-        d += nvj;                  /* degree(i) += |j| */
-        Ci[pn++] = j;             /* place j in node list of i */
-        h += j;                    /* compute hash for node i */
+        if ((nvj = nv[j]) <= 0) continue; /* node j dead or in Lk */
+        d += nvj;                         /* degree(i) += |j| */
+        Ci[pn++] = j;                     /* place j in node list of i */
+        h += j;                           /* compute hash for node i */
       }
-      if(d == 0)                     /* check for mass elimination */
+      if (d == 0) /* check for mass elimination */
       {
-        Cp[i] = amd_flip (k);      /* absorb i into k */
+        Cp[i] = amd_flip(k); /* absorb i into k */
         nvi = -nv[i];
-        dk -= nvi;                 /* |Lk| -= |i| */
-        nvk += nvi;                /* |k| += nv[i] */
+        dk -= nvi;  /* |Lk| -= |i| */
+        nvk += nvi; /* |k| += nv[i] */
         nel += nvi;
         nv[i] = 0;
-        elen[i] = -1;             /* node i is dead */
-      }
-      else
-      {
-        degree[i] = std::min<StorageIndex> (degree[i], d);   /* update degree(i) */
-        Ci[pn] = Ci[p3];         /* move first node to end */
-        Ci[p3] = Ci[p1];         /* move 1st el. to end of Ei */
-        Ci[p1] = k;               /* add k as 1st element in of Ei */
-        len[i] = pn - p1 + 1;     /* new len of adj. list of node i */
-        h %= n;                    /* finalize hash of i */
-        next[i] = hhead[h];      /* place i in hash bucket */
+        elen[i] = -1; /* node i is dead */
+      } else {
+        degree[i] = std::min<StorageIndex>(degree[i], d); /* update degree(i) */
+        Ci[pn] = Ci[p3];                                  /* move first node to end */
+        Ci[p3] = Ci[p1];                                  /* move 1st el. to end of Ei */
+        Ci[p1] = k;                                       /* add k as 1st element in of Ei */
+        len[i] = pn - p1 + 1;                             /* new len of adj. list of node i */
+        h %= n;                                           /* finalize hash of i */
+        next[i] = hhead[h];                               /* place i in hash bucket */
         hhead[h] = i;
-        last[i] = h;      /* save hash of i in last[i] */
+        last[i] = h; /* save hash of i in last[i] */
       }
-    }                                   /* scan2 is done */
-    degree[k] = dk;                   /* finalize |Lk| */
+    }               /* scan2 is done */
+    degree[k] = dk; /* finalize |Lk| */
     lemax = std::max<StorageIndex>(lemax, dk);
-    mark = internal::cs_wclear<StorageIndex>(mark+lemax, lemax, w, n);    /* clear w */
-    
+    mark = internal::cs_wclear<StorageIndex>(mark + lemax, lemax, w, n); /* clear w */
+
     /* --- Supernode detection ------------------------------------------ */
-    for(pk = pk1; pk < pk2; pk++)
-    {
+    for (pk = pk1; pk < pk2; pk++) {
       i = Ci[pk];
-      if(nv[i] >= 0) continue;         /* skip if i is dead */
-      h = last[i];                      /* scan hash bucket of node i */
+      if (nv[i] >= 0) continue; /* skip if i is dead */
+      h = last[i];              /* scan hash bucket of node i */
       i = hhead[h];
-      hhead[h] = -1;                    /* hash bucket will be empty */
-      for(; i != -1 && next[i] != -1; i = next[i], mark++)
-      {
+      hhead[h] = -1; /* hash bucket will be empty */
+      for (; i != -1 && next[i] != -1; i = next[i], mark++) {
         ln = len[i];
         eln = elen[i];
-        for(p = Cp[i]+1; p <= Cp[i] + ln-1; p++) w[Ci[p]] = mark;
+        for (p = Cp[i] + 1; p <= Cp[i] + ln - 1; p++) w[Ci[p]] = mark;
         jlast = i;
-        for(j = next[i]; j != -1; ) /* compare i with all j */
+        for (j = next[i]; j != -1;) /* compare i with all j */
         {
           ok = (len[j] == ln) && (elen[j] == eln);
-          for(p = Cp[j] + 1; ok && p <= Cp[j] + ln - 1; p++)
-          {
-            if(w[Ci[p]] != mark) ok = 0;    /* compare i and j*/
+          for (p = Cp[j] + 1; ok && p <= Cp[j] + ln - 1; p++) {
+            if (w[Ci[p]] != mark) ok = 0; /* compare i and j*/
           }
-          if(ok)                     /* i and j are identical */
+          if (ok) /* i and j are identical */
           {
-            Cp[j] = amd_flip (i);  /* absorb j into i */
+            Cp[j] = amd_flip(i); /* absorb j into i */
             nv[i] += nv[j];
             nv[j] = 0;
-            elen[j] = -1;         /* node j is dead */
-            j = next[j];          /* delete j from hash bucket */
+            elen[j] = -1; /* node j is dead */
+            j = next[j];  /* delete j from hash bucket */
             next[jlast] = j;
-          }
-          else
-          {
-            jlast = j;             /* j and i are different */
+          } else {
+            jlast = j; /* j and i are different */
             j = next[j];
           }
         }
       }
     }
-    
+
     /* --- Finalize new element------------------------------------------ */
-    for(p = pk1, pk = pk1; pk < pk2; pk++)   /* finalize Lk */
+    for (p = pk1, pk = pk1; pk < pk2; pk++) /* finalize Lk */
     {
       i = Ci[pk];
-      if((nvi = -nv[i]) <= 0) continue;/* skip if i is dead */
-      nv[i] = nvi;                      /* restore nv[i] */
-      d = degree[i] + dk - nvi;         /* compute external degree(i) */
-      d = std::min<StorageIndex> (d, n - nel - nvi);
-      if(head[d] != -1) last[head[d]] = i;
-      next[i] = head[d];               /* put i back in degree list */
+      if ((nvi = -nv[i]) <= 0) continue; /* skip if i is dead */
+      nv[i] = nvi;                       /* restore nv[i] */
+      d = degree[i] + dk - nvi;          /* compute external degree(i) */
+      d = std::min<StorageIndex>(d, n - nel - nvi);
+      if (head[d] != -1) last[head[d]] = i;
+      next[i] = head[d]; /* put i back in degree list */
       last[i] = -1;
       head[d] = i;
-      mindeg = std::min<StorageIndex> (mindeg, d);       /* find new minimum degree */
+      mindeg = std::min<StorageIndex>(mindeg, d); /* find new minimum degree */
       degree[i] = d;
-      Ci[p++] = i;                      /* place i in Lk */
+      Ci[p++] = i; /* place i in Lk */
     }
-    nv[k] = nvk;                      /* # nodes absorbed into k */
-    if((len[k] = p-pk1) == 0)         /* length of adj list of element k*/
+    nv[k] = nvk;                 /* # nodes absorbed into k */
+    if ((len[k] = p - pk1) == 0) /* length of adj list of element k*/
     {
-      Cp[k] = -1;                   /* k is a root of the tree */
-      w[k] = 0;                     /* k is now a dead element */
+      Cp[k] = -1; /* k is a root of the tree */
+      w[k] = 0;   /* k is now a dead element */
     }
-    if(elenk != 0) cnz = p;           /* free unused space in Lk */
+    if (elenk != 0) cnz = p; /* free unused space in Lk */
   }
-  
+
   /* --- Postordering ----------------------------------------------------- */
-  for(i = 0; i < n; i++) Cp[i] = amd_flip (Cp[i]);/* fix assembly tree */
-  for(j = 0; j <= n; j++) head[j] = -1;
-  for(j = n; j >= 0; j--)              /* place unordered nodes in lists */
+  for (i = 0; i < n; i++) Cp[i] = amd_flip(Cp[i]); /* fix assembly tree */
+  for (j = 0; j <= n; j++) head[j] = -1;
+  for (j = n; j >= 0; j--) /* place unordered nodes in lists */
   {
-    if(nv[j] > 0) continue;          /* skip if j is an element */
-    next[j] = head[Cp[j]];          /* place j in list of its parent */
+    if (nv[j] > 0) continue; /* skip if j is an element */
+    next[j] = head[Cp[j]];   /* place j in list of its parent */
     head[Cp[j]] = j;
   }
-  for(e = n; e >= 0; e--)              /* place elements in lists */
+  for (e = n; e >= 0; e--) /* place elements in lists */
   {
-    if(nv[e] <= 0) continue;         /* skip unless e is an element */
-    if(Cp[e] != -1)
-    {
-      next[e] = head[Cp[e]];      /* place e in list of its parent */
+    if (nv[e] <= 0) continue; /* skip unless e is an element */
+    if (Cp[e] != -1) {
+      next[e] = head[Cp[e]]; /* place e in list of its parent */
       head[Cp[e]] = e;
     }
   }
-  for(k = 0, i = 0; i <= n; i++)       /* postorder the assembly tree */
+  for (k = 0, i = 0; i <= n; i++) /* postorder the assembly tree */
   {
-    if(Cp[i] == -1) k = internal::cs_tdfs<StorageIndex>(i, k, head, next, perm.indices().data(), w);
+    if (Cp[i] == -1) k = internal::cs_tdfs<StorageIndex>(i, k, head, next, perm.indices().data(), w);
   }
-  
+
   perm.indices().conservativeResize(n);
 }
 
-} // namespace internal
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_AMD_H
+#endif  // EIGEN_SPARSE_AMD_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h
index 8e339a7..7bce3d5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h
@@ -43,7 +43,6 @@
 //
 //       http://www.suitesparse.com
 
-
 #ifndef EIGEN_COLAMD_H
 #define EIGEN_COLAMD_H
 
@@ -56,7 +55,6 @@
 #define COLAMD_NDEBUG
 #endif /* NDEBUG */
 
-
 /* ========================================================================== */
 /* === Knob and statistics definitions ====================================== */
 /* ========================================================================== */
@@ -116,16 +114,10 @@
 const int Empty = -1;
 
 /* Row and column status */
-enum RowColumnStatus {
-  Alive = 0,
-  Dead = -1
-};
+enum RowColumnStatus { Alive = 0, Dead = -1 };
 
 /* Column status */
-enum ColumnStatus {
-  DeadPrincipal = -1,
-  DeadNonPrincipal = -2
-};
+enum ColumnStatus { DeadPrincipal = -1, DeadNonPrincipal = -2 };
 
 /* ========================================================================== */
 /* === Colamd reporting mechanism =========================================== */
@@ -133,36 +125,31 @@
 
 // == Row and Column structures ==
 template <typename IndexType>
-struct ColStructure
-{
-  IndexType start ;   /* index for A of first row in this column, or Dead */
+struct ColStructure {
+  IndexType start; /* index for A of first row in this column, or Dead */
   /* if column is dead */
-  IndexType length ;  /* number of rows in this column */
-  union
-  {
-    IndexType thickness ; /* number of original columns represented by this */
+  IndexType length; /* number of rows in this column */
+  union {
+    IndexType thickness; /* number of original columns represented by this */
     /* col, if the column is alive */
-    IndexType parent ;  /* parent in parent tree super-column structure, if */
+    IndexType parent; /* parent in parent tree super-column structure, if */
     /* the column is dead */
-  } shared1 ;
-  union
-  {
-    IndexType score ; /* the score used to maintain heap, if col is alive */
-    IndexType order ; /* pivot ordering of this column, if col is dead */
-  } shared2 ;
-  union
-  {
-    IndexType headhash ;  /* head of a hash bucket, if col is at the head of */
+  } shared1;
+  union {
+    IndexType score; /* the score used to maintain heap, if col is alive */
+    IndexType order; /* pivot ordering of this column, if col is dead */
+  } shared2;
+  union {
+    IndexType headhash; /* head of a hash bucket, if col is at the head of */
     /* a degree list */
-    IndexType hash ;  /* hash value, if col is not in a degree list */
-    IndexType prev ;  /* previous column in degree list, if col is in a */
+    IndexType hash; /* hash value, if col is not in a degree list */
+    IndexType prev; /* previous column in degree list, if col is in a */
     /* degree list (but not at the head of a degree list) */
-  } shared3 ;
-  union
-  {
-    IndexType degree_next ; /* next column, if col is in a degree list */
-    IndexType hash_next ;   /* next column, if col is in a hash list */
-  } shared4 ;
+  } shared3;
+  union {
+    IndexType degree_next; /* next column, if col is in a degree list */
+    IndexType hash_next;   /* next column, if col is in a hash list */
+  } shared4;
 
   inline bool is_dead() const { return start < Alive; }
 
@@ -173,31 +160,26 @@
   inline void kill_principal() { start = DeadPrincipal; }
 
   inline void kill_non_principal() { start = DeadNonPrincipal; }
-
 };
 
 template <typename IndexType>
-struct RowStructure
-{
-  IndexType start ;   /* index for A of first col in this row */
-  IndexType length ;  /* number of principal columns in this row */
-  union
-  {
-    IndexType degree ;  /* number of principal & non-principal columns in row */
-    IndexType p ;   /* used as a row pointer in init_rows_cols () */
-  } shared1 ;
-  union
-  {
-    IndexType mark ;  /* for computing set differences and marking dead rows*/
-    IndexType first_column ;/* first column in row (used in garbage collection) */
-  } shared2 ;
+struct RowStructure {
+  IndexType start;  /* index for A of first col in this row */
+  IndexType length; /* number of principal columns in this row */
+  union {
+    IndexType degree; /* number of principal & non-principal columns in row */
+    IndexType p;      /* used as a row pointer in init_rows_cols () */
+  } shared1;
+  union {
+    IndexType mark;         /* for computing set differences and marking dead rows*/
+    IndexType first_column; /* first column in row (used in garbage collection) */
+  } shared2;
 
   inline bool is_dead() const { return shared2.mark < Alive; }
 
   inline bool is_alive() const { return shared2.mark >= Alive; }
 
   inline void kill() { shared2.mark = Dead; }
-
 };
 
 /* ========================================================================== */
@@ -219,34 +201,43 @@
   gcc -pedantic warning messages.
 */
 template <typename IndexType>
-inline IndexType colamd_c(IndexType n_col)
-{ return IndexType( ((n_col) + 1) * sizeof (ColStructure<IndexType>) / sizeof (IndexType) ) ; }
+inline IndexType colamd_c(IndexType n_col) {
+  return IndexType(((n_col) + 1) * sizeof(ColStructure<IndexType>) / sizeof(IndexType));
+}
 
 template <typename IndexType>
-inline IndexType  colamd_r(IndexType n_row)
-{ return IndexType(((n_row) + 1) * sizeof (RowStructure<IndexType>) / sizeof (IndexType)); }
+inline IndexType colamd_r(IndexType n_row) {
+  return IndexType(((n_row) + 1) * sizeof(RowStructure<IndexType>) / sizeof(IndexType));
+}
 
 // Prototypes of non-user callable routines
 template <typename IndexType>
-static IndexType init_rows_cols (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> col [], IndexType A [], IndexType p [], IndexType stats[NStats] );
+static IndexType init_rows_cols(IndexType n_row, IndexType n_col, RowStructure<IndexType> Row[],
+                                ColStructure<IndexType> col[], IndexType A[], IndexType p[], IndexType stats[NStats]);
 
 template <typename IndexType>
-static void init_scoring (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType head [], double knobs[NKnobs], IndexType *p_n_row2, IndexType *p_n_col2, IndexType *p_max_deg);
+static void init_scoring(IndexType n_row, IndexType n_col, RowStructure<IndexType> Row[], ColStructure<IndexType> Col[],
+                         IndexType A[], IndexType head[], double knobs[NKnobs], IndexType *p_n_row2,
+                         IndexType *p_n_col2, IndexType *p_max_deg);
 
 template <typename IndexType>
-static IndexType find_ordering (IndexType n_row, IndexType n_col, IndexType Alen, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType head [], IndexType n_col2, IndexType max_deg, IndexType pfree);
+static IndexType find_ordering(IndexType n_row, IndexType n_col, IndexType Alen, RowStructure<IndexType> Row[],
+                               ColStructure<IndexType> Col[], IndexType A[], IndexType head[], IndexType n_col2,
+                               IndexType max_deg, IndexType pfree);
 
 template <typename IndexType>
-static void order_children (IndexType n_col, ColStructure<IndexType> Col [], IndexType p []);
+static void order_children(IndexType n_col, ColStructure<IndexType> Col[], IndexType p[]);
 
 template <typename IndexType>
-static void detect_super_cols (ColStructure<IndexType> Col [], IndexType A [], IndexType head [], IndexType row_start, IndexType row_length ) ;
+static void detect_super_cols(ColStructure<IndexType> Col[], IndexType A[], IndexType head[], IndexType row_start,
+                              IndexType row_length);
 
 template <typename IndexType>
-static IndexType garbage_collection (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType *pfree) ;
+static IndexType garbage_collection(IndexType n_row, IndexType n_col, RowStructure<IndexType> Row[],
+                                    ColStructure<IndexType> Col[], IndexType A[], IndexType *pfree);
 
 template <typename IndexType>
-static inline  IndexType clear_mark (IndexType n_row, RowStructure<IndexType> Row [] ) ;
+static inline IndexType clear_mark(IndexType n_row, RowStructure<IndexType> Row[]);
 
 /* === No debugging ========================================================= */
 
@@ -256,8 +247,7 @@
 #define COLAMD_DEBUG3(params) ;
 #define COLAMD_DEBUG4(params) ;
 
-#define COLAMD_ASSERT(expression) ((void) 0)
-
+#define COLAMD_ASSERT(expression) ((void)0)
 
 /**
  * \brief Returns the recommended value of Alen
@@ -274,12 +264,11 @@
  * \return recommended value of Alen for use by colamd
  */
 template <typename IndexType>
-inline IndexType recommended ( IndexType nnz, IndexType n_row, IndexType n_col)
-{
+inline IndexType recommended(IndexType nnz, IndexType n_row, IndexType n_col) {
   if ((nnz) < 0 || (n_row) < 0 || (n_col) < 0)
     return (-1);
   else
-    return (2 * (nnz) + colamd_c (n_col) + colamd_r (n_row) + (n_col) + ((nnz) / 5));
+    return (2 * (nnz) + colamd_c(n_col) + colamd_r(n_row) + (n_col) + ((nnz) / 5));
 }
 
 /**
@@ -303,22 +292,19 @@
  * \param knobs parameter settings for colamd
  */
 
-static inline void set_defaults(double knobs[NKnobs])
-{
+static inline void set_defaults(double knobs[NKnobs]) {
   /* === Local variables ================================================== */
 
-  int i ;
+  int i;
 
-  if (!knobs)
-  {
-    return ;      /* no knobs to initialize */
+  if (!knobs) {
+    return; /* no knobs to initialize */
   }
-  for (i = 0 ; i < NKnobs ; i++)
-  {
-    knobs [i] = 0 ;
+  for (i = 0; i < NKnobs; i++) {
+    knobs[i] = 0;
   }
-  knobs [Colamd::DenseRow] = 0.5 ;  /* ignore rows over 50% dense */
-  knobs [Colamd::DenseCol] = 0.5 ;  /* ignore columns over 50% dense */
+  knobs[Colamd::DenseRow] = 0.5; /* ignore rows over 50% dense */
+  knobs[Colamd::DenseCol] = 0.5; /* ignore columns over 50% dense */
 }
 
 /**
@@ -339,144 +325,135 @@
  * \param stats colamd output statistics and error codes
  */
 template <typename IndexType>
-static bool compute_ordering(IndexType n_row, IndexType n_col, IndexType Alen, IndexType *A, IndexType *p, double knobs[NKnobs], IndexType stats[NStats])
-{
+static bool compute_ordering(IndexType n_row, IndexType n_col, IndexType Alen, IndexType *A, IndexType *p,
+                             double knobs[NKnobs], IndexType stats[NStats]) {
   /* === Local variables ================================================== */
 
-  IndexType i ;     /* loop index */
-  IndexType nnz ;     /* nonzeros in A */
-  IndexType Row_size ;    /* size of Row [], in integers */
-  IndexType Col_size ;    /* size of Col [], in integers */
-  IndexType need ;      /* minimum required length of A */
-  Colamd::RowStructure<IndexType> *Row ;   /* pointer into A of Row [0..n_row] array */
-  Colamd::ColStructure<IndexType> *Col ;   /* pointer into A of Col [0..n_col] array */
-  IndexType n_col2 ;    /* number of non-dense, non-empty columns */
-  IndexType n_row2 ;    /* number of non-dense, non-empty rows */
-  IndexType ngarbage ;    /* number of garbage collections performed */
-  IndexType max_deg ;   /* maximum row degree */
-  double default_knobs [NKnobs] ; /* default knobs array */
-
+  IndexType i;                          /* loop index */
+  IndexType nnz;                        /* nonzeros in A */
+  IndexType Row_size;                   /* size of Row [], in integers */
+  IndexType Col_size;                   /* size of Col [], in integers */
+  IndexType need;                       /* minimum required length of A */
+  Colamd::RowStructure<IndexType> *Row; /* pointer into A of Row [0..n_row] array */
+  Colamd::ColStructure<IndexType> *Col; /* pointer into A of Col [0..n_col] array */
+  IndexType n_col2;                     /* number of non-dense, non-empty columns */
+  IndexType n_row2;                     /* number of non-dense, non-empty rows */
+  IndexType ngarbage;                   /* number of garbage collections performed */
+  IndexType max_deg;                    /* maximum row degree */
+  double default_knobs[NKnobs];         /* default knobs array */
 
   /* === Check the input arguments ======================================== */
 
-  if (!stats)
-  {
-    COLAMD_DEBUG0 (("colamd: stats not present\n")) ;
-    return (false) ;
+  if (!stats) {
+    COLAMD_DEBUG0(("colamd: stats not present\n"));
+    return (false);
   }
-  for (i = 0 ; i < NStats ; i++)
-  {
-    stats [i] = 0 ;
+  for (i = 0; i < NStats; i++) {
+    stats[i] = 0;
   }
-  stats [Colamd::Status] = Colamd::Ok ;
-  stats [Colamd::Info1] = -1 ;
-  stats [Colamd::Info2] = -1 ;
+  stats[Colamd::Status] = Colamd::Ok;
+  stats[Colamd::Info1] = -1;
+  stats[Colamd::Info2] = -1;
 
-  if (!A)   /* A is not present */
+  if (!A) /* A is not present */
   {
-    stats [Colamd::Status] = Colamd::ErrorANotPresent ;
-    COLAMD_DEBUG0 (("colamd: A not present\n")) ;
-    return (false) ;
+    stats[Colamd::Status] = Colamd::ErrorANotPresent;
+    COLAMD_DEBUG0(("colamd: A not present\n"));
+    return (false);
   }
 
-  if (!p)   /* p is not present */
+  if (!p) /* p is not present */
   {
-    stats [Colamd::Status] = Colamd::ErrorPNotPresent ;
-    COLAMD_DEBUG0 (("colamd: p not present\n")) ;
-    return (false) ;
+    stats[Colamd::Status] = Colamd::ErrorPNotPresent;
+    COLAMD_DEBUG0(("colamd: p not present\n"));
+    return (false);
   }
 
-  if (n_row < 0)  /* n_row must be >= 0 */
+  if (n_row < 0) /* n_row must be >= 0 */
   {
-    stats [Colamd::Status] = Colamd::ErrorNrowNegative ;
-    stats [Colamd::Info1] = n_row ;
-    COLAMD_DEBUG0 (("colamd: nrow negative %d\n", n_row)) ;
-    return (false) ;
+    stats[Colamd::Status] = Colamd::ErrorNrowNegative;
+    stats[Colamd::Info1] = n_row;
+    COLAMD_DEBUG0(("colamd: nrow negative %d\n", n_row));
+    return (false);
   }
 
-  if (n_col < 0)  /* n_col must be >= 0 */
+  if (n_col < 0) /* n_col must be >= 0 */
   {
-    stats [Colamd::Status] = Colamd::ErrorNcolNegative ;
-    stats [Colamd::Info1] = n_col ;
-    COLAMD_DEBUG0 (("colamd: ncol negative %d\n", n_col)) ;
-    return (false) ;
+    stats[Colamd::Status] = Colamd::ErrorNcolNegative;
+    stats[Colamd::Info1] = n_col;
+    COLAMD_DEBUG0(("colamd: ncol negative %d\n", n_col));
+    return (false);
   }
 
-  nnz = p [n_col] ;
-  if (nnz < 0)  /* nnz must be >= 0 */
+  nnz = p[n_col];
+  if (nnz < 0) /* nnz must be >= 0 */
   {
-    stats [Colamd::Status] = Colamd::ErrorNnzNegative ;
-    stats [Colamd::Info1] = nnz ;
-    COLAMD_DEBUG0 (("colamd: number of entries negative %d\n", nnz)) ;
-    return (false) ;
+    stats[Colamd::Status] = Colamd::ErrorNnzNegative;
+    stats[Colamd::Info1] = nnz;
+    COLAMD_DEBUG0(("colamd: number of entries negative %d\n", nnz));
+    return (false);
   }
 
-  if (p [0] != 0)
-  {
-    stats [Colamd::Status] = Colamd::ErrorP0Nonzero ;
-    stats [Colamd::Info1] = p [0] ;
-    COLAMD_DEBUG0 (("colamd: p[0] not zero %d\n", p [0])) ;
-    return (false) ;
+  if (p[0] != 0) {
+    stats[Colamd::Status] = Colamd::ErrorP0Nonzero;
+    stats[Colamd::Info1] = p[0];
+    COLAMD_DEBUG0(("colamd: p[0] not zero %d\n", p[0]));
+    return (false);
   }
 
   /* === If no knobs, set default knobs =================================== */
 
-  if (!knobs)
-  {
-    set_defaults (default_knobs) ;
-    knobs = default_knobs ;
+  if (!knobs) {
+    set_defaults(default_knobs);
+    knobs = default_knobs;
   }
 
   /* === Allocate the Row and Col arrays from array A ===================== */
 
-  Col_size = colamd_c (n_col) ;
-  Row_size = colamd_r (n_row) ;
-  need = 2*nnz + n_col + Col_size + Row_size ;
+  Col_size = colamd_c(n_col);
+  Row_size = colamd_r(n_row);
+  need = 2 * nnz + n_col + Col_size + Row_size;
 
-  if (need > Alen)
-  {
+  if (need > Alen) {
     /* not enough space in array A to perform the ordering */
-    stats [Colamd::Status] = Colamd::ErrorATooSmall ;
-    stats [Colamd::Info1] = need ;
-    stats [Colamd::Info2] = Alen ;
-    COLAMD_DEBUG0 (("colamd: Need Alen >= %d, given only Alen = %d\n", need,Alen));
-    return (false) ;
+    stats[Colamd::Status] = Colamd::ErrorATooSmall;
+    stats[Colamd::Info1] = need;
+    stats[Colamd::Info2] = Alen;
+    COLAMD_DEBUG0(("colamd: Need Alen >= %d, given only Alen = %d\n", need, Alen));
+    return (false);
   }
 
-  Alen -= Col_size + Row_size ;
-  Col = (ColStructure<IndexType> *) &A [Alen] ;
-  Row = (RowStructure<IndexType> *) &A [Alen + Col_size] ;
+  Alen -= Col_size + Row_size;
+  Col = (ColStructure<IndexType> *)&A[Alen];
+  Row = (RowStructure<IndexType> *)&A[Alen + Col_size];
 
   /* === Construct the row and column data structures ===================== */
 
-  if (!Colamd::init_rows_cols (n_row, n_col, Row, Col, A, p, stats))
-  {
+  if (!Colamd::init_rows_cols(n_row, n_col, Row, Col, A, p, stats)) {
     /* input matrix is invalid */
-    COLAMD_DEBUG0 (("colamd: Matrix invalid\n")) ;
-    return (false) ;
+    COLAMD_DEBUG0(("colamd: Matrix invalid\n"));
+    return (false);
   }
 
   /* === Initialize scores, kill dense rows/columns ======================= */
 
-  Colamd::init_scoring (n_row, n_col, Row, Col, A, p, knobs,
-		&n_row2, &n_col2, &max_deg) ;
+  Colamd::init_scoring(n_row, n_col, Row, Col, A, p, knobs, &n_row2, &n_col2, &max_deg);
 
   /* === Order the supercolumns =========================================== */
 
-  ngarbage = Colamd::find_ordering (n_row, n_col, Alen, Row, Col, A, p,
-			    n_col2, max_deg, 2*nnz) ;
+  ngarbage = Colamd::find_ordering(n_row, n_col, Alen, Row, Col, A, p, n_col2, max_deg, 2 * nnz);
 
   /* === Order the non-principal columns ================================== */
 
-  Colamd::order_children (n_col, Col, p) ;
+  Colamd::order_children(n_col, Col, p);
 
   /* === Return statistics in stats ======================================= */
 
-  stats [Colamd::DenseRow] = n_row - n_row2 ;
-  stats [Colamd::DenseCol] = n_col - n_col2 ;
-  stats [Colamd::DefragCount] = ngarbage ;
-  COLAMD_DEBUG0 (("colamd: done.\n")) ;
-  return (true) ;
+  stats[Colamd::DenseRow] = n_row - n_row2;
+  stats[Colamd::DenseCol] = n_col - n_col2;
+  stats[Colamd::DefragCount] = ngarbage;
+  COLAMD_DEBUG0(("colamd: done.\n"));
+  return (true);
 }
 
 /* ========================================================================== */
@@ -498,112 +475,102 @@
   true otherwise.  Not user-callable.
 */
 template <typename IndexType>
-static IndexType init_rows_cols  /* returns true if OK, or false otherwise */
-  (
-    /* === Parameters ======================================================= */
+static IndexType init_rows_cols /* returns true if OK, or false otherwise */
+    (
+        /* === Parameters ======================================================= */
 
-    IndexType n_row,      /* number of rows of A */
-    IndexType n_col,      /* number of columns of A */
-    RowStructure<IndexType> Row [],    /* of size n_row+1 */
-    ColStructure<IndexType> Col [],    /* of size n_col+1 */
-    IndexType A [],     /* row indices of A, of size Alen */
-    IndexType p [],     /* pointers to columns in A, of size n_col+1 */
-    IndexType stats [NStats]  /* colamd statistics */
-    )
-{
+        IndexType n_row,               /* number of rows of A */
+        IndexType n_col,               /* number of columns of A */
+        RowStructure<IndexType> Row[], /* of size n_row+1 */
+        ColStructure<IndexType> Col[], /* of size n_col+1 */
+        IndexType A[],                 /* row indices of A, of size Alen */
+        IndexType p[],                 /* pointers to columns in A, of size n_col+1 */
+        IndexType stats[NStats]        /* colamd statistics */
+    ) {
   /* === Local variables ================================================== */
 
-  IndexType col ;     /* a column index */
-  IndexType row ;     /* a row index */
-  IndexType *cp ;     /* a column pointer */
-  IndexType *cp_end ;   /* a pointer to the end of a column */
-  IndexType *rp ;     /* a row pointer */
-  IndexType *rp_end ;   /* a pointer to the end of a row */
-  IndexType last_row ;    /* previous row */
+  IndexType col;      /* a column index */
+  IndexType row;      /* a row index */
+  IndexType *cp;      /* a column pointer */
+  IndexType *cp_end;  /* a pointer to the end of a column */
+  IndexType *rp;      /* a row pointer */
+  IndexType *rp_end;  /* a pointer to the end of a row */
+  IndexType last_row; /* previous row */
 
   /* === Initialize columns, and check column pointers ==================== */
 
-  for (col = 0 ; col < n_col ; col++)
-  {
-    Col [col].start = p [col] ;
-    Col [col].length = p [col+1] - p [col] ;
+  for (col = 0; col < n_col; col++) {
+    Col[col].start = p[col];
+    Col[col].length = p[col + 1] - p[col];
 
-    if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200
+    if ((Col[col].length) < 0)  // extra parentheses to work-around gcc bug 10200
     {
       /* column pointers must be non-decreasing */
-      stats [Colamd::Status] = Colamd::ErrorColLengthNegative ;
-      stats [Colamd::Info1] = col ;
-      stats [Colamd::Info2] = Col [col].length ;
-      COLAMD_DEBUG0 (("colamd: col %d length %d < 0\n", col, Col [col].length)) ;
-      return (false) ;
+      stats[Colamd::Status] = Colamd::ErrorColLengthNegative;
+      stats[Colamd::Info1] = col;
+      stats[Colamd::Info2] = Col[col].length;
+      COLAMD_DEBUG0(("colamd: col %d length %d < 0\n", col, Col[col].length));
+      return (false);
     }
 
-    Col [col].shared1.thickness = 1 ;
-    Col [col].shared2.score = 0 ;
-    Col [col].shared3.prev = Empty ;
-    Col [col].shared4.degree_next = Empty ;
+    Col[col].shared1.thickness = 1;
+    Col[col].shared2.score = 0;
+    Col[col].shared3.prev = Empty;
+    Col[col].shared4.degree_next = Empty;
   }
 
   /* p [0..n_col] no longer needed, used as "head" in subsequent routines */
 
   /* === Scan columns, compute row degrees, and check row indices ========= */
 
-  stats [Info3] = 0 ;  /* number of duplicate or unsorted row indices*/
+  stats[Info3] = 0; /* number of duplicate or unsorted row indices*/
 
-  for (row = 0 ; row < n_row ; row++)
-  {
-    Row [row].length = 0 ;
-    Row [row].shared2.mark = -1 ;
+  for (row = 0; row < n_row; row++) {
+    Row[row].length = 0;
+    Row[row].shared2.mark = -1;
   }
 
-  for (col = 0 ; col < n_col ; col++)
-  {
-    last_row = -1 ;
+  for (col = 0; col < n_col; col++) {
+    last_row = -1;
 
-    cp = &A [p [col]] ;
-    cp_end = &A [p [col+1]] ;
+    cp = &A[p[col]];
+    cp_end = &A[p[col + 1]];
 
-    while (cp < cp_end)
-    {
-      row = *cp++ ;
+    while (cp < cp_end) {
+      row = *cp++;
 
       /* make sure row indices within range */
-      if (row < 0 || row >= n_row)
-      {
-	stats [Colamd::Status] = Colamd::ErrorRowIndexOutOfBounds ;
-	stats [Colamd::Info1] = col ;
-	stats [Colamd::Info2] = row ;
-	stats [Colamd::Info3] = n_row ;
-	COLAMD_DEBUG0 (("colamd: row %d col %d out of bounds\n", row, col)) ;
-	return (false) ;
+      if (row < 0 || row >= n_row) {
+        stats[Colamd::Status] = Colamd::ErrorRowIndexOutOfBounds;
+        stats[Colamd::Info1] = col;
+        stats[Colamd::Info2] = row;
+        stats[Colamd::Info3] = n_row;
+        COLAMD_DEBUG0(("colamd: row %d col %d out of bounds\n", row, col));
+        return (false);
       }
 
-      if (row <= last_row || Row [row].shared2.mark == col)
-      {
-	/* row index are unsorted or repeated (or both), thus col */
-	/* is jumbled.  This is a notice, not an error condition. */
-	stats [Colamd::Status] = Colamd::OkButJumbled ;
-	stats [Colamd::Info1] = col ;
-	stats [Colamd::Info2] = row ;
-	(stats [Colamd::Info3]) ++ ;
-	COLAMD_DEBUG1 (("colamd: row %d col %d unsorted/duplicate\n",row,col));
+      if (row <= last_row || Row[row].shared2.mark == col) {
+        /* row index are unsorted or repeated (or both), thus col */
+        /* is jumbled.  This is a notice, not an error condition. */
+        stats[Colamd::Status] = Colamd::OkButJumbled;
+        stats[Colamd::Info1] = col;
+        stats[Colamd::Info2] = row;
+        (stats[Colamd::Info3])++;
+        COLAMD_DEBUG1(("colamd: row %d col %d unsorted/duplicate\n", row, col));
       }
 
-      if (Row [row].shared2.mark != col)
-      {
-	Row [row].length++ ;
-      }
-      else
-      {
-	/* this is a repeated entry in the column, */
-	/* it will be removed */
-	Col [col].length-- ;
+      if (Row[row].shared2.mark != col) {
+        Row[row].length++;
+      } else {
+        /* this is a repeated entry in the column, */
+        /* it will be removed */
+        Col[col].length--;
       }
 
       /* mark the row as having been seen in this column */
-      Row [row].shared2.mark = col ;
+      Row[row].shared2.mark = col;
 
-      last_row = row ;
+      last_row = row;
     }
   }
 
@@ -611,64 +578,52 @@
 
   /* row form of the matrix starts directly after the column */
   /* form of matrix in A */
-  Row [0].start = p [n_col] ;
-  Row [0].shared1.p = Row [0].start ;
-  Row [0].shared2.mark = -1 ;
-  for (row = 1 ; row < n_row ; row++)
-  {
-    Row [row].start = Row [row-1].start + Row [row-1].length ;
-    Row [row].shared1.p = Row [row].start ;
-    Row [row].shared2.mark = -1 ;
+  Row[0].start = p[n_col];
+  Row[0].shared1.p = Row[0].start;
+  Row[0].shared2.mark = -1;
+  for (row = 1; row < n_row; row++) {
+    Row[row].start = Row[row - 1].start + Row[row - 1].length;
+    Row[row].shared1.p = Row[row].start;
+    Row[row].shared2.mark = -1;
   }
 
   /* === Create row form ================================================== */
 
-  if (stats [Status] == OkButJumbled)
-  {
+  if (stats[Status] == OkButJumbled) {
     /* if cols jumbled, watch for repeated row indices */
-    for (col = 0 ; col < n_col ; col++)
-    {
-      cp = &A [p [col]] ;
-      cp_end = &A [p [col+1]] ;
-      while (cp < cp_end)
-      {
-	row = *cp++ ;
-	if (Row [row].shared2.mark != col)
-	{
-	  A [(Row [row].shared1.p)++] = col ;
-	  Row [row].shared2.mark = col ;
-	}
+    for (col = 0; col < n_col; col++) {
+      cp = &A[p[col]];
+      cp_end = &A[p[col + 1]];
+      while (cp < cp_end) {
+        row = *cp++;
+        if (Row[row].shared2.mark != col) {
+          A[(Row[row].shared1.p)++] = col;
+          Row[row].shared2.mark = col;
+        }
       }
     }
-  }
-  else
-  {
+  } else {
     /* if cols not jumbled, we don't need the mark (this is faster) */
-    for (col = 0 ; col < n_col ; col++)
-    {
-      cp = &A [p [col]] ;
-      cp_end = &A [p [col+1]] ;
-      while (cp < cp_end)
-      {
-	A [(Row [*cp++].shared1.p)++] = col ;
+    for (col = 0; col < n_col; col++) {
+      cp = &A[p[col]];
+      cp_end = &A[p[col + 1]];
+      while (cp < cp_end) {
+        A[(Row[*cp++].shared1.p)++] = col;
       }
     }
   }
 
   /* === Clear the row marks and set row degrees ========================== */
 
-  for (row = 0 ; row < n_row ; row++)
-  {
-    Row [row].shared2.mark = 0 ;
-    Row [row].shared1.degree = Row [row].length ;
+  for (row = 0; row < n_row; row++) {
+    Row[row].shared2.mark = 0;
+    Row[row].shared1.degree = Row[row].length;
   }
 
   /* === See if we need to re-create columns ============================== */
 
-  if (stats [Status] == OkButJumbled)
-  {
-    COLAMD_DEBUG0 (("colamd: reconstructing column form, matrix jumbled\n")) ;
-
+  if (stats[Status] == OkButJumbled) {
+    COLAMD_DEBUG0(("colamd: reconstructing column form, matrix jumbled\n"));
 
     /* === Compute col pointers ========================================= */
 
@@ -676,35 +631,31 @@
     /* Note, we may have a gap between the col form and the row */
     /* form if there were duplicate entries, if so, it will be */
     /* removed upon the first garbage collection */
-    Col [0].start = 0 ;
-    p [0] = Col [0].start ;
-    for (col = 1 ; col < n_col ; col++)
-    {
+    Col[0].start = 0;
+    p[0] = Col[0].start;
+    for (col = 1; col < n_col; col++) {
       /* note that the lengths here are for pruned columns, i.e. */
       /* no duplicate row indices will exist for these columns */
-      Col [col].start = Col [col-1].start + Col [col-1].length ;
-      p [col] = Col [col].start ;
+      Col[col].start = Col[col - 1].start + Col[col - 1].length;
+      p[col] = Col[col].start;
     }
 
     /* === Re-create col form =========================================== */
 
-    for (row = 0 ; row < n_row ; row++)
-    {
-      rp = &A [Row [row].start] ;
-      rp_end = rp + Row [row].length ;
-      while (rp < rp_end)
-      {
-	A [(p [*rp++])++] = row ;
+    for (row = 0; row < n_row; row++) {
+      rp = &A[Row[row].start];
+      rp_end = rp + Row[row].length;
+      while (rp < rp_end) {
+        A[(p[*rp++])++] = row;
       }
     }
   }
 
   /* === Done.  Matrix is not (or no longer) jumbled ====================== */
 
-  return (true) ;
+  return (true);
 }
 
-
 /* ========================================================================== */
 /* === init_scoring ========================================================= */
 /* ========================================================================== */
@@ -714,112 +665,99 @@
   each column, and places all columns in the degree lists.  Not user-callable.
 */
 template <typename IndexType>
-static void init_scoring
-  (
+static void init_scoring(
     /* === Parameters ======================================================= */
 
-    IndexType n_row,      /* number of rows of A */
-    IndexType n_col,      /* number of columns of A */
-    RowStructure<IndexType> Row [],    /* of size n_row+1 */
-    ColStructure<IndexType> Col [],    /* of size n_col+1 */
-    IndexType A [],     /* column form and row form of A */
-    IndexType head [],    /* of size n_col+1 */
-    double knobs [NKnobs],/* parameters */
-    IndexType *p_n_row2,    /* number of non-dense, non-empty rows */
-    IndexType *p_n_col2,    /* number of non-dense, non-empty columns */
-    IndexType *p_max_deg    /* maximum row degree */
-    )
-{
+    IndexType n_row,               /* number of rows of A */
+    IndexType n_col,               /* number of columns of A */
+    RowStructure<IndexType> Row[], /* of size n_row+1 */
+    ColStructure<IndexType> Col[], /* of size n_col+1 */
+    IndexType A[],                 /* column form and row form of A */
+    IndexType head[],              /* of size n_col+1 */
+    double knobs[NKnobs],          /* parameters */
+    IndexType *p_n_row2,           /* number of non-dense, non-empty rows */
+    IndexType *p_n_col2,           /* number of non-dense, non-empty columns */
+    IndexType *p_max_deg           /* maximum row degree */
+) {
   /* === Local variables ================================================== */
 
-  IndexType c ;     /* a column index */
-  IndexType r, row ;    /* a row index */
-  IndexType *cp ;     /* a column pointer */
-  IndexType deg ;     /* degree of a row or column */
-  IndexType *cp_end ;   /* a pointer to the end of a column */
-  IndexType *new_cp ;   /* new column pointer */
-  IndexType col_length ;    /* length of pruned column */
-  IndexType score ;     /* current column score */
-  IndexType n_col2 ;    /* number of non-dense, non-empty columns */
-  IndexType n_row2 ;    /* number of non-dense, non-empty rows */
-  IndexType dense_row_count ; /* remove rows with more entries than this */
-  IndexType dense_col_count ; /* remove cols with more entries than this */
-  IndexType min_score ;   /* smallest column score */
-  IndexType max_deg ;   /* maximum row degree */
-  IndexType next_col ;    /* Used to add to degree list.*/
-
+  IndexType c;               /* a column index */
+  IndexType r, row;          /* a row index */
+  IndexType *cp;             /* a column pointer */
+  IndexType deg;             /* degree of a row or column */
+  IndexType *cp_end;         /* a pointer to the end of a column */
+  IndexType *new_cp;         /* new column pointer */
+  IndexType col_length;      /* length of pruned column */
+  IndexType score;           /* current column score */
+  IndexType n_col2;          /* number of non-dense, non-empty columns */
+  IndexType n_row2;          /* number of non-dense, non-empty rows */
+  IndexType dense_row_count; /* remove rows with more entries than this */
+  IndexType dense_col_count; /* remove cols with more entries than this */
+  IndexType min_score;       /* smallest column score */
+  IndexType max_deg;         /* maximum row degree */
+  IndexType next_col;        /* Used to add to degree list.*/
 
   /* === Extract knobs ==================================================== */
 
-  dense_row_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseRow] * n_col), n_col)) ;
-  dense_col_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseCol] * n_row), n_row)) ;
-  COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ;
-  max_deg = 0 ;
-  n_col2 = n_col ;
-  n_row2 = n_row ;
+  dense_row_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs[Colamd::DenseRow] * n_col), n_col));
+  dense_col_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs[Colamd::DenseCol] * n_row), n_row));
+  COLAMD_DEBUG1(("colamd: densecount: %d %d\n", dense_row_count, dense_col_count));
+  max_deg = 0;
+  n_col2 = n_col;
+  n_row2 = n_row;
 
   /* === Kill empty columns =============================================== */
 
   /* Put the empty columns at the end in their natural order, so that LU */
   /* factorization can proceed as far as possible. */
-  for (c = n_col-1 ; c >= 0 ; c--)
-  {
-    deg = Col [c].length ;
-    if (deg == 0)
-    {
+  for (c = n_col - 1; c >= 0; c--) {
+    deg = Col[c].length;
+    if (deg == 0) {
       /* this is a empty column, kill and order it last */
-      Col [c].shared2.order = --n_col2 ;
-      Col[c].kill_principal() ;
+      Col[c].shared2.order = --n_col2;
+      Col[c].kill_principal();
     }
   }
-  COLAMD_DEBUG1 (("colamd: null columns killed: %d\n", n_col - n_col2)) ;
+  COLAMD_DEBUG1(("colamd: null columns killed: %d\n", n_col - n_col2));
 
   /* === Kill dense columns =============================================== */
 
   /* Put the dense columns at the end, in their natural order */
-  for (c = n_col-1 ; c >= 0 ; c--)
-  {
+  for (c = n_col - 1; c >= 0; c--) {
     /* skip any dead columns */
-    if (Col[c].is_dead())
-    {
-      continue ;
+    if (Col[c].is_dead()) {
+      continue;
     }
-    deg = Col [c].length ;
-    if (deg > dense_col_count)
-    {
+    deg = Col[c].length;
+    if (deg > dense_col_count) {
       /* this is a dense column, kill and order it last */
-      Col [c].shared2.order = --n_col2 ;
+      Col[c].shared2.order = --n_col2;
       /* decrement the row degrees */
-      cp = &A [Col [c].start] ;
-      cp_end = cp + Col [c].length ;
-      while (cp < cp_end)
-      {
-	Row [*cp++].shared1.degree-- ;
+      cp = &A[Col[c].start];
+      cp_end = cp + Col[c].length;
+      while (cp < cp_end) {
+        Row[*cp++].shared1.degree--;
       }
-      Col[c].kill_principal() ;
+      Col[c].kill_principal();
     }
   }
-  COLAMD_DEBUG1 (("colamd: Dense and null columns killed: %d\n", n_col - n_col2)) ;
+  COLAMD_DEBUG1(("colamd: Dense and null columns killed: %d\n", n_col - n_col2));
 
   /* === Kill dense and empty rows ======================================== */
 
-  for (r = 0 ; r < n_row ; r++)
-  {
-    deg = Row [r].shared1.degree ;
-    COLAMD_ASSERT (deg >= 0 && deg <= n_col) ;
-    if (deg > dense_row_count || deg == 0)
-    {
+  for (r = 0; r < n_row; r++) {
+    deg = Row[r].shared1.degree;
+    COLAMD_ASSERT(deg >= 0 && deg <= n_col);
+    if (deg > dense_row_count || deg == 0) {
       /* kill a dense or empty row */
-      Row[r].kill() ;
-      --n_row2 ;
-    }
-    else
-    {
+      Row[r].kill();
+      --n_row2;
+    } else {
       /* keep track of max degree of remaining rows */
-      max_deg = numext::maxi(max_deg, deg) ;
+      max_deg = numext::maxi(max_deg, deg);
     }
   }
-  COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ;
+  COLAMD_DEBUG1(("colamd: Dense and null rows killed: %d\n", n_row - n_row2));
 
   /* === Compute initial column scores ==================================== */
 
@@ -829,54 +767,46 @@
   /* pruned in the code below. */
 
   /* now find the initial matlab score for each column */
-  for (c = n_col-1 ; c >= 0 ; c--)
-  {
+  for (c = n_col - 1; c >= 0; c--) {
     /* skip dead column */
-    if (Col[c].is_dead())
-    {
-      continue ;
+    if (Col[c].is_dead()) {
+      continue;
     }
-    score = 0 ;
-    cp = &A [Col [c].start] ;
-    new_cp = cp ;
-    cp_end = cp + Col [c].length ;
-    while (cp < cp_end)
-    {
+    score = 0;
+    cp = &A[Col[c].start];
+    new_cp = cp;
+    cp_end = cp + Col[c].length;
+    while (cp < cp_end) {
       /* get a row */
-      row = *cp++ ;
+      row = *cp++;
       /* skip if dead */
-      if (Row[row].is_dead())
-      {
-	continue ;
+      if (Row[row].is_dead()) {
+        continue;
       }
       /* compact the column */
-      *new_cp++ = row ;
+      *new_cp++ = row;
       /* add row's external degree */
-      score += Row [row].shared1.degree - 1 ;
+      score += Row[row].shared1.degree - 1;
       /* guard against integer overflow */
-      score = numext::mini(score, n_col) ;
+      score = numext::mini(score, n_col);
     }
     /* determine pruned column length */
-    col_length = (IndexType) (new_cp - &A [Col [c].start]) ;
-    if (col_length == 0)
-    {
+    col_length = (IndexType)(new_cp - &A[Col[c].start]);
+    if (col_length == 0) {
       /* a newly-made null column (all rows in this col are "dense" */
       /* and have already been killed) */
-      COLAMD_DEBUG2 (("Newly null killed: %d\n", c)) ;
-      Col [c].shared2.order = --n_col2 ;
-      Col[c].kill_principal() ;
-    }
-    else
-    {
+      COLAMD_DEBUG2(("Newly null killed: %d\n", c));
+      Col[c].shared2.order = --n_col2;
+      Col[c].kill_principal();
+    } else {
       /* set column length and set score */
-      COLAMD_ASSERT (score >= 0) ;
-      COLAMD_ASSERT (score <= n_col) ;
-      Col [c].length = col_length ;
-      Col [c].shared2.score = score ;
+      COLAMD_ASSERT(score >= 0);
+      COLAMD_ASSERT(score <= n_col);
+      Col[c].length = col_length;
+      Col[c].shared2.score = score;
     }
   }
-  COLAMD_DEBUG1 (("colamd: Dense, null, and newly-null columns killed: %d\n",
-		  n_col-n_col2)) ;
+  COLAMD_DEBUG1(("colamd: Dense, null, and newly-null columns killed: %d\n", n_col - n_col2));
 
   /* At this point, all empty rows and columns are dead.  All live columns */
   /* are "clean" (containing no dead rows) and simplicial (no supercolumns */
@@ -885,62 +815,52 @@
 
   /* === Initialize degree lists ========================================== */
 
-
   /* clear the hash buckets */
-  for (c = 0 ; c <= n_col ; c++)
-  {
-    head [c] = Empty ;
+  for (c = 0; c <= n_col; c++) {
+    head[c] = Empty;
   }
-  min_score = n_col ;
+  min_score = n_col;
   /* place in reverse order, so low column indices are at the front */
   /* of the lists.  This is to encourage natural tie-breaking */
-  for (c = n_col-1 ; c >= 0 ; c--)
-  {
+  for (c = n_col - 1; c >= 0; c--) {
     /* only add principal columns to degree lists */
-    if (Col[c].is_alive())
-    {
-      COLAMD_DEBUG4 (("place %d score %d minscore %d ncol %d\n",
-		      c, Col [c].shared2.score, min_score, n_col)) ;
+    if (Col[c].is_alive()) {
+      COLAMD_DEBUG4(("place %d score %d minscore %d ncol %d\n", c, Col[c].shared2.score, min_score, n_col));
 
       /* === Add columns score to DList =============================== */
 
-      score = Col [c].shared2.score ;
+      score = Col[c].shared2.score;
 
-      COLAMD_ASSERT (min_score >= 0) ;
-      COLAMD_ASSERT (min_score <= n_col) ;
-      COLAMD_ASSERT (score >= 0) ;
-      COLAMD_ASSERT (score <= n_col) ;
-      COLAMD_ASSERT (head [score] >= Empty) ;
+      COLAMD_ASSERT(min_score >= 0);
+      COLAMD_ASSERT(min_score <= n_col);
+      COLAMD_ASSERT(score >= 0);
+      COLAMD_ASSERT(score <= n_col);
+      COLAMD_ASSERT(head[score] >= Empty);
 
       /* now add this column to dList at proper score location */
-      next_col = head [score] ;
-      Col [c].shared3.prev = Empty ;
-      Col [c].shared4.degree_next = next_col ;
+      next_col = head[score];
+      Col[c].shared3.prev = Empty;
+      Col[c].shared4.degree_next = next_col;
 
       /* if there already was a column with the same score, set its */
       /* previous pointer to this new column */
-      if (next_col != Empty)
-      {
-	Col [next_col].shared3.prev = c ;
+      if (next_col != Empty) {
+        Col[next_col].shared3.prev = c;
       }
-      head [score] = c ;
+      head[score] = c;
 
       /* see if this score is less than current min */
-      min_score = numext::mini(min_score, score) ;
-
-
+      min_score = numext::mini(min_score, score);
     }
   }
 
-
   /* === Return number of remaining columns, and max row degree =========== */
 
-  *p_n_col2 = n_col2 ;
-  *p_n_row2 = n_row2 ;
-  *p_max_deg = max_deg ;
+  *p_n_col2 = n_col2;
+  *p_n_row2 = n_row2;
+  *p_max_deg = max_deg;
 }
 
-
 /* ========================================================================== */
 /* === find_ordering ======================================================== */
 /* ========================================================================== */
@@ -952,197 +872,180 @@
 */
 template <typename IndexType>
 static IndexType find_ordering /* return the number of garbage collections */
-  (
-    /* === Parameters ======================================================= */
+    (
+        /* === Parameters ======================================================= */
 
-    IndexType n_row,      /* number of rows of A */
-    IndexType n_col,      /* number of columns of A */
-    IndexType Alen,     /* size of A, 2*nnz + n_col or larger */
-    RowStructure<IndexType> Row [],    /* of size n_row+1 */
-    ColStructure<IndexType> Col [],    /* of size n_col+1 */
-    IndexType A [],     /* column form and row form of A */
-    IndexType head [],    /* of size n_col+1 */
-    IndexType n_col2,     /* Remaining columns to order */
-    IndexType max_deg,    /* Maximum row degree */
-    IndexType pfree     /* index of first free slot (2*nnz on entry) */
-    )
-{
+        IndexType n_row,               /* number of rows of A */
+        IndexType n_col,               /* number of columns of A */
+        IndexType Alen,                /* size of A, 2*nnz + n_col or larger */
+        RowStructure<IndexType> Row[], /* of size n_row+1 */
+        ColStructure<IndexType> Col[], /* of size n_col+1 */
+        IndexType A[],                 /* column form and row form of A */
+        IndexType head[],              /* of size n_col+1 */
+        IndexType n_col2,              /* Remaining columns to order */
+        IndexType max_deg,             /* Maximum row degree */
+        IndexType pfree                /* index of first free slot (2*nnz on entry) */
+    ) {
   /* === Local variables ================================================== */
 
-  IndexType k ;     /* current pivot ordering step */
-  IndexType pivot_col ;   /* current pivot column */
-  IndexType *cp ;     /* a column pointer */
-  IndexType *rp ;     /* a row pointer */
-  IndexType pivot_row ;   /* current pivot row */
-  IndexType *new_cp ;   /* modified column pointer */
-  IndexType *new_rp ;   /* modified row pointer */
-  IndexType pivot_row_start ; /* pointer to start of pivot row */
-  IndexType pivot_row_degree ;  /* number of columns in pivot row */
-  IndexType pivot_row_length ;  /* number of supercolumns in pivot row */
-  IndexType pivot_col_score ; /* score of pivot column */
-  IndexType needed_memory ;   /* free space needed for pivot row */
-  IndexType *cp_end ;   /* pointer to the end of a column */
-  IndexType *rp_end ;   /* pointer to the end of a row */
-  IndexType row ;     /* a row index */
-  IndexType col ;     /* a column index */
-  IndexType max_score ;   /* maximum possible score */
-  IndexType cur_score ;   /* score of current column */
-  unsigned int hash ;   /* hash value for supernode detection */
-  IndexType head_column ;   /* head of hash bucket */
-  IndexType first_col ;   /* first column in hash bucket */
-  IndexType tag_mark ;    /* marker value for mark array */
-  IndexType row_mark ;    /* Row [row].shared2.mark */
-  IndexType set_difference ;  /* set difference size of row with pivot row */
-  IndexType min_score ;   /* smallest column score */
-  IndexType col_thickness ;   /* "thickness" (no. of columns in a supercol) */
-  IndexType max_mark ;    /* maximum value of tag_mark */
-  IndexType pivot_col_thickness ; /* number of columns represented by pivot col */
-  IndexType prev_col ;    /* Used by Dlist operations. */
-  IndexType next_col ;    /* Used by Dlist operations. */
-  IndexType ngarbage ;    /* number of garbage collections performed */
-
+  IndexType k;                   /* current pivot ordering step */
+  IndexType pivot_col;           /* current pivot column */
+  IndexType *cp;                 /* a column pointer */
+  IndexType *rp;                 /* a row pointer */
+  IndexType pivot_row;           /* current pivot row */
+  IndexType *new_cp;             /* modified column pointer */
+  IndexType *new_rp;             /* modified row pointer */
+  IndexType pivot_row_start;     /* pointer to start of pivot row */
+  IndexType pivot_row_degree;    /* number of columns in pivot row */
+  IndexType pivot_row_length;    /* number of supercolumns in pivot row */
+  IndexType pivot_col_score;     /* score of pivot column */
+  IndexType needed_memory;       /* free space needed for pivot row */
+  IndexType *cp_end;             /* pointer to the end of a column */
+  IndexType *rp_end;             /* pointer to the end of a row */
+  IndexType row;                 /* a row index */
+  IndexType col;                 /* a column index */
+  IndexType max_score;           /* maximum possible score */
+  IndexType cur_score;           /* score of current column */
+  unsigned int hash;             /* hash value for supernode detection */
+  IndexType head_column;         /* head of hash bucket */
+  IndexType first_col;           /* first column in hash bucket */
+  IndexType tag_mark;            /* marker value for mark array */
+  IndexType row_mark;            /* Row [row].shared2.mark */
+  IndexType set_difference;      /* set difference size of row with pivot row */
+  IndexType min_score;           /* smallest column score */
+  IndexType col_thickness;       /* "thickness" (no. of columns in a supercol) */
+  IndexType max_mark;            /* maximum value of tag_mark */
+  IndexType pivot_col_thickness; /* number of columns represented by pivot col */
+  IndexType prev_col;            /* Used by Dlist operations. */
+  IndexType next_col;            /* Used by Dlist operations. */
+  IndexType ngarbage;            /* number of garbage collections performed */
 
   /* === Initialization and clear mark ==================================== */
 
-  max_mark = INT_MAX - n_col ;  /* INT_MAX defined in <limits.h> */
-  tag_mark = Colamd::clear_mark (n_row, Row) ;
-  min_score = 0 ;
-  ngarbage = 0 ;
-  COLAMD_DEBUG1 (("colamd: Ordering, n_col2=%d\n", n_col2)) ;
+  max_mark = INT_MAX - n_col; /* INT_MAX defined in <limits.h> */
+  tag_mark = Colamd::clear_mark(n_row, Row);
+  min_score = 0;
+  ngarbage = 0;
+  COLAMD_DEBUG1(("colamd: Ordering, n_col2=%d\n", n_col2));
 
   /* === Order the columns ================================================ */
 
-  for (k = 0 ; k < n_col2 ; /* 'k' is incremented below */)
-  {
-
+  for (k = 0; k < n_col2; /* 'k' is incremented below */) {
     /* === Select pivot column, and order it ============================ */
 
     /* make sure degree list isn't empty */
-    COLAMD_ASSERT (min_score >= 0) ;
-    COLAMD_ASSERT (min_score <= n_col) ;
-    COLAMD_ASSERT (head [min_score] >= Empty) ;
+    COLAMD_ASSERT(min_score >= 0);
+    COLAMD_ASSERT(min_score <= n_col);
+    COLAMD_ASSERT(head[min_score] >= Empty);
 
     /* get pivot column from head of minimum degree list */
-    while (min_score < n_col && head [min_score] == Empty)
-    {
-      min_score++ ;
+    while (min_score < n_col && head[min_score] == Empty) {
+      min_score++;
     }
-    pivot_col = head [min_score] ;
-    COLAMD_ASSERT (pivot_col >= 0 && pivot_col <= n_col) ;
-    next_col = Col [pivot_col].shared4.degree_next ;
-    head [min_score] = next_col ;
-    if (next_col != Empty)
-    {
-      Col [next_col].shared3.prev = Empty ;
+    pivot_col = head[min_score];
+    COLAMD_ASSERT(pivot_col >= 0 && pivot_col <= n_col);
+    next_col = Col[pivot_col].shared4.degree_next;
+    head[min_score] = next_col;
+    if (next_col != Empty) {
+      Col[next_col].shared3.prev = Empty;
     }
 
-    COLAMD_ASSERT (Col[pivot_col].is_alive()) ;
-    COLAMD_DEBUG3 (("Pivot col: %d\n", pivot_col)) ;
+    COLAMD_ASSERT(Col[pivot_col].is_alive());
+    COLAMD_DEBUG3(("Pivot col: %d\n", pivot_col));
 
     /* remember score for defrag check */
-    pivot_col_score = Col [pivot_col].shared2.score ;
+    pivot_col_score = Col[pivot_col].shared2.score;
 
     /* the pivot column is the kth column in the pivot order */
-    Col [pivot_col].shared2.order = k ;
+    Col[pivot_col].shared2.order = k;
 
     /* increment order count by column thickness */
-    pivot_col_thickness = Col [pivot_col].shared1.thickness ;
-    k += pivot_col_thickness ;
-    COLAMD_ASSERT (pivot_col_thickness > 0) ;
+    pivot_col_thickness = Col[pivot_col].shared1.thickness;
+    k += pivot_col_thickness;
+    COLAMD_ASSERT(pivot_col_thickness > 0);
 
     /* === Garbage_collection, if necessary ============================= */
 
-    needed_memory = numext::mini(pivot_col_score, n_col - k) ;
-    if (pfree + needed_memory >= Alen)
-    {
-      pfree = Colamd::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ;
-      ngarbage++ ;
+    needed_memory = numext::mini(pivot_col_score, n_col - k);
+    if (pfree + needed_memory >= Alen) {
+      pfree = Colamd::garbage_collection(n_row, n_col, Row, Col, A, &A[pfree]);
+      ngarbage++;
       /* after garbage collection we will have enough */
-      COLAMD_ASSERT (pfree + needed_memory < Alen) ;
+      COLAMD_ASSERT(pfree + needed_memory < Alen);
       /* garbage collection has wiped out the Row[].shared2.mark array */
-      tag_mark = Colamd::clear_mark (n_row, Row) ;
-
+      tag_mark = Colamd::clear_mark(n_row, Row);
     }
 
     /* === Compute pivot row pattern ==================================== */
 
     /* get starting location for this new merged row */
-    pivot_row_start = pfree ;
+    pivot_row_start = pfree;
 
     /* initialize new row counts to zero */
-    pivot_row_degree = 0 ;
+    pivot_row_degree = 0;
 
     /* tag pivot column as having been visited so it isn't included */
     /* in merged pivot row */
-    Col [pivot_col].shared1.thickness = -pivot_col_thickness ;
+    Col[pivot_col].shared1.thickness = -pivot_col_thickness;
 
     /* pivot row is the union of all rows in the pivot column pattern */
-    cp = &A [Col [pivot_col].start] ;
-    cp_end = cp + Col [pivot_col].length ;
-    while (cp < cp_end)
-    {
+    cp = &A[Col[pivot_col].start];
+    cp_end = cp + Col[pivot_col].length;
+    while (cp < cp_end) {
       /* get a row */
-      row = *cp++ ;
-      COLAMD_DEBUG4 (("Pivot col pattern %d %d\n", Row[row].is_alive(), row)) ;
+      row = *cp++;
+      COLAMD_DEBUG4(("Pivot col pattern %d %d\n", Row[row].is_alive(), row));
       /* skip if row is dead */
-      if (Row[row].is_dead())
-      {
-	continue ;
+      if (Row[row].is_dead()) {
+        continue;
       }
-      rp = &A [Row [row].start] ;
-      rp_end = rp + Row [row].length ;
-      while (rp < rp_end)
-      {
-	/* get a column */
-	col = *rp++ ;
-	/* add the column, if alive and untagged */
-	col_thickness = Col [col].shared1.thickness ;
-	if (col_thickness > 0 && Col[col].is_alive())
-	{
-	  /* tag column in pivot row */
-	  Col [col].shared1.thickness = -col_thickness ;
-	  COLAMD_ASSERT (pfree < Alen) ;
-	  /* place column in pivot row */
-	  A [pfree++] = col ;
-	  pivot_row_degree += col_thickness ;
-	}
+      rp = &A[Row[row].start];
+      rp_end = rp + Row[row].length;
+      while (rp < rp_end) {
+        /* get a column */
+        col = *rp++;
+        /* add the column, if alive and untagged */
+        col_thickness = Col[col].shared1.thickness;
+        if (col_thickness > 0 && Col[col].is_alive()) {
+          /* tag column in pivot row */
+          Col[col].shared1.thickness = -col_thickness;
+          COLAMD_ASSERT(pfree < Alen);
+          /* place column in pivot row */
+          A[pfree++] = col;
+          pivot_row_degree += col_thickness;
+        }
       }
     }
 
     /* clear tag on pivot column */
-    Col [pivot_col].shared1.thickness = pivot_col_thickness ;
-    max_deg = numext::maxi(max_deg, pivot_row_degree) ;
-
+    Col[pivot_col].shared1.thickness = pivot_col_thickness;
+    max_deg = numext::maxi(max_deg, pivot_row_degree);
 
     /* === Kill all rows used to construct pivot row ==================== */
 
     /* also kill pivot row, temporarily */
-    cp = &A [Col [pivot_col].start] ;
-    cp_end = cp + Col [pivot_col].length ;
-    while (cp < cp_end)
-    {
+    cp = &A[Col[pivot_col].start];
+    cp_end = cp + Col[pivot_col].length;
+    while (cp < cp_end) {
       /* may be killing an already dead row */
-      row = *cp++ ;
-      COLAMD_DEBUG3 (("Kill row in pivot col: %d\n", row)) ;
-      Row[row].kill() ;
+      row = *cp++;
+      COLAMD_DEBUG3(("Kill row in pivot col: %d\n", row));
+      Row[row].kill();
     }
 
     /* === Select a row index to use as the new pivot row =============== */
 
-    pivot_row_length = pfree - pivot_row_start ;
-    if (pivot_row_length > 0)
-    {
+    pivot_row_length = pfree - pivot_row_start;
+    if (pivot_row_length > 0) {
       /* pick the "pivot" row arbitrarily (first row in col) */
-      pivot_row = A [Col [pivot_col].start] ;
-      COLAMD_DEBUG3 (("Pivotal row is %d\n", pivot_row)) ;
-    }
-    else
-    {
+      pivot_row = A[Col[pivot_col].start];
+      COLAMD_DEBUG3(("Pivotal row is %d\n", pivot_row));
+    } else {
       /* there is no pivot row, since it is of zero length */
-      pivot_row = Empty ;
-      COLAMD_ASSERT (pivot_row_length == 0) ;
+      pivot_row = Empty;
+      COLAMD_ASSERT(pivot_row_length == 0);
     }
-    COLAMD_ASSERT (Col [pivot_col].length > 0 || pivot_row_length == 0) ;
+    COLAMD_ASSERT(Col[pivot_col].length > 0 || pivot_row_length == 0);
 
     /* === Approximate degree computation =============================== */
 
@@ -1165,180 +1068,159 @@
 
     /* === Compute set differences ====================================== */
 
-    COLAMD_DEBUG3 (("** Computing set differences phase. **\n")) ;
+    COLAMD_DEBUG3(("** Computing set differences phase. **\n"));
 
     /* pivot row is currently dead - it will be revived later. */
 
-    COLAMD_DEBUG3 (("Pivot row: ")) ;
+    COLAMD_DEBUG3(("Pivot row: "));
     /* for each column in pivot row */
-    rp = &A [pivot_row_start] ;
-    rp_end = rp + pivot_row_length ;
-    while (rp < rp_end)
-    {
-      col = *rp++ ;
-      COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ;
-      COLAMD_DEBUG3 (("Col: %d\n", col)) ;
+    rp = &A[pivot_row_start];
+    rp_end = rp + pivot_row_length;
+    while (rp < rp_end) {
+      col = *rp++;
+      COLAMD_ASSERT(Col[col].is_alive() && col != pivot_col);
+      COLAMD_DEBUG3(("Col: %d\n", col));
 
       /* clear tags used to construct pivot row pattern */
-      col_thickness = -Col [col].shared1.thickness ;
-      COLAMD_ASSERT (col_thickness > 0) ;
-      Col [col].shared1.thickness = col_thickness ;
+      col_thickness = -Col[col].shared1.thickness;
+      COLAMD_ASSERT(col_thickness > 0);
+      Col[col].shared1.thickness = col_thickness;
 
       /* === Remove column from degree list =========================== */
 
-      cur_score = Col [col].shared2.score ;
-      prev_col = Col [col].shared3.prev ;
-      next_col = Col [col].shared4.degree_next ;
-      COLAMD_ASSERT (cur_score >= 0) ;
-      COLAMD_ASSERT (cur_score <= n_col) ;
-      COLAMD_ASSERT (cur_score >= Empty) ;
-      if (prev_col == Empty)
-      {
-	head [cur_score] = next_col ;
+      cur_score = Col[col].shared2.score;
+      prev_col = Col[col].shared3.prev;
+      next_col = Col[col].shared4.degree_next;
+      COLAMD_ASSERT(cur_score >= 0);
+      COLAMD_ASSERT(cur_score <= n_col);
+      COLAMD_ASSERT(cur_score >= Empty);
+      if (prev_col == Empty) {
+        head[cur_score] = next_col;
+      } else {
+        Col[prev_col].shared4.degree_next = next_col;
       }
-      else
-      {
-	Col [prev_col].shared4.degree_next = next_col ;
-      }
-      if (next_col != Empty)
-      {
-	Col [next_col].shared3.prev = prev_col ;
+      if (next_col != Empty) {
+        Col[next_col].shared3.prev = prev_col;
       }
 
       /* === Scan the column ========================================== */
 
-      cp = &A [Col [col].start] ;
-      cp_end = cp + Col [col].length ;
-      while (cp < cp_end)
-      {
-	/* get a row */
-	row = *cp++ ;
-	/* skip if dead */
-	if (Row[row].is_dead())
-	{
-	  continue ;
-	}
-  row_mark = Row [row].shared2.mark ;
-	COLAMD_ASSERT (row != pivot_row) ;
-	set_difference = row_mark - tag_mark ;
-	/* check if the row has been seen yet */
-	if (set_difference < 0)
-	{
-	  COLAMD_ASSERT (Row [row].shared1.degree <= max_deg) ;
-	  set_difference = Row [row].shared1.degree ;
-	}
-	/* subtract column thickness from this row's set difference */
-	set_difference -= col_thickness ;
-	COLAMD_ASSERT (set_difference >= 0) ;
-	/* absorb this row if the set difference becomes zero */
-	if (set_difference == 0)
-	{
-	  COLAMD_DEBUG3 (("aggressive absorption. Row: %d\n", row)) ;
-	  Row[row].kill() ;
-	}
-	else
-	{
-	  /* save the new mark */
-	  Row [row].shared2.mark = set_difference + tag_mark ;
-	}
+      cp = &A[Col[col].start];
+      cp_end = cp + Col[col].length;
+      while (cp < cp_end) {
+        /* get a row */
+        row = *cp++;
+        /* skip if dead */
+        if (Row[row].is_dead()) {
+          continue;
+        }
+        row_mark = Row[row].shared2.mark;
+        COLAMD_ASSERT(row != pivot_row);
+        set_difference = row_mark - tag_mark;
+        /* check if the row has been seen yet */
+        if (set_difference < 0) {
+          COLAMD_ASSERT(Row[row].shared1.degree <= max_deg);
+          set_difference = Row[row].shared1.degree;
+        }
+        /* subtract column thickness from this row's set difference */
+        set_difference -= col_thickness;
+        COLAMD_ASSERT(set_difference >= 0);
+        /* absorb this row if the set difference becomes zero */
+        if (set_difference == 0) {
+          COLAMD_DEBUG3(("aggressive absorption. Row: %d\n", row));
+          Row[row].kill();
+        } else {
+          /* save the new mark */
+          Row[row].shared2.mark = set_difference + tag_mark;
+        }
       }
     }
 
-
     /* === Add up set differences for each column ======================= */
 
-    COLAMD_DEBUG3 (("** Adding set differences phase. **\n")) ;
+    COLAMD_DEBUG3(("** Adding set differences phase. **\n"));
 
     /* for each column in pivot row */
-    rp = &A [pivot_row_start] ;
-    rp_end = rp + pivot_row_length ;
-    while (rp < rp_end)
-    {
+    rp = &A[pivot_row_start];
+    rp_end = rp + pivot_row_length;
+    while (rp < rp_end) {
       /* get a column */
-      col = *rp++ ;
-      COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ;
-      hash = 0 ;
-      cur_score = 0 ;
-      cp = &A [Col [col].start] ;
+      col = *rp++;
+      COLAMD_ASSERT(Col[col].is_alive() && col != pivot_col);
+      hash = 0;
+      cur_score = 0;
+      cp = &A[Col[col].start];
       /* compact the column */
-      new_cp = cp ;
-      cp_end = cp + Col [col].length ;
+      new_cp = cp;
+      cp_end = cp + Col[col].length;
 
-      COLAMD_DEBUG4 (("Adding set diffs for Col: %d.\n", col)) ;
+      COLAMD_DEBUG4(("Adding set diffs for Col: %d.\n", col));
 
-      while (cp < cp_end)
-      {
-	/* get a row */
-	row = *cp++ ;
-	COLAMD_ASSERT(row >= 0 && row < n_row) ;
-	/* skip if dead */
-	if (Row [row].is_dead())
-	{
-	  continue ;
-	}
-  row_mark = Row [row].shared2.mark ;
-	COLAMD_ASSERT (row_mark > tag_mark) ;
-	/* compact the column */
-	*new_cp++ = row ;
-	/* compute hash function */
-	hash += row ;
-	/* add set difference */
-	cur_score += row_mark - tag_mark ;
-	/* integer overflow... */
-	cur_score = numext::mini(cur_score, n_col) ;
+      while (cp < cp_end) {
+        /* get a row */
+        row = *cp++;
+        COLAMD_ASSERT(row >= 0 && row < n_row);
+        /* skip if dead */
+        if (Row[row].is_dead()) {
+          continue;
+        }
+        row_mark = Row[row].shared2.mark;
+        COLAMD_ASSERT(row_mark > tag_mark);
+        /* compact the column */
+        *new_cp++ = row;
+        /* compute hash function */
+        hash += row;
+        /* add set difference */
+        cur_score += row_mark - tag_mark;
+        /* integer overflow... */
+        cur_score = numext::mini(cur_score, n_col);
       }
 
       /* recompute the column's length */
-      Col [col].length = (IndexType) (new_cp - &A [Col [col].start]) ;
+      Col[col].length = (IndexType)(new_cp - &A[Col[col].start]);
 
       /* === Further mass elimination ================================= */
 
-      if (Col [col].length == 0)
-      {
-	COLAMD_DEBUG4 (("further mass elimination. Col: %d\n", col)) ;
-	/* nothing left but the pivot row in this column */
-	Col[col].kill_principal() ;
-	pivot_row_degree -= Col [col].shared1.thickness ;
-	COLAMD_ASSERT (pivot_row_degree >= 0) ;
-	/* order it */
-	Col [col].shared2.order = k ;
-	/* increment order count by column thickness */
-	k += Col [col].shared1.thickness ;
-      }
-      else
-      {
-	/* === Prepare for supercolumn detection ==================== */
+      if (Col[col].length == 0) {
+        COLAMD_DEBUG4(("further mass elimination. Col: %d\n", col));
+        /* nothing left but the pivot row in this column */
+        Col[col].kill_principal();
+        pivot_row_degree -= Col[col].shared1.thickness;
+        COLAMD_ASSERT(pivot_row_degree >= 0);
+        /* order it */
+        Col[col].shared2.order = k;
+        /* increment order count by column thickness */
+        k += Col[col].shared1.thickness;
+      } else {
+        /* === Prepare for supercolumn detection ==================== */
 
-	COLAMD_DEBUG4 (("Preparing supercol detection for Col: %d.\n", col)) ;
+        COLAMD_DEBUG4(("Preparing supercol detection for Col: %d.\n", col));
 
-	/* save score so far */
-	Col [col].shared2.score = cur_score ;
+        /* save score so far */
+        Col[col].shared2.score = cur_score;
 
-	/* add column to hash table, for supercolumn detection */
-	hash %= n_col + 1 ;
+        /* add column to hash table, for supercolumn detection */
+        hash %= n_col + 1;
 
-	COLAMD_DEBUG4 ((" Hash = %d, n_col = %d.\n", hash, n_col)) ;
-	COLAMD_ASSERT (hash <= n_col) ;
+        COLAMD_DEBUG4((" Hash = %d, n_col = %d.\n", hash, n_col));
+        COLAMD_ASSERT(hash <= n_col);
 
-	head_column = head [hash] ;
-	if (head_column > Empty)
-	{
-	  /* degree list "hash" is non-empty, use prev (shared3) of */
-	  /* first column in degree list as head of hash bucket */
-	  first_col = Col [head_column].shared3.headhash ;
-	  Col [head_column].shared3.headhash = col ;
-	}
-	else
-	{
-	  /* degree list "hash" is empty, use head as hash bucket */
-	  first_col = - (head_column + 2) ;
-	  head [hash] = - (col + 2) ;
-	}
-	Col [col].shared4.hash_next = first_col ;
+        head_column = head[hash];
+        if (head_column > Empty) {
+          /* degree list "hash" is non-empty, use prev (shared3) of */
+          /* first column in degree list as head of hash bucket */
+          first_col = Col[head_column].shared3.headhash;
+          Col[head_column].shared3.headhash = col;
+        } else {
+          /* degree list "hash" is empty, use head as hash bucket */
+          first_col = -(head_column + 2);
+          head[hash] = -(col + 2);
+        }
+        Col[col].shared4.hash_next = first_col;
 
-	/* save hash function in Col [col].shared3.hash */
-	Col [col].shared3.hash = (IndexType) hash ;
-	COLAMD_ASSERT (Col[col].is_alive()) ;
+        /* save hash function in Col [col].shared3.hash */
+        Col[col].shared3.hash = (IndexType)hash;
+        COLAMD_ASSERT(Col[col].is_alive());
       }
     }
 
@@ -1346,105 +1228,98 @@
 
     /* === Supercolumn detection ======================================== */
 
-    COLAMD_DEBUG3 (("** Supercolumn detection phase. **\n")) ;
+    COLAMD_DEBUG3(("** Supercolumn detection phase. **\n"));
 
-    Colamd::detect_super_cols (Col, A, head, pivot_row_start, pivot_row_length) ;
+    Colamd::detect_super_cols(Col, A, head, pivot_row_start, pivot_row_length);
 
     /* === Kill the pivotal column ====================================== */
 
-    Col[pivot_col].kill_principal() ;
+    Col[pivot_col].kill_principal();
 
     /* === Clear mark =================================================== */
 
-    tag_mark += (max_deg + 1) ;
-    if (tag_mark >= max_mark)
-    {
-      COLAMD_DEBUG2 (("clearing tag_mark\n")) ;
-      tag_mark = Colamd::clear_mark (n_row, Row) ;
+    tag_mark += (max_deg + 1);
+    if (tag_mark >= max_mark) {
+      COLAMD_DEBUG2(("clearing tag_mark\n"));
+      tag_mark = Colamd::clear_mark(n_row, Row);
     }
 
     /* === Finalize the new pivot row, and column scores ================ */
 
-    COLAMD_DEBUG3 (("** Finalize scores phase. **\n")) ;
+    COLAMD_DEBUG3(("** Finalize scores phase. **\n"));
 
     /* for each column in pivot row */
-    rp = &A [pivot_row_start] ;
+    rp = &A[pivot_row_start];
     /* compact the pivot row */
-    new_rp = rp ;
-    rp_end = rp + pivot_row_length ;
-    while (rp < rp_end)
-    {
-      col = *rp++ ;
+    new_rp = rp;
+    rp_end = rp + pivot_row_length;
+    while (rp < rp_end) {
+      col = *rp++;
       /* skip dead columns */
-      if (Col[col].is_dead())
-      {
-	continue ;
+      if (Col[col].is_dead()) {
+        continue;
       }
-      *new_rp++ = col ;
+      *new_rp++ = col;
       /* add new pivot row to column */
-      A [Col [col].start + (Col [col].length++)] = pivot_row ;
+      A[Col[col].start + (Col[col].length++)] = pivot_row;
 
       /* retrieve score so far and add on pivot row's degree. */
       /* (we wait until here for this in case the pivot */
       /* row's degree was reduced due to mass elimination). */
-      cur_score = Col [col].shared2.score + pivot_row_degree ;
+      cur_score = Col[col].shared2.score + pivot_row_degree;
 
       /* calculate the max possible score as the number of */
       /* external columns minus the 'k' value minus the */
       /* columns thickness */
-      max_score = n_col - k - Col [col].shared1.thickness ;
+      max_score = n_col - k - Col[col].shared1.thickness;
 
       /* make the score the external degree of the union-of-rows */
-      cur_score -= Col [col].shared1.thickness ;
+      cur_score -= Col[col].shared1.thickness;
 
       /* make sure score is less or equal than the max score */
-      cur_score = numext::mini(cur_score, max_score) ;
-      COLAMD_ASSERT (cur_score >= 0) ;
+      cur_score = numext::mini(cur_score, max_score);
+      COLAMD_ASSERT(cur_score >= 0);
 
       /* store updated score */
-      Col [col].shared2.score = cur_score ;
+      Col[col].shared2.score = cur_score;
 
       /* === Place column back in degree list ========================= */
 
-      COLAMD_ASSERT (min_score >= 0) ;
-      COLAMD_ASSERT (min_score <= n_col) ;
-      COLAMD_ASSERT (cur_score >= 0) ;
-      COLAMD_ASSERT (cur_score <= n_col) ;
-      COLAMD_ASSERT (head [cur_score] >= Empty) ;
-      next_col = head [cur_score] ;
-      Col [col].shared4.degree_next = next_col ;
-      Col [col].shared3.prev = Empty ;
-      if (next_col != Empty)
-      {
-	Col [next_col].shared3.prev = col ;
+      COLAMD_ASSERT(min_score >= 0);
+      COLAMD_ASSERT(min_score <= n_col);
+      COLAMD_ASSERT(cur_score >= 0);
+      COLAMD_ASSERT(cur_score <= n_col);
+      COLAMD_ASSERT(head[cur_score] >= Empty);
+      next_col = head[cur_score];
+      Col[col].shared4.degree_next = next_col;
+      Col[col].shared3.prev = Empty;
+      if (next_col != Empty) {
+        Col[next_col].shared3.prev = col;
       }
-      head [cur_score] = col ;
+      head[cur_score] = col;
 
       /* see if this score is less than current min */
-      min_score = numext::mini(min_score, cur_score) ;
-
+      min_score = numext::mini(min_score, cur_score);
     }
 
     /* === Resurrect the new pivot row ================================== */
 
-    if (pivot_row_degree > 0)
-    {
+    if (pivot_row_degree > 0) {
       /* update pivot row length to reflect any cols that were killed */
       /* during super-col detection and mass elimination */
-      Row [pivot_row].start  = pivot_row_start ;
-      Row [pivot_row].length = (IndexType) (new_rp - &A[pivot_row_start]) ;
-      Row [pivot_row].shared1.degree = pivot_row_degree ;
-      Row [pivot_row].shared2.mark = 0 ;
+      Row[pivot_row].start = pivot_row_start;
+      Row[pivot_row].length = (IndexType)(new_rp - &A[pivot_row_start]);
+      Row[pivot_row].shared1.degree = pivot_row_degree;
+      Row[pivot_row].shared2.mark = 0;
       /* pivot row is no longer dead */
     }
   }
 
   /* === All principal columns have now been ordered ====================== */
 
-  return (ngarbage) ;
+  return (ngarbage);
 }
 
-
 /* ========================================================================== */
 /* === order_children ======================================================= */
 /* ========================================================================== */
@@ -1462,74 +1337,66 @@
   columns.  Not user-callable.
 */
 template <typename IndexType>
-static inline  void order_children
-(
-  /* === Parameters ======================================================= */
+static inline void order_children(
+    /* === Parameters ======================================================= */
 
-  IndexType n_col,      /* number of columns of A */
-  ColStructure<IndexType> Col [],    /* of size n_col+1 */
-  IndexType p []      /* p [0 ... n_col-1] is the column permutation*/
-  )
-{
+    IndexType n_col,               /* number of columns of A */
+    ColStructure<IndexType> Col[], /* of size n_col+1 */
+    IndexType p[]                  /* p [0 ... n_col-1] is the column permutation*/
+) {
   /* === Local variables ================================================== */
 
-  IndexType i ;     /* loop counter for all columns */
-  IndexType c ;     /* column index */
-  IndexType parent ;    /* index of column's parent */
-  IndexType order ;     /* column's order */
+  IndexType i;      /* loop counter for all columns */
+  IndexType c;      /* column index */
+  IndexType parent; /* index of column's parent */
+  IndexType order;  /* column's order */
 
   /* === Order each non-principal column ================================== */
 
-  for (i = 0 ; i < n_col ; i++)
-  {
+  for (i = 0; i < n_col; i++) {
     /* find an un-ordered non-principal column */
-    COLAMD_ASSERT (col_is_dead(Col, i)) ;
-    if (!Col[i].is_dead_principal() && Col [i].shared2.order == Empty)
-    {
-      parent = i ;
+    COLAMD_ASSERT(col_is_dead(Col, i));
+    if (!Col[i].is_dead_principal() && Col[i].shared2.order == Empty) {
+      parent = i;
       /* once found, find its principal parent */
-      do
-      {
-	parent = Col [parent].shared1.parent ;
-      } while (!Col[parent].is_dead_principal()) ;
+      do {
+        parent = Col[parent].shared1.parent;
+      } while (!Col[parent].is_dead_principal());
 
       /* now, order all un-ordered non-principal columns along path */
       /* to this parent.  collapse tree at the same time */
-      c = i ;
+      c = i;
       /* get order of parent */
-      order = Col [parent].shared2.order ;
+      order = Col[parent].shared2.order;
 
-      do
-      {
-	COLAMD_ASSERT (Col [c].shared2.order == Empty) ;
+      do {
+        COLAMD_ASSERT(Col[c].shared2.order == Empty);
 
-	/* order this column */
-	Col [c].shared2.order = order++ ;
-	/* collaps tree */
-	Col [c].shared1.parent = parent ;
+        /* order this column */
+        Col[c].shared2.order = order++;
+        /* collaps tree */
+        Col[c].shared1.parent = parent;
 
-	/* get immediate parent of this column */
-	c = Col [c].shared1.parent ;
+        /* get immediate parent of this column */
+        c = Col[c].shared1.parent;
 
-	/* continue until we hit an ordered column.  There are */
-	/* guaranteed not to be anymore unordered columns */
-	/* above an ordered column */
-      } while (Col [c].shared2.order == Empty) ;
+        /* continue until we hit an ordered column.  There are */
+        /* guaranteed not to be anymore unordered columns */
+        /* above an ordered column */
+      } while (Col[c].shared2.order == Empty);
 
       /* re-order the super_col parent to largest order for this group */
-      Col [parent].shared2.order = order ;
+      Col[parent].shared2.order = order;
     }
   }
 
   /* === Generate the permutation ========================================= */
 
-  for (c = 0 ; c < n_col ; c++)
-  {
-    p [Col [c].shared2.order] = c ;
+  for (c = 0; c < n_col; c++) {
+    p[Col[c].shared2.order] = c;
   }
 }
 
-
 /* ========================================================================== */
 /* === detect_super_cols ==================================================== */
 /* ========================================================================== */
@@ -1563,144 +1430,124 @@
   Not user-callable.
 */
 template <typename IndexType>
-static void detect_super_cols
-(
-  /* === Parameters ======================================================= */
+static void detect_super_cols(
+    /* === Parameters ======================================================= */
 
-  ColStructure<IndexType> Col [],    /* of size n_col+1 */
-  IndexType A [],     /* row indices of A */
-  IndexType head [],    /* head of degree lists and hash buckets */
-  IndexType row_start,    /* pointer to set of columns to check */
-  IndexType row_length    /* number of columns to check */
-)
-{
+    ColStructure<IndexType> Col[], /* of size n_col+1 */
+    IndexType A[],                 /* row indices of A */
+    IndexType head[],              /* head of degree lists and hash buckets */
+    IndexType row_start,           /* pointer to set of columns to check */
+    IndexType row_length           /* number of columns to check */
+) {
   /* === Local variables ================================================== */
 
-  IndexType hash ;      /* hash value for a column */
-  IndexType *rp ;     /* pointer to a row */
-  IndexType c ;     /* a column index */
-  IndexType super_c ;   /* column index of the column to absorb into */
-  IndexType *cp1 ;      /* column pointer for column super_c */
-  IndexType *cp2 ;      /* column pointer for column c */
-  IndexType length ;    /* length of column super_c */
-  IndexType prev_c ;    /* column preceding c in hash bucket */
-  IndexType i ;     /* loop counter */
-  IndexType *rp_end ;   /* pointer to the end of the row */
-  IndexType col ;     /* a column index in the row to check */
-  IndexType head_column ;   /* first column in hash bucket or degree list */
-  IndexType first_col ;   /* first column in hash bucket */
+  IndexType hash;        /* hash value for a column */
+  IndexType *rp;         /* pointer to a row */
+  IndexType c;           /* a column index */
+  IndexType super_c;     /* column index of the column to absorb into */
+  IndexType *cp1;        /* column pointer for column super_c */
+  IndexType *cp2;        /* column pointer for column c */
+  IndexType length;      /* length of column super_c */
+  IndexType prev_c;      /* column preceding c in hash bucket */
+  IndexType i;           /* loop counter */
+  IndexType *rp_end;     /* pointer to the end of the row */
+  IndexType col;         /* a column index in the row to check */
+  IndexType head_column; /* first column in hash bucket or degree list */
+  IndexType first_col;   /* first column in hash bucket */
 
   /* === Consider each column in the row ================================== */
 
-  rp = &A [row_start] ;
-  rp_end = rp + row_length ;
-  while (rp < rp_end)
-  {
-    col = *rp++ ;
-    if (Col[col].is_dead())
-    {
-      continue ;
+  rp = &A[row_start];
+  rp_end = rp + row_length;
+  while (rp < rp_end) {
+    col = *rp++;
+    if (Col[col].is_dead()) {
+      continue;
     }
 
     /* get hash number for this column */
-    hash = Col [col].shared3.hash ;
-    COLAMD_ASSERT (hash <= n_col) ;
+    hash = Col[col].shared3.hash;
+    COLAMD_ASSERT(hash <= n_col);
 
     /* === Get the first column in this hash bucket ===================== */
 
-    head_column = head [hash] ;
-    if (head_column > Empty)
-    {
-      first_col = Col [head_column].shared3.headhash ;
-    }
-    else
-    {
-      first_col = - (head_column + 2) ;
+    head_column = head[hash];
+    if (head_column > Empty) {
+      first_col = Col[head_column].shared3.headhash;
+    } else {
+      first_col = -(head_column + 2);
     }
 
     /* === Consider each column in the hash bucket ====================== */
 
-    for (super_c = first_col ; super_c != Empty ;
-	 super_c = Col [super_c].shared4.hash_next)
-    {
-      COLAMD_ASSERT (Col [super_c].is_alive()) ;
-      COLAMD_ASSERT (Col [super_c].shared3.hash == hash) ;
-      length = Col [super_c].length ;
+    for (super_c = first_col; super_c != Empty; super_c = Col[super_c].shared4.hash_next) {
+      COLAMD_ASSERT(Col[super_c].is_alive());
+      COLAMD_ASSERT(Col[super_c].shared3.hash == hash);
+      length = Col[super_c].length;
 
       /* prev_c is the column preceding column c in the hash bucket */
-      prev_c = super_c ;
+      prev_c = super_c;
 
       /* === Compare super_c with all columns after it ================ */
 
-      for (c = Col [super_c].shared4.hash_next ;
-	   c != Empty ; c = Col [c].shared4.hash_next)
-      {
-	COLAMD_ASSERT (c != super_c) ;
-	COLAMD_ASSERT (Col[c].is_alive()) ;
-	COLAMD_ASSERT (Col [c].shared3.hash == hash) ;
+      for (c = Col[super_c].shared4.hash_next; c != Empty; c = Col[c].shared4.hash_next) {
+        COLAMD_ASSERT(c != super_c);
+        COLAMD_ASSERT(Col[c].is_alive());
+        COLAMD_ASSERT(Col[c].shared3.hash == hash);
 
-	/* not identical if lengths or scores are different */
-	if (Col [c].length != length ||
-	    Col [c].shared2.score != Col [super_c].shared2.score)
-	{
-	  prev_c = c ;
-	  continue ;
-	}
+        /* not identical if lengths or scores are different */
+        if (Col[c].length != length || Col[c].shared2.score != Col[super_c].shared2.score) {
+          prev_c = c;
+          continue;
+        }
 
-	/* compare the two columns */
-	cp1 = &A [Col [super_c].start] ;
-	cp2 = &A [Col [c].start] ;
+        /* compare the two columns */
+        cp1 = &A[Col[super_c].start];
+        cp2 = &A[Col[c].start];
 
-	for (i = 0 ; i < length ; i++)
-	{
-	  /* the columns are "clean" (no dead rows) */
-	  COLAMD_ASSERT ( cp1->is_alive() );
-	  COLAMD_ASSERT ( cp2->is_alive() );
-	  /* row indices will same order for both supercols, */
-	  /* no gather scatter necessary */
-	  if (*cp1++ != *cp2++)
-	  {
-	    break ;
-	  }
-	}
+        for (i = 0; i < length; i++) {
+          /* the columns are "clean" (no dead rows) */
+          COLAMD_ASSERT(cp1->is_alive());
+          COLAMD_ASSERT(cp2->is_alive());
+          /* row indices will same order for both supercols, */
+          /* no gather scatter necessary */
+          if (*cp1++ != *cp2++) {
+            break;
+          }
+        }
 
-	/* the two columns are different if the for-loop "broke" */
-	if (i != length)
-	{
-	  prev_c = c ;
-	  continue ;
-	}
+        /* the two columns are different if the for-loop "broke" */
+        if (i != length) {
+          prev_c = c;
+          continue;
+        }
 
-	/* === Got it!  two columns are identical =================== */
+        /* === Got it!  two columns are identical =================== */
 
-	COLAMD_ASSERT (Col [c].shared2.score == Col [super_c].shared2.score) ;
+        COLAMD_ASSERT(Col[c].shared2.score == Col[super_c].shared2.score);
 
-	Col [super_c].shared1.thickness += Col [c].shared1.thickness ;
-	Col [c].shared1.parent = super_c ;
-	Col[c].kill_non_principal() ;
-	/* order c later, in order_children() */
-	Col [c].shared2.order = Empty ;
-	/* remove c from hash bucket */
-	Col [prev_c].shared4.hash_next = Col [c].shared4.hash_next ;
+        Col[super_c].shared1.thickness += Col[c].shared1.thickness;
+        Col[c].shared1.parent = super_c;
+        Col[c].kill_non_principal();
+        /* order c later, in order_children() */
+        Col[c].shared2.order = Empty;
+        /* remove c from hash bucket */
+        Col[prev_c].shared4.hash_next = Col[c].shared4.hash_next;
       }
     }
 
     /* === Empty this hash bucket ======================================= */
 
-    if (head_column > Empty)
-    {
+    if (head_column > Empty) {
       /* corresponding degree list "hash" is not empty */
-      Col [head_column].shared3.headhash = Empty ;
-    }
-    else
-    {
+      Col[head_column].shared3.headhash = Empty;
+    } else {
       /* corresponding degree list "hash" is empty */
-      head [hash] = Empty ;
+      head[hash] = Empty;
     }
   }
 }
 
-
 /* ========================================================================== */
 /* === garbage_collection =================================================== */
 /* ========================================================================== */
@@ -1714,118 +1561,101 @@
   Not user-callable.
 */
 template <typename IndexType>
-static IndexType garbage_collection  /* returns the new value of pfree */
-  (
-    /* === Parameters ======================================================= */
+static IndexType garbage_collection /* returns the new value of pfree */
+    (
+        /* === Parameters ======================================================= */
 
-    IndexType n_row,      /* number of rows */
-    IndexType n_col,      /* number of columns */
-    RowStructure<IndexType> Row [],    /* row info */
-    ColStructure<IndexType> Col [],    /* column info */
-    IndexType A [],     /* A [0 ... Alen-1] holds the matrix */
-    IndexType *pfree      /* &A [0] ... pfree is in use */
-    )
-{
+        IndexType n_row,               /* number of rows */
+        IndexType n_col,               /* number of columns */
+        RowStructure<IndexType> Row[], /* row info */
+        ColStructure<IndexType> Col[], /* column info */
+        IndexType A[],                 /* A [0 ... Alen-1] holds the matrix */
+        IndexType *pfree               /* &A [0] ... pfree is in use */
+    ) {
   /* === Local variables ================================================== */
 
-  IndexType *psrc ;     /* source pointer */
-  IndexType *pdest ;    /* destination pointer */
-  IndexType j ;     /* counter */
-  IndexType r ;     /* a row index */
-  IndexType c ;     /* a column index */
-  IndexType length ;    /* length of a row or column */
+  IndexType *psrc;  /* source pointer */
+  IndexType *pdest; /* destination pointer */
+  IndexType j;      /* counter */
+  IndexType r;      /* a row index */
+  IndexType c;      /* a column index */
+  IndexType length; /* length of a row or column */
 
   /* === Defragment the columns =========================================== */
 
-  pdest = &A[0] ;
-  for (c = 0 ; c < n_col ; c++)
-  {
-    if (Col[c].is_alive())
-    {
-      psrc = &A [Col [c].start] ;
+  pdest = &A[0];
+  for (c = 0; c < n_col; c++) {
+    if (Col[c].is_alive()) {
+      psrc = &A[Col[c].start];
 
       /* move and compact the column */
-      COLAMD_ASSERT (pdest <= psrc) ;
-      Col [c].start = (IndexType) (pdest - &A [0]) ;
-      length = Col [c].length ;
-      for (j = 0 ; j < length ; j++)
-      {
-	r = *psrc++ ;
-	if (Row[r].is_alive())
-	{
-	  *pdest++ = r ;
-	}
+      COLAMD_ASSERT(pdest <= psrc);
+      Col[c].start = (IndexType)(pdest - &A[0]);
+      length = Col[c].length;
+      for (j = 0; j < length; j++) {
+        r = *psrc++;
+        if (Row[r].is_alive()) {
+          *pdest++ = r;
+        }
       }
-      Col [c].length = (IndexType) (pdest - &A [Col [c].start]) ;
+      Col[c].length = (IndexType)(pdest - &A[Col[c].start]);
     }
   }
 
   /* === Prepare to defragment the rows =================================== */
 
-  for (r = 0 ; r < n_row ; r++)
-  {
-    if (Row[r].is_alive())
-    {
-      if (Row [r].length == 0)
-      {
+  for (r = 0; r < n_row; r++) {
+    if (Row[r].is_alive()) {
+      if (Row[r].length == 0) {
         /* this row is of zero length.  cannot compact it, so kill it */
-        COLAMD_DEBUG3 (("Defrag row kill\n")) ;
-        Row[r].kill() ;
-      }
-      else
-      {
+        COLAMD_DEBUG3(("Defrag row kill\n"));
+        Row[r].kill();
+      } else {
         /* save first column index in Row [r].shared2.first_column */
-        psrc = &A [Row [r].start] ;
-        Row [r].shared2.first_column = *psrc ;
-        COLAMD_ASSERT (Row[r].is_alive()) ;
+        psrc = &A[Row[r].start];
+        Row[r].shared2.first_column = *psrc;
+        COLAMD_ASSERT(Row[r].is_alive());
         /* flag the start of the row with the one's complement of row */
-        *psrc = ones_complement(r) ;
-
+        *psrc = ones_complement(r);
       }
     }
   }
 
   /* === Defragment the rows ============================================== */
 
-  psrc = pdest ;
-  while (psrc < pfree)
-  {
+  psrc = pdest;
+  while (psrc < pfree) {
     /* find a negative number ... the start of a row */
-    if (*psrc++ < 0)
-    {
-      psrc-- ;
+    if (*psrc++ < 0) {
+      psrc--;
       /* get the row index */
-      r = ones_complement(*psrc) ;
-      COLAMD_ASSERT (r >= 0 && r < n_row) ;
+      r = ones_complement(*psrc);
+      COLAMD_ASSERT(r >= 0 && r < n_row);
       /* restore first column index */
-      *psrc = Row [r].shared2.first_column ;
-      COLAMD_ASSERT (Row[r].is_alive()) ;
+      *psrc = Row[r].shared2.first_column;
+      COLAMD_ASSERT(Row[r].is_alive());
 
       /* move and compact the row */
-      COLAMD_ASSERT (pdest <= psrc) ;
-      Row [r].start = (IndexType) (pdest - &A [0]) ;
-      length = Row [r].length ;
-      for (j = 0 ; j < length ; j++)
-      {
-	c = *psrc++ ;
-	if (Col[c].is_alive())
-	{
-	  *pdest++ = c ;
-	}
+      COLAMD_ASSERT(pdest <= psrc);
+      Row[r].start = (IndexType)(pdest - &A[0]);
+      length = Row[r].length;
+      for (j = 0; j < length; j++) {
+        c = *psrc++;
+        if (Col[c].is_alive()) {
+          *pdest++ = c;
+        }
       }
-      Row [r].length = (IndexType) (pdest - &A [Row [r].start]) ;
-
+      Row[r].length = (IndexType)(pdest - &A[Row[r].start]);
     }
   }
   /* ensure we found all the rows */
-  COLAMD_ASSERT (debug_rows == 0) ;
+  COLAMD_ASSERT(debug_rows == 0);
 
   /* === Return the new value of pfree ==================================== */
 
-  return ((IndexType) (pdest - &A [0])) ;
+  return ((IndexType)(pdest - &A[0]));
 }
 
-
 /* ========================================================================== */
 /* === clear_mark =========================================================== */
 /* ========================================================================== */
@@ -1835,29 +1665,26 @@
   Return value is the new tag_mark.  Not user-callable.
 */
 template <typename IndexType>
-static inline  IndexType clear_mark  /* return the new value for tag_mark */
-  (
-      /* === Parameters ======================================================= */
+static inline IndexType clear_mark /* return the new value for tag_mark */
+    (
+        /* === Parameters ======================================================= */
 
-    IndexType n_row,    /* number of rows in A */
-    RowStructure<IndexType> Row [] /* Row [0 ... n_row-1].shared2.mark is set to zero */
-    )
-{
+        IndexType n_row,              /* number of rows in A */
+        RowStructure<IndexType> Row[] /* Row [0 ... n_row-1].shared2.mark is set to zero */
+    ) {
   /* === Local variables ================================================== */
 
-  IndexType r ;
+  IndexType r;
 
-  for (r = 0 ; r < n_row ; r++)
-  {
-    if (Row[r].is_alive())
-    {
-      Row [r].shared2.mark = 0 ;
+  for (r = 0; r < n_row; r++) {
+    if (Row[r].is_alive()) {
+      Row[r].shared2.mark = 0;
     }
   }
-  return (1) ;
+  return (1);
 }
 
-} // namespace Colamd
+}  // namespace Colamd
 
-} // namespace internal
+}  // namespace internal
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/InternalHeaderCheck.h
new file mode 100644
index 0000000..713c447
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_ORDERINGMETHODS_MODULE_H
+#error "Please include Eigen/OrderingMethods instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h
index c578970..9a1c535 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h
@@ -1,4 +1,4 @@
- 
+
 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
 //
@@ -11,143 +11,138 @@
 #ifndef EIGEN_ORDERING_H
 #define EIGEN_ORDERING_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
-  
+
 #include "Eigen_Colamd.h"
 
 namespace internal {
-    
+
 /** \internal
-  * \ingroup OrderingMethods_Module
-  * \param[in] A the input non-symmetric matrix
-  * \param[out] symmat the symmetric pattern A^T+A from the input matrix \a A.
-  * FIXME: The values should not be considered here
-  */
-template<typename MatrixType> 
-void ordering_helper_at_plus_a(const MatrixType& A, MatrixType& symmat)
-{
+ * \ingroup OrderingMethods_Module
+ * \param[in] A the input non-symmetric matrix
+ * \param[out] symmat the symmetric pattern A^T+A from the input matrix \a A.
+ * FIXME: The values should not be considered here
+ */
+template <typename MatrixType>
+void ordering_helper_at_plus_a(const MatrixType& A, MatrixType& symmat) {
   MatrixType C;
-  C = A.transpose(); // NOTE: Could be  costly
-  for (int i = 0; i < C.rows(); i++) 
-  {
-      for (typename MatrixType::InnerIterator it(C, i); it; ++it)
-        it.valueRef() = typename MatrixType::Scalar(0);
+  C = A.transpose();  // NOTE: Could be  costly
+  for (int i = 0; i < C.rows(); i++) {
+    for (typename MatrixType::InnerIterator it(C, i); it; ++it) it.valueRef() = typename MatrixType::Scalar(0);
   }
   symmat = C + A;
 }
-    
-}
+
+}  // namespace internal
 
 /** \ingroup OrderingMethods_Module
-  * \class AMDOrdering
-  *
-  * Functor computing the \em approximate \em minimum \em degree ordering
-  * If the matrix is not structurally symmetric, an ordering of A^T+A is computed
-  * \tparam  StorageIndex The type of indices of the matrix 
-  * \sa COLAMDOrdering
-  */
+ * \class AMDOrdering
+ *
+ * Functor computing the \em approximate \em minimum \em degree ordering
+ * If the matrix is not structurally symmetric, an ordering of A^T+A is computed
+ * \tparam  StorageIndex The type of indices of the matrix
+ * \sa COLAMDOrdering
+ */
 template <typename StorageIndex>
-class AMDOrdering
-{
-  public:
-    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
-    
-    /** Compute the permutation vector from a sparse matrix
-     * This routine is much faster if the input matrix is column-major     
-     */
-    template <typename MatrixType>
-    void operator()(const MatrixType& mat, PermutationType& perm)
-    {
-      // Compute the symmetric pattern
-      SparseMatrix<typename MatrixType::Scalar, ColMajor, StorageIndex> symm;
-      internal::ordering_helper_at_plus_a(mat,symm); 
-    
-      // Call the AMD routine 
-      //m_mat.prune(keep_diag());
-      internal::minimum_degree_ordering(symm, perm);
-    }
-    
-    /** Compute the permutation with a selfadjoint matrix */
-    template <typename SrcType, unsigned int SrcUpLo> 
-    void operator()(const SparseSelfAdjointView<SrcType, SrcUpLo>& mat, PermutationType& perm)
-    { 
-      SparseMatrix<typename SrcType::Scalar, ColMajor, StorageIndex> C; C = mat;
-      
-      // Call the AMD routine 
-      // m_mat.prune(keep_diag()); //Remove the diagonal elements 
-      internal::minimum_degree_ordering(C, perm);
-    }
+class AMDOrdering {
+ public:
+  typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+
+  /** Compute the permutation vector from a sparse matrix
+   * This routine is much faster if the input matrix is column-major
+   */
+  template <typename MatrixType>
+  void operator()(const MatrixType& mat, PermutationType& perm) {
+    // Compute the symmetric pattern
+    SparseMatrix<typename MatrixType::Scalar, ColMajor, StorageIndex> symm;
+    internal::ordering_helper_at_plus_a(mat, symm);
+
+    // Call the AMD routine
+    // m_mat.prune(keep_diag());
+    internal::minimum_degree_ordering(symm, perm);
+  }
+
+  /** Compute the permutation with a selfadjoint matrix */
+  template <typename SrcType, unsigned int SrcUpLo>
+  void operator()(const SparseSelfAdjointView<SrcType, SrcUpLo>& mat, PermutationType& perm) {
+    SparseMatrix<typename SrcType::Scalar, ColMajor, StorageIndex> C;
+    C = mat;
+
+    // Call the AMD routine
+    // m_mat.prune(keep_diag()); //Remove the diagonal elements
+    internal::minimum_degree_ordering(C, perm);
+  }
 };
 
 /** \ingroup OrderingMethods_Module
-  * \class NaturalOrdering
-  *
-  * Functor computing the natural ordering (identity)
-  * 
-  * \note Returns an empty permutation matrix
-  * \tparam  StorageIndex The type of indices of the matrix 
-  */
+ * \class NaturalOrdering
+ *
+ * Functor computing the natural ordering (identity)
+ *
+ * \note Returns an empty permutation matrix
+ * \tparam  StorageIndex The type of indices of the matrix
+ */
 template <typename StorageIndex>
-class NaturalOrdering
-{
-  public:
-    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
-    
-    /** Compute the permutation vector from a column-major sparse matrix */
-    template <typename MatrixType>
-    void operator()(const MatrixType& /*mat*/, PermutationType& perm)
-    {
-      perm.resize(0); 
-    }
-    
+class NaturalOrdering {
+ public:
+  typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+
+  /** Compute the permutation vector from a column-major sparse matrix */
+  template <typename MatrixType>
+  void operator()(const MatrixType& /*mat*/, PermutationType& perm) {
+    perm.resize(0);
+  }
 };
 
 /** \ingroup OrderingMethods_Module
-  * \class COLAMDOrdering
-  *
-  * \tparam  StorageIndex The type of indices of the matrix 
-  * 
-  * Functor computing the \em column \em approximate \em minimum \em degree ordering 
-  * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()).
-  */
-template<typename StorageIndex>
-class COLAMDOrdering
-{
-  public:
-    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType; 
-    typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
-    
-    /** Compute the permutation vector \a perm form the sparse matrix \a mat
-      * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
-      */
-    template <typename MatrixType>
-    void operator() (const MatrixType& mat, PermutationType& perm)
-    {
-      eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering");
-      
-      StorageIndex m = StorageIndex(mat.rows());
-      StorageIndex n = StorageIndex(mat.cols());
-      StorageIndex nnz = StorageIndex(mat.nonZeros());
-      // Get the recommended value of Alen to be used by colamd
-      StorageIndex Alen = internal::Colamd::recommended(nnz, m, n); 
-      // Set the default parameters
-      double knobs [internal::Colamd::NKnobs]; 
-      StorageIndex stats [internal::Colamd::NStats];
-      internal::Colamd::set_defaults(knobs);
-      
-      IndexVector p(n+1), A(Alen); 
-      for(StorageIndex i=0; i <= n; i++)   p(i) = mat.outerIndexPtr()[i];
-      for(StorageIndex i=0; i < nnz; i++)  A(i) = mat.innerIndexPtr()[i];
-      // Call Colamd routine to compute the ordering 
-      StorageIndex info = internal::Colamd::compute_ordering(m, n, Alen, A.data(), p.data(), knobs, stats); 
-      EIGEN_UNUSED_VARIABLE(info);
-      eigen_assert( info && "COLAMD failed " );
-      
-      perm.resize(n);
-      for (StorageIndex i = 0; i < n; i++) perm.indices()(p(i)) = i;
-    }
+ * \class COLAMDOrdering
+ *
+ * \tparam  StorageIndex The type of indices of the matrix
+ *
+ * Functor computing the \em column \em approximate \em minimum \em degree ordering
+ * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()).
+ */
+template <typename StorageIndex>
+class COLAMDOrdering {
+ public:
+  typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+  typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+
+  /** Compute the permutation vector \a perm form the sparse matrix \a mat
+   * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+   */
+  template <typename MatrixType>
+  void operator()(const MatrixType& mat, PermutationType& perm) {
+    eigen_assert(mat.isCompressed() &&
+                 "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it "
+                 "to COLAMDOrdering");
+
+    StorageIndex m = StorageIndex(mat.rows());
+    StorageIndex n = StorageIndex(mat.cols());
+    StorageIndex nnz = StorageIndex(mat.nonZeros());
+    // Get the recommended value of Alen to be used by colamd
+    StorageIndex Alen = internal::Colamd::recommended(nnz, m, n);
+    // Set the default parameters
+    double knobs[internal::Colamd::NKnobs];
+    StorageIndex stats[internal::Colamd::NStats];
+    internal::Colamd::set_defaults(knobs);
+
+    IndexVector p(n + 1), A(Alen);
+    for (StorageIndex i = 0; i <= n; i++) p(i) = mat.outerIndexPtr()[i];
+    for (StorageIndex i = 0; i < nnz; i++) A(i) = mat.innerIndexPtr()[i];
+    // Call Colamd routine to compute the ordering
+    StorageIndex info = internal::Colamd::compute_ordering(m, n, Alen, A.data(), p.data(), knobs, stats);
+    EIGEN_UNUSED_VARIABLE(info);
+    eigen_assert(info && "COLAMD failed ");
+
+    perm.resize(n);
+    for (StorageIndex i = 0; i < n; i++) perm.indices()(p(i)) = i;
+  }
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h
index 9b677e9..f1de6fd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h
@@ -11,78 +11,88 @@
 #ifndef EIGEN_COLPIVOTINGHOUSEHOLDERQR_H
 #define EIGEN_COLPIVOTINGHOUSEHOLDERQR_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename _MatrixType> struct traits<ColPivHouseholderQR<_MatrixType> >
- : traits<_MatrixType>
-{
+template <typename MatrixType_, typename PermutationIndex_>
+struct traits<ColPivHouseholderQR<MatrixType_, PermutationIndex_>> : traits<MatrixType_> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
-  typedef int StorageIndex;
+  typedef PermutationIndex_ PermutationIndex;
   enum { Flags = 0 };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup QR_Module
-  *
-  * \class ColPivHouseholderQR
-  *
-  * \brief Householder rank-revealing QR decomposition of a matrix with column-pivoting
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the QR decomposition
-  *
-  * This class performs a rank-revealing QR decomposition of a matrix \b A into matrices \b P, \b Q and \b R
-  * such that
-  * \f[
-  *  \mathbf{A} \, \mathbf{P} = \mathbf{Q} \, \mathbf{R}
-  * \f]
-  * by using Householder transformations. Here, \b P is a permutation matrix, \b Q a unitary matrix and \b R an
-  * upper triangular matrix.
-  *
-  * This decomposition performs column pivoting in order to be rank-revealing and improve
-  * numerical stability. It is slower than HouseholderQR, and faster than FullPivHouseholderQR.
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  * 
-  * \sa MatrixBase::colPivHouseholderQr()
-  */
-template<typename _MatrixType> class ColPivHouseholderQR
-        : public SolverBase<ColPivHouseholderQR<_MatrixType> >
-{
-  public:
+ *
+ * \class ColPivHouseholderQR
+ *
+ * \brief Householder rank-revealing QR decomposition of a matrix with column-pivoting
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the QR decomposition
+ *
+ * This class performs a rank-revealing QR decomposition of a matrix \b A into matrices \b P, \b Q and \b R
+ * such that
+ * \f[
+ *  \mathbf{A} \, \mathbf{P} = \mathbf{Q} \, \mathbf{R}
+ * \f]
+ * by using Householder transformations. Here, \b P is a permutation matrix, \b Q a unitary matrix and \b R an
+ * upper triangular matrix.
+ *
+ * This decomposition performs column pivoting in order to be rank-revealing and improve
+ * numerical stability. It is slower than HouseholderQR, and faster than FullPivHouseholderQR.
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * \sa MatrixBase::colPivHouseholderQr()
+ */
+template <typename MatrixType_, typename PermutationIndex_>
+class ColPivHouseholderQR : public SolverBase<ColPivHouseholderQR<MatrixType_, PermutationIndex_>> {
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef SolverBase<ColPivHouseholderQR> Base;
+  friend class SolverBase<ColPivHouseholderQR>;
+  typedef PermutationIndex_ PermutationIndex;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(ColPivHouseholderQR)
 
-    typedef _MatrixType MatrixType;
-    typedef SolverBase<ColPivHouseholderQR> Base;
-    friend class SolverBase<ColPivHouseholderQR>;
+  enum {
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
+  typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime, PermutationIndex> PermutationType;
+  typedef typename internal::plain_row_type<MatrixType, PermutationIndex>::type IntRowVectorType;
+  typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
+  typedef typename internal::plain_row_type<MatrixType, RealScalar>::type RealRowVectorType;
+  typedef HouseholderSequence<MatrixType, internal::remove_all_t<typename HCoeffsType::ConjugateReturnType>>
+      HouseholderSequenceType;
+  typedef typename MatrixType::PlainObject PlainObject;
 
-    EIGEN_GENERIC_PUBLIC_INTERFACE(ColPivHouseholderQR)
-    enum {
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
-    typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationType;
-    typedef typename internal::plain_row_type<MatrixType, Index>::type IntRowVectorType;
-    typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
-    typedef typename internal::plain_row_type<MatrixType, RealScalar>::type RealRowVectorType;
-    typedef HouseholderSequence<MatrixType,typename internal::remove_all<typename HCoeffsType::ConjugateReturnType>::type> HouseholderSequenceType;
-    typedef typename MatrixType::PlainObject PlainObject;
+ private:
+  void init(Index rows, Index cols) {
+    Index diag = numext::mini(rows, cols);
+    m_hCoeffs.resize(diag);
+    m_colsPermutation.resize(cols);
+    m_colsTranspositions.resize(cols);
+    m_temp.resize(cols);
+    m_colNormsUpdated.resize(cols);
+    m_colNormsDirect.resize(cols);
+    m_isInitialized = false;
+    m_usePrescribedThreshold = false;
+  }
 
-  private:
-
-    typedef typename PermutationType::StorageIndex PermIndexType;
-
-  public:
-
-    /**
-    * \brief Default Constructor.
-    *
-    * The default constructor is useful in cases in which the user intends to
-    * perform decompositions via ColPivHouseholderQR::compute(const MatrixType&).
-    */
-    ColPivHouseholderQR()
+ public:
+  /**
+   * \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via ColPivHouseholderQR::compute(const MatrixType&).
+   */
+  ColPivHouseholderQR()
       : m_qr(),
         m_hCoeffs(),
         m_colsPermutation(),
@@ -93,398 +103,367 @@
         m_isInitialized(false),
         m_usePrescribedThreshold(false) {}
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa ColPivHouseholderQR()
-      */
-    ColPivHouseholderQR(Index rows, Index cols)
-      : m_qr(rows, cols),
-        m_hCoeffs((std::min)(rows,cols)),
-        m_colsPermutation(PermIndexType(cols)),
-        m_colsTranspositions(cols),
-        m_temp(cols),
-        m_colNormsUpdated(cols),
-        m_colNormsDirect(cols),
-        m_isInitialized(false),
-        m_usePrescribedThreshold(false) {}
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa ColPivHouseholderQR()
+   */
+  ColPivHouseholderQR(Index rows, Index cols) : m_qr(rows, cols) { init(rows, cols); }
 
-    /** \brief Constructs a QR factorization from a given matrix
-      *
-      * This constructor computes the QR factorization of the matrix \a matrix by calling
-      * the method compute(). It is a short cut for:
-      *
-      * \code
-      * ColPivHouseholderQR<MatrixType> qr(matrix.rows(), matrix.cols());
-      * qr.compute(matrix);
-      * \endcode
-      *
-      * \sa compute()
-      */
-    template<typename InputType>
-    explicit ColPivHouseholderQR(const EigenBase<InputType>& matrix)
-      : m_qr(matrix.rows(), matrix.cols()),
-        m_hCoeffs((std::min)(matrix.rows(),matrix.cols())),
-        m_colsPermutation(PermIndexType(matrix.cols())),
-        m_colsTranspositions(matrix.cols()),
-        m_temp(matrix.cols()),
-        m_colNormsUpdated(matrix.cols()),
-        m_colNormsDirect(matrix.cols()),
-        m_isInitialized(false),
-        m_usePrescribedThreshold(false)
-    {
-      compute(matrix.derived());
-    }
+  /** \brief Constructs a QR factorization from a given matrix
+   *
+   * This constructor computes the QR factorization of the matrix \a matrix by calling
+   * the method compute(). It is a short cut for:
+   *
+   * \code
+   * ColPivHouseholderQR<MatrixType> qr(matrix.rows(), matrix.cols());
+   * qr.compute(matrix);
+   * \endcode
+   *
+   * \sa compute()
+   */
+  template <typename InputType>
+  explicit ColPivHouseholderQR(const EigenBase<InputType>& matrix) : m_qr(matrix.rows(), matrix.cols()) {
+    init(matrix.rows(), matrix.cols());
+    compute(matrix.derived());
+  }
 
-    /** \brief Constructs a QR factorization from a given matrix
-      *
-      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref.
-      *
-      * \sa ColPivHouseholderQR(const EigenBase&)
-      */
-    template<typename InputType>
-    explicit ColPivHouseholderQR(EigenBase<InputType>& matrix)
-      : m_qr(matrix.derived()),
-        m_hCoeffs((std::min)(matrix.rows(),matrix.cols())),
-        m_colsPermutation(PermIndexType(matrix.cols())),
-        m_colsTranspositions(matrix.cols()),
-        m_temp(matrix.cols()),
-        m_colNormsUpdated(matrix.cols()),
-        m_colNormsDirect(matrix.cols()),
-        m_isInitialized(false),
-        m_usePrescribedThreshold(false)
-    {
-      computeInPlace();
-    }
+  /** \brief Constructs a QR factorization from a given matrix
+   *
+   * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c
+   * MatrixType is a Eigen::Ref.
+   *
+   * \sa ColPivHouseholderQR(const EigenBase&)
+   */
+  template <typename InputType>
+  explicit ColPivHouseholderQR(EigenBase<InputType>& matrix) : m_qr(matrix.derived()) {
+    init(matrix.rows(), matrix.cols());
+    computeInPlace();
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
-      * *this is the QR decomposition, if any exists.
-      *
-      * \param b the right-hand-side of the equation to solve.
-      *
-      * \returns a solution.
-      *
-      * \note_about_checking_solutions
-      *
-      * \note_about_arbitrary_choice_of_solution
-      *
-      * Example: \include ColPivHouseholderQR_solve.cpp
-      * Output: \verbinclude ColPivHouseholderQR_solve.out
-      */
-    template<typename Rhs>
-    inline const Solve<ColPivHouseholderQR, Rhs>
-    solve(const MatrixBase<Rhs>& b) const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
+   * *this is the QR decomposition, if any exists.
+   *
+   * \param b the right-hand-side of the equation to solve.
+   *
+   * \returns a solution.
+   *
+   * \note_about_checking_solutions
+   *
+   * \note_about_arbitrary_choice_of_solution
+   *
+   * Example: \include ColPivHouseholderQR_solve.cpp
+   * Output: \verbinclude ColPivHouseholderQR_solve.out
+   */
+  template <typename Rhs>
+  inline const Solve<ColPivHouseholderQR, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
-    HouseholderSequenceType householderQ() const;
-    HouseholderSequenceType matrixQ() const
-    {
-      return householderQ();
-    }
+  HouseholderSequenceType householderQ() const;
+  HouseholderSequenceType matrixQ() const { return householderQ(); }
 
-    /** \returns a reference to the matrix where the Householder QR decomposition is stored
-      */
-    const MatrixType& matrixQR() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return m_qr;
-    }
+  /** \returns a reference to the matrix where the Householder QR decomposition is stored
+   */
+  const MatrixType& matrixQR() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return m_qr;
+  }
 
-    /** \returns a reference to the matrix where the result Householder QR is stored
-     * \warning The strict lower part of this matrix contains internal values.
-     * Only the upper triangular part should be referenced. To get it, use
-     * \code matrixR().template triangularView<Upper>() \endcode
-     * For rank-deficient matrices, use
-     * \code
-     * matrixR().topLeftCorner(rank(), rank()).template triangularView<Upper>()
-     * \endcode
-     */
-    const MatrixType& matrixR() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return m_qr;
-    }
+  /** \returns a reference to the matrix where the result Householder QR is stored
+   * \warning The strict lower part of this matrix contains internal values.
+   * Only the upper triangular part should be referenced. To get it, use
+   * \code matrixR().template triangularView<Upper>() \endcode
+   * For rank-deficient matrices, use
+   * \code
+   * matrixR().topLeftCorner(rank(), rank()).template triangularView<Upper>()
+   * \endcode
+   */
+  const MatrixType& matrixR() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return m_qr;
+  }
 
-    template<typename InputType>
-    ColPivHouseholderQR& compute(const EigenBase<InputType>& matrix);
+  template <typename InputType>
+  ColPivHouseholderQR& compute(const EigenBase<InputType>& matrix);
 
-    /** \returns a const reference to the column permutation matrix */
-    const PermutationType& colsPermutation() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return m_colsPermutation;
-    }
+  /** \returns a const reference to the column permutation matrix */
+  const PermutationType& colsPermutation() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return m_colsPermutation;
+  }
 
-    /** \returns the absolute value of the determinant of the matrix of which
-      * *this is the QR decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the QR decomposition has already been computed.
-      *
-      * \note This is only for square matrices.
-      *
-      * \warning a determinant can be very big or small, so for matrices
-      * of large enough dimension, there is a risk of overflow/underflow.
-      * One way to work around that is to use logAbsDeterminant() instead.
-      *
-      * \sa logAbsDeterminant(), MatrixBase::determinant()
-      */
-    typename MatrixType::RealScalar absDeterminant() const;
+  /** \returns the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa absDeterminant(), logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::Scalar determinant() const;
 
-    /** \returns the natural log of the absolute value of the determinant of the matrix of which
-      * *this is the QR decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the QR decomposition has already been computed.
-      *
-      * \note This is only for square matrices.
-      *
-      * \note This method is useful to work around the risk of overflow/underflow that's inherent
-      * to determinant computation.
-      *
-      * \sa absDeterminant(), MatrixBase::determinant()
-      */
-    typename MatrixType::RealScalar logAbsDeterminant() const;
+  /** \returns the absolute value of the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa determinant(), logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar absDeterminant() const;
 
-    /** \returns the rank of the matrix of which *this is the QR decomposition.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline Index rank() const
-    {
-      using std::abs;
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      RealScalar premultiplied_threshold = abs(m_maxpivot) * threshold();
-      Index result = 0;
-      for(Index i = 0; i < m_nonzero_pivots; ++i)
-        result += (abs(m_qr.coeff(i,i)) > premultiplied_threshold);
-      return result;
-    }
+  /** \returns the natural log of the absolute value of the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \note This method is useful to work around the risk of overflow/underflow that's inherent
+   * to determinant computation.
+   *
+   * \sa determinant(), absDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar logAbsDeterminant() const;
 
-    /** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline Index dimensionOfKernel() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return cols() - rank();
-    }
+  /** \returns the rank of the matrix of which *this is the QR decomposition.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline Index rank() const {
+    using std::abs;
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    RealScalar premultiplied_threshold = abs(m_maxpivot) * threshold();
+    Index result = 0;
+    for (Index i = 0; i < m_nonzero_pivots; ++i) result += (abs(m_qr.coeff(i, i)) > premultiplied_threshold);
+    return result;
+  }
 
-    /** \returns true if the matrix of which *this is the QR decomposition represents an injective
-      *          linear map, i.e. has trivial kernel; false otherwise.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isInjective() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return rank() == cols();
-    }
+  /** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline Index dimensionOfKernel() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return cols() - rank();
+  }
 
-    /** \returns true if the matrix of which *this is the QR decomposition represents a surjective
-      *          linear map; false otherwise.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isSurjective() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return rank() == rows();
-    }
+  /** \returns true if the matrix of which *this is the QR decomposition represents an injective
+   *          linear map, i.e. has trivial kernel; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isInjective() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return rank() == cols();
+  }
 
-    /** \returns true if the matrix of which *this is the QR decomposition is invertible.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isInvertible() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return isInjective() && isSurjective();
-    }
+  /** \returns true if the matrix of which *this is the QR decomposition represents a surjective
+   *          linear map; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isSurjective() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return rank() == rows();
+  }
 
-    /** \returns the inverse of the matrix of which *this is the QR decomposition.
-      *
-      * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
-      *       Use isInvertible() to first determine whether this matrix is invertible.
-      */
-    inline const Inverse<ColPivHouseholderQR> inverse() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return Inverse<ColPivHouseholderQR>(*this);
-    }
+  /** \returns true if the matrix of which *this is the QR decomposition is invertible.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isInvertible() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return isInjective() && isSurjective();
+  }
 
-    inline Index rows() const { return m_qr.rows(); }
-    inline Index cols() const { return m_qr.cols(); }
+  /** \returns the inverse of the matrix of which *this is the QR decomposition.
+   *
+   * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
+   *       Use isInvertible() to first determine whether this matrix is invertible.
+   */
+  inline const Inverse<ColPivHouseholderQR> inverse() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return Inverse<ColPivHouseholderQR>(*this);
+  }
 
-    /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
-      *
-      * For advanced uses only.
-      */
-    const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
+  inline Index rows() const { return m_qr.rows(); }
+  inline Index cols() const { return m_qr.cols(); }
 
-    /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
-      * who need to determine when pivots are to be considered nonzero. This is not used for the
-      * QR decomposition itself.
-      *
-      * When it needs to get the threshold value, Eigen calls threshold(). By default, this
-      * uses a formula to automatically determine a reasonable threshold.
-      * Once you have called the present method setThreshold(const RealScalar&),
-      * your value is used instead.
-      *
-      * \param threshold The new value to use as the threshold.
-      *
-      * A pivot will be considered nonzero if its absolute value is strictly greater than
-      *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
-      * where maxpivot is the biggest pivot.
-      *
-      * If you want to come back to the default behavior, call setThreshold(Default_t)
-      */
-    ColPivHouseholderQR& setThreshold(const RealScalar& threshold)
-    {
-      m_usePrescribedThreshold = true;
-      m_prescribedThreshold = threshold;
-      return *this;
-    }
+  /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
+   *
+   * For advanced uses only.
+   */
+  const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
 
-    /** Allows to come back to the default behavior, letting Eigen use its default formula for
-      * determining the threshold.
-      *
-      * You should pass the special object Eigen::Default as parameter here.
-      * \code qr.setThreshold(Eigen::Default); \endcode
-      *
-      * See the documentation of setThreshold(const RealScalar&).
-      */
-    ColPivHouseholderQR& setThreshold(Default_t)
-    {
-      m_usePrescribedThreshold = false;
-      return *this;
-    }
+  /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
+   * who need to determine when pivots are to be considered nonzero. This is not used for the
+   * QR decomposition itself.
+   *
+   * When it needs to get the threshold value, Eigen calls threshold(). By default, this
+   * uses a formula to automatically determine a reasonable threshold.
+   * Once you have called the present method setThreshold(const RealScalar&),
+   * your value is used instead.
+   *
+   * \param threshold The new value to use as the threshold.
+   *
+   * A pivot will be considered nonzero if its absolute value is strictly greater than
+   *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
+   * where maxpivot is the biggest pivot.
+   *
+   * If you want to come back to the default behavior, call setThreshold(Default_t)
+   */
+  ColPivHouseholderQR& setThreshold(const RealScalar& threshold) {
+    m_usePrescribedThreshold = true;
+    m_prescribedThreshold = threshold;
+    return *this;
+  }
 
-    /** Returns the threshold that will be used by certain methods such as rank().
-      *
-      * See the documentation of setThreshold(const RealScalar&).
-      */
-    RealScalar threshold() const
-    {
-      eigen_assert(m_isInitialized || m_usePrescribedThreshold);
-      return m_usePrescribedThreshold ? m_prescribedThreshold
-      // this formula comes from experimenting (see "LU precision tuning" thread on the list)
-      // and turns out to be identical to Higham's formula used already in LDLt.
-                                      : NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
-    }
+  /** Allows to come back to the default behavior, letting Eigen use its default formula for
+   * determining the threshold.
+   *
+   * You should pass the special object Eigen::Default as parameter here.
+   * \code qr.setThreshold(Eigen::Default); \endcode
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  ColPivHouseholderQR& setThreshold(Default_t) {
+    m_usePrescribedThreshold = false;
+    return *this;
+  }
 
-    /** \returns the number of nonzero pivots in the QR decomposition.
-      * Here nonzero is meant in the exact sense, not in a fuzzy sense.
-      * So that notion isn't really intrinsically interesting, but it is
-      * still useful when implementing algorithms.
-      *
-      * \sa rank()
-      */
-    inline Index nonzeroPivots() const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return m_nonzero_pivots;
-    }
+  /** Returns the threshold that will be used by certain methods such as rank().
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  RealScalar threshold() const {
+    eigen_assert(m_isInitialized || m_usePrescribedThreshold);
+    return m_usePrescribedThreshold ? m_prescribedThreshold
+                                    // this formula comes from experimenting (see "LU precision tuning" thread on the
+                                    // list) and turns out to be identical to Higham's formula used already in LDLt.
+                                    : NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
+  }
 
-    /** \returns the absolute value of the biggest pivot, i.e. the biggest
-      *          diagonal coefficient of R.
-      */
-    RealScalar maxPivot() const { return m_maxpivot; }
+  /** \returns the number of nonzero pivots in the QR decomposition.
+   * Here nonzero is meant in the exact sense, not in a fuzzy sense.
+   * So that notion isn't really intrinsically interesting, but it is
+   * still useful when implementing algorithms.
+   *
+   * \sa rank()
+   */
+  inline Index nonzeroPivots() const {
+    eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
+    return m_nonzero_pivots;
+  }
 
-    /** \brief Reports whether the QR factorization was successful.
-      *
-      * \note This function always returns \c Success. It is provided for compatibility
-      * with other factorization routines.
-      * \returns \c Success
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
-      return Success;
-    }
+  /** \returns the absolute value of the biggest pivot, i.e. the biggest
+   *          diagonal coefficient of R.
+   */
+  RealScalar maxPivot() const { return m_maxpivot; }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename RhsType, typename DstType>
-    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+  /** \brief Reports whether the QR factorization was successful.
+   *
+   * \note This function always returns \c Success. It is provided for compatibility
+   * with other factorization routines.
+   * \returns \c Success
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+    return Success;
+  }
 
-    template<bool Conjugate, typename RhsType, typename DstType>
-    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
-    #endif
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-  protected:
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
+#endif
 
-    friend class CompleteOrthogonalDecomposition<MatrixType>;
+ protected:
+  friend class CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>;
 
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-    void computeInPlace();
+  void computeInPlace();
 
-    MatrixType m_qr;
-    HCoeffsType m_hCoeffs;
-    PermutationType m_colsPermutation;
-    IntRowVectorType m_colsTranspositions;
-    RowVectorType m_temp;
-    RealRowVectorType m_colNormsUpdated;
-    RealRowVectorType m_colNormsDirect;
-    bool m_isInitialized, m_usePrescribedThreshold;
-    RealScalar m_prescribedThreshold, m_maxpivot;
-    Index m_nonzero_pivots;
-    Index m_det_pq;
+  MatrixType m_qr;
+  HCoeffsType m_hCoeffs;
+  PermutationType m_colsPermutation;
+  IntRowVectorType m_colsTranspositions;
+  RowVectorType m_temp;
+  RealRowVectorType m_colNormsUpdated;
+  RealRowVectorType m_colNormsDirect;
+  bool m_isInitialized, m_usePrescribedThreshold;
+  RealScalar m_prescribedThreshold, m_maxpivot;
+  Index m_nonzero_pivots;
+  Index m_det_p;
 };
 
-template<typename MatrixType>
-typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType>::absDeterminant() const
-{
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::Scalar ColPivHouseholderQR<MatrixType, PermutationIndex>::determinant() const {
+  eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
+  eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
+  Scalar detQ;
+  internal::householder_determinant<HCoeffsType, Scalar, NumTraits<Scalar>::IsComplex>::run(m_hCoeffs, detQ);
+  return m_qr.diagonal().prod() * detQ * Scalar(m_det_p);
+}
+
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType, PermutationIndex>::absDeterminant() const {
   using std::abs;
   eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
   return abs(m_qr.diagonal().prod());
 }
 
-template<typename MatrixType>
-typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType>::logAbsDeterminant() const
-{
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType, PermutationIndex>::logAbsDeterminant() const {
   eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
   return m_qr.diagonal().cwiseAbs().array().log().sum();
 }
 
 /** Performs the QR factorization of the given matrix \a matrix. The result of
-  * the factorization is stored into \c *this, and a reference to \c *this
-  * is returned.
-  *
-  * \sa class ColPivHouseholderQR, ColPivHouseholderQR(const MatrixType&)
-  */
-template<typename MatrixType>
-template<typename InputType>
-ColPivHouseholderQR<MatrixType>& ColPivHouseholderQR<MatrixType>::compute(const EigenBase<InputType>& matrix)
-{
+ * the factorization is stored into \c *this, and a reference to \c *this
+ * is returned.
+ *
+ * \sa class ColPivHouseholderQR, ColPivHouseholderQR(const MatrixType&)
+ */
+template <typename MatrixType, typename PermutationIndex>
+template <typename InputType>
+ColPivHouseholderQR<MatrixType, PermutationIndex>& ColPivHouseholderQR<MatrixType, PermutationIndex>::compute(
+    const EigenBase<InputType>& matrix) {
   m_qr = matrix.derived();
   computeInPlace();
   return *this;
 }
 
-template<typename MatrixType>
-void ColPivHouseholderQR<MatrixType>::computeInPlace()
-{
-  check_template_parameters();
-
-  // the column permutation is stored as int indices, so just to be sure:
-  eigen_assert(m_qr.cols()<=NumTraits<int>::highest());
+template <typename MatrixType, typename PermutationIndex>
+void ColPivHouseholderQR<MatrixType, PermutationIndex>::computeInPlace() {
+  eigen_assert(m_qr.cols() <= NumTraits<PermutationIndex>::highest());
 
   using std::abs;
 
@@ -508,27 +487,26 @@
     m_colNormsUpdated.coeffRef(k) = m_colNormsDirect.coeffRef(k);
   }
 
-  RealScalar threshold_helper =  numext::abs2<RealScalar>(m_colNormsUpdated.maxCoeff() * NumTraits<RealScalar>::epsilon()) / RealScalar(rows);
+  RealScalar threshold_helper =
+      numext::abs2<RealScalar>(m_colNormsUpdated.maxCoeff() * NumTraits<RealScalar>::epsilon()) / RealScalar(rows);
   RealScalar norm_downdate_threshold = numext::sqrt(NumTraits<RealScalar>::epsilon());
 
-  m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
+  m_nonzero_pivots = size;  // the generic case is that in which all pivots are nonzero (invertible case)
   m_maxpivot = RealScalar(0);
 
-  for(Index k = 0; k < size; ++k)
-  {
+  for (Index k = 0; k < size; ++k) {
     // first, we look up in our table m_colNormsUpdated which column has the biggest norm
     Index biggest_col_index;
-    RealScalar biggest_col_sq_norm = numext::abs2(m_colNormsUpdated.tail(cols-k).maxCoeff(&biggest_col_index));
+    RealScalar biggest_col_sq_norm = numext::abs2(m_colNormsUpdated.tail(cols - k).maxCoeff(&biggest_col_index));
     biggest_col_index += k;
 
     // Track the number of meaningful pivots but do not stop the decomposition to make
     // sure that the initial matrix is properly reproduced. See bug 941.
-    if(m_nonzero_pivots==size && biggest_col_sq_norm < threshold_helper * RealScalar(rows-k))
-      m_nonzero_pivots = k;
+    if (m_nonzero_pivots == size && biggest_col_sq_norm < threshold_helper * RealScalar(rows - k)) m_nonzero_pivots = k;
 
     // apply the transposition to the columns
-    m_colsTranspositions.coeffRef(k) = biggest_col_index;
-    if(k != biggest_col_index) {
+    m_colsTranspositions.coeffRef(k) = static_cast<PermutationIndex>(biggest_col_index);
+    if (k != biggest_col_index) {
       m_qr.col(k).swap(m_qr.col(biggest_col_index));
       std::swap(m_colNormsUpdated.coeffRef(k), m_colNormsUpdated.coeffRef(biggest_col_index));
       std::swap(m_colNormsDirect.coeffRef(k), m_colNormsDirect.coeffRef(biggest_col_index));
@@ -537,17 +515,17 @@
 
     // generate the householder vector, store it below the diagonal
     RealScalar beta;
-    m_qr.col(k).tail(rows-k).makeHouseholderInPlace(m_hCoeffs.coeffRef(k), beta);
+    m_qr.col(k).tail(rows - k).makeHouseholderInPlace(m_hCoeffs.coeffRef(k), beta);
 
     // apply the householder transformation to the diagonal coefficient
-    m_qr.coeffRef(k,k) = beta;
+    m_qr.coeffRef(k, k) = beta;
 
     // remember the maximum absolute value of diagonal coefficients
-    if(abs(beta) > m_maxpivot) m_maxpivot = abs(beta);
+    if (abs(beta) > m_maxpivot) m_maxpivot = abs(beta);
 
     // apply the householder transformation
-    m_qr.bottomRightCorner(rows-k, cols-k-1)
-        .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows-k-1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k+1));
+    m_qr.bottomRightCorner(rows - k, cols - k - 1)
+        .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows - k - 1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k + 1));
 
     // update our table of norms of the columns
     for (Index j = k + 1; j < cols; ++j) {
@@ -555,12 +533,12 @@
       // http://www.netlib.org/lapack/lawnspdf/lawn176.pdf
       // and used in LAPACK routines xGEQPF and xGEQP3.
       // See lines 278-297 in http://www.netlib.org/lapack/explore-html/dc/df4/sgeqpf_8f_source.html
-      if (m_colNormsUpdated.coeffRef(j) != RealScalar(0)) {
+      if (!numext::is_exactly_zero(m_colNormsUpdated.coeffRef(j))) {
         RealScalar temp = abs(m_qr.coeffRef(k, j)) / m_colNormsUpdated.coeffRef(j);
         temp = (RealScalar(1) + temp) * (RealScalar(1) - temp);
-        temp = temp <  RealScalar(0) ? RealScalar(0) : temp;
-        RealScalar temp2 = temp * numext::abs2<RealScalar>(m_colNormsUpdated.coeffRef(j) /
-                                                           m_colNormsDirect.coeffRef(j));
+        temp = temp < RealScalar(0) ? RealScalar(0) : temp;
+        RealScalar temp2 =
+            temp * numext::abs2<RealScalar>(m_colNormsUpdated.coeffRef(j) / m_colNormsDirect.coeffRef(j));
         if (temp2 <= norm_downdate_threshold) {
           // The updated norm has become too inaccurate so re-compute the column
           // norm directly.
@@ -573,102 +551,101 @@
     }
   }
 
-  m_colsPermutation.setIdentity(PermIndexType(cols));
-  for(PermIndexType k = 0; k < size/*m_nonzero_pivots*/; ++k)
-    m_colsPermutation.applyTranspositionOnTheRight(k, PermIndexType(m_colsTranspositions.coeff(k)));
+  m_colsPermutation.setIdentity(cols);
+  for (Index k = 0; k < size /*m_nonzero_pivots*/; ++k)
+    m_colsPermutation.applyTranspositionOnTheRight(k, static_cast<Index>(m_colsTranspositions.coeff(k)));
 
-  m_det_pq = (number_of_transpositions%2) ? -1 : 1;
+  m_det_p = (number_of_transpositions % 2) ? -1 : 1;
   m_isInitialized = true;
 }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename _MatrixType>
-template<typename RhsType, typename DstType>
-void ColPivHouseholderQR<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, typename PermutationIndex_>
+template <typename RhsType, typename DstType>
+void ColPivHouseholderQR<MatrixType_, PermutationIndex_>::_solve_impl(const RhsType& rhs, DstType& dst) const {
   const Index nonzero_pivots = nonzeroPivots();
 
-  if(nonzero_pivots == 0)
-  {
+  if (nonzero_pivots == 0) {
     dst.setZero();
     return;
   }
 
   typename RhsType::PlainObject c(rhs);
 
-  c.applyOnTheLeft(householderQ().setLength(nonzero_pivots).adjoint() );
+  c.applyOnTheLeft(householderQ().setLength(nonzero_pivots).adjoint());
 
   m_qr.topLeftCorner(nonzero_pivots, nonzero_pivots)
       .template triangularView<Upper>()
       .solveInPlace(c.topRows(nonzero_pivots));
 
-  for(Index i = 0; i < nonzero_pivots; ++i) dst.row(m_colsPermutation.indices().coeff(i)) = c.row(i);
-  for(Index i = nonzero_pivots; i < cols(); ++i) dst.row(m_colsPermutation.indices().coeff(i)).setZero();
+  for (Index i = 0; i < nonzero_pivots; ++i) dst.row(m_colsPermutation.indices().coeff(i)) = c.row(i);
+  for (Index i = nonzero_pivots; i < cols(); ++i) dst.row(m_colsPermutation.indices().coeff(i)).setZero();
 }
 
-template<typename _MatrixType>
-template<bool Conjugate, typename RhsType, typename DstType>
-void ColPivHouseholderQR<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, typename PermutationIndex_>
+template <bool Conjugate, typename RhsType, typename DstType>
+void ColPivHouseholderQR<MatrixType_, PermutationIndex_>::_solve_impl_transposed(const RhsType& rhs,
+                                                                                 DstType& dst) const {
   const Index nonzero_pivots = nonzeroPivots();
 
-  if(nonzero_pivots == 0)
-  {
+  if (nonzero_pivots == 0) {
     dst.setZero();
     return;
   }
 
-  typename RhsType::PlainObject c(m_colsPermutation.transpose()*rhs);
+  typename RhsType::PlainObject c(m_colsPermutation.transpose() * rhs);
 
   m_qr.topLeftCorner(nonzero_pivots, nonzero_pivots)
-        .template triangularView<Upper>()
-        .transpose().template conjugateIf<Conjugate>()
-        .solveInPlace(c.topRows(nonzero_pivots));
+      .template triangularView<Upper>()
+      .transpose()
+      .template conjugateIf<Conjugate>()
+      .solveInPlace(c.topRows(nonzero_pivots));
 
   dst.topRows(nonzero_pivots) = c.topRows(nonzero_pivots);
-  dst.bottomRows(rows()-nonzero_pivots).setZero();
+  dst.bottomRows(rows() - nonzero_pivots).setZero();
 
-  dst.applyOnTheLeft(householderQ().setLength(nonzero_pivots).template conjugateIf<!Conjugate>() );
+  dst.applyOnTheLeft(householderQ().setLength(nonzero_pivots).template conjugateIf<!Conjugate>());
 }
 #endif
 
 namespace internal {
 
-template<typename DstXprType, typename MatrixType>
-struct Assignment<DstXprType, Inverse<ColPivHouseholderQR<MatrixType> >, internal::assign_op<typename DstXprType::Scalar,typename ColPivHouseholderQR<MatrixType>::Scalar>, Dense2Dense>
-{
-  typedef ColPivHouseholderQR<MatrixType> QrType;
+template <typename DstXprType, typename MatrixType, typename PermutationIndex>
+struct Assignment<DstXprType, Inverse<ColPivHouseholderQR<MatrixType, PermutationIndex>>,
+                  internal::assign_op<typename DstXprType::Scalar,
+                                      typename ColPivHouseholderQR<MatrixType, PermutationIndex>::Scalar>,
+                  Dense2Dense> {
+  typedef ColPivHouseholderQR<MatrixType, PermutationIndex> QrType;
   typedef Inverse<QrType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename QrType::Scalar> &)
-  {
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename QrType::Scalar>&) {
     dst = src.nestedExpression().solve(MatrixType::Identity(src.rows(), src.cols()));
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \returns the matrix Q as a sequence of householder transformations.
-  * You can extract the meaningful part only by using:
-  * \code qr.householderQ().setLength(qr.nonzeroPivots()) \endcode*/
-template<typename MatrixType>
-typename ColPivHouseholderQR<MatrixType>::HouseholderSequenceType ColPivHouseholderQR<MatrixType>
-  ::householderQ() const
-{
+ * You can extract the meaningful part only by using:
+ * \code qr.householderQ().setLength(qr.nonzeroPivots()) \endcode*/
+template <typename MatrixType, typename PermutationIndex>
+typename ColPivHouseholderQR<MatrixType, PermutationIndex>::HouseholderSequenceType
+ColPivHouseholderQR<MatrixType, PermutationIndex>::householderQ() const {
   eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
   return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate());
 }
 
 /** \return the column-pivoting Householder QR decomposition of \c *this.
-  *
-  * \sa class ColPivHouseholderQR
-  */
-template<typename Derived>
-const ColPivHouseholderQR<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::colPivHouseholderQr() const
-{
-  return ColPivHouseholderQR<PlainObject>(eval());
+ *
+ * \sa class ColPivHouseholderQR
+ */
+template <typename Derived>
+template <typename PermutationIndexType>
+const ColPivHouseholderQR<typename MatrixBase<Derived>::PlainObject, PermutationIndexType>
+MatrixBase<Derived>::colPivHouseholderQr() const {
+  return ColPivHouseholderQR<PlainObject, PermutationIndexType>(eval());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COLPIVOTINGHOUSEHOLDERQR_H
+#endif  // EIGEN_COLPIVOTINGHOUSEHOLDERQR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h
index 486d337..8566e96 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h
@@ -10,75 +10,69 @@
 #ifndef EIGEN_COMPLETEORTHOGONALDECOMPOSITION_H
 #define EIGEN_COMPLETEORTHOGONALDECOMPOSITION_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template <typename _MatrixType>
-struct traits<CompleteOrthogonalDecomposition<_MatrixType> >
-    : traits<_MatrixType> {
+template <typename MatrixType_, typename PermutationIndex_>
+struct traits<CompleteOrthogonalDecomposition<MatrixType_, PermutationIndex_>> : traits<MatrixType_> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
-  typedef int StorageIndex;
+  typedef PermutationIndex_ PermutationIndex;
   enum { Flags = 0 };
 };
 
 }  // end namespace internal
 
 /** \ingroup QR_Module
-  *
-  * \class CompleteOrthogonalDecomposition
-  *
-  * \brief Complete orthogonal decomposition (COD) of a matrix.
-  *
-  * \param MatrixType the type of the matrix of which we are computing the COD.
-  *
-  * This class performs a rank-revealing complete orthogonal decomposition of a
-  * matrix  \b A into matrices \b P, \b Q, \b T, and \b Z such that
-  * \f[
-  *  \mathbf{A} \, \mathbf{P} = \mathbf{Q} \,
-  *                     \begin{bmatrix} \mathbf{T} &  \mathbf{0} \\
-  *                                     \mathbf{0} & \mathbf{0} \end{bmatrix} \, \mathbf{Z}
-  * \f]
-  * by using Householder transformations. Here, \b P is a permutation matrix,
-  * \b Q and \b Z are unitary matrices and \b T an upper triangular matrix of
-  * size rank-by-rank. \b A may be rank deficient.
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  * 
-  * \sa MatrixBase::completeOrthogonalDecomposition()
-  */
-template <typename _MatrixType> class CompleteOrthogonalDecomposition
-          : public SolverBase<CompleteOrthogonalDecomposition<_MatrixType> >
-{
+ *
+ * \class CompleteOrthogonalDecomposition
+ *
+ * \brief Complete orthogonal decomposition (COD) of a matrix.
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the COD.
+ *
+ * This class performs a rank-revealing complete orthogonal decomposition of a
+ * matrix  \b A into matrices \b P, \b Q, \b T, and \b Z such that
+ * \f[
+ *  \mathbf{A} \, \mathbf{P} = \mathbf{Q} \,
+ *                     \begin{bmatrix} \mathbf{T} &  \mathbf{0} \\
+ *                                     \mathbf{0} & \mathbf{0} \end{bmatrix} \, \mathbf{Z}
+ * \f]
+ * by using Householder transformations. Here, \b P is a permutation matrix,
+ * \b Q and \b Z are unitary matrices and \b T an upper triangular matrix of
+ * size rank-by-rank. \b A may be rank deficient.
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * \sa MatrixBase::completeOrthogonalDecomposition()
+ */
+template <typename MatrixType_, typename PermutationIndex_>
+class CompleteOrthogonalDecomposition
+    : public SolverBase<CompleteOrthogonalDecomposition<MatrixType_, PermutationIndex_>> {
  public:
-  typedef _MatrixType MatrixType;
+  typedef MatrixType_ MatrixType;
   typedef SolverBase<CompleteOrthogonalDecomposition> Base;
 
-  template<typename Derived>
+  template <typename Derived>
   friend struct internal::solve_assertion;
-
+  typedef PermutationIndex_ PermutationIndex;
   EIGEN_GENERIC_PUBLIC_INTERFACE(CompleteOrthogonalDecomposition)
   enum {
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
   };
   typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
-  typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime>
-      PermutationType;
-  typedef typename internal::plain_row_type<MatrixType, Index>::type
-      IntRowVectorType;
+  typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime, PermutationIndex> PermutationType;
+  typedef typename internal::plain_row_type<MatrixType, Index>::type IntRowVectorType;
   typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
-  typedef typename internal::plain_row_type<MatrixType, RealScalar>::type
-      RealRowVectorType;
-  typedef HouseholderSequence<
-      MatrixType, typename internal::remove_all<
-                      typename HCoeffsType::ConjugateReturnType>::type>
+  typedef typename internal::plain_row_type<MatrixType, RealScalar>::type RealRowVectorType;
+  typedef HouseholderSequence<MatrixType, internal::remove_all_t<typename HCoeffsType::ConjugateReturnType>>
       HouseholderSequenceType;
   typedef typename MatrixType::PlainObject PlainObject;
 
- private:
-  typedef typename PermutationType::Index PermIndexType;
-
  public:
   /**
    * \brief Default Constructor.
@@ -118,27 +112,24 @@
   explicit CompleteOrthogonalDecomposition(const EigenBase<InputType>& matrix)
       : m_cpqr(matrix.rows(), matrix.cols()),
         m_zCoeffs((std::min)(matrix.rows(), matrix.cols())),
-        m_temp(matrix.cols())
-  {
+        m_temp(matrix.cols()) {
     compute(matrix.derived());
   }
 
   /** \brief Constructs a complete orthogonal decomposition from a given matrix
-    *
-    * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref.
-    *
-    * \sa CompleteOrthogonalDecomposition(const EigenBase&)
-    */
-  template<typename InputType>
+   *
+   * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c
+   * MatrixType is a Eigen::Ref.
+   *
+   * \sa CompleteOrthogonalDecomposition(const EigenBase&)
+   */
+  template <typename InputType>
   explicit CompleteOrthogonalDecomposition(EigenBase<InputType>& matrix)
-    : m_cpqr(matrix.derived()),
-      m_zCoeffs((std::min)(matrix.rows(), matrix.cols())),
-      m_temp(matrix.cols())
-  {
+      : m_cpqr(matrix.derived()), m_zCoeffs((std::min)(matrix.rows(), matrix.cols())), m_temp(matrix.cols()) {
     computeInPlace();
-  } 
+  }
 
-  #ifdef EIGEN_PARSED_BY_DOXYGEN
+#ifdef EIGEN_PARSED_BY_DOXYGEN
   /** This method computes the minimum-norm solution X to a least squares
    * problem \f[\mathrm{minimize} \|A X - B\|, \f] where \b A is the matrix of
    * which \c *this is the complete orthogonal decomposition.
@@ -149,9 +140,8 @@
    *
    */
   template <typename Rhs>
-  inline const Solve<CompleteOrthogonalDecomposition, Rhs> solve(
-      const MatrixBase<Rhs>& b) const;
-  #endif
+  inline const Solve<CompleteOrthogonalDecomposition, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
   HouseholderSequenceType householderQ(void) const;
   HouseholderSequenceType matrixQ(void) const { return m_cpqr.householderQ(); }
@@ -177,7 +167,7 @@
    * \code matrixT().template triangularView<Upper>() \endcode
    * For rank-deficient matrices, use
    * \code
-   * matrixR().topLeftCorner(rank(), rank()).template triangularView<Upper>()
+   * matrixT().topLeftCorner(rank(), rank()).template triangularView<Upper>()
    * \endcode
    */
   const MatrixType& matrixT() const { return m_cpqr.matrixQR(); }
@@ -191,9 +181,22 @@
   }
 
   /** \returns a const reference to the column permutation matrix */
-  const PermutationType& colsPermutation() const {
-    return m_cpqr.colsPermutation();
-  }
+  const PermutationType& colsPermutation() const { return m_cpqr.colsPermutation(); }
+
+  /** \returns the determinant of the matrix of which
+   * *this is the complete orthogonal decomposition. It has only linear
+   * complexity (that is, O(n) where n is the dimension of the square matrix)
+   * as the complete orthogonal decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa absDeterminant(), logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::Scalar determinant() const;
 
   /** \returns the absolute value of the determinant of the matrix of which
    * *this is the complete orthogonal decomposition. It has only linear
@@ -206,7 +209,7 @@
    * of large enough dimension, there is a risk of overflow/underflow.
    * One way to work around that is to use logAbsDeterminant() instead.
    *
-   * \sa logAbsDeterminant(), MatrixBase::determinant()
+   * \sa determinant(), logAbsDeterminant(), MatrixBase::determinant()
    */
   typename MatrixType::RealScalar absDeterminant() const;
 
@@ -221,7 +224,7 @@
    * \note This method is useful to work around the risk of overflow/underflow
    * that's inherent to determinant computation.
    *
-   * \sa absDeterminant(), MatrixBase::determinant()
+   * \sa determinant(), absDeterminant(), MatrixBase::determinant()
    */
   typename MatrixType::RealScalar logAbsDeterminant() const;
 
@@ -275,8 +278,7 @@
    * \warning: Do not compute \c this->pseudoInverse()*rhs to solve a linear systems.
    * It is more efficient and numerically stable to call \c this->solve(rhs).
    */
-  inline const Inverse<CompleteOrthogonalDecomposition> pseudoInverse() const
-  {
+  inline const Inverse<CompleteOrthogonalDecomposition> pseudoInverse() const {
     eigen_assert(m_cpqr.m_isInitialized && "CompleteOrthogonalDecomposition is not initialized.");
     return Inverse<CompleteOrthogonalDecomposition>(*this);
   }
@@ -372,26 +374,25 @@
   template <typename RhsType, typename DstType>
   void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-  template<bool Conjugate, typename RhsType, typename DstType>
-  void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
 #endif
 
  protected:
-  static void check_template_parameters() {
-    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-  }
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-  template<bool Transpose_, typename Rhs>
+  template <bool Transpose_, typename Rhs>
   void _check_solve_assertion(const Rhs& b) const {
-      EIGEN_ONLY_USED_FOR_DEBUG(b);
-      eigen_assert(m_cpqr.m_isInitialized && "CompleteOrthogonalDecomposition is not initialized.");
-      eigen_assert((Transpose_?derived().cols():derived().rows())==b.rows() && "CompleteOrthogonalDecomposition::solve(): invalid number of rows of the right hand side matrix b");
+    EIGEN_ONLY_USED_FOR_DEBUG(b);
+    eigen_assert(m_cpqr.m_isInitialized && "CompleteOrthogonalDecomposition is not initialized.");
+    eigen_assert((Transpose_ ? derived().cols() : derived().rows()) == b.rows() &&
+                 "CompleteOrthogonalDecomposition::solve(): invalid number of rows of the right hand side matrix b");
   }
 
   void computeInPlace();
 
   /** Overwrites \b rhs with \f$ \mathbf{Z} * \mathbf{rhs} \f$ or
-   *  \f$ \mathbf{\overline Z} * \mathbf{rhs} \f$ if \c Conjugate 
+   *  \f$ \mathbf{\overline Z} * \mathbf{rhs} \f$ if \c Conjugate
    *  is set to \c true.
    */
   template <bool Conjugate, typename Rhs>
@@ -402,20 +403,24 @@
   template <typename Rhs>
   void applyZAdjointOnTheLeftInPlace(Rhs& rhs) const;
 
-  ColPivHouseholderQR<MatrixType> m_cpqr;
+  ColPivHouseholderQR<MatrixType, PermutationIndex> m_cpqr;
   HCoeffsType m_zCoeffs;
   RowVectorType m_temp;
 };
 
-template <typename MatrixType>
-typename MatrixType::RealScalar
-CompleteOrthogonalDecomposition<MatrixType>::absDeterminant() const {
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::Scalar CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::determinant() const {
+  return m_cpqr.determinant();
+}
+
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::RealScalar CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::absDeterminant() const {
   return m_cpqr.absDeterminant();
 }
 
-template <typename MatrixType>
-typename MatrixType::RealScalar
-CompleteOrthogonalDecomposition<MatrixType>::logAbsDeterminant() const {
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::RealScalar CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::logAbsDeterminant()
+    const {
   return m_cpqr.logAbsDeterminant();
 }
 
@@ -426,13 +431,9 @@
  * \sa class CompleteOrthogonalDecomposition,
  * CompleteOrthogonalDecomposition(const MatrixType&)
  */
-template <typename MatrixType>
-void CompleteOrthogonalDecomposition<MatrixType>::computeInPlace()
-{
-  check_template_parameters();
-
-  // the column permutation is stored as int indices, so just to be sure:
-  eigen_assert(m_cpqr.cols() <= NumTraits<int>::highest());
+template <typename MatrixType, typename PermutationIndex>
+void CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::computeInPlace() {
+  eigen_assert(m_cpqr.cols() <= NumTraits<PermutationIndex>::highest());
 
   const Index rank = m_cpqr.rank();
   const Index cols = m_cpqr.cols();
@@ -457,59 +458,50 @@
         // Given the API for Householder reflectors, it is more convenient if
         // we swap the leading parts of columns k and r-1 (zero-based) to form
         // the matrix X_k = [X(0:k, k), X(0:k, r:n)]
-        m_cpqr.m_qr.col(k).head(k + 1).swap(
-            m_cpqr.m_qr.col(rank - 1).head(k + 1));
+        m_cpqr.m_qr.col(k).head(k + 1).swap(m_cpqr.m_qr.col(rank - 1).head(k + 1));
       }
       // Construct Householder reflector Z(k) to zero out the last row of X_k,
       // i.e. choose Z(k) such that
       // [X(k, k), X(k, r:n)] * Z(k) = [beta, 0, .., 0].
       RealScalar beta;
-      m_cpqr.m_qr.row(k)
-          .tail(cols - rank + 1)
-          .makeHouseholderInPlace(m_zCoeffs(k), beta);
+      m_cpqr.m_qr.row(k).tail(cols - rank + 1).makeHouseholderInPlace(m_zCoeffs(k), beta);
       m_cpqr.m_qr(k, rank - 1) = beta;
       if (k > 0) {
         // Apply Z(k) to the first k rows of X_k
         m_cpqr.m_qr.topRightCorner(k, cols - rank + 1)
-            .applyHouseholderOnTheRight(
-                m_cpqr.m_qr.row(k).tail(cols - rank).adjoint(), m_zCoeffs(k),
-                &m_temp(0));
+            .applyHouseholderOnTheRight(m_cpqr.m_qr.row(k).tail(cols - rank).adjoint(), m_zCoeffs(k), &m_temp(0));
       }
       if (k != rank - 1) {
         // Swap X(0:k,k) back to its proper location.
-        m_cpqr.m_qr.col(k).head(k + 1).swap(
-            m_cpqr.m_qr.col(rank - 1).head(k + 1));
+        m_cpqr.m_qr.col(k).head(k + 1).swap(m_cpqr.m_qr.col(rank - 1).head(k + 1));
       }
     }
   }
 }
 
-template <typename MatrixType>
+template <typename MatrixType, typename PermutationIndex>
 template <bool Conjugate, typename Rhs>
-void CompleteOrthogonalDecomposition<MatrixType>::applyZOnTheLeftInPlace(
-    Rhs& rhs) const {
+void CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::applyZOnTheLeftInPlace(Rhs& rhs) const {
   const Index cols = this->cols();
   const Index nrhs = rhs.cols();
   const Index rank = this->rank();
   Matrix<typename Rhs::Scalar, Dynamic, 1> temp((std::max)(cols, nrhs));
-  for (Index k = rank-1; k >= 0; --k) {
+  for (Index k = rank - 1; k >= 0; --k) {
     if (k != rank - 1) {
       rhs.row(k).swap(rhs.row(rank - 1));
     }
     rhs.middleRows(rank - 1, cols - rank + 1)
-        .applyHouseholderOnTheLeft(
-            matrixQTZ().row(k).tail(cols - rank).transpose().template conjugateIf<!Conjugate>(), zCoeffs().template conjugateIf<Conjugate>()(k),
-            &temp(0));
+        .applyHouseholderOnTheLeft(matrixQTZ().row(k).tail(cols - rank).transpose().template conjugateIf<!Conjugate>(),
+                                   zCoeffs().template conjugateIf<Conjugate>()(k), &temp(0));
     if (k != rank - 1) {
       rhs.row(k).swap(rhs.row(rank - 1));
     }
   }
 }
 
-template <typename MatrixType>
+template <typename MatrixType, typename PermutationIndex>
 template <typename Rhs>
-void CompleteOrthogonalDecomposition<MatrixType>::applyZAdjointOnTheLeftInPlace(
-    Rhs& rhs) const {
+void CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::applyZAdjointOnTheLeftInPlace(Rhs& rhs) const {
   const Index cols = this->cols();
   const Index nrhs = rhs.cols();
   const Index rank = this->rank();
@@ -519,9 +511,7 @@
       rhs.row(k).swap(rhs.row(rank - 1));
     }
     rhs.middleRows(rank - 1, cols - rank + 1)
-        .applyHouseholderOnTheLeft(
-            matrixQTZ().row(k).tail(cols - rank).adjoint(), zCoeffs()(k),
-            &temp(0));
+        .applyHouseholderOnTheLeft(matrixQTZ().row(k).tail(cols - rank).adjoint(), zCoeffs()(k), &temp(0));
     if (k != rank - 1) {
       rhs.row(k).swap(rhs.row(rank - 1));
     }
@@ -529,10 +519,10 @@
 }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template <typename _MatrixType>
+template <typename MatrixType_, typename PermutationIndex_>
 template <typename RhsType, typename DstType>
-void CompleteOrthogonalDecomposition<_MatrixType>::_solve_impl(
-    const RhsType& rhs, DstType& dst) const {
+void CompleteOrthogonalDecomposition<MatrixType_, PermutationIndex_>::_solve_impl(const RhsType& rhs,
+                                                                                  DstType& dst) const {
   const Index rank = this->rank();
   if (rank == 0) {
     dst.setZero();
@@ -544,10 +534,7 @@
   c.applyOnTheLeft(matrixQ().setLength(rank).adjoint());
 
   // Solve T z = c(1:rank, :)
-  dst.topRows(rank) = matrixT()
-                          .topLeftCorner(rank, rank)
-                          .template triangularView<Upper>()
-                          .solve(c.topRows(rank));
+  dst.topRows(rank) = matrixT().topLeftCorner(rank, rank).template triangularView<Upper>().solve(c.topRows(rank));
 
   const Index cols = this->cols();
   if (rank < cols) {
@@ -561,10 +548,10 @@
   dst = colsPermutation() * dst;
 }
 
-template<typename _MatrixType>
-template<bool Conjugate, typename RhsType, typename DstType>
-void CompleteOrthogonalDecomposition<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, typename PermutationIndex_>
+template <bool Conjugate, typename RhsType, typename DstType>
+void CompleteOrthogonalDecomposition<MatrixType_, PermutationIndex_>::_solve_impl_transposed(const RhsType& rhs,
+                                                                                             DstType& dst) const {
   const Index rank = this->rank();
 
   if (rank == 0) {
@@ -572,60 +559,66 @@
     return;
   }
 
-  typename RhsType::PlainObject c(colsPermutation().transpose()*rhs);
+  typename RhsType::PlainObject c(colsPermutation().transpose() * rhs);
 
   if (rank < cols()) {
     applyZOnTheLeftInPlace<!Conjugate>(c);
   }
 
-  matrixT().topLeftCorner(rank, rank)
-           .template triangularView<Upper>()
-           .transpose().template conjugateIf<Conjugate>()
-           .solveInPlace(c.topRows(rank));
+  matrixT()
+      .topLeftCorner(rank, rank)
+      .template triangularView<Upper>()
+      .transpose()
+      .template conjugateIf<Conjugate>()
+      .solveInPlace(c.topRows(rank));
 
   dst.topRows(rank) = c.topRows(rank);
-  dst.bottomRows(rows()-rank).setZero();
+  dst.bottomRows(rows() - rank).setZero();
 
-  dst.applyOnTheLeft(householderQ().setLength(rank).template conjugateIf<!Conjugate>() );
+  dst.applyOnTheLeft(householderQ().setLength(rank).template conjugateIf<!Conjugate>());
 }
 #endif
 
 namespace internal {
 
-template<typename MatrixType>
-struct traits<Inverse<CompleteOrthogonalDecomposition<MatrixType> > >
-  : traits<typename Transpose<typename MatrixType::PlainObject>::PlainObject>
-{
+template <typename MatrixType, typename PermutationIndex>
+struct traits<Inverse<CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>>>
+    : traits<typename Transpose<typename MatrixType::PlainObject>::PlainObject> {
   enum { Flags = 0 };
 };
 
-template<typename DstXprType, typename MatrixType>
-struct Assignment<DstXprType, Inverse<CompleteOrthogonalDecomposition<MatrixType> >, internal::assign_op<typename DstXprType::Scalar,typename CompleteOrthogonalDecomposition<MatrixType>::Scalar>, Dense2Dense>
-{
-  typedef CompleteOrthogonalDecomposition<MatrixType> CodType;
+template <typename DstXprType, typename MatrixType, typename PermutationIndex>
+struct Assignment<DstXprType, Inverse<CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>>,
+                  internal::assign_op<typename DstXprType::Scalar,
+                                      typename CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::Scalar>,
+                  Dense2Dense> {
+  typedef CompleteOrthogonalDecomposition<MatrixType, PermutationIndex> CodType;
   typedef Inverse<CodType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename CodType::Scalar> &)
-  {
-    typedef Matrix<typename CodType::Scalar, CodType::RowsAtCompileTime, CodType::RowsAtCompileTime, 0, CodType::MaxRowsAtCompileTime, CodType::MaxRowsAtCompileTime> IdentityMatrixType;
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename CodType::Scalar>&) {
+    typedef Matrix<typename CodType::Scalar, CodType::RowsAtCompileTime, CodType::RowsAtCompileTime, 0,
+                   CodType::MaxRowsAtCompileTime, CodType::MaxRowsAtCompileTime>
+        IdentityMatrixType;
     dst = src.nestedExpression().solve(IdentityMatrixType::Identity(src.cols(), src.cols()));
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \returns the matrix Q as a sequence of householder transformations */
-template <typename MatrixType>
-typename CompleteOrthogonalDecomposition<MatrixType>::HouseholderSequenceType
-CompleteOrthogonalDecomposition<MatrixType>::householderQ() const {
+template <typename MatrixType, typename PermutationIndex>
+typename CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::HouseholderSequenceType
+CompleteOrthogonalDecomposition<MatrixType, PermutationIndex>::householderQ() const {
   return m_cpqr.householderQ();
 }
 
 /** \return the complete orthogonal decomposition of \c *this.
-  *
-  * \sa class CompleteOrthogonalDecomposition
-  */
+ *
+ * \sa class CompleteOrthogonalDecomposition
+ */
 template <typename Derived>
-const CompleteOrthogonalDecomposition<typename MatrixBase<Derived>::PlainObject>
+template <typename PermutationIndex>
+const CompleteOrthogonalDecomposition<typename MatrixBase<Derived>::PlainObject, PermutationIndex>
 MatrixBase<Derived>::completeOrthogonalDecomposition() const {
   return CompleteOrthogonalDecomposition<PlainObject>(eval());
 }
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h
index d0664a1..d93a5d1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h
@@ -11,82 +11,83 @@
 #ifndef EIGEN_FULLPIVOTINGHOUSEHOLDERQR_H
 #define EIGEN_FULLPIVOTINGHOUSEHOLDERQR_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename _MatrixType> struct traits<FullPivHouseholderQR<_MatrixType> >
- : traits<_MatrixType>
-{
+template <typename MatrixType_, typename PermutationIndex_>
+struct traits<FullPivHouseholderQR<MatrixType_, PermutationIndex_> > : traits<MatrixType_> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
-  typedef int StorageIndex;
+  typedef PermutationIndex_ PermutationIndex;
   enum { Flags = 0 };
 };
 
-template<typename MatrixType> struct FullPivHouseholderQRMatrixQReturnType;
+template <typename MatrixType, typename PermutationIndex>
+struct FullPivHouseholderQRMatrixQReturnType;
 
-template<typename MatrixType>
-struct traits<FullPivHouseholderQRMatrixQReturnType<MatrixType> >
-{
+template <typename MatrixType, typename PermutationIndex>
+struct traits<FullPivHouseholderQRMatrixQReturnType<MatrixType, PermutationIndex> > {
   typedef typename MatrixType::PlainObject ReturnType;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup QR_Module
-  *
-  * \class FullPivHouseholderQR
-  *
-  * \brief Householder rank-revealing QR decomposition of a matrix with full pivoting
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the QR decomposition
-  *
-  * This class performs a rank-revealing QR decomposition of a matrix \b A into matrices \b P, \b P', \b Q and \b R
-  * such that 
-  * \f[
-  *  \mathbf{P} \, \mathbf{A} \, \mathbf{P}' = \mathbf{Q} \, \mathbf{R}
-  * \f]
-  * by using Householder transformations. Here, \b P and \b P' are permutation matrices, \b Q a unitary matrix 
-  * and \b R an upper triangular matrix.
-  *
-  * This decomposition performs a very prudent full pivoting in order to be rank-revealing and achieve optimal
-  * numerical stability. The trade-off is that it is slower than HouseholderQR and ColPivHouseholderQR.
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  * 
-  * \sa MatrixBase::fullPivHouseholderQr()
-  */
-template<typename _MatrixType> class FullPivHouseholderQR
-        : public SolverBase<FullPivHouseholderQR<_MatrixType> >
-{
-  public:
+ *
+ * \class FullPivHouseholderQR
+ *
+ * \brief Householder rank-revealing QR decomposition of a matrix with full pivoting
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the QR decomposition
+ *
+ * This class performs a rank-revealing QR decomposition of a matrix \b A into matrices \b P, \b P', \b Q and \b R
+ * such that
+ * \f[
+ *  \mathbf{P} \, \mathbf{A} \, \mathbf{P}' = \mathbf{Q} \, \mathbf{R}
+ * \f]
+ * by using Householder transformations. Here, \b P and \b P' are permutation matrices, \b Q a unitary matrix
+ * and \b R an upper triangular matrix.
+ *
+ * This decomposition performs a very prudent full pivoting in order to be rank-revealing and achieve optimal
+ * numerical stability. The trade-off is that it is slower than HouseholderQR and ColPivHouseholderQR.
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * \sa MatrixBase::fullPivHouseholderQr()
+ */
+template <typename MatrixType_, typename PermutationIndex_>
+class FullPivHouseholderQR : public SolverBase<FullPivHouseholderQR<MatrixType_, PermutationIndex_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef SolverBase<FullPivHouseholderQR> Base;
+  friend class SolverBase<FullPivHouseholderQR>;
+  typedef PermutationIndex_ PermutationIndex;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(FullPivHouseholderQR)
 
-    typedef _MatrixType MatrixType;
-    typedef SolverBase<FullPivHouseholderQR> Base;
-    friend class SolverBase<FullPivHouseholderQR>;
+  enum {
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  typedef internal::FullPivHouseholderQRMatrixQReturnType<MatrixType, PermutationIndex> MatrixQReturnType;
+  typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
+  typedef Matrix<PermutationIndex, 1, internal::min_size_prefer_dynamic(ColsAtCompileTime, RowsAtCompileTime), RowMajor,
+                 1, internal::min_size_prefer_fixed(MaxColsAtCompileTime, MaxRowsAtCompileTime)>
+      IntDiagSizeVectorType;
+  typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime, PermutationIndex> PermutationType;
+  typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
+  typedef typename internal::plain_col_type<MatrixType>::type ColVectorType;
+  typedef typename MatrixType::PlainObject PlainObject;
 
-    EIGEN_GENERIC_PUBLIC_INTERFACE(FullPivHouseholderQR)
-    enum {
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    typedef internal::FullPivHouseholderQRMatrixQReturnType<MatrixType> MatrixQReturnType;
-    typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
-    typedef Matrix<StorageIndex, 1,
-                   EIGEN_SIZE_MIN_PREFER_DYNAMIC(ColsAtCompileTime,RowsAtCompileTime), RowMajor, 1,
-                   EIGEN_SIZE_MIN_PREFER_FIXED(MaxColsAtCompileTime,MaxRowsAtCompileTime)> IntDiagSizeVectorType;
-    typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationType;
-    typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
-    typedef typename internal::plain_col_type<MatrixType>::type ColVectorType;
-    typedef typename MatrixType::PlainObject PlainObject;
-
-    /** \brief Default Constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via FullPivHouseholderQR::compute(const MatrixType&).
-      */
-    FullPivHouseholderQR()
+  /** \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via FullPivHouseholderQR::compute(const MatrixType&).
+   */
+  FullPivHouseholderQR()
       : m_qr(),
         m_hCoeffs(),
         m_rows_transpositions(),
@@ -96,36 +97,36 @@
         m_isInitialized(false),
         m_usePrescribedThreshold(false) {}
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa FullPivHouseholderQR()
-      */
-    FullPivHouseholderQR(Index rows, Index cols)
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa FullPivHouseholderQR()
+   */
+  FullPivHouseholderQR(Index rows, Index cols)
       : m_qr(rows, cols),
-        m_hCoeffs((std::min)(rows,cols)),
-        m_rows_transpositions((std::min)(rows,cols)),
-        m_cols_transpositions((std::min)(rows,cols)),
+        m_hCoeffs((std::min)(rows, cols)),
+        m_rows_transpositions((std::min)(rows, cols)),
+        m_cols_transpositions((std::min)(rows, cols)),
         m_cols_permutation(cols),
         m_temp(cols),
         m_isInitialized(false),
         m_usePrescribedThreshold(false) {}
 
-    /** \brief Constructs a QR factorization from a given matrix
-      *
-      * This constructor computes the QR factorization of the matrix \a matrix by calling
-      * the method compute(). It is a short cut for:
-      * 
-      * \code
-      * FullPivHouseholderQR<MatrixType> qr(matrix.rows(), matrix.cols());
-      * qr.compute(matrix);
-      * \endcode
-      * 
-      * \sa compute()
-      */
-    template<typename InputType>
-    explicit FullPivHouseholderQR(const EigenBase<InputType>& matrix)
+  /** \brief Constructs a QR factorization from a given matrix
+   *
+   * This constructor computes the QR factorization of the matrix \a matrix by calling
+   * the method compute(). It is a short cut for:
+   *
+   * \code
+   * FullPivHouseholderQR<MatrixType> qr(matrix.rows(), matrix.cols());
+   * qr.compute(matrix);
+   * \endcode
+   *
+   * \sa compute()
+   */
+  template <typename InputType>
+  explicit FullPivHouseholderQR(const EigenBase<InputType>& matrix)
       : m_qr(matrix.rows(), matrix.cols()),
         m_hCoeffs((std::min)(matrix.rows(), matrix.cols())),
         m_rows_transpositions((std::min)(matrix.rows(), matrix.cols())),
@@ -133,19 +134,19 @@
         m_cols_permutation(matrix.cols()),
         m_temp(matrix.cols()),
         m_isInitialized(false),
-        m_usePrescribedThreshold(false)
-    {
-      compute(matrix.derived());
-    }
+        m_usePrescribedThreshold(false) {
+    compute(matrix.derived());
+  }
 
-    /** \brief Constructs a QR factorization from a given matrix
-      *
-      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref.
-      *
-      * \sa FullPivHouseholderQR(const EigenBase&)
-      */
-    template<typename InputType>
-    explicit FullPivHouseholderQR(EigenBase<InputType>& matrix)
+  /** \brief Constructs a QR factorization from a given matrix
+   *
+   * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c
+   * MatrixType is a Eigen::Ref.
+   *
+   * \sa FullPivHouseholderQR(const EigenBase&)
+   */
+  template <typename InputType>
+  explicit FullPivHouseholderQR(EigenBase<InputType>& matrix)
       : m_qr(matrix.derived()),
         m_hCoeffs((std::min)(matrix.rows(), matrix.cols())),
         m_rows_transpositions((std::min)(matrix.rows(), matrix.cols())),
@@ -153,319 +154,318 @@
         m_cols_permutation(matrix.cols()),
         m_temp(matrix.cols()),
         m_isInitialized(false),
-        m_usePrescribedThreshold(false)
-    {
-      computeInPlace();
-    }
+        m_usePrescribedThreshold(false) {
+    computeInPlace();
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
-      * \c *this is the QR decomposition.
-      *
-      * \param b the right-hand-side of the equation to solve.
-      *
-      * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A,
-      * and an arbitrary solution otherwise.
-      *
-      * \note_about_checking_solutions
-      *
-      * \note_about_arbitrary_choice_of_solution
-      *
-      * Example: \include FullPivHouseholderQR_solve.cpp
-      * Output: \verbinclude FullPivHouseholderQR_solve.out
-      */
-    template<typename Rhs>
-    inline const Solve<FullPivHouseholderQR, Rhs>
-    solve(const MatrixBase<Rhs>& b) const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
+   * \c *this is the QR decomposition.
+   *
+   * \param b the right-hand-side of the equation to solve.
+   *
+   * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A,
+   * and an arbitrary solution otherwise.
+   *
+   * \note_about_checking_solutions
+   *
+   * \note_about_arbitrary_choice_of_solution
+   *
+   * Example: \include FullPivHouseholderQR_solve.cpp
+   * Output: \verbinclude FullPivHouseholderQR_solve.out
+   */
+  template <typename Rhs>
+  inline const Solve<FullPivHouseholderQR, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
-    /** \returns Expression object representing the matrix Q
-      */
-    MatrixQReturnType matrixQ(void) const;
+  /** \returns Expression object representing the matrix Q
+   */
+  MatrixQReturnType matrixQ(void) const;
 
-    /** \returns a reference to the matrix where the Householder QR decomposition is stored
-      */
-    const MatrixType& matrixQR() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return m_qr;
-    }
+  /** \returns a reference to the matrix where the Householder QR decomposition is stored
+   */
+  const MatrixType& matrixQR() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return m_qr;
+  }
 
-    template<typename InputType>
-    FullPivHouseholderQR& compute(const EigenBase<InputType>& matrix);
+  template <typename InputType>
+  FullPivHouseholderQR& compute(const EigenBase<InputType>& matrix);
 
-    /** \returns a const reference to the column permutation matrix */
-    const PermutationType& colsPermutation() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return m_cols_permutation;
-    }
+  /** \returns a const reference to the column permutation matrix */
+  const PermutationType& colsPermutation() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return m_cols_permutation;
+  }
 
-    /** \returns a const reference to the vector of indices representing the rows transpositions */
-    const IntDiagSizeVectorType& rowsTranspositions() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return m_rows_transpositions;
-    }
+  /** \returns a const reference to the vector of indices representing the rows transpositions */
+  const IntDiagSizeVectorType& rowsTranspositions() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return m_rows_transpositions;
+  }
 
-    /** \returns the absolute value of the determinant of the matrix of which
-      * *this is the QR decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the QR decomposition has already been computed.
-      *
-      * \note This is only for square matrices.
-      *
-      * \warning a determinant can be very big or small, so for matrices
-      * of large enough dimension, there is a risk of overflow/underflow.
-      * One way to work around that is to use logAbsDeterminant() instead.
-      *
-      * \sa logAbsDeterminant(), MatrixBase::determinant()
-      */
-    typename MatrixType::RealScalar absDeterminant() const;
+  /** \returns the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa absDeterminant(), logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::Scalar determinant() const;
 
-    /** \returns the natural log of the absolute value of the determinant of the matrix of which
-      * *this is the QR decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the QR decomposition has already been computed.
-      *
-      * \note This is only for square matrices.
-      *
-      * \note This method is useful to work around the risk of overflow/underflow that's inherent
-      * to determinant computation.
-      *
-      * \sa absDeterminant(), MatrixBase::determinant()
-      */
-    typename MatrixType::RealScalar logAbsDeterminant() const;
+  /** \returns the absolute value of the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa determinant(), logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar absDeterminant() const;
 
-    /** \returns the rank of the matrix of which *this is the QR decomposition.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline Index rank() const
-    {
-      using std::abs;
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      RealScalar premultiplied_threshold = abs(m_maxpivot) * threshold();
-      Index result = 0;
-      for(Index i = 0; i < m_nonzero_pivots; ++i)
-        result += (abs(m_qr.coeff(i,i)) > premultiplied_threshold);
-      return result;
-    }
+  /** \returns the natural log of the absolute value of the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \note This method is useful to work around the risk of overflow/underflow that's inherent
+   * to determinant computation.
+   *
+   * \sa determinant(), absDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar logAbsDeterminant() const;
 
-    /** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline Index dimensionOfKernel() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return cols() - rank();
-    }
+  /** \returns the rank of the matrix of which *this is the QR decomposition.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline Index rank() const {
+    using std::abs;
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    RealScalar premultiplied_threshold = abs(m_maxpivot) * threshold();
+    Index result = 0;
+    for (Index i = 0; i < m_nonzero_pivots; ++i) result += (abs(m_qr.coeff(i, i)) > premultiplied_threshold);
+    return result;
+  }
 
-    /** \returns true if the matrix of which *this is the QR decomposition represents an injective
-      *          linear map, i.e. has trivial kernel; false otherwise.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isInjective() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return rank() == cols();
-    }
+  /** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline Index dimensionOfKernel() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return cols() - rank();
+  }
 
-    /** \returns true if the matrix of which *this is the QR decomposition represents a surjective
-      *          linear map; false otherwise.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isSurjective() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return rank() == rows();
-    }
+  /** \returns true if the matrix of which *this is the QR decomposition represents an injective
+   *          linear map, i.e. has trivial kernel; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isInjective() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return rank() == cols();
+  }
 
-    /** \returns true if the matrix of which *this is the QR decomposition is invertible.
-      *
-      * \note This method has to determine which pivots should be considered nonzero.
-      *       For that, it uses the threshold value that you can control by calling
-      *       setThreshold(const RealScalar&).
-      */
-    inline bool isInvertible() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return isInjective() && isSurjective();
-    }
+  /** \returns true if the matrix of which *this is the QR decomposition represents a surjective
+   *          linear map; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isSurjective() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return rank() == rows();
+  }
 
-    /** \returns the inverse of the matrix of which *this is the QR decomposition.
-      *
-      * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
-      *       Use isInvertible() to first determine whether this matrix is invertible.
-      */
-    inline const Inverse<FullPivHouseholderQR> inverse() const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return Inverse<FullPivHouseholderQR>(*this);
-    }
+  /** \returns true if the matrix of which *this is the QR decomposition is invertible.
+   *
+   * \note This method has to determine which pivots should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline bool isInvertible() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return isInjective() && isSurjective();
+  }
 
-    inline Index rows() const { return m_qr.rows(); }
-    inline Index cols() const { return m_qr.cols(); }
-    
-    /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
-      * 
-      * For advanced uses only.
-      */
-    const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
+  /** \returns the inverse of the matrix of which *this is the QR decomposition.
+   *
+   * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
+   *       Use isInvertible() to first determine whether this matrix is invertible.
+   */
+  inline const Inverse<FullPivHouseholderQR> inverse() const {
+    eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
+    return Inverse<FullPivHouseholderQR>(*this);
+  }
 
-    /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
-      * who need to determine when pivots are to be considered nonzero. This is not used for the
-      * QR decomposition itself.
-      *
-      * When it needs to get the threshold value, Eigen calls threshold(). By default, this
-      * uses a formula to automatically determine a reasonable threshold.
-      * Once you have called the present method setThreshold(const RealScalar&),
-      * your value is used instead.
-      *
-      * \param threshold The new value to use as the threshold.
-      *
-      * A pivot will be considered nonzero if its absolute value is strictly greater than
-      *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
-      * where maxpivot is the biggest pivot.
-      *
-      * If you want to come back to the default behavior, call setThreshold(Default_t)
-      */
-    FullPivHouseholderQR& setThreshold(const RealScalar& threshold)
-    {
-      m_usePrescribedThreshold = true;
-      m_prescribedThreshold = threshold;
-      return *this;
-    }
+  inline Index rows() const { return m_qr.rows(); }
+  inline Index cols() const { return m_qr.cols(); }
 
-    /** Allows to come back to the default behavior, letting Eigen use its default formula for
-      * determining the threshold.
-      *
-      * You should pass the special object Eigen::Default as parameter here.
-      * \code qr.setThreshold(Eigen::Default); \endcode
-      *
-      * See the documentation of setThreshold(const RealScalar&).
-      */
-    FullPivHouseholderQR& setThreshold(Default_t)
-    {
-      m_usePrescribedThreshold = false;
-      return *this;
-    }
+  /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
+   *
+   * For advanced uses only.
+   */
+  const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
 
-    /** Returns the threshold that will be used by certain methods such as rank().
-      *
-      * See the documentation of setThreshold(const RealScalar&).
-      */
-    RealScalar threshold() const
-    {
-      eigen_assert(m_isInitialized || m_usePrescribedThreshold);
-      return m_usePrescribedThreshold ? m_prescribedThreshold
-      // this formula comes from experimenting (see "LU precision tuning" thread on the list)
-      // and turns out to be identical to Higham's formula used already in LDLt.
-                                      : NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
-    }
+  /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
+   * who need to determine when pivots are to be considered nonzero. This is not used for the
+   * QR decomposition itself.
+   *
+   * When it needs to get the threshold value, Eigen calls threshold(). By default, this
+   * uses a formula to automatically determine a reasonable threshold.
+   * Once you have called the present method setThreshold(const RealScalar&),
+   * your value is used instead.
+   *
+   * \param threshold The new value to use as the threshold.
+   *
+   * A pivot will be considered nonzero if its absolute value is strictly greater than
+   *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
+   * where maxpivot is the biggest pivot.
+   *
+   * If you want to come back to the default behavior, call setThreshold(Default_t)
+   */
+  FullPivHouseholderQR& setThreshold(const RealScalar& threshold) {
+    m_usePrescribedThreshold = true;
+    m_prescribedThreshold = threshold;
+    return *this;
+  }
 
-    /** \returns the number of nonzero pivots in the QR decomposition.
-      * Here nonzero is meant in the exact sense, not in a fuzzy sense.
-      * So that notion isn't really intrinsically interesting, but it is
-      * still useful when implementing algorithms.
-      *
-      * \sa rank()
-      */
-    inline Index nonzeroPivots() const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return m_nonzero_pivots;
-    }
+  /** Allows to come back to the default behavior, letting Eigen use its default formula for
+   * determining the threshold.
+   *
+   * You should pass the special object Eigen::Default as parameter here.
+   * \code qr.setThreshold(Eigen::Default); \endcode
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  FullPivHouseholderQR& setThreshold(Default_t) {
+    m_usePrescribedThreshold = false;
+    return *this;
+  }
 
-    /** \returns the absolute value of the biggest pivot, i.e. the biggest
-      *          diagonal coefficient of U.
-      */
-    RealScalar maxPivot() const { return m_maxpivot; }
+  /** Returns the threshold that will be used by certain methods such as rank().
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  RealScalar threshold() const {
+    eigen_assert(m_isInitialized || m_usePrescribedThreshold);
+    return m_usePrescribedThreshold ? m_prescribedThreshold
+                                    // this formula comes from experimenting (see "LU precision tuning" thread on the
+                                    // list) and turns out to be identical to Higham's formula used already in LDLt.
+                                    : NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
+  }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename RhsType, typename DstType>
-    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+  /** \returns the number of nonzero pivots in the QR decomposition.
+   * Here nonzero is meant in the exact sense, not in a fuzzy sense.
+   * So that notion isn't really intrinsically interesting, but it is
+   * still useful when implementing algorithms.
+   *
+   * \sa rank()
+   */
+  inline Index nonzeroPivots() const {
+    eigen_assert(m_isInitialized && "LU is not initialized.");
+    return m_nonzero_pivots;
+  }
 
-    template<bool Conjugate, typename RhsType, typename DstType>
-    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
-    #endif
+  /** \returns the absolute value of the biggest pivot, i.e. the biggest
+   *          diagonal coefficient of U.
+   */
+  RealScalar maxPivot() const { return m_maxpivot; }
 
-  protected:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
+#endif
 
-    void computeInPlace();
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-    MatrixType m_qr;
-    HCoeffsType m_hCoeffs;
-    IntDiagSizeVectorType m_rows_transpositions;
-    IntDiagSizeVectorType m_cols_transpositions;
-    PermutationType m_cols_permutation;
-    RowVectorType m_temp;
-    bool m_isInitialized, m_usePrescribedThreshold;
-    RealScalar m_prescribedThreshold, m_maxpivot;
-    Index m_nonzero_pivots;
-    RealScalar m_precision;
-    Index m_det_pq;
+  void computeInPlace();
+
+  MatrixType m_qr;
+  HCoeffsType m_hCoeffs;
+  IntDiagSizeVectorType m_rows_transpositions;
+  IntDiagSizeVectorType m_cols_transpositions;
+  PermutationType m_cols_permutation;
+  RowVectorType m_temp;
+  bool m_isInitialized, m_usePrescribedThreshold;
+  RealScalar m_prescribedThreshold, m_maxpivot;
+  Index m_nonzero_pivots;
+  RealScalar m_precision;
+  Index m_det_p;
 };
 
-template<typename MatrixType>
-typename MatrixType::RealScalar FullPivHouseholderQR<MatrixType>::absDeterminant() const
-{
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::Scalar FullPivHouseholderQR<MatrixType, PermutationIndex>::determinant() const {
+  eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
+  eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
+  Scalar detQ;
+  internal::householder_determinant<HCoeffsType, Scalar, NumTraits<Scalar>::IsComplex>::run(m_hCoeffs, detQ);
+  return m_qr.diagonal().prod() * detQ * Scalar(m_det_p);
+}
+
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::RealScalar FullPivHouseholderQR<MatrixType, PermutationIndex>::absDeterminant() const {
   using std::abs;
   eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
   return abs(m_qr.diagonal().prod());
 }
 
-template<typename MatrixType>
-typename MatrixType::RealScalar FullPivHouseholderQR<MatrixType>::logAbsDeterminant() const
-{
+template <typename MatrixType, typename PermutationIndex>
+typename MatrixType::RealScalar FullPivHouseholderQR<MatrixType, PermutationIndex>::logAbsDeterminant() const {
   eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
   return m_qr.diagonal().cwiseAbs().array().log().sum();
 }
 
 /** Performs the QR factorization of the given matrix \a matrix. The result of
-  * the factorization is stored into \c *this, and a reference to \c *this
-  * is returned.
-  *
-  * \sa class FullPivHouseholderQR, FullPivHouseholderQR(const MatrixType&)
-  */
-template<typename MatrixType>
-template<typename InputType>
-FullPivHouseholderQR<MatrixType>& FullPivHouseholderQR<MatrixType>::compute(const EigenBase<InputType>& matrix)
-{
+ * the factorization is stored into \c *this, and a reference to \c *this
+ * is returned.
+ *
+ * \sa class FullPivHouseholderQR, FullPivHouseholderQR(const MatrixType&)
+ */
+template <typename MatrixType, typename PermutationIndex>
+template <typename InputType>
+FullPivHouseholderQR<MatrixType, PermutationIndex>& FullPivHouseholderQR<MatrixType, PermutationIndex>::compute(
+    const EigenBase<InputType>& matrix) {
   m_qr = matrix.derived();
   computeInPlace();
   return *this;
 }
 
-template<typename MatrixType>
-void FullPivHouseholderQR<MatrixType>::computeInPlace()
-{
-  check_template_parameters();
-
+template <typename MatrixType, typename PermutationIndex>
+void FullPivHouseholderQR<MatrixType, PermutationIndex>::computeInPlace() {
+  eigen_assert(m_qr.cols() <= NumTraits<PermutationIndex>::highest());
   using std::abs;
   Index rows = m_qr.rows();
   Index cols = m_qr.cols();
-  Index size = (std::min)(rows,cols);
+  Index size = (std::min)(rows, cols);
 
-  
   m_hCoeffs.resize(size);
 
   m_temp.resize(cols);
@@ -478,132 +478,122 @@
 
   RealScalar biggest(0);
 
-  m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
+  m_nonzero_pivots = size;  // the generic case is that in which all pivots are nonzero (invertible case)
   m_maxpivot = RealScalar(0);
 
-  for (Index k = 0; k < size; ++k)
-  {
+  for (Index k = 0; k < size; ++k) {
     Index row_of_biggest_in_corner, col_of_biggest_in_corner;
     typedef internal::scalar_score_coeff_op<Scalar> Scoring;
     typedef typename Scoring::result_type Score;
 
-    Score score = m_qr.bottomRightCorner(rows-k, cols-k)
+    Score score = m_qr.bottomRightCorner(rows - k, cols - k)
                       .unaryExpr(Scoring())
                       .maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner);
     row_of_biggest_in_corner += k;
     col_of_biggest_in_corner += k;
-    RealScalar biggest_in_corner = internal::abs_knowing_score<Scalar>()(m_qr(row_of_biggest_in_corner, col_of_biggest_in_corner), score);
-    if(k==0) biggest = biggest_in_corner;
+    RealScalar biggest_in_corner =
+        internal::abs_knowing_score<Scalar>()(m_qr(row_of_biggest_in_corner, col_of_biggest_in_corner), score);
+    if (k == 0) biggest = biggest_in_corner;
 
     // if the corner is negligible, then we have less than full rank, and we can finish early
-    if(internal::isMuchSmallerThan(biggest_in_corner, biggest, m_precision))
-    {
+    if (internal::isMuchSmallerThan(biggest_in_corner, biggest, m_precision)) {
       m_nonzero_pivots = k;
-      for(Index i = k; i < size; i++)
-      {
-        m_rows_transpositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
-        m_cols_transpositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
+      for (Index i = k; i < size; i++) {
+        m_rows_transpositions.coeffRef(i) = internal::convert_index<PermutationIndex>(i);
+        m_cols_transpositions.coeffRef(i) = internal::convert_index<PermutationIndex>(i);
         m_hCoeffs.coeffRef(i) = Scalar(0);
       }
       break;
     }
 
-    m_rows_transpositions.coeffRef(k) = internal::convert_index<StorageIndex>(row_of_biggest_in_corner);
-    m_cols_transpositions.coeffRef(k) = internal::convert_index<StorageIndex>(col_of_biggest_in_corner);
-    if(k != row_of_biggest_in_corner) {
-      m_qr.row(k).tail(cols-k).swap(m_qr.row(row_of_biggest_in_corner).tail(cols-k));
+    m_rows_transpositions.coeffRef(k) = internal::convert_index<PermutationIndex>(row_of_biggest_in_corner);
+    m_cols_transpositions.coeffRef(k) = internal::convert_index<PermutationIndex>(col_of_biggest_in_corner);
+    if (k != row_of_biggest_in_corner) {
+      m_qr.row(k).tail(cols - k).swap(m_qr.row(row_of_biggest_in_corner).tail(cols - k));
       ++number_of_transpositions;
     }
-    if(k != col_of_biggest_in_corner) {
+    if (k != col_of_biggest_in_corner) {
       m_qr.col(k).swap(m_qr.col(col_of_biggest_in_corner));
       ++number_of_transpositions;
     }
 
     RealScalar beta;
-    m_qr.col(k).tail(rows-k).makeHouseholderInPlace(m_hCoeffs.coeffRef(k), beta);
-    m_qr.coeffRef(k,k) = beta;
+    m_qr.col(k).tail(rows - k).makeHouseholderInPlace(m_hCoeffs.coeffRef(k), beta);
+    m_qr.coeffRef(k, k) = beta;
 
     // remember the maximum absolute value of diagonal coefficients
-    if(abs(beta) > m_maxpivot) m_maxpivot = abs(beta);
+    if (abs(beta) > m_maxpivot) m_maxpivot = abs(beta);
 
-    m_qr.bottomRightCorner(rows-k, cols-k-1)
-        .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows-k-1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k+1));
+    m_qr.bottomRightCorner(rows - k, cols - k - 1)
+        .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows - k - 1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k + 1));
   }
 
   m_cols_permutation.setIdentity(cols);
-  for(Index k = 0; k < size; ++k)
-    m_cols_permutation.applyTranspositionOnTheRight(k, m_cols_transpositions.coeff(k));
+  for (Index k = 0; k < size; ++k) m_cols_permutation.applyTranspositionOnTheRight(k, m_cols_transpositions.coeff(k));
 
-  m_det_pq = (number_of_transpositions%2) ? -1 : 1;
+  m_det_p = (number_of_transpositions % 2) ? -1 : 1;
   m_isInitialized = true;
 }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename _MatrixType>
-template<typename RhsType, typename DstType>
-void FullPivHouseholderQR<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, typename PermutationIndex_>
+template <typename RhsType, typename DstType>
+void FullPivHouseholderQR<MatrixType_, PermutationIndex_>::_solve_impl(const RhsType& rhs, DstType& dst) const {
   const Index l_rank = rank();
 
   // FIXME introduce nonzeroPivots() and use it here. and more generally,
   // make the same improvements in this dec as in FullPivLU.
-  if(l_rank==0)
-  {
+  if (l_rank == 0) {
     dst.setZero();
     return;
   }
 
   typename RhsType::PlainObject c(rhs);
 
-  Matrix<typename RhsType::Scalar,1,RhsType::ColsAtCompileTime> temp(rhs.cols());
-  for (Index k = 0; k < l_rank; ++k)
-  {
-    Index remainingSize = rows()-k;
+  Matrix<typename RhsType::Scalar, 1, RhsType::ColsAtCompileTime> temp(rhs.cols());
+  for (Index k = 0; k < l_rank; ++k) {
+    Index remainingSize = rows() - k;
     c.row(k).swap(c.row(m_rows_transpositions.coeff(k)));
     c.bottomRightCorner(remainingSize, rhs.cols())
-      .applyHouseholderOnTheLeft(m_qr.col(k).tail(remainingSize-1),
-                               m_hCoeffs.coeff(k), &temp.coeffRef(0));
+        .applyHouseholderOnTheLeft(m_qr.col(k).tail(remainingSize - 1), m_hCoeffs.coeff(k), &temp.coeffRef(0));
   }
 
-  m_qr.topLeftCorner(l_rank, l_rank)
-      .template triangularView<Upper>()
-      .solveInPlace(c.topRows(l_rank));
+  m_qr.topLeftCorner(l_rank, l_rank).template triangularView<Upper>().solveInPlace(c.topRows(l_rank));
 
-  for(Index i = 0; i < l_rank; ++i) dst.row(m_cols_permutation.indices().coeff(i)) = c.row(i);
-  for(Index i = l_rank; i < cols(); ++i) dst.row(m_cols_permutation.indices().coeff(i)).setZero();
+  for (Index i = 0; i < l_rank; ++i) dst.row(m_cols_permutation.indices().coeff(i)) = c.row(i);
+  for (Index i = l_rank; i < cols(); ++i) dst.row(m_cols_permutation.indices().coeff(i)).setZero();
 }
 
-template<typename _MatrixType>
-template<bool Conjugate, typename RhsType, typename DstType>
-void FullPivHouseholderQR<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_, typename PermutationIndex_>
+template <bool Conjugate, typename RhsType, typename DstType>
+void FullPivHouseholderQR<MatrixType_, PermutationIndex_>::_solve_impl_transposed(const RhsType& rhs,
+                                                                                  DstType& dst) const {
   const Index l_rank = rank();
 
-  if(l_rank == 0)
-  {
+  if (l_rank == 0) {
     dst.setZero();
     return;
   }
 
-  typename RhsType::PlainObject c(m_cols_permutation.transpose()*rhs);
+  typename RhsType::PlainObject c(m_cols_permutation.transpose() * rhs);
 
   m_qr.topLeftCorner(l_rank, l_rank)
-         .template triangularView<Upper>()
-         .transpose().template conjugateIf<Conjugate>()
-         .solveInPlace(c.topRows(l_rank));
+      .template triangularView<Upper>()
+      .transpose()
+      .template conjugateIf<Conjugate>()
+      .solveInPlace(c.topRows(l_rank));
 
   dst.topRows(l_rank) = c.topRows(l_rank);
-  dst.bottomRows(rows()-l_rank).setZero();
+  dst.bottomRows(rows() - l_rank).setZero();
 
   Matrix<Scalar, 1, DstType::ColsAtCompileTime> temp(dst.cols());
   const Index size = (std::min)(rows(), cols());
-  for (Index k = size-1; k >= 0; --k)
-  {
-    Index remainingSize = rows()-k;
+  for (Index k = size - 1; k >= 0; --k) {
+    Index remainingSize = rows() - k;
 
     dst.bottomRightCorner(remainingSize, dst.cols())
-       .applyHouseholderOnTheLeft(m_qr.col(k).tail(remainingSize-1).template conjugateIf<!Conjugate>(),
-                                  m_hCoeffs.template conjugateIf<Conjugate>().coeff(k), &temp.coeffRef(0));
+        .applyHouseholderOnTheLeft(m_qr.col(k).tail(remainingSize - 1).template conjugateIf<!Conjugate>(),
+                                   m_hCoeffs.template conjugateIf<Conjugate>().coeff(k), &temp.coeffRef(0));
 
     dst.row(k).swap(dst.row(m_rows_transpositions.coeff(k)));
   }
@@ -611,52 +601,49 @@
 #endif
 
 namespace internal {
-  
-template<typename DstXprType, typename MatrixType>
-struct Assignment<DstXprType, Inverse<FullPivHouseholderQR<MatrixType> >, internal::assign_op<typename DstXprType::Scalar,typename FullPivHouseholderQR<MatrixType>::Scalar>, Dense2Dense>
-{
-  typedef FullPivHouseholderQR<MatrixType> QrType;
+
+template <typename DstXprType, typename MatrixType, typename PermutationIndex>
+struct Assignment<DstXprType, Inverse<FullPivHouseholderQR<MatrixType, PermutationIndex> >,
+                  internal::assign_op<typename DstXprType::Scalar,
+                                      typename FullPivHouseholderQR<MatrixType, PermutationIndex>::Scalar>,
+                  Dense2Dense> {
+  typedef FullPivHouseholderQR<MatrixType, PermutationIndex> QrType;
   typedef Inverse<QrType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename QrType::Scalar> &)
-  {    
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename QrType::Scalar>&) {
     dst = src.nestedExpression().solve(MatrixType::Identity(src.rows(), src.cols()));
   }
 };
 
 /** \ingroup QR_Module
-  *
-  * \brief Expression type for return value of FullPivHouseholderQR::matrixQ()
-  *
-  * \tparam MatrixType type of underlying dense matrix
-  */
-template<typename MatrixType> struct FullPivHouseholderQRMatrixQReturnType
-  : public ReturnByValue<FullPivHouseholderQRMatrixQReturnType<MatrixType> >
-{
-public:
-  typedef typename FullPivHouseholderQR<MatrixType>::IntDiagSizeVectorType IntDiagSizeVectorType;
+ *
+ * \brief Expression type for return value of FullPivHouseholderQR::matrixQ()
+ *
+ * \tparam MatrixType type of underlying dense matrix
+ */
+template <typename MatrixType, typename PermutationIndex>
+struct FullPivHouseholderQRMatrixQReturnType
+    : public ReturnByValue<FullPivHouseholderQRMatrixQReturnType<MatrixType, PermutationIndex> > {
+ public:
+  typedef typename FullPivHouseholderQR<MatrixType, PermutationIndex>::IntDiagSizeVectorType IntDiagSizeVectorType;
   typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
   typedef Matrix<typename MatrixType::Scalar, 1, MatrixType::RowsAtCompileTime, RowMajor, 1,
-                 MatrixType::MaxRowsAtCompileTime> WorkVectorType;
+                 MatrixType::MaxRowsAtCompileTime>
+      WorkVectorType;
 
-  FullPivHouseholderQRMatrixQReturnType(const MatrixType&       qr,
-                                        const HCoeffsType&      hCoeffs,
+  FullPivHouseholderQRMatrixQReturnType(const MatrixType& qr, const HCoeffsType& hCoeffs,
                                         const IntDiagSizeVectorType& rowsTranspositions)
-    : m_qr(qr),
-      m_hCoeffs(hCoeffs),
-      m_rowsTranspositions(rowsTranspositions)
-  {}
+      : m_qr(qr), m_hCoeffs(hCoeffs), m_rowsTranspositions(rowsTranspositions) {}
 
   template <typename ResultType>
-  void evalTo(ResultType& result) const
-  {
+  void evalTo(ResultType& result) const {
     const Index rows = m_qr.rows();
     WorkVectorType workspace(rows);
     evalTo(result, workspace);
   }
 
   template <typename ResultType>
-  void evalTo(ResultType& result, WorkVectorType& workspace) const
-  {
+  void evalTo(ResultType& result, WorkVectorType& workspace) const {
     using numext::conj;
     // compute the product H'_0 H'_1 ... H'_n-1,
     // where H_k is the k-th Householder transformation I - h_k v_k v_k'
@@ -666,10 +653,9 @@
     const Index size = (std::min)(rows, cols);
     workspace.resize(rows);
     result.setIdentity(rows, rows);
-    for (Index k = size-1; k >= 0; k--)
-    {
-      result.block(k, k, rows-k, rows-k)
-            .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows-k-1), conj(m_hCoeffs.coeff(k)), &workspace.coeffRef(k));
+    for (Index k = size - 1; k >= 0; k--) {
+      result.block(k, k, rows - k, rows - k)
+          .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows - k - 1), conj(m_hCoeffs.coeff(k)), &workspace.coeffRef(k));
       result.row(k).swap(result.row(m_rowsTranspositions.coeff(k)));
     }
   }
@@ -677,7 +663,7 @@
   Index rows() const { return m_qr.rows(); }
   Index cols() const { return m_qr.rows(); }
 
-protected:
+ protected:
   typename MatrixType::Nested m_qr;
   typename HCoeffsType::Nested m_hCoeffs;
   typename IntDiagSizeVectorType::Nested m_rowsTranspositions;
@@ -688,26 +674,26 @@
 //  : public evaluator<ReturnByValue<FullPivHouseholderQRMatrixQReturnType<MatrixType> > >
 // {};
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename MatrixType>
-inline typename FullPivHouseholderQR<MatrixType>::MatrixQReturnType FullPivHouseholderQR<MatrixType>::matrixQ() const
-{
+template <typename MatrixType, typename PermutationIndex>
+inline typename FullPivHouseholderQR<MatrixType, PermutationIndex>::MatrixQReturnType
+FullPivHouseholderQR<MatrixType, PermutationIndex>::matrixQ() const {
   eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
   return MatrixQReturnType(m_qr, m_hCoeffs, m_rows_transpositions);
 }
 
 /** \return the full-pivoting Householder QR decomposition of \c *this.
-  *
-  * \sa class FullPivHouseholderQR
-  */
-template<typename Derived>
-const FullPivHouseholderQR<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::fullPivHouseholderQr() const
-{
-  return FullPivHouseholderQR<PlainObject>(eval());
+ *
+ * \sa class FullPivHouseholderQR
+ */
+template <typename Derived>
+template <typename PermutationIndex>
+const FullPivHouseholderQR<typename MatrixBase<Derived>::PlainObject, PermutationIndex>
+MatrixBase<Derived>::fullPivHouseholderQr() const {
+  return FullPivHouseholderQR<PlainObject, PermutationIndex>(eval());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_FULLPIVOTINGHOUSEHOLDERQR_H
+#endif  // EIGEN_FULLPIVOTINGHOUSEHOLDERQR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h
index 801739f..9e73672 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h
@@ -12,249 +12,295 @@
 #ifndef EIGEN_QR_H
 #define EIGEN_QR_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-template<typename _MatrixType> struct traits<HouseholderQR<_MatrixType> >
- : traits<_MatrixType>
-{
+template <typename MatrixType_>
+struct traits<HouseholderQR<MatrixType_>> : traits<MatrixType_> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
   typedef int StorageIndex;
   enum { Flags = 0 };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup QR_Module
-  *
-  *
-  * \class HouseholderQR
-  *
-  * \brief Householder QR decomposition of a matrix
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the QR decomposition
-  *
-  * This class performs a QR decomposition of a matrix \b A into matrices \b Q and \b R
-  * such that 
-  * \f[
-  *  \mathbf{A} = \mathbf{Q} \, \mathbf{R}
-  * \f]
-  * by using Householder transformations. Here, \b Q a unitary matrix and \b R an upper triangular matrix.
-  * The result is stored in a compact way compatible with LAPACK.
-  *
-  * Note that no pivoting is performed. This is \b not a rank-revealing decomposition.
-  * If you want that feature, use FullPivHouseholderQR or ColPivHouseholderQR instead.
-  *
-  * This Householder QR decomposition is faster, but less numerically stable and less feature-full than
-  * FullPivHouseholderQR or ColPivHouseholderQR.
-  *
-  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  *
-  * \sa MatrixBase::householderQr()
-  */
-template<typename _MatrixType> class HouseholderQR
-        : public SolverBase<HouseholderQR<_MatrixType> >
-{
-  public:
+ *
+ *
+ * \class HouseholderQR
+ *
+ * \brief Householder QR decomposition of a matrix
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the QR decomposition
+ *
+ * This class performs a QR decomposition of a matrix \b A into matrices \b Q and \b R
+ * such that
+ * \f[
+ *  \mathbf{A} = \mathbf{Q} \, \mathbf{R}
+ * \f]
+ * by using Householder transformations. Here, \b Q a unitary matrix and \b R an upper triangular matrix.
+ * The result is stored in a compact way compatible with LAPACK.
+ *
+ * Note that no pivoting is performed. This is \b not a rank-revealing decomposition.
+ * If you want that feature, use FullPivHouseholderQR or ColPivHouseholderQR instead.
+ *
+ * This Householder QR decomposition is faster, but less numerically stable and less feature-full than
+ * FullPivHouseholderQR or ColPivHouseholderQR.
+ *
+ * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+ *
+ * \sa MatrixBase::householderQr()
+ */
+template <typename MatrixType_>
+class HouseholderQR : public SolverBase<HouseholderQR<MatrixType_>> {
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef SolverBase<HouseholderQR> Base;
+  friend class SolverBase<HouseholderQR>;
 
-    typedef _MatrixType MatrixType;
-    typedef SolverBase<HouseholderQR> Base;
-    friend class SolverBase<HouseholderQR>;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(HouseholderQR)
+  enum {
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime, (MatrixType::Flags & RowMajorBit) ? RowMajor : ColMajor,
+                 MaxRowsAtCompileTime, MaxRowsAtCompileTime>
+      MatrixQType;
+  typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
+  typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
+  typedef HouseholderSequence<MatrixType, internal::remove_all_t<typename HCoeffsType::ConjugateReturnType>>
+      HouseholderSequenceType;
 
-    EIGEN_GENERIC_PUBLIC_INTERFACE(HouseholderQR)
-    enum {
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime, (MatrixType::Flags&RowMajorBit) ? RowMajor : ColMajor, MaxRowsAtCompileTime, MaxRowsAtCompileTime> MatrixQType;
-    typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
-    typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
-    typedef HouseholderSequence<MatrixType,typename internal::remove_all<typename HCoeffsType::ConjugateReturnType>::type> HouseholderSequenceType;
+  /**
+   * \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via HouseholderQR::compute(const MatrixType&).
+   */
+  HouseholderQR() : m_qr(), m_hCoeffs(), m_temp(), m_isInitialized(false) {}
 
-    /**
-      * \brief Default Constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via HouseholderQR::compute(const MatrixType&).
-      */
-    HouseholderQR() : m_qr(), m_hCoeffs(), m_temp(), m_isInitialized(false) {}
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa HouseholderQR()
+   */
+  HouseholderQR(Index rows, Index cols)
+      : m_qr(rows, cols), m_hCoeffs((std::min)(rows, cols)), m_temp(cols), m_isInitialized(false) {}
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem \a size.
-      * \sa HouseholderQR()
-      */
-    HouseholderQR(Index rows, Index cols)
-      : m_qr(rows, cols),
-        m_hCoeffs((std::min)(rows,cols)),
-        m_temp(cols),
-        m_isInitialized(false) {}
-
-    /** \brief Constructs a QR factorization from a given matrix
-      *
-      * This constructor computes the QR factorization of the matrix \a matrix by calling
-      * the method compute(). It is a short cut for:
-      * 
-      * \code
-      * HouseholderQR<MatrixType> qr(matrix.rows(), matrix.cols());
-      * qr.compute(matrix);
-      * \endcode
-      * 
-      * \sa compute()
-      */
-    template<typename InputType>
-    explicit HouseholderQR(const EigenBase<InputType>& matrix)
+  /** \brief Constructs a QR factorization from a given matrix
+   *
+   * This constructor computes the QR factorization of the matrix \a matrix by calling
+   * the method compute(). It is a short cut for:
+   *
+   * \code
+   * HouseholderQR<MatrixType> qr(matrix.rows(), matrix.cols());
+   * qr.compute(matrix);
+   * \endcode
+   *
+   * \sa compute()
+   */
+  template <typename InputType>
+  explicit HouseholderQR(const EigenBase<InputType>& matrix)
       : m_qr(matrix.rows(), matrix.cols()),
-        m_hCoeffs((std::min)(matrix.rows(),matrix.cols())),
+        m_hCoeffs((std::min)(matrix.rows(), matrix.cols())),
         m_temp(matrix.cols()),
-        m_isInitialized(false)
-    {
-      compute(matrix.derived());
-    }
+        m_isInitialized(false) {
+    compute(matrix.derived());
+  }
 
-
-    /** \brief Constructs a QR factorization from a given matrix
-      *
-      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
-      * \c MatrixType is a Eigen::Ref.
-      *
-      * \sa HouseholderQR(const EigenBase&)
-      */
-    template<typename InputType>
-    explicit HouseholderQR(EigenBase<InputType>& matrix)
+  /** \brief Constructs a QR factorization from a given matrix
+   *
+   * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
+   * \c MatrixType is a Eigen::Ref.
+   *
+   * \sa HouseholderQR(const EigenBase&)
+   */
+  template <typename InputType>
+  explicit HouseholderQR(EigenBase<InputType>& matrix)
       : m_qr(matrix.derived()),
-        m_hCoeffs((std::min)(matrix.rows(),matrix.cols())),
+        m_hCoeffs((std::min)(matrix.rows(), matrix.cols())),
         m_temp(matrix.cols()),
-        m_isInitialized(false)
-    {
-      computeInPlace();
-    }
+        m_isInitialized(false) {
+    computeInPlace();
+  }
 
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
-      * *this is the QR decomposition, if any exists.
-      *
-      * \param b the right-hand-side of the equation to solve.
-      *
-      * \returns a solution.
-      *
-      * \note_about_checking_solutions
-      *
-      * \note_about_arbitrary_choice_of_solution
-      *
-      * Example: \include HouseholderQR_solve.cpp
-      * Output: \verbinclude HouseholderQR_solve.out
-      */
-    template<typename Rhs>
-    inline const Solve<HouseholderQR, Rhs>
-    solve(const MatrixBase<Rhs>& b) const;
-    #endif
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
+   * *this is the QR decomposition, if any exists.
+   *
+   * \param b the right-hand-side of the equation to solve.
+   *
+   * \returns a solution.
+   *
+   * \note_about_checking_solutions
+   *
+   * \note_about_arbitrary_choice_of_solution
+   *
+   * Example: \include HouseholderQR_solve.cpp
+   * Output: \verbinclude HouseholderQR_solve.out
+   */
+  template <typename Rhs>
+  inline const Solve<HouseholderQR, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
-    /** This method returns an expression of the unitary matrix Q as a sequence of Householder transformations.
-      *
-      * The returned expression can directly be used to perform matrix products. It can also be assigned to a dense Matrix object.
-      * Here is an example showing how to recover the full or thin matrix Q, as well as how to perform matrix products using operator*:
-      *
-      * Example: \include HouseholderQR_householderQ.cpp
-      * Output: \verbinclude HouseholderQR_householderQ.out
-      */
-    HouseholderSequenceType householderQ() const
-    {
-      eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
-      return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate());
-    }
+  /** This method returns an expression of the unitary matrix Q as a sequence of Householder transformations.
+   *
+   * The returned expression can directly be used to perform matrix products. It can also be assigned to a dense Matrix
+   * object. Here is an example showing how to recover the full or thin matrix Q, as well as how to perform matrix
+   * products using operator*:
+   *
+   * Example: \include HouseholderQR_householderQ.cpp
+   * Output: \verbinclude HouseholderQR_householderQ.out
+   */
+  HouseholderSequenceType householderQ() const {
+    eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
+    return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate());
+  }
 
-    /** \returns a reference to the matrix where the Householder QR decomposition is stored
-      * in a LAPACK-compatible way.
-      */
-    const MatrixType& matrixQR() const
-    {
-        eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
-        return m_qr;
-    }
+  /** \returns a reference to the matrix where the Householder QR decomposition is stored
+   * in a LAPACK-compatible way.
+   */
+  const MatrixType& matrixQR() const {
+    eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
+    return m_qr;
+  }
 
-    template<typename InputType>
-    HouseholderQR& compute(const EigenBase<InputType>& matrix) {
-      m_qr = matrix.derived();
-      computeInPlace();
-      return *this;
-    }
+  template <typename InputType>
+  HouseholderQR& compute(const EigenBase<InputType>& matrix) {
+    m_qr = matrix.derived();
+    computeInPlace();
+    return *this;
+  }
 
-    /** \returns the absolute value of the determinant of the matrix of which
-      * *this is the QR decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the QR decomposition has already been computed.
-      *
-      * \note This is only for square matrices.
-      *
-      * \warning a determinant can be very big or small, so for matrices
-      * of large enough dimension, there is a risk of overflow/underflow.
-      * One way to work around that is to use logAbsDeterminant() instead.
-      *
-      * \sa logAbsDeterminant(), MatrixBase::determinant()
-      */
-    typename MatrixType::RealScalar absDeterminant() const;
+  /** \returns the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa absDeterminant(), logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::Scalar determinant() const;
 
-    /** \returns the natural log of the absolute value of the determinant of the matrix of which
-      * *this is the QR decomposition. It has only linear complexity
-      * (that is, O(n) where n is the dimension of the square matrix)
-      * as the QR decomposition has already been computed.
-      *
-      * \note This is only for square matrices.
-      *
-      * \note This method is useful to work around the risk of overflow/underflow that's inherent
-      * to determinant computation.
-      *
-      * \sa absDeterminant(), MatrixBase::determinant()
-      */
-    typename MatrixType::RealScalar logAbsDeterminant() const;
+  /** \returns the absolute value of the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa determinant(), logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar absDeterminant() const;
 
-    inline Index rows() const { return m_qr.rows(); }
-    inline Index cols() const { return m_qr.cols(); }
+  /** \returns the natural log of the absolute value of the determinant of the matrix of which
+   * *this is the QR decomposition. It has only linear complexity
+   * (that is, O(n) where n is the dimension of the square matrix)
+   * as the QR decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \note This method is useful to work around the risk of overflow/underflow that's inherent
+   * to determinant computation.
+   *
+   * \sa determinant(), absDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar logAbsDeterminant() const;
 
-    /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
-      * 
-      * For advanced uses only.
-      */
-    const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
+  inline Index rows() const { return m_qr.rows(); }
+  inline Index cols() const { return m_qr.cols(); }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename RhsType, typename DstType>
-    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+  /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
+   *
+   * For advanced uses only.
+   */
+  const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
 
-    template<bool Conjugate, typename RhsType, typename DstType>
-    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
-    #endif
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-  protected:
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
+#endif
 
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-    }
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-    void computeInPlace();
+  void computeInPlace();
 
-    MatrixType m_qr;
-    HCoeffsType m_hCoeffs;
-    RowVectorType m_temp;
-    bool m_isInitialized;
+  MatrixType m_qr;
+  HCoeffsType m_hCoeffs;
+  RowVectorType m_temp;
+  bool m_isInitialized;
 };
 
-template<typename MatrixType>
-typename MatrixType::RealScalar HouseholderQR<MatrixType>::absDeterminant() const
-{
+namespace internal {
+
+/** \internal */
+template <typename HCoeffs, typename Scalar, bool IsComplex>
+struct householder_determinant {
+  static void run(const HCoeffs& hCoeffs, Scalar& out_det) {
+    out_det = Scalar(1);
+    Index size = hCoeffs.rows();
+    for (Index i = 0; i < size; i++) {
+      // For each valid reflection Q_n,
+      // det(Q_n) = - conj(h_n) / h_n
+      // where h_n is the Householder coefficient.
+      if (hCoeffs(i) != Scalar(0)) out_det *= -numext::conj(hCoeffs(i)) / hCoeffs(i);
+    }
+  }
+};
+
+/** \internal */
+template <typename HCoeffs, typename Scalar>
+struct householder_determinant<HCoeffs, Scalar, false> {
+  static void run(const HCoeffs& hCoeffs, Scalar& out_det) {
+    bool negated = false;
+    Index size = hCoeffs.rows();
+    for (Index i = 0; i < size; i++) {
+      // Each valid reflection negates the determinant.
+      if (hCoeffs(i) != Scalar(0)) negated ^= true;
+    }
+    out_det = negated ? Scalar(-1) : Scalar(1);
+  }
+};
+
+}  // end namespace internal
+
+template <typename MatrixType>
+typename MatrixType::Scalar HouseholderQR<MatrixType>::determinant() const {
+  eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
+  eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
+  Scalar detQ;
+  internal::householder_determinant<HCoeffsType, Scalar, NumTraits<Scalar>::IsComplex>::run(m_hCoeffs, detQ);
+  return m_qr.diagonal().prod() * detQ;
+}
+
+template <typename MatrixType>
+typename MatrixType::RealScalar HouseholderQR<MatrixType>::absDeterminant() const {
   using std::abs;
   eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
   return abs(m_qr.diagonal().prod());
 }
 
-template<typename MatrixType>
-typename MatrixType::RealScalar HouseholderQR<MatrixType>::logAbsDeterminant() const
-{
+template <typename MatrixType>
+typename MatrixType::RealScalar HouseholderQR<MatrixType>::logAbsDeterminant() const {
   eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
   return m_qr.diagonal().cwiseAbs().array().log().sum();
@@ -263,73 +309,101 @@
 namespace internal {
 
 /** \internal */
-template<typename MatrixQR, typename HCoeffs>
-void householder_qr_inplace_unblocked(MatrixQR& mat, HCoeffs& hCoeffs, typename MatrixQR::Scalar* tempData = 0)
-{
+template <typename MatrixQR, typename HCoeffs>
+void householder_qr_inplace_unblocked(MatrixQR& mat, HCoeffs& hCoeffs, typename MatrixQR::Scalar* tempData = 0) {
   typedef typename MatrixQR::Scalar Scalar;
   typedef typename MatrixQR::RealScalar RealScalar;
   Index rows = mat.rows();
   Index cols = mat.cols();
-  Index size = (std::min)(rows,cols);
+  Index size = (std::min)(rows, cols);
 
   eigen_assert(hCoeffs.size() == size);
 
-  typedef Matrix<Scalar,MatrixQR::ColsAtCompileTime,1> TempType;
+  typedef Matrix<Scalar, MatrixQR::ColsAtCompileTime, 1> TempType;
   TempType tempVector;
-  if(tempData==0)
-  {
+  if (tempData == 0) {
     tempVector.resize(cols);
     tempData = tempVector.data();
   }
 
-  for(Index k = 0; k < size; ++k)
-  {
+  for (Index k = 0; k < size; ++k) {
     Index remainingRows = rows - k;
     Index remainingCols = cols - k - 1;
 
     RealScalar beta;
     mat.col(k).tail(remainingRows).makeHouseholderInPlace(hCoeffs.coeffRef(k), beta);
-    mat.coeffRef(k,k) = beta;
+    mat.coeffRef(k, k) = beta;
 
     // apply H to remaining part of m_qr from the left
     mat.bottomRightCorner(remainingRows, remainingCols)
-        .applyHouseholderOnTheLeft(mat.col(k).tail(remainingRows-1), hCoeffs.coeffRef(k), tempData+k+1);
+        .applyHouseholderOnTheLeft(mat.col(k).tail(remainingRows - 1), hCoeffs.coeffRef(k), tempData + k + 1);
   }
 }
 
+// TODO: add a corresponding public API for updating a QR factorization
+/** \internal
+ * Basically a modified copy of @c Eigen::internal::householder_qr_inplace_unblocked that
+ * performs a rank-1 update of the QR matrix in compact storage. This function assumes, that
+ * the first @c k-1 columns of the matrix @c mat contain the QR decomposition of \f$A^N\f$ up to
+ * column k-1. Then the QR decomposition of the k-th column (given by @c newColumn) is computed by
+ * applying the k-1 Householder projectors on it and finally compute the projector \f$H_k\f$ of
+ * it. On exit the matrix @c mat and the vector @c hCoeffs contain the QR decomposition of the
+ * first k columns of \f$A^N\f$. The \a tempData argument must point to at least mat.cols() scalars.  */
+template <typename MatrixQR, typename HCoeffs, typename VectorQR>
+void householder_qr_inplace_update(MatrixQR& mat, HCoeffs& hCoeffs, const VectorQR& newColumn,
+                                   typename MatrixQR::Index k, typename MatrixQR::Scalar* tempData) {
+  typedef typename MatrixQR::Index Index;
+  typedef typename MatrixQR::RealScalar RealScalar;
+  Index rows = mat.rows();
+
+  eigen_assert(k < mat.cols());
+  eigen_assert(k < rows);
+  eigen_assert(hCoeffs.size() == mat.cols());
+  eigen_assert(newColumn.size() == rows);
+  eigen_assert(tempData);
+
+  // Store new column in mat at column k
+  mat.col(k) = newColumn;
+  // Apply H = H_1...H_{k-1} on newColumn (skip if k=0)
+  for (Index i = 0; i < k; ++i) {
+    Index remainingRows = rows - i;
+    mat.col(k)
+        .tail(remainingRows)
+        .applyHouseholderOnTheLeft(mat.col(i).tail(remainingRows - 1), hCoeffs.coeffRef(i), tempData + i + 1);
+  }
+  // Construct Householder projector in-place in column k
+  RealScalar beta;
+  mat.col(k).tail(rows - k).makeHouseholderInPlace(hCoeffs.coeffRef(k), beta);
+  mat.coeffRef(k, k) = beta;
+}
+
 /** \internal */
-template<typename MatrixQR, typename HCoeffs,
-  typename MatrixQRScalar = typename MatrixQR::Scalar,
-  bool InnerStrideIsOne = (MatrixQR::InnerStrideAtCompileTime == 1 && HCoeffs::InnerStrideAtCompileTime == 1)>
-struct householder_qr_inplace_blocked
-{
+template <typename MatrixQR, typename HCoeffs, typename MatrixQRScalar = typename MatrixQR::Scalar,
+          bool InnerStrideIsOne = (MatrixQR::InnerStrideAtCompileTime == 1 && HCoeffs::InnerStrideAtCompileTime == 1)>
+struct householder_qr_inplace_blocked {
   // This is specialized for LAPACK-supported Scalar types in HouseholderQR_LAPACKE.h
-  static void run(MatrixQR& mat, HCoeffs& hCoeffs, Index maxBlockSize=32,
-      typename MatrixQR::Scalar* tempData = 0)
-  {
+  static void run(MatrixQR& mat, HCoeffs& hCoeffs, Index maxBlockSize = 32, typename MatrixQR::Scalar* tempData = 0) {
     typedef typename MatrixQR::Scalar Scalar;
-    typedef Block<MatrixQR,Dynamic,Dynamic> BlockType;
+    typedef Block<MatrixQR, Dynamic, Dynamic> BlockType;
 
     Index rows = mat.rows();
     Index cols = mat.cols();
     Index size = (std::min)(rows, cols);
 
-    typedef Matrix<Scalar,Dynamic,1,ColMajor,MatrixQR::MaxColsAtCompileTime,1> TempType;
+    typedef Matrix<Scalar, Dynamic, 1, ColMajor, MatrixQR::MaxColsAtCompileTime, 1> TempType;
     TempType tempVector;
-    if(tempData==0)
-    {
+    if (tempData == 0) {
       tempVector.resize(cols);
       tempData = tempVector.data();
     }
 
-    Index blockSize = (std::min)(maxBlockSize,size);
+    Index blockSize = (std::min)(maxBlockSize, size);
 
     Index k = 0;
-    for (k = 0; k < size; k += blockSize)
-    {
-      Index bs = (std::min)(size-k,blockSize);  // actual size of the block
-      Index tcols = cols - k - bs;              // trailing columns
-      Index brows = rows-k;                     // rows of the block
+    for (k = 0; k < size; k += blockSize) {
+      Index bs = (std::min)(size - k, blockSize);  // actual size of the block
+      Index tcols = cols - k - bs;                 // trailing columns
+      Index brows = rows - k;                      // rows of the block
 
       // partition the matrix:
       //        A00 | A01 | A02
@@ -339,75 +413,68 @@
       // and update [A21^T A22^T]^T using level 3 operations.
       // Finally, the algorithm continue on A22
 
-      BlockType A11_21 = mat.block(k,k,brows,bs);
-      Block<HCoeffs,Dynamic,1> hCoeffsSegment = hCoeffs.segment(k,bs);
+      BlockType A11_21 = mat.block(k, k, brows, bs);
+      Block<HCoeffs, Dynamic, 1> hCoeffsSegment = hCoeffs.segment(k, bs);
 
       householder_qr_inplace_unblocked(A11_21, hCoeffsSegment, tempData);
 
-      if(tcols)
-      {
-        BlockType A21_22 = mat.block(k,k+bs,brows,tcols);
-        apply_block_householder_on_the_left(A21_22,A11_21,hCoeffsSegment, false); // false == backward
+      if (tcols) {
+        BlockType A21_22 = mat.block(k, k + bs, brows, tcols);
+        apply_block_householder_on_the_left(A21_22, A11_21, hCoeffsSegment, false);  // false == backward
       }
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename _MatrixType>
-template<typename RhsType, typename DstType>
-void HouseholderQR<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_>
+template <typename RhsType, typename DstType>
+void HouseholderQR<MatrixType_>::_solve_impl(const RhsType& rhs, DstType& dst) const {
   const Index rank = (std::min)(rows(), cols());
 
   typename RhsType::PlainObject c(rhs);
 
-  c.applyOnTheLeft(householderQ().setLength(rank).adjoint() );
+  c.applyOnTheLeft(householderQ().setLength(rank).adjoint());
 
-  m_qr.topLeftCorner(rank, rank)
-      .template triangularView<Upper>()
-      .solveInPlace(c.topRows(rank));
+  m_qr.topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(c.topRows(rank));
 
   dst.topRows(rank) = c.topRows(rank);
-  dst.bottomRows(cols()-rank).setZero();
+  dst.bottomRows(cols() - rank).setZero();
 }
 
-template<typename _MatrixType>
-template<bool Conjugate, typename RhsType, typename DstType>
-void HouseholderQR<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
+template <typename MatrixType_>
+template <bool Conjugate, typename RhsType, typename DstType>
+void HouseholderQR<MatrixType_>::_solve_impl_transposed(const RhsType& rhs, DstType& dst) const {
   const Index rank = (std::min)(rows(), cols());
 
   typename RhsType::PlainObject c(rhs);
 
   m_qr.topLeftCorner(rank, rank)
       .template triangularView<Upper>()
-      .transpose().template conjugateIf<Conjugate>()
+      .transpose()
+      .template conjugateIf<Conjugate>()
       .solveInPlace(c.topRows(rank));
 
   dst.topRows(rank) = c.topRows(rank);
-  dst.bottomRows(rows()-rank).setZero();
+  dst.bottomRows(rows() - rank).setZero();
 
-  dst.applyOnTheLeft(householderQ().setLength(rank).template conjugateIf<!Conjugate>() );
+  dst.applyOnTheLeft(householderQ().setLength(rank).template conjugateIf<!Conjugate>());
 }
 #endif
 
 /** Performs the QR factorization of the given matrix \a matrix. The result of
-  * the factorization is stored into \c *this, and a reference to \c *this
-  * is returned.
-  *
-  * \sa class HouseholderQR, HouseholderQR(const MatrixType&)
-  */
-template<typename MatrixType>
-void HouseholderQR<MatrixType>::computeInPlace()
-{
-  check_template_parameters();
-  
+ * the factorization is stored into \c *this, and a reference to \c *this
+ * is returned.
+ *
+ * \sa class HouseholderQR, HouseholderQR(const MatrixType&)
+ */
+template <typename MatrixType>
+void HouseholderQR<MatrixType>::computeInPlace() {
   Index rows = m_qr.rows();
   Index cols = m_qr.cols();
-  Index size = (std::min)(rows,cols);
+  Index size = (std::min)(rows, cols);
 
   m_hCoeffs.resize(size);
 
@@ -419,16 +486,14 @@
 }
 
 /** \return the Householder QR decomposition of \c *this.
-  *
-  * \sa class HouseholderQR
-  */
-template<typename Derived>
-const HouseholderQR<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::householderQr() const
-{
+ *
+ * \sa class HouseholderQR
+ */
+template <typename Derived>
+const HouseholderQR<typename MatrixBase<Derived>::PlainObject> MatrixBase<Derived>::householderQr() const {
   return HouseholderQR<PlainObject>(eval());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_QR_H
+#endif  // EIGEN_QR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/InternalHeaderCheck.h
new file mode 100644
index 0000000..bf8df01
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_QR_MODULE_H
+#error "Please include Eigen/QR instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h
index 17f8e44..7948ca3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h
@@ -1,9 +1,9 @@
 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
-// 
+//
 // We used the "A Divide-And-Conquer Algorithm for the Bidiagonal SVD"
 // research report written by Ming Gu and Stanley C.Eisenstat
-// The code variable names correspond to the names they used in their 
+// The code variable names correspond to the names they used in their
 // report
 //
 // Copyright (C) 2013 Gauthier Brun <brun.gauthier@gmail.com>
@@ -27,26 +27,51 @@
 #define eigen_internal_assert(X) assert(X);
 #endif
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
+#include <iostream>
+#endif
+
 namespace Eigen {
 
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
 IOFormat bdcsvdfmt(8, 0, ", ", "\n", "  [", "]");
 #endif
-  
-template<typename _MatrixType> class BDCSVD;
+
+template <typename MatrixType_, int Options>
+class BDCSVD;
 
 namespace internal {
 
-template<typename _MatrixType> 
-struct traits<BDCSVD<_MatrixType> >
-        : traits<_MatrixType>
-{
-  typedef _MatrixType MatrixType;
-};  
+template <typename MatrixType_, int Options>
+struct traits<BDCSVD<MatrixType_, Options> > : svd_traits<MatrixType_, Options> {
+  typedef MatrixType_ MatrixType;
+};
 
-} // end namespace internal
-  
-  
+template <typename MatrixType, int Options>
+struct allocate_small_svd {
+  static void run(JacobiSVD<MatrixType, Options>& smallSvd, Index rows, Index cols, unsigned int computationOptions) {
+    (void)computationOptions;
+    smallSvd = JacobiSVD<MatrixType, Options>(rows, cols);
+  }
+};
+
+EIGEN_DIAGNOSTICS(push)
+EIGEN_DISABLE_DEPRECATED_WARNING
+
+template <typename MatrixType>
+struct allocate_small_svd<MatrixType, 0> {
+  static void run(JacobiSVD<MatrixType>& smallSvd, Index rows, Index cols, unsigned int computationOptions) {
+    smallSvd = JacobiSVD<MatrixType>(rows, cols, computationOptions);
+  }
+};
+
+EIGEN_DIAGNOSTICS(pop)
+
+}  // end namespace internal
+
 /** \ingroup SVD_Module
  *
  *
@@ -54,7 +79,14 @@
  *
  * \brief class Bidiagonal Divide and Conquer SVD
  *
- * \tparam _MatrixType the type of the matrix of which we are computing the SVD decomposition
+ * \tparam MatrixType_ the type of the matrix of which we are computing the SVD decomposition
+ *
+ * \tparam Options_ this optional parameter allows one to specify options for computing unitaries \a U and \a V.
+ *                  Possible values are #ComputeThinU, #ComputeThinV, #ComputeFullU, #ComputeFullV, and
+ *                  #DisableQRDecomposition. It is not possible to request both the thin and full version of \a U or
+ *                  \a V. By default, unitaries are not computed. BDCSVD uses R-Bidiagonalization to improve
+ *                  performance on tall and wide matrices. For backwards compatility, the option
+ *                  #DisableQRDecomposition can be used to disable this optimization.
  *
  * This class first reduces the input matrix to bi-diagonal form using class UpperBidiagonalization,
  * and then performs a divide-and-conquer diagonalization. Small blocks are diagonalized using class JacobiSVD.
@@ -69,40 +101,44 @@
  *
  * \sa class JacobiSVD
  */
-template<typename _MatrixType> 
-class BDCSVD : public SVDBase<BDCSVD<_MatrixType> >
-{
+template <typename MatrixType_, int Options_>
+class BDCSVD : public SVDBase<BDCSVD<MatrixType_, Options_> > {
   typedef SVDBase<BDCSVD> Base;
-    
-public:
-  using Base::rows;
+
+ public:
   using Base::cols;
   using Base::computeU;
   using Base::computeV;
-  
-  typedef _MatrixType MatrixType;
-  typedef typename MatrixType::Scalar Scalar;
-  typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+  using Base::diagSize;
+  using Base::rows;
+
+  typedef MatrixType_ MatrixType;
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::RealScalar RealScalar;
   typedef typename NumTraits<RealScalar>::Literal Literal;
+  typedef typename Base::Index Index;
   enum {
-    RowsAtCompileTime = MatrixType::RowsAtCompileTime, 
-    ColsAtCompileTime = MatrixType::ColsAtCompileTime, 
-    DiagSizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime, ColsAtCompileTime), 
-    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, 
-    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, 
-    MaxDiagSizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_FIXED(MaxRowsAtCompileTime, MaxColsAtCompileTime), 
-    MatrixOptions = MatrixType::Options
+    Options = Options_,
+    QRDecomposition = Options & internal::QRPreconditionerBits,
+    ComputationOptions = Options & internal::ComputationOptionsBits,
+    RowsAtCompileTime = Base::RowsAtCompileTime,
+    ColsAtCompileTime = Base::ColsAtCompileTime,
+    DiagSizeAtCompileTime = Base::DiagSizeAtCompileTime,
+    MaxRowsAtCompileTime = Base::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = Base::MaxColsAtCompileTime,
+    MaxDiagSizeAtCompileTime = Base::MaxDiagSizeAtCompileTime,
+    MatrixOptions = Base::MatrixOptions
   };
 
   typedef typename Base::MatrixUType MatrixUType;
   typedef typename Base::MatrixVType MatrixVType;
   typedef typename Base::SingularValuesType SingularValuesType;
-  
+
   typedef Matrix<Scalar, Dynamic, Dynamic, ColMajor> MatrixX;
   typedef Matrix<RealScalar, Dynamic, Dynamic, ColMajor> MatrixXr;
   typedef Matrix<RealScalar, Dynamic, 1> VectorType;
   typedef Array<RealScalar, Dynamic, 1> ArrayXr;
-  typedef Array<Index,1,Dynamic> ArrayXi;
+  typedef Array<Index, 1, Dynamic> ArrayXi;
   typedef Ref<ArrayXr> ArrayRef;
   typedef Ref<ArrayXi> IndicesRef;
 
@@ -111,163 +147,210 @@
    * The default constructor is useful in cases in which the user intends to
    * perform decompositions via BDCSVD::compute(const MatrixType&).
    */
-  BDCSVD() : m_algoswap(16), m_isTranspose(false), m_compU(false), m_compV(false), m_numIters(0)
-  {}
-
+  BDCSVD() : m_algoswap(16), m_isTranspose(false), m_compU(false), m_compV(false), m_numIters(0) {}
 
   /** \brief Default Constructor with memory preallocation
    *
    * Like the default constructor but with preallocation of the internal data
-   * according to the specified problem size.
+   * according to the specified problem size and \a Options template parameter.
    * \sa BDCSVD()
    */
-  BDCSVD(Index rows, Index cols, unsigned int computationOptions = 0)
-    : m_algoswap(16), m_numIters(0)
-  {
+  BDCSVD(Index rows, Index cols) : m_algoswap(16), m_numIters(0) {
+    allocate(rows, cols, internal::get_computation_options(Options));
+  }
+
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem size and the \a computationOptions.
+   *
+   * One \b cannot request unitiaries using both the \a Options template parameter
+   * and the constructor. If possible, prefer using the \a Options template parameter.
+   *
+   * \param computationOptions specifification for computing Thin/Full unitaries U/V
+   * \sa BDCSVD()
+   *
+   * \deprecated Will be removed in the next major Eigen version. Options should
+   * be specified in the \a Options template parameter.
+   */
+  EIGEN_DEPRECATED BDCSVD(Index rows, Index cols, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) {
+    internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, rows, cols);
     allocate(rows, cols, computationOptions);
   }
 
-  /** \brief Constructor performing the decomposition of given matrix.
+  /** \brief Constructor performing the decomposition of given matrix, using the custom options specified
+   *         with the \a Options template paramter.
    *
    * \param matrix the matrix to decompose
-   * \param computationOptions optional parameter allowing to specify if you want full or thin U or V unitaries to be computed.
-   *                           By default, none is computed. This is a bit - field, the possible bits are #ComputeFullU, #ComputeThinU, 
-   *                           #ComputeFullV, #ComputeThinV.
-   *
-   * Thin unitaries are only available if your matrix type has a Dynamic number of columns (for example MatrixXf). They also are not
-   * available with the (non - default) FullPivHouseholderQR preconditioner.
    */
-  BDCSVD(const MatrixType& matrix, unsigned int computationOptions = 0)
-    : m_algoswap(16), m_numIters(0)
-  {
-    compute(matrix, computationOptions);
+  BDCSVD(const MatrixType& matrix) : m_algoswap(16), m_numIters(0) {
+    compute_impl(matrix, internal::get_computation_options(Options));
   }
 
-  ~BDCSVD() 
-  {
-  }
-  
-  /** \brief Method performing the decomposition of given matrix using custom options.
+  /** \brief Constructor performing the decomposition of given matrix using specified options
+   *         for computing unitaries.
+   *
+   *  One \b cannot request unitiaries using both the \a Options template parameter
+   *  and the constructor. If possible, prefer using the \a Options template parameter.
    *
    * \param matrix the matrix to decompose
-   * \param computationOptions optional parameter allowing to specify if you want full or thin U or V unitaries to be computed.
-   *                           By default, none is computed. This is a bit - field, the possible bits are #ComputeFullU, #ComputeThinU, 
-   *                           #ComputeFullV, #ComputeThinV.
+   * \param computationOptions specifification for computing Thin/Full unitaries U/V
    *
-   * Thin unitaries are only available if your matrix type has a Dynamic number of columns (for example MatrixXf). They also are not
-   * available with the (non - default) FullPivHouseholderQR preconditioner.
+   * \deprecated Will be removed in the next major Eigen version. Options should
+   * be specified in the \a Options template parameter.
    */
-  BDCSVD& compute(const MatrixType& matrix, unsigned int computationOptions);
-
-  /** \brief Method performing the decomposition of given matrix using current options.
-   *
-   * \param matrix the matrix to decompose
-   *
-   * This method uses the current \a computationOptions, as already passed to the constructor or to compute(const MatrixType&, unsigned int).
-   */
-  BDCSVD& compute(const MatrixType& matrix)
-  {
-    return compute(matrix, this->m_computationOptions);
+  EIGEN_DEPRECATED BDCSVD(const MatrixType& matrix, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) {
+    internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, matrix.rows(), matrix.cols());
+    compute_impl(matrix, computationOptions);
   }
 
-  void setSwitchSize(int s) 
-  {
-    eigen_assert(s>3 && "BDCSVD the size of the algo switch has to be greater than 3");
+  ~BDCSVD() {}
+
+  /** \brief Method performing the decomposition of given matrix. Computes Thin/Full unitaries U/V if specified
+   *         using the \a Options template parameter or the class constructor.
+   *
+   * \param matrix the matrix to decompose
+   */
+  BDCSVD& compute(const MatrixType& matrix) { return compute_impl(matrix, m_computationOptions); }
+
+  /** \brief Method performing the decomposition of given matrix, as specified by
+   *         the `computationOptions` parameter.
+   *
+   * \param matrix the matrix to decompose
+   * \param computationOptions specify whether to compute Thin/Full unitaries U/V
+   *
+   * \deprecated Will be removed in the next major Eigen version. Options should
+   * be specified in the \a Options template parameter.
+   */
+  EIGEN_DEPRECATED BDCSVD& compute(const MatrixType& matrix, unsigned int computationOptions) {
+    internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, matrix.rows(), matrix.cols());
+    return compute_impl(matrix, computationOptions);
+  }
+
+  void setSwitchSize(int s) {
+    eigen_assert(s >= 3 && "BDCSVD the size of the algo switch has to be at least 3.");
     m_algoswap = s;
   }
- 
-private:
-  void allocate(Index rows, Index cols, unsigned int computationOptions);
+
+ private:
+  BDCSVD& compute_impl(const MatrixType& matrix, unsigned int computationOptions);
   void divide(Index firstCol, Index lastCol, Index firstRowW, Index firstColW, Index shift);
   void computeSVDofM(Index firstCol, Index n, MatrixXr& U, VectorType& singVals, MatrixXr& V);
-  void computeSingVals(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm, VectorType& singVals, ArrayRef shifts, ArrayRef mus);
-  void perturbCol0(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm, const VectorType& singVals, const ArrayRef& shifts, const ArrayRef& mus, ArrayRef zhat);
-  void computeSingVecs(const ArrayRef& zhat, const ArrayRef& diag, const IndicesRef& perm, const VectorType& singVals, const ArrayRef& shifts, const ArrayRef& mus, MatrixXr& U, MatrixXr& V);
+  void computeSingVals(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm, VectorType& singVals,
+                       ArrayRef shifts, ArrayRef mus);
+  void perturbCol0(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm, const VectorType& singVals,
+                   const ArrayRef& shifts, const ArrayRef& mus, ArrayRef zhat);
+  void computeSingVecs(const ArrayRef& zhat, const ArrayRef& diag, const IndicesRef& perm, const VectorType& singVals,
+                       const ArrayRef& shifts, const ArrayRef& mus, MatrixXr& U, MatrixXr& V);
   void deflation43(Index firstCol, Index shift, Index i, Index size);
-  void deflation44(Index firstColu , Index firstColm, Index firstRowW, Index firstColW, Index i, Index j, Index size);
+  void deflation44(Index firstColu, Index firstColm, Index firstRowW, Index firstColW, Index i, Index j, Index size);
   void deflation(Index firstCol, Index lastCol, Index k, Index firstRowW, Index firstColW, Index shift);
-  template<typename HouseholderU, typename HouseholderV, typename NaiveU, typename NaiveV>
-  void copyUV(const HouseholderU &householderU, const HouseholderV &householderV, const NaiveU &naiveU, const NaiveV &naivev);
-  void structured_update(Block<MatrixXr,Dynamic,Dynamic> A, const MatrixXr &B, Index n1);
-  static RealScalar secularEq(RealScalar x, const ArrayRef& col0, const ArrayRef& diag, const IndicesRef &perm, const ArrayRef& diagShifted, RealScalar shift);
+  template <typename HouseholderU, typename HouseholderV, typename NaiveU, typename NaiveV>
+  void copyUV(const HouseholderU& householderU, const HouseholderV& householderV, const NaiveU& naiveU,
+              const NaiveV& naivev);
+  void structured_update(Block<MatrixXr, Dynamic, Dynamic> A, const MatrixXr& B, Index n1);
+  static RealScalar secularEq(RealScalar x, const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm,
+                              const ArrayRef& diagShifted, RealScalar shift);
+  template <typename SVDType>
+  void computeBaseCase(SVDType& svd, Index n, Index firstCol, Index firstRowW, Index firstColW, Index shift);
 
-protected:
+ protected:
+  void allocate(Index rows, Index cols, unsigned int computationOptions);
   MatrixXr m_naiveU, m_naiveV;
   MatrixXr m_computed;
   Index m_nRec;
   ArrayXr m_workspace;
   ArrayXi m_workspaceI;
   int m_algoswap;
-  bool m_isTranspose, m_compU, m_compV;
-  
-  using Base::m_singularValues;
-  using Base::m_diagSize;
-  using Base::m_computeFullU;
-  using Base::m_computeFullV;
+  bool m_isTranspose, m_compU, m_compV, m_useQrDecomp;
+  JacobiSVD<MatrixType, ComputationOptions> smallSvd;
+  HouseholderQR<MatrixX> qrDecomp;
+  internal::UpperBidiagonalization<MatrixX> bid;
+  MatrixX copyWorkspace;
+  MatrixX reducedTriangle;
+
+  using Base::m_computationOptions;
   using Base::m_computeThinU;
   using Base::m_computeThinV;
-  using Base::m_matrixU;
-  using Base::m_matrixV;
   using Base::m_info;
   using Base::m_isInitialized;
+  using Base::m_matrixU;
+  using Base::m_matrixV;
   using Base::m_nonzeroSingularValues;
+  using Base::m_singularValues;
 
-public:  
+ public:
   int m_numIters;
-}; //end class BDCSVD
-
+};  // end class BDCSVD
 
 // Method to allocate and initialize matrix and attributes
-template<typename MatrixType>
-void BDCSVD<MatrixType>::allocate(Eigen::Index rows, Eigen::Index cols, unsigned int computationOptions)
-{
-  m_isTranspose = (cols > rows);
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::allocate(Index rows, Index cols, unsigned int computationOptions) {
+  if (Base::allocate(rows, cols, computationOptions)) return;
 
-  if (Base::allocate(rows, cols, computationOptions))
-    return;
-  
-  m_computed = MatrixXr::Zero(m_diagSize + 1, m_diagSize );
+  if (cols < m_algoswap)
+    internal::allocate_small_svd<MatrixType, ComputationOptions>::run(smallSvd, rows, cols, computationOptions);
+
+  m_computed = MatrixXr::Zero(diagSize() + 1, diagSize());
   m_compU = computeV();
   m_compV = computeU();
-  if (m_isTranspose)
-    std::swap(m_compU, m_compV);
-  
-  if (m_compU) m_naiveU = MatrixXr::Zero(m_diagSize + 1, m_diagSize + 1 );
-  else         m_naiveU = MatrixXr::Zero(2, m_diagSize + 1 );
-  
-  if (m_compV) m_naiveV = MatrixXr::Zero(m_diagSize, m_diagSize);
-  
-  m_workspace.resize((m_diagSize+1)*(m_diagSize+1)*3);
-  m_workspaceI.resize(3*m_diagSize);
-}// end allocate
+  m_isTranspose = (cols > rows);
+  if (m_isTranspose) std::swap(m_compU, m_compV);
 
-template<typename MatrixType>
-BDCSVD<MatrixType>& BDCSVD<MatrixType>::compute(const MatrixType& matrix, unsigned int computationOptions) 
-{
+  // kMinAspectRatio is the crossover point that determines if we perform R-Bidiagonalization
+  // or bidiagonalize the input matrix directly.
+  // It is based off of LAPACK's dgesdd routine, which uses 11.0/6.0
+  // we use a larger scalar to prevent a regression for relatively square matrices.
+  constexpr Index kMinAspectRatio = 4;
+  constexpr bool disableQrDecomp = static_cast<int>(QRDecomposition) == static_cast<int>(DisableQRDecomposition);
+  m_useQrDecomp = !disableQrDecomp && ((rows / kMinAspectRatio > cols) || (cols / kMinAspectRatio > rows));
+  if (m_useQrDecomp) {
+    qrDecomp = HouseholderQR<MatrixX>((std::max)(rows, cols), (std::min)(rows, cols));
+    reducedTriangle = MatrixX(diagSize(), diagSize());
+  }
+
+  copyWorkspace = MatrixX(m_isTranspose ? cols : rows, m_isTranspose ? rows : cols);
+  bid = internal::UpperBidiagonalization<MatrixX>(m_useQrDecomp ? diagSize() : copyWorkspace.rows(),
+                                                  m_useQrDecomp ? diagSize() : copyWorkspace.cols());
+
+  if (m_compU)
+    m_naiveU = MatrixXr::Zero(diagSize() + 1, diagSize() + 1);
+  else
+    m_naiveU = MatrixXr::Zero(2, diagSize() + 1);
+
+  if (m_compV) m_naiveV = MatrixXr::Zero(diagSize(), diagSize());
+
+  m_workspace.resize((diagSize() + 1) * (diagSize() + 1) * 3);
+  m_workspaceI.resize(3 * diagSize());
+}  // end allocate
+
+template <typename MatrixType, int Options>
+BDCSVD<MatrixType, Options>& BDCSVD<MatrixType, Options>::compute_impl(const MatrixType& matrix,
+                                                                       unsigned int computationOptions) {
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-  std::cout << "\n\n\n======================================================================================================================\n\n\n";
+  std::cout << "\n\n\n================================================================================================="
+               "=====================\n\n\n";
 #endif
-  allocate(matrix.rows(), matrix.cols(), computationOptions);
   using std::abs;
 
+  allocate(matrix.rows(), matrix.cols(), computationOptions);
+
   const RealScalar considerZero = (std::numeric_limits<RealScalar>::min)();
-  
+
   //**** step -1 - If the problem is too small, directly falls back to JacobiSVD and return
-  if(matrix.cols() < m_algoswap)
-  {
-    // FIXME this line involves temporaries
-    JacobiSVD<MatrixType> jsvd(matrix,computationOptions);
+  if (matrix.cols() < m_algoswap) {
+    smallSvd.compute(matrix);
     m_isInitialized = true;
-    m_info = jsvd.info();
+    m_info = smallSvd.info();
     if (m_info == Success || m_info == NoConvergence) {
-      if(computeU()) m_matrixU = jsvd.matrixU();
-      if(computeV()) m_matrixV = jsvd.matrixV();
-      m_singularValues = jsvd.singularValues();
-      m_nonzeroSingularValues = jsvd.nonzeroSingularValues();
+      if (computeU()) m_matrixU = smallSvd.matrixU();
+      if (computeV()) m_matrixV = smallSvd.matrixV();
+      m_singularValues = smallSvd.singularValues();
+      m_nonzeroSingularValues = smallSvd.nonzeroSingularValues();
     }
     return *this;
   }
-  
+
   //**** step 0 - Copy the input matrix and apply scaling to reduce over/under-flows
   RealScalar scale = matrix.cwiseAbs().template maxCoeff<PropagateNaN>();
   if (!(numext::isfinite)(scale)) {
@@ -276,312 +359,325 @@
     return *this;
   }
 
-  if(scale==Literal(0)) scale = Literal(1);
-  MatrixX copy;
-  if (m_isTranspose) copy = matrix.adjoint()/scale;
-  else               copy = matrix/scale;
-  
-  //**** step 1 - Bidiagonalization
-  // FIXME this line involves temporaries
-  internal::UpperBidiagonalization<MatrixX> bid(copy);
+  if (numext::is_exactly_zero(scale)) scale = Literal(1);
+
+  if (m_isTranspose)
+    copyWorkspace = matrix.adjoint() / scale;
+  else
+    copyWorkspace = matrix / scale;
+
+  //**** step 1 - Bidiagonalization.
+  // If the problem is sufficiently rectangular, we perform R-Bidiagonalization: compute A = Q(R/0)
+  // and then bidiagonalize R. Otherwise, if the problem is relatively square, we
+  // bidiagonalize the input matrix directly.
+  if (m_useQrDecomp) {
+    qrDecomp.compute(copyWorkspace);
+    reducedTriangle = qrDecomp.matrixQR().topRows(diagSize());
+    reducedTriangle.template triangularView<StrictlyLower>().setZero();
+    bid.compute(reducedTriangle);
+  } else {
+    bid.compute(copyWorkspace);
+  }
 
   //**** step 2 - Divide & Conquer
   m_naiveU.setZero();
   m_naiveV.setZero();
   // FIXME this line involves a temporary matrix
-  m_computed.topRows(m_diagSize) = bid.bidiagonal().toDenseMatrix().transpose();
+  m_computed.topRows(diagSize()) = bid.bidiagonal().toDenseMatrix().transpose();
   m_computed.template bottomRows<1>().setZero();
-  divide(0, m_diagSize - 1, 0, 0, 0);
+  divide(0, diagSize() - 1, 0, 0, 0);
   if (m_info != Success && m_info != NoConvergence) {
     m_isInitialized = true;
     return *this;
   }
-    
+
   //**** step 3 - Copy singular values and vectors
-  for (int i=0; i<m_diagSize; i++)
-  {
+  for (int i = 0; i < diagSize(); i++) {
     RealScalar a = abs(m_computed.coeff(i, i));
     m_singularValues.coeffRef(i) = a * scale;
-    if (a<considerZero)
-    {
+    if (a < considerZero) {
       m_nonzeroSingularValues = i;
-      m_singularValues.tail(m_diagSize - i - 1).setZero();
+      m_singularValues.tail(diagSize() - i - 1).setZero();
       break;
-    }
-    else if (i == m_diagSize - 1)
-    {
+    } else if (i == diagSize() - 1) {
       m_nonzeroSingularValues = i + 1;
       break;
     }
   }
 
-#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-//   std::cout << "m_naiveU\n" << m_naiveU << "\n\n";
-//   std::cout << "m_naiveV\n" << m_naiveV << "\n\n";
-#endif
-  if(m_isTranspose) copyUV(bid.householderV(), bid.householderU(), m_naiveV, m_naiveU);
-  else              copyUV(bid.householderU(), bid.householderV(), m_naiveU, m_naiveV);
+  //**** step 4 - Finalize unitaries U and V
+  if (m_isTranspose)
+    copyUV(bid.householderV(), bid.householderU(), m_naiveV, m_naiveU);
+  else
+    copyUV(bid.householderU(), bid.householderV(), m_naiveU, m_naiveV);
+
+  if (m_useQrDecomp) {
+    if (m_isTranspose && computeV())
+      m_matrixV.applyOnTheLeft(qrDecomp.householderQ());
+    else if (!m_isTranspose && computeU())
+      m_matrixU.applyOnTheLeft(qrDecomp.householderQ());
+  }
 
   m_isInitialized = true;
   return *this;
-}// end compute
+}  // end compute
 
-
-template<typename MatrixType>
-template<typename HouseholderU, typename HouseholderV, typename NaiveU, typename NaiveV>
-void BDCSVD<MatrixType>::copyUV(const HouseholderU &householderU, const HouseholderV &householderV, const NaiveU &naiveU, const NaiveV &naiveV)
-{
+template <typename MatrixType, int Options>
+template <typename HouseholderU, typename HouseholderV, typename NaiveU, typename NaiveV>
+void BDCSVD<MatrixType, Options>::copyUV(const HouseholderU& householderU, const HouseholderV& householderV,
+                                         const NaiveU& naiveU, const NaiveV& naiveV) {
   // Note exchange of U and V: m_matrixU is set from m_naiveV and vice versa
-  if (computeU())
-  {
-    Index Ucols = m_computeThinU ? m_diagSize : householderU.cols();
-    m_matrixU = MatrixX::Identity(householderU.cols(), Ucols);
-    m_matrixU.topLeftCorner(m_diagSize, m_diagSize) = naiveV.template cast<Scalar>().topLeftCorner(m_diagSize, m_diagSize);
-    householderU.applyThisOnTheLeft(m_matrixU); // FIXME this line involves a temporary buffer
+  if (computeU()) {
+    Index Ucols = m_computeThinU ? diagSize() : rows();
+    m_matrixU = MatrixX::Identity(rows(), Ucols);
+    m_matrixU.topLeftCorner(diagSize(), diagSize()) =
+        naiveV.template cast<Scalar>().topLeftCorner(diagSize(), diagSize());
+    // FIXME the following conditionals involve temporary buffers
+    if (m_useQrDecomp)
+      m_matrixU.topLeftCorner(householderU.cols(), diagSize()).applyOnTheLeft(householderU);
+    else
+      m_matrixU.applyOnTheLeft(householderU);
   }
-  if (computeV())
-  {
-    Index Vcols = m_computeThinV ? m_diagSize : householderV.cols();
-    m_matrixV = MatrixX::Identity(householderV.cols(), Vcols);
-    m_matrixV.topLeftCorner(m_diagSize, m_diagSize) = naiveU.template cast<Scalar>().topLeftCorner(m_diagSize, m_diagSize);
-    householderV.applyThisOnTheLeft(m_matrixV); // FIXME this line involves a temporary buffer
+  if (computeV()) {
+    Index Vcols = m_computeThinV ? diagSize() : cols();
+    m_matrixV = MatrixX::Identity(cols(), Vcols);
+    m_matrixV.topLeftCorner(diagSize(), diagSize()) =
+        naiveU.template cast<Scalar>().topLeftCorner(diagSize(), diagSize());
+    // FIXME the following conditionals involve temporary buffers
+    if (m_useQrDecomp)
+      m_matrixV.topLeftCorner(householderV.cols(), diagSize()).applyOnTheLeft(householderV);
+    else
+      m_matrixV.applyOnTheLeft(householderV);
   }
 }
 
 /** \internal
-  * Performs A = A * B exploiting the special structure of the matrix A. Splitting A as:
-  *  A = [A1]
-  *      [A2]
-  * such that A1.rows()==n1, then we assume that at least half of the columns of A1 and A2 are zeros.
-  * We can thus pack them prior to the the matrix product. However, this is only worth the effort if the matrix is large
-  * enough.
-  */
-template<typename MatrixType>
-void BDCSVD<MatrixType>::structured_update(Block<MatrixXr,Dynamic,Dynamic> A, const MatrixXr &B, Index n1)
-{
+ * Performs A = A * B exploiting the special structure of the matrix A. Splitting A as:
+ *  A = [A1]
+ *      [A2]
+ * such that A1.rows()==n1, then we assume that at least half of the columns of A1 and A2 are zeros.
+ * We can thus pack them prior to the the matrix product. However, this is only worth the effort if the matrix is large
+ * enough.
+ */
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::structured_update(Block<MatrixXr, Dynamic, Dynamic> A, const MatrixXr& B, Index n1) {
   Index n = A.rows();
-  if(n>100)
-  {
+  if (n > 100) {
     // If the matrices are large enough, let's exploit the sparse structure of A by
     // splitting it in half (wrt n1), and packing the non-zero columns.
     Index n2 = n - n1;
-    Map<MatrixXr> A1(m_workspace.data()      , n1, n);
-    Map<MatrixXr> A2(m_workspace.data()+ n1*n, n2, n);
-    Map<MatrixXr> B1(m_workspace.data()+  n*n, n,  n);
-    Map<MatrixXr> B2(m_workspace.data()+2*n*n, n,  n);
-    Index k1=0, k2=0;
-    for(Index j=0; j<n; ++j)
-    {
-      if( (A.col(j).head(n1).array()!=Literal(0)).any() )
-      {
+    Map<MatrixXr> A1(m_workspace.data(), n1, n);
+    Map<MatrixXr> A2(m_workspace.data() + n1 * n, n2, n);
+    Map<MatrixXr> B1(m_workspace.data() + n * n, n, n);
+    Map<MatrixXr> B2(m_workspace.data() + 2 * n * n, n, n);
+    Index k1 = 0, k2 = 0;
+    for (Index j = 0; j < n; ++j) {
+      if ((A.col(j).head(n1).array() != Literal(0)).any()) {
         A1.col(k1) = A.col(j).head(n1);
         B1.row(k1) = B.row(j);
         ++k1;
       }
-      if( (A.col(j).tail(n2).array()!=Literal(0)).any() )
-      {
+      if ((A.col(j).tail(n2).array() != Literal(0)).any()) {
         A2.col(k2) = A.col(j).tail(n2);
         B2.row(k2) = B.row(j);
         ++k2;
       }
     }
-  
-    A.topRows(n1).noalias()    = A1.leftCols(k1) * B1.topRows(k1);
+
+    A.topRows(n1).noalias() = A1.leftCols(k1) * B1.topRows(k1);
     A.bottomRows(n2).noalias() = A2.leftCols(k2) * B2.topRows(k2);
-  }
-  else
-  {
-    Map<MatrixXr,Aligned> tmp(m_workspace.data(),n,n);
-    tmp.noalias() = A*B;
+  } else {
+    Map<MatrixXr, Aligned> tmp(m_workspace.data(), n, n);
+    tmp.noalias() = A * B;
     A = tmp;
   }
 }
 
-// The divide algorithm is done "in place", we are always working on subsets of the same matrix. The divide methods takes as argument the 
-// place of the submatrix we are currently working on.
+template <typename MatrixType, int Options>
+template <typename SVDType>
+void BDCSVD<MatrixType, Options>::computeBaseCase(SVDType& svd, Index n, Index firstCol, Index firstRowW,
+                                                  Index firstColW, Index shift) {
+  svd.compute(m_computed.block(firstCol, firstCol, n + 1, n));
+  m_info = svd.info();
+  if (m_info != Success && m_info != NoConvergence) return;
+  if (m_compU)
+    m_naiveU.block(firstCol, firstCol, n + 1, n + 1).real() = svd.matrixU();
+  else {
+    m_naiveU.row(0).segment(firstCol, n + 1).real() = svd.matrixU().row(0);
+    m_naiveU.row(1).segment(firstCol, n + 1).real() = svd.matrixU().row(n);
+  }
+  if (m_compV) m_naiveV.block(firstRowW, firstColW, n, n).real() = svd.matrixV();
+  m_computed.block(firstCol + shift, firstCol + shift, n + 1, n).setZero();
+  m_computed.diagonal().segment(firstCol + shift, n) = svd.singularValues().head(n);
+}
+
+// The divide algorithm is done "in place", we are always working on subsets of the same matrix. The divide methods
+// takes as argument the place of the submatrix we are currently working on.
 
 //@param firstCol : The Index of the first column of the submatrix of m_computed and for m_naiveU;
-//@param lastCol : The Index of the last column of the submatrix of m_computed and for m_naiveU; 
+//@param lastCol : The Index of the last column of the submatrix of m_computed and for m_naiveU;
 // lastCol + 1 - firstCol is the size of the submatrix.
-//@param firstRowW : The Index of the first row of the matrix W that we are to change. (see the reference paper section 1 for more information on W)
-//@param firstRowW : Same as firstRowW with the column.
-//@param shift : Each time one takes the left submatrix, one must add 1 to the shift. Why? Because! We actually want the last column of the U submatrix 
-// to become the first column (*coeff) and to shift all the other columns to the right. There are more details on the reference paper.
-template<typename MatrixType>
-void BDCSVD<MatrixType>::divide(Eigen::Index firstCol, Eigen::Index lastCol, Eigen::Index firstRowW, Eigen::Index firstColW, Eigen::Index shift)
-{
+//@param firstRowW : The Index of the first row of the matrix W that we are to change. (see the reference paper section
+// 1 for more information on W)
+//@param firstColW : Same as firstRowW with the column.
+//@param shift : Each time one takes the left submatrix, one must add 1 to the shift. Why? Because! We actually want the
+// last column of the U submatrix
+// to become the first column (*coeff) and to shift all the other columns to the right. There are more details on the
+// reference paper.
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::divide(Index firstCol, Index lastCol, Index firstRowW, Index firstColW, Index shift) {
   // requires rows = cols + 1;
+  using std::abs;
   using std::pow;
   using std::sqrt;
-  using std::abs;
   const Index n = lastCol - firstCol + 1;
-  const Index k = n/2;
+  const Index k = n / 2;
   const RealScalar considerZero = (std::numeric_limits<RealScalar>::min)();
   RealScalar alphaK;
-  RealScalar betaK; 
-  RealScalar r0; 
+  RealScalar betaK;
+  RealScalar r0;
   RealScalar lambda, phi, c0, s0;
   VectorType l, f;
-  // We use the other algorithm which is more efficient for small 
+  // We use the other algorithm which is more efficient for small
   // matrices.
-  if (n < m_algoswap)
-  {
-    // FIXME this line involves temporaries
-    JacobiSVD<MatrixXr> b(m_computed.block(firstCol, firstCol, n + 1, n), ComputeFullU | (m_compV ? ComputeFullV : 0));
-    m_info = b.info();
-    if (m_info != Success && m_info != NoConvergence) return;
-    if (m_compU)
-      m_naiveU.block(firstCol, firstCol, n + 1, n + 1).real() = b.matrixU();
-    else 
-    {
-      m_naiveU.row(0).segment(firstCol, n + 1).real() = b.matrixU().row(0);
-      m_naiveU.row(1).segment(firstCol, n + 1).real() = b.matrixU().row(n);
+  if (n < m_algoswap) {
+    // FIXME this block involves temporaries
+    if (m_compV) {
+      JacobiSVD<MatrixXr, ComputeFullU | ComputeFullV> baseSvd;
+      computeBaseCase(baseSvd, n, firstCol, firstRowW, firstColW, shift);
+    } else {
+      JacobiSVD<MatrixXr, ComputeFullU> baseSvd;
+      computeBaseCase(baseSvd, n, firstCol, firstRowW, firstColW, shift);
     }
-    if (m_compV) m_naiveV.block(firstRowW, firstColW, n, n).real() = b.matrixV();
-    m_computed.block(firstCol + shift, firstCol + shift, n + 1, n).setZero();
-    m_computed.diagonal().segment(firstCol + shift, n) = b.singularValues().head(n);
     return;
   }
   // We use the divide and conquer algorithm
-  alphaK =  m_computed(firstCol + k, firstCol + k);
+  alphaK = m_computed(firstCol + k, firstCol + k);
   betaK = m_computed(firstCol + k + 1, firstCol + k);
   // The divide must be done in that order in order to have good results. Divide change the data inside the submatrices
-  // and the divide of the right submatrice reads one column of the left submatrice. That's why we need to treat the 
-  // right submatrix before the left one. 
+  // and the divide of the right submatrice reads one column of the left submatrice. That's why we need to treat the
+  // right submatrix before the left one.
   divide(k + 1 + firstCol, lastCol, k + 1 + firstRowW, k + 1 + firstColW, shift);
   if (m_info != Success && m_info != NoConvergence) return;
   divide(firstCol, k - 1 + firstCol, firstRowW, firstColW + 1, shift + 1);
   if (m_info != Success && m_info != NoConvergence) return;
 
-  if (m_compU)
-  {
+  if (m_compU) {
     lambda = m_naiveU(firstCol + k, firstCol + k);
     phi = m_naiveU(firstCol + k + 1, lastCol + 1);
-  } 
-  else 
-  {
+  } else {
     lambda = m_naiveU(1, firstCol + k);
     phi = m_naiveU(0, lastCol + 1);
   }
   r0 = sqrt((abs(alphaK * lambda) * abs(alphaK * lambda)) + abs(betaK * phi) * abs(betaK * phi));
-  if (m_compU)
-  {
+  if (m_compU) {
     l = m_naiveU.row(firstCol + k).segment(firstCol, k);
     f = m_naiveU.row(firstCol + k + 1).segment(firstCol + k + 1, n - k - 1);
-  } 
-  else 
-  {
+  } else {
     l = m_naiveU.row(1).segment(firstCol, k);
     f = m_naiveU.row(0).segment(firstCol + k + 1, n - k - 1);
   }
-  if (m_compV) m_naiveV(firstRowW+k, firstColW) = Literal(1);
-  if (r0<considerZero)
-  {
+  if (m_compV) m_naiveV(firstRowW + k, firstColW) = Literal(1);
+  if (r0 < considerZero) {
     c0 = Literal(1);
     s0 = Literal(0);
-  }
-  else
-  {
+  } else {
     c0 = alphaK * lambda / r0;
     s0 = betaK * phi / r0;
   }
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(m_naiveU.allFinite());
-  assert(m_naiveV.allFinite());
-  assert(m_computed.allFinite());
+  eigen_internal_assert(m_naiveU.allFinite());
+  eigen_internal_assert(m_naiveV.allFinite());
+  eigen_internal_assert(m_computed.allFinite());
 #endif
-  
-  if (m_compU)
-  {
-    MatrixXr q1 (m_naiveU.col(firstCol + k).segment(firstCol, k + 1));     
+
+  if (m_compU) {
+    MatrixXr q1(m_naiveU.col(firstCol + k).segment(firstCol, k + 1));
     // we shiftW Q1 to the right
-    for (Index i = firstCol + k - 1; i >= firstCol; i--) 
+    for (Index i = firstCol + k - 1; i >= firstCol; i--)
       m_naiveU.col(i + 1).segment(firstCol, k + 1) = m_naiveU.col(i).segment(firstCol, k + 1);
     // we shift q1 at the left with a factor c0
-    m_naiveU.col(firstCol).segment( firstCol, k + 1) = (q1 * c0);
+    m_naiveU.col(firstCol).segment(firstCol, k + 1) = (q1 * c0);
     // last column = q1 * - s0
-    m_naiveU.col(lastCol + 1).segment(firstCol, k + 1) = (q1 * ( - s0));
+    m_naiveU.col(lastCol + 1).segment(firstCol, k + 1) = (q1 * (-s0));
     // first column = q2 * s0
-    m_naiveU.col(firstCol).segment(firstCol + k + 1, n - k) = m_naiveU.col(lastCol + 1).segment(firstCol + k + 1, n - k) * s0; 
+    m_naiveU.col(firstCol).segment(firstCol + k + 1, n - k) =
+        m_naiveU.col(lastCol + 1).segment(firstCol + k + 1, n - k) * s0;
     // q2 *= c0
     m_naiveU.col(lastCol + 1).segment(firstCol + k + 1, n - k) *= c0;
-  } 
-  else 
-  {
+  } else {
     RealScalar q1 = m_naiveU(0, firstCol + k);
     // we shift Q1 to the right
-    for (Index i = firstCol + k - 1; i >= firstCol; i--) 
-      m_naiveU(0, i + 1) = m_naiveU(0, i);
+    for (Index i = firstCol + k - 1; i >= firstCol; i--) m_naiveU(0, i + 1) = m_naiveU(0, i);
     // we shift q1 at the left with a factor c0
     m_naiveU(0, firstCol) = (q1 * c0);
     // last column = q1 * - s0
-    m_naiveU(0, lastCol + 1) = (q1 * ( - s0));
+    m_naiveU(0, lastCol + 1) = (q1 * (-s0));
     // first column = q2 * s0
-    m_naiveU(1, firstCol) = m_naiveU(1, lastCol + 1) *s0; 
+    m_naiveU(1, firstCol) = m_naiveU(1, lastCol + 1) * s0;
     // q2 *= c0
     m_naiveU(1, lastCol + 1) *= c0;
     m_naiveU.row(1).segment(firstCol + 1, k).setZero();
     m_naiveU.row(0).segment(firstCol + k + 1, n - k - 1).setZero();
   }
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(m_naiveU.allFinite());
-  assert(m_naiveV.allFinite());
-  assert(m_computed.allFinite());
+  eigen_internal_assert(m_naiveU.allFinite());
+  eigen_internal_assert(m_naiveV.allFinite());
+  eigen_internal_assert(m_computed.allFinite());
 #endif
-  
+
   m_computed(firstCol + shift, firstCol + shift) = r0;
   m_computed.col(firstCol + shift).segment(firstCol + shift + 1, k) = alphaK * l.transpose().real();
   m_computed.col(firstCol + shift).segment(firstCol + shift + k + 1, n - k - 1) = betaK * f.transpose().real();
 
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-  ArrayXr tmp1 = (m_computed.block(firstCol+shift, firstCol+shift, n, n)).jacobiSvd().singularValues();
+  ArrayXr tmp1 = (m_computed.block(firstCol + shift, firstCol + shift, n, n)).jacobiSvd().singularValues();
 #endif
   // Second part: try to deflate singular values in combined matrix
   deflation(firstCol, lastCol, k, firstRowW, firstColW, shift);
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-  ArrayXr tmp2 = (m_computed.block(firstCol+shift, firstCol+shift, n, n)).jacobiSvd().singularValues();
+  ArrayXr tmp2 = (m_computed.block(firstCol + shift, firstCol + shift, n, n)).jacobiSvd().singularValues();
   std::cout << "\n\nj1 = " << tmp1.transpose().format(bdcsvdfmt) << "\n";
   std::cout << "j2 = " << tmp2.transpose().format(bdcsvdfmt) << "\n\n";
-  std::cout << "err:      " << ((tmp1-tmp2).abs()>1e-12*tmp2.abs()).transpose() << "\n";
+  std::cout << "err:      " << ((tmp1 - tmp2).abs() > 1e-12 * tmp2.abs()).transpose() << "\n";
   static int count = 0;
   std::cout << "# " << ++count << "\n\n";
-  assert((tmp1-tmp2).matrix().norm() < 1e-14*tmp2.matrix().norm());
-//   assert(count<681);
-//   assert(((tmp1-tmp2).abs()<1e-13*tmp2.abs()).all());
+  eigen_internal_assert((tmp1 - tmp2).matrix().norm() < 1e-14 * tmp2.matrix().norm());
+//   eigen_internal_assert(count<681);
+//   eigen_internal_assert(((tmp1-tmp2).abs()<1e-13*tmp2.abs()).all());
 #endif
-  
+
   // Third part: compute SVD of combined matrix
   MatrixXr UofSVD, VofSVD;
   VectorType singVals;
   computeSVDofM(firstCol + shift, n, UofSVD, singVals, VofSVD);
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(UofSVD.allFinite());
-  assert(VofSVD.allFinite());
+  eigen_internal_assert(UofSVD.allFinite());
+  eigen_internal_assert(VofSVD.allFinite());
 #endif
-  
+
   if (m_compU)
-    structured_update(m_naiveU.block(firstCol, firstCol, n + 1, n + 1), UofSVD, (n+2)/2);
-  else
-  {
-    Map<Matrix<RealScalar,2,Dynamic>,Aligned> tmp(m_workspace.data(),2,n+1);
-    tmp.noalias() = m_naiveU.middleCols(firstCol, n+1) * UofSVD;
+    structured_update(m_naiveU.block(firstCol, firstCol, n + 1, n + 1), UofSVD, (n + 2) / 2);
+  else {
+    Map<Matrix<RealScalar, 2, Dynamic>, Aligned> tmp(m_workspace.data(), 2, n + 1);
+    tmp.noalias() = m_naiveU.middleCols(firstCol, n + 1) * UofSVD;
     m_naiveU.middleCols(firstCol, n + 1) = tmp;
   }
-  
-  if (m_compV)  structured_update(m_naiveV.block(firstRowW, firstColW, n, n), VofSVD, (n+1)/2);
-  
+
+  if (m_compV) structured_update(m_naiveV.block(firstRowW, firstColW, n, n), VofSVD, (n + 1) / 2);
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(m_naiveU.allFinite());
-  assert(m_naiveV.allFinite());
-  assert(m_computed.allFinite());
+  eigen_internal_assert(m_naiveU.allFinite());
+  eigen_internal_assert(m_naiveV.allFinite());
+  eigen_internal_assert(m_computed.allFinite());
 #endif
-  
+
   m_computed.block(firstCol + shift, firstCol + shift, n, n).setZero();
   m_computed.block(firstCol + shift, firstCol + shift, n, n).diagonal() = singVals;
-}// end divide
+}  // end divide
 
 // Compute SVD of m_computed.block(firstCol, firstCol, n + 1, n); this block only has non-zeros in
 // the first column and on the diagonal and has undergone deflation, so diagonal is in increasing
@@ -590,470 +686,482 @@
 //
 // TODO Opportunities for optimization: better root finding algo, better stopping criterion, better
 // handling of round-off errors, be consistent in ordering
-// For instance, to solve the secular equation using FMM, see http://www.stat.uchicago.edu/~lekheng/courses/302/classics/greengard-rokhlin.pdf
-template <typename MatrixType>
-void BDCSVD<MatrixType>::computeSVDofM(Eigen::Index firstCol, Eigen::Index n, MatrixXr& U, VectorType& singVals, MatrixXr& V)
-{
+// For instance, to solve the secular equation using FMM, see
+// http://www.stat.uchicago.edu/~lekheng/courses/302/classics/greengard-rokhlin.pdf
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::computeSVDofM(Index firstCol, Index n, MatrixXr& U, VectorType& singVals,
+                                                MatrixXr& V) {
   const RealScalar considerZero = (std::numeric_limits<RealScalar>::min)();
   using std::abs;
   ArrayRef col0 = m_computed.col(firstCol).segment(firstCol, n);
-  m_workspace.head(n) =  m_computed.block(firstCol, firstCol, n, n).diagonal();
+  m_workspace.head(n) = m_computed.block(firstCol, firstCol, n, n).diagonal();
   ArrayRef diag = m_workspace.head(n);
   diag(0) = Literal(0);
 
   // Allocate space for singular values and vectors
   singVals.resize(n);
-  U.resize(n+1, n+1);
+  U.resize(n + 1, n + 1);
   if (m_compV) V.resize(n, n);
 
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-  if (col0.hasNaN() || diag.hasNaN())
-    std::cout << "\n\nHAS NAN\n\n";
+  if (col0.hasNaN() || diag.hasNaN()) std::cout << "\n\nHAS NAN\n\n";
 #endif
-  
+
   // Many singular values might have been deflated, the zero ones have been moved to the end,
   // but others are interleaved and we must ignore them at this stage.
   // To this end, let's compute a permutation skipping them:
   Index actual_n = n;
-  while(actual_n>1 && diag(actual_n-1)==Literal(0)) {--actual_n; eigen_internal_assert(col0(actual_n)==Literal(0)); }
-  Index m = 0; // size of the deflated problem
-  for(Index k=0;k<actual_n;++k)
-    if(abs(col0(k))>considerZero)
-      m_workspaceI(m++) = k;
-  Map<ArrayXi> perm(m_workspaceI.data(),m);
-  
-  Map<ArrayXr> shifts(m_workspace.data()+1*n, n);
-  Map<ArrayXr> mus(m_workspace.data()+2*n, n);
-  Map<ArrayXr> zhat(m_workspace.data()+3*n, n);
+  while (actual_n > 1 && numext::is_exactly_zero(diag(actual_n - 1))) {
+    --actual_n;
+    eigen_internal_assert(numext::is_exactly_zero(col0(actual_n)));
+  }
+  Index m = 0;  // size of the deflated problem
+  for (Index k = 0; k < actual_n; ++k)
+    if (abs(col0(k)) > considerZero) m_workspaceI(m++) = k;
+  Map<ArrayXi> perm(m_workspaceI.data(), m);
+
+  Map<ArrayXr> shifts(m_workspace.data() + 1 * n, n);
+  Map<ArrayXr> mus(m_workspace.data() + 2 * n, n);
+  Map<ArrayXr> zhat(m_workspace.data() + 3 * n, n);
 
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
   std::cout << "computeSVDofM using:\n";
   std::cout << "  z: " << col0.transpose() << "\n";
   std::cout << "  d: " << diag.transpose() << "\n";
 #endif
-  
+
   // Compute singVals, shifts, and mus
   computeSingVals(col0, diag, perm, singVals, shifts, mus);
-  
+
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-  std::cout << "  j:        " << (m_computed.block(firstCol, firstCol, n, n)).jacobiSvd().singularValues().transpose().reverse() << "\n\n";
+  std::cout << "  j:        "
+            << (m_computed.block(firstCol, firstCol, n, n)).jacobiSvd().singularValues().transpose().reverse()
+            << "\n\n";
   std::cout << "  sing-val: " << singVals.transpose() << "\n";
   std::cout << "  mu:       " << mus.transpose() << "\n";
   std::cout << "  shift:    " << shifts.transpose() << "\n";
-  
+
   {
     std::cout << "\n\n    mus:    " << mus.head(actual_n).transpose() << "\n\n";
-    std::cout << "    check1 (expect0) : " << ((singVals.array()-(shifts+mus)) / singVals.array()).head(actual_n).transpose() << "\n\n";
-    assert((((singVals.array()-(shifts+mus)) / singVals.array()).head(actual_n) >= 0).all());
-    std::cout << "    check2 (>0)      : " << ((singVals.array()-diag) / singVals.array()).head(actual_n).transpose() << "\n\n";
-    assert((((singVals.array()-diag) / singVals.array()).head(actual_n) >= 0).all());
+    std::cout << "    check1 (expect0) : "
+              << ((singVals.array() - (shifts + mus)) / singVals.array()).head(actual_n).transpose() << "\n\n";
+    eigen_internal_assert((((singVals.array() - (shifts + mus)) / singVals.array()).head(actual_n) >= 0).all());
+    std::cout << "    check2 (>0)      : " << ((singVals.array() - diag) / singVals.array()).head(actual_n).transpose()
+              << "\n\n";
+    eigen_internal_assert((((singVals.array() - diag) / singVals.array()).head(actual_n) >= 0).all());
   }
 #endif
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(singVals.allFinite());
-  assert(mus.allFinite());
-  assert(shifts.allFinite());
+  eigen_internal_assert(singVals.allFinite());
+  eigen_internal_assert(mus.allFinite());
+  eigen_internal_assert(shifts.allFinite());
 #endif
-  
+
   // Compute zhat
   perturbCol0(col0, diag, perm, singVals, shifts, mus, zhat);
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
   std::cout << "  zhat: " << zhat.transpose() << "\n";
 #endif
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(zhat.allFinite());
+  eigen_internal_assert(zhat.allFinite());
 #endif
-  
+
   computeSingVecs(zhat, diag, perm, singVals, shifts, mus, U, V);
-  
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
-  std::cout << "U^T U: " << (U.transpose() * U - MatrixXr(MatrixXr::Identity(U.cols(),U.cols()))).norm() << "\n";
-  std::cout << "V^T V: " << (V.transpose() * V - MatrixXr(MatrixXr::Identity(V.cols(),V.cols()))).norm() << "\n";
+
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
+  std::cout << "U^T U: " << (U.transpose() * U - MatrixXr(MatrixXr::Identity(U.cols(), U.cols()))).norm() << "\n";
+  std::cout << "V^T V: " << (V.transpose() * V - MatrixXr(MatrixXr::Identity(V.cols(), V.cols()))).norm() << "\n";
 #endif
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(m_naiveU.allFinite());
-  assert(m_naiveV.allFinite());
-  assert(m_computed.allFinite());
-  assert(U.allFinite());
-  assert(V.allFinite());
-//   assert((U.transpose() * U - MatrixXr(MatrixXr::Identity(U.cols(),U.cols()))).norm() < 100*NumTraits<RealScalar>::epsilon() * n);
-//   assert((V.transpose() * V - MatrixXr(MatrixXr::Identity(V.cols(),V.cols()))).norm() < 100*NumTraits<RealScalar>::epsilon() * n);
+  eigen_internal_assert(m_naiveU.allFinite());
+  eigen_internal_assert(m_naiveV.allFinite());
+  eigen_internal_assert(m_computed.allFinite());
+  eigen_internal_assert(U.allFinite());
+  eigen_internal_assert(V.allFinite());
+//   eigen_internal_assert((U.transpose() * U - MatrixXr(MatrixXr::Identity(U.cols(),U.cols()))).norm() <
+//   100*NumTraits<RealScalar>::epsilon() * n); eigen_internal_assert((V.transpose() * V -
+//   MatrixXr(MatrixXr::Identity(V.cols(),V.cols()))).norm() < 100*NumTraits<RealScalar>::epsilon() * n);
 #endif
-  
+
   // Because of deflation, the singular values might not be completely sorted.
   // Fortunately, reordering them is a O(n) problem
-  for(Index i=0; i<actual_n-1; ++i)
-  {
-    if(singVals(i)>singVals(i+1))
-    {
+  for (Index i = 0; i < actual_n - 1; ++i) {
+    if (singVals(i) > singVals(i + 1)) {
       using std::swap;
-      swap(singVals(i),singVals(i+1));
-      U.col(i).swap(U.col(i+1));
-      if(m_compV) V.col(i).swap(V.col(i+1));
+      swap(singVals(i), singVals(i + 1));
+      U.col(i).swap(U.col(i + 1));
+      if (m_compV) V.col(i).swap(V.col(i + 1));
     }
   }
 
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
   {
-    bool singular_values_sorted = (((singVals.segment(1,actual_n-1)-singVals.head(actual_n-1))).array() >= 0).all();
-    if(!singular_values_sorted)
-      std::cout << "Singular values are not sorted: " << singVals.segment(1,actual_n).transpose() << "\n";
-    assert(singular_values_sorted);
+    bool singular_values_sorted =
+        (((singVals.segment(1, actual_n - 1) - singVals.head(actual_n - 1))).array() >= 0).all();
+    if (!singular_values_sorted)
+      std::cout << "Singular values are not sorted: " << singVals.segment(1, actual_n).transpose() << "\n";
+    eigen_internal_assert(singular_values_sorted);
   }
 #endif
-  
+
   // Reverse order so that singular values in increased order
   // Because of deflation, the zeros singular-values are already at the end
   singVals.head(actual_n).reverseInPlace();
   U.leftCols(actual_n).rowwise().reverseInPlace();
   if (m_compV) V.leftCols(actual_n).rowwise().reverseInPlace();
-  
+
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-  JacobiSVD<MatrixXr> jsvd(m_computed.block(firstCol, firstCol, n, n) );
+  JacobiSVD<MatrixXr> jsvd(m_computed.block(firstCol, firstCol, n, n));
   std::cout << "  * j:        " << jsvd.singularValues().transpose() << "\n\n";
   std::cout << "  * sing-val: " << singVals.transpose() << "\n";
 //   std::cout << "  * err:      " << ((jsvd.singularValues()-singVals)>1e-13*singVals.norm()).transpose() << "\n";
 #endif
 }
 
-template <typename MatrixType>
-typename BDCSVD<MatrixType>::RealScalar BDCSVD<MatrixType>::secularEq(RealScalar mu, const ArrayRef& col0, const ArrayRef& diag, const IndicesRef &perm, const ArrayRef& diagShifted, RealScalar shift)
-{
+template <typename MatrixType, int Options>
+typename BDCSVD<MatrixType, Options>::RealScalar BDCSVD<MatrixType, Options>::secularEq(
+    RealScalar mu, const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm, const ArrayRef& diagShifted,
+    RealScalar shift) {
   Index m = perm.size();
   RealScalar res = Literal(1);
-  for(Index i=0; i<m; ++i)
-  {
+  for (Index i = 0; i < m; ++i) {
     Index j = perm(i);
     // The following expression could be rewritten to involve only a single division,
     // but this would make the expression more sensitive to overflow.
     res += (col0(j) / (diagShifted(j) - mu)) * (col0(j) / (diag(j) + shift + mu));
   }
   return res;
-
 }
 
-template <typename MatrixType>
-void BDCSVD<MatrixType>::computeSingVals(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef &perm,
-                                         VectorType& singVals, ArrayRef shifts, ArrayRef mus)
-{
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::computeSingVals(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm,
+                                                  VectorType& singVals, ArrayRef shifts, ArrayRef mus) {
   using std::abs;
-  using std::swap;
   using std::sqrt;
+  using std::swap;
 
   Index n = col0.size();
   Index actual_n = n;
   // Note that here actual_n is computed based on col0(i)==0 instead of diag(i)==0 as above
   // because 1) we have diag(i)==0 => col0(i)==0 and 2) if col0(i)==0, then diag(i) is already a singular value.
-  while(actual_n>1 && col0(actual_n-1)==Literal(0)) --actual_n;
+  while (actual_n > 1 && numext::is_exactly_zero(col0(actual_n - 1))) --actual_n;
 
-  for (Index k = 0; k < n; ++k)
-  {
-    if (col0(k) == Literal(0) || actual_n==1)
-    {
+  for (Index k = 0; k < n; ++k) {
+    if (numext::is_exactly_zero(col0(k)) || actual_n == 1) {
       // if col0(k) == 0, then entry is deflated, so singular value is on diagonal
       // if actual_n==1, then the deflated problem is already diagonalized
-      singVals(k) = k==0 ? col0(0) : diag(k);
+      singVals(k) = k == 0 ? col0(0) : diag(k);
       mus(k) = Literal(0);
-      shifts(k) = k==0 ? col0(0) : diag(k);
+      shifts(k) = k == 0 ? col0(0) : diag(k);
       continue;
-    } 
+    }
 
     // otherwise, use secular equation to find singular value
     RealScalar left = diag(k);
-    RealScalar right; // was: = (k != actual_n-1) ? diag(k+1) : (diag(actual_n-1) + col0.matrix().norm());
-    if(k==actual_n-1)
-      right = (diag(actual_n-1) + col0.matrix().norm());
-    else
-    {
+    RealScalar right;  // was: = (k != actual_n-1) ? diag(k+1) : (diag(actual_n-1) + col0.matrix().norm());
+    if (k == actual_n - 1)
+      right = (diag(actual_n - 1) + col0.matrix().norm());
+    else {
       // Skip deflated singular values,
       // recall that at this stage we assume that z[j]!=0 and all entries for which z[j]==0 have been put aside.
       // This should be equivalent to using perm[]
-      Index l = k+1;
-      while(col0(l)==Literal(0)) { ++l; eigen_internal_assert(l<actual_n); }
+      Index l = k + 1;
+      while (numext::is_exactly_zero(col0(l))) {
+        ++l;
+        eigen_internal_assert(l < actual_n);
+      }
       right = diag(l);
     }
 
     // first decide whether it's closer to the left end or the right end
-    RealScalar mid = left + (right-left) / Literal(2);
+    RealScalar mid = left + (right - left) / Literal(2);
     RealScalar fMid = secularEq(mid, col0, diag, perm, diag, Literal(0));
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-    std::cout << "right-left = " << right-left << "\n";
-//     std::cout << "fMid = " << fMid << " " << secularEq(mid-left, col0, diag, perm, ArrayXr(diag-left), left)
-//                            << " " << secularEq(mid-right, col0, diag, perm, ArrayXr(diag-right), right)   << "\n";
-    std::cout << "     = " << secularEq(left+RealScalar(0.000001)*(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.1)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.2)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.3)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.4)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.49)    *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.5)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.51)    *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.6)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.7)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.8)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.9)     *(right-left), col0, diag, perm, diag, 0)
-              << " "       << secularEq(left+RealScalar(0.999999)*(right-left), col0, diag, perm, diag, 0) << "\n";
+    std::cout << "right-left = " << right - left << "\n";
+    //     std::cout << "fMid = " << fMid << " " << secularEq(mid-left, col0, diag, perm, ArrayXr(diag-left), left)
+    //                            << " " << secularEq(mid-right, col0, diag, perm, ArrayXr(diag-right), right)   <<
+    //                            "\n";
+    std::cout << "     = " << secularEq(left + RealScalar(0.000001) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.1) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.2) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.3) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.4) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.49) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.5) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.51) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.6) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.7) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.8) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.9) * (right - left), col0, diag, perm, diag, 0) << " "
+              << secularEq(left + RealScalar(0.999999) * (right - left), col0, diag, perm, diag, 0) << "\n";
 #endif
-    RealScalar shift = (k == actual_n-1 || fMid > Literal(0)) ? left : right;
-    
+    RealScalar shift = (k == actual_n - 1 || fMid > Literal(0)) ? left : right;
+
     // measure everything relative to shift
-    Map<ArrayXr> diagShifted(m_workspace.data()+4*n, n);
+    Map<ArrayXr> diagShifted(m_workspace.data() + 4 * n, n);
     diagShifted = diag - shift;
 
-    if(k!=actual_n-1)
-    {
+    if (k != actual_n - 1) {
       // check that after the shift, f(mid) is still negative:
       RealScalar midShifted = (right - left) / RealScalar(2);
-      if(shift==right)
-        midShifted = -midShifted;
+      // we can test exact equality here, because shift comes from `... ? left : right`
+      if (numext::equal_strict(shift, right)) midShifted = -midShifted;
       RealScalar fMidShifted = secularEq(midShifted, col0, diag, perm, diagShifted, shift);
-      if(fMidShifted>0)
-      {
+      if (fMidShifted > 0) {
         // fMid was erroneous, fix it:
-        shift =  fMidShifted > Literal(0) ? left : right;
+        shift = fMidShifted > Literal(0) ? left : right;
         diagShifted = diag - shift;
       }
     }
-    
+
     // initial guess
     RealScalar muPrev, muCur;
-    if (shift == left)
-    {
+    // we can test exact equality here, because shift comes from `... ? left : right`
+    if (numext::equal_strict(shift, left)) {
       muPrev = (right - left) * RealScalar(0.1);
-      if (k == actual_n-1) muCur = right - left;
-      else                 muCur = (right - left) * RealScalar(0.5);
-    }
-    else
-    {
+      if (k == actual_n - 1)
+        muCur = right - left;
+      else
+        muCur = (right - left) * RealScalar(0.5);
+    } else {
       muPrev = -(right - left) * RealScalar(0.1);
       muCur = -(right - left) * RealScalar(0.5);
     }
 
     RealScalar fPrev = secularEq(muPrev, col0, diag, perm, diagShifted, shift);
     RealScalar fCur = secularEq(muCur, col0, diag, perm, diagShifted, shift);
-    if (abs(fPrev) < abs(fCur))
-    {
+    if (abs(fPrev) < abs(fCur)) {
       swap(fPrev, fCur);
       swap(muPrev, muCur);
     }
 
     // rational interpolation: fit a function of the form a / mu + b through the two previous
     // iterates and use its zero to compute the next iterate
-    bool useBisection = fPrev*fCur>Literal(0);
-    while (fCur!=Literal(0) && abs(muCur - muPrev) > Literal(8) * NumTraits<RealScalar>::epsilon() * numext::maxi<RealScalar>(abs(muCur), abs(muPrev)) && abs(fCur - fPrev)>NumTraits<RealScalar>::epsilon() && !useBisection)
-    {
+    bool useBisection = fPrev * fCur > Literal(0);
+    while (!numext::is_exactly_zero(fCur) &&
+           abs(muCur - muPrev) >
+               Literal(8) * NumTraits<RealScalar>::epsilon() * numext::maxi<RealScalar>(abs(muCur), abs(muPrev)) &&
+           abs(fCur - fPrev) > NumTraits<RealScalar>::epsilon() && !useBisection) {
       ++m_numIters;
 
       // Find a and b such that the function f(mu) = a / mu + b matches the current and previous samples.
-      RealScalar a = (fCur - fPrev) / (Literal(1)/muCur - Literal(1)/muPrev);
+      RealScalar a = (fCur - fPrev) / (Literal(1) / muCur - Literal(1) / muPrev);
       RealScalar b = fCur - a / muCur;
       // And find mu such that f(mu)==0:
-      RealScalar muZero = -a/b;
+      RealScalar muZero = -a / b;
       RealScalar fZero = secularEq(muZero, col0, diag, perm, diagShifted, shift);
 
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-      assert((numext::isfinite)(fZero));
+      eigen_internal_assert((numext::isfinite)(fZero));
 #endif
-      
+
       muPrev = muCur;
       fPrev = fCur;
       muCur = muZero;
       fCur = fZero;
-      
-      if (shift == left  && (muCur < Literal(0) || muCur > right - left)) useBisection = true;
-      if (shift == right && (muCur < -(right - left) || muCur > Literal(0))) useBisection = true;
-      if (abs(fCur)>abs(fPrev)) useBisection = true;
+
+      // we can test exact equality here, because shift comes from `... ? left : right`
+      if (numext::equal_strict(shift, left) && (muCur < Literal(0) || muCur > right - left)) useBisection = true;
+      if (numext::equal_strict(shift, right) && (muCur < -(right - left) || muCur > Literal(0))) useBisection = true;
+      if (abs(fCur) > abs(fPrev)) useBisection = true;
     }
 
     // fall back on bisection method if rational interpolation did not work
-    if (useBisection)
-    {
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
+    if (useBisection) {
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
       std::cout << "useBisection for k = " << k << ", actual_n = " << actual_n << "\n";
 #endif
       RealScalar leftShifted, rightShifted;
-      if (shift == left)
-      {
+      // we can test exact equality here, because shift comes from `... ? left : right`
+      if (numext::equal_strict(shift, left)) {
         // to avoid overflow, we must have mu > max(real_min, |z(k)|/sqrt(real_max)),
         // the factor 2 is to be more conservative
-        leftShifted = numext::maxi<RealScalar>( (std::numeric_limits<RealScalar>::min)(), Literal(2) * abs(col0(k)) / sqrt((std::numeric_limits<RealScalar>::max)()) );
+        leftShifted =
+            numext::maxi<RealScalar>((std::numeric_limits<RealScalar>::min)(),
+                                     Literal(2) * abs(col0(k)) / sqrt((std::numeric_limits<RealScalar>::max)()));
 
         // check that we did it right:
-        eigen_internal_assert( (numext::isfinite)( (col0(k)/leftShifted)*(col0(k)/(diag(k)+shift+leftShifted)) ) );
+        eigen_internal_assert(
+            (numext::isfinite)((col0(k) / leftShifted) * (col0(k) / (diag(k) + shift + leftShifted))));
         // I don't understand why the case k==0 would be special there:
         // if (k == 0) rightShifted = right - left; else
-        rightShifted = (k==actual_n-1) ? right : ((right - left) * RealScalar(0.51)); // theoretically we can take 0.5, but let's be safe
-      }
-      else
-      {
+        rightShifted = (k == actual_n - 1)
+                           ? right
+                           : ((right - left) * RealScalar(0.51));  // theoretically we can take 0.5, but let's be safe
+      } else {
         leftShifted = -(right - left) * RealScalar(0.51);
-        if(k+1<n)
-          rightShifted = -numext::maxi<RealScalar>( (std::numeric_limits<RealScalar>::min)(), abs(col0(k+1)) / sqrt((std::numeric_limits<RealScalar>::max)()) );
+        if (k + 1 < n)
+          rightShifted = -numext::maxi<RealScalar>((std::numeric_limits<RealScalar>::min)(),
+                                                   abs(col0(k + 1)) / sqrt((std::numeric_limits<RealScalar>::max)()));
         else
           rightShifted = -(std::numeric_limits<RealScalar>::min)();
       }
 
       RealScalar fLeft = secularEq(leftShifted, col0, diag, perm, diagShifted, shift);
-      eigen_internal_assert(fLeft<Literal(0));
+      eigen_internal_assert(fLeft < Literal(0));
 
-#if defined EIGEN_INTERNAL_DEBUGGING || defined EIGEN_BDCSVD_SANITY_CHECKS
+#if defined EIGEN_BDCSVD_DEBUG_VERBOSE || defined EIGEN_BDCSVD_SANITY_CHECKS || defined EIGEN_INTERNAL_DEBUGGING
       RealScalar fRight = secularEq(rightShifted, col0, diag, perm, diagShifted, shift);
 #endif
 
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-      if(!(numext::isfinite)(fLeft))
+      if (!(numext::isfinite)(fLeft))
         std::cout << "f(" << leftShifted << ") =" << fLeft << " ; " << left << " " << shift << " " << right << "\n";
-      assert((numext::isfinite)(fLeft));
+      eigen_internal_assert((numext::isfinite)(fLeft));
 
-      if(!(numext::isfinite)(fRight))
+      if (!(numext::isfinite)(fRight))
         std::cout << "f(" << rightShifted << ") =" << fRight << " ; " << left << " " << shift << " " << right << "\n";
-      // assert((numext::isfinite)(fRight));
+        // eigen_internal_assert((numext::isfinite)(fRight));
 #endif
-    
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
-      if(!(fLeft * fRight<0))
-      {
-        std::cout << "f(leftShifted) using  leftShifted=" << leftShifted << " ;  diagShifted(1:10):" << diagShifted.head(10).transpose()  << "\n ; "
-                  << "left==shift=" << bool(left==shift) << " ; left-shift = " << (left-shift) << "\n";
-        std::cout << "k=" << k << ", " <<  fLeft << " * " << fRight << " == " << fLeft * fRight << "  ;  "
-                  << "[" << left << " .. " << right << "] -> [" << leftShifted << " " << rightShifted << "], shift=" << shift
-                  << " ,  f(right)=" << secularEq(0,     col0, diag, perm, diagShifted, shift)
-                           << " == " << secularEq(right, col0, diag, perm, diag, 0) << " == " << fRight << "\n";
+
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
+      if (!(fLeft * fRight < 0)) {
+        std::cout << "f(leftShifted) using  leftShifted=" << leftShifted
+                  << " ;  diagShifted(1:10):" << diagShifted.head(10).transpose() << "\n ; "
+                  << "left==shift=" << bool(left == shift) << " ; left-shift = " << (left - shift) << "\n";
+        std::cout << "k=" << k << ", " << fLeft << " * " << fRight << " == " << fLeft * fRight << "  ;  "
+                  << "[" << left << " .. " << right << "] -> [" << leftShifted << " " << rightShifted
+                  << "], shift=" << shift << " ,  f(right)=" << secularEq(0, col0, diag, perm, diagShifted, shift)
+                  << " == " << secularEq(right, col0, diag, perm, diag, 0) << " == " << fRight << "\n";
       }
 #endif
       eigen_internal_assert(fLeft * fRight < Literal(0));
 
-      if(fLeft<Literal(0))
-      {
-        while (rightShifted - leftShifted > Literal(2) * NumTraits<RealScalar>::epsilon() * numext::maxi<RealScalar>(abs(leftShifted), abs(rightShifted)))
-        {
+      if (fLeft < Literal(0)) {
+        while (rightShifted - leftShifted > Literal(2) * NumTraits<RealScalar>::epsilon() *
+                                                numext::maxi<RealScalar>(abs(leftShifted), abs(rightShifted))) {
           RealScalar midShifted = (leftShifted + rightShifted) / Literal(2);
           fMid = secularEq(midShifted, col0, diag, perm, diagShifted, shift);
           eigen_internal_assert((numext::isfinite)(fMid));
 
-          if (fLeft * fMid < Literal(0))
-          {
+          if (fLeft * fMid < Literal(0)) {
             rightShifted = midShifted;
-          }
-          else
-          {
+          } else {
             leftShifted = midShifted;
             fLeft = fMid;
           }
         }
         muCur = (leftShifted + rightShifted) / Literal(2);
-      }
-      else 
-      {
+      } else {
         // We have a problem as shifting on the left or right give either a positive or negative value
         // at the middle of [left,right]...
         // Instead fo abbording or entering an infinite loop,
         // let's just use the middle as the estimated zero-crossing:
         muCur = (right - left) * RealScalar(0.5);
-        if(shift == right)
-          muCur = -muCur;
+        // we can test exact equality here, because shift comes from `... ? left : right`
+        if (numext::equal_strict(shift, right)) muCur = -muCur;
       }
     }
-      
+
     singVals[k] = shift + muCur;
     shifts[k] = shift;
     mus[k] = muCur;
 
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
-    if(k+1<n)
-      std::cout << "found " << singVals[k] << " == " << shift << " + " << muCur << " from " << diag(k) << " .. "  << diag(k+1) << "\n";
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
+    if (k + 1 < n)
+      std::cout << "found " << singVals[k] << " == " << shift << " + " << muCur << " from " << diag(k) << " .. "
+                << diag(k + 1) << "\n";
 #endif
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-    assert(k==0 || singVals[k]>=singVals[k-1]);
-    assert(singVals[k]>=diag(k));
+    eigen_internal_assert(k == 0 || singVals[k] >= singVals[k - 1]);
+    eigen_internal_assert(singVals[k] >= diag(k));
 #endif
 
     // perturb singular value slightly if it equals diagonal entry to avoid division by zero later
     // (deflation is supposed to avoid this from happening)
     // - this does no seem to be necessary anymore -
-//     if (singVals[k] == left) singVals[k] *= 1 + NumTraits<RealScalar>::epsilon();
-//     if (singVals[k] == right) singVals[k] *= 1 - NumTraits<RealScalar>::epsilon();
+    // if (singVals[k] == left) singVals[k] *= 1 + NumTraits<RealScalar>::epsilon();
+    // if (singVals[k] == right) singVals[k] *= 1 - NumTraits<RealScalar>::epsilon();
   }
 }
 
-
 // zhat is perturbation of col0 for which singular vectors can be computed stably (see Section 3.1)
-template <typename MatrixType>
-void BDCSVD<MatrixType>::perturbCol0
-   (const ArrayRef& col0, const ArrayRef& diag, const IndicesRef &perm, const VectorType& singVals,
-    const ArrayRef& shifts, const ArrayRef& mus, ArrayRef zhat)
-{
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::perturbCol0(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm,
+                                              const VectorType& singVals, const ArrayRef& shifts, const ArrayRef& mus,
+                                              ArrayRef zhat) {
   using std::sqrt;
   Index n = col0.size();
   Index m = perm.size();
-  if(m==0)
-  {
+  if (m == 0) {
     zhat.setZero();
     return;
   }
-  Index lastIdx = perm(m-1);
+  Index lastIdx = perm(m - 1);
   // The offset permits to skip deflated entries while computing zhat
-  for (Index k = 0; k < n; ++k)
-  {
-    if (col0(k) == Literal(0)) // deflated
+  for (Index k = 0; k < n; ++k) {
+    if (numext::is_exactly_zero(col0(k)))  // deflated
       zhat(k) = Literal(0);
-    else
-    {
+    else {
       // see equation (3.6)
       RealScalar dk = diag(k);
       RealScalar prod = (singVals(lastIdx) + dk) * (mus(lastIdx) + (shifts(lastIdx) - dk));
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-      if(prod<0) {
+      if (prod < 0) {
         std::cout << "k = " << k << " ;  z(k)=" << col0(k) << ", diag(k)=" << dk << "\n";
-        std::cout << "prod = " << "(" << singVals(lastIdx) << " + " << dk << ") * (" << mus(lastIdx) << " + (" << shifts(lastIdx) << " - " << dk << "))" << "\n";
-        std::cout << "     = " << singVals(lastIdx) + dk << " * " << mus(lastIdx) + (shifts(lastIdx) - dk) <<  "\n";
+        std::cout << "prod = "
+                  << "(" << singVals(lastIdx) << " + " << dk << ") * (" << mus(lastIdx) << " + (" << shifts(lastIdx)
+                  << " - " << dk << "))"
+                  << "\n";
+        std::cout << "     = " << singVals(lastIdx) + dk << " * " << mus(lastIdx) + (shifts(lastIdx) - dk) << "\n";
       }
-      assert(prod>=0);
+      eigen_internal_assert(prod >= 0);
 #endif
 
-      for(Index l = 0; l<m; ++l)
-      {
+      for (Index l = 0; l < m; ++l) {
         Index i = perm(l);
-        if(i!=k)
-        {
+        if (i != k) {
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-          if(i>=k && (l==0 || l-1>=m))
-          {
+          if (i >= k && (l == 0 || l - 1 >= m)) {
             std::cout << "Error in perturbCol0\n";
-            std::cout << "  " << k << "/" << n << " "  << l << "/" << m << " " << i << "/" << n << " ; " << col0(k) << " " << diag(k) << " "  <<  "\n";
-            std::cout << "  " <<diag(i) << "\n";
-            Index j = (i<k /*|| l==0*/) ? i : perm(l-1);
-            std::cout << "  " << "j=" << j << "\n";
+            std::cout << "  " << k << "/" << n << " " << l << "/" << m << " " << i << "/" << n << " ; " << col0(k)
+                      << " " << diag(k) << " "
+                      << "\n";
+            std::cout << "  " << diag(i) << "\n";
+            Index j = (i < k /*|| l==0*/) ? i : perm(l - 1);
+            std::cout << "  "
+                      << "j=" << j << "\n";
           }
 #endif
-          Index j = i<k ? i : perm(l-1);
+          // Avoid index out of bounds.
+          // Will end up setting zhat(k) = 0.
+          if (i >= k && l == 0) {
+            m_info = NumericalIssue;
+            prod = 0;
+            break;
+          }
+          Index j = i < k ? i : l > 0 ? perm(l - 1) : i;
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-          if(!(dk!=Literal(0) || diag(i)!=Literal(0)))
-          {
+          if (!(dk != Literal(0) || diag(i) != Literal(0))) {
             std::cout << "k=" << k << ", i=" << i << ", l=" << l << ", perm.size()=" << perm.size() << "\n";
           }
-          assert(dk!=Literal(0) || diag(i)!=Literal(0));
+          eigen_internal_assert(dk != Literal(0) || diag(i) != Literal(0));
 #endif
-          prod *= ((singVals(j)+dk) / ((diag(i)+dk))) * ((mus(j)+(shifts(j)-dk)) / ((diag(i)-dk)));
+          prod *= ((singVals(j) + dk) / ((diag(i) + dk))) * ((mus(j) + (shifts(j) - dk)) / ((diag(i) - dk)));
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-          assert(prod>=0);
+          eigen_internal_assert(prod >= 0);
 #endif
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-          if(i!=k && numext::abs(((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) - 1) > 0.9 )
-            std::cout << "     " << ((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) << " == (" << (singVals(j)+dk) << " * " << (mus(j)+(shifts(j)-dk))
-                       << ") / (" << (diag(i)+dk) << " * " << (diag(i)-dk) << ")\n";
+          if (i != k &&
+              numext::abs(((singVals(j) + dk) * (mus(j) + (shifts(j) - dk))) / ((diag(i) + dk) * (diag(i) - dk)) - 1) >
+                  0.9)
+            std::cout << "     "
+                      << ((singVals(j) + dk) * (mus(j) + (shifts(j) - dk))) / ((diag(i) + dk) * (diag(i) - dk))
+                      << " == (" << (singVals(j) + dk) << " * " << (mus(j) + (shifts(j) - dk)) << ") / ("
+                      << (diag(i) + dk) << " * " << (diag(i) - dk) << ")\n";
 #endif
         }
       }
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-      std::cout << "zhat(" << k << ") =  sqrt( " << prod << ")  ;  " << (singVals(lastIdx) + dk) << " * " << mus(lastIdx) + shifts(lastIdx) << " - " << dk << "\n";
+      std::cout << "zhat(" << k << ") =  sqrt( " << prod << ")  ;  " << (singVals(lastIdx) + dk) << " * "
+                << mus(lastIdx) + shifts(lastIdx) << " - " << dk << "\n";
 #endif
       RealScalar tmp = sqrt(prod);
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-      assert((numext::isfinite)(tmp));
+      eigen_internal_assert((numext::isfinite)(tmp));
 #endif
       zhat(k) = col0(k) > Literal(0) ? RealScalar(tmp) : RealScalar(-tmp);
     }
@@ -1061,254 +1169,246 @@
 }
 
 // compute singular vectors
-template <typename MatrixType>
-void BDCSVD<MatrixType>::computeSingVecs
-   (const ArrayRef& zhat, const ArrayRef& diag, const IndicesRef &perm, const VectorType& singVals,
-    const ArrayRef& shifts, const ArrayRef& mus, MatrixXr& U, MatrixXr& V)
-{
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::computeSingVecs(const ArrayRef& zhat, const ArrayRef& diag, const IndicesRef& perm,
+                                                  const VectorType& singVals, const ArrayRef& shifts,
+                                                  const ArrayRef& mus, MatrixXr& U, MatrixXr& V) {
   Index n = zhat.size();
   Index m = perm.size();
-  
-  for (Index k = 0; k < n; ++k)
-  {
-    if (zhat(k) == Literal(0))
-    {
-      U.col(k) = VectorType::Unit(n+1, k);
+
+  for (Index k = 0; k < n; ++k) {
+    if (numext::is_exactly_zero(zhat(k))) {
+      U.col(k) = VectorType::Unit(n + 1, k);
       if (m_compV) V.col(k) = VectorType::Unit(n, k);
-    }
-    else
-    {
+    } else {
       U.col(k).setZero();
-      for(Index l=0;l<m;++l)
-      {
+      for (Index l = 0; l < m; ++l) {
         Index i = perm(l);
-        U(i,k) = zhat(i)/(((diag(i) - shifts(k)) - mus(k)) )/( (diag(i) + singVals[k]));
+        U(i, k) = zhat(i) / (((diag(i) - shifts(k)) - mus(k))) / ((diag(i) + singVals[k]));
       }
-      U(n,k) = Literal(0);
+      U(n, k) = Literal(0);
       U.col(k).normalize();
-    
-      if (m_compV)
-      {
+
+      if (m_compV) {
         V.col(k).setZero();
-        for(Index l=1;l<m;++l)
-        {
+        for (Index l = 1; l < m; ++l) {
           Index i = perm(l);
-          V(i,k) = diag(i) * zhat(i) / (((diag(i) - shifts(k)) - mus(k)) )/( (diag(i) + singVals[k]));
+          V(i, k) = diag(i) * zhat(i) / (((diag(i) - shifts(k)) - mus(k))) / ((diag(i) + singVals[k]));
         }
-        V(0,k) = Literal(-1);
+        V(0, k) = Literal(-1);
         V.col(k).normalize();
       }
     }
   }
-  U.col(n) = VectorType::Unit(n+1, n);
+  U.col(n) = VectorType::Unit(n + 1, n);
 }
 
-
 // page 12_13
 // i >= 1, di almost null and zi non null.
 // We use a rotation to zero out zi applied to the left of M
-template <typename MatrixType>
-void BDCSVD<MatrixType>::deflation43(Eigen::Index firstCol, Eigen::Index shift, Eigen::Index i, Eigen::Index size)
-{
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::deflation43(Index firstCol, Index shift, Index i, Index size) {
   using std::abs;
-  using std::sqrt;
   using std::pow;
+  using std::sqrt;
   Index start = firstCol + shift;
   RealScalar c = m_computed(start, start);
-  RealScalar s = m_computed(start+i, start);
-  RealScalar r = numext::hypot(c,s);
-  if (r == Literal(0))
-  {
-    m_computed(start+i, start+i) = Literal(0);
+  RealScalar s = m_computed(start + i, start);
+  RealScalar r = numext::hypot(c, s);
+  if (numext::is_exactly_zero(r)) {
+    m_computed(start + i, start + i) = Literal(0);
     return;
   }
-  m_computed(start,start) = r;  
-  m_computed(start+i, start) = Literal(0);
-  m_computed(start+i, start+i) = Literal(0);
-  
-  JacobiRotation<RealScalar> J(c/r,-s/r);
-  if (m_compU)  m_naiveU.middleRows(firstCol, size+1).applyOnTheRight(firstCol, firstCol+i, J);
-  else          m_naiveU.applyOnTheRight(firstCol, firstCol+i, J);
-}// end deflation 43
+  m_computed(start, start) = r;
+  m_computed(start + i, start) = Literal(0);
+  m_computed(start + i, start + i) = Literal(0);
 
+  JacobiRotation<RealScalar> J(c / r, -s / r);
+  if (m_compU)
+    m_naiveU.middleRows(firstCol, size + 1).applyOnTheRight(firstCol, firstCol + i, J);
+  else
+    m_naiveU.applyOnTheRight(firstCol, firstCol + i, J);
+}  // end deflation 43
 
 // page 13
 // i,j >= 1, i!=j and |di - dj| < epsilon * norm2(M)
 // We apply two rotations to have zj = 0;
 // TODO deflation44 is still broken and not properly tested
-template <typename MatrixType>
-void BDCSVD<MatrixType>::deflation44(Eigen::Index firstColu , Eigen::Index firstColm, Eigen::Index firstRowW, Eigen::Index firstColW, Eigen::Index i, Eigen::Index j, Eigen::Index size)
-{
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::deflation44(Index firstColu, Index firstColm, Index firstRowW, Index firstColW,
+                                              Index i, Index j, Index size) {
   using std::abs;
-  using std::sqrt;
   using std::conj;
   using std::pow;
-  RealScalar c = m_computed(firstColm+i, firstColm);
-  RealScalar s = m_computed(firstColm+j, firstColm);
+  using std::sqrt;
+  RealScalar c = m_computed(firstColm + i, firstColm);
+  RealScalar s = m_computed(firstColm + j, firstColm);
   RealScalar r = sqrt(numext::abs2(c) + numext::abs2(s));
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
   std::cout << "deflation 4.4: " << i << "," << j << " -> " << c << " " << s << " " << r << " ; "
-    << m_computed(firstColm + i-1, firstColm)  << " "
-    << m_computed(firstColm + i, firstColm)  << " "
-    << m_computed(firstColm + i+1, firstColm) << " "
-    << m_computed(firstColm + i+2, firstColm) << "\n";
-  std::cout << m_computed(firstColm + i-1, firstColm + i-1)  << " "
-    << m_computed(firstColm + i, firstColm+i)  << " "
-    << m_computed(firstColm + i+1, firstColm+i+1) << " "
-    << m_computed(firstColm + i+2, firstColm+i+2) << "\n";
+            << m_computed(firstColm + i - 1, firstColm) << " " << m_computed(firstColm + i, firstColm) << " "
+            << m_computed(firstColm + i + 1, firstColm) << " " << m_computed(firstColm + i + 2, firstColm) << "\n";
+  std::cout << m_computed(firstColm + i - 1, firstColm + i - 1) << " " << m_computed(firstColm + i, firstColm + i)
+            << " " << m_computed(firstColm + i + 1, firstColm + i + 1) << " "
+            << m_computed(firstColm + i + 2, firstColm + i + 2) << "\n";
 #endif
-  if (r==Literal(0))
-  {
+  if (numext::is_exactly_zero(r)) {
     m_computed(firstColm + i, firstColm + i) = m_computed(firstColm + j, firstColm + j);
     return;
   }
-  c/=r;
-  s/=r;
+  c /= r;
+  s /= r;
   m_computed(firstColm + i, firstColm) = r;
   m_computed(firstColm + j, firstColm + j) = m_computed(firstColm + i, firstColm + i);
   m_computed(firstColm + j, firstColm) = Literal(0);
 
-  JacobiRotation<RealScalar> J(c,-s);
-  if (m_compU)  m_naiveU.middleRows(firstColu, size+1).applyOnTheRight(firstColu + i, firstColu + j, J);
-  else          m_naiveU.applyOnTheRight(firstColu+i, firstColu+j, J);
-  if (m_compV)  m_naiveV.middleRows(firstRowW, size).applyOnTheRight(firstColW + i, firstColW + j, J);
-}// end deflation 44
-
+  JacobiRotation<RealScalar> J(c, -s);
+  if (m_compU)
+    m_naiveU.middleRows(firstColu, size + 1).applyOnTheRight(firstColu + i, firstColu + j, J);
+  else
+    m_naiveU.applyOnTheRight(firstColu + i, firstColu + j, J);
+  if (m_compV) m_naiveV.middleRows(firstRowW, size).applyOnTheRight(firstColW + i, firstColW + j, J);
+}  // end deflation 44
 
 // acts on block from (firstCol+shift, firstCol+shift) to (lastCol+shift, lastCol+shift) [inclusive]
-template <typename MatrixType>
-void BDCSVD<MatrixType>::deflation(Eigen::Index firstCol, Eigen::Index lastCol, Eigen::Index k, Eigen::Index firstRowW, Eigen::Index firstColW, Eigen::Index shift)
-{
-  using std::sqrt;
+template <typename MatrixType, int Options>
+void BDCSVD<MatrixType, Options>::deflation(Index firstCol, Index lastCol, Index k, Index firstRowW, Index firstColW,
+                                            Index shift) {
   using std::abs;
+  using std::sqrt;
   const Index length = lastCol + 1 - firstCol;
-  
-  Block<MatrixXr,Dynamic,1> col0(m_computed, firstCol+shift, firstCol+shift, length, 1);
+
+  Block<MatrixXr, Dynamic, 1> col0(m_computed, firstCol + shift, firstCol + shift, length, 1);
   Diagonal<MatrixXr> fulldiag(m_computed);
-  VectorBlock<Diagonal<MatrixXr>,Dynamic> diag(fulldiag, firstCol+shift, length);
-  
+  VectorBlock<Diagonal<MatrixXr>, Dynamic> diag(fulldiag, firstCol + shift, length);
+
   const RealScalar considerZero = (std::numeric_limits<RealScalar>::min)();
-  RealScalar maxDiag = diag.tail((std::max)(Index(1),length-1)).cwiseAbs().maxCoeff();
-  RealScalar epsilon_strict = numext::maxi<RealScalar>(considerZero,NumTraits<RealScalar>::epsilon() * maxDiag);
-  RealScalar epsilon_coarse = Literal(8) * NumTraits<RealScalar>::epsilon() * numext::maxi<RealScalar>(col0.cwiseAbs().maxCoeff(), maxDiag);
-  
+  RealScalar maxDiag = diag.tail((std::max)(Index(1), length - 1)).cwiseAbs().maxCoeff();
+  RealScalar epsilon_strict = numext::maxi<RealScalar>(considerZero, NumTraits<RealScalar>::epsilon() * maxDiag);
+  RealScalar epsilon_coarse =
+      Literal(8) * NumTraits<RealScalar>::epsilon() * numext::maxi<RealScalar>(col0.cwiseAbs().maxCoeff(), maxDiag);
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(m_naiveU.allFinite());
-  assert(m_naiveV.allFinite());
-  assert(m_computed.allFinite());
+  eigen_internal_assert(m_naiveU.allFinite());
+  eigen_internal_assert(m_naiveV.allFinite());
+  eigen_internal_assert(m_computed.allFinite());
 #endif
 
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE  
-  std::cout << "\ndeflate:" << diag.head(k+1).transpose() << "  |  " << diag.segment(k+1,length-k-1).transpose() << "\n";
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
+  std::cout << "\ndeflate:" << diag.head(k + 1).transpose() << "  |  "
+            << diag.segment(k + 1, length - k - 1).transpose() << "\n";
 #endif
-  
-  //condition 4.1
-  if (diag(0) < epsilon_coarse)
-  { 
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
+
+  // condition 4.1
+  if (diag(0) < epsilon_coarse) {
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
     std::cout << "deflation 4.1, because " << diag(0) << " < " << epsilon_coarse << "\n";
 #endif
     diag(0) = epsilon_coarse;
   }
 
-  //condition 4.2
-  for (Index i=1;i<length;++i)
-    if (abs(col0(i)) < epsilon_strict)
-    {
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
-      std::cout << "deflation 4.2, set z(" << i << ") to zero because " << abs(col0(i)) << " < " << epsilon_strict << "  (diag(" << i << ")=" << diag(i) << ")\n";
+  // condition 4.2
+  for (Index i = 1; i < length; ++i)
+    if (abs(col0(i)) < epsilon_strict) {
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
+      std::cout << "deflation 4.2, set z(" << i << ") to zero because " << abs(col0(i)) << " < " << epsilon_strict
+                << "  (diag(" << i << ")=" << diag(i) << ")\n";
 #endif
       col0(i) = Literal(0);
     }
 
-  //condition 4.3
-  for (Index i=1;i<length; i++)
-    if (diag(i) < epsilon_coarse)
-    {
-#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
-      std::cout << "deflation 4.3, cancel z(" << i << ")=" << col0(i) << " because diag(" << i << ")=" << diag(i) << " < " << epsilon_coarse << "\n";
+  // condition 4.3
+  for (Index i = 1; i < length; i++)
+    if (diag(i) < epsilon_coarse) {
+#ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
+      std::cout << "deflation 4.3, cancel z(" << i << ")=" << col0(i) << " because diag(" << i << ")=" << diag(i)
+                << " < " << epsilon_coarse << "\n";
 #endif
       deflation43(firstCol, shift, i, length);
     }
 
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(m_naiveU.allFinite());
-  assert(m_naiveV.allFinite());
-  assert(m_computed.allFinite());
+  eigen_internal_assert(m_naiveU.allFinite());
+  eigen_internal_assert(m_naiveV.allFinite());
+  eigen_internal_assert(m_computed.allFinite());
 #endif
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
   std::cout << "to be sorted: " << diag.transpose() << "\n\n";
   std::cout << "            : " << col0.transpose() << "\n\n";
 #endif
   {
-    // Check for total deflation
-    // If we have a total deflation, then we have to consider col0(0)==diag(0) as a singular value during sorting
-    bool total_deflation = (col0.tail(length-1).array()<considerZero).all();
-    
+    // Check for total deflation:
+    // If we have a total deflation, then we have to consider col0(0)==diag(0) as a singular value during sorting.
+    const bool total_deflation = (col0.tail(length - 1).array().abs() < considerZero).all();
+
     // Sort the diagonal entries, since diag(1:k-1) and diag(k:length) are already sorted, let's do a sorted merge.
     // First, compute the respective permutation.
-    Index *permutation = m_workspaceI.data();
+    Index* permutation = m_workspaceI.data();
     {
       permutation[0] = 0;
       Index p = 1;
-      
+
       // Move deflated diagonal entries at the end.
-      for(Index i=1; i<length; ++i)
-        if(abs(diag(i))<considerZero)
-          permutation[p++] = i;
-        
-      Index i=1, j=k+1;
-      for( ; p < length; ++p)
-      {
-             if (i > k)             permutation[p] = j++;
-        else if (j >= length)       permutation[p] = i++;
-        else if (diag(i) < diag(j)) permutation[p] = j++;
-        else                        permutation[p] = i++;
+      for (Index i = 1; i < length; ++i)
+        if (abs(diag(i)) < considerZero) permutation[p++] = i;
+
+      Index i = 1, j = k + 1;
+      for (; p < length; ++p) {
+        if (i > k)
+          permutation[p] = j++;
+        else if (j >= length)
+          permutation[p] = i++;
+        else if (diag(i) < diag(j))
+          permutation[p] = j++;
+        else
+          permutation[p] = i++;
       }
     }
-    
+
     // If we have a total deflation, then we have to insert diag(0) at the right place
-    if(total_deflation)
-    {
-      for(Index i=1; i<length; ++i)
-      {
+    if (total_deflation) {
+      for (Index i = 1; i < length; ++i) {
         Index pi = permutation[i];
-        if(abs(diag(pi))<considerZero || diag(0)<diag(pi))
-          permutation[i-1] = permutation[i];
-        else
-        {
-          permutation[i-1] = 0;
+        if (abs(diag(pi)) < considerZero || diag(0) < diag(pi))
+          permutation[i - 1] = permutation[i];
+        else {
+          permutation[i - 1] = 0;
           break;
         }
       }
     }
-    
+
     // Current index of each col, and current column of each index
-    Index *realInd = m_workspaceI.data()+length;
-    Index *realCol = m_workspaceI.data()+2*length;
-    
-    for(int pos = 0; pos< length; pos++)
-    {
+    Index* realInd = m_workspaceI.data() + length;
+    Index* realCol = m_workspaceI.data() + 2 * length;
+
+    for (int pos = 0; pos < length; pos++) {
       realCol[pos] = pos;
       realInd[pos] = pos;
     }
-    
-    for(Index i = total_deflation?0:1; i < length; i++)
-    {
-      const Index pi = permutation[length - (total_deflation ? i+1 : i)];
+
+    for (Index i = total_deflation ? 0 : 1; i < length; i++) {
+      const Index pi = permutation[length - (total_deflation ? i + 1 : i)];
       const Index J = realCol[pi];
-      
+
       using std::swap;
       // swap diagonal and first column entries:
       swap(diag(i), diag(J));
-      if(i!=0 && J!=0) swap(col0(i), col0(J));
+      if (i != 0 && J != 0) swap(col0(i), col0(J));
 
       // change columns
-      if (m_compU) m_naiveU.col(firstCol+i).segment(firstCol, length + 1).swap(m_naiveU.col(firstCol+J).segment(firstCol, length + 1));
-      else         m_naiveU.col(firstCol+i).segment(0, 2)                .swap(m_naiveU.col(firstCol+J).segment(0, 2));
-      if (m_compV) m_naiveV.col(firstColW + i).segment(firstRowW, length).swap(m_naiveV.col(firstColW + J).segment(firstRowW, length));
+      if (m_compU)
+        m_naiveU.col(firstCol + i)
+            .segment(firstCol, length + 1)
+            .swap(m_naiveU.col(firstCol + J).segment(firstCol, length + 1));
+      else
+        m_naiveU.col(firstCol + i).segment(0, 2).swap(m_naiveU.col(firstCol + J).segment(0, 2));
+      if (m_compV)
+        m_naiveV.col(firstColW + i)
+            .segment(firstRowW, length)
+            .swap(m_naiveV.col(firstColW + J).segment(firstRowW, length));
 
-      //update real pos
+      // update real pos
       const Index realI = realInd[i];
       realCol[realI] = J;
       realCol[pi] = i;
@@ -1320,47 +1420,60 @@
   std::cout << "sorted: " << diag.transpose().format(bdcsvdfmt) << "\n";
   std::cout << "      : " << col0.transpose() << "\n\n";
 #endif
-    
-  //condition 4.4
+
+  // condition 4.4
   {
-    Index i = length-1;
-    while(i>0 && (abs(diag(i))<considerZero || abs(col0(i))<considerZero)) --i;
-    for(; i>1;--i)
-       if( (diag(i) - diag(i-1)) < NumTraits<RealScalar>::epsilon()*maxDiag )
-      {
+    Index i = length - 1;
+    while (i > 0 && (abs(diag(i)) < considerZero || abs(col0(i)) < considerZero)) --i;
+    for (; i > 1; --i)
+      if ((diag(i) - diag(i - 1)) < NumTraits<RealScalar>::epsilon() * maxDiag) {
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-        std::cout << "deflation 4.4 with i = " << i << " because " << diag(i) << " - " << diag(i-1) << " == " << (diag(i) - diag(i-1)) << " < " << NumTraits<RealScalar>::epsilon()*/*diag(i)*/maxDiag << "\n";
+        std::cout << "deflation 4.4 with i = " << i << " because " << diag(i) << " - " << diag(i - 1)
+                  << " == " << (diag(i) - diag(i - 1)) << " < "
+                  << NumTraits<RealScalar>::epsilon() * /*diag(i)*/ maxDiag << "\n";
 #endif
-        eigen_internal_assert(abs(diag(i) - diag(i-1))<epsilon_coarse && " diagonal entries are not properly sorted");
-        deflation44(firstCol, firstCol + shift, firstRowW, firstColW, i-1, i, length);
+        eigen_internal_assert(abs(diag(i) - diag(i - 1)) < epsilon_coarse &&
+                              " diagonal entries are not properly sorted");
+        deflation44(firstCol, firstCol + shift, firstRowW, firstColW, i - 1, i, length);
       }
   }
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  for(Index j=2;j<length;++j)
-    assert(diag(j-1)<=diag(j) || abs(diag(j))<considerZero);
+  for (Index j = 2; j < length; ++j) eigen_internal_assert(diag(j - 1) <= diag(j) || abs(diag(j)) < considerZero);
 #endif
-  
+
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(m_naiveU.allFinite());
-  assert(m_naiveV.allFinite());
-  assert(m_computed.allFinite());
+  eigen_internal_assert(m_naiveU.allFinite());
+  eigen_internal_assert(m_naiveV.allFinite());
+  eigen_internal_assert(m_computed.allFinite());
 #endif
-}//end deflation
+}  // end deflation
 
 /** \svd_module
-  *
-  * \return the singular value decomposition of \c *this computed by Divide & Conquer algorithm
-  *
-  * \sa class BDCSVD
-  */
-template<typename Derived>
-BDCSVD<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::bdcSvd(unsigned int computationOptions) const
-{
-  return BDCSVD<PlainObject>(*this, computationOptions);
+ *
+ * \return the singular value decomposition of \c *this computed by Divide & Conquer algorithm
+ *
+ * \sa class BDCSVD
+ */
+template <typename Derived>
+template <int Options>
+BDCSVD<typename MatrixBase<Derived>::PlainObject, Options> MatrixBase<Derived>::bdcSvd() const {
+  return BDCSVD<PlainObject, Options>(*this);
 }
 
-} // end namespace Eigen
+/** \svd_module
+ *
+ * \return the singular value decomposition of \c *this computed by Divide & Conquer algorithm
+ *
+ * \sa class BDCSVD
+ */
+template <typename Derived>
+template <int Options>
+BDCSVD<typename MatrixBase<Derived>::PlainObject, Options> MatrixBase<Derived>::bdcSvd(
+    unsigned int computationOptions) const {
+  return BDCSVD<PlainObject, Options>(*this, computationOptions);
+}
+
+}  // end namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/InternalHeaderCheck.h
new file mode 100644
index 0000000..fa67b96
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_SVD_MODULE_H
+#error "Please include Eigen/SVD instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h
index 9d95acd..aec1931 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h
@@ -11,13 +11,16 @@
 #ifndef EIGEN_JACOBISVD_H
 #define EIGEN_JACOBISVD_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
+
 // forward declaration (needed by ICC)
 // the empty body is required by MSVC
-template<typename MatrixType, int QRPreconditioner,
-         bool IsComplex = NumTraits<typename MatrixType::Scalar>::IsComplex>
+template <typename MatrixType, int Options, bool IsComplex = NumTraits<typename MatrixType::Scalar>::IsComplex>
 struct svd_precondition_2x2_block_to_be_real {};
 
 /*** QR preconditioners (R-SVD)
@@ -29,320 +32,331 @@
 
 enum { PreconditionIfMoreColsThanRows, PreconditionIfMoreRowsThanCols };
 
-template<typename MatrixType, int QRPreconditioner, int Case>
-struct qr_preconditioner_should_do_anything
-{
-  enum { a = MatrixType::RowsAtCompileTime != Dynamic &&
-             MatrixType::ColsAtCompileTime != Dynamic &&
-             MatrixType::ColsAtCompileTime <= MatrixType::RowsAtCompileTime,
-         b = MatrixType::RowsAtCompileTime != Dynamic &&
-             MatrixType::ColsAtCompileTime != Dynamic &&
-             MatrixType::RowsAtCompileTime <= MatrixType::ColsAtCompileTime,
-         ret = !( (QRPreconditioner == NoQRPreconditioner) ||
-                  (Case == PreconditionIfMoreColsThanRows && bool(a)) ||
-                  (Case == PreconditionIfMoreRowsThanCols && bool(b)) )
+template <typename MatrixType, int QRPreconditioner, int Case>
+struct qr_preconditioner_should_do_anything {
+  enum {
+    a = MatrixType::RowsAtCompileTime != Dynamic && MatrixType::ColsAtCompileTime != Dynamic &&
+        MatrixType::ColsAtCompileTime <= MatrixType::RowsAtCompileTime,
+    b = MatrixType::RowsAtCompileTime != Dynamic && MatrixType::ColsAtCompileTime != Dynamic &&
+        MatrixType::RowsAtCompileTime <= MatrixType::ColsAtCompileTime,
+    ret = !((QRPreconditioner == NoQRPreconditioner) || (Case == PreconditionIfMoreColsThanRows && bool(a)) ||
+            (Case == PreconditionIfMoreRowsThanCols && bool(b)))
   };
 };
 
-template<typename MatrixType, int QRPreconditioner, int Case,
-         bool DoAnything = qr_preconditioner_should_do_anything<MatrixType, QRPreconditioner, Case>::ret
-> struct qr_preconditioner_impl {};
+template <typename MatrixType, int Options, int QRPreconditioner, int Case,
+          bool DoAnything = qr_preconditioner_should_do_anything<MatrixType, QRPreconditioner, Case>::ret>
+struct qr_preconditioner_impl {};
 
-template<typename MatrixType, int QRPreconditioner, int Case>
-class qr_preconditioner_impl<MatrixType, QRPreconditioner, Case, false>
-{
-public:
-  void allocate(const JacobiSVD<MatrixType, QRPreconditioner>&) {}
-  bool run(JacobiSVD<MatrixType, QRPreconditioner>&, const MatrixType&)
-  {
-    return false;
-  }
+template <typename MatrixType, int Options, int QRPreconditioner, int Case>
+class qr_preconditioner_impl<MatrixType, Options, QRPreconditioner, Case, false> {
+ public:
+  void allocate(const JacobiSVD<MatrixType, Options>&) {}
+  bool run(JacobiSVD<MatrixType, Options>&, const MatrixType&) { return false; }
 };
 
 /*** preconditioner using FullPivHouseholderQR ***/
 
-template<typename MatrixType>
-class qr_preconditioner_impl<MatrixType, FullPivHouseholderQRPreconditioner, PreconditionIfMoreRowsThanCols, true>
-{
-public:
+template <typename MatrixType, int Options>
+class qr_preconditioner_impl<MatrixType, Options, FullPivHouseholderQRPreconditioner, PreconditionIfMoreRowsThanCols,
+                             true> {
+ public:
   typedef typename MatrixType::Scalar Scalar;
-  enum
-  {
-    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime
-  };
-  typedef Matrix<Scalar, 1, RowsAtCompileTime, RowMajor, 1, MaxRowsAtCompileTime> WorkspaceType;
+  typedef JacobiSVD<MatrixType, Options> SVDType;
 
-  void allocate(const JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner>& svd)
-  {
-    if (svd.rows() != m_qr.rows() || svd.cols() != m_qr.cols())
-    {
-      m_qr.~QRType();
-      ::new (&m_qr) QRType(svd.rows(), svd.cols());
+  enum { WorkspaceSize = MatrixType::RowsAtCompileTime, MaxWorkspaceSize = MatrixType::MaxRowsAtCompileTime };
+
+  typedef Matrix<Scalar, 1, WorkspaceSize, RowMajor, 1, MaxWorkspaceSize> WorkspaceType;
+
+  void allocate(const SVDType& svd) {
+    if (svd.rows() != m_qr.rows() || svd.cols() != m_qr.cols()) {
+      internal::destroy_at(&m_qr);
+      internal::construct_at(&m_qr, svd.rows(), svd.cols());
     }
     if (svd.m_computeFullU) m_workspace.resize(svd.rows());
   }
 
-  bool run(JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner>& svd, const MatrixType& matrix)
-  {
-    if(matrix.rows() > matrix.cols())
-    {
+  bool run(SVDType& svd, const MatrixType& matrix) {
+    if (matrix.rows() > matrix.cols()) {
       m_qr.compute(matrix);
-      svd.m_workMatrix = m_qr.matrixQR().block(0,0,matrix.cols(),matrix.cols()).template triangularView<Upper>();
-      if(svd.m_computeFullU) m_qr.matrixQ().evalTo(svd.m_matrixU, m_workspace);
-      if(svd.computeV()) svd.m_matrixV = m_qr.colsPermutation();
+      svd.m_workMatrix = m_qr.matrixQR().block(0, 0, matrix.cols(), matrix.cols()).template triangularView<Upper>();
+      if (svd.m_computeFullU) m_qr.matrixQ().evalTo(svd.m_matrixU, m_workspace);
+      if (svd.computeV()) svd.m_matrixV = m_qr.colsPermutation();
       return true;
     }
     return false;
   }
-private:
+
+ private:
   typedef FullPivHouseholderQR<MatrixType> QRType;
   QRType m_qr;
   WorkspaceType m_workspace;
 };
 
-template<typename MatrixType>
-class qr_preconditioner_impl<MatrixType, FullPivHouseholderQRPreconditioner, PreconditionIfMoreColsThanRows, true>
-{
-public:
+template <typename MatrixType, int Options>
+class qr_preconditioner_impl<MatrixType, Options, FullPivHouseholderQRPreconditioner, PreconditionIfMoreColsThanRows,
+                             true> {
+ public:
   typedef typename MatrixType::Scalar Scalar;
-  enum
-  {
+  typedef JacobiSVD<MatrixType, Options> SVDType;
+
+  enum {
     RowsAtCompileTime = MatrixType::RowsAtCompileTime,
     ColsAtCompileTime = MatrixType::ColsAtCompileTime,
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    Options = MatrixType::Options
+    MatrixOptions = MatrixType::Options
   };
 
-  typedef typename internal::make_proper_matrix_type<
-    Scalar, ColsAtCompileTime, RowsAtCompileTime, Options, MaxColsAtCompileTime, MaxRowsAtCompileTime
-  >::type TransposeTypeWithSameStorageOrder;
+  typedef typename internal::make_proper_matrix_type<Scalar, ColsAtCompileTime, RowsAtCompileTime, MatrixOptions,
+                                                     MaxColsAtCompileTime, MaxRowsAtCompileTime>::type
+      TransposeTypeWithSameStorageOrder;
 
-  void allocate(const JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner>& svd)
-  {
-    if (svd.cols() != m_qr.rows() || svd.rows() != m_qr.cols())
-    {
-      m_qr.~QRType();
-      ::new (&m_qr) QRType(svd.cols(), svd.rows());
+  void allocate(const SVDType& svd) {
+    if (svd.cols() != m_qr.rows() || svd.rows() != m_qr.cols()) {
+      internal::destroy_at(&m_qr);
+      internal::construct_at(&m_qr, svd.cols(), svd.rows());
     }
     m_adjoint.resize(svd.cols(), svd.rows());
     if (svd.m_computeFullV) m_workspace.resize(svd.cols());
   }
 
-  bool run(JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner>& svd, const MatrixType& matrix)
-  {
-    if(matrix.cols() > matrix.rows())
-    {
+  bool run(SVDType& svd, const MatrixType& matrix) {
+    if (matrix.cols() > matrix.rows()) {
       m_adjoint = matrix.adjoint();
       m_qr.compute(m_adjoint);
-      svd.m_workMatrix = m_qr.matrixQR().block(0,0,matrix.rows(),matrix.rows()).template triangularView<Upper>().adjoint();
-      if(svd.m_computeFullV) m_qr.matrixQ().evalTo(svd.m_matrixV, m_workspace);
-      if(svd.computeU()) svd.m_matrixU = m_qr.colsPermutation();
+      svd.m_workMatrix =
+          m_qr.matrixQR().block(0, 0, matrix.rows(), matrix.rows()).template triangularView<Upper>().adjoint();
+      if (svd.m_computeFullV) m_qr.matrixQ().evalTo(svd.m_matrixV, m_workspace);
+      if (svd.computeU()) svd.m_matrixU = m_qr.colsPermutation();
       return true;
-    }
-    else return false;
+    } else
+      return false;
   }
-private:
+
+ private:
   typedef FullPivHouseholderQR<TransposeTypeWithSameStorageOrder> QRType;
   QRType m_qr;
   TransposeTypeWithSameStorageOrder m_adjoint;
-  typename internal::plain_row_type<MatrixType>::type m_workspace;
+  typename plain_row_type<MatrixType>::type m_workspace;
 };
 
 /*** preconditioner using ColPivHouseholderQR ***/
 
-template<typename MatrixType>
-class qr_preconditioner_impl<MatrixType, ColPivHouseholderQRPreconditioner, PreconditionIfMoreRowsThanCols, true>
-{
-public:
-  void allocate(const JacobiSVD<MatrixType, ColPivHouseholderQRPreconditioner>& svd)
-  {
-    if (svd.rows() != m_qr.rows() || svd.cols() != m_qr.cols())
-    {
-      m_qr.~QRType();
-      ::new (&m_qr) QRType(svd.rows(), svd.cols());
+template <typename MatrixType, int Options>
+class qr_preconditioner_impl<MatrixType, Options, ColPivHouseholderQRPreconditioner, PreconditionIfMoreRowsThanCols,
+                             true> {
+ public:
+  typedef typename MatrixType::Scalar Scalar;
+  typedef JacobiSVD<MatrixType, Options> SVDType;
+
+  enum {
+    WorkspaceSize = internal::traits<SVDType>::MatrixUColsAtCompileTime,
+    MaxWorkspaceSize = internal::traits<SVDType>::MatrixUMaxColsAtCompileTime
+  };
+
+  typedef Matrix<Scalar, 1, WorkspaceSize, RowMajor, 1, MaxWorkspaceSize> WorkspaceType;
+
+  void allocate(const SVDType& svd) {
+    if (svd.rows() != m_qr.rows() || svd.cols() != m_qr.cols()) {
+      internal::destroy_at(&m_qr);
+      internal::construct_at(&m_qr, svd.rows(), svd.cols());
     }
-    if (svd.m_computeFullU) m_workspace.resize(svd.rows());
-    else if (svd.m_computeThinU) m_workspace.resize(svd.cols());
+    if (svd.m_computeFullU)
+      m_workspace.resize(svd.rows());
+    else if (svd.m_computeThinU)
+      m_workspace.resize(svd.cols());
   }
 
-  bool run(JacobiSVD<MatrixType, ColPivHouseholderQRPreconditioner>& svd, const MatrixType& matrix)
-  {
-    if(matrix.rows() > matrix.cols())
-    {
+  bool run(SVDType& svd, const MatrixType& matrix) {
+    if (matrix.rows() > matrix.cols()) {
       m_qr.compute(matrix);
-      svd.m_workMatrix = m_qr.matrixQR().block(0,0,matrix.cols(),matrix.cols()).template triangularView<Upper>();
-      if(svd.m_computeFullU) m_qr.householderQ().evalTo(svd.m_matrixU, m_workspace);
-      else if(svd.m_computeThinU)
-      {
+      svd.m_workMatrix = m_qr.matrixQR().block(0, 0, matrix.cols(), matrix.cols()).template triangularView<Upper>();
+      if (svd.m_computeFullU)
+        m_qr.householderQ().evalTo(svd.m_matrixU, m_workspace);
+      else if (svd.m_computeThinU) {
         svd.m_matrixU.setIdentity(matrix.rows(), matrix.cols());
         m_qr.householderQ().applyThisOnTheLeft(svd.m_matrixU, m_workspace);
       }
-      if(svd.computeV()) svd.m_matrixV = m_qr.colsPermutation();
+      if (svd.computeV()) svd.m_matrixV = m_qr.colsPermutation();
       return true;
     }
     return false;
   }
 
-private:
+ private:
   typedef ColPivHouseholderQR<MatrixType> QRType;
   QRType m_qr;
-  typename internal::plain_col_type<MatrixType>::type m_workspace;
+  WorkspaceType m_workspace;
 };
 
-template<typename MatrixType>
-class qr_preconditioner_impl<MatrixType, ColPivHouseholderQRPreconditioner, PreconditionIfMoreColsThanRows, true>
-{
-public:
+template <typename MatrixType, int Options>
+class qr_preconditioner_impl<MatrixType, Options, ColPivHouseholderQRPreconditioner, PreconditionIfMoreColsThanRows,
+                             true> {
+ public:
   typedef typename MatrixType::Scalar Scalar;
-  enum
-  {
+  typedef JacobiSVD<MatrixType, Options> SVDType;
+
+  enum {
     RowsAtCompileTime = MatrixType::RowsAtCompileTime,
     ColsAtCompileTime = MatrixType::ColsAtCompileTime,
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    Options = MatrixType::Options
+    MatrixOptions = MatrixType::Options,
+    WorkspaceSize = internal::traits<SVDType>::MatrixVColsAtCompileTime,
+    MaxWorkspaceSize = internal::traits<SVDType>::MatrixVMaxColsAtCompileTime
   };
 
-  typedef typename internal::make_proper_matrix_type<
-    Scalar, ColsAtCompileTime, RowsAtCompileTime, Options, MaxColsAtCompileTime, MaxRowsAtCompileTime
-  >::type TransposeTypeWithSameStorageOrder;
+  typedef Matrix<Scalar, WorkspaceSize, 1, ColMajor, MaxWorkspaceSize, 1> WorkspaceType;
 
-  void allocate(const JacobiSVD<MatrixType, ColPivHouseholderQRPreconditioner>& svd)
-  {
-    if (svd.cols() != m_qr.rows() || svd.rows() != m_qr.cols())
-    {
-      m_qr.~QRType();
-      ::new (&m_qr) QRType(svd.cols(), svd.rows());
+  typedef typename internal::make_proper_matrix_type<Scalar, ColsAtCompileTime, RowsAtCompileTime, MatrixOptions,
+                                                     MaxColsAtCompileTime, MaxRowsAtCompileTime>::type
+      TransposeTypeWithSameStorageOrder;
+
+  void allocate(const SVDType& svd) {
+    if (svd.cols() != m_qr.rows() || svd.rows() != m_qr.cols()) {
+      internal::destroy_at(&m_qr);
+      internal::construct_at(&m_qr, svd.cols(), svd.rows());
     }
-    if (svd.m_computeFullV) m_workspace.resize(svd.cols());
-    else if (svd.m_computeThinV) m_workspace.resize(svd.rows());
+    if (svd.m_computeFullV)
+      m_workspace.resize(svd.cols());
+    else if (svd.m_computeThinV)
+      m_workspace.resize(svd.rows());
     m_adjoint.resize(svd.cols(), svd.rows());
   }
 
-  bool run(JacobiSVD<MatrixType, ColPivHouseholderQRPreconditioner>& svd, const MatrixType& matrix)
-  {
-    if(matrix.cols() > matrix.rows())
-    {
+  bool run(SVDType& svd, const MatrixType& matrix) {
+    if (matrix.cols() > matrix.rows()) {
       m_adjoint = matrix.adjoint();
       m_qr.compute(m_adjoint);
 
-      svd.m_workMatrix = m_qr.matrixQR().block(0,0,matrix.rows(),matrix.rows()).template triangularView<Upper>().adjoint();
-      if(svd.m_computeFullV) m_qr.householderQ().evalTo(svd.m_matrixV, m_workspace);
-      else if(svd.m_computeThinV)
-      {
+      svd.m_workMatrix =
+          m_qr.matrixQR().block(0, 0, matrix.rows(), matrix.rows()).template triangularView<Upper>().adjoint();
+      if (svd.m_computeFullV)
+        m_qr.householderQ().evalTo(svd.m_matrixV, m_workspace);
+      else if (svd.m_computeThinV) {
         svd.m_matrixV.setIdentity(matrix.cols(), matrix.rows());
         m_qr.householderQ().applyThisOnTheLeft(svd.m_matrixV, m_workspace);
       }
-      if(svd.computeU()) svd.m_matrixU = m_qr.colsPermutation();
+      if (svd.computeU()) svd.m_matrixU = m_qr.colsPermutation();
       return true;
-    }
-    else return false;
+    } else
+      return false;
   }
 
-private:
+ private:
   typedef ColPivHouseholderQR<TransposeTypeWithSameStorageOrder> QRType;
   QRType m_qr;
   TransposeTypeWithSameStorageOrder m_adjoint;
-  typename internal::plain_row_type<MatrixType>::type m_workspace;
+  WorkspaceType m_workspace;
 };
 
 /*** preconditioner using HouseholderQR ***/
 
-template<typename MatrixType>
-class qr_preconditioner_impl<MatrixType, HouseholderQRPreconditioner, PreconditionIfMoreRowsThanCols, true>
-{
-public:
-  void allocate(const JacobiSVD<MatrixType, HouseholderQRPreconditioner>& svd)
-  {
-    if (svd.rows() != m_qr.rows() || svd.cols() != m_qr.cols())
-    {
-      m_qr.~QRType();
-      ::new (&m_qr) QRType(svd.rows(), svd.cols());
+template <typename MatrixType, int Options>
+class qr_preconditioner_impl<MatrixType, Options, HouseholderQRPreconditioner, PreconditionIfMoreRowsThanCols, true> {
+ public:
+  typedef typename MatrixType::Scalar Scalar;
+  typedef JacobiSVD<MatrixType, Options> SVDType;
+
+  enum {
+    WorkspaceSize = internal::traits<SVDType>::MatrixUColsAtCompileTime,
+    MaxWorkspaceSize = internal::traits<SVDType>::MatrixUMaxColsAtCompileTime
+  };
+
+  typedef Matrix<Scalar, 1, WorkspaceSize, RowMajor, 1, MaxWorkspaceSize> WorkspaceType;
+
+  void allocate(const SVDType& svd) {
+    if (svd.rows() != m_qr.rows() || svd.cols() != m_qr.cols()) {
+      internal::destroy_at(&m_qr);
+      internal::construct_at(&m_qr, svd.rows(), svd.cols());
     }
-    if (svd.m_computeFullU) m_workspace.resize(svd.rows());
-    else if (svd.m_computeThinU) m_workspace.resize(svd.cols());
+    if (svd.m_computeFullU)
+      m_workspace.resize(svd.rows());
+    else if (svd.m_computeThinU)
+      m_workspace.resize(svd.cols());
   }
 
-  bool run(JacobiSVD<MatrixType, HouseholderQRPreconditioner>& svd, const MatrixType& matrix)
-  {
-    if(matrix.rows() > matrix.cols())
-    {
+  bool run(SVDType& svd, const MatrixType& matrix) {
+    if (matrix.rows() > matrix.cols()) {
       m_qr.compute(matrix);
-      svd.m_workMatrix = m_qr.matrixQR().block(0,0,matrix.cols(),matrix.cols()).template triangularView<Upper>();
-      if(svd.m_computeFullU) m_qr.householderQ().evalTo(svd.m_matrixU, m_workspace);
-      else if(svd.m_computeThinU)
-      {
+      svd.m_workMatrix = m_qr.matrixQR().block(0, 0, matrix.cols(), matrix.cols()).template triangularView<Upper>();
+      if (svd.m_computeFullU)
+        m_qr.householderQ().evalTo(svd.m_matrixU, m_workspace);
+      else if (svd.m_computeThinU) {
         svd.m_matrixU.setIdentity(matrix.rows(), matrix.cols());
         m_qr.householderQ().applyThisOnTheLeft(svd.m_matrixU, m_workspace);
       }
-      if(svd.computeV()) svd.m_matrixV.setIdentity(matrix.cols(), matrix.cols());
+      if (svd.computeV()) svd.m_matrixV.setIdentity(matrix.cols(), matrix.cols());
       return true;
     }
     return false;
   }
-private:
+
+ private:
   typedef HouseholderQR<MatrixType> QRType;
   QRType m_qr;
-  typename internal::plain_col_type<MatrixType>::type m_workspace;
+  WorkspaceType m_workspace;
 };
 
-template<typename MatrixType>
-class qr_preconditioner_impl<MatrixType, HouseholderQRPreconditioner, PreconditionIfMoreColsThanRows, true>
-{
-public:
+template <typename MatrixType, int Options>
+class qr_preconditioner_impl<MatrixType, Options, HouseholderQRPreconditioner, PreconditionIfMoreColsThanRows, true> {
+ public:
   typedef typename MatrixType::Scalar Scalar;
-  enum
-  {
+  typedef JacobiSVD<MatrixType, Options> SVDType;
+
+  enum {
     RowsAtCompileTime = MatrixType::RowsAtCompileTime,
     ColsAtCompileTime = MatrixType::ColsAtCompileTime,
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    Options = MatrixType::Options
+    MatrixOptions = MatrixType::Options,
+    WorkspaceSize = internal::traits<SVDType>::MatrixVColsAtCompileTime,
+    MaxWorkspaceSize = internal::traits<SVDType>::MatrixVMaxColsAtCompileTime
   };
 
-  typedef typename internal::make_proper_matrix_type<
-    Scalar, ColsAtCompileTime, RowsAtCompileTime, Options, MaxColsAtCompileTime, MaxRowsAtCompileTime
-  >::type TransposeTypeWithSameStorageOrder;
+  typedef Matrix<Scalar, WorkspaceSize, 1, ColMajor, MaxWorkspaceSize, 1> WorkspaceType;
 
-  void allocate(const JacobiSVD<MatrixType, HouseholderQRPreconditioner>& svd)
-  {
-    if (svd.cols() != m_qr.rows() || svd.rows() != m_qr.cols())
-    {
-      m_qr.~QRType();
-      ::new (&m_qr) QRType(svd.cols(), svd.rows());
+  typedef typename internal::make_proper_matrix_type<Scalar, ColsAtCompileTime, RowsAtCompileTime, MatrixOptions,
+                                                     MaxColsAtCompileTime, MaxRowsAtCompileTime>::type
+      TransposeTypeWithSameStorageOrder;
+
+  void allocate(const SVDType& svd) {
+    if (svd.cols() != m_qr.rows() || svd.rows() != m_qr.cols()) {
+      internal::destroy_at(&m_qr);
+      internal::construct_at(&m_qr, svd.cols(), svd.rows());
     }
-    if (svd.m_computeFullV) m_workspace.resize(svd.cols());
-    else if (svd.m_computeThinV) m_workspace.resize(svd.rows());
+    if (svd.m_computeFullV)
+      m_workspace.resize(svd.cols());
+    else if (svd.m_computeThinV)
+      m_workspace.resize(svd.rows());
     m_adjoint.resize(svd.cols(), svd.rows());
   }
 
-  bool run(JacobiSVD<MatrixType, HouseholderQRPreconditioner>& svd, const MatrixType& matrix)
-  {
-    if(matrix.cols() > matrix.rows())
-    {
+  bool run(SVDType& svd, const MatrixType& matrix) {
+    if (matrix.cols() > matrix.rows()) {
       m_adjoint = matrix.adjoint();
       m_qr.compute(m_adjoint);
 
-      svd.m_workMatrix = m_qr.matrixQR().block(0,0,matrix.rows(),matrix.rows()).template triangularView<Upper>().adjoint();
-      if(svd.m_computeFullV) m_qr.householderQ().evalTo(svd.m_matrixV, m_workspace);
-      else if(svd.m_computeThinV)
-      {
+      svd.m_workMatrix =
+          m_qr.matrixQR().block(0, 0, matrix.rows(), matrix.rows()).template triangularView<Upper>().adjoint();
+      if (svd.m_computeFullV)
+        m_qr.householderQ().evalTo(svd.m_matrixV, m_workspace);
+      else if (svd.m_computeThinV) {
         svd.m_matrixV.setIdentity(matrix.cols(), matrix.rows());
         m_qr.householderQ().applyThisOnTheLeft(svd.m_matrixV, m_workspace);
       }
-      if(svd.computeU()) svd.m_matrixU.setIdentity(matrix.rows(), matrix.rows());
+      if (svd.computeU()) svd.m_matrixU.setIdentity(matrix.rows(), matrix.rows());
       return true;
-    }
-    else return false;
+    } else
+      return false;
   }
 
-private:
+ private:
   typedef HouseholderQR<TransposeTypeWithSameStorageOrder> QRType;
   QRType m_qr;
   TransposeTypeWithSameStorageOrder m_adjoint;
-  typename internal::plain_row_type<MatrixType>::type m_workspace;
+  WorkspaceType m_workspace;
 };
 
 /*** 2x2 SVD implementation
@@ -350,326 +364,323 @@
  *** JacobiSVD consists in performing a series of 2x2 SVD subproblems
  ***/
 
-template<typename MatrixType, int QRPreconditioner>
-struct svd_precondition_2x2_block_to_be_real<MatrixType, QRPreconditioner, false>
-{
-  typedef JacobiSVD<MatrixType, QRPreconditioner> SVD;
+template <typename MatrixType, int Options>
+struct svd_precondition_2x2_block_to_be_real<MatrixType, Options, false> {
+  typedef JacobiSVD<MatrixType, Options> SVD;
   typedef typename MatrixType::RealScalar RealScalar;
   static bool run(typename SVD::WorkMatrixType&, SVD&, Index, Index, RealScalar&) { return true; }
 };
 
-template<typename MatrixType, int QRPreconditioner>
-struct svd_precondition_2x2_block_to_be_real<MatrixType, QRPreconditioner, true>
-{
-  typedef JacobiSVD<MatrixType, QRPreconditioner> SVD;
+template <typename MatrixType, int Options>
+struct svd_precondition_2x2_block_to_be_real<MatrixType, Options, true> {
+  typedef JacobiSVD<MatrixType, Options> SVD;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
-  static bool run(typename SVD::WorkMatrixType& work_matrix, SVD& svd, Index p, Index q, RealScalar& maxDiagEntry)
-  {
-    using std::sqrt;
+  static bool run(typename SVD::WorkMatrixType& work_matrix, SVD& svd, Index p, Index q, RealScalar& maxDiagEntry) {
     using std::abs;
+    using std::sqrt;
     Scalar z;
     JacobiRotation<Scalar> rot;
-    RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p,p)) + numext::abs2(work_matrix.coeff(q,p)));
+    RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p, p)) + numext::abs2(work_matrix.coeff(q, p)));
 
     const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
     const RealScalar precision = NumTraits<Scalar>::epsilon();
 
-    if(n==0)
-    {
+    if (numext::is_exactly_zero(n)) {
       // make sure first column is zero
-      work_matrix.coeffRef(p,p) = work_matrix.coeffRef(q,p) = Scalar(0);
+      work_matrix.coeffRef(p, p) = work_matrix.coeffRef(q, p) = Scalar(0);
 
-      if(abs(numext::imag(work_matrix.coeff(p,q)))>considerAsZero)
-      {
-        // work_matrix.coeff(p,q) can be zero if work_matrix.coeff(q,p) is not zero but small enough to underflow when computing n
-        z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q);
+      if (abs(numext::imag(work_matrix.coeff(p, q))) > considerAsZero) {
+        // work_matrix.coeff(p,q) can be zero if work_matrix.coeff(q,p) is not zero but small enough to underflow when
+        // computing n
+        z = abs(work_matrix.coeff(p, q)) / work_matrix.coeff(p, q);
         work_matrix.row(p) *= z;
-        if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z);
+        if (svd.computeU()) svd.m_matrixU.col(p) *= conj(z);
       }
-      if(abs(numext::imag(work_matrix.coeff(q,q)))>considerAsZero)
-      {
-        z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
+      if (abs(numext::imag(work_matrix.coeff(q, q))) > considerAsZero) {
+        z = abs(work_matrix.coeff(q, q)) / work_matrix.coeff(q, q);
         work_matrix.row(q) *= z;
-        if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
+        if (svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
       }
       // otherwise the second row is already zero, so we have nothing to do.
-    }
-    else
-    {
-      rot.c() = conj(work_matrix.coeff(p,p)) / n;
-      rot.s() = work_matrix.coeff(q,p) / n;
-      work_matrix.applyOnTheLeft(p,q,rot);
-      if(svd.computeU()) svd.m_matrixU.applyOnTheRight(p,q,rot.adjoint());
-      if(abs(numext::imag(work_matrix.coeff(p,q)))>considerAsZero)
-      {
-        z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q);
+    } else {
+      rot.c() = conj(work_matrix.coeff(p, p)) / n;
+      rot.s() = work_matrix.coeff(q, p) / n;
+      work_matrix.applyOnTheLeft(p, q, rot);
+      if (svd.computeU()) svd.m_matrixU.applyOnTheRight(p, q, rot.adjoint());
+      if (abs(numext::imag(work_matrix.coeff(p, q))) > considerAsZero) {
+        z = abs(work_matrix.coeff(p, q)) / work_matrix.coeff(p, q);
         work_matrix.col(q) *= z;
-        if(svd.computeV()) svd.m_matrixV.col(q) *= z;
+        if (svd.computeV()) svd.m_matrixV.col(q) *= z;
       }
-      if(abs(numext::imag(work_matrix.coeff(q,q)))>considerAsZero)
-      {
-        z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
+      if (abs(numext::imag(work_matrix.coeff(q, q))) > considerAsZero) {
+        z = abs(work_matrix.coeff(q, q)) / work_matrix.coeff(q, q);
         work_matrix.row(q) *= z;
-        if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
+        if (svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
       }
     }
 
     // update largest diagonal entry
-    maxDiagEntry = numext::maxi<RealScalar>(maxDiagEntry,numext::maxi<RealScalar>(abs(work_matrix.coeff(p,p)), abs(work_matrix.coeff(q,q))));
+    maxDiagEntry = numext::maxi<RealScalar>(
+        maxDiagEntry, numext::maxi<RealScalar>(abs(work_matrix.coeff(p, p)), abs(work_matrix.coeff(q, q))));
     // and check whether the 2x2 block is already diagonal
     RealScalar threshold = numext::maxi<RealScalar>(considerAsZero, precision * maxDiagEntry);
-    return abs(work_matrix.coeff(p,q))>threshold || abs(work_matrix.coeff(q,p)) > threshold;
+    return abs(work_matrix.coeff(p, q)) > threshold || abs(work_matrix.coeff(q, p)) > threshold;
   }
 };
 
-template<typename _MatrixType, int QRPreconditioner> 
-struct traits<JacobiSVD<_MatrixType,QRPreconditioner> >
-        : traits<_MatrixType>
-{
-  typedef _MatrixType MatrixType;
+template <typename MatrixType_, int Options>
+struct traits<JacobiSVD<MatrixType_, Options> > : svd_traits<MatrixType_, Options> {
+  typedef MatrixType_ MatrixType;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup SVD_Module
-  *
-  *
-  * \class JacobiSVD
-  *
-  * \brief Two-sided Jacobi SVD decomposition of a rectangular matrix
-  *
-  * \tparam _MatrixType the type of the matrix of which we are computing the SVD decomposition
-  * \tparam QRPreconditioner this optional parameter allows to specify the type of QR decomposition that will be used internally
-  *                        for the R-SVD step for non-square matrices. See discussion of possible values below.
-  *
-  * SVD decomposition consists in decomposing any n-by-p matrix \a A as a product
-  *   \f[ A = U S V^* \f]
-  * where \a U is a n-by-n unitary, \a V is a p-by-p unitary, and \a S is a n-by-p real positive matrix which is zero outside of its main diagonal;
-  * the diagonal entries of S are known as the \em singular \em values of \a A and the columns of \a U and \a V are known as the left
-  * and right \em singular \em vectors of \a A respectively.
-  *
-  * Singular values are always sorted in decreasing order.
-  *
-  * This JacobiSVD decomposition computes only the singular values by default. If you want \a U or \a V, you need to ask for them explicitly.
-  *
-  * You can ask for only \em thin \a U or \a V to be computed, meaning the following. In case of a rectangular n-by-p matrix, letting \a m be the
-  * smaller value among \a n and \a p, there are only \a m singular vectors; the remaining columns of \a U and \a V do not correspond to actual
-  * singular vectors. Asking for \em thin \a U or \a V means asking for only their \a m first columns to be formed. So \a U is then a n-by-m matrix,
-  * and \a V is then a p-by-m matrix. Notice that thin \a U and \a V are all you need for (least squares) solving.
-  *
-  * Here's an example demonstrating basic usage:
-  * \include JacobiSVD_basic.cpp
-  * Output: \verbinclude JacobiSVD_basic.out
-  *
-  * This JacobiSVD class is a two-sided Jacobi R-SVD decomposition, ensuring optimal reliability and accuracy. The downside is that it's slower than
-  * bidiagonalizing SVD algorithms for large square matrices; however its complexity is still \f$ O(n^2p) \f$ where \a n is the smaller dimension and
-  * \a p is the greater dimension, meaning that it is still of the same order of complexity as the faster bidiagonalizing R-SVD algorithms.
-  * In particular, like any R-SVD, it takes advantage of non-squareness in that its complexity is only linear in the greater dimension.
-  *
-  * If the input matrix has inf or nan coefficients, the result of the computation is undefined, but the computation is guaranteed to
-  * terminate in finite (and reasonable) time.
-  *
-  * The possible values for QRPreconditioner are:
-  * \li ColPivHouseholderQRPreconditioner is the default. In practice it's very safe. It uses column-pivoting QR.
-  * \li FullPivHouseholderQRPreconditioner, is the safest and slowest. It uses full-pivoting QR.
-  *     Contrary to other QRs, it doesn't allow computing thin unitaries.
-  * \li HouseholderQRPreconditioner is the fastest, and less safe and accurate than the pivoting variants. It uses non-pivoting QR.
-  *     This is very similar in safety and accuracy to the bidiagonalization process used by bidiagonalizing SVD algorithms (since bidiagonalization
-  *     is inherently non-pivoting). However the resulting SVD is still more reliable than bidiagonalizing SVDs because the Jacobi-based iterarive
-  *     process is more reliable than the optimized bidiagonal SVD iterations.
-  * \li NoQRPreconditioner allows not to use a QR preconditioner at all. This is useful if you know that you will only be computing
-  *     JacobiSVD decompositions of square matrices. Non-square matrices require a QR preconditioner. Using this option will result in
-  *     faster compilation and smaller executable code. It won't significantly speed up computation, since JacobiSVD is always checking
-  *     if QR preconditioning is needed before applying it anyway.
-  *
-  * \sa MatrixBase::jacobiSvd()
-  */
-template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
- : public SVDBase<JacobiSVD<_MatrixType,QRPreconditioner> >
-{
-    typedef SVDBase<JacobiSVD> Base;
-  public:
+ *
+ *
+ * \class JacobiSVD
+ *
+ * \brief Two-sided Jacobi SVD decomposition of a rectangular matrix
+ *
+ * \tparam MatrixType_ the type of the matrix of which we are computing the SVD decomposition
+ * \tparam Options this optional parameter allows one to specify the type of QR decomposition that will be used
+ * internally for the R-SVD step for non-square matrices. Additionally, it allows one to specify whether to compute thin
+ * or full unitaries \a U and \a V. See discussion of possible values below.
+ *
+ * SVD decomposition consists in decomposing any n-by-p matrix \a A as a product
+ *   \f[ A = U S V^* \f]
+ * where \a U is a n-by-n unitary, \a V is a p-by-p unitary, and \a S is a n-by-p real positive matrix which is zero
+ * outside of its main diagonal; the diagonal entries of S are known as the \em singular \em values of \a A and the
+ * columns of \a U and \a V are known as the left and right \em singular \em vectors of \a A respectively.
+ *
+ * Singular values are always sorted in decreasing order.
+ *
+ * This JacobiSVD decomposition computes only the singular values by default. If you want \a U or \a V, you need to ask
+ * for them explicitly.
+ *
+ * You can ask for only \em thin \a U or \a V to be computed, meaning the following. In case of a rectangular n-by-p
+ * matrix, letting \a m be the smaller value among \a n and \a p, there are only \a m singular vectors; the remaining
+ * columns of \a U and \a V do not correspond to actual singular vectors. Asking for \em thin \a U or \a V means asking
+ * for only their \a m first columns to be formed. So \a U is then a n-by-m matrix, and \a V is then a p-by-m matrix.
+ * Notice that thin \a U and \a V are all you need for (least squares) solving.
+ *
+ * Here's an example demonstrating basic usage:
+ * \include JacobiSVD_basic.cpp
+ * Output: \verbinclude JacobiSVD_basic.out
+ *
+ * This JacobiSVD class is a two-sided Jacobi R-SVD decomposition, ensuring optimal reliability and accuracy. The
+ * downside is that it's slower than bidiagonalizing SVD algorithms for large square matrices; however its complexity is
+ * still \f$ O(n^2p) \f$ where \a n is the smaller dimension and \a p is the greater dimension, meaning that it is still
+ * of the same order of complexity as the faster bidiagonalizing R-SVD algorithms. In particular, like any R-SVD, it
+ * takes advantage of non-squareness in that its complexity is only linear in the greater dimension.
+ *
+ * If the input matrix has inf or nan coefficients, the result of the computation is undefined, but the computation is
+ * guaranteed to terminate in finite (and reasonable) time.
+ *
+ * The possible QR preconditioners that can be set with Options template parameter are:
+ * \li ColPivHouseholderQRPreconditioner is the default. In practice it's very safe. It uses column-pivoting QR.
+ * \li FullPivHouseholderQRPreconditioner, is the safest and slowest. It uses full-pivoting QR.
+ *     Contrary to other QRs, it doesn't allow computing thin unitaries.
+ * \li HouseholderQRPreconditioner is the fastest, and less safe and accurate than the pivoting variants. It uses
+ * non-pivoting QR. This is very similar in safety and accuracy to the bidiagonalization process used by bidiagonalizing
+ * SVD algorithms (since bidiagonalization is inherently non-pivoting). However the resulting SVD is still more reliable
+ * than bidiagonalizing SVDs because the Jacobi-based iterarive process is more reliable than the optimized bidiagonal
+ * SVD iterations. \li NoQRPreconditioner allows not to use a QR preconditioner at all. This is useful if you know that
+ * you will only be computing JacobiSVD decompositions of square matrices. Non-square matrices require a QR
+ * preconditioner. Using this option will result in faster compilation and smaller executable code. It won't
+ * significantly speed up computation, since JacobiSVD is always checking if QR preconditioning is needed before
+ * applying it anyway.
+ *
+ * One may also use the Options template parameter to specify how the unitaries should be computed. The options are
+ * #ComputeThinU, #ComputeThinV, #ComputeFullU, #ComputeFullV. It is not possible to request both the thin and full
+ * versions of a unitary. By default, unitaries will not be computed.
+ *
+ * You can set the QRPreconditioner and unitary options together: JacobiSVD<MatrixType,
+ * ColPivHouseholderQRPreconditioner | ComputeThinU | ComputeFullV>
+ *
+ * \sa MatrixBase::jacobiSvd()
+ */
+template <typename MatrixType_, int Options_>
+class JacobiSVD : public SVDBase<JacobiSVD<MatrixType_, Options_> > {
+  typedef SVDBase<JacobiSVD> Base;
 
-    typedef _MatrixType MatrixType;
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      DiagSizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime,ColsAtCompileTime),
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-      MaxDiagSizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_FIXED(MaxRowsAtCompileTime,MaxColsAtCompileTime),
-      MatrixOptions = MatrixType::Options
-    };
+ public:
+  typedef MatrixType_ MatrixType;
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::RealScalar RealScalar;
+  typedef typename Base::Index Index;
+  enum : int {
+    Options = Options_,
+    QRPreconditioner = internal::get_qr_preconditioner(Options),
+    RowsAtCompileTime = Base::RowsAtCompileTime,
+    ColsAtCompileTime = Base::ColsAtCompileTime,
+    DiagSizeAtCompileTime = Base::DiagSizeAtCompileTime,
+    MaxRowsAtCompileTime = Base::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = Base::MaxColsAtCompileTime,
+    MaxDiagSizeAtCompileTime = Base::MaxDiagSizeAtCompileTime,
+    MatrixOptions = Base::MatrixOptions
+  };
 
-    typedef typename Base::MatrixUType MatrixUType;
-    typedef typename Base::MatrixVType MatrixVType;
-    typedef typename Base::SingularValuesType SingularValuesType;
-    
-    typedef typename internal::plain_row_type<MatrixType>::type RowType;
-    typedef typename internal::plain_col_type<MatrixType>::type ColType;
-    typedef Matrix<Scalar, DiagSizeAtCompileTime, DiagSizeAtCompileTime,
-                   MatrixOptions, MaxDiagSizeAtCompileTime, MaxDiagSizeAtCompileTime>
-            WorkMatrixType;
+  typedef typename Base::MatrixUType MatrixUType;
+  typedef typename Base::MatrixVType MatrixVType;
+  typedef typename Base::SingularValuesType SingularValuesType;
+  typedef Matrix<Scalar, DiagSizeAtCompileTime, DiagSizeAtCompileTime, MatrixOptions, MaxDiagSizeAtCompileTime,
+                 MaxDiagSizeAtCompileTime>
+      WorkMatrixType;
 
-    /** \brief Default Constructor.
-      *
-      * The default constructor is useful in cases in which the user intends to
-      * perform decompositions via JacobiSVD::compute(const MatrixType&).
-      */
-    JacobiSVD()
-    {}
+  /** \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via JacobiSVD::compute(const MatrixType&).
+   */
+  JacobiSVD() {}
 
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem size and \a Options template parameter.
+   *
+   * \sa JacobiSVD()
+   */
+  JacobiSVD(Index rows, Index cols) { allocate(rows, cols, internal::get_computation_options(Options)); }
 
-    /** \brief Default Constructor with memory preallocation
-      *
-      * Like the default constructor but with preallocation of the internal data
-      * according to the specified problem size.
-      * \sa JacobiSVD()
-      */
-    JacobiSVD(Index rows, Index cols, unsigned int computationOptions = 0)
-    {
-      allocate(rows, cols, computationOptions);
-    }
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem size.
+   *
+   * One \b cannot request unitaries using both the \a Options template parameter
+   * and the constructor. If possible, prefer using the \a Options template parameter.
+   *
+   * \param computationOptions specify whether to compute Thin/Full unitaries U/V
+   * \sa JacobiSVD()
+   *
+   * \deprecated Will be removed in the next major Eigen version. Options should
+   * be specified in the \a Options template parameter.
+   */
+  EIGEN_DEPRECATED JacobiSVD(Index rows, Index cols, unsigned int computationOptions) {
+    internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, rows, cols);
+    allocate(rows, cols, computationOptions);
+  }
 
-    /** \brief Constructor performing the decomposition of given matrix.
-     *
-     * \param matrix the matrix to decompose
-     * \param computationOptions optional parameter allowing to specify if you want full or thin U or V unitaries to be computed.
-     *                           By default, none is computed. This is a bit-field, the possible bits are #ComputeFullU, #ComputeThinU,
-     *                           #ComputeFullV, #ComputeThinV.
-     *
-     * Thin unitaries are only available if your matrix type has a Dynamic number of columns (for example MatrixXf). They also are not
-     * available with the (non-default) FullPivHouseholderQR preconditioner.
-     */
-    explicit JacobiSVD(const MatrixType& matrix, unsigned int computationOptions = 0)
-    {
-      compute(matrix, computationOptions);
-    }
+  /** \brief Constructor performing the decomposition of given matrix, using the custom options specified
+   *         with the \a Options template paramter.
+   *
+   * \param matrix the matrix to decompose
+   */
+  explicit JacobiSVD(const MatrixType& matrix) { compute_impl(matrix, internal::get_computation_options(Options)); }
 
-    /** \brief Method performing the decomposition of given matrix using custom options.
-     *
-     * \param matrix the matrix to decompose
-     * \param computationOptions optional parameter allowing to specify if you want full or thin U or V unitaries to be computed.
-     *                           By default, none is computed. This is a bit-field, the possible bits are #ComputeFullU, #ComputeThinU,
-     *                           #ComputeFullV, #ComputeThinV.
-     *
-     * Thin unitaries are only available if your matrix type has a Dynamic number of columns (for example MatrixXf). They also are not
-     * available with the (non-default) FullPivHouseholderQR preconditioner.
-     */
-    JacobiSVD& compute(const MatrixType& matrix, unsigned int computationOptions);
+  /** \brief Constructor performing the decomposition of given matrix using specified options
+   *         for computing unitaries.
+   *
+   *  One \b cannot request unitiaries using both the \a Options template parameter
+   *  and the constructor. If possible, prefer using the \a Options template parameter.
+   *
+   * \param matrix the matrix to decompose
+   * \param computationOptions specify whether to compute Thin/Full unitaries U/V
+   *
+   * \deprecated Will be removed in the next major Eigen version. Options should
+   * be specified in the \a Options template parameter.
+   */
+  // EIGEN_DEPRECATED // TODO(cantonios): re-enable after fixing a few 3p libraries that error on deprecation warnings.
+  JacobiSVD(const MatrixType& matrix, unsigned int computationOptions) {
+    internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, matrix.rows(), matrix.cols());
+    compute_impl(matrix, computationOptions);
+  }
 
-    /** \brief Method performing the decomposition of given matrix using current options.
-     *
-     * \param matrix the matrix to decompose
-     *
-     * This method uses the current \a computationOptions, as already passed to the constructor or to compute(const MatrixType&, unsigned int).
-     */
-    JacobiSVD& compute(const MatrixType& matrix)
-    {
-      return compute(matrix, m_computationOptions);
-    }
+  /** \brief Method performing the decomposition of given matrix. Computes Thin/Full unitaries U/V if specified
+   *         using the \a Options template parameter or the class constructor.
+   *
+   * \param matrix the matrix to decompose
+   */
+  JacobiSVD& compute(const MatrixType& matrix) { return compute_impl(matrix, m_computationOptions); }
 
-    using Base::computeU;
-    using Base::computeV;
-    using Base::rows;
-    using Base::cols;
-    using Base::rank;
+  /** \brief Method performing the decomposition of given matrix, as specified by
+   *         the `computationOptions` parameter.
+   *
+   * \param matrix the matrix to decompose
+   * \param computationOptions specify whether to compute Thin/Full unitaries U/V
+   *
+   * \deprecated Will be removed in the next major Eigen version. Options should
+   * be specified in the \a Options template parameter.
+   */
+  EIGEN_DEPRECATED JacobiSVD& compute(const MatrixType& matrix, unsigned int computationOptions) {
+    internal::check_svd_options_assertions<MatrixType, Options>(m_computationOptions, matrix.rows(), matrix.cols());
+    return compute_impl(matrix, computationOptions);
+  }
 
-  private:
-    void allocate(Index rows, Index cols, unsigned int computationOptions);
+  using Base::cols;
+  using Base::computeU;
+  using Base::computeV;
+  using Base::diagSize;
+  using Base::rank;
+  using Base::rows;
 
-  protected:
-    using Base::m_matrixU;
-    using Base::m_matrixV;
-    using Base::m_singularValues;
-    using Base::m_info;
-    using Base::m_isInitialized;
-    using Base::m_isAllocated;
-    using Base::m_usePrescribedThreshold;
-    using Base::m_computeFullU;
-    using Base::m_computeThinU;
-    using Base::m_computeFullV;
-    using Base::m_computeThinV;
-    using Base::m_computationOptions;
-    using Base::m_nonzeroSingularValues;
-    using Base::m_rows;
-    using Base::m_cols;
-    using Base::m_diagSize;
-    using Base::m_prescribedThreshold;
-    WorkMatrixType m_workMatrix;
+ private:
+  void allocate(Index rows, Index cols, unsigned int computationOptions);
+  JacobiSVD& compute_impl(const MatrixType& matrix, unsigned int computationOptions);
 
-    template<typename __MatrixType, int _QRPreconditioner, bool _IsComplex>
-    friend struct internal::svd_precondition_2x2_block_to_be_real;
-    template<typename __MatrixType, int _QRPreconditioner, int _Case, bool _DoAnything>
-    friend struct internal::qr_preconditioner_impl;
+ protected:
+  using Base::m_computationOptions;
+  using Base::m_computeFullU;
+  using Base::m_computeFullV;
+  using Base::m_computeThinU;
+  using Base::m_computeThinV;
+  using Base::m_info;
+  using Base::m_isAllocated;
+  using Base::m_isInitialized;
+  using Base::m_matrixU;
+  using Base::m_matrixV;
+  using Base::m_nonzeroSingularValues;
+  using Base::m_prescribedThreshold;
+  using Base::m_singularValues;
+  using Base::m_usePrescribedThreshold;
+  using Base::ShouldComputeThinU;
+  using Base::ShouldComputeThinV;
 
-    internal::qr_preconditioner_impl<MatrixType, QRPreconditioner, internal::PreconditionIfMoreColsThanRows> m_qr_precond_morecols;
-    internal::qr_preconditioner_impl<MatrixType, QRPreconditioner, internal::PreconditionIfMoreRowsThanCols> m_qr_precond_morerows;
-    MatrixType m_scaledMatrix;
+  EIGEN_STATIC_ASSERT(!(ShouldComputeThinU && int(QRPreconditioner) == int(FullPivHouseholderQRPreconditioner)) &&
+                          !(ShouldComputeThinU && int(QRPreconditioner) == int(FullPivHouseholderQRPreconditioner)),
+                      "JacobiSVD: can't compute thin U or thin V with the FullPivHouseholderQR preconditioner. "
+                      "Use the ColPivHouseholderQR preconditioner instead.")
+
+  template <typename MatrixType__, int Options__, bool IsComplex_>
+  friend struct internal::svd_precondition_2x2_block_to_be_real;
+  template <typename MatrixType__, int Options__, int QRPreconditioner_, int Case_, bool DoAnything_>
+  friend struct internal::qr_preconditioner_impl;
+
+  internal::qr_preconditioner_impl<MatrixType, Options, QRPreconditioner, internal::PreconditionIfMoreColsThanRows>
+      m_qr_precond_morecols;
+  internal::qr_preconditioner_impl<MatrixType, Options, QRPreconditioner, internal::PreconditionIfMoreRowsThanCols>
+      m_qr_precond_morerows;
+  WorkMatrixType m_workMatrix;
+  MatrixType m_scaledMatrix;
 };
 
-template<typename MatrixType, int QRPreconditioner>
-void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Eigen::Index rows, Eigen::Index cols, unsigned int computationOptions)
-{
-  eigen_assert(rows >= 0 && cols >= 0);
+template <typename MatrixType, int Options>
+void JacobiSVD<MatrixType, Options>::allocate(Index rows_, Index cols_, unsigned int computationOptions_) {
+  if (Base::allocate(rows_, cols_, computationOptions_)) return;
 
-  if (m_isAllocated &&
-      rows == m_rows &&
-      cols == m_cols &&
-      computationOptions == m_computationOptions)
-  {
-    return;
-  }
+  eigen_assert(!(ShouldComputeThinU && int(QRPreconditioner) == int(FullPivHouseholderQRPreconditioner)) &&
+               !(ShouldComputeThinU && int(QRPreconditioner) == int(FullPivHouseholderQRPreconditioner)) &&
+               "JacobiSVD: can't compute thin U or thin V with the FullPivHouseholderQR preconditioner. "
+               "Use the ColPivHouseholderQR preconditioner instead.");
 
-  m_rows = rows;
-  m_cols = cols;
-  m_info = Success;
-  m_isInitialized = false;
-  m_isAllocated = true;
-  m_computationOptions = computationOptions;
-  m_computeFullU = (computationOptions & ComputeFullU) != 0;
-  m_computeThinU = (computationOptions & ComputeThinU) != 0;
-  m_computeFullV = (computationOptions & ComputeFullV) != 0;
-  m_computeThinV = (computationOptions & ComputeThinV) != 0;
-  eigen_assert(!(m_computeFullU && m_computeThinU) && "JacobiSVD: you can't ask for both full and thin U");
-  eigen_assert(!(m_computeFullV && m_computeThinV) && "JacobiSVD: you can't ask for both full and thin V");
-  eigen_assert(EIGEN_IMPLIES(m_computeThinU || m_computeThinV, MatrixType::ColsAtCompileTime==Dynamic) &&
-              "JacobiSVD: thin U and V are only available when your matrix has a dynamic number of columns.");
-  if (QRPreconditioner == FullPivHouseholderQRPreconditioner)
-  {
-      eigen_assert(!(m_computeThinU || m_computeThinV) &&
-              "JacobiSVD: can't compute thin U or thin V with the FullPivHouseholderQR preconditioner. "
-              "Use the ColPivHouseholderQR preconditioner instead.");
-  }
-  m_diagSize = (std::min)(m_rows, m_cols);
-  m_singularValues.resize(m_diagSize);
-  if(RowsAtCompileTime==Dynamic)
-    m_matrixU.resize(m_rows, m_computeFullU ? m_rows
-                            : m_computeThinU ? m_diagSize
-                            : 0);
-  if(ColsAtCompileTime==Dynamic)
-    m_matrixV.resize(m_cols, m_computeFullV ? m_cols
-                            : m_computeThinV ? m_diagSize
-                            : 0);
-  m_workMatrix.resize(m_diagSize, m_diagSize);
-  
-  if(m_cols>m_rows)   m_qr_precond_morecols.allocate(*this);
-  if(m_rows>m_cols)   m_qr_precond_morerows.allocate(*this);
-  if(m_rows!=m_cols)  m_scaledMatrix.resize(rows,cols);
+  m_workMatrix.resize(diagSize(), diagSize());
+  if (cols() > rows()) m_qr_precond_morecols.allocate(*this);
+  if (rows() > cols()) m_qr_precond_morerows.allocate(*this);
+  if (rows() != cols()) m_scaledMatrix.resize(rows(), cols());
 }
 
-template<typename MatrixType, int QRPreconditioner>
-JacobiSVD<MatrixType, QRPreconditioner>&
-JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsigned int computationOptions)
-{
+template <typename MatrixType, int Options>
+JacobiSVD<MatrixType, Options>& JacobiSVD<MatrixType, Options>::compute_impl(const MatrixType& matrix,
+                                                                             unsigned int computationOptions) {
   using std::abs;
+
   allocate(matrix.rows(), matrix.cols(), computationOptions);
 
-  // currently we stop when we reach precision 2*epsilon as the last bit of precision can require an unreasonable number of iterations,
-  // only worsening the precision of U and V as we accumulate more rotations
+  // currently we stop when we reach precision 2*epsilon as the last bit of precision can require an unreasonable number
+  // of iterations, only worsening the precision of U and V as we accumulate more rotations
   const RealScalar precision = RealScalar(2) * NumTraits<Scalar>::epsilon();
 
   // limit for denormal numbers to be considered zero in order to avoid infinite loops (see bug 286)
@@ -680,112 +691,102 @@
   if (!(numext::isfinite)(scale)) {
     m_isInitialized = true;
     m_info = InvalidInput;
+    m_nonzeroSingularValues = 0;
     return *this;
   }
-  if(scale==RealScalar(0)) scale = RealScalar(1);
-  
+  if (numext::is_exactly_zero(scale)) scale = RealScalar(1);
+
   /*** step 1. The R-SVD step: we use a QR decomposition to reduce to the case of a square matrix */
 
-  if(m_rows!=m_cols)
-  {
+  if (rows() != cols()) {
     m_scaledMatrix = matrix / scale;
     m_qr_precond_morecols.run(*this, m_scaledMatrix);
     m_qr_precond_morerows.run(*this, m_scaledMatrix);
-  }
-  else
-  {
-    m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize) / scale;
-    if(m_computeFullU) m_matrixU.setIdentity(m_rows,m_rows);
-    if(m_computeThinU) m_matrixU.setIdentity(m_rows,m_diagSize);
-    if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols);
-    if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize);
+  } else {
+    m_workMatrix =
+        matrix.template topLeftCorner<DiagSizeAtCompileTime, DiagSizeAtCompileTime>(diagSize(), diagSize()) / scale;
+    if (m_computeFullU) m_matrixU.setIdentity(rows(), rows());
+    if (m_computeThinU) m_matrixU.setIdentity(rows(), diagSize());
+    if (m_computeFullV) m_matrixV.setIdentity(cols(), cols());
+    if (m_computeThinV) m_matrixV.setIdentity(cols(), diagSize());
   }
 
   /*** step 2. The main Jacobi SVD iteration. ***/
   RealScalar maxDiagEntry = m_workMatrix.cwiseAbs().diagonal().maxCoeff();
 
   bool finished = false;
-  while(!finished)
-  {
+  while (!finished) {
     finished = true;
 
     // do a sweep: for all index pairs (p,q), perform SVD of the corresponding 2x2 sub-matrix
 
-    for(Index p = 1; p < m_diagSize; ++p)
-    {
-      for(Index q = 0; q < p; ++q)
-      {
+    for (Index p = 1; p < diagSize(); ++p) {
+      for (Index q = 0; q < p; ++q) {
         // if this 2x2 sub-matrix is not diagonal already...
         // notice that this comparison will evaluate to false if any NaN is involved, ensuring that NaN's don't
         // keep us iterating forever. Similarly, small denormal numbers are considered zero.
         RealScalar threshold = numext::maxi<RealScalar>(considerAsZero, precision * maxDiagEntry);
-        if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold)
-        {
+        if (abs(m_workMatrix.coeff(p, q)) > threshold || abs(m_workMatrix.coeff(q, p)) > threshold) {
           finished = false;
           // perform SVD decomposition of 2x2 sub-matrix corresponding to indices p,q to make it diagonal
           // the complex to real operation returns true if the updated 2x2 block is not already diagonal
-          if(internal::svd_precondition_2x2_block_to_be_real<MatrixType, QRPreconditioner>::run(m_workMatrix, *this, p, q, maxDiagEntry))
-          {
+          if (internal::svd_precondition_2x2_block_to_be_real<MatrixType, Options>::run(m_workMatrix, *this, p, q,
+                                                                                        maxDiagEntry)) {
             JacobiRotation<RealScalar> j_left, j_right;
             internal::real_2x2_jacobi_svd(m_workMatrix, p, q, &j_left, &j_right);
 
             // accumulate resulting Jacobi rotations
-            m_workMatrix.applyOnTheLeft(p,q,j_left);
-            if(computeU()) m_matrixU.applyOnTheRight(p,q,j_left.transpose());
+            m_workMatrix.applyOnTheLeft(p, q, j_left);
+            if (computeU()) m_matrixU.applyOnTheRight(p, q, j_left.transpose());
 
-            m_workMatrix.applyOnTheRight(p,q,j_right);
-            if(computeV()) m_matrixV.applyOnTheRight(p,q,j_right);
+            m_workMatrix.applyOnTheRight(p, q, j_right);
+            if (computeV()) m_matrixV.applyOnTheRight(p, q, j_right);
 
             // keep track of the largest diagonal coefficient
-            maxDiagEntry = numext::maxi<RealScalar>(maxDiagEntry,numext::maxi<RealScalar>(abs(m_workMatrix.coeff(p,p)), abs(m_workMatrix.coeff(q,q))));
+            maxDiagEntry = numext::maxi<RealScalar>(
+                maxDiagEntry, numext::maxi<RealScalar>(abs(m_workMatrix.coeff(p, p)), abs(m_workMatrix.coeff(q, q))));
           }
         }
       }
     }
   }
 
-  /*** step 3. The work matrix is now diagonal, so ensure it's positive so its diagonal entries are the singular values ***/
+  /*** step 3. The work matrix is now diagonal, so ensure it's positive so its diagonal entries are the singular values
+   * ***/
 
-  for(Index i = 0; i < m_diagSize; ++i)
-  {
+  for (Index i = 0; i < diagSize(); ++i) {
     // For a complex matrix, some diagonal coefficients might note have been
     // treated by svd_precondition_2x2_block_to_be_real, and the imaginary part
     // of some diagonal entry might not be null.
-    if(NumTraits<Scalar>::IsComplex && abs(numext::imag(m_workMatrix.coeff(i,i)))>considerAsZero)
-    {
-      RealScalar a = abs(m_workMatrix.coeff(i,i));
+    if (NumTraits<Scalar>::IsComplex && abs(numext::imag(m_workMatrix.coeff(i, i))) > considerAsZero) {
+      RealScalar a = abs(m_workMatrix.coeff(i, i));
       m_singularValues.coeffRef(i) = abs(a);
-      if(computeU()) m_matrixU.col(i) *= m_workMatrix.coeff(i,i)/a;
-    }
-    else
-    {
+      if (computeU()) m_matrixU.col(i) *= m_workMatrix.coeff(i, i) / a;
+    } else {
       // m_workMatrix.coeff(i,i) is already real, no difficulty:
-      RealScalar a = numext::real(m_workMatrix.coeff(i,i));
+      RealScalar a = numext::real(m_workMatrix.coeff(i, i));
       m_singularValues.coeffRef(i) = abs(a);
-      if(computeU() && (a<RealScalar(0))) m_matrixU.col(i) = -m_matrixU.col(i);
+      if (computeU() && (a < RealScalar(0))) m_matrixU.col(i) = -m_matrixU.col(i);
     }
   }
-  
+
   m_singularValues *= scale;
 
   /*** step 4. Sort singular values in descending order and compute the number of nonzero singular values ***/
 
-  m_nonzeroSingularValues = m_diagSize;
-  for(Index i = 0; i < m_diagSize; i++)
-  {
+  m_nonzeroSingularValues = diagSize();
+  for (Index i = 0; i < diagSize(); i++) {
     Index pos;
-    RealScalar maxRemainingSingularValue = m_singularValues.tail(m_diagSize-i).maxCoeff(&pos);
-    if(maxRemainingSingularValue == RealScalar(0))
-    {
+    RealScalar maxRemainingSingularValue = m_singularValues.tail(diagSize() - i).maxCoeff(&pos);
+    if (numext::is_exactly_zero(maxRemainingSingularValue)) {
       m_nonzeroSingularValues = i;
       break;
     }
-    if(pos)
-    {
+    if (pos) {
       pos += i;
       std::swap(m_singularValues.coeffRef(i), m_singularValues.coeffRef(pos));
-      if(computeU()) m_matrixU.col(pos).swap(m_matrixU.col(i));
-      if(computeV()) m_matrixV.col(pos).swap(m_matrixV.col(i));
+      if (computeU()) m_matrixU.col(pos).swap(m_matrixU.col(i));
+      if (computeV()) m_matrixV.col(pos).swap(m_matrixV.col(i));
     }
   }
 
@@ -794,19 +795,25 @@
 }
 
 /** \svd_module
-  *
-  * \return the singular value decomposition of \c *this computed by two-sided
-  * Jacobi transformations.
-  *
-  * \sa class JacobiSVD
-  */
-template<typename Derived>
-JacobiSVD<typename MatrixBase<Derived>::PlainObject>
-MatrixBase<Derived>::jacobiSvd(unsigned int computationOptions) const
-{
-  return JacobiSVD<PlainObject>(*this, computationOptions);
+ *
+ * \return the singular value decomposition of \c *this computed by two-sided
+ * Jacobi transformations.
+ *
+ * \sa class JacobiSVD
+ */
+template <typename Derived>
+template <int Options>
+JacobiSVD<typename MatrixBase<Derived>::PlainObject, Options> MatrixBase<Derived>::jacobiSvd() const {
+  return JacobiSVD<PlainObject, Options>(*this);
 }
 
-} // end namespace Eigen
+template <typename Derived>
+template <int Options>
+JacobiSVD<typename MatrixBase<Derived>::PlainObject, Options> MatrixBase<Derived>::jacobiSvd(
+    unsigned int computationOptions) const {
+  return JacobiSVD<PlainObject, Options>(*this, computationOptions);
+}
 
-#endif // EIGEN_JACOBISVD_H
+}  // end namespace Eigen
+
+#endif  // EIGEN_JACOBISVD_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h
index bc7ab88..ae2843b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h
@@ -16,18 +16,73 @@
 #ifndef EIGEN_SVDBASE_H
 #define EIGEN_SVDBASE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-template<typename Derived> struct traits<SVDBase<Derived> >
- : traits<Derived>
-{
+
+enum OptionsMasks {
+  QRPreconditionerBits = NoQRPreconditioner | HouseholderQRPreconditioner | ColPivHouseholderQRPreconditioner |
+                         FullPivHouseholderQRPreconditioner,
+  ComputationOptionsBits = ComputeThinU | ComputeFullU | ComputeThinV | ComputeFullV
+};
+
+constexpr int get_qr_preconditioner(int options) { return options & QRPreconditionerBits; }
+
+constexpr int get_computation_options(int options) { return options & ComputationOptionsBits; }
+
+constexpr bool should_svd_compute_thin_u(int options) { return (options & ComputeThinU) != 0; }
+constexpr bool should_svd_compute_full_u(int options) { return (options & ComputeFullU) != 0; }
+constexpr bool should_svd_compute_thin_v(int options) { return (options & ComputeThinV) != 0; }
+constexpr bool should_svd_compute_full_v(int options) { return (options & ComputeFullV) != 0; }
+
+template <typename MatrixType, int Options>
+void check_svd_options_assertions(unsigned int computationOptions, Index rows, Index cols) {
+  EIGEN_STATIC_ASSERT((Options & ComputationOptionsBits) == 0,
+                      "SVDBase: Cannot request U or V using both static and runtime options, even if they match. "
+                      "Requesting unitaries at runtime is DEPRECATED: "
+                      "Prefer requesting unitaries statically, using the Options template parameter.");
+  eigen_assert(
+      !(should_svd_compute_thin_u(computationOptions) && cols < rows && MatrixType::RowsAtCompileTime != Dynamic) &&
+      !(should_svd_compute_thin_v(computationOptions) && rows < cols && MatrixType::ColsAtCompileTime != Dynamic) &&
+      "SVDBase: If thin U is requested at runtime, your matrix must have more rows than columns or a dynamic number of "
+      "rows."
+      "Similarly, if thin V is requested at runtime, you matrix must have more columns than rows or a dynamic number "
+      "of columns.");
+  (void)computationOptions;
+  (void)rows;
+  (void)cols;
+}
+
+template <typename Derived>
+struct traits<SVDBase<Derived> > : traits<Derived> {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
   typedef int StorageIndex;
   enum { Flags = 0 };
 };
-}
+
+template <typename MatrixType, int Options_>
+struct svd_traits : traits<MatrixType> {
+  static constexpr int Options = Options_;
+  static constexpr bool ShouldComputeFullU = internal::should_svd_compute_full_u(Options);
+  static constexpr bool ShouldComputeThinU = internal::should_svd_compute_thin_u(Options);
+  static constexpr bool ShouldComputeFullV = internal::should_svd_compute_full_v(Options);
+  static constexpr bool ShouldComputeThinV = internal::should_svd_compute_thin_v(Options);
+  enum {
+    DiagSizeAtCompileTime =
+        internal::min_size_prefer_dynamic(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime),
+    MaxDiagSizeAtCompileTime =
+        internal::min_size_prefer_dynamic(MatrixType::MaxRowsAtCompileTime, MatrixType::MaxColsAtCompileTime),
+    MatrixUColsAtCompileTime = ShouldComputeThinU ? DiagSizeAtCompileTime : MatrixType::RowsAtCompileTime,
+    MatrixVColsAtCompileTime = ShouldComputeThinV ? DiagSizeAtCompileTime : MatrixType::ColsAtCompileTime,
+    MatrixUMaxColsAtCompileTime = ShouldComputeThinU ? MaxDiagSizeAtCompileTime : MatrixType::MaxRowsAtCompileTime,
+    MatrixVMaxColsAtCompileTime = ShouldComputeThinV ? MaxDiagSizeAtCompileTime : MatrixType::MaxColsAtCompileTime
+  };
+};
+}  // namespace internal
 
 /** \ingroup SVD_Module
  *
@@ -40,66 +95,83 @@
  *
  * SVD decomposition consists in decomposing any n-by-p matrix \a A as a product
  *   \f[ A = U S V^* \f]
- * where \a U is a n-by-n unitary, \a V is a p-by-p unitary, and \a S is a n-by-p real positive matrix which is zero outside of its main diagonal;
- * the diagonal entries of S are known as the \em singular \em values of \a A and the columns of \a U and \a V are known as the left
- * and right \em singular \em vectors of \a A respectively.
+ * where \a U is a n-by-n unitary, \a V is a p-by-p unitary, and \a S is a n-by-p real positive matrix which is zero
+ * outside of its main diagonal; the diagonal entries of S are known as the \em singular \em values of \a A and the
+ * columns of \a U and \a V are known as the left and right \em singular \em vectors of \a A respectively.
  *
  * Singular values are always sorted in decreasing order.
  *
- * 
- * You can ask for only \em thin \a U or \a V to be computed, meaning the following. In case of a rectangular n-by-p matrix, letting \a m be the
- * smaller value among \a n and \a p, there are only \a m singular vectors; the remaining columns of \a U and \a V do not correspond to actual
- * singular vectors. Asking for \em thin \a U or \a V means asking for only their \a m first columns to be formed. So \a U is then a n-by-m matrix,
- * and \a V is then a p-by-m matrix. Notice that thin \a U and \a V are all you need for (least squares) solving.
- * 
- * The status of the computation can be retrived using the \a info() method. Unless \a info() returns \a Success, the results should be not
- * considered well defined.
- *  
- * If the input matrix has inf or nan coefficients, the result of the computation is undefined, and \a info() will return \a InvalidInput, but the computation is guaranteed to
- * terminate in finite (and reasonable) time.
- * \sa class BDCSVD, class JacobiSVD
+ *
+ * You can ask for only \em thin \a U or \a V to be computed, meaning the following. In case of a rectangular n-by-p
+ * matrix, letting \a m be the smaller value among \a n and \a p, there are only \a m singular vectors; the remaining
+ * columns of \a U and \a V do not correspond to actual singular vectors. Asking for \em thin \a U or \a V means asking
+ * for only their \a m first columns to be formed. So \a U is then a n-by-m matrix, and \a V is then a p-by-m matrix.
+ * Notice that thin \a U and \a V are all you need for (least squares) solving.
+ *
+ * The status of the computation can be retrieved using the \a info() method. Unless \a info() returns \a Success, the
+ * results should be not considered well defined.
+ *
+ * If the input matrix has inf or nan coefficients, the result of the computation is undefined, and \a info() will
+ * return \a InvalidInput, but the computation is guaranteed to terminate in finite (and reasonable) time. \sa class
+ * BDCSVD, class JacobiSVD
  */
-template<typename Derived> class SVDBase
- : public SolverBase<SVDBase<Derived> >
-{
-public: 
-   
-  template<typename Derived_>
+template <typename Derived>
+class SVDBase : public SolverBase<SVDBase<Derived> > {
+ public:
+  template <typename Derived_>
   friend struct internal::solve_assertion;
 
   typedef typename internal::traits<Derived>::MatrixType MatrixType;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
   typedef typename Eigen::internal::traits<SVDBase>::StorageIndex StorageIndex;
-  typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
+
+  static constexpr bool ShouldComputeFullU = internal::traits<Derived>::ShouldComputeFullU;
+  static constexpr bool ShouldComputeThinU = internal::traits<Derived>::ShouldComputeThinU;
+  static constexpr bool ShouldComputeFullV = internal::traits<Derived>::ShouldComputeFullV;
+  static constexpr bool ShouldComputeThinV = internal::traits<Derived>::ShouldComputeThinV;
+
   enum {
     RowsAtCompileTime = MatrixType::RowsAtCompileTime,
     ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-    DiagSizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime,ColsAtCompileTime),
+    DiagSizeAtCompileTime = internal::min_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime),
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    MaxDiagSizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_FIXED(MaxRowsAtCompileTime,MaxColsAtCompileTime),
-    MatrixOptions = MatrixType::Options
+    MaxDiagSizeAtCompileTime = internal::min_size_prefer_fixed(MaxRowsAtCompileTime, MaxColsAtCompileTime),
+    MatrixOptions = MatrixType::Options,
+    MatrixUColsAtCompileTime = internal::traits<Derived>::MatrixUColsAtCompileTime,
+    MatrixVColsAtCompileTime = internal::traits<Derived>::MatrixVColsAtCompileTime,
+    MatrixUMaxColsAtCompileTime = internal::traits<Derived>::MatrixUMaxColsAtCompileTime,
+    MatrixVMaxColsAtCompileTime = internal::traits<Derived>::MatrixVMaxColsAtCompileTime
   };
 
-  typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime, MatrixOptions, MaxRowsAtCompileTime, MaxRowsAtCompileTime> MatrixUType;
-  typedef Matrix<Scalar, ColsAtCompileTime, ColsAtCompileTime, MatrixOptions, MaxColsAtCompileTime, MaxColsAtCompileTime> MatrixVType;
+  EIGEN_STATIC_ASSERT(!(ShouldComputeFullU && ShouldComputeThinU), "SVDBase: Cannot request both full and thin U")
+  EIGEN_STATIC_ASSERT(!(ShouldComputeFullV && ShouldComputeThinV), "SVDBase: Cannot request both full and thin V")
+
+  typedef
+      typename internal::make_proper_matrix_type<Scalar, RowsAtCompileTime, MatrixUColsAtCompileTime, MatrixOptions,
+                                                 MaxRowsAtCompileTime, MatrixUMaxColsAtCompileTime>::type MatrixUType;
+  typedef
+      typename internal::make_proper_matrix_type<Scalar, ColsAtCompileTime, MatrixVColsAtCompileTime, MatrixOptions,
+                                                 MaxColsAtCompileTime, MatrixVMaxColsAtCompileTime>::type MatrixVType;
+
   typedef typename internal::plain_diag_type<MatrixType, RealScalar>::type SingularValuesType;
-  
+
   Derived& derived() { return *static_cast<Derived*>(this); }
   const Derived& derived() const { return *static_cast<const Derived*>(this); }
 
   /** \returns the \a U matrix.
    *
    * For the SVD decomposition of a n-by-p matrix, letting \a m be the minimum of \a n and \a p,
-   * the U matrix is n-by-n if you asked for \link Eigen::ComputeFullU ComputeFullU \endlink, and is n-by-m if you asked for \link Eigen::ComputeThinU ComputeThinU \endlink.
+   * the U matrix is n-by-n if you asked for \link Eigen::ComputeFullU ComputeFullU \endlink, and is n-by-m if you asked
+   * for \link Eigen::ComputeThinU ComputeThinU \endlink.
    *
    * The \a m first columns of \a U are the left singular vectors of the matrix being decomposed.
    *
    * This method asserts that you asked for \a U to be computed.
    */
-  const MatrixUType& matrixU() const
-  {
+  const MatrixUType& matrixU() const {
     _check_compute_assertions();
     eigen_assert(computeU() && "This SVD decomposition didn't compute U. Did you ask for it?");
     return m_matrixU;
@@ -108,14 +180,14 @@
   /** \returns the \a V matrix.
    *
    * For the SVD decomposition of a n-by-p matrix, letting \a m be the minimum of \a n and \a p,
-   * the V matrix is p-by-p if you asked for \link Eigen::ComputeFullV ComputeFullV \endlink, and is p-by-m if you asked for \link Eigen::ComputeThinV ComputeThinV \endlink.
+   * the V matrix is p-by-p if you asked for \link Eigen::ComputeFullV ComputeFullV \endlink, and is p-by-m if you asked
+   * for \link Eigen::ComputeThinV ComputeThinV \endlink.
    *
    * The \a m first columns of \a V are the right singular vectors of the matrix being decomposed.
    *
    * This method asserts that you asked for \a V to be computed.
    */
-  const MatrixVType& matrixV() const
-  {
+  const MatrixVType& matrixV() const {
     _check_compute_assertions();
     eigen_assert(computeV() && "This SVD decomposition didn't compute V. Did you ask for it?");
     return m_matrixV;
@@ -126,82 +198,76 @@
    * For the SVD decomposition of a n-by-p matrix, letting \a m be the minimum of \a n and \a p, the
    * returned vector has size \a m.  Singular values are always sorted in decreasing order.
    */
-  const SingularValuesType& singularValues() const
-  {
+  const SingularValuesType& singularValues() const {
     _check_compute_assertions();
     return m_singularValues;
   }
 
   /** \returns the number of singular values that are not exactly 0 */
-  Index nonzeroSingularValues() const
-  {
+  Index nonzeroSingularValues() const {
     _check_compute_assertions();
     return m_nonzeroSingularValues;
   }
-  
+
   /** \returns the rank of the matrix of which \c *this is the SVD.
-    *
-    * \note This method has to determine which singular values should be considered nonzero.
-    *       For that, it uses the threshold value that you can control by calling
-    *       setThreshold(const RealScalar&).
-    */
-  inline Index rank() const
-  {
+   *
+   * \note This method has to determine which singular values should be considered nonzero.
+   *       For that, it uses the threshold value that you can control by calling
+   *       setThreshold(const RealScalar&).
+   */
+  inline Index rank() const {
     using std::abs;
     _check_compute_assertions();
-    if(m_singularValues.size()==0) return 0;
-    RealScalar premultiplied_threshold = numext::maxi<RealScalar>(m_singularValues.coeff(0) * threshold(), (std::numeric_limits<RealScalar>::min)());
-    Index i = m_nonzeroSingularValues-1;
-    while(i>=0 && m_singularValues.coeff(i) < premultiplied_threshold) --i;
-    return i+1;
+    if (m_singularValues.size() == 0) return 0;
+    RealScalar premultiplied_threshold =
+        numext::maxi<RealScalar>(m_singularValues.coeff(0) * threshold(), (std::numeric_limits<RealScalar>::min)());
+    Index i = m_nonzeroSingularValues - 1;
+    while (i >= 0 && m_singularValues.coeff(i) < premultiplied_threshold) --i;
+    return i + 1;
   }
-  
+
   /** Allows to prescribe a threshold to be used by certain methods, such as rank() and solve(),
-    * which need to determine when singular values are to be considered nonzero.
-    * This is not used for the SVD decomposition itself.
-    *
-    * When it needs to get the threshold value, Eigen calls threshold().
-    * The default is \c NumTraits<Scalar>::epsilon()
-    *
-    * \param threshold The new value to use as the threshold.
-    *
-    * A singular value will be considered nonzero if its value is strictly greater than
-    *  \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$.
-    *
-    * If you want to come back to the default behavior, call setThreshold(Default_t)
-    */
-  Derived& setThreshold(const RealScalar& threshold)
-  {
+   * which need to determine when singular values are to be considered nonzero.
+   * This is not used for the SVD decomposition itself.
+   *
+   * When it needs to get the threshold value, Eigen calls threshold().
+   * The default is \c NumTraits<Scalar>::epsilon()
+   *
+   * \param threshold The new value to use as the threshold.
+   *
+   * A singular value will be considered nonzero if its value is strictly greater than
+   *  \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$.
+   *
+   * If you want to come back to the default behavior, call setThreshold(Default_t)
+   */
+  Derived& setThreshold(const RealScalar& threshold) {
     m_usePrescribedThreshold = true;
     m_prescribedThreshold = threshold;
     return derived();
   }
 
   /** Allows to come back to the default behavior, letting Eigen use its default formula for
-    * determining the threshold.
-    *
-    * You should pass the special object Eigen::Default as parameter here.
-    * \code svd.setThreshold(Eigen::Default); \endcode
-    *
-    * See the documentation of setThreshold(const RealScalar&).
-    */
-  Derived& setThreshold(Default_t)
-  {
+   * determining the threshold.
+   *
+   * You should pass the special object Eigen::Default as parameter here.
+   * \code svd.setThreshold(Eigen::Default); \endcode
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  Derived& setThreshold(Default_t) {
     m_usePrescribedThreshold = false;
     return derived();
   }
 
   /** Returns the threshold that will be used by certain methods such as rank().
-    *
-    * See the documentation of setThreshold(const RealScalar&).
-    */
-  RealScalar threshold() const
-  {
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  RealScalar threshold() const {
     eigen_assert(m_isInitialized || m_usePrescribedThreshold);
     // this temporary is needed to workaround a MSVC issue
-    Index diagSize = (std::max<Index>)(1,m_diagSize);
-    return m_usePrescribedThreshold ? m_prescribedThreshold
-                                    : RealScalar(diagSize)*NumTraits<Scalar>::epsilon();
+    Index diagSize = (std::max<Index>)(1, m_diagSize);
+    return m_usePrescribedThreshold ? m_prescribedThreshold : RealScalar(diagSize) * NumTraits<Scalar>::epsilon();
   }
 
   /** \returns true if \a U (full or thin) is asked for in this SVD decomposition */
@@ -209,65 +275,59 @@
   /** \returns true if \a V (full or thin) is asked for in this SVD decomposition */
   inline bool computeV() const { return m_computeFullV || m_computeThinV; }
 
-  inline Index rows() const { return m_rows; }
-  inline Index cols() const { return m_cols; }
-  
-  #ifdef EIGEN_PARSED_BY_DOXYGEN
-  /** \returns a (least squares) solution of \f$ A x = b \f$ using the current SVD decomposition of A.
-    *
-    * \param b the right-hand-side of the equation to solve.
-    *
-    * \note Solving requires both U and V to be computed. Thin U and V are enough, there is no need for full U or V.
-    *
-    * \note SVD solving is implicitly least-squares. Thus, this method serves both purposes of exact solving and least-squares solving.
-    * In other words, the returned solution is guaranteed to minimize the Euclidean norm \f$ \Vert A x - b \Vert \f$.
-    */
-  template<typename Rhs>
-  inline const Solve<Derived, Rhs>
-  solve(const MatrixBase<Rhs>& b) const;
-  #endif
+  inline Index rows() const { return m_rows.value(); }
+  inline Index cols() const { return m_cols.value(); }
+  inline Index diagSize() const { return m_diagSize.value(); }
 
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** \returns a (least squares) solution of \f$ A x = b \f$ using the current SVD decomposition of A.
+   *
+   * \param b the right-hand-side of the equation to solve.
+   *
+   * \note Solving requires both U and V to be computed. Thin U and V are enough, there is no need for full U or V.
+   *
+   * \note SVD solving is implicitly least-squares. Thus, this method serves both purposes of exact solving and
+   * least-squares solving. In other words, the returned solution is guaranteed to minimize the Euclidean norm \f$ \Vert
+   * A x - b \Vert \f$.
+   */
+  template <typename Rhs>
+  inline const Solve<Derived, Rhs> solve(const MatrixBase<Rhs>& b) const;
+#endif
 
   /** \brief Reports whether previous computation was successful.
    *
    * \returns \c Success if computation was successful.
    */
-  EIGEN_DEVICE_FUNC
-  ComputationInfo info() const
-  {
+  EIGEN_DEVICE_FUNC ComputationInfo info() const {
     eigen_assert(m_isInitialized && "SVD is not initialized.");
     return m_info;
   }
 
-  #ifndef EIGEN_PARSED_BY_DOXYGEN
-  template<typename RhsType, typename DstType>
-  void _solve_impl(const RhsType &rhs, DstType &dst) const;
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
 
-  template<bool Conjugate, typename RhsType, typename DstType>
-  void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
-  #endif
+  template <bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const;
+#endif
 
-protected:
+ protected:
+  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
-  static void check_template_parameters()
-  {
-    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
-  }
+  void _check_compute_assertions() const { eigen_assert(m_isInitialized && "SVD is not initialized."); }
 
-  void _check_compute_assertions() const {
-    eigen_assert(m_isInitialized && "SVD is not initialized.");
-  }
-
-  template<bool Transpose_, typename Rhs>
+  template <bool Transpose_, typename Rhs>
   void _check_solve_assertion(const Rhs& b) const {
-      EIGEN_ONLY_USED_FOR_DEBUG(b);
-      _check_compute_assertions();
-      eigen_assert(computeU() && computeV() && "SVDBase::solve(): Both unitaries U and V are required to be computed (thin unitaries suffice).");
-      eigen_assert((Transpose_?cols():rows())==b.rows() && "SVDBase::solve(): invalid number of rows of the right hand side matrix b");
+    EIGEN_ONLY_USED_FOR_DEBUG(b);
+    _check_compute_assertions();
+    eigen_assert(computeU() && computeV() &&
+                 "SVDBase::solve(): Both unitaries U and V are required to be computed (thin unitaries suffice).");
+    eigen_assert((Transpose_ ? cols() : rows()) == b.rows() &&
+                 "SVDBase::solve(): invalid number of rows of the right hand side matrix b");
   }
 
   // return true if already allocated
-  bool allocate(Index rows, Index cols, unsigned int computationOptions) ;
+  bool allocate(Index rows, Index cols, unsigned int computationOptions);
 
   MatrixUType m_matrixU;
   MatrixVType m_matrixV;
@@ -277,7 +337,10 @@
   bool m_computeFullU, m_computeThinU;
   bool m_computeFullV, m_computeThinV;
   unsigned int m_computationOptions;
-  Index m_nonzeroSingularValues, m_rows, m_cols, m_diagSize;
+  Index m_nonzeroSingularValues;
+  internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
+  internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
+  internal::variable_if_dynamic<Index, DiagSizeAtCompileTime> m_diagSize;
   RealScalar m_prescribedThreshold;
 
   /** \brief Default Constructor.
@@ -285,92 +348,90 @@
    * Default constructor of SVDBase
    */
   SVDBase()
-    : m_info(Success),
-      m_isInitialized(false),
-      m_isAllocated(false),
-      m_usePrescribedThreshold(false),
-      m_computeFullU(false),
-      m_computeThinU(false),
-      m_computeFullV(false),
-      m_computeThinV(false),
-      m_computationOptions(0),
-      m_rows(-1), m_cols(-1), m_diagSize(0)
-  {
-    check_template_parameters();
-  }
-
-
+      : m_matrixU(MatrixUType()),
+        m_matrixV(MatrixVType()),
+        m_singularValues(SingularValuesType()),
+        m_info(Success),
+        m_isInitialized(false),
+        m_isAllocated(false),
+        m_usePrescribedThreshold(false),
+        m_computeFullU(false),
+        m_computeThinU(false),
+        m_computeFullV(false),
+        m_computeThinV(false),
+        m_computationOptions(0),
+        m_nonzeroSingularValues(0),
+        m_rows(RowsAtCompileTime),
+        m_cols(ColsAtCompileTime),
+        m_diagSize(DiagSizeAtCompileTime),
+        m_prescribedThreshold(0) {}
 };
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename Derived>
-template<typename RhsType, typename DstType>
-void SVDBase<Derived>::_solve_impl(const RhsType &rhs, DstType &dst) const
-{
+template <typename Derived>
+template <typename RhsType, typename DstType>
+void SVDBase<Derived>::_solve_impl(const RhsType& rhs, DstType& dst) const {
   // A = U S V^*
   // So A^{-1} = V S^{-1} U^*
 
-  Matrix<typename RhsType::Scalar, Dynamic, RhsType::ColsAtCompileTime, 0, MatrixType::MaxRowsAtCompileTime, RhsType::MaxColsAtCompileTime> tmp;
+  Matrix<typename RhsType::Scalar, Dynamic, RhsType::ColsAtCompileTime, 0, MatrixType::MaxRowsAtCompileTime,
+         RhsType::MaxColsAtCompileTime>
+      tmp;
   Index l_rank = rank();
-  tmp.noalias() =  m_matrixU.leftCols(l_rank).adjoint() * rhs;
+  tmp.noalias() = m_matrixU.leftCols(l_rank).adjoint() * rhs;
   tmp = m_singularValues.head(l_rank).asDiagonal().inverse() * tmp;
   dst = m_matrixV.leftCols(l_rank) * tmp;
 }
 
-template<typename Derived>
-template<bool Conjugate, typename RhsType, typename DstType>
-void SVDBase<Derived>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
-{
+template <typename Derived>
+template <bool Conjugate, typename RhsType, typename DstType>
+void SVDBase<Derived>::_solve_impl_transposed(const RhsType& rhs, DstType& dst) const {
   // A = U S V^*
   // So  A^{-*} = U S^{-1} V^*
   // And A^{-T} = U_conj S^{-1} V^T
-  Matrix<typename RhsType::Scalar, Dynamic, RhsType::ColsAtCompileTime, 0, MatrixType::MaxRowsAtCompileTime, RhsType::MaxColsAtCompileTime> tmp;
+  Matrix<typename RhsType::Scalar, Dynamic, RhsType::ColsAtCompileTime, 0, MatrixType::MaxRowsAtCompileTime,
+         RhsType::MaxColsAtCompileTime>
+      tmp;
   Index l_rank = rank();
 
-  tmp.noalias() =  m_matrixV.leftCols(l_rank).transpose().template conjugateIf<Conjugate>() * rhs;
+  tmp.noalias() = m_matrixV.leftCols(l_rank).transpose().template conjugateIf<Conjugate>() * rhs;
   tmp = m_singularValues.head(l_rank).asDiagonal().inverse() * tmp;
   dst = m_matrixU.template conjugateIf<!Conjugate>().leftCols(l_rank) * tmp;
 }
 #endif
 
-template<typename MatrixType>
-bool SVDBase<MatrixType>::allocate(Index rows, Index cols, unsigned int computationOptions)
-{
+template <typename Derived>
+bool SVDBase<Derived>::allocate(Index rows, Index cols, unsigned int computationOptions) {
   eigen_assert(rows >= 0 && cols >= 0);
 
-  if (m_isAllocated &&
-      rows == m_rows &&
-      cols == m_cols &&
-      computationOptions == m_computationOptions)
-  {
+  if (m_isAllocated && rows == m_rows.value() && cols == m_cols.value() && computationOptions == m_computationOptions) {
     return true;
   }
 
-  m_rows = rows;
-  m_cols = cols;
+  m_rows.setValue(rows);
+  m_cols.setValue(cols);
   m_info = Success;
   m_isInitialized = false;
   m_isAllocated = true;
   m_computationOptions = computationOptions;
-  m_computeFullU = (computationOptions & ComputeFullU) != 0;
-  m_computeThinU = (computationOptions & ComputeThinU) != 0;
-  m_computeFullV = (computationOptions & ComputeFullV) != 0;
-  m_computeThinV = (computationOptions & ComputeThinV) != 0;
+  m_computeFullU = ShouldComputeFullU || internal::should_svd_compute_full_u(computationOptions);
+  m_computeThinU = ShouldComputeThinU || internal::should_svd_compute_thin_u(computationOptions);
+  m_computeFullV = ShouldComputeFullV || internal::should_svd_compute_full_v(computationOptions);
+  m_computeThinV = ShouldComputeThinV || internal::should_svd_compute_thin_v(computationOptions);
+
   eigen_assert(!(m_computeFullU && m_computeThinU) && "SVDBase: you can't ask for both full and thin U");
   eigen_assert(!(m_computeFullV && m_computeThinV) && "SVDBase: you can't ask for both full and thin V");
-  eigen_assert(EIGEN_IMPLIES(m_computeThinU || m_computeThinV, MatrixType::ColsAtCompileTime==Dynamic) &&
-	       "SVDBase: thin U and V are only available when your matrix has a dynamic number of columns.");
 
-  m_diagSize = (std::min)(m_rows, m_cols);
-  m_singularValues.resize(m_diagSize);
-  if(RowsAtCompileTime==Dynamic)
-    m_matrixU.resize(m_rows, m_computeFullU ? m_rows : m_computeThinU ? m_diagSize : 0);
-  if(ColsAtCompileTime==Dynamic)
-    m_matrixV.resize(m_cols, m_computeFullV ? m_cols : m_computeThinV ? m_diagSize : 0);
+  m_diagSize.setValue(numext::mini(m_rows.value(), m_cols.value()));
+  m_singularValues.resize(m_diagSize.value());
+  if (RowsAtCompileTime == Dynamic)
+    m_matrixU.resize(m_rows.value(), m_computeFullU ? m_rows.value() : m_computeThinU ? m_diagSize.value() : 0);
+  if (ColsAtCompileTime == Dynamic)
+    m_matrixV.resize(m_cols.value(), m_computeFullV ? m_cols.value() : m_computeThinV ? m_diagSize.value() : 0);
 
   return false;
 }
 
-}// end namespace
+}  // namespace Eigen
 
-#endif // EIGEN_SVDBASE_H
+#endif  // EIGEN_SVDBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h
index 997defc..d78b30b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h
@@ -11,213 +11,200 @@
 #ifndef EIGEN_BIDIAGONALIZATION_H
 #define EIGEN_BIDIAGONALIZATION_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 // UpperBidiagonalization will probably be replaced by a Bidiagonalization class, don't want to make it stable API.
 // At the same time, it's useful to keep for now as it's about the only thing that is testing the BandMatrix class.
 
-template<typename _MatrixType> class UpperBidiagonalization
-{
-  public:
+template <typename MatrixType_>
+class UpperBidiagonalization {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    ColsAtCompileTimeMinusOne = internal::decrement_size<ColsAtCompileTime>::ret
+  };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef Eigen::Index Index;  ///< \deprecated since Eigen 3.3
+  typedef Matrix<Scalar, 1, ColsAtCompileTime> RowVectorType;
+  typedef Matrix<Scalar, RowsAtCompileTime, 1> ColVectorType;
+  typedef BandMatrix<RealScalar, ColsAtCompileTime, ColsAtCompileTime, 1, 0, RowMajor> BidiagonalType;
+  typedef Matrix<Scalar, ColsAtCompileTime, 1> DiagVectorType;
+  typedef Matrix<Scalar, ColsAtCompileTimeMinusOne, 1> SuperDiagVectorType;
+  typedef HouseholderSequence<
+      const MatrixType, const internal::remove_all_t<typename Diagonal<const MatrixType, 0>::ConjugateReturnType> >
+      HouseholderUSequenceType;
+  typedef HouseholderSequence<const internal::remove_all_t<typename MatrixType::ConjugateReturnType>,
+                              Diagonal<const MatrixType, 1>, OnTheRight>
+      HouseholderVSequenceType;
 
-    typedef _MatrixType MatrixType;
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      ColsAtCompileTimeMinusOne = internal::decrement_size<ColsAtCompileTime>::ret
-    };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    typedef Matrix<Scalar, 1, ColsAtCompileTime> RowVectorType;
-    typedef Matrix<Scalar, RowsAtCompileTime, 1> ColVectorType;
-    typedef BandMatrix<RealScalar, ColsAtCompileTime, ColsAtCompileTime, 1, 0, RowMajor> BidiagonalType;
-    typedef Matrix<Scalar, ColsAtCompileTime, 1> DiagVectorType;
-    typedef Matrix<Scalar, ColsAtCompileTimeMinusOne, 1> SuperDiagVectorType;
-    typedef HouseholderSequence<
-              const MatrixType,
-              const typename internal::remove_all<typename Diagonal<const MatrixType,0>::ConjugateReturnType>::type
-            > HouseholderUSequenceType;
-    typedef HouseholderSequence<
-              const typename internal::remove_all<typename MatrixType::ConjugateReturnType>::type,
-              Diagonal<const MatrixType,1>,
-              OnTheRight
-            > HouseholderVSequenceType;
-    
-    /**
-    * \brief Default Constructor.
-    *
-    * The default constructor is useful in cases in which the user intends to
-    * perform decompositions via Bidiagonalization::compute(const MatrixType&).
-    */
-    UpperBidiagonalization() : m_householder(), m_bidiagonal(), m_isInitialized(false) {}
+  /**
+   * \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via Bidiagonalization::compute(const MatrixType&).
+   */
+  UpperBidiagonalization() : m_householder(), m_bidiagonal(0, 0), m_isInitialized(false) {}
 
-    explicit UpperBidiagonalization(const MatrixType& matrix)
+  explicit UpperBidiagonalization(const MatrixType& matrix)
       : m_householder(matrix.rows(), matrix.cols()),
         m_bidiagonal(matrix.cols(), matrix.cols()),
-        m_isInitialized(false)
-    {
-      compute(matrix);
-    }
-    
-    UpperBidiagonalization& compute(const MatrixType& matrix);
-    UpperBidiagonalization& computeUnblocked(const MatrixType& matrix);
-    
-    const MatrixType& householder() const { return m_householder; }
-    const BidiagonalType& bidiagonal() const { return m_bidiagonal; }
-    
-    const HouseholderUSequenceType householderU() const
-    {
-      eigen_assert(m_isInitialized && "UpperBidiagonalization is not initialized.");
-      return HouseholderUSequenceType(m_householder, m_householder.diagonal().conjugate());
-    }
+        m_isInitialized(false) {
+    compute(matrix);
+  }
 
-    const HouseholderVSequenceType householderV() // const here gives nasty errors and i'm lazy
-    {
-      eigen_assert(m_isInitialized && "UpperBidiagonalization is not initialized.");
-      return HouseholderVSequenceType(m_householder.conjugate(), m_householder.const_derived().template diagonal<1>())
-             .setLength(m_householder.cols()-1)
-             .setShift(1);
-    }
-    
-  protected:
-    MatrixType m_householder;
-    BidiagonalType m_bidiagonal;
-    bool m_isInitialized;
+  UpperBidiagonalization(Index rows, Index cols)
+      : m_householder(rows, cols), m_bidiagonal(cols, cols), m_isInitialized(false) {}
+
+  UpperBidiagonalization& compute(const MatrixType& matrix);
+  UpperBidiagonalization& computeUnblocked(const MatrixType& matrix);
+
+  const MatrixType& householder() const { return m_householder; }
+  const BidiagonalType& bidiagonal() const { return m_bidiagonal; }
+
+  const HouseholderUSequenceType householderU() const {
+    eigen_assert(m_isInitialized && "UpperBidiagonalization is not initialized.");
+    return HouseholderUSequenceType(m_householder, m_householder.diagonal().conjugate());
+  }
+
+  const HouseholderVSequenceType householderV()  // const here gives nasty errors and i'm lazy
+  {
+    eigen_assert(m_isInitialized && "UpperBidiagonalization is not initialized.");
+    return HouseholderVSequenceType(m_householder.conjugate(), m_householder.const_derived().template diagonal<1>())
+        .setLength(m_householder.cols() - 1)
+        .setShift(1);
+  }
+
+ protected:
+  MatrixType m_householder;
+  BidiagonalType m_bidiagonal;
+  bool m_isInitialized;
 };
 
 // Standard upper bidiagonalization without fancy optimizations
 // This version should be faster for small matrix size
-template<typename MatrixType>
-void upperbidiagonalization_inplace_unblocked(MatrixType& mat,
-                                              typename MatrixType::RealScalar *diagonal,
-                                              typename MatrixType::RealScalar *upper_diagonal,
-                                              typename MatrixType::Scalar* tempData = 0)
-{
+template <typename MatrixType>
+void upperbidiagonalization_inplace_unblocked(MatrixType& mat, typename MatrixType::RealScalar* diagonal,
+                                              typename MatrixType::RealScalar* upper_diagonal,
+                                              typename MatrixType::Scalar* tempData = 0) {
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = mat.rows();
   Index cols = mat.cols();
 
-  typedef Matrix<Scalar,Dynamic,1,ColMajor,MatrixType::MaxRowsAtCompileTime,1> TempType;
+  typedef Matrix<Scalar, Dynamic, 1, ColMajor, MatrixType::MaxRowsAtCompileTime, 1> TempType;
   TempType tempVector;
-  if(tempData==0)
-  {
+  if (tempData == 0) {
     tempVector.resize(rows);
     tempData = tempVector.data();
   }
 
-  for (Index k = 0; /* breaks at k==cols-1 below */ ; ++k)
-  {
+  for (Index k = 0; /* breaks at k==cols-1 below */; ++k) {
     Index remainingRows = rows - k;
     Index remainingCols = cols - k - 1;
 
     // construct left householder transform in-place in A
-    mat.col(k).tail(remainingRows)
-       .makeHouseholderInPlace(mat.coeffRef(k,k), diagonal[k]);
+    mat.col(k).tail(remainingRows).makeHouseholderInPlace(mat.coeffRef(k, k), diagonal[k]);
     // apply householder transform to remaining part of A on the left
     mat.bottomRightCorner(remainingRows, remainingCols)
-       .applyHouseholderOnTheLeft(mat.col(k).tail(remainingRows-1), mat.coeff(k,k), tempData);
+        .applyHouseholderOnTheLeft(mat.col(k).tail(remainingRows - 1), mat.coeff(k, k), tempData);
 
-    if(k == cols-1) break;
+    if (k == cols - 1) break;
 
     // construct right householder transform in-place in mat
-    mat.row(k).tail(remainingCols)
-       .makeHouseholderInPlace(mat.coeffRef(k,k+1), upper_diagonal[k]);
+    mat.row(k).tail(remainingCols).makeHouseholderInPlace(mat.coeffRef(k, k + 1), upper_diagonal[k]);
     // apply householder transform to remaining part of mat on the left
-    mat.bottomRightCorner(remainingRows-1, remainingCols)
-       .applyHouseholderOnTheRight(mat.row(k).tail(remainingCols-1).adjoint(), mat.coeff(k,k+1), tempData);
+    mat.bottomRightCorner(remainingRows - 1, remainingCols)
+        .applyHouseholderOnTheRight(mat.row(k).tail(remainingCols - 1).adjoint(), mat.coeff(k, k + 1), tempData);
   }
 }
 
 /** \internal
-  * Helper routine for the block reduction to upper bidiagonal form.
-  *
-  * Let's partition the matrix A:
-  * 
-  *      | A00 A01 |
-  *  A = |         |
-  *      | A10 A11 |
-  *
-  * This function reduces to bidiagonal form the left \c rows x \a blockSize vertical panel [A00/A10]
-  * and the \a blockSize x \c cols horizontal panel [A00 A01] of the matrix \a A. The bottom-right block A11
-  * is updated using matrix-matrix products:
-  *   A22 -= V * Y^T - X * U^T
-  * where V and U contains the left and right Householder vectors. U and V are stored in A10, and A01
-  * respectively, and the update matrices X and Y are computed during the reduction.
-  * 
-  */
-template<typename MatrixType>
-void upperbidiagonalization_blocked_helper(MatrixType& A,
-                                           typename MatrixType::RealScalar *diagonal,
-                                           typename MatrixType::RealScalar *upper_diagonal,
-                                           Index bs,
-                                           Ref<Matrix<typename MatrixType::Scalar, Dynamic, Dynamic,
-                                                      traits<MatrixType>::Flags & RowMajorBit> > X,
-                                           Ref<Matrix<typename MatrixType::Scalar, Dynamic, Dynamic,
-                                                      traits<MatrixType>::Flags & RowMajorBit> > Y)
-{
+ * Helper routine for the block reduction to upper bidiagonal form.
+ *
+ * Let's partition the matrix A:
+ *
+ *      | A00 A01 |
+ *  A = |         |
+ *      | A10 A11 |
+ *
+ * This function reduces to bidiagonal form the left \c rows x \a blockSize vertical panel [A00/A10]
+ * and the \a blockSize x \c cols horizontal panel [A00 A01] of the matrix \a A. The bottom-right block A11
+ * is updated using matrix-matrix products:
+ *   A22 -= V * Y^T - X * U^T
+ * where V and U contains the left and right Householder vectors. U and V are stored in A10, and A01
+ * respectively, and the update matrices X and Y are computed during the reduction.
+ *
+ */
+template <typename MatrixType>
+void upperbidiagonalization_blocked_helper(
+    MatrixType& A, typename MatrixType::RealScalar* diagonal, typename MatrixType::RealScalar* upper_diagonal, Index bs,
+    Ref<Matrix<typename MatrixType::Scalar, Dynamic, Dynamic, traits<MatrixType>::Flags & RowMajorBit> > X,
+    Ref<Matrix<typename MatrixType::Scalar, Dynamic, Dynamic, traits<MatrixType>::Flags & RowMajorBit> > Y) {
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   typedef typename NumTraits<RealScalar>::Literal Literal;
-  enum { StorageOrder = traits<MatrixType>::Flags & RowMajorBit };
-  typedef InnerStride<int(StorageOrder) == int(ColMajor) ? 1 : Dynamic> ColInnerStride;
-  typedef InnerStride<int(StorageOrder) == int(ColMajor) ? Dynamic : 1> RowInnerStride;
-  typedef Ref<Matrix<Scalar, Dynamic, 1>, 0, ColInnerStride>    SubColumnType;
-  typedef Ref<Matrix<Scalar, 1, Dynamic>, 0, RowInnerStride>    SubRowType;
-  typedef Ref<Matrix<Scalar, Dynamic, Dynamic, StorageOrder > > SubMatType;
-  
+  static constexpr int StorageOrder = (traits<MatrixType>::Flags & RowMajorBit) ? RowMajor : ColMajor;
+  typedef InnerStride<StorageOrder == ColMajor ? 1 : Dynamic> ColInnerStride;
+  typedef InnerStride<StorageOrder == ColMajor ? Dynamic : 1> RowInnerStride;
+  typedef Ref<Matrix<Scalar, Dynamic, 1>, 0, ColInnerStride> SubColumnType;
+  typedef Ref<Matrix<Scalar, 1, Dynamic>, 0, RowInnerStride> SubRowType;
+  typedef Ref<Matrix<Scalar, Dynamic, Dynamic, StorageOrder> > SubMatType;
+
   Index brows = A.rows();
   Index bcols = A.cols();
 
   Scalar tau_u, tau_u_prev(0), tau_v;
 
-  for(Index k = 0; k < bs; ++k)
-  {
+  for (Index k = 0; k < bs; ++k) {
     Index remainingRows = brows - k;
     Index remainingCols = bcols - k - 1;
 
-    SubMatType X_k1( X.block(k,0, remainingRows,k) );
-    SubMatType V_k1( A.block(k,0, remainingRows,k) );
+    SubMatType X_k1(X.block(k, 0, remainingRows, k));
+    SubMatType V_k1(A.block(k, 0, remainingRows, k));
 
     // 1 - update the k-th column of A
     SubColumnType v_k = A.col(k).tail(remainingRows);
-          v_k -= V_k1 * Y.row(k).head(k).adjoint();
-    if(k) v_k -= X_k1 * A.col(k).head(k);
-    
+    v_k -= V_k1 * Y.row(k).head(k).adjoint();
+    if (k) v_k -= X_k1 * A.col(k).head(k);
+
     // 2 - construct left Householder transform in-place
     v_k.makeHouseholderInPlace(tau_v, diagonal[k]);
-       
-    if(k+1<bcols)
-    {
-      SubMatType Y_k  ( Y.block(k+1,0, remainingCols, k+1) );
-      SubMatType U_k1 ( A.block(0,k+1, k,remainingCols) );
-      
+
+    if (k + 1 < bcols) {
+      SubMatType Y_k(Y.block(k + 1, 0, remainingCols, k + 1));
+      SubMatType U_k1(A.block(0, k + 1, k, remainingCols));
+
       // this eases the application of Householder transforAions
       // A(k,k) will store tau_v later
-      A(k,k) = Scalar(1);
+      A(k, k) = Scalar(1);
 
       // 3 - Compute y_k^T = tau_v * ( A^T*v_k - Y_k-1*V_k-1^T*v_k - U_k-1*X_k-1^T*v_k )
       {
-        SubColumnType y_k( Y.col(k).tail(remainingCols) );
-        
+        SubColumnType y_k(Y.col(k).tail(remainingCols));
+
         // let's use the beginning of column k of Y as a temporary vector
-        SubColumnType tmp( Y.col(k).head(k) );
-        y_k.noalias()  = A.block(k,k+1, remainingRows,remainingCols).adjoint() * v_k; // bottleneck
-        tmp.noalias()  = V_k1.adjoint()  * v_k;
+        SubColumnType tmp(Y.col(k).head(k));
+        y_k.noalias() = A.block(k, k + 1, remainingRows, remainingCols).adjoint() * v_k;  // bottleneck
+        tmp.noalias() = V_k1.adjoint() * v_k;
         y_k.noalias() -= Y_k.leftCols(k) * tmp;
-        tmp.noalias()  = X_k1.adjoint()  * v_k;
-        y_k.noalias() -= U_k1.adjoint()  * tmp;
+        tmp.noalias() = X_k1.adjoint() * v_k;
+        y_k.noalias() -= U_k1.adjoint() * tmp;
         y_k *= numext::conj(tau_v);
       }
 
       // 4 - update k-th row of A (it will become u_k)
-      SubRowType u_k( A.row(k).tail(remainingCols) );
+      SubRowType u_k(A.row(k).tail(remainingCols));
       u_k = u_k.conjugate();
       {
-        u_k -= Y_k * A.row(k).head(k+1).adjoint();
-        if(k) u_k -= U_k1.adjoint() * X.row(k).head(k).adjoint();
+        u_k -= Y_k * A.row(k).head(k + 1).adjoint();
+        if (k) u_k -= U_k1.adjoint() * X.row(k).head(k).adjoint();
       }
 
       // 5 - construct right Householder transform in-place
@@ -225,96 +212,82 @@
 
       // this eases the application of Householder transformations
       // A(k,k+1) will store tau_u later
-      A(k,k+1) = Scalar(1);
+      A(k, k + 1) = Scalar(1);
 
       // 6 - Compute x_k = tau_u * ( A*u_k - X_k-1*U_k-1^T*u_k - V_k*Y_k^T*u_k )
       {
-        SubColumnType x_k ( X.col(k).tail(remainingRows-1) );
-        
+        SubColumnType x_k(X.col(k).tail(remainingRows - 1));
+
         // let's use the beginning of column k of X as a temporary vectors
         // note that tmp0 and tmp1 overlaps
-        SubColumnType tmp0 ( X.col(k).head(k) ),
-                      tmp1 ( X.col(k).head(k+1) );
-                    
-        x_k.noalias()   = A.block(k+1,k+1, remainingRows-1,remainingCols) * u_k.transpose(); // bottleneck
-        tmp0.noalias()  = U_k1 * u_k.transpose();
-        x_k.noalias()  -= X_k1.bottomRows(remainingRows-1) * tmp0;
-        tmp1.noalias()  = Y_k.adjoint() * u_k.transpose();
-        x_k.noalias()  -= A.block(k+1,0, remainingRows-1,k+1) * tmp1;
+        SubColumnType tmp0(X.col(k).head(k)), tmp1(X.col(k).head(k + 1));
+
+        x_k.noalias() = A.block(k + 1, k + 1, remainingRows - 1, remainingCols) * u_k.transpose();  // bottleneck
+        tmp0.noalias() = U_k1 * u_k.transpose();
+        x_k.noalias() -= X_k1.bottomRows(remainingRows - 1) * tmp0;
+        tmp1.noalias() = Y_k.adjoint() * u_k.transpose();
+        x_k.noalias() -= A.block(k + 1, 0, remainingRows - 1, k + 1) * tmp1;
         x_k *= numext::conj(tau_u);
         tau_u = numext::conj(tau_u);
         u_k = u_k.conjugate();
       }
 
-      if(k>0) A.coeffRef(k-1,k) = tau_u_prev;
+      if (k > 0) A.coeffRef(k - 1, k) = tau_u_prev;
       tau_u_prev = tau_u;
-    }
-    else
-      A.coeffRef(k-1,k) = tau_u_prev;
+    } else
+      A.coeffRef(k - 1, k) = tau_u_prev;
 
-    A.coeffRef(k,k) = tau_v;
+    A.coeffRef(k, k) = tau_v;
   }
-  
-  if(bs<bcols)
-    A.coeffRef(bs-1,bs) = tau_u_prev;
+
+  if (bs < bcols) A.coeffRef(bs - 1, bs) = tau_u_prev;
 
   // update A22
-  if(bcols>bs && brows>bs)
-  {
-    SubMatType A11( A.bottomRightCorner(brows-bs,bcols-bs) );
-    SubMatType A10( A.block(bs,0, brows-bs,bs) );
-    SubMatType A01( A.block(0,bs, bs,bcols-bs) );
-    Scalar tmp = A01(bs-1,0);
-    A01(bs-1,0) = Literal(1);
-    A11.noalias() -= A10 * Y.topLeftCorner(bcols,bs).bottomRows(bcols-bs).adjoint();
-    A11.noalias() -= X.topLeftCorner(brows,bs).bottomRows(brows-bs) * A01;
-    A01(bs-1,0) = tmp;
+  if (bcols > bs && brows > bs) {
+    SubMatType A11(A.bottomRightCorner(brows - bs, bcols - bs));
+    SubMatType A10(A.block(bs, 0, brows - bs, bs));
+    SubMatType A01(A.block(0, bs, bs, bcols - bs));
+    Scalar tmp = A01(bs - 1, 0);
+    A01(bs - 1, 0) = Literal(1);
+    A11.noalias() -= A10 * Y.topLeftCorner(bcols, bs).bottomRows(bcols - bs).adjoint();
+    A11.noalias() -= X.topLeftCorner(brows, bs).bottomRows(brows - bs) * A01;
+    A01(bs - 1, 0) = tmp;
   }
 }
 
 /** \internal
-  *
-  * Implementation of a block-bidiagonal reduction.
-  * It is based on the following paper:
-  *   The Design of a Parallel Dense Linear Algebra Software Library: Reduction to Hessenberg, Tridiagonal, and Bidiagonal Form.
-  *   by Jaeyoung Choi, Jack J. Dongarra, David W. Walker. (1995)
-  *   section 3.3
-  */
-template<typename MatrixType, typename BidiagType>
-void upperbidiagonalization_inplace_blocked(MatrixType& A, BidiagType& bidiagonal,
-                                            Index maxBlockSize=32,
-                                            typename MatrixType::Scalar* /*tempData*/ = 0)
-{
+ *
+ * Implementation of a block-bidiagonal reduction.
+ * It is based on the following paper:
+ *   The Design of a Parallel Dense Linear Algebra Software Library: Reduction to Hessenberg, Tridiagonal, and
+ * Bidiagonal Form. by Jaeyoung Choi, Jack J. Dongarra, David W. Walker. (1995) section 3.3
+ */
+template <typename MatrixType, typename BidiagType>
+void upperbidiagonalization_inplace_blocked(MatrixType& A, BidiagType& bidiagonal, Index maxBlockSize = 32,
+                                            typename MatrixType::Scalar* /*tempData*/ = 0) {
   typedef typename MatrixType::Scalar Scalar;
-  typedef Block<MatrixType,Dynamic,Dynamic> BlockType;
+  typedef Block<MatrixType, Dynamic, Dynamic> BlockType;
 
   Index rows = A.rows();
   Index cols = A.cols();
   Index size = (std::min)(rows, cols);
 
   // X and Y are work space
-  enum { StorageOrder = traits<MatrixType>::Flags & RowMajorBit };
-  Matrix<Scalar,
-         MatrixType::RowsAtCompileTime,
-         Dynamic,
-         StorageOrder,
-         MatrixType::MaxRowsAtCompileTime> X(rows,maxBlockSize);
-  Matrix<Scalar,
-         MatrixType::ColsAtCompileTime,
-         Dynamic,
-         StorageOrder,
-         MatrixType::MaxColsAtCompileTime> Y(cols,maxBlockSize);
-  Index blockSize = (std::min)(maxBlockSize,size);
+  static constexpr int StorageOrder = (traits<MatrixType>::Flags & RowMajorBit) ? RowMajor : ColMajor;
+  Matrix<Scalar, MatrixType::RowsAtCompileTime, Dynamic, StorageOrder, MatrixType::MaxRowsAtCompileTime> X(
+      rows, maxBlockSize);
+  Matrix<Scalar, MatrixType::ColsAtCompileTime, Dynamic, StorageOrder, MatrixType::MaxColsAtCompileTime> Y(
+      cols, maxBlockSize);
+  Index blockSize = (std::min)(maxBlockSize, size);
 
   Index k = 0;
-  for(k = 0; k < size; k += blockSize)
-  {
-    Index bs = (std::min)(size-k,blockSize);  // actual size of the block
-    Index brows = rows - k;                   // rows of the block
-    Index bcols = cols - k;                   // columns of the block
+  for (k = 0; k < size; k += blockSize) {
+    Index bs = (std::min)(size - k, blockSize);  // actual size of the block
+    Index brows = rows - k;                      // rows of the block
+    Index bcols = cols - k;                      // columns of the block
 
     // partition the matrix A:
-    // 
+    //
     //      | A00 A01 A02 |
     //      |             |
     // A  = | A10 A11 A12 |
@@ -327,37 +300,32 @@
     //  B = |         |
     //      | A21 A22 |
 
-    BlockType B = A.block(k,k,brows,bcols);
-    
+    BlockType B = A.block(k, k, brows, bcols);
+
     // This stage performs the bidiagonalization of A11, A21, A12, and updating of A22.
     // Finally, the algorithm continue on the updated A22.
     //
     // However, if B is too small, or A22 empty, then let's use an unblocked strategy
-    if(k+bs==cols || bcols<48) // somewhat arbitrary threshold
+
+    auto upper_diagonal = bidiagonal.template diagonal<1>();
+    typename MatrixType::RealScalar* upper_diagonal_ptr =
+        upper_diagonal.size() > 0 ? &upper_diagonal.coeffRef(k) : nullptr;
+
+    if (k + bs == cols || bcols < 48)  // somewhat arbitrary threshold
     {
-      upperbidiagonalization_inplace_unblocked(B,
-                                               &(bidiagonal.template diagonal<0>().coeffRef(k)),
-                                               &(bidiagonal.template diagonal<1>().coeffRef(k)),
-                                               X.data()
-                                              );
-      break; // We're done
-    }
-    else
-    {
-      upperbidiagonalization_blocked_helper<BlockType>( B,
-                                                        &(bidiagonal.template diagonal<0>().coeffRef(k)),
-                                                        &(bidiagonal.template diagonal<1>().coeffRef(k)),
-                                                        bs,
-                                                        X.topLeftCorner(brows,bs),
-                                                        Y.topLeftCorner(bcols,bs)
-                                                      );
+      upperbidiagonalization_inplace_unblocked(B, &(bidiagonal.template diagonal<0>().coeffRef(k)), upper_diagonal_ptr,
+                                               X.data());
+      break;  // We're done
+    } else {
+      upperbidiagonalization_blocked_helper<BlockType>(B, &(bidiagonal.template diagonal<0>().coeffRef(k)),
+                                                       upper_diagonal_ptr, bs, X.topLeftCorner(brows, bs),
+                                                       Y.topLeftCorner(bcols, bs));
     }
   }
 }
 
-template<typename _MatrixType>
-UpperBidiagonalization<_MatrixType>& UpperBidiagonalization<_MatrixType>::computeUnblocked(const _MatrixType& matrix)
-{
+template <typename MatrixType_>
+UpperBidiagonalization<MatrixType_>& UpperBidiagonalization<MatrixType_>::computeUnblocked(const MatrixType_& matrix) {
   Index rows = matrix.rows();
   Index cols = matrix.cols();
   EIGEN_ONLY_USED_FOR_DEBUG(cols);
@@ -368,18 +336,15 @@
 
   ColVectorType temp(rows);
 
-  upperbidiagonalization_inplace_unblocked(m_householder,
-                                           &(m_bidiagonal.template diagonal<0>().coeffRef(0)),
-                                           &(m_bidiagonal.template diagonal<1>().coeffRef(0)),
-                                           temp.data());
+  upperbidiagonalization_inplace_unblocked(m_householder, &(m_bidiagonal.template diagonal<0>().coeffRef(0)),
+                                           &(m_bidiagonal.template diagonal<1>().coeffRef(0)), temp.data());
 
   m_isInitialized = true;
   return *this;
 }
 
-template<typename _MatrixType>
-UpperBidiagonalization<_MatrixType>& UpperBidiagonalization<_MatrixType>::compute(const _MatrixType& matrix)
-{
+template <typename MatrixType_>
+UpperBidiagonalization<MatrixType_>& UpperBidiagonalization<MatrixType_>::compute(const MatrixType_& matrix) {
   Index rows = matrix.rows();
   Index cols = matrix.cols();
   EIGEN_ONLY_USED_FOR_DEBUG(rows);
@@ -389,7 +354,7 @@
 
   m_householder = matrix;
   upperbidiagonalization_inplace_blocked(m_householder, m_bidiagonal);
-            
+
   m_isInitialized = true;
   return *this;
 }
@@ -407,8 +372,8 @@
 }
 #endif
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_BIDIAGONALIZATION_H
+#endif  // EIGEN_BIDIAGONALIZATION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/InternalHeaderCheck.h
new file mode 100644
index 0000000..f8d8762
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_SPARSECHOLESKY_MODULE_H
+#error "Please include Eigen/SparseCholesky instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h
index 9f93e32..423287b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h
@@ -10,531 +10,482 @@
 #ifndef EIGEN_SIMPLICIAL_CHOLESKY_H
 #define EIGEN_SIMPLICIAL_CHOLESKY_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-enum SimplicialCholeskyMode {
-  SimplicialCholeskyLLT,
-  SimplicialCholeskyLDLT
-};
+namespace Eigen {
+
+enum SimplicialCholeskyMode { SimplicialCholeskyLLT, SimplicialCholeskyLDLT };
 
 namespace internal {
-  template<typename CholMatrixType, typename InputMatrixType>
-  struct simplicial_cholesky_grab_input {
-    typedef CholMatrixType const * ConstCholMatrixPtr;
-    static void run(const InputMatrixType& input, ConstCholMatrixPtr &pmat, CholMatrixType &tmp)
-    {
-      tmp = input;
-      pmat = &tmp;
-    }
-  };
-  
-  template<typename MatrixType>
-  struct simplicial_cholesky_grab_input<MatrixType,MatrixType> {
-    typedef MatrixType const * ConstMatrixPtr;
-    static void run(const MatrixType& input, ConstMatrixPtr &pmat, MatrixType &/*tmp*/)
-    {
-      pmat = &input;
-    }
-  };
-} // end namespace internal
+template <typename CholMatrixType, typename InputMatrixType>
+struct simplicial_cholesky_grab_input {
+  typedef CholMatrixType const* ConstCholMatrixPtr;
+  static void run(const InputMatrixType& input, ConstCholMatrixPtr& pmat, CholMatrixType& tmp) {
+    tmp = input;
+    pmat = &tmp;
+  }
+};
+
+template <typename MatrixType>
+struct simplicial_cholesky_grab_input<MatrixType, MatrixType> {
+  typedef MatrixType const* ConstMatrixPtr;
+  static void run(const MatrixType& input, ConstMatrixPtr& pmat, MatrixType& /*tmp*/) { pmat = &input; }
+};
+}  // end namespace internal
 
 /** \ingroup SparseCholesky_Module
-  * \brief A base class for direct sparse Cholesky factorizations
-  *
-  * This is a base class for LL^T and LDL^T Cholesky factorizations of sparse matrices that are
-  * selfadjoint and positive definite. These factorizations allow for solving A.X = B where
-  * X and B can be either dense or sparse.
-  * 
-  * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
-  * such that the factorized matrix is P A P^-1.
-  *
-  * \tparam Derived the type of the derived class, that is the actual factorization type.
-  *
-  */
-template<typename Derived>
-class SimplicialCholeskyBase : public SparseSolverBase<Derived>
-{
-    typedef SparseSolverBase<Derived> Base;
-    using Base::m_isInitialized;
-    
-  public:
-    typedef typename internal::traits<Derived>::MatrixType MatrixType;
-    typedef typename internal::traits<Derived>::OrderingType OrderingType;
-    enum { UpLo = internal::traits<Derived>::UpLo };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
-    typedef CholMatrixType const * ConstCholMatrixPtr;
-    typedef Matrix<Scalar,Dynamic,1> VectorType;
-    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+ * \brief A base class for direct sparse Cholesky factorizations
+ *
+ * This is a base class for LL^T and LDL^T Cholesky factorizations of sparse matrices that are
+ * selfadjoint and positive definite. These factorizations allow for solving A.X = B where
+ * X and B can be either dense or sparse.
+ *
+ * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+ * such that the factorized matrix is P A P^-1.
+ *
+ * \tparam Derived the type of the derived class, that is the actual factorization type.
+ *
+ */
+template <typename Derived>
+class SimplicialCholeskyBase : public SparseSolverBase<Derived> {
+  typedef SparseSolverBase<Derived> Base;
+  using Base::m_isInitialized;
 
-    enum {
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
+ public:
+  typedef typename internal::traits<Derived>::MatrixType MatrixType;
+  typedef typename internal::traits<Derived>::OrderingType OrderingType;
+  enum { UpLo = internal::traits<Derived>::UpLo };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> CholMatrixType;
+  typedef CholMatrixType const* ConstCholMatrixPtr;
+  typedef Matrix<Scalar, Dynamic, 1> VectorType;
+  typedef Matrix<StorageIndex, Dynamic, 1> VectorI;
 
-  public:
-    
-    using Base::derived;
+  enum { ColsAtCompileTime = MatrixType::ColsAtCompileTime, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime };
 
-    /** Default constructor */
-    SimplicialCholeskyBase()
-      : m_info(Success),
-        m_factorizationIsOk(false),
-        m_analysisIsOk(false),
-        m_shiftOffset(0),
-        m_shiftScale(1)
-    {}
+ public:
+  using Base::derived;
 
-    explicit SimplicialCholeskyBase(const MatrixType& matrix)
-      : m_info(Success),
-        m_factorizationIsOk(false),
-        m_analysisIsOk(false),
-        m_shiftOffset(0),
-        m_shiftScale(1)
-    {
-      derived().compute(matrix);
-    }
+  /** Default constructor */
+  SimplicialCholeskyBase()
+      : m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false), m_shiftOffset(0), m_shiftScale(1) {}
 
-    ~SimplicialCholeskyBase()
-    {
-    }
+  explicit SimplicialCholeskyBase(const MatrixType& matrix)
+      : m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false), m_shiftOffset(0), m_shiftScale(1) {
+    derived().compute(matrix);
+  }
 
-    Derived& derived() { return *static_cast<Derived*>(this); }
-    const Derived& derived() const { return *static_cast<const Derived*>(this); }
-    
-    inline Index cols() const { return m_matrix.cols(); }
-    inline Index rows() const { return m_matrix.rows(); }
-    
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful,
-      *          \c NumericalIssue if the matrix.appears to be negative.
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
-      return m_info;
-    }
-    
-    /** \returns the permutation P
-      * \sa permutationPinv() */
-    const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& permutationP() const
-    { return m_P; }
-    
-    /** \returns the inverse P^-1 of the permutation P
-      * \sa permutationP() */
-    const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& permutationPinv() const
-    { return m_Pinv; }
+  ~SimplicialCholeskyBase() {}
 
-    /** Sets the shift parameters that will be used to adjust the diagonal coefficients during the numerical factorization.
-      *
-      * During the numerical factorization, the diagonal coefficients are transformed by the following linear model:\n
-      * \c d_ii = \a offset + \a scale * \c d_ii
-      *
-      * The default is the identity transformation with \a offset=0, and \a scale=1.
-      *
-      * \returns a reference to \c *this.
-      */
-    Derived& setShift(const RealScalar& offset, const RealScalar& scale = 1)
-    {
-      m_shiftOffset = offset;
-      m_shiftScale = scale;
-      return derived();
-    }
+  Derived& derived() { return *static_cast<Derived*>(this); }
+  const Derived& derived() const { return *static_cast<const Derived*>(this); }
+
+  inline Index cols() const { return m_matrix.cols(); }
+  inline Index rows() const { return m_matrix.rows(); }
+
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful,
+   *          \c NumericalIssue if the matrix.appears to be negative.
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+    return m_info;
+  }
+
+  /** \returns the permutation P
+   * \sa permutationPinv() */
+  const PermutationMatrix<Dynamic, Dynamic, StorageIndex>& permutationP() const { return m_P; }
+
+  /** \returns the inverse P^-1 of the permutation P
+   * \sa permutationP() */
+  const PermutationMatrix<Dynamic, Dynamic, StorageIndex>& permutationPinv() const { return m_Pinv; }
+
+  /** Sets the shift parameters that will be used to adjust the diagonal coefficients during the numerical
+   * factorization.
+   *
+   * During the numerical factorization, the diagonal coefficients are transformed by the following linear model:\n
+   * \c d_ii = \a offset + \a scale * \c d_ii
+   *
+   * The default is the identity transformation with \a offset=0, and \a scale=1.
+   *
+   * \returns a reference to \c *this.
+   */
+  Derived& setShift(const RealScalar& offset, const RealScalar& scale = 1) {
+    m_shiftOffset = offset;
+    m_shiftScale = scale;
+    return derived();
+  }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal */
-    template<typename Stream>
-    void dumpMemory(Stream& s)
-    {
-      int total = 0;
-      s << "  L:        " << ((total+=(m_matrix.cols()+1) * sizeof(int) + m_matrix.nonZeros()*(sizeof(int)+sizeof(Scalar))) >> 20) << "Mb" << "\n";
-      s << "  diag:     " << ((total+=m_diag.size() * sizeof(Scalar)) >> 20) << "Mb" << "\n";
-      s << "  tree:     " << ((total+=m_parent.size() * sizeof(int)) >> 20) << "Mb" << "\n";
-      s << "  nonzeros: " << ((total+=m_nonZerosPerCol.size() * sizeof(int)) >> 20) << "Mb" << "\n";
-      s << "  perm:     " << ((total+=m_P.size() * sizeof(int)) >> 20) << "Mb" << "\n";
-      s << "  perm^-1:  " << ((total+=m_Pinv.size() * sizeof(int)) >> 20) << "Mb" << "\n";
-      s << "  TOTAL:    " << (total>> 20) << "Mb" << "\n";
+  /** \internal */
+  template <typename Stream>
+  void dumpMemory(Stream& s) {
+    int total = 0;
+    s << "  L:        "
+      << ((total += (m_matrix.cols() + 1) * sizeof(int) + m_matrix.nonZeros() * (sizeof(int) + sizeof(Scalar))) >> 20)
+      << "Mb"
+      << "\n";
+    s << "  diag:     " << ((total += m_diag.size() * sizeof(Scalar)) >> 20) << "Mb"
+      << "\n";
+    s << "  tree:     " << ((total += m_parent.size() * sizeof(int)) >> 20) << "Mb"
+      << "\n";
+    s << "  nonzeros: " << ((total += m_nonZerosPerCol.size() * sizeof(int)) >> 20) << "Mb"
+      << "\n";
+    s << "  perm:     " << ((total += m_P.size() * sizeof(int)) >> 20) << "Mb"
+      << "\n";
+    s << "  perm^-1:  " << ((total += m_Pinv.size() * sizeof(int)) >> 20) << "Mb"
+      << "\n";
+    s << "  TOTAL:    " << (total >> 20) << "Mb"
+      << "\n";
+  }
+
+  /** \internal */
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const MatrixBase<Rhs>& b, MatrixBase<Dest>& dest) const {
+    eigen_assert(m_factorizationIsOk &&
+                 "The decomposition is not in a valid state for solving, you must first call either compute() or "
+                 "symbolic()/numeric()");
+    eigen_assert(m_matrix.rows() == b.rows());
+
+    if (m_info != Success) return;
+
+    if (m_P.size() > 0)
+      dest = m_P * b;
+    else
+      dest = b;
+
+    if (m_matrix.nonZeros() > 0)  // otherwise L==I
+      derived().matrixL().solveInPlace(dest);
+
+    if (m_diag.size() > 0) dest = m_diag.asDiagonal().inverse() * dest;
+
+    if (m_matrix.nonZeros() > 0)  // otherwise U==I
+      derived().matrixU().solveInPlace(dest);
+
+    if (m_P.size() > 0) dest = m_Pinv * dest;
+  }
+
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const SparseMatrixBase<Rhs>& b, SparseMatrixBase<Dest>& dest) const {
+    internal::solve_sparse_through_dense_panels(derived(), b, dest);
+  }
+
+#endif  // EIGEN_PARSED_BY_DOXYGEN
+
+ protected:
+  /** Computes the sparse Cholesky decomposition of \a matrix */
+  template <bool DoLDLT>
+  void compute(const MatrixType& matrix) {
+    eigen_assert(matrix.rows() == matrix.cols());
+    Index size = matrix.cols();
+    CholMatrixType tmp(size, size);
+    ConstCholMatrixPtr pmat;
+    ordering(matrix, pmat, tmp);
+    analyzePattern_preordered(*pmat, DoLDLT);
+    factorize_preordered<DoLDLT>(*pmat);
+  }
+
+  template <bool DoLDLT>
+  void factorize(const MatrixType& a) {
+    eigen_assert(a.rows() == a.cols());
+    Index size = a.cols();
+    CholMatrixType tmp(size, size);
+    ConstCholMatrixPtr pmat;
+
+    if (m_P.size() == 0 && (int(UpLo) & int(Upper)) == Upper) {
+      // If there is no ordering, try to directly use the input matrix without any copy
+      internal::simplicial_cholesky_grab_input<CholMatrixType, MatrixType>::run(a, pmat, tmp);
+    } else {
+      tmp.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>().twistedBy(m_P);
+      pmat = &tmp;
     }
 
-    /** \internal */
-    template<typename Rhs,typename Dest>
-    void _solve_impl(const MatrixBase<Rhs> &b, MatrixBase<Dest> &dest) const
-    {
-      eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
-      eigen_assert(m_matrix.rows()==b.rows());
+    factorize_preordered<DoLDLT>(*pmat);
+  }
 
-      if(m_info!=Success)
-        return;
+  template <bool DoLDLT>
+  void factorize_preordered(const CholMatrixType& a);
 
-      if(m_P.size()>0)
-        dest = m_P * b;
-      else
-        dest = b;
+  void analyzePattern(const MatrixType& a, bool doLDLT) {
+    eigen_assert(a.rows() == a.cols());
+    Index size = a.cols();
+    CholMatrixType tmp(size, size);
+    ConstCholMatrixPtr pmat;
+    ordering(a, pmat, tmp);
+    analyzePattern_preordered(*pmat, doLDLT);
+  }
+  void analyzePattern_preordered(const CholMatrixType& a, bool doLDLT);
 
-      if(m_matrix.nonZeros()>0) // otherwise L==I
-        derived().matrixL().solveInPlace(dest);
+  void ordering(const MatrixType& a, ConstCholMatrixPtr& pmat, CholMatrixType& ap);
 
-      if(m_diag.size()>0)
-        dest = m_diag.asDiagonal().inverse() * dest;
+  /** keeps off-diagonal entries; drops diagonal entries */
+  struct keep_diag {
+    inline bool operator()(const Index& row, const Index& col, const Scalar&) const { return row != col; }
+  };
 
-      if (m_matrix.nonZeros()>0) // otherwise U==I
-        derived().matrixU().solveInPlace(dest);
+  mutable ComputationInfo m_info;
+  bool m_factorizationIsOk;
+  bool m_analysisIsOk;
 
-      if(m_P.size()>0)
-        dest = m_Pinv * dest;
-    }
-    
-    template<typename Rhs,typename Dest>
-    void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
-    {
-      internal::solve_sparse_through_dense_panels(derived(), b, dest);
-    }
+  CholMatrixType m_matrix;
+  VectorType m_diag;  // the diagonal coefficients (LDLT mode)
+  VectorI m_parent;   // elimination tree
+  VectorI m_nonZerosPerCol;
+  PermutationMatrix<Dynamic, Dynamic, StorageIndex> m_P;     // the permutation
+  PermutationMatrix<Dynamic, Dynamic, StorageIndex> m_Pinv;  // the inverse permutation
 
-#endif // EIGEN_PARSED_BY_DOXYGEN
-
-  protected:
-    
-    /** Computes the sparse Cholesky decomposition of \a matrix */
-    template<bool DoLDLT>
-    void compute(const MatrixType& matrix)
-    {
-      eigen_assert(matrix.rows()==matrix.cols());
-      Index size = matrix.cols();
-      CholMatrixType tmp(size,size);
-      ConstCholMatrixPtr pmat;
-      ordering(matrix, pmat, tmp);
-      analyzePattern_preordered(*pmat, DoLDLT);
-      factorize_preordered<DoLDLT>(*pmat);
-    }
-    
-    template<bool DoLDLT>
-    void factorize(const MatrixType& a)
-    {
-      eigen_assert(a.rows()==a.cols());
-      Index size = a.cols();
-      CholMatrixType tmp(size,size);
-      ConstCholMatrixPtr pmat;
-      
-      if(m_P.size() == 0 && (int(UpLo) & int(Upper)) == Upper)
-      {
-        // If there is no ordering, try to directly use the input matrix without any copy
-        internal::simplicial_cholesky_grab_input<CholMatrixType,MatrixType>::run(a, pmat, tmp);
-      }
-      else
-      {
-        tmp.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>().twistedBy(m_P);
-        pmat = &tmp;
-      }
-      
-      factorize_preordered<DoLDLT>(*pmat);
-    }
-
-    template<bool DoLDLT>
-    void factorize_preordered(const CholMatrixType& a);
-
-    void analyzePattern(const MatrixType& a, bool doLDLT)
-    {
-      eigen_assert(a.rows()==a.cols());
-      Index size = a.cols();
-      CholMatrixType tmp(size,size);
-      ConstCholMatrixPtr pmat;
-      ordering(a, pmat, tmp);
-      analyzePattern_preordered(*pmat,doLDLT);
-    }
-    void analyzePattern_preordered(const CholMatrixType& a, bool doLDLT);
-    
-    void ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap);
-
-    /** keeps off-diagonal entries; drops diagonal entries */
-    struct keep_diag {
-      inline bool operator() (const Index& row, const Index& col, const Scalar&) const
-      {
-        return row!=col;
-      }
-    };
-
-    mutable ComputationInfo m_info;
-    bool m_factorizationIsOk;
-    bool m_analysisIsOk;
-    
-    CholMatrixType m_matrix;
-    VectorType m_diag;                                // the diagonal coefficients (LDLT mode)
-    VectorI m_parent;                                 // elimination tree
-    VectorI m_nonZerosPerCol;
-    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_P;     // the permutation
-    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_Pinv;  // the inverse permutation
-
-    RealScalar m_shiftOffset;
-    RealScalar m_shiftScale;
+  RealScalar m_shiftOffset;
+  RealScalar m_shiftScale;
 };
 
-template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialLLT;
-template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialLDLT;
-template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialCholesky;
+template <typename MatrixType_, int UpLo_ = Lower,
+          typename Ordering_ = AMDOrdering<typename MatrixType_::StorageIndex> >
+class SimplicialLLT;
+template <typename MatrixType_, int UpLo_ = Lower,
+          typename Ordering_ = AMDOrdering<typename MatrixType_::StorageIndex> >
+class SimplicialLDLT;
+template <typename MatrixType_, int UpLo_ = Lower,
+          typename Ordering_ = AMDOrdering<typename MatrixType_::StorageIndex> >
+class SimplicialCholesky;
 
 namespace internal {
 
-template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
-{
-  typedef _MatrixType MatrixType;
-  typedef _Ordering OrderingType;
-  enum { UpLo = _UpLo };
-  typedef typename MatrixType::Scalar                         Scalar;
-  typedef typename MatrixType::StorageIndex                   StorageIndex;
-  typedef SparseMatrix<Scalar, ColMajor, StorageIndex>        CholMatrixType;
-  typedef TriangularView<const CholMatrixType, Eigen::Lower>  MatrixL;
-  typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::Upper>   MatrixU;
+template <typename MatrixType_, int UpLo_, typename Ordering_>
+struct traits<SimplicialLLT<MatrixType_, UpLo_, Ordering_> > {
+  typedef MatrixType_ MatrixType;
+  typedef Ordering_ OrderingType;
+  enum { UpLo = UpLo_ };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> CholMatrixType;
+  typedef TriangularView<const CholMatrixType, Eigen::Lower> MatrixL;
+  typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::Upper> MatrixU;
   static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); }
   static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); }
 };
 
-template<typename _MatrixType,int _UpLo, typename _Ordering> struct traits<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
-{
-  typedef _MatrixType MatrixType;
-  typedef _Ordering OrderingType;
-  enum { UpLo = _UpLo };
-  typedef typename MatrixType::Scalar                             Scalar;
-  typedef typename MatrixType::StorageIndex                       StorageIndex;
-  typedef SparseMatrix<Scalar, ColMajor, StorageIndex>            CholMatrixType;
-  typedef TriangularView<const CholMatrixType, Eigen::UnitLower>  MatrixL;
+template <typename MatrixType_, int UpLo_, typename Ordering_>
+struct traits<SimplicialLDLT<MatrixType_, UpLo_, Ordering_> > {
+  typedef MatrixType_ MatrixType;
+  typedef Ordering_ OrderingType;
+  enum { UpLo = UpLo_ };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> CholMatrixType;
+  typedef TriangularView<const CholMatrixType, Eigen::UnitLower> MatrixL;
   typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::UnitUpper> MatrixU;
   static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); }
   static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); }
 };
 
-template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
-{
-  typedef _MatrixType MatrixType;
-  typedef _Ordering OrderingType;
-  enum { UpLo = _UpLo };
+template <typename MatrixType_, int UpLo_, typename Ordering_>
+struct traits<SimplicialCholesky<MatrixType_, UpLo_, Ordering_> > {
+  typedef MatrixType_ MatrixType;
+  typedef Ordering_ OrderingType;
+  enum { UpLo = UpLo_ };
 };
 
-}
+}  // namespace internal
 
 /** \ingroup SparseCholesky_Module
-  * \class SimplicialLLT
-  * \brief A direct sparse LLT Cholesky factorizations
-  *
-  * This class provides a LL^T Cholesky factorizations of sparse matrices that are
-  * selfadjoint and positive definite. The factorization allows for solving A.X = B where
-  * X and B can be either dense or sparse.
-  * 
-  * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
-  * such that the factorized matrix is P A P^-1.
-  *
-  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
-  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
-  *               or Upper. Default is Lower.
-  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
-  *
-  * \implsparsesolverconcept
-  *
-  * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering
-  */
-template<typename _MatrixType, int _UpLo, typename _Ordering>
-    class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
-{
-public:
-    typedef _MatrixType MatrixType;
-    enum { UpLo = _UpLo };
-    typedef SimplicialCholeskyBase<SimplicialLLT> Base;
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef SparseMatrix<Scalar,ColMajor,Index> CholMatrixType;
-    typedef Matrix<Scalar,Dynamic,1> VectorType;
-    typedef internal::traits<SimplicialLLT> Traits;
-    typedef typename Traits::MatrixL  MatrixL;
-    typedef typename Traits::MatrixU  MatrixU;
-public:
-    /** Default constructor */
-    SimplicialLLT() : Base() {}
-    /** Constructs and performs the LLT factorization of \a matrix */
-    explicit SimplicialLLT(const MatrixType& matrix)
-        : Base(matrix) {}
+ * \class SimplicialLLT
+ * \brief A direct sparse LLT Cholesky factorizations
+ *
+ * This class provides a LL^T Cholesky factorizations of sparse matrices that are
+ * selfadjoint and positive definite. The factorization allows for solving A.X = B where
+ * X and B can be either dense or sparse.
+ *
+ * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+ * such that the factorized matrix is P A P^-1.
+ *
+ * \tparam MatrixType_ the type of the sparse matrix A, it must be a SparseMatrix<>
+ * \tparam UpLo_ the triangular part that will be used for the computations. It can be Lower
+ *               or Upper. Default is Lower.
+ * \tparam Ordering_ The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering
+ */
+template <typename MatrixType_, int UpLo_, typename Ordering_>
+class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<MatrixType_, UpLo_, Ordering_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum { UpLo = UpLo_ };
+  typedef SimplicialCholeskyBase<SimplicialLLT> Base;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, Index> CholMatrixType;
+  typedef Matrix<Scalar, Dynamic, 1> VectorType;
+  typedef internal::traits<SimplicialLLT> Traits;
+  typedef typename Traits::MatrixL MatrixL;
+  typedef typename Traits::MatrixU MatrixU;
 
-    /** \returns an expression of the factor L */
-    inline const MatrixL matrixL() const {
-        eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
-        return Traits::getL(Base::m_matrix);
-    }
+ public:
+  /** Default constructor */
+  SimplicialLLT() : Base() {}
+  /** Constructs and performs the LLT factorization of \a matrix */
+  explicit SimplicialLLT(const MatrixType& matrix) : Base(matrix) {}
 
-    /** \returns an expression of the factor U (= L^*) */
-    inline const MatrixU matrixU() const {
-        eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
-        return Traits::getU(Base::m_matrix);
-    }
-    
-    /** Computes the sparse Cholesky decomposition of \a matrix */
-    SimplicialLLT& compute(const MatrixType& matrix)
-    {
-      Base::template compute<false>(matrix);
-      return *this;
-    }
+  /** \returns an expression of the factor L */
+  inline const MatrixL matrixL() const {
+    eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
+    return Traits::getL(Base::m_matrix);
+  }
 
-    /** Performs a symbolic decomposition on the sparcity of \a matrix.
-      *
-      * This function is particularly useful when solving for several problems having the same structure.
-      *
-      * \sa factorize()
-      */
-    void analyzePattern(const MatrixType& a)
-    {
-      Base::analyzePattern(a, false);
-    }
+  /** \returns an expression of the factor U (= L^*) */
+  inline const MatrixU matrixU() const {
+    eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
+    return Traits::getU(Base::m_matrix);
+  }
 
-    /** Performs a numeric decomposition of \a matrix
-      *
-      * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
-      *
-      * \sa analyzePattern()
-      */
-    void factorize(const MatrixType& a)
-    {
-      Base::template factorize<false>(a);
-    }
+  /** Computes the sparse Cholesky decomposition of \a matrix */
+  SimplicialLLT& compute(const MatrixType& matrix) {
+    Base::template compute<false>(matrix);
+    return *this;
+  }
 
-    /** \returns the determinant of the underlying matrix from the current factorization */
-    Scalar determinant() const
-    {
-      Scalar detL = Base::m_matrix.diagonal().prod();
-      return numext::abs2(detL);
-    }
+  /** Performs a symbolic decomposition on the sparcity of \a matrix.
+   *
+   * This function is particularly useful when solving for several problems having the same structure.
+   *
+   * \sa factorize()
+   */
+  void analyzePattern(const MatrixType& a) { Base::analyzePattern(a, false); }
+
+  /** Performs a numeric decomposition of \a matrix
+   *
+   * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+   *
+   * \sa analyzePattern()
+   */
+  void factorize(const MatrixType& a) { Base::template factorize<false>(a); }
+
+  /** \returns the determinant of the underlying matrix from the current factorization */
+  Scalar determinant() const {
+    Scalar detL = Base::m_matrix.diagonal().prod();
+    return numext::abs2(detL);
+  }
 };
 
 /** \ingroup SparseCholesky_Module
-  * \class SimplicialLDLT
-  * \brief A direct sparse LDLT Cholesky factorizations without square root.
-  *
-  * This class provides a LDL^T Cholesky factorizations without square root of sparse matrices that are
-  * selfadjoint and positive definite. The factorization allows for solving A.X = B where
-  * X and B can be either dense or sparse.
-  * 
-  * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
-  * such that the factorized matrix is P A P^-1.
-  *
-  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
-  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
-  *               or Upper. Default is Lower.
-  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
-  *
-  * \implsparsesolverconcept
-  *
-  * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering
-  */
-template<typename _MatrixType, int _UpLo, typename _Ordering>
-    class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
-{
-public:
-    typedef _MatrixType MatrixType;
-    enum { UpLo = _UpLo };
-    typedef SimplicialCholeskyBase<SimplicialLDLT> Base;
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
-    typedef Matrix<Scalar,Dynamic,1> VectorType;
-    typedef internal::traits<SimplicialLDLT> Traits;
-    typedef typename Traits::MatrixL  MatrixL;
-    typedef typename Traits::MatrixU  MatrixU;
-public:
-    /** Default constructor */
-    SimplicialLDLT() : Base() {}
+ * \class SimplicialLDLT
+ * \brief A direct sparse LDLT Cholesky factorizations without square root.
+ *
+ * This class provides a LDL^T Cholesky factorizations without square root of sparse matrices that are
+ * selfadjoint and positive definite. The factorization allows for solving A.X = B where
+ * X and B can be either dense or sparse.
+ *
+ * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+ * such that the factorized matrix is P A P^-1.
+ *
+ * \tparam MatrixType_ the type of the sparse matrix A, it must be a SparseMatrix<>
+ * \tparam UpLo_ the triangular part that will be used for the computations. It can be Lower
+ *               or Upper. Default is Lower.
+ * \tparam Ordering_ The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering
+ */
+template <typename MatrixType_, int UpLo_, typename Ordering_>
+class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<MatrixType_, UpLo_, Ordering_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum { UpLo = UpLo_ };
+  typedef SimplicialCholeskyBase<SimplicialLDLT> Base;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> CholMatrixType;
+  typedef Matrix<Scalar, Dynamic, 1> VectorType;
+  typedef internal::traits<SimplicialLDLT> Traits;
+  typedef typename Traits::MatrixL MatrixL;
+  typedef typename Traits::MatrixU MatrixU;
 
-    /** Constructs and performs the LLT factorization of \a matrix */
-    explicit SimplicialLDLT(const MatrixType& matrix)
-        : Base(matrix) {}
+ public:
+  /** Default constructor */
+  SimplicialLDLT() : Base() {}
 
-    /** \returns a vector expression of the diagonal D */
-    inline const VectorType vectorD() const {
-        eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
-        return Base::m_diag;
-    }
-    /** \returns an expression of the factor L */
-    inline const MatrixL matrixL() const {
-        eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
-        return Traits::getL(Base::m_matrix);
-    }
+  /** Constructs and performs the LLT factorization of \a matrix */
+  explicit SimplicialLDLT(const MatrixType& matrix) : Base(matrix) {}
 
-    /** \returns an expression of the factor U (= L^*) */
-    inline const MatrixU matrixU() const {
-        eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
-        return Traits::getU(Base::m_matrix);
-    }
+  /** \returns a vector expression of the diagonal D */
+  inline const VectorType vectorD() const {
+    eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+    return Base::m_diag;
+  }
+  /** \returns an expression of the factor L */
+  inline const MatrixL matrixL() const {
+    eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+    return Traits::getL(Base::m_matrix);
+  }
 
-    /** Computes the sparse Cholesky decomposition of \a matrix */
-    SimplicialLDLT& compute(const MatrixType& matrix)
-    {
-      Base::template compute<true>(matrix);
-      return *this;
-    }
-    
-    /** Performs a symbolic decomposition on the sparcity of \a matrix.
-      *
-      * This function is particularly useful when solving for several problems having the same structure.
-      *
-      * \sa factorize()
-      */
-    void analyzePattern(const MatrixType& a)
-    {
-      Base::analyzePattern(a, true);
-    }
+  /** \returns an expression of the factor U (= L^*) */
+  inline const MatrixU matrixU() const {
+    eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+    return Traits::getU(Base::m_matrix);
+  }
 
-    /** Performs a numeric decomposition of \a matrix
-      *
-      * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
-      *
-      * \sa analyzePattern()
-      */
-    void factorize(const MatrixType& a)
-    {
-      Base::template factorize<true>(a);
-    }
+  /** Computes the sparse Cholesky decomposition of \a matrix */
+  SimplicialLDLT& compute(const MatrixType& matrix) {
+    Base::template compute<true>(matrix);
+    return *this;
+  }
 
-    /** \returns the determinant of the underlying matrix from the current factorization */
-    Scalar determinant() const
-    {
-      return Base::m_diag.prod();
-    }
+  /** Performs a symbolic decomposition on the sparcity of \a matrix.
+   *
+   * This function is particularly useful when solving for several problems having the same structure.
+   *
+   * \sa factorize()
+   */
+  void analyzePattern(const MatrixType& a) { Base::analyzePattern(a, true); }
+
+  /** Performs a numeric decomposition of \a matrix
+   *
+   * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+   *
+   * \sa analyzePattern()
+   */
+  void factorize(const MatrixType& a) { Base::template factorize<true>(a); }
+
+  /** \returns the determinant of the underlying matrix from the current factorization */
+  Scalar determinant() const { return Base::m_diag.prod(); }
 };
 
 /** \deprecated use SimplicialLDLT or class SimplicialLLT
-  * \ingroup SparseCholesky_Module
-  * \class SimplicialCholesky
-  *
-  * \sa class SimplicialLDLT, class SimplicialLLT
-  */
-template<typename _MatrixType, int _UpLo, typename _Ordering>
-    class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
-{
-public:
-    typedef _MatrixType MatrixType;
-    enum { UpLo = _UpLo };
-    typedef SimplicialCholeskyBase<SimplicialCholesky> Base;
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
-    typedef Matrix<Scalar,Dynamic,1> VectorType;
-    typedef internal::traits<SimplicialCholesky> Traits;
-    typedef internal::traits<SimplicialLDLT<MatrixType,UpLo> > LDLTTraits;
-    typedef internal::traits<SimplicialLLT<MatrixType,UpLo>  > LLTTraits;
-  public:
-    SimplicialCholesky() : Base(), m_LDLT(true) {}
+ * \ingroup SparseCholesky_Module
+ * \class SimplicialCholesky
+ *
+ * \sa class SimplicialLDLT, class SimplicialLLT
+ */
+template <typename MatrixType_, int UpLo_, typename Ordering_>
+class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<MatrixType_, UpLo_, Ordering_> > {
+ public:
+  typedef MatrixType_ MatrixType;
+  enum { UpLo = UpLo_ };
+  typedef SimplicialCholeskyBase<SimplicialCholesky> Base;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> CholMatrixType;
+  typedef Matrix<Scalar, Dynamic, 1> VectorType;
+  typedef internal::traits<SimplicialCholesky> Traits;
+  typedef internal::traits<SimplicialLDLT<MatrixType, UpLo> > LDLTTraits;
+  typedef internal::traits<SimplicialLLT<MatrixType, UpLo> > LLTTraits;
 
-    explicit SimplicialCholesky(const MatrixType& matrix)
-      : Base(), m_LDLT(true)
-    {
-      compute(matrix);
-    }
+ public:
+  SimplicialCholesky() : Base(), m_LDLT(true) {}
 
-    SimplicialCholesky& setMode(SimplicialCholeskyMode mode)
-    {
-      switch(mode)
-      {
+  explicit SimplicialCholesky(const MatrixType& matrix) : Base(), m_LDLT(true) { compute(matrix); }
+
+  SimplicialCholesky& setMode(SimplicialCholeskyMode mode) {
+    switch (mode) {
       case SimplicialCholeskyLLT:
         m_LDLT = false;
         break;
@@ -543,155 +494,139 @@
         break;
       default:
         break;
-      }
-
-      return *this;
     }
 
-    inline const VectorType vectorD() const {
-        eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
-        return Base::m_diag;
-    }
-    inline const CholMatrixType rawMatrix() const {
-        eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
-        return Base::m_matrix;
-    }
-    
-    /** Computes the sparse Cholesky decomposition of \a matrix */
-    SimplicialCholesky& compute(const MatrixType& matrix)
+    return *this;
+  }
+
+  inline const VectorType vectorD() const {
+    eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
+    return Base::m_diag;
+  }
+  inline const CholMatrixType rawMatrix() const {
+    eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
+    return Base::m_matrix;
+  }
+
+  /** Computes the sparse Cholesky decomposition of \a matrix */
+  SimplicialCholesky& compute(const MatrixType& matrix) {
+    if (m_LDLT)
+      Base::template compute<true>(matrix);
+    else
+      Base::template compute<false>(matrix);
+    return *this;
+  }
+
+  /** Performs a symbolic decomposition on the sparcity of \a matrix.
+   *
+   * This function is particularly useful when solving for several problems having the same structure.
+   *
+   * \sa factorize()
+   */
+  void analyzePattern(const MatrixType& a) { Base::analyzePattern(a, m_LDLT); }
+
+  /** Performs a numeric decomposition of \a matrix
+   *
+   * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+   *
+   * \sa analyzePattern()
+   */
+  void factorize(const MatrixType& a) {
+    if (m_LDLT)
+      Base::template factorize<true>(a);
+    else
+      Base::template factorize<false>(a);
+  }
+
+  /** \internal */
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const MatrixBase<Rhs>& b, MatrixBase<Dest>& dest) const {
+    eigen_assert(Base::m_factorizationIsOk &&
+                 "The decomposition is not in a valid state for solving, you must first call either compute() or "
+                 "symbolic()/numeric()");
+    eigen_assert(Base::m_matrix.rows() == b.rows());
+
+    if (Base::m_info != Success) return;
+
+    if (Base::m_P.size() > 0)
+      dest = Base::m_P * b;
+    else
+      dest = b;
+
+    if (Base::m_matrix.nonZeros() > 0)  // otherwise L==I
     {
-      if(m_LDLT)
-        Base::template compute<true>(matrix);
+      if (m_LDLT)
+        LDLTTraits::getL(Base::m_matrix).solveInPlace(dest);
       else
-        Base::template compute<false>(matrix);
-      return *this;
+        LLTTraits::getL(Base::m_matrix).solveInPlace(dest);
     }
 
-    /** Performs a symbolic decomposition on the sparcity of \a matrix.
-      *
-      * This function is particularly useful when solving for several problems having the same structure.
-      *
-      * \sa factorize()
-      */
-    void analyzePattern(const MatrixType& a)
-    {
-      Base::analyzePattern(a, m_LDLT);
-    }
+    if (Base::m_diag.size() > 0) dest = Base::m_diag.real().asDiagonal().inverse() * dest;
 
-    /** Performs a numeric decomposition of \a matrix
-      *
-      * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
-      *
-      * \sa analyzePattern()
-      */
-    void factorize(const MatrixType& a)
+    if (Base::m_matrix.nonZeros() > 0)  // otherwise I==I
     {
-      if(m_LDLT)
-        Base::template factorize<true>(a);
+      if (m_LDLT)
+        LDLTTraits::getU(Base::m_matrix).solveInPlace(dest);
       else
-        Base::template factorize<false>(a);
+        LLTTraits::getU(Base::m_matrix).solveInPlace(dest);
     }
 
-    /** \internal */
-    template<typename Rhs,typename Dest>
-    void _solve_impl(const MatrixBase<Rhs> &b, MatrixBase<Dest> &dest) const
-    {
-      eigen_assert(Base::m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
-      eigen_assert(Base::m_matrix.rows()==b.rows());
+    if (Base::m_P.size() > 0) dest = Base::m_Pinv * dest;
+  }
 
-      if(Base::m_info!=Success)
-        return;
+  /** \internal */
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const SparseMatrixBase<Rhs>& b, SparseMatrixBase<Dest>& dest) const {
+    internal::solve_sparse_through_dense_panels(*this, b, dest);
+  }
 
-      if(Base::m_P.size()>0)
-        dest = Base::m_P * b;
-      else
-        dest = b;
-
-      if(Base::m_matrix.nonZeros()>0) // otherwise L==I
-      {
-        if(m_LDLT)
-          LDLTTraits::getL(Base::m_matrix).solveInPlace(dest);
-        else
-          LLTTraits::getL(Base::m_matrix).solveInPlace(dest);
-      }
-
-      if(Base::m_diag.size()>0)
-        dest = Base::m_diag.real().asDiagonal().inverse() * dest;
-
-      if (Base::m_matrix.nonZeros()>0) // otherwise I==I
-      {
-        if(m_LDLT)
-          LDLTTraits::getU(Base::m_matrix).solveInPlace(dest);
-        else
-          LLTTraits::getU(Base::m_matrix).solveInPlace(dest);
-      }
-
-      if(Base::m_P.size()>0)
-        dest = Base::m_Pinv * dest;
+  Scalar determinant() const {
+    if (m_LDLT) {
+      return Base::m_diag.prod();
+    } else {
+      Scalar detL = Diagonal<const CholMatrixType>(Base::m_matrix).prod();
+      return numext::abs2(detL);
     }
-    
-    /** \internal */
-    template<typename Rhs,typename Dest>
-    void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
-    {
-      internal::solve_sparse_through_dense_panels(*this, b, dest);
-    }
-    
-    Scalar determinant() const
-    {
-      if(m_LDLT)
-      {
-        return Base::m_diag.prod();
-      }
-      else
-      {
-        Scalar detL = Diagonal<const CholMatrixType>(Base::m_matrix).prod();
-        return numext::abs2(detL);
-      }
-    }
-    
-  protected:
-    bool m_LDLT;
+  }
+
+ protected:
+  bool m_LDLT;
 };
 
-template<typename Derived>
-void SimplicialCholeskyBase<Derived>::ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap)
-{
-  eigen_assert(a.rows()==a.cols());
+template <typename Derived>
+void SimplicialCholeskyBase<Derived>::ordering(const MatrixType& a, ConstCholMatrixPtr& pmat, CholMatrixType& ap) {
+  eigen_assert(a.rows() == a.cols());
   const Index size = a.rows();
   pmat = &ap;
   // Note that ordering methods compute the inverse permutation
-  if(!internal::is_same<OrderingType,NaturalOrdering<Index> >::value)
-  {
+  if (!internal::is_same<OrderingType, NaturalOrdering<Index> >::value) {
     {
       CholMatrixType C;
       C = a.template selfadjointView<UpLo>();
-      
+
       OrderingType ordering;
-      ordering(C,m_Pinv);
+      ordering(C, m_Pinv);
     }
 
-    if(m_Pinv.size()>0) m_P = m_Pinv.inverse();
-    else                m_P.resize(0);
-    
-    ap.resize(size,size);
+    if (m_Pinv.size() > 0)
+      m_P = m_Pinv.inverse();
+    else
+      m_P.resize(0);
+
+    ap.resize(size, size);
     ap.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>().twistedBy(m_P);
-  }
-  else
-  {
+  } else {
     m_Pinv.resize(0);
     m_P.resize(0);
-    if(int(UpLo)==int(Lower) || MatrixType::IsRowMajor)
-    {
+    if (int(UpLo) == int(Lower) || MatrixType::IsRowMajor) {
       // we have to transpose the lower part to to the upper one
-      ap.resize(size,size);
+      ap.resize(size, size);
       ap.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>();
-    }
-    else
-      internal::simplicial_cholesky_grab_input<CholMatrixType,MatrixType>::run(a, pmat, ap);
-  }  
+    } else
+      internal::simplicial_cholesky_grab_input<CholMatrixType, MatrixType>::run(a, pmat, ap);
+  }
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SIMPLICIAL_CHOLESKY_H
+#endif  // EIGEN_SIMPLICIAL_CHOLESKY_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
index 72e1740..abfbbe6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
@@ -20,11 +20,13 @@
 #ifndef EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
 #define EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename Derived>
-void SimplicialCholeskyBase<Derived>::analyzePattern_preordered(const CholMatrixType& ap, bool doLDLT)
-{
+template <typename Derived>
+void SimplicialCholeskyBase<Derived>::analyzePattern_preordered(const CholMatrixType& ap, bool doLDLT) {
   const StorageIndex size = StorageIndex(ap.rows());
   m_matrix.resize(size, size);
   m_parent.resize(size);
@@ -32,25 +34,20 @@
 
   ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0);
 
-  for(StorageIndex k = 0; k < size; ++k)
-  {
+  for (StorageIndex k = 0; k < size; ++k) {
     /* L(k,:) pattern: all nodes reachable in etree from nz in A(0:k-1,k) */
-    m_parent[k] = -1;             /* parent of k is not yet known */
-    tags[k] = k;                  /* mark node k as visited */
-    m_nonZerosPerCol[k] = 0;      /* count of nonzeros in column k of L */
-    for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it)
-    {
+    m_parent[k] = -1;        /* parent of k is not yet known */
+    tags[k] = k;             /* mark node k as visited */
+    m_nonZerosPerCol[k] = 0; /* count of nonzeros in column k of L */
+    for (typename CholMatrixType::InnerIterator it(ap, k); it; ++it) {
       StorageIndex i = it.index();
-      if(i < k)
-      {
+      if (i < k) {
         /* follow path from i to root of etree, stop at flagged node */
-        for(; tags[i] != k; i = m_parent[i])
-        {
+        for (; tags[i] != k; i = m_parent[i]) {
           /* find parent of i if not yet determined */
-          if (m_parent[i] == -1)
-            m_parent[i] = k;
-          m_nonZerosPerCol[i]++;        /* L (k,i) is nonzero */
-          tags[i] = k;                  /* mark i as visited */
+          if (m_parent[i] == -1) m_parent[i] = k;
+          m_nonZerosPerCol[i]++; /* L (k,i) is nonzero */
+          tags[i] = k;           /* mark i as visited */
         }
       }
     }
@@ -59,28 +56,25 @@
   /* construct Lp index array from m_nonZerosPerCol column counts */
   StorageIndex* Lp = m_matrix.outerIndexPtr();
   Lp[0] = 0;
-  for(StorageIndex k = 0; k < size; ++k)
-    Lp[k+1] = Lp[k] + m_nonZerosPerCol[k] + (doLDLT ? 0 : 1);
+  for (StorageIndex k = 0; k < size; ++k) Lp[k + 1] = Lp[k] + m_nonZerosPerCol[k] + (doLDLT ? 0 : 1);
 
   m_matrix.resizeNonZeros(Lp[size]);
 
-  m_isInitialized     = true;
-  m_info              = Success;
-  m_analysisIsOk      = true;
+  m_isInitialized = true;
+  m_info = Success;
+  m_analysisIsOk = true;
   m_factorizationIsOk = false;
 }
 
-
-template<typename Derived>
-template<bool DoLDLT>
-void SimplicialCholeskyBase<Derived>::factorize_preordered(const CholMatrixType& ap)
-{
+template <typename Derived>
+template <bool DoLDLT>
+void SimplicialCholeskyBase<Derived>::factorize_preordered(const CholMatrixType& ap) {
   using std::sqrt;
 
   eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
-  eigen_assert(ap.rows()==ap.cols());
-  eigen_assert(m_parent.size()==ap.rows());
-  eigen_assert(m_nonZerosPerCol.size()==ap.rows());
+  eigen_assert(ap.rows() == ap.cols());
+  eigen_assert(m_parent.size() == ap.rows());
+  eigen_assert(m_nonZerosPerCol.size() == ap.rows());
 
   const StorageIndex size = StorageIndex(ap.rows());
   const StorageIndex* Lp = m_matrix.outerIndexPtr();
@@ -88,80 +82,70 @@
   Scalar* Lx = m_matrix.valuePtr();
 
   ei_declare_aligned_stack_constructed_variable(Scalar, y, size, 0);
-  ei_declare_aligned_stack_constructed_variable(StorageIndex,  pattern, size, 0);
-  ei_declare_aligned_stack_constructed_variable(StorageIndex,  tags, size, 0);
+  ei_declare_aligned_stack_constructed_variable(StorageIndex, pattern, size, 0);
+  ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0);
 
   bool ok = true;
   m_diag.resize(DoLDLT ? size : 0);
 
-  for(StorageIndex k = 0; k < size; ++k)
-  {
+  for (StorageIndex k = 0; k < size; ++k) {
     // compute nonzero pattern of kth row of L, in topological order
-    y[k] = Scalar(0);                     // Y(0:k) is now all zero
-    StorageIndex top = size;               // stack for pattern is empty
-    tags[k] = k;                    // mark node k as visited
-    m_nonZerosPerCol[k] = 0;        // count of nonzeros in column k of L
-    for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it)
-    {
+    y[k] = Scalar(0);         // Y(0:k) is now all zero
+    StorageIndex top = size;  // stack for pattern is empty
+    tags[k] = k;              // mark node k as visited
+    m_nonZerosPerCol[k] = 0;  // count of nonzeros in column k of L
+    for (typename CholMatrixType::InnerIterator it(ap, k); it; ++it) {
       StorageIndex i = it.index();
-      if(i <= k)
-      {
-        y[i] += numext::conj(it.value());            /* scatter A(i,k) into Y (sum duplicates) */
+      if (i <= k) {
+        y[i] += numext::conj(it.value()); /* scatter A(i,k) into Y (sum duplicates) */
         Index len;
-        for(len = 0; tags[i] != k; i = m_parent[i])
-        {
-          pattern[len++] = i;     /* L(k,i) is nonzero */
-          tags[i] = k;            /* mark i as visited */
+        for (len = 0; tags[i] != k; i = m_parent[i]) {
+          pattern[len++] = i; /* L(k,i) is nonzero */
+          tags[i] = k;        /* mark i as visited */
         }
-        while(len > 0)
-          pattern[--top] = pattern[--len];
+        while (len > 0) pattern[--top] = pattern[--len];
       }
     }
 
     /* compute numerical values kth row of L (a sparse triangular solve) */
 
-    RealScalar d = numext::real(y[k]) * m_shiftScale + m_shiftOffset;    // get D(k,k), apply the shift function, and clear Y(k)
+    RealScalar d =
+        numext::real(y[k]) * m_shiftScale + m_shiftOffset;  // get D(k,k), apply the shift function, and clear Y(k)
     y[k] = Scalar(0);
-    for(; top < size; ++top)
-    {
-      Index i = pattern[top];       /* pattern[top:n-1] is pattern of L(:,k) */
-      Scalar yi = y[i];             /* get and clear Y(i) */
+    for (; top < size; ++top) {
+      Index i = pattern[top]; /* pattern[top:n-1] is pattern of L(:,k) */
+      Scalar yi = y[i];       /* get and clear Y(i) */
       y[i] = Scalar(0);
 
       /* the nonzero entry L(k,i) */
       Scalar l_ki;
-      if(DoLDLT)
+      if (DoLDLT)
         l_ki = yi / numext::real(m_diag[i]);
       else
         yi = l_ki = yi / Lx[Lp[i]];
 
       Index p2 = Lp[i] + m_nonZerosPerCol[i];
       Index p;
-      for(p = Lp[i] + (DoLDLT ? 0 : 1); p < p2; ++p)
-        y[Li[p]] -= numext::conj(Lx[p]) * yi;
+      for (p = Lp[i] + (DoLDLT ? 0 : 1); p < p2; ++p) y[Li[p]] -= numext::conj(Lx[p]) * yi;
       d -= numext::real(l_ki * numext::conj(yi));
-      Li[p] = k;                          /* store L(k,i) in column form of L */
+      Li[p] = k; /* store L(k,i) in column form of L */
       Lx[p] = l_ki;
-      ++m_nonZerosPerCol[i];              /* increment count of nonzeros in col i */
+      ++m_nonZerosPerCol[i]; /* increment count of nonzeros in col i */
     }
-    if(DoLDLT)
-    {
+    if (DoLDLT) {
       m_diag[k] = d;
-      if(d == RealScalar(0))
-      {
-        ok = false;                         /* failure, D(k,k) is zero */
+      if (d == RealScalar(0)) {
+        ok = false; /* failure, D(k,k) is zero */
         break;
       }
-    }
-    else
-    {
+    } else {
       Index p = Lp[k] + m_nonZerosPerCol[k]++;
-      Li[p] = k ;                /* store L(k,k) = sqrt (d) in column k */
-      if(d <= RealScalar(0)) {
-        ok = false;              /* failure, matrix is not positive definite */
+      Li[p] = k; /* store L(k,k) = sqrt (d) in column k */
+      if (d <= RealScalar(0)) {
+        ok = false; /* failure, matrix is not positive definite */
         break;
       }
-      Lx[p] = sqrt(d) ;
+      Lx[p] = sqrt(d);
     }
   }
 
@@ -169,6 +153,6 @@
   m_factorizationIsOk = true;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
+#endif  // EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h
index 2cb7747..9f265f0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h
@@ -10,142 +10,132 @@
 #ifndef EIGEN_AMBIVECTOR_H
 #define EIGEN_AMBIVECTOR_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \internal
-  * Hybrid sparse/dense vector class designed for intensive read-write operations.
-  *
-  * See BasicSparseLLT and SparseProduct for usage examples.
-  */
-template<typename _Scalar, typename _StorageIndex>
-class AmbiVector
-{
-  public:
-    typedef _Scalar Scalar;
-    typedef _StorageIndex StorageIndex;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+ * Hybrid sparse/dense vector class designed for intensive read-write operations.
+ *
+ * See BasicSparseLLT and SparseProduct for usage examples.
+ */
+template <typename Scalar_, typename StorageIndex_>
+class AmbiVector {
+ public:
+  typedef Scalar_ Scalar;
+  typedef StorageIndex_ StorageIndex;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    explicit AmbiVector(Index size)
-      : m_buffer(0), m_zero(0), m_size(0), m_end(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1)
-    {
-      resize(size);
+  explicit AmbiVector(Index size)
+      : m_buffer(0), m_zero(0), m_size(0), m_end(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1) {
+    resize(size);
+  }
+
+  void init(double estimatedDensity);
+  void init(int mode);
+
+  Index nonZeros() const;
+
+  /** Specifies a sub-vector to work on */
+  void setBounds(Index start, Index end) {
+    m_start = convert_index(start);
+    m_end = convert_index(end);
+  }
+
+  void setZero();
+
+  void restart();
+  Scalar& coeffRef(Index i);
+  Scalar& coeff(Index i);
+
+  class Iterator;
+
+  ~AmbiVector() { delete[] m_buffer; }
+
+  void resize(Index size) {
+    if (m_allocatedSize < size) reallocate(size);
+    m_size = convert_index(size);
+  }
+
+  StorageIndex size() const { return m_size; }
+
+ protected:
+  StorageIndex convert_index(Index idx) { return internal::convert_index<StorageIndex>(idx); }
+
+  void reallocate(Index size) {
+    // if the size of the matrix is not too large, let's allocate a bit more than needed such
+    // that we can handle dense vector even in sparse mode.
+    delete[] m_buffer;
+    if (size < 1000) {
+      Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1) / sizeof(Scalar);
+      m_allocatedElements = convert_index((allocSize * sizeof(Scalar)) / sizeof(ListEl));
+      m_buffer = new Scalar[allocSize];
+    } else {
+      m_allocatedElements = convert_index((size * sizeof(Scalar)) / sizeof(ListEl));
+      m_buffer = new Scalar[size];
     }
+    m_size = convert_index(size);
+    m_start = 0;
+    m_end = m_size;
+  }
 
-    void init(double estimatedDensity);
-    void init(int mode);
+  void reallocateSparse() {
+    Index copyElements = m_allocatedElements;
+    m_allocatedElements = (std::min)(StorageIndex(m_allocatedElements * 1.5), m_size);
+    Index allocSize = m_allocatedElements * sizeof(ListEl);
+    allocSize = (allocSize + sizeof(Scalar) - 1) / sizeof(Scalar);
+    Scalar* newBuffer = new Scalar[allocSize];
+    std::memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl));
+    delete[] m_buffer;
+    m_buffer = newBuffer;
+  }
 
-    Index nonZeros() const;
+ protected:
+  // element type of the linked list
+  struct ListEl {
+    StorageIndex next;
+    StorageIndex index;
+    Scalar value;
+  };
 
-    /** Specifies a sub-vector to work on */
-    void setBounds(Index start, Index end) { m_start = convert_index(start); m_end = convert_index(end); }
+  // used to store data in both mode
+  Scalar* m_buffer;
+  Scalar m_zero;
+  StorageIndex m_size;
+  StorageIndex m_start;
+  StorageIndex m_end;
+  StorageIndex m_allocatedSize;
+  StorageIndex m_allocatedElements;
+  StorageIndex m_mode;
 
-    void setZero();
-
-    void restart();
-    Scalar& coeffRef(Index i);
-    Scalar& coeff(Index i);
-
-    class Iterator;
-
-    ~AmbiVector() { delete[] m_buffer; }
-
-    void resize(Index size)
-    {
-      if (m_allocatedSize < size)
-        reallocate(size);
-      m_size = convert_index(size);
-    }
-
-    StorageIndex size() const { return m_size; }
-
-  protected:
-    StorageIndex convert_index(Index idx)
-    {
-      return internal::convert_index<StorageIndex>(idx);
-    }
-
-    void reallocate(Index size)
-    {
-      // if the size of the matrix is not too large, let's allocate a bit more than needed such
-      // that we can handle dense vector even in sparse mode.
-      delete[] m_buffer;
-      if (size<1000)
-      {
-        Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar);
-        m_allocatedElements = convert_index((allocSize*sizeof(Scalar))/sizeof(ListEl));
-        m_buffer = new Scalar[allocSize];
-      }
-      else
-      {
-        m_allocatedElements = convert_index((size*sizeof(Scalar))/sizeof(ListEl));
-        m_buffer = new Scalar[size];
-      }
-      m_size = convert_index(size);
-      m_start = 0;
-      m_end = m_size;
-    }
-
-    void reallocateSparse()
-    {
-      Index copyElements = m_allocatedElements;
-      m_allocatedElements = (std::min)(StorageIndex(m_allocatedElements*1.5),m_size);
-      Index allocSize = m_allocatedElements * sizeof(ListEl);
-      allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar);
-      Scalar* newBuffer = new Scalar[allocSize];
-      std::memcpy(newBuffer,  m_buffer,  copyElements * sizeof(ListEl));
-      delete[] m_buffer;
-      m_buffer = newBuffer;
-    }
-
-  protected:
-    // element type of the linked list
-    struct ListEl
-    {
-      StorageIndex next;
-      StorageIndex index;
-      Scalar value;
-    };
-
-    // used to store data in both mode
-    Scalar* m_buffer;
-    Scalar m_zero;
-    StorageIndex m_size;
-    StorageIndex m_start;
-    StorageIndex m_end;
-    StorageIndex m_allocatedSize;
-    StorageIndex m_allocatedElements;
-    StorageIndex m_mode;
-
-    // linked list mode
-    StorageIndex m_llStart;
-    StorageIndex m_llCurrent;
-    StorageIndex m_llSize;
+  // linked list mode
+  StorageIndex m_llStart;
+  StorageIndex m_llCurrent;
+  StorageIndex m_llSize;
 };
 
 /** \returns the number of non zeros in the current sub vector */
-template<typename _Scalar,typename _StorageIndex>
-Index AmbiVector<_Scalar,_StorageIndex>::nonZeros() const
-{
-  if (m_mode==IsSparse)
+template <typename Scalar_, typename StorageIndex_>
+Index AmbiVector<Scalar_, StorageIndex_>::nonZeros() const {
+  if (m_mode == IsSparse)
     return m_llSize;
   else
     return m_end - m_start;
 }
 
-template<typename _Scalar,typename _StorageIndex>
-void AmbiVector<_Scalar,_StorageIndex>::init(double estimatedDensity)
-{
-  if (estimatedDensity>0.1)
+template <typename Scalar_, typename StorageIndex_>
+void AmbiVector<Scalar_, StorageIndex_>::init(double estimatedDensity) {
+  if (estimatedDensity > 0.1)
     init(IsDense);
   else
     init(IsSparse);
 }
 
-template<typename _Scalar,typename _StorageIndex>
-void AmbiVector<_Scalar,_StorageIndex>::init(int mode)
-{
+template <typename Scalar_, typename StorageIndex_>
+void AmbiVector<Scalar_, StorageIndex_>::init(int mode) {
   m_mode = mode;
   // This is only necessary in sparse mode, but we set these unconditionally to avoid some maybe-uninitialized warnings
   // if (m_mode==IsSparse)
@@ -156,45 +146,36 @@
 }
 
 /** Must be called whenever we might perform a write access
-  * with an index smaller than the previous one.
-  *
-  * Don't worry, this function is extremely cheap.
-  */
-template<typename _Scalar,typename _StorageIndex>
-void AmbiVector<_Scalar,_StorageIndex>::restart()
-{
+ * with an index smaller than the previous one.
+ *
+ * Don't worry, this function is extremely cheap.
+ */
+template <typename Scalar_, typename StorageIndex_>
+void AmbiVector<Scalar_, StorageIndex_>::restart() {
   m_llCurrent = m_llStart;
 }
 
 /** Set all coefficients of current subvector to zero */
-template<typename _Scalar,typename _StorageIndex>
-void AmbiVector<_Scalar,_StorageIndex>::setZero()
-{
-  if (m_mode==IsDense)
-  {
-    for (Index i=m_start; i<m_end; ++i)
-      m_buffer[i] = Scalar(0);
-  }
-  else
-  {
-    eigen_assert(m_mode==IsSparse);
+template <typename Scalar_, typename StorageIndex_>
+void AmbiVector<Scalar_, StorageIndex_>::setZero() {
+  if (m_mode == IsDense) {
+    for (Index i = m_start; i < m_end; ++i) m_buffer[i] = Scalar(0);
+  } else {
+    eigen_assert(m_mode == IsSparse);
     m_llSize = 0;
     m_llStart = -1;
   }
 }
 
-template<typename _Scalar,typename _StorageIndex>
-_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeffRef(Index i)
-{
-  if (m_mode==IsDense)
+template <typename Scalar_, typename StorageIndex_>
+Scalar_& AmbiVector<Scalar_, StorageIndex_>::coeffRef(Index i) {
+  if (m_mode == IsDense)
     return m_buffer[i];
-  else
-  {
+  else {
     ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
     // TODO factorize the following code to reduce code generation
-    eigen_assert(m_mode==IsSparse);
-    if (m_llSize==0)
-    {
+    eigen_assert(m_mode == IsSparse);
+    if (m_llSize == 0) {
       // this is the first element
       m_llStart = 0;
       m_llCurrent = 0;
@@ -203,9 +184,7 @@
       llElements[0].index = convert_index(i);
       llElements[0].next = -1;
       return llElements[0].value;
-    }
-    else if (i<llElements[m_llStart].index)
-    {
+    } else if (i < llElements[m_llStart].index) {
       // this is going to be the new first element of the list
       ListEl& el = llElements[m_llSize];
       el.value = Scalar(0);
@@ -215,30 +194,24 @@
       ++m_llSize;
       m_llCurrent = m_llStart;
       return el.value;
-    }
-    else
-    {
+    } else {
       StorageIndex nextel = llElements[m_llCurrent].next;
-      eigen_assert(i>=llElements[m_llCurrent].index && "you must call restart() before inserting an element with lower or equal index");
-      while (nextel >= 0 && llElements[nextel].index<=i)
-      {
+      eigen_assert(i >= llElements[m_llCurrent].index &&
+                   "you must call restart() before inserting an element with lower or equal index");
+      while (nextel >= 0 && llElements[nextel].index <= i) {
         m_llCurrent = nextel;
         nextel = llElements[nextel].next;
       }
 
-      if (llElements[m_llCurrent].index==i)
-      {
+      if (llElements[m_llCurrent].index == i) {
         // the coefficient already exists and we found it !
         return llElements[m_llCurrent].value;
-      }
-      else
-      {
-        if (m_llSize>=m_allocatedElements)
-        {
+      } else {
+        if (m_llSize >= m_allocatedElements) {
           reallocateSparse();
           llElements = reinterpret_cast<ListEl*>(m_buffer);
         }
-        eigen_internal_assert(m_llSize<m_allocatedElements && "internal error: overflow in sparse mode");
+        eigen_internal_assert(m_llSize < m_allocatedElements && "internal error: overflow in sparse mode");
         // let's insert a new coefficient
         ListEl& el = llElements[m_llSize];
         el.value = Scalar(0);
@@ -252,26 +225,20 @@
   }
 }
 
-template<typename _Scalar,typename _StorageIndex>
-_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeff(Index i)
-{
-  if (m_mode==IsDense)
+template <typename Scalar_, typename StorageIndex_>
+Scalar_& AmbiVector<Scalar_, StorageIndex_>::coeff(Index i) {
+  if (m_mode == IsDense)
     return m_buffer[i];
-  else
-  {
+  else {
     ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
-    eigen_assert(m_mode==IsSparse);
-    if ((m_llSize==0) || (i<llElements[m_llStart].index))
-    {
+    eigen_assert(m_mode == IsSparse);
+    if ((m_llSize == 0) || (i < llElements[m_llStart].index)) {
       return m_zero;
-    }
-    else
-    {
+    } else {
       Index elid = m_llStart;
-      while (elid >= 0 && llElements[elid].index<i)
-        elid = llElements[elid].next;
+      while (elid >= 0 && llElements[elid].index < i) elid = llElements[elid].next;
 
-      if (llElements[elid].index==i)
+      if (llElements[elid].index == i)
         return llElements[m_llCurrent].value;
       else
         return m_zero;
@@ -280,99 +247,83 @@
 }
 
 /** Iterator over the nonzero coefficients */
-template<typename _Scalar,typename _StorageIndex>
-class AmbiVector<_Scalar,_StorageIndex>::Iterator
-{
-  public:
-    typedef _Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+template <typename Scalar_, typename StorageIndex_>
+class AmbiVector<Scalar_, StorageIndex_>::Iterator {
+ public:
+  typedef Scalar_ Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    /** Default constructor
-      * \param vec the vector on which we iterate
-      * \param epsilon the minimal value used to prune zero coefficients.
-      * In practice, all coefficients having a magnitude smaller than \a epsilon
-      * are skipped.
-      */
-    explicit Iterator(const AmbiVector& vec, const RealScalar& epsilon = 0)
-      : m_vector(vec)
-    {
-      using std::abs;
-      m_epsilon = epsilon;
-      m_isDense = m_vector.m_mode==IsDense;
-      if (m_isDense)
-      {
-        m_currentEl = 0;   // this is to avoid a compilation warning
-        m_cachedValue = 0; // this is to avoid a compilation warning
-        m_cachedIndex = m_vector.m_start-1;
-        ++(*this);
-      }
-      else
-      {
-        ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
-        m_currentEl = m_vector.m_llStart;
-        while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon)
-          m_currentEl = llElements[m_currentEl].next;
-        if (m_currentEl<0)
-        {
-          m_cachedValue = 0; // this is to avoid a compilation warning
-          m_cachedIndex = -1;
-        }
-        else
-        {
-          m_cachedIndex = llElements[m_currentEl].index;
-          m_cachedValue = llElements[m_currentEl].value;
-        }
+  /** Default constructor
+   * \param vec the vector on which we iterate
+   * \param epsilon the minimal value used to prune zero coefficients.
+   * In practice, all coefficients having a magnitude smaller than \a epsilon
+   * are skipped.
+   */
+  explicit Iterator(const AmbiVector& vec, const RealScalar& epsilon = 0) : m_vector(vec) {
+    using std::abs;
+    m_epsilon = epsilon;
+    m_isDense = m_vector.m_mode == IsDense;
+    if (m_isDense) {
+      m_currentEl = 0;    // this is to avoid a compilation warning
+      m_cachedValue = 0;  // this is to avoid a compilation warning
+      m_cachedIndex = m_vector.m_start - 1;
+      ++(*this);
+    } else {
+      ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+      m_currentEl = m_vector.m_llStart;
+      while (m_currentEl >= 0 && abs(llElements[m_currentEl].value) <= m_epsilon)
+        m_currentEl = llElements[m_currentEl].next;
+      if (m_currentEl < 0) {
+        m_cachedValue = 0;  // this is to avoid a compilation warning
+        m_cachedIndex = -1;
+      } else {
+        m_cachedIndex = llElements[m_currentEl].index;
+        m_cachedValue = llElements[m_currentEl].value;
       }
     }
+  }
 
-    StorageIndex index() const { return m_cachedIndex; }
-    Scalar value() const { return m_cachedValue; }
+  StorageIndex index() const { return m_cachedIndex; }
+  Scalar value() const { return m_cachedValue; }
 
-    operator bool() const { return m_cachedIndex>=0; }
+  operator bool() const { return m_cachedIndex >= 0; }
 
-    Iterator& operator++()
-    {
-      using std::abs;
-      if (m_isDense)
-      {
-        do {
-          ++m_cachedIndex;
-        } while (m_cachedIndex<m_vector.m_end && abs(m_vector.m_buffer[m_cachedIndex])<=m_epsilon);
-        if (m_cachedIndex<m_vector.m_end)
-          m_cachedValue = m_vector.m_buffer[m_cachedIndex];
-        else
-          m_cachedIndex=-1;
-      }
+  Iterator& operator++() {
+    using std::abs;
+    if (m_isDense) {
+      do {
+        ++m_cachedIndex;
+      } while (m_cachedIndex < m_vector.m_end && abs(m_vector.m_buffer[m_cachedIndex]) <= m_epsilon);
+      if (m_cachedIndex < m_vector.m_end)
+        m_cachedValue = m_vector.m_buffer[m_cachedIndex];
       else
-      {
-        ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
-        do {
-          m_currentEl = llElements[m_currentEl].next;
-        } while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon);
-        if (m_currentEl<0)
-        {
-          m_cachedIndex = -1;
-        }
-        else
-        {
-          m_cachedIndex = llElements[m_currentEl].index;
-          m_cachedValue = llElements[m_currentEl].value;
-        }
+        m_cachedIndex = -1;
+    } else {
+      ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+      do {
+        m_currentEl = llElements[m_currentEl].next;
+      } while (m_currentEl >= 0 && abs(llElements[m_currentEl].value) <= m_epsilon);
+      if (m_currentEl < 0) {
+        m_cachedIndex = -1;
+      } else {
+        m_cachedIndex = llElements[m_currentEl].index;
+        m_cachedValue = llElements[m_currentEl].value;
       }
-      return *this;
     }
+    return *this;
+  }
 
-  protected:
-    const AmbiVector& m_vector; // the target vector
-    StorageIndex m_currentEl;   // the current element in sparse/linked-list mode
-    RealScalar m_epsilon;       // epsilon used to prune zero coefficients
-    StorageIndex m_cachedIndex; // current coordinate
-    Scalar m_cachedValue;       // current value
-    bool m_isDense;             // mode of the vector
+ protected:
+  const AmbiVector& m_vector;  // the target vector
+  StorageIndex m_currentEl;    // the current element in sparse/linked-list mode
+  RealScalar m_epsilon;        // epsilon used to prune zero coefficients
+  StorageIndex m_cachedIndex;  // current coordinate
+  Scalar m_cachedValue;        // current value
+  bool m_isDense;              // mode of the vector
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_AMBIVECTOR_H
+#endif  // EIGEN_AMBIVECTOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h
index acd986f..123c89c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h
@@ -10,265 +10,192 @@
 #ifndef EIGEN_COMPRESSED_STORAGE_H
 #define EIGEN_COMPRESSED_STORAGE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \internal
-  * Stores a sparse set of values as a list of values and a list of indices.
-  *
-  */
-template<typename _Scalar,typename _StorageIndex>
-class CompressedStorage
-{
-  public:
+ * Stores a sparse set of values as a list of values and a list of indices.
+ *
+ */
+template <typename Scalar_, typename StorageIndex_>
+class CompressedStorage {
+ public:
+  typedef Scalar_ Scalar;
+  typedef StorageIndex_ StorageIndex;
 
-    typedef _Scalar Scalar;
-    typedef _StorageIndex StorageIndex;
+ protected:
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-  protected:
+ public:
+  CompressedStorage() : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0) {}
 
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+  explicit CompressedStorage(Index size) : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0) { resize(size); }
 
-  public:
+  CompressedStorage(const CompressedStorage& other) : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0) {
+    *this = other;
+  }
 
-    CompressedStorage()
-      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
-    {}
-
-    explicit CompressedStorage(Index size)
-      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
-    {
-      resize(size);
+  CompressedStorage& operator=(const CompressedStorage& other) {
+    resize(other.size());
+    if (other.size() > 0) {
+      internal::smart_copy(other.m_values, other.m_values + m_size, m_values);
+      internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices);
     }
+    return *this;
+  }
 
-    CompressedStorage(const CompressedStorage& other)
-      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
-    {
-      *this = other;
+  void swap(CompressedStorage& other) {
+    std::swap(m_values, other.m_values);
+    std::swap(m_indices, other.m_indices);
+    std::swap(m_size, other.m_size);
+    std::swap(m_allocatedSize, other.m_allocatedSize);
+  }
+
+  ~CompressedStorage() {
+    conditional_aligned_delete_auto<Scalar, true>(m_values, m_allocatedSize);
+    conditional_aligned_delete_auto<StorageIndex, true>(m_indices, m_allocatedSize);
+  }
+
+  void reserve(Index size) {
+    Index newAllocatedSize = m_size + size;
+    if (newAllocatedSize > m_allocatedSize) reallocate(newAllocatedSize);
+  }
+
+  void squeeze() {
+    if (m_allocatedSize > m_size) reallocate(m_size);
+  }
+
+  void resize(Index size, double reserveSizeFactor = 0) {
+    if (m_allocatedSize < size) {
+      Index realloc_size =
+          (std::min<Index>)(NumTraits<StorageIndex>::highest(), size + Index(reserveSizeFactor * double(size)));
+      if (realloc_size < size) internal::throw_std_bad_alloc();
+      reallocate(realloc_size);
     }
+    m_size = size;
+  }
 
-    CompressedStorage& operator=(const CompressedStorage& other)
-    {
-      resize(other.size());
-      if(other.size()>0)
-      {
-        internal::smart_copy(other.m_values,  other.m_values  + m_size, m_values);
-        internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices);
+  void append(const Scalar& v, Index i) {
+    Index id = m_size;
+    resize(m_size + 1, 1);
+    m_values[id] = v;
+    m_indices[id] = internal::convert_index<StorageIndex>(i);
+  }
+
+  inline Index size() const { return m_size; }
+  inline Index allocatedSize() const { return m_allocatedSize; }
+  inline void clear() { m_size = 0; }
+
+  const Scalar* valuePtr() const { return m_values; }
+  Scalar* valuePtr() { return m_values; }
+  const StorageIndex* indexPtr() const { return m_indices; }
+  StorageIndex* indexPtr() { return m_indices; }
+
+  inline Scalar& value(Index i) {
+    eigen_internal_assert(m_values != 0);
+    return m_values[i];
+  }
+  inline const Scalar& value(Index i) const {
+    eigen_internal_assert(m_values != 0);
+    return m_values[i];
+  }
+
+  inline StorageIndex& index(Index i) {
+    eigen_internal_assert(m_indices != 0);
+    return m_indices[i];
+  }
+  inline const StorageIndex& index(Index i) const {
+    eigen_internal_assert(m_indices != 0);
+    return m_indices[i];
+  }
+
+  /** \returns the largest \c k such that for all \c j in [0,k) index[\c j]\<\a key */
+  inline Index searchLowerIndex(Index key) const { return searchLowerIndex(0, m_size, key); }
+
+  /** \returns the largest \c k in [start,end) such that for all \c j in [start,k) index[\c j]\<\a key */
+  inline Index searchLowerIndex(Index start, Index end, Index key) const {
+    return static_cast<Index>(std::distance(m_indices, std::lower_bound(m_indices + start, m_indices + end, key)));
+  }
+
+  /** \returns the stored value at index \a key
+   * If the value does not exist, then the value \a defaultValue is returned without any insertion. */
+  inline Scalar at(Index key, const Scalar& defaultValue = Scalar(0)) const {
+    if (m_size == 0)
+      return defaultValue;
+    else if (key == m_indices[m_size - 1])
+      return m_values[m_size - 1];
+    // ^^  optimization: let's first check if it is the last coefficient
+    // (very common in high level algorithms)
+    const Index id = searchLowerIndex(0, m_size - 1, key);
+    return ((id < m_size) && (m_indices[id] == key)) ? m_values[id] : defaultValue;
+  }
+
+  /** Like at(), but the search is performed in the range [start,end) */
+  inline Scalar atInRange(Index start, Index end, Index key, const Scalar& defaultValue = Scalar(0)) const {
+    if (start >= end)
+      return defaultValue;
+    else if (end > start && key == m_indices[end - 1])
+      return m_values[end - 1];
+    // ^^  optimization: let's first check if it is the last coefficient
+    // (very common in high level algorithms)
+    const Index id = searchLowerIndex(start, end - 1, key);
+    return ((id < end) && (m_indices[id] == key)) ? m_values[id] : defaultValue;
+  }
+
+  /** \returns a reference to the value at index \a key
+   * If the value does not exist, then the value \a defaultValue is inserted
+   * such that the keys are sorted. */
+  inline Scalar& atWithInsertion(Index key, const Scalar& defaultValue = Scalar(0)) {
+    Index id = searchLowerIndex(0, m_size, key);
+    if (id >= m_size || m_indices[id] != key) {
+      if (m_allocatedSize < m_size + 1) {
+        Index newAllocatedSize = 2 * (m_size + 1);
+        m_values = conditional_aligned_realloc_new_auto<Scalar, true>(m_values, newAllocatedSize, m_allocatedSize);
+        m_indices =
+            conditional_aligned_realloc_new_auto<StorageIndex, true>(m_indices, newAllocatedSize, m_allocatedSize);
+        m_allocatedSize = newAllocatedSize;
       }
-      return *this;
-    }
-
-    void swap(CompressedStorage& other)
-    {
-      std::swap(m_values, other.m_values);
-      std::swap(m_indices, other.m_indices);
-      std::swap(m_size, other.m_size);
-      std::swap(m_allocatedSize, other.m_allocatedSize);
-    }
-
-    ~CompressedStorage()
-    {
-      delete[] m_values;
-      delete[] m_indices;
-    }
-
-    void reserve(Index size)
-    {
-      Index newAllocatedSize = m_size + size;
-      if (newAllocatedSize > m_allocatedSize)
-        reallocate(newAllocatedSize);
-    }
-
-    void squeeze()
-    {
-      if (m_allocatedSize>m_size)
-        reallocate(m_size);
-    }
-
-    void resize(Index size, double reserveSizeFactor = 0)
-    {
-      if (m_allocatedSize<size)
-      {
-        Index realloc_size = (std::min<Index>)(NumTraits<StorageIndex>::highest(),  size + Index(reserveSizeFactor*double(size)));
-        if(realloc_size<size)
-          internal::throw_std_bad_alloc();
-        reallocate(realloc_size);
+      if (m_size > id) {
+        internal::smart_memmove(m_values + id, m_values + m_size, m_values + id + 1);
+        internal::smart_memmove(m_indices + id, m_indices + m_size, m_indices + id + 1);
       }
-      m_size = size;
+      m_size++;
+      m_indices[id] = internal::convert_index<StorageIndex>(key);
+      m_values[id] = defaultValue;
     }
+    return m_values[id];
+  }
 
-    void append(const Scalar& v, Index i)
-    {
-      Index id = m_size;
-      resize(m_size+1, 1);
-      m_values[id] = v;
-      m_indices[id] = internal::convert_index<StorageIndex>(i);
-    }
+  inline void moveChunk(Index from, Index to, Index chunkSize) {
+    eigen_internal_assert(chunkSize >= 0 && to + chunkSize <= m_size);
+    internal::smart_memmove(m_values + from, m_values + from + chunkSize, m_values + to);
+    internal::smart_memmove(m_indices + from, m_indices + from + chunkSize, m_indices + to);
+  }
 
-    inline Index size() const { return m_size; }
-    inline Index allocatedSize() const { return m_allocatedSize; }
-    inline void clear() { m_size = 0; }
+ protected:
+  inline void reallocate(Index size) {
+#ifdef EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
+    EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
+#endif
+    eigen_internal_assert(size != m_allocatedSize);
+    m_values = conditional_aligned_realloc_new_auto<Scalar, true>(m_values, size, m_allocatedSize);
+    m_indices = conditional_aligned_realloc_new_auto<StorageIndex, true>(m_indices, size, m_allocatedSize);
+    m_allocatedSize = size;
+  }
 
-    const Scalar* valuePtr() const { return m_values; }
-    Scalar* valuePtr() { return m_values; }
-    const StorageIndex* indexPtr() const { return m_indices; }
-    StorageIndex* indexPtr() { return m_indices; }
-
-    inline Scalar& value(Index i) { eigen_internal_assert(m_values!=0); return m_values[i]; }
-    inline const Scalar& value(Index i) const { eigen_internal_assert(m_values!=0); return m_values[i]; }
-
-    inline StorageIndex& index(Index i) { eigen_internal_assert(m_indices!=0); return m_indices[i]; }
-    inline const StorageIndex& index(Index i) const { eigen_internal_assert(m_indices!=0); return m_indices[i]; }
-
-    /** \returns the largest \c k such that for all \c j in [0,k) index[\c j]\<\a key */
-    inline Index searchLowerIndex(Index key) const
-    {
-      return searchLowerIndex(0, m_size, key);
-    }
-
-    /** \returns the largest \c k in [start,end) such that for all \c j in [start,k) index[\c j]\<\a key */
-    inline Index searchLowerIndex(Index start, Index end, Index key) const
-    {
-      while(end>start)
-      {
-        Index mid = (end+start)>>1;
-        if (m_indices[mid]<key)
-          start = mid+1;
-        else
-          end = mid;
-      }
-      return start;
-    }
-
-    /** \returns the stored value at index \a key
-      * If the value does not exist, then the value \a defaultValue is returned without any insertion. */
-    inline Scalar at(Index key, const Scalar& defaultValue = Scalar(0)) const
-    {
-      if (m_size==0)
-        return defaultValue;
-      else if (key==m_indices[m_size-1])
-        return m_values[m_size-1];
-      // ^^  optimization: let's first check if it is the last coefficient
-      // (very common in high level algorithms)
-      const Index id = searchLowerIndex(0,m_size-1,key);
-      return ((id<m_size) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
-    }
-
-    /** Like at(), but the search is performed in the range [start,end) */
-    inline Scalar atInRange(Index start, Index end, Index key, const Scalar &defaultValue = Scalar(0)) const
-    {
-      if (start>=end)
-        return defaultValue;
-      else if (end>start && key==m_indices[end-1])
-        return m_values[end-1];
-      // ^^  optimization: let's first check if it is the last coefficient
-      // (very common in high level algorithms)
-      const Index id = searchLowerIndex(start,end-1,key);
-      return ((id<end) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
-    }
-
-    /** \returns a reference to the value at index \a key
-      * If the value does not exist, then the value \a defaultValue is inserted
-      * such that the keys are sorted. */
-    inline Scalar& atWithInsertion(Index key, const Scalar& defaultValue = Scalar(0))
-    {
-      Index id = searchLowerIndex(0,m_size,key);
-      if (id>=m_size || m_indices[id]!=key)
-      {
-        if (m_allocatedSize<m_size+1)
-        {
-          m_allocatedSize = 2*(m_size+1);
-          internal::scoped_array<Scalar> newValues(m_allocatedSize);
-          internal::scoped_array<StorageIndex> newIndices(m_allocatedSize);
-
-          // copy first chunk
-          internal::smart_copy(m_values,  m_values +id, newValues.ptr());
-          internal::smart_copy(m_indices, m_indices+id, newIndices.ptr());
-
-          // copy the rest
-          if(m_size>id)
-          {
-            internal::smart_copy(m_values +id,  m_values +m_size, newValues.ptr() +id+1);
-            internal::smart_copy(m_indices+id,  m_indices+m_size, newIndices.ptr()+id+1);
-          }
-          std::swap(m_values,newValues.ptr());
-          std::swap(m_indices,newIndices.ptr());
-        }
-        else if(m_size>id)
-        {
-          internal::smart_memmove(m_values +id, m_values +m_size, m_values +id+1);
-          internal::smart_memmove(m_indices+id, m_indices+m_size, m_indices+id+1);
-        }
-        m_size++;
-        m_indices[id] = internal::convert_index<StorageIndex>(key);
-        m_values[id] = defaultValue;
-      }
-      return m_values[id];
-    }
-
-    void moveChunk(Index from, Index to, Index chunkSize)
-    {
-      eigen_internal_assert(to+chunkSize <= m_size);
-      if(to>from && from+chunkSize>to)
-      {
-        // move backward
-        internal::smart_memmove(m_values+from,  m_values+from+chunkSize,  m_values+to);
-        internal::smart_memmove(m_indices+from, m_indices+from+chunkSize, m_indices+to);
-      }
-      else
-      {
-        internal::smart_copy(m_values+from,  m_values+from+chunkSize,  m_values+to);
-        internal::smart_copy(m_indices+from, m_indices+from+chunkSize, m_indices+to);
-      }
-    }
-
-    void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
-    {
-      Index k = 0;
-      Index n = size();
-      for (Index i=0; i<n; ++i)
-      {
-        if (!internal::isMuchSmallerThan(value(i), reference, epsilon))
-        {
-          value(k) = value(i);
-          index(k) = index(i);
-          ++k;
-        }
-      }
-      resize(k,0);
-    }
-
-  protected:
-
-    inline void reallocate(Index size)
-    {
-      #ifdef EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
-        EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
-      #endif
-      eigen_internal_assert(size!=m_allocatedSize);
-      internal::scoped_array<Scalar> newValues(size);
-      internal::scoped_array<StorageIndex> newIndices(size);
-      Index copySize = (std::min)(size, m_size);
-      if (copySize>0) {
-        internal::smart_copy(m_values, m_values+copySize, newValues.ptr());
-        internal::smart_copy(m_indices, m_indices+copySize, newIndices.ptr());
-      }
-      std::swap(m_values,newValues.ptr());
-      std::swap(m_indices,newIndices.ptr());
-      m_allocatedSize = size;
-    }
-
-  protected:
-    Scalar* m_values;
-    StorageIndex* m_indices;
-    Index m_size;
-    Index m_allocatedSize;
-
+ protected:
+  Scalar* m_values;
+  StorageIndex* m_indices;
+  Index m_size;
+  Index m_allocatedSize;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_COMPRESSED_STORAGE_H
+#endif  // EIGEN_COMPRESSED_STORAGE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
index 9486502..3c6e797 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
@@ -10,27 +10,30 @@
 #ifndef EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
 #define EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, typename ResultType>
-static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, bool sortedInsertion = false)
-{
-  typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
-  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
-  typedef typename remove_all<ResultType>::type::Scalar ResScalar;
+template <typename Lhs, typename Rhs, typename ResultType>
+static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res,
+                                                    bool sortedInsertion = false) {
+  typedef typename remove_all_t<Lhs>::Scalar LhsScalar;
+  typedef typename remove_all_t<Rhs>::Scalar RhsScalar;
+  typedef typename remove_all_t<ResultType>::Scalar ResScalar;
 
   // make sure to call innerSize/outerSize since we fake the storage order.
   Index rows = lhs.innerSize();
   Index cols = rhs.outerSize();
   eigen_assert(lhs.outerSize() == rhs.innerSize());
 
-  ei_declare_aligned_stack_constructed_variable(bool,   mask,     rows, 0);
-  ei_declare_aligned_stack_constructed_variable(ResScalar, values,   rows, 0);
-  ei_declare_aligned_stack_constructed_variable(Index,  indices,  rows, 0);
+  ei_declare_aligned_stack_constructed_variable(bool, mask, rows, 0);
+  ei_declare_aligned_stack_constructed_variable(ResScalar, values, rows, 0);
+  ei_declare_aligned_stack_constructed_variable(Index, indices, rows, 0);
 
-  std::memset(mask,0,sizeof(bool)*rows);
+  std::memset(mask, 0, sizeof(bool) * rows);
 
   evaluator<Lhs> lhsEval(lhs);
   evaluator<Rhs> rhsEval(rhs);
@@ -46,45 +49,35 @@
   res.setZero();
   res.reserve(Index(estimated_nnz_prod));
   // we compute each column of the result, one after the other
-  for (Index j=0; j<cols; ++j)
-  {
-
+  for (Index j = 0; j < cols; ++j) {
     res.startVec(j);
     Index nnz = 0;
-    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
-    {
+    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt) {
       RhsScalar y = rhsIt.value();
       Index k = rhsIt.index();
-      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
-      {
+      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt) {
         Index i = lhsIt.index();
         LhsScalar x = lhsIt.value();
-        if(!mask[i])
-        {
+        if (!mask[i]) {
           mask[i] = true;
           values[i] = x * y;
           indices[nnz] = i;
           ++nnz;
-        }
-        else
+        } else
           values[i] += x * y;
       }
     }
-    if(!sortedInsertion)
-    {
+    if (!sortedInsertion) {
       // unordered insertion
-      for(Index k=0; k<nnz; ++k)
-      {
+      for (Index k = 0; k < nnz; ++k) {
         Index i = indices[k];
-        res.insertBackByOuterInnerUnordered(j,i) = values[i];
+        res.insertBackByOuterInnerUnordered(j, i) = values[i];
         mask[i] = false;
       }
-    }
-    else
-    {
+    } else {
       // alternative ordered insertion code:
-      const Index t200 = rows/11; // 11 == (log2(200)*1.39)
-      const Index t = (rows*100)/139;
+      const Index t200 = rows / 11;  // 11 == (log2(200)*1.39)
+      const Index t = (rows * 100) / 139;
 
       // FIXME reserve nnz non zeros
       // FIXME implement faster sorting algorithms for very small nnz
@@ -92,25 +85,19 @@
       // otherwise => loop through the entire vector
       // In order to avoid to perform an expensive log2 when the
       // result is clearly very sparse we use a linear bound up to 200.
-      if((nnz<200 && nnz<t200) || nnz * numext::log2(int(nnz)) < t)
-      {
-        if(nnz>1) std::sort(indices,indices+nnz);
-        for(Index k=0; k<nnz; ++k)
-        {
+      if ((nnz < 200 && nnz < t200) || nnz * numext::log2(int(nnz)) < t) {
+        if (nnz > 1) std::sort(indices, indices + nnz);
+        for (Index k = 0; k < nnz; ++k) {
           Index i = indices[k];
-          res.insertBackByOuterInner(j,i) = values[i];
+          res.insertBackByOuterInner(j, i) = values[i];
           mask[i] = false;
         }
-      }
-      else
-      {
+      } else {
         // dense path
-        for(Index i=0; i<rows; ++i)
-        {
-          if(mask[i])
-          {
+        for (Index i = 0; i < rows; ++i) {
+          if (mask[i]) {
             mask[i] = false;
-            res.insertBackByOuterInner(j,i) = values[i];
+            res.insertBackByOuterInner(j, i) = values[i];
           }
         }
       }
@@ -119,234 +106,203 @@
   res.finalize();
 }
 
-
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, typename ResultType,
-  int LhsStorageOrder = (traits<Lhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
-  int RhsStorageOrder = (traits<Rhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
-  int ResStorageOrder = (traits<ResultType>::Flags&RowMajorBit) ? RowMajor : ColMajor>
+// Helper template to generate new sparse matrix types
+template <class Source, int Order>
+using WithStorageOrder = SparseMatrix<typename Source::Scalar, Order, typename Source::StorageIndex>;
+
+template <typename Lhs, typename Rhs, typename ResultType,
+          int LhsStorageOrder = (traits<Lhs>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+          int RhsStorageOrder = (traits<Rhs>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+          int ResStorageOrder = (traits<ResultType>::Flags & RowMajorBit) ? RowMajor : ColMajor>
 struct conservative_sparse_sparse_product_selector;
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
-{
-  typedef typename remove_all<Lhs>::type LhsCleaned;
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, ColMajor, ColMajor, ColMajor> {
+  typedef remove_all_t<Lhs> LhsCleaned;
   typedef typename LhsCleaned::Scalar Scalar;
 
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrixAux;
-    typedef typename sparse_eval<ColMajorMatrixAux,ResultType::RowsAtCompileTime,ResultType::ColsAtCompileTime,ColMajorMatrixAux::Flags>::type ColMajorMatrix;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using RowMajorMatrix = WithStorageOrder<ResultType, RowMajor>;
+    using ColMajorMatrixAux = WithStorageOrder<ResultType, ColMajor>;
 
     // If the result is tall and thin (in the extreme case a column vector)
     // then it is faster to sort the coefficients inplace instead of transposing twice.
     // FIXME, the following heuristic is probably not very good.
-    if(lhs.rows()>rhs.cols())
-    {
-      ColMajorMatrix resCol(lhs.rows(),rhs.cols());
+    if (lhs.rows() > rhs.cols()) {
+      using ColMajorMatrix = typename sparse_eval<ColMajorMatrixAux, ResultType::RowsAtCompileTime,
+                                                  ResultType::ColsAtCompileTime, ColMajorMatrixAux::Flags>::type;
+      ColMajorMatrix resCol(lhs.rows(), rhs.cols());
       // perform sorted insertion
-      internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol, true);
+      internal::conservative_sparse_sparse_product_impl<Lhs, Rhs, ColMajorMatrix>(lhs, rhs, resCol, true);
       res = resCol.markAsRValue();
-    }
-    else
-    {
-      ColMajorMatrixAux resCol(lhs.rows(),rhs.cols());
+    } else {
+      ColMajorMatrixAux resCol(lhs.rows(), rhs.cols());
       // resort to transpose to sort the entries
-      internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrixAux>(lhs, rhs, resCol, false);
+      internal::conservative_sparse_sparse_product_impl<Lhs, Rhs, ColMajorMatrixAux>(lhs, rhs, resCol, false);
       RowMajorMatrix resRow(resCol);
       res = resRow.markAsRValue();
     }
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,ColMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRhs;
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, RowMajor, ColMajor, ColMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using RowMajorRhs = WithStorageOrder<Rhs, RowMajor>;
+    using RowMajorRes = WithStorageOrder<ResultType, RowMajor>;
     RowMajorRhs rhsRow = rhs;
     RowMajorRes resRow(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<RowMajorRhs,Lhs,RowMajorRes>(rhsRow, lhs, resRow);
+    internal::conservative_sparse_sparse_product_impl<RowMajorRhs, Lhs, RowMajorRes>(rhsRow, lhs, resRow);
     res = resRow;
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,ColMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorLhs;
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, ColMajor, RowMajor, ColMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using RowMajorLhs = WithStorageOrder<Lhs, RowMajor>;
+    using RowMajorRes = WithStorageOrder<ResultType, RowMajor>;
     RowMajorLhs lhsRow = lhs;
     RowMajorRes resRow(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<Rhs,RowMajorLhs,RowMajorRes>(rhs, lhsRow, resRow);
+    internal::conservative_sparse_sparse_product_impl<Rhs, RowMajorLhs, RowMajorRes>(rhs, lhsRow, resRow);
     res = resRow;
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,ColMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
-    RowMajorMatrix resRow(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, RowMajor, RowMajor, ColMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using RowMajorRes = WithStorageOrder<ResultType, RowMajor>;
+    RowMajorRes resRow(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Rhs, Lhs, RowMajorRes>(rhs, lhs, resRow);
     res = resRow;
   }
 };
 
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, ColMajor, ColMajor, RowMajor> {
+  typedef typename traits<remove_all_t<Lhs>>::Scalar Scalar;
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,RowMajor>
-{
-  typedef typename traits<typename remove_all<Lhs>::type>::Scalar Scalar;
-
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
-    ColMajorMatrix resCol(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol);
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using ColMajorRes = WithStorageOrder<ResultType, ColMajor>;
+    ColMajorRes resCol(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Lhs, Rhs, ColMajorRes>(lhs, rhs, resCol);
     res = resCol;
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,RowMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, RowMajor, ColMajor, RowMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using ColMajorLhs = WithStorageOrder<Lhs, ColMajor>;
+    using ColMajorRes = WithStorageOrder<ResultType, ColMajor>;
     ColMajorLhs lhsCol = lhs;
     ColMajorRes resCol(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<ColMajorLhs,Rhs,ColMajorRes>(lhsCol, rhs, resCol);
+    internal::conservative_sparse_sparse_product_impl<ColMajorLhs, Rhs, ColMajorRes>(lhsCol, rhs, resCol);
     res = resCol;
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,RowMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, ColMajor, RowMajor, RowMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using ColMajorRhs = WithStorageOrder<Rhs, ColMajor>;
+    using ColMajorRes = WithStorageOrder<ResultType, ColMajor>;
     ColMajorRhs rhsCol = rhs;
     ColMajorRes resCol(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<Lhs,ColMajorRhs,ColMajorRes>(lhs, rhsCol, resCol);
+    internal::conservative_sparse_sparse_product_impl<Lhs, ColMajorRhs, ColMajorRes>(lhs, rhsCol, resCol);
     res = resCol;
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,RowMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
-    RowMajorMatrix resRow(lhs.rows(),rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
+template <typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs, Rhs, ResultType, RowMajor, RowMajor, RowMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using ColMajorRes = WithStorageOrder<ResultType, ColMajor>;
+    using RowMajorRes = WithStorageOrder<ResultType, RowMajor>;
+    RowMajorRes resRow(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Rhs, Lhs, RowMajorRes>(rhs, lhs, resRow);
     // sort the non zeros:
-    ColMajorMatrix resCol(resRow);
+    ColMajorRes resCol(resRow);
     res = resCol;
   }
 };
 
-} // end namespace internal
-
+}  // end namespace internal
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, typename ResultType>
-static void sparse_sparse_to_dense_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-{
-  typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
-  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+template <typename Lhs, typename Rhs, typename ResultType>
+static void sparse_sparse_to_dense_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+  typedef typename remove_all_t<Lhs>::Scalar LhsScalar;
+  typedef typename remove_all_t<Rhs>::Scalar RhsScalar;
   Index cols = rhs.outerSize();
   eigen_assert(lhs.outerSize() == rhs.innerSize());
 
   evaluator<Lhs> lhsEval(lhs);
   evaluator<Rhs> rhsEval(rhs);
 
-  for (Index j=0; j<cols; ++j)
-  {
-    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
-    {
+  for (Index j = 0; j < cols; ++j) {
+    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt) {
       RhsScalar y = rhsIt.value();
       Index k = rhsIt.index();
-      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
-      {
+      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt) {
         Index i = lhsIt.index();
         LhsScalar x = lhsIt.value();
-        res.coeffRef(i,j) += x * y;
+        res.coeffRef(i, j) += x * y;
       }
     }
   }
 }
 
-
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, typename ResultType,
-  int LhsStorageOrder = (traits<Lhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
-  int RhsStorageOrder = (traits<Rhs>::Flags&RowMajorBit) ? RowMajor : ColMajor>
+template <typename Lhs, typename Rhs, typename ResultType,
+          int LhsStorageOrder = (traits<Lhs>::Flags & RowMajorBit) ? RowMajor : ColMajor,
+          int RhsStorageOrder = (traits<Rhs>::Flags & RowMajorBit) ? RowMajor : ColMajor>
 struct sparse_sparse_to_dense_product_selector;
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    internal::sparse_sparse_to_dense_product_impl<Lhs,Rhs,ResultType>(lhs, rhs, res);
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs, Rhs, ResultType, ColMajor, ColMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    internal::sparse_sparse_to_dense_product_impl<Lhs, Rhs, ResultType>(lhs, rhs, res);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs, Rhs, ResultType, RowMajor, ColMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using ColMajorLhs = WithStorageOrder<Lhs, ColMajor>;
     ColMajorLhs lhsCol(lhs);
-    internal::sparse_sparse_to_dense_product_impl<ColMajorLhs,Rhs,ResultType>(lhsCol, rhs, res);
+    internal::sparse_sparse_to_dense_product_impl<ColMajorLhs, Rhs, ResultType>(lhsCol, rhs, res);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
-    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs, Rhs, ResultType, ColMajor, RowMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
+    using ColMajorRhs = WithStorageOrder<Rhs, ColMajor>;
     ColMajorRhs rhsCol(rhs);
-    internal::sparse_sparse_to_dense_product_impl<Lhs,ColMajorRhs,ResultType>(lhs, rhsCol, res);
+    internal::sparse_sparse_to_dense_product_impl<Lhs, ColMajorRhs, ResultType>(lhs, rhsCol, res);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor>
-{
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
-  {
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs, Rhs, ResultType, RowMajor, RowMajor> {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) {
     Transpose<ResultType> trRes(res);
-    internal::sparse_sparse_to_dense_product_impl<Rhs,Lhs,Transpose<ResultType> >(rhs, lhs, trRes);
+    internal::sparse_sparse_to_dense_product_impl<Rhs, Lhs, Transpose<ResultType>>(rhs, lhs, trRes);
   }
 };
 
+}  // end namespace internal
 
-} // end namespace internal
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
+#endif  // EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/InternalHeaderCheck.h
new file mode 100644
index 0000000..9de5936
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_SPARSECORE_MODULE_H
+#error "Please include Eigen/SparseCore instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h
deleted file mode 100644
index 67718c8..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h
+++ /dev/null
@@ -1,67 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MAPPED_SPARSEMATRIX_H
-#define EIGEN_MAPPED_SPARSEMATRIX_H
-
-namespace Eigen {
-
-/** \deprecated Use Map<SparseMatrix<> >
-  * \class MappedSparseMatrix
-  *
-  * \brief Sparse matrix
-  *
-  * \param _Scalar the scalar type, i.e. the type of the coefficients
-  *
-  * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
-  *
-  */
-namespace internal {
-template<typename _Scalar, int _Flags, typename _StorageIndex>
-struct traits<MappedSparseMatrix<_Scalar, _Flags, _StorageIndex> > : traits<SparseMatrix<_Scalar, _Flags, _StorageIndex> >
-{};
-} // end namespace internal
-
-template<typename _Scalar, int _Flags, typename _StorageIndex>
-class MappedSparseMatrix
-  : public Map<SparseMatrix<_Scalar, _Flags, _StorageIndex> >
-{
-    typedef Map<SparseMatrix<_Scalar, _Flags, _StorageIndex> > Base;
-
-  public:
-    
-    typedef typename Base::StorageIndex StorageIndex;
-    typedef typename Base::Scalar Scalar;
-
-    inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0)
-      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr)
-    {}
-
-    /** Empty destructor */
-    inline ~MappedSparseMatrix() {}
-};
-
-namespace internal {
-
-template<typename _Scalar, int _Options, typename _StorageIndex>
-struct evaluator<MappedSparseMatrix<_Scalar,_Options,_StorageIndex> >
-  : evaluator<SparseCompressedBase<MappedSparseMatrix<_Scalar,_Options,_StorageIndex> > >
-{
-  typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType;
-  typedef evaluator<SparseCompressedBase<XprType> > Base;
-  
-  evaluator() : Base() {}
-  explicit evaluator(const XprType &mat) : Base(mat) {}
-};
-
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_MAPPED_SPARSEMATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h
index 905485c..f2da519 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h
@@ -10,111 +10,118 @@
 #ifndef EIGEN_SPARSEASSIGN_H
 #define EIGEN_SPARSEASSIGN_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-template<typename Derived>    
-template<typename OtherDerived>
-Derived& SparseMatrixBase<Derived>::operator=(const EigenBase<OtherDerived> &other)
-{
+namespace Eigen {
+
+template <typename Derived>
+template <typename OtherDerived>
+Derived &SparseMatrixBase<Derived>::operator=(const EigenBase<OtherDerived> &other) {
   internal::call_assignment_no_alias(derived(), other.derived());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-Derived& SparseMatrixBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
-{
+template <typename Derived>
+template <typename OtherDerived>
+Derived &SparseMatrixBase<Derived>::operator=(const ReturnByValue<OtherDerived> &other) {
   // TODO use the evaluator mechanism
   other.evalTo(derived());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-inline Derived& SparseMatrixBase<Derived>::operator=(const SparseMatrixBase<OtherDerived>& other)
-{
+template <typename Derived>
+template <typename OtherDerived>
+inline Derived &SparseMatrixBase<Derived>::operator=(const SparseMatrixBase<OtherDerived> &other) {
   // by default sparse evaluation do not alias, so we can safely bypass the generic call_assignment routine
-  internal::Assignment<Derived,OtherDerived,internal::assign_op<Scalar,typename OtherDerived::Scalar> >
-          ::run(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+  internal::Assignment<Derived, OtherDerived, internal::assign_op<Scalar, typename OtherDerived::Scalar>>::run(
+      derived(), other.derived(), internal::assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-template<typename Derived>
-inline Derived& SparseMatrixBase<Derived>::operator=(const Derived& other)
-{
+template <typename Derived>
+inline Derived &SparseMatrixBase<Derived>::operator=(const Derived &other) {
   internal::call_assignment_no_alias(derived(), other.derived());
   return derived();
 }
 
 namespace internal {
 
-template<>
+template <>
 struct storage_kind_to_evaluator_kind<Sparse> {
   typedef IteratorBased Kind;
 };
 
-template<>
+template <>
 struct storage_kind_to_shape<Sparse> {
   typedef SparseShape Shape;
 };
 
 struct Sparse2Sparse {};
-struct Sparse2Dense  {};
+struct Sparse2Dense {};
 
-template<> struct AssignmentKind<SparseShape, SparseShape>           { typedef Sparse2Sparse Kind; };
-template<> struct AssignmentKind<SparseShape, SparseTriangularShape> { typedef Sparse2Sparse Kind; };
-template<> struct AssignmentKind<DenseShape,  SparseShape>           { typedef Sparse2Dense  Kind; };
-template<> struct AssignmentKind<DenseShape,  SparseTriangularShape> { typedef Sparse2Dense  Kind; };
+template <>
+struct AssignmentKind<SparseShape, SparseShape> {
+  typedef Sparse2Sparse Kind;
+};
+template <>
+struct AssignmentKind<SparseShape, SparseTriangularShape> {
+  typedef Sparse2Sparse Kind;
+};
+template <>
+struct AssignmentKind<DenseShape, SparseShape> {
+  typedef Sparse2Dense Kind;
+};
+template <>
+struct AssignmentKind<DenseShape, SparseTriangularShape> {
+  typedef Sparse2Dense Kind;
+};
 
-
-template<typename DstXprType, typename SrcXprType>
-void assign_sparse_to_sparse(DstXprType &dst, const SrcXprType &src)
-{
+template <typename DstXprType, typename SrcXprType>
+void assign_sparse_to_sparse(DstXprType &dst, const SrcXprType &src) {
   typedef typename DstXprType::Scalar Scalar;
   typedef internal::evaluator<DstXprType> DstEvaluatorType;
   typedef internal::evaluator<SrcXprType> SrcEvaluatorType;
 
   SrcEvaluatorType srcEvaluator(src);
 
-  const bool transpose = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit);
-  const Index outerEvaluationSize = (SrcEvaluatorType::Flags&RowMajorBit) ? src.rows() : src.cols();
-  if ((!transpose) && src.isRValue())
-  {
+  constexpr bool transpose = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit);
+  const Index outerEvaluationSize = (SrcEvaluatorType::Flags & RowMajorBit) ? src.rows() : src.cols();
+
+  Index reserveSize = 0;
+  for (Index j = 0; j < outerEvaluationSize; ++j)
+    for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it) reserveSize++;
+
+  if ((!transpose) && src.isRValue()) {
     // eval without temporary
     dst.resize(src.rows(), src.cols());
     dst.setZero();
-    dst.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2));
-    for (Index j=0; j<outerEvaluationSize; ++j)
-    {
+    dst.reserve(reserveSize);
+    for (Index j = 0; j < outerEvaluationSize; ++j) {
       dst.startVec(j);
-      for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it)
-      {
+      for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it) {
         Scalar v = it.value();
-        dst.insertBackByOuterInner(j,it.index()) = v;
+        dst.insertBackByOuterInner(j, it.index()) = v;
       }
     }
     dst.finalize();
-  }
-  else
-  {
+  } else {
     // eval through a temporary
-    eigen_assert(( ((internal::traits<DstXprType>::SupportedAccessPatterns & OuterRandomAccessPattern)==OuterRandomAccessPattern) ||
-              (!((DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit)))) &&
-              "the transpose operation is supposed to be handled in SparseMatrix::operator=");
+    eigen_assert((((internal::traits<DstXprType>::SupportedAccessPatterns & OuterRandomAccessPattern) ==
+                   OuterRandomAccessPattern) ||
+                  (!((DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit)))) &&
+                 "the transpose operation is supposed to be handled in SparseMatrix::operator=");
 
     enum { Flip = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit) };
 
-    
     DstXprType temp(src.rows(), src.cols());
 
-    temp.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2));
-    for (Index j=0; j<outerEvaluationSize; ++j)
-    {
+    temp.reserve(reserveSize);
+    for (Index j = 0; j < outerEvaluationSize; ++j) {
       temp.startVec(j);
-      for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it)
-      {
+      for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it) {
         Scalar v = it.value();
-        temp.insertBackByOuterInner(Flip?it.index():j,Flip?j:it.index()) = v;
+        temp.insertBackByOuterInner(Flip ? it.index() : j, Flip ? j : it.index()) = v;
       }
     }
     temp.finalize();
@@ -124,61 +131,56 @@
 }
 
 // Generic Sparse to Sparse assignment
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Sparse>
-{
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  {
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Sparse> {
+  static void run(DstXprType &dst, const SrcXprType &src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar> & /*func*/) {
     assign_sparse_to_sparse(dst.derived(), src.derived());
   }
 };
 
 // Generic Sparse to Dense assignment
-template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
-struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Dense, Weak>
-{
-  static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
-    if(internal::is_same<Functor,internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> >::value)
+template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Dense, Weak> {
+  static void run(DstXprType &dst, const SrcXprType &src, const Functor &func) {
+    if (internal::is_same<Functor,
+                          internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>>::value)
       dst.setZero();
-    
+
     internal::evaluator<SrcXprType> srcEval(src);
     resize_if_allowed(dst, src, func);
     internal::evaluator<DstXprType> dstEval(dst);
-    
-    const Index outerEvaluationSize = (internal::evaluator<SrcXprType>::Flags&RowMajorBit) ? src.rows() : src.cols();
-    for (Index j=0; j<outerEvaluationSize; ++j)
-      for (typename internal::evaluator<SrcXprType>::InnerIterator i(srcEval,j); i; ++i)
-        func.assignCoeff(dstEval.coeffRef(i.row(),i.col()), i.value());
+
+    const Index outerEvaluationSize = (internal::evaluator<SrcXprType>::Flags & RowMajorBit) ? src.rows() : src.cols();
+    for (Index j = 0; j < outerEvaluationSize; ++j)
+      for (typename internal::evaluator<SrcXprType>::InnerIterator i(srcEval, j); i; ++i)
+        func.assignCoeff(dstEval.coeffRef(i.row(), i.col()), i.value());
   }
 };
 
 // Specialization for dense ?= dense +/- sparse and dense ?= sparse +/- dense
-template<typename DstXprType, typename Func1, typename Func2>
-struct assignment_from_dense_op_sparse
-{
-  template<typename SrcXprType, typename InitialFunc>
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  void run(DstXprType &dst, const SrcXprType &src, const InitialFunc& /*func*/)
-  {
-    #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN
+template <typename DstXprType, typename Func1, typename Func2>
+struct assignment_from_dense_op_sparse {
+  template <typename SrcXprType, typename InitialFunc>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src,
+                                                        const InitialFunc & /*func*/) {
+#ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN
     EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN
-    #endif
+#endif
 
     call_assignment_no_alias(dst, src.lhs(), Func1());
     call_assignment_no_alias(dst, src.rhs(), Func2());
   }
 
   // Specialization for dense1 = sparse + dense2; -> dense1 = dense2; dense1 += sparse;
-  template<typename Lhs, typename Rhs, typename Scalar>
+  template <typename Lhs, typename Rhs, typename Scalar>
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  typename internal::enable_if<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type
-  run(DstXprType &dst, const CwiseBinaryOp<internal::scalar_sum_op<Scalar,Scalar>, const Lhs, const Rhs> &src,
-      const internal::assign_op<typename DstXprType::Scalar,Scalar>& /*func*/)
-  {
-    #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN
+      std::enable_if_t<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape, DenseShape>::value>
+      run(DstXprType &dst, const CwiseBinaryOp<internal::scalar_sum_op<Scalar, Scalar>, const Lhs, const Rhs> &src,
+          const internal::assign_op<typename DstXprType::Scalar, Scalar> & /*func*/) {
+#ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN
     EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN
-    #endif
+#endif
 
     // Apply the dense matrix first, then the sparse one.
     call_assignment_no_alias(dst, src.rhs(), Func1());
@@ -186,52 +188,50 @@
   }
 
   // Specialization for dense1 = sparse - dense2; -> dense1 = -dense2; dense1 += sparse;
-  template<typename Lhs, typename Rhs, typename Scalar>
+  template <typename Lhs, typename Rhs, typename Scalar>
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  typename internal::enable_if<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type
-  run(DstXprType &dst, const CwiseBinaryOp<internal::scalar_difference_op<Scalar,Scalar>, const Lhs, const Rhs> &src,
-      const internal::assign_op<typename DstXprType::Scalar,Scalar>& /*func*/)
-  {
-    #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN
+      std::enable_if_t<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape, DenseShape>::value>
+      run(DstXprType &dst,
+          const CwiseBinaryOp<internal::scalar_difference_op<Scalar, Scalar>, const Lhs, const Rhs> &src,
+          const internal::assign_op<typename DstXprType::Scalar, Scalar> & /*func*/) {
+#ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN
     EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN
-    #endif
+#endif
 
     // Apply the dense matrix first, then the sparse one.
     call_assignment_no_alias(dst, -src.rhs(), Func1());
-    call_assignment_no_alias(dst,  src.lhs(), add_assign_op<typename DstXprType::Scalar,typename Lhs::Scalar>());
+    call_assignment_no_alias(dst, src.lhs(), add_assign_op<typename DstXprType::Scalar, typename Lhs::Scalar>());
   }
 };
 
-#define EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(ASSIGN_OP,BINOP,ASSIGN_OP2) \
-  template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar> \
-  struct Assignment<DstXprType, CwiseBinaryOp<internal::BINOP<Scalar,Scalar>, const Lhs, const Rhs>, internal::ASSIGN_OP<typename DstXprType::Scalar,Scalar>, \
-                    Sparse2Dense, \
-                    typename internal::enable_if<   internal::is_same<typename internal::evaluator_traits<Lhs>::Shape,DenseShape>::value \
-                                                 || internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type> \
-    : assignment_from_dense_op_sparse<DstXprType, internal::ASSIGN_OP<typename DstXprType::Scalar,typename Lhs::Scalar>, internal::ASSIGN_OP2<typename DstXprType::Scalar,typename Rhs::Scalar> > \
-  {}
+#define EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(ASSIGN_OP, BINOP, ASSIGN_OP2)                                        \
+  template <typename DstXprType, typename Lhs, typename Rhs, typename Scalar>                                   \
+  struct Assignment<                                                                                            \
+      DstXprType, CwiseBinaryOp<internal::BINOP<Scalar, Scalar>, const Lhs, const Rhs>,                         \
+      internal::ASSIGN_OP<typename DstXprType::Scalar, Scalar>, Sparse2Dense,                                   \
+      std::enable_if_t<internal::is_same<typename internal::evaluator_traits<Lhs>::Shape, DenseShape>::value || \
+                       internal::is_same<typename internal::evaluator_traits<Rhs>::Shape, DenseShape>::value>>  \
+      : assignment_from_dense_op_sparse<DstXprType,                                                             \
+                                        internal::ASSIGN_OP<typename DstXprType::Scalar, typename Lhs::Scalar>, \
+                                        internal::ASSIGN_OP2<typename DstXprType::Scalar, typename Rhs::Scalar>> {}
 
-EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op,    scalar_sum_op,add_assign_op);
-EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_sum_op,add_assign_op);
-EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_sum_op,sub_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op, scalar_sum_op, add_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op, scalar_sum_op, add_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op, scalar_sum_op, sub_assign_op);
 
-EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op,    scalar_difference_op,sub_assign_op);
-EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_difference_op,sub_assign_op);
-EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_difference_op,add_assign_op);
-
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op, scalar_difference_op, sub_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op, scalar_difference_op, sub_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op, scalar_difference_op, add_assign_op);
 
 // Specialization for "dst = dec.solve(rhs)"
 // NOTE we need to specialize it for Sparse2Sparse to avoid ambiguous specialization error
-template<typename DstXprType, typename DecType, typename RhsType, typename Scalar>
-struct Assignment<DstXprType, Solve<DecType,RhsType>, internal::assign_op<Scalar,Scalar>, Sparse2Sparse>
-{
-  typedef Solve<DecType,RhsType> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
-  {
+template <typename DstXprType, typename DecType, typename RhsType, typename Scalar>
+struct Assignment<DstXprType, Solve<DecType, RhsType>, internal::assign_op<Scalar, Scalar>, Sparse2Sparse> {
+  typedef Solve<DecType, RhsType> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar, Scalar> &) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
     src.dec()._solve_impl(src.rhs(), dst);
   }
@@ -239,32 +239,41 @@
 
 struct Diagonal2Sparse {};
 
-template<> struct AssignmentKind<SparseShape,DiagonalShape> { typedef Diagonal2Sparse Kind; };
+template <>
+struct AssignmentKind<SparseShape, DiagonalShape> {
+  typedef Diagonal2Sparse Kind;
+};
 
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Diagonal2Sparse>
-{
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Diagonal2Sparse> {
   typedef typename DstXprType::StorageIndex StorageIndex;
   typedef typename DstXprType::Scalar Scalar;
 
-  template<int Options, typename AssignFunc>
-  static void run(SparseMatrix<Scalar,Options,StorageIndex> &dst, const SrcXprType &src, const AssignFunc &func)
-  { dst.assignDiagonal(src.diagonal(), func); }
-  
-  template<typename DstDerived>
-  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  { dst.derived().diagonal() = src.diagonal(); }
-  
-  template<typename DstDerived>
-  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  { dst.derived().diagonal() += src.diagonal(); }
-  
-  template<typename DstDerived>
-  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
-  { dst.derived().diagonal() -= src.diagonal(); }
+  template <int Options, typename AssignFunc>
+  static void run(SparseMatrix<Scalar, Options, StorageIndex> &dst, const SrcXprType &src, const AssignFunc &func) {
+    dst.assignDiagonal(src.diagonal(), func);
+  }
+
+  template <typename DstDerived>
+  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar> & /*func*/) {
+    dst.derived().diagonal() = src.diagonal();
+  }
+
+  template <typename DstDerived>
+  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src,
+                  const internal::add_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar> & /*func*/) {
+    dst.derived().diagonal() += src.diagonal();
+  }
+
+  template <typename DstDerived>
+  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src,
+                  const internal::sub_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar> & /*func*/) {
+    dst.derived().diagonal() -= src.diagonal();
+  }
 };
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSEASSIGN_H
+#endif  // EIGEN_SPARSEASSIGN_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h
index 5b4f6cc..1342f4e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h
@@ -10,491 +10,454 @@
 #ifndef EIGEN_SPARSE_BLOCK_H
 #define EIGEN_SPARSE_BLOCK_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 // Subset of columns or rows
-template<typename XprType, int BlockRows, int BlockCols>
-class BlockImpl<XprType,BlockRows,BlockCols,true,Sparse>
-  : public SparseMatrixBase<Block<XprType,BlockRows,BlockCols,true> >
-{
-    typedef typename internal::remove_all<typename XprType::Nested>::type _MatrixTypeNested;
-    typedef Block<XprType, BlockRows, BlockCols, true> BlockType;
-public:
-    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
-protected:
-    enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
-    typedef SparseMatrixBase<BlockType> Base;
-    using Base::convert_index;
-public:
-    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+template <typename XprType, int BlockRows, int BlockCols>
+class BlockImpl<XprType, BlockRows, BlockCols, true, Sparse>
+    : public SparseMatrixBase<Block<XprType, BlockRows, BlockCols, true> > {
+  typedef internal::remove_all_t<typename XprType::Nested> MatrixTypeNested_;
+  typedef Block<XprType, BlockRows, BlockCols, true> BlockType;
 
-    inline BlockImpl(XprType& xpr, Index i)
-      : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize)
-    {}
+ public:
+  enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
 
-    inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
-      : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols))
-    {}
+ protected:
+  enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
+  typedef SparseMatrixBase<BlockType> Base;
+  using Base::convert_index;
 
-    EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
-    EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+ public:
+  EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
 
-    Index nonZeros() const
-    {
-      typedef internal::evaluator<XprType> EvaluatorType;
-      EvaluatorType matEval(m_matrix);
-      Index nnz = 0;
-      Index end = m_outerStart + m_outerSize.value();
-      for(Index j=m_outerStart; j<end; ++j)
-        for(typename EvaluatorType::InnerIterator it(matEval, j); it; ++it)
-          ++nnz;
-      return nnz;
-    }
+  inline BlockImpl(XprType& xpr, Index i) : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize) {}
 
-    inline const Scalar coeff(Index row, Index col) const
-    {
-      return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
-    }
+  inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+      : m_matrix(xpr),
+        m_outerStart(convert_index(IsRowMajor ? startRow : startCol)),
+        m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols)) {}
 
-    inline const Scalar coeff(Index index) const
-    {
-      return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index :  m_outerStart);
-    }
+  EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+  EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
 
-    inline const XprType& nestedExpression() const { return m_matrix; }
-    inline XprType& nestedExpression() { return m_matrix; }
-    Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
-    Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
-    Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
-    Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+  Index nonZeros() const {
+    typedef internal::evaluator<XprType> EvaluatorType;
+    EvaluatorType matEval(m_matrix);
+    Index nnz = 0;
+    Index end = m_outerStart + m_outerSize.value();
+    for (Index j = m_outerStart; j < end; ++j)
+      for (typename EvaluatorType::InnerIterator it(matEval, j); it; ++it) ++nnz;
+    return nnz;
+  }
 
-  protected:
+  inline const Scalar coeff(Index row, Index col) const {
+    return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart));
+  }
 
-    typename internal::ref_selector<XprType>::non_const_type m_matrix;
-    Index m_outerStart;
-    const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
+  inline const Scalar coeff(Index index) const {
+    return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart);
+  }
 
-  protected:
-    // Disable assignment with clear error message.
-    // Note that simply removing operator= yields compilation errors with ICC+MSVC
-    template<typename T>
-    BlockImpl& operator=(const T&)
-    {
-      EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
-      return *this;
-    }
+  inline const XprType& nestedExpression() const { return m_matrix; }
+  inline XprType& nestedExpression() { return m_matrix; }
+  Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
+  Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
+  Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+  Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+ protected:
+  typename internal::ref_selector<XprType>::non_const_type m_matrix;
+  Index m_outerStart;
+  const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
+
+ protected:
+  // Disable assignment with clear error message.
+  // Note that simply removing operator= yields compilation errors with ICC+MSVC
+  template <typename T>
+  BlockImpl& operator=(const T&) {
+    EIGEN_STATIC_ASSERT(sizeof(T) == 0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
+    return *this;
+  }
 };
 
-
 /***************************************************************************
-* specialization for SparseMatrix
-***************************************************************************/
+ * specialization for SparseMatrix
+ ***************************************************************************/
 
 namespace internal {
 
-template<typename SparseMatrixType, int BlockRows, int BlockCols>
-class sparse_matrix_block_impl
-  : public SparseCompressedBase<Block<SparseMatrixType,BlockRows,BlockCols,true> >
-{
-    typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _MatrixTypeNested;
-    typedef Block<SparseMatrixType, BlockRows, BlockCols, true> BlockType;
-    typedef SparseCompressedBase<Block<SparseMatrixType,BlockRows,BlockCols,true> > Base;
-    using Base::convert_index;
-public:
-    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
-    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
-protected:
-    typedef typename Base::IndexVector IndexVector;
-    enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
-public:
+template <typename SparseMatrixType, int BlockRows, int BlockCols>
+class sparse_matrix_block_impl : public SparseCompressedBase<Block<SparseMatrixType, BlockRows, BlockCols, true> > {
+  typedef internal::remove_all_t<typename SparseMatrixType::Nested> MatrixTypeNested_;
+  typedef Block<SparseMatrixType, BlockRows, BlockCols, true> BlockType;
+  typedef SparseCompressedBase<Block<SparseMatrixType, BlockRows, BlockCols, true> > Base;
+  using Base::convert_index;
 
-    inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index i)
-      : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize)
-    {}
+ public:
+  enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+  EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+ protected:
+  typedef typename Base::IndexVector IndexVector;
+  enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
 
-    inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
-      : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols))
-    {}
+ public:
+  inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index i)
+      : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize) {}
 
-    template<typename OtherDerived>
-    inline BlockType& operator=(const SparseMatrixBase<OtherDerived>& other)
-    {
-      typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _NestedMatrixType;
-      _NestedMatrixType& matrix = m_matrix;
-      // This assignment is slow if this vector set is not empty
-      // and/or it is not at the end of the nonzeros of the underlying matrix.
+  inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows,
+                                  Index blockCols)
+      : m_matrix(xpr),
+        m_outerStart(convert_index(IsRowMajor ? startRow : startCol)),
+        m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols)) {}
 
-      // 1 - eval to a temporary to avoid transposition and/or aliasing issues
-      Ref<const SparseMatrix<Scalar, IsRowMajor ? RowMajor : ColMajor, StorageIndex> > tmp(other.derived());
-      eigen_internal_assert(tmp.outerSize()==m_outerSize.value());
+  template <typename OtherDerived>
+  inline BlockType& operator=(const SparseMatrixBase<OtherDerived>& other) {
+    typedef internal::remove_all_t<typename SparseMatrixType::Nested> NestedMatrixType_;
+    NestedMatrixType_& matrix = m_matrix;
+    // This assignment is slow if this vector set is not empty
+    // and/or it is not at the end of the nonzeros of the underlying matrix.
 
-      // 2 - let's check whether there is enough allocated memory
-      Index nnz           = tmp.nonZeros();
-      Index start         = m_outerStart==0 ? 0 : m_matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block
-      Index end           = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending position of the current block
-      Index block_size    = end - start;                                                // available room in the current block
-      Index tail_size     = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end;
+    // 1 - eval to a temporary to avoid transposition and/or aliasing issues
+    Ref<const SparseMatrix<Scalar, IsRowMajor ? RowMajor : ColMajor, StorageIndex> > tmp(other.derived());
+    eigen_internal_assert(tmp.outerSize() == m_outerSize.value());
 
-      Index free_size     = m_matrix.isCompressed()
-                          ? Index(matrix.data().allocatedSize()) + block_size
-                          : block_size;
+    // 2 - let's check whether there is enough allocated memory
+    Index nnz = tmp.nonZeros();
+    Index start =
+        m_outerStart == 0 ? 0 : m_matrix.outerIndexPtr()[m_outerStart];        // starting position of the current block
+    Index end = m_matrix.outerIndexPtr()[m_outerStart + m_outerSize.value()];  // ending position of the current block
+    Index block_size = end - start;                                            // available room in the current block
+    Index tail_size = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end;
 
-      Index tmp_start = tmp.outerIndexPtr()[0];
+    Index free_size = m_matrix.isCompressed() ? Index(matrix.data().allocatedSize()) + block_size : block_size;
 
-      bool update_trailing_pointers = false;
-      if(nnz>free_size)
-      {
-        // realloc manually to reduce copies
-        typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz);
+    Index tmp_start = tmp.outerIndexPtr()[0];
 
-        internal::smart_copy(m_matrix.valuePtr(),       m_matrix.valuePtr() + start,      newdata.valuePtr());
-        internal::smart_copy(m_matrix.innerIndexPtr(),  m_matrix.innerIndexPtr() + start, newdata.indexPtr());
+    bool update_trailing_pointers = false;
+    if (nnz > free_size) {
+      // realloc manually to reduce copies
+      typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz);
 
-        internal::smart_copy(tmp.valuePtr() + tmp_start,      tmp.valuePtr() + tmp_start + nnz,       newdata.valuePtr() + start);
-        internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz,  newdata.indexPtr() + start);
+      internal::smart_copy(m_matrix.valuePtr(), m_matrix.valuePtr() + start, newdata.valuePtr());
+      internal::smart_copy(m_matrix.innerIndexPtr(), m_matrix.innerIndexPtr() + start, newdata.indexPtr());
 
-        internal::smart_copy(matrix.valuePtr()+end,       matrix.valuePtr()+end + tail_size,      newdata.valuePtr()+start+nnz);
-        internal::smart_copy(matrix.innerIndexPtr()+end,  matrix.innerIndexPtr()+end + tail_size, newdata.indexPtr()+start+nnz);
+      internal::smart_copy(tmp.valuePtr() + tmp_start, tmp.valuePtr() + tmp_start + nnz, newdata.valuePtr() + start);
+      internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz,
+                           newdata.indexPtr() + start);
 
-        newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz);
+      internal::smart_copy(matrix.valuePtr() + end, matrix.valuePtr() + end + tail_size,
+                           newdata.valuePtr() + start + nnz);
+      internal::smart_copy(matrix.innerIndexPtr() + end, matrix.innerIndexPtr() + end + tail_size,
+                           newdata.indexPtr() + start + nnz);
 
-        matrix.data().swap(newdata);
+      newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz);
+
+      matrix.data().swap(newdata);
+
+      update_trailing_pointers = true;
+    } else {
+      if (m_matrix.isCompressed() && nnz != block_size) {
+        // no need to realloc, simply copy the tail at its respective position and insert tmp
+        matrix.data().resize(start + nnz + tail_size);
+
+        internal::smart_memmove(matrix.valuePtr() + end, matrix.valuePtr() + end + tail_size,
+                                matrix.valuePtr() + start + nnz);
+        internal::smart_memmove(matrix.innerIndexPtr() + end, matrix.innerIndexPtr() + end + tail_size,
+                                matrix.innerIndexPtr() + start + nnz);
 
         update_trailing_pointers = true;
       }
-      else
-      {
-        if(m_matrix.isCompressed() && nnz!=block_size)
-        {
-          // no need to realloc, simply copy the tail at its respective position and insert tmp
-          matrix.data().resize(start + nnz + tail_size);
 
-          internal::smart_memmove(matrix.valuePtr()+end,      matrix.valuePtr() + end+tail_size,      matrix.valuePtr() + start+nnz);
-          internal::smart_memmove(matrix.innerIndexPtr()+end, matrix.innerIndexPtr() + end+tail_size, matrix.innerIndexPtr() + start+nnz);
+      internal::smart_copy(tmp.valuePtr() + tmp_start, tmp.valuePtr() + tmp_start + nnz, matrix.valuePtr() + start);
+      internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz,
+                           matrix.innerIndexPtr() + start);
+    }
 
-          update_trailing_pointers = true;
-        }
-
-        internal::smart_copy(tmp.valuePtr() + tmp_start,      tmp.valuePtr() + tmp_start + nnz,       matrix.valuePtr() + start);
-        internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz,  matrix.innerIndexPtr() + start);
+    // update outer index pointers and innerNonZeros
+    if (IsVectorAtCompileTime) {
+      if (!m_matrix.isCompressed()) matrix.innerNonZeroPtr()[m_outerStart] = StorageIndex(nnz);
+      matrix.outerIndexPtr()[m_outerStart] = StorageIndex(start);
+    } else {
+      StorageIndex p = StorageIndex(start);
+      for (Index k = 0; k < m_outerSize.value(); ++k) {
+        StorageIndex nnz_k = internal::convert_index<StorageIndex>(tmp.innerVector(k).nonZeros());
+        if (!m_matrix.isCompressed()) matrix.innerNonZeroPtr()[m_outerStart + k] = nnz_k;
+        matrix.outerIndexPtr()[m_outerStart + k] = p;
+        p += nnz_k;
       }
+    }
 
-      // update outer index pointers and innerNonZeros
-      if(IsVectorAtCompileTime)
-      {
-        if(!m_matrix.isCompressed())
-          matrix.innerNonZeroPtr()[m_outerStart] = StorageIndex(nnz);
-        matrix.outerIndexPtr()[m_outerStart] = StorageIndex(start);
+    if (update_trailing_pointers) {
+      StorageIndex offset = internal::convert_index<StorageIndex>(nnz - block_size);
+      for (Index k = m_outerStart + m_outerSize.value(); k <= matrix.outerSize(); ++k) {
+        matrix.outerIndexPtr()[k] += offset;
       }
-      else
-      {
-        StorageIndex p = StorageIndex(start);
-        for(Index k=0; k<m_outerSize.value(); ++k)
-        {
-          StorageIndex nnz_k = internal::convert_index<StorageIndex>(tmp.innerVector(k).nonZeros());
-          if(!m_matrix.isCompressed())
-            matrix.innerNonZeroPtr()[m_outerStart+k] = nnz_k;
-          matrix.outerIndexPtr()[m_outerStart+k] = p;
-          p += nnz_k;
-        }
-      }
-
-      if(update_trailing_pointers)
-      {
-        StorageIndex offset = internal::convert_index<StorageIndex>(nnz - block_size);
-        for(Index k = m_outerStart + m_outerSize.value(); k<=matrix.outerSize(); ++k)
-        {
-          matrix.outerIndexPtr()[k] += offset;
-        }
-      }
-
-      return derived();
     }
 
-    inline BlockType& operator=(const BlockType& other)
-    {
-      return operator=<BlockType>(other);
-    }
+    return derived();
+  }
 
-    inline const Scalar* valuePtr() const
-    { return m_matrix.valuePtr(); }
-    inline Scalar* valuePtr()
-    { return m_matrix.valuePtr(); }
+  inline BlockType& operator=(const BlockType& other) { return operator= <BlockType>(other); }
 
-    inline const StorageIndex* innerIndexPtr() const
-    { return m_matrix.innerIndexPtr(); }
-    inline StorageIndex* innerIndexPtr()
-    { return m_matrix.innerIndexPtr(); }
+  inline const Scalar* valuePtr() const { return m_matrix.valuePtr(); }
+  inline Scalar* valuePtr() { return m_matrix.valuePtr(); }
 
-    inline const StorageIndex* outerIndexPtr() const
-    { return m_matrix.outerIndexPtr() + m_outerStart; }
-    inline StorageIndex* outerIndexPtr()
-    { return m_matrix.outerIndexPtr() + m_outerStart; }
+  inline const StorageIndex* innerIndexPtr() const { return m_matrix.innerIndexPtr(); }
+  inline StorageIndex* innerIndexPtr() { return m_matrix.innerIndexPtr(); }
 
-    inline const StorageIndex* innerNonZeroPtr() const
-    { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); }
-    inline StorageIndex* innerNonZeroPtr()
-    { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); }
+  inline const StorageIndex* outerIndexPtr() const { return m_matrix.outerIndexPtr() + m_outerStart; }
+  inline StorageIndex* outerIndexPtr() { return m_matrix.outerIndexPtr() + m_outerStart; }
 
-    bool isCompressed() const { return m_matrix.innerNonZeroPtr()==0; }
+  inline const StorageIndex* innerNonZeroPtr() const {
+    return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr() + m_outerStart);
+  }
+  inline StorageIndex* innerNonZeroPtr() { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr() + m_outerStart); }
 
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      return m_matrix.coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
-    }
+  bool isCompressed() const { return m_matrix.innerNonZeroPtr() == 0; }
 
-    inline const Scalar coeff(Index row, Index col) const
-    {
-      return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
-    }
+  inline Scalar& coeffRef(Index row, Index col) {
+    return m_matrix.coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart));
+  }
 
-    inline const Scalar coeff(Index index) const
-    {
-      return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index :  m_outerStart);
-    }
+  inline const Scalar coeff(Index row, Index col) const {
+    return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart));
+  }
 
-    const Scalar& lastCoeff() const
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(sparse_matrix_block_impl);
-      eigen_assert(Base::nonZeros()>0);
-      if(m_matrix.isCompressed())
-        return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1];
-      else
-        return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1];
-    }
+  inline const Scalar coeff(Index index) const {
+    return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart);
+  }
 
-    EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
-    EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+  const Scalar& lastCoeff() const {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(sparse_matrix_block_impl);
+    eigen_assert(Base::nonZeros() > 0);
+    if (m_matrix.isCompressed())
+      return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart + 1] - 1];
+    else
+      return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart] + m_matrix.innerNonZeroPtr()[m_outerStart] - 1];
+  }
 
-    inline const SparseMatrixType& nestedExpression() const { return m_matrix; }
-    inline SparseMatrixType& nestedExpression() { return m_matrix; }
-    Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
-    Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
-    Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
-    Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+  EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+  EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
 
-  protected:
+  inline const SparseMatrixType& nestedExpression() const { return m_matrix; }
+  inline SparseMatrixType& nestedExpression() { return m_matrix; }
+  Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
+  Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
+  Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+  Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
 
-    typename internal::ref_selector<SparseMatrixType>::non_const_type m_matrix;
-    Index m_outerStart;
-    const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
-
+ protected:
+  typename internal::ref_selector<SparseMatrixType>::non_const_type m_matrix;
+  Index m_outerStart;
+  const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
 };
 
-} // namespace internal
+}  // namespace internal
 
-template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
-class BlockImpl<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true,Sparse>
-  : public internal::sparse_matrix_block_impl<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols>
-{
-public:
-  typedef _StorageIndex StorageIndex;
-  typedef SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType;
-  typedef internal::sparse_matrix_block_impl<SparseMatrixType,BlockRows,BlockCols> Base;
-  inline BlockImpl(SparseMatrixType& xpr, Index i)
-    : Base(xpr, i)
-  {}
+template <typename Scalar_, int Options_, typename StorageIndex_, int BlockRows, int BlockCols>
+class BlockImpl<SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true, Sparse>
+    : public internal::sparse_matrix_block_impl<SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols> {
+ public:
+  typedef StorageIndex_ StorageIndex;
+  typedef SparseMatrix<Scalar_, Options_, StorageIndex_> SparseMatrixType;
+  typedef internal::sparse_matrix_block_impl<SparseMatrixType, BlockRows, BlockCols> Base;
+  inline BlockImpl(SparseMatrixType& xpr, Index i) : Base(xpr, i) {}
 
   inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
-    : Base(xpr, startRow, startCol, blockRows, blockCols)
-  {}
+      : Base(xpr, startRow, startCol, blockRows, blockCols) {}
 
   using Base::operator=;
 };
 
-template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
-class BlockImpl<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true,Sparse>
-  : public internal::sparse_matrix_block_impl<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols>
-{
-public:
-  typedef _StorageIndex StorageIndex;
-  typedef const SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType;
-  typedef internal::sparse_matrix_block_impl<SparseMatrixType,BlockRows,BlockCols> Base;
-  inline BlockImpl(SparseMatrixType& xpr, Index i)
-    : Base(xpr, i)
-  {}
+template <typename Scalar_, int Options_, typename StorageIndex_, int BlockRows, int BlockCols>
+class BlockImpl<const SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true, Sparse>
+    : public internal::sparse_matrix_block_impl<const SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows,
+                                                BlockCols> {
+ public:
+  typedef StorageIndex_ StorageIndex;
+  typedef const SparseMatrix<Scalar_, Options_, StorageIndex_> SparseMatrixType;
+  typedef internal::sparse_matrix_block_impl<SparseMatrixType, BlockRows, BlockCols> Base;
+  inline BlockImpl(SparseMatrixType& xpr, Index i) : Base(xpr, i) {}
 
   inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
-    : Base(xpr, startRow, startCol, blockRows, blockCols)
-  {}
+      : Base(xpr, startRow, startCol, blockRows, blockCols) {}
 
   using Base::operator=;
-private:
-  template<typename Derived> BlockImpl(const SparseMatrixBase<Derived>& xpr, Index i);
-  template<typename Derived> BlockImpl(const SparseMatrixBase<Derived>& xpr);
+
+ private:
+  template <typename Derived>
+  BlockImpl(const SparseMatrixBase<Derived>& xpr, Index i);
+  template <typename Derived>
+  BlockImpl(const SparseMatrixBase<Derived>& xpr);
 };
 
 //----------
 
 /** Generic implementation of sparse Block expression.
-  * Real-only.
-  */
-template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
-class BlockImpl<XprType,BlockRows,BlockCols,InnerPanel,Sparse>
-  : public SparseMatrixBase<Block<XprType,BlockRows,BlockCols,InnerPanel> >, internal::no_assignment_operator
-{
-    typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
-    typedef SparseMatrixBase<BlockType> Base;
-    using Base::convert_index;
-public:
-    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
-    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+ * Real-only.
+ */
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+class BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, Sparse>
+    : public SparseMatrixBase<Block<XprType, BlockRows, BlockCols, InnerPanel> >, internal::no_assignment_operator {
+  typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
+  typedef SparseMatrixBase<BlockType> Base;
+  using Base::convert_index;
 
-    typedef typename internal::remove_all<typename XprType::Nested>::type _MatrixTypeNested;
+ public:
+  enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+  EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
 
-    /** Column or Row constructor
-      */
-    inline BlockImpl(XprType& xpr, Index i)
+  typedef internal::remove_all_t<typename XprType::Nested> MatrixTypeNested_;
+
+  /** Column or Row constructor
+   */
+  inline BlockImpl(XprType& xpr, Index i)
       : m_matrix(xpr),
-        m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? convert_index(i) : 0),
-        m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? convert_index(i) : 0),
-        m_blockRows(BlockRows==1 ? 1 : xpr.rows()),
-        m_blockCols(BlockCols==1 ? 1 : xpr.cols())
-    {}
+        m_startRow((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) ? convert_index(i) : 0),
+        m_startCol((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) ? convert_index(i) : 0),
+        m_blockRows(BlockRows == 1 ? 1 : xpr.rows()),
+        m_blockCols(BlockCols == 1 ? 1 : xpr.cols()) {}
 
-    /** Dynamic-size constructor
-      */
-    inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
-      : m_matrix(xpr), m_startRow(convert_index(startRow)), m_startCol(convert_index(startCol)), m_blockRows(convert_index(blockRows)), m_blockCols(convert_index(blockCols))
-    {}
+  /** Dynamic-size constructor
+   */
+  inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+      : m_matrix(xpr),
+        m_startRow(convert_index(startRow)),
+        m_startCol(convert_index(startCol)),
+        m_blockRows(convert_index(blockRows)),
+        m_blockCols(convert_index(blockCols)) {}
 
-    inline Index rows() const { return m_blockRows.value(); }
-    inline Index cols() const { return m_blockCols.value(); }
+  inline Index rows() const { return m_blockRows.value(); }
+  inline Index cols() const { return m_blockCols.value(); }
 
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      return m_matrix.coeffRef(row + m_startRow.value(), col + m_startCol.value());
-    }
+  inline Scalar& coeffRef(Index row, Index col) {
+    return m_matrix.coeffRef(row + m_startRow.value(), col + m_startCol.value());
+  }
 
-    inline const Scalar coeff(Index row, Index col) const
-    {
-      return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value());
-    }
+  inline const Scalar coeff(Index row, Index col) const {
+    return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value());
+  }
 
-    inline Scalar& coeffRef(Index index)
-    {
-      return m_matrix.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
-                               m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
-    }
+  inline Scalar& coeffRef(Index index) {
+    return m_matrix.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                             m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+  }
 
-    inline const Scalar coeff(Index index) const
-    {
-      return m_matrix.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
-                            m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
-    }
+  inline const Scalar coeff(Index index) const {
+    return m_matrix.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                          m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+  }
 
-    inline const XprType& nestedExpression() const { return m_matrix; }
-    inline XprType& nestedExpression() { return m_matrix; }
-    Index startRow() const { return m_startRow.value(); }
-    Index startCol() const { return m_startCol.value(); }
-    Index blockRows() const { return m_blockRows.value(); }
-    Index blockCols() const { return m_blockCols.value(); }
+  inline const XprType& nestedExpression() const { return m_matrix; }
+  inline XprType& nestedExpression() { return m_matrix; }
+  Index startRow() const { return m_startRow.value(); }
+  Index startCol() const { return m_startCol.value(); }
+  Index blockRows() const { return m_blockRows.value(); }
+  Index blockCols() const { return m_blockCols.value(); }
 
-  protected:
-//     friend class internal::GenericSparseBlockInnerIteratorImpl<XprType,BlockRows,BlockCols,InnerPanel>;
-    friend struct internal::unary_evaluator<Block<XprType,BlockRows,BlockCols,InnerPanel>, internal::IteratorBased, Scalar >;
+ protected:
+  //     friend class internal::GenericSparseBlockInnerIteratorImpl<XprType,BlockRows,BlockCols,InnerPanel>;
+  friend struct internal::unary_evaluator<Block<XprType, BlockRows, BlockCols, InnerPanel>, internal::IteratorBased,
+                                          Scalar>;
 
-    Index nonZeros() const { return Dynamic; }
+  Index nonZeros() const { return Dynamic; }
 
-    typename internal::ref_selector<XprType>::non_const_type m_matrix;
-    const internal::variable_if_dynamic<Index, XprType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;
-    const internal::variable_if_dynamic<Index, XprType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
-    const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_blockRows;
-    const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_blockCols;
+  typename internal::ref_selector<XprType>::non_const_type m_matrix;
+  const internal::variable_if_dynamic<Index, XprType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;
+  const internal::variable_if_dynamic<Index, XprType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
+  const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_blockRows;
+  const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_blockCols;
 
-  protected:
-    // Disable assignment with clear error message.
-    // Note that simply removing operator= yields compilation errors with ICC+MSVC
-    template<typename T>
-    BlockImpl& operator=(const T&)
-    {
-      EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
-      return *this;
-    }
-
+ protected:
+  // Disable assignment with clear error message.
+  // Note that simply removing operator= yields compilation errors with ICC+MSVC
+  template <typename T>
+  BlockImpl& operator=(const T&) {
+    EIGEN_STATIC_ASSERT(sizeof(T) == 0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
+    return *this;
+  }
 };
 
 namespace internal {
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
-struct unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased >
- : public evaluator_base<Block<ArgType,BlockRows,BlockCols,InnerPanel> >
-{
-    class InnerVectorInnerIterator;
-    class OuterVectorInnerIterator;
-  public:
-    typedef Block<ArgType,BlockRows,BlockCols,InnerPanel> XprType;
-    typedef typename XprType::StorageIndex StorageIndex;
-    typedef typename XprType::Scalar Scalar;
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IteratorBased>
+    : public evaluator_base<Block<ArgType, BlockRows, BlockCols, InnerPanel> > {
+  class InnerVectorInnerIterator;
+  class OuterVectorInnerIterator;
 
-    enum {
-      IsRowMajor = XprType::IsRowMajor,
+ public:
+  typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename XprType::Scalar Scalar;
 
-      OuterVector =  (BlockCols==1 && ArgType::IsRowMajor)
-                    | // FIXME | instead of || to please GCC 4.4.0 stupid warning "suggest parentheses around &&".
-                      // revert to || as soon as not needed anymore.
-                     (BlockRows==1 && !ArgType::IsRowMajor),
+  enum {
+    IsRowMajor = XprType::IsRowMajor,
+    OuterVector = (BlockCols == 1 && ArgType::IsRowMajor) || (BlockRows == 1 && !ArgType::IsRowMajor),
+    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+    Flags = XprType::Flags
+  };
 
-      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-      Flags = XprType::Flags
-    };
+  typedef std::conditional_t<OuterVector, OuterVectorInnerIterator, InnerVectorInnerIterator> InnerIterator;
 
-    typedef typename internal::conditional<OuterVector,OuterVectorInnerIterator,InnerVectorInnerIterator>::type InnerIterator;
+  explicit unary_evaluator(const XprType& op) : m_argImpl(op.nestedExpression()), m_block(op) {}
 
-    explicit unary_evaluator(const XprType& op)
-      : m_argImpl(op.nestedExpression()), m_block(op)
-    {}
-
-    inline Index nonZerosEstimate() const {
-      const Index nnz = m_block.nonZeros();
-      if(nnz < 0) {
-        // Scale the non-zero estimate for the underlying expression linearly with block size.
-        // Return zero if the underlying block is empty.
-        const Index nested_sz = m_block.nestedExpression().size();        
-        return nested_sz == 0 ? 0 : m_argImpl.nonZerosEstimate() * m_block.size() / nested_sz;
-      }
-      return nnz;
+  inline Index nonZerosEstimate() const {
+    const Index nnz = m_block.nonZeros();
+    if (nnz < 0) {
+      // Scale the non-zero estimate for the underlying expression linearly with block size.
+      // Return zero if the underlying block is empty.
+      const Index nested_sz = m_block.nestedExpression().size();
+      return nested_sz == 0 ? 0 : m_argImpl.nonZerosEstimate() * m_block.size() / nested_sz;
     }
+    return nnz;
+  }
 
-  protected:
-    typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+ protected:
+  typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
 
-    evaluator<ArgType> m_argImpl;
-    const XprType &m_block;
+  evaluator<ArgType> m_argImpl;
+  const XprType& m_block;
 };
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
-class unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased>::InnerVectorInnerIterator
- : public EvalIterator
-{
-  // NOTE MSVC fails to compile if we don't explicitely "import" IsRowMajor from unary_evaluator
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+class unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IteratorBased>::InnerVectorInnerIterator
+    : public EvalIterator {
+  // NOTE MSVC fails to compile if we don't explicitly "import" IsRowMajor from unary_evaluator
   //      because the base class EvalIterator has a private IsRowMajor enum too. (bug #1786)
   // NOTE We cannot call it IsRowMajor because it would shadow unary_evaluator::IsRowMajor
   enum { XprIsRowMajor = unary_evaluator::IsRowMajor };
   const XprType& m_block;
   Index m_end;
-public:
 
+ public:
   EIGEN_STRONG_INLINE InnerVectorInnerIterator(const unary_evaluator& aEval, Index outer)
-    : EvalIterator(aEval.m_argImpl, outer + (XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol())),
-      m_block(aEval.m_block),
-      m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows())
-  {
-    while( (EvalIterator::operator bool()) && (EvalIterator::index() < (XprIsRowMajor ? m_block.startCol() : m_block.startRow())) )
+      : EvalIterator(aEval.m_argImpl, outer + (XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol())),
+        m_block(aEval.m_block),
+        m_end(XprIsRowMajor ? aEval.m_block.startCol() + aEval.m_block.blockCols()
+                            : aEval.m_block.startRow() + aEval.m_block.blockRows()) {
+    while ((EvalIterator::operator bool()) &&
+           (EvalIterator::index() < (XprIsRowMajor ? m_block.startCol() : m_block.startRow())))
       EvalIterator::operator++();
   }
 
-  inline StorageIndex index() const { return EvalIterator::index() - convert_index<StorageIndex>(XprIsRowMajor ? m_block.startCol() : m_block.startRow()); }
-  inline Index outer()  const { return EvalIterator::outer() - (XprIsRowMajor ? m_block.startRow() : m_block.startCol()); }
-  inline Index row()    const { return EvalIterator::row()   - m_block.startRow(); }
-  inline Index col()    const { return EvalIterator::col()   - m_block.startCol(); }
+  inline StorageIndex index() const {
+    return EvalIterator::index() - convert_index<StorageIndex>(XprIsRowMajor ? m_block.startCol() : m_block.startRow());
+  }
+  inline Index outer() const {
+    return EvalIterator::outer() - (XprIsRowMajor ? m_block.startRow() : m_block.startCol());
+  }
+  inline Index row() const { return EvalIterator::row() - m_block.startRow(); }
+  inline Index col() const { return EvalIterator::col() - m_block.startCol(); }
 
   inline operator bool() const { return EvalIterator::operator bool() && EvalIterator::index() < m_end; }
 };
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
-class unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased>::OuterVectorInnerIterator
-{
+template <typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+class unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IteratorBased>::OuterVectorInnerIterator {
   // NOTE see above
   enum { XprIsRowMajor = unary_evaluator::IsRowMajor };
   const unary_evaluator& m_eval;
@@ -502,42 +465,42 @@
   const Index m_innerIndex;
   Index m_end;
   EvalIterator m_it;
-public:
 
+ public:
   EIGEN_STRONG_INLINE OuterVectorInnerIterator(const unary_evaluator& aEval, Index outer)
-    : m_eval(aEval),
-      m_outerPos( (XprIsRowMajor ? aEval.m_block.startCol() : aEval.m_block.startRow()) ),
-      m_innerIndex(XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol()),
-      m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows()),
-      m_it(m_eval.m_argImpl, m_outerPos)
-  {
+      : m_eval(aEval),
+        m_outerPos((XprIsRowMajor ? aEval.m_block.startCol() : aEval.m_block.startRow())),
+        m_innerIndex(XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol()),
+        m_end(XprIsRowMajor ? aEval.m_block.startCol() + aEval.m_block.blockCols()
+                            : aEval.m_block.startRow() + aEval.m_block.blockRows()),
+        m_it(m_eval.m_argImpl, m_outerPos) {
     EIGEN_UNUSED_VARIABLE(outer);
-    eigen_assert(outer==0);
+    eigen_assert(outer == 0);
 
-    while(m_it && m_it.index() < m_innerIndex) ++m_it;
-    if((!m_it) || (m_it.index()!=m_innerIndex))
-      ++(*this);
+    while (m_it && m_it.index() < m_innerIndex) ++m_it;
+    if ((!m_it) || (m_it.index() != m_innerIndex)) ++(*this);
   }
 
-  inline StorageIndex index() const { return convert_index<StorageIndex>(m_outerPos - (XprIsRowMajor ? m_eval.m_block.startCol() : m_eval.m_block.startRow())); }
-  inline Index outer()  const { return 0; }
-  inline Index row()    const { return XprIsRowMajor ? 0 : index(); }
-  inline Index col()    const { return XprIsRowMajor ? index() : 0; }
+  inline StorageIndex index() const {
+    return convert_index<StorageIndex>(m_outerPos -
+                                       (XprIsRowMajor ? m_eval.m_block.startCol() : m_eval.m_block.startRow()));
+  }
+  inline Index outer() const { return 0; }
+  inline Index row() const { return XprIsRowMajor ? 0 : index(); }
+  inline Index col() const { return XprIsRowMajor ? index() : 0; }
 
   inline Scalar value() const { return m_it.value(); }
   inline Scalar& valueRef() { return m_it.valueRef(); }
 
-  inline OuterVectorInnerIterator& operator++()
-  {
+  inline OuterVectorInnerIterator& operator++() {
     // search next non-zero entry
-    while(++m_outerPos<m_end)
-    {
+    while (++m_outerPos < m_end) {
       // Restart iterator at the next inner-vector:
-      m_it.~EvalIterator();
-      ::new (&m_it) EvalIterator(m_eval.m_argImpl, m_outerPos);
+      internal::destroy_at(&m_it);
+      internal::construct_at(&m_it, m_eval.m_argImpl, m_outerPos);
       // search for the key m_innerIndex in the current outer-vector
-      while(m_it && m_it.index() < m_innerIndex) ++m_it;
-      if(m_it && m_it.index()==m_innerIndex) break;
+      while (m_it && m_it.index() < m_innerIndex) ++m_it;
+      if (m_it && m_it.index() == m_innerIndex) break;
     }
     return *this;
   }
@@ -545,27 +508,27 @@
   inline operator bool() const { return m_outerPos < m_end; }
 };
 
-template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
-struct unary_evaluator<Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true>, IteratorBased>
-  : evaluator<SparseCompressedBase<Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> > >
-{
-  typedef Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> XprType;
+template <typename Scalar_, int Options_, typename StorageIndex_, int BlockRows, int BlockCols>
+struct unary_evaluator<Block<SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true>, IteratorBased>
+    : evaluator<
+          SparseCompressedBase<Block<SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true> > > {
+  typedef Block<SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true> XprType;
   typedef evaluator<SparseCompressedBase<XprType> > Base;
-  explicit unary_evaluator(const XprType &xpr) : Base(xpr) {}
+  explicit unary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
-template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
-struct unary_evaluator<Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true>, IteratorBased>
-  : evaluator<SparseCompressedBase<Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> > >
-{
-  typedef Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> XprType;
+template <typename Scalar_, int Options_, typename StorageIndex_, int BlockRows, int BlockCols>
+struct unary_evaluator<Block<const SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true>,
+                       IteratorBased>
+    : evaluator<SparseCompressedBase<
+          Block<const SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true> > > {
+  typedef Block<const SparseMatrix<Scalar_, Options_, StorageIndex_>, BlockRows, BlockCols, true> XprType;
   typedef evaluator<SparseCompressedBase<XprType> > Base;
-  explicit unary_evaluator(const XprType &xpr) : Base(xpr) {}
+  explicit unary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
-} // end namespace internal
+}  // end namespace internal
 
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_SPARSE_BLOCK_H
+#endif  // EIGEN_SPARSE_BLOCK_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h
index ebe02d1..76575c9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h
@@ -7,11 +7,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
+/*
 
-/* 
- 
- * NOTE: This file is the modified version of sp_coletree.c file in SuperLU 
- 
+ * NOTE: This file is the modified version of sp_coletree.c file in SuperLU
+
  * -- SuperLU routine (version 3.1) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -31,55 +30,54 @@
 #ifndef SPARSE_COLETREE_H
 #define SPARSE_COLETREE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-/** Find the root of the tree/set containing the vertex i : Use Path halving */ 
-template<typename Index, typename IndexVector>
-Index etree_find (Index i, IndexVector& pp)
-{
-  Index p = pp(i); // Parent 
-  Index gp = pp(p); // Grand parent 
-  while (gp != p) 
-  {
-    pp(i) = gp; // Parent pointer on find path is changed to former grand parent
-    i = gp; 
+/** Find the root of the tree/set containing the vertex i : Use Path halving */
+template <typename Index, typename IndexVector>
+Index etree_find(Index i, IndexVector& pp) {
+  Index p = pp(i);   // Parent
+  Index gp = pp(p);  // Grand parent
+  while (gp != p) {
+    pp(i) = gp;  // Parent pointer on find path is changed to former grand parent
+    i = gp;
     p = pp(i);
     gp = pp(p);
   }
-  return p; 
+  return p;
 }
 
 /** Compute the column elimination tree of a sparse matrix
-  * \param mat The matrix in column-major format. 
-  * \param parent The elimination tree
-  * \param firstRowElt The column index of the first element in each row
-  * \param perm The permutation to apply to the column of \b mat
-  */
+ * \param mat The matrix in column-major format.
+ * \param parent The elimination tree
+ * \param firstRowElt The column index of the first element in each row
+ * \param perm The permutation to apply to the column of \b mat
+ */
 template <typename MatrixType, typename IndexVector>
-int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowElt, typename MatrixType::StorageIndex *perm=0)
-{
+int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowElt,
+             typename MatrixType::StorageIndex* perm = 0) {
   typedef typename MatrixType::StorageIndex StorageIndex;
-  StorageIndex nc = convert_index<StorageIndex>(mat.cols()); // Number of columns
+  StorageIndex nc = convert_index<StorageIndex>(mat.cols());  // Number of columns
   StorageIndex m = convert_index<StorageIndex>(mat.rows());
-  StorageIndex diagSize = (std::min)(nc,m);
-  IndexVector root(nc); // root of subtree of etree 
+  StorageIndex diagSize = (std::min)(nc, m);
+  IndexVector root(nc);  // root of subtree of etree
   root.setZero();
-  IndexVector pp(nc); // disjoint sets 
-  pp.setZero(); // Initialize disjoint sets 
+  IndexVector pp(nc);  // disjoint sets
+  pp.setZero();        // Initialize disjoint sets
   parent.resize(mat.cols());
-  //Compute first nonzero column in each row 
+  // Compute first nonzero column in each row
   firstRowElt.resize(m);
   firstRowElt.setConstant(nc);
-  firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1);
+  firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize - 1);
   bool found_diag;
-  for (StorageIndex col = 0; col < nc; col++)
-  {
+  for (StorageIndex col = 0; col < nc; col++) {
     StorageIndex pcol = col;
-    if(perm) pcol  = perm[col];
-    for (typename MatrixType::InnerIterator it(mat, pcol); it; ++it)
-    { 
+    if (perm) pcol = perm[col];
+    for (typename MatrixType::InnerIterator it(mat, pcol); it; ++it) {
       Index row = it.row();
       firstRowElt(row) = (std::min)(firstRowElt(row), col);
     }
@@ -89,118 +87,108 @@
     Thus each row clique in A'*A is replaced by a star
     centered at its first vertex, which has the same fill. */
   StorageIndex rset, cset, rroot;
-  for (StorageIndex col = 0; col < nc; col++) 
-  {
-    found_diag = col>=m;
-    pp(col) = col; 
-    cset = col; 
-    root(cset) = col; 
-    parent(col) = nc; 
+  for (StorageIndex col = 0; col < nc; col++) {
+    found_diag = col >= m;
+    pp(col) = col;
+    cset = col;
+    root(cset) = col;
+    parent(col) = nc;
     /* The diagonal element is treated here even if it does not exist in the matrix
-     * hence the loop is executed once more */ 
+     * hence the loop is executed once more */
     StorageIndex pcol = col;
-    if(perm) pcol  = perm[col];
-    for (typename MatrixType::InnerIterator it(mat, pcol); it||!found_diag; ++it)
-    { //  A sequence of interleaved find and union is performed 
+    if (perm) pcol = perm[col];
+    for (typename MatrixType::InnerIterator it(mat, pcol); it || !found_diag;
+         ++it) {  //  A sequence of interleaved find and union is performed
       Index i = col;
-      if(it) i = it.index();
+      if (it) i = it.index();
       if (i == col) found_diag = true;
-      
+
       StorageIndex row = firstRowElt(i);
-      if (row >= col) continue; 
-      rset = internal::etree_find(row, pp); // Find the name of the set containing row
+      if (row >= col) continue;
+      rset = internal::etree_find(row, pp);  // Find the name of the set containing row
       rroot = root(rset);
-      if (rroot != col) 
-      {
-        parent(rroot) = col; 
-        pp(cset) = rset; 
-        cset = rset; 
-        root(cset) = col; 
+      if (rroot != col) {
+        parent(rroot) = col;
+        pp(cset) = rset;
+        cset = rset;
+        root(cset) = col;
       }
     }
   }
-  return 0;  
+  return 0;
 }
 
-/** 
-  * Depth-first search from vertex n.  No recursion.
-  * This routine was contributed by Cédric Doucet, CEDRAT Group, Meylan, France.
-*/
-template <typename IndexVector>
-void nr_etdfs (typename IndexVector::Scalar n, IndexVector& parent, IndexVector& first_kid, IndexVector& next_kid, IndexVector& post, typename IndexVector::Scalar postnum)
-{
-  typedef typename IndexVector::Scalar StorageIndex;
-  StorageIndex current = n, first, next;
-  while (postnum != n) 
-  {
-    // No kid for the current node
-    first = first_kid(current);
-    
-    // no kid for the current node
-    if (first == -1) 
-    {
-      // Numbering this node because it has no kid 
-      post(current) = postnum++;
-      
-      // looking for the next kid 
-      next = next_kid(current); 
-      while (next == -1) 
-      {
-        // No more kids : back to the parent node
-        current = parent(current); 
-        // numbering the parent node 
-        post(current) = postnum++;
-        
-        // Get the next kid 
-        next = next_kid(current); 
-      }
-      // stopping criterion 
-      if (postnum == n+1) return; 
-      
-      // Updating current node 
-      current = next; 
-    }
-    else 
-    {
-      current = first; 
-    }
-  }
-}
-
-
 /**
-  * \brief Post order a tree 
-  * \param n the number of nodes
-  * \param parent Input tree
-  * \param post postordered tree
-  */
+ * Depth-first search from vertex n.  No recursion.
+ * This routine was contributed by Cédric Doucet, CEDRAT Group, Meylan, France.
+ */
 template <typename IndexVector>
-void treePostorder(typename IndexVector::Scalar n, IndexVector& parent, IndexVector& post)
-{
+void nr_etdfs(typename IndexVector::Scalar n, IndexVector& parent, IndexVector& first_kid, IndexVector& next_kid,
+              IndexVector& post, typename IndexVector::Scalar postnum) {
   typedef typename IndexVector::Scalar StorageIndex;
-  IndexVector first_kid, next_kid; // Linked list of children 
-  StorageIndex postnum; 
-  // Allocate storage for working arrays and results 
-  first_kid.resize(n+1); 
-  next_kid.setZero(n+1);
-  post.setZero(n+1);
-  
-  // Set up structure describing children
-  first_kid.setConstant(-1); 
-  for (StorageIndex v = n-1; v >= 0; v--) 
-  {
-    StorageIndex dad = parent(v);
-    next_kid(v) = first_kid(dad); 
-    first_kid(dad) = v; 
+  StorageIndex current = n, first, next;
+  while (postnum != n) {
+    // No kid for the current node
+    first = first_kid(current);
+
+    // no kid for the current node
+    if (first == -1) {
+      // Numbering this node because it has no kid
+      post(current) = postnum++;
+
+      // looking for the next kid
+      next = next_kid(current);
+      while (next == -1) {
+        // No more kids : back to the parent node
+        current = parent(current);
+        // numbering the parent node
+        post(current) = postnum++;
+
+        // Get the next kid
+        next = next_kid(current);
+      }
+      // stopping criterion
+      if (postnum == n + 1) return;
+
+      // Updating current node
+      current = next;
+    } else {
+      current = first;
+    }
   }
-  
+}
+
+/**
+ * \brief Post order a tree
+ * \param n the number of nodes
+ * \param parent Input tree
+ * \param post postordered tree
+ */
+template <typename IndexVector>
+void treePostorder(typename IndexVector::Scalar n, IndexVector& parent, IndexVector& post) {
+  typedef typename IndexVector::Scalar StorageIndex;
+  IndexVector first_kid, next_kid;  // Linked list of children
+  StorageIndex postnum;
+  // Allocate storage for working arrays and results
+  first_kid.resize(n + 1);
+  next_kid.setZero(n + 1);
+  post.setZero(n + 1);
+
+  // Set up structure describing children
+  first_kid.setConstant(-1);
+  for (StorageIndex v = n - 1; v >= 0; v--) {
+    StorageIndex dad = parent(v);
+    next_kid(v) = first_kid(dad);
+    first_kid(dad) = v;
+  }
+
   // Depth-first search from dummy root vertex #n
-  postnum = 0; 
+  postnum = 0;
   internal::nr_etdfs(n, parent, first_kid, next_kid, post, postnum);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // SPARSE_COLETREE_H
+#endif  // SPARSE_COLETREE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h
index 6a2c7a8..c168283 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h
@@ -10,361 +10,582 @@
 #ifndef EIGEN_SPARSE_COMPRESSED_BASE_H
 #define EIGEN_SPARSE_COMPRESSED_BASE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-template<typename Derived> class SparseCompressedBase;
-  
+namespace Eigen {
+
+template <typename Derived>
+class SparseCompressedBase;
+
 namespace internal {
 
-template<typename Derived>
-struct traits<SparseCompressedBase<Derived> > : traits<Derived>
-{};
+template <typename Derived>
+struct traits<SparseCompressedBase<Derived>> : traits<Derived> {};
 
-} // end namespace internal
+template <typename Derived, class Comp, bool IsVector>
+struct inner_sort_impl;
+
+}  // end namespace internal
 
 /** \ingroup SparseCore_Module
-  * \class SparseCompressedBase
-  * \brief Common base class for sparse [compressed]-{row|column}-storage format.
-  *
-  * This class defines the common interface for all derived classes implementing the compressed sparse storage format, such as:
-  *  - SparseMatrix
-  *  - Ref<SparseMatrixType,Options>
-  *  - Map<SparseMatrixType>
-  *
-  */
-template<typename Derived>
-class SparseCompressedBase
-  : public SparseMatrixBase<Derived>
-{
-  public:
-    typedef SparseMatrixBase<Derived> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(SparseCompressedBase)
-    using Base::operator=;
-    using Base::IsRowMajor;
-    
-    class InnerIterator;
-    class ReverseInnerIterator;
-    
-  protected:
-    typedef typename Base::IndexVector IndexVector;
-    Eigen::Map<IndexVector> innerNonZeros() { return Eigen::Map<IndexVector>(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); }
-    const  Eigen::Map<const IndexVector> innerNonZeros() const { return Eigen::Map<const IndexVector>(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); }
-        
-  public:
-    
-    /** \returns the number of non zero coefficients */
-    inline Index nonZeros() const
-    {
-      if(Derived::IsVectorAtCompileTime && outerIndexPtr()==0)
-        return derived().nonZeros();
-      else if(isCompressed())
-        return outerIndexPtr()[derived().outerSize()]-outerIndexPtr()[0];
-      else if(derived().outerSize()==0)
-        return 0;
-      else
-        return innerNonZeros().sum();
-    }
-    
-    /** \returns a const pointer to the array of values.
-      * This function is aimed at interoperability with other libraries.
-      * \sa innerIndexPtr(), outerIndexPtr() */
-    inline const Scalar* valuePtr() const { return derived().valuePtr(); }
-    /** \returns a non-const pointer to the array of values.
-      * This function is aimed at interoperability with other libraries.
-      * \sa innerIndexPtr(), outerIndexPtr() */
-    inline Scalar* valuePtr() { return derived().valuePtr(); }
+ * \class SparseCompressedBase
+ * \brief Common base class for sparse [compressed]-{row|column}-storage format.
+ *
+ * This class defines the common interface for all derived classes implementing the compressed sparse storage format,
+ * such as:
+ *  - SparseMatrix
+ *  - Ref<SparseMatrixType,Options>
+ *  - Map<SparseMatrixType>
+ *
+ */
+template <typename Derived>
+class SparseCompressedBase : public SparseMatrixBase<Derived> {
+ public:
+  typedef SparseMatrixBase<Derived> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(SparseCompressedBase)
+  using Base::operator=;
+  using Base::IsRowMajor;
 
-    /** \returns a const pointer to the array of inner indices.
-      * This function is aimed at interoperability with other libraries.
-      * \sa valuePtr(), outerIndexPtr() */
-    inline const StorageIndex* innerIndexPtr() const { return derived().innerIndexPtr(); }
-    /** \returns a non-const pointer to the array of inner indices.
-      * This function is aimed at interoperability with other libraries.
-      * \sa valuePtr(), outerIndexPtr() */
-    inline StorageIndex* innerIndexPtr() { return derived().innerIndexPtr(); }
+  class InnerIterator;
+  class ReverseInnerIterator;
 
-    /** \returns a const pointer to the array of the starting positions of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \warning it returns the null pointer 0 for SparseVector
-      * \sa valuePtr(), innerIndexPtr() */
-    inline const StorageIndex* outerIndexPtr() const { return derived().outerIndexPtr(); }
-    /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \warning it returns the null pointer 0 for SparseVector
-      * \sa valuePtr(), innerIndexPtr() */
-    inline StorageIndex* outerIndexPtr() { return derived().outerIndexPtr(); }
+ protected:
+  typedef typename Base::IndexVector IndexVector;
+  Eigen::Map<IndexVector> innerNonZeros() {
+    return Eigen::Map<IndexVector>(innerNonZeroPtr(), isCompressed() ? 0 : derived().outerSize());
+  }
+  const Eigen::Map<const IndexVector> innerNonZeros() const {
+    return Eigen::Map<const IndexVector>(innerNonZeroPtr(), isCompressed() ? 0 : derived().outerSize());
+  }
 
-    /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \warning it returns the null pointer 0 in compressed mode */
-    inline const StorageIndex* innerNonZeroPtr() const { return derived().innerNonZeroPtr(); }
-    /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \warning it returns the null pointer 0 in compressed mode */
-    inline StorageIndex* innerNonZeroPtr() { return derived().innerNonZeroPtr(); }
-    
-    /** \returns whether \c *this is in compressed form. */
-    inline bool isCompressed() const { return innerNonZeroPtr()==0; }
+ public:
+  /** \returns the number of non zero coefficients */
+  inline Index nonZeros() const {
+    if (Derived::IsVectorAtCompileTime && outerIndexPtr() == 0)
+      return derived().nonZeros();
+    else if (derived().outerSize() == 0)
+      return 0;
+    else if (isCompressed())
+      return outerIndexPtr()[derived().outerSize()] - outerIndexPtr()[0];
+    else
+      return innerNonZeros().sum();
+  }
 
-    /** \returns a read-only view of the stored coefficients as a 1D array expression.
-      *
-      * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
-      *
-      * \sa valuePtr(), isCompressed() */
-    const Map<const Array<Scalar,Dynamic,1> > coeffs() const { eigen_assert(isCompressed()); return Array<Scalar,Dynamic,1>::Map(valuePtr(),nonZeros()); }
+  /** \returns a const pointer to the array of values.
+   * This function is aimed at interoperability with other libraries.
+   * \sa innerIndexPtr(), outerIndexPtr() */
+  inline const Scalar* valuePtr() const { return derived().valuePtr(); }
+  /** \returns a non-const pointer to the array of values.
+   * This function is aimed at interoperability with other libraries.
+   * \sa innerIndexPtr(), outerIndexPtr() */
+  inline Scalar* valuePtr() { return derived().valuePtr(); }
 
-    /** \returns a read-write view of the stored coefficients as a 1D array expression
-      *
-      * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
-      *
-      * Here is an example:
-      * \include SparseMatrix_coeffs.cpp
-      * and the output is:
-      * \include SparseMatrix_coeffs.out
-      *
-      * \sa valuePtr(), isCompressed() */
-    Map<Array<Scalar,Dynamic,1> > coeffs() { eigen_assert(isCompressed()); return Array<Scalar,Dynamic,1>::Map(valuePtr(),nonZeros()); }
+  /** \returns a const pointer to the array of inner indices.
+   * This function is aimed at interoperability with other libraries.
+   * \sa valuePtr(), outerIndexPtr() */
+  inline const StorageIndex* innerIndexPtr() const { return derived().innerIndexPtr(); }
+  /** \returns a non-const pointer to the array of inner indices.
+   * This function is aimed at interoperability with other libraries.
+   * \sa valuePtr(), outerIndexPtr() */
+  inline StorageIndex* innerIndexPtr() { return derived().innerIndexPtr(); }
 
-  protected:
-    /** Default constructor. Do nothing. */
-    SparseCompressedBase() {}
+  /** \returns a const pointer to the array of the starting positions of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \warning it returns the null pointer 0 for SparseVector
+   * \sa valuePtr(), innerIndexPtr() */
+  inline const StorageIndex* outerIndexPtr() const { return derived().outerIndexPtr(); }
+  /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \warning it returns the null pointer 0 for SparseVector
+   * \sa valuePtr(), innerIndexPtr() */
+  inline StorageIndex* outerIndexPtr() { return derived().outerIndexPtr(); }
 
-    /** \internal return the index of the coeff at (row,col) or just before if it does not exist.
-      * This is an analogue of std::lower_bound.
-      */
-    internal::LowerBoundIndex lower_bound(Index row, Index col) const
-    {
-      eigen_internal_assert(row>=0 && row<this->rows() && col>=0 && col<this->cols());
+  /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \warning it returns the null pointer 0 in compressed mode */
+  inline const StorageIndex* innerNonZeroPtr() const { return derived().innerNonZeroPtr(); }
+  /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \warning it returns the null pointer 0 in compressed mode */
+  inline StorageIndex* innerNonZeroPtr() { return derived().innerNonZeroPtr(); }
 
-      const Index outer = Derived::IsRowMajor ? row : col;
-      const Index inner = Derived::IsRowMajor ? col : row;
+  /** \returns whether \c *this is in compressed form. */
+  inline bool isCompressed() const { return innerNonZeroPtr() == 0; }
 
-      Index start = this->outerIndexPtr()[outer];
-      Index end = this->isCompressed() ? this->outerIndexPtr()[outer+1] : this->outerIndexPtr()[outer] + this->innerNonZeroPtr()[outer];
-      eigen_assert(end>=start && "you are using a non finalized sparse matrix or written coefficient does not exist");
-      internal::LowerBoundIndex p;
-      p.value = std::lower_bound(this->innerIndexPtr()+start, this->innerIndexPtr()+end,inner) - this->innerIndexPtr();
-      p.found = (p.value<end) && (this->innerIndexPtr()[p.value]==inner);
-      return p;
-    }
+  /** \returns a read-only view of the stored coefficients as a 1D array expression.
+   *
+   * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
+   *
+   * \sa valuePtr(), isCompressed() */
+  const Map<const Array<Scalar, Dynamic, 1>> coeffs() const {
+    eigen_assert(isCompressed());
+    return Array<Scalar, Dynamic, 1>::Map(valuePtr(), nonZeros());
+  }
 
-    friend struct internal::evaluator<SparseCompressedBase<Derived> >;
+  /** \returns a read-write view of the stored coefficients as a 1D array expression
+   *
+   * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
+   *
+   * Here is an example:
+   * \include SparseMatrix_coeffs.cpp
+   * and the output is:
+   * \include SparseMatrix_coeffs.out
+   *
+   * \sa valuePtr(), isCompressed() */
+  Map<Array<Scalar, Dynamic, 1>> coeffs() {
+    eigen_assert(isCompressed());
+    return Array<Scalar, Dynamic, 1>::Map(valuePtr(), nonZeros());
+  }
 
-  private:
-    template<typename OtherDerived> explicit SparseCompressedBase(const SparseCompressedBase<OtherDerived>&);
+  /** sorts the inner vectors in the range [begin,end) with respect to `Comp`
+   * \sa innerIndicesAreSorted() */
+  template <class Comp = std::less<>>
+  inline void sortInnerIndices(Index begin, Index end) {
+    eigen_assert(begin >= 0 && end <= derived().outerSize() && end >= begin);
+    internal::inner_sort_impl<Derived, Comp, IsVectorAtCompileTime>::run(*this, begin, end);
+  }
+
+  /** \returns the index of the first inner vector in the range [begin,end) that is not sorted with respect to `Comp`,
+   * or `end` if the range is fully sorted \sa sortInnerIndices() */
+  template <class Comp = std::less<>>
+  inline Index innerIndicesAreSorted(Index begin, Index end) const {
+    eigen_assert(begin >= 0 && end <= derived().outerSize() && end >= begin);
+    return internal::inner_sort_impl<Derived, Comp, IsVectorAtCompileTime>::check(*this, begin, end);
+  }
+
+  /** sorts the inner vectors in the range [0,outerSize) with respect to `Comp`
+   * \sa innerIndicesAreSorted() */
+  template <class Comp = std::less<>>
+  inline void sortInnerIndices() {
+    Index begin = 0;
+    Index end = derived().outerSize();
+    internal::inner_sort_impl<Derived, Comp, IsVectorAtCompileTime>::run(*this, begin, end);
+  }
+
+  /** \returns the index of the first inner vector in the range [0,outerSize) that is not sorted with respect to `Comp`,
+   * or `outerSize` if the range is fully sorted \sa sortInnerIndices() */
+  template <class Comp = std::less<>>
+  inline Index innerIndicesAreSorted() const {
+    Index begin = 0;
+    Index end = derived().outerSize();
+    return internal::inner_sort_impl<Derived, Comp, IsVectorAtCompileTime>::check(*this, begin, end);
+  }
+
+ protected:
+  /** Default constructor. Do nothing. */
+  SparseCompressedBase() {}
+
+  /** \internal return the index of the coeff at (row,col) or just before if it does not exist.
+   * This is an analogue of std::lower_bound.
+   */
+  internal::LowerBoundIndex lower_bound(Index row, Index col) const {
+    eigen_internal_assert(row >= 0 && row < this->rows() && col >= 0 && col < this->cols());
+
+    const Index outer = Derived::IsRowMajor ? row : col;
+    const Index inner = Derived::IsRowMajor ? col : row;
+
+    Index start = this->outerIndexPtr()[outer];
+    Index end = this->isCompressed() ? this->outerIndexPtr()[outer + 1]
+                                     : this->outerIndexPtr()[outer] + this->innerNonZeroPtr()[outer];
+    eigen_assert(end >= start && "you are using a non finalized sparse matrix or written coefficient does not exist");
+    internal::LowerBoundIndex p;
+    p.value =
+        std::lower_bound(this->innerIndexPtr() + start, this->innerIndexPtr() + end, inner) - this->innerIndexPtr();
+    p.found = (p.value < end) && (this->innerIndexPtr()[p.value] == inner);
+    return p;
+  }
+
+  friend struct internal::evaluator<SparseCompressedBase<Derived>>;
+
+ private:
+  template <typename OtherDerived>
+  explicit SparseCompressedBase(const SparseCompressedBase<OtherDerived>&);
 };
 
-template<typename Derived>
-class SparseCompressedBase<Derived>::InnerIterator
-{
-  public:
-    InnerIterator()
-      : m_values(0), m_indices(0), m_outer(0), m_id(0), m_end(0)
-    {}
+template <typename Derived>
+class SparseCompressedBase<Derived>::InnerIterator {
+ public:
+  InnerIterator() : m_values(0), m_indices(0), m_outer(0), m_id(0), m_end(0) {}
 
-    InnerIterator(const InnerIterator& other)
-      : m_values(other.m_values), m_indices(other.m_indices), m_outer(other.m_outer), m_id(other.m_id), m_end(other.m_end)
-    {}
+  InnerIterator(const InnerIterator& other)
+      : m_values(other.m_values),
+        m_indices(other.m_indices),
+        m_outer(other.m_outer),
+        m_id(other.m_id),
+        m_end(other.m_end) {}
 
-    InnerIterator& operator=(const InnerIterator& other)
-    {
-      m_values = other.m_values;
-      m_indices = other.m_indices;
-      const_cast<OuterType&>(m_outer).setValue(other.m_outer.value());
-      m_id = other.m_id;
-      m_end = other.m_end;
-      return *this;
-    }
+  InnerIterator& operator=(const InnerIterator& other) {
+    m_values = other.m_values;
+    m_indices = other.m_indices;
+    const_cast<OuterType&>(m_outer).setValue(other.m_outer.value());
+    m_id = other.m_id;
+    m_end = other.m_end;
+    return *this;
+  }
 
-    InnerIterator(const SparseCompressedBase& mat, Index outer)
-      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer)
-    {
-      if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0)
-      {
-        m_id = 0;
-        m_end = mat.nonZeros();
-      }
+  InnerIterator(const SparseCompressedBase& mat, Index outer)
+      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer) {
+    if (Derived::IsVectorAtCompileTime && mat.outerIndexPtr() == 0) {
+      m_id = 0;
+      m_end = mat.nonZeros();
+    } else {
+      m_id = mat.outerIndexPtr()[outer];
+      if (mat.isCompressed())
+        m_end = mat.outerIndexPtr()[outer + 1];
       else
-      {
-        m_id = mat.outerIndexPtr()[outer];
-        if(mat.isCompressed())
-          m_end = mat.outerIndexPtr()[outer+1];
-        else
-          m_end = m_id + mat.innerNonZeroPtr()[outer];
-      }
+        m_end = m_id + mat.innerNonZeroPtr()[outer];
     }
+  }
 
-    explicit InnerIterator(const SparseCompressedBase& mat)
-      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_id(0), m_end(mat.nonZeros())
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-    }
+  explicit InnerIterator(const SparseCompressedBase& mat) : InnerIterator(mat, Index(0)) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  }
 
-    explicit InnerIterator(const internal::CompressedStorage<Scalar,StorageIndex>& data)
-      : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_id(0), m_end(data.size())
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-    }
+  explicit InnerIterator(const internal::CompressedStorage<Scalar, StorageIndex>& data)
+      : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_id(0), m_end(data.size()) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  }
 
-    inline InnerIterator& operator++() { m_id++; return *this; }
-    inline InnerIterator& operator+=(Index i) { m_id += i ; return *this; }
+  inline InnerIterator& operator++() {
+    m_id++;
+    return *this;
+  }
+  inline InnerIterator& operator+=(Index i) {
+    m_id += i;
+    return *this;
+  }
 
-    inline InnerIterator operator+(Index i) 
-    { 
-        InnerIterator result = *this;
-        result += i;
-        return result;
-    }
+  inline InnerIterator operator+(Index i) {
+    InnerIterator result = *this;
+    result += i;
+    return result;
+  }
 
-    inline const Scalar& value() const { return m_values[m_id]; }
-    inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id]); }
+  inline const Scalar& value() const { return m_values[m_id]; }
+  inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id]); }
 
-    inline StorageIndex index() const { return m_indices[m_id]; }
-    inline Index outer() const { return m_outer.value(); }
-    inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
-    inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
+  inline StorageIndex index() const { return m_indices[m_id]; }
+  inline Index outer() const { return m_outer.value(); }
+  inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
+  inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
 
-    inline operator bool() const { return (m_id < m_end); }
+  inline operator bool() const { return (m_id < m_end); }
 
-  protected:
-    const Scalar* m_values;
-    const StorageIndex* m_indices;
-    typedef internal::variable_if_dynamic<Index,Derived::IsVectorAtCompileTime?0:Dynamic> OuterType;
-    const OuterType m_outer;
-    Index m_id;
-    Index m_end;
-  private:
-    // If you get here, then you're not using the right InnerIterator type, e.g.:
-    //   SparseMatrix<double,RowMajor> A;
-    //   SparseMatrix<double>::InnerIterator it(A,0);
-    template<typename T> InnerIterator(const SparseMatrixBase<T>&, Index outer);
+ protected:
+  const Scalar* m_values;
+  const StorageIndex* m_indices;
+  typedef internal::variable_if_dynamic<Index, Derived::IsVectorAtCompileTime ? 0 : Dynamic> OuterType;
+  const OuterType m_outer;
+  Index m_id;
+  Index m_end;
+
+ private:
+  // If you get here, then you're not using the right InnerIterator type, e.g.:
+  //   SparseMatrix<double,RowMajor> A;
+  //   SparseMatrix<double>::InnerIterator it(A,0);
+  template <typename T>
+  InnerIterator(const SparseMatrixBase<T>&, Index outer);
 };
 
-template<typename Derived>
-class SparseCompressedBase<Derived>::ReverseInnerIterator
-{
-  public:
-    ReverseInnerIterator(const SparseCompressedBase& mat, Index outer)
-      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer)
-    {
-      if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0)
-      {
-        m_start = 0;
-        m_id = mat.nonZeros();
-      }
+template <typename Derived>
+class SparseCompressedBase<Derived>::ReverseInnerIterator {
+ public:
+  ReverseInnerIterator(const SparseCompressedBase& mat, Index outer)
+      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer) {
+    if (Derived::IsVectorAtCompileTime && mat.outerIndexPtr() == 0) {
+      m_start = 0;
+      m_id = mat.nonZeros();
+    } else {
+      m_start = mat.outerIndexPtr()[outer];
+      if (mat.isCompressed())
+        m_id = mat.outerIndexPtr()[outer + 1];
       else
-      {
-        m_start = mat.outerIndexPtr()[outer];
-        if(mat.isCompressed())
-          m_id = mat.outerIndexPtr()[outer+1];
-        else
-          m_id = m_start + mat.innerNonZeroPtr()[outer];
-      }
+        m_id = m_start + mat.innerNonZeroPtr()[outer];
     }
+  }
 
-    explicit ReverseInnerIterator(const SparseCompressedBase& mat)
-      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_start(0), m_id(mat.nonZeros())
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-    }
+  explicit ReverseInnerIterator(const SparseCompressedBase& mat)
+      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_start(0), m_id(mat.nonZeros()) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  }
 
-    explicit ReverseInnerIterator(const internal::CompressedStorage<Scalar,StorageIndex>& data)
-      : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_start(0), m_id(data.size())
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-    }
+  explicit ReverseInnerIterator(const internal::CompressedStorage<Scalar, StorageIndex>& data)
+      : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_start(0), m_id(data.size()) {
+    EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  }
 
-    inline ReverseInnerIterator& operator--() { --m_id; return *this; }
-    inline ReverseInnerIterator& operator-=(Index i) { m_id -= i; return *this; }
+  inline ReverseInnerIterator& operator--() {
+    --m_id;
+    return *this;
+  }
+  inline ReverseInnerIterator& operator-=(Index i) {
+    m_id -= i;
+    return *this;
+  }
 
-    inline ReverseInnerIterator operator-(Index i) 
-    {
-        ReverseInnerIterator result = *this;
-        result -= i;
-        return result;
-    }
+  inline ReverseInnerIterator operator-(Index i) {
+    ReverseInnerIterator result = *this;
+    result -= i;
+    return result;
+  }
 
-    inline const Scalar& value() const { return m_values[m_id-1]; }
-    inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id-1]); }
+  inline const Scalar& value() const { return m_values[m_id - 1]; }
+  inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id - 1]); }
 
-    inline StorageIndex index() const { return m_indices[m_id-1]; }
-    inline Index outer() const { return m_outer.value(); }
-    inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
-    inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
+  inline StorageIndex index() const { return m_indices[m_id - 1]; }
+  inline Index outer() const { return m_outer.value(); }
+  inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
+  inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
 
-    inline operator bool() const { return (m_id > m_start); }
+  inline operator bool() const { return (m_id > m_start); }
 
-  protected:
-    const Scalar* m_values;
-    const StorageIndex* m_indices;
-    typedef internal::variable_if_dynamic<Index,Derived::IsVectorAtCompileTime?0:Dynamic> OuterType;
-    const OuterType m_outer;
-    Index m_start;
-    Index m_id;
+ protected:
+  const Scalar* m_values;
+  const StorageIndex* m_indices;
+  typedef internal::variable_if_dynamic<Index, Derived::IsVectorAtCompileTime ? 0 : Dynamic> OuterType;
+  const OuterType m_outer;
+  Index m_start;
+  Index m_id;
 };
 
 namespace internal {
 
-template<typename Derived>
-struct evaluator<SparseCompressedBase<Derived> >
-  : evaluator_base<Derived>
-{
+// modified from https://artificial-mind.net/blog/2020/11/28/std-sort-multiple-ranges
+
+template <typename Scalar, typename StorageIndex>
+class StorageVal;
+template <typename Scalar, typename StorageIndex>
+class StorageRef;
+template <typename Scalar, typename StorageIndex>
+class CompressedStorageIterator;
+
+// class to hold an index/value pair
+template <typename Scalar, typename StorageIndex>
+class StorageVal {
+ public:
+  StorageVal(const StorageIndex& innerIndex, const Scalar& value) : m_innerIndex(innerIndex), m_value(value) {}
+  StorageVal(const StorageVal& other) : m_innerIndex(other.m_innerIndex), m_value(other.m_value) {}
+  StorageVal(StorageVal&& other) = default;
+
+  inline const StorageIndex& key() const { return m_innerIndex; }
+  inline StorageIndex& key() { return m_innerIndex; }
+  inline const Scalar& value() const { return m_value; }
+  inline Scalar& value() { return m_value; }
+
+  // enables StorageVal to be compared with respect to any type that is convertible to StorageIndex
+  inline operator StorageIndex() const { return m_innerIndex; }
+
+ protected:
+  StorageIndex m_innerIndex;
+  Scalar m_value;
+
+ private:
+  StorageVal() = delete;
+};
+// class to hold an index/value iterator pair
+// used to define assignment, swap, and comparison operators for CompressedStorageIterator
+template <typename Scalar, typename StorageIndex>
+class StorageRef {
+ public:
+  using value_type = StorageVal<Scalar, StorageIndex>;
+
+  // StorageRef Needs to be move-able for sort on macos.
+  StorageRef(StorageRef&& other) = default;
+
+  inline StorageRef& operator=(const StorageRef& other) {
+    key() = other.key();
+    value() = other.value();
+    return *this;
+  }
+  inline StorageRef& operator=(const value_type& other) {
+    key() = other.key();
+    value() = other.value();
+    return *this;
+  }
+  inline operator value_type() const { return value_type(key(), value()); }
+  inline friend void swap(const StorageRef& a, const StorageRef& b) {
+    std::iter_swap(a.keyPtr(), b.keyPtr());
+    std::iter_swap(a.valuePtr(), b.valuePtr());
+  }
+
+  inline const StorageIndex& key() const { return *m_innerIndexIterator; }
+  inline StorageIndex& key() { return *m_innerIndexIterator; }
+  inline const Scalar& value() const { return *m_valueIterator; }
+  inline Scalar& value() { return *m_valueIterator; }
+  inline StorageIndex* keyPtr() const { return m_innerIndexIterator; }
+  inline Scalar* valuePtr() const { return m_valueIterator; }
+
+  // enables StorageRef to be compared with respect to any type that is convertible to StorageIndex
+  inline operator StorageIndex() const { return *m_innerIndexIterator; }
+
+ protected:
+  StorageIndex* m_innerIndexIterator;
+  Scalar* m_valueIterator;
+
+ private:
+  StorageRef() = delete;
+  // these constructors are called by the CompressedStorageIterator constructors for convenience only
+  StorageRef(StorageIndex* innerIndexIterator, Scalar* valueIterator)
+      : m_innerIndexIterator(innerIndexIterator), m_valueIterator(valueIterator) {}
+  StorageRef(const StorageRef& other)
+      : m_innerIndexIterator(other.m_innerIndexIterator), m_valueIterator(other.m_valueIterator) {}
+
+  friend class CompressedStorageIterator<Scalar, StorageIndex>;
+};
+
+// STL-compatible iterator class that operates on inner indices and values
+template <typename Scalar, typename StorageIndex>
+class CompressedStorageIterator {
+ public:
+  using iterator_category = std::random_access_iterator_tag;
+  using reference = StorageRef<Scalar, StorageIndex>;
+  using difference_type = Index;
+  using value_type = typename reference::value_type;
+  using pointer = value_type*;
+
+  CompressedStorageIterator() = delete;
+  CompressedStorageIterator(difference_type index, StorageIndex* innerIndexPtr, Scalar* valuePtr)
+      : m_index(index), m_data(innerIndexPtr, valuePtr) {}
+  CompressedStorageIterator(difference_type index, reference data) : m_index(index), m_data(data) {}
+  CompressedStorageIterator(const CompressedStorageIterator& other) : m_index(other.m_index), m_data(other.m_data) {}
+  CompressedStorageIterator(CompressedStorageIterator&& other) = default;
+  inline CompressedStorageIterator& operator=(const CompressedStorageIterator& other) {
+    m_index = other.m_index;
+    m_data = other.m_data;
+    return *this;
+  }
+
+  inline CompressedStorageIterator operator+(difference_type offset) const {
+    return CompressedStorageIterator(m_index + offset, m_data);
+  }
+  inline CompressedStorageIterator operator-(difference_type offset) const {
+    return CompressedStorageIterator(m_index - offset, m_data);
+  }
+  inline difference_type operator-(const CompressedStorageIterator& other) const { return m_index - other.m_index; }
+  inline CompressedStorageIterator& operator++() {
+    ++m_index;
+    return *this;
+  }
+  inline CompressedStorageIterator& operator--() {
+    --m_index;
+    return *this;
+  }
+  inline CompressedStorageIterator& operator+=(difference_type offset) {
+    m_index += offset;
+    return *this;
+  }
+  inline CompressedStorageIterator& operator-=(difference_type offset) {
+    m_index -= offset;
+    return *this;
+  }
+  inline reference operator*() const { return reference(m_data.keyPtr() + m_index, m_data.valuePtr() + m_index); }
+
+#define MAKE_COMP(OP) \
+  inline bool operator OP(const CompressedStorageIterator& other) const { return m_index OP other.m_index; }
+  MAKE_COMP(<)
+  MAKE_COMP(>)
+  MAKE_COMP(>=)
+  MAKE_COMP(<=)
+  MAKE_COMP(!=)
+  MAKE_COMP(==)
+#undef MAKE_COMP
+
+ protected:
+  difference_type m_index;
+  reference m_data;
+};
+
+template <typename Derived, class Comp, bool IsVector>
+struct inner_sort_impl {
+  typedef typename Derived::Scalar Scalar;
+  typedef typename Derived::StorageIndex StorageIndex;
+  static inline void run(SparseCompressedBase<Derived>& obj, Index begin, Index end) {
+    const bool is_compressed = obj.isCompressed();
+    for (Index outer = begin; outer < end; outer++) {
+      Index begin_offset = obj.outerIndexPtr()[outer];
+      Index end_offset = is_compressed ? obj.outerIndexPtr()[outer + 1] : (begin_offset + obj.innerNonZeroPtr()[outer]);
+      CompressedStorageIterator<Scalar, StorageIndex> begin_it(begin_offset, obj.innerIndexPtr(), obj.valuePtr());
+      CompressedStorageIterator<Scalar, StorageIndex> end_it(end_offset, obj.innerIndexPtr(), obj.valuePtr());
+      std::sort(begin_it, end_it, Comp());
+    }
+  }
+  static inline Index check(const SparseCompressedBase<Derived>& obj, Index begin, Index end) {
+    const bool is_compressed = obj.isCompressed();
+    for (Index outer = begin; outer < end; outer++) {
+      Index begin_offset = obj.outerIndexPtr()[outer];
+      Index end_offset = is_compressed ? obj.outerIndexPtr()[outer + 1] : (begin_offset + obj.innerNonZeroPtr()[outer]);
+      const StorageIndex* begin_it = obj.innerIndexPtr() + begin_offset;
+      const StorageIndex* end_it = obj.innerIndexPtr() + end_offset;
+      bool is_sorted = std::is_sorted(begin_it, end_it, Comp());
+      if (!is_sorted) return outer;
+    }
+    return end;
+  }
+};
+template <typename Derived, class Comp>
+struct inner_sort_impl<Derived, Comp, true> {
+  typedef typename Derived::Scalar Scalar;
+  typedef typename Derived::StorageIndex StorageIndex;
+  static inline void run(SparseCompressedBase<Derived>& obj, Index, Index) {
+    Index begin_offset = 0;
+    Index end_offset = obj.nonZeros();
+    CompressedStorageIterator<Scalar, StorageIndex> begin_it(begin_offset, obj.innerIndexPtr(), obj.valuePtr());
+    CompressedStorageIterator<Scalar, StorageIndex> end_it(end_offset, obj.innerIndexPtr(), obj.valuePtr());
+    std::sort(begin_it, end_it, Comp());
+  }
+  static inline Index check(const SparseCompressedBase<Derived>& obj, Index, Index) {
+    Index begin_offset = 0;
+    Index end_offset = obj.nonZeros();
+    const StorageIndex* begin_it = obj.innerIndexPtr() + begin_offset;
+    const StorageIndex* end_it = obj.innerIndexPtr() + end_offset;
+    return std::is_sorted(begin_it, end_it, Comp()) ? 1 : 0;
+  }
+};
+
+template <typename Derived>
+struct evaluator<SparseCompressedBase<Derived>> : evaluator_base<Derived> {
   typedef typename Derived::Scalar Scalar;
   typedef typename Derived::InnerIterator InnerIterator;
-  
-  enum {
-    CoeffReadCost = NumTraits<Scalar>::ReadCost,
-    Flags = Derived::Flags
-  };
-  
-  evaluator() : m_matrix(0), m_zero(0)
-  {
-    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
-  }
-  explicit evaluator(const Derived &mat) : m_matrix(&mat), m_zero(0)
-  {
-    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
-  }
-  
-  inline Index nonZerosEstimate() const {
-    return m_matrix->nonZeros();
-  }
-  
+
+  enum { CoeffReadCost = NumTraits<Scalar>::ReadCost, Flags = Derived::Flags };
+
+  evaluator() : m_matrix(0), m_zero(0) { EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); }
+  explicit evaluator(const Derived& mat) : m_matrix(&mat), m_zero(0) { EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); }
+
+  inline Index nonZerosEstimate() const { return m_matrix->nonZeros(); }
+
   operator Derived&() { return m_matrix->const_cast_derived(); }
   operator const Derived&() const { return *m_matrix; }
-  
-  typedef typename DenseCoeffsBase<Derived,ReadOnlyAccessors>::CoeffReturnType CoeffReturnType;
-  const Scalar& coeff(Index row, Index col) const
-  {
-    Index p = find(row,col);
 
-    if(p==Dynamic)
+  typedef typename DenseCoeffsBase<Derived, ReadOnlyAccessors>::CoeffReturnType CoeffReturnType;
+  const Scalar& coeff(Index row, Index col) const {
+    Index p = find(row, col);
+
+    if (p == Dynamic)
       return m_zero;
     else
       return m_matrix->const_cast_derived().valuePtr()[p];
   }
 
-  Scalar& coeffRef(Index row, Index col)
-  {
-    Index p = find(row,col);
-    eigen_assert(p!=Dynamic && "written coefficient does not exist");
+  Scalar& coeffRef(Index row, Index col) {
+    Index p = find(row, col);
+    eigen_assert(p != Dynamic && "written coefficient does not exist");
     return m_matrix->const_cast_derived().valuePtr()[p];
   }
 
-protected:
-
-  Index find(Index row, Index col) const
-  {
-    internal::LowerBoundIndex p = m_matrix->lower_bound(row,col);
+ protected:
+  Index find(Index row, Index col) const {
+    internal::LowerBoundIndex p = m_matrix->lower_bound(row, col);
     return p.found ? p.value : Dynamic;
   }
 
-  const Derived *m_matrix;
+  const Derived* m_matrix;
   const Scalar m_zero;
 };
 
-}
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_COMPRESSED_BASE_H
+#endif  // EIGEN_SPARSE_COMPRESSED_BASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
index 9b0d3f9..6858263 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
@@ -10,7 +10,10 @@
 #ifndef EIGEN_SPARSE_CWISE_BINARY_OP_H
 #define EIGEN_SPARSE_CWISE_BINARY_OP_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 // Here we have to handle 3 cases:
 //  1 - sparse op dense
@@ -32,76 +35,68 @@
 // TODO to ease compiler job, we could specialize product/quotient with a scalar
 //      and fallback to cwise-unary evaluator using bind1st_op and bind2nd_op.
 
-template<typename BinaryOp, typename Lhs, typename Rhs>
-class CwiseBinaryOpImpl<BinaryOp, Lhs, Rhs, Sparse>
-  : public SparseMatrixBase<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-{
-  public:
-    typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> Derived;
-    typedef SparseMatrixBase<Derived> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(Derived)
-    CwiseBinaryOpImpl()
-    {
-      EIGEN_STATIC_ASSERT((
-                (!internal::is_same<typename internal::traits<Lhs>::StorageKind,
-                                    typename internal::traits<Rhs>::StorageKind>::value)
-            ||  ((internal::evaluator<Lhs>::Flags&RowMajorBit) == (internal::evaluator<Rhs>::Flags&RowMajorBit))),
-            THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH);
-    }
+template <typename BinaryOp, typename Lhs, typename Rhs>
+class CwiseBinaryOpImpl<BinaryOp, Lhs, Rhs, Sparse> : public SparseMatrixBase<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > {
+ public:
+  typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> Derived;
+  typedef SparseMatrixBase<Derived> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(Derived)
+  EIGEN_STATIC_ASSERT(((!internal::is_same<typename internal::traits<Lhs>::StorageKind,
+                                           typename internal::traits<Rhs>::StorageKind>::value) ||
+                       ((internal::evaluator<Lhs>::Flags & RowMajorBit) ==
+                        (internal::evaluator<Rhs>::Flags & RowMajorBit))),
+                      THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH)
 };
 
 namespace internal {
 
-  
-// Generic "sparse OP sparse"
-template<typename XprType> struct binary_sparse_evaluator;
+// The default evaluator performs an "arithmetic" operation on two input arrays.
+// Given input arrays 'lhs' and 'rhs' and binary functor 'func',
+// the sparse destination array 'dst' is evaluated as follows:
+//   if lhs(i,j) and rhs(i,j) are present, dst(i,j) = func(lhs(i,j), rhs(i,j))
+//   if lhs(i,j) is present and rhs(i,j) is null, dst(i,j) = func(lhs(i,j), 0)
+//   if lhs(i,j) is null and rhs(i,j) is present, dst(i,j) = func(0, rhs(i,j))
 
-template<typename BinaryOp, typename Lhs, typename Rhs>
+// Generic "sparse OP sparse"
+template <typename XprType>
+struct binary_sparse_evaluator;
+
+template <typename BinaryOp, typename Lhs, typename Rhs>
 struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IteratorBased, IteratorBased>
-  : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-{
-protected:
-  typedef typename evaluator<Lhs>::InnerIterator  LhsIterator;
-  typedef typename evaluator<Rhs>::InnerIterator  RhsIterator;
+    : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > {
+ protected:
+  typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+  typedef typename evaluator<Rhs>::InnerIterator RhsIterator;
   typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
   typedef typename traits<XprType>::Scalar Scalar;
   typedef typename XprType::StorageIndex StorageIndex;
-public:
 
-  class InnerIterator
-  {
-  public:
-    
+ public:
+  class InnerIterator {
+   public:
     EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
-      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor)
-    {
+        : m_lhsIter(aEval.m_lhsImpl, outer),
+          m_rhsIter(aEval.m_rhsImpl, outer),
+          m_functor(aEval.m_functor),
+          m_value(Scalar(0)) {
       this->operator++();
     }
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    {
-      if (m_lhsIter && m_rhsIter && (m_lhsIter.index() == m_rhsIter.index()))
-      {
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
+      if (m_lhsIter && m_rhsIter && (m_lhsIter.index() == m_rhsIter.index())) {
         m_id = m_lhsIter.index();
         m_value = m_functor(m_lhsIter.value(), m_rhsIter.value());
         ++m_lhsIter;
         ++m_rhsIter;
-      }
-      else if (m_lhsIter && (!m_rhsIter || (m_lhsIter.index() < m_rhsIter.index())))
-      {
+      } else if (m_lhsIter && (!m_rhsIter || (m_lhsIter.index() < m_rhsIter.index()))) {
         m_id = m_lhsIter.index();
         m_value = m_functor(m_lhsIter.value(), Scalar(0));
         ++m_lhsIter;
-      }
-      else if (m_rhsIter && (!m_lhsIter || (m_lhsIter.index() > m_rhsIter.index())))
-      {
+      } else if (m_rhsIter && (!m_lhsIter || (m_lhsIter.index() > m_rhsIter.index()))) {
         m_id = m_rhsIter.index();
         m_value = m_functor(Scalar(0), m_rhsIter.value());
         ++m_rhsIter;
-      }
-      else
-      {
-        m_value = Scalar(0); // this is to avoid a compilation warning
+      } else {
         m_id = -1;
       }
       return *this;
@@ -114,94 +109,88 @@
     EIGEN_STRONG_INLINE Index row() const { return Lhs::IsRowMajor ? m_lhsIter.row() : index(); }
     EIGEN_STRONG_INLINE Index col() const { return Lhs::IsRowMajor ? index() : m_lhsIter.col(); }
 
-    EIGEN_STRONG_INLINE operator bool() const { return m_id>=0; }
+    EIGEN_STRONG_INLINE operator bool() const { return m_id >= 0; }
 
-  protected:
+   protected:
     LhsIterator m_lhsIter;
     RhsIterator m_rhsIter;
     const BinaryOp& m_functor;
     Scalar m_value;
     StorageIndex m_id;
   };
-  
-  
+
   enum {
-    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    CoeffReadCost =
+        int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
     Flags = XprType::Flags
   };
-  
-  explicit binary_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_lhsImpl(xpr.lhs()), 
-      m_rhsImpl(xpr.rhs())  
-  {
+
+  explicit binary_evaluator(const XprType& xpr) : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-  
-  inline Index nonZerosEstimate() const {
-    return m_lhsImpl.nonZerosEstimate() + m_rhsImpl.nonZerosEstimate();
-  }
 
-protected:
+  inline Index nonZerosEstimate() const { return m_lhsImpl.nonZerosEstimate() + m_rhsImpl.nonZerosEstimate(); }
+
+ protected:
   const BinaryOp m_functor;
   evaluator<Lhs> m_lhsImpl;
   evaluator<Rhs> m_rhsImpl;
 };
 
 // dense op sparse
-template<typename BinaryOp, typename Lhs, typename Rhs>
+template <typename BinaryOp, typename Lhs, typename Rhs>
 struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IndexBased, IteratorBased>
-  : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-{
-protected:
-  typedef typename evaluator<Rhs>::InnerIterator  RhsIterator;
+    : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > {
+ protected:
+  typedef typename evaluator<Rhs>::InnerIterator RhsIterator;
   typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
   typedef typename traits<XprType>::Scalar Scalar;
   typedef typename XprType::StorageIndex StorageIndex;
-public:
 
-  class InnerIterator
-  {
-    enum { IsRowMajor = (int(Rhs::Flags)&RowMajorBit)==RowMajorBit };
-  public:
+ public:
+  class InnerIterator {
+    enum { IsRowMajor = (int(Rhs::Flags) & RowMajorBit) == RowMajorBit };
 
+   public:
     EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
-      : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.rhs().innerSize())
-    {
+        : m_lhsEval(aEval.m_lhsImpl),
+          m_rhsIter(aEval.m_rhsImpl, outer),
+          m_functor(aEval.m_functor),
+          m_value(0),
+          m_id(-1),
+          m_innerSize(aEval.m_expr.rhs().innerSize()) {
       this->operator++();
     }
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    {
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
       ++m_id;
-      if(m_id<m_innerSize)
-      {
-        Scalar lhsVal = m_lhsEval.coeff(IsRowMajor?m_rhsIter.outer():m_id,
-                                        IsRowMajor?m_id:m_rhsIter.outer());
-        if(m_rhsIter && m_rhsIter.index()==m_id)
-        {
+      if (m_id < m_innerSize) {
+        Scalar lhsVal = m_lhsEval.coeff(IsRowMajor ? m_rhsIter.outer() : m_id, IsRowMajor ? m_id : m_rhsIter.outer());
+        if (m_rhsIter && m_rhsIter.index() == m_id) {
           m_value = m_functor(lhsVal, m_rhsIter.value());
           ++m_rhsIter;
-        }
-        else
+        } else
           m_value = m_functor(lhsVal, Scalar(0));
       }
 
       return *this;
     }
 
-    EIGEN_STRONG_INLINE Scalar value() const { eigen_internal_assert(m_id<m_innerSize); return m_value; }
+    EIGEN_STRONG_INLINE Scalar value() const {
+      eigen_internal_assert(m_id < m_innerSize);
+      return m_value;
+    }
 
     EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
     EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); }
     EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_rhsIter.outer() : m_id; }
     EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_rhsIter.outer(); }
 
-    EIGEN_STRONG_INLINE operator bool() const { return m_id<m_innerSize; }
+    EIGEN_STRONG_INLINE operator bool() const { return m_id < m_innerSize; }
 
-  protected:
-    const evaluator<Lhs> &m_lhsEval;
+   protected:
+    const evaluator<Lhs>& m_lhsEval;
     RhsIterator m_rhsIter;
     const BinaryOp& m_functor;
     Scalar m_value;
@@ -209,216 +198,198 @@
     StorageIndex m_innerSize;
   };
 
-
   enum {
-    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    CoeffReadCost =
+        int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
     Flags = XprType::Flags
   };
 
   explicit binary_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_lhsImpl(xpr.lhs()),
-      m_rhsImpl(xpr.rhs()),
-      m_expr(xpr)
-  {
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()), m_expr(xpr) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
-  inline Index nonZerosEstimate() const {
-    return m_expr.size();
-  }
+  inline Index nonZerosEstimate() const { return m_expr.size(); }
 
-protected:
+ protected:
   const BinaryOp m_functor;
   evaluator<Lhs> m_lhsImpl;
   evaluator<Rhs> m_rhsImpl;
-  const XprType &m_expr;
+  const XprType& m_expr;
 };
 
 // sparse op dense
-template<typename BinaryOp, typename Lhs, typename Rhs>
+template <typename BinaryOp, typename Lhs, typename Rhs>
 struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IteratorBased, IndexBased>
-  : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
-{
-protected:
-  typedef typename evaluator<Lhs>::InnerIterator  LhsIterator;
+    : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > {
+ protected:
+  typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
   typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
   typedef typename traits<XprType>::Scalar Scalar;
   typedef typename XprType::StorageIndex StorageIndex;
-public:
 
-  class InnerIterator
-  {
-    enum { IsRowMajor = (int(Lhs::Flags)&RowMajorBit)==RowMajorBit };
-  public:
+ public:
+  class InnerIterator {
+    enum { IsRowMajor = (int(Lhs::Flags) & RowMajorBit) == RowMajorBit };
 
+   public:
     EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
-      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.lhs().innerSize())
-    {
+        : m_lhsIter(aEval.m_lhsImpl, outer),
+          m_rhsEval(aEval.m_rhsImpl),
+          m_functor(aEval.m_functor),
+          m_value(0),
+          m_id(-1),
+          m_innerSize(aEval.m_expr.lhs().innerSize()) {
       this->operator++();
     }
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    {
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
       ++m_id;
-      if(m_id<m_innerSize)
-      {
-        Scalar rhsVal = m_rhsEval.coeff(IsRowMajor?m_lhsIter.outer():m_id,
-                                        IsRowMajor?m_id:m_lhsIter.outer());
-        if(m_lhsIter && m_lhsIter.index()==m_id)
-        {
+      if (m_id < m_innerSize) {
+        Scalar rhsVal = m_rhsEval.coeff(IsRowMajor ? m_lhsIter.outer() : m_id, IsRowMajor ? m_id : m_lhsIter.outer());
+        if (m_lhsIter && m_lhsIter.index() == m_id) {
           m_value = m_functor(m_lhsIter.value(), rhsVal);
           ++m_lhsIter;
-        }
-        else
-          m_value = m_functor(Scalar(0),rhsVal);
+        } else
+          m_value = m_functor(Scalar(0), rhsVal);
       }
 
       return *this;
     }
 
-    EIGEN_STRONG_INLINE Scalar value() const { eigen_internal_assert(m_id<m_innerSize); return m_value; }
+    EIGEN_STRONG_INLINE Scalar value() const {
+      eigen_internal_assert(m_id < m_innerSize);
+      return m_value;
+    }
 
     EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
     EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
     EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_lhsIter.outer() : m_id; }
     EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_lhsIter.outer(); }
 
-    EIGEN_STRONG_INLINE operator bool() const { return m_id<m_innerSize; }
+    EIGEN_STRONG_INLINE operator bool() const { return m_id < m_innerSize; }
 
-  protected:
+   protected:
     LhsIterator m_lhsIter;
-    const evaluator<Rhs> &m_rhsEval;
+    const evaluator<Rhs>& m_rhsEval;
     const BinaryOp& m_functor;
     Scalar m_value;
     StorageIndex m_id;
     StorageIndex m_innerSize;
   };
 
-
   enum {
-    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    CoeffReadCost =
+        int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
     Flags = XprType::Flags
   };
 
   explicit binary_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_lhsImpl(xpr.lhs()),
-      m_rhsImpl(xpr.rhs()),
-      m_expr(xpr)
-  {
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()), m_expr(xpr) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
 
-  inline Index nonZerosEstimate() const {
-    return m_expr.size();
-  }
+  inline Index nonZerosEstimate() const { return m_expr.size(); }
 
-protected:
+ protected:
   const BinaryOp m_functor;
   evaluator<Lhs> m_lhsImpl;
   evaluator<Rhs> m_rhsImpl;
-  const XprType &m_expr;
+  const XprType& m_expr;
 };
 
-template<typename T,
-         typename LhsKind   = typename evaluator_traits<typename T::Lhs>::Kind,
-         typename RhsKind   = typename evaluator_traits<typename T::Rhs>::Kind,
-         typename LhsScalar = typename traits<typename T::Lhs>::Scalar,
-         typename RhsScalar = typename traits<typename T::Rhs>::Scalar> struct sparse_conjunction_evaluator;
+template <typename T, typename LhsKind = typename evaluator_traits<typename T::Lhs>::Kind,
+          typename RhsKind = typename evaluator_traits<typename T::Rhs>::Kind,
+          typename LhsScalar = typename traits<typename T::Lhs>::Scalar,
+          typename RhsScalar = typename traits<typename T::Rhs>::Scalar>
+struct sparse_conjunction_evaluator;
 
 // "sparse .* sparse"
-template<typename T1, typename T2, typename Lhs, typename Rhs>
-struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IteratorBased, IteratorBased>
-  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
-{
-  typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+template <typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs>, IteratorBased, IteratorBased>
+    : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs> XprType;
   typedef sparse_conjunction_evaluator<XprType> Base;
   explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 // "dense .* sparse"
-template<typename T1, typename T2, typename Lhs, typename Rhs>
-struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IndexBased, IteratorBased>
-  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
-{
-  typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+template <typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs>, IndexBased, IteratorBased>
+    : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs> XprType;
   typedef sparse_conjunction_evaluator<XprType> Base;
   explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 // "sparse .* dense"
-template<typename T1, typename T2, typename Lhs, typename Rhs>
-struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IteratorBased, IndexBased>
-  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
-{
-  typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+template <typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs>, IteratorBased, IndexBased>
+    : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_product_op<T1, T2>, Lhs, Rhs> XprType;
   typedef sparse_conjunction_evaluator<XprType> Base;
   explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
 // "sparse ./ dense"
-template<typename T1, typename T2, typename Lhs, typename Rhs>
-struct binary_evaluator<CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs>, IteratorBased, IndexBased>
-  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs> >
-{
-  typedef CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs> XprType;
+template <typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_quotient_op<T1, T2>, Lhs, Rhs>, IteratorBased, IndexBased>
+    : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_quotient_op<T1, T2>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_quotient_op<T1, T2>, Lhs, Rhs> XprType;
   typedef sparse_conjunction_evaluator<XprType> Base;
   explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
 // "sparse && sparse"
-template<typename Lhs, typename Rhs>
-struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IteratorBased, IteratorBased>
-  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
-{
-  typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+template <typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs>, IteratorBased, IteratorBased>
+    : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs> XprType;
   typedef sparse_conjunction_evaluator<XprType> Base;
   explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 // "dense && sparse"
-template<typename Lhs, typename Rhs>
-struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IndexBased, IteratorBased>
-  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
-{
-  typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+template <typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs>, IndexBased, IteratorBased>
+    : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs> XprType;
   typedef sparse_conjunction_evaluator<XprType> Base;
   explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 // "sparse && dense"
-template<typename Lhs, typename Rhs>
-struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IteratorBased, IndexBased>
-  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
-{
-  typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+template <typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs>, IteratorBased, IndexBased>
+    : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_boolean_and_op<bool>, Lhs, Rhs> XprType;
   typedef sparse_conjunction_evaluator<XprType> Base;
   explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
+// The conjunction "^" evaluator performs a logical "and" or set "intersection" operation on two input arrays.
+// Given input arrays 'lhs' and 'rhs' and binary functor 'func',
+// the sparse destination array 'dst' is evaluated as follows:
+//   if lhs(i,j) and rhs(i,j) are present, dst(i,j) = func(lhs(i,j), rhs(i,j))
+//   if lhs(i,j) is present and rhs(i,j) is null, dst(i,j) is null
+//   if lhs(i,j) is null and rhs(i,j) is present, dst(i,j) is null
+
 // "sparse ^ sparse"
-template<typename XprType>
-struct sparse_conjunction_evaluator<XprType, IteratorBased, IteratorBased>
-  : evaluator_base<XprType>
-{
-protected:
+template <typename XprType>
+struct sparse_conjunction_evaluator<XprType, IteratorBased, IteratorBased> : evaluator_base<XprType> {
+ protected:
   typedef typename XprType::Functor BinaryOp;
   typedef typename XprType::Lhs LhsArg;
   typedef typename XprType::Rhs RhsArg;
-  typedef typename evaluator<LhsArg>::InnerIterator  LhsIterator;
-  typedef typename evaluator<RhsArg>::InnerIterator  RhsIterator;
+  typedef typename evaluator<LhsArg>::InnerIterator LhsIterator;
+  typedef typename evaluator<RhsArg>::InnerIterator RhsIterator;
   typedef typename XprType::StorageIndex StorageIndex;
   typedef typename traits<XprType>::Scalar Scalar;
-public:
 
-  class InnerIterator
-  {
-  public:
-    
+ public:
+  class InnerIterator {
+   public:
     EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
-      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor)
-    {
-      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
-      {
+        : m_lhsIter(aEval.m_lhsImpl, outer), m_rhsIter(aEval.m_rhsImpl, outer), m_functor(aEval.m_functor) {
+      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index())) {
         if (m_lhsIter.index() < m_rhsIter.index())
           ++m_lhsIter;
         else
@@ -426,12 +397,10 @@
       }
     }
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    {
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
       ++m_lhsIter;
       ++m_rhsIter;
-      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
-      {
+      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index())) {
         if (m_lhsIter.index() < m_rhsIter.index())
           ++m_lhsIter;
         else
@@ -439,7 +408,7 @@
       }
       return *this;
     }
-    
+
     EIGEN_STRONG_INLINE Scalar value() const { return m_functor(m_lhsIter.value(), m_rhsIter.value()); }
 
     EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); }
@@ -449,70 +418,64 @@
 
     EIGEN_STRONG_INLINE operator bool() const { return (m_lhsIter && m_rhsIter); }
 
-  protected:
+   protected:
     LhsIterator m_lhsIter;
     RhsIterator m_rhsIter;
     const BinaryOp& m_functor;
   };
-  
-  
+
   enum {
-    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) +
+                    int(functor_traits<BinaryOp>::Cost),
     Flags = XprType::Flags
   };
-  
+
   explicit sparse_conjunction_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_lhsImpl(xpr.lhs()), 
-      m_rhsImpl(xpr.rhs())  
-  {
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-  
+
   inline Index nonZerosEstimate() const {
     return (std::min)(m_lhsImpl.nonZerosEstimate(), m_rhsImpl.nonZerosEstimate());
   }
 
-protected:
+ protected:
   const BinaryOp m_functor;
   evaluator<LhsArg> m_lhsImpl;
   evaluator<RhsArg> m_rhsImpl;
 };
 
 // "dense ^ sparse"
-template<typename XprType>
-struct sparse_conjunction_evaluator<XprType, IndexBased, IteratorBased>
-  : evaluator_base<XprType>
-{
-protected:
+template <typename XprType>
+struct sparse_conjunction_evaluator<XprType, IndexBased, IteratorBased> : evaluator_base<XprType> {
+ protected:
   typedef typename XprType::Functor BinaryOp;
   typedef typename XprType::Lhs LhsArg;
   typedef typename XprType::Rhs RhsArg;
   typedef evaluator<LhsArg> LhsEvaluator;
-  typedef typename evaluator<RhsArg>::InnerIterator  RhsIterator;
+  typedef typename evaluator<RhsArg>::InnerIterator RhsIterator;
   typedef typename XprType::StorageIndex StorageIndex;
   typedef typename traits<XprType>::Scalar Scalar;
-public:
 
-  class InnerIterator
-  {
-    enum { IsRowMajor = (int(RhsArg::Flags)&RowMajorBit)==RowMajorBit };
+ public:
+  class InnerIterator {
+    enum { IsRowMajor = (int(RhsArg::Flags) & RowMajorBit) == RowMajorBit };
 
-  public:
-    
+   public:
     EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
-      : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_outer(outer)
-    {}
+        : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl, outer), m_functor(aEval.m_functor), m_outer(outer) {}
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    {
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
       ++m_rhsIter;
       return *this;
     }
 
-    EIGEN_STRONG_INLINE Scalar value() const
-    { return m_functor(m_lhsEval.coeff(IsRowMajor?m_outer:m_rhsIter.index(),IsRowMajor?m_rhsIter.index():m_outer), m_rhsIter.value()); }
+    EIGEN_STRONG_INLINE Scalar value() const {
+      return m_functor(
+          m_lhsEval.coeff(IsRowMajor ? m_outer : m_rhsIter.index(), IsRowMajor ? m_rhsIter.index() : m_outer),
+          m_rhsIter.value());
+    }
 
     EIGEN_STRONG_INLINE StorageIndex index() const { return m_rhsIter.index(); }
     EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); }
@@ -520,45 +483,38 @@
     EIGEN_STRONG_INLINE Index col() const { return m_rhsIter.col(); }
 
     EIGEN_STRONG_INLINE operator bool() const { return m_rhsIter; }
-    
-  protected:
-    const LhsEvaluator &m_lhsEval;
+
+   protected:
+    const LhsEvaluator& m_lhsEval;
     RhsIterator m_rhsIter;
     const BinaryOp& m_functor;
     const Index m_outer;
   };
-  
-  
+
   enum {
-    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) +
+                    int(functor_traits<BinaryOp>::Cost),
     Flags = XprType::Flags
   };
-  
+
   explicit sparse_conjunction_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_lhsImpl(xpr.lhs()), 
-      m_rhsImpl(xpr.rhs())  
-  {
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-  
-  inline Index nonZerosEstimate() const {
-    return m_rhsImpl.nonZerosEstimate();
-  }
 
-protected:
+  inline Index nonZerosEstimate() const { return m_rhsImpl.nonZerosEstimate(); }
+
+ protected:
   const BinaryOp m_functor;
   evaluator<LhsArg> m_lhsImpl;
   evaluator<RhsArg> m_rhsImpl;
 };
 
 // "sparse ^ dense"
-template<typename XprType>
-struct sparse_conjunction_evaluator<XprType, IteratorBased, IndexBased>
-  : evaluator_base<XprType>
-{
-protected:
+template <typename XprType>
+struct sparse_conjunction_evaluator<XprType, IteratorBased, IndexBased> : evaluator_base<XprType> {
+ protected:
   typedef typename XprType::Functor BinaryOp;
   typedef typename XprType::Lhs LhsArg;
   typedef typename XprType::Rhs RhsArg;
@@ -566,27 +522,24 @@
   typedef evaluator<RhsArg> RhsEvaluator;
   typedef typename XprType::StorageIndex StorageIndex;
   typedef typename traits<XprType>::Scalar Scalar;
-public:
 
-  class InnerIterator
-  {
-    enum { IsRowMajor = (int(LhsArg::Flags)&RowMajorBit)==RowMajorBit };
+ public:
+  class InnerIterator {
+    enum { IsRowMajor = (int(LhsArg::Flags) & RowMajorBit) == RowMajorBit };
 
-  public:
-    
+   public:
     EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
-      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_outer(outer)
-    {}
+        : m_lhsIter(aEval.m_lhsImpl, outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_outer(outer) {}
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    {
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
       ++m_lhsIter;
       return *this;
     }
 
-    EIGEN_STRONG_INLINE Scalar value() const
-    { return m_functor(m_lhsIter.value(),
-                       m_rhsEval.coeff(IsRowMajor?m_outer:m_lhsIter.index(),IsRowMajor?m_lhsIter.index():m_outer)); }
+    EIGEN_STRONG_INLINE Scalar value() const {
+      return m_functor(m_lhsIter.value(), m_rhsEval.coeff(IsRowMajor ? m_outer : m_lhsIter.index(),
+                                                          IsRowMajor ? m_lhsIter.index() : m_outer));
+    }
 
     EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); }
     EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
@@ -594,129 +547,392 @@
     EIGEN_STRONG_INLINE Index col() const { return m_lhsIter.col(); }
 
     EIGEN_STRONG_INLINE operator bool() const { return m_lhsIter; }
-    
-  protected:
+
+   protected:
     LhsIterator m_lhsIter;
-    const evaluator<RhsArg> &m_rhsEval;
+    const evaluator<RhsArg>& m_rhsEval;
     const BinaryOp& m_functor;
     const Index m_outer;
   };
-  
-  
+
   enum {
-    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) +
+                    int(functor_traits<BinaryOp>::Cost),
     Flags = XprType::Flags
   };
-  
+
   explicit sparse_conjunction_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_lhsImpl(xpr.lhs()), 
-      m_rhsImpl(xpr.rhs())  
-  {
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-  
-  inline Index nonZerosEstimate() const {
-    return m_lhsImpl.nonZerosEstimate();
-  }
 
-protected:
+  inline Index nonZerosEstimate() const { return m_lhsImpl.nonZerosEstimate(); }
+
+ protected:
   const BinaryOp m_functor;
   evaluator<LhsArg> m_lhsImpl;
   evaluator<RhsArg> m_rhsImpl;
 };
 
-}
+template <typename T, typename LhsKind = typename evaluator_traits<typename T::Lhs>::Kind,
+          typename RhsKind = typename evaluator_traits<typename T::Rhs>::Kind,
+          typename LhsScalar = typename traits<typename T::Lhs>::Scalar,
+          typename RhsScalar = typename traits<typename T::Rhs>::Scalar>
+struct sparse_disjunction_evaluator;
+
+// The disjunction "v" evaluator performs a logical "or" or set "union" operation on two input arrays.
+// Given input arrays 'lhs' and 'rhs' and binary functor 'func',
+// the sparse destination array 'dst' is evaluated as follows:
+//   if lhs(i,j) and rhs(i,j) are present, dst(i,j) = func(lhs(i,j), rhs(i,j))
+//   if lhs(i,j) is present and rhs(i,j) is null, dst(i,j) = lhs(i,j)
+//   if lhs(i,j) is null and rhs(i,j) is present, dst(i,j) = rhs(i,j)
+
+// "sparse v sparse"
+template <typename XprType>
+struct sparse_disjunction_evaluator<XprType, IteratorBased, IteratorBased> : evaluator_base<XprType> {
+ protected:
+  typedef typename XprType::Functor BinaryOp;
+  typedef typename XprType::Lhs LhsArg;
+  typedef typename XprType::Rhs RhsArg;
+  typedef typename evaluator<LhsArg>::InnerIterator LhsIterator;
+  typedef typename evaluator<RhsArg>::InnerIterator RhsIterator;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename traits<XprType>::Scalar Scalar;
+
+ public:
+  class InnerIterator {
+   public:
+    EIGEN_STRONG_INLINE InnerIterator(const sparse_disjunction_evaluator& aEval, Index outer)
+        : m_lhsIter(aEval.m_lhsImpl, outer),
+          m_rhsIter(aEval.m_rhsImpl, outer),
+          m_functor(aEval.m_functor),
+          m_value(Scalar(0)) {
+      this->operator++();
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
+      if (m_lhsIter && m_rhsIter && (m_lhsIter.index() == m_rhsIter.index())) {
+        m_id = m_lhsIter.index();
+        m_value = m_functor(m_lhsIter.value(), m_rhsIter.value());
+        ++m_lhsIter;
+        ++m_rhsIter;
+      } else if (m_lhsIter && (!m_rhsIter || (m_lhsIter.index() < m_rhsIter.index()))) {
+        m_id = m_lhsIter.index();
+        m_value = m_lhsIter.value();
+        ++m_lhsIter;
+      } else if (m_rhsIter && (!m_lhsIter || (m_lhsIter.index() > m_rhsIter.index()))) {
+        m_id = m_rhsIter.index();
+        m_value = m_rhsIter.value();
+        ++m_rhsIter;
+      } else {
+        m_id = -1;
+      }
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return m_value; }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+    EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return LhsArg::IsRowMajor ? m_lhsIter.row() : index(); }
+    EIGEN_STRONG_INLINE Index col() const { return LhsArg::IsRowMajor ? index() : m_lhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_id >= 0; }
+
+   protected:
+    LhsIterator m_lhsIter;
+    RhsIterator m_rhsIter;
+    const BinaryOp& m_functor;
+    Scalar m_value;
+    StorageIndex m_id;
+  };
+
+  enum {
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) +
+                    int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+
+  explicit sparse_disjunction_evaluator(const XprType& xpr)
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()) {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  inline Index nonZerosEstimate() const { return m_lhsImpl.nonZerosEstimate() + m_rhsImpl.nonZerosEstimate(); }
+
+ protected:
+  const BinaryOp m_functor;
+  evaluator<LhsArg> m_lhsImpl;
+  evaluator<RhsArg> m_rhsImpl;
+};
+
+// "dense v sparse"
+template <typename XprType>
+struct sparse_disjunction_evaluator<XprType, IndexBased, IteratorBased> : evaluator_base<XprType> {
+ protected:
+  typedef typename XprType::Functor BinaryOp;
+  typedef typename XprType::Lhs LhsArg;
+  typedef typename XprType::Rhs RhsArg;
+  typedef evaluator<LhsArg> LhsEvaluator;
+  typedef typename evaluator<RhsArg>::InnerIterator RhsIterator;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename traits<XprType>::Scalar Scalar;
+
+ public:
+  class InnerIterator {
+    enum { IsRowMajor = (int(RhsArg::Flags) & RowMajorBit) == RowMajorBit };
+
+   public:
+    EIGEN_STRONG_INLINE InnerIterator(const sparse_disjunction_evaluator& aEval, Index outer)
+        : m_lhsEval(aEval.m_lhsImpl),
+          m_rhsIter(aEval.m_rhsImpl, outer),
+          m_functor(aEval.m_functor),
+          m_value(0),
+          m_id(-1),
+          m_innerSize(aEval.m_expr.rhs().innerSize()) {
+      this->operator++();
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
+      ++m_id;
+      if (m_id < m_innerSize) {
+        Scalar lhsVal = m_lhsEval.coeff(IsRowMajor ? m_rhsIter.outer() : m_id, IsRowMajor ? m_id : m_rhsIter.outer());
+        if (m_rhsIter && m_rhsIter.index() == m_id) {
+          m_value = m_functor(lhsVal, m_rhsIter.value());
+          ++m_rhsIter;
+        } else
+          m_value = lhsVal;
+      }
+
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const {
+      eigen_internal_assert(m_id < m_innerSize);
+      return m_value;
+    }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+    EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_rhsIter.outer() : m_id; }
+    EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_rhsIter.outer(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_id < m_innerSize; }
+
+   protected:
+    const evaluator<LhsArg>& m_lhsEval;
+    RhsIterator m_rhsIter;
+    const BinaryOp& m_functor;
+    Scalar m_value;
+    StorageIndex m_id;
+    StorageIndex m_innerSize;
+  };
+
+  enum {
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) +
+                    int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+
+  explicit sparse_disjunction_evaluator(const XprType& xpr)
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()), m_expr(xpr) {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  inline Index nonZerosEstimate() const { return m_expr.size(); }
+
+ protected:
+  const BinaryOp m_functor;
+  evaluator<LhsArg> m_lhsImpl;
+  evaluator<RhsArg> m_rhsImpl;
+  const XprType& m_expr;
+};
+
+// "sparse v dense"
+template <typename XprType>
+struct sparse_disjunction_evaluator<XprType, IteratorBased, IndexBased> : evaluator_base<XprType> {
+ protected:
+  typedef typename XprType::Functor BinaryOp;
+  typedef typename XprType::Lhs LhsArg;
+  typedef typename XprType::Rhs RhsArg;
+  typedef typename evaluator<LhsArg>::InnerIterator LhsIterator;
+  typedef evaluator<RhsArg> RhsEvaluator;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename traits<XprType>::Scalar Scalar;
+
+ public:
+  class InnerIterator {
+    enum { IsRowMajor = (int(LhsArg::Flags) & RowMajorBit) == RowMajorBit };
+
+   public:
+    EIGEN_STRONG_INLINE InnerIterator(const sparse_disjunction_evaluator& aEval, Index outer)
+        : m_lhsIter(aEval.m_lhsImpl, outer),
+          m_rhsEval(aEval.m_rhsImpl),
+          m_functor(aEval.m_functor),
+          m_value(0),
+          m_id(-1),
+          m_innerSize(aEval.m_expr.lhs().innerSize()) {
+      this->operator++();
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
+      ++m_id;
+      if (m_id < m_innerSize) {
+        Scalar rhsVal = m_rhsEval.coeff(IsRowMajor ? m_lhsIter.outer() : m_id, IsRowMajor ? m_id : m_lhsIter.outer());
+        if (m_lhsIter && m_lhsIter.index() == m_id) {
+          m_value = m_functor(m_lhsIter.value(), rhsVal);
+          ++m_lhsIter;
+        } else
+          m_value = rhsVal;
+      }
+
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const {
+      eigen_internal_assert(m_id < m_innerSize);
+      return m_value;
+    }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+    EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_lhsIter.outer() : m_id; }
+    EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_lhsIter.outer(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_id < m_innerSize; }
+
+   protected:
+    LhsIterator m_lhsIter;
+    const evaluator<RhsArg>& m_rhsEval;
+    const BinaryOp& m_functor;
+    Scalar m_value;
+    StorageIndex m_id;
+    StorageIndex m_innerSize;
+  };
+
+  enum {
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) +
+                    int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+
+  explicit sparse_disjunction_evaluator(const XprType& xpr)
+      : m_functor(xpr.functor()), m_lhsImpl(xpr.lhs()), m_rhsImpl(xpr.rhs()), m_expr(xpr) {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  inline Index nonZerosEstimate() const { return m_expr.size(); }
+
+ protected:
+  const BinaryOp m_functor;
+  evaluator<LhsArg> m_lhsImpl;
+  evaluator<RhsArg> m_rhsImpl;
+  const XprType& m_expr;
+};
+
+// when DupFunc is wrapped with scalar_dup_op, use disjunction evaulator
+template <typename T1, typename T2, typename DupFunc, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_disjunction_op<DupFunc, T1, T2>, Lhs, Rhs>, IteratorBased, IteratorBased>
+    : sparse_disjunction_evaluator<CwiseBinaryOp<scalar_disjunction_op<DupFunc, T1, T2>, Lhs, Rhs> > {
+  typedef CwiseBinaryOp<scalar_disjunction_op<DupFunc, T1, T2>, Lhs, Rhs> XprType;
+  typedef sparse_disjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+}  // namespace internal
 
 /***************************************************************************
-* Implementation of SparseMatrixBase and SparseCwise functions/operators
-***************************************************************************/
+ * Implementation of SparseMatrixBase and SparseCwise functions/operators
+ ***************************************************************************/
 
-template<typename Derived>
-template<typename OtherDerived>
-Derived& SparseMatrixBase<Derived>::operator+=(const EigenBase<OtherDerived> &other)
-{
-  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+template <typename Derived>
+template <typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator+=(const EigenBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-Derived& SparseMatrixBase<Derived>::operator-=(const EigenBase<OtherDerived> &other)
-{
-  call_assignment(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+template <typename Derived>
+template <typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator-=(const EigenBase<OtherDerived>& other) {
+  call_assignment(derived(), other.derived(), internal::assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_STRONG_INLINE Derived &
-SparseMatrixBase<Derived>::operator-=(const SparseMatrixBase<OtherDerived> &other)
-{
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_STRONG_INLINE Derived& SparseMatrixBase<Derived>::operator-=(const SparseMatrixBase<OtherDerived>& other) {
   return derived() = derived() - other.derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_STRONG_INLINE Derived &
-SparseMatrixBase<Derived>::operator+=(const SparseMatrixBase<OtherDerived>& other)
-{
+template <typename Derived>
+template <typename OtherDerived>
+EIGEN_STRONG_INLINE Derived& SparseMatrixBase<Derived>::operator+=(const SparseMatrixBase<OtherDerived>& other) {
   return derived() = derived() + other.derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-Derived& SparseMatrixBase<Derived>::operator+=(const DiagonalBase<OtherDerived>& other)
-{
-  call_assignment_no_alias(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+template <typename Derived>
+template <typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator+=(const DiagonalBase<OtherDerived>& other) {
+  call_assignment_no_alias(derived(), other.derived(),
+                           internal::add_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-Derived& SparseMatrixBase<Derived>::operator-=(const DiagonalBase<OtherDerived>& other)
-{
-  call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+template <typename Derived>
+template <typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator-=(const DiagonalBase<OtherDerived>& other) {
+  call_assignment_no_alias(derived(), other.derived(),
+                           internal::sub_assign_op<Scalar, typename OtherDerived::Scalar>());
   return derived();
 }
-    
-template<typename Derived>
-template<typename OtherDerived>
+
+template <typename Derived>
+template <typename OtherDerived>
 EIGEN_STRONG_INLINE const typename SparseMatrixBase<Derived>::template CwiseProductDenseReturnType<OtherDerived>::Type
-SparseMatrixBase<Derived>::cwiseProduct(const MatrixBase<OtherDerived> &other) const
-{
+SparseMatrixBase<Derived>::cwiseProduct(const MatrixBase<OtherDerived>& other) const {
   return typename CwiseProductDenseReturnType<OtherDerived>::Type(derived(), other.derived());
 }
 
-template<typename DenseDerived, typename SparseDerived>
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>
-operator+(const MatrixBase<DenseDerived> &a, const SparseMatrixBase<SparseDerived> &b)
-{
-  return CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>(a.derived(), b.derived());
+template <typename DenseDerived, typename SparseDerived>
+EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar, typename SparseDerived::Scalar>,
+                  const DenseDerived, const SparseDerived>
+    operator+(const MatrixBase<DenseDerived>& a, const SparseMatrixBase<SparseDerived>& b) {
+  return CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar, typename SparseDerived::Scalar>,
+                       const DenseDerived, const SparseDerived>(a.derived(), b.derived());
 }
 
-template<typename SparseDerived, typename DenseDerived>
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>
-operator+(const SparseMatrixBase<SparseDerived> &a, const MatrixBase<DenseDerived> &b)
-{
-  return CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>(a.derived(), b.derived());
+template <typename SparseDerived, typename DenseDerived>
+EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar, typename DenseDerived::Scalar>,
+                  const SparseDerived, const DenseDerived>
+    operator+(const SparseMatrixBase<SparseDerived>& a, const MatrixBase<DenseDerived>& b) {
+  return CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar, typename DenseDerived::Scalar>,
+                       const SparseDerived, const DenseDerived>(a.derived(), b.derived());
 }
 
-template<typename DenseDerived, typename SparseDerived>
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>
-operator-(const MatrixBase<DenseDerived> &a, const SparseMatrixBase<SparseDerived> &b)
-{
-  return CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>(a.derived(), b.derived());
+template <typename DenseDerived, typename SparseDerived>
+EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar, typename SparseDerived::Scalar>,
+                  const DenseDerived, const SparseDerived>
+    operator-(const MatrixBase<DenseDerived>& a, const SparseMatrixBase<SparseDerived>& b) {
+  return CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar, typename SparseDerived::Scalar>,
+                       const DenseDerived, const SparseDerived>(a.derived(), b.derived());
 }
 
-template<typename SparseDerived, typename DenseDerived>
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>
-operator-(const SparseMatrixBase<SparseDerived> &a, const MatrixBase<DenseDerived> &b)
-{
-  return CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>(a.derived(), b.derived());
+template <typename SparseDerived, typename DenseDerived>
+EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar, typename DenseDerived::Scalar>,
+                  const SparseDerived, const DenseDerived>
+    operator-(const SparseMatrixBase<SparseDerived>& a, const MatrixBase<DenseDerived>& b) {
+  return CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar, typename DenseDerived::Scalar>,
+                       const SparseDerived, const DenseDerived>(a.derived(), b.derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_CWISE_BINARY_OP_H
+#endif  // EIGEN_SPARSE_CWISE_BINARY_OP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
index 32dac0f..9fc1e66 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
@@ -10,141 +10,133 @@
 #ifndef EIGEN_SPARSE_CWISE_UNARY_OP_H
 #define EIGEN_SPARSE_CWISE_UNARY_OP_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-  
-template<typename UnaryOp, typename ArgType>
-struct unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>
-  : public evaluator_base<CwiseUnaryOp<UnaryOp,ArgType> >
-{
-  public:
-    typedef CwiseUnaryOp<UnaryOp, ArgType> XprType;
 
-    class InnerIterator;
-    
-    enum {
-      CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
-      Flags = XprType::Flags
-    };
-    
-    explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression())
-    {
-      EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
-      EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
-    }
-    
-    inline Index nonZerosEstimate() const {
-      return m_argImpl.nonZerosEstimate();
-    }
+template <typename UnaryOp, typename ArgType>
+struct unary_evaluator<CwiseUnaryOp<UnaryOp, ArgType>, IteratorBased>
+    : public evaluator_base<CwiseUnaryOp<UnaryOp, ArgType> > {
+ public:
+  typedef CwiseUnaryOp<UnaryOp, ArgType> XprType;
 
-  protected:
-    typedef typename evaluator<ArgType>::InnerIterator        EvalIterator;
-    
-    const UnaryOp m_functor;
-    evaluator<ArgType> m_argImpl;
+  class InnerIterator;
+
+  enum {
+    CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+
+  explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression()) {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  inline Index nonZerosEstimate() const { return m_argImpl.nonZerosEstimate(); }
+
+ protected:
+  typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+  const UnaryOp m_functor;
+  evaluator<ArgType> m_argImpl;
 };
 
-template<typename UnaryOp, typename ArgType>
-class unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::InnerIterator
-    : public unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::EvalIterator
-{
-  protected:
-    typedef typename XprType::Scalar Scalar;
-    typedef typename unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::EvalIterator Base;
-  public:
+template <typename UnaryOp, typename ArgType>
+class unary_evaluator<CwiseUnaryOp<UnaryOp, ArgType>, IteratorBased>::InnerIterator
+    : public unary_evaluator<CwiseUnaryOp<UnaryOp, ArgType>, IteratorBased>::EvalIterator {
+ protected:
+  typedef typename XprType::Scalar Scalar;
+  typedef typename unary_evaluator<CwiseUnaryOp<UnaryOp, ArgType>, IteratorBased>::EvalIterator Base;
 
-    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
-      : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor)
-    {}
+ public:
+  EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+      : Base(unaryOp.m_argImpl, outer), m_functor(unaryOp.m_functor) {}
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    { Base::operator++(); return *this; }
+  EIGEN_STRONG_INLINE InnerIterator& operator++() {
+    Base::operator++();
+    return *this;
+  }
 
-    EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
+  EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
 
-  protected:
-    const UnaryOp m_functor;
-  private:
-    Scalar& valueRef();
+ protected:
+  const UnaryOp m_functor;
+
+ private:
+  Scalar& valueRef();
 };
 
-template<typename ViewOp, typename ArgType>
-struct unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>
-  : public evaluator_base<CwiseUnaryView<ViewOp,ArgType> >
-{
-  public:
-    typedef CwiseUnaryView<ViewOp, ArgType> XprType;
+template <typename ViewOp, typename ArgType>
+struct unary_evaluator<CwiseUnaryView<ViewOp, ArgType>, IteratorBased>
+    : public evaluator_base<CwiseUnaryView<ViewOp, ArgType> > {
+ public:
+  typedef CwiseUnaryView<ViewOp, ArgType> XprType;
 
-    class InnerIterator;
-    
-    enum {
-      CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<ViewOp>::Cost),
-      Flags = XprType::Flags
-    };
-    
-    explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression())
-    {
-      EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<ViewOp>::Cost);
-      EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
-    }
+  class InnerIterator;
 
-  protected:
-    typedef typename evaluator<ArgType>::InnerIterator        EvalIterator;
-    
-    const ViewOp m_functor;
-    evaluator<ArgType> m_argImpl;
+  enum {
+    CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<ViewOp>::Cost),
+    Flags = XprType::Flags
+  };
+
+  explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression()) {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<ViewOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+ protected:
+  typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+  const ViewOp m_functor;
+  evaluator<ArgType> m_argImpl;
 };
 
-template<typename ViewOp, typename ArgType>
-class unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::InnerIterator
-    : public unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::EvalIterator
-{
-  protected:
-    typedef typename XprType::Scalar Scalar;
-    typedef typename unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::EvalIterator Base;
-  public:
+template <typename ViewOp, typename ArgType>
+class unary_evaluator<CwiseUnaryView<ViewOp, ArgType>, IteratorBased>::InnerIterator
+    : public unary_evaluator<CwiseUnaryView<ViewOp, ArgType>, IteratorBased>::EvalIterator {
+ protected:
+  typedef typename XprType::Scalar Scalar;
+  typedef typename unary_evaluator<CwiseUnaryView<ViewOp, ArgType>, IteratorBased>::EvalIterator Base;
 
-    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
-      : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor)
-    {}
+ public:
+  EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+      : Base(unaryOp.m_argImpl, outer), m_functor(unaryOp.m_functor) {}
 
-    EIGEN_STRONG_INLINE InnerIterator& operator++()
-    { Base::operator++(); return *this; }
+  EIGEN_STRONG_INLINE InnerIterator& operator++() {
+    Base::operator++();
+    return *this;
+  }
 
-    EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
-    EIGEN_STRONG_INLINE Scalar& valueRef() { return m_functor(Base::valueRef()); }
+  EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
+  EIGEN_STRONG_INLINE Scalar& valueRef() { return m_functor(Base::valueRef()); }
 
-  protected:
-    const ViewOp m_functor;
+ protected:
+  const ViewOp m_functor;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename Derived>
-EIGEN_STRONG_INLINE Derived&
-SparseMatrixBase<Derived>::operator*=(const Scalar& other)
-{
+template <typename Derived>
+EIGEN_STRONG_INLINE Derived& SparseMatrixBase<Derived>::operator*=(const Scalar& other) {
   typedef typename internal::evaluator<Derived>::InnerIterator EvalIterator;
   internal::evaluator<Derived> thisEval(derived());
-  for (Index j=0; j<outerSize(); ++j)
-    for (EvalIterator i(thisEval,j); i; ++i)
-      i.valueRef() *= other;
+  for (Index j = 0; j < outerSize(); ++j)
+    for (EvalIterator i(thisEval, j); i; ++i) i.valueRef() *= other;
   return derived();
 }
 
-template<typename Derived>
-EIGEN_STRONG_INLINE Derived&
-SparseMatrixBase<Derived>::operator/=(const Scalar& other)
-{
+template <typename Derived>
+EIGEN_STRONG_INLINE Derived& SparseMatrixBase<Derived>::operator/=(const Scalar& other) {
   typedef typename internal::evaluator<Derived>::InnerIterator EvalIterator;
   internal::evaluator<Derived> thisEval(derived());
-  for (Index j=0; j<outerSize(); ++j)
-    for (EvalIterator i(thisEval,j); i; ++i)
-      i.valueRef() /= other;
+  for (Index j = 0; j < outerSize(); ++j)
+    for (EvalIterator i(thisEval, j); i; ++i) i.valueRef() /= other;
   return derived();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_CWISE_UNARY_OP_H
+#endif  // EIGEN_SPARSE_CWISE_UNARY_OP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h
index f005a18..db70810 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h
@@ -10,71 +10,81 @@
 #ifndef EIGEN_SPARSEDENSEPRODUCT_H
 #define EIGEN_SPARSEDENSEPRODUCT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template <> struct product_promote_storage_type<Sparse,Dense, OuterProduct> { typedef Sparse ret; };
-template <> struct product_promote_storage_type<Dense,Sparse, OuterProduct> { typedef Sparse ret; };
+template <>
+struct product_promote_storage_type<Sparse, Dense, OuterProduct> {
+  typedef Sparse ret;
+};
+template <>
+struct product_promote_storage_type<Dense, Sparse, OuterProduct> {
+  typedef Sparse ret;
+};
 
-template<typename SparseLhsType, typename DenseRhsType, typename DenseResType,
-         typename AlphaType,
-         int LhsStorageOrder = ((SparseLhsType::Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor,
-         bool ColPerCol = ((DenseRhsType::Flags&RowMajorBit)==0) || DenseRhsType::ColsAtCompileTime==1>
+template <typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType,
+          int LhsStorageOrder = ((SparseLhsType::Flags & RowMajorBit) == RowMajorBit) ? RowMajor : ColMajor,
+          bool ColPerCol = ((DenseRhsType::Flags & RowMajorBit) == 0) || DenseRhsType::ColsAtCompileTime == 1>
 struct sparse_time_dense_product_impl;
 
-template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
-struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, RowMajor, true>
-{
-  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
-  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
-  typedef typename internal::remove_all<DenseResType>::type Res;
+template <typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType, DenseRhsType, DenseResType, typename DenseResType::Scalar,
+                                      RowMajor, true> {
+  typedef internal::remove_all_t<SparseLhsType> Lhs;
+  typedef internal::remove_all_t<DenseRhsType> Rhs;
+  typedef internal::remove_all_t<DenseResType> Res;
   typedef typename evaluator<Lhs>::InnerIterator LhsInnerIterator;
   typedef evaluator<Lhs> LhsEval;
-  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
-  {
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res,
+                  const typename Res::Scalar& alpha) {
     LhsEval lhsEval(lhs);
-    
+
     Index n = lhs.outerSize();
 #ifdef EIGEN_HAS_OPENMP
     Eigen::initParallel();
     Index threads = Eigen::nbThreads();
 #endif
-    
-    for(Index c=0; c<rhs.cols(); ++c)
-    {
+
+    for (Index c = 0; c < rhs.cols(); ++c) {
 #ifdef EIGEN_HAS_OPENMP
       // This 20000 threshold has been found experimentally on 2D and 3D Poisson problems.
       // It basically represents the minimal amount of work to be done to be worth it.
-      if(threads>1 && lhsEval.nonZerosEstimate() > 20000)
-      {
-        #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads)
-        for(Index i=0; i<n; ++i)
-          processRow(lhsEval,rhs,res,alpha,i,c);
-      }
-      else
+      if (threads > 1 && lhsEval.nonZerosEstimate() > 20000) {
+#pragma omp parallel for schedule(dynamic, (n + threads * 4 - 1) / (threads * 4)) num_threads(threads)
+        for (Index i = 0; i < n; ++i) processRow(lhsEval, rhs, res, alpha, i, c);
+      } else
 #endif
       {
-        for(Index i=0; i<n; ++i)
-          processRow(lhsEval,rhs,res,alpha,i,c);
+        for (Index i = 0; i < n; ++i) processRow(lhsEval, rhs, res, alpha, i, c);
       }
     }
   }
-  
-  static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha, Index i, Index col)
-  {
-    typename Res::Scalar tmp(0);
-    for(LhsInnerIterator it(lhsEval,i); it ;++it)
-      tmp += it.value() * rhs.coeff(it.index(),col);
-    res.coeffRef(i,col) += alpha * tmp;
+
+  static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, DenseResType& res,
+                         const typename Res::Scalar& alpha, Index i, Index col) {
+    // Two accumulators, which breaks the dependency chain on the accumulator
+    // and allows more instruction-level parallelism in the following loop
+    typename Res::Scalar tmp_a(0);
+    typename Res::Scalar tmp_b(0);
+    for (LhsInnerIterator it(lhsEval, i); it; ++it) {
+      tmp_a += it.value() * rhs.coeff(it.index(), col);
+      ++it;
+      if (it) {
+        tmp_b += it.value() * rhs.coeff(it.index(), col);
+      }
+    }
+    res.coeffRef(i, col) += alpha * (tmp_a + tmp_b);
   }
-  
 };
 
 // FIXME: what is the purpose of the following specialization? Is it for the BlockedSparse format?
 // -> let's disable it for now as it is conflicting with generic scalar*matrix and matrix*scalar operators
-// template<typename T1, typename T2/*, int _Options, typename _StrideType*/>
-// struct ScalarBinaryOpTraits<T1, Ref<T2/*, _Options, _StrideType*/> >
+// template<typename T1, typename T2/*, int Options_, typename StrideType_*/>
+// struct ScalarBinaryOpTraits<T1, Ref<T2/*, Options_, StrideType_*/> >
 // {
 //   enum {
 //     Defined = 1
@@ -82,40 +92,35 @@
 //   typedef typename CwiseUnaryOp<scalar_multiple2_op<T1, typename T2::Scalar>, T2>::PlainObject ReturnType;
 // };
 
-template<typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
-struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, AlphaType, ColMajor, true>
-{
-  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
-  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
-  typedef typename internal::remove_all<DenseResType>::type Res;
+template <typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
+struct sparse_time_dense_product_impl<SparseLhsType, DenseRhsType, DenseResType, AlphaType, ColMajor, true> {
+  typedef internal::remove_all_t<SparseLhsType> Lhs;
+  typedef internal::remove_all_t<DenseRhsType> Rhs;
+  typedef internal::remove_all_t<DenseResType> Res;
   typedef evaluator<Lhs> LhsEval;
   typedef typename LhsEval::InnerIterator LhsInnerIterator;
-  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
-  {
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha) {
     LhsEval lhsEval(lhs);
-    for(Index c=0; c<rhs.cols(); ++c)
-    {
-      for(Index j=0; j<lhs.outerSize(); ++j)
-      {
-//        typename Res::Scalar rhs_j = alpha * rhs.coeff(j,c);
-        typename ScalarBinaryOpTraits<AlphaType, typename Rhs::Scalar>::ReturnType rhs_j(alpha * rhs.coeff(j,c));
-        for(LhsInnerIterator it(lhsEval,j); it ;++it)
-          res.coeffRef(it.index(),c) += it.value() * rhs_j;
+    for (Index c = 0; c < rhs.cols(); ++c) {
+      for (Index j = 0; j < lhs.outerSize(); ++j) {
+        //        typename Res::Scalar rhs_j = alpha * rhs.coeff(j,c);
+        typename ScalarBinaryOpTraits<AlphaType, typename Rhs::Scalar>::ReturnType rhs_j(alpha * rhs.coeff(j, c));
+        for (LhsInnerIterator it(lhsEval, j); it; ++it) res.coeffRef(it.index(), c) += it.value() * rhs_j;
       }
     }
   }
 };
 
-template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
-struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, RowMajor, false>
-{
-  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
-  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
-  typedef typename internal::remove_all<DenseResType>::type Res;
+template <typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType, DenseRhsType, DenseResType, typename DenseResType::Scalar,
+                                      RowMajor, false> {
+  typedef internal::remove_all_t<SparseLhsType> Lhs;
+  typedef internal::remove_all_t<DenseRhsType> Rhs;
+  typedef internal::remove_all_t<DenseResType> Res;
   typedef evaluator<Lhs> LhsEval;
   typedef typename LhsEval::InnerIterator LhsInnerIterator;
-  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
-  {
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res,
+                  const typename Res::Scalar& alpha) {
     Index n = lhs.rows();
     LhsEval lhsEval(lhs);
 
@@ -124,219 +129,188 @@
     Index threads = Eigen::nbThreads();
     // This 20000 threshold has been found experimentally on 2D and 3D Poisson problems.
     // It basically represents the minimal amount of work to be done to be worth it.
-    if(threads>1 && lhsEval.nonZerosEstimate()*rhs.cols() > 20000)
-    {
-      #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads)
-      for(Index i=0; i<n; ++i)
-        processRow(lhsEval,rhs,res,alpha,i);
-    }
-    else
+    if (threads > 1 && lhsEval.nonZerosEstimate() * rhs.cols() > 20000) {
+#pragma omp parallel for schedule(dynamic, (n + threads * 4 - 1) / (threads * 4)) num_threads(threads)
+      for (Index i = 0; i < n; ++i) processRow(lhsEval, rhs, res, alpha, i);
+    } else
 #endif
     {
-      for(Index i=0; i<n; ++i)
-        processRow(lhsEval, rhs, res, alpha, i);
+      for (Index i = 0; i < n; ++i) processRow(lhsEval, rhs, res, alpha, i);
     }
   }
 
-  static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, Res& res, const typename Res::Scalar& alpha, Index i)
-  {
+  static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, Res& res, const typename Res::Scalar& alpha,
+                         Index i) {
     typename Res::RowXpr res_i(res.row(i));
-    for(LhsInnerIterator it(lhsEval,i); it ;++it)
-      res_i += (alpha*it.value()) * rhs.row(it.index());
+    for (LhsInnerIterator it(lhsEval, i); it; ++it) res_i += (alpha * it.value()) * rhs.row(it.index());
   }
 };
 
-template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
-struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, ColMajor, false>
-{
-  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
-  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
-  typedef typename internal::remove_all<DenseResType>::type Res;
+template <typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType, DenseRhsType, DenseResType, typename DenseResType::Scalar,
+                                      ColMajor, false> {
+  typedef internal::remove_all_t<SparseLhsType> Lhs;
+  typedef internal::remove_all_t<DenseRhsType> Rhs;
+  typedef internal::remove_all_t<DenseResType> Res;
   typedef typename evaluator<Lhs>::InnerIterator LhsInnerIterator;
-  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
-  {
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res,
+                  const typename Res::Scalar& alpha) {
     evaluator<Lhs> lhsEval(lhs);
-    for(Index j=0; j<lhs.outerSize(); ++j)
-    {
+    for (Index j = 0; j < lhs.outerSize(); ++j) {
       typename Rhs::ConstRowXpr rhs_j(rhs.row(j));
-      for(LhsInnerIterator it(lhsEval,j); it ;++it)
-        res.row(it.index()) += (alpha*it.value()) * rhs_j;
+      for (LhsInnerIterator it(lhsEval, j); it; ++it) res.row(it.index()) += (alpha * it.value()) * rhs_j;
     }
   }
 };
 
-template<typename SparseLhsType, typename DenseRhsType, typename DenseResType,typename AlphaType>
-inline void sparse_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
-{
-  sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, AlphaType>::run(lhs, rhs, res, alpha);
+template <typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
+inline void sparse_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res,
+                                      const AlphaType& alpha) {
+  sparse_time_dense_product_impl<SparseLhsType, DenseRhsType, DenseResType, AlphaType>::run(lhs, rhs, res, alpha);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, int ProductType>
+template <typename Lhs, typename Rhs, int ProductType>
 struct generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType>
- : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,SparseShape,DenseShape,ProductType> >
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
-  template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
-    typedef typename nested_eval<Lhs,((Rhs::Flags&RowMajorBit)==0) ? 1 : Rhs::ColsAtCompileTime>::type LhsNested;
-    typedef typename nested_eval<Rhs,((Lhs::Flags&RowMajorBit)==0) ? 1 : Dynamic>::type RhsNested;
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType> > {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
+
+  template <typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
+    typedef typename nested_eval<Lhs, ((Rhs::Flags & RowMajorBit) == 0) ? 1 : Rhs::ColsAtCompileTime>::type LhsNested;
+    typedef typename nested_eval<Rhs, ((Lhs::Flags & RowMajorBit) == 0) ? 1 : Dynamic>::type RhsNested;
     LhsNested lhsNested(lhs);
     RhsNested rhsNested(rhs);
     internal::sparse_time_dense_product(lhsNested, rhsNested, dst, alpha);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductType>
+template <typename Lhs, typename Rhs, int ProductType>
 struct generic_product_impl<Lhs, Rhs, SparseTriangularShape, DenseShape, ProductType>
-  : generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType>
-{};
+    : generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType> {};
 
-template<typename Lhs, typename Rhs, int ProductType>
+template <typename Lhs, typename Rhs, int ProductType>
 struct generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType>
-  : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,SparseShape,ProductType> >
-{
-  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
-  template<typename Dst>
-  static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-  {
-    typedef typename nested_eval<Lhs,((Rhs::Flags&RowMajorBit)==0) ? Dynamic : 1>::type LhsNested;
-    typedef typename nested_eval<Rhs,((Lhs::Flags&RowMajorBit)==RowMajorBit) ? 1 : Lhs::RowsAtCompileTime>::type RhsNested;
+    : generic_product_impl_base<Lhs, Rhs, generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType> > {
+  typedef typename Product<Lhs, Rhs>::Scalar Scalar;
+
+  template <typename Dst>
+  static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
+    typedef typename nested_eval<Lhs, ((Rhs::Flags & RowMajorBit) == 0) ? Dynamic : 1>::type LhsNested;
+    typedef typename nested_eval<Rhs, ((Lhs::Flags & RowMajorBit) == RowMajorBit) ? 1 : Lhs::RowsAtCompileTime>::type
+        RhsNested;
     LhsNested lhsNested(lhs);
     RhsNested rhsNested(rhs);
-    
+
     // transpose everything
     Transpose<Dst> dstT(dst);
     internal::sparse_time_dense_product(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha);
   }
 };
 
-template<typename Lhs, typename Rhs, int ProductType>
+template <typename Lhs, typename Rhs, int ProductType>
 struct generic_product_impl<Lhs, Rhs, DenseShape, SparseTriangularShape, ProductType>
-  : generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType>
-{};
+    : generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType> {};
 
-template<typename LhsT, typename RhsT, bool NeedToTranspose>
-struct sparse_dense_outer_product_evaluator
-{
-protected:
-  typedef typename conditional<NeedToTranspose,RhsT,LhsT>::type Lhs1;
-  typedef typename conditional<NeedToTranspose,LhsT,RhsT>::type ActualRhs;
-  typedef Product<LhsT,RhsT,DefaultProduct> ProdXprType;
-  
+template <typename LhsT, typename RhsT, bool NeedToTranspose>
+struct sparse_dense_outer_product_evaluator {
+ protected:
+  typedef std::conditional_t<NeedToTranspose, RhsT, LhsT> Lhs1;
+  typedef std::conditional_t<NeedToTranspose, LhsT, RhsT> ActualRhs;
+  typedef Product<LhsT, RhsT, DefaultProduct> ProdXprType;
+
   // if the actual left-hand side is a dense vector,
   // then build a sparse-view so that we can seamlessly iterate over it.
-  typedef typename conditional<is_same<typename internal::traits<Lhs1>::StorageKind,Sparse>::value,
-            Lhs1, SparseView<Lhs1> >::type ActualLhs;
-  typedef typename conditional<is_same<typename internal::traits<Lhs1>::StorageKind,Sparse>::value,
-            Lhs1 const&, SparseView<Lhs1> >::type LhsArg;
-            
+  typedef std::conditional_t<is_same<typename internal::traits<Lhs1>::StorageKind, Sparse>::value, Lhs1,
+                             SparseView<Lhs1> >
+      ActualLhs;
+  typedef std::conditional_t<is_same<typename internal::traits<Lhs1>::StorageKind, Sparse>::value, Lhs1 const&,
+                             SparseView<Lhs1> >
+      LhsArg;
+
   typedef evaluator<ActualLhs> LhsEval;
   typedef evaluator<ActualRhs> RhsEval;
   typedef typename evaluator<ActualLhs>::InnerIterator LhsIterator;
   typedef typename ProdXprType::Scalar Scalar;
-  
-public:
-  enum {
-    Flags = NeedToTranspose ? RowMajorBit : 0,
-    CoeffReadCost = HugeCost
-  };
-  
-  class InnerIterator : public LhsIterator
-  {
-  public:
-    InnerIterator(const sparse_dense_outer_product_evaluator &xprEval, Index outer)
-      : LhsIterator(xprEval.m_lhsXprImpl, 0),
-        m_outer(outer),
-        m_empty(false),
-        m_factor(get(xprEval.m_rhsXprImpl, outer, typename internal::traits<ActualRhs>::StorageKind() ))
-    {}
-    
+
+ public:
+  enum { Flags = NeedToTranspose ? RowMajorBit : 0, CoeffReadCost = HugeCost };
+
+  class InnerIterator : public LhsIterator {
+   public:
+    InnerIterator(const sparse_dense_outer_product_evaluator& xprEval, Index outer)
+        : LhsIterator(xprEval.m_lhsXprImpl, 0),
+          m_outer(outer),
+          m_empty(false),
+          m_factor(get(xprEval.m_rhsXprImpl, outer, typename internal::traits<ActualRhs>::StorageKind())) {}
+
     EIGEN_STRONG_INLINE Index outer() const { return m_outer; }
-    EIGEN_STRONG_INLINE Index row()   const { return NeedToTranspose ? m_outer : LhsIterator::index(); }
-    EIGEN_STRONG_INLINE Index col()   const { return NeedToTranspose ? LhsIterator::index() : m_outer; }
+    EIGEN_STRONG_INLINE Index row() const { return NeedToTranspose ? m_outer : LhsIterator::index(); }
+    EIGEN_STRONG_INLINE Index col() const { return NeedToTranspose ? LhsIterator::index() : m_outer; }
 
     EIGEN_STRONG_INLINE Scalar value() const { return LhsIterator::value() * m_factor; }
     EIGEN_STRONG_INLINE operator bool() const { return LhsIterator::operator bool() && (!m_empty); }
-    
-  protected:
-    Scalar get(const RhsEval &rhs, Index outer, Dense = Dense()) const
-    {
-      return rhs.coeff(outer);
-    }
-    
-    Scalar get(const RhsEval &rhs, Index outer, Sparse = Sparse())
-    {
+
+   protected:
+    Scalar get(const RhsEval& rhs, Index outer, Dense = Dense()) const { return rhs.coeff(outer); }
+
+    Scalar get(const RhsEval& rhs, Index outer, Sparse = Sparse()) {
       typename RhsEval::InnerIterator it(rhs, outer);
-      if (it && it.index()==0 && it.value()!=Scalar(0))
-        return it.value();
+      if (it && it.index() == 0 && it.value() != Scalar(0)) return it.value();
       m_empty = true;
       return Scalar(0);
     }
-    
+
     Index m_outer;
     bool m_empty;
     Scalar m_factor;
   };
-  
-  sparse_dense_outer_product_evaluator(const Lhs1 &lhs, const ActualRhs &rhs)
-     : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs)
-  {
+
+  sparse_dense_outer_product_evaluator(const Lhs1& lhs, const ActualRhs& rhs)
+      : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-  
+
   // transpose case
-  sparse_dense_outer_product_evaluator(const ActualRhs &rhs, const Lhs1 &lhs)
-     : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs)
-  {
+  sparse_dense_outer_product_evaluator(const ActualRhs& rhs, const Lhs1& lhs)
+      : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs) {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-    
-protected:
+
+ protected:
   const LhsArg m_lhs;
   evaluator<ActualLhs> m_lhsXprImpl;
   evaluator<ActualRhs> m_rhsXprImpl;
 };
 
 // sparse * dense outer product
-template<typename Lhs, typename Rhs>
+template <typename Lhs, typename Rhs>
 struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, OuterProduct, SparseShape, DenseShape>
-  : sparse_dense_outer_product_evaluator<Lhs,Rhs, Lhs::IsRowMajor>
-{
-  typedef sparse_dense_outer_product_evaluator<Lhs,Rhs, Lhs::IsRowMajor> Base;
-  
+    : sparse_dense_outer_product_evaluator<Lhs, Rhs, Lhs::IsRowMajor> {
+  typedef sparse_dense_outer_product_evaluator<Lhs, Rhs, Lhs::IsRowMajor> Base;
+
   typedef Product<Lhs, Rhs> XprType;
   typedef typename XprType::PlainObject PlainObject;
 
-  explicit product_evaluator(const XprType& xpr)
-    : Base(xpr.lhs(), xpr.rhs())
-  {}
-  
+  explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs()) {}
 };
 
-template<typename Lhs, typename Rhs>
+template <typename Lhs, typename Rhs>
 struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, OuterProduct, DenseShape, SparseShape>
-  : sparse_dense_outer_product_evaluator<Lhs,Rhs, Rhs::IsRowMajor>
-{
-  typedef sparse_dense_outer_product_evaluator<Lhs,Rhs, Rhs::IsRowMajor> Base;
-  
+    : sparse_dense_outer_product_evaluator<Lhs, Rhs, Rhs::IsRowMajor> {
+  typedef sparse_dense_outer_product_evaluator<Lhs, Rhs, Rhs::IsRowMajor> Base;
+
   typedef Product<Lhs, Rhs> XprType;
   typedef typename XprType::PlainObject PlainObject;
 
-  explicit product_evaluator(const XprType& xpr)
-    : Base(xpr.lhs(), xpr.rhs())
-  {}
-  
+  explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs()) {}
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSEDENSEPRODUCT_H
+#endif  // EIGEN_SPARSEDENSEPRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h
index 941c03b..1f72a6b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h
@@ -10,7 +10,10 @@
 #ifndef EIGEN_SPARSE_DIAGONAL_PRODUCT_H
 #define EIGEN_SPARSE_DIAGONAL_PRODUCT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 // The product of a diagonal matrix with a sparse matrix can be easily
 // implemented using expression template.
@@ -26,113 +29,110 @@
 
 namespace internal {
 
-enum {
-  SDP_AsScalarProduct,
-  SDP_AsCwiseProduct
-};
-  
-template<typename SparseXprType, typename DiagonalCoeffType, int SDP_Tag>
+enum { SDP_AsScalarProduct, SDP_AsCwiseProduct };
+
+template <typename SparseXprType, typename DiagonalCoeffType, int SDP_Tag>
 struct sparse_diagonal_product_evaluator;
 
-template<typename Lhs, typename Rhs, int ProductTag>
+template <typename Lhs, typename Rhs, int ProductTag>
 struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, ProductTag, DiagonalShape, SparseShape>
-  : public sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType, Rhs::Flags&RowMajorBit?SDP_AsScalarProduct:SDP_AsCwiseProduct>
-{
+    : public sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType,
+                                               Rhs::Flags & RowMajorBit ? SDP_AsScalarProduct : SDP_AsCwiseProduct> {
   typedef Product<Lhs, Rhs, DefaultProduct> XprType;
-  enum { CoeffReadCost = HugeCost, Flags = Rhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags
-  
-  typedef sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType, Rhs::Flags&RowMajorBit?SDP_AsScalarProduct:SDP_AsCwiseProduct> Base;
-  explicit product_evaluator(const XprType& xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {}
+  enum { CoeffReadCost = HugeCost, Flags = Rhs::Flags & RowMajorBit, Alignment = 0 };  // FIXME CoeffReadCost & Flags
+
+  typedef sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType,
+                                            Rhs::Flags & RowMajorBit ? SDP_AsScalarProduct : SDP_AsCwiseProduct>
+      Base;
+  explicit product_evaluator(const XprType &xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {}
 };
 
-template<typename Lhs, typename Rhs, int ProductTag>
+template <typename Lhs, typename Rhs, int ProductTag>
 struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, ProductTag, SparseShape, DiagonalShape>
-  : public sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct>
-{
+    : public sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>,
+                                               Lhs::Flags & RowMajorBit ? SDP_AsCwiseProduct : SDP_AsScalarProduct> {
   typedef Product<Lhs, Rhs, DefaultProduct> XprType;
-  enum { CoeffReadCost = HugeCost, Flags = Lhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags
-  
-  typedef sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct> Base;
-  explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs().diagonal().transpose()) {}
+  enum { CoeffReadCost = HugeCost, Flags = Lhs::Flags & RowMajorBit, Alignment = 0 };  // FIXME CoeffReadCost & Flags
+
+  typedef sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>,
+                                            Lhs::Flags & RowMajorBit ? SDP_AsCwiseProduct : SDP_AsScalarProduct>
+      Base;
+  explicit product_evaluator(const XprType &xpr) : Base(xpr.lhs(), xpr.rhs().diagonal().transpose()) {}
 };
 
-template<typename SparseXprType, typename DiagonalCoeffType>
-struct sparse_diagonal_product_evaluator<SparseXprType, DiagonalCoeffType, SDP_AsScalarProduct>
-{
-protected:
+template <typename SparseXprType, typename DiagonalCoeffType>
+struct sparse_diagonal_product_evaluator<SparseXprType, DiagonalCoeffType, SDP_AsScalarProduct> {
+ protected:
   typedef typename evaluator<SparseXprType>::InnerIterator SparseXprInnerIterator;
   typedef typename SparseXprType::Scalar Scalar;
-  
-public:
-  class InnerIterator : public SparseXprInnerIterator
-  {
-  public:
+
+ public:
+  class InnerIterator : public SparseXprInnerIterator {
+   public:
     InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer)
-      : SparseXprInnerIterator(xprEval.m_sparseXprImpl, outer),
-        m_coeff(xprEval.m_diagCoeffImpl.coeff(outer))
-    {}
-    
+        : SparseXprInnerIterator(xprEval.m_sparseXprImpl, outer), m_coeff(xprEval.m_diagCoeffImpl.coeff(outer)) {}
+
     EIGEN_STRONG_INLINE Scalar value() const { return m_coeff * SparseXprInnerIterator::value(); }
-  protected:
+
+   protected:
     typename DiagonalCoeffType::Scalar m_coeff;
   };
-  
+
   sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagonalCoeffType &diagCoeff)
-    : m_sparseXprImpl(sparseXpr), m_diagCoeffImpl(diagCoeff)
-  {}
+      : m_sparseXprImpl(sparseXpr), m_diagCoeffImpl(diagCoeff) {}
 
   Index nonZerosEstimate() const { return m_sparseXprImpl.nonZerosEstimate(); }
-    
-protected:
+
+ protected:
   evaluator<SparseXprType> m_sparseXprImpl;
   evaluator<DiagonalCoeffType> m_diagCoeffImpl;
 };
 
-
-template<typename SparseXprType, typename DiagCoeffType>
-struct sparse_diagonal_product_evaluator<SparseXprType, DiagCoeffType, SDP_AsCwiseProduct>
-{
+template <typename SparseXprType, typename DiagCoeffType>
+struct sparse_diagonal_product_evaluator<SparseXprType, DiagCoeffType, SDP_AsCwiseProduct> {
   typedef typename SparseXprType::Scalar Scalar;
   typedef typename SparseXprType::StorageIndex StorageIndex;
-  
-  typedef typename nested_eval<DiagCoeffType,SparseXprType::IsRowMajor ? SparseXprType::RowsAtCompileTime
-                                                                       : SparseXprType::ColsAtCompileTime>::type DiagCoeffNested;
-  
-  class InnerIterator
-  {
+
+  typedef typename nested_eval<DiagCoeffType, SparseXprType::IsRowMajor ? SparseXprType::RowsAtCompileTime
+                                                                        : SparseXprType::ColsAtCompileTime>::type
+      DiagCoeffNested;
+
+  class InnerIterator {
     typedef typename evaluator<SparseXprType>::InnerIterator SparseXprIter;
-  public:
+
+   public:
     InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer)
-      : m_sparseIter(xprEval.m_sparseXprEval, outer), m_diagCoeffNested(xprEval.m_diagCoeffNested)
-    {}
-    
+        : m_sparseIter(xprEval.m_sparseXprEval, outer), m_diagCoeffNested(xprEval.m_diagCoeffNested) {}
+
     inline Scalar value() const { return m_sparseIter.value() * m_diagCoeffNested.coeff(index()); }
-    inline StorageIndex index() const  { return m_sparseIter.index(); }
-    inline Index outer() const  { return m_sparseIter.outer(); }
-    inline Index col() const    { return SparseXprType::IsRowMajor ? m_sparseIter.index() : m_sparseIter.outer(); }
-    inline Index row() const    { return SparseXprType::IsRowMajor ? m_sparseIter.outer() : m_sparseIter.index(); }
-    
-    EIGEN_STRONG_INLINE InnerIterator& operator++() { ++m_sparseIter; return *this; }
-    inline operator bool() const  { return m_sparseIter; }
-    
-  protected:
+    inline StorageIndex index() const { return m_sparseIter.index(); }
+    inline Index outer() const { return m_sparseIter.outer(); }
+    inline Index col() const { return SparseXprType::IsRowMajor ? m_sparseIter.index() : m_sparseIter.outer(); }
+    inline Index row() const { return SparseXprType::IsRowMajor ? m_sparseIter.outer() : m_sparseIter.index(); }
+
+    EIGEN_STRONG_INLINE InnerIterator &operator++() {
+      ++m_sparseIter;
+      return *this;
+    }
+    inline operator bool() const { return m_sparseIter; }
+
+   protected:
     SparseXprIter m_sparseIter;
     DiagCoeffNested m_diagCoeffNested;
   };
-  
+
   sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagCoeffType &diagCoeff)
-    : m_sparseXprEval(sparseXpr), m_diagCoeffNested(diagCoeff)
-  {}
+      : m_sparseXprEval(sparseXpr), m_diagCoeffNested(diagCoeff) {}
 
   Index nonZerosEstimate() const { return m_sparseXprEval.nonZerosEstimate(); }
-    
-protected:
+
+ protected:
   evaluator<SparseXprType> m_sparseXprEval;
   DiagCoeffNested m_diagCoeffNested;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_DIAGONAL_PRODUCT_H
+#endif  // EIGEN_SPARSE_DIAGONAL_PRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h
index 38bc4aa..aa876ec 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h
@@ -10,61 +10,60 @@
 #ifndef EIGEN_SPARSE_DOT_H
 #define EIGEN_SPARSE_DOT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-template<typename Derived>
-template<typename OtherDerived>
-typename internal::traits<Derived>::Scalar
-SparseMatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
-{
+namespace Eigen {
+
+template <typename Derived>
+template <typename OtherDerived>
+typename internal::traits<Derived>::Scalar SparseMatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
-  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
-    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived, OtherDerived)
+  EIGEN_STATIC_ASSERT(
+      (internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+      YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
 
   eigen_assert(size() == other.size());
-  eigen_assert(other.size()>0 && "you are using a non initialized vector");
+  eigen_assert(other.size() > 0 && "you are using a non initialized vector");
 
   internal::evaluator<Derived> thisEval(derived());
   typename internal::evaluator<Derived>::InnerIterator i(thisEval, 0);
   Scalar res(0);
-  while (i)
-  {
+  while (i) {
     res += numext::conj(i.value()) * other.coeff(i.index());
     ++i;
   }
   return res;
 }
 
-template<typename Derived>
-template<typename OtherDerived>
-typename internal::traits<Derived>::Scalar
-SparseMatrixBase<Derived>::dot(const SparseMatrixBase<OtherDerived>& other) const
-{
+template <typename Derived>
+template <typename OtherDerived>
+typename internal::traits<Derived>::Scalar SparseMatrixBase<Derived>::dot(
+    const SparseMatrixBase<OtherDerived>& other) const {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
-  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
-  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
-    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived, OtherDerived)
+  EIGEN_STATIC_ASSERT(
+      (internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+      YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
 
   eigen_assert(size() == other.size());
 
   internal::evaluator<Derived> thisEval(derived());
   typename internal::evaluator<Derived>::InnerIterator i(thisEval, 0);
-  
-  internal::evaluator<OtherDerived>  otherEval(other.derived());
+
+  internal::evaluator<OtherDerived> otherEval(other.derived());
   typename internal::evaluator<OtherDerived>::InnerIterator j(otherEval, 0);
 
   Scalar res(0);
-  while (i && j)
-  {
-    if (i.index()==j.index())
-    {
+  while (i && j) {
+    if (i.index() == j.index()) {
       res += numext::conj(i.value()) * j.value();
-      ++i; ++j;
-    }
-    else if (i.index()<j.index())
+      ++i;
+      ++j;
+    } else if (i.index() < j.index())
       ++i;
     else
       ++j;
@@ -72,27 +71,23 @@
   return res;
 }
 
-template<typename Derived>
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
-SparseMatrixBase<Derived>::squaredNorm() const
-{
+template <typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real SparseMatrixBase<Derived>::squaredNorm()
+    const {
   return numext::real((*this).cwiseAbs2().sum());
 }
 
-template<typename Derived>
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
-SparseMatrixBase<Derived>::norm() const
-{
+template <typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real SparseMatrixBase<Derived>::norm() const {
   using std::sqrt;
   return sqrt(squaredNorm());
 }
 
-template<typename Derived>
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
-SparseMatrixBase<Derived>::blueNorm() const
-{
+template <typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real SparseMatrixBase<Derived>::blueNorm()
+    const {
   return internal::blueNorm_impl(*this);
 }
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_DOT_H
+#endif  // EIGEN_SPARSE_DOT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h
index 7d47eb9..2285845 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h
@@ -10,20 +10,22 @@
 #ifndef EIGEN_SPARSE_FUZZY_H
 #define EIGEN_SPARSE_FUZZY_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
-  
-template<typename Derived>
-template<typename OtherDerived>
-bool SparseMatrixBase<Derived>::isApprox(const SparseMatrixBase<OtherDerived>& other, const RealScalar &prec) const
-{
-  const typename internal::nested_eval<Derived,2,PlainObject>::type actualA(derived());
-  typename internal::conditional<bool(IsRowMajor)==bool(OtherDerived::IsRowMajor),
-    const typename internal::nested_eval<OtherDerived,2,PlainObject>::type,
-    const PlainObject>::type actualB(other.derived());
+
+template <typename Derived>
+template <typename OtherDerived>
+bool SparseMatrixBase<Derived>::isApprox(const SparseMatrixBase<OtherDerived>& other, const RealScalar& prec) const {
+  const typename internal::nested_eval<Derived, 2, PlainObject>::type actualA(derived());
+  std::conditional_t<bool(IsRowMajor) == bool(OtherDerived::IsRowMajor),
+                     const typename internal::nested_eval<OtherDerived, 2, PlainObject>::type, const PlainObject>
+      actualB(other.derived());
 
   return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_FUZZY_H
+#endif  // EIGEN_SPARSE_FUZZY_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h
index f99be33..73e29c7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h
@@ -10,296 +10,286 @@
 #ifndef EIGEN_SPARSE_MAP_H
 #define EIGEN_SPARSE_MAP_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct traits<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
-{
-  typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct traits<Map<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> >
+    : public traits<SparseMatrix<MatScalar, MatOptions, MatIndex> > {
+  typedef SparseMatrix<MatScalar, MatOptions, MatIndex> PlainObjectType;
   typedef traits<PlainObjectType> TraitsBase;
-  enum {
-    Flags = TraitsBase::Flags & (~NestByRefBit)
-  };
+  enum { Flags = TraitsBase::Flags & (~NestByRefBit) };
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct traits<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
-{
-  typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct traits<Map<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> >
+    : public traits<SparseMatrix<MatScalar, MatOptions, MatIndex> > {
+  typedef SparseMatrix<MatScalar, MatOptions, MatIndex> PlainObjectType;
   typedef traits<PlainObjectType> TraitsBase;
-  enum {
-    Flags = TraitsBase::Flags & (~ (NestByRefBit | LvalueBit))
-  };
+  enum { Flags = TraitsBase::Flags & (~(NestByRefBit | LvalueBit)) };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename Derived,
-         int Level = internal::accessors_level<Derived>::has_write_access ? WriteAccessors : ReadOnlyAccessors
-> class SparseMapBase;
+template <typename Derived,
+          int Level = internal::accessors_level<Derived>::has_write_access ? WriteAccessors : ReadOnlyAccessors>
+class SparseMapBase;
 
 /** \ingroup SparseCore_Module
-  * class SparseMapBase
-  * \brief Common base class for Map and Ref instance of sparse matrix and vector.
-  */
-template<typename Derived>
-class SparseMapBase<Derived,ReadOnlyAccessors>
-  : public SparseCompressedBase<Derived>
-{
-  public:
-    typedef SparseCompressedBase<Derived> Base;
-    typedef typename Base::Scalar Scalar;
-    typedef typename Base::StorageIndex StorageIndex;
-    enum { IsRowMajor = Base::IsRowMajor };
-    using Base::operator=;
-  protected:
-    
-    typedef typename internal::conditional<
-                         bool(internal::is_lvalue<Derived>::value),
-                         Scalar *, const Scalar *>::type ScalarPointer;
-    typedef typename internal::conditional<
-                         bool(internal::is_lvalue<Derived>::value),
-                         StorageIndex *, const StorageIndex *>::type IndexPointer;
+ * class SparseMapBase
+ * \brief Common base class for Map and Ref instance of sparse matrix and vector.
+ */
+template <typename Derived>
+class SparseMapBase<Derived, ReadOnlyAccessors> : public SparseCompressedBase<Derived> {
+ public:
+  typedef SparseCompressedBase<Derived> Base;
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::StorageIndex StorageIndex;
+  enum { IsRowMajor = Base::IsRowMajor };
+  using Base::operator=;
 
-    Index   m_outerSize;
-    Index   m_innerSize;
-    Array<StorageIndex,2,1>  m_zero_nnz;
-    IndexPointer  m_outerIndex;
-    IndexPointer  m_innerIndices;
-    ScalarPointer m_values;
-    IndexPointer  m_innerNonZeros;
+ protected:
+  typedef std::conditional_t<bool(internal::is_lvalue<Derived>::value), Scalar*, const Scalar*> ScalarPointer;
+  typedef std::conditional_t<bool(internal::is_lvalue<Derived>::value), StorageIndex*, const StorageIndex*>
+      IndexPointer;
 
-  public:
+  Index m_outerSize;
+  Index m_innerSize;
+  Array<StorageIndex, 2, 1> m_zero_nnz;
+  IndexPointer m_outerIndex;
+  IndexPointer m_innerIndices;
+  ScalarPointer m_values;
+  IndexPointer m_innerNonZeros;
 
-    /** \copydoc SparseMatrixBase::rows() */
-    inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
-    /** \copydoc SparseMatrixBase::cols() */
-    inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
-    /** \copydoc SparseMatrixBase::innerSize() */
-    inline Index innerSize() const { return m_innerSize; }
-    /** \copydoc SparseMatrixBase::outerSize() */
-    inline Index outerSize() const { return m_outerSize; }
-    /** \copydoc SparseCompressedBase::nonZeros */
-    inline Index nonZeros() const { return m_zero_nnz[1]; }
-    
-    /** \copydoc SparseCompressedBase::isCompressed */
-    bool isCompressed() const { return m_innerNonZeros==0; }
+ public:
+  /** \copydoc SparseMatrixBase::rows() */
+  inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+  /** \copydoc SparseMatrixBase::cols() */
+  inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+  /** \copydoc SparseMatrixBase::innerSize() */
+  inline Index innerSize() const { return m_innerSize; }
+  /** \copydoc SparseMatrixBase::outerSize() */
+  inline Index outerSize() const { return m_outerSize; }
+  /** \copydoc SparseCompressedBase::nonZeros */
+  inline Index nonZeros() const { return m_zero_nnz[1]; }
 
-    //----------------------------------------
-    // direct access interface
-    /** \copydoc SparseMatrix::valuePtr */
-    inline const Scalar* valuePtr() const { return m_values; }
-    /** \copydoc SparseMatrix::innerIndexPtr */
-    inline const StorageIndex* innerIndexPtr() const { return m_innerIndices; }
-    /** \copydoc SparseMatrix::outerIndexPtr */
-    inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
-    /** \copydoc SparseMatrix::innerNonZeroPtr */
-    inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
-    //----------------------------------------
+  /** \copydoc SparseCompressedBase::isCompressed */
+  bool isCompressed() const { return m_innerNonZeros == 0; }
 
-    /** \copydoc SparseMatrix::coeff */
-    inline Scalar coeff(Index row, Index col) const
-    {
-      const Index outer = IsRowMajor ? row : col;
-      const Index inner = IsRowMajor ? col : row;
+  //----------------------------------------
+  // direct access interface
+  /** \copydoc SparseMatrix::valuePtr */
+  inline const Scalar* valuePtr() const { return m_values; }
+  /** \copydoc SparseMatrix::innerIndexPtr */
+  inline const StorageIndex* innerIndexPtr() const { return m_innerIndices; }
+  /** \copydoc SparseMatrix::outerIndexPtr */
+  inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
+  /** \copydoc SparseMatrix::innerNonZeroPtr */
+  inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
+  //----------------------------------------
 
-      Index start = m_outerIndex[outer];
-      Index end = isCompressed() ? m_outerIndex[outer+1] : start + m_innerNonZeros[outer];
-      if (start==end)
-        return Scalar(0);
-      else if (end>0 && inner==m_innerIndices[end-1])
-        return m_values[end-1];
-      // ^^  optimization: let's first check if it is the last coefficient
-      // (very common in high level algorithms)
+  /** \copydoc SparseMatrix::coeff */
+  inline Scalar coeff(Index row, Index col) const {
+    const Index outer = IsRowMajor ? row : col;
+    const Index inner = IsRowMajor ? col : row;
 
-      const StorageIndex* r = std::lower_bound(&m_innerIndices[start],&m_innerIndices[end-1],inner);
-      const Index id = r-&m_innerIndices[0];
-      return ((*r==inner) && (id<end)) ? m_values[id] : Scalar(0);
-    }
+    Index start = m_outerIndex[outer];
+    Index end = isCompressed() ? m_outerIndex[outer + 1] : start + m_innerNonZeros[outer];
+    if (start == end)
+      return Scalar(0);
+    else if (end > 0 && inner == m_innerIndices[end - 1])
+      return m_values[end - 1];
+    // ^^  optimization: let's first check if it is the last coefficient
+    // (very common in high level algorithms)
 
-    inline SparseMapBase(Index rows, Index cols, Index nnz, IndexPointer outerIndexPtr, IndexPointer innerIndexPtr,
-                              ScalarPointer valuePtr, IndexPointer innerNonZerosPtr = 0)
-      : m_outerSize(IsRowMajor?rows:cols), m_innerSize(IsRowMajor?cols:rows), m_zero_nnz(0,internal::convert_index<StorageIndex>(nnz)), m_outerIndex(outerIndexPtr),
-        m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(innerNonZerosPtr)
-    {}
+    const StorageIndex* r = std::lower_bound(&m_innerIndices[start], &m_innerIndices[end - 1], inner);
+    const Index id = r - &m_innerIndices[0];
+    return ((*r == inner) && (id < end)) ? m_values[id] : Scalar(0);
+  }
 
-    // for vectors
-    inline SparseMapBase(Index size, Index nnz, IndexPointer innerIndexPtr, ScalarPointer valuePtr)
-      : m_outerSize(1), m_innerSize(size), m_zero_nnz(0,internal::convert_index<StorageIndex>(nnz)), m_outerIndex(m_zero_nnz.data()),
-        m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(0)
-    {}
+  inline SparseMapBase(Index rows, Index cols, Index nnz, IndexPointer outerIndexPtr, IndexPointer innerIndexPtr,
+                       ScalarPointer valuePtr, IndexPointer innerNonZerosPtr = 0)
+      : m_outerSize(IsRowMajor ? rows : cols),
+        m_innerSize(IsRowMajor ? cols : rows),
+        m_zero_nnz(0, internal::convert_index<StorageIndex>(nnz)),
+        m_outerIndex(outerIndexPtr),
+        m_innerIndices(innerIndexPtr),
+        m_values(valuePtr),
+        m_innerNonZeros(innerNonZerosPtr) {}
 
-    /** Empty destructor */
-    inline ~SparseMapBase() {}
+  // for vectors
+  inline SparseMapBase(Index size, Index nnz, IndexPointer innerIndexPtr, ScalarPointer valuePtr)
+      : m_outerSize(1),
+        m_innerSize(size),
+        m_zero_nnz(0, internal::convert_index<StorageIndex>(nnz)),
+        m_outerIndex(m_zero_nnz.data()),
+        m_innerIndices(innerIndexPtr),
+        m_values(valuePtr),
+        m_innerNonZeros(0) {}
 
-  protected:
-    inline SparseMapBase() {}
+  /** Empty destructor */
+  inline ~SparseMapBase() {}
+
+ protected:
+  inline SparseMapBase() {}
 };
 
 /** \ingroup SparseCore_Module
-  * class SparseMapBase
-  * \brief Common base class for writable Map and Ref instance of sparse matrix and vector.
-  */
-template<typename Derived>
-class SparseMapBase<Derived,WriteAccessors>
-  : public SparseMapBase<Derived,ReadOnlyAccessors>
-{
-    typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
-    
-  public:
-    typedef SparseMapBase<Derived, ReadOnlyAccessors> Base;
-    typedef typename Base::Scalar Scalar;
-    typedef typename Base::StorageIndex StorageIndex;
-    enum { IsRowMajor = Base::IsRowMajor };
-    
-    using Base::operator=;
+ * class SparseMapBase
+ * \brief Common base class for writable Map and Ref instance of sparse matrix and vector.
+ */
+template <typename Derived>
+class SparseMapBase<Derived, WriteAccessors> : public SparseMapBase<Derived, ReadOnlyAccessors> {
+  typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
 
-  public:
-    
-    //----------------------------------------
-    // direct access interface
-    using Base::valuePtr;
-    using Base::innerIndexPtr;
-    using Base::outerIndexPtr;
-    using Base::innerNonZeroPtr;
-    /** \copydoc SparseMatrix::valuePtr */
-    inline Scalar* valuePtr()              { return Base::m_values; }
-    /** \copydoc SparseMatrix::innerIndexPtr */
-    inline StorageIndex* innerIndexPtr()   { return Base::m_innerIndices; }
-    /** \copydoc SparseMatrix::outerIndexPtr */
-    inline StorageIndex* outerIndexPtr()   { return Base::m_outerIndex; }
-    /** \copydoc SparseMatrix::innerNonZeroPtr */
-    inline StorageIndex* innerNonZeroPtr() { return Base::m_innerNonZeros; }
-    //----------------------------------------
+ public:
+  typedef SparseMapBase<Derived, ReadOnlyAccessors> Base;
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::StorageIndex StorageIndex;
+  enum { IsRowMajor = Base::IsRowMajor };
 
-    /** \copydoc SparseMatrix::coeffRef */
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      const Index outer = IsRowMajor ? row : col;
-      const Index inner = IsRowMajor ? col : row;
+  using Base::operator=;
 
-      Index start = Base::m_outerIndex[outer];
-      Index end = Base::isCompressed() ? Base::m_outerIndex[outer+1] : start + Base::m_innerNonZeros[outer];
-      eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
-      eigen_assert(end>start && "coeffRef cannot be called on a zero coefficient");
-      StorageIndex* r = std::lower_bound(&Base::m_innerIndices[start],&Base::m_innerIndices[end],inner);
-      const Index id = r - &Base::m_innerIndices[0];
-      eigen_assert((*r==inner) && (id<end) && "coeffRef cannot be called on a zero coefficient");
-      return const_cast<Scalar*>(Base::m_values)[id];
-    }
-    
-    inline SparseMapBase(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr,
-                         Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
-      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
-    {}
+ public:
+  //----------------------------------------
+  // direct access interface
+  using Base::innerIndexPtr;
+  using Base::innerNonZeroPtr;
+  using Base::outerIndexPtr;
+  using Base::valuePtr;
+  /** \copydoc SparseMatrix::valuePtr */
+  inline Scalar* valuePtr() { return Base::m_values; }
+  /** \copydoc SparseMatrix::innerIndexPtr */
+  inline StorageIndex* innerIndexPtr() { return Base::m_innerIndices; }
+  /** \copydoc SparseMatrix::outerIndexPtr */
+  inline StorageIndex* outerIndexPtr() { return Base::m_outerIndex; }
+  /** \copydoc SparseMatrix::innerNonZeroPtr */
+  inline StorageIndex* innerNonZeroPtr() { return Base::m_innerNonZeros; }
+  //----------------------------------------
 
-    // for vectors
-    inline SparseMapBase(Index size, Index nnz, StorageIndex* innerIndexPtr, Scalar* valuePtr)
-      : Base(size, nnz, innerIndexPtr, valuePtr)
-    {}
+  /** \copydoc SparseMatrix::coeffRef */
+  inline Scalar& coeffRef(Index row, Index col) {
+    const Index outer = IsRowMajor ? row : col;
+    const Index inner = IsRowMajor ? col : row;
 
-    /** Empty destructor */
-    inline ~SparseMapBase() {}
+    Index start = Base::m_outerIndex[outer];
+    Index end = Base::isCompressed() ? Base::m_outerIndex[outer + 1] : start + Base::m_innerNonZeros[outer];
+    eigen_assert(end >= start && "you probably called coeffRef on a non finalized matrix");
+    eigen_assert(end > start && "coeffRef cannot be called on a zero coefficient");
+    StorageIndex* r = std::lower_bound(&Base::m_innerIndices[start], &Base::m_innerIndices[end], inner);
+    const Index id = r - &Base::m_innerIndices[0];
+    eigen_assert((*r == inner) && (id < end) && "coeffRef cannot be called on a zero coefficient");
+    return const_cast<Scalar*>(Base::m_values)[id];
+  }
 
-  protected:
-    inline SparseMapBase() {}
+  inline SparseMapBase(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr,
+                       Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
+      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr) {}
+
+  // for vectors
+  inline SparseMapBase(Index size, Index nnz, StorageIndex* innerIndexPtr, Scalar* valuePtr)
+      : Base(size, nnz, innerIndexPtr, valuePtr) {}
+
+  /** Empty destructor */
+  inline ~SparseMapBase() {}
+
+ protected:
+  inline SparseMapBase() {}
 };
 
 /** \ingroup SparseCore_Module
-  *
-  * \brief Specialization of class Map for SparseMatrix-like storage.
-  *
-  * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix.
-  *
-  * \sa class Map, class SparseMatrix, class Ref<SparseMatrixType,Options>
-  */
+ *
+ * \brief Specialization of class Map for SparseMatrix-like storage.
+ *
+ * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of
+ * class SparseMatrix.
+ *
+ * \sa class Map, class SparseMatrix, class Ref<SparseMatrixType,Options>
+ */
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-class Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
-  : public SparseMapBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Map<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>
+    : public SparseMapBase<Map<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> >
 #else
-template<typename SparseMatrixType>
-class Map<SparseMatrixType>
-  : public SparseMapBase<Derived,WriteAccessors>
+template <typename SparseMatrixType>
+class Map<SparseMatrixType> : public SparseMapBase<Derived, WriteAccessors>
 #endif
 {
-  public:
-    typedef SparseMapBase<Map> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
-    enum { IsRowMajor = Base::IsRowMajor };
+ public:
+  typedef SparseMapBase<Map> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
+  enum { IsRowMajor = Base::IsRowMajor };
 
-  public:
-
-    /** Constructs a read-write Map to a sparse matrix of size \a rows x \a cols, containing \a nnz non-zero coefficients,
-      * stored as a sparse format as defined by the pointers \a outerIndexPtr, \a innerIndexPtr, and \a valuePtr.
-      * If the optional parameter \a innerNonZerosPtr is the null pointer, then a standard compressed format is assumed.
-      *
-      * This constructor is available only if \c SparseMatrixType is non-const.
-      *
-      * More details on the expected storage schemes are given in the \ref TutorialSparse "manual pages".
-      */
-    inline Map(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr,
-               StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
-      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
-    {}
+ public:
+  /** Constructs a read-write Map to a sparse matrix of size \a rows x \a cols, containing \a nnz non-zero coefficients,
+   * stored as a sparse format as defined by the pointers \a outerIndexPtr, \a innerIndexPtr, and \a valuePtr.
+   * If the optional parameter \a innerNonZerosPtr is the null pointer, then a standard compressed format is assumed.
+   * The inner indices must be sorted appropriately.
+   *
+   * This constructor is available only if \c SparseMatrixType is non-const.
+   *
+   * More details on the expected storage schemes are given in the \ref TutorialSparse "manual pages".
+   */
+  inline Map(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr,
+             Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
+      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr) {}
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** Empty destructor */
-    inline ~Map() {}
+  /** Empty destructor */
+  inline ~Map() {}
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-class Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
-  : public SparseMapBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-{
-  public:
-    typedef SparseMapBase<Map> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
-    enum { IsRowMajor = Base::IsRowMajor };
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Map<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>
+    : public SparseMapBase<Map<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> > {
+ public:
+  typedef SparseMapBase<Map> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
+  enum { IsRowMajor = Base::IsRowMajor };
 
-  public:
+ public:
 #endif
-    /** This is the const version of the above constructor.
-      *
-      * This constructor is available only if \c SparseMatrixType is const, e.g.:
-      * \code Map<const SparseMatrix<double> >  \endcode
-      */
-    inline Map(Index rows, Index cols, Index nnz, const StorageIndex* outerIndexPtr,
-               const StorageIndex* innerIndexPtr, const Scalar* valuePtr, const StorageIndex* innerNonZerosPtr = 0)
-      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
-    {}
+  /** This is the const version of the above constructor.
+   *
+   * This constructor is available only if \c SparseMatrixType is const, e.g.:
+   * \code Map<const SparseMatrix<double> >  \endcode
+   */
+  inline Map(Index rows, Index cols, Index nnz, const StorageIndex* outerIndexPtr, const StorageIndex* innerIndexPtr,
+             const Scalar* valuePtr, const StorageIndex* innerNonZerosPtr = 0)
+      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr) {}
 
-    /** Empty destructor */
-    inline ~Map() {}
+  /** Empty destructor */
+  inline ~Map() {}
 };
 
 namespace internal {
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct evaluator<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
-{
-  typedef evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
-  typedef Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Map<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> >
+    : evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> > > {
+  typedef evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> > >
+      Base;
+  typedef Map<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> XprType;
   evaluator() : Base() {}
-  explicit evaluator(const XprType &mat) : Base(mat) {}
+  explicit evaluator(const XprType& mat) : Base(mat) {}
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct evaluator<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : evaluator<SparseCompressedBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
-{
-  typedef evaluator<SparseCompressedBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
-  typedef Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Map<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> >
+    : evaluator<SparseCompressedBase<Map<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> > > {
+  typedef evaluator<
+      SparseCompressedBase<Map<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> > >
+      Base;
+  typedef Map<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> XprType;
   evaluator() : Base() {}
-  explicit evaluator(const XprType &mat) : Base(mat) {}
+  explicit evaluator(const XprType& mat) : Base(mat) {}
 };
 
-}
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_MAP_H
+#endif  // EIGEN_SPARSE_MAP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h
index 616b4a0..19dd40c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h
@@ -10,44 +10,47 @@
 #ifndef EIGEN_SPARSEMATRIX_H
 #define EIGEN_SPARSEMATRIX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \ingroup SparseCore_Module
-  *
-  * \class SparseMatrix
-  *
-  * \brief A versatible sparse matrix representation
-  *
-  * This class implements a more versatile variants of the common \em compressed row/column storage format.
-  * Each colmun's (resp. row) non zeros are stored as a pair of value with associated row (resp. colmiun) index.
-  * All the non zeros are stored in a single large buffer. Unlike the \em compressed format, there might be extra
-  * space in between the nonzeros of two successive colmuns (resp. rows) such that insertion of new non-zero
-  * can be done with limited memory reallocation and copies.
-  *
-  * A call to the function makeCompressed() turns the matrix into the standard \em compressed format
-  * compatible with many library.
-  *
-  * More details on this storage sceheme are given in the \ref TutorialSparse "manual pages".
-  *
-  * \tparam _Scalar the scalar type, i.e. the type of the coefficients
-  * \tparam _Options Union of bit flags controlling the storage scheme. Currently the only possibility
-  *                 is ColMajor or RowMajor. The default is 0 which means column-major.
-  * \tparam _StorageIndex the type of the indices. It has to be a \b signed type (e.g., short, int, std::ptrdiff_t). Default is \c int.
-  *
-  * \warning In %Eigen 3.2, the undocumented type \c SparseMatrix::Index was improperly defined as the storage index type (e.g., int),
-  *          whereas it is now (starting from %Eigen 3.3) deprecated and always defined as Eigen::Index.
-  *          Codes making use of \c SparseMatrix::Index, might thus likely have to be changed to use \c SparseMatrix::StorageIndex instead.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIX_PLUGIN.
-  */
+ *
+ * \class SparseMatrix
+ *
+ * \brief A versatible sparse matrix representation
+ *
+ * This class implements a more versatile variants of the common \em compressed row/column storage format.
+ * Each colmun's (resp. row) non zeros are stored as a pair of value with associated row (resp. colmiun) index.
+ * All the non zeros are stored in a single large buffer. Unlike the \em compressed format, there might be extra
+ * space in between the nonzeros of two successive colmuns (resp. rows) such that insertion of new non-zero
+ * can be done with limited memory reallocation and copies.
+ *
+ * A call to the function makeCompressed() turns the matrix into the standard \em compressed format
+ * compatible with many library.
+ *
+ * More details on this storage sceheme are given in the \ref TutorialSparse "manual pages".
+ *
+ * \tparam Scalar_ the scalar type, i.e. the type of the coefficients
+ * \tparam Options_ Union of bit flags controlling the storage scheme. Currently the only possibility
+ *                 is ColMajor or RowMajor. The default is 0 which means column-major.
+ * \tparam StorageIndex_ the type of the indices. It has to be a \b signed type (e.g., short, int, std::ptrdiff_t).
+ * Default is \c int.
+ *
+ * \warning In %Eigen 3.2, the undocumented type \c SparseMatrix::Index was improperly defined as the storage index type
+ * (e.g., int), whereas it is now (starting from %Eigen 3.3) deprecated and always defined as Eigen::Index. Codes making
+ * use of \c SparseMatrix::Index, might thus likely have to be changed to use \c SparseMatrix::StorageIndex instead.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIX_PLUGIN.
+ */
 
 namespace internal {
-template<typename _Scalar, int _Options, typename _StorageIndex>
-struct traits<SparseMatrix<_Scalar, _Options, _StorageIndex> >
-{
-  typedef _Scalar Scalar;
-  typedef _StorageIndex StorageIndex;
+template <typename Scalar_, int Options_, typename StorageIndex_>
+struct traits<SparseMatrix<Scalar_, Options_, StorageIndex_>> {
+  typedef Scalar_ Scalar;
+  typedef StorageIndex_ StorageIndex;
   typedef Sparse StorageKind;
   typedef MatrixXpr XprKind;
   enum {
@@ -55,21 +58,20 @@
     ColsAtCompileTime = Dynamic,
     MaxRowsAtCompileTime = Dynamic,
     MaxColsAtCompileTime = Dynamic,
-    Flags = _Options | NestByRefBit | LvalueBit | CompressedAccessBit,
+    Flags = Options_ | NestByRefBit | LvalueBit | CompressedAccessBit,
     SupportedAccessPatterns = InnerRandomAccessPattern
   };
 };
 
-template<typename _Scalar, int _Options, typename _StorageIndex, int DiagIndex>
-struct traits<Diagonal<SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
-{
-  typedef SparseMatrix<_Scalar, _Options, _StorageIndex> MatrixType;
+template <typename Scalar_, int Options_, typename StorageIndex_, int DiagIndex>
+struct traits<Diagonal<SparseMatrix<Scalar_, Options_, StorageIndex_>, DiagIndex>> {
+  typedef SparseMatrix<Scalar_, Options_, StorageIndex_> MatrixType;
   typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested;
+  typedef std::remove_reference_t<MatrixTypeNested> MatrixTypeNested_;
 
-  typedef _Scalar Scalar;
+  typedef Scalar_ Scalar;
   typedef Dense StorageKind;
-  typedef _StorageIndex StorageIndex;
+  typedef StorageIndex_ StorageIndex;
   typedef MatrixXpr XprKind;
 
   enum {
@@ -81,947 +83,989 @@
   };
 };
 
-template<typename _Scalar, int _Options, typename _StorageIndex, int DiagIndex>
-struct traits<Diagonal<const SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
- : public traits<Diagonal<SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
-{
-  enum {
-    Flags = 0
-  };
+template <typename Scalar_, int Options_, typename StorageIndex_, int DiagIndex>
+struct traits<Diagonal<const SparseMatrix<Scalar_, Options_, StorageIndex_>, DiagIndex>>
+    : public traits<Diagonal<SparseMatrix<Scalar_, Options_, StorageIndex_>, DiagIndex>> {
+  enum { Flags = 0 };
 };
 
-} // end namespace internal
+template <typename StorageIndex>
+struct sparse_reserve_op {
+  EIGEN_DEVICE_FUNC sparse_reserve_op(Index begin, Index end, Index size) {
+    Index range = numext::mini(end - begin, size);
+    m_begin = begin;
+    m_end = begin + range;
+    m_val = StorageIndex(size / range);
+    m_remainder = StorageIndex(size % range);
+  }
+  template <typename IndexType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE StorageIndex operator()(IndexType i) const {
+    if ((i >= m_begin) && (i < m_end))
+      return m_val + ((i - m_begin) < m_remainder ? 1 : 0);
+    else
+      return 0;
+  }
+  StorageIndex m_val, m_remainder;
+  Index m_begin, m_end;
+};
 
-template<typename _Scalar, int _Options, typename _StorageIndex>
-class SparseMatrix
-  : public SparseCompressedBase<SparseMatrix<_Scalar, _Options, _StorageIndex> >
-{
-    typedef SparseCompressedBase<SparseMatrix> Base;
-    using Base::convert_index;
-    friend class SparseVector<_Scalar,0,_StorageIndex>;
-    template<typename, typename, typename, typename, typename>
-    friend struct internal::Assignment;
-  public:
-    using Base::isCompressed;
-    using Base::nonZeros;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(SparseMatrix)
-    using Base::operator+=;
-    using Base::operator-=;
+template <typename Scalar>
+struct functor_traits<sparse_reserve_op<Scalar>> {
+  enum { Cost = 1, PacketAccess = false, IsRepeatable = true };
+};
 
-    typedef MappedSparseMatrix<Scalar,Flags> Map;
-    typedef Diagonal<SparseMatrix> DiagonalReturnType;
-    typedef Diagonal<const SparseMatrix> ConstDiagonalReturnType;
-    typedef typename Base::InnerIterator InnerIterator;
-    typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
-    
+}  // end namespace internal
 
-    using Base::IsRowMajor;
-    typedef internal::CompressedStorage<Scalar,StorageIndex> Storage;
-    enum {
-      Options = _Options
-    };
+template <typename Scalar_, int Options_, typename StorageIndex_>
+class SparseMatrix : public SparseCompressedBase<SparseMatrix<Scalar_, Options_, StorageIndex_>> {
+  typedef SparseCompressedBase<SparseMatrix> Base;
+  using Base::convert_index;
+  friend class SparseVector<Scalar_, 0, StorageIndex_>;
+  template <typename, typename, typename, typename, typename>
+  friend struct internal::Assignment;
 
-    typedef typename Base::IndexVector IndexVector;
-    typedef typename Base::ScalarVector ScalarVector;
-  protected:
-    typedef SparseMatrix<Scalar,(Flags&~RowMajorBit)|(IsRowMajor?RowMajorBit:0)> TransposedSparseMatrix;
+ public:
+  using Base::isCompressed;
+  using Base::nonZeros;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(SparseMatrix)
+  using Base::operator+=;
+  using Base::operator-=;
 
-    Index m_outerSize;
-    Index m_innerSize;
-    StorageIndex* m_outerIndex;
-    StorageIndex* m_innerNonZeros;     // optional, if null then the data is compressed
-    Storage m_data;
+  typedef Eigen::Map<SparseMatrix<Scalar, Options_, StorageIndex>> Map;
+  typedef Diagonal<SparseMatrix> DiagonalReturnType;
+  typedef Diagonal<const SparseMatrix> ConstDiagonalReturnType;
+  typedef typename Base::InnerIterator InnerIterator;
+  typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
 
-  public:
-    
-    /** \returns the number of rows of the matrix */
-    inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
-    /** \returns the number of columns of the matrix */
-    inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+  using Base::IsRowMajor;
+  typedef internal::CompressedStorage<Scalar, StorageIndex> Storage;
+  enum { Options = Options_ };
 
-    /** \returns the number of rows (resp. columns) of the matrix if the storage order column major (resp. row major) */
-    inline Index innerSize() const { return m_innerSize; }
-    /** \returns the number of columns (resp. rows) of the matrix if the storage order column major (resp. row major) */
-    inline Index outerSize() const { return m_outerSize; }
-    
-    /** \returns a const pointer to the array of values.
-      * This function is aimed at interoperability with other libraries.
-      * \sa innerIndexPtr(), outerIndexPtr() */
-    inline const Scalar* valuePtr() const { return m_data.valuePtr(); }
-    /** \returns a non-const pointer to the array of values.
-      * This function is aimed at interoperability with other libraries.
-      * \sa innerIndexPtr(), outerIndexPtr() */
-    inline Scalar* valuePtr() { return m_data.valuePtr(); }
+  typedef typename Base::IndexVector IndexVector;
+  typedef typename Base::ScalarVector ScalarVector;
 
-    /** \returns a const pointer to the array of inner indices.
-      * This function is aimed at interoperability with other libraries.
-      * \sa valuePtr(), outerIndexPtr() */
-    inline const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
-    /** \returns a non-const pointer to the array of inner indices.
-      * This function is aimed at interoperability with other libraries.
-      * \sa valuePtr(), outerIndexPtr() */
-    inline StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
+ protected:
+  typedef SparseMatrix<Scalar, IsRowMajor ? ColMajor : RowMajor, StorageIndex> TransposedSparseMatrix;
 
-    /** \returns a const pointer to the array of the starting positions of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \sa valuePtr(), innerIndexPtr() */
-    inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
-    /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \sa valuePtr(), innerIndexPtr() */
-    inline StorageIndex* outerIndexPtr() { return m_outerIndex; }
+  Index m_outerSize;
+  Index m_innerSize;
+  StorageIndex* m_outerIndex;
+  StorageIndex* m_innerNonZeros;  // optional, if null then the data is compressed
+  Storage m_data;
 
-    /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \warning it returns the null pointer 0 in compressed mode */
-    inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
-    /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
-      * This function is aimed at interoperability with other libraries.
-      * \warning it returns the null pointer 0 in compressed mode */
-    inline StorageIndex* innerNonZeroPtr() { return m_innerNonZeros; }
+ public:
+  /** \returns the number of rows of the matrix */
+  inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+  /** \returns the number of columns of the matrix */
+  inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
 
-    /** \internal */
-    inline Storage& data() { return m_data; }
-    /** \internal */
-    inline const Storage& data() const { return m_data; }
+  /** \returns the number of rows (resp. columns) of the matrix if the storage order column major (resp. row major) */
+  inline Index innerSize() const { return m_innerSize; }
+  /** \returns the number of columns (resp. rows) of the matrix if the storage order column major (resp. row major) */
+  inline Index outerSize() const { return m_outerSize; }
 
-    /** \returns the value of the matrix at position \a i, \a j
-      * This function returns Scalar(0) if the element is an explicit \em zero */
-    inline Scalar coeff(Index row, Index col) const
-    {
-      eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
-      
-      const Index outer = IsRowMajor ? row : col;
-      const Index inner = IsRowMajor ? col : row;
-      Index end = m_innerNonZeros ? m_outerIndex[outer] + m_innerNonZeros[outer] : m_outerIndex[outer+1];
-      return m_data.atInRange(m_outerIndex[outer], end, StorageIndex(inner));
-    }
+  /** \returns a const pointer to the array of values.
+   * This function is aimed at interoperability with other libraries.
+   * \sa innerIndexPtr(), outerIndexPtr() */
+  inline const Scalar* valuePtr() const { return m_data.valuePtr(); }
+  /** \returns a non-const pointer to the array of values.
+   * This function is aimed at interoperability with other libraries.
+   * \sa innerIndexPtr(), outerIndexPtr() */
+  inline Scalar* valuePtr() { return m_data.valuePtr(); }
 
-    /** \returns a non-const reference to the value of the matrix at position \a i, \a j
-      *
-      * If the element does not exist then it is inserted via the insert(Index,Index) function
-      * which itself turns the matrix into a non compressed form if that was not the case.
-      *
-      * This is a O(log(nnz_j)) operation (binary search) plus the cost of insert(Index,Index)
-      * function if the element does not already exist.
-      */
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
-      
-      const Index outer = IsRowMajor ? row : col;
-      const Index inner = IsRowMajor ? col : row;
+  /** \returns a const pointer to the array of inner indices.
+   * This function is aimed at interoperability with other libraries.
+   * \sa valuePtr(), outerIndexPtr() */
+  inline const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
+  /** \returns a non-const pointer to the array of inner indices.
+   * This function is aimed at interoperability with other libraries.
+   * \sa valuePtr(), outerIndexPtr() */
+  inline StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
 
-      Index start = m_outerIndex[outer];
-      Index end = m_innerNonZeros ? m_outerIndex[outer] + m_innerNonZeros[outer] : m_outerIndex[outer+1];
-      eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
-      if(end<=start)
-        return insert(row,col);
-      const Index p = m_data.searchLowerIndex(start,end-1,StorageIndex(inner));
-      if((p<end) && (m_data.index(p)==inner))
-        return m_data.value(p);
-      else
-        return insert(row,col);
-    }
+  /** \returns a const pointer to the array of the starting positions of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \sa valuePtr(), innerIndexPtr() */
+  inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
+  /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \sa valuePtr(), innerIndexPtr() */
+  inline StorageIndex* outerIndexPtr() { return m_outerIndex; }
 
-    /** \returns a reference to a novel non zero coefficient with coordinates \a row x \a col.
-      * The non zero coefficient must \b not already exist.
-      *
-      * If the matrix \c *this is in compressed mode, then \c *this is turned into uncompressed
-      * mode while reserving room for 2 x this->innerSize() non zeros if reserve(Index) has not been called earlier.
-      * In this case, the insertion procedure is optimized for a \e sequential insertion mode where elements are assumed to be
-      * inserted by increasing outer-indices.
-      * 
-      * If that's not the case, then it is strongly recommended to either use a triplet-list to assemble the matrix, or to first
-      * call reserve(const SizesType &) to reserve the appropriate number of non-zero elements per inner vector.
-      *
-      * Assuming memory has been appropriately reserved, this function performs a sorted insertion in O(1)
-      * if the elements of each inner vector are inserted in increasing inner index order, and in O(nnz_j) for a random insertion.
-      *
-      */
-    Scalar& insert(Index row, Index col);
+  /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \warning it returns the null pointer 0 in compressed mode */
+  inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
+  /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
+   * This function is aimed at interoperability with other libraries.
+   * \warning it returns the null pointer 0 in compressed mode */
+  inline StorageIndex* innerNonZeroPtr() { return m_innerNonZeros; }
 
-  public:
+  /** \internal */
+  inline Storage& data() { return m_data; }
+  /** \internal */
+  inline const Storage& data() const { return m_data; }
 
-    /** Removes all non zeros but keep allocated memory
-      *
-      * This function does not free the currently allocated memory. To release as much as memory as possible,
-      * call \code mat.data().squeeze(); \endcode after resizing it.
-      * 
-      * \sa resize(Index,Index), data()
-      */
-    inline void setZero()
-    {
-      m_data.clear();
-      memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex));
-      if(m_innerNonZeros)
-        memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex));
-    }
+  /** \returns the value of the matrix at position \a i, \a j
+   * This function returns Scalar(0) if the element is an explicit \em zero */
+  inline Scalar coeff(Index row, Index col) const {
+    eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
 
-    /** Preallocates \a reserveSize non zeros.
-      *
-      * Precondition: the matrix must be in compressed mode. */
-    inline void reserve(Index reserveSize)
-    {
-      eigen_assert(isCompressed() && "This function does not make sense in non compressed mode.");
-      m_data.reserve(reserveSize);
-    }
-    
-    #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** Preallocates \a reserveSize[\c j] non zeros for each column (resp. row) \c j.
-      *
-      * This function turns the matrix in non-compressed mode.
-      * 
-      * The type \c SizesType must expose the following interface:
-        \code
-        typedef value_type;
-        const value_type& operator[](i) const;
-        \endcode
-      * for \c i in the [0,this->outerSize()[ range.
-      * Typical choices include std::vector<int>, Eigen::VectorXi, Eigen::VectorXi::Constant, etc.
-      */
-    template<class SizesType>
-    inline void reserve(const SizesType& reserveSizes);
-    #else
-    template<class SizesType>
-    inline void reserve(const SizesType& reserveSizes, const typename SizesType::value_type& enableif =
-    #if (!EIGEN_COMP_MSVC) || (EIGEN_COMP_MSVC>=1500) // MSVC 2005 fails to compile with this typename
-        typename
-    #endif
-        SizesType::value_type())
-    {
-      EIGEN_UNUSED_VARIABLE(enableif);
-      reserveInnerVectors(reserveSizes);
-    }
-    #endif // EIGEN_PARSED_BY_DOXYGEN
-  protected:
-    template<class SizesType>
-    inline void reserveInnerVectors(const SizesType& reserveSizes)
-    {
-      if(isCompressed())
-      {
-        Index totalReserveSize = 0;
-        // turn the matrix into non-compressed mode
-        m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
-        if (!m_innerNonZeros) internal::throw_std_bad_alloc();
-        
-        // temporarily use m_innerSizes to hold the new starting points.
-        StorageIndex* newOuterIndex = m_innerNonZeros;
-        
-        StorageIndex count = 0;
-        for(Index j=0; j<m_outerSize; ++j)
-        {
-          newOuterIndex[j] = count;
-          count += reserveSizes[j] + (m_outerIndex[j+1]-m_outerIndex[j]);
-          totalReserveSize += reserveSizes[j];
-        }
-        m_data.reserve(totalReserveSize);
-        StorageIndex previousOuterIndex = m_outerIndex[m_outerSize];
-        for(Index j=m_outerSize-1; j>=0; --j)
-        {
-          StorageIndex innerNNZ = previousOuterIndex - m_outerIndex[j];
-          for(Index i=innerNNZ-1; i>=0; --i)
-          {
-            m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i);
-            m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i);
-          }
-          previousOuterIndex = m_outerIndex[j];
-          m_outerIndex[j] = newOuterIndex[j];
-          m_innerNonZeros[j] = innerNNZ;
-        }
-        if(m_outerSize>0)
-          m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1];
-        
-        m_data.resize(m_outerIndex[m_outerSize]);
-      }
-      else
-      {
-        StorageIndex* newOuterIndex = static_cast<StorageIndex*>(std::malloc((m_outerSize+1)*sizeof(StorageIndex)));
-        if (!newOuterIndex) internal::throw_std_bad_alloc();
-        
-        StorageIndex count = 0;
-        for(Index j=0; j<m_outerSize; ++j)
-        {
-          newOuterIndex[j] = count;
-          StorageIndex alreadyReserved = (m_outerIndex[j+1]-m_outerIndex[j]) - m_innerNonZeros[j];
-          StorageIndex toReserve = std::max<StorageIndex>(reserveSizes[j], alreadyReserved);
-          count += toReserve + m_innerNonZeros[j];
-        }
-        newOuterIndex[m_outerSize] = count;
-        
-        m_data.resize(count);
-        for(Index j=m_outerSize-1; j>=0; --j)
-        {
-          Index offset = newOuterIndex[j] - m_outerIndex[j];
-          if(offset>0)
-          {
-            StorageIndex innerNNZ = m_innerNonZeros[j];
-            for(Index i=innerNNZ-1; i>=0; --i)
-            {
-              m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i);
-              m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i);
-            }
-          }
-        }
-        
-        std::swap(m_outerIndex, newOuterIndex);
-        std::free(newOuterIndex);
-      }
-      
-    }
-  public:
+    const Index outer = IsRowMajor ? row : col;
+    const Index inner = IsRowMajor ? col : row;
+    Index end = m_innerNonZeros ? m_outerIndex[outer] + m_innerNonZeros[outer] : m_outerIndex[outer + 1];
+    return m_data.atInRange(m_outerIndex[outer], end, inner);
+  }
 
-    //--- low level purely coherent filling ---
-
-    /** \internal
-      * \returns a reference to the non zero coefficient at position \a row, \a col assuming that:
-      * - the nonzero does not already exist
-      * - the new coefficient is the last one according to the storage order
-      *
-      * Before filling a given inner vector you must call the statVec(Index) function.
-      *
-      * After an insertion session, you should call the finalize() function.
-      *
-      * \sa insert, insertBackByOuterInner, startVec */
-    inline Scalar& insertBack(Index row, Index col)
-    {
-      return insertBackByOuterInner(IsRowMajor?row:col, IsRowMajor?col:row);
-    }
-
-    /** \internal
-      * \sa insertBack, startVec */
-    inline Scalar& insertBackByOuterInner(Index outer, Index inner)
-    {
-      eigen_assert(Index(m_outerIndex[outer+1]) == m_data.size() && "Invalid ordered insertion (invalid outer index)");
-      eigen_assert( (m_outerIndex[outer+1]-m_outerIndex[outer]==0 || m_data.index(m_data.size()-1)<inner) && "Invalid ordered insertion (invalid inner index)");
-      Index p = m_outerIndex[outer+1];
-      ++m_outerIndex[outer+1];
-      m_data.append(Scalar(0), inner);
-      return m_data.value(p);
-    }
-
-    /** \internal
-      * \warning use it only if you know what you are doing */
-    inline Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner)
-    {
-      Index p = m_outerIndex[outer+1];
-      ++m_outerIndex[outer+1];
-      m_data.append(Scalar(0), inner);
-      return m_data.value(p);
-    }
-
-    /** \internal
-      * \sa insertBack, insertBackByOuterInner */
-    inline void startVec(Index outer)
-    {
-      eigen_assert(m_outerIndex[outer]==Index(m_data.size()) && "You must call startVec for each inner vector sequentially");
-      eigen_assert(m_outerIndex[outer+1]==0 && "You must call startVec for each inner vector sequentially");
-      m_outerIndex[outer+1] = m_outerIndex[outer];
-    }
-
-    /** \internal
-      * Must be called after inserting a set of non zero entries using the low level compressed API.
-      */
-    inline void finalize()
-    {
-      if(isCompressed())
-      {
-        StorageIndex size = internal::convert_index<StorageIndex>(m_data.size());
-        Index i = m_outerSize;
-        // find the last filled column
-        while (i>=0 && m_outerIndex[i]==0)
-          --i;
-        ++i;
-        while (i<=m_outerSize)
-        {
-          m_outerIndex[i] = size;
-          ++i;
-        }
+  /** \returns a non-const reference to the value of the matrix at position \a i, \a j
+   *
+   * If the element does not exist then it is inserted via the insert(Index,Index) function
+   * which itself turns the matrix into a non compressed form if that was not the case.
+   *
+   * This is a O(log(nnz_j)) operation (binary search) plus the cost of insert(Index,Index)
+   * function if the element does not already exist.
+   */
+  inline Scalar& coeffRef(Index row, Index col) {
+    eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
+    const Index outer = IsRowMajor ? row : col;
+    const Index inner = IsRowMajor ? col : row;
+    Index start = m_outerIndex[outer];
+    Index end = isCompressed() ? m_outerIndex[outer + 1] : m_outerIndex[outer] + m_innerNonZeros[outer];
+    eigen_assert(end >= start && "you probably called coeffRef on a non finalized matrix");
+    Index dst = start == end ? end : m_data.searchLowerIndex(start, end, inner);
+    if (dst == end) {
+      Index capacity = m_outerIndex[outer + 1] - end;
+      if (capacity > 0) {
+        // implies uncompressed: push to back of vector
+        m_innerNonZeros[outer]++;
+        m_data.index(end) = StorageIndex(inner);
+        m_data.value(end) = Scalar(0);
+        return m_data.value(end);
       }
     }
+    if ((dst < end) && (m_data.index(dst) == inner))
+      // this coefficient exists, return a refernece to it
+      return m_data.value(dst);
+    else
+      // insertion will require reconfiguring the buffer
+      return insertAtByOuterInner(outer, inner, dst);
+  }
 
-    //---
+  /** \returns a reference to a novel non zero coefficient with coordinates \a row x \a col.
+   * The non zero coefficient must \b not already exist.
+   *
+   * If the matrix \c *this is in compressed mode, then \c *this is turned into uncompressed
+   * mode while reserving room for 2 x this->innerSize() non zeros if reserve(Index) has not been called earlier.
+   * In this case, the insertion procedure is optimized for a \e sequential insertion mode where elements are assumed to
+   * be inserted by increasing outer-indices.
+   *
+   * If that's not the case, then it is strongly recommended to either use a triplet-list to assemble the matrix, or to
+   * first call reserve(const SizesType &) to reserve the appropriate number of non-zero elements per inner vector.
+   *
+   * Assuming memory has been appropriately reserved, this function performs a sorted insertion in O(1)
+   * if the elements of each inner vector are inserted in increasing inner index order, and in O(nnz_j) for a random
+   * insertion.
+   *
+   */
+  inline Scalar& insert(Index row, Index col);
 
-    template<typename InputIterators>
-    void setFromTriplets(const InputIterators& begin, const InputIterators& end);
-
-    template<typename InputIterators,typename DupFunctor>
-    void setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func);
-
-    void sumupDuplicates() { collapseDuplicates(internal::scalar_sum_op<Scalar,Scalar>()); }
-
-    template<typename DupFunctor>
-    void collapseDuplicates(DupFunctor dup_func = DupFunctor());
-
-    //---
-    
-    /** \internal
-      * same as insert(Index,Index) except that the indices are given relative to the storage order */
-    Scalar& insertByOuterInner(Index j, Index i)
-    {
-      return insert(IsRowMajor ? j : i, IsRowMajor ? i : j);
+ public:
+  /** Removes all non zeros but keep allocated memory
+   *
+   * This function does not free the currently allocated memory. To release as much as memory as possible,
+   * call \code mat.data().squeeze(); \endcode after resizing it.
+   *
+   * \sa resize(Index,Index), data()
+   */
+  inline void setZero() {
+    m_data.clear();
+    std::fill_n(m_outerIndex, m_outerSize + 1, StorageIndex(0));
+    if (m_innerNonZeros) {
+      std::fill_n(m_innerNonZeros, m_outerSize, StorageIndex(0));
     }
+  }
 
-    /** Turns the matrix into the \em compressed format.
-      */
-    void makeCompressed()
-    {
-      if(isCompressed())
-        return;
-      
-      eigen_internal_assert(m_outerIndex!=0 && m_outerSize>0);
-      
-      Index oldStart = m_outerIndex[1];
-      m_outerIndex[1] = m_innerNonZeros[0];
-      for(Index j=1; j<m_outerSize; ++j)
-      {
-        Index nextOldStart = m_outerIndex[j+1];
-        Index offset = oldStart - m_outerIndex[j];
-        if(offset>0)
-        {
-          for(Index k=0; k<m_innerNonZeros[j]; ++k)
-          {
-            m_data.index(m_outerIndex[j]+k) = m_data.index(oldStart+k);
-            m_data.value(m_outerIndex[j]+k) = m_data.value(oldStart+k);
-          }
-        }
-        m_outerIndex[j+1] = m_outerIndex[j] + m_innerNonZeros[j];
-        oldStart = nextOldStart;
+  /** Preallocates \a reserveSize non zeros.
+   *
+   * Precondition: the matrix must be in compressed mode. */
+  inline void reserve(Index reserveSize) {
+    eigen_assert(isCompressed() && "This function does not make sense in non compressed mode.");
+    m_data.reserve(reserveSize);
+  }
+
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+  /** Preallocates \a reserveSize[\c j] non zeros for each column (resp. row) \c j.
+    *
+    * This function turns the matrix in non-compressed mode.
+    *
+    * The type \c SizesType must expose the following interface:
+      \code
+      typedef value_type;
+      const value_type& operator[](i) const;
+      \endcode
+    * for \c i in the [0,this->outerSize()[ range.
+    * Typical choices include std::vector<int>, Eigen::VectorXi, Eigen::VectorXi::Constant, etc.
+    */
+  template <class SizesType>
+  inline void reserve(const SizesType& reserveSizes);
+#else
+  template <class SizesType>
+  inline void reserve(const SizesType& reserveSizes,
+                      const typename SizesType::value_type& enableif = typename SizesType::value_type()) {
+    EIGEN_UNUSED_VARIABLE(enableif);
+    reserveInnerVectors(reserveSizes);
+  }
+#endif  // EIGEN_PARSED_BY_DOXYGEN
+ protected:
+  template <class SizesType>
+  inline void reserveInnerVectors(const SizesType& reserveSizes) {
+    if (isCompressed()) {
+      Index totalReserveSize = 0;
+      for (Index j = 0; j < m_outerSize; ++j) totalReserveSize += internal::convert_index<Index>(reserveSizes[j]);
+
+      // if reserveSizes is empty, don't do anything!
+      if (totalReserveSize == 0) return;
+
+      // turn the matrix into non-compressed mode
+      m_innerNonZeros = internal::conditional_aligned_new_auto<StorageIndex, true>(m_outerSize);
+
+      // temporarily use m_innerSizes to hold the new starting points.
+      StorageIndex* newOuterIndex = m_innerNonZeros;
+
+      Index count = 0;
+      for (Index j = 0; j < m_outerSize; ++j) {
+        newOuterIndex[j] = internal::convert_index<StorageIndex>(count);
+        Index reserveSize = internal::convert_index<Index>(reserveSizes[j]);
+        count += reserveSize + internal::convert_index<Index>(m_outerIndex[j + 1] - m_outerIndex[j]);
       }
-      std::free(m_innerNonZeros);
-      m_innerNonZeros = 0;
+
+      m_data.reserve(totalReserveSize);
+      StorageIndex previousOuterIndex = m_outerIndex[m_outerSize];
+      for (Index j = m_outerSize - 1; j >= 0; --j) {
+        StorageIndex innerNNZ = previousOuterIndex - m_outerIndex[j];
+        StorageIndex begin = m_outerIndex[j];
+        StorageIndex end = begin + innerNNZ;
+        StorageIndex target = newOuterIndex[j];
+        internal::smart_memmove(innerIndexPtr() + begin, innerIndexPtr() + end, innerIndexPtr() + target);
+        internal::smart_memmove(valuePtr() + begin, valuePtr() + end, valuePtr() + target);
+        previousOuterIndex = m_outerIndex[j];
+        m_outerIndex[j] = newOuterIndex[j];
+        m_innerNonZeros[j] = innerNNZ;
+      }
+      if (m_outerSize > 0)
+        m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize - 1] + m_innerNonZeros[m_outerSize - 1] +
+                                    internal::convert_index<StorageIndex>(reserveSizes[m_outerSize - 1]);
+
       m_data.resize(m_outerIndex[m_outerSize]);
-      m_data.squeeze();
-    }
+    } else {
+      StorageIndex* newOuterIndex = internal::conditional_aligned_new_auto<StorageIndex, true>(m_outerSize + 1);
 
-    /** Turns the matrix into the uncompressed mode */
-    void uncompress()
-    {
-      if(m_innerNonZeros != 0)
-        return; 
-      m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
-      for (Index i = 0; i < m_outerSize; i++)
-      {
-        m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i]; 
+      Index count = 0;
+      for (Index j = 0; j < m_outerSize; ++j) {
+        newOuterIndex[j] = internal::convert_index<StorageIndex>(count);
+        Index alreadyReserved =
+            internal::convert_index<Index>(m_outerIndex[j + 1] - m_outerIndex[j] - m_innerNonZeros[j]);
+        Index reserveSize = internal::convert_index<Index>(reserveSizes[j]);
+        Index toReserve = numext::maxi(reserveSize, alreadyReserved);
+        count += toReserve + internal::convert_index<Index>(m_innerNonZeros[j]);
+      }
+      newOuterIndex[m_outerSize] = internal::convert_index<StorageIndex>(count);
+
+      m_data.resize(count);
+      for (Index j = m_outerSize - 1; j >= 0; --j) {
+        StorageIndex innerNNZ = m_innerNonZeros[j];
+        StorageIndex begin = m_outerIndex[j];
+        StorageIndex target = newOuterIndex[j];
+        m_data.moveChunk(begin, target, innerNNZ);
+      }
+
+      std::swap(m_outerIndex, newOuterIndex);
+      internal::conditional_aligned_delete_auto<StorageIndex, true>(newOuterIndex, m_outerSize + 1);
+    }
+  }
+
+ public:
+  //--- low level purely coherent filling ---
+
+  /** \internal
+   * \returns a reference to the non zero coefficient at position \a row, \a col assuming that:
+   * - the nonzero does not already exist
+   * - the new coefficient is the last one according to the storage order
+   *
+   * Before filling a given inner vector you must call the statVec(Index) function.
+   *
+   * After an insertion session, you should call the finalize() function.
+   *
+   * \sa insert, insertBackByOuterInner, startVec */
+  inline Scalar& insertBack(Index row, Index col) {
+    return insertBackByOuterInner(IsRowMajor ? row : col, IsRowMajor ? col : row);
+  }
+
+  /** \internal
+   * \sa insertBack, startVec */
+  inline Scalar& insertBackByOuterInner(Index outer, Index inner) {
+    eigen_assert(Index(m_outerIndex[outer + 1]) == m_data.size() && "Invalid ordered insertion (invalid outer index)");
+    eigen_assert((m_outerIndex[outer + 1] - m_outerIndex[outer] == 0 || m_data.index(m_data.size() - 1) < inner) &&
+                 "Invalid ordered insertion (invalid inner index)");
+    StorageIndex p = m_outerIndex[outer + 1];
+    ++m_outerIndex[outer + 1];
+    m_data.append(Scalar(0), inner);
+    return m_data.value(p);
+  }
+
+  /** \internal
+   * \warning use it only if you know what you are doing */
+  inline Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner) {
+    StorageIndex p = m_outerIndex[outer + 1];
+    ++m_outerIndex[outer + 1];
+    m_data.append(Scalar(0), inner);
+    return m_data.value(p);
+  }
+
+  /** \internal
+   * \sa insertBack, insertBackByOuterInner */
+  inline void startVec(Index outer) {
+    eigen_assert(m_outerIndex[outer] == Index(m_data.size()) &&
+                 "You must call startVec for each inner vector sequentially");
+    eigen_assert(m_outerIndex[outer + 1] == 0 && "You must call startVec for each inner vector sequentially");
+    m_outerIndex[outer + 1] = m_outerIndex[outer];
+  }
+
+  /** \internal
+   * Must be called after inserting a set of non zero entries using the low level compressed API.
+   */
+  inline void finalize() {
+    if (isCompressed()) {
+      StorageIndex size = internal::convert_index<StorageIndex>(m_data.size());
+      Index i = m_outerSize;
+      // find the last filled column
+      while (i >= 0 && m_outerIndex[i] == 0) --i;
+      ++i;
+      while (i <= m_outerSize) {
+        m_outerIndex[i] = size;
+        ++i;
       }
     }
+  }
 
-    /** Suppresses all nonzeros which are \b much \b smaller \b than \a reference under the tolerance \a epsilon */
-    void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
-    {
-      prune(default_prunning_func(reference,epsilon));
+  // remove outer vectors j, j+1 ... j+num-1 and resize the matrix
+  void removeOuterVectors(Index j, Index num = 1) {
+    eigen_assert(num >= 0 && j >= 0 && j + num <= m_outerSize && "Invalid parameters");
+
+    const Index newRows = IsRowMajor ? m_outerSize - num : rows();
+    const Index newCols = IsRowMajor ? cols() : m_outerSize - num;
+
+    const Index begin = j + num;
+    const Index end = m_outerSize;
+    const Index target = j;
+
+    // if the removed vectors are not empty, uncompress the matrix
+    if (m_outerIndex[j + num] > m_outerIndex[j]) uncompress();
+
+    // shift m_outerIndex and m_innerNonZeros [num] to the left
+    internal::smart_memmove(m_outerIndex + begin, m_outerIndex + end + 1, m_outerIndex + target);
+    if (!isCompressed())
+      internal::smart_memmove(m_innerNonZeros + begin, m_innerNonZeros + end, m_innerNonZeros + target);
+
+    // if m_outerIndex[0] > 0, shift the data within the first vector while it is easy to do so
+    if (m_outerIndex[0] > StorageIndex(0)) {
+      uncompress();
+      const Index from = internal::convert_index<Index>(m_outerIndex[0]);
+      const Index to = Index(0);
+      const Index chunkSize = internal::convert_index<Index>(m_innerNonZeros[0]);
+      m_data.moveChunk(from, to, chunkSize);
+      m_outerIndex[0] = StorageIndex(0);
     }
-    
-    /** Turns the matrix into compressed format, and suppresses all nonzeros which do not satisfy the predicate \a keep.
-      * The functor type \a KeepFunc must implement the following function:
-      * \code
-      * bool operator() (const Index& row, const Index& col, const Scalar& value) const;
-      * \endcode
-      * \sa prune(Scalar,RealScalar)
-      */
-    template<typename KeepFunc>
-    void prune(const KeepFunc& keep = KeepFunc())
-    {
-      // TODO optimize the uncompressed mode to avoid moving and allocating the data twice
-      makeCompressed();
 
-      StorageIndex k = 0;
-      for(Index j=0; j<m_outerSize; ++j)
-      {
-        Index previousStart = m_outerIndex[j];
+    // truncate the matrix to the smaller size
+    conservativeResize(newRows, newCols);
+  }
+
+  // insert empty outer vectors at indices j, j+1 ... j+num-1 and resize the matrix
+  void insertEmptyOuterVectors(Index j, Index num = 1) {
+    EIGEN_USING_STD(fill_n);
+    eigen_assert(num >= 0 && j >= 0 && j < m_outerSize && "Invalid parameters");
+
+    const Index newRows = IsRowMajor ? m_outerSize + num : rows();
+    const Index newCols = IsRowMajor ? cols() : m_outerSize + num;
+
+    const Index begin = j;
+    const Index end = m_outerSize;
+    const Index target = j + num;
+
+    // expand the matrix to the larger size
+    conservativeResize(newRows, newCols);
+
+    // shift m_outerIndex and m_innerNonZeros [num] to the right
+    internal::smart_memmove(m_outerIndex + begin, m_outerIndex + end + 1, m_outerIndex + target);
+    // m_outerIndex[begin] == m_outerIndex[target], set all indices in this range to same value
+    fill_n(m_outerIndex + begin, num, m_outerIndex[begin]);
+
+    if (!isCompressed()) {
+      internal::smart_memmove(m_innerNonZeros + begin, m_innerNonZeros + end, m_innerNonZeros + target);
+      // set the nonzeros of the newly inserted vectors to 0
+      fill_n(m_innerNonZeros + begin, num, StorageIndex(0));
+    }
+  }
+
+  template <typename InputIterators>
+  void setFromTriplets(const InputIterators& begin, const InputIterators& end);
+
+  template <typename InputIterators, typename DupFunctor>
+  void setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func);
+
+  template <typename Derived, typename DupFunctor>
+  void collapseDuplicates(DenseBase<Derived>& wi, DupFunctor dup_func = DupFunctor());
+
+  template <typename InputIterators>
+  void setFromSortedTriplets(const InputIterators& begin, const InputIterators& end);
+
+  template <typename InputIterators, typename DupFunctor>
+  void setFromSortedTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func);
+
+  template <typename InputIterators>
+  void insertFromTriplets(const InputIterators& begin, const InputIterators& end);
+
+  template <typename InputIterators, typename DupFunctor>
+  void insertFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func);
+
+  template <typename InputIterators>
+  void insertFromSortedTriplets(const InputIterators& begin, const InputIterators& end);
+
+  template <typename InputIterators, typename DupFunctor>
+  void insertFromSortedTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func);
+
+  //---
+
+  /** \internal
+   * same as insert(Index,Index) except that the indices are given relative to the storage order */
+  Scalar& insertByOuterInner(Index j, Index i) {
+    Index start = m_outerIndex[j];
+    Index end = isCompressed() ? m_outerIndex[j + 1] : start + m_innerNonZeros[j];
+    Index dst = start == end ? end : m_data.searchLowerIndex(start, end, i);
+    if (dst == end) {
+      Index capacity = m_outerIndex[j + 1] - end;
+      if (capacity > 0) {
+        // implies uncompressed: push to back of vector
+        m_innerNonZeros[j]++;
+        m_data.index(end) = StorageIndex(i);
+        m_data.value(end) = Scalar(0);
+        return m_data.value(end);
+      }
+    }
+    eigen_assert((dst == end || m_data.index(dst) != i) &&
+                 "you cannot insert an element that already exists, you must call coeffRef to this end");
+    return insertAtByOuterInner(j, i, dst);
+  }
+
+  /** Turns the matrix into the \em compressed format.
+   */
+  void makeCompressed() {
+    if (isCompressed()) return;
+
+    eigen_internal_assert(m_outerIndex != 0 && m_outerSize > 0);
+
+    StorageIndex start = m_outerIndex[1];
+    m_outerIndex[1] = m_innerNonZeros[0];
+    // try to move fewer, larger contiguous chunks
+    Index copyStart = start;
+    Index copyTarget = m_innerNonZeros[0];
+    for (Index j = 1; j < m_outerSize; j++) {
+      StorageIndex end = start + m_innerNonZeros[j];
+      StorageIndex nextStart = m_outerIndex[j + 1];
+      // dont forget to move the last chunk!
+      bool breakUpCopy = (end != nextStart) || (j == m_outerSize - 1);
+      if (breakUpCopy) {
+        Index chunkSize = end - copyStart;
+        if (chunkSize > 0) m_data.moveChunk(copyStart, copyTarget, chunkSize);
+        copyStart = nextStart;
+        copyTarget += chunkSize;
+      }
+      start = nextStart;
+      m_outerIndex[j + 1] = m_outerIndex[j] + m_innerNonZeros[j];
+    }
+    m_data.resize(m_outerIndex[m_outerSize]);
+
+    // release as much memory as possible
+    internal::conditional_aligned_delete_auto<StorageIndex, true>(m_innerNonZeros, m_outerSize);
+    m_innerNonZeros = 0;
+    m_data.squeeze();
+  }
+
+  /** Turns the matrix into the uncompressed mode */
+  void uncompress() {
+    if (!isCompressed()) return;
+    m_innerNonZeros = internal::conditional_aligned_new_auto<StorageIndex, true>(m_outerSize);
+    if (m_outerIndex[m_outerSize] == 0)
+      std::fill_n(m_innerNonZeros, m_outerSize, StorageIndex(0));
+    else
+      for (Index j = 0; j < m_outerSize; j++) m_innerNonZeros[j] = m_outerIndex[j + 1] - m_outerIndex[j];
+  }
+
+  /** Suppresses all nonzeros which are \b much \b smaller \b than \a reference under the tolerance \a epsilon */
+  void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision()) {
+    prune(default_prunning_func(reference, epsilon));
+  }
+
+  /** Turns the matrix into compressed format, and suppresses all nonzeros which do not satisfy the predicate \a keep.
+   * The functor type \a KeepFunc must implement the following function:
+   * \code
+   * bool operator() (const Index& row, const Index& col, const Scalar& value) const;
+   * \endcode
+   * \sa prune(Scalar,RealScalar)
+   */
+  template <typename KeepFunc>
+  void prune(const KeepFunc& keep = KeepFunc()) {
+    StorageIndex k = 0;
+    for (Index j = 0; j < m_outerSize; ++j) {
+      StorageIndex previousStart = m_outerIndex[j];
+      if (isCompressed())
         m_outerIndex[j] = k;
-        Index end = m_outerIndex[j+1];
-        for(Index i=previousStart; i<end; ++i)
-        {
-          if(keep(IsRowMajor?j:m_data.index(i), IsRowMajor?m_data.index(i):j, m_data.value(i)))
-          {
-            m_data.value(k) = m_data.value(i);
-            m_data.index(k) = m_data.index(i);
-            ++k;
-          }
-        }
-      }
-      m_outerIndex[m_outerSize] = k;
-      m_data.resize(k,0);
-    }
-
-    /** Resizes the matrix to a \a rows x \a cols matrix leaving old values untouched.
-      *
-      * If the sizes of the matrix are decreased, then the matrix is turned to \b uncompressed-mode
-      * and the storage of the out of bounds coefficients is kept and reserved.
-      * Call makeCompressed() to pack the entries and squeeze extra memory.
-      *
-      * \sa reserve(), setZero(), makeCompressed()
-      */
-    void conservativeResize(Index rows, Index cols) 
-    {
-      // No change
-      if (this->rows() == rows && this->cols() == cols) return;
-      
-      // If one dimension is null, then there is nothing to be preserved
-      if(rows==0 || cols==0) return resize(rows,cols);
-
-      Index innerChange = IsRowMajor ? cols - this->cols() : rows - this->rows();
-      Index outerChange = IsRowMajor ? rows - this->rows() : cols - this->cols();
-      StorageIndex newInnerSize = convert_index(IsRowMajor ? cols : rows);
-
-      // Deals with inner non zeros
-      if (m_innerNonZeros)
-      {
-        // Resize m_innerNonZeros
-        StorageIndex *newInnerNonZeros = static_cast<StorageIndex*>(std::realloc(m_innerNonZeros, (m_outerSize + outerChange) * sizeof(StorageIndex)));
-        if (!newInnerNonZeros) internal::throw_std_bad_alloc();
-        m_innerNonZeros = newInnerNonZeros;
-        
-        for(Index i=m_outerSize; i<m_outerSize+outerChange; i++)          
-          m_innerNonZeros[i] = 0;
-      } 
-      else if (innerChange < 0) 
-      {
-        // Inner size decreased: allocate a new m_innerNonZeros
-        m_innerNonZeros = static_cast<StorageIndex*>(std::malloc((m_outerSize + outerChange) * sizeof(StorageIndex)));
-        if (!m_innerNonZeros) internal::throw_std_bad_alloc();
-        for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++)
-          m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i];
-        for(Index i = m_outerSize; i < m_outerSize + outerChange; i++)
-          m_innerNonZeros[i] = 0;
-      }
-      
-      // Change the m_innerNonZeros in case of a decrease of inner size
-      if (m_innerNonZeros && innerChange < 0)
-      {
-        for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++)
-        {
-          StorageIndex &n = m_innerNonZeros[i];
-          StorageIndex start = m_outerIndex[i];
-          while (n > 0 && m_data.index(start+n-1) >= newInnerSize) --n; 
-        }
-      }
-      
-      m_innerSize = newInnerSize;
-
-      // Re-allocate outer index structure if necessary
-      if (outerChange == 0)
-        return;
-          
-      StorageIndex *newOuterIndex = static_cast<StorageIndex*>(std::realloc(m_outerIndex, (m_outerSize + outerChange + 1) * sizeof(StorageIndex)));
-      if (!newOuterIndex) internal::throw_std_bad_alloc();
-      m_outerIndex = newOuterIndex;
-      if (outerChange > 0)
-      {
-        StorageIndex lastIdx = m_outerSize == 0 ? 0 : m_outerIndex[m_outerSize];
-        for(Index i=m_outerSize; i<m_outerSize+outerChange+1; i++)          
-          m_outerIndex[i] = lastIdx; 
-      }
-      m_outerSize += outerChange;
-    }
-    
-    /** Resizes the matrix to a \a rows x \a cols matrix and initializes it to zero.
-      * 
-      * This function does not free the currently allocated memory. To release as much as memory as possible,
-      * call \code mat.data().squeeze(); \endcode after resizing it.
-      * 
-      * \sa reserve(), setZero()
-      */
-    void resize(Index rows, Index cols)
-    {
-      const Index outerSize = IsRowMajor ? rows : cols;
-      m_innerSize = IsRowMajor ? cols : rows;
-      m_data.clear();
-      if (m_outerSize != outerSize || m_outerSize==0)
-      {
-        std::free(m_outerIndex);
-        m_outerIndex = static_cast<StorageIndex*>(std::malloc((outerSize + 1) * sizeof(StorageIndex)));
-        if (!m_outerIndex) internal::throw_std_bad_alloc();
-        
-        m_outerSize = outerSize;
-      }
-      if(m_innerNonZeros)
-      {
-        std::free(m_innerNonZeros);
-        m_innerNonZeros = 0;
-      }
-      memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex));
-    }
-
-    /** \internal
-      * Resize the nonzero vector to \a size */
-    void resizeNonZeros(Index size)
-    {
-      m_data.resize(size);
-    }
-
-    /** \returns a const expression of the diagonal coefficients. */
-    const ConstDiagonalReturnType diagonal() const { return ConstDiagonalReturnType(*this); }
-    
-    /** \returns a read-write expression of the diagonal coefficients.
-      * \warning If the diagonal entries are written, then all diagonal
-      * entries \b must already exist, otherwise an assertion will be raised.
-      */
-    DiagonalReturnType diagonal() { return DiagonalReturnType(*this); }
-
-    /** Default constructor yielding an empty \c 0 \c x \c 0 matrix */
-    inline SparseMatrix()
-      : m_outerSize(-1), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
-    {
-      check_template_parameters();
-      resize(0, 0);
-    }
-
-    /** Constructs a \a rows \c x \a cols empty matrix */
-    inline SparseMatrix(Index rows, Index cols)
-      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
-    {
-      check_template_parameters();
-      resize(rows, cols);
-    }
-
-    /** Constructs a sparse matrix from the sparse expression \a other */
-    template<typename OtherDerived>
-    inline SparseMatrix(const SparseMatrixBase<OtherDerived>& other)
-      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
-    {
-      EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
-        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
-      check_template_parameters();
-      const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator<OtherDerived>::Flags & RowMajorBit);
-      if (needToTranspose)
-        *this = other.derived();
       else
-      {
-        #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-          EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-        #endif
-        internal::call_assignment_no_alias(*this, other.derived());
+        k = m_outerIndex[j];
+      StorageIndex end = isCompressed() ? m_outerIndex[j + 1] : previousStart + m_innerNonZeros[j];
+      for (StorageIndex i = previousStart; i < end; ++i) {
+        StorageIndex row = IsRowMajor ? StorageIndex(j) : m_data.index(i);
+        StorageIndex col = IsRowMajor ? m_data.index(i) : StorageIndex(j);
+        bool keepEntry = keep(row, col, m_data.value(i));
+        if (keepEntry) {
+          m_data.value(k) = m_data.value(i);
+          m_data.index(k) = m_data.index(i);
+          ++k;
+        } else if (!isCompressed())
+          m_innerNonZeros[j]--;
       }
     }
-    
-    /** Constructs a sparse matrix from the sparse selfadjoint view \a other */
-    template<typename OtherDerived, unsigned int UpLo>
-    inline SparseMatrix(const SparseSelfAdjointView<OtherDerived, UpLo>& other)
-      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
-    {
-      check_template_parameters();
-      Base::operator=(other);
+    if (isCompressed()) {
+      m_outerIndex[m_outerSize] = k;
+      m_data.resize(k, 0);
+    }
+  }
+
+  /** Resizes the matrix to a \a rows x \a cols matrix leaving old values untouched.
+   *
+   * If the sizes of the matrix are decreased, then the matrix is turned to \b uncompressed-mode
+   * and the storage of the out of bounds coefficients is kept and reserved.
+   * Call makeCompressed() to pack the entries and squeeze extra memory.
+   *
+   * \sa reserve(), setZero(), makeCompressed()
+   */
+  void conservativeResize(Index rows, Index cols) {
+    // If one dimension is null, then there is nothing to be preserved
+    if (rows == 0 || cols == 0) return resize(rows, cols);
+
+    Index newOuterSize = IsRowMajor ? rows : cols;
+    Index newInnerSize = IsRowMajor ? cols : rows;
+
+    Index innerChange = newInnerSize - m_innerSize;
+    Index outerChange = newOuterSize - m_outerSize;
+
+    if (outerChange != 0) {
+      m_outerIndex = internal::conditional_aligned_realloc_new_auto<StorageIndex, true>(m_outerIndex, newOuterSize + 1,
+                                                                                        m_outerSize + 1);
+
+      if (!isCompressed())
+        m_innerNonZeros = internal::conditional_aligned_realloc_new_auto<StorageIndex, true>(m_innerNonZeros,
+                                                                                             newOuterSize, m_outerSize);
+
+      if (outerChange > 0) {
+        StorageIndex lastIdx = m_outerSize == 0 ? StorageIndex(0) : m_outerIndex[m_outerSize];
+        std::fill_n(m_outerIndex + m_outerSize, outerChange + 1, lastIdx);
+
+        if (!isCompressed()) std::fill_n(m_innerNonZeros + m_outerSize, outerChange, StorageIndex(0));
+      }
+    }
+    m_outerSize = newOuterSize;
+
+    if (innerChange < 0) {
+      for (Index j = 0; j < m_outerSize; j++) {
+        Index start = m_outerIndex[j];
+        Index end = isCompressed() ? m_outerIndex[j + 1] : start + m_innerNonZeros[j];
+        Index lb = m_data.searchLowerIndex(start, end, newInnerSize);
+        if (lb != end) {
+          uncompress();
+          m_innerNonZeros[j] = StorageIndex(lb - start);
+        }
+      }
+    }
+    m_innerSize = newInnerSize;
+
+    Index newSize = m_outerIndex[m_outerSize];
+    eigen_assert(newSize <= m_data.size());
+    m_data.resize(newSize);
+  }
+
+  /** Resizes the matrix to a \a rows x \a cols matrix and initializes it to zero.
+   *
+   * This function does not free the currently allocated memory. To release as much as memory as possible,
+   * call \code mat.data().squeeze(); \endcode after resizing it.
+   *
+   * \sa reserve(), setZero()
+   */
+  void resize(Index rows, Index cols) {
+    const Index outerSize = IsRowMajor ? rows : cols;
+    m_innerSize = IsRowMajor ? cols : rows;
+    m_data.clear();
+
+    if ((m_outerIndex == 0) || (m_outerSize != outerSize)) {
+      m_outerIndex = internal::conditional_aligned_realloc_new_auto<StorageIndex, true>(m_outerIndex, outerSize + 1,
+                                                                                        m_outerSize + 1);
+      m_outerSize = outerSize;
     }
 
-    /** Copy constructor (it performs a deep copy) */
-    inline SparseMatrix(const SparseMatrix& other)
-      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
-    {
-      check_template_parameters();
+    internal::conditional_aligned_delete_auto<StorageIndex, true>(m_innerNonZeros, m_outerSize);
+    m_innerNonZeros = 0;
+
+    std::fill_n(m_outerIndex, m_outerSize + 1, StorageIndex(0));
+  }
+
+  /** \internal
+   * Resize the nonzero vector to \a size */
+  void resizeNonZeros(Index size) { m_data.resize(size); }
+
+  /** \returns a const expression of the diagonal coefficients. */
+  const ConstDiagonalReturnType diagonal() const { return ConstDiagonalReturnType(*this); }
+
+  /** \returns a read-write expression of the diagonal coefficients.
+   * \warning If the diagonal entries are written, then all diagonal
+   * entries \b must already exist, otherwise an assertion will be raised.
+   */
+  DiagonalReturnType diagonal() { return DiagonalReturnType(*this); }
+
+  /** Default constructor yielding an empty \c 0 \c x \c 0 matrix */
+  inline SparseMatrix() : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) { resize(0, 0); }
+
+  /** Constructs a \a rows \c x \a cols empty matrix */
+  inline SparseMatrix(Index rows, Index cols) : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) {
+    resize(rows, cols);
+  }
+
+  /** Constructs a sparse matrix from the sparse expression \a other */
+  template <typename OtherDerived>
+  inline SparseMatrix(const SparseMatrixBase<OtherDerived>& other)
+      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) {
+    EIGEN_STATIC_ASSERT(
+        (internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+    const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator<OtherDerived>::Flags & RowMajorBit);
+    if (needToTranspose)
       *this = other.derived();
+    else {
+#ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+      EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+#endif
+      internal::call_assignment_no_alias(*this, other.derived());
     }
+  }
 
-    /** \brief Copy constructor with in-place evaluation */
-    template<typename OtherDerived>
-    SparseMatrix(const ReturnByValue<OtherDerived>& other)
-      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
-    {
-      check_template_parameters();
+  /** Constructs a sparse matrix from the sparse selfadjoint view \a other */
+  template <typename OtherDerived, unsigned int UpLo>
+  inline SparseMatrix(const SparseSelfAdjointView<OtherDerived, UpLo>& other)
+      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) {
+    Base::operator=(other);
+  }
+
+  inline SparseMatrix(SparseMatrix&& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) {
+    *this = other.derived().markAsRValue();
+  }
+
+  /** Copy constructor (it performs a deep copy) */
+  inline SparseMatrix(const SparseMatrix& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) {
+    *this = other.derived();
+  }
+
+  /** \brief Copy constructor with in-place evaluation */
+  template <typename OtherDerived>
+  SparseMatrix(const ReturnByValue<OtherDerived>& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) {
+    initAssignment(other);
+    other.evalTo(*this);
+  }
+
+  /** \brief Copy constructor with in-place evaluation */
+  template <typename OtherDerived>
+  explicit SparseMatrix(const DiagonalBase<OtherDerived>& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) {
+    *this = other.derived();
+  }
+
+  /** Swaps the content of two sparse matrices of the same type.
+   * This is a fast operation that simply swaps the underlying pointers and parameters. */
+  inline void swap(SparseMatrix& other) {
+    // EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n");
+    std::swap(m_outerIndex, other.m_outerIndex);
+    std::swap(m_innerSize, other.m_innerSize);
+    std::swap(m_outerSize, other.m_outerSize);
+    std::swap(m_innerNonZeros, other.m_innerNonZeros);
+    m_data.swap(other.m_data);
+  }
+
+  /** Sets *this to the identity matrix.
+   * This function also turns the matrix into compressed mode, and drop any reserved memory. */
+  inline void setIdentity() {
+    eigen_assert(m_outerSize == m_innerSize && "ONLY FOR SQUARED MATRICES");
+    internal::conditional_aligned_delete_auto<StorageIndex, true>(m_innerNonZeros, m_outerSize);
+    m_innerNonZeros = 0;
+    m_data.resize(m_outerSize);
+    // is it necessary to squeeze?
+    m_data.squeeze();
+    std::iota(m_outerIndex, m_outerIndex + m_outerSize + 1, StorageIndex(0));
+    std::iota(innerIndexPtr(), innerIndexPtr() + m_outerSize, StorageIndex(0));
+    std::fill_n(valuePtr(), m_outerSize, Scalar(1));
+  }
+
+  inline SparseMatrix& operator=(const SparseMatrix& other) {
+    if (other.isRValue()) {
+      swap(other.const_cast_derived());
+    } else if (this != &other) {
+#ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+      EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+#endif
       initAssignment(other);
-      other.evalTo(*this);
-    }
-    
-    /** \brief Copy constructor with in-place evaluation */
-    template<typename OtherDerived>
-    explicit SparseMatrix(const DiagonalBase<OtherDerived>& other)
-      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
-    {
-      check_template_parameters();
-      *this = other.derived();
-    }
-
-    /** Swaps the content of two sparse matrices of the same type.
-      * This is a fast operation that simply swaps the underlying pointers and parameters. */
-    inline void swap(SparseMatrix& other)
-    {
-      //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n");
-      std::swap(m_outerIndex, other.m_outerIndex);
-      std::swap(m_innerSize, other.m_innerSize);
-      std::swap(m_outerSize, other.m_outerSize);
-      std::swap(m_innerNonZeros, other.m_innerNonZeros);
-      m_data.swap(other.m_data);
-    }
-
-    /** Sets *this to the identity matrix.
-      * This function also turns the matrix into compressed mode, and drop any reserved memory. */
-    inline void setIdentity()
-    {
-      eigen_assert(rows() == cols() && "ONLY FOR SQUARED MATRICES");
-      this->m_data.resize(rows());
-      Eigen::Map<IndexVector>(this->m_data.indexPtr(), rows()).setLinSpaced(0, StorageIndex(rows()-1));
-      Eigen::Map<ScalarVector>(this->m_data.valuePtr(), rows()).setOnes();
-      Eigen::Map<IndexVector>(this->m_outerIndex, rows()+1).setLinSpaced(0, StorageIndex(rows()));
-      std::free(m_innerNonZeros);
-      m_innerNonZeros = 0;
-    }
-    inline SparseMatrix& operator=(const SparseMatrix& other)
-    {
-      if (other.isRValue())
-      {
-        swap(other.const_cast_derived());
+      if (other.isCompressed()) {
+        internal::smart_copy(other.m_outerIndex, other.m_outerIndex + m_outerSize + 1, m_outerIndex);
+        m_data = other.m_data;
+      } else {
+        Base::operator=(other);
       }
-      else if(this!=&other)
-      {
-        #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-          EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-        #endif
-        initAssignment(other);
-        if(other.isCompressed())
-        {
-          internal::smart_copy(other.m_outerIndex, other.m_outerIndex + m_outerSize + 1, m_outerIndex);
-          m_data = other.m_data;
-        }
-        else
-        {
-          Base::operator=(other);
-        }
-      }
-      return *this;
     }
+    return *this;
+  }
+
+  inline SparseMatrix& operator=(SparseMatrix&& other) { return *this = other.derived().markAsRValue(); }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename OtherDerived>
-    inline SparseMatrix& operator=(const EigenBase<OtherDerived>& other)
-    { return Base::operator=(other.derived()); }
+  template <typename OtherDerived>
+  inline SparseMatrix& operator=(const EigenBase<OtherDerived>& other) {
+    return Base::operator=(other.derived());
+  }
 
-    template<typename Lhs, typename Rhs>
-    inline SparseMatrix& operator=(const Product<Lhs,Rhs,AliasFreeProduct>& other);
-#endif // EIGEN_PARSED_BY_DOXYGEN
+  template <typename Lhs, typename Rhs>
+  inline SparseMatrix& operator=(const Product<Lhs, Rhs, AliasFreeProduct>& other);
+#endif  // EIGEN_PARSED_BY_DOXYGEN
 
-    template<typename OtherDerived>
-    EIGEN_DONT_INLINE SparseMatrix& operator=(const SparseMatrixBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  EIGEN_DONT_INLINE SparseMatrix& operator=(const SparseMatrixBase<OtherDerived>& other);
 
-    friend std::ostream & operator << (std::ostream & s, const SparseMatrix& m)
-    {
-      EIGEN_DBG_SPARSE(
-        s << "Nonzero entries:\n";
-        if(m.isCompressed())
-        {
-          for (Index i=0; i<m.nonZeros(); ++i)
-            s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
-        }
-        else
-        {
-          for (Index i=0; i<m.outerSize(); ++i)
-          {
+#ifndef EIGEN_NO_IO
+  friend std::ostream& operator<<(std::ostream& s, const SparseMatrix& m) {
+    EIGEN_DBG_SPARSE(
+        s << "Nonzero entries:\n"; if (m.isCompressed()) {
+          for (Index i = 0; i < m.nonZeros(); ++i) s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+        } else {
+          for (Index i = 0; i < m.outerSize(); ++i) {
             Index p = m.m_outerIndex[i];
-            Index pe = m.m_outerIndex[i]+m.m_innerNonZeros[i];
-            Index k=p;
-            for (; k<pe; ++k) {
+            Index pe = m.m_outerIndex[i] + m.m_innerNonZeros[i];
+            Index k = p;
+            for (; k < pe; ++k) {
               s << "(" << m.m_data.value(k) << "," << m.m_data.index(k) << ") ";
             }
-            for (; k<m.m_outerIndex[i+1]; ++k) {
+            for (; k < m.m_outerIndex[i + 1]; ++k) {
               s << "(_,_) ";
             }
           }
-        }
-        s << std::endl;
-        s << std::endl;
-        s << "Outer pointers:\n";
-        for (Index i=0; i<m.outerSize(); ++i) {
-          s << m.m_outerIndex[i] << " ";
-        }
-        s << " $" << std::endl;
-        if(!m.isCompressed())
-        {
+        } s << std::endl;
+        s << std::endl; s << "Outer pointers:\n";
+        for (Index i = 0; i < m.outerSize(); ++i) { s << m.m_outerIndex[i] << " "; } s << " $" << std::endl;
+        if (!m.isCompressed()) {
           s << "Inner non zeros:\n";
-          for (Index i=0; i<m.outerSize(); ++i) {
+          for (Index i = 0; i < m.outerSize(); ++i) {
             s << m.m_innerNonZeros[i] << " ";
           }
           s << " $" << std::endl;
-        }
-        s << std::endl;
-      );
-      s << static_cast<const SparseMatrixBase<SparseMatrix>&>(m);
-      return s;
-    }
-
-    /** Destructor */
-    inline ~SparseMatrix()
-    {
-      std::free(m_outerIndex);
-      std::free(m_innerNonZeros);
-    }
-
-    /** Overloaded for performance */
-    Scalar sum() const;
-    
-#   ifdef EIGEN_SPARSEMATRIX_PLUGIN
-#     include EIGEN_SPARSEMATRIX_PLUGIN
-#   endif
-
-protected:
-
-    template<typename Other>
-    void initAssignment(const Other& other)
-    {
-      resize(other.rows(), other.cols());
-      if(m_innerNonZeros)
-      {
-        std::free(m_innerNonZeros);
-        m_innerNonZeros = 0;
-      }
-    }
-
-    /** \internal
-      * \sa insert(Index,Index) */
-    EIGEN_DONT_INLINE Scalar& insertCompressed(Index row, Index col);
-
-    /** \internal
-      * A vector object that is equal to 0 everywhere but v at the position i */
-    class SingletonVector
-    {
-        StorageIndex m_index;
-        StorageIndex m_value;
-      public:
-        typedef StorageIndex value_type;
-        SingletonVector(Index i, Index v)
-          : m_index(convert_index(i)), m_value(convert_index(v))
-        {}
-
-        StorageIndex operator[](Index i) const { return i==m_index ? m_value : 0; }
-    };
-
-    /** \internal
-      * \sa insert(Index,Index) */
-    EIGEN_DONT_INLINE Scalar& insertUncompressed(Index row, Index col);
-
-public:
-    /** \internal
-      * \sa insert(Index,Index) */
-    EIGEN_STRONG_INLINE Scalar& insertBackUncompressed(Index row, Index col)
-    {
-      const Index outer = IsRowMajor ? row : col;
-      const Index inner = IsRowMajor ? col : row;
-
-      eigen_assert(!isCompressed());
-      eigen_assert(m_innerNonZeros[outer]<=(m_outerIndex[outer+1] - m_outerIndex[outer]));
-
-      Index p = m_outerIndex[outer] + m_innerNonZeros[outer]++;
-      m_data.index(p) = convert_index(inner);
-      return (m_data.value(p) = Scalar(0));
-    }
-protected:
-    struct IndexPosPair {
-      IndexPosPair(Index a_i, Index a_p) : i(a_i), p(a_p) {}
-      Index i;
-      Index p;
-    };
-
-    /** \internal assign \a diagXpr to the diagonal of \c *this
-      * There are different strategies:
-      *   1 - if *this is overwritten (Func==assign_op) or *this is empty, then we can work treat *this as a dense vector expression.
-      *   2 - otherwise, for each diagonal coeff,
-      *     2.a - if it already exists, then we update it,
-      *     2.b - otherwise, if *this is uncompressed and that the current inner-vector has empty room for at least 1 element, then we perform an in-place insertion.
-      *     2.c - otherwise, we'll have to reallocate and copy everything, so instead of doing so for each new element, it is recorded in a std::vector.
-      *   3 - at the end, if some entries failed to be inserted in-place, then we alloc a new buffer, copy each chunk at the right position, and insert the new elements.
-      * 
-      * TODO: some piece of code could be isolated and reused for a general in-place update strategy.
-      * TODO: if we start to defer the insertion of some elements (i.e., case 2.c executed once),
-      *       then it *might* be better to disable case 2.b since they will have to be copied anyway.
-      */
-    template<typename DiagXpr, typename Func>
-    void assignDiagonal(const DiagXpr diagXpr, const Func& assignFunc)
-    {
-      Index n = diagXpr.size();
-
-      const bool overwrite = internal::is_same<Func, internal::assign_op<Scalar,Scalar> >::value;
-      if(overwrite)
-      {
-        if((this->rows()!=n) || (this->cols()!=n))
-          this->resize(n, n);
-      }
-
-      if(m_data.size()==0 || overwrite)
-      {
-        typedef Array<StorageIndex,Dynamic,1> ArrayXI;  
-        this->makeCompressed();
-        this->resizeNonZeros(n);
-        Eigen::Map<ArrayXI>(this->innerIndexPtr(), n).setLinSpaced(0,StorageIndex(n)-1);
-        Eigen::Map<ArrayXI>(this->outerIndexPtr(), n+1).setLinSpaced(0,StorageIndex(n));
-        Eigen::Map<Array<Scalar,Dynamic,1> > values = this->coeffs();
-        values.setZero();
-        internal::call_assignment_no_alias(values, diagXpr, assignFunc);
-      }
-      else
-      {
-        bool isComp = isCompressed();
-        internal::evaluator<DiagXpr> diaEval(diagXpr);
-        std::vector<IndexPosPair> newEntries;
-
-        // 1 - try in-place update and record insertion failures
-        for(Index i = 0; i<n; ++i)
-        {
-          internal::LowerBoundIndex lb = this->lower_bound(i,i);
-          Index p = lb.value;
-          if(lb.found)
-          {
-            // the coeff already exists
-            assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i));
-          }
-          else if((!isComp) && m_innerNonZeros[i] < (m_outerIndex[i+1]-m_outerIndex[i]))
-          {
-            // non compressed mode with local room for inserting one element
-            m_data.moveChunk(p, p+1, m_outerIndex[i]+m_innerNonZeros[i]-p);
-            m_innerNonZeros[i]++;
-            m_data.value(p) = Scalar(0);
-            m_data.index(p) = StorageIndex(i);
-            assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i));
-          }
-          else
-          {
-            // defer insertion
-            newEntries.push_back(IndexPosPair(i,p));
-          }
-        }
-        // 2 - insert deferred entries
-        Index n_entries = Index(newEntries.size());
-        if(n_entries>0)
-        {
-          Storage newData(m_data.size()+n_entries);
-          Index prev_p = 0;
-          Index prev_i = 0;
-          for(Index k=0; k<n_entries;++k)
-          {
-            Index i = newEntries[k].i;
-            Index p = newEntries[k].p;
-            internal::smart_copy(m_data.valuePtr()+prev_p, m_data.valuePtr()+p, newData.valuePtr()+prev_p+k);
-            internal::smart_copy(m_data.indexPtr()+prev_p, m_data.indexPtr()+p, newData.indexPtr()+prev_p+k);
-            for(Index j=prev_i;j<i;++j)
-              m_outerIndex[j+1] += k;
-            if(!isComp)
-              m_innerNonZeros[i]++;
-            prev_p = p;
-            prev_i = i;
-            newData.value(p+k) = Scalar(0);
-            newData.index(p+k) = StorageIndex(i);
-            assignFunc.assignCoeff(newData.value(p+k), diaEval.coeff(i));
-          }
-          {
-            internal::smart_copy(m_data.valuePtr()+prev_p, m_data.valuePtr()+m_data.size(), newData.valuePtr()+prev_p+n_entries);
-            internal::smart_copy(m_data.indexPtr()+prev_p, m_data.indexPtr()+m_data.size(), newData.indexPtr()+prev_p+n_entries);
-            for(Index j=prev_i+1;j<=m_outerSize;++j)
-              m_outerIndex[j] += n_entries;
-          }
-          m_data.swap(newData);
-        }
-      }
-    }
-
-private:
-  static void check_template_parameters()
-  {
-    EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE);
-    EIGEN_STATIC_ASSERT((Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS);
+        } s
+        << std::endl;);
+    s << static_cast<const SparseMatrixBase<SparseMatrix>&>(m);
+    return s;
   }
+#endif
+
+  /** Destructor */
+  inline ~SparseMatrix() {
+    internal::conditional_aligned_delete_auto<StorageIndex, true>(m_outerIndex, m_outerSize + 1);
+    internal::conditional_aligned_delete_auto<StorageIndex, true>(m_innerNonZeros, m_outerSize);
+  }
+
+  /** Overloaded for performance */
+  Scalar sum() const;
+
+#ifdef EIGEN_SPARSEMATRIX_PLUGIN
+#include EIGEN_SPARSEMATRIX_PLUGIN
+#endif
+
+ protected:
+  template <typename Other>
+  void initAssignment(const Other& other) {
+    resize(other.rows(), other.cols());
+    internal::conditional_aligned_delete_auto<StorageIndex, true>(m_innerNonZeros, m_outerSize);
+    m_innerNonZeros = 0;
+  }
+
+  /** \internal
+   * \sa insert(Index,Index) */
+  EIGEN_DEPRECATED EIGEN_DONT_INLINE Scalar& insertCompressed(Index row, Index col);
+
+  /** \internal
+   * A vector object that is equal to 0 everywhere but v at the position i */
+  class SingletonVector {
+    StorageIndex m_index;
+    StorageIndex m_value;
+
+   public:
+    typedef StorageIndex value_type;
+    SingletonVector(Index i, Index v) : m_index(convert_index(i)), m_value(convert_index(v)) {}
+
+    StorageIndex operator[](Index i) const { return i == m_index ? m_value : 0; }
+  };
+
+  /** \internal
+   * \sa insert(Index,Index) */
+  EIGEN_DEPRECATED EIGEN_DONT_INLINE Scalar& insertUncompressed(Index row, Index col);
+
+ public:
+  /** \internal
+   * \sa insert(Index,Index) */
+  EIGEN_STRONG_INLINE Scalar& insertBackUncompressed(Index row, Index col) {
+    const Index outer = IsRowMajor ? row : col;
+    const Index inner = IsRowMajor ? col : row;
+
+    eigen_assert(!isCompressed());
+    eigen_assert(m_innerNonZeros[outer] <= (m_outerIndex[outer + 1] - m_outerIndex[outer]));
+
+    Index p = m_outerIndex[outer] + m_innerNonZeros[outer]++;
+    m_data.index(p) = StorageIndex(inner);
+    m_data.value(p) = Scalar(0);
+    return m_data.value(p);
+  }
+
+ protected:
+  struct IndexPosPair {
+    IndexPosPair(Index a_i, Index a_p) : i(a_i), p(a_p) {}
+    Index i;
+    Index p;
+  };
+
+  /** \internal assign \a diagXpr to the diagonal of \c *this
+   * There are different strategies:
+   *   1 - if *this is overwritten (Func==assign_op) or *this is empty, then we can work treat *this as a dense vector
+   * expression. 2 - otherwise, for each diagonal coeff, 2.a - if it already exists, then we update it, 2.b - if the
+   * correct position is at the end of the vector, and there is capacity, push to back 2.b - otherwise, the insertion
+   * requires a data move, record insertion locations and handle in a second pass 3 - at the end, if some entries failed
+   * to be updated in-place, then we alloc a new buffer, copy each chunk at the right position, and insert the new
+   * elements.
+   */
+  template <typename DiagXpr, typename Func>
+  void assignDiagonal(const DiagXpr diagXpr, const Func& assignFunc) {
+    constexpr StorageIndex kEmptyIndexVal(-1);
+    typedef typename ScalarVector::AlignedMapType ValueMap;
+
+    Index n = diagXpr.size();
+
+    const bool overwrite = internal::is_same<Func, internal::assign_op<Scalar, Scalar>>::value;
+    if (overwrite) {
+      if ((m_outerSize != n) || (m_innerSize != n)) resize(n, n);
+    }
+
+    if (m_data.size() == 0 || overwrite) {
+      internal::conditional_aligned_delete_auto<StorageIndex, true>(m_innerNonZeros, m_outerSize);
+      m_innerNonZeros = 0;
+      resizeNonZeros(n);
+      ValueMap valueMap(valuePtr(), n);
+      std::iota(m_outerIndex, m_outerIndex + n + 1, StorageIndex(0));
+      std::iota(innerIndexPtr(), innerIndexPtr() + n, StorageIndex(0));
+      valueMap.setZero();
+      internal::call_assignment_no_alias(valueMap, diagXpr, assignFunc);
+    } else {
+      internal::evaluator<DiagXpr> diaEval(diagXpr);
+
+      ei_declare_aligned_stack_constructed_variable(StorageIndex, tmp, n, 0);
+      typename IndexVector::AlignedMapType insertionLocations(tmp, n);
+      insertionLocations.setConstant(kEmptyIndexVal);
+
+      Index deferredInsertions = 0;
+      Index shift = 0;
+
+      for (Index j = 0; j < n; j++) {
+        Index begin = m_outerIndex[j];
+        Index end = isCompressed() ? m_outerIndex[j + 1] : begin + m_innerNonZeros[j];
+        Index capacity = m_outerIndex[j + 1] - end;
+        Index dst = m_data.searchLowerIndex(begin, end, j);
+        // the entry exists: update it now
+        if (dst != end && m_data.index(dst) == StorageIndex(j))
+          assignFunc.assignCoeff(m_data.value(dst), diaEval.coeff(j));
+        // the entry belongs at the back of the vector: push to back
+        else if (dst == end && capacity > 0)
+          assignFunc.assignCoeff(insertBackUncompressed(j, j), diaEval.coeff(j));
+        // the insertion requires a data move, record insertion location and handle in second pass
+        else {
+          insertionLocations.coeffRef(j) = StorageIndex(dst);
+          deferredInsertions++;
+          // if there is no capacity, all vectors to the right of this are shifted
+          if (capacity == 0) shift++;
+        }
+      }
+
+      if (deferredInsertions > 0) {
+        m_data.resize(m_data.size() + shift);
+        Index copyEnd = isCompressed() ? m_outerIndex[m_outerSize]
+                                       : m_outerIndex[m_outerSize - 1] + m_innerNonZeros[m_outerSize - 1];
+        for (Index j = m_outerSize - 1; deferredInsertions > 0; j--) {
+          Index begin = m_outerIndex[j];
+          Index end = isCompressed() ? m_outerIndex[j + 1] : begin + m_innerNonZeros[j];
+          Index capacity = m_outerIndex[j + 1] - end;
+
+          bool doInsertion = insertionLocations(j) >= 0;
+          bool breakUpCopy = doInsertion && (capacity > 0);
+          // break up copy for sorted insertion into inactive nonzeros
+          // optionally, add another criterium, i.e. 'breakUpCopy || (capacity > threhsold)'
+          // where `threshold >= 0` to skip inactive nonzeros in each vector
+          // this reduces the total number of copied elements, but requires more moveChunk calls
+          if (breakUpCopy) {
+            Index copyBegin = m_outerIndex[j + 1];
+            Index to = copyBegin + shift;
+            Index chunkSize = copyEnd - copyBegin;
+            m_data.moveChunk(copyBegin, to, chunkSize);
+            copyEnd = end;
+          }
+
+          m_outerIndex[j + 1] += shift;
+
+          if (doInsertion) {
+            // if there is capacity, shift into the inactive nonzeros
+            if (capacity > 0) shift++;
+            Index copyBegin = insertionLocations(j);
+            Index to = copyBegin + shift;
+            Index chunkSize = copyEnd - copyBegin;
+            m_data.moveChunk(copyBegin, to, chunkSize);
+            Index dst = to - 1;
+            m_data.index(dst) = StorageIndex(j);
+            m_data.value(dst) = Scalar(0);
+            assignFunc.assignCoeff(m_data.value(dst), diaEval.coeff(j));
+            if (!isCompressed()) m_innerNonZeros[j]++;
+            shift--;
+            deferredInsertions--;
+            copyEnd = copyBegin;
+          }
+        }
+      }
+      eigen_assert((shift == 0) && (deferredInsertions == 0));
+    }
+  }
+
+  /* These functions are used to avoid a redundant binary search operation in functions such as coeffRef() and assume
+   * `dst` is the appropriate sorted insertion point */
+  EIGEN_STRONG_INLINE Scalar& insertAtByOuterInner(Index outer, Index inner, Index dst);
+  Scalar& insertCompressedAtByOuterInner(Index outer, Index inner, Index dst);
+  Scalar& insertUncompressedAtByOuterInner(Index outer, Index inner, Index dst);
+
+ private:
+  EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned, THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE)
+  EIGEN_STATIC_ASSERT((Options & (ColMajor | RowMajor)) == Options, INVALID_MATRIX_TEMPLATE_PARAMETERS)
 
   struct default_prunning_func {
     default_prunning_func(const Scalar& ref, const RealScalar& eps) : reference(ref), epsilon(eps) {}
-    inline bool operator() (const Index&, const Index&, const Scalar& value) const
-    {
+    inline bool operator()(const Index&, const Index&, const Scalar& value) const {
       return !internal::isMuchSmallerThan(value, reference, epsilon);
     }
     Scalar reference;
@@ -1031,56 +1075,186 @@
 
 namespace internal {
 
-template<typename InputIterator, typename SparseMatrixType, typename DupFunctor>
-void set_from_triplets(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat, DupFunctor dup_func)
-{
-  enum { IsRowMajor = SparseMatrixType::IsRowMajor };
-  typedef typename SparseMatrixType::Scalar Scalar;
-  typedef typename SparseMatrixType::StorageIndex StorageIndex;
-  SparseMatrix<Scalar,IsRowMajor?ColMajor:RowMajor,StorageIndex> trMat(mat.rows(),mat.cols());
+// Creates a compressed sparse matrix from a range of unsorted triplets
+// Requires temporary storage to handle duplicate entries
+template <typename InputIterator, typename SparseMatrixType, typename DupFunctor>
+void set_from_triplets(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat,
+                       DupFunctor dup_func) {
+  constexpr bool IsRowMajor = SparseMatrixType::IsRowMajor;
+  using StorageIndex = typename SparseMatrixType::StorageIndex;
+  using IndexMap = typename VectorX<StorageIndex>::AlignedMapType;
+  using TransposedSparseMatrix =
+      SparseMatrix<typename SparseMatrixType::Scalar, IsRowMajor ? ColMajor : RowMajor, StorageIndex>;
 
-  if(begin!=end)
-  {
-    // pass 1: count the nnz per inner-vector
-    typename SparseMatrixType::IndexVector wi(trMat.outerSize());
-    wi.setZero();
-    for(InputIterator it(begin); it!=end; ++it)
-    {
-      eigen_assert(it->row()>=0 && it->row()<mat.rows() && it->col()>=0 && it->col()<mat.cols());
-      wi(IsRowMajor ? it->col() : it->row())++;
-    }
+  if (begin == end) return;
 
-    // pass 2: insert all the elements into trMat
-    trMat.reserve(wi);
-    for(InputIterator it(begin); it!=end; ++it)
-      trMat.insertBackUncompressed(it->row(),it->col()) = it->value();
+  // There are two strategies to consider for constructing a matrix from unordered triplets:
+  // A) construct the 'mat' in its native storage order and sort in-place (less memory); or,
+  // B) construct the transposed matrix and use an implicit sort upon assignment to `mat` (less time).
+  // This routine uses B) for faster execution time.
+  TransposedSparseMatrix trmat(mat.rows(), mat.cols());
 
-    // pass 3:
-    trMat.collapseDuplicates(dup_func);
+  // scan triplets to determine allocation size before constructing matrix
+  Index nonZeros = 0;
+  for (InputIterator it(begin); it != end; ++it) {
+    eigen_assert(it->row() >= 0 && it->row() < mat.rows() && it->col() >= 0 && it->col() < mat.cols());
+    StorageIndex j = convert_index<StorageIndex>(IsRowMajor ? it->col() : it->row());
+    if (nonZeros == NumTraits<StorageIndex>::highest()) internal::throw_std_bad_alloc();
+    trmat.outerIndexPtr()[j + 1]++;
+    nonZeros++;
   }
 
-  // pass 4: transposed copy -> implicit sorting
-  mat = trMat;
+  std::partial_sum(trmat.outerIndexPtr(), trmat.outerIndexPtr() + trmat.outerSize() + 1, trmat.outerIndexPtr());
+  eigen_assert(nonZeros == trmat.outerIndexPtr()[trmat.outerSize()]);
+  trmat.resizeNonZeros(nonZeros);
+
+  // construct temporary array to track insertions (outersize) and collapse duplicates (innersize)
+  ei_declare_aligned_stack_constructed_variable(StorageIndex, tmp, numext::maxi(mat.innerSize(), mat.outerSize()), 0);
+  smart_copy(trmat.outerIndexPtr(), trmat.outerIndexPtr() + trmat.outerSize(), tmp);
+
+  // push triplets to back of each vector
+  for (InputIterator it(begin); it != end; ++it) {
+    StorageIndex j = convert_index<StorageIndex>(IsRowMajor ? it->col() : it->row());
+    StorageIndex i = convert_index<StorageIndex>(IsRowMajor ? it->row() : it->col());
+    StorageIndex k = tmp[j];
+    trmat.data().index(k) = i;
+    trmat.data().value(k) = it->value();
+    tmp[j]++;
+  }
+
+  IndexMap wi(tmp, trmat.innerSize());
+  trmat.collapseDuplicates(wi, dup_func);
+  // implicit sorting
+  mat = trmat;
 }
 
+// Creates a compressed sparse matrix from a sorted range of triplets
+template <typename InputIterator, typename SparseMatrixType, typename DupFunctor>
+void set_from_triplets_sorted(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat,
+                              DupFunctor dup_func) {
+  constexpr bool IsRowMajor = SparseMatrixType::IsRowMajor;
+  using StorageIndex = typename SparseMatrixType::StorageIndex;
+
+  if (begin == end) return;
+
+  constexpr StorageIndex kEmptyIndexValue(-1);
+  // deallocate inner nonzeros if present and zero outerIndexPtr
+  mat.resize(mat.rows(), mat.cols());
+  // use outer indices to count non zero entries (excluding duplicate entries)
+  StorageIndex previous_j = kEmptyIndexValue;
+  StorageIndex previous_i = kEmptyIndexValue;
+  // scan triplets to determine allocation size before constructing matrix
+  Index nonZeros = 0;
+  for (InputIterator it(begin); it != end; ++it) {
+    eigen_assert(it->row() >= 0 && it->row() < mat.rows() && it->col() >= 0 && it->col() < mat.cols());
+    StorageIndex j = convert_index<StorageIndex>(IsRowMajor ? it->row() : it->col());
+    StorageIndex i = convert_index<StorageIndex>(IsRowMajor ? it->col() : it->row());
+    eigen_assert(j > previous_j || (j == previous_j && i >= previous_i));
+    // identify duplicates by examining previous location
+    bool duplicate = (previous_j == j) && (previous_i == i);
+    if (!duplicate) {
+      if (nonZeros == NumTraits<StorageIndex>::highest()) internal::throw_std_bad_alloc();
+      nonZeros++;
+      mat.outerIndexPtr()[j + 1]++;
+      previous_j = j;
+      previous_i = i;
+    }
+  }
+
+  // finalize outer indices and allocate memory
+  std::partial_sum(mat.outerIndexPtr(), mat.outerIndexPtr() + mat.outerSize() + 1, mat.outerIndexPtr());
+  eigen_assert(nonZeros == mat.outerIndexPtr()[mat.outerSize()]);
+  mat.resizeNonZeros(nonZeros);
+
+  previous_i = kEmptyIndexValue;
+  previous_j = kEmptyIndexValue;
+  Index back = 0;
+  for (InputIterator it(begin); it != end; ++it) {
+    StorageIndex j = convert_index<StorageIndex>(IsRowMajor ? it->row() : it->col());
+    StorageIndex i = convert_index<StorageIndex>(IsRowMajor ? it->col() : it->row());
+    bool duplicate = (previous_j == j) && (previous_i == i);
+    if (duplicate) {
+      mat.data().value(back - 1) = dup_func(mat.data().value(back - 1), it->value());
+    } else {
+      // push triplets to back
+      mat.data().index(back) = i;
+      mat.data().value(back) = it->value();
+      previous_j = j;
+      previous_i = i;
+      back++;
+    }
+  }
+  eigen_assert(back == nonZeros);
+  // matrix is finalized
 }
 
+// thin wrapper around a generic binary functor to use the sparse disjunction evaulator instead of the default
+// "arithmetic" evaulator
+template <typename DupFunctor, typename LhsScalar, typename RhsScalar = LhsScalar>
+struct scalar_disjunction_op {
+  using result_type = typename result_of<DupFunctor(LhsScalar, RhsScalar)>::type;
+  scalar_disjunction_op(const DupFunctor& op) : m_functor(op) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
+    return m_functor(a, b);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const DupFunctor& functor() const { return m_functor; }
+  const DupFunctor& m_functor;
+};
 
-/** Fill the matrix \c *this with the list of \em triplets defined by the iterator range \a begin - \a end.
+template <typename DupFunctor, typename LhsScalar, typename RhsScalar>
+struct functor_traits<scalar_disjunction_op<DupFunctor, LhsScalar, RhsScalar>> : public functor_traits<DupFunctor> {};
+
+// Creates a compressed sparse matrix from its existing entries and those from an unsorted range of triplets
+template <typename InputIterator, typename SparseMatrixType, typename DupFunctor>
+void insert_from_triplets(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat,
+                          DupFunctor dup_func) {
+  using Scalar = typename SparseMatrixType::Scalar;
+  using SrcXprType =
+      CwiseBinaryOp<scalar_disjunction_op<DupFunctor, Scalar>, const SparseMatrixType, const SparseMatrixType>;
+
+  // set_from_triplets is necessary to sort the inner indices and remove the duplicate entries
+  SparseMatrixType trips(mat.rows(), mat.cols());
+  set_from_triplets(begin, end, trips, dup_func);
+
+  SrcXprType src = mat.binaryExpr(trips, scalar_disjunction_op<DupFunctor, Scalar>(dup_func));
+  // the sparse assignment procedure creates a temporary matrix and swaps the final result
+  assign_sparse_to_sparse<SparseMatrixType, SrcXprType>(mat, src);
+}
+
+// Creates a compressed sparse matrix from its existing entries and those from an sorted range of triplets
+template <typename InputIterator, typename SparseMatrixType, typename DupFunctor>
+void insert_from_triplets_sorted(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat,
+                                 DupFunctor dup_func) {
+  using Scalar = typename SparseMatrixType::Scalar;
+  using SrcXprType =
+      CwiseBinaryOp<scalar_disjunction_op<DupFunctor, Scalar>, const SparseMatrixType, const SparseMatrixType>;
+
+  // TODO: process triplets without making a copy
+  SparseMatrixType trips(mat.rows(), mat.cols());
+  set_from_triplets_sorted(begin, end, trips, dup_func);
+
+  SrcXprType src = mat.binaryExpr(trips, scalar_disjunction_op<DupFunctor, Scalar>(dup_func));
+  // the sparse assignment procedure creates a temporary matrix and swaps the final result
+  assign_sparse_to_sparse<SparseMatrixType, SrcXprType>(mat, src);
+}
+
+}  // namespace internal
+
+/** Fill the matrix \c *this with the list of \em triplets defined in the half-open range from \a begin to \a end.
   *
   * A \em triplet is a tuple (i,j,value) defining a non-zero element.
-  * The input list of triplets does not have to be sorted, and can contains duplicated elements.
+  * The input list of triplets does not have to be sorted, and may contain duplicated elements.
   * In any case, the result is a \b sorted and \b compressed sparse matrix where the duplicates have been summed up.
   * This is a \em O(n) operation, with \em n the number of triplet elements.
-  * The initial contents of \c *this is destroyed.
+  * The initial contents of \c *this are destroyed.
   * The matrix \c *this must be properly resized beforehand using the SparseMatrix(Index,Index) constructor,
   * or the resize(Index,Index) method. The sizes are not extracted from the triplet list.
   *
   * The \a InputIterators value_type must provide the following interface:
   * \code
   * Scalar value() const; // the value
-  * Scalar row() const;   // the row index i
-  * Scalar col() const;   // the column index j
+  * IndexType row() const;   // the row index i
+  * IndexType col() const;   // the column index j
   * \endcode
   * See for instance the Eigen::Triplet template class.
   *
@@ -1103,111 +1277,237 @@
   * an abstract iterator over a complex data-structure that would be expensive to evaluate. The triplets should rather
   * be explicitly stored into a std::vector for instance.
   */
-template<typename Scalar, int _Options, typename _StorageIndex>
-template<typename InputIterators>
-void SparseMatrix<Scalar,_Options,_StorageIndex>::setFromTriplets(const InputIterators& begin, const InputIterators& end)
-{
-  internal::set_from_triplets<InputIterators, SparseMatrix<Scalar,_Options,_StorageIndex> >(begin, end, *this, internal::scalar_sum_op<Scalar,Scalar>());
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::setFromTriplets(const InputIterators& begin,
+                                                                    const InputIterators& end) {
+  internal::set_from_triplets<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>>(
+      begin, end, *this, internal::scalar_sum_op<Scalar, Scalar>());
 }
 
 /** The same as setFromTriplets but when duplicates are met the functor \a dup_func is applied:
+ * \code
+ * value = dup_func(OldValue, NewValue)
+ * \endcode
+ * Here is a C++11 example keeping the latest entry only:
+ * \code
+ * mat.setFromTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; });
+ * \endcode
+ */
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators, typename DupFunctor>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::setFromTriplets(const InputIterators& begin,
+                                                                    const InputIterators& end, DupFunctor dup_func) {
+  internal::set_from_triplets<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>, DupFunctor>(
+      begin, end, *this, dup_func);
+}
+
+/** The same as setFromTriplets but triplets are assumed to be pre-sorted. This is faster and requires less temporary
+ * storage. Two triplets `a` and `b` are appropriately ordered if: \code ColMajor: ((a.col() != b.col()) ? (a.col() <
+ * b.col()) : (a.row() < b.row()) RowMajor: ((a.row() != b.row()) ? (a.row() < b.row()) : (a.col() < b.col()) \endcode
+ */
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::setFromSortedTriplets(const InputIterators& begin,
+                                                                          const InputIterators& end) {
+  internal::set_from_triplets_sorted<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>>(
+      begin, end, *this, internal::scalar_sum_op<Scalar, Scalar>());
+}
+
+/** The same as setFromSortedTriplets but when duplicates are met the functor \a dup_func is applied:
+ * \code
+ * value = dup_func(OldValue, NewValue)
+ * \endcode
+ * Here is a C++11 example keeping the latest entry only:
+ * \code
+ * mat.setFromSortedTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; });
+ * \endcode
+ */
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators, typename DupFunctor>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::setFromSortedTriplets(const InputIterators& begin,
+                                                                          const InputIterators& end,
+                                                                          DupFunctor dup_func) {
+  internal::set_from_triplets_sorted<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>, DupFunctor>(
+      begin, end, *this, dup_func);
+}
+
+/** Insert a batch of elements into the matrix \c *this with the list of \em triplets defined in the half-open range
+  from \a begin to \a end.
+  *
+  * A \em triplet is a tuple (i,j,value) defining a non-zero element.
+  * The input list of triplets does not have to be sorted, and may contain duplicated elements.
+  * In any case, the result is a \b sorted and \b compressed sparse matrix where the duplicates have been summed up.
+  * This is a \em O(n) operation, with \em n the number of triplet elements.
+  * The initial contents of \c *this are preserved (except for the summation of duplicate elements).
+  * The matrix \c *this must be properly sized beforehand. The sizes are not extracted from the triplet list.
+  *
+  * The \a InputIterators value_type must provide the following interface:
   * \code
-  * value = dup_func(OldValue, NewValue)
-  * \endcode 
-  * Here is a C++11 example keeping the latest entry only:
-  * \code
-  * mat.setFromTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; });
+  * Scalar value() const; // the value
+  * IndexType row() const;   // the row index i
+  * IndexType col() const;   // the column index j
   * \endcode
+  * See for instance the Eigen::Triplet template class.
+  *
+  * Here is a typical usage example:
+  * \code
+    SparseMatrixType m(rows,cols); // m contains nonzero entries
+    typedef Triplet<double> T;
+    std::vector<T> tripletList;
+    tripletList.reserve(estimation_of_entries);
+    for(...)
+    {
+      // ...
+      tripletList.push_back(T(i,j,v_ij));
+    }
+
+    m.insertFromTriplets(tripletList.begin(), tripletList.end());
+    // m is ready to go!
+  * \endcode
+  *
+  * \warning The list of triplets is read multiple times (at least twice). Therefore, it is not recommended to define
+  * an abstract iterator over a complex data-structure that would be expensive to evaluate. The triplets should rather
+  * be explicitly stored into a std::vector for instance.
   */
-template<typename Scalar, int _Options, typename _StorageIndex>
-template<typename InputIterators,typename DupFunctor>
-void SparseMatrix<Scalar,_Options,_StorageIndex>::setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func)
-{
-  internal::set_from_triplets<InputIterators, SparseMatrix<Scalar,_Options,_StorageIndex>, DupFunctor>(begin, end, *this, dup_func);
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::insertFromTriplets(const InputIterators& begin,
+                                                                       const InputIterators& end) {
+  internal::insert_from_triplets<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>>(
+      begin, end, *this, internal::scalar_sum_op<Scalar, Scalar>());
+}
+
+/** The same as insertFromTriplets but when duplicates are met the functor \a dup_func is applied:
+ * \code
+ * value = dup_func(OldValue, NewValue)
+ * \endcode
+ * Here is a C++11 example keeping the latest entry only:
+ * \code
+ * mat.insertFromTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; });
+ * \endcode
+ */
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators, typename DupFunctor>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::insertFromTriplets(const InputIterators& begin,
+                                                                       const InputIterators& end, DupFunctor dup_func) {
+  internal::insert_from_triplets<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>, DupFunctor>(
+      begin, end, *this, dup_func);
+}
+
+/** The same as insertFromTriplets but triplets are assumed to be pre-sorted. This is faster and requires less temporary
+ * storage. Two triplets `a` and `b` are appropriately ordered if: \code ColMajor: ((a.col() != b.col()) ? (a.col() <
+ * b.col()) : (a.row() < b.row()) RowMajor: ((a.row() != b.row()) ? (a.row() < b.row()) : (a.col() < b.col()) \endcode
+ */
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::insertFromSortedTriplets(const InputIterators& begin,
+                                                                             const InputIterators& end) {
+  internal::insert_from_triplets_sorted<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>>(
+      begin, end, *this, internal::scalar_sum_op<Scalar, Scalar>());
+}
+
+/** The same as insertFromSortedTriplets but when duplicates are met the functor \a dup_func is applied:
+ * \code
+ * value = dup_func(OldValue, NewValue)
+ * \endcode
+ * Here is a C++11 example keeping the latest entry only:
+ * \code
+ * mat.insertFromSortedTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; });
+ * \endcode
+ */
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename InputIterators, typename DupFunctor>
+void SparseMatrix<Scalar, Options_, StorageIndex_>::insertFromSortedTriplets(const InputIterators& begin,
+                                                                             const InputIterators& end,
+                                                                             DupFunctor dup_func) {
+  internal::insert_from_triplets_sorted<InputIterators, SparseMatrix<Scalar, Options_, StorageIndex_>, DupFunctor>(
+      begin, end, *this, dup_func);
 }
 
 /** \internal */
-template<typename Scalar, int _Options, typename _StorageIndex>
-template<typename DupFunctor>
-void SparseMatrix<Scalar,_Options,_StorageIndex>::collapseDuplicates(DupFunctor dup_func)
-{
-  eigen_assert(!isCompressed());
-  // TODO, in practice we should be able to use m_innerNonZeros for that task
-  IndexVector wi(innerSize());
-  wi.fill(-1);
+template <typename Scalar_, int Options_, typename StorageIndex_>
+template <typename Derived, typename DupFunctor>
+void SparseMatrix<Scalar_, Options_, StorageIndex_>::collapseDuplicates(DenseBase<Derived>& wi, DupFunctor dup_func) {
+  // removes duplicate entries and compresses the matrix
+  // the excess allocated memory is not released
+  // the inner indices do not need to be sorted, nor is the matrix returned in a sorted state
+  eigen_assert(wi.size() == m_innerSize);
+  constexpr StorageIndex kEmptyIndexValue(-1);
+  wi.setConstant(kEmptyIndexValue);
   StorageIndex count = 0;
+  const bool is_compressed = isCompressed();
   // for each inner-vector, wi[inner_index] will hold the position of first element into the index/value buffers
-  for(Index j=0; j<outerSize(); ++j)
-  {
-    StorageIndex start   = count;
-    Index oldEnd  = m_outerIndex[j]+m_innerNonZeros[j];
-    for(Index k=m_outerIndex[j]; k<oldEnd; ++k)
-    {
-      Index i = m_data.index(k);
-      if(wi(i)>=start)
-      {
-        // we already meet this entry => accumulate it
+  for (Index j = 0; j < m_outerSize; ++j) {
+    const StorageIndex newBegin = count;
+    const StorageIndex end = is_compressed ? m_outerIndex[j + 1] : m_outerIndex[j] + m_innerNonZeros[j];
+    for (StorageIndex k = m_outerIndex[j]; k < end; ++k) {
+      StorageIndex i = m_data.index(k);
+      if (wi(i) >= newBegin) {
+        // entry at k is a duplicate
+        // accumulate it into the primary entry located at wi(i)
         m_data.value(wi(i)) = dup_func(m_data.value(wi(i)), m_data.value(k));
-      }
-      else
-      {
+      } else {
+        // k is the primary entry in j with inner index i
+        // shift it to the left and record its location at wi(i)
+        m_data.index(count) = i;
         m_data.value(count) = m_data.value(k);
-        m_data.index(count) = m_data.index(k);
         wi(i) = count;
         ++count;
       }
     }
-    m_outerIndex[j] = start;
+    m_outerIndex[j] = newBegin;
   }
   m_outerIndex[m_outerSize] = count;
+  m_data.resize(count);
 
-  // turn the matrix into compressed form
-  std::free(m_innerNonZeros);
+  // turn the matrix into compressed form (if it is not already)
+  internal::conditional_aligned_delete_auto<StorageIndex, true>(m_innerNonZeros, m_outerSize);
   m_innerNonZeros = 0;
-  m_data.resize(m_outerIndex[m_outerSize]);
 }
 
-template<typename Scalar, int _Options, typename _StorageIndex>
-template<typename OtherDerived>
-EIGEN_DONT_INLINE SparseMatrix<Scalar,_Options,_StorageIndex>& SparseMatrix<Scalar,_Options,_StorageIndex>::operator=(const SparseMatrixBase<OtherDerived>& other)
-{
-  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
-        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+/** \internal */
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename OtherDerived>
+EIGEN_DONT_INLINE SparseMatrix<Scalar, Options_, StorageIndex_>&
+SparseMatrix<Scalar, Options_, StorageIndex_>::operator=(const SparseMatrixBase<OtherDerived>& other) {
+  EIGEN_STATIC_ASSERT(
+      (internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+      YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
 
-  #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-    EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-  #endif
-      
+#ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+  EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+#endif
+
   const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator<OtherDerived>::Flags & RowMajorBit);
-  if (needToTranspose)
-  {
-    #ifdef EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
-      EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
-    #endif
+  if (needToTranspose) {
+#ifdef EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
+    EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
+#endif
     // two passes algorithm:
     //  1 - compute the number of coeffs per dest inner vector
     //  2 - do the actual copy/eval
     // Since each coeff of the rhs has to be evaluated twice, let's evaluate it if needed
-    typedef typename internal::nested_eval<OtherDerived,2,typename internal::plain_matrix_type<OtherDerived>::type >::type OtherCopy;
-    typedef typename internal::remove_all<OtherCopy>::type _OtherCopy;
-    typedef internal::evaluator<_OtherCopy> OtherCopyEval;
+    typedef
+        typename internal::nested_eval<OtherDerived, 2, typename internal::plain_matrix_type<OtherDerived>::type>::type
+            OtherCopy;
+    typedef internal::remove_all_t<OtherCopy> OtherCopy_;
+    typedef internal::evaluator<OtherCopy_> OtherCopyEval;
     OtherCopy otherCopy(other.derived());
     OtherCopyEval otherCopyEval(otherCopy);
 
-    SparseMatrix dest(other.rows(),other.cols());
-    Eigen::Map<IndexVector> (dest.m_outerIndex,dest.outerSize()).setZero();
+    SparseMatrix dest(other.rows(), other.cols());
+    Eigen::Map<IndexVector>(dest.m_outerIndex, dest.outerSize()).setZero();
 
     // pass 1
     // FIXME the above copy could be merged with that pass
-    for (Index j=0; j<otherCopy.outerSize(); ++j)
-      for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it)
-        ++dest.m_outerIndex[it.index()];
+    for (Index j = 0; j < otherCopy.outerSize(); ++j)
+      for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it) ++dest.m_outerIndex[it.index()];
 
     // prefix sum
     StorageIndex count = 0;
     IndexVector positions(dest.outerSize());
-    for (Index j=0; j<dest.outerSize(); ++j)
-    {
+    for (Index j = 0; j < dest.outerSize(); ++j) {
       StorageIndex tmp = dest.m_outerIndex[j];
       dest.m_outerIndex[j] = count;
       positions[j] = count;
@@ -1217,10 +1517,8 @@
     // alloc
     dest.m_data.resize(count);
     // pass 2
-    for (StorageIndex j=0; j<otherCopy.outerSize(); ++j)
-    {
-      for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it)
-      {
+    for (StorageIndex j = 0; j < otherCopy.outerSize(); ++j) {
+      for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it) {
         Index pos = positions[it.index()]++;
         dest.m_data.index(pos) = j;
         dest.m_data.value(pos) = it.value();
@@ -1228,11 +1526,8 @@
     }
     this->swap(dest);
     return *this;
-  }
-  else
-  {
-    if(other.isRValue())
-    {
+  } else {
+    if (other.isRValue()) {
       initAssignment(other.derived());
     }
     // there is no special optimization
@@ -1240,279 +1535,294 @@
   }
 }
 
-template<typename _Scalar, int _Options, typename _StorageIndex>
-typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insert(Index row, Index col)
-{
-  eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
-  
-  const Index outer = IsRowMajor ? row : col;
-  const Index inner = IsRowMajor ? col : row;
-  
-  if(isCompressed())
-  {
-    if(nonZeros()==0)
-    {
-      // reserve space if not already done
-      if(m_data.allocatedSize()==0)
-        m_data.reserve(2*m_innerSize);
-      
-      // turn the matrix into non-compressed mode
-      m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
-      if(!m_innerNonZeros) internal::throw_std_bad_alloc();
-      
-      memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex));
-      
-      // pack all inner-vectors to the end of the pre-allocated space
-      // and allocate the entire free-space to the first inner-vector
-      StorageIndex end = convert_index(m_data.allocatedSize());
-      for(Index j=1; j<=m_outerSize; ++j)
-        m_outerIndex[j] = end;
-    }
-    else
-    {
-      // turn the matrix into non-compressed mode
-      m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
-      if(!m_innerNonZeros) internal::throw_std_bad_alloc();
-      for(Index j=0; j<m_outerSize; ++j)
-        m_innerNonZeros[j] = m_outerIndex[j+1]-m_outerIndex[j];
-    }
-  }
-  
-  // check whether we can do a fast "push back" insertion
-  Index data_end = m_data.allocatedSize();
-  
-  // First case: we are filling a new inner vector which is packed at the end.
-  // We assume that all remaining inner-vectors are also empty and packed to the end.
-  if(m_outerIndex[outer]==data_end)
-  {
-    eigen_internal_assert(m_innerNonZeros[outer]==0);
-    
-    // pack previous empty inner-vectors to end of the used-space
-    // and allocate the entire free-space to the current inner-vector.
-    StorageIndex p = convert_index(m_data.size());
-    Index j = outer;
-    while(j>=0 && m_innerNonZeros[j]==0)
-      m_outerIndex[j--] = p;
-    
-    // push back the new element
-    ++m_innerNonZeros[outer];
-    m_data.append(Scalar(0), inner);
-    
-    // check for reallocation
-    if(data_end != m_data.allocatedSize())
-    {
-      // m_data has been reallocated
-      //  -> move remaining inner-vectors back to the end of the free-space
-      //     so that the entire free-space is allocated to the current inner-vector.
-      eigen_internal_assert(data_end < m_data.allocatedSize());
-      StorageIndex new_end = convert_index(m_data.allocatedSize());
-      for(Index k=outer+1; k<=m_outerSize; ++k)
-        if(m_outerIndex[k]==data_end)
-          m_outerIndex[k] = new_end;
-    }
-    return m_data.value(p);
-  }
-  
-  // Second case: the next inner-vector is packed to the end
-  // and the current inner-vector end match the used-space.
-  if(m_outerIndex[outer+1]==data_end && m_outerIndex[outer]+m_innerNonZeros[outer]==m_data.size())
-  {
-    eigen_internal_assert(outer+1==m_outerSize || m_innerNonZeros[outer+1]==0);
-    
-    // add space for the new element
-    ++m_innerNonZeros[outer];
-    m_data.resize(m_data.size()+1);
-    
-    // check for reallocation
-    if(data_end != m_data.allocatedSize())
-    {
-      // m_data has been reallocated
-      //  -> move remaining inner-vectors back to the end of the free-space
-      //     so that the entire free-space is allocated to the current inner-vector.
-      eigen_internal_assert(data_end < m_data.allocatedSize());
-      StorageIndex new_end = convert_index(m_data.allocatedSize());
-      for(Index k=outer+1; k<=m_outerSize; ++k)
-        if(m_outerIndex[k]==data_end)
-          m_outerIndex[k] = new_end;
-    }
-    
-    // and insert it at the right position (sorted insertion)
-    Index startId = m_outerIndex[outer];
-    Index p = m_outerIndex[outer]+m_innerNonZeros[outer]-1;
-    while ( (p > startId) && (m_data.index(p-1) > inner) )
-    {
-      m_data.index(p) = m_data.index(p-1);
-      m_data.value(p) = m_data.value(p-1);
-      --p;
-    }
-    
-    m_data.index(p) = convert_index(inner);
-    return (m_data.value(p) = Scalar(0));
-  }
-  
-  if(m_data.size() != m_data.allocatedSize())
-  {
-    // make sure the matrix is compatible to random un-compressed insertion:
-    m_data.resize(m_data.allocatedSize());
-    this->reserveInnerVectors(Array<StorageIndex,Dynamic,1>::Constant(m_outerSize, 2));
-  }
-  
-  return insertUncompressed(row,col);
+template <typename Scalar_, int Options_, typename StorageIndex_>
+inline typename SparseMatrix<Scalar_, Options_, StorageIndex_>::Scalar&
+SparseMatrix<Scalar_, Options_, StorageIndex_>::insert(Index row, Index col) {
+  return insertByOuterInner(IsRowMajor ? row : col, IsRowMajor ? col : row);
 }
-    
-template<typename _Scalar, int _Options, typename _StorageIndex>
-EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertUncompressed(Index row, Index col)
-{
+
+template <typename Scalar_, int Options_, typename StorageIndex_>
+EIGEN_STRONG_INLINE typename SparseMatrix<Scalar_, Options_, StorageIndex_>::Scalar&
+SparseMatrix<Scalar_, Options_, StorageIndex_>::insertAtByOuterInner(Index outer, Index inner, Index dst) {
+  // random insertion into compressed matrix is very slow
+  uncompress();
+  return insertUncompressedAtByOuterInner(outer, inner, dst);
+}
+
+template <typename Scalar_, int Options_, typename StorageIndex_>
+EIGEN_DEPRECATED EIGEN_DONT_INLINE typename SparseMatrix<Scalar_, Options_, StorageIndex_>::Scalar&
+SparseMatrix<Scalar_, Options_, StorageIndex_>::insertUncompressed(Index row, Index col) {
   eigen_assert(!isCompressed());
-
-  const Index outer = IsRowMajor ? row : col;
-  const StorageIndex inner = convert_index(IsRowMajor ? col : row);
-
-  Index room = m_outerIndex[outer+1] - m_outerIndex[outer];
-  StorageIndex innerNNZ = m_innerNonZeros[outer];
-  if(innerNNZ>=room)
-  {
-    // this inner vector is full, we need to reallocate the whole buffer :(
-    reserve(SingletonVector(outer,std::max<StorageIndex>(2,innerNNZ)));
+  Index outer = IsRowMajor ? row : col;
+  Index inner = IsRowMajor ? col : row;
+  Index start = m_outerIndex[outer];
+  Index end = start + m_innerNonZeros[outer];
+  Index dst = start == end ? end : m_data.searchLowerIndex(start, end, inner);
+  if (dst == end) {
+    Index capacity = m_outerIndex[outer + 1] - end;
+    if (capacity > 0) {
+      // implies uncompressed: push to back of vector
+      m_innerNonZeros[outer]++;
+      m_data.index(end) = StorageIndex(inner);
+      m_data.value(end) = Scalar(0);
+      return m_data.value(end);
+    }
   }
-
-  Index startId = m_outerIndex[outer];
-  Index p = startId + m_innerNonZeros[outer];
-  while ( (p > startId) && (m_data.index(p-1) > inner) )
-  {
-    m_data.index(p) = m_data.index(p-1);
-    m_data.value(p) = m_data.value(p-1);
-    --p;
-  }
-  eigen_assert((p<=startId || m_data.index(p-1)!=inner) && "you cannot insert an element that already exists, you must call coeffRef to this end");
-
-  m_innerNonZeros[outer]++;
-
-  m_data.index(p) = inner;
-  return (m_data.value(p) = Scalar(0));
+  eigen_assert((dst == end || m_data.index(dst) != inner) &&
+               "you cannot insert an element that already exists, you must call coeffRef to this end");
+  return insertUncompressedAtByOuterInner(outer, inner, dst);
 }
 
-template<typename _Scalar, int _Options, typename _StorageIndex>
-EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertCompressed(Index row, Index col)
-{
+template <typename Scalar_, int Options_, typename StorageIndex_>
+EIGEN_DEPRECATED EIGEN_DONT_INLINE typename SparseMatrix<Scalar_, Options_, StorageIndex_>::Scalar&
+SparseMatrix<Scalar_, Options_, StorageIndex_>::insertCompressed(Index row, Index col) {
   eigen_assert(isCompressed());
+  Index outer = IsRowMajor ? row : col;
+  Index inner = IsRowMajor ? col : row;
+  Index start = m_outerIndex[outer];
+  Index end = m_outerIndex[outer + 1];
+  Index dst = start == end ? end : m_data.searchLowerIndex(start, end, inner);
+  eigen_assert((dst == end || m_data.index(dst) != inner) &&
+               "you cannot insert an element that already exists, you must call coeffRef to this end");
+  return insertCompressedAtByOuterInner(outer, inner, dst);
+}
 
-  const Index outer = IsRowMajor ? row : col;
-  const Index inner = IsRowMajor ? col : row;
-
-  Index previousOuter = outer;
-  if (m_outerIndex[outer+1]==0)
-  {
-    // we start a new inner vector
-    while (previousOuter>=0 && m_outerIndex[previousOuter]==0)
-    {
-      m_outerIndex[previousOuter] = convert_index(m_data.size());
-      --previousOuter;
-    }
-    m_outerIndex[outer+1] = m_outerIndex[outer];
+template <typename Scalar_, int Options_, typename StorageIndex_>
+typename SparseMatrix<Scalar_, Options_, StorageIndex_>::Scalar&
+SparseMatrix<Scalar_, Options_, StorageIndex_>::insertCompressedAtByOuterInner(Index outer, Index inner, Index dst) {
+  eigen_assert(isCompressed());
+  // compressed insertion always requires expanding the buffer
+  // first, check if there is adequate allocated memory
+  if (m_data.allocatedSize() <= m_data.size()) {
+    // if there is no capacity for a single insertion, double the capacity
+    // increase capacity by a mininum of 32
+    Index minReserve = 32;
+    Index reserveSize = numext::maxi(minReserve, m_data.allocatedSize());
+    m_data.reserve(reserveSize);
   }
+  m_data.resize(m_data.size() + 1);
+  Index chunkSize = m_outerIndex[m_outerSize] - dst;
+  // shift the existing data to the right if necessary
+  m_data.moveChunk(dst, dst + 1, chunkSize);
+  // update nonzero counts
+  // potentially O(outerSize) bottleneck!
+  for (Index j = outer; j < m_outerSize; j++) m_outerIndex[j + 1]++;
+  // initialize the coefficient
+  m_data.index(dst) = StorageIndex(inner);
+  m_data.value(dst) = Scalar(0);
+  // return a reference to the coefficient
+  return m_data.value(dst);
+}
 
-  // here we have to handle the tricky case where the outerIndex array
-  // starts with: [ 0 0 0 0 0 1 ...] and we are inserted in, e.g.,
-  // the 2nd inner vector...
-  bool isLastVec = (!(previousOuter==-1 && m_data.size()!=0))
-                && (std::size_t(m_outerIndex[outer+1]) == m_data.size());
-
-  std::size_t startId = m_outerIndex[outer];
-  // FIXME let's make sure sizeof(long int) == sizeof(std::size_t)
-  std::size_t p = m_outerIndex[outer+1];
-  ++m_outerIndex[outer+1];
-
-  double reallocRatio = 1;
-  if (m_data.allocatedSize()<=m_data.size())
-  {
-    // if there is no preallocated memory, let's reserve a minimum of 32 elements
-    if (m_data.size()==0)
-    {
-      m_data.reserve(32);
-    }
-    else
-    {
-      // we need to reallocate the data, to reduce multiple reallocations
-      // we use a smart resize algorithm based on the current filling ratio
-      // in addition, we use double to avoid integers overflows
-      double nnzEstimate = double(m_outerIndex[outer])*double(m_outerSize)/double(outer+1);
-      reallocRatio = (nnzEstimate-double(m_data.size()))/double(m_data.size());
-      // furthermore we bound the realloc ratio to:
-      //   1) reduce multiple minor realloc when the matrix is almost filled
-      //   2) avoid to allocate too much memory when the matrix is almost empty
-      reallocRatio = (std::min)((std::max)(reallocRatio,1.5),8.);
-    }
-  }
-  m_data.resize(m_data.size()+1,reallocRatio);
-
-  if (!isLastVec)
-  {
-    if (previousOuter==-1)
-    {
-      // oops wrong guess.
-      // let's correct the outer offsets
-      for (Index k=0; k<=(outer+1); ++k)
-        m_outerIndex[k] = 0;
-      Index k=outer+1;
-      while(m_outerIndex[k]==0)
-        m_outerIndex[k++] = 1;
-      while (k<=m_outerSize && m_outerIndex[k]!=0)
-        m_outerIndex[k++]++;
-      p = 0;
-      --k;
-      k = m_outerIndex[k]-1;
-      while (k>0)
-      {
-        m_data.index(k) = m_data.index(k-1);
-        m_data.value(k) = m_data.value(k-1);
-        k--;
+template <typename Scalar_, int Options_, typename StorageIndex_>
+typename SparseMatrix<Scalar_, Options_, StorageIndex_>::Scalar&
+SparseMatrix<Scalar_, Options_, StorageIndex_>::insertUncompressedAtByOuterInner(Index outer, Index inner, Index dst) {
+  eigen_assert(!isCompressed());
+  // find a vector with capacity, starting at `outer` and searching to the left and right
+  for (Index leftTarget = outer - 1, rightTarget = outer; (leftTarget >= 0) || (rightTarget < m_outerSize);) {
+    if (rightTarget < m_outerSize) {
+      Index start = m_outerIndex[rightTarget];
+      Index end = start + m_innerNonZeros[rightTarget];
+      Index nextStart = m_outerIndex[rightTarget + 1];
+      Index capacity = nextStart - end;
+      if (capacity > 0) {
+        // move [dst, end) to dst+1 and insert at dst
+        Index chunkSize = end - dst;
+        if (chunkSize > 0) m_data.moveChunk(dst, dst + 1, chunkSize);
+        m_innerNonZeros[outer]++;
+        for (Index j = outer; j < rightTarget; j++) m_outerIndex[j + 1]++;
+        m_data.index(dst) = StorageIndex(inner);
+        m_data.value(dst) = Scalar(0);
+        return m_data.value(dst);
       }
+      rightTarget++;
     }
-    else
-    {
-      // we are not inserting into the last inner vec
-      // update outer indices:
-      Index j = outer+2;
-      while (j<=m_outerSize && m_outerIndex[j]!=0)
-        m_outerIndex[j++]++;
-      --j;
-      // shift data of last vecs:
-      Index k = m_outerIndex[j]-1;
-      while (k>=Index(p))
-      {
-        m_data.index(k) = m_data.index(k-1);
-        m_data.value(k) = m_data.value(k-1);
-        k--;
+    if (leftTarget >= 0) {
+      Index start = m_outerIndex[leftTarget];
+      Index end = start + m_innerNonZeros[leftTarget];
+      Index nextStart = m_outerIndex[leftTarget + 1];
+      Index capacity = nextStart - end;
+      if (capacity > 0) {
+        // tricky: dst is a lower bound, so we must insert at dst-1 when shifting left
+        // move [nextStart, dst) to nextStart-1 and insert at dst-1
+        Index chunkSize = dst - nextStart;
+        if (chunkSize > 0) m_data.moveChunk(nextStart, nextStart - 1, chunkSize);
+        m_innerNonZeros[outer]++;
+        for (Index j = leftTarget; j < outer; j++) m_outerIndex[j + 1]--;
+        m_data.index(dst - 1) = StorageIndex(inner);
+        m_data.value(dst - 1) = Scalar(0);
+        return m_data.value(dst - 1);
       }
+      leftTarget--;
     }
   }
 
-  while ( (p > startId) && (m_data.index(p-1) > inner) )
-  {
-    m_data.index(p) = m_data.index(p-1);
-    m_data.value(p) = m_data.value(p-1);
-    --p;
+  // no room for interior insertion
+  // nonZeros() == m_data.size()
+  // record offset as outerIndxPtr will change
+  Index dst_offset = dst - m_outerIndex[outer];
+  // allocate space for random insertion
+  if (m_data.allocatedSize() == 0) {
+    // fast method to allocate space for one element per vector in empty matrix
+    m_data.resize(m_outerSize);
+    std::iota(m_outerIndex, m_outerIndex + m_outerSize + 1, StorageIndex(0));
+  } else {
+    // check for integer overflow: if maxReserveSize == 0, insertion is not possible
+    Index maxReserveSize = static_cast<Index>(NumTraits<StorageIndex>::highest()) - m_data.allocatedSize();
+    eigen_assert(maxReserveSize > 0);
+    if (m_outerSize <= maxReserveSize) {
+      // allocate space for one additional element per vector
+      reserveInnerVectors(IndexVector::Constant(m_outerSize, 1));
+    } else {
+      // handle the edge case where StorageIndex is insufficient to reserve outerSize additional elements
+      // allocate space for one additional element in the interval [outer,maxReserveSize)
+      typedef internal::sparse_reserve_op<StorageIndex> ReserveSizesOp;
+      typedef CwiseNullaryOp<ReserveSizesOp, IndexVector> ReserveSizesXpr;
+      ReserveSizesXpr reserveSizesXpr(m_outerSize, 1, ReserveSizesOp(outer, m_outerSize, maxReserveSize));
+      reserveInnerVectors(reserveSizesXpr);
+    }
   }
-
-  m_data.index(p) = inner;
-  return (m_data.value(p) = Scalar(0));
+  // insert element at `dst` with new outer indices
+  Index start = m_outerIndex[outer];
+  Index end = start + m_innerNonZeros[outer];
+  Index new_dst = start + dst_offset;
+  Index chunkSize = end - new_dst;
+  if (chunkSize > 0) m_data.moveChunk(new_dst, new_dst + 1, chunkSize);
+  m_innerNonZeros[outer]++;
+  m_data.index(new_dst) = StorageIndex(inner);
+  m_data.value(new_dst) = Scalar(0);
+  return m_data.value(new_dst);
 }
 
 namespace internal {
 
-template<typename _Scalar, int _Options, typename _StorageIndex>
-struct evaluator<SparseMatrix<_Scalar,_Options,_StorageIndex> >
-  : evaluator<SparseCompressedBase<SparseMatrix<_Scalar,_Options,_StorageIndex> > >
-{
-  typedef evaluator<SparseCompressedBase<SparseMatrix<_Scalar,_Options,_StorageIndex> > > Base;
-  typedef SparseMatrix<_Scalar,_Options,_StorageIndex> SparseMatrixType;
+template <typename Scalar_, int Options_, typename StorageIndex_>
+struct evaluator<SparseMatrix<Scalar_, Options_, StorageIndex_>>
+    : evaluator<SparseCompressedBase<SparseMatrix<Scalar_, Options_, StorageIndex_>>> {
+  typedef evaluator<SparseCompressedBase<SparseMatrix<Scalar_, Options_, StorageIndex_>>> Base;
+  typedef SparseMatrix<Scalar_, Options_, StorageIndex_> SparseMatrixType;
   evaluator() : Base() {}
-  explicit evaluator(const SparseMatrixType &mat) : Base(mat) {}
+  explicit evaluator(const SparseMatrixType& mat) : Base(mat) {}
 };
 
-}
+}  // namespace internal
 
-} // end namespace Eigen
+// Specialization for SparseMatrix.
+// Serializes [rows, cols, isCompressed, outerSize, innerBufferSize,
+// innerNonZeros, outerIndices, innerIndices, values].
+template <typename Scalar, int Options, typename StorageIndex>
+class Serializer<SparseMatrix<Scalar, Options, StorageIndex>, void> {
+ public:
+  typedef SparseMatrix<Scalar, Options, StorageIndex> SparseMat;
 
-#endif // EIGEN_SPARSEMATRIX_H
+  struct Header {
+    typename SparseMat::Index rows;
+    typename SparseMat::Index cols;
+    bool compressed;
+    Index outer_size;
+    Index inner_buffer_size;
+  };
+
+  EIGEN_DEVICE_FUNC size_t size(const SparseMat& value) const {
+    // innerNonZeros.
+    std::size_t num_storage_indices = value.isCompressed() ? 0 : value.outerSize();
+    // Outer indices.
+    num_storage_indices += value.outerSize() + 1;
+    // Inner indices.
+    const StorageIndex inner_buffer_size = value.outerIndexPtr()[value.outerSize()];
+    num_storage_indices += inner_buffer_size;
+    // Values.
+    std::size_t num_values = inner_buffer_size;
+    return sizeof(Header) + sizeof(Scalar) * num_values + sizeof(StorageIndex) * num_storage_indices;
+  }
+
+  EIGEN_DEVICE_FUNC uint8_t* serialize(uint8_t* dest, uint8_t* end, const SparseMat& value) {
+    if (EIGEN_PREDICT_FALSE(dest == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(dest + size(value) > end)) return nullptr;
+
+    const size_t header_bytes = sizeof(Header);
+    Header header = {value.rows(), value.cols(), value.isCompressed(), value.outerSize(),
+                     value.outerIndexPtr()[value.outerSize()]};
+    EIGEN_USING_STD(memcpy)
+    memcpy(dest, &header, header_bytes);
+    dest += header_bytes;
+
+    // innerNonZeros.
+    if (!header.compressed) {
+      std::size_t data_bytes = sizeof(StorageIndex) * header.outer_size;
+      memcpy(dest, value.innerNonZeroPtr(), data_bytes);
+      dest += data_bytes;
+    }
+
+    // Outer indices.
+    std::size_t data_bytes = sizeof(StorageIndex) * (header.outer_size + 1);
+    memcpy(dest, value.outerIndexPtr(), data_bytes);
+    dest += data_bytes;
+
+    // Inner indices.
+    data_bytes = sizeof(StorageIndex) * header.inner_buffer_size;
+    memcpy(dest, value.innerIndexPtr(), data_bytes);
+    dest += data_bytes;
+
+    // Values.
+    data_bytes = sizeof(Scalar) * header.inner_buffer_size;
+    memcpy(dest, value.valuePtr(), data_bytes);
+    dest += data_bytes;
+
+    return dest;
+  }
+
+  EIGEN_DEVICE_FUNC const uint8_t* deserialize(const uint8_t* src, const uint8_t* end, SparseMat& value) const {
+    if (EIGEN_PREDICT_FALSE(src == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(src + sizeof(Header) > end)) return nullptr;
+
+    const size_t header_bytes = sizeof(Header);
+    Header header;
+    EIGEN_USING_STD(memcpy)
+    memcpy(&header, src, header_bytes);
+    src += header_bytes;
+
+    value.setZero();
+    value.resize(header.rows, header.cols);
+    if (header.compressed) {
+      value.makeCompressed();
+    } else {
+      value.uncompress();
+    }
+
+    // Adjust value ptr size.
+    value.data().resize(header.inner_buffer_size);
+
+    // Initialize compressed state and inner non-zeros.
+    if (!header.compressed) {
+      // Inner non-zero counts.
+      std::size_t data_bytes = sizeof(StorageIndex) * header.outer_size;
+      if (EIGEN_PREDICT_FALSE(src + data_bytes > end)) return nullptr;
+      memcpy(value.innerNonZeroPtr(), src, data_bytes);
+      src += data_bytes;
+    }
+
+    // Outer indices.
+    std::size_t data_bytes = sizeof(StorageIndex) * (header.outer_size + 1);
+    if (EIGEN_PREDICT_FALSE(src + data_bytes > end)) return nullptr;
+    memcpy(value.outerIndexPtr(), src, data_bytes);
+    src += data_bytes;
+
+    // Inner indices.
+    data_bytes = sizeof(StorageIndex) * header.inner_buffer_size;
+    if (EIGEN_PREDICT_FALSE(src + data_bytes > end)) return nullptr;
+    memcpy(value.innerIndexPtr(), src, data_bytes);
+    src += data_bytes;
+
+    // Values.
+    data_bytes = sizeof(Scalar) * header.inner_buffer_size;
+    if (EIGEN_PREDICT_FALSE(src + data_bytes > end)) return nullptr;
+    memcpy(value.valuePtr(), src, data_bytes);
+    src += data_bytes;
+    return src;
+  }
+};
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_SPARSEMATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h
index 229449f..b58bb38 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h
@@ -10,389 +10,391 @@
 #ifndef EIGEN_SPARSEMATRIXBASE_H
 #define EIGEN_SPARSEMATRIXBASE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \ingroup SparseCore_Module
-  *
-  * \class SparseMatrixBase
-  *
-  * \brief Base class of any sparse matrices or sparse expressions
-  *
-  * \tparam Derived is the derived type, e.g. a sparse matrix type, or an expression, etc.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIXBASE_PLUGIN.
-  */
-template<typename Derived> class SparseMatrixBase
-  : public EigenBase<Derived>
-{
-  public:
+ *
+ * \class SparseMatrixBase
+ *
+ * \brief Base class of any sparse matrices or sparse expressions
+ *
+ * \tparam Derived is the derived type, e.g. a sparse matrix type, or an expression, etc.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIXBASE_PLUGIN.
+ */
+template <typename Derived>
+class SparseMatrixBase : public EigenBase<Derived> {
+ public:
+  typedef typename internal::traits<Derived>::Scalar Scalar;
 
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    
-    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
-      *
-      * It is an alias for the Scalar type */
-    typedef Scalar value_type;
-    
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+  /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
+   *
+   * It is an alias for the Scalar type */
+  typedef Scalar value_type;
 
-    /** The integer type used to \b store indices within a SparseMatrix.
-      * For a \c SparseMatrix<Scalar,Options,IndexType> it an alias of the third template parameter \c IndexType. */
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+  typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+  typedef typename internal::traits<Derived>::StorageKind StorageKind;
 
-    typedef typename internal::add_const_on_value_type_if_arithmetic<
-                         typename internal::packet_traits<Scalar>::type
-                     >::type PacketReturnType;
+  /** The integer type used to \b store indices within a SparseMatrix.
+   * For a \c SparseMatrix<Scalar,Options,IndexType> it an alias of the third template parameter \c IndexType. */
+  typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
 
-    typedef SparseMatrixBase StorageBaseType;
+  typedef typename internal::add_const_on_value_type_if_arithmetic<typename internal::packet_traits<Scalar>::type>::type
+      PacketReturnType;
 
-    typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
-    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
-    
-    template<typename OtherDerived>
-    Derived& operator=(const EigenBase<OtherDerived> &other);
+  typedef SparseMatrixBase StorageBaseType;
 
-    enum {
+  typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+  typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
 
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-        /**< The number of rows at compile-time. This is just a copy of the value provided
-          * by the \a Derived type. If a value is not known at compile-time,
-          * it is set to the \a Dynamic constant.
-          * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+  template <typename OtherDerived>
+  Derived& operator=(const EigenBase<OtherDerived>& other);
 
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-        /**< The number of columns at compile-time. This is just a copy of the value provided
-          * by the \a Derived type. If a value is not known at compile-time,
-          * it is set to the \a Dynamic constant.
-          * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+  enum {
 
+    RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+    /**< The number of rows at compile-time. This is just a copy of the value provided
+     * by the \a Derived type. If a value is not known at compile-time,
+     * it is set to the \a Dynamic constant.
+     * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
 
-      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
-                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
-        /**< This is equal to the number of coefficients, i.e. the number of
-          * rows times the number of columns, or to \a Dynamic if this is not
-          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+    ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+    /**< The number of columns at compile-time. This is just a copy of the value provided
+     * by the \a Derived type. If a value is not known at compile-time,
+     * it is set to the \a Dynamic constant.
+     * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
 
-      MaxRowsAtCompileTime = RowsAtCompileTime,
-      MaxColsAtCompileTime = ColsAtCompileTime,
+    SizeAtCompileTime = (internal::size_of_xpr_at_compile_time<Derived>::ret),
+    /**< This is equal to the number of coefficients, i.e. the number of
+     * rows times the number of columns, or to \a Dynamic if this is not
+     * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
 
-      MaxSizeAtCompileTime = (internal::size_at_compile_time<MaxRowsAtCompileTime,
-                                                      MaxColsAtCompileTime>::ret),
+    MaxRowsAtCompileTime = RowsAtCompileTime,
+    MaxColsAtCompileTime = ColsAtCompileTime,
 
-      IsVectorAtCompileTime = RowsAtCompileTime == 1 || ColsAtCompileTime == 1,
-        /**< This is set to true if either the number of rows or the number of
-          * columns is known at compile-time to be equal to 1. Indeed, in that case,
-          * we are dealing with a column-vector (if there is only one column) or with
-          * a row-vector (if there is only one row). */
+    MaxSizeAtCompileTime = internal::size_at_compile_time(MaxRowsAtCompileTime, MaxColsAtCompileTime),
 
-      NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2,
-        /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors,
-         * and 2 for matrices.
-         */
+    IsVectorAtCompileTime = RowsAtCompileTime == 1 || ColsAtCompileTime == 1,
+    /**< This is set to true if either the number of rows or the number of
+     * columns is known at compile-time to be equal to 1. Indeed, in that case,
+     * we are dealing with a column-vector (if there is only one column) or with
+     * a row-vector (if there is only one row). */
 
-      Flags = internal::traits<Derived>::Flags,
-        /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
-          * constructed from this one. See the \ref flags "list of flags".
-          */
+    NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0
+                    : bool(IsVectorAtCompileTime)  ? 1
+                                                   : 2,
+    /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors,
+     * and 2 for matrices.
+     */
 
-      IsRowMajor = Flags&RowMajorBit ? 1 : 0,
-      
-      InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
-                             : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+    Flags = internal::traits<Derived>::Flags,
+    /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
+     * constructed from this one. See the \ref flags "list of flags".
+     */
 
-      #ifndef EIGEN_PARSED_BY_DOXYGEN
-      _HasDirectAccess = (int(Flags)&DirectAccessBit) ? 1 : 0 // workaround sunCC
-      #endif
-    };
+    IsRowMajor = Flags & RowMajorBit ? 1 : 0,
 
-    /** \internal the return type of MatrixBase::adjoint() */
-    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-                        CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, Eigen::Transpose<const Derived> >,
-                        Transpose<const Derived>
-                     >::type AdjointReturnType;
-    typedef Transpose<Derived> TransposeReturnType;
-    typedef typename internal::add_const<Transpose<const Derived> >::type ConstTransposeReturnType;
-
-    // FIXME storage order do not match evaluator storage order
-    typedef SparseMatrix<Scalar, Flags&RowMajorBit ? RowMajor : ColMajor, StorageIndex> PlainObject;
+    InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
+                             : int(IsRowMajor)          ? int(ColsAtCompileTime)
+                                                        : int(RowsAtCompileTime),
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is the "real scalar" type; if the \a Scalar type is already real numbers
-      * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If
-      * \a Scalar is \a std::complex<T> then RealScalar is \a T.
-      *
-      * \sa class NumTraits
-      */
-    typedef typename NumTraits<Scalar>::Real RealScalar;
+    HasDirectAccess_ = (int(Flags) & DirectAccessBit) ? 1 : 0  // workaround sunCC
+#endif
+  };
 
-    /** \internal the return type of coeff()
-      */
-    typedef typename internal::conditional<_HasDirectAccess, const Scalar&, Scalar>::type CoeffReturnType;
+  /** \internal the return type of MatrixBase::adjoint() */
+  typedef std::conditional_t<NumTraits<Scalar>::IsComplex,
+                             CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, Eigen::Transpose<const Derived> >,
+                             Transpose<const Derived> >
+      AdjointReturnType;
+  typedef Transpose<Derived> TransposeReturnType;
+  typedef Transpose<const Derived> ConstTransposeReturnType;
 
-    /** \internal Represents a matrix with all coefficients equal to one another*/
-    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,Matrix<Scalar,Dynamic,Dynamic> > ConstantReturnType;
+  // FIXME storage order do not match evaluator storage order
+  typedef SparseMatrix<Scalar, Flags & RowMajorBit ? RowMajor : ColMajor, StorageIndex> PlainObject;
 
-    /** type of the equivalent dense matrix */
-    typedef Matrix<Scalar,RowsAtCompileTime,ColsAtCompileTime> DenseMatrixType;
-    /** type of the equivalent square matrix */
-    typedef Matrix<Scalar,EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime),
-                          EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** This is the "real scalar" type; if the \a Scalar type is already real numbers
+   * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If
+   * \a Scalar is \a std::complex<T> then RealScalar is \a T.
+   *
+   * \sa class NumTraits
+   */
+  typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
-    inline Derived& derived() { return *static_cast<Derived*>(this); }
-    inline Derived& const_cast_derived() const
-    { return *static_cast<Derived*>(const_cast<SparseMatrixBase*>(this)); }
+  /** \internal the return type of coeff()
+   */
+  typedef std::conditional_t<HasDirectAccess_, const Scalar&, Scalar> CoeffReturnType;
 
-    typedef EigenBase<Derived> Base;
+  /** \internal Represents a matrix with all coefficients equal to one another*/
+  typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>, Matrix<Scalar, Dynamic, Dynamic> > ConstantReturnType;
 
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+  /** type of the equivalent dense matrix */
+  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> DenseMatrixType;
+  /** type of the equivalent square matrix */
+  typedef Matrix<Scalar, internal::max_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime),
+                 internal::max_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime)>
+      SquareMatrixType;
+
+  inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+  inline Derived& derived() { return *static_cast<Derived*>(this); }
+  inline Derived& const_cast_derived() const { return *static_cast<Derived*>(const_cast<SparseMatrixBase*>(this)); }
+
+  typedef EigenBase<Derived> Base;
+
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
 
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::SparseMatrixBase
 #ifdef EIGEN_PARSED_BY_DOXYGEN
-#define EIGEN_DOC_UNARY_ADDONS(METHOD,OP)           /** <p>This method does not change the sparsity of \c *this: the OP is applied to explicitly stored coefficients only. \sa SparseCompressedBase::coeffs() </p> */
-#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL      /** <p> \warning This method returns a read-only expression for any sparse matrices. \sa \ref TutorialSparse_SubMatrices "Sparse block operations" </p> */
-#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND) /** <p> \warning This method returns a read-write expression for COND sparse matrices only. Otherwise, the returned expression is read-only. \sa \ref TutorialSparse_SubMatrices "Sparse block operations" </p> */
+#define EIGEN_DOC_UNARY_ADDONS(METHOD,                                                                               \
+                               OP) /** <p>This method does not change the sparsity of \c *this: the OP is applied to \
+                                      explicitly stored coefficients only. \sa SparseCompressedBase::coeffs() </p> */
+#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL /** <p> \warning This method returns a read-only expression for any   \
+                                                  sparse matrices. \sa \ref TutorialSparse_SubMatrices "Sparse block \
+                                                  operations" </p> */
+#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(                                                                       \
+    COND) /** <p> \warning This method returns a read-write expression for COND sparse matrices only. Otherwise, the \
+             returned expression is read-only. \sa \ref TutorialSparse_SubMatrices "Sparse block operations" </p> */
 #else
-#define EIGEN_DOC_UNARY_ADDONS(X,Y)
+#define EIGEN_DOC_UNARY_ADDONS(X, Y)
 #define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 #define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
 #endif
-#   include "../plugins/CommonCwiseUnaryOps.h"
-#   include "../plugins/CommonCwiseBinaryOps.h"
-#   include "../plugins/MatrixCwiseUnaryOps.h"
-#   include "../plugins/MatrixCwiseBinaryOps.h"
-#   include "../plugins/BlockMethods.h"
-#   ifdef EIGEN_SPARSEMATRIXBASE_PLUGIN
-#     include EIGEN_SPARSEMATRIXBASE_PLUGIN
-#   endif
+#include "../plugins/CommonCwiseUnaryOps.inc"
+#include "../plugins/CommonCwiseBinaryOps.inc"
+#include "../plugins/MatrixCwiseUnaryOps.inc"
+#include "../plugins/MatrixCwiseBinaryOps.inc"
+#include "../plugins/BlockMethods.inc"
+#ifdef EIGEN_SPARSEMATRIXBASE_PLUGIN
+#include EIGEN_SPARSEMATRIXBASE_PLUGIN
+#endif
 #undef EIGEN_CURRENT_STORAGE_BASE_CLASS
 #undef EIGEN_DOC_UNARY_ADDONS
 #undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 #undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
 
-    /** \returns the number of rows. \sa cols() */
-    inline Index rows() const { return derived().rows(); }
-    /** \returns the number of columns. \sa rows() */
-    inline Index cols() const { return derived().cols(); }
-    /** \returns the number of coefficients, which is \a rows()*cols().
-      * \sa rows(), cols(). */
-    inline Index size() const { return rows() * cols(); }
-    /** \returns true if either the number of rows or the number of columns is equal to 1.
-      * In other words, this function returns
-      * \code rows()==1 || cols()==1 \endcode
-      * \sa rows(), cols(), IsVectorAtCompileTime. */
-    inline bool isVector() const { return rows()==1 || cols()==1; }
-    /** \returns the size of the storage major dimension,
-      * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */
-    Index outerSize() const { return (int(Flags)&RowMajorBit) ? this->rows() : this->cols(); }
-    /** \returns the size of the inner dimension according to the storage order,
-      * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */
-    Index innerSize() const { return (int(Flags)&RowMajorBit) ? this->cols() : this->rows(); }
+  /** \returns the number of rows. \sa cols() */
+  inline Index rows() const { return derived().rows(); }
+  /** \returns the number of columns. \sa rows() */
+  inline Index cols() const { return derived().cols(); }
+  /** \returns the number of coefficients, which is \a rows()*cols().
+   * \sa rows(), cols(). */
+  inline Index size() const { return rows() * cols(); }
+  /** \returns true if either the number of rows or the number of columns is equal to 1.
+   * In other words, this function returns
+   * \code rows()==1 || cols()==1 \endcode
+   * \sa rows(), cols(), IsVectorAtCompileTime. */
+  inline bool isVector() const { return rows() == 1 || cols() == 1; }
+  /** \returns the size of the storage major dimension,
+   * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */
+  Index outerSize() const { return (int(Flags) & RowMajorBit) ? this->rows() : this->cols(); }
+  /** \returns the size of the inner dimension according to the storage order,
+   * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */
+  Index innerSize() const { return (int(Flags) & RowMajorBit) ? this->cols() : this->rows(); }
 
-    bool isRValue() const { return m_isRValue; }
-    Derived& markAsRValue() { m_isRValue = true; return derived(); }
+  bool isRValue() const { return m_isRValue; }
+  Derived& markAsRValue() {
+    m_isRValue = true;
+    return derived();
+  }
 
-    SparseMatrixBase() : m_isRValue(false) { /* TODO check flags */ }
+  SparseMatrixBase() : m_isRValue(false) { /* TODO check flags */
+  }
 
-    
-    template<typename OtherDerived>
-    Derived& operator=(const ReturnByValue<OtherDerived>& other);
+  template <typename OtherDerived>
+  Derived& operator=(const ReturnByValue<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    inline Derived& operator=(const SparseMatrixBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  inline Derived& operator=(const SparseMatrixBase<OtherDerived>& other);
 
-    inline Derived& operator=(const Derived& other);
+  inline Derived& operator=(const Derived& other);
 
-  protected:
+ protected:
+  template <typename OtherDerived>
+  inline Derived& assign(const OtherDerived& other);
 
-    template<typename OtherDerived>
-    inline Derived& assign(const OtherDerived& other);
+  template <typename OtherDerived>
+  inline void assignGeneric(const OtherDerived& other);
 
-    template<typename OtherDerived>
-    inline void assignGeneric(const OtherDerived& other);
+ public:
+#ifndef EIGEN_NO_IO
+  friend std::ostream& operator<<(std::ostream& s, const SparseMatrixBase& m) {
+    typedef typename Derived::Nested Nested;
+    typedef internal::remove_all_t<Nested> NestedCleaned;
 
-  public:
-
-    friend std::ostream & operator << (std::ostream & s, const SparseMatrixBase& m)
-    {
-      typedef typename Derived::Nested Nested;
-      typedef typename internal::remove_all<Nested>::type NestedCleaned;
-
-      if (Flags&RowMajorBit)
-      {
-        Nested nm(m.derived());
-        internal::evaluator<NestedCleaned> thisEval(nm);
-        for (Index row=0; row<nm.outerSize(); ++row)
-        {
-          Index col = 0;
-          for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, row); it; ++it)
-          {
-            for ( ; col<it.index(); ++col)
-              s << "0 ";
-            s << it.value() << " ";
-            ++col;
-          }
-          for ( ; col<m.cols(); ++col)
-            s << "0 ";
-          s << std::endl;
+    if (Flags & RowMajorBit) {
+      Nested nm(m.derived());
+      internal::evaluator<NestedCleaned> thisEval(nm);
+      for (Index row = 0; row < nm.outerSize(); ++row) {
+        Index col = 0;
+        for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, row); it; ++it) {
+          for (; col < it.index(); ++col) s << "0 ";
+          s << it.value() << " ";
+          ++col;
         }
+        for (; col < m.cols(); ++col) s << "0 ";
+        s << std::endl;
       }
-      else
-      {
-        Nested nm(m.derived());
-        internal::evaluator<NestedCleaned> thisEval(nm);
-        if (m.cols() == 1) {
-          Index row = 0;
-          for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, 0); it; ++it)
-          {
-            for ( ; row<it.index(); ++row)
-              s << "0" << std::endl;
-            s << it.value() << std::endl;
-            ++row;
-          }
-          for ( ; row<m.rows(); ++row)
-            s << "0" << std::endl;
+    } else {
+      Nested nm(m.derived());
+      internal::evaluator<NestedCleaned> thisEval(nm);
+      if (m.cols() == 1) {
+        Index row = 0;
+        for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, 0); it; ++it) {
+          for (; row < it.index(); ++row) s << "0" << std::endl;
+          s << it.value() << std::endl;
+          ++row;
         }
-        else
-        {
-          SparseMatrix<Scalar, RowMajorBit, StorageIndex> trans = m;
-          s << static_cast<const SparseMatrixBase<SparseMatrix<Scalar, RowMajorBit, StorageIndex> >&>(trans);
-        }
+        for (; row < m.rows(); ++row) s << "0" << std::endl;
+      } else {
+        SparseMatrix<Scalar, RowMajorBit, StorageIndex> trans = m;
+        s << static_cast<const SparseMatrixBase<SparseMatrix<Scalar, RowMajorBit, StorageIndex> >&>(trans);
       }
-      return s;
     }
+    return s;
+  }
+#endif
 
-    template<typename OtherDerived>
-    Derived& operator+=(const SparseMatrixBase<OtherDerived>& other);
-    template<typename OtherDerived>
-    Derived& operator-=(const SparseMatrixBase<OtherDerived>& other);
-    
-    template<typename OtherDerived>
-    Derived& operator+=(const DiagonalBase<OtherDerived>& other);
-    template<typename OtherDerived>
-    Derived& operator-=(const DiagonalBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  Derived& operator+=(const SparseMatrixBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  Derived& operator-=(const SparseMatrixBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    Derived& operator+=(const EigenBase<OtherDerived> &other);
-    template<typename OtherDerived>
-    Derived& operator-=(const EigenBase<OtherDerived> &other);
+  template <typename OtherDerived>
+  Derived& operator+=(const DiagonalBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  Derived& operator-=(const DiagonalBase<OtherDerived>& other);
 
-    Derived& operator*=(const Scalar& other);
-    Derived& operator/=(const Scalar& other);
+  template <typename OtherDerived>
+  Derived& operator+=(const EigenBase<OtherDerived>& other);
+  template <typename OtherDerived>
+  Derived& operator-=(const EigenBase<OtherDerived>& other);
 
-    template<typename OtherDerived> struct CwiseProductDenseReturnType {
-      typedef CwiseBinaryOp<internal::scalar_product_op<typename ScalarBinaryOpTraits<
-                                                          typename internal::traits<Derived>::Scalar,
-                                                          typename internal::traits<OtherDerived>::Scalar
-                                                        >::ReturnType>,
-                            const Derived,
-                            const OtherDerived
-                          > Type;
-    };
+  Derived& operator*=(const Scalar& other);
+  Derived& operator/=(const Scalar& other);
 
-    template<typename OtherDerived>
-    EIGEN_STRONG_INLINE const typename CwiseProductDenseReturnType<OtherDerived>::Type
-    cwiseProduct(const MatrixBase<OtherDerived> &other) const;
+  template <typename OtherDerived>
+  struct CwiseProductDenseReturnType {
+    typedef CwiseBinaryOp<
+        internal::scalar_product_op<typename ScalarBinaryOpTraits<
+            typename internal::traits<Derived>::Scalar, typename internal::traits<OtherDerived>::Scalar>::ReturnType>,
+        const Derived, const OtherDerived>
+        Type;
+  };
 
-    // sparse * diagonal
-    template<typename OtherDerived>
-    const Product<Derived,OtherDerived>
-    operator*(const DiagonalBase<OtherDerived> &other) const
-    { return Product<Derived,OtherDerived>(derived(), other.derived()); }
+  template <typename OtherDerived>
+  EIGEN_STRONG_INLINE const typename CwiseProductDenseReturnType<OtherDerived>::Type cwiseProduct(
+      const MatrixBase<OtherDerived>& other) const;
 
-    // diagonal * sparse
-    template<typename OtherDerived> friend
-    const Product<OtherDerived,Derived>
-    operator*(const DiagonalBase<OtherDerived> &lhs, const SparseMatrixBase& rhs)
-    { return Product<OtherDerived,Derived>(lhs.derived(), rhs.derived()); }
-    
-    // sparse * sparse
-    template<typename OtherDerived>
-    const Product<Derived,OtherDerived,AliasFreeProduct>
-    operator*(const SparseMatrixBase<OtherDerived> &other) const;
-    
-    // sparse * dense
-    template<typename OtherDerived>
-    const Product<Derived,OtherDerived>
-    operator*(const MatrixBase<OtherDerived> &other) const
-    { return Product<Derived,OtherDerived>(derived(), other.derived()); }
-    
-    // dense * sparse
-    template<typename OtherDerived> friend
-    const Product<OtherDerived,Derived>
-    operator*(const MatrixBase<OtherDerived> &lhs, const SparseMatrixBase& rhs)
-    { return Product<OtherDerived,Derived>(lhs.derived(), rhs.derived()); }
-    
-     /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */
-    SparseSymmetricPermutationProduct<Derived,Upper|Lower> twistedBy(const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm) const
-    {
-      return SparseSymmetricPermutationProduct<Derived,Upper|Lower>(derived(), perm);
-    }
+  // sparse * diagonal
+  template <typename OtherDerived>
+  const Product<Derived, OtherDerived> operator*(const DiagonalBase<OtherDerived>& other) const {
+    return Product<Derived, OtherDerived>(derived(), other.derived());
+  }
 
-    template<typename OtherDerived>
-    Derived& operator*=(const SparseMatrixBase<OtherDerived>& other);
+  // diagonal * sparse
+  template <typename OtherDerived>
+  friend const Product<OtherDerived, Derived> operator*(const DiagonalBase<OtherDerived>& lhs,
+                                                        const SparseMatrixBase& rhs) {
+    return Product<OtherDerived, Derived>(lhs.derived(), rhs.derived());
+  }
 
-    template<int Mode>
-    inline const TriangularView<const Derived, Mode> triangularView() const;
-    
-    template<unsigned int UpLo> struct SelfAdjointViewReturnType { typedef SparseSelfAdjointView<Derived, UpLo> Type; };
-    template<unsigned int UpLo> struct ConstSelfAdjointViewReturnType { typedef const SparseSelfAdjointView<const Derived, UpLo> Type; };
+  // sparse * sparse
+  template <typename OtherDerived>
+  const Product<Derived, OtherDerived, AliasFreeProduct> operator*(const SparseMatrixBase<OtherDerived>& other) const;
 
-    template<unsigned int UpLo> inline 
-    typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
-    template<unsigned int UpLo> inline
-    typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
+  // sparse * dense
+  template <typename OtherDerived>
+  const Product<Derived, OtherDerived> operator*(const MatrixBase<OtherDerived>& other) const {
+    return Product<Derived, OtherDerived>(derived(), other.derived());
+  }
 
-    template<typename OtherDerived> Scalar dot(const MatrixBase<OtherDerived>& other) const;
-    template<typename OtherDerived> Scalar dot(const SparseMatrixBase<OtherDerived>& other) const;
-    RealScalar squaredNorm() const;
-    RealScalar norm()  const;
-    RealScalar blueNorm() const;
+  // dense * sparse
+  template <typename OtherDerived>
+  friend const Product<OtherDerived, Derived> operator*(const MatrixBase<OtherDerived>& lhs,
+                                                        const SparseMatrixBase& rhs) {
+    return Product<OtherDerived, Derived>(lhs.derived(), rhs.derived());
+  }
 
-    TransposeReturnType transpose() { return TransposeReturnType(derived()); }
-    const ConstTransposeReturnType transpose() const { return ConstTransposeReturnType(derived()); }
-    const AdjointReturnType adjoint() const { return AdjointReturnType(transpose()); }
+  /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */
+  SparseSymmetricPermutationProduct<Derived, Upper | Lower> twistedBy(
+      const PermutationMatrix<Dynamic, Dynamic, StorageIndex>& perm) const {
+    return SparseSymmetricPermutationProduct<Derived, Upper | Lower>(derived(), perm);
+  }
 
-    DenseMatrixType toDense() const
-    {
-      return DenseMatrixType(derived());
-    }
+  template <typename OtherDerived>
+  Derived& operator*=(const SparseMatrixBase<OtherDerived>& other);
 
-    template<typename OtherDerived>
-    bool isApprox(const SparseMatrixBase<OtherDerived>& other,
-                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+  template <int Mode>
+  inline const TriangularView<const Derived, Mode> triangularView() const;
 
-    template<typename OtherDerived>
-    bool isApprox(const MatrixBase<OtherDerived>& other,
-                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
-    { return toDense().isApprox(other,prec); }
+  template <unsigned int UpLo>
+  struct SelfAdjointViewReturnType {
+    typedef SparseSelfAdjointView<Derived, UpLo> Type;
+  };
+  template <unsigned int UpLo>
+  struct ConstSelfAdjointViewReturnType {
+    typedef const SparseSelfAdjointView<const Derived, UpLo> Type;
+  };
 
-    /** \returns the matrix or vector obtained by evaluating this expression.
-      *
-      * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
-      * a const reference, in order to avoid a useless copy.
-      */
-    inline const typename internal::eval<Derived>::type eval() const
-    { return typename internal::eval<Derived>::type(derived()); }
+  template <unsigned int UpLo>
+  inline typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
+  template <unsigned int UpLo>
+  inline typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
 
-    Scalar sum() const;
-    
-    inline const SparseView<Derived>
-    pruned(const Scalar& reference = Scalar(0), const RealScalar& epsilon = NumTraits<Scalar>::dummy_precision()) const;
+  template <typename OtherDerived>
+  Scalar dot(const MatrixBase<OtherDerived>& other) const;
+  template <typename OtherDerived>
+  Scalar dot(const SparseMatrixBase<OtherDerived>& other) const;
+  RealScalar squaredNorm() const;
+  RealScalar norm() const;
+  RealScalar blueNorm() const;
 
-  protected:
+  TransposeReturnType transpose() { return TransposeReturnType(derived()); }
+  const ConstTransposeReturnType transpose() const { return ConstTransposeReturnType(derived()); }
+  const AdjointReturnType adjoint() const { return AdjointReturnType(transpose()); }
 
-    bool m_isRValue;
+  DenseMatrixType toDense() const { return DenseMatrixType(derived()); }
 
-    static inline StorageIndex convert_index(const Index idx) {
-      return internal::convert_index<StorageIndex>(idx);
-    }
-  private:
-    template<typename Dest> void evalTo(Dest &) const;
+  template <typename OtherDerived>
+  bool isApprox(const SparseMatrixBase<OtherDerived>& other,
+                const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
+  template <typename OtherDerived>
+  bool isApprox(const MatrixBase<OtherDerived>& other,
+                const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const {
+    return toDense().isApprox(other, prec);
+  }
+
+  /** \returns the matrix or vector obtained by evaluating this expression.
+   *
+   * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
+   * a const reference, in order to avoid a useless copy.
+   */
+  inline const typename internal::eval<Derived>::type eval() const {
+    return typename internal::eval<Derived>::type(derived());
+  }
+
+  Scalar sum() const;
+
+  inline const SparseView<Derived> pruned(const Scalar& reference = Scalar(0),
+                                          const RealScalar& epsilon = NumTraits<Scalar>::dummy_precision()) const;
+
+ protected:
+  bool m_isRValue;
+
+  static inline StorageIndex convert_index(const Index idx) { return internal::convert_index<StorageIndex>(idx); }
+
+ private:
+  template <typename Dest>
+  void evalTo(Dest&) const;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSEMATRIXBASE_H
+#endif  // EIGEN_SPARSEMATRIXBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h
index ef38357..56f572d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h
@@ -12,167 +12,238 @@
 
 // This file implements sparse * permutation products
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename ExpressionType, int Side, bool Transposed>
-struct permutation_matrix_product<ExpressionType, Side, Transposed, SparseShape>
-{
-    typedef typename nested_eval<ExpressionType, 1>::type MatrixType;
-    typedef typename remove_all<MatrixType>::type MatrixTypeCleaned;
-
-    typedef typename MatrixTypeCleaned::Scalar Scalar;
-    typedef typename MatrixTypeCleaned::StorageIndex StorageIndex;
-
-    enum {
-      SrcStorageOrder = MatrixTypeCleaned::Flags&RowMajorBit ? RowMajor : ColMajor,
-      MoveOuter = SrcStorageOrder==RowMajor ? Side==OnTheLeft : Side==OnTheRight
-    };
-    
-    typedef typename internal::conditional<MoveOuter,
-        SparseMatrix<Scalar,SrcStorageOrder,StorageIndex>,
-        SparseMatrix<Scalar,int(SrcStorageOrder)==RowMajor?ColMajor:RowMajor,StorageIndex> >::type ReturnType;
-
-    template<typename Dest,typename PermutationType>
-    static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr)
-    {
-      MatrixType mat(xpr);
-      if(MoveOuter)
-      {
-        SparseMatrix<Scalar,SrcStorageOrder,StorageIndex> tmp(mat.rows(), mat.cols());
-        Matrix<StorageIndex,Dynamic,1> sizes(mat.outerSize());
-        for(Index j=0; j<mat.outerSize(); ++j)
-        {
-          Index jp = perm.indices().coeff(j);
-          sizes[((Side==OnTheLeft) ^ Transposed) ? jp : j] = StorageIndex(mat.innerVector(((Side==OnTheRight) ^ Transposed) ? jp : j).nonZeros());
-        }
-        tmp.reserve(sizes);
-        for(Index j=0; j<mat.outerSize(); ++j)
-        {
-          Index jp = perm.indices().coeff(j);
-          Index jsrc = ((Side==OnTheRight) ^ Transposed) ? jp : j;
-          Index jdst = ((Side==OnTheLeft) ^ Transposed) ? jp : j;
-          for(typename MatrixTypeCleaned::InnerIterator it(mat,jsrc); it; ++it)
-            tmp.insertByOuterInner(jdst,it.index()) = it.value();
-        }
-        dst = tmp;
-      }
-      else
-      {
-        SparseMatrix<Scalar,int(SrcStorageOrder)==RowMajor?ColMajor:RowMajor,StorageIndex> tmp(mat.rows(), mat.cols());
-        Matrix<StorageIndex,Dynamic,1> sizes(tmp.outerSize());
-        sizes.setZero();
-        PermutationMatrix<Dynamic,Dynamic,StorageIndex> perm_cpy;
-        if((Side==OnTheLeft) ^ Transposed)
-          perm_cpy = perm;
-        else
-          perm_cpy = perm.transpose();
-
-        for(Index j=0; j<mat.outerSize(); ++j)
-          for(typename MatrixTypeCleaned::InnerIterator it(mat,j); it; ++it)
-            sizes[perm_cpy.indices().coeff(it.index())]++;
-        tmp.reserve(sizes);
-        for(Index j=0; j<mat.outerSize(); ++j)
-          for(typename MatrixTypeCleaned::InnerIterator it(mat,j); it; ++it)
-            tmp.insertByOuterInner(perm_cpy.indices().coeff(it.index()),j) = it.value();
-        dst = tmp;
-      }
-    }
+template <typename ExpressionType, typename PlainObjectType,
+          bool NeedEval = !is_same<ExpressionType, PlainObjectType>::value>
+struct XprHelper {
+  XprHelper(const ExpressionType& xpr) : m_xpr(xpr) {}
+  inline const PlainObjectType& xpr() const { return m_xpr; }
+  // this is a new PlainObjectType initialized by xpr
+  const PlainObjectType m_xpr;
+};
+template <typename ExpressionType, typename PlainObjectType>
+struct XprHelper<ExpressionType, PlainObjectType, false> {
+  XprHelper(const ExpressionType& xpr) : m_xpr(xpr) {}
+  inline const PlainObjectType& xpr() const { return m_xpr; }
+  // this is a reference to xpr
+  const PlainObjectType& m_xpr;
 };
 
-}
+template <typename PermDerived, bool NeedInverseEval>
+struct PermHelper {
+  using IndicesType = typename PermDerived::IndicesType;
+  using PermutationIndex = typename IndicesType::Scalar;
+  using type = PermutationMatrix<IndicesType::SizeAtCompileTime, IndicesType::MaxSizeAtCompileTime, PermutationIndex>;
+  PermHelper(const PermDerived& perm) : m_perm(perm.inverse()) {}
+  inline const type& perm() const { return m_perm; }
+  // this is a new PermutationMatrix initialized by perm.inverse()
+  const type m_perm;
+};
+template <typename PermDerived>
+struct PermHelper<PermDerived, false> {
+  using type = PermDerived;
+  PermHelper(const PermDerived& perm) : m_perm(perm) {}
+  inline const type& perm() const { return m_perm; }
+  // this is a reference to perm
+  const type& m_perm;
+};
+
+template <typename ExpressionType, int Side, bool Transposed>
+struct permutation_matrix_product<ExpressionType, Side, Transposed, SparseShape> {
+  using MatrixType = typename nested_eval<ExpressionType, 1>::type;
+  using MatrixTypeCleaned = remove_all_t<MatrixType>;
+
+  using Scalar = typename MatrixTypeCleaned::Scalar;
+  using StorageIndex = typename MatrixTypeCleaned::StorageIndex;
+
+  // the actual "return type" is `Dest`. this is a temporary type
+  using ReturnType = SparseMatrix<Scalar, MatrixTypeCleaned::IsRowMajor ? RowMajor : ColMajor, StorageIndex>;
+  using TmpHelper = XprHelper<ExpressionType, ReturnType>;
+
+  static constexpr bool NeedOuterPermutation = ExpressionType::IsRowMajor ? Side == OnTheLeft : Side == OnTheRight;
+  static constexpr bool NeedInversePermutation = Transposed ? Side == OnTheLeft : Side == OnTheRight;
+
+  template <typename Dest, typename PermutationType>
+  static inline void permute_outer(Dest& dst, const PermutationType& perm, const ExpressionType& xpr) {
+    // if ExpressionType is not ReturnType, evaluate `xpr` (allocation)
+    // otherwise, just reference `xpr`
+    // TODO: handle trivial expressions such as CwiseBinaryOp without temporary
+    const TmpHelper tmpHelper(xpr);
+    const ReturnType& tmp = tmpHelper.xpr();
+
+    ReturnType result(tmp.rows(), tmp.cols());
+
+    for (Index j = 0; j < tmp.outerSize(); j++) {
+      Index jp = perm.indices().coeff(j);
+      Index jsrc = NeedInversePermutation ? jp : j;
+      Index jdst = NeedInversePermutation ? j : jp;
+      Index begin = tmp.outerIndexPtr()[jsrc];
+      Index end = tmp.isCompressed() ? tmp.outerIndexPtr()[jsrc + 1] : begin + tmp.innerNonZeroPtr()[jsrc];
+      result.outerIndexPtr()[jdst + 1] += end - begin;
+    }
+
+    std::partial_sum(result.outerIndexPtr(), result.outerIndexPtr() + result.outerSize() + 1, result.outerIndexPtr());
+    result.resizeNonZeros(result.nonZeros());
+
+    for (Index j = 0; j < tmp.outerSize(); j++) {
+      Index jp = perm.indices().coeff(j);
+      Index jsrc = NeedInversePermutation ? jp : j;
+      Index jdst = NeedInversePermutation ? j : jp;
+      Index begin = tmp.outerIndexPtr()[jsrc];
+      Index end = tmp.isCompressed() ? tmp.outerIndexPtr()[jsrc + 1] : begin + tmp.innerNonZeroPtr()[jsrc];
+      Index target = result.outerIndexPtr()[jdst];
+      smart_copy(tmp.innerIndexPtr() + begin, tmp.innerIndexPtr() + end, result.innerIndexPtr() + target);
+      smart_copy(tmp.valuePtr() + begin, tmp.valuePtr() + end, result.valuePtr() + target);
+    }
+    dst = std::move(result);
+  }
+
+  template <typename Dest, typename PermutationType>
+  static inline void permute_inner(Dest& dst, const PermutationType& perm, const ExpressionType& xpr) {
+    using InnerPermHelper = PermHelper<PermutationType, NeedInversePermutation>;
+    using InnerPermType = typename InnerPermHelper::type;
+
+    // if ExpressionType is not ReturnType, evaluate `xpr` (allocation)
+    // otherwise, just reference `xpr`
+    // TODO: handle trivial expressions such as CwiseBinaryOp without temporary
+    const TmpHelper tmpHelper(xpr);
+    const ReturnType& tmp = tmpHelper.xpr();
+
+    // if inverse permutation of inner indices is requested, calculate perm.inverse() (allocation)
+    // otherwise, just reference `perm`
+    const InnerPermHelper permHelper(perm);
+    const InnerPermType& innerPerm = permHelper.perm();
+
+    ReturnType result(tmp.rows(), tmp.cols());
+
+    for (Index j = 0; j < tmp.outerSize(); j++) {
+      Index begin = tmp.outerIndexPtr()[j];
+      Index end = tmp.isCompressed() ? tmp.outerIndexPtr()[j + 1] : begin + tmp.innerNonZeroPtr()[j];
+      result.outerIndexPtr()[j + 1] += end - begin;
+    }
+
+    std::partial_sum(result.outerIndexPtr(), result.outerIndexPtr() + result.outerSize() + 1, result.outerIndexPtr());
+    result.resizeNonZeros(result.nonZeros());
+
+    for (Index j = 0; j < tmp.outerSize(); j++) {
+      Index begin = tmp.outerIndexPtr()[j];
+      Index end = tmp.isCompressed() ? tmp.outerIndexPtr()[j + 1] : begin + tmp.innerNonZeroPtr()[j];
+      Index target = result.outerIndexPtr()[j];
+      std::transform(tmp.innerIndexPtr() + begin, tmp.innerIndexPtr() + end, result.innerIndexPtr() + target,
+                     [&innerPerm](StorageIndex i) { return innerPerm.indices().coeff(i); });
+      smart_copy(tmp.valuePtr() + begin, tmp.valuePtr() + end, result.valuePtr() + target);
+    }
+    // the inner indices were permuted, and must be sorted
+    result.sortInnerIndices();
+    dst = std::move(result);
+  }
+
+  template <typename Dest, typename PermutationType, bool DoOuter = NeedOuterPermutation,
+            std::enable_if_t<DoOuter, int> = 0>
+  static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr) {
+    permute_outer(dst, perm, xpr);
+  }
+
+  template <typename Dest, typename PermutationType, bool DoOuter = NeedOuterPermutation,
+            std::enable_if_t<!DoOuter, int> = 0>
+  static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr) {
+    permute_inner(dst, perm, xpr);
+  }
+};
+
+}  // namespace internal
 
 namespace internal {
 
-template <int ProductTag> struct product_promote_storage_type<Sparse,             PermutationStorage, ProductTag> { typedef Sparse ret; };
-template <int ProductTag> struct product_promote_storage_type<PermutationStorage, Sparse,             ProductTag> { typedef Sparse ret; };
+template <int ProductTag>
+struct product_promote_storage_type<Sparse, PermutationStorage, ProductTag> {
+  typedef Sparse ret;
+};
+template <int ProductTag>
+struct product_promote_storage_type<PermutationStorage, Sparse, ProductTag> {
+  typedef Sparse ret;
+};
 
-// TODO, the following two overloads are only needed to define the right temporary type through 
+// TODO, the following two overloads are only needed to define the right temporary type through
 // typename traits<permutation_sparse_matrix_product<Rhs,Lhs,OnTheRight,false> >::ReturnType
 // whereas it should be correctly handled by traits<Product<> >::PlainObject
 
-template<typename Lhs, typename Rhs, int ProductTag>
+template <typename Lhs, typename Rhs, int ProductTag>
 struct product_evaluator<Product<Lhs, Rhs, AliasFreeProduct>, ProductTag, PermutationShape, SparseShape>
-  : public evaluator<typename permutation_matrix_product<Rhs,OnTheLeft,false,SparseShape>::ReturnType>
-{
+    : public evaluator<typename permutation_matrix_product<Rhs, OnTheLeft, false, SparseShape>::ReturnType> {
   typedef Product<Lhs, Rhs, AliasFreeProduct> XprType;
-  typedef typename permutation_matrix_product<Rhs,OnTheLeft,false,SparseShape>::ReturnType PlainObject;
+  typedef typename permutation_matrix_product<Rhs, OnTheLeft, false, SparseShape>::ReturnType PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  enum {
-    Flags = Base::Flags | EvalBeforeNestingBit
-  };
+  enum { Flags = Base::Flags | EvalBeforeNestingBit };
 
-  explicit product_evaluator(const XprType& xpr)
-    : m_result(xpr.rows(), xpr.cols())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
+  explicit product_evaluator(const XprType& xpr) : m_result(xpr.rows(), xpr.cols()) {
+    internal::construct_at<Base>(this, m_result);
     generic_product_impl<Lhs, Rhs, PermutationShape, SparseShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
-template<typename Lhs, typename Rhs, int ProductTag>
-struct product_evaluator<Product<Lhs, Rhs, AliasFreeProduct>, ProductTag, SparseShape, PermutationShape >
-  : public evaluator<typename permutation_matrix_product<Lhs,OnTheRight,false,SparseShape>::ReturnType>
-{
+template <typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, AliasFreeProduct>, ProductTag, SparseShape, PermutationShape>
+    : public evaluator<typename permutation_matrix_product<Lhs, OnTheRight, false, SparseShape>::ReturnType> {
   typedef Product<Lhs, Rhs, AliasFreeProduct> XprType;
-  typedef typename permutation_matrix_product<Lhs,OnTheRight,false,SparseShape>::ReturnType PlainObject;
+  typedef typename permutation_matrix_product<Lhs, OnTheRight, false, SparseShape>::ReturnType PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  enum {
-    Flags = Base::Flags | EvalBeforeNestingBit
-  };
+  enum { Flags = Base::Flags | EvalBeforeNestingBit };
 
-  explicit product_evaluator(const XprType& xpr)
-    : m_result(xpr.rows(), xpr.cols())
-  {
+  explicit product_evaluator(const XprType& xpr) : m_result(xpr.rows(), xpr.cols()) {
     ::new (static_cast<Base*>(this)) Base(m_result);
     generic_product_impl<Lhs, Rhs, SparseShape, PermutationShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \returns the matrix with the permutation applied to the columns
-  */
-template<typename SparseDerived, typename PermDerived>
-inline const Product<SparseDerived, PermDerived, AliasFreeProduct>
-operator*(const SparseMatrixBase<SparseDerived>& matrix, const PermutationBase<PermDerived>& perm)
-{ return Product<SparseDerived, PermDerived, AliasFreeProduct>(matrix.derived(), perm.derived()); }
+ */
+template <typename SparseDerived, typename PermDerived>
+inline const Product<SparseDerived, PermDerived, AliasFreeProduct> operator*(
+    const SparseMatrixBase<SparseDerived>& matrix, const PermutationBase<PermDerived>& perm) {
+  return Product<SparseDerived, PermDerived, AliasFreeProduct>(matrix.derived(), perm.derived());
+}
 
 /** \returns the matrix with the permutation applied to the rows
-  */
-template<typename SparseDerived, typename PermDerived>
-inline const Product<PermDerived, SparseDerived, AliasFreeProduct>
-operator*( const PermutationBase<PermDerived>& perm, const SparseMatrixBase<SparseDerived>& matrix)
-{ return  Product<PermDerived, SparseDerived, AliasFreeProduct>(perm.derived(), matrix.derived()); }
-
+ */
+template <typename SparseDerived, typename PermDerived>
+inline const Product<PermDerived, SparseDerived, AliasFreeProduct> operator*(
+    const PermutationBase<PermDerived>& perm, const SparseMatrixBase<SparseDerived>& matrix) {
+  return Product<PermDerived, SparseDerived, AliasFreeProduct>(perm.derived(), matrix.derived());
+}
 
 /** \returns the matrix with the inverse permutation applied to the columns.
-  */
-template<typename SparseDerived, typename PermutationType>
-inline const Product<SparseDerived, Inverse<PermutationType>, AliasFreeProduct>
-operator*(const SparseMatrixBase<SparseDerived>& matrix, const InverseImpl<PermutationType, PermutationStorage>& tperm)
-{
+ */
+template <typename SparseDerived, typename PermutationType>
+inline const Product<SparseDerived, Inverse<PermutationType>, AliasFreeProduct> operator*(
+    const SparseMatrixBase<SparseDerived>& matrix, const InverseImpl<PermutationType, PermutationStorage>& tperm) {
   return Product<SparseDerived, Inverse<PermutationType>, AliasFreeProduct>(matrix.derived(), tperm.derived());
 }
 
 /** \returns the matrix with the inverse permutation applied to the rows.
-  */
-template<typename SparseDerived, typename PermutationType>
-inline const Product<Inverse<PermutationType>, SparseDerived, AliasFreeProduct>
-operator*(const InverseImpl<PermutationType,PermutationStorage>& tperm, const SparseMatrixBase<SparseDerived>& matrix)
-{
+ */
+template <typename SparseDerived, typename PermutationType>
+inline const Product<Inverse<PermutationType>, SparseDerived, AliasFreeProduct> operator*(
+    const InverseImpl<PermutationType, PermutationStorage>& tperm, const SparseMatrixBase<SparseDerived>& matrix) {
   return Product<Inverse<PermutationType>, SparseDerived, AliasFreeProduct>(tperm.derived(), matrix.derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H
+#endif  // EIGEN_SPARSE_SELFADJOINTVIEW_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h
index af8a774..249dabc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h
@@ -10,172 +10,169 @@
 #ifndef EIGEN_SPARSEPRODUCT_H
 #define EIGEN_SPARSEPRODUCT_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \returns an expression of the product of two sparse matrices.
-  * By default a conservative product preserving the symbolic non zeros is performed.
-  * The automatic pruning of the small values can be achieved by calling the pruned() function
-  * in which case a totally different product algorithm is employed:
-  * \code
-  * C = (A*B).pruned();             // suppress numerical zeros (exact)
-  * C = (A*B).pruned(ref);
-  * C = (A*B).pruned(ref,epsilon);
-  * \endcode
-  * where \c ref is a meaningful non zero reference value.
-  * */
-template<typename Derived>
-template<typename OtherDerived>
-inline const Product<Derived,OtherDerived,AliasFreeProduct>
-SparseMatrixBase<Derived>::operator*(const SparseMatrixBase<OtherDerived> &other) const
-{
-  return Product<Derived,OtherDerived,AliasFreeProduct>(derived(), other.derived());
+ * By default a conservative product preserving the symbolic non zeros is performed.
+ * The automatic pruning of the small values can be achieved by calling the pruned() function
+ * in which case a totally different product algorithm is employed:
+ * \code
+ * C = (A*B).pruned();             // suppress numerical zeros (exact)
+ * C = (A*B).pruned(ref);
+ * C = (A*B).pruned(ref,epsilon);
+ * \endcode
+ * where \c ref is a meaningful non zero reference value.
+ * */
+template <typename Derived>
+template <typename OtherDerived>
+inline const Product<Derived, OtherDerived, AliasFreeProduct> SparseMatrixBase<Derived>::operator*(
+    const SparseMatrixBase<OtherDerived>& other) const {
+  return Product<Derived, OtherDerived, AliasFreeProduct>(derived(), other.derived());
 }
 
 namespace internal {
 
 // sparse * sparse
-template<typename Lhs, typename Rhs, int ProductType>
-struct generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
-{
-  template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
-  {
+template <typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType> {
+  template <typename Dest>
+  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) {
     evalTo(dst, lhs, rhs, typename evaluator_traits<Dest>::Shape());
   }
 
   // dense += sparse * sparse
-  template<typename Dest,typename ActualLhs>
-  static void addTo(Dest& dst, const ActualLhs& lhs, const Rhs& rhs, typename enable_if<is_same<typename evaluator_traits<Dest>::Shape,DenseShape>::value,int*>::type* = 0)
-  {
-    typedef typename nested_eval<ActualLhs,Dynamic>::type LhsNested;
-    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+  template <typename Dest, typename ActualLhs>
+  static void addTo(Dest& dst, const ActualLhs& lhs, const Rhs& rhs,
+                    std::enable_if_t<is_same<typename evaluator_traits<Dest>::Shape, DenseShape>::value, int*>* = 0) {
+    typedef typename nested_eval<ActualLhs, Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs, Dynamic>::type RhsNested;
     LhsNested lhsNested(lhs);
     RhsNested rhsNested(rhs);
-    internal::sparse_sparse_to_dense_product_selector<typename remove_all<LhsNested>::type,
-                                                      typename remove_all<RhsNested>::type, Dest>::run(lhsNested,rhsNested,dst);
+    internal::sparse_sparse_to_dense_product_selector<remove_all_t<LhsNested>, remove_all_t<RhsNested>, Dest>::run(
+        lhsNested, rhsNested, dst);
   }
 
   // dense -= sparse * sparse
-  template<typename Dest>
-  static void subTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, typename enable_if<is_same<typename evaluator_traits<Dest>::Shape,DenseShape>::value,int*>::type* = 0)
-  {
+  template <typename Dest>
+  static void subTo(Dest& dst, const Lhs& lhs, const Rhs& rhs,
+                    std::enable_if_t<is_same<typename evaluator_traits<Dest>::Shape, DenseShape>::value, int*>* = 0) {
     addTo(dst, -lhs, rhs);
   }
 
-protected:
-
+ protected:
   // sparse = sparse * sparse
-  template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, SparseShape)
-  {
-    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
-    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+  template <typename Dest>
+  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, SparseShape) {
+    typedef typename nested_eval<Lhs, Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs, Dynamic>::type RhsNested;
     LhsNested lhsNested(lhs);
     RhsNested rhsNested(rhs);
-    internal::conservative_sparse_sparse_product_selector<typename remove_all<LhsNested>::type,
-                                                          typename remove_all<RhsNested>::type, Dest>::run(lhsNested,rhsNested,dst);
+    internal::conservative_sparse_sparse_product_selector<remove_all_t<LhsNested>, remove_all_t<RhsNested>, Dest>::run(
+        lhsNested, rhsNested, dst);
   }
 
   // dense = sparse * sparse
-  template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, DenseShape)
-  {
+  template <typename Dest>
+  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, DenseShape) {
     dst.setZero();
     addTo(dst, lhs, rhs);
   }
 };
 
 // sparse * sparse-triangular
-template<typename Lhs, typename Rhs, int ProductType>
+template <typename Lhs, typename Rhs, int ProductType>
 struct generic_product_impl<Lhs, Rhs, SparseShape, SparseTriangularShape, ProductType>
- : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
-{};
+    : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType> {};
 
 // sparse-triangular * sparse
-template<typename Lhs, typename Rhs, int ProductType>
+template <typename Lhs, typename Rhs, int ProductType>
 struct generic_product_impl<Lhs, Rhs, SparseTriangularShape, SparseShape, ProductType>
- : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
-{};
+    : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType> {};
 
 // dense = sparse-product (can be sparse*sparse, sparse*perm, etc.)
-template< typename DstXprType, typename Lhs, typename Rhs>
-struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
-{
-  typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
-  {
+template <typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<
+    DstXprType, Product<Lhs, Rhs, AliasFreeProduct>,
+    internal::assign_op<typename DstXprType::Scalar, typename Product<Lhs, Rhs, AliasFreeProduct>::Scalar>,
+    Sparse2Dense> {
+  typedef Product<Lhs, Rhs, AliasFreeProduct> SrcXprType;
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>&) {
     Index dstRows = src.rows();
     Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
-    
-    generic_product_impl<Lhs, Rhs>::evalTo(dst,src.lhs(),src.rhs());
+    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
+
+    generic_product_impl<Lhs, Rhs>::evalTo(dst, src.lhs(), src.rhs());
   }
 };
 
 // dense += sparse-product (can be sparse*sparse, sparse*perm, etc.)
-template< typename DstXprType, typename Lhs, typename Rhs>
-struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::add_assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
-{
-  typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
-  {
-    generic_product_impl<Lhs, Rhs>::addTo(dst,src.lhs(),src.rhs());
+template <typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<
+    DstXprType, Product<Lhs, Rhs, AliasFreeProduct>,
+    internal::add_assign_op<typename DstXprType::Scalar, typename Product<Lhs, Rhs, AliasFreeProduct>::Scalar>,
+    Sparse2Dense> {
+  typedef Product<Lhs, Rhs, AliasFreeProduct> SrcXprType;
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::add_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>&) {
+    generic_product_impl<Lhs, Rhs>::addTo(dst, src.lhs(), src.rhs());
   }
 };
 
 // dense -= sparse-product (can be sparse*sparse, sparse*perm, etc.)
-template< typename DstXprType, typename Lhs, typename Rhs>
-struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::sub_assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
-{
-  typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
-  {
-    generic_product_impl<Lhs, Rhs>::subTo(dst,src.lhs(),src.rhs());
+template <typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<
+    DstXprType, Product<Lhs, Rhs, AliasFreeProduct>,
+    internal::sub_assign_op<typename DstXprType::Scalar, typename Product<Lhs, Rhs, AliasFreeProduct>::Scalar>,
+    Sparse2Dense> {
+  typedef Product<Lhs, Rhs, AliasFreeProduct> SrcXprType;
+  static void run(DstXprType& dst, const SrcXprType& src,
+                  const internal::sub_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>&) {
+    generic_product_impl<Lhs, Rhs>::subTo(dst, src.lhs(), src.rhs());
   }
 };
 
-template<typename Lhs, typename Rhs, int Options>
+template <typename Lhs, typename Rhs, int Options>
 struct unary_evaluator<SparseView<Product<Lhs, Rhs, Options> >, IteratorBased>
- : public evaluator<typename Product<Lhs, Rhs, DefaultProduct>::PlainObject>
-{
+    : public evaluator<typename Product<Lhs, Rhs, DefaultProduct>::PlainObject> {
   typedef SparseView<Product<Lhs, Rhs, Options> > XprType;
   typedef typename XprType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  explicit unary_evaluator(const XprType& xpr)
-    : m_result(xpr.rows(), xpr.cols())
-  {
+  explicit unary_evaluator(const XprType& xpr) : m_result(xpr.rows(), xpr.cols()) {
     using std::abs;
-    ::new (static_cast<Base*>(this)) Base(m_result);
-    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
-    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    internal::construct_at<Base>(this, m_result);
+    typedef typename nested_eval<Lhs, Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs, Dynamic>::type RhsNested;
     LhsNested lhsNested(xpr.nestedExpression().lhs());
     RhsNested rhsNested(xpr.nestedExpression().rhs());
 
-    internal::sparse_sparse_product_with_pruning_selector<typename remove_all<LhsNested>::type,
-                                                          typename remove_all<RhsNested>::type, PlainObject>::run(lhsNested,rhsNested,m_result,
-                                                                                                                  abs(xpr.reference())*xpr.epsilon());
+    internal::sparse_sparse_product_with_pruning_selector<remove_all_t<LhsNested>, remove_all_t<RhsNested>,
+                                                          PlainObject>::run(lhsNested, rhsNested, m_result,
+                                                                            abs(xpr.reference()) * xpr.epsilon());
   }
 
-protected:
+ protected:
   PlainObject m_result;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 // sparse matrix = sparse-product (can be sparse*sparse, sparse*perm, etc.)
-template<typename Scalar, int _Options, typename _StorageIndex>
-template<typename Lhs, typename Rhs>
-SparseMatrix<Scalar,_Options,_StorageIndex>& SparseMatrix<Scalar,_Options,_StorageIndex>::operator=(const Product<Lhs,Rhs,AliasFreeProduct>& src)
-{
+template <typename Scalar, int Options_, typename StorageIndex_>
+template <typename Lhs, typename Rhs>
+SparseMatrix<Scalar, Options_, StorageIndex_>& SparseMatrix<Scalar, Options_, StorageIndex_>::operator=(
+    const Product<Lhs, Rhs, AliasFreeProduct>& src) {
   // std::cout << "in Assignment : " << DstOptions << "\n";
-  SparseMatrix dst(src.rows(),src.cols());
-  internal::generic_product_impl<Lhs, Rhs>::evalTo(dst,src.lhs(),src.rhs());
+  SparseMatrix dst(src.rows(), src.cols());
+  internal::generic_product_impl<Lhs, Rhs>::evalTo(dst, src.lhs(), src.rhs());
   this->swap(dst);
   return *this;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSEPRODUCT_H
+#endif  // EIGEN_SPARSEPRODUCT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h
index 4587749..732e4f7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h
@@ -10,40 +10,38 @@
 #ifndef EIGEN_SPARSEREDUX_H
 #define EIGEN_SPARSEREDUX_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-template<typename Derived>
-typename internal::traits<Derived>::Scalar
-SparseMatrixBase<Derived>::sum() const
-{
-  eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+namespace Eigen {
+
+template <typename Derived>
+typename internal::traits<Derived>::Scalar SparseMatrixBase<Derived>::sum() const {
+  eigen_assert(rows() > 0 && cols() > 0 && "you are using a non initialized matrix");
   Scalar res(0);
   internal::evaluator<Derived> thisEval(derived());
-  for (Index j=0; j<outerSize(); ++j)
-    for (typename internal::evaluator<Derived>::InnerIterator iter(thisEval,j); iter; ++iter)
-      res += iter.value();
+  for (Index j = 0; j < outerSize(); ++j)
+    for (typename internal::evaluator<Derived>::InnerIterator iter(thisEval, j); iter; ++iter) res += iter.value();
   return res;
 }
 
-template<typename _Scalar, int _Options, typename _Index>
-typename internal::traits<SparseMatrix<_Scalar,_Options,_Index> >::Scalar
-SparseMatrix<_Scalar,_Options,_Index>::sum() const
-{
-  eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
-  if(this->isCompressed())
-    return Matrix<Scalar,1,Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
+template <typename Scalar_, int Options_, typename Index_>
+typename internal::traits<SparseMatrix<Scalar_, Options_, Index_> >::Scalar
+SparseMatrix<Scalar_, Options_, Index_>::sum() const {
+  eigen_assert(rows() > 0 && cols() > 0 && "you are using a non initialized matrix");
+  if (this->isCompressed())
+    return Matrix<Scalar, 1, Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
   else
     return Base::sum();
 }
 
-template<typename _Scalar, int _Options, typename _Index>
-typename internal::traits<SparseVector<_Scalar,_Options, _Index> >::Scalar
-SparseVector<_Scalar,_Options,_Index>::sum() const
-{
-  eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
-  return Matrix<Scalar,1,Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
+template <typename Scalar_, int Options_, typename Index_>
+typename internal::traits<SparseVector<Scalar_, Options_, Index_> >::Scalar
+SparseVector<Scalar_, Options_, Index_>::sum() const {
+  eigen_assert(rows() > 0 && cols() > 0 && "you are using a non initialized matrix");
+  return Matrix<Scalar, 1, Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSEREDUX_H
+#endif  // EIGEN_SPARSEREDUX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h
index 748f87d..c205e6d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h
@@ -10,388 +10,361 @@
 #ifndef EIGEN_SPARSE_REF_H
 #define EIGEN_SPARSE_REF_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 enum {
-  StandardCompressedFormat = 2 /**< used by Ref<SparseMatrix> to specify whether the input storage must be in standard compressed form */
+  StandardCompressedFormat =
+      2 /**< used by Ref<SparseMatrix> to specify whether the input storage must be in standard compressed form */
 };
-  
+
 namespace internal {
 
-template<typename Derived> class SparseRefBase;
+template <typename Derived>
+class SparseRefBase;
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
-struct traits<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
-  : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
-{
-  typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
-  enum {
-    Options = _Options,
-    Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit
-  };
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options_, typename StrideType_>
+struct traits<Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options_, StrideType_>>
+    : public traits<SparseMatrix<MatScalar, MatOptions, MatIndex>> {
+  typedef SparseMatrix<MatScalar, MatOptions, MatIndex> PlainObjectType;
+  enum { Options = Options_, Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit };
 
-  template<typename Derived> struct match {
+  template <typename Derived>
+  struct match {
     enum {
-      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
-      MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && StorageOrderMatch
+      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime ||
+                          ((PlainObjectType::Flags & RowMajorBit) == (Derived::Flags & RowMajorBit)),
+      MatchAtCompileTime = (Derived::Flags & CompressedAccessBit) && StorageOrderMatch
     };
-    typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
+    typedef std::conditional_t<MatchAtCompileTime, internal::true_type, internal::false_type> type;
   };
-  
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
-struct traits<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
-  : public traits<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
-{
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options_, typename StrideType_>
+struct traits<Ref<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options_, StrideType_>>
+    : public traits<Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options_, StrideType_>> {
   enum {
-    Flags = (traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
+    Flags =
+        (traits<SparseMatrix<MatScalar, MatOptions, MatIndex>>::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
   };
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
-struct traits<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
-  : public traits<SparseVector<MatScalar,MatOptions,MatIndex> >
-{
-  typedef SparseVector<MatScalar,MatOptions,MatIndex> PlainObjectType;
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options_, typename StrideType_>
+struct traits<Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options_, StrideType_>>
+    : public traits<SparseVector<MatScalar, MatOptions, MatIndex>> {
+  typedef SparseVector<MatScalar, MatOptions, MatIndex> PlainObjectType;
+  enum { Options = Options_, Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit };
+
+  template <typename Derived>
+  struct match {
+    enum { MatchAtCompileTime = (Derived::Flags & CompressedAccessBit) && Derived::IsVectorAtCompileTime };
+    typedef std::conditional_t<MatchAtCompileTime, internal::true_type, internal::false_type> type;
+  };
+};
+
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options_, typename StrideType_>
+struct traits<Ref<const SparseVector<MatScalar, MatOptions, MatIndex>, Options_, StrideType_>>
+    : public traits<Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options_, StrideType_>> {
   enum {
-    Options = _Options,
-    Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit
-  };
-
-  template<typename Derived> struct match {
-    enum {
-      MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && Derived::IsVectorAtCompileTime
-    };
-    typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
-  };
-
-};
-
-template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
-struct traits<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
-  : public traits<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
-{
-  enum {
-    Flags = (traits<SparseVector<MatScalar,MatOptions,MatIndex> >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
+    Flags =
+        (traits<SparseVector<MatScalar, MatOptions, MatIndex>>::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
   };
 };
 
-template<typename Derived>
-struct traits<SparseRefBase<Derived> > : public traits<Derived> {};
+template <typename Derived>
+struct traits<SparseRefBase<Derived>> : public traits<Derived> {};
 
-template<typename Derived> class SparseRefBase
-  : public SparseMapBase<Derived>
-{
-public:
-
+template <typename Derived>
+class SparseRefBase : public SparseMapBase<Derived> {
+ public:
   typedef SparseMapBase<Derived> Base;
   EIGEN_SPARSE_PUBLIC_INTERFACE(SparseRefBase)
 
   SparseRefBase()
-    : Base(RowsAtCompileTime==Dynamic?0:RowsAtCompileTime,ColsAtCompileTime==Dynamic?0:ColsAtCompileTime, 0, 0, 0, 0, 0)
-  {}
-  
-protected:
+      : Base(RowsAtCompileTime == Dynamic ? 0 : RowsAtCompileTime, ColsAtCompileTime == Dynamic ? 0 : ColsAtCompileTime,
+             0, 0, 0, 0, 0) {}
 
-  template<typename Expression>
-  void construct(Expression& expr)
-  {
-    if(expr.outerIndexPtr()==0)
-      ::new (static_cast<Base*>(this)) Base(expr.size(), expr.nonZeros(), expr.innerIndexPtr(), expr.valuePtr());
+ protected:
+  template <typename Expression>
+  void construct(Expression& expr) {
+    if (expr.outerIndexPtr() == 0)
+      internal::construct_at<Base>(this, expr.size(), expr.nonZeros(), expr.innerIndexPtr(), expr.valuePtr());
     else
-      ::new (static_cast<Base*>(this)) Base(expr.rows(), expr.cols(), expr.nonZeros(), expr.outerIndexPtr(), expr.innerIndexPtr(), expr.valuePtr(), expr.innerNonZeroPtr());
+      internal::construct_at<Base>(this, expr.rows(), expr.cols(), expr.nonZeros(), expr.outerIndexPtr(),
+                                   expr.innerIndexPtr(), expr.valuePtr(), expr.innerNonZeroPtr());
   }
 };
 
-} // namespace internal
-
-
-/** 
-  * \ingroup SparseCore_Module
-  *
-  * \brief A sparse matrix expression referencing an existing sparse expression
-  *
-  * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix.
-  * \tparam Options specifies whether the a standard compressed format is required \c Options is  \c #StandardCompressedFormat, or \c 0.
-  *                The default is \c 0.
-  *
-  * \sa class Ref
-  */
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-class Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType >
-  : public internal::SparseRefBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType > >
-#else
-template<typename SparseMatrixType, int Options>
-class Ref<SparseMatrixType, Options>
-  : public SparseMapBase<Derived,WriteAccessors> // yes, that's weird to use Derived here, but that works!
-#endif
-{
-    typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
-    typedef internal::traits<Ref> Traits;
-    template<int OtherOptions>
-    inline Ref(const SparseMatrix<MatScalar,OtherOptions,MatIndex>& expr);
-    template<int OtherOptions>
-    inline Ref(const MappedSparseMatrix<MatScalar,OtherOptions,MatIndex>& expr);
-  public:
-
-    typedef internal::SparseRefBase<Ref> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
-
-
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<int OtherOptions>
-    inline Ref(SparseMatrix<MatScalar,OtherOptions,MatIndex>& expr)
-    {
-      EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseMatrix<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
-      Base::construct(expr.derived());
-    }
-    
-    template<int OtherOptions>
-    inline Ref(MappedSparseMatrix<MatScalar,OtherOptions,MatIndex>& expr)
-    {
-      EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseMatrix<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
-      Base::construct(expr.derived());
-    }
-    
-    template<typename Derived>
-    inline Ref(const SparseCompressedBase<Derived>& expr)
-    #else
-    /** Implicit constructor from any sparse expression (2D matrix or 1D vector) */
-    template<typename Derived>
-    inline Ref(SparseCompressedBase<Derived>& expr)
-    #endif
-    {
-      EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
-      EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
-      Base::construct(expr.const_cast_derived());
-    }
-};
-
-// this is the const ref version
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-class Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
-  : public internal::SparseRefBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-{
-    typedef SparseMatrix<MatScalar,MatOptions,MatIndex> TPlainObjectType;
-    typedef internal::traits<Ref> Traits;
-  public:
-
-    typedef internal::SparseRefBase<Ref> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
-
-    template<typename Derived>
-    inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false)
-    {
-      construct(expr.derived(), typename Traits::template match<Derived>::type());
-    }
-
-    inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
-      // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
-    }
-
-    template<typename OtherRef>
-    inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
-      construct(other.derived(), typename Traits::template match<OtherRef>::type());
-    }
-
-    ~Ref() {
-      if(m_hasCopy) {
-        TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
-        obj->~TPlainObjectType();
-      }
-    }
-
-  protected:
-
-    template<typename Expression>
-    void construct(const Expression& expr,internal::true_type)
-    {
-      if((Options & int(StandardCompressedFormat)) && (!expr.isCompressed()))
-      {
-        TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
-        ::new (obj) TPlainObjectType(expr);
-        m_hasCopy = true;
-        Base::construct(*obj);
-      }
-      else
-      {
-        Base::construct(expr);
-      }
-    }
-
-    template<typename Expression>
-    void construct(const Expression& expr, internal::false_type)
-    {
-      TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
-      ::new (obj) TPlainObjectType(expr);
-      m_hasCopy = true;
-      Base::construct(*obj);
-    }
-
-  protected:
-    typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
-    bool m_hasCopy;
-};
-
-
+}  // namespace internal
 
 /**
-  * \ingroup SparseCore_Module
-  *
-  * \brief A sparse vector expression referencing an existing sparse vector expression
-  *
-  * \tparam SparseVectorType the equivalent sparse vector type of the referenced data, it must be a template instance of class SparseVector.
-  *
-  * \sa class Ref
-  */
+ * \ingroup SparseCore_Module
+ *
+ * \brief A sparse matrix expression referencing an existing sparse expression
+ *
+ * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of
+ * class SparseMatrix. \tparam Options specifies whether the a standard compressed format is required \c Options is  \c
+ * #StandardCompressedFormat, or \c 0. The default is \c 0.
+ *
+ * \sa class Ref
+ */
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-class Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType >
-  : public internal::SparseRefBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType > >
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>
+    : public internal::SparseRefBase<Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>>
 #else
-template<typename SparseVectorType>
-class Ref<SparseVectorType>
-  : public SparseMapBase<Derived,WriteAccessors>
+template <typename SparseMatrixType, int Options>
+class Ref<SparseMatrixType, Options>
+    : public SparseMapBase<Derived, WriteAccessors>  // yes, that's weird to use Derived here, but that works!
 #endif
 {
-    typedef SparseVector<MatScalar,MatOptions,MatIndex> PlainObjectType;
-    typedef internal::traits<Ref> Traits;
-    template<int OtherOptions>
-    inline Ref(const SparseVector<MatScalar,OtherOptions,MatIndex>& expr);
-  public:
+  typedef SparseMatrix<MatScalar, MatOptions, MatIndex> PlainObjectType;
+  typedef internal::traits<Ref> Traits;
+  template <int OtherOptions>
+  inline Ref(const SparseMatrix<MatScalar, OtherOptions, MatIndex>& expr);
+  template <int OtherOptions>
+  inline Ref(const Map<SparseMatrix<MatScalar, OtherOptions, MatIndex>>& expr);
 
-    typedef internal::SparseRefBase<Ref> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+ public:
+  typedef internal::SparseRefBase<Ref> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<int OtherOptions>
-    inline Ref(SparseVector<MatScalar,OtherOptions,MatIndex>& expr)
-    {
-      EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseVector<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      Base::construct(expr.derived());
-    }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <int OtherOptions>
+  inline Ref(SparseMatrix<MatScalar, OtherOptions, MatIndex>& expr) {
+    EIGEN_STATIC_ASSERT(
+        bool(Traits::template match<SparseMatrix<MatScalar, OtherOptions, MatIndex>>::MatchAtCompileTime),
+        STORAGE_LAYOUT_DOES_NOT_MATCH);
+    eigen_assert(((Options & int(StandardCompressedFormat)) == 0) || (expr.isCompressed()));
+    Base::construct(expr.derived());
+  }
 
-    template<typename Derived>
-    inline Ref(const SparseCompressedBase<Derived>& expr)
-    #else
-    /** Implicit constructor from any 1D sparse vector expression */
-    template<typename Derived>
-    inline Ref(SparseCompressedBase<Derived>& expr)
-    #endif
-    {
-      EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
-      EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      Base::construct(expr.const_cast_derived());
-    }
+  template <int OtherOptions>
+  inline Ref(Map<SparseMatrix<MatScalar, OtherOptions, MatIndex>>& expr) {
+    EIGEN_STATIC_ASSERT(
+        bool(Traits::template match<SparseMatrix<MatScalar, OtherOptions, MatIndex>>::MatchAtCompileTime),
+        STORAGE_LAYOUT_DOES_NOT_MATCH);
+    eigen_assert(((Options & int(StandardCompressedFormat)) == 0) || (expr.isCompressed()));
+    Base::construct(expr.derived());
+  }
+
+  template <typename Derived>
+  inline Ref(const SparseCompressedBase<Derived>& expr)
+#else
+  /** Implicit constructor from any sparse expression (2D matrix or 1D vector) */
+  template <typename Derived>
+  inline Ref(SparseCompressedBase<Derived>& expr)
+#endif
+  {
+    EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+    EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+    eigen_assert(((Options & int(StandardCompressedFormat)) == 0) || (expr.isCompressed()));
+    Base::construct(expr.const_cast_derived());
+  }
 };
 
 // this is the const ref version
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-class Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType>
-  : public internal::SparseRefBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-{
-    typedef SparseVector<MatScalar,MatOptions,MatIndex> TPlainObjectType;
-    typedef internal::traits<Ref> Traits;
-  public:
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>
+    : public internal::SparseRefBase<Ref<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>> {
+  typedef SparseMatrix<MatScalar, MatOptions, MatIndex> TPlainObjectType;
+  typedef internal::traits<Ref> Traits;
 
-    typedef internal::SparseRefBase<Ref> Base;
-    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+ public:
+  typedef internal::SparseRefBase<Ref> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
 
-    template<typename Derived>
-    inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false)
-    {
-      construct(expr.derived(), typename Traits::template match<Derived>::type());
+  template <typename Derived>
+  inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false) {
+    construct(expr.derived(), typename Traits::template match<Derived>::type());
+  }
+
+  inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
+    // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+  }
+
+  template <typename OtherRef>
+  inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
+    construct(other.derived(), typename Traits::template match<OtherRef>::type());
+  }
+
+  ~Ref() {
+    if (m_hasCopy) {
+      internal::destroy_at(reinterpret_cast<TPlainObjectType*>(&m_storage));
     }
+  }
 
-    inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
-      // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
-    }
-
-    template<typename OtherRef>
-    inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
-      construct(other.derived(), typename Traits::template match<OtherRef>::type());
-    }
-
-    ~Ref() {
-      if(m_hasCopy) {
-        TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
-        obj->~TPlainObjectType();
-      }
-    }
-
-  protected:
-
-    template<typename Expression>
-    void construct(const Expression& expr,internal::true_type)
-    {
-      Base::construct(expr);
-    }
-
-    template<typename Expression>
-    void construct(const Expression& expr, internal::false_type)
-    {
-      TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
-      ::new (obj) TPlainObjectType(expr);
+ protected:
+  template <typename Expression>
+  void construct(const Expression& expr, internal::true_type) {
+    if ((Options & int(StandardCompressedFormat)) && (!expr.isCompressed())) {
+      TPlainObjectType* obj = internal::construct_at(reinterpret_cast<TPlainObjectType*>(&m_storage), expr);
       m_hasCopy = true;
       Base::construct(*obj);
+    } else {
+      Base::construct(expr);
     }
+  }
 
-  protected:
-    typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
-    bool m_hasCopy;
+  template <typename Expression>
+  void construct(const Expression& expr, internal::false_type) {
+    TPlainObjectType* obj = internal::construct_at(reinterpret_cast<TPlainObjectType*>(&m_storage), expr);
+    m_hasCopy = true;
+    Base::construct(*obj);
+  }
+
+ protected:
+  typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
+  bool m_hasCopy;
+};
+
+/**
+ * \ingroup SparseCore_Module
+ *
+ * \brief A sparse vector expression referencing an existing sparse vector expression
+ *
+ * \tparam SparseVectorType the equivalent sparse vector type of the referenced data, it must be a template instance of
+ * class SparseVector.
+ *
+ * \sa class Ref
+ */
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>
+    : public internal::SparseRefBase<Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>>
+#else
+template <typename SparseVectorType>
+class Ref<SparseVectorType> : public SparseMapBase<Derived, WriteAccessors>
+#endif
+{
+  typedef SparseVector<MatScalar, MatOptions, MatIndex> PlainObjectType;
+  typedef internal::traits<Ref> Traits;
+  template <int OtherOptions>
+  inline Ref(const SparseVector<MatScalar, OtherOptions, MatIndex>& expr);
+
+ public:
+  typedef internal::SparseRefBase<Ref> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <int OtherOptions>
+  inline Ref(SparseVector<MatScalar, OtherOptions, MatIndex>& expr) {
+    EIGEN_STATIC_ASSERT(
+        bool(Traits::template match<SparseVector<MatScalar, OtherOptions, MatIndex>>::MatchAtCompileTime),
+        STORAGE_LAYOUT_DOES_NOT_MATCH);
+    Base::construct(expr.derived());
+  }
+
+  template <typename Derived>
+  inline Ref(const SparseCompressedBase<Derived>& expr)
+#else
+  /** Implicit constructor from any 1D sparse vector expression */
+  template <typename Derived>
+  inline Ref(SparseCompressedBase<Derived>& expr)
+#endif
+  {
+    EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+    EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+    Base::construct(expr.const_cast_derived());
+  }
+};
+
+// this is the const ref version
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<const SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>
+    : public internal::SparseRefBase<Ref<const SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>> {
+  typedef SparseVector<MatScalar, MatOptions, MatIndex> TPlainObjectType;
+  typedef internal::traits<Ref> Traits;
+
+ public:
+  typedef internal::SparseRefBase<Ref> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+  template <typename Derived>
+  inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false) {
+    construct(expr.derived(), typename Traits::template match<Derived>::type());
+  }
+
+  inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
+    // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+  }
+
+  template <typename OtherRef>
+  inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
+    construct(other.derived(), typename Traits::template match<OtherRef>::type());
+  }
+
+  ~Ref() {
+    if (m_hasCopy) {
+      internal::destroy_at(reinterpret_cast<TPlainObjectType*>(&m_storage));
+    }
+  }
+
+ protected:
+  template <typename Expression>
+  void construct(const Expression& expr, internal::true_type) {
+    Base::construct(expr);
+  }
+
+  template <typename Expression>
+  void construct(const Expression& expr, internal::false_type) {
+    TPlainObjectType* obj = internal::construct_at(reinterpret_cast<TPlainObjectType*>(&m_storage), expr);
+    m_hasCopy = true;
+    Base::construct(*obj);
+  }
+
+ protected:
+  typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
+  bool m_hasCopy;
 };
 
 namespace internal {
 
-// FIXME shall we introduce a general evaluatior_ref that we can specialize for any sparse object once, and thus remove this copy-pasta thing...
+// FIXME shall we introduce a general evaluatior_ref that we can specialize for any sparse object once, and thus remove
+// this copy-pasta thing...
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct evaluator<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
-{
-  typedef evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
-  typedef Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>>
+    : evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>>> {
+  typedef evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>>> Base;
+  typedef Ref<SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> XprType;
   evaluator() : Base() {}
-  explicit evaluator(const XprType &mat) : Base(mat) {}
+  explicit evaluator(const XprType& mat) : Base(mat) {}
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct evaluator<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
-{
-  typedef evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
-  typedef Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>>
+    : evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>>> {
+  typedef evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType>>>
+      Base;
+  typedef Ref<const SparseMatrix<MatScalar, MatOptions, MatIndex>, Options, StrideType> XprType;
   evaluator() : Base() {}
-  explicit evaluator(const XprType &mat) : Base(mat) {}
+  explicit evaluator(const XprType& mat) : Base(mat) {}
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct evaluator<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
-{
-  typedef evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
-  typedef Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>>
+    : evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>>> {
+  typedef evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>>> Base;
+  typedef Ref<SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType> XprType;
   evaluator() : Base() {}
-  explicit evaluator(const XprType &mat) : Base(mat) {}
+  explicit evaluator(const XprType& mat) : Base(mat) {}
 };
 
-template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
-struct evaluator<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
-  : evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
-{
-  typedef evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
-  typedef Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+template <typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<const SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>>
+    : evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>>> {
+  typedef evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType>>>
+      Base;
+  typedef Ref<const SparseVector<MatScalar, MatOptions, MatIndex>, Options, StrideType> XprType;
   evaluator() : Base() {}
-  explicit evaluator(const XprType &mat) : Base(mat) {}
+  explicit evaluator(const XprType& mat) : Base(mat) {}
 };
 
-}
+}  // namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_REF_H
+#endif  // EIGEN_SPARSE_REF_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h
index 85b00e1..129899c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h
@@ -10,194 +10,192 @@
 #ifndef EIGEN_SPARSE_SELFADJOINTVIEW_H
 #define EIGEN_SPARSE_SELFADJOINTVIEW_H
 
-namespace Eigen { 
-  
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
+
 /** \ingroup SparseCore_Module
-  * \class SparseSelfAdjointView
-  *
-  * \brief Pseudo expression to manipulate a triangular sparse matrix as a selfadjoint matrix.
-  *
-  * \param MatrixType the type of the dense matrix storing the coefficients
-  * \param Mode can be either \c #Lower or \c #Upper
-  *
-  * This class is an expression of a sefladjoint matrix from a triangular part of a matrix
-  * with given dense storage of the coefficients. It is the return type of MatrixBase::selfadjointView()
-  * and most of the time this is the only way that it is used.
-  *
-  * \sa SparseMatrixBase::selfadjointView()
-  */
+ * \class SparseSelfAdjointView
+ *
+ * \brief Pseudo expression to manipulate a triangular sparse matrix as a selfadjoint matrix.
+ *
+ * \param MatrixType the type of the dense matrix storing the coefficients
+ * \param Mode can be either \c #Lower or \c #Upper
+ *
+ * This class is an expression of a sefladjoint matrix from a triangular part of a matrix
+ * with given dense storage of the coefficients. It is the return type of MatrixBase::selfadjointView()
+ * and most of the time this is the only way that it is used.
+ *
+ * \sa SparseMatrixBase::selfadjointView()
+ */
 namespace internal {
-  
-template<typename MatrixType, unsigned int Mode>
-struct traits<SparseSelfAdjointView<MatrixType,Mode> > : traits<MatrixType> {
-};
 
-template<int SrcMode,int DstMode,typename MatrixType,int DestOrder>
-void permute_symm_to_symm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm = 0);
+template <typename MatrixType, unsigned int Mode>
+struct traits<SparseSelfAdjointView<MatrixType, Mode> > : traits<MatrixType> {};
 
-template<int Mode,typename MatrixType,int DestOrder>
-void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm = 0);
+template <int SrcMode, int DstMode, typename MatrixType, int DestOrder>
+void permute_symm_to_symm(
+    const MatrixType& mat,
+    SparseMatrix<typename MatrixType::Scalar, DestOrder, typename MatrixType::StorageIndex>& _dest,
+    const typename MatrixType::StorageIndex* perm = 0);
 
-}
+template <int Mode, typename MatrixType, int DestOrder>
+void permute_symm_to_fullsymm(
+    const MatrixType& mat,
+    SparseMatrix<typename MatrixType::Scalar, DestOrder, typename MatrixType::StorageIndex>& _dest,
+    const typename MatrixType::StorageIndex* perm = 0);
 
-template<typename MatrixType, unsigned int _Mode> class SparseSelfAdjointView
-  : public EigenBase<SparseSelfAdjointView<MatrixType,_Mode> >
-{
-  public:
-    
-    enum {
-      Mode = _Mode,
-      TransposeMode = ((Mode & Upper) ? Lower : 0) | ((Mode & Lower) ? Upper : 0),
-      RowsAtCompileTime = internal::traits<SparseSelfAdjointView>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<SparseSelfAdjointView>::ColsAtCompileTime
-    };
+}  // namespace internal
 
-    typedef EigenBase<SparseSelfAdjointView> Base;
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
-    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
-    typedef typename internal::remove_all<MatrixTypeNested>::type _MatrixTypeNested;
-    
-    explicit inline SparseSelfAdjointView(MatrixType& matrix) : m_matrix(matrix)
-    {
-      eigen_assert(rows()==cols() && "SelfAdjointView is only for squared matrices");
-    }
+template <typename MatrixType, unsigned int Mode_>
+class SparseSelfAdjointView : public EigenBase<SparseSelfAdjointView<MatrixType, Mode_> > {
+ public:
+  enum {
+    Mode = Mode_,
+    TransposeMode = ((Mode & Upper) ? Lower : 0) | ((Mode & Lower) ? Upper : 0),
+    RowsAtCompileTime = internal::traits<SparseSelfAdjointView>::RowsAtCompileTime,
+    ColsAtCompileTime = internal::traits<SparseSelfAdjointView>::ColsAtCompileTime
+  };
 
-    inline Index rows() const { return m_matrix.rows(); }
-    inline Index cols() const { return m_matrix.cols(); }
+  typedef EigenBase<SparseSelfAdjointView> Base;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef Matrix<StorageIndex, Dynamic, 1> VectorI;
+  typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+  typedef internal::remove_all_t<MatrixTypeNested> MatrixTypeNested_;
 
-    /** \internal \returns a reference to the nested matrix */
-    const _MatrixTypeNested& matrix() const { return m_matrix; }
-    typename internal::remove_reference<MatrixTypeNested>::type& matrix() { return m_matrix; }
+  explicit inline SparseSelfAdjointView(MatrixType& matrix) : m_matrix(matrix) {
+    eigen_assert(rows() == cols() && "SelfAdjointView is only for squared matrices");
+  }
 
-    /** \returns an expression of the matrix product between a sparse self-adjoint matrix \c *this and a sparse matrix \a rhs.
-      *
-      * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product.
-      * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product.
-      */
-    template<typename OtherDerived>
-    Product<SparseSelfAdjointView, OtherDerived>
-    operator*(const SparseMatrixBase<OtherDerived>& rhs) const
-    {
-      return Product<SparseSelfAdjointView, OtherDerived>(*this, rhs.derived());
-    }
+  inline Index rows() const { return m_matrix.rows(); }
+  inline Index cols() const { return m_matrix.cols(); }
 
-    /** \returns an expression of the matrix product between a sparse matrix \a lhs and a sparse self-adjoint matrix \a rhs.
-      *
-      * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product.
-      * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product.
-      */
-    template<typename OtherDerived> friend
-    Product<OtherDerived, SparseSelfAdjointView>
-    operator*(const SparseMatrixBase<OtherDerived>& lhs, const SparseSelfAdjointView& rhs)
-    {
-      return Product<OtherDerived, SparseSelfAdjointView>(lhs.derived(), rhs);
-    }
-    
-    /** Efficient sparse self-adjoint matrix times dense vector/matrix product */
-    template<typename OtherDerived>
-    Product<SparseSelfAdjointView,OtherDerived>
-    operator*(const MatrixBase<OtherDerived>& rhs) const
-    {
-      return Product<SparseSelfAdjointView,OtherDerived>(*this, rhs.derived());
-    }
+  /** \internal \returns a reference to the nested matrix */
+  const MatrixTypeNested_& matrix() const { return m_matrix; }
+  std::remove_reference_t<MatrixTypeNested>& matrix() { return m_matrix; }
 
-    /** Efficient dense vector/matrix times sparse self-adjoint matrix product */
-    template<typename OtherDerived> friend
-    Product<OtherDerived,SparseSelfAdjointView>
-    operator*(const MatrixBase<OtherDerived>& lhs, const SparseSelfAdjointView& rhs)
-    {
-      return Product<OtherDerived,SparseSelfAdjointView>(lhs.derived(), rhs);
-    }
+  /** \returns an expression of the matrix product between a sparse self-adjoint matrix \c *this and a sparse matrix \a
+   * rhs.
+   *
+   * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix
+   * product. Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing
+   * the product.
+   */
+  template <typename OtherDerived>
+  Product<SparseSelfAdjointView, OtherDerived> operator*(const SparseMatrixBase<OtherDerived>& rhs) const {
+    return Product<SparseSelfAdjointView, OtherDerived>(*this, rhs.derived());
+  }
 
-    /** Perform a symmetric rank K update of the selfadjoint matrix \c *this:
-      * \f$ this = this + \alpha ( u u^* ) \f$ where \a u is a vector or matrix.
-      *
-      * \returns a reference to \c *this
-      *
-      * To perform \f$ this = this + \alpha ( u^* u ) \f$ you can simply
-      * call this function with u.adjoint().
-      */
-    template<typename DerivedU>
-    SparseSelfAdjointView& rankUpdate(const SparseMatrixBase<DerivedU>& u, const Scalar& alpha = Scalar(1));
-    
-    /** \returns an expression of P H P^-1 */
-    // TODO implement twists in a more evaluator friendly fashion
-    SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode> twistedBy(const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm) const
-    {
-      return SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode>(m_matrix, perm);
-    }
+  /** \returns an expression of the matrix product between a sparse matrix \a lhs and a sparse self-adjoint matrix \a
+   * rhs.
+   *
+   * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix
+   * product. Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing
+   * the product.
+   */
+  template <typename OtherDerived>
+  friend Product<OtherDerived, SparseSelfAdjointView> operator*(const SparseMatrixBase<OtherDerived>& lhs,
+                                                                const SparseSelfAdjointView& rhs) {
+    return Product<OtherDerived, SparseSelfAdjointView>(lhs.derived(), rhs);
+  }
 
-    template<typename SrcMatrixType,int SrcMode>
-    SparseSelfAdjointView& operator=(const SparseSymmetricPermutationProduct<SrcMatrixType,SrcMode>& permutedMatrix)
-    {
-      internal::call_assignment_no_alias_no_transpose(*this, permutedMatrix);
-      return *this;
-    }
+  /** Efficient sparse self-adjoint matrix times dense vector/matrix product */
+  template <typename OtherDerived>
+  Product<SparseSelfAdjointView, OtherDerived> operator*(const MatrixBase<OtherDerived>& rhs) const {
+    return Product<SparseSelfAdjointView, OtherDerived>(*this, rhs.derived());
+  }
 
-    SparseSelfAdjointView& operator=(const SparseSelfAdjointView& src)
-    {
-      PermutationMatrix<Dynamic,Dynamic,StorageIndex> pnull;
-      return *this = src.twistedBy(pnull);
-    }
+  /** Efficient dense vector/matrix times sparse self-adjoint matrix product */
+  template <typename OtherDerived>
+  friend Product<OtherDerived, SparseSelfAdjointView> operator*(const MatrixBase<OtherDerived>& lhs,
+                                                                const SparseSelfAdjointView& rhs) {
+    return Product<OtherDerived, SparseSelfAdjointView>(lhs.derived(), rhs);
+  }
 
-    // Since we override the copy-assignment operator, we need to explicitly re-declare the copy-constructor
-    EIGEN_DEFAULT_COPY_CONSTRUCTOR(SparseSelfAdjointView)
+  /** Perform a symmetric rank K update of the selfadjoint matrix \c *this:
+   * \f$ this = this + \alpha ( u u^* ) \f$ where \a u is a vector or matrix.
+   *
+   * \returns a reference to \c *this
+   *
+   * To perform \f$ this = this + \alpha ( u^* u ) \f$ you can simply
+   * call this function with u.adjoint().
+   */
+  template <typename DerivedU>
+  SparseSelfAdjointView& rankUpdate(const SparseMatrixBase<DerivedU>& u, const Scalar& alpha = Scalar(1));
 
-    template<typename SrcMatrixType,unsigned int SrcMode>
-    SparseSelfAdjointView& operator=(const SparseSelfAdjointView<SrcMatrixType,SrcMode>& src)
-    {
-      PermutationMatrix<Dynamic,Dynamic,StorageIndex> pnull;
-      return *this = src.twistedBy(pnull);
-    }
-    
-    void resize(Index rows, Index cols)
-    {
-      EIGEN_ONLY_USED_FOR_DEBUG(rows);
-      EIGEN_ONLY_USED_FOR_DEBUG(cols);
-      eigen_assert(rows == this->rows() && cols == this->cols()
-                && "SparseSelfadjointView::resize() does not actually allow to resize.");
-    }
-    
-  protected:
+  /** \returns an expression of P H P^-1 */
+  // TODO implement twists in a more evaluator friendly fashion
+  SparseSymmetricPermutationProduct<MatrixTypeNested_, Mode> twistedBy(
+      const PermutationMatrix<Dynamic, Dynamic, StorageIndex>& perm) const {
+    return SparseSymmetricPermutationProduct<MatrixTypeNested_, Mode>(m_matrix, perm);
+  }
 
-    MatrixTypeNested m_matrix;
-    //mutable VectorI m_countPerRow;
-    //mutable VectorI m_countPerCol;
-  private:
-    template<typename Dest> void evalTo(Dest &) const;
+  template <typename SrcMatrixType, int SrcMode>
+  SparseSelfAdjointView& operator=(const SparseSymmetricPermutationProduct<SrcMatrixType, SrcMode>& permutedMatrix) {
+    internal::call_assignment_no_alias_no_transpose(*this, permutedMatrix);
+    return *this;
+  }
+
+  SparseSelfAdjointView& operator=(const SparseSelfAdjointView& src) {
+    PermutationMatrix<Dynamic, Dynamic, StorageIndex> pnull;
+    return *this = src.twistedBy(pnull);
+  }
+
+  // Since we override the copy-assignment operator, we need to explicitly re-declare the copy-constructor
+  EIGEN_DEFAULT_COPY_CONSTRUCTOR(SparseSelfAdjointView)
+
+  template <typename SrcMatrixType, unsigned int SrcMode>
+  SparseSelfAdjointView& operator=(const SparseSelfAdjointView<SrcMatrixType, SrcMode>& src) {
+    PermutationMatrix<Dynamic, Dynamic, StorageIndex> pnull;
+    return *this = src.twistedBy(pnull);
+  }
+
+  void resize(Index rows, Index cols) {
+    EIGEN_ONLY_USED_FOR_DEBUG(rows);
+    EIGEN_ONLY_USED_FOR_DEBUG(cols);
+    eigen_assert(rows == this->rows() && cols == this->cols() &&
+                 "SparseSelfadjointView::resize() does not actually allow to resize.");
+  }
+
+ protected:
+  MatrixTypeNested m_matrix;
+  // mutable VectorI m_countPerRow;
+  // mutable VectorI m_countPerCol;
+ private:
+  template <typename Dest>
+  void evalTo(Dest&) const;
 };
 
 /***************************************************************************
-* Implementation of SparseMatrixBase methods
-***************************************************************************/
+ * Implementation of SparseMatrixBase methods
+ ***************************************************************************/
 
-template<typename Derived>
-template<unsigned int UpLo>
-typename SparseMatrixBase<Derived>::template ConstSelfAdjointViewReturnType<UpLo>::Type SparseMatrixBase<Derived>::selfadjointView() const
-{
+template <typename Derived>
+template <unsigned int UpLo>
+typename SparseMatrixBase<Derived>::template ConstSelfAdjointViewReturnType<UpLo>::Type
+SparseMatrixBase<Derived>::selfadjointView() const {
   return SparseSelfAdjointView<const Derived, UpLo>(derived());
 }
 
-template<typename Derived>
-template<unsigned int UpLo>
-typename SparseMatrixBase<Derived>::template SelfAdjointViewReturnType<UpLo>::Type SparseMatrixBase<Derived>::selfadjointView()
-{
+template <typename Derived>
+template <unsigned int UpLo>
+typename SparseMatrixBase<Derived>::template SelfAdjointViewReturnType<UpLo>::Type
+SparseMatrixBase<Derived>::selfadjointView() {
   return SparseSelfAdjointView<Derived, UpLo>(derived());
 }
 
 /***************************************************************************
-* Implementation of SparseSelfAdjointView methods
-***************************************************************************/
+ * Implementation of SparseSelfAdjointView methods
+ ***************************************************************************/
 
-template<typename MatrixType, unsigned int Mode>
-template<typename DerivedU>
-SparseSelfAdjointView<MatrixType,Mode>&
-SparseSelfAdjointView<MatrixType,Mode>::rankUpdate(const SparseMatrixBase<DerivedU>& u, const Scalar& alpha)
-{
-  SparseMatrix<Scalar,(MatrixType::Flags&RowMajorBit)?RowMajor:ColMajor> tmp = u * u.adjoint();
-  if(alpha==Scalar(0))
+template <typename MatrixType, unsigned int Mode>
+template <typename DerivedU>
+SparseSelfAdjointView<MatrixType, Mode>& SparseSelfAdjointView<MatrixType, Mode>::rankUpdate(
+    const SparseMatrixBase<DerivedU>& u, const Scalar& alpha) {
+  SparseMatrix<Scalar, (MatrixType::Flags & RowMajorBit) ? RowMajor : ColMajor> tmp = u * u.adjoint();
+  if (alpha == Scalar(0))
     m_matrix = tmp.template triangularView<Mode>();
   else
     m_matrix += alpha * tmp.template triangularView<Mode>();
@@ -206,305 +204,273 @@
 }
 
 namespace internal {
-  
+
 // TODO currently a selfadjoint expression has the form SelfAdjointView<.,.>
 //      in the future selfadjoint-ness should be defined by the expression traits
-//      such that Transpose<SelfAdjointView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
-template<typename MatrixType, unsigned int Mode>
-struct evaluator_traits<SparseSelfAdjointView<MatrixType,Mode> >
-{
+//      such that Transpose<SelfAdjointView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to
+//      make it work)
+template <typename MatrixType, unsigned int Mode>
+struct evaluator_traits<SparseSelfAdjointView<MatrixType, Mode> > {
   typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
   typedef SparseSelfAdjointShape Shape;
 };
 
 struct SparseSelfAdjoint2Sparse {};
 
-template<> struct AssignmentKind<SparseShape,SparseSelfAdjointShape> { typedef SparseSelfAdjoint2Sparse Kind; };
-template<> struct AssignmentKind<SparseSelfAdjointShape,SparseShape> { typedef Sparse2Sparse Kind; };
+template <>
+struct AssignmentKind<SparseShape, SparseSelfAdjointShape> {
+  typedef SparseSelfAdjoint2Sparse Kind;
+};
+template <>
+struct AssignmentKind<SparseSelfAdjointShape, SparseShape> {
+  typedef Sparse2Sparse Kind;
+};
 
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, SparseSelfAdjoint2Sparse>
-{
+template <typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, SparseSelfAdjoint2Sparse> {
   typedef typename DstXprType::StorageIndex StorageIndex;
-  typedef internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> AssignOpType;
+  typedef internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar> AssignOpType;
 
-  template<typename DestScalar,int StorageOrder>
-  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src, const AssignOpType&/*func*/)
-  {
+  template <typename DestScalar, int StorageOrder>
+  static void run(SparseMatrix<DestScalar, StorageOrder, StorageIndex>& dst, const SrcXprType& src,
+                  const AssignOpType& /*func*/) {
     internal::permute_symm_to_fullsymm<SrcXprType::Mode>(src.matrix(), dst);
   }
 
-  // FIXME: the handling of += and -= in sparse matrices should be cleanup so that next two overloads could be reduced to:
-  template<typename DestScalar,int StorageOrder,typename AssignFunc>
-  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src, const AssignFunc& func)
-  {
-    SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+  // FIXME: the handling of += and -= in sparse matrices should be cleanup so that next two overloads could be reduced
+  // to:
+  template <typename DestScalar, int StorageOrder, typename AssignFunc>
+  static void run(SparseMatrix<DestScalar, StorageOrder, StorageIndex>& dst, const SrcXprType& src,
+                  const AssignFunc& func) {
+    SparseMatrix<DestScalar, StorageOrder, StorageIndex> tmp(src.rows(), src.cols());
     run(tmp, src, AssignOpType());
     call_assignment_no_alias_no_transpose(dst, tmp, func);
   }
 
-  template<typename DestScalar,int StorageOrder>
-  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src,
-                  const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>& /* func */)
-  {
-    SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+  template <typename DestScalar, int StorageOrder>
+  static void run(SparseMatrix<DestScalar, StorageOrder, StorageIndex>& dst, const SrcXprType& src,
+                  const internal::add_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /* func */) {
+    SparseMatrix<DestScalar, StorageOrder, StorageIndex> tmp(src.rows(), src.cols());
     run(tmp, src, AssignOpType());
     dst += tmp;
   }
 
-  template<typename DestScalar,int StorageOrder>
-  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src,
-                  const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>& /* func */)
-  {
-    SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+  template <typename DestScalar, int StorageOrder>
+  static void run(SparseMatrix<DestScalar, StorageOrder, StorageIndex>& dst, const SrcXprType& src,
+                  const internal::sub_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /* func */) {
+    SparseMatrix<DestScalar, StorageOrder, StorageIndex> tmp(src.rows(), src.cols());
     run(tmp, src, AssignOpType());
     dst -= tmp;
   }
-  
-  template<typename DestScalar>
-  static void run(DynamicSparseMatrix<DestScalar,ColMajor,StorageIndex>& dst, const SrcXprType &src, const AssignOpType&/*func*/)
-  {
-    // TODO directly evaluate into dst;
-    SparseMatrix<DestScalar,ColMajor,StorageIndex> tmp(dst.rows(),dst.cols());
-    internal::permute_symm_to_fullsymm<SrcXprType::Mode>(src.matrix(), tmp);
-    dst = tmp;
-  }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /***************************************************************************
-* Implementation of sparse self-adjoint time dense matrix
-***************************************************************************/
+ * Implementation of sparse self-adjoint time dense matrix
+ ***************************************************************************/
 
 namespace internal {
 
-template<int Mode, typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
-inline void sparse_selfadjoint_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
-{
+template <int Mode, typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
+inline void sparse_selfadjoint_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res,
+                                                  const AlphaType& alpha) {
   EIGEN_ONLY_USED_FOR_DEBUG(alpha);
-  
-  typedef typename internal::nested_eval<SparseLhsType,DenseRhsType::MaxColsAtCompileTime>::type SparseLhsTypeNested;
-  typedef typename internal::remove_all<SparseLhsTypeNested>::type SparseLhsTypeNestedCleaned;
+
+  typedef typename internal::nested_eval<SparseLhsType, DenseRhsType::MaxColsAtCompileTime>::type SparseLhsTypeNested;
+  typedef internal::remove_all_t<SparseLhsTypeNested> SparseLhsTypeNestedCleaned;
   typedef evaluator<SparseLhsTypeNestedCleaned> LhsEval;
   typedef typename LhsEval::InnerIterator LhsIterator;
   typedef typename SparseLhsType::Scalar LhsScalar;
-  
+
   enum {
-    LhsIsRowMajor = (LhsEval::Flags&RowMajorBit)==RowMajorBit,
-    ProcessFirstHalf =
-              ((Mode&(Upper|Lower))==(Upper|Lower))
-          || ( (Mode&Upper) && !LhsIsRowMajor)
-          || ( (Mode&Lower) && LhsIsRowMajor),
+    LhsIsRowMajor = (LhsEval::Flags & RowMajorBit) == RowMajorBit,
+    ProcessFirstHalf = ((Mode & (Upper | Lower)) == (Upper | Lower)) || ((Mode & Upper) && !LhsIsRowMajor) ||
+                       ((Mode & Lower) && LhsIsRowMajor),
     ProcessSecondHalf = !ProcessFirstHalf
   };
-  
+
   SparseLhsTypeNested lhs_nested(lhs);
   LhsEval lhsEval(lhs_nested);
 
   // work on one column at once
-  for (Index k=0; k<rhs.cols(); ++k)
-  {
-    for (Index j=0; j<lhs.outerSize(); ++j)
-    {
-      LhsIterator i(lhsEval,j);
+  for (Index k = 0; k < rhs.cols(); ++k) {
+    for (Index j = 0; j < lhs.outerSize(); ++j) {
+      LhsIterator i(lhsEval, j);
       // handle diagonal coeff
-      if (ProcessSecondHalf)
-      {
-        while (i && i.index()<j) ++i;
-        if(i && i.index()==j)
-        {
-          res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
+      if (ProcessSecondHalf) {
+        while (i && i.index() < j) ++i;
+        if (i && i.index() == j) {
+          res.coeffRef(j, k) += alpha * i.value() * rhs.coeff(j, k);
           ++i;
         }
       }
 
       // premultiplied rhs for scatters
-      typename ScalarBinaryOpTraits<AlphaType, typename DenseRhsType::Scalar>::ReturnType rhs_j(alpha*rhs(j,k));
+      typename ScalarBinaryOpTraits<AlphaType, typename DenseRhsType::Scalar>::ReturnType rhs_j(alpha * rhs(j, k));
       // accumulator for partial scalar product
       typename DenseResType::Scalar res_j(0);
-      for(; (ProcessFirstHalf ? i && i.index() < j : i) ; ++i)
-      {
+      for (; (ProcessFirstHalf ? i && i.index() < j : i); ++i) {
         LhsScalar lhs_ij = i.value();
-        if(!LhsIsRowMajor) lhs_ij = numext::conj(lhs_ij);
-        res_j += lhs_ij * rhs.coeff(i.index(),k);
-        res(i.index(),k) += numext::conj(lhs_ij) * rhs_j;
+        if (!LhsIsRowMajor) lhs_ij = numext::conj(lhs_ij);
+        res_j += lhs_ij * rhs.coeff(i.index(), k);
+        res(i.index(), k) += numext::conj(lhs_ij) * rhs_j;
       }
-      res.coeffRef(j,k) += alpha * res_j;
+      res.coeffRef(j, k) += alpha * res_j;
 
       // handle diagonal coeff
-      if (ProcessFirstHalf && i && (i.index()==j))
-        res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
+      if (ProcessFirstHalf && i && (i.index() == j)) res.coeffRef(j, k) += alpha * i.value() * rhs.coeff(j, k);
     }
   }
 }
 
-
-template<typename LhsView, typename Rhs, int ProductType>
+template <typename LhsView, typename Rhs, int ProductType>
 struct generic_product_impl<LhsView, Rhs, SparseSelfAdjointShape, DenseShape, ProductType>
-: generic_product_impl_base<LhsView, Rhs, generic_product_impl<LhsView, Rhs, SparseSelfAdjointShape, DenseShape, ProductType> >
-{
-  template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const LhsView& lhsView, const Rhs& rhs, const typename Dest::Scalar& alpha)
-  {
-    typedef typename LhsView::_MatrixTypeNested Lhs;
-    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
-    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    : generic_product_impl_base<LhsView, Rhs,
+                                generic_product_impl<LhsView, Rhs, SparseSelfAdjointShape, DenseShape, ProductType> > {
+  template <typename Dest>
+  static void scaleAndAddTo(Dest& dst, const LhsView& lhsView, const Rhs& rhs, const typename Dest::Scalar& alpha) {
+    typedef typename LhsView::MatrixTypeNested_ Lhs;
+    typedef typename nested_eval<Lhs, Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs, Dynamic>::type RhsNested;
     LhsNested lhsNested(lhsView.matrix());
     RhsNested rhsNested(rhs);
-    
+
     internal::sparse_selfadjoint_time_dense_product<LhsView::Mode>(lhsNested, rhsNested, dst, alpha);
   }
 };
 
-template<typename Lhs, typename RhsView, int ProductType>
+template <typename Lhs, typename RhsView, int ProductType>
 struct generic_product_impl<Lhs, RhsView, DenseShape, SparseSelfAdjointShape, ProductType>
-: generic_product_impl_base<Lhs, RhsView, generic_product_impl<Lhs, RhsView, DenseShape, SparseSelfAdjointShape, ProductType> >
-{
-  template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const RhsView& rhsView, const typename Dest::Scalar& alpha)
-  {
-    typedef typename RhsView::_MatrixTypeNested Rhs;
-    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
-    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    : generic_product_impl_base<Lhs, RhsView,
+                                generic_product_impl<Lhs, RhsView, DenseShape, SparseSelfAdjointShape, ProductType> > {
+  template <typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const RhsView& rhsView, const typename Dest::Scalar& alpha) {
+    typedef typename RhsView::MatrixTypeNested_ Rhs;
+    typedef typename nested_eval<Lhs, Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs, Dynamic>::type RhsNested;
     LhsNested lhsNested(lhs);
     RhsNested rhsNested(rhsView.matrix());
-    
+
     // transpose everything
     Transpose<Dest> dstT(dst);
-    internal::sparse_selfadjoint_time_dense_product<RhsView::TransposeMode>(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha);
+    internal::sparse_selfadjoint_time_dense_product<RhsView::TransposeMode>(rhsNested.transpose(),
+                                                                            lhsNested.transpose(), dstT, alpha);
   }
 };
 
 // NOTE: these two overloads are needed to evaluate the sparse selfadjoint view into a full sparse matrix
 // TODO: maybe the copy could be handled by generic_product_impl so that these overloads would not be needed anymore
 
-template<typename LhsView, typename Rhs, int ProductTag>
+template <typename LhsView, typename Rhs, int ProductTag>
 struct product_evaluator<Product<LhsView, Rhs, DefaultProduct>, ProductTag, SparseSelfAdjointShape, SparseShape>
-  : public evaluator<typename Product<typename Rhs::PlainObject, Rhs, DefaultProduct>::PlainObject>
-{
+    : public evaluator<typename Product<typename Rhs::PlainObject, Rhs, DefaultProduct>::PlainObject> {
   typedef Product<LhsView, Rhs, DefaultProduct> XprType;
   typedef typename XprType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  product_evaluator(const XprType& xpr)
-    : m_lhs(xpr.lhs()), m_result(xpr.rows(), xpr.cols())
-  {
-    ::new (static_cast<Base*>(this)) Base(m_result);
-    generic_product_impl<typename Rhs::PlainObject, Rhs, SparseShape, SparseShape, ProductTag>::evalTo(m_result, m_lhs, xpr.rhs());
+  product_evaluator(const XprType& xpr) : m_lhs(xpr.lhs()), m_result(xpr.rows(), xpr.cols()) {
+    internal::construct_at<Base>(this, m_result);
+    generic_product_impl<typename Rhs::PlainObject, Rhs, SparseShape, SparseShape, ProductTag>::evalTo(m_result, m_lhs,
+                                                                                                       xpr.rhs());
   }
-  
-protected:
+
+ protected:
   typename Rhs::PlainObject m_lhs;
   PlainObject m_result;
 };
 
-template<typename Lhs, typename RhsView, int ProductTag>
+template <typename Lhs, typename RhsView, int ProductTag>
 struct product_evaluator<Product<Lhs, RhsView, DefaultProduct>, ProductTag, SparseShape, SparseSelfAdjointShape>
-  : public evaluator<typename Product<Lhs, typename Lhs::PlainObject, DefaultProduct>::PlainObject>
-{
+    : public evaluator<typename Product<Lhs, typename Lhs::PlainObject, DefaultProduct>::PlainObject> {
   typedef Product<Lhs, RhsView, DefaultProduct> XprType;
   typedef typename XprType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
 
-  product_evaluator(const XprType& xpr)
-    : m_rhs(xpr.rhs()), m_result(xpr.rows(), xpr.cols())
-  {
+  product_evaluator(const XprType& xpr) : m_rhs(xpr.rhs()), m_result(xpr.rows(), xpr.cols()) {
     ::new (static_cast<Base*>(this)) Base(m_result);
-    generic_product_impl<Lhs, typename Lhs::PlainObject, SparseShape, SparseShape, ProductTag>::evalTo(m_result, xpr.lhs(), m_rhs);
+    generic_product_impl<Lhs, typename Lhs::PlainObject, SparseShape, SparseShape, ProductTag>::evalTo(
+        m_result, xpr.lhs(), m_rhs);
   }
-  
-protected:
+
+ protected:
   typename Lhs::PlainObject m_rhs;
   PlainObject m_result;
 };
 
-} // namespace internal
+}  // namespace internal
 
 /***************************************************************************
-* Implementation of symmetric copies and permutations
-***************************************************************************/
+ * Implementation of symmetric copies and permutations
+ ***************************************************************************/
 namespace internal {
 
-template<int Mode,typename MatrixType,int DestOrder>
-void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm)
-{
+template <int Mode, typename MatrixType, int DestOrder>
+void permute_symm_to_fullsymm(
+    const MatrixType& mat,
+    SparseMatrix<typename MatrixType::Scalar, DestOrder, typename MatrixType::StorageIndex>& _dest,
+    const typename MatrixType::StorageIndex* perm) {
   typedef typename MatrixType::StorageIndex StorageIndex;
   typedef typename MatrixType::Scalar Scalar;
-  typedef SparseMatrix<Scalar,DestOrder,StorageIndex> Dest;
-  typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+  typedef SparseMatrix<Scalar, DestOrder, StorageIndex> Dest;
+  typedef Matrix<StorageIndex, Dynamic, 1> VectorI;
   typedef evaluator<MatrixType> MatEval;
   typedef typename evaluator<MatrixType>::InnerIterator MatIterator;
-  
+
   MatEval matEval(mat);
   Dest& dest(_dest.derived());
-  enum {
-    StorageOrderMatch = int(Dest::IsRowMajor) == int(MatrixType::IsRowMajor)
-  };
-  
+  enum { StorageOrderMatch = int(Dest::IsRowMajor) == int(MatrixType::IsRowMajor) };
+
   Index size = mat.rows();
   VectorI count;
   count.resize(size);
   count.setZero();
-  dest.resize(size,size);
-  for(Index j = 0; j<size; ++j)
-  {
+  dest.resize(size, size);
+  for (Index j = 0; j < size; ++j) {
     Index jp = perm ? perm[j] : j;
-    for(MatIterator it(matEval,j); it; ++it)
-    {
+    for (MatIterator it(matEval, j); it; ++it) {
       Index i = it.index();
       Index r = it.row();
       Index c = it.col();
       Index ip = perm ? perm[i] : i;
-      if(Mode==int(Upper|Lower))
+      if (Mode == int(Upper | Lower))
         count[StorageOrderMatch ? jp : ip]++;
-      else if(r==c)
+      else if (r == c)
         count[ip]++;
-      else if(( Mode==Lower && r>c) || ( Mode==Upper && r<c))
-      {
+      else if ((Mode == Lower && r > c) || (Mode == Upper && r < c)) {
         count[ip]++;
         count[jp]++;
       }
     }
   }
   Index nnz = count.sum();
-  
+
   // reserve space
   dest.resizeNonZeros(nnz);
   dest.outerIndexPtr()[0] = 0;
-  for(Index j=0; j<size; ++j)
-    dest.outerIndexPtr()[j+1] = dest.outerIndexPtr()[j] + count[j];
-  for(Index j=0; j<size; ++j)
-    count[j] = dest.outerIndexPtr()[j];
-  
+  for (Index j = 0; j < size; ++j) dest.outerIndexPtr()[j + 1] = dest.outerIndexPtr()[j] + count[j];
+  for (Index j = 0; j < size; ++j) count[j] = dest.outerIndexPtr()[j];
+
   // copy data
-  for(StorageIndex j = 0; j<size; ++j)
-  {
-    for(MatIterator it(matEval,j); it; ++it)
-    {
+  for (StorageIndex j = 0; j < size; ++j) {
+    for (MatIterator it(matEval, j); it; ++it) {
       StorageIndex i = internal::convert_index<StorageIndex>(it.index());
       Index r = it.row();
       Index c = it.col();
-      
+
       StorageIndex jp = perm ? perm[j] : j;
       StorageIndex ip = perm ? perm[i] : i;
-      
-      if(Mode==int(Upper|Lower))
-      {
+
+      if (Mode == int(Upper | Lower)) {
         Index k = count[StorageOrderMatch ? jp : ip]++;
         dest.innerIndexPtr()[k] = StorageOrderMatch ? ip : jp;
         dest.valuePtr()[k] = it.value();
-      }
-      else if(r==c)
-      {
+      } else if (r == c) {
         Index k = count[ip]++;
         dest.innerIndexPtr()[k] = ip;
         dest.valuePtr()[k] = it.value();
-      }
-      else if(( (Mode&Lower)==Lower && r>c) || ( (Mode&Upper)==Upper && r<c))
-      {
-        if(!StorageOrderMatch)
-          std::swap(ip,jp);
+      } else if (((Mode & Lower) == Lower && r > c) || ((Mode & Upper) == Upper && r < c)) {
+        if (!StorageOrderMatch) std::swap(ip, jp);
         Index k = count[jp]++;
         dest.innerIndexPtr()[k] = ip;
         dest.valuePtr()[k] = it.value();
@@ -516,66 +482,58 @@
   }
 }
 
-template<int _SrcMode,int _DstMode,typename MatrixType,int DstOrder>
-void permute_symm_to_symm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DstOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm)
-{
+template <int SrcMode_, int DstMode_, typename MatrixType, int DstOrder>
+void permute_symm_to_symm(const MatrixType& mat,
+                          SparseMatrix<typename MatrixType::Scalar, DstOrder, typename MatrixType::StorageIndex>& _dest,
+                          const typename MatrixType::StorageIndex* perm) {
   typedef typename MatrixType::StorageIndex StorageIndex;
   typedef typename MatrixType::Scalar Scalar;
-  SparseMatrix<Scalar,DstOrder,StorageIndex>& dest(_dest.derived());
-  typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+  SparseMatrix<Scalar, DstOrder, StorageIndex>& dest(_dest.derived());
+  typedef Matrix<StorageIndex, Dynamic, 1> VectorI;
   typedef evaluator<MatrixType> MatEval;
   typedef typename evaluator<MatrixType>::InnerIterator MatIterator;
 
   enum {
     SrcOrder = MatrixType::IsRowMajor ? RowMajor : ColMajor,
     StorageOrderMatch = int(SrcOrder) == int(DstOrder),
-    DstMode = DstOrder==RowMajor ? (_DstMode==Upper ? Lower : Upper) : _DstMode,
-    SrcMode = SrcOrder==RowMajor ? (_SrcMode==Upper ? Lower : Upper) : _SrcMode
+    DstMode = DstOrder == RowMajor ? (DstMode_ == Upper ? Lower : Upper) : DstMode_,
+    SrcMode = SrcOrder == RowMajor ? (SrcMode_ == Upper ? Lower : Upper) : SrcMode_
   };
 
   MatEval matEval(mat);
-  
+
   Index size = mat.rows();
   VectorI count(size);
   count.setZero();
-  dest.resize(size,size);
-  for(StorageIndex j = 0; j<size; ++j)
-  {
+  dest.resize(size, size);
+  for (StorageIndex j = 0; j < size; ++j) {
     StorageIndex jp = perm ? perm[j] : j;
-    for(MatIterator it(matEval,j); it; ++it)
-    {
+    for (MatIterator it(matEval, j); it; ++it) {
       StorageIndex i = it.index();
-      if((int(SrcMode)==int(Lower) && i<j) || (int(SrcMode)==int(Upper) && i>j))
-        continue;
-                  
+      if ((int(SrcMode) == int(Lower) && i < j) || (int(SrcMode) == int(Upper) && i > j)) continue;
+
       StorageIndex ip = perm ? perm[i] : i;
-      count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++;
+      count[int(DstMode) == int(Lower) ? (std::min)(ip, jp) : (std::max)(ip, jp)]++;
     }
   }
   dest.outerIndexPtr()[0] = 0;
-  for(Index j=0; j<size; ++j)
-    dest.outerIndexPtr()[j+1] = dest.outerIndexPtr()[j] + count[j];
+  for (Index j = 0; j < size; ++j) dest.outerIndexPtr()[j + 1] = dest.outerIndexPtr()[j] + count[j];
   dest.resizeNonZeros(dest.outerIndexPtr()[size]);
-  for(Index j=0; j<size; ++j)
-    count[j] = dest.outerIndexPtr()[j];
-  
-  for(StorageIndex j = 0; j<size; ++j)
-  {
-    
-    for(MatIterator it(matEval,j); it; ++it)
-    {
+  for (Index j = 0; j < size; ++j) count[j] = dest.outerIndexPtr()[j];
+
+  for (StorageIndex j = 0; j < size; ++j) {
+    for (MatIterator it(matEval, j); it; ++it) {
       StorageIndex i = it.index();
-      if((int(SrcMode)==int(Lower) && i<j) || (int(SrcMode)==int(Upper) && i>j))
-        continue;
-                  
+      if ((int(SrcMode) == int(Lower) && i < j) || (int(SrcMode) == int(Upper) && i > j)) continue;
+
       StorageIndex jp = perm ? perm[j] : j;
-      StorageIndex ip = perm? perm[i] : i;
-      
-      Index k = count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++;
-      dest.innerIndexPtr()[k] = int(DstMode)==int(Lower) ? (std::max)(ip,jp) : (std::min)(ip,jp);
-      
-      if(!StorageOrderMatch) std::swap(ip,jp);
-      if( ((int(DstMode)==int(Lower) && ip<jp) || (int(DstMode)==int(Upper) && ip>jp)))
+      StorageIndex ip = perm ? perm[i] : i;
+
+      Index k = count[int(DstMode) == int(Lower) ? (std::min)(ip, jp) : (std::max)(ip, jp)]++;
+      dest.innerIndexPtr()[k] = int(DstMode) == int(Lower) ? (std::max)(ip, jp) : (std::min)(ip, jp);
+
+      if (!StorageOrderMatch) std::swap(ip, jp);
+      if (((int(DstMode) == int(Lower) && ip < jp) || (int(DstMode) == int(Upper) && ip > jp)))
         dest.valuePtr()[k] = numext::conj(it.value());
       else
         dest.valuePtr()[k] = it.value();
@@ -583,77 +541,73 @@
   }
 }
 
-}
+}  // namespace internal
 
 // TODO implement twists in a more evaluator friendly fashion
 
 namespace internal {
 
-template<typename MatrixType, int Mode>
-struct traits<SparseSymmetricPermutationProduct<MatrixType,Mode> > : traits<MatrixType> {
-};
+template <typename MatrixType, int Mode>
+struct traits<SparseSymmetricPermutationProduct<MatrixType, Mode> > : traits<MatrixType> {};
 
-}
+}  // namespace internal
 
-template<typename MatrixType,int Mode>
-class SparseSymmetricPermutationProduct
-  : public EigenBase<SparseSymmetricPermutationProduct<MatrixType,Mode> >
-{
-  public:
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    enum {
-      RowsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::ColsAtCompileTime
-    };
-  protected:
-    typedef PermutationMatrix<Dynamic,Dynamic,StorageIndex> Perm;
-  public:
-    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
-    typedef typename MatrixType::Nested MatrixTypeNested;
-    typedef typename internal::remove_all<MatrixTypeNested>::type NestedExpression;
-    
-    SparseSymmetricPermutationProduct(const MatrixType& mat, const Perm& perm)
-      : m_matrix(mat), m_perm(perm)
-    {}
-    
-    inline Index rows() const { return m_matrix.rows(); }
-    inline Index cols() const { return m_matrix.cols(); }
-        
-    const NestedExpression& matrix() const { return m_matrix; }
-    const Perm& perm() const { return m_perm; }
-    
-  protected:
-    MatrixTypeNested m_matrix;
-    const Perm& m_perm;
+template <typename MatrixType, int Mode>
+class SparseSymmetricPermutationProduct : public EigenBase<SparseSymmetricPermutationProduct<MatrixType, Mode> > {
+ public:
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  enum {
+    RowsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::RowsAtCompileTime,
+    ColsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::ColsAtCompileTime
+  };
 
+ protected:
+  typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> Perm;
+
+ public:
+  typedef Matrix<StorageIndex, Dynamic, 1> VectorI;
+  typedef typename MatrixType::Nested MatrixTypeNested;
+  typedef internal::remove_all_t<MatrixTypeNested> NestedExpression;
+
+  SparseSymmetricPermutationProduct(const MatrixType& mat, const Perm& perm) : m_matrix(mat), m_perm(perm) {}
+
+  inline Index rows() const { return m_matrix.rows(); }
+  inline Index cols() const { return m_matrix.cols(); }
+
+  const NestedExpression& matrix() const { return m_matrix; }
+  const Perm& perm() const { return m_perm; }
+
+ protected:
+  MatrixTypeNested m_matrix;
+  const Perm& m_perm;
 };
 
 namespace internal {
-  
-template<typename DstXprType, typename MatrixType, int Mode, typename Scalar>
-struct Assignment<DstXprType, SparseSymmetricPermutationProduct<MatrixType,Mode>, internal::assign_op<Scalar,typename MatrixType::Scalar>, Sparse2Sparse>
-{
-  typedef SparseSymmetricPermutationProduct<MatrixType,Mode> SrcXprType;
+
+template <typename DstXprType, typename MatrixType, int Mode, typename Scalar>
+struct Assignment<DstXprType, SparseSymmetricPermutationProduct<MatrixType, Mode>,
+                  internal::assign_op<Scalar, typename MatrixType::Scalar>, Sparse2Sparse> {
+  typedef SparseSymmetricPermutationProduct<MatrixType, Mode> SrcXprType;
   typedef typename DstXprType::StorageIndex DstIndex;
-  template<int Options>
-  static void run(SparseMatrix<Scalar,Options,DstIndex> &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename MatrixType::Scalar> &)
-  {
+  template <int Options>
+  static void run(SparseMatrix<Scalar, Options, DstIndex>& dst, const SrcXprType& src,
+                  const internal::assign_op<Scalar, typename MatrixType::Scalar>&) {
     // internal::permute_symm_to_fullsymm<Mode>(m_matrix,_dest,m_perm.indices().data());
-    SparseMatrix<Scalar,(Options&RowMajor)==RowMajor ? ColMajor : RowMajor, DstIndex> tmp;
-    internal::permute_symm_to_fullsymm<Mode>(src.matrix(),tmp,src.perm().indices().data());
+    SparseMatrix<Scalar, (Options & RowMajor) == RowMajor ? ColMajor : RowMajor, DstIndex> tmp;
+    internal::permute_symm_to_fullsymm<Mode>(src.matrix(), tmp, src.perm().indices().data());
     dst = tmp;
   }
-  
-  template<typename DestType,unsigned int DestMode>
-  static void run(SparseSelfAdjointView<DestType,DestMode>& dst, const SrcXprType &src, const internal::assign_op<Scalar,typename MatrixType::Scalar> &)
-  {
-    internal::permute_symm_to_symm<Mode,DestMode>(src.matrix(),dst.matrix(),src.perm().indices().data());
+
+  template <typename DestType, unsigned int DestMode>
+  static void run(SparseSelfAdjointView<DestType, DestMode>& dst, const SrcXprType& src,
+                  const internal::assign_op<Scalar, typename MatrixType::Scalar>&) {
+    internal::permute_symm_to_symm<Mode, DestMode>(src.matrix(), dst.matrix(), src.perm().indices().data());
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H
+#endif  // EIGEN_SPARSE_SELFADJOINTVIEW_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h
index b4c9a42..d67a677 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h
@@ -10,115 +10,106 @@
 #ifndef EIGEN_SPARSESOLVERBASE_H
 #define EIGEN_SPARSESOLVERBASE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-  /** \internal
-  * Helper functions to solve with a sparse right-hand-side and result.
-  * The rhs is decomposed into small vertical panels which are solved through dense temporaries.
-  */
-template<typename Decomposition, typename Rhs, typename Dest>
-typename enable_if<Rhs::ColsAtCompileTime!=1 && Dest::ColsAtCompileTime!=1>::type
-solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest)
-{
-  EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+/** \internal
+ * Helper functions to solve with a sparse right-hand-side and result.
+ * The rhs is decomposed into small vertical panels which are solved through dense temporaries.
+ */
+template <typename Decomposition, typename Rhs, typename Dest>
+std::enable_if_t<Rhs::ColsAtCompileTime != 1 && Dest::ColsAtCompileTime != 1> solve_sparse_through_dense_panels(
+    const Decomposition& dec, const Rhs& rhs, Dest& dest) {
+  EIGEN_STATIC_ASSERT((Dest::Flags & RowMajorBit) == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
   typedef typename Dest::Scalar DestScalar;
   // we process the sparse rhs per block of NbColsAtOnce columns temporarily stored into a dense matrix.
   static const Index NbColsAtOnce = 4;
   Index rhsCols = rhs.cols();
   Index size = rhs.rows();
   // the temporary matrices do not need more columns than NbColsAtOnce:
-  Index tmpCols = (std::min)(rhsCols, NbColsAtOnce); 
-  Eigen::Matrix<DestScalar,Dynamic,Dynamic> tmp(size,tmpCols);
-  Eigen::Matrix<DestScalar,Dynamic,Dynamic> tmpX(size,tmpCols);
-  for(Index k=0; k<rhsCols; k+=NbColsAtOnce)
-  {
-    Index actualCols = std::min<Index>(rhsCols-k, NbColsAtOnce);
-    tmp.leftCols(actualCols) = rhs.middleCols(k,actualCols);
+  Index tmpCols = (std::min)(rhsCols, NbColsAtOnce);
+  Eigen::Matrix<DestScalar, Dynamic, Dynamic> tmp(size, tmpCols);
+  Eigen::Matrix<DestScalar, Dynamic, Dynamic> tmpX(size, tmpCols);
+  for (Index k = 0; k < rhsCols; k += NbColsAtOnce) {
+    Index actualCols = std::min<Index>(rhsCols - k, NbColsAtOnce);
+    tmp.leftCols(actualCols) = rhs.middleCols(k, actualCols);
     tmpX.leftCols(actualCols) = dec.solve(tmp.leftCols(actualCols));
-    dest.middleCols(k,actualCols) = tmpX.leftCols(actualCols).sparseView();
+    dest.middleCols(k, actualCols) = tmpX.leftCols(actualCols).sparseView();
   }
 }
 
 // Overload for vector as rhs
-template<typename Decomposition, typename Rhs, typename Dest>
-typename enable_if<Rhs::ColsAtCompileTime==1 || Dest::ColsAtCompileTime==1>::type
-solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest)
-{
+template <typename Decomposition, typename Rhs, typename Dest>
+std::enable_if_t<Rhs::ColsAtCompileTime == 1 || Dest::ColsAtCompileTime == 1> solve_sparse_through_dense_panels(
+    const Decomposition& dec, const Rhs& rhs, Dest& dest) {
   typedef typename Dest::Scalar DestScalar;
   Index size = rhs.rows();
-  Eigen::Matrix<DestScalar,Dynamic,1> rhs_dense(rhs);
-  Eigen::Matrix<DestScalar,Dynamic,1> dest_dense(size);
+  Eigen::Matrix<DestScalar, Dynamic, 1> rhs_dense(rhs);
+  Eigen::Matrix<DestScalar, Dynamic, 1> dest_dense(size);
   dest_dense = dec.solve(rhs_dense);
   dest = dest_dense.sparseView();
 }
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \class SparseSolverBase
-  * \ingroup SparseCore_Module
-  * \brief A base class for sparse solvers
-  *
-  * \tparam Derived the actual type of the solver.
-  *
-  */
-template<typename Derived>
-class SparseSolverBase : internal::noncopyable
-{
-  public:
+ * \ingroup SparseCore_Module
+ * \brief A base class for sparse solvers
+ *
+ * \tparam Derived the actual type of the solver.
+ *
+ */
+template <typename Derived>
+class SparseSolverBase : internal::noncopyable {
+ public:
+  /** Default constructor */
+  SparseSolverBase() : m_isInitialized(false) {}
 
-    /** Default constructor */
-    SparseSolverBase()
-      : m_isInitialized(false)
-    {}
+  SparseSolverBase(SparseSolverBase&& other) : internal::noncopyable{}, m_isInitialized{other.m_isInitialized} {}
 
-    ~SparseSolverBase()
-    {}
+  ~SparseSolverBase() {}
 
-    Derived& derived() { return *static_cast<Derived*>(this); }
-    const Derived& derived() const { return *static_cast<const Derived*>(this); }
-    
-    /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
-      *
-      * \sa compute()
-      */
-    template<typename Rhs>
-    inline const Solve<Derived, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "Solver is not initialized.");
-      eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
-      return Solve<Derived, Rhs>(derived(), b.derived());
-    }
-    
-    /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
-      *
-      * \sa compute()
-      */
-    template<typename Rhs>
-    inline const Solve<Derived, Rhs>
-    solve(const SparseMatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "Solver is not initialized.");
-      eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
-      return Solve<Derived, Rhs>(derived(), b.derived());
-    }
-    
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal default implementation of solving with a sparse rhs */
-    template<typename Rhs,typename Dest>
-    void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
-    {
-      internal::solve_sparse_through_dense_panels(derived(), b.derived(), dest.derived());
-    }
-    #endif // EIGEN_PARSED_BY_DOXYGEN
+  Derived& derived() { return *static_cast<Derived*>(this); }
+  const Derived& derived() const { return *static_cast<const Derived*>(this); }
 
-  protected:
-    
-    mutable bool m_isInitialized;
+  /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
+   *
+   * \sa compute()
+   */
+  template <typename Rhs>
+  inline const Solve<Derived, Rhs> solve(const MatrixBase<Rhs>& b) const {
+    eigen_assert(m_isInitialized && "Solver is not initialized.");
+    eigen_assert(derived().rows() == b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+    return Solve<Derived, Rhs>(derived(), b.derived());
+  }
+
+  /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
+   *
+   * \sa compute()
+   */
+  template <typename Rhs>
+  inline const Solve<Derived, Rhs> solve(const SparseMatrixBase<Rhs>& b) const {
+    eigen_assert(m_isInitialized && "Solver is not initialized.");
+    eigen_assert(derived().rows() == b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+    return Solve<Derived, Rhs>(derived(), b.derived());
+  }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  /** \internal default implementation of solving with a sparse rhs */
+  template <typename Rhs, typename Dest>
+  void _solve_impl(const SparseMatrixBase<Rhs>& b, SparseMatrixBase<Dest>& dest) const {
+    internal::solve_sparse_through_dense_panels(derived(), b.derived(), dest.derived());
+  }
+#endif  // EIGEN_PARSED_BY_DOXYGEN
+
+ protected:
+  mutable bool m_isInitialized;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSESOLVERBASE_H
+#endif  // EIGEN_SPARSESOLVERBASE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
index 88820a4..6e1c9cf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
@@ -10,39 +10,41 @@
 #ifndef EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
 #define EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-
 // perform a pseudo in-place sparse * sparse product assuming all matrices are col major
-template<typename Lhs, typename Rhs, typename ResultType>
-static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, const typename ResultType::RealScalar& tolerance)
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res,
+                                                    const typename ResultType::RealScalar& tolerance) {
   // return sparse_sparse_product_with_pruning_impl2(lhs,rhs,res);
 
-  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
-  typedef typename remove_all<ResultType>::type::Scalar ResScalar;
-  typedef typename remove_all<Lhs>::type::StorageIndex StorageIndex;
+  typedef typename remove_all_t<Rhs>::Scalar RhsScalar;
+  typedef typename remove_all_t<ResultType>::Scalar ResScalar;
+  typedef typename remove_all_t<Lhs>::StorageIndex StorageIndex;
 
   // make sure to call innerSize/outerSize since we fake the storage order.
   Index rows = lhs.innerSize();
   Index cols = rhs.outerSize();
-  //Index size = lhs.outerSize();
+  // Index size = lhs.outerSize();
   eigen_assert(lhs.outerSize() == rhs.innerSize());
 
   // allocate a temporary buffer
-  AmbiVector<ResScalar,StorageIndex> tempVector(rows);
+  AmbiVector<ResScalar, StorageIndex> tempVector(rows);
 
   // mimics a resizeByInnerOuter:
-  if(ResultType::IsRowMajor)
+  if (ResultType::IsRowMajor)
     res.resize(cols, rows);
   else
     res.resize(rows, cols);
-  
+
   evaluator<Lhs> lhsEval(lhs);
   evaluator<Rhs> rhsEval(rhs);
-  
+
   // estimate the number of non zero entries
   // given a rhs column containing Y non zeros, we assume that the respective Y columns
   // of the lhs differs in average of one non zeros, thus the number of non zeros for
@@ -52,147 +54,131 @@
   Index estimated_nnz_prod = lhsEval.nonZerosEstimate() + rhsEval.nonZerosEstimate();
 
   res.reserve(estimated_nnz_prod);
-  double ratioColRes = double(estimated_nnz_prod)/(double(lhs.rows())*double(rhs.cols()));
-  for (Index j=0; j<cols; ++j)
-  {
+  double ratioColRes = double(estimated_nnz_prod) / (double(lhs.rows()) * double(rhs.cols()));
+  for (Index j = 0; j < cols; ++j) {
     // FIXME:
-    //double ratioColRes = (double(rhs.innerVector(j).nonZeros()) + double(lhs.nonZeros())/double(lhs.cols()))/double(lhs.rows());
+    // double ratioColRes = (double(rhs.innerVector(j).nonZeros()) +
+    // double(lhs.nonZeros())/double(lhs.cols()))/double(lhs.rows());
     // let's do a more accurate determination of the nnz ratio for the current column j of res
     tempVector.init(ratioColRes);
     tempVector.setZero();
-    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
-    {
+    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt) {
       // FIXME should be written like this: tmp += rhsIt.value() * lhs.col(rhsIt.index())
       tempVector.restart();
       RhsScalar x = rhsIt.value();
-      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, rhsIt.index()); lhsIt; ++lhsIt)
-      {
+      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, rhsIt.index()); lhsIt; ++lhsIt) {
         tempVector.coeffRef(lhsIt.index()) += lhsIt.value() * x;
       }
     }
     res.startVec(j);
-    for (typename AmbiVector<ResScalar,StorageIndex>::Iterator it(tempVector,tolerance); it; ++it)
-      res.insertBackByOuterInner(j,it.index()) = it.value();
+    for (typename AmbiVector<ResScalar, StorageIndex>::Iterator it(tempVector, tolerance); it; ++it)
+      res.insertBackByOuterInner(j, it.index()) = it.value();
   }
   res.finalize();
 }
 
-template<typename Lhs, typename Rhs, typename ResultType,
-  int LhsStorageOrder = traits<Lhs>::Flags&RowMajorBit,
-  int RhsStorageOrder = traits<Rhs>::Flags&RowMajorBit,
-  int ResStorageOrder = traits<ResultType>::Flags&RowMajorBit>
+template <typename Lhs, typename Rhs, typename ResultType, int LhsStorageOrder = traits<Lhs>::Flags & RowMajorBit,
+          int RhsStorageOrder = traits<Rhs>::Flags & RowMajorBit,
+          int ResStorageOrder = traits<ResultType>::Flags & RowMajorBit>
 struct sparse_sparse_product_with_pruning_selector;
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, ColMajor, ColMajor, ColMajor> {
   typedef typename ResultType::RealScalar RealScalar;
 
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
-    typename remove_all<ResultType>::type _res(res.rows(), res.cols());
-    internal::sparse_sparse_product_with_pruning_impl<Lhs,Rhs,ResultType>(lhs, rhs, _res, tolerance);
-    res.swap(_res);
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
+    remove_all_t<ResultType> res_(res.rows(), res.cols());
+    internal::sparse_sparse_product_with_pruning_impl<Lhs, Rhs, ResultType>(lhs, rhs, res_, tolerance);
+    res.swap(res_);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,RowMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, ColMajor, ColMajor, RowMajor> {
   typedef typename ResultType::RealScalar RealScalar;
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
     // we need a col-major matrix to hold the result
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> SparseTemporaryType;
-    SparseTemporaryType _res(res.rows(), res.cols());
-    internal::sparse_sparse_product_with_pruning_impl<Lhs,Rhs,SparseTemporaryType>(lhs, rhs, _res, tolerance);
-    res = _res;
+    typedef SparseMatrix<typename ResultType::Scalar, ColMajor, typename ResultType::StorageIndex> SparseTemporaryType;
+    SparseTemporaryType res_(res.rows(), res.cols());
+    internal::sparse_sparse_product_with_pruning_impl<Lhs, Rhs, SparseTemporaryType>(lhs, rhs, res_, tolerance);
+    res = res_;
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,RowMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, RowMajor, RowMajor, RowMajor> {
   typedef typename ResultType::RealScalar RealScalar;
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
     // let's transpose the product to get a column x column product
-    typename remove_all<ResultType>::type _res(res.rows(), res.cols());
-    internal::sparse_sparse_product_with_pruning_impl<Rhs,Lhs,ResultType>(rhs, lhs, _res, tolerance);
-    res.swap(_res);
+    remove_all_t<ResultType> res_(res.rows(), res.cols());
+    internal::sparse_sparse_product_with_pruning_impl<Rhs, Lhs, ResultType>(rhs, lhs, res_, tolerance);
+    res.swap(res_);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,ColMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, RowMajor, RowMajor, ColMajor> {
   typedef typename ResultType::RealScalar RealScalar;
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
-    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
-    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
+    typedef SparseMatrix<typename Lhs::Scalar, ColMajor, typename Lhs::StorageIndex> ColMajorMatrixLhs;
+    typedef SparseMatrix<typename Rhs::Scalar, ColMajor, typename Lhs::StorageIndex> ColMajorMatrixRhs;
     ColMajorMatrixLhs colLhs(lhs);
     ColMajorMatrixRhs colRhs(rhs);
-    internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,ColMajorMatrixRhs,ResultType>(colLhs, colRhs, res, tolerance);
+    internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs, ColMajorMatrixRhs, ResultType>(colLhs, colRhs,
+                                                                                                        res, tolerance);
 
     // let's transpose the product to get a column x column product
-//     typedef SparseMatrix<typename ResultType::Scalar> SparseTemporaryType;
-//     SparseTemporaryType _res(res.cols(), res.rows());
-//     sparse_sparse_product_with_pruning_impl<Rhs,Lhs,SparseTemporaryType>(rhs, lhs, _res);
-//     res = _res.transpose();
+    //     typedef SparseMatrix<typename ResultType::Scalar> SparseTemporaryType;
+    //     SparseTemporaryType res_(res.cols(), res.rows());
+    //     sparse_sparse_product_with_pruning_impl<Rhs,Lhs,SparseTemporaryType>(rhs, lhs, res_);
+    //     res = res_.transpose();
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,RowMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, ColMajor, RowMajor, RowMajor> {
   typedef typename ResultType::RealScalar RealScalar;
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
-    typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixLhs;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
+    typedef SparseMatrix<typename Lhs::Scalar, RowMajor, typename Lhs::StorageIndex> RowMajorMatrixLhs;
     RowMajorMatrixLhs rowLhs(lhs);
-    sparse_sparse_product_with_pruning_selector<RowMajorMatrixLhs,Rhs,ResultType,RowMajor,RowMajor>(rowLhs,rhs,res,tolerance);
+    sparse_sparse_product_with_pruning_selector<RowMajorMatrixLhs, Rhs, ResultType, RowMajor, RowMajor>(rowLhs, rhs,
+                                                                                                        res, tolerance);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,RowMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, RowMajor, ColMajor, RowMajor> {
   typedef typename ResultType::RealScalar RealScalar;
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
-    typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixRhs;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
+    typedef SparseMatrix<typename Rhs::Scalar, RowMajor, typename Lhs::StorageIndex> RowMajorMatrixRhs;
     RowMajorMatrixRhs rowRhs(rhs);
-    sparse_sparse_product_with_pruning_selector<Lhs,RowMajorMatrixRhs,ResultType,RowMajor,RowMajor,RowMajor>(lhs,rowRhs,res,tolerance);
+    sparse_sparse_product_with_pruning_selector<Lhs, RowMajorMatrixRhs, ResultType, RowMajor, RowMajor, RowMajor>(
+        lhs, rowRhs, res, tolerance);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,ColMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, ColMajor, RowMajor, ColMajor> {
   typedef typename ResultType::RealScalar RealScalar;
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
-    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
+    typedef SparseMatrix<typename Rhs::Scalar, ColMajor, typename Lhs::StorageIndex> ColMajorMatrixRhs;
     ColMajorMatrixRhs colRhs(rhs);
-    internal::sparse_sparse_product_with_pruning_impl<Lhs,ColMajorMatrixRhs,ResultType>(lhs, colRhs, res, tolerance);
+    internal::sparse_sparse_product_with_pruning_impl<Lhs, ColMajorMatrixRhs, ResultType>(lhs, colRhs, res, tolerance);
   }
 };
 
-template<typename Lhs, typename Rhs, typename ResultType>
-struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,ColMajor>
-{
+template <typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs, Rhs, ResultType, RowMajor, ColMajor, ColMajor> {
   typedef typename ResultType::RealScalar RealScalar;
-  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
-  {
-    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) {
+    typedef SparseMatrix<typename Lhs::Scalar, ColMajor, typename Lhs::StorageIndex> ColMajorMatrixLhs;
     ColMajorMatrixLhs colLhs(lhs);
-    internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,Rhs,ResultType>(colLhs, rhs, res, tolerance);
+    internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs, Rhs, ResultType>(colLhs, rhs, res, tolerance);
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
+#endif  // EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h
index 3757d4c..158f778 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h
@@ -10,83 +10,74 @@
 #ifndef EIGEN_SPARSETRANSPOSE_H
 #define EIGEN_SPARSETRANSPOSE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
-  template<typename MatrixType,int CompressedAccess=int(MatrixType::Flags&CompressedAccessBit)>
-  class SparseTransposeImpl
-    : public SparseMatrixBase<Transpose<MatrixType> >
-  {};
-  
-  template<typename MatrixType>
-  class SparseTransposeImpl<MatrixType,CompressedAccessBit>
-    : public SparseCompressedBase<Transpose<MatrixType> >
-  {
-    typedef SparseCompressedBase<Transpose<MatrixType> > Base;
-  public:
-    using Base::derived;
-    typedef typename Base::Scalar Scalar;
-    typedef typename Base::StorageIndex StorageIndex;
+template <typename MatrixType, int CompressedAccess = int(MatrixType::Flags & CompressedAccessBit)>
+class SparseTransposeImpl : public SparseMatrixBase<Transpose<MatrixType> > {};
 
-    inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); }
-    
-    inline const Scalar* valuePtr() const { return derived().nestedExpression().valuePtr(); }
-    inline const StorageIndex* innerIndexPtr() const { return derived().nestedExpression().innerIndexPtr(); }
-    inline const StorageIndex* outerIndexPtr() const { return derived().nestedExpression().outerIndexPtr(); }
-    inline const StorageIndex* innerNonZeroPtr() const { return derived().nestedExpression().innerNonZeroPtr(); }
+template <typename MatrixType>
+class SparseTransposeImpl<MatrixType, CompressedAccessBit> : public SparseCompressedBase<Transpose<MatrixType> > {
+  typedef SparseCompressedBase<Transpose<MatrixType> > Base;
 
-    inline Scalar* valuePtr() { return derived().nestedExpression().valuePtr(); }
-    inline StorageIndex* innerIndexPtr() { return derived().nestedExpression().innerIndexPtr(); }
-    inline StorageIndex* outerIndexPtr() { return derived().nestedExpression().outerIndexPtr(); }
-    inline StorageIndex* innerNonZeroPtr() { return derived().nestedExpression().innerNonZeroPtr(); }
+ public:
+  using Base::derived;
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::StorageIndex StorageIndex;
+
+  inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); }
+
+  inline const Scalar* valuePtr() const { return derived().nestedExpression().valuePtr(); }
+  inline const StorageIndex* innerIndexPtr() const { return derived().nestedExpression().innerIndexPtr(); }
+  inline const StorageIndex* outerIndexPtr() const { return derived().nestedExpression().outerIndexPtr(); }
+  inline const StorageIndex* innerNonZeroPtr() const { return derived().nestedExpression().innerNonZeroPtr(); }
+
+  inline Scalar* valuePtr() { return derived().nestedExpression().valuePtr(); }
+  inline StorageIndex* innerIndexPtr() { return derived().nestedExpression().innerIndexPtr(); }
+  inline StorageIndex* outerIndexPtr() { return derived().nestedExpression().outerIndexPtr(); }
+  inline StorageIndex* innerNonZeroPtr() { return derived().nestedExpression().innerNonZeroPtr(); }
+};
+}  // namespace internal
+
+template <typename MatrixType>
+class TransposeImpl<MatrixType, Sparse> : public internal::SparseTransposeImpl<MatrixType> {
+ protected:
+  typedef internal::SparseTransposeImpl<MatrixType> Base;
+};
+
+namespace internal {
+
+template <typename ArgType>
+struct unary_evaluator<Transpose<ArgType>, IteratorBased> : public evaluator_base<Transpose<ArgType> > {
+  typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+ public:
+  typedef Transpose<ArgType> XprType;
+
+  inline Index nonZerosEstimate() const { return m_argImpl.nonZerosEstimate(); }
+
+  class InnerIterator : public EvalIterator {
+   public:
+    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+        : EvalIterator(unaryOp.m_argImpl, outer) {}
+
+    Index row() const { return EvalIterator::col(); }
+    Index col() const { return EvalIterator::row(); }
   };
-}
-  
-template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>
-  : public internal::SparseTransposeImpl<MatrixType>
-{
-  protected:
-    typedef internal::SparseTransposeImpl<MatrixType> Base;
+
+  enum { CoeffReadCost = evaluator<ArgType>::CoeffReadCost, Flags = XprType::Flags };
+
+  explicit unary_evaluator(const XprType& op) : m_argImpl(op.nestedExpression()) {}
+
+ protected:
+  evaluator<ArgType> m_argImpl;
 };
 
-namespace internal {
-  
-template<typename ArgType>
-struct unary_evaluator<Transpose<ArgType>, IteratorBased>
-  : public evaluator_base<Transpose<ArgType> >
-{
-    typedef typename evaluator<ArgType>::InnerIterator        EvalIterator;
-  public:
-    typedef Transpose<ArgType> XprType;
-    
-    inline Index nonZerosEstimate() const {
-      return m_argImpl.nonZerosEstimate();
-    }
+}  // end namespace internal
 
-    class InnerIterator : public EvalIterator
-    {
-    public:
-      EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
-        : EvalIterator(unaryOp.m_argImpl,outer)
-      {}
-      
-      Index row() const { return EvalIterator::col(); }
-      Index col() const { return EvalIterator::row(); }
-    };
-    
-    enum {
-      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-      Flags = XprType::Flags
-    };
-    
-    explicit unary_evaluator(const XprType& op) :m_argImpl(op.nestedExpression()) {}
+}  // end namespace Eigen
 
-  protected:
-    evaluator<ArgType> m_argImpl;
-};
-
-} // end namespace internal
-
-} // end namespace Eigen
-
-#endif // EIGEN_SPARSETRANSPOSE_H
+#endif  // EIGEN_SPARSETRANSPOSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h
index 9ac1202..a6c3eaa 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h
@@ -11,179 +11,167 @@
 #ifndef EIGEN_SPARSE_TRIANGULARVIEW_H
 #define EIGEN_SPARSE_TRIANGULARVIEW_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 /** \ingroup SparseCore_Module
-  *
-  * \brief Base class for a triangular part in a \b sparse matrix
-  *
-  * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be instantiated.
-  * It extends class TriangularView with additional methods which are available for sparse expressions only.
-  *
-  * \sa class TriangularView, SparseMatrixBase::triangularView()
-  */
-template<typename MatrixType, unsigned int Mode> class TriangularViewImpl<MatrixType,Mode,Sparse>
-  : public SparseMatrixBase<TriangularView<MatrixType,Mode> >
-{
-    enum { SkipFirst = ((Mode&Lower) && !(MatrixType::Flags&RowMajorBit))
-                    || ((Mode&Upper) &&  (MatrixType::Flags&RowMajorBit)),
-           SkipLast = !SkipFirst,
-           SkipDiag = (Mode&ZeroDiag) ? 1 : 0,
-           HasUnitDiag = (Mode&UnitDiag) ? 1 : 0
-    };
-    
-    typedef TriangularView<MatrixType,Mode> TriangularViewType;
-    
-  protected:
-    // dummy solve function to make TriangularView happy.
-    void solve() const;
+ *
+ * \brief Base class for a triangular part in a \b sparse matrix
+ *
+ * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be
+ * instantiated. It extends class TriangularView with additional methods which are available for sparse expressions
+ * only.
+ *
+ * \sa class TriangularView, SparseMatrixBase::triangularView()
+ */
+template <typename MatrixType, unsigned int Mode>
+class TriangularViewImpl<MatrixType, Mode, Sparse> : public SparseMatrixBase<TriangularView<MatrixType, Mode> > {
+  enum {
+    SkipFirst =
+        ((Mode & Lower) && !(MatrixType::Flags & RowMajorBit)) || ((Mode & Upper) && (MatrixType::Flags & RowMajorBit)),
+    SkipLast = !SkipFirst,
+    SkipDiag = (Mode & ZeroDiag) ? 1 : 0,
+    HasUnitDiag = (Mode & UnitDiag) ? 1 : 0
+  };
 
-    typedef SparseMatrixBase<TriangularViewType> Base;
-  public:
-    
-    EIGEN_SPARSE_PUBLIC_INTERFACE(TriangularViewType)
-    
-    typedef typename MatrixType::Nested MatrixTypeNested;
-    typedef typename internal::remove_reference<MatrixTypeNested>::type MatrixTypeNestedNonRef;
-    typedef typename internal::remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
+  typedef TriangularView<MatrixType, Mode> TriangularViewType;
 
-    template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _solve_impl(const RhsType &rhs, DstType &dst) const {
-      if(!(internal::is_same<RhsType,DstType>::value && internal::extract_data(dst) == internal::extract_data(rhs)))
-        dst = rhs;
-      this->solveInPlace(dst);
-    }
+ protected:
+  // dummy solve function to make TriangularView happy.
+  void solve() const;
 
-    /** Applies the inverse of \c *this to the dense vector or matrix \a other, "in-place" */
-    template<typename OtherDerived> void solveInPlace(MatrixBase<OtherDerived>& other) const;
+  typedef SparseMatrixBase<TriangularViewType> Base;
 
-    /** Applies the inverse of \c *this to the sparse vector or matrix \a other, "in-place" */
-    template<typename OtherDerived> void solveInPlace(SparseMatrixBase<OtherDerived>& other) const;
-  
+ public:
+  EIGEN_SPARSE_PUBLIC_INTERFACE(TriangularViewType)
+
+  typedef typename MatrixType::Nested MatrixTypeNested;
+  typedef std::remove_reference_t<MatrixTypeNested> MatrixTypeNestedNonRef;
+  typedef internal::remove_all_t<MatrixTypeNested> MatrixTypeNestedCleaned;
+
+  template <typename RhsType, typename DstType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _solve_impl(const RhsType& rhs, DstType& dst) const {
+    if (!(internal::is_same<RhsType, DstType>::value && internal::extract_data(dst) == internal::extract_data(rhs)))
+      dst = rhs;
+    this->solveInPlace(dst);
+  }
+
+  /** Applies the inverse of \c *this to the dense vector or matrix \a other, "in-place" */
+  template <typename OtherDerived>
+  void solveInPlace(MatrixBase<OtherDerived>& other) const;
+
+  /** Applies the inverse of \c *this to the sparse vector or matrix \a other, "in-place" */
+  template <typename OtherDerived>
+  void solveInPlace(SparseMatrixBase<OtherDerived>& other) const;
 };
 
 namespace internal {
 
-template<typename ArgType, unsigned int Mode>
-struct unary_evaluator<TriangularView<ArgType,Mode>, IteratorBased>
- : evaluator_base<TriangularView<ArgType,Mode> >
-{
-  typedef TriangularView<ArgType,Mode> XprType;
-  
-protected:
-  
+template <typename ArgType, unsigned int Mode>
+struct unary_evaluator<TriangularView<ArgType, Mode>, IteratorBased> : evaluator_base<TriangularView<ArgType, Mode> > {
+  typedef TriangularView<ArgType, Mode> XprType;
+
+ protected:
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::StorageIndex StorageIndex;
   typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
-  
-  enum { SkipFirst = ((Mode&Lower) && !(ArgType::Flags&RowMajorBit))
-                    || ((Mode&Upper) &&  (ArgType::Flags&RowMajorBit)),
-         SkipLast = !SkipFirst,
-         SkipDiag = (Mode&ZeroDiag) ? 1 : 0,
-         HasUnitDiag = (Mode&UnitDiag) ? 1 : 0
-  };
-  
-public:
-  
-  enum {
-    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-    Flags = XprType::Flags
-  };
-    
-  explicit unary_evaluator(const XprType &xpr) : m_argImpl(xpr.nestedExpression()), m_arg(xpr.nestedExpression()) {}
-  
-  inline Index nonZerosEstimate() const {
-    return m_argImpl.nonZerosEstimate();
-  }
-  
-  class InnerIterator : public EvalIterator
-  {
-      typedef EvalIterator Base;
-    public:
 
-      EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& xprEval, Index outer)
-        : Base(xprEval.m_argImpl,outer), m_returnOne(false), m_containsDiag(Base::outer()<xprEval.m_arg.innerSize())
-      {
-        if(SkipFirst)
-        {
-          while((*this) && ((HasUnitDiag||SkipDiag)  ? this->index()<=outer : this->index()<outer))
-            Base::operator++();
-          if(HasUnitDiag)
-            m_returnOne = m_containsDiag;
-        }
-        else if(HasUnitDiag && ((!Base::operator bool()) || Base::index()>=Base::outer()))
-        {
-          if((!SkipFirst) && Base::operator bool())
-            Base::operator++();
+  enum {
+    SkipFirst =
+        ((Mode & Lower) && !(ArgType::Flags & RowMajorBit)) || ((Mode & Upper) && (ArgType::Flags & RowMajorBit)),
+    SkipLast = !SkipFirst,
+    SkipDiag = (Mode & ZeroDiag) ? 1 : 0,
+    HasUnitDiag = (Mode & UnitDiag) ? 1 : 0
+  };
+
+ public:
+  enum { CoeffReadCost = evaluator<ArgType>::CoeffReadCost, Flags = XprType::Flags };
+
+  explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_arg(xpr.nestedExpression()) {}
+
+  inline Index nonZerosEstimate() const { return m_argImpl.nonZerosEstimate(); }
+
+  class InnerIterator : public EvalIterator {
+    typedef EvalIterator Base;
+
+   public:
+    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& xprEval, Index outer)
+        : Base(xprEval.m_argImpl, outer),
+          m_returnOne(false),
+          m_containsDiag(Base::outer() < xprEval.m_arg.innerSize()) {
+      if (SkipFirst) {
+        while ((*this) && ((HasUnitDiag || SkipDiag) ? this->index() <= outer : this->index() < outer))
+          Base::operator++();
+        if (HasUnitDiag) m_returnOne = m_containsDiag;
+      } else if (HasUnitDiag && ((!Base::operator bool()) || Base::index() >= Base::outer())) {
+        if ((!SkipFirst) && Base::operator bool()) Base::operator++();
+        m_returnOne = m_containsDiag;
+      }
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
+      if (HasUnitDiag && m_returnOne)
+        m_returnOne = false;
+      else {
+        Base::operator++();
+        if (HasUnitDiag && (!SkipFirst) && ((!Base::operator bool()) || Base::index() >= Base::outer())) {
+          if ((!SkipFirst) && Base::operator bool()) Base::operator++();
           m_returnOne = m_containsDiag;
         }
       }
+      return *this;
+    }
 
-      EIGEN_STRONG_INLINE InnerIterator& operator++()
-      {
-        if(HasUnitDiag && m_returnOne)
-          m_returnOne = false;
+    EIGEN_STRONG_INLINE operator bool() const {
+      if (HasUnitDiag && m_returnOne) return true;
+      if (SkipFirst)
+        return Base::operator bool();
+      else {
+        if (SkipDiag)
+          return (Base::operator bool() && this->index() < this->outer());
         else
-        {
-          Base::operator++();
-          if(HasUnitDiag && (!SkipFirst) && ((!Base::operator bool()) || Base::index()>=Base::outer()))
-          {
-            if((!SkipFirst) && Base::operator bool())
-              Base::operator++();
-            m_returnOne = m_containsDiag;
-          }
-        }
-        return *this;
+          return (Base::operator bool() && this->index() <= this->outer());
       }
-      
-      EIGEN_STRONG_INLINE operator bool() const
-      {
-        if(HasUnitDiag && m_returnOne)
-          return true;
-        if(SkipFirst) return  Base::operator bool();
-        else
-        {
-          if (SkipDiag) return (Base::operator bool() && this->index() < this->outer());
-          else return (Base::operator bool() && this->index() <= this->outer());
-        }
-      }
+    }
 
-//       inline Index row() const { return (ArgType::Flags&RowMajorBit ? Base::outer() : this->index()); }
-//       inline Index col() const { return (ArgType::Flags&RowMajorBit ? this->index() : Base::outer()); }
-      inline StorageIndex index() const
-      {
-        if(HasUnitDiag && m_returnOne)  return internal::convert_index<StorageIndex>(Base::outer());
-        else                            return Base::index();
-      }
-      inline Scalar value() const
-      {
-        if(HasUnitDiag && m_returnOne)  return Scalar(1);
-        else                            return Base::value();
-      }
+    inline Index row() const { return (ArgType::Flags & RowMajorBit ? Base::outer() : this->index()); }
+    inline Index col() const { return (ArgType::Flags & RowMajorBit ? this->index() : Base::outer()); }
+    inline StorageIndex index() const {
+      if (HasUnitDiag && m_returnOne)
+        return internal::convert_index<StorageIndex>(Base::outer());
+      else
+        return Base::index();
+    }
+    inline Scalar value() const {
+      if (HasUnitDiag && m_returnOne)
+        return Scalar(1);
+      else
+        return Base::value();
+    }
 
-    protected:
-      bool m_returnOne;
-      bool m_containsDiag;
-    private:
-      Scalar& valueRef();
+   protected:
+    bool m_returnOne;
+    bool m_containsDiag;
+
+   private:
+    Scalar& valueRef();
   };
-  
-protected:
+
+ protected:
   evaluator<ArgType> m_argImpl;
   const ArgType& m_arg;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-template<typename Derived>
-template<int Mode>
-inline const TriangularView<const Derived, Mode>
-SparseMatrixBase<Derived>::triangularView() const
-{
+template <typename Derived>
+template <int Mode>
+inline const TriangularView<const Derived, Mode> SparseMatrixBase<Derived>::triangularView() const {
   return TriangularView<const Derived, Mode>(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSE_TRIANGULARVIEW_H
+#endif  // EIGEN_SPARSE_TRIANGULARVIEW_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h
index ceb9368..33cedaf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h
@@ -10,7 +10,10 @@
 #ifndef EIGEN_SPARSEUTIL_H
 #define EIGEN_SPARSEUTIL_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 #ifdef NDEBUG
 #define EIGEN_DBG_SPARSE(X)
@@ -18,127 +21,149 @@
 #define EIGEN_DBG_SPARSE(X) X
 #endif
 
-#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \
-template<typename OtherDerived> \
-EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::SparseMatrixBase<OtherDerived>& other) \
-{ \
-  return Base::operator Op(other.derived()); \
-} \
-EIGEN_STRONG_INLINE Derived& operator Op(const Derived& other) \
-{ \
-  return Base::operator Op(other); \
-}
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op)                                    \
+  template <typename OtherDerived>                                                               \
+  EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::SparseMatrixBase<OtherDerived>& other) { \
+    return Base::operator Op(other.derived());                                                   \
+  }                                                                                              \
+  EIGEN_STRONG_INLINE Derived& operator Op(const Derived & other) { return Base::operator Op(other); }
 
 #define EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \
-template<typename Other> \
-EIGEN_STRONG_INLINE Derived& operator Op(const Other& scalar) \
-{ \
-  return Base::operator Op(scalar); \
-}
+  template <typename Other>                                          \
+  EIGEN_STRONG_INLINE Derived& operator Op(const Other & scalar) {   \
+    return Base::operator Op(scalar);                                \
+  }
 
-#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
-EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, =)
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, =)
 
+#define EIGEN_SPARSE_PUBLIC_INTERFACE(Derived) EIGEN_GENERIC_PUBLIC_INTERFACE(Derived)
 
-#define EIGEN_SPARSE_PUBLIC_INTERFACE(Derived) \
-  EIGEN_GENERIC_PUBLIC_INTERFACE(Derived)
+const int CoherentAccessPattern = 0x1;
+const int InnerRandomAccessPattern = 0x2 | CoherentAccessPattern;
+const int OuterRandomAccessPattern = 0x4 | CoherentAccessPattern;
+const int RandomAccessPattern = 0x8 | OuterRandomAccessPattern | InnerRandomAccessPattern;
 
-  
-const int CoherentAccessPattern     = 0x1;
-const int InnerRandomAccessPattern  = 0x2 | CoherentAccessPattern;
-const int OuterRandomAccessPattern  = 0x4 | CoherentAccessPattern;
-const int RandomAccessPattern       = 0x8 | OuterRandomAccessPattern | InnerRandomAccessPattern;
+template <typename Scalar_, int Flags_ = 0, typename StorageIndex_ = int>
+class SparseMatrix;
+template <typename Scalar_, int Flags_ = 0, typename StorageIndex_ = int>
+class SparseVector;
 
-template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class SparseMatrix;
-template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class DynamicSparseMatrix;
-template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class SparseVector;
-template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class MappedSparseMatrix;
+template <typename MatrixType, unsigned int UpLo>
+class SparseSelfAdjointView;
+template <typename Lhs, typename Rhs>
+class SparseDiagonalProduct;
+template <typename MatrixType>
+class SparseView;
 
-template<typename MatrixType, unsigned int UpLo>  class SparseSelfAdjointView;
-template<typename Lhs, typename Rhs>              class SparseDiagonalProduct;
-template<typename MatrixType> class SparseView;
+template <typename Lhs, typename Rhs>
+class SparseSparseProduct;
+template <typename Lhs, typename Rhs>
+class SparseTimeDenseProduct;
+template <typename Lhs, typename Rhs>
+class DenseTimeSparseProduct;
+template <typename Lhs, typename Rhs, bool Transpose>
+class SparseDenseOuterProduct;
 
-template<typename Lhs, typename Rhs>        class SparseSparseProduct;
-template<typename Lhs, typename Rhs>        class SparseTimeDenseProduct;
-template<typename Lhs, typename Rhs>        class DenseTimeSparseProduct;
-template<typename Lhs, typename Rhs, bool Transpose> class SparseDenseOuterProduct;
+template <typename Lhs, typename Rhs>
+struct SparseSparseProductReturnType;
+template <typename Lhs, typename Rhs,
+          int InnerSize = internal::min_size_prefer_fixed(internal::traits<Lhs>::ColsAtCompileTime,
+                                                          internal::traits<Rhs>::RowsAtCompileTime)>
+struct DenseSparseProductReturnType;
 
-template<typename Lhs, typename Rhs> struct SparseSparseProductReturnType;
-template<typename Lhs, typename Rhs,
-         int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct DenseSparseProductReturnType;
-         
-template<typename Lhs, typename Rhs,
-         int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct SparseDenseProductReturnType;
-template<typename MatrixType,int UpLo> class SparseSymmetricPermutationProduct;
+template <typename Lhs, typename Rhs,
+          int InnerSize = internal::min_size_prefer_fixed(internal::traits<Lhs>::ColsAtCompileTime,
+                                                          internal::traits<Rhs>::RowsAtCompileTime)>
+struct SparseDenseProductReturnType;
+template <typename MatrixType, int UpLo>
+class SparseSymmetricPermutationProduct;
 
 namespace internal {
 
-template<typename T,int Rows,int Cols,int Flags> struct sparse_eval;
+template <typename T, int Rows, int Cols, int Flags>
+struct sparse_eval;
 
-template<typename T> struct eval<T,Sparse>
-  : sparse_eval<T, traits<T>::RowsAtCompileTime,traits<T>::ColsAtCompileTime,traits<T>::Flags>
-{};
-
-template<typename T,int Cols,int Flags> struct sparse_eval<T,1,Cols,Flags> {
-    typedef typename traits<T>::Scalar _Scalar;
-    typedef typename traits<T>::StorageIndex _StorageIndex;
-  public:
-    typedef SparseVector<_Scalar, RowMajor, _StorageIndex> type;
+template <typename T>
+struct eval<T, Sparse> : sparse_eval<T, traits<T>::RowsAtCompileTime, traits<T>::ColsAtCompileTime, traits<T>::Flags> {
 };
 
-template<typename T,int Rows,int Flags> struct sparse_eval<T,Rows,1,Flags> {
-    typedef typename traits<T>::Scalar _Scalar;
-    typedef typename traits<T>::StorageIndex _StorageIndex;
-  public:
-    typedef SparseVector<_Scalar, ColMajor, _StorageIndex> type;
+template <typename T, int Cols, int Flags>
+struct sparse_eval<T, 1, Cols, Flags> {
+  typedef typename traits<T>::Scalar Scalar_;
+  typedef typename traits<T>::StorageIndex StorageIndex_;
+
+ public:
+  typedef SparseVector<Scalar_, RowMajor, StorageIndex_> type;
+};
+
+template <typename T, int Rows, int Flags>
+struct sparse_eval<T, Rows, 1, Flags> {
+  typedef typename traits<T>::Scalar Scalar_;
+  typedef typename traits<T>::StorageIndex StorageIndex_;
+
+ public:
+  typedef SparseVector<Scalar_, ColMajor, StorageIndex_> type;
 };
 
 // TODO this seems almost identical to plain_matrix_type<T, Sparse>
-template<typename T,int Rows,int Cols,int Flags> struct sparse_eval {
-    typedef typename traits<T>::Scalar _Scalar;
-    typedef typename traits<T>::StorageIndex _StorageIndex;
-    enum { _Options = ((Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor };
-  public:
-    typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type;
+template <typename T, int Rows, int Cols, int Flags>
+struct sparse_eval {
+  typedef typename traits<T>::Scalar Scalar_;
+  typedef typename traits<T>::StorageIndex StorageIndex_;
+  enum { Options_ = ((Flags & RowMajorBit) == RowMajorBit) ? RowMajor : ColMajor };
+
+ public:
+  typedef SparseMatrix<Scalar_, Options_, StorageIndex_> type;
 };
 
-template<typename T,int Flags> struct sparse_eval<T,1,1,Flags> {
-    typedef typename traits<T>::Scalar _Scalar;
-  public:
-    typedef Matrix<_Scalar, 1, 1> type;
+template <typename T, int Flags>
+struct sparse_eval<T, 1, 1, Flags> {
+  typedef typename traits<T>::Scalar Scalar_;
+
+ public:
+  typedef Matrix<Scalar_, 1, 1> type;
 };
 
-template<typename T> struct plain_matrix_type<T,Sparse>
-{
-  typedef typename traits<T>::Scalar _Scalar;
-  typedef typename traits<T>::StorageIndex _StorageIndex;
-  enum { _Options = ((evaluator<T>::Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor };
-  public:
-    typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type;
+template <typename T>
+struct plain_matrix_type<T, Sparse> {
+  typedef typename traits<T>::Scalar Scalar_;
+  typedef typename traits<T>::StorageIndex StorageIndex_;
+  enum { Options_ = ((evaluator<T>::Flags & RowMajorBit) == RowMajorBit) ? RowMajor : ColMajor };
+
+ public:
+  typedef SparseMatrix<Scalar_, Options_, StorageIndex_> type;
 };
 
-template<typename T>
-struct plain_object_eval<T,Sparse>
-  : sparse_eval<T, traits<T>::RowsAtCompileTime,traits<T>::ColsAtCompileTime, evaluator<T>::Flags>
-{};
+template <typename T>
+struct plain_object_eval<T, Sparse>
+    : sparse_eval<T, traits<T>::RowsAtCompileTime, traits<T>::ColsAtCompileTime, evaluator<T>::Flags> {};
 
-template<typename Decomposition, typename RhsType>
-struct solve_traits<Decomposition,RhsType,Sparse>
-{
-  typedef typename sparse_eval<RhsType, RhsType::RowsAtCompileTime, RhsType::ColsAtCompileTime,traits<RhsType>::Flags>::type PlainObject;
+template <typename Decomposition, typename RhsType>
+struct solve_traits<Decomposition, RhsType, Sparse> {
+  typedef typename sparse_eval<RhsType, RhsType::RowsAtCompileTime, RhsType::ColsAtCompileTime,
+                               traits<RhsType>::Flags>::type PlainObject;
 };
 
-template<typename Derived>
-struct generic_xpr_base<Derived, MatrixXpr, Sparse>
-{
+template <typename Derived>
+struct generic_xpr_base<Derived, MatrixXpr, Sparse> {
   typedef SparseMatrixBase<Derived> type;
 };
 
-struct SparseTriangularShape  { static std::string debugName() { return "SparseTriangularShape"; } };
-struct SparseSelfAdjointShape { static std::string debugName() { return "SparseSelfAdjointShape"; } };
+struct SparseTriangularShape {
+  static std::string debugName() { return "SparseTriangularShape"; }
+};
+struct SparseSelfAdjointShape {
+  static std::string debugName() { return "SparseSelfAdjointShape"; }
+};
 
-template<> struct glue_shapes<SparseShape,SelfAdjointShape> { typedef SparseSelfAdjointShape type;  };
-template<> struct glue_shapes<SparseShape,TriangularShape > { typedef SparseTriangularShape  type;  };
+template <>
+struct glue_shapes<SparseShape, SelfAdjointShape> {
+  typedef SparseSelfAdjointShape type;
+};
+template <>
+struct glue_shapes<SparseShape, TriangularShape> {
+  typedef SparseTriangularShape type;
+};
 
 // return type of SparseCompressedBase::lower_bound;
 struct LowerBoundIndex {
@@ -148,25 +173,22 @@
   bool found;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup SparseCore_Module
-  *
-  * \class Triplet
-  *
-  * \brief A small structure to hold a non zero as a triplet (i,j,value).
-  *
-  * \sa SparseMatrix::setFromTriplets()
-  */
-template<typename Scalar, typename StorageIndex=typename SparseMatrix<Scalar>::StorageIndex >
-class Triplet
-{
-public:
+ *
+ * \class Triplet
+ *
+ * \brief A small structure to hold a non zero as a triplet (i,j,value).
+ *
+ * \sa SparseMatrix::setFromTriplets()
+ */
+template <typename Scalar, typename StorageIndex = typename SparseMatrix<Scalar>::StorageIndex>
+class Triplet {
+ public:
   Triplet() : m_row(0), m_col(0), m_value(0) {}
 
-  Triplet(const StorageIndex& i, const StorageIndex& j, const Scalar& v = Scalar(0))
-    : m_row(i), m_col(j), m_value(v)
-  {}
+  Triplet(const StorageIndex& i, const StorageIndex& j, const Scalar& v = Scalar(0)) : m_row(i), m_col(j), m_value(v) {}
 
   /** \returns the row index of the element */
   const StorageIndex& row() const { return m_row; }
@@ -176,11 +198,12 @@
 
   /** \returns the value of the element */
   const Scalar& value() const { return m_value; }
-protected:
+
+ protected:
   StorageIndex m_row, m_col;
   Scalar m_value;
 };
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSEUTIL_H
+#endif  // EIGEN_SPARSEUTIL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h
index 05779be..0733718 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h
@@ -10,469 +10,496 @@
 #ifndef EIGEN_SPARSEVECTOR_H
 #define EIGEN_SPARSEVECTOR_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \ingroup SparseCore_Module
-  * \class SparseVector
-  *
-  * \brief a sparse vector class
-  *
-  * \tparam _Scalar the scalar type, i.e. the type of the coefficients
-  *
-  * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEVECTOR_PLUGIN.
-  */
+ * \class SparseVector
+ *
+ * \brief a sparse vector class
+ *
+ * \tparam Scalar_ the scalar type, i.e. the type of the coefficients
+ *
+ * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEVECTOR_PLUGIN.
+ */
 
 namespace internal {
-template<typename _Scalar, int _Options, typename _StorageIndex>
-struct traits<SparseVector<_Scalar, _Options, _StorageIndex> >
-{
-  typedef _Scalar Scalar;
-  typedef _StorageIndex StorageIndex;
+template <typename Scalar_, int Options_, typename StorageIndex_>
+struct traits<SparseVector<Scalar_, Options_, StorageIndex_> > {
+  typedef Scalar_ Scalar;
+  typedef StorageIndex_ StorageIndex;
   typedef Sparse StorageKind;
   typedef MatrixXpr XprKind;
   enum {
-    IsColVector = (_Options & RowMajorBit) ? 0 : 1,
+    IsColVector = (Options_ & RowMajorBit) ? 0 : 1,
 
     RowsAtCompileTime = IsColVector ? Dynamic : 1,
     ColsAtCompileTime = IsColVector ? 1 : Dynamic,
     MaxRowsAtCompileTime = RowsAtCompileTime,
     MaxColsAtCompileTime = ColsAtCompileTime,
-    Flags = _Options | NestByRefBit | LvalueBit | (IsColVector ? 0 : RowMajorBit) | CompressedAccessBit,
+    Flags = Options_ | NestByRefBit | LvalueBit | (IsColVector ? 0 : RowMajorBit) | CompressedAccessBit,
     SupportedAccessPatterns = InnerRandomAccessPattern
   };
 };
 
 // Sparse-Vector-Assignment kinds:
-enum {
-  SVA_RuntimeSwitch,
-  SVA_Inner,
-  SVA_Outer
-};
+enum { SVA_RuntimeSwitch, SVA_Inner, SVA_Outer };
 
-template< typename Dest, typename Src,
-          int AssignmentKind = !bool(Src::IsVectorAtCompileTime) ? SVA_RuntimeSwitch
-                             : Src::InnerSizeAtCompileTime==1 ? SVA_Outer
-                             : SVA_Inner>
+template <typename Dest, typename Src,
+          int AssignmentKind = !bool(Src::IsVectorAtCompileTime)  ? SVA_RuntimeSwitch
+                               : Src::InnerSizeAtCompileTime == 1 ? SVA_Outer
+                                                                  : SVA_Inner>
 struct sparse_vector_assign_selector;
 
-}
+}  // namespace internal
 
-template<typename _Scalar, int _Options, typename _StorageIndex>
-class SparseVector
-  : public SparseCompressedBase<SparseVector<_Scalar, _Options, _StorageIndex> >
-{
-    typedef SparseCompressedBase<SparseVector> Base;
-    using Base::convert_index;
-  public:
-    EIGEN_SPARSE_PUBLIC_INTERFACE(SparseVector)
-    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, +=)
-    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, -=)
-    
-    typedef internal::CompressedStorage<Scalar,StorageIndex> Storage;
-    enum { IsColVector = internal::traits<SparseVector>::IsColVector };
-    
-    enum {
-      Options = _Options
-    };
-    
-    EIGEN_STRONG_INLINE Index rows() const { return IsColVector ? m_size : 1; }
-    EIGEN_STRONG_INLINE Index cols() const { return IsColVector ? 1 : m_size; }
-    EIGEN_STRONG_INLINE Index innerSize() const { return m_size; }
-    EIGEN_STRONG_INLINE Index outerSize() const { return 1; }
+template <typename Scalar_, int Options_, typename StorageIndex_>
+class SparseVector : public SparseCompressedBase<SparseVector<Scalar_, Options_, StorageIndex_> > {
+  typedef SparseCompressedBase<SparseVector> Base;
+  using Base::convert_index;
 
-    EIGEN_STRONG_INLINE const Scalar* valuePtr() const { return m_data.valuePtr(); }
-    EIGEN_STRONG_INLINE Scalar* valuePtr() { return m_data.valuePtr(); }
+ public:
+  EIGEN_SPARSE_PUBLIC_INTERFACE(SparseVector)
+  EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, +=)
+  EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, -=)
 
-    EIGEN_STRONG_INLINE const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
-    EIGEN_STRONG_INLINE StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
+  typedef internal::CompressedStorage<Scalar, StorageIndex> Storage;
+  enum { IsColVector = internal::traits<SparseVector>::IsColVector };
 
-    inline const StorageIndex* outerIndexPtr() const { return 0; }
-    inline StorageIndex* outerIndexPtr() { return 0; }
-    inline const StorageIndex* innerNonZeroPtr() const { return 0; }
-    inline StorageIndex* innerNonZeroPtr() { return 0; }
-    
-    /** \internal */
-    inline Storage& data() { return m_data; }
-    /** \internal */
-    inline const Storage& data() const { return m_data; }
+  enum { Options = Options_ };
 
-    inline Scalar coeff(Index row, Index col) const
-    {
-      eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
-      return coeff(IsColVector ? row : col);
+  EIGEN_STRONG_INLINE Index rows() const { return IsColVector ? m_size : 1; }
+  EIGEN_STRONG_INLINE Index cols() const { return IsColVector ? 1 : m_size; }
+  EIGEN_STRONG_INLINE Index innerSize() const { return m_size; }
+  EIGEN_STRONG_INLINE Index outerSize() const { return 1; }
+
+  EIGEN_STRONG_INLINE const Scalar* valuePtr() const { return m_data.valuePtr(); }
+  EIGEN_STRONG_INLINE Scalar* valuePtr() { return m_data.valuePtr(); }
+
+  EIGEN_STRONG_INLINE const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
+  EIGEN_STRONG_INLINE StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
+
+  inline const StorageIndex* outerIndexPtr() const { return 0; }
+  inline StorageIndex* outerIndexPtr() { return 0; }
+  inline const StorageIndex* innerNonZeroPtr() const { return 0; }
+  inline StorageIndex* innerNonZeroPtr() { return 0; }
+
+  /** \internal */
+  inline Storage& data() { return m_data; }
+  /** \internal */
+  inline const Storage& data() const { return m_data; }
+
+  inline Scalar coeff(Index row, Index col) const {
+    eigen_assert(IsColVector ? (col == 0 && row >= 0 && row < m_size) : (row == 0 && col >= 0 && col < m_size));
+    return coeff(IsColVector ? row : col);
+  }
+  inline Scalar coeff(Index i) const {
+    eigen_assert(i >= 0 && i < m_size);
+    return m_data.at(StorageIndex(i));
+  }
+
+  inline Scalar& coeffRef(Index row, Index col) {
+    eigen_assert(IsColVector ? (col == 0 && row >= 0 && row < m_size) : (row == 0 && col >= 0 && col < m_size));
+    return coeffRef(IsColVector ? row : col);
+  }
+
+  /** \returns a reference to the coefficient value at given index \a i
+   * This operation involes a log(rho*size) binary search. If the coefficient does not
+   * exist yet, then a sorted insertion into a sequential buffer is performed.
+   *
+   * This insertion might be very costly if the number of nonzeros above \a i is large.
+   */
+  inline Scalar& coeffRef(Index i) {
+    eigen_assert(i >= 0 && i < m_size);
+
+    return m_data.atWithInsertion(StorageIndex(i));
+  }
+
+ public:
+  typedef typename Base::InnerIterator InnerIterator;
+  typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
+
+  inline void setZero() { m_data.clear(); }
+
+  /** \returns the number of non zero coefficients */
+  inline Index nonZeros() const { return m_data.size(); }
+
+  inline void startVec(Index outer) {
+    EIGEN_UNUSED_VARIABLE(outer);
+    eigen_assert(outer == 0);
+  }
+
+  inline Scalar& insertBackByOuterInner(Index outer, Index inner) {
+    EIGEN_UNUSED_VARIABLE(outer);
+    eigen_assert(outer == 0);
+    return insertBack(inner);
+  }
+  inline Scalar& insertBack(Index i) {
+    m_data.append(0, i);
+    return m_data.value(m_data.size() - 1);
+  }
+
+  Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner) {
+    EIGEN_UNUSED_VARIABLE(outer);
+    eigen_assert(outer == 0);
+    return insertBackUnordered(inner);
+  }
+  inline Scalar& insertBackUnordered(Index i) {
+    m_data.append(0, i);
+    return m_data.value(m_data.size() - 1);
+  }
+
+  inline Scalar& insert(Index row, Index col) {
+    eigen_assert(IsColVector ? (col == 0 && row >= 0 && row < m_size) : (row == 0 && col >= 0 && col < m_size));
+
+    Index inner = IsColVector ? row : col;
+    Index outer = IsColVector ? col : row;
+    EIGEN_ONLY_USED_FOR_DEBUG(outer);
+    eigen_assert(outer == 0);
+    return insert(inner);
+  }
+  Scalar& insert(Index i) {
+    eigen_assert(i >= 0 && i < m_size);
+
+    Index startId = 0;
+    Index p = Index(m_data.size()) - 1;
+    // TODO smart realloc
+    m_data.resize(p + 2, 1);
+
+    while ((p >= startId) && (m_data.index(p) > i)) {
+      m_data.index(p + 1) = m_data.index(p);
+      m_data.value(p + 1) = m_data.value(p);
+      --p;
     }
-    inline Scalar coeff(Index i) const
-    {
-      eigen_assert(i>=0 && i<m_size);
-      return m_data.at(StorageIndex(i));
-    }
+    m_data.index(p + 1) = convert_index(i);
+    m_data.value(p + 1) = 0;
+    return m_data.value(p + 1);
+  }
 
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
-      return coeffRef(IsColVector ? row : col);
-    }
+  /**
+   */
+  inline void reserve(Index reserveSize) { m_data.reserve(reserveSize); }
 
-    /** \returns a reference to the coefficient value at given index \a i
-      * This operation involes a log(rho*size) binary search. If the coefficient does not
-      * exist yet, then a sorted insertion into a sequential buffer is performed.
-      *
-      * This insertion might be very costly if the number of nonzeros above \a i is large.
-      */
-    inline Scalar& coeffRef(Index i)
-    {
-      eigen_assert(i>=0 && i<m_size);
+  inline void finalize() {}
 
-      return m_data.atWithInsertion(StorageIndex(i));
-    }
+  /** \copydoc SparseMatrix::prune(const Scalar&,const RealScalar&) */
+  Index prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision()) {
+    return prune([&](const Scalar& val) { return !internal::isMuchSmallerThan(val, reference, epsilon); });
+  }
 
-  public:
-
-    typedef typename Base::InnerIterator InnerIterator;
-    typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
-
-    inline void setZero() { m_data.clear(); }
-
-    /** \returns the number of non zero coefficients */
-    inline Index nonZeros() const  { return m_data.size(); }
-
-    inline void startVec(Index outer)
-    {
-      EIGEN_UNUSED_VARIABLE(outer);
-      eigen_assert(outer==0);
-    }
-
-    inline Scalar& insertBackByOuterInner(Index outer, Index inner)
-    {
-      EIGEN_UNUSED_VARIABLE(outer);
-      eigen_assert(outer==0);
-      return insertBack(inner);
-    }
-    inline Scalar& insertBack(Index i)
-    {
-      m_data.append(0, i);
-      return m_data.value(m_data.size()-1);
-    }
-    
-    Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner)
-    {
-      EIGEN_UNUSED_VARIABLE(outer);
-      eigen_assert(outer==0);
-      return insertBackUnordered(inner);
-    }
-    inline Scalar& insertBackUnordered(Index i)
-    {
-      m_data.append(0, i);
-      return m_data.value(m_data.size()-1);
-    }
-
-    inline Scalar& insert(Index row, Index col)
-    {
-      eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
-      
-      Index inner = IsColVector ? row : col;
-      Index outer = IsColVector ? col : row;
-      EIGEN_ONLY_USED_FOR_DEBUG(outer);
-      eigen_assert(outer==0);
-      return insert(inner);
-    }
-    Scalar& insert(Index i)
-    {
-      eigen_assert(i>=0 && i<m_size);
-      
-      Index startId = 0;
-      Index p = Index(m_data.size()) - 1;
-      // TODO smart realloc
-      m_data.resize(p+2,1);
-
-      while ( (p >= startId) && (m_data.index(p) > i) )
-      {
-        m_data.index(p+1) = m_data.index(p);
-        m_data.value(p+1) = m_data.value(p);
-        --p;
+  /**
+   * \brief Prunes the entries of the vector based on a `predicate`
+   * \tparam F Type of the predicate.
+   * \param keep_predicate The predicate that is used to test whether a value should be kept. A callable that
+   * gets passed om a `Scalar` value and returns a boolean. If the predicate returns true, the value is kept.
+   * \return The new number of structural non-zeros.
+   */
+  template <class F>
+  Index prune(F&& keep_predicate) {
+    Index k = 0;
+    Index n = m_data.size();
+    for (Index i = 0; i < n; ++i) {
+      if (keep_predicate(m_data.value(i))) {
+        m_data.value(k) = std::move(m_data.value(i));
+        m_data.index(k) = m_data.index(i);
+        ++k;
       }
-      m_data.index(p+1) = convert_index(i);
-      m_data.value(p+1) = 0;
-      return m_data.value(p+1);
     }
+    m_data.resize(k);
+    return k;
+  }
 
-    /**
-      */
-    inline void reserve(Index reserveSize) { m_data.reserve(reserveSize); }
+  /** Resizes the sparse vector to \a rows x \a cols
+   *
+   * This method is provided for compatibility with matrices.
+   * For a column vector, \a cols must be equal to 1.
+   * For a row vector, \a rows must be equal to 1.
+   *
+   * \sa resize(Index)
+   */
+  void resize(Index rows, Index cols) {
+    eigen_assert((IsColVector ? cols : rows) == 1 && "Outer dimension must equal 1");
+    resize(IsColVector ? rows : cols);
+  }
 
+  /** Resizes the sparse vector to \a newSize
+   * This method deletes all entries, thus leaving an empty sparse vector
+   *
+   * \sa  conservativeResize(), setZero() */
+  void resize(Index newSize) {
+    m_size = newSize;
+    m_data.clear();
+  }
 
-    inline void finalize() {}
-
-    /** \copydoc SparseMatrix::prune(const Scalar&,const RealScalar&) */
-    void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
-    {
-      m_data.prune(reference,epsilon);
+  /** Resizes the sparse vector to \a newSize, while leaving old values untouched.
+   *
+   * If the size of the vector is decreased, then the storage of the out-of bounds coefficients is kept and reserved.
+   * Call .data().squeeze() to free extra memory.
+   *
+   * \sa reserve(), setZero()
+   */
+  void conservativeResize(Index newSize) {
+    if (newSize < m_size) {
+      Index i = 0;
+      while (i < m_data.size() && m_data.index(i) < newSize) ++i;
+      m_data.resize(i);
     }
+    m_size = newSize;
+  }
 
-    /** Resizes the sparse vector to \a rows x \a cols
-      *
-      * This method is provided for compatibility with matrices.
-      * For a column vector, \a cols must be equal to 1.
-      * For a row vector, \a rows must be equal to 1.
-      *
-      * \sa resize(Index)
-      */
-    void resize(Index rows, Index cols)
-    {
-      eigen_assert((IsColVector ? cols : rows)==1 && "Outer dimension must equal 1");
-      resize(IsColVector ? rows : cols);
+  void resizeNonZeros(Index size) { m_data.resize(size); }
+
+  inline SparseVector() : m_size(0) { resize(0); }
+
+  explicit inline SparseVector(Index size) : m_size(0) { resize(size); }
+
+  inline SparseVector(Index rows, Index cols) : m_size(0) { resize(rows, cols); }
+
+  template <typename OtherDerived>
+  inline SparseVector(const SparseMatrixBase<OtherDerived>& other) : m_size(0) {
+#ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+    EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+#endif
+    *this = other.derived();
+  }
+
+  inline SparseVector(const SparseVector& other) : Base(other), m_size(0) { *this = other.derived(); }
+
+  /** Swaps the values of \c *this and \a other.
+   * Overloaded for performance: this version performs a \em shallow swap by swapping pointers and attributes only.
+   * \sa SparseMatrixBase::swap()
+   */
+  inline void swap(SparseVector& other) {
+    std::swap(m_size, other.m_size);
+    m_data.swap(other.m_data);
+  }
+
+  template <int OtherOptions>
+  inline void swap(SparseMatrix<Scalar, OtherOptions, StorageIndex>& other) {
+    eigen_assert(other.outerSize() == 1);
+    std::swap(m_size, other.m_innerSize);
+    m_data.swap(other.m_data);
+  }
+
+  inline SparseVector& operator=(const SparseVector& other) {
+    if (other.isRValue()) {
+      swap(other.const_cast_derived());
+    } else {
+      resize(other.size());
+      m_data = other.m_data;
     }
+    return *this;
+  }
 
-    /** Resizes the sparse vector to \a newSize
-      * This method deletes all entries, thus leaving an empty sparse vector
-      *
-      * \sa  conservativeResize(), setZero() */
-    void resize(Index newSize)
-    {
-      m_size = newSize;
-      m_data.clear();
-    }
+  template <typename OtherDerived>
+  inline SparseVector& operator=(const SparseMatrixBase<OtherDerived>& other) {
+    SparseVector tmp(other.size());
+    internal::sparse_vector_assign_selector<SparseVector, OtherDerived>::run(tmp, other.derived());
+    this->swap(tmp);
+    return *this;
+  }
 
-    /** Resizes the sparse vector to \a newSize, while leaving old values untouched.
-      *
-      * If the size of the vector is decreased, then the storage of the out-of bounds coefficients is kept and reserved.
-      * Call .data().squeeze() to free extra memory.
-      *
-      * \sa reserve(), setZero()
-      */
-    void conservativeResize(Index newSize)
-    {
-      if (newSize < m_size)
-      {
-        Index i = 0;
-        while (i<m_data.size() && m_data.index(i)<newSize) ++i;
-        m_data.resize(i);
-      }
-      m_size = newSize;
-    }
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename Lhs, typename Rhs>
+  inline SparseVector& operator=(const SparseSparseProduct<Lhs, Rhs>& product) {
+    return Base::operator=(product);
+  }
+#endif
 
-    void resizeNonZeros(Index size) { m_data.resize(size); }
+#ifndef EIGEN_NO_IO
+  friend std::ostream& operator<<(std::ostream& s, const SparseVector& m) {
+    for (Index i = 0; i < m.nonZeros(); ++i) s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+    s << std::endl;
+    return s;
+  }
+#endif
 
-    inline SparseVector() : m_size(0) { check_template_parameters(); resize(0); }
+  /** Destructor */
+  inline ~SparseVector() {}
 
-    explicit inline SparseVector(Index size) : m_size(0) { check_template_parameters(); resize(size); }
+  /** Overloaded for performance */
+  Scalar sum() const;
 
-    inline SparseVector(Index rows, Index cols) : m_size(0) { check_template_parameters(); resize(rows,cols); }
+ public:
+  /** \internal \deprecated use setZero() and reserve() */
+  EIGEN_DEPRECATED void startFill(Index reserve) {
+    setZero();
+    m_data.reserve(reserve);
+  }
 
-    template<typename OtherDerived>
-    inline SparseVector(const SparseMatrixBase<OtherDerived>& other)
-      : m_size(0)
-    {
-      #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-        EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
-      #endif
-      check_template_parameters();
-      *this = other.derived();
-    }
+  /** \internal \deprecated use insertBack(Index,Index) */
+  EIGEN_DEPRECATED Scalar& fill(Index r, Index c) {
+    eigen_assert(r == 0 || c == 0);
+    return fill(IsColVector ? r : c);
+  }
 
-    inline SparseVector(const SparseVector& other)
-      : Base(other), m_size(0)
-    {
-      check_template_parameters();
-      *this = other.derived();
-    }
+  /** \internal \deprecated use insertBack(Index) */
+  EIGEN_DEPRECATED Scalar& fill(Index i) {
+    m_data.append(0, i);
+    return m_data.value(m_data.size() - 1);
+  }
 
-    /** Swaps the values of \c *this and \a other.
-      * Overloaded for performance: this version performs a \em shallow swap by swapping pointers and attributes only.
-      * \sa SparseMatrixBase::swap()
-      */
-    inline void swap(SparseVector& other)
-    {
-      std::swap(m_size, other.m_size);
-      m_data.swap(other.m_data);
-    }
+  /** \internal \deprecated use insert(Index,Index) */
+  EIGEN_DEPRECATED Scalar& fillrand(Index r, Index c) {
+    eigen_assert(r == 0 || c == 0);
+    return fillrand(IsColVector ? r : c);
+  }
 
-    template<int OtherOptions>
-    inline void swap(SparseMatrix<Scalar,OtherOptions,StorageIndex>& other)
-    {
-      eigen_assert(other.outerSize()==1);
-      std::swap(m_size, other.m_innerSize);
-      m_data.swap(other.m_data);
-    }
+  /** \internal \deprecated use insert(Index) */
+  EIGEN_DEPRECATED Scalar& fillrand(Index i) { return insert(i); }
 
-    inline SparseVector& operator=(const SparseVector& other)
-    {
-      if (other.isRValue())
-      {
-        swap(other.const_cast_derived());
-      }
-      else
-      {
-        resize(other.size());
-        m_data = other.m_data;
-      }
-      return *this;
-    }
+  /** \internal \deprecated use finalize() */
+  EIGEN_DEPRECATED void endFill() {}
 
-    template<typename OtherDerived>
-    inline SparseVector& operator=(const SparseMatrixBase<OtherDerived>& other)
-    {
-      SparseVector tmp(other.size());
-      internal::sparse_vector_assign_selector<SparseVector,OtherDerived>::run(tmp,other.derived());
-      this->swap(tmp);
-      return *this;
-    }
+  // These two functions were here in the 3.1 release, so let's keep them in case some code rely on them.
+  /** \internal \deprecated use data() */
+  EIGEN_DEPRECATED Storage& _data() { return m_data; }
+  /** \internal \deprecated use data() */
+  EIGEN_DEPRECATED const Storage& _data() const { return m_data; }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    template<typename Lhs, typename Rhs>
-    inline SparseVector& operator=(const SparseSparseProduct<Lhs,Rhs>& product)
-    {
-      return Base::operator=(product);
-    }
-    #endif
+#ifdef EIGEN_SPARSEVECTOR_PLUGIN
+#include EIGEN_SPARSEVECTOR_PLUGIN
+#endif
 
-    friend std::ostream & operator << (std::ostream & s, const SparseVector& m)
-    {
-      for (Index i=0; i<m.nonZeros(); ++i)
-        s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
-      s << std::endl;
-      return s;
-    }
+ protected:
+  EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned, THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE)
+  EIGEN_STATIC_ASSERT((Options_ & (ColMajor | RowMajor)) == Options, INVALID_MATRIX_TEMPLATE_PARAMETERS)
 
-    /** Destructor */
-    inline ~SparseVector() {}
-
-    /** Overloaded for performance */
-    Scalar sum() const;
-
-  public:
-
-    /** \internal \deprecated use setZero() and reserve() */
-    EIGEN_DEPRECATED void startFill(Index reserve)
-    {
-      setZero();
-      m_data.reserve(reserve);
-    }
-
-    /** \internal \deprecated use insertBack(Index,Index) */
-    EIGEN_DEPRECATED Scalar& fill(Index r, Index c)
-    {
-      eigen_assert(r==0 || c==0);
-      return fill(IsColVector ? r : c);
-    }
-
-    /** \internal \deprecated use insertBack(Index) */
-    EIGEN_DEPRECATED Scalar& fill(Index i)
-    {
-      m_data.append(0, i);
-      return m_data.value(m_data.size()-1);
-    }
-
-    /** \internal \deprecated use insert(Index,Index) */
-    EIGEN_DEPRECATED Scalar& fillrand(Index r, Index c)
-    {
-      eigen_assert(r==0 || c==0);
-      return fillrand(IsColVector ? r : c);
-    }
-
-    /** \internal \deprecated use insert(Index) */
-    EIGEN_DEPRECATED Scalar& fillrand(Index i)
-    {
-      return insert(i);
-    }
-
-    /** \internal \deprecated use finalize() */
-    EIGEN_DEPRECATED void endFill() {}
-    
-    // These two functions were here in the 3.1 release, so let's keep them in case some code rely on them.
-    /** \internal \deprecated use data() */
-    EIGEN_DEPRECATED Storage& _data() { return m_data; }
-    /** \internal \deprecated use data() */
-    EIGEN_DEPRECATED const Storage& _data() const { return m_data; }
-    
-#   ifdef EIGEN_SPARSEVECTOR_PLUGIN
-#     include EIGEN_SPARSEVECTOR_PLUGIN
-#   endif
-
-protected:
-  
-    static void check_template_parameters()
-    {
-      EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE);
-      EIGEN_STATIC_ASSERT((_Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS);
-    }
-    
-    Storage m_data;
-    Index m_size;
+  Storage m_data;
+  Index m_size;
 };
 
 namespace internal {
 
-template<typename _Scalar, int _Options, typename _Index>
-struct evaluator<SparseVector<_Scalar,_Options,_Index> >
-  : evaluator_base<SparseVector<_Scalar,_Options,_Index> >
-{
-  typedef SparseVector<_Scalar,_Options,_Index> SparseVectorType;
+template <typename Scalar_, int Options_, typename Index_>
+struct evaluator<SparseVector<Scalar_, Options_, Index_> > : evaluator_base<SparseVector<Scalar_, Options_, Index_> > {
+  typedef SparseVector<Scalar_, Options_, Index_> SparseVectorType;
   typedef evaluator_base<SparseVectorType> Base;
   typedef typename SparseVectorType::InnerIterator InnerIterator;
   typedef typename SparseVectorType::ReverseInnerIterator ReverseInnerIterator;
-  
-  enum {
-    CoeffReadCost = NumTraits<_Scalar>::ReadCost,
-    Flags = SparseVectorType::Flags
-  };
+
+  enum { CoeffReadCost = NumTraits<Scalar_>::ReadCost, Flags = SparseVectorType::Flags };
 
   evaluator() : Base() {}
-  
-  explicit evaluator(const SparseVectorType &mat) : m_matrix(&mat)
-  {
-    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
-  }
-  
-  inline Index nonZerosEstimate() const {
-    return m_matrix->nonZeros();
-  }
-  
+
+  explicit evaluator(const SparseVectorType& mat) : m_matrix(&mat) { EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); }
+
+  inline Index nonZerosEstimate() const { return m_matrix->nonZeros(); }
+
   operator SparseVectorType&() { return m_matrix->const_cast_derived(); }
   operator const SparseVectorType&() const { return *m_matrix; }
-  
-  const SparseVectorType *m_matrix;
+
+  const SparseVectorType* m_matrix;
 };
 
-template< typename Dest, typename Src>
-struct sparse_vector_assign_selector<Dest,Src,SVA_Inner> {
+template <typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest, Src, SVA_Inner> {
   static void run(Dest& dst, const Src& src) {
-    eigen_internal_assert(src.innerSize()==src.size());
+    eigen_internal_assert(src.innerSize() == src.size());
     typedef internal::evaluator<Src> SrcEvaluatorType;
     SrcEvaluatorType srcEval(src);
-    for(typename SrcEvaluatorType::InnerIterator it(srcEval, 0); it; ++it)
-      dst.insert(it.index()) = it.value();
+    for (typename SrcEvaluatorType::InnerIterator it(srcEval, 0); it; ++it) dst.insert(it.index()) = it.value();
   }
 };
 
-template< typename Dest, typename Src>
-struct sparse_vector_assign_selector<Dest,Src,SVA_Outer> {
+template <typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest, Src, SVA_Outer> {
   static void run(Dest& dst, const Src& src) {
-    eigen_internal_assert(src.outerSize()==src.size());
+    eigen_internal_assert(src.outerSize() == src.size());
     typedef internal::evaluator<Src> SrcEvaluatorType;
     SrcEvaluatorType srcEval(src);
-    for(Index i=0; i<src.size(); ++i)
-    {
+    for (Index i = 0; i < src.size(); ++i) {
       typename SrcEvaluatorType::InnerIterator it(srcEval, i);
-      if(it)
-        dst.insert(i) = it.value();
+      if (it) dst.insert(i) = it.value();
     }
   }
 };
 
-template< typename Dest, typename Src>
-struct sparse_vector_assign_selector<Dest,Src,SVA_RuntimeSwitch> {
+template <typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest, Src, SVA_RuntimeSwitch> {
   static void run(Dest& dst, const Src& src) {
-    if(src.outerSize()==1)  sparse_vector_assign_selector<Dest,Src,SVA_Inner>::run(dst, src);
-    else                    sparse_vector_assign_selector<Dest,Src,SVA_Outer>::run(dst, src);
+    if (src.outerSize() == 1)
+      sparse_vector_assign_selector<Dest, Src, SVA_Inner>::run(dst, src);
+    else
+      sparse_vector_assign_selector<Dest, Src, SVA_Outer>::run(dst, src);
   }
 };
 
-}
+}  // namespace internal
 
-} // end namespace Eigen
+// Specialization for SparseVector.
+// Serializes [size, numNonZeros, innerIndices, values].
+template <typename Scalar, int Options, typename StorageIndex>
+class Serializer<SparseVector<Scalar, Options, StorageIndex>, void> {
+ public:
+  typedef SparseVector<Scalar, Options, StorageIndex> SparseMat;
 
-#endif // EIGEN_SPARSEVECTOR_H
+  struct Header {
+    typename SparseMat::Index size;
+    Index num_non_zeros;
+  };
+
+  EIGEN_DEVICE_FUNC size_t size(const SparseMat& value) const {
+    return sizeof(Header) + (sizeof(Scalar) + sizeof(StorageIndex)) * value.nonZeros();
+  }
+
+  EIGEN_DEVICE_FUNC uint8_t* serialize(uint8_t* dest, uint8_t* end, const SparseMat& value) {
+    if (EIGEN_PREDICT_FALSE(dest == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(dest + size(value) > end)) return nullptr;
+
+    const size_t header_bytes = sizeof(Header);
+    Header header = {value.innerSize(), value.nonZeros()};
+    EIGEN_USING_STD(memcpy)
+    memcpy(dest, &header, header_bytes);
+    dest += header_bytes;
+
+    // Inner indices.
+    std::size_t data_bytes = sizeof(StorageIndex) * header.num_non_zeros;
+    memcpy(dest, value.innerIndexPtr(), data_bytes);
+    dest += data_bytes;
+
+    // Values.
+    data_bytes = sizeof(Scalar) * header.num_non_zeros;
+    memcpy(dest, value.valuePtr(), data_bytes);
+    dest += data_bytes;
+
+    return dest;
+  }
+
+  EIGEN_DEVICE_FUNC const uint8_t* deserialize(const uint8_t* src, const uint8_t* end, SparseMat& value) const {
+    if (EIGEN_PREDICT_FALSE(src == nullptr)) return nullptr;
+    if (EIGEN_PREDICT_FALSE(src + sizeof(Header) > end)) return nullptr;
+
+    const size_t header_bytes = sizeof(Header);
+    Header header;
+    EIGEN_USING_STD(memcpy)
+    memcpy(&header, src, header_bytes);
+    src += header_bytes;
+
+    value.setZero();
+    value.resize(header.size);
+    value.resizeNonZeros(header.num_non_zeros);
+
+    // Inner indices.
+    std::size_t data_bytes = sizeof(StorageIndex) * header.num_non_zeros;
+    if (EIGEN_PREDICT_FALSE(src + data_bytes > end)) return nullptr;
+    memcpy(value.innerIndexPtr(), src, data_bytes);
+    src += data_bytes;
+
+    // Values.
+    data_bytes = sizeof(Scalar) * header.num_non_zeros;
+    if (EIGEN_PREDICT_FALSE(src + data_bytes > end)) return nullptr;
+    memcpy(value.valuePtr(), src, data_bytes);
+    src += data_bytes;
+    return src;
+  }
+};
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_SPARSEVECTOR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h
index 92b3d1f..7220bee 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h
@@ -11,64 +11,63 @@
 #ifndef EIGEN_SPARSEVIEW_H
 #define EIGEN_SPARSEVIEW_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename MatrixType>
-struct traits<SparseView<MatrixType> > : traits<MatrixType>
-{
+template <typename MatrixType>
+struct traits<SparseView<MatrixType> > : traits<MatrixType> {
   typedef typename MatrixType::StorageIndex StorageIndex;
   typedef Sparse StorageKind;
-  enum {
-    Flags = int(traits<MatrixType>::Flags) & (RowMajorBit)
-  };
+  enum { Flags = int(traits<MatrixType>::Flags) & (RowMajorBit) };
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup SparseCore_Module
-  * \class SparseView
-  *
-  * \brief Expression of a dense or sparse matrix with zero or too small values removed
-  *
-  * \tparam MatrixType the type of the object of which we are removing the small entries
-  *
-  * This class represents an expression of a given dense or sparse matrix with
-  * entries smaller than \c reference * \c epsilon are removed.
-  * It is the return type of MatrixBase::sparseView() and SparseMatrixBase::pruned()
-  * and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::sparseView(), SparseMatrixBase::pruned()
-  */
-template<typename MatrixType>
-class SparseView : public SparseMatrixBase<SparseView<MatrixType> >
-{
+ * \class SparseView
+ *
+ * \brief Expression of a dense or sparse matrix with zero or too small values removed
+ *
+ * \tparam MatrixType the type of the object of which we are removing the small entries
+ *
+ * This class represents an expression of a given dense or sparse matrix with
+ * entries smaller than \c reference * \c epsilon are removed.
+ * It is the return type of MatrixBase::sparseView() and SparseMatrixBase::pruned()
+ * and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::sparseView(), SparseMatrixBase::pruned()
+ */
+template <typename MatrixType>
+class SparseView : public SparseMatrixBase<SparseView<MatrixType> > {
   typedef typename MatrixType::Nested MatrixTypeNested;
-  typedef typename internal::remove_all<MatrixTypeNested>::type _MatrixTypeNested;
-  typedef SparseMatrixBase<SparseView > Base;
-public:
+  typedef internal::remove_all_t<MatrixTypeNested> MatrixTypeNested_;
+  typedef SparseMatrixBase<SparseView> Base;
+
+ public:
   EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView)
-  typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+  typedef internal::remove_all_t<MatrixType> NestedExpression;
 
   explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0),
-                      const RealScalar &epsilon = NumTraits<Scalar>::dummy_precision())
-    : m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {}
+                      const RealScalar& epsilon = NumTraits<Scalar>::dummy_precision())
+      : m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {}
 
   inline Index rows() const { return m_matrix.rows(); }
   inline Index cols() const { return m_matrix.cols(); }
 
   inline Index innerSize() const { return m_matrix.innerSize(); }
   inline Index outerSize() const { return m_matrix.outerSize(); }
-  
+
   /** \returns the nested expression */
-  const typename internal::remove_all<MatrixTypeNested>::type&
-  nestedExpression() const { return m_matrix; }
-  
+  const internal::remove_all_t<MatrixTypeNested>& nestedExpression() const { return m_matrix; }
+
   Scalar reference() const { return m_reference; }
   RealScalar epsilon() const { return m_epsilon; }
-  
-protected:
+
+ protected:
   MatrixTypeNested m_matrix;
   Scalar m_reference;
   RealScalar m_epsilon;
@@ -79,176 +78,148 @@
 // TODO find a way to unify the two following variants
 // This is tricky because implementing an inner iterator on top of an IndexBased evaluator is
 // not easy because the evaluators do not expose the sizes of the underlying expression.
-  
-template<typename ArgType>
-struct unary_evaluator<SparseView<ArgType>, IteratorBased>
-  : public evaluator_base<SparseView<ArgType> >
-{
-    typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
-  public:
-    typedef SparseView<ArgType> XprType;
-    
-    class InnerIterator : public EvalIterator
-    {
-      protected:
-        typedef typename XprType::Scalar Scalar;
-      public:
 
-        EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
-          : EvalIterator(sve.m_argImpl,outer), m_view(sve.m_view)
-        {
-          incrementToNonZero();
-        }
+template <typename ArgType>
+struct unary_evaluator<SparseView<ArgType>, IteratorBased> : public evaluator_base<SparseView<ArgType> > {
+  typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
 
-        EIGEN_STRONG_INLINE InnerIterator& operator++()
-        {
-          EvalIterator::operator++();
-          incrementToNonZero();
-          return *this;
-        }
+ public:
+  typedef SparseView<ArgType> XprType;
 
-        using EvalIterator::value;
-
-      protected:
-        const XprType &m_view;
-
-      private:
-        void incrementToNonZero()
-        {
-          while((bool(*this)) && internal::isMuchSmallerThan(value(), m_view.reference(), m_view.epsilon()))
-          {
-            EvalIterator::operator++();
-          }
-        }
-    };
-    
-    enum {
-      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-      Flags = XprType::Flags
-    };
-    
-    explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
-
-  protected:
-    evaluator<ArgType> m_argImpl;
-    const XprType &m_view;
-};
-
-template<typename ArgType>
-struct unary_evaluator<SparseView<ArgType>, IndexBased>
-  : public evaluator_base<SparseView<ArgType> >
-{
-  public:
-    typedef SparseView<ArgType> XprType;
-  protected:
-    enum { IsRowMajor = (XprType::Flags&RowMajorBit)==RowMajorBit };
+  class InnerIterator : public EvalIterator {
+   protected:
     typedef typename XprType::Scalar Scalar;
-    typedef typename XprType::StorageIndex StorageIndex;
-  public:
-    
-    class InnerIterator
-    {
-      public:
 
-        EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
-          : m_sve(sve), m_inner(0), m_outer(outer), m_end(sve.m_view.innerSize())
-        {
-          incrementToNonZero();
-        }
+   public:
+    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
+        : EvalIterator(sve.m_argImpl, outer), m_view(sve.m_view) {
+      incrementToNonZero();
+    }
 
-        EIGEN_STRONG_INLINE InnerIterator& operator++()
-        {
-          m_inner++;
-          incrementToNonZero();
-          return *this;
-        }
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
+      EvalIterator::operator++();
+      incrementToNonZero();
+      return *this;
+    }
 
-        EIGEN_STRONG_INLINE Scalar value() const
-        {
-          return (IsRowMajor) ? m_sve.m_argImpl.coeff(m_outer, m_inner)
-                              : m_sve.m_argImpl.coeff(m_inner, m_outer);
-        }
+    using EvalIterator::value;
 
-        EIGEN_STRONG_INLINE StorageIndex index() const { return m_inner; }
-        inline Index row() const { return IsRowMajor ? m_outer : index(); }
-        inline Index col() const { return IsRowMajor ? index() : m_outer; }
+   protected:
+    const XprType& m_view;
 
-        EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; }
+   private:
+    void incrementToNonZero() {
+      while ((bool(*this)) && internal::isMuchSmallerThan(value(), m_view.reference(), m_view.epsilon())) {
+        EvalIterator::operator++();
+      }
+    }
+  };
 
-      protected:
-        const unary_evaluator &m_sve;
-        Index m_inner;
-        const Index m_outer;
-        const Index m_end;
+  enum { CoeffReadCost = evaluator<ArgType>::CoeffReadCost, Flags = XprType::Flags };
 
-      private:
-        void incrementToNonZero()
-        {
-          while((bool(*this)) && internal::isMuchSmallerThan(value(), m_sve.m_view.reference(), m_sve.m_view.epsilon()))
-          {
-            m_inner++;
-          }
-        }
-    };
-    
-    enum {
-      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-      Flags = XprType::Flags
-    };
-    
-    explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
+  explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
 
-  protected:
-    evaluator<ArgType> m_argImpl;
-    const XprType &m_view;
+ protected:
+  evaluator<ArgType> m_argImpl;
+  const XprType& m_view;
 };
 
-} // end namespace internal
+template <typename ArgType>
+struct unary_evaluator<SparseView<ArgType>, IndexBased> : public evaluator_base<SparseView<ArgType> > {
+ public:
+  typedef SparseView<ArgType> XprType;
+
+ protected:
+  enum { IsRowMajor = (XprType::Flags & RowMajorBit) == RowMajorBit };
+  typedef typename XprType::Scalar Scalar;
+  typedef typename XprType::StorageIndex StorageIndex;
+
+ public:
+  class InnerIterator {
+   public:
+    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
+        : m_sve(sve), m_inner(0), m_outer(outer), m_end(sve.m_view.innerSize()) {
+      incrementToNonZero();
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++() {
+      m_inner++;
+      incrementToNonZero();
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const {
+      return (IsRowMajor) ? m_sve.m_argImpl.coeff(m_outer, m_inner) : m_sve.m_argImpl.coeff(m_inner, m_outer);
+    }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_inner; }
+    inline Index row() const { return IsRowMajor ? m_outer : index(); }
+    inline Index col() const { return IsRowMajor ? index() : m_outer; }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner >= 0; }
+
+   protected:
+    const unary_evaluator& m_sve;
+    Index m_inner;
+    const Index m_outer;
+    const Index m_end;
+
+   private:
+    void incrementToNonZero() {
+      while ((bool(*this)) && internal::isMuchSmallerThan(value(), m_sve.m_view.reference(), m_sve.m_view.epsilon())) {
+        m_inner++;
+      }
+    }
+  };
+
+  enum { CoeffReadCost = evaluator<ArgType>::CoeffReadCost, Flags = XprType::Flags };
+
+  explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
+
+ protected:
+  evaluator<ArgType> m_argImpl;
+  const XprType& m_view;
+};
+
+}  // end namespace internal
 
 /** \ingroup SparseCore_Module
-  *
-  * \returns a sparse expression of the dense expression \c *this with values smaller than
-  * \a reference * \a epsilon removed.
-  *
-  * This method is typically used when prototyping to convert a quickly assembled dense Matrix \c D to a SparseMatrix \c S:
-  * \code
-  * MatrixXd D(n,m);
-  * SparseMatrix<double> S;
-  * S = D.sparseView();             // suppress numerical zeros (exact)
-  * S = D.sparseView(reference);
-  * S = D.sparseView(reference,epsilon);
-  * \endcode
-  * where \a reference is a meaningful non zero reference value,
-  * and \a epsilon is a tolerance factor defaulting to NumTraits<Scalar>::dummy_precision().
-  *
-  * \sa SparseMatrixBase::pruned(), class SparseView */
-template<typename Derived>
+ *
+ * \returns a sparse expression of the dense expression \c *this with values smaller than
+ * \a reference * \a epsilon removed.
+ *
+ * This method is typically used when prototyping to convert a quickly assembled dense Matrix \c D to a SparseMatrix \c
+ * S: \code MatrixXd D(n,m); SparseMatrix<double> S; S = D.sparseView();             // suppress numerical zeros (exact)
+ * S = D.sparseView(reference);
+ * S = D.sparseView(reference,epsilon);
+ * \endcode
+ * where \a reference is a meaningful non zero reference value,
+ * and \a epsilon is a tolerance factor defaulting to NumTraits<Scalar>::dummy_precision().
+ *
+ * \sa SparseMatrixBase::pruned(), class SparseView */
+template <typename Derived>
 const SparseView<Derived> MatrixBase<Derived>::sparseView(const Scalar& reference,
-                                                          const typename NumTraits<Scalar>::Real& epsilon) const
-{
+                                                          const typename NumTraits<Scalar>::Real& epsilon) const {
   return SparseView<Derived>(derived(), reference, epsilon);
 }
 
 /** \returns an expression of \c *this with values smaller than
-  * \a reference * \a epsilon removed.
-  *
-  * This method is typically used in conjunction with the product of two sparse matrices
-  * to automatically prune the smallest values as follows:
-  * \code
-  * C = (A*B).pruned();             // suppress numerical zeros (exact)
-  * C = (A*B).pruned(ref);
-  * C = (A*B).pruned(ref,epsilon);
-  * \endcode
-  * where \c ref is a meaningful non zero reference value.
-  * */
-template<typename Derived>
-const SparseView<Derived>
-SparseMatrixBase<Derived>::pruned(const Scalar& reference,
-                                  const RealScalar& epsilon) const
-{
+ * \a reference * \a epsilon removed.
+ *
+ * This method is typically used in conjunction with the product of two sparse matrices
+ * to automatically prune the smallest values as follows:
+ * \code
+ * C = (A*B).pruned();             // suppress numerical zeros (exact)
+ * C = (A*B).pruned(ref);
+ * C = (A*B).pruned(ref,epsilon);
+ * \endcode
+ * where \c ref is a meaningful non zero reference value.
+ * */
+template <typename Derived>
+const SparseView<Derived> SparseMatrixBase<Derived>::pruned(const Scalar& reference, const RealScalar& epsilon) const {
   return SparseView<Derived>(derived(), reference, epsilon);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
index 7cb2c26..7753a24 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
@@ -10,50 +10,44 @@
 #ifndef EIGEN_SPARSETRIANGULARSOLVER_H
 #define EIGEN_SPARSETRIANGULARSOLVER_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, int Mode,
-  int UpLo = (Mode & Lower)
-           ? Lower
-           : (Mode & Upper)
-           ? Upper
-           : -1,
-  int StorageOrder = int(traits<Lhs>::Flags) & RowMajorBit>
+template <typename Lhs, typename Rhs, int Mode,
+          int UpLo = (Mode & Lower)   ? Lower
+                     : (Mode & Upper) ? Upper
+                                      : -1,
+          int StorageOrder = int(traits<Lhs>::Flags) & RowMajorBit>
 struct sparse_solve_triangular_selector;
 
 // forward substitution, row-major
-template<typename Lhs, typename Rhs, int Mode>
-struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Lower,RowMajor>
-{
+template <typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs, Rhs, Mode, Lower, RowMajor> {
   typedef typename Rhs::Scalar Scalar;
   typedef evaluator<Lhs> LhsEval;
   typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
-  static void run(const Lhs& lhs, Rhs& other)
-  {
+  static void run(const Lhs& lhs, Rhs& other) {
     LhsEval lhsEval(lhs);
-    for(Index col=0 ; col<other.cols() ; ++col)
-    {
-      for(Index i=0; i<lhs.rows(); ++i)
-      {
-        Scalar tmp = other.coeff(i,col);
+    for (Index col = 0; col < other.cols(); ++col) {
+      for (Index i = 0; i < lhs.rows(); ++i) {
+        Scalar tmp = other.coeff(i, col);
         Scalar lastVal(0);
         Index lastIndex = 0;
-        for(LhsIterator it(lhsEval, i); it; ++it)
-        {
+        for (LhsIterator it(lhsEval, i); it; ++it) {
           lastVal = it.value();
           lastIndex = it.index();
-          if(lastIndex==i)
-            break;
-          tmp -= lastVal * other.coeff(lastIndex,col);
+          if (lastIndex == i) break;
+          tmp -= lastVal * other.coeff(lastIndex, col);
         }
         if (Mode & UnitDiag)
-          other.coeffRef(i,col) = tmp;
-        else
-        {
-          eigen_assert(lastIndex==i);
-          other.coeffRef(i,col) = tmp/lastVal;
+          other.coeffRef(i, col) = tmp;
+        else {
+          eigen_assert(lastIndex == i);
+          other.coeffRef(i, col) = tmp / lastVal;
         }
       }
     }
@@ -61,73 +55,59 @@
 };
 
 // backward substitution, row-major
-template<typename Lhs, typename Rhs, int Mode>
-struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Upper,RowMajor>
-{
+template <typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs, Rhs, Mode, Upper, RowMajor> {
   typedef typename Rhs::Scalar Scalar;
   typedef evaluator<Lhs> LhsEval;
   typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
-  static void run(const Lhs& lhs, Rhs& other)
-  {
+  static void run(const Lhs& lhs, Rhs& other) {
     LhsEval lhsEval(lhs);
-    for(Index col=0 ; col<other.cols() ; ++col)
-    {
-      for(Index i=lhs.rows()-1 ; i>=0 ; --i)
-      {
-        Scalar tmp = other.coeff(i,col);
+    for (Index col = 0; col < other.cols(); ++col) {
+      for (Index i = lhs.rows() - 1; i >= 0; --i) {
+        Scalar tmp = other.coeff(i, col);
         Scalar l_ii(0);
         LhsIterator it(lhsEval, i);
-        while(it && it.index()<i)
-          ++it;
-        if(!(Mode & UnitDiag))
-        {
-          eigen_assert(it && it.index()==i);
+        while (it && it.index() < i) ++it;
+        if (!(Mode & UnitDiag)) {
+          eigen_assert(it && it.index() == i);
           l_ii = it.value();
           ++it;
-        }
-        else if (it && it.index() == i)
+        } else if (it && it.index() == i)
           ++it;
-        for(; it; ++it)
-        {
-          tmp -= it.value() * other.coeff(it.index(),col);
+        for (; it; ++it) {
+          tmp -= it.value() * other.coeff(it.index(), col);
         }
 
-        if (Mode & UnitDiag)  other.coeffRef(i,col) = tmp;
-        else                  other.coeffRef(i,col) = tmp/l_ii;
+        if (Mode & UnitDiag)
+          other.coeffRef(i, col) = tmp;
+        else
+          other.coeffRef(i, col) = tmp / l_ii;
       }
     }
   }
 };
 
 // forward substitution, col-major
-template<typename Lhs, typename Rhs, int Mode>
-struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Lower,ColMajor>
-{
+template <typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs, Rhs, Mode, Lower, ColMajor> {
   typedef typename Rhs::Scalar Scalar;
   typedef evaluator<Lhs> LhsEval;
   typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
-  static void run(const Lhs& lhs, Rhs& other)
-  {
+  static void run(const Lhs& lhs, Rhs& other) {
     LhsEval lhsEval(lhs);
-    for(Index col=0 ; col<other.cols() ; ++col)
-    {
-      for(Index i=0; i<lhs.cols(); ++i)
-      {
-        Scalar& tmp = other.coeffRef(i,col);
-        if (tmp!=Scalar(0)) // optimization when other is actually sparse
+    for (Index col = 0; col < other.cols(); ++col) {
+      for (Index i = 0; i < lhs.cols(); ++i) {
+        Scalar& tmp = other.coeffRef(i, col);
+        if (!numext::is_exactly_zero(tmp))  // optimization when other is actually sparse
         {
           LhsIterator it(lhsEval, i);
-          while(it && it.index()<i)
-            ++it;
-          if(!(Mode & UnitDiag))
-          {
-            eigen_assert(it && it.index()==i);
+          while (it && it.index() < i) ++it;
+          if (!(Mode & UnitDiag)) {
+            eigen_assert(it && it.index() == i);
             tmp /= it.value();
           }
-          if (it && it.index()==i)
-            ++it;
-          for(; it; ++it)
-            other.coeffRef(it.index(), col) -= tmp * it.value();
+          if (it && it.index() == i) ++it;
+          for (; it; ++it) other.coeffRef(it.index(), col) -= tmp * it.value();
         }
       }
     }
@@ -135,61 +115,53 @@
 };
 
 // backward substitution, col-major
-template<typename Lhs, typename Rhs, int Mode>
-struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Upper,ColMajor>
-{
+template <typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs, Rhs, Mode, Upper, ColMajor> {
   typedef typename Rhs::Scalar Scalar;
   typedef evaluator<Lhs> LhsEval;
   typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
-  static void run(const Lhs& lhs, Rhs& other)
-  {
+  static void run(const Lhs& lhs, Rhs& other) {
     LhsEval lhsEval(lhs);
-    for(Index col=0 ; col<other.cols() ; ++col)
-    {
-      for(Index i=lhs.cols()-1; i>=0; --i)
-      {
-        Scalar& tmp = other.coeffRef(i,col);
-        if (tmp!=Scalar(0)) // optimization when other is actually sparse
+    for (Index col = 0; col < other.cols(); ++col) {
+      for (Index i = lhs.cols() - 1; i >= 0; --i) {
+        Scalar& tmp = other.coeffRef(i, col);
+        if (!numext::is_exactly_zero(tmp))  // optimization when other is actually sparse
         {
-          if(!(Mode & UnitDiag))
-          {
+          if (!(Mode & UnitDiag)) {
             // TODO replace this by a binary search. make sure the binary search is safe for partially sorted elements
             LhsIterator it(lhsEval, i);
-            while(it && it.index()!=i)
-              ++it;
-            eigen_assert(it && it.index()==i);
-            other.coeffRef(i,col) /= it.value();
+            while (it && it.index() != i) ++it;
+            eigen_assert(it && it.index() == i);
+            other.coeffRef(i, col) /= it.value();
           }
           LhsIterator it(lhsEval, i);
-          for(; it && it.index()<i; ++it)
-            other.coeffRef(it.index(), col) -= tmp * it.value();
+          for (; it && it.index() < i; ++it) other.coeffRef(it.index(), col) -= tmp * it.value();
         }
       }
     }
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 
-template<typename ExpressionType,unsigned int Mode>
-template<typename OtherDerived>
-void TriangularViewImpl<ExpressionType,Mode,Sparse>::solveInPlace(MatrixBase<OtherDerived>& other) const
-{
+template <typename ExpressionType, unsigned int Mode>
+template <typename OtherDerived>
+void TriangularViewImpl<ExpressionType, Mode, Sparse>::solveInPlace(MatrixBase<OtherDerived>& other) const {
   eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows());
-  eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+  eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper | Lower)));
 
   enum { copy = internal::traits<OtherDerived>::Flags & RowMajorBit };
 
-  typedef typename internal::conditional<copy,
-    typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::type OtherCopy;
+  typedef std::conditional_t<copy, typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>
+      OtherCopy;
   OtherCopy otherCopy(other.derived());
 
-  internal::sparse_solve_triangular_selector<ExpressionType, typename internal::remove_reference<OtherCopy>::type, Mode>::run(derived().nestedExpression(), otherCopy);
+  internal::sparse_solve_triangular_selector<ExpressionType, std::remove_reference_t<OtherCopy>, Mode>::run(
+      derived().nestedExpression(), otherCopy);
 
-  if (copy)
-    other = otherCopy;
+  if (copy) other = otherCopy;
 }
 #endif
 
@@ -197,119 +169,98 @@
 
 namespace internal {
 
-template<typename Lhs, typename Rhs, int Mode,
-  int UpLo = (Mode & Lower)
-           ? Lower
-           : (Mode & Upper)
-           ? Upper
-           : -1,
-  int StorageOrder = int(Lhs::Flags) & (RowMajorBit)>
+template <typename Lhs, typename Rhs, int Mode,
+          int UpLo = (Mode & Lower)   ? Lower
+                     : (Mode & Upper) ? Upper
+                                      : -1,
+          int StorageOrder = int(Lhs::Flags) & (RowMajorBit)>
 struct sparse_solve_triangular_sparse_selector;
 
 // forward substitution, col-major
-template<typename Lhs, typename Rhs, int Mode, int UpLo>
-struct sparse_solve_triangular_sparse_selector<Lhs,Rhs,Mode,UpLo,ColMajor>
-{
+template <typename Lhs, typename Rhs, int Mode, int UpLo>
+struct sparse_solve_triangular_sparse_selector<Lhs, Rhs, Mode, UpLo, ColMajor> {
   typedef typename Rhs::Scalar Scalar;
-  typedef typename promote_index_type<typename traits<Lhs>::StorageIndex,
-                                      typename traits<Rhs>::StorageIndex>::type StorageIndex;
-  static void run(const Lhs& lhs, Rhs& other)
-  {
-    const bool IsLower = (UpLo==Lower);
-    AmbiVector<Scalar,StorageIndex> tempVector(other.rows()*2);
-    tempVector.setBounds(0,other.rows());
+  typedef typename promote_index_type<typename traits<Lhs>::StorageIndex, typename traits<Rhs>::StorageIndex>::type
+      StorageIndex;
+  static void run(const Lhs& lhs, Rhs& other) {
+    const bool IsLower = (UpLo == Lower);
+    AmbiVector<Scalar, StorageIndex> tempVector(other.rows() * 2);
+    tempVector.setBounds(0, other.rows());
 
     Rhs res(other.rows(), other.cols());
     res.reserve(other.nonZeros());
 
-    for(Index col=0 ; col<other.cols() ; ++col)
-    {
+    for (Index col = 0; col < other.cols(); ++col) {
       // FIXME estimate number of non zeros
-      tempVector.init(.99/*float(other.col(col).nonZeros())/float(other.rows())*/);
+      tempVector.init(.99 /*float(other.col(col).nonZeros())/float(other.rows())*/);
       tempVector.setZero();
       tempVector.restart();
-      for (typename Rhs::InnerIterator rhsIt(other, col); rhsIt; ++rhsIt)
-      {
+      for (typename Rhs::InnerIterator rhsIt(other, col); rhsIt; ++rhsIt) {
         tempVector.coeffRef(rhsIt.index()) = rhsIt.value();
       }
 
-      for(Index i=IsLower?0:lhs.cols()-1;
-          IsLower?i<lhs.cols():i>=0;
-          i+=IsLower?1:-1)
-      {
+      for (Index i = IsLower ? 0 : lhs.cols() - 1; IsLower ? i < lhs.cols() : i >= 0; i += IsLower ? 1 : -1) {
         tempVector.restart();
         Scalar& ci = tempVector.coeffRef(i);
-        if (ci!=Scalar(0))
-        {
+        if (!numext::is_exactly_zero(ci)) {
           // find
           typename Lhs::InnerIterator it(lhs, i);
-          if(!(Mode & UnitDiag))
-          {
-            if (IsLower)
-            {
-              eigen_assert(it.index()==i);
+          if (!(Mode & UnitDiag)) {
+            if (IsLower) {
+              eigen_assert(it.index() == i);
               ci /= it.value();
-            }
-            else
-              ci /= lhs.coeff(i,i);
+            } else
+              ci /= lhs.coeff(i, i);
           }
           tempVector.restart();
-          if (IsLower)
-          {
-            if (it.index()==i)
-              ++it;
-            for(; it; ++it)
-              tempVector.coeffRef(it.index()) -= ci * it.value();
-          }
-          else
-          {
-            for(; it && it.index()<i; ++it)
-              tempVector.coeffRef(it.index()) -= ci * it.value();
+          if (IsLower) {
+            if (it.index() == i) ++it;
+            for (; it; ++it) tempVector.coeffRef(it.index()) -= ci * it.value();
+          } else {
+            for (; it && it.index() < i; ++it) tempVector.coeffRef(it.index()) -= ci * it.value();
           }
         }
       }
 
-
-//       Index count = 0;
+      //       Index count = 0;
       // FIXME compute a reference value to filter zeros
-      for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
-      {
-//         ++ count;
-//         std::cerr << "fill " << it.index() << ", " << col << "\n";
-//         std::cout << it.value() << "  ";
+      for (typename AmbiVector<Scalar, StorageIndex>::Iterator it(tempVector /*,1e-12*/); it; ++it) {
+        //         ++ count;
+        //         std::cerr << "fill " << it.index() << ", " << col << "\n";
+        //         std::cout << it.value() << "  ";
         // FIXME use insertBack
         res.insert(it.index(), col) = it.value();
       }
-//       std::cout << "tempVector.nonZeros() == " << int(count) << " / " << (other.rows()) << "\n";
+      //       std::cout << "tempVector.nonZeros() == " << int(count) << " / " << (other.rows()) << "\n";
     }
     res.finalize();
     other = res.markAsRValue();
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-template<typename ExpressionType,unsigned int Mode>
-template<typename OtherDerived>
-void TriangularViewImpl<ExpressionType,Mode,Sparse>::solveInPlace(SparseMatrixBase<OtherDerived>& other) const
-{
+template <typename ExpressionType, unsigned int Mode>
+template <typename OtherDerived>
+void TriangularViewImpl<ExpressionType, Mode, Sparse>::solveInPlace(SparseMatrixBase<OtherDerived>& other) const {
   eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows());
-  eigen_assert( (!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+  eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper | Lower)));
 
-//   enum { copy = internal::traits<OtherDerived>::Flags & RowMajorBit };
+  //   enum { copy = internal::traits<OtherDerived>::Flags & RowMajorBit };
 
-//   typedef typename internal::conditional<copy,
-//     typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::type OtherCopy;
-//   OtherCopy otherCopy(other.derived());
+  //   typedef std::conditional_t<copy,
+  //     typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&> OtherCopy;
+  //   OtherCopy otherCopy(other.derived());
 
-  internal::sparse_solve_triangular_sparse_selector<ExpressionType, OtherDerived, Mode>::run(derived().nestedExpression(), other.derived());
+  internal::sparse_solve_triangular_sparse_selector<ExpressionType, OtherDerived, Mode>::run(
+      derived().nestedExpression(), other.derived());
 
-//   if (copy)
-//     other = otherCopy;
+  //   if (copy)
+  //     other = otherCopy;
 }
 #endif
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_SPARSETRIANGULARSOLVER_H
+#endif  // EIGEN_SPARSETRIANGULARSOLVER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/InternalHeaderCheck.h
new file mode 100644
index 0000000..78ebfcc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_SPARSELU_MODULE_H
+#error "Please include Eigen/SparseLU instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h
index 0c8d893..aee3d94 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h
@@ -8,916 +8,883 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 #ifndef EIGEN_SPARSE_LU_H
 #define EIGEN_SPARSE_LU_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template <typename _MatrixType, typename _OrderingType = COLAMDOrdering<typename _MatrixType::StorageIndex> > class SparseLU;
-template <typename MappedSparseMatrixType> struct SparseLUMatrixLReturnType;
-template <typename MatrixLType, typename MatrixUType> struct SparseLUMatrixUReturnType;
+template <typename MatrixType_, typename OrderingType_ = COLAMDOrdering<typename MatrixType_::StorageIndex>>
+class SparseLU;
+template <typename MappedSparseMatrixType>
+struct SparseLUMatrixLReturnType;
+template <typename MatrixLType, typename MatrixUType>
+struct SparseLUMatrixUReturnType;
 
-template <bool Conjugate,class SparseLUType>
-class SparseLUTransposeView : public SparseSolverBase<SparseLUTransposeView<Conjugate,SparseLUType> >
-{
-protected:
-  typedef SparseSolverBase<SparseLUTransposeView<Conjugate,SparseLUType> > APIBase;
+template <bool Conjugate, class SparseLUType>
+class SparseLUTransposeView : public SparseSolverBase<SparseLUTransposeView<Conjugate, SparseLUType>> {
+ protected:
+  typedef SparseSolverBase<SparseLUTransposeView<Conjugate, SparseLUType>> APIBase;
   using APIBase::m_isInitialized;
-public:
+
+ public:
   typedef typename SparseLUType::Scalar Scalar;
   typedef typename SparseLUType::StorageIndex StorageIndex;
   typedef typename SparseLUType::MatrixType MatrixType;
   typedef typename SparseLUType::OrderingType OrderingType;
 
-  enum {
-    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-  };
+  enum { ColsAtCompileTime = MatrixType::ColsAtCompileTime, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime };
 
-  SparseLUTransposeView() : m_sparseLU(NULL) {}
-  SparseLUTransposeView(const SparseLUTransposeView& view) {
+  SparseLUTransposeView() : APIBase(), m_sparseLU(NULL) {}
+  SparseLUTransposeView(const SparseLUTransposeView& view) : APIBase() {
     this->m_sparseLU = view.m_sparseLU;
+    this->m_isInitialized = view.m_isInitialized;
   }
-  void setIsInitialized(const bool isInitialized) {this->m_isInitialized = isInitialized;}
-  void setSparseLU(SparseLUType* sparseLU) {m_sparseLU = sparseLU;}
+  void setIsInitialized(const bool isInitialized) { this->m_isInitialized = isInitialized; }
+  void setSparseLU(SparseLUType* sparseLU) { m_sparseLU = sparseLU; }
   using APIBase::_solve_impl;
-  template<typename Rhs, typename Dest>
-  bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &X_base) const
-  {
+  template <typename Rhs, typename Dest>
+  bool _solve_impl(const MatrixBase<Rhs>& B, MatrixBase<Dest>& X_base) const {
     Dest& X(X_base.derived());
     eigen_assert(m_sparseLU->info() == Success && "The matrix should be factorized first");
-    EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,
-                        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
-
+    EIGEN_STATIC_ASSERT((Dest::Flags & RowMajorBit) == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
 
     // this ugly const_cast_derived() helps to detect aliasing when applying the permutations
-    for(Index j = 0; j < B.cols(); ++j){
+    for (Index j = 0; j < B.cols(); ++j) {
       X.col(j) = m_sparseLU->colsPermutation() * B.const_cast_derived().col(j);
     }
-    //Forward substitution with transposed or adjoint of U
+    // Forward substitution with transposed or adjoint of U
     m_sparseLU->matrixU().template solveTransposedInPlace<Conjugate>(X);
 
-    //Backward substitution with transposed or adjoint of L
+    // Backward substitution with transposed or adjoint of L
     m_sparseLU->matrixL().template solveTransposedInPlace<Conjugate>(X);
 
     // Permute back the solution
-    for (Index j = 0; j < B.cols(); ++j)
-      X.col(j) = m_sparseLU->rowsPermutation().transpose() * X.col(j);
+    for (Index j = 0; j < B.cols(); ++j) X.col(j) = m_sparseLU->rowsPermutation().transpose() * X.col(j);
     return true;
   }
   inline Index rows() const { return m_sparseLU->rows(); }
   inline Index cols() const { return m_sparseLU->cols(); }
 
-private:
-  SparseLUType *m_sparseLU;
+ private:
+  SparseLUType* m_sparseLU;
   SparseLUTransposeView& operator=(const SparseLUTransposeView&);
 };
 
-
 /** \ingroup SparseLU_Module
-  * \class SparseLU
-  * 
-  * \brief Sparse supernodal LU factorization for general matrices
-  * 
-  * This class implements the supernodal LU factorization for general matrices.
-  * It uses the main techniques from the sequential SuperLU package 
-  * (http://crd-legacy.lbl.gov/~xiaoye/SuperLU/). It handles transparently real 
-  * and complex arithmetic with single and double precision, depending on the 
-  * scalar type of your input matrix. 
-  * The code has been optimized to provide BLAS-3 operations during supernode-panel updates. 
-  * It benefits directly from the built-in high-performant Eigen BLAS routines. 
-  * Moreover, when the size of a supernode is very small, the BLAS calls are avoided to 
-  * enable a better optimization from the compiler. For best performance, 
-  * you should compile it with NDEBUG flag to avoid the numerous bounds checking on vectors. 
-  * 
-  * An important parameter of this class is the ordering method. It is used to reorder the columns 
-  * (and eventually the rows) of the matrix to reduce the number of new elements that are created during 
-  * numerical factorization. The cheapest method available is COLAMD. 
-  * See  \link OrderingMethods_Module the OrderingMethods module \endlink for the list of 
-  * built-in and external ordering methods. 
-  *
-  * Simple example with key steps 
-  * \code
-  * VectorXd x(n), b(n);
-  * SparseMatrix<double> A;
-  * SparseLU<SparseMatrix<double>, COLAMDOrdering<int> >   solver;
-  * // fill A and b;
-  * // Compute the ordering permutation vector from the structural pattern of A
-  * solver.analyzePattern(A); 
-  * // Compute the numerical factorization 
-  * solver.factorize(A); 
-  * //Use the factors to solve the linear system 
-  * x = solver.solve(b); 
-  * \endcode
-  * 
-  * \warning The input matrix A should be in a \b compressed and \b column-major form.
-  * Otherwise an expensive copy will be made. You can call the inexpensive makeCompressed() to get a compressed matrix.
-  * 
-  * \note Unlike the initial SuperLU implementation, there is no step to equilibrate the matrix. 
-  * For badly scaled matrices, this step can be useful to reduce the pivoting during factorization. 
-  * If this is the case for your matrices, you can try the basic scaling method at
-  *  "unsupported/Eigen/src/IterativeSolvers/Scaling.h"
-  * 
-  * \tparam _MatrixType The type of the sparse matrix. It must be a column-major SparseMatrix<>
-  * \tparam _OrderingType The ordering method to use, either AMD, COLAMD or METIS. Default is COLMAD
-  *
-  * \implsparsesolverconcept
-  * 
-  * \sa \ref TutorialSparseSolverConcept
-  * \sa \ref OrderingMethods_Module
-  */
-template <typename _MatrixType, typename _OrderingType>
-class SparseLU : public SparseSolverBase<SparseLU<_MatrixType,_OrderingType> >, public internal::SparseLUImpl<typename _MatrixType::Scalar, typename _MatrixType::StorageIndex>
-{
-  protected:
-    typedef SparseSolverBase<SparseLU<_MatrixType,_OrderingType> > APIBase;
-    using APIBase::m_isInitialized;
-  public:
-    using APIBase::_solve_impl;
-    
-    typedef _MatrixType MatrixType; 
-    typedef _OrderingType OrderingType;
-    typedef typename MatrixType::Scalar Scalar; 
-    typedef typename MatrixType::RealScalar RealScalar; 
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> NCMatrix;
-    typedef internal::MappedSuperNodalMatrix<Scalar, StorageIndex> SCMatrix;
-    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
-    typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
-    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
-    typedef internal::SparseLUImpl<Scalar, StorageIndex> Base;
+ * \class SparseLU
+ *
+ * \brief Sparse supernodal LU factorization for general matrices
+ *
+ * This class implements the supernodal LU factorization for general matrices.
+ * It uses the main techniques from the sequential SuperLU package
+ * (http://crd-legacy.lbl.gov/~xiaoye/SuperLU/). It handles transparently real
+ * and complex arithmetic with single and double precision, depending on the
+ * scalar type of your input matrix.
+ * The code has been optimized to provide BLAS-3 operations during supernode-panel updates.
+ * It benefits directly from the built-in high-performant Eigen BLAS routines.
+ * Moreover, when the size of a supernode is very small, the BLAS calls are avoided to
+ * enable a better optimization from the compiler. For best performance,
+ * you should compile it with NDEBUG flag to avoid the numerous bounds checking on vectors.
+ *
+ * An important parameter of this class is the ordering method. It is used to reorder the columns
+ * (and eventually the rows) of the matrix to reduce the number of new elements that are created during
+ * numerical factorization. The cheapest method available is COLAMD.
+ * See  \link OrderingMethods_Module the OrderingMethods module \endlink for the list of
+ * built-in and external ordering methods.
+ *
+ * Simple example with key steps
+ * \code
+ * VectorXd x(n), b(n);
+ * SparseMatrix<double> A;
+ * SparseLU<SparseMatrix<double>, COLAMDOrdering<int> >   solver;
+ * // fill A and b;
+ * // Compute the ordering permutation vector from the structural pattern of A
+ * solver.analyzePattern(A);
+ * // Compute the numerical factorization
+ * solver.factorize(A);
+ * //Use the factors to solve the linear system
+ * x = solver.solve(b);
+ * \endcode
+ *
+ * \warning The input matrix A should be in a \b compressed and \b column-major form.
+ * Otherwise an expensive copy will be made. You can call the inexpensive makeCompressed() to get a compressed matrix.
+ *
+ * \note Unlike the initial SuperLU implementation, there is no step to equilibrate the matrix.
+ * For badly scaled matrices, this step can be useful to reduce the pivoting during factorization.
+ * If this is the case for your matrices, you can try the basic scaling method at
+ *  "unsupported/Eigen/src/IterativeSolvers/Scaling.h"
+ *
+ * \tparam MatrixType_ The type of the sparse matrix. It must be a column-major SparseMatrix<>
+ * \tparam OrderingType_ The ordering method to use, either AMD, COLAMD or METIS. Default is COLMAD
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa \ref TutorialSparseSolverConcept
+ * \sa \ref OrderingMethods_Module
+ */
+template <typename MatrixType_, typename OrderingType_>
+class SparseLU : public SparseSolverBase<SparseLU<MatrixType_, OrderingType_>>,
+                 public internal::SparseLUImpl<typename MatrixType_::Scalar, typename MatrixType_::StorageIndex> {
+ protected:
+  typedef SparseSolverBase<SparseLU<MatrixType_, OrderingType_>> APIBase;
+  using APIBase::m_isInitialized;
 
-    enum {
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    
-  public:
+ public:
+  using APIBase::_solve_impl;
 
-    SparseLU():m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1)
-    {
-      initperfvalues(); 
-    }
-    explicit SparseLU(const MatrixType& matrix)
-      : m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1)
-    {
-      initperfvalues(); 
-      compute(matrix);
-    }
-    
-    ~SparseLU()
-    {
-      // Free all explicit dynamic pointers 
-    }
-    
-    void analyzePattern (const MatrixType& matrix);
-    void factorize (const MatrixType& matrix);
-    void simplicialfactorize(const MatrixType& matrix);
-    
-    /**
-      * Compute the symbolic and numeric factorization of the input sparse matrix.
-      * The input matrix should be in column-major storage. 
-      */
-    void compute (const MatrixType& matrix)
-    {
-      // Analyze 
-      analyzePattern(matrix); 
-      //Factorize
-      factorize(matrix);
-    } 
+  typedef MatrixType_ MatrixType;
+  typedef OrderingType_ OrderingType;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> NCMatrix;
+  typedef internal::MappedSuperNodalMatrix<Scalar, StorageIndex> SCMatrix;
+  typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
+  typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+  typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+  typedef internal::SparseLUImpl<Scalar, StorageIndex> Base;
 
-    /** \returns an expression of the transposed of the factored matrix.
-      *
-      * A typical usage is to solve for the transposed problem A^T x = b:
-      * \code
-      * solver.compute(A);
-      * x = solver.transpose().solve(b);
-      * \endcode
-      *
-      * \sa adjoint(), solve()
-      */
-    const SparseLUTransposeView<false,SparseLU<_MatrixType,_OrderingType> > transpose()
-    {
-      SparseLUTransposeView<false,  SparseLU<_MatrixType,_OrderingType> > transposeView;
-      transposeView.setSparseLU(this);
-      transposeView.setIsInitialized(this->m_isInitialized);
-      return transposeView;
-    }
+  enum { ColsAtCompileTime = MatrixType::ColsAtCompileTime, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime };
 
+ public:
+  SparseLU()
+      : m_lastError(""), m_Ustore(0, 0, 0, 0, 0, 0), m_symmetricmode(false), m_diagpivotthresh(1.0), m_detPermR(1) {
+    initperfvalues();
+  }
+  explicit SparseLU(const MatrixType& matrix)
+      : m_lastError(""), m_Ustore(0, 0, 0, 0, 0, 0), m_symmetricmode(false), m_diagpivotthresh(1.0), m_detPermR(1) {
+    initperfvalues();
+    compute(matrix);
+  }
 
-    /** \returns an expression of the adjoint of the factored matrix
-      *
-      * A typical usage is to solve for the adjoint problem A' x = b:
-      * \code
-      * solver.compute(A);
-      * x = solver.adjoint().solve(b);
-      * \endcode
-      *
-      * For real scalar types, this function is equivalent to transpose().
-      *
-      * \sa transpose(), solve()
-      */
-    const SparseLUTransposeView<true, SparseLU<_MatrixType,_OrderingType> > adjoint()
-    {
-      SparseLUTransposeView<true,  SparseLU<_MatrixType,_OrderingType> > adjointView;
-      adjointView.setSparseLU(this);
-      adjointView.setIsInitialized(this->m_isInitialized);
-      return adjointView;
-    }
-    
-    inline Index rows() const { return m_mat.rows(); }
-    inline Index cols() const { return m_mat.cols(); }
-    /** Indicate that the pattern of the input matrix is symmetric */
-    void isSymmetric(bool sym)
-    {
-      m_symmetricmode = sym;
-    }
-    
-    /** \returns an expression of the matrix L, internally stored as supernodes
-      * The only operation available with this expression is the triangular solve
-      * \code
-      * y = b; matrixL().solveInPlace(y);
-      * \endcode
-      */
-    SparseLUMatrixLReturnType<SCMatrix> matrixL() const
-    {
-      return SparseLUMatrixLReturnType<SCMatrix>(m_Lstore);
-    }
-    /** \returns an expression of the matrix U,
-      * The only operation available with this expression is the triangular solve
-      * \code
-      * y = b; matrixU().solveInPlace(y);
-      * \endcode
-      */
-    SparseLUMatrixUReturnType<SCMatrix,MappedSparseMatrix<Scalar,ColMajor,StorageIndex> > matrixU() const
-    {
-      return SparseLUMatrixUReturnType<SCMatrix, MappedSparseMatrix<Scalar,ColMajor,StorageIndex> >(m_Lstore, m_Ustore);
-    }
+  ~SparseLU() {
+    // Free all explicit dynamic pointers
+  }
 
-    /**
-      * \returns a reference to the row matrix permutation \f$ P_r \f$ such that \f$P_r A P_c^T = L U\f$
-      * \sa colsPermutation()
-      */
-    inline const PermutationType& rowsPermutation() const
-    {
-      return m_perm_r;
-    }
-    /**
-      * \returns a reference to the column matrix permutation\f$ P_c^T \f$ such that \f$P_r A P_c^T = L U\f$
-      * \sa rowsPermutation()
-      */
-    inline const PermutationType& colsPermutation() const
-    {
-      return m_perm_c;
-    }
-    /** Set the threshold used for a diagonal entry to be an acceptable pivot. */
-    void setPivotThreshold(const RealScalar& thresh)
-    {
-      m_diagpivotthresh = thresh; 
-    }
+  void analyzePattern(const MatrixType& matrix);
+  void factorize(const MatrixType& matrix);
+  void simplicialfactorize(const MatrixType& matrix);
+
+  /**
+   * Compute the symbolic and numeric factorization of the input sparse matrix.
+   * The input matrix should be in column-major storage.
+   */
+  void compute(const MatrixType& matrix) {
+    // Analyze
+    analyzePattern(matrix);
+    // Factorize
+    factorize(matrix);
+  }
+
+  /** \returns an expression of the transposed of the factored matrix.
+   *
+   * A typical usage is to solve for the transposed problem A^T x = b:
+   * \code
+   * solver.compute(A);
+   * x = solver.transpose().solve(b);
+   * \endcode
+   *
+   * \sa adjoint(), solve()
+   */
+  const SparseLUTransposeView<false, SparseLU<MatrixType_, OrderingType_>> transpose() {
+    SparseLUTransposeView<false, SparseLU<MatrixType_, OrderingType_>> transposeView;
+    transposeView.setSparseLU(this);
+    transposeView.setIsInitialized(this->m_isInitialized);
+    return transposeView;
+  }
+
+  /** \returns an expression of the adjoint of the factored matrix
+   *
+   * A typical usage is to solve for the adjoint problem A' x = b:
+   * \code
+   * solver.compute(A);
+   * x = solver.adjoint().solve(b);
+   * \endcode
+   *
+   * For real scalar types, this function is equivalent to transpose().
+   *
+   * \sa transpose(), solve()
+   */
+  const SparseLUTransposeView<true, SparseLU<MatrixType_, OrderingType_>> adjoint() {
+    SparseLUTransposeView<true, SparseLU<MatrixType_, OrderingType_>> adjointView;
+    adjointView.setSparseLU(this);
+    adjointView.setIsInitialized(this->m_isInitialized);
+    return adjointView;
+  }
+
+  inline Index rows() const { return m_mat.rows(); }
+  inline Index cols() const { return m_mat.cols(); }
+  /** Indicate that the pattern of the input matrix is symmetric */
+  void isSymmetric(bool sym) { m_symmetricmode = sym; }
+
+  /** \returns an expression of the matrix L, internally stored as supernodes
+   * The only operation available with this expression is the triangular solve
+   * \code
+   * y = b; matrixL().solveInPlace(y);
+   * \endcode
+   */
+  SparseLUMatrixLReturnType<SCMatrix> matrixL() const { return SparseLUMatrixLReturnType<SCMatrix>(m_Lstore); }
+  /** \returns an expression of the matrix U,
+   * The only operation available with this expression is the triangular solve
+   * \code
+   * y = b; matrixU().solveInPlace(y);
+   * \endcode
+   */
+  SparseLUMatrixUReturnType<SCMatrix, Map<SparseMatrix<Scalar, ColMajor, StorageIndex>>> matrixU() const {
+    return SparseLUMatrixUReturnType<SCMatrix, Map<SparseMatrix<Scalar, ColMajor, StorageIndex>>>(m_Lstore, m_Ustore);
+  }
+
+  /**
+   * \returns a reference to the row matrix permutation \f$ P_r \f$ such that \f$P_r A P_c^T = L U\f$
+   * \sa colsPermutation()
+   */
+  inline const PermutationType& rowsPermutation() const { return m_perm_r; }
+  /**
+   * \returns a reference to the column matrix permutation\f$ P_c^T \f$ such that \f$P_r A P_c^T = L U\f$
+   * \sa rowsPermutation()
+   */
+  inline const PermutationType& colsPermutation() const { return m_perm_c; }
+  /** Set the threshold used for a diagonal entry to be an acceptable pivot. */
+  void setPivotThreshold(const RealScalar& thresh) { m_diagpivotthresh = thresh; }
 
 #ifdef EIGEN_PARSED_BY_DOXYGEN
-    /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
-      *
-      * \warning the destination matrix X in X = this->solve(B) must be colmun-major.
-      *
-      * \sa compute()
-      */
-    template<typename Rhs>
-    inline const Solve<SparseLU, Rhs> solve(const MatrixBase<Rhs>& B) const;
-#endif // EIGEN_PARSED_BY_DOXYGEN
-    
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful,
-      *          \c NumericalIssue if the LU factorization reports a problem, zero diagonal for instance
-      *          \c InvalidInput if the input matrix is invalid
-      *
-      * \sa iparm()          
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
-      return m_info;
-    }
-    
-    /**
-      * \returns A string describing the type of error
-      */
-    std::string lastErrorMessage() const
-    {
-      return m_lastError; 
-    }
+  /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
+   *
+   * \warning the destination matrix X in X = this->solve(B) must be colmun-major.
+   *
+   * \sa compute()
+   */
+  template <typename Rhs>
+  inline const Solve<SparseLU, Rhs> solve(const MatrixBase<Rhs>& B) const;
+#endif  // EIGEN_PARSED_BY_DOXYGEN
 
-    template<typename Rhs, typename Dest>
-    bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &X_base) const
-    {
-      Dest& X(X_base.derived());
-      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first");
-      EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,
-                        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
-      
-      // Permute the right hand side to form X = Pr*B
-      // on return, X is overwritten by the computed solution
-      X.resize(B.rows(),B.cols());
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful,
+   *          \c NumericalIssue if the LU factorization reports a problem, zero diagonal for instance
+   *          \c InvalidInput if the input matrix is invalid
+   *
+   * \sa iparm()
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+    return m_info;
+  }
 
-      // this ugly const_cast_derived() helps to detect aliasing when applying the permutations
-      for(Index j = 0; j < B.cols(); ++j)
-        X.col(j) = rowsPermutation() * B.const_cast_derived().col(j);
-      
-      //Forward substitution with L
-      this->matrixL().solveInPlace(X);
-      this->matrixU().solveInPlace(X);
-      
-      // Permute back the solution 
-      for (Index j = 0; j < B.cols(); ++j)
-        X.col(j) = colsPermutation().inverse() * X.col(j);
-      
-      return true; 
-    }
-    
-    /**
-      * \returns the absolute value of the determinant of the matrix of which
-      * *this is the QR decomposition.
-      *
-      * \warning a determinant can be very big or small, so for matrices
-      * of large enough dimension, there is a risk of overflow/underflow.
-      * One way to work around that is to use logAbsDeterminant() instead.
-      *
-      * \sa logAbsDeterminant(), signDeterminant()
-      */
-    Scalar absDeterminant()
-    {
-      using std::abs;
-      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
-      // Initialize with the determinant of the row matrix
-      Scalar det = Scalar(1.);
-      // Note that the diagonal blocks of U are stored in supernodes,
-      // which are available in the  L part :)
-      for (Index j = 0; j < this->cols(); ++j)
-      {
-        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
-        {
-          if(it.index() == j)
-          {
-            det *= abs(it.value());
-            break;
-          }
+  /**
+   * \returns A string describing the type of error
+   */
+  std::string lastErrorMessage() const { return m_lastError; }
+
+  template <typename Rhs, typename Dest>
+  bool _solve_impl(const MatrixBase<Rhs>& B, MatrixBase<Dest>& X_base) const {
+    Dest& X(X_base.derived());
+    eigen_assert(m_factorizationIsOk && "The matrix should be factorized first");
+    EIGEN_STATIC_ASSERT((Dest::Flags & RowMajorBit) == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+
+    // Permute the right hand side to form X = Pr*B
+    // on return, X is overwritten by the computed solution
+    X.resize(B.rows(), B.cols());
+
+    // this ugly const_cast_derived() helps to detect aliasing when applying the permutations
+    for (Index j = 0; j < B.cols(); ++j) X.col(j) = rowsPermutation() * B.const_cast_derived().col(j);
+
+    // Forward substitution with L
+    this->matrixL().solveInPlace(X);
+    this->matrixU().solveInPlace(X);
+
+    // Permute back the solution
+    for (Index j = 0; j < B.cols(); ++j) X.col(j) = colsPermutation().inverse() * X.col(j);
+
+    return true;
+  }
+
+  /**
+   * \returns the absolute value of the determinant of the matrix of which
+   * *this is the QR decomposition.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa logAbsDeterminant(), signDeterminant()
+   */
+  Scalar absDeterminant() {
+    using std::abs;
+    eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+    // Initialize with the determinant of the row matrix
+    Scalar det = Scalar(1.);
+    // Note that the diagonal blocks of U are stored in supernodes,
+    // which are available in the  L part :)
+    for (Index j = 0; j < this->cols(); ++j) {
+      for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) {
+        if (it.index() == j) {
+          det *= abs(it.value());
+          break;
         }
       }
-      return det;
     }
+    return det;
+  }
 
-    /** \returns the natural log of the absolute value of the determinant of the matrix
-      * of which **this is the QR decomposition
-      *
-      * \note This method is useful to work around the risk of overflow/underflow that's
-      * inherent to the determinant computation.
-      *
-      * \sa absDeterminant(), signDeterminant()
-      */
-    Scalar logAbsDeterminant() const
-    {
-      using std::log;
-      using std::abs;
+  /** \returns the natural log of the absolute value of the determinant of the matrix
+   * of which **this is the QR decomposition
+   *
+   * \note This method is useful to work around the risk of overflow/underflow that's
+   * inherent to the determinant computation.
+   *
+   * \sa absDeterminant(), signDeterminant()
+   */
+  Scalar logAbsDeterminant() const {
+    using std::abs;
+    using std::log;
 
-      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
-      Scalar det = Scalar(0.);
-      for (Index j = 0; j < this->cols(); ++j)
-      {
-        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
-        {
-          if(it.row() < j) continue;
-          if(it.row() == j)
-          {
-            det += log(abs(it.value()));
-            break;
-          }
+    eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+    Scalar det = Scalar(0.);
+    for (Index j = 0; j < this->cols(); ++j) {
+      for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) {
+        if (it.row() < j) continue;
+        if (it.row() == j) {
+          det += log(abs(it.value()));
+          break;
         }
       }
-      return det;
     }
+    return det;
+  }
 
-    /** \returns A number representing the sign of the determinant
-      *
-      * \sa absDeterminant(), logAbsDeterminant()
-      */
-    Scalar signDeterminant()
-    {
-      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
-      // Initialize with the determinant of the row matrix
-      Index det = 1;
-      // Note that the diagonal blocks of U are stored in supernodes,
-      // which are available in the  L part :)
-      for (Index j = 0; j < this->cols(); ++j)
-      {
-        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
-        {
-          if(it.index() == j)
-          {
-            if(it.value()<0)
-              det = -det;
-            else if(it.value()==0)
-              return 0;
-            break;
-          }
+  /** \returns A number representing the sign of the determinant
+   *
+   * \sa absDeterminant(), logAbsDeterminant()
+   */
+  Scalar signDeterminant() {
+    eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+    // Initialize with the determinant of the row matrix
+    Index det = 1;
+    // Note that the diagonal blocks of U are stored in supernodes,
+    // which are available in the  L part :)
+    for (Index j = 0; j < this->cols(); ++j) {
+      for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) {
+        if (it.index() == j) {
+          if (it.value() < 0)
+            det = -det;
+          else if (it.value() == 0)
+            return 0;
+          break;
         }
       }
-      return det * m_detPermR * m_detPermC;
     }
-    
-    /** \returns The determinant of the matrix.
-      *
-      * \sa absDeterminant(), logAbsDeterminant()
-      */
-    Scalar determinant()
-    {
-      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
-      // Initialize with the determinant of the row matrix
-      Scalar det = Scalar(1.);
-      // Note that the diagonal blocks of U are stored in supernodes,
-      // which are available in the  L part :)
-      for (Index j = 0; j < this->cols(); ++j)
-      {
-        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
-        {
-          if(it.index() == j)
-          {
-            det *= it.value();
-            break;
-          }
+    return det * m_detPermR * m_detPermC;
+  }
+
+  /** \returns The determinant of the matrix.
+   *
+   * \sa absDeterminant(), logAbsDeterminant()
+   */
+  Scalar determinant() {
+    eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+    // Initialize with the determinant of the row matrix
+    Scalar det = Scalar(1.);
+    // Note that the diagonal blocks of U are stored in supernodes,
+    // which are available in the  L part :)
+    for (Index j = 0; j < this->cols(); ++j) {
+      for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) {
+        if (it.index() == j) {
+          det *= it.value();
+          break;
         }
       }
-      return (m_detPermR * m_detPermC) > 0 ? det : -det;
     }
+    return (m_detPermR * m_detPermC) > 0 ? det : -det;
+  }
 
-    Index nnzL() const { return m_nnzL; };
-    Index nnzU() const { return m_nnzU; };
+  Index nnzL() const { return m_nnzL; }
+  Index nnzU() const { return m_nnzU; }
 
-  protected:
-    // Functions 
-    void initperfvalues()
-    {
-      m_perfv.panel_size = 16;
-      m_perfv.relax = 1; 
-      m_perfv.maxsuper = 128; 
-      m_perfv.rowblk = 16; 
-      m_perfv.colblk = 8; 
-      m_perfv.fillfactor = 20;  
-    }
-      
-    // Variables 
-    mutable ComputationInfo m_info;
-    bool m_factorizationIsOk;
-    bool m_analysisIsOk;
-    std::string m_lastError;
-    NCMatrix m_mat; // The input (permuted ) matrix 
-    SCMatrix m_Lstore; // The lower triangular matrix (supernodal)
-    MappedSparseMatrix<Scalar,ColMajor,StorageIndex> m_Ustore; // The upper triangular matrix
-    PermutationType m_perm_c; // Column permutation 
-    PermutationType m_perm_r ; // Row permutation
-    IndexVector m_etree; // Column elimination tree 
-    
-    typename Base::GlobalLU_t m_glu; 
-                               
-    // SparseLU options 
-    bool m_symmetricmode;
-    // values for performance 
-    internal::perfvalues m_perfv;
-    RealScalar m_diagpivotthresh; // Specifies the threshold used for a diagonal entry to be an acceptable pivot
-    Index m_nnzL, m_nnzU; // Nonzeros in L and U factors
-    Index m_detPermR, m_detPermC; // Determinants of the permutation matrices
-  private:
-    // Disable copy constructor 
-    SparseLU (const SparseLU& );
-}; // End class SparseLU
+ protected:
+  // Functions
+  void initperfvalues() {
+    m_perfv.panel_size = 16;
+    m_perfv.relax = 1;
+    m_perfv.maxsuper = 128;
+    m_perfv.rowblk = 16;
+    m_perfv.colblk = 8;
+    m_perfv.fillfactor = 20;
+  }
 
+  // Variables
+  mutable ComputationInfo m_info;
+  bool m_factorizationIsOk;
+  bool m_analysisIsOk;
+  std::string m_lastError;
+  NCMatrix m_mat;                                              // The input (permuted ) matrix
+  SCMatrix m_Lstore;                                           // The lower triangular matrix (supernodal)
+  Map<SparseMatrix<Scalar, ColMajor, StorageIndex>> m_Ustore;  // The upper triangular matrix
+  PermutationType m_perm_c;                                    // Column permutation
+  PermutationType m_perm_r;                                    // Row permutation
+  IndexVector m_etree;                                         // Column elimination tree
 
+  typename Base::GlobalLU_t m_glu;
+
+  // SparseLU options
+  bool m_symmetricmode;
+  // values for performance
+  internal::perfvalues m_perfv;
+  RealScalar m_diagpivotthresh;  // Specifies the threshold used for a diagonal entry to be an acceptable pivot
+  Index m_nnzL, m_nnzU;          // Nonzeros in L and U factors
+  Index m_detPermR, m_detPermC;  // Determinants of the permutation matrices
+ private:
+  // Disable copy constructor
+  SparseLU(const SparseLU&);
+};  // End class SparseLU
 
 // Functions needed by the anaysis phase
-/** 
-  * Compute the column permutation to minimize the fill-in
-  * 
-  *  - Apply this permutation to the input matrix - 
-  * 
-  *  - Compute the column elimination tree on the permuted matrix 
-  * 
-  *  - Postorder the elimination tree and the column permutation
-  * 
-  */
+/**
+ * Compute the column permutation to minimize the fill-in
+ *
+ *  - Apply this permutation to the input matrix -
+ *
+ *  - Compute the column elimination tree on the permuted matrix
+ *
+ *  - Postorder the elimination tree and the column permutation
+ *
+ */
 template <typename MatrixType, typename OrderingType>
-void SparseLU<MatrixType, OrderingType>::analyzePattern(const MatrixType& mat)
-{
-  
-  //TODO  It is possible as in SuperLU to compute row and columns scaling vectors to equilibrate the matrix mat.
-  
-  // Firstly, copy the whole input matrix. 
+void SparseLU<MatrixType, OrderingType>::analyzePattern(const MatrixType& mat) {
+  // TODO  It is possible as in SuperLU to compute row and columns scaling vectors to equilibrate the matrix mat.
+
+  // Firstly, copy the whole input matrix.
   m_mat = mat;
-  
+
   // Compute fill-in ordering
-  OrderingType ord; 
-  ord(m_mat,m_perm_c);
-  
+  OrderingType ord;
+  ord(m_mat, m_perm_c);
+
   // Apply the permutation to the column of the input  matrix
-  if (m_perm_c.size())
-  {
-    m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers. FIXME : This vector is filled but not subsequently used.  
+  if (m_perm_c.size()) {
+    m_mat.uncompress();  // NOTE: The effect of this command is only to create the InnerNonzeros pointers. FIXME : This
+                         // vector is filled but not subsequently used.
     // Then, permute only the column pointers
-    ei_declare_aligned_stack_constructed_variable(StorageIndex,outerIndexPtr,mat.cols()+1,mat.isCompressed()?const_cast<StorageIndex*>(mat.outerIndexPtr()):0);
-    
-    // If the input matrix 'mat' is uncompressed, then the outer-indices do not match the ones of m_mat, and a copy is thus needed.
-    if(!mat.isCompressed()) 
-      IndexVector::Map(outerIndexPtr, mat.cols()+1) = IndexVector::Map(m_mat.outerIndexPtr(),mat.cols()+1);
-    
+    ei_declare_aligned_stack_constructed_variable(
+        StorageIndex, outerIndexPtr, mat.cols() + 1,
+        mat.isCompressed() ? const_cast<StorageIndex*>(mat.outerIndexPtr()) : 0);
+
+    // If the input matrix 'mat' is uncompressed, then the outer-indices do not match the ones of m_mat, and a copy is
+    // thus needed.
+    if (!mat.isCompressed())
+      IndexVector::Map(outerIndexPtr, mat.cols() + 1) = IndexVector::Map(m_mat.outerIndexPtr(), mat.cols() + 1);
+
     // Apply the permutation and compute the nnz per column.
-    for (Index i = 0; i < mat.cols(); i++)
-    {
+    for (Index i = 0; i < mat.cols(); i++) {
       m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i];
-      m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i];
+      m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i + 1] - outerIndexPtr[i];
     }
   }
-  
-  // Compute the column elimination tree of the permuted matrix 
+
+  // Compute the column elimination tree of the permuted matrix
   IndexVector firstRowElt;
-  internal::coletree(m_mat, m_etree,firstRowElt); 
-     
+  internal::coletree(m_mat, m_etree, firstRowElt);
+
   // In symmetric mode, do not do postorder here
   if (!m_symmetricmode) {
-    IndexVector post, iwork; 
+    IndexVector post, iwork;
     // Post order etree
-    internal::treePostorder(StorageIndex(m_mat.cols()), m_etree, post); 
-      
-   
-    // Renumber etree in postorder 
-    Index m = m_mat.cols(); 
-    iwork.resize(m+1);
+    internal::treePostorder(StorageIndex(m_mat.cols()), m_etree, post);
+
+    // Renumber etree in postorder
+    Index m = m_mat.cols();
+    iwork.resize(m + 1);
     for (Index i = 0; i < m; ++i) iwork(post(i)) = post(m_etree(i));
     m_etree = iwork;
-    
+
     // Postmultiply A*Pc by post, i.e reorder the matrix according to the postorder of the etree
-    PermutationType post_perm(m); 
-    for (Index i = 0; i < m; i++) 
-      post_perm.indices()(i) = post(i); 
-        
+    PermutationType post_perm(m);
+    for (Index i = 0; i < m; i++) post_perm.indices()(i) = post(i);
+
     // Combine the two permutations : postorder the permutation for future use
-    if(m_perm_c.size()) {
+    if (m_perm_c.size()) {
       m_perm_c = post_perm * m_perm_c;
     }
-    
-  } // end postordering 
-  
-  m_analysisIsOk = true; 
+
+  }  // end postordering
+
+  m_analysisIsOk = true;
 }
 
 // Functions needed by the numerical factorization phase
 
-
-/** 
-  *  - Numerical factorization 
-  *  - Interleaved with the symbolic factorization 
-  * On exit,  info is 
-  * 
-  *    = 0: successful factorization
-  * 
-  *    > 0: if info = i, and i is
-  * 
-  *       <= A->ncol: U(i,i) is exactly zero. The factorization has
-  *          been completed, but the factor U is exactly singular,
-  *          and division by zero will occur if it is used to solve a
-  *          system of equations.
-  * 
-  *       > A->ncol: number of bytes allocated when memory allocation
-  *         failure occurred, plus A->ncol. If lwork = -1, it is
-  *         the estimated amount of space needed, plus A->ncol.  
-  */
+/**
+ *  - Numerical factorization
+ *  - Interleaved with the symbolic factorization
+ * On exit,  info is
+ *
+ *    = 0: successful factorization
+ *
+ *    > 0: if info = i, and i is
+ *
+ *       <= A->ncol: U(i,i) is exactly zero. The factorization has
+ *          been completed, but the factor U is exactly singular,
+ *          and division by zero will occur if it is used to solve a
+ *          system of equations.
+ *
+ *       > A->ncol: number of bytes allocated when memory allocation
+ *         failure occurred, plus A->ncol. If lwork = -1, it is
+ *         the estimated amount of space needed, plus A->ncol.
+ */
 template <typename MatrixType, typename OrderingType>
-void SparseLU<MatrixType, OrderingType>::factorize(const MatrixType& matrix)
-{
+void SparseLU<MatrixType, OrderingType>::factorize(const MatrixType& matrix) {
   using internal::emptyIdxLU;
-  eigen_assert(m_analysisIsOk && "analyzePattern() should be called first"); 
+  eigen_assert(m_analysisIsOk && "analyzePattern() should be called first");
   eigen_assert((matrix.rows() == matrix.cols()) && "Only for squared matrices");
-  
+
   m_isInitialized = true;
-  
+
   // Apply the column permutation computed in analyzepattern()
-  //   m_mat = matrix * m_perm_c.inverse(); 
+  //   m_mat = matrix * m_perm_c.inverse();
   m_mat = matrix;
-  if (m_perm_c.size()) 
-  {
-    m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers.
-    //Then, permute only the column pointers
-    const StorageIndex * outerIndexPtr;
-    if (matrix.isCompressed()) outerIndexPtr = matrix.outerIndexPtr();
-    else
-    {
-      StorageIndex* outerIndexPtr_t = new StorageIndex[matrix.cols()+1];
-      for(Index i = 0; i <= matrix.cols(); i++) outerIndexPtr_t[i] = m_mat.outerIndexPtr()[i];
+  if (m_perm_c.size()) {
+    m_mat.uncompress();  // NOTE: The effect of this command is only to create the InnerNonzeros pointers.
+    // Then, permute only the column pointers
+    const StorageIndex* outerIndexPtr;
+    if (matrix.isCompressed())
+      outerIndexPtr = matrix.outerIndexPtr();
+    else {
+      StorageIndex* outerIndexPtr_t = new StorageIndex[matrix.cols() + 1];
+      for (Index i = 0; i <= matrix.cols(); i++) outerIndexPtr_t[i] = m_mat.outerIndexPtr()[i];
       outerIndexPtr = outerIndexPtr_t;
     }
-    for (Index i = 0; i < matrix.cols(); i++)
-    {
+    for (Index i = 0; i < matrix.cols(); i++) {
       m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i];
-      m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i];
+      m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i + 1] - outerIndexPtr[i];
     }
-    if(!matrix.isCompressed()) delete[] outerIndexPtr;
-  } 
-  else 
-  { //FIXME This should not be needed if the empty permutation is handled transparently
+    if (!matrix.isCompressed()) delete[] outerIndexPtr;
+  } else {  // FIXME This should not be needed if the empty permutation is handled transparently
     m_perm_c.resize(matrix.cols());
-    for(StorageIndex i = 0; i < matrix.cols(); ++i) m_perm_c.indices()(i) = i;
+    for (StorageIndex i = 0; i < matrix.cols(); ++i) m_perm_c.indices()(i) = i;
   }
-  
+
   Index m = m_mat.rows();
   Index n = m_mat.cols();
   Index nnz = m_mat.nonZeros();
   Index maxpanel = m_perfv.panel_size * m;
   // Allocate working storage common to the factor routines
   Index lwork = 0;
-  Index info = Base::memInit(m, n, nnz, lwork, m_perfv.fillfactor, m_perfv.panel_size, m_glu); 
-  if (info) 
-  {
-    m_lastError = "UNABLE TO ALLOCATE WORKING MEMORY\n\n" ;
+  Index info = Base::memInit(m, n, nnz, lwork, m_perfv.fillfactor, m_perfv.panel_size, m_glu);
+  if (info) {
+    m_lastError = "UNABLE TO ALLOCATE WORKING MEMORY\n\n";
     m_factorizationIsOk = false;
-    return ; 
+    return;
   }
-  
-  // Set up pointers for integer working arrays 
-  IndexVector segrep(m); segrep.setZero();
-  IndexVector parent(m); parent.setZero();
-  IndexVector xplore(m); xplore.setZero();
+
+  // Set up pointers for integer working arrays
+  IndexVector segrep(m);
+  segrep.setZero();
+  IndexVector parent(m);
+  parent.setZero();
+  IndexVector xplore(m);
+  xplore.setZero();
   IndexVector repfnz(maxpanel);
   IndexVector panel_lsub(maxpanel);
-  IndexVector xprune(n); xprune.setZero();
-  IndexVector marker(m*internal::LUNoMarker); marker.setZero();
-  
-  repfnz.setConstant(-1); 
+  IndexVector xprune(n);
+  xprune.setZero();
+  IndexVector marker(m * internal::LUNoMarker);
+  marker.setZero();
+
+  repfnz.setConstant(-1);
   panel_lsub.setConstant(-1);
-  
-  // Set up pointers for scalar working arrays 
-  ScalarVector dense; 
+
+  // Set up pointers for scalar working arrays
+  ScalarVector dense;
   dense.setZero(maxpanel);
-  ScalarVector tempv; 
-  tempv.setZero(internal::LUnumTempV(m, m_perfv.panel_size, m_perfv.maxsuper, /*m_perfv.rowblk*/m) );
-  
+  ScalarVector tempv;
+  tempv.setZero(internal::LUnumTempV(m, m_perfv.panel_size, m_perfv.maxsuper, /*m_perfv.rowblk*/ m));
+
   // Compute the inverse of perm_c
-  PermutationType iperm_c(m_perm_c.inverse()); 
-  
+  PermutationType iperm_c(m_perm_c.inverse());
+
   // Identify initial relaxed snodes
   IndexVector relax_end(n);
-  if ( m_symmetricmode == true ) 
+  if (m_symmetricmode == true)
     Base::heap_relax_snode(n, m_etree, m_perfv.relax, marker, relax_end);
   else
     Base::relax_snode(n, m_etree, m_perfv.relax, marker, relax_end);
-  
-  
-  m_perm_r.resize(m); 
+
+  m_perm_r.resize(m);
   m_perm_r.indices().setConstant(-1);
   marker.setConstant(-1);
-  m_detPermR = 1; // Record the determinant of the row permutation
-  
-  m_glu.supno(0) = emptyIdxLU; m_glu.xsup.setConstant(0);
+  m_detPermR = 1;  // Record the determinant of the row permutation
+
+  m_glu.supno(0) = emptyIdxLU;
+  m_glu.xsup.setConstant(0);
   m_glu.xsup(0) = m_glu.xlsub(0) = m_glu.xusub(0) = m_glu.xlusup(0) = Index(0);
-  
+
   // Work on one 'panel' at a time. A panel is one of the following :
   //  (a) a relaxed supernode at the bottom of the etree, or
   //  (b) panel_size contiguous columns, <panel_size> defined by the user
-  Index jcol; 
-  Index pivrow; // Pivotal row number in the original row matrix
-  Index nseg1; // Number of segments in U-column above panel row jcol
-  Index nseg; // Number of segments in each U-column 
-  Index irep; 
-  Index i, k, jj; 
-  for (jcol = 0; jcol < n; )
-  {
-    // Adjust panel size so that a panel won't overlap with the next relaxed snode. 
-    Index panel_size = m_perfv.panel_size; // upper bound on panel width
-    for (k = jcol + 1; k < (std::min)(jcol+panel_size, n); k++)
-    {
-      if (relax_end(k) != emptyIdxLU) 
-      {
-        panel_size = k - jcol; 
-        break; 
+  Index jcol;
+  Index pivrow;  // Pivotal row number in the original row matrix
+  Index nseg1;   // Number of segments in U-column above panel row jcol
+  Index nseg;    // Number of segments in each U-column
+  Index irep;
+  Index i, k, jj;
+  for (jcol = 0; jcol < n;) {
+    // Adjust panel size so that a panel won't overlap with the next relaxed snode.
+    Index panel_size = m_perfv.panel_size;  // upper bound on panel width
+    for (k = jcol + 1; k < (std::min)(jcol + panel_size, n); k++) {
+      if (relax_end(k) != emptyIdxLU) {
+        panel_size = k - jcol;
+        break;
       }
     }
-    if (k == n) 
-      panel_size = n - jcol; 
-      
-    // Symbolic outer factorization on a panel of columns 
-    Base::panel_dfs(m, panel_size, jcol, m_mat, m_perm_r.indices(), nseg1, dense, panel_lsub, segrep, repfnz, xprune, marker, parent, xplore, m_glu); 
-    
-    // Numeric sup-panel updates in topological order 
-    Base::panel_bmod(m, panel_size, jcol, nseg1, dense, tempv, segrep, repfnz, m_glu); 
-    
-    // Sparse LU within the panel, and below the panel diagonal 
-    for ( jj = jcol; jj< jcol + panel_size; jj++) 
-    {
-      k = (jj - jcol) * m; // Column index for w-wide arrays 
-      
-      nseg = nseg1; // begin after all the panel segments
-      //Depth-first-search for the current column
+    if (k == n) panel_size = n - jcol;
+
+    // Symbolic outer factorization on a panel of columns
+    Base::panel_dfs(m, panel_size, jcol, m_mat, m_perm_r.indices(), nseg1, dense, panel_lsub, segrep, repfnz, xprune,
+                    marker, parent, xplore, m_glu);
+
+    // Numeric sup-panel updates in topological order
+    Base::panel_bmod(m, panel_size, jcol, nseg1, dense, tempv, segrep, repfnz, m_glu);
+
+    // Sparse LU within the panel, and below the panel diagonal
+    for (jj = jcol; jj < jcol + panel_size; jj++) {
+      k = (jj - jcol) * m;  // Column index for w-wide arrays
+
+      nseg = nseg1;  // begin after all the panel segments
+      // Depth-first-search for the current column
       VectorBlock<IndexVector> panel_lsubk(panel_lsub, k, m);
-      VectorBlock<IndexVector> repfnz_k(repfnz, k, m); 
-      info = Base::column_dfs(m, jj, m_perm_r.indices(), m_perfv.maxsuper, nseg, panel_lsubk, segrep, repfnz_k, xprune, marker, parent, xplore, m_glu); 
-      if ( info ) 
-      {
-        m_lastError =  "UNABLE TO EXPAND MEMORY IN COLUMN_DFS() ";
-        m_info = NumericalIssue; 
-        m_factorizationIsOk = false; 
-        return; 
+      VectorBlock<IndexVector> repfnz_k(repfnz, k, m);
+      info = Base::column_dfs(m, jj, m_perm_r.indices(), m_perfv.maxsuper, nseg, panel_lsubk, segrep, repfnz_k, xprune,
+                              marker, parent, xplore, m_glu);
+      if (info) {
+        m_lastError = "UNABLE TO EXPAND MEMORY IN COLUMN_DFS() ";
+        m_info = NumericalIssue;
+        m_factorizationIsOk = false;
+        return;
       }
-      // Numeric updates to this column 
-      VectorBlock<ScalarVector> dense_k(dense, k, m); 
-      VectorBlock<IndexVector> segrep_k(segrep, nseg1, m-nseg1); 
-      info = Base::column_bmod(jj, (nseg - nseg1), dense_k, tempv, segrep_k, repfnz_k, jcol, m_glu); 
-      if ( info ) 
-      {
+      // Numeric updates to this column
+      VectorBlock<ScalarVector> dense_k(dense, k, m);
+      VectorBlock<IndexVector> segrep_k(segrep, nseg1, m - nseg1);
+      info = Base::column_bmod(jj, (nseg - nseg1), dense_k, tempv, segrep_k, repfnz_k, jcol, m_glu);
+      if (info) {
         m_lastError = "UNABLE TO EXPAND MEMORY IN COLUMN_BMOD() ";
-        m_info = NumericalIssue; 
-        m_factorizationIsOk = false; 
-        return; 
+        m_info = NumericalIssue;
+        m_factorizationIsOk = false;
+        return;
       }
-      
+
       // Copy the U-segments to ucol(*)
-      info = Base::copy_to_ucol(jj, nseg, segrep, repfnz_k ,m_perm_r.indices(), dense_k, m_glu); 
-      if ( info ) 
-      {
+      info = Base::copy_to_ucol(jj, nseg, segrep, repfnz_k, m_perm_r.indices(), dense_k, m_glu);
+      if (info) {
         m_lastError = "UNABLE TO EXPAND MEMORY IN COPY_TO_UCOL() ";
-        m_info = NumericalIssue; 
-        m_factorizationIsOk = false; 
-        return; 
+        m_info = NumericalIssue;
+        m_factorizationIsOk = false;
+        return;
       }
-      
-      // Form the L-segment 
+
+      // Form the L-segment
       info = Base::pivotL(jj, m_diagpivotthresh, m_perm_r.indices(), iperm_c.indices(), pivrow, m_glu);
-      if ( info ) 
-      {
-        m_lastError = "THE MATRIX IS STRUCTURALLY SINGULAR ... ZERO COLUMN AT ";
+      if (info) {
+        m_lastError = "THE MATRIX IS STRUCTURALLY SINGULAR";
+#ifndef EIGEN_NO_IO
         std::ostringstream returnInfo;
-        returnInfo << info; 
+        returnInfo << " ... ZERO COLUMN AT ";
+        returnInfo << info;
         m_lastError += returnInfo.str();
-        m_info = NumericalIssue; 
-        m_factorizationIsOk = false; 
-        return; 
+#endif
+        m_info = NumericalIssue;
+        m_factorizationIsOk = false;
+        return;
       }
-      
+
       // Update the determinant of the row permutation matrix
-      // FIXME: the following test is not correct, we should probably take iperm_c into account and pivrow is not directly the row pivot.
+      // FIXME: the following test is not correct, we should probably take iperm_c into account and pivrow is not
+      // directly the row pivot.
       if (pivrow != jj) m_detPermR = -m_detPermR;
 
       // Prune columns (0:jj-1) using column jj
-      Base::pruneL(jj, m_perm_r.indices(), pivrow, nseg, segrep, repfnz_k, xprune, m_glu); 
-      
-      // Reset repfnz for this column 
-      for (i = 0; i < nseg; i++)
-      {
-        irep = segrep(i); 
-        repfnz_k(irep) = emptyIdxLU; 
+      Base::pruneL(jj, m_perm_r.indices(), pivrow, nseg, segrep, repfnz_k, xprune, m_glu);
+
+      // Reset repfnz for this column
+      for (i = 0; i < nseg; i++) {
+        irep = segrep(i);
+        repfnz_k(irep) = emptyIdxLU;
       }
-    } // end SparseLU within the panel  
+    }                    // end SparseLU within the panel
     jcol += panel_size;  // Move to the next panel
-  } // end for -- end elimination 
-  
+  }                      // end for -- end elimination
+
   m_detPermR = m_perm_r.determinant();
   m_detPermC = m_perm_c.determinant();
-  
-  // Count the number of nonzeros in factors 
-  Base::countnz(n, m_nnzL, m_nnzU, m_glu); 
-  // Apply permutation  to the L subscripts 
+
+  // Count the number of nonzeros in factors
+  Base::countnz(n, m_nnzL, m_nnzU, m_glu);
+  // Apply permutation  to the L subscripts
   Base::fixupL(n, m_perm_r.indices(), m_glu);
-  
-  // Create supernode matrix L 
-  m_Lstore.setInfos(m, n, m_glu.lusup, m_glu.xlusup, m_glu.lsub, m_glu.xlsub, m_glu.supno, m_glu.xsup); 
-  // Create the column major upper sparse matrix  U; 
-  new (&m_Ustore) MappedSparseMatrix<Scalar, ColMajor, StorageIndex> ( m, n, m_nnzU, m_glu.xusub.data(), m_glu.usub.data(), m_glu.ucol.data() );
-  
+
+  // Create supernode matrix L
+  m_Lstore.setInfos(m, n, m_glu.lusup, m_glu.xlusup, m_glu.lsub, m_glu.xlsub, m_glu.supno, m_glu.xsup);
+  // Create the column major upper sparse matrix  U;
+  new (&m_Ustore) Map<SparseMatrix<Scalar, ColMajor, StorageIndex>>(m, n, m_nnzU, m_glu.xusub.data(), m_glu.usub.data(),
+                                                                    m_glu.ucol.data());
+
   m_info = Success;
   m_factorizationIsOk = true;
 }
 
-template<typename MappedSupernodalType>
-struct SparseLUMatrixLReturnType : internal::no_assignment_operator
-{
+template <typename MappedSupernodalType>
+struct SparseLUMatrixLReturnType : internal::no_assignment_operator {
   typedef typename MappedSupernodalType::Scalar Scalar;
-  explicit SparseLUMatrixLReturnType(const MappedSupernodalType& mapL) : m_mapL(mapL)
-  { }
+  explicit SparseLUMatrixLReturnType(const MappedSupernodalType& mapL) : m_mapL(mapL) {}
   Index rows() const { return m_mapL.rows(); }
   Index cols() const { return m_mapL.cols(); }
-  template<typename Dest>
-  void solveInPlace( MatrixBase<Dest> &X) const
-  {
+  template <typename Dest>
+  void solveInPlace(MatrixBase<Dest>& X) const {
     m_mapL.solveInPlace(X);
   }
-  template<bool Conjugate, typename Dest>
-  void solveTransposedInPlace( MatrixBase<Dest> &X) const
-  {
+  template <bool Conjugate, typename Dest>
+  void solveTransposedInPlace(MatrixBase<Dest>& X) const {
     m_mapL.template solveTransposedInPlace<Conjugate>(X);
   }
 
+  SparseMatrix<Scalar, ColMajor, Index> toSparse() const {
+    ArrayXi colCount = ArrayXi::Ones(cols());
+    for (Index i = 0; i < cols(); i++) {
+      typename MappedSupernodalType::InnerIterator iter(m_mapL, i);
+      for (; iter; ++iter) {
+        if (iter.row() > iter.col()) {
+          colCount(iter.col())++;
+        }
+      }
+    }
+    SparseMatrix<Scalar, ColMajor, Index> sL(rows(), cols());
+    sL.reserve(colCount);
+    for (Index i = 0; i < cols(); i++) {
+      sL.insert(i, i) = 1.0;
+      typename MappedSupernodalType::InnerIterator iter(m_mapL, i);
+      for (; iter; ++iter) {
+        if (iter.row() > iter.col()) {
+          sL.insert(iter.row(), iter.col()) = iter.value();
+        }
+      }
+    }
+    sL.makeCompressed();
+    return sL;
+  }
+
   const MappedSupernodalType& m_mapL;
 };
 
-template<typename MatrixLType, typename MatrixUType>
-struct SparseLUMatrixUReturnType : internal::no_assignment_operator
-{
+template <typename MatrixLType, typename MatrixUType>
+struct SparseLUMatrixUReturnType : internal::no_assignment_operator {
   typedef typename MatrixLType::Scalar Scalar;
-  SparseLUMatrixUReturnType(const MatrixLType& mapL, const MatrixUType& mapU)
-  : m_mapL(mapL),m_mapU(mapU)
-  { }
+  SparseLUMatrixUReturnType(const MatrixLType& mapL, const MatrixUType& mapU) : m_mapL(mapL), m_mapU(mapU) {}
   Index rows() const { return m_mapL.rows(); }
   Index cols() const { return m_mapL.cols(); }
 
-  template<typename Dest>   void solveInPlace(MatrixBase<Dest> &X) const
-  {
+  template <typename Dest>
+  void solveInPlace(MatrixBase<Dest>& X) const {
     Index nrhs = X.cols();
-    Index n    = X.rows();
     // Backward solve with U
-    for (Index k = m_mapL.nsuper(); k >= 0; k--)
-    {
+    for (Index k = m_mapL.nsuper(); k >= 0; k--) {
       Index fsupc = m_mapL.supToCol()[k];
-      Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension
-      Index nsupc = m_mapL.supToCol()[k+1] - fsupc;
+      Index lda = m_mapL.colIndexPtr()[fsupc + 1] - m_mapL.colIndexPtr()[fsupc];  // leading dimension
+      Index nsupc = m_mapL.supToCol()[k + 1] - fsupc;
       Index luptr = m_mapL.colIndexPtr()[fsupc];
 
-      if (nsupc == 1)
-      {
-        for (Index j = 0; j < nrhs; j++)
-        {
+      if (nsupc == 1) {
+        for (Index j = 0; j < nrhs; j++) {
           X(fsupc, j) /= m_mapL.valuePtr()[luptr];
         }
-      }
-      else
-      {
+      } else {
         // FIXME: the following lines should use Block expressions and not Map!
-        Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
-        Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X.coeffRef(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+        Map<const Matrix<Scalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<>> A(&(m_mapL.valuePtr()[luptr]), nsupc,
+                                                                                  nsupc, OuterStride<>(lda));
+        typename Dest::RowsBlockXpr U = X.derived().middleRows(fsupc, nsupc);
         U = A.template triangularView<Upper>().solve(U);
       }
 
-      for (Index j = 0; j < nrhs; ++j)
-      {
-        for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++)
-        {
+      for (Index j = 0; j < nrhs; ++j) {
+        for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++) {
           typename MatrixUType::InnerIterator it(m_mapU, jcol);
-          for ( ; it; ++it)
-          {
+          for (; it; ++it) {
             Index irow = it.index();
             X(irow, j) -= X(jcol, j) * it.value();
           }
         }
       }
-    } // End For U-solve
+    }  // End For U-solve
   }
 
-  template<bool Conjugate, typename Dest>   void solveTransposedInPlace(MatrixBase<Dest> &X) const
-  {
+  template <bool Conjugate, typename Dest>
+  void solveTransposedInPlace(MatrixBase<Dest>& X) const {
     using numext::conj;
     Index nrhs = X.cols();
-    Index n    = X.rows();
     // Forward solve with U
-    for (Index k = 0; k <=  m_mapL.nsuper(); k++)
-    {
+    for (Index k = 0; k <= m_mapL.nsuper(); k++) {
       Index fsupc = m_mapL.supToCol()[k];
-      Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension
-      Index nsupc = m_mapL.supToCol()[k+1] - fsupc;
+      Index lda = m_mapL.colIndexPtr()[fsupc + 1] - m_mapL.colIndexPtr()[fsupc];  // leading dimension
+      Index nsupc = m_mapL.supToCol()[k + 1] - fsupc;
       Index luptr = m_mapL.colIndexPtr()[fsupc];
 
-      for (Index j = 0; j < nrhs; ++j)
-      {
-        for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++)
-        {
+      for (Index j = 0; j < nrhs; ++j) {
+        for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++) {
           typename MatrixUType::InnerIterator it(m_mapU, jcol);
-          for ( ; it; ++it)
-          {
+          for (; it; ++it) {
             Index irow = it.index();
-            X(jcol, j) -= X(irow, j) * (Conjugate? conj(it.value()): it.value());
+            X(jcol, j) -= X(irow, j) * (Conjugate ? conj(it.value()) : it.value());
           }
         }
       }
-      if (nsupc == 1)
-      {
-        for (Index j = 0; j < nrhs; j++)
-        {
-          X(fsupc, j) /= (Conjugate? conj(m_mapL.valuePtr()[luptr]) : m_mapL.valuePtr()[luptr]);
+      if (nsupc == 1) {
+        for (Index j = 0; j < nrhs; j++) {
+          X(fsupc, j) /= (Conjugate ? conj(m_mapL.valuePtr()[luptr]) : m_mapL.valuePtr()[luptr]);
         }
-      }
-      else
-      {
-        Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
-        Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
-        if(Conjugate)
+      } else {
+        Map<const Matrix<Scalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<>> A(&(m_mapL.valuePtr()[luptr]), nsupc,
+                                                                                  nsupc, OuterStride<>(lda));
+        typename Dest::RowsBlockXpr U = X.derived().middleRows(fsupc, nsupc);
+        if (Conjugate)
           U = A.adjoint().template triangularView<Lower>().solve(U);
         else
           U = A.transpose().template triangularView<Lower>().solve(U);
       }
-    }// End For U-solve
+    }  // End For U-solve
   }
 
+  SparseMatrix<Scalar, RowMajor, Index> toSparse() {
+    ArrayXi rowCount = ArrayXi::Zero(rows());
+    for (Index i = 0; i < cols(); i++) {
+      typename MatrixLType::InnerIterator iter(m_mapL, i);
+      for (; iter; ++iter) {
+        if (iter.row() <= iter.col()) {
+          rowCount(iter.row())++;
+        }
+      }
+    }
+
+    SparseMatrix<Scalar, RowMajor, Index> sU(rows(), cols());
+    sU.reserve(rowCount);
+    for (Index i = 0; i < cols(); i++) {
+      typename MatrixLType::InnerIterator iter(m_mapL, i);
+      for (; iter; ++iter) {
+        if (iter.row() <= iter.col()) {
+          sU.insert(iter.row(), iter.col()) = iter.value();
+        }
+      }
+    }
+    sU.makeCompressed();
+    const SparseMatrix<Scalar, RowMajor, Index> u = m_mapU;  // convert to RowMajor
+    sU += u;
+    return sU;
+  }
 
   const MatrixLType& m_mapL;
   const MatrixUType& m_mapU;
 };
 
-} // End namespace Eigen 
+}  // End namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h
index fc0cfc4..96b9c65 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h
@@ -9,58 +9,71 @@
 #ifndef SPARSELU_IMPL_H
 #define SPARSELU_IMPL_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
-  
-/** \ingroup SparseLU_Module
-  * \class SparseLUImpl
-  * Base class for sparseLU
-  */
-template <typename Scalar, typename StorageIndex>
-class SparseLUImpl
-{
-  public:
-    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
-    typedef Matrix<StorageIndex,Dynamic,1> IndexVector; 
-    typedef Matrix<Scalar,Dynamic,Dynamic,ColMajor> ScalarMatrix;
-    typedef Map<ScalarMatrix, 0,  OuterStride<> > MappedMatrixBlock;
-    typedef typename ScalarVector::RealScalar RealScalar; 
-    typedef Ref<Matrix<Scalar,Dynamic,1> > BlockScalarVector;
-    typedef Ref<Matrix<StorageIndex,Dynamic,1> > BlockIndexVector;
-    typedef LU_GlobalLU_t<IndexVector, ScalarVector> GlobalLU_t; 
-    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> MatrixType; 
-    
-  protected:
-     template <typename VectorType>
-     Index expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions);
-     Index memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size,  GlobalLU_t& glu); 
-     template <typename VectorType>
-     Index memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions);
-     void heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end); 
-     void relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end); 
-     Index snode_dfs(const Index jcol, const Index kcol,const MatrixType& mat,  IndexVector& xprune, IndexVector& marker, GlobalLU_t& glu); 
-     Index snode_bmod (const Index jcol, const Index fsupc, ScalarVector& dense, GlobalLU_t& glu);
-     Index pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu);
-     template <typename Traits>
-     void dfs_kernel(const StorageIndex jj, IndexVector& perm_r,
-                    Index& nseg, IndexVector& panel_lsub, IndexVector& segrep,
-                    Ref<IndexVector> repfnz_col, IndexVector& xprune, Ref<IndexVector> marker, IndexVector& parent,
-                    IndexVector& xplore, GlobalLU_t& glu, Index& nextl_col, Index krow, Traits& traits);
-     void panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
-    
-     void panel_bmod(const Index m, const Index w, const Index jcol, const Index nseg, ScalarVector& dense, ScalarVector& tempv, IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu);
-     Index column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg,  BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
-     Index column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv, BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu); 
-     Index copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu); 
-     void pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg, const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu);
-     void countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu); 
-     void fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu); 
-     
-     template<typename , typename >
-     friend struct column_dfs_traits;
-}; 
 
-} // end namespace internal
-} // namespace Eigen
+/** \ingroup SparseLU_Module
+ * \class SparseLUImpl
+ * Base class for sparseLU
+ */
+template <typename Scalar, typename StorageIndex>
+class SparseLUImpl {
+ public:
+  typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
+  typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+  typedef Matrix<Scalar, Dynamic, Dynamic, ColMajor> ScalarMatrix;
+  typedef Map<ScalarMatrix, 0, OuterStride<> > MappedMatrixBlock;
+  typedef typename ScalarVector::RealScalar RealScalar;
+  typedef Ref<Matrix<Scalar, Dynamic, 1> > BlockScalarVector;
+  typedef Ref<Matrix<StorageIndex, Dynamic, 1> > BlockIndexVector;
+  typedef LU_GlobalLU_t<IndexVector, ScalarVector> GlobalLU_t;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> MatrixType;
+
+ protected:
+  template <typename VectorType>
+  Index expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions);
+  Index memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu);
+  template <typename VectorType>
+  Index memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions);
+  void heap_relax_snode(const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants,
+                        IndexVector& relax_end);
+  void relax_snode(const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants,
+                   IndexVector& relax_end);
+  Index snode_dfs(const Index jcol, const Index kcol, const MatrixType& mat, IndexVector& xprune, IndexVector& marker,
+                  GlobalLU_t& glu);
+  Index snode_bmod(const Index jcol, const Index fsupc, ScalarVector& dense, GlobalLU_t& glu);
+  Index pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c,
+               Index& pivrow, GlobalLU_t& glu);
+  template <typename Traits>
+  void dfs_kernel(const StorageIndex jj, IndexVector& perm_r, Index& nseg, IndexVector& panel_lsub, IndexVector& segrep,
+                  Ref<IndexVector> repfnz_col, IndexVector& xprune, Ref<IndexVector> marker, IndexVector& parent,
+                  IndexVector& xplore, GlobalLU_t& glu, Index& nextl_col, Index krow, Traits& traits);
+  void panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg,
+                 ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz,
+                 IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
+
+  void panel_bmod(const Index m, const Index w, const Index jcol, const Index nseg, ScalarVector& dense,
+                  ScalarVector& tempv, IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu);
+  Index column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg,
+                   BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune,
+                   IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
+  Index column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv,
+                    BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu);
+  Index copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, BlockIndexVector repfnz,
+                     IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu);
+  void pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg,
+              const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu);
+  void countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu);
+  void fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu);
+
+  template <typename, typename>
+  friend struct column_dfs_traits;
+};
+
+}  // end namespace internal
+}  // namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h
index 349bfd5..22affd2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h
@@ -7,10 +7,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
- 
- * NOTE: This file is the modified version of [s,d,c,z]memory.c files in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]memory.c files in SuperLU
+
  * -- SuperLU routine (version 3.1) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -31,196 +31,180 @@
 #ifndef EIGEN_SPARSELU_MEMORY
 #define EIGEN_SPARSELU_MEMORY
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
-  
+
 enum { LUNoMarker = 3 };
-enum {emptyIdxLU = -1};
-inline Index LUnumTempV(Index& m, Index& w, Index& t, Index& b)
-{
-  return (std::max)(m, (t+b)*w);
+enum { emptyIdxLU = -1 };
+inline Index LUnumTempV(Index& m, Index& w, Index& t, Index& b) { return (std::max)(m, (t + b) * w); }
+
+template <typename Scalar>
+inline Index LUTempSpace(Index& m, Index& w) {
+  return (2 * w + 4 + LUNoMarker) * m * sizeof(Index) + (w + 1) * m * sizeof(Scalar);
 }
 
-template< typename Scalar>
-inline Index LUTempSpace(Index&m, Index& w)
-{
-  return (2*w + 4 + LUNoMarker) * m * sizeof(Index) + (w + 1) * m * sizeof(Scalar);
-}
-
-
-
-
-/** 
-  * Expand the existing storage to accommodate more fill-ins
-  * \param vec Valid pointer to the vector to allocate or expand
-  * \param[in,out] length  At input, contain the current length of the vector that is to be increased. At output, length of the newly allocated vector
-  * \param[in] nbElts Current number of elements in the factors
-  * \param keep_prev  1: use length  and do not expand the vector; 0: compute new_len and expand
-  * \param[in,out] num_expansions Number of times the memory has been expanded
-  */
+/**
+ * Expand the existing storage to accommodate more fill-ins
+ * \param vec Valid pointer to the vector to allocate or expand
+ * \param[in,out] length  At input, contain the current length of the vector that is to be increased. At output, length
+ * of the newly allocated vector \param[in] nbElts Current number of elements in the factors \param keep_prev  1: use
+ * length  and do not expand the vector; 0: compute new_len and expand \param[in,out] num_expansions Number of times the
+ * memory has been expanded
+ */
 template <typename Scalar, typename StorageIndex>
 template <typename VectorType>
-Index  SparseLUImpl<Scalar,StorageIndex>::expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions) 
-{
-  
-  float alpha = 1.5; // Ratio of the memory increase 
-  Index new_len; // New size of the allocated memory
-  
-  if(num_expansions == 0 || keep_prev) 
-    new_len = length ; // First time allocate requested
-  else 
-    new_len = (std::max)(length+1,Index(alpha * length));
-  
-  VectorType old_vec; // Temporary vector to hold the previous values   
-  if (nbElts > 0 )
-    old_vec = vec.segment(0,nbElts); 
-  
-  //Allocate or expand the current vector
+Index SparseLUImpl<Scalar, StorageIndex>::expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev,
+                                                 Index& num_expansions) {
+  float alpha = 1.5;  // Ratio of the memory increase
+  Index new_len;      // New size of the allocated memory
+
+  if (num_expansions == 0 || keep_prev)
+    new_len = length;  // First time allocate requested
+  else
+    new_len = (std::max)(length + 1, Index(alpha * length));
+
+  VectorType old_vec;  // Temporary vector to hold the previous values
+  if (nbElts > 0) old_vec = vec.segment(0, nbElts);
+
+    // Allocate or expand the current vector
 #ifdef EIGEN_EXCEPTIONS
   try
 #endif
   {
-    vec.resize(new_len); 
+    vec.resize(new_len);
   }
 #ifdef EIGEN_EXCEPTIONS
-  catch(std::bad_alloc& )
+  catch (std::bad_alloc&)
 #else
-  if(!vec.size())
+  if (!vec.size())
 #endif
   {
-    if (!num_expansions)
-    {
+    if (!num_expansions) {
       // First time to allocate from LUMemInit()
       // Let LUMemInit() deals with it.
       return -1;
     }
-    if (keep_prev)
-    {
+    if (keep_prev) {
       // In this case, the memory length should not not be reduced
       return new_len;
-    }
-    else 
-    {
-      // Reduce the size and increase again 
-      Index tries = 0; // Number of attempts
-      do 
-      {
-        alpha = (alpha + 1)/2;
-        new_len = (std::max)(length+1,Index(alpha * length));
+    } else {
+      // Reduce the size and increase again
+      Index tries = 0;  // Number of attempts
+      do {
+        alpha = (alpha + 1) / 2;
+        new_len = (std::max)(length + 1, Index(alpha * length));
 #ifdef EIGEN_EXCEPTIONS
         try
 #endif
         {
-          vec.resize(new_len); 
+          vec.resize(new_len);
         }
 #ifdef EIGEN_EXCEPTIONS
-        catch(std::bad_alloc& )
+        catch (std::bad_alloc&)
 #else
         if (!vec.size())
 #endif
         {
-          tries += 1; 
-          if ( tries > 10) return new_len; 
+          tries += 1;
+          if (tries > 10) return new_len;
         }
       } while (!vec.size());
     }
   }
-  //Copy the previous values to the newly allocated space 
-  if (nbElts > 0)
-    vec.segment(0, nbElts) = old_vec;   
-   
-  
-  length  = new_len;
-  if(num_expansions) ++num_expansions;
-  return 0; 
+  // Copy the previous values to the newly allocated space
+  if (nbElts > 0) vec.segment(0, nbElts) = old_vec;
+
+  length = new_len;
+  if (num_expansions) ++num_expansions;
+  return 0;
 }
 
 /**
  * \brief  Allocate various working space for the numerical factorization phase.
- * \param m number of rows of the input matrix 
- * \param n number of columns 
- * \param annz number of initial nonzeros in the matrix 
+ * \param m number of rows of the input matrix
+ * \param n number of columns
+ * \param annz number of initial nonzeros in the matrix
  * \param lwork  if lwork=-1, this routine returns an estimated size of the required memory
  * \param glu persistent data to facilitate multiple factors : will be deleted later ??
  * \param fillratio estimated ratio of fill in the factors
  * \param panel_size Size of a panel
- * \return an estimated size of the required memory if lwork = -1; otherwise, return the size of actually allocated memory when allocation failed, and 0 on success
- * \note Unlike SuperLU, this routine does not support successive factorization with the same pattern and the same row permutation
+ * \return an estimated size of the required memory if lwork = -1; otherwise, return the size of actually allocated
+ * memory when allocation failed, and 0 on success \note Unlike SuperLU, this routine does not support successive
+ * factorization with the same pattern and the same row permutation
  */
 template <typename Scalar, typename StorageIndex>
-Index SparseLUImpl<Scalar,StorageIndex>::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size,  GlobalLU_t& glu)
-{
-  Index& num_expansions = glu.num_expansions; //No memory expansions so far
+Index SparseLUImpl<Scalar, StorageIndex>::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio,
+                                                  Index panel_size, GlobalLU_t& glu) {
+  Index& num_expansions = glu.num_expansions;  // No memory expansions so far
   num_expansions = 0;
-  glu.nzumax = glu.nzlumax = (std::min)(fillratio * (annz+1) / n, m) * n; // estimated number of nonzeros in U 
-  glu.nzlmax = (std::max)(Index(4), fillratio) * (annz+1) / 4; // estimated  nnz in L factor
+  glu.nzumax = glu.nzlumax = (std::min)(fillratio * (annz + 1) / n, m) * n;  // estimated number of nonzeros in U
+  glu.nzlmax = (std::max)(Index(4), fillratio) * (annz + 1) / 4;             // estimated  nnz in L factor
   // Return the estimated size to the user if necessary
   Index tempSpace;
-  tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar);
-  if (lwork == emptyIdxLU) 
-  {
+  tempSpace = (2 * panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar);
+  if (lwork == emptyIdxLU) {
     Index estimated_size;
-    estimated_size = (5 * n + 5) * sizeof(Index)  + tempSpace
-                    + (glu.nzlmax + glu.nzumax) * sizeof(Index) + (glu.nzlumax+glu.nzumax) *  sizeof(Scalar) + n; 
+    estimated_size = (5 * n + 5) * sizeof(Index) + tempSpace + (glu.nzlmax + glu.nzumax) * sizeof(Index) +
+                     (glu.nzlumax + glu.nzumax) * sizeof(Scalar) + n;
     return estimated_size;
   }
-  
-  // Setup the required space 
-  
+
+  // Setup the required space
+
   // First allocate Integer pointers for L\U factors
-  glu.xsup.resize(n+1);
-  glu.supno.resize(n+1);
-  glu.xlsub.resize(n+1);
-  glu.xlusup.resize(n+1);
-  glu.xusub.resize(n+1);
+  glu.xsup.resize(n + 1);
+  glu.supno.resize(n + 1);
+  glu.xlsub.resize(n + 1);
+  glu.xlusup.resize(n + 1);
+  glu.xusub.resize(n + 1);
 
   // Reserve memory for L/U factors
-  do 
-  {
-    if(     (expand<ScalarVector>(glu.lusup, glu.nzlumax, 0, 0, num_expansions)<0)
-        ||  (expand<ScalarVector>(glu.ucol,  glu.nzumax,  0, 0, num_expansions)<0)
-        ||  (expand<IndexVector> (glu.lsub,  glu.nzlmax,  0, 0, num_expansions)<0)
-        ||  (expand<IndexVector> (glu.usub,  glu.nzumax,  0, 1, num_expansions)<0) )
-    {
-      //Reduce the estimated size and retry
+  do {
+    if ((expand<ScalarVector>(glu.lusup, glu.nzlumax, 0, 0, num_expansions) < 0) ||
+        (expand<ScalarVector>(glu.ucol, glu.nzumax, 0, 0, num_expansions) < 0) ||
+        (expand<IndexVector>(glu.lsub, glu.nzlmax, 0, 0, num_expansions) < 0) ||
+        (expand<IndexVector>(glu.usub, glu.nzumax, 0, 1, num_expansions) < 0)) {
+      // Reduce the estimated size and retry
       glu.nzlumax /= 2;
       glu.nzumax /= 2;
       glu.nzlmax /= 2;
-      if (glu.nzlumax < annz ) return glu.nzlumax; 
+      if (glu.nzlumax < annz) return glu.nzlumax;
     }
   } while (!glu.lusup.size() || !glu.ucol.size() || !glu.lsub.size() || !glu.usub.size());
-  
+
   ++num_expansions;
   return 0;
-  
-} // end LuMemInit
 
-/** 
- * \brief Expand the existing storage 
- * \param vec vector to expand 
+}  // end LuMemInit
+
+/**
+ * \brief Expand the existing storage
+ * \param vec vector to expand
  * \param[in,out] maxlen On input, previous size of vec (Number of elements to copy ). on output, new size
  * \param nbElts current number of elements in the vector.
  * \param memtype Type of the element to expand
- * \param num_expansions Number of expansions 
+ * \param num_expansions Number of expansions
  * \return 0 on success, > 0 size of the memory allocated so far
  */
 template <typename Scalar, typename StorageIndex>
 template <typename VectorType>
-Index SparseLUImpl<Scalar,StorageIndex>::memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions)
-{
-  Index failed_size; 
+Index SparseLUImpl<Scalar, StorageIndex>::memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype,
+                                                   Index& num_expansions) {
+  Index failed_size;
   if (memtype == USUB)
-     failed_size = this->expand<VectorType>(vec, maxlen, nbElts, 1, num_expansions);
+    failed_size = this->expand<VectorType>(vec, maxlen, nbElts, 1, num_expansions);
   else
     failed_size = this->expand<VectorType>(vec, maxlen, nbElts, 0, num_expansions);
 
-  if (failed_size)
-    return failed_size; 
-  
-  return 0 ;  
+  if (failed_size) return failed_size;
+
+  return 0;
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
-#endif // EIGEN_SPARSELU_MEMORY
+}  // end namespace Eigen
+#endif  // EIGEN_SPARSELU_MEMORY
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h
index cf5ec44..2afab01 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h
@@ -7,26 +7,26 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
+/*
  * NOTE: This file comes from a partly modified version of files slu_[s,d,c,z]defs.h
  * -- SuperLU routine (version 4.1) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
  * November, 2010
- * 
+ *
  * Global data structures used in LU factorization -
- * 
+ *
  *   nsuper: #supernodes = nsuper + 1, numbered [0, nsuper].
  *   (xsup,supno): supno[i] is the supernode no to which i belongs;
  *  xsup(s) points to the beginning of the s-th supernode.
  *  e.g.   supno 0 1 2 2 3 3 3 4 4 4 4 4   (n=12)
  *          xsup 0 1 2 4 7 12
- *  Note: dfs will be performed on supernode rep. relative to the new 
+ *  Note: dfs will be performed on supernode rep. relative to the new
  *        row pivoting ordering
  *
  *   (xlsub,lsub): lsub[*] contains the compressed subscript of
  *  rectangular supernodes; xlsub[j] points to the starting
- *  location of the j-th column in lsub[*]. Note that xlsub 
+ *  location of the j-th column in lsub[*]. Note that xlsub
  *  is indexed by column.
  *  Storage: original row subscripts
  *
@@ -68,43 +68,46 @@
 
 #ifndef EIGEN_LU_STRUCTS
 #define EIGEN_LU_STRUCTS
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
-  
-typedef enum {LUSUP, UCOL, LSUB, USUB, LLVL, ULVL} MemType; 
+
+enum MemType { LUSUP, UCOL, LSUB, USUB, LLVL, ULVL };
 
 template <typename IndexVector, typename ScalarVector>
 struct LU_GlobalLU_t {
-  typedef typename IndexVector::Scalar StorageIndex; 
-  IndexVector xsup; //First supernode column ... xsup(s) points to the beginning of the s-th supernode
-  IndexVector supno; // Supernode number corresponding to this column (column to supernode mapping)
-  ScalarVector  lusup; // nonzero values of L ordered by columns 
-  IndexVector lsub; // Compressed row indices of L rectangular supernodes. 
-  IndexVector xlusup; // pointers to the beginning of each column in lusup
-  IndexVector xlsub; // pointers to the beginning of each column in lsub
-  Index   nzlmax; // Current max size of lsub
-  Index   nzlumax; // Current max size of lusup
-  ScalarVector  ucol; // nonzero values of U ordered by columns 
-  IndexVector usub; // row indices of U columns in ucol
-  IndexVector xusub; // Pointers to the beginning of each column of U in ucol 
-  Index   nzumax; // Current max size of ucol
-  Index   n; // Number of columns in the matrix  
-  Index   num_expansions; 
+  typedef typename IndexVector::Scalar StorageIndex;
+  IndexVector xsup;    // First supernode column ... xsup(s) points to the beginning of the s-th supernode
+  IndexVector supno;   // Supernode number corresponding to this column (column to supernode mapping)
+  ScalarVector lusup;  // nonzero values of L ordered by columns
+  IndexVector lsub;    // Compressed row indices of L rectangular supernodes.
+  IndexVector xlusup;  // pointers to the beginning of each column in lusup
+  IndexVector xlsub;   // pointers to the beginning of each column in lsub
+  Index nzlmax;        // Current max size of lsub
+  Index nzlumax;       // Current max size of lusup
+  ScalarVector ucol;   // nonzero values of U ordered by columns
+  IndexVector usub;    // row indices of U columns in ucol
+  IndexVector xusub;   // Pointers to the beginning of each column of U in ucol
+  Index nzumax;        // Current max size of ucol
+  Index n;             // Number of columns in the matrix
+  Index num_expansions;
 };
 
 // Values to set for performance
 struct perfvalues {
-  Index panel_size; // a panel consists of at most <panel_size> consecutive columns
-  Index relax; // To control degree of relaxing supernodes. If the number of nodes (columns) 
-                // in a subtree of the elimination tree is less than relax, this subtree is considered 
-                // as one supernode regardless of the row structures of those columns
-  Index maxsuper; // The maximum size for a supernode in complete LU
-  Index rowblk; // The minimum row dimension for 2-D blocking to be used;
-  Index colblk; // The minimum column dimension for 2-D blocking to be used;
-  Index fillfactor; // The estimated fills factors for L and U, compared with A
-}; 
+  Index panel_size;  // a panel consists of at most <panel_size> consecutive columns
+  Index relax;       // To control degree of relaxing supernodes. If the number of nodes (columns)
+                     // in a subtree of the elimination tree is less than relax, this subtree is considered
+                     // as one supernode regardless of the row structures of those columns
+  Index maxsuper;    // The maximum size for a supernode in complete LU
+  Index rowblk;      // The minimum row dimension for 2-D blocking to be used;
+  Index colblk;      // The minimum column dimension for 2-D blocking to be used;
+  Index fillfactor;  // The estimated fills factors for L and U, compared with A
+};
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
-#endif // EIGEN_LU_STRUCTS
+}  // end namespace Eigen
+#endif  // EIGEN_LU_STRUCTS
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
index 0be293d..eb15909 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
@@ -11,365 +11,309 @@
 #ifndef EIGEN_SPARSELU_SUPERNODAL_MATRIX_H
 #define EIGEN_SPARSELU_SUPERNODAL_MATRIX_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
 /** \ingroup SparseLU_Module
  * \brief a class to manipulate the L supernodal factor from the SparseLU factorization
- * 
- * This class  contain the data to easily store 
- * and manipulate the supernodes during the factorization and solution phase of Sparse LU. 
+ *
+ * This class  contain the data to easily store
+ * and manipulate the supernodes during the factorization and solution phase of Sparse LU.
  * Only the lower triangular matrix has supernodes.
- * 
+ *
  * NOTE : This class corresponds to the SCformat structure in SuperLU
- * 
+ *
  */
 /* TODO
- * InnerIterator as for sparsematrix 
- * SuperInnerIterator to iterate through all supernodes 
+ * InnerIterator as for sparsematrix
+ * SuperInnerIterator to iterate through all supernodes
  * Function for triangular solve
  */
-template <typename _Scalar, typename _StorageIndex>
-class MappedSuperNodalMatrix
-{
-  public:
-    typedef _Scalar Scalar; 
-    typedef _StorageIndex StorageIndex;
-    typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
-    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
-  public:
-    MappedSuperNodalMatrix()
-    {
-      
-    }
-    MappedSuperNodalMatrix(Index m, Index n,  ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
-             IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col )
-    {
-      setInfos(m, n, nzval, nzval_colptr, rowind, rowind_colptr, col_to_sup, sup_to_col);
-    }
-    
-    ~MappedSuperNodalMatrix()
-    {
-      
-    }
-    /**
-     * Set appropriate pointers for the lower triangular supernodal matrix
-     * These infos are available at the end of the numerical factorization
-     * FIXME This class will be modified such that it can be use in the course 
-     * of the factorization.
-     */
-    void setInfos(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
-             IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col )
-    {
-      m_row = m;
-      m_col = n; 
-      m_nzval = nzval.data(); 
-      m_nzval_colptr = nzval_colptr.data(); 
-      m_rowind = rowind.data(); 
-      m_rowind_colptr = rowind_colptr.data(); 
-      m_nsuper = col_to_sup(n); 
-      m_col_to_sup = col_to_sup.data(); 
-      m_sup_to_col = sup_to_col.data(); 
-    }
-    
-    /**
-     * Number of rows
-     */
-    Index rows() const { return m_row; }
-    
-    /**
-     * Number of columns
-     */
-    Index cols() const { return m_col; }
-    
-    /**
-     * Return the array of nonzero values packed by column
-     * 
-     * The size is nnz
-     */
-    Scalar* valuePtr() {  return m_nzval; }
-    
-    const Scalar* valuePtr() const 
-    {
-      return m_nzval; 
-    }
-    /**
-     * Return the pointers to the beginning of each column in \ref valuePtr()
-     */
-    StorageIndex* colIndexPtr()
-    {
-      return m_nzval_colptr; 
-    }
-    
-    const StorageIndex* colIndexPtr() const
-    {
-      return m_nzval_colptr; 
-    }
-    
-    /**
-     * Return the array of compressed row indices of all supernodes
-     */
-    StorageIndex* rowIndex()  { return m_rowind; }
-    
-    const StorageIndex* rowIndex() const
-    {
-      return m_rowind; 
-    }
-    
-    /**
-     * Return the location in \em rowvaluePtr() which starts each column
-     */
-    StorageIndex* rowIndexPtr() { return m_rowind_colptr; }
-    
-    const StorageIndex* rowIndexPtr() const
-    {
-      return m_rowind_colptr; 
-    }
-    
-    /** 
-     * Return the array of column-to-supernode mapping 
-     */
-    StorageIndex* colToSup()  { return m_col_to_sup; }
-    
-    const StorageIndex* colToSup() const
-    {
-      return m_col_to_sup;       
-    }
-    /**
-     * Return the array of supernode-to-column mapping
-     */
-    StorageIndex* supToCol() { return m_sup_to_col; }
-    
-    const StorageIndex* supToCol() const
-    {
-      return m_sup_to_col;
-    }
-    
-    /**
-     * Return the number of supernodes
-     */
-    Index nsuper() const
-    {
-      return m_nsuper; 
-    }
-    
-    class InnerIterator; 
-    template<typename Dest>
-    void solveInPlace( MatrixBase<Dest>&X) const;
-    template<bool Conjugate, typename Dest>
-    void solveTransposedInPlace( MatrixBase<Dest>&X) const;
+template <typename Scalar_, typename StorageIndex_>
+class MappedSuperNodalMatrix {
+ public:
+  typedef Scalar_ Scalar;
+  typedef StorageIndex_ StorageIndex;
+  typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+  typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
 
-    
-      
-      
-    
-  protected:
-    Index m_row; // Number of rows
-    Index m_col; // Number of columns
-    Index m_nsuper; // Number of supernodes
-    Scalar* m_nzval; //array of nonzero values packed by column
-    StorageIndex* m_nzval_colptr; //nzval_colptr[j] Stores the location in nzval[] which starts column j
-    StorageIndex* m_rowind; // Array of compressed row indices of rectangular supernodes
-    StorageIndex* m_rowind_colptr; //rowind_colptr[j] stores the location in rowind[] which starts column j
-    StorageIndex* m_col_to_sup; // col_to_sup[j] is the supernode number to which column j belongs
-    StorageIndex* m_sup_to_col; //sup_to_col[s] points to the starting column of the s-th supernode
-    
-  private :
+ public:
+  MappedSuperNodalMatrix() {}
+  MappedSuperNodalMatrix(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
+                         IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col) {
+    setInfos(m, n, nzval, nzval_colptr, rowind, rowind_colptr, col_to_sup, sup_to_col);
+  }
+
+  ~MappedSuperNodalMatrix() {}
+  /**
+   * Set appropriate pointers for the lower triangular supernodal matrix
+   * These infos are available at the end of the numerical factorization
+   * FIXME This class will be modified such that it can be use in the course
+   * of the factorization.
+   */
+  void setInfos(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
+                IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col) {
+    m_row = m;
+    m_col = n;
+    m_nzval = nzval.data();
+    m_nzval_colptr = nzval_colptr.data();
+    m_rowind = rowind.data();
+    m_rowind_colptr = rowind_colptr.data();
+    m_nsuper = col_to_sup(n);
+    m_col_to_sup = col_to_sup.data();
+    m_sup_to_col = sup_to_col.data();
+  }
+
+  /**
+   * Number of rows
+   */
+  Index rows() const { return m_row; }
+
+  /**
+   * Number of columns
+   */
+  Index cols() const { return m_col; }
+
+  /**
+   * Return the array of nonzero values packed by column
+   *
+   * The size is nnz
+   */
+  Scalar* valuePtr() { return m_nzval; }
+
+  const Scalar* valuePtr() const { return m_nzval; }
+  /**
+   * Return the pointers to the beginning of each column in \ref valuePtr()
+   */
+  StorageIndex* colIndexPtr() { return m_nzval_colptr; }
+
+  const StorageIndex* colIndexPtr() const { return m_nzval_colptr; }
+
+  /**
+   * Return the array of compressed row indices of all supernodes
+   */
+  StorageIndex* rowIndex() { return m_rowind; }
+
+  const StorageIndex* rowIndex() const { return m_rowind; }
+
+  /**
+   * Return the location in \em rowvaluePtr() which starts each column
+   */
+  StorageIndex* rowIndexPtr() { return m_rowind_colptr; }
+
+  const StorageIndex* rowIndexPtr() const { return m_rowind_colptr; }
+
+  /**
+   * Return the array of column-to-supernode mapping
+   */
+  StorageIndex* colToSup() { return m_col_to_sup; }
+
+  const StorageIndex* colToSup() const { return m_col_to_sup; }
+  /**
+   * Return the array of supernode-to-column mapping
+   */
+  StorageIndex* supToCol() { return m_sup_to_col; }
+
+  const StorageIndex* supToCol() const { return m_sup_to_col; }
+
+  /**
+   * Return the number of supernodes
+   */
+  Index nsuper() const { return m_nsuper; }
+
+  class InnerIterator;
+  template <typename Dest>
+  void solveInPlace(MatrixBase<Dest>& X) const;
+  template <bool Conjugate, typename Dest>
+  void solveTransposedInPlace(MatrixBase<Dest>& X) const;
+
+ protected:
+  Index m_row;                    // Number of rows
+  Index m_col;                    // Number of columns
+  Index m_nsuper;                 // Number of supernodes
+  Scalar* m_nzval;                // array of nonzero values packed by column
+  StorageIndex* m_nzval_colptr;   // nzval_colptr[j] Stores the location in nzval[] which starts column j
+  StorageIndex* m_rowind;         // Array of compressed row indices of rectangular supernodes
+  StorageIndex* m_rowind_colptr;  // rowind_colptr[j] stores the location in rowind[] which starts column j
+  StorageIndex* m_col_to_sup;     // col_to_sup[j] is the supernode number to which column j belongs
+  StorageIndex* m_sup_to_col;     // sup_to_col[s] points to the starting column of the s-th supernode
+
+ private:
 };
 
 /**
-  * \brief InnerIterator class to iterate over nonzero values of the current column in the supernodal matrix L
-  * 
-  */
-template<typename Scalar, typename StorageIndex>
-class MappedSuperNodalMatrix<Scalar,StorageIndex>::InnerIterator
-{
-  public:
-     InnerIterator(const MappedSuperNodalMatrix& mat, Index outer)
+ * \brief InnerIterator class to iterate over nonzero values of the current column in the supernodal matrix L
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+class MappedSuperNodalMatrix<Scalar, StorageIndex>::InnerIterator {
+ public:
+  InnerIterator(const MappedSuperNodalMatrix& mat, Index outer)
       : m_matrix(mat),
         m_outer(outer),
         m_supno(mat.colToSup()[outer]),
         m_idval(mat.colIndexPtr()[outer]),
         m_startidval(m_idval),
-        m_endidval(mat.colIndexPtr()[outer+1]),
+        m_endidval(mat.colIndexPtr()[outer + 1]),
         m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]),
-        m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1])
-    {}
-    inline InnerIterator& operator++()
-    { 
-      m_idval++; 
-      m_idrow++;
-      return *this;
-    }
-    inline Scalar value() const { return m_matrix.valuePtr()[m_idval]; }
-    
-    inline Scalar& valueRef() { return const_cast<Scalar&>(m_matrix.valuePtr()[m_idval]); }
-    
-    inline Index index() const { return m_matrix.rowIndex()[m_idrow]; }
-    inline Index row() const { return index(); }
-    inline Index col() const { return m_outer; }
-    
-    inline Index supIndex() const { return m_supno; }
-    
-    inline operator bool() const 
-    { 
-      return ( (m_idval < m_endidval) && (m_idval >= m_startidval)
-                && (m_idrow < m_endidrow) );
-    }
-    
-  protected:
-    const MappedSuperNodalMatrix& m_matrix; // Supernodal lower triangular matrix 
-    const Index m_outer;                    // Current column 
-    const Index m_supno;                    // Current SuperNode number
-    Index m_idval;                          // Index to browse the values in the current column
-    const Index m_startidval;               // Start of the column value
-    const Index m_endidval;                 // End of the column value
-    Index m_idrow;                          // Index to browse the row indices 
-    Index m_endidrow;                       // End index of row indices of the current column
+        m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]] + 1]) {}
+  inline InnerIterator& operator++() {
+    m_idval++;
+    m_idrow++;
+    return *this;
+  }
+  inline Scalar value() const { return m_matrix.valuePtr()[m_idval]; }
+
+  inline Scalar& valueRef() { return const_cast<Scalar&>(m_matrix.valuePtr()[m_idval]); }
+
+  inline Index index() const { return m_matrix.rowIndex()[m_idrow]; }
+  inline Index row() const { return index(); }
+  inline Index col() const { return m_outer; }
+
+  inline Index supIndex() const { return m_supno; }
+
+  inline operator bool() const {
+    return ((m_idval < m_endidval) && (m_idval >= m_startidval) && (m_idrow < m_endidrow));
+  }
+
+ protected:
+  const MappedSuperNodalMatrix& m_matrix;  // Supernodal lower triangular matrix
+  const Index m_outer;                     // Current column
+  const Index m_supno;                     // Current SuperNode number
+  Index m_idval;                           // Index to browse the values in the current column
+  const Index m_startidval;                // Start of the column value
+  const Index m_endidval;                  // End of the column value
+  Index m_idrow;                           // Index to browse the row indices
+  Index m_endidrow;                        // End index of row indices of the current column
 };
 
 /**
  * \brief Solve with the supernode triangular matrix
- * 
+ *
  */
-template<typename Scalar, typename Index_>
-template<typename Dest>
-void MappedSuperNodalMatrix<Scalar,Index_>::solveInPlace( MatrixBase<Dest>&X) const
-{
-    /* Explicit type conversion as the Index type of MatrixBase<Dest> may be wider than Index */
-//    eigen_assert(X.rows() <= NumTraits<Index>::highest());
-//    eigen_assert(X.cols() <= NumTraits<Index>::highest());
-    Index n    = int(X.rows());
-    Index nrhs = Index(X.cols());
-    const Scalar * Lval = valuePtr();                 // Nonzero values 
-    Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor> work(n, nrhs);     // working vector
-    work.setZero();
-    for (Index k = 0; k <= nsuper(); k ++)
-    {
-      Index fsupc = supToCol()[k];                    // First column of the current supernode 
-      Index istart = rowIndexPtr()[fsupc];            // Pointer index to the subscript of the current column
-      Index nsupr = rowIndexPtr()[fsupc+1] - istart;  // Number of rows in the current supernode
-      Index nsupc = supToCol()[k+1] - fsupc;          // Number of columns in the current supernode
-      Index nrow = nsupr - nsupc;                     // Number of rows in the non-diagonal part of the supernode
-      Index irow;                                     //Current index row
-      
-      if (nsupc == 1 )
-      {
-        for (Index j = 0; j < nrhs; j++)
-        {
-          InnerIterator it(*this, fsupc);
-          ++it; // Skip the diagonal element
-          for (; it; ++it)
-          {
-            irow = it.row();
-            X(irow, j) -= X(fsupc, j) * it.value();
-          }
-        }
-      }
-      else
-      {
-        // The supernode has more than one column 
-        Index luptr = colIndexPtr()[fsupc]; 
-        Index lda = colIndexPtr()[fsupc+1] - luptr;
-        
-        // Triangular solve 
-        Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) );
-        Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
-        U = A.template triangularView<UnitLower>().solve(U); 
-        
-        // Matrix-vector product 
-        new (&A) Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
-        work.topRows(nrow).noalias() = A * U;
-        
-        //Begin Scatter 
-        for (Index j = 0; j < nrhs; j++)
-        {
-          Index iptr = istart + nsupc; 
-          for (Index i = 0; i < nrow; i++)
-          {
-            irow = rowIndex()[iptr]; 
-            X(irow, j) -= work(i, j); // Scatter operation
-            work(i, j) = Scalar(0); 
-            iptr++;
-          }
-        }
-      }
-    } 
-}
-
-template<typename Scalar, typename Index_>
-template<bool Conjugate, typename Dest>
-void MappedSuperNodalMatrix<Scalar,Index_>::solveTransposedInPlace( MatrixBase<Dest>&X) const
-{
-    using numext::conj;
-  Index n    = int(X.rows());
+template <typename Scalar, typename Index_>
+template <typename Dest>
+void MappedSuperNodalMatrix<Scalar, Index_>::solveInPlace(MatrixBase<Dest>& X) const {
+  /* Explicit type conversion as the Index type of MatrixBase<Dest> may be wider than Index */
+  //    eigen_assert(X.rows() <= NumTraits<Index>::highest());
+  //    eigen_assert(X.cols() <= NumTraits<Index>::highest());
+  Index n = int(X.rows());
   Index nrhs = Index(X.cols());
-  const Scalar * Lval = valuePtr();                 // Nonzero values
-  Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor> work(n, nrhs);     // working vector
+  const Scalar* Lval = valuePtr();                                           // Nonzero values
+  Matrix<Scalar, Dynamic, Dest::ColsAtCompileTime, ColMajor> work(n, nrhs);  // working vector
   work.setZero();
-  for (Index k = nsuper(); k >= 0; k--)
-  {
-    Index fsupc = supToCol()[k];                    // First column of the current supernode
-    Index istart = rowIndexPtr()[fsupc];            // Pointer index to the subscript of the current column
-    Index nsupr = rowIndexPtr()[fsupc+1] - istart;  // Number of rows in the current supernode
-    Index nsupc = supToCol()[k+1] - fsupc;          // Number of columns in the current supernode
-    Index nrow = nsupr - nsupc;                     // Number of rows in the non-diagonal part of the supernode
-    Index irow;                                     //Current index row
+  for (Index k = 0; k <= nsuper(); k++) {
+    Index fsupc = supToCol()[k];                      // First column of the current supernode
+    Index istart = rowIndexPtr()[fsupc];              // Pointer index to the subscript of the current column
+    Index nsupr = rowIndexPtr()[fsupc + 1] - istart;  // Number of rows in the current supernode
+    Index nsupc = supToCol()[k + 1] - fsupc;          // Number of columns in the current supernode
+    Index nrow = nsupr - nsupc;                       // Number of rows in the non-diagonal part of the supernode
+    Index irow;                                       // Current index row
 
-    if (nsupc == 1 )
-    {
-      for (Index j = 0; j < nrhs; j++)
-      {
+    if (nsupc == 1) {
+      for (Index j = 0; j < nrhs; j++) {
         InnerIterator it(*this, fsupc);
-        ++it; // Skip the diagonal element
-        for (; it; ++it)
-        {
+        ++it;  // Skip the diagonal element
+        for (; it; ++it) {
           irow = it.row();
-          X(fsupc,j) -= X(irow, j) * (Conjugate?conj(it.value()):it.value());
+          X(irow, j) -= X(fsupc, j) * it.value();
+        }
+      }
+    } else {
+      // The supernode has more than one column
+      Index luptr = colIndexPtr()[fsupc];
+      Index lda = colIndexPtr()[fsupc + 1] - luptr;
+
+      // Triangular solve
+      Map<const Matrix<Scalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<> > A(&(Lval[luptr]), nsupc, nsupc,
+                                                                                 OuterStride<>(lda));
+      typename Dest::RowsBlockXpr U = X.derived().middleRows(fsupc, nsupc);
+      U = A.template triangularView<UnitLower>().solve(U);
+      // Matrix-vector product
+      new (&A) Map<const Matrix<Scalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<> >(&(Lval[luptr + nsupc]), nrow,
+                                                                                        nsupc, OuterStride<>(lda));
+      work.topRows(nrow).noalias() = A * U;
+
+      // Begin Scatter
+      for (Index j = 0; j < nrhs; j++) {
+        Index iptr = istart + nsupc;
+        for (Index i = 0; i < nrow; i++) {
+          irow = rowIndex()[iptr];
+          X(irow, j) -= work(i, j);  // Scatter operation
+          work(i, j) = Scalar(0);
+          iptr++;
         }
       }
     }
-    else
-    {
+  }
+}
+
+template <typename Scalar, typename Index_>
+template <bool Conjugate, typename Dest>
+void MappedSuperNodalMatrix<Scalar, Index_>::solveTransposedInPlace(MatrixBase<Dest>& X) const {
+  using numext::conj;
+  Index n = int(X.rows());
+  Index nrhs = Index(X.cols());
+  const Scalar* Lval = valuePtr();                                           // Nonzero values
+  Matrix<Scalar, Dynamic, Dest::ColsAtCompileTime, ColMajor> work(n, nrhs);  // working vector
+  work.setZero();
+  for (Index k = nsuper(); k >= 0; k--) {
+    Index fsupc = supToCol()[k];                      // First column of the current supernode
+    Index istart = rowIndexPtr()[fsupc];              // Pointer index to the subscript of the current column
+    Index nsupr = rowIndexPtr()[fsupc + 1] - istart;  // Number of rows in the current supernode
+    Index nsupc = supToCol()[k + 1] - fsupc;          // Number of columns in the current supernode
+    Index nrow = nsupr - nsupc;                       // Number of rows in the non-diagonal part of the supernode
+    Index irow;                                       // Current index row
+
+    if (nsupc == 1) {
+      for (Index j = 0; j < nrhs; j++) {
+        InnerIterator it(*this, fsupc);
+        ++it;  // Skip the diagonal element
+        for (; it; ++it) {
+          irow = it.row();
+          X(fsupc, j) -= X(irow, j) * (Conjugate ? conj(it.value()) : it.value());
+        }
+      }
+    } else {
       // The supernode has more than one column
       Index luptr = colIndexPtr()[fsupc];
-      Index lda = colIndexPtr()[fsupc+1] - luptr;
+      Index lda = colIndexPtr()[fsupc + 1] - luptr;
 
-      //Begin Gather
-      for (Index j = 0; j < nrhs; j++)
-      {
+      // Begin Gather
+      for (Index j = 0; j < nrhs; j++) {
         Index iptr = istart + nsupc;
-        for (Index i = 0; i < nrow; i++)
-        {
+        for (Index i = 0; i < nrow; i++) {
           irow = rowIndex()[iptr];
-          work.topRows(nrow)(i,j)= X(irow,j); // Gather operation
+          work.topRows(nrow)(i, j) = X(irow, j);  // Gather operation
           iptr++;
         }
       }
 
       // Matrix-vector product with transposed submatrix
-      Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
-      Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
-      if(Conjugate)
+      Map<const Matrix<Scalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<> > A(&(Lval[luptr + nsupc]), nrow, nsupc,
+                                                                                 OuterStride<>(lda));
+      typename Dest::RowsBlockXpr U = X.derived().middleRows(fsupc, nsupc);
+      if (Conjugate)
         U = U - A.adjoint() * work.topRows(nrow);
       else
         U = U - A.transpose() * work.topRows(nrow);
 
       // Triangular solve (of transposed diagonal block)
-      new (&A) Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > ( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) );
-      if(Conjugate)
+      new (&A) Map<const Matrix<Scalar, Dynamic, Dynamic, ColMajor>, 0, OuterStride<> >(&(Lval[luptr]), nsupc, nsupc,
+                                                                                        OuterStride<>(lda));
+      if (Conjugate)
         U = A.adjoint().template triangularView<UnitUpper>().solve(U);
       else
         U = A.transpose().template triangularView<UnitUpper>().solve(U);
-
     }
-
   }
 }
 
+}  // end namespace internal
 
-} // end namespace internal
+}  // end namespace Eigen
 
-} // end namespace Eigen
-
-#endif // EIGEN_SPARSELU_MATRIX_H
+#endif  // EIGEN_SPARSELU_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h
index 9e3dab4..ef087cd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h
@@ -7,10 +7,12 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-
 #ifndef EIGEN_SPARSELU_UTILS_H
 #define EIGEN_SPARSELU_UTILS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
@@ -18,63 +20,56 @@
  * \brief Count Nonzero elements in the factors
  */
 template <typename Scalar, typename StorageIndex>
-void SparseLUImpl<Scalar,StorageIndex>::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu)
-{
- nnzL = 0; 
- nnzU = (glu.xusub)(n); 
- Index nsuper = (glu.supno)(n); 
- Index jlen; 
- Index i, j, fsupc;
- if (n <= 0 ) return; 
- // For each supernode
- for (i = 0; i <= nsuper; i++)
- {
-   fsupc = glu.xsup(i); 
-   jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 
-   
-   for (j = fsupc; j < glu.xsup(i+1); j++)
-   {
-     nnzL += jlen; 
-     nnzU += j - fsupc + 1; 
-     jlen--; 
-   }
- }
+void SparseLUImpl<Scalar, StorageIndex>::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu) {
+  nnzL = 0;
+  nnzU = (glu.xusub)(n);
+  Index nsuper = (glu.supno)(n);
+  Index jlen;
+  Index i, j, fsupc;
+  if (n <= 0) return;
+  // For each supernode
+  for (i = 0; i <= nsuper; i++) {
+    fsupc = glu.xsup(i);
+    jlen = glu.xlsub(fsupc + 1) - glu.xlsub(fsupc);
+
+    for (j = fsupc; j < glu.xsup(i + 1); j++) {
+      nnzL += jlen;
+      nnzU += j - fsupc + 1;
+      jlen--;
+    }
+  }
 }
 
 /**
- * \brief Fix up the data storage lsub for L-subscripts. 
- * 
- * It removes the subscripts sets for structural pruning, 
+ * \brief Fix up the data storage lsub for L-subscripts.
+ *
+ * It removes the subscripts sets for structural pruning,
  * and applies permutation to the remaining subscripts
- * 
+ *
  */
 template <typename Scalar, typename StorageIndex>
-void SparseLUImpl<Scalar,StorageIndex>::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu)
-{
-  Index fsupc, i, j, k, jstart; 
-  
-  StorageIndex nextl = 0; 
-  Index nsuper = (glu.supno)(n); 
-  
-  // For each supernode 
-  for (i = 0; i <= nsuper; i++)
-  {
-    fsupc = glu.xsup(i); 
-    jstart = glu.xlsub(fsupc); 
-    glu.xlsub(fsupc) = nextl; 
-    for (j = jstart; j < glu.xlsub(fsupc + 1); j++)
-    {
-      glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A
+void SparseLUImpl<Scalar, StorageIndex>::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu) {
+  Index fsupc, i, j, k, jstart;
+
+  StorageIndex nextl = 0;
+  Index nsuper = (glu.supno)(n);
+
+  // For each supernode
+  for (i = 0; i <= nsuper; i++) {
+    fsupc = glu.xsup(i);
+    jstart = glu.xlsub(fsupc);
+    glu.xlsub(fsupc) = nextl;
+    for (j = jstart; j < glu.xlsub(fsupc + 1); j++) {
+      glu.lsub(nextl) = perm_r(glu.lsub(j));  // Now indexed into P*A
       nextl++;
     }
-    for (k = fsupc+1; k < glu.xsup(i+1); k++)
-      glu.xlsub(k) = nextl; // other columns in supernode i
+    for (k = fsupc + 1; k < glu.xsup(i + 1); k++) glu.xlsub(k) = nextl;  // other columns in supernode i
   }
-  
-  glu.xlsub(n) = nextl; 
+
+  glu.xlsub(n) = nextl;
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
-#endif // EIGEN_SPARSELU_UTILS_H
+}  // end namespace Eigen
+#endif  // EIGEN_SPARSELU_UTILS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h
index b57f068..8435b56 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h
@@ -8,10 +8,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
- 
- * NOTE: This file is the modified version of xcolumn_bmod.c file in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of xcolumn_bmod.c file in SuperLU
+
  * -- SuperLU routine (version 3.0) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -31,151 +31,147 @@
 #ifndef SPARSELU_COLUMN_BMOD_H
 #define SPARSELU_COLUMN_BMOD_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 /**
  * \brief Performs numeric block updates (sup-col) in topological order
- * 
+ *
  * \param jcol current column to update
  * \param nseg Number of segments in the U part
  * \param dense Store the full representation of the column
- * \param tempv working array 
+ * \param tempv working array
  * \param segrep segment representative ...
  * \param repfnz ??? First nonzero column in each row ???  ...
  * \param fpanelc First column in the current panel
- * \param glu Global LU data. 
- * \return 0 - successful return 
+ * \param glu Global LU data.
+ * \return 0 - successful return
  *         > 0 - number of bytes allocated when run out of space
- * 
+ *
  */
 template <typename Scalar, typename StorageIndex>
-Index SparseLUImpl<Scalar,StorageIndex>::column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv,
-                                                     BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu)
-{
-  Index  jsupno, k, ksub, krep, ksupno; 
-  Index lptr, nrow, isub, irow, nextlu, new_next, ufirst; 
-  Index fsupc, nsupc, nsupr, luptr, kfnz, no_zeros; 
+Index SparseLUImpl<Scalar, StorageIndex>::column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense,
+                                                      ScalarVector& tempv, BlockIndexVector segrep,
+                                                      BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu) {
+  Index jsupno, k, ksub, krep, ksupno;
+  Index lptr, nrow, isub, irow, nextlu, new_next, ufirst;
+  Index fsupc, nsupc, nsupr, luptr, kfnz, no_zeros;
   /* krep = representative of current k-th supernode
-    * fsupc =  first supernodal column
-    * nsupc = number of columns in a supernode
-    * nsupr = number of rows in a supernode
-    * luptr = location of supernodal LU-block in storage
-    * kfnz = first nonz in the k-th supernodal segment
-    * no_zeros = no lf leading zeros in a supernodal U-segment
-    */
-  
+   * fsupc =  first supernodal column
+   * nsupc = number of columns in a supernode
+   * nsupr = number of rows in a supernode
+   * luptr = location of supernodal LU-block in storage
+   * kfnz = first nonz in the k-th supernodal segment
+   * no_zeros = no lf leading zeros in a supernodal U-segment
+   */
+
   jsupno = glu.supno(jcol);
-  // For each nonzero supernode segment of U[*,j] in topological order 
-  k = nseg - 1; 
-  Index d_fsupc; // distance between the first column of the current panel and the 
-               // first column of the current snode
-  Index fst_col; // First column within small LU update
-  Index segsize; 
-  for (ksub = 0; ksub < nseg; ksub++)
-  {
-    krep = segrep(k); k--; 
-    ksupno = glu.supno(krep); 
-    if (jsupno != ksupno )
-    {
-      // outside the rectangular supernode 
-      fsupc = glu.xsup(ksupno); 
-      fst_col = (std::max)(fsupc, fpanelc); 
-      
-      // Distance from the current supernode to the current panel; 
+  // For each nonzero supernode segment of U[*,j] in topological order
+  k = nseg - 1;
+  Index d_fsupc;  // distance between the first column of the current panel and the
+                  // first column of the current snode
+  Index fst_col;  // First column within small LU update
+  Index segsize;
+  for (ksub = 0; ksub < nseg; ksub++) {
+    krep = segrep(k);
+    k--;
+    ksupno = glu.supno(krep);
+    if (jsupno != ksupno) {
+      // outside the rectangular supernode
+      fsupc = glu.xsup(ksupno);
+      fst_col = (std::max)(fsupc, fpanelc);
+
+      // Distance from the current supernode to the current panel;
       // d_fsupc = 0 if fsupc > fpanelc
-      d_fsupc = fst_col - fsupc; 
-      
-      luptr = glu.xlusup(fst_col) + d_fsupc; 
-      lptr = glu.xlsub(fsupc) + d_fsupc; 
-      
-      kfnz = repfnz(krep); 
-      kfnz = (std::max)(kfnz, fpanelc); 
-      
-      segsize = krep - kfnz + 1; 
-      nsupc = krep - fst_col + 1; 
-      nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 
+      d_fsupc = fst_col - fsupc;
+
+      luptr = glu.xlusup(fst_col) + d_fsupc;
+      lptr = glu.xlsub(fsupc) + d_fsupc;
+
+      kfnz = repfnz(krep);
+      kfnz = (std::max)(kfnz, fpanelc);
+
+      segsize = krep - kfnz + 1;
+      nsupc = krep - fst_col + 1;
+      nsupr = glu.xlsub(fsupc + 1) - glu.xlsub(fsupc);
       nrow = nsupr - d_fsupc - nsupc;
-      Index lda = glu.xlusup(fst_col+1) - glu.xlusup(fst_col);
-      
-      
-      // Perform a triangular solver and block update, 
+      Index lda = glu.xlusup(fst_col + 1) - glu.xlusup(fst_col);
+
+      // Perform a triangular solver and block update,
       // then scatter the result of sup-col update to dense
-      no_zeros = kfnz - fst_col; 
-      if(segsize==1)
+      no_zeros = kfnz - fst_col;
+      if (segsize == 1)
         LU_kernel_bmod<1>::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
       else
         LU_kernel_bmod<Dynamic>::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
-    } // end if jsupno 
-  } // end for each segment
-  
+    }  // end if jsupno
+  }    // end for each segment
+
   // Process the supernodal portion of  L\U[*,j]
-  nextlu = glu.xlusup(jcol); 
+  nextlu = glu.xlusup(jcol);
   fsupc = glu.xsup(jsupno);
-  
+
   // copy the SPA dense into L\U[*,j]
-  Index mem; 
-  new_next = nextlu + glu.xlsub(fsupc + 1) - glu.xlsub(fsupc); 
+  Index mem;
+  new_next = nextlu + glu.xlsub(fsupc + 1) - glu.xlsub(fsupc);
   Index offset = internal::first_multiple<Index>(new_next, internal::packet_traits<Scalar>::size) - new_next;
-  if(offset)
-    new_next += offset;
-  while (new_next > glu.nzlumax )
-  {
-    mem = memXpand<ScalarVector>(glu.lusup, glu.nzlumax, nextlu, LUSUP, glu.num_expansions);  
-    if (mem) return mem; 
+  if (offset) new_next += offset;
+  while (new_next > glu.nzlumax) {
+    mem = memXpand<ScalarVector>(glu.lusup, glu.nzlumax, nextlu, LUSUP, glu.num_expansions);
+    if (mem) return mem;
   }
-  
-  for (isub = glu.xlsub(fsupc); isub < glu.xlsub(fsupc+1); isub++)
-  {
+
+  for (isub = glu.xlsub(fsupc); isub < glu.xlsub(fsupc + 1); isub++) {
     irow = glu.lsub(isub);
     glu.lusup(nextlu) = dense(irow);
-    dense(irow) = Scalar(0.0); 
-    ++nextlu; 
+    dense(irow) = Scalar(0.0);
+    ++nextlu;
   }
-  
-  if(offset)
-  {
-    glu.lusup.segment(nextlu,offset).setZero();
+
+  if (offset) {
+    glu.lusup.segment(nextlu, offset).setZero();
     nextlu += offset;
   }
-  glu.xlusup(jcol + 1) = StorageIndex(nextlu);  // close L\U(*,jcol); 
-  
+  glu.xlusup(jcol + 1) = StorageIndex(nextlu);  // close L\U(*,jcol);
+
   /* For more updates within the panel (also within the current supernode),
    * should start from the first column of the panel, or the first column
    * of the supernode, whichever is bigger. There are two cases:
    *  1) fsupc < fpanelc, then fst_col <-- fpanelc
    *  2) fsupc >= fpanelc, then fst_col <-- fsupc
    */
-  fst_col = (std::max)(fsupc, fpanelc); 
-  
-  if (fst_col  < jcol)
-  {
+  fst_col = (std::max)(fsupc, fpanelc);
+
+  if (fst_col < jcol) {
     // Distance between the current supernode and the current panel
     // d_fsupc = 0 if fsupc >= fpanelc
-    d_fsupc = fst_col - fsupc; 
-    
-    lptr = glu.xlsub(fsupc) + d_fsupc; 
-    luptr = glu.xlusup(fst_col) + d_fsupc; 
-    nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); // leading dimension
-    nsupc = jcol - fst_col; // excluding jcol 
-    nrow = nsupr - d_fsupc - nsupc; 
-    
-    // points to the beginning of jcol in snode L\U(jsupno) 
-    ufirst = glu.xlusup(jcol) + d_fsupc; 
-    Index lda = glu.xlusup(jcol+1) - glu.xlusup(jcol);
-    MappedMatrixBlock A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
-    VectorBlock<ScalarVector> u(glu.lusup, ufirst, nsupc); 
-    u = A.template triangularView<UnitLower>().solve(u); 
-    
-    new (&A) MappedMatrixBlock ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
-    VectorBlock<ScalarVector> l(glu.lusup, ufirst+nsupc, nrow); 
+    d_fsupc = fst_col - fsupc;
+
+    lptr = glu.xlsub(fsupc) + d_fsupc;
+    luptr = glu.xlusup(fst_col) + d_fsupc;
+    nsupr = glu.xlsub(fsupc + 1) - glu.xlsub(fsupc);  // leading dimension
+    nsupc = jcol - fst_col;                           // excluding jcol
+    nrow = nsupr - d_fsupc - nsupc;
+
+    // points to the beginning of jcol in snode L\U(jsupno)
+    ufirst = glu.xlusup(jcol) + d_fsupc;
+    Index lda = glu.xlusup(jcol + 1) - glu.xlusup(jcol);
+    MappedMatrixBlock A(&(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda));
+    VectorBlock<ScalarVector> u(glu.lusup, ufirst, nsupc);
+    u = A.template triangularView<UnitLower>().solve(u);
+
+    new (&A) MappedMatrixBlock(&(glu.lusup.data()[luptr + nsupc]), nrow, nsupc, OuterStride<>(lda));
+    VectorBlock<ScalarVector> l(glu.lusup, ufirst + nsupc, nrow);
     l.noalias() -= A * u;
-    
-  } // End if fst_col
-  return 0; 
+
+  }  // End if fst_col
+  return 0;
 }
 
-} // end namespace internal
-} // end namespace Eigen
+}  // end namespace internal
+}  // end namespace Eigen
 
-#endif // SPARSELU_COLUMN_BMOD_H
+#endif  // SPARSELU_COLUMN_BMOD_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h
index 5a2c941..e5fb771 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h
@@ -7,10 +7,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
- 
- * NOTE: This file is the modified version of [s,d,c,z]column_dfs.c file in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]column_dfs.c file in SuperLU
+
  * -- SuperLU routine (version 2.0) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -30,150 +30,139 @@
 #ifndef SPARSELU_COLUMN_DFS_H
 #define SPARSELU_COLUMN_DFS_H
 
-template <typename Scalar, typename StorageIndex> class SparseLUImpl;
+template <typename Scalar, typename StorageIndex>
+class SparseLUImpl;
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename IndexVector, typename ScalarVector>
-struct column_dfs_traits : no_assignment_operator
-{
+template <typename IndexVector, typename ScalarVector>
+struct column_dfs_traits : no_assignment_operator {
   typedef typename ScalarVector::Scalar Scalar;
   typedef typename IndexVector::Scalar StorageIndex;
-  column_dfs_traits(Index jcol, Index& jsuper, typename SparseLUImpl<Scalar, StorageIndex>::GlobalLU_t& glu, SparseLUImpl<Scalar, StorageIndex>& luImpl)
-   : m_jcol(jcol), m_jsuper_ref(jsuper), m_glu(glu), m_luImpl(luImpl)
- {}
-  bool update_segrep(Index /*krep*/, Index /*jj*/)
-  {
-    return true;
-  }
-  void mem_expand(IndexVector& lsub, Index& nextl, Index chmark)
-  {
-    if (nextl >= m_glu.nzlmax)
-      m_luImpl.memXpand(lsub, m_glu.nzlmax, nextl, LSUB, m_glu.num_expansions); 
-    if (chmark != (m_jcol-1)) m_jsuper_ref = emptyIdxLU;
+  column_dfs_traits(Index jcol, Index& jsuper, typename SparseLUImpl<Scalar, StorageIndex>::GlobalLU_t& glu,
+                    SparseLUImpl<Scalar, StorageIndex>& luImpl)
+      : m_jcol(jcol), m_jsuper_ref(jsuper), m_glu(glu), m_luImpl(luImpl) {}
+  bool update_segrep(Index /*krep*/, Index /*jj*/) { return true; }
+  void mem_expand(IndexVector& lsub, Index& nextl, Index chmark) {
+    if (nextl >= m_glu.nzlmax) m_luImpl.memXpand(lsub, m_glu.nzlmax, nextl, LSUB, m_glu.num_expansions);
+    if (chmark != (m_jcol - 1)) m_jsuper_ref = emptyIdxLU;
   }
   enum { ExpandMem = true };
-  
+
   Index m_jcol;
   Index& m_jsuper_ref;
   typename SparseLUImpl<Scalar, StorageIndex>::GlobalLU_t& m_glu;
   SparseLUImpl<Scalar, StorageIndex>& m_luImpl;
 };
 
-
 /**
  * \brief Performs a symbolic factorization on column jcol and decide the supernode boundary
- * 
+ *
  * A supernode representative is the last column of a supernode.
- * The nonzeros in U[*,j] are segments that end at supernodes representatives. 
- * The routine returns a list of the supernodal representatives 
- * in topological order of the dfs that generates them. 
- * The location of the first nonzero in each supernodal segment 
- * (supernodal entry location) is also returned. 
- * 
+ * The nonzeros in U[*,j] are segments that end at supernodes representatives.
+ * The routine returns a list of the supernodal representatives
+ * in topological order of the dfs that generates them.
+ * The location of the first nonzero in each supernodal segment
+ * (supernodal entry location) is also returned.
+ *
  * \param m number of rows in the matrix
- * \param jcol Current column 
+ * \param jcol Current column
  * \param perm_r Row permutation
  * \param maxsuper  Maximum number of column allowed in a supernode
  * \param [in,out] nseg Number of segments in current U[*,j] - new segments appended
  * \param lsub_col defines the rhs vector to start the dfs
- * \param [in,out] segrep Segment representatives - new segments appended 
+ * \param [in,out] segrep Segment representatives - new segments appended
  * \param repfnz  First nonzero location in each row
- * \param xprune 
+ * \param xprune
  * \param marker  marker[i] == jj, if i was visited during dfs of current column jj;
  * \param parent
  * \param xplore working array
- * \param glu global LU data 
+ * \param glu global LU data
  * \return 0 success
  *         > 0 number of bytes allocated when run out of space
- * 
+ *
  */
 template <typename Scalar, typename StorageIndex>
-Index SparseLUImpl<Scalar,StorageIndex>::column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg,
-                                                    BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune,
-                                                    IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu)
-{
-  
-  Index jsuper = glu.supno(jcol); 
-  Index nextl = glu.xlsub(jcol); 
-  VectorBlock<IndexVector> marker2(marker, 2*m, m); 
-  
-  
+Index SparseLUImpl<Scalar, StorageIndex>::column_dfs(const Index m, const Index jcol, IndexVector& perm_r,
+                                                     Index maxsuper, Index& nseg, BlockIndexVector lsub_col,
+                                                     IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune,
+                                                     IndexVector& marker, IndexVector& parent, IndexVector& xplore,
+                                                     GlobalLU_t& glu) {
+  Index jsuper = glu.supno(jcol);
+  Index nextl = glu.xlsub(jcol);
+  VectorBlock<IndexVector> marker2(marker, 2 * m, m);
+
   column_dfs_traits<IndexVector, ScalarVector> traits(jcol, jsuper, glu, *this);
-  
-  // For each nonzero in A(*,jcol) do dfs 
-  for (Index k = 0; ((k < m) ? lsub_col[k] != emptyIdxLU : false) ; k++)
-  {
-    Index krow = lsub_col(k); 
-    lsub_col(k) = emptyIdxLU; 
-    Index kmark = marker2(krow); 
-    
-    // krow was visited before, go to the next nonz; 
+
+  // For each nonzero in A(*,jcol) do dfs
+  for (Index k = 0; ((k < m) ? lsub_col[k] != emptyIdxLU : false); k++) {
+    Index krow = lsub_col(k);
+    lsub_col(k) = emptyIdxLU;
+    Index kmark = marker2(krow);
+
+    // krow was visited before, go to the next nonz;
     if (kmark == jcol) continue;
-    
-    dfs_kernel(StorageIndex(jcol), perm_r, nseg, glu.lsub, segrep, repfnz, xprune, marker2, parent,
-                   xplore, glu, nextl, krow, traits);
-  } // for each nonzero ... 
-  
+
+    dfs_kernel(StorageIndex(jcol), perm_r, nseg, glu.lsub, segrep, repfnz, xprune, marker2, parent, xplore, glu, nextl,
+               krow, traits);
+  }  // for each nonzero ...
+
   Index fsupc;
   StorageIndex nsuper = glu.supno(jcol);
   StorageIndex jcolp1 = StorageIndex(jcol) + 1;
   Index jcolm1 = jcol - 1;
-  
+
   // check to see if j belongs in the same supernode as j-1
-  if ( jcol == 0 )
-  { // Do nothing for column 0 
-    nsuper = glu.supno(0) = 0 ;
-  }
-  else 
-  {
-    fsupc = glu.xsup(nsuper); 
-    StorageIndex jptr = glu.xlsub(jcol); // Not yet compressed
-    StorageIndex jm1ptr = glu.xlsub(jcolm1); 
-    
+  if (jcol == 0) {  // Do nothing for column 0
+    nsuper = glu.supno(0) = 0;
+  } else {
+    fsupc = glu.xsup(nsuper);
+    StorageIndex jptr = glu.xlsub(jcol);  // Not yet compressed
+    StorageIndex jm1ptr = glu.xlsub(jcolm1);
+
     // Use supernodes of type T2 : see SuperLU paper
-    if ( (nextl-jptr != jptr-jm1ptr-1) ) jsuper = emptyIdxLU;
-    
+    if ((nextl - jptr != jptr - jm1ptr - 1)) jsuper = emptyIdxLU;
+
     // Make sure the number of columns in a supernode doesn't
     // exceed threshold
-    if ( (jcol - fsupc) >= maxsuper) jsuper = emptyIdxLU; 
-    
+    if ((jcol - fsupc) >= maxsuper) jsuper = emptyIdxLU;
+
     /* If jcol starts a new supernode, reclaim storage space in
-     * glu.lsub from previous supernode. Note we only store 
-     * the subscript set of the first and last columns of 
+     * glu.lsub from previous supernode. Note we only store
+     * the subscript set of the first and last columns of
      * a supernode. (first for num values, last for pruning)
      */
-    if (jsuper == emptyIdxLU)
-    { // starts a new supernode 
-      if ( (fsupc < jcolm1-1) ) 
-      { // >= 3 columns in nsuper
-        StorageIndex ito = glu.xlsub(fsupc+1);
-        glu.xlsub(jcolm1) = ito; 
-        StorageIndex istop = ito + jptr - jm1ptr; 
-        xprune(jcolm1) = istop; // initialize xprune(jcol-1)
-        glu.xlsub(jcol) = istop; 
-        
-        for (StorageIndex ifrom = jm1ptr; ifrom < nextl; ++ifrom, ++ito)
-          glu.lsub(ito) = glu.lsub(ifrom); 
+    if (jsuper == emptyIdxLU) {    // starts a new supernode
+      if ((fsupc < jcolm1 - 1)) {  // >= 3 columns in nsuper
+        StorageIndex ito = glu.xlsub(fsupc + 1);
+        glu.xlsub(jcolm1) = ito;
+        StorageIndex istop = ito + jptr - jm1ptr;
+        xprune(jcolm1) = istop;  // initialize xprune(jcol-1)
+        glu.xlsub(jcol) = istop;
+
+        for (StorageIndex ifrom = jm1ptr; ifrom < nextl; ++ifrom, ++ito) glu.lsub(ito) = glu.lsub(ifrom);
         nextl = ito;  // = istop + length(jcol)
       }
-      nsuper++; 
-      glu.supno(jcol) = nsuper; 
-    } // if a new supernode 
-  } // end else:  jcol > 0
-  
+      nsuper++;
+      glu.supno(jcol) = nsuper;
+    }  // if a new supernode
+  }    // end else:  jcol > 0
+
   // Tidy up the pointers before exit
-  glu.xsup(nsuper+1) = jcolp1; 
-  glu.supno(jcolp1) = nsuper; 
+  glu.xsup(nsuper + 1) = jcolp1;
+  glu.supno(jcolp1) = nsuper;
   xprune(jcol) = StorageIndex(nextl);  // Initialize upper bound for pruning
-  glu.xlsub(jcolp1) = StorageIndex(nextl); 
-  
-  return 0; 
+  glu.xlsub(jcolp1) = StorageIndex(nextl);
+
+  return 0;
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
index c32d8d8..12e7650 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
@@ -6,10 +6,10 @@
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-/* 
- 
- * NOTE: This file is the modified version of [s,d,c,z]copy_to_ucol.c file in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]copy_to_ucol.c file in SuperLU
+
  * -- SuperLU routine (version 2.0) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -29,79 +29,78 @@
 #ifndef SPARSELU_COPY_TO_UCOL_H
 #define SPARSELU_COPY_TO_UCOL_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
 /**
  * \brief Performs numeric block updates (sup-col) in topological order
- * 
+ *
  * \param jcol current column to update
  * \param nseg Number of segments in the U part
  * \param segrep segment representative ...
  * \param repfnz First nonzero column in each row  ...
- * \param perm_r Row permutation 
+ * \param perm_r Row permutation
  * \param dense Store the full representation of the column
- * \param glu Global LU data. 
- * \return 0 - successful return 
+ * \param glu Global LU data.
+ * \return 0 - successful return
  *         > 0 - number of bytes allocated when run out of space
- * 
+ *
  */
 template <typename Scalar, typename StorageIndex>
-Index SparseLUImpl<Scalar,StorageIndex>::copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep,
-                                                      BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu)
-{  
-  Index ksub, krep, ksupno; 
-    
+Index SparseLUImpl<Scalar, StorageIndex>::copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep,
+                                                       BlockIndexVector repfnz, IndexVector& perm_r,
+                                                       BlockScalarVector dense, GlobalLU_t& glu) {
+  Index ksub, krep, ksupno;
+
   Index jsupno = glu.supno(jcol);
-  
-  // For each nonzero supernode segment of U[*,j] in topological order 
-  Index k = nseg - 1, i; 
-  StorageIndex nextu = glu.xusub(jcol); 
-  Index kfnz, isub, segsize; 
-  Index new_next,irow; 
-  Index fsupc, mem; 
-  for (ksub = 0; ksub < nseg; ksub++)
-  {
-    krep = segrep(k); k--; 
-    ksupno = glu.supno(krep); 
-    if (jsupno != ksupno ) // should go into ucol(); 
+
+  // For each nonzero supernode segment of U[*,j] in topological order
+  Index k = nseg - 1, i;
+  StorageIndex nextu = glu.xusub(jcol);
+  Index kfnz, isub, segsize;
+  Index new_next, irow;
+  Index fsupc, mem;
+  for (ksub = 0; ksub < nseg; ksub++) {
+    krep = segrep(k);
+    k--;
+    ksupno = glu.supno(krep);
+    if (jsupno != ksupno)  // should go into ucol();
     {
-      kfnz = repfnz(krep); 
-      if (kfnz != emptyIdxLU)
-      { // Nonzero U-segment 
-        fsupc = glu.xsup(ksupno); 
-        isub = glu.xlsub(fsupc) + kfnz - fsupc; 
-        segsize = krep - kfnz + 1; 
-        new_next = nextu + segsize; 
-        while (new_next > glu.nzumax) 
-        {
-          mem = memXpand<ScalarVector>(glu.ucol, glu.nzumax, nextu, UCOL, glu.num_expansions); 
-          if (mem) return mem; 
-          mem = memXpand<IndexVector>(glu.usub, glu.nzumax, nextu, USUB, glu.num_expansions); 
-          if (mem) return mem; 
-          
+      kfnz = repfnz(krep);
+      if (kfnz != emptyIdxLU) {  // Nonzero U-segment
+        fsupc = glu.xsup(ksupno);
+        isub = glu.xlsub(fsupc) + kfnz - fsupc;
+        segsize = krep - kfnz + 1;
+        new_next = nextu + segsize;
+        while (new_next > glu.nzumax) {
+          mem = memXpand<ScalarVector>(glu.ucol, glu.nzumax, nextu, UCOL, glu.num_expansions);
+          if (mem) return mem;
+          mem = memXpand<IndexVector>(glu.usub, glu.nzumax, nextu, USUB, glu.num_expansions);
+          if (mem) return mem;
         }
-        
-        for (i = 0; i < segsize; i++)
-        {
-          irow = glu.lsub(isub); 
-          glu.usub(nextu) = perm_r(irow); // Unlike the L part, the U part is stored in its final order
-          glu.ucol(nextu) = dense(irow); 
-          dense(irow) = Scalar(0.0); 
+
+        for (i = 0; i < segsize; i++) {
+          irow = glu.lsub(isub);
+          glu.usub(nextu) = perm_r(irow);  // Unlike the L part, the U part is stored in its final order
+          glu.ucol(nextu) = dense(irow);
+          dense(irow) = Scalar(0.0);
           nextu++;
           isub++;
         }
-        
-      } // end nonzero U-segment 
-      
-    } // end if jsupno 
-    
-  } // end for each segment
-  glu.xusub(jcol + 1) = nextu; // close U(*,jcol)
-  return 0; 
+
+      }  // end nonzero U-segment
+
+    }  // end if jsupno
+
+  }                             // end for each segment
+  glu.xusub(jcol + 1) = nextu;  // close U(*,jcol)
+  return 0;
 }
 
-} // namespace internal
-} // end namespace Eigen
+}  // namespace internal
+}  // end namespace Eigen
 
-#endif // SPARSELU_COPY_TO_UCOL_H
+#endif  // SPARSELU_COPY_TO_UCOL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h
deleted file mode 100644
index e37c2fe..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h
+++ /dev/null
@@ -1,280 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_SPARSELU_GEMM_KERNEL_H
-#define EIGEN_SPARSELU_GEMM_KERNEL_H
-
-namespace Eigen {
-
-namespace internal {
-
-
-/** \internal
-  * A general matrix-matrix product kernel optimized for the SparseLU factorization.
-  *  - A, B, and C must be column major
-  *  - lda and ldc must be multiples of the respective packet size
-  *  - C must have the same alignment as A
-  */
-template<typename Scalar>
-EIGEN_DONT_INLINE
-void sparselu_gemm(Index m, Index n, Index d, const Scalar* A, Index lda, const Scalar* B, Index ldb, Scalar* C, Index ldc)
-{
-  using namespace Eigen::internal;
-  
-  typedef typename packet_traits<Scalar>::type Packet;
-  enum {
-    NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
-    PacketSize = packet_traits<Scalar>::size,
-    PM = 8,                             // peeling in M
-    RN = 2,                             // register blocking
-    RK = NumberOfRegisters>=16 ? 4 : 2, // register blocking
-    BM = 4096/sizeof(Scalar),           // number of rows of A-C per chunk
-    SM = PM*PacketSize                  // step along M
-  };
-  Index d_end = (d/RK)*RK;    // number of columns of A (rows of B) suitable for full register blocking
-  Index n_end = (n/RN)*RN;    // number of columns of B-C suitable for processing RN columns at once
-  Index i0 = internal::first_default_aligned(A,m);
-  
-  eigen_internal_assert(((lda%PacketSize)==0) && ((ldc%PacketSize)==0) && (i0==internal::first_default_aligned(C,m)));
-  
-  // handle the non aligned rows of A and C without any optimization:
-  for(Index i=0; i<i0; ++i)
-  {
-    for(Index j=0; j<n; ++j)
-    {
-      Scalar c = C[i+j*ldc];
-      for(Index k=0; k<d; ++k)
-        c += B[k+j*ldb] * A[i+k*lda];
-      C[i+j*ldc] = c;
-    }
-  }
-  // process the remaining rows per chunk of BM rows
-  for(Index ib=i0; ib<m; ib+=BM)
-  {
-    Index actual_b = std::min<Index>(BM, m-ib);                 // actual number of rows
-    Index actual_b_end1 = (actual_b/SM)*SM;                   // actual number of rows suitable for peeling
-    Index actual_b_end2 = (actual_b/PacketSize)*PacketSize;   // actual number of rows suitable for vectorization
-    
-    // Let's process two columns of B-C at once
-    for(Index j=0; j<n_end; j+=RN)
-    {
-      const Scalar* Bc0 = B+(j+0)*ldb;
-      const Scalar* Bc1 = B+(j+1)*ldb;
-      
-      for(Index k=0; k<d_end; k+=RK)
-      {
-        
-        // load and expand a RN x RK block of B
-        Packet b00, b10, b20, b30, b01, b11, b21, b31;
-                  { b00 = pset1<Packet>(Bc0[0]); }
-                  { b10 = pset1<Packet>(Bc0[1]); }
-        if(RK==4) { b20 = pset1<Packet>(Bc0[2]); }
-        if(RK==4) { b30 = pset1<Packet>(Bc0[3]); }
-                  { b01 = pset1<Packet>(Bc1[0]); }
-                  { b11 = pset1<Packet>(Bc1[1]); }
-        if(RK==4) { b21 = pset1<Packet>(Bc1[2]); }
-        if(RK==4) { b31 = pset1<Packet>(Bc1[3]); }
-        
-        Packet a0, a1, a2, a3, c0, c1, t0, t1;
-        
-        const Scalar* A0 = A+ib+(k+0)*lda;
-        const Scalar* A1 = A+ib+(k+1)*lda;
-        const Scalar* A2 = A+ib+(k+2)*lda;
-        const Scalar* A3 = A+ib+(k+3)*lda;
-        
-        Scalar* C0 = C+ib+(j+0)*ldc;
-        Scalar* C1 = C+ib+(j+1)*ldc;
-        
-                  a0 = pload<Packet>(A0);
-                  a1 = pload<Packet>(A1);
-        if(RK==4)
-        {
-          a2 = pload<Packet>(A2);
-          a3 = pload<Packet>(A3);
-        }
-        else
-        {
-          // workaround "may be used uninitialized in this function" warning
-          a2 = a3 = a0;
-        }
-        
-#define KMADD(c, a, b, tmp) {tmp = b; tmp = pmul(a,tmp); c = padd(c,tmp);}
-#define WORK(I)  \
-                     c0 = pload<Packet>(C0+i+(I)*PacketSize);    \
-                     c1 = pload<Packet>(C1+i+(I)*PacketSize);    \
-                     KMADD(c0, a0, b00, t0)                      \
-                     KMADD(c1, a0, b01, t1)                      \
-                     a0 = pload<Packet>(A0+i+(I+1)*PacketSize);  \
-                     KMADD(c0, a1, b10, t0)                      \
-                     KMADD(c1, a1, b11, t1)                      \
-                     a1 = pload<Packet>(A1+i+(I+1)*PacketSize);  \
-          if(RK==4){ KMADD(c0, a2, b20, t0)                     }\
-          if(RK==4){ KMADD(c1, a2, b21, t1)                     }\
-          if(RK==4){ a2 = pload<Packet>(A2+i+(I+1)*PacketSize); }\
-          if(RK==4){ KMADD(c0, a3, b30, t0)                     }\
-          if(RK==4){ KMADD(c1, a3, b31, t1)                     }\
-          if(RK==4){ a3 = pload<Packet>(A3+i+(I+1)*PacketSize); }\
-                     pstore(C0+i+(I)*PacketSize, c0);            \
-                     pstore(C1+i+(I)*PacketSize, c1)
-        
-        // process rows of A' - C' with aggressive vectorization and peeling 
-        for(Index i=0; i<actual_b_end1; i+=PacketSize*8)
-        {
-          EIGEN_ASM_COMMENT("SPARSELU_GEMML_KERNEL1");
-                    prefetch((A0+i+(5)*PacketSize));
-                    prefetch((A1+i+(5)*PacketSize));
-          if(RK==4) prefetch((A2+i+(5)*PacketSize));
-          if(RK==4) prefetch((A3+i+(5)*PacketSize));
-
-          WORK(0);
-          WORK(1);
-          WORK(2);
-          WORK(3);
-          WORK(4);
-          WORK(5);
-          WORK(6);
-          WORK(7);
-        }
-        // process the remaining rows with vectorization only
-        for(Index i=actual_b_end1; i<actual_b_end2; i+=PacketSize)
-        {
-          WORK(0);
-        }
-#undef WORK
-        // process the remaining rows without vectorization
-        for(Index i=actual_b_end2; i<actual_b; ++i)
-        {
-          if(RK==4)
-          {
-            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1]+A2[i]*Bc0[2]+A3[i]*Bc0[3];
-            C1[i] += A0[i]*Bc1[0]+A1[i]*Bc1[1]+A2[i]*Bc1[2]+A3[i]*Bc1[3];
-          }
-          else
-          {
-            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1];
-            C1[i] += A0[i]*Bc1[0]+A1[i]*Bc1[1];
-          }
-        }
-        
-        Bc0 += RK;
-        Bc1 += RK;
-      } // peeled loop on k
-    } // peeled loop on the columns j
-    // process the last column (we now perform a matrix-vector product)
-    if((n-n_end)>0)
-    {
-      const Scalar* Bc0 = B+(n-1)*ldb;
-      
-      for(Index k=0; k<d_end; k+=RK)
-      {
-        
-        // load and expand a 1 x RK block of B
-        Packet b00, b10, b20, b30;
-                  b00 = pset1<Packet>(Bc0[0]);
-                  b10 = pset1<Packet>(Bc0[1]);
-        if(RK==4) b20 = pset1<Packet>(Bc0[2]);
-        if(RK==4) b30 = pset1<Packet>(Bc0[3]);
-        
-        Packet a0, a1, a2, a3, c0, t0/*, t1*/;
-        
-        const Scalar* A0 = A+ib+(k+0)*lda;
-        const Scalar* A1 = A+ib+(k+1)*lda;
-        const Scalar* A2 = A+ib+(k+2)*lda;
-        const Scalar* A3 = A+ib+(k+3)*lda;
-        
-        Scalar* C0 = C+ib+(n_end)*ldc;
-        
-                  a0 = pload<Packet>(A0);
-                  a1 = pload<Packet>(A1);
-        if(RK==4)
-        {
-          a2 = pload<Packet>(A2);
-          a3 = pload<Packet>(A3);
-        }
-        else
-        {
-          // workaround "may be used uninitialized in this function" warning
-          a2 = a3 = a0;
-        }
-        
-#define WORK(I) \
-                   c0 = pload<Packet>(C0+i+(I)*PacketSize);     \
-                   KMADD(c0, a0, b00, t0)                       \
-                   a0 = pload<Packet>(A0+i+(I+1)*PacketSize);   \
-                   KMADD(c0, a1, b10, t0)                       \
-                   a1 = pload<Packet>(A1+i+(I+1)*PacketSize);   \
-        if(RK==4){ KMADD(c0, a2, b20, t0)                      }\
-        if(RK==4){ a2 = pload<Packet>(A2+i+(I+1)*PacketSize);  }\
-        if(RK==4){ KMADD(c0, a3, b30, t0)                      }\
-        if(RK==4){ a3 = pload<Packet>(A3+i+(I+1)*PacketSize);  }\
-                   pstore(C0+i+(I)*PacketSize, c0);
-        
-        // aggressive vectorization and peeling
-        for(Index i=0; i<actual_b_end1; i+=PacketSize*8)
-        {
-          EIGEN_ASM_COMMENT("SPARSELU_GEMML_KERNEL2");
-          WORK(0);
-          WORK(1);
-          WORK(2);
-          WORK(3);
-          WORK(4);
-          WORK(5);
-          WORK(6);
-          WORK(7);
-        }
-        // vectorization only
-        for(Index i=actual_b_end1; i<actual_b_end2; i+=PacketSize)
-        {
-          WORK(0);
-        }
-        // remaining scalars
-        for(Index i=actual_b_end2; i<actual_b; ++i)
-        {
-          if(RK==4) 
-            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1]+A2[i]*Bc0[2]+A3[i]*Bc0[3];
-          else
-            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1];
-        }
-        
-        Bc0 += RK;
-#undef WORK
-      }
-    }
-    
-    // process the last columns of A, corresponding to the last rows of B
-    Index rd = d-d_end;
-    if(rd>0)
-    {
-      for(Index j=0; j<n; ++j)
-      {
-        enum {
-          Alignment = PacketSize>1 ? Aligned : 0
-        };
-        typedef Map<Matrix<Scalar,Dynamic,1>, Alignment > MapVector;
-        typedef Map<const Matrix<Scalar,Dynamic,1>, Alignment > ConstMapVector;
-        if(rd==1)       MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b);
-        
-        else if(rd==2)  MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b)
-                                                        + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b);
-        
-        else            MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b)
-                                                        + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b)
-                                                        + B[2+d_end+j*ldb] * ConstMapVector(A+(d_end+2)*lda+ib, actual_b);
-      }
-    }
-  
-  } // blocking on the rows of A and C
-}
-#undef KMADD
-
-} // namespace internal
-
-} // namespace Eigen
-
-#endif // EIGEN_SPARSELU_GEMM_KERNEL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
index 7aecbca..8df830b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
@@ -28,80 +28,73 @@
 #ifndef SPARSELU_HEAP_RELAX_SNODE_H
 #define SPARSELU_HEAP_RELAX_SNODE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
-/** 
+/**
  * \brief Identify the initial relaxed supernodes
- * 
- * This routine applied to a symmetric elimination tree. 
+ *
+ * This routine applied to a symmetric elimination tree.
  * It assumes that the matrix has been reordered according to the postorder of the etree
  * \param n The number of columns
- * \param et elimination tree 
- * \param relax_columns Maximum number of columns allowed in a relaxed snode 
+ * \param et elimination tree
+ * \param relax_columns Maximum number of columns allowed in a relaxed snode
  * \param descendants Number of descendants of each node in the etree
  * \param relax_end last column in a supernode
  */
 template <typename Scalar, typename StorageIndex>
-void SparseLUImpl<Scalar,StorageIndex>::heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end)
-{
-  
-  // The etree may not be postordered, but its heap ordered  
+void SparseLUImpl<Scalar, StorageIndex>::heap_relax_snode(const Index n, IndexVector& et, const Index relax_columns,
+                                                          IndexVector& descendants, IndexVector& relax_end) {
+  // The etree may not be postordered, but its heap ordered
   IndexVector post;
-  internal::treePostorder(StorageIndex(n), et, post); // Post order etree
-  IndexVector inv_post(n+1); 
-  for (StorageIndex i = 0; i < n+1; ++i) inv_post(post(i)) = i; // inv_post = post.inverse()???
-  
-  // Renumber etree in postorder 
+  internal::treePostorder(StorageIndex(n), et, post);  // Post order etree
+  IndexVector inv_post(n + 1);
+  for (StorageIndex i = 0; i < n + 1; ++i) inv_post(post(i)) = i;  // inv_post = post.inverse()???
+
+  // Renumber etree in postorder
   IndexVector iwork(n);
-  IndexVector et_save(n+1);
-  for (Index i = 0; i < n; ++i)
-  {
+  IndexVector et_save(n + 1);
+  for (Index i = 0; i < n; ++i) {
     iwork(post(i)) = post(et(i));
   }
-  et_save = et; // Save the original etree
-  et = iwork; 
-  
+  et_save = et;  // Save the original etree
+  et = iwork;
+
   // compute the number of descendants of each node in the etree
   relax_end.setConstant(emptyIdxLU);
-  Index j, parent; 
+  Index j, parent;
   descendants.setZero();
-  for (j = 0; j < n; j++) 
-  {
+  for (j = 0; j < n; j++) {
     parent = et(j);
-    if (parent != n) // not the dummy root
+    if (parent != n)  // not the dummy root
       descendants(parent) += descendants(j) + 1;
   }
   // Identify the relaxed supernodes by postorder traversal of the etree
-  Index snode_start; // beginning of a snode 
+  Index snode_start;  // beginning of a snode
   StorageIndex k;
-  StorageIndex l; 
-  for (j = 0; j < n; )
-  {
+  StorageIndex l;
+  for (j = 0; j < n;) {
     parent = et(j);
-    snode_start = j; 
-    while ( parent != n && descendants(parent) < relax_columns ) 
-    {
-      j = parent; 
+    snode_start = j;
+    while (parent != n && descendants(parent) < relax_columns) {
+      j = parent;
       parent = et(j);
     }
-    // Found a supernode in postordered etree, j is the last column 
+    // Found a supernode in postordered etree, j is the last column
     k = StorageIndex(n);
-    for (Index i = snode_start; i <= j; ++i)
-      k = (std::min)(k, inv_post(i));
+    for (Index i = snode_start; i <= j; ++i) k = (std::min)(k, inv_post(i));
     l = inv_post(j);
-    if ( (l - k) == (j - snode_start) )  // Same number of columns in the snode
+    if ((l - k) == (j - snode_start))  // Same number of columns in the snode
     {
       // This is also a supernode in the original etree
-      relax_end(k) = l; // Record last column 
-    }
-    else 
-    {
-      for (Index i = snode_start; i <= j; ++i) 
-      {
+      relax_end(k) = l;  // Record last column
+    } else {
+      for (Index i = snode_start; i <= j; ++i) {
         l = inv_post(i);
-        if (descendants(i) == 0) 
-        {
+        if (descendants(i) == 0) {
           relax_end(l) = l;
         }
       }
@@ -109,13 +102,13 @@
     j++;
     // Search for a new leaf
     while (descendants(j) != 0 && j < n) j++;
-  } // End postorder traversal of the etree
-  
+  }  // End postorder traversal of the etree
+
   // Recover the original etree
-  et = et_save; 
+  et = et_save;
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
-#endif // SPARSELU_HEAP_RELAX_SNODE_H
+}  // end namespace Eigen
+#endif  // SPARSELU_HEAP_RELAX_SNODE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
index 8c1b3e8..54bda0c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
@@ -11,120 +11,123 @@
 #ifndef SPARSELU_KERNEL_BMOD_H
 #define SPARSELU_KERNEL_BMOD_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
-  
-template <int SegSizeAtCompileTime> struct LU_kernel_bmod
-{
+
+template <int SegSizeAtCompileTime>
+struct LU_kernel_bmod {
   /** \internal
-    * \brief Performs numeric block updates from a given supernode to a single column
-    *
-    * \param segsize Size of the segment (and blocks ) to use for updates
-    * \param[in,out] dense Packed values of the original matrix
-    * \param tempv temporary vector to use for updates
-    * \param lusup array containing the supernodes
-    * \param lda Leading dimension in the supernode
-    * \param nrow Number of rows in the rectangular part of the supernode
-    * \param lsub compressed row subscripts of supernodes
-    * \param lptr pointer to the first column of the current supernode in lsub
-    * \param no_zeros Number of nonzeros elements before the diagonal part of the supernode
-    */
+   * \brief Performs numeric block updates from a given supernode to a single column
+   *
+   * \param segsize Size of the segment (and blocks ) to use for updates
+   * \param[in,out] dense Packed values of the original matrix
+   * \param tempv temporary vector to use for updates
+   * \param lusup array containing the supernodes
+   * \param lda Leading dimension in the supernode
+   * \param nrow Number of rows in the rectangular part of the supernode
+   * \param lsub compressed row subscripts of supernodes
+   * \param lptr pointer to the first column of the current supernode in lsub
+   * \param no_zeros Number of nonzeros elements before the diagonal part of the supernode
+   */
   template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
-  static EIGEN_DONT_INLINE void run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda,
-                                    const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros);
+  static EIGEN_DONT_INLINE void run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv,
+                                    ScalarVector& lusup, Index& luptr, const Index lda, const Index nrow,
+                                    IndexVector& lsub, const Index lptr, const Index no_zeros);
 };
 
 template <int SegSizeAtCompileTime>
 template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
-EIGEN_DONT_INLINE void LU_kernel_bmod<SegSizeAtCompileTime>::run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda,
-                                                                  const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros)
-{
+EIGEN_DONT_INLINE void LU_kernel_bmod<SegSizeAtCompileTime>::run(const Index segsize, BlockScalarVector& dense,
+                                                                 ScalarVector& tempv, ScalarVector& lusup, Index& luptr,
+                                                                 const Index lda, const Index nrow, IndexVector& lsub,
+                                                                 const Index lptr, const Index no_zeros) {
   typedef typename ScalarVector::Scalar Scalar;
   // First, copy U[*,j] segment from dense(*) to tempv(*)
-  // The result of triangular solve is in tempv[*]; 
-    // The result of matric-vector update is in dense[*]
-  Index isub = lptr + no_zeros; 
+  // The result of triangular solve is in tempv[*];
+  // The result of matric-vector update is in dense[*]
+  Index isub = lptr + no_zeros;
   Index i;
   Index irow;
-  for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++)
-  {
-    irow = lsub(isub); 
-    tempv(i) = dense(irow); 
-    ++isub; 
+  for (i = 0; i < ((SegSizeAtCompileTime == Dynamic) ? segsize : SegSizeAtCompileTime); i++) {
+    irow = lsub(isub);
+    tempv(i) = dense(irow);
+    ++isub;
   }
   // Dense triangular solve -- start effective triangle
-  luptr += lda * no_zeros + no_zeros; 
-  // Form Eigen matrix and vector 
-  Map<Matrix<Scalar,SegSizeAtCompileTime,SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) );
-  Map<Matrix<Scalar,SegSizeAtCompileTime,1> > u(tempv.data(), segsize);
-  
-  u = A.template triangularView<UnitLower>().solve(u); 
-  
-  // Dense matrix-vector product y <-- B*x 
+  luptr += lda * no_zeros + no_zeros;
+  // Form Eigen matrix and vector
+  Map<Matrix<Scalar, SegSizeAtCompileTime, SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > A(
+      &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda));
+  Map<Matrix<Scalar, SegSizeAtCompileTime, 1> > u(tempv.data(), segsize);
+
+  u = A.template triangularView<UnitLower>().solve(u);
+
+  // Dense matrix-vector product y <-- B*x
   luptr += segsize;
   const Index PacketSize = internal::packet_traits<Scalar>::size;
   Index ldl = internal::first_multiple(nrow, PacketSize);
-  Map<Matrix<Scalar,Dynamic,SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) );
-  Index aligned_offset = internal::first_default_aligned(tempv.data()+segsize, PacketSize);
-  Index aligned_with_B_offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize))%PacketSize;
-  Map<Matrix<Scalar,Dynamic,1>, 0, OuterStride<> > l(tempv.data()+segsize+aligned_offset+aligned_with_B_offset, nrow, OuterStride<>(ldl) );
-  
-  l.setZero();
-  internal::sparselu_gemm<Scalar>(l.rows(), l.cols(), B.cols(), B.data(), B.outerStride(), u.data(), u.outerStride(), l.data(), l.outerStride());
-  
-  // Scatter tempv[] into SPA dense[] as a temporary storage 
+  Map<Matrix<Scalar, Dynamic, SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > B(&(lusup.data()[luptr]), nrow,
+                                                                                    segsize, OuterStride<>(lda));
+  Index aligned_offset = internal::first_default_aligned(tempv.data() + segsize, PacketSize);
+  Index aligned_with_B_offset = (PacketSize - internal::first_default_aligned(B.data(), PacketSize)) % PacketSize;
+  Map<Matrix<Scalar, Dynamic, 1>, 0, OuterStride<> > l(tempv.data() + segsize + aligned_offset + aligned_with_B_offset,
+                                                       nrow, OuterStride<>(ldl));
+
+  l.noalias() = B * u;
+
+  // Scatter tempv[] into SPA dense[] as a temporary storage
   isub = lptr + no_zeros;
-  for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++)
-  {
-    irow = lsub(isub++); 
+  for (i = 0; i < ((SegSizeAtCompileTime == Dynamic) ? segsize : SegSizeAtCompileTime); i++) {
+    irow = lsub(isub++);
     dense(irow) = tempv(i);
   }
-  
+
   // Scatter l into SPA dense[]
-  for (i = 0; i < nrow; i++)
-  {
-    irow = lsub(isub++); 
+  for (i = 0; i < nrow; i++) {
+    irow = lsub(isub++);
     dense(irow) -= l(i);
-  } 
+  }
 }
 
-template <> struct LU_kernel_bmod<1>
-{
+template <>
+struct LU_kernel_bmod<1> {
   template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
-  static EIGEN_DONT_INLINE void run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr,
-                                    const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros);
+  static EIGEN_DONT_INLINE void run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/,
+                                    ScalarVector& lusup, Index& luptr, const Index lda, const Index nrow,
+                                    IndexVector& lsub, const Index lptr, const Index no_zeros);
 };
 
-
 template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
-EIGEN_DONT_INLINE void LU_kernel_bmod<1>::run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr,
-                                              const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros)
-{
+EIGEN_DONT_INLINE void LU_kernel_bmod<1>::run(const Index /*segsize*/, BlockScalarVector& dense,
+                                              ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr,
+                                              const Index lda, const Index nrow, IndexVector& lsub, const Index lptr,
+                                              const Index no_zeros) {
   typedef typename ScalarVector::Scalar Scalar;
   typedef typename IndexVector::Scalar StorageIndex;
   Scalar f = dense(lsub(lptr + no_zeros));
   luptr += lda * no_zeros + no_zeros + 1;
   const Scalar* a(lusup.data() + luptr);
-  const StorageIndex*  irow(lsub.data()+lptr + no_zeros + 1);
+  const StorageIndex* irow(lsub.data() + lptr + no_zeros + 1);
   Index i = 0;
-  for (; i+1 < nrow; i+=2)
-  {
+  for (; i + 1 < nrow; i += 2) {
     Index i0 = *(irow++);
     Index i1 = *(irow++);
     Scalar a0 = *(a++);
     Scalar a1 = *(a++);
     Scalar d0 = dense.coeff(i0);
     Scalar d1 = dense.coeff(i1);
-    d0 -= f*a0;
-    d1 -= f*a1;
+    d0 -= f * a0;
+    d1 -= f * a1;
     dense.coeffRef(i0) = d0;
     dense.coeffRef(i1) = d1;
   }
-  if(i<nrow)
-    dense.coeffRef(*(irow++)) -= f * *(a++);
+  if (i < nrow) dense.coeffRef(*(irow++)) -= f * *(a++);
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
-#endif // SPARSELU_KERNEL_BMOD_H
+}  // end namespace Eigen
+#endif  // SPARSELU_KERNEL_BMOD_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h
index f052001..505d982 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h
@@ -8,10 +8,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
- 
- * NOTE: This file is the modified version of [s,d,c,z]panel_bmod.c file in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]panel_bmod.c file in SuperLU
+
  * -- SuperLU routine (version 3.0) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -31,193 +31,185 @@
 #ifndef SPARSELU_PANEL_BMOD_H
 #define SPARSELU_PANEL_BMOD_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
 /**
  * \brief Performs numeric block updates (sup-panel) in topological order.
- * 
+ *
  * Before entering this routine, the original nonzeros in the panel
  * were already copied into the spa[m,w]
- * 
+ *
  * \param m number of rows in the matrix
  * \param w Panel size
  * \param jcol Starting  column of the panel
  * \param nseg Number of segments in the U part
- * \param dense Store the full representation of the panel 
- * \param tempv working array 
+ * \param dense Store the full representation of the panel
+ * \param tempv working array
  * \param segrep segment representative... first row in the segment
  * \param repfnz First nonzero rows
- * \param glu Global LU data. 
- * 
- * 
+ * \param glu Global LU data.
+ *
+ *
  */
 template <typename Scalar, typename StorageIndex>
-void SparseLUImpl<Scalar,StorageIndex>::panel_bmod(const Index m, const Index w, const Index jcol, 
-                                            const Index nseg, ScalarVector& dense, ScalarVector& tempv,
-                                            IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu)
-{
-  
-  Index ksub,jj,nextl_col; 
-  Index fsupc, nsupc, nsupr, nrow; 
-  Index krep, kfnz; 
-  Index lptr; // points to the row subscripts of a supernode 
-  Index luptr; // ...
-  Index segsize,no_zeros ; 
+void SparseLUImpl<Scalar, StorageIndex>::panel_bmod(const Index m, const Index w, const Index jcol, const Index nseg,
+                                                    ScalarVector& dense, ScalarVector& tempv, IndexVector& segrep,
+                                                    IndexVector& repfnz, GlobalLU_t& glu) {
+  Index ksub, jj, nextl_col;
+  Index fsupc, nsupc, nsupr, nrow;
+  Index krep, kfnz;
+  Index lptr;   // points to the row subscripts of a supernode
+  Index luptr;  // ...
+  Index segsize, no_zeros;
   // For each nonz supernode segment of U[*,j] in topological order
-  Index k = nseg - 1; 
+  Index k = nseg - 1;
   const Index PacketSize = internal::packet_traits<Scalar>::size;
-  
-  for (ksub = 0; ksub < nseg; ksub++)
-  { // For each updating supernode
+
+  for (ksub = 0; ksub < nseg; ksub++) {  // For each updating supernode
     /* krep = representative of current k-th supernode
      * fsupc =  first supernodal column
      * nsupc = number of columns in a supernode
      * nsupr = number of rows in a supernode
      */
-    krep = segrep(k); k--; 
-    fsupc = glu.xsup(glu.supno(krep)); 
-    nsupc = krep - fsupc + 1; 
-    nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 
-    nrow = nsupr - nsupc; 
-    lptr = glu.xlsub(fsupc); 
-    
+    krep = segrep(k);
+    k--;
+    fsupc = glu.xsup(glu.supno(krep));
+    nsupc = krep - fsupc + 1;
+    nsupr = glu.xlsub(fsupc + 1) - glu.xlsub(fsupc);
+    nrow = nsupr - nsupc;
+    lptr = glu.xlsub(fsupc);
+
     // loop over the panel columns to detect the actual number of columns and rows
     Index u_rows = 0;
     Index u_cols = 0;
-    for (jj = jcol; jj < jcol + w; jj++)
-    {
-      nextl_col = (jj-jcol) * m; 
-      VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
-      
-      kfnz = repfnz_col(krep); 
-      if ( kfnz == emptyIdxLU ) 
-        continue; // skip any zero segment
-      
+    for (jj = jcol; jj < jcol + w; jj++) {
+      nextl_col = (jj - jcol) * m;
+      VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m);  // First nonzero column index for each row
+
+      kfnz = repfnz_col(krep);
+      if (kfnz == emptyIdxLU) continue;  // skip any zero segment
+
       segsize = krep - kfnz + 1;
       u_cols++;
-      u_rows = (std::max)(segsize,u_rows);
+      u_rows = (std::max)(segsize, u_rows);
     }
-    
-    if(nsupc >= 2)
-    { 
+
+    if (nsupc >= 2) {
       Index ldu = internal::first_multiple<Index>(u_rows, PacketSize);
-      Map<ScalarMatrix, Aligned,  OuterStride<> > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu));
-      
+      Map<ScalarMatrix, Aligned, OuterStride<> > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu));
+
       // gather U
       Index u_col = 0;
-      for (jj = jcol; jj < jcol + w; jj++)
-      {
-        nextl_col = (jj-jcol) * m; 
-        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
-        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
-        
-        kfnz = repfnz_col(krep); 
-        if ( kfnz == emptyIdxLU ) 
-          continue; // skip any zero segment
-        
+      for (jj = jcol; jj < jcol + w; jj++) {
+        nextl_col = (jj - jcol) * m;
+        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m);  // First nonzero column index for each row
+        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m);   // Scatter/gather entire matrix column from/to here
+
+        kfnz = repfnz_col(krep);
+        if (kfnz == emptyIdxLU) continue;  // skip any zero segment
+
         segsize = krep - kfnz + 1;
-        luptr = glu.xlusup(fsupc);    
-        no_zeros = kfnz - fsupc; 
-        
+        luptr = glu.xlusup(fsupc);
+        no_zeros = kfnz - fsupc;
+
         Index isub = lptr + no_zeros;
-        Index off = u_rows-segsize;
-        for (Index i = 0; i < off; i++) U(i,u_col) = 0;
-        for (Index i = 0; i < segsize; i++)
-        {
-          Index irow = glu.lsub(isub); 
-          U(i+off,u_col) = dense_col(irow); 
-          ++isub; 
+        Index off = u_rows - segsize;
+        for (Index i = 0; i < off; i++) U(i, u_col) = 0;
+        for (Index i = 0; i < segsize; i++) {
+          Index irow = glu.lsub(isub);
+          U(i + off, u_col) = dense_col(irow);
+          ++isub;
         }
         u_col++;
       }
       // solve U = A^-1 U
       luptr = glu.xlusup(fsupc);
-      Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc);
+      Index lda = glu.xlusup(fsupc + 1) - glu.xlusup(fsupc);
       no_zeros = (krep - u_rows + 1) - fsupc;
       luptr += lda * no_zeros + no_zeros;
-      MappedMatrixBlock A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) );
+      MappedMatrixBlock A(glu.lusup.data() + luptr, u_rows, u_rows, OuterStride<>(lda));
       U = A.template triangularView<UnitLower>().solve(U);
-      
+
       // update
       luptr += u_rows;
-      MappedMatrixBlock B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) );
-      eigen_assert(tempv.size()>w*ldu + nrow*w + 1);
-      
+      MappedMatrixBlock B(glu.lusup.data() + luptr, nrow, u_rows, OuterStride<>(lda));
+      eigen_assert(tempv.size() > w * ldu + nrow * w + 1);
+
       Index ldl = internal::first_multiple<Index>(nrow, PacketSize);
-      Index offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize)) % PacketSize;
-      MappedMatrixBlock L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl));
-      
-      L.setZero();
-      internal::sparselu_gemm<Scalar>(L.rows(), L.cols(), B.cols(), B.data(), B.outerStride(), U.data(), U.outerStride(), L.data(), L.outerStride());
-      
+      Index offset = (PacketSize - internal::first_default_aligned(B.data(), PacketSize)) % PacketSize;
+      MappedMatrixBlock L(tempv.data() + w * ldu + offset, nrow, u_cols, OuterStride<>(ldl));
+
+      L.noalias() = B * U;
+
       // scatter U and L
       u_col = 0;
-      for (jj = jcol; jj < jcol + w; jj++)
-      {
-        nextl_col = (jj-jcol) * m; 
-        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
-        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
-        
-        kfnz = repfnz_col(krep); 
-        if ( kfnz == emptyIdxLU ) 
-          continue; // skip any zero segment
-        
+      for (jj = jcol; jj < jcol + w; jj++) {
+        nextl_col = (jj - jcol) * m;
+        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m);  // First nonzero column index for each row
+        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m);   // Scatter/gather entire matrix column from/to here
+
+        kfnz = repfnz_col(krep);
+        if (kfnz == emptyIdxLU) continue;  // skip any zero segment
+
         segsize = krep - kfnz + 1;
-        no_zeros = kfnz - fsupc; 
+        no_zeros = kfnz - fsupc;
         Index isub = lptr + no_zeros;
-        
-        Index off = u_rows-segsize;
-        for (Index i = 0; i < segsize; i++)
-        {
-          Index irow = glu.lsub(isub++); 
-          dense_col(irow) = U.coeff(i+off,u_col);
-          U.coeffRef(i+off,u_col) = 0;
+
+        Index off = u_rows - segsize;
+        for (Index i = 0; i < segsize; i++) {
+          Index irow = glu.lsub(isub++);
+          dense_col(irow) = U.coeff(i + off, u_col);
+          U.coeffRef(i + off, u_col) = 0;
         }
-        
+
         // Scatter l into SPA dense[]
-        for (Index i = 0; i < nrow; i++)
-        {
-          Index irow = glu.lsub(isub++); 
-          dense_col(irow) -= L.coeff(i,u_col);
-          L.coeffRef(i,u_col) = 0;
+        for (Index i = 0; i < nrow; i++) {
+          Index irow = glu.lsub(isub++);
+          dense_col(irow) -= L.coeff(i, u_col);
+          L.coeffRef(i, u_col) = 0;
         }
         u_col++;
       }
-    }
-    else // level 2 only
+    } else  // level 2 only
     {
       // Sequence through each column in the panel
-      for (jj = jcol; jj < jcol + w; jj++)
-      {
-        nextl_col = (jj-jcol) * m; 
-        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
-        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
-        
-        kfnz = repfnz_col(krep); 
-        if ( kfnz == emptyIdxLU ) 
-          continue; // skip any zero segment
-        
+      for (jj = jcol; jj < jcol + w; jj++) {
+        nextl_col = (jj - jcol) * m;
+        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m);  // First nonzero column index for each row
+        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m);   // Scatter/gather entire matrix column from/to here
+
+        kfnz = repfnz_col(krep);
+        if (kfnz == emptyIdxLU) continue;  // skip any zero segment
+
         segsize = krep - kfnz + 1;
         luptr = glu.xlusup(fsupc);
-        
-        Index lda = glu.xlusup(fsupc+1)-glu.xlusup(fsupc);// nsupr
-        
-        // Perform a trianglar solve and block update, 
+
+        Index lda = glu.xlusup(fsupc + 1) - glu.xlusup(fsupc);  // nsupr
+
+        // Perform a trianglar solve and block update,
         // then scatter the result of sup-col update to dense[]
-        no_zeros = kfnz - fsupc; 
-              if(segsize==1)  LU_kernel_bmod<1>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
-        else  if(segsize==2)  LU_kernel_bmod<2>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
-        else  if(segsize==3)  LU_kernel_bmod<3>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
-        else                  LU_kernel_bmod<Dynamic>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); 
-      } // End for each column in the panel 
+        no_zeros = kfnz - fsupc;
+        if (segsize == 1)
+          LU_kernel_bmod<1>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+        else if (segsize == 2)
+          LU_kernel_bmod<2>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+        else if (segsize == 3)
+          LU_kernel_bmod<3>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+        else
+          LU_kernel_bmod<Dynamic>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr,
+                                       no_zeros);
+      }  // End for each column in the panel
     }
-    
-  } // End for each updating supernode
-} // end panel bmod
 
-} // end namespace internal
+  }  // End for each updating supernode
+}  // end panel bmod
 
-} // end namespace Eigen
+}  // end namespace internal
 
-#endif // SPARSELU_PANEL_BMOD_H
+}  // end namespace Eigen
+
+#endif  // SPARSELU_PANEL_BMOD_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h
index 155df73..df31548 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h
@@ -7,10 +7,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
- 
- * NOTE: This file is the modified version of [s,d,c,z]panel_dfs.c file in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]panel_dfs.c file in SuperLU
+
  * -- SuperLU routine (version 2.0) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -30,22 +30,20 @@
 #ifndef SPARSELU_PANEL_DFS_H
 #define SPARSELU_PANEL_DFS_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
-  
-template<typename IndexVector>
-struct panel_dfs_traits
-{
+
+template <typename IndexVector>
+struct panel_dfs_traits {
   typedef typename IndexVector::Scalar StorageIndex;
-  panel_dfs_traits(Index jcol, StorageIndex* marker)
-    : m_jcol(jcol), m_marker(marker)
-  {}
-  bool update_segrep(Index krep, StorageIndex jj)
-  {
-    if(m_marker[krep]<m_jcol)
-    {
-      m_marker[krep] = jj; 
+  panel_dfs_traits(Index jcol, StorageIndex* marker) : m_jcol(jcol), m_marker(marker) {}
+  bool update_segrep(Index krep, StorageIndex jj) {
+    if (m_marker[krep] < m_jcol) {
+      m_marker[krep] = jj;
       return true;
     }
     return false;
@@ -56,147 +54,127 @@
   StorageIndex* m_marker;
 };
 
-
 template <typename Scalar, typename StorageIndex>
 template <typename Traits>
-void SparseLUImpl<Scalar,StorageIndex>::dfs_kernel(const StorageIndex jj, IndexVector& perm_r,
-                   Index& nseg, IndexVector& panel_lsub, IndexVector& segrep,
-                   Ref<IndexVector> repfnz_col, IndexVector& xprune, Ref<IndexVector> marker, IndexVector& parent,
-                   IndexVector& xplore, GlobalLU_t& glu,
-                   Index& nextl_col, Index krow, Traits& traits
-                  )
-{
-  
+void SparseLUImpl<Scalar, StorageIndex>::dfs_kernel(const StorageIndex jj, IndexVector& perm_r, Index& nseg,
+                                                    IndexVector& panel_lsub, IndexVector& segrep,
+                                                    Ref<IndexVector> repfnz_col, IndexVector& xprune,
+                                                    Ref<IndexVector> marker, IndexVector& parent, IndexVector& xplore,
+                                                    GlobalLU_t& glu, Index& nextl_col, Index krow, Traits& traits) {
   StorageIndex kmark = marker(krow);
-      
+
   // For each unmarked krow of jj
-  marker(krow) = jj; 
-  StorageIndex kperm = perm_r(krow); 
-  if (kperm == emptyIdxLU ) {
+  marker(krow) = jj;
+  StorageIndex kperm = perm_r(krow);
+  if (kperm == emptyIdxLU) {
     // krow is in L : place it in structure of L(*, jj)
     panel_lsub(nextl_col++) = StorageIndex(krow);  // krow is indexed into A
-    
+
     traits.mem_expand(panel_lsub, nextl_col, kmark);
-  }
-  else 
-  {
+  } else {
     // krow is in U : if its supernode-representative krep
     // has been explored, update repfnz(*)
     // krep = supernode representative of the current row
-    StorageIndex krep = glu.xsup(glu.supno(kperm)+1) - 1; 
+    StorageIndex krep = glu.xsup(glu.supno(kperm) + 1) - 1;
     // First nonzero element in the current column:
-    StorageIndex myfnz = repfnz_col(krep); 
-    
-    if (myfnz != emptyIdxLU )
-    {
+    StorageIndex myfnz = repfnz_col(krep);
+
+    if (myfnz != emptyIdxLU) {
       // Representative visited before
-      if (myfnz > kperm ) repfnz_col(krep) = kperm; 
-      
-    }
-    else 
-    {
+      if (myfnz > kperm) repfnz_col(krep) = kperm;
+
+    } else {
       // Otherwise, perform dfs starting at krep
-      StorageIndex oldrep = emptyIdxLU; 
-      parent(krep) = oldrep; 
-      repfnz_col(krep) = kperm; 
-      StorageIndex xdfs =  glu.xlsub(krep); 
-      Index maxdfs = xprune(krep); 
-      
+      StorageIndex oldrep = emptyIdxLU;
+      parent(krep) = oldrep;
+      repfnz_col(krep) = kperm;
+      StorageIndex xdfs = glu.xlsub(krep);
+      Index maxdfs = xprune(krep);
+
       StorageIndex kpar;
-      do 
-      {
+      do {
         // For each unmarked kchild of krep
-        while (xdfs < maxdfs) 
-        {
-          StorageIndex kchild = glu.lsub(xdfs); 
-          xdfs++; 
-          StorageIndex chmark = marker(kchild); 
-          
-          if (chmark != jj ) 
-          {
-            marker(kchild) = jj; 
-            StorageIndex chperm = perm_r(kchild); 
-            
-            if (chperm == emptyIdxLU) 
-            {
+        while (xdfs < maxdfs) {
+          StorageIndex kchild = glu.lsub(xdfs);
+          xdfs++;
+          StorageIndex chmark = marker(kchild);
+
+          if (chmark != jj) {
+            marker(kchild) = jj;
+            StorageIndex chperm = perm_r(kchild);
+
+            if (chperm == emptyIdxLU) {
               // case kchild is in L: place it in L(*, j)
               panel_lsub(nextl_col++) = kchild;
               traits.mem_expand(panel_lsub, nextl_col, chmark);
-            }
-            else
-            {
+            } else {
               // case kchild is in U :
-              // chrep = its supernode-rep. If its rep has been explored, 
+              // chrep = its supernode-rep. If its rep has been explored,
               // update its repfnz(*)
-              StorageIndex chrep = glu.xsup(glu.supno(chperm)+1) - 1; 
-              myfnz = repfnz_col(chrep); 
-              
-              if (myfnz != emptyIdxLU) 
-              { // Visited before 
-                if (myfnz > chperm) 
-                  repfnz_col(chrep) = chperm; 
-              }
-              else 
-              { // Cont. dfs at snode-rep of kchild
-                xplore(krep) = xdfs; 
-                oldrep = krep; 
-                krep = chrep; // Go deeper down G(L)
-                parent(krep) = oldrep; 
-                repfnz_col(krep) = chperm; 
-                xdfs = glu.xlsub(krep); 
-                maxdfs = xprune(krep); 
-                
-              } // end if myfnz != -1
-            } // end if chperm == -1 
-                
-          } // end if chmark !=jj
-        } // end while xdfs < maxdfs
-        
+              StorageIndex chrep = glu.xsup(glu.supno(chperm) + 1) - 1;
+              myfnz = repfnz_col(chrep);
+
+              if (myfnz != emptyIdxLU) {  // Visited before
+                if (myfnz > chperm) repfnz_col(chrep) = chperm;
+              } else {  // Cont. dfs at snode-rep of kchild
+                xplore(krep) = xdfs;
+                oldrep = krep;
+                krep = chrep;  // Go deeper down G(L)
+                parent(krep) = oldrep;
+                repfnz_col(krep) = chperm;
+                xdfs = glu.xlsub(krep);
+                maxdfs = xprune(krep);
+
+              }  // end if myfnz != -1
+            }    // end if chperm == -1
+
+          }  // end if chmark !=jj
+        }    // end while xdfs < maxdfs
+
         // krow has no more unexplored nbrs :
-        //    Place snode-rep krep in postorder DFS, if this 
-        //    segment is seen for the first time. (Note that 
+        //    Place snode-rep krep in postorder DFS, if this
+        //    segment is seen for the first time. (Note that
         //    "repfnz(krep)" may change later.)
         //    Baktrack dfs to its parent
-        if(traits.update_segrep(krep,jj))
-        //if (marker1(krep) < jcol )
+        if (traits.update_segrep(krep, jj))
+        // if (marker1(krep) < jcol )
         {
-          segrep(nseg) = krep; 
-          ++nseg; 
-          //marker1(krep) = jj; 
+          segrep(nseg) = krep;
+          ++nseg;
+          // marker1(krep) = jj;
         }
-        
-        kpar = parent(krep); // Pop recursion, mimic recursion 
-        if (kpar == emptyIdxLU) 
-          break; // dfs done 
-        krep = kpar; 
-        xdfs = xplore(krep); 
-        maxdfs = xprune(krep); 
 
-      } while (kpar != emptyIdxLU); // Do until empty stack 
-      
-    } // end if (myfnz = -1)
+        kpar = parent(krep);            // Pop recursion, mimic recursion
+        if (kpar == emptyIdxLU) break;  // dfs done
+        krep = kpar;
+        xdfs = xplore(krep);
+        maxdfs = xprune(krep);
 
-  } // end if (kperm == -1)   
+      } while (kpar != emptyIdxLU);  // Do until empty stack
+
+    }  // end if (myfnz = -1)
+
+  }  // end if (kperm == -1)
 }
 
 /**
  * \brief Performs a symbolic factorization on a panel of columns [jcol, jcol+w)
- * 
+ *
  * A supernode representative is the last column of a supernode.
  * The nonzeros in U[*,j] are segments that end at supernodes representatives
- * 
- * The routine returns a list of the supernodal representatives 
- * in topological order of the dfs that generates them. This list is 
- * a superset of the topological order of each individual column within 
+ *
+ * The routine returns a list of the supernodal representatives
+ * in topological order of the dfs that generates them. This list is
+ * a superset of the topological order of each individual column within
  * the panel.
- * The location of the first nonzero in each supernodal segment 
- * (supernodal entry location) is also returned. Each column has 
- * a separate list for this purpose. 
- * 
+ * The location of the first nonzero in each supernodal segment
+ * (supernodal entry location) is also returned. Each column has
+ * a separate list for this purpose.
+ *
  * Two markers arrays are used for dfs :
  *    marker[i] == jj, if i was visited during dfs of current column jj;
- *    marker1[i] >= jcol, if i was visited by earlier columns in this panel; 
- * 
+ *    marker1[i] >= jcol, if i was visited by earlier columns in this panel;
+ *
  * \param[in] m number of rows in the matrix
  * \param[in] w Panel size
  * \param[in] jcol Starting  column of the panel
@@ -204,7 +182,7 @@
  * \param[in] perm_r Row permutation
  * \param[out] nseg Number of U segments
  * \param[out] dense Accumulate the column vectors of the panel
- * \param[out] panel_lsub Subscripts of the row in the panel 
+ * \param[out] panel_lsub Subscripts of the row in the panel
  * \param[out] segrep Segment representative i.e first nonzero row of each segment
  * \param[out] repfnz First nonzero location in each row
  * \param[out] xprune The pruned elimination tree
@@ -212,47 +190,46 @@
  * \param  parent The elimination tree
  * \param xplore work vector
  * \param glu The global data structure
- * 
+ *
  */
 
 template <typename Scalar, typename StorageIndex>
-void SparseLUImpl<Scalar,StorageIndex>::panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu)
-{
-  Index nextl_col; // Next available position in panel_lsub[*,jj] 
-  
-  // Initialize pointers 
-  VectorBlock<IndexVector> marker1(marker, m, m); 
-  nseg = 0; 
-  
+void SparseLUImpl<Scalar, StorageIndex>::panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A,
+                                                   IndexVector& perm_r, Index& nseg, ScalarVector& dense,
+                                                   IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz,
+                                                   IndexVector& xprune, IndexVector& marker, IndexVector& parent,
+                                                   IndexVector& xplore, GlobalLU_t& glu) {
+  Index nextl_col;  // Next available position in panel_lsub[*,jj]
+
+  // Initialize pointers
+  VectorBlock<IndexVector> marker1(marker, m, m);
+  nseg = 0;
+
   panel_dfs_traits<IndexVector> traits(jcol, marker1.data());
-  
-  // For each column in the panel 
-  for (StorageIndex jj = StorageIndex(jcol); jj < jcol + w; jj++) 
-  {
-    nextl_col = (jj - jcol) * m; 
-    
-    VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero location in each row
-    VectorBlock<ScalarVector> dense_col(dense,nextl_col, m); // Accumulate a column vector here
-    
-    
+
+  // For each column in the panel
+  for (StorageIndex jj = StorageIndex(jcol); jj < jcol + w; jj++) {
+    nextl_col = (jj - jcol) * m;
+
+    VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m);  // First nonzero location in each row
+    VectorBlock<ScalarVector> dense_col(dense, nextl_col, m);   // Accumulate a column vector here
+
     // For each nnz in A[*, jj] do depth first search
-    for (typename MatrixType::InnerIterator it(A, jj); it; ++it)
-    {
-      Index krow = it.row(); 
+    for (typename MatrixType::InnerIterator it(A, jj); it; ++it) {
+      Index krow = it.row();
       dense_col(krow) = it.value();
-      
-      StorageIndex kmark = marker(krow); 
-      if (kmark == jj) 
-        continue; // krow visited before, go to the next nonzero
-      
-      dfs_kernel(jj, perm_r, nseg, panel_lsub, segrep, repfnz_col, xprune, marker, parent,
-                   xplore, glu, nextl_col, krow, traits);
-    }// end for nonzeros in column jj
-    
-  } // end for column jj
+
+      StorageIndex kmark = marker(krow);
+      if (kmark == jj) continue;  // krow visited before, go to the next nonzero
+
+      dfs_kernel(jj, perm_r, nseg, panel_lsub, segrep, repfnz_col, xprune, marker, parent, xplore, glu, nextl_col, krow,
+                 traits);
+    }  // end for nonzeros in column jj
+
+  }  // end for column jj
 }
 
-} // end namespace internal
-} // end namespace Eigen
+}  // end namespace internal
+}  // end namespace Eigen
 
-#endif // SPARSELU_PANEL_DFS_H
+#endif  // SPARSELU_PANEL_DFS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h
index a86dac9..ada511e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h
@@ -7,10 +7,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
- 
- * NOTE: This file is the modified version of xpivotL.c file in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of xpivotL.c file in SuperLU
+
  * -- SuperLU routine (version 3.0) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -30,12 +30,15 @@
 #ifndef SPARSELU_PIVOTL_H
 #define SPARSELU_PIVOTL_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
-  
+
 /**
  * \brief Performs the numerical pivotin on the current column of L, and the CDIV operation.
- * 
+ *
  * Pivot policy :
  * (1) Compute thresh = u * max_(i>=j) abs(A_ij);
  * (2) IF user specifies pivot row k and abs(A_kj) >= thresh THEN
@@ -44,64 +47,63 @@
  *           pivot row = j;
  *       ELSE
  *           pivot row = m;
- * 
+ *
  *   Note: If you absolutely want to use a given pivot order, then set u=0.0.
- * 
+ *
  * \param jcol The current column of L
  * \param diagpivotthresh diagonal pivoting threshold
  * \param[in,out] perm_r Row permutation (threshold pivoting)
  * \param[in] iperm_c column permutation - used to finf diagonal of Pc*A*Pc'
  * \param[out] pivrow  The pivot row
  * \param glu Global LU data
- * \return 0 if success, i > 0 if U(i,i) is exactly zero 
- * 
+ * \return 0 if success, i > 0 if U(i,i) is exactly zero
+ *
  */
 template <typename Scalar, typename StorageIndex>
-Index SparseLUImpl<Scalar,StorageIndex>::pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu)
-{
-  
-  Index fsupc = (glu.xsup)((glu.supno)(jcol)); // First column in the supernode containing the column jcol
-  Index nsupc = jcol - fsupc; // Number of columns in the supernode portion, excluding jcol; nsupc >=0
-  Index lptr = glu.xlsub(fsupc); // pointer to the starting location of the row subscripts for this supernode portion
-  Index nsupr = glu.xlsub(fsupc+1) - lptr; // Number of rows in the supernode
-  Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc); // leading dimension
-  Scalar* lu_sup_ptr = &(glu.lusup.data()[glu.xlusup(fsupc)]); // Start of the current supernode
-  Scalar* lu_col_ptr = &(glu.lusup.data()[glu.xlusup(jcol)]); // Start of jcol in the supernode
-  StorageIndex* lsub_ptr = &(glu.lsub.data()[lptr]); // Start of row indices of the supernode
-  
-  // Determine the largest abs numerical value for partial pivoting 
-  Index diagind = iperm_c(jcol); // diagonal index 
+Index SparseLUImpl<Scalar, StorageIndex>::pivotL(const Index jcol, const RealScalar& diagpivotthresh,
+                                                 IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow,
+                                                 GlobalLU_t& glu) {
+  Index fsupc = (glu.xsup)((glu.supno)(jcol));  // First column in the supernode containing the column jcol
+  Index nsupc = jcol - fsupc;                   // Number of columns in the supernode portion, excluding jcol; nsupc >=0
+  Index lptr = glu.xlsub(fsupc);  // pointer to the starting location of the row subscripts for this supernode portion
+  Index nsupr = glu.xlsub(fsupc + 1) - lptr;                    // Number of rows in the supernode
+  Index lda = glu.xlusup(fsupc + 1) - glu.xlusup(fsupc);        // leading dimension
+  Scalar* lu_sup_ptr = &(glu.lusup.data()[glu.xlusup(fsupc)]);  // Start of the current supernode
+  Scalar* lu_col_ptr = &(glu.lusup.data()[glu.xlusup(jcol)]);   // Start of jcol in the supernode
+  StorageIndex* lsub_ptr = &(glu.lsub.data()[lptr]);            // Start of row indices of the supernode
+
+  // Determine the largest abs numerical value for partial pivoting
+  Index diagind = iperm_c(jcol);  // diagonal index
   RealScalar pivmax(-1.0);
-  Index pivptr = nsupc; 
-  Index diag = emptyIdxLU; 
+  Index pivptr = nsupc;
+  Index diag = emptyIdxLU;
   RealScalar rtemp;
-  Index isub, icol, itemp, k; 
+  Index isub, icol, itemp, k;
   for (isub = nsupc; isub < nsupr; ++isub) {
     using std::abs;
     rtemp = abs(lu_col_ptr[isub]);
     if (rtemp > pivmax) {
-      pivmax = rtemp; 
+      pivmax = rtemp;
       pivptr = isub;
-    } 
+    }
     if (lsub_ptr[isub] == diagind) diag = isub;
   }
-  
+
   // Test for singularity
-  if ( pivmax <= RealScalar(0.0) ) {
+  if (pivmax <= RealScalar(0.0)) {
     // if pivmax == -1, the column is structurally empty, otherwise it is only numerically zero
     pivrow = pivmax < RealScalar(0.0) ? diagind : lsub_ptr[pivptr];
     perm_r(pivrow) = StorageIndex(jcol);
-    return (jcol+1);
+    return (jcol + 1);
   }
-  
-  RealScalar thresh = diagpivotthresh * pivmax; 
-  
-  // Choose appropriate pivotal element 
-  
+
+  RealScalar thresh = diagpivotthresh * pivmax;
+
+  // Choose appropriate pivotal element
+
   {
     // Test if the diagonal element can be used as a pivot (given the threshold value)
-    if (diag >= 0 ) 
-    {
+    if (diag >= 0) {
       // Diagonal element exists
       using std::abs;
       rtemp = abs(lu_col_ptr[diag]);
@@ -109,29 +111,26 @@
     }
     pivrow = lsub_ptr[pivptr];
   }
-  
+
   // Record pivot row
   perm_r(pivrow) = StorageIndex(jcol);
   // Interchange row subscripts
-  if (pivptr != nsupc )
-  {
-    std::swap( lsub_ptr[pivptr], lsub_ptr[nsupc] );
+  if (pivptr != nsupc) {
+    std::swap(lsub_ptr[pivptr], lsub_ptr[nsupc]);
     // Interchange numerical values as well, for the two rows in the whole snode
     // such that L is indexed the same way as A
-    for (icol = 0; icol <= nsupc; icol++)
-    {
-      itemp = pivptr + icol * lda; 
+    for (icol = 0; icol <= nsupc; icol++) {
+      itemp = pivptr + icol * lda;
       std::swap(lu_sup_ptr[itemp], lu_sup_ptr[nsupc + icol * lda]);
     }
   }
   // cdiv operations
   Scalar temp = Scalar(1.0) / lu_col_ptr[nsupc];
-  for (k = nsupc+1; k < nsupr; k++)
-    lu_col_ptr[k] *= temp; 
+  for (k = nsupc + 1; k < nsupr; k++) lu_col_ptr[k] *= temp;
   return 0;
 }
 
-} // end namespace internal
-} // end namespace Eigen
+}  // end namespace internal
+}  // end namespace Eigen
 
-#endif // SPARSELU_PIVOTL_H
+#endif  // SPARSELU_PIVOTL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h
index ad32fed..4f51d59 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h
@@ -7,10 +7,10 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* 
- 
- * NOTE: This file is the modified version of [s,d,c,z]pruneL.c file in SuperLU 
- 
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]pruneL.c file in SuperLU
+
  * -- SuperLU routine (version 2.0) --
  * Univ. of California Berkeley, Xerox Palo Alto Research Center,
  * and Lawrence Berkeley National Lab.
@@ -30,6 +30,9 @@
 #ifndef SPARSELU_PRUNEL_H
 #define SPARSELU_PRUNEL_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
@@ -37,100 +40,91 @@
  * \brief Prunes the L-structure.
  *
  * It prunes the L-structure  of supernodes whose L-structure contains the current pivot row "pivrow"
- * 
- * 
+ *
+ *
  * \param jcol The current column of L
  * \param[in] perm_r Row permutation
  * \param[out] pivrow  The pivot row
  * \param nseg Number of segments
- * \param segrep 
+ * \param segrep
  * \param repfnz
- * \param[out] xprune 
+ * \param[out] xprune
  * \param glu Global LU data
- * 
+ *
  */
 template <typename Scalar, typename StorageIndex>
-void SparseLUImpl<Scalar,StorageIndex>::pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg,
-                                               const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu)
-{
+void SparseLUImpl<Scalar, StorageIndex>::pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow,
+                                                const Index nseg, const IndexVector& segrep, BlockIndexVector repfnz,
+                                                IndexVector& xprune, GlobalLU_t& glu) {
   // For each supernode-rep irep in U(*,j]
-  Index jsupno = glu.supno(jcol); 
-  Index i,irep,irep1; 
-  bool movnum, do_prune = false; 
-  Index kmin = 0, kmax = 0, minloc, maxloc,krow; 
-  for (i = 0; i < nseg; i++)
-  {
-    irep = segrep(i); 
-    irep1 = irep + 1; 
-    do_prune = false; 
-    
-    // Don't prune with a zero U-segment 
-    if (repfnz(irep) == emptyIdxLU) continue; 
-    
+  Index jsupno = glu.supno(jcol);
+  Index i, irep, irep1;
+  bool movnum, do_prune = false;
+  Index kmin = 0, kmax = 0, minloc, maxloc, krow;
+  for (i = 0; i < nseg; i++) {
+    irep = segrep(i);
+    irep1 = irep + 1;
+    do_prune = false;
+
+    // Don't prune with a zero U-segment
+    if (repfnz(irep) == emptyIdxLU) continue;
+
     // If a snode overlaps with the next panel, then the U-segment
-    // is fragmented into two parts -- irep and irep1. We should let 
-    // pruning occur at the rep-column in irep1s snode. 
-    if (glu.supno(irep) == glu.supno(irep1) ) continue; // don't prune 
-    
+    // is fragmented into two parts -- irep and irep1. We should let
+    // pruning occur at the rep-column in irep1s snode.
+    if (glu.supno(irep) == glu.supno(irep1)) continue;  // don't prune
+
     // If it has not been pruned & it has a nonz in row L(pivrow,i)
-    if (glu.supno(irep) != jsupno )
-    {
-      if ( xprune (irep) >= glu.xlsub(irep1) )
-      {
+    if (glu.supno(irep) != jsupno) {
+      if (xprune(irep) >= glu.xlsub(irep1)) {
         kmin = glu.xlsub(irep);
-        kmax = glu.xlsub(irep1) - 1; 
-        for (krow = kmin; krow <= kmax; krow++)
-        {
-          if (glu.lsub(krow) == pivrow) 
-          {
-            do_prune = true; 
-            break; 
+        kmax = glu.xlsub(irep1) - 1;
+        for (krow = kmin; krow <= kmax; krow++) {
+          if (glu.lsub(krow) == pivrow) {
+            do_prune = true;
+            break;
           }
         }
       }
-      
-      if (do_prune) 
-      {
+
+      if (do_prune) {
         // do a quicksort-type partition
         // movnum=true means that the num values have to be exchanged
-        movnum = false; 
-        if (irep == glu.xsup(glu.supno(irep)) ) // Snode of size 1 
-          movnum = true; 
-        
-        while (kmin <= kmax)
-        {
+        movnum = false;
+        if (irep == glu.xsup(glu.supno(irep)))  // Snode of size 1
+          movnum = true;
+
+        while (kmin <= kmax) {
           if (perm_r(glu.lsub(kmax)) == emptyIdxLU)
-            kmax--; 
-          else if ( perm_r(glu.lsub(kmin)) != emptyIdxLU)
+            kmax--;
+          else if (perm_r(glu.lsub(kmin)) != emptyIdxLU)
             kmin++;
-          else 
-          {
+          else {
             // kmin below pivrow (not yet pivoted), and kmax
             // above pivrow: interchange the two suscripts
-            std::swap(glu.lsub(kmin), glu.lsub(kmax)); 
-            
-            // If the supernode has only one column, then we 
+            std::swap(glu.lsub(kmin), glu.lsub(kmax));
+
+            // If the supernode has only one column, then we
             // only keep one set of subscripts. For any subscript
-            // intercnahge performed, similar interchange must be 
-            // done on the numerical values. 
-            if (movnum) 
-            {
-              minloc = glu.xlusup(irep) + ( kmin - glu.xlsub(irep) ); 
-              maxloc = glu.xlusup(irep) + ( kmax - glu.xlsub(irep) ); 
-              std::swap(glu.lusup(minloc), glu.lusup(maxloc)); 
+            // intercnahge performed, similar interchange must be
+            // done on the numerical values.
+            if (movnum) {
+              minloc = glu.xlusup(irep) + (kmin - glu.xlsub(irep));
+              maxloc = glu.xlusup(irep) + (kmax - glu.xlsub(irep));
+              std::swap(glu.lusup(minloc), glu.lusup(maxloc));
             }
             kmin++;
             kmax--;
           }
-        } // end while 
-        
-        xprune(irep) = StorageIndex(kmin);  //Pruning 
-      } // end if do_prune 
-    } // end pruning 
-  } // End for each U-segment
+        }  // end while
+
+        xprune(irep) = StorageIndex(kmin);  // Pruning
+      }                                     // end if do_prune
+    }                                       // end pruning
+  }                                         // End for each U-segment
 }
 
-} // end namespace internal
-} // end namespace Eigen
+}  // end namespace internal
+}  // end namespace Eigen
 
-#endif // SPARSELU_PRUNEL_H
+#endif  // SPARSELU_PRUNEL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h
index c408d01..df3869e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h
@@ -28,56 +28,54 @@
 #ifndef SPARSELU_RELAX_SNODE_H
 #define SPARSELU_RELAX_SNODE_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
- 
-/** 
+
+/**
  * \brief Identify the initial relaxed supernodes
- * 
- * This routine is applied to a column elimination tree. 
+ *
+ * This routine is applied to a column elimination tree.
  * It assumes that the matrix has been reordered according to the postorder of the etree
  * \param n  the number of columns
- * \param et elimination tree 
- * \param relax_columns Maximum number of columns allowed in a relaxed snode 
+ * \param et elimination tree
+ * \param relax_columns Maximum number of columns allowed in a relaxed snode
  * \param descendants Number of descendants of each node in the etree
  * \param relax_end last column in a supernode
  */
 template <typename Scalar, typename StorageIndex>
-void SparseLUImpl<Scalar,StorageIndex>::relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end)
-{
-  
+void SparseLUImpl<Scalar, StorageIndex>::relax_snode(const Index n, IndexVector& et, const Index relax_columns,
+                                                     IndexVector& descendants, IndexVector& relax_end) {
   // compute the number of descendants of each node in the etree
-  Index parent; 
+  Index parent;
   relax_end.setConstant(emptyIdxLU);
   descendants.setZero();
-  for (Index j = 0; j < n; j++) 
-  {
+  for (Index j = 0; j < n; j++) {
     parent = et(j);
-    if (parent != n) // not the dummy root
+    if (parent != n)  // not the dummy root
       descendants(parent) += descendants(j) + 1;
   }
   // Identify the relaxed supernodes by postorder traversal of the etree
-  Index snode_start; // beginning of a snode 
-  for (Index j = 0; j < n; )
-  {
+  Index snode_start;  // beginning of a snode
+  for (Index j = 0; j < n;) {
     parent = et(j);
-    snode_start = j; 
-    while ( parent != n && descendants(parent) < relax_columns ) 
-    {
-      j = parent; 
+    snode_start = j;
+    while (parent != n && descendants(parent) < relax_columns) {
+      j = parent;
       parent = et(j);
     }
-    // Found a supernode in postordered etree, j is the last column 
-    relax_end(snode_start) = StorageIndex(j); // Record last column
+    // Found a supernode in postordered etree, j is the last column
+    relax_end(snode_start) = StorageIndex(j);  // Record last column
     j++;
     // Search for a new leaf
     while (descendants(j) != 0 && j < n) j++;
-  } // End postorder traversal of the etree
-  
+  }  // End postorder traversal of the etree
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/InternalHeaderCheck.h
new file mode 100644
index 0000000..0564e93
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_SPARSEQR_MODULE_H
+#error "Please include Eigen/SparseQR instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h
index d1fb96f..3e3352f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h
@@ -11,469 +11,457 @@
 #ifndef EIGEN_SPARSE_QR_H
 #define EIGEN_SPARSE_QR_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename MatrixType, typename OrderingType> class SparseQR;
-template<typename SparseQRType> struct SparseQRMatrixQReturnType;
-template<typename SparseQRType> struct SparseQRMatrixQTransposeReturnType;
-template<typename SparseQRType, typename Derived> struct SparseQR_QProduct;
+template <typename MatrixType, typename OrderingType>
+class SparseQR;
+template <typename SparseQRType>
+struct SparseQRMatrixQReturnType;
+template <typename SparseQRType>
+struct SparseQRMatrixQTransposeReturnType;
+template <typename SparseQRType, typename Derived>
+struct SparseQR_QProduct;
 namespace internal {
-  template <typename SparseQRType> struct traits<SparseQRMatrixQReturnType<SparseQRType> >
-  {
-    typedef typename SparseQRType::MatrixType ReturnType;
-    typedef typename ReturnType::StorageIndex StorageIndex;
-    typedef typename ReturnType::StorageKind StorageKind;
-    enum {
-      RowsAtCompileTime = Dynamic,
-      ColsAtCompileTime = Dynamic
-    };
-  };
-  template <typename SparseQRType> struct traits<SparseQRMatrixQTransposeReturnType<SparseQRType> >
-  {
-    typedef typename SparseQRType::MatrixType ReturnType;
-  };
-  template <typename SparseQRType, typename Derived> struct traits<SparseQR_QProduct<SparseQRType, Derived> >
-  {
-    typedef typename Derived::PlainObject ReturnType;
-  };
-} // End namespace internal
+template <typename SparseQRType>
+struct traits<SparseQRMatrixQReturnType<SparseQRType> > {
+  typedef typename SparseQRType::MatrixType ReturnType;
+  typedef typename ReturnType::StorageIndex StorageIndex;
+  typedef typename ReturnType::StorageKind StorageKind;
+  enum { RowsAtCompileTime = Dynamic, ColsAtCompileTime = Dynamic };
+};
+template <typename SparseQRType>
+struct traits<SparseQRMatrixQTransposeReturnType<SparseQRType> > {
+  typedef typename SparseQRType::MatrixType ReturnType;
+};
+template <typename SparseQRType, typename Derived>
+struct traits<SparseQR_QProduct<SparseQRType, Derived> > {
+  typedef typename Derived::PlainObject ReturnType;
+};
+}  // End namespace internal
 
 /**
-  * \ingroup SparseQR_Module
-  * \class SparseQR
-  * \brief Sparse left-looking QR factorization with numerical column pivoting
-  * 
-  * This class implements a left-looking QR decomposition of sparse matrices
-  * with numerical column pivoting.
-  * When a column has a norm less than a given tolerance
-  * it is implicitly permuted to the end. The QR factorization thus obtained is 
-  * given by A*P = Q*R where R is upper triangular or trapezoidal. 
-  * 
-  * P is the column permutation which is the product of the fill-reducing and the
-  * numerical permutations. Use colsPermutation() to get it.
-  * 
-  * Q is the orthogonal matrix represented as products of Householder reflectors. 
-  * Use matrixQ() to get an expression and matrixQ().adjoint() to get the adjoint.
-  * You can then apply it to a vector.
-  * 
-  * R is the sparse triangular or trapezoidal matrix. The later occurs when A is rank-deficient.
-  * matrixR().topLeftCorner(rank(), rank()) always returns a triangular factor of full rank.
-  * 
-  * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<>
-  * \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module 
-  *  OrderingMethods \endlink module for the list of built-in and external ordering methods.
-  * 
-  * \implsparsesolverconcept
-  *
-  * The numerical pivoting strategy and default threshold are the same as in SuiteSparse QR, and
-  * detailed in the following paper:
-  * <i>
-  * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
-  * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011.
-  * </i>
-  * Even though it is qualified as "rank-revealing", this strategy might fail for some 
-  * rank deficient problems. When this class is used to solve linear or least-square problems
-  * it is thus strongly recommended to check the accuracy of the computed solution. If it
-  * failed, it usually helps to increase the threshold with setPivotThreshold.
-  * 
-  * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
-  * \warning For complex matrices matrixQ().transpose() will actually return the adjoint matrix.
-  * 
-  */
-template<typename _MatrixType, typename _OrderingType>
-class SparseQR : public SparseSolverBase<SparseQR<_MatrixType,_OrderingType> >
-{
-  protected:
-    typedef SparseSolverBase<SparseQR<_MatrixType,_OrderingType> > Base;
-    using Base::m_isInitialized;
-  public:
-    using Base::_solve_impl;
-    typedef _MatrixType MatrixType;
-    typedef _OrderingType OrderingType;
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef typename MatrixType::StorageIndex StorageIndex;
-    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> QRMatrixType;
-    typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
-    typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
-    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+ * \ingroup SparseQR_Module
+ * \class SparseQR
+ * \brief Sparse left-looking QR factorization with numerical column pivoting
+ *
+ * This class implements a left-looking QR decomposition of sparse matrices
+ * with numerical column pivoting.
+ * When a column has a norm less than a given tolerance
+ * it is implicitly permuted to the end. The QR factorization thus obtained is
+ * given by A*P = Q*R where R is upper triangular or trapezoidal.
+ *
+ * P is the column permutation which is the product of the fill-reducing and the
+ * numerical permutations. Use colsPermutation() to get it.
+ *
+ * Q is the orthogonal matrix represented as products of Householder reflectors.
+ * Use matrixQ() to get an expression and matrixQ().adjoint() to get the adjoint.
+ * You can then apply it to a vector.
+ *
+ * R is the sparse triangular or trapezoidal matrix. The later occurs when A is rank-deficient.
+ * matrixR().topLeftCorner(rank(), rank()) always returns a triangular factor of full rank.
+ *
+ * \tparam MatrixType_ The type of the sparse matrix A, must be a column-major SparseMatrix<>
+ * \tparam OrderingType_ The fill-reducing ordering method. See the \link OrderingMethods_Module
+ *  OrderingMethods \endlink module for the list of built-in and external ordering methods.
+ *
+ * \implsparsesolverconcept
+ *
+ * The numerical pivoting strategy and default threshold are the same as in SuiteSparse QR, and
+ * detailed in the following paper:
+ * <i>
+ * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
+ * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011.
+ * </i>
+ * Even though it is qualified as "rank-revealing", this strategy might fail for some
+ * rank deficient problems. When this class is used to solve linear or least-square problems
+ * it is thus strongly recommended to check the accuracy of the computed solution. If it
+ * failed, it usually helps to increase the threshold with setPivotThreshold.
+ *
+ * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
+ * \warning For complex matrices matrixQ().transpose() will actually return the adjoint matrix.
+ *
+ */
+template <typename MatrixType_, typename OrderingType_>
+class SparseQR : public SparseSolverBase<SparseQR<MatrixType_, OrderingType_> > {
+ protected:
+  typedef SparseSolverBase<SparseQR<MatrixType_, OrderingType_> > Base;
+  using Base::m_isInitialized;
 
-    enum {
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
-    };
-    
-  public:
-    SparseQR () :  m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
-    { }
-    
-    /** Construct a QR factorization of the matrix \a mat.
-      * 
-      * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
-      * 
-      * \sa compute()
-      */
-    explicit SparseQR(const MatrixType& mat) : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
-    {
-      compute(mat);
-    }
-    
-    /** Computes the QR factorization of the sparse matrix \a mat.
-      * 
-      * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
-      * 
-      * \sa analyzePattern(), factorize()
-      */
-    void compute(const MatrixType& mat)
-    {
-      analyzePattern(mat);
-      factorize(mat);
-    }
-    void analyzePattern(const MatrixType& mat);
-    void factorize(const MatrixType& mat);
-    
-    /** \returns the number of rows of the represented matrix. 
-      */
-    inline Index rows() const { return m_pmat.rows(); }
-    
-    /** \returns the number of columns of the represented matrix. 
-      */
-    inline Index cols() const { return m_pmat.cols();}
-    
-    /** \returns a const reference to the \b sparse upper triangular matrix R of the QR factorization.
-      * \warning The entries of the returned matrix are not sorted. This means that using it in algorithms
-      *          expecting sorted entries will fail. This include random coefficient accesses (SpaseMatrix::coeff()),
-      *          and coefficient-wise operations. Matrix products and triangular solves are fine though.
-      *
-      * To sort the entries, you can assign it to a row-major matrix, and if a column-major matrix
-      * is required, you can copy it again:
-      * \code
-      * SparseMatrix<double>          R  = qr.matrixR();  // column-major, not sorted!
-      * SparseMatrix<double,RowMajor> Rr = qr.matrixR();  // row-major, sorted
-      * SparseMatrix<double>          Rc = Rr;            // column-major, sorted
-      * \endcode
-      */
-    const QRMatrixType& matrixR() const { return m_R; }
-    
-    /** \returns the number of non linearly dependent columns as determined by the pivoting threshold.
-      *
-      * \sa setPivotThreshold()
-      */
-    Index rank() const
-    {
-      eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
-      return m_nonzeropivots; 
-    }
-    
-    /** \returns an expression of the matrix Q as products of sparse Householder reflectors.
-    * The common usage of this function is to apply it to a dense matrix or vector
-    * \code
-    * VectorXd B1, B2;
-    * // Initialize B1
-    * B2 = matrixQ() * B1;
-    * \endcode
-    *
-    * To get a plain SparseMatrix representation of Q:
-    * \code
-    * SparseMatrix<double> Q;
-    * Q = SparseQR<SparseMatrix<double> >(A).matrixQ();
-    * \endcode
-    * Internally, this call simply performs a sparse product between the matrix Q
-    * and a sparse identity matrix. However, due to the fact that the sparse
-    * reflectors are stored unsorted, two transpositions are needed to sort
-    * them before performing the product.
-    */
-    SparseQRMatrixQReturnType<SparseQR> matrixQ() const 
-    { return SparseQRMatrixQReturnType<SparseQR>(*this); }
-    
-    /** \returns a const reference to the column permutation P that was applied to A such that A*P = Q*R
-      * It is the combination of the fill-in reducing permutation and numerical column pivoting.
-      */
-    const PermutationType& colsPermutation() const
-    { 
-      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
-      return m_outputPerm_c;
-    }
-    
-    /** \returns A string describing the type of error.
-      * This method is provided to ease debugging, not to handle errors.
-      */
-    std::string lastErrorMessage() const { return m_lastError; }
-    
-    /** \internal */
-    template<typename Rhs, typename Dest>
-    bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &dest) const
-    {
-      eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
-      eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+ public:
+  using Base::_solve_impl;
+  typedef MatrixType_ MatrixType;
+  typedef OrderingType_ OrderingType;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex> QRMatrixType;
+  typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+  typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
+  typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
 
-      Index rank = this->rank();
-      
-      // Compute Q^* * b;
-      typename Dest::PlainObject y, b;
-      y = this->matrixQ().adjoint() * B;
-      b = y;
-      
-      // Solve with the triangular matrix R
-      y.resize((std::max<Index>)(cols(),y.rows()),y.cols());
-      y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(b.topRows(rank));
-      y.bottomRows(y.rows()-rank).setZero();
-      
-      // Apply the column permutation
-      if (m_perm_c.size())  dest = colsPermutation() * y.topRows(cols());
-      else                  dest = y.topRows(cols());
-      
-      m_info = Success;
-      return true;
-    }
+  enum { ColsAtCompileTime = MatrixType::ColsAtCompileTime, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime };
 
-    /** Sets the threshold that is used to determine linearly dependent columns during the factorization.
-      *
-      * In practice, if during the factorization the norm of the column that has to be eliminated is below
-      * this threshold, then the entire column is treated as zero, and it is moved at the end.
-      */
-    void setPivotThreshold(const RealScalar& threshold)
-    {
-      m_useDefaultThreshold = false;
-      m_threshold = threshold;
-    }
-    
-    /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
-      *
-      * \sa compute()
-      */
-    template<typename Rhs>
-    inline const Solve<SparseQR, Rhs> solve(const MatrixBase<Rhs>& B) const 
-    {
-      eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
-      eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
-      return Solve<SparseQR, Rhs>(*this, B.derived());
-    }
-    template<typename Rhs>
-    inline const Solve<SparseQR, Rhs> solve(const SparseMatrixBase<Rhs>& B) const
-    {
-          eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
-          eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
-          return Solve<SparseQR, Rhs>(*this, B.derived());
-    }
-    
-    /** \brief Reports whether previous computation was successful.
-      *
-      * \returns \c Success if computation was successful,
-      *          \c NumericalIssue if the QR factorization reports a numerical problem
-      *          \c InvalidInput if the input matrix is invalid
-      *
-      * \sa iparm()          
-      */
-    ComputationInfo info() const
-    {
-      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
-      return m_info;
-    }
+ public:
+  SparseQR()
+      : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true), m_isQSorted(false), m_isEtreeOk(false) {}
 
+  /** Construct a QR factorization of the matrix \a mat.
+   *
+   * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+   *
+   * \sa compute()
+   */
+  explicit SparseQR(const MatrixType& mat)
+      : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true), m_isQSorted(false), m_isEtreeOk(false) {
+    compute(mat);
+  }
 
-    /** \internal */
-    inline void _sort_matrix_Q()
-    {
-      if(this->m_isQSorted) return;
-      // The matrix Q is sorted during the transposition
-      SparseMatrix<Scalar, RowMajor, Index> mQrm(this->m_Q);
-      this->m_Q = mQrm;
-      this->m_isQSorted = true;
-    }
+  /** Computes the QR factorization of the sparse matrix \a mat.
+   *
+   * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+   *
+   * \sa analyzePattern(), factorize()
+   */
+  void compute(const MatrixType& mat) {
+    analyzePattern(mat);
+    factorize(mat);
+  }
+  void analyzePattern(const MatrixType& mat);
+  void factorize(const MatrixType& mat);
 
-    
-  protected:
-    bool m_analysisIsok;
-    bool m_factorizationIsok;
-    mutable ComputationInfo m_info;
-    std::string m_lastError;
-    QRMatrixType m_pmat;            // Temporary matrix
-    QRMatrixType m_R;               // The triangular factor matrix
-    QRMatrixType m_Q;               // The orthogonal reflectors
-    ScalarVector m_hcoeffs;         // The Householder coefficients
-    PermutationType m_perm_c;       // Fill-reducing  Column  permutation
-    PermutationType m_pivotperm;    // The permutation for rank revealing
-    PermutationType m_outputPerm_c; // The final column permutation
-    RealScalar m_threshold;         // Threshold to determine null Householder reflections
-    bool m_useDefaultThreshold;     // Use default threshold
-    Index m_nonzeropivots;          // Number of non zero pivots found
-    IndexVector m_etree;            // Column elimination tree
-    IndexVector m_firstRowElt;      // First element in each row
-    bool m_isQSorted;               // whether Q is sorted or not
-    bool m_isEtreeOk;               // whether the elimination tree match the initial input matrix
-    
-    template <typename, typename > friend struct SparseQR_QProduct;
-    
+  /** \returns the number of rows of the represented matrix.
+   */
+  inline Index rows() const { return m_pmat.rows(); }
+
+  /** \returns the number of columns of the represented matrix.
+   */
+  inline Index cols() const { return m_pmat.cols(); }
+
+  /** \returns a const reference to the \b sparse upper triangular matrix R of the QR factorization.
+   * \warning The entries of the returned matrix are not sorted. This means that using it in algorithms
+   *          expecting sorted entries will fail. This include random coefficient accesses (SpaseMatrix::coeff()),
+   *          and coefficient-wise operations. Matrix products and triangular solves are fine though.
+   *
+   * To sort the entries, you can assign it to a row-major matrix, and if a column-major matrix
+   * is required, you can copy it again:
+   * \code
+   * SparseMatrix<double>          R  = qr.matrixR();  // column-major, not sorted!
+   * SparseMatrix<double,RowMajor> Rr = qr.matrixR();  // row-major, sorted
+   * SparseMatrix<double>          Rc = Rr;            // column-major, sorted
+   * \endcode
+   */
+  const QRMatrixType& matrixR() const { return m_R; }
+
+  /** \returns the number of non linearly dependent columns as determined by the pivoting threshold.
+   *
+   * \sa setPivotThreshold()
+   */
+  Index rank() const {
+    eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+    return m_nonzeropivots;
+  }
+
+  /** \returns an expression of the matrix Q as products of sparse Householder reflectors.
+   * The common usage of this function is to apply it to a dense matrix or vector
+   * \code
+   * VectorXd B1, B2;
+   * // Initialize B1
+   * B2 = matrixQ() * B1;
+   * \endcode
+   *
+   * To get a plain SparseMatrix representation of Q:
+   * \code
+   * SparseMatrix<double> Q;
+   * Q = SparseQR<SparseMatrix<double> >(A).matrixQ();
+   * \endcode
+   * Internally, this call simply performs a sparse product between the matrix Q
+   * and a sparse identity matrix. However, due to the fact that the sparse
+   * reflectors are stored unsorted, two transpositions are needed to sort
+   * them before performing the product.
+   */
+  SparseQRMatrixQReturnType<SparseQR> matrixQ() const { return SparseQRMatrixQReturnType<SparseQR>(*this); }
+
+  /** \returns a const reference to the column permutation P that was applied to A such that A*P = Q*R
+   * It is the combination of the fill-in reducing permutation and numerical column pivoting.
+   */
+  const PermutationType& colsPermutation() const {
+    eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+    return m_outputPerm_c;
+  }
+
+  /** \returns A string describing the type of error.
+   * This method is provided to ease debugging, not to handle errors.
+   */
+  std::string lastErrorMessage() const { return m_lastError; }
+
+  /** \internal */
+  template <typename Rhs, typename Dest>
+  bool _solve_impl(const MatrixBase<Rhs>& B, MatrixBase<Dest>& dest) const {
+    eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+    eigen_assert(this->rows() == B.rows() &&
+                 "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+
+    Index rank = this->rank();
+
+    // Compute Q^* * b;
+    typename Dest::PlainObject y, b;
+    y = this->matrixQ().adjoint() * B;
+    b = y;
+
+    // Solve with the triangular matrix R
+    y.resize((std::max<Index>)(cols(), y.rows()), y.cols());
+    y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(b.topRows(rank));
+    y.bottomRows(y.rows() - rank).setZero();
+
+    // Apply the column permutation
+    if (m_perm_c.size())
+      dest = colsPermutation() * y.topRows(cols());
+    else
+      dest = y.topRows(cols());
+
+    m_info = Success;
+    return true;
+  }
+
+  /** Sets the threshold that is used to determine linearly dependent columns during the factorization.
+   *
+   * In practice, if during the factorization the norm of the column that has to be eliminated is below
+   * this threshold, then the entire column is treated as zero, and it is moved at the end.
+   */
+  void setPivotThreshold(const RealScalar& threshold) {
+    m_useDefaultThreshold = false;
+    m_threshold = threshold;
+  }
+
+  /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
+   *
+   * \sa compute()
+   */
+  template <typename Rhs>
+  inline const Solve<SparseQR, Rhs> solve(const MatrixBase<Rhs>& B) const {
+    eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+    eigen_assert(this->rows() == B.rows() &&
+                 "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+    return Solve<SparseQR, Rhs>(*this, B.derived());
+  }
+  template <typename Rhs>
+  inline const Solve<SparseQR, Rhs> solve(const SparseMatrixBase<Rhs>& B) const {
+    eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+    eigen_assert(this->rows() == B.rows() &&
+                 "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+    return Solve<SparseQR, Rhs>(*this, B.derived());
+  }
+
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful,
+   *          \c NumericalIssue if the QR factorization reports a numerical problem
+   *          \c InvalidInput if the input matrix is invalid
+   *
+   * \sa iparm()
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+    return m_info;
+  }
+
+  /** \internal */
+  inline void _sort_matrix_Q() {
+    if (this->m_isQSorted) return;
+    // The matrix Q is sorted during the transposition
+    SparseMatrix<Scalar, RowMajor, Index> mQrm(this->m_Q);
+    this->m_Q = mQrm;
+    this->m_isQSorted = true;
+  }
+
+ protected:
+  bool m_analysisIsok;
+  bool m_factorizationIsok;
+  mutable ComputationInfo m_info;
+  std::string m_lastError;
+  QRMatrixType m_pmat;             // Temporary matrix
+  QRMatrixType m_R;                // The triangular factor matrix
+  QRMatrixType m_Q;                // The orthogonal reflectors
+  ScalarVector m_hcoeffs;          // The Householder coefficients
+  PermutationType m_perm_c;        // Fill-reducing  Column  permutation
+  PermutationType m_pivotperm;     // The permutation for rank revealing
+  PermutationType m_outputPerm_c;  // The final column permutation
+  RealScalar m_threshold;          // Threshold to determine null Householder reflections
+  bool m_useDefaultThreshold;      // Use default threshold
+  Index m_nonzeropivots;           // Number of non zero pivots found
+  IndexVector m_etree;             // Column elimination tree
+  IndexVector m_firstRowElt;       // First element in each row
+  bool m_isQSorted;                // whether Q is sorted or not
+  bool m_isEtreeOk;                // whether the elimination tree match the initial input matrix
+
+  template <typename, typename>
+  friend struct SparseQR_QProduct;
 };
 
-/** \brief Preprocessing step of a QR factorization 
-  * 
-  * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
-  * 
-  * In this step, the fill-reducing permutation is computed and applied to the columns of A
-  * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited.
-  * 
-  * \note In this step it is assumed that there is no empty row in the matrix \a mat.
-  */
+/** \brief Preprocessing step of a QR factorization
+ *
+ * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+ *
+ * In this step, the fill-reducing permutation is computed and applied to the columns of A
+ * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited.
+ *
+ * \note In this step it is assumed that there is no empty row in the matrix \a mat.
+ */
 template <typename MatrixType, typename OrderingType>
-void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
-{
-  eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
+void SparseQR<MatrixType, OrderingType>::analyzePattern(const MatrixType& mat) {
+  eigen_assert(
+      mat.isCompressed() &&
+      "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
   // Copy to a column major matrix if the input is rowmajor
-  typename internal::conditional<MatrixType::IsRowMajor,QRMatrixType,const MatrixType&>::type matCpy(mat);
+  std::conditional_t<MatrixType::IsRowMajor, QRMatrixType, const MatrixType&> matCpy(mat);
   // Compute the column fill reducing ordering
-  OrderingType ord; 
-  ord(matCpy, m_perm_c); 
+  OrderingType ord;
+  ord(matCpy, m_perm_c);
   Index n = mat.cols();
   Index m = mat.rows();
-  Index diagSize = (std::min)(m,n);
-  
-  if (!m_perm_c.size())
-  {
+  Index diagSize = (std::min)(m, n);
+
+  if (!m_perm_c.size()) {
     m_perm_c.resize(n);
-    m_perm_c.indices().setLinSpaced(n, 0,StorageIndex(n-1));
+    m_perm_c.indices().setLinSpaced(n, 0, StorageIndex(n - 1));
   }
-  
+
   // Compute the column elimination tree of the permuted matrix
   m_outputPerm_c = m_perm_c.inverse();
   internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
   m_isEtreeOk = true;
-  
+
   m_R.resize(m, n);
   m_Q.resize(m, diagSize);
-  
+
   // Allocate space for nonzero elements: rough estimation
-  m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree
-  m_Q.reserve(2*mat.nonZeros());
+  m_R.reserve(2 * mat.nonZeros());  // FIXME Get a more accurate estimation through symbolic factorization with the
+                                    // etree
+  m_Q.reserve(2 * mat.nonZeros());
   m_hcoeffs.resize(diagSize);
   m_analysisIsok = true;
 }
 
 /** \brief Performs the numerical QR factorization of the input matrix
-  * 
-  * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with
-  * a matrix having the same sparsity pattern than \a mat.
-  * 
-  * \param mat The sparse column-major matrix
-  */
+ *
+ * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with
+ * a matrix having the same sparsity pattern than \a mat.
+ *
+ * \param mat The sparse column-major matrix
+ */
 template <typename MatrixType, typename OrderingType>
-void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
-{
+void SparseQR<MatrixType, OrderingType>::factorize(const MatrixType& mat) {
   using std::abs;
-  
+
   eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step");
   StorageIndex m = StorageIndex(mat.rows());
   StorageIndex n = StorageIndex(mat.cols());
-  StorageIndex diagSize = (std::min)(m,n);
-  IndexVector mark((std::max)(m,n)); mark.setConstant(-1);  // Record the visited nodes
-  IndexVector Ridx(n), Qidx(m);                             // Store temporarily the row indexes for the current column of R and Q
-  Index nzcolR, nzcolQ;                                     // Number of nonzero for the current column of R and Q
-  ScalarVector tval(m);                                     // The dense vector used to compute the current column
+  StorageIndex diagSize = (std::min)(m, n);
+  IndexVector mark((std::max)(m, n));
+  mark.setConstant(-1);          // Record the visited nodes
+  IndexVector Ridx(n), Qidx(m);  // Store temporarily the row indexes for the current column of R and Q
+  Index nzcolR, nzcolQ;          // Number of nonzero for the current column of R and Q
+  ScalarVector tval(m);          // The dense vector used to compute the current column
   RealScalar pivotThreshold = m_threshold;
-  
+
   m_R.setZero();
   m_Q.setZero();
   m_pmat = mat;
-  if(!m_isEtreeOk)
-  {
+  if (!m_isEtreeOk) {
     m_outputPerm_c = m_perm_c.inverse();
     internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
     m_isEtreeOk = true;
   }
 
-  m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
-  
+  m_pmat.uncompress();  // To have the innerNonZeroPtr allocated
+
   // Apply the fill-in reducing permutation lazily:
   {
     // If the input is row major, copy the original column indices,
     // otherwise directly use the input matrix
-    // 
+    //
     IndexVector originalOuterIndicesCpy;
-    const StorageIndex *originalOuterIndices = mat.outerIndexPtr();
-    if(MatrixType::IsRowMajor)
-    {
-      originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1);
+    const StorageIndex* originalOuterIndices = mat.outerIndexPtr();
+    if (MatrixType::IsRowMajor) {
+      originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(), n + 1);
       originalOuterIndices = originalOuterIndicesCpy.data();
     }
-    
-    for (int i = 0; i < n; i++)
-    {
+
+    for (int i = 0; i < n; i++) {
       Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i;
-      m_pmat.outerIndexPtr()[p] = originalOuterIndices[i]; 
-      m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i]; 
+      m_pmat.outerIndexPtr()[p] = originalOuterIndices[i];
+      m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i + 1] - originalOuterIndices[i];
     }
   }
-  
+
   /* Compute the default threshold as in MatLab, see:
    * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
-   * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 
+   * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3
    */
-  if(m_useDefaultThreshold) 
-  {
+  if (m_useDefaultThreshold) {
     RealScalar max2Norm = 0.0;
     for (int j = 0; j < n; j++) max2Norm = numext::maxi(max2Norm, m_pmat.col(j).norm());
-    if(max2Norm==RealScalar(0))
-      max2Norm = RealScalar(1);
+    if (max2Norm == RealScalar(0)) max2Norm = RealScalar(1);
     pivotThreshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
   }
-  
+
   // Initialize the numerical permutation
   m_pivotperm.setIdentity(n);
-  
-  StorageIndex nonzeroCol = 0; // Record the number of valid pivots
+
+  StorageIndex nonzeroCol = 0;  // Record the number of valid pivots
   m_Q.startVec(0);
 
   // Left looking rank-revealing QR factorization: compute a column of R and Q at a time
-  for (StorageIndex col = 0; col < n; ++col)
-  {
+  for (StorageIndex col = 0; col < n; ++col) {
     mark.setConstant(-1);
     m_R.startVec(col);
     mark(nonzeroCol) = col;
     Qidx(0) = nonzeroCol;
-    nzcolR = 0; nzcolQ = 1;
-    bool found_diag = nonzeroCol>=m;
-    tval.setZero(); 
-    
+    nzcolR = 0;
+    nzcolQ = 1;
+    bool found_diag = nonzeroCol >= m;
+    tval.setZero();
+
     // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e.,
-    // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k.
-    // Note: if the diagonal entry does not exist, then its contribution must be explicitly added,
-    // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
-    for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
-    {
+    // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node
+    // k. Note: if the diagonal entry does not exist, then its contribution must be explicitly added, thus the trick
+    // with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
+    for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) {
       StorageIndex curIdx = nonzeroCol;
-      if(itp) curIdx = StorageIndex(itp.row());
-      if(curIdx == nonzeroCol) found_diag = true;
-      
+      if (itp) curIdx = StorageIndex(itp.row());
+      if (curIdx == nonzeroCol) found_diag = true;
+
       // Get the nonzeros indexes of the current column of R
-      StorageIndex st = m_firstRowElt(curIdx); // The traversal of the etree starts here
-      if (st < 0 )
-      {
+      StorageIndex st = m_firstRowElt(curIdx);  // The traversal of the etree starts here
+      if (st < 0) {
         m_lastError = "Empty row found during numerical factorization";
         m_info = InvalidInput;
         return;
       }
 
-      // Traverse the etree 
+      // Traverse the etree
       Index bi = nzcolR;
-      for (; mark(st) != col; st = m_etree(st))
-      {
+      for (; mark(st) != col; st = m_etree(st)) {
         Ridx(nzcolR) = st;  // Add this row to the list,
         mark(st) = col;     // and mark this row as visited
         nzcolR++;
       }
 
       // Reverse the list to get the topological ordering
-      Index nt = nzcolR-bi;
-      for(Index i = 0; i < nt/2; i++) std::swap(Ridx(bi+i), Ridx(nzcolR-i-1));
-       
+      Index nt = nzcolR - bi;
+      for (Index i = 0; i < nt / 2; i++) std::swap(Ridx(bi + i), Ridx(nzcolR - i - 1));
+
       // Copy the current (curIdx,pcol) value of the input matrix
-      if(itp) tval(curIdx) = itp.value();
-      else    tval(curIdx) = Scalar(0);
-      
+      if (itp)
+        tval(curIdx) = itp.value();
+      else
+        tval(curIdx) = Scalar(0);
+
       // Compute the pattern of Q(:,k)
-      if(curIdx > nonzeroCol && mark(curIdx) != col ) 
-      {
+      if (curIdx > nonzeroCol && mark(curIdx) != col) {
         Qidx(nzcolQ) = curIdx;  // Add this row to the pattern of Q,
         mark(curIdx) = col;     // and mark it as visited
         nzcolQ++;
@@ -481,110 +469,91 @@
     }
 
     // Browse all the indexes of R(:,col) in reverse order
-    for (Index i = nzcolR-1; i >= 0; i--)
-    {
+    for (Index i = nzcolR - 1; i >= 0; i--) {
       Index curIdx = Ridx(i);
-      
+
       // Apply the curIdx-th householder vector to the current column (temporarily stored into tval)
       Scalar tdot(0);
-      
+
       // First compute q' * tval
       tdot = m_Q.col(curIdx).dot(tval);
 
       tdot *= m_hcoeffs(curIdx);
-      
+
       // Then update tval = tval - q * tau
-      // FIXME: tval -= tdot * m_Q.col(curIdx) should amount to the same (need to check/add support for efficient "dense ?= sparse")
-      for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
-        tval(itq.row()) -= itq.value() * tdot;
+      // FIXME: tval -= tdot * m_Q.col(curIdx) should amount to the same (need to check/add support for efficient "dense
+      // ?= sparse")
+      for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq) tval(itq.row()) -= itq.value() * tdot;
 
       // Detect fill-in for the current column of Q
-      if(m_etree(Ridx(i)) == nonzeroCol)
-      {
-        for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
-        {
+      if (m_etree(Ridx(i)) == nonzeroCol) {
+        for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq) {
           StorageIndex iQ = StorageIndex(itq.row());
-          if (mark(iQ) != col)
-          {
+          if (mark(iQ) != col) {
             Qidx(nzcolQ++) = iQ;  // Add this row to the pattern of Q,
             mark(iQ) = col;       // and mark it as visited
           }
         }
       }
-    } // End update current column
-    
+    }  // End update current column
+
     Scalar tau = RealScalar(0);
     RealScalar beta = 0;
-    
-    if(nonzeroCol < diagSize)
-    {
+
+    if (nonzeroCol < diagSize) {
       // Compute the Householder reflection that eliminate the current column
       // FIXME this step should call the Householder module.
       Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
-      
+
       // First, the squared norm of Q((col+1):m, col)
       RealScalar sqrNorm = 0.;
       for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
-      if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
-      {
+      if (sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0)) {
         beta = numext::real(c0);
         tval(Qidx(0)) = 1;
-      }
-      else
-      {
+      } else {
         using std::sqrt;
         beta = sqrt(numext::abs2(c0) + sqrNorm);
-        if(numext::real(c0) >= RealScalar(0))
-          beta = -beta;
+        if (numext::real(c0) >= RealScalar(0)) beta = -beta;
         tval(Qidx(0)) = 1;
-        for (Index itq = 1; itq < nzcolQ; ++itq)
-          tval(Qidx(itq)) /= (c0 - beta);
-        tau = numext::conj((beta-c0) / beta);
-          
+        for (Index itq = 1; itq < nzcolQ; ++itq) tval(Qidx(itq)) /= (c0 - beta);
+        tau = numext::conj((beta - c0) / beta);
       }
     }
 
     // Insert values in R
-    for (Index  i = nzcolR-1; i >= 0; i--)
-    {
+    for (Index i = nzcolR - 1; i >= 0; i--) {
       Index curIdx = Ridx(i);
-      if(curIdx < nonzeroCol) 
-      {
+      if (curIdx < nonzeroCol) {
         m_R.insertBackByOuterInnerUnordered(col, curIdx) = tval(curIdx);
         tval(curIdx) = Scalar(0.);
       }
     }
 
-    if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold)
-    {
+    if (nonzeroCol < diagSize && abs(beta) >= pivotThreshold) {
       m_R.insertBackByOuterInner(col, nonzeroCol) = beta;
       // The householder coefficient
       m_hcoeffs(nonzeroCol) = tau;
       // Record the householder reflections
-      for (Index itq = 0; itq < nzcolQ; ++itq)
-      {
+      for (Index itq = 0; itq < nzcolQ; ++itq) {
         Index iQ = Qidx(itq);
-        m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ);
+        m_Q.insertBackByOuterInnerUnordered(nonzeroCol, iQ) = tval(iQ);
         tval(iQ) = Scalar(0.);
       }
       nonzeroCol++;
-      if(nonzeroCol<diagSize)
-        m_Q.startVec(nonzeroCol);
-    }
-    else
-    {
+      if (nonzeroCol < diagSize) m_Q.startVec(nonzeroCol);
+    } else {
       // Zero pivot found: move implicitly this column to the end
-      for (Index j = nonzeroCol; j < n-1; j++) 
-        std::swap(m_pivotperm.indices()(j), m_pivotperm.indices()[j+1]);
-      
+      for (Index j = nonzeroCol; j < n - 1; j++) std::swap(m_pivotperm.indices()(j), m_pivotperm.indices()[j + 1]);
+
       // Recompute the column elimination tree
       internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data());
       m_isEtreeOk = false;
     }
   }
-  
-  m_hcoeffs.tail(diagSize-nonzeroCol).setZero();
-  
+
+  m_hcoeffs.tail(diagSize - nonzeroCol).setZero();
+
   // Finalize the column pointers of the sparse matrices R and Q
   m_Q.finalize();
   m_Q.makeCompressed();
@@ -593,166 +562,145 @@
   m_isQSorted = false;
 
   m_nonzeropivots = nonzeroCol;
-  
-  if(nonzeroCol<n)
-  {
+
+  if (nonzeroCol < n) {
     // Permute the triangular factor to put the 'dead' columns to the end
     QRMatrixType tempR(m_R);
     m_R = tempR * m_pivotperm;
-    
+
     // Update the column permutation
     m_outputPerm_c = m_outputPerm_c * m_pivotperm;
   }
-  
-  m_isInitialized = true; 
+
+  m_isInitialized = true;
   m_factorizationIsok = true;
   m_info = Success;
 }
 
 template <typename SparseQRType, typename Derived>
-struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived> >
-{
+struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived> > {
   typedef typename SparseQRType::QRMatrixType MatrixType;
   typedef typename SparseQRType::Scalar Scalar;
-  // Get the references 
-  SparseQR_QProduct(const SparseQRType& qr, const Derived& other, bool transpose) : 
-  m_qr(qr),m_other(other),m_transpose(transpose) {}
+  // Get the references
+  SparseQR_QProduct(const SparseQRType& qr, const Derived& other, bool transpose)
+      : m_qr(qr), m_other(other), m_transpose(transpose) {}
   inline Index rows() const { return m_qr.matrixQ().rows(); }
   inline Index cols() const { return m_other.cols(); }
-  
+
   // Assign to a vector
-  template<typename DesType>
-  void evalTo(DesType& res) const
-  {
+  template <typename DesType>
+  void evalTo(DesType& res) const {
     Index m = m_qr.rows();
     Index n = m_qr.cols();
-    Index diagSize = (std::min)(m,n);
+    Index diagSize = (std::min)(m, n);
     res = m_other;
-    if (m_transpose)
-    {
+    if (m_transpose) {
       eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
-      //Compute res = Q' * other column by column
-      for(Index j = 0; j < res.cols(); j++){
-        for (Index k = 0; k < diagSize; k++)
-        {
+      // Compute res = Q' * other column by column
+      for (Index j = 0; j < res.cols(); j++) {
+        for (Index k = 0; k < diagSize; k++) {
           Scalar tau = Scalar(0);
           tau = m_qr.m_Q.col(k).dot(res.col(j));
-          if(tau==Scalar(0)) continue;
+          if (tau == Scalar(0)) continue;
           tau = tau * m_qr.m_hcoeffs(k);
           res.col(j) -= tau * m_qr.m_Q.col(k);
         }
       }
-    }
-    else
-    {
+    } else {
       eigen_assert(m_qr.matrixQ().cols() == m_other.rows() && "Non conforming object sizes");
 
       res.conservativeResize(rows(), cols());
 
       // Compute res = Q * other column by column
-      for(Index j = 0; j < res.cols(); j++)
-      {
-        Index start_k = internal::is_identity<Derived>::value ? numext::mini(j,diagSize-1) : diagSize-1;
-        for (Index k = start_k; k >=0; k--)
-        {
+      for (Index j = 0; j < res.cols(); j++) {
+        Index start_k = internal::is_identity<Derived>::value ? numext::mini(j, diagSize - 1) : diagSize - 1;
+        for (Index k = start_k; k >= 0; k--) {
           Scalar tau = Scalar(0);
           tau = m_qr.m_Q.col(k).dot(res.col(j));
-          if(tau==Scalar(0)) continue;
+          if (tau == Scalar(0)) continue;
           tau = tau * numext::conj(m_qr.m_hcoeffs(k));
           res.col(j) -= tau * m_qr.m_Q.col(k);
         }
       }
     }
   }
-  
+
   const SparseQRType& m_qr;
   const Derived& m_other;
-  bool m_transpose; // TODO this actually means adjoint
+  bool m_transpose;  // TODO this actually means adjoint
 };
 
-template<typename SparseQRType>
-struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<SparseQRType> >
-{  
+template <typename SparseQRType>
+struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<SparseQRType> > {
   typedef typename SparseQRType::Scalar Scalar;
-  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
-  enum {
-    RowsAtCompileTime = Dynamic,
-    ColsAtCompileTime = Dynamic
-  };
+  typedef Matrix<Scalar, Dynamic, Dynamic> DenseMatrix;
+  enum { RowsAtCompileTime = Dynamic, ColsAtCompileTime = Dynamic };
   explicit SparseQRMatrixQReturnType(const SparseQRType& qr) : m_qr(qr) {}
-  template<typename Derived>
-  SparseQR_QProduct<SparseQRType, Derived> operator*(const MatrixBase<Derived>& other)
-  {
-    return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(),false);
+  template <typename Derived>
+  SparseQR_QProduct<SparseQRType, Derived> operator*(const MatrixBase<Derived>& other) {
+    return SparseQR_QProduct<SparseQRType, Derived>(m_qr, other.derived(), false);
   }
   // To use for operations with the adjoint of Q
-  SparseQRMatrixQTransposeReturnType<SparseQRType> adjoint() const
-  {
+  SparseQRMatrixQTransposeReturnType<SparseQRType> adjoint() const {
     return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
   }
   inline Index rows() const { return m_qr.rows(); }
   inline Index cols() const { return m_qr.rows(); }
   // To use for operations with the transpose of Q FIXME this is the same as adjoint at the moment
-  SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const
-  {
+  SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const {
     return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
   }
   const SparseQRType& m_qr;
 };
 
 // TODO this actually represents the adjoint of Q
-template<typename SparseQRType>
-struct SparseQRMatrixQTransposeReturnType
-{
+template <typename SparseQRType>
+struct SparseQRMatrixQTransposeReturnType {
   explicit SparseQRMatrixQTransposeReturnType(const SparseQRType& qr) : m_qr(qr) {}
-  template<typename Derived>
-  SparseQR_QProduct<SparseQRType,Derived> operator*(const MatrixBase<Derived>& other)
-  {
-    return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(), true);
+  template <typename Derived>
+  SparseQR_QProduct<SparseQRType, Derived> operator*(const MatrixBase<Derived>& other) {
+    return SparseQR_QProduct<SparseQRType, Derived>(m_qr, other.derived(), true);
   }
   const SparseQRType& m_qr;
 };
 
 namespace internal {
-  
-template<typename SparseQRType>
-struct evaluator_traits<SparseQRMatrixQReturnType<SparseQRType> >
-{
+
+template <typename SparseQRType>
+struct evaluator_traits<SparseQRMatrixQReturnType<SparseQRType> > {
   typedef typename SparseQRType::MatrixType MatrixType;
   typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
   typedef SparseShape Shape;
 };
 
-template< typename DstXprType, typename SparseQRType>
-struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>, internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>, Sparse2Sparse>
-{
+template <typename DstXprType, typename SparseQRType>
+struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>,
+                  internal::assign_op<typename DstXprType::Scalar, typename DstXprType::Scalar>, Sparse2Sparse> {
   typedef SparseQRMatrixQReturnType<SparseQRType> SrcXprType;
   typedef typename DstXprType::Scalar Scalar;
   typedef typename DstXprType::StorageIndex StorageIndex;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &/*func*/)
-  {
+  static void run(DstXprType& dst, const SrcXprType& src, const internal::assign_op<Scalar, Scalar>& /*func*/) {
     typename DstXprType::PlainObject idMat(src.rows(), src.cols());
     idMat.setIdentity();
     // Sort the sparse householder reflectors if needed
-    const_cast<SparseQRType *>(&src.m_qr)->_sort_matrix_Q();
+    const_cast<SparseQRType*>(&src.m_qr)->_sort_matrix_Q();
     dst = SparseQR_QProduct<SparseQRType, DstXprType>(src.m_qr, idMat, false);
   }
 };
 
-template< typename DstXprType, typename SparseQRType>
-struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>, internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>, Sparse2Dense>
-{
+template <typename DstXprType, typename SparseQRType>
+struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>,
+                  internal::assign_op<typename DstXprType::Scalar, typename DstXprType::Scalar>, Sparse2Dense> {
   typedef SparseQRMatrixQReturnType<SparseQRType> SrcXprType;
   typedef typename DstXprType::Scalar Scalar;
   typedef typename DstXprType::StorageIndex StorageIndex;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &/*func*/)
-  {
+  static void run(DstXprType& dst, const SrcXprType& src, const internal::assign_op<Scalar, Scalar>& /*func*/) {
     dst = src.m_qr.matrixQ() * DstXprType::Identity(src.m_qr.rows(), src.m_qr.rows());
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h
index b8b8a04..fd1ac99 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h
@@ -10,40 +10,39 @@
 #ifndef EIGEN_MISC_IMAGE_H
 #define EIGEN_MISC_IMAGE_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \class image_retval_base
-  *
-  */
-template<typename DecompositionType>
-struct traits<image_retval_base<DecompositionType> >
-{
+ *
+ */
+template <typename DecompositionType>
+struct traits<image_retval_base<DecompositionType> > {
   typedef typename DecompositionType::MatrixType MatrixType;
-  typedef Matrix<
-    typename MatrixType::Scalar,
-    MatrixType::RowsAtCompileTime, // the image is a subspace of the destination space, whose
-                                   // dimension is the number of rows of the original matrix
-    Dynamic,                       // we don't know at compile time the dimension of the image (the rank)
-    MatrixType::Options,
-    MatrixType::MaxRowsAtCompileTime, // the image matrix will consist of columns from the original matrix,
-    MatrixType::MaxColsAtCompileTime  // so it has the same number of rows and at most as many columns.
-  > ReturnType;
+  typedef Matrix<typename MatrixType::Scalar,
+                 MatrixType::RowsAtCompileTime,  // the image is a subspace of the destination space, whose
+                                                 // dimension is the number of rows of the original matrix
+                 Dynamic,                        // we don't know at compile time the dimension of the image (the rank)
+                 MatrixType::Options,
+                 MatrixType::MaxRowsAtCompileTime,  // the image matrix will consist of columns from the original
+                                                    // matrix,
+                 MatrixType::MaxColsAtCompileTime   // so it has the same number of rows and at most as many columns.
+                 >
+      ReturnType;
 };
 
-template<typename _DecompositionType> struct image_retval_base
- : public ReturnByValue<image_retval_base<_DecompositionType> >
-{
-  typedef _DecompositionType DecompositionType;
+template <typename DecompositionType_>
+struct image_retval_base : public ReturnByValue<image_retval_base<DecompositionType_> > {
+  typedef DecompositionType_ DecompositionType;
   typedef typename DecompositionType::MatrixType MatrixType;
   typedef ReturnByValue<image_retval_base> Base;
 
   image_retval_base(const DecompositionType& dec, const MatrixType& originalMatrix)
-    : m_dec(dec), m_rank(dec.rank()),
-      m_cols(m_rank == 0 ? 1 : m_rank),
-      m_originalMatrix(originalMatrix)
-  {}
+      : m_dec(dec), m_rank(dec.rank()), m_cols(m_rank == 0 ? 1 : m_rank), m_originalMatrix(originalMatrix) {}
 
   inline Index rows() const { return m_dec.rows(); }
   inline Index cols() const { return m_cols; }
@@ -51,32 +50,31 @@
   inline const DecompositionType& dec() const { return m_dec; }
   inline const MatrixType& originalMatrix() const { return m_originalMatrix; }
 
-  template<typename Dest> inline void evalTo(Dest& dst) const
-  {
+  template <typename Dest>
+  inline void evalTo(Dest& dst) const {
     static_cast<const image_retval<DecompositionType>*>(this)->evalTo(dst);
   }
 
-  protected:
-    const DecompositionType& m_dec;
-    Index m_rank, m_cols;
-    const MatrixType& m_originalMatrix;
+ protected:
+  const DecompositionType& m_dec;
+  Index m_rank, m_cols;
+  const MatrixType& m_originalMatrix;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-#define EIGEN_MAKE_IMAGE_HELPERS(DecompositionType) \
-  typedef typename DecompositionType::MatrixType MatrixType; \
-  typedef typename MatrixType::Scalar Scalar; \
-  typedef typename MatrixType::RealScalar RealScalar; \
+#define EIGEN_MAKE_IMAGE_HELPERS(DecompositionType)                   \
+  typedef typename DecompositionType::MatrixType MatrixType;          \
+  typedef typename MatrixType::Scalar Scalar;                         \
+  typedef typename MatrixType::RealScalar RealScalar;                 \
   typedef Eigen::internal::image_retval_base<DecompositionType> Base; \
-  using Base::dec; \
-  using Base::originalMatrix; \
-  using Base::rank; \
-  using Base::rows; \
-  using Base::cols; \
-  image_retval(const DecompositionType& dec, const MatrixType& originalMatrix) \
-    : Base(dec, originalMatrix) {}
+  using Base::dec;                                                    \
+  using Base::originalMatrix;                                         \
+  using Base::rank;                                                   \
+  using Base::rows;                                                   \
+  using Base::cols;                                                   \
+  image_retval(const DecompositionType& dec, const MatrixType& originalMatrix) : Base(dec, originalMatrix) {}
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MISC_IMAGE_H
+#endif  // EIGEN_MISC_IMAGE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/InternalHeaderCheck.h
new file mode 100644
index 0000000..1cea572
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/InternalHeaderCheck.h
@@ -0,0 +1,3 @@
+#ifndef EIGEN_CORE_MODULE_H
+#error "Please include Eigen/Core instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h
index bef5d6f..55c3efe 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h
@@ -10,70 +10,68 @@
 #ifndef EIGEN_MISC_KERNEL_H
 #define EIGEN_MISC_KERNEL_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \class kernel_retval_base
-  *
-  */
-template<typename DecompositionType>
-struct traits<kernel_retval_base<DecompositionType> >
-{
+ *
+ */
+template <typename DecompositionType>
+struct traits<kernel_retval_base<DecompositionType> > {
   typedef typename DecompositionType::MatrixType MatrixType;
-  typedef Matrix<
-    typename MatrixType::Scalar,
-    MatrixType::ColsAtCompileTime, // the number of rows in the "kernel matrix"
-                                   // is the number of cols of the original matrix
-                                   // so that the product "matrix * kernel = zero" makes sense
-    Dynamic,                       // we don't know at compile-time the dimension of the kernel
-    MatrixType::Options,
-    MatrixType::MaxColsAtCompileTime, // see explanation for 2nd template parameter
-    MatrixType::MaxColsAtCompileTime // the kernel is a subspace of the domain space,
-                                     // whose dimension is the number of columns of the original matrix
-  > ReturnType;
+  typedef Matrix<typename MatrixType::Scalar,
+                 MatrixType::ColsAtCompileTime,  // the number of rows in the "kernel matrix"
+                                                 // is the number of cols of the original matrix
+                                                 // so that the product "matrix * kernel = zero" makes sense
+                 Dynamic,                        // we don't know at compile-time the dimension of the kernel
+                 MatrixType::Options,
+                 MatrixType::MaxColsAtCompileTime,  // see explanation for 2nd template parameter
+                 MatrixType::MaxColsAtCompileTime   // the kernel is a subspace of the domain space,
+                                                    // whose dimension is the number of columns of the original matrix
+                 >
+      ReturnType;
 };
 
-template<typename _DecompositionType> struct kernel_retval_base
- : public ReturnByValue<kernel_retval_base<_DecompositionType> >
-{
-  typedef _DecompositionType DecompositionType;
+template <typename DecompositionType_>
+struct kernel_retval_base : public ReturnByValue<kernel_retval_base<DecompositionType_> > {
+  typedef DecompositionType_ DecompositionType;
   typedef ReturnByValue<kernel_retval_base> Base;
 
   explicit kernel_retval_base(const DecompositionType& dec)
-    : m_dec(dec),
-      m_rank(dec.rank()),
-      m_cols(m_rank==dec.cols() ? 1 : dec.cols() - m_rank)
-  {}
+      : m_dec(dec), m_rank(dec.rank()), m_cols(m_rank == dec.cols() ? 1 : dec.cols() - m_rank) {}
 
   inline Index rows() const { return m_dec.cols(); }
   inline Index cols() const { return m_cols; }
   inline Index rank() const { return m_rank; }
   inline const DecompositionType& dec() const { return m_dec; }
 
-  template<typename Dest> inline void evalTo(Dest& dst) const
-  {
+  template <typename Dest>
+  inline void evalTo(Dest& dst) const {
     static_cast<const kernel_retval<DecompositionType>*>(this)->evalTo(dst);
   }
 
-  protected:
-    const DecompositionType& m_dec;
-    Index m_rank, m_cols;
+ protected:
+  const DecompositionType& m_dec;
+  Index m_rank, m_cols;
 };
 
-} // end namespace internal
+}  // end namespace internal
 
-#define EIGEN_MAKE_KERNEL_HELPERS(DecompositionType) \
-  typedef typename DecompositionType::MatrixType MatrixType; \
-  typedef typename MatrixType::Scalar Scalar; \
-  typedef typename MatrixType::RealScalar RealScalar; \
+#define EIGEN_MAKE_KERNEL_HELPERS(DecompositionType)                   \
+  typedef typename DecompositionType::MatrixType MatrixType;           \
+  typedef typename MatrixType::Scalar Scalar;                          \
+  typedef typename MatrixType::RealScalar RealScalar;                  \
   typedef Eigen::internal::kernel_retval_base<DecompositionType> Base; \
-  using Base::dec; \
-  using Base::rank; \
-  using Base::rows; \
-  using Base::cols; \
+  using Base::dec;                                                     \
+  using Base::rank;                                                    \
+  using Base::rows;                                                    \
+  using Base::cols;                                                    \
   kernel_retval(const DecompositionType& dec) : Base(dec) {}
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MISC_KERNEL_H
+#endif  // EIGEN_MISC_KERNEL_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h
index abb4d3c..332a5ab 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h
@@ -11,31 +11,29 @@
 #ifndef EIGEN_REALSVD2X2_H
 #define EIGEN_REALSVD2X2_H
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
 namespace internal {
 
-template<typename MatrixType, typename RealScalar, typename Index>
-void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q,
-                         JacobiRotation<RealScalar> *j_left,
-                         JacobiRotation<RealScalar> *j_right)
-{
-  using std::sqrt;
+template <typename MatrixType, typename RealScalar, typename Index>
+void real_2x2_jacobi_svd(const MatrixType &matrix, Index p, Index q, JacobiRotation<RealScalar> *j_left,
+                         JacobiRotation<RealScalar> *j_right) {
   using std::abs;
-  Matrix<RealScalar,2,2> m;
-  m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)),
-       numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q));
+  using std::sqrt;
+  Matrix<RealScalar, 2, 2> m;
+  m << numext::real(matrix.coeff(p, p)), numext::real(matrix.coeff(p, q)), numext::real(matrix.coeff(q, p)),
+      numext::real(matrix.coeff(q, q));
   JacobiRotation<RealScalar> rot1;
-  RealScalar t = m.coeff(0,0) + m.coeff(1,1);
-  RealScalar d = m.coeff(1,0) - m.coeff(0,1);
+  RealScalar t = m.coeff(0, 0) + m.coeff(1, 1);
+  RealScalar d = m.coeff(1, 0) - m.coeff(0, 1);
 
-  if(abs(d) < (std::numeric_limits<RealScalar>::min)())
-  {
+  if (abs(d) < (std::numeric_limits<RealScalar>::min)()) {
     rot1.s() = RealScalar(0);
     rot1.c() = RealScalar(1);
-  }
-  else
-  {
+  } else {
     // If d!=0, then t/d cannot overflow because the magnitude of the
     // entries forming d are not too small compared to the ones forming t.
     RealScalar u = t / d;
@@ -43,13 +41,13 @@
     rot1.s() = RealScalar(1) / tmp;
     rot1.c() = u / tmp;
   }
-  m.applyOnTheLeft(0,1,rot1);
-  j_right->makeJacobi(m,0,1);
+  m.applyOnTheLeft(0, 1, rot1);
+  j_right->makeJacobi(m, 0, 1);
   *j_left = rot1 * j_right->transpose();
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_REALSVD2X2_H
+#endif  // EIGEN_REALSVD2X2_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h
deleted file mode 100644
index 0e5d544..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h
+++ /dev/null
@@ -1,358 +0,0 @@
-
-/** \returns an expression of the coefficient wise product of \c *this and \a other
-  *
-  * \sa MatrixBase::cwiseProduct
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,product)
-operator*(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,product)(derived(), other.derived());
-}
-
-/** \returns an expression of the coefficient wise quotient of \c *this and \a other
-  *
-  * \sa MatrixBase::cwiseQuotient
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_quotient_op<Scalar,typename OtherDerived::Scalar>, const Derived, const OtherDerived>
-operator/(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return CwiseBinaryOp<internal::scalar_quotient_op<Scalar,typename OtherDerived::Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
-}
-
-/** \returns an expression of the coefficient-wise min of \c *this and \a other
-  *
-  * Example: \include Cwise_min.cpp
-  * Output: \verbinclude Cwise_min.out
-  *
-  * \sa max()
-  */
-EIGEN_MAKE_CWISE_BINARY_OP(min,min)
-
-/** \returns an expression of the coefficient-wise min of \c *this and scalar \a other
-  *
-  * \sa max()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_min_op<Scalar,Scalar>, const Derived,
-                                        const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >
-#ifdef EIGEN_PARSED_BY_DOXYGEN
-min
-#else
-(min)
-#endif
-(const Scalar &other) const
-{
-  return (min)(Derived::PlainObject::Constant(rows(), cols(), other));
-}
-
-/** \returns an expression of the coefficient-wise max of \c *this and \a other
-  *
-  * Example: \include Cwise_max.cpp
-  * Output: \verbinclude Cwise_max.out
-  *
-  * \sa min()
-  */
-EIGEN_MAKE_CWISE_BINARY_OP(max,max)
-
-/** \returns an expression of the coefficient-wise max of \c *this and scalar \a other
-  *
-  * \sa min()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_max_op<Scalar,Scalar>, const Derived,
-                                        const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >
-#ifdef EIGEN_PARSED_BY_DOXYGEN
-max
-#else
-(max)
-#endif
-(const Scalar &other) const
-{
-  return (max)(Derived::PlainObject::Constant(rows(), cols(), other));
-}
-
-/** \returns an expression of the coefficient-wise absdiff of \c *this and \a other
-  *
-  * Example: \include Cwise_absolute_difference.cpp
-  * Output: \verbinclude Cwise_absolute_difference.out
-  *
-  * \sa absolute_difference()
-  */
-EIGEN_MAKE_CWISE_BINARY_OP(absolute_difference,absolute_difference)
-
-/** \returns an expression of the coefficient-wise absolute_difference of \c *this and scalar \a other
-  *
-  * \sa absolute_difference()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_absolute_difference_op<Scalar,Scalar>, const Derived,
-                                        const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >
-#ifdef EIGEN_PARSED_BY_DOXYGEN
-absolute_difference
-#else
-(absolute_difference)
-#endif
-(const Scalar &other) const
-{
-  return (absolute_difference)(Derived::PlainObject::Constant(rows(), cols(), other));
-}
-
-/** \returns an expression of the coefficient-wise power of \c *this to the given array of \a exponents.
-  *
-  * This function computes the coefficient-wise power.
-  *
-  * Example: \include Cwise_array_power_array.cpp
-  * Output: \verbinclude Cwise_array_power_array.out
-  */
-EIGEN_MAKE_CWISE_BINARY_OP(pow,pow)
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(pow,pow)
-#else
-/** \returns an expression of the coefficients of \c *this rasied to the constant power \a exponent
-  *
-  * \tparam T is the scalar type of \a exponent. It must be compatible with the scalar type of the given expression.
-  *
-  * This function computes the coefficient-wise power. The function MatrixBase::pow() in the
-  * unsupported module MatrixFunctions computes the matrix power.
-  *
-  * Example: \include Cwise_pow.cpp
-  * Output: \verbinclude Cwise_pow.out
-  *
-  * \sa ArrayBase::pow(ArrayBase), square(), cube(), exp(), log()
-  */
-template<typename T>
-const CwiseBinaryOp<internal::scalar_pow_op<Scalar,T>,Derived,Constant<T> > pow(const T& exponent) const;
-#endif
-
-
-// TODO code generating macros could be moved to Macros.h and could include generation of documentation
-#define EIGEN_MAKE_CWISE_COMP_OP(OP, COMPARATOR) \
-template<typename OtherDerived> \
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_cmp_op<Scalar, typename OtherDerived::Scalar, internal::cmp_ ## COMPARATOR>, const Derived, const OtherDerived> \
-OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const \
-{ \
-  return CwiseBinaryOp<internal::scalar_cmp_op<Scalar, typename OtherDerived::Scalar, internal::cmp_ ## COMPARATOR>, const Derived, const OtherDerived>(derived(), other.derived()); \
-}\
-typedef CwiseBinaryOp<internal::scalar_cmp_op<Scalar,Scalar, internal::cmp_ ## COMPARATOR>, const Derived, const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> > Cmp ## COMPARATOR ## ReturnType; \
-typedef CwiseBinaryOp<internal::scalar_cmp_op<Scalar,Scalar, internal::cmp_ ## COMPARATOR>, const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject>, const Derived > RCmp ## COMPARATOR ## ReturnType; \
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Cmp ## COMPARATOR ## ReturnType \
-OP(const Scalar& s) const { \
-  return this->OP(Derived::PlainObject::Constant(rows(), cols(), s)); \
-} \
-EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE const RCmp ## COMPARATOR ## ReturnType \
-OP(const Scalar& s, const EIGEN_CURRENT_STORAGE_BASE_CLASS<Derived>& d) { \
-  return Derived::PlainObject::Constant(d.rows(), d.cols(), s).OP(d); \
-}
-
-#define EIGEN_MAKE_CWISE_COMP_R_OP(OP, R_OP, RCOMPARATOR) \
-template<typename OtherDerived> \
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_cmp_op<typename OtherDerived::Scalar, Scalar, internal::cmp_##RCOMPARATOR>, const OtherDerived, const Derived> \
-OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const \
-{ \
-  return CwiseBinaryOp<internal::scalar_cmp_op<typename OtherDerived::Scalar, Scalar, internal::cmp_##RCOMPARATOR>, const OtherDerived, const Derived>(other.derived(), derived()); \
-} \
-EIGEN_DEVICE_FUNC \
-inline const RCmp ## RCOMPARATOR ## ReturnType \
-OP(const Scalar& s) const { \
-  return Derived::PlainObject::Constant(rows(), cols(), s).R_OP(*this); \
-} \
-friend inline const Cmp ## RCOMPARATOR ## ReturnType \
-OP(const Scalar& s, const Derived& d) { \
-  return d.R_OP(Derived::PlainObject::Constant(d.rows(), d.cols(), s)); \
-}
-
-
-
-/** \returns an expression of the coefficient-wise \< operator of *this and \a other
-  *
-  * Example: \include Cwise_less.cpp
-  * Output: \verbinclude Cwise_less.out
-  *
-  * \sa all(), any(), operator>(), operator<=()
-  */
-EIGEN_MAKE_CWISE_COMP_OP(operator<, LT)
-
-/** \returns an expression of the coefficient-wise \<= operator of *this and \a other
-  *
-  * Example: \include Cwise_less_equal.cpp
-  * Output: \verbinclude Cwise_less_equal.out
-  *
-  * \sa all(), any(), operator>=(), operator<()
-  */
-EIGEN_MAKE_CWISE_COMP_OP(operator<=, LE)
-
-/** \returns an expression of the coefficient-wise \> operator of *this and \a other
-  *
-  * Example: \include Cwise_greater.cpp
-  * Output: \verbinclude Cwise_greater.out
-  *
-  * \sa all(), any(), operator>=(), operator<()
-  */
-EIGEN_MAKE_CWISE_COMP_R_OP(operator>, operator<, LT)
-
-/** \returns an expression of the coefficient-wise \>= operator of *this and \a other
-  *
-  * Example: \include Cwise_greater_equal.cpp
-  * Output: \verbinclude Cwise_greater_equal.out
-  *
-  * \sa all(), any(), operator>(), operator<=()
-  */
-EIGEN_MAKE_CWISE_COMP_R_OP(operator>=, operator<=, LE)
-
-/** \returns an expression of the coefficient-wise == operator of *this and \a other
-  *
-  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
-  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
-  * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
-  * isMuchSmallerThan().
-  *
-  * Example: \include Cwise_equal_equal.cpp
-  * Output: \verbinclude Cwise_equal_equal.out
-  *
-  * \sa all(), any(), isApprox(), isMuchSmallerThan()
-  */
-EIGEN_MAKE_CWISE_COMP_OP(operator==, EQ)
-
-/** \returns an expression of the coefficient-wise != operator of *this and \a other
-  *
-  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
-  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
-  * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
-  * isMuchSmallerThan().
-  *
-  * Example: \include Cwise_not_equal.cpp
-  * Output: \verbinclude Cwise_not_equal.out
-  *
-  * \sa all(), any(), isApprox(), isMuchSmallerThan()
-  */
-EIGEN_MAKE_CWISE_COMP_OP(operator!=, NEQ)
-
-
-#undef EIGEN_MAKE_CWISE_COMP_OP
-#undef EIGEN_MAKE_CWISE_COMP_R_OP
-
-// scalar addition
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-EIGEN_MAKE_SCALAR_BINARY_OP(operator+,sum)
-#else
-/** \returns an expression of \c *this with each coeff incremented by the constant \a scalar
-  *
-  * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
-  *
-  * Example: \include Cwise_plus.cpp
-  * Output: \verbinclude Cwise_plus.out
-  *
-  * \sa operator+=(), operator-()
-  */
-template<typename T>
-const CwiseBinaryOp<internal::scalar_sum_op<Scalar,T>,Derived,Constant<T> > operator+(const T& scalar) const;
-/** \returns an expression of \a expr with each coeff incremented by the constant \a scalar
-  *
-  * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
-  */
-template<typename T> friend
-const CwiseBinaryOp<internal::scalar_sum_op<T,Scalar>,Constant<T>,Derived> operator+(const T& scalar, const StorageBaseType& expr);
-#endif
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-EIGEN_MAKE_SCALAR_BINARY_OP(operator-,difference)
-#else
-/** \returns an expression of \c *this with each coeff decremented by the constant \a scalar
-  *
-  * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
-  *
-  * Example: \include Cwise_minus.cpp
-  * Output: \verbinclude Cwise_minus.out
-  *
-  * \sa operator+=(), operator-()
-  */
-template<typename T>
-const CwiseBinaryOp<internal::scalar_difference_op<Scalar,T>,Derived,Constant<T> > operator-(const T& scalar) const;
-/** \returns an expression of the constant matrix of value \a scalar decremented by the coefficients of \a expr
-  *
-  * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
-  */
-template<typename T> friend
-const CwiseBinaryOp<internal::scalar_difference_op<T,Scalar>,Constant<T>,Derived> operator-(const T& scalar, const StorageBaseType& expr);
-#endif
-
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(operator/,quotient)
-#else
-  /**
-    * \brief Component-wise division of the scalar \a s by array elements of \a a.
-    *
-    * \tparam Scalar is the scalar type of \a x. It must be compatible with the scalar type of the given array expression (\c Derived::Scalar).
-    */
-  template<typename T> friend
-  inline const CwiseBinaryOp<internal::scalar_quotient_op<T,Scalar>,Constant<T>,Derived>
-  operator/(const T& s,const StorageBaseType& a);
-#endif
-
-/** \returns an expression of the coefficient-wise ^ operator of *this and \a other
- *
- * \warning this operator is for expression of bool only.
- *
- * Example: \include Cwise_boolean_xor.cpp
- * Output: \verbinclude Cwise_boolean_xor.out
- *
- * \sa operator&&(), select()
- */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-inline const CwiseBinaryOp<internal::scalar_boolean_xor_op, const Derived, const OtherDerived>
-operator^(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  EIGEN_STATIC_ASSERT((internal::is_same<bool,Scalar>::value && internal::is_same<bool,typename OtherDerived::Scalar>::value),
-                      THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL);
-  return CwiseBinaryOp<internal::scalar_boolean_xor_op, const Derived, const OtherDerived>(derived(),other.derived());
-}
-
-// NOTE disabled until we agree on argument order
-#if 0
-/** \cpp11 \returns an expression of the coefficient-wise polygamma function.
-  *
-  * \specialfunctions_module
-  *
-  * It returns the \a n -th derivative of the digamma(psi) evaluated at \c *this.
-  *
-  * \warning Be careful with the order of the parameters: x.polygamma(n) is equivalent to polygamma(n,x)
-  *
-  * \sa Eigen::polygamma()
-  */
-template<typename DerivedN>
-inline const CwiseBinaryOp<internal::scalar_polygamma_op<Scalar>, const DerivedN, const Derived>
-polygamma(const EIGEN_CURRENT_STORAGE_BASE_CLASS<DerivedN> &n) const
-{
-  return CwiseBinaryOp<internal::scalar_polygamma_op<Scalar>, const DerivedN, const Derived>(n.derived(), this->derived());
-}
-#endif
-
-/** \returns an expression of the coefficient-wise zeta function.
-  *
-  * \specialfunctions_module
-  *
-  * It returns the Riemann zeta function of two arguments \c *this and \a q:
-  *
-  * \param q is the shift, it must be > 0
-  *
-  * \note *this is the exponent, it must be > 1.
-  * \note This function supports only float and double scalar types. To support other scalar types, the user has
-  * to provide implementations of zeta(T,T) for any scalar type T to be supported.
-  *
-  * This method is an alias for zeta(*this,q);
-  *
-  * \sa Eigen::zeta()
-  */
-template<typename DerivedQ>
-inline const CwiseBinaryOp<internal::scalar_zeta_op<Scalar>, const Derived, const DerivedQ>
-zeta(const EIGEN_CURRENT_STORAGE_BASE_CLASS<DerivedQ> &q) const
-{
-  return CwiseBinaryOp<internal::scalar_zeta_op<Scalar>, const Derived, const DerivedQ>(this->derived(), q.derived());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc
new file mode 100644
index 0000000..10c7a3e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc
@@ -0,0 +1,347 @@
+
+/** \returns an expression of the coefficient wise product of \c *this and \a other
+ *
+ * \sa MatrixBase::cwiseProduct
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(Derived, OtherDerived, product) operator*(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const {
+  return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived, OtherDerived, product)(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient wise quotient of \c *this and \a other
+ *
+ * \sa MatrixBase::cwiseQuotient
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryOp<
+    internal::scalar_quotient_op<Scalar, typename OtherDerived::Scalar>, const Derived, const OtherDerived>
+operator/(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const {
+  return CwiseBinaryOp<internal::scalar_quotient_op<Scalar, typename OtherDerived::Scalar>, const Derived,
+                       const OtherDerived>(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise min of \c *this and \a other
+ *
+ * Example: \include Cwise_min.cpp
+ * Output: \verbinclude Cwise_min.out
+ *
+ * \sa max()
+ */
+template <int NaNPropagation = PropagateFast, typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_min_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+    min
+#else
+    (min)
+#endif
+    (const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const {
+  return CwiseBinaryOp<internal::scalar_min_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>(
+      derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise min of \c *this and scalar \a other
+ *
+ * \sa max()
+ */
+template <int NaNPropagation = PropagateFast>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_min_op<Scalar, Scalar, NaNPropagation>, const Derived,
+                  const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+    min
+#else
+    (min)
+#endif
+    (const Scalar &other) const {
+  return (min<NaNPropagation>)(Derived::PlainObject::Constant(rows(), cols(), other));
+}
+
+/** \returns an expression of the coefficient-wise max of \c *this and \a other
+ *
+ * Example: \include Cwise_max.cpp
+ * Output: \verbinclude Cwise_max.out
+ *
+ * \sa min()
+ */
+template <int NaNPropagation = PropagateFast, typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_max_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+    max
+#else
+    (max)
+#endif
+    (const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const {
+  return CwiseBinaryOp<internal::scalar_max_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>(
+      derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise max of \c *this and scalar \a other
+ *
+ * \sa min()
+ */
+template <int NaNPropagation = PropagateFast>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_max_op<Scalar, Scalar, NaNPropagation>, const Derived,
+                  const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+    max
+#else
+    (max)
+#endif
+    (const Scalar &other) const {
+  return (max<NaNPropagation>)(Derived::PlainObject::Constant(rows(), cols(), other));
+}
+
+/** \returns an expression of the coefficient-wise absdiff of \c *this and \a other
+ *
+ * Example: \include Cwise_absolute_difference.cpp
+ * Output: \verbinclude Cwise_absolute_difference.out
+ *
+ * \sa absolute_difference()
+ */
+EIGEN_MAKE_CWISE_BINARY_OP(absolute_difference, absolute_difference)
+
+/** \returns an expression of the coefficient-wise absolute_difference of \c *this and scalar \a other
+ *
+ * \sa absolute_difference()
+ */
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_absolute_difference_op<Scalar, Scalar>, const Derived,
+                  const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+    absolute_difference
+#else
+    (absolute_difference)
+#endif
+    (const Scalar &other) const {
+  return (absolute_difference)(Derived::PlainObject::Constant(rows(), cols(), other));
+}
+
+/** \returns an expression of the coefficient-wise power of \c *this to the given array of \a exponents.
+ *
+ * This function computes the coefficient-wise power.
+ *
+ * Example: \include Cwise_array_power_array.cpp
+ * Output: \verbinclude Cwise_array_power_array.out
+ */
+EIGEN_MAKE_CWISE_BINARY_OP(pow, pow)
+
+/** \returns an expression of the coefficient-wise atan2(\c *this, \a y), where \a y is the given array argument.
+ *
+ * This function computes the coefficient-wise atan2.
+ *
+ */
+EIGEN_MAKE_CWISE_BINARY_OP(atan2, atan2)
+
+// TODO code generating macros could be moved to Macros.h and could include generation of documentation
+#define EIGEN_MAKE_CWISE_COMP_OP(OP, COMPARATOR)                                                                     \
+  template <typename OtherDerived>                                                                                   \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const                                                                        \
+      CwiseBinaryOp<internal::scalar_cmp_op<Scalar, typename OtherDerived::Scalar, internal::cmp_##COMPARATOR>,      \
+                    const Derived, const OtherDerived>                                                               \
+      OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const {                                        \
+    return CwiseBinaryOp<internal::scalar_cmp_op<Scalar, typename OtherDerived::Scalar, internal::cmp_##COMPARATOR>, \
+                         const Derived, const OtherDerived>(derived(), other.derived());                             \
+  }                                                                                                                  \
+  typedef CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_##COMPARATOR>, const Derived,          \
+                        const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >                    \
+      Cmp##COMPARATOR##ReturnType;                                                                                   \
+  typedef CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_##COMPARATOR>,                         \
+                        const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject>, const Derived>      \
+      RCmp##COMPARATOR##ReturnType;                                                                                  \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Cmp##COMPARATOR##ReturnType OP(const Scalar &s) const {                \
+    return this->OP(Derived::PlainObject::Constant(rows(), cols(), s));                                              \
+  }                                                                                                                  \
+  EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE const RCmp##COMPARATOR##ReturnType OP(                                \
+      const Scalar &s, const EIGEN_CURRENT_STORAGE_BASE_CLASS<Derived> &d) {                                         \
+    return Derived::PlainObject::Constant(d.rows(), d.cols(), s).OP(d);                                              \
+  }
+
+#define EIGEN_MAKE_CWISE_COMP_R_OP(OP, R_OP, RCOMPARATOR)                                                             \
+  template <typename OtherDerived>                                                                                    \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const                                                                         \
+      CwiseBinaryOp<internal::scalar_cmp_op<typename OtherDerived::Scalar, Scalar, internal::cmp_##RCOMPARATOR>,      \
+                    const OtherDerived, const Derived>                                                                \
+      OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const {                                         \
+    return CwiseBinaryOp<internal::scalar_cmp_op<typename OtherDerived::Scalar, Scalar, internal::cmp_##RCOMPARATOR>, \
+                         const OtherDerived, const Derived>(other.derived(), derived());                              \
+  }                                                                                                                   \
+  EIGEN_DEVICE_FUNC inline const RCmp##RCOMPARATOR##ReturnType OP(const Scalar &s) const {                            \
+    return Derived::PlainObject::Constant(rows(), cols(), s).R_OP(*this);                                             \
+  }                                                                                                                   \
+  friend inline const Cmp##RCOMPARATOR##ReturnType OP(const Scalar &s, const Derived &d) {                            \
+    return d.R_OP(Derived::PlainObject::Constant(d.rows(), d.cols(), s));                                             \
+  }
+
+/** \returns an expression of the coefficient-wise \< operator of *this and \a other
+ *
+ * Example: \include Cwise_less.cpp
+ * Output: \verbinclude Cwise_less.out
+ *
+ * \sa all(), any(), operator>(), operator<=()
+ */
+EIGEN_MAKE_CWISE_COMP_OP(operator<, LT)
+
+/** \returns an expression of the coefficient-wise \<= operator of *this and \a other
+ *
+ * Example: \include Cwise_less_equal.cpp
+ * Output: \verbinclude Cwise_less_equal.out
+ *
+ * \sa all(), any(), operator>=(), operator<()
+ */
+EIGEN_MAKE_CWISE_COMP_OP(operator<=, LE)
+
+/** \returns an expression of the coefficient-wise \> operator of *this and \a other
+ *
+ * Example: \include Cwise_greater.cpp
+ * Output: \verbinclude Cwise_greater.out
+ *
+ * \sa all(), any(), operator>=(), operator<()
+ */
+EIGEN_MAKE_CWISE_COMP_R_OP(operator>, operator<, LT)
+
+/** \returns an expression of the coefficient-wise \>= operator of *this and \a other
+ *
+ * Example: \include Cwise_greater_equal.cpp
+ * Output: \verbinclude Cwise_greater_equal.out
+ *
+ * \sa all(), any(), operator>(), operator<=()
+ */
+EIGEN_MAKE_CWISE_COMP_R_OP(operator>=, operator<=, LE)
+
+/** \returns an expression of the coefficient-wise == operator of *this and \a other
+ *
+ * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+ * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+ * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
+ * isMuchSmallerThan().
+ *
+ * Example: \include Cwise_equal_equal.cpp
+ * Output: \verbinclude Cwise_equal_equal.out
+ *
+ * \sa all(), any(), isApprox(), isMuchSmallerThan()
+ */
+EIGEN_MAKE_CWISE_COMP_OP(operator==, EQ)
+
+/** \returns an expression of the coefficient-wise != operator of *this and \a other
+ *
+ * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+ * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+ * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
+ * isMuchSmallerThan().
+ *
+ * Example: \include Cwise_not_equal.cpp
+ * Output: \verbinclude Cwise_not_equal.out
+ *
+ * \sa all(), any(), isApprox(), isMuchSmallerThan()
+ */
+EIGEN_MAKE_CWISE_COMP_OP(operator!=, NEQ)
+
+#undef EIGEN_MAKE_CWISE_COMP_OP
+#undef EIGEN_MAKE_CWISE_COMP_R_OP
+
+// scalar addition
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+EIGEN_MAKE_SCALAR_BINARY_OP(operator+, sum)
+#else
+/** \returns an expression of \c *this with each coeff incremented by the constant \a scalar
+ *
+ * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
+ *
+ * Example: \include Cwise_plus.cpp
+ * Output: \verbinclude Cwise_plus.out
+ *
+ * \sa operator+=(), operator-()
+ */
+template <typename T>
+const CwiseBinaryOp<internal::scalar_sum_op<Scalar, T>, Derived, Constant<T> > operator+(const T &scalar) const;
+/** \returns an expression of \a expr with each coeff incremented by the constant \a scalar
+ *
+ * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
+ */
+template <typename T>
+friend const CwiseBinaryOp<internal::scalar_sum_op<T, Scalar>, Constant<T>, Derived> operator+(
+    const T &scalar, const StorageBaseType &expr);
+#endif
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+EIGEN_MAKE_SCALAR_BINARY_OP(operator-, difference)
+#else
+/** \returns an expression of \c *this with each coeff decremented by the constant \a scalar
+ *
+ * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
+ *
+ * Example: \include Cwise_minus.cpp
+ * Output: \verbinclude Cwise_minus.out
+ *
+ * \sa operator+=(), operator-()
+ */
+template <typename T>
+const CwiseBinaryOp<internal::scalar_difference_op<Scalar, T>, Derived, Constant<T> > operator-(const T &scalar) const;
+/** \returns an expression of the constant matrix of value \a scalar decremented by the coefficients of \a expr
+ *
+ * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
+ */
+template <typename T>
+friend const CwiseBinaryOp<internal::scalar_difference_op<T, Scalar>, Constant<T>, Derived> operator-(
+    const T &scalar, const StorageBaseType &expr);
+#endif
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(operator/, quotient)
+#else
+/**
+ * \brief Component-wise division of the scalar \a s by array elements of \a a.
+ *
+ * \tparam Scalar is the scalar type of \a x. It must be compatible with the scalar type of the given array expression
+ * (\c Derived::Scalar).
+ */
+template <typename T>
+friend inline const CwiseBinaryOp<internal::scalar_quotient_op<T, Scalar>, Constant<T>, Derived> operator/(
+    const T &s, const StorageBaseType &a);
+#endif
+
+// NOTE disabled until we agree on argument order
+#if 0
+/** \cpp11 \returns an expression of the coefficient-wise polygamma function.
+  *
+  * \specialfunctions_module
+  *
+  * It returns the \a n -th derivative of the digamma(psi) evaluated at \c *this.
+  *
+  * \warning Be careful with the order of the parameters: x.polygamma(n) is equivalent to polygamma(n,x)
+  *
+  * \sa Eigen::polygamma()
+  */
+template<typename DerivedN>
+inline const CwiseBinaryOp<internal::scalar_polygamma_op<Scalar>, const DerivedN, const Derived>
+polygamma(const EIGEN_CURRENT_STORAGE_BASE_CLASS<DerivedN> &n) const
+{
+  return CwiseBinaryOp<internal::scalar_polygamma_op<Scalar>, const DerivedN, const Derived>(n.derived(), this->derived());
+}
+#endif
+
+/** \returns an expression of the coefficient-wise zeta function.
+ *
+ * \specialfunctions_module
+ *
+ * It returns the Riemann zeta function of two arguments \c *this and \a q:
+ *
+ * \param q is the shift, it must be > 0
+ *
+ * \note *this is the exponent, it must be > 1.
+ * \note This function supports only float and double scalar types. To support other scalar types, the user has
+ * to provide implementations of zeta(T,T) for any scalar type T to be supported.
+ *
+ * This method is an alias for zeta(*this,q);
+ *
+ * \sa Eigen::zeta()
+ */
+template <typename DerivedQ>
+inline const CwiseBinaryOp<internal::scalar_zeta_op<Scalar>, const Derived, const DerivedQ> zeta(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<DerivedQ> &q) const {
+  return CwiseBinaryOp<internal::scalar_zeta_op<Scalar>, const Derived, const DerivedQ>(this->derived(), q.derived());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h
deleted file mode 100644
index 13c55f4..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h
+++ /dev/null
@@ -1,696 +0,0 @@
-
-
-typedef CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived> AbsReturnType;
-typedef CwiseUnaryOp<internal::scalar_arg_op<Scalar>, const Derived> ArgReturnType;
-typedef CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const Derived> Abs2ReturnType;
-typedef CwiseUnaryOp<internal::scalar_sqrt_op<Scalar>, const Derived> SqrtReturnType;
-typedef CwiseUnaryOp<internal::scalar_rsqrt_op<Scalar>, const Derived> RsqrtReturnType;
-typedef CwiseUnaryOp<internal::scalar_sign_op<Scalar>, const Derived> SignReturnType;
-typedef CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const Derived> InverseReturnType;
-typedef CwiseUnaryOp<internal::scalar_boolean_not_op<Scalar>, const Derived> BooleanNotReturnType;
-
-typedef CwiseUnaryOp<internal::scalar_exp_op<Scalar>, const Derived> ExpReturnType;
-typedef CwiseUnaryOp<internal::scalar_expm1_op<Scalar>, const Derived> Expm1ReturnType;
-typedef CwiseUnaryOp<internal::scalar_log_op<Scalar>, const Derived> LogReturnType;
-typedef CwiseUnaryOp<internal::scalar_log1p_op<Scalar>, const Derived> Log1pReturnType;
-typedef CwiseUnaryOp<internal::scalar_log10_op<Scalar>, const Derived> Log10ReturnType;
-typedef CwiseUnaryOp<internal::scalar_log2_op<Scalar>, const Derived> Log2ReturnType;
-typedef CwiseUnaryOp<internal::scalar_cos_op<Scalar>, const Derived> CosReturnType;
-typedef CwiseUnaryOp<internal::scalar_sin_op<Scalar>, const Derived> SinReturnType;
-typedef CwiseUnaryOp<internal::scalar_tan_op<Scalar>, const Derived> TanReturnType;
-typedef CwiseUnaryOp<internal::scalar_acos_op<Scalar>, const Derived> AcosReturnType;
-typedef CwiseUnaryOp<internal::scalar_asin_op<Scalar>, const Derived> AsinReturnType;
-typedef CwiseUnaryOp<internal::scalar_atan_op<Scalar>, const Derived> AtanReturnType;
-typedef CwiseUnaryOp<internal::scalar_tanh_op<Scalar>, const Derived> TanhReturnType;
-typedef CwiseUnaryOp<internal::scalar_logistic_op<Scalar>, const Derived> LogisticReturnType;
-typedef CwiseUnaryOp<internal::scalar_sinh_op<Scalar>, const Derived> SinhReturnType;
-#if EIGEN_HAS_CXX11_MATH
-typedef CwiseUnaryOp<internal::scalar_atanh_op<Scalar>, const Derived> AtanhReturnType;
-typedef CwiseUnaryOp<internal::scalar_asinh_op<Scalar>, const Derived> AsinhReturnType;
-typedef CwiseUnaryOp<internal::scalar_acosh_op<Scalar>, const Derived> AcoshReturnType;
-#endif
-typedef CwiseUnaryOp<internal::scalar_cosh_op<Scalar>, const Derived> CoshReturnType;
-typedef CwiseUnaryOp<internal::scalar_square_op<Scalar>, const Derived> SquareReturnType;
-typedef CwiseUnaryOp<internal::scalar_cube_op<Scalar>, const Derived> CubeReturnType;
-typedef CwiseUnaryOp<internal::scalar_round_op<Scalar>, const Derived> RoundReturnType;
-typedef CwiseUnaryOp<internal::scalar_rint_op<Scalar>, const Derived> RintReturnType;
-typedef CwiseUnaryOp<internal::scalar_floor_op<Scalar>, const Derived> FloorReturnType;
-typedef CwiseUnaryOp<internal::scalar_ceil_op<Scalar>, const Derived> CeilReturnType;
-typedef CwiseUnaryOp<internal::scalar_isnan_op<Scalar>, const Derived> IsNaNReturnType;
-typedef CwiseUnaryOp<internal::scalar_isinf_op<Scalar>, const Derived> IsInfReturnType;
-typedef CwiseUnaryOp<internal::scalar_isfinite_op<Scalar>, const Derived> IsFiniteReturnType;
-
-/** \returns an expression of the coefficient-wise absolute value of \c *this
-  *
-  * Example: \include Cwise_abs.cpp
-  * Output: \verbinclude Cwise_abs.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_abs">Math functions</a>, abs2()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const AbsReturnType
-abs() const
-{
-  return AbsReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise phase angle of \c *this
-  *
-  * Example: \include Cwise_arg.cpp
-  * Output: \verbinclude Cwise_arg.out
-  *
-  * \sa abs()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const ArgReturnType
-arg() const
-{
-  return ArgReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise squared absolute value of \c *this
-  *
-  * Example: \include Cwise_abs2.cpp
-  * Output: \verbinclude Cwise_abs2.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_abs2">Math functions</a>, abs(), square()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const Abs2ReturnType
-abs2() const
-{
-  return Abs2ReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise exponential of *this.
-  *
-  * This function computes the coefficient-wise exponential. The function MatrixBase::exp() in the
-  * unsupported module MatrixFunctions computes the matrix exponential.
-  *
-  * Example: \include Cwise_exp.cpp
-  * Output: \verbinclude Cwise_exp.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_exp">Math functions</a>, pow(), log(), sin(), cos()
-  */
-EIGEN_DEVICE_FUNC
-inline const ExpReturnType
-exp() const
-{
-  return ExpReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise exponential of *this minus 1.
-  *
-  * In exact arithmetic, \c x.expm1() is equivalent to \c x.exp() - 1,
-  * however, with finite precision, this function is much more accurate when \c x is close to zero.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_expm1">Math functions</a>, exp()
-  */
-EIGEN_DEVICE_FUNC
-inline const Expm1ReturnType
-expm1() const
-{
-  return Expm1ReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise logarithm of *this.
-  *
-  * This function computes the coefficient-wise logarithm. The function MatrixBase::log() in the
-  * unsupported module MatrixFunctions computes the matrix logarithm.
-  *
-  * Example: \include Cwise_log.cpp
-  * Output: \verbinclude Cwise_log.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log">Math functions</a>, log()
-  */
-EIGEN_DEVICE_FUNC
-inline const LogReturnType
-log() const
-{
-  return LogReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise logarithm of 1 plus \c *this.
-  *
-  * In exact arithmetic, \c x.log() is equivalent to \c (x+1).log(),
-  * however, with finite precision, this function is much more accurate when \c x is close to zero.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log1p">Math functions</a>, log()
-  */
-EIGEN_DEVICE_FUNC
-inline const Log1pReturnType
-log1p() const
-{
-  return Log1pReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise base-10 logarithm of *this.
-  *
-  * This function computes the coefficient-wise base-10 logarithm.
-  *
-  * Example: \include Cwise_log10.cpp
-  * Output: \verbinclude Cwise_log10.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log10">Math functions</a>, log()
-  */
-EIGEN_DEVICE_FUNC
-inline const Log10ReturnType
-log10() const
-{
-  return Log10ReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise base-2 logarithm of *this.
-  *
-  * This function computes the coefficient-wise base-2 logarithm.
-  *
-  */
-EIGEN_DEVICE_FUNC
-inline const Log2ReturnType
-log2() const
-{
-  return Log2ReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise square root of *this.
-  *
-  * This function computes the coefficient-wise square root. The function MatrixBase::sqrt() in the
-  * unsupported module MatrixFunctions computes the matrix square root.
-  *
-  * Example: \include Cwise_sqrt.cpp
-  * Output: \verbinclude Cwise_sqrt.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_sqrt">Math functions</a>, pow(), square()
-  */
-EIGEN_DEVICE_FUNC
-inline const SqrtReturnType
-sqrt() const
-{
-  return SqrtReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise inverse square root of *this.
-  *
-  * This function computes the coefficient-wise inverse square root.
-  *
-  * Example: \include Cwise_sqrt.cpp
-  * Output: \verbinclude Cwise_sqrt.out
-  *
-  * \sa pow(), square()
-  */
-EIGEN_DEVICE_FUNC
-inline const RsqrtReturnType
-rsqrt() const
-{
-  return RsqrtReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise signum of *this.
-  *
-  * This function computes the coefficient-wise signum.
-  *
-  * Example: \include Cwise_sign.cpp
-  * Output: \verbinclude Cwise_sign.out
-  *
-  * \sa pow(), square()
-  */
-EIGEN_DEVICE_FUNC
-inline const SignReturnType
-sign() const
-{
-  return SignReturnType(derived());
-}
-
-
-/** \returns an expression of the coefficient-wise cosine of *this.
-  *
-  * This function computes the coefficient-wise cosine. The function MatrixBase::cos() in the
-  * unsupported module MatrixFunctions computes the matrix cosine.
-  *
-  * Example: \include Cwise_cos.cpp
-  * Output: \verbinclude Cwise_cos.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cos">Math functions</a>, sin(), acos()
-  */
-EIGEN_DEVICE_FUNC
-inline const CosReturnType
-cos() const
-{
-  return CosReturnType(derived());
-}
-
-
-/** \returns an expression of the coefficient-wise sine of *this.
-  *
-  * This function computes the coefficient-wise sine. The function MatrixBase::sin() in the
-  * unsupported module MatrixFunctions computes the matrix sine.
-  *
-  * Example: \include Cwise_sin.cpp
-  * Output: \verbinclude Cwise_sin.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_sin">Math functions</a>, cos(), asin()
-  */
-EIGEN_DEVICE_FUNC
-inline const SinReturnType
-sin() const
-{
-  return SinReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise tan of *this.
-  *
-  * Example: \include Cwise_tan.cpp
-  * Output: \verbinclude Cwise_tan.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_tan">Math functions</a>, cos(), sin()
-  */
-EIGEN_DEVICE_FUNC
-inline const TanReturnType
-tan() const
-{
-  return TanReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise arc tan of *this.
-  *
-  * Example: \include Cwise_atan.cpp
-  * Output: \verbinclude Cwise_atan.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_atan">Math functions</a>, tan(), asin(), acos()
-  */
-EIGEN_DEVICE_FUNC
-inline const AtanReturnType
-atan() const
-{
-  return AtanReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise arc cosine of *this.
-  *
-  * Example: \include Cwise_acos.cpp
-  * Output: \verbinclude Cwise_acos.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_acos">Math functions</a>, cos(), asin()
-  */
-EIGEN_DEVICE_FUNC
-inline const AcosReturnType
-acos() const
-{
-  return AcosReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise arc sine of *this.
-  *
-  * Example: \include Cwise_asin.cpp
-  * Output: \verbinclude Cwise_asin.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_asin">Math functions</a>, sin(), acos()
-  */
-EIGEN_DEVICE_FUNC
-inline const AsinReturnType
-asin() const
-{
-  return AsinReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise hyperbolic tan of *this.
-  *
-  * Example: \include Cwise_tanh.cpp
-  * Output: \verbinclude Cwise_tanh.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_tanh">Math functions</a>, tan(), sinh(), cosh()
-  */
-EIGEN_DEVICE_FUNC
-inline const TanhReturnType
-tanh() const
-{
-  return TanhReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise hyperbolic sin of *this.
-  *
-  * Example: \include Cwise_sinh.cpp
-  * Output: \verbinclude Cwise_sinh.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_sinh">Math functions</a>, sin(), tanh(), cosh()
-  */
-EIGEN_DEVICE_FUNC
-inline const SinhReturnType
-sinh() const
-{
-  return SinhReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise hyperbolic cos of *this.
-  *
-  * Example: \include Cwise_cosh.cpp
-  * Output: \verbinclude Cwise_cosh.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cosh">Math functions</a>, tanh(), sinh(), cosh()
-  */
-EIGEN_DEVICE_FUNC
-inline const CoshReturnType
-cosh() const
-{
-  return CoshReturnType(derived());
-}
-
-#if EIGEN_HAS_CXX11_MATH
-/** \returns an expression of the coefficient-wise inverse hyperbolic tan of *this.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_atanh">Math functions</a>, atanh(), asinh(), acosh()
-  */
-EIGEN_DEVICE_FUNC
-inline const AtanhReturnType
-atanh() const
-{
-  return AtanhReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise inverse hyperbolic sin of *this.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_asinh">Math functions</a>, atanh(), asinh(), acosh()
-  */
-EIGEN_DEVICE_FUNC
-inline const AsinhReturnType
-asinh() const
-{
-  return AsinhReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise inverse hyperbolic cos of *this.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_acosh">Math functions</a>, atanh(), asinh(), acosh()
-  */
-EIGEN_DEVICE_FUNC
-inline const AcoshReturnType
-acosh() const
-{
-  return AcoshReturnType(derived());
-}
-#endif
-
-/** \returns an expression of the coefficient-wise logistic of *this.
-  */
-EIGEN_DEVICE_FUNC
-inline const LogisticReturnType
-logistic() const
-{
-  return LogisticReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise inverse of *this.
-  *
-  * Example: \include Cwise_inverse.cpp
-  * Output: \verbinclude Cwise_inverse.out
-  *
-  * \sa operator/(), operator*()
-  */
-EIGEN_DEVICE_FUNC
-inline const InverseReturnType
-inverse() const
-{
-  return InverseReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise square of *this.
-  *
-  * Example: \include Cwise_square.cpp
-  * Output: \verbinclude Cwise_square.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_squareE">Math functions</a>, abs2(), cube(), pow()
-  */
-EIGEN_DEVICE_FUNC
-inline const SquareReturnType
-square() const
-{
-  return SquareReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise cube of *this.
-  *
-  * Example: \include Cwise_cube.cpp
-  * Output: \verbinclude Cwise_cube.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cube">Math functions</a>, square(), pow()
-  */
-EIGEN_DEVICE_FUNC
-inline const CubeReturnType
-cube() const
-{
-  return CubeReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise rint of *this.
-  *
-  * Example: \include Cwise_rint.cpp
-  * Output: \verbinclude Cwise_rint.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_rint">Math functions</a>, ceil(), floor()
-  */
-EIGEN_DEVICE_FUNC
-inline const RintReturnType
-rint() const
-{
-  return RintReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise round of *this.
-  *
-  * Example: \include Cwise_round.cpp
-  * Output: \verbinclude Cwise_round.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_round">Math functions</a>, ceil(), floor()
-  */
-EIGEN_DEVICE_FUNC
-inline const RoundReturnType
-round() const
-{
-  return RoundReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise floor of *this.
-  *
-  * Example: \include Cwise_floor.cpp
-  * Output: \verbinclude Cwise_floor.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_floor">Math functions</a>, ceil(), round()
-  */
-EIGEN_DEVICE_FUNC
-inline const FloorReturnType
-floor() const
-{
-  return FloorReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise ceil of *this.
-  *
-  * Example: \include Cwise_ceil.cpp
-  * Output: \verbinclude Cwise_ceil.out
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_ceil">Math functions</a>, floor(), round()
-  */
-EIGEN_DEVICE_FUNC
-inline const CeilReturnType
-ceil() const
-{
-  return CeilReturnType(derived());
-}
-
-template<int N> struct ShiftRightXpr {
-  typedef CwiseUnaryOp<internal::scalar_shift_right_op<Scalar, N>, const Derived> Type;
-};
-
-/** \returns an expression of \c *this with the \a Scalar type arithmetically
-  * shifted right by \a N bit positions.
-  *
-  * The template parameter \a N specifies the number of bit positions to shift.
-  * 
-  * \sa shiftLeft()
-  */
-template<int N>
-EIGEN_DEVICE_FUNC
-typename ShiftRightXpr<N>::Type
-shiftRight() const
-{
-  return typename ShiftRightXpr<N>::Type(derived());
-}
-
-
-template<int N> struct ShiftLeftXpr {
-  typedef CwiseUnaryOp<internal::scalar_shift_left_op<Scalar, N>, const Derived> Type;
-};
-
-/** \returns an expression of \c *this with the \a Scalar type logically
-  * shifted left by \a N bit positions.
-  *
-  * The template parameter \a N specifies the number of bit positions to shift.
-  *
-  * \sa shiftRight()
-  */
-template<int N>
-EIGEN_DEVICE_FUNC
-typename ShiftLeftXpr<N>::Type
-shiftLeft() const
-{
-  return typename ShiftLeftXpr<N>::Type(derived());
-}
-
-/** \returns an expression of the coefficient-wise isnan of *this.
-  *
-  * Example: \include Cwise_isNaN.cpp
-  * Output: \verbinclude Cwise_isNaN.out
-  *
-  * \sa isfinite(), isinf()
-  */
-EIGEN_DEVICE_FUNC
-inline const IsNaNReturnType
-isNaN() const
-{
-  return IsNaNReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise isinf of *this.
-  *
-  * Example: \include Cwise_isInf.cpp
-  * Output: \verbinclude Cwise_isInf.out
-  *
-  * \sa isnan(), isfinite()
-  */
-EIGEN_DEVICE_FUNC
-inline const IsInfReturnType
-isInf() const
-{
-  return IsInfReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise isfinite of *this.
-  *
-  * Example: \include Cwise_isFinite.cpp
-  * Output: \verbinclude Cwise_isFinite.out
-  *
-  * \sa isnan(), isinf()
-  */
-EIGEN_DEVICE_FUNC
-inline const IsFiniteReturnType
-isFinite() const
-{
-  return IsFiniteReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise ! operator of *this
-  *
-  * \warning this operator is for expression of bool only.
-  *
-  * Example: \include Cwise_boolean_not.cpp
-  * Output: \verbinclude Cwise_boolean_not.out
-  *
-  * \sa operator!=()
-  */
-EIGEN_DEVICE_FUNC
-inline const BooleanNotReturnType
-operator!() const
-{
-  EIGEN_STATIC_ASSERT((internal::is_same<bool,Scalar>::value),
-                      THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL);
-  return BooleanNotReturnType(derived());
-}
-
-
-// --- SpecialFunctions module ---
-
-typedef CwiseUnaryOp<internal::scalar_lgamma_op<Scalar>, const Derived> LgammaReturnType;
-typedef CwiseUnaryOp<internal::scalar_digamma_op<Scalar>, const Derived> DigammaReturnType;
-typedef CwiseUnaryOp<internal::scalar_erf_op<Scalar>, const Derived> ErfReturnType;
-typedef CwiseUnaryOp<internal::scalar_erfc_op<Scalar>, const Derived> ErfcReturnType;
-typedef CwiseUnaryOp<internal::scalar_ndtri_op<Scalar>, const Derived> NdtriReturnType;
-
-/** \cpp11 \returns an expression of the coefficient-wise ln(|gamma(*this)|).
-  *
-  * \specialfunctions_module
-  *
-  * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
-  * or float/double in non c++11 mode, the user has to provide implementations of lgamma(T) for any scalar
-  * type T to be supported.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_lgamma">Math functions</a>, digamma()
-  */
-EIGEN_DEVICE_FUNC
-inline const LgammaReturnType
-lgamma() const
-{
-  return LgammaReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise digamma (psi, derivative of lgamma).
-  *
-  * \specialfunctions_module
-  *
-  * \note This function supports only float and double scalar types. To support other scalar types,
-  * the user has to provide implementations of digamma(T) for any scalar
-  * type T to be supported.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_digamma">Math functions</a>, Eigen::digamma(), Eigen::polygamma(), lgamma()
-  */
-EIGEN_DEVICE_FUNC
-inline const DigammaReturnType
-digamma() const
-{
-  return DigammaReturnType(derived());
-}
-
-/** \cpp11 \returns an expression of the coefficient-wise Gauss error
-  * function of *this.
-  *
-  * \specialfunctions_module
-  *
-  * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
-  * or float/double in non c++11 mode, the user has to provide implementations of erf(T) for any scalar
-  * type T to be supported.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_erf">Math functions</a>, erfc()
-  */
-EIGEN_DEVICE_FUNC
-inline const ErfReturnType
-erf() const
-{
-  return ErfReturnType(derived());
-}
-
-/** \cpp11 \returns an expression of the coefficient-wise Complementary error
-  * function of *this.
-  *
-  * \specialfunctions_module
-  *
-  * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
-  * or float/double in non c++11 mode, the user has to provide implementations of erfc(T) for any scalar
-  * type T to be supported.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_erfc">Math functions</a>, erf()
-  */
-EIGEN_DEVICE_FUNC
-inline const ErfcReturnType
-erfc() const
-{
-  return ErfcReturnType(derived());
-}
-
-/** \returns an expression of the coefficient-wise inverse of the CDF of the Normal distribution function
-  * function of *this.
-  *
-  * \specialfunctions_module
-  * 
-  * In other words, considering `x = ndtri(y)`, it returns the argument, x, for which the area under the
-  * Gaussian probability density function (integrated from minus infinity to x) is equal to y.
-  *
-  * \note This function supports only float and double scalar types. To support other scalar types,
-  * the user has to provide implementations of ndtri(T) for any scalar type T to be supported.
-  *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_ndtri">Math functions</a>
-  */
-EIGEN_DEVICE_FUNC
-inline const NdtriReturnType
-ndtri() const
-{
-  return NdtriReturnType(derived());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc
new file mode 100644
index 0000000..d03edc2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc
@@ -0,0 +1,525 @@
+
+
+typedef CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived> AbsReturnType;
+typedef CwiseUnaryOp<internal::scalar_arg_op<Scalar>, const Derived> ArgReturnType;
+typedef CwiseUnaryOp<internal::scalar_carg_op<Scalar>, const Derived> CArgReturnType;
+typedef CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const Derived> Abs2ReturnType;
+typedef CwiseUnaryOp<internal::scalar_sqrt_op<Scalar>, const Derived> SqrtReturnType;
+typedef CwiseUnaryOp<internal::scalar_cbrt_op<Scalar>, const Derived> CbrtReturnType;
+typedef CwiseUnaryOp<internal::scalar_rsqrt_op<Scalar>, const Derived> RsqrtReturnType;
+typedef CwiseUnaryOp<internal::scalar_sign_op<Scalar>, const Derived> SignReturnType;
+typedef CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const Derived> InverseReturnType;
+typedef CwiseUnaryOp<internal::scalar_boolean_not_op<Scalar>, const Derived> BooleanNotReturnType;
+typedef CwiseUnaryOp<internal::scalar_bitwise_not_op<Scalar>, const Derived> BitwiseNotReturnType;
+
+typedef CwiseUnaryOp<internal::scalar_exp_op<Scalar>, const Derived> ExpReturnType;
+typedef CwiseUnaryOp<internal::scalar_expm1_op<Scalar>, const Derived> Expm1ReturnType;
+typedef CwiseUnaryOp<internal::scalar_log_op<Scalar>, const Derived> LogReturnType;
+typedef CwiseUnaryOp<internal::scalar_log1p_op<Scalar>, const Derived> Log1pReturnType;
+typedef CwiseUnaryOp<internal::scalar_log10_op<Scalar>, const Derived> Log10ReturnType;
+typedef CwiseUnaryOp<internal::scalar_log2_op<Scalar>, const Derived> Log2ReturnType;
+typedef CwiseUnaryOp<internal::scalar_cos_op<Scalar>, const Derived> CosReturnType;
+typedef CwiseUnaryOp<internal::scalar_sin_op<Scalar>, const Derived> SinReturnType;
+typedef CwiseUnaryOp<internal::scalar_tan_op<Scalar>, const Derived> TanReturnType;
+typedef CwiseUnaryOp<internal::scalar_acos_op<Scalar>, const Derived> AcosReturnType;
+typedef CwiseUnaryOp<internal::scalar_asin_op<Scalar>, const Derived> AsinReturnType;
+typedef CwiseUnaryOp<internal::scalar_atan_op<Scalar>, const Derived> AtanReturnType;
+typedef CwiseUnaryOp<internal::scalar_tanh_op<Scalar>, const Derived> TanhReturnType;
+typedef CwiseUnaryOp<internal::scalar_logistic_op<Scalar>, const Derived> LogisticReturnType;
+typedef CwiseUnaryOp<internal::scalar_sinh_op<Scalar>, const Derived> SinhReturnType;
+typedef CwiseUnaryOp<internal::scalar_atanh_op<Scalar>, const Derived> AtanhReturnType;
+typedef CwiseUnaryOp<internal::scalar_asinh_op<Scalar>, const Derived> AsinhReturnType;
+typedef CwiseUnaryOp<internal::scalar_acosh_op<Scalar>, const Derived> AcoshReturnType;
+typedef CwiseUnaryOp<internal::scalar_cosh_op<Scalar>, const Derived> CoshReturnType;
+typedef CwiseUnaryOp<internal::scalar_square_op<Scalar>, const Derived> SquareReturnType;
+typedef CwiseUnaryOp<internal::scalar_cube_op<Scalar>, const Derived> CubeReturnType;
+typedef CwiseUnaryOp<internal::scalar_round_op<Scalar>, const Derived> RoundReturnType;
+typedef CwiseUnaryOp<internal::scalar_rint_op<Scalar>, const Derived> RintReturnType;
+typedef CwiseUnaryOp<internal::scalar_floor_op<Scalar>, const Derived> FloorReturnType;
+typedef CwiseUnaryOp<internal::scalar_ceil_op<Scalar>, const Derived> CeilReturnType;
+typedef CwiseUnaryOp<internal::scalar_isnan_op<Scalar>, const Derived> IsNaNReturnType;
+typedef CwiseUnaryOp<internal::scalar_isinf_op<Scalar>, const Derived> IsInfReturnType;
+typedef CwiseUnaryOp<internal::scalar_isfinite_op<Scalar>, const Derived> IsFiniteReturnType;
+
+/** \returns an expression of the coefficient-wise absolute value of \c *this
+ *
+ * Example: \include Cwise_abs.cpp
+ * Output: \verbinclude Cwise_abs.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_abs">Math functions</a>, abs2()
+ */
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const AbsReturnType abs() const { return AbsReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise phase angle of \c *this
+ *
+ * Example: \include Cwise_arg.cpp
+ * Output: \verbinclude Cwise_arg.out
+ *
+ * \sa abs()
+ */
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ArgReturnType arg() const { return ArgReturnType(derived()); }
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CArgReturnType carg() const { return CArgReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise squared absolute value of \c *this
+ *
+ * Example: \include Cwise_abs2.cpp
+ * Output: \verbinclude Cwise_abs2.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_abs2">Math functions</a>, abs(), square()
+ */
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Abs2ReturnType abs2() const { return Abs2ReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise exponential of *this.
+ *
+ * This function computes the coefficient-wise exponential. The function MatrixBase::exp() in the
+ * unsupported module MatrixFunctions computes the matrix exponential.
+ *
+ * Example: \include Cwise_exp.cpp
+ * Output: \verbinclude Cwise_exp.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_exp">Math functions</a>, pow(), log(), sin(), cos()
+ */
+EIGEN_DEVICE_FUNC inline const ExpReturnType exp() const { return ExpReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise exponential of *this minus 1.
+ *
+ * In exact arithmetic, \c x.expm1() is equivalent to \c x.exp() - 1,
+ * however, with finite precision, this function is much more accurate when \c x is close to zero.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_expm1">Math functions</a>, exp()
+ */
+EIGEN_DEVICE_FUNC inline const Expm1ReturnType expm1() const { return Expm1ReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise logarithm of *this.
+ *
+ * This function computes the coefficient-wise logarithm. The function MatrixBase::log() in the
+ * unsupported module MatrixFunctions computes the matrix logarithm.
+ *
+ * Example: \include Cwise_log.cpp
+ * Output: \verbinclude Cwise_log.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log">Math functions</a>, log()
+ */
+EIGEN_DEVICE_FUNC inline const LogReturnType log() const { return LogReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise logarithm of 1 plus \c *this.
+ *
+ * In exact arithmetic, \c x.log() is equivalent to \c (x+1).log(),
+ * however, with finite precision, this function is much more accurate when \c x is close to zero.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log1p">Math functions</a>, log()
+ */
+EIGEN_DEVICE_FUNC inline const Log1pReturnType log1p() const { return Log1pReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise base-10 logarithm of *this.
+ *
+ * This function computes the coefficient-wise base-10 logarithm.
+ *
+ * Example: \include Cwise_log10.cpp
+ * Output: \verbinclude Cwise_log10.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log10">Math functions</a>, log()
+ */
+EIGEN_DEVICE_FUNC inline const Log10ReturnType log10() const { return Log10ReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise base-2 logarithm of *this.
+ *
+ * This function computes the coefficient-wise base-2 logarithm.
+ *
+ */
+EIGEN_DEVICE_FUNC inline const Log2ReturnType log2() const { return Log2ReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise square root of *this.
+ *
+ * This function computes the coefficient-wise square root. The function MatrixBase::sqrt() in the
+ * unsupported module MatrixFunctions computes the matrix square root.
+ *
+ * Example: \include Cwise_sqrt.cpp
+ * Output: \verbinclude Cwise_sqrt.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_sqrt">Math functions</a>, pow(), square(), cbrt()
+ */
+EIGEN_DEVICE_FUNC inline const SqrtReturnType sqrt() const { return SqrtReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise cube root of *this.
+ *
+ * This function computes the coefficient-wise cube root.
+ *
+ * Example: \include Cwise_cbrt.cpp
+ * Output: \verbinclude Cwise_cbrt.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cbrt">Math functions</a>, sqrt(), pow(), square()
+ */
+EIGEN_DEVICE_FUNC inline const CbrtReturnType cbrt() const { return CbrtReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise inverse square root of *this.
+ *
+ * This function computes the coefficient-wise inverse square root.
+ *
+ * Example: \include Cwise_sqrt.cpp
+ * Output: \verbinclude Cwise_sqrt.out
+ *
+ * \sa pow(), square()
+ */
+EIGEN_DEVICE_FUNC inline const RsqrtReturnType rsqrt() const { return RsqrtReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise signum of *this.
+ *
+ * This function computes the coefficient-wise signum.
+ *
+ * Example: \include Cwise_sign.cpp
+ * Output: \verbinclude Cwise_sign.out
+ *
+ * \sa pow(), square()
+ */
+EIGEN_DEVICE_FUNC inline const SignReturnType sign() const { return SignReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise cosine of *this.
+ *
+ * This function computes the coefficient-wise cosine. The function MatrixBase::cos() in the
+ * unsupported module MatrixFunctions computes the matrix cosine.
+ *
+ * Example: \include Cwise_cos.cpp
+ * Output: \verbinclude Cwise_cos.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cos">Math functions</a>, sin(), acos()
+ */
+EIGEN_DEVICE_FUNC inline const CosReturnType cos() const { return CosReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise sine of *this.
+ *
+ * This function computes the coefficient-wise sine. The function MatrixBase::sin() in the
+ * unsupported module MatrixFunctions computes the matrix sine.
+ *
+ * Example: \include Cwise_sin.cpp
+ * Output: \verbinclude Cwise_sin.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_sin">Math functions</a>, cos(), asin()
+ */
+EIGEN_DEVICE_FUNC inline const SinReturnType sin() const { return SinReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise tan of *this.
+ *
+ * Example: \include Cwise_tan.cpp
+ * Output: \verbinclude Cwise_tan.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_tan">Math functions</a>, cos(), sin()
+ */
+EIGEN_DEVICE_FUNC inline const TanReturnType tan() const { return TanReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise arc tan of *this.
+ *
+ * Example: \include Cwise_atan.cpp
+ * Output: \verbinclude Cwise_atan.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_atan">Math functions</a>, tan(), asin(), acos()
+ */
+EIGEN_DEVICE_FUNC inline const AtanReturnType atan() const { return AtanReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise arc cosine of *this.
+ *
+ * Example: \include Cwise_acos.cpp
+ * Output: \verbinclude Cwise_acos.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_acos">Math functions</a>, cos(), asin()
+ */
+EIGEN_DEVICE_FUNC inline const AcosReturnType acos() const { return AcosReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise arc sine of *this.
+ *
+ * Example: \include Cwise_asin.cpp
+ * Output: \verbinclude Cwise_asin.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_asin">Math functions</a>, sin(), acos()
+ */
+EIGEN_DEVICE_FUNC inline const AsinReturnType asin() const { return AsinReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise hyperbolic tan of *this.
+ *
+ * Example: \include Cwise_tanh.cpp
+ * Output: \verbinclude Cwise_tanh.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_tanh">Math functions</a>, tan(), sinh(), cosh()
+ */
+EIGEN_DEVICE_FUNC inline const TanhReturnType tanh() const { return TanhReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise hyperbolic sin of *this.
+ *
+ * Example: \include Cwise_sinh.cpp
+ * Output: \verbinclude Cwise_sinh.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_sinh">Math functions</a>, sin(), tanh(), cosh()
+ */
+EIGEN_DEVICE_FUNC inline const SinhReturnType sinh() const { return SinhReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise hyperbolic cos of *this.
+ *
+ * Example: \include Cwise_cosh.cpp
+ * Output: \verbinclude Cwise_cosh.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cosh">Math functions</a>, tanh(), sinh(), cosh()
+ */
+EIGEN_DEVICE_FUNC inline const CoshReturnType cosh() const { return CoshReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise inverse hyperbolic tan of *this.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_atanh">Math functions</a>, atanh(), asinh(), acosh()
+ */
+EIGEN_DEVICE_FUNC inline const AtanhReturnType atanh() const { return AtanhReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise inverse hyperbolic sin of *this.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_asinh">Math functions</a>, atanh(), asinh(), acosh()
+ */
+EIGEN_DEVICE_FUNC inline const AsinhReturnType asinh() const { return AsinhReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise inverse hyperbolic cos of *this.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_acosh">Math functions</a>, atanh(), asinh(), acosh()
+ */
+EIGEN_DEVICE_FUNC inline const AcoshReturnType acosh() const { return AcoshReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise logistic of *this.
+ */
+EIGEN_DEVICE_FUNC inline const LogisticReturnType logistic() const { return LogisticReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise inverse of *this.
+ *
+ * Example: \include Cwise_inverse.cpp
+ * Output: \verbinclude Cwise_inverse.out
+ *
+ * \sa operator/(), operator*()
+ */
+EIGEN_DEVICE_FUNC inline const InverseReturnType inverse() const { return InverseReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise square of *this.
+ *
+ * Example: \include Cwise_square.cpp
+ * Output: \verbinclude Cwise_square.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_squareE">Math functions</a>, abs2(), cube(), pow()
+ */
+EIGEN_DEVICE_FUNC inline const SquareReturnType square() const { return SquareReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise cube of *this.
+ *
+ * Example: \include Cwise_cube.cpp
+ * Output: \verbinclude Cwise_cube.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cube">Math functions</a>, square(), pow()
+ */
+EIGEN_DEVICE_FUNC inline const CubeReturnType cube() const { return CubeReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise rint of *this.
+ *
+ * Example: \include Cwise_rint.cpp
+ * Output: \verbinclude Cwise_rint.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_rint">Math functions</a>, ceil(), floor()
+ */
+EIGEN_DEVICE_FUNC inline const RintReturnType rint() const { return RintReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise round of *this.
+ *
+ * Example: \include Cwise_round.cpp
+ * Output: \verbinclude Cwise_round.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_round">Math functions</a>, ceil(), floor()
+ */
+EIGEN_DEVICE_FUNC inline const RoundReturnType round() const { return RoundReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise floor of *this.
+ *
+ * Example: \include Cwise_floor.cpp
+ * Output: \verbinclude Cwise_floor.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_floor">Math functions</a>, ceil(), round()
+ */
+EIGEN_DEVICE_FUNC inline const FloorReturnType floor() const { return FloorReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise ceil of *this.
+ *
+ * Example: \include Cwise_ceil.cpp
+ * Output: \verbinclude Cwise_ceil.out
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_ceil">Math functions</a>, floor(), round()
+ */
+EIGEN_DEVICE_FUNC inline const CeilReturnType ceil() const { return CeilReturnType(derived()); }
+
+template <int N>
+struct ShiftRightXpr {
+  typedef CwiseUnaryOp<internal::scalar_shift_right_op<Scalar, N>, const Derived> Type;
+};
+
+/** \returns an expression of \c *this with the \a Scalar type arithmetically
+ * shifted right by \a N bit positions.
+ *
+ * The template parameter \a N specifies the number of bit positions to shift.
+ *
+ * \sa shiftLeft()
+ */
+template <int N>
+EIGEN_DEVICE_FUNC typename ShiftRightXpr<N>::Type shiftRight() const {
+  return typename ShiftRightXpr<N>::Type(derived());
+}
+
+template <int N>
+struct ShiftLeftXpr {
+  typedef CwiseUnaryOp<internal::scalar_shift_left_op<Scalar, N>, const Derived> Type;
+};
+
+/** \returns an expression of \c *this with the \a Scalar type logically
+ * shifted left by \a N bit positions.
+ *
+ * The template parameter \a N specifies the number of bit positions to shift.
+ *
+ * \sa shiftRight()
+ */
+template <int N>
+EIGEN_DEVICE_FUNC typename ShiftLeftXpr<N>::Type shiftLeft() const {
+  return typename ShiftLeftXpr<N>::Type(derived());
+}
+
+/** \returns an expression of the coefficient-wise isnan of *this.
+ *
+ * Example: \include Cwise_isNaN.cpp
+ * Output: \verbinclude Cwise_isNaN.out
+ *
+ * \sa isfinite(), isinf()
+ */
+EIGEN_DEVICE_FUNC inline const IsNaNReturnType isNaN() const { return IsNaNReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise isinf of *this.
+ *
+ * Example: \include Cwise_isInf.cpp
+ * Output: \verbinclude Cwise_isInf.out
+ *
+ * \sa isnan(), isfinite()
+ */
+EIGEN_DEVICE_FUNC inline const IsInfReturnType isInf() const { return IsInfReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise isfinite of *this.
+ *
+ * Example: \include Cwise_isFinite.cpp
+ * Output: \verbinclude Cwise_isFinite.out
+ *
+ * \sa isnan(), isinf()
+ */
+EIGEN_DEVICE_FUNC inline const IsFiniteReturnType isFinite() const { return IsFiniteReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise ! operator of *this
+ *
+ * Example: \include Cwise_boolean_not.cpp
+ * Output: \verbinclude Cwise_boolean_not.out
+ *
+ * \sa operator!=()
+ */
+EIGEN_DEVICE_FUNC inline const BooleanNotReturnType operator!() const { return BooleanNotReturnType(derived()); }
+
+/** \returns an expression of the bitwise ~ operator of *this
+ */
+EIGEN_DEVICE_FUNC inline const BitwiseNotReturnType operator~() const { return BitwiseNotReturnType(derived()); }
+
+// --- SpecialFunctions module ---
+
+typedef CwiseUnaryOp<internal::scalar_lgamma_op<Scalar>, const Derived> LgammaReturnType;
+typedef CwiseUnaryOp<internal::scalar_digamma_op<Scalar>, const Derived> DigammaReturnType;
+typedef CwiseUnaryOp<internal::scalar_erf_op<Scalar>, const Derived> ErfReturnType;
+typedef CwiseUnaryOp<internal::scalar_erfc_op<Scalar>, const Derived> ErfcReturnType;
+typedef CwiseUnaryOp<internal::scalar_ndtri_op<Scalar>, const Derived> NdtriReturnType;
+
+/** \cpp11 \returns an expression of the coefficient-wise ln(|gamma(*this)|).
+ *
+ * \specialfunctions_module
+ *
+ * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
+ * or float/double in non c++11 mode, the user has to provide implementations of lgamma(T) for any scalar
+ * type T to be supported.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_lgamma">Math functions</a>, digamma()
+ */
+EIGEN_DEVICE_FUNC inline const LgammaReturnType lgamma() const { return LgammaReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise digamma (psi, derivative of lgamma).
+ *
+ * \specialfunctions_module
+ *
+ * \note This function supports only float and double scalar types. To support other scalar types,
+ * the user has to provide implementations of digamma(T) for any scalar
+ * type T to be supported.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_digamma">Math functions</a>, Eigen::digamma(),
+ * Eigen::polygamma(), lgamma()
+ */
+EIGEN_DEVICE_FUNC inline const DigammaReturnType digamma() const { return DigammaReturnType(derived()); }
+
+/** \cpp11 \returns an expression of the coefficient-wise Gauss error
+ * function of *this.
+ *
+ * \specialfunctions_module
+ *
+ * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
+ * or float/double in non c++11 mode, the user has to provide implementations of erf(T) for any scalar
+ * type T to be supported.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_erf">Math functions</a>, erfc()
+ */
+EIGEN_DEVICE_FUNC inline const ErfReturnType erf() const { return ErfReturnType(derived()); }
+
+/** \cpp11 \returns an expression of the coefficient-wise Complementary error
+ * function of *this.
+ *
+ * \specialfunctions_module
+ *
+ * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
+ * or float/double in non c++11 mode, the user has to provide implementations of erfc(T) for any scalar
+ * type T to be supported.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_erfc">Math functions</a>, erf()
+ */
+EIGEN_DEVICE_FUNC inline const ErfcReturnType erfc() const { return ErfcReturnType(derived()); }
+
+/** \returns an expression of the coefficient-wise inverse of the CDF of the Normal distribution function
+ * function of *this.
+ *
+ * \specialfunctions_module
+ *
+ * In other words, considering `x = ndtri(y)`, it returns the argument, x, for which the area under the
+ * Gaussian probability density function (integrated from minus infinity to x) is equal to y.
+ *
+ * \note This function supports only float and double scalar types. To support other scalar types,
+ * the user has to provide implementations of ndtri(T) for any scalar type T to be supported.
+ *
+ * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_ndtri">Math functions</a>
+ */
+EIGEN_DEVICE_FUNC inline const NdtriReturnType ndtri() const { return NdtriReturnType(derived()); }
+
+template <typename ScalarExponent>
+using UnaryPowReturnType =
+    std::enable_if_t<internal::is_arithmetic<typename NumTraits<ScalarExponent>::Real>::value,
+                     CwiseUnaryOp<internal::scalar_unary_pow_op<Scalar, ScalarExponent>, const Derived>>;
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template <typename ScalarExponent>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryPowReturnType<ScalarExponent> pow(
+    const ScalarExponent& exponent) const {
+  return UnaryPowReturnType<ScalarExponent>(derived(), internal::scalar_unary_pow_op<Scalar, ScalarExponent>(exponent));
+#else
+/** \returns an expression of the coefficients of \c *this rasied to the constant power \a exponent
+ *
+ * \tparam T is the scalar type of \a exponent. It must be compatible with the scalar type of the given expression.
+ *
+ * This function computes the coefficient-wise power. The function MatrixBase::pow() in the
+ * unsupported module MatrixFunctions computes the matrix power.
+ *
+ * Example: \include Cwise_pow.cpp
+ * Output: \verbinclude Cwise_pow.out
+ *
+ * \sa ArrayBase::pow(ArrayBase), square(), cube(), exp(), log()
+ */
+template <typename ScalarExponent>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryPowReturnType<ScalarExponent> pow(
+    const ScalarExponent& exponent) const;
+#endif
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.h
deleted file mode 100644
index 63a52a6..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.h
+++ /dev/null
@@ -1,1442 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-
-/// \internal expression type of a column */
-typedef Block<Derived, internal::traits<Derived>::RowsAtCompileTime, 1, !IsRowMajor> ColXpr;
-typedef const Block<const Derived, internal::traits<Derived>::RowsAtCompileTime, 1, !IsRowMajor> ConstColXpr;
-/// \internal expression type of a row */
-typedef Block<Derived, 1, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> RowXpr;
-typedef const Block<const Derived, 1, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> ConstRowXpr;
-/// \internal expression type of a block of whole columns */
-typedef Block<Derived, internal::traits<Derived>::RowsAtCompileTime, Dynamic, !IsRowMajor> ColsBlockXpr;
-typedef const Block<const Derived, internal::traits<Derived>::RowsAtCompileTime, Dynamic, !IsRowMajor> ConstColsBlockXpr;
-/// \internal expression type of a block of whole rows */
-typedef Block<Derived, Dynamic, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> RowsBlockXpr;
-typedef const Block<const Derived, Dynamic, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> ConstRowsBlockXpr;
-/// \internal expression type of a block of whole columns */
-template<int N> struct NColsBlockXpr { typedef Block<Derived, internal::traits<Derived>::RowsAtCompileTime, N, !IsRowMajor> Type; };
-template<int N> struct ConstNColsBlockXpr { typedef const Block<const Derived, internal::traits<Derived>::RowsAtCompileTime, N, !IsRowMajor> Type; };
-/// \internal expression type of a block of whole rows */
-template<int N> struct NRowsBlockXpr { typedef Block<Derived, N, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> Type; };
-template<int N> struct ConstNRowsBlockXpr { typedef const Block<const Derived, N, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> Type; };
-/// \internal expression of a block */
-typedef Block<Derived> BlockXpr;
-typedef const Block<const Derived> ConstBlockXpr;
-/// \internal expression of a block of fixed sizes */
-template<int Rows, int Cols> struct FixedBlockXpr { typedef Block<Derived,Rows,Cols> Type; };
-template<int Rows, int Cols> struct ConstFixedBlockXpr { typedef Block<const Derived,Rows,Cols> Type; };
-
-typedef VectorBlock<Derived> SegmentReturnType;
-typedef const VectorBlock<const Derived> ConstSegmentReturnType;
-template<int Size> struct FixedSegmentReturnType { typedef VectorBlock<Derived, Size> Type; };
-template<int Size> struct ConstFixedSegmentReturnType { typedef const VectorBlock<const Derived, Size> Type; };
-
-/// \internal inner-vector
-typedef Block<Derived,IsRowMajor?1:Dynamic,IsRowMajor?Dynamic:1,true>       InnerVectorReturnType;
-typedef Block<const Derived,IsRowMajor?1:Dynamic,IsRowMajor?Dynamic:1,true> ConstInnerVectorReturnType;
-
-/// \internal set of inner-vectors
-typedef Block<Derived,Dynamic,Dynamic,true> InnerVectorsReturnType;
-typedef Block<const Derived,Dynamic,Dynamic,true> ConstInnerVectorsReturnType;
-
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-/// \returns an expression of a block in \c *this with either dynamic or fixed sizes.
-///
-/// \param  startRow  the first row in the block
-/// \param  startCol  the first column in the block
-/// \param  blockRows number of rows in the block, specified at either run-time or compile-time
-/// \param  blockCols number of columns in the block, specified at either run-time or compile-time
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example using runtime (aka dynamic) sizes: \include MatrixBase_block_int_int_int_int.cpp
-/// Output: \verbinclude MatrixBase_block_int_int_int_int.out
-///
-/// \newin{3.4}:
-///
-/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments. In the later case, \c n plays the role of a runtime fallback value in case \c N equals Eigen::Dynamic.
-/// Here is an example with a fixed number of rows \c NRows and dynamic number of columns \c cols:
-/// \code
-/// mat.block(i,j,fix<NRows>,cols)
-/// \endcode
-///
-/// This function thus fully covers the features offered by the following overloads block<NRows,NCols>(Index, Index),
-/// and block<NRows,NCols>(Index, Index, Index, Index) that are thus obsolete. Indeed, this generic version avoids
-/// redundancy, it preserves the argument order, and prevents the need to rely on the template keyword in templated code.
-///
-/// but with less redundancy and more consistency as it does not modify the argument order
-/// and seamlessly enable hybrid fixed/dynamic sizes.
-///
-/// \note Even in the case that the returned expression has dynamic size, in the case
-/// when it is applied to a fixed-size matrix, it inherits a fixed maximal size,
-/// which means that evaluating it does not cause a dynamic memory allocation.
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa class Block, fix, fix<N>(int)
-///
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename FixedBlockXpr<...,...>::Type
-#endif
-block(Index startRow, Index startCol, NRowsType blockRows, NColsType blockCols)
-{
-  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type(
-            derived(), startRow, startCol, internal::get_runtime_value(blockRows), internal::get_runtime_value(blockCols));
-}
-
-/// This is the const version of block(Index,Index,NRowsType,NColsType)
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-const typename ConstFixedBlockXpr<...,...>::Type
-#endif
-block(Index startRow, Index startCol, NRowsType blockRows, NColsType blockCols) const
-{
-  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type(
-            derived(), startRow, startCol, internal::get_runtime_value(blockRows), internal::get_runtime_value(blockCols));
-}
-
-
-
-/// \returns a expression of a top-right corner of \c *this with either dynamic or fixed sizes.
-///
-/// \param cRows the number of rows in the corner
-/// \param cCols the number of columns in the corner
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example with dynamic sizes: \include MatrixBase_topRightCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_topRightCorner_int_int.out
-///
-/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename FixedBlockXpr<...,...>::Type
-#endif
-topRightCorner(NRowsType cRows, NColsType cCols)
-{
-  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, cols() - internal::get_runtime_value(cCols), internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// This is the const version of topRightCorner(NRowsType, NColsType).
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-const typename ConstFixedBlockXpr<...,...>::Type
-#endif
-topRightCorner(NRowsType cRows, NColsType cCols) const
-{
-  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, cols() - internal::get_runtime_value(cCols), internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// \returns an expression of a fixed-size top-right corner of \c *this.
-///
-/// \tparam CRows the number of rows in the corner
-/// \tparam CCols the number of columns in the corner
-///
-/// Example: \include MatrixBase_template_int_int_topRightCorner.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_topRightCorner.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa class Block, block<int,int>(Index,Index)
-///
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type topRightCorner()
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - CCols);
-}
-
-/// This is the const version of topRightCorner<int, int>().
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type topRightCorner() const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - CCols);
-}
-
-/// \returns an expression of a top-right corner of \c *this.
-///
-/// \tparam CRows number of rows in corner as specified at compile-time
-/// \tparam CCols number of columns in corner as specified at compile-time
-/// \param  cRows number of rows in corner as specified at run-time
-/// \param  cCols number of columns in corner as specified at run-time
-///
-/// This function is mainly useful for corners where the number of rows is specified at compile-time
-/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
-/// information should not contradict. In other words, \a cRows should equal \a CRows unless
-/// \a CRows is \a Dynamic, and the same for the number of columns.
-///
-/// Example: \include MatrixBase_template_int_int_topRightCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_topRightCorner_int_int.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa class Block
-///
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type topRightCorner(Index cRows, Index cCols)
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - cCols, cRows, cCols);
-}
-
-/// This is the const version of topRightCorner<int, int>(Index, Index).
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type topRightCorner(Index cRows, Index cCols) const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - cCols, cRows, cCols);
-}
-
-
-
-/// \returns an expression of a top-left corner of \c *this  with either dynamic or fixed sizes.
-///
-/// \param cRows the number of rows in the corner
-/// \param cCols the number of columns in the corner
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example: \include MatrixBase_topLeftCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_topLeftCorner_int_int.out
-///
-/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename FixedBlockXpr<...,...>::Type
-#endif
-topLeftCorner(NRowsType cRows, NColsType cCols)
-{
-  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, 0, internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// This is the const version of topLeftCorner(Index, Index).
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-const typename ConstFixedBlockXpr<...,...>::Type
-#endif
-topLeftCorner(NRowsType cRows, NColsType cCols) const
-{
-  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, 0, internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// \returns an expression of a fixed-size top-left corner of \c *this.
-///
-/// The template parameters CRows and CCols are the number of rows and columns in the corner.
-///
-/// Example: \include MatrixBase_template_int_int_topLeftCorner.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_topLeftCorner.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type topLeftCorner()
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0);
-}
-
-/// This is the const version of topLeftCorner<int, int>().
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type topLeftCorner() const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0);
-}
-
-/// \returns an expression of a top-left corner of \c *this.
-///
-/// \tparam CRows number of rows in corner as specified at compile-time
-/// \tparam CCols number of columns in corner as specified at compile-time
-/// \param  cRows number of rows in corner as specified at run-time
-/// \param  cCols number of columns in corner as specified at run-time
-///
-/// This function is mainly useful for corners where the number of rows is specified at compile-time
-/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
-/// information should not contradict. In other words, \a cRows should equal \a CRows unless
-/// \a CRows is \a Dynamic, and the same for the number of columns.
-///
-/// Example: \include MatrixBase_template_int_int_topLeftCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_topLeftCorner_int_int.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa class Block
-///
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type topLeftCorner(Index cRows, Index cCols)
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0, cRows, cCols);
-}
-
-/// This is the const version of topLeftCorner<int, int>(Index, Index).
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type topLeftCorner(Index cRows, Index cCols) const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0, cRows, cCols);
-}
-
-
-
-/// \returns an expression of a bottom-right corner of \c *this  with either dynamic or fixed sizes.
-///
-/// \param cRows the number of rows in the corner
-/// \param cCols the number of columns in the corner
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example: \include MatrixBase_bottomRightCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_bottomRightCorner_int_int.out
-///
-/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename FixedBlockXpr<...,...>::Type
-#endif
-bottomRightCorner(NRowsType cRows, NColsType cCols)
-{
-  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), rows() - internal::get_runtime_value(cRows), cols() - internal::get_runtime_value(cCols),
-                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// This is the const version of bottomRightCorner(NRowsType, NColsType).
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-const typename ConstFixedBlockXpr<...,...>::Type
-#endif
-bottomRightCorner(NRowsType cRows, NColsType cCols) const
-{
-  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), rows() - internal::get_runtime_value(cRows), cols() - internal::get_runtime_value(cCols),
-                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// \returns an expression of a fixed-size bottom-right corner of \c *this.
-///
-/// The template parameters CRows and CCols are the number of rows and columns in the corner.
-///
-/// Example: \include MatrixBase_template_int_int_bottomRightCorner.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_bottomRightCorner.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type bottomRightCorner()
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, cols() - CCols);
-}
-
-/// This is the const version of bottomRightCorner<int, int>().
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomRightCorner() const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, cols() - CCols);
-}
-
-/// \returns an expression of a bottom-right corner of \c *this.
-///
-/// \tparam CRows number of rows in corner as specified at compile-time
-/// \tparam CCols number of columns in corner as specified at compile-time
-/// \param  cRows number of rows in corner as specified at run-time
-/// \param  cCols number of columns in corner as specified at run-time
-///
-/// This function is mainly useful for corners where the number of rows is specified at compile-time
-/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
-/// information should not contradict. In other words, \a cRows should equal \a CRows unless
-/// \a CRows is \a Dynamic, and the same for the number of columns.
-///
-/// Example: \include MatrixBase_template_int_int_bottomRightCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_bottomRightCorner_int_int.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa class Block
-///
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type bottomRightCorner(Index cRows, Index cCols)
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
-}
-
-/// This is the const version of bottomRightCorner<int, int>(Index, Index).
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomRightCorner(Index cRows, Index cCols) const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
-}
-
-
-
-/// \returns an expression of a bottom-left corner of \c *this  with either dynamic or fixed sizes.
-///
-/// \param cRows the number of rows in the corner
-/// \param cCols the number of columns in the corner
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example: \include MatrixBase_bottomLeftCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_bottomLeftCorner_int_int.out
-///
-/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename FixedBlockXpr<...,...>::Type
-#endif
-bottomLeftCorner(NRowsType cRows, NColsType cCols)
-{
-  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), rows() - internal::get_runtime_value(cRows), 0,
-                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// This is the const version of bottomLeftCorner(NRowsType, NColsType).
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename ConstFixedBlockXpr<...,...>::Type
-#endif
-bottomLeftCorner(NRowsType cRows, NColsType cCols) const
-{
-  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), rows() - internal::get_runtime_value(cRows), 0,
-                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
-}
-
-/// \returns an expression of a fixed-size bottom-left corner of \c *this.
-///
-/// The template parameters CRows and CCols are the number of rows and columns in the corner.
-///
-/// Example: \include MatrixBase_template_int_int_bottomLeftCorner.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_bottomLeftCorner.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type bottomLeftCorner()
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, 0);
-}
-
-/// This is the const version of bottomLeftCorner<int, int>().
-template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomLeftCorner() const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, 0);
-}
-
-/// \returns an expression of a bottom-left corner of \c *this.
-///
-/// \tparam CRows number of rows in corner as specified at compile-time
-/// \tparam CCols number of columns in corner as specified at compile-time
-/// \param  cRows number of rows in corner as specified at run-time
-/// \param  cCols number of columns in corner as specified at run-time
-///
-/// This function is mainly useful for corners where the number of rows is specified at compile-time
-/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
-/// information should not contradict. In other words, \a cRows should equal \a CRows unless
-/// \a CRows is \a Dynamic, and the same for the number of columns.
-///
-/// Example: \include MatrixBase_template_int_int_bottomLeftCorner_int_int.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_bottomLeftCorner_int_int.out
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa class Block
-///
-template<int CRows, int CCols>
-EIGEN_STRONG_INLINE
-typename FixedBlockXpr<CRows,CCols>::Type bottomLeftCorner(Index cRows, Index cCols)
-{
-  return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, 0, cRows, cCols);
-}
-
-/// This is the const version of bottomLeftCorner<int, int>(Index, Index).
-template<int CRows, int CCols>
-EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomLeftCorner(Index cRows, Index cCols) const
-{
-  return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, 0, cRows, cCols);
-}
-
-
-
-/// \returns a block consisting of the top rows of \c *this.
-///
-/// \param n the number of rows in the block
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-///
-/// Example: \include MatrixBase_topRows_int.cpp
-/// Output: \verbinclude MatrixBase_topRows_int.out
-///
-/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NRowsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-#else
-typename NRowsBlockXpr<...>::Type
-#endif
-topRows(NRowsType n)
-{
-  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-            (derived(), 0, 0, internal::get_runtime_value(n), cols());
-}
-
-/// This is the const version of topRows(NRowsType).
-template<typename NRowsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-#else
-const typename ConstNRowsBlockXpr<...>::Type
-#endif
-topRows(NRowsType n) const
-{
-  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-            (derived(), 0, 0, internal::get_runtime_value(n), cols());
-}
-
-/// \returns a block consisting of the top rows of \c *this.
-///
-/// \tparam N the number of rows in the block as specified at compile-time
-/// \param n the number of rows in the block as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include MatrixBase_template_int_topRows.cpp
-/// Output: \verbinclude MatrixBase_template_int_topRows.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename NRowsBlockXpr<N>::Type topRows(Index n = N)
-{
-  return typename NRowsBlockXpr<N>::Type(derived(), 0, 0, n, cols());
-}
-
-/// This is the const version of topRows<int>().
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstNRowsBlockXpr<N>::Type topRows(Index n = N) const
-{
-  return typename ConstNRowsBlockXpr<N>::Type(derived(), 0, 0, n, cols());
-}
-
-
-
-/// \returns a block consisting of the bottom rows of \c *this.
-///
-/// \param n the number of rows in the block
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-///
-/// Example: \include MatrixBase_bottomRows_int.cpp
-/// Output: \verbinclude MatrixBase_bottomRows_int.out
-///
-/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NRowsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-#else
-typename NRowsBlockXpr<...>::Type
-#endif
-bottomRows(NRowsType n)
-{
-  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-            (derived(), rows() - internal::get_runtime_value(n), 0, internal::get_runtime_value(n), cols());
-}
-
-/// This is the const version of bottomRows(NRowsType).
-template<typename NRowsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-#else
-const typename ConstNRowsBlockXpr<...>::Type
-#endif
-bottomRows(NRowsType n) const
-{
-  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-            (derived(), rows() - internal::get_runtime_value(n), 0, internal::get_runtime_value(n), cols());
-}
-
-/// \returns a block consisting of the bottom rows of \c *this.
-///
-/// \tparam N the number of rows in the block as specified at compile-time
-/// \param n the number of rows in the block as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include MatrixBase_template_int_bottomRows.cpp
-/// Output: \verbinclude MatrixBase_template_int_bottomRows.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename NRowsBlockXpr<N>::Type bottomRows(Index n = N)
-{
-  return typename NRowsBlockXpr<N>::Type(derived(), rows() - n, 0, n, cols());
-}
-
-/// This is the const version of bottomRows<int>().
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstNRowsBlockXpr<N>::Type bottomRows(Index n = N) const
-{
-  return typename ConstNRowsBlockXpr<N>::Type(derived(), rows() - n, 0, n, cols());
-}
-
-
-
-/// \returns a block consisting of a range of rows of \c *this.
-///
-/// \param startRow the index of the first row in the block
-/// \param n the number of rows in the block
-/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
-///
-/// Example: \include DenseBase_middleRows_int.cpp
-/// Output: \verbinclude DenseBase_middleRows_int.out
-///
-/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NRowsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-#else
-typename NRowsBlockXpr<...>::Type
-#endif
-middleRows(Index startRow, NRowsType n)
-{
-  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-            (derived(), startRow, 0, internal::get_runtime_value(n), cols());
-}
-
-/// This is the const version of middleRows(Index,NRowsType).
-template<typename NRowsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-#else
-const typename ConstNRowsBlockXpr<...>::Type
-#endif
-middleRows(Index startRow, NRowsType n) const
-{
-  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
-            (derived(), startRow, 0, internal::get_runtime_value(n), cols());
-}
-
-/// \returns a block consisting of a range of rows of \c *this.
-///
-/// \tparam N the number of rows in the block as specified at compile-time
-/// \param startRow the index of the first row in the block
-/// \param n the number of rows in the block as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include DenseBase_template_int_middleRows.cpp
-/// Output: \verbinclude DenseBase_template_int_middleRows.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename NRowsBlockXpr<N>::Type middleRows(Index startRow, Index n = N)
-{
-  return typename NRowsBlockXpr<N>::Type(derived(), startRow, 0, n, cols());
-}
-
-/// This is the const version of middleRows<int>().
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstNRowsBlockXpr<N>::Type middleRows(Index startRow, Index n = N) const
-{
-  return typename ConstNRowsBlockXpr<N>::Type(derived(), startRow, 0, n, cols());
-}
-
-
-
-/// \returns a block consisting of the left columns of \c *this.
-///
-/// \param n the number of columns in the block
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example: \include MatrixBase_leftCols_int.cpp
-/// Output: \verbinclude MatrixBase_leftCols_int.out
-///
-/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename NColsBlockXpr<...>::Type
-#endif
-leftCols(NColsType n)
-{
-  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, 0, rows(), internal::get_runtime_value(n));
-}
-
-/// This is the const version of leftCols(NColsType).
-template<typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-#else
-const typename ConstNColsBlockXpr<...>::Type
-#endif
-leftCols(NColsType n) const
-{
-  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, 0, rows(), internal::get_runtime_value(n));
-}
-
-/// \returns a block consisting of the left columns of \c *this.
-///
-/// \tparam N the number of columns in the block as specified at compile-time
-/// \param n the number of columns in the block as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include MatrixBase_template_int_leftCols.cpp
-/// Output: \verbinclude MatrixBase_template_int_leftCols.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename NColsBlockXpr<N>::Type leftCols(Index n = N)
-{
-  return typename NColsBlockXpr<N>::Type(derived(), 0, 0, rows(), n);
-}
-
-/// This is the const version of leftCols<int>().
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstNColsBlockXpr<N>::Type leftCols(Index n = N) const
-{
-  return typename ConstNColsBlockXpr<N>::Type(derived(), 0, 0, rows(), n);
-}
-
-
-
-/// \returns a block consisting of the right columns of \c *this.
-///
-/// \param n the number of columns in the block
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example: \include MatrixBase_rightCols_int.cpp
-/// Output: \verbinclude MatrixBase_rightCols_int.out
-///
-/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename NColsBlockXpr<...>::Type
-#endif
-rightCols(NColsType n)
-{
-  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, cols() - internal::get_runtime_value(n), rows(), internal::get_runtime_value(n));
-}
-
-/// This is the const version of rightCols(NColsType).
-template<typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-#else
-const typename ConstNColsBlockXpr<...>::Type
-#endif
-rightCols(NColsType n) const
-{
-  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, cols() - internal::get_runtime_value(n), rows(), internal::get_runtime_value(n));
-}
-
-/// \returns a block consisting of the right columns of \c *this.
-///
-/// \tparam N the number of columns in the block as specified at compile-time
-/// \param n the number of columns in the block as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include MatrixBase_template_int_rightCols.cpp
-/// Output: \verbinclude MatrixBase_template_int_rightCols.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename NColsBlockXpr<N>::Type rightCols(Index n = N)
-{
-  return typename NColsBlockXpr<N>::Type(derived(), 0, cols() - n, rows(), n);
-}
-
-/// This is the const version of rightCols<int>().
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstNColsBlockXpr<N>::Type rightCols(Index n = N) const
-{
-  return typename ConstNColsBlockXpr<N>::Type(derived(), 0, cols() - n, rows(), n);
-}
-
-
-
-/// \returns a block consisting of a range of columns of \c *this.
-///
-/// \param startCol the index of the first column in the block
-/// \param numCols the number of columns in the block
-/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
-///
-/// Example: \include DenseBase_middleCols_int.cpp
-/// Output: \verbinclude DenseBase_middleCols_int.out
-///
-/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-#else
-typename NColsBlockXpr<...>::Type
-#endif
-middleCols(Index startCol, NColsType numCols)
-{
-  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, startCol, rows(), internal::get_runtime_value(numCols));
-}
-
-/// This is the const version of middleCols(Index,NColsType).
-template<typename NColsType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-#else
-const typename ConstNColsBlockXpr<...>::Type
-#endif
-middleCols(Index startCol, NColsType numCols) const
-{
-  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
-            (derived(), 0, startCol, rows(), internal::get_runtime_value(numCols));
-}
-
-/// \returns a block consisting of a range of columns of \c *this.
-///
-/// \tparam N the number of columns in the block as specified at compile-time
-/// \param startCol the index of the first column in the block
-/// \param n the number of columns in the block as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include DenseBase_template_int_middleCols.cpp
-/// Output: \verbinclude DenseBase_template_int_middleCols.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename NColsBlockXpr<N>::Type middleCols(Index startCol, Index n = N)
-{
-  return typename NColsBlockXpr<N>::Type(derived(), 0, startCol, rows(), n);
-}
-
-/// This is the const version of middleCols<int>().
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstNColsBlockXpr<N>::Type middleCols(Index startCol, Index n = N) const
-{
-  return typename ConstNColsBlockXpr<N>::Type(derived(), 0, startCol, rows(), n);
-}
-
-
-
-/// \returns a fixed-size expression of a block of \c *this.
-///
-/// The template parameters \a NRows and \a NCols are the number of
-/// rows and columns in the block.
-///
-/// \param startRow the first row in the block
-/// \param startCol the first column in the block
-///
-/// Example: \include MatrixBase_block_int_int.cpp
-/// Output: \verbinclude MatrixBase_block_int_int.out
-///
-/// \note The usage of of this overload is discouraged from %Eigen 3.4, better used the generic
-/// block(Index,Index,NRowsType,NColsType), here is the one-to-one equivalence:
-/// \code
-/// mat.template block<NRows,NCols>(i,j)  <-->  mat.block(i,j,fix<NRows>,fix<NCols>)
-/// \endcode
-///
-/// \note since block is a templated member, the keyword template has to be used
-/// if the matrix type is also a template parameter: \code m.template block<3,3>(1,1); \endcode
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int NRows, int NCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol)
-{
-  return typename FixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol);
-}
-
-/// This is the const version of block<>(Index, Index). */
-template<int NRows, int NCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol) const
-{
-  return typename ConstFixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol);
-}
-
-/// \returns an expression of a block of \c *this.
-///
-/// \tparam NRows number of rows in block as specified at compile-time
-/// \tparam NCols number of columns in block as specified at compile-time
-/// \param  startRow  the first row in the block
-/// \param  startCol  the first column in the block
-/// \param  blockRows number of rows in block as specified at run-time
-/// \param  blockCols number of columns in block as specified at run-time
-///
-/// This function is mainly useful for blocks where the number of rows is specified at compile-time
-/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
-/// information should not contradict. In other words, \a blockRows should equal \a NRows unless
-/// \a NRows is \a Dynamic, and the same for the number of columns.
-///
-/// Example: \include MatrixBase_template_int_int_block_int_int_int_int.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_block_int_int_int_int.out
-///
-/// \note The usage of of this overload is discouraged from %Eigen 3.4, better used the generic
-/// block(Index,Index,NRowsType,NColsType), here is the one-to-one complete equivalence:
-/// \code
-/// mat.template block<NRows,NCols>(i,j,rows,cols)     <-->  mat.block(i,j,fix<NRows>(rows),fix<NCols>(cols))
-/// \endcode
-/// If we known that, e.g., NRows==Dynamic and NCols!=Dynamic, then the equivalence becomes:
-/// \code
-/// mat.template block<Dynamic,NCols>(i,j,rows,NCols)  <-->  mat.block(i,j,rows,fix<NCols>)
-/// \endcode
-///
-EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-///
-/// \sa block(Index,Index,NRowsType,NColsType), class Block
-///
-template<int NRows, int NCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol,
-                                                  Index blockRows, Index blockCols)
-{
-  return typename FixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol, blockRows, blockCols);
-}
-
-/// This is the const version of block<>(Index, Index, Index, Index).
-template<int NRows, int NCols>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const typename ConstFixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol,
-                                                              Index blockRows, Index blockCols) const
-{
-  return typename ConstFixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol, blockRows, blockCols);
-}
-
-/// \returns an expression of the \a i-th column of \c *this. Note that the numbering starts at 0.
-///
-/// Example: \include MatrixBase_col.cpp
-/// Output: \verbinclude MatrixBase_col.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
-/**
-  * \sa row(), class Block */
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-ColXpr col(Index i)
-{
-  return ColXpr(derived(), i);
-}
-
-/// This is the const version of col().
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-ConstColXpr col(Index i) const
-{
-  return ConstColXpr(derived(), i);
-}
-
-/// \returns an expression of the \a i-th row of \c *this. Note that the numbering starts at 0.
-///
-/// Example: \include MatrixBase_row.cpp
-/// Output: \verbinclude MatrixBase_row.out
-///
-EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
-/**
-  * \sa col(), class Block */
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-RowXpr row(Index i)
-{
-  return RowXpr(derived(), i);
-}
-
-/// This is the const version of row(). */
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-ConstRowXpr row(Index i) const
-{
-  return ConstRowXpr(derived(), i);
-}
-
-/// \returns an expression of a segment (i.e. a vector block) in \c *this with either dynamic or fixed sizes.
-///
-/// \only_for_vectors
-///
-/// \param start the first coefficient in the segment
-/// \param n the number of coefficients in the segment
-/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
-///
-/// Example: \include MatrixBase_segment_int_int.cpp
-/// Output: \verbinclude MatrixBase_segment_int_int.out
-///
-/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-/// \note Even in the case that the returned expression has dynamic size, in the case
-/// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
-/// which means that evaluating it does not cause a dynamic memory allocation.
-///
-/// \sa block(Index,Index,NRowsType,NColsType), fix<N>, fix<N>(int), class Block
-///
-template<typename NType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-#else
-typename FixedSegmentReturnType<...>::Type
-#endif
-segment(Index start, NType n)
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-            (derived(), start, internal::get_runtime_value(n));
-}
-
-
-/// This is the const version of segment(Index,NType).
-template<typename NType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-#else
-const typename ConstFixedSegmentReturnType<...>::Type
-#endif
-segment(Index start, NType n) const
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-            (derived(), start, internal::get_runtime_value(n));
-}
-
-/// \returns an expression of the first coefficients of \c *this with either dynamic or fixed sizes.
-///
-/// \only_for_vectors
-///
-/// \param n the number of coefficients in the segment
-/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
-///
-/// Example: \include MatrixBase_start_int.cpp
-/// Output: \verbinclude MatrixBase_start_int.out
-///
-/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-/// \note Even in the case that the returned expression has dynamic size, in the case
-/// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
-/// which means that evaluating it does not cause a dynamic memory allocation.
-///
-/// \sa class Block, block(Index,Index)
-///
-template<typename NType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-#else
-typename FixedSegmentReturnType<...>::Type
-#endif
-head(NType n)
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-              (derived(), 0, internal::get_runtime_value(n));
-}
-
-/// This is the const version of head(NType).
-template<typename NType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-#else
-const typename ConstFixedSegmentReturnType<...>::Type
-#endif
-head(NType n) const
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-            (derived(), 0, internal::get_runtime_value(n));
-}
-
-/// \returns an expression of a last coefficients of \c *this with either dynamic or fixed sizes.
-///
-/// \only_for_vectors
-///
-/// \param n the number of coefficients in the segment
-/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
-///
-/// Example: \include MatrixBase_end_int.cpp
-/// Output: \verbinclude MatrixBase_end_int.out
-///
-/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments.
-/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
-///
-/// \note Even in the case that the returned expression has dynamic size, in the case
-/// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
-/// which means that evaluating it does not cause a dynamic memory allocation.
-///
-/// \sa class Block, block(Index,Index)
-///
-template<typename NType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-#else
-typename FixedSegmentReturnType<...>::Type
-#endif
-tail(NType n)
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-            (derived(), this->size() - internal::get_runtime_value(n), internal::get_runtime_value(n));
-}
-
-/// This is the const version of tail(Index).
-template<typename NType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-#else
-const typename ConstFixedSegmentReturnType<...>::Type
-#endif
-tail(NType n) const
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
-            (derived(), this->size() - internal::get_runtime_value(n), internal::get_runtime_value(n));
-}
-
-/// \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this
-///
-/// \only_for_vectors
-///
-/// \tparam N the number of coefficients in the segment as specified at compile-time
-/// \param start the index of the first element in the segment
-/// \param n the number of coefficients in the segment as specified at compile-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include MatrixBase_template_int_segment.cpp
-/// Output: \verbinclude MatrixBase_template_int_segment.out
-///
-/// \sa segment(Index,NType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedSegmentReturnType<N>::Type segment(Index start, Index n = N)
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename FixedSegmentReturnType<N>::Type(derived(), start, n);
-}
-
-/// This is the const version of segment<int>(Index).
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstFixedSegmentReturnType<N>::Type segment(Index start, Index n = N) const
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename ConstFixedSegmentReturnType<N>::Type(derived(), start, n);
-}
-
-/// \returns a fixed-size expression of the first coefficients of \c *this.
-///
-/// \only_for_vectors
-///
-/// \tparam N the number of coefficients in the segment as specified at compile-time
-/// \param  n the number of coefficients in the segment as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include MatrixBase_template_int_start.cpp
-/// Output: \verbinclude MatrixBase_template_int_start.out
-///
-/// \sa head(NType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedSegmentReturnType<N>::Type head(Index n = N)
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename FixedSegmentReturnType<N>::Type(derived(), 0, n);
-}
-
-/// This is the const version of head<int>().
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstFixedSegmentReturnType<N>::Type head(Index n = N) const
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename ConstFixedSegmentReturnType<N>::Type(derived(), 0, n);
-}
-
-/// \returns a fixed-size expression of the last coefficients of \c *this.
-///
-/// \only_for_vectors
-///
-/// \tparam N the number of coefficients in the segment as specified at compile-time
-/// \param  n the number of coefficients in the segment as specified at run-time
-///
-/// The compile-time and run-time information should not contradict. In other words,
-/// \a n should equal \a N unless \a N is \a Dynamic.
-///
-/// Example: \include MatrixBase_template_int_end.cpp
-/// Output: \verbinclude MatrixBase_template_int_end.out
-///
-/// \sa tail(NType), class Block
-///
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename FixedSegmentReturnType<N>::Type tail(Index n = N)
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename FixedSegmentReturnType<N>::Type(derived(), size() - n);
-}
-
-/// This is the const version of tail<int>.
-template<int N>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename ConstFixedSegmentReturnType<N>::Type tail(Index n = N) const
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return typename ConstFixedSegmentReturnType<N>::Type(derived(), size() - n);
-}
-
-/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
-/// is col-major (resp. row-major).
-///
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-InnerVectorReturnType innerVector(Index outer)
-{ return InnerVectorReturnType(derived(), outer); }
-
-/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
-/// is col-major (resp. row-major). Read-only.
-///
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const ConstInnerVectorReturnType innerVector(Index outer) const
-{ return ConstInnerVectorReturnType(derived(), outer); }
-
-/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
-/// is col-major (resp. row-major).
-///
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-InnerVectorsReturnType
-innerVectors(Index outerStart, Index outerSize)
-{
-  return Block<Derived,Dynamic,Dynamic,true>(derived(),
-                                             IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart,
-                                             IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize);
-
-}
-
-/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
-/// is col-major (resp. row-major). Read-only.
-///
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-const ConstInnerVectorsReturnType
-innerVectors(Index outerStart, Index outerSize) const
-{
-  return Block<const Derived,Dynamic,Dynamic,true>(derived(),
-                                                  IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart,
-                                                  IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize);
-
-}
-
-/** \returns the i-th subvector (column or vector) according to the \c Direction
-  * \sa subVectors()
-  */
-template<DirectionType Direction>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename internal::conditional<Direction==Vertical,ColXpr,RowXpr>::type
-subVector(Index i)
-{
-  return typename internal::conditional<Direction==Vertical,ColXpr,RowXpr>::type(derived(),i);
-}
-
-/** This is the const version of subVector(Index) */
-template<DirectionType Direction>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-typename internal::conditional<Direction==Vertical,ConstColXpr,ConstRowXpr>::type
-subVector(Index i) const
-{
-  return typename internal::conditional<Direction==Vertical,ConstColXpr,ConstRowXpr>::type(derived(),i);
-}
-
-/** \returns the number of subvectors (rows or columns) in the direction \c Direction
-  * \sa subVector(Index)
-  */
-template<DirectionType Direction>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
-Index subVectors() const
-{ return (Direction==Vertical)?cols():rows(); }
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc
new file mode 100644
index 0000000..122a2f4
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc
@@ -0,0 +1,1370 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+
+/// \internal expression type of a column */
+typedef Block<Derived, internal::traits<Derived>::RowsAtCompileTime, 1, !IsRowMajor> ColXpr;
+typedef const Block<const Derived, internal::traits<Derived>::RowsAtCompileTime, 1, !IsRowMajor> ConstColXpr;
+/// \internal expression type of a row */
+typedef Block<Derived, 1, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> RowXpr;
+typedef const Block<const Derived, 1, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> ConstRowXpr;
+/// \internal expression type of a block of whole columns */
+typedef Block<Derived, internal::traits<Derived>::RowsAtCompileTime, Dynamic, !IsRowMajor> ColsBlockXpr;
+typedef const Block<const Derived, internal::traits<Derived>::RowsAtCompileTime, Dynamic, !IsRowMajor>
+    ConstColsBlockXpr;
+/// \internal expression type of a block of whole rows */
+typedef Block<Derived, Dynamic, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> RowsBlockXpr;
+typedef const Block<const Derived, Dynamic, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> ConstRowsBlockXpr;
+/// \internal expression type of a block of whole columns */
+template <int N>
+struct NColsBlockXpr {
+  typedef Block<Derived, internal::traits<Derived>::RowsAtCompileTime, N, !IsRowMajor> Type;
+};
+template <int N>
+struct ConstNColsBlockXpr {
+  typedef const Block<const Derived, internal::traits<Derived>::RowsAtCompileTime, N, !IsRowMajor> Type;
+};
+/// \internal expression type of a block of whole rows */
+template <int N>
+struct NRowsBlockXpr {
+  typedef Block<Derived, N, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> Type;
+};
+template <int N>
+struct ConstNRowsBlockXpr {
+  typedef const Block<const Derived, N, internal::traits<Derived>::ColsAtCompileTime, IsRowMajor> Type;
+};
+/// \internal expression of a block */
+typedef Block<Derived> BlockXpr;
+typedef const Block<const Derived> ConstBlockXpr;
+/// \internal expression of a block of fixed sizes */
+template <int Rows, int Cols>
+struct FixedBlockXpr {
+  typedef Block<Derived, Rows, Cols> Type;
+};
+template <int Rows, int Cols>
+struct ConstFixedBlockXpr {
+  typedef Block<const Derived, Rows, Cols> Type;
+};
+
+typedef VectorBlock<Derived> SegmentReturnType;
+typedef const VectorBlock<const Derived> ConstSegmentReturnType;
+template <int Size>
+struct FixedSegmentReturnType {
+  typedef VectorBlock<Derived, Size> Type;
+};
+template <int Size>
+struct ConstFixedSegmentReturnType {
+  typedef const VectorBlock<const Derived, Size> Type;
+};
+
+/// \internal inner-vector
+typedef Block<Derived, IsRowMajor ? 1 : Dynamic, IsRowMajor ? Dynamic : 1, true> InnerVectorReturnType;
+typedef Block<const Derived, IsRowMajor ? 1 : Dynamic, IsRowMajor ? Dynamic : 1, true> ConstInnerVectorReturnType;
+
+/// \internal set of inner-vectors
+typedef Block<Derived, Dynamic, Dynamic, true> InnerVectorsReturnType;
+typedef Block<const Derived, Dynamic, Dynamic, true> ConstInnerVectorsReturnType;
+
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
+
+/// \returns an expression of a block in \c *this with either dynamic or fixed sizes.
+///
+/// \param  startRow  the first row in the block
+/// \param  startCol  the first column in the block
+/// \param  blockRows number of rows in the block, specified at either run-time or compile-time
+/// \param  blockCols number of columns in the block, specified at either run-time or compile-time
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example using runtime (aka dynamic) sizes: \include MatrixBase_block_int_int_int_int.cpp
+/// Output: \verbinclude MatrixBase_block_int_int_int_int.out
+///
+/// \newin{3.4}:
+///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing
+/// Eigen::fix<N>, or Eigen::fix<N>(n) as arguments. In the later case, \c n plays the role of a runtime fallback value
+/// in case \c N equals Eigen::Dynamic. Here is an example with a fixed number of rows \c NRows and dynamic number of
+/// columns \c cols: \code mat.block(i,j,fix<NRows>,cols) \endcode
+///
+/// This function thus fully covers the features offered by the following overloads block<NRows,NCols>(Index, Index),
+/// and block<NRows,NCols>(Index, Index, Index, Index) that are thus obsolete. Indeed, this generic version avoids
+/// redundancy, it preserves the argument order, and prevents the need to rely on the template keyword in templated
+/// code.
+///
+/// but with less redundancy and more consistency as it does not modify the argument order
+/// and seamlessly enable hybrid fixed/dynamic sizes.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
+/// when it is applied to a fixed-size matrix, it inherits a fixed maximal size,
+/// which means that evaluating it does not cause a dynamic memory allocation.
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa class Block, fix, fix<N>(int)
+///
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                           internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename FixedBlockXpr<..., ...>::Type
+#endif
+    block(Index startRow, Index startCol, NRowsType blockRows, NColsType blockCols) {
+  return
+      typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                             internal::get_fixed_value<NColsType>::value>::Type(derived(), startRow, startCol,
+                                                                                internal::get_runtime_value(blockRows),
+                                                                                internal::get_runtime_value(blockCols));
+}
+
+/// This is the const version of block(Index,Index,NRowsType,NColsType)
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                      internal::get_fixed_value<NColsType>::value>::Type
+#else
+    const typename ConstFixedBlockXpr<..., ...>::Type
+#endif
+    block(Index startRow, Index startCol, NRowsType blockRows, NColsType blockCols) const {
+  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                     internal::get_fixed_value<NColsType>::value>::Type(derived(), startRow, startCol,
+                                                                                        internal::get_runtime_value(
+                                                                                            blockRows),
+                                                                                        internal::get_runtime_value(
+                                                                                            blockCols));
+}
+
+/// \returns a expression of a top-right corner of \c *this with either dynamic or fixed sizes.
+///
+/// \param cRows the number of rows in the corner
+/// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example with dynamic sizes: \include MatrixBase_topRightCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_topRightCorner_int_int.out
+///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing
+/// Eigen::fix<N>, or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink
+/// for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                           internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename FixedBlockXpr<..., ...>::Type
+#endif
+    topRightCorner(NRowsType cRows, NColsType cCols) {
+  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                internal::get_fixed_value<NColsType>::value>::Type(derived(), 0,
+                                                                                   cols() - internal::get_runtime_value(
+                                                                                                cCols),
+                                                                                   internal::get_runtime_value(cRows),
+                                                                                   internal::get_runtime_value(cCols));
+}
+
+/// This is the const version of topRightCorner(NRowsType, NColsType).
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                      internal::get_fixed_value<NColsType>::value>::Type
+#else
+    const typename ConstFixedBlockXpr<..., ...>::Type
+#endif
+    topRightCorner(NRowsType cRows, NColsType cCols) const {
+  return
+      typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                  internal::get_fixed_value<NColsType>::value>::Type(derived(), 0,
+                                                                                     cols() -
+                                                                                         internal::get_runtime_value(
+                                                                                             cCols),
+                                                                                     internal::get_runtime_value(cRows),
+                                                                                     internal::get_runtime_value(
+                                                                                         cCols));
+}
+
+/// \returns an expression of a fixed-size top-right corner of \c *this.
+///
+/// \tparam CRows the number of rows in the corner
+/// \tparam CCols the number of columns in the corner
+///
+/// Example: \include MatrixBase_template_int_int_topRightCorner.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_topRightCorner.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa class Block, block<int,int>(Index,Index)
+///
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type topRightCorner() {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), 0, cols() - CCols);
+}
+
+/// This is the const version of topRightCorner<int, int>().
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type topRightCorner() const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), 0, cols() - CCols);
+}
+
+/// \returns an expression of a top-right corner of \c *this.
+///
+/// \tparam CRows number of rows in corner as specified at compile-time
+/// \tparam CCols number of columns in corner as specified at compile-time
+/// \param  cRows number of rows in corner as specified at run-time
+/// \param  cCols number of columns in corner as specified at run-time
+///
+/// This function is mainly useful for corners where the number of rows is specified at compile-time
+/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
+/// information should not contradict. In other words, \a cRows should equal \a CRows unless
+/// \a CRows is \a Dynamic, and the same for the number of columns.
+///
+/// Example: \include MatrixBase_template_int_int_topRightCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_topRightCorner_int_int.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa class Block
+///
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type topRightCorner(Index cRows,
+                                                                                                Index cCols) {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), 0, cols() - cCols, cRows, cCols);
+}
+
+/// This is the const version of topRightCorner<int, int>(Index, Index).
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type topRightCorner(
+    Index cRows, Index cCols) const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), 0, cols() - cCols, cRows, cCols);
+}
+
+/// \returns an expression of a top-left corner of \c *this  with either dynamic or fixed sizes.
+///
+/// \param cRows the number of rows in the corner
+/// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example: \include MatrixBase_topLeftCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_topLeftCorner_int_int.out
+///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing
+/// Eigen::fix<N>, or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink
+/// for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                           internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename FixedBlockXpr<..., ...>::Type
+#endif
+    topLeftCorner(NRowsType cRows, NColsType cCols) {
+  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                internal::get_fixed_value<NColsType>::value>::Type(derived(), 0, 0,
+                                                                                   internal::get_runtime_value(cRows),
+                                                                                   internal::get_runtime_value(cCols));
+}
+
+/// This is the const version of topLeftCorner(Index, Index).
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                      internal::get_fixed_value<NColsType>::value>::Type
+#else
+    const typename ConstFixedBlockXpr<..., ...>::Type
+#endif
+    topLeftCorner(NRowsType cRows, NColsType cCols) const {
+  return
+      typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                  internal::get_fixed_value<NColsType>::value>::Type(derived(), 0, 0,
+                                                                                     internal::get_runtime_value(cRows),
+                                                                                     internal::get_runtime_value(
+                                                                                         cCols));
+}
+
+/// \returns an expression of a fixed-size top-left corner of \c *this.
+///
+/// The template parameters CRows and CCols are the number of rows and columns in the corner.
+///
+/// Example: \include MatrixBase_template_int_int_topLeftCorner.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_topLeftCorner.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type topLeftCorner() {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), 0, 0);
+}
+
+/// This is the const version of topLeftCorner<int, int>().
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type topLeftCorner() const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), 0, 0);
+}
+
+/// \returns an expression of a top-left corner of \c *this.
+///
+/// \tparam CRows number of rows in corner as specified at compile-time
+/// \tparam CCols number of columns in corner as specified at compile-time
+/// \param  cRows number of rows in corner as specified at run-time
+/// \param  cCols number of columns in corner as specified at run-time
+///
+/// This function is mainly useful for corners where the number of rows is specified at compile-time
+/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
+/// information should not contradict. In other words, \a cRows should equal \a CRows unless
+/// \a CRows is \a Dynamic, and the same for the number of columns.
+///
+/// Example: \include MatrixBase_template_int_int_topLeftCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_topLeftCorner_int_int.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa class Block
+///
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type topLeftCorner(Index cRows,
+                                                                                               Index cCols) {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), 0, 0, cRows, cCols);
+}
+
+/// This is the const version of topLeftCorner<int, int>(Index, Index).
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type topLeftCorner(
+    Index cRows, Index cCols) const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), 0, 0, cRows, cCols);
+}
+
+/// \returns an expression of a bottom-right corner of \c *this  with either dynamic or fixed sizes.
+///
+/// \param cRows the number of rows in the corner
+/// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example: \include MatrixBase_bottomRightCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_bottomRightCorner_int_int.out
+///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing
+/// Eigen::fix<N>, or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink
+/// for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                           internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename FixedBlockXpr<..., ...>::Type
+#endif
+    bottomRightCorner(NRowsType cRows, NColsType cCols) {
+  return
+      typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value, internal::get_fixed_value<NColsType>::value>::
+          Type(derived(), rows() - internal::get_runtime_value(cRows), cols() - internal::get_runtime_value(cCols),
+               internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
+}
+
+/// This is the const version of bottomRightCorner(NRowsType, NColsType).
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                      internal::get_fixed_value<NColsType>::value>::Type
+#else
+    const typename ConstFixedBlockXpr<..., ...>::Type
+#endif
+    bottomRightCorner(NRowsType cRows, NColsType cCols) const {
+  return typename ConstFixedBlockXpr<
+      internal::get_fixed_value<NRowsType>::value,
+      internal::get_fixed_value<NColsType>::value>::Type(derived(), rows() - internal::get_runtime_value(cRows),
+                                                         cols() - internal::get_runtime_value(cCols),
+                                                         internal::get_runtime_value(cRows),
+                                                         internal::get_runtime_value(cCols));
+}
+
+/// \returns an expression of a fixed-size bottom-right corner of \c *this.
+///
+/// The template parameters CRows and CCols are the number of rows and columns in the corner.
+///
+/// Example: \include MatrixBase_template_int_int_bottomRightCorner.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_bottomRightCorner.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type bottomRightCorner() {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), rows() - CRows, cols() - CCols);
+}
+
+/// This is the const version of bottomRightCorner<int, int>().
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type bottomRightCorner() const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), rows() - CRows, cols() - CCols);
+}
+
+/// \returns an expression of a bottom-right corner of \c *this.
+///
+/// \tparam CRows number of rows in corner as specified at compile-time
+/// \tparam CCols number of columns in corner as specified at compile-time
+/// \param  cRows number of rows in corner as specified at run-time
+/// \param  cCols number of columns in corner as specified at run-time
+///
+/// This function is mainly useful for corners where the number of rows is specified at compile-time
+/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
+/// information should not contradict. In other words, \a cRows should equal \a CRows unless
+/// \a CRows is \a Dynamic, and the same for the number of columns.
+///
+/// Example: \include MatrixBase_template_int_int_bottomRightCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_bottomRightCorner_int_int.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa class Block
+///
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type bottomRightCorner(Index cRows,
+                                                                                                   Index cCols) {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
+}
+
+/// This is the const version of bottomRightCorner<int, int>(Index, Index).
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type bottomRightCorner(
+    Index cRows, Index cCols) const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
+}
+
+/// \returns an expression of a bottom-left corner of \c *this  with either dynamic or fixed sizes.
+///
+/// \param cRows the number of rows in the corner
+/// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example: \include MatrixBase_bottomLeftCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_bottomLeftCorner_int_int.out
+///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing
+/// Eigen::fix<N>, or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink
+/// for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                           internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename FixedBlockXpr<..., ...>::Type
+#endif
+    bottomLeftCorner(NRowsType cRows, NColsType cCols) {
+  return
+      typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                             internal::get_fixed_value<NColsType>::value>::Type(derived(),
+                                                                                rows() -
+                                                                                    internal::get_runtime_value(cRows),
+                                                                                0, internal::get_runtime_value(cRows),
+                                                                                internal::get_runtime_value(cCols));
+}
+
+/// This is the const version of bottomLeftCorner(NRowsType, NColsType).
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,
+                                internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename ConstFixedBlockXpr<..., ...>::Type
+#endif
+    bottomLeftCorner(NRowsType cRows, NColsType cCols) const {
+  return typename ConstFixedBlockXpr<
+      internal::get_fixed_value<NRowsType>::value,
+      internal::get_fixed_value<NColsType>::value>::Type(derived(), rows() - internal::get_runtime_value(cRows), 0,
+                                                         internal::get_runtime_value(cRows),
+                                                         internal::get_runtime_value(cCols));
+}
+
+/// \returns an expression of a fixed-size bottom-left corner of \c *this.
+///
+/// The template parameters CRows and CCols are the number of rows and columns in the corner.
+///
+/// Example: \include MatrixBase_template_int_int_bottomLeftCorner.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_bottomLeftCorner.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type bottomLeftCorner() {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), rows() - CRows, 0);
+}
+
+/// This is the const version of bottomLeftCorner<int, int>().
+template <int CRows, int CCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type bottomLeftCorner() const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), rows() - CRows, 0);
+}
+
+/// \returns an expression of a bottom-left corner of \c *this.
+///
+/// \tparam CRows number of rows in corner as specified at compile-time
+/// \tparam CCols number of columns in corner as specified at compile-time
+/// \param  cRows number of rows in corner as specified at run-time
+/// \param  cCols number of columns in corner as specified at run-time
+///
+/// This function is mainly useful for corners where the number of rows is specified at compile-time
+/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
+/// information should not contradict. In other words, \a cRows should equal \a CRows unless
+/// \a CRows is \a Dynamic, and the same for the number of columns.
+///
+/// Example: \include MatrixBase_template_int_int_bottomLeftCorner_int_int.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_bottomLeftCorner_int_int.out
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa class Block
+///
+template <int CRows, int CCols>
+EIGEN_STRONG_INLINE typename FixedBlockXpr<CRows, CCols>::Type bottomLeftCorner(Index cRows, Index cCols) {
+  return typename FixedBlockXpr<CRows, CCols>::Type(derived(), rows() - cRows, 0, cRows, cCols);
+}
+
+/// This is the const version of bottomLeftCorner<int, int>(Index, Index).
+template <int CRows, int CCols>
+EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<CRows, CCols>::Type bottomLeftCorner(Index cRows,
+                                                                                           Index cCols) const {
+  return typename ConstFixedBlockXpr<CRows, CCols>::Type(derived(), rows() - cRows, 0, cRows, cCols);
+}
+
+/// \returns a block consisting of the top rows of \c *this.
+///
+/// \param n the number of rows in the block
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+///
+/// Example: \include MatrixBase_topRows_int.cpp
+/// Output: \verbinclude MatrixBase_topRows_int.out
+///
+/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+    typename NRowsBlockXpr<...>::Type
+#endif
+    topRows(NRowsType n) {
+  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type(
+      derived(), 0, 0, internal::get_runtime_value(n), cols());
+}
+
+/// This is the const version of topRows(NRowsType).
+template <typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+    const typename ConstNRowsBlockXpr<...>::Type
+#endif
+    topRows(NRowsType n) const {
+  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type(
+      derived(), 0, 0, internal::get_runtime_value(n), cols());
+}
+
+/// \returns a block consisting of the top rows of \c *this.
+///
+/// \tparam N the number of rows in the block as specified at compile-time
+/// \param n the number of rows in the block as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include MatrixBase_template_int_topRows.cpp
+/// Output: \verbinclude MatrixBase_template_int_topRows.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NRowsBlockXpr<N>::Type topRows(Index n = N) {
+  return typename NRowsBlockXpr<N>::Type(derived(), 0, 0, n, cols());
+}
+
+/// This is the const version of topRows<int>().
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstNRowsBlockXpr<N>::Type topRows(Index n = N) const {
+  return typename ConstNRowsBlockXpr<N>::Type(derived(), 0, 0, n, cols());
+}
+
+/// \returns a block consisting of the bottom rows of \c *this.
+///
+/// \param n the number of rows in the block
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+///
+/// Example: \include MatrixBase_bottomRows_int.cpp
+/// Output: \verbinclude MatrixBase_bottomRows_int.out
+///
+/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+    typename NRowsBlockXpr<...>::Type
+#endif
+    bottomRows(NRowsType n) {
+  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type(
+      derived(), rows() - internal::get_runtime_value(n), 0, internal::get_runtime_value(n), cols());
+}
+
+/// This is the const version of bottomRows(NRowsType).
+template <typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+    const typename ConstNRowsBlockXpr<...>::Type
+#endif
+    bottomRows(NRowsType n) const {
+  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type(
+      derived(), rows() - internal::get_runtime_value(n), 0, internal::get_runtime_value(n), cols());
+}
+
+/// \returns a block consisting of the bottom rows of \c *this.
+///
+/// \tparam N the number of rows in the block as specified at compile-time
+/// \param n the number of rows in the block as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include MatrixBase_template_int_bottomRows.cpp
+/// Output: \verbinclude MatrixBase_template_int_bottomRows.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NRowsBlockXpr<N>::Type bottomRows(Index n = N) {
+  return typename NRowsBlockXpr<N>::Type(derived(), rows() - n, 0, n, cols());
+}
+
+/// This is the const version of bottomRows<int>().
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstNRowsBlockXpr<N>::Type bottomRows(Index n = N) const {
+  return typename ConstNRowsBlockXpr<N>::Type(derived(), rows() - n, 0, n, cols());
+}
+
+/// \returns a block consisting of a range of rows of \c *this.
+///
+/// \param startRow the index of the first row in the block
+/// \param n the number of rows in the block
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+///
+/// Example: \include DenseBase_middleRows_int.cpp
+/// Output: \verbinclude DenseBase_middleRows_int.out
+///
+/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+    typename NRowsBlockXpr<...>::Type
+#endif
+    middleRows(Index startRow, NRowsType n) {
+  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type(
+      derived(), startRow, 0, internal::get_runtime_value(n), cols());
+}
+
+/// This is the const version of middleRows(Index,NRowsType).
+template <typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+    const typename ConstNRowsBlockXpr<...>::Type
+#endif
+    middleRows(Index startRow, NRowsType n) const {
+  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type(
+      derived(), startRow, 0, internal::get_runtime_value(n), cols());
+}
+
+/// \returns a block consisting of a range of rows of \c *this.
+///
+/// \tparam N the number of rows in the block as specified at compile-time
+/// \param startRow the index of the first row in the block
+/// \param n the number of rows in the block as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include DenseBase_template_int_middleRows.cpp
+/// Output: \verbinclude DenseBase_template_int_middleRows.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NRowsBlockXpr<N>::Type middleRows(Index startRow, Index n = N) {
+  return typename NRowsBlockXpr<N>::Type(derived(), startRow, 0, n, cols());
+}
+
+/// This is the const version of middleRows<int>().
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstNRowsBlockXpr<N>::Type middleRows(Index startRow,
+                                                                                      Index n = N) const {
+  return typename ConstNRowsBlockXpr<N>::Type(derived(), startRow, 0, n, cols());
+}
+
+/// \returns a block consisting of the left columns of \c *this.
+///
+/// \param n the number of columns in the block
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example: \include MatrixBase_leftCols_int.cpp
+/// Output: \verbinclude MatrixBase_leftCols_int.out
+///
+/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename NColsBlockXpr<...>::Type
+#endif
+    leftCols(NColsType n) {
+  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type(derived(), 0, 0, rows(),
+                                                                                   internal::get_runtime_value(n));
+}
+
+/// This is the const version of leftCols(NColsType).
+template <typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+    const typename ConstNColsBlockXpr<...>::Type
+#endif
+    leftCols(NColsType n) const {
+  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type(derived(), 0, 0, rows(),
+                                                                                        internal::get_runtime_value(n));
+}
+
+/// \returns a block consisting of the left columns of \c *this.
+///
+/// \tparam N the number of columns in the block as specified at compile-time
+/// \param n the number of columns in the block as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include MatrixBase_template_int_leftCols.cpp
+/// Output: \verbinclude MatrixBase_template_int_leftCols.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NColsBlockXpr<N>::Type leftCols(Index n = N) {
+  return typename NColsBlockXpr<N>::Type(derived(), 0, 0, rows(), n);
+}
+
+/// This is the const version of leftCols<int>().
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstNColsBlockXpr<N>::Type leftCols(Index n = N) const {
+  return typename ConstNColsBlockXpr<N>::Type(derived(), 0, 0, rows(), n);
+}
+
+/// \returns a block consisting of the right columns of \c *this.
+///
+/// \param n the number of columns in the block
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example: \include MatrixBase_rightCols_int.cpp
+/// Output: \verbinclude MatrixBase_rightCols_int.out
+///
+/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename NColsBlockXpr<...>::Type
+#endif
+    rightCols(NColsType n) {
+  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type(
+      derived(), 0, cols() - internal::get_runtime_value(n), rows(), internal::get_runtime_value(n));
+}
+
+/// This is the const version of rightCols(NColsType).
+template <typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+    const typename ConstNColsBlockXpr<...>::Type
+#endif
+    rightCols(NColsType n) const {
+  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type(
+      derived(), 0, cols() - internal::get_runtime_value(n), rows(), internal::get_runtime_value(n));
+}
+
+/// \returns a block consisting of the right columns of \c *this.
+///
+/// \tparam N the number of columns in the block as specified at compile-time
+/// \param n the number of columns in the block as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include MatrixBase_template_int_rightCols.cpp
+/// Output: \verbinclude MatrixBase_template_int_rightCols.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NColsBlockXpr<N>::Type rightCols(Index n = N) {
+  return typename NColsBlockXpr<N>::Type(derived(), 0, cols() - n, rows(), n);
+}
+
+/// This is the const version of rightCols<int>().
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstNColsBlockXpr<N>::Type rightCols(Index n = N) const {
+  return typename ConstNColsBlockXpr<N>::Type(derived(), 0, cols() - n, rows(), n);
+}
+
+/// \returns a block consisting of a range of columns of \c *this.
+///
+/// \param startCol the index of the first column in the block
+/// \param numCols the number of columns in the block
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
+///
+/// Example: \include DenseBase_middleCols_int.cpp
+/// Output: \verbinclude DenseBase_middleCols_int.out
+///
+/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+    typename NColsBlockXpr<...>::Type
+#endif
+    middleCols(Index startCol, NColsType numCols) {
+  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type(
+      derived(), 0, startCol, rows(), internal::get_runtime_value(numCols));
+}
+
+/// This is the const version of middleCols(Index,NColsType).
+template <typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+    const typename ConstNColsBlockXpr<...>::Type
+#endif
+    middleCols(Index startCol, NColsType numCols) const {
+  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type(
+      derived(), 0, startCol, rows(), internal::get_runtime_value(numCols));
+}
+
+/// \returns a block consisting of a range of columns of \c *this.
+///
+/// \tparam N the number of columns in the block as specified at compile-time
+/// \param startCol the index of the first column in the block
+/// \param n the number of columns in the block as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include DenseBase_template_int_middleCols.cpp
+/// Output: \verbinclude DenseBase_template_int_middleCols.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column - major)
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NColsBlockXpr<N>::Type middleCols(Index startCol, Index n = N) {
+  return typename NColsBlockXpr<N>::Type(derived(), 0, startCol, rows(), n);
+}
+
+/// This is the const version of middleCols<int>().
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstNColsBlockXpr<N>::Type middleCols(Index startCol,
+                                                                                      Index n = N) const {
+  return typename ConstNColsBlockXpr<N>::Type(derived(), 0, startCol, rows(), n);
+}
+
+/// \returns a fixed-size expression of a block of \c *this.
+///
+/// The template parameters \a NRows and \a NCols are the number of
+/// rows and columns in the block.
+///
+/// \param startRow the first row in the block
+/// \param startCol the first column in the block
+///
+/// Example: \include MatrixBase_block_int_int.cpp
+/// Output: \verbinclude MatrixBase_block_int_int.out
+///
+/// \note The usage of of this overload is discouraged from %Eigen 3.4, better used the generic
+/// block(Index,Index,NRowsType,NColsType), here is the one-to-one equivalence:
+/// \code
+/// mat.template block<NRows,NCols>(i,j)  <-->  mat.block(i,j,fix<NRows>,fix<NCols>)
+/// \endcode
+///
+/// \note since block is a templated member, the keyword template has to be used
+/// if the matrix type is also a template parameter: \code m.template block<3,3>(1,1); \endcode
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int NRows, int NCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<NRows, NCols>::Type block(Index startRow, Index startCol) {
+  return typename FixedBlockXpr<NRows, NCols>::Type(derived(), startRow, startCol);
+}
+
+/// This is the const version of block<>(Index, Index). */
+template <int NRows, int NCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<NRows, NCols>::Type block(
+    Index startRow, Index startCol) const {
+  return typename ConstFixedBlockXpr<NRows, NCols>::Type(derived(), startRow, startCol);
+}
+
+/// \returns an expression of a block of \c *this.
+///
+/// \tparam NRows number of rows in block as specified at compile-time
+/// \tparam NCols number of columns in block as specified at compile-time
+/// \param  startRow  the first row in the block
+/// \param  startCol  the first column in the block
+/// \param  blockRows number of rows in block as specified at run-time
+/// \param  blockCols number of columns in block as specified at run-time
+///
+/// This function is mainly useful for blocks where the number of rows is specified at compile-time
+/// and the number of columns is specified at run-time, or vice versa. The compile-time and run-time
+/// information should not contradict. In other words, \a blockRows should equal \a NRows unless
+/// \a NRows is \a Dynamic, and the same for the number of columns.
+///
+/// Example: \include MatrixBase_template_int_int_block_int_int_int_int.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_block_int_int_int_int.out
+///
+/// \note The usage of of this overload is discouraged from %Eigen 3.4, better used the generic
+/// block(Index,Index,NRowsType,NColsType), here is the one-to-one complete equivalence:
+/// \code
+/// mat.template block<NRows,NCols>(i,j,rows,cols)     <-->  mat.block(i,j,fix<NRows>(rows),fix<NCols>(cols))
+/// \endcode
+/// If we known that, e.g., NRows==Dynamic and NCols!=Dynamic, then the equivalence becomes:
+/// \code
+/// mat.template block<Dynamic,NCols>(i,j,rows,NCols)  <-->  mat.block(i,j,rows,fix<NCols>)
+/// \endcode
+///
+EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+///
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
+///
+template <int NRows, int NCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedBlockXpr<NRows, NCols>::Type block(Index startRow, Index startCol,
+                                                                                       Index blockRows,
+                                                                                       Index blockCols) {
+  return typename FixedBlockXpr<NRows, NCols>::Type(derived(), startRow, startCol, blockRows, blockCols);
+}
+
+/// This is the const version of block<>(Index, Index, Index, Index).
+template <int NRows, int NCols>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename ConstFixedBlockXpr<NRows, NCols>::Type block(
+    Index startRow, Index startCol, Index blockRows, Index blockCols) const {
+  return typename ConstFixedBlockXpr<NRows, NCols>::Type(derived(), startRow, startCol, blockRows, blockCols);
+}
+
+/// \returns an expression of the \a i-th column of \c *this. Note that the numbering starts at 0.
+///
+/// Example: \include MatrixBase_col.cpp
+/// Output: \verbinclude MatrixBase_col.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column - major)
+/**
+ * \sa row(), class Block */
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ColXpr col(Index i) { return ColXpr(derived(), i); }
+
+/// This is the const version of col().
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ConstColXpr col(Index i) const { return ConstColXpr(derived(), i); }
+
+/// \returns an expression of the \a i-th row of \c *this. Note that the numbering starts at 0.
+///
+/// Example: \include MatrixBase_row.cpp
+/// Output: \verbinclude MatrixBase_row.out
+///
+EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row - major)
+/**
+ * \sa col(), class Block */
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE RowXpr row(Index i) { return RowXpr(derived(), i); }
+
+/// This is the const version of row(). */
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ConstRowXpr row(Index i) const { return ConstRowXpr(derived(), i); }
+
+/// \returns an expression of a segment (i.e. a vector block) in \c *this with either dynamic or fixed sizes.
+///
+/// \only_for_vectors
+///
+/// \param start the first coefficient in the segment
+/// \param n the number of coefficients in the segment
+/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
+///
+/// Example: \include MatrixBase_segment_int_int.cpp
+/// Output: \verbinclude MatrixBase_segment_int_int.out
+///
+/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
+/// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
+/// which means that evaluating it does not cause a dynamic memory allocation.
+///
+/// \sa block(Index,Index,NRowsType,NColsType), fix<N>, fix<N>(int), class Block
+///
+template <typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+    typename FixedSegmentReturnType<...>::Type
+#endif
+    segment(Index start, NType n) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type(derived(), start,
+                                                                                        internal::get_runtime_value(n));
+}
+
+/// This is the const version of segment(Index,NType).
+template <typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+    const typename ConstFixedSegmentReturnType<...>::Type
+#endif
+    segment(Index start, NType n) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type(
+      derived(), start, internal::get_runtime_value(n));
+}
+
+/// \returns an expression of the first coefficients of \c *this with either dynamic or fixed sizes.
+///
+/// \only_for_vectors
+///
+/// \param n the number of coefficients in the segment
+/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
+///
+/// Example: \include MatrixBase_start_int.cpp
+/// Output: \verbinclude MatrixBase_start_int.out
+///
+/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
+/// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
+/// which means that evaluating it does not cause a dynamic memory allocation.
+///
+/// \sa class Block, block(Index,Index)
+///
+template <typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+    typename FixedSegmentReturnType<...>::Type
+#endif
+    head(NType n) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type(derived(), 0,
+                                                                                        internal::get_runtime_value(n));
+}
+
+/// This is the const version of head(NType).
+template <typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+    const typename ConstFixedSegmentReturnType<...>::Type
+#endif
+    head(NType n) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type(
+      derived(), 0, internal::get_runtime_value(n));
+}
+
+/// \returns an expression of a last coefficients of \c *this with either dynamic or fixed sizes.
+///
+/// \only_for_vectors
+///
+/// \param n the number of coefficients in the segment
+/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
+///
+/// Example: \include MatrixBase_end_int.cpp
+/// Output: \verbinclude MatrixBase_end_int.out
+///
+/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
+/// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
+/// which means that evaluating it does not cause a dynamic memory allocation.
+///
+/// \sa class Block, block(Index,Index)
+///
+template <typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+    typename FixedSegmentReturnType<...>::Type
+#endif
+    tail(NType n) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type(
+      derived(), this->size() - internal::get_runtime_value(n), internal::get_runtime_value(n));
+}
+
+/// This is the const version of tail(Index).
+template <typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+    const typename ConstFixedSegmentReturnType<...>::Type
+#endif
+    tail(NType n) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type(
+      derived(), this->size() - internal::get_runtime_value(n), internal::get_runtime_value(n));
+}
+
+/// \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this
+///
+/// \only_for_vectors
+///
+/// \tparam N the number of coefficients in the segment as specified at compile-time
+/// \param start the index of the first element in the segment
+/// \param n the number of coefficients in the segment as specified at compile-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include MatrixBase_template_int_segment.cpp
+/// Output: \verbinclude MatrixBase_template_int_segment.out
+///
+/// \sa segment(Index,NType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedSegmentReturnType<N>::Type segment(Index start, Index n = N) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename FixedSegmentReturnType<N>::Type(derived(), start, n);
+}
+
+/// This is the const version of segment<int>(Index).
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstFixedSegmentReturnType<N>::Type segment(Index start,
+                                                                                            Index n = N) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename ConstFixedSegmentReturnType<N>::Type(derived(), start, n);
+}
+
+/// \returns a fixed-size expression of the first coefficients of \c *this.
+///
+/// \only_for_vectors
+///
+/// \tparam N the number of coefficients in the segment as specified at compile-time
+/// \param  n the number of coefficients in the segment as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include MatrixBase_template_int_start.cpp
+/// Output: \verbinclude MatrixBase_template_int_start.out
+///
+/// \sa head(NType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedSegmentReturnType<N>::Type head(Index n = N) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename FixedSegmentReturnType<N>::Type(derived(), 0, n);
+}
+
+/// This is the const version of head<int>().
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstFixedSegmentReturnType<N>::Type head(Index n = N) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename ConstFixedSegmentReturnType<N>::Type(derived(), 0, n);
+}
+
+/// \returns a fixed-size expression of the last coefficients of \c *this.
+///
+/// \only_for_vectors
+///
+/// \tparam N the number of coefficients in the segment as specified at compile-time
+/// \param  n the number of coefficients in the segment as specified at run-time
+///
+/// The compile-time and run-time information should not contradict. In other words,
+/// \a n should equal \a N unless \a N is \a Dynamic.
+///
+/// Example: \include MatrixBase_template_int_end.cpp
+/// Output: \verbinclude MatrixBase_template_int_end.out
+///
+/// \sa tail(NType), class Block
+///
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename FixedSegmentReturnType<N>::Type tail(Index n = N) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename FixedSegmentReturnType<N>::Type(derived(), size() - n);
+}
+
+/// This is the const version of tail<int>.
+template <int N>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ConstFixedSegmentReturnType<N>::Type tail(Index n = N) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename ConstFixedSegmentReturnType<N>::Type(derived(), size() - n);
+}
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major).
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE InnerVectorReturnType innerVector(Index outer) {
+  return InnerVectorReturnType(derived(), outer);
+}
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major). Read-only.
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ConstInnerVectorReturnType innerVector(Index outer) const {
+  return ConstInnerVectorReturnType(derived(), outer);
+}
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major).
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE InnerVectorsReturnType innerVectors(Index outerStart, Index outerSize) {
+  return Block<Derived, Dynamic, Dynamic, true>(derived(), IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart,
+                                                IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize);
+}
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major). Read-only.
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ConstInnerVectorsReturnType innerVectors(Index outerStart,
+                                                                                     Index outerSize) const {
+  return Block<const Derived, Dynamic, Dynamic, true>(derived(), IsRowMajor ? outerStart : 0,
+                                                      IsRowMajor ? 0 : outerStart, IsRowMajor ? outerSize : rows(),
+                                                      IsRowMajor ? cols() : outerSize);
+}
+
+/** \returns the i-th subvector (column or vector) according to the \c Direction
+ * \sa subVectors()
+ */
+template <DirectionType Direction>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::conditional_t<Direction == Vertical, ColXpr, RowXpr> subVector(Index i) {
+  return std::conditional_t<Direction == Vertical, ColXpr, RowXpr>(derived(), i);
+}
+
+/** This is the const version of subVector(Index) */
+template <DirectionType Direction>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::conditional_t<Direction == Vertical, ConstColXpr, ConstRowXpr> subVector(
+    Index i) const {
+  return std::conditional_t<Direction == Vertical, ConstColXpr, ConstRowXpr>(derived(), i);
+}
+
+/** \returns the number of subvectors (rows or columns) in the direction \c Direction
+ * \sa subVector(Index)
+ */
+template <DirectionType Direction>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index subVectors() const {
+  return (Direction == Vertical) ? cols() : rows();
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.h
deleted file mode 100644
index 8b6730e..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.h
+++ /dev/null
@@ -1,115 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2016 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-// This file is a base class plugin containing common coefficient wise functions.
-
-/** \returns an expression of the difference of \c *this and \a other
-  *
-  * \note If you want to substract a given scalar from all coefficients, see Cwise::operator-().
-  *
-  * \sa class CwiseBinaryOp, operator-=()
-  */
-EIGEN_MAKE_CWISE_BINARY_OP(operator-,difference)
-
-/** \returns an expression of the sum of \c *this and \a other
-  *
-  * \note If you want to add a given scalar to all coefficients, see Cwise::operator+().
-  *
-  * \sa class CwiseBinaryOp, operator+=()
-  */
-EIGEN_MAKE_CWISE_BINARY_OP(operator+,sum)
-
-/** \returns an expression of a custom coefficient-wise operator \a func of *this and \a other
-  *
-  * The template parameter \a CustomBinaryOp is the type of the functor
-  * of the custom operator (see class CwiseBinaryOp for an example)
-  *
-  * Here is an example illustrating the use of custom functors:
-  * \include class_CwiseBinaryOp.cpp
-  * Output: \verbinclude class_CwiseBinaryOp.out
-  *
-  * \sa class CwiseBinaryOp, operator+(), operator-(), cwiseProduct()
-  */
-template<typename CustomBinaryOp, typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<CustomBinaryOp, const Derived, const OtherDerived>
-binaryExpr(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other, const CustomBinaryOp& func = CustomBinaryOp()) const
-{
-  return CwiseBinaryOp<CustomBinaryOp, const Derived, const OtherDerived>(derived(), other.derived(), func);
-}
-
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-EIGEN_MAKE_SCALAR_BINARY_OP(operator*,product)
-#else
-/** \returns an expression of \c *this scaled by the scalar factor \a scalar
-  *
-  * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
-  */
-template<typename T>
-const CwiseBinaryOp<internal::scalar_product_op<Scalar,T>,Derived,Constant<T> > operator*(const T& scalar) const;
-/** \returns an expression of \a expr scaled by the scalar factor \a scalar
-  *
-  * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
-  */
-template<typename T> friend
-const CwiseBinaryOp<internal::scalar_product_op<T,Scalar>,Constant<T>,Derived> operator*(const T& scalar, const StorageBaseType& expr);
-#endif
-
-
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(operator/,quotient)
-#else
-/** \returns an expression of \c *this divided by the scalar value \a scalar
-  *
-  * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
-  */
-template<typename T>
-const CwiseBinaryOp<internal::scalar_quotient_op<Scalar,T>,Derived,Constant<T> > operator/(const T& scalar) const;
-#endif
-
-/** \returns an expression of the coefficient-wise boolean \b and operator of \c *this and \a other
-  *
-  * \warning this operator is for expression of bool only.
-  *
-  * Example: \include Cwise_boolean_and.cpp
-  * Output: \verbinclude Cwise_boolean_and.out
-  *
-  * \sa operator||(), select()
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-inline const CwiseBinaryOp<internal::scalar_boolean_and_op, const Derived, const OtherDerived>
-operator&&(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  EIGEN_STATIC_ASSERT((internal::is_same<bool,Scalar>::value && internal::is_same<bool,typename OtherDerived::Scalar>::value),
-                      THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL);
-  return CwiseBinaryOp<internal::scalar_boolean_and_op, const Derived, const OtherDerived>(derived(),other.derived());
-}
-
-/** \returns an expression of the coefficient-wise boolean \b or operator of \c *this and \a other
-  *
-  * \warning this operator is for expression of bool only.
-  *
-  * Example: \include Cwise_boolean_or.cpp
-  * Output: \verbinclude Cwise_boolean_or.out
-  *
-  * \sa operator&&(), select()
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-inline const CwiseBinaryOp<internal::scalar_boolean_or_op, const Derived, const OtherDerived>
-operator||(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  EIGEN_STATIC_ASSERT((internal::is_same<bool,Scalar>::value && internal::is_same<bool,typename OtherDerived::Scalar>::value),
-                      THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL);
-  return CwiseBinaryOp<internal::scalar_boolean_or_op, const Derived, const OtherDerived>(derived(),other.derived());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc
new file mode 100644
index 0000000..95f338a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc
@@ -0,0 +1,133 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2016 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+// This file is a base class plugin containing common coefficient wise functions.
+
+/** \returns an expression of the difference of \c *this and \a other
+ *
+ * \note If you want to subtract a given scalar from all coefficients, see Cwise::operator-().
+ *
+ * \sa class CwiseBinaryOp, operator-=()
+ */
+EIGEN_MAKE_CWISE_BINARY_OP(operator-, difference)
+
+/** \returns an expression of the sum of \c *this and \a other
+ *
+ * \note If you want to add a given scalar to all coefficients, see Cwise::operator+().
+ *
+ * \sa class CwiseBinaryOp, operator+=()
+ */
+EIGEN_MAKE_CWISE_BINARY_OP(operator+, sum)
+
+/** \returns an expression of a custom coefficient-wise operator \a func of *this and \a other
+ *
+ * The template parameter \a CustomBinaryOp is the type of the functor
+ * of the custom operator (see class CwiseBinaryOp for an example)
+ *
+ * Here is an example illustrating the use of custom functors:
+ * \include class_CwiseBinaryOp.cpp
+ * Output: \verbinclude class_CwiseBinaryOp.out
+ *
+ * \sa class CwiseBinaryOp, operator+(), operator-(), cwiseProduct()
+ */
+template <typename CustomBinaryOp, typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryOp<CustomBinaryOp, const Derived, const OtherDerived> binaryExpr(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other, const CustomBinaryOp& func = CustomBinaryOp()) const {
+  return CwiseBinaryOp<CustomBinaryOp, const Derived, const OtherDerived>(derived(), other.derived(), func);
+}
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+EIGEN_MAKE_SCALAR_BINARY_OP(operator*, product)
+#else
+/** \returns an expression of \c *this scaled by the scalar factor \a scalar
+ *
+ * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
+ */
+template <typename T>
+const CwiseBinaryOp<internal::scalar_product_op<Scalar, T>, Derived, Constant<T> > operator*(const T& scalar) const;
+/** \returns an expression of \a expr scaled by the scalar factor \a scalar
+ *
+ * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
+ */
+template <typename T>
+friend const CwiseBinaryOp<internal::scalar_product_op<T, Scalar>, Constant<T>, Derived> operator*(
+    const T& scalar, const StorageBaseType& expr);
+#endif
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(operator/, quotient)
+#else
+/** \returns an expression of \c *this divided by the scalar value \a scalar
+ *
+ * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression.
+ */
+template <typename T>
+const CwiseBinaryOp<internal::scalar_quotient_op<Scalar, T>, Derived, Constant<T> > operator/(const T& scalar) const;
+#endif
+
+/** \returns an expression of the coefficient-wise boolean \b and operator of \c *this and \a other
+ *
+ * Example: \include Cwise_boolean_and.cpp
+ * Output: \verbinclude Cwise_boolean_and.out
+ *
+ * \sa operator||(), select()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryOp<internal::scalar_boolean_and_op<Scalar>, const Derived, const OtherDerived>
+operator&&(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_boolean_and_op<Scalar>, const Derived, const OtherDerived>(derived(),
+                                                                                                   other.derived());
+}
+
+/** \returns an expression of the coefficient-wise boolean \b or operator of \c *this and \a other
+ *
+ * Example: \include Cwise_boolean_or.cpp
+ * Output: \verbinclude Cwise_boolean_or.out
+ *
+ * \sa operator&&(), select()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryOp<internal::scalar_boolean_or_op<Scalar>, const Derived, const OtherDerived>
+operator||(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_boolean_or_op<Scalar>, const Derived, const OtherDerived>(derived(),
+                                                                                                  other.derived());
+}
+
+/** \returns an expression of the bitwise \b and operator of \c *this and \a other
+ *
+ * \sa operator|(), operator^()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryOp<internal::scalar_bitwise_and_op<Scalar>, const Derived, const OtherDerived>
+operator&(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_bitwise_and_op<Scalar>, const Derived, const OtherDerived>(derived(),
+                                                                                                   other.derived());
+}
+
+/** \returns an expression of the bitwise boolean \b or operator of \c *this and \a other
+ *
+ * \sa operator&(), operator^()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryOp<internal::scalar_bitwise_or_op<Scalar>, const Derived, const OtherDerived>
+operator|(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_bitwise_or_op<Scalar>, const Derived, const OtherDerived>(derived(),
+                                                                                                  other.derived());
+}
+
+/** \returns an expression of the bitwise xor operator of *this and \a other
+ * \sa operator&(), operator|()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryOp<internal::scalar_bitwise_xor_op<Scalar>, const Derived, const OtherDerived>
+operator^(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_bitwise_xor_op<Scalar>, const Derived, const OtherDerived>(derived(),
+                                                                                                   other.derived());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.h
deleted file mode 100644
index 5418dc4..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.h
+++ /dev/null
@@ -1,177 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-// This file is a base class plugin containing common coefficient wise functions.
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-
-/** \internal the return type of conjugate() */
-typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-                    const CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, const Derived>,
-                    const Derived&
-                  >::type ConjugateReturnType;
-/** \internal the return type of real() const */
-typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-                    const CwiseUnaryOp<internal::scalar_real_op<Scalar>, const Derived>,
-                    const Derived&
-                  >::type RealReturnType;
-/** \internal the return type of real() */
-typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-                    CwiseUnaryView<internal::scalar_real_ref_op<Scalar>, Derived>,
-                    Derived&
-                  >::type NonConstRealReturnType;
-/** \internal the return type of imag() const */
-typedef CwiseUnaryOp<internal::scalar_imag_op<Scalar>, const Derived> ImagReturnType;
-/** \internal the return type of imag() */
-typedef CwiseUnaryView<internal::scalar_imag_ref_op<Scalar>, Derived> NonConstImagReturnType;
-
-typedef CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const Derived> NegativeReturnType;
-
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-/// \returns an expression of the opposite of \c *this
-///
-EIGEN_DOC_UNARY_ADDONS(operator-,opposite)
-///
-EIGEN_DEVICE_FUNC
-inline const NegativeReturnType
-operator-() const { return NegativeReturnType(derived()); }
-
-
-template<class NewType> struct CastXpr { typedef typename internal::cast_return_type<Derived,const CwiseUnaryOp<internal::scalar_cast_op<Scalar, NewType>, const Derived> >::type Type; };
-
-/// \returns an expression of \c *this with the \a Scalar type casted to
-/// \a NewScalar.
-///
-/// The template parameter \a NewScalar is the type we are casting the scalars to.
-///
-EIGEN_DOC_UNARY_ADDONS(cast,conversion function)
-///
-/// \sa class CwiseUnaryOp
-///
-template<typename NewType>
-EIGEN_DEVICE_FUNC
-typename CastXpr<NewType>::Type
-cast() const
-{
-  return typename CastXpr<NewType>::Type(derived());
-}
-
-/// \returns an expression of the complex conjugate of \c *this.
-///
-EIGEN_DOC_UNARY_ADDONS(conjugate,complex conjugate)
-///
-/// \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_conj">Math functions</a>, MatrixBase::adjoint()
-EIGEN_DEVICE_FUNC
-inline ConjugateReturnType
-conjugate() const
-{
-  return ConjugateReturnType(derived());
-}
-
-/// \returns an expression of the complex conjugate of \c *this if Cond==true, returns derived() otherwise.
-///
-EIGEN_DOC_UNARY_ADDONS(conjugate,complex conjugate)
-///
-/// \sa conjugate()
-template<bool Cond>
-EIGEN_DEVICE_FUNC
-inline typename internal::conditional<Cond,ConjugateReturnType,const Derived&>::type
-conjugateIf() const
-{
-  typedef typename internal::conditional<Cond,ConjugateReturnType,const Derived&>::type ReturnType;
-  return ReturnType(derived());
-}
-
-/// \returns a read-only expression of the real part of \c *this.
-///
-EIGEN_DOC_UNARY_ADDONS(real,real part function)
-///
-/// \sa imag()
-EIGEN_DEVICE_FUNC
-inline RealReturnType
-real() const { return RealReturnType(derived()); }
-
-/// \returns an read-only expression of the imaginary part of \c *this.
-///
-EIGEN_DOC_UNARY_ADDONS(imag,imaginary part function)
-///
-/// \sa real()
-EIGEN_DEVICE_FUNC
-inline const ImagReturnType
-imag() const { return ImagReturnType(derived()); }
-
-/// \brief Apply a unary operator coefficient-wise
-/// \param[in]  func  Functor implementing the unary operator
-/// \tparam  CustomUnaryOp Type of \a func
-/// \returns An expression of a custom coefficient-wise unary operator \a func of *this
-///
-/// The function \c ptr_fun() from the C++ standard library can be used to make functors out of normal functions.
-///
-/// Example:
-/// \include class_CwiseUnaryOp_ptrfun.cpp
-/// Output: \verbinclude class_CwiseUnaryOp_ptrfun.out
-///
-/// Genuine functors allow for more possibilities, for instance it may contain a state.
-///
-/// Example:
-/// \include class_CwiseUnaryOp.cpp
-/// Output: \verbinclude class_CwiseUnaryOp.out
-///
-EIGEN_DOC_UNARY_ADDONS(unaryExpr,unary function)
-///
-/// \sa unaryViewExpr, binaryExpr, class CwiseUnaryOp
-///
-template<typename CustomUnaryOp>
-EIGEN_DEVICE_FUNC
-inline const CwiseUnaryOp<CustomUnaryOp, const Derived>
-unaryExpr(const CustomUnaryOp& func = CustomUnaryOp()) const
-{
-  return CwiseUnaryOp<CustomUnaryOp, const Derived>(derived(), func);
-}
-
-/// \returns an expression of a custom coefficient-wise unary operator \a func of *this
-///
-/// The template parameter \a CustomUnaryOp is the type of the functor
-/// of the custom unary operator.
-///
-/// Example:
-/// \include class_CwiseUnaryOp.cpp
-/// Output: \verbinclude class_CwiseUnaryOp.out
-///
-EIGEN_DOC_UNARY_ADDONS(unaryViewExpr,unary function)
-///
-/// \sa unaryExpr, binaryExpr class CwiseUnaryOp
-///
-template<typename CustomViewOp>
-EIGEN_DEVICE_FUNC
-inline const CwiseUnaryView<CustomViewOp, const Derived>
-unaryViewExpr(const CustomViewOp& func = CustomViewOp()) const
-{
-  return CwiseUnaryView<CustomViewOp, const Derived>(derived(), func);
-}
-
-/// \returns a non const expression of the real part of \c *this.
-///
-EIGEN_DOC_UNARY_ADDONS(real,real part function)
-///
-/// \sa imag()
-EIGEN_DEVICE_FUNC
-inline NonConstRealReturnType
-real() { return NonConstRealReturnType(derived()); }
-
-/// \returns a non const expression of the imaginary part of \c *this.
-///
-EIGEN_DOC_UNARY_ADDONS(imag,imaginary part function)
-///
-/// \sa real()
-EIGEN_DEVICE_FUNC
-inline NonConstImagReturnType
-imag() { return NonConstImagReturnType(derived()); }
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.inc
new file mode 100644
index 0000000..f20f2f8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.inc
@@ -0,0 +1,152 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+// This file is a base class plugin containing common coefficient wise functions.
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+
+/** \internal the return type of conjugate() */
+typedef std::conditional_t<NumTraits<Scalar>::IsComplex,
+                           const CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, const Derived>, const Derived&>
+    ConjugateReturnType;
+/** \internal the return type of real() const */
+typedef std::conditional_t<NumTraits<Scalar>::IsComplex,
+                           const CwiseUnaryOp<internal::scalar_real_op<Scalar>, const Derived>, const Derived&>
+    RealReturnType;
+/** \internal the return type of real() */
+typedef std::conditional_t<NumTraits<Scalar>::IsComplex, CwiseUnaryView<internal::scalar_real_ref_op<Scalar>, Derived>,
+                           Derived&>
+    NonConstRealReturnType;
+/** \internal the return type of imag() const */
+typedef CwiseUnaryOp<internal::scalar_imag_op<Scalar>, const Derived> ImagReturnType;
+/** \internal the return type of imag() */
+typedef CwiseUnaryView<internal::scalar_imag_ref_op<Scalar>, Derived> NonConstImagReturnType;
+
+typedef CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const Derived> NegativeReturnType;
+
+#endif  // not EIGEN_PARSED_BY_DOXYGEN
+
+/// \returns an expression of the opposite of \c *this
+///
+EIGEN_DOC_UNARY_ADDONS(operator-, opposite)
+///
+EIGEN_DEVICE_FUNC inline const NegativeReturnType operator-() const { return NegativeReturnType(derived()); }
+
+template <class NewType>
+struct CastXpr {
+  typedef typename internal::cast_return_type<
+      Derived, const CwiseUnaryOp<internal::core_cast_op<Scalar, NewType>, const Derived> >::type Type;
+};
+
+/// \returns an expression of \c *this with the \a Scalar type casted to
+/// \a NewScalar.
+///
+/// The template parameter \a NewScalar is the type we are casting the scalars to.
+///
+EIGEN_DOC_UNARY_ADDONS(cast, conversion function)
+///
+/// \sa class CwiseUnaryOp
+///
+template <typename NewType>
+EIGEN_DEVICE_FUNC typename CastXpr<NewType>::Type cast() const {
+  return typename CastXpr<NewType>::Type(derived());
+}
+
+/// \returns an expression of the complex conjugate of \c *this.
+///
+EIGEN_DOC_UNARY_ADDONS(conjugate, complex conjugate)
+///
+/// \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_conj">Math functions</a>, MatrixBase::adjoint()
+EIGEN_DEVICE_FUNC inline ConjugateReturnType conjugate() const { return ConjugateReturnType(derived()); }
+
+/// \returns an expression of the complex conjugate of \c *this if Cond==true, returns derived() otherwise.
+///
+EIGEN_DOC_UNARY_ADDONS(conjugate, complex conjugate)
+///
+/// \sa conjugate()
+template <bool Cond>
+EIGEN_DEVICE_FUNC inline std::conditional_t<Cond, ConjugateReturnType, const Derived&> conjugateIf() const {
+  typedef std::conditional_t<Cond, ConjugateReturnType, const Derived&> ReturnType;
+  return ReturnType(derived());
+}
+
+/// \returns a read-only expression of the real part of \c *this.
+///
+EIGEN_DOC_UNARY_ADDONS(real, real part function)
+///
+/// \sa imag()
+EIGEN_DEVICE_FUNC inline RealReturnType real() const { return RealReturnType(derived()); }
+
+/// \returns an read-only expression of the imaginary part of \c *this.
+///
+EIGEN_DOC_UNARY_ADDONS(imag, imaginary part function)
+///
+/// \sa real()
+EIGEN_DEVICE_FUNC inline const ImagReturnType imag() const { return ImagReturnType(derived()); }
+
+/// \brief Apply a unary operator coefficient-wise
+/// \param[in]  func  Functor implementing the unary operator
+/// \tparam  CustomUnaryOp Type of \a func
+/// \returns An expression of a custom coefficient-wise unary operator \a func of *this
+///
+/// The function \c ptr_fun() from the C++ standard library can be used to make functors out of normal functions.
+///
+/// Example:
+/// \include class_CwiseUnaryOp_ptrfun.cpp
+/// Output: \verbinclude class_CwiseUnaryOp_ptrfun.out
+///
+/// Genuine functors allow for more possibilities, for instance it may contain a state.
+///
+/// Example:
+/// \include class_CwiseUnaryOp.cpp
+/// Output: \verbinclude class_CwiseUnaryOp.out
+///
+EIGEN_DOC_UNARY_ADDONS(unaryExpr, unary function)
+///
+/// \sa unaryViewExpr, binaryExpr, class CwiseUnaryOp
+///
+template <typename CustomUnaryOp>
+EIGEN_DEVICE_FUNC inline const CwiseUnaryOp<CustomUnaryOp, const Derived> unaryExpr(
+    const CustomUnaryOp& func = CustomUnaryOp()) const {
+  return CwiseUnaryOp<CustomUnaryOp, const Derived>(derived(), func);
+}
+
+/// \returns an expression of a custom coefficient-wise unary operator \a func of *this
+///
+/// The template parameter \a CustomUnaryOp is the type of the functor
+/// of the custom unary operator.
+///
+/// Example:
+/// \include class_CwiseUnaryOp.cpp
+/// Output: \verbinclude class_CwiseUnaryOp.out
+///
+EIGEN_DOC_UNARY_ADDONS(unaryViewExpr, unary function)
+///
+/// \sa unaryExpr, binaryExpr class CwiseUnaryOp
+///
+template <typename CustomViewOp>
+EIGEN_DEVICE_FUNC inline const CwiseUnaryView<CustomViewOp, const Derived> unaryViewExpr(
+    const CustomViewOp& func = CustomViewOp()) const {
+  return CwiseUnaryView<CustomViewOp, const Derived>(derived(), func);
+}
+
+/// \returns a non const expression of the real part of \c *this.
+///
+EIGEN_DOC_UNARY_ADDONS(real, real part function)
+///
+/// \sa imag()
+EIGEN_DEVICE_FUNC inline NonConstRealReturnType real() { return NonConstRealReturnType(derived()); }
+
+/// \returns a non const expression of the imaginary part of \c *this.
+///
+EIGEN_DOC_UNARY_ADDONS(imag, imaginary part function)
+///
+/// \sa real()
+EIGEN_DEVICE_FUNC inline NonConstImagReturnType imag() { return NonConstImagReturnType(derived()); }
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.h
deleted file mode 100644
index 5bfb19a..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.h
+++ /dev/null
@@ -1,262 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#if !defined(EIGEN_PARSED_BY_DOXYGEN)
-
-// This file is automatically included twice to generate const and non-const versions
-
-#ifndef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
-#define EIGEN_INDEXED_VIEW_METHOD_CONST const
-#define EIGEN_INDEXED_VIEW_METHOD_TYPE  ConstIndexedViewType
-#else
-#define EIGEN_INDEXED_VIEW_METHOD_CONST
-#define EIGEN_INDEXED_VIEW_METHOD_TYPE IndexedViewType
-#endif
-
-#ifndef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
-protected:
-
-// define some aliases to ease readability
-
-template<typename Indices>
-struct IvcRowType : public internal::IndexedViewCompatibleType<Indices,RowsAtCompileTime> {};
-
-template<typename Indices>
-struct IvcColType : public internal::IndexedViewCompatibleType<Indices,ColsAtCompileTime> {};
-
-template<typename Indices>
-struct IvcType : public internal::IndexedViewCompatibleType<Indices,SizeAtCompileTime> {};
-
-typedef typename internal::IndexedViewCompatibleType<Index,1>::type IvcIndex;
-
-template<typename Indices>
-typename IvcRowType<Indices>::type
-ivcRow(const Indices& indices) const {
-  return internal::makeIndexedViewCompatible(indices, internal::variable_if_dynamic<Index,RowsAtCompileTime>(derived().rows()),Specialized);
-}
-
-template<typename Indices>
-typename IvcColType<Indices>::type
-ivcCol(const Indices& indices) const {
-  return internal::makeIndexedViewCompatible(indices, internal::variable_if_dynamic<Index,ColsAtCompileTime>(derived().cols()),Specialized);
-}
-
-template<typename Indices>
-typename IvcColType<Indices>::type
-ivcSize(const Indices& indices) const {
-  return internal::makeIndexedViewCompatible(indices, internal::variable_if_dynamic<Index,SizeAtCompileTime>(derived().size()),Specialized);
-}
-
-public:
-
-#endif
-
-template<typename RowIndices, typename ColIndices>
-struct EIGEN_INDEXED_VIEW_METHOD_TYPE {
-  typedef IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,
-                      typename IvcRowType<RowIndices>::type,
-                      typename IvcColType<ColIndices>::type> type;
-};
-
-// This is the generic version
-
-template<typename RowIndices, typename ColIndices>
-typename internal::enable_if<internal::valid_indexed_view_overload<RowIndices,ColIndices>::value
-  && internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::ReturnAsIndexedView,
-  typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type >::type
-operator()(const RowIndices& rowIndices, const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  return typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type
-            (derived(), ivcRow(rowIndices), ivcCol(colIndices));
-}
-
-// The following overload returns a Block<> object
-
-template<typename RowIndices, typename ColIndices>
-typename internal::enable_if<internal::valid_indexed_view_overload<RowIndices,ColIndices>::value
-  && internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::ReturnAsBlock,
-  typename internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::BlockType>::type
-operator()(const RowIndices& rowIndices, const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  typedef typename internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::BlockType BlockType;
-  typename IvcRowType<RowIndices>::type actualRowIndices = ivcRow(rowIndices);
-  typename IvcColType<ColIndices>::type actualColIndices = ivcCol(colIndices);
-  return BlockType(derived(),
-                   internal::first(actualRowIndices),
-                   internal::first(actualColIndices),
-                   internal::size(actualRowIndices),
-                   internal::size(actualColIndices));
-}
-
-// The following overload returns a Scalar
-
-template<typename RowIndices, typename ColIndices>
-typename internal::enable_if<internal::valid_indexed_view_overload<RowIndices,ColIndices>::value
-  && internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::ReturnAsScalar,
-  CoeffReturnType >::type
-operator()(const RowIndices& rowIndices, const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  return Base::operator()(internal::eval_expr_given_size(rowIndices,rows()),internal::eval_expr_given_size(colIndices,cols()));
-}
-
-#if EIGEN_HAS_STATIC_ARRAY_TEMPLATE
-
-// The following three overloads are needed to handle raw Index[N] arrays.
-
-template<typename RowIndicesT, std::size_t RowIndicesN, typename ColIndices>
-IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN],typename IvcColType<ColIndices>::type>
-operator()(const RowIndicesT (&rowIndices)[RowIndicesN], const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN],typename IvcColType<ColIndices>::type>
-                    (derived(), rowIndices, ivcCol(colIndices));
-}
-
-template<typename RowIndices, typename ColIndicesT, std::size_t ColIndicesN>
-IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcRowType<RowIndices>::type, const ColIndicesT (&)[ColIndicesN]>
-operator()(const RowIndices& rowIndices, const ColIndicesT (&colIndices)[ColIndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcRowType<RowIndices>::type,const ColIndicesT (&)[ColIndicesN]>
-                    (derived(), ivcRow(rowIndices), colIndices);
-}
-
-template<typename RowIndicesT, std::size_t RowIndicesN, typename ColIndicesT, std::size_t ColIndicesN>
-IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN], const ColIndicesT (&)[ColIndicesN]>
-operator()(const RowIndicesT (&rowIndices)[RowIndicesN], const ColIndicesT (&colIndices)[ColIndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN],const ColIndicesT (&)[ColIndicesN]>
-                    (derived(), rowIndices, colIndices);
-}
-
-#endif // EIGEN_HAS_STATIC_ARRAY_TEMPLATE
-
-// Overloads for 1D vectors/arrays
-
-template<typename Indices>
-typename internal::enable_if<
-  IsRowMajor && (!(internal::get_compile_time_incr<typename IvcType<Indices>::type>::value==1 || internal::is_valid_index_type<Indices>::value)),
-  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,typename IvcType<Indices>::type> >::type
-operator()(const Indices& indices) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,typename IvcType<Indices>::type>
-            (derived(), IvcIndex(0), ivcCol(indices));
-}
-
-template<typename Indices>
-typename internal::enable_if<
-  (!IsRowMajor) && (!(internal::get_compile_time_incr<typename IvcType<Indices>::type>::value==1 || internal::is_valid_index_type<Indices>::value)),
-  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcType<Indices>::type,IvcIndex> >::type
-operator()(const Indices& indices) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcType<Indices>::type,IvcIndex>
-            (derived(), ivcRow(indices), IvcIndex(0));
-}
-
-template<typename Indices>
-typename internal::enable_if<
-  (internal::get_compile_time_incr<typename IvcType<Indices>::type>::value==1) && (!internal::is_valid_index_type<Indices>::value) && (!symbolic::is_symbolic<Indices>::value),
-  VectorBlock<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,internal::array_size<Indices>::value> >::type
-operator()(const Indices& indices) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  typename IvcType<Indices>::type actualIndices = ivcSize(indices);
-  return VectorBlock<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,internal::array_size<Indices>::value>
-            (derived(), internal::first(actualIndices), internal::size(actualIndices));
-}
-
-template<typename IndexType>
-typename internal::enable_if<symbolic::is_symbolic<IndexType>::value, CoeffReturnType >::type
-operator()(const IndexType& id) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  return Base::operator()(internal::eval_expr_given_size(id,size()));
-}
-
-#if EIGEN_HAS_STATIC_ARRAY_TEMPLATE
-
-template<typename IndicesT, std::size_t IndicesN>
-typename internal::enable_if<IsRowMajor,
-  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,const IndicesT (&)[IndicesN]> >::type
-operator()(const IndicesT (&indices)[IndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,const IndicesT (&)[IndicesN]>
-            (derived(), IvcIndex(0), indices);
-}
-
-template<typename IndicesT, std::size_t IndicesN>
-typename internal::enable_if<!IsRowMajor,
-  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const IndicesT (&)[IndicesN],IvcIndex> >::type
-operator()(const IndicesT (&indices)[IndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
-{
-  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const IndicesT (&)[IndicesN],IvcIndex>
-            (derived(), indices, IvcIndex(0));
-}
-
-#endif // EIGEN_HAS_STATIC_ARRAY_TEMPLATE
-
-#undef EIGEN_INDEXED_VIEW_METHOD_CONST
-#undef EIGEN_INDEXED_VIEW_METHOD_TYPE
-
-#ifndef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
-#define EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
-#include "IndexedViewMethods.h"
-#undef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
-#endif
-
-#else // EIGEN_PARSED_BY_DOXYGEN
-
-/**
-  * \returns a generic submatrix view defined by the rows and columns indexed \a rowIndices and \a colIndices respectively.
-  *
-  * Each parameter must either be:
-  *  - An integer indexing a single row or column
-  *  - Eigen::all indexing the full set of respective rows or columns in increasing order
-  *  - An ArithmeticSequence as returned by the Eigen::seq and Eigen::seqN functions
-  *  - Any %Eigen's vector/array of integers or expressions
-  *  - Plain C arrays: \c int[N]
-  *  - And more generally any type exposing the following two member functions:
-  * \code
-  * <integral type> operator[](<integral type>) const;
-  * <integral type> size() const;
-  * \endcode
-  * where \c <integral \c type>  stands for any integer type compatible with Eigen::Index (i.e. \c std::ptrdiff_t).
-  *
-  * The last statement implies compatibility with \c std::vector, \c std::valarray, \c std::array, many of the Range-v3's ranges, etc.
-  *
-  * If the submatrix can be represented using a starting position \c (i,j) and positive sizes \c (rows,columns), then this
-  * method will returns a Block object after extraction of the relevant information from the passed arguments. This is the case
-  * when all arguments are either:
-  *  - An integer
-  *  - Eigen::all
-  *  - An ArithmeticSequence with compile-time increment strictly equal to 1, as returned by Eigen::seq(a,b), and Eigen::seqN(a,N).
-  *
-  * Otherwise a more general IndexedView<Derived,RowIndices',ColIndices'> object will be returned, after conversion of the inputs
-  * to more suitable types \c RowIndices' and \c ColIndices'.
-  *
-  * For 1D vectors and arrays, you better use the operator()(const Indices&) overload, which behave the same way but taking a single parameter.
-  *
-  * See also this <a href="https://stackoverflow.com/questions/46110917/eigen-replicate-items-along-one-dimension-without-useless-allocations">question</a> and its answer for an example of how to duplicate coefficients.
-  *
-  * \sa operator()(const Indices&), class Block, class IndexedView, DenseBase::block(Index,Index,Index,Index)
-  */
-template<typename RowIndices, typename ColIndices>
-IndexedView_or_Block
-operator()(const RowIndices& rowIndices, const ColIndices& colIndices);
-
-/** This is an overload of operator()(const RowIndices&, const ColIndices&) for 1D vectors or arrays
-  *
-  * \only_for_vectors
-  */
-template<typename Indices>
-IndexedView_or_VectorBlock
-operator()(const Indices& indices);
-
-#endif  // EIGEN_PARSED_BY_DOXYGEN
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.inc
new file mode 100644
index 0000000..26e7b5f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.inc
@@ -0,0 +1,351 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#if !defined(EIGEN_PARSED_BY_DOXYGEN)
+
+protected:
+// define some aliases to ease readability
+
+template <typename Indices>
+using IvcRowType = typename internal::IndexedViewCompatibleType<Indices, RowsAtCompileTime>::type;
+
+template <typename Indices>
+using IvcColType = typename internal::IndexedViewCompatibleType<Indices, ColsAtCompileTime>::type;
+
+template <typename Indices>
+using IvcType = typename internal::IndexedViewCompatibleType<Indices, SizeAtCompileTime>::type;
+
+typedef typename internal::IndexedViewCompatibleType<Index, 1>::type IvcIndex;
+
+template <typename Indices>
+inline IvcRowType<Indices> ivcRow(const Indices& indices) const {
+  return internal::makeIndexedViewCompatible(
+      indices, internal::variable_if_dynamic<Index, RowsAtCompileTime>(derived().rows()), Specialized);
+}
+
+template <typename Indices>
+inline IvcColType<Indices> ivcCol(const Indices& indices) const {
+  return internal::makeIndexedViewCompatible(
+      indices, internal::variable_if_dynamic<Index, ColsAtCompileTime>(derived().cols()), Specialized);
+}
+
+template <typename Indices>
+inline IvcType<Indices> ivcSize(const Indices& indices) const {
+  return internal::makeIndexedViewCompatible(
+      indices, internal::variable_if_dynamic<Index, SizeAtCompileTime>(derived().size()), Specialized);
+}
+
+// this helper class assumes internal::valid_indexed_view_overload<RowIndices, ColIndices>::value == true
+template <typename RowIndices, typename ColIndices,
+          bool UseSymbolic =
+              internal::traits<IndexedView<Derived, IvcRowType<RowIndices>, IvcColType<ColIndices>>>::ReturnAsScalar,
+          bool UseBlock =
+              internal::traits<IndexedView<Derived, IvcRowType<RowIndices>, IvcColType<ColIndices>>>::ReturnAsBlock,
+          bool UseGeneric = internal::traits<
+              IndexedView<Derived, IvcRowType<RowIndices>, IvcColType<ColIndices>>>::ReturnAsIndexedView>
+struct IndexedViewSelector;
+
+// Generic
+template <typename RowIndices, typename ColIndices>
+struct IndexedViewSelector<RowIndices, ColIndices, false, false, true> {
+  using ReturnType = IndexedView<Derived, IvcRowType<RowIndices>, IvcColType<ColIndices>>;
+  using ConstReturnType = IndexedView<const Derived, IvcRowType<RowIndices>, IvcColType<ColIndices>>;
+
+  static inline ReturnType run(Derived& derived, const RowIndices& rowIndices, const ColIndices& colIndices) {
+    return ReturnType(derived, derived.ivcRow(rowIndices), derived.ivcCol(colIndices));
+  }
+  static inline ConstReturnType run(const Derived& derived, const RowIndices& rowIndices,
+                                    const ColIndices& colIndices) {
+    return ConstReturnType(derived, derived.ivcRow(rowIndices), derived.ivcCol(colIndices));
+  }
+};
+
+// Block
+template <typename RowIndices, typename ColIndices>
+struct IndexedViewSelector<RowIndices, ColIndices, false, true, false> {
+  using IndexedViewType = IndexedView<Derived, IvcRowType<RowIndices>, IvcColType<ColIndices>>;
+  using ConstIndexedViewType = IndexedView<const Derived, IvcRowType<RowIndices>, IvcColType<ColIndices>>;
+  using ReturnType = typename internal::traits<IndexedViewType>::BlockType;
+  using ConstReturnType = typename internal::traits<ConstIndexedViewType>::BlockType;
+
+  static inline ReturnType run(Derived& derived, const RowIndices& rowIndices, const ColIndices& colIndices) {
+    IvcRowType<RowIndices> actualRowIndices = derived.ivcRow(rowIndices);
+    IvcColType<ColIndices> actualColIndices = derived.ivcCol(colIndices);
+    return ReturnType(derived, internal::first(actualRowIndices), internal::first(actualColIndices),
+                      internal::index_list_size(actualRowIndices), internal::index_list_size(actualColIndices));
+  }
+  static inline ConstReturnType run(const Derived& derived, const RowIndices& rowIndices,
+                                    const ColIndices& colIndices) {
+    IvcRowType<RowIndices> actualRowIndices = derived.ivcRow(rowIndices);
+    IvcColType<ColIndices> actualColIndices = derived.ivcCol(colIndices);
+    return ConstReturnType(derived, internal::first(actualRowIndices), internal::first(actualColIndices),
+                           internal::index_list_size(actualRowIndices), internal::index_list_size(actualColIndices));
+  }
+};
+
+// Symbolic
+template <typename RowIndices, typename ColIndices>
+struct IndexedViewSelector<RowIndices, ColIndices, true, false, false> {
+  using ReturnType = typename DenseBase<Derived>::Scalar&;
+  using ConstReturnType = typename DenseBase<Derived>::CoeffReturnType;
+
+  static inline ReturnType run(Derived& derived, const RowIndices& rowIndices, const ColIndices& colIndices) {
+    return derived(internal::eval_expr_given_size(rowIndices, derived.rows()),
+                   internal::eval_expr_given_size(colIndices, derived.cols()));
+  }
+  static inline ConstReturnType run(const Derived& derived, const RowIndices& rowIndices,
+                                    const ColIndices& colIndices) {
+    return derived(internal::eval_expr_given_size(rowIndices, derived.rows()),
+                   internal::eval_expr_given_size(colIndices, derived.cols()));
+  }
+};
+
+// this helper class assumes internal::is_valid_index_type<Indices>::value == false
+template <typename Indices, bool UseSymbolic = symbolic::is_symbolic<Indices>::value,
+          bool UseBlock = !UseSymbolic && internal::get_compile_time_incr<IvcType<Indices>>::value == 1,
+          bool UseGeneric = !UseSymbolic && !UseBlock>
+struct VectorIndexedViewSelector;
+
+// Generic
+template <typename Indices>
+struct VectorIndexedViewSelector<Indices, false, false, true> {
+  static constexpr bool IsRowMajor = DenseBase<Derived>::IsRowMajor;
+
+  using RowMajorReturnType = IndexedView<Derived, IvcIndex, IvcType<Indices>>;
+  using ConstRowMajorReturnType = IndexedView<const Derived, IvcIndex, IvcType<Indices>>;
+
+  using ColMajorReturnType = IndexedView<Derived, IvcType<Indices>, IvcIndex>;
+  using ConstColMajorReturnType = IndexedView<const Derived, IvcType<Indices>, IvcIndex>;
+
+  using ReturnType = typename internal::conditional<IsRowMajor, RowMajorReturnType, ColMajorReturnType>::type;
+  using ConstReturnType =
+      typename internal::conditional<IsRowMajor, ConstRowMajorReturnType, ConstColMajorReturnType>::type;
+
+  template <bool UseRowMajor = IsRowMajor, std::enable_if_t<UseRowMajor, bool> = true>
+  static inline RowMajorReturnType run(Derived& derived, const Indices& indices) {
+    return RowMajorReturnType(derived, IvcIndex(0), derived.ivcCol(indices));
+  }
+  template <bool UseRowMajor = IsRowMajor, std::enable_if_t<UseRowMajor, bool> = true>
+  static inline ConstRowMajorReturnType run(const Derived& derived, const Indices& indices) {
+    return ConstRowMajorReturnType(derived, IvcIndex(0), derived.ivcCol(indices));
+  }
+  template <bool UseRowMajor = IsRowMajor, std::enable_if_t<!UseRowMajor, bool> = true>
+  static inline ColMajorReturnType run(Derived& derived, const Indices& indices) {
+    return ColMajorReturnType(derived, derived.ivcRow(indices), IvcIndex(0));
+  }
+  template <bool UseRowMajor = IsRowMajor, std::enable_if_t<!UseRowMajor, bool> = true>
+  static inline ConstColMajorReturnType run(const Derived& derived, const Indices& indices) {
+    return ConstColMajorReturnType(derived, derived.ivcRow(indices), IvcIndex(0));
+  }
+};
+
+// Block
+template <typename Indices>
+struct VectorIndexedViewSelector<Indices, false, true, false> {
+  using ReturnType = VectorBlock<Derived, internal::array_size<Indices>::value>;
+  using ConstReturnType = VectorBlock<const Derived, internal::array_size<Indices>::value>;
+
+  static inline ReturnType run(Derived& derived, const Indices& indices) {
+    IvcType<Indices> actualIndices = derived.ivcSize(indices);
+    return ReturnType(derived, internal::first(actualIndices), internal::index_list_size(actualIndices));
+  }
+  static inline ConstReturnType run(const Derived& derived, const Indices& indices) {
+    IvcType<Indices> actualIndices = derived.ivcSize(indices);
+    return ConstReturnType(derived, internal::first(actualIndices), internal::index_list_size(actualIndices));
+  }
+};
+
+// Symbolic
+template <typename Indices>
+struct VectorIndexedViewSelector<Indices, true, false, false> {
+  using ReturnType = typename DenseBase<Derived>::Scalar&;
+  using ConstReturnType = typename DenseBase<Derived>::CoeffReturnType;
+
+  static inline ReturnType run(Derived& derived, const Indices& id) {
+    return derived(internal::eval_expr_given_size(id, derived.size()));
+  }
+  static inline ConstReturnType run(const Derived& derived, const Indices& id) {
+    return derived(internal::eval_expr_given_size(id, derived.size()));
+  }
+};
+
+// SFINAE dummy types
+
+template <typename RowIndices, typename ColIndices>
+using EnableOverload = std::enable_if_t<
+    internal::valid_indexed_view_overload<RowIndices, ColIndices>::value && internal::is_lvalue<Derived>::value, bool>;
+
+template <typename RowIndices, typename ColIndices>
+using EnableConstOverload =
+    std::enable_if_t<internal::valid_indexed_view_overload<RowIndices, ColIndices>::value, bool>;
+
+template <typename Indices>
+using EnableVectorOverload =
+    std::enable_if_t<!internal::is_valid_index_type<Indices>::value && internal::is_lvalue<Derived>::value, bool>;
+
+template <typename Indices>
+using EnableConstVectorOverload = std::enable_if_t<!internal::is_valid_index_type<Indices>::value, bool>;
+
+public:
+// Public API for 2D matrices/arrays
+
+// non-const versions
+
+template <typename RowIndices, typename ColIndices>
+using IndexedViewType = typename IndexedViewSelector<RowIndices, ColIndices>::ReturnType;
+
+template <typename RowIndices, typename ColIndices, EnableOverload<RowIndices, ColIndices> = true>
+IndexedViewType<RowIndices, ColIndices> operator()(const RowIndices& rowIndices, const ColIndices& colIndices) {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), rowIndices, colIndices);
+}
+
+template <typename RowType, size_t RowSize, typename ColIndices, typename RowIndices = Array<RowType, RowSize, 1>,
+          EnableOverload<RowIndices, ColIndices> = true>
+IndexedViewType<RowIndices, ColIndices> operator()(const RowType (&rowIndices)[RowSize], const ColIndices& colIndices) {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), RowIndices{rowIndices}, colIndices);
+}
+
+template <typename RowIndices, typename ColType, size_t ColSize, typename ColIndices = Array<ColType, ColSize, 1>,
+          EnableOverload<RowIndices, ColIndices> = true>
+IndexedViewType<RowIndices, ColIndices> operator()(const RowIndices& rowIndices, const ColType (&colIndices)[ColSize]) {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), rowIndices, ColIndices{colIndices});
+}
+
+template <typename RowType, size_t RowSize, typename ColType, size_t ColSize,
+          typename RowIndices = Array<RowType, RowSize, 1>, typename ColIndices = Array<ColType, ColSize, 1>,
+          EnableOverload<RowIndices, ColIndices> = true>
+IndexedViewType<RowIndices, ColIndices> operator()(const RowType (&rowIndices)[RowSize],
+                                                   const ColType (&colIndices)[ColSize]) {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), RowIndices{rowIndices}, ColIndices{colIndices});
+}
+
+// const versions
+
+template <typename RowIndices, typename ColIndices>
+using ConstIndexedViewType = typename IndexedViewSelector<RowIndices, ColIndices>::ConstReturnType;
+
+template <typename RowIndices, typename ColIndices, EnableConstOverload<RowIndices, ColIndices> = true>
+ConstIndexedViewType<RowIndices, ColIndices> operator()(const RowIndices& rowIndices,
+                                                        const ColIndices& colIndices) const {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), rowIndices, colIndices);
+}
+
+template <typename RowType, size_t RowSize, typename ColIndices, typename RowIndices = Array<RowType, RowSize, 1>,
+          EnableConstOverload<RowIndices, ColIndices> = true>
+ConstIndexedViewType<RowIndices, ColIndices> operator()(const RowType (&rowIndices)[RowSize],
+                                                        const ColIndices& colIndices) const {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), RowIndices{rowIndices}, colIndices);
+}
+
+template <typename RowIndices, typename ColType, size_t ColSize, typename ColIndices = Array<ColType, ColSize, 1>,
+          EnableConstOverload<RowIndices, ColIndices> = true>
+ConstIndexedViewType<RowIndices, ColIndices> operator()(const RowIndices& rowIndices,
+                                                        const ColType (&colIndices)[ColSize]) const {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), rowIndices, ColIndices{colIndices});
+}
+
+template <typename RowType, size_t RowSize, typename ColType, size_t ColSize,
+          typename RowIndices = Array<RowType, RowSize, 1>, typename ColIndices = Array<ColType, ColSize, 1>,
+          EnableConstOverload<RowIndices, ColIndices> = true>
+ConstIndexedViewType<RowIndices, ColIndices> operator()(const RowType (&rowIndices)[RowSize],
+                                                        const ColType (&colIndices)[ColSize]) const {
+  return IndexedViewSelector<RowIndices, ColIndices>::run(derived(), RowIndices{rowIndices}, ColIndices{colIndices});
+}
+
+// Public API for 1D vectors/arrays
+
+// non-const versions
+
+template <typename Indices>
+using VectorIndexedViewType = typename VectorIndexedViewSelector<Indices>::ReturnType;
+
+template <typename Indices, EnableVectorOverload<Indices> = true>
+VectorIndexedViewType<Indices> operator()(const Indices& indices) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return VectorIndexedViewSelector<Indices>::run(derived(), indices);
+}
+
+template <typename IndexType, size_t Size, typename Indices = Array<IndexType, Size, 1>,
+          EnableVectorOverload<Indices> = true>
+VectorIndexedViewType<Indices> operator()(const IndexType (&indices)[Size]) {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return VectorIndexedViewSelector<Indices>::run(derived(), Indices{indices});
+}
+
+// const versions
+
+template <typename Indices>
+using ConstVectorIndexedViewType = typename VectorIndexedViewSelector<Indices>::ConstReturnType;
+
+template <typename Indices, EnableConstVectorOverload<Indices> = true>
+ConstVectorIndexedViewType<Indices> operator()(const Indices& indices) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return VectorIndexedViewSelector<Indices>::run(derived(), indices);
+}
+
+template <typename IndexType, size_t Size, typename Indices = Array<IndexType, Size, 1>,
+          EnableConstVectorOverload<Indices> = true>
+ConstVectorIndexedViewType<Indices> operator()(const IndexType (&indices)[Size]) const {
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return VectorIndexedViewSelector<Indices>::run(derived(), Indices{indices});
+}
+
+#else  // EIGEN_PARSED_BY_DOXYGEN
+
+/**
+ * \returns a generic submatrix view defined by the rows and columns indexed \a rowIndices and \a colIndices
+ * respectively.
+ *
+ * Each parameter must either be:
+ *  - An integer indexing a single row or column
+ *  - Eigen::placeholders::all indexing the full set of respective rows or columns in increasing order
+ *  - An ArithmeticSequence as returned by the Eigen::seq and Eigen::seqN functions
+ *  - Any %Eigen's vector/array of integers or expressions
+ *  - Plain C arrays: \c int[N]
+ *  - And more generally any type exposing the following two member functions:
+ * \code
+ * <integral type> operator[](<integral type>) const;
+ * <integral type> size() const;
+ * \endcode
+ * where \c <integral \c type>  stands for any integer type compatible with Eigen::Index (i.e. \c std::ptrdiff_t).
+ *
+ * The last statement implies compatibility with \c std::vector, \c std::valarray, \c std::array, many of the Range-v3's
+ * ranges, etc.
+ *
+ * If the submatrix can be represented using a starting position \c (i,j) and positive sizes \c (rows,columns), then
+ * this method will returns a Block object after extraction of the relevant information from the passed arguments. This
+ * is the case when all arguments are either:
+ *  - An integer
+ *  - Eigen::placeholders::all
+ *  - An ArithmeticSequence with compile-time increment strictly equal to 1, as returned by Eigen::seq(a,b), and
+ * Eigen::seqN(a,N).
+ *
+ * Otherwise a more general IndexedView<Derived,RowIndices',ColIndices'> object will be returned, after conversion of
+ * the inputs to more suitable types \c RowIndices' and \c ColIndices'.
+ *
+ * For 1D vectors and arrays, you better use the operator()(const Indices&) overload, which behave the same way but
+ * taking a single parameter.
+ *
+ * See also this <a
+ * href="https://stackoverflow.com/questions/46110917/eigen-replicate-items-along-one-dimension-without-useless-allocations">question</a>
+ * and its answer for an example of how to duplicate coefficients.
+ *
+ * \sa operator()(const Indices&), class Block, class IndexedView, DenseBase::block(Index,Index,Index,Index)
+ */
+template <typename RowIndices, typename ColIndices>
+IndexedView_or_Block operator()(const RowIndices& rowIndices, const ColIndices& colIndices);
+
+/** This is an overload of operator()(const RowIndices&, const ColIndices&) for 1D vectors or arrays
+ *
+ * \only_for_vectors
+ */
+template <typename Indices>
+IndexedView_or_VectorBlock operator()(const Indices& indices);
+
+#endif  // EIGEN_PARSED_BY_DOXYGEN
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/InternalHeaderCheck.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/InternalHeaderCheck.inc
new file mode 100644
index 0000000..ac6821d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/InternalHeaderCheck.inc
@@ -0,0 +1,3 @@
+#ifndef EIGEN_CORE_MODULE_H
+#error "Please include Eigen/plugins instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h
deleted file mode 100644
index a0feef8..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h
+++ /dev/null
@@ -1,152 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-// This file is a base class plugin containing matrix specifics coefficient wise functions.
-
-/** \returns an expression of the Schur product (coefficient wise product) of *this and \a other
-  *
-  * Example: \include MatrixBase_cwiseProduct.cpp
-  * Output: \verbinclude MatrixBase_cwiseProduct.out
-  *
-  * \sa class CwiseBinaryOp, cwiseAbs2
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,product)
-cwiseProduct(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,product)(derived(), other.derived());
-}
-
-/** \returns an expression of the coefficient-wise == operator of *this and \a other
-  *
-  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
-  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
-  * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
-  * isMuchSmallerThan().
-  *
-  * Example: \include MatrixBase_cwiseEqual.cpp
-  * Output: \verbinclude MatrixBase_cwiseEqual.out
-  *
-  * \sa cwiseNotEqual(), isApprox(), isMuchSmallerThan()
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-inline const CwiseBinaryOp<numext::equal_to<Scalar>, const Derived, const OtherDerived>
-cwiseEqual(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return CwiseBinaryOp<numext::equal_to<Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
-}
-
-/** \returns an expression of the coefficient-wise != operator of *this and \a other
-  *
-  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
-  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
-  * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
-  * isMuchSmallerThan().
-  *
-  * Example: \include MatrixBase_cwiseNotEqual.cpp
-  * Output: \verbinclude MatrixBase_cwiseNotEqual.out
-  *
-  * \sa cwiseEqual(), isApprox(), isMuchSmallerThan()
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-inline const CwiseBinaryOp<numext::not_equal_to<Scalar>, const Derived, const OtherDerived>
-cwiseNotEqual(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return CwiseBinaryOp<numext::not_equal_to<Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
-}
-
-/** \returns an expression of the coefficient-wise min of *this and \a other
-  *
-  * Example: \include MatrixBase_cwiseMin.cpp
-  * Output: \verbinclude MatrixBase_cwiseMin.out
-  *
-  * \sa class CwiseBinaryOp, max()
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_min_op<Scalar,Scalar>, const Derived, const OtherDerived>
-cwiseMin(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return CwiseBinaryOp<internal::scalar_min_op<Scalar,Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
-}
-
-/** \returns an expression of the coefficient-wise min of *this and scalar \a other
-  *
-  * \sa class CwiseBinaryOp, min()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_min_op<Scalar,Scalar>, const Derived, const ConstantReturnType>
-cwiseMin(const Scalar &other) const
-{
-  return cwiseMin(Derived::Constant(rows(), cols(), other));
-}
-
-/** \returns an expression of the coefficient-wise max of *this and \a other
-  *
-  * Example: \include MatrixBase_cwiseMax.cpp
-  * Output: \verbinclude MatrixBase_cwiseMax.out
-  *
-  * \sa class CwiseBinaryOp, min()
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_max_op<Scalar,Scalar>, const Derived, const OtherDerived>
-cwiseMax(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return CwiseBinaryOp<internal::scalar_max_op<Scalar,Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
-}
-
-/** \returns an expression of the coefficient-wise max of *this and scalar \a other
-  *
-  * \sa class CwiseBinaryOp, min()
-  */
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_max_op<Scalar,Scalar>, const Derived, const ConstantReturnType>
-cwiseMax(const Scalar &other) const
-{
-  return cwiseMax(Derived::Constant(rows(), cols(), other));
-}
-
-
-/** \returns an expression of the coefficient-wise quotient of *this and \a other
-  *
-  * Example: \include MatrixBase_cwiseQuotient.cpp
-  * Output: \verbinclude MatrixBase_cwiseQuotient.out
-  *
-  * \sa class CwiseBinaryOp, cwiseProduct(), cwiseInverse()
-  */
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_quotient_op<Scalar>, const Derived, const OtherDerived>
-cwiseQuotient(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
-{
-  return CwiseBinaryOp<internal::scalar_quotient_op<Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
-}
-
-typedef CwiseBinaryOp<internal::scalar_cmp_op<Scalar,Scalar,internal::cmp_EQ>, const Derived, const ConstantReturnType> CwiseScalarEqualReturnType;
-
-/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s
-  *
-  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
-  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
-  * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
-  * isMuchSmallerThan().
-  *
-  * \sa cwiseEqual(const MatrixBase<OtherDerived> &) const
-  */
-EIGEN_DEVICE_FUNC
-inline const CwiseScalarEqualReturnType
-cwiseEqual(const Scalar& s) const
-{
-  return CwiseScalarEqualReturnType(derived(), Derived::Constant(rows(), cols(), s), internal::scalar_cmp_op<Scalar,Scalar,internal::cmp_EQ>());
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.inc
new file mode 100644
index 0000000..fae92d8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.inc
@@ -0,0 +1,331 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+// This file is a base class plugin containing matrix specifics coefficient wise functions.
+
+/** \returns an expression of the Schur product (coefficient wise product) of *this and \a other
+ *
+ * Example: \include MatrixBase_cwiseProduct.cpp
+ * Output: \verbinclude MatrixBase_cwiseProduct.out
+ *
+ * \sa class CwiseBinaryOp, cwiseAbs2
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(Derived, OtherDerived, product)
+    cwiseProduct(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived, OtherDerived, product)(derived(), other.derived());
+}
+
+template <typename OtherDerived>
+using CwiseBinaryEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_EQ>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryNotEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_NEQ>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryLessReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LT>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryGreaterReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GT>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryLessOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LE>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryGreaterOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GE>, const Derived, const OtherDerived>;
+
+/** \returns an expression of the coefficient-wise == operator of *this and \a other
+ *
+ * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+ * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+ * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
+ * isMuchSmallerThan().
+ *
+ * Example: \include MatrixBase_cwiseEqual.cpp
+ * Output: \verbinclude MatrixBase_cwiseEqual.out
+ *
+ * \sa cwiseNotEqual(), isApprox(), isMuchSmallerThan()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryEqualReturnType<OtherDerived> cwiseEqual(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise != operator of *this and \a other
+ *
+ * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+ * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+ * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
+ * isMuchSmallerThan().
+ *
+ * Example: \include MatrixBase_cwiseNotEqual.cpp
+ * Output: \verbinclude MatrixBase_cwiseNotEqual.out
+ *
+ * \sa cwiseEqual(), isApprox(), isMuchSmallerThan()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryNotEqualReturnType<OtherDerived> cwiseNotEqual(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryNotEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise < operator of *this and \a other */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryLessReturnType<OtherDerived> cwiseLess(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryLessReturnType<OtherDerived>(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise > operator of *this and \a other */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryGreaterReturnType<OtherDerived> cwiseGreater(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryGreaterReturnType<OtherDerived>(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise <= operator of *this and \a other */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryLessOrEqualReturnType<OtherDerived> cwiseLessOrEqual(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryLessOrEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise >= operator of *this and \a other */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC inline const CwiseBinaryGreaterOrEqualReturnType<OtherDerived> cwiseGreaterOrEqual(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryGreaterOrEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise min of *this and \a other
+ *
+ * Example: \include MatrixBase_cwiseMin.cpp
+ * Output: \verbinclude MatrixBase_cwiseMin.out
+ *
+ * \sa class CwiseBinaryOp, max()
+ */
+template <int NaNPropagation = PropagateFast, typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_min_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>
+    cwiseMin(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_min_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>(
+      derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise min of *this and scalar \a other
+ *
+ * \sa class CwiseBinaryOp, min()
+ */
+template <int NaNPropagation = PropagateFast>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_min_op<Scalar, Scalar, NaNPropagation>, const Derived, const ConstantReturnType>
+    cwiseMin(const Scalar& other) const {
+  return cwiseMin<NaNPropagation>(Derived::Constant(rows(), cols(), other));
+}
+
+/** \returns an expression of the coefficient-wise max of *this and \a other
+ *
+ * Example: \include MatrixBase_cwiseMax.cpp
+ * Output: \verbinclude MatrixBase_cwiseMax.out
+ *
+ * \sa class CwiseBinaryOp, min()
+ */
+template <int NaNPropagation = PropagateFast, typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_max_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>
+    cwiseMax(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_max_op<Scalar, Scalar, NaNPropagation>, const Derived, const OtherDerived>(
+      derived(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise max of *this and scalar \a other
+ *
+ * \sa class CwiseBinaryOp, min()
+ */
+template <int NaNPropagation = PropagateFast>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_max_op<Scalar, Scalar, NaNPropagation>, const Derived, const ConstantReturnType>
+    cwiseMax(const Scalar& other) const {
+  return cwiseMax<NaNPropagation>(Derived::Constant(rows(), cols(), other));
+}
+
+/** \returns an expression of the coefficient-wise quotient of *this and \a other
+ *
+ * Example: \include MatrixBase_cwiseQuotient.cpp
+ * Output: \verbinclude MatrixBase_cwiseQuotient.out
+ *
+ * \sa class CwiseBinaryOp, cwiseProduct(), cwiseInverse()
+ */
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const
+    CwiseBinaryOp<internal::scalar_quotient_op<Scalar>, const Derived, const OtherDerived>
+    cwiseQuotient(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryOp<internal::scalar_quotient_op<Scalar>, const Derived, const OtherDerived>(derived(),
+                                                                                                other.derived());
+}
+
+using CwiseScalarEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_EQ>, const Derived, const ConstantReturnType>;
+using CwiseScalarNotEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_NEQ>, const Derived, const ConstantReturnType>;
+using CwiseScalarLessReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LT>, const Derived, const ConstantReturnType>;
+using CwiseScalarGreaterReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GT>, const Derived, const ConstantReturnType>;
+using CwiseScalarLessOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LE>, const Derived, const ConstantReturnType>;
+using CwiseScalarGreaterOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GE>, const Derived, const ConstantReturnType>;
+
+/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s
+ *
+ * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+ * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+ * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
+ * isMuchSmallerThan().
+ *
+ * \sa cwiseEqual(const MatrixBase<OtherDerived> &) const
+ */
+EIGEN_DEVICE_FUNC inline const CwiseScalarEqualReturnType cwiseEqual(const Scalar& s) const {
+  return CwiseScalarEqualReturnType(derived(), Derived::Constant(rows(), cols(), s));
+}
+
+/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s
+ *
+ * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+ * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+ * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
+ * isMuchSmallerThan().
+ *
+ * \sa cwiseEqual(const MatrixBase<OtherDerived> &) const
+ */
+EIGEN_DEVICE_FUNC inline const CwiseScalarNotEqualReturnType cwiseNotEqual(const Scalar& s) const {
+  return CwiseScalarNotEqualReturnType(derived(), Derived::Constant(rows(), cols(), s));
+}
+
+/** \returns an expression of the coefficient-wise < operator of \c *this and a scalar \a s */
+EIGEN_DEVICE_FUNC inline const CwiseScalarLessReturnType cwiseLess(const Scalar& s) const {
+  return CwiseScalarLessReturnType(derived(), Derived::Constant(rows(), cols(), s));
+}
+
+/** \returns an expression of the coefficient-wise > operator of \c *this and a scalar \a s */
+EIGEN_DEVICE_FUNC inline const CwiseScalarGreaterReturnType cwiseGreater(const Scalar& s) const {
+  return CwiseScalarGreaterReturnType(derived(), Derived::Constant(rows(), cols(), s));
+}
+
+/** \returns an expression of the coefficient-wise <= operator of \c *this and a scalar \a s */
+EIGEN_DEVICE_FUNC inline const CwiseScalarLessOrEqualReturnType cwiseLessOrEqual(const Scalar& s) const {
+  return CwiseScalarLessOrEqualReturnType(derived(), Derived::Constant(rows(), cols(), s));
+}
+
+/** \returns an expression of the coefficient-wise >= operator of \c *this and a scalar \a s */
+EIGEN_DEVICE_FUNC inline const CwiseScalarGreaterOrEqualReturnType cwiseGreaterOrEqual(const Scalar& s) const {
+  return CwiseScalarGreaterOrEqualReturnType(derived(), Derived::Constant(rows(), cols(), s));
+}
+
+template <typename OtherDerived>
+using CwiseBinaryTypedEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_EQ, true>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryTypedNotEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_NEQ, true>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryTypedLessReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LT, true>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryTypedGreaterReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GT, true>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryTypedLessOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LE, true>, const Derived, const OtherDerived>;
+template <typename OtherDerived>
+using CwiseBinaryTypedGreaterOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GE, true>, const Derived, const OtherDerived>;
+
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryTypedEqualReturnType<OtherDerived> cwiseTypedEqual(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryTypedEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryTypedNotEqualReturnType<OtherDerived> cwiseTypedNotEqual(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryTypedNotEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryTypedLessReturnType<OtherDerived> cwiseTypedLess(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryTypedLessReturnType<OtherDerived>(derived(), other.derived());
+}
+
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryTypedGreaterReturnType<OtherDerived> cwiseTypedGreater(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryTypedGreaterReturnType<OtherDerived>(derived(), other.derived());
+}
+
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryTypedLessOrEqualReturnType<OtherDerived> cwiseTypedLessOrEqual(
+    const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryTypedLessOrEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryTypedGreaterOrEqualReturnType<OtherDerived>
+cwiseTypedGreaterOrEqual(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived>& other) const {
+  return CwiseBinaryTypedGreaterOrEqualReturnType<OtherDerived>(derived(), other.derived());
+}
+
+using CwiseScalarTypedEqualReturnType = CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_EQ, true>,
+                                                      const Derived, const ConstantReturnType>;
+using CwiseScalarTypedNotEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_NEQ, true>, const Derived,
+                  const ConstantReturnType>;
+using CwiseScalarTypedLessReturnType = CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LT, true>,
+                                                     const Derived, const ConstantReturnType>;
+using CwiseScalarTypedGreaterReturnType = CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GT, true>,
+                                                        const Derived, const ConstantReturnType>;
+using CwiseScalarTypedLessOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_LE, true>, const Derived,
+                  const ConstantReturnType>;
+using CwiseScalarTypedGreaterOrEqualReturnType =
+    CwiseBinaryOp<internal::scalar_cmp_op<Scalar, Scalar, internal::cmp_GE, true>, const Derived,
+                  const ConstantReturnType>;
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseScalarTypedEqualReturnType cwiseTypedEqual(const Scalar& s) const {
+  return CwiseScalarTypedEqualReturnType(derived(), ConstantReturnType(rows(), cols(), s));
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseScalarTypedNotEqualReturnType
+cwiseTypedNotEqual(const Scalar& s) const {
+  return CwiseScalarTypedNotEqualReturnType(derived(), ConstantReturnType(rows(), cols(), s));
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseScalarTypedLessReturnType cwiseTypedLess(const Scalar& s) const {
+  return CwiseScalarTypedLessReturnType(derived(), ConstantReturnType(rows(), cols(), s));
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseScalarTypedGreaterReturnType cwiseTypedGreater(const Scalar& s) const {
+  return CwiseScalarTypedGreaterReturnType(derived(), ConstantReturnType(rows(), cols(), s));
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseScalarTypedLessOrEqualReturnType
+cwiseTypedLessOrEqual(const Scalar& s) const {
+  return CwiseScalarTypedLessOrEqualReturnType(derived(), ConstantReturnType(rows(), cols(), s));
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseScalarTypedGreaterOrEqualReturnType
+cwiseTypedGreaterOrEqual(const Scalar& s) const {
+  return CwiseScalarTypedGreaterOrEqualReturnType(derived(), ConstantReturnType(rows(), cols(), s));
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h
deleted file mode 100644
index 0514d8f..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h
+++ /dev/null
@@ -1,95 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-// This file is included into the body of the base classes supporting matrix specific coefficient-wise functions.
-// This include MatrixBase and SparseMatrixBase.
-
-
-typedef CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived> CwiseAbsReturnType;
-typedef CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const Derived> CwiseAbs2ReturnType;
-typedef CwiseUnaryOp<internal::scalar_arg_op<Scalar>, const Derived> CwiseArgReturnType;
-typedef CwiseUnaryOp<internal::scalar_sqrt_op<Scalar>, const Derived> CwiseSqrtReturnType;
-typedef CwiseUnaryOp<internal::scalar_sign_op<Scalar>, const Derived> CwiseSignReturnType;
-typedef CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const Derived> CwiseInverseReturnType;
-
-/// \returns an expression of the coefficient-wise absolute value of \c *this
-///
-/// Example: \include MatrixBase_cwiseAbs.cpp
-/// Output: \verbinclude MatrixBase_cwiseAbs.out
-///
-EIGEN_DOC_UNARY_ADDONS(cwiseAbs,absolute value)
-///
-/// \sa cwiseAbs2()
-///
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseAbsReturnType
-cwiseAbs() const { return CwiseAbsReturnType(derived()); }
-
-/// \returns an expression of the coefficient-wise squared absolute value of \c *this
-///
-/// Example: \include MatrixBase_cwiseAbs2.cpp
-/// Output: \verbinclude MatrixBase_cwiseAbs2.out
-///
-EIGEN_DOC_UNARY_ADDONS(cwiseAbs2,squared absolute value)
-///
-/// \sa cwiseAbs()
-///
-EIGEN_DEVICE_FUNC
-EIGEN_STRONG_INLINE const CwiseAbs2ReturnType
-cwiseAbs2() const { return CwiseAbs2ReturnType(derived()); }
-
-/// \returns an expression of the coefficient-wise square root of *this.
-///
-/// Example: \include MatrixBase_cwiseSqrt.cpp
-/// Output: \verbinclude MatrixBase_cwiseSqrt.out
-///
-EIGEN_DOC_UNARY_ADDONS(cwiseSqrt,square-root)
-///
-/// \sa cwisePow(), cwiseSquare()
-///
-EIGEN_DEVICE_FUNC
-inline const CwiseSqrtReturnType
-cwiseSqrt() const { return CwiseSqrtReturnType(derived()); }
-
-/// \returns an expression of the coefficient-wise signum of *this.
-///
-/// Example: \include MatrixBase_cwiseSign.cpp
-/// Output: \verbinclude MatrixBase_cwiseSign.out
-///
-EIGEN_DOC_UNARY_ADDONS(cwiseSign,sign function)
-///
-EIGEN_DEVICE_FUNC
-inline const CwiseSignReturnType
-cwiseSign() const { return CwiseSignReturnType(derived()); }
-
-
-/// \returns an expression of the coefficient-wise inverse of *this.
-///
-/// Example: \include MatrixBase_cwiseInverse.cpp
-/// Output: \verbinclude MatrixBase_cwiseInverse.out
-///
-EIGEN_DOC_UNARY_ADDONS(cwiseInverse,inverse)
-///
-/// \sa cwiseProduct()
-///
-EIGEN_DEVICE_FUNC
-inline const CwiseInverseReturnType
-cwiseInverse() const { return CwiseInverseReturnType(derived()); }
-
-/// \returns an expression of the coefficient-wise phase angle of \c *this
-///
-/// Example: \include MatrixBase_cwiseArg.cpp
-/// Output: \verbinclude MatrixBase_cwiseArg.out
-///
-EIGEN_DOC_UNARY_ADDONS(cwiseArg,arg)
-
-EIGEN_DEVICE_FUNC
-inline const CwiseArgReturnType
-cwiseArg() const { return CwiseArgReturnType(derived()); }
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc
new file mode 100644
index 0000000..b23f4a5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc
@@ -0,0 +1,112 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+// This file is included into the body of the base classes supporting matrix specific coefficient-wise functions.
+// This include MatrixBase and SparseMatrixBase.
+
+typedef CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived> CwiseAbsReturnType;
+typedef CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const Derived> CwiseAbs2ReturnType;
+typedef CwiseUnaryOp<internal::scalar_arg_op<Scalar>, const Derived> CwiseArgReturnType;
+typedef CwiseUnaryOp<internal::scalar_carg_op<Scalar>, const Derived> CwiseCArgReturnType;
+typedef CwiseUnaryOp<internal::scalar_sqrt_op<Scalar>, const Derived> CwiseSqrtReturnType;
+typedef CwiseUnaryOp<internal::scalar_cbrt_op<Scalar>, const Derived> CwiseCbrtReturnType;
+typedef CwiseUnaryOp<internal::scalar_sign_op<Scalar>, const Derived> CwiseSignReturnType;
+typedef CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const Derived> CwiseInverseReturnType;
+
+/// \returns an expression of the coefficient-wise absolute value of \c *this
+///
+/// Example: \include MatrixBase_cwiseAbs.cpp
+/// Output: \verbinclude MatrixBase_cwiseAbs.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseAbs, absolute value)
+///
+/// \sa cwiseAbs2()
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseAbsReturnType cwiseAbs() const {
+  return CwiseAbsReturnType(derived());
+}
+
+/// \returns an expression of the coefficient-wise squared absolute value of \c *this
+///
+/// Example: \include MatrixBase_cwiseAbs2.cpp
+/// Output: \verbinclude MatrixBase_cwiseAbs2.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseAbs2, squared absolute value)
+///
+/// \sa cwiseAbs()
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseAbs2ReturnType cwiseAbs2() const {
+  return CwiseAbs2ReturnType(derived());
+}
+
+/// \returns an expression of the coefficient-wise square root of *this.
+///
+/// Example: \include MatrixBase_cwiseSqrt.cpp
+/// Output: \verbinclude MatrixBase_cwiseSqrt.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseSqrt, square - root)
+///
+/// \sa cwisePow(), cwiseSquare(), cwiseCbrt()
+///
+EIGEN_DEVICE_FUNC inline const CwiseSqrtReturnType cwiseSqrt() const { return CwiseSqrtReturnType(derived()); }
+
+/// \returns an expression of the coefficient-wise cube root of *this.
+///
+/// Example: \include MatrixBase_cwiseCbrt.cpp
+/// Output: \verbinclude MatrixBase_cwiseCbrt.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseCbrt, cube - root)
+///
+/// \sa cwiseSqrt(), cwiseSquare(), cwisePow()
+///
+EIGEN_DEVICE_FUNC inline const CwiseCbrtReturnType cwiseCbrt() const { return CwiseSCbrtReturnType(derived()); }
+
+/// \returns an expression of the coefficient-wise signum of *this.
+///
+/// Example: \include MatrixBase_cwiseSign.cpp
+/// Output: \verbinclude MatrixBase_cwiseSign.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseSign, sign function)
+///
+EIGEN_DEVICE_FUNC inline const CwiseSignReturnType cwiseSign() const { return CwiseSignReturnType(derived()); }
+
+/// \returns an expression of the coefficient-wise inverse of *this.
+///
+/// Example: \include MatrixBase_cwiseInverse.cpp
+/// Output: \verbinclude MatrixBase_cwiseInverse.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseInverse, inverse)
+///
+/// \sa cwiseProduct()
+///
+EIGEN_DEVICE_FUNC inline const CwiseInverseReturnType cwiseInverse() const { return CwiseInverseReturnType(derived()); }
+
+/// \returns an expression of the coefficient-wise phase angle of \c *this
+///
+/// Example: \include MatrixBase_cwiseArg.cpp
+/// Output: \verbinclude MatrixBase_cwiseArg.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseArg, arg)
+
+EIGEN_DEVICE_FUNC inline const CwiseArgReturnType cwiseArg() const { return CwiseArgReturnType(derived()); }
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseCArgReturnType cwiseCArg() const {
+  return CwiseCArgReturnType(derived());
+}
+
+template <typename ScalarExponent>
+using CwisePowReturnType =
+    std::enable_if_t<internal::is_arithmetic<typename NumTraits<ScalarExponent>::Real>::value,
+                     CwiseUnaryOp<internal::scalar_unary_pow_op<Scalar, ScalarExponent>, const Derived>>;
+
+template <typename ScalarExponent>
+EIGEN_DEVICE_FUNC inline const CwisePowReturnType<ScalarExponent> cwisePow(const ScalarExponent& exponent) const {
+  return CwisePowReturnType<ScalarExponent>(derived(), internal::scalar_unary_pow_op<Scalar, ScalarExponent>(exponent));
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.h
deleted file mode 100644
index 482a6b0..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.h
+++ /dev/null
@@ -1,149 +0,0 @@
-
-#ifdef EIGEN_PARSED_BY_DOXYGEN
-
-/// \returns an expression of \c *this with reshaped sizes.
-///
-/// \param nRows the number of rows in the reshaped expression, specified at either run-time or compile-time, or AutoSize
-/// \param nCols the number of columns in the reshaped expression, specified at either run-time or compile-time, or AutoSize
-/// \tparam Order specifies whether the coefficients should be processed in column-major-order (ColMajor), in row-major-order (RowMajor),
-///               or follows the \em natural order of the nested expression (AutoOrder). The default is ColMajor.
-/// \tparam NRowsType the type of the value handling the number of rows, typically Index.
-/// \tparam NColsType the type of the value handling the number of columns, typically Index.
-///
-/// Dynamic size example: \include MatrixBase_reshaped_int_int.cpp
-/// Output: \verbinclude MatrixBase_reshaped_int_int.out
-///
-/// The number of rows \a nRows and columns \a nCols can also be specified at compile-time by passing Eigen::fix<N>,
-/// or Eigen::fix<N>(n) as arguments. In the later case, \c n plays the role of a runtime fallback value in case \c N equals Eigen::Dynamic.
-/// Here is an example with a fixed number of rows and columns:
-/// \include MatrixBase_reshaped_fixed.cpp
-/// Output: \verbinclude MatrixBase_reshaped_fixed.out
-///
-/// Finally, one of the sizes parameter can be automatically deduced from the other one by passing AutoSize as in the following example:
-/// \include MatrixBase_reshaped_auto.cpp
-/// Output: \verbinclude MatrixBase_reshaped_auto.out
-/// AutoSize does preserve compile-time sizes when possible, i.e., when the sizes of the input are known at compile time \b and
-/// that the other size is passed at compile-time using Eigen::fix<N> as above.
-///
-/// \sa class Reshaped, fix, fix<N>(int)
-///
-template<int Order = ColMajor, typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC
-inline Reshaped<Derived,...>
-reshaped(NRowsType nRows, NColsType nCols);
-
-/// This is the const version of reshaped(NRowsType,NColsType).
-template<int Order = ColMajor, typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC
-inline const Reshaped<const Derived,...>
-reshaped(NRowsType nRows, NColsType nCols) const;
-
-/// \returns an expression of \c *this with columns (or rows) stacked to a linear column vector
-///
-/// \tparam Order specifies whether the coefficients should be processed in column-major-order (ColMajor), in row-major-order (RowMajor),
-///               or follows the \em natural order of the nested expression (AutoOrder). The default is ColMajor.
-///
-/// This overloads is essentially a shortcut for `A.reshaped<Order>(AutoSize,fix<1>)`.
-///
-/// - If `Order==ColMajor` (the default), then it returns a column-vector from the stacked columns of \c *this.
-/// - If `Order==RowMajor`, then it returns a column-vector from the stacked rows of \c *this.
-/// - If `Order==AutoOrder`, then it returns a column-vector with elements stacked following the storage order of \c *this.
-///   This mode is the recommended one when the particular ordering of the element is not relevant.
-///
-/// Example:
-/// \include MatrixBase_reshaped_to_vector.cpp
-/// Output: \verbinclude MatrixBase_reshaped_to_vector.out
-///
-/// If you want more control, you can still fall back to reshaped(NRowsType,NColsType).
-///
-/// \sa reshaped(NRowsType,NColsType), class Reshaped
-///
-template<int Order = ColMajor>
-EIGEN_DEVICE_FUNC
-inline Reshaped<Derived,...>
-reshaped();
-
-/// This is the const version of reshaped().
-template<int Order = ColMajor>
-EIGEN_DEVICE_FUNC
-inline const Reshaped<const Derived,...>
-reshaped() const;
-
-#else
-
-// This file is automatically included twice to generate const and non-const versions
-
-#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
-#define EIGEN_RESHAPED_METHOD_CONST const
-#else
-#define EIGEN_RESHAPED_METHOD_CONST
-#endif
-
-#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
-
-// This part is included once
-
-#endif
-
-template<typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC
-inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
-                internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
-                internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value>
-reshaped(NRowsType nRows, NColsType nCols) EIGEN_RESHAPED_METHOD_CONST
-{
-  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
-                  internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
-                  internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value>
-                (derived(),
-                 internal::get_runtime_reshape_size(nRows,internal::get_runtime_value(nCols),size()),
-                 internal::get_runtime_reshape_size(nCols,internal::get_runtime_value(nRows),size()));
-}
-
-template<int Order, typename NRowsType, typename NColsType>
-EIGEN_DEVICE_FUNC
-inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
-                internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
-                internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value,
-                internal::get_compiletime_reshape_order<Flags,Order>::value>
-reshaped(NRowsType nRows, NColsType nCols) EIGEN_RESHAPED_METHOD_CONST
-{
-  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
-                  internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
-                  internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value,
-                  internal::get_compiletime_reshape_order<Flags,Order>::value>
-                (derived(),
-                 internal::get_runtime_reshape_size(nRows,internal::get_runtime_value(nCols),size()),
-                 internal::get_runtime_reshape_size(nCols,internal::get_runtime_value(nRows),size()));
-}
-
-// Views as linear vectors
-
-EIGEN_DEVICE_FUNC
-inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,SizeAtCompileTime,1>
-reshaped() EIGEN_RESHAPED_METHOD_CONST
-{
-  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,SizeAtCompileTime,1>(derived(),size(),1);
-}
-
-template<int Order>
-EIGEN_DEVICE_FUNC
-inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1,
-                internal::get_compiletime_reshape_order<Flags,Order>::value>
-reshaped() EIGEN_RESHAPED_METHOD_CONST
-{
-  EIGEN_STATIC_ASSERT(Order==RowMajor || Order==ColMajor || Order==AutoOrder, INVALID_TEMPLATE_PARAMETER);
-  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1,
-                  internal::get_compiletime_reshape_order<Flags,Order>::value>
-                (derived(), size(), 1);
-}
-
-#undef EIGEN_RESHAPED_METHOD_CONST
-
-#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
-#define EIGEN_RESHAPED_METHOD_2ND_PASS
-#include "ReshapedMethods.h"
-#undef EIGEN_RESHAPED_METHOD_2ND_PASS
-#endif
-
-#endif // EIGEN_PARSED_BY_DOXYGEN
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.inc b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.inc
new file mode 100644
index 0000000..c1f90e7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.inc
@@ -0,0 +1,133 @@
+
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+
+/// \returns an expression of \c *this with reshaped sizes.
+///
+/// \param nRows the number of rows in the reshaped expression, specified at either run-time or compile-time, or
+/// AutoSize \param nCols the number of columns in the reshaped expression, specified at either run-time or
+/// compile-time, or AutoSize \tparam Order specifies whether the coefficients should be processed in column-major-order
+/// (ColMajor), in row-major-order (RowMajor),
+///               or follows the \em natural order of the nested expression (AutoOrder). The default is ColMajor.
+/// \tparam NRowsType the type of the value handling the number of rows, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns, typically Index.
+///
+/// Dynamic size example: \include MatrixBase_reshaped_int_int.cpp
+/// Output: \verbinclude MatrixBase_reshaped_int_int.out
+///
+/// The number of rows \a nRows and columns \a nCols can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments. In the later case, \c n plays the role of a runtime fallback value in case \c N
+/// equals Eigen::Dynamic. Here is an example with a fixed number of rows and columns: \include
+/// MatrixBase_reshaped_fixed.cpp Output: \verbinclude MatrixBase_reshaped_fixed.out
+///
+/// Finally, one of the sizes parameter can be automatically deduced from the other one by passing AutoSize as in the
+/// following example: \include MatrixBase_reshaped_auto.cpp Output: \verbinclude MatrixBase_reshaped_auto.out AutoSize
+/// does preserve compile-time sizes when possible, i.e., when the sizes of the input are known at compile time \b and
+/// that the other size is passed at compile-time using Eigen::fix<N> as above.
+///
+/// \sa class Reshaped, fix, fix<N>(int)
+///
+template <int Order = ColMajor, typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC inline Reshaped<Derived, ...> reshaped(NRowsType nRows, NColsType nCols);
+
+/// This is the const version of reshaped(NRowsType,NColsType).
+template <int Order = ColMajor, typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC inline const Reshaped<const Derived, ...> reshaped(NRowsType nRows, NColsType nCols) const;
+
+/// \returns an expression of \c *this with columns (or rows) stacked to a linear column vector
+///
+/// \tparam Order specifies whether the coefficients should be processed in column-major-order (ColMajor), in
+/// row-major-order (RowMajor),
+///               or follows the \em natural order of the nested expression (AutoOrder). The default is ColMajor.
+///
+/// This overloads is essentially a shortcut for `A.reshaped<Order>(AutoSize,fix<1>)`.
+///
+/// - If `Order==ColMajor` (the default), then it returns a column-vector from the stacked columns of \c *this.
+/// - If `Order==RowMajor`, then it returns a column-vector from the stacked rows of \c *this.
+/// - If `Order==AutoOrder`, then it returns a column-vector with elements stacked following the storage order of \c
+/// *this.
+///   This mode is the recommended one when the particular ordering of the element is not relevant.
+///
+/// Example:
+/// \include MatrixBase_reshaped_to_vector.cpp
+/// Output: \verbinclude MatrixBase_reshaped_to_vector.out
+///
+/// If you want more control, you can still fall back to reshaped(NRowsType,NColsType).
+///
+/// \sa reshaped(NRowsType,NColsType), class Reshaped
+///
+template <int Order = ColMajor>
+EIGEN_DEVICE_FUNC inline Reshaped<Derived, ...> reshaped();
+
+/// This is the const version of reshaped().
+template <int Order = ColMajor>
+EIGEN_DEVICE_FUNC inline const Reshaped<const Derived, ...> reshaped() const;
+
+#else
+
+// This file is automatically included twice to generate const and non-const versions
+
+#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
+#define EIGEN_RESHAPED_METHOD_CONST const
+#else
+#define EIGEN_RESHAPED_METHOD_CONST
+#endif
+
+#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
+
+// This part is included once
+
+#endif
+
+template <typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC inline Reshaped<
+    EIGEN_RESHAPED_METHOD_CONST Derived,
+    internal::get_compiletime_reshape_size<NRowsType, NColsType, SizeAtCompileTime>::value,
+    internal::get_compiletime_reshape_size<NColsType, NRowsType, SizeAtCompileTime>::value>
+reshaped(NRowsType nRows, NColsType nCols) EIGEN_RESHAPED_METHOD_CONST {
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
+                  internal::get_compiletime_reshape_size<NRowsType, NColsType, SizeAtCompileTime>::value,
+                  internal::get_compiletime_reshape_size<NColsType, NRowsType, SizeAtCompileTime>::value>(
+      derived(), internal::get_runtime_reshape_size(nRows, internal::get_runtime_value(nCols), size()),
+      internal::get_runtime_reshape_size(nCols, internal::get_runtime_value(nRows), size()));
+}
+
+template <int Order, typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC inline Reshaped<
+    EIGEN_RESHAPED_METHOD_CONST Derived,
+    internal::get_compiletime_reshape_size<NRowsType, NColsType, SizeAtCompileTime>::value,
+    internal::get_compiletime_reshape_size<NColsType, NRowsType, SizeAtCompileTime>::value,
+    internal::get_compiletime_reshape_order(Flags, Order)>
+reshaped(NRowsType nRows, NColsType nCols) EIGEN_RESHAPED_METHOD_CONST {
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
+                  internal::get_compiletime_reshape_size<NRowsType, NColsType, SizeAtCompileTime>::value,
+                  internal::get_compiletime_reshape_size<NColsType, NRowsType, SizeAtCompileTime>::value,
+                  internal::get_compiletime_reshape_order(Flags, Order)>(
+      derived(), internal::get_runtime_reshape_size(nRows, internal::get_runtime_value(nCols), size()),
+      internal::get_runtime_reshape_size(nCols, internal::get_runtime_value(nRows), size()));
+}
+
+// Views as linear vectors
+
+EIGEN_DEVICE_FUNC inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1> reshaped()
+    EIGEN_RESHAPED_METHOD_CONST {
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1>(derived(), size(), 1);
+}
+
+template <int Order>
+EIGEN_DEVICE_FUNC inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1,
+                                  internal::get_compiletime_reshape_order(Flags, Order)>
+reshaped() EIGEN_RESHAPED_METHOD_CONST {
+  EIGEN_STATIC_ASSERT(Order == RowMajor || Order == ColMajor || Order == AutoOrder, INVALID_TEMPLATE_PARAMETER);
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1,
+                  internal::get_compiletime_reshape_order(Flags, Order)>(derived(), size(), 1);
+}
+
+#undef EIGEN_RESHAPED_METHOD_CONST
+
+#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
+#define EIGEN_RESHAPED_METHOD_2ND_PASS
+#include "ReshapedMethods.inc"
+#undef EIGEN_RESHAPED_METHOD_2ND_PASS
+#endif
+
+#endif  // EIGEN_PARSED_BY_DOXYGEN
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions
index 20c23d1..845ee0b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions
@@ -8,8 +8,8 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-#ifndef EIGEN_MATRIX_FUNCTIONS
-#define EIGEN_MATRIX_FUNCTIONS
+#ifndef EIGEN_MATRIX_FUNCTIONS_MODULE_H
+#define EIGEN_MATRIX_FUNCTIONS_MODULE_H
 
 #include <cfloat>
 #include <list>
@@ -19,52 +19,53 @@
 #include "../../Eigen/Eigenvalues"
 
 /**
-  * \defgroup MatrixFunctions_Module Matrix functions module
-  * \brief This module aims to provide various methods for the computation of
-  * matrix functions. 
-  *
-  * To use this module, add 
-  * \code
-  * #include <unsupported/Eigen/MatrixFunctions>
-  * \endcode
-  * at the start of your source file.
-  *
-  * This module defines the following MatrixBase methods.
-  *  - \ref matrixbase_cos "MatrixBase::cos()", for computing the matrix cosine
-  *  - \ref matrixbase_cosh "MatrixBase::cosh()", for computing the matrix hyperbolic cosine
-  *  - \ref matrixbase_exp "MatrixBase::exp()", for computing the matrix exponential
-  *  - \ref matrixbase_log "MatrixBase::log()", for computing the matrix logarithm
-  *  - \ref matrixbase_pow "MatrixBase::pow()", for computing the matrix power
-  *  - \ref matrixbase_matrixfunction "MatrixBase::matrixFunction()", for computing general matrix functions
-  *  - \ref matrixbase_sin "MatrixBase::sin()", for computing the matrix sine
-  *  - \ref matrixbase_sinh "MatrixBase::sinh()", for computing the matrix hyperbolic sine
-  *  - \ref matrixbase_sqrt "MatrixBase::sqrt()", for computing the matrix square root
-  *
-  * These methods are the main entry points to this module. 
-  *
-  * %Matrix functions are defined as follows.  Suppose that \f$ f \f$
-  * is an entire function (that is, a function on the complex plane
-  * that is everywhere complex differentiable).  Then its Taylor
-  * series
-  * \f[ f(0) + f'(0) x + \frac{f''(0)}{2} x^2 + \frac{f'''(0)}{3!} x^3 + \cdots \f]
-  * converges to \f$ f(x) \f$. In this case, we can define the matrix
-  * function by the same series:
-  * \f[ f(M) = f(0) + f'(0) M + \frac{f''(0)}{2} M^2 + \frac{f'''(0)}{3!} M^3 + \cdots \f]
-  *
-  */
+ * \defgroup MatrixFunctions_Module Matrix functions module
+ * \brief This module aims to provide various methods for the computation of
+ * matrix functions.
+ *
+ * To use this module, add
+ * \code
+ * #include <unsupported/Eigen/MatrixFunctions>
+ * \endcode
+ * at the start of your source file.
+ *
+ * This module defines the following MatrixBase methods.
+ *  - \ref matrixbase_cos "MatrixBase::cos()", for computing the matrix cosine
+ *  - \ref matrixbase_cosh "MatrixBase::cosh()", for computing the matrix hyperbolic cosine
+ *  - \ref matrixbase_exp "MatrixBase::exp()", for computing the matrix exponential
+ *  - \ref matrixbase_log "MatrixBase::log()", for computing the matrix logarithm
+ *  - \ref matrixbase_pow "MatrixBase::pow()", for computing the matrix power
+ *  - \ref matrixbase_matrixfunction "MatrixBase::matrixFunction()", for computing general matrix functions
+ *  - \ref matrixbase_sin "MatrixBase::sin()", for computing the matrix sine
+ *  - \ref matrixbase_sinh "MatrixBase::sinh()", for computing the matrix hyperbolic sine
+ *  - \ref matrixbase_sqrt "MatrixBase::sqrt()", for computing the matrix square root
+ *
+ * These methods are the main entry points to this module.
+ *
+ * %Matrix functions are defined as follows.  Suppose that \f$ f \f$
+ * is an entire function (that is, a function on the complex plane
+ * that is everywhere complex differentiable).  Then its Taylor
+ * series
+ * \f[ f(0) + f'(0) x + \frac{f''(0)}{2} x^2 + \frac{f'''(0)}{3!} x^3 + \cdots \f]
+ * converges to \f$ f(x) \f$. In this case, we can define the matrix
+ * function by the same series:
+ * \f[ f(M) = f(0) + f'(0) M + \frac{f''(0)}{2} M^2 + \frac{f'''(0)}{3!} M^3 + \cdots \f]
+ *
+ */
 
 #include "../../Eigen/src/Core/util/DisableStupidWarnings.h"
 
+// IWYU pragma: begin_exports
 #include "src/MatrixFunctions/MatrixExponential.h"
 #include "src/MatrixFunctions/MatrixFunction.h"
 #include "src/MatrixFunctions/MatrixSquareRoot.h"
 #include "src/MatrixFunctions/MatrixLogarithm.h"
 #include "src/MatrixFunctions/MatrixPower.h"
+// IWYU pragma: end_exports
 
 #include "../../Eigen/src/Core/util/ReenableStupidWarnings.h"
 
-
-/** 
+/**
 \page matrixbaseextra_page
 \ingroup MatrixFunctions_Module
 
@@ -180,7 +181,7 @@
 \param[in]  M  invertible matrix whose logarithm is to be computed.
 \returns    expression representing the matrix logarithm root of \p M.
 
-The matrix logarithm of \f$ M \f$ is a matrix \f$ X \f$ such that 
+The matrix logarithm of \f$ M \f$ is a matrix \f$ X \f$ such that
 \f$ \exp(X) = M \f$ where exp denotes the matrix exponential. As for
 the scalar logarithm, the equation \f$ \exp(X) = M \f$ may have
 multiple solutions; this function returns a matrix whose eigenvalues
@@ -207,14 +208,14 @@
 SIAM 2008. ISBN 978-0-898716-46-7.
 
 Example: The following program checks that
-\f[ \log \left[ \begin{array}{ccc} 
+\f[ \log \left[ \begin{array}{ccc}
       \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
       \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
       0 & 0 & 1
     \end{array} \right] = \left[ \begin{array}{ccc}
-      0 & \frac14\pi & 0 \\ 
+      0 & \frac14\pi & 0 \\
       -\frac14\pi & 0 & 0 \\
-      0 & 0 & 0 
+      0 & 0 & 0
     \end{array} \right]. \f]
 This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
 the z-axis. This is the inverse of the example used in the
@@ -226,7 +227,7 @@
 \note \p M has to be a matrix of \c float, \c double, `long
 double`, \c complex<float>, \c complex<double>, or `complex<long double>`.
 
-\sa MatrixBase::exp(), MatrixBase::matrixFunction(), 
+\sa MatrixBase::exp(), MatrixBase::matrixFunction(),
     class MatrixLogarithmAtomic, MatrixBase::sqrt().
 
 
@@ -288,7 +289,7 @@
        0, 0, 6, 7,
        0, 0, 8, 9;
   std::cout << A.pow(0.37) << std::endl;
-  
+
   // The 1 makes eigenvalue 0 non-semisimple.
   A.coeffRef(0, 1) = 1;
 
@@ -341,18 +342,18 @@
 Compute a matrix function.
 
 \code
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
-\endcode
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename
+internal::traits<Derived>::Scalar>::type f) const \endcode
 
 \param[in]  M  argument of matrix function, should be a square matrix.
 \param[in]  f  an entire function; \c f(x,n) should compute the n-th
 derivative of f at x.
 \returns  expression representing \p f applied to \p M.
 
-Suppose that \p M is a matrix whose entries have type \c Scalar. 
+Suppose that \p M is a matrix whose entries have type \c Scalar.
 Then, the second argument, \p f, should be a function with prototype
-\code 
-ComplexScalar f(ComplexScalar, int) 
+\code
+ComplexScalar f(ComplexScalar, int)
 \endcode
 where \c ComplexScalar = \c std::complex<Scalar> if \c Scalar is
 real (e.g., \c float or \c double) and \c ComplexScalar =
@@ -360,17 +361,17 @@
 should be \f$ f^{(n)}(x) \f$, the n-th derivative of f at x.
 
 This routine uses the algorithm described in:
-Philip Davies and Nicholas J. Higham, 
-"A Schur-Parlett algorithm for computing matrix functions", 
+Philip Davies and Nicholas J. Higham,
+"A Schur-Parlett algorithm for computing matrix functions",
 <em>SIAM J. %Matrix Anal. Applic.</em>, <b>25</b>:464&ndash;485, 2003.
 
 The actual work is done by the MatrixFunction class.
 
 Example: The following program checks that
-\f[ \exp \left[ \begin{array}{ccc} 
-      0 & \frac14\pi & 0 \\ 
+\f[ \exp \left[ \begin{array}{ccc}
+      0 & \frac14\pi & 0 \\
       -\frac14\pi & 0 & 0 \\
-      0 & 0 & 0 
+      0 & 0 & 0
     \end{array} \right] = \left[ \begin{array}{ccc}
       \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
       \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
@@ -383,7 +384,7 @@
 \include MatrixFunction.cpp
 Output: \verbinclude MatrixFunction.out
 
-Note that the function \c expfn is defined for complex numbers 
+Note that the function \c expfn is defined for complex numbers
 \c x, even though the matrix \c A is over the reals. Instead of
 \c expfn, we could also have used StdStemFunctions::exp:
 \code
@@ -450,7 +451,7 @@
 it should have no eigenvalues which are real and negative (pairs of
 complex conjugate eigenvalues are allowed). In that case, the matrix
 has a square root which is also real, and this is the square root
-computed by this function. 
+computed by this function.
 
 The matrix square root is computed by first reducing the matrix to
 quasi-triangular form with the real Schur decomposition. The square
@@ -482,12 +483,12 @@
 52/53:127&ndash;140, 1983.
 
 Example: The following program checks that the square root of
-\f[ \left[ \begin{array}{cc} 
+\f[ \left[ \begin{array}{cc}
               \cos(\frac13\pi) & -\sin(\frac13\pi) \\
               \sin(\frac13\pi) & \cos(\frac13\pi)
     \end{array} \right], \f]
 corresponding to a rotation over 60 degrees, is a rotation over 30 degrees:
-\f[ \left[ \begin{array}{cc} 
+\f[ \left[ \begin{array}{cc}
               \cos(\frac16\pi) & -\sin(\frac16\pi) \\
               \sin(\frac16\pi) & \cos(\frac16\pi)
     \end{array} \right]. \f]
@@ -500,5 +501,4 @@
 
 */
 
-#endif // EIGEN_MATRIX_FUNCTIONS
-
+#endif  // EIGEN_MATRIX_FUNCTIONS_MODULE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/InternalHeaderCheck.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/InternalHeaderCheck.h
new file mode 100644
index 0000000..4fc840e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/InternalHeaderCheck.h
@@ -0,0 +1,4 @@
+#ifndef EIGEN_MATRIX_FUNCTIONS_MODULE_H
+#error \
+    "Please include unsupported/Eigen/MatrixFunctions instead of including headers inside the src directory directly."
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
index 02284b0..ff955e1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
@@ -13,6 +13,9 @@
 
 #include "StemFunction.h"
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 namespace internal {
 
@@ -21,21 +24,18 @@
  * This struct is used by CwiseUnaryOp to scale a matrix by \f$ 2^{-s} \f$.
  */
 template <typename RealScalar>
-struct MatrixExponentialScalingOp
-{
+struct MatrixExponentialScalingOp {
   /** \brief Constructor.
    *
    * \param[in] squarings  The integer \f$ s \f$ in this document.
    */
-  MatrixExponentialScalingOp(int squarings) : m_squarings(squarings) { }
-
+  MatrixExponentialScalingOp(int squarings) : m_squarings(squarings) {}
 
   /** \brief Scale a matrix coefficient.
    *
    * \param[in,out] x  The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
    */
-  inline const RealScalar operator() (const RealScalar& x) const
-  {
+  inline const RealScalar operator()(const RealScalar& x) const {
     using std::ldexp;
     return ldexp(x, -m_squarings);
   }
@@ -46,14 +46,13 @@
    *
    * \param[in,out] x  The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
    */
-  inline const ComplexScalar operator() (const ComplexScalar& x) const
-  {
+  inline const ComplexScalar operator()(const ComplexScalar& x) const {
     using std::ldexp;
     return ComplexScalar(ldexp(x.real(), -m_squarings), ldexp(x.imag(), -m_squarings));
   }
 
-  private:
-    int m_squarings;
+ private:
+  int m_squarings;
 };
 
 /** \brief Compute the (3,3)-Pad&eacute; approximant to the exponential.
@@ -62,8 +61,7 @@
  *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
  */
 template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade3(const MatA& A, MatU& U, MatV& V)
-{
+void matrix_exp_pade3(const MatA& A, MatU& U, MatV& V) {
   typedef typename MatA::PlainObject MatrixType;
   typedef typename NumTraits<typename traits<MatA>::Scalar>::Real RealScalar;
   const RealScalar b[] = {120.L, 60.L, 12.L, 1.L};
@@ -79,8 +77,7 @@
  *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
  */
 template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade5(const MatA& A, MatU& U, MatV& V)
-{
+void matrix_exp_pade5(const MatA& A, MatU& U, MatV& V) {
   typedef typename MatA::PlainObject MatrixType;
   typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
   const RealScalar b[] = {30240.L, 15120.L, 3360.L, 420.L, 30.L, 1.L};
@@ -97,19 +94,16 @@
  *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
  */
 template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade7(const MatA& A, MatU& U, MatV& V)
-{
+void matrix_exp_pade7(const MatA& A, MatU& U, MatV& V) {
   typedef typename MatA::PlainObject MatrixType;
   typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
   const RealScalar b[] = {17297280.L, 8648640.L, 1995840.L, 277200.L, 25200.L, 1512.L, 56.L, 1.L};
   const MatrixType A2 = A * A;
   const MatrixType A4 = A2 * A2;
   const MatrixType A6 = A4 * A2;
-  const MatrixType tmp = b[7] * A6 + b[5] * A4 + b[3] * A2 
-    + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  const MatrixType tmp = b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
   U.noalias() = A * tmp;
   V = b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
-
 }
 
 /** \brief Compute the (9,9)-Pad&eacute; approximant to the exponential.
@@ -118,18 +112,17 @@
  *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
  */
 template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade9(const MatA& A, MatU& U, MatV& V)
-{
+void matrix_exp_pade9(const MatA& A, MatU& U, MatV& V) {
   typedef typename MatA::PlainObject MatrixType;
   typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
   const RealScalar b[] = {17643225600.L, 8821612800.L, 2075673600.L, 302702400.L, 30270240.L,
-                          2162160.L, 110880.L, 3960.L, 90.L, 1.L};
+                          2162160.L,     110880.L,     3960.L,       90.L,        1.L};
   const MatrixType A2 = A * A;
   const MatrixType A4 = A2 * A2;
   const MatrixType A6 = A4 * A2;
   const MatrixType A8 = A6 * A2;
-  const MatrixType tmp = b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 
-    + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  const MatrixType tmp =
+      b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
   U.noalias() = A * tmp;
   V = b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
 }
@@ -140,17 +133,27 @@
  *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
  */
 template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade13(const MatA& A, MatU& U, MatV& V)
-{
+void matrix_exp_pade13(const MatA& A, MatU& U, MatV& V) {
   typedef typename MatA::PlainObject MatrixType;
   typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {64764752532480000.L, 32382376266240000.L, 7771770303897600.L,
-                          1187353796428800.L, 129060195264000.L, 10559470521600.L, 670442572800.L,
-                          33522128640.L, 1323241920.L, 40840800.L, 960960.L, 16380.L, 182.L, 1.L};
+  const RealScalar b[] = {64764752532480000.L,
+                          32382376266240000.L,
+                          7771770303897600.L,
+                          1187353796428800.L,
+                          129060195264000.L,
+                          10559470521600.L,
+                          670442572800.L,
+                          33522128640.L,
+                          1323241920.L,
+                          40840800.L,
+                          960960.L,
+                          16380.L,
+                          182.L,
+                          1.L};
   const MatrixType A2 = A * A;
   const MatrixType A4 = A2 * A2;
   const MatrixType A6 = A4 * A2;
-  V = b[13] * A6 + b[11] * A4 + b[9] * A2; // used for temporary storage
+  V = b[13] * A6 + b[11] * A4 + b[9] * A2;  // used for temporary storage
   MatrixType tmp = A6 * V;
   tmp += b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
   U.noalias() = A * tmp;
@@ -168,51 +171,57 @@
  */
 #if LDBL_MANT_DIG > 64
 template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade17(const MatA& A, MatU& U, MatV& V)
-{
+void matrix_exp_pade17(const MatA& A, MatU& U, MatV& V) {
   typedef typename MatA::PlainObject MatrixType;
   typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {830034394580628357120000.L, 415017197290314178560000.L,
-                          100610229646136770560000.L, 15720348382208870400000.L,
-                          1774878043152614400000.L, 153822763739893248000.L, 10608466464820224000.L,
-                          595373117923584000.L, 27563570274240000.L, 1060137318240000.L,
-                          33924394183680.L, 899510451840.L, 19554575040.L, 341863200.L, 4651200.L,
-                          46512.L, 306.L, 1.L};
+  const RealScalar b[] = {830034394580628357120000.L,
+                          415017197290314178560000.L,
+                          100610229646136770560000.L,
+                          15720348382208870400000.L,
+                          1774878043152614400000.L,
+                          153822763739893248000.L,
+                          10608466464820224000.L,
+                          595373117923584000.L,
+                          27563570274240000.L,
+                          1060137318240000.L,
+                          33924394183680.L,
+                          899510451840.L,
+                          19554575040.L,
+                          341863200.L,
+                          4651200.L,
+                          46512.L,
+                          306.L,
+                          1.L};
   const MatrixType A2 = A * A;
   const MatrixType A4 = A2 * A2;
   const MatrixType A6 = A4 * A2;
   const MatrixType A8 = A4 * A4;
-  V = b[17] * A8 + b[15] * A6 + b[13] * A4 + b[11] * A2; // used for temporary storage
+  V = b[17] * A8 + b[15] * A6 + b[13] * A4 + b[11] * A2;  // used for temporary storage
   MatrixType tmp = A8 * V;
-  tmp += b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 
-    + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  tmp += b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
   U.noalias() = A * tmp;
   tmp = b[16] * A8 + b[14] * A6 + b[12] * A4 + b[10] * A2;
   V.noalias() = tmp * A8;
-  V += b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 
-    + b[0] * MatrixType::Identity(A.rows(), A.cols());
+  V += b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
 }
 #endif
 
 template <typename MatrixType, typename RealScalar = typename NumTraits<typename traits<MatrixType>::Scalar>::Real>
-struct matrix_exp_computeUV
-{
+struct matrix_exp_computeUV {
   /** \brief Compute Pad&eacute; approximant to the exponential.
-    *
-    * Computes \c U, \c V and \c squarings such that \f$ (V+U)(V-U)^{-1} \f$ is a Pad&eacute;
-    * approximant of \f$ \exp(2^{-\mbox{squarings}}M) \f$ around \f$ M = 0 \f$, where \f$ M \f$
-    * denotes the matrix \c arg. The degree of the Pad&eacute; approximant and the value of squarings
-    * are chosen such that the approximation error is no more than the round-off error.
-    */
+   *
+   * Computes \c U, \c V and \c squarings such that \f$ (V+U)(V-U)^{-1} \f$ is a Pad&eacute;
+   * approximant of \f$ \exp(2^{-\mbox{squarings}}M) \f$ around \f$ M = 0 \f$, where \f$ M \f$
+   * denotes the matrix \c arg. The degree of the Pad&eacute; approximant and the value of squarings
+   * are chosen such that the approximation error is no more than the round-off error.
+   */
   static void run(const MatrixType& arg, MatrixType& U, MatrixType& V, int& squarings);
 };
 
 template <typename MatrixType>
-struct matrix_exp_computeUV<MatrixType, float>
-{
+struct matrix_exp_computeUV<MatrixType, float> {
   template <typename ArgType>
-  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
-  {
+  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) {
     using std::frexp;
     using std::pow;
     const float l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
@@ -232,12 +241,10 @@
 };
 
 template <typename MatrixType>
-struct matrix_exp_computeUV<MatrixType, double>
-{
+struct matrix_exp_computeUV<MatrixType, double> {
   typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
   template <typename ArgType>
-  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
-  {
+  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) {
     using std::frexp;
     using std::pow;
     const RealScalar l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
@@ -259,25 +266,23 @@
     }
   }
 };
-  
+
 template <typename MatrixType>
-struct matrix_exp_computeUV<MatrixType, long double>
-{
+struct matrix_exp_computeUV<MatrixType, long double> {
   template <typename ArgType>
-  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
-  {
-#if   LDBL_MANT_DIG == 53   // double precision
+  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) {
+#if LDBL_MANT_DIG == 53  // double precision
     matrix_exp_computeUV<MatrixType, double>::run(arg, U, V, squarings);
-  
+
 #else
-  
+
     using std::frexp;
     using std::pow;
     const long double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
     squarings = 0;
-  
-#if LDBL_MANT_DIG <= 64   // extended precision
-  
+
+#if LDBL_MANT_DIG <= 64  // extended precision
+
     if (l1norm < 4.1968497232266989671e-003L) {
       matrix_exp_pade3(arg, U, V);
     } else if (l1norm < 1.1848116734693823091e-001L) {
@@ -293,9 +298,9 @@
       MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
       matrix_exp_pade13(A, U, V);
     }
-  
+
 #elif LDBL_MANT_DIG <= 106  // double-double
-  
+
     if (l1norm < 3.2787892205607026992947488108213e-005L) {
       matrix_exp_pade3(arg, U, V);
     } else if (l1norm < 6.4467025060072760084130906076332e-003L) {
@@ -313,9 +318,9 @@
       MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
       matrix_exp_pade17(A, U, V);
     }
-  
+
 #elif LDBL_MANT_DIG <= 113  // quadruple precision
-  
+
     if (l1norm < 1.639394610288918690547467954466970e-005L) {
       matrix_exp_pade3(arg, U, V);
     } else if (l1norm < 4.253237712165275566025884344433009e-003L) {
@@ -333,46 +338,48 @@
       MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
       matrix_exp_pade17(A, U, V);
     }
-  
+
 #else
-  
+
     // this case should be handled in compute()
-    eigen_assert(false && "Bug in MatrixExponential"); 
-  
+    eigen_assert(false && "Bug in MatrixExponential");
+
 #endif
 #endif  // LDBL_MANT_DIG
   }
 };
 
-template<typename T> struct is_exp_known_type : false_type {};
-template<> struct is_exp_known_type<float> : true_type {};
-template<> struct is_exp_known_type<double> : true_type {};
+template <typename T>
+struct is_exp_known_type : false_type {};
+template <>
+struct is_exp_known_type<float> : true_type {};
+template <>
+struct is_exp_known_type<double> : true_type {};
 #if LDBL_MANT_DIG <= 113
-template<> struct is_exp_known_type<long double> : true_type {};
+template <>
+struct is_exp_known_type<long double> : true_type {};
 #endif
 
 template <typename ArgType, typename ResultType>
-void matrix_exp_compute(const ArgType& arg, ResultType &result, true_type) // natively supported scalar type
+void matrix_exp_compute(const ArgType& arg, ResultType& result, true_type)  // natively supported scalar type
 {
   typedef typename ArgType::PlainObject MatrixType;
   MatrixType U, V;
   int squarings;
-  matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings); // Pade approximant is (U+V) / (-U+V)
+  matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings);  // Pade approximant is (U+V) / (-U+V)
   MatrixType numer = U + V;
   MatrixType denom = -U + V;
   result = denom.partialPivLu().solve(numer);
-  for (int i=0; i<squarings; i++)
-    result *= result;   // undo scaling by repeated squaring
+  for (int i = 0; i < squarings; i++) result *= result;  // undo scaling by repeated squaring
 }
 
-
 /* Computes the matrix exponential
  *
  * \param arg    argument of matrix exponential (should be plain object)
  * \param result variable in which result will be stored
  */
 template <typename ArgType, typename ResultType>
-void matrix_exp_compute(const ArgType& arg, ResultType &result, false_type) // default
+void matrix_exp_compute(const ArgType& arg, ResultType& result, false_type)  // default
 {
   typedef typename ArgType::PlainObject MatrixType;
   typedef typename traits<MatrixType>::Scalar Scalar;
@@ -381,61 +388,57 @@
   result = arg.matrixFunction(internal::stem_function_exp<ComplexScalar>);
 }
 
-} // end namespace Eigen::internal
+}  // namespace internal
 
 /** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix exponential of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix exponential.
-  *
-  * This class holds the argument to the matrix exponential until it is assigned or evaluated for
-  * some other reason (so the argument should not be changed in the meantime). It is the return type
-  * of MatrixBase::exp() and most of the time this is the only way it is used.
-  */
-template<typename Derived> struct MatrixExponentialReturnValue
-: public ReturnByValue<MatrixExponentialReturnValue<Derived> >
-{
-  public:
-    /** \brief Constructor.
-      *
-      * \param src %Matrix (expression) forming the argument of the matrix exponential.
-      */
-    MatrixExponentialReturnValue(const Derived& src) : m_src(src) { }
+ *
+ * \brief Proxy for the matrix exponential of some matrix (expression).
+ *
+ * \tparam Derived  Type of the argument to the matrix exponential.
+ *
+ * This class holds the argument to the matrix exponential until it is assigned or evaluated for
+ * some other reason (so the argument should not be changed in the meantime). It is the return type
+ * of MatrixBase::exp() and most of the time this is the only way it is used.
+ */
+template <typename Derived>
+struct MatrixExponentialReturnValue : public ReturnByValue<MatrixExponentialReturnValue<Derived> > {
+ public:
+  /** \brief Constructor.
+   *
+   * \param src %Matrix (expression) forming the argument of the matrix exponential.
+   */
+  MatrixExponentialReturnValue(const Derived& src) : m_src(src) {}
 
-    /** \brief Compute the matrix exponential.
-      *
-      * \param result the matrix exponential of \p src in the constructor.
-      */
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      const typename internal::nested_eval<Derived, 10>::type tmp(m_src);
-      internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type<typename Derived::RealScalar>());
-    }
+  /** \brief Compute the matrix exponential.
+   *
+   * \param result the matrix exponential of \p src in the constructor.
+   */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    const typename internal::nested_eval<Derived, 10>::type tmp(m_src);
+    internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type<typename Derived::RealScalar>());
+  }
 
-    Index rows() const { return m_src.rows(); }
-    Index cols() const { return m_src.cols(); }
+  Index rows() const { return m_src.rows(); }
+  Index cols() const { return m_src.cols(); }
 
-  protected:
-    const typename internal::ref_selector<Derived>::type m_src;
+ protected:
+  const typename internal::ref_selector<Derived>::type m_src;
 };
 
 namespace internal {
-template<typename Derived>
-struct traits<MatrixExponentialReturnValue<Derived> >
-{
+template <typename Derived>
+struct traits<MatrixExponentialReturnValue<Derived> > {
   typedef typename Derived::PlainObject ReturnType;
 };
-}
+}  // namespace internal
 
 template <typename Derived>
-const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const
-{
+const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const {
   eigen_assert(rows() == cols());
   return MatrixExponentialReturnValue<Derived>(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATRIX_EXPONENTIAL
+#endif  // EIGEN_MATRIX_EXPONENTIAL
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
index cc12ab6..68336a5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
@@ -12,8 +12,10 @@
 
 #include "StemFunction.h"
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 
@@ -21,37 +23,34 @@
 static const float matrix_function_separation = 0.1f;
 
 /** \ingroup MatrixFunctions_Module
-  * \class MatrixFunctionAtomic
-  * \brief Helper class for computing matrix functions of atomic matrices.
-  *
-  * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
-  */
+ * \class MatrixFunctionAtomic
+ * \brief Helper class for computing matrix functions of atomic matrices.
+ *
+ * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
+ */
 template <typename MatrixType>
-class MatrixFunctionAtomic 
-{
-  public:
+class MatrixFunctionAtomic {
+ public:
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename stem_function<Scalar>::type StemFunction;
 
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename stem_function<Scalar>::type StemFunction;
+  /** \brief Constructor
+   * \param[in]  f  matrix function to compute.
+   */
+  MatrixFunctionAtomic(StemFunction f) : m_f(f) {}
 
-    /** \brief Constructor
-      * \param[in]  f  matrix function to compute.
-      */
-    MatrixFunctionAtomic(StemFunction f) : m_f(f) { }
+  /** \brief Compute matrix function of atomic matrix
+   * \param[in]  A  argument of matrix function, should be upper triangular and atomic
+   * \returns  f(A), the matrix function evaluated at the given matrix
+   */
+  MatrixType compute(const MatrixType& A);
 
-    /** \brief Compute matrix function of atomic matrix
-      * \param[in]  A  argument of matrix function, should be upper triangular and atomic
-      * \returns  f(A), the matrix function evaluated at the given matrix
-      */
-    MatrixType compute(const MatrixType& A);
-
-  private:
-    StemFunction* m_f;
+ private:
+  StemFunction* m_f;
 };
 
 template <typename MatrixType>
-typename NumTraits<typename MatrixType::Scalar>::Real matrix_function_compute_mu(const MatrixType& A)
-{
+typename NumTraits<typename MatrixType::Scalar>::Real matrix_function_compute_mu(const MatrixType& A) {
   typedef typename plain_col_type<MatrixType>::type VectorType;
   Index rows = A.rows();
   const MatrixType N = MatrixType::Identity(rows, rows) - A;
@@ -61,8 +60,7 @@
 }
 
 template <typename MatrixType>
-MatrixType MatrixFunctionAtomic<MatrixType>::compute(const MatrixType& A)
-{
+MatrixType MatrixFunctionAtomic<MatrixType>::compute(const MatrixType& A) {
   // TODO: Use that A is upper triangular
   typedef typename NumTraits<Scalar>::Real RealScalar;
   Index rows = A.rows();
@@ -72,10 +70,10 @@
   MatrixType F = m_f(avgEival, 0) * MatrixType::Identity(rows, rows);
   MatrixType P = Ashifted;
   MatrixType Fincr;
-  for (Index s = 1; double(s) < 1.1 * double(rows) + 10.0; s++) { // upper limit is fairly arbitrary
+  for (Index s = 1; double(s) < 1.1 * double(rows) + 10.0; s++) {  // upper limit is fairly arbitrary
     Fincr = m_f(avgEival, static_cast<int>(s)) * P;
     F += Fincr;
-    P = Scalar(RealScalar(1)/RealScalar(s + 1)) * P * Ashifted;
+    P = Scalar(RealScalar(1) / RealScalar(s + 1)) * P * Ashifted;
 
     // test whether Taylor series converged
     const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff();
@@ -86,52 +84,48 @@
       for (Index r = 0; r < rows; r++) {
         RealScalar mx = 0;
         for (Index i = 0; i < rows; i++)
-          mx = (std::max)(mx, std::abs(m_f(Ashifted(i, i) + avgEival, static_cast<int>(s+r))));
-        if (r != 0)
-          rfactorial *= RealScalar(r);
+          mx = (std::max)(mx, std::abs(m_f(Ashifted(i, i) + avgEival, static_cast<int>(s + r))));
+        if (r != 0) rfactorial *= RealScalar(r);
         delta = (std::max)(delta, mx / rfactorial);
       }
       const RealScalar P_norm = P.cwiseAbs().rowwise().sum().maxCoeff();
-      if (mu * delta * P_norm < NumTraits<Scalar>::epsilon() * F_norm) // series converged
+      if (mu * delta * P_norm < NumTraits<Scalar>::epsilon() * F_norm)  // series converged
         break;
     }
   }
   return F;
 }
 
-/** \brief Find cluster in \p clusters containing some value 
-  * \param[in] key Value to find
-  * \returns Iterator to cluster containing \p key, or \c clusters.end() if no cluster in \p m_clusters
-  * contains \p key.
-  */
+/** \brief Find cluster in \p clusters containing some value
+ * \param[in] key Value to find
+ * \returns Iterator to cluster containing \p key, or \c clusters.end() if no cluster in \p m_clusters
+ * contains \p key.
+ */
 template <typename Index, typename ListOfClusters>
-typename ListOfClusters::iterator matrix_function_find_cluster(Index key, ListOfClusters& clusters)
-{
+typename ListOfClusters::iterator matrix_function_find_cluster(Index key, ListOfClusters& clusters) {
   typename std::list<Index>::iterator j;
   for (typename ListOfClusters::iterator i = clusters.begin(); i != clusters.end(); ++i) {
     j = std::find(i->begin(), i->end(), key);
-    if (j != i->end())
-      return i;
+    if (j != i->end()) return i;
   }
   return clusters.end();
 }
 
 /** \brief Partition eigenvalues in clusters of ei'vals close to each other
-  * 
-  * \param[in]  eivals    Eigenvalues
-  * \param[out] clusters  Resulting partition of eigenvalues
-  *
-  * The partition satisfies the following two properties:
-  * # Any eigenvalue in a certain cluster is at most matrix_function_separation() away from another eigenvalue
-  *   in the same cluster.
-  * # The distance between two eigenvalues in different clusters is more than matrix_function_separation().  
-  * The implementation follows Algorithm 4.1 in the paper of Davies and Higham.
-  */
+ *
+ * \param[in]  eivals    Eigenvalues
+ * \param[out] clusters  Resulting partition of eigenvalues
+ *
+ * The partition satisfies the following two properties:
+ * # Any eigenvalue in a certain cluster is at most matrix_function_separation() away from another eigenvalue
+ *   in the same cluster.
+ * # The distance between two eigenvalues in different clusters is more than matrix_function_separation().
+ * The implementation follows Algorithm 4.1 in the paper of Davies and Higham.
+ */
 template <typename EivalsType, typename Cluster>
-void matrix_function_partition_eigenvalues(const EivalsType& eivals, std::list<Cluster>& clusters)
-{
+void matrix_function_partition_eigenvalues(const EivalsType& eivals, std::list<Cluster>& clusters) {
   typedef typename EivalsType::RealScalar RealScalar;
-  for (Index i=0; i<eivals.rows(); ++i) {
+  for (Index i = 0; i < eivals.rows(); ++i) {
     // Find cluster containing i-th ei'val, adding a new cluster if necessary
     typename std::list<Cluster>::iterator qi = matrix_function_find_cluster(i, clusters);
     if (qi == clusters.end()) {
@@ -143,9 +137,9 @@
     }
 
     // Look for other element to add to the set
-    for (Index j=i+1; j<eivals.rows(); ++j) {
-      if (abs(eivals(j) - eivals(i)) <= RealScalar(matrix_function_separation)
-          && std::find(qi->begin(), qi->end(), j) == qi->end()) {
+    for (Index j = i + 1; j < eivals.rows(); ++j) {
+      if (abs(eivals(j) - eivals(i)) <= RealScalar(matrix_function_separation) &&
+          std::find(qi->begin(), qi->end(), j) == qi->end()) {
         typename std::list<Cluster>::iterator qj = matrix_function_find_cluster(j, clusters);
         if (qj == clusters.end()) {
           qi->push_back(j);
@@ -160,8 +154,7 @@
 
 /** \brief Compute size of each cluster given a partitioning */
 template <typename ListOfClusters, typename Index>
-void matrix_function_compute_cluster_size(const ListOfClusters& clusters, Matrix<Index, Dynamic, 1>& clusterSize)
-{
+void matrix_function_compute_cluster_size(const ListOfClusters& clusters, Matrix<Index, Dynamic, 1>& clusterSize) {
   const Index numClusters = static_cast<Index>(clusters.size());
   clusterSize.setZero(numClusters);
   Index clusterIndex = 0;
@@ -173,19 +166,17 @@
 
 /** \brief Compute start of each block using clusterSize */
 template <typename VectorType>
-void matrix_function_compute_block_start(const VectorType& clusterSize, VectorType& blockStart)
-{
+void matrix_function_compute_block_start(const VectorType& clusterSize, VectorType& blockStart) {
   blockStart.resize(clusterSize.rows());
   blockStart(0) = 0;
   for (Index i = 1; i < clusterSize.rows(); i++) {
-    blockStart(i) = blockStart(i-1) + clusterSize(i-1);
+    blockStart(i) = blockStart(i - 1) + clusterSize(i - 1);
   }
 }
 
 /** \brief Compute mapping of eigenvalue indices to cluster indices */
 template <typename EivalsType, typename ListOfClusters, typename VectorType>
-void matrix_function_compute_map(const EivalsType& eivals, const ListOfClusters& clusters, VectorType& eivalToCluster)
-{
+void matrix_function_compute_map(const EivalsType& eivals, const ListOfClusters& clusters, VectorType& eivalToCluster) {
   eivalToCluster.resize(eivals.rows());
   Index clusterIndex = 0;
   for (typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
@@ -200,8 +191,8 @@
 
 /** \brief Compute permutation which groups ei'vals in same cluster together */
 template <typename DynVectorType, typename VectorType>
-void matrix_function_compute_permutation(const DynVectorType& blockStart, const DynVectorType& eivalToCluster, VectorType& permutation)
-{
+void matrix_function_compute_permutation(const DynVectorType& blockStart, const DynVectorType& eivalToCluster,
+                                         VectorType& permutation) {
   DynVectorType indexNextEntry = blockStart;
   permutation.resize(eivalToCluster.rows());
   for (Index i = 0; i < eivalToCluster.rows(); i++) {
@@ -209,70 +200,68 @@
     permutation[i] = indexNextEntry[cluster];
     ++indexNextEntry[cluster];
   }
-}  
+}
 
 /** \brief Permute Schur decomposition in U and T according to permutation */
 template <typename VectorType, typename MatrixType>
-void matrix_function_permute_schur(VectorType& permutation, MatrixType& U, MatrixType& T)
-{
+void matrix_function_permute_schur(VectorType& permutation, MatrixType& U, MatrixType& T) {
   for (Index i = 0; i < permutation.rows() - 1; i++) {
     Index j;
     for (j = i; j < permutation.rows(); j++) {
       if (permutation(j) == i) break;
     }
     eigen_assert(permutation(j) == i);
-    for (Index k = j-1; k >= i; k--) {
+    for (Index k = j - 1; k >= i; k--) {
       JacobiRotation<typename MatrixType::Scalar> rotation;
-      rotation.makeGivens(T(k, k+1), T(k+1, k+1) - T(k, k));
-      T.applyOnTheLeft(k, k+1, rotation.adjoint());
-      T.applyOnTheRight(k, k+1, rotation);
-      U.applyOnTheRight(k, k+1, rotation);
-      std::swap(permutation.coeffRef(k), permutation.coeffRef(k+1));
+      rotation.makeGivens(T(k, k + 1), T(k + 1, k + 1) - T(k, k));
+      T.applyOnTheLeft(k, k + 1, rotation.adjoint());
+      T.applyOnTheRight(k, k + 1, rotation);
+      U.applyOnTheRight(k, k + 1, rotation);
+      std::swap(permutation.coeffRef(k), permutation.coeffRef(k + 1));
     }
   }
 }
 
 /** \brief Compute block diagonal part of matrix function.
-  *
-  * This routine computes the matrix function applied to the block diagonal part of \p T (which should be
-  * upper triangular), with the blocking given by \p blockStart and \p clusterSize. The matrix function of
-  * each diagonal block is computed by \p atomic. The off-diagonal parts of \p fT are set to zero.
-  */
+ *
+ * This routine computes the matrix function applied to the block diagonal part of \p T (which should be
+ * upper triangular), with the blocking given by \p blockStart and \p clusterSize. The matrix function of
+ * each diagonal block is computed by \p atomic. The off-diagonal parts of \p fT are set to zero.
+ */
 template <typename MatrixType, typename AtomicType, typename VectorType>
-void matrix_function_compute_block_atomic(const MatrixType& T, AtomicType& atomic, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
-{ 
+void matrix_function_compute_block_atomic(const MatrixType& T, AtomicType& atomic, const VectorType& blockStart,
+                                          const VectorType& clusterSize, MatrixType& fT) {
   fT.setZero(T.rows(), T.cols());
   for (Index i = 0; i < clusterSize.rows(); ++i) {
-    fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
-      = atomic.compute(T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i)));
+    fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i)) =
+        atomic.compute(T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i)));
   }
 }
 
-/** \brief Solve a triangular Sylvester equation AX + XB = C 
-  *
-  * \param[in]  A  the matrix A; should be square and upper triangular
-  * \param[in]  B  the matrix B; should be square and upper triangular
-  * \param[in]  C  the matrix C; should have correct size.
-  *
-  * \returns the solution X.
-  *
-  * If A is m-by-m and B is n-by-n, then both C and X are m-by-n.  The (i,j)-th component of the Sylvester
-  * equation is
-  * \f[ 
-  *     \sum_{k=i}^m A_{ik} X_{kj} + \sum_{k=1}^j X_{ik} B_{kj} = C_{ij}. 
-  * \f]
-  * This can be re-arranged to yield:
-  * \f[ 
-  *     X_{ij} = \frac{1}{A_{ii} + B_{jj}} \Bigl( C_{ij}
-  *     - \sum_{k=i+1}^m A_{ik} X_{kj} - \sum_{k=1}^{j-1} X_{ik} B_{kj} \Bigr).
-  * \f]
-  * It is assumed that A and B are such that the numerator is never zero (otherwise the Sylvester equation
-  * does not have a unique solution). In that case, these equations can be evaluated in the order 
-  * \f$ i=m,\ldots,1 \f$ and \f$ j=1,\ldots,n \f$.
-  */
+/** \brief Solve a triangular Sylvester equation AX + XB = C
+ *
+ * \param[in]  A  the matrix A; should be square and upper triangular
+ * \param[in]  B  the matrix B; should be square and upper triangular
+ * \param[in]  C  the matrix C; should have correct size.
+ *
+ * \returns the solution X.
+ *
+ * If A is m-by-m and B is n-by-n, then both C and X are m-by-n.  The (i,j)-th component of the Sylvester
+ * equation is
+ * \f[
+ *     \sum_{k=i}^m A_{ik} X_{kj} + \sum_{k=1}^j X_{ik} B_{kj} = C_{ij}.
+ * \f]
+ * This can be re-arranged to yield:
+ * \f[
+ *     X_{ij} = \frac{1}{A_{ii} + B_{jj}} \Bigl( C_{ij}
+ *     - \sum_{k=i+1}^m A_{ik} X_{kj} - \sum_{k=1}^{j-1} X_{ik} B_{kj} \Bigr).
+ * \f]
+ * It is assumed that A and B are such that the numerator is never zero (otherwise the Sylvester equation
+ * does not have a unique solution). In that case, these equations can be evaluated in the order
+ * \f$ i=m,\ldots,1 \f$ and \f$ j=1,\ldots,n \f$.
+ */
 template <typename MatrixType>
-MatrixType matrix_function_solve_triangular_sylvester(const MatrixType& A, const MatrixType& B, const MatrixType& C)
-{
+MatrixType matrix_function_solve_triangular_sylvester(const MatrixType& A, const MatrixType& B, const MatrixType& C) {
   eigen_assert(A.rows() == A.cols());
   eigen_assert(A.isUpperTriangular());
   eigen_assert(B.rows() == B.cols());
@@ -288,40 +277,39 @@
 
   for (Index i = m - 1; i >= 0; --i) {
     for (Index j = 0; j < n; ++j) {
-
       // Compute AX = \sum_{k=i+1}^m A_{ik} X_{kj}
       Scalar AX;
       if (i == m - 1) {
-	AX = 0; 
+        AX = 0;
       } else {
-	Matrix<Scalar,1,1> AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i);
-	AX = AXmatrix(0,0);
+        Matrix<Scalar, 1, 1> AXmatrix = A.row(i).tail(m - 1 - i) * X.col(j).tail(m - 1 - i);
+        AX = AXmatrix(0, 0);
       }
 
       // Compute XB = \sum_{k=1}^{j-1} X_{ik} B_{kj}
       Scalar XB;
       if (j == 0) {
-	XB = 0; 
+        XB = 0;
       } else {
-	Matrix<Scalar,1,1> XBmatrix = X.row(i).head(j) * B.col(j).head(j);
-	XB = XBmatrix(0,0);
+        Matrix<Scalar, 1, 1> XBmatrix = X.row(i).head(j) * B.col(j).head(j);
+        XB = XBmatrix(0, 0);
       }
 
-      X(i,j) = (C(i,j) - AX - XB) / (A(i,i) + B(j,j));
+      X(i, j) = (C(i, j) - AX - XB) / (A(i, i) + B(j, j));
     }
   }
   return X;
 }
 
 /** \brief Compute part of matrix function above block diagonal.
-  *
-  * This routine completes the computation of \p fT, denoting a matrix function applied to the triangular
-  * matrix \p T. It assumes that the block diagonal part of \p fT has already been computed. The part below
-  * the diagonal is zero, because \p T is upper triangular.
-  */
+ *
+ * This routine completes the computation of \p fT, denoting a matrix function applied to the triangular
+ * matrix \p T. It assumes that the block diagonal part of \p fT has already been computed. The part below
+ * the diagonal is zero, because \p T is upper triangular.
+ */
 template <typename MatrixType, typename VectorType>
-void matrix_function_compute_above_diagonal(const MatrixType& T, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
-{ 
+void matrix_function_compute_above_diagonal(const MatrixType& T, const VectorType& blockStart,
+                                            const VectorType& clusterSize, MatrixType& fT) {
   typedef internal::traits<MatrixType> Traits;
   typedef typename MatrixType::Scalar Scalar;
   static const int Options = MatrixType::Options;
@@ -331,67 +319,64 @@
     for (Index i = 0; i < clusterSize.rows() - k; i++) {
       // compute (i, i+k) block
       DynMatrixType A = T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i));
-      DynMatrixType B = -T.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
-      DynMatrixType C = fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
-        * T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k));
-      C -= T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
-        * fT.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
+      DynMatrixType B = -T.block(blockStart(i + k), blockStart(i + k), clusterSize(i + k), clusterSize(i + k));
+      DynMatrixType C = fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i)) *
+                        T.block(blockStart(i), blockStart(i + k), clusterSize(i), clusterSize(i + k));
+      C -= T.block(blockStart(i), blockStart(i + k), clusterSize(i), clusterSize(i + k)) *
+           fT.block(blockStart(i + k), blockStart(i + k), clusterSize(i + k), clusterSize(i + k));
       for (Index m = i + 1; m < i + k; m++) {
-        C += fT.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
-          * T.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
-        C -= T.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
-          * fT.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
+        C += fT.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m)) *
+             T.block(blockStart(m), blockStart(i + k), clusterSize(m), clusterSize(i + k));
+        C -= T.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m)) *
+             fT.block(blockStart(m), blockStart(i + k), clusterSize(m), clusterSize(i + k));
       }
-      fT.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
-        = matrix_function_solve_triangular_sylvester(A, B, C);
+      fT.block(blockStart(i), blockStart(i + k), clusterSize(i), clusterSize(i + k)) =
+          matrix_function_solve_triangular_sylvester(A, B, C);
     }
   }
 }
 
 /** \ingroup MatrixFunctions_Module
-  * \brief Class for computing matrix functions.
-  * \tparam  MatrixType  type of the argument of the matrix function,
-  *                      expected to be an instantiation of the Matrix class template.
-  * \tparam  AtomicType  type for computing matrix function of atomic blocks.
-  * \tparam  IsComplex   used internally to select correct specialization.
-  *
-  * This class implements the Schur-Parlett algorithm for computing matrix functions. The spectrum of the
-  * matrix is divided in clustered of eigenvalues that lies close together. This class delegates the
-  * computation of the matrix function on every block corresponding to these clusters to an object of type
-  * \p AtomicType and uses these results to compute the matrix function of the whole matrix. The class
-  * \p AtomicType should have a \p compute() member function for computing the matrix function of a block.
-  *
-  * \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic
-  */
+ * \brief Class for computing matrix functions.
+ * \tparam  MatrixType  type of the argument of the matrix function,
+ *                      expected to be an instantiation of the Matrix class template.
+ * \tparam  AtomicType  type for computing matrix function of atomic blocks.
+ * \tparam  IsComplex   used internally to select correct specialization.
+ *
+ * This class implements the Schur-Parlett algorithm for computing matrix functions. The spectrum of the
+ * matrix is divided in clustered of eigenvalues that lies close together. This class delegates the
+ * computation of the matrix function on every block corresponding to these clusters to an object of type
+ * \p AtomicType and uses these results to compute the matrix function of the whole matrix. The class
+ * \p AtomicType should have a \p compute() member function for computing the matrix function of a block.
+ *
+ * \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic
+ */
 template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
-struct matrix_function_compute
-{  
-    /** \brief Compute the matrix function.
-      *
-      * \param[in]  A       argument of matrix function, should be a square matrix.
-      * \param[in]  atomic  class for computing matrix function of atomic blocks.
-      * \param[out] result  the function \p f applied to \p A, as
-      * specified in the constructor.
-      *
-      * See MatrixBase::matrixFunction() for details on how this computation
-      * is implemented.
-      */
-    template <typename AtomicType, typename ResultType> 
-    static void run(const MatrixType& A, AtomicType& atomic, ResultType &result);    
+struct matrix_function_compute {
+  /** \brief Compute the matrix function.
+   *
+   * \param[in]  A       argument of matrix function, should be a square matrix.
+   * \param[in]  atomic  class for computing matrix function of atomic blocks.
+   * \param[out] result  the function \p f applied to \p A, as
+   * specified in the constructor.
+   *
+   * See MatrixBase::matrixFunction() for details on how this computation
+   * is implemented.
+   */
+  template <typename AtomicType, typename ResultType>
+  static void run(const MatrixType& A, AtomicType& atomic, ResultType& result);
 };
 
-/** \internal \ingroup MatrixFunctions_Module 
-  * \brief Partial specialization of MatrixFunction for real matrices
-  *
-  * This converts the real matrix to a complex matrix, compute the matrix function of that matrix, and then
-  * converts the result back to a real matrix.
-  */
+/** \internal \ingroup MatrixFunctions_Module
+ * \brief Partial specialization of MatrixFunction for real matrices
+ *
+ * This converts the real matrix to a complex matrix, compute the matrix function of that matrix, and then
+ * converts the result back to a real matrix.
+ */
 template <typename MatrixType>
-struct matrix_function_compute<MatrixType, 0>
-{  
+struct matrix_function_compute<MatrixType, 0> {
   template <typename MatA, typename AtomicType, typename ResultType>
-  static void run(const MatA& A, AtomicType& atomic, ResultType &result)
-  {
+  static void run(const MatA& A, AtomicType& atomic, ResultType& result) {
     typedef internal::traits<MatrixType> Traits;
     typedef typename Traits::Scalar Scalar;
     static const int Rows = Traits::RowsAtCompileTime, Cols = Traits::ColsAtCompileTime;
@@ -407,40 +392,38 @@
   }
 };
 
-/** \internal \ingroup MatrixFunctions_Module 
-  * \brief Partial specialization of MatrixFunction for complex matrices
-  */
+/** \internal \ingroup MatrixFunctions_Module
+ * \brief Partial specialization of MatrixFunction for complex matrices
+ */
 template <typename MatrixType>
-struct matrix_function_compute<MatrixType, 1>
-{
+struct matrix_function_compute<MatrixType, 1> {
   template <typename MatA, typename AtomicType, typename ResultType>
-  static void run(const MatA& A, AtomicType& atomic, ResultType &result)
-  {
+  static void run(const MatA& A, AtomicType& atomic, ResultType& result) {
     typedef internal::traits<MatrixType> Traits;
-    
+
     // compute Schur decomposition of A
     const ComplexSchur<MatrixType> schurOfA(A);
-    eigen_assert(schurOfA.info()==Success);
+    eigen_assert(schurOfA.info() == Success);
     MatrixType T = schurOfA.matrixT();
     MatrixType U = schurOfA.matrixU();
 
     // partition eigenvalues into clusters of ei'vals "close" to each other
-    std::list<std::list<Index> > clusters; 
+    std::list<std::list<Index> > clusters;
     matrix_function_partition_eigenvalues(T.diagonal(), clusters);
 
     // compute size of each cluster
     Matrix<Index, Dynamic, 1> clusterSize;
     matrix_function_compute_cluster_size(clusters, clusterSize);
 
-    // blockStart[i] is row index at which block corresponding to i-th cluster starts 
-    Matrix<Index, Dynamic, 1> blockStart; 
+    // blockStart[i] is row index at which block corresponding to i-th cluster starts
+    Matrix<Index, Dynamic, 1> blockStart;
     matrix_function_compute_block_start(clusterSize, blockStart);
 
-    // compute map so that eivalToCluster[i] = j means that i-th ei'val is in j-th cluster 
+    // compute map so that eivalToCluster[i] = j means that i-th ei'val is in j-th cluster
     Matrix<Index, Dynamic, 1> eivalToCluster;
     matrix_function_compute_map(T.diagonal(), clusters, eivalToCluster);
 
-    // compute permutation which groups ei'vals in same cluster together 
+    // compute permutation which groups ei'vals in same cluster together
     Matrix<Index, Traits::RowsAtCompileTime, 1> permutation;
     matrix_function_compute_permutation(blockStart, eivalToCluster, permutation);
 
@@ -448,122 +431,113 @@
     matrix_function_permute_schur(permutation, U, T);
 
     // compute result
-    MatrixType fT; // matrix function applied to T
+    MatrixType fT;  // matrix function applied to T
     matrix_function_compute_block_atomic(T, atomic, blockStart, clusterSize, fT);
     matrix_function_compute_above_diagonal(T, blockStart, clusterSize, fT);
     result = U * (fT.template triangularView<Upper>() * U.adjoint());
   }
 };
 
-} // end of namespace internal
+}  // end of namespace internal
 
 /** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix function of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix function.
-  *
-  * This class holds the argument to the matrix function until it is assigned or evaluated for some other
-  * reason (so the argument should not be changed in the meantime). It is the return type of
-  * matrixBase::matrixFunction() and related functions and most of the time this is the only way it is used.
-  */
-template<typename Derived> class MatrixFunctionReturnValue
-: public ReturnByValue<MatrixFunctionReturnValue<Derived> >
-{
-  public:
-    typedef typename Derived::Scalar Scalar;
-    typedef typename internal::stem_function<Scalar>::type StemFunction;
+ *
+ * \brief Proxy for the matrix function of some matrix (expression).
+ *
+ * \tparam Derived  Type of the argument to the matrix function.
+ *
+ * This class holds the argument to the matrix function until it is assigned or evaluated for some other
+ * reason (so the argument should not be changed in the meantime). It is the return type of
+ * matrixBase::matrixFunction() and related functions and most of the time this is the only way it is used.
+ */
+template <typename Derived>
+class MatrixFunctionReturnValue : public ReturnByValue<MatrixFunctionReturnValue<Derived> > {
+ public:
+  typedef typename Derived::Scalar Scalar;
+  typedef typename internal::stem_function<Scalar>::type StemFunction;
 
-  protected:
-    typedef typename internal::ref_selector<Derived>::type DerivedNested;
+ protected:
+  typedef typename internal::ref_selector<Derived>::type DerivedNested;
 
-  public:
+ public:
+  /** \brief Constructor.
+   *
+   * \param[in] A  %Matrix (expression) forming the argument of the matrix function.
+   * \param[in] f  Stem function for matrix function under consideration.
+   */
+  MatrixFunctionReturnValue(const Derived& A, StemFunction f) : m_A(A), m_f(f) {}
 
-    /** \brief Constructor.
-      *
-      * \param[in] A  %Matrix (expression) forming the argument of the matrix function.
-      * \param[in] f  Stem function for matrix function under consideration.
-      */
-    MatrixFunctionReturnValue(const Derived& A, StemFunction f) : m_A(A), m_f(f) { }
+  /** \brief Compute the matrix function.
+   *
+   * \param[out] result \p f applied to \p A, where \p f and \p A are as in the constructor.
+   */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    typedef typename internal::nested_eval<Derived, 10>::type NestedEvalType;
+    typedef internal::remove_all_t<NestedEvalType> NestedEvalTypeClean;
+    typedef internal::traits<NestedEvalTypeClean> Traits;
+    typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
+    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime>
+        DynMatrixType;
 
-    /** \brief Compute the matrix function.
-      *
-      * \param[out] result \p f applied to \p A, where \p f and \p A are as in the constructor.
-      */
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      typedef typename internal::nested_eval<Derived, 10>::type NestedEvalType;
-      typedef typename internal::remove_all<NestedEvalType>::type NestedEvalTypeClean;
-      typedef internal::traits<NestedEvalTypeClean> Traits;
-      typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
-      typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime> DynMatrixType;
+    typedef internal::MatrixFunctionAtomic<DynMatrixType> AtomicType;
+    AtomicType atomic(m_f);
 
-      typedef internal::MatrixFunctionAtomic<DynMatrixType> AtomicType;
-      AtomicType atomic(m_f);
+    internal::matrix_function_compute<typename NestedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
+  }
 
-      internal::matrix_function_compute<typename NestedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
-    }
+  Index rows() const { return m_A.rows(); }
+  Index cols() const { return m_A.cols(); }
 
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
-
-  private:
-    const DerivedNested m_A;
-    StemFunction *m_f;
+ private:
+  const DerivedNested m_A;
+  StemFunction* m_f;
 };
 
 namespace internal {
-template<typename Derived>
-struct traits<MatrixFunctionReturnValue<Derived> >
-{
+template <typename Derived>
+struct traits<MatrixFunctionReturnValue<Derived> > {
   typedef typename Derived::PlainObject ReturnType;
 };
-}
-
+}  // namespace internal
 
 /********** MatrixBase methods **********/
 
-
 template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
-{
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(
+    typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const {
   eigen_assert(rows() == cols());
   return MatrixFunctionReturnValue<Derived>(derived(), f);
 }
 
 template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const
-{
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const {
   eigen_assert(rows() == cols());
   typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
   return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sin<ComplexScalar>);
 }
 
 template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const
-{
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const {
   eigen_assert(rows() == cols());
   typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
   return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cos<ComplexScalar>);
 }
 
 template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const
-{
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const {
   eigen_assert(rows() == cols());
   typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
   return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sinh<ComplexScalar>);
 }
 
 template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
-{
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const {
   eigen_assert(rows() == cols());
   typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
   return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cosh<ComplexScalar>);
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATRIX_FUNCTION_H
+#endif  // EIGEN_MATRIX_FUNCTION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
index e917013..4228166 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
@@ -11,31 +11,34 @@
 #ifndef EIGEN_MATRIX_LOGARITHM
 #define EIGEN_MATRIX_LOGARITHM
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
 
-namespace internal { 
+namespace Eigen {
+
+namespace internal {
 
 template <typename Scalar>
-struct matrix_log_min_pade_degree 
-{
+struct matrix_log_min_pade_degree {
   static const int value = 3;
 };
 
 template <typename Scalar>
-struct matrix_log_max_pade_degree 
-{
+struct matrix_log_max_pade_degree {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  static const int value = std::numeric_limits<RealScalar>::digits<= 24?  5:  // single precision
-                           std::numeric_limits<RealScalar>::digits<= 53?  7:  // double precision
-                           std::numeric_limits<RealScalar>::digits<= 64?  8:  // extended precision
-                           std::numeric_limits<RealScalar>::digits<=106? 10:  // double-double
-                                                                         11;  // quadruple precision
+  static const int value = std::numeric_limits<RealScalar>::digits <= 24 ? 5 :  // single precision
+                               std::numeric_limits<RealScalar>::digits <= 53 ? 7
+                                                                             :  // double precision
+                               std::numeric_limits<RealScalar>::digits <= 64 ? 8
+                                                                             :  // extended precision
+                               std::numeric_limits<RealScalar>::digits <= 106 ? 10
+                                                                              :  // double-double
+                               11;                                               // quadruple precision
 };
 
 /** \brief Compute logarithm of 2x2 triangular matrix. */
 template <typename MatrixType>
-void matrix_log_compute_2x2(const MatrixType& A, MatrixType& result)
-{
+void matrix_log_compute_2x2(const MatrixType& A, MatrixType& result) {
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   using std::abs;
@@ -43,187 +46,184 @@
   using std::imag;
   using std::log;
 
-  Scalar logA00 = log(A(0,0));
-  Scalar logA11 = log(A(1,1));
+  Scalar logA00 = log(A(0, 0));
+  Scalar logA11 = log(A(1, 1));
 
-  result(0,0) = logA00;
-  result(1,0) = Scalar(0);
-  result(1,1) = logA11;
+  result(0, 0) = logA00;
+  result(1, 0) = Scalar(0);
+  result(1, 1) = logA11;
 
-  Scalar y = A(1,1) - A(0,0);
-  if (y==Scalar(0))
-  {
-    result(0,1) = A(0,1) / A(0,0);
-  }
-  else if ((abs(A(0,0)) < RealScalar(0.5)*abs(A(1,1))) || (abs(A(0,0)) > 2*abs(A(1,1))))
-  {
-    result(0,1) = A(0,1) * (logA11 - logA00) / y;
-  }
-  else
-  {
+  Scalar y = A(1, 1) - A(0, 0);
+  if (y == Scalar(0)) {
+    result(0, 1) = A(0, 1) / A(0, 0);
+  } else if ((abs(A(0, 0)) < RealScalar(0.5) * abs(A(1, 1))) || (abs(A(0, 0)) > 2 * abs(A(1, 1)))) {
+    result(0, 1) = A(0, 1) * (logA11 - logA00) / y;
+  } else {
     // computation in previous branch is inaccurate if A(1,1) \approx A(0,0)
-    RealScalar unwindingNumber = ceil((imag(logA11 - logA00) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI));
-    result(0,1) = A(0,1) * (numext::log1p(y/A(0,0)) + Scalar(0,RealScalar(2*EIGEN_PI)*unwindingNumber)) / y;
+    RealScalar unwindingNumber = ceil((imag(logA11 - logA00) - RealScalar(EIGEN_PI)) / RealScalar(2 * EIGEN_PI));
+    result(0, 1) = A(0, 1) * (numext::log1p(y / A(0, 0)) + Scalar(0, RealScalar(2 * EIGEN_PI) * unwindingNumber)) / y;
   }
 }
 
 /* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = float) */
-inline int matrix_log_get_pade_degree(float normTminusI)
-{
-  const float maxNormForPade[] = { 2.5111573934555054e-1 /* degree = 3 */ , 4.0535837411880493e-1,
-            5.3149729967117310e-1 };
+inline int matrix_log_get_pade_degree(float normTminusI) {
+  const float maxNormForPade[] = {2.5111573934555054e-1 /* degree = 3 */, 4.0535837411880493e-1, 5.3149729967117310e-1};
   const int minPadeDegree = matrix_log_min_pade_degree<float>::value;
   const int maxPadeDegree = matrix_log_max_pade_degree<float>::value;
   int degree = minPadeDegree;
-  for (; degree <= maxPadeDegree; ++degree) 
-    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
-      break;
+  for (; degree <= maxPadeDegree; ++degree)
+    if (normTminusI <= maxNormForPade[degree - minPadeDegree]) break;
   return degree;
 }
 
 /* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = double) */
-inline int matrix_log_get_pade_degree(double normTminusI)
-{
-  const double maxNormForPade[] = { 1.6206284795015624e-2 /* degree = 3 */ , 5.3873532631381171e-2,
-            1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 };
+inline int matrix_log_get_pade_degree(double normTminusI) {
+  const double maxNormForPade[] = {1.6206284795015624e-2 /* degree = 3 */, 5.3873532631381171e-2, 1.1352802267628681e-1,
+                                   1.8662860613541288e-1, 2.642960831111435e-1};
   const int minPadeDegree = matrix_log_min_pade_degree<double>::value;
   const int maxPadeDegree = matrix_log_max_pade_degree<double>::value;
   int degree = minPadeDegree;
   for (; degree <= maxPadeDegree; ++degree)
-    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
-      break;
+    if (normTminusI <= maxNormForPade[degree - minPadeDegree]) break;
   return degree;
 }
 
 /* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = long double) */
-inline int matrix_log_get_pade_degree(long double normTminusI)
-{
-#if   LDBL_MANT_DIG == 53         // double precision
-  const long double maxNormForPade[] = { 1.6206284795015624e-2L /* degree = 3 */ , 5.3873532631381171e-2L,
-            1.1352802267628681e-1L, 1.8662860613541288e-1L, 2.642960831111435e-1L };
-#elif LDBL_MANT_DIG <= 64         // extended precision
-  const long double maxNormForPade[] = { 5.48256690357782863103e-3L /* degree = 3 */, 2.34559162387971167321e-2L,
-            5.84603923897347449857e-2L, 1.08486423756725170223e-1L, 1.68385767881294446649e-1L,
-            2.32777776523703892094e-1L };
-#elif LDBL_MANT_DIG <= 106        // double-double
-  const long double maxNormForPade[] = { 8.58970550342939562202529664318890e-5L /* degree = 3 */,
-            9.34074328446359654039446552677759e-4L, 4.26117194647672175773064114582860e-3L,
-            1.21546224740281848743149666560464e-2L, 2.61100544998339436713088248557444e-2L,
-            4.66170074627052749243018566390567e-2L, 7.32585144444135027565872014932387e-2L,
-            1.05026503471351080481093652651105e-1L };
-#else                             // quadruple precision
-  const long double maxNormForPade[] = { 4.7419931187193005048501568167858103e-5L /* degree = 3 */,
-            5.8853168473544560470387769480192666e-4L, 2.9216120366601315391789493628113520e-3L,
-            8.8415758124319434347116734705174308e-3L, 1.9850836029449446668518049562565291e-2L,
-            3.6688019729653446926585242192447447e-2L, 5.9290962294020186998954055264528393e-2L,
-            8.6998436081634343903250580992127677e-2L, 1.1880960220216759245467951592883642e-1L };
+inline int matrix_log_get_pade_degree(long double normTminusI) {
+#if LDBL_MANT_DIG == 53  // double precision
+  const long double maxNormForPade[] = {1.6206284795015624e-2L /* degree = 3 */, 5.3873532631381171e-2L,
+                                        1.1352802267628681e-1L, 1.8662860613541288e-1L, 2.642960831111435e-1L};
+#elif LDBL_MANT_DIG <= 64   // extended precision
+  const long double maxNormForPade[] = {5.48256690357782863103e-3L /* degree = 3 */,
+                                        2.34559162387971167321e-2L,
+                                        5.84603923897347449857e-2L,
+                                        1.08486423756725170223e-1L,
+                                        1.68385767881294446649e-1L,
+                                        2.32777776523703892094e-1L};
+#elif LDBL_MANT_DIG <= 106  // double-double
+  const long double maxNormForPade[] = {8.58970550342939562202529664318890e-5L /* degree = 3 */,
+                                        9.34074328446359654039446552677759e-4L,
+                                        4.26117194647672175773064114582860e-3L,
+                                        1.21546224740281848743149666560464e-2L,
+                                        2.61100544998339436713088248557444e-2L,
+                                        4.66170074627052749243018566390567e-2L,
+                                        7.32585144444135027565872014932387e-2L,
+                                        1.05026503471351080481093652651105e-1L};
+#else                       // quadruple precision
+  const long double maxNormForPade[] = {4.7419931187193005048501568167858103e-5L /* degree = 3 */,
+                                        5.8853168473544560470387769480192666e-4L,
+                                        2.9216120366601315391789493628113520e-3L,
+                                        8.8415758124319434347116734705174308e-3L,
+                                        1.9850836029449446668518049562565291e-2L,
+                                        3.6688019729653446926585242192447447e-2L,
+                                        5.9290962294020186998954055264528393e-2L,
+                                        8.6998436081634343903250580992127677e-2L,
+                                        1.1880960220216759245467951592883642e-1L};
 #endif
   const int minPadeDegree = matrix_log_min_pade_degree<long double>::value;
   const int maxPadeDegree = matrix_log_max_pade_degree<long double>::value;
   int degree = minPadeDegree;
   for (; degree <= maxPadeDegree; ++degree)
-    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
-      break;
+    if (normTminusI <= maxNormForPade[degree - minPadeDegree]) break;
   return degree;
 }
 
 /* \brief Compute Pade approximation to matrix logarithm */
 template <typename MatrixType>
-void matrix_log_compute_pade(MatrixType& result, const MatrixType& T, int degree)
-{
+void matrix_log_compute_pade(MatrixType& result, const MatrixType& T, int degree) {
   typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
   const int minPadeDegree = 3;
   const int maxPadeDegree = 11;
-  assert(degree >= minPadeDegree && degree <= maxPadeDegree);
+  eigen_assert(degree >= minPadeDegree && degree <= maxPadeDegree);
   // FIXME this creates float-conversion-warnings if these are enabled.
   // Either manually convert each value, or disable the warning locally
-  const RealScalar nodes[][maxPadeDegree] = { 
-    { 0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L,  // degree 3
-      0.8872983346207416885179265399782400L }, 
-    { 0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L,  // degree 4
-      0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L },
-    { 0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L,  // degree 5
-      0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
-      0.9530899229693319963988134391496965L },
-    { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L,  // degree 6
-      0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
-      0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L },
-    { 0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L,  // degree 7
-      0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
-      0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
-      0.9745539561713792622630948420239256L },
-    { 0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L,  // degree 8
-      0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
-      0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
-      0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L },
-    { 0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L,  // degree 9
-      0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
-      0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
-      0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
-      0.9840801197538130449177881014518364L },
-    { 0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L,  // degree 10
-      0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
-      0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
-      0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
-      0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L },
-    { 0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L,  // degree 11
-      0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
-      0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
-      0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
-      0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
-      0.9891143290730284964019690005614287L } };
+  const RealScalar nodes[][maxPadeDegree] = {
+      {0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L,  // degree 3
+       0.8872983346207416885179265399782400L},
+      {0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L,  // degree 4
+       0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L},
+      {0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L,  // degree 5
+       0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
+       0.9530899229693319963988134391496965L},
+      {0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L,  // degree 6
+       0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
+       0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L},
+      {0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L,  // degree 7
+       0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
+       0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
+       0.9745539561713792622630948420239256L},
+      {0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L,  // degree 8
+       0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
+       0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
+       0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L},
+      {0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L,  // degree 9
+       0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
+       0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
+       0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
+       0.9840801197538130449177881014518364L},
+      {0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L,  // degree 10
+       0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
+       0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
+       0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
+       0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L},
+      {0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L,  // degree 11
+       0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
+       0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
+       0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
+       0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
+       0.9891143290730284964019690005614287L}};
 
-  const RealScalar weights[][maxPadeDegree] = { 
-    { 0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L,  // degree 3
-      0.2777777777777777777777777777777778L },
-    { 0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L,  // degree 4
-      0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L },
-    { 0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L,  // degree 5
-      0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
-      0.1184634425280945437571320203599587L },
-    { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L,  // degree 6
-      0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
-      0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L },
-    { 0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L,  // degree 7
-      0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
-      0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
-      0.0647424830844348466353057163395410L },
-    { 0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L,  // degree 8
-      0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
-      0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
-      0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L },
-    { 0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L,  // degree 9
-      0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
-      0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
-      0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
-      0.0406371941807872059859460790552618L },
-    { 0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L,  // degree 10
-      0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
-      0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
-      0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
-      0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L },
-    { 0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L,  // degree 11
-      0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
-      0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
-      0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
-      0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
-      0.0278342835580868332413768602212743L } };
+  const RealScalar weights[][maxPadeDegree] = {
+      {0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L,  // degree 3
+       0.2777777777777777777777777777777778L},
+      {0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L,  // degree 4
+       0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L},
+      {0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L,  // degree 5
+       0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
+       0.1184634425280945437571320203599587L},
+      {0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L,  // degree 6
+       0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
+       0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L},
+      {0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L,  // degree 7
+       0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
+       0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
+       0.0647424830844348466353057163395410L},
+      {0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L,  // degree 8
+       0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
+       0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
+       0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L},
+      {0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L,  // degree 9
+       0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
+       0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
+       0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
+       0.0406371941807872059859460790552618L},
+      {0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L,  // degree 10
+       0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
+       0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
+       0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
+       0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L},
+      {0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L,  // degree 11
+       0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
+       0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
+       0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
+       0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
+       0.0278342835580868332413768602212743L}};
 
   MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
   result.setZero(T.rows(), T.rows());
   for (int k = 0; k < degree; ++k) {
-    RealScalar weight = weights[degree-minPadeDegree][k];
-    RealScalar node = nodes[degree-minPadeDegree][k];
-    result += weight * (MatrixType::Identity(T.rows(), T.rows()) + node * TminusI)
-                       .template triangularView<Upper>().solve(TminusI);
+    RealScalar weight = weights[degree - minPadeDegree][k];
+    RealScalar node = nodes[degree - minPadeDegree][k];
+    result +=
+        weight *
+        (MatrixType::Identity(T.rows(), T.rows()) + node * TminusI).template triangularView<Upper>().solve(TminusI);
   }
-} 
+}
 
-/** \brief Compute logarithm of triangular matrices with size > 2. 
-  * \details This uses a inverse scale-and-square algorithm. */
+/** \brief Compute logarithm of triangular matrices with size > 2.
+ * \details This uses a inverse scale-and-square algorithm. */
 template <typename MatrixType>
-void matrix_log_compute_big(const MatrixType& A, MatrixType& result)
-{
+void matrix_log_compute_big(const MatrixType& A, MatrixType& result) {
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   using std::pow;
@@ -234,20 +234,21 @@
   MatrixType T = A, sqrtT;
 
   const int maxPadeDegree = matrix_log_max_pade_degree<Scalar>::value;
-  const RealScalar maxNormForPade = RealScalar(
-                                    maxPadeDegree<= 5? 5.3149729967117310e-1L:                    // single precision
-                                    maxPadeDegree<= 7? 2.6429608311114350e-1L:                    // double precision
-                                    maxPadeDegree<= 8? 2.32777776523703892094e-1L:                // extended precision
-                                    maxPadeDegree<=10? 1.05026503471351080481093652651105e-1L:    // double-double
-                                                       1.1880960220216759245467951592883642e-1L); // quadruple precision
+  const RealScalar maxNormForPade = RealScalar(maxPadeDegree <= 5 ? 5.3149729967117310e-1L :  // single precision
+                                                   maxPadeDegree <= 7 ? 2.6429608311114350e-1L
+                                                                      :  // double precision
+                                                   maxPadeDegree <= 8 ? 2.32777776523703892094e-1L
+                                                                      :  // extended precision
+                                                   maxPadeDegree <= 10 ? 1.05026503471351080481093652651105e-1L
+                                                                       :                       // double-double
+                                                   1.1880960220216759245467951592883642e-1L);  // quadruple precision
 
   while (true) {
     RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff();
     if (normTminusI < maxNormForPade) {
       degree = matrix_log_get_pade_degree(normTminusI);
       int degree2 = matrix_log_get_pade_degree(normTminusI / RealScalar(2));
-      if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) 
-        break;
+      if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) break;
       ++numberOfExtraSquareRoots;
     }
     matrix_sqrt_triangular(T, sqrtT);
@@ -256,35 +257,33 @@
   }
 
   matrix_log_compute_pade(result, T, degree);
-  result *= pow(RealScalar(2), RealScalar(numberOfSquareRoots)); // TODO replace by bitshift if possible
+  result *= pow(RealScalar(2), RealScalar(numberOfSquareRoots));  // TODO replace by bitshift if possible
 }
 
 /** \ingroup MatrixFunctions_Module
-  * \class MatrixLogarithmAtomic
-  * \brief Helper class for computing matrix logarithm of atomic matrices.
-  *
-  * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
-  *
-  * \sa class MatrixFunctionAtomic, MatrixBase::log()
-  */
+ * \class MatrixLogarithmAtomic
+ * \brief Helper class for computing matrix logarithm of atomic matrices.
+ *
+ * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
+ *
+ * \sa class MatrixFunctionAtomic, MatrixBase::log()
+ */
 template <typename MatrixType>
-class MatrixLogarithmAtomic
-{
-public:
+class MatrixLogarithmAtomic {
+ public:
   /** \brief Compute matrix logarithm of atomic matrix
-    * \param[in]  A  argument of matrix logarithm, should be upper triangular and atomic
-    * \returns  The logarithm of \p A.
-    */
+   * \param[in]  A  argument of matrix logarithm, should be upper triangular and atomic
+   * \returns  The logarithm of \p A.
+   */
   MatrixType compute(const MatrixType& A);
 };
 
 template <typename MatrixType>
-MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A)
-{
+MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A) {
   using std::log;
   MatrixType result(A.rows(), A.rows());
   if (A.rows() == 1)
-    result(0,0) = log(A(0,0));
+    result(0, 0) = log(A(0, 0));
   else if (A.rows() == 2)
     matrix_log_compute_2x2(A, result);
   else
@@ -292,82 +291,76 @@
   return result;
 }
 
-} // end of namespace internal
+}  // end of namespace internal
 
 /** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix logarithm of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix function.
-  *
-  * This class holds the argument to the matrix function until it is
-  * assigned or evaluated for some other reason (so the argument
-  * should not be changed in the meantime). It is the return type of
-  * MatrixBase::log() and most of the time this is the only way it
-  * is used.
-  */
-template<typename Derived> class MatrixLogarithmReturnValue
-: public ReturnByValue<MatrixLogarithmReturnValue<Derived> >
-{
-public:
+ *
+ * \brief Proxy for the matrix logarithm of some matrix (expression).
+ *
+ * \tparam Derived  Type of the argument to the matrix function.
+ *
+ * This class holds the argument to the matrix function until it is
+ * assigned or evaluated for some other reason (so the argument
+ * should not be changed in the meantime). It is the return type of
+ * MatrixBase::log() and most of the time this is the only way it
+ * is used.
+ */
+template <typename Derived>
+class MatrixLogarithmReturnValue : public ReturnByValue<MatrixLogarithmReturnValue<Derived> > {
+ public:
   typedef typename Derived::Scalar Scalar;
   typedef typename Derived::Index Index;
 
-protected:
+ protected:
   typedef typename internal::ref_selector<Derived>::type DerivedNested;
 
-public:
-
+ public:
   /** \brief Constructor.
-    *
-    * \param[in]  A  %Matrix (expression) forming the argument of the matrix logarithm.
-    */
-  explicit MatrixLogarithmReturnValue(const Derived& A) : m_A(A) { }
-  
+   *
+   * \param[in]  A  %Matrix (expression) forming the argument of the matrix logarithm.
+   */
+  explicit MatrixLogarithmReturnValue(const Derived& A) : m_A(A) {}
+
   /** \brief Compute the matrix logarithm.
-    *
-    * \param[out]  result  Logarithm of \c A, where \c A is as specified in the constructor.
-    */
+   *
+   * \param[out]  result  Logarithm of \c A, where \c A is as specified in the constructor.
+   */
   template <typename ResultType>
-  inline void evalTo(ResultType& result) const
-  {
+  inline void evalTo(ResultType& result) const {
     typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
-    typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
+    typedef internal::remove_all_t<DerivedEvalType> DerivedEvalTypeClean;
     typedef internal::traits<DerivedEvalTypeClean> Traits;
     typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
-    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime> DynMatrixType;
+    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime>
+        DynMatrixType;
     typedef internal::MatrixLogarithmAtomic<DynMatrixType> AtomicType;
     AtomicType atomic;
-    
+
     internal::matrix_function_compute<typename DerivedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
   }
 
   Index rows() const { return m_A.rows(); }
   Index cols() const { return m_A.cols(); }
-  
-private:
+
+ private:
   const DerivedNested m_A;
 };
 
 namespace internal {
-  template<typename Derived>
-  struct traits<MatrixLogarithmReturnValue<Derived> >
-  {
-    typedef typename Derived::PlainObject ReturnType;
-  };
-}
-
+template <typename Derived>
+struct traits<MatrixLogarithmReturnValue<Derived> > {
+  typedef typename Derived::PlainObject ReturnType;
+};
+}  // namespace internal
 
 /********** MatrixBase method **********/
 
-
 template <typename Derived>
-const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
-{
+const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const {
   eigen_assert(rows() == cols());
   return MatrixLogarithmReturnValue<Derived>(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATRIX_LOGARITHM
+#endif  // EIGEN_MATRIX_LOGARITHM
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
index d7672d7..bff619a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
@@ -10,9 +10,13 @@
 #ifndef EIGEN_MATRIX_POWER
 #define EIGEN_MATRIX_POWER
 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
 namespace Eigen {
 
-template<typename MatrixType> class MatrixPower;
+template <typename MatrixType>
+class MatrixPower;
 
 /**
  * \ingroup MatrixFunctions_Module
@@ -35,36 +39,35 @@
  * template<typename MatrixType>
  * struct traits<MatrixPower<MatrixType>::ReturnValue>;
  */
-template<typename MatrixType>
-class MatrixPowerParenthesesReturnValue : public ReturnByValue< MatrixPowerParenthesesReturnValue<MatrixType> >
-{
-  public:
-    typedef typename MatrixType::RealScalar RealScalar;
+template <typename MatrixType>
+class MatrixPowerParenthesesReturnValue : public ReturnByValue<MatrixPowerParenthesesReturnValue<MatrixType> > {
+ public:
+  typedef typename MatrixType::RealScalar RealScalar;
 
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] pow  %MatrixPower storing the base.
-     * \param[in] p    scalar, the exponent of the matrix power.
-     */
-    MatrixPowerParenthesesReturnValue(MatrixPower<MatrixType>& pow, RealScalar p) : m_pow(pow), m_p(p)
-    { }
+  /**
+   * \brief Constructor.
+   *
+   * \param[in] pow  %MatrixPower storing the base.
+   * \param[in] p    scalar, the exponent of the matrix power.
+   */
+  MatrixPowerParenthesesReturnValue(MatrixPower<MatrixType>& pow, RealScalar p) : m_pow(pow), m_p(p) {}
 
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[out] result
-     */
-    template<typename ResultType>
-    inline void evalTo(ResultType& result) const
-    { m_pow.compute(result, m_p); }
+  /**
+   * \brief Compute the matrix power.
+   *
+   * \param[out] result
+   */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    m_pow.compute(result, m_p);
+  }
 
-    Index rows() const { return m_pow.rows(); }
-    Index cols() const { return m_pow.cols(); }
+  Index rows() const { return m_pow.rows(); }
+  Index cols() const { return m_pow.cols(); }
 
-  private:
-    MatrixPower<MatrixType>& m_pow;
-    const RealScalar m_p;
+ private:
+  MatrixPower<MatrixType>& m_pow;
+  const RealScalar m_p;
 };
 
 /**
@@ -82,71 +85,64 @@
  * insist that this be nested into MatrixPower. This class is here to
  * facilitate future development of triangular matrix functions.
  */
-template<typename MatrixType>
-class MatrixPowerAtomic : internal::noncopyable
-{
-  private:
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime
-    };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef std::complex<RealScalar> ComplexScalar;
-    typedef Block<MatrixType,Dynamic,Dynamic> ResultType;
+template <typename MatrixType>
+class MatrixPowerAtomic : internal::noncopyable {
+ private:
+  enum { RowsAtCompileTime = MatrixType::RowsAtCompileTime, MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef std::complex<RealScalar> ComplexScalar;
+  typedef Block<MatrixType, Dynamic, Dynamic> ResultType;
 
-    const MatrixType& m_A;
-    RealScalar m_p;
+  const MatrixType& m_A;
+  RealScalar m_p;
 
-    void computePade(int degree, const MatrixType& IminusT, ResultType& res) const;
-    void compute2x2(ResultType& res, RealScalar p) const;
-    void computeBig(ResultType& res) const;
-    static int getPadeDegree(float normIminusT);
-    static int getPadeDegree(double normIminusT);
-    static int getPadeDegree(long double normIminusT);
-    static ComplexScalar computeSuperDiag(const ComplexScalar&, const ComplexScalar&, RealScalar p);
-    static RealScalar computeSuperDiag(RealScalar, RealScalar, RealScalar p);
+  void computePade(int degree, const MatrixType& IminusT, ResultType& res) const;
+  void compute2x2(ResultType& res, RealScalar p) const;
+  void computeBig(ResultType& res) const;
+  static int getPadeDegree(float normIminusT);
+  static int getPadeDegree(double normIminusT);
+  static int getPadeDegree(long double normIminusT);
+  static ComplexScalar computeSuperDiag(const ComplexScalar&, const ComplexScalar&, RealScalar p);
+  static RealScalar computeSuperDiag(RealScalar, RealScalar, RealScalar p);
 
-  public:
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] T  the base of the matrix power.
-     * \param[in] p  the exponent of the matrix power, should be in
-     * \f$ (-1, 1) \f$.
-     *
-     * The class stores a reference to T, so it should not be changed
-     * (or destroyed) before evaluation. Only the upper triangular
-     * part of T is read.
-     */
-    MatrixPowerAtomic(const MatrixType& T, RealScalar p);
-    
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[out] res  \f$ A^p \f$ where A and p are specified in the
-     * constructor.
-     */
-    void compute(ResultType& res) const;
+ public:
+  /**
+   * \brief Constructor.
+   *
+   * \param[in] T  the base of the matrix power.
+   * \param[in] p  the exponent of the matrix power, should be in
+   * \f$ (-1, 1) \f$.
+   *
+   * The class stores a reference to T, so it should not be changed
+   * (or destroyed) before evaluation. Only the upper triangular
+   * part of T is read.
+   */
+  MatrixPowerAtomic(const MatrixType& T, RealScalar p);
+
+  /**
+   * \brief Compute the matrix power.
+   *
+   * \param[out] res  \f$ A^p \f$ where A and p are specified in the
+   * constructor.
+   */
+  void compute(ResultType& res) const;
 };
 
-template<typename MatrixType>
-MatrixPowerAtomic<MatrixType>::MatrixPowerAtomic(const MatrixType& T, RealScalar p) :
-  m_A(T), m_p(p)
-{
+template <typename MatrixType>
+MatrixPowerAtomic<MatrixType>::MatrixPowerAtomic(const MatrixType& T, RealScalar p) : m_A(T), m_p(p) {
   eigen_assert(T.rows() == T.cols());
   eigen_assert(p > -1 && p < 1);
 }
 
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::compute(ResultType& res) const
-{
+template <typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::compute(ResultType& res) const {
   using std::pow;
   switch (m_A.rows()) {
     case 0:
       break;
     case 1:
-      res(0,0) = pow(m_A(0,0), m_p);
+      res(0, 0) = pow(m_A(0, 0), m_p);
       break;
     case 2:
       compute2x2(res, m_p);
@@ -156,66 +152,68 @@
   }
 }
 
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::computePade(int degree, const MatrixType& IminusT, ResultType& res) const
-{
-  int i = 2*degree;
-  res = (m_p-RealScalar(degree)) / RealScalar(2*i-2) * IminusT;
+template <typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::computePade(int degree, const MatrixType& IminusT, ResultType& res) const {
+  int i = 2 * degree;
+  res = (m_p - RealScalar(degree)) / RealScalar(2 * i - 2) * IminusT;
 
   for (--i; i; --i) {
-    res = (MatrixType::Identity(IminusT.rows(), IminusT.cols()) + res).template triangularView<Upper>()
-	.solve((i==1 ? -m_p : i&1 ? (-m_p-RealScalar(i/2))/RealScalar(2*i) : (m_p-RealScalar(i/2))/RealScalar(2*i-2)) * IminusT).eval();
+    res = (MatrixType::Identity(IminusT.rows(), IminusT.cols()) + res)
+              .template triangularView<Upper>()
+              .solve((i == 1  ? -m_p
+                      : i & 1 ? (-m_p - RealScalar(i / 2)) / RealScalar(2 * i)
+                              : (m_p - RealScalar(i / 2)) / RealScalar(2 * i - 2)) *
+                     IminusT)
+              .eval();
   }
   res += MatrixType::Identity(IminusT.rows(), IminusT.cols());
 }
 
 // This function assumes that res has the correct size (see bug 614)
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::compute2x2(ResultType& res, RealScalar p) const
-{
+template <typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::compute2x2(ResultType& res, RealScalar p) const {
   using std::abs;
   using std::pow;
-  res.coeffRef(0,0) = pow(m_A.coeff(0,0), p);
+  res.coeffRef(0, 0) = pow(m_A.coeff(0, 0), p);
 
-  for (Index i=1; i < m_A.cols(); ++i) {
-    res.coeffRef(i,i) = pow(m_A.coeff(i,i), p);
-    if (m_A.coeff(i-1,i-1) == m_A.coeff(i,i))
-      res.coeffRef(i-1,i) = p * pow(m_A.coeff(i,i), p-1);
-    else if (2*abs(m_A.coeff(i-1,i-1)) < abs(m_A.coeff(i,i)) || 2*abs(m_A.coeff(i,i)) < abs(m_A.coeff(i-1,i-1)))
-      res.coeffRef(i-1,i) = (res.coeff(i,i)-res.coeff(i-1,i-1)) / (m_A.coeff(i,i)-m_A.coeff(i-1,i-1));
+  for (Index i = 1; i < m_A.cols(); ++i) {
+    res.coeffRef(i, i) = pow(m_A.coeff(i, i), p);
+    if (m_A.coeff(i - 1, i - 1) == m_A.coeff(i, i))
+      res.coeffRef(i - 1, i) = p * pow(m_A.coeff(i, i), p - 1);
+    else if (2 * abs(m_A.coeff(i - 1, i - 1)) < abs(m_A.coeff(i, i)) ||
+             2 * abs(m_A.coeff(i, i)) < abs(m_A.coeff(i - 1, i - 1)))
+      res.coeffRef(i - 1, i) =
+          (res.coeff(i, i) - res.coeff(i - 1, i - 1)) / (m_A.coeff(i, i) - m_A.coeff(i - 1, i - 1));
     else
-      res.coeffRef(i-1,i) = computeSuperDiag(m_A.coeff(i,i), m_A.coeff(i-1,i-1), p);
-    res.coeffRef(i-1,i) *= m_A.coeff(i-1,i);
+      res.coeffRef(i - 1, i) = computeSuperDiag(m_A.coeff(i, i), m_A.coeff(i - 1, i - 1), p);
+    res.coeffRef(i - 1, i) *= m_A.coeff(i - 1, i);
   }
 }
 
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::computeBig(ResultType& res) const
-{
+template <typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::computeBig(ResultType& res) const {
   using std::ldexp;
   const int digits = std::numeric_limits<RealScalar>::digits;
-  const RealScalar maxNormForPade = RealScalar(
-                                    digits <=  24? 4.3386528e-1L                            // single precision
-                                  : digits <=  53? 2.789358995219730e-1L                    // double precision
-                                  : digits <=  64? 2.4471944416607995472e-1L                // extended precision
-                                  : digits <= 106? 1.1016843812851143391275867258512e-1L    // double-double
-                                  :                9.134603732914548552537150753385375e-2L); // quadruple precision
+  const RealScalar maxNormForPade =
+      RealScalar(digits <= 24    ? 4.3386528e-1L                              // single precision
+                 : digits <= 53  ? 2.789358995219730e-1L                      // double precision
+                 : digits <= 64  ? 2.4471944416607995472e-1L                  // extended precision
+                 : digits <= 106 ? 1.1016843812851143391275867258512e-1L      // double-double
+                                 : 9.134603732914548552537150753385375e-2L);  // quadruple precision
   MatrixType IminusT, sqrtT, T = m_A.template triangularView<Upper>();
   RealScalar normIminusT;
   int degree, degree2, numberOfSquareRoots = 0;
   bool hasExtraSquareRoot = false;
 
-  for (Index i=0; i < m_A.cols(); ++i)
-    eigen_assert(m_A(i,i) != RealScalar(0));
+  for (Index i = 0; i < m_A.cols(); ++i) eigen_assert(m_A(i, i) != RealScalar(0));
 
   while (true) {
     IminusT = MatrixType::Identity(m_A.rows(), m_A.cols()) - T;
     normIminusT = IminusT.cwiseAbs().colwise().sum().maxCoeff();
     if (normIminusT < maxNormForPade) {
       degree = getPadeDegree(normIminusT);
-      degree2 = getPadeDegree(normIminusT/2);
-      if (degree - degree2 <= 1 || hasExtraSquareRoot)
-	break;
+      degree2 = getPadeDegree(normIminusT / 2);
+      if (degree - degree2 <= 1 || hasExtraSquareRoot) break;
       hasExtraSquareRoot = true;
     }
     matrix_sqrt_triangular(T, sqrtT);
@@ -230,66 +228,70 @@
   }
   compute2x2(res, m_p);
 }
-  
-template<typename MatrixType>
-inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(float normIminusT)
-{
-  const float maxNormForPade[] = { 2.8064004e-1f /* degree = 3 */ , 4.3386528e-1f };
+
+template <typename MatrixType>
+inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(float normIminusT) {
+  const float maxNormForPade[] = {2.8064004e-1f /* degree = 3 */, 4.3386528e-1f};
   int degree = 3;
   for (; degree <= 4; ++degree)
-    if (normIminusT <= maxNormForPade[degree - 3])
-      break;
+    if (normIminusT <= maxNormForPade[degree - 3]) break;
   return degree;
 }
 
-template<typename MatrixType>
-inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(double normIminusT)
-{
-  const double maxNormForPade[] = { 1.884160592658218e-2 /* degree = 3 */ , 6.038881904059573e-2, 1.239917516308172e-1,
-      1.999045567181744e-1, 2.789358995219730e-1 };
+template <typename MatrixType>
+inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(double normIminusT) {
+  const double maxNormForPade[] = {1.884160592658218e-2 /* degree = 3 */, 6.038881904059573e-2, 1.239917516308172e-1,
+                                   1.999045567181744e-1, 2.789358995219730e-1};
   int degree = 3;
   for (; degree <= 7; ++degree)
-    if (normIminusT <= maxNormForPade[degree - 3])
-      break;
+    if (normIminusT <= maxNormForPade[degree - 3]) break;
   return degree;
 }
 
-template<typename MatrixType>
-inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(long double normIminusT)
-{
-#if   LDBL_MANT_DIG == 53
+template <typename MatrixType>
+inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(long double normIminusT) {
+#if LDBL_MANT_DIG == 53
   const int maxPadeDegree = 7;
-  const double maxNormForPade[] = { 1.884160592658218e-2L /* degree = 3 */ , 6.038881904059573e-2L, 1.239917516308172e-1L,
-      1.999045567181744e-1L, 2.789358995219730e-1L };
+  const double maxNormForPade[] = {1.884160592658218e-2L /* degree = 3 */, 6.038881904059573e-2L, 1.239917516308172e-1L,
+                                   1.999045567181744e-1L, 2.789358995219730e-1L};
 #elif LDBL_MANT_DIG <= 64
   const int maxPadeDegree = 8;
-  const long double maxNormForPade[] = { 6.3854693117491799460e-3L /* degree = 3 */ , 2.6394893435456973676e-2L,
-      6.4216043030404063729e-2L, 1.1701165502926694307e-1L, 1.7904284231268670284e-1L, 2.4471944416607995472e-1L };
+  const long double maxNormForPade[] = {6.3854693117491799460e-3L /* degree = 3 */,
+                                        2.6394893435456973676e-2L,
+                                        6.4216043030404063729e-2L,
+                                        1.1701165502926694307e-1L,
+                                        1.7904284231268670284e-1L,
+                                        2.4471944416607995472e-1L};
 #elif LDBL_MANT_DIG <= 106
   const int maxPadeDegree = 10;
-  const double maxNormForPade[] = { 1.0007161601787493236741409687186e-4L /* degree = 3 */ ,
-      1.0007161601787493236741409687186e-3L, 4.7069769360887572939882574746264e-3L, 1.3220386624169159689406653101695e-2L,
-      2.8063482381631737920612944054906e-2L, 4.9625993951953473052385361085058e-2L, 7.7367040706027886224557538328171e-2L,
-      1.1016843812851143391275867258512e-1L };
+  const double maxNormForPade[] = {1.0007161601787493236741409687186e-4L /* degree = 3 */,
+                                   1.0007161601787493236741409687186e-3L,
+                                   4.7069769360887572939882574746264e-3L,
+                                   1.3220386624169159689406653101695e-2L,
+                                   2.8063482381631737920612944054906e-2L,
+                                   4.9625993951953473052385361085058e-2L,
+                                   7.7367040706027886224557538328171e-2L,
+                                   1.1016843812851143391275867258512e-1L};
 #else
   const int maxPadeDegree = 10;
-  const double maxNormForPade[] = { 5.524506147036624377378713555116378e-5L /* degree = 3 */ ,
-      6.640600568157479679823602193345995e-4L, 3.227716520106894279249709728084626e-3L,
-      9.619593944683432960546978734646284e-3L, 2.134595382433742403911124458161147e-2L,
-      3.908166513900489428442993794761185e-2L, 6.266780814639442865832535460550138e-2L,
-      9.134603732914548552537150753385375e-2L };
+  const double maxNormForPade[] = {5.524506147036624377378713555116378e-5L /* degree = 3 */,
+                                   6.640600568157479679823602193345995e-4L,
+                                   3.227716520106894279249709728084626e-3L,
+                                   9.619593944683432960546978734646284e-3L,
+                                   2.134595382433742403911124458161147e-2L,
+                                   3.908166513900489428442993794761185e-2L,
+                                   6.266780814639442865832535460550138e-2L,
+                                   9.134603732914548552537150753385375e-2L};
 #endif
   int degree = 3;
   for (; degree <= maxPadeDegree; ++degree)
-    if (normIminusT <= maxNormForPade[degree - 3])
-      break;
+    if (normIminusT <= static_cast<long double>(maxNormForPade[degree - 3])) break;
   return degree;
 }
 
-template<typename MatrixType>
-inline typename MatrixPowerAtomic<MatrixType>::ComplexScalar
-MatrixPowerAtomic<MatrixType>::computeSuperDiag(const ComplexScalar& curr, const ComplexScalar& prev, RealScalar p)
-{
+template <typename MatrixType>
+inline typename MatrixPowerAtomic<MatrixType>::ComplexScalar MatrixPowerAtomic<MatrixType>::computeSuperDiag(
+    const ComplexScalar& curr, const ComplexScalar& prev, RealScalar p) {
   using std::ceil;
   using std::exp;
   using std::log;
@@ -297,20 +299,21 @@
 
   ComplexScalar logCurr = log(curr);
   ComplexScalar logPrev = log(prev);
-  RealScalar unwindingNumber = ceil((numext::imag(logCurr - logPrev) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI));
-  ComplexScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2) + ComplexScalar(0, RealScalar(EIGEN_PI)*unwindingNumber);
+  RealScalar unwindingNumber =
+      ceil((numext::imag(logCurr - logPrev) - RealScalar(EIGEN_PI)) / RealScalar(2 * EIGEN_PI));
+  ComplexScalar w =
+      numext::log1p((curr - prev) / prev) / RealScalar(2) + ComplexScalar(0, RealScalar(EIGEN_PI) * unwindingNumber);
   return RealScalar(2) * exp(RealScalar(0.5) * p * (logCurr + logPrev)) * sinh(p * w) / (curr - prev);
 }
 
-template<typename MatrixType>
-inline typename MatrixPowerAtomic<MatrixType>::RealScalar
-MatrixPowerAtomic<MatrixType>::computeSuperDiag(RealScalar curr, RealScalar prev, RealScalar p)
-{
+template <typename MatrixType>
+inline typename MatrixPowerAtomic<MatrixType>::RealScalar MatrixPowerAtomic<MatrixType>::computeSuperDiag(
+    RealScalar curr, RealScalar prev, RealScalar p) {
   using std::exp;
   using std::log;
   using std::sinh;
 
-  RealScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2);
+  RealScalar w = numext::log1p((curr - prev) / prev) / RealScalar(2);
   return 2 * exp(p * (log(curr) + log(prev)) / 2) * sinh(p * w) / (curr - prev);
 }
 
@@ -333,126 +336,118 @@
  * \include MatrixPower_optimal.cpp
  * Output: \verbinclude MatrixPower_optimal.out
  */
-template<typename MatrixType>
-class MatrixPower : internal::noncopyable
-{
-  private:
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
+template <typename MatrixType>
+class MatrixPower : internal::noncopyable {
+ private:
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
 
-  public:
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] A  the base of the matrix power.
-     *
-     * The class stores a reference to A, so it should not be changed
-     * (or destroyed) before evaluation.
-     */
-    explicit MatrixPower(const MatrixType& A) :
-      m_A(A),
-      m_conditionNumber(0),
-      m_rank(A.cols()),
-      m_nulls(0)
-    { eigen_assert(A.rows() == A.cols()); }
+ public:
+  /**
+   * \brief Constructor.
+   *
+   * \param[in] A  the base of the matrix power.
+   *
+   * The class stores a reference to A, so it should not be changed
+   * (or destroyed) before evaluation.
+   */
+  explicit MatrixPower(const MatrixType& A) : m_A(A), m_conditionNumber(0), m_rank(A.cols()), m_nulls(0) {
+    eigen_assert(A.rows() == A.cols());
+  }
 
-    /**
-     * \brief Returns the matrix power.
-     *
-     * \param[in] p  exponent, a real scalar.
-     * \return The expression \f$ A^p \f$, where A is specified in the
-     * constructor.
-     */
-    const MatrixPowerParenthesesReturnValue<MatrixType> operator()(RealScalar p)
-    { return MatrixPowerParenthesesReturnValue<MatrixType>(*this, p); }
+  /**
+   * \brief Returns the matrix power.
+   *
+   * \param[in] p  exponent, a real scalar.
+   * \return The expression \f$ A^p \f$, where A is specified in the
+   * constructor.
+   */
+  const MatrixPowerParenthesesReturnValue<MatrixType> operator()(RealScalar p) {
+    return MatrixPowerParenthesesReturnValue<MatrixType>(*this, p);
+  }
 
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[in]  p    exponent, a real scalar.
-     * \param[out] res  \f$ A^p \f$ where A is specified in the
-     * constructor.
-     */
-    template<typename ResultType>
-    void compute(ResultType& res, RealScalar p);
-    
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
+  /**
+   * \brief Compute the matrix power.
+   *
+   * \param[in]  p    exponent, a real scalar.
+   * \param[out] res  \f$ A^p \f$ where A is specified in the
+   * constructor.
+   */
+  template <typename ResultType>
+  void compute(ResultType& res, RealScalar p);
 
-  private:
-    typedef std::complex<RealScalar> ComplexScalar;
-    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0,
-              MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime> ComplexMatrix;
+  Index rows() const { return m_A.rows(); }
+  Index cols() const { return m_A.cols(); }
 
-    /** \brief Reference to the base of matrix power. */
-    typename MatrixType::Nested m_A;
+ private:
+  typedef std::complex<RealScalar> ComplexScalar;
+  typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime>
+      ComplexMatrix;
 
-    /** \brief Temporary storage. */
-    MatrixType m_tmp;
+  /** \brief Reference to the base of matrix power. */
+  typename MatrixType::Nested m_A;
 
-    /** \brief Store the result of Schur decomposition. */
-    ComplexMatrix m_T, m_U;
-    
-    /** \brief Store fractional power of m_T. */
-    ComplexMatrix m_fT;
+  /** \brief Temporary storage. */
+  MatrixType m_tmp;
 
-    /**
-     * \brief Condition number of m_A.
-     *
-     * It is initialized as 0 to avoid performing unnecessary Schur
-     * decomposition, which is the bottleneck.
-     */
-    RealScalar m_conditionNumber;
+  /** \brief Store the result of Schur decomposition. */
+  ComplexMatrix m_T, m_U;
 
-    /** \brief Rank of m_A. */
-    Index m_rank;
-    
-    /** \brief Rank deficiency of m_A. */
-    Index m_nulls;
+  /** \brief Store fractional power of m_T. */
+  ComplexMatrix m_fT;
 
-    /**
-     * \brief Split p into integral part and fractional part.
-     *
-     * \param[in]  p        The exponent.
-     * \param[out] p        The fractional part ranging in \f$ (-1, 1) \f$.
-     * \param[out] intpart  The integral part.
-     *
-     * Only if the fractional part is nonzero, it calls initialize().
-     */
-    void split(RealScalar& p, RealScalar& intpart);
+  /**
+   * \brief Condition number of m_A.
+   *
+   * It is initialized as 0 to avoid performing unnecessary Schur
+   * decomposition, which is the bottleneck.
+   */
+  RealScalar m_conditionNumber;
 
-    /** \brief Perform Schur decomposition for fractional power. */
-    void initialize();
+  /** \brief Rank of m_A. */
+  Index m_rank;
 
-    template<typename ResultType>
-    void computeIntPower(ResultType& res, RealScalar p);
+  /** \brief Rank deficiency of m_A. */
+  Index m_nulls;
 
-    template<typename ResultType>
-    void computeFracPower(ResultType& res, RealScalar p);
+  /**
+   * \brief Split p into integral part and fractional part.
+   *
+   * \param[in]  p        The exponent.
+   * \param[out] p        The fractional part ranging in \f$ (-1, 1) \f$.
+   * \param[out] intpart  The integral part.
+   *
+   * Only if the fractional part is nonzero, it calls initialize().
+   */
+  void split(RealScalar& p, RealScalar& intpart);
 
-    template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-    static void revertSchur(
-        Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-        const ComplexMatrix& T,
-        const ComplexMatrix& U);
+  /** \brief Perform Schur decomposition for fractional power. */
+  void initialize();
 
-    template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-    static void revertSchur(
-        Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-        const ComplexMatrix& T,
-        const ComplexMatrix& U);
+  template <typename ResultType>
+  void computeIntPower(ResultType& res, RealScalar p);
+
+  template <typename ResultType>
+  void computeFracPower(ResultType& res, RealScalar p);
+
+  template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  static void revertSchur(Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res, const ComplexMatrix& T,
+                          const ComplexMatrix& U);
+
+  template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  static void revertSchur(Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res, const ComplexMatrix& T,
+                          const ComplexMatrix& U);
 };
 
-template<typename MatrixType>
-template<typename ResultType>
-void MatrixPower<MatrixType>::compute(ResultType& res, RealScalar p)
-{
+template <typename MatrixType>
+template <typename ResultType>
+void MatrixPower<MatrixType>::compute(ResultType& res, RealScalar p) {
   using std::pow;
   switch (cols()) {
     case 0:
       break;
     case 1:
-      res(0,0) = pow(m_A.coeff(0,0), p);
+      res(0, 0) = pow(m_A.coeff(0, 0), p);
       break;
     default:
       RealScalar intpart;
@@ -464,9 +459,8 @@
   }
 }
 
-template<typename MatrixType>
-void MatrixPower<MatrixType>::split(RealScalar& p, RealScalar& intpart)
-{
+template <typename MatrixType>
+void MatrixPower<MatrixType>::split(RealScalar& p, RealScalar& intpart) {
   using std::floor;
   using std::pow;
 
@@ -475,19 +469,17 @@
 
   // Perform Schur decomposition if it is not yet performed and the power is
   // not an integer.
-  if (!m_conditionNumber && p)
-    initialize();
+  if (!m_conditionNumber && p) initialize();
 
   // Choose the more stable of intpart = floor(p) and intpart = ceil(p).
-  if (p > RealScalar(0.5) && p > (1-p) * pow(m_conditionNumber, p)) {
+  if (p > RealScalar(0.5) && p > (1 - p) * pow(m_conditionNumber, p)) {
     --p;
     ++intpart;
   }
 }
 
-template<typename MatrixType>
-void MatrixPower<MatrixType>::initialize()
-{
+template <typename MatrixType>
+void MatrixPower<MatrixType>::initialize() {
   const ComplexSchur<MatrixType> schurOfA(m_A);
   JacobiRotation<ComplexScalar> rot;
   ComplexScalar eigenvalue;
@@ -498,18 +490,17 @@
   m_conditionNumber = m_T.diagonal().array().abs().maxCoeff() / m_T.diagonal().array().abs().minCoeff();
 
   // Move zero eigenvalues to the bottom right corner.
-  for (Index i = cols()-1; i>=0; --i) {
-    if (m_rank <= 2)
-      return;
-    if (m_T.coeff(i,i) == RealScalar(0)) {
-      for (Index j=i+1; j < m_rank; ++j) {
-        eigenvalue = m_T.coeff(j,j);
-        rot.makeGivens(m_T.coeff(j-1,j), eigenvalue);
-        m_T.applyOnTheRight(j-1, j, rot);
-        m_T.applyOnTheLeft(j-1, j, rot.adjoint());
-        m_T.coeffRef(j-1,j-1) = eigenvalue;
-        m_T.coeffRef(j,j) = RealScalar(0);
-        m_U.applyOnTheRight(j-1, j, rot);
+  for (Index i = cols() - 1; i >= 0; --i) {
+    if (m_rank <= 2) return;
+    if (m_T.coeff(i, i) == RealScalar(0)) {
+      for (Index j = i + 1; j < m_rank; ++j) {
+        eigenvalue = m_T.coeff(j, j);
+        rot.makeGivens(m_T.coeff(j - 1, j), eigenvalue);
+        m_T.applyOnTheRight(j - 1, j, rot);
+        m_T.applyOnTheLeft(j - 1, j, rot.adjoint());
+        m_T.coeffRef(j - 1, j - 1) = eigenvalue;
+        m_T.coeffRef(j, j) = RealScalar(0);
+        m_U.applyOnTheRight(j - 1, j, rot);
       }
       --m_rank;
     }
@@ -517,67 +508,62 @@
 
   m_nulls = rows() - m_rank;
   if (m_nulls) {
-    eigen_assert(m_T.bottomRightCorner(m_nulls, m_nulls).isZero()
-        && "Base of matrix power should be invertible or with a semisimple zero eigenvalue.");
+    eigen_assert(m_T.bottomRightCorner(m_nulls, m_nulls).isZero() &&
+                 "Base of matrix power should be invertible or with a semisimple zero eigenvalue.");
     m_fT.bottomRows(m_nulls).fill(RealScalar(0));
   }
 }
 
-template<typename MatrixType>
-template<typename ResultType>
-void MatrixPower<MatrixType>::computeIntPower(ResultType& res, RealScalar p)
-{
+template <typename MatrixType>
+template <typename ResultType>
+void MatrixPower<MatrixType>::computeIntPower(ResultType& res, RealScalar p) {
   using std::abs;
   using std::fmod;
   RealScalar pp = abs(p);
 
-  if (p<0) 
+  if (p < 0)
     m_tmp = m_A.inverse();
-  else     
+  else
     m_tmp = m_A;
 
   while (true) {
-    if (fmod(pp, 2) >= 1)
-      res = m_tmp * res;
+    if (fmod(pp, 2) >= 1) res = m_tmp * res;
     pp /= 2;
-    if (pp < 1)
-      break;
+    if (pp < 1) break;
     m_tmp *= m_tmp;
   }
 }
 
-template<typename MatrixType>
-template<typename ResultType>
-void MatrixPower<MatrixType>::computeFracPower(ResultType& res, RealScalar p)
-{
-  Block<ComplexMatrix,Dynamic,Dynamic> blockTp(m_fT, 0, 0, m_rank, m_rank);
+template <typename MatrixType>
+template <typename ResultType>
+void MatrixPower<MatrixType>::computeFracPower(ResultType& res, RealScalar p) {
+  Block<ComplexMatrix, Dynamic, Dynamic> blockTp(m_fT, 0, 0, m_rank, m_rank);
   eigen_assert(m_conditionNumber);
   eigen_assert(m_rank + m_nulls == rows());
 
   MatrixPowerAtomic<ComplexMatrix>(m_T.topLeftCorner(m_rank, m_rank), p).compute(blockTp);
   if (m_nulls) {
-    m_fT.topRightCorner(m_rank, m_nulls) = m_T.topLeftCorner(m_rank, m_rank).template triangularView<Upper>()
-        .solve(blockTp * m_T.topRightCorner(m_rank, m_nulls));
+    m_fT.topRightCorner(m_rank, m_nulls) = m_T.topLeftCorner(m_rank, m_rank)
+                                               .template triangularView<Upper>()
+                                               .solve(blockTp * m_T.topRightCorner(m_rank, m_nulls));
   }
   revertSchur(m_tmp, m_fT, m_U);
   res = m_tmp * res;
 }
 
-template<typename MatrixType>
-template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-inline void MatrixPower<MatrixType>::revertSchur(
-    Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-    const ComplexMatrix& T,
-    const ComplexMatrix& U)
-{ res.noalias() = U * (T.template triangularView<Upper>() * U.adjoint()); }
+template <typename MatrixType>
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+inline void MatrixPower<MatrixType>::revertSchur(Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
+                                                 const ComplexMatrix& T, const ComplexMatrix& U) {
+  res.noalias() = U * (T.template triangularView<Upper>() * U.adjoint());
+}
 
-template<typename MatrixType>
-template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-inline void MatrixPower<MatrixType>::revertSchur(
-    Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-    const ComplexMatrix& T,
-    const ComplexMatrix& U)
-{ res.noalias() = (U * (T.template triangularView<Upper>() * U.adjoint())).real(); }
+template <typename MatrixType>
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+inline void MatrixPower<MatrixType>::revertSchur(Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
+                                                 const ComplexMatrix& T, const ComplexMatrix& U) {
+  res.noalias() = (U * (T.template triangularView<Upper>() * U.adjoint())).real();
+}
 
 /**
  * \ingroup MatrixFunctions_Module
@@ -592,38 +578,37 @@
  * MatrixBase::pow() and related functions and most of the
  * time this is the only way it is used.
  */
-template<typename Derived>
-class MatrixPowerReturnValue : public ReturnByValue< MatrixPowerReturnValue<Derived> >
-{
-  public:
-    typedef typename Derived::PlainObject PlainObject;
-    typedef typename Derived::RealScalar RealScalar;
+template <typename Derived>
+class MatrixPowerReturnValue : public ReturnByValue<MatrixPowerReturnValue<Derived> > {
+ public:
+  typedef typename Derived::PlainObject PlainObject;
+  typedef typename Derived::RealScalar RealScalar;
 
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] A  %Matrix (expression), the base of the matrix power.
-     * \param[in] p  real scalar, the exponent of the matrix power.
-     */
-    MatrixPowerReturnValue(const Derived& A, RealScalar p) : m_A(A), m_p(p)
-    { }
+  /**
+   * \brief Constructor.
+   *
+   * \param[in] A  %Matrix (expression), the base of the matrix power.
+   * \param[in] p  real scalar, the exponent of the matrix power.
+   */
+  MatrixPowerReturnValue(const Derived& A, RealScalar p) : m_A(A), m_p(p) {}
 
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
-     * constructor.
-     */
-    template<typename ResultType>
-    inline void evalTo(ResultType& result) const
-    { MatrixPower<PlainObject>(m_A.eval()).compute(result, m_p); }
+  /**
+   * \brief Compute the matrix power.
+   *
+   * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
+   * constructor.
+   */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    MatrixPower<PlainObject>(m_A.eval()).compute(result, m_p);
+  }
 
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
+  Index rows() const { return m_A.rows(); }
+  Index cols() const { return m_A.cols(); }
 
-  private:
-    const Derived& m_A;
-    const RealScalar m_p;
+ private:
+  const Derived& m_A;
+  const RealScalar m_p;
 };
 
 /**
@@ -639,67 +624,71 @@
  * MatrixBase::pow() and related functions and most of the
  * time this is the only way it is used.
  */
-template<typename Derived>
-class MatrixComplexPowerReturnValue : public ReturnByValue< MatrixComplexPowerReturnValue<Derived> >
-{
-  public:
-    typedef typename Derived::PlainObject PlainObject;
-    typedef typename std::complex<typename Derived::RealScalar> ComplexScalar;
+template <typename Derived>
+class MatrixComplexPowerReturnValue : public ReturnByValue<MatrixComplexPowerReturnValue<Derived> > {
+ public:
+  typedef typename Derived::PlainObject PlainObject;
+  typedef typename std::complex<typename Derived::RealScalar> ComplexScalar;
 
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] A  %Matrix (expression), the base of the matrix power.
-     * \param[in] p  complex scalar, the exponent of the matrix power.
-     */
-    MatrixComplexPowerReturnValue(const Derived& A, const ComplexScalar& p) : m_A(A), m_p(p)
-    { }
+  /**
+   * \brief Constructor.
+   *
+   * \param[in] A  %Matrix (expression), the base of the matrix power.
+   * \param[in] p  complex scalar, the exponent of the matrix power.
+   */
+  MatrixComplexPowerReturnValue(const Derived& A, const ComplexScalar& p) : m_A(A), m_p(p) {}
 
-    /**
-     * \brief Compute the matrix power.
-     *
-     * Because \p p is complex, \f$ A^p \f$ is simply evaluated as \f$
-     * \exp(p \log(A)) \f$.
-     *
-     * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
-     * constructor.
-     */
-    template<typename ResultType>
-    inline void evalTo(ResultType& result) const
-    { result = (m_p * m_A.log()).exp(); }
+  /**
+   * \brief Compute the matrix power.
+   *
+   * Because \p p is complex, \f$ A^p \f$ is simply evaluated as \f$
+   * \exp(p \log(A)) \f$.
+   *
+   * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
+   * constructor.
+   */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    result = (m_p * m_A.log()).exp();
+  }
 
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
+  Index rows() const { return m_A.rows(); }
+  Index cols() const { return m_A.cols(); }
 
-  private:
-    const Derived& m_A;
-    const ComplexScalar m_p;
+ private:
+  const Derived& m_A;
+  const ComplexScalar m_p;
 };
 
 namespace internal {
 
-template<typename MatrixPowerType>
-struct traits< MatrixPowerParenthesesReturnValue<MatrixPowerType> >
-{ typedef typename MatrixPowerType::PlainObject ReturnType; };
+template <typename MatrixPowerType>
+struct traits<MatrixPowerParenthesesReturnValue<MatrixPowerType> > {
+  typedef typename MatrixPowerType::PlainObject ReturnType;
+};
 
-template<typename Derived>
-struct traits< MatrixPowerReturnValue<Derived> >
-{ typedef typename Derived::PlainObject ReturnType; };
+template <typename Derived>
+struct traits<MatrixPowerReturnValue<Derived> > {
+  typedef typename Derived::PlainObject ReturnType;
+};
 
-template<typename Derived>
-struct traits< MatrixComplexPowerReturnValue<Derived> >
-{ typedef typename Derived::PlainObject ReturnType; };
+template <typename Derived>
+struct traits<MatrixComplexPowerReturnValue<Derived> > {
+  typedef typename Derived::PlainObject ReturnType;
+};
 
+}  // namespace internal
+
+template <typename Derived>
+const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(const RealScalar& p) const {
+  return MatrixPowerReturnValue<Derived>(derived(), p);
 }
 
-template<typename Derived>
-const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(const RealScalar& p) const
-{ return MatrixPowerReturnValue<Derived>(derived(), p); }
+template <typename Derived>
+const MatrixComplexPowerReturnValue<Derived> MatrixBase<Derived>::pow(const std::complex<RealScalar>& p) const {
+  return MatrixComplexPowerReturnValue<Derived>(derived(), p);
+}
 
-template<typename Derived>
-const MatrixComplexPowerReturnValue<Derived> MatrixBase<Derived>::pow(const std::complex<RealScalar>& p) const
-{ return MatrixComplexPowerReturnValue<Derived>(derived(), p); }
+}  // namespace Eigen
 
-} // namespace Eigen
-
-#endif // EIGEN_MATRIX_POWER
+#endif  // EIGEN_MATRIX_POWER
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
index e363e77..b11eb74 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
@@ -10,123 +10,116 @@
 #ifndef EIGEN_MATRIX_SQUARE_ROOT
 #define EIGEN_MATRIX_SQUARE_ROOT
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 // pre:  T.block(i,i,2,2) has complex conjugate eigenvalues
 // post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2)
 template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType& T, Index i, ResultType& sqrtT)
-{
+void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType& T, Index i, ResultType& sqrtT) {
   // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere
   //       in EigenSolver. If we expose it, we could call it directly from here.
   typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,2,2> block = T.template block<2,2>(i,i);
-  EigenSolver<Matrix<Scalar,2,2> > es(block);
-  sqrtT.template block<2,2>(i,i)
-    = (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
+  Matrix<Scalar, 2, 2> block = T.template block<2, 2>(i, i);
+  EigenSolver<Matrix<Scalar, 2, 2> > es(block);
+  sqrtT.template block<2, 2>(i, i) =
+      (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
 }
 
 // pre:  block structure of T is such that (i,j) is a 1x1 block,
 //       all blocks of sqrtT to left of and below (i,j) are correct
 // post: sqrtT(i,j) has the correct value
 template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
-{
+void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
   typedef typename traits<MatrixType>::Scalar Scalar;
-  Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value();
-  sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j));
+  Scalar tmp = (sqrtT.row(i).segment(i + 1, j - i - 1) * sqrtT.col(j).segment(i + 1, j - i - 1)).value();
+  sqrtT.coeffRef(i, j) = (T.coeff(i, j) - tmp) / (sqrtT.coeff(i, i) + sqrtT.coeff(j, j));
 }
 
 // similar to compute1x1offDiagonalBlock()
 template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
-{
+void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
   typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j);
-  if (j-i > 1)
-    rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2);
-  Matrix<Scalar,2,2> A = sqrtT.coeff(i,i) * Matrix<Scalar,2,2>::Identity();
-  A += sqrtT.template block<2,2>(j,j).transpose();
-  sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose());
+  Matrix<Scalar, 1, 2> rhs = T.template block<1, 2>(i, j);
+  if (j - i > 1) rhs -= sqrtT.block(i, i + 1, 1, j - i - 1) * sqrtT.block(i + 1, j, j - i - 1, 2);
+  Matrix<Scalar, 2, 2> A = sqrtT.coeff(i, i) * Matrix<Scalar, 2, 2>::Identity();
+  A += sqrtT.template block<2, 2>(j, j).transpose();
+  sqrtT.template block<1, 2>(i, j).transpose() = A.fullPivLu().solve(rhs.transpose());
 }
 
 // similar to compute1x1offDiagonalBlock()
 template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
-{
+void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
   typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j);
-  if (j-i > 2)
-    rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1);
-  Matrix<Scalar,2,2> A = sqrtT.coeff(j,j) * Matrix<Scalar,2,2>::Identity();
-  A += sqrtT.template block<2,2>(i,i);
-  sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs);
+  Matrix<Scalar, 2, 1> rhs = T.template block<2, 1>(i, j);
+  if (j - i > 2) rhs -= sqrtT.block(i, i + 2, 2, j - i - 2) * sqrtT.block(i + 2, j, j - i - 2, 1);
+  Matrix<Scalar, 2, 2> A = sqrtT.coeff(j, j) * Matrix<Scalar, 2, 2>::Identity();
+  A += sqrtT.template block<2, 2>(i, i);
+  sqrtT.template block<2, 1>(i, j) = A.fullPivLu().solve(rhs);
 }
 
 // solves the equation A X + X B = C where all matrices are 2-by-2
 template <typename MatrixType>
-void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType& X, const MatrixType& A, const MatrixType& B, const MatrixType& C)
-{
+void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType& X, const MatrixType& A, const MatrixType& B,
+                                                           const MatrixType& C) {
   typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero();
-  coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0);
-  coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1);
-  coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0);
-  coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1);
-  coeffMatrix.coeffRef(0,1) = B.coeff(1,0);
-  coeffMatrix.coeffRef(0,2) = A.coeff(0,1);
-  coeffMatrix.coeffRef(1,0) = B.coeff(0,1);
-  coeffMatrix.coeffRef(1,3) = A.coeff(0,1);
-  coeffMatrix.coeffRef(2,0) = A.coeff(1,0);
-  coeffMatrix.coeffRef(2,3) = B.coeff(1,0);
-  coeffMatrix.coeffRef(3,1) = A.coeff(1,0);
-  coeffMatrix.coeffRef(3,2) = B.coeff(0,1);
+  Matrix<Scalar, 4, 4> coeffMatrix = Matrix<Scalar, 4, 4>::Zero();
+  coeffMatrix.coeffRef(0, 0) = A.coeff(0, 0) + B.coeff(0, 0);
+  coeffMatrix.coeffRef(1, 1) = A.coeff(0, 0) + B.coeff(1, 1);
+  coeffMatrix.coeffRef(2, 2) = A.coeff(1, 1) + B.coeff(0, 0);
+  coeffMatrix.coeffRef(3, 3) = A.coeff(1, 1) + B.coeff(1, 1);
+  coeffMatrix.coeffRef(0, 1) = B.coeff(1, 0);
+  coeffMatrix.coeffRef(0, 2) = A.coeff(0, 1);
+  coeffMatrix.coeffRef(1, 0) = B.coeff(0, 1);
+  coeffMatrix.coeffRef(1, 3) = A.coeff(0, 1);
+  coeffMatrix.coeffRef(2, 0) = A.coeff(1, 0);
+  coeffMatrix.coeffRef(2, 3) = B.coeff(1, 0);
+  coeffMatrix.coeffRef(3, 1) = A.coeff(1, 0);
+  coeffMatrix.coeffRef(3, 2) = B.coeff(0, 1);
 
-  Matrix<Scalar,4,1> rhs;
-  rhs.coeffRef(0) = C.coeff(0,0);
-  rhs.coeffRef(1) = C.coeff(0,1);
-  rhs.coeffRef(2) = C.coeff(1,0);
-  rhs.coeffRef(3) = C.coeff(1,1);
+  Matrix<Scalar, 4, 1> rhs;
+  rhs.coeffRef(0) = C.coeff(0, 0);
+  rhs.coeffRef(1) = C.coeff(0, 1);
+  rhs.coeffRef(2) = C.coeff(1, 0);
+  rhs.coeffRef(3) = C.coeff(1, 1);
 
-  Matrix<Scalar,4,1> result;
+  Matrix<Scalar, 4, 1> result;
   result = coeffMatrix.fullPivLu().solve(rhs);
 
-  X.coeffRef(0,0) = result.coeff(0);
-  X.coeffRef(0,1) = result.coeff(1);
-  X.coeffRef(1,0) = result.coeff(2);
-  X.coeffRef(1,1) = result.coeff(3);
+  X.coeffRef(0, 0) = result.coeff(0);
+  X.coeffRef(0, 1) = result.coeff(1);
+  X.coeffRef(1, 0) = result.coeff(2);
+  X.coeffRef(1, 1) = result.coeff(3);
 }
 
 // similar to compute1x1offDiagonalBlock()
 template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
-{
+void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
   typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i);
-  Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j);
-  Matrix<Scalar,2,2> C = T.template block<2,2>(i,j);
-  if (j-i > 2)
-    C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2);
-  Matrix<Scalar,2,2> X;
+  Matrix<Scalar, 2, 2> A = sqrtT.template block<2, 2>(i, i);
+  Matrix<Scalar, 2, 2> B = sqrtT.template block<2, 2>(j, j);
+  Matrix<Scalar, 2, 2> C = T.template block<2, 2>(i, j);
+  if (j - i > 2) C -= sqrtT.block(i, i + 2, 2, j - i - 2) * sqrtT.block(i + 2, j, j - i - 2, 2);
+  Matrix<Scalar, 2, 2> X;
   matrix_sqrt_quasi_triangular_solve_auxiliary_equation(X, A, B, C);
-  sqrtT.template block<2,2>(i,j) = X;
+  sqrtT.template block<2, 2>(i, j) = X;
 }
 
 // pre:  T is quasi-upper-triangular and sqrtT is a zero matrix of the same size
 // post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
 template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT)
-{
+void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT) {
   using std::sqrt;
   const Index size = T.rows();
   for (Index i = 0; i < size; i++) {
-    if (i == size - 1 || T.coeff(i+1, i) == 0) {
-      eigen_assert(T(i,i) >= 0);
-      sqrtT.coeffRef(i,i) = sqrt(T.coeff(i,i));
-    }
-    else {
+    if (i == size - 1 || T.coeff(i + 1, i) == 0) {
+      eigen_assert(T(i, i) >= 0);
+      sqrtT.coeffRef(i, i) = sqrt(T.coeff(i, i));
+    } else {
       matrix_sqrt_quasi_triangular_2x2_diagonal_block(T, i, sqrtT);
       ++i;
     }
@@ -136,73 +129,69 @@
 // pre:  T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T.
 // post: sqrtT is the square root of T.
 template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT)
-{
+void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT) {
   const Index size = T.rows();
   for (Index j = 1; j < size; j++) {
-      if (T.coeff(j, j-1) != 0)  // if T(j-1:j, j-1:j) is a 2-by-2 block
-	continue;
-    for (Index i = j-1; i >= 0; i--) {
-      if (i > 0 && T.coeff(i, i-1) != 0)  // if T(i-1:i, i-1:i) is a 2-by-2 block
-	continue;
-      bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0);
-      bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0);
-      if (iBlockIs2x2 && jBlockIs2x2) 
+    if (T.coeff(j, j - 1) != 0)  // if T(j-1:j, j-1:j) is a 2-by-2 block
+      continue;
+    for (Index i = j - 1; i >= 0; i--) {
+      if (i > 0 && T.coeff(i, i - 1) != 0)  // if T(i-1:i, i-1:i) is a 2-by-2 block
+        continue;
+      bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i + 1, i) != 0);
+      bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j + 1, j) != 0);
+      if (iBlockIs2x2 && jBlockIs2x2)
         matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(T, i, j, sqrtT);
-      else if (iBlockIs2x2 && !jBlockIs2x2) 
+      else if (iBlockIs2x2 && !jBlockIs2x2)
         matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(T, i, j, sqrtT);
-      else if (!iBlockIs2x2 && jBlockIs2x2) 
+      else if (!iBlockIs2x2 && jBlockIs2x2)
         matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(T, i, j, sqrtT);
-      else if (!iBlockIs2x2 && !jBlockIs2x2) 
+      else if (!iBlockIs2x2 && !jBlockIs2x2)
         matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(T, i, j, sqrtT);
     }
   }
 }
 
-} // end of namespace internal
+}  // end of namespace internal
 
 /** \ingroup MatrixFunctions_Module
-  * \brief Compute matrix square root of quasi-triangular matrix.
-  *
-  * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
-  *                      expected to be an instantiation of the Matrix class template.
-  * \tparam  ResultType  type of \p result, where result is to be stored.
-  * \param[in]  arg      argument of matrix square root.
-  * \param[out] result   matrix square root of upper Hessenberg part of \p arg.
-  *
-  * This function computes the square root of the upper quasi-triangular matrix stored in the upper
-  * Hessenberg part of \p arg.  Only the upper Hessenberg part of \p result is updated, the rest is
-  * not touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
-  *
-  * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
-  */
-template <typename MatrixType, typename ResultType> 
-void matrix_sqrt_quasi_triangular(const MatrixType &arg, ResultType &result)
-{
+ * \brief Compute matrix square root of quasi-triangular matrix.
+ *
+ * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
+ *                      expected to be an instantiation of the Matrix class template.
+ * \tparam  ResultType  type of \p result, where result is to be stored.
+ * \param[in]  arg      argument of matrix square root.
+ * \param[out] result   matrix square root of upper Hessenberg part of \p arg.
+ *
+ * This function computes the square root of the upper quasi-triangular matrix stored in the upper
+ * Hessenberg part of \p arg.  Only the upper Hessenberg part of \p result is updated, the rest is
+ * not touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
+ *
+ * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
+ */
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular(const MatrixType& arg, ResultType& result) {
   eigen_assert(arg.rows() == arg.cols());
   result.resize(arg.rows(), arg.cols());
   internal::matrix_sqrt_quasi_triangular_diagonal(arg, result);
   internal::matrix_sqrt_quasi_triangular_off_diagonal(arg, result);
 }
 
-
 /** \ingroup MatrixFunctions_Module
-  * \brief Compute matrix square root of triangular matrix.
-  *
-  * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
-  *                      expected to be an instantiation of the Matrix class template.
-  * \tparam  ResultType  type of \p result, where result is to be stored.
-  * \param[in]  arg      argument of matrix square root.
-  * \param[out] result   matrix square root of upper triangular part of \p arg.
-  *
-  * Only the upper triangular part (including the diagonal) of \p result is updated, the rest is not
-  * touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
-  *
-  * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
-  */
-template <typename MatrixType, typename ResultType> 
-void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
-{
+ * \brief Compute matrix square root of triangular matrix.
+ *
+ * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
+ *                      expected to be an instantiation of the Matrix class template.
+ * \tparam  ResultType  type of \p result, where result is to be stored.
+ * \param[in]  arg      argument of matrix square root.
+ * \param[out] result   matrix square root of upper triangular part of \p arg.
+ *
+ * Only the upper triangular part (including the diagonal) of \p result is updated, the rest is not
+ * touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
+ *
+ * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
+ */
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_triangular(const MatrixType& arg, ResultType& result) {
   using std::sqrt;
   typedef typename MatrixType::Scalar Scalar;
 
@@ -212,157 +201,146 @@
   // This uses that the square root of triangular matrices can be computed directly.
   result.resize(arg.rows(), arg.cols());
   for (Index i = 0; i < arg.rows(); i++) {
-    result.coeffRef(i,i) = sqrt(arg.coeff(i,i));
+    result.coeffRef(i, i) = sqrt(arg.coeff(i, i));
   }
   for (Index j = 1; j < arg.cols(); j++) {
-    for (Index i = j-1; i >= 0; i--) {
+    for (Index i = j - 1; i >= 0; i--) {
       // if i = j-1, then segment has length 0 so tmp = 0
-      Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value();
+      Scalar tmp = (result.row(i).segment(i + 1, j - i - 1) * result.col(j).segment(i + 1, j - i - 1)).value();
       // denominator may be zero if original matrix is singular
-      result.coeffRef(i,j) = (arg.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j));
+      result.coeffRef(i, j) = (arg.coeff(i, j) - tmp) / (result.coeff(i, i) + result.coeff(j, j));
     }
   }
 }
 
-
 namespace internal {
 
 /** \ingroup MatrixFunctions_Module
-  * \brief Helper struct for computing matrix square roots of general matrices.
-  * \tparam  MatrixType  type of the argument of the matrix square root,
-  *                      expected to be an instantiation of the Matrix class template.
-  *
-  * \sa MatrixSquareRootTriangular, MatrixSquareRootQuasiTriangular, MatrixBase::sqrt()
-  */
+ * \brief Helper struct for computing matrix square roots of general matrices.
+ * \tparam  MatrixType  type of the argument of the matrix square root,
+ *                      expected to be an instantiation of the Matrix class template.
+ *
+ * \sa MatrixSquareRootTriangular, MatrixSquareRootQuasiTriangular, MatrixBase::sqrt()
+ */
 template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
-struct matrix_sqrt_compute
-{
+struct matrix_sqrt_compute {
   /** \brief Compute the matrix square root
-    *
-    * \param[in]  arg     matrix whose square root is to be computed.
-    * \param[out] result  square root of \p arg.
-    *
-    * See MatrixBase::sqrt() for details on how this computation is implemented.
-    */
-  template <typename ResultType> static void run(const MatrixType &arg, ResultType &result);    
+   *
+   * \param[in]  arg     matrix whose square root is to be computed.
+   * \param[out] result  square root of \p arg.
+   *
+   * See MatrixBase::sqrt() for details on how this computation is implemented.
+   */
+  template <typename ResultType>
+  static void run(const MatrixType& arg, ResultType& result);
 };
 
-
 // ********** Partial specialization for real matrices **********
 
 template <typename MatrixType>
-struct matrix_sqrt_compute<MatrixType, 0>
-{
+struct matrix_sqrt_compute<MatrixType, 0> {
   typedef typename MatrixType::PlainObject PlainType;
   template <typename ResultType>
-  static void run(const MatrixType &arg, ResultType &result)
-  {
+  static void run(const MatrixType& arg, ResultType& result) {
     eigen_assert(arg.rows() == arg.cols());
 
     // Compute Schur decomposition of arg
     const RealSchur<PlainType> schurOfA(arg);
     const PlainType& T = schurOfA.matrixT();
     const PlainType& U = schurOfA.matrixU();
-    
+
     // Compute square root of T
     PlainType sqrtT = PlainType::Zero(arg.rows(), arg.cols());
     matrix_sqrt_quasi_triangular(T, sqrtT);
-    
+
     // Compute square root of arg
     result = U * sqrtT * U.adjoint();
   }
 };
 
-
 // ********** Partial specialization for complex matrices **********
 
 template <typename MatrixType>
-struct matrix_sqrt_compute<MatrixType, 1>
-{
+struct matrix_sqrt_compute<MatrixType, 1> {
   typedef typename MatrixType::PlainObject PlainType;
   template <typename ResultType>
-  static void run(const MatrixType &arg, ResultType &result)
-  {
+  static void run(const MatrixType& arg, ResultType& result) {
     eigen_assert(arg.rows() == arg.cols());
 
     // Compute Schur decomposition of arg
     const ComplexSchur<PlainType> schurOfA(arg);
     const PlainType& T = schurOfA.matrixT();
     const PlainType& U = schurOfA.matrixU();
-    
+
     // Compute square root of T
     PlainType sqrtT;
     matrix_sqrt_triangular(T, sqrtT);
-    
+
     // Compute square root of arg
     result = U * (sqrtT.template triangularView<Upper>() * U.adjoint());
   }
 };
 
-} // end namespace internal
+}  // end namespace internal
 
 /** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix square root of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix square root.
-  *
-  * This class holds the argument to the matrix square root until it
-  * is assigned or evaluated for some other reason (so the argument
-  * should not be changed in the meantime). It is the return type of
-  * MatrixBase::sqrt() and most of the time this is the only way it is
-  * used.
-  */
-template<typename Derived> class MatrixSquareRootReturnValue
-: public ReturnByValue<MatrixSquareRootReturnValue<Derived> >
-{
-  protected:
-    typedef typename internal::ref_selector<Derived>::type DerivedNested;
+ *
+ * \brief Proxy for the matrix square root of some matrix (expression).
+ *
+ * \tparam Derived  Type of the argument to the matrix square root.
+ *
+ * This class holds the argument to the matrix square root until it
+ * is assigned or evaluated for some other reason (so the argument
+ * should not be changed in the meantime). It is the return type of
+ * MatrixBase::sqrt() and most of the time this is the only way it is
+ * used.
+ */
+template <typename Derived>
+class MatrixSquareRootReturnValue : public ReturnByValue<MatrixSquareRootReturnValue<Derived> > {
+ protected:
+  typedef typename internal::ref_selector<Derived>::type DerivedNested;
 
-  public:
-    /** \brief Constructor.
-      *
-      * \param[in]  src  %Matrix (expression) forming the argument of the
-      * matrix square root.
-      */
-    explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { }
+ public:
+  /** \brief Constructor.
+   *
+   * \param[in]  src  %Matrix (expression) forming the argument of the
+   * matrix square root.
+   */
+  explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) {}
 
-    /** \brief Compute the matrix square root.
-      *
-      * \param[out]  result  the matrix square root of \p src in the
-      * constructor.
-      */
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
-      typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
-      DerivedEvalType tmp(m_src);
-      internal::matrix_sqrt_compute<DerivedEvalTypeClean>::run(tmp, result);
-    }
+  /** \brief Compute the matrix square root.
+   *
+   * \param[out]  result  the matrix square root of \p src in the
+   * constructor.
+   */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const {
+    typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
+    typedef internal::remove_all_t<DerivedEvalType> DerivedEvalTypeClean;
+    DerivedEvalType tmp(m_src);
+    internal::matrix_sqrt_compute<DerivedEvalTypeClean>::run(tmp, result);
+  }
 
-    Index rows() const { return m_src.rows(); }
-    Index cols() const { return m_src.cols(); }
+  Index rows() const { return m_src.rows(); }
+  Index cols() const { return m_src.cols(); }
 
-  protected:
-    const DerivedNested m_src;
+ protected:
+  const DerivedNested m_src;
 };
 
 namespace internal {
-template<typename Derived>
-struct traits<MatrixSquareRootReturnValue<Derived> >
-{
+template <typename Derived>
+struct traits<MatrixSquareRootReturnValue<Derived> > {
   typedef typename Derived::PlainObject ReturnType;
 };
-}
+}  // namespace internal
 
 template <typename Derived>
-const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
-{
+const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const {
   eigen_assert(rows() == cols());
   return MatrixSquareRootReturnValue<Derived>(derived());
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_MATRIX_FUNCTION
+#endif  // EIGEN_MATRIX_FUNCTION
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
index 7604df9..8050c37 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
@@ -10,108 +10,106 @@
 #ifndef EIGEN_STEM_FUNCTION
 #define EIGEN_STEM_FUNCTION
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 namespace internal {
 
 /** \brief The exponential function (and its derivatives). */
 template <typename Scalar>
-Scalar stem_function_exp(Scalar x, int)
-{
+Scalar stem_function_exp(Scalar x, int) {
   using std::exp;
   return exp(x);
 }
 
 /** \brief Cosine (and its derivatives). */
 template <typename Scalar>
-Scalar stem_function_cos(Scalar x, int n)
-{
+Scalar stem_function_cos(Scalar x, int n) {
   using std::cos;
   using std::sin;
   Scalar res;
 
   switch (n % 4) {
-  case 0: 
-    res = std::cos(x);
-    break;
-  case 1:
-    res = -std::sin(x);
-    break;
-  case 2:
-    res = -std::cos(x);
-    break;
-  case 3:
-    res = std::sin(x);
-    break;
+    case 0:
+      res = std::cos(x);
+      break;
+    case 1:
+      res = -std::sin(x);
+      break;
+    case 2:
+      res = -std::cos(x);
+      break;
+    case 3:
+      res = std::sin(x);
+      break;
   }
   return res;
 }
 
 /** \brief Sine (and its derivatives). */
 template <typename Scalar>
-Scalar stem_function_sin(Scalar x, int n)
-{
+Scalar stem_function_sin(Scalar x, int n) {
   using std::cos;
   using std::sin;
   Scalar res;
 
   switch (n % 4) {
-  case 0:
-    res = std::sin(x);
-    break;
-  case 1:
-    res = std::cos(x);
-    break;
-  case 2:
-    res = -std::sin(x);
-    break;
-  case 3:
-    res = -std::cos(x);
-    break;
+    case 0:
+      res = std::sin(x);
+      break;
+    case 1:
+      res = std::cos(x);
+      break;
+    case 2:
+      res = -std::sin(x);
+      break;
+    case 3:
+      res = -std::cos(x);
+      break;
   }
   return res;
 }
 
 /** \brief Hyperbolic cosine (and its derivatives). */
 template <typename Scalar>
-Scalar stem_function_cosh(Scalar x, int n)
-{
+Scalar stem_function_cosh(Scalar x, int n) {
   using std::cosh;
   using std::sinh;
   Scalar res;
-  
+
   switch (n % 2) {
-  case 0:
-    res = std::cosh(x);
-    break;
-  case 1:
-    res = std::sinh(x);
-    break;
+    case 0:
+      res = std::cosh(x);
+      break;
+    case 1:
+      res = std::sinh(x);
+      break;
   }
   return res;
 }
-	
+
 /** \brief Hyperbolic sine (and its derivatives). */
 template <typename Scalar>
-Scalar stem_function_sinh(Scalar x, int n)
-{
+Scalar stem_function_sinh(Scalar x, int n) {
   using std::cosh;
   using std::sinh;
   Scalar res;
-  
+
   switch (n % 2) {
-  case 0:
-    res = std::sinh(x);
-    break;
-  case 1:
-    res = std::cosh(x);
-    break;
+    case 0:
+      res = std::sinh(x);
+      break;
+    case 1:
+      res = std::cosh(x);
+      break;
   }
   return res;
 }
 
-} // end namespace internal
+}  // end namespace internal
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_STEM_FUNCTION
+#endif  // EIGEN_STEM_FUNCTION
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
index 650d05d..8ea7b2a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
@@ -23,82 +23,79 @@
 
 #include "gcem_incl/gcem_options.hpp"
 
-namespace gcem
-{
-    #include "gcem_incl/quadrature/gauss_legendre_50.hpp"
+#include "gcem_incl/quadrature/gauss_legendre_50.hpp"
 
-    #include "gcem_incl/is_inf.hpp"
-    #include "gcem_incl/is_nan.hpp"
-    #include "gcem_incl/is_finite.hpp"
-    
-    #include "gcem_incl/signbit.hpp"
-    #include "gcem_incl/copysign.hpp"
-    #include "gcem_incl/neg_zero.hpp"
-    #include "gcem_incl/sgn.hpp"
+#include "gcem_incl/is_inf.hpp"
+#include "gcem_incl/is_nan.hpp"
+#include "gcem_incl/is_finite.hpp"
 
-    #include "gcem_incl/abs.hpp"
-    #include "gcem_incl/ceil.hpp"
-    #include "gcem_incl/floor.hpp"
-    #include "gcem_incl/trunc.hpp"
-    #include "gcem_incl/is_odd.hpp"
-    #include "gcem_incl/is_even.hpp"
-    #include "gcem_incl/max.hpp"
-    #include "gcem_incl/min.hpp"
-    #include "gcem_incl/sqrt.hpp"
-    #include "gcem_incl/inv_sqrt.hpp"
-    #include "gcem_incl/hypot.hpp"
+#include "gcem_incl/signbit.hpp"
+#include "gcem_incl/copysign.hpp"
+#include "gcem_incl/neg_zero.hpp"
+#include "gcem_incl/sgn.hpp"
 
-    #include "gcem_incl/find_exponent.hpp"
-    #include "gcem_incl/find_fraction.hpp"
-    #include "gcem_incl/find_whole.hpp"
-    #include "gcem_incl/mantissa.hpp"
-    #include "gcem_incl/round.hpp"
-    #include "gcem_incl/fmod.hpp"
+#include "gcem_incl/abs.hpp"
+#include "gcem_incl/ceil.hpp"
+#include "gcem_incl/floor.hpp"
+#include "gcem_incl/trunc.hpp"
+#include "gcem_incl/is_odd.hpp"
+#include "gcem_incl/is_even.hpp"
+#include "gcem_incl/max.hpp"
+#include "gcem_incl/min.hpp"
+#include "gcem_incl/sqrt.hpp"
+#include "gcem_incl/inv_sqrt.hpp"
+#include "gcem_incl/hypot.hpp"
 
-    #include "gcem_incl/pow_integral.hpp"
-    #include "gcem_incl/exp.hpp"
-    #include "gcem_incl/expm1.hpp"
-    #include "gcem_incl/log.hpp"
-    #include "gcem_incl/log1p.hpp"
-    #include "gcem_incl/log2.hpp"
-    #include "gcem_incl/log10.hpp"
-    #include "gcem_incl/pow.hpp"
+#include "gcem_incl/find_exponent.hpp"
+#include "gcem_incl/find_fraction.hpp"
+#include "gcem_incl/find_whole.hpp"
+#include "gcem_incl/mantissa.hpp"
+#include "gcem_incl/round.hpp"
+#include "gcem_incl/fmod.hpp"
 
-    #include "gcem_incl/gcd.hpp"
-    #include "gcem_incl/lcm.hpp"
+#include "gcem_incl/pow_integral.hpp"
+#include "gcem_incl/exp.hpp"
+#include "gcem_incl/expm1.hpp"
+#include "gcem_incl/log.hpp"
+#include "gcem_incl/log1p.hpp"
+#include "gcem_incl/log2.hpp"
+#include "gcem_incl/log10.hpp"
+#include "gcem_incl/pow.hpp"
 
-    #include "gcem_incl/tan.hpp"
-    #include "gcem_incl/cos.hpp"
-    #include "gcem_incl/sin.hpp"
+#include "gcem_incl/gcd.hpp"
+#include "gcem_incl/lcm.hpp"
 
-    #include "gcem_incl/atan.hpp"
-    #include "gcem_incl/atan2.hpp"
-    #include "gcem_incl/acos.hpp"
-    #include "gcem_incl/asin.hpp"
+#include "gcem_incl/tan.hpp"
+#include "gcem_incl/cos.hpp"
+#include "gcem_incl/sin.hpp"
 
-    #include "gcem_incl/tanh.hpp"
-    #include "gcem_incl/cosh.hpp"
-    #include "gcem_incl/sinh.hpp"
+#include "gcem_incl/atan.hpp"
+#include "gcem_incl/atan2.hpp"
+#include "gcem_incl/acos.hpp"
+#include "gcem_incl/asin.hpp"
 
-    #include "gcem_incl/atanh.hpp"
-    #include "gcem_incl/acosh.hpp"
-    #include "gcem_incl/asinh.hpp"
+#include "gcem_incl/tanh.hpp"
+#include "gcem_incl/cosh.hpp"
+#include "gcem_incl/sinh.hpp"
 
-    #include "gcem_incl/binomial_coef.hpp"
-    #include "gcem_incl/lgamma.hpp"
-    #include "gcem_incl/tgamma.hpp"
-    #include "gcem_incl/factorial.hpp"
-    #include "gcem_incl/lbeta.hpp"
-    #include "gcem_incl/beta.hpp"
-    #include "gcem_incl/lmgamma.hpp"
-    #include "gcem_incl/log_binomial_coef.hpp"
+#include "gcem_incl/atanh.hpp"
+#include "gcem_incl/acosh.hpp"
+#include "gcem_incl/asinh.hpp"
 
-    #include "gcem_incl/erf.hpp"
-    #include "gcem_incl/erf_inv.hpp"
-    #include "gcem_incl/incomplete_beta.hpp"
-    #include "gcem_incl/incomplete_beta_inv.hpp"
-    #include "gcem_incl/incomplete_gamma.hpp"
-    #include "gcem_incl/incomplete_gamma_inv.hpp"
-}
+#include "gcem_incl/binomial_coef.hpp"
+#include "gcem_incl/lgamma.hpp"
+#include "gcem_incl/tgamma.hpp"
+#include "gcem_incl/factorial.hpp"
+#include "gcem_incl/lbeta.hpp"
+#include "gcem_incl/beta.hpp"
+#include "gcem_incl/lmgamma.hpp"
+#include "gcem_incl/log_binomial_coef.hpp"
+
+#include "gcem_incl/erf.hpp"
+#include "gcem_incl/erf_inv.hpp"
+#include "gcem_incl/incomplete_beta.hpp"
+#include "gcem_incl/incomplete_beta_inv.hpp"
+#include "gcem_incl/incomplete_gamma.hpp"
+#include "gcem_incl/incomplete_gamma_inv.hpp"
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
index 6d7b66d..8e8330b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_abs_HPP
 #define _gcem_abs_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 /**
  * Compile-time absolute value function
  *
@@ -34,12 +40,18 @@
 abs(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return( // deal with signed-zeros
             x == T(0) ? \
                 T(0) :
             // else
             x < T(0) ? \
                 - x : x );
+  } else {
+    return std::abs(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
index a47003d..2e24d2a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_acos_HPP
 #define _gcem_acos_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -78,7 +84,13 @@
 acos(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::acos_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::acos(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
index 8767200..778c36c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_acosh_HPP
 #define _gcem_acosh_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -62,7 +68,13 @@
 acosh(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::acosh_compute( static_cast<return_t<T>>(x) );
+  } else {
+    return std::acosh(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
index 6a79e87..39784aa 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_asin_HPP
 #define _gcem_asin_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -76,7 +82,13 @@
 asin(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::asin_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::asin(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
index a5f3ff6..0a59693 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_asinh_HPP
 #define _gcem_asinh_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -59,8 +65,13 @@
 asinh(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::asinh_compute( static_cast<return_t<T>>(x) );
+  } else {
+    return std::asinh(x);
+  }
 }
 
+}
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
index 3f46974..19b9d4e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
@@ -29,6 +29,12 @@
 #ifndef _gcem_atan_HPP
 #define _gcem_atan_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -149,7 +155,13 @@
 atan(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::atan_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::atan(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
index 5ca55b7..07a02cf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_atan2_HPP
 #define _gcem_atan2_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -82,7 +88,13 @@
 atan2(const T1 y, const T2 x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::atan2_type_check(x,y);
+  } else {
+    return std::atan2(y, x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
index dfb4dc3..5c4475b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_atanh_HPP
 #define _gcem_atanh_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -73,7 +79,13 @@
 atanh(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::atanh_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::atanh(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
index e43d4fc..3439af1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_beta_HPP
 #define _gcem_beta_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 /**
  * Compile-time beta function
  *
@@ -36,7 +42,17 @@
 beta(const T1 a, const T2 b)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return exp( lbeta(a,b) );
+  } else {
+#ifdef __cpp_lib_math_special_functions
+    return std::beta(a, b);
+#else
+    return exp( lbeta(a,b) );
+#endif
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
index 0fc17f3..a1f764b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
@@ -21,6 +21,9 @@
 #ifndef _gcem_binomial_coef_HPP
 #define _gcem_binomial_coef_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -88,4 +91,6 @@
     return internal::binomial_coef_type_check(n,k);
 }
 
-#endif
\ No newline at end of file
+}
+
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
index ff1097b..2611e65 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_ceil_HPP
 #define _gcem_ceil_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -124,7 +130,13 @@
 ceil(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::ceil_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::ceil(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
index c1741f7..d118199 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_copysign_HPP
 #define _gcem_copysign_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 /**
  * Compile-time copy sign function
  *
@@ -35,7 +41,13 @@
 copysign(const T1 x, const T2 y)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return( signbit(x) != signbit(y) ? -x : x );
+  } else {
+    return std::copysign(x, y);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
index 82f4c60..be73b55 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_cos_HPP
 #define _gcem_cos_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -77,7 +83,13 @@
 cos(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::cos_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::cos(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
index fc89c0d..53e5bb1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_cosh_HPP
 #define _gcem_cosh_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -59,7 +65,13 @@
 cosh(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::cosh_compute( static_cast<return_t<T>>(x) );
+  } else {
+    return std::cosh(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
index d0bc83a..319669d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_erf_HPP
 #define _gcem_erf_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -137,7 +143,13 @@
 erf(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::erf_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::erf(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
index 412d686..c4f0d9d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
@@ -28,6 +28,9 @@
 #ifndef _gcem_erf_inv_HPP
 #define _gcem_erf_inv_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -260,5 +263,6 @@
     return internal::erf_inv_begin( static_cast<return_t<T>>(p) );
 }
 
+}
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
index 595ffc2..1708542 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_exp_HPP
 #define _gcem_exp_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -124,7 +130,13 @@
 exp(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::exp_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::exp(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
index 70c9ecf..6adddc9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_expm1_HPP
 #define _gcem_expm1_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -70,7 +76,13 @@
 expm1(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::expm1_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::expm1(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
index ffb9c82..977e697 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_factorial_HPP
 #define _gcem_factorial_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -95,4 +98,6 @@
     return internal::factorial_recur(x);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
index 200e4e9..4ffbcf2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_find_exponent_HPP
 #define _gcem_find_exponent_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -54,4 +57,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
index 5ed3d26..7689313 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_find_fraction_HPP
 #define _gcem_find_fraction_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -43,4 +46,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
index d193632..561050e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_find_whole_HPP
 #define _gcem_find_whole_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -43,4 +46,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
index 8b260ff..3b3b86a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_floor_HPP
 #define _gcem_floor_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -124,7 +130,13 @@
 floor(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::floor_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::floor(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
index 02459ef..58c4544 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_fmod_HPP
 #define _gcem_fmod_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -64,7 +70,13 @@
 fmod(const T1 x, const T2 y)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::fmod_type_check(x,y);
+  } else {
+    return std::fmod(x, y);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
index 1e277fb..c5b14a2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
@@ -21,6 +21,9 @@
 #ifndef _gcem_gcd_HPP
 #define _gcem_gcd_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -79,4 +82,6 @@
     return internal::gcd_type_check(a,b);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
index 01ad4e9..13ea80c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
@@ -27,16 +27,42 @@
 #ifndef _gcem_hypot_HPP
 #define _gcem_hypot_HPP
 
+#include <algorithm>
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
 template<typename T>
 constexpr
 T
-hypot_compute(const T x, const T ydx)
+hypot_compute(const T x, const T y)
 noexcept
 {
-    return abs(x) * sqrt( T(1) + (ydx * ydx) );
+    T a = std::max(abs(x), abs(y));
+    if (a) {
+      return a * sqrt((x / a) * (x / a) + (y / a) * (y / a));
+    } else {
+      return {};
+    }
+}
+
+template<typename T>
+constexpr
+T
+hypot_compute(const T x, const T y, const T z)
+noexcept
+{
+    T a = std::max({abs(x), abs(y), abs(z)});
+    if (a) {
+      return a * sqrt((x / a) * (x / a) + (y / a) * (y / a) + (z / a) * (z / a));
+    } else {
+      return {};
+    }
 }
 
 template<typename T>
@@ -56,7 +82,35 @@
             GCLIM<T>::min() > abs(y) ? \
                 abs(x) :
             // else
-            hypot_compute(x, y/x) );
+            hypot_compute(x, y) );
+}
+
+template<typename T>
+constexpr
+T
+hypot_vals_check(const T x, const T y, const T z)
+noexcept
+{
+    return( any_nan(x, y, z) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            any_inf(x,y,z) ? \
+                GCLIM<T>::infinity() :
+            // indistinguishable from zero or one
+            GCLIM<T>::min() > abs(x) && GCLIM<T>::min() > abs(y) ? \
+                abs(z) :
+            GCLIM<T>::min() > abs(x) && GCLIM<T>::min() > abs(z) ? \
+                abs(y) :
+            GCLIM<T>::min() > abs(y) && GCLIM<T>::min() > abs(z) ? \
+                abs(x) :
+            GCLIM<T>::min() > abs(x) ? \
+                hypot_vals_check(y, z) :
+            GCLIM<T>::min() > abs(y) ? \
+                hypot_vals_check(x, z) :
+            GCLIM<T>::min() > abs(z) ? \
+                hypot_vals_check(x, y) :
+            // else
+            hypot_compute(x, y, z) );
 }
 
 template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
@@ -68,6 +122,15 @@
     return hypot_vals_check(static_cast<TC>(x),static_cast<TC>(y));
 }
 
+template<typename T1, typename T2, typename T3, typename TC = common_return_t<T1,T2,T3>>
+constexpr
+TC
+hypot_type_check(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return hypot_vals_check(static_cast<TC>(x),static_cast<TC>(y),static_cast<TC>(z));
+}
+
 }
 
 /**
@@ -84,7 +147,35 @@
 hypot(const T1 x, const T2 y)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::hypot_type_check(x,y);
+  } else {
+    return std::hypot(x, y);
+  }
+}
+
+/**
+ * Compile-time Pythagorean addition function
+ *
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @param z a real-valued input.
+ * @return Computes \f$ x \oplus y \oplus z = \sqrt{x^2 + y^2 + z^2} \f$.
+ */
+
+template<typename T1, typename T2, typename T3>
+constexpr
+common_return_t<T1,T2,T3>
+hypot(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+  if (std::is_constant_evaluated()) {
+    return internal::hypot_type_check(x,y,z);
+  } else {
+    return std::hypot(x, y, z);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
index dbb9f60..681cc59 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
@@ -27,6 +27,9 @@
 #ifndef _gcem_incomplete_beta_HPP
 #define _gcem_incomplete_beta_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -191,4 +194,6 @@
     return internal::incomplete_beta_type_check(a,b,z);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
index 9f575a3..3878d37 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_incomplete_beta_inv_HPP
 #define _gcem_incomplete_beta_inv_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -349,4 +352,6 @@
     return internal::incomplete_beta_inv_type_check(a,b,p);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
index 9ee4146..0432b7d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_incomplete_gamma_HPP
 #define _gcem_incomplete_gamma_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -244,4 +247,6 @@
     return internal::incomplete_gamma_type_check(a,x);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
index e5976d0..6b1575b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_incomplete_gamma_inv_HPP
 #define _gcem_incomplete_gamma_inv_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -268,4 +271,6 @@
     return internal::incomplete_gamma_inv_type_check(a,p);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
index d0e33fb..466e76f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_inv_sqrt_HPP
 #define _gcem_inv_sqrt_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -85,4 +88,6 @@
     return internal::inv_sqrt_check( static_cast<return_t<T>>(x) );
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
index de0641d..5aff6a3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_is_even_HPP
 #define _gcem_is_even_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -38,4 +41,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
index b632fa3..805cbc4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_is_finite_HPP
 #define _gcem_is_finite_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -75,4 +78,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
index 568614f..fdc31de 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_is_inf_HPP
 #define _gcem_is_inf_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -169,4 +172,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
index a3fcbc6..74092e6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_is_nan_HPP
 #define _gcem_is_nan_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -77,4 +80,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
index a74a8d3..a76802f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_is_odd_HPP
 #define _gcem_is_odd_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -39,4 +42,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
index 60c87b4..78f6849 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
@@ -21,6 +21,9 @@
 #ifndef _gcem_lbeta_HPP
 #define _gcem_lbeta_HPP
 
+namespace gcem
+{
+
 /**
  * Compile-time log-beta function
  *
@@ -39,4 +42,6 @@
     return( (lgamma(a) + lgamma(b)) - lgamma(a+b) );
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
index a7ca776..aa1f339 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
@@ -21,6 +21,9 @@
 #ifndef _gcem_lcm_HPP
 #define _gcem_lcm_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -62,4 +65,6 @@
     return internal::lcm_type_check(a,b);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
index 507c6d4..e349ee8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
@@ -28,6 +28,12 @@
 #ifndef _gcem_lgamma_HPP
 #define _gcem_lgamma_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -129,7 +135,13 @@
 lgamma(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::lgamma_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::lgamma(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
index 58915dc..b009de1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_lmgamma_HPP
 #define _gcem_lmgamma_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -70,4 +73,6 @@
     return internal::lmgamma_recur(static_cast<return_t<T1>>(a),p);
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
index c2e24b0..15577c8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_log_HPP
 #define _gcem_log_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -180,7 +186,13 @@
 log(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::log_integral_check( x );
+  } else {
+    return std::log(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
index cda8894..0e9a4db 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_log10_HPP
 #define _gcem_log10_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -53,7 +59,13 @@
 log10(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::log10_check( x );
+  } else {
+    return std::log10(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
index ccd08b8..ed41daf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_log1p_HPP
 #define _gcem_log1p_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -74,7 +80,13 @@
 log1p(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::log1p_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::log1p(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
index a97fed4..f8dc729 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_log2_HPP
 #define _gcem_log2_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -53,7 +59,13 @@
 log2(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::log2_check( x );
+  } else {
+    return std::log2(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
index 2bcaadd..c4ba2fa 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
@@ -21,6 +21,9 @@
 #ifndef _gcem_log_binomial_coef_HPP
 #define _gcem_log_binomial_coef_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -62,4 +65,6 @@
     return internal::log_binomial_coef_type_check(n,k);
 }
 
-#endif
\ No newline at end of file
+}
+
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
index af23ea2..4c95110 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_mantissa_HPP
 #define _gcem_mantissa_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -44,4 +47,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
index ddc3e4e..9f3901b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_max_HPP
 #define _gcem_max_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 /**
  * Compile-time pairwise maximum function
  *
@@ -35,7 +41,13 @@
 max(const T1 x, const T2 y)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return( y < x ? x : y );
+  } else {
+    return std::max(x, y);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
index 5ce70b3..a35bcf6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_min_HPP
 #define _gcem_min_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 /**
  * Compile-time pairwise minimum function
  *
@@ -35,7 +41,13 @@
 min(const T1 x, const T2 y)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return( y > x ? x : y );
+  } else {
+    return std::min(x, y);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
index 79d24a4..9092303 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
@@ -22,6 +22,9 @@
  * extract signbit for signed zeros
  */
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -35,3 +38,5 @@
 }
 
 }
+
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
index 3891ede..372166d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_pow_HPP
 #define _gcem_pow_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -76,7 +82,13 @@
 pow(const T1 base, const T2 exp_term)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::pow_check(base,exp_term);
+  } else {
+    return std::pow(base, exp_term);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
index 4e67155..589d164 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
@@ -25,6 +25,9 @@
 #ifndef _gcem_pow_integral_HPP
 #define _gcem_pow_integral_HPP
 
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -125,4 +128,6 @@
 
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
index 295f43d..a80d2bb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
@@ -22,6 +22,9 @@
  * Gauss-Legendre quadrature: 30 points
  */
 
+namespace gcem
+{
+
 static const long double gauss_legendre_30_points[30] = \
 {
     -0.05147184255531769583302521316672L,
@@ -89,3 +92,5 @@
     0.00796819249616660561546588347467L,
     0.00796819249616660561546588347467L\
 };
+
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
index d4e448c..85244da 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
@@ -22,6 +22,9 @@
  * Gauss-Legendre quadrature: 50 points
  */
 
+namespace gcem
+{
+
 static const long double gauss_legendre_50_points[50] = \
 {
     -0.03109833832718887611232898966595L,
@@ -129,3 +132,5 @@
     0.00290862255315514095840072434286L,
     0.00290862255315514095840072434286L\
 };
+
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
index 9ac4a09..10fae64 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_round_HPP
 #define _gcem_round_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -119,7 +125,13 @@
 round(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::round_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::round(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
index e2eec9e..4c4adfa 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
@@ -21,6 +21,9 @@
 #ifndef _gcem_sgn_HPP
 #define _gcem_sgn_HPP
 
+namespace gcem
+{
+
 /**
  * Compile-time sign function
  *
@@ -42,4 +45,6 @@
                 0 );
 }
 
+}
+
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
index 282e244..40ec1dd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_signbit_HPP
 #define _gcem_signbit_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 /**
  * Compile-time sign bit detection function
  *
@@ -34,11 +40,17 @@
 signbit(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
 #ifdef _MSC_VER
     return( (x == T(0)) ? (_fpclass(x) == _FPCLASS_NZ) : (x < T(0)) );
 #else
     return GCEM_SIGNBIT(x);
 #endif
+  } else {
+    return std::signbit(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
index 56c8dca..c7c1760 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
@@ -27,6 +27,12 @@
 #ifndef _gcem_sin_HPP
 #define _gcem_sin_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -79,7 +85,13 @@
 sin(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::sin_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::sin(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
index fe3ecdd..9174d5d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_sinh_HPP
 #define _gcem_sinh_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -59,7 +65,13 @@
 sinh(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::sinh_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::sinh(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
index 1b2753c..201d89f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_sqrt_HPP
 #define _gcem_sqrt_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -103,7 +109,13 @@
 sqrt(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::sqrt_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::sqrt(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
index 386cce0..08c12d4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_tan_HPP
 #define _gcem_tan_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -134,7 +140,13 @@
 tan(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::tan_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::tan(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
index 30b4318..f25dabd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_tanh_HPP
 #define _gcem_tanh_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -83,7 +89,13 @@
 tanh(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::tanh_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::tanh(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
index deffd3a..1d9fcee 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
@@ -25,6 +25,12 @@
 #ifndef _gcem_tgamma_HPP
 #define _gcem_tgamma_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -74,7 +80,13 @@
 tgamma(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::tgamma_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::tgamma(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
index af3f448..f7a31ce 100644
--- a/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
+++ b/third_party/allwpilib/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
@@ -21,6 +21,12 @@
 #ifndef _gcem_trunc_HPP
 #define _gcem_trunc_HPP
 
+#include <cmath>
+#include <type_traits>
+
+namespace gcem
+{
+
 namespace internal
 {
 
@@ -115,7 +121,13 @@
 trunc(const T x)
 noexcept
 {
+  if (std::is_constant_evaluated()) {
     return internal::trunc_check( static_cast<return_t<T>>(x) );
+  } else {
+    return std::trunc(x);
+  }
+}
+
 }
 
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/proto/plant.proto b/third_party/allwpilib/wpimath/src/main/proto/plant.proto
index d0d9eab..86200d1 100644
--- a/third_party/allwpilib/wpimath/src/main/proto/plant.proto
+++ b/third_party/allwpilib/wpimath/src/main/proto/plant.proto
@@ -10,7 +10,4 @@
   double stall_current = 3;
   double free_current = 4;
   double free_speed = 5;
-  double r = 6;
-  double kv = 7;
-  double kt = 8;
 }
diff --git a/third_party/allwpilib/wpimath/src/main/proto/trajectory.proto b/third_party/allwpilib/wpimath/src/main/proto/trajectory.proto
index a37a501..03ece13 100644
--- a/third_party/allwpilib/wpimath/src/main/proto/trajectory.proto
+++ b/third_party/allwpilib/wpimath/src/main/proto/trajectory.proto
@@ -15,6 +15,5 @@
 }
 
 message ProtobufTrajectory {
-  double total_time = 1;
   repeated ProtobufTrajectoryState states = 2;
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DARETest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
index 0d697bd..50ff38a 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
@@ -42,8 +42,7 @@
                     .times((B.transpose().times(X).times(B).plus(R)).inv())
                     .times(B.transpose().times(X).times(A)))
             .plus(Q);
-    assertMatrixEqual(
-        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+    assertMatrixEqual(new Matrix<>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
   }
 
   <States extends Num, Inputs extends Num> void assertDARESolution(
@@ -63,8 +62,7 @@
                     .times((B.transpose().times(X).times(B).plus(R)).inv())
                     .times(B.transpose().times(X).times(A).plus(N.transpose())))
             .plus(Q);
-    assertMatrixEqual(
-        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+    assertMatrixEqual(new Matrix<>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
   }
 
   @Test
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
index 8ee11e5..d4bc035 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
@@ -18,29 +18,28 @@
 class MatrixTest {
   @Test
   void testMatrixMultiplication() {
-    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 0.0, 1.0);
-    var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 0.0, 0.0, 2.5);
+    var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 0.0, 1.0);
+    var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 0.0, 0.0, 2.5);
 
     Matrix<N2, N2> result = mat1.times(mat2);
 
-    assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5));
+    assertEquals(result, MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 2.5, 0.0, 2.5));
 
-    var mat3 = Matrix.mat(Nat.N2(), Nat.N3()).fill(1.0, 3.0, 0.5, 2.0, 4.3, 1.2);
+    var mat3 = MatBuilder.fill(Nat.N2(), Nat.N3(), 1.0, 3.0, 0.5, 2.0, 4.3, 1.2);
     var mat4 =
-        Matrix.mat(Nat.N3(), Nat.N4())
-            .fill(3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0);
+        MatBuilder.fill(
+            Nat.N3(), Nat.N4(), 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0);
 
     Matrix<N2, N4> result2 = mat3.times(mat4);
 
     assertTrue(
-        Matrix.mat(Nat.N2(), Nat.N4())
-            .fill(12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53)
+        MatBuilder.fill(Nat.N2(), Nat.N4(), 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53)
             .isEqual(result2, 1E-9));
   }
 
   @Test
   void testMatrixVectorMultiplication() {
-    var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 1.0, 0.0, 1.0);
+    var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 1.0, 0.0, 1.0);
 
     var vec = VecBuilder.fill(3.0, 2.0);
 
@@ -54,19 +53,19 @@
 
     Matrix<N1, N3> transpose = vec.transpose();
 
-    assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose);
+    assertEquals(MatBuilder.fill(Nat.N1(), Nat.N3(), 1.0, 2.0, 3.0), transpose);
   }
 
   @Test
   void testSolve() {
-    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
+    var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0);
     var vec1 = VecBuilder.fill(1.0, 2.0);
 
     var solve1 = mat1.solve(vec1);
 
     assertEquals(VecBuilder.fill(0.0, 0.5), solve1);
 
-    var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
+    var mat2 = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
     var vec2 = VecBuilder.fill(1.0, 2.0, 3.0);
 
     var solve2 = mat2.solve(vec2);
@@ -76,7 +75,7 @@
 
   @Test
   void testInverse() {
-    var mat = Matrix.mat(Nat.N3(), Nat.N3()).fill(1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5);
+    var mat = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5);
 
     var inv = mat.inv();
 
@@ -87,33 +86,33 @@
 
   @Test
   void testUninvertableMatrix() {
-    var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 2.0, 1.0);
+    var singularMatrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 2.0, 1.0);
 
     assertThrows(SingularMatrixException.class, singularMatrix::inv);
   }
 
   @Test
   void testMatrixScalarArithmetic() {
-    var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
+    var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0);
 
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
+    assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
 
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
+    assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
 
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0), mat.times(2.0));
+    assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 4.0, 6.0, 8.0), mat.times(2.0));
 
-    assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3));
+    assertTrue(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3));
   }
 
   @Test
   void testMatrixMatrixArithmetic() {
-    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
+    var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0);
 
-    var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(5.0, 6.0, 7.0, 8.0);
+    var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 5.0, 6.0, 7.0, 8.0);
 
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0), mat1.minus(mat2));
+    assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), -4.0, -4.0, -4.0, -4.0), mat1.minus(mat2));
 
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0), mat1.plus(mat2));
+    assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 8.0, 10.0, 12.0), mat1.plus(mat2));
   }
 
   @Test
@@ -121,14 +120,14 @@
     var matrix = Matrix.eye(Nat.N2());
     var result = matrix.exp();
 
-    assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
+    assertTrue(result.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9));
 
-    matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
+    matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4);
     result = matrix.times(0.01).exp();
 
     assertTrue(
         result.isEqual(
-            Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
+            MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993),
             1E-8));
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
index ee33f01..4883e6f 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -15,8 +15,6 @@
 import edu.wpi.first.wpilibj.UtilityClassTest;
 import java.util.ArrayList;
 import java.util.List;
-import org.ejml.dense.row.MatrixFeatures_DDRM;
-import org.ejml.simple.SimpleMatrix;
 import org.junit.jupiter.api.Test;
 
 class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
@@ -61,48 +59,48 @@
 
     // First eigenvalue is uncontrollable and unstable.
     // Second eigenvalue is controllable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
     assertFalse(StateSpaceUtil.isStabilizable(A, B));
 
     // First eigenvalue is uncontrollable and marginally stable.
     // Second eigenvalue is controllable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
     assertFalse(StateSpaceUtil.isStabilizable(A, B));
 
     // First eigenvalue is uncontrollable and stable.
     // Second eigenvalue is controllable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
     assertTrue(StateSpaceUtil.isStabilizable(A, B));
 
     // First eigenvalue is uncontrollable and stable.
     // Second eigenvalue is controllable and unstable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
     assertTrue(StateSpaceUtil.isStabilizable(A, B));
   }
 
   @Test
   void testIsDetectable() {
     Matrix<N2, N2> A;
-    Matrix<N1, N2> C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1);
+    Matrix<N1, N2> C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1);
 
     // First eigenvalue is unobservable and unstable.
     // Second eigenvalue is observable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
     assertFalse(StateSpaceUtil.isDetectable(A, C));
 
     // First eigenvalue is unobservable and marginally stable.
     // Second eigenvalue is observable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
     assertFalse(StateSpaceUtil.isDetectable(A, C));
 
     // First eigenvalue is unobservable and stable.
     // Second eigenvalue is observable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
     assertTrue(StateSpaceUtil.isDetectable(A, C));
 
     // First eigenvalue is unobservable and stable.
     // Second eigenvalue is observable and unstable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
     assertTrue(StateSpaceUtil.isDetectable(A, C));
   }
 
@@ -143,37 +141,14 @@
     var wrappedResult = wrappedMatrix.exp();
 
     assertTrue(
-        wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
+        wrappedResult.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9));
 
-    var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
+    var matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4);
     wrappedResult = matrix.times(0.01).exp();
 
     assertTrue(
         wrappedResult.isEqual(
-            Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
-            1E-8));
-  }
-
-  @Test
-  void testSimpleMatrixExp() {
-    SimpleMatrix matrix = SimpleMatrixUtils.eye(2);
-    var result = SimpleMatrixUtils.exp(matrix);
-
-    assertTrue(
-        MatrixFeatures_DDRM.isIdentical(
-            result.getDDRM(),
-            new SimpleMatrix(2, 2, true, new double[] {Math.E, 0, 0, Math.E}).getDDRM(),
-            1E-9));
-
-    matrix = new SimpleMatrix(2, 2, true, new double[] {1, 2, 3, 4});
-    result = SimpleMatrixUtils.exp(matrix.scale(0.01));
-
-    assertTrue(
-        MatrixFeatures_DDRM.isIdentical(
-            result.getDDRM(),
-            new SimpleMatrix(
-                    2, 2, true, new double[] {1.01035625, 0.02050912, 0.03076368, 1.04111993})
-                .getDDRM(),
+            MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993),
             1E-8));
   }
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
index fcd9de1..b5e808a 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
@@ -6,6 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
@@ -17,8 +18,7 @@
   @Test
   void testCalculate() {
     ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
-        new ControlAffinePlantInversionFeedforward<N2, N1>(
-            Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+        new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
 
     assertEquals(
         48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
@@ -27,21 +27,19 @@
   @Test
   void testCalculateState() {
     ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
-        new ControlAffinePlantInversionFeedforward<N2, N1>(
-            Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+        new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
 
     assertEquals(
         48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
   }
 
   protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
-    return Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(1.000, 0, 0, 1.000)
+    return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000)
         .times(x)
         .plus(VecBuilder.fill(0, 1).times(u));
   }
 
   protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
-    return Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
+    return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000).times(x);
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
index faf5563..4ec558d 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
@@ -9,8 +9,10 @@
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import org.junit.jupiter.api.Test;
 
@@ -26,52 +28,38 @@
 
     var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha);
 
-    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
-    var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
 
     // Ensure voltage exceeds acceleration before limiting
     {
       final var accels =
-          plant
-              .getA()
-              .times(xAccelLimiter)
-              .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
+          plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
       assertTrue(Math.abs(a) > maxA);
     }
     {
       final var accels =
-          plant
-              .getA()
-              .times(xAccelLimiter)
-              .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0)));
+          plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(-12.0, 12.0)));
       final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
       assertTrue(Math.abs(alpha) > maxAlpha);
     }
 
     // Forward
-    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    var u = VecBuilder.fill(12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       final var accels =
           plant
               .getA()
               .times(xAccelLimiter)
-              .plus(
-                  plant
-                      .getB()
-                      .times(
-                          new MatBuilder<>(Nat.N2(), Nat.N1())
-                              .fill(voltages.left, voltages.right)));
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
       final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
       assertTrue(Math.abs(a) <= maxA);
@@ -79,28 +67,20 @@
     }
 
     // Backward
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    u = VecBuilder.fill(-12.0, -12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       final var accels =
           plant
               .getA()
               .times(xAccelLimiter)
-              .plus(
-                  plant
-                      .getB()
-                      .times(
-                          new MatBuilder<>(Nat.N2(), Nat.N1())
-                              .fill(voltages.left, voltages.right)));
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
       final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
       assertTrue(Math.abs(a) <= maxA);
@@ -108,28 +88,20 @@
     }
 
     // Rotate CCW
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    u = VecBuilder.fill(-12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       final var accels =
           plant
               .getA()
               .times(xAccelLimiter)
-              .plus(
-                  plant
-                      .getB()
-                      .times(
-                          new MatBuilder<>(Nat.N2(), Nat.N1())
-                              .fill(voltages.left, voltages.right)));
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
       final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
       assertTrue(Math.abs(a) <= maxA);
@@ -148,21 +120,18 @@
     // unconstrained systems should match
     var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3);
 
-    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
-    var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
 
     // Forward
-    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    var u = VecBuilder.fill(12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
       assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
@@ -171,17 +140,14 @@
     // Backward
     x.fill(0.0);
     xAccelLimiter.fill(0.0);
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    u = VecBuilder.fill(-12.0, -12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
       assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
@@ -190,17 +156,14 @@
     // Rotate CCW
     x.fill(0.0);
     xAccelLimiter.fill(0.0);
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    u = VecBuilder.fill(-12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
       assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
@@ -220,16 +183,13 @@
     var accelLimiter =
         new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
 
-    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
-    var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
 
     // Ensure voltage exceeds acceleration before limiting
     {
       final var accels =
-          plant
-              .getA()
-              .times(xAccelLimiter)
-              .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
+          plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
       assertTrue(Math.abs(a) > maxA);
       assertTrue(Math.abs(a) > -minA);
@@ -237,55 +197,39 @@
 
     // a should always be within [minA, maxA]
     // Forward
-    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    var u = VecBuilder.fill(12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       final var accels =
           plant
               .getA()
               .times(xAccelLimiter)
-              .plus(
-                  plant
-                      .getB()
-                      .times(
-                          new MatBuilder<>(Nat.N2(), Nat.N1())
-                              .fill(voltages.left, voltages.right)));
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
       assertTrue(minA <= a && a <= maxA);
     }
 
     // Backward
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    u = VecBuilder.fill(-12.0, -12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       final var voltages =
           accelLimiter.calculate(
               xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
       xAccelLimiter =
-          plant.calculateX(
-              xAccelLimiter,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
-              dt);
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
 
       final var accels =
           plant
               .getA()
               .times(xAccelLimiter)
-              .plus(
-                  plant
-                      .getB()
-                      .times(
-                          new MatBuilder<>(Nat.N2(), Nat.N1())
-                              .fill(voltages.left, voltages.right)));
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
       assertTrue(minA <= a && a <= maxA);
     }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
index 6c978ac..1499302 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
@@ -6,7 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.numbers.N1;
@@ -27,8 +27,8 @@
     assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
     assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
 
-    var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka);
-    var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka);
+    var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka);
+    var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka);
     final double dt = 0.02;
     var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
index 674cba3..90f2127 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
@@ -7,8 +7,9 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import org.junit.jupiter.api.Test;
@@ -20,13 +21,13 @@
 
     var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
 
-    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant);
+    var imf = new ImplicitModelFollower<>(plant, plant);
 
-    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
-    var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
 
     // Forward
-    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    var u = VecBuilder.fill(12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -36,7 +37,7 @@
     }
 
     // Backward
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    u = VecBuilder.fill(-12.0, -12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -46,7 +47,7 @@
     }
 
     // Rotate CCW
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    u = VecBuilder.fill(-12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -65,13 +66,13 @@
     // Linear acceleration is slower, but angular acceleration is the same
     var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
 
-    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef);
+    var imf = new ImplicitModelFollower<>(plant, plantRef);
 
-    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
-    var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
 
     // Forward
-    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    var u = VecBuilder.fill(12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -83,7 +84,7 @@
     // Backward
     x.fill(0.0);
     xImf.fill(0.0);
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    u = VecBuilder.fill(-12.0, -12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -95,7 +96,7 @@
     // Rotate CCW
     x.fill(0.0);
     xImf.fill(0.0);
-    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    u = VecBuilder.fill(-12.0, 12.0);
     for (double t = 0.0; t < 3.0; t += dt) {
       x = plant.calculateX(x, u, dt);
       xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
index efb9c5e..e977de8 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
@@ -87,9 +87,14 @@
     final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
 
     var x =
-        new MatBuilder<>(Nat.N5(), Nat.N1())
-            .fill(
-                robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0);
+        MatBuilder.fill(
+            Nat.N5(),
+            Nat.N1(),
+            robotPose.getX(),
+            robotPose.getY(),
+            robotPose.getRotation().getRadians(),
+            0.0,
+            0.0);
 
     final var totalTime = trajectory.getTotalTimeSeconds();
     for (int i = 0; i < (totalTime / kDt); ++i) {
@@ -105,7 +110,7 @@
           NumericalIntegration.rkdp(
               LTVDifferentialDriveControllerTest::dynamics,
               x,
-              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right),
+              VecBuilder.fill(output.left, output.right),
               kDt);
     }
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
index 1aba6d7..8901330 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
@@ -6,6 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
@@ -16,11 +17,11 @@
 class LinearPlantInversionFeedforwardTest {
   @Test
   void testCalculate() {
-    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+    Matrix<N2, N2> A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
     Matrix<N2, N1> B = VecBuilder.fill(0, 1);
 
     LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
-        new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
+        new LinearPlantInversionFeedforward<>(A, B, 0.02);
 
     assertEquals(
         47.502599,
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
index 634ed1c..e915388 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
@@ -6,6 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -111,10 +112,10 @@
 
   @Test
   void testMatrixOverloadsWithSingleIntegrator() {
-    var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0);
-    var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
-    var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
-    var R = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+    var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0);
+    var B = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
+    var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
+    var R = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
 
     // QR overload
     var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
@@ -124,7 +125,7 @@
     assertEquals(0.99750312499512261, K.get(1, 1), 1e-10);
 
     // QRN overload
-    var N = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+    var N = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
     var Kimf = new LinearQuadraticRegulator<>(A, B, Q, R, N, 0.005).getK();
     assertEquals(1.0, Kimf.get(0, 0), 1e-10);
     assertEquals(0.0, Kimf.get(0, 1), 1e-10);
@@ -137,10 +138,10 @@
     double Kv = 3.02;
     double Ka = 0.642;
 
-    var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / Ka);
-    var B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1.0 / Ka);
-    var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.2);
-    var R = Matrix.mat(Nat.N1(), Nat.N1()).fill(0.25);
+    var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / Ka);
+    var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1.0 / Ka);
+    var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.2);
+    var R = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.25);
 
     // QR overload
     var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
@@ -148,7 +149,7 @@
     assertEquals(0.51182128351092726, K.get(0, 1), 1e-10);
 
     // QRN overload
-    var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0));
+    var Aref = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / (Ka * 5.0));
     var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
     assertEquals(0.0, Kimf.get(0, 0), 1e-10);
     assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10);
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
index fd4e428..e690daf 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
@@ -6,7 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.numbers.N1;
@@ -20,8 +20,8 @@
     double Ka = 0.6;
     double dt = 0.02;
 
-    var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-Kv / Ka);
-    var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / Ka);
+    var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -Kv / Ka);
+    var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka);
 
     var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
     var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java
new file mode 100644
index 0000000..5c036d5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java
@@ -0,0 +1,27 @@
+// 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.
+
+package edu.wpi.first.math.controller.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward;
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardProtoTest {
+  private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufArmFeedforward proto = ArmFeedforward.proto.createMessage();
+    ArmFeedforward.proto.pack(proto, DATA);
+
+    ArmFeedforward data = ArmFeedforward.proto.unpack(proto);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java
new file mode 100644
index 0000000..c3bf587
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java
@@ -0,0 +1,27 @@
+// 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.
+
+package edu.wpi.first.math.controller.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelVoltagesProtoTest {
+  private static final DifferentialDriveWheelVoltages DATA =
+      new DifferentialDriveWheelVoltages(0.174, 0.191);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDifferentialDriveWheelVoltages proto =
+        DifferentialDriveWheelVoltages.proto.createMessage();
+    DifferentialDriveWheelVoltages.proto.pack(proto, DATA);
+
+    DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.proto.unpack(proto);
+    assertEquals(DATA.left, data.left);
+    assertEquals(DATA.right, data.right);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java
new file mode 100644
index 0000000..e624aff
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java
@@ -0,0 +1,27 @@
+// 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.
+
+package edu.wpi.first.math.controller.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardProtoTest {
+  private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufElevatorFeedforward proto = ElevatorFeedforward.proto.createMessage();
+    ElevatorFeedforward.proto.pack(proto, DATA);
+
+    ElevatorFeedforward data = ElevatorFeedforward.proto.unpack(proto);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java
new file mode 100644
index 0000000..42a452d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java
@@ -0,0 +1,30 @@
+// 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.
+
+package edu.wpi.first.math.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardStructTest {
+  private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ArmFeedforward.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ArmFeedforward.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ArmFeedforward data = ArmFeedforward.struct.unpack(buffer);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java
new file mode 100644
index 0000000..0c6ea66
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelVoltagesStructTest {
+  private static final DifferentialDriveWheelVoltages DATA =
+      new DifferentialDriveWheelVoltages(0.174, 0.191);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelVoltages.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveWheelVoltages.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.struct.unpack(buffer);
+    assertEquals(DATA.left, data.left);
+    assertEquals(DATA.right, data.right);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java
new file mode 100644
index 0000000..a5a8352
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java
@@ -0,0 +1,30 @@
+// 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.
+
+package edu.wpi.first.math.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardStructTest {
+  private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ElevatorFeedforward.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ElevatorFeedforward.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ElevatorFeedforward data = ElevatorFeedforward.struct.unpack(buffer);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
index c36e72f..eb30e49 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
@@ -6,6 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
@@ -16,7 +17,7 @@
   void testMean() {
     // 3 states, 2 sigmas
     var sigmas =
-        Matrix.mat(Nat.N3(), Nat.N2()).fill(1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2);
+        MatBuilder.fill(Nat.N3(), Nat.N2(), 1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2);
     // Weights need to produce the mean of the sigmas
     var weights = new Matrix<>(Nat.N2(), Nat.N1());
     weights.fill(1.0 / sigmas.getNumCols());
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index e0e202f..08c5a15 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -254,7 +255,7 @@
           "Estimator converged to one vision measurement: "
               + estimator.getEstimatedPosition().toString()
               + " -> "
-              + measurement.toString();
+              + measurement;
 
       var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
       var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -263,7 +264,7 @@
               measurement.getRotation().getDegrees()
                   - estimator.getEstimatedPosition().getRotation().getDegrees());
 
-      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
     }
   }
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
index 112ef90..8204fec 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
@@ -22,7 +22,6 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -114,7 +113,7 @@
             dtSeconds);
 
     List<Pose2d> waypoints =
-        Arrays.asList(
+        List.of(
             new Pose2d(2.75, 22.521, new Rotation2d()),
             new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
     var trajectory =
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
index 848fe93..f5f0925 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
@@ -7,6 +7,7 @@
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
@@ -47,16 +48,16 @@
   // Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ
   LinearSystem<N6, N3, N3> m_swerveObserverSystem =
       new LinearSystem<>(
-          Matrix.mat(Nat.N6(), Nat.N6())
-              .fill( // A
-                  0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-                  0, 0, 0, 0, 0, 0, 0, 0, 0),
-          Matrix.mat(Nat.N6(), Nat.N3())
-              .fill( // B
-                  0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1),
-          Matrix.mat(Nat.N3(), Nat.N6())
-              .fill( // C
-                  1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0),
+          MatBuilder.fill(
+              Nat.N6(), Nat.N6(), // A
+              0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+              0, 0, 0, 0, 0, 0, 0),
+          MatBuilder.fill(
+              Nat.N6(), Nat.N3(), // B
+              0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1),
+          MatBuilder.fill(
+              Nat.N3(), Nat.N6(), // C
+              1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0),
           new Matrix<>(Nat.N3(), Nat.N3())); // D
 
   @Test
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index d844c5d..ea08a5f 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -267,7 +268,7 @@
           "Estimator converged to one vision measurement: "
               + estimator.getEstimatedPosition().toString()
               + " -> "
-              + measurement.toString();
+              + measurement;
 
       var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
       var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -276,7 +277,7 @@
               measurement.getRotation().getDegrees()
                   - estimator.getEstimatedPosition().getRotation().getDegrees());
 
-      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
     }
   }
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
index 2131c35..468eb51 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
@@ -6,7 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import org.junit.jupiter.api.Test;
@@ -17,13 +17,23 @@
     var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
     var points =
         merweScaledSigmaPoints.squareRootSigmaPoints(
-            VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
+            VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1));
 
     assertTrue(
         points.isEqual(
-            Matrix.mat(Nat.N2(), Nat.N5())
-                .fill(
-                    0.0, 0.00173205, 0.0, -0.00173205, 0.0, 0.0, 0.0, 0.00173205, 0.0, -0.00173205),
+            MatBuilder.fill(
+                Nat.N2(),
+                Nat.N5(),
+                0.0,
+                0.00173205,
+                0.0,
+                -0.00173205,
+                0.0,
+                0.0,
+                0.0,
+                0.00173205,
+                0.0,
+                -0.00173205),
             1E-6));
   }
 
@@ -32,12 +42,23 @@
     var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
     var points =
         merweScaledSigmaPoints.squareRootSigmaPoints(
-            VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10)));
+            VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10)));
 
     assertTrue(
         points.isEqual(
-            Matrix.mat(Nat.N2(), Nat.N5())
-                .fill(1.0, 1.00173205, 1.0, 0.99826795, 1.0, 2.0, 2.0, 2.00547723, 2.0, 1.99452277),
+            MatBuilder.fill(
+                Nat.N2(),
+                Nat.N5(),
+                1.0,
+                1.00173205,
+                1.0,
+                0.99826795,
+                1.0,
+                2.0,
+                2.0,
+                2.00547723,
+                2.0,
+                1.99452277),
             1E-6));
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index 9fcd852..f827a10 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -288,7 +289,7 @@
           "Estimator converged to one vision measurement: "
               + estimator.getEstimatedPosition().toString()
               + " -> "
-              + measurement.toString();
+              + measurement;
 
       var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
       var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -297,7 +298,7 @@
               measurement.getRotation().getDegrees()
                   - estimator.getEstimatedPosition().getRotation().getDegrees());
 
-      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
     }
   }
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
index a8e4d3b..bcc33ae 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -8,6 +8,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -25,7 +26,6 @@
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -136,7 +136,7 @@
             dtSeconds);
 
     List<Pose2d> waypoints =
-        Arrays.asList(
+        List.of(
             new Pose2d(2.75, 22.521, new Rotation2d()),
             new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
     var trajectory =
@@ -264,7 +264,7 @@
             VecBuilder.fill(0.0, 0.0),
             dtSeconds);
 
-    var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0);
+    var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0);
     observer.setP(P);
 
     assertTrue(observer.getP().isEqual(P, 1e-9));
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
index ec43cd5..72ea174 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
@@ -129,9 +129,9 @@
         1,
         3,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // df/dx = cos(x)
-        (double x) -> Math.cos(x),
+        Math::cos,
         h,
         -20.0,
         20.0);
@@ -140,7 +140,7 @@
         1,
         3,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // df/dx = 1/x
         (double x) -> 1.0 / x,
         h,
@@ -162,7 +162,7 @@
         2,
         5,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // d²f/dx² = -sin(x)
         (double x) -> -Math.sin(x),
         h,
@@ -173,7 +173,7 @@
         2,
         5,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // d²f/dx² = -1/x²
         (double x) -> -1.0 / (x * x),
         h,
@@ -201,9 +201,9 @@
         1,
         2,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // df/dx = cos(x)
-        (double x) -> Math.cos(x),
+        Math::cos,
         h,
         -20.0,
         20.0);
@@ -212,7 +212,7 @@
         1,
         2,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // df/dx = 1/x
         (double x) -> 1.0 / x,
         h,
@@ -234,7 +234,7 @@
         2,
         4,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // d²f/dx² = -sin(x)
         (double x) -> -Math.sin(x),
         h,
@@ -245,7 +245,7 @@
         2,
         4,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // d²f/dx² = -1/x²
         (double x) -> -1.0 / (x * x),
         h,
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
index d57362d..2c683c2 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
@@ -8,6 +8,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import edu.wpi.first.units.Units;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -15,6 +16,15 @@
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testNewWithMeasures() {
+    var pose = new Pose2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45));
+
+    assertEquals(0.1524, pose.getX(), kEpsilon);
+    assertEquals(0.2032, pose.getY(), kEpsilon);
+    assertEquals(Math.PI / 4, pose.getRotation().getRadians(), kEpsilon);
+  }
+
+  @Test
   void testRotateBy() {
     final double x = 1.0;
     final double y = 2.0;
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
index ee1d98c..3ef8a48 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
@@ -11,7 +11,7 @@
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.util.Units;
-import java.util.Arrays;
+import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Pose3dTest {
@@ -194,7 +194,7 @@
   @Test
   void testComplexTwists() {
     var initial_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(0.698303, -0.959096, 0.271076),
                 new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
@@ -212,7 +212,7 @@
                 new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
 
     var final_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(-0.230448, -0.511957, 0.198406),
                 new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
@@ -267,7 +267,7 @@
   @Test
   void testTwistNaN() {
     var initial_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(6.32, 4.12, 0.00),
                 new Rotation3d(
@@ -277,7 +277,7 @@
                 new Rotation3d(
                     new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
     var final_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(6.33, 4.15, 0.00),
                 new Rotation3d(
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
index 6702f1d..7f3e469 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -8,12 +8,20 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import edu.wpi.first.units.Units;
 import org.junit.jupiter.api.Test;
 
 class Rotation2dTest {
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testNewWithMeasures() {
+    var rot = new Rotation2d(Units.Degrees.of(45));
+
+    assertEquals(Math.PI / 4, rot.getRadians(), kEpsilon);
+  }
+
+  @Test
   void testRadiansToDegrees() {
     var rot1 = Rotation2d.fromRadians(Math.PI / 3);
     var rot2 = Rotation2d.fromRadians(Math.PI / 4);
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
index d80344d..008a65f 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
@@ -93,8 +93,7 @@
     assertEquals(expected2, rot2);
 
     // Matrix that isn't orthogonal
-    final var R3 =
-        new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
+    final var R3 = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
     assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3));
 
     // Matrix that's orthogonal but not special orthogonal
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
index 93d8a0e..5817f4e 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
@@ -6,9 +6,22 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.units.Units;
 import org.junit.jupiter.api.Test;
 
 class Transform2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithMeasures() {
+    var transform =
+        new Transform2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45));
+
+    assertEquals(0.1524, transform.getX(), kEpsilon);
+    assertEquals(0.2032, transform.getY(), kEpsilon);
+    assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon);
+  }
+
   @Test
   void testInverse() {
     var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
index 762425b..cd68dbe 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.geometry;
 
+import static edu.wpi.first.units.Units.Inches;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
@@ -16,6 +17,15 @@
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testNewWithMeasures() {
+    var translation = new Translation3d(Inches.of(6), Inches.of(8), Inches.of(16));
+
+    assertEquals(0.1524, translation.getX(), kEpsilon);
+    assertEquals(0.2032, translation.getY(), kEpsilon);
+    assertEquals(0.4064, translation.getZ(), kEpsilon);
+  }
+
+  @Test
   void testSum() {
     var one = new Translation3d(1.0, 3.0, 5.0);
     var two = new Translation3d(2.0, 5.0, 8.0);
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java
new file mode 100644
index 0000000..05ba185
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java
@@ -0,0 +1,28 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
+import org.junit.jupiter.api.Test;
+
+class Pose2dProtoTest {
+  private static final Pose2d DATA =
+      new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufPose2d proto = Pose2d.proto.createMessage();
+    Pose2d.proto.pack(proto, DATA);
+
+    Pose2d data = Pose2d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java
new file mode 100644
index 0000000..1043408
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import org.junit.jupiter.api.Test;
+
+class Pose3dProtoTest {
+  private static final Pose3d DATA =
+      new Pose3d(
+          new Translation3d(1.1, 2.2, 1.1),
+          new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufPose3d proto = Pose3d.proto.createMessage();
+    Pose3d.proto.pack(proto, DATA);
+
+    Pose3d data = Pose3d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java
new file mode 100644
index 0000000..9129c19
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java
@@ -0,0 +1,27 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import org.junit.jupiter.api.Test;
+
+class QuaternionProtoTest {
+  private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufQuaternion proto = Quaternion.proto.createMessage();
+    Quaternion.proto.pack(proto, DATA);
+
+    Quaternion data = Quaternion.proto.unpack(proto);
+    assertEquals(DATA.getW(), data.getW());
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java
new file mode 100644
index 0000000..6021ec0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java
@@ -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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
+import org.junit.jupiter.api.Test;
+
+class Rotation2dProtoTest {
+  private static final Rotation2d DATA = new Rotation2d(1.91);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRotation2d proto = Rotation2d.proto.createMessage();
+    Rotation2d.proto.pack(proto, DATA);
+
+    Rotation2d data = Rotation2d.proto.unpack(proto);
+    assertEquals(DATA.getRadians(), data.getRadians());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java
new file mode 100644
index 0000000..5860114
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java
@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dProtoTest {
+  private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRotation3d proto = Rotation3d.proto.createMessage();
+    Rotation3d.proto.pack(proto, DATA);
+
+    Rotation3d data = Rotation3d.proto.unpack(proto);
+    assertEquals(DATA.getQuaternion(), data.getQuaternion());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java
new file mode 100644
index 0000000..78294a1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java
@@ -0,0 +1,28 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
+import org.junit.jupiter.api.Test;
+
+class Transform2dProtoTest {
+  private static final Transform2d DATA =
+      new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTransform2d proto = Transform2d.proto.createMessage();
+    Transform2d.proto.pack(proto, DATA);
+
+    Transform2d data = Transform2d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java
new file mode 100644
index 0000000..fd72334
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+import org.junit.jupiter.api.Test;
+
+class Transform3dProtoTest {
+  private static final Transform3d DATA =
+      new Transform3d(
+          new Translation3d(0.3504, 22.9, 3.504),
+          new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTransform3d proto = Transform3d.proto.createMessage();
+    Transform3d.proto.pack(proto, DATA);
+
+    Transform3d data = Transform3d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java
new file mode 100644
index 0000000..07b9080
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java
@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import org.junit.jupiter.api.Test;
+
+class Translation2dProtoTest {
+  private static final Translation2d DATA = new Translation2d(3.504, 22.9);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTranslation2d proto = Translation2d.proto.createMessage();
+    Translation2d.proto.pack(proto, DATA);
+
+    Translation2d data = Translation2d.proto.unpack(proto);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java
new file mode 100644
index 0000000..d94e177
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java
@@ -0,0 +1,26 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import org.junit.jupiter.api.Test;
+
+class Translation3dProtoTest {
+  private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTranslation3d proto = Translation3d.proto.createMessage();
+    Translation3d.proto.pack(proto, DATA);
+
+    Translation3d data = Translation3d.proto.unpack(proto);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java
new file mode 100644
index 0000000..e7a0631
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java
@@ -0,0 +1,26 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import org.junit.jupiter.api.Test;
+
+class Twist2dProtoTest {
+  private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTwist2d proto = Twist2d.proto.createMessage();
+    Twist2d.proto.pack(proto, DATA);
+
+    Twist2d data = Twist2d.proto.unpack(proto);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dtheta, data.dtheta);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java
new file mode 100644
index 0000000..70e0f07
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import org.junit.jupiter.api.Test;
+
+class Twist3dProtoTest {
+  private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTwist3d proto = Twist3d.proto.createMessage();
+    Twist3d.proto.pack(proto, DATA);
+
+    Twist3d data = Twist3d.proto.unpack(proto);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dz, data.dz);
+    assertEquals(DATA.rx, data.rx);
+    assertEquals(DATA.ry, data.ry);
+    assertEquals(DATA.rz, data.rz);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java
new file mode 100644
index 0000000..0a8c62a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Pose2dStructTest {
+  private static final Pose2d DATA =
+      new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Pose2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Pose2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Pose2d data = Pose2d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java
new file mode 100644
index 0000000..e2aac46
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java
@@ -0,0 +1,34 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Pose3dStructTest {
+  private static final Pose3d DATA =
+      new Pose3d(
+          new Translation3d(1.1, 2.2, 1.1),
+          new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Pose3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Pose3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Pose3d data = Pose3d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java
new file mode 100644
index 0000000..41d4604
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java
@@ -0,0 +1,30 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class QuaternionStructTest {
+  private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Quaternion.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Quaternion.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Quaternion data = Quaternion.struct.unpack(buffer);
+    assertEquals(DATA.getW(), data.getW());
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java
new file mode 100644
index 0000000..62569b5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java
@@ -0,0 +1,27 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rotation2dStructTest {
+  private static final Rotation2d DATA = new Rotation2d(1.91);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rotation2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rotation2d data = Rotation2d.struct.unpack(buffer);
+    assertEquals(DATA.getRadians(), data.getRadians());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java
new file mode 100644
index 0000000..7f275e7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java
@@ -0,0 +1,28 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dStructTest {
+  private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rotation3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rotation3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rotation3d data = Rotation3d.struct.unpack(buffer);
+    assertEquals(DATA.getQuaternion(), data.getQuaternion());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java
new file mode 100644
index 0000000..cf583d4
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Transform2dStructTest {
+  private static final Transform2d DATA =
+      new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Transform2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Transform2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Transform2d data = Transform2d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java
new file mode 100644
index 0000000..10a5100
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java
@@ -0,0 +1,34 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Transform3dStructTest {
+  private static final Transform3d DATA =
+      new Transform3d(
+          new Translation3d(0.3504, 22.9, 3.504),
+          new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Transform3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Transform3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Transform3d data = Transform3d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java
new file mode 100644
index 0000000..366df17
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java
@@ -0,0 +1,28 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Translation2dStructTest {
+  private static final Translation2d DATA = new Translation2d(3.504, 22.9);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Translation2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Translation2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Translation2d data = Translation2d.struct.unpack(buffer);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java
new file mode 100644
index 0000000..fc48981
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Translation3dStructTest {
+  private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Translation3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Translation3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Translation3d data = Translation3d.struct.unpack(buffer);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java
new file mode 100644
index 0000000..6d4d174
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Twist2dStructTest {
+  private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Twist2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Twist2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Twist2d data = Twist2d.struct.unpack(buffer);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dtheta, data.dtheta);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java
new file mode 100644
index 0000000..131eb2f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java
@@ -0,0 +1,32 @@
+// 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.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Twist3dStructTest {
+  private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Twist3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Twist3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Twist3d data = Twist3d.struct.unpack(buffer);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dz, data.dz);
+    assertEquals(DATA.rx, data.rx);
+    assertEquals(DATA.ry, data.ry);
+    assertEquals(DATA.rz, data.rz);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
index e65d27f..a66f714 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
@@ -12,19 +12,40 @@
 
 class TimeInterpolatableBufferTest {
   @Test
+  void testAddSample() {
+    TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+    // No entries
+    buffer.addSample(1.0, new Rotation2d());
+    assertEquals(0.0, buffer.getSample(1.0).get().getRadians(), 0.001);
+
+    // New entry at start of container
+    buffer.addSample(0.0, new Rotation2d(1.0));
+    assertEquals(1.0, buffer.getSample(0.0).get().getRadians(), 0.001);
+
+    // New entry in middle of container
+    buffer.addSample(0.5, new Rotation2d(0.5));
+    assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
+
+    // Override sample
+    buffer.addSample(0.5, new Rotation2d(1.0));
+    assertEquals(1.0, buffer.getSample(0.5).get().getRadians(), 0.001);
+  }
+
+  @Test
   void testInterpolation() {
     TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
 
-    buffer.addSample(0, new Rotation2d());
-    assertEquals(0, buffer.getSample(0).get().getRadians(), 0.001);
-    buffer.addSample(1, new Rotation2d(1));
+    buffer.addSample(0.0, new Rotation2d());
+    assertEquals(0.0, buffer.getSample(0.0).get().getRadians(), 0.001);
+    buffer.addSample(1.0, new Rotation2d(1.0));
     assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
     assertEquals(1.0, buffer.getSample(1.0).get().getRadians(), 0.001);
-    buffer.addSample(3, new Rotation2d(2));
-    assertEquals(1.5, buffer.getSample(2).get().getRadians(), 0.001);
+    buffer.addSample(3.0, new Rotation2d(2.0));
+    assertEquals(1.5, buffer.getSample(2.0).get().getRadians(), 0.001);
 
     buffer.addSample(10.5, new Rotation2d(2));
-    assertEquals(new Rotation2d(1), buffer.getSample(0).get());
+    assertEquals(new Rotation2d(1.0), buffer.getSample(0.0).get());
   }
 
   @Test
@@ -32,12 +53,12 @@
     TimeInterpolatableBuffer<Pose2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
 
     // We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
-    buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
-    buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
+    buffer.addSample(0.0, new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(90.0)));
+    buffer.addSample(1.0, new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0.0)));
     Pose2d sample = buffer.getSample(0.5).get();
 
-    assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
-    assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);
-    assertEquals(45, sample.getRotation().getDegrees(), 0.01);
+    assertEquals(1.0 - 1.0 / Math.sqrt(2.0), sample.getTranslation().getX(), 0.01);
+    assertEquals(1.0 / Math.sqrt(2.0), sample.getTranslation().getY(), 0.01);
+    assertEquals(45.0, sample.getRotation().getDegrees(), 0.01);
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
index 2401342..53af407 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.InchesPerSecond;
+import static edu.wpi.first.units.Units.RPM;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
@@ -45,6 +47,19 @@
   }
 
   @Test
+  void testMeasureConstructor() {
+    var vx = InchesPerSecond.of(14.52);
+    var vy = InchesPerSecond.zero();
+    var omega = RPM.of(0.02);
+    var speeds = new ChassisSpeeds(vx, vy, omega);
+
+    assertAll(
+        () -> assertEquals(0.368808, speeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0, speeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.002094395102, speeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
+  @Test
   void testFromFieldRelativeSpeeds() {
     final var chassisSpeeds =
         ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
index 18d3ec7..28fbfd8 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
@@ -238,4 +238,18 @@
         () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
         () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
   }
+
+  @Test
+  void testDesaturateNegativeSpeeds() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7);
+    wheelSpeeds.desaturate(5.5);
+
+    final double kFactor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
+  }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
index cf6a439..405749f 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
@@ -172,8 +172,8 @@
     }
 
     assertEquals(
-        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.35, "Incorrect max error");
     assertEquals(
         1.0,
         odometryDistanceTravelled / trajectoryDistanceTravelled,
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java
new file mode 100644
index 0000000..055e8e7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java
@@ -0,0 +1,26 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufChassisSpeeds;
+import org.junit.jupiter.api.Test;
+
+class ChassisSpeedsProtoTest {
+  private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufChassisSpeeds proto = ChassisSpeeds.proto.createMessage();
+    ChassisSpeeds.proto.pack(proto, DATA);
+
+    ChassisSpeeds data = ChassisSpeeds.proto.unpack(proto);
+    assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond);
+    assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond);
+    assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java
new file mode 100644
index 0000000..0d426ad
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java
@@ -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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveKinematics;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveKinematicsProtoTest {
+  private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDifferentialDriveKinematics proto = DifferentialDriveKinematics.proto.createMessage();
+    DifferentialDriveKinematics.proto.pack(proto, DATA);
+
+    DifferentialDriveKinematics data = DifferentialDriveKinematics.proto.unpack(proto);
+    assertEquals(DATA.trackWidthMeters, data.trackWidthMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java
new file mode 100644
index 0000000..1f0b237
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java
@@ -0,0 +1,26 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelSpeeds;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelSpeedsProtoTest {
+  private static final DifferentialDriveWheelSpeeds DATA =
+      new DifferentialDriveWheelSpeeds(1.74, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDifferentialDriveWheelSpeeds proto = DifferentialDriveWheelSpeeds.proto.createMessage();
+    DifferentialDriveWheelSpeeds.proto.pack(proto, DATA);
+
+    DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.proto.unpack(proto);
+    assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond);
+    assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java
new file mode 100644
index 0000000..a94cfab
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java
@@ -0,0 +1,33 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveKinematics;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveKinematicsProtoTest {
+  private static final MecanumDriveKinematics DATA =
+      new MecanumDriveKinematics(
+          new Translation2d(19.1, 2.2),
+          new Translation2d(35.04, 1.91),
+          new Translation2d(1.74, 3.504),
+          new Translation2d(3.504, 1.91));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufMecanumDriveKinematics proto = MecanumDriveKinematics.proto.createMessage();
+    MecanumDriveKinematics.proto.pack(proto, DATA);
+
+    MecanumDriveKinematics data = MecanumDriveKinematics.proto.unpack(proto);
+    assertEquals(DATA.getFrontLeft(), data.getFrontLeft());
+    assertEquals(DATA.getFrontRight(), data.getFrontRight());
+    assertEquals(DATA.getRearLeft(), data.getRearLeft());
+    assertEquals(DATA.getRearRight(), data.getRearRight());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java
new file mode 100644
index 0000000..3e4551d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java
@@ -0,0 +1,28 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelPositions;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelPositionsProtoTest {
+  private static final MecanumDriveWheelPositions DATA =
+      new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufMecanumDriveWheelPositions proto = MecanumDriveWheelPositions.proto.createMessage();
+    MecanumDriveWheelPositions.proto.pack(proto, DATA);
+
+    MecanumDriveWheelPositions data = MecanumDriveWheelPositions.proto.unpack(proto);
+    assertEquals(DATA.frontLeftMeters, data.frontLeftMeters);
+    assertEquals(DATA.frontRightMeters, data.frontRightMeters);
+    assertEquals(DATA.rearLeftMeters, data.rearLeftMeters);
+    assertEquals(DATA.rearRightMeters, data.rearRightMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java
new file mode 100644
index 0000000..3e1d589
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java
@@ -0,0 +1,28 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelSpeeds;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelSpeedsProtoTest {
+  private static final MecanumDriveWheelSpeeds DATA =
+      new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufMecanumDriveWheelSpeeds proto = MecanumDriveWheelSpeeds.proto.createMessage();
+    MecanumDriveWheelSpeeds.proto.pack(proto, DATA);
+
+    MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.proto.unpack(proto);
+    assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond);
+    assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond);
+    assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond);
+    assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java
new file mode 100644
index 0000000..459d119
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java
@@ -0,0 +1,27 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModulePosition;
+import org.junit.jupiter.api.Test;
+
+class SwerveModulePositionProtoTest {
+  private static final SwerveModulePosition DATA =
+      new SwerveModulePosition(3.504, new Rotation2d(17.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufSwerveModulePosition proto = SwerveModulePosition.proto.createMessage();
+    SwerveModulePosition.proto.pack(proto, DATA);
+
+    SwerveModulePosition data = SwerveModulePosition.proto.unpack(proto);
+    assertEquals(DATA.distanceMeters, data.distanceMeters);
+    assertEquals(DATA.angle, data.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java
new file mode 100644
index 0000000..269b7b1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java
@@ -0,0 +1,26 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleState;
+import org.junit.jupiter.api.Test;
+
+class SwerveModuleStateProtoTest {
+  private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufSwerveModuleState proto = SwerveModuleState.proto.createMessage();
+    SwerveModuleState.proto.pack(proto, DATA);
+
+    SwerveModuleState data = SwerveModuleState.proto.unpack(proto);
+    assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
+    assertEquals(DATA.angle, data.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java
new file mode 100644
index 0000000..16ccdb9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ChassisSpeedsStructTest {
+  private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ChassisSpeeds.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ChassisSpeeds.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ChassisSpeeds data = ChassisSpeeds.struct.unpack(buffer);
+    assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond);
+    assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond);
+    assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java
new file mode 100644
index 0000000..ffcf7e7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java
@@ -0,0 +1,27 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveKinematicsStructTest {
+  private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveKinematics.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveKinematics.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveKinematics data = DifferentialDriveKinematics.struct.unpack(buffer);
+    assertEquals(DATA.trackWidthMeters, data.trackWidthMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java
new file mode 100644
index 0000000..720f941
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelSpeedsStructTest {
+  private static final DifferentialDriveWheelSpeeds DATA =
+      new DifferentialDriveWheelSpeeds(1.74, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelSpeeds.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveWheelSpeeds.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.struct.unpack(buffer);
+    assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond);
+    assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java
new file mode 100644
index 0000000..a8d36f6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java
@@ -0,0 +1,36 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveKinematicsStructTest {
+  private static final MecanumDriveKinematics DATA =
+      new MecanumDriveKinematics(
+          new Translation2d(19.1, 2.2),
+          new Translation2d(35.04, 1.91),
+          new Translation2d(1.74, 3.504),
+          new Translation2d(3.504, 1.91));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveKinematics.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    MecanumDriveKinematics.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    MecanumDriveKinematics data = MecanumDriveKinematics.struct.unpack(buffer);
+    assertEquals(DATA.getFrontLeft(), data.getFrontLeft());
+    assertEquals(DATA.getFrontRight(), data.getFrontRight());
+    assertEquals(DATA.getRearLeft(), data.getRearLeft());
+    assertEquals(DATA.getRearRight(), data.getRearRight());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java
new file mode 100644
index 0000000..69f8e03
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelPositionsStructTest {
+  private static final MecanumDriveWheelPositions DATA =
+      new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelPositions.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    MecanumDriveWheelPositions.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    MecanumDriveWheelPositions data = MecanumDriveWheelPositions.struct.unpack(buffer);
+    assertEquals(DATA.frontLeftMeters, data.frontLeftMeters);
+    assertEquals(DATA.frontRightMeters, data.frontRightMeters);
+    assertEquals(DATA.rearLeftMeters, data.rearLeftMeters);
+    assertEquals(DATA.rearRightMeters, data.rearRightMeters);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java
new file mode 100644
index 0000000..b962acd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelSpeedsStructTest {
+  private static final MecanumDriveWheelSpeeds DATA =
+      new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelSpeeds.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    MecanumDriveWheelSpeeds.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.struct.unpack(buffer);
+    assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond);
+    assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond);
+    assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond);
+    assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java
new file mode 100644
index 0000000..819d981
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java
@@ -0,0 +1,30 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class SwerveModulePositionStructTest {
+  private static final SwerveModulePosition DATA =
+      new SwerveModulePosition(3.504, new Rotation2d(17.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(SwerveModulePosition.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    SwerveModulePosition.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    SwerveModulePosition data = SwerveModulePosition.struct.unpack(buffer);
+    assertEquals(DATA.distanceMeters, data.distanceMeters);
+    assertEquals(DATA.angle, data.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java
new file mode 100644
index 0000000..625514c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class SwerveModuleStateStructTest {
+  private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(SwerveModuleState.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    SwerveModuleState.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    SwerveModuleState data = SwerveModuleState.struct.unpack(buffer);
+    assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
+    assertEquals(DATA.angle, data.angle);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java
new file mode 100644
index 0000000..baf9275
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java
@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.optimization;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.function.DoubleUnaryOperator;
+import org.junit.jupiter.api.Test;
+
+class SimulatedAnnealingTest {
+  @Test
+  void testDoubleFunctionOptimizationHeartBeat() {
+    DoubleUnaryOperator function = x -> -(x + Math.sin(x)) * Math.exp(-x * x) + 1;
+
+    double stepSize = 10.0;
+
+    var simulatedAnnealing =
+        new SimulatedAnnealing<Double>(
+            2.0,
+            x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3),
+            function::applyAsDouble);
+
+    double solution = simulatedAnnealing.solve(-1.0, 5000);
+
+    assertEquals(0.68, solution, 1e-1);
+  }
+
+  @Test
+  void testDoubleFunctionOptimizationMultimodal() {
+    DoubleUnaryOperator function = x -> Math.sin(x) + Math.sin((10.0 / 3.0) * x);
+
+    double stepSize = 10.0;
+
+    var simulatedAnnealing =
+        new SimulatedAnnealing<Double>(
+            2.0,
+            x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7),
+            function::applyAsDouble);
+
+    double solution = simulatedAnnealing.solve(-1.0, 5000);
+
+    assertEquals(5.146, solution, 1e-1);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java
new file mode 100644
index 0000000..cf43177
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java
@@ -0,0 +1,108 @@
+// 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.
+
+package edu.wpi.first.math.path;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.CircularBuffer;
+import org.junit.jupiter.api.Test;
+
+class TravelingSalesmanTest {
+  /**
+   * Returns true if the cycles represented by the two lists match.
+   *
+   * @param expected The expected cycle.
+   * @param actual The actual cycle.
+   */
+  private boolean isMatchingCycle(Pose2d[] expected, Pose2d[] actual) {
+    assertEquals(expected.length, actual.length);
+
+    // Check actual has expected cycle (forward)
+    var actualBufferForward = new CircularBuffer<Pose2d>(actual.length);
+    for (int i = 0; i < actual.length; ++i) {
+      actualBufferForward.addLast(actual[i % actual.length]);
+    }
+    boolean matchesExpectedForward = true;
+    for (int i = 0; i < expected.length; ++i) {
+      matchesExpectedForward &= expected[i].equals(actualBufferForward.get(i));
+    }
+
+    // Check actual has expected cycle (reverse)
+    var actualBufferReverse = new CircularBuffer<Pose2d>(actual.length);
+    for (int i = 0; i < actual.length; ++i) {
+      actualBufferReverse.addFirst(actual[(1 + i) % actual.length]);
+    }
+
+    boolean matchesExpectedReverse = true;
+    for (int i = 0; i < expected.length; ++i) {
+      matchesExpectedReverse &= expected[i].equals(actualBufferReverse.get(i));
+    }
+
+    // Actual may be reversed from expected, but that's still valid
+    return matchesExpectedForward || matchesExpectedReverse;
+  }
+
+  @Test
+  void testFiveLengthPathWithDistanceCost() {
+    // ...................
+    // ........2..........
+    // ..0..........4.....
+    // ...................
+    // ....3.....1........
+    // ...................
+    var poses =
+        new Pose2d[] {
+          new Pose2d(3, 3, new Rotation2d(0)),
+          new Pose2d(11, 5, new Rotation2d(0)),
+          new Pose2d(9, 2, new Rotation2d(0)),
+          new Pose2d(5, 5, new Rotation2d(0)),
+          new Pose2d(14, 3, new Rotation2d(0))
+        };
+
+    var traveler = new TravelingSalesman();
+    var solution = traveler.solve(poses, 500);
+
+    assertEquals(5, solution.length);
+    var expected = new Pose2d[] {poses[0], poses[2], poses[4], poses[1], poses[3]};
+    assertTrue(isMatchingCycle(expected, solution));
+  }
+
+  @Test
+  void testTenLengthPathWithDistanceCost() {
+    // ....6.3..1.2.......
+    // ..4................
+    // .............9.....
+    // .0.................
+    // .....7..5...8......
+    // ...................
+    var poses =
+        new Pose2d[] {
+          new Pose2d(2, 4, new Rotation2d(0)),
+          new Pose2d(10, 1, new Rotation2d(0)),
+          new Pose2d(12, 1, new Rotation2d(0)),
+          new Pose2d(7, 1, new Rotation2d(0)),
+          new Pose2d(3, 2, new Rotation2d(0)),
+          new Pose2d(9, 5, new Rotation2d(0)),
+          new Pose2d(5, 1, new Rotation2d(0)),
+          new Pose2d(6, 5, new Rotation2d(0)),
+          new Pose2d(13, 5, new Rotation2d(0)),
+          new Pose2d(14, 3, new Rotation2d(0))
+        };
+
+    var traveler = new TravelingSalesman();
+    var solution = traveler.solve(poses, 500);
+
+    assertEquals(10, solution.length);
+    var expected =
+        new Pose2d[] {
+          poses[0], poses[4], poses[6], poses[3], poses[1], poses[2], poses[9], poses[8], poses[5],
+          poses[7]
+        };
+    assertTrue(isMatchingCycle(expected, solution));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
index 8e3d02b..ecff333 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -151,7 +150,7 @@
         () ->
             run(
                 new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
-                Arrays.asList(new Translation2d(10, 10.5)),
+                List.of(new Translation2d(10, 10.5)),
                 new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
index 7f62933..830135d 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
@@ -19,7 +19,7 @@
   // analytically,
   @Test
   void testDiscretizeA() {
-    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0);
     final var x0 = VecBuilder.fill(1, 1);
 
     final var discA = Discretization.discretizeA(contA, 1.0);
@@ -37,8 +37,8 @@
   // analytically,
   @Test
   void testDiscretizeAB() {
-    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
-    final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1);
+    final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0);
+    final var contB = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1);
 
     final var x0 = VecBuilder.fill(1, 1);
     final var u = VecBuilder.fill(1);
@@ -63,8 +63,8 @@
   //                                               0
   @Test
   void testDiscretizeSlowModelAQ() {
-    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
-    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+    final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0);
+    final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
 
     final double dt = 1.0;
 
@@ -95,8 +95,8 @@
   //                                               0
   @Test
   void testDiscretizeFastModelAQ() {
-    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
-    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
+    final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -1406.29);
+    final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0025, 0, 0, 1);
 
     final var dt = 0.005;
 
@@ -125,8 +125,8 @@
   // Test that DiscretizeR() works
   @Test
   void testDiscretizeR() {
-    var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0);
-    var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0);
+    var contR = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 0.0, 0.0, 1.0);
+    var discRTruth = MatBuilder.fill(Nat.N2(), Nat.N2(), 4.0, 0.0, 0.0, 2.0);
 
     var discR = Discretization.discretizeR(contR, 0.5);
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
index 3f316b3..0c2ca23 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
@@ -7,6 +7,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
@@ -23,31 +24,31 @@
         model
             .getA()
             .isEqual(
-                Matrix.mat(Nat.N2(), Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132),
+                MatBuilder.fill(Nat.N2(), Nat.N2(), -10.14132, 3.06598, 3.06598, -10.14132),
                 0.001));
 
     assertTrue(
         model
             .getB()
             .isEqual(
-                Matrix.mat(Nat.N2(), Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
+                MatBuilder.fill(Nat.N2(), Nat.N2(), 4.2590, -1.28762, -1.2876, 4.2590), 0.001));
 
     assertTrue(
-        model.getC().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001));
+        model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), 0.001));
 
     assertTrue(
-        model.getD().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001));
+        model.getD().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0), 0.001));
   }
 
   @Test
   void testElevatorSystem() {
     var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
     assertTrue(
-        model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -99.05473), 0.001));
+        model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001));
 
     assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
 
-    assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), 0.001));
+    assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), 0.001));
 
     assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
   }
@@ -68,7 +69,7 @@
   void testDCMotorSystem() {
     var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0);
     assertTrue(
-        model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -26.87032), 0.001));
+        model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001));
 
     assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001));
 
@@ -85,7 +86,7 @@
     var ka = 0.5;
     var model = LinearSystemId.identifyPositionSystem(kv, ka);
 
-    assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka));
+    assertEquals(model.getA(), MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kv / ka));
     assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
   }
 
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
index 9b47148..a0c7689 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
@@ -34,12 +34,7 @@
   void testZeroRKDP() {
     var y1 =
         NumericalIntegration.rkdp(
-            (x, u) -> {
-              return VecBuilder.fill(0);
-            },
-            VecBuilder.fill(0),
-            VecBuilder.fill(0),
-            0.1);
+            (x, u) -> VecBuilder.fill(0), VecBuilder.fill(0), VecBuilder.fill(0), 0.1);
 
     assertEquals(0.0, y1.get(0, 0), 1e-3);
   }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
index 3db878e..bb28b73 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
@@ -14,8 +14,8 @@
 
 class RungeKuttaTimeVaryingTest {
   private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
-    return new MatBuilder<>(Nat.N1(), Nat.N1())
-        .fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0));
+    return MatBuilder.fill(
+        Nat.N1(), Nat.N1(), 12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0));
   }
 
   // Tests RK4 with a time varying solution. From
@@ -31,10 +31,9 @@
 
     final var y1 =
         RungeKuttaTimeVarying.rungeKuttaTimeVarying(
-            (Double t, Matrix<N1, N1> x) -> {
-              return new MatBuilder<>(Nat.N1(), Nat.N1())
-                  .fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
-            },
+            (Double t, Matrix<N1, N1> x) ->
+                MatBuilder.fill(
+                    Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)),
             5.0,
             y0,
             1.0);
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java
new file mode 100644
index 0000000..3a6e64a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java
@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.math.system.plant.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.proto.Plant.ProtobufDCMotor;
+import edu.wpi.first.math.system.plant.DCMotor;
+import org.junit.jupiter.api.Test;
+
+class DCMotorProtoTest {
+  private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDCMotor proto = DCMotor.proto.createMessage();
+    DCMotor.proto.pack(proto, DATA);
+
+    DCMotor data = DCMotor.proto.unpack(proto);
+    assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts);
+    assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters);
+    assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps);
+    assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps);
+    assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec);
+    assertEquals(DATA.rOhms, data.rOhms);
+    assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt);
+    assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java
new file mode 100644
index 0000000..806254d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java
@@ -0,0 +1,34 @@
+// 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.
+
+package edu.wpi.first.math.system.plant.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DCMotorStructTest {
+  private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DCMotor.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DCMotor.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DCMotor data = DCMotor.struct.unpack(buffer);
+    assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts);
+    assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters);
+    assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps);
+    assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps);
+    assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec);
+    assertEquals(DATA.rOhms, data.rOhms);
+    assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt);
+    assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
index 917edb4..58ee830 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
@@ -11,7 +11,6 @@
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
@@ -89,7 +88,7 @@
         () ->
             TrajectoryGenerator.generateTrajectory(
                 new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
-                new ArrayList<Translation2d>(),
+                new ArrayList<>(),
                 new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
                 config));
 
@@ -97,7 +96,7 @@
         () ->
             TrajectoryGenerator.generateTrajectory(
                 new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
-                new ArrayList<Translation2d>(),
+                new ArrayList<>(),
                 new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
                 config.setReversed(true)));
   }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
index 45ee1d3..4b0f3fc 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
@@ -7,6 +7,7 @@
 import static edu.wpi.first.math.util.Units.feetToMeters;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -15,7 +16,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -72,7 +72,7 @@
   void testMalformedTrajectory() {
     var traj =
         TrajectoryGenerator.generateTrajectory(
-            Arrays.asList(
+            List.of(
                 new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
                 new Pose2d(1, 0, Rotation2d.fromDegrees(180))),
             new TrajectoryConfig(feetToMeters(12), feetToMeters(12)));
@@ -80,4 +80,21 @@
     assertEquals(traj.getStates().size(), 1);
     assertEquals(traj.getTotalTimeSeconds(), 0);
   }
+
+  @Test
+  void testQuinticCurvatureOptimization() {
+    Trajectory t =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
+                new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
+                new Pose2d(-1, 0, Rotation2d.fromDegrees(270)),
+                new Pose2d(0, -1, Rotation2d.fromDegrees(360)),
+                new Pose2d(1, 0, Rotation2d.fromDegrees(90))),
+            new TrajectoryConfig(2, 2));
+
+    for (int i = 1; i < t.getStates().size() - 1; ++i) {
+      assertNotEquals(0, t.getStates().get(i).curvatureRadPerMeter);
+    }
+  }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
index fb28b69..64dfa47 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -59,7 +59,7 @@
 
     TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 450; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
     }
     assertEquals(state, goal);
   }
@@ -81,7 +81,7 @@
         profile = new TrapezoidProfile(constraints);
       }
 
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
       double estimatedVel = (state.position - lastPos) / kDt;
 
       if (i >= 400) {
@@ -107,7 +107,7 @@
 
     TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 400; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
     }
     assertEquals(state, goal);
   }
@@ -120,14 +120,14 @@
 
     TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 200; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
     }
     assertNotEquals(state, goal);
 
     goal = new TrapezoidProfile.State(0.0, 0.0);
     profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 550; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
     }
     assertEquals(state, goal);
   }
@@ -141,13 +141,13 @@
 
     TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 200; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
     }
     assertNear(constraints.maxVelocity, state.velocity, 10e-5);
 
     profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 2000; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
     }
     assertEquals(state, goal);
   }
@@ -160,7 +160,7 @@
 
     TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
       assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
     }
   }
@@ -176,7 +176,7 @@
     double predictedTimeLeft = profile.timeLeftUntil(goal.position);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
       if (!reachedGoal && state.equals(goal)) {
         // Expected value using for loop index is just an approximation since
         // the time left in the profile doesn't increase linearly at the
@@ -198,7 +198,7 @@
     double predictedTimeLeft = profile.timeLeftUntil(1);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
       if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
         assertNear(predictedTimeLeft, i / 100.0, 2e-2);
         reachedGoal = true;
@@ -217,7 +217,7 @@
     double predictedTimeLeft = profile.timeLeftUntil(goal.position);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
       if (!reachedGoal && state.equals(goal)) {
         // Expected value using for loop index is just an approximation since
         // the time left in the profile doesn't increase linearly at the
@@ -239,7 +239,7 @@
     double predictedTimeLeft = profile.timeLeftUntil(-1);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
       if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
         assertNear(predictedTimeLeft, i / 100.0, 2e-2);
         reachedGoal = true;
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java
new file mode 100644
index 0000000..0477bb7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java
@@ -0,0 +1,40 @@
+// 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.
+
+package edu.wpi.first.math.trajectory.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectory;
+import edu.wpi.first.math.trajectory.Trajectory;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryProtoTest {
+  private static final Trajectory DATA =
+      new Trajectory(
+          List.of(
+              new Trajectory.State(
+                  1.1, 2.2, 3.3, new Pose2d(new Translation2d(1.1, 2.2), new Rotation2d(2.2)), 6.6),
+              new Trajectory.State(
+                  2.1, 2.2, 3.3, new Pose2d(new Translation2d(2.1, 2.2), new Rotation2d(2.2)), 6.6),
+              new Trajectory.State(
+                  3.1,
+                  2.2,
+                  3.3,
+                  new Pose2d(new Translation2d(3.1, 2.2), new Rotation2d(2.2)),
+                  6.6)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTrajectory proto = Trajectory.proto.createMessage();
+    Trajectory.proto.pack(proto, DATA);
+
+    Trajectory data = Trajectory.proto.unpack(proto);
+    assertEquals(DATA.getStates(), data.getStates());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java
new file mode 100644
index 0000000..8a9fdae
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java
@@ -0,0 +1,33 @@
+// 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.
+
+package edu.wpi.first.math.trajectory.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryStateProtoTest {
+  private static final Trajectory.State DATA =
+      new Trajectory.State(
+          1.91, 4.4, 17.4, new Pose2d(new Translation2d(1.74, 19.1), new Rotation2d(22.9)), 0.174);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTrajectoryState proto = Trajectory.State.proto.createMessage();
+    Trajectory.State.proto.pack(proto, DATA);
+
+    Trajectory.State data = Trajectory.State.proto.unpack(proto);
+    assertEquals(DATA.timeSeconds, data.timeSeconds);
+    assertEquals(DATA.velocityMetersPerSecond, data.velocityMetersPerSecond);
+    assertEquals(DATA.accelerationMetersPerSecondSq, data.accelerationMetersPerSecondSq);
+    assertEquals(DATA.poseMeters, data.poseMeters);
+    assertEquals(DATA.curvatureRadPerMeter, data.curvatureRadPerMeter);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 758dc30..4de13d3 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -63,13 +63,12 @@
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
-  frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
-  static_cast<void>(vec);
+  [[maybe_unused]] frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
-  frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
-  static_cast<void>(vec);
+  [[maybe_unused]] frc::Vectord<2> vec =
+      frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
 }
 
 TEST(StateSpaceUtilTest, IsStabilizable) {
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp
new file mode 100644
index 0000000..b0e57c7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp
@@ -0,0 +1,33 @@
+// 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 <gtest/gtest.h>
+
+#include "controller.pb.h"
+#include "frc/controller/ArmFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::ArmFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_rad;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad;
+const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ArmFeedforwardProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  ArmFeedforward unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp
new file mode 100644
index 0000000..1adad90
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "controller.pb.h"
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DifferentialDriveWheelVoltages>;
+
+const DifferentialDriveWheelVoltages kExpectedData =
+    DifferentialDriveWheelVoltages{0.174_V, 0.191_V};
+}  // namespace
+
+TEST(DifferentialDriveWheelVoltagesProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DifferentialDriveWheelVoltages unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp
new file mode 100644
index 0000000..115a3dc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp
@@ -0,0 +1,34 @@
+// 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 <gtest/gtest.h>
+
+#include "controller.pb.h"
+#include "frc/controller/ElevatorFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::ElevatorFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_m;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m;
+
+constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ElevatorFeedforwardProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  ElevatorFeedforward unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp
new file mode 100644
index 0000000..a76f525
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp
@@ -0,0 +1,33 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/controller/ArmFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::ArmFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_rad;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad;
+const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ArmFeedforwardStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  ArmFeedforward unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp
new file mode 100644
index 0000000..c72ac4a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;
+const DifferentialDriveWheelVoltages kExpectedData{
+    DifferentialDriveWheelVoltages{0.174_V, 0.191_V}};
+}  // namespace
+
+TEST(DifferentialDriveWheelVoltagesStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  DifferentialDriveWheelVoltages unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp
new file mode 100644
index 0000000..2e5c43a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp
@@ -0,0 +1,34 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/controller/ElevatorFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::ElevatorFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_m;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m;
+
+constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ElevatorFeedforwardStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  ElevatorFeedforward unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 2a5ce80..d631cfc 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -49,15 +49,13 @@
           k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }
 
-frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
-                                      const frc::Vectord<2>& u) {
-  static_cast<void>(u);
+frc::Vectord<3> LocalMeasurementModel(
+    const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
   return frc::Vectord<3>{x(2), x(3), x(4)};
 }
 
-frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
-                                       const frc::Vectord<2>& u) {
-  static_cast<void>(u);
+frc::Vectord<5> GlobalMeasurementModel(
+    const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
   return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 0f97a88..66be508 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -51,15 +51,13 @@
           k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }
 
-frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
-                                      const frc::Vectord<2>& u) {
-  static_cast<void>(u);
+frc::Vectord<3> LocalMeasurementModel(
+    const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
   return frc::Vectord<3>{x(2), x(3), x(4)};
 }
 
-frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
-                                       const frc::Vectord<2>& u) {
-  static_cast<void>(u);
+frc::Vectord<5> GlobalMeasurementModel(
+    const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
   return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp
new file mode 100644
index 0000000..c6e9d49
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Pose2d>;
+
+const Pose2d kExpectedData =
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}};
+}  // namespace
+
+TEST(Pose2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Pose2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp
new file mode 100644
index 0000000..d8847ca
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp
@@ -0,0 +1,29 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Pose3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Pose3d>;
+
+const Pose3d kExpectedData =
+    Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m},
+           Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}};
+}  // namespace
+
+TEST(Pose3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Pose3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp
new file mode 100644
index 0000000..92338df
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp
@@ -0,0 +1,29 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Quaternion.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Quaternion>;
+
+const Quaternion kExpectedData = Quaternion{1.1, 0.191, 35.04, 19.1};
+}  // namespace
+
+TEST(QuaternionProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Quaternion unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.W(), unpacked_data.W());
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp
new file mode 100644
index 0000000..3ab9e1b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp
@@ -0,0 +1,26 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Rotation2d>;
+
+const Rotation2d kExpectedData = Rotation2d{1.91_rad};
+}  // namespace
+
+TEST(Rotation2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Rotation2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Radians().value(), unpacked_data.Radians().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp
new file mode 100644
index 0000000..a83b78c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Rotation3d>;
+
+const Rotation3d kExpectedData =
+    Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}};
+}  // namespace
+
+TEST(Rotation3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Rotation3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp
new file mode 100644
index 0000000..4b975f3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Transform2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Transform2d>;
+
+const Transform2d kExpectedData =
+    Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}};
+}  // namespace
+
+TEST(Transform2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Transform2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp
new file mode 100644
index 0000000..3a86421
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp
@@ -0,0 +1,29 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Transform3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Transform3d>;
+
+const Transform3d kExpectedData =
+    Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m},
+                Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}};
+}  // namespace
+
+TEST(Transform3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Transform3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp
new file mode 100644
index 0000000..e6a8959
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Translation2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Translation2d>;
+
+const Translation2d kExpectedData = Translation2d{3.504_m, 22.9_m};
+}  // namespace
+
+TEST(Translation2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Translation2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.X().value(), unpacked_data.X().value());
+  EXPECT_EQ(kExpectedData.Y().value(), unpacked_data.Y().value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp
new file mode 100644
index 0000000..3f6d59f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Translation3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Translation3d>;
+
+const Translation3d kExpectedData = Translation3d{35.04_m, 22.9_m, 3.504_m};
+}  // namespace
+
+TEST(Translation3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Translation3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp
new file mode 100644
index 0000000..d9f4fae
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Twist2d>;
+
+const Twist2d kExpectedData = Twist2d{2.29_m, 35.04_m, 35.04_rad};
+}  // namespace
+
+TEST(Twist2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Twist2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp
new file mode 100644
index 0000000..b4e7bf0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp
@@ -0,0 +1,32 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Twist3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Twist3d>;
+
+const Twist3d kExpectedData =
+    Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad};
+}  // namespace
+
+TEST(Twist3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Twist3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value());
+  EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value());
+  EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value());
+  EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp
new file mode 100644
index 0000000..f8763a6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Pose2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Pose2d>;
+const Pose2d kExpectedData{
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}};
+}  // namespace
+
+TEST(Pose2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Pose2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp
new file mode 100644
index 0000000..19a503a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Pose3d>;
+const Pose3d kExpectedData{
+    Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m},
+           Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}};
+}  // namespace
+
+TEST(Pose3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Pose3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp
new file mode 100644
index 0000000..d21eaf1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Quaternion.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Quaternion>;
+const Quaternion kExpectedData{Quaternion{1.1, 0.191, 35.04, 19.1}};
+}  // namespace
+
+TEST(QuaternionStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Quaternion unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.W(), unpacked_data.W());
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp
new file mode 100644
index 0000000..b215c9e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp
@@ -0,0 +1,25 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Rotation2d>;
+const Rotation2d kExpectedData{Rotation2d{1.91_rad}};
+}  // namespace
+
+TEST(Rotation2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Rotation2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Radians(), unpacked_data.Radians());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp
new file mode 100644
index 0000000..9d7dd6c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp
@@ -0,0 +1,26 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Rotation3d>;
+const Rotation3d kExpectedData{
+    Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}};
+}  // namespace
+
+TEST(Rotation3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Rotation3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp
new file mode 100644
index 0000000..e3841e2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Transform2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Transform2d>;
+const Transform2d kExpectedData{
+    Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}};
+}  // namespace
+
+TEST(Transform2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Transform2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp
new file mode 100644
index 0000000..a98fd79
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Transform3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Transform3d>;
+const Transform3d kExpectedData{
+    Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m},
+                Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}};
+}  // namespace
+
+TEST(Transform3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Transform3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp
new file mode 100644
index 0000000..bc081dc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp
@@ -0,0 +1,26 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Translation2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Translation2d>;
+const Translation2d kExpectedData{Translation2d{3.504_m, 22.9_m}};
+}  // namespace
+
+TEST(Translation2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Translation2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp
new file mode 100644
index 0000000..41a58ed
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Translation3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Translation3d>;
+const Translation3d kExpectedData{Translation3d{35.04_m, 22.9_m, 3.504_m}};
+}  // namespace
+
+TEST(Translation3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Translation3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp
new file mode 100644
index 0000000..7bdc8b0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Twist2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Twist2d>;
+const Twist2d kExpectedData{Twist2d{2.29_m, 35.04_m, 35.04_rad}};
+}  // namespace
+
+TEST(Twist2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Twist2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp
new file mode 100644
index 0000000..eb72b54
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp
@@ -0,0 +1,31 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Twist3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Twist3d>;
+const Twist3d kExpectedData{
+    Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}};
+}  // namespace
+
+TEST(Twist3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Twist3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value());
+  EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value());
+  EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value());
+  EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
index e448db2..b41ba3b 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
@@ -11,6 +11,26 @@
 #include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "units/time.h"
 
+TEST(TimeInterpolatableBufferTest, AddSample) {
+  frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
+
+  // No entries
+  buffer.AddSample(1_s, 0_rad);
+  EXPECT_TRUE(buffer.Sample(1_s).value() == 0_rad);
+
+  // New entry at start of container
+  buffer.AddSample(0_s, 1_rad);
+  EXPECT_TRUE(buffer.Sample(0_s).value() == 1_rad);
+
+  // New entry in middle of container
+  buffer.AddSample(0.5_s, 0.5_rad);
+  EXPECT_TRUE(buffer.Sample(0.5_s).value() == 0.5_rad);
+
+  // Override sample
+  buffer.AddSample(0.5_s, 1_rad);
+  EXPECT_TRUE(buffer.Sample(0.5_s).value() == 1_rad);
+}
+
 TEST(TimeInterpolatableBufferTest, Interpolation) {
   frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
 
@@ -28,11 +48,14 @@
 
 TEST(TimeInterpolatableBufferTest, Pose2d) {
   frc::TimeInterpolatableBuffer<frc::Pose2d> buffer{10_s};
+
   // We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
   buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
   buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
   frc::Pose2d sample = buffer.Sample(0.5_s).value();
-  EXPECT_TRUE(std::abs(sample.X().value() - (1 - 1 / std::sqrt(2))) < 0.01);
-  EXPECT_TRUE(std::abs(sample.Y().value() - (1 / std::sqrt(2))) < 0.01);
-  EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45) < 0.01);
+
+  EXPECT_TRUE(std::abs(sample.X().value() - (1.0 - 1.0 / std::sqrt(2.0))) <
+              0.01);
+  EXPECT_TRUE(std::abs(sample.Y().value() - (1.0 / std::sqrt(2.0))) < 0.01);
+  EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45.0) < 0.01);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index 04a3f1f..3fe1471 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -215,3 +215,15 @@
   EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
   EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
 }
+
+TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
+  MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps};
+  wheelSpeeds.Desaturate(5.5_mps);
+
+  constexpr double kFactor = 5.5 / 7.0;
+
+  EXPECT_NEAR(wheelSpeeds.frontLeft.value(), -5.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearRight.value(), -7.0 * kFactor, 1E-9);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp
new file mode 100644
index 0000000..375909a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp
@@ -0,0 +1,29 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::ChassisSpeeds>;
+
+const ChassisSpeeds kExpectedData =
+    ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s};
+}  // namespace
+
+TEST(ChassisSpeedsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  ChassisSpeeds unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value());
+  EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value());
+  EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp
new file mode 100644
index 0000000..4d57108
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DifferentialDriveKinematics>;
+
+const DifferentialDriveKinematics kExpectedData =
+    DifferentialDriveKinematics{1.74_m};
+}  // namespace
+
+TEST(DifferentialDriveKinematicsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DifferentialDriveKinematics unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp
new file mode 100644
index 0000000..83e317f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>;
+
+const DifferentialDriveWheelSpeeds kExpectedData =
+    DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps};
+}  // namespace
+
+TEST(DifferentialDriveWheelSpeedsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DifferentialDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp
new file mode 100644
index 0000000..c601fc3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp
@@ -0,0 +1,31 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::MecanumDriveKinematics>;
+
+const MecanumDriveKinematics kExpectedData = MecanumDriveKinematics{
+    Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m},
+    Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}};
+}  // namespace
+
+TEST(MecanumDriveKinematicsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  MecanumDriveKinematics unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.GetFrontLeft(), unpacked_data.GetFrontLeft());
+  EXPECT_EQ(kExpectedData.GetFrontRight(), unpacked_data.GetFrontRight());
+  EXPECT_EQ(kExpectedData.GetRearLeft(), unpacked_data.GetRearLeft());
+  EXPECT_EQ(kExpectedData.GetRearRight(), unpacked_data.GetRearRight());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp
new file mode 100644
index 0000000..e63ffbe
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp
@@ -0,0 +1,30 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::MecanumDriveWheelPositions>;
+
+const MecanumDriveWheelPositions kExpectedData =
+    MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m};
+}  // namespace
+
+TEST(MecanumDriveWheelPositionsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  MecanumDriveWheelPositions unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp
new file mode 100644
index 0000000..ae91c3a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp
@@ -0,0 +1,30 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::MecanumDriveWheelSpeeds>;
+
+const MecanumDriveWheelSpeeds kExpectedData =
+    MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps};
+}  // namespace
+
+TEST(MecanumDriveWheelSpeedsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  MecanumDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp
new file mode 100644
index 0000000..c511b9f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/SwerveModulePosition.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::SwerveModulePosition>;
+
+const SwerveModulePosition kExpectedData =
+    SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}};
+}  // namespace
+
+TEST(SwerveModulePositionProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  SwerveModulePosition unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp
new file mode 100644
index 0000000..e210db9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/SwerveModuleState.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::SwerveModuleState>;
+
+const SwerveModuleState kExpectedData =
+    SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}};
+}  // namespace
+
+TEST(SwerveModuleStateProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  SwerveModuleState unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp
new file mode 100644
index 0000000..79f2a35
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp
@@ -0,0 +1,28 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::ChassisSpeeds>;
+const ChassisSpeeds kExpectedData{
+    ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}};
+}  // namespace
+
+TEST(ChassisSpeedsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  ChassisSpeeds unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value());
+  EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value());
+  EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp
new file mode 100644
index 0000000..80b4ad5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp
@@ -0,0 +1,26 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DifferentialDriveKinematics>;
+const DifferentialDriveKinematics kExpectedData{
+    DifferentialDriveKinematics{1.74_m}};
+}  // namespace
+
+TEST(DifferentialDriveKinematicsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  DifferentialDriveKinematics unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp
new file mode 100644
index 0000000..8635197
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelSpeeds>;
+const DifferentialDriveWheelSpeeds kExpectedData{
+    DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}};
+}  // namespace
+
+TEST(DifferentialDriveWheelSpeedsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  DifferentialDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp
new file mode 100644
index 0000000..920c668
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp
@@ -0,0 +1,30 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::MecanumDriveKinematics>;
+const MecanumDriveKinematics kExpectedData{MecanumDriveKinematics{
+    Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m},
+    Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}}};
+}  // namespace
+
+TEST(MecanumDriveKinematicsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  MecanumDriveKinematics unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.GetFrontLeft(), unpacked_data.GetFrontLeft());
+  EXPECT_EQ(kExpectedData.GetFrontRight(), unpacked_data.GetFrontRight());
+  EXPECT_EQ(kExpectedData.GetRearLeft(), unpacked_data.GetRearLeft());
+  EXPECT_EQ(kExpectedData.GetRearRight(), unpacked_data.GetRearRight());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp
new file mode 100644
index 0000000..085f69b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp
@@ -0,0 +1,29 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelPositions>;
+const MecanumDriveWheelPositions kExpectedData{
+    MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m}};
+}  // namespace
+
+TEST(MecanumDriveWheelPositionsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  MecanumDriveWheelPositions unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp
new file mode 100644
index 0000000..83a2349
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp
@@ -0,0 +1,29 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelSpeeds>;
+const MecanumDriveWheelSpeeds kExpectedData{
+    MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}};
+}  // namespace
+
+TEST(MecanumDriveWheelSpeedsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  MecanumDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp
new file mode 100644
index 0000000..2f92bf7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::SwerveModulePosition>;
+const SwerveModulePosition kExpectedData{
+    SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}}};
+}  // namespace
+
+TEST(SwerveModulePositionStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  SwerveModulePosition unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp
new file mode 100644
index 0000000..5edc99c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp
@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/SwerveModuleState.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::SwerveModuleState>;
+const SwerveModuleState kExpectedData{
+    SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}};
+}  // namespace
+
+TEST(SwerveModuleStateStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  SwerveModuleState unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/optimization/SimulatedAnnealingTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/optimization/SimulatedAnnealingTest.cpp
new file mode 100644
index 0000000..5c7748a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/optimization/SimulatedAnnealingTest.cpp
@@ -0,0 +1,57 @@
+// 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 <algorithm>
+#include <cmath>
+#include <random>
+
+#include <gtest/gtest.h>
+
+#include "frc/optimization/SimulatedAnnealing.h"
+
+TEST(SimulatedAnnealingTest, DoubleFunctionOptimizationHeartBeat) {
+  auto function = [](double x) {
+    return -(x + std::sin(x)) * std::exp(-x * x) + 1;
+  };
+
+  constexpr double stepSize = 10.0;
+
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+  std::uniform_real_distribution<> distr{0.0, 1.0};
+
+  frc::SimulatedAnnealing<double> simulatedAnnealing{
+      2.0,
+      [&](const double& x) {
+        return std::clamp(x + (distr(gen) - 0.5) * stepSize, -3.0, 3.0);
+      },
+      [&](const double& x) { return function(x); }};
+
+  double solution = simulatedAnnealing.Solve(-1.0, 5000);
+
+  EXPECT_NEAR(0.68, solution, 1e-1);
+}
+
+TEST(SimulatedAnnealingTest, DoubleFunctionOptimizationMultimodal) {
+  auto function = [](double x) {
+    return std::sin(x) + std::sin((10.0 / 3.0) * x);
+  };
+
+  constexpr double stepSize = 10.0;
+
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+  std::uniform_real_distribution<> distr{0.0, 1.0};
+
+  frc::SimulatedAnnealing<double> simulatedAnnealing{
+      2.0,
+      [&](const double& x) {
+        return std::clamp(x + (distr(gen) - 0.5) * stepSize, 0.0, 7.0);
+      },
+      [&](const double& x) { return function(x); }};
+
+  double solution = simulatedAnnealing.Solve(-1.0, 5000);
+
+  EXPECT_NEAR(5.146, solution, 1e-1);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/path/TravelingSalesmanTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/path/TravelingSalesmanTest.cpp
new file mode 100644
index 0000000..f46eb20
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/path/TravelingSalesmanTest.cpp
@@ -0,0 +1,144 @@
+// 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 <cassert>
+#include <span>
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <wpi/array.h>
+#include <wpi/circular_buffer.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/path/TravelingSalesman.h"
+
+/**
+ * Returns true if the cycles represented by the two lists match.
+ *
+ * @param expected The expected cycle.
+ * @param actual The actual cycle.
+ */
+bool IsMatchingCycle(std::span<const frc::Pose2d> expected,
+                     std::span<const frc::Pose2d> actual) {
+  assert(expected.size() == actual.size());
+
+  // Check actual has expected cycle (forward)
+  wpi::circular_buffer<frc::Pose2d> actualBufferForward{expected.size()};
+  for (size_t i = 0; i < actual.size(); ++i) {
+    actualBufferForward.push_back(actual[i % actual.size()]);
+  }
+  bool matchesExpectedForward = true;
+  for (size_t i = 0; i < expected.size(); ++i) {
+    matchesExpectedForward &= (expected[i] == actualBufferForward[i]);
+  }
+
+  // Check actual has expected cycle (reverse)
+  wpi::circular_buffer<frc::Pose2d> actualBufferReverse{expected.size()};
+  for (size_t i = 0; i < actual.size(); ++i) {
+    actualBufferReverse.push_front(actual[(1 + i) % actual.size()]);
+  }
+  bool matchesExpectedReverse = true;
+  for (size_t i = 0; i < expected.size(); ++i) {
+    matchesExpectedReverse &= (expected[i] == actualBufferReverse[i]);
+  }
+
+  // Actual may be reversed from expected, but that's still valid
+  return matchesExpectedForward || matchesExpectedReverse;
+}
+
+TEST(TravelingSalesmanTest, FiveLengthStaticPathWithDistanceCost) {
+  // ...................
+  // ........2..........
+  // ..0..........4.....
+  // ...................
+  // ....3.....1........
+  // ...................
+  wpi::array<frc::Pose2d, 5> poses{
+      frc::Pose2d{3_m, 3_m, 0_rad}, frc::Pose2d{11_m, 5_m, 0_rad},
+      frc::Pose2d{9_m, 2_m, 0_rad}, frc::Pose2d{5_m, 5_m, 0_rad},
+      frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  wpi::array<frc::Pose2d, 5> solution = traveler.Solve(poses, 500);
+
+  wpi::array<frc::Pose2d, 5> expected{poses[0], poses[2], poses[4], poses[1],
+                                      poses[3]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}
+
+TEST(TravelingSalesmanTest, FiveLengthDynamicPathWithDistanceCost) {
+  // ...................
+  // ........2..........
+  // ..0..........4.....
+  // ...................
+  // ....3.....1........
+  // ...................
+  wpi::array<frc::Pose2d, 5> poses{
+      frc::Pose2d{3_m, 3_m, 0_rad}, frc::Pose2d{11_m, 5_m, 0_rad},
+      frc::Pose2d{9_m, 2_m, 0_rad}, frc::Pose2d{5_m, 5_m, 0_rad},
+      frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  std::vector<frc::Pose2d> solution =
+      traveler.Solve(std::span<const frc::Pose2d>{poses}, 500);
+
+  ASSERT_EQ(5u, solution.size());
+  wpi::array<frc::Pose2d, 5> expected{poses[0], poses[2], poses[4], poses[1],
+                                      poses[3]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}
+
+TEST(TravelingSalesmanTest, TenLengthStaticPathWithDistanceCost) {
+  // ....6.3..1.2.......
+  // ..4................
+  // .............9.....
+  // .0.................
+  // .....7..5...8......
+  // ...................
+  wpi::array<frc::Pose2d, 10> poses{
+      frc::Pose2d{2_m, 4_m, 0_rad},  frc::Pose2d{10_m, 1_m, 0_rad},
+      frc::Pose2d{12_m, 1_m, 0_rad}, frc::Pose2d{7_m, 1_m, 0_rad},
+      frc::Pose2d{3_m, 2_m, 0_rad},  frc::Pose2d{9_m, 5_m, 0_rad},
+      frc::Pose2d{5_m, 1_m, 0_rad},  frc::Pose2d{6_m, 5_m, 0_rad},
+      frc::Pose2d{13_m, 5_m, 0_rad}, frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  wpi::array<frc::Pose2d, 10> solution = traveler.Solve(poses, 500);
+
+  wpi::array<frc::Pose2d, 10> expected{poses[0], poses[4], poses[6], poses[3],
+                                       poses[1], poses[2], poses[9], poses[8],
+                                       poses[5], poses[7]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}
+
+TEST(TravelingSalesmanTest, TenLengthDynamicPathWithDistanceCost) {
+  // ....6.3..1.2.......
+  // ..4................
+  // .............9.....
+  // .0.................
+  // .....7..5...8......
+  // ...................
+  wpi::array<frc::Pose2d, 10> poses{
+      frc::Pose2d{2_m, 4_m, 0_rad},  frc::Pose2d{10_m, 1_m, 0_rad},
+      frc::Pose2d{12_m, 1_m, 0_rad}, frc::Pose2d{7_m, 1_m, 0_rad},
+      frc::Pose2d{3_m, 2_m, 0_rad},  frc::Pose2d{9_m, 5_m, 0_rad},
+      frc::Pose2d{5_m, 1_m, 0_rad},  frc::Pose2d{6_m, 5_m, 0_rad},
+      frc::Pose2d{13_m, 5_m, 0_rad}, frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  std::vector<frc::Pose2d> solution =
+      traveler.Solve(std::span<const frc::Pose2d>{poses}, 500);
+
+  ASSERT_EQ(10u, solution.size());
+  wpi::array<frc::Pose2d, 10> expected{poses[0], poses[4], poses[6], poses[3],
+                                       poses[1], poses[2], poses[9], poses[8],
+                                       poses[5], poses[7]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index c98d190..3a4acc0 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -21,9 +21,6 @@
  protected:
   static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
                   const Pose2d& b) {
-    // Start the timer.
-    const auto start = std::chrono::high_resolution_clock::now();
-
     // Generate and parameterize the spline.
 
     const auto [startCV, endCV] =
@@ -40,13 +37,6 @@
       poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
     }
 
-    // End timer.
-    const auto finish = std::chrono::high_resolution_clock::now();
-
-    // Calculate the duration (used when benchmarking)
-    const auto duration =
-        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
-
     for (unsigned int i = 0; i < poses.size() - 1; i++) {
       auto& p0 = poses[i];
       auto& p1 = poses[i + 1];
@@ -87,8 +77,6 @@
     EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
     EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
                 b.Rotation().Radians().value(), 1E-9);
-
-    static_cast<void>(duration);
   }
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index a0df9bb..5f80f3a 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -20,20 +20,10 @@
 class QuinticHermiteSplineTest : public ::testing::Test {
  protected:
   static void Run(const Pose2d& a, const Pose2d& b) {
-    // Start the timer.
-    const auto start = std::chrono::high_resolution_clock::now();
-
     // Generate and parameterize the spline.
     const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0];
     const auto poses = SplineParameterizer::Parameterize(spline);
 
-    // End timer.
-    const auto finish = std::chrono::high_resolution_clock::now();
-
-    // Calculate the duration (used when benchmarking)
-    const auto duration =
-        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
-
     for (unsigned int i = 0; i < poses.size() - 1; i++) {
       auto& p0 = poses[i];
       auto& p1 = poses[i + 1];
@@ -59,8 +49,6 @@
     EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
     EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
                 b.Rotation().Radians().value(), 1E-9);
-
-    static_cast<void>(duration);
   }
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp
new file mode 100644
index 0000000..b664bef
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp
@@ -0,0 +1,42 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/system/plant/DCMotor.h"
+#include "plant.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DCMotor>;
+
+const DCMotor kExpectedData = DCMotor{units::volt_t{1.91},
+                                      units::newton_meter_t{19.1},
+                                      units::ampere_t{1.74},
+                                      units::ampere_t{2.29},
+                                      units::radians_per_second_t{2.2},
+                                      2};
+}  // namespace
+
+TEST(DCMotorProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DCMotor unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.nominalVoltage.value(),
+            unpacked_data.nominalVoltage.value());
+  EXPECT_EQ(kExpectedData.stallTorque.value(),
+            unpacked_data.stallTorque.value());
+  EXPECT_EQ(kExpectedData.stallCurrent.value(),
+            unpacked_data.stallCurrent.value());
+  EXPECT_EQ(kExpectedData.freeCurrent.value(),
+            unpacked_data.freeCurrent.value());
+  EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value());
+  EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value());
+  EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value());
+  EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp
new file mode 100644
index 0000000..50d5c8b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp
@@ -0,0 +1,41 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/system/plant/DCMotor.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DCMotor>;
+const DCMotor kExpectedData = DCMotor{units::volt_t{1.91},
+                                      units::newton_meter_t{19.1},
+                                      units::ampere_t{1.74},
+                                      units::ampere_t{2.29},
+                                      units::radians_per_second_t{2.2},
+                                      2};
+}  // namespace
+
+TEST(DCMotorStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  DCMotor unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.nominalVoltage.value(),
+            unpacked_data.nominalVoltage.value());
+  EXPECT_EQ(kExpectedData.stallTorque.value(),
+            unpacked_data.stallTorque.value());
+  EXPECT_EQ(kExpectedData.stallCurrent.value(),
+            unpacked_data.stallCurrent.value());
+  EXPECT_EQ(kExpectedData.freeCurrent.value(),
+            unpacked_data.freeCurrent.value());
+  EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value());
+  EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value());
+  EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value());
+  EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 8b50160..f2ab4cd 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -41,3 +41,17 @@
   ASSERT_EQ(t.States().size(), 1u);
   ASSERT_EQ(t.TotalTime(), 0_s);
 }
+
+TEST(TrajectoryGenerationTest, CurvatureOptimization) {
+  auto t = TrajectoryGenerator::GenerateTrajectory(
+      {{1_m, 0_m, 90_deg},
+       {0_m, 1_m, 180_deg},
+       {-1_m, 0_m, 270_deg},
+       {0_m, -1_m, 0_deg},
+       {1_m, 0_m, 90_deg}},
+      TrajectoryConfig{12_fps, 12_fps_sq});
+
+  for (size_t i = 1; i < t.States().size() - 1; ++i) {
+    EXPECT_NE(0, t.States()[i].curvature.to<double>());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index b2e5b18..09c1ed5 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -34,7 +34,7 @@
 
   frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 450; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
   }
   EXPECT_EQ(state, goal);
 }
@@ -47,8 +47,8 @@
   frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
 
   frc::TrapezoidProfile<units::meter> profile{constraints};
-  auto state = profile.Calculate(kDt, goal,
-                                 frc::TrapezoidProfile<units::meter>::State{});
+  auto state = profile.Calculate(
+      kDt, frc::TrapezoidProfile<units::meter>::State{}, goal);
 
   auto lastPos = state.position;
   for (int i = 0; i < 1600; ++i) {
@@ -57,7 +57,7 @@
       profile = frc::TrapezoidProfile<units::meter>{constraints};
     }
 
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
     auto estimatedVel = (state.position - lastPos) / kDt;
 
     if (i >= 400) {
@@ -83,7 +83,7 @@
 
   frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 400; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
   }
   EXPECT_EQ(state, goal);
 }
@@ -96,14 +96,14 @@
 
   frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 200; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
   }
   EXPECT_NE(state, goal);
 
   goal = {0.0_m, 0.0_mps};
   profile = frc::TrapezoidProfile<units::meter>{constraints};
   for (int i = 0; i < 550; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
   }
   EXPECT_EQ(state, goal);
 }
@@ -117,13 +117,13 @@
 
   frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 200; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
   }
   EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
 
   profile = frc::TrapezoidProfile<units::meter>{constraints};
   for (int i = 0; i < 2000; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
   }
   EXPECT_EQ(state, goal);
 }
@@ -136,7 +136,7 @@
 
   frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
     EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
   }
 }
@@ -155,7 +155,7 @@
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
       // time left in the profile doesn't increase linearly at the endpoints
@@ -179,7 +179,7 @@
   auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
     if (!reachedGoal &&
         (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
       EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
@@ -202,7 +202,7 @@
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
       // time left in the profile doesn't increase linearly at the endpoints
@@ -226,7 +226,7 @@
   auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
     if (!reachedGoal &&
         (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
       EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp
new file mode 100644
index 0000000..400689b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp
@@ -0,0 +1,35 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/trajectory/Trajectory.h"
+#include "trajectory.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Trajectory>;
+
+const Trajectory kExpectedData = Trajectory{std::vector<frc::Trajectory::State>{
+    Trajectory::State{1.1_s, 2.2_mps, 3.3_mps_sq,
+                      Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)),
+                      units::curvature_t{6.6}},
+    Trajectory::State{2.1_s, 2.2_mps, 3.3_mps_sq,
+                      Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)),
+                      units::curvature_t{6.6}},
+    Trajectory::State{3.1_s, 2.2_mps, 3.3_mps_sq,
+                      Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)),
+                      units::curvature_t{6.6}}}};
+}  // namespace
+
+TEST(TrajectoryProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Trajectory unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.States(), unpacked_data.States());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp
new file mode 100644
index 0000000..1d0b049
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp
@@ -0,0 +1,34 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/trajectory/Trajectory.h"
+#include "trajectory.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Trajectory::State>;
+
+const Trajectory::State kExpectedData = Trajectory::State{
+    1.91_s, 4.4_mps, 17.4_mps_sq,
+    Pose2d{Translation2d{1.74_m, 19.1_m}, Rotation2d{22.9_rad}},
+    units::curvature_t{0.174}};
+}  // namespace
+
+TEST(TrajectoryStateProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Trajectory::State unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.t.value(), unpacked_data.t.value());
+  EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data.velocity.value());
+  EXPECT_EQ(kExpectedData.acceleration.value(),
+            unpacked_data.acceleration.value());
+  EXPECT_EQ(kExpectedData.pose, unpacked_data.pose);
+  EXPECT_EQ(kExpectedData.curvature.value(), unpacked_data.curvature.value());
+}
diff --git a/third_party/allwpilib/wpimath/wpimath-config.cmake.in b/third_party/allwpilib/wpimath/wpimath-config.cmake.in
index 9100d79..8e2c370 100644
--- a/third_party/allwpilib/wpimath/wpimath-config.cmake.in
+++ b/third_party/allwpilib/wpimath/wpimath-config.cmake.in
@@ -8,3 +8,8 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpimath.cmake)
+if(@WITH_JAVA@)
+    @WPIUNITS_DEP_REPLACE@
+    @FILENAME_DEP_REPLACE@
+    include(${SELF_DIR}/wpimath_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/wpinet/CMakeLists.txt b/third_party/allwpilib/wpinet/CMakeLists.txt
index f1eda76..b2c7e04 100644
--- a/third_party/allwpilib/wpinet/CMakeLists.txt
+++ b/third_party/allwpilib/wpinet/CMakeLists.txt
@@ -8,63 +8,73 @@
 file(GLOB wpinet_jni_src src/main/native/cpp/jni/WPINetJNI.cpp)
 
 # Java bindings
-if (WITH_JAVA)
-  find_package(Java REQUIRED)
-  find_package(JNI REQUIRED)
-  include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    find_package(JNI REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  set(CMAKE_JNI_TARGET true)
+    set(CMAKE_JNI_TARGET true)
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
 
-  add_jar(wpinet_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar OUTPUT_NAME wpinet GENERATE_NATIVE_HEADERS wpinet_jni_headers)
+    add_jar(
+        wpinet_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS wpiutil_jar
+        OUTPUT_NAME wpinet
+        GENERATE_NATIVE_HEADERS wpinet_jni_headers
+    )
 
-  get_property(WPINET_JAR_FILE TARGET wpinet_jar PROPERTY JAR_FILE)
-  install(FILES ${WPINET_JAR_FILE} DESTINATION "${java_lib_dest}")
+    install_jar(wpinet_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS wpinet_jar FILE wpinet_jar.cmake DESTINATION share/wpinet)
 
-  set_property(TARGET wpinet_jar PROPERTY FOLDER "java")
+    add_library(wpinetjni ${wpinet_jni_src})
+    wpilib_target_warnings(wpinetjni)
+    target_link_libraries(wpinetjni PUBLIC wpinet)
 
-  add_library(wpinetjni ${wpinet_jni_src})
-  wpilib_target_warnings(wpinetjni)
-  target_link_libraries(wpinetjni PUBLIC wpinet)
+    set_property(TARGET wpinetjni PROPERTY FOLDER "libraries")
 
-  set_property(TARGET wpinetjni PROPERTY FOLDER "libraries")
+    target_link_libraries(wpinetjni PRIVATE wpinet_jni_headers)
+    add_dependencies(wpinetjni wpinet_jar)
 
-  target_link_libraries(wpinetjni PRIVATE wpinet_jni_headers)
-  add_dependencies(wpinetjni wpinet_jar)
-
-  install(TARGETS wpinetjni EXPORT wpinetjni)
-
+    install(TARGETS wpinetjni EXPORT wpinetjni)
 endif()
 
-if (WITH_JAVA_SOURCE)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  file(GLOB WPINET_SOURCES src/main/java/edu/wpi/first/net/*.java)
-  add_jar(wpinet_src_jar
-  RESOURCES NAMESPACE "edu/wpi/first/net" ${WPINET_SOURCES}
-  OUTPUT_NAME wpinet-sources)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB WPINET_SOURCES src/main/java/edu/wpi/first/net/*.java)
+    add_jar(
+        wpinet_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/net" ${WPINET_SOURCES}
+        OUTPUT_NAME wpinet-sources
+    )
 
-  get_property(WPINET_SRC_JAR_FILE TARGET wpinet_src_jar PROPERTY JAR_FILE)
-  install(FILES ${WPINET_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+    get_property(WPINET_SRC_JAR_FILE TARGET wpinet_src_jar PROPERTY JAR_FILE)
+    install(FILES ${WPINET_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-  set_property(TARGET wpinet_src_jar PROPERTY FOLDER "java")
+    set_property(TARGET wpinet_src_jar PROPERTY FOLDER "java")
 endif()
 
 set(THREADS_PREFER_PTHREAD_FLAG ON)
 find_package(Threads REQUIRED)
 
-if (NOT MSVC AND NOT APPLE)
+if(NOT MSVC AND NOT APPLE)
     find_library(ATOMIC NAMES atomic libatomic.so.1)
-    if (ATOMIC)
+    if(ATOMIC)
         message(STATUS "Found libatomic: ${ATOMIC}")
     endif()
 endif()
 
 generate_resources(src/main/native/resources generated/main/cpp WPI wpi wpinet_resources_src)
 
-file(GLOB_RECURSE wpinet_native_src src/main/native/cpp/*.cpp src/main/native/thirdparty/tcpsockets/cpp/*.cpp)
+file(
+    GLOB_RECURSE wpinet_native_src
+    src/main/native/cpp/*.cpp
+    src/main/native/thirdparty/tcpsockets/cpp/*.cpp
+)
 list(REMOVE_ITEM wpinet_native_src ${wpinet_jni_src})
 file(GLOB_RECURSE wpinet_unix_src src/main/native/unix/*.cpp)
 file(GLOB_RECURSE wpinet_linux_src src/main/native/linux/*.cpp)
@@ -123,17 +133,22 @@
 wpilib_target_warnings(wpinet)
 target_link_libraries(wpinet PUBLIC wpiutil)
 
-if (NOT USE_SYSTEM_LIBUV)
+if(NOT USE_SYSTEM_LIBUV)
     target_sources(wpinet PRIVATE ${uv_native_src})
-    install(DIRECTORY src/main/native/thirdparty/libuv/include/ DESTINATION "${include_dest}/wpinet")
-    target_include_directories(wpinet PRIVATE
-        src/main/native/thirdparty/libuv/src)
-    target_include_directories(wpinet PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/libuv/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpinet>)
+    install(
+        DIRECTORY src/main/native/thirdparty/libuv/include/
+        DESTINATION "${include_dest}/wpinet"
+    )
+    target_include_directories(wpinet PRIVATE src/main/native/thirdparty/libuv/src)
+    target_include_directories(
+        wpinet
+        PUBLIC
+            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/libuv/include>
+            $<INSTALL_INTERFACE:${include_dest}/wpinet>
+    )
     if(NOT MSVC)
         target_sources(wpinet PRIVATE ${uv_unix_src})
-        if (APPLE)
+        if(APPLE)
             target_sources(wpinet PRIVATE ${uv_darwin_src})
         else()
             target_sources(wpinet PRIVATE ${uv_linux_src})
@@ -150,38 +165,41 @@
     target_link_libraries(wpinet PUBLIC $<IF:$<TARGET_EXISTS:libuv::uv_a>,libuv::uv_a,libuv::uv>)
 endif()
 
-if (MSVC)
+if(MSVC)
     target_sources(wpinet PRIVATE ${wpinet_windows_src})
-else ()
+else()
     target_sources(wpinet PRIVATE ${wpinet_unix_src})
-    if (APPLE)
+    if(APPLE)
         target_sources(wpinet PRIVATE ${wpinet_macos_src})
     else()
         target_sources(wpinet PRIVATE ${wpinet_linux_src})
     endif()
 endif()
 
-install(DIRECTORY src/main/native/thirdparty/tcpsockets/include/ DESTINATION "${include_dest}/wpinet")
-target_include_directories(wpinet PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/tcpsockets/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpinet>)
+install(
+    DIRECTORY src/main/native/thirdparty/tcpsockets/include/
+    DESTINATION "${include_dest}/wpinet"
+)
+target_include_directories(
+    wpinet
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/tcpsockets/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpinet>
+)
 
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpinet")
-target_include_directories(wpinet PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpinet>)
+target_include_directories(
+    wpinet
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpinet>
+)
 
 install(TARGETS wpinet EXPORT wpinet)
 
-if (WITH_FLAT_INSTALL)
-    set (wpinet_config_dir ${wpilib_dest})
-else()
-    set (wpinet_config_dir share/wpinet)
-endif()
-
-configure_file(wpinet-config.cmake.in ${WPILIB_BINARY_DIR}/wpinet-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/wpinet-config.cmake DESTINATION ${wpinet_config_dir})
-install(EXPORT wpinet DESTINATION ${wpinet_config_dir})
+configure_file(wpinet-config.cmake.in ${WPILIB_BINARY_DIR}/wpinet-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/wpinet-config.cmake DESTINATION share/wpinet)
+install(EXPORT wpinet DESTINATION share/wpinet)
 
 subdir_list(wpinet_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
 foreach(example ${wpinet_examples})
@@ -194,10 +212,10 @@
     endif()
 endforeach()
 
-if (UNIX AND NOT APPLE)
-    set (LIBUTIL -lutil)
+if(UNIX AND NOT APPLE)
+    set(LIBUTIL -lutil)
 else()
-    set (LIBUTIL)
+    set(LIBUTIL)
 endif()
 
 file(GLOB netconsoleServer_src src/netconsoleServer/native/cpp/*.cpp)
@@ -213,7 +231,7 @@
 set_property(TARGET netconsoleServer PROPERTY FOLDER "examples")
 set_property(TARGET netconsoleTee PROPERTY FOLDER "examples")
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(wpinet src/test/native/cpp)
     target_include_directories(wpinet_test PRIVATE src/test/native/include src/main/native/cpp)
     target_link_libraries(wpinet_test wpinet ${LIBUTIL} gmock_main wpiutil_testlib)
diff --git a/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceAnnouncer.java b/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceAnnouncer.java
index 9ccb322..39989e1 100644
--- a/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceAnnouncer.java
+++ b/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceAnnouncer.java
@@ -25,6 +25,7 @@
    * @param port port
    * @param txt txt
    */
+  @SuppressWarnings("this-escape")
   public MulticastServiceAnnouncer(
       String serviceName, String serviceType, int port, Map<String, String> txt) {
     String[] keys = txt.keySet().toArray(String[]::new);
@@ -41,6 +42,7 @@
    * @param serviceType service type
    * @param port port
    */
+  @SuppressWarnings("this-escape")
   public MulticastServiceAnnouncer(String serviceName, String serviceType, int port) {
     m_handle =
         WPINetJNI.createMulticastServiceAnnouncer(serviceName, serviceType, port, null, null);
diff --git a/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceResolver.java b/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceResolver.java
index b676f4c..8d70fd7 100644
--- a/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceResolver.java
+++ b/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/MulticastServiceResolver.java
@@ -21,6 +21,7 @@
    *
    * @param serviceType service type to look for
    */
+  @SuppressWarnings("this-escape")
   public MulticastServiceResolver(String serviceType) {
     m_handle = WPINetJNI.createMulticastServiceResolver(serviceType);
     m_cleanable = WPICleaner.register(this, cleanupAction(m_handle));
diff --git a/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java b/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java
index d0bcf7d..4f0e189 100644
--- a/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java
+++ b/third_party/allwpilib/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java
@@ -8,20 +8,35 @@
 import java.io.IOException;
 import java.util.concurrent.atomic.AtomicBoolean;
 
+/** WPINet JNI. */
 public class WPINetJNI {
   static boolean libraryLoaded = false;
   static RuntimeLoader<WPINetJNI> loader = null;
 
+  /** Sets whether JNI should be loaded in the static block. */
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
     public static boolean getExtractOnStaticLoad() {
       return extractOnStaticLoad.get();
     }
 
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
     public static void setExtractOnStaticLoad(boolean load) {
       extractOnStaticLoad.set(load);
     }
+
+    /** Utility class. */
+    private Helper() {}
   }
 
   static {
@@ -54,8 +69,21 @@
     libraryLoaded = true;
   }
 
+  /**
+   * Forward a local TCP port to a remote host and port. Note that local ports less than 1024 won't
+   * work as a normal user.
+   *
+   * @param port local port number
+   * @param remoteHost remote IP address / DNS name
+   * @param remotePort remote port number
+   */
   public static native void addPortForwarder(int port, String remoteHost, int remotePort);
 
+  /**
+   * Stop TCP forwarding on a port.
+   *
+   * @param port local port number
+   */
   public static native void removePortForwarder(int port);
 
   public static native int createMulticastServiceAnnouncer(
@@ -82,4 +110,7 @@
   public static native int getMulticastServiceResolverEventHandle(int handle);
 
   public static native ServiceData[] getMulticastServiceResolverData(int handle);
+
+  /** Utility class. */
+  private WPINetJNI() {}
 }
diff --git a/third_party/allwpilib/wpinet/src/main/native/cpp/UDPClient.cpp b/third_party/allwpilib/wpinet/src/main/native/cpp/UDPClient.cpp
index 5963d60..1596b59 100644
--- a/third_party/allwpilib/wpinet/src/main/native/cpp/UDPClient.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/cpp/UDPClient.cpp
@@ -110,6 +110,7 @@
 #endif
   }
 
+  // NOLINTNEXTLINE(modernize-avoid-bind)
   int result = bind(m_lsd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
   if (result != 0) {
     WPI_ERROR(m_logger, "bind() failed: {}", SocketStrerror());
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv.h b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv.h
index d5342b0..aff06cb 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv.h
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv.h
@@ -1876,6 +1876,18 @@
 UV_EXTERN void* uv_loop_get_data(const uv_loop_t*);
 UV_EXTERN void uv_loop_set_data(uv_loop_t*, void* data);
 
+/* String utilities needed internally for dealing with Windows. */
+size_t uv_utf16_length_as_wtf8(const uint16_t* utf16,
+                               ssize_t utf16_len);
+int uv_utf16_to_wtf8(const uint16_t* utf16,
+                     ssize_t utf16_len,
+                     char** wtf8_ptr,
+                     size_t* wtf8_len_ptr);
+ssize_t uv_wtf8_length_as_utf16(const char* wtf8);
+void uv_wtf8_to_utf16(const char* wtf8,
+                      uint16_t* utf16,
+                      size_t utf16_len);
+
 /* Don't export the private CPP symbols. */
 #undef UV_HANDLE_TYPE_PRIVATE
 #undef UV_REQ_TYPE_PRIVATE
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h
index 24fac8d..b17220f 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h
@@ -31,7 +31,7 @@
  */
 
 #define UV_VERSION_MAJOR 1
-#define UV_VERSION_MINOR 46
+#define UV_VERSION_MINOR 47
 #define UV_VERSION_PATCH 0
 #define UV_VERSION_IS_RELEASE 1
 #define UV_VERSION_SUFFIX ""
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/fs-poll.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/fs-poll.cpp
index 1a7ca70..5a39dae 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/fs-poll.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/fs-poll.cpp
@@ -34,7 +34,6 @@
 #include <stdlib.h>
 #include <string.h>
 
-
 struct poll_ctx {
   uv_fs_poll_t* parent_handle;
   int busy_polling;
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp
index 36a39a0..fe60fb5 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011, 2018 Ben Noordhuis <info@bnoordhuis.nl>
+/* Copyright libuv contributors. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -18,11 +18,56 @@
  */
 
 #include "uv.h"
+#include "uv-common.h"
 #include "idna.h"
 #include <assert.h>
 #include <string.h>
 #include <limits.h> /* UINT_MAX */
 
+
+static int32_t uv__wtf8_decode1(const char** input) {
+  uint32_t code_point;
+  uint8_t b1;
+  uint8_t b2;
+  uint8_t b3;
+  uint8_t b4;
+
+  b1 = **input;
+  if (b1 <= 0x7F)
+    return b1; /* ASCII code point */
+  if (b1 < 0xC2)
+    return -1; /* invalid: continuation byte */
+  code_point = b1;
+
+  b2 = *++*input;
+  if ((b2 & 0xC0) != 0x80)
+    return -1; /* invalid: not a continuation byte */
+  code_point = (code_point << 6) | (b2 & 0x3F);
+  if (b1 <= 0xDF)
+    return 0x7FF & code_point; /* two-byte character */
+
+  b3 = *++*input;
+  if ((b3 & 0xC0) != 0x80)
+    return -1; /* invalid: not a continuation byte */
+  code_point = (code_point << 6) | (b3 & 0x3F);
+  if (b1 <= 0xEF)
+    return 0xFFFF & code_point; /* three-byte character */
+
+  b4 = *++*input;
+  if ((b4 & 0xC0) != 0x80)
+    return -1; /* invalid: not a continuation byte */
+  code_point = (code_point << 6) | (b4 & 0x3F);
+  if (b1 <= 0xF4) {
+    code_point &= 0x1FFFFF;
+    if (code_point <= 0x10FFFF)
+      return code_point; /* four-byte character */
+  }
+
+  /* code point too large */
+  return -1;
+}
+
+
 static unsigned uv__utf8_decode1_slow(const char** p,
                                       const char* pe,
                                       unsigned a) {
@@ -89,6 +134,7 @@
   return a;
 }
 
+
 unsigned uv__utf8_decode1(const char** p, const char* pe) {
   unsigned a;
 
@@ -102,6 +148,7 @@
   return uv__utf8_decode1_slow(p, pe, a);
 }
 
+
 static int uv__idna_toascii_label(const char* s, const char* se,
                                   char** d, char* de) {
   static const char alphabet[] = "abcdefghijklmnopqrstuvwxyz0123456789";
@@ -267,7 +314,8 @@
   return 0;
 }
 
-long uv__idna_toascii(const char* s, const char* se, char* d, char* de) {
+
+ssize_t uv__idna_toascii(const char* s, const char* se, char* d, char* de) {
   const char* si;
   const char* st;
   unsigned c;
@@ -313,3 +361,195 @@
 
   return d - ds;  /* Number of bytes written. */
 }
+
+
+ssize_t uv_wtf8_length_as_utf16(const char* source_ptr) {
+  size_t w_target_len = 0;
+  int32_t code_point;
+
+  do {
+    code_point = uv__wtf8_decode1(&source_ptr);
+    if (code_point < 0)
+      return -1;
+    if (code_point > 0xFFFF)
+      w_target_len++;
+    w_target_len++;
+  } while (*source_ptr++);
+
+  return w_target_len;
+}
+
+
+void uv_wtf8_to_utf16(const char* source_ptr,
+                      uint16_t* w_target,
+                      [[maybe_unused]] size_t w_target_len) {
+  int32_t code_point;
+
+  do {
+    code_point = uv__wtf8_decode1(&source_ptr);
+    /* uv_wtf8_length_as_utf16 should have been called and checked first. */
+    assert(code_point >= 0);
+    if (code_point > 0x10000) {
+      assert(code_point < 0x10FFFF);
+      *w_target++ = (((code_point - 0x10000) >> 10) + 0xD800);
+      *w_target++ = ((code_point - 0x10000) & 0x3FF) + 0xDC00;
+      w_target_len -= 2;
+    } else {
+      *w_target++ = code_point;
+      w_target_len -= 1;
+    }
+  } while (*source_ptr++);
+
+  assert(w_target_len == 0);
+}
+
+
+static int32_t uv__get_surrogate_value(const uint16_t* w_source_ptr,
+                                       ssize_t w_source_len) {
+  uint16_t u;
+  uint16_t next;
+
+  u = w_source_ptr[0];
+  if (u >= 0xD800 && u <= 0xDBFF && w_source_len != 1) {
+    next = w_source_ptr[1];
+    if (next >= 0xDC00 && next <= 0xDFFF)
+      return 0x10000 + ((u - 0xD800) << 10) + (next - 0xDC00);
+  }
+  return u;
+}
+
+
+size_t uv_utf16_length_as_wtf8(const uint16_t* w_source_ptr,
+                               ssize_t w_source_len) {
+  size_t target_len;
+  int32_t code_point;
+
+  target_len = 0;
+  while (w_source_len) {
+    code_point = uv__get_surrogate_value(w_source_ptr, w_source_len);
+    /* Can be invalid UTF-8 but must be valid WTF-8. */
+    assert(code_point >= 0);
+    if (w_source_len < 0 && code_point == 0)
+      break;
+    if (code_point < 0x80)
+      target_len += 1;
+    else if (code_point < 0x800)
+      target_len += 2;
+    else if (code_point < 0x10000)
+      target_len += 3;
+    else {
+      target_len += 4;
+      w_source_ptr++;
+      if (w_source_len > 0)
+        w_source_len--;
+    }
+    w_source_ptr++;
+    if (w_source_len > 0)
+      w_source_len--;
+  }
+
+  return target_len;
+}
+
+
+int uv_utf16_to_wtf8(const uint16_t* w_source_ptr,
+                     ssize_t w_source_len,
+                     char** target_ptr,
+                     size_t* target_len_ptr) {
+  size_t target_len;
+  char* target;
+  char* target_end;
+  int32_t code_point;
+
+  /* If *target_ptr is provided, then *target_len_ptr must be its length
+   * (excluding space for NUL), otherwise we will compute the target_len_ptr
+   * length and may return a new allocation in *target_ptr if target_ptr is
+   * provided. */
+  if (target_ptr == NULL || *target_ptr == NULL) {
+    target_len = uv_utf16_length_as_wtf8(w_source_ptr, w_source_len);
+    if (target_len_ptr != NULL)
+      *target_len_ptr = target_len;
+  } else {
+    target_len = *target_len_ptr;
+  }
+
+  if (target_ptr == NULL)
+    return 0;
+
+  if (*target_ptr == NULL) {
+    target = (char*)uv__malloc(target_len + 1);
+    if (target == NULL) {
+      return UV_ENOMEM;
+    }
+    *target_ptr = target;
+  } else {
+    target = *target_ptr;
+  }
+
+  target_end = target + target_len;
+
+  while (target != target_end && w_source_len) {
+    code_point = uv__get_surrogate_value(w_source_ptr, w_source_len);
+    /* Can be invalid UTF-8 but must be valid WTF-8. */
+    assert(code_point >= 0);
+    if (w_source_len < 0 && code_point == 0) {
+      w_source_len = 0;
+      break;
+    }
+    if (code_point < 0x80) {
+      *target++ = code_point;
+    } else if (code_point < 0x800) {
+      *target++ = 0xC0 | (code_point >> 6);
+      if (target == target_end)
+        break;
+      *target++ = 0x80 | (code_point & 0x3F);
+    } else if (code_point < 0x10000) {
+      *target++ = 0xE0 | (code_point >> 12);
+      if (target == target_end)
+        break;
+      *target++ = 0x80 | ((code_point >> 6) & 0x3F);
+      if (target == target_end)
+        break;
+      *target++ = 0x80 | (code_point & 0x3F);
+    } else {
+      *target++ = 0xF0 | (code_point >> 18);
+      if (target == target_end)
+        break;
+      *target++ = 0x80 | ((code_point >> 12) & 0x3F);
+      if (target == target_end)
+        break;
+      *target++ = 0x80 | ((code_point >> 6) & 0x3F);
+      if (target == target_end)
+        break;
+      *target++ = 0x80 | (code_point & 0x3F);
+      /* uv__get_surrogate_value consumed 2 input characters */
+      w_source_ptr++;
+      if (w_source_len > 0)
+        w_source_len--;
+    }
+    target_len = target - *target_ptr;
+    w_source_ptr++;
+    if (w_source_len > 0)
+      w_source_len--;
+  }
+
+  if (target != target_end && target_len_ptr != NULL)
+    /* Did not fill all of the provided buffer, so update the target_len_ptr
+     * output with the space used. */
+    *target_len_ptr = target - *target_ptr;
+
+  /* Check if input fit into target exactly. */
+  if (w_source_len < 0 && target == target_end && w_source_ptr[0] == 0)
+    w_source_len = 0;
+
+  *target++ = '\0';
+
+  /* Characters remained after filling the buffer, compute the remaining length now. */
+  if (w_source_len) {
+    if (target_len_ptr != NULL)
+      *target_len_ptr = target_len + uv_utf16_length_as_wtf8(w_source_ptr, w_source_len);
+    return UV_ENOBUFS;
+  }
+
+  return 0;
+}
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.h b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.h
index 8e0c592..ea6b4df 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.h
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/idna.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011, 2018 Ben Noordhuis <info@bnoordhuis.nl>
+/* Copyright libuv contributors. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -26,6 +26,6 @@
  * is the number of bytes written to |d|, including the trailing nul byte.
  * A return value < 0 is a libuv error code. |s| and |d| can not overlap.
  */
-long uv__idna_toascii(const char* s, const char* se, char* d, char* de);
+ssize_t uv__idna_toascii(const char* s, const char* se, char* d, char* de);
 
 #endif  /* UV_SRC_IDNA_H_ */
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/core.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/core.cpp
index ce7fd2c..1a52716 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/core.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/core.cpp
@@ -576,7 +576,6 @@
   return peerfd;
 }
 
-
 #if defined(__APPLE__)
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdollar-in-identifier-extension"
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp
index 9ee5cd8..dc93d23 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp
@@ -209,7 +209,7 @@
   if (cpuspeed == 0)
     /* If sysctl hw.cputype == CPU_TYPE_ARM64, the correct value is unavailable
      * from Apple, but we can hard-code it here to a plausible value. */
-    cpuspeed = 2400000000;
+    cpuspeed = 2400000000U;
 
   if (host_processor_info(mach_host_self(), PROCESSOR_CPU_LOAD_INFO, &numcpus,
                           (processor_info_array_t*)&info,
@@ -235,7 +235,7 @@
     cpu_info->cpu_times.irq = 0;
 
     cpu_info->model = uv__strdup(model);
-    cpu_info->speed = cpuspeed/1000000;
+    cpu_info->speed = (int)(cpuspeed / 1000000);
   }
   vm_deallocate(mach_task_self(), (vm_address_t)info, msg_type);
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp
index aba190a..afdf0c6 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp
@@ -41,27 +41,12 @@
 #include <sys/stat.h>
 #include <sys/time.h>
 #include <sys/uio.h>
-#include <pthread.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
 
 #include <atomic>
 
-#if defined(__DragonFly__)        ||                                      \
-    defined(__FreeBSD__)          ||                                      \
-    defined(__OpenBSD__)          ||                                      \
-    defined(__NetBSD__)
-# define HAVE_PREADV 1
-#else
-# define HAVE_PREADV 0
-#endif
-
-/* preadv() and pwritev() were added in Android N (level 24) */
-#if defined(__linux__) && !(defined(__ANDROID__) && __ANDROID_API__ < 24)
-# define TRY_PREADV 1
-#endif
-
 #if defined(__linux__)
 # include <sys/sendfile.h>
 #endif
@@ -99,6 +84,15 @@
 # include <sys/statfs.h>
 #endif
 
+#if defined(__CYGWIN__) ||                                                    \
+    (defined(__HAIKU__) && B_HAIKU_VERSION < B_HAIKU_VERSION_1_PRE_BETA_5) || \
+    (defined(__sun) && !defined(__illumos__))
+#define preadv(fd, bufs, nbufs, off)                                          \
+  pread(fd, (bufs)->iov_base, (bufs)->iov_len, off)
+#define pwritev(fd, bufs, nbufs, off)                                         \
+  pwrite(fd, (bufs)->iov_base, (bufs)->iov_len, off)
+#endif
+
 #if defined(_AIX) && _XOPEN_SOURCE <= 600
 extern char *mkdtemp(char *template); /* See issue #740 on AIX < 7 */
 #endif
@@ -412,123 +406,57 @@
 }
 
 
-#if !HAVE_PREADV
-static ssize_t uv__fs_preadv(uv_file fd,
-                             uv_buf_t* bufs,
-                             unsigned int nbufs,
-                             off_t off) {
-  uv_buf_t* buf;
-  uv_buf_t* end;
-  ssize_t result;
-  ssize_t rc;
-  size_t pos;
-
-  assert(nbufs > 0);
-
-  result = 0;
-  pos = 0;
-  buf = bufs + 0;
-  end = bufs + nbufs;
-
-  for (;;) {
-    do
-      rc = pread(fd, buf->base + pos, buf->len - pos, off + result);
-    while (rc == -1 && errno == EINTR);
-
-    if (rc == 0)
-      break;
-
-    if (rc == -1 && result == 0)
-      return UV__ERR(errno);
-
-    if (rc == -1)
-      break;  /* We read some data so return that, ignore the error. */
-
-    pos += rc;
-    result += rc;
-
-    if (pos < buf->len)
-      continue;
-
-    pos = 0;
-    buf += 1;
-
-    if (buf == end)
-      break;
-  }
-
-  return result;
-}
-#endif
-
-
 static ssize_t uv__fs_read(uv_fs_t* req) {
-#if TRY_PREADV
-  static std::atomic<int> no_preadv;
-#endif
+  const struct iovec* bufs;
   unsigned int iovmax;
-  ssize_t result;
+  size_t nbufs;
+  ssize_t r;
+  off_t off;
+  int fd;
+
+  fd = req->file;
+  off = req->off;
+  bufs = (const struct iovec*) req->bufs;
+  nbufs = req->nbufs;
 
   iovmax = uv__getiovmax();
-  if (req->nbufs > iovmax)
-    req->nbufs = iovmax;
+  if (nbufs > iovmax)
+    nbufs = iovmax;
 
-  if (req->off < 0) {
-    if (req->nbufs == 1)
-      result = read(req->file, req->bufs[0].base, req->bufs[0].len);
-    else
-      result = readv(req->file, (struct iovec*) req->bufs, req->nbufs);
+  r = 0;
+  if (off < 0) {
+    if (nbufs == 1)
+      r = read(fd, bufs->iov_base, bufs->iov_len);
+    else if (nbufs > 1)
+      r = readv(fd, bufs, nbufs);
   } else {
-    if (req->nbufs == 1) {
-      result = pread(req->file, req->bufs[0].base, req->bufs[0].len, req->off);
-      goto done;
-    }
-
-#if HAVE_PREADV
-    result = preadv(req->file, (struct iovec*) req->bufs, req->nbufs, req->off);
-#else
-# if TRY_PREADV
-    if (atomic_load_explicit(&no_preadv, std::memory_order_relaxed)) retry:
-# endif
-    {
-      result = uv__fs_preadv(req->file, req->bufs, req->nbufs, req->off);
-    }
-# if TRY_PREADV
-    else {
-      result = preadv(req->file,
-                      (struct iovec*) req->bufs,
-                      req->nbufs,
-                      req->off);
-      if (result == -1 && errno == ENOSYS) {
-        atomic_store_explicit(&no_preadv, 1, std::memory_order_relaxed);
-        goto retry;
-      }
-    }
-# endif
-#endif
+    if (nbufs == 1)
+      r = pread(fd, bufs->iov_base, bufs->iov_len, off);
+    else if (nbufs > 1)
+      r = preadv(fd, bufs, nbufs, off);
   }
 
-done:
-  /* Early cleanup of bufs allocation, since we're done with it. */
-  if (req->bufs != req->bufsml)
-    uv__free(req->bufs);
-
-  req->bufs = NULL;
-  req->nbufs = 0;
-
 #ifdef __PASE__
   /* PASE returns EOPNOTSUPP when reading a directory, convert to EISDIR */
-  if (result == -1 && errno == EOPNOTSUPP) {
+  if (r == -1 && errno == EOPNOTSUPP) {
     struct stat buf;
     ssize_t rc;
-    rc = uv__fstat(req->file, &buf);
+    rc = uv__fstat(fd, &buf);
     if (rc == 0 && S_ISDIR(buf.st_mode)) {
       errno = EISDIR;
     }
   }
 #endif
 
-  return result;
+  /* We don't own the buffer list in the synchronous case. */
+  if (req->cb != NULL)
+    if (req->bufs != req->bufsml)
+      uv__free(req->bufs);
+
+  req->bufs = NULL;
+  req->nbufs = 0;
+
+  return r;
 }
 
 
@@ -1163,65 +1091,34 @@
 
 
 static ssize_t uv__fs_write(uv_fs_t* req) {
-#if TRY_PREADV
-  static std::atomic<int> no_pwritev;
-#endif
+  const struct iovec* bufs;
+  size_t nbufs;
   ssize_t r;
+  off_t off;
+  int fd;
 
-  /* Serialize writes on OS X, concurrent write() and pwrite() calls result in
-   * data loss. We can't use a per-file descriptor lock, the descriptor may be
-   * a dup().
-   */
-#if defined(__APPLE__)
-  static pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
+  fd = req->file;
+  off = req->off;
+  bufs = (const struct iovec*) req->bufs;
+  nbufs = req->nbufs;
 
-  if (pthread_mutex_lock(&lock))
-    abort();
-#endif
-
-  if (req->off < 0) {
-    if (req->nbufs == 1)
-      r = write(req->file, req->bufs[0].base, req->bufs[0].len);
-    else
-      r = writev(req->file, (struct iovec*) req->bufs, req->nbufs);
+  r = 0;
+  if (off < 0) {
+    if (nbufs == 1)
+      r = write(fd, bufs->iov_base, bufs->iov_len);
+    else if (nbufs > 1)
+      r = writev(fd, bufs, nbufs);
   } else {
-    if (req->nbufs == 1) {
-      r = pwrite(req->file, req->bufs[0].base, req->bufs[0].len, req->off);
-      goto done;
-    }
-#if HAVE_PREADV
-    r = pwritev(req->file, (struct iovec*) req->bufs, req->nbufs, req->off);
-#else
-# if TRY_PREADV
-    if (atomic_load_explicit(&no_pwritev, std::memory_order_relaxed)) retry:
-# endif
-    {
-      r = pwrite(req->file, req->bufs[0].base, req->bufs[0].len, req->off);
-    }
-# if TRY_PREADV
-    else {
-      r = pwritev(req->file,
-                  (struct iovec*) req->bufs,
-                  req->nbufs,
-                  req->off);
-      if (r == -1 && errno == ENOSYS) {
-        atomic_store_explicit(&no_pwritev, 1, std::memory_order_relaxed);
-        goto retry;
-      }
-    }
-# endif
-#endif
+    if (nbufs == 1)
+      r = pwrite(fd, bufs->iov_base, bufs->iov_len, off);
+    else if (nbufs > 1)
+      r = pwritev(fd, bufs, nbufs, off);
   }
 
-done:
-#if defined(__APPLE__)
-  if (pthread_mutex_unlock(&lock))
-    abort();
-#endif
-
   return r;
 }
 
+
 static ssize_t uv__fs_copyfile(uv_fs_t* req) {
   uv_fs_t fs_req;
   uv_file srcfd;
@@ -1981,9 +1878,14 @@
   if (bufs == NULL || nbufs == 0)
     return UV_EINVAL;
 
+  req->off = off;
   req->file = file;
-
+  req->bufs = (uv_buf_t*) bufs;  /* Safe, doesn't mutate |bufs| */
   req->nbufs = nbufs;
+
+  if (cb == NULL)
+    goto post;
+
   req->bufs = req->bufsml;
   if (nbufs > ARRAY_SIZE(req->bufsml))
     req->bufs = (uv_buf_t*)uv__malloc(nbufs * sizeof(*bufs));
@@ -1993,12 +1895,10 @@
 
   memcpy(req->bufs, bufs, nbufs * sizeof(*bufs));
 
-  req->off = off;
+  if (uv__iou_fs_read_or_write(loop, req, /* is_read */ 1))
+    return 0;
 
-  if (cb != NULL)
-    if (uv__iou_fs_read_or_write(loop, req, /* is_read */ 1))
-      return 0;
-
+post:
   POST;
 }
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp
index ffe0f91..939a426 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp
@@ -30,6 +30,9 @@
 #include <sys/types.h>
 #include <sys/event.h>
 #include <sys/time.h>
+#if defined(__FreeBSD__)
+#include <sys/user.h>
+#endif
 #include <unistd.h>
 #include <fcntl.h>
 #include <time.h>
@@ -264,6 +267,9 @@
 
     if (nfds == -1)
       assert(errno == EINTR);
+    else if (nfds == 0)
+      /* Unlimited timeout should only return with events or signal. */
+      assert(timeout != -1);
 
     if (pset != NULL)
       pthread_sigmask(SIG_UNBLOCK, pset, NULL);
@@ -288,8 +294,6 @@
         timeout = user_timeout;
         reset_timeout = 0;
       } else if (nfds == 0) {
-        /* Reached the user timeout value. */
-        assert(timeout != -1);
         return;
       }
 
@@ -481,6 +485,16 @@
    */
   if (fcntl(handle->event_watcher.fd, F_GETPATH, pathbuf) == 0)
     path = uv__basename_r(pathbuf);
+#elif defined(F_KINFO)
+  /* We try to get the file info reference from the file descriptor.
+   * the struct's kf_structsize must be initialised beforehand
+   * whether with the KINFO_FILE_SIZE constant or this way.
+   */
+  struct kinfo_file kf;
+  kf.kf_structsize = sizeof(kf);
+
+  if (fcntl(handle->event_watcher.fd, F_KINFO, &kf) == 0)
+    path = uv__basename_r(kf.kf_path);
 #endif
   handle->cb(handle, path, events, 0);
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp
index d365b62..95a2fe2 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp
@@ -78,6 +78,8 @@
 #  define __NR_copy_file_range 379
 # elif defined(__arc__)
 #  define __NR_copy_file_range 285
+# elif defined(__riscv)
+#  define __NR_copy_file_range 285
 # endif
 #endif /* __NR_copy_file_range */
 
@@ -94,6 +96,8 @@
 #  define __NR_statx 383
 # elif defined(__s390__)
 #  define __NR_statx 379
+# elif defined(__riscv)
+#  define __NR_statx 291
 # endif
 #endif /* __NR_statx */
 
@@ -110,6 +114,8 @@
 #  define __NR_getrandom 359
 # elif defined(__s390__)
 #  define __NR_getrandom 349
+# elif defined(__riscv)
+#  define __NR_getrandom 278
 # endif
 #endif /* __NR_getrandom */
 
@@ -318,17 +324,64 @@
   unsigned major;
   unsigned minor;
   unsigned patch;
+  char v_sig[256];
+  char* needle;
 
   version = std::atomic_load_explicit(&cached_version, std::memory_order_relaxed);
   if (version != 0)
     return version;
 
+  /* Check /proc/version_signature first as it's the way to get the mainline
+   * kernel version in Ubuntu. The format is:
+   *   Ubuntu ubuntu_kernel_version mainline_kernel_version
+   * For example:
+   *   Ubuntu 5.15.0-79.86-generic 5.15.111
+   */
+  if (0 == uv__slurp("/proc/version_signature", v_sig, sizeof(v_sig)))
+    if (3 == sscanf(v_sig, "Ubuntu %*s %u.%u.%u", &major, &minor, &patch))
+      goto calculate_version;
+
   if (-1 == uname(&u))
     return 0;
 
+  /* In Debian we need to check `version` instead of `release` to extract the
+   * mainline kernel version. This is an example of how it looks like:
+   *  #1 SMP Debian 5.10.46-4 (2021-08-03)
+   */
+  needle = strstr(u.version, "Debian ");
+  if (needle != NULL)
+    if (3 == sscanf(needle, "Debian %u.%u.%u", &major, &minor, &patch))
+      goto calculate_version;
+
   if (3 != sscanf(u.release, "%u.%u.%u", &major, &minor, &patch))
     return 0;
 
+  /* Handle it when the process runs under the UNAME26 personality:
+   *
+   * - kernels >= 3.x identify as 2.6.40+x
+   * - kernels >= 4.x identify as 2.6.60+x
+   *
+   * UNAME26 is a poorly conceived hack that doesn't let us distinguish
+   * between 4.x kernels and 5.x/6.x kernels so we conservatively assume
+   * that 2.6.60+x means 4.x.
+   *
+   * Fun fact of the day: it's technically possible to observe the actual
+   * kernel version for a brief moment because uname() first copies out the
+   * real release string before overwriting it with the backcompat string.
+   */
+  if (major == 2 && minor == 6) {
+    if (patch >= 60) {
+      major = 4;
+      minor = patch - 60;
+      patch = 0;
+    } else if (patch >= 40) {
+      major = 3;
+      minor = patch - 40;
+      patch = 0;
+    }
+  }
+
+calculate_version:
   version = major * 65536 + minor * 256 + patch;
   std::atomic_store_explicit(&cached_version, version, std::memory_order_relaxed);
 
@@ -423,6 +476,9 @@
 static int uv__use_io_uring(void) {
 #if defined(__ANDROID_API__)
   return 0;  /* Possibly available but blocked by seccomp. */
+#elif defined(__arm__) && __SIZEOF_POINTER__ == 4
+  /* See https://github.com/libuv/libuv/issues/4158. */
+  return 0;  /* All 32 bits kernels appear buggy. */
 #else
   /* Ternary: unknown=0, yes=1, no=-1 */
   static std::atomic<int> use_io_uring;
@@ -432,8 +488,14 @@
   use = std::atomic_load_explicit(&use_io_uring, std::memory_order_relaxed);
 
   if (use == 0) {
+    /* Older kernels have a bug where the sqpoll thread uses 100% CPU. */
+    use = uv__kernel_version() >= /* 5.10.186 */ 0x050ABA ? 1 : -1;
+
+    /* But users can still enable it if they so desire. */
     val = getenv("UV_USE_IO_URING");
-    use = val == NULL || atoi(val) ? 1 : -1;
+    if (val != NULL)
+      use = atoi(val) ? 1 : -1;
+
     std::atomic_store_explicit(&use_io_uring, use, std::memory_order_relaxed);
   }
 
@@ -757,7 +819,9 @@
 int uv__iou_fs_close(uv_loop_t* loop, uv_fs_t* req) {
   struct uv__io_uring_sqe* sqe;
   struct uv__iou* iou;
+  int kv;
 
+  kv = uv__kernel_version();
   /* Work around a poorly understood bug in older kernels where closing a file
    * descriptor pointing to /foo/bar results in ETXTBSY errors when trying to
    * execve("/foo/bar") later on. The bug seems to have been fixed somewhere
@@ -765,10 +829,17 @@
    * but good candidates are the several data race fixes. Interestingly, it
    * seems to manifest only when running under Docker so the possibility of
    * a Docker bug can't be completely ruled out either. Yay, computers.
+   * Also, disable on non-longterm versions between 5.16.0 (non-longterm) and
+   * 6.1.0 (longterm). Starting with longterm 6.1.x, the issue seems to be
+   * solved.
    */
-  if (uv__kernel_version() < /* 5.15.90 */ 0x050F5A)
+  if (kv < /* 5.15.90 */ 0x050F5A)
     return 0;
 
+  if (kv >= /* 5.16.0 */ 0x050A00 && kv < /* 6.1.0 */ 0x060100)
+    return 0;
+
+
   iou = &uv__get_internal_fields(loop)->iou;
 
   sqe = uv__iou_get_sqe(iou, loop, req);
@@ -1365,41 +1436,20 @@
      */
     SAVE_ERRNO(uv__update_time(loop));
 
-    if (nfds == 0) {
+    if (nfds == -1)
+      assert(errno == EINTR);
+    else if (nfds == 0)
+      /* Unlimited timeout should only return with events or signal. */
       assert(timeout != -1);
 
+    if (nfds == 0 || nfds == -1) {
       if (reset_timeout != 0) {
         timeout = user_timeout;
         reset_timeout = 0;
+      } else if (nfds == 0) {
+        return;
       }
 
-      if (timeout == -1)
-        continue;
-
-      if (timeout == 0)
-        break;
-
-      /* We may have been inside the system call for longer than |timeout|
-       * milliseconds so we need to update the timestamp to avoid drift.
-       */
-      goto update_timeout;
-    }
-
-    if (nfds == -1) {
-      if (errno != EINTR)
-        abort();
-
-      if (reset_timeout != 0) {
-        timeout = user_timeout;
-        reset_timeout = 0;
-      }
-
-      if (timeout == -1)
-        continue;
-
-      if (timeout == 0)
-        break;
-
       /* Interrupted by a signal. Update timeout and poll again. */
       goto update_timeout;
     }
@@ -1510,13 +1560,13 @@
       break;
     }
 
+update_timeout:
     if (timeout == 0)
       break;
 
     if (timeout == -1)
       continue;
 
-update_timeout:
     assert(timeout > 0);
 
     real_timeout -= (loop->time - base);
@@ -1719,11 +1769,8 @@
     return UV__ERR(errno);
   }
 
-  /* Skip first line. */
-  if (!fgets(buf, sizeof(buf), fp)) {
-    uv__free(cpus);
-    return UV__ERR(errno);
-  }
+  if (NULL == fgets(buf, sizeof(buf), fp))
+    abort();
 
   for (;;) {
     memset(&t, 0, sizeof(t));
@@ -1734,10 +1781,8 @@
     if (n != 7)
       break;
 
-    /* Skip rest of line. */
-    if (!fgets(buf, sizeof(buf), fp)) {
-      break;
-    }
+    if (NULL == fgets(buf, sizeof(buf), fp))
+      abort();
 
     if (cpu >= ARRAY_SIZE(*cpus))
       continue;
@@ -1817,9 +1862,8 @@
     if (fp == NULL)
       continue;
 
-    if (0 > fscanf(fp, "%llu", &(*cpus)[cpu].freq)) {
-      (*cpus)[cpu].freq = 0llu;
-    }
+    if (1 != fscanf(fp, "%llu", &(*cpus)[cpu].freq))
+      abort();
     fclose(fp);
     fp = NULL;
   }
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp
index 63aba5a..bc4206e 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp
@@ -279,6 +279,8 @@
 
 
 int uv__signal_loop_fork(uv_loop_t* loop) {
+  struct uv__queue* q;
+
   if (loop->signal_pipefd[0] == -1)
     return 0;
   uv__io_stop(loop, &loop->signal_io_watcher, POLLIN);
@@ -286,6 +288,19 @@
   uv__close(loop->signal_pipefd[1]);
   loop->signal_pipefd[0] = -1;
   loop->signal_pipefd[1] = -1;
+
+  uv__queue_foreach(q, &loop->handle_queue) {
+    uv_handle_t* handle = uv__queue_data(q, uv_handle_t, handle_queue);
+    uv_signal_t* sh;
+
+    if (handle->type != UV_SIGNAL)
+      continue;
+
+    sh = (uv_signal_t*) handle;
+    sh->caught_signals = 0;
+    sh->dispatched_signals = 0;
+  }
+
   return uv__signal_loop_once_init(loop);
 }
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp
index d6c848f..4c4213a 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp
@@ -27,6 +27,17 @@
 #include <assert.h>
 #include <errno.h>
 
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#if defined(__PASE__)
+#include <as400_protos.h>
+#define ifaddrs ifaddrs_pase
+#define getifaddrs Qp2getifaddrs
+#define freeifaddrs Qp2freeifaddrs
+#else
+#include <ifaddrs.h>
+#endif
 
 static int maybe_bind_socket(int fd) {
   union uv__sockaddr s;
@@ -198,11 +209,50 @@
 }
 
 
+static int uv__is_ipv6_link_local(const struct sockaddr* addr) {
+  const struct sockaddr_in6* a6;
+  uint8_t b[2];
+
+  if (addr->sa_family != AF_INET6)
+    return 0;
+
+  a6 = (const struct sockaddr_in6*) addr;
+  memcpy(b, &a6->sin6_addr, sizeof(b));
+
+  return b[0] == 0xFE && b[1] == 0x80;
+}
+
+
+static int uv__ipv6_link_local_scope_id(void) {
+  struct sockaddr_in6* a6;
+  struct ifaddrs* ifa;
+  struct ifaddrs* p;
+  int rv;
+
+  if (getifaddrs(&ifa))
+    return 0;
+
+  for (p = ifa; p != NULL; p = p->ifa_next)
+    if (uv__is_ipv6_link_local(p->ifa_addr))
+      break;
+
+  rv = 0;
+  if (p != NULL) {
+    a6 = (struct sockaddr_in6*) p->ifa_addr;
+    rv = a6->sin6_scope_id;
+  }
+
+  freeifaddrs(ifa);
+  return rv;
+}
+
+
 int uv__tcp_connect(uv_connect_t* req,
                     uv_tcp_t* handle,
                     const struct sockaddr* addr,
                     unsigned int addrlen,
                     uv_connect_cb cb) {
+  struct sockaddr_in6 tmp6;
   int err;
   int r;
 
@@ -220,6 +270,14 @@
   if (err)
     return err;
 
+  if (uv__is_ipv6_link_local(addr)) {
+    memcpy(&tmp6, addr, sizeof(tmp6));
+    if (tmp6.sin6_scope_id == 0) {
+      tmp6.sin6_scope_id = uv__ipv6_link_local_scope_id();
+      addr = (const struct sockaddr*) &tmp6;
+    }
+  }
+
   do {
     errno = 0;
     r = connect(uv__stream_fd(handle), addr, addrlen);
@@ -374,28 +432,39 @@
 
 
 int uv__tcp_keepalive(int fd, int on, unsigned int delay) {
+  int intvl;
+  int cnt;
+
+  (void) &intvl;
+  (void) &cnt;
+    
   if (setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &on, sizeof(on)))
     return UV__ERR(errno);
 
+  if (!on)
+    return 0;
+
 #ifdef TCP_KEEPIDLE
-  if (on) {
-    int intvl = 1;  /*  1 second; same as default on Win32 */
-    int cnt = 10;  /* 10 retries; same as hardcoded on Win32 */
-    if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE, &delay, sizeof(delay)))
-      return UV__ERR(errno);
-    if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &intvl, sizeof(intvl)))
-      return UV__ERR(errno);
-    if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT, &cnt, sizeof(cnt)))
-      return UV__ERR(errno);
-  }
+  if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE, &delay, sizeof(delay)))
+    return UV__ERR(errno);
+/* Solaris/SmartOS, if you don't support keep-alive,
+ * then don't advertise it in your system headers...
+ */
+/* FIXME(bnoordhuis) That's possibly because sizeof(delay) should be 1. */
+#elif defined(TCP_KEEPALIVE) && !defined(__sun)
+  if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPALIVE, &delay, sizeof(delay)))
+    return UV__ERR(errno);
 #endif
 
-  /* Solaris/SmartOS, if you don't support keep-alive,
-   * then don't advertise it in your system headers...
-   */
-  /* FIXME(bnoordhuis) That's possibly because sizeof(delay) should be 1. */
-#if defined(TCP_KEEPALIVE) && !defined(__sun)
-  if (on && setsockopt(fd, IPPROTO_TCP, TCP_KEEPALIVE, &delay, sizeof(delay)))
+#ifdef TCP_KEEPINTVL
+  intvl = 1;  /*  1 second; same as default on Win32 */
+  if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &intvl, sizeof(intvl)))
+    return UV__ERR(errno);
+#endif
+
+#ifdef TCP_KEEPCNT
+  cnt = 10;  /* 10 retries; same as hardcoded on Win32 */
+  if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT, &cnt, sizeof(cnt)))
     return UV__ERR(errno);
 #endif
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp
index f860094..688c042 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp
@@ -782,11 +782,33 @@
     abort();
 }
 
+#if defined(__APPLE__) && defined(__MACH__)
+
+void uv_cond_wait(uv_cond_t* cond, uv_mutex_t* mutex) {
+  int r;
+
+  errno = 0;
+  r = pthread_cond_wait(cond, mutex);
+
+  /* Workaround for a bug in OS X at least up to 13.6
+   * See https://github.com/libuv/libuv/issues/4165
+   */
+  if (r == EINVAL)
+    if (errno == EBUSY)
+      return;
+
+  if (r)
+    abort();
+}
+
+#else /* !(defined(__APPLE__) && defined(__MACH__)) */
+
 void uv_cond_wait(uv_cond_t* cond, uv_mutex_t* mutex) {
   if (pthread_cond_wait(cond, mutex))
     abort();
 }
 
+#endif
 
 int uv_cond_timedwait(uv_cond_t* cond, uv_mutex_t* mutex, uint64_t timeout) {
   int r;
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp
index 5c6d841..1ce25c2 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp
@@ -559,6 +559,9 @@
   if (loop == NULL)
     loop = uv_default_loop();
 
+  if (stream == NULL)
+    stream = stderr;
+
   uv__queue_foreach(q, &loop->handle_queue) {
     h = uv__queue_data(q, uv_handle_t, handle_queue);
 
@@ -799,7 +802,6 @@
   }
 }
 
-
 #ifdef __clang__
 # pragma clang diagnostic push
 # pragma clang diagnostic ignored "-Wvarargs"
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp
index 676be4d..d88400f 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp
@@ -27,18 +27,17 @@
 
 int uv_dlopen(const char* filename, uv_lib_t* lib) {
   WCHAR filename_w[32768];
+  ssize_t r;
 
   lib->handle = NULL;
   lib->errmsg = NULL;
 
-  if (!MultiByteToWideChar(CP_UTF8,
-                           0,
-                           filename,
-                           -1,
-                           filename_w,
-                           ARRAY_SIZE(filename_w))) {
-    return uv__dlerror(lib, filename, GetLastError());
-  }
+  r = uv_wtf8_length_as_utf16(filename);
+  if (r < 0)
+    return uv__dlerror(lib, filename, ERROR_NO_UNICODE_TRANSLATION);
+  if ((size_t) r > ARRAY_SIZE(filename_w))
+    return uv__dlerror(lib, filename, ERROR_INSUFFICIENT_BUFFER);
+  uv_wtf8_to_utf16(filename, (uint16_t*)filename_w, r);
 
   lib->handle = LoadLibraryExW(filename_w, NULL, LOAD_WITH_ALTERED_SEARCH_PATH);
   if (lib->handle == NULL) {
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp
index 3244a4e..381220b 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp
@@ -159,7 +159,8 @@
                       uv_fs_event_cb cb,
                       const char* path,
                       unsigned int flags) {
-  int name_size, is_path_dir, size;
+  int is_path_dir;
+  size_t size;
   DWORD attr, last_error;
   WCHAR* dir = NULL, *dir_to_watch, *pathw = NULL;
   DWORD short_path_buffer_len;
@@ -178,23 +179,9 @@
 
   uv__handle_start(handle);
 
-  /* Convert name to UTF16. */
-
-  name_size = MultiByteToWideChar(CP_UTF8, 0, path, -1, NULL, 0) *
-              sizeof(WCHAR);
-  pathw = (WCHAR*)uv__malloc(name_size);
-  if (!pathw) {
-    uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
-  }
-
-  if (!MultiByteToWideChar(CP_UTF8,
-                           0,
-                           path,
-                           -1,
-                           pathw,
-                           name_size / sizeof(WCHAR))) {
-    return uv_translate_sys_error(GetLastError());
-  }
+  last_error = uv__convert_utf8_to_utf16(path, &pathw);
+  if (last_error)
+    goto error_uv;
 
   /* Determine whether path is a file or a directory. */
   attr = GetFileAttributesW(pathw);
@@ -335,6 +322,9 @@
   return 0;
 
 error:
+  last_error = uv_translate_sys_error(last_error);
+
+error_uv:
   if (handle->path) {
     uv__free(handle->path);
     handle->path = NULL;
@@ -367,7 +357,7 @@
 
   uv__free(short_path);
 
-  return uv_translate_sys_error(last_error);
+  return last_error;
 }
 
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp
index f415ddc..26ba539 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp
@@ -33,13 +33,16 @@
 #include <stdio.h>
 
 #include "uv.h"
+
+/* <winioctl.h> requires <windows.h>, included via "uv.h" above, but needs to
+   be included before our "winapi.h", included via "internal.h" below. */
+#include <winioctl.h>
+
 #include "internal.h"
 #include "req-inl.h"
 #include "handle-inl.h"
 #include "fs-fd-hash-inl.h"
 
-#include <winioctl.h>
-
 #pragma comment(lib, "Advapi32.lib")
 
 #define UV_FS_FREE_PATHS         0x0002
@@ -147,279 +150,6 @@
 }
 
 
-static int32_t fs__decode_wtf8_char(const char** input) {
-  uint32_t code_point;
-  uint8_t b1;
-  uint8_t b2;
-  uint8_t b3;
-  uint8_t b4;
-
-  b1 = **input;
-  if (b1 <= 0x7F)
-    return b1; /* ASCII code point */
-  if (b1 < 0xC2)
-    return -1; /* invalid: continuation byte */
-  code_point = b1;
-
-  b2 = *++*input;
-  if ((b2 & 0xC0) != 0x80)
-    return -1; /* invalid: not a continuation byte */
-  code_point = (code_point << 6) | (b2 & 0x3F);
-  if (b1 <= 0xDF)
-    return 0x7FF & code_point; /* two-byte character */
-
-  b3 = *++*input;
-  if ((b3 & 0xC0) != 0x80)
-    return -1; /* invalid: not a continuation byte */
-  code_point = (code_point << 6) | (b3 & 0x3F);
-  if (b1 <= 0xEF)
-    return 0xFFFF & code_point; /* three-byte character */
-
-  b4 = *++*input;
-  if ((b4 & 0xC0) != 0x80)
-    return -1; /* invalid: not a continuation byte */
-  code_point = (code_point << 6) | (b4 & 0x3F);
-  if (b1 <= 0xF4)
-    if (code_point <= 0x10FFFF)
-      return code_point; /* four-byte character */
-
-  /* code point too large */
-  return -1;
-}
-
-
-static ssize_t fs__get_length_wtf8(const char* source_ptr) {
-  size_t w_target_len = 0;
-  int32_t code_point;
-
-  do {
-    code_point = fs__decode_wtf8_char(&source_ptr);
-    if (code_point < 0)
-      return -1;
-    if (code_point > 0xFFFF)
-      w_target_len++;
-    w_target_len++;
-  } while (*source_ptr++);
-  return w_target_len;
-}
-
-
-static void fs__wtf8_to_wide(const char* source_ptr, WCHAR* w_target) {
-  int32_t code_point;
-
-  do {
-    code_point = fs__decode_wtf8_char(&source_ptr);
-    /* fs__get_length_wtf8 should have been called and checked first. */
-    assert(code_point >= 0);
-    if (code_point > 0x10000) {
-      assert(code_point < 0x10FFFF);
-      *w_target++ = (((code_point - 0x10000) >> 10) + 0xD800);
-      *w_target++ = ((code_point - 0x10000) & 0x3FF) + 0xDC00;
-    } else {
-      *w_target++ = code_point;
-    }
-  } while (*source_ptr++);
-}
-
-
-INLINE static int fs__capture_path(uv_fs_t* req, const char* path,
-    const char* new_path, const int copy_path) {
-  WCHAR* buf;
-  WCHAR* pos;
-  size_t buf_sz = 0;
-  size_t path_len = 0;
-  ssize_t pathw_len = 0;
-  ssize_t new_pathw_len = 0;
-
-  /* new_path can only be set if path is also set. */
-  assert(new_path == NULL || path != NULL);
-
-  if (path != NULL) {
-    pathw_len = fs__get_length_wtf8(path);
-    if (pathw_len < 0)
-      return ERROR_INVALID_NAME;
-    buf_sz += pathw_len * sizeof(WCHAR);
-  }
-
-  if (path != NULL && copy_path) {
-    path_len = 1 + strlen(path);
-    buf_sz += path_len;
-  }
-
-  if (new_path != NULL) {
-    new_pathw_len = fs__get_length_wtf8(new_path);
-    if (new_pathw_len < 0)
-      return ERROR_INVALID_NAME;
-    buf_sz += new_pathw_len * sizeof(WCHAR);
-  }
-
-
-  if (buf_sz == 0) {
-    req->file.pathw = NULL;
-    req->fs.info.new_pathw = NULL;
-    req->path = NULL;
-    return 0;
-  }
-
-  buf = (WCHAR *)uv__malloc(buf_sz);
-  if (buf == NULL) {
-    return ERROR_OUTOFMEMORY;
-  }
-
-  pos = buf;
-
-  if (path != NULL) {
-    fs__wtf8_to_wide(path, pos);
-    req->file.pathw = pos;
-    pos += pathw_len;
-  } else {
-    req->file.pathw = NULL;
-  }
-
-  if (new_path != NULL) {
-    fs__wtf8_to_wide(new_path, pos);
-    req->fs.info.new_pathw = pos;
-    pos += new_pathw_len;
-  } else {
-    req->fs.info.new_pathw = NULL;
-  }
-
-  req->path = path;
-  if (path != NULL && copy_path) {
-    memcpy(pos, path, path_len);
-    assert(path_len == buf_sz - (pos - buf) * sizeof(WCHAR));
-    req->path = (char*) pos;
-  }
-
-  req->flags |= UV_FS_FREE_PATHS;
-
-  return 0;
-}
-
-
-
-INLINE static void uv__fs_req_init(uv_loop_t* loop, uv_fs_t* req,
-    uv_fs_type fs_type, const uv_fs_cb cb) {
-  uv__once_init();
-  UV_REQ_INIT(req, UV_FS);
-  req->loop = loop;
-  req->flags = 0;
-  req->fs_type = fs_type;
-  req->sys_errno_ = 0;
-  req->result = 0;
-  req->ptr = NULL;
-  req->path = NULL;
-  req->cb = cb;
-  memset(&req->fs, 0, sizeof(req->fs));
-}
-
-
-static int32_t fs__get_surrogate_value(const WCHAR* w_source_ptr,
-                                       size_t w_source_len) {
-  WCHAR u;
-  WCHAR next;
-
-  u = w_source_ptr[0];
-  if (u >= 0xD800 && u <= 0xDBFF && w_source_len > 1) {
-    next = w_source_ptr[1];
-    if (next >= 0xDC00 && next <= 0xDFFF)
-      return 0x10000 + ((u - 0xD800) << 10) + (next - 0xDC00);
-  }
-  return u;
-}
-
-
-static size_t fs__get_length_wide(const WCHAR* w_source_ptr,
-                                  size_t w_source_len) {
-  size_t target_len;
-  int32_t code_point;
-
-  target_len = 0;
-  for (; w_source_len; w_source_len--, w_source_ptr++) {
-    code_point = fs__get_surrogate_value(w_source_ptr, w_source_len);
-    /* Can be invalid UTF-8 but must be valid WTF-8. */
-    assert(code_point >= 0);
-    if (code_point < 0x80)
-      target_len += 1;
-    else if (code_point < 0x800)
-      target_len += 2;
-    else if (code_point < 0x10000)
-      target_len += 3;
-    else {
-      target_len += 4;
-      w_source_ptr++;
-      w_source_len--;
-    }
-  }
-  return target_len;
-}
-
-
-static int fs__wide_to_wtf8(WCHAR* w_source_ptr,
-                            size_t w_source_len,
-                            char** target_ptr,
-                            size_t* target_len_ptr) {
-  size_t target_len;
-  char* target;
-  int32_t code_point;
-
-  /* If *target_ptr is provided, then *target_len_ptr must be its length
-   * (excluding space for null), otherwise we will compute the target_len_ptr
-   * length and may return a new allocation in *target_ptr if target_ptr is
-   * provided. */
-  if (target_ptr == NULL || *target_ptr == NULL) {
-    target_len = fs__get_length_wide(w_source_ptr, w_source_len);
-    if (target_len_ptr != NULL)
-      *target_len_ptr = target_len;
-  } else {
-    target_len = *target_len_ptr;
-  }
-
-  if (target_ptr == NULL)
-    return 0;
-
-  if (*target_ptr == NULL) {
-    target = (char *)uv__malloc(target_len + 1);
-    if (target == NULL) {
-      SetLastError(ERROR_OUTOFMEMORY);
-      return -1;
-    }
-    *target_ptr = target;
-  } else {
-    target = *target_ptr;
-  }
-
-  for (; w_source_len; w_source_len--, w_source_ptr++) {
-    code_point = fs__get_surrogate_value(w_source_ptr, w_source_len);
-    /* Can be invalid UTF-8 but must be valid WTF-8. */
-    assert(code_point >= 0);
-
-    if (code_point < 0x80) {
-      *target++ = code_point;
-    } else if (code_point < 0x800) {
-      *target++ = 0xC0 | (code_point >> 6);
-      *target++ = 0x80 | (code_point & 0x3F);
-    } else if (code_point < 0x10000) {
-      *target++ = 0xE0 | (code_point >> 12);
-      *target++ = 0x80 | ((code_point >> 6) & 0x3F);
-      *target++ = 0x80 | (code_point & 0x3F);
-    } else {
-      *target++ = 0xF0 | (code_point >> 18);
-      *target++ = 0x80 | ((code_point >> 12) & 0x3F);
-      *target++ = 0x80 | ((code_point >> 6) & 0x3F);
-      *target++ = 0x80 | (code_point & 0x3F);
-      w_source_ptr++;
-      w_source_len--;
-    }
-  }
-  assert((size_t) (target - *target_ptr) == target_len);
-
-  *target++ = '\0';
-
-  return 0;
-}
-
-
 INLINE static int fs__readlink_handle(HANDLE handle,
                                       char** target_ptr,
                                       size_t* target_len_ptr) {
@@ -553,7 +283,98 @@
   }
 
   assert(target_ptr == NULL || *target_ptr == NULL);
-  return fs__wide_to_wtf8(w_target, w_target_len, target_ptr, target_len_ptr);
+  return uv_utf16_to_wtf8((const uint16_t*)w_target, w_target_len, target_ptr, target_len_ptr);
+}
+
+
+INLINE static int fs__capture_path(uv_fs_t* req, const char* path,
+    const char* new_path, const int copy_path) {
+  WCHAR* buf;
+  WCHAR* pos;
+  size_t buf_sz = 0;
+  size_t path_len = 0;
+  ssize_t pathw_len = 0;
+  ssize_t new_pathw_len = 0;
+
+  /* new_path can only be set if path is also set. */
+  assert(new_path == NULL || path != NULL);
+
+  if (path != NULL) {
+    pathw_len = uv_wtf8_length_as_utf16(path);
+    if (pathw_len < 0)
+      return ERROR_INVALID_NAME;
+    buf_sz += pathw_len * sizeof(WCHAR);
+  }
+
+  if (path != NULL && copy_path) {
+    path_len = 1 + strlen(path);
+    buf_sz += path_len;
+  }
+
+  if (new_path != NULL) {
+    new_pathw_len = uv_wtf8_length_as_utf16(new_path);
+    if (new_pathw_len < 0)
+      return ERROR_INVALID_NAME;
+    buf_sz += new_pathw_len * sizeof(WCHAR);
+  }
+
+
+  if (buf_sz == 0) {
+    req->file.pathw = NULL;
+    req->fs.info.new_pathw = NULL;
+    req->path = NULL;
+    return 0;
+  }
+
+  buf = (WCHAR*)uv__malloc(buf_sz);
+  if (buf == NULL) {
+    return ERROR_OUTOFMEMORY;
+  }
+
+  pos = buf;
+
+  if (path != NULL) {
+    uv_wtf8_to_utf16(path, (uint16_t*)pos, pathw_len);
+    req->file.pathw = pos;
+    pos += pathw_len;
+  } else {
+    req->file.pathw = NULL;
+  }
+
+  if (new_path != NULL) {
+    uv_wtf8_to_utf16(new_path, (uint16_t*)pos, new_pathw_len);
+    req->fs.info.new_pathw = pos;
+    pos += new_pathw_len;
+  } else {
+    req->fs.info.new_pathw = NULL;
+  }
+
+  req->path = path;
+  if (path != NULL && copy_path) {
+    memcpy(pos, path, path_len);
+    assert(path_len == buf_sz - (pos - buf) * sizeof(WCHAR));
+    req->path = (char*) pos;
+  }
+
+  req->flags |= UV_FS_FREE_PATHS;
+
+  return 0;
+}
+
+
+INLINE static void uv__fs_req_init(uv_loop_t* loop, uv_fs_t* req,
+    uv_fs_type fs_type, const uv_fs_cb cb) {
+  uv__once_init();
+  UV_REQ_INIT(req, UV_FS);
+  req->loop = loop;
+  req->flags = 0;
+  req->fs_type = fs_type;
+  req->sys_errno_ = 0;
+  req->result = 0;
+  req->ptr = NULL;
+  req->path = NULL;
+  req->cb = cb;
+  memset(&req->fs, 0, sizeof(req->fs));
 }
 
 
@@ -1572,7 +1393,7 @@
         continue;
 
       /* Compute the space required to store the filename as WTF-8. */
-      wtf8_len = fs__get_length_wide(&info->FileName[0], wchar_len);
+      wtf8_len = uv_utf16_length_as_wtf8((const uint16_t*)&info->FileName[0], wchar_len);
 
       /* Resize the dirent array if needed. */
       if (dirents_used >= dirents_size) {
@@ -1600,8 +1421,8 @@
 
       /* Convert file name to UTF-8. */
       wtf8 = &dirent->d_name[0];
-      if (fs__wide_to_wtf8(&info->FileName[0], wchar_len, &wtf8, &wtf8_len) == -1)
-        goto win32_error;
+      if (uv_utf16_to_wtf8((const uint16_t*)&info->FileName[0], wchar_len, &wtf8, &wtf8_len) != 0)
+        goto out_of_memory_error;
 
       /* Fill out the type field. */
       if (info->FileAttributes & FILE_ATTRIBUTE_DEVICE)
@@ -2827,7 +2648,7 @@
   }
 
   assert(*realpath_ptr == NULL);
-  r = fs__wide_to_wtf8(w_realpath_ptr, w_realpath_len, realpath_ptr, NULL);
+  r = uv_utf16_to_wtf8((const uint16_t*)w_realpath_ptr, w_realpath_len, realpath_ptr, NULL);
   uv__free(w_realpath_buf);
   return r;
 }
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp
index dfab860..5bc63d8 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp
@@ -104,13 +104,14 @@
  */
 static void uv__getaddrinfo_done(struct uv__work* w, int status) {
   uv_getaddrinfo_t* req;
-  int addrinfo_len = 0;
-  int name_len = 0;
+  size_t addrinfo_len = 0;
+  ssize_t name_len = 0;
   size_t addrinfo_struct_len = ALIGNED_SIZE(sizeof(struct addrinfo));
   struct addrinfoW* addrinfow_ptr;
   struct addrinfo* addrinfo_ptr;
   char* alloc_ptr = NULL;
   char* cur_ptr = NULL;
+  int r;
 
   req = container_of(w, uv_getaddrinfo_t, work_req);
 
@@ -131,19 +132,12 @@
       addrinfo_len += addrinfo_struct_len +
           ALIGNED_SIZE(addrinfow_ptr->ai_addrlen);
       if (addrinfow_ptr->ai_canonname != NULL) {
-        name_len = WideCharToMultiByte(CP_UTF8,
-                                       0,
-                                       addrinfow_ptr->ai_canonname,
-                                       -1,
-                                       NULL,
-                                       0,
-                                       NULL,
-                                       NULL);
-        if (name_len == 0) {
-          req->retcode = uv_translate_sys_error(GetLastError());
+        name_len = uv_utf16_length_as_wtf8((const uint16_t*)addrinfow_ptr->ai_canonname, -1);
+        if (name_len < 0) {
+          req->retcode = name_len;
           goto complete;
         }
-        addrinfo_len += ALIGNED_SIZE(name_len);
+        addrinfo_len += ALIGNED_SIZE(name_len + 1);
       }
       addrinfow_ptr = addrinfow_ptr->ai_next;
     }
@@ -182,27 +176,14 @@
 
         /* convert canonical name to UTF-8 */
         if (addrinfow_ptr->ai_canonname != NULL) {
-          name_len = WideCharToMultiByte(CP_UTF8,
-                                         0,
-                                         addrinfow_ptr->ai_canonname,
-                                         -1,
-                                         NULL,
-                                         0,
-                                         NULL,
-                                         NULL);
-          assert(name_len > 0);
-          assert(cur_ptr + name_len <= alloc_ptr + addrinfo_len);
-          name_len = WideCharToMultiByte(CP_UTF8,
-                                         0,
-                                         addrinfow_ptr->ai_canonname,
-                                         -1,
-                                         cur_ptr,
-                                         name_len,
-                                         NULL,
-                                         NULL);
-          assert(name_len > 0);
+          name_len = alloc_ptr + addrinfo_len - cur_ptr;
+          r = uv__copy_utf16_to_utf8(addrinfow_ptr->ai_canonname,
+                                     -1,
+                                     cur_ptr,
+                                     (size_t*)&name_len);
+          assert(r == 0);
           addrinfo_ptr->ai_canonname = cur_ptr;
-          cur_ptr += ALIGNED_SIZE(name_len);
+          cur_ptr += ALIGNED_SIZE(name_len + 1);
         }
         assert(cur_ptr <= alloc_ptr + addrinfo_len);
 
@@ -261,12 +242,11 @@
                    const char* service,
                    const struct addrinfo* hints) {
   char hostname_ascii[256];
-  int nodesize = 0;
-  int servicesize = 0;
-  int hintssize = 0;
+  size_t nodesize = 0;
+  size_t servicesize = 0;
+  size_t hintssize = 0;
   char* alloc_ptr = NULL;
-  int err;
-  long rc;
+  ssize_t rc;
 
   if (req == NULL || (node == NULL && service == NULL)) {
     return UV_EINVAL;
@@ -286,56 +266,36 @@
                           hostname_ascii + sizeof(hostname_ascii));
     if (rc < 0)
       return rc;
-    nodesize = ALIGNED_SIZE(MultiByteToWideChar(CP_UTF8, 0, hostname_ascii,
-                                                -1, NULL, 0) * sizeof(WCHAR));
-    if (nodesize == 0) {
-      err = GetLastError();
-      goto error;
-    }
+    nodesize = strlen(hostname_ascii) + 1;
     node = hostname_ascii;
   }
 
   if (service != NULL) {
-    servicesize = ALIGNED_SIZE(MultiByteToWideChar(CP_UTF8,
-                                                   0,
-                                                   service,
-                                                   -1,
-                                                   NULL,
-                                                   0) *
-                               sizeof(WCHAR));
-    if (servicesize == 0) {
-      err = GetLastError();
-      goto error;
-    }
+    rc = uv_wtf8_length_as_utf16(service);
+    if (rc < 0)
+       return rc;
+    servicesize = rc;
   }
   if (hints != NULL) {
     hintssize = ALIGNED_SIZE(sizeof(struct addrinfoW));
   }
 
   /* allocate memory for inputs, and partition it as needed */
-  alloc_ptr = (char*)uv__malloc(nodesize + servicesize + hintssize);
-  if (!alloc_ptr) {
-    err = WSAENOBUFS;
-    goto error;
-  }
+  alloc_ptr = (char*)uv__malloc(ALIGNED_SIZE(nodesize * sizeof(WCHAR)) +
+                                ALIGNED_SIZE(servicesize * sizeof(WCHAR)) +
+                                hintssize);
+  if (!alloc_ptr)
+    return UV_ENOMEM;
 
   /* save alloc_ptr now so we can free if error */
-  req->alloc = (void*)alloc_ptr;
+  req->alloc = (void*) alloc_ptr;
 
   /* Convert node string to UTF16 into allocated memory and save pointer in the
-   * request. */
+   * request. The node here has been converted to ascii. */
   if (node != NULL) {
-    req->node = (WCHAR*)alloc_ptr;
-    if (MultiByteToWideChar(CP_UTF8,
-                            0,
-                            node,
-                            -1,
-                            (WCHAR*) alloc_ptr,
-                            nodesize / sizeof(WCHAR)) == 0) {
-      err = GetLastError();
-      goto error;
-    }
-    alloc_ptr += nodesize;
+    req->node = (WCHAR*) alloc_ptr;
+    uv_wtf8_to_utf16(node, (uint16_t*) alloc_ptr, nodesize);
+    alloc_ptr += ALIGNED_SIZE(nodesize * sizeof(WCHAR));
   } else {
     req->node = NULL;
   }
@@ -343,24 +303,16 @@
   /* Convert service string to UTF16 into allocated memory and save pointer in
    * the req. */
   if (service != NULL) {
-    req->service = (WCHAR*)alloc_ptr;
-    if (MultiByteToWideChar(CP_UTF8,
-                            0,
-                            service,
-                            -1,
-                            (WCHAR*) alloc_ptr,
-                            servicesize / sizeof(WCHAR)) == 0) {
-      err = GetLastError();
-      goto error;
-    }
-    alloc_ptr += servicesize;
+    req->service = (WCHAR*) alloc_ptr;
+    uv_wtf8_to_utf16(service, (uint16_t*) alloc_ptr, servicesize);
+    alloc_ptr += ALIGNED_SIZE(servicesize * sizeof(WCHAR));
   } else {
     req->service = NULL;
   }
 
   /* copy hints to allocated memory and save pointer in req */
   if (hints != NULL) {
-    req->addrinfow = (struct addrinfoW*)alloc_ptr;
+    req->addrinfow = (struct addrinfoW*) alloc_ptr;
     req->addrinfow->ai_family = hints->ai_family;
     req->addrinfow->ai_socktype = hints->ai_socktype;
     req->addrinfow->ai_protocol = hints->ai_protocol;
@@ -387,19 +339,11 @@
     uv__getaddrinfo_done(&req->work_req, 0);
     return req->retcode;
   }
-
-error:
-  if (req != NULL) {
-    uv__free(req->alloc);
-    req->alloc = NULL;
-  }
-  return uv_translate_sys_error(err);
 }
 
 int uv_if_indextoname(unsigned int ifindex, char* buffer, size_t* size) {
   NET_LUID luid;
   wchar_t wname[NDIS_IF_MAX_STRING_SIZE + 1]; /* Add one for the NUL. */
-  DWORD bufsize;
   int r;
 
   if (buffer == NULL || size == NULL || *size == 0)
@@ -415,31 +359,7 @@
   if (r != 0)
     return uv_translate_sys_error(r);
 
-  /* Check how much space we need */
-  bufsize = WideCharToMultiByte(CP_UTF8, 0, wname, -1, NULL, 0, NULL, NULL);
-
-  if (bufsize == 0) {
-    return uv_translate_sys_error(GetLastError());
-  } else if (bufsize > *size) {
-    *size = bufsize;
-    return UV_ENOBUFS;
-  }
-
-  /* Convert to UTF-8 */
-  bufsize = WideCharToMultiByte(CP_UTF8,
-                                0,
-                                wname,
-                                -1,
-                                buffer,
-                                *size,
-                                NULL,
-                                NULL);
-
-  if (bufsize == 0)
-    return uv_translate_sys_error(GetLastError());
-
-  *size = bufsize - 1;
-  return 0;
+  return uv__copy_utf16_to_utf8(wname, -1, buffer, size);
 }
 
 int uv_if_indextoiid(unsigned int ifindex, char* buffer, size_t* size) {
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp
index b377338..3286317 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp
@@ -42,6 +42,7 @@
   uv_getnameinfo_t* req;
   WCHAR host[NI_MAXHOST];
   WCHAR service[NI_MAXSERV];
+  size_t size;
   int ret;
 
   req = container_of(w, uv_getnameinfo_t, work_req);
@@ -57,29 +58,17 @@
     return;
   }
 
-  ret = WideCharToMultiByte(CP_UTF8,
-                            0,
-                            host,
-                            -1,
-                            req->host,
-                            sizeof(req->host),
-                            NULL,
-                            NULL);
-  if (ret == 0) {
-    req->retcode = uv_translate_sys_error(GetLastError());
+  size = sizeof(req->host);
+  ret = uv__copy_utf16_to_utf8(host, -1, req->host, &size);
+  if (ret < 0) {
+    req->retcode = ret;
     return;
   }
 
-  ret = WideCharToMultiByte(CP_UTF8,
-                            0,
-                            service,
-                            -1,
-                            req->service,
-                            sizeof(req->service),
-                            NULL,
-                            NULL);
-  if (ret == 0) {
-    req->retcode = uv_translate_sys_error(GetLastError());
+  size = sizeof(req->service);
+  ret = uv__copy_utf16_to_utf8(service, -1, req->service, &size);
+  if (ret < 0) {
+    req->retcode = ret;
   }
 }
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h
index 9672fbc..867dea5 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h
@@ -257,8 +257,9 @@
 
 uint64_t uv__hrtime(unsigned int scale);
 __declspec(noreturn) void uv_fatal_error(const int errorno, const char* syscall);
-int uv__convert_utf16_to_utf8(const WCHAR* utf16, int utf16len, char** utf8);
-int uv__convert_utf8_to_utf16(const char* utf8, int utf8len, WCHAR** utf16);
+int uv__convert_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char** utf8);
+int uv__copy_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char* utf8, size_t *size);
+int uv__convert_utf8_to_utf16(const char* utf8, WCHAR** utf16);
 
 typedef int (WINAPI *uv__peersockfunc)(SOCKET, struct sockaddr*, int*);
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp
index 258d6a6..b2348bd 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp
@@ -51,7 +51,7 @@
 
 /* Pipe prefix */
 static char pipe_prefix[] = "\\\\?\\pipe";
-static const int pipe_prefix_len = sizeof(pipe_prefix) - 1;
+static const size_t pipe_prefix_len = sizeof(pipe_prefix) - 1;
 
 /* IPC incoming xfer queue item. */
 typedef struct {
@@ -705,7 +705,7 @@
                   size_t namelen,
                   unsigned int flags) {
   uv_loop_t* loop = handle->loop;
-  int i, err, nameSize;
+  int i, err;
   uv_pipe_accept_t* req;
 
   if (flags & ~UV_PIPE_NO_TRUNCATE) {
@@ -744,9 +744,8 @@
 
   handle->pipe.serv.accept_reqs = (uv_pipe_accept_t*)
     uv__malloc(sizeof(uv_pipe_accept_t) * handle->pipe.serv.pending_instances);
-  if (!handle->pipe.serv.accept_reqs) {
-    uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
-  }
+  if (!handle->pipe.serv.accept_reqs)
+    return UV_ENOMEM;
 
   for (i = 0; i < handle->pipe.serv.pending_instances; i++) {
     req = &handle->pipe.serv.accept_reqs[i];
@@ -756,22 +755,9 @@
     req->next_pending = NULL;
   }
 
-  /* Convert name to UTF16. */
-  nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR);
-  handle->name = (WCHAR*)uv__malloc(nameSize);
-  if (!handle->name) {
-    uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
-  }
-
-  if (!MultiByteToWideChar(CP_UTF8,
-                           0,
-                           name,
-                           -1,
-                           handle->name,
-                           nameSize / sizeof(WCHAR))) {
-    err = GetLastError();
-    goto error;
-  }
+  err = uv__convert_utf8_to_utf16(name, &handle->name);
+  if (err)
+    return err;
 
   /*
    * Attempt to create the first pipe with FILE_FLAG_FIRST_PIPE_INSTANCE.
@@ -797,10 +783,8 @@
   return 0;
 
 error:
-  if (handle->name) {
-    uv__free(handle->name);
-    handle->name = NULL;
-  }
+  uv__free(handle->name);
+  handle->name = NULL;
 
   return uv_translate_sys_error(err);
 }
@@ -863,7 +847,8 @@
                      unsigned int flags,
                      uv_connect_cb cb) {
   uv_loop_t* loop = handle->loop;
-  int err, nameSize;
+  int err;
+  size_t nameSize;
   HANDLE pipeHandle = INVALID_HANDLE_VALUE;
   DWORD duplex_flags;
 
@@ -906,26 +891,16 @@
   }
   uv__pipe_connection_init(handle);
 
-  /* Convert name to UTF16. */
-  nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR);
-  handle->name = (WCHAR*)uv__malloc(nameSize);
-  if (!handle->name) {
-    uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
-  }
-
-  if (!MultiByteToWideChar(CP_UTF8,
-                           0,
-                           name,
-                           -1,
-                           handle->name,
-                           nameSize / sizeof(WCHAR))) {
-    err = GetLastError();
+  err = uv__convert_utf8_to_utf16(name, &handle->name);
+  if (err) {
+    err = ERROR_NO_UNICODE_TRANSLATION;
     goto error;
   }
 
   pipeHandle = open_named_pipe(handle->name, &duplex_flags);
   if (pipeHandle == INVALID_HANDLE_VALUE) {
     if (GetLastError() == ERROR_PIPE_BUSY) {
+      nameSize = (wcslen(handle->name) + 1) * sizeof(WCHAR);
       req->u.connect.name = (WCHAR *)uv__malloc(nameSize);
       if (!req->u.connect.name) {
         uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
@@ -2441,7 +2416,6 @@
   FILE_NAME_INFORMATION tmp_name_info;
   FILE_NAME_INFORMATION* name_info;
   WCHAR* name_buf;
-  unsigned int addrlen;
   unsigned int name_size;
   unsigned int name_len;
   int err;
@@ -2452,46 +2426,7 @@
   if (handle->name != NULL) {
     /* The user might try to query the name before we are connected,
      * and this is just easier to return the cached value if we have it. */
-    name_buf = handle->name;
-    name_len = wcslen(name_buf);
-
-    /* check how much space we need */
-    addrlen = WideCharToMultiByte(CP_UTF8,
-                                  0,
-                                  name_buf,
-                                  name_len,
-                                  NULL,
-                                  0,
-                                  NULL,
-                                  NULL);
-    if (!addrlen) {
-      *size = 0;
-      err = uv_translate_sys_error(GetLastError());
-      return err;
-    } else if (addrlen >= *size) {
-      *size = addrlen + 1;
-      err = UV_ENOBUFS;
-      goto error;
-    }
-
-    addrlen = WideCharToMultiByte(CP_UTF8,
-                                  0,
-                                  name_buf,
-                                  name_len,
-                                  buffer,
-                                  addrlen,
-                                  NULL,
-                                  NULL);
-    if (!addrlen) {
-      *size = 0;
-      err = uv_translate_sys_error(GetLastError());
-      return err;
-    }
-
-    *size = addrlen;
-    buffer[addrlen] = '\0';
-
-    return 0;
+    return uv__copy_utf16_to_utf8(handle->name, -1, buffer, size);
   }
 
   if (handle->handle == INVALID_HANDLE_VALUE) {
@@ -2519,8 +2454,7 @@
     name_info = (FILE_NAME_INFORMATION*)uv__malloc(name_size);
     if (!name_info) {
       *size = 0;
-      err = UV_ENOMEM;
-      goto cleanup;
+      return UV_ENOMEM;
     }
 
     nt_status = pNtQueryInformationFile(handle->handle,
@@ -2553,51 +2487,19 @@
 
   name_len /= sizeof(WCHAR);
 
-  /* check how much space we need */
-  addrlen = WideCharToMultiByte(CP_UTF8,
-                                0,
-                                name_buf,
-                                name_len,
-                                NULL,
-                                0,
-                                NULL,
-                                NULL);
-  if (!addrlen) {
+  /* "\\\\.\\pipe" + name */
+  if (*size < pipe_prefix_len) {
     *size = 0;
-    err = uv_translate_sys_error(GetLastError());
-    goto error;
-  } else if (pipe_prefix_len + addrlen >= *size) {
-    /* "\\\\.\\pipe" + name */
-    *size = pipe_prefix_len + addrlen + 1;
-    err = UV_ENOBUFS;
-    goto error;
   }
-
-  memcpy(buffer, pipe_prefix, pipe_prefix_len);
-  addrlen = WideCharToMultiByte(CP_UTF8,
-                                0,
-                                name_buf,
-                                name_len,
-                                buffer+pipe_prefix_len,
-                                *size-pipe_prefix_len,
-                                NULL,
-                                NULL);
-  if (!addrlen) {
-    *size = 0;
-    err = uv_translate_sys_error(GetLastError());
-    goto error;
+  else {
+    memcpy(buffer, pipe_prefix, pipe_prefix_len);
+    *size -= pipe_prefix_len;
   }
-
-  addrlen += pipe_prefix_len;
-  *size = addrlen;
-  buffer[addrlen] = '\0';
-
-  err = 0;
+  err = uv__copy_utf16_to_utf8(name_buf, name_len, buffer+pipe_prefix_len, size);
+  *size += pipe_prefix_len;
 
 error:
   uv__free(name_info);
-
-cleanup:
   return err;
 }
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp
index 18816d3..eeb0f6a 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp
@@ -35,6 +35,7 @@
 #include "handle-inl.h"
 #include "req-inl.h"
 
+
 #define SIGKILL         9
 
 
@@ -103,38 +104,26 @@
                                &info,
                                sizeof info))
     uv_fatal_error(GetLastError(), "SetInformationJobObject");
+
+
+  if (!AssignProcessToJobObject(uv_global_job_handle_, GetCurrentProcess())) {
+    /* Make sure this handle is functional. The Windows kernel has a bug that
+     * if the first use of AssignProcessToJobObject is for a Windows Store
+     * program, subsequent attempts to use the handle with fail with
+     * INVALID_PARAMETER (87). This is possibly because all uses of the handle
+     * must be for the same Terminal Services session. We can ensure it is tied
+     * to our current session now by adding ourself to it. We could remove
+     * ourself afterwards, but there doesn't seem to be a reason to.
+     */
+    DWORD err = GetLastError();
+    if (err != ERROR_ACCESS_DENIED)
+      uv_fatal_error(err, "AssignProcessToJobObject");
+  }
 }
 
 
 static int uv__utf8_to_utf16_alloc(const char* s, WCHAR** ws_ptr) {
-  int ws_len, r;
-  WCHAR* ws;
-
-  ws_len = MultiByteToWideChar(CP_UTF8,
-                               0,
-                               s,
-                               -1,
-                               NULL,
-                               0);
-  if (ws_len <= 0) {
-    return GetLastError();
-  }
-
-  ws = (WCHAR*) uv__malloc(ws_len * sizeof(WCHAR));
-  if (ws == NULL) {
-    return ERROR_OUTOFMEMORY;
-  }
-
-  r = MultiByteToWideChar(CP_UTF8,
-                          0,
-                          s,
-                          -1,
-                          ws,
-                          ws_len);
-  assert(r == ws_len);
-
-  *ws_ptr = ws;
-  return 0;
+  return uv__convert_utf8_to_utf16(s, ws_ptr);
 }
 
 
@@ -394,7 +383,7 @@
                                   name_has_ext);
 
     while (result == NULL) {
-      if (*dir_end == L'\0') {
+      if (dir_end == NULL || *dir_end == L'\0') {
         break;
       }
 
@@ -537,21 +526,15 @@
 
   /* Count the required size. */
   for (arg = args; *arg; arg++) {
-    DWORD arg_len;
+    ssize_t arg_len;
 
-    arg_len = MultiByteToWideChar(CP_UTF8,
-                                  0,
-                                  *arg,
-                                  -1,
-                                  NULL,
-                                  0);
-    if (arg_len == 0) {
-      return GetLastError();
-    }
+    arg_len = uv_wtf8_length_as_utf16(*arg);
+    if (arg_len < 0)
+      return arg_len;
 
     dst_len += arg_len;
 
-    if (arg_len > temp_buffer_len)
+    if ((size_t) arg_len > temp_buffer_len)
       temp_buffer_len = arg_len;
 
     arg_count++;
@@ -562,34 +545,28 @@
   dst_len = dst_len * 2 + arg_count * 2;
 
   /* Allocate buffer for the final command line. */
-  dst = (WCHAR*) uv__malloc(dst_len * sizeof(WCHAR));
+  dst = (WCHAR*)uv__malloc(dst_len * sizeof(WCHAR));
   if (dst == NULL) {
-    err = ERROR_OUTOFMEMORY;
+    err = UV_ENOMEM;
     goto error;
   }
 
   /* Allocate temporary working buffer. */
-  temp_buffer = (WCHAR*) uv__malloc(temp_buffer_len * sizeof(WCHAR));
+  temp_buffer = (WCHAR*)uv__malloc(temp_buffer_len * sizeof(WCHAR));
   if (temp_buffer == NULL) {
-    err = ERROR_OUTOFMEMORY;
+    err = UV_ENOMEM;
     goto error;
   }
 
   pos = dst;
   for (arg = args; *arg; arg++) {
-    DWORD arg_len;
+    ssize_t arg_len;
 
     /* Convert argument to wide char. */
-    arg_len = MultiByteToWideChar(CP_UTF8,
-                                  0,
-                                  *arg,
-                                  -1,
-                                  temp_buffer,
-                                  (int) (dst + dst_len - pos));
-    if (arg_len == 0) {
-      err = GetLastError();
-      goto error;
-    }
+    arg_len = uv_wtf8_length_as_utf16(*arg);
+    assert(arg_len > 0);
+    assert(temp_buffer_len >= (size_t) arg_len);
+    uv_wtf8_to_utf16(*arg, (uint16_t*)temp_buffer, arg_len);
 
     if (verbatim_arguments) {
       /* Copy verbatim. */
@@ -601,6 +578,7 @@
     }
 
     *pos++ = *(arg + 1) ? L' ' : L'\0';
+    assert(pos <= dst + dst_len);
   }
 
   uv__free(temp_buffer);
@@ -686,28 +664,22 @@
   WCHAR* ptr;
   char** env;
   size_t env_len = 0;
-  int len;
+  size_t len;
   size_t i;
-  DWORD var_size;
+  size_t var_size;
   size_t env_block_count = 1; /* 1 for null-terminator */
   WCHAR* dst_copy;
   WCHAR** ptr_copy;
   WCHAR** env_copy;
-  DWORD required_vars_value_len[ARRAY_SIZE(required_vars)];
+  size_t required_vars_value_len[ARRAY_SIZE(required_vars)];
 
   /* first pass: determine size in UTF-16 */
   for (env = env_block; *env; env++) {
-    int len;
+    ssize_t len;
     if (strchr(*env, '=')) {
-      len = MultiByteToWideChar(CP_UTF8,
-                                0,
-                                *env,
-                                -1,
-                                NULL,
-                                0);
-      if (len <= 0) {
-        return GetLastError();
-      }
+      len = uv_wtf8_length_as_utf16(*env);
+      if (len < 0)
+        return len;
       env_len += len;
       env_block_count++;
     }
@@ -716,25 +688,19 @@
   /* second pass: copy to UTF-16 environment block */
   dst_copy = (WCHAR*)uv__malloc(env_len * sizeof(WCHAR));
   if (dst_copy == NULL && env_len > 0) {
-    return ERROR_OUTOFMEMORY;
+    return UV_ENOMEM;
   }
   env_copy = (WCHAR**)alloca(env_block_count * sizeof(WCHAR*));
 
   ptr = dst_copy;
   ptr_copy = env_copy;
   for (env = env_block; *env; env++) {
+    ssize_t len;
     if (strchr(*env, '=')) {
-      len = MultiByteToWideChar(CP_UTF8,
-                                0,
-                                *env,
-                                -1,
-                                ptr,
-                                (int) (env_len - (ptr - dst_copy)));
-      if (len <= 0) {
-        DWORD err = GetLastError();
-        uv__free(dst_copy);
-        return err;
-      }
+      len = uv_wtf8_length_as_utf16(*env);
+      assert(len > 0);
+      assert((size_t) len <= env_len - (ptr - dst_copy));
+      uv_wtf8_to_utf16(*env, (uint16_t*)ptr, len);
       *ptr_copy++ = ptr;
       ptr += len;
     }
@@ -752,7 +718,7 @@
       cmp = -1;
     } else {
       cmp = env_strncmp(required_vars[i].wide_eq,
-                       required_vars[i].len,
+                        required_vars[i].len,
                         *ptr_copy);
     }
     if (cmp < 0) {
@@ -775,7 +741,7 @@
   dst = (WCHAR*)uv__malloc((1+env_len) * sizeof(WCHAR));
   if (!dst) {
     uv__free(dst_copy);
-    return ERROR_OUTOFMEMORY;
+    return UV_ENOMEM;
   }
 
   for (ptr = dst, ptr_copy = env_copy, i = 0;
@@ -973,26 +939,26 @@
 
   err = uv__utf8_to_utf16_alloc(options->file, &application);
   if (err)
-    goto done;
+    goto done_uv;
 
   err = make_program_args(
       options->args,
       options->flags & UV_PROCESS_WINDOWS_VERBATIM_ARGUMENTS,
       &arguments);
   if (err)
-    goto done;
+    goto done_uv;
 
   if (options->env) {
      err = make_program_env(options->env, &env);
      if (err)
-       goto done;
+       goto done_uv;
   }
 
   if (options->cwd) {
     /* Explicit cwd */
     err = uv__utf8_to_utf16_alloc(options->cwd, &cwd);
     if (err)
-      goto done;
+      goto done_uv;
 
   } else {
     /* Inherit cwd */
@@ -1023,22 +989,19 @@
     DWORD path_len, r;
 
     path_len = GetEnvironmentVariableW(L"PATH", NULL, 0);
-    if (path_len == 0) {
-      err = GetLastError();
-      goto done;
-    }
+    if (path_len != 0) {
+      alloc_path = (WCHAR*) uv__malloc(path_len * sizeof(WCHAR));
+      if (alloc_path == NULL) {
+        err = ERROR_OUTOFMEMORY;
+        goto done;
+      }
+      path = alloc_path;
 
-    alloc_path = (WCHAR*) uv__malloc(path_len * sizeof(WCHAR));
-    if (alloc_path == NULL) {
-      err = ERROR_OUTOFMEMORY;
-      goto done;
-    }
-    path = alloc_path;
-
-    r = GetEnvironmentVariableW(L"PATH", path, path_len);
-    if (r == 0 || r >= path_len) {
-      err = GetLastError();
-      goto done;
+      r = GetEnvironmentVariableW(L"PATH", path, path_len);
+      if (r == 0 || r >= path_len) {
+        err = GetLastError();
+        goto done;
+      }
     }
   }
 
@@ -1100,6 +1063,7 @@
      * breakaway.
      */
     process_flags |= DETACHED_PROCESS | CREATE_NEW_PROCESS_GROUP;
+    process_flags |= CREATE_SUSPENDED;
   }
 
   if (!CreateProcessW(application_path,
@@ -1117,11 +1081,6 @@
     goto done;
   }
 
-  /* Spawn succeeded. Beyond this point, failure is reported asynchronously. */
-
-  process->process_handle = info.hProcess;
-  process->pid = info.dwProcessId;
-
   /* If the process isn't spawned as detached, assign to the global job object
    * so windows will kill it when the parent process dies. */
   if (!(options->flags & UV_PROCESS_DETACHED)) {
@@ -1144,6 +1103,19 @@
     }
   }
 
+  if (process_flags & CREATE_SUSPENDED) {
+    if (ResumeThread(info.hThread) == ((DWORD)-1)) {
+      err = GetLastError();
+      TerminateProcess(info.hProcess, 1);
+      goto done;
+    }
+  }
+
+  /* Spawn succeeded. Beyond this point, failure is reported asynchronously. */
+
+  process->process_handle = info.hProcess;
+  process->pid = info.dwProcessId;
+
   /* Set IPC pid to all IPC pipes. */
   for (i = 0; i < options->stdio_count; i++) {
     const uv_stdio_container_t* fdopt = &options->stdio[i];
@@ -1171,8 +1143,13 @@
    * made or the handle is closed, whichever happens first. */
   uv__handle_start(process);
 
+  goto done_uv;
+
   /* Cleanup, whether we succeeded or failed. */
  done:
+  err = uv_translate_sys_error(err);
+
+ done_uv:
   uv__free(application);
   uv__free(application_path);
   uv__free(arguments);
@@ -1186,7 +1163,7 @@
     child_stdio_buffer = NULL;
   }
 
-  return uv_translate_sys_error(err);
+  return err;
 }
 
 
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp
index 9bb3d9e..7adf3cd 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp
@@ -486,9 +486,11 @@
   uv_loop_t* loop;
   uv_tty_t* handle;
   uv_req_t* req;
-  DWORD bytes, read_bytes;
+  DWORD bytes;
+  size_t read_bytes;
   WCHAR utf16[MAX_INPUT_BUFFER_LENGTH / 3];
-  DWORD chars, read_chars;
+  DWORD chars;
+  DWORD read_chars;
   LONG status;
   COORD pos;
   BOOL read_console_success;
@@ -529,16 +531,13 @@
                                       NULL);
 
   if (read_console_success) {
-    read_bytes = WideCharToMultiByte(CP_UTF8,
-                                     0,
-                                     utf16,
-                                     read_chars,
-                                     handle->tty.rd.read_line_buffer.base,
-                                     bytes,
-                                     NULL,
-                                     NULL);
+    read_bytes = bytes;
+    uv_utf16_to_wtf8((const uint16_t*)utf16,
+                     read_chars,
+                     &handle->tty.rd.read_line_buffer.base,
+                     &read_bytes);
     SET_REQ_SUCCESS(req);
-    req->u.io.overlapped.InternalHigh = read_bytes;
+    req->u.io.overlapped.InternalHigh = (DWORD) read_bytes;
   } else {
     SET_REQ_ERROR(req, GetLastError());
   }
@@ -802,7 +801,9 @@
       }
 
       if (KEV.uChar.UnicodeChar != 0) {
-        int prefix_len, char_len;
+        int prefix_len;
+        size_t char_len;
+        char* last_key_buf;
 
         /* Character key pressed */
         if (KEV.uChar.UnicodeChar >= 0xD800 &&
@@ -823,38 +824,31 @@
           prefix_len = 0;
         }
 
-        if (KEV.uChar.UnicodeChar >= 0xDC00 &&
-            KEV.uChar.UnicodeChar < 0xE000) {
+        char_len = sizeof handle->tty.rd.last_key;
+        last_key_buf = &handle->tty.rd.last_key[prefix_len];
+        if (handle->tty.rd.last_utf16_high_surrogate) {
           /* UTF-16 surrogate pair */
           WCHAR utf16_buffer[2];
           utf16_buffer[0] = handle->tty.rd.last_utf16_high_surrogate;
           utf16_buffer[1] = KEV.uChar.UnicodeChar;
-          char_len = WideCharToMultiByte(CP_UTF8,
-                                         0,
-                                         utf16_buffer,
-                                         2,
-                                         &handle->tty.rd.last_key[prefix_len],
-                                         sizeof handle->tty.rd.last_key,
-                                         NULL,
-                                         NULL);
+          if (uv_utf16_to_wtf8((const uint16_t*)utf16_buffer,
+                               2,
+                               &last_key_buf,
+                               &char_len))
+            char_len = 0;
+          handle->tty.rd.last_utf16_high_surrogate = 0;
         } else {
           /* Single UTF-16 character */
-          char_len = WideCharToMultiByte(CP_UTF8,
-                                         0,
-                                         &KEV.uChar.UnicodeChar,
-                                         1,
-                                         &handle->tty.rd.last_key[prefix_len],
-                                         sizeof handle->tty.rd.last_key,
-                                         NULL,
-                                         NULL);
+          if (uv_utf16_to_wtf8((const uint16_t*)&KEV.uChar.UnicodeChar,
+                               1,
+                               &last_key_buf,
+                               &char_len))
+            char_len = 0;
         }
 
-        /* Whatever happened, the last character wasn't a high surrogate. */
-        handle->tty.rd.last_utf16_high_surrogate = 0;
-
         /* If the utf16 character(s) couldn't be converted something must be
          * wrong. */
-        if (!char_len) {
+        if (char_len == 0) {
           handle->flags &= ~UV_HANDLE_READING;
           DECREASE_ACTIVE_COUNT(loop, handle);
           handle->read_cb((uv_stream_t*) handle,
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp
index 4b76417..14295c4 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp
@@ -105,7 +105,7 @@
 
 
 int uv_exepath(char* buffer, size_t* size_ptr) {
-  int utf8_len, utf16_buffer_len, utf16_len;
+  size_t utf8_len, utf16_buffer_len, utf16_len;
   WCHAR* utf16_buffer;
   int err;
 
@@ -133,25 +133,17 @@
   }
 
   /* Convert to UTF-8 */
-  utf8_len = WideCharToMultiByte(CP_UTF8,
-                                 0,
-                                 utf16_buffer,
-                                 -1,
-                                 buffer,
-                                 (int) *size_ptr,
-                                 NULL,
-                                 NULL);
-  if (utf8_len == 0) {
-    err = GetLastError();
-    goto error;
+  utf8_len = *size_ptr - 1; /* Reserve space for NUL */
+  err = uv_utf16_to_wtf8((const uint16_t*)utf16_buffer, utf16_len, &buffer, &utf8_len);
+  if (err == UV_ENOBUFS) {
+    utf8_len = *size_ptr - 1;
+    err = 0;
   }
+  *size_ptr = utf8_len;
 
   uv__free(utf16_buffer);
 
-  /* utf8_len *does* include the terminating null at this point, but the
-   * returned size shouldn't. */
-  *size_ptr = utf8_len - 1;
-  return 0;
+  return err;
 
  error:
   uv__free(utf16_buffer);
@@ -214,45 +206,14 @@
   }
 
   r = uv__cwd(&utf16_buffer, &utf16_len);
-  if (r < 0) {
+  if (r < 0)
     return r;
-  }
 
-  /* Check how much space we need */
-  r = WideCharToMultiByte(CP_UTF8,
-                          0,
-                          utf16_buffer,
-                          -1,
-                          NULL,
-                          0,
-                          NULL,
-                          NULL);
-  if (r == 0) {
-    uv__free(utf16_buffer);
-    return uv_translate_sys_error(GetLastError());
-  } else if (r > (int) *size) {
-    uv__free(utf16_buffer);
-    *size = r;
-    return UV_ENOBUFS;
-  }
+  r = uv__copy_utf16_to_utf8(utf16_buffer, utf16_len, buffer, size);
 
-  /* Convert to UTF-8 */
-  r = WideCharToMultiByte(CP_UTF8,
-                          0,
-                          utf16_buffer,
-                          -1,
-                          buffer,
-                          *size > INT_MAX ? INT_MAX : (int) *size,
-                          NULL,
-                          NULL);
   uv__free(utf16_buffer);
 
-  if (r == 0) {
-    return uv_translate_sys_error(GetLastError());
-  }
-
-  *size = r - 1;
-  return 0;
+  return r;
 }
 
 
@@ -262,33 +223,10 @@
   WCHAR drive_letter, env_var[4];
   int r;
 
-  if (dir == NULL) {
-    return UV_EINVAL;
-  }
-
-  utf16_len = MultiByteToWideChar(CP_UTF8,
-                                  0,
-                                  dir,
-                                  -1,
-                                  NULL,
-                                  0);
-  if (utf16_len == 0) {
-    return uv_translate_sys_error(GetLastError());
-  }
-  utf16_buffer = (WCHAR*)uv__malloc(utf16_len * sizeof(WCHAR));
-  if (utf16_buffer == NULL) {
-    return UV_ENOMEM;
-  }
-
-  if (MultiByteToWideChar(CP_UTF8,
-                          0,
-                          dir,
-                          -1,
-                          utf16_buffer,
-                          utf16_len) == 0) {
-    uv__free(utf16_buffer);
-    return uv_translate_sys_error(GetLastError());
-  }
+  /* Convert to UTF-16 */
+  r = uv__convert_utf8_to_utf16(dir, &utf16_buffer);
+  if (r)
+    return r;
 
   if (!SetCurrentDirectoryW(utf16_buffer)) {
     uv__free(utf16_buffer);
@@ -426,29 +364,14 @@
 
   uv__once_init();
 
-  /* Find out how big the buffer for the wide-char title must be */
-  length = MultiByteToWideChar(CP_UTF8, 0, title, -1, NULL, 0);
-  if (!length) {
-    err = GetLastError();
-    goto done;
-  }
-
-  /* Convert to wide-char string */
-  title_w = (WCHAR*)uv__malloc(sizeof(WCHAR) * length);
-  if (!title_w) {
-    uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc");
-  }
-
-  length = MultiByteToWideChar(CP_UTF8, 0, title, -1, title_w, length);
-  if (!length) {
-    err = GetLastError();
-    goto done;
-  }
+  err = uv__convert_utf8_to_utf16(title, &title_w);
+  if (err)
+    return err;
 
   /* If the title must be truncated insert a \0 terminator there */
-  if (length > MAX_TITLE_LENGTH) {
+  length = wcslen(title_w);
+  if (length >= MAX_TITLE_LENGTH)
     title_w[MAX_TITLE_LENGTH - 1] = L'\0';
-  }
 
   if (!SetConsoleTitleW(title_w)) {
     err = GetLastError();
@@ -470,20 +393,19 @@
 
 static int uv__get_process_title(void) {
   WCHAR title_w[MAX_TITLE_LENGTH];
+  DWORD wlen;
 
-  if (!GetConsoleTitleW(title_w, sizeof(title_w) / sizeof(WCHAR))) {
-    return -1;
-  }
+  wlen = GetConsoleTitleW(title_w, sizeof(title_w) / sizeof(WCHAR));
+  if (wlen == 0)
+    return uv_translate_sys_error(GetLastError());
 
-  if (uv__convert_utf16_to_utf8(title_w, -1, &process_title) != 0)
-    return -1;
-
-  return 0;
+  return uv__convert_utf16_to_utf8(title_w, wlen, &process_title);
 }
 
 
 int uv_get_process_title(char* buffer, size_t size) {
   size_t len;
+  int r;
 
   if (buffer == NULL || size == 0)
     return UV_EINVAL;
@@ -495,9 +417,12 @@
    * If the process_title was never read before nor explicitly set,
    * we must query it with getConsoleTitleW
    */
-  if (!process_title && uv__get_process_title() == -1) {
-    LeaveCriticalSection(&process_title_lock);
-    return uv_translate_sys_error(GetLastError());
+  if (process_title == NULL) {
+    r = uv__get_process_title();
+    if (r) {
+      LeaveCriticalSection(&process_title_lock);
+      return r;
+    }
   }
 
   assert(process_title);
@@ -844,19 +769,8 @@
       continue;
 
     /* Compute the size of the interface name. */
-    name_size = WideCharToMultiByte(CP_UTF8,
-                                    0,
-                                    adapter->FriendlyName,
-                                    -1,
-                                    NULL,
-                                    0,
-                                    NULL,
-                                    FALSE);
-    if (name_size <= 0) {
-      uv__free(win_address_buf);
-      return uv_translate_sys_error(GetLastError());
-    }
-    uv_address_buf_size += name_size;
+    name_size = uv_utf16_length_as_wtf8((const uint16_t*)adapter->FriendlyName, -1);
+    uv_address_buf_size += name_size + 1;
 
     /* Count the number of addresses associated with this interface, and
      * compute the size. */
@@ -886,30 +800,25 @@
        adapter != NULL;
        adapter = adapter->Next) {
     IP_ADAPTER_UNICAST_ADDRESS* unicast_address;
-    int name_size;
-    size_t max_name_size;
+    size_t name_size;
+    int r;
 
     if (adapter->OperStatus != IfOperStatusUp ||
         adapter->FirstUnicastAddress == NULL)
       continue;
 
     /* Convert the interface name to UTF8. */
-    max_name_size = (char*) uv_address_buf + uv_address_buf_size - name_buf;
-    if (max_name_size > (size_t) INT_MAX)
-      max_name_size = INT_MAX;
-    name_size = WideCharToMultiByte(CP_UTF8,
-                                    0,
-                                    adapter->FriendlyName,
-                                    -1,
-                                    name_buf,
-                                    (int) max_name_size,
-                                    NULL,
-                                    FALSE);
-    if (name_size <= 0) {
+    name_size = (char*) uv_address_buf + uv_address_buf_size - name_buf;
+    r = uv__copy_utf16_to_utf8(adapter->FriendlyName,
+                               -1,
+                               name_buf,
+                               &name_size);
+    if (r) {
       uv__free(win_address_buf);
       uv__free(uv_address_buf);
-      return uv_translate_sys_error(GetLastError());
+      return r;
     }
+    name_size += 1; /* Add NUL byte. */
 
     /* Add an uv_interface_address_t element for every unicast address. */
     for (unicast_address = (IP_ADAPTER_UNICAST_ADDRESS*)
@@ -1072,7 +981,6 @@
 
 int uv_os_tmpdir(char* buffer, size_t* size) {
   wchar_t *path;
-  DWORD bufsize;
   size_t len;
 
   if (buffer == NULL || size == NULL || *size == 0)
@@ -1089,7 +997,7 @@
   if (path == NULL) {
     return UV_ENOMEM;
   }
-  len  = GetTempPathW(len, path);
+  len = GetTempPathW(len, path);
 
   if (len == 0) {
     uv__free(path);
@@ -1104,34 +1012,7 @@
     path[len] = L'\0';
   }
 
-  /* Check how much space we need */
-  bufsize = WideCharToMultiByte(CP_UTF8, 0, path, -1, NULL, 0, NULL, NULL);
-
-  if (bufsize == 0) {
-    uv__free(path);
-    return uv_translate_sys_error(GetLastError());
-  } else if (bufsize > *size) {
-    uv__free(path);
-    *size = bufsize;
-    return UV_ENOBUFS;
-  }
-
-  /* Convert to UTF-8 */
-  bufsize = WideCharToMultiByte(CP_UTF8,
-                                0,
-                                path,
-                                -1,
-                                buffer,
-                                *size,
-                                NULL,
-                                NULL);
-  uv__free(path);
-
-  if (bufsize == 0)
-    return uv_translate_sys_error(GetLastError());
-
-  *size = bufsize - 1;
-  return 0;
+  return uv__copy_utf16_to_utf8(path, len, buffer, size);
 }
 
 
@@ -1142,95 +1023,71 @@
  * If utf16 is null terminated, utf16len can be set to -1, otherwise it must
  * be specified.
  */
-int uv__convert_utf16_to_utf8(const WCHAR* utf16, int utf16len, char** utf8) {
-  DWORD bufsize;
+int uv__convert_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char** utf8) {
+  size_t utf8_len = 0;
 
   if (utf16 == NULL)
     return UV_EINVAL;
 
-  /* Check how much space we need */
-  bufsize = WideCharToMultiByte(CP_UTF8,
-                                0,
-                                utf16,
-                                utf16len,
-                                NULL,
-                                0,
-                                NULL,
-                                NULL);
-
-  if (bufsize == 0)
-    return uv_translate_sys_error(GetLastError());
-
-  /* Allocate the destination buffer adding an extra byte for the terminating
-   * NULL. If utf16len is not -1 WideCharToMultiByte will not add it, so
-   * we do it ourselves always, just in case. */
-  *utf8 = (char*)uv__malloc(bufsize + 1);
-
-  if (*utf8 == NULL)
-    return UV_ENOMEM;
-
-  /* Convert to UTF-8 */
-  bufsize = WideCharToMultiByte(CP_UTF8,
-                                0,
-                                utf16,
-                                utf16len,
-                                *utf8,
-                                bufsize,
-                                NULL,
-                                NULL);
-
-  if (bufsize == 0) {
-    uv__free(*utf8);
-    *utf8 = NULL;
-    return uv_translate_sys_error(GetLastError());
-  }
-
-  (*utf8)[bufsize] = '\0';
-  return 0;
+   *utf8 = NULL;
+   return uv_utf16_to_wtf8((const uint16_t*)utf16, utf16len, utf8, &utf8_len);
 }
 
 
 /*
  * Converts a UTF-8 string into a UTF-16 one. The resulting string is
  * null-terminated.
- *
- * If utf8 is null terminated, utf8len can be set to -1, otherwise it must
- * be specified.
  */
-int uv__convert_utf8_to_utf16(const char* utf8, int utf8len, WCHAR** utf16) {
+int uv__convert_utf8_to_utf16(const char* utf8, WCHAR** utf16) {
   int bufsize;
 
   if (utf8 == NULL)
     return UV_EINVAL;
 
-  /* Check how much space we need */
-  bufsize = MultiByteToWideChar(CP_UTF8, 0, utf8, utf8len, NULL, 0);
+  /* Check how much space we need (including NUL). */
+  bufsize = uv_wtf8_length_as_utf16(utf8);
+  if (bufsize < 0)
+    return UV__EINVAL;
 
-  if (bufsize == 0)
-    return uv_translate_sys_error(GetLastError());
-
-  /* Allocate the destination buffer adding an extra byte for the terminating
-   * NULL. If utf8len is not -1 MultiByteToWideChar will not add it, so
-   * we do it ourselves always, just in case. */
-  *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * (bufsize + 1));
+  /* Allocate the destination buffer. */
+  *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * bufsize);
 
   if (*utf16 == NULL)
     return UV_ENOMEM;
 
   /* Convert to UTF-16 */
-  bufsize = MultiByteToWideChar(CP_UTF8, 0, utf8, utf8len, *utf16, bufsize);
+  uv_wtf8_to_utf16(utf8, (uint16_t*)*utf16, bufsize);
 
-  if (bufsize == 0) {
-    uv__free(*utf16);
-    *utf16 = NULL;
-    return uv_translate_sys_error(GetLastError());
-  }
-
-  (*utf16)[bufsize] = L'\0';
   return 0;
 }
 
 
+/*
+ * Converts a UTF-16 string into a UTF-8 one in an existing buffer. The
+ * resulting string is null-terminated.
+ *
+ * If utf16 is null terminated, utf16len can be set to -1, otherwise it must
+ * be specified.
+ */
+int uv__copy_utf16_to_utf8(const WCHAR* utf16buffer, size_t utf16len, char* utf8, size_t *size) {
+  int r;
+
+  if (utf8 == NULL || size == NULL)
+    return UV_EINVAL;
+
+  if (*size == 0) {
+    *size = uv_utf16_length_as_wtf8((const uint16_t*)utf16buffer, utf16len);
+    r = UV_ENOBUFS;
+  } else {
+    *size -= 1; /* Reserve space for NUL. */
+    r = uv_utf16_to_wtf8((const uint16_t*)utf16buffer, utf16len, &utf8, size);
+  }
+  if (r == UV_ENOBUFS)
+    *size += 1; /* Add space for NUL. */
+  return r;
+}
+
+
 static int uv__getpwuid_r(uv_passwd_t* pwd) {
   HANDLE token;
   wchar_t username[UNLEN + 1];
@@ -1395,14 +1252,13 @@
   wchar_t* var;
   DWORD varlen;
   wchar_t* name_w;
-  DWORD bufsize;
   size_t len;
   int r;
 
   if (name == NULL || buffer == NULL || size == NULL || *size == 0)
     return UV_EINVAL;
 
-  r = uv__convert_utf8_to_utf16(name, -1, &name_w);
+  r = uv__convert_utf8_to_utf16(name, &name_w);
 
   if (r != 0)
     return r;
@@ -1443,35 +1299,7 @@
     }
   }
 
-  /* Check how much space we need */
-  bufsize = WideCharToMultiByte(CP_UTF8, 0, var, -1, NULL, 0, NULL, NULL);
-
-  if (bufsize == 0) {
-    r = uv_translate_sys_error(GetLastError());
-    goto fail;
-  } else if (bufsize > *size) {
-    *size = bufsize;
-    r = UV_ENOBUFS;
-    goto fail;
-  }
-
-  /* Convert to UTF-8 */
-  bufsize = WideCharToMultiByte(CP_UTF8,
-                                0,
-                                var,
-                                -1,
-                                buffer,
-                                *size,
-                                NULL,
-                                NULL);
-
-  if (bufsize == 0) {
-    r = uv_translate_sys_error(GetLastError());
-    goto fail;
-  }
-
-  *size = bufsize - 1;
-  r = 0;
+  r = uv__copy_utf16_to_utf8(var, len, buffer, size);
 
 fail:
 
@@ -1493,12 +1321,12 @@
   if (name == NULL || value == NULL)
     return UV_EINVAL;
 
-  r = uv__convert_utf8_to_utf16(name, -1, &name_w);
+  r = uv__convert_utf8_to_utf16(name, &name_w);
 
   if (r != 0)
     return r;
 
-  r = uv__convert_utf8_to_utf16(value, -1, &value_w);
+  r = uv__convert_utf8_to_utf16(value, &value_w);
 
   if (r != 0) {
     uv__free(name_w);
@@ -1523,7 +1351,7 @@
   if (name == NULL)
     return UV_EINVAL;
 
-  r = uv__convert_utf8_to_utf16(name, -1, &name_w);
+  r = uv__convert_utf8_to_utf16(name, &name_w);
 
   if (r != 0)
     return r;
@@ -1540,9 +1368,6 @@
 
 int uv_os_gethostname(char* buffer, size_t* size) {
   WCHAR buf[UV_MAXHOSTNAMESIZE];
-  size_t len;
-  char* utf8_str;
-  int convert_result;
 
   if (buffer == NULL || size == NULL || *size == 0)
     return UV_EINVAL;
@@ -1555,22 +1380,7 @@
   if (pGetHostNameW(buf, UV_MAXHOSTNAMESIZE) != 0)
     return uv_translate_sys_error(WSAGetLastError());
 
-  convert_result = uv__convert_utf16_to_utf8(buf, -1, &utf8_str);
-
-  if (convert_result != 0)
-    return convert_result;
-
-  len = strlen(utf8_str);
-  if (len >= *size) {
-    *size = len + 1;
-    uv__free(utf8_str);
-    return UV_ENOBUFS;
-  }
-
-  memcpy(buffer, utf8_str, len + 1);
-  uv__free(utf8_str);
-  *size = len;
-  return 0;
+  return uv__copy_utf16_to_utf8(buf, -1, buffer, size);
 }
 
 
@@ -1676,7 +1486,7 @@
   HKEY registry_key;
   WCHAR product_name_w[256];
   DWORD product_name_w_size;
-  int version_size;
+  size_t version_size;
   int processor_level;
   int r;
 
@@ -1707,7 +1517,7 @@
   r = RegOpenKeyExW(HKEY_LOCAL_MACHINE,
                     L"SOFTWARE\\Microsoft\\Windows NT\\CurrentVersion",
                     0,
-                    KEY_QUERY_VALUE,
+                    KEY_QUERY_VALUE | KEY_WOW64_64KEY,
                     &registry_key);
 
   if (r == ERROR_SUCCESS) {
@@ -1738,37 +1548,29 @@
         }
       }
 
-      version_size = WideCharToMultiByte(CP_UTF8,
-                                         0,
-                                         product_name_w,
-                                         -1,
-                                         buffer->version,
-                                         sizeof(buffer->version),
-                                         NULL,
-                                         NULL);
-      if (version_size == 0) {
-        r = uv_translate_sys_error(GetLastError());
+      version_size = sizeof(buffer->version);
+      r = uv__copy_utf16_to_utf8(product_name_w,
+                                 -1,
+                                 buffer->version,
+                                 &version_size);
+      if (r)
         goto error;
-      }
     }
   }
 
   /* Append service pack information to the version if present. */
   if (os_info.szCSDVersion[0] != L'\0') {
     if (version_size > 0)
-      buffer->version[version_size - 1] = ' ';
+      buffer->version[version_size++] = ' ';
 
-    if (WideCharToMultiByte(CP_UTF8,
-                            0,
-                            os_info.szCSDVersion,
-                            -1,
-                            buffer->version + version_size,
-                            sizeof(buffer->version) - version_size,
-                            NULL,
-                            NULL) == 0) {
-      r = uv_translate_sys_error(GetLastError());
+    version_size = sizeof(buffer->version) - version_size;
+    r = uv__copy_utf16_to_utf8(os_info.szCSDVersion,
+                               -1,
+                               buffer->version + 
+                                 sizeof(buffer->version) - version_size,
+                               &version_size);
+    if (r)
       goto error;
-    }
   }
 
   /* Populate the sysname field. */
diff --git a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/winsock.cpp b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/winsock.cpp
index cda82bc..7843e9f 100644
--- a/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/winsock.cpp
+++ b/third_party/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/win/winsock.cpp
@@ -25,7 +25,6 @@
 #include "uv.h"
 #include "internal.h"
 
-
 #pragma comment(lib, "Ws2_32.lib")
 
 /* Whether there are any non-IFS LSPs stacked on TCP */
diff --git a/third_party/allwpilib/wpinet/wpinet-config.cmake.in b/third_party/allwpilib/wpinet/wpinet-config.cmake.in
index f54fe45..e62719b 100644
--- a/third_party/allwpilib/wpinet/wpinet-config.cmake.in
+++ b/third_party/allwpilib/wpinet/wpinet-config.cmake.in
@@ -5,3 +5,6 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpinet.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/wpinet_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/wpiunits/CMakeLists.txt b/third_party/allwpilib/wpiunits/CMakeLists.txt
new file mode 100644
index 0000000..bdbe42b
--- /dev/null
+++ b/third_party/allwpilib/wpiunits/CMakeLists.txt
@@ -0,0 +1,16 @@
+project(wpiunits)
+
+# Java bindings
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+
+    add_jar(wpiunits_jar ${JAVA_SOURCES} OUTPUT_NAME wpiunits)
+
+    install_jar(wpiunits_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS wpiunits_jar FILE wpiunits.cmake DESTINATION share/wpiunits)
+    install(FILES wpiunits-config.cmake DESTINATION share/wpiunits)
+endif()
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Angle.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Angle.java
index 105a935..8fd20bb 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Angle.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Angle.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of angular dimension.
+ *
+ * <p>This is the base type for units of angular dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Angle&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Degrees} and {@link Units#Radians}) can be found in the
+ * {@link Units} class.
+ */
 // technically, angles are unitless dimensions
 // eg Mass * Distance * Velocity<Angle> is equivalent to (Mass * Distance) / Time - otherwise known
 // as Power - in other words, Velocity<Angle> is /actually/ Frequency
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java
index 5e342da..28a36bf 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java
@@ -20,11 +20,11 @@
   public static final Velocity<Distance> Velocity =
       new Velocity<>(Distance, Time, "Meter per Second", "m/s");
 
-  /** The standard unit of mass, grams. */
+  /** The standard unit of mass, kilograms. */
   public static final Mass Mass = new Mass(1, "Kilogram", "Kg");
 
-  /** The standard unit of angles, revolutions. */
-  public static final Angle Angle = new Angle(1, "Revolution", "R");
+  /** The standard unit of angles, radians. */
+  public static final Angle Angle = new Angle(1, "Radian", "rad");
 
   /** The standard "unitless" unit. */
   public static final Dimensionless Value = new Dimensionless(1, "<?>", "<?>");
@@ -41,5 +41,6 @@
   /** The standard unit of power, watts. */
   public static final Power Power = new Power(1, "Watt", "W");
 
+  /** The standard unit of temperature, kelvin. */
   public static final Temperature Temperature = new Temperature(x -> x, x -> x, "Kelvin", "K");
 }
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Current.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Current.java
index 88418c5..e1153fd 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Current.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Current.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of electic current dimension.
+ *
+ * <p>This is the base type for units of current dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Current&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Amps} and {@link Units#Milliamps}) can be found in the
+ * {@link Units} class.
+ */
 public class Current extends Unit<Current> {
   Current(double baseUnitEquivalent, String name, String symbol) {
     super(Current.class, baseUnitEquivalent, name, symbol);
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Distance.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Distance.java
index d5512dd..2e32419 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Distance.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Distance.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of angular dimension.
+ *
+ * <p>This is the base type for units of distance dimension. It is also used to specify the
+ * dimension for {@link Measure}: <code>Measure&lt;Distance&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Meters} and {@link Units#Inches}) can be found in the
+ * {@link Units} class.
+ */
 public class Distance extends Unit<Distance> {
   /** Creates a new unit with the given name and multiplier to the base unit. */
   Distance(double baseUnitEquivalent, String name, String symbol) {
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Energy.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Energy.java
index d43adf9..d3513d5 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Energy.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Energy.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of energy dimension.
+ *
+ * <p>This is the base type for units of energy dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Energy&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Joules} and {@link Units#Kilojoules}) can be found in the
+ * {@link Units} class.
+ */
 public class Energy extends Unit<Energy> {
   protected Energy(
       UnaryFunction toBaseConverter, UnaryFunction fromBaseConverter, String name, String symbol) {
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/ImmutableMeasure.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/ImmutableMeasure.java
index 84fb732..cc7f09f 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/ImmutableMeasure.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/ImmutableMeasure.java
@@ -7,10 +7,10 @@
 import java.util.Objects;
 
 /**
- * A measure holds the magnitude and unit of some dimension, such as distance, time, or speed. A
- * measure is <i>immutable</i> and <i>type safe</i>, making it easy to use in concurrent situations
- * and gives compile-time safety. Two measures with the same <i>unit</i> and <i>magnitude</i> are
- * effectively the same object.
+ * A measure holds the magnitude and unit of some dimension, such as distance, time, or speed. An
+ * immutable measure is <i>immutable</i> and <i>type safe</i>, making it easy to use in concurrent
+ * situations and gives compile-time safety. Two measures with the same <i>unit</i> and
+ * <i>magnitude</i> are effectively equivalent objects.
  *
  * @param <U> the unit type of the measure
  */
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mass.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mass.java
index c9dc0bb..eb4c45d 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mass.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mass.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of mass dimension.
+ *
+ * <p>This is the base type for units of mass dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Mass&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Grams} and {@link Units#Pounds}) can be found in the {@link
+ * Units} class.
+ */
 public class Mass extends Unit<Mass> {
   /** Creates a new unit with the given name and multiplier to the base unit. */
   Mass(double baseUnitEquivalent, String name, String symbol) {
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Measure.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Measure.java
index 908eb2d..8982e46 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Measure.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Measure.java
@@ -6,7 +6,7 @@
 
 /**
  * A measure holds the magnitude and unit of some dimension, such as distance, time, or speed. Two
- * measures with the same <i>unit</i> and <i>magnitude</i> are effectively the same object.
+ * measures with the same <i>unit</i> and <i>magnitude</i> are effectively equivalent objects.
  *
  * @param <U> the unit type of the measure
  */
@@ -179,6 +179,21 @@
   }
 
   /**
+   * Creates a velocity measure equivalent to this one per a unit of time.
+   *
+   * <pre>
+   *   Radians.of(3.14).per(Second) // Velocity&lt;Angle&gt; equivalent to RadiansPerSecond.of(3.14)
+   * </pre>
+   *
+   * @param time the unit of time
+   * @return the velocity measure
+   */
+  default Measure<Velocity<U>> per(Time time) {
+    var newUnit = unit().per(time);
+    return newUnit.of(magnitude());
+  }
+
+  /**
    * Adds another measure to this one. The resulting measure has the same unit as this one.
    *
    * @param other the measure to add to this one
@@ -371,7 +386,7 @@
 
   /**
    * Returns a string representation of this measurement in a longhand form. The name of the backing
-   * unit is used, rather than its symbol, and the magnitude is represented in a full string, no
+   * unit is used, rather than its symbol, and the magnitude is represented in a full string, not
    * scientific notation. (Very large values may be represented in scientific notation, however)
    *
    * @return the long form representation of this measurement
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mult.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mult.java
index 7b6beb3..8a0a5fd 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mult.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Mult.java
@@ -8,9 +8,7 @@
 import java.util.Objects;
 
 /**
- * A combinatory unit type that is equivalent to the product of two other others. For example,
- * Newton * Meters for torque could be represented as a unit of <code>
- * Mult&lt;Force, Distance, Torque&gt;</code>
+ * A combinatory unit type that is equivalent to the product of two other others.
  *
  * @param <A> the type of the first unit in the result
  * @param <B> the type of the second unit in the result
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/MutableMeasure.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/MutableMeasure.java
index ad035f7..25ca5ab 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/MutableMeasure.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/MutableMeasure.java
@@ -8,13 +8,13 @@
 
 /**
  * A specialization of {@link Measure} that allows for mutability. This is intended to be used for
- * memory use reasons (such as on the memory-restricted RoboRIO 1 or 2 or SBC coprocessors) and
+ * memory use reasons (such as on the memory-restricted roboRIO 1 or 2 or SBC coprocessors) and
  * should NOT be exposed in the public API for a class that uses it.
  *
- * <p>The advantage of using this class is that only one instance of a measurement object will exist
- * at a time, as opposed to instantiating a new immutable instance every time a value is fetched.
- * This can greatly reduce memory pressure, but comes at the cost of increased code complexity and
- * sensitivity to race conditions if used poorly.
+ * <p>The advantage of using this class is to reuse one instance of a measurement object, as opposed
+ * to instantiating a new immutable instance every time an operation is performed. This will reduce
+ * memory pressure, but comes at the cost of increased code complexity and sensitivity to race
+ * conditions if misused.
  *
  * <p>Any unsafe methods are prefixed with {@code mut_*}, such as {@link #mut_plus(Measure)} or
  * {@link #mut_replace(Measure)}. These methods will change the internal state of the measurement
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Per.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Per.java
index 3d8099f..e22b7d6 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Per.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Per.java
@@ -11,9 +11,7 @@
  * Generic combinatory unit type that represents the proportion of one unit to another, such as
  * Meters per Second or Radians per Celsius.
  *
- * <p>Note that due to restrictions with the Java type system, velocities (change per unit time) are
- * represented by the {@link Velocity} class. Accelerations are represented by {@code
- * Velocity<Velocity<X>>}, and so on.
+ * <p>Note: {@link Velocity} is used to represent the velocity dimension.
  *
  * @param <N> the type of the numerator unit
  * @param <D> the type of the denominator unit
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Power.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Power.java
index cb93c66..627f1cd 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Power.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Power.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of power dimension.
+ *
+ * <p>This is the base type for units of power dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Power&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Watts} and {@link Units#Horsepower}) can be found in the
+ * {@link Units} class.
+ */
 public class Power extends Unit<Power> {
   Power(double baseUnitEquivalent, String name, String symbol) {
     super(Power.class, baseUnitEquivalent, name, symbol);
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Temperature.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Temperature.java
index 0a82e40..c1f27f4 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Temperature.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Temperature.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of temperature dimension.
+ *
+ * <p>This is the base type for units of temperature dimension. It is also used to specify the
+ * dimension for {@link Measure}: <code>Measure&lt;Temperature&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Celsius} and {@link Units#Fahrenheit}) can be found in the
+ * {@link Units} class.
+ */
 public class Temperature extends Unit<Temperature> {
   Temperature(
       UnaryFunction toBaseConverter, UnaryFunction fromBaseConverter, String name, String symbol) {
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Time.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Time.java
index ad91343..858ef35 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Time.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Time.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of time dimension.
+ *
+ * <p>This is the base type for units of time dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Time&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Seconds} and {@link Units#Milliseconds}) can be found in
+ * the {@link Units} class.
+ */
 public class Time extends Unit<Time> {
   /** Creates a new unit with the given name and multiplier to the base unit. */
   Time(double baseUnitEquivalent, String name, String symbol) {
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Unit.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Unit.java
index df960ea..e2b3d82 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Unit.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Unit.java
@@ -7,7 +7,10 @@
 import java.util.Objects;
 
 /**
- * A unit is some unit of measurement that defines a quantity, such as grams, meters, or seconds.
+ * Unit of measurement that defines a quantity, such as grams, meters, or seconds.
+ *
+ * <p>This is the base class for units. Actual units (such as {@link Units#Grams} and {@link
+ * Units#Meters}) can be found in the {@link Units} class.
  *
  * @param <U> the self type, e.g. {@code class SomeUnit extends Unit<SomeUnit>}
  */
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Units.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Units.java
index ecf0768..bf0ebd6 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Units.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Units.java
@@ -6,6 +6,7 @@
 
 import java.util.Locale;
 
+/** Contains a set of predefined units. */
 public final class Units {
   private Units() {
     // Prevent instantiation
@@ -38,10 +39,9 @@
   public static final Time Minute = Minutes; // singularized alias
 
   // Angle
-  public static final Angle Revolutions = BaseUnits.Angle;
-  public static final Angle Rotations = new Angle(1, "Rotation", "R"); // alias
-  public static final Angle Radians =
-      derive(Revolutions).splitInto(2 * Math.PI).named("Radian").symbol("rad").make();
+  public static final Angle Radians = BaseUnits.Angle;
+  public static final Angle Revolutions = new Angle(2 * Math.PI, "Revolution", "R");
+  public static final Angle Rotations = new Angle(2 * Math.PI, "Rotation", "R"); // alias revolution
   public static final Angle Degrees =
       derive(Revolutions).splitInto(360).named("Degree").symbol("°").make();
 
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Velocity.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Velocity.java
index 87cffb7..185c59e 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Velocity.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Velocity.java
@@ -7,6 +7,19 @@
 import edu.wpi.first.units.collections.LongToObjectHashMap;
 import java.util.Objects;
 
+/**
+ * Unit of velocity dimension that is a combination of a distance unit (numerator) and a time unit
+ * (denominator).
+ *
+ * <p>This is the base type for units of velocity dimension. It is also used in combination with a
+ * distance dimension to specify the dimension for {@link Measure}. For example: <code>
+ * Measure&lt;Velocity&lt;Distance&gt;&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#MetersPerSecond} and {@link Units#RPM}) can be found in the
+ * {@link Units} class.
+ *
+ * @param <D> the distance unit, such as {@link Angle} or {@link Distance}
+ */
 public class Velocity<D extends Unit<D>> extends Unit<Velocity<D>> {
   private final D m_unit;
   private final Time m_period;
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Voltage.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Voltage.java
index 3c30203..9006d19 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Voltage.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/Voltage.java
@@ -4,6 +4,15 @@
 
 package edu.wpi.first.units;
 
+/**
+ * Unit of electric voltage dimension.
+ *
+ * <p>This is the base type for units of voltage dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Voltage&gt;</code>.
+ *
+ * <p>Actual units (such as {@link Units#Volts} and {@link Units#Millivolts}) can be found in the
+ * {@link Units} class.
+ */
 public class Voltage extends Unit<Voltage> {
   Voltage(double baseUnitEquivalent, String name, String symbol) {
     super(Voltage.class, baseUnitEquivalent, name, symbol);
diff --git a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/collections/LongToObjectHashMap.java b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/collections/LongToObjectHashMap.java
index ff2e94d..f47ee29 100644
--- a/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/collections/LongToObjectHashMap.java
+++ b/third_party/allwpilib/wpiunits/src/main/java/edu/wpi/first/units/collections/LongToObjectHashMap.java
@@ -54,6 +54,9 @@
   @SuppressWarnings("unchecked")
   private V[] m_values = (V[]) new Object[m_capacity];
 
+  /** Default constructor. */
+  public LongToObjectHashMap() {}
+
   /**
    * Puts a value {@code value} corresponding to key {@code key} in the map.
    *
diff --git a/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java b/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java
index 6bc867d..b2c729e 100644
--- a/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java
+++ b/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java
@@ -78,7 +78,7 @@
   }
 
   @Test
-  void testPerUnitTime() {
+  void testPerMeasureTime() {
     var measure = Units.Kilograms.of(144);
     var dt = Units.Milliseconds.of(53);
 
@@ -90,6 +90,16 @@
   }
 
   @Test
+  void testPerUnitTime() {
+    var measure = Units.Kilograms.of(144);
+    var result = measure.per(Units.Millisecond);
+
+    assertEquals(Velocity.class, result.unit().getClass());
+    assertEquals(144_000.0, result.baseUnitMagnitude(), 1e-5);
+    assertEquals(Units.Kilograms.per(Units.Milliseconds), result.unit());
+  }
+
+  @Test
   void testTimesMeasure() {
     var m1 = Units.Volts.of(1.567);
     var m2 = Units.Kilograms.of(8.4e-5);
@@ -176,6 +186,7 @@
   }
 
   @Test
+  @SuppressWarnings("SelfComparison")
   void testCompare() {
     var unit = new ExampleUnit(7);
     var base = unit.of(1);
diff --git a/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java b/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java
index 4659111..c428cad 100644
--- a/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java
+++ b/third_party/allwpilib/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java
@@ -203,13 +203,14 @@
 
   @Test
   void testRevolutions() {
-    testBaseUnit(Revolutions);
+    assertEquals(1, Revolutions.convertFrom(2 * Math.PI, Radians), thresh);
     assertEquals("Revolution", Revolutions.name());
     assertEquals("R", Revolutions.symbol());
   }
 
   @Test
   void testRadians() {
+    testBaseUnit(Radians);
     assertEquals(2 * Math.PI, Radians.convertFrom(1, Revolutions), thresh);
     assertEquals(2 * Math.PI, Radians.convertFrom(360, Degrees), thresh);
     assertEquals("Radian", Radians.name());
diff --git a/third_party/allwpilib/wpiunits/wpiunits-config.cmake b/third_party/allwpilib/wpiunits/wpiunits-config.cmake
new file mode 100644
index 0000000..e04a6bc
--- /dev/null
+++ b/third_party/allwpilib/wpiunits/wpiunits-config.cmake
@@ -0,0 +1,2 @@
+get_filename_component(SELF_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+include(${SELF_DIR}/wpiunits.cmake)
diff --git a/third_party/allwpilib/wpiutil/CMakeLists.txt b/third_party/allwpilib/wpiutil/CMakeLists.txt
index a319293..a3f6f19 100644
--- a/third_party/allwpilib/wpiutil/CMakeLists.txt
+++ b/third_party/allwpilib/wpiutil/CMakeLists.txt
@@ -6,104 +6,118 @@
 include(AddTest)
 include(DownloadAndCheck)
 
-file(GLOB wpiutil_jni_src src/main/native/cpp/jni/WPIUtilJNI.cpp src/main/native/cpp/jni/DataLogJNI.cpp)
+file(
+    GLOB wpiutil_jni_src
+    src/main/native/cpp/jni/WPIUtilJNI.cpp
+    src/main/native/cpp/jni/DataLogJNI.cpp
+)
 
 # Java bindings
-if (WITH_JAVA)
-  find_package(Java REQUIRED)
-  find_package(JNI REQUIRED)
-  include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    find_package(JNI REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.15.2.jar")
+    if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.15.2.jar")
         set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
         set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson")
 
         message(STATUS "Downloading Jackson jarfiles...")
 
         download_and_check("${BASE_URL}com/fasterxml/jackson/core/jackson-core/2.15.2/jackson-core-2.15.2.jar"
-            "${JAR_ROOT}/jackson-core-2.15.2.jar")
+            "${JAR_ROOT}/jackson-core-2.15.2.jar"
+        )
         download_and_check("${BASE_URL}com/fasterxml/jackson/core/jackson-databind/2.15.2/jackson-databind-2.15.2.jar"
-            "${JAR_ROOT}/jackson-databind-2.15.2.jar")
+            "${JAR_ROOT}/jackson-databind-2.15.2.jar"
+        )
         download_and_check("${BASE_URL}com/fasterxml/jackson/core/jackson-annotations/2.15.2/jackson-annotations-2.15.2.jar"
-            "${JAR_ROOT}/jackson-annotations-2.15.2.jar")
+            "${JAR_ROOT}/jackson-annotations-2.15.2.jar"
+        )
 
         message(STATUS "All files downloaded.")
     endif()
 
-  file(GLOB JACKSON_JARS
-        ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar)
+    file(GLOB JACKSON_JARS ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar)
 
-  if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/quickbuf-runtime-1.3.2.jar")
+    if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/quickbuf-runtime-1.3.3.jar")
         set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
         set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf")
 
         message(STATUS "Downloading Quickbuf jarfile...")
-        file(DOWNLOAD "${BASE_URL}us/hebi/quickbuf/quickbuf-runtime/1.3.2/quickbuf-runtime-1.3.2.jar"
-            "${JAR_ROOT}/quickbuf-runtime-1.3.2.jar")
+        file(
+            DOWNLOAD
+                "${BASE_URL}us/hebi/quickbuf/quickbuf-runtime/1.3.3/quickbuf-runtime-1.3.3.jar"
+                "${JAR_ROOT}/quickbuf-runtime-1.3.3.jar"
+        )
 
         message(STATUS "Downloaded.")
-  endif()
+    endif()
 
-  file(GLOB QUICKBUF_JAR
-        ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar)
+    file(GLOB QUICKBUF_JAR ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar)
 
-  set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${JACKSON_JARS} ${QUICKBUF_JAR})
+    set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${JACKSON_JARS} ${QUICKBUF_JAR})
 
-  set(CMAKE_JNI_TARGET true)
+    set(CMAKE_JNI_TARGET true)
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
 
-  add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${JACKSON_JARS} ${QUICKBUF_JAR} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers)
+    add_jar(
+        wpiutil_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS ${JACKSON_JARS} ${QUICKBUF_JAR}
+        OUTPUT_NAME wpiutil
+        GENERATE_NATIVE_HEADERS wpiutil_jni_headers
+    )
 
-  get_property(WPIUTIL_JAR_FILE TARGET wpiutil_jar PROPERTY JAR_FILE)
-  install(FILES ${WPIUTIL_JAR_FILE} DESTINATION "${java_lib_dest}")
+    install_jar(wpiutil_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS wpiutil_jar FILE wpiutil_jar.cmake DESTINATION share/wpiutil)
 
-  set_property(TARGET wpiutil_jar PROPERTY FOLDER "java")
+    add_library(wpiutiljni ${wpiutil_jni_src})
+    wpilib_target_warnings(wpiutiljni)
+    target_link_libraries(wpiutiljni PUBLIC wpiutil)
 
-  add_library(wpiutiljni ${wpiutil_jni_src})
-  wpilib_target_warnings(wpiutiljni)
-  target_link_libraries(wpiutiljni PUBLIC wpiutil)
+    set_property(TARGET wpiutiljni PROPERTY FOLDER "libraries")
 
-  set_property(TARGET wpiutiljni PROPERTY FOLDER "libraries")
+    target_link_libraries(wpiutiljni PRIVATE wpiutil_jni_headers)
+    add_dependencies(wpiutiljni wpiutil_jar)
 
-  target_link_libraries(wpiutiljni PRIVATE wpiutil_jni_headers)
-  add_dependencies(wpiutiljni wpiutil_jar)
-
-  install(TARGETS wpiutiljni EXPORT wpiutiljni)
-
+    install(TARGETS wpiutiljni EXPORT wpiutiljni)
 endif()
 
-if (WITH_JAVA_SOURCE)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  file(GLOB WPIUTIL_SOURCES src/main/java/edu/wpi/first/util/*.java)
-  file(GLOB WPIUTIL_CLEANUP_SOURCES src/main/java/edu/wpi/first/util/cleanup/*.java)
-  file(GLOB WPIUTIL_CONCURRENT_SOURCES src/main/java/edu/wpi/first/util/concurrent/*.java)
-  file(GLOB WPIUTIL_DATALOG_SOURCES src/main/java/edu/wpi/first/util/datalog/*.java)
-  file(GLOB WPIUTIL_FUNCTION_SOURCES src/main/java/edu/wpi/first/util/function/*.java)
-  file(GLOB WPIUTIL_SENDABLE_SOURCES src/main/java/edu/wpi/first/util/sendable/*.java)
-  add_jar(wpiutil_src_jar
-  RESOURCES NAMESPACE "edu/wpi/first/util" ${WPIUTIL_SOURCES}
-  NAMESPACE "edu/wpi/first/util/cleanup" ${WPIUTIL_CLEANUP_SOURCES}
-  NAMESPACE "edu/wpi/first/util/concurrent" ${WPIUTIL_CONCURRENT_SOURCES}
-  NAMESPACE "edu/wpi/first/util/datalog" ${WPIUTIL_DATALOG_SOURCES}
-  NAMESPACE "edu/wpi/first/util/function" ${WPIUTIL_FUNCTION_SOURCES}
-  NAMESPACE "edu/wpi/first/util/sendable" ${WPIUTIL_SENDABLE_SOURCES}
-  OUTPUT_NAME wpiutil-sources)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB WPIUTIL_SOURCES src/main/java/edu/wpi/first/util/*.java)
+    file(GLOB WPIUTIL_CLEANUP_SOURCES src/main/java/edu/wpi/first/util/cleanup/*.java)
+    file(GLOB WPIUTIL_CONCURRENT_SOURCES src/main/java/edu/wpi/first/util/concurrent/*.java)
+    file(GLOB WPIUTIL_DATALOG_SOURCES src/main/java/edu/wpi/first/util/datalog/*.java)
+    file(GLOB WPIUTIL_FUNCTION_SOURCES src/main/java/edu/wpi/first/util/function/*.java)
+    file(GLOB WPIUTIL_SENDABLE_SOURCES src/main/java/edu/wpi/first/util/sendable/*.java)
+    add_jar(
+        wpiutil_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/util" ${WPIUTIL_SOURCES}
+        NAMESPACE "edu/wpi/first/util/cleanup" ${WPIUTIL_CLEANUP_SOURCES}
+        NAMESPACE "edu/wpi/first/util/concurrent" ${WPIUTIL_CONCURRENT_SOURCES}
+        NAMESPACE "edu/wpi/first/util/datalog" ${WPIUTIL_DATALOG_SOURCES}
+        NAMESPACE "edu/wpi/first/util/function" ${WPIUTIL_FUNCTION_SOURCES}
+        NAMESPACE "edu/wpi/first/util/sendable" ${WPIUTIL_SENDABLE_SOURCES}
+        OUTPUT_NAME wpiutil-sources
+    )
 
-  get_property(WPIUTIL_SRC_JAR_FILE TARGET wpiutil_src_jar PROPERTY JAR_FILE)
-  install(FILES ${WPIUTIL_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+    get_property(WPIUTIL_SRC_JAR_FILE TARGET wpiutil_src_jar PROPERTY JAR_FILE)
+    install(FILES ${WPIUTIL_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-  set_property(TARGET wpiutil_src_jar PROPERTY FOLDER "java")
+    set_property(TARGET wpiutil_src_jar PROPERTY FOLDER "java")
 endif()
 
 set(THREADS_PREFER_PTHREAD_FLAG ON)
 find_package(Threads REQUIRED)
 
-if (NOT MSVC AND NOT APPLE AND NOT ANDROID)
+if(NOT MSVC AND NOT APPLE AND NOT ANDROID)
     find_library(ATOMIC NAMES atomic libatomic.so.1)
-    if (ATOMIC)
+    if(ATOMIC)
         message(STATUS "Found libatomic: ${ATOMIC}")
     else()
         message(STATUS "libatomic not found. If build fails, install libatomic")
@@ -112,10 +126,13 @@
 
 generate_resources(src/main/native/resources generated/main/cpp WPI wpi wpiutil_resources_src)
 
-file(GLOB_RECURSE wpiutil_native_src src/main/native/cpp/*.cpp
-                                     src/main/native/thirdparty/json/cpp/*.cpp
-                                     src/main/native/thirdparty/llvm/cpp/*.cpp
-                                     src/main/native/thirdparty/mpack/src/*.cpp)
+file(
+    GLOB_RECURSE wpiutil_native_src
+    src/main/native/cpp/*.cpp
+    src/main/native/thirdparty/json/cpp/*.cpp
+    src/main/native/thirdparty/llvm/cpp/*.cpp
+    src/main/native/thirdparty/mpack/src/*.cpp
+)
 list(REMOVE_ITEM wpiutil_native_src ${wpiutil_jni_src})
 file(GLOB_RECURSE wpiutil_unix_src src/main/native/unix/*.cpp)
 file(GLOB_RECURSE wpiutil_linux_src src/main/native/linux/*.cpp)
@@ -131,24 +148,32 @@
 set_property(TARGET wpiutil PROPERTY FOLDER "libraries")
 
 target_compile_features(wpiutil PUBLIC cxx_std_20)
-if (MSVC)
-    target_compile_options(wpiutil PUBLIC /permissive- /Zc:preprocessor /Zc:throwingNew /MP /bigobj)
+if(MSVC)
+    target_compile_options(
+        wpiutil
+        PUBLIC /permissive- /Zc:preprocessor /Zc:throwingNew /MP /bigobj
+    )
     target_compile_definitions(wpiutil PRIVATE -D_CRT_SECURE_NO_WARNINGS)
 endif()
 wpilib_target_warnings(wpiutil)
 target_link_libraries(wpiutil protobuf::libprotobuf Threads::Threads ${CMAKE_DL_LIBS})
 
-if (ATOMIC)
+if(ATOMIC)
     target_link_libraries(wpiutil ${ATOMIC})
 endif()
 
-
-if (NOT USE_SYSTEM_FMTLIB)
+if(NOT USE_SYSTEM_FMTLIB)
     target_sources(wpiutil PRIVATE ${fmtlib_native_src})
-    install(DIRECTORY src/main/native/thirdparty/fmtlib/include/ DESTINATION "${include_dest}/wpiutil")
-    target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/fmtlib/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+    install(
+        DIRECTORY src/main/native/thirdparty/fmtlib/include/
+        DESTINATION "${include_dest}/wpiutil"
+    )
+    target_include_directories(
+        wpiutil
+        PUBLIC
+            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/fmtlib/include>
+            $<INSTALL_INTERFACE:${include_dest}/wpiutil>
+    )
 else()
     find_package(fmt CONFIG REQUIRED)
     target_link_libraries(wpiutil fmt::fmt)
@@ -161,11 +186,11 @@
     endif()
 endif()
 
-if (MSVC)
+if(MSVC)
     target_sources(wpiutil PRIVATE ${wpiutil_windows_src})
-else ()
+else()
     target_sources(wpiutil PRIVATE ${wpiutil_unix_src})
-    if (APPLE)
+    if(APPLE)
         target_sources(wpiutil PRIVATE ${wpiutil_macos_src})
     else()
         target_sources(wpiutil PRIVATE ${wpiutil_linux_src})
@@ -173,47 +198,59 @@
 endif()
 
 install(DIRECTORY src/main/native/thirdparty/memory/include/ DESTINATION "${include_dest}/wpiutil")
-target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/memory/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+target_include_directories(
+    wpiutil
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/memory/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpiutil>
+)
 
 install(DIRECTORY src/main/native/thirdparty/json/include/ DESTINATION "${include_dest}/wpiutil")
-target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/json/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+target_include_directories(
+    wpiutil
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/json/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpiutil>
+)
 
 install(DIRECTORY src/main/native/thirdparty/llvm/include/ DESTINATION "${include_dest}/wpiutil")
-target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/llvm/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+target_include_directories(
+    wpiutil
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/llvm/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpiutil>
+)
 
 install(DIRECTORY src/main/native/thirdparty/mpack/include/ DESTINATION "${include_dest}/wpiutil")
-target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/mpack/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+target_include_directories(
+    wpiutil
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/mpack/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpiutil>
+)
 
 install(DIRECTORY src/main/native/thirdparty/sigslot/include/ DESTINATION "${include_dest}/wpiutil")
-target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/sigslot/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+target_include_directories(
+    wpiutil
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/sigslot/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpiutil>
+)
 
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpiutil")
-target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+target_include_directories(
+    wpiutil
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/wpiutil>
+)
 
 install(TARGETS wpiutil EXPORT wpiutil)
 
-if (WITH_FLAT_INSTALL)
-    set (wpiutil_config_dir ${wpilib_dest})
-else()
-    set (wpiutil_config_dir share/wpiutil)
-endif()
-
-configure_file(wpiutil-config.cmake.in ${WPILIB_BINARY_DIR}/wpiutil-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/wpiutil-config.cmake DESTINATION ${wpiutil_config_dir})
-install(EXPORT wpiutil DESTINATION ${wpiutil_config_dir})
+configure_file(wpiutil-config.cmake.in ${WPILIB_BINARY_DIR}/wpiutil-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/wpiutil-config.cmake DESTINATION share/wpiutil)
+install(EXPORT wpiutil DESTINATION share/wpiutil)
 
 subdir_list(wpiutil_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
 foreach(example ${wpiutil_examples})
@@ -226,7 +263,7 @@
     endif()
 endforeach()
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     file(GLOB_RECURSE wpiutil_testlib_src src/test/native/include/*.h)
     add_library(wpiutil_testlib INTERFACE ${wpiutil_test_src})
     target_include_directories(wpiutil_testlib INTERFACE src/test/native/include)
diff --git a/third_party/allwpilib/wpiutil/build.gradle b/third_party/allwpilib/wpiutil/build.gradle
index ab13092..7e4cc16 100644
--- a/third_party/allwpilib/wpiutil/build.gradle
+++ b/third_party/allwpilib/wpiutil/build.gradle
@@ -147,12 +147,6 @@
             }
         }
     }
-
-    exeSplitSetup = {
-        if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-            nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
-        }
-    }
 }
 
 def examplesMap = [:];
@@ -252,9 +246,6 @@
                 targetBuildTypes 'debug'
                 binaries.all {
                     lib library: 'wpiutil', linkage: 'shared'
-                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
-                    }
                 }
                 sources {
                     cpp {
@@ -275,7 +266,7 @@
             if (!(it instanceof NativeBinarySpec)) return
                 if (it.component.name != 'wpiutil' && it.component.name != 'wpiutilBase') return
                 if (it.targetPlatform.name != nativeUtils.wpi.platforms.roborio) return
-                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries')
+                nativeUtils.useRequiredLibrary(it, 'chipobject_headers')
         }
     }
 }
@@ -294,7 +285,7 @@
     api "com.fasterxml.jackson.core:jackson-annotations:2.15.2"
     api "com.fasterxml.jackson.core:jackson-core:2.15.2"
     api "com.fasterxml.jackson.core:jackson-databind:2.15.2"
-    api 'us.hebi.quickbuf:quickbuf-runtime:1.3.2'
+    api 'us.hebi.quickbuf:quickbuf-runtime:1.3.3'
 
     printlogImplementation sourceSets.main.output
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java
index 729c8b1..ff68d4d 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java
@@ -4,11 +4,13 @@
 
 package edu.wpi.first.util;
 
-import java.util.Arrays;
-
-/** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */
-public class CircularBuffer {
-  private double[] m_data;
+/**
+ * This is a simple circular buffer so we don't need to "bucket brigade" copy old values.
+ *
+ * @param <T> Buffer element type.
+ */
+public class CircularBuffer<T> {
+  private T[] m_data;
 
   // Index of element at front of buffer
   private int m_front;
@@ -19,11 +21,11 @@
   /**
    * Create a CircularBuffer with the provided size.
    *
-   * @param size The size of the circular buffer.
+   * @param size Maximum number of buffer elements.
    */
+  @SuppressWarnings("unchecked")
   public CircularBuffer(int size) {
-    m_data = new double[size];
-    Arrays.fill(m_data, 0.0);
+    m_data = (T[]) new Object[size];
   }
 
   /**
@@ -40,7 +42,7 @@
    *
    * @return value at front of buffer
    */
-  public double getFirst() {
+  public T getFirst() {
     return m_data[m_front];
   }
 
@@ -48,11 +50,14 @@
    * Get value at back of buffer.
    *
    * @return value at back of buffer
+   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
+   *     size())
    */
-  public double getLast() {
+  @SuppressWarnings("unchecked")
+  public T getLast() {
     // If there are no elements in the buffer, do nothing
     if (m_length == 0) {
-      return 0.0;
+      throw new IndexOutOfBoundsException("getLast() called on an empty container");
     }
 
     return m_data[(m_front + m_length - 1) % m_data.length];
@@ -64,7 +69,7 @@
    *
    * @param value The value to push.
    */
-  public void addFirst(double value) {
+  public void addFirst(T value) {
     if (m_data.length == 0) {
       return;
     }
@@ -84,7 +89,7 @@
    *
    * @param value The value to push.
    */
-  public void addLast(double value) {
+  public void addLast(T value) {
     if (m_data.length == 0) {
       return;
     }
@@ -103,14 +108,17 @@
    * Pop value at front of buffer.
    *
    * @return value at front of buffer
+   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
+   *     size())
    */
-  public double removeFirst() {
+  @SuppressWarnings("unchecked")
+  public T removeFirst() {
     // If there are no elements in the buffer, do nothing
     if (m_length == 0) {
-      return 0.0;
+      throw new IndexOutOfBoundsException("removeFirst() called on an empty container");
     }
 
-    double temp = m_data[m_front];
+    T temp = m_data[m_front];
     m_front = moduloInc(m_front);
     m_length--;
     return temp;
@@ -120,11 +128,14 @@
    * Pop value at back of buffer.
    *
    * @return value at back of buffer
+   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
+   *     size())
    */
-  public double removeLast() {
+  @SuppressWarnings("unchecked")
+  public T removeLast() {
     // If there are no elements in the buffer, do nothing
     if (m_length == 0) {
-      return 0.0;
+      throw new IndexOutOfBoundsException("removeLast() called on an empty container");
     }
 
     m_length--;
@@ -138,8 +149,9 @@
    *
    * @param size New buffer size.
    */
+  @SuppressWarnings("unchecked")
   public void resize(int size) {
-    double[] newBuffer = new double[size];
+    var newBuffer = (T[]) new Object[size];
     m_length = Math.min(m_length, size);
     for (int i = 0; i < m_length; i++) {
       newBuffer[i] = m_data[(m_front + i) % m_data.length];
@@ -150,7 +162,6 @@
 
   /** Sets internal buffer contents to zero. */
   public void clear() {
-    Arrays.fill(m_data, 0.0);
     m_front = 0;
     m_length = 0;
   }
@@ -161,23 +172,25 @@
    * @param index Index into the buffer.
    * @return Element at index starting from front of buffer.
    */
-  public double get(int index) {
+  public T get(int index) {
     return m_data[(m_front + index) % m_data.length];
   }
 
   /**
-   * Increment an index modulo the length of the m_data buffer.
+   * Increment an index modulo the length of the buffer.
    *
    * @param index Index into the buffer.
+   * @return The incremented index.
    */
   private int moduloInc(int index) {
     return (index + 1) % m_data.length;
   }
 
   /**
-   * Decrement an index modulo the length of the m_data buffer.
+   * Decrement an index modulo the length of the buffer.
    *
    * @param index Index into the buffer.
+   * @return The decremented index.
    */
   private int moduloDec(int index) {
     if (index == 0) {
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CombinedRuntimeLoader.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CombinedRuntimeLoader.java
index 09e739d..8fe4839 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CombinedRuntimeLoader.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CombinedRuntimeLoader.java
@@ -16,11 +16,17 @@
 import java.util.Map;
 import java.util.Objects;
 
+/** Loads dynamic libraries for all platforms. */
 public final class CombinedRuntimeLoader {
   private CombinedRuntimeLoader() {}
 
   private static String extractionDirectory;
 
+  /**
+   * Returns library extraction directory.
+   *
+   * @return Library extraction directory.
+   */
   public static synchronized String getExtractionDirectory() {
     return extractionDirectory;
   }
@@ -29,6 +35,12 @@
     extractionDirectory = directory;
   }
 
+  /**
+   * Sets DLL directory.
+   *
+   * @param directory Directory.
+   * @return DLL directory.
+   */
   public static native String setDllDirectory(String directory);
 
   private static String getLoadErrorMessage(String libraryName, UnsatisfiedLinkError ule) {
@@ -59,8 +71,7 @@
   @SuppressWarnings("unchecked")
   public static <T> List<String> extractLibraries(Class<T> clazz, String resourceName)
       throws IOException {
-    TypeReference<HashMap<String, Object>> typeRef =
-        new TypeReference<HashMap<String, Object>>() {};
+    TypeReference<HashMap<String, Object>> typeRef = new TypeReference<>() {};
     ObjectMapper mapper = new ObjectMapper();
     Map<String, Object> map;
     try (var stream = clazz.getResourceAsStream(resourceName)) {
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java
new file mode 100644
index 0000000..548f14b
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java
@@ -0,0 +1,191 @@
+// 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.
+
+package edu.wpi.first.util;
+
+import java.util.Arrays;
+
+/** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */
+public class DoubleCircularBuffer {
+  private double[] m_data;
+
+  // Index of element at front of buffer
+  private int m_front;
+
+  // Number of elements used in buffer
+  private int m_length;
+
+  /**
+   * Create a CircularBuffer with the provided size.
+   *
+   * @param size The size of the circular buffer.
+   */
+  public DoubleCircularBuffer(int size) {
+    m_data = new double[size];
+    Arrays.fill(m_data, 0.0);
+  }
+
+  /**
+   * Returns number of elements in buffer.
+   *
+   * @return number of elements in buffer
+   */
+  public int size() {
+    return m_length;
+  }
+
+  /**
+   * Get value at front of buffer.
+   *
+   * @return value at front of buffer
+   */
+  public double getFirst() {
+    return m_data[m_front];
+  }
+
+  /**
+   * Get value at back of buffer.
+   *
+   * @return value at back of buffer
+   */
+  public double getLast() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    return m_data[(m_front + m_length - 1) % m_data.length];
+  }
+
+  /**
+   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
+   * full.
+   *
+   * @param value The value to push.
+   */
+  public void addFirst(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_front = moduloDec(m_front);
+
+    m_data[m_front] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
+   * full.
+   *
+   * @param value The value to push.
+   */
+  public void addLast(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_data[(m_front + m_length) % m_data.length] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = moduloInc(m_front);
+    }
+  }
+
+  /**
+   * Pop value at front of buffer.
+   *
+   * @return value at front of buffer
+   */
+  public double removeFirst() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    double temp = m_data[m_front];
+    m_front = moduloInc(m_front);
+    m_length--;
+    return temp;
+  }
+
+  /**
+   * Pop value at back of buffer.
+   *
+   * @return value at back of buffer
+   */
+  public double removeLast() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    m_length--;
+    return m_data[(m_front + m_length) % m_data.length];
+  }
+
+  /**
+   * Resizes internal buffer to given size.
+   *
+   * <p>A new buffer is allocated because arrays are not resizable.
+   *
+   * @param size New buffer size.
+   */
+  public void resize(int size) {
+    double[] newBuffer = new double[size];
+    m_length = Math.min(m_length, size);
+    for (int i = 0; i < m_length; i++) {
+      newBuffer[i] = m_data[(m_front + i) % m_data.length];
+    }
+    m_data = newBuffer;
+    m_front = 0;
+  }
+
+  /** Sets internal buffer contents to zero. */
+  public void clear() {
+    Arrays.fill(m_data, 0.0);
+    m_front = 0;
+    m_length = 0;
+  }
+
+  /**
+   * Get the element at the provided index relative to the start of the buffer.
+   *
+   * @param index Index into the buffer.
+   * @return Element at index starting from front of buffer.
+   */
+  public double get(int index) {
+    return m_data[(m_front + index) % m_data.length];
+  }
+
+  /**
+   * Increment an index modulo the length of the buffer.
+   *
+   * @param index Index into the buffer.
+   * @return The incremented index.
+   */
+  private int moduloInc(int index) {
+    return (index + 1) % m_data.length;
+  }
+
+  /**
+   * Decrement an index modulo the length of the buffer.
+   *
+   * @param index Index into the buffer.
+   * @return The decremented index.
+   */
+  private int moduloDec(int index) {
+    if (index == 0) {
+      return m_data.length - 1;
+    } else {
+      return index - 1;
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java
index 4d2c800..60fb5d1 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java
@@ -8,10 +8,14 @@
 import java.util.List;
 import java.util.concurrent.locks.ReentrantLock;
 
+/** A thread-safe container for handling events. */
 public class EventVector {
   private final ReentrantLock m_lock = new ReentrantLock();
   private final List<Integer> m_events = new ArrayList<>();
 
+  /** Default constructor. */
+  public EventVector() {}
+
   /**
    * Adds an event to the event vector.
    *
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
index 2c54d00..8efe917 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
@@ -10,12 +10,17 @@
  * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
  * from points that are defined. This uses linear interpolation.
  *
+ * @param <K> Key type.
+ * @param <V> Value type.
  * @deprecated Use {@link edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap} instead
  */
 @Deprecated(forRemoval = true, since = "2024")
 public class InterpolatingTreeMap<K extends Number, V extends Number> {
   private final TreeMap<K, V> m_map = new TreeMap<>();
 
+  /** Default constructor. */
+  public InterpolatingTreeMap() {}
+
   /**
    * Inserts a key-value pair.
    *
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/PixelFormat.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/PixelFormat.java
new file mode 100644
index 0000000..809bfd0
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/PixelFormat.java
@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.util;
+
+/** Image pixel format. */
+public enum PixelFormat {
+  /** Unknown format. */
+  kUnknown(0),
+  /** Motion-JPEG (compressed image data). */
+  kMJPEG(1),
+  /** YUY 4:2:2, 16 bpp. */
+  kYUYV(2),
+  /** RGB 5-6-5, 16 bpp. */
+  kRGB565(3),
+  /** BGR 8-8-8, 24 bpp. */
+  kBGR(4),
+  /** Grayscale, 8 bpp. */
+  kGray(5),
+  /** Grayscale, 16 bpp. */
+  kY16(6),
+  /** YUV 4:2:2, 16 bpp. */
+  kUYVY(7);
+
+  private final int value;
+
+  PixelFormat(int value) {
+    this.value = value;
+  }
+
+  /**
+   * Gets the integer value of the pixel format.
+   *
+   * @return Integer value
+   */
+  public int getValue() {
+    return value;
+  }
+
+  private static final PixelFormat[] s_values = values();
+
+  /**
+   * Gets a PixelFormat enum value from its integer value.
+   *
+   * @param pixelFormat integer value
+   * @return Enum value
+   */
+  public static PixelFormat getFromInt(int pixelFormat) {
+    return s_values[pixelFormat];
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java
new file mode 100644
index 0000000..dd074bf
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java
@@ -0,0 +1,188 @@
+// 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.
+
+package edu.wpi.first.util;
+
+import java.nio.ByteBuffer;
+
+/**
+ * Class for storing raw frame data between image read call.
+ *
+ * <p>Data is reused for each frame read, rather then reallocating every frame.
+ */
+public class RawFrame implements AutoCloseable {
+  private long m_nativeObj;
+  private ByteBuffer m_data;
+  private int m_width;
+  private int m_height;
+  private int m_stride;
+  private PixelFormat m_pixelFormat = PixelFormat.kUnknown;
+
+  /** Construct a new empty RawFrame. */
+  public RawFrame() {
+    m_nativeObj = WPIUtilJNI.allocateRawFrame();
+  }
+
+  /**
+   * Close the RawFrame, releasing native resources. Any images currently using the data will be
+   * invalidated.
+   */
+  @Override
+  public void close() {
+    WPIUtilJNI.freeRawFrame(m_nativeObj);
+    m_nativeObj = 0;
+  }
+
+  /**
+   * Called from JNI to set data in class.
+   *
+   * @param data A native ByteBuffer pointing to the frame data.
+   * @param width The width of the frame, in pixels
+   * @param height The height of the frame, in pixels
+   * @param stride The number of bytes in each row of image data
+   * @param pixelFormat The PixelFormat of the frame
+   */
+  void setDataJNI(ByteBuffer data, int width, int height, int stride, int pixelFormat) {
+    m_data = data;
+    m_width = width;
+    m_height = height;
+    m_stride = stride;
+    m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
+  }
+
+  /**
+   * Called from JNI to set info in class.
+   *
+   * @param width The width of the frame, in pixels
+   * @param height The height of the frame, in pixels
+   * @param stride The number of bytes in each row of image data
+   * @param pixelFormat The PixelFormat of the frame
+   */
+  void setInfoJNI(int width, int height, int stride, int pixelFormat) {
+    m_width = width;
+    m_height = height;
+    m_stride = stride;
+    m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
+  }
+
+  /**
+   * Set frame data.
+   *
+   * @param data A native ByteBuffer pointing to the frame data.
+   * @param width The width of the frame, in pixels
+   * @param height The height of the frame, in pixels
+   * @param stride The number of bytes in each row of image data
+   * @param pixelFormat The PixelFormat of the frame
+   */
+  public void setData(ByteBuffer data, int width, int height, int stride, PixelFormat pixelFormat) {
+    if (!data.isDirect()) {
+      throw new UnsupportedOperationException("ByteBuffer must be direct");
+    }
+    m_data = data;
+    m_width = width;
+    m_height = height;
+    m_stride = stride;
+    m_pixelFormat = pixelFormat;
+    WPIUtilJNI.setRawFrameData(
+        m_nativeObj, data, data.limit(), width, height, stride, pixelFormat.getValue());
+  }
+
+  /**
+   * Call to set frame information.
+   *
+   * @param width The width of the frame, in pixels
+   * @param height The height of the frame, in pixels
+   * @param stride The number of bytes in each row of image data
+   * @param pixelFormat The PixelFormat of the frame
+   */
+  public void setInfo(int width, int height, int stride, PixelFormat pixelFormat) {
+    m_width = width;
+    m_height = height;
+    m_stride = stride;
+    m_pixelFormat = pixelFormat;
+    WPIUtilJNI.setRawFrameInfo(
+        m_nativeObj,
+        m_data != null ? m_data.limit() : 0,
+        width,
+        height,
+        stride,
+        pixelFormat.getValue());
+  }
+
+  /**
+   * Get the pointer to native representation of this frame.
+   *
+   * @return The pointer to native representation of this frame.
+   */
+  public long getNativeObj() {
+    return m_nativeObj;
+  }
+
+  /**
+   * Get a ByteBuffer pointing to the frame data. This ByteBuffer is backed by the frame directly.
+   * Its lifetime is controlled by the frame. If a new frame gets read, it will overwrite the
+   * current one.
+   *
+   * @return A ByteBuffer pointing to the frame data.
+   */
+  public ByteBuffer getData() {
+    return m_data;
+  }
+
+  /**
+   * Get a long (is a uint8_t* in native code) pointing to the frame data. This pointer is backed by
+   * the frame directly. Its lifetime is controlled by the frame. If a new frame gets read, it will
+   * overwrite the current one.
+   *
+   * @return A long pointing to the frame data.
+   */
+  public long getDataPtr() {
+    return WPIUtilJNI.getRawFrameDataPtr(m_nativeObj);
+  }
+
+  /**
+   * Get the total size of the data stored in the frame, in bytes.
+   *
+   * @return The total size of the data stored in the frame.
+   */
+  public int getSize() {
+    return m_data != null ? m_data.limit() : 0;
+  }
+
+  /**
+   * Get the width of the image.
+   *
+   * @return The width of the image, in pixels.
+   */
+  public int getWidth() {
+    return m_width;
+  }
+
+  /**
+   * Get the height of the image.
+   *
+   * @return The height of the image, in pixels.
+   */
+  public int getHeight() {
+    return m_height;
+  }
+
+  /**
+   * Get the number of bytes in each row of image data.
+   *
+   * @return The image data stride, in bytes.
+   */
+  public int getStride() {
+    return m_stride;
+  }
+
+  /**
+   * Get the PixelFormat of the frame.
+   *
+   * @return The PixelFormat of the frame.
+   */
+  public PixelFormat getPixelFormat() {
+    return m_pixelFormat;
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeDetector.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeDetector.java
index 550339e..72593dc 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeDetector.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeDetector.java
@@ -6,6 +6,9 @@
 
 import java.io.File;
 
+/**
+ * A utility class for detecting and providing platform-specific such as OS and CPU architecture.
+ */
 public final class RuntimeDetector {
   private static String filePrefix;
   private static String fileExtension;
@@ -131,32 +134,57 @@
   }
 
   /**
-   * check if architecture is Arm64.
+   * Check if architecture is Arm64.
    *
-   * @return if architecture is Arm64
+   * @return if architecture is Arm64.
    */
   public static boolean isArm64() {
     String arch = System.getProperty("os.arch");
     return "aarch64".equals(arch) || "arm64".equals(arch);
   }
 
+  /**
+   * Check if OS is Linux.
+   *
+   * @return if OS is Linux.
+   */
   public static boolean isLinux() {
     return System.getProperty("os.name").startsWith("Linux");
   }
 
+  /**
+   * Check if OS is Windows.
+   *
+   * @return if OS is Windows.
+   */
   public static boolean isWindows() {
     return System.getProperty("os.name").startsWith("Windows");
   }
 
+  /**
+   * Check if OS is Mac.
+   *
+   * @return if OS is Mac.
+   */
   public static boolean isMac() {
     return System.getProperty("os.name").startsWith("Mac");
   }
 
+  /**
+   * Check if OS is 32bit Intel.
+   *
+   * @return if OS is 32bit Intel.
+   */
   public static boolean is32BitIntel() {
     String arch = System.getProperty("os.arch");
     return "x86".equals(arch) || "i386".equals(arch);
   }
 
+  /**
+   * Check if OS is 64bit Intel.
+   *
+   * @return if OS is 64bit Intel.
+   */
   public static boolean is64BitIntel() {
     String arch = System.getProperty("os.arch");
     return "amd64".equals(arch) || "x86_64".equals(arch);
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java
index f24ace3..474666e 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java
@@ -17,6 +17,11 @@
 import java.util.Locale;
 import java.util.Scanner;
 
+/**
+ * Loads a native library at runtime.
+ *
+ * @param <T> The class to load.
+ */
 public final class RuntimeLoader<T> {
   private static String defaultExtractionRoot;
 
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPISerializable.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPISerializable.java
new file mode 100644
index 0000000..200deb5
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPISerializable.java
@@ -0,0 +1,8 @@
+// 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.
+
+package edu.wpi.first.util;
+
+/** Marker interface to indicate a class is serializable using WPI serialization methods. */
+public interface WPISerializable {}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
index 9929b48..0bd5b21 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
@@ -5,22 +5,38 @@
 package edu.wpi.first.util;
 
 import java.io.IOException;
+import java.nio.ByteBuffer;
 import java.util.concurrent.atomic.AtomicBoolean;
 
+/** WPIUtil JNI. */
 public class WPIUtilJNI {
   static boolean libraryLoaded = false;
   static RuntimeLoader<WPIUtilJNI> loader = null;
 
+  /** Sets whether JNI should be loaded in the static block. */
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
     public static boolean getExtractOnStaticLoad() {
       return extractOnStaticLoad.get();
     }
 
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
     public static void setExtractOnStaticLoad(boolean load) {
       extractOnStaticLoad.set(load);
     }
+
+    /** Utility class. */
+    private Helper() {}
   }
 
   static {
@@ -54,32 +70,115 @@
     libraryLoaded = true;
   }
 
+  /**
+   * Write the given string to stderr.
+   *
+   * @param str String to write.
+   */
   public static native void writeStderr(String str);
 
+  /** Enable mock time. */
   public static native void enableMockTime();
 
+  /** Disable mock time. */
   public static native void disableMockTime();
 
+  /**
+   * Set mock time.
+   *
+   * @param time The desired time in microseconds.
+   */
   public static native void setMockTime(long time);
 
+  /**
+   * Returns the time.
+   *
+   * @return The time.
+   */
   public static native long now();
 
+  /**
+   * Returns the system time.
+   *
+   * @return The system time.
+   */
   public static native long getSystemTime();
 
+  /**
+   * Creates an event. Events have binary state (signaled or not signaled) and may be either
+   * automatically reset or manually reset. Automatic-reset events go to non-signaled state when a
+   * WaitForObject is woken up by the event; manual-reset events require ResetEvent() to be called
+   * to set the event to non-signaled state; if ResetEvent() is not called, any waiter on that event
+   * will immediately wake when called.
+   *
+   * @param manualReset true for manual reset, false for automatic reset
+   * @param initialState true to make the event initially in signaled state
+   * @return Event handle
+   */
   public static native int createEvent(boolean manualReset, boolean initialState);
 
+  /**
+   * Destroys an event. Destruction wakes up any waiters.
+   *
+   * @param eventHandle event handle
+   */
   public static native void destroyEvent(int eventHandle);
 
+  /**
+   * Sets an event to signaled state.
+   *
+   * @param eventHandle event handle
+   */
   public static native void setEvent(int eventHandle);
 
+  /**
+   * Sets an event to non-signaled state.
+   *
+   * @param eventHandle event handle
+   */
   public static native void resetEvent(int eventHandle);
 
+  /**
+   * Creates a semaphore. Semaphores keep an internal counter. Releasing the semaphore increases the
+   * count. A semaphore with a non-zero count is considered signaled. When a waiter wakes up it
+   * atomically decrements the count by 1. This is generally useful in a single-supplier,
+   * multiple-consumer scenario.
+   *
+   * @param initialCount initial value for the semaphore's internal counter
+   * @param maximumCount maximum value for the samephore's internal counter
+   * @return Semaphore handle
+   */
   public static native int createSemaphore(int initialCount, int maximumCount);
 
+  /**
+   * Destroys a semaphore. Destruction wakes up any waiters.
+   *
+   * @param semHandle semaphore handle
+   */
   public static native void destroySemaphore(int semHandle);
 
+  /**
+   * Releases N counts of a semaphore.
+   *
+   * @param semHandle semaphore handle
+   * @param releaseCount amount to add to semaphore's internal counter; must be positive
+   * @return True on successful release, false on failure (e.g. release count would exceed maximum
+   *     value, or handle invalid)
+   */
   public static native boolean releaseSemaphore(int semHandle, int releaseCount);
 
+  static native long allocateRawFrame();
+
+  static native void freeRawFrame(long frame);
+
+  static native long getRawFrameDataPtr(long frame);
+
+  static native void setRawFrameData(
+      long frame, ByteBuffer data, int size, int width, int height, int stride, int pixelFormat);
+
+  static native void setRawFrameInfo(
+      long frame, int size, int width, int height, int stride, int pixelFormat);
+
   /**
    * Waits for a handle to be signaled.
    *
@@ -124,4 +223,7 @@
    */
   public static native int[] waitForObjectsTimeout(int[] handles, double timeout)
       throws InterruptedException;
+
+  /** Utility class. */
+  protected WPIUtilJNI() {}
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/CleanupPool.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/CleanupPool.java
index fab8316..4b8c211 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/CleanupPool.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/CleanupPool.java
@@ -14,7 +14,10 @@
 public class CleanupPool implements AutoCloseable {
   // Use a Deque instead of a Stack, as Stack's iterators go the wrong way, and docs
   // state ArrayDeque is faster anyway.
-  private final Deque<AutoCloseable> m_closers = new ArrayDeque<AutoCloseable>();
+  private final Deque<AutoCloseable> m_closers = new ArrayDeque<>();
+
+  /** Default constructor. */
+  public CleanupPool() {}
 
   /**
    * Registers an object in the object stack for cleanup.
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/SkipCleanup.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/SkipCleanup.java
index e2bc72e..fcbcb7e 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/SkipCleanup.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/cleanup/SkipCleanup.java
@@ -9,6 +9,7 @@
 import java.lang.annotation.RetentionPolicy;
 import java.lang.annotation.Target;
 
+/** Attribute for telling JVM to skip object cleanup. */
 @Target(ElementType.FIELD)
 @Retention(RetentionPolicy.RUNTIME)
 public @interface SkipCleanup {}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanArrayLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanArrayLogEntry.java
index 718d460..21ac596 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanArrayLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanArrayLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log array of boolean values. */
 public class BooleanArrayLogEntry extends DataLogEntry {
+  /** The data type for boolean array values. */
   public static final String kDataType = "boolean[]";
 
+  /**
+   * Constructs a boolean array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public BooleanArrayLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a boolean array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public BooleanArrayLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a boolean array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public BooleanArrayLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a boolean array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public BooleanArrayLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanLogEntry.java
index 503b83b..c413bfa 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/BooleanLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log boolean values. */
 public class BooleanLogEntry extends DataLogEntry {
+  /** The data type for boolean values. */
   public static final String kDataType = "boolean";
 
+  /**
+   * Constructs a boolean log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public BooleanLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a boolean log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public BooleanLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a boolean log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public BooleanLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a boolean log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public BooleanLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java
index 97c629f..f48b081 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java
@@ -494,6 +494,11 @@
     DataLogJNI.appendStringArray(m_impl, entry, arr, timestamp);
   }
 
+  /**
+   * Gets the JNI implementation handle.
+   *
+   * @return data log handle.
+   */
   public long getImpl() {
     return m_impl;
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogEntry.java
index 4beaff2..8502428 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogEntry.java
@@ -6,15 +6,39 @@
 
 /** Log entry base class. */
 public class DataLogEntry {
+  /**
+   * Constructs a data log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param type Data type
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   protected DataLogEntry(DataLog log, String name, String type, String metadata, long timestamp) {
     m_log = log;
     m_entry = log.start(name, type, metadata, timestamp);
   }
 
+  /**
+   * Constructs a data log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param type Data type
+   * @param metadata metadata
+   */
   protected DataLogEntry(DataLog log, String name, String type, String metadata) {
     this(log, name, type, metadata, 0);
   }
 
+  /**
+   * Constructs a data log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param type Data type
+   */
   protected DataLogEntry(DataLog log, String name, String type) {
     this(log, name, type, "");
   }
@@ -52,6 +76,9 @@
     finish(0);
   }
 
+  /** The data log instance associated with the entry. */
   protected final DataLog m_log;
+
+  /** The data log entry index. */
   protected final int m_entry;
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java
index f94a86f..c764339 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java
@@ -7,35 +7,145 @@
 import edu.wpi.first.util.WPIUtilJNI;
 import java.nio.ByteBuffer;
 
+/**
+ * DataLog wpiutil JNI Functions.
+ *
+ * @see "wpiutil/DataLog.h"
+ */
 public class DataLogJNI extends WPIUtilJNI {
+  /**
+   * Create a new Data Log. The log will be initially created with a temporary filename.
+   *
+   * @param dir directory to store the log
+   * @param filename filename to use; if none provided, a random filename is generated of the form
+   *     "wpilog_{}.wpilog"
+   * @param period time between automatic flushes to disk, in seconds; this is a time/storage
+   *     tradeoff
+   * @param extraHeader extra header data
+   * @return data log implementation handle
+   */
   static native long create(String dir, String filename, double period, String extraHeader);
 
+  /**
+   * Change log filename.
+   *
+   * @param impl data log implementation handle
+   * @param filename filename
+   */
   static native void setFilename(long impl, String filename);
 
+  /**
+   * Explicitly flushes the log data to disk.
+   *
+   * @param impl data log implementation handle
+   */
   static native void flush(long impl);
 
+  /**
+   * Pauses appending of data records to the log. While paused, no data records are saved (e.g.
+   * AppendX is a no-op). Has no effect on entry starts / finishes / metadata changes.
+   *
+   * @param impl data log implementation handle
+   */
   static native void pause(long impl);
 
+  /**
+   * Resumes appending of data records to the log. If called after Stop(), opens a new file (with
+   * random name if SetFilename was not called after Stop()) and appends Start records and schema
+   * data values for all previously started entries and schemas.
+   *
+   * @param impl data log implementation handle
+   */
   static native void resume(long impl);
 
+  /**
+   * Stops appending all records to the log, and closes the log file.
+   *
+   * @param impl data log implementation handle
+   */
   static native void stop(long impl);
 
+  /**
+   * Registers a data schema. Data schemas provide information for how a certain data type string
+   * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g.
+   * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In the data log, schemas
+   * are saved just like normal records, with the name being generated from the provided name:
+   * "/.schema/&lt;name&gt;". Duplicate calls to this function with the same name are silently
+   * ignored.
+   *
+   * @param impl data log implementation handle
+   * @param name Name (the string passed as the data type for records using this schema)
+   * @param type Type of schema (e.g. "protobuf", "struct", etc)
+   * @param schema Schema data
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void addSchema(long impl, String name, String type, byte[] schema, long timestamp);
 
   static native void addSchemaString(
       long impl, String name, String type, String schema, long timestamp);
 
+  /**
+   * Start an entry. Duplicate names are allowed (with the same type), and result in the same index
+   * being returned (Start/Finish are reference counted). A duplicate name with a different type
+   * will result in an error message being printed to the console and 0 being returned (which will
+   * be ignored by the Append functions).
+   *
+   * @param impl data log implementation handle
+   * @param name Name
+   * @param type Data type
+   * @param metadata Initial metadata (e.g. data properties)
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   * @return Entry index
+   */
   static native int start(long impl, String name, String type, String metadata, long timestamp);
 
+  /**
+   * Finish an entry.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void finish(long impl, int entry, long timestamp);
 
+  /**
+   * Updates the metadata for an entry.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index
+   * @param metadata New metadata for the entry
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void setMetadata(long impl, int entry, String metadata, long timestamp);
 
+  /**
+   * Closes the data log implementation handle.
+   *
+   * @param impl data log implementation handle
+   */
   static native void close(long impl);
 
+  /**
+   * Appends a raw record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by WPI_DataLog_Start()
+   * @param data Byte array to record
+   * @param len Length of byte array
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendRaw(
       long impl, int entry, byte[] data, int start, int len, long timestamp);
 
+  /**
+   * Appends a raw record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by WPI_DataLog_Start()
+   * @param data ByteBuffer to record
+   * @param len Length of byte array
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static void appendRaw(long impl, int entry, ByteBuffer data, int start, int len, long timestamp) {
     if (data.isDirect()) {
       if (start < 0) {
@@ -58,23 +168,106 @@
   private static native void appendRawBuffer(
       long impl, int entry, ByteBuffer data, int start, int len, long timestamp);
 
+  /**
+   * Appends a boolean record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param value Boolean value to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendBoolean(long impl, int entry, boolean value, long timestamp);
 
+  /**
+   * Appends an integer record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param value Integer value to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendInteger(long impl, int entry, long value, long timestamp);
 
+  /**
+   * Appends a float record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param value Float value to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendFloat(long impl, int entry, float value, long timestamp);
 
+  /**
+   * Appends a double record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param value Double value to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendDouble(long impl, int entry, double value, long timestamp);
 
+  /**
+   * Appends a string record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param value String value to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendString(long impl, int entry, String value, long timestamp);
 
+  /**
+   * Appends a boolean array record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param arr Boolean array to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendBooleanArray(long impl, int entry, boolean[] value, long timestamp);
 
+  /**
+   * Appends an integer array record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param arr Integer array to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendIntegerArray(long impl, int entry, long[] value, long timestamp);
 
+  /**
+   * Appends a float array record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param arr Float array to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendFloatArray(long impl, int entry, float[] value, long timestamp);
 
+  /**
+   * Appends a double array record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param arr Double array to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendDoubleArray(long impl, int entry, double[] value, long timestamp);
 
+  /**
+   * Appends a string array record to the log.
+   *
+   * @param impl data log implementation handle
+   * @param entry Entry index, as returned by Start()
+   * @param arr String array to record
+   * @param timestamp Time stamp (may be 0 to indicate now)
+   */
   static native void appendStringArray(long impl, int entry, String[] value, long timestamp);
+
+  /** Utility class. */
+  private DataLogJNI() {}
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleArrayLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleArrayLogEntry.java
index 67ef8c3..485a9c8 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleArrayLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleArrayLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log array of double values. */
 public class DoubleArrayLogEntry extends DataLogEntry {
+  /** The data type for double array values. */
   public static final String kDataType = "double[]";
 
+  /**
+   * Constructs a double array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public DoubleArrayLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a double array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public DoubleArrayLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a double array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public DoubleArrayLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a double array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public DoubleArrayLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleLogEntry.java
index f16c27e..a089df2 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/DoubleLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log double values. */
 public class DoubleLogEntry extends DataLogEntry {
+  /** The data type for double values. */
   public static final String kDataType = "double";
 
+  /**
+   * Constructs a double log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public DoubleLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a double log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public DoubleLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a double log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public DoubleLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a double log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public DoubleLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatArrayLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatArrayLogEntry.java
index 3a0b7e0..be25970 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatArrayLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatArrayLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log array of float values. */
 public class FloatArrayLogEntry extends DataLogEntry {
+  /** The data type for float array values. */
   public static final String kDataType = "float[]";
 
+  /**
+   * Constructs a float array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public FloatArrayLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a float array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public FloatArrayLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a float array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public FloatArrayLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a float array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public FloatArrayLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatLogEntry.java
index 28adc34..28f83cb 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/FloatLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log float values. */
 public class FloatLogEntry extends DataLogEntry {
+  /** The data type for float values. */
   public static final String kDataType = "float";
 
+  /**
+   * Constructs a float log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public FloatLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a float log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public FloatLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a float log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public FloatLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a float log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public FloatLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerArrayLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerArrayLogEntry.java
index 2cffc8d..d2f8f0e 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerArrayLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerArrayLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log array of integer values. */
 public class IntegerArrayLogEntry extends DataLogEntry {
+  /** The data type for integer array values. */
   public static final String kDataType = "int64[]";
 
+  /**
+   * Constructs a integer array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public IntegerArrayLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a integer array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public IntegerArrayLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a integer array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public IntegerArrayLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a integer array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public IntegerArrayLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerLogEntry.java
index 142ca5d..395a208 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/IntegerLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log integer values. */
 public class IntegerLogEntry extends DataLogEntry {
+  /** The data type for integer values. */
   public static final String kDataType = "int64";
 
+  /**
+   * Constructs a integer log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public IntegerLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a integer log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public IntegerLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a integer log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public IntegerLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a integer log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public IntegerLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java
index 1db2647..9e7fa43 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java
@@ -37,7 +37,7 @@
    */
   public static <T, MessageType extends ProtoMessage<?>> ProtobufLogEntry<T> create(
       DataLog log, String name, Protobuf<T, MessageType> proto, String metadata, long timestamp) {
-    return new ProtobufLogEntry<T>(log, name, proto, metadata, timestamp);
+    return new ProtobufLogEntry<>(log, name, proto, metadata, timestamp);
   }
 
   /**
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/RawLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/RawLogEntry.java
index 972fc03..a9e3373 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/RawLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/RawLogEntry.java
@@ -8,28 +8,74 @@
 
 /** Log raw byte array values. */
 public class RawLogEntry extends DataLogEntry {
+  /** The data type for raw values. */
   public static final String kDataType = "raw";
 
+  /**
+   * Constructs a raw log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param type Data type
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public RawLogEntry(DataLog log, String name, String metadata, String type, long timestamp) {
     super(log, name, type, metadata, timestamp);
   }
 
+  /**
+   * Constructs a raw log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param type Data type
+   */
   public RawLogEntry(DataLog log, String name, String metadata, String type) {
     this(log, name, metadata, type, 0);
   }
 
+  /**
+   * Constructs a raw log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public RawLogEntry(DataLog log, String name, String metadata, long timestamp) {
     this(log, name, metadata, kDataType, timestamp);
   }
 
+  /**
+   * Constructs a raw log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public RawLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a raw log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public RawLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a raw log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public RawLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringArrayLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringArrayLogEntry.java
index 37bdeb1..f0a6dde 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringArrayLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringArrayLogEntry.java
@@ -6,20 +6,49 @@
 
 /** Log array of string values. */
 public class StringArrayLogEntry extends DataLogEntry {
+  /** The data type for string array values. */
   public static final String kDataType = "string[]";
 
+  /**
+   * Constructs a string array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public StringArrayLogEntry(DataLog log, String name, String metadata, long timestamp) {
     super(log, name, kDataType, metadata, timestamp);
   }
 
+  /**
+   * Constructs a string array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public StringArrayLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a string array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public StringArrayLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a string array log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public StringArrayLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringLogEntry.java
index 0722dc0..27c8aef 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StringLogEntry.java
@@ -6,28 +6,74 @@
 
 /** Log string values. */
 public class StringLogEntry extends DataLogEntry {
+  /** The data type for string values. */
   public static final String kDataType = "string";
 
+  /**
+   * Constructs a String log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param type Data type
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public StringLogEntry(DataLog log, String name, String metadata, String type, long timestamp) {
     super(log, name, type, metadata, timestamp);
   }
 
+  /**
+   * Constructs a String log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param type Data type
+   */
   public StringLogEntry(DataLog log, String name, String metadata, String type) {
     this(log, name, metadata, type, 0);
   }
 
+  /**
+   * Constructs a String log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public StringLogEntry(DataLog log, String name, String metadata, long timestamp) {
     this(log, name, metadata, kDataType, timestamp);
   }
 
+  /**
+   * Constructs a String log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param metadata metadata
+   */
   public StringLogEntry(DataLog log, String name, String metadata) {
     this(log, name, metadata, 0);
   }
 
+  /**
+   * Constructs a String log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   * @param timestamp entry creation timestamp (0=now)
+   */
   public StringLogEntry(DataLog log, String name, long timestamp) {
     this(log, name, "", timestamp);
   }
 
+  /**
+   * Constructs a String log entry.
+   *
+   * @param log datalog
+   * @param name name of the entry
+   */
   public StringLogEntry(DataLog log, String name) {
     this(log, name, 0);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java
index 0f6cb2e..b3a31c9 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java
@@ -35,7 +35,7 @@
    */
   public static <T> StructArrayLogEntry<T> create(
       DataLog log, String name, Struct<T> struct, String metadata, long timestamp) {
-    return new StructArrayLogEntry<T>(log, name, struct, metadata, timestamp);
+    return new StructArrayLogEntry<>(log, name, struct, metadata, timestamp);
   }
 
   /**
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java
index a227c32..0d09182 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java
@@ -34,7 +34,7 @@
    */
   public static <T> StructLogEntry<T> create(
       DataLog log, String name, Struct<T> struct, String metadata, long timestamp) {
-    return new StructLogEntry<T>(log, name, struct, metadata, timestamp);
+    return new StructLogEntry<>(log, name, struct, metadata, timestamp);
   }
 
   /**
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java
index af3e466..1f8fdf0 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java
@@ -26,9 +26,17 @@
     m_proto = proto;
   }
 
+  /**
+   * Creates a ProtobufBuffer for the given Protobuf object.
+   *
+   * @param <T> The type to serialize.
+   * @param <MessageType> The Protobuf message type.
+   * @param proto The Protobuf object.
+   * @return A ProtobufBuffer for the given Protobuf object.
+   */
   public static <T, MessageType extends ProtoMessage<?>> ProtobufBuffer<T, MessageType> create(
       Protobuf<T, MessageType> proto) {
-    return new ProtobufBuffer<T, MessageType>(proto);
+    return new ProtobufBuffer<>(proto);
   }
 
   /**
@@ -61,7 +69,7 @@
     m_msg.clearQuick();
     m_proto.pack(m_msg, value);
     int size = m_msg.getSerializedSize();
-    if (size < m_buf.capacity()) {
+    if (size > m_buf.capacity()) {
       m_buf = ByteBuffer.allocateDirect(size * 2);
       m_sink.setOutput(m_buf);
     }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufSerializable.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufSerializable.java
new file mode 100644
index 0000000..ac75065
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufSerializable.java
@@ -0,0 +1,15 @@
+// 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.
+
+package edu.wpi.first.util.protobuf;
+
+import edu.wpi.first.util.WPISerializable;
+
+/**
+ * Marker interface to indicate a class is serializable using Protobuf serialization.
+ *
+ * <p>While this cannot be enforced by the interface, any class implementing this interface should
+ * provide a public final static `proto` member variable.
+ */
+public interface ProtobufSerializable extends WPISerializable {}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java
index db822ce..ee93725 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java
@@ -15,10 +15,14 @@
 import java.util.function.LongSupplier;
 import java.util.function.Supplier;
 
+/** Helper class for building Sendable dashboard representations. */
 public interface SendableBuilder extends AutoCloseable {
   /** The backend kinds used for the sendable builder. */
   enum BackendKind {
+    /** Unknown. */
     kUnknown,
+
+    /** NetworkTables. */
     kNetworkTables
   }
 
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableRegistry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableRegistry.java
index 6f0ad41..025b802 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableRegistry.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableRegistry.java
@@ -506,6 +506,9 @@
 
     /** Sendable builder for the sendable. */
     public SendableBuilder builder;
+
+    /** Default constructor. */
+    public CallbackData() {}
   }
 
   // As foreachLiveWindow is single threaded, cache the components it
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java
index 6ff2236..9c69e1f 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java
@@ -4,34 +4,70 @@
 
 package edu.wpi.first.util.struct;
 
+/** Exception thrown when encountering a bad schema. */
 public class BadSchemaException extends Exception {
+  /** The bad schema field. */
   private final String m_field;
 
-  public BadSchemaException(String s) {
-    super(s);
+  /**
+   * Constructs a BadSchemaException.
+   *
+   * @param message the detail message.
+   */
+  public BadSchemaException(String message) {
+    super(message);
     m_field = "";
   }
 
+  /**
+   * Constructs a BadSchemaException.
+   *
+   * @param message the detail message.
+   * @param cause The cause (which is saved for later retrieval by the {@link #getCause()} method).
+   */
   public BadSchemaException(String message, Throwable cause) {
     super(message, cause);
     m_field = "";
   }
 
+  /**
+   * Constructs a BadSchemaException.
+   *
+   * @param cause The cause (which is saved for later retrieval by the {@link #getCause()} method).
+   */
   public BadSchemaException(Throwable cause) {
     super(cause);
     m_field = "";
   }
 
-  public BadSchemaException(String field, String s) {
-    super(s);
+  /**
+   * Constructs a BadSchemaException.
+   *
+   * @param field The bad schema field.
+   * @param message the detail message.
+   */
+  public BadSchemaException(String field, String message) {
+    super(message);
     m_field = field;
   }
 
+  /**
+   * Constructs a BadSchemaException.
+   *
+   * @param field The bad schema field.
+   * @param message the detail message.
+   * @param cause The cause (which is saved for later retrieval by the {@link #getCause()} method).
+   */
   public BadSchemaException(String field, String message, Throwable cause) {
     super(message, cause);
     m_field = field;
   }
 
+  /**
+   * Gets the name of the bad schema field.
+   *
+   * @return The name of the bad schema field, or an empty string if not applicable.
+   */
   public String getField() {
     return m_field;
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java
index 165f7db..1f9fdc3 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java
@@ -593,24 +593,24 @@
       case 1:
         {
           byte val = m_data.get(field.m_offset + arrIndex);
-          val &= ~(field.getBitMask() << field.m_bitShift);
-          val |= (value & field.getBitMask()) << field.m_bitShift;
+          val &= (byte) ~(field.getBitMask() << field.m_bitShift);
+          val |= (byte) ((value & field.getBitMask()) << field.m_bitShift);
           m_data.put(field.m_offset + arrIndex, val);
           break;
         }
       case 2:
         {
           short val = m_data.getShort(field.m_offset + arrIndex * 2);
-          val &= ~(field.getBitMask() << field.m_bitShift);
-          val |= (value & field.getBitMask()) << field.m_bitShift;
+          val &= (short) ~(field.getBitMask() << field.m_bitShift);
+          val |= (short) ((value & field.getBitMask()) << field.m_bitShift);
           m_data.putShort(field.m_offset + arrIndex * 2, val);
           break;
         }
       case 4:
         {
           int val = m_data.getInt(field.m_offset + arrIndex * 4);
-          val &= ~(field.getBitMask() << field.m_bitShift);
-          val |= (value & field.getBitMask()) << field.m_bitShift;
+          val &= (int) ~(field.getBitMask() << field.m_bitShift);
+          val |= (int) ((value & field.getBitMask()) << field.m_bitShift);
           m_data.putInt(field.m_offset + arrIndex * 4, val);
           break;
         }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java
index 0e8aa18..a5d04e0 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java
@@ -12,7 +12,7 @@
 /**
  * Reusable buffer for serialization/deserialization to/from a raw struct.
  *
- * @param <T> object type
+ * @param <T> Object type.
  */
 public final class StructBuffer<T> {
   private StructBuffer(Struct<T> struct) {
@@ -21,8 +21,15 @@
     m_struct = struct;
   }
 
+  /**
+   * Returns a StructBuffer for the given struct.
+   *
+   * @param struct A struct.
+   * @param <T> Object type.
+   * @return A StructBuffer for the given struct.
+   */
   public static <T> StructBuffer<T> create(Struct<T> struct) {
-    return new StructBuffer<T>(struct);
+    return new StructBuffer<>(struct);
   }
 
   /**
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java
index be7343f..459fa9e 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java
@@ -14,6 +14,9 @@
 
 /** Database of raw struct dynamic descriptors. */
 public class StructDescriptorDatabase {
+  /** Default constructor. */
+  public StructDescriptorDatabase() {}
+
   /**
    * Adds a structure schema to the database. If the struct references other structs that have not
    * yet been added, it will not be valid until those structs are also added.
@@ -33,7 +36,7 @@
     }
 
     // turn parsed schema into descriptors
-    StructDescriptor theStruct = m_structs.computeIfAbsent(name, k -> new StructDescriptor(k));
+    StructDescriptor theStruct = m_structs.computeIfAbsent(name, StructDescriptor::new);
     theStruct.m_schema = schema;
     theStruct.m_fields.clear();
     boolean isValid = true;
@@ -76,7 +79,7 @@
 
         // cross-reference struct, creating a placeholder if necessary
         StructDescriptor aStruct =
-            m_structs.computeIfAbsent(decl.typeString, k -> new StructDescriptor(k));
+            m_structs.computeIfAbsent(decl.typeString, StructDescriptor::new);
 
         // if the struct isn't valid, we can't be valid either
         if (aStruct.isValid()) {
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java
index 28d5d8e..4b3cf17 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java
@@ -6,29 +6,46 @@
 
 /** Known data types for raw struct dynamic fields (see StructFieldDescriptor). */
 public enum StructFieldType {
+  /** bool. */
   kBool("bool", false, false, 1),
+  /** char. */
   kChar("char", false, false, 1),
+  /** int8. */
   kInt8("int8", true, false, 1),
+  /** int16. */
   kInt16("int16", true, false, 2),
+  /** int32. */
   kInt32("int32", true, false, 4),
+  /** int64. */
   kInt64("int64", true, false, 8),
+  /** uint8. */
   kUint8("uint8", false, true, 1),
+  /** uint16. */
   kUint16("uint16", false, true, 2),
+  /** uint32. */
   kUint32("uint32", false, true, 4),
+  /** uint64. */
   kUint64("uint64", false, true, 8),
+  /** float. */
   kFloat("float", false, false, 4),
+  /** double. */
   kDouble("double", false, false, 8),
+  /** struct. */
   kStruct("struct", false, false, 0);
 
+  /** The name of the data type. */
   @SuppressWarnings("MemberName")
   public final String name;
 
+  /** Indicates if the data type is a signed integer. */
   @SuppressWarnings("MemberName")
   public final boolean isInt;
 
+  /** Indicates if the data type is an unsigned integer. */
   @SuppressWarnings("MemberName")
   public final boolean isUint;
 
+  /** The size (in bytes) of the data type. */
   @SuppressWarnings("MemberName")
   public final int size;
 
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructSerializable.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructSerializable.java
new file mode 100644
index 0000000..a8d1fdd
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/StructSerializable.java
@@ -0,0 +1,15 @@
+// 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.
+
+package edu.wpi.first.util.struct;
+
+import edu.wpi.first.util.WPISerializable;
+
+/**
+ * Marker interface to indicate a class is serializable using Struct serialization.
+ *
+ * <p>While this cannot be enforced by the interface, any class implementing this interface should
+ * provide a public final static `struct` member variable.
+ */
+public interface StructSerializable extends WPISerializable {}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java
index 9fa843c..5e71c79 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java
@@ -4,24 +4,50 @@
 
 package edu.wpi.first.util.struct.parser;
 
+/** Exception for parsing errors. */
 public class ParseException extends Exception {
+  /** The parser position. */
   private final int m_pos;
 
+  /**
+   * Constructs a ParseException.
+   *
+   * @param pos The parser position.
+   * @param s Reason for parse failure.
+   */
   public ParseException(int pos, String s) {
     super(s);
     m_pos = pos;
   }
 
+  /**
+   * Constructs a ParseException.
+   *
+   * @param pos The parser position.
+   * @param message Reason for parse failure.
+   * @param cause Exception that caused the parser failure.
+   */
   public ParseException(int pos, String message, Throwable cause) {
     super(message, cause);
     m_pos = pos;
   }
 
+  /**
+   * Constructs a ParseException.
+   *
+   * @param pos The parser position.
+   * @param cause Exception that caused the parser failure.
+   */
   public ParseException(int pos, Throwable cause) {
     super(cause);
     m_pos = pos;
   }
 
+  /**
+   * Returns position in parsed string.
+   *
+   * @return Position in parsed string.
+   */
   public int getPosition() {
     return m_pos;
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java
index 8184ae5..29743ab 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java
@@ -8,18 +8,26 @@
 
 /** Raw struct schema declaration. */
 public class ParsedDeclaration {
+  /** Type string. */
   @SuppressWarnings("MemberName")
   public String typeString;
 
+  /** Name. */
   @SuppressWarnings("MemberName")
   public String name;
 
+  /** Enum values. */
   @SuppressWarnings("MemberName")
   public Map<String, Long> enumValues;
 
+  /** Array size. */
   @SuppressWarnings("MemberName")
   public int arraySize = 1;
 
+  /** Bit width. */
   @SuppressWarnings("MemberName")
   public int bitWidth;
+
+  /** Default constructor. */
+  public ParsedDeclaration() {}
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java
index 2ca1753..2f7312a 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java
@@ -9,6 +9,10 @@
 
 /** Raw struct schema. */
 public class ParsedSchema {
+  /** Declarations. */
   @SuppressWarnings("MemberName")
   public List<ParsedDeclaration> declarations = new ArrayList<>();
+
+  /** Default constructor. */
+  public ParsedSchema() {}
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java
index 85afa4a..fb74c6f 100644
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java
@@ -6,17 +6,40 @@
 
 /** A lexed raw struct schema token. */
 public enum TokenKind {
+  /** Unknown. */
   kUnknown("unknown"),
+
+  /** Integer. */
   kInteger("integer"),
+
+  /** Identifier. */
   kIdentifier("identifier"),
+
+  /** Left square bracket. */
   kLeftBracket("'['"),
+
+  /** Right square bracket. */
   kRightBracket("']'"),
+
+  /** Left curly brace. */
   kLeftBrace("'{'"),
+
+  /** Right curly brace. */
   kRightBrace("'}'"),
+
+  /** Colon. */
   kColon("':'"),
+
+  /** Semicolon. */
   kSemicolon("';'"),
+
+  /** Comma. */
   kComma("','"),
+
+  /** Equals. */
   kEquals("'='"),
+
+  /** End of input. */
   kEndOfInput("<EOF>");
 
   private final String m_name;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/DataLog.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/DataLog.cpp
index d05a49e..40dab2a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/DataLog.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/DataLog.cpp
@@ -179,7 +179,7 @@
 DataLog::~DataLog() {
   {
     std::scoped_lock lock{m_mutex};
-    m_state = kShutdown;
+    m_shutdown = true;
     m_doFlush = true;
   }
   m_cond.notify_all();
@@ -419,7 +419,7 @@
   uintmax_t written = 0;
 
   std::unique_lock lock{m_mutex};
-  while (m_state != kShutdown) {
+  do {
     bool doFlush = false;
     auto timeoutTime = std::chrono::steady_clock::now() + periodTime;
     if (m_cond.wait_until(lock, timeoutTime) == std::cv_status::timeout) {
@@ -557,7 +557,7 @@
       }
       toWrite.resize(0);
     }
-  }
+  } while (!m_shutdown);
 }
 
 void DataLog::WriterThreadMain(
@@ -580,7 +580,7 @@
   std::vector<Buffer> toWrite;
 
   std::unique_lock lock{m_mutex};
-  while (m_state != kShutdown) {
+  do {
     bool doFlush = false;
     auto timeoutTime = std::chrono::steady_clock::now() + periodTime;
     if (m_cond.wait_until(lock, timeoutTime) == std::cv_status::timeout) {
@@ -614,7 +614,7 @@
       }
       toWrite.resize(0);
     }
-  }
+  } while (!m_shutdown);
 
   write({});  // indicate EOF
 }
@@ -743,8 +743,10 @@
     std::memcpy(buf, data.data(), kBlockSize);
     data = data.subspan(kBlockSize);
   }
-  uint8_t* buf = Reserve(data.size());
-  std::memcpy(buf, data.data(), data.size());
+  if (!data.empty()) {
+    uint8_t* buf = Reserve(data.size());
+    std::memcpy(buf, data.data(), data.size());
+  }
 }
 
 void DataLog::AppendStringImpl(std::string_view str) {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/RawFrame.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/RawFrame.cpp
new file mode 100644
index 0000000..2bc36ed
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/RawFrame.cpp
@@ -0,0 +1,49 @@
+// 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 "wpi/RawFrame.h"
+
+#include <wpi/MemAlloc.h>
+
+#include <cstring>
+
+extern "C" {
+int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize) {
+  if (frame->capacity >= requestedSize) {
+    return 0;
+  }
+  WPI_FreeRawFrameData(frame);
+  frame->data = static_cast<uint8_t*>(wpi::safe_malloc(requestedSize));
+  frame->capacity = requestedSize;
+  frame->size = 0;
+  return 1;
+}
+
+void WPI_FreeRawFrameData(WPI_RawFrame* frame) {
+  if (frame->data) {
+    if (frame->freeFunc) {
+      frame->freeFunc(frame->freeCbData, frame->data, frame->capacity);
+    } else {
+      std::free(frame->data);
+    }
+    frame->data = nullptr;
+    frame->freeFunc = nullptr;
+    frame->freeCbData = nullptr;
+    frame->capacity = 0;
+  }
+}
+
+void WPI_SetRawFrameData(WPI_RawFrame* frame, void* data, size_t size,
+                         size_t capacity, void* cbdata,
+                         void (*freeFunc)(void* cbdata, void* data,
+                                          size_t capacity)) {
+  WPI_FreeRawFrameData(frame);
+  frame->data = static_cast<uint8_t*>(data);
+  frame->freeFunc = freeFunc;
+  frame->freeCbData = cbdata;
+  frame->capacity = capacity;
+  frame->size = size;
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
index eb55fd0..26d1c93 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
@@ -9,6 +9,7 @@
 #include <fmt/format.h>
 
 #include "edu_wpi_first_util_WPIUtilJNI.h"
+#include "wpi/RawFrame.h"
 #include "wpi/Synchronization.h"
 #include "wpi/jni_util.h"
 #include "wpi/timestamp.h"
@@ -317,4 +318,95 @@
   return MakeJIntArray(env, signaled);
 }
 
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    allocateRawFrame
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_allocateRawFrame
+  (JNIEnv*, jclass)
+{
+  return reinterpret_cast<jlong>(new wpi::RawFrame);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    freeRawFrame
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_freeRawFrame
+  (JNIEnv*, jclass, jlong frame)
+{
+  delete reinterpret_cast<wpi::RawFrame*>(frame);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    getRawFrameDataPtr
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_getRawFrameDataPtr
+  (JNIEnv* env, jclass, jlong frame)
+{
+  auto* f = reinterpret_cast<wpi::RawFrame*>(frame);
+  if (!f) {
+    wpi::ThrowNullPointerException(env, "frame is null");
+    return 0;
+  }
+  return reinterpret_cast<jlong>(f->data);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    setRawFrameData
+ * Signature: (JLjava/lang/Object;IIIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameData
+  (JNIEnv* env, jclass, jlong frame, jobject data, jint size, jint width,
+   jint height, jint stride, jint pixelFormat)
+{
+  auto* f = reinterpret_cast<wpi::RawFrame*>(frame);
+  if (!f) {
+    wpi::ThrowNullPointerException(env, "frame is null");
+    return;
+  }
+  auto buf = env->GetDirectBufferAddress(data);
+  if (!buf) {
+    wpi::ThrowNullPointerException(env, "data is null");
+    return;
+  }
+  // there's no way to free a passed-in direct byte buffer
+  f->SetData(buf, size, env->GetDirectBufferCapacity(data), nullptr,
+             [](void*, void*, size_t) {});
+  f->width = width;
+  f->height = height;
+  f->stride = stride;
+  f->pixelFormat = pixelFormat;
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    setRawFrameInfo
+ * Signature: (JIIIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameInfo
+  (JNIEnv* env, jclass, jlong frame, jint size, jint width, jint height,
+   jint stride, jint pixelFormat)
+{
+  auto* f = reinterpret_cast<wpi::RawFrame*>(frame);
+  if (!f) {
+    wpi::ThrowNullPointerException(env, "frame is null");
+    return;
+  }
+  f->width = width;
+  f->height = height;
+  f->stride = stride;
+  f->pixelFormat = pixelFormat;
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp
index c7e2fa9..c811964 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp
@@ -5,6 +5,7 @@
 #include "wpi/timestamp.h"
 
 #include <atomic>
+#include <optional>
 
 #ifdef __FRC_ROBORIO__
 #include <stdint.h>
@@ -12,9 +13,7 @@
 #pragma GCC diagnostic ignored "-Wpedantic"
 #pragma GCC diagnostic ignored "-Wignored-qualifiers"
 #include <FRC_FPGA_ChipObject/RoboRIO_FRC_ChipObject_Aliases.h>
-#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/nInterfaceGlobals.h>
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tHMB.h>
-#include <FRC_NetworkCommunication/LoadOut.h>
 #pragma GCC diagnostic pop
 namespace fpga {
 using namespace nFPGA;
@@ -50,64 +49,107 @@
                                              const char* memoryName,
                                              size_t* memorySize,
                                              void** virtualAddress);
-struct HMBHolder {
-  ~HMBHolder() {
-    if (hmb) {
-      closeHmb(hmb->getSystemInterface()->getHandle(), hmbName);
-      dlclose(niFpga);
-    }
-  }
-  explicit operator bool() const { return hmb != nullptr; }
-  void Configure() {
-    nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
-        nLoadOut::getTargetClass();
+using NiFpga_FindRegisterFunc = NiFpga_Status (*)(NiFpga_Session session,
+                                                  const char* registerName,
+                                                  uint32_t* registerOffset);
+using NiFpga_ReadU32Func = NiFpga_Status (*)(NiFpga_Session session,
+                                             uint32_t indicator,
+                                             uint32_t* value);
+using NiFpga_WriteU32Func = NiFpga_Status (*)(NiFpga_Session session,
+                                              uint32_t control, uint32_t value);
+static void dlcloseWrapper(void* handle) {
+  dlclose(handle);
+}
+static std::atomic_flag hmbInitialized = ATOMIC_FLAG_INIT;
+static std::atomic_flag nowUseDefaultOnFailure = ATOMIC_FLAG_INIT;
+struct HMBLowLevel {
+  ~HMBLowLevel() { Reset(); }
+  bool Configure(const NiFpga_Session session) {
     int32_t status = 0;
-    hmb.reset(fpga::tHMB::create(&status));
-    niFpga = dlopen("libNiFpga.so", RTLD_LAZY);
+    niFpga.reset(dlopen("libNiFpga.so", RTLD_LAZY));
     if (!niFpga) {
-      hmb = nullptr;
-      return;
+      fmt::print(stderr, "Could not open libNiFpga.so\n");
+      return false;
     }
     NiFpga_OpenHmbFunc openHmb = reinterpret_cast<NiFpga_OpenHmbFunc>(
-        dlsym(niFpga, "NiFpgaDll_OpenHmb"));
+        dlsym(niFpga.get(), "NiFpgaDll_OpenHmb"));
     closeHmb = reinterpret_cast<NiFpga_CloseHmbFunc>(
-        dlsym(niFpga, "NiFpgaDll_CloseHmb"));
-    if (openHmb == nullptr || closeHmb == nullptr) {
+        dlsym(niFpga.get(), "NiFpgaDll_CloseHmb"));
+    NiFpga_FindRegisterFunc findRegister =
+        reinterpret_cast<NiFpga_FindRegisterFunc>(
+            dlsym(niFpga.get(), "NiFpgaDll_FindRegister"));
+    NiFpga_ReadU32Func readU32 = reinterpret_cast<NiFpga_ReadU32Func>(
+        dlsym(niFpga.get(), "NiFpgaDll_ReadU32"));
+    NiFpga_WriteU32Func writeU32 = reinterpret_cast<NiFpga_WriteU32Func>(
+        dlsym(niFpga.get(), "NiFpgaDll_WriteU32"));
+    if (openHmb == nullptr || closeHmb == nullptr || findRegister == nullptr ||
+        writeU32 == nullptr || readU32 == nullptr) {
+      fmt::print(stderr, "Could not find HMB symbols in libNiFpga.so\n");
+      niFpga = nullptr;
+      return false;
+    }
+    uint32_t hmbConfigRegister = 0;
+    status = findRegister(session, "HMB.Config", &hmbConfigRegister);
+    if (status != 0) {
+      fmt::print(stderr, "Failed to find HMB.Config register, status code {}\n",
+                 status);
       closeHmb = nullptr;
-      dlclose(niFpga);
-      hmb = nullptr;
-      return;
+      niFpga = nullptr;
+      return false;
     }
     size_t hmbBufferSize = 0;
     status =
-        openHmb(hmb->getSystemInterface()->getHandle(), hmbName, &hmbBufferSize,
+        openHmb(session, hmbName, &hmbBufferSize,
                 reinterpret_cast<void**>(const_cast<uint32_t**>(&hmbBuffer)));
     if (status != 0) {
+      fmt::print(stderr, "Failed to open HMB, status code {}\n", status);
       closeHmb = nullptr;
-      dlclose(niFpga);
-      hmb = nullptr;
-      return;
+      niFpga = nullptr;
+      return false;
     }
-    auto cfg = hmb->readConfig(&status);
+    fpga::tHMB::tConfig cfg;
+    uint32_t read = 0;
+    status = readU32(session, hmbConfigRegister, &read);
+    cfg.value = read;
     cfg.Enables_Timestamp = 1;
-    hmb->writeConfig(cfg, &status);
+    status = writeU32(session, hmbConfigRegister, cfg.value);
+    hmbSession.emplace(session);
+    hmbInitialized.test_and_set();
+    return true;
   }
   void Reset() {
-    if (hmb) {
-      std::unique_ptr<fpga::tHMB> oldHmb;
-      oldHmb.swap(hmb);
-      closeHmb(oldHmb->getSystemInterface()->getHandle(), hmbName);
-      closeHmb = nullptr;
-      hmbBuffer = nullptr;
-      oldHmb.reset();
-      dlclose(niFpga);
+    hmbInitialized.clear();
+    std::optional<NiFpga_Session> oldSesh;
+    hmbSession.swap(oldSesh);
+    if (oldSesh.has_value()) {
+      closeHmb(oldSesh.value(), hmbName);
       niFpga = nullptr;
     }
   }
-  std::unique_ptr<fpga::tHMB> hmb;
-  void* niFpga = nullptr;
+  std::optional<NiFpga_Session> hmbSession;
   NiFpga_CloseHmbFunc closeHmb = nullptr;
   volatile uint32_t* hmbBuffer = nullptr;
+  std::unique_ptr<void, decltype(&dlcloseWrapper)> niFpga{nullptr,
+                                                          dlcloseWrapper};
+};
+struct HMBHolder {
+  void Configure(void* col, std::unique_ptr<fpga::tHMB> hmbObject) {
+    hmb = std::move(hmbObject);
+    chipObjectLibrary.reset(col);
+    if (!lowLevel.Configure(hmb->getSystemInterface()->getHandle())) {
+      hmb = nullptr;
+      chipObjectLibrary = nullptr;
+    }
+  }
+  void Reset() {
+    lowLevel.Reset();
+    hmb = nullptr;
+    chipObjectLibrary = nullptr;
+  }
+  HMBLowLevel lowLevel;
+  std::unique_ptr<fpga::tHMB> hmb;
+  std::unique_ptr<void, decltype(&dlcloseWrapper)> chipObjectLibrary{
+      nullptr, dlcloseWrapper};
 };
 static HMBHolder hmb;
 }  // namespace
@@ -186,10 +228,26 @@
 
 static std::atomic<uint64_t (*)()> now_impl{wpi::NowDefault};
 
-void wpi::impl::SetupNowRio() {
+void wpi::impl::SetupNowDefaultOnRio() {
 #ifdef __FRC_ROBORIO__
-  if (!hmb) {
-    hmb.Configure();
+  nowUseDefaultOnFailure.test_and_set();
+#endif
+}
+
+#ifdef __FRC_ROBORIO__
+template <>
+void wpi::impl::SetupNowRio(void* chipObjectLibrary,
+                            std::unique_ptr<fpga::tHMB> hmbObject) {
+  if (!hmbInitialized.test()) {
+    hmb.Configure(chipObjectLibrary, std::move(hmbObject));
+  }
+}
+#endif
+
+void wpi::impl::SetupNowRio(uint32_t session) {
+#ifdef __FRC_ROBORIO__
+  if (!hmbInitialized.test()) {
+    hmb.lowLevel.Configure(session);
   }
 #endif
 }
@@ -207,25 +265,29 @@
 uint64_t wpi::Now() {
 #ifdef __FRC_ROBORIO__
   // Same code as HAL_GetFPGATime()
-  if (!hmb) {
-    std::fputs(
-        "FPGA not yet configured in wpi::Now(). Time will not be correct",
-        stderr);
-    std::fflush(stderr);
-    return 0;
+  if (!hmbInitialized.test()) {
+    if (nowUseDefaultOnFailure.test()) {
+      return timestamp() - offset_val;
+    } else {
+      fmt::print(
+          stderr,
+          "FPGA not yet configured in wpi::Now(). Time will not be correct.\n");
+      std::fflush(stderr);
+      return 1;
+    }
   }
 
   asm("dmb");
-  uint64_t upper1 = hmb.hmbBuffer[timestampUpperOffset];
+  uint64_t upper1 = hmb.lowLevel.hmbBuffer[timestampUpperOffset];
   asm("dmb");
-  uint32_t lower = hmb.hmbBuffer[timestampLowerOffset];
+  uint32_t lower = hmb.lowLevel.hmbBuffer[timestampLowerOffset];
   asm("dmb");
-  uint64_t upper2 = hmb.hmbBuffer[timestampUpperOffset];
+  uint64_t upper2 = hmb.lowLevel.hmbBuffer[timestampUpperOffset];
 
   if (upper1 != upper2) {
     // Rolled over between the lower call, reread lower
     asm("dmb");
-    lower = hmb.hmbBuffer[timestampLowerOffset];
+    lower = hmb.lowLevel.hmbBuffer[timestampLowerOffset];
   }
   // 5 is added here because the time to write from the FPGA
   // to the HMB buffer is longer then the time to read
@@ -244,8 +306,12 @@
 
 extern "C" {
 
-void WPI_Impl_SetupNowRio(void) {
-  return wpi::impl::SetupNowRio();
+void WPI_Impl_SetupNowUseDefaultOnRio(void) {
+  return wpi::impl::SetupNowDefaultOnRio();
+}
+
+void WPI_Impl_SetupNowRioWithSession(uint32_t session) {
+  return wpi::impl::SetupNowRio(session);
 }
 
 void WPI_Impl_ShutdownNowRio(void) {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h
index 1fd2502..112bfc3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h
@@ -5,6 +5,8 @@
 #pragma once
 
 #include <algorithm>
+#include <cstddef>
+#include <utility>
 #include <vector>
 
 namespace wpi {
@@ -15,4 +17,19 @@
                                                 T const& item) {
   return vec.insert(std::upper_bound(vec.begin(), vec.end(), item), item);
 }
+
+/**
+ * Calls f(i, elem) for each element of elems where i is the index of the
+ * element in elems and elem is the element.
+ *
+ * @param f The callback.
+ * @param elems The elements.
+ */
+template <typename F, typename... Ts>
+constexpr void for_each(F&& f, Ts&&... elems) {
+  [&]<size_t... Is>(std::index_sequence<Is...>) {
+    (f(Is, elems), ...);
+  }(std::index_sequence_for<Ts...>{});
+}
+
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLog.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLog.h
index 99db964..2f79a3a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLog.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLog.h
@@ -16,6 +16,7 @@
 #include <string>
 #include <string_view>
 #include <thread>
+#include <tuple>
 #include <utility>
 #include <vector>
 #include <version>
@@ -263,16 +264,20 @@
    * name are silently ignored.
    *
    * @tparam T struct serializable type
+   * @param info optional struct type info
    * @param timestamp Time stamp (0 to indicate now)
    */
-  template <StructSerializable T>
-  void AddStructSchema(int64_t timestamp = 0) {
+  template <typename T, typename... I>
+    requires StructSerializable<T, I...>
+  void AddStructSchema(const I&... info, int64_t timestamp = 0) {
     if (timestamp == 0) {
       timestamp = Now();
     }
-    ForEachStructSchema<T>([this, timestamp](auto typeString, auto schema) {
-      AddSchema(typeString, "structschema", schema, timestamp);
-    });
+    ForEachStructSchema<T>(
+        [this, timestamp](auto typeString, auto schema) {
+          this->AddSchema(typeString, "structschema", schema, timestamp);
+        },
+        info...);
   }
 
   /**
@@ -486,12 +491,12 @@
   mutable wpi::mutex m_mutex;
   wpi::condition_variable m_cond;
   bool m_doFlush{false};
+  bool m_shutdown{false};
   enum State {
     kStart,
     kActive,
     kPaused,
     kStopped,
-    kShutdown,
   } m_state = kActive;
   double m_period;
   std::string m_extraHeader;
@@ -946,19 +951,22 @@
 /**
  * Log raw struct serializable objects.
  */
-template <StructSerializable T>
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
 class StructLogEntry : public DataLogEntry {
-  using S = Struct<T>;
+  using S = Struct<T, I...>;
 
  public:
   StructLogEntry() = default;
-  StructLogEntry(DataLog& log, std::string_view name, int64_t timestamp = 0)
-      : StructLogEntry{log, name, {}, timestamp} {}
+  StructLogEntry(DataLog& log, std::string_view name, I... info,
+                 int64_t timestamp = 0)
+      : StructLogEntry{log, name, {}, std::move(info)..., timestamp} {}
   StructLogEntry(DataLog& log, std::string_view name, std::string_view metadata,
-                 int64_t timestamp = 0) {
+                 I... info, int64_t timestamp = 0)
+      : m_info{std::move(info)...} {
     m_log = &log;
-    log.AddStructSchema<T>(timestamp);
-    m_entry = log.Start(name, S::kTypeString, metadata, timestamp);
+    log.AddStructSchema<T, I...>(info..., timestamp);
+    m_entry = log.Start(name, S::GetTypeString(info...), metadata, timestamp);
   }
 
   /**
@@ -968,31 +976,46 @@
    * @param timestamp Time stamp (may be 0 to indicate now)
    */
   void Append(const T& data, int64_t timestamp = 0) {
-    uint8_t buf[S::kSize];
-    S::Pack(buf, data);
+    if constexpr (sizeof...(I) == 0) {
+      if constexpr (wpi::is_constexpr([] { S::GetSize(); })) {
+        uint8_t buf[S::GetSize()];
+        S::Pack(buf, data);
+        m_log->AppendRaw(m_entry, buf, timestamp);
+        return;
+      }
+    }
+    wpi::SmallVector<uint8_t, 128> buf;
+    buf.resize_for_overwrite(std::apply(S::GetSize, m_info));
+    std::apply([&](const I&... info) { S::Pack(buf, data, info...); }, m_info);
     m_log->AppendRaw(m_entry, buf, timestamp);
   }
+
+ private:
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 /**
  * Log raw struct serializable array of objects.
  */
-template <StructSerializable T>
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
 class StructArrayLogEntry : public DataLogEntry {
-  using S = Struct<T>;
+  using S = Struct<T, I...>;
 
  public:
   StructArrayLogEntry() = default;
-  StructArrayLogEntry(DataLog& log, std::string_view name,
+  StructArrayLogEntry(DataLog& log, std::string_view name, I... info,
                       int64_t timestamp = 0)
-      : StructArrayLogEntry{log, name, {}, timestamp} {}
+      : StructArrayLogEntry{log, name, {}, std::move(info)..., timestamp} {}
   StructArrayLogEntry(DataLog& log, std::string_view name,
-                      std::string_view metadata, int64_t timestamp = 0) {
+                      std::string_view metadata, I... info,
+                      int64_t timestamp = 0)
+      : m_info{std::move(info)...} {
     m_log = &log;
-    log.AddStructSchema<T>(timestamp);
-    m_entry =
-        log.Start(name, MakeStructArrayTypeString<T, std::dynamic_extent>(),
-                  metadata, timestamp);
+    log.AddStructSchema<T, I...>(info..., timestamp);
+    m_entry = log.Start(
+        name, MakeStructArrayTypeString<T, std::dynamic_extent>(info...),
+        metadata, timestamp);
   }
 
   /**
@@ -1007,9 +1030,14 @@
              std::convertible_to<std::ranges::range_value_t<U>, T>
 #endif
   void Append(U&& data, int64_t timestamp = 0) {
-    m_buf.Write(std::forward<U>(data), [&](auto bytes) {
-      m_log->AppendRaw(m_entry, bytes, timestamp);
-    });
+    std::apply(
+        [&](const I&... info) {
+          m_buf.Write(
+              std::forward<U>(data),
+              [&](auto bytes) { m_log->AppendRaw(m_entry, bytes, timestamp); },
+              info...);
+        },
+        m_info);
   }
 
   /**
@@ -1019,12 +1047,19 @@
    * @param timestamp Time stamp (may be 0 to indicate now)
    */
   void Append(std::span<const T> data, int64_t timestamp = 0) {
-    m_buf.Write(
-        data, [&](auto bytes) { m_log->AppendRaw(m_entry, bytes, timestamp); });
+    std::apply(
+        [&](const I&... info) {
+          m_buf.Write(
+              data,
+              [&](auto bytes) { m_log->AppendRaw(m_entry, bytes, timestamp); },
+              info...);
+        },
+        m_info);
   }
 
  private:
-  StructArrayBuffer<T> m_buf;
+  StructArrayBuffer<T, I...> m_buf;
+  [[no_unique_address]] std::tuple<I...> m_info;
 };
 
 /**
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLogReader.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLogReader.h
index b1153e4..cb9a8cb 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLogReader.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DataLogReader.h
@@ -288,7 +288,7 @@
 
   pointer operator->() const { return &this->operator*(); }
 
- private:
+ protected:
   const DataLogReader* m_reader;
   size_t m_pos;
   mutable bool m_valid = false;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h
index 03a7d3f..8514be3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h
@@ -7,6 +7,7 @@
 
 #include <string>
 #include <string_view>
+#include <typeinfo>
 
 namespace wpi {
 
@@ -18,6 +19,15 @@
  */
 std::string Demangle(std::string_view mangledSymbol);
 
+/**
+ * Returns the type name of an object
+ * @param type The object
+ */
+template <typename T>
+std::string GetTypeName(const T& type) {
+  return Demangle(typeid(type).name());
+}
+
 }  // namespace wpi
 
 #endif  // WPIUTIL_WPI_DEMANGLE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/RawFrame.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/RawFrame.h
new file mode 100644
index 0000000..1345ff1
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/RawFrame.h
@@ -0,0 +1,142 @@
+// 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.
+
+#ifndef WPIUTIL_WPI_RAWFRAME_H_
+#define WPIUTIL_WPI_RAWFRAME_H_
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+#include <concepts>
+#include <cstddef>
+#else
+
+#include <stddef.h>  // NOLINT
+
+#endif
+
+#ifdef WPI_RAWFRAME_JNI
+#include "jni_util.h"
+#endif
+
+// NOLINT
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Raw Frame
+ */
+typedef struct WPI_RawFrame {  // NOLINT
+  // image data
+  uint8_t* data;
+  // function to free image data (may be NULL)
+  void (*freeFunc)(void* cbdata, void* data, size_t capacity);
+  void* freeCbData;  // data passed to freeFunc
+  size_t capacity;   // data buffer capacity, in bytes
+  size_t size;       // actual size of data, in bytes
+  int pixelFormat;   // WPI_PixelFormat
+  int width;         // width of image, in pixels
+  int height;        // height of image, in pixels
+  int stride;        // size of each row of data, in bytes (may be 0)
+} WPI_RawFrame;
+
+/**
+ * Pixel formats
+ */
+enum WPI_PixelFormat {
+  WPI_PIXFMT_UNKNOWN = 0,  // unknown
+  WPI_PIXFMT_MJPEG,        // Motion-JPEG (compressed image data)
+  WPI_PIXFMT_YUYV,         // YUV 4:2:2, 16 bpp
+  WPI_PIXFMT_RGB565,       // RGB 5-6-5, 16 bpp
+  WPI_PIXFMT_BGR,          // BGR 8-8-8, 24 bpp
+  WPI_PIXFMT_GRAY,         // Grayscale, 8 bpp
+  WPI_PIXFMT_Y16,          // Grayscale, 16 bpp
+  WPI_PIXFMT_UYVY,         // YUV 4:2:2, 16 bpp
+};
+
+// Returns nonzero if the frame data was allocated/reallocated
+int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize);
+void WPI_FreeRawFrameData(WPI_RawFrame* frame);
+void WPI_SetRawFrameData(WPI_RawFrame* frame, void* data, size_t size,
+                         size_t capacity, void* cbdata,
+                         void (*freeFunc)(void* cbdata, void* data,
+                                          size_t capacity));
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#ifdef __cplusplus
+namespace wpi {
+struct RawFrame : public WPI_RawFrame {
+  RawFrame() {
+    data = nullptr;
+    freeFunc = nullptr;
+    freeCbData = nullptr;
+    capacity = 0;
+    size = 0;
+    pixelFormat = WPI_PIXFMT_UNKNOWN;
+    width = 0;
+    height = 0;
+  }
+  RawFrame(const RawFrame&) = delete;
+  RawFrame& operator=(const RawFrame&) = delete;
+  RawFrame(RawFrame&& rhs) noexcept : WPI_RawFrame{rhs} {
+    rhs.data = nullptr;
+    rhs.freeFunc = nullptr;
+    rhs.freeCbData = nullptr;
+    rhs.capacity = 0;
+    rhs.size = 0;
+  }
+  RawFrame& operator=(RawFrame&& rhs) noexcept {
+    *static_cast<WPI_RawFrame*>(this) = rhs;
+    rhs.data = nullptr;
+    rhs.freeFunc = nullptr;
+    rhs.freeCbData = nullptr;
+    rhs.capacity = 0;
+    rhs.size = 0;
+    return *this;
+  }
+
+  void SetData(void* data, size_t size, size_t capacity, void* cbdata,
+               void (*freeFunc)(void* cbdata, void* data, size_t capacity)) {
+    WPI_SetRawFrameData(this, data, size, capacity, cbdata, freeFunc);
+  }
+
+  // returns true if the frame data was allocated/reallocated
+  bool Reserve(size_t size) {
+    return WPI_AllocateRawFrameData(this, size) != 0;
+  }
+
+  ~RawFrame() { WPI_FreeRawFrameData(this); }
+};
+
+#ifdef WPI_RAWFRAME_JNI
+template <std::same_as<wpi::RawFrame> T>
+void SetFrameData(JNIEnv* env, jclass rawFrameCls, jobject jframe,
+                  const T& frame, bool newData) {
+  if (newData) {
+    static jmethodID setData = env->GetMethodID(rawFrameCls, "setDataJNI",
+                                                "(Ljava/nio/ByteBuffer;IIII)V");
+    env->CallVoidMethod(
+        jframe, setData, env->NewDirectByteBuffer(frame.data, frame.size),
+        static_cast<jint>(frame.width), static_cast<jint>(frame.height),
+        static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat));
+  } else {
+    static jmethodID setInfo =
+        env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIII)V");
+    env->CallVoidMethod(jframe, setInfo, static_cast<jint>(frame.width),
+                        static_cast<jint>(frame.height),
+                        static_cast<jint>(frame.stride),
+                        static_cast<jint>(frame.pixelFormat));
+  }
+}
+#endif
+
+}  // namespace wpi
+#endif
+
+#endif  // WPIUTIL_WPI_RAWFRAME_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h
index c54e2f5..a40a627 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <cstddef>
+#include <iterator>
 #include <vector>
 
 namespace wpi {
@@ -12,10 +13,17 @@
 /**
  * This is a simple circular buffer so we don't need to "bucket brigade" copy
  * old values.
+ *
+ * @tparam T Buffer element type.
  */
 template <class T>
 class circular_buffer {
  public:
+  /**
+   * Constructs a circular buffer.
+   *
+   * @param size Maximum number of buffer elements.
+   */
   explicit circular_buffer(size_t size) : m_data(size, T{}) {}
 
   circular_buffer(const circular_buffer&) = default;
@@ -32,7 +40,7 @@
     using reference = T&;
 
     iterator(circular_buffer* buffer, size_t index)
-        : m_buffer(buffer), m_index(index) {}
+        : m_buffer{buffer}, m_index{index} {}
 
     iterator& operator++() {
       ++m_index;
@@ -60,7 +68,7 @@
     using const_reference = const T&;
 
     const_iterator(const circular_buffer* buffer, size_t index)
-        : m_buffer(buffer), m_index(index) {}
+        : m_buffer{buffer}, m_index{index} {}
 
     const_iterator& operator++() {
       ++m_index;
@@ -268,16 +276,18 @@
   size_t m_length = 0;
 
   /**
-   * Increment an index modulo the length of the buffer.
+   * Increment an index modulo the size of the buffer.
    *
-   * @return The result of the modulo operation.
+   * @param index Index into the buffer.
+   * @return The incremented index.
    */
   size_t ModuloInc(size_t index) { return (index + 1) % m_data.size(); }
 
   /**
-   * Decrement an index modulo the length of the buffer.
+   * Decrement an index modulo the size of the buffer.
    *
-   * @return The result of the modulo operation.
+   * @param index Index into the buffer.
+   * @return The decremented index.
    */
   size_t ModuloDec(size_t index) {
     if (index == 0) {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ct_string.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ct_string.h
index 9f0ef90..2c7796b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ct_string.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ct_string.h
@@ -31,7 +31,7 @@
 
   template <size_t M>
     requires(M <= (N + 1))
-  consteval ct_string(Char const (&s)[M]) {  // NOLINT
+  constexpr ct_string(Char const (&s)[M]) {  // NOLINT
     if constexpr (M == (N + 1)) {
       if (s[N] != Char{}) {
         throw std::logic_error{"char array not null terminated"};
@@ -50,7 +50,7 @@
     }
   }
 
-  explicit consteval ct_string(std::basic_string_view<Char, Traits> s) {
+  explicit constexpr ct_string(std::basic_string_view<Char, Traits> s) {
     // avoid dependency on <algorithm>
     // auto p = std::ranges::copy(s, chars.begin()).out;
     auto p = chars.begin();
@@ -63,6 +63,64 @@
     }
   }
 
+  constexpr bool operator==(const ct_string<Char, Traits, N>&) const = default;
+
+  constexpr bool operator==(const std::basic_string<Char, Traits>& rhs) const {
+    if (size() != rhs.size()) {
+      return false;
+    }
+
+    for (size_t i = 0; i < size(); ++i) {
+      if (chars[i] != rhs[i]) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  constexpr bool operator==(std::basic_string_view<Char, Traits> rhs) const {
+    if (size() != rhs.size()) {
+      return false;
+    }
+
+    for (size_t i = 0; i < size(); ++i) {
+      if (chars[i] != rhs[i]) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  template <size_t M>
+    requires(N + 1 == M)
+  constexpr bool operator==(Char const (&rhs)[M]) const {
+    for (size_t i = 0; i < M; ++i) {
+      if (chars[i] != rhs[i]) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  constexpr bool operator==(const Char* rhs) const {
+    for (size_t i = 0; i < N + 1; ++i) {
+      if (chars[i] != rhs[i]) {
+        return false;
+      }
+
+      // If index of rhs's null terminator is less than lhs's size - 1, rhs is
+      // shorter than lhs
+      if (rhs[i] == '\0' && i < N) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
   constexpr auto size() const noexcept { return N; }
 
   constexpr auto begin() const noexcept { return chars.begin(); }
@@ -71,6 +129,11 @@
   constexpr auto data() const noexcept { return chars.data(); }
   constexpr auto c_str() const noexcept { return chars.data(); }
 
+  constexpr operator std::basic_string<Char, Traits>()  // NOLINT
+      const noexcept {
+    return std::basic_string<Char, Traits>{chars.data(), N};
+  }
+
   constexpr operator std::basic_string_view<Char, Traits>()  // NOLINT
       const noexcept {
     return std::basic_string_view<Char, Traits>{chars.data(), N};
@@ -82,13 +145,13 @@
 
 inline namespace literals {
 template <ct_string S>
-consteval auto operator""_ct_string() {
+constexpr auto operator""_ct_string() {
   return S;
 }
 }  // namespace literals
 
 template <typename Char, typename Traits, size_t N1, size_t N2>
-consteval auto operator+(ct_string<Char, Traits, N1> const& s1,
+constexpr auto operator+(ct_string<Char, Traits, N1> const& s1,
                          ct_string<Char, Traits, N2> const& s2) noexcept {
   return Concat(s1, s2);
 }
@@ -102,7 +165,7 @@
  * @return concatenated string
  */
 template <typename Char, typename Traits, size_t N1, size_t... N>
-consteval auto Concat(ct_string<Char, Traits, N1> const& s1,
+constexpr auto Concat(ct_string<Char, Traits, N1> const& s1,
                       ct_string<Char, Traits, N> const&... s) {
   // Need a dummy array to instantiate a ct_string.
   constexpr Char dummy[1] = {};
@@ -136,7 +199,7 @@
 template <intmax_t N, int Base = 10, typename Char = char,
           typename Traits = std::char_traits<Char>>
   requires(Base >= 2 && Base <= 36)
-consteval auto NumToCtString() {
+constexpr auto NumToCtString() {
   constexpr char digits[] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
 
   auto buflen = [] {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h
index 4115420..1913d7c 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h
@@ -15,12 +15,20 @@
 
 namespace wpi {
 
+/**
+ * Helper class for building Sendable dashboard representations.
+ */
 class SendableBuilder {
  public:
   /**
    * The backend kinds used for the sendable builder.
    */
-  enum BackendKind { kUnknown, kNetworkTables };
+  enum BackendKind {
+    /// Unknown.
+    kUnknown,
+    /// NetworkTables.
+    kNetworkTables
+  };
 
   virtual ~SendableBuilder() = default;
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
index 76498a1..0d38f25 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
@@ -6,12 +6,16 @@
 
 #include <array>
 #include <cstddef>
+#include <iterator>
 
 namespace wpi {
 
 /**
  * This is a simple circular buffer so we don't need to "bucket brigade" copy
  * old values.
+ *
+ * @tparam T Buffer element type.
+ * @tparam N Maximum number of buffer elements.
  */
 template <class T, size_t N>
 class static_circular_buffer {
@@ -27,7 +31,7 @@
     using reference = T&;
 
     iterator(static_circular_buffer* buffer, size_t index)
-        : m_buffer(buffer), m_index(index) {}
+        : m_buffer{buffer}, m_index{index} {}
 
     iterator& operator++() {
       ++m_index;
@@ -55,7 +59,7 @@
     using const_reference = const T&;
 
     const_iterator(const static_circular_buffer* buffer, size_t index)
-        : m_buffer(buffer), m_index(index) {}
+        : m_buffer{buffer}, m_index{index} {}
 
     const_iterator& operator++() {
       ++m_index;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h
index b88894c..4254090 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h
@@ -32,18 +32,31 @@
  * Known data types for raw struct dynamic fields (see StructFieldDescriptor).
  */
 enum class StructFieldType {
+  /// bool.
   kBool,
+  /// char.
   kChar,
+  /// int8.
   kInt8,
+  /// int16.
   kInt16,
+  /// int32.
   kInt32,
+  /// int64.
   kInt64,
+  /// uint8.
   kUint8,
+  /// uint16.
   kUint16,
+  /// uint32.
   kUint32,
+  /// uint64.
   kUint64,
+  /// float.
   kFloat,
+  /// double.
   kDouble,
+  /// struct.
   kStruct
 };
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/SchemaParser.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/SchemaParser.h
index a8cf1a4..4e5a1df 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/SchemaParser.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/SchemaParser.h
@@ -17,18 +17,31 @@
  * A lexed raw struct schema token.
  */
 struct Token {
+  /** A lexed raw struct schema token kind. */
   enum Kind {
+    /// Unknown.
     kUnknown,
+    /// Integer.
     kInteger,
+    /// Identifier.
     kIdentifier,
+    /// Left square bracket.
     kLeftBracket,
+    /// Right square bracket.
     kRightBracket,
+    /// Left curly brace.
     kLeftBrace,
+    /// Right curly brace.
     kRightBrace,
+    /// Colon.
     kColon,
+    /// Semicolon.
     kSemicolon,
+    /// Comma.
     kComma,
+    /// Equals.
     kEquals,
+    /// End of input.
     kEndOfInput,
   };
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/Struct.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/Struct.h
index 1336707..085b50b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/Struct.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/struct/Struct.h
@@ -7,6 +7,7 @@
 #include <stdint.h>
 
 #include <concepts>
+#include <memory>
 #include <span>
 #include <string>
 #include <string_view>
@@ -14,12 +15,14 @@
 #include <utility>
 #include <vector>
 
+#include <fmt/format.h>
+
 #include "wpi/Endian.h"
-#include "wpi/MathExtras.h"
 #include "wpi/bit.h"
 #include "wpi/ct_string.h"
 #include "wpi/function_ref.h"
 #include "wpi/mutex.h"
+#include "wpi/type_traits.h"
 
 namespace wpi {
 
@@ -29,8 +32,9 @@
  * StructSerializable concept.
  *
  * @tparam T type to serialize/deserialize
+ * @tparam I optional struct type info
  */
-template <typename T>
+template <typename T, typename... I>
 struct Struct {};
 
 /**
@@ -45,37 +49,47 @@
  * Implementations must define a template specialization for wpi::Struct with
  * T being the type that is being serialized/deserialized, with the following
  * static members (as enforced by this concept):
- * - std::string_view kTypeString: the type string
- * - size_t kSize: the structure size in bytes
- * - std::string_view kSchema: the struct schema
- * - T Unpack(std::span<const uint8_t, kSize>): function for deserialization
- * - void Pack(std::span<uint8_t, kSize>, T&& value): function for
+ * - std::string_view GetTypeString(): function that returns the type string
+ * - size_t GetSize(): function that returns the structure size in bytes
+ * - std::string_view GetSchema(): function that returns the struct schema
+ * - T Unpack(std::span<const uint8_t>): function for deserialization
+ * - void Pack(std::span<uint8_t>, T&& value): function for
  *   serialization
  *
+ * If possible, the GetTypeString(), GetSize(), and GetSchema() functions should
+ * be marked constexpr. GetTypeString() and GetSchema() may return types other
+ * than std::string_view, as long as the return value is convertible to
+ * std::string_view.
+ *
  * If the struct has nested structs, implementations should also meet the
  * requirements of HasNestedStruct<T>.
  */
-template <typename T>
-concept StructSerializable =
-    requires(std::span<const uint8_t> in, std::span<uint8_t> out, T&& value) {
-      typename Struct<typename std::remove_cvref_t<T>>;
-      {
-        Struct<typename std::remove_cvref_t<T>>::kTypeString
-      } -> std::convertible_to<std::string_view>;
-      {
-        Struct<typename std::remove_cvref_t<T>>::kSize
-      } -> std::convertible_to<size_t>;
-      {
-        Struct<typename std::remove_cvref_t<T>>::kSchema
-      } -> std::convertible_to<std::string_view>;
-      {
-        Struct<typename std::remove_cvref_t<T>>::Unpack(
-            in.subspan<0, Struct<typename std::remove_cvref_t<T>>::kSize>())
-      } -> std::same_as<typename std::remove_cvref_t<T>>;
-      Struct<typename std::remove_cvref_t<T>>::Pack(
-          out.subspan<0, Struct<typename std::remove_cvref_t<T>>::kSize>(),
-          std::forward<T>(value));
-    };
+template <typename T, typename... I>
+concept StructSerializable = requires(std::span<const uint8_t> in,
+                                      std::span<uint8_t> out, T&& value,
+                                      const I&... info) {
+  typename Struct<typename std::remove_cvref_t<T>,
+                  typename std::remove_cvref_t<I>...>;
+  {
+    Struct<typename std::remove_cvref_t<T>,
+           typename std::remove_cvref_t<I>...>::GetTypeString(info...)
+  } -> std::convertible_to<std::string_view>;
+  {
+    Struct<typename std::remove_cvref_t<T>,
+           typename std::remove_cvref_t<I>...>::GetSize(info...)
+  } -> std::convertible_to<size_t>;
+  {
+    Struct<typename std::remove_cvref_t<T>,
+           typename std::remove_cvref_t<I>...>::GetSchema(info...)
+  } -> std::convertible_to<std::string_view>;
+  {
+    Struct<typename std::remove_cvref_t<T>,
+           typename std::remove_cvref_t<I>...>::Unpack(in, info...)
+  } -> std::same_as<typename std::remove_cvref_t<T>>;
+  Struct<typename std::remove_cvref_t<T>,
+         typename std::remove_cvref_t<I>...>::Pack(out, std::forward<T>(value),
+                                                   info...);
+};
 
 /**
  * Specifies that a type is capable of in-place raw struct deserialization.
@@ -84,11 +98,12 @@
  * wpi::Struct<T> static member `void UnpackInto(T*, std::span<const uint8_t>)`
  * to update the pointed-to T with the contents of the span.
  */
-template <typename T>
+template <typename T, typename... I>
 concept MutableStructSerializable =
-    StructSerializable<T> && requires(T* out, std::span<const uint8_t> in) {
-      Struct<typename std::remove_cvref_t<T>>::UnpackInto(
-          out, in.subspan<0, Struct<typename std::remove_cvref_t<T>>::kSize>());
+    StructSerializable<T, I...> &&
+    requires(T* out, std::span<const uint8_t> in, const I&... info) {
+      Struct<typename std::remove_cvref_t<T>,
+             typename std::remove_cvref_t<I>...>::UnpackInto(out, in, info...);
     };
 
 /**
@@ -100,11 +115,13 @@
  * fn)` (or equivalent) and call ForEachNestedStruct<Type> on each nested struct
  * type.
  */
-template <typename T>
+template <typename T, typename... I>
 concept HasNestedStruct =
-    StructSerializable<T> &&
-    requires(function_ref<void(std::string_view, std::string_view)> fn) {
-      Struct<typename std::remove_cvref_t<T>>::ForEachNested(fn);
+    StructSerializable<T, I...> &&
+    requires(function_ref<void(std::string_view, std::string_view)> fn,
+             const I&... info) {
+      Struct<typename std::remove_cvref_t<T>,
+             typename std::remove_cvref_t<I>...>::ForEachNested(fn, info...);
     };
 
 /**
@@ -112,11 +129,14 @@
  *
  * @tparam T object type
  * @param data raw struct data
+ * @param info optional struct type info
  * @return Deserialized object
  */
-template <StructSerializable T>
-inline T UnpackStruct(std::span<const uint8_t, Struct<T>::kSize> data) {
-  return Struct<T>::Unpack(data);
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
+inline T UnpackStruct(std::span<const uint8_t> data, const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  return S::Unpack(data, info...);
 }
 
 /**
@@ -126,11 +146,14 @@
  * @tparam T object type
  * @tparam Offset starting offset
  * @param data raw struct data
+ * @param info optional struct type info
  * @return Deserialized object
  */
-template <StructSerializable T, size_t Offset>
-inline T UnpackStruct(std::span<const uint8_t> data) {
-  return Struct<T>::Unpack(data.template subspan<Offset, Struct<T>::kSize>());
+template <typename T, size_t Offset, typename... I>
+  requires StructSerializable<T, I...>
+inline T UnpackStruct(std::span<const uint8_t> data, const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  return S::Unpack(data.subspan(Offset), info...);
 }
 
 /**
@@ -138,12 +161,14 @@
  *
  * @param data struct storage (mutable, output)
  * @param value object
+ * @param info optional struct type info
  */
-template <StructSerializable T>
-inline void PackStruct(
-    std::span<uint8_t, Struct<typename std::remove_cvref_t<T>>::kSize> data,
-    T&& value) {
-  Struct<typename std::remove_cvref_t<T>>::Pack(data, std::forward<T>(value));
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
+inline void PackStruct(std::span<uint8_t> data, T&& value, const I&... info) {
+  using S = Struct<typename std::remove_cvref_t<T>,
+                   typename std::remove_cvref_t<I>...>;
+  S::Pack(data, std::forward<T>(value), info...);
 }
 
 /**
@@ -153,13 +178,14 @@
  * @tparam Offset starting offset
  * @param data struct storage (mutable, output)
  * @param value object
+ * @param info optional struct type info
  */
-template <size_t Offset, StructSerializable T>
-inline void PackStruct(std::span<uint8_t> data, T&& value) {
-  Struct<typename std::remove_cvref_t<T>>::Pack(
-      data.template subspan<Offset,
-                            Struct<typename std::remove_cvref_t<T>>::kSize>(),
-      std::forward<T>(value));
+template <size_t Offset, typename T, typename... I>
+  requires StructSerializable<T, I...>
+inline void PackStruct(std::span<uint8_t> data, T&& value, const I&... info) {
+  using S = Struct<typename std::remove_cvref_t<T>,
+                   typename std::remove_cvref_t<I>...>;
+  S::Pack(data.subspan(Offset), std::forward<T>(value), info...);
 }
 
 /**
@@ -167,14 +193,17 @@
  *
  * @param out object (output)
  * @param data raw struct data
+ * @param info optional struct type info
  */
-template <StructSerializable T>
-inline void UnpackStructInto(T* out,
-                             std::span<const uint8_t, Struct<T>::kSize> data) {
-  if constexpr (MutableStructSerializable<T>) {
-    Struct<T>::UnpackInto(out, data);
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
+inline void UnpackStructInto(T* out, std::span<const uint8_t> data,
+                             const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  if constexpr (MutableStructSerializable<T, I...>) {
+    S::UnpackInto(out, data, info...);
   } else {
-    *out = UnpackStruct<T>(data);
+    *out = UnpackStruct<T>(data, info...);
   }
 }
 
@@ -186,14 +215,17 @@
  * @tparam Offset starting offset
  * @param out object (output)
  * @param data raw struct data
+ * @param info optional struct type info
  */
-template <size_t Offset, StructSerializable T>
-inline void UnpackStructInto(T* out, std::span<const uint8_t> data) {
-  if constexpr (MutableStructSerializable<T>) {
-    Struct<T>::UnpackInto(out,
-                          data.template subspan<Offset, Struct<T>::kSize>());
+template <size_t Offset, typename T, typename... I>
+  requires StructSerializable<T, I...>
+inline void UnpackStructInto(T* out, std::span<const uint8_t> data,
+                             const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  if constexpr (MutableStructSerializable<T, I...>) {
+    S::UnpackInto(out, data.subspan(Offset), info...);
   } else {
-    *out = UnpackStruct<T, Offset>(data);
+    *out = UnpackStruct<T, Offset>(data, info...);
   }
 }
 
@@ -201,68 +233,116 @@
  * Get the type string for a raw struct serializable type
  *
  * @tparam T serializable type
+ * @param info optional struct type info
  * @return type string
  */
-template <StructSerializable T>
-constexpr auto GetStructTypeString() {
-  return Struct<T>::kTypeString;
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
+constexpr auto GetStructTypeString(const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  return S::GetTypeString(info...);
 }
 
-template <StructSerializable T, size_t N>
-consteval auto MakeStructArrayTypeString() {
-  using namespace literals;
-  if constexpr (N == std::dynamic_extent) {
-    return Concat(
-        ct_string<char, std::char_traits<char>, Struct<T>::kTypeString.size()>{
-            Struct<T>::kTypeString},
-        "[]"_ct_string);
+/**
+ * Get the size for a raw struct serializable type
+ *
+ * @tparam T serializable type
+ * @param info optional struct type info
+ * @return size
+ */
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
+constexpr size_t GetStructSize(const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  return S::GetSize(info...);
+}
+
+template <typename T, size_t N, typename... I>
+  requires StructSerializable<T, I...>
+constexpr auto MakeStructArrayTypeString(const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  if constexpr (sizeof...(I) == 0 &&
+                is_constexpr([&] { S::GetTypeString(info...); })) {
+    constexpr auto typeString = S::GetTypeString(info...);
+    using namespace literals;
+    if constexpr (N == std::dynamic_extent) {
+      return Concat(
+          ct_string<char, std::char_traits<char>, typeString.size()>{
+              typeString},
+          "[]"_ct_string);
+    } else {
+      return Concat(
+          ct_string<char, std::char_traits<char>, typeString.size()>{
+              typeString},
+          "["_ct_string, NumToCtString<N>(), "]"_ct_string);
+    }
   } else {
-    return Concat(
-        ct_string<char, std::char_traits<char>, Struct<T>::kTypeString.size()>{
-            Struct<T>::kTypeString},
-        "["_ct_string, NumToCtString<N>(), "]"_ct_string);
+    if constexpr (N == std::dynamic_extent) {
+      return fmt::format("{}[]", S::GetTypeString(info...));
+    } else {
+      return fmt::format("{}[{}]", S::GetTypeString(info...), N);
+    }
   }
 }
 
-template <StructSerializable T, size_t N>
-consteval auto MakeStructArraySchema() {
-  using namespace literals;
-  if constexpr (N == std::dynamic_extent) {
-    return Concat(
-        ct_string<char, std::char_traits<char>, Struct<T>::kSchema.size()>{
-            Struct<T>::kSchema},
-        "[]"_ct_string);
+template <typename T, size_t N, typename... I>
+  requires StructSerializable<T, I...>
+constexpr auto MakeStructArraySchema(const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  if constexpr (sizeof...(I) == 0 &&
+                is_constexpr([&] { S::GetSchema(info...); })) {
+    constexpr auto schema = S::GetSchema(info...);
+    using namespace literals;
+    if constexpr (N == std::dynamic_extent) {
+      return Concat(
+          ct_string<char, std::char_traits<char>, schema.size()>{schema},
+          "[]"_ct_string);
+    } else {
+      return Concat(
+          ct_string<char, std::char_traits<char>, schema.size()>{schema},
+          "["_ct_string, NumToCtString<N>(), "]"_ct_string);
+    }
   } else {
-    return Concat(
-        ct_string<char, std::char_traits<char>, Struct<T>::kSchema.size()>{
-            Struct<T>::kSchema},
-        "["_ct_string, NumToCtString<N>(), "]"_ct_string);
+    if constexpr (N == std::dynamic_extent) {
+      return fmt::format("{}[]", S::GetSchema(info...));
+    } else {
+      return fmt::format("{}[{}]", S::GetSchema(info...), N);
+    }
   }
 }
 
-template <StructSerializable T>
-constexpr std::string_view GetStructSchema() {
-  return Struct<T>::kSchema;
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
+constexpr std::string_view GetStructSchema(const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  return S::GetSchema(info...);
 }
 
-template <StructSerializable T>
-constexpr std::span<const uint8_t> GetStructSchemaBytes() {
-  return {reinterpret_cast<const uint8_t*>(Struct<T>::kSchema.data()),
-          Struct<T>::kSchema.size()};
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
+constexpr std::span<const uint8_t> GetStructSchemaBytes(const I&... info) {
+  using S = Struct<T, typename std::remove_cvref_t<I>...>;
+  auto schema = S::GetSchema(info...);
+  return {reinterpret_cast<const uint8_t*>(schema.data()), schema.size()};
 }
 
-template <StructSerializable T>
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
 void ForEachStructSchema(
-    std::invocable<std::string_view, std::string_view> auto fn) {
-  if constexpr (HasNestedStruct<T>) {
-    Struct<T>::ForEachNested(fn);
+    std::invocable<std::string_view, std::string_view> auto fn,
+    const I&... info) {
+  using S = Struct<typename std::remove_cvref_t<T>,
+                   typename std::remove_cvref_t<I>...>;
+  if constexpr (HasNestedStruct<T, I...>) {
+    S::ForEachNested(fn, info...);
   }
-  fn(Struct<T>::kTypeString, Struct<T>::kSchema);
+  fn(S::GetTypeString(info...), S::GetSchema(info...));
 }
 
-template <StructSerializable T>
+template <typename T, typename... I>
+  requires StructSerializable<T, I...>
 class StructArrayBuffer {
-  using S = Struct<T>;
+  using S = Struct<T, I...>;
 
  public:
   StructArrayBuffer() = default;
@@ -281,25 +361,26 @@
       std::convertible_to<std::ranges::range_value_t<U>, T> &&
 #endif
       std::invocable<F, std::span<const uint8_t>>
-    void Write(U&& data, F&& func) {
-    if ((std::size(data) * S::kSize) < 256) {
+    void Write(U&& data, F&& func, const I&... info) {
+    auto size = S::GetSize(info...);
+    if ((std::size(data) * size) < 256) {
       // use the stack
       uint8_t buf[256];
       auto out = buf;
       for (auto&& val : data) {
-        S::Pack(std::span<uint8_t, S::kSize>{out, out + S::kSize},
-                std::forward<decltype(val)>(val));
-        out += S::kSize;
+        S::Pack(std::span<uint8_t>{std::to_address(out), size},
+                std::forward<decltype(val)>(val), info...);
+        out += size;
       }
       func(std::span<uint8_t>{buf, out});
     } else {
       std::scoped_lock lock{m_mutex};
-      m_buf.resize(std::size(data) * S::kSize);
+      m_buf.resize(std::size(data) * size);
       auto out = m_buf.begin();
       for (auto&& val : data) {
-        S::Pack(std::span<uint8_t, S::kSize>{out, out + S::kSize},
-                std::forward<decltype(val)>(val));
-        out += S::kSize;
+        S::Pack(std::span<uint8_t>{std::to_address(out), size},
+                std::forward<decltype(val)>(val), info...);
+        out += size;
       }
       func(m_buf);
     }
@@ -313,38 +394,49 @@
 /**
  * Raw struct support for fixed-size arrays of other structs.
  */
-template <StructSerializable T, size_t N>
-struct Struct<std::array<T, N>> {
-  static constexpr auto kTypeString = MakeStructArrayTypeString<T, N>();
-  static constexpr size_t kSize = N * Struct<T>::kSize;
-  static constexpr auto kSchema = MakeStructArraySchema<T, N>();
-  static std::array<T, N> Unpack(std::span<const uint8_t, kSize> data) {
+template <typename T, size_t N, typename... I>
+  requires StructSerializable<T, I...>
+struct Struct<std::array<T, N>, I...> {
+  static constexpr auto GetTypeString(const I&... info) {
+    return MakeStructArrayTypeString<T, N>(info...);
+  }
+  static constexpr size_t GetSize(const I&... info) {
+    return N * GetStructSize<T>(info...);
+  }
+  static constexpr auto GetSchema(const I&... info) {
+    return MakeStructArraySchema<T, N>(info...);
+  }
+  static std::array<T, N> Unpack(std::span<const uint8_t> data,
+                                 const I&... info) {
+    auto size = GetStructSize<T>(info...);
     std::array<T, N> result;
     for (size_t i = 0; i < N; ++i) {
-      result[i] = UnpackStruct<T, 0>(data);
-      data = data.subspan(Struct<T>::kSize);
+      result[i] = UnpackStruct<T, 0>(data, info...);
+      data = data.subspan(size);
     }
     return result;
   }
-  static void Pack(std::span<uint8_t, kSize> data,
-                   std::span<const T, N> values) {
+  static void Pack(std::span<uint8_t> data, std::span<const T, N> values,
+                   const I&... info) {
+    auto size = GetStructSize<T>(info...);
     std::span<uint8_t> unsizedData = data;
     for (auto&& val : values) {
-      PackStruct<0>(unsizedData, val);
-      unsizedData = unsizedData.subspan(Struct<T>::kSize);
+      PackStruct(unsizedData, val, info...);
+      unsizedData = unsizedData.subspan(size);
     }
   }
-  static void UnpackInto(std::array<T, N>* out,
-                         std::span<const uint8_t, kSize> data) {
-    UnpackInto(std::span{*out}, data);
+  static void UnpackInto(std::array<T, N>* out, std::span<const uint8_t> data,
+                         const I&... info) {
+    UnpackInto(std::span{*out}, data, info...);
   }
   // alternate span-based function
-  static void UnpackInto(std::span<T, N> out,
-                         std::span<const uint8_t, kSize> data) {
+  static void UnpackInto(std::span<T, N> out, std::span<const uint8_t> data,
+                         const I&... info) {
+    auto size = GetStructSize<T>(info...);
     std::span<const uint8_t> unsizedData = data;
     for (size_t i = 0; i < N; ++i) {
-      UnpackStructInto<0, T>(&out[i], unsizedData);
-      unsizedData = unsizedData.subspan(Struct<T>::kSize);
+      UnpackStructInto(&out[i], unsizedData, info...);
+      unsizedData = unsizedData.subspan(size);
     }
   }
 };
@@ -355,11 +447,11 @@
  */
 template <>
 struct Struct<bool> {
-  static constexpr std::string_view kTypeString = "struct:bool";
-  static constexpr size_t kSize = 1;
-  static constexpr std::string_view kSchema = "bool value";
-  static bool Unpack(std::span<const uint8_t, 1> data) { return data[0]; }
-  static void Pack(std::span<uint8_t, 1> data, bool value) {
+  static constexpr std::string_view GetTypeString() { return "struct:bool"; }
+  static constexpr size_t GetSize() { return 1; }
+  static constexpr std::string_view GetSchema() { return "bool value"; }
+  static bool Unpack(std::span<const uint8_t> data) { return data[0]; }
+  static void Pack(std::span<uint8_t> data, bool value) {
     data[0] = static_cast<char>(value ? 1 : 0);
   }
 };
@@ -370,13 +462,11 @@
  */
 template <>
 struct Struct<uint8_t> {
-  static constexpr std::string_view kTypeString = "struct:uint8";
-  static constexpr size_t kSize = 1;
-  static constexpr std::string_view kSchema = "uint8 value";
-  static uint8_t Unpack(std::span<const uint8_t, 1> data) { return data[0]; }
-  static void Pack(std::span<uint8_t, 1> data, uint8_t value) {
-    data[0] = value;
-  }
+  static constexpr std::string_view GetTypeString() { return "struct:uint8"; }
+  static constexpr size_t GetSize() { return 1; }
+  static constexpr std::string_view GetSchema() { return "uint8 value"; }
+  static uint8_t Unpack(std::span<const uint8_t> data) { return data[0]; }
+  static void Pack(std::span<uint8_t> data, uint8_t value) { data[0] = value; }
 };
 
 /**
@@ -385,9 +475,9 @@
  */
 template <>
 struct Struct<int8_t> {
-  static constexpr std::string_view kTypeString = "struct:int8";
-  static constexpr size_t kSize = 1;
-  static constexpr std::string_view kSchema = "int8 value";
+  static constexpr std::string_view GetTypeString() { return "struct:int8"; }
+  static constexpr size_t GetSize() { return 1; }
+  static constexpr std::string_view GetSchema() { return "int8 value"; }
   static int8_t Unpack(std::span<const uint8_t, 1> data) { return data[0]; }
   static void Pack(std::span<uint8_t, 1> data, int8_t value) {
     data[0] = value;
@@ -400,13 +490,13 @@
  */
 template <>
 struct Struct<uint16_t> {
-  static constexpr std::string_view kTypeString = "struct:uint16";
-  static constexpr size_t kSize = 2;
-  static constexpr std::string_view kSchema = "uint16 value";
-  static uint16_t Unpack(std::span<const uint8_t, 2> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:uint16"; }
+  static constexpr size_t GetSize() { return 2; }
+  static constexpr std::string_view GetSchema() { return "uint16 value"; }
+  static uint16_t Unpack(std::span<const uint8_t> data) {
     return support::endian::read16le(data.data());
   }
-  static void Pack(std::span<uint8_t, 2> data, uint16_t value) {
+  static void Pack(std::span<uint8_t> data, uint16_t value) {
     support::endian::write16le(data.data(), value);
   }
 };
@@ -417,13 +507,13 @@
  */
 template <>
 struct Struct<int16_t> {
-  static constexpr std::string_view kTypeString = "struct:int16";
-  static constexpr size_t kSize = 2;
-  static constexpr std::string_view kSchema = "int16 value";
-  static int16_t Unpack(std::span<const uint8_t, 2> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:int16"; }
+  static constexpr size_t GetSize() { return 2; }
+  static constexpr std::string_view GetSchema() { return "int16 value"; }
+  static int16_t Unpack(std::span<const uint8_t> data) {
     return support::endian::read16le(data.data());
   }
-  static void Pack(std::span<uint8_t, 2> data, int16_t value) {
+  static void Pack(std::span<uint8_t> data, int16_t value) {
     support::endian::write16le(data.data(), value);
   }
 };
@@ -434,13 +524,13 @@
  */
 template <>
 struct Struct<uint32_t> {
-  static constexpr std::string_view kTypeString = "struct:uint32";
-  static constexpr size_t kSize = 4;
-  static constexpr std::string_view kSchema = "uint32 value";
-  static uint32_t Unpack(std::span<const uint8_t, 4> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:uint32"; }
+  static constexpr size_t GetSize() { return 4; }
+  static constexpr std::string_view GetSchema() { return "uint32 value"; }
+  static uint32_t Unpack(std::span<const uint8_t> data) {
     return support::endian::read32le(data.data());
   }
-  static void Pack(std::span<uint8_t, 4> data, uint32_t value) {
+  static void Pack(std::span<uint8_t> data, uint32_t value) {
     support::endian::write32le(data.data(), value);
   }
 };
@@ -451,13 +541,13 @@
  */
 template <>
 struct Struct<int32_t> {
-  static constexpr std::string_view kTypeString = "struct:int32";
-  static constexpr size_t kSize = 4;
-  static constexpr std::string_view kSchema = "int32 value";
-  static int32_t Unpack(std::span<const uint8_t, 4> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:int32"; }
+  static constexpr size_t GetSize() { return 4; }
+  static constexpr std::string_view GetSchema() { return "int32 value"; }
+  static int32_t Unpack(std::span<const uint8_t> data) {
     return support::endian::read32le(data.data());
   }
-  static void Pack(std::span<uint8_t, 4> data, int32_t value) {
+  static void Pack(std::span<uint8_t> data, int32_t value) {
     support::endian::write32le(data.data(), value);
   }
 };
@@ -468,13 +558,13 @@
  */
 template <>
 struct Struct<uint64_t> {
-  static constexpr std::string_view kTypeString = "struct:uint64";
-  static constexpr size_t kSize = 8;
-  static constexpr std::string_view kSchema = "uint64 value";
-  static uint64_t Unpack(std::span<const uint8_t, 8> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:uint64"; }
+  static constexpr size_t GetSize() { return 8; }
+  static constexpr std::string_view GetSchema() { return "uint64 value"; }
+  static uint64_t Unpack(std::span<const uint8_t> data) {
     return support::endian::read64le(data.data());
   }
-  static void Pack(std::span<uint8_t, 8> data, uint64_t value) {
+  static void Pack(std::span<uint8_t> data, uint64_t value) {
     support::endian::write64le(data.data(), value);
   }
 };
@@ -485,13 +575,13 @@
  */
 template <>
 struct Struct<int64_t> {
-  static constexpr std::string_view kTypeString = "struct:int64";
-  static constexpr size_t kSize = 8;
-  static constexpr std::string_view kSchema = "int64 value";
-  static int64_t Unpack(std::span<const uint8_t, 8> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:int64"; }
+  static constexpr size_t GetSize() { return 8; }
+  static constexpr std::string_view GetSchema() { return "int64 value"; }
+  static int64_t Unpack(std::span<const uint8_t> data) {
     return support::endian::read64le(data.data());
   }
-  static void Pack(std::span<uint8_t, 8> data, int64_t value) {
+  static void Pack(std::span<uint8_t> data, int64_t value) {
     support::endian::write64le(data.data(), value);
   }
 };
@@ -502,13 +592,13 @@
  */
 template <>
 struct Struct<float> {
-  static constexpr std::string_view kTypeString = "struct:float";
-  static constexpr size_t kSize = 4;
-  static constexpr std::string_view kSchema = "float value";
-  static float Unpack(std::span<const uint8_t, 4> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:float"; }
+  static constexpr size_t GetSize() { return 4; }
+  static constexpr std::string_view GetSchema() { return "float value"; }
+  static float Unpack(std::span<const uint8_t> data) {
     return bit_cast<float>(support::endian::read32le(data.data()));
   }
-  static void Pack(std::span<uint8_t, 4> data, float value) {
+  static void Pack(std::span<uint8_t> data, float value) {
     support::endian::write32le(data.data(), bit_cast<uint32_t>(value));
   }
 };
@@ -519,13 +609,13 @@
  */
 template <>
 struct Struct<double> {
-  static constexpr std::string_view kTypeString = "struct:double";
-  static constexpr size_t kSize = 8;
-  static constexpr std::string_view kSchema = "double value";
-  static double Unpack(std::span<const uint8_t, 8> data) {
+  static constexpr std::string_view GetTypeString() { return "struct:double"; }
+  static constexpr size_t GetSize() { return 8; }
+  static constexpr std::string_view GetSchema() { return "double value"; }
+  static double Unpack(std::span<const uint8_t> data) {
     return bit_cast<double>(support::endian::read64le(data.data()));
   }
-  static void Pack(std::span<uint8_t, 8> data, double value) {
+  static void Pack(std::span<uint8_t> data, double value) {
     support::endian::write64le(data.data(), bit_cast<uint64_t>(value));
   }
 };
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h
index c232481..4d61055 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h
@@ -8,15 +8,13 @@
 #include <stdint.h>
 
 #ifdef __cplusplus
-extern "C" {
+#include <memory>  // NOLINT
+
 #endif
 
-/**
- * Initialize the on-Rio Now() implementation to use the FPGA timestamp.
- * No effect on non-Rio platforms. This is called by HAL_Initialize() and
- * thus should generally not be called by user code.
- */
-void WPI_Impl_SetupNowRio(void);
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 /**
  * De-initialize the on-Rio Now() implementation. No effect on non-Rio
@@ -63,11 +61,31 @@
 
 namespace impl {
 /**
+ * Initialize the on-Rio Now() implementation to use the desktop timestamp.
+ * No effect on non-Rio platforms. This should only be used for testing
+ * purposes if the HAL is not available.
+ */
+void SetupNowDefaultOnRio();
+
+/**
  * Initialize the on-Rio Now() implementation to use the FPGA timestamp.
  * No effect on non-Rio platforms. This is called by HAL_Initialize() and
  * thus should generally not be called by user code.
  */
-void SetupNowRio();
+#ifdef __FRC_ROBORIO__
+template <typename T>
+void SetupNowRio(void* chipObjectLibrary, std::unique_ptr<T> hmbObject);
+#else
+template <typename T>
+inline void SetupNowRio(void*, std::unique_ptr<T>) {}
+#endif
+
+/**
+ * Initialize the on-Rio Now() implementation to use the FPGA timestamp.
+ * No effect on non-Rio platforms. This take an FPGA session that has
+ * already been initialized, and is used from LabVIEW.
+ */
+void SetupNowRio(uint32_t session);
 
 /**
  * De-initialize the on-Rio Now() implementation. No effect on non-Rio
diff --git a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h
index 1fe1388..915d895 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h
@@ -1435,7 +1435,7 @@
   // Only map owning types because mapping views can be unsafe.
   template <typename T, typename U = format_as_t<T>,
             FMT_ENABLE_IF(std::is_arithmetic<U>::value)>
-  FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(this->map(U())) {
+  FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(map(U())) {
     return map(format_as(val));
   }
 
@@ -1459,13 +1459,13 @@
                           !is_string<U>::value && !is_char<U>::value &&
                           !is_named_arg<U>::value &&
                           !std::is_arithmetic<format_as_t<U>>::value)>
-  FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(this->do_map(val)) {
+  FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(do_map(val)) {
     return do_map(val);
   }
 
   template <typename T, FMT_ENABLE_IF(is_named_arg<T>::value)>
   FMT_CONSTEXPR FMT_INLINE auto map(const T& named_arg)
-      -> decltype(this->map(named_arg.value)) {
+      -> decltype(map(named_arg.value)) {
     return map(named_arg.value);
   }
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp
index 72cdf09..3ee53a2 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp
@@ -230,6 +230,7 @@
     fs::file_t f, std::string_view bufferName, std::error_code& ec) {
   constexpr size_t ChunkSize = 4096 * 4;
   SmallVector<uint8_t, ChunkSize> buffer;
+  uint64_t size = 0;
 #ifdef _WIN32
   DWORD readBytes;
 #else
@@ -237,23 +238,23 @@
 #endif
   // Read into Buffer until we hit EOF.
   do {
-    size_t prevSize = buffer.size();
-    buffer.resize_for_overwrite(prevSize + ChunkSize);
+    buffer.resize_for_overwrite(size + ChunkSize);
 #ifdef _WIN32
-    if (!ReadFile(f, buffer.begin() + prevSize, ChunkSize, &readBytes,
-                  nullptr)) {
+    if (!ReadFile(f, buffer.begin() + size, ChunkSize, &readBytes, nullptr)) {
       ec = mapWindowsError(GetLastError());
       return nullptr;
     }
 #else
-    readBytes = sys::RetryAfterSignal(-1, ::read, f, buffer.begin() + prevSize,
+    readBytes = sys::RetryAfterSignal(-1, ::read, f, buffer.begin() + size,
                                       ChunkSize);
     if (readBytes == -1) {
       ec = std::error_code(errno, std::generic_category());
       return nullptr;
     }
 #endif
+    size += readBytes;
   } while (readBytes != 0);
+  buffer.truncate(size);
 
   return GetMemBufferCopyImpl(buffer, bufferName, ec);
 }
@@ -370,7 +371,7 @@
 
       // If this not a file or a block device (e.g. it's a named pipe
       // or character device), we can't mmap it, so error out.
-      if (status.st_mode != S_IFREG && status.st_mode != S_IFBLK) {
+      if (!S_ISREG(status.st_mode) && !S_ISBLK(status.st_mode)) {
         ec = make_error_code(errc::invalid_argument);
         return nullptr;
       }
@@ -433,7 +434,7 @@
       // If this not a file or a block device (e.g. it's a named pipe
       // or character device), we can't trust the size. Create the memory
       // buffer by copying off the stream.
-      if (status.st_mode != S_IFREG && status.st_mode != S_IFBLK) {
+      if (!S_ISREG(status.st_mode) && !S_ISBLK(status.st_mode)) {
         return GetMemoryBufferForStream(f, filename, ec);
       }
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/MemoryBuffer.h b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/MemoryBuffer.h
index 7907c07..b5eaea4 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/MemoryBuffer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/MemoryBuffer.h
@@ -62,6 +62,11 @@
 
   std::span<const uint8_t> GetBuffer() const { return {begin(), end()}; }
 
+  std::span<const char> GetCharBuffer() const {
+    return {reinterpret_cast<const char*>(begin()),
+            reinterpret_cast<const char*>(end())};
+  }
+
   /// Return an identifier for this buffer, typically the filename it was read
   /// from.
   virtual std::string_view GetBufferIdentifier() const {
@@ -145,6 +150,10 @@
   uint8_t* begin() { return const_cast<uint8_t*>(MemoryBuffer::begin()); }
   uint8_t* end() { return const_cast<uint8_t*>(MemoryBuffer::end()); }
   std::span<uint8_t> GetBuffer() { return {begin(), end()}; }
+  std::span<char> GetCharBuffer() const {
+    return {reinterpret_cast<char*>(const_cast<uint8_t*>(begin())),
+            reinterpret_cast<char*>(const_cast<uint8_t*>(end()))};
+  }
 
   static std::unique_ptr<WritableMemoryBuffer> GetFile(
       std::string_view filename, std::error_code& ec, int64_t fileSize = -1);
@@ -196,6 +205,10 @@
   uint8_t* begin() { return const_cast<uint8_t*>(MemoryBuffer::begin()); }
   uint8_t* end() { return const_cast<uint8_t*>(MemoryBuffer::end()); }
   std::span<uint8_t> GetBuffer() { return {begin(), end()}; }
+  std::span<char> GetCharBuffer() const {
+    return {reinterpret_cast<char*>(const_cast<uint8_t*>(begin())),
+            reinterpret_cast<char*>(const_cast<uint8_t*>(end()))};
+  }
 
   static std::unique_ptr<WriteThroughMemoryBuffer> GetFile(
       std::string_view filename, std::error_code& ec, int64_t fileSize = -1);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/type_traits.h b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/type_traits.h
index d74fd0f..5166f2a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/type_traits.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/llvm/include/wpi/type_traits.h
@@ -76,6 +76,11 @@
 
 } // end namespace detail
 
+// https://stackoverflow.com/questions/55288555/c-check-if-statement-can-be-evaluated-constexpr
+template<class Lambda, int=(Lambda{}(), 0)>
+constexpr bool is_constexpr(Lambda) { return true; }
+constexpr bool is_constexpr(...) { return false; }
+
 } // end namespace wpi
 
 #endif // WPIUTIL_WPI_TYPE_TRAITS_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/include/google/protobuf/io/coded_stream.h b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/include/google/protobuf/io/coded_stream.h
index 6c0dd4a..a102cec 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/include/google/protobuf/io/coded_stream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/include/google/protobuf/io/coded_stream.h
@@ -681,7 +681,14 @@
     if (PROTOBUF_PREDICT_FALSE(end_ - ptr < static_cast<int>(size))) {
       return WriteRawFallback(data, size, ptr);
     }
+#if __GNUC__ >= 13
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstringop-overflow="
+#endif  // __GNUC__ >= 13
     std::memcpy(ptr, data, size);
+#if __GNUC__ >= 13
+#pragma GCC diagnostic pop
+#endif  // __GNUC__ >= 13
     return ptr + size;
   }
   // Writes the buffer specified by data, size to the stream. Possibly by
diff --git a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/src/unknown_field_set.cpp b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/src/unknown_field_set.cpp
index 74c358e..c058735 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/src/unknown_field_set.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/thirdparty/protobuf/src/unknown_field_set.cpp
@@ -96,9 +96,16 @@
   if (fields_.empty()) {
     fields_ = std::move(other->fields_);
   } else {
+#if __GNUC__ >= 13
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstringop-overflow="
+#endif  // __GNUC__ >= 13
     fields_.insert(fields_.end(),
                    std::make_move_iterator(other->fields_.begin()),
                    std::make_move_iterator(other->fields_.end()));
+#if __GNUC__ >= 13
+#pragma GCC diagnostic pop
+#endif  // __GNUC__ >= 13
   }
   other->fields_.clear();
 }
diff --git a/third_party/allwpilib/wpiutil/src/printlog/java/printlog/PrintLog.java b/third_party/allwpilib/wpiutil/src/printlog/java/printlog/PrintLog.java
index e9339b6..b3d972b 100644
--- a/third_party/allwpilib/wpiutil/src/printlog/java/printlog/PrintLog.java
+++ b/third_party/allwpilib/wpiutil/src/printlog/java/printlog/PrintLog.java
@@ -10,9 +10,9 @@
 import java.time.LocalDateTime;
 import java.time.ZoneOffset;
 import java.time.format.DateTimeFormatter;
-import java.util.Arrays;
 import java.util.HashMap;
 import java.util.InputMismatchException;
+import java.util.List;
 import java.util.Map;
 
 public final class PrintLog {
@@ -133,11 +133,11 @@
           } else if ("boolean".equals(entry.type)) {
             System.out.println("  " + record.getBoolean());
           } else if ("double[]".equals(entry.type)) {
-            System.out.println("  " + Arrays.asList(record.getDoubleArray()));
+            System.out.println("  " + List.of(record.getDoubleArray()));
           } else if ("int64[]".equals(entry.type)) {
-            System.out.println("  " + Arrays.asList(record.getIntegerArray()));
+            System.out.println("  " + List.of(record.getIntegerArray()));
           } else if ("string[]".equals(entry.type)) {
-            System.out.println("  " + Arrays.asList(record.getStringArray()));
+            System.out.println("  " + List.of(record.getStringArray()));
           }
         } catch (InputMismatchException ex) {
           System.out.println("  invalid");
diff --git a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java
index 7972754..a040ba5 100644
--- a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java
+++ b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java
@@ -21,7 +21,7 @@
 
   @Test
   void addFirstTest() {
-    CircularBuffer queue = new CircularBuffer(8);
+    var queue = new CircularBuffer<Double>(8);
 
     for (double value : m_values) {
       queue.addFirst(value);
@@ -34,7 +34,7 @@
 
   @Test
   void addLastTest() {
-    CircularBuffer queue = new CircularBuffer(8);
+    var queue = new CircularBuffer<Double>(8);
 
     for (double value : m_values) {
       queue.addLast(value);
@@ -47,7 +47,7 @@
 
   @Test
   void pushPopTest() {
-    CircularBuffer queue = new CircularBuffer(3);
+    var queue = new CircularBuffer<Double>(3);
 
     // Insert three elements into the buffer
     queue.addLast(1.0);
@@ -91,22 +91,20 @@
 
   @Test
   void resetTest() {
-    CircularBuffer queue = new CircularBuffer(5);
+    var queue = new CircularBuffer<Double>(5);
 
     for (int i = 0; i < 6; i++) {
-      queue.addLast(i);
+      queue.addLast((double) i);
     }
 
     queue.clear();
 
-    for (int i = 0; i < 5; i++) {
-      assertEquals(0.0, queue.get(i), 0.00005);
-    }
+    assertEquals(0, queue.size());
   }
 
   @Test
   void resizeTest() {
-    CircularBuffer queue = new CircularBuffer(5);
+    var queue = new CircularBuffer<Double>(5);
 
     /* Buffer contains {1, 2, 3, _, _}
      *                  ^ front
diff --git a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java
new file mode 100644
index 0000000..58a39f4
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java
@@ -0,0 +1,213 @@
+// 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.
+
+package edu.wpi.first.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class DoubleCircularBufferTest {
+  private final double[] m_values = {
+    751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913
+  };
+  private final double[] m_addFirstOut = {
+    799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657
+  };
+  private final double[] m_addLastOut = {
+    342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913
+  };
+
+  @Test
+  void addFirstTest() {
+    var queue = new DoubleCircularBuffer(8);
+
+    for (double value : m_values) {
+      queue.addFirst(value);
+    }
+
+    for (int i = 0; i < m_addFirstOut.length; i++) {
+      assertEquals(m_addFirstOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void addLastTest() {
+    var queue = new DoubleCircularBuffer(8);
+
+    for (double value : m_values) {
+      queue.addLast(value);
+    }
+
+    for (int i = 0; i < m_addLastOut.length; i++) {
+      assertEquals(m_addLastOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void pushPopTest() {
+    var queue = new DoubleCircularBuffer(3);
+
+    // Insert three elements into the buffer
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+    assertEquals(3.0, queue.get(2), 0.00005);
+
+    /*
+     * The buffer is full now, so pushing subsequent elements will overwrite the
+     * front-most elements.
+     */
+
+    queue.addLast(4.0); // Overwrite 1 with 4
+
+    // The buffer now contains 2, 3, and 4
+    assertEquals(2.0, queue.get(0), 0.00005);
+    assertEquals(3.0, queue.get(1), 0.00005);
+    assertEquals(4.0, queue.get(2), 0.00005);
+
+    queue.addLast(5.0); // Overwrite 2 with 5
+
+    // The buffer now contains 3, 4, and 5
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+    assertEquals(5.0, queue.get(2), 0.00005);
+
+    assertEquals(5.0, queue.removeLast(), 0.00005); // 5 is removed
+
+    // The buffer now contains 3 and 4
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+
+    assertEquals(3.0, queue.removeFirst(), 0.00005); // 3 is removed
+
+    // Leaving only one element with value == 4
+    assertEquals(4.0, queue.get(0), 0.00005);
+  }
+
+  @Test
+  void resetTest() {
+    var queue = new DoubleCircularBuffer(5);
+
+    for (int i = 0; i < 6; i++) {
+      queue.addLast(i);
+    }
+
+    queue.clear();
+
+    for (int i = 0; i < 5; i++) {
+      assertEquals(0.0, queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void resizeTest() {
+    var queue = new DoubleCircularBuffer(5);
+
+    /* Buffer contains {1, 2, 3, _, _}
+     *                  ^ front
+     */
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {_, 1, 2, 3, _}
+     *                     ^ front
+     */
+    queue.addLast(0.0);
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.removeFirst();
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {_, _, 1, 2, 3}
+     *                        ^ front
+     */
+    queue.addLast(0.0);
+    queue.addLast(0.0);
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.removeFirst();
+    queue.removeFirst();
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {3, _, _, 1, 2}
+     *                           ^ front
+     */
+    queue.addLast(3.0);
+    queue.addFirst(2.0);
+    queue.addFirst(1.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {2, 3, _, _, 1}
+     *                              ^ front
+     */
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.addFirst(1.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    // Test addLast() after resize
+    queue.addLast(3.0);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+    assertEquals(3.0, queue.get(2), 0.00005);
+
+    // Test addFirst() after resize
+    queue.addFirst(4.0);
+    assertEquals(4.0, queue.get(0), 0.00005);
+    assertEquals(1.0, queue.get(1), 0.00005);
+    assertEquals(2.0, queue.get(2), 0.00005);
+    assertEquals(3.0, queue.get(3), 0.00005);
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java
index 52ba8da..b985b2a 100644
--- a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java
+++ b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java
@@ -15,21 +15,21 @@
   @Test
   void testEmpty() {
     Parser p = new Parser("");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertTrue(schema.declarations.isEmpty());
   }
 
   @Test
   void testEmptySemicolon() {
     Parser p = new Parser(";");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertTrue(schema.declarations.isEmpty());
   }
 
   @Test
   void testSimple() {
     Parser p = new Parser("int32 a");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -40,14 +40,14 @@
   @Test
   void testSimpleTrailingSemi() {
     Parser p = new Parser("int32 a;");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
   }
 
   @Test
   void testArray() {
     Parser p = new Parser("int32 a[2]");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -58,14 +58,14 @@
   @Test
   void testArrayTrailingSemi() {
     Parser p = new Parser("int32 a[2];");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
   }
 
   @Test
   void testBitfield() {
     Parser p = new Parser("int32 a:2");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -76,14 +76,14 @@
   @Test
   void testBitfieldTrailingSemi() {
     Parser p = new Parser("int32 a:2;");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
   }
 
   @Test
   void testEnumKeyword() {
     Parser p = new Parser("enum {x=1} int32 a;");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -95,7 +95,7 @@
   @Test
   void testEnumNoKeyword() {
     Parser p = new Parser("{x=1} int32 a;");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -107,7 +107,7 @@
   @Test
   void testEnumNoValues() {
     Parser p = new Parser("{} int32 a;");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -118,7 +118,7 @@
   @Test
   void testEnumMultipleValues() {
     Parser p = new Parser("{x=1,y=-2} int32 a;");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -131,7 +131,7 @@
   @Test
   void testEnumTrailingComma() {
     Parser p = new Parser("{x=1,y=2,} int32 a;");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 1);
     var decl = schema.declarations.get(0);
     assertEquals(decl.typeString, "int32");
@@ -144,7 +144,7 @@
   @Test
   void testMultipleNoTrailingSemi() {
     Parser p = new Parser("int32 a; int16 b");
-    ParsedSchema schema = assertDoesNotThrow(() -> p.parse());
+    ParsedSchema schema = assertDoesNotThrow(p::parse);
     assertEquals(schema.declarations.size(), 2);
     assertEquals(schema.declarations.get(0).typeString, "int32");
     assertEquals(schema.declarations.get(0).name, "a");
@@ -155,58 +155,55 @@
   @Test
   void testErrBitfieldArray() {
     Parser p = new Parser("int32 a[1]:2");
-    assertThrows(ParseException.class, () -> p.parse(), "10: expected ';', got ':'");
+    assertThrows(ParseException.class, p::parse, "10: expected ';', got ':'");
   }
 
   @Test
   void testErrNoArrayValue() {
     Parser p = new Parser("int32 a[]");
-    assertThrows(ParseException.class, () -> p.parse(), "8: expected integer, got ']'");
+    assertThrows(ParseException.class, p::parse, "8: expected integer, got ']'");
   }
 
   @Test
   void testErrNoBitfieldValue() {
     Parser p = new Parser("int32 a:");
-    assertThrows(ParseException.class, () -> p.parse(), "8: expected integer, got ''");
+    assertThrows(ParseException.class, p::parse, "8: expected integer, got ''");
   }
 
   @Test
   void testErrNoNameArray() {
     Parser p = new Parser("int32 [2]");
-    assertThrows(ParseException.class, () -> p.parse(), "6: expected identifier, got '['");
+    assertThrows(ParseException.class, p::parse, "6: expected identifier, got '['");
   }
 
   @Test
   void testErrNoNameBitField() {
     Parser p = new Parser("int32 :2");
-    assertThrows(ParseException.class, () -> p.parse(), "6: expected identifier, got ':'");
+    assertThrows(ParseException.class, p::parse, "6: expected identifier, got ':'");
   }
 
   @Test
   void testNegativeBitField() {
     Parser p = new Parser("int32 a:-1");
     assertThrows(
-        ParseException.class, () -> p.parse(), "8: bitfield width '-1' is not a positive integer");
+        ParseException.class, p::parse, "8: bitfield width '-1' is not a positive integer");
   }
 
   @Test
   void testNegativeArraySize() {
     Parser p = new Parser("int32 a[-1]");
-    assertThrows(
-        ParseException.class, () -> p.parse(), "8: array size '-1' is not a positive integer");
+    assertThrows(ParseException.class, p::parse, "8: array size '-1' is not a positive integer");
   }
 
   @Test
   void testZeroBitField() {
     Parser p = new Parser("int32 a:0");
-    assertThrows(
-        ParseException.class, () -> p.parse(), "8: bitfield width '0' is not a positive integer");
+    assertThrows(ParseException.class, p::parse, "8: bitfield width '0' is not a positive integer");
   }
 
   @Test
   void testZeroArraySize() {
     Parser p = new Parser("int32 a[0]");
-    assertThrows(
-        ParseException.class, () -> p.parse(), "8: array size '0' is not a positive integer");
+    assertThrows(ParseException.class, p::parse, "8: array size '0' is not a positive integer");
   }
 }
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/ArrayTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/ArrayTest.cpp
index f70b201..d3a12a4 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/ArrayTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/ArrayTest.cpp
@@ -16,20 +16,17 @@
 }  // namespace
 
 TEST(ArrayTest, CopyableTypeCompiles) {
-  constexpr wpi::array<int, 3> arr1{1, 2, 3};
-  static_cast<void>(arr1);
+  [[maybe_unused]] constexpr wpi::array<int, 3> arr1{1, 2, 3};
 
   // Test deduction guide
-  constexpr wpi::array arr2{1, 2, 3};
-  static_cast<void>(arr2);
+  [[maybe_unused]] constexpr wpi::array arr2{1, 2, 3};
 }
 
 TEST(ArrayTest, MoveOnlyTypeCompiles) {
-  constexpr wpi::array<MoveOnlyType, 3> arr1{MoveOnlyType{}, MoveOnlyType{},
-                                             MoveOnlyType{}};
-  static_cast<void>(arr1);
+  [[maybe_unused]] constexpr wpi::array<MoveOnlyType, 3> arr1{
+      MoveOnlyType{}, MoveOnlyType{}, MoveOnlyType{}};
 
   // Test deduction guide
-  constexpr wpi::array arr2{MoveOnlyType{}, MoveOnlyType{}, MoveOnlyType{}};
-  static_cast<void>(arr2);
+  [[maybe_unused]] constexpr wpi::array arr2{MoveOnlyType{}, MoveOnlyType{},
+                                             MoveOnlyType{}};
 }
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/DataLogTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/DataLogTest.cpp
new file mode 100644
index 0000000..d06ad33
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/DataLogTest.cpp
@@ -0,0 +1,249 @@
+// 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 <array>
+
+#include <gtest/gtest.h>
+
+#include "wpi/DataLog.h"
+
+namespace {
+struct ThingA {
+  int x = 0;
+};
+
+struct ThingB {
+  int x = 0;
+};
+
+struct ThingC {
+  int x = 0;
+};
+
+struct Info1 {
+  int info = 0;
+};
+
+struct Info2 {
+  int info = 0;
+};
+}  // namespace
+
+template <>
+struct wpi::Struct<ThingA> {
+  static constexpr std::string_view GetTypeString() { return "struct:ThingA"; }
+  static constexpr size_t GetSize() { return 1; }
+  static constexpr std::string_view GetSchema() { return "uint8 value"; }
+  static ThingA Unpack(std::span<const uint8_t> data) {
+    return ThingA{.x = data[0]};
+  }
+  static void Pack(std::span<uint8_t> data, const ThingA& value) {
+    data[0] = value.x;
+  }
+};
+
+template <>
+struct wpi::Struct<ThingB, Info1> {
+  static constexpr std::string_view GetTypeString(const Info1&) {
+    return "struct:ThingB";
+  }
+  static constexpr size_t GetSize(const Info1&) { return 1; }
+  static constexpr std::string_view GetSchema(const Info1&) {
+    return "uint8 value";
+  }
+  static ThingB Unpack(std::span<const uint8_t> data, const Info1&) {
+    return ThingB{.x = data[0]};
+  }
+  static void Pack(std::span<uint8_t> data, const ThingB& value, const Info1&) {
+    data[0] = value.x;
+  }
+};
+
+template <>
+struct wpi::Struct<ThingC> {
+  static constexpr std::string_view GetTypeString() { return "struct:ThingC"; }
+  static constexpr size_t GetSize() { return 1; }
+  static constexpr std::string_view GetSchema() { return "uint8 value"; }
+  static ThingC Unpack(std::span<const uint8_t> data) {
+    return ThingC{.x = data[0]};
+  }
+  static void Pack(std::span<uint8_t> data, const ThingC& value) {
+    data[0] = value.x;
+  }
+};
+
+template <>
+struct wpi::Struct<ThingC, Info1> {
+  static constexpr std::string_view GetTypeString(const Info1&) {
+    return "struct:ThingC";
+  }
+  static constexpr size_t GetSize(const Info1&) { return 1; }
+  static constexpr std::string_view GetSchema(const Info1&) {
+    return "uint8 value";
+  }
+  static ThingC Unpack(std::span<const uint8_t> data, const Info1&) {
+    return ThingC{.x = data[0]};
+  }
+  static void Pack(std::span<uint8_t> data, const ThingC& value, const Info1&) {
+    data[0] = value.x;
+  }
+};
+
+template <>
+struct wpi::Struct<ThingC, Info2> {
+  static constexpr std::string_view GetTypeString(const Info2&) {
+    return "struct:ThingC";
+  }
+  static constexpr size_t GetSize(const Info2&) { return 1; }
+  static constexpr std::string_view GetSchema(const Info2&) {
+    return "uint8 value";
+  }
+  static ThingC Unpack(std::span<const uint8_t> data, const Info2&) {
+    return ThingC{.x = data[0]};
+  }
+  static void Pack(std::span<uint8_t> data, const ThingC& value, const Info2&) {
+    data[0] = value.x;
+  }
+};
+
+static_assert(wpi::StructSerializable<ThingA>);
+static_assert(!wpi::StructSerializable<ThingA, Info1>);
+
+static_assert(!wpi::StructSerializable<ThingB>);
+static_assert(wpi::StructSerializable<ThingB, Info1>);
+static_assert(!wpi::StructSerializable<ThingB, Info2>);
+
+static_assert(wpi::StructSerializable<ThingC>);
+static_assert(wpi::StructSerializable<ThingC, Info1>);
+static_assert(wpi::StructSerializable<ThingC, Info2>);
+
+TEST(DataLogTest, SimpleInt) {
+  std::vector<uint8_t> data;
+  {
+    wpi::log::DataLog log{
+        [&](auto out) { data.insert(data.end(), out.begin(), out.end()); }};
+    int entry = log.Start("test", "int64");
+    log.AppendInteger(entry, 1, 0);
+  }
+  ASSERT_EQ(data.size(), 66u);
+}
+
+TEST(DataLogTest, StructA) {
+  wpi::log::DataLog log{[](auto) {}};
+  [[maybe_unused]] wpi::log::StructLogEntry<ThingA> entry0;
+  wpi::log::StructLogEntry<ThingA> entry{log, "a", 5};
+  entry.Append(ThingA{});
+  entry.Append(ThingA{}, 7);
+}
+
+TEST(DataLogTest, StructArrayA) {
+  wpi::log::DataLog log{[](auto) {}};
+  [[maybe_unused]] wpi::log::StructArrayLogEntry<ThingA> entry0;
+  wpi::log::StructArrayLogEntry<ThingA> entry{log, "a", 5};
+  entry.Append({{ThingA{}, ThingA{}}});
+  entry.Append({{ThingA{}, ThingA{}}}, 7);
+}
+
+TEST(DataLogTest, StructFixedArrayA) {
+  wpi::log::DataLog log{[](auto) {}};
+  [[maybe_unused]] wpi::log::StructArrayLogEntry<std::array<ThingA, 2>> entry0;
+  wpi::log::StructLogEntry<std::array<ThingA, 2>> entry{log, "a", 5};
+  std::array<ThingA, 2> arr;
+  entry.Append(arr);
+  entry.Append(arr, 7);
+}
+
+TEST(DataLogTest, StructB) {
+  wpi::log::DataLog log{[](auto) {}};
+  Info1 info;
+  [[maybe_unused]] wpi::log::StructLogEntry<ThingB, Info1> entry0;
+  wpi::log::StructLogEntry<ThingB, Info1> entry{log, "b", info, 5};
+  entry.Append(ThingB{});
+  entry.Append(ThingB{}, 7);
+}
+
+TEST(DataLogTest, StructArrayB) {
+  wpi::log::DataLog log{[](auto) {}};
+  Info1 info;
+  [[maybe_unused]] wpi::log::StructArrayLogEntry<ThingB, Info1> entry0;
+  wpi::log::StructArrayLogEntry<ThingB, Info1> entry{log, "a", info, 5};
+  entry.Append({{ThingB{}, ThingB{}}});
+  entry.Append({{ThingB{}, ThingB{}}}, 7);
+}
+
+TEST(DataLogTest, StructFixedArrayB) {
+  wpi::log::DataLog log{[](auto) {}};
+  Info1 info;
+  wpi::log::StructLogEntry<std::array<ThingB, 2>, Info1> entry{log, "a", info,
+                                                               5};
+  std::array<ThingB, 2> arr;
+  entry.Append(arr);
+  entry.Append(arr, 7);
+}
+
+TEST(DataLogTest, StructC) {
+  wpi::log::DataLog log{[](auto) {}};
+  {
+    wpi::log::StructLogEntry<ThingC> entry{log, "c", 5};
+    entry.Append(ThingC{});
+    entry.Append(ThingC{}, 7);
+  }
+  {
+    Info1 info;
+    wpi::log::StructLogEntry<ThingC, Info1> entry{log, "c1", info, 5};
+    entry.Append(ThingC{});
+    entry.Append(ThingC{}, 7);
+  }
+  {
+    Info2 info;
+    wpi::log::StructLogEntry<ThingC, Info2> entry{log, "c2", info, 5};
+    entry.Append(ThingC{});
+    entry.Append(ThingC{}, 7);
+  }
+}
+
+TEST(DataLogTest, StructArrayC) {
+  wpi::log::DataLog log{[](auto) {}};
+  {
+    wpi::log::StructArrayLogEntry<ThingC> entry{log, "c", 5};
+    entry.Append({{ThingC{}, ThingC{}}});
+    entry.Append({{ThingC{}, ThingC{}}}, 7);
+  }
+  {
+    Info1 info;
+    wpi::log::StructArrayLogEntry<ThingC, Info1> entry{log, "c1", info, 5};
+    entry.Append({{ThingC{}, ThingC{}}});
+    entry.Append({{ThingC{}, ThingC{}}}, 7);
+  }
+  {
+    Info2 info;
+    wpi::log::StructArrayLogEntry<ThingC, Info2> entry{log, "c2", info, 5};
+    entry.Append({{ThingC{}, ThingC{}}});
+    entry.Append({{ThingC{}, ThingC{}}}, 7);
+  }
+}
+
+TEST(DataLogTest, StructFixedArrayC) {
+  wpi::log::DataLog log{[](auto) {}};
+  std::array<ThingC, 2> arr;
+  {
+    wpi::log::StructLogEntry<std::array<ThingC, 2>> entry{log, "c", 5};
+    entry.Append(arr);
+    entry.Append(arr, 7);
+  }
+  {
+    Info1 info;
+    wpi::log::StructLogEntry<std::array<ThingC, 2>, Info1> entry{log, "c1",
+                                                                 info, 5};
+    entry.Append(arr);
+    entry.Append(arr, 7);
+  }
+  {
+    Info2 info;
+    wpi::log::StructLogEntry<std::array<ThingC, 2>, Info2> entry{log, "c2",
+                                                                 info, 5};
+    entry.Append(arr);
+    entry.Append(arr, 7);
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in b/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in
index 3f696c8..3926ecb 100644
--- a/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in
+++ b/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in
@@ -2,6 +2,7 @@
 @FILENAME_DEP_REPLACE@
 set(THREADS_PREFER_PTHREAD_FLAG ON)
 find_dependency(Threads)
+find_dependency(Protobuf)
 @FMTLIB_SYSTEM_REPLACE@
 
 if(@USE_SYSTEM_FMTLIB@)
@@ -10,3 +11,6 @@
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpiutil.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/wpiutil_jar.cmake)
+endif()
diff --git a/third_party/allwpilib/xrpVendordep/CMakeLists.txt b/third_party/allwpilib/xrpVendordep/CMakeLists.txt
index 40d40a7..52cd2e4 100644
--- a/third_party/allwpilib/xrpVendordep/CMakeLists.txt
+++ b/third_party/allwpilib/xrpVendordep/CMakeLists.txt
@@ -4,38 +4,50 @@
 include(CompileWarnings)
 include(AddTest)
 
-if (WITH_JAVA)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-  add_jar(xrpVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME xrpVendordep)
+    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+    add_jar(
+        xrpVendordep_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS
+            hal_jar
+            ntcore_jar
+            cscore_jar
+            cameraserver_jar
+            wpimath_jar
+            wpiunits_jar
+            wpiutil_jar
+            wpilibj_jar
+        OUTPUT_NAME xrpVendordep
+    )
 
-  get_property(xrpVendordep_JAR_FILE TARGET xrpVendordep_jar PROPERTY JAR_FILE)
-  install(FILES ${xrpVendordep_JAR_FILE} DESTINATION "${java_lib_dest}")
-
-  set_property(TARGET xrpVendordep_jar PROPERTY FOLDER "java")
-
-  if (WITH_FLAT_INSTALL)
-      set (xrpVendordep_config_dir ${wpilib_dest})
-  else()
-      set (xrpVendordep_config_dir share/xrpVendordep)
-  endif()
+    install_jar(xrpVendordep_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(
+        TARGETS xrpVendordep_jar
+        FILE xrpVendordep_jar.cmake
+        DESTINATION share/xrpVendordep
+    )
 endif()
 
-if (WITH_JAVA_SOURCE)
-  find_package(Java REQUIRED)
-  include(UseJava)
-  file(GLOB XRPVENDORDEP_SOURCES src/main/java/edu/wpi/first/wpilibj/xrp/*.java)
-  add_jar(xrpVendordep_src_jar
-  RESOURCES NAMESPACE "edu/wpi/first/wpilibj/xrp" ${XRPVENDORDEP_SOURCES}
-  OUTPUT_NAME xrpVendordep-sources)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB XRPVENDORDEP_SOURCES src/main/java/edu/wpi/first/wpilibj/xrp/*.java)
+    add_jar(
+        xrpVendordep_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/wpilibj/xrp" ${XRPVENDORDEP_SOURCES}
+        OUTPUT_NAME xrpVendordep-sources
+    )
 
-  get_property(xrpVendordep_src_JAR_FILE TARGET xrpVendordep_src_jar PROPERTY JAR_FILE)
-  install(FILES ${xrpVendordep_src_JAR_FILE} DESTINATION "${java_lib_dest}")
+    get_property(xrpVendordep_src_JAR_FILE TARGET xrpVendordep_src_jar PROPERTY JAR_FILE)
+    install(FILES ${xrpVendordep_src_JAR_FILE} DESTINATION "${java_lib_dest}")
 
-  set_property(TARGET xrpVendordep_src_jar PROPERTY FOLDER "java")
+    set_property(TARGET xrpVendordep_src_jar PROPERTY FOLDER "java")
 endif()
 
 file(GLOB_RECURSE xrpVendordep_native_src src/main/native/cpp/*.cpp)
@@ -47,25 +59,22 @@
 wpilib_target_warnings(xrpVendordep)
 target_link_libraries(xrpVendordep wpilibc)
 
-target_include_directories(xrpVendordep PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/xrpVendordep>)
+target_include_directories(
+    xrpVendordep
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/xrpVendordep>
+)
 
-install(TARGETS xrpVendordep EXPORT xrpVendordep DESTINATION "${main_lib_dest}")
+install(TARGETS xrpVendordep EXPORT xrpvendordep DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/xrpVendordep")
 
-if (FLAT_INSTALL_WPILIB)
-     set(xrpVendordep_config_dir ${wpilib_dest})
- else()
-     set(xrpVendordep_config_dir share/xrpVendordep)
- endif()
+configure_file(xrpvendordep-config.cmake.in ${WPILIB_BINARY_DIR}/xrpvendordep-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/xrpvendordep-config.cmake DESTINATION share/xrpVendordep)
+install(EXPORT xrpvendordep DESTINATION share/xrpVendordep)
 
- configure_file(xrpVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake)
- install(FILES ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake DESTINATION ${xrpVendordep_config_dir})
- install(EXPORT xrpVendordep DESTINATION ${xrpVendordep_config_dir})
-
- if (WITH_TESTS)
-     wpilib_add_test(xrpVendordep src/test/native/cpp)
-     target_include_directories(xrpVendordep_test PRIVATE src/test/native/include)
-     target_link_libraries(xrpVendordep_test xrpVendordep gmock_main)
- endif()
+if(WITH_TESTS)
+    wpilib_add_test(xrpVendordep src/test/native/cpp)
+    target_include_directories(xrpVendordep_test PRIVATE src/test/native/include)
+    target_link_libraries(xrpVendordep_test xrpVendordep gmock_main)
+endif()
diff --git a/third_party/allwpilib/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPMotor.java b/third_party/allwpilib/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPMotor.java
index 957f9e7..6db4fc2 100644
--- a/third_party/allwpilib/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPMotor.java
+++ b/third_party/allwpilib/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPMotor.java
@@ -17,6 +17,7 @@
  *
  * <p>A SimDevice based motor controller representing the motors on an XRP robot
  */
+@SuppressWarnings("removal")
 public class XRPMotor implements MotorController {
   private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
   private static HashSet<Integer> s_registeredDevices = new HashSet<>();
diff --git a/third_party/allwpilib/xrpVendordep/src/main/native/cpp/xrp/XRPMotor.cpp b/third_party/allwpilib/xrpVendordep/src/main/native/cpp/xrp/XRPMotor.cpp
index 02d1d58..6d16f7b 100644
--- a/third_party/allwpilib/xrpVendordep/src/main/native/cpp/xrp/XRPMotor.cpp
+++ b/third_party/allwpilib/xrpVendordep/src/main/native/cpp/xrp/XRPMotor.cpp
@@ -6,6 +6,8 @@
 
 #include <frc/Errors.h>
 
+#include <wpi/deprecated.h>
+
 using namespace frc;
 
 std::map<int, std::string> XRPMotor::s_simDeviceMap = {
@@ -27,6 +29,8 @@
   s_registeredDevices.insert(deviceNum);
 }
 
+WPI_IGNORE_DEPRECATED
+
 XRPMotor::XRPMotor(int deviceNum) {
   CheckDeviceAllocation(deviceNum);
 
@@ -42,6 +46,8 @@
   }
 }
 
+WPI_UNIGNORE_DEPRECATED
+
 void XRPMotor::Set(double speed) {
   if (m_simSpeed) {
     bool invert = false;
diff --git a/third_party/allwpilib/xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h b/third_party/allwpilib/xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h
index c174cbf..8a220c0 100644
--- a/third_party/allwpilib/xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h
+++ b/third_party/allwpilib/xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h
@@ -12,9 +12,12 @@
 #include <string>
 
 #include <hal/SimDevice.h>
+#include <wpi/deprecated.h>
 
 namespace frc {
 
+WPI_IGNORE_DEPRECATED
+
 class XRPMotor : public frc::MotorController, public frc::MotorSafety {
  public:
   explicit XRPMotor(int deviceNum);
@@ -43,4 +46,6 @@
   static void CheckDeviceAllocation(int deviceNum);
 };
 
+WPI_UNIGNORE_DEPRECATED
+
 }  // namespace frc
diff --git a/third_party/allwpilib/xrpVendordep/xrpVendordep-config.cmake.in b/third_party/allwpilib/xrpVendordep/xrpVendordep-config.cmake.in
deleted file mode 100644
index ad06e4c..0000000
--- a/third_party/allwpilib/xrpVendordep/xrpVendordep-config.cmake.in
+++ /dev/null
@@ -1,11 +0,0 @@
-include(CMakeFindDependencyMacro)
- @WPIUTIL_DEP_REPLACE@
- @NTCORE_DEP_REPLACE@
- @CSCORE_DEP_REPLACE@
- @CAMERASERVER_DEP_REPLACE@
- @HAL_DEP_REPLACE@
- @WPILIBC_DEP_REPLACE@
- @WPIMATH_DEP_REPLACE@
-
- @FILENAME_DEP_REPLACE@
- include(${SELF_DIR}/xrpVendordep.cmake)
diff --git a/third_party/allwpilib/xrpVendordep/xrpvendordep-config.cmake.in b/third_party/allwpilib/xrpVendordep/xrpvendordep-config.cmake.in
new file mode 100644
index 0000000..8cb66bb
--- /dev/null
+++ b/third_party/allwpilib/xrpVendordep/xrpvendordep-config.cmake.in
@@ -0,0 +1,14 @@
+include(CMakeFindDependencyMacro)
+@WPIUTIL_DEP_REPLACE@
+@NTCORE_DEP_REPLACE@
+@CSCORE_DEP_REPLACE@
+@CAMERASERVER_DEP_REPLACE@
+@HAL_DEP_REPLACE@
+@WPILIBC_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+
+@FILENAME_DEP_REPLACE@
+include(${SELF_DIR}/xrpvendordep.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/xrpVendordep_jar.cmake)
+endif()
diff --git a/third_party/flatbuffers/BUILD.bazel b/third_party/flatbuffers/BUILD.bazel
index dc5e735..2c91f5c 100644
--- a/third_party/flatbuffers/BUILD.bazel
+++ b/third_party/flatbuffers/BUILD.bazel
@@ -41,6 +41,7 @@
     name = "flatbuffers",
     hdrs = ["//:public_headers"],
     copts = ["-Wno-cast-align"],
+    defines = ["FLATBUFFERS_USE_STD_SPAN"],
     linkstatic = 1,
     strip_include_prefix = "/include",
     deps = ["//src:flatbuffers"],
diff --git a/third_party/foxglove/schemas/schemas.BUILD b/third_party/foxglove/schemas/schemas.BUILD
index a1a5d29..51b762f 100644
--- a/third_party/foxglove/schemas/schemas.BUILD
+++ b/third_party/foxglove/schemas/schemas.BUILD
@@ -1,12 +1,12 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "DEFAULT_FLATC_ARGS")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "DEFAULT_FLATC_ARGS", "flatbuffer_cc_library")
+load("@org_frc971//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 FLATC_ARGS = [arg for arg in DEFAULT_FLATC_ARGS if arg != "--require-explicit-ids"]
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "schemas",
     srcs = glob(["*.fbs"]),
     flatc_args = FLATC_ARGS,
-    gen_reflections = True,
     visibility = ["//visibility:public"],
 )
 
diff --git a/third_party/y2024/field/2024.png b/third_party/y2024/field/2024.png
new file mode 100644
index 0000000..ae1af58
--- /dev/null
+++ b/third_party/y2024/field/2024.png
Binary files differ
diff --git a/third_party/y2024/field/BUILD b/third_party/y2024/field/BUILD
new file mode 100644
index 0000000..04346f5
--- /dev/null
+++ b/third_party/y2024/field/BUILD
@@ -0,0 +1,10 @@
+filegroup(
+    name = "pictures",
+    srcs = [
+     # Picture from the FIRST inspires field drawings.
+     # https://www.firstinspires.org/robotics/frc/playing-field
+     # Copyright 2024 FIRST
+     "2024.png",
+ ],
+    visibility = ["//visibility:public"],
+)
\ No newline at end of file
diff --git a/tools/build_rules/jinja2_generator.py b/tools/build_rules/jinja2_generator.py
index 3575e99..82222c3 100644
--- a/tools/build_rules/jinja2_generator.py
+++ b/tools/build_rules/jinja2_generator.py
@@ -19,11 +19,15 @@
         type=json.loads,
         help="Dictionary of parameters to replace in the template.")
     parser.add_argument("output", type=str, help="Output file to create.")
+    parser.add_argument(
+        "genfiles_dir",
+        type=str,
+        help="Directory where generated JSON files will be available.")
     args = parser.parse_args(sys.argv[1:])
 
     with open(args.template, 'r') as input_file:
-        template = jinja2.Environment(
-            loader=jinja2.FileSystemLoader(".")).from_string(input_file.read())
+        template = jinja2.Environment(loader=jinja2.FileSystemLoader(
+            [".", args.genfiles_dir])).from_string(input_file.read())
 
     output = template.render(args.replacements)
     with open(args.output, 'w') as config_file:
diff --git a/tools/build_rules/template.bzl b/tools/build_rules/template.bzl
index 1ad06d8..d3547d8 100644
--- a/tools/build_rules/template.bzl
+++ b/tools/build_rules/template.bzl
@@ -8,7 +8,8 @@
         tools = [ctx.executable._jinja2],
         progress_message = "Generating " + out.short_path,
         outputs = [out],
-        command = ctx.executable._jinja2.path + " " + ctx.files.src[0].path + " '" + str(parameters) + "' " + out.path,
+        # TODO(james): Is the genfiles_dir the correct thing?
+        command = ctx.executable._jinja2.path + " " + ctx.files.src[0].path + " '" + str(parameters) + "' " + out.path + " " + ctx.genfiles_dir.path,
     )
 
     return [DefaultInfo(files = depset([out])), OutputGroupInfo(out = depset([out]))]
diff --git a/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index 7c530e8..bc1007e 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -1,43 +1,39 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "claw_goal_fbs",
     srcs = [
         "claw_goal.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "claw_position_fbs",
     srcs = [
         "claw_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "claw_output_fbs",
     srcs = [
         "claw_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "claw_status_fbs",
     srcs = [
         "claw_status.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2014/control_loops/shooter/BUILD b/y2014/control_loops/shooter/BUILD
index 7552462..5c4e8fe 100644
--- a/y2014/control_loops/shooter/BUILD
+++ b/y2014/control_loops/shooter/BUILD
@@ -1,43 +1,39 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_goal_fbs",
     srcs = [
         "shooter_goal.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_position_fbs",
     srcs = [
         "shooter_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_output_fbs",
     srcs = [
         "shooter_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_status_fbs",
     srcs = [
         "shooter_status.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2014/queues/BUILD b/y2014/queues/BUILD
index 87a0de4..8c76aee 100644
--- a/y2014/queues/BUILD
+++ b/y2014/queues/BUILD
@@ -1,21 +1,19 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "auto_mode_fbs",
     srcs = [
         "auto_mode.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "hot_goal_fbs",
     srcs = [
         "hot_goal.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
diff --git a/y2014_bot3/control_loops/rollers/BUILD b/y2014_bot3/control_loops/rollers/BUILD
index ccbd94d..f3116de 100644
--- a/y2014_bot3/control_loops/rollers/BUILD
+++ b/y2014_bot3/control_loops/rollers/BUILD
@@ -1,40 +1,36 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "rollers_goal_fbs",
     srcs = [
         "rollers_goal.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "rollers_position_fbs",
     srcs = [
         "rollers_position.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "rollers_output_fbs",
     srcs = [
         "rollers_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "rollers_status_fbs",
     srcs = [
         "rollers_status.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index 403cfa8..0033677 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
@@ -10,12 +10,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_action_fbs",
     srcs = [
         "superstructure_action.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -92,12 +91,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "vision_align_action_fbs",
     srcs = [
         "vision_align_action.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 9284bea..707fa9c 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -52,22 +52,31 @@
           // Intake
           {
               0.0,
-              {Values::kZeroingSampleSize,
-               Values::kIntakeEncoderIndexDifference, 0.0, 0.3},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kIntakeEncoderIndexDifference,
+               0.0,
+               0.3},
           },
 
           // Shoulder
           {
               0.0,
-              {Values::kZeroingSampleSize,
-               Values::kShoulderEncoderIndexDifference, 0.0, 0.3},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kShoulderEncoderIndexDifference,
+               0.0,
+               0.3},
           },
 
           // Wrist
           {
               0.0,
-              {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
-               0.0, 0.3},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kWristEncoderIndexDifference,
+               0.0,
+               0.3},
           },
 
           0.0,
@@ -84,18 +93,23 @@
               // Value to add to the pot reading for the intake.
               -4.550531 + 150.40906362 * M_PI / 180.0 + 0.5098 - 0.0178 -
                   0.0725,
-              {Values::kZeroingSampleSize,
+              {{},
+               Values::kZeroingSampleSize,
                Values::kIntakeEncoderIndexDifference,
                // Location of an index pulse.
-               0.018008, 2.5},
+               0.018008,
+               2.5},
           },
 
           // Shoulder
           {
               // Value to add to the pot reading for the shoulder.
               -2.86275657117,
-              {Values::kZeroingSampleSize,
-               Values::kShoulderEncoderIndexDifference, 0.097312, 2.5},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kShoulderEncoderIndexDifference,
+               0.097312,
+               2.5},
           },
 
           // Wrist
@@ -104,8 +118,11 @@
               3.2390714288298668 + -0.06138835 * M_PI / 180.0 + 0.0078 -
                   0.0548 - 0.0167 + 0.002 - 0.0026 - 0.1040 - 0.0035 - 0.0012 +
                   0.0166 - 0.017 + 0.148 + 0.004 + 0.024701 - 0.0741,
-              {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
-               0.000820, 2.5},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kWristEncoderIndexDifference,
+               0.000820,
+               2.5},
           },
 
           0.0,
@@ -121,16 +138,22 @@
               // Hard stop is 160.0185751389329 degrees.
               -4.2193 + (160.0185751389329 * M_PI / 180.0 + 0.02 - 0.0235) +
                   0.0549 - 0.104 + 0.019 - 0.938 + 0.660 - 0.002 - 0.2081,
-              {Values::kZeroingSampleSize,
-               Values::kIntakeEncoderIndexDifference, 0.332370, 1.3},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kIntakeEncoderIndexDifference,
+               0.332370,
+               1.3},
           },
 
           // Shoulder (Now calibrated at 0)
           {
               -1.0016 - 0.0841 + 0.06138835 * M_PI / 180.0 + 1.07838 - 1.0441 +
                   0.0034 + 0.0065 - 0.0505,
-              {Values::kZeroingSampleSize,
-               Values::kShoulderEncoderIndexDifference, 0.027180, 1.3},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kShoulderEncoderIndexDifference,
+               0.027180,
+               1.3},
           },
 
           // Wrist
@@ -138,8 +161,11 @@
               3.326328571170133 - 0.06138835 * M_PI / 180.0 - 0.177 + 0.0323 -
                   0.023 + 0.0488 + 0.0120 - 0.0005 - 0.0784 - 0.0010 - 0.080 +
                   0.1245,
-              {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
-               -0.263227, 1.3},
+              {{},
+               Values::kZeroingSampleSize,
+               Values::kWristEncoderIndexDifference,
+               -0.263227,
+               1.3},
           },
 
           0.011,
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
index 6f8d7e1..4e23089 100644
--- a/y2016/control_loops/shooter/BUILD
+++ b/y2016/control_loops/shooter/BUILD
@@ -1,40 +1,36 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_goal_fbs",
     srcs = [
         "shooter_goal.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_position_fbs",
     srcs = [
         "shooter_position.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_output_fbs",
     srcs = [
         "shooter_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_status_fbs",
     srcs = [
         "shooter_status.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 3c6a771..211b55c 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -1,47 +1,43 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
 genrule(
diff --git a/y2016/queues/BUILD b/y2016/queues/BUILD
index 43694d6..e07fb3b 100644
--- a/y2016/queues/BUILD
+++ b/y2016/queues/BUILD
@@ -1,12 +1,11 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "ball_detector_fbs",
     srcs = [
         "ball_detector.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index fed16c3..f1925a6 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -1,15 +1,14 @@
 load("@rules_cc//cc:defs.bzl", "cc_proto_library")
 load("@rules_proto//proto:defs.bzl", "proto_library")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools:platforms.bzl", "platforms")
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "vision_fbs",
     srcs = [
         "vision.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index b658573..0e98085 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -50,7 +50,7 @@
 #include "y2016/control_loops/shooter/shooter_output_generated.h"
 #include "y2016/control_loops/shooter/shooter_position_generated.h"
 #include "y2016/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_static.h"
 #include "y2016/queues/ball_detector_generated.h"
 
 using ::frc971::wpilib::LoopOutputHandler;
@@ -156,7 +156,7 @@
         shooter_position_sender_(
             event_loop->MakeSender<shooter::Position>("/shooter")),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -294,34 +294,17 @@
     }
 
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
 
-      frc971::PotAndIndexPositionT intake;
-      CopyPosition(intake_encoder_, &intake, intake_translate,
+      CopyPosition(intake_encoder_, builder->add_intake(), intake_translate,
                    intake_pot_translate, false, values.intake.pot_offset);
-      flatbuffers::Offset<frc971::PotAndIndexPosition> intake_offset =
-          frc971::PotAndIndexPosition::Pack(*builder.fbb(), &intake);
-
-      frc971::PotAndIndexPositionT shoulder;
-      CopyPosition(shoulder_encoder_, &shoulder, shoulder_translate,
-                   shoulder_pot_translate, false, values.shoulder.pot_offset);
-      flatbuffers::Offset<frc971::PotAndIndexPosition> shoulder_offset =
-          frc971::PotAndIndexPosition::Pack(*builder.fbb(), &shoulder);
-
-      frc971::PotAndIndexPositionT wrist;
-      CopyPosition(wrist_encoder_, &wrist, wrist_translate, wrist_pot_translate,
-                   true, values.wrist.pot_offset);
-      flatbuffers::Offset<frc971::PotAndIndexPosition> wrist_offset =
-          frc971::PotAndIndexPosition::Pack(*builder.fbb(), &wrist);
-
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
-
-      position_builder.add_intake(intake_offset);
-      position_builder.add_shoulder(shoulder_offset);
-      position_builder.add_wrist(wrist_offset);
-
-      builder.CheckOk(builder.Send(position_builder.Finish()));
+      CopyPosition(shoulder_encoder_, builder->add_shoulder(),
+                   shoulder_translate, shoulder_pot_translate, false,
+                   values.shoulder.pot_offset);
+      CopyPosition(wrist_encoder_, builder->add_wrist(), wrist_translate,
+                   wrist_pot_translate, true, values.wrist.pot_offset);
+      builder.CheckOk(builder.Send());
     }
 
     {
@@ -351,7 +334,7 @@
   ::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
   ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
   ::aos::Sender<shooter::Position> shooter_position_sender_;
-  ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+  ::aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   ::aos::Sender<::frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
 
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index b974b15..5339798 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -1,50 +1,46 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
index aa6c96b..c4045b8 100644
--- a/y2017/vision/BUILD
+++ b/y2017/vision/BUILD
@@ -1,16 +1,15 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary")
 load("@rules_cc//cc:defs.bzl", "cc_proto_library")
 load("@rules_proto//proto:defs.bzl", "proto_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "vision_fbs",
     srcs = [
         "vision.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index d299c1d..822c3f4 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -54,7 +54,7 @@
 #include "frc971/wpilib/wpilib_robot_base.h"
 #include "y2017/constants.h"
 #include "y2017/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_position_static.h"
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
@@ -132,7 +132,7 @@
             event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
                 "/aos")),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -230,55 +230,31 @@
     const auto values = constants::GetValues();
 
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
-      frc971::PotAndAbsolutePositionT intake;
-      CopyPosition(intake_encoder_, &intake,
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
+
+      CopyPosition(intake_encoder_, builder->add_intake(),
                    Values::kIntakeEncoderCountsPerRevolution,
                    Values::kIntakeEncoderRatio, intake_pot_translate, true,
                    values.intake.pot_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake);
-
-      frc971::HallEffectAndPositionT indexer;
-      CopyPosition(indexer_counter_, &indexer,
-                   Values::kIndexerEncoderCountsPerRevolution,
-                   Values::kIndexerEncoderRatio, true);
-      flatbuffers::Offset<frc971::HallEffectAndPosition> indexer_offset =
-          frc971::HallEffectAndPosition::Pack(*builder.fbb(), &indexer);
-
-      frc971::IndexPositionT hood;
-      CopyPosition(hood_encoder_, &hood,
+      CopyPosition(hood_encoder_, builder->add_hood(),
                    Values::kHoodEncoderCountsPerRevolution,
                    Values::kHoodEncoderRatio, true);
-      flatbuffers::Offset<frc971::IndexPosition> hood_offset =
-          frc971::IndexPosition::Pack(*builder.fbb(), &hood);
 
-      frc971::HallEffectAndPositionT turret;
-      CopyPosition(turret_counter_, &turret,
+      superstructure::ColumnPositionStatic *column = builder->add_column();
+      CopyPosition(turret_counter_, column->add_turret(),
                    Values::kTurretEncoderCountsPerRevolution,
                    Values::kTurretEncoderRatio, false);
-      flatbuffers::Offset<frc971::HallEffectAndPosition> turret_offset =
-          frc971::HallEffectAndPosition::Pack(*builder.fbb(), &turret);
+      CopyPosition(indexer_counter_, column->add_indexer(),
+                   Values::kIndexerEncoderCountsPerRevolution,
+                   Values::kIndexerEncoderRatio, true);
 
-      superstructure::ColumnPosition::Builder column_builder =
-          builder.MakeBuilder<superstructure::ColumnPosition>();
-      column_builder.add_indexer(indexer_offset);
-      column_builder.add_turret(turret_offset);
-      flatbuffers::Offset<superstructure::ColumnPosition> column_offset =
-          column_builder.Finish();
-
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
-
-      position_builder.add_column(column_offset);
-      position_builder.add_hood(hood_offset);
-      position_builder.add_intake(intake_offset);
-      position_builder.add_theta_shooter(
+      builder->set_theta_shooter(
           encoder_translate(shooter_encoder_->GetRaw(),
                             Values::kShooterEncoderCountsPerRevolution,
                             Values::kShooterEncoderRatio));
 
-      builder.CheckOk(builder.Send(position_builder.Finish()));
+      builder.CheckOk(builder.Send());
     }
 
     {
@@ -299,7 +275,7 @@
 
  private:
   ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
-  ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+  ::aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   ::aos::Sender<::frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
 
diff --git a/y2018/BUILD b/y2018/BUILD
index 34be4dc..e3531b2 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -1,8 +1,8 @@
 load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 load("@rules_cc//cc:defs.bzl", "cc_proto_library")
 load("@rules_proto//proto:defs.bzl", "proto_library")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 robot_downloader(
     start_binaries = [
@@ -109,12 +109,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "status_light_fbs",
     srcs = [
         "status_light.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index a3cb808..c7bfdc0 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -1,49 +1,45 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2018/vision/BUILD b/y2018/vision/BUILD
index 09a70d9..a2c2d8c 100644
--- a/y2018/vision/BUILD
+++ b/y2018/vision/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 cc_binary(
     name = "image_streamer",
@@ -17,12 +17,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "vision_fbs",
     srcs = [
         "vision.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 9f5255d..cf71bdd 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -49,7 +49,7 @@
 #include "frc971/wpilib/wpilib_robot_base.h"
 #include "y2018/constants.h"
 #include "y2018/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_static.h"
 #include "y2018/status_light_generated.h"
 #include "y2018/vision/vision_generated.h"
 
@@ -147,7 +147,7 @@
   SensorReader(::aos::ShmEventLoop *event_loop)
       : ::frc971::wpilib::SensorReader(event_loop),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -302,107 +302,60 @@
     const auto values = constants::GetValues();
 
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
+
+      superstructure::ArmPositionStatic *arm_position = builder->add_arm();
 
       // Proximal arm
-      frc971::PotAndAbsolutePositionT arm_proximal;
-      CopyPosition(proximal_encoder_, &arm_proximal,
+      CopyPosition(proximal_encoder_, arm_position->add_proximal(),
                    Values::kProximalEncoderCountsPerRevolution(),
                    Values::kProximalEncoderRatio(), proximal_pot_translate,
                    true, values.arm_proximal.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_proximal_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &arm_proximal);
 
       // Distal arm
-      frc971::PotAndAbsolutePositionT arm_distal;
-      CopyPosition(distal_encoder_, &arm_distal,
+      CopyPosition(distal_encoder_, arm_position->add_distal(),
                    Values::kDistalEncoderCountsPerRevolution(),
                    Values::kDistalEncoderRatio(), distal_pot_translate, true,
                    values.arm_distal.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_distal_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &arm_distal);
-
-      superstructure::ArmPosition::Builder arm_position_builder =
-          builder.MakeBuilder<superstructure::ArmPosition>();
-      arm_position_builder.add_proximal(arm_proximal_offset);
-      arm_position_builder.add_distal(arm_distal_offset);
-
-      flatbuffers::Offset<superstructure::ArmPosition> arm_position_offset =
-          arm_position_builder.Finish();
 
       // Left intake
-      frc971::PotAndAbsolutePositionT left_intake_motor_position;
-      CopyPosition(left_intake_encoder_, &left_intake_motor_position,
+      superstructure::IntakeElasticSensorsStatic *left_intake =
+          builder->add_left_intake();
+
+      CopyPosition(left_intake_encoder_, left_intake->add_motor_position(),
                    Values::kIntakeMotorEncoderCountsPerRevolution(),
                    Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
                    false, values.left_intake.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition>
-          left_intake_motor_position_offset =
-              frc971::PotAndAbsolutePosition::Pack(*builder.fbb(),
-                                                   &left_intake_motor_position);
+
+      left_intake->set_spring_angle(
+          intake_spring_translate(left_intake_spring_angle_->GetVoltage()) +
+          values.left_intake.spring_offset);
+      left_intake->set_beam_break(!left_intake_cube_detector_->Get());
 
       // Right intake
-      frc971::PotAndAbsolutePositionT right_intake_motor_position;
-      CopyPosition(right_intake_encoder_, &right_intake_motor_position,
+      superstructure::IntakeElasticSensorsStatic *right_intake =
+          builder->add_right_intake();
+      CopyPosition(right_intake_encoder_, right_intake->add_motor_position(),
                    Values::kIntakeMotorEncoderCountsPerRevolution(),
                    Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
                    true, values.right_intake.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition>
-          right_intake_motor_position_offset =
-              frc971::PotAndAbsolutePosition::Pack(
-                  *builder.fbb(), &right_intake_motor_position);
-
-      superstructure::IntakeElasticSensors::Builder
-          left_intake_sensors_builder =
-              builder.MakeBuilder<superstructure::IntakeElasticSensors>();
-
-      left_intake_sensors_builder.add_motor_position(
-          left_intake_motor_position_offset);
-      left_intake_sensors_builder.add_spring_angle(
-          intake_spring_translate(left_intake_spring_angle_->GetVoltage()) +
-          values.left_intake.spring_offset);
-      left_intake_sensors_builder.add_beam_break(
-          !left_intake_cube_detector_->Get());
-
-      flatbuffers::Offset<superstructure::IntakeElasticSensors>
-          left_intake_offset = left_intake_sensors_builder.Finish();
-
-      superstructure::IntakeElasticSensors::Builder
-          right_intake_sensors_builder =
-              builder.MakeBuilder<superstructure::IntakeElasticSensors>();
-
-      right_intake_sensors_builder.add_motor_position(
-          right_intake_motor_position_offset);
-      right_intake_sensors_builder.add_spring_angle(
+      right_intake->set_spring_angle(
           -intake_spring_translate(right_intake_spring_angle_->GetVoltage()) +
           values.right_intake.spring_offset);
-      right_intake_sensors_builder.add_beam_break(
-          !right_intake_cube_detector_->Get());
+      right_intake->set_beam_break(!right_intake_cube_detector_->Get());
 
-      flatbuffers::Offset<control_loops::superstructure::IntakeElasticSensors>
-          right_intake_offset = right_intake_sensors_builder.Finish();
+      builder->set_claw_beambreak_triggered(!claw_beambreak_->Get());
+      builder->set_box_back_beambreak_triggered(!box_back_beambreak_->Get());
 
-      superstructure::Position::Builder superstructure_builder =
-          builder.MakeBuilder<superstructure::Position>();
+      builder->set_box_distance(lidar_lite_.last_width() / 0.00001 / 100.0 / 2);
 
-      superstructure_builder.add_left_intake(left_intake_offset);
-      superstructure_builder.add_right_intake(right_intake_offset);
-      superstructure_builder.add_arm(arm_position_offset);
-
-      superstructure_builder.add_claw_beambreak_triggered(
-          !claw_beambreak_->Get());
-      superstructure_builder.add_box_back_beambreak_triggered(
-          !box_back_beambreak_->Get());
-
-      superstructure_builder.add_box_distance(lidar_lite_.last_width() /
-                                              0.00001 / 100.0 / 2);
-
-      builder.CheckOk(builder.Send(superstructure_builder.Finish()));
+      builder.CheckOk(builder.Send());
     }
   }
 
  private:
-  ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+  ::aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   ::aos::Sender<::frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
 
diff --git a/y2019/BUILD b/y2019/BUILD
index 864cb57..fee2db9 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,8 +1,8 @@
 load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 load("@rules_cc//cc:defs.bzl", "cc_proto_library")
 load("@rules_proto//proto:defs.bzl", "proto_library")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 robot_downloader(
     data = [
@@ -162,22 +162,20 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "camera_log_fbs",
     srcs = [
         "camera_log.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "status_light_fbs",
     srcs = [
         "status_light.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 380899a..0332252 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -77,8 +77,8 @@
   // Elevator constants.
   elevator_params->zeroing_voltage = 3.0;
   elevator_params->operating_voltage = 12.0;
-  elevator_params->zeroing_profile_params = {0.1, 1.0};
-  elevator_params->default_profile_params = {4.0, 13.0};
+  elevator_params->zeroing_profile_params = {{}, 0.1, 1.0};
+  elevator_params->default_profile_params = {{}, 4.0, 13.0};
   elevator_params->range = Values::kElevatorRange();
   elevator_params->make_integral_loop =
       &control_loops::superstructure::elevator::MakeIntegralElevatorLoop;
@@ -93,8 +93,8 @@
   // Wrist constants.
   wrist_params->zeroing_voltage = 4.0;
   wrist_params->operating_voltage = 12.0;
-  wrist_params->zeroing_profile_params = {0.5, 2.0};
-  wrist_params->default_profile_params = {10.0, 40.0};
+  wrist_params->zeroing_profile_params = {{}, 0.5, 2.0};
+  wrist_params->default_profile_params = {{}, 10.0, 40.0};
   wrist_params->range = Values::kWristRange();
   wrist_params->make_integral_loop =
       &control_loops::superstructure::wrist::MakeIntegralWristLoop;
@@ -109,8 +109,8 @@
   // Intake constants.
   intake->zeroing_voltage = 3.0;
   intake->operating_voltage = 12.0;
-  intake->zeroing_profile_params = {0.5, 3.0};
-  intake->default_profile_params = {6.0, 30.0};
+  intake->zeroing_profile_params = {{}, 0.5, 3.0};
+  intake->default_profile_params = {{}, 6.0, 30.0};
   intake->range = Values::kIntakeRange();
   intake->make_integral_loop =
       control_loops::superstructure::intake::MakeIntegralIntakeLoop;
@@ -125,8 +125,8 @@
   // Stilts constants.
   stilts_params->zeroing_voltage = 3.0;
   stilts_params->operating_voltage = 12.0;
-  stilts_params->zeroing_profile_params = {0.1, 0.5};
-  stilts_params->default_profile_params = {0.15, 0.5};
+  stilts_params->zeroing_profile_params = {{}, 0.1, 0.5};
+  stilts_params->default_profile_params = {{}, 0.15, 0.5};
   stilts_params->range = Values::kStiltsRange();
   stilts_params->make_integral_loop =
       &control_loops::superstructure::stilts::MakeIntegralStiltsLoop;
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 2ac689b..9da671d 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
 load("//tools/build_rules:select.bzl", "cpu_select")
 
@@ -104,20 +104,18 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "target_selector_fbs",
     srcs = ["target_selector.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "camera_fbs",
     srcs = [
         "camera.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index f97c6f0..9336cb7 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -1,53 +1,49 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
 )
 
 cc_library(
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index e918aaf..b3a5953 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -52,7 +52,7 @@
 #include "y2019/constants.h"
 #include "y2019/control_loops/drivetrain/camera_generated.h"
 #include "y2019/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_static.h"
 #include "y2019/jevois/spi.h"
 #include "y2019/status_light_generated.h"
 
@@ -139,7 +139,7 @@
             event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
                 "/autonomous")),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -254,63 +254,40 @@
     const auto values = constants::GetValues();
 
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
 
       // Elevator
-      frc971::PotAndAbsolutePositionT elevator;
-      CopyPosition(elevator_encoder_, &elevator,
+      CopyPosition(elevator_encoder_, builder->add_elevator(),
                    Values::kElevatorEncoderCountsPerRevolution(),
                    Values::kElevatorEncoderRatio(), elevator_pot_translate,
                    false, values.elevator.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &elevator);
-
       // Intake
-      frc971::AbsolutePositionT intake_joint;
-      CopyPosition(intake_encoder_, &intake_joint,
+      CopyPosition(intake_encoder_, builder->add_intake_joint(),
                    Values::kIntakeEncoderCountsPerRevolution(),
                    Values::kIntakeEncoderRatio(), false);
-      flatbuffers::Offset<frc971::AbsolutePosition> intake_joint_offset =
-          frc971::AbsolutePosition::Pack(*builder.fbb(), &intake_joint);
-
       // Wrist
-      frc971::PotAndAbsolutePositionT wrist;
-      CopyPosition(wrist_encoder_, &wrist,
+      CopyPosition(wrist_encoder_, builder->add_wrist(),
                    Values::kWristEncoderCountsPerRevolution(),
                    Values::kWristEncoderRatio(), wrist_pot_translate, false,
                    values.wrist.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &wrist);
-
       // Stilts
-      frc971::PotAndAbsolutePositionT stilts;
-      CopyPosition(stilts_encoder_, &stilts,
+      CopyPosition(stilts_encoder_, builder->add_stilts(),
                    Values::kStiltsEncoderCountsPerRevolution(),
                    Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
                    values.stilts.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &stilts);
-
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
-
-      position_builder.add_elevator(elevator_offset);
-      position_builder.add_intake_joint(intake_joint_offset);
-      position_builder.add_wrist(wrist_offset);
-      position_builder.add_stilts(stilts_offset);
 
       // Suction
       constexpr float kMinVoltage = 0.5;
       constexpr float kMaxVoltage = 2.1;
-      position_builder.add_suction_pressure(
+      builder->set_suction_pressure(
           (vacuum_sensor_->GetVoltage() - kMinVoltage) /
           (kMaxVoltage - kMinVoltage));
 
-      position_builder.add_platform_left_detect(!platform_left_detect_->Get());
-      position_builder.add_platform_right_detect(
-          !platform_right_detect_->Get());
+      builder->set_platform_left_detect(!platform_left_detect_->Get());
+      builder->set_platform_right_detect(!platform_right_detect_->Get());
 
-      builder.CheckOk(builder.Send(position_builder.Finish()));
+      builder.CheckOk(builder.Send());
     }
 
     {
@@ -334,7 +311,7 @@
 
  private:
   ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
-  ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+  ::aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   ::aos::Sender<::frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
 
diff --git a/y2020/BUILD b/y2020/BUILD
index 04ca0d8..001ef89 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -1,6 +1,6 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//frc971:downloader.bzl", "robot_downloader")
 load("//aos:config.bzl", "aos_config")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("//tools/build_rules:template.bzl", "jinja2_template")
 load("//aos/util:config_validator_macro.bzl", "config_validator_test")
 
@@ -300,12 +300,11 @@
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "setpoint_fbs",
     srcs = [
         "setpoint.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 6218f9b..e30ef0e 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -57,8 +57,8 @@
   // Hood constants.
   hood->zeroing_voltage = 2.0;
   hood->operating_voltage = 12.0;
-  hood->zeroing_profile_params = {0.5, 3.0};
-  hood->default_profile_params = {6.0, 30.0};
+  hood->zeroing_profile_params = {{}, 0.5, 3.0};
+  hood->default_profile_params = {{}, 6.0, 30.0};
   hood->range = Values::kHoodRange();
   hood->make_integral_loop =
       control_loops::superstructure::hood::MakeIntegralHoodLoop;
@@ -90,8 +90,8 @@
   // Intake constants.
   intake->zeroing_voltage = 3.0;
   intake->operating_voltage = 12.0;
-  intake->zeroing_profile_params = {0.5, 3.0};
-  intake->default_profile_params = {6.0, 30.0};
+  intake->zeroing_profile_params = {{}, 0.5, 3.0};
+  intake->default_profile_params = {{}, 6.0, 30.0};
   intake->range = Values::kIntakeRange();
   intake->make_integral_loop =
       control_loops::superstructure::intake::MakeIntegralIntakeLoop;
@@ -112,8 +112,8 @@
   turret_params->zeroing_voltage = 4.0;
   turret_params->operating_voltage = 8.0;
   // TODO(austin): Tune these.
-  turret_params->zeroing_profile_params = {0.5, 2.0};
-  turret_params->default_profile_params = {15.0, 40.0};
+  turret_params->zeroing_profile_params = {{}, 0.5, 2.0};
+  turret_params->default_profile_params = {{}, 15.0, 40.0};
   turret_params->range = Values::kTurretRange();
   turret_params->make_integral_loop =
       &control_loops::superstructure::turret::MakeIntegralTurretLoop;
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 8191404..5e859ab 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -1,5 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 load("//aos:config.bzl", "aos_config")
 
@@ -57,10 +57,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "localizer_debug_fbs",
     srcs = ["localizer_debug.fbs"],
-    gen_reflections = 1,
     visibility = ["//visibility:public"],
 )
 
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
index 3833536..0650421 100644
--- a/y2020/control_loops/python/BUILD
+++ b/y2020/control_loops/python/BUILD
@@ -113,19 +113,6 @@
     ],
 )
 
-py_library(
-    name = "flywheel",
-    srcs = [
-        "flywheel.py",
-    ],
-    target_compatible_with = ["@platforms//cpu:x86_64"],
-    deps = [
-        "//frc971/control_loops/python:controls",
-        "@pip//matplotlib",
-        "@pip//pygobject",
-    ],
-)
-
 py_binary(
     name = "accelerator",
     srcs = [
@@ -134,8 +121,8 @@
     legacy_create_init = False,
     target_compatible_with = ["@platforms//cpu:x86_64"],
     deps = [
-        ":flywheel",
         ":python_init",
+        "//frc971/control_loops/python:flywheel",
         "@pip//glog",
         "@pip//python_gflags",
     ],
@@ -149,8 +136,8 @@
     legacy_create_init = False,
     target_compatible_with = ["@platforms//cpu:x86_64"],
     deps = [
-        ":flywheel",
         ":python_init",
+        "//frc971/control_loops/python:flywheel",
         "@pip//glog",
         "@pip//python_gflags",
     ],
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 54ec9d3..752f8b5 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -1,7 +1,7 @@
 #!/usr/bin/python3
 
 from frc971.control_loops.python import control_loop
-from y2020.control_loops.python import flywheel
+from frc971.control_loops.python import flywheel
 import numpy
 
 import sys
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 7e703fc..29af431 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -1,7 +1,7 @@
 #!/usr/bin/python3
 
 from frc971.control_loops.python import control_loop
-from y2020.control_loops.python import flywheel
+from frc971.control_loops.python import flywheel
 import numpy
 
 import sys
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
deleted file mode 100755
index 9153bc6..0000000
--- a/y2020/control_loops/python/flywheel.py
+++ /dev/null
@@ -1,325 +0,0 @@
-from frc971.control_loops.python import control_loop
-from frc971.control_loops.python import controls
-import numpy
-from matplotlib import pylab
-
-import glog
-
-
-class FlywheelParams(object):
-
-    def __init__(self,
-                 name,
-                 motor,
-                 G,
-                 J,
-                 q_pos,
-                 q_vel,
-                 q_voltage,
-                 r_pos,
-                 controller_poles,
-                 dt=0.00505):
-        self.name = name
-        self.motor = motor
-        self.G = G
-        self.J = J
-        self.q_pos = q_pos
-        self.q_vel = q_vel
-        self.q_voltage = q_voltage
-        self.r_pos = r_pos
-        self.dt = dt
-        self.controller_poles = controller_poles
-
-
-class VelocityFlywheel(control_loop.HybridControlLoop):
-
-    def __init__(self, params, name="Flywheel"):
-        super(VelocityFlywheel, self).__init__(name=name)
-        self.params = params
-        # Set Motor
-        self.motor = self.params.motor
-        # Moment of inertia of the flywheel wheel in kg m^2
-        self.J = self.params.J
-        # Gear ratio
-        self.G = self.params.G
-        # Control loop time step
-        self.dt = self.params.dt
-
-        # State feedback matrices
-        # [angular velocity]
-        self.A_continuous = numpy.matrix([[
-            -self.motor.Kt / self.motor.Kv /
-            (self.J * self.G * self.G * self.motor.resistance)
-        ]])
-        self.B_continuous = numpy.matrix(
-            [[self.motor.Kt / (self.J * self.G * self.motor.resistance)]])
-        self.C = numpy.matrix([[1]])
-        self.D = numpy.matrix([[0]])
-
-        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
-                                                   self.B_continuous, self.dt)
-
-        self.PlaceControllerPoles(self.params.controller_poles)
-
-        # Generated controller not used.
-        self.PlaceObserverPoles([0.3])
-
-        self.U_max = numpy.matrix([[12.0]])
-        self.U_min = numpy.matrix([[-12.0]])
-
-        qff_vel = 8.0
-        self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
-
-        self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
-
-        glog.debug('K: %s', str(self.K))
-        glog.debug('Poles: %s',
-                   str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
-
-
-class Flywheel(VelocityFlywheel):
-
-    def __init__(self, params, name="Flywheel"):
-        super(Flywheel, self).__init__(params, name=name)
-
-        self.A_continuous_unaugmented = self.A_continuous
-        self.B_continuous_unaugmented = self.B_continuous
-
-        self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
-        self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
-        self.A_continuous[0, 1] = 1
-
-        self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
-        self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
-
-        # State feedback matrices
-        # [position, angular velocity]
-        self.C = numpy.matrix([[1, 0]])
-        self.D = numpy.matrix([[0]])
-
-        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
-                                                   self.B_continuous, self.dt)
-
-        rpl = 0.45
-        ipl = 0.07
-        self.PlaceObserverPoles([rpl + 1j * ipl, rpl - 1j * ipl])
-
-        self.K_unaugmented = self.K
-        self.K = numpy.matrix(numpy.zeros((1, 2)))
-        self.K[0, 1:2] = self.K_unaugmented
-        self.Kff_unaugmented = self.Kff
-        self.Kff = numpy.matrix(numpy.zeros((1, 2)))
-        self.Kff[0, 1:2] = self.Kff_unaugmented
-
-        self.InitializeState()
-
-
-class IntegralFlywheel(Flywheel):
-
-    def __init__(self, params, name="IntegralFlywheel"):
-        super(IntegralFlywheel, self).__init__(params, name=name)
-
-        self.A_continuous_unaugmented = self.A_continuous
-        self.B_continuous_unaugmented = self.B_continuous
-
-        self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
-        self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
-        self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
-
-        self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
-        self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
-
-        # states
-        # [position, velocity, voltage_error]
-        self.C_unaugmented = self.C
-        self.C = numpy.matrix(numpy.zeros((1, 3)))
-        self.C[0:1, 0:2] = self.C_unaugmented
-
-        glog.debug('A_continuous %s' % str(self.A_continuous))
-        glog.debug('B_continuous %s' % str(self.B_continuous))
-        glog.debug('C %s' % str(self.C))
-
-        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
-                                                   self.B_continuous, self.dt)
-
-        glog.debug('A %s' % str(self.A))
-        glog.debug('B %s' % str(self.B))
-
-        q_pos = self.params.q_pos
-        q_vel = self.params.q_vel
-        q_voltage = self.params.q_voltage
-        self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
-                                          [0.0, (q_vel**2.0), 0.0],
-                                          [0.0, 0.0, (q_voltage**2.0)]])
-
-        r_pos = self.params.r_pos
-        self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
-
-        _, _, self.Q, self.R = controls.kalmd(A_continuous=self.A_continuous,
-                                              B_continuous=self.B_continuous,
-                                              Q_continuous=self.Q_continuous,
-                                              R_continuous=self.R_continuous,
-                                              dt=self.dt)
-
-        glog.debug('Q_discrete %s' % (str(self.Q)))
-        glog.debug('R_discrete %s' % (str(self.R)))
-
-        self.KalmanGain, self.P_steady_state = controls.kalman(A=self.A,
-                                                               B=self.B,
-                                                               C=self.C,
-                                                               Q=self.Q,
-                                                               R=self.R)
-        self.L = self.A * self.KalmanGain
-
-        self.K_unaugmented = self.K
-        self.K = numpy.matrix(numpy.zeros((1, 3)))
-        self.K[0, 0:2] = self.K_unaugmented
-        self.K[0, 2] = 1
-        self.Kff_unaugmented = self.Kff
-        self.Kff = numpy.matrix(numpy.zeros((1, 3)))
-        self.Kff[0, 0:2] = self.Kff_unaugmented
-
-        self.InitializeState()
-
-
-def PlotSpinup(params, goal, iterations=400):
-    """Runs the flywheel plant with an initial condition and goal.
-
-    Args:
-        flywheel: Flywheel object to use.
-        goal: goal state.
-        iterations: Number of timesteps to run the model for.
-        controller_flywheel: Flywheel object to get K from, or None if we should
-             use flywheel.
-        observer_flywheel: Flywheel object to use for the observer, or None if we
-            should use the actual state.
-    """
-
-    # Various lists for graphing things.
-    t = []
-    x = []
-    v = []
-    a = []
-    x_hat = []
-    u = []
-    offset = []
-
-    flywheel = Flywheel(params, params.name)
-    controller_flywheel = IntegralFlywheel(params, params.name)
-    observer_flywheel = IntegralFlywheel(params, params.name)
-    vbat = 12.0
-
-    if t:
-        initial_t = t[-1] + flywheel.dt
-    else:
-        initial_t = 0
-
-    for i in range(iterations):
-        X_hat = flywheel.X
-
-        if observer_flywheel is not None:
-            X_hat = observer_flywheel.X_hat
-            x_hat.append(observer_flywheel.X_hat[1, 0])
-
-        ff_U = controller_flywheel.Kff * (goal - observer_flywheel.A * goal)
-
-        U = controller_flywheel.K * (goal - X_hat) + ff_U
-        U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
-        x.append(flywheel.X[0, 0])
-
-        if v:
-            last_v = v[-1]
-        else:
-            last_v = 0
-
-        v.append(flywheel.X[1, 0])
-        a.append((v[-1] - last_v) / flywheel.dt)
-
-        if observer_flywheel is not None:
-            observer_flywheel.Y = flywheel.Y
-            observer_flywheel.CorrectHybridObserver(U)
-            offset.append(observer_flywheel.X_hat[2, 0])
-
-        applied_U = U.copy()
-        if i > 200:
-            applied_U += 2
-        flywheel.Update(applied_U)
-
-        if observer_flywheel is not None:
-            observer_flywheel.PredictHybridObserver(U, flywheel.dt)
-
-        t.append(initial_t + i * flywheel.dt)
-        u.append(U[0, 0])
-
-    pylab.subplot(3, 1, 1)
-    pylab.plot(t, v, label='x')
-    pylab.plot(t, x_hat, label='x_hat')
-    pylab.legend()
-
-    pylab.subplot(3, 1, 2)
-    pylab.plot(t, u, label='u')
-    pylab.plot(t, offset, label='voltage_offset')
-    pylab.legend()
-
-    pylab.subplot(3, 1, 3)
-    pylab.plot(t, a, label='a')
-    pylab.legend()
-
-    pylab.show()
-
-
-def WriteFlywheel(params, plant_files, controller_files, namespace):
-    """Writes out the constants for a flywheel to a file.
-
-    Args:
-      params: list of Flywheel Params, the
-        parameters defining the system.
-      plant_files: list of strings, the cc and h files for the plant.
-      controller_files: list of strings, the cc and h files for the integral
-        controller.
-      namespaces: list of strings, the namespace list to use.
-    """
-    # Write the generated constants out to a file.
-    flywheels = []
-    integral_flywheels = []
-
-    if type(params) is list:
-        name = params[0].name
-        for index, param in enumerate(params):
-            flywheels.append(Flywheel(param, name=param.name + str(index)))
-            integral_flywheels.append(
-                IntegralFlywheel(param,
-                                 name='Integral' + param.name + str(index)))
-    else:
-        name = params.name
-        flywheels.append(Flywheel(params, params.name))
-        integral_flywheels.append(
-            IntegralFlywheel(params, name='Integral' + params.name))
-
-    loop_writer = control_loop.ControlLoopWriter(name,
-                                                 flywheels,
-                                                 namespaces=namespace)
-    loop_writer.AddConstant(
-        control_loop.Constant('kOutputRatio', '%f', flywheels[0].G))
-    loop_writer.AddConstant(
-        control_loop.Constant('kFreeSpeed', '%f',
-                              flywheels[0].motor.free_speed))
-    loop_writer.AddConstant(
-        control_loop.Constant('kBemf',
-                              '%f',
-                              flywheels[0].motor.Kv * flywheels[0].G,
-                              comment="// Radians/sec / volt"))
-    loop_writer.AddConstant(
-        control_loop.Constant('kResistance',
-                              '%f',
-                              flywheels[0].motor.resistance,
-                              comment="// Ohms"))
-    loop_writer.Write(plant_files[0], plant_files[1])
-
-    integral_loop_writer = control_loop.ControlLoopWriter(
-        'Integral' + name,
-        integral_flywheels,
-        namespaces=namespace,
-        plant_type='StateFeedbackHybridPlant',
-        observer_type='HybridKalman')
-    integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 83af834..c77f35d 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -1,28 +1,26 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
@@ -35,33 +33,33 @@
     deps = [
         "//frc971/control_loops:control_loops_ts_fbs",
         "//frc971/control_loops:profiled_subsystem_ts_fbs",
+        "//frc971/control_loops/flywheel:flywheel_controller_status_ts_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//frc971/control_loops/flywheel:flywheel_controller_status_fbs",
+    ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
 )
 
 cc_library(
@@ -133,6 +131,7 @@
         "//frc971/control_loops:position_sensor_sim",
         "//frc971/control_loops:team_number_test_environment",
         "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+        "//frc971/control_loops/flywheel:flywheel_test_plant",
         "//y2020/control_loops/superstructure/hood:hood_plants",
         "//y2020/control_loops/superstructure/intake:intake_plants",
         "//y2020/control_loops/superstructure/shooter:shooter_plants",
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index 4b0bf03..96c933f 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 package(default_visibility = ["//visibility:public"])
 
@@ -20,50 +20,31 @@
     ],
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
-        ":flywheel_controller",
         "//frc971/control_loops:control_loop",
         "//frc971/control_loops:profiled_subsystem",
+        "//frc971/control_loops/flywheel:flywheel_controller",
         "//y2020/control_loops/superstructure:superstructure_goal_fbs",
         "//y2020/control_loops/superstructure:superstructure_output_fbs",
         "//y2020/control_loops/superstructure:superstructure_position_fbs",
         "//y2020/control_loops/superstructure:superstructure_status_fbs",
-    ],
-)
-
-cc_library(
-    name = "flywheel_controller",
-    srcs = [
-        "flywheel_controller.cc",
-    ],
-    hdrs = [
-        "flywheel_controller.h",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    deps = [
-        "//frc971/control_loops:control_loop",
-        "//frc971/control_loops:profiled_subsystem",
-        "//y2020/control_loops/superstructure:superstructure_goal_fbs",
-        "//y2020/control_loops/superstructure:superstructure_status_fbs",
         "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
         "//y2020/control_loops/superstructure/finisher:finisher_plants",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_tuning_readings_fbs",
     srcs = [
         "shooter_tuning_readings.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "shooter_tuning_params_fbs",
     srcs = [
         "shooter_tuning_params.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
deleted file mode 100644
index a25ed53..0000000
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ /dev/null
@@ -1,175 +0,0 @@
-#include "y2020/control_loops/superstructure/shooter/flywheel_controller.h"
-
-#include <chrono>
-
-#include "aos/logging/logging.h"
-#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
-#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
-
-namespace y2020 {
-namespace control_loops {
-namespace superstructure {
-namespace shooter {
-
-// Class to current limit battery current for a flywheel controller.
-class CurrentLimitedStateFeedbackController
-    : public StateFeedbackLoop<3, 1, 1, double,
-                               StateFeedbackHybridPlant<3, 1, 1>,
-                               HybridKalman<3, 1, 1>> {
- public:
-  // Builds a CurrentLimitedStateFeedbackController given the coefficients, bemf
-  // coefficient (units of radians/sec / volt), and motor resistance in ohms.
-  CurrentLimitedStateFeedbackController(
-      StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
-                        HybridKalman<3, 1, 1>> &&other,
-      double bemf, double resistance)
-      : StateFeedbackLoop(std::move(other)),
-        bemf_(bemf),
-        resistance_(resistance) {}
-
-  double resistance() const { return resistance_; }
-  double bemf() const { return bemf_; }
-
-  void CapU() override {
-    const double bemf_voltage = X_hat(1) / bemf_;
-    // Solve the system of equations:
-    //
-    //   motor_current = (u - bemf_voltage) / resistance
-    //   battery_current = ((u - bemf_voltage) / resistance) * u / 12.0
-    //   0.0 = u * u - u * bemf_voltage - max_current * 12.0 * resistance
-    //
-    // And we have a quadratic!
-    const double a = 1;
-    const double b = -bemf_voltage;
-    const double c_positive = -70.0 * 12.0 * resistance_;
-    const double c_negative = -25.0 * 12.0 * resistance_;
-
-    // Root is always positive.
-    const double root_positive = std::sqrt(b * b - 4.0 * a * c_positive);
-    const double root_negative = std::sqrt(b * b - 4.0 * a * c_negative);
-    const double upper_limit = (-b + root_positive) / (2.0 * a);
-    const double lower_limit = (-b - root_negative) / (2.0 * a);
-
-    // Limit to the battery voltage and the current limit voltage.
-    mutable_U(0, 0) = std::clamp(U(0, 0), lower_limit, upper_limit);
-    if (R(1) > 50.0) {
-      mutable_U(0, 0) = std::clamp(U(0, 0), 1.0, 12.0);
-    } else {
-      mutable_U(0, 0) = std::clamp(U(0, 0), 0.0, 12.0);
-    }
-  }
-
- private:
-  double bemf_ = 0.0;
-  double resistance_ = 0.0;
-};
-
-FlywheelController::FlywheelController(
-    StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
-                      HybridKalman<3, 1, 1>> &&loop,
-    double bemf, double resistance)
-    : loop_(new CurrentLimitedStateFeedbackController(std::move(loop), bemf,
-                                                      resistance)) {
-  history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
-      0, ::aos::monotonic_clock::epoch()));
-  Y_.setZero();
-}
-
-void FlywheelController::set_goal(double angular_velocity_goal) {
-  loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
-  last_goal_ = angular_velocity_goal;
-}
-
-void FlywheelController::set_position(
-    double current_position,
-    const aos::monotonic_clock::time_point position_timestamp) {
-  // Project time forwards.
-  const int newest_history_position =
-      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
-
-  if (!first_) {
-    loop_->UpdateObserver(
-        loop_->U(),
-        position_timestamp - std::get<1>(history_[newest_history_position]));
-  } else {
-    first_ = false;
-  }
-
-  // Update position in the model.
-  Y_ << current_position;
-
-  // Add the position to the history.
-  history_[history_position_] =
-      std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
-                                                            position_timestamp);
-  history_position_ = (history_position_ + 1) % kHistoryLength;
-
-  loop_->Correct(Y_);
-}
-
-double FlywheelController::voltage() const { return loop_->U(0, 0); }
-
-double FlywheelController::current() const {
-  return ((voltage() - (velocity() / loop_->bemf())) / (loop_->resistance())) *
-         voltage() / 12.0;
-}
-
-void FlywheelController::Update(bool disabled) {
-  loop_->mutable_R() = loop_->next_R();
-  if (loop_->R(1, 0) < 1.0) {
-    // Kill power at low angular velocities.
-    disabled = true;
-  }
-
-  loop_->UpdateController(disabled);
-}
-
-flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
-    flatbuffers::FlatBufferBuilder *fbb) {
-  // Compute the oldest point in the history.
-  const int oldest_history_position = history_position_;
-  const int newest_history_position =
-      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
-  const int second_newest_history_position =
-      ((newest_history_position == 0) ? kHistoryLength
-                                      : newest_history_position) -
-      1;
-
-  const double total_loop_time = ::aos::time::DurationInSeconds(
-      std::get<1>(history_[newest_history_position]) -
-      std::get<1>(history_[oldest_history_position]));
-
-  const double distance_traveled =
-      std::get<0>(history_[newest_history_position]) -
-      std::get<0>(history_[oldest_history_position]);
-
-  const double last_loop_time = ::aos::time::DurationInSeconds(
-      std::get<1>(history_[newest_history_position]) -
-      std::get<1>(history_[second_newest_history_position]));
-
-  const double last_distance_traveled =
-      std::get<0>(history_[newest_history_position]) -
-      std::get<0>(history_[second_newest_history_position]);
-
-  // Compute the distance moved over that time period.
-  avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
-
-  FlywheelControllerStatusBuilder builder(*fbb);
-
-  builder.add_avg_angular_velocity(avg_angular_velocity_);
-  builder.add_dt_angular_velocity(last_distance_traveled / last_loop_time);
-  builder.add_angular_velocity(loop_->X_hat(1, 0));
-  builder.add_voltage_error(loop_->X_hat(2, 0));
-  builder.add_commanded_current(current());
-  builder.add_angular_velocity_goal(last_goal_);
-  return builder.Finish();
-}
-
-FlywheelController::~FlywheelController() {}
-
-double FlywheelController::velocity() const { return loop_->X_hat(1, 0); }
-
-}  // namespace shooter
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2020
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
deleted file mode 100644
index 1d56407..0000000
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ /dev/null
@@ -1,83 +0,0 @@
-#ifndef Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
-#define Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
-
-#include <memory>
-
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
-#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
-#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2020 {
-namespace control_loops {
-namespace superstructure {
-namespace shooter {
-
-class CurrentLimitedStateFeedbackController;
-
-// Handles the velocity control of each flywheel.
-class FlywheelController {
- public:
-  FlywheelController(
-      StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
-                        HybridKalman<3, 1, 1>> &&loop,
-      double bemf, double resistance);
-
-  ~FlywheelController();
-
-  // Sets the velocity goal in radians/sec
-  void set_goal(double angular_velocity_goal);
-  double goal() const { return last_goal_; }
-  // Sets the current encoder position in radians
-  void set_position(double current_position,
-                    const aos::monotonic_clock::time_point position_timestamp);
-
-  // Populates the status structure.
-  flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
-      flatbuffers::FlatBufferBuilder *fbb);
-
-  // Returns the control loop calculated voltage.
-  double voltage() const;
-
-  // Returns the expected battery current for the last U.
-  double current() const;
-
-  // Returns the instantaneous velocity.
-  double velocity() const;
-
-  // Executes the control loop for a cycle.
-  void Update(bool disabled);
-
-  double avg_angular_velocity() { return avg_angular_velocity_; }
-
- private:
-  // The current sensor measurement.
-  Eigen::Matrix<double, 1, 1> Y_;
-  // The control loop.
-  ::std::unique_ptr<CurrentLimitedStateFeedbackController> loop_;
-
-  // History array for calculating a filtered angular velocity.
-  static constexpr int kHistoryLength = 10;
-  ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
-               kHistoryLength>
-      history_;
-  ptrdiff_t history_position_ = 0;
-
-  // Average velocity logging.
-  double avg_angular_velocity_ = 0;
-
-  double last_goal_ = 0;
-
-  bool first_ = true;
-
-  DISALLOW_COPY_AND_ASSIGN(FlywheelController);
-};
-
-}  // namespace shooter
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2020
-
-#endif  // Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 9be6273..c400e34 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -5,7 +5,9 @@
 
 #include "aos/logging/logging.h"
 #include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
 #include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
+#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
 
 namespace y2020 {
 namespace control_loops {
@@ -78,11 +80,11 @@
     UpToSpeed(goal);
   }
 
-  flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
-      finisher_.SetStatus(fbb);
-  flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
-      accelerator_left_.SetStatus(fbb);
-  flatbuffers::Offset<FlywheelControllerStatus>
+  flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
+      finisher_status_offset = finisher_.SetStatus(fbb);
+  flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
+      accelerator_left_status_offset = accelerator_left_.SetStatus(fbb);
+  flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
       accelerator_right_status_offset = accelerator_right_.SetStatus(fbb);
 
   ShooterStatusBuilder status_builder(*fbb);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index ccbb326..a1d7be3 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -2,8 +2,8 @@
 #define Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
 
 #include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
 #include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/shooter/flywheel_controller.h"
 #include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_position_generated.h"
@@ -42,7 +42,8 @@
   // flywheel and when it gets shot.
   static constexpr double kMinFinisherVelocityDipWithBall = 5.0;
 
-  FlywheelController finisher_, accelerator_left_, accelerator_right_;
+  frc971::control_loops::flywheel::FlywheelController finisher_,
+      accelerator_left_, accelerator_right_;
 
   void UpToSpeed(const ShooterGoal *goal);
   bool finisher_ready_ = false;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index d2051df..8b34fc9 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -11,6 +11,7 @@
 #include "aos/network/team_number.h"
 #include "frc971/control_loops/capped_test_plant.h"
 #include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/flywheel/flywheel_test_plant.h"
 #include "frc971/control_loops/position_sensor_sim.h"
 #include "frc971/control_loops/team_number_test_environment.h"
 #include "y2020/constants.h"
@@ -51,39 +52,6 @@
 typedef Superstructure::PotAndAbsoluteEncoderSubsystem
     PotAndAbsoluteEncoderSubsystem;
 
-class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
- public:
-  explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other, double bemf,
-                         double resistance)
-      : StateFeedbackPlant<2, 1, 1>(::std::move(other)),
-        bemf_(bemf),
-        resistance_(resistance) {}
-
-  void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
-    EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
-    EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
-  }
-
-  double motor_current(const Eigen::Matrix<double, 1, 1> U) const {
-    return (U(0) - X(1) / bemf_) / resistance_;
-  }
-
-  double battery_current(const Eigen::Matrix<double, 1, 1> U) const {
-    return motor_current(U) * U(0) / 12.0;
-  }
-
-  double voltage_offset() const { return voltage_offset_; }
-  void set_voltage_offset(double voltage_offset) {
-    voltage_offset_ = voltage_offset;
-  }
-
- private:
-  double voltage_offset_ = 0.0;
-
-  double bemf_;
-  double resistance_;
-};
-
 // Class which simulates the superstructure and sends out queue messages with
 // the position.
 class SuperstructureSimulation {
@@ -111,14 +79,16 @@
                             .turret.subsystem_params.zeroing_constants
                             .one_revolution_distance),
         accelerator_left_plant_(
-            new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
-                              accelerator::kBemf, accelerator::kResistance)),
+            new frc971::control_loops::flywheel::FlywheelPlant(
+                accelerator::MakeAcceleratorPlant(), accelerator::kBemf,
+                accelerator::kResistance)),
         accelerator_right_plant_(
-            new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
-                              accelerator::kBemf, accelerator::kResistance)),
-        finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant(),
-                                          finisher::kBemf,
-                                          finisher::kResistance)) {
+            new frc971::control_loops::flywheel::FlywheelPlant(
+                accelerator::MakeAcceleratorPlant(), accelerator::kBemf,
+                accelerator::kResistance)),
+        finisher_plant_(new frc971::control_loops::flywheel::FlywheelPlant(
+            finisher::MakeFinisherPlant(), finisher::kBemf,
+            finisher::kResistance)) {
     InitializeHoodPosition(constants::Values::kHoodRange().upper);
     InitializeIntakePosition(constants::Values::kIntakeRange().upper);
     InitializeTurretPosition(constants::Values::kTurretRange().middle());
@@ -413,9 +383,12 @@
   ::std::unique_ptr<CappedTestPlant> turret_plant_;
   PositionSensorSimulator turret_encoder_;
 
-  ::std::unique_ptr<FlywheelPlant> accelerator_left_plant_;
-  ::std::unique_ptr<FlywheelPlant> accelerator_right_plant_;
-  ::std::unique_ptr<FlywheelPlant> finisher_plant_;
+  ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+      accelerator_left_plant_;
+  ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+      accelerator_right_plant_;
+  ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+      finisher_plant_;
 
   // The acceleration limits to check for while moving.
   double peak_hood_acceleration_ = 1e10;
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 611661c..d3f98e5 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -1,5 +1,6 @@
 include "frc971/control_loops/control_loops.fbs";
 include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/flywheel/flywheel_controller_status.fbs";
 
 namespace y2020.control_loops.superstructure;
 
@@ -9,36 +10,14 @@
   TURRET
 }
 
-table FlywheelControllerStatus {
-  // The current average velocity in radians/second over the last kHistoryLength
-  // in shooter.h
-  avg_angular_velocity:double (id: 0);
-
-  // The current instantaneous filtered velocity in radians/second.
-  angular_velocity:double (id: 1);
-
-  // The target speed selected by the lookup table or from manual override
-  // Can be compared to velocity to determine if ready.
-  angular_velocity_goal:double (id: 2);
-
-  // Voltage error.
-  voltage_error:double (id: 3);
-
-  // The commanded battery current.
-  commanded_current:double (id: 4);
-
-  // The angular velocity of the flywheel computed using delta x / delta t
-  dt_angular_velocity:double (id: 5);
-}
-
 table ShooterStatus {
   // The final wheel shooting the ball
-  finisher:FlywheelControllerStatus (id: 0);
+  finisher:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 0);
 
   // The subsystem to accelerate the ball before the finisher
   // Velocity is the fastest (top) wheel
-  accelerator_left:FlywheelControllerStatus (id: 1);
-  accelerator_right:FlywheelControllerStatus (id: 2);
+  accelerator_left:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 1);
+  accelerator_right:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 2);
 
   // If the shooter is ready, this is true.  Note: don't use this for status
   // checking, only for plotting.  Changes in goal take time to impact this.
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index aa4e5bf..ccff6b7 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -1,9 +1,8 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "galactic_search_path_fbs",
     srcs = ["galactic_search_path.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2020:__subpackages__"],
 )
diff --git a/y2020/vision/sift/BUILD b/y2020/vision/sift/BUILD
index 1920cce..1d4083e 100644
--- a/y2020/vision/sift/BUILD
+++ b/y2020/vision/sift/BUILD
@@ -1,5 +1,6 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load(":fast_gaussian.bzl", "fast_gaussian")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_py_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 # Note that this file is also used directly by :fast_gaussian_halide_generator,
@@ -229,10 +230,9 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "sift_fbs",
     srcs = ["sift.fbs"],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
@@ -244,13 +244,12 @@
     visibility = ["//y2020:__subpackages__"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "sift_training_fbs",
     srcs = ["sift_training.fbs"],
-    gen_reflections = True,
-    includes = [":sift_fbs_includes"],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
+    deps = [":sift_fbs"],
 )
 
 py_binary(
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 2f7b184..c9cec8a 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -53,7 +53,7 @@
 #include "y2020/constants.h"
 #include "y2020/control_loops/superstructure/shooter/shooter_tuning_readings_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_position_static.h"
 
 DEFINE_bool(shooter_tuning, true,
             "If true, reads from ball beambreak sensors and sends shooter "
@@ -161,7 +161,7 @@
             event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
                 "/autonomous")),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -288,65 +288,43 @@
     const constants::Values &values = constants::GetValues();
 
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
 
       // TODO(alex): check new absolute encoder api.
       // Hood
-      frc971::AbsoluteAndAbsolutePositionT hood;
-      CopyPosition(hood_encoder_, &hood,
+      CopyPosition(hood_encoder_, builder->add_hood(),
                    Values::kHoodEncoderCountsPerRevolution(),
                    Values::kHoodEncoderRatio(),
                    Values::kHoodSingleTurnEncoderRatio(), false);
-      flatbuffers::Offset<frc971::AbsoluteAndAbsolutePosition> hood_offset =
-          frc971::AbsoluteAndAbsolutePosition::Pack(*builder.fbb(), &hood);
-
       // Intake
-      frc971::AbsolutePositionT intake_joint;
-      CopyPosition(intake_joint_encoder_, &intake_joint,
+      CopyPosition(intake_joint_encoder_, builder->add_intake_joint(),
                    Values::kIntakeEncoderCountsPerRevolution(),
                    Values::kIntakeEncoderRatio(), false);
-      flatbuffers::Offset<frc971::AbsolutePosition> intake_joint_offset =
-          frc971::AbsolutePosition::Pack(*builder.fbb(), &intake_joint);
-
       // Turret
-      frc971::PotAndAbsolutePositionT turret;
-      CopyPosition(turret_encoder_, &turret,
+      CopyPosition(turret_encoder_, builder->add_turret(),
                    Values::kTurretEncoderCountsPerRevolution(),
                    Values::kTurretEncoderRatio(), turret_pot_translate, true,
                    values.turret.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
-
       // Shooter
-      y2020::control_loops::superstructure::ShooterPositionT shooter;
-      shooter.theta_finisher =
+      y2020::control_loops::superstructure::ShooterPositionStatic *shooter =
+          builder->add_shooter();
+      shooter->set_theta_finisher(
           encoder_translate(-finisher_encoder_->GetRaw(),
                             Values::kFinisherEncoderCountsPerRevolution(),
-                            Values::kFinisherEncoderRatio());
+                            Values::kFinisherEncoderRatio()));
 
-      shooter.theta_accelerator_left =
+      shooter->set_theta_accelerator_left(
           encoder_translate(-left_accelerator_encoder_->GetRaw(),
                             Values::kAcceleratorEncoderCountsPerRevolution(),
-                            Values::kAcceleratorEncoderRatio());
-      shooter.theta_accelerator_right =
+                            Values::kAcceleratorEncoderRatio()));
+      shooter->set_theta_accelerator_right(
           encoder_translate(right_accelerator_encoder_->GetRaw(),
                             Values::kAcceleratorEncoderCountsPerRevolution(),
-                            Values::kAcceleratorEncoderRatio());
-      flatbuffers::Offset<y2020::control_loops::superstructure::ShooterPosition>
-          shooter_offset =
-              y2020::control_loops::superstructure::ShooterPosition::Pack(
-                  *builder.fbb(), &shooter);
+                            Values::kAcceleratorEncoderRatio()));
+      builder->set_intake_beambreak_triggered(ball_intake_beambreak_->Get());
 
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
-      position_builder.add_hood(hood_offset);
-      position_builder.add_intake_joint(intake_joint_offset);
-      position_builder.add_turret(turret_offset);
-      position_builder.add_shooter(shooter_offset);
-      position_builder.add_intake_beambreak_triggered(
-          ball_intake_beambreak_->Get());
-
-      builder.CheckOk(builder.Send(position_builder.Finish()));
+      builder.CheckOk(builder.Send());
     }
 
     {
@@ -386,7 +364,7 @@
 
  private:
   ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
-  ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+  ::aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   ::aos::Sender<::frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
   ::aos::Sender<superstructure::shooter::TuningReadings>
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index 28c35aa..5783b4f 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -2,7 +2,8 @@
 import {Connection} from '../../aos/network/www/proxy';
 import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated';
 import {LocalizerDebug, RejectionReason, ImageMatchDebug} from '../control_loops/drivetrain/localizer_debug_generated';
-import {Status as SuperstructureStatus, FlywheelControllerStatus} from '../control_loops/superstructure/superstructure_status_generated'
+import {Status as SuperstructureStatus} from '../control_loops/superstructure/superstructure_status_generated'
+import {FlywheelControllerStatus} from '../../frc971/control_loops/flywheel/flywheel_controller_status_generated'
 import {ImageMatchResult} from '../vision/sift/sift_generated';
 
 import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
index be18cbe..606c6df 100644
--- a/y2021_bot3/control_loops/superstructure/BUILD
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -1,49 +1,45 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
diff --git a/y2022/BUILD b/y2022/BUILD
index 8d9d901..a839f37 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -1,6 +1,6 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//frc971:downloader.bzl", "robot_downloader")
 load("//aos:config.bzl", "aos_config")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("//tools/build_rules:template.bzl", "jinja2_template")
 load("//aos/util:config_validator_macro.bzl", "config_validator_test")
 
@@ -303,12 +303,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "setpoint_fbs",
     srcs = [
         "setpoint.fbs",
     ],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
 )
 
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 8ba3367..04ca14c 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -39,8 +39,8 @@
 
   intake_params.zeroing_voltage = 3.0;
   intake_params.operating_voltage = 12.0;
-  intake_params.zeroing_profile_params = {0.5, 3.0};
-  intake_params.default_profile_params = {6.0, 30.0};
+  intake_params.zeroing_profile_params = {{}, 0.5, 3.0};
+  intake_params.default_profile_params = {{}, 6.0, 30.0};
   intake_params.range = Values::kIntakeRange();
   intake_params.make_integral_loop =
       control_loops::superstructure::intake::MakeIntegralIntakeLoop;
@@ -70,9 +70,9 @@
 
   turret_params->zeroing_voltage = 4.0;
   turret_params->operating_voltage = 12.0;
-  turret_params->zeroing_profile_params = {0.5, 2.0};
-  turret_params->default_profile_params = {10.0, 20.0};
-  turret_params->default_profile_params = {15.0, 20.0};
+  turret_params->zeroing_profile_params = {{}, 0.5, 2.0};
+  turret_params->default_profile_params = {{}, 10.0, 20.0};
+  turret_params->default_profile_params = {{}, 15.0, 20.0};
   turret_params->range = *turret_range;
   turret_params->make_integral_loop =
       control_loops::superstructure::turret::MakeIntegralTurretLoop;
@@ -89,8 +89,8 @@
   auto *const climber = &r.climber;
   climber->subsystem_params.zeroing_voltage = 3.0;
   climber->subsystem_params.operating_voltage = 12.0;
-  climber->subsystem_params.zeroing_profile_params = {0.5, 0.1};
-  climber->subsystem_params.default_profile_params = {5.0, 1.0};
+  climber->subsystem_params.zeroing_profile_params = {{}, 0.5, 0.1};
+  climber->subsystem_params.default_profile_params = {{}, 5.0, 1.0};
   climber->subsystem_params.range = Values::kClimberRange();
   climber->subsystem_params.make_integral_loop =
       control_loops::superstructure::climber::MakeIntegralClimberLoop;
@@ -99,8 +99,8 @@
   Values::PotConstants flipper_arms;
   flipper_arms.subsystem_params.zeroing_voltage = 3.0;
   flipper_arms.subsystem_params.operating_voltage = 12.0;
-  flipper_arms.subsystem_params.zeroing_profile_params = {0.5, 0.1};
-  flipper_arms.subsystem_params.default_profile_params = {6.0, 1.0};
+  flipper_arms.subsystem_params.zeroing_profile_params = {{}, 0.5, 0.1};
+  flipper_arms.subsystem_params.default_profile_params = {{}, 6.0, 1.0};
   flipper_arms.subsystem_params.range = Values::kFlipperArmRange();
 
   auto *const flipper_arm_right = &r.flipper_arm_right;
@@ -119,8 +119,8 @@
 
   catapult_params->zeroing_voltage = 4.0;
   catapult_params->operating_voltage = 12.0;
-  catapult_params->zeroing_profile_params = {0.5, 2.0};
-  catapult_params->default_profile_params = {15.0, 40.0};
+  catapult_params->zeroing_profile_params = {{}, 0.5, 2.0};
+  catapult_params->default_profile_params = {{}, 15.0, 40.0};
   catapult_params->range = Values::kCatapultRange();
   catapult_params->make_integral_loop =
       &control_loops::superstructure::catapult::MakeIntegralCatapultLoop;
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index e370f0e..0f2737d 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -1,46 +1,42 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_can_position_fbs",
     srcs = [
         "superstructure_can_position.fbs",
     ],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
@@ -55,15 +51,14 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index 7f076e8..63f720f 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -1,5 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
 
@@ -16,12 +16,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "localizer_status_fbs",
     srcs = [
         "localizer_status.fbs",
     ],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
     deps = [
@@ -42,16 +41,15 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "localizer_visualization_fbs",
     srcs = ["localizer_visualization.fbs"],
-    gen_reflections = 1,
-    includes = [
-        ":localizer_status_fbs_includes",
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops/drivetrain:drivetrain_status_fbs_includes",
-    ],
     visibility = ["//visibility:public"],
+    deps = [
+        ":localizer_status_fbs",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+    ],
 )
 
 flatbuffer_ts_library(
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 99fb95d..523ee04 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -1,5 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 ts_project(
@@ -176,15 +176,14 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "ball_color_fbs",
     srcs = ["ball_color.fbs"],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/input:joystick_state_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2022:__subpackages__"],
+    deps = [
+        "//frc971/input:joystick_state_fbs",
+    ],
 )
 
 cc_library(
@@ -231,15 +230,14 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "target_estimate_fbs",
     srcs = ["target_estimate.fbs"],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/vision:calibration_fbs_includes",
-    ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2022:__subpackages__"],
+    deps = [
+        "//frc971/vision:calibration_fbs",
+    ],
 )
 
 flatbuffer_ts_library(
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 5b3cd96..55cf510 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -55,7 +55,7 @@
 #include "y2022/control_loops/superstructure/led_indicator.h"
 #include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_position_static.h"
 
 using ::aos::monotonic_clock;
 using ::y2022::constants::Values;
@@ -167,7 +167,7 @@
             event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
                 "/autonomous")),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -227,74 +227,44 @@
   void RunIteration() override {
     superstructure_reading_->Set(true);
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
 
-      frc971::PotAndAbsolutePositionT catapult;
-      CopyPosition(catapult_encoder_, &catapult,
+      CopyPosition(catapult_encoder_, builder->add_catapult(),
                    Values::kCatapultEncoderCountsPerRevolution(),
                    Values::kCatapultEncoderRatio(), catapult_pot_translate,
                    false, values_->catapult.potentiometer_offset);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> catapult_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &catapult);
 
-      frc971::RelativePositionT climber;
-      CopyPosition(*climber_potentiometer_, &climber, climber_pot_translate,
-                   false, values_->climber.potentiometer_offset);
-      flatbuffers::Offset<frc971::RelativePosition> climber_offset =
-          frc971::RelativePosition::Pack(*builder.fbb(), &climber);
+      CopyPosition(*climber_potentiometer_, builder->add_climber(),
+                   climber_pot_translate, false,
+                   values_->climber.potentiometer_offset);
 
-      frc971::RelativePositionT flipper_arm_left;
-      CopyPosition(*flipper_arm_left_potentiometer_, &flipper_arm_left,
-                   flipper_arms_pot_translate, false,
-                   values_->flipper_arm_left.potentiometer_offset);
+      CopyPosition(*flipper_arm_left_potentiometer_,
+                   builder->add_flipper_arm_left(), flipper_arms_pot_translate,
+                   false, values_->flipper_arm_left.potentiometer_offset);
 
-      frc971::RelativePositionT flipper_arm_right;
-      CopyPosition(*flipper_arm_right_potentiometer_, &flipper_arm_right,
-                   flipper_arms_pot_translate, true,
-                   values_->flipper_arm_right.potentiometer_offset);
+      CopyPosition(*flipper_arm_right_potentiometer_,
+                   builder->add_flipper_arm_right(), flipper_arms_pot_translate,
+                   true, values_->flipper_arm_right.potentiometer_offset);
 
       // Intake
-      frc971::PotAndAbsolutePositionT intake_front;
-      CopyPosition(intake_encoder_front_, &intake_front,
+      CopyPosition(intake_encoder_front_, builder->add_intake_front(),
                    Values::kIntakeEncoderCountsPerRevolution(),
                    Values::kIntakeEncoderRatio(), intake_pot_translate, true,
                    values_->intake_front.potentiometer_offset);
-      frc971::PotAndAbsolutePositionT intake_back;
-      CopyPosition(intake_encoder_back_, &intake_back,
+      CopyPosition(intake_encoder_back_, builder->add_intake_back(),
                    Values::kIntakeEncoderCountsPerRevolution(),
                    Values::kIntakeEncoderRatio(), intake_pot_translate, true,
                    values_->intake_back.potentiometer_offset);
-      frc971::PotAndAbsolutePositionT turret;
-      CopyPosition(turret_encoder_, &turret,
+      CopyPosition(turret_encoder_, builder->add_turret(),
                    Values::kTurretEncoderCountsPerRevolution(),
                    Values::kTurretEncoderRatio(), turret_pot_translate, false,
                    values_->turret.potentiometer_offset);
 
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset_front =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake_front);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset_back =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake_back);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
-      flatbuffers::Offset<frc971::RelativePosition> flipper_arm_left_offset =
-          frc971::RelativePosition::Pack(*builder.fbb(), &flipper_arm_left);
-      flatbuffers::Offset<frc971::RelativePosition> flipper_arm_right_offset =
-          frc971::RelativePosition::Pack(*builder.fbb(), &flipper_arm_right);
-
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
-      position_builder.add_climber(climber_offset);
-      position_builder.add_flipper_arm_left(flipper_arm_left_offset);
-      position_builder.add_flipper_arm_right(flipper_arm_right_offset);
-      position_builder.add_intake_front(intake_offset_front);
-      position_builder.add_intake_back(intake_offset_back);
-      position_builder.add_turret(turret_offset);
-      position_builder.add_intake_beambreak_front(
-          intake_beambreak_front_->Get());
-      position_builder.add_intake_beambreak_back(intake_beambreak_back_->Get());
-      position_builder.add_turret_beambreak(turret_beambreak_->Get());
-      position_builder.add_catapult(catapult_offset);
-      builder.CheckOk(builder.Send(position_builder.Finish()));
+      builder->set_intake_beambreak_front(intake_beambreak_front_->Get());
+      builder->set_intake_beambreak_back(intake_beambreak_back_->Get());
+      builder->set_turret_beambreak(turret_beambreak_->Get());
+      builder.CheckOk(builder.Send());
     }
 
     {
@@ -454,7 +424,7 @@
   std::shared_ptr<const Values> values_;
 
   aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
-  aos::Sender<superstructure::Position> superstructure_position_sender_;
+  aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   aos::Sender<frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
   ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
diff --git a/y2022_bot3/constants.cc b/y2022_bot3/constants.cc
index 82982aa..ef5ad32 100644
--- a/y2022_bot3/constants.cc
+++ b/y2022_bot3/constants.cc
@@ -44,8 +44,8 @@
 
   intake_params.zeroing_voltage = 3.0;
   intake_params.operating_voltage = 12.0;
-  intake_params.zeroing_profile_params = {0.5, 3.0};
-  intake_params.default_profile_params = {6.0, 30.0};
+  intake_params.zeroing_profile_params = {{}, 0.5, 3.0};
+  intake_params.default_profile_params = {{}, 6.0, 30.0};
   intake_params.range = Values::kIntakeRange();
   intake_params.make_integral_loop =
       control_loops::superstructure::intake::MakeIntegralIntakeLoop;
@@ -66,8 +66,8 @@
 
   climber_params.zeroing_voltage = 3.0;
   climber_params.operating_voltage = 12.0;
-  climber_params.zeroing_profile_params = {0.5, 0.1};
-  climber_params.default_profile_params = {5.0, 1.0};
+  climber_params.zeroing_profile_params = {{}, 0.5, 0.1};
+  climber_params.default_profile_params = {{}, 5.0, 1.0};
   climber_params.range = Values::kClimberRange();
   climber_params.make_integral_loop =
       control_loops::superstructure::climber::MakeIntegralClimberLoop;
diff --git a/y2022_bot3/control_loops/superstructure/BUILD b/y2022_bot3/control_loops/superstructure/BUILD
index 5b44e2a..a3f74f9 100644
--- a/y2022_bot3/control_loops/superstructure/BUILD
+++ b/y2022_bot3/control_loops/superstructure/BUILD
@@ -1,37 +1,34 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
@@ -46,15 +43,14 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
-    includes = [
-        "//frc971/control_loops:control_loops_fbs_includes",
-        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
diff --git a/y2022_bot3/wpilib_interface.cc b/y2022_bot3/wpilib_interface.cc
index 4641e84..c441f6e 100644
--- a/y2022_bot3/wpilib_interface.cc
+++ b/y2022_bot3/wpilib_interface.cc
@@ -51,7 +51,7 @@
 #include "frc971/wpilib/wpilib_robot_base.h"
 #include "y2022_bot3/constants.h"
 #include "y2022_bot3/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2022_bot3/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2022_bot3/control_loops/superstructure/superstructure_position_static.h"
 
 using ::aos::monotonic_clock;
 using ::y2022_bot3::constants::Values;
@@ -112,7 +112,7 @@
             event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
                 "/autonomous")),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -195,11 +195,11 @@
 
   void RunIteration() override {
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
 
       // Climbers
-      frc971::PotAndAbsolutePositionT climber_right;
-      CopyPosition(climber_encoder_right_, &climber_right,
+      CopyPosition(climber_encoder_right_, builder->add_climber_right(),
                    Values::kClimberEncoderCountsPerRevolution(),
                    (Values::kClimberEncoderRatio() *
                     Values::kClimberEncoderCountsPerRevolution()) /
@@ -207,8 +207,7 @@
                    climber_pot_translate, true,
                    values_->climber_right.potentiometer_offset);
 
-      frc971::PotAndAbsolutePositionT climber_left;
-      CopyPosition(climber_encoder_left_, &climber_left,
+      CopyPosition(climber_encoder_left_, builder->add_climber_left(),
                    Values::kClimberEncoderCountsPerRevolution(),
                    (Values::kClimberEncoderRatio() *
                     Values::kClimberEncoderCountsPerRevolution()) /
@@ -217,25 +216,12 @@
                    values_->climber_left.potentiometer_offset);
 
       // Intake
-      frc971::PotAndAbsolutePositionT intake;
-      CopyPosition(intake_encoder_, &intake,
+      CopyPosition(intake_encoder_, builder->add_intake(),
                    Values::kIntakeEncoderCountsPerRevolution(),
                    Values::kIntakeEncoderRatio(), intake_pot_translate, true,
                    values_->intake.potentiometer_offset);
 
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> climber_offset_right =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &climber_right);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> climber_offset_left =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &climber_left);
-
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
-      position_builder.add_intake(intake_offset);
-      position_builder.add_climber_right(climber_offset_right);
-      position_builder.add_climber_left(climber_offset_left);
-      builder.CheckOk(builder.Send(position_builder.Finish()));
+      builder.CheckOk(builder.Send());
     }
 
     {
@@ -318,7 +304,7 @@
   std::shared_ptr<const Values> values_;
 
   aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
-  aos::Sender<superstructure::Position> superstructure_position_sender_;
+  aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   aos::Sender<frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
   ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
diff --git a/y2023/BUILD b/y2023/BUILD
index d934927..2fb9e78 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -372,16 +372,3 @@
     ],
     target_compatible_with = ["@platforms//os:linux"],
 )
-
-cc_binary(
-    name = "ssd_profiler",
-    srcs = [
-        "ssd_profiler.cc",
-    ],
-    deps = [
-        "//aos:init",
-        "//aos/containers:resizeable_buffer",
-        "//aos/time",
-        "@com_github_google_glog//:glog",
-    ],
-)
diff --git a/y2023/autonomous/BUILD b/y2023/autonomous/BUILD
index cfcc19c..3f2066f 100644
--- a/y2023/autonomous/BUILD
+++ b/y2023/autonomous/BUILD
@@ -48,7 +48,7 @@
         "//aos/events:event_loop",
         "//aos/logging",
         "//aos/util:phased_loop",
-        "//frc971/autonomous:base_autonomous_actor",
+        "//frc971/autonomous:user_button_localized_autonomous_actor",
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
         "//frc971/control_loops/drivetrain:drivetrain_config",
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 8891223..e452d21 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -39,15 +39,12 @@
 namespace chrono = ::std::chrono;
 
 AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
-    : frc971::autonomous::BaseAutonomousActor(
+    : frc971::autonomous::UserButtonLocalizedAutonomousActor(
           event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
       localizer_control_sender_(
           event_loop->MakeSender<
               ::frc971::control_loops::drivetrain::LocalizerControl>(
               "/drivetrain")),
-      joystick_state_fetcher_(
-          event_loop->MakeFetcher<aos::JoystickState>("/aos")),
-      robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
       auto_splines_(),
       arm_goal_position_(control_loops::superstructure::arm::StartingIndex()),
       superstructure_goal_sender_(
@@ -57,57 +54,9 @@
           event_loop
               ->MakeFetcher<::y2023::control_loops::superstructure::Status>(
                   "/superstructure")),
-      points_(control_loops::superstructure::arm::PointList()) {
-  drivetrain_status_fetcher_.Fetch();
-  replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
-
-  event_loop->OnRun([this, event_loop]() {
-    replan_timer_->Schedule(event_loop->monotonic_now());
-    button_poll_->Schedule(event_loop->monotonic_now(),
-                           chrono::milliseconds(50));
-  });
-
-  // TODO(james): Really need to refactor this code since we keep using it.
-  button_poll_ = event_loop->AddTimer([this]() {
-    const aos::monotonic_clock::time_point now =
-        this->event_loop()->context().monotonic_event_time;
-    if (robot_state_fetcher_.Fetch()) {
-      if (robot_state_fetcher_->user_button()) {
-        user_indicated_safe_to_reset_ = true;
-        MaybeSendStartingPosition();
-      }
-    }
-    if (joystick_state_fetcher_.Fetch()) {
-      if (joystick_state_fetcher_->has_alliance() &&
-          (joystick_state_fetcher_->alliance() != alliance_)) {
-        alliance_ = joystick_state_fetcher_->alliance();
-        is_planned_ = false;
-        // Only kick the planning out by 2 seconds. If we end up enabled in
-        // that second, then we will kick it out further based on the code
-        // below.
-        replan_timer_->Schedule(now + std::chrono::seconds(2));
-      }
-      if (joystick_state_fetcher_->enabled()) {
-        if (!is_planned_) {
-          // Only replan once we've been disabled for 5 seconds.
-          replan_timer_->Schedule(now + std::chrono::seconds(5));
-        }
-      }
-    }
-  });
-}
+      points_(control_loops::superstructure::arm::PointList()) {}
 
 void AutonomousActor::Replan() {
-  if (!drivetrain_status_fetcher_.Fetch()) {
-    replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
-    AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
-    return;
-  }
-
-  if (alliance_ == aos::Alliance::kInvalid) {
-    return;
-  }
-  sent_starting_position_ = false;
   if (FLAGS_spline_auto) {
     test_spline_ =
         PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
@@ -158,23 +107,7 @@
   MaybeSendStartingPosition();
 }
 
-void AutonomousActor::MaybeSendStartingPosition() {
-  if (is_planned_ && user_indicated_safe_to_reset_ &&
-      !sent_starting_position_) {
-    CHECK(starting_position_);
-    SendStartingPosition(starting_position_.value());
-  }
-}
-
 void AutonomousActor::Reset() {
-  InitializeEncoders();
-  ResetDrivetrain();
-
-  joystick_state_fetcher_.Fetch();
-  CHECK(joystick_state_fetcher_.get() != nullptr)
-      << "Expect at least one JoystickState message before running auto...";
-  alliance_ = joystick_state_fetcher_->alliance();
-
   wrist_goal_ = 0.0;
   roller_goal_ = control_loops::superstructure::RollerGoal::IDLE;
   arm_goal_position_ = control_loops::superstructure::arm::StartingIndex();
@@ -182,23 +115,8 @@
   SendSuperstructureGoal();
 }
 
-bool AutonomousActor::RunAction(
+bool AutonomousActor::Run(
     const ::frc971::autonomous::AutonomousActionParams *params) {
-  Reset();
-
-  AOS_LOG(INFO, "Params are %d\n", params->mode());
-
-  if (!user_indicated_safe_to_reset_) {
-    AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
-    CHECK(starting_position_);
-    SendStartingPosition(starting_position_.value());
-  }
-  // Clear this so that we don't accidentally resend things as soon as we
-  // replan later.
-  user_indicated_safe_to_reset_ = false;
-  is_planned_ = false;
-  starting_position_.reset();
-
   AOS_LOG(INFO, "Params are %d\n", params->mode());
   if (alliance_ == aos::Alliance::kInvalid) {
     AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index 31ae4f5..5a5cd06 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -3,7 +3,7 @@
 
 #include "aos/actions/actions.h"
 #include "aos/actions/actor.h"
-#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/autonomous/user_button_localized_autonomous_actor.h"
 #include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
@@ -14,13 +14,11 @@
 namespace y2023 {
 namespace autonomous {
 
-class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+class AutonomousActor
+    : public ::frc971::autonomous::UserButtonLocalizedAutonomousActor {
  public:
   explicit AutonomousActor(::aos::EventLoop *event_loop);
 
-  bool RunAction(
-      const ::frc971::autonomous::AutonomousActionParams *params) override;
-
  private:
   void set_arm_goal_position(uint32_t requested_arm_goal_position) {
     arm_goal_position_ = requested_arm_goal_position;
@@ -48,39 +46,27 @@
   void IntakeCube();
   void Balance();
 
+  bool Run(const ::frc971::autonomous::AutonomousActionParams *params) override;
+  void Replan() override;
+  void SendStartingPosition(const Eigen::Vector3d &start) override;
+  void Reset() override;
+
   [[nodiscard]] bool WaitForArmGoal(double distance_to_go = 0.01);
 
   [[nodiscard]] bool WaitForPreloaded();
 
-  void Reset();
-
-  void SendStartingPosition(const Eigen::Vector3d &start);
-  void MaybeSendStartingPosition();
   void SplineAuto();
   void ChargedUp();
   void ChargedUpCableSide();
-  void Replan();
 
   aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
       localizer_control_sender_;
-  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
-  aos::Fetcher<aos::RobotState> robot_state_fetcher_;
 
   double wrist_goal_;
   control_loops::superstructure::RollerGoal roller_goal_ =
       control_loops::superstructure::RollerGoal::IDLE;
 
-  aos::TimerHandler *replan_timer_;
-  aos::TimerHandler *button_poll_;
-
-  aos::Alliance alliance_ = aos::Alliance::kInvalid;
   AutonomousSplines auto_splines_;
-  bool user_indicated_safe_to_reset_ = false;
-  bool sent_starting_position_ = false;
-
-  bool is_planned_ = false;
-
-  std::optional<Eigen::Vector3d> starting_position_;
 
   uint32_t arm_goal_position_;
   bool preloaded_ = false;
diff --git a/y2023/constants.cc b/y2023/constants.cc
index dcce935..6b63cda 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -49,8 +49,8 @@
 
   wrist->subsystem_params.zeroing_voltage = 3.0;
   wrist->subsystem_params.operating_voltage = 12.0;
-  wrist->subsystem_params.zeroing_profile_params = {0.5, 3.0};
-  wrist->subsystem_params.default_profile_params = {0.5, 5.0};
+  wrist->subsystem_params.zeroing_profile_params = {{}, 0.5, 3.0};
+  wrist->subsystem_params.default_profile_params = {{}, 0.5, 5.0};
   wrist->subsystem_params.range = Values::kCompWristRange();
   wrist->subsystem_params.make_integral_loop =
       control_loops::superstructure::wrist::MakeIntegralWristLoop;
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index c91aa56..512e400 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:template.bzl", "jinja2_template")
 
 cc_library(
@@ -49,10 +49,9 @@
     visibility = ["//y2023:__subpackages__"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "constants_fbs",
     srcs = ["constants.fbs"],
-    gen_reflections = True,
     visibility = ["//visibility:public"],
     deps = [
         "//frc971/vision:calibration_fbs",
@@ -61,10 +60,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "constants_list_fbs",
     srcs = ["constants_list.fbs"],
-    gen_reflections = True,
     visibility = ["//visibility:public"],
     deps = [":constants_fbs"],
 )
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index f6b5655..01c360b 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -1,5 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//aos:config.bzl", "aos_config")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 genrule(
     name = "genrule_drivetrain",
@@ -115,21 +115,19 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "target_selector_status_fbs",
     srcs = [
         ":target_selector_status.fbs",
     ],
-    gen_reflections = 1,
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "target_selector_hint_fbs",
     srcs = [
         ":target_selector_hint.fbs",
     ],
-    gen_reflections = 1,
     visibility = ["//visibility:public"],
     deps = [
         "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 3d35dae..f62d790 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -1,35 +1,32 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
     deps = [
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
     deps = [
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
@@ -49,12 +46,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
     deps = [
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index 768a54e..2f4e7d6 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 cc_library(
     name = "arm",
@@ -73,12 +73,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "arm_trajectories_fbs",
     srcs = [
         "arm_trajectories.fbs",
     ],
-    gen_reflections = 1,
     visibility = ["//visibility:public"],
 )
 
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index f9d0d28..eb0f886 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:js.bzl", "ts_project")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
@@ -15,12 +15,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "status_fbs",
     srcs = [
         "status.fbs",
     ],
-    gen_reflections = True,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
     deps = [
@@ -39,12 +38,11 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "visualization_fbs",
     srcs = [
         "visualization.fbs",
     ],
-    gen_reflections = True,
     visibility = ["//visibility:public"],
     deps = [
         ":status_fbs",
@@ -60,24 +58,22 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "scoring_map_fbs",
     srcs = [
         "scoring_map.fbs",
     ],
-    gen_reflections = True,
     visibility = ["//visibility:public"],
     deps = [
         "//frc971/vision:target_map_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "relative_scoring_map_fbs",
     srcs = [
         "relative_scoring_map.fbs",
     ],
-    gen_reflections = True,
     visibility = ["//visibility:public"],
     deps = [
         ":scoring_map_fbs",
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 512bb58..07911a8 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,7 +1,7 @@
 load("//frc971:halide.bzl", "halide_library")
 load("//tools/build_rules:select.bzl", "cpu_select")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 
 cc_binary(
     name = "camera_reader",
@@ -263,6 +263,7 @@
         "//frc971/control_loops:pose",
         "//frc971/vision:calibration_fbs",
         "//frc971/vision:charuco_lib",
+        "//frc971/vision:extrinsics_calibration",
         "//frc971/vision:target_mapper",
         "//third_party:opencv",
         "//y2023/constants:constants_fbs",
@@ -271,6 +272,23 @@
     ],
 )
 
+py_test(
+    name = "calibrate_multi_cameras_test",
+    srcs = ["calibrate_multi_cameras_test.py"],
+    args = [
+        "--calibrate_binary",
+        "$(location :calibrate_multi_cameras)",
+        "--logfile",
+        "external/calibrate_multi_cameras_data/fbs_log-268",
+    ],
+    data = [
+        ":calibrate_multi_cameras",
+        "//y2023/vision/calib_files",
+        "@calibrate_multi_cameras_data",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+)
+
 cc_binary(
     name = "game_pieces_detector",
     srcs = [
@@ -308,10 +326,9 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "game_pieces_fbs",
     srcs = ["game_pieces.fbs"],
-    gen_reflections = 1,
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
 )
diff --git a/y2023/vision/calibrate_multi_cameras.cc b/y2023/vision/calibrate_multi_cameras.cc
index 4cd70c2..d3cfa6f 100644
--- a/y2023/vision/calibrate_multi_cameras.cc
+++ b/y2023/vision/calibrate_multi_cameras.cc
@@ -9,6 +9,7 @@
 #include "frc971/control_loops/quaternion_utils.h"
 #include "frc971/vision/calibration_generated.h"
 #include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/extrinsics_calibration.h"
 #include "frc971/vision/target_mapper.h"
 #include "frc971/vision/visualize_robot.h"
 // clang-format off
@@ -31,14 +32,26 @@
               "If set, override the log's config file with this one.");
 DEFINE_string(constants_path, "y2023/constants/constants.json",
               "Path to the constant file");
+DEFINE_double(max_pose_error, 5e-5,
+              "Throw out target poses with a higher pose error than this");
+DEFINE_double(
+    max_pose_error_ratio, 0.4,
+    "Throw out target poses with a higher pose error ratio than this");
+DEFINE_string(output_folder, "/tmp",
+              "Directory in which to store the updated calibration files");
 DEFINE_string(target_type, "charuco_diamond",
               "Type of target being used [aruco, charuco, charuco_diamond]");
 DEFINE_int32(team_number, 0,
              "Required: Use the calibration for a node with this team number");
+DEFINE_bool(use_full_logs, false,
+            "If true, extract data from logs with images");
 DEFINE_uint64(
     wait_key, 1,
     "Time in ms to wait between images (0 to wait indefinitely until click)");
 
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+
 // Calibrate extrinsic relationship between cameras using two targets
 // seen jointly between cameras.  Uses two types of information: 1)
 // when a single camera sees two targets, we estimate the pose between
@@ -50,7 +63,6 @@
 // and then map each subsequent camera based on the data collected and
 // the extrinsic poses computed here.
 
-// TODO<Jim>: Should export a new extrinsic file for each camera
 // TODO<Jim>: Not currently using estimate from pi1->pi4-- should do full
 // estimation, and probably also include camera->imu extrinsics from all
 // cameras, not just pi1
@@ -159,20 +171,10 @@
                             translation_variance, rotation_variance);
 }
 
-void ProcessImage(aos::EventLoop *event_loop, cv::Mat rgb_image,
-                  const aos::monotonic_clock::time_point eof,
-                  aos::distributed_clock::time_point distributed_eof,
-                  frc971::vision::CharucoExtractor &charuco_extractor,
-                  std::string pi_name) {
-  std::vector<cv::Vec4i> charuco_ids;
-  std::vector<std::vector<cv::Point2f>> charuco_corners;
-  bool valid = false;
-  std::vector<Eigen::Vector3d> rvecs_eigen;
-  std::vector<Eigen::Vector3d> tvecs_eigen;
-  charuco_extractor.ProcessImage(rgb_image, eof, event_loop->monotonic_now(),
-                                 charuco_ids, charuco_corners, valid,
-                                 rvecs_eigen, tvecs_eigen);
-
+void HandlePoses(cv::Mat rgb_image,
+                 std::vector<TargetMapper::TargetPose> target_poses,
+                 aos::distributed_clock::time_point distributed_eof,
+                 std::string node_name) {
   // This is used to transform points for visualization
   // Assumes targets are aligned with x->right, y->up, z->out of board
   Eigen::Affine3d H_world_board;
@@ -184,178 +186,228 @@
   }
 
   bool draw_vis = false;
-  if (valid) {
-    CHECK_LE(tvecs_eigen.size(), 2u)
-        << "Can't handle more than two tags in field of view";
-    if (tvecs_eigen.size() == 2) {
-      draw_vis = true;
-      VLOG(2) << "Saw two boards in same view from " << pi_name;
-      // Handle when we see two boards at once
-      // We'll store them referenced to the lower id board
-      int from_index = 0;
-      int to_index = 1;
-      if (charuco_ids[from_index][0] > charuco_ids[to_index][0]) {
-        std::swap<int>(from_index, to_index);
+  CHECK_LE(target_poses.size(), 2u)
+      << "Can't handle more than two tags in field of view";
+  if (target_poses.size() == 2) {
+    draw_vis = true;
+    VLOG(1) << "Saw two boards in same view from " << node_name;
+    int from_index = 0;
+    int to_index = 1;
+    // Handle when we see two boards at once
+    // We'll store them referenced to the lower id board
+    if (target_poses[from_index].id > target_poses[to_index].id) {
+      std::swap<int>(from_index, to_index);
+    }
+
+    // Create "from" (A) and "to" (B) transforms
+    Eigen::Affine3d H_camera_boardA =
+        PoseUtils::Pose3dToAffine3d(target_poses[from_index].pose);
+    Eigen::Affine3d H_camera_boardB =
+        PoseUtils::Pose3dToAffine3d(target_poses[to_index].pose);
+
+    Eigen::Affine3d H_boardA_boardB =
+        H_camera_boardA.inverse() * H_camera_boardB;
+
+    TimestampedPiDetection boardA_boardB{
+        .time = distributed_eof,
+        .H_camera_target = H_boardA_boardB,
+        .pi_name = node_name,
+        .board_id = target_poses[from_index].id};
+
+    VLOG(1) << "Map from board " << from_index << " to " << to_index << " is\n"
+            << H_boardA_boardB.matrix();
+    // Store this observation of the transform between two boards
+    two_board_extrinsics_list.push_back(boardA_boardB);
+
+    if (FLAGS_visualize) {
+      vis_robot_.DrawFrameAxes(
+          H_world_board,
+          std::string("board ") + std::to_string(target_poses[from_index].id),
+          cv::Scalar(0, 255, 0));
+      vis_robot_.DrawFrameAxes(
+          H_world_board * boardA_boardB.H_camera_target,
+          std::string("board ") + std::to_string(target_poses[to_index].id),
+          cv::Scalar(255, 0, 0));
+      VLOG(2) << "Detection map from camera " << node_name << " to board "
+              << target_poses[from_index].id << " is\n"
+              << H_camera_boardA.matrix() << "\n and inverse is\n"
+              << H_camera_boardA.inverse().matrix()
+              << "\n and with world to board rotation is\n"
+              << H_world_board * H_camera_boardA.inverse().matrix();
+      vis_robot_.DrawRobotOutline(H_world_board * H_camera_boardA.inverse(),
+                                  node_name, cv::Scalar(0, 0, 255));
+    }
+  } else if (target_poses.size() == 1) {
+    VLOG(1) << "Saw single board in camera " << node_name;
+    Eigen::Affine3d H_camera2_board2 =
+        PoseUtils::Pose3dToAffine3d(target_poses[0].pose);
+    TimestampedPiDetection new_observation{.time = distributed_eof,
+                                           .H_camera_target = H_camera2_board2,
+                                           .pi_name = node_name,
+                                           .board_id = target_poses[0].id};
+
+    // Only take two observations if they're within half an image cycle of each
+    // other (i.e., as close in time as possible)
+    if (std::abs((distributed_eof - last_observation.time).count()) <
+        kImagePeriodMs / 2.0 * 1000000.0) {
+      // Sort by pi numbering, since this is how we will handle them
+      std::pair<TimestampedPiDetection, TimestampedPiDetection> new_pair;
+      if (last_observation.pi_name < new_observation.pi_name) {
+        new_pair = std::pair(last_observation, new_observation);
+      } else if (last_observation.pi_name > new_observation.pi_name) {
+        new_pair = std::pair(new_observation, last_observation);
+      } else {
+        LOG(WARNING) << "Got 2 observations in a row from same pi. Probably "
+                        "not too much of an issue???";
       }
+      detection_list.push_back(new_pair);
 
-      // Create "from" (A) and "to" (B) transforms
-      Eigen::Quaternion<double> rotationA(
-          frc971::controls::ToQuaternionFromRotationVector(
-              rvecs_eigen[from_index]));
-      Eigen::Translation3d translationA(tvecs_eigen[from_index]);
-      Eigen::Affine3d H_camera_boardA = translationA * rotationA;
+      // This bit is just for visualization and checking purposes-- use the
+      // last two-board observation to figure out the current estimate
+      // between the two cameras
+      if (FLAGS_visualize && two_board_extrinsics_list.size() > 0) {
+        draw_vis = true;
+        TimestampedPiDetection &last_two_board_ext =
+            two_board_extrinsics_list[two_board_extrinsics_list.size() - 1];
+        Eigen::Affine3d &H_boardA_boardB = last_two_board_ext.H_camera_target;
+        int boardA_boardB_id = last_two_board_ext.board_id;
 
-      Eigen::Quaternion<double> rotationB(
-          frc971::controls::ToQuaternionFromRotationVector(
-              rvecs_eigen[to_index]));
-      Eigen::Translation3d translationB(tvecs_eigen[to_index]);
-      Eigen::Affine3d H_camera_boardB = translationB * rotationB;
+        TimestampedPiDetection camera1_boardA = new_pair.first;
+        TimestampedPiDetection camera2_boardB = new_pair.second;
+        // If camera1_boardA doesn't point to the first target, then swap
+        // these two
+        if (camera1_boardA.board_id != boardA_boardB_id) {
+          camera1_boardA = new_pair.second;
+          camera2_boardB = new_pair.first;
+        }
+        VLOG(1) << "Camera " << camera1_boardA.pi_name << " seeing board "
+                << camera1_boardA.board_id << " and camera "
+                << camera2_boardB.pi_name << " seeing board "
+                << camera2_boardB.board_id;
 
-      Eigen::Affine3d H_boardA_boardB =
-          H_camera_boardA.inverse() * H_camera_boardB;
-
-      TimestampedPiDetection boardA_boardB{
-          .time = distributed_eof,
-          .H_camera_target = H_boardA_boardB,
-          .pi_name = pi_name,
-          .board_id = charuco_ids[from_index][0]};
-
-      VLOG(1) << "Map from board " << from_index << " to " << to_index
-              << " is\n"
-              << H_boardA_boardB.matrix();
-      // Store this observation of the transform between two boards
-      two_board_extrinsics_list.push_back(boardA_boardB);
-
-      if (FLAGS_visualize) {
+        vis_robot_.DrawRobotOutline(
+            H_world_board * camera1_boardA.H_camera_target.inverse(),
+            camera1_boardA.pi_name, cv::Scalar(0, 0, 255));
+        vis_robot_.DrawRobotOutline(
+            H_world_board * H_boardA_boardB *
+                camera2_boardB.H_camera_target.inverse(),
+            camera2_boardB.pi_name, cv::Scalar(128, 128, 0));
         vis_robot_.DrawFrameAxes(
             H_world_board,
-            std::string("board ") + std::to_string(charuco_ids[from_index][0]),
+            std::string("Board ") + std::to_string(last_two_board_ext.board_id),
             cv::Scalar(0, 255, 0));
-        vis_robot_.DrawFrameAxes(
-            H_world_board * boardA_boardB.H_camera_target,
-            std::string("board ") + std::to_string(charuco_ids[to_index][0]),
-            cv::Scalar(255, 0, 0));
-        VLOG(2) << "Detection map from camera " << pi_name << " to board "
-                << charuco_ids[from_index][0] << " is\n"
-                << H_camera_boardA.matrix() << "\n and inverse is\n"
-                << H_camera_boardA.inverse().matrix()
-                << "\n and with world to board rotation is\n"
-                << H_world_board * H_camera_boardA.inverse().matrix();
-        vis_robot_.DrawRobotOutline(H_world_board * H_camera_boardA.inverse(),
-                                    pi_name, cv::Scalar(0, 0, 255));
+        vis_robot_.DrawFrameAxes(H_world_board * H_boardA_boardB, "Board B",
+                                 cv::Scalar(255, 0, 0));
       }
-
+      VLOG(1) << "Storing observation between " << new_pair.first.pi_name
+              << ", target " << new_pair.first.board_id << " and "
+              << new_pair.second.pi_name << ", target "
+              << new_pair.second.board_id;
     } else {
-      VLOG(1) << "Saw single board in camera " << pi_name;
-      Eigen::Quaternion<double> rotation(
-          frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[0]));
-      Eigen::Translation3d translation(tvecs_eigen[0]);
-      Eigen::Affine3d H_camera2_board2 = translation * rotation;
-      TimestampedPiDetection new_observation{
-          .time = distributed_eof,
-          .H_camera_target = H_camera2_board2,
-          .pi_name = pi_name,
-          .board_id = charuco_ids[0][0]};
-
-      VLOG(2) << "Checking versus last result from " << last_observation.pi_name
-              << " at time " << last_observation.time << " with delta time = "
-              << std::abs((distributed_eof - last_observation.time).count());
-      // Now, check if this new observation can be paired with a
-      // previous single board observation.
-
-      // Only take two observations if they're near enough to each
-      // other in time.  This should be within +/- kImagePeriodMs of
-      // each other (e.g., +/-16.666ms for 30 Hz capture).  This
-      // should make sure we're always getting the closest images, and
-      // not miss too many possible pairs, between cameras
-
-      // TODO<Jim>: Should also check that (rotational) velocity of the robot is
-      // small
-      if (std::abs((distributed_eof - last_observation.time).count()) <
-          static_cast<int>(kImagePeriodMs / 2.0 * 1000000)) {
-        // Sort by pi numbering, since this is how we will handle them
-        std::pair<TimestampedPiDetection, TimestampedPiDetection> new_pair;
-        if (last_observation.pi_name < new_observation.pi_name) {
-          new_pair = std::pair(last_observation, new_observation);
-        } else if (last_observation.pi_name > new_observation.pi_name) {
-          new_pair = std::pair(new_observation, last_observation);
-        } else {
-          LOG(WARNING) << "Got 2 observations in a row from same pi. Probably "
-                          "not too much of an issue???";
-        }
-        detection_list.push_back(new_pair);
-
-        // This bit is just for visualization and checking purposes-- use the
-        // last two-board observation to figure out the current estimate between
-        // the two cameras
-        if (FLAGS_visualize && two_board_extrinsics_list.size() > 0) {
-          draw_vis = true;
-          TimestampedPiDetection &last_two_board_ext =
-              two_board_extrinsics_list[two_board_extrinsics_list.size() - 1];
-          Eigen::Affine3d &H_boardA_boardB = last_two_board_ext.H_camera_target;
-          int boardA_boardB_id = last_two_board_ext.board_id;
-
-          TimestampedPiDetection camera1_boardA = new_pair.first;
-          TimestampedPiDetection camera2_boardB = new_pair.second;
-          // If camera1_boardA doesn't point to the first target, then swap
-          // these two
-          if (camera1_boardA.board_id != boardA_boardB_id) {
-            camera1_boardA = new_pair.second;
-            camera2_boardB = new_pair.first;
-          }
-          VLOG(1) << "Camera " << camera1_boardA.pi_name << " seeing board "
-                  << camera1_boardA.board_id << " and camera "
-                  << camera2_boardB.pi_name << " seeing board "
-                  << camera2_boardB.board_id;
-
-          vis_robot_.DrawRobotOutline(
-              H_world_board * camera1_boardA.H_camera_target.inverse(),
-              camera1_boardA.pi_name, cv::Scalar(0, 0, 255));
-          vis_robot_.DrawRobotOutline(
-              H_world_board * H_boardA_boardB *
-                  camera2_boardB.H_camera_target.inverse(),
-              camera2_boardB.pi_name, cv::Scalar(128, 128, 0));
-          vis_robot_.DrawFrameAxes(
-              H_world_board,
-              std::string("Board ") +
-                  std::to_string(last_two_board_ext.board_id),
-              cv::Scalar(0, 255, 0));
-          vis_robot_.DrawFrameAxes(H_world_board * H_boardA_boardB, "Board B",
-                                   cv::Scalar(255, 0, 0));
-          Eigen::Affine3d H_camera1_camera2 =
-              camera1_boardA.H_camera_target * H_boardA_boardB *
-              camera2_boardB.H_camera_target.inverse();
-
-          VLOG(1) << "Storing observation between " << new_pair.first.pi_name
-                  << ", target " << new_pair.first.board_id << " and "
-                  << new_pair.second.pi_name << ", target "
-                  << new_pair.second.board_id
-                  << " and camera-to-camera offset:\n"
-                  << H_camera1_camera2.matrix();
-        }
-      } else {
-        VLOG(2) << "Storing observation for " << pi_name << " at time "
-                << distributed_eof;
-        last_observation = new_observation;
-      }
+      VLOG(2) << "Storing observation for " << node_name << " at time "
+              << distributed_eof;
+      last_observation = new_observation;
     }
   }
 
   if (FLAGS_visualize) {
-    std::string image_name = pi_name + " Image";
-    cv::Mat rgb_small;
-    cv::resize(rgb_image, rgb_small, cv::Size(), 0.5, 0.5);
-    cv::imshow(image_name, rgb_small);
-    cv::waitKey(FLAGS_wait_key);
+    if (!rgb_image.empty()) {
+      std::string image_name = node_name + " Image";
+      cv::Mat rgb_small;
+      cv::resize(rgb_image, rgb_small, cv::Size(), 0.5, 0.5);
+      cv::imshow(image_name, rgb_small);
+      cv::waitKey(FLAGS_wait_key);
+    }
 
     if (draw_vis) {
       cv::imshow("View", vis_robot_.image_);
-      cv::waitKey(1);
+      cv::waitKey(FLAGS_wait_key);
       vis_robot_.ClearImage();
     }
   }
 }
 
+void HandleTargetMap(const TargetMap &map,
+                     aos::distributed_clock::time_point distributed_eof,
+                     std::string node_name) {
+  VLOG(1) << "Got april tag map call from node " << node_name;
+  // Create empty RGB image in this case
+  cv::Mat rgb_image;
+  std::vector<TargetMapper::TargetPose> target_poses;
+
+  for (const auto *target_pose_fbs : *map.target_poses()) {
+    // Skip detections with invalid ids
+    if (static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) <
+            FLAGS_min_target_id ||
+        static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) >
+            FLAGS_max_target_id) {
+      LOG(INFO) << "Skipping tag with invalid id of " << target_pose_fbs->id();
+      continue;
+    }
+
+    // Skip detections with high pose errors
+    if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
+      LOG(INFO) << "Skipping tag " << target_pose_fbs->id()
+                << " due to pose error of " << target_pose_fbs->pose_error();
+      continue;
+    }
+    // Skip detections with high pose error ratios
+    if (target_pose_fbs->pose_error_ratio() > FLAGS_max_pose_error_ratio) {
+      LOG(INFO) << "Skipping tag " << target_pose_fbs->id()
+                << " due to pose error ratio of "
+                << target_pose_fbs->pose_error_ratio();
+      continue;
+    }
+
+    const TargetMapper::TargetPose target_pose =
+        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+
+    target_poses.emplace_back(target_pose);
+
+    Eigen::Affine3d H_camera_target =
+        PoseUtils::Pose3dToAffine3d(target_pose.pose);
+    VLOG(2) << node_name << " saw target " << target_pose.id
+            << " from TargetMap at timestamp " << distributed_eof
+            << " with pose = " << H_camera_target.matrix();
+  }
+  HandlePoses(rgb_image, target_poses, distributed_eof, node_name);
+}
+
+void HandleImage(aos::EventLoop *event_loop, cv::Mat rgb_image,
+                 const aos::monotonic_clock::time_point eof,
+                 aos::distributed_clock::time_point distributed_eof,
+                 frc971::vision::CharucoExtractor &charuco_extractor,
+                 std::string node_name) {
+  std::vector<cv::Vec4i> charuco_ids;
+  std::vector<std::vector<cv::Point2f>> charuco_corners;
+  bool valid = false;
+  std::vector<Eigen::Vector3d> rvecs_eigen;
+  std::vector<Eigen::Vector3d> tvecs_eigen;
+  // Why eof vs. distributed_eof?
+  charuco_extractor.ProcessImage(rgb_image, eof, event_loop->monotonic_now(),
+                                 charuco_ids, charuco_corners, valid,
+                                 rvecs_eigen, tvecs_eigen);
+  if (rvecs_eigen.size() > 0 && !valid) {
+    LOG(WARNING) << "Charuco extractor returned not valid";
+    return;
+  }
+
+  std::vector<TargetMapper::TargetPose> target_poses;
+  for (size_t i = 0; i < tvecs_eigen.size(); i++) {
+    Eigen::Quaterniond rotation(
+        frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[i]));
+    ceres::examples::Pose3d pose(Eigen::Vector3d(tvecs_eigen[i]), rotation);
+    TargetMapper::TargetPose target_pose{charuco_ids[i][0], pose};
+    target_poses.emplace_back(target_pose);
+
+    Eigen::Affine3d H_camera_target = PoseUtils::Pose3dToAffine3d(pose);
+    VLOG(2) << node_name << " saw target " << target_pose.id
+            << " from image at timestamp " << distributed_eof
+            << " with pose = " << H_camera_target.matrix();
+  }
+  HandlePoses(rgb_image, target_poses, distributed_eof, node_name);
+}
+
 void ExtrinsicsMain(int argc, char *argv[]) {
   vis_robot_ = VisualizeRobot(cv::Size(1000, 1000));
   vis_robot_.ClearImage();
@@ -375,14 +427,13 @@
 
   constexpr size_t kNumPis = 4;
   for (size_t i = 1; i <= kNumPis; i++) {
-    reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
-                              "frc971.vision.TargetMap");
-    reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
-                              "foxglove.ImageAnnotations");
     reader.RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
                               "y2023.Constants");
   }
+
   reader.RemapLoggedChannel("/imu/constants", "y2023.Constants");
+  reader.RemapLoggedChannel("/logger/constants", "y2023.Constants");
+  reader.RemapLoggedChannel("/roborio/constants", "y2023.Constants");
   reader.Register();
 
   SendSimulationConstants(reader.event_loop_factory(), FLAGS_team_number,
@@ -394,6 +445,7 @@
   node_list.push_back("pi2");
   node_list.push_back("pi3");
   node_list.push_back("pi4");
+  std::vector<const calibration::CameraCalibration *> calibration_list;
 
   std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops;
   std::vector<frc971::vision::CharucoExtractor *> charuco_extractors;
@@ -414,6 +466,7 @@
 
     const calibration::CameraCalibration *calibration =
         FindCameraCalibration(constants_fetcher.constants(), node);
+    calibration_list.push_back(calibration);
 
     frc971::vision::TargetType target_type =
         frc971::vision::TargetTypeFromString(FLAGS_target_type);
@@ -430,22 +483,42 @@
     VLOG(1) << "Got extrinsics for " << node << " as\n"
             << default_extrinsics.back().matrix();
 
-    frc971::vision::ImageCallback *image_callback =
-        new frc971::vision::ImageCallback(
-            detection_event_loops[i].get(), "/" + node_list[i] + "/camera",
-            [&reader, &charuco_extractors, &detection_event_loops, &node_list,
-             i](cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
-              aos::distributed_clock::time_point pi_distributed_time =
-                  reader.event_loop_factory()
-                      ->GetNodeEventLoopFactory(
-                          detection_event_loops[i].get()->node())
-                      ->ToDistributedClock(eof);
-              ProcessImage(detection_event_loops[i].get(), rgb_image, eof,
-                           pi_distributed_time, *charuco_extractors[i],
-                           node_list[i]);
-            });
+    if (FLAGS_use_full_logs) {
+      LOG(INFO) << "Set up image callback for node " << node_list[i];
+      frc971::vision::ImageCallback *image_callback =
+          new frc971::vision::ImageCallback(
+              detection_event_loops[i].get(), "/" + node_list[i] + "/camera",
+              [&reader, &charuco_extractors, &detection_event_loops, &node_list,
+               i](cv::Mat rgb_image,
+                  const aos::monotonic_clock::time_point eof) {
+                aos::distributed_clock::time_point pi_distributed_time =
+                    reader.event_loop_factory()
+                        ->GetNodeEventLoopFactory(
+                            detection_event_loops[i].get()->node())
+                        ->ToDistributedClock(eof);
+                HandleImage(detection_event_loops[i].get(), rgb_image, eof,
+                            pi_distributed_time, *charuco_extractors[i],
+                            node_list[i]);
+              });
 
-    image_callbacks.emplace_back(image_callback);
+      image_callbacks.emplace_back(image_callback);
+    } else {
+      detection_event_loops[i]->MakeWatcher(
+          "/camera", [&reader, &detection_event_loops, &node_list,
+                      i](const TargetMap &map) {
+            aos::distributed_clock::time_point pi_distributed_time =
+                reader.event_loop_factory()
+                    ->GetNodeEventLoopFactory(detection_event_loops[i]->node())
+                    ->ToDistributedClock(aos::monotonic_clock::time_point(
+                        aos::monotonic_clock::duration(
+                            map.monotonic_timestamp_ns())));
+
+            HandleTargetMap(map, pi_distributed_time, node_list[i]);
+          });
+      LOG(INFO) << "Created watcher for using the detection event loop for "
+                << node_list[i] << " with i = " << i << " and size "
+                << detection_event_loops.size();
+    }
   }
 
   reader.event_loop_factory()->Run();
@@ -502,7 +575,8 @@
         // Now, get the camera2->boardA map (notice it's boardA, same as
         // camera1, so we can compute the difference based both on boardA)
         Eigen::Affine3d H_camera2_boardA = pose2.H_camera_target;
-        // If pose2 isn't referenced to boardA (base_target_id), correct that
+        // If pose2 isn't referenced to boardA (base_target_id), correct
+        // that
         if (pose2.board_id != base_target_id) {
           // pose2.H_camera_target references boardB, so map back to boardA
           H_camera2_boardA =
@@ -568,6 +642,54 @@
                 << default_extrinsics[i + 1].matrix();
       LOG(INFO) << "--> Updated Extrinsic for " << node_list[i + 1] << " is \n"
                 << next_extrinsic.matrix();
+
+      // Wirte out this extrinsic to a file
+      flatbuffers::FlatBufferBuilder fbb;
+      flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+          fbb.CreateVector(
+              frc971::vision::MatrixToVector(next_extrinsic.matrix()));
+      calibration::TransformationMatrix::Builder matrix_builder(fbb);
+      matrix_builder.add_data(data_offset);
+      flatbuffers::Offset<calibration::TransformationMatrix>
+          extrinsic_calibration_offset = matrix_builder.Finish();
+
+      calibration::CameraCalibration::Builder calibration_builder(fbb);
+      calibration_builder.add_fixed_extrinsics(extrinsic_calibration_offset);
+      const aos::realtime_clock::time_point realtime_now =
+          aos::realtime_clock::now();
+      calibration_builder.add_calibration_timestamp(
+          realtime_now.time_since_epoch().count());
+      fbb.Finish(calibration_builder.Finish());
+      aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+          solved_extrinsics = fbb.Release();
+
+      aos::FlatbufferDetachedBuffer<
+          frc971::vision::calibration::CameraCalibration>
+          cal_copy = aos::RecursiveCopyFlatBuffer(calibration_list[i + 1]);
+      cal_copy.mutable_message()->clear_fixed_extrinsics();
+      cal_copy.mutable_message()->clear_calibration_timestamp();
+      aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+          merged_calibration = aos::MergeFlatBuffers(
+              &cal_copy.message(), &solved_extrinsics.message());
+
+      std::stringstream time_ss;
+      time_ss << realtime_now;
+
+      // Assumes node_list name is of form "pi#" to create camera id
+      const std::string calibration_filename =
+          FLAGS_output_folder +
+          absl::StrFormat("/calibration_pi-%d-%s_cam-%s_%s.json",
+                          FLAGS_team_number, node_list[i + 1].substr(2, 3),
+                          calibration_list[i + 1]->camera_id()->data(),
+                          time_ss.str());
+
+      LOG(INFO) << calibration_filename << " -> "
+                << aos::FlatbufferToJson(merged_calibration,
+                                         {.multi_line = true});
+
+      aos::util::WriteStringToFileOrDie(
+          calibration_filename,
+          aos::FlatbufferToJson(merged_calibration, {.multi_line = true}));
     }
   }
 
diff --git a/y2023/vision/calibrate_multi_cameras_test.py b/y2023/vision/calibrate_multi_cameras_test.py
new file mode 100755
index 0000000..2b54355
--- /dev/null
+++ b/y2023/vision/calibrate_multi_cameras_test.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+# This script runs the multi camera calibration code through its paces using log data
+# It uses the AprilTag output logs, rather than directly from images
+import argparse
+import os
+import subprocess
+import sys
+import time
+from typing import Sequence, Text
+
+
+# Compare two json files that may have a different timestamp
+def compare_files(gt_file: str, calc_file: str):
+    with open(gt_file, "r") as f_gt:
+        with open(calc_file, "r") as f_calc:
+            while True:
+                line_gt = f_gt.readline()
+                line_calc = f_calc.readline()
+                if not line_gt:
+                    if not line_calc:
+                        return True
+                    else:
+                        return False
+
+                # timestamp field will be different, so ignore this line
+                if "timestamp" in line_gt:
+                    if "timestamp" in line_calc:
+                        continue
+                    else:
+                        return False
+
+                # Compare line and return False if different
+                if line_gt != line_calc:
+                    print("Lines don't match!")
+                    print("\tGround truth file:", line_gt, end='')
+                    print("\tCalculated file:", line_calc, end='')
+                    return False
+            return True
+
+    return False
+
+
+# Run through the calibration routine and file checking with max_pose_error arg
+def check_calib_match(args, max_pose_error: float):
+    calibrate_result = subprocess.run(
+        [
+            args.calibrate_binary,
+            args.logfile,
+            "--target_type",
+            "apriltag",
+            "--team_number",
+            "971",
+            "--max_pose_error",
+            str(max_pose_error),
+        ],
+        stdout=subprocess.PIPE,
+        stderr=subprocess.PIPE,
+        encoding="utf-8",
+    )
+
+    calibrate_result.check_returncode()
+
+    # Check for the 3 pi's that get calibrated
+    for pi in [2, 3, 4]:
+        pi_name = "pi-971-" + str(pi)
+        # Look for calculated calibration file in /tmp dir with pi_name
+        calc_calib_dir = "/tmp/"
+        files = os.listdir(calc_calib_dir)
+        calc_file = ""
+        # Read the calculated files in reverse order, so that we pick
+        # up the most newly created file each time
+        for file in files[::-1]:
+            # check if file contains substring pi_name
+            if pi_name in file:
+                calc_file = calc_calib_dir + file
+
+        # Next find the "ground truth" file with this pi_name
+        external_dir = 'external/calibrate_multi_cameras_data/'
+        files = os.listdir(external_dir)
+        gt_file = ""
+        for file in files[::-1]:
+            if pi_name in file:
+                gt_file = external_dir + file
+
+        if calc_file != "" and gt_file != "":
+            if not compare_files(gt_file, calc_file):
+                return False
+
+    return True
+
+
+def main(argv: Sequence[Text]):
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--logfile",
+                        required=True,
+                        default="calib1",
+                        help="Path to logfile.")
+    parser.add_argument(
+        "--calibrate_binary",
+        required=False,
+        default=
+        "/home/jimostrowski/code/FRC/971-Robot-Code/bazel-bin/y2023/vision/calibrate_multi_cameras",
+        help="Path to calibrate_multi_cameras binary",
+    )
+    args = parser.parse_args(argv)
+
+    # Run once with correct max_pose_error
+    # These were the flags used to create the test file
+    # max_pose_error = 5e-5
+    # max_pose_error_ratio = 0.4
+    if not check_calib_match(args, 5e-5):
+        return -1
+
+    # And once with the incorrect value for max_pose_error to see that it fails
+    if check_calib_match(args, 1e-5):
+        return -1
+
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main(sys.argv[1:]))
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 18383af..83705b9 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -160,6 +160,8 @@
   }
 
   reader_->RemapLoggedChannel("/imu/constants", "y2023.Constants");
+  reader_->RemapLoggedChannel("/logger/constants", "y2023.Constants");
+  reader_->RemapLoggedChannel("/roborio/constants", "y2023.Constants");
 
   reader_->Register();
 
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index b4d3acb..3bf6027 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -58,7 +58,7 @@
 #include "y2023/constants.h"
 #include "y2023/control_loops/superstructure/led_indicator.h"
 #include "y2023/control_loops/superstructure/superstructure_output_generated.h"
-#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_position_static.h"
 
 DEFINE_bool(ctre_diag_server, false,
             "If true, enable the diagnostics server for interacting with "
@@ -223,9 +223,9 @@
 
   ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
 
-  flatbuffers::Offset<frc971::control_loops::CANFalcon> WritePosition(
+  flatbuffers::Offset<frc971::control_loops::CANTalonFX> WritePosition(
       flatbuffers::FlatBufferBuilder *fbb) {
-    frc971::control_loops::CANFalcon::Builder builder(*fbb);
+    frc971::control_loops::CANTalonFX::Builder builder(*fbb);
     builder.add_id(device_id_);
     builder.add_device_temp(device_temp());
     builder.add_supply_voltage(supply_voltage());
@@ -338,7 +338,7 @@
       falcon->RefreshNontimesyncedSignals();
     }
 
-    aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANFalcon>,
+    aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANTalonFX>,
                     kCANFalconCount>
         falcons;
 
@@ -350,14 +350,15 @@
     auto falcons_list =
         builder.fbb()
             ->CreateVector<
-                flatbuffers::Offset<frc971::control_loops::CANFalcon>>(falcons);
+                flatbuffers::Offset<frc971::control_loops::CANTalonFX>>(
+                falcons);
 
     frc971::control_loops::drivetrain::CANPosition::Builder
         can_position_builder =
             builder
                 .MakeBuilder<frc971::control_loops::drivetrain::CANPosition>();
 
-    can_position_builder.add_falcons(falcons_list);
+    can_position_builder.add_talonfxs(falcons_list);
     can_position_builder.add_timestamp(right_front_->GetTimestamp());
     can_position_builder.add_status(static_cast<int>(status));
 
@@ -407,7 +408,7 @@
             event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
                 "/autonomous")),
         superstructure_position_sender_(
-            event_loop->MakeSender<superstructure::Position>(
+            event_loop->MakeSender<superstructure::PositionStatic>(
                 "/superstructure")),
         drivetrain_position_sender_(
             event_loop
@@ -440,62 +441,54 @@
   void RunIteration() override {
     superstructure_reading_->Set(true);
     {
-      auto builder = superstructure_position_sender_.MakeBuilder();
-      frc971::PotAndAbsolutePositionT proximal;
-      CopyPosition(proximal_encoder_, &proximal,
+      aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+          superstructure_position_sender_.MakeStaticBuilder();
+
+      superstructure::ArmPositionStatic *arm = builder->add_arm();
+
+      CopyPosition(proximal_encoder_, arm->add_proximal(),
                    Values::kProximalEncoderCountsPerRevolution(),
                    Values::kProximalEncoderRatio(), proximal_pot_translate,
                    true, values_->arm_proximal.potentiometer_offset);
-      frc971::PotAndAbsolutePositionT distal;
       CopyPosition(
-          distal_encoder_, &distal, Values::kDistalEncoderCountsPerRevolution(),
+          distal_encoder_, arm->add_distal(),
+          Values::kDistalEncoderCountsPerRevolution(),
           values_->arm_distal.zeroing.one_revolution_distance / (M_PI * 2.0),
           distal_pot_translate, true, values_->arm_distal.potentiometer_offset);
-      frc971::PotAndAbsolutePositionT roll_joint;
-      CopyPosition(roll_joint_encoder_, &roll_joint,
+      CopyPosition(roll_joint_encoder_, arm->add_roll_joint(),
                    Values::kRollJointEncoderCountsPerRevolution(),
                    Values::kRollJointEncoderRatio(), roll_joint_pot_translate,
                    false, values_->roll_joint.potentiometer_offset);
-      frc971::AbsolutePositionT wrist;
-      CopyPosition(wrist_encoder_, &wrist,
+      CopyPosition(wrist_encoder_, builder->add_wrist(),
                    Values::kWristEncoderCountsPerRevolution(),
                    values_->wrist.subsystem_params.zeroing_constants
                            .one_revolution_distance /
                        (M_PI * 2.0),
                    values_->wrist_flipped);
 
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &proximal);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &distal);
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> roll_joint_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &roll_joint);
-      flatbuffers::Offset<superstructure::ArmPosition> arm_offset =
-          superstructure::CreateArmPosition(*builder.fbb(), proximal_offset,
-                                            distal_offset, roll_joint_offset);
-      flatbuffers::Offset<frc971::AbsolutePosition> wrist_offset =
-          frc971::AbsolutePosition::Pack(*builder.fbb(), &wrist);
-
       flatbuffers::Offset<superstructure::CANFalcon> roller_falcon_offset;
       auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
       if (optional_roller_falcon.has_value()) {
-        roller_falcon_offset = superstructure::CANFalcon::Pack(
-            *builder.fbb(), &optional_roller_falcon.value());
+        superstructure::CANFalconT roller_falcon_buffer =
+            optional_roller_falcon.value();
+
+        superstructure::CANFalconStatic *roller_falcon =
+            builder->add_roller_falcon();
+        roller_falcon->set_id(roller_falcon_buffer.id);
+        roller_falcon->set_supply_current(roller_falcon_buffer.supply_current);
+        roller_falcon->set_torque_current(roller_falcon_buffer.torque_current);
+        roller_falcon->set_supply_voltage(roller_falcon_buffer.supply_voltage);
+        roller_falcon->set_device_temp(roller_falcon_buffer.device_temp);
+        roller_falcon->set_position(roller_falcon_buffer.position);
+        roller_falcon->set_duty_cycle(roller_falcon_buffer.duty_cycle);
       }
 
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
-
-      position_builder.add_arm(arm_offset);
-      position_builder.add_wrist(wrist_offset);
-      position_builder.add_end_effector_cube_beam_break(
+      builder->set_end_effector_cube_beam_break(
           end_effector_cube_beam_break_->Get());
-      position_builder.add_cone_position(cone_position_sensor_.last_width() /
-                                         cone_position_sensor_.last_period());
-      if (!roller_falcon_offset.IsNull()) {
-        position_builder.add_roller_falcon(roller_falcon_offset);
-      }
-      builder.CheckOk(builder.Send(position_builder.Finish()));
+      builder->set_cone_position(cone_position_sensor_.last_width() /
+                                 cone_position_sensor_.last_period());
+
+      builder.CheckOk(builder.Send());
     }
 
     {
@@ -642,7 +635,7 @@
   std::shared_ptr<const Values> values_;
 
   aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
-  aos::Sender<superstructure::Position> superstructure_position_sender_;
+  aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
   aos::Sender<frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
   ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index d6ff479..29f71a2 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -553,22 +553,22 @@
 
     if (this.drivetrainCANPosition) {
       this.falconRightFrontPosition.innerHTML = //TODO: (niko) Improve this so that falcons are not hard-coded
-      this.drivetrainCANPosition.falcons(0).position().toString();
+      this.drivetrainCANPosition.talonfxs(0).position().toString();
 
       this.falconRightBackPosition.innerHTML =
-      this.drivetrainCANPosition.falcons(1).position().toString();
+      this.drivetrainCANPosition.talonfxs(1).position().toString();
 
       this.falconRightUnderPosition.innerHTML =
-      this.drivetrainCANPosition.falcons(2).position().toString();
+      this.drivetrainCANPosition.talonfxs(2).position().toString();
 
       this.falconLeftFrontPosition.innerHTML =
-      this.drivetrainCANPosition.falcons(3).position().toString();
+      this.drivetrainCANPosition.talonfxs(3).position().toString();
 
       this.falconLeftBackPosition.innerHTML =
-      this.drivetrainCANPosition.falcons(4).position().toString();
+      this.drivetrainCANPosition.talonfxs(4).position().toString();
 
       this.falconLeftUnderPosition.innerHTML =
-      this.drivetrainCANPosition.falcons(5).position().toString();
+      this.drivetrainCANPosition.talonfxs(5).position().toString();
   }
 
     if (this.localizerOutput) {
diff --git a/y2023_bot3/BUILD b/y2023_bot3/BUILD
index c9258f0..86625da 100644
--- a/y2023_bot3/BUILD
+++ b/y2023_bot3/BUILD
@@ -158,7 +158,6 @@
         "//frc971/shooter_interpolation:interpolation",
         "//frc971/zeroing:pot_and_absolute_encoder",
         "//y2023_bot3/control_loops/drivetrain:polydrivetrain_plants",
-        "//y2023_bot3/control_loops/superstructure/pivot_joint:pivot_joint_plants",
         "@com_github_google_glog//:glog",
         "@com_google_absl//absl/base",
     ],
diff --git a/y2023_bot3/autonomous/autonomous_actor.cc b/y2023_bot3/autonomous/autonomous_actor.cc
index fc794b1..7c722be 100644
--- a/y2023_bot3/autonomous/autonomous_actor.cc
+++ b/y2023_bot3/autonomous/autonomous_actor.cc
@@ -161,9 +161,6 @@
       << "Expect at least one JoystickState message before running auto...";
   alliance_ = joystick_state_fetcher_->alliance();
 
-  preloaded_ = false;
-  roller_goal_ = control_loops::superstructure::RollerGoal::IDLE;
-  pivot_goal_ = control_loops::superstructure::PivotGoal::NEUTRAL;
   SendSuperstructureGoal();
 }
 
@@ -227,101 +224,25 @@
 
   auto &splines = *charged_up_splines_;
 
-  AOS_LOG(INFO, "Going to preload");
-
-  // Tell the superstructure that a cube was preloaded
-  if (!WaitForPreloaded()) {
-    return;
-  }
-
-  // Place & Spit firt cube high
-  AOS_LOG(INFO, "Moving arm to front high scoring position");
-
-  HighScore();
-  std::this_thread::sleep_for(chrono::milliseconds(600));
-
-  SpitHigh();
-  std::this_thread::sleep_for(chrono::milliseconds(600));
-
-  StopSpitting();
-
-  std::this_thread::sleep_for(chrono::milliseconds(200));
-  AOS_LOG(
-      INFO, "Placed first cube (HIGH) %lf s\n",
-      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
-  if (FLAGS_one_piece) {
-    return;
-  }
-
   // Drive to second cube
   if (!splines[0].WaitForPlan()) {
     return;
   }
   splines[0].Start();
 
-  // Move arm into position to intake cube and intake.
-  AOS_LOG(INFO, "Moving arm to back pickup position");
-
-  Pickup();
-
-  std::this_thread::sleep_for(chrono::milliseconds(500));
-  Intake();
-
-  AOS_LOG(
-      INFO, "Turning on rollers %lf s\n",
-      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
-  if (!splines[0].WaitForSplineDistanceRemaining(0.02)) {
-    return;
-  }
-
-  AOS_LOG(
-      INFO, "Got there %lf s\n",
-      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
-  // Drive back to grid
   if (!splines[1].WaitForPlan()) {
     return;
   }
   splines[1].Start();
-  std::this_thread::sleep_for(chrono::milliseconds(600));
 
-  // Place Low
-  AOS_LOG(INFO, "Moving arm to front mid scoring position");
-
-  MidScore();
-
-  std::this_thread::sleep_for(chrono::milliseconds(600));
   if (!splines[1].WaitForSplineDistanceRemaining(0.1)) return;
 
-  Spit();
-  std::this_thread::sleep_for(chrono::milliseconds(400));
-  StopSpitting();
-
-  AOS_LOG(
-      INFO, "Placed second cube (MID) %lf s\n",
-      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
   // Drive to third cube
   if (!splines[2].WaitForPlan()) {
     return;
   }
   splines[2].Start();
 
-  std::this_thread::sleep_for(chrono::milliseconds(500));
-  // Move arm into position to intake cube and intake.
-  AOS_LOG(INFO, "Moving arm to back pickup position");
-
-  Pickup();
-
-  std::this_thread::sleep_for(chrono::milliseconds(250));
-  Intake();
-
-  AOS_LOG(
-      INFO, "Turning on rollers %lf s\n",
-      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
   if (!splines[2].WaitForSplineDistanceRemaining(0.02)) {
     return;
   }
@@ -337,56 +258,17 @@
   splines[3].Start();
   std::this_thread::sleep_for(chrono::milliseconds(600));
 
-  // Place Low
-  AOS_LOG(INFO, "Moving arm to front low scoring position");
-
-  LowScore();
-
-  std::this_thread::sleep_for(chrono::milliseconds(600));
   if (!splines[3].WaitForSplineDistanceRemaining(0.1)) return;
-
-  Spit();
-  std::this_thread::sleep_for(chrono::milliseconds(600));
-  StopSpitting();
-
-  AOS_LOG(
-      INFO, "Placed low cube (LOW) %lf s\n",
-      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
 }
 
 // Charged Up Place and Mobility Autonomous (middle)
 void AutonomousActor::ChargedUpMiddle() {
-  aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
-
   CHECK(charged_up_middle_splines_);
 
   auto &splines = *charged_up_middle_splines_;
 
   AOS_LOG(INFO, "Going to preload");
 
-  // Tell the superstructure that a cube was preloaded
-  if (!WaitForPreloaded()) {
-    return;
-  }
-
-  // Place & Spit firt cube mid
-  AOS_LOG(INFO, "Moving arm to front mid scoring position");
-
-  MidScore();
-  std::this_thread::sleep_for(chrono::milliseconds(300));
-
-  Spit();
-  std::this_thread::sleep_for(chrono::milliseconds(300));
-
-  StopSpitting();
-
-  std::this_thread::sleep_for(chrono::milliseconds(100));
-  AOS_LOG(
-      INFO, "Placed first cube (Mid) %lf s\n",
-      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
-  // Drive to second cube
-
   if (!splines[0].WaitForPlan()) {
     return;
   }
@@ -399,83 +281,11 @@
   control_loops::superstructure::Goal::Builder superstructure_builder =
       builder.MakeBuilder<control_loops::superstructure::Goal>();
 
-  superstructure_builder.add_pivot_goal(pivot_goal_);
-  superstructure_builder.add_roller_goal(roller_goal_);
-  superstructure_builder.add_preloaded_with_cube(preloaded_);
-
   if (builder.Send(superstructure_builder.Finish()) !=
       aos::RawSender::Error::kOk) {
     AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
   }
 }
 
-[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
-  set_preloaded(true);
-  SendSuperstructureGoal();
-
-  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
-                                      event_loop()->monotonic_now(),
-                                      ActorBase::kLoopOffset);
-
-  bool loaded = false;
-  while (!loaded) {
-    if (ShouldCancel()) {
-      return false;
-    }
-
-    phased_loop.SleepUntilNext();
-    superstructure_status_fetcher_.Fetch();
-    CHECK(superstructure_status_fetcher_.get() != nullptr);
-
-    loaded = (superstructure_status_fetcher_->end_effector_state() ==
-              control_loops::superstructure::EndEffectorState::LOADED);
-  }
-
-  set_preloaded(false);
-  SendSuperstructureGoal();
-
-  return true;
-}
-
-void AutonomousActor::HighScore() {
-  set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_HIGH_FRONT);
-  SendSuperstructureGoal();
-}
-void AutonomousActor::MidScore() {
-  set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_MID_FRONT);
-  SendSuperstructureGoal();
-}
-void AutonomousActor::LowScore() {
-  set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_LOW_FRONT);
-  SendSuperstructureGoal();
-}
-void AutonomousActor::Spit() {
-  set_roller_goal(control_loops::superstructure::RollerGoal::SPIT);
-  SendSuperstructureGoal();
-}
-void AutonomousActor::SpitHigh() {
-  set_roller_goal(control_loops::superstructure::RollerGoal::SPIT_HIGH);
-  SendSuperstructureGoal();
-}
-
-void AutonomousActor::StopSpitting() {
-  set_roller_goal(control_loops::superstructure::RollerGoal::IDLE);
-  SendSuperstructureGoal();
-}
-void AutonomousActor::Intake() {
-  set_roller_goal(control_loops::superstructure::RollerGoal::INTAKE_CUBE);
-  SendSuperstructureGoal();
-}
-
-void AutonomousActor::Pickup() {
-  set_pivot_goal(control_loops::superstructure::PivotGoal::PICKUP_BACK);
-  SendSuperstructureGoal();
-}
-
-void AutonomousActor::Neutral() {
-  set_pivot_goal(control_loops::superstructure::PivotGoal::NEUTRAL);
-  SendSuperstructureGoal();
-}
-
 }  // namespace autonomous
 }  // namespace y2023_bot3
diff --git a/y2023_bot3/autonomous/autonomous_actor.h b/y2023_bot3/autonomous/autonomous_actor.h
index ad33eed..bc0a35c 100644
--- a/y2023_bot3/autonomous/autonomous_actor.h
+++ b/y2023_bot3/autonomous/autonomous_actor.h
@@ -11,8 +11,6 @@
 #include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
 
-// TODO<FILIP>: Add NEUTRAL pivot pose.
-
 namespace y2023_bot3 {
 namespace autonomous {
 
@@ -24,32 +22,8 @@
       const ::frc971::autonomous::AutonomousActionParams *params) override;
 
  private:
-  void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
-
-  void set_pivot_goal(
-      control_loops::superstructure::PivotGoal requested_pivot_goal) {
-    pivot_goal_ = requested_pivot_goal;
-  }
-
-  void set_roller_goal(
-      control_loops::superstructure::RollerGoal requested_roller_goal) {
-    roller_goal_ = requested_roller_goal;
-  }
-
   void SendSuperstructureGoal();
 
-  [[nodiscard]] bool WaitForPreloaded();
-
-  void HighScore();
-  void MidScore();
-  void LowScore();
-  void Spit();
-  void SpitHigh();
-  void StopSpitting();
-  void Pickup();
-  void Intake();
-  void Neutral();
-
   void Reset();
 
   void SendStartingPosition(const Eigen::Vector3d &start);
@@ -76,11 +50,6 @@
 
   std::optional<Eigen::Vector3d> starting_position_;
 
-  bool preloaded_ = false;
-
-  control_loops::superstructure::PivotGoal pivot_goal_;
-  control_loops::superstructure::RollerGoal roller_goal_;
-
   aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
   aos::Fetcher<y2023_bot3::control_loops::superstructure::Status>
       superstructure_status_fetcher_;
diff --git a/y2023_bot3/constants.cc b/y2023_bot3/constants.cc
index 0a99b94..7f77d4d 100644
--- a/y2023_bot3/constants.cc
+++ b/y2023_bot3/constants.cc
@@ -12,7 +12,6 @@
 
 #include "aos/mutex/mutex.h"
 #include "aos/network/team_number.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint/integral_pivot_joint_plant.h"
 
 namespace y2023_bot3 {
 namespace constants {
@@ -21,41 +20,13 @@
   LOG(INFO) << "creating a Constants for team: " << team;
 
   Values r;
-  auto *const pivot_joint = &r.pivot_joint;
-
-  r.pivot_joint_flipped = true;
-
-  pivot_joint->subsystem_params.zeroing_voltage = 3.0;
-  pivot_joint->subsystem_params.operating_voltage = 12.0;
-  pivot_joint->subsystem_params.zeroing_profile_params = {0.5, 3.0};
-  pivot_joint->subsystem_params.default_profile_params = {5.0, 5.0};
-  pivot_joint->subsystem_params.range = Values::kPivotJointRange();
-  pivot_joint->subsystem_params.make_integral_loop =
-      control_loops::superstructure::pivot_joint::MakeIntegralPivotJointLoop;
-  pivot_joint->subsystem_params.zeroing_constants.average_filter_size =
-      Values::kZeroingSampleSize;
-  pivot_joint->subsystem_params.zeroing_constants.one_revolution_distance =
-      M_PI * 2.0 * constants::Values::kPivotJointEncoderRatio();
-  pivot_joint->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
-  pivot_joint->subsystem_params.zeroing_constants.moving_buffer_size = 20;
-  pivot_joint->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
 
   switch (team) {
     // A set of constants for tests.
     case 1:
-
-      pivot_joint->potentiometer_offset = 0.0;
-
-      pivot_joint->subsystem_params.zeroing_constants
-          .measured_absolute_position = 0.0;
-
       break;
 
     case kThirdRobotTeamNumber:
-      pivot_joint->subsystem_params.zeroing_constants
-          .measured_absolute_position = 0.564786179025525;
-
-      pivot_joint->potentiometer_offset = 0.304569457401797 + 2.66972311194163;
       break;
 
     default:
diff --git a/y2023_bot3/constants.h b/y2023_bot3/constants.h
index ba7267c..60fe404 100644
--- a/y2023_bot3/constants.h
+++ b/y2023_bot3/constants.h
@@ -10,7 +10,6 @@
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
 #include "frc971/zeroing/pot_and_absolute_encoder.h"
 #include "y2023_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint/pivot_joint_plant.h"
 namespace y2023_bot3 {
 namespace constants {
 
@@ -40,12 +39,6 @@
   static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
   static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
 
-  static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
-  static constexpr double kRollerStatorCurrentLimit() { return 100.0; }
-
-  static constexpr double kPivotSupplyCurrentLimit() { return 40.0; }
-  static constexpr double kPivotStatorCurrentLimit() { return 200.0; }
-
   // timeout to ensure code doesn't get stuck after releasing the "intake"
   // button
   static constexpr std::chrono::milliseconds kExtraIntakingTime() {
@@ -63,51 +56,6 @@
     return (rotations * (2.0 * M_PI)) *
            control_loops::drivetrain::kHighOutputRatio;
   }
-
-  // Pivot Joint
-  static constexpr double kPivotJointEncoderCountsPerRevolution() {
-    return 4096.0;
-  }
-
-  static constexpr double kPivotJointEncoderRatio() {
-    return (24.0 / 64.0) * (15.0 / 60.0);
-  }
-
-  static constexpr double kPivotJointPotRatio() {
-    return (24.0 / 64.0) * (15.0 / 60.0);
-  }
-
-  static constexpr double kPivotJointPotRadiansPerVolt() {
-    return kPivotJointPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
-           (2 * M_PI /*radians*/);
-  }
-
-  static constexpr double kMaxPivotJointEncoderPulsesPerSecond() {
-    return control_loops::superstructure::pivot_joint::kFreeSpeed /
-           (2.0 * M_PI) *
-           control_loops::superstructure::pivot_joint::kOutputRatio /
-           kPivotJointEncoderRatio() * kPivotJointEncoderCountsPerRevolution();
-  }
-
-  static constexpr ::frc971::constants::Range kPivotJointRange() {
-    return ::frc971::constants::Range{
-        .lower_hard = -1.78879503977269,  // Back Hard
-        .upper_hard = 1.76302285774785,   // Front Hard
-        .lower = -1.77156498873494,       // Back Soft
-        .upper = 1.76555657862879,        // Front Soft
-    };
-  }
-
-  struct PotAndAbsEncoderConstants {
-    ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
-        ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
-        subsystem_params;
-    double potentiometer_offset;
-  };
-
-  PotAndAbsEncoderConstants pivot_joint;
-
-  bool pivot_joint_flipped;
 };
 
 // Creates and returns a Values instance for the constants.
diff --git a/y2023_bot3/constants/BUILD b/y2023_bot3/constants/BUILD
index 6833ec4..967bba0 100644
--- a/y2023_bot3/constants/BUILD
+++ b/y2023_bot3/constants/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//tools/build_rules:template.bzl", "jinja2_template")
 
 cc_library(
@@ -35,17 +35,15 @@
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "constants_fbs",
     srcs = ["constants.fbs"],
-    gen_reflections = True,
     visibility = ["//visibility:public"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "constants_list_fbs",
     srcs = ["constants_list.fbs"],
-    gen_reflections = True,
     visibility = ["//visibility:public"],
     deps = [":constants_fbs"],
 )
diff --git a/y2023_bot3/control_loops/python/BUILD b/y2023_bot3/control_loops/python/BUILD
index 2753020..fe7666f 100644
--- a/y2023_bot3/control_loops/python/BUILD
+++ b/y2023_bot3/control_loops/python/BUILD
@@ -55,19 +55,3 @@
     visibility = ["//visibility:public"],
     deps = ["//y2023_bot3/control_loops:python_init"],
 )
-
-py_binary(
-    name = "pivot_joint",
-    srcs = [
-        "pivot_joint.py",
-    ],
-    legacy_create_init = False,
-    target_compatible_with = ["@platforms//cpu:x86_64"],
-    deps = [
-        ":python_init",
-        "//frc971/control_loops/python:angular_system",
-        "//frc971/control_loops/python:controls",
-        "@pip//glog",
-        "@pip//python_gflags",
-    ],
-)
diff --git a/y2023_bot3/control_loops/python/pivot_joint.py b/y2023_bot3/control_loops/python/pivot_joint.py
deleted file mode 100644
index baea920..0000000
--- a/y2023_bot3/control_loops/python/pivot_joint.py
+++ /dev/null
@@ -1,60 +0,0 @@
-#!/usr/bin/python3
-
-from aos.util.trapezoid_profile import TrapezoidProfile
-from frc971.control_loops.python import control_loop
-from frc971.control_loops.python import angular_system
-from frc971.control_loops.python import controls
-import numpy
-import sys
-from matplotlib import pylab
-import gflags
-import glog
-
-FLAGS = gflags.FLAGS
-
-try:
-    gflags.DEFINE_bool("plot", False, "If true, plot the loop response.")
-except gflags.DuplicateFlagError:
-    pass
-
-kPivotJoint = angular_system.AngularSystemParams(
-    name="PivotJoint",
-    motor=control_loop.Falcon(),
-    G=(14 / 50) * (24 / 64) * (24 / 64) * (15 / 60),
-    # Use parallel axis theorem to get the moment of inertia around
-    # the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
-    J=(0.13372 * 2.00),
-    q_pos=0.80,
-    q_vel=80.0,
-    kalman_q_pos=0.12,
-    kalman_q_vel=2.0,
-    kalman_q_voltage=1.5,
-    kalman_r_position=0.05,
-    radius=5.71 * 0.0254,
-)
-
-
-def main(argv):
-    if FLAGS.plot:
-        R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
-        angular_system.PlotKick(kPivotJoint, R)
-        angular_system.PlotMotion(kPivotJoint, R)
-        return
-
-    # Write the generated constants out to a file.
-    if len(argv) != 5:
-        glog.fatal(
-            "Expected .h file name and .cc file name for the pivot joint and integral pivot joint."
-        )
-    else:
-        namespaces = [
-            "y2023_bot3", "control_loops", "superstructure", "pivot_joint"
-        ]
-        angular_system.WriteAngularSystem(kPivotJoint, argv[1:3], argv[3:5],
-                                          namespaces)
-
-
-if __name__ == "__main__":
-    argv = FLAGS(sys.argv)
-    glog.init()
-    sys.exit(main(argv))
diff --git a/y2023_bot3/control_loops/superstructure/BUILD b/y2023_bot3/control_loops/superstructure/BUILD
index 8630ef9..d74d033 100644
--- a/y2023_bot3/control_loops/superstructure/BUILD
+++ b/y2023_bot3/control_loops/superstructure/BUILD
@@ -1,34 +1,31 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
 
 package(default_visibility = ["//visibility:public"])
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_goal_fbs",
     srcs = [
         "superstructure_goal.fbs",
     ],
-    gen_reflections = 1,
     deps = [
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_output_fbs",
     srcs = [
         "superstructure_output.fbs",
     ],
-    gen_reflections = 1,
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_status_fbs",
     srcs = [
         "superstructure_status.fbs",
     ],
-    gen_reflections = 1,
     deps = [
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
@@ -46,39 +43,20 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "superstructure_position_fbs",
     srcs = [
         "superstructure_position.fbs",
     ],
-    gen_reflections = 1,
     deps = [
         "//frc971:can_configuration_fbs",
-        "//frc971/control_loops:can_falcon_fbs",
+        "//frc971/control_loops:can_talonfx_fbs",
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
     ],
 )
 
 cc_library(
-    name = "end_effector",
-    srcs = [
-        "end_effector.cc",
-    ],
-    hdrs = [
-        "end_effector.h",
-    ],
-    deps = [
-        ":superstructure_goal_fbs",
-        ":superstructure_status_fbs",
-        "//aos/events:event_loop",
-        "//aos/time",
-        "//frc971/control_loops:control_loop",
-        "//y2023_bot3:constants",
-    ],
-)
-
-cc_library(
     name = "superstructure_lib",
     srcs = [
         "superstructure.cc",
@@ -89,8 +67,6 @@
     data = [
     ],
     deps = [
-        ":end_effector",
-        ":pivot_joint",
         ":superstructure_goal_fbs",
         ":superstructure_output_fbs",
         ":superstructure_position_fbs",
@@ -189,21 +165,3 @@
         "//aos/network:team_number",
     ],
 )
-
-cc_library(
-    name = "pivot_joint",
-    srcs = [
-        "pivot_joint.cc",
-    ],
-    hdrs = [
-        "pivot_joint.h",
-    ],
-    deps = [
-        ":superstructure_goal_fbs",
-        ":superstructure_status_fbs",
-        "//aos/events:event_loop",
-        "//aos/time",
-        "//frc971/control_loops:control_loop",
-        "//y2023_bot3:constants",
-    ],
-)
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.cc b/y2023_bot3/control_loops/superstructure/end_effector.cc
deleted file mode 100644
index 665aa9d..0000000
--- a/y2023_bot3/control_loops/superstructure/end_effector.cc
+++ /dev/null
@@ -1,120 +0,0 @@
-#include "y2023_bot3/control_loops/superstructure/end_effector.h"
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-using ::aos::monotonic_clock;
-
-EndEffector::EndEffector()
-    : state_(EndEffectorState::IDLE), timer_(aos::monotonic_clock::min_time) {}
-
-void EndEffector::RunIteration(
-    const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
-    bool beambreak_status, double *roller_voltage, bool preloaded_with_cube) {
-  *roller_voltage = 0.0;
-
-  // If we started off preloaded, skip to the loaded state.
-  // Make sure we weren't already there just in case.
-  if (preloaded_with_cube) {
-    switch (state_) {
-      case EndEffectorState::IDLE:
-      case EndEffectorState::INTAKING:
-        state_ = EndEffectorState::LOADED;
-        break;
-      case EndEffectorState::LOADED:
-        break;
-      case EndEffectorState::SPITTING:
-        break;
-      case EndEffectorState::SPITTING_MID:
-        break;
-      case EndEffectorState::SPITTING_HIGH:
-        break;
-    }
-  }
-
-  if (roller_goal == RollerGoal::SPIT) {
-    state_ = EndEffectorState::SPITTING;
-  }
-
-  if (roller_goal == RollerGoal::SPIT_MID) {
-    state_ = EndEffectorState::SPITTING_MID;
-  }
-
-  if (roller_goal == RollerGoal::SPIT_HIGH) {
-    state_ = EndEffectorState::SPITTING_HIGH;
-  }
-
-  switch (state_) {
-    case EndEffectorState::IDLE:
-      // If idle and intake requested, intake
-      if (roller_goal == RollerGoal::INTAKE_CUBE) {
-        state_ = EndEffectorState::INTAKING;
-        timer_ = timestamp;
-      }
-      break;
-    case EndEffectorState::INTAKING:
-      // If intaking and beam break is not triggered, keep intaking
-      if (roller_goal == RollerGoal::INTAKE_CUBE) {
-        timer_ = timestamp;
-      }
-
-      if (beambreak_status) {
-        // Beam has been broken, switch to loaded.
-        state_ = EndEffectorState::LOADED;
-        break;
-      } else if (timestamp > timer_ + constants::Values::kExtraIntakingTime()) {
-        // Intaking failed, waited 1 second with no beambreak
-        state_ = EndEffectorState::IDLE;
-        break;
-      }
-
-      *roller_voltage = kRollerCubeSuckVoltage();
-
-      break;
-    case EndEffectorState::LOADED:
-      timer_ = timestamp;
-      if (!preloaded_with_cube && !beambreak_status) {
-        state_ = EndEffectorState::INTAKING;
-        break;
-      }
-
-      break;
-
-    case EndEffectorState::SPITTING:
-      *roller_voltage = kRollerCubeSpitVoltage();
-
-      if (roller_goal == RollerGoal::IDLE) {
-        state_ = EndEffectorState::IDLE;
-      }
-
-      break;
-
-    case EndEffectorState::SPITTING_MID:
-      *roller_voltage = kRollerCubeSpitMidVoltage();
-
-      if (roller_goal == RollerGoal::IDLE) {
-        state_ = EndEffectorState::IDLE;
-      }
-
-      break;
-
-    case EndEffectorState::SPITTING_HIGH:
-      *roller_voltage = kRollerCubeSpitHighVoltage();
-
-      if (roller_goal == RollerGoal::IDLE) {
-        state_ = EndEffectorState::IDLE;
-      }
-      break;
-  }
-}
-
-void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
-
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2023_bot3
\ No newline at end of file
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.h b/y2023_bot3/control_loops/superstructure/end_effector.h
deleted file mode 100644
index be0abc0..0000000
--- a/y2023_bot3/control_loops/superstructure/end_effector.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
-#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class EndEffector {
- public:
-  static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
-  static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
-  static constexpr double kRollerCubeSpitMidVoltage() { return 5.0; }
-  static constexpr double kRollerCubeSpitHighVoltage() { return 6.37; }
-
-  EndEffector();
-  void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
-                    RollerGoal roller_goal, bool beambreak_status,
-                    double *intake_roller_voltage, bool preloaded_with_cube);
-  EndEffectorState state() const { return state_; }
-  void Reset();
-
- private:
-  EndEffectorState state_;
-
-  // variable which records the last time at which "intake" button was pressed
-  aos::monotonic_clock::time_point timer_;
-};
-
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2023_bot3
-
-#endif  // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
deleted file mode 100644
index be7e646..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.cc
+++ /dev/null
@@ -1,83 +0,0 @@
-#include "pivot_joint.h"
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-PivotJoint::PivotJoint(std::shared_ptr<const constants::Values> values)
-    : pivot_joint_(values->pivot_joint.subsystem_params) {}
-
-flatbuffers::Offset<
-    frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-PivotJoint::RunIteration(PivotGoal goal, double *output,
-                         const frc971::PotAndAbsolutePosition *position,
-                         flatbuffers::FlatBufferBuilder *status_fbb) {
-  double pivot_goal = 0;
-  switch (goal) {
-    case PivotGoal::NEUTRAL:
-      pivot_goal = 0;
-      break;
-
-    case PivotGoal::PICKUP_FRONT:
-      pivot_goal = 1.74609993820075;
-      break;
-
-    case PivotGoal::PICKUP_BACK:
-      pivot_goal = -1.7763851077235;
-      break;
-
-    case PivotGoal::SCORE_LOW_FRONT:
-      pivot_goal = 1.74609993820075;
-      break;
-
-    case PivotGoal::SCORE_LOW_BACK:
-      pivot_goal = -1.7763851077235;
-      break;
-
-    case PivotGoal::SCORE_MID_FRONT:
-      pivot_goal = 0.846887672907125;
-      break;
-
-    case PivotGoal::SCORE_MID_BACK:
-      pivot_goal = -0.763222056740831;
-      break;
-
-    case PivotGoal::SCORE_HIGH_FRONT:
-      pivot_goal = 0.846887672907125;
-      break;
-
-    case PivotGoal::SCORE_HIGH_BACK:
-      pivot_goal = -0.763222056740831;
-      break;
-  }
-
-  flatbuffers::Offset<
-      frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal>
-      pivot_joint_offset = frc971::control_loops::
-          CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
-              *status_fbb, pivot_goal,
-              frc971::CreateProfileParameters(*status_fbb, 5.0, 20.0));
-
-  status_fbb->Finish(pivot_joint_offset);
-
-  flatbuffers::Offset<
-      frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-      pivot_joint_status_offset = pivot_joint_.Iterate(
-          flatbuffers::GetRoot<frc971::control_loops::
-                                   StaticZeroingSingleDOFProfiledSubsystemGoal>(
-              status_fbb->GetBufferPointer()),
-          position, output, status_fbb);
-
-  return pivot_joint_status_offset;
-}
-
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2023_bot3
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.h b/y2023_bot3/control_loops/superstructure/pivot_joint.h
deleted file mode 100644
index 1eff122..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
-#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class PivotJoint {
-  using PotAndAbsoluteEncoderSubsystem =
-      ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
-          ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
-          ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
-
- public:
-  PivotJoint(std::shared_ptr<const constants::Values> values);
-
-  flatbuffers::Offset<
-      frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-  RunIteration(PivotGoal goal, double *output,
-               const frc971::PotAndAbsolutePosition *position,
-               flatbuffers::FlatBufferBuilder *status_fbb);
-
-  bool zeroed() const { return pivot_joint_.zeroed(); }
-
-  bool estopped() const { return pivot_joint_.estopped(); }
-
-  // variable which records the last time at which "intake" button was pressed
-  aos::monotonic_clock::time_point timer_;
-
- private:
-  PotAndAbsoluteEncoderSubsystem pivot_joint_;
-};
-
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2023_bot3
-
-#endif  // Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD b/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
deleted file mode 100644
index ae413ab..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
+++ /dev/null
@@ -1,34 +0,0 @@
-package(default_visibility = ["//y2023_bot3:__subpackages__"])
-
-genrule(
-    name = "genrule_pivot_joint",
-    outs = [
-        "pivot_joint_plant.h",
-        "pivot_joint_plant.cc",
-        "integral_pivot_joint_plant.h",
-        "integral_pivot_joint_plant.cc",
-    ],
-    cmd = "$(location //y2023_bot3/control_loops/python:pivot_joint) $(OUTS)",
-    target_compatible_with = ["@platforms//os:linux"],
-    tools = [
-        "//y2023_bot3/control_loops/python:pivot_joint",
-    ],
-)
-
-cc_library(
-    name = "pivot_joint_plants",
-    srcs = [
-        "integral_pivot_joint_plant.cc",
-        "pivot_joint_plant.cc",
-    ],
-    hdrs = [
-        "integral_pivot_joint_plant.h",
-        "pivot_joint_plant.h",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//visibility:public"],
-    deps = [
-        "//frc971/control_loops:hybrid_state_feedback_loop",
-        "//frc971/control_loops:state_feedback_loop",
-    ],
-)
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 6061a76..e77245e 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -23,9 +23,7 @@
                                const ::std::string &name)
     : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
                                                                     name),
-      values_(values),
-      end_effector_(),
-      pivot_joint_(values) {
+      values_(values) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -33,41 +31,23 @@
                                   const Position *position,
                                   aos::Sender<Output>::Builder *output,
                                   aos::Sender<Status>::Builder *status) {
-  const monotonic_clock::time_point timestamp =
-      event_loop()->context().monotonic_event_time;
+  (void)unsafe_goal;
+  (void)position;
 
   if (WasReset()) {
     AOS_LOG(ERROR, "WPILib reset, restarting\n");
-    end_effector_.Reset();
   }
 
   OutputT output_struct;
 
-  end_effector_.RunIteration(
-      timestamp,
-      unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
-      position->end_effector_cube_beam_break(), &output_struct.roller_voltage,
-      unsafe_goal != nullptr ? unsafe_goal->preloaded_with_cube() : false);
-
-  flatbuffers::Offset<
-      frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-      pivot_joint_offset = pivot_joint_.RunIteration(
-          unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
-                                 : PivotGoal::NEUTRAL,
-          output != nullptr ? &(output_struct.pivot_joint_voltage) : nullptr,
-          position->pivot_joint_position(), status->fbb());
-
   Status::Builder status_builder = status->MakeBuilder<Status>();
 
-  status_builder.add_zeroed(pivot_joint_.zeroed());
-  status_builder.add_estopped(pivot_joint_.estopped());
-  status_builder.add_pivot_joint(pivot_joint_offset);
-  status_builder.add_end_effector_state(end_effector_.state());
-
   if (output) {
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }
 
+  status_builder.add_zeroed(true);
+
   (void)status->Send(status_builder.Finish());
 }
 
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.h b/y2023_bot3/control_loops/superstructure/superstructure.h
index efc95a8..d0cd0db 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.h
+++ b/y2023_bot3/control_loops/superstructure/superstructure.h
@@ -9,8 +9,6 @@
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "y2023_bot3/constants.h"
 #include "y2023_bot3/constants/constants_generated.h"
-#include "y2023_bot3/control_loops/superstructure/end_effector.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint.h"
 #include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2023_bot3/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2023_bot3/control_loops/superstructure/superstructure_position_generated.h"
@@ -34,8 +32,6 @@
 
   double robot_velocity() const;
 
-  inline const EndEffector &end_effector() const { return end_effector_; }
-
  protected:
   virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
                             aos::Sender<Output>::Builder *output,
@@ -46,12 +42,8 @@
   std::optional<double> LateralOffsetForTimeOfFlight(double reading);
 
   std::shared_ptr<const constants::Values> values_;
-  EndEffector end_effector_;
 
   aos::Alliance alliance_ = aos::Alliance::kInvalid;
-
-  PivotJoint pivot_joint_;
-
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
 
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
index d2b8e0b..326a7b0 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -2,30 +2,7 @@
 
 namespace y2023_bot3.control_loops.superstructure;
 
-enum PivotGoal: ubyte {
-  NEUTRAL = 0,
-  PICKUP_FRONT = 1,
-  PICKUP_BACK = 2,
-  SCORE_LOW_FRONT = 3,
-  SCORE_LOW_BACK = 4,
-  SCORE_MID_FRONT = 5,
-  SCORE_MID_BACK = 6,
-  SCORE_HIGH_FRONT = 7,
-  SCORE_HIGH_BACK = 8,
-}
-
-enum RollerGoal: ubyte {
-    IDLE = 0,
-    INTAKE_CUBE = 1,
-    SPIT = 2,
-    SPIT_MID = 3,
-    SPIT_HIGH = 4,
-}
-
 table Goal {
-    pivot_goal:PivotGoal (id: 0);
-    roller_goal:RollerGoal (id: 1);
-    preloaded_with_cube:bool (id: 2);
 }
 
 
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 10f8f6e..370ec17 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -31,11 +31,6 @@
 using ::frc971::control_loops::PositionSensorSimulator;
 using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
 using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
-using PotAndAbsoluteEncoderSimulator =
-    frc971::control_loops::SubsystemSimulator<
-        frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
-        Superstructure::PotAndAbsoluteEncoderSubsystem::State,
-        constants::Values::PotAndAbsEncoderConstants>;
 
 // Class which simulates the superstructure and sends out queue messages with
 // the position.
@@ -50,15 +45,7 @@
         superstructure_status_fetcher_(
             event_loop_->MakeFetcher<Status>("/superstructure")),
         superstructure_output_fetcher_(
-            event_loop_->MakeFetcher<Output>("/superstructure")),
-        pivot_joint_(new CappedTestPlant(pivot_joint::MakePivotJointPlant()),
-                     PositionSensorSimulator(
-                         values->pivot_joint.subsystem_params.zeroing_constants
-                             .one_revolution_distance),
-                     values->pivot_joint, constants::Values::kPivotJointRange(),
-                     values->pivot_joint.subsystem_params.zeroing_constants
-                         .measured_absolute_position,
-                     dt) {
+            event_loop_->MakeFetcher<Output>("/superstructure")) {
     (void)values;
     phased_loop_handle_ = event_loop_->AddPhasedLoop(
         [this](int) {
@@ -66,10 +53,6 @@
           if (!first_) {
             EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
             EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
-
-            pivot_joint_.Simulate(
-                superstructure_output_fetcher_->pivot_joint_voltage(),
-                superstructure_status_fetcher_->pivot_joint());
           }
           first_ = false;
           SendPositionMessage();
@@ -82,38 +65,20 @@
     ::aos::Sender<Position>::Builder builder =
         superstructure_position_sender_.MakeBuilder();
 
-    frc971::PotAndAbsolutePosition::Builder pivot_joint_builder =
-        builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
-    flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_joint_offset =
-        pivot_joint_.encoder()->GetSensorValues(&pivot_joint_builder);
-
     Position::Builder position_builder = builder.MakeBuilder<Position>();
-    position_builder.add_end_effector_cube_beam_break(
-        end_effector_cube_beam_break_);
-    position_builder.add_pivot_joint_position(pivot_joint_offset);
 
     CHECK_EQ(builder.Send(position_builder.Finish()),
              aos::RawSender::Error::kOk);
   }
 
-  void set_end_effector_cube_beam_break(bool triggered) {
-    end_effector_cube_beam_break_ = triggered;
-  }
-
-  PotAndAbsoluteEncoderSimulator *pivot_joint() { return &pivot_joint_; }
-
  private:
   ::aos::EventLoop *event_loop_;
   ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
 
-  bool end_effector_cube_beam_break_ = false;
-
   ::aos::Sender<Position> superstructure_position_sender_;
   ::aos::Fetcher<Status> superstructure_status_fetcher_;
   ::aos::Fetcher<Output> superstructure_output_fetcher_;
 
-  PotAndAbsoluteEncoderSimulator pivot_joint_;
-
   bool first_ = true;
 };
 
@@ -167,51 +132,6 @@
     ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
     ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
         << ": No status";
-
-    if (superstructure_goal_fetcher_->has_pivot_goal()) {
-      double pivot_goal = 0.0;
-
-      switch (superstructure_goal_fetcher_->pivot_goal()) {
-        case PivotGoal::NEUTRAL:
-          pivot_goal = 0;
-          break;
-
-        case PivotGoal::PICKUP_FRONT:
-          pivot_goal = 1.74609993820075;
-          break;
-
-        case PivotGoal::PICKUP_BACK:
-          pivot_goal = -1.7763851077235;
-          break;
-
-        case PivotGoal::SCORE_LOW_FRONT:
-          pivot_goal = 1.74609993820075;
-          break;
-
-        case PivotGoal::SCORE_LOW_BACK:
-          pivot_goal = -1.7763851077235;
-          break;
-
-        case PivotGoal::SCORE_MID_FRONT:
-          pivot_goal = 0.846887672907125;
-          break;
-
-        case PivotGoal::SCORE_MID_BACK:
-          pivot_goal = -0.763222056740831;
-          break;
-
-        case PivotGoal::SCORE_HIGH_FRONT:
-          pivot_goal = 0.846887672907125;
-          break;
-
-        case PivotGoal::SCORE_HIGH_BACK:
-          pivot_goal = -0.763222056740831;
-          break;
-      }
-
-      EXPECT_NEAR(pivot_goal,
-                  superstructure_status_fetcher_->pivot_joint()->position(), 3);
-    }
   }
 
   void CheckIfZeroed() {
@@ -323,323 +243,6 @@
   CheckIfZeroed();
 }
 
-TEST_F(SuperstructureTest, PivotGoal) {
-  SetEnabled(true);
-  WaitUntilZeroed();
-
-  PivotGoal pivot_goal = PivotGoal::PICKUP_FRONT;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_pivot_goal(pivot_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  RunFor(dt() * 30);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  VerifyNearGoal();
-
-  pivot_goal = PivotGoal::PICKUP_BACK;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_pivot_goal(pivot_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  RunFor(dt() * 30);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  VerifyNearGoal();
-
-  pivot_goal = PivotGoal::SCORE_LOW_FRONT;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_pivot_goal(pivot_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  RunFor(dt() * 30);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  VerifyNearGoal();
-
-  pivot_goal = PivotGoal::SCORE_LOW_BACK;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_pivot_goal(pivot_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  RunFor(dt() * 30);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  VerifyNearGoal();
-
-  pivot_goal = PivotGoal::SCORE_MID_FRONT;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_pivot_goal(pivot_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  RunFor(dt() * 30);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  VerifyNearGoal();
-
-  pivot_goal = PivotGoal::SCORE_MID_BACK;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_pivot_goal(pivot_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  RunFor(dt() * 30);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  VerifyNearGoal();
-
-  pivot_goal = PivotGoal::NEUTRAL;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_pivot_goal(pivot_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  RunFor(dt() * 30);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  VerifyNearGoal();
-}
-
-TEST_F(SuperstructureTest, EndEffectorGoal) {
-  SetEnabled(true);
-  WaitUntilZeroed();
-
-  double spit_voltage = EndEffector::kRollerCubeSpitVoltage();
-  double suck_voltage = EndEffector::kRollerCubeSuckVoltage();
-
-  RollerGoal roller_goal = RollerGoal::INTAKE_CUBE;
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_roller_goal(roller_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-  superstructure_plant_.set_end_effector_cube_beam_break(false);
-
-  // This makes sure that we intake as normal when
-  // requesting intake.
-  RunFor(constants::Values::kExtraIntakingTime());
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::INTAKING);
-
-  superstructure_plant_.set_end_effector_cube_beam_break(true);
-
-  // Checking that after the beambreak is set once intaking that the
-  // state changes to LOADED.
-  RunFor(dt());
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::LOADED);
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_roller_goal(RollerGoal::IDLE);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-  superstructure_plant_.set_end_effector_cube_beam_break(false);
-
-  //  Checking that it's going back to intaking because we lost the
-  //  beambreak sensor.
-  RunFor(dt() * 2);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::INTAKING);
-
-  // Checking that we go back to idle after beambreak is lost and we
-  // set our goal to idle.
-  RunFor(dt() * 2 + constants::Values::kExtraIntakingTime());
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::IDLE);
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_roller_goal(roller_goal);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  // Going through intake -> loaded -> spitting
-  // Making sure that it's intaking normally.
-  RunFor(constants::Values::kExtraIntakingTime());
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::INTAKING);
-
-  superstructure_plant_.set_end_effector_cube_beam_break(true);
-
-  // Checking that it's loaded once beambreak is sensing something.
-  RunFor(dt());
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::LOADED);
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_roller_goal(RollerGoal::SPIT);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-  superstructure_plant_.set_end_effector_cube_beam_break(true);
-  // Checking that it stays spitting until 2 seconds after the
-  // beambreak is lost.
-  RunFor(dt() * 10);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), spit_voltage);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::SPITTING);
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
-    goal_builder.add_roller_goal(RollerGoal::IDLE);
-
-    builder.CheckOk(builder.Send(goal_builder.Finish()));
-  }
-
-  // Checking that it goes to idle after it's given time to stop spitting.
-  RunFor(dt() * 3);
-
-  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
-  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::IDLE);
-}
-
-// Test that we are able to signal that the cube was preloaded
-TEST_F(SuperstructureTest, Preloaded) {
-  SetEnabled(true);
-  WaitUntilZeroed();
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_preloaded_with_cube(true);
-    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
-  }
-
-  RunFor(dt());
-
-  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-  EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
-            EndEffectorState::LOADED);
-}
-
-// Tests that the end effector does nothing when the goal is to remain
-// still.
-TEST_F(SuperstructureTest, DoesNothing) {
-  SetEnabled(true);
-  WaitUntilZeroed();
-
-  {
-    auto builder = superstructure_goal_sender_.MakeBuilder();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
-    goal_builder.add_roller_goal(RollerGoal::IDLE);
-
-    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
-  }
-  RunFor(chrono::seconds(10));
-  VerifyNearGoal();
-
-  EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
-}
 // Tests that loops can reach a goal.
 TEST_F(SuperstructureTest, ReachesGoal) {
   SetEnabled(true);
@@ -649,8 +252,6 @@
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_roller_goal(RollerGoal::IDLE);
-
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
   // Give it a lot of time to get there.
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
index 652d247..98dbcf7 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -1,10 +1,6 @@
 namespace y2023_bot3.control_loops.superstructure;
 
 table Output {
-  // Pivot joint voltage
-  pivot_joint_voltage:double (id: 0);
-  // Roller voltage on the end effector (positive is spitting, negative is sucking)
-  roller_voltage:double (id: 1);
 }
 
 root_type Output;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
index a629e56..dc6282f 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -1,18 +1,9 @@
 include "frc971/control_loops/control_loops.fbs";
 include "frc971/can_configuration.fbs";
-include "frc971/control_loops/can_falcon.fbs";
 
 namespace y2023_bot3.control_loops.superstructure;
 
 table Position {
-  // The position of the pivot joint in radians
-  pivot_joint_position:frc971.PotAndAbsolutePosition (id: 0);
-
-  // If this is true, the cube beam break is triggered.
-  end_effector_cube_beam_break:bool (id: 1);
-
-  // Roller falcon data
-  roller_falcon:frc971.control_loops.CANFalcon (id: 2);
 }
 
 root_type Position;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
index 4492cb9..b36e0da 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -3,35 +3,12 @@
 
 namespace y2023_bot3.control_loops.superstructure;
 
-enum EndEffectorState : ubyte {
-  // Not doing anything
-  IDLE = 0,
-  // Intaking the game object into the end effector
-  INTAKING = 1,
-  // The game object is loaded into the end effector
-  LOADED = 2,
-  // Waiting for the arm to be at shooting goal and then telling the
-  // end effector to spit
-  SPITTING = 3,
-  // Waiting for the arm to be at MID shooting goal and then telling the
-  // end effector to spit mid
-  SPITTING_MID = 4,
-  // Waiting for the arm to be at HIGH shooting goal and then telling the
-  // end effector to spit mid
-  SPITTING_HIGH = 5
-}
-
 table Status {
   // All subsystems know their location.
   zeroed:bool (id: 0);
 
   // If true, we have aborted. This is the or of all subsystem estops.
   estopped:bool (id: 1);
-
-  // The current state of the pivot.
-  pivot_joint:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
-
-  end_effector_state:EndEffectorState (id: 3);
 }
 
 root_type Status;
diff --git a/y2023_bot3/joystick_reader.cc b/y2023_bot3/joystick_reader.cc
index fd32652..4c42bdc 100644
--- a/y2023_bot3/joystick_reader.cc
+++ b/y2023_bot3/joystick_reader.cc
@@ -31,8 +31,6 @@
 using frc971::input::driver_station::JoystickAxis;
 using frc971::input::driver_station::POVLocation;
 using Side = frc971::control_loops::drivetrain::RobotSide;
-using y2023_bot3::control_loops::superstructure::PivotGoal;
-using y2023_bot3::control_loops::superstructure::RollerGoal;
 
 namespace y2023_bot3 {
 namespace input {
@@ -90,33 +88,6 @@
       superstructure::Goal::Builder superstructure_goal_builder =
           builder.MakeBuilder<superstructure::Goal>();
 
-      RollerGoal roller_goal = RollerGoal::IDLE;
-      PivotGoal pivot_goal = PivotGoal::NEUTRAL;
-
-      if (data.IsPressed(kSpit)) {
-        roller_goal = RollerGoal::SPIT;
-      } else if (data.IsPressed(kSpitHigh)) {
-        roller_goal = RollerGoal::SPIT_HIGH;
-      }
-
-      if (data.IsPressed(kScore)) {
-        pivot_goal = PivotGoal::SCORE_LOW_FRONT;
-      } else if (data.IsPressed(kScoreBack)) {
-        pivot_goal = PivotGoal::SCORE_LOW_BACK;
-      } else if (data.IsPressed(kScoreMid)) {
-        pivot_goal = PivotGoal::SCORE_MID_FRONT;
-      } else if (data.IsPressed(kScoreMidBack)) {
-        pivot_goal = PivotGoal::SCORE_MID_BACK;
-      } else if (data.IsPressed(kPickup)) {
-        pivot_goal = PivotGoal::PICKUP_FRONT;
-        roller_goal = RollerGoal::INTAKE_CUBE;
-      } else if (data.IsPressed(kPickupBack)) {
-        pivot_goal = PivotGoal::PICKUP_BACK;
-        roller_goal = RollerGoal::INTAKE_CUBE;
-      }
-
-      superstructure_goal_builder.add_roller_goal(roller_goal);
-      superstructure_goal_builder.add_pivot_goal(pivot_goal);
       if (builder.Send(superstructure_goal_builder.Finish()) !=
           aos::RawSender::Error::kOk) {
         AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
@@ -146,4 +117,4 @@
   event_loop.Run();
 
   return 0;
-}
\ No newline at end of file
+}
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index e91f6e7..231f2c6 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -88,20 +88,15 @@
          control_loops::drivetrain::kWheelRadius;
 }
 
-double pivot_pot_translate(double voltage) {
-  return voltage * Values::kPivotJointPotRadiansPerVolt();
-}
-
 constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
     Values::kMaxDrivetrainEncoderPulsesPerSecond(),
-    Values::kMaxPivotJointEncoderPulsesPerSecond(),
 });
 static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
               "fast encoders are too fast");
 
 }  // namespace
 
-static constexpr int kCANFalconCount = 6;
+static constexpr int kCANTalonFXCount = 6;
 static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
 
 class Falcon {
@@ -180,67 +175,11 @@
     PrintConfigs();
   }
 
-  void WriteRollerConfigs() {
-    ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
-    current_limits.StatorCurrentLimit =
-        constants::Values::kRollerStatorCurrentLimit();
-    current_limits.StatorCurrentLimitEnable = true;
-    current_limits.SupplyCurrentLimit =
-        constants::Values::kRollerSupplyCurrentLimit();
-    current_limits.SupplyCurrentLimitEnable = true;
-
-    ctre::phoenix6::configs::MotorOutputConfigs output_configs;
-    output_configs.NeutralMode =
-        ctre::phoenix6::signals::NeutralModeValue::Brake;
-    output_configs.DutyCycleNeutralDeadband = 0;
-
-    ctre::phoenix6::configs::TalonFXConfiguration configuration;
-    configuration.CurrentLimits = current_limits;
-    configuration.MotorOutput = output_configs;
-
-    ctre::phoenix::StatusCode status =
-        talon_.GetConfigurator().Apply(configuration);
-    if (!status.IsOK()) {
-      AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
-              status.GetName(), status.GetDescription());
-    }
-
-    PrintConfigs();
-  }
-
-  void WritePivotConfigs() {
-    ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
-    current_limits.StatorCurrentLimit =
-        constants::Values::kPivotStatorCurrentLimit();
-    current_limits.StatorCurrentLimitEnable = true;
-    current_limits.SupplyCurrentLimit =
-        constants::Values::kPivotSupplyCurrentLimit();
-    current_limits.SupplyCurrentLimitEnable = true;
-
-    ctre::phoenix6::configs::MotorOutputConfigs output_configs;
-    output_configs.NeutralMode =
-        ctre::phoenix6::signals::NeutralModeValue::Brake;
-    output_configs.DutyCycleNeutralDeadband = 0;
-
-    ctre::phoenix6::configs::TalonFXConfiguration configuration;
-    configuration.CurrentLimits = current_limits;
-    configuration.MotorOutput = output_configs;
-
-    ctre::phoenix::StatusCode status =
-        talon_.GetConfigurator().Apply(configuration);
-    if (!status.IsOK()) {
-      AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
-              status.GetName(), status.GetDescription());
-    }
-
-    PrintConfigs();
-  }
-
   ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
 
-  flatbuffers::Offset<frc971::control_loops::CANFalcon> WritePosition(
+  flatbuffers::Offset<frc971::control_loops::CANTalonFX> WritePosition(
       flatbuffers::FlatBufferBuilder *fbb) {
-    frc971::control_loops::CANFalcon::Builder builder(*fbb);
+    frc971::control_loops::CANTalonFX::Builder builder(*fbb);
     builder.add_id(device_id_);
     builder.add_device_temp(device_temp());
     builder.add_supply_voltage(supply_voltage());
@@ -302,8 +241,7 @@
         can_position_sender_(
             event_loop
                 ->MakeSender<frc971::control_loops::drivetrain::CANPosition>(
-                    "/drivetrain")),
-        roller_falcon_data_(std::nullopt) {
+                    "/drivetrain")) {
     event_loop->SetRuntimeRealtimePriority(40);
     event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({1}));
     timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
@@ -316,22 +254,9 @@
   }
 
   void set_falcons(std::shared_ptr<Falcon> right_front,
-                   std::shared_ptr<Falcon> right_back,
-                   std::shared_ptr<Falcon> left_front,
-                   std::shared_ptr<Falcon> left_back,
-                   std::shared_ptr<Falcon> roller_falcon,
-                   std::shared_ptr<Falcon> pivot_falcon) {
+                   std::shared_ptr<Falcon> right_back) {
     right_front_ = std::move(right_front);
     right_back_ = std::move(right_back);
-    left_front_ = std::move(left_front);
-    left_back_ = std::move(left_back);
-    roller_falcon_ = std::move(roller_falcon);
-    pivot_falcon_ = std::move(pivot_falcon);
-  }
-
-  std::optional<frc971::control_loops::CANFalconT> roller_falcon_data() {
-    std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
-    return roller_falcon_data_;
   }
 
  private:
@@ -346,49 +271,34 @@
 
     auto builder = can_position_sender_.MakeBuilder();
 
-    for (auto falcon : {right_front_, right_back_, left_front_, left_back_,
-                        roller_falcon_, pivot_falcon_}) {
+    for (auto falcon : {right_front_, right_back_}) {
       falcon->RefreshNontimesyncedSignals();
     }
 
-    aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANFalcon>,
-                    kCANFalconCount>
+    aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANTalonFX>,
+                    kCANTalonFXCount>
         falcons;
 
-    for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
+    for (auto falcon : {right_front_, right_back_}) {
       falcons.push_back(falcon->WritePosition(builder.fbb()));
     }
 
     auto falcons_list =
         builder.fbb()
             ->CreateVector<
-                flatbuffers::Offset<frc971::control_loops::CANFalcon>>(falcons);
+                flatbuffers::Offset<frc971::control_loops::CANTalonFX>>(
+                falcons);
 
     frc971::control_loops::drivetrain::CANPosition::Builder
         can_position_builder =
             builder
                 .MakeBuilder<frc971::control_loops::drivetrain::CANPosition>();
 
-    can_position_builder.add_falcons(falcons_list);
+    can_position_builder.add_talonfxs(falcons_list);
     can_position_builder.add_timestamp(right_front_->GetTimestamp());
     can_position_builder.add_status(static_cast<int>(status));
 
     builder.CheckOk(builder.Send(can_position_builder.Finish()));
-
-    {
-      std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
-      frc971::control_loops::CANFalconT roller_falcon_data;
-      roller_falcon_data.id = roller_falcon_->device_id();
-      roller_falcon_data.supply_current = roller_falcon_->supply_current();
-      roller_falcon_data.torque_current = -roller_falcon_->torque_current();
-      roller_falcon_data.supply_voltage = roller_falcon_->supply_voltage();
-      roller_falcon_data.device_temp = roller_falcon_->device_temp();
-      roller_falcon_data.position = -roller_falcon_->position();
-      roller_falcon_data.duty_cycle = roller_falcon_->duty_cycle();
-      roller_falcon_data_ =
-          std::make_optional<frc971::control_loops::CANFalconT>(
-              roller_falcon_data);
-    }
   }
 
   aos::EventLoop *event_loop_;
@@ -397,12 +307,7 @@
   aos::Sender<frc971::control_loops::drivetrain::CANPosition>
       can_position_sender_;
 
-  std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_,
-      roller_falcon_, pivot_falcon_;
-
-  std::optional<frc971::control_loops::CANFalconT> roller_falcon_data_;
-
-  aos::stl_mutex roller_mutex_;
+  std::shared_ptr<Falcon> right_front_, right_back_;
 
   // Pointer to the timer handler used to modify the wakeup.
   ::aos::TimerHandler *timer_handler_;
@@ -452,32 +357,8 @@
     {
       auto builder = superstructure_position_sender_.MakeBuilder();
 
-      flatbuffers::Offset<frc971::control_loops::CANFalcon>
-          roller_falcon_offset;
-      frc971::PotAndAbsolutePositionT pivot;
-      CopyPosition(pivot_encoder_, &pivot,
-                   Values::kPivotJointEncoderCountsPerRevolution(),
-                   Values::kPivotJointEncoderRatio(), pivot_pot_translate, true,
-                   values_->pivot_joint.potentiometer_offset);
-
-      auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
-      if (optional_roller_falcon.has_value()) {
-        roller_falcon_offset = frc971::control_loops::CANFalcon::Pack(
-            *builder.fbb(), &optional_roller_falcon.value());
-      }
-
-      flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_offset =
-          frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &pivot);
-
       superstructure::Position::Builder position_builder =
           builder.MakeBuilder<superstructure::Position>();
-      position_builder.add_end_effector_cube_beam_break(
-          !end_effector_cube_beam_break_->Get());
-      position_builder.add_pivot_joint_position(pivot_offset);
-
-      if (!roller_falcon_offset.IsNull()) {
-        position_builder.add_roller_falcon(roller_falcon_offset);
-      }
       builder.CheckOk(builder.Send(position_builder.Finish()));
     }
 
@@ -556,26 +437,6 @@
     superstructure_reading_ = superstructure_reading;
   }
 
-  void set_end_effector_cube_beam_break(
-      ::std::unique_ptr<frc::DigitalInput> sensor) {
-    end_effector_cube_beam_break_ = ::std::move(sensor);
-  }
-
-  void set_pivot_encoder(::std::unique_ptr<frc::Encoder> encoder) {
-    fast_encoder_filter_.Add(encoder.get());
-    pivot_encoder_.set_encoder(::std::move(encoder));
-  }
-
-  void set_pivot_absolute_pwm(
-      ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
-    pivot_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
-  }
-
-  void set_pivot_potentiometer(
-      ::std::unique_ptr<frc::AnalogInput> potentiometer) {
-    pivot_encoder_.set_potentiometer(::std::move(potentiometer));
-  }
-
  private:
   std::shared_ptr<const Values> values_;
 
@@ -587,95 +448,12 @@
 
   std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
 
-  std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_,
-      end_effector_cube_beam_break_;
+  std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
 
   frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
 
-  frc971::wpilib::AbsoluteEncoderAndPotentiometer pivot_encoder_;
-
   CANSensorReader *can_sensor_reader_;
 };
-
-class SuperstructureCANWriter
-    : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
- public:
-  SuperstructureCANWriter(::aos::EventLoop *event_loop)
-      : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
-            event_loop, "/superstructure") {
-    event_loop->SetRuntimeRealtimePriority(
-        constants::Values::kSuperstructureCANWriterPriority);
-
-    event_loop->OnRun([this]() { WriteConfigs(); });
-  };
-
-  void HandleCANConfiguration(const frc971::CANConfiguration &configuration) {
-    roller_falcon_->PrintConfigs();
-    pivot_falcon_->PrintConfigs();
-    if (configuration.reapply()) {
-      WriteConfigs();
-    }
-  }
-
-  void set_roller_falcon(std::shared_ptr<Falcon> roller_falcon) {
-    roller_falcon_ = std::move(roller_falcon);
-  }
-
-  void set_pivot_falcon(std::shared_ptr<Falcon> pivot_falcon) {
-    pivot_falcon_ = std::move(pivot_falcon);
-  }
-
- private:
-  void WriteConfigs() {
-    roller_falcon_->WriteRollerConfigs();
-    pivot_falcon_->WritePivotConfigs();
-  }
-
-  void Write(const superstructure::Output &output) override {
-    ctre::phoenix6::controls::DutyCycleOut roller_control(
-        SafeSpeed(-output.roller_voltage()));
-    roller_control.UpdateFreqHz = 0_Hz;
-    roller_control.EnableFOC = true;
-
-    ctre::phoenix6::controls::DutyCycleOut pivot_control(
-        SafeSpeed(output.pivot_joint_voltage()));
-    pivot_control.UpdateFreqHz = 0_Hz;
-    pivot_control.EnableFOC = true;
-
-    ctre::phoenix::StatusCode status =
-        roller_falcon_->talon()->SetControl(roller_control);
-
-    if (!status.IsOK()) {
-      AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
-              status.GetName(), status.GetDescription());
-    }
-
-    status = pivot_falcon_->talon()->SetControl(pivot_control);
-
-    if (!status.IsOK()) {
-      AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
-              status.GetName(), status.GetDescription());
-    }
-  }
-
-  void Stop() override {
-    AOS_LOG(WARNING, "Superstructure CAN output too old.\n");
-    ctre::phoenix6::controls::DutyCycleOut stop_command(0.0);
-    stop_command.UpdateFreqHz = 0_Hz;
-    stop_command.EnableFOC = true;
-
-    roller_falcon_->talon()->SetControl(stop_command);
-    pivot_falcon_->talon()->SetControl(stop_command);
-  }
-
-  double SafeSpeed(double voltage) {
-    return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
-  }
-
-  std::shared_ptr<Falcon> roller_falcon_;
-  std::shared_ptr<Falcon> pivot_falcon_;
-};
-
 class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
                              ::frc971::control_loops::drivetrain::Output> {
  public:
@@ -690,25 +468,17 @@
   }
 
   void set_falcons(std::shared_ptr<Falcon> right_front,
-                   std::shared_ptr<Falcon> right_back,
-                   std::shared_ptr<Falcon> left_front,
-                   std::shared_ptr<Falcon> left_back) {
+                   std::shared_ptr<Falcon> right_back) {
     right_front_ = std::move(right_front);
     right_back_ = std::move(right_back);
-    left_front_ = std::move(left_front);
-    left_back_ = std::move(left_back);
   }
 
   void set_right_inverted(ctre::phoenix6::signals::InvertedValue invert) {
     right_inverted_ = invert;
   }
 
-  void set_left_inverted(ctre::phoenix6::signals::InvertedValue invert) {
-    left_inverted_ = invert;
-  }
-
   void HandleCANConfiguration(const frc971::CANConfiguration &configuration) {
-    for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
+    for (auto falcon : {right_front_, right_back_}) {
       falcon->PrintConfigs();
     }
     if (configuration.reapply()) {
@@ -721,34 +491,15 @@
     for (auto falcon : {right_front_.get(), right_back_.get()}) {
       falcon->WriteConfigs(right_inverted_);
     }
-
-    for (auto falcon : {left_front_.get(), left_back_.get()}) {
-      falcon->WriteConfigs(left_inverted_);
-    }
   }
 
   void Write(
       const ::frc971::control_loops::drivetrain::Output &output) override {
-    ctre::phoenix6::controls::DutyCycleOut left_control(
-        SafeSpeed(output.left_voltage()));
-    left_control.UpdateFreqHz = 0_Hz;
-    left_control.EnableFOC = true;
-
     ctre::phoenix6::controls::DutyCycleOut right_control(
         SafeSpeed(output.right_voltage()));
     right_control.UpdateFreqHz = 0_Hz;
     right_control.EnableFOC = true;
 
-    for (auto falcon : {left_front_.get(), left_back_.get()}) {
-      ctre::phoenix::StatusCode status =
-          falcon->talon()->SetControl(left_control);
-
-      if (!status.IsOK()) {
-        AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
-                status.GetName(), status.GetDescription());
-      }
-    }
-
     for (auto falcon : {right_front_.get(), right_back_.get()}) {
       ctre::phoenix::StatusCode status =
           falcon->talon()->SetControl(right_control);
@@ -766,8 +517,7 @@
     stop_command.UpdateFreqHz = 0_Hz;
     stop_command.EnableFOC = true;
 
-    for (auto falcon : {right_front_.get(), right_back_.get(),
-                        left_front_.get(), left_back_.get()}) {
+    for (auto falcon : {right_front_.get(), right_back_.get()}) {
       falcon->talon()->SetControl(stop_command);
     }
   }
@@ -776,8 +526,58 @@
     return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
   }
 
-  ctre::phoenix6::signals::InvertedValue left_inverted_, right_inverted_;
-  std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_;
+  ctre::phoenix6::signals::InvertedValue right_inverted_;
+  std::shared_ptr<Falcon> right_front_, right_back_;
+};
+class PWMDrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+                                ::frc971::control_loops::drivetrain::Output> {
+ public:
+  static constexpr double kMaxBringupPower = 12.0;
+
+  PWMDrivetrainWriter(::aos::EventLoop *event_loop)
+      : ::frc971::wpilib::LoopOutputHandler<
+            ::frc971::control_loops::drivetrain::Output>(event_loop,
+                                                         "/drivetrain") {}
+
+  void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+    left_controller0_ = ::std::move(t);
+    reversed_left0_ = reversed;
+  }
+
+  void set_left_controller1(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+    left_controller1_ = ::std::move(t);
+    reversed_left1_ = reversed;
+  }
+
+ private:
+  void Write(
+      const ::frc971::control_loops::drivetrain::Output &output) override {
+    left_controller0_->SetSpeed(
+        SafeSpeed(reversed_left0_, output.left_voltage()));
+
+    if (left_controller1_) {
+      left_controller1_->SetSpeed(
+          SafeSpeed(reversed_left1_, output.left_voltage()));
+    }
+  }
+  void Stop() override {
+    AOS_LOG(WARNING, "drivetrain output too old\n");
+    left_controller0_->SetDisabled();
+
+    if (left_controller1_) {
+      left_controller1_->SetDisabled();
+    }
+  }
+
+  double SafeSpeed(bool reversed, double voltage) {
+    return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -kMaxBringupPower,
+                        kMaxBringupPower) /
+            12.0);
+  }
+
+  ::std::unique_ptr<::frc::PWM> left_controller0_, left_controller1_;
+
+  bool reversed_right0_, reversed_left0_, reversed_right1_, reversed_left1_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -813,46 +613,36 @@
         std::make_shared<Falcon>(1, "Drivetrain Bus", &signals_registry);
     std::shared_ptr<Falcon> right_back =
         std::make_shared<Falcon>(0, "Drivetrain Bus", &signals_registry);
-    std::shared_ptr<Falcon> left_front =
-        std::make_shared<Falcon>(2, "Drivetrain Bus", &signals_registry);
-    std::shared_ptr<Falcon> left_back =
-        std::make_shared<Falcon>(3, "Drivetrain Bus", &signals_registry);
-    std::shared_ptr<Falcon> roller =
-        std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
-    std::shared_ptr<Falcon> pivot =
-        std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
 
     // Thread 3.
+    ::aos::ShmEventLoop output_event_loop(&config.message());
+    PWMDrivetrainWriter drivetrain_writer(&output_event_loop);
+    drivetrain_writer.set_left_controller0(
+        ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
+
+    AddLoop(&output_event_loop);
+
+    // Thread 4
     ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
     can_sensor_reader_event_loop.set_name("CANSensorReader");
     CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop,
                                       std::move(signals_registry));
-
-    can_sensor_reader.set_falcons(right_front, right_back, left_front,
-                                  left_back, roller, pivot);
-
+    can_sensor_reader.set_falcons(right_front, right_back);
     AddLoop(&can_sensor_reader_event_loop);
 
-    // Thread 4.
+    // Thread 5.
     ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
     SensorReader sensor_reader(&sensor_reader_event_loop, values,
                                &can_sensor_reader);
     sensor_reader.set_pwm_trigger(true);
-    sensor_reader.set_drivetrain_left_encoder(make_encoder(4));
-    sensor_reader.set_drivetrain_right_encoder(make_encoder(5));
+    sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
+    sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
     sensor_reader.set_superstructure_reading(superstructure_reading);
     sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(3));
 
-    sensor_reader.set_end_effector_cube_beam_break(
-        make_unique<frc::DigitalInput>(22));
-
-    sensor_reader.set_pivot_encoder(make_encoder(0));
-    sensor_reader.set_pivot_absolute_pwm(make_unique<frc::DigitalInput>(0));
-    sensor_reader.set_pivot_potentiometer(make_unique<frc::AnalogInput>(0));
-
     AddLoop(&sensor_reader_event_loop);
 
-    // Thread 5.
+    // Thread 6.
     // Set up CAN.
     if (!FLAGS_ctre_diag_server) {
       c_Phoenix_Diagnostics_SetSecondsToStart(-1);
@@ -866,24 +656,16 @@
 
     ::aos::ShmEventLoop can_output_event_loop(&config.message());
     can_output_event_loop.set_name("CANOutputWriter");
-    DrivetrainWriter drivetrain_writer(&can_output_event_loop);
+    DrivetrainWriter can_drivetrain_writer(&can_output_event_loop);
 
-    drivetrain_writer.set_falcons(right_front, right_back, left_front,
-                                  left_back);
-    drivetrain_writer.set_right_inverted(
+    can_drivetrain_writer.set_falcons(right_front, right_back);
+    can_drivetrain_writer.set_right_inverted(
         ctre::phoenix6::signals::InvertedValue::CounterClockwise_Positive);
-    drivetrain_writer.set_left_inverted(
-        ctre::phoenix6::signals::InvertedValue::Clockwise_Positive);
-
-    SuperstructureCANWriter superstructure_can_writer(&can_output_event_loop);
-    superstructure_can_writer.set_roller_falcon(roller);
-    superstructure_can_writer.set_pivot_falcon(pivot);
 
     can_output_event_loop.MakeWatcher(
-        "/roborio", [&drivetrain_writer, &superstructure_can_writer](
+        "/roborio", [&can_drivetrain_writer](
                         const frc971::CANConfiguration &configuration) {
-          drivetrain_writer.HandleCANConfiguration(configuration);
-          superstructure_can_writer.HandleCANConfiguration(configuration);
+          can_drivetrain_writer.HandleCANConfiguration(configuration);
         });
 
     AddLoop(&can_output_event_loop);
diff --git a/y2023_bot4/BUILD b/y2023_bot4/BUILD
index 1a4ed64..095731a 100644
--- a/y2023_bot4/BUILD
+++ b/y2023_bot4/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
 load("//frc971:downloader.bzl", "robot_downloader")
 load("//aos:config.bzl", "aos_config")
 load("//aos/util:config_validator_macro.bzl", "config_validator_test")
@@ -80,18 +80,16 @@
     ],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_position_fbs",
     srcs = ["drivetrain_position.fbs"],
-    gen_reflections = 1,
     deps = ["//frc971/control_loops:control_loops_fbs"],
 )
 
-flatbuffer_cc_library(
+static_flatbuffer(
     name = "drivetrain_can_position_fbs",
     srcs = ["drivetrain_can_position.fbs"],
-    gen_reflections = 1,
-    deps = ["//frc971/control_loops:can_falcon_fbs"],
+    deps = ["//frc971/control_loops:can_talonfx_fbs"],
 )
 
 cc_binary(
@@ -144,8 +142,8 @@
         "//aos/events:shm_event_loop",
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/wpilib:can_sensor_reader",
-        "//frc971/wpilib:falcon",
         "//frc971/wpilib:sensor_reader",
+        "//frc971/wpilib:talonfx",
         "//frc971/wpilib:wpilib_robot_base",
         "//frc971/wpilib/swerve:swerve_drivetrain_writer",
     ],
diff --git a/y2023_bot4/drivetrain_can_position.fbs b/y2023_bot4/drivetrain_can_position.fbs
index e8c1235..ef8edf7 100644
--- a/y2023_bot4/drivetrain_can_position.fbs
+++ b/y2023_bot4/drivetrain_can_position.fbs
@@ -1,9 +1,9 @@
-include "frc971/control_loops/can_falcon.fbs";
+include "frc971/control_loops/can_talonfx.fbs";
 namespace y2023_bot4;
 
 table SwerveModuleCANPosition {
-  rotation: frc971.control_loops.CANFalcon (id: 0);
-  translation: frc971.control_loops.CANFalcon (id: 1);
+  rotation: frc971.control_loops.CANTalonFX (id: 0);
+  translation: frc971.control_loops.CANTalonFX (id: 1);
 }
 
 // CAN Readings from the CAN sensor reader loop for each swerve module
diff --git a/y2023_bot4/wpilib_interface.cc b/y2023_bot4/wpilib_interface.cc
index a744ae0..9dd3604 100644
--- a/y2023_bot4/wpilib_interface.cc
+++ b/y2023_bot4/wpilib_interface.cc
@@ -5,12 +5,12 @@
 #include "aos/init.h"
 #include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/wpilib/can_sensor_reader.h"
-#include "frc971/wpilib/falcon.h"
 #include "frc971/wpilib/sensor_reader.h"
 #include "frc971/wpilib/swerve/swerve_drivetrain_writer.h"
+#include "frc971/wpilib/talonfx.h"
 #include "frc971/wpilib/wpilib_robot_base.h"
 #include "y2023_bot4/constants.h"
-#include "y2023_bot4/drivetrain_can_position_generated.h"
+#include "y2023_bot4/drivetrain_can_position_static.h"
 #include "y2023_bot4/drivetrain_position_generated.h"
 
 DEFINE_bool(ctre_diag_server, false,
@@ -18,7 +18,7 @@
             "devices on the CAN bus using Phoenix Tuner");
 
 using frc971::wpilib::CANSensorReader;
-using frc971::wpilib::Falcon;
+using frc971::wpilib::TalonFX;
 using frc971::wpilib::swerve::DrivetrainWriter;
 using frc971::wpilib::swerve::SwerveModule;
 
@@ -43,21 +43,13 @@
   return builder.Finish();
 }
 
-flatbuffers::Offset<SwerveModuleCANPosition> can_module_offset(
-    SwerveModuleCANPosition::Builder builder,
-    std::shared_ptr<SwerveModule> module) {
-  std::optional<flatbuffers::Offset<control_loops::CANFalcon>> rotation_offset =
-      module->rotation->TakeOffset();
-  std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
-      translation_offset = module->translation->TakeOffset();
+void populate_can_module(SwerveModuleCANPositionStatic *can_position,
+                         std::shared_ptr<SwerveModule> module) {
+  auto rotation = can_position->add_rotation();
+  module->rotation->SerializePosition(rotation, 1.0);
 
-  CHECK(rotation_offset.has_value());
-  CHECK(translation_offset.has_value());
-
-  builder.add_rotation(rotation_offset.value());
-  builder.add_translation(translation_offset.value());
-
-  return builder.Finish();
+  auto translation = can_position->add_translation();
+  module->translation->SerializePosition(translation, 1.0);
 }
 
 constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
@@ -185,27 +177,27 @@
         aos::configuration::ReadConfig("aos_config.json");
 
     std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
-    std::vector<std::shared_ptr<Falcon>> falcons;
+    std::vector<std::shared_ptr<TalonFX>> falcons;
 
     // TODO(max): Change the CanBus names with TalonFX software.
     std::shared_ptr<SwerveModule> front_left = std::make_shared<SwerveModule>(
-        frc971::wpilib::FalconParams{6, false},
-        frc971::wpilib::FalconParams{5, false}, "Drivetrain Bus",
+        frc971::wpilib::TalonFXParams{6, false},
+        frc971::wpilib::TalonFXParams{5, false}, "Drivetrain Bus",
         &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
         constants::Values::kDrivetrainSupplyCurrentLimit());
     std::shared_ptr<SwerveModule> front_right = std::make_shared<SwerveModule>(
-        frc971::wpilib::FalconParams{3, false},
-        frc971::wpilib::FalconParams{4, false}, "Drivetrain Bus",
+        frc971::wpilib::TalonFXParams{3, false},
+        frc971::wpilib::TalonFXParams{4, false}, "Drivetrain Bus",
         &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
         constants::Values::kDrivetrainSupplyCurrentLimit());
     std::shared_ptr<SwerveModule> back_left = std::make_shared<SwerveModule>(
-        frc971::wpilib::FalconParams{8, false},
-        frc971::wpilib::FalconParams{7, false}, "Drivetrain Bus",
+        frc971::wpilib::TalonFXParams{8, false},
+        frc971::wpilib::TalonFXParams{7, false}, "Drivetrain Bus",
         &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
         constants::Values::kDrivetrainSupplyCurrentLimit());
     std::shared_ptr<SwerveModule> back_right = std::make_shared<SwerveModule>(
-        frc971::wpilib::FalconParams{2, false},
-        frc971::wpilib::FalconParams{1, false}, "Drivetrain Bus",
+        frc971::wpilib::TalonFXParams{2, false},
+        frc971::wpilib::TalonFXParams{1, false}, "Drivetrain Bus",
         &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
         constants::Values::kDrivetrainSupplyCurrentLimit());
 
@@ -225,8 +217,8 @@
     falcons.push_back(back_right->rotation);
     falcons.push_back(back_right->translation);
 
-    aos::Sender<AbsoluteCANPosition> can_position_sender =
-        can_sensor_reader_event_loop.MakeSender<AbsoluteCANPosition>(
+    aos::Sender<AbsoluteCANPositionStatic> can_position_sender =
+        can_sensor_reader_event_loop.MakeSender<AbsoluteCANPositionStatic>(
             "/drivetrain");
 
     CANSensorReader can_sensor_reader(
@@ -236,31 +228,24 @@
           // TODO(max): use status properly in the flatbuffer.
           (void)status;
 
-          auto builder = can_position_sender.MakeBuilder();
+          aos::Sender<AbsoluteCANPositionStatic>::StaticBuilder builder =
+              can_position_sender.MakeStaticBuilder();
 
           for (auto falcon : falcons) {
             falcon->RefreshNontimesyncedSignals();
-            falcon->SerializePosition(builder.fbb(), 1.0);
           }
 
-          auto front_left_offset = can_module_offset(
-              builder.MakeBuilder<SwerveModuleCANPosition>(), front_left);
-          auto front_right_offset = can_module_offset(
-              builder.MakeBuilder<SwerveModuleCANPosition>(), front_right);
-          auto back_left_offset = can_module_offset(
-              builder.MakeBuilder<SwerveModuleCANPosition>(), back_left);
-          auto back_right_offset = can_module_offset(
-              builder.MakeBuilder<SwerveModuleCANPosition>(), back_right);
+          auto front_left_flatbuffer = builder->add_front_left();
+          auto front_right_flatbuffer = builder->add_front_right();
+          auto back_left_flatbuffer = builder->add_back_left();
+          auto back_right_flatbuffer = builder->add_back_right();
 
-          AbsoluteCANPosition::Builder can_position_builder =
-              builder.MakeBuilder<AbsoluteCANPosition>();
+          populate_can_module(front_left_flatbuffer, front_left);
+          populate_can_module(front_right_flatbuffer, front_right);
+          populate_can_module(back_left_flatbuffer, back_left);
+          populate_can_module(back_right_flatbuffer, back_right);
 
-          can_position_builder.add_front_left(front_left_offset);
-          can_position_builder.add_front_right(front_right_offset);
-          can_position_builder.add_back_left(back_left_offset);
-          can_position_builder.add_back_right(back_right_offset);
-
-          builder.CheckOk(builder.Send(can_position_builder.Finish()));
+          builder.CheckOk(builder.Send());
         });
 
     AddLoop(&can_sensor_reader_event_loop);
@@ -284,8 +269,8 @@
         &drivetrain_writer_event_loop,
         constants::Values::kDrivetrainWriterPriority, 12);
 
-    drivetrain_writer.set_falcons(front_left, front_right, back_left,
-                                  back_right);
+    drivetrain_writer.set_talonfxs(front_left, front_right, back_left,
+                                   back_right);
 
     AddLoop(&drivetrain_writer_event_loop);
 
diff --git a/y2024/BUILD b/y2024/BUILD
new file mode 100644
index 0000000..010b17c
--- /dev/null
+++ b/y2024/BUILD
@@ -0,0 +1,267 @@
+load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
+load("//aos/util:config_validator_macro.bzl", "config_validator_test")
+
+config_validator_test(
+    name = "config_validator_test",
+    config = "//y2024:aos_config",
+)
+
+robot_downloader(
+    binaries = [
+        "//aos/network:web_proxy_main",
+        "//aos/events/logging:log_cat",
+        "//y2024/constants:constants_sender",
+        "//aos/events:aos_timing_report_streamer",
+    ],
+    data = [
+        ":aos_config",
+        "//aos/starter:roborio_irq_config.json",
+        "//y2024/constants:constants.json",
+        "@ctre_phoenix6_tools_athena//:shared_libraries",
+        "@ctre_phoenix_cci_athena//:shared_libraries",
+    ],
+    dirs = [
+        "//y2024/www:www_files",
+        "//y2024/autonomous:splines",
+    ],
+    start_binaries = [
+        "//aos/events/logging:logger_main",
+        "//aos/network:web_proxy_main",
+        "//aos/starter:irq_affinity",
+        "//y2024/autonomous:binaries",
+        ":joystick_reader",
+        ":wpilib_interface",
+        "//frc971/can_logger",
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//y2024/control_loops/drivetrain:drivetrain",
+        "//y2024/control_loops/drivetrain:trajectory_generator",
+        "//y2024/control_loops/superstructure:superstructure",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+    name = "orin_download",
+    binaries = [
+        "//aos/starter:irq_affinity",
+        "//aos/util:foxglove_websocket",
+        "//aos/events:aos_timing_report_streamer",
+        "//y2024/constants:constants_sender",
+        "//aos/network:web_proxy_main",
+        ":joystick_republish",
+        "//aos/events/logging:log_cat",
+        "//frc971/image_streamer:image_streamer",
+    ],
+    data = [
+        ":aos_config",
+        "//y2024/constants:constants.json",
+        "//y2024/vision:image_streamer_start",
+        "//y2024/www:www_files",
+    ],
+    dirs = [
+        "//y2024/www:www_files",
+        "//frc971/image_streamer/www:www_files",
+    ],
+    start_binaries = [
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//aos/network:web_proxy_main",
+        "//aos/starter:irq_affinity",
+        "//y2024/vision:image_logger",
+        "//aos/events/logging:logger_main",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+    target_type = "orin",
+)
+
+aos_config(
+    name = "aos_config",
+    src = "y2024.json",
+    flatbuffers = [
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//aos/network:timestamp_fbs",
+        "//frc971/input:robot_state_fbs",
+        "//frc971/vision:vision_fbs",
+        "//frc971/vision:target_map_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":config_imu",
+        ":config_roborio",
+    ],
+)
+
+aos_config(
+    name = "config_imu",
+    src = "y2024_imu.json",
+    flatbuffers = [
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//y2024/constants:constants_fbs",
+        "//aos/network:timestamp_fbs",
+        "//aos/network:remote_message_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/events:aos_config",
+        "//frc971/control_loops/drivetrain:aos_config",
+    ],
+)
+
+aos_config(
+    name = "config_roborio",
+    src = "y2024_roborio.json",
+    flatbuffers = [
+        "//frc971:can_configuration_fbs",
+        "//aos/network:remote_message_fbs",
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        #y2019 stuff shouldn't be here (e.g. target selector)
+        "//y2024/constants:constants_fbs",
+        "//aos/network:timestamp_fbs",
+        "//y2024/control_loops/superstructure:superstructure_goal_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+        "//y2024/control_loops/superstructure:superstructure_output_fbs",
+        "//y2024/control_loops/superstructure:superstructure_position_fbs",
+        "//y2024/control_loops/superstructure:superstructure_status_fbs",
+        "//frc971/can_logger:can_logging_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos/events:aos_config",
+        "//frc971/autonomous:aos_config",
+        "//frc971/control_loops/drivetrain:aos_config",
+        "//frc971/input:aos_config",
+    ],
+)
+
+cc_library(
+    name = "constants",
+    srcs = [
+        "constants.cc",
+    ],
+    hdrs = [
+        "constants.h",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/mutex",
+        "//aos/network:team_number",
+        "//frc971:constants",
+        "//frc971/control_loops:pose",
+        "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+        "//frc971/shooter_interpolation:interpolation",
+        "//frc971/zeroing:absolute_encoder",
+        "//frc971/zeroing:pot_and_absolute_encoder",
+        "//y2024/control_loops/drivetrain:polydrivetrain_plants",
+        "@com_github_google_glog//:glog",
+        "@com_google_absl//absl/base",
+    ],
+)
+
+cc_binary(
+    name = "wpilib_interface",
+    srcs = [
+        "wpilib_interface.cc",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:roborio"],
+    deps = [
+        ":constants",
+        "//aos:init",
+        "//aos:math",
+        "//aos/containers:sized_array",
+        "//aos/events:shm_event_loop",
+        "//aos/logging",
+        "//aos/stl_mutex",
+        "//aos/time",
+        "//aos/util:log_interval",
+        "//aos/util:phased_loop",
+        "//aos/util:wrapping_counter",
+        "//frc971:can_configuration_fbs",
+        "//frc971/autonomous:auto_mode_fbs",
+        "//frc971/control_loops:control_loop",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+        "//frc971/input:robot_state_fbs",
+        "//frc971/queues:gyro_fbs",
+        "//frc971/wpilib:ADIS16448",
+        "//frc971/wpilib:buffered_pcm",
+        "//frc971/wpilib:dma",
+        "//frc971/wpilib:drivetrain_writer",
+        "//frc971/wpilib:encoder_and_potentiometer",
+        "//frc971/wpilib:joystick_sender",
+        "//frc971/wpilib:logging_fbs",
+        "//frc971/wpilib:pdp_fetcher",
+        "//frc971/wpilib:sensor_reader",
+        "//frc971/wpilib:wpilib_robot_base",
+        "//third_party:phoenix",
+        "//third_party:phoenix6",
+        "//third_party:wpilib",
+        "//y2024/control_loops/superstructure:superstructure_output_fbs",
+        "//y2024/control_loops/superstructure:superstructure_position_fbs",
+    ],
+)
+
+cc_binary(
+    name = "joystick_reader",
+    srcs = [
+        ":joystick_reader.cc",
+    ],
+    deps = [
+        ":constants",
+        "//aos:init",
+        "//aos/actions:action_lib",
+        "//aos/logging",
+        "//frc971/autonomous:auto_fbs",
+        "//frc971/autonomous:base_autonomous_actor",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//frc971/input:action_joystick_input",
+        "//frc971/input:drivetrain_input",
+        "//frc971/input:joystick_input",
+        "//frc971/input:redundant_joystick_data",
+        "//y2024/control_loops/drivetrain:drivetrain_base",
+        "//y2024/control_loops/superstructure:superstructure_goal_fbs",
+        "//y2024/control_loops/superstructure:superstructure_status_fbs",
+    ],
+)
+
+cc_binary(
+    name = "joystick_republish",
+    srcs = [
+        "joystick_republish.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos:configuration",
+        "//aos:flatbuffer_merge",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/input:joystick_state_fbs",
+        "@com_github_google_glog//:glog",
+    ],
+)
+
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
+
+sh_binary(
+    name = "log_web_proxy",
+    srcs = ["log_web_proxy.sh"],
+    data = [
+        ":aos_config",
+        "//aos/network:log_web_proxy_main",
+        "//y2024/www:files",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/y2024/__init__.py b/y2024/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024/__init__.py
diff --git a/y2024/autonomous/BUILD b/y2024/autonomous/BUILD
new file mode 100644
index 0000000..611f3cd
--- /dev/null
+++ b/y2024/autonomous/BUILD
@@ -0,0 +1,74 @@
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+filegroup(
+    name = "binaries.stripped",
+    srcs = [
+        ":autonomous_action.stripped",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+filegroup(
+    name = "binaries",
+    srcs = [
+        ":autonomous_action",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+filegroup(
+    name = "spline_jsons",
+    srcs = glob([
+        "splines/*.json",
+    ]),
+    visibility = ["//visibility:public"],
+)
+
+aos_downloader_dir(
+    name = "splines",
+    srcs = [
+        ":spline_jsons",
+    ],
+    dir = "splines",
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
+
+cc_library(
+    name = "autonomous_action_lib",
+    srcs = [
+        "auto_splines.cc",
+        "autonomous_actor.cc",
+    ],
+    hdrs = [
+        "auto_splines.h",
+        "autonomous_actor.h",
+    ],
+    deps = [
+        "//aos/events:event_loop",
+        "//aos/logging",
+        "//aos/util:phased_loop",
+        "//frc971/autonomous:base_autonomous_actor",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_config",
+        "//frc971/control_loops/drivetrain:localizer_fbs",
+        "//y2024:constants",
+        "//y2024/control_loops/drivetrain:drivetrain_base",
+        "//y2024/control_loops/superstructure:superstructure_goal_fbs",
+        "//y2024/control_loops/superstructure:superstructure_status_fbs",
+    ],
+)
+
+cc_binary(
+    name = "autonomous_action",
+    srcs = [
+        "autonomous_actor_main.cc",
+    ],
+    deps = [
+        ":autonomous_action_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/autonomous:auto_fbs",
+    ],
+)
diff --git a/y2024/autonomous/auto_splines.cc b/y2024/autonomous/auto_splines.cc
new file mode 100644
index 0000000..d2bd3a1
--- /dev/null
+++ b/y2024/autonomous/auto_splines.cc
@@ -0,0 +1,126 @@
+#include "y2024/autonomous/auto_splines.h"
+
+#include "aos/flatbuffer_merge.h"
+#include "frc971/control_loops/control_loops_generated.h"
+
+namespace y2024 {
+namespace autonomous {
+
+namespace {
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+    aos::Alliance alliance) {
+  frc971::MultiSpline *spline =
+      GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+  flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
+
+  // For 2024: The field is mirrored across the center line, and is not
+  // rotationally symmetric. As such, we only flip the X coordinates when
+  // changing side of the field.
+  if (alliance == aos::Alliance::kBlue) {
+    for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+      spline_x->Mutate(ii, -spline_x->Get(ii));
+    }
+  }
+  return spline_offset;
+}
+}  // namespace
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    aos::Alliance alliance) {
+  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+
+  {
+    frc971::Constraint::Builder longitudinal_constraint_builder =
+        builder->MakeBuilder<frc971::Constraint>();
+    longitudinal_constraint_builder.add_constraint_type(
+        frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
+    longitudinal_constraint_builder.add_value(1.0);
+    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+  }
+
+  {
+    frc971::Constraint::Builder lateral_constraint_builder =
+        builder->MakeBuilder<frc971::Constraint>();
+    lateral_constraint_builder.add_constraint_type(
+        frc971::ConstraintType::LATERAL_ACCELERATION);
+    lateral_constraint_builder.add_value(1.0);
+    lateral_constraint_offset = lateral_constraint_builder.Finish();
+  }
+
+  {
+    frc971::Constraint::Builder voltage_constraint_builder =
+        builder->MakeBuilder<frc971::Constraint>();
+    voltage_constraint_builder.add_constraint_type(
+        frc971::ConstraintType::VOLTAGE);
+    voltage_constraint_builder.add_value(6.0);
+    voltage_constraint_offset = voltage_constraint_builder.Finish();
+  }
+
+  flatbuffers::Offset<
+      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+      constraints_offset =
+          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+              {longitudinal_constraint_offset, lateral_constraint_offset,
+               voltage_constraint_offset});
+
+  const float startx = 0.4;
+  const float starty = 3.4;
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+      builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
+                                           0.6f + startx, 0.4f + startx,
+                                           0.4f + startx, 1.0f + startx});
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+      builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
+                                           starty - 0.3f, starty - 0.7f,
+                                           starty - 1.0f, starty - 1.0f});
+
+  frc971::MultiSpline::Builder multispline_builder =
+      builder->MakeBuilder<frc971::MultiSpline>();
+
+  multispline_builder.add_spline_count(1);
+  multispline_builder.add_constraints(constraints_offset);
+  multispline_builder.add_spline_x(spline_x_offset);
+  multispline_builder.add_spline_y(spline_y_offset);
+
+  return FixSpline(builder, multispline_builder.Finish(), alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    aos::Alliance alliance) {
+  return FixSpline(
+      builder,
+      aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
+      alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    aos::Alliance alliance) {
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+      builder->fbb()->CreateVector<float>(
+          {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+      builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
+
+  frc971::MultiSpline::Builder multispline_builder =
+      builder->MakeBuilder<frc971::MultiSpline>();
+
+  multispline_builder.add_spline_count(1);
+  multispline_builder.add_spline_x(spline_x_offset);
+  multispline_builder.add_spline_y(spline_y_offset);
+
+  return FixSpline(builder, multispline_builder.Finish(), alliance);
+}
+
+}  // namespace autonomous
+}  // namespace y2024
diff --git a/y2024/autonomous/auto_splines.h b/y2024/autonomous/auto_splines.h
new file mode 100644
index 0000000..efdfb33
--- /dev/null
+++ b/y2024/autonomous/auto_splines.h
@@ -0,0 +1,45 @@
+#ifndef Y2024_AUTONOMOUS_AUTO_SPLINES_H_
+#define Y2024_AUTONOMOUS_AUTO_SPLINES_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/input/joystick_state_generated.h"
+/*
+
+  The cooridinate system for the autonomous splines is the same as the spline
+  python generator and drivetrain spline systems.
+
+*/
+
+namespace y2024 {
+namespace autonomous {
+
+class AutonomousSplines {
+ public:
+  AutonomousSplines()
+      : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+            "splines/test_spline.json")) {}
+  static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
+      aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+          *builder,
+      aos::Alliance alliance);
+  static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
+      aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+          *builder,
+      aos::Alliance alliance);
+
+  flatbuffers::Offset<frc971::MultiSpline> TestSpline(
+      aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+          *builder,
+      aos::Alliance alliance);
+
+ private:
+  aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
+};
+
+}  // namespace autonomous
+}  // namespace y2024
+
+#endif  // Y2024_AUTONOMOUS_AUTO_SPLINES_H_
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
new file mode 100644
index 0000000..fd42a56
--- /dev/null
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -0,0 +1,196 @@
+#include "y2024/autonomous/autonomous_actor.h"
+
+#include <chrono>
+#include <cinttypes>
+#include <cmath>
+
+#include "aos/logging/logging.h"
+#include "aos/util/math.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2024/autonomous/auto_splines.h"
+#include "y2024/constants.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
+
+namespace y2024 {
+namespace autonomous {
+
+using ::frc971::ProfileParametersT;
+
+ProfileParametersT MakeProfileParameters(float max_velocity,
+                                         float max_acceleration) {
+  ProfileParametersT result;
+  result.max_velocity = max_velocity;
+  result.max_acceleration = max_acceleration;
+  return result;
+}
+
+using ::aos::monotonic_clock;
+using frc971::CreateProfileParameters;
+using ::frc971::ProfileParametersT;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::drivetrain::LocalizerControl;
+namespace chrono = ::std::chrono;
+
+AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
+    : frc971::autonomous::BaseAutonomousActor(
+          event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+      localizer_control_sender_(
+          event_loop->MakeSender<
+              ::frc971::control_loops::drivetrain::LocalizerControl>(
+              "/drivetrain")),
+      joystick_state_fetcher_(
+          event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+      robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
+      auto_splines_(),
+      superstructure_goal_sender_(
+          event_loop->MakeSender<::y2024::control_loops::superstructure::Goal>(
+              "/superstructure")),
+      superstructure_status_fetcher_(
+          event_loop
+              ->MakeFetcher<::y2024::control_loops::superstructure::Status>(
+                  "/superstructure")) {
+  drivetrain_status_fetcher_.Fetch();
+  replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
+
+  event_loop->OnRun([this, event_loop]() {
+    replan_timer_->Schedule(event_loop->monotonic_now());
+    button_poll_->Schedule(event_loop->monotonic_now(),
+                           chrono::milliseconds(50));
+  });
+
+  // TODO(james): Really need to refactor this code since we keep using it.
+  button_poll_ = event_loop->AddTimer([this]() {
+    const aos::monotonic_clock::time_point now =
+        this->event_loop()->context().monotonic_event_time;
+    if (robot_state_fetcher_.Fetch()) {
+      if (robot_state_fetcher_->user_button()) {
+        user_indicated_safe_to_reset_ = true;
+        MaybeSendStartingPosition();
+      }
+    }
+    if (joystick_state_fetcher_.Fetch()) {
+      if (joystick_state_fetcher_->has_alliance() &&
+          (joystick_state_fetcher_->alliance() != alliance_)) {
+        alliance_ = joystick_state_fetcher_->alliance();
+        is_planned_ = false;
+        // Only kick the planning out by 2 seconds. If we end up enabled in
+        // that second, then we will kick it out further based on the code
+        // below.
+        replan_timer_->Schedule(now + std::chrono::seconds(2));
+      }
+      if (joystick_state_fetcher_->enabled()) {
+        if (!is_planned_) {
+          // Only replan once we've been disabled for 5 seconds.
+          replan_timer_->Schedule(now + std::chrono::seconds(5));
+        }
+      }
+    }
+  });
+}
+
+void AutonomousActor::Replan() {
+  if (!drivetrain_status_fetcher_.Fetch()) {
+    replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
+    AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
+    return;
+  }
+
+  if (alliance_ == aos::Alliance::kInvalid) {
+    return;
+  }
+  sent_starting_position_ = false;
+  if (FLAGS_spline_auto) {
+    test_spline_ =
+        PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+                             std::placeholders::_1, alliance_),
+                   SplineDirection::kForward);
+
+    starting_position_ = test_spline_->starting_position();
+  }
+
+  is_planned_ = true;
+
+  MaybeSendStartingPosition();
+}
+
+void AutonomousActor::MaybeSendStartingPosition() {
+  if (is_planned_ && user_indicated_safe_to_reset_ &&
+      !sent_starting_position_) {
+    CHECK(starting_position_);
+    SendStartingPosition(starting_position_.value());
+  }
+}
+
+void AutonomousActor::Reset() {
+  InitializeEncoders();
+  ResetDrivetrain();
+
+  joystick_state_fetcher_.Fetch();
+  CHECK(joystick_state_fetcher_.get() != nullptr)
+      << "Expect at least one JoystickState message before running auto...";
+  alliance_ = joystick_state_fetcher_->alliance();
+  preloaded_ = false;
+}
+
+bool AutonomousActor::RunAction(
+    const ::frc971::autonomous::AutonomousActionParams *params) {
+  Reset();
+
+  AOS_LOG(INFO, "Params are %d\n", params->mode());
+
+  if (!user_indicated_safe_to_reset_) {
+    AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+    CHECK(starting_position_);
+    SendStartingPosition(starting_position_.value());
+  }
+  // Clear this so that we don't accidentally resend things as soon as we
+  // replan later.
+  user_indicated_safe_to_reset_ = false;
+  is_planned_ = false;
+  starting_position_.reset();
+
+  AOS_LOG(INFO, "Params are %d\n", params->mode());
+  if (alliance_ == aos::Alliance::kInvalid) {
+    AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
+    return false;
+  }
+  if (FLAGS_spline_auto) {
+    SplineAuto();
+  } else {
+    AOS_LOG(WARNING, "No auto mode selected.");
+  }
+  return true;
+}
+
+void AutonomousActor::SplineAuto() {
+  CHECK(test_spline_);
+
+  if (!test_spline_->WaitForPlan()) return;
+  test_spline_->Start();
+
+  if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
+  // Set up the starting position for the blue alliance.
+
+  auto builder = localizer_control_sender_.MakeBuilder();
+
+  LocalizerControl::Builder localizer_control_builder =
+      builder.MakeBuilder<LocalizerControl>();
+  localizer_control_builder.add_x(start(0));
+  localizer_control_builder.add_y(start(1));
+  localizer_control_builder.add_theta(start(2));
+  localizer_control_builder.add_theta_uncertainty(0.00001);
+  AOS_LOG(INFO, "User button pressed, x: %f y: %f theta: %f", start(0),
+          start(1), start(2));
+  if (builder.Send(localizer_control_builder.Finish()) !=
+      aos::RawSender::Error::kOk) {
+    AOS_LOG(ERROR, "Failed to reset localizer.\n");
+  }
+}
+}  // namespace autonomous
+}  // namespace y2024
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
new file mode 100644
index 0000000..6603080
--- /dev/null
+++ b/y2024/autonomous/autonomous_actor.h
@@ -0,0 +1,66 @@
+#ifndef Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
+#define Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
+
+#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2024/autonomous/auto_splines.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024 {
+namespace autonomous {
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+  explicit AutonomousActor(::aos::EventLoop *event_loop);
+
+  bool RunAction(
+      const ::frc971::autonomous::AutonomousActionParams *params) override;
+
+ private:
+  void SendSuperstructureGoal();
+
+  void Reset();
+
+  void SendStartingPosition(const Eigen::Vector3d &start);
+  void MaybeSendStartingPosition();
+  void SplineAuto();
+  void Replan();
+
+  aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
+      localizer_control_sender_;
+  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+  aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+
+  aos::TimerHandler *replan_timer_;
+  aos::TimerHandler *button_poll_;
+
+  aos::Alliance alliance_ = aos::Alliance::kInvalid;
+  AutonomousSplines auto_splines_;
+  bool user_indicated_safe_to_reset_ = false;
+  bool sent_starting_position_ = false;
+
+  bool is_planned_ = false;
+
+  std::optional<Eigen::Vector3d> starting_position_;
+
+  bool preloaded_ = false;
+
+  aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
+  aos::Fetcher<y2024::control_loops::superstructure::Status>
+      superstructure_status_fetcher_;
+
+  std::optional<SplineHandle> test_spline_;
+
+  // List of arm angles from arm::PointsList
+  const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
+};
+
+}  // namespace autonomous
+}  // namespace y2024
+
+#endif  // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
diff --git a/y2024/autonomous/autonomous_actor_main.cc b/y2024/autonomous/autonomous_actor_main.cc
new file mode 100644
index 0000000..4a8e146
--- /dev/null
+++ b/y2024/autonomous/autonomous_actor_main.cc
@@ -0,0 +1,19 @@
+#include <cstdio>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2024/autonomous/autonomous_actor.h"
+
+int main(int argc, char *argv[]) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("aos_config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::y2024::autonomous::AutonomousActor autonomous(&event_loop);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024/constants.cc b/y2024/constants.cc
new file mode 100644
index 0000000..cdaf28c
--- /dev/null
+++ b/y2024/constants.cc
@@ -0,0 +1,50 @@
+#include "y2024/constants.h"
+
+#include <cinttypes>
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "absl/base/call_once.h"
+#include "glog/logging.h"
+
+#include "aos/mutex/mutex.h"
+#include "aos/network/team_number.h"
+
+namespace y2024 {
+namespace constants {
+
+Values MakeValues(uint16_t team) {
+  LOG(INFO) << "creating a Constants for team: " << team;
+
+  Values r;
+
+  switch (team) {
+    // A set of constants for tests.
+    case 1:
+      break;
+
+    case kCompTeamNumber:
+      break;
+
+    case kPracticeTeamNumber:
+      break;
+
+    case kCodingRobotTeamNumber:
+      break;
+
+    default:
+      LOG(FATAL) << "unknown team: " << team;
+
+      // TODO(milind): add pot range checks once we add ranges
+  }
+
+  return r;
+}
+
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
+
+}  // namespace constants
+}  // namespace y2024
diff --git a/y2024/constants.h b/y2024/constants.h
new file mode 100644
index 0000000..b9e5a7d
--- /dev/null
+++ b/y2024/constants.h
@@ -0,0 +1,70 @@
+#ifndef Y2024_CONSTANTS_H_
+#define Y2024_CONSTANTS_H_
+
+#include <array>
+#include <cmath>
+#include <cstdint>
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace y2024 {
+namespace constants {
+
+constexpr uint16_t kCompTeamNumber = 971;
+constexpr uint16_t kPracticeTeamNumber = 9971;
+constexpr uint16_t kCodingRobotTeamNumber = 7971;
+
+struct Values {
+  static const int kZeroingSampleSize = 200;
+
+  static const int kSuperstructureCANWriterPriority = 35;
+  static const int kDrivetrainWriterPriority = 35;
+  static const int kDrivetrainTxPriority = 36;
+  static const int kDrivetrainRxPriority = 36;
+
+  // TODO: These values will need to be changed for the 2024 robot.
+  static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
+  static constexpr double kDrivetrainEncoderCountsPerRevolution() {
+    return kDrivetrainCyclesPerRevolution() * 4;
+  }
+  static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
+  static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
+    return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
+           control_loops::drivetrain::kHighOutputRatio /
+           constants::Values::kDrivetrainEncoderRatio() *
+           kDrivetrainEncoderCountsPerRevolution();
+  }
+
+  static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
+  static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
+
+  static double DrivetrainEncoderToMeters(int32_t in) {
+    return ((static_cast<double>(in) /
+             kDrivetrainEncoderCountsPerRevolution()) *
+            (2.0 * M_PI)) *
+           kDrivetrainEncoderRatio() * control_loops::drivetrain::kWheelRadius;
+  }
+
+  static double DrivetrainCANEncoderToMeters(double rotations) {
+    return (rotations * (2.0 * M_PI)) *
+           control_loops::drivetrain::kHighOutputRatio;
+  }
+};
+
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+constants::Values MakeValues(uint16_t team);
+
+// Calls MakeValues with aos::network::GetTeamNumber()
+constants::Values MakeValues();
+
+}  // namespace constants
+}  // namespace y2024
+
+#endif  // Y2024_CONSTANTS_H_
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
new file mode 100644
index 0000000..0062fa8
--- /dev/null
+++ b/y2024/constants/7971.json
@@ -0,0 +1,5 @@
+{
+  "robot": {
+  },
+  {% include 'y2024/constants/common.json' %}
+}
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
new file mode 100644
index 0000000..0062fa8
--- /dev/null
+++ b/y2024/constants/971.json
@@ -0,0 +1,5 @@
+{
+  "robot": {
+  },
+  {% include 'y2024/constants/common.json' %}
+}
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
new file mode 100644
index 0000000..0062fa8
--- /dev/null
+++ b/y2024/constants/9971.json
@@ -0,0 +1,5 @@
+{
+  "robot": {
+  },
+  {% include 'y2024/constants/common.json' %}
+}
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
new file mode 100644
index 0000000..73e92f0
--- /dev/null
+++ b/y2024/constants/BUILD
@@ -0,0 +1,91 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:template.bzl", "jinja2_template")
+
+cc_library(
+    name = "simulated_constants_sender",
+    srcs = ["simulated_constants_sender.cc"],
+    hdrs = ["simulated_constants_sender.h"],
+    data = [":test_constants.json"],
+    visibility = ["//y2024:__subpackages__"],
+    deps = [
+        ":constants_fbs",
+        ":constants_list_fbs",
+        "//aos/events:simulated_event_loop",
+        "//aos/testing:path",
+        "//frc971/constants:constants_sender_lib",
+    ],
+)
+
+jinja2_template(
+    name = "test_constants.json",
+    src = "test_constants.jinja2.json",
+    includes = glob(["test_data/*.json"]),
+    parameters = {},
+    visibility = ["//visibility:public"],
+)
+
+jinja2_template(
+    name = "constants.json",
+    src = "constants.jinja2.json",
+    includes = [
+        "7971.json",
+        "971.json",
+        "9971.json",
+        "common.json",
+        ":target_map",
+        "//y2024/vision/maps",
+    ],
+    parameters = {},
+    visibility = ["//visibility:public"],
+)
+
+filegroup(
+    name = "target_map",
+    srcs = [
+        "target_map.json",
+    ],
+    visibility = ["//y2024:__subpackages__"],
+)
+
+static_flatbuffer(
+    name = "constants_fbs",
+    srcs = ["constants.fbs"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/vision:target_map_fbs",
+    ],
+)
+
+static_flatbuffer(
+    name = "constants_list_fbs",
+    srcs = ["constants_list.fbs"],
+    visibility = ["//visibility:public"],
+    deps = [":constants_fbs"],
+)
+
+cc_binary(
+    name = "constants_sender",
+    srcs = ["constants_sender.cc"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":constants_fbs",
+        ":constants_list_fbs",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//aos/testing:path",
+        "//frc971/constants:constants_sender_lib",
+    ],
+)
+
+cc_test(
+    name = "constants_validator_test",
+    srcs = ["constants_validator_test.cc"],
+    data = [":constants.json"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":constants_list_fbs",
+        "//aos:json_to_flatbuffer",
+        "//aos/testing:googletest",
+        "@com_github_google_glog//:glog",
+    ],
+)
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
new file mode 100644
index 0000000..1655127
--- /dev/null
+++ b/y2024/constants/common.json
@@ -0,0 +1 @@
+  "target_map": {% include 'y2024/constants/target_map.json' %}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
new file mode 100644
index 0000000..ad34617
--- /dev/null
+++ b/y2024/constants/constants.fbs
@@ -0,0 +1,15 @@
+include "frc971/vision/target_map.fbs";
+
+namespace y2024;
+
+
+table RobotConstants {
+
+}
+
+table Constants {
+  target_map:frc971.vision.TargetMap (id: 0);
+  robot:RobotConstants (id: 1);
+}
+
+root_type Constants;
diff --git a/y2024/constants/constants.jinja2.json b/y2024/constants/constants.jinja2.json
new file mode 100644
index 0000000..c27e588
--- /dev/null
+++ b/y2024/constants/constants.jinja2.json
@@ -0,0 +1,16 @@
+{
+  "constants": [
+    {
+      "team": 7971,
+      "data": {% include 'y2024/constants/7971.json' %}
+    },
+    {
+      "team": 971,
+      "data": {% include 'y2024/constants/971.json' %}
+    },
+    {
+      "team": 9971,
+      "data": {% include 'y2024/constants/9971.json' %}
+    }
+  ]
+}
diff --git a/y2024/constants/constants_list.fbs b/y2024/constants/constants_list.fbs
new file mode 100644
index 0000000..dbebc19
--- /dev/null
+++ b/y2024/constants/constants_list.fbs
@@ -0,0 +1,14 @@
+include "y2024/constants/constants.fbs";
+
+namespace y2024;
+
+table TeamAndConstants {
+  team:long (id: 0);
+  data:Constants (id: 1);
+}
+
+table ConstantsList {
+  constants:[TeamAndConstants] (id: 0);
+}
+
+root_type ConstantsList;
diff --git a/y2024/constants/constants_sender.cc b/y2024/constants/constants_sender.cc
new file mode 100644
index 0000000..bc442a2
--- /dev/null
+++ b/y2024/constants/constants_sender.cc
@@ -0,0 +1,24 @@
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/constants/constants_list_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the AOS config.");
+DEFINE_string(constants_path, "constants.json", "Path to the constant file");
+
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+  aos::ShmEventLoop event_loop(&config.message());
+  frc971::constants::ConstantSender<y2024::Constants, y2024::ConstantsList>
+      constants_sender(&event_loop, FLAGS_constants_path);
+  // Don't need to call Run().
+  return 0;
+}
diff --git a/y2024/constants/constants_validator_test.cc b/y2024/constants/constants_validator_test.cc
new file mode 100644
index 0000000..238bfc3
--- /dev/null
+++ b/y2024/constants/constants_validator_test.cc
@@ -0,0 +1,21 @@
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+#include "aos/json_to_flatbuffer.h"
+#include "y2024/constants/constants_list_generated.h"
+
+namespace y2024 {
+namespace constants {
+namespace testing {
+class ConstantsValidatorTest : public ::testing::Test {};
+
+TEST_F(ConstantsValidatorTest, CheckConstants) {
+  CHECK_NOTNULL(aos::JsonFileToFlatbuffer<y2024::ConstantsList>(
+                    "y2024/constants/constants.json")
+                    .message()
+                    .constants());
+}
+
+}  // namespace testing
+}  // namespace constants
+}  // namespace y2024
diff --git a/y2024/constants/simulated_constants_sender.cc b/y2024/constants/simulated_constants_sender.cc
new file mode 100644
index 0000000..28189f8
--- /dev/null
+++ b/y2024/constants/simulated_constants_sender.cc
@@ -0,0 +1,18 @@
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/constants/constants_list_generated.h"
+
+namespace y2024 {
+bool SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+                             std::string constants_path) {
+  for (const aos::Node *node : factory->nodes()) {
+    std::unique_ptr<aos::EventLoop> event_loop =
+        factory->MakeEventLoop("constants_sender", node);
+    frc971::constants::ConstantSender<Constants, ConstantsList> sender(
+        event_loop.get(), constants_path, team, "/constants");
+  }
+  return true;
+}
+}  // namespace y2024
diff --git a/y2024/constants/simulated_constants_sender.h b/y2024/constants/simulated_constants_sender.h
new file mode 100644
index 0000000..5ecc4da
--- /dev/null
+++ b/y2024/constants/simulated_constants_sender.h
@@ -0,0 +1,16 @@
+#ifndef Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#define Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2024 {
+// Returns true, to allow this to be easily called in the initializer list of a
+// constructor.
+bool SendSimulationConstants(
+    aos::SimulatedEventLoopFactory *factory, int team,
+    std::string constants_path =
+        aos::testing::ArtifactPath("y2024/constants/test_constants.json"));
+}  // namespace y2024
+
+#endif  // Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2024/constants/target_map.json b/y2024/constants/target_map.json
new file mode 100644
index 0000000..544b7b4
--- /dev/null
+++ b/y2024/constants/target_map.json
@@ -0,0 +1,3 @@
+{
+    
+}
\ No newline at end of file
diff --git a/y2024/constants/test_constants.jinja2.json b/y2024/constants/test_constants.jinja2.json
new file mode 100644
index 0000000..c6b77dd
--- /dev/null
+++ b/y2024/constants/test_constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+  "constants": [
+    {
+      "team": 7971,
+      "data": {}
+    }
+  ]
+}
diff --git a/y2024/control_loops/BUILD b/y2024/control_loops/BUILD
new file mode 100644
index 0000000..1747d2d
--- /dev/null
+++ b/y2024/control_loops/BUILD
@@ -0,0 +1,6 @@
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    visibility = ["//visibility:public"],
+    deps = ["//y2024:python_init"],
+)
diff --git a/y2024/control_loops/__init__.py b/y2024/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024/control_loops/__init__.py
diff --git a/y2024/control_loops/drivetrain/BUILD b/y2024/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..e3c627a
--- /dev/null
+++ b/y2024/control_loops/drivetrain/BUILD
@@ -0,0 +1,116 @@
+load("//aos:config.bzl", "aos_config")
+
+genrule(
+    name = "genrule_drivetrain",
+    outs = [
+        "drivetrain_dog_motor_plant.h",
+        "drivetrain_dog_motor_plant.cc",
+        "kalman_drivetrain_motor_plant.h",
+        "kalman_drivetrain_motor_plant.cc",
+    ],
+    cmd = "$(location //y2024/control_loops/python:drivetrain) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2024/control_loops/python:drivetrain",
+    ],
+)
+
+genrule(
+    name = "genrule_polydrivetrain",
+    outs = [
+        "polydrivetrain_dog_motor_plant.h",
+        "polydrivetrain_dog_motor_plant.cc",
+        "polydrivetrain_cim_plant.h",
+        "polydrivetrain_cim_plant.cc",
+        "hybrid_velocity_drivetrain.h",
+        "hybrid_velocity_drivetrain.cc",
+    ],
+    cmd = "$(location //y2024/control_loops/python:polydrivetrain) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2024/control_loops/python:polydrivetrain",
+    ],
+)
+
+cc_library(
+    name = "polydrivetrain_plants",
+    srcs = [
+        "drivetrain_dog_motor_plant.cc",
+        "hybrid_velocity_drivetrain.cc",
+        "kalman_drivetrain_motor_plant.cc",
+        "polydrivetrain_dog_motor_plant.cc",
+    ],
+    hdrs = [
+        "drivetrain_dog_motor_plant.h",
+        "hybrid_velocity_drivetrain.h",
+        "kalman_drivetrain_motor_plant.h",
+        "polydrivetrain_dog_motor_plant.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
+
+cc_library(
+    name = "drivetrain_base",
+    srcs = [
+        "drivetrain_base.cc",
+    ],
+    hdrs = [
+        "drivetrain_base.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":polydrivetrain_plants",
+        "//frc971:shifter_hall_effect",
+        "//frc971/control_loops/drivetrain:drivetrain_config",
+    ],
+)
+
+cc_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain_main.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":drivetrain_base",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/constants:constants_sender_lib",
+        "//frc971/control_loops/drivetrain:drivetrain_lib",
+        "//frc971/control_loops/drivetrain/localization:puppet_localizer",
+        "//y2024/constants:constants_fbs",
+    ],
+)
+
+aos_config(
+    name = "simulation_config",
+    src = "drivetrain_simulation_config.json",
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops/drivetrain:simulation_channels",
+        "//y2024:aos_config",
+    ],
+)
+
+cc_binary(
+    name = "trajectory_generator",
+    srcs = [
+        "trajectory_generator_main.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":drivetrain_base",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/control_loops/drivetrain:trajectory_generator",
+    ],
+)
diff --git a/y2024/control_loops/drivetrain/drivetrain_base.cc b/y2024/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..88b74f9
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,90 @@
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+
+#include <chrono>
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2024/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2024/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::LineFollowConfig;
+
+namespace chrono = ::std::chrono;
+
+namespace y2024 {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+  // Yaw of the IMU relative to the robot frame.
+  static constexpr double kImuYaw = 0.0;
+  static DrivetrainConfig<double> kDrivetrainConfig{
+      ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
+      ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+      ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+
+      drivetrain::MakeDrivetrainLoop,
+      drivetrain::MakeVelocityDrivetrainLoop,
+      drivetrain::MakeKFDrivetrainLoop,
+      drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+      chrono::duration_cast<chrono::nanoseconds>(
+          chrono::duration<double>(drivetrain::kDt)),
+      drivetrain::kRobotRadius,
+      drivetrain::kWheelRadius,
+      drivetrain::kV,
+
+      drivetrain::kHighGearRatio,
+      drivetrain::kLowGearRatio,
+      drivetrain::kJ,
+      drivetrain::kMass,
+      kThreeStateDriveShifter,
+      kThreeStateDriveShifter,
+      true /* default_high_gear */,
+      0 /* down_offset if using constants use
+     constants::GetValues().down_error */
+      ,
+      0.7 /* wheel_non_linearity */,
+      1.2 /* quickturn_wheel_multiplier */,
+      1.2 /* wheel_multiplier */,
+      true /*pistol_grip_shift_enables_line_follow*/,
+      (Eigen::Matrix<double, 3, 3>() << std::cos(kImuYaw), -std::sin(kImuYaw),
+       0.0, std::sin(kImuYaw), std::cos(kImuYaw), 0.0, 0.0, 0.0, 1.0)
+          .finished(),
+      false /*is_simulated*/,
+      DownEstimatorConfig{.gravity_threshold = 0.015,
+                          .do_accel_corrections = 1000},
+      LineFollowConfig{
+          .Q = Eigen::Matrix3d((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+                                    << 1.0 / ::std::pow(0.1, 2),
+                                1.0 / ::std::pow(1.0, 2),
+                                1.0 / ::std::pow(1.0, 2))
+                                   .finished()
+                                   .asDiagonal()),
+          .R = Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+                                    << 10.0 / ::std::pow(12.0, 2),
+                                10.0 / ::std::pow(12.0, 2))
+                                   .finished()
+                                   .asDiagonal()),
+          .max_controllable_offset = 0.5},
+      frc971::control_loops::drivetrain::PistolTopButtonUse::kNone,
+      frc971::control_loops::drivetrain::PistolSecondButtonUse::kTurn1,
+      frc971::control_loops::drivetrain::PistolBottomButtonUse::
+          kControlLoopDriving,
+  };
+
+  return kDrivetrainConfig;
+};
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace y2024
diff --git a/y2024/control_loops/drivetrain/drivetrain_base.h b/y2024/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..807981a
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2024 {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace y2024
+
+#endif  // Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2024/control_loops/drivetrain/drivetrain_main.cc b/y2024/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..196fbb7
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,33 @@
+#include <memory>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("aos_config.json");
+
+  frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+  aos::ShmEventLoop event_loop(&config.message());
+  std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+      localizer = std::make_unique<
+          ::frc971::control_loops::drivetrain::PuppetLocalizer>(
+          &event_loop,
+          ::y2024::control_loops::drivetrain::GetDrivetrainConfig());
+  std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
+      y2024::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+      localizer.get());
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024/control_loops/drivetrain/drivetrain_simulation_config.json b/y2024/control_loops/drivetrain/drivetrain_simulation_config.json
new file mode 100644
index 0000000..f0886b0
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -0,0 +1,6 @@
+{
+  "imports": [
+    "../../y2024.json",
+    "../../../frc971/control_loops/drivetrain/drivetrain_simulation_channels.json"
+  ]
+}
diff --git a/y2024/control_loops/drivetrain/trajectory_generator_main.cc b/y2024/control_loops/drivetrain/trajectory_generator_main.cc
new file mode 100644
index 0000000..260f4a3
--- /dev/null
+++ b/y2024/control_loops/drivetrain/trajectory_generator_main.cc
@@ -0,0 +1,39 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::TrajectoryGenerator;
+
+DEFINE_bool(skip_renicing, false,
+            "If true, skip renicing the trajectory generator.");
+
+int main(int argc, char *argv[]) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("aos_config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  TrajectoryGenerator generator(
+      &event_loop, ::y2024::control_loops::drivetrain::GetDrivetrainConfig());
+
+  event_loop.OnRun([]() {
+    if (FLAGS_skip_renicing) {
+      LOG(WARNING) << "Ignoring request to renice to -20 due to "
+                      "--skip_renicing.";
+    } else {
+      errno = 0;
+      setpriority(PRIO_PROCESS, 0, -20);
+      PCHECK(errno == 0)
+          << ": Renicing to -20 failed, use --skip_renicing to skip renicing.";
+    }
+  });
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024/control_loops/python/BUILD b/y2024/control_loops/python/BUILD
new file mode 100644
index 0000000..4451fb9
--- /dev/null
+++ b/y2024/control_loops/python/BUILD
@@ -0,0 +1,53 @@
+package(default_visibility = ["//y2024:__subpackages__"])
+
+py_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain.py",
+    ],
+    legacy_create_init = False,
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:drivetrain",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
+
+py_binary(
+    name = "polydrivetrain",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    legacy_create_init = False,
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:polydrivetrain",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
+
+py_library(
+    name = "polydrivetrain_lib",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops/python:controls",
+        "//frc971/control_loops/python:drivetrain",
+        "//frc971/control_loops/python:polydrivetrain",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
+
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    visibility = ["//visibility:public"],
+    deps = ["//y2024/control_loops:python_init"],
+)
diff --git a/y2024/control_loops/python/__init__.py b/y2024/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024/control_loops/python/__init__.py
diff --git a/y2024/control_loops/python/drivetrain.py b/y2024/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..64dcc47
--- /dev/null
+++ b/y2024/control_loops/python/drivetrain.py
@@ -0,0 +1,47 @@
+#!/usr/bin/python3
+
+from __future__ import print_function
+from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+kDrivetrain = drivetrain.DrivetrainParams(
+    J=6.5,
+    mass=68.0,
+    # TODO(austin): Measure radius a bit better.
+    robot_radius=0.39,
+    wheel_radius=2.5 * 0.0254,
+    motor_type=control_loop.KrakenFOC(),
+    num_motors=3,
+    G=(14.0 / 52.0) * (26.0 / 58.0),
+    q_pos=0.24,
+    q_vel=2.5,
+    efficiency=0.92,
+    has_imu=False,
+    force=True,
+    kf_q_voltage=1.0,
+    controller_poles=[0.82, 0.82])
+
+
+def main(argv):
+    argv = FLAGS(argv)
+    glog.init()
+
+    if FLAGS.plot:
+        drivetrain.PlotDrivetrainMotions(kDrivetrain)
+    elif len(argv) != 5:
+        print("Expected .h file name and .cc file name")
+    else:
+        # Write the generated constants out to a file.
+        drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2024', kDrivetrain)
+
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))
diff --git a/y2024/control_loops/python/polydrivetrain.py b/y2024/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..99a9b53
--- /dev/null
+++ b/y2024/control_loops/python/polydrivetrain.py
@@ -0,0 +1,33 @@
+#!/usr/bin/python3
+
+import sys
+from y2024.control_loops.python import drivetrain
+from frc971.control_loops.python import polydrivetrain
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+    pass
+
+
+def main(argv):
+    if FLAGS.plot:
+        polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+    elif len(argv) != 7:
+        glog.fatal('Expected .h file name and .cc file name')
+    else:
+        polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+                                           'y2024', drivetrain.kDrivetrain)
+
+
+if __name__ == '__main__':
+    argv = FLAGS(sys.argv)
+    glog.init()
+    sys.exit(main(argv))
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..996d28b
--- /dev/null
+++ b/y2024/control_loops/superstructure/BUILD
@@ -0,0 +1,151 @@
+load("//tools/build_rules:js.bzl", "ts_project")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+package(default_visibility = ["//visibility:public"])
+
+static_flatbuffer(
+    name = "superstructure_goal_fbs",
+    srcs = [
+        "superstructure_goal.fbs",
+    ],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
+)
+
+static_flatbuffer(
+    name = "superstructure_output_fbs",
+    srcs = [
+        "superstructure_output.fbs",
+    ],
+)
+
+static_flatbuffer(
+    name = "superstructure_status_fbs",
+    srcs = [
+        "superstructure_status.fbs",
+    ],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+    ],
+)
+
+flatbuffer_ts_library(
+    name = "superstructure_status_ts_fbs",
+    srcs = [
+        "superstructure_status.fbs",
+    ],
+    deps = [
+        "//frc971/control_loops:control_loops_ts_fbs",
+        "//frc971/control_loops:profiled_subsystem_ts_fbs",
+    ],
+)
+
+static_flatbuffer(
+    name = "superstructure_position_fbs",
+    srcs = [
+        "superstructure_position.fbs",
+    ],
+    deps = [
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+    ],
+)
+
+cc_library(
+    name = "superstructure_lib",
+    srcs = [
+        "superstructure.cc",
+    ],
+    hdrs = [
+        "superstructure.h",
+    ],
+    data = [
+    ],
+    deps = [
+        ":superstructure_goal_fbs",
+        ":superstructure_output_fbs",
+        ":superstructure_position_fbs",
+        ":superstructure_status_fbs",
+        "//aos:flatbuffer_merge",
+        "//aos/events:event_loop",
+        "//frc971/constants:constants_sender_lib",
+        "//frc971/control_loops:control_loop",
+        "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+        "//frc971/shooter_interpolation:interpolation",
+        "//frc971/zeroing:absolute_encoder",
+        "//frc971/zeroing:pot_and_absolute_encoder",
+        "//y2024:constants",
+        "//y2024/constants:constants_fbs",
+        "//y2024/constants:simulated_constants_sender",
+    ],
+)
+
+cc_binary(
+    name = "superstructure",
+    srcs = [
+        "superstructure_main.cc",
+    ],
+    deps = [
+        ":superstructure_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+    ],
+)
+
+cc_test(
+    name = "superstructure_lib_test",
+    srcs = [
+        "superstructure_lib_test.cc",
+    ],
+    data = [
+        "//y2024:aos_config",
+    ],
+    deps = [
+        ":superstructure_goal_fbs",
+        ":superstructure_lib",
+        ":superstructure_output_fbs",
+        ":superstructure_position_fbs",
+        ":superstructure_status_fbs",
+        "//aos:json_to_flatbuffer",
+        "//aos:math",
+        "//aos/events/logging:log_writer",
+        "//aos/testing:googletest",
+        "//aos/time",
+        "//frc971/control_loops:capped_test_plant",
+        "//frc971/control_loops:control_loop_test",
+        "//frc971/control_loops:position_sensor_sim",
+        "//frc971/control_loops:subsystem_simulator",
+        "//frc971/control_loops:team_number_test_environment",
+        "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+    ],
+)
+
+cc_binary(
+    name = "superstructure_replay",
+    srcs = ["superstructure_replay.cc"],
+    deps = [
+        ":superstructure_lib",
+        "//aos:configuration",
+        "//aos:init",
+        "//aos/events:simulated_event_loop",
+        "//aos/events/logging:log_reader",
+        "//aos/network:team_number",
+    ],
+)
+
+ts_project(
+    name = "superstructure_plotter",
+    srcs = ["superstructure_plotter.ts"],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos/network/www:aos_plotter",
+        "//aos/network/www:colors",
+        "//aos/network/www:proxy",
+    ],
+)
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..abb5536
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,78 @@
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/wrap.h"
+
+DEFINE_bool(ignore_distance, false,
+            "If true, ignore distance when shooting and obay joystick_reader");
+
+namespace y2024 {
+namespace control_loops {
+namespace superstructure {
+
+using ::aos::monotonic_clock;
+
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::RelativeEncoderProfiledJointStatus;
+
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+                               std::shared_ptr<const constants::Values> values,
+                               const ::std::string &name)
+    : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+                                                                    name),
+      values_(values),
+      constants_fetcher_(event_loop),
+      drivetrain_status_fetcher_(
+          event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+              "/drivetrain")),
+      joystick_state_fetcher_(
+          event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+  event_loop->SetRuntimeRealtimePriority(30);
+}
+
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+                                  const Position *position,
+                                  aos::Sender<Output>::Builder *output,
+                                  aos::Sender<Status>::Builder *status) {
+  const monotonic_clock::time_point timestamp =
+      event_loop()->context().monotonic_event_time;
+
+  (void)timestamp;
+  (void)unsafe_goal;
+  (void)position;
+
+  if (WasReset()) {
+    AOS_LOG(ERROR, "WPILib reset, restarting\n");
+  }
+
+  OutputT output_struct;
+  if (joystick_state_fetcher_.Fetch() &&
+      joystick_state_fetcher_->has_alliance()) {
+    alliance_ = joystick_state_fetcher_->alliance();
+  }
+  drivetrain_status_fetcher_.Fetch();
+
+  if (output) {
+    output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
+  }
+
+  Status::Builder status_builder = status->MakeBuilder<Status>();
+  status_builder.add_zeroed(true);
+  status_builder.add_estopped(false);
+
+  (void)status->Send(status_builder.Finish());
+}
+
+double Superstructure::robot_velocity() const {
+  return (drivetrain_status_fetcher_.get() != nullptr
+              ? drivetrain_status_fetcher_->robot_speed()
+              : 0.0);
+}
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2024
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..1d7cdf5
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -0,0 +1,54 @@
+#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024/constants.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+    : public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
+ public:
+  explicit Superstructure(::aos::EventLoop *event_loop,
+                          std::shared_ptr<const constants::Values> values,
+                          const ::std::string &name = "/superstructure");
+
+  double robot_velocity() const;
+
+ protected:
+  virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+                            aos::Sender<Output>::Builder *output,
+                            aos::Sender<Status>::Builder *status) override;
+
+ private:
+  std::shared_ptr<const constants::Values> values_;
+  frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+
+  aos::Fetcher<frc971::control_loops::drivetrain::Status>
+      drivetrain_status_fetcher_;
+  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+
+  aos::Alliance alliance_ = aos::Alliance::kInvalid;
+
+  DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2024
+
+#endif  // Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..1c4b3dd
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2024.control_loops.superstructure;
+
+table Goal {
+}
+
+
+root_type Goal;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..85e16b2
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,287 @@
+#include <chrono>
+#include <memory>
+
+#include "gtest/gtest.h"
+
+#include "aos/events/logging/log_writer.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/subsystem_simulator.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "y2024/constants/simulated_constants_sender.h"
+#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+DEFINE_string(output_folder, "",
+              "If set, logs all channels to the provided logfile.");
+
+namespace y2024 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+namespace chrono = std::chrono;
+
+using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
+using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+    CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+
+class SuperstructureSimulation {
+ public:
+  SuperstructureSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+      : event_loop_(event_loop),
+        dt_(dt),
+        superstructure_position_sender_(
+            event_loop_->MakeSender<Position>("/superstructure")),
+        superstructure_status_fetcher_(
+            event_loop_->MakeFetcher<Status>("/superstructure")),
+        superstructure_output_fetcher_(
+            event_loop_->MakeFetcher<Output>("/superstructure")) {
+    (void)dt_;
+    phased_loop_handle_ = event_loop_->AddPhasedLoop(
+        [this](int) {
+          // Skip this the first time.
+          if (!first_) {
+            EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+            EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+          }
+          first_ = false;
+          SendPositionMessage();
+        },
+        dt);
+  }
+
+  // Sends a queue message with the position of the superstructure.
+  void SendPositionMessage() {
+    ::aos::Sender<Position>::Builder builder =
+        superstructure_position_sender_.MakeBuilder();
+
+    Position::Builder position_builder = builder.MakeBuilder<Position>();
+    CHECK_EQ(builder.Send(position_builder.Finish()),
+             aos::RawSender::Error::kOk);
+  }
+
+ private:
+  ::aos::EventLoop *event_loop_;
+  const chrono::nanoseconds dt_;
+  ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+  ::aos::Sender<Position> superstructure_position_sender_;
+  ::aos::Fetcher<Status> superstructure_status_fetcher_;
+  ::aos::Fetcher<Output> superstructure_output_fetcher_;
+
+  bool first_ = true;
+};
+
+class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
+ public:
+  SuperstructureTest()
+      : ::frc971::testing::ControlLoopTest(
+            aos::configuration::ReadConfig("y2024/aos_config.json"),
+            std::chrono::microseconds(5050)),
+        values_(std::make_shared<constants::Values>(constants::MakeValues())),
+        simulated_constants_dummy_(SendSimulationConstants(
+            event_loop_factory(), 7971, "y2024/constants/test_constants.json")),
+        roborio_(aos::configuration::GetNode(configuration(), "roborio")),
+        logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
+        superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
+        superstructure_(superstructure_event_loop.get(), (values_)),
+        test_event_loop_(MakeEventLoop("test", roborio_)),
+        superstructure_goal_fetcher_(
+            test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+        superstructure_goal_sender_(
+            test_event_loop_->MakeSender<Goal>("/superstructure")),
+        superstructure_status_fetcher_(
+            test_event_loop_->MakeFetcher<Status>("/superstructure")),
+        superstructure_output_fetcher_(
+            test_event_loop_->MakeFetcher<Output>("/superstructure")),
+        superstructure_position_fetcher_(
+            test_event_loop_->MakeFetcher<Position>("/superstructure")),
+        superstructure_position_sender_(
+            test_event_loop_->MakeSender<Position>("/superstructure")),
+        drivetrain_status_sender_(
+            test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+        superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+        superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
+    (void)values_;
+    set_team_id(frc971::control_loops::testing::kTeamNumber);
+
+    SetEnabled(true);
+
+    if (!FLAGS_output_folder.empty()) {
+      unlink(FLAGS_output_folder.c_str());
+      logger_event_loop_ = MakeEventLoop("logger", roborio_);
+      logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+      logger_->StartLoggingOnRun(FLAGS_output_folder);
+    }
+  }
+
+  void VerifyNearGoal() {
+    superstructure_goal_fetcher_.Fetch();
+    superstructure_status_fetcher_.Fetch();
+
+    ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
+    ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
+        << ": No status";
+  }
+
+  void CheckIfZeroed() {
+    superstructure_status_fetcher_.Fetch();
+    ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
+  }
+
+  void WaitUntilZeroed() {
+    int i = 0;
+    do {
+      i++;
+      RunFor(dt());
+      superstructure_status_fetcher_.Fetch();
+      // 2 Seconds
+
+      ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+
+      // Since there is a delay when sending running, make sure we have a
+      // status before checking it.
+    } while (superstructure_status_fetcher_.get() == nullptr ||
+             !superstructure_status_fetcher_.get()->zeroed());
+  }
+
+  void SendRobotVelocity(double robot_velocity) {
+    SendDrivetrainStatus(robot_velocity, {0.0, 0.0}, 0.0);
+  }
+
+  void SendDrivetrainStatus(double robot_velocity, Eigen::Vector2d pos,
+                            double theta) {
+    // Send a robot velocity to test compensation
+    auto builder = drivetrain_status_sender_.MakeBuilder();
+    auto drivetrain_status_builder = builder.MakeBuilder<DrivetrainStatus>();
+    drivetrain_status_builder.add_robot_speed(robot_velocity);
+    drivetrain_status_builder.add_estimated_left_velocity(robot_velocity);
+    drivetrain_status_builder.add_estimated_right_velocity(robot_velocity);
+    drivetrain_status_builder.add_x(pos.x());
+    drivetrain_status_builder.add_y(pos.y());
+    drivetrain_status_builder.add_theta(theta);
+    builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
+  }
+
+  std::shared_ptr<const constants::Values> values_;
+  const bool simulated_constants_dummy_;
+
+  const aos::Node *const roborio_;
+  const aos::Node *const logger_pi_;
+
+  ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+  ::y2024::control_loops::superstructure::Superstructure superstructure_;
+  ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+  ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+  ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+  ::aos::Sender<Goal> superstructure_goal_sender_;
+  ::aos::Fetcher<Status> superstructure_status_fetcher_;
+  ::aos::Fetcher<Output> superstructure_output_fetcher_;
+  ::aos::Fetcher<Position> superstructure_position_fetcher_;
+  ::aos::Sender<Position> superstructure_position_sender_;
+  ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+
+  ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+  SuperstructureSimulation superstructure_plant_;
+
+  std::unique_ptr<aos::EventLoop> logger_event_loop_;
+  std::unique_ptr<aos::logger::Logger> logger_;
+
+  const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
+};
+
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
+
+TEST_F(SuperstructureTest, DoesNothing) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+  RunFor(chrono::seconds(10));
+  VerifyNearGoal();
+
+  EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+}
+
+// Tests that loops can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  // Give it a lot of time to get there.
+  RunFor(chrono::seconds(15));
+
+  VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating)
+// behaviour.
+TEST_F(SuperstructureTest, SaturationTest) {
+  SetEnabled(true);
+
+  // Zero it before we move.
+  WaitUntilZeroed();
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+  RunFor(chrono::seconds(20));
+  VerifyNearGoal();
+
+  // Try a low acceleration move with a high max velocity and verify the
+  // acceleration is capped like expected.
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  // TODO(Milo): Make this a sane time
+  RunFor(chrono::seconds(20));
+  VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+  RunFor(chrono::seconds(2));
+}
+
+// Tests that running disabled works
+TEST_F(SuperstructureTest, DisableTest) {
+  RunFor(chrono::seconds(2));
+  CheckIfZeroed();
+}
+}  // namespace testing
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2024
diff --git a/y2024/control_loops/superstructure/superstructure_main.cc b/y2024/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..58932c7
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,28 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+DEFINE_string(arm_trajectories, "arm_trajectories_generated.bfbs",
+              "The path to the generated arm trajectories bfbs file.");
+
+using y2024::control_loops::superstructure::Superstructure;
+
+int main(int argc, char **argv) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("aos_config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+
+  frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+  std::shared_ptr<const y2024::constants::Values> values =
+      std::make_shared<const y2024::constants::Values>(
+          y2024::constants::MakeValues());
+  Superstructure superstructure(&event_loop, values);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024/control_loops/superstructure/superstructure_output.fbs b/y2024/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..6f0df4a
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,6 @@
+namespace y2024.control_loops.superstructure;
+
+table Output {
+}
+
+root_type Output;
diff --git a/y2024/control_loops/superstructure/superstructure_plotter.ts b/y2024/control_loops/superstructure/superstructure_plotter.ts
new file mode 100644
index 0000000..3498f77
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_plotter.ts
@@ -0,0 +1,30 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from '../../../aos/network/www/aos_plotter';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../../aos/network/www/colors';
+import * as proxy from '../../../aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH * 2;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+
+export function plotSuperstructure(conn: Connection, element: Element): void {
+  const aosPlotter = new AosPlotter(conn);
+  //const goal = aosPlotter.addMessageSource(
+  //    '/superstructure', 'y2024.control_loops.superstructure.Goal');
+  //const output = aosPlotter.addMessageSource(
+  //    '/superstructure', 'y2024.control_loops.superstructure.Output');
+  //const status = aosPlotter.addMessageSource(
+  //    '/superstructure', 'y2024.control_loops.superstructure.Status');
+  const position = aosPlotter.addMessageSource(
+      '/superstructure', 'y2024.control_loops.superstructure.Position');
+  //const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+  const positionPlot =
+      aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+  positionPlot.plot.getAxisLabels().setTitle('States');
+  positionPlot.plot.getAxisLabels().setXLabel(TIME);
+  positionPlot.plot.getAxisLabels().setYLabel('wonky state units');
+  positionPlot.plot.setDefaultYRange([-1.0, 2.0]);
+}
diff --git a/y2024/control_loops/superstructure/superstructure_position.fbs b/y2024/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..83d3d23
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,8 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2024.control_loops.superstructure;
+
+table Position {
+}
+
+root_type Position;
diff --git a/y2024/control_loops/superstructure/superstructure_replay.cc b/y2024/control_loops/superstructure/superstructure_replay.cc
new file mode 100644
index 0000000..bd7f9fd
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_replay.cc
@@ -0,0 +1,75 @@
+// This binary allows us to replay the superstructure code over existing
+// logfile. When you run this code, it generates a new logfile with the data all
+// replayed, so that it can then be run through the plotting tool or analyzed
+// in some other way. The original superstructure status data will be on the
+// /original/superstructure channel.
+#include "gflags/gflags.h"
+
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "aos/network/team_number.h"
+#include "y2024/constants.h"
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/superstructure_replay/",
+              "Logs all channels to the provided logfile.");
+
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+
+  aos::network::OverrideTeamNumber(FLAGS_team);
+
+  // open logfiles
+  aos::logger::LogReader reader(
+      aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+  // TODO(james): Actually enforce not sending on the same buses as the logfile
+  // spews out.
+  reader.RemapLoggedChannel("/superstructure",
+                            "y2024.control_loops.superstructure.Status");
+  reader.RemapLoggedChannel("/superstructure",
+                            "y2024.control_loops.superstructure.Output");
+
+  aos::SimulatedEventLoopFactory factory(reader.configuration());
+  reader.Register(&factory);
+
+  aos::NodeEventLoopFactory *roborio =
+      factory.GetNodeEventLoopFactory("roborio");
+
+  unlink(FLAGS_output_folder.c_str());
+  std::unique_ptr<aos::EventLoop> logger_event_loop =
+      roborio->MakeEventLoop("logger");
+  auto logger = std::make_unique<aos::logger::Logger>(logger_event_loop.get());
+  logger->StartLoggingOnRun(FLAGS_output_folder);
+
+  roborio->OnStartup([roborio]() {
+    roborio->AlwaysStart<y2024::control_loops::superstructure::Superstructure>(
+        "superstructure", std::make_shared<y2024::constants::Values>(
+                              y2024::constants::MakeValues()));
+  });
+
+  std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
+  print_loop->SkipAosLog();
+  print_loop->MakeWatcher(
+      "/aos", [&print_loop](const aos::logging::LogMessageFbs &msg) {
+        LOG(INFO) << print_loop->context().monotonic_event_time << " "
+                  << aos::FlatbufferToJson(&msg);
+      });
+  print_loop->MakeWatcher(
+      "/superstructure",
+      [&](const y2024::control_loops::superstructure::Status &status) {
+        if (status.estopped()) {
+          LOG(ERROR) << "Estopped";
+        }
+      });
+
+  factory.Run();
+
+  reader.Deregister();
+
+  return 0;
+}
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..7e8cc27
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,14 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2024.control_loops.superstructure;
+
+table Status {
+  // All subsystems know their location.
+  zeroed:bool (id: 0);
+
+  // If true, we have aborted. This is the or of all subsystem estops.
+  estopped:bool (id: 1);
+}
+
+root_type Status;
diff --git a/y2024/copy_logs.sh b/y2024/copy_logs.sh
new file mode 100755
index 0000000..4d5669c
--- /dev/null
+++ b/y2024/copy_logs.sh
@@ -0,0 +1,29 @@
+#!/bin/bash
+
+# Helper script to copy most recent logs off of the pis
+
+set -e
+
+ROBOT_PREFIX="79" # ..71  (Should be one of 79, 89, 99, or 9)
+PI_LIST="2 3"     # Should be some set of {1,2,3,4,5,6}
+
+LOG_FILE_PATH=/media/sda1/fbs_log-current
+if [[ -z $1 || ! -d $1 ]]; then
+    echo "Please specify the base directory to store the logs ('$1' not found)"
+    exit -1
+fi
+
+# Create output directory based on given directory + a timestamp
+OUTPUT_DIR=$1"/"`date +"%Y-%m-%dT%H-%M-%S"`
+mkdir ${OUTPUT_DIR}
+
+echo "Copying logs from the robot ${ROBOT_PREFIX}71 and pis ${PI_LIST}"
+echo "Storing logs in folder ${OUTPUT_DIR}"
+
+for pi in $PI_LIST; do
+    echo "========================================================"
+    echo "Copying logs from pi-${ROBOT_PREFIX}71-$pi"
+    echo "========================================================"
+    scp -r pi@10.${ROBOT_PREFIX}.71.10${pi}:${LOG_FILE_PATH} ${OUTPUT_DIR}/fbs_log-pi${pi}
+done
+
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
new file mode 100644
index 0000000..ebc2485
--- /dev/null
+++ b/y2024/joystick_reader.cc
@@ -0,0 +1,87 @@
+#include <unistd.h>
+
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/input/action_joystick_input.h"
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/drivetrain_input.h"
+#include "frc971/input/joystick_input.h"
+#include "frc971/input/redundant_joystick_data.h"
+#include "frc971/zeroing/wrap.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+using frc971::CreateProfileParameters;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::input::driver_station::ButtonLocation;
+using frc971::input::driver_station::ControlBit;
+using frc971::input::driver_station::JoystickAxis;
+using frc971::input::driver_station::POVLocation;
+using Side = frc971::control_loops::drivetrain::RobotSide;
+
+namespace y2024 {
+namespace input {
+namespace joysticks {
+
+class Reader : public ::frc971::input::ActionJoystickInput {
+ public:
+  Reader(::aos::EventLoop *event_loop)
+      : ::frc971::input::ActionJoystickInput(
+            event_loop,
+            ::y2024::control_loops::drivetrain::GetDrivetrainConfig(),
+            ::frc971::input::DrivetrainInputReader::InputType::kPistol,
+            {.use_redundant_joysticks = true}),
+        superstructure_goal_sender_(
+            event_loop->MakeSender<control_loops::superstructure::Goal>(
+                "/superstructure")),
+        superstructure_status_fetcher_(
+            event_loop->MakeFetcher<control_loops::superstructure::Status>(
+                "/superstructure")) {}
+
+  void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
+
+  void HandleTeleop(
+      const ::frc971::input::driver_station::Data &data) override {
+    (void)data;
+    superstructure_status_fetcher_.Fetch();
+    if (!superstructure_status_fetcher_.get()) {
+      AOS_LOG(ERROR, "Got no superstructure status message.\n");
+      return;
+    }
+  }
+
+ private:
+  ::aos::Sender<control_loops::superstructure::Goal>
+      superstructure_goal_sender_;
+  ::aos::Fetcher<control_loops::superstructure::Status>
+      superstructure_status_fetcher_;
+};
+
+}  // namespace joysticks
+}  // namespace input
+}  // namespace y2024
+
+int main(int argc, char **argv) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("aos_config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::y2024::input::joysticks::Reader reader(&event_loop);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024/joystick_republish.cc b/y2024/joystick_republish.cc
new file mode 100644
index 0000000..ae6f340
--- /dev/null
+++ b/y2024/joystick_republish.cc
@@ -0,0 +1,34 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "frc971/input/joystick_state_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Config file to use.");
+
+int main(int argc, char *argv[]) {
+  aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+  aos::ShmEventLoop event_loop(&config.message());
+
+  aos::Sender<aos::JoystickState> sender(
+      event_loop.MakeSender<aos::JoystickState>("/imu/aos"));
+
+  event_loop.MakeWatcher(
+      "/roborio/aos", [&](const aos::JoystickState &joystick_state) {
+        auto builder = sender.MakeBuilder();
+        flatbuffers::Offset<aos::JoystickState> state_fbs =
+            aos::CopyFlatBuffer(&joystick_state, builder.fbb());
+        builder.CheckOk(builder.Send(state_fbs));
+      });
+
+  event_loop.Run();
+  return 0;
+}
diff --git a/y2024/log_web_proxy.sh b/y2024/log_web_proxy.sh
new file mode 100755
index 0000000..8161e7f
--- /dev/null
+++ b/y2024/log_web_proxy.sh
@@ -0,0 +1 @@
+./aos/network/log_web_proxy_main --data_dir=y2024/www $@
diff --git a/y2024/pi_send_joystick.sh b/y2024/pi_send_joystick.sh
new file mode 100755
index 0000000..9b60e02
--- /dev/null
+++ b/y2024/pi_send_joystick.sh
@@ -0,0 +1,18 @@
+#!/usr/bin/sh
+
+# Helper script to spoof joystick state of robot enabled, to triggger image logging
+
+# Currently, this is going through pi6.  Need to set the right IP address for the bot
+imu_pi6="pi@10.79.71.106"
+
+# TODO(milind): add logger in the future
+echo "Sending Joystick command '$1' to $imu_pi6"
+ssh ${imu_pi6} "bin/aos_send /imu/aos aos.JoystickState '{\"enabled\": $1}'"
+
+if [ $1 = "false" ]
+then
+    # This extra sleep is necessary to make sure the logs rotate to a new file
+    sleep 6
+    echo "Sending Joystick command '$1' to $imu_pi6"
+    ssh ${imu_pi6} "bin/aos_send /imu/aos aos.JoystickState '{\"enabled\": $1}'"
+fi
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
new file mode 100644
index 0000000..98b5197
--- /dev/null
+++ b/y2024/vision/BUILD
@@ -0,0 +1,24 @@
+filegroup(
+    name = "image_streamer_start",
+    srcs = ["image_streamer_start.sh"],
+    visibility = ["//visibility:public"],
+)
+
+cc_binary(
+    name = "image_logger",
+    srcs = [
+        "image_logger.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos:configuration",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//aos/events/logging:log_writer",
+        "//aos/logging:log_namer",
+        "//frc971/input:joystick_state_fbs",
+        "@com_github_gflags_gflags//:gflags",
+        "@com_github_google_glog//:glog",
+    ],
+)
diff --git a/y2024/vision/README.md b/y2024/vision/README.md
new file mode 100644
index 0000000..c81581f
--- /dev/null
+++ b/y2024/vision/README.md
@@ -0,0 +1,25 @@
+How to use the extrinsic calibration for camera-to-camera calibration
+
+This all assumes you have cameras that have been intrinsically
+calibrated, and that pi1 has a valid extrinsic calibration (from robot
+origin / IMU to the pi1 camera).
+
+It by default will compute the extrinsics for each of the other cameras (pi2,
+pi3, pi4) relative to the robot origin (IMU origin).
+
+Steps:
+* Place two Aruco Diamond markers about 1 meter apart
+  (center-to-center) at a height so that they will be in view of the
+  cameras when the robot is about 1-2m away
+* Start with the robot in a position that both markers are fully in
+  view by one camera
+* Enable logging for all cameras
+* Rotate the robot in place through a full circle, so that each camera sees both tags, and at times just one tag.
+* Stop the logging and copy the files to your laptop
+* Run the calibration code on the resulting files, e.g.,
+  * `bazel run -c opt //y2023/vision:calibrate_multi_cameras -- /data/PATH_TO_LOGS --team_number 971 --target_type charuco_diamond
+  * Can add "--visualize" flag to see the camera views and marker detections
+  * I've sometimes found it necessary to add the "--skip_missing_forwarding_entries" flag-- as guided by the logging messages
+* From the output, copy the calculated ("Updated") extrinsic into the
+  corresponding calibration file for the right robot and camera in the
+  calib_files directory
diff --git a/y2024/vision/foxglove_image_converter.cc b/y2024/vision/foxglove_image_converter.cc
new file mode 100644
index 0000000..bfaafc8
--- /dev/null
+++ b/y2024/vision/foxglove_image_converter.cc
@@ -0,0 +1,20 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/vision/foxglove_image_converter_lib.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+  aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  frc971::vision::FoxgloveImageConverter converter(
+      &event_loop, "/camera", "/camera",
+      frc971::vision::ImageCompression::kJpeg);
+
+  event_loop.Run();
+}
diff --git a/y2024/vision/image_logger.cc b/y2024/vision/image_logger.cc
new file mode 100644
index 0000000..45e25f6
--- /dev/null
+++ b/y2024/vision/image_logger.cc
@@ -0,0 +1,112 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/log_namer.h"
+#include "frc971/input/joystick_state_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Config file to use.");
+
+DEFINE_double(rotate_every, 0.0,
+              "If set, rotate the logger after this many seconds");
+DECLARE_int32(flush_size);
+DEFINE_double(disabled_time, 5.0,
+              "Continue logging if disabled for this amount of time or less");
+DEFINE_bool(direct, false,
+            "If true, write using O_DIRECT and write 512 byte aligned blocks "
+            "whenever possible.");
+
+std::unique_ptr<aos::logger::MultiNodeFilesLogNamer> MakeLogNamer(
+    aos::EventLoop *event_loop) {
+  std::optional<std::string> log_name =
+      aos::logging::MaybeGetLogName("fbs_log");
+
+  if (!log_name.has_value()) {
+    return nullptr;
+  }
+
+  return std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
+      event_loop, std::make_unique<aos::logger::RenamableFileBackend>(
+                      absl::StrCat(log_name.value(), "/"), FLAGS_direct));
+}
+
+int main(int argc, char *argv[]) {
+  gflags::SetUsageMessage(
+      "This program provides a simple logger binary that logs all SHMEM data "
+      "directly to a file specified at the command line when the robot is "
+      "enabled and for a bit of time after.");
+  aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  bool logging = false;
+  bool enabled = false;
+  aos::monotonic_clock::time_point last_disable_time =
+      event_loop.monotonic_now();
+  aos::monotonic_clock::time_point last_rotation_time =
+      event_loop.monotonic_now();
+  aos::logger::Logger logger(&event_loop);
+
+  if (FLAGS_rotate_every != 0.0) {
+    logger.set_on_logged_period([&](aos::monotonic_clock::time_point) {
+      const auto now = event_loop.monotonic_now();
+      if (logging && now > last_rotation_time + std::chrono::duration<double>(
+                                                    FLAGS_rotate_every)) {
+        logger.Rotate();
+        last_rotation_time = now;
+      }
+    });
+  }
+
+  event_loop.OnRun([]() {
+    errno = 0;
+    setpriority(PRIO_PROCESS, 0, -20);
+    PCHECK(errno == 0) << ": Renicing to -20 failed.";
+  });
+
+  event_loop.MakeWatcher(
+      "/imu/aos", [&](const aos::JoystickState &joystick_state) {
+        const auto timestamp = event_loop.context().monotonic_event_time;
+        // Store the last time we got disabled
+        if (enabled && !joystick_state.enabled()) {
+          last_disable_time = timestamp;
+        }
+        enabled = joystick_state.enabled();
+
+        if (!logging && enabled) {
+          auto log_namer = MakeLogNamer(&event_loop);
+          if (log_namer == nullptr) {
+            return;
+          }
+
+          // Start logging if we just got enabled
+          LOG(INFO) << "Starting logging";
+          logger.StartLogging(std::move(log_namer));
+          logging = true;
+          last_rotation_time = event_loop.monotonic_now();
+        } else if (logging && !enabled &&
+                   (timestamp - last_disable_time) >
+                       std::chrono::duration<double>(FLAGS_disabled_time)) {
+          // Stop logging if we've been disabled for a non-negligible amount of
+          // time
+          LOG(INFO) << "Stopping logging";
+          logger.StopLogging(event_loop.monotonic_now());
+          logging = false;
+        }
+      });
+
+  event_loop.Run();
+
+  LOG(INFO) << "Shutting down";
+
+  return 0;
+}
diff --git a/y2024/vision/image_streamer_start.sh b/y2024/vision/image_streamer_start.sh
new file mode 100755
index 0000000..48d9da7
--- /dev/null
+++ b/y2024/vision/image_streamer_start.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+# Some configurations to avoid dropping frames
+# 640x480@30fps, 400x300@60fps.
+# Bitrate 500000-1500000
+WIDTH=640
+HEIGHT=480
+BITRATE=1500000
+LISTEN_ON="/camera/downsized"
+# Don't interfere with field webpage
+STREAMING_PORT=1181
+
+# Handle weirdness with openssl and gstreamer
+export OPENSSL_CONF=""
+
+# Enable for verbose logging
+#export GST_DEBUG=4
+
+export LD_LIBRARY_PATH=/usr/lib/aarch64-linux-gnu/gstreamer-1.0
+
+exec ./image_streamer --width=$WIDTH --height=$HEIGHT --bitrate=$BITRATE --listen_on=$LISTEN_ON --config=aos_config.json --streaming_port=$STREAMING_PORT
+
diff --git a/y2024/vision/localization_verifier.cc b/y2024/vision/localization_verifier.cc
new file mode 100644
index 0000000..c16f874
--- /dev/null
+++ b/y2024/vision/localization_verifier.cc
@@ -0,0 +1,108 @@
+#include "glog/logging.h"
+
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2023/localizer/localizer.h"
+#include "y2023/localizer/utils.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+DEFINE_uint64(min_april_id, 1,
+              "Minimum april id to consider as part of the field and ignore");
+DEFINE_uint64(max_april_id, 8,
+              "Maximum april id to consider as part of the field and ignore");
+
+// This binary allows us to place extra april tags on the field and verify
+// that we can compute their field pose correctly
+
+namespace y2023::vision {
+
+using localizer::Localizer;
+
+Localizer::Transform LocalizerOutputToTransform(
+    const frc971::controls::LocalizerOutput &localizer) {
+  const auto T_field_robot =
+      Eigen::Translation3d(localizer.x(), localizer.y(), 0.0);
+
+  Eigen::AngleAxisd robot_yaw_angle(localizer.theta(),
+                                    Eigen::Vector3d::UnitZ());
+  const auto R_field_robot = Eigen::Quaterniond(robot_yaw_angle);
+  const Localizer::Transform H_field_robot =
+      (T_field_robot * R_field_robot).matrix();
+  return H_field_robot;
+}
+
+void HandleDetections(
+    const frc971::vision::TargetMap &detections,
+    const Localizer::Transform &H_robot_camera,
+    aos::Fetcher<frc971::controls::LocalizerOutput> *localizer_fetcher) {
+  localizer_fetcher->Fetch();
+  CHECK(localizer_fetcher->get());
+
+  for (const auto *target_pose : *detections.target_poses()) {
+    // Only look at april tags not part of the field
+    if (target_pose->id() >= FLAGS_min_april_id &&
+        target_pose->id() <= FLAGS_max_april_id) {
+      continue;
+    }
+
+    const Localizer::Transform H_camera_target =
+        localizer::PoseToTransform(target_pose);
+    const Localizer::Transform H_field_robot =
+        LocalizerOutputToTransform(*localizer_fetcher->get());
+
+    // Get the field-based target pose using the detection, extrinsics, and
+    // localizer output
+    const Localizer::Transform H_field_target =
+        H_field_robot * H_robot_camera * H_camera_target;
+
+    LOG(INFO) << "Field to target " << target_pose->id();
+    LOG(INFO) << "H_field_robot " << H_field_robot;
+    LOG(INFO) << "H_robot_camera " << H_robot_camera;
+    LOG(INFO) << "H_camera_target " << H_camera_target;
+    LOG(INFO) << "Transform: " << H_field_target;
+    LOG(INFO) << "Translation: "
+              << Eigen::Affine3d(H_field_target).translation();
+    LOG(INFO);
+  }
+}
+
+void LocalizationVerifierMain() {
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  frc971::constants::WaitForConstants<Constants>(&config.message());
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
+
+  aos::Fetcher<frc971::controls::LocalizerOutput> localizer_fetcher =
+      event_loop.MakeFetcher<frc971::controls::LocalizerOutput>("/localizer");
+
+  for (const auto *camera : *constants_fetcher.constants().cameras()) {
+    CHECK(camera->has_calibration());
+    Localizer::Transform H_robot_camera =
+        frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
+            *camera->calibration()->fixed_extrinsics());
+
+    const std::string_view pi_name =
+        camera->calibration()->node_name()->string_view();
+    event_loop.MakeWatcher(
+        absl::StrCat("/", pi_name, "/camera"),
+        [H_robot_camera,
+         &localizer_fetcher](const frc971::vision::TargetMap &target_map) {
+          HandleDetections(target_map, H_robot_camera, &localizer_fetcher);
+        });
+  }
+  event_loop.Run();
+}
+
+}  // namespace y2023::vision
+
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  y2023::vision::LocalizationVerifierMain();
+}
diff --git a/y2024/vision/maps/BUILD b/y2024/vision/maps/BUILD
new file mode 100644
index 0000000..38191a4
--- /dev/null
+++ b/y2024/vision/maps/BUILD
@@ -0,0 +1,7 @@
+filegroup(
+    name = "maps",
+    srcs = glob([
+        "*.json",
+    ]),
+    visibility = ["//visibility:public"],
+)
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
new file mode 100644
index 0000000..8c1cfd3
--- /dev/null
+++ b/y2024/wpilib_interface.cc
@@ -0,0 +1,292 @@
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cinttypes>
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include "ctre/phoenix/CANifier.h"
+
+#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/Servo.h"
+#include "frc971/wpilib/ahal/TalonFX.h"
+#include "frc971/wpilib/ahal/VictorSP.h"
+#undef ERROR
+
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
+#include "ctre/phoenix6/TalonFX.hpp"
+
+#include "aos/commonmath.h"
+#include "aos/containers/sized_array.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/realtime.h"
+#include "aos/time/time.h"
+#include "aos/util/log_interval.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/wrapping_counter.h"
+#include "frc971/autonomous/auto_mode_generated.h"
+#include "frc971/can_configuration_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/input/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2024/constants.h"
+#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
+
+DEFINE_bool(ctre_diag_server, false,
+            "If true, enable the diagnostics server for interacting with "
+            "devices on the CAN bus using Phoenix Tuner");
+
+using ::aos::monotonic_clock;
+using ::frc971::CANConfiguration;
+using ::y2024::constants::Values;
+namespace superstructure = ::y2024::control_loops::superstructure;
+namespace drivetrain = ::y2024::control_loops::drivetrain;
+namespace chrono = ::std::chrono;
+using std::make_unique;
+
+namespace y2024 {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
+// DMA stuff and then removing the * 2.0 in *_translate.
+// The low bit is direction.
+
+double drivetrain_velocity_translate(double in) {
+  return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
+          (2.0 * M_PI)) *
+         Values::kDrivetrainEncoderRatio() *
+         control_loops::drivetrain::kWheelRadius;
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
+    Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+              "fast encoders are too fast");
+
+}  // namespace
+
+static constexpr int kCANFalconCount = 6;
+static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+  SensorReader(::aos::ShmEventLoop *event_loop,
+               std::shared_ptr<const Values> values)
+      : ::frc971::wpilib::SensorReader(event_loop),
+        values_(std::move(values)),
+        auto_mode_sender_(
+            event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+                "/autonomous")),
+        superstructure_position_sender_(
+            event_loop->MakeSender<superstructure::Position>(
+                "/superstructure")),
+        drivetrain_position_sender_(
+            event_loop
+                ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+                    "/drivetrain")),
+        gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
+            "/drivetrain")){};
+  void Start() override { AddToDMA(&imu_yaw_rate_reader_); }
+
+  // Auto mode switches.
+  void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+    autonomous_modes_.at(i) = ::std::move(sensor);
+  }
+
+  void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+    imu_yaw_rate_input_ = ::std::move(sensor);
+    imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
+  }
+
+  void RunIteration() override {
+    {
+      auto builder = superstructure_position_sender_.MakeBuilder();
+
+      superstructure::Position::Builder position_builder =
+          builder.MakeBuilder<superstructure::Position>();
+      builder.CheckOk(builder.Send(position_builder.Finish()));
+    }
+
+    {
+      auto builder = drivetrain_position_sender_.MakeBuilder();
+      frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+          builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+      drivetrain_builder.add_left_encoder(
+          constants::Values::DrivetrainEncoderToMeters(
+              drivetrain_left_encoder_->GetRaw()));
+      drivetrain_builder.add_left_speed(
+          drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+
+      drivetrain_builder.add_right_encoder(
+          -constants::Values::DrivetrainEncoderToMeters(
+              drivetrain_right_encoder_->GetRaw()));
+      drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+          drivetrain_right_encoder_->GetPeriod()));
+
+      builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
+    }
+
+    {
+      auto builder = gyro_sender_.MakeBuilder();
+      ::frc971::sensors::GyroReading::Builder gyro_reading_builder =
+          builder.MakeBuilder<::frc971::sensors::GyroReading>();
+      // +/- 2000 deg / sec
+      constexpr double kMaxVelocity = 4000;  // degrees / second
+      constexpr double kVelocityRadiansPerSecond =
+          kMaxVelocity / 360 * (2.0 * M_PI);
+
+      // Only part of the full range is used to prevent being 100% on or off.
+      constexpr double kScaledRangeLow = 0.1;
+      constexpr double kScaledRangeHigh = 0.9;
+
+      constexpr double kPWMFrequencyHz = 200;
+      double velocity_duty_cycle =
+          imu_yaw_rate_reader_.last_width() * kPWMFrequencyHz;
+
+      constexpr double kDutyCycleScale =
+          1 / (kScaledRangeHigh - kScaledRangeLow);
+      // scale from 0.1 - 0.9 to 0 - 1
+      double rescaled_velocity_duty_cycle =
+          (velocity_duty_cycle - kScaledRangeLow) * kDutyCycleScale;
+
+      if (!std::isnan(rescaled_velocity_duty_cycle)) {
+        gyro_reading_builder.add_velocity((rescaled_velocity_duty_cycle - 0.5) *
+                                          kVelocityRadiansPerSecond);
+      }
+      builder.CheckOk(builder.Send(gyro_reading_builder.Finish()));
+    }
+
+    {
+      auto builder = auto_mode_sender_.MakeBuilder();
+
+      uint32_t mode = 0;
+      for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+        if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+          mode |= 1 << i;
+        }
+      }
+
+      auto auto_mode_builder =
+          builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+      auto_mode_builder.add_mode(mode);
+
+      builder.CheckOk(builder.Send(auto_mode_builder.Finish()));
+    }
+  }
+
+ private:
+  std::shared_ptr<const Values> values_;
+
+  aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
+  aos::Sender<superstructure::Position> superstructure_position_sender_;
+  aos::Sender<frc971::control_loops::drivetrain::Position>
+      drivetrain_position_sender_;
+  ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
+
+  std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
+  std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+
+  frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+  ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+    return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+                                     frc::Encoder::k4X);
+  }
+
+  void Run() override {
+    std::shared_ptr<const Values> values =
+        std::make_shared<const Values>(constants::MakeValues());
+
+    aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+        aos::configuration::ReadConfig("aos_config.json");
+
+    // Thread 1.
+    ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
+    ::frc971::wpilib::JoystickSender joystick_sender(
+        &joystick_sender_event_loop);
+    AddLoop(&joystick_sender_event_loop);
+
+    // Thread 2.
+    ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
+    ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
+    AddLoop(&pdp_fetcher_event_loop);
+
+    // Thread 3.
+    ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+    SensorReader sensor_reader(&sensor_reader_event_loop, values);
+    sensor_reader.set_pwm_trigger(true);
+    sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
+    sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
+    sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
+
+    AddLoop(&sensor_reader_event_loop);
+
+    // Thread 4.
+    // Set up CAN.
+    if (!FLAGS_ctre_diag_server) {
+      c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+      c_Phoenix_Diagnostics_Dispose();
+    }
+
+    ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
+        constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
+    ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
+        constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+
+    ::aos::ShmEventLoop can_output_event_loop(&config.message());
+    can_output_event_loop.set_name("CANOutputWriter");
+
+    // Thread 5
+    // Set up superstructure output.
+    ::aos::ShmEventLoop output_event_loop(&config.message());
+    output_event_loop.set_name("PWMOutputWriter");
+
+    AddLoop(&output_event_loop);
+
+    // Thread 6
+
+    RunLoops();
+  }
+};
+
+}  // namespace wpilib
+}  // namespace y2024
+
+AOS_ROBOT_CLASS(::y2024::wpilib::WPILibRobot);
diff --git a/y2024/www/BUILD b/y2024/www/BUILD
new file mode 100644
index 0000000..726a354
--- /dev/null
+++ b/y2024/www/BUILD
@@ -0,0 +1,67 @@
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+filegroup(
+    name = "files",
+    srcs = glob([
+        "**/*.html",
+        "**/*.css",
+        "**/*.png",
+    ]) + ["2024.png"],
+    visibility = ["//visibility:public"],
+)
+
+#Need to add 2024 field png
+genrule(
+    name = "2024_field_png",
+    srcs = ["//third_party/y2023/field:pictures"],
+    outs = ["2024.png"],
+    cmd = "cp third_party/y2023/field/2023.png $@",
+)
+
+ts_project(
+    name = "field_main",
+    srcs = [
+        "constants.ts",
+        "field_handler.ts",
+        "field_main.ts",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos/network:connect_ts_fbs",
+        "//aos/network:message_bridge_client_ts_fbs",
+        "//aos/network:message_bridge_server_ts_fbs",
+        "//aos/network:web_proxy_ts_fbs",
+        "//aos/network/www:proxy",
+        "//frc971/control_loops:control_loops_ts_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_can_position_ts_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_position_ts_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+        "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
+        "//frc971/vision:target_map_ts_fbs",
+        "//y2024/control_loops/superstructure:superstructure_status_ts_fbs",
+        "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+    ],
+)
+
+rollup_bundle(
+    name = "field_main_bundle",
+    entry_point = "field_main.ts",
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2024:__subpackages__"],
+    deps = [
+        ":field_main",
+    ],
+)
+
+aos_downloader_dir(
+    name = "www_files",
+    srcs = [
+        ":field_main_bundle.min.js",
+        ":files",
+        "//frc971/analysis:plot_index_bundle.min.js",
+    ],
+    dir = "www",
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
diff --git a/y2024/www/constants.ts b/y2024/www/constants.ts
new file mode 100644
index 0000000..97d6e28
--- /dev/null
+++ b/y2024/www/constants.ts
@@ -0,0 +1,8 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+// Numbers are slightly hand-tuned to match the PNG that we are using.
+export const FIELD_WIDTH = 26 * FT_TO_M + 11.25 * IN_TO_M;
+export const FIELD_LENGTH = 54 * FT_TO_M + 3.25 * IN_TO_M;
+
diff --git a/y2024/www/field.html b/y2024/www/field.html
new file mode 100644
index 0000000..8c3b291
--- /dev/null
+++ b/y2024/www/field.html
@@ -0,0 +1,87 @@
+<html>
+  <head>
+    <script src="field_main_bundle.min.js" defer></script>
+    <link rel="stylesheet" href="styles.css">
+  </head>
+  <body>
+    <div id="field"> </div>
+    <div id="legend"> </div>
+    <div id="readouts">
+      <table>
+        <tr>
+          <th colspan="2">Robot State</th>
+        </tr>
+        <tr>
+          <td>X</td>
+          <td id="x"> NA </td>
+        </tr>
+        <tr>
+          <td>Y</td>
+          <td id="y"> NA </td>
+        </tr>
+        <tr>
+          <td>Theta</td>
+          <td id="theta"> NA </td>
+        </tr>
+      </table>
+
+      <table>
+        <tr>
+          <th colspan="2">Images</th>
+        </tr>
+        <tr>
+          <td> Images Accepted </td>
+          <td id="images_accepted"> NA </td>
+        </tr>
+      </table>
+
+      <table>
+        <tr>
+          <th colspan="2">Superstructure</th>
+    </tr>
+    <!-- TODO: Add superstructure state -->
+  </table>
+  <table>
+    <tr>
+      <th colspan="2">Game Piece</th>
+    </tr>
+    <tr>
+      <td>Game Piece Held</td>
+      <td id="game_piece"> NA </td>
+    </tr>
+    <tr>
+      <td>Game Piece Position (+ = left, 0 = empty)</td>
+      <td id="game_piece_position"> NA </td>
+    </tr>
+  </table>
+
+  <h3>Zeroing Faults:</h3>
+  <p id="zeroing_faults"> NA </p>
+  </div>
+  <div id="middle_readouts">
+    <div id="vision_readouts">
+    </div>
+    <div id="message_bridge_status">
+      <div>
+        <div>Node</div>
+        <div>Client</div>
+        <div>Server</div>
+      </div>
+    </div>
+  </div>
+  <table>
+      <tr>
+        <th colspan="2"> Drivetrain Encoder Positions </th>
+      </tr>
+      <tr>
+        <td> Left Encoder Position</td>
+        <td id="left_drivetrain_encoder"> NA </td>
+      </tr>
+      <tr>
+        <td> Right Encoder Position</td>
+        <td id="right_drivetrain_encoder"> NA </td>
+      </tr>
+  </table>
+  </body>
+</html>
+
diff --git a/y2024/www/field_handler.ts b/y2024/www/field_handler.ts
new file mode 100644
index 0000000..6c5f2e3
--- /dev/null
+++ b/y2024/www/field_handler.ts
@@ -0,0 +1,25 @@
+import {ByteBuffer} from 'flatbuffers'
+import {ClientStatistics} from '../../aos/network/message_bridge_client_generated'
+import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
+import {Connection} from '../../aos/network/www/proxy'
+import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Position as DrivetrainPosition} from '../../frc971/control_loops/drivetrain/drivetrain_position_generated'
+import {CANPosition as DrivetrainCANPosition} from '../../frc971/control_loops/drivetrain/drivetrain_can_position_generated'
+import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
+import {TargetMap} from '../../frc971/vision/target_map_generated'
+
+
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+// (0,0) is field center, +X is toward red DS
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
+
+const ROBOT_WIDTH = 25 * IN_TO_M;
+const ROBOT_LENGTH = 32 * IN_TO_M;
+
+export class FieldHandler {
+  constructor(private readonly connection: Connection) {
+  }
+}
diff --git a/y2024/www/field_main.ts b/y2024/www/field_main.ts
new file mode 100644
index 0000000..24bc23a
--- /dev/null
+++ b/y2024/www/field_main.ts
@@ -0,0 +1,11 @@
+import {Connection} from '../../aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+
diff --git a/y2024/www/index.html b/y2024/www/index.html
new file mode 100644
index 0000000..e4e185e
--- /dev/null
+++ b/y2024/www/index.html
@@ -0,0 +1,6 @@
+<html>
+  <body>
+    <a href="field.html">Field Visualization</a><br>
+    <a href="plotter.html">Plots</a>
+  </body>
+</html>
diff --git a/y2024/www/plotter.html b/y2024/www/plotter.html
new file mode 100644
index 0000000..629ceaa
--- /dev/null
+++ b/y2024/www/plotter.html
@@ -0,0 +1,7 @@
+<html>
+  <head>
+    <script src="plot_index_bundle.min.js" defer></script>
+  </head>
+  <body>
+  </body>
+</html>
diff --git a/y2024/www/styles.css b/y2024/www/styles.css
new file mode 100644
index 0000000..c2c44d2
--- /dev/null
+++ b/y2024/www/styles.css
@@ -0,0 +1,74 @@
+.channel {
+  display: flex;
+  border-bottom: 1px solid;
+  font-size: 24px;
+}
+#field {
+  display: inline-block
+}
+
+#readouts,
+#middle_readouts
+{
+  display: inline-block;
+  vertical-align: top;
+  float: right;
+}
+
+
+#legend {
+  display: inline-block;
+}
+
+table, th, td {
+  border: 1px solid black;
+  border-collapse: collapse;
+  padding: 5px;
+  margin: 10px;
+}
+
+th, td {
+  text-align: right;
+  width: 70px;
+}
+
+td:first-child {
+  width: 150px;
+}
+
+.connected, .near {
+  background-color: LightGreen;
+  border-radius: 10px;
+}
+
+.zeroing {
+  background-color: yellow;
+  border-radius: 10px;
+}
+
+.faulted {
+  background-color: red;
+  border-radius: 10px;
+}
+
+#vision_readouts > div {
+  display: table-row;
+  padding: 5px;
+}
+
+#vision_readouts > div > div {
+  display: table-cell;
+  padding: 5px;
+  text-align: right;
+}
+
+#message_bridge_status > div {
+  display: table-row;
+  padding: 5px;
+}
+
+#message_bridge_status > div > div {
+  display: table-cell;
+  padding: 5px;
+  text-align: right;
+}
diff --git a/y2024/y2024.json b/y2024/y2024.json
new file mode 100644
index 0000000..c887499
--- /dev/null
+++ b/y2024/y2024.json
@@ -0,0 +1,18 @@
+{
+  "channel_storage_duration": 10000000000,
+  "maps": [
+    {
+      "match": {
+        "name": "/aos",
+        "type": "aos.RobotState"
+      },
+      "rename": {
+        "name": "/roborio/aos"
+      }
+    }
+  ],
+  "imports": [
+    "y2024_roborio.json",
+    "y2024_imu.json"
+  ]
+}
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
new file mode 100644
index 0000000..d2b5faa
--- /dev/null
+++ b/y2024/y2024_imu.json
@@ -0,0 +1,253 @@
+{
+  "channels": [
+    {
+      "name": "/imu/aos",
+      "type": "aos.JoystickState",
+      "source_node": "imu",
+      "frequency": 100
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.timing.Report",
+      "source_node": "imu",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 4096
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.logging.LogMessageFbs",
+      "source_node": "imu",
+      "frequency": 200,
+      "num_senders": 20
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.starter.Status",
+      "source_node": "imu",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 2048
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.starter.StarterRpc",
+      "source_node": "imu",
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.message_bridge.ServerStatistics",
+      "source_node": "imu",
+      "max_size": 2048,
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.message_bridge.ClientStatistics",
+      "source_node": "imu",
+      "frequency": 20,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.logging.DynamicLogCommand",
+      "source_node": "imu",
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.message_bridge.Timestamp",
+      "source_node": "imu",
+      "frequency": 15,
+      "num_senders": 2,
+      "logger_nodes": [
+        "roborio"
+      ],
+      "max_size": 400,
+      "destination_nodes": [
+        {
+          "name": "roborio",
+          "priority": 1,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "imu"
+          ],
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/imu/aos/remote_timestamps/roborio/imu/aos/aos-message_bridge-Timestamp",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 20,
+      "source_node": "imu",
+      "max_size": 208
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.starter.StarterRpc",
+      "source_node": "roborio",
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "roborio"
+          ],
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+      "type": "aos.message_bridge.RemoteMessage",
+      "source_node": "roborio",
+      "logger": "NOT_LOGGED",
+      "frequency": 20,
+      "num_senders": 2,
+      "max_size": 200
+    },
+    {
+      "name": "/localizer",
+      "type": "frc971.IMUValuesBatch",
+      "source_node": "imu",
+      "frequency": 2200,
+      "max_size": 1600,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/constants",
+      "type": "y2024.Constants",
+      "source_node": "imu",
+      "frequency": 1,
+      "num_senders": 2,
+      "max_size": 65536
+    }
+  ],
+  "applications": [
+    {
+      "name": "message_bridge_client",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "localizer",
+      "executable_name": "localizer_main",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "imu",
+      "executable_name": "imu_main",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "joystick_republish",
+      "executable_name": "joystick_republish",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "message_bridge_server",
+      "executable_name": "message_bridge_server",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "localizer_logger",
+      "executable_name": "logger_main",
+      "args": [
+        "--logging_folder",
+        "",
+        "--snappy_compress",
+        "--rotate_every", "30.0"
+      ],
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "web_proxy",
+      "executable_name": "web_proxy_main",
+      "args": [
+        "--min_ice_port=5800",
+        "--max_ice_port=5810"
+      ],
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "foxglove_websocket",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "constants_sender",
+      "autorestart": false,
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    }
+  ],
+  "maps": [
+    {
+      "match": {
+        "name": "/constants*",
+        "source_node": "imu"
+      },
+      "rename": {
+        "name": "/imu/constants"
+      }
+    },
+    {
+      "match": {
+        "name": "/aos*",
+        "source_node": "imu"
+      },
+      "rename": {
+        "name": "/imu/aos"
+      }
+    }
+  ],
+  "nodes": [
+    {
+      "name": "imu",
+      "hostname": "pi6",
+      "hostnames": [
+        "pi-971-6",
+        "pi-7971-6",
+        "pi-8971-6",
+        "pi-9971-6"
+      ],
+      "port": 9971
+    },
+    {
+      "name": "roborio"
+    }
+  ]
+}
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
new file mode 100644
index 0000000..93caacc
--- /dev/null
+++ b/y2024/y2024_roborio.json
@@ -0,0 +1,453 @@
+{
+  "channels": [
+    {
+      "name": "/roborio/aos",
+      "type": "aos.JoystickState",
+      "source_node": "roborio",
+      "frequency": 100,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "time_to_live": 50000000
+        }
+      ]
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.RobotState",
+      "source_node": "roborio",
+      "frequency": 250
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.timing.Report",
+      "source_node": "roborio",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 8192
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.logging.LogMessageFbs",
+      "source_node": "roborio",
+      "frequency": 500,
+      "max_size": 1000,
+      "num_senders": 20
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.starter.Status",
+      "source_node": "roborio",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 2000
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.starter.StarterRpc",
+      "source_node": "roborio",
+      "frequency": 10,
+      "max_size": 400,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.message_bridge.ServerStatistics",
+      "source_node": "roborio",
+      "max_size": 2048,
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.message_bridge.ClientStatistics",
+      "source_node": "roborio",
+      "frequency": 20,
+      "max_size": 2000,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.logging.DynamicLogCommand",
+      "source_node": "roborio",
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 20,
+      "source_node": "roborio",
+      "max_size": 208
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.message_bridge.Timestamp",
+      "source_node": "roborio",
+      "frequency": 15,
+      "num_senders": 2,
+      "max_size": 512,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 1,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "roborio"
+          ],
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/superstructure",
+      "type": "y2024.control_loops.superstructure.Goal",
+      "source_node": "roborio",
+      "frequency": 250,
+      "max_size": 512
+    },
+    {
+      "name": "/superstructure",
+      "type": "y2024.control_loops.superstructure.Status",
+      "source_node": "roborio",
+      "frequency": 400,
+      "num_senders": 2
+    },
+    {
+      "name": "/superstructure",
+      "type": "y2024.control_loops.superstructure.Output",
+      "source_node": "roborio",
+      "frequency": 250,
+      "num_senders": 2,
+      "max_size": 224
+    },
+    {
+      "name": "/superstructure",
+      "type": "y2024.control_loops.superstructure.Position",
+      "source_node": "roborio",
+      "frequency": 250,
+      "num_senders": 2,
+      "max_size": 448
+    },
+    {
+      "name": "/can",
+      "type": "frc971.can_logger.CanFrame",
+      "source_node": "roborio",
+      "frequency": 6000,
+      "num_senders": 2,
+      "max_size": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.CANPosition",
+      "source_node": "roborio",
+      "frequency": 220,
+      "num_senders": 2,
+      "max_size": 400
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.sensors.GyroReading",
+      "source_node": "roborio",
+      "frequency": 250,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.sensors.Uid",
+      "source_node": "roborio",
+      "frequency": 250,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.fb.Trajectory",
+      "source_node": "roborio",
+      "max_size": 600000,
+      "frequency": 10,
+      "logger": "NOT_LOGGED",
+      "num_senders": 2,
+      "read_method": "PIN",
+      "num_readers": 10
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.SplineGoal",
+      "source_node": "roborio",
+      "frequency": 10
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Goal",
+      "source_node": "roborio",
+      "max_size": 224,
+      "frequency": 250
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Position",
+      "source_node": "roborio",
+      "frequency": 400,
+      "max_size": 112,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Output",
+      "source_node": "roborio",
+      "frequency": 400,
+      "max_size": 80,
+      "num_senders": 2,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Status",
+      "source_node": "roborio",
+      "frequency": 400,
+      "max_size": 1616,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.LocalizerControl",
+      "source_node": "roborio",
+      "frequency": 250,
+      "max_size": 96,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "roborio"
+          ],
+          "time_to_live": 0
+        }
+      ]
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-LocalizerControl",
+      "type": "aos.message_bridge.RemoteMessage",
+      "source_node": "roborio",
+      "logger": "NOT_LOGGED",
+      "frequency": 400,
+      "num_senders": 2,
+      "max_size": 200
+    },
+    {
+      "name": "/autonomous",
+      "type": "aos.common.actions.Status",
+      "source_node": "roborio"
+    },
+    {
+      "name": "/autonomous",
+      "type": "frc971.autonomous.Goal",
+      "source_node": "roborio"
+    },
+    {
+      "name": "/autonomous",
+      "type": "frc971.autonomous.AutonomousMode",
+      "source_node": "roborio",
+      "frequency": 250
+    },
+
+    {
+      "name": "/roborio",
+      "type": "frc971.CANConfiguration",
+      "source_node": "roborio",
+      "frequency": 2
+    },
+    {
+      "name": "/roborio/constants",
+      "type": "y2024.Constants",
+      "source_node": "roborio",
+      "frequency": 1,
+      "num_senders": 2,
+      "max_size": 65536
+    }
+  ],
+  "applications": [
+    {
+      "name": "drivetrain",
+      "executable_name": "drivetrain",
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "trajectory_generator",
+      "executable_name": "trajectory_generator",
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "superstructure",
+      "executable_name": "superstructure",
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_irq_affinity",
+      "executable_name": "irq_affinity",
+      "args": [
+        "--irq_config=/home/admin/bin/roborio_irq_config.json"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "joystick_reader",
+      "executable_name": "joystick_reader",
+      "args": [
+        "--nodie_on_malloc"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "wpilib_interface",
+      "executable_name": "wpilib_interface",
+      "args": [
+        "--nodie_on_malloc"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "autonomous_action",
+      "executable_name": "autonomous_action",
+      "args": [
+        "--nodie_on_malloc"
+      ],
+      "autostart": true,
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_web_proxy",
+      "executable_name": "web_proxy_main",
+      "args": [
+        "--min_ice_port=5800",
+        "--max_ice_port=5810"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_message_bridge_client",
+      "executable_name": "message_bridge_client",
+      "args": [
+        "--rt_priority=16",
+        "--sinit_max_init_timeout=5000"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_message_bridge_server",
+      "executable_name": "message_bridge_server",
+      "args": [
+        "--rt_priority=16"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "logger",
+      "executable_name": "logger_main",
+      "args": [
+        "--snappy_compress",
+        "--logging_folder=/home/admin/logs/",
+        "--rotate_every", "30.0"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "constants_sender_roborio",
+      "executable_name": "constants_sender",
+      "autorestart": false,
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "can_logger",
+      "executable_name": "can_logger",
+      "nodes": [
+        "roborio"
+      ]
+    }
+  ],
+  "maps": [
+    {
+      "match": {
+        "name": "/constants*",
+        "source_node": "roborio"
+      },
+      "rename": {
+        "name": "/roborio/constants"
+      }
+    },
+    {
+      "match": {
+        "name": "/aos*",
+        "source_node": "roborio"
+      },
+      "rename": {
+        "name": "/roborio/aos"
+      }
+    }
+  ],
+  "nodes": [
+    {
+      "name": "roborio",
+      "hostname": "roborio",
+      "hostnames": [
+        "roboRIO-971-FRC",
+        "roboRIO-6971-FRC",
+        "roboRIO-7971-FRC",
+        "roboRIO-8971-FRC",
+        "roboRIO-9971-FRC"
+      ],
+      "port": 9971
+    },
+    {
+      "name": "imu"
+    }
+  ]
+}
diff --git a/y2024_defense/BUILD b/y2024_defense/BUILD
new file mode 100644
index 0000000..63b9502
--- /dev/null
+++ b/y2024_defense/BUILD
@@ -0,0 +1,238 @@
+load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
+load("//aos/util:config_validator_macro.bzl", "config_validator_test")
+
+config_validator_test(
+    name = "config_validator_test",
+    config = "//y2024_defense:aos_config",
+)
+
+robot_downloader(
+    binaries = [
+        "//aos/network:web_proxy_main",
+        "//aos/events/logging:log_cat",
+        "//aos/events:aos_timing_report_streamer",
+    ],
+    data = [
+        ":aos_config",
+        "//aos/starter:roborio_irq_config.json",
+        "@ctre_phoenix6_api_cpp_athena//:shared_libraries",
+        "@ctre_phoenix6_tools_athena//:shared_libraries",
+        "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+        "@ctre_phoenix_cci_athena//:shared_libraries",
+    ],
+    dirs = [
+        "//y2024_defense/www:www_files",
+    ],
+    start_binaries = [
+        "//aos/events/logging:logger_main",
+        "//aos/network:web_proxy_main",
+        "//aos/starter:irq_affinity",
+        ":joystick_reader",
+        ":wpilib_interface",
+        "//frc971/can_logger",
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//y2024_defense/control_loops/drivetrain:drivetrain",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+    name = "pi_download",
+    binaries = [
+        "//aos/starter:irq_affinity",
+        "//aos/util:foxglove_websocket",
+        "//aos/events:aos_timing_report_streamer",
+        "//aos/network:web_proxy_main",
+        "//aos/events/logging:log_cat",
+        "//y2024_defense/rockpi:imu_main",
+        "//frc971/image_streamer:image_streamer",
+    ],
+    data = [
+        ":aos_config",
+        "//frc971/rockpi:rockpi_config.json",
+        "//y2024_defense/www:www_files",
+        "@game_pieces_edgetpu_model//file",
+    ],
+    dirs = [
+        "//frc971/image_streamer/www:www_files",
+    ],
+    start_binaries = [
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//aos/network:web_proxy_main",
+        "//aos/starter:irq_affinity",
+        "//aos/events/logging:logger_main",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+    target_type = "pi",
+)
+
+aos_config(
+    name = "aos_config",
+    src = "y2024_defense.json",
+    flatbuffers = [
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//aos/network:timestamp_fbs",
+        "//frc971/input:robot_state_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":config_imu",
+        ":config_roborio",
+    ],
+)
+
+aos_config(
+    name = "config_imu",
+    src = "y2024_defense_imu.json",
+    flatbuffers = [
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//aos/network:timestamp_fbs",
+        "//aos/network:remote_message_fbs",
+        "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/events:aos_config",
+        "//frc971/control_loops/drivetrain:aos_config",
+    ],
+)
+
+aos_config(
+    name = "config_roborio",
+    src = "y2024_defense_roborio.json",
+    flatbuffers = [
+        "//frc971:can_configuration_fbs",
+        "//aos/network:remote_message_fbs",
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//aos/network:timestamp_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+        "//frc971/can_logger:can_logging_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos/events:aos_config",
+        "//frc971/autonomous:aos_config",
+        "//frc971/control_loops/drivetrain:aos_config",
+        "//frc971/input:aos_config",
+        "//frc971/wpilib:aos_config",
+    ],
+)
+
+cc_library(
+    name = "constants",
+    srcs = [
+        "constants.cc",
+    ],
+    hdrs = [
+        "constants.h",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/mutex",
+        "//aos/network:team_number",
+        "//frc971:constants",
+        "//frc971/control_loops:pose",
+        "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+        "//frc971/shooter_interpolation:interpolation",
+        "//frc971/zeroing:absolute_encoder",
+        "//frc971/zeroing:pot_and_absolute_encoder",
+        "//y2024_defense/control_loops/drivetrain:polydrivetrain_plants",
+        "@com_github_google_glog//:glog",
+        "@com_google_absl//absl/base",
+    ],
+)
+
+cc_binary(
+    name = "wpilib_interface",
+    srcs = [
+        "wpilib_interface.cc",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:roborio"],
+    deps = [
+        ":constants",
+        "//aos:init",
+        "//aos:math",
+        "//aos/containers:sized_array",
+        "//aos/events:shm_event_loop",
+        "//aos/logging",
+        "//aos/stl_mutex",
+        "//aos/time",
+        "//aos/util:log_interval",
+        "//aos/util:phased_loop",
+        "//aos/util:wrapping_counter",
+        "//frc971:can_configuration_fbs",
+        "//frc971/autonomous:auto_mode_fbs",
+        "//frc971/control_loops:control_loop",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+        "//frc971/input:robot_state_fbs",
+        "//frc971/queues:gyro_fbs",
+        "//frc971/wpilib:ADIS16448",
+        "//frc971/wpilib:buffered_pcm",
+        "//frc971/wpilib:can_drivetrain_writer",
+        "//frc971/wpilib:can_sensor_reader",
+        "//frc971/wpilib:drivetrain_writer",
+        "//frc971/wpilib:encoder_and_potentiometer",
+        "//frc971/wpilib:interrupt_edge_counting",
+        "//frc971/wpilib:joystick_sender",
+        "//frc971/wpilib:logging_fbs",
+        "//frc971/wpilib:loop_output_handler",
+        "//frc971/wpilib:pdp_fetcher",
+        "//frc971/wpilib:sensor_reader",
+        "//frc971/wpilib:talonfx",
+        "//frc971/wpilib:wpilib_interface",
+        "//frc971/wpilib:wpilib_robot_base",
+        "//third_party:phoenix",
+        "//third_party:phoenix6",
+        "//third_party:wpilib",
+    ],
+)
+
+cc_binary(
+    name = "joystick_reader",
+    srcs = [
+        ":joystick_reader.cc",
+    ],
+    deps = [
+        ":constants",
+        "//aos:init",
+        "//aos/actions:action_lib",
+        "//aos/logging",
+        "//frc971/autonomous:auto_fbs",
+        "//frc971/autonomous:base_autonomous_actor",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//frc971/input:action_joystick_input",
+        "//frc971/input:drivetrain_input",
+        "//frc971/input:joystick_input",
+        "//frc971/input:redundant_joystick_data",
+        "//y2024_defense/control_loops/drivetrain:drivetrain_base",
+    ],
+)
+
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
+
+sh_binary(
+    name = "log_web_proxy",
+    srcs = ["log_web_proxy.sh"],
+    data = [
+        ":aos_config",
+        "//aos/network:log_web_proxy_main",
+        "//y2024_defense/www:field_main_bundle.min.js",
+        "//y2024_defense/www:files",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/y2024_defense/__init__.py b/y2024_defense/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_defense/__init__.py
diff --git a/y2024_defense/constants.cc b/y2024_defense/constants.cc
new file mode 100644
index 0000000..fa7a14c
--- /dev/null
+++ b/y2024_defense/constants.cc
@@ -0,0 +1,30 @@
+#include "y2024_defense/constants.h"
+
+#include <cinttypes>
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "absl/base/call_once.h"
+#include "glog/logging.h"
+
+#include "aos/mutex/mutex.h"
+#include "aos/network/team_number.h"
+
+namespace y2024_defense {
+namespace constants {
+
+Values MakeValues(uint16_t team) {
+  LOG(INFO) << "creating a Constants for team: " << team;
+
+  Values r;
+
+  return r;
+}
+
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
+
+}  // namespace constants
+}  // namespace y2024_defense
diff --git a/y2024_defense/constants.h b/y2024_defense/constants.h
new file mode 100644
index 0000000..7e359f4
--- /dev/null
+++ b/y2024_defense/constants.h
@@ -0,0 +1,66 @@
+#ifndef Y2023_CONSTANTS_H_
+#define Y2023_CONSTANTS_H_
+
+#include <array>
+#include <cmath>
+#include <cstdint>
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace y2024_defense {
+namespace constants {
+
+constexpr uint16_t kTeamNumber = 9972;
+
+struct Values {
+  static const int kZeroingSampleSize = 200;
+
+  static const int kDrivetrainWriterPriority = 35;
+  static const int kDrivetrainTxPriority = 36;
+  static const int kDrivetrainRxPriority = 36;
+
+  static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
+  static constexpr double kDrivetrainEncoderCountsPerRevolution() {
+    return kDrivetrainCyclesPerRevolution() * 4;
+  }
+  static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
+  static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
+    return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
+           control_loops::drivetrain::kHighOutputRatio /
+           constants::Values::kDrivetrainEncoderRatio() *
+           kDrivetrainEncoderCountsPerRevolution();
+  }
+
+  static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
+  static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
+
+  static double DrivetrainEncoderToMeters(int32_t in) {
+    return ((static_cast<double>(in) /
+             kDrivetrainEncoderCountsPerRevolution()) *
+            (2.0 * M_PI)) *
+           kDrivetrainEncoderRatio() * control_loops::drivetrain::kWheelRadius;
+  }
+
+  static double DrivetrainCANEncoderToMeters(double rotations) {
+    return (rotations * (2.0 * M_PI)) *
+           control_loops::drivetrain::kHighOutputRatio;
+  }
+};
+
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+Values MakeValues(uint16_t team);
+
+// Calls MakeValues with aos::network::GetTeamNumber()
+Values MakeValues();
+
+}  // namespace constants
+}  // namespace y2024_defense
+
+#endif  // Y2023_CONSTANTS_H_
diff --git a/y2024_defense/control_loops/BUILD b/y2024_defense/control_loops/BUILD
new file mode 100644
index 0000000..8dff3b6
--- /dev/null
+++ b/y2024_defense/control_loops/BUILD
@@ -0,0 +1,7 @@
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = ["//y2024_defense:python_init"],
+)
diff --git a/y2024_defense/control_loops/__init__.py b/y2024_defense/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_defense/control_loops/__init__.py
diff --git a/y2024_defense/control_loops/drivetrain/BUILD b/y2024_defense/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..664f9a1
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/BUILD
@@ -0,0 +1,99 @@
+load("//aos:config.bzl", "aos_config")
+
+genrule(
+    name = "genrule_drivetrain",
+    outs = [
+        "drivetrain_dog_motor_plant.h",
+        "drivetrain_dog_motor_plant.cc",
+        "kalman_drivetrain_motor_plant.h",
+        "kalman_drivetrain_motor_plant.cc",
+    ],
+    cmd = "$(location //y2024_defense/control_loops/python:drivetrain) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2024_defense/control_loops/python:drivetrain",
+    ],
+)
+
+genrule(
+    name = "genrule_polydrivetrain",
+    outs = [
+        "polydrivetrain_dog_motor_plant.h",
+        "polydrivetrain_dog_motor_plant.cc",
+        "polydrivetrain_cim_plant.h",
+        "polydrivetrain_cim_plant.cc",
+        "hybrid_velocity_drivetrain.h",
+        "hybrid_velocity_drivetrain.cc",
+    ],
+    cmd = "$(location //y2024_defense/control_loops/python:polydrivetrain) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2024_defense/control_loops/python:polydrivetrain",
+    ],
+)
+
+cc_library(
+    name = "polydrivetrain_plants",
+    srcs = [
+        "drivetrain_dog_motor_plant.cc",
+        "hybrid_velocity_drivetrain.cc",
+        "kalman_drivetrain_motor_plant.cc",
+        "polydrivetrain_dog_motor_plant.cc",
+    ],
+    hdrs = [
+        "drivetrain_dog_motor_plant.h",
+        "hybrid_velocity_drivetrain.h",
+        "kalman_drivetrain_motor_plant.h",
+        "polydrivetrain_dog_motor_plant.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
+
+cc_library(
+    name = "drivetrain_base",
+    srcs = [
+        "drivetrain_base.cc",
+    ],
+    hdrs = [
+        "drivetrain_base.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":polydrivetrain_plants",
+        "//frc971:shifter_hall_effect",
+        "//frc971/control_loops/drivetrain:drivetrain_config",
+    ],
+)
+
+cc_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain_main.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":drivetrain_base",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/control_loops/drivetrain:drivetrain_lib",
+        "//frc971/control_loops/drivetrain/localization:puppet_localizer",
+    ],
+)
+
+aos_config(
+    name = "simulation_config",
+    src = "drivetrain_simulation_config.json",
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops/drivetrain:simulation_channels",
+        "//y2024_defense:aos_config",
+    ],
+)
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_base.cc b/y2024_defense/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..5d14833
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,90 @@
+#include "y2024_defense/control_loops/drivetrain/drivetrain_base.h"
+
+#include <chrono>
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024_defense/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2024_defense/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2024_defense/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::LineFollowConfig;
+
+namespace chrono = ::std::chrono;
+
+namespace y2024_defense {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+  // Yaw of the IMU relative to the robot frame.
+  static constexpr double kImuYaw = 0.0;
+  static DrivetrainConfig<double> kDrivetrainConfig{
+      ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
+      ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+      ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+
+      drivetrain::MakeDrivetrainLoop,
+      drivetrain::MakeVelocityDrivetrainLoop,
+      drivetrain::MakeKFDrivetrainLoop,
+      drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+      chrono::duration_cast<chrono::nanoseconds>(
+          chrono::duration<double>(drivetrain::kDt)),
+      drivetrain::kRobotRadius,
+      drivetrain::kWheelRadius,
+      drivetrain::kV,
+
+      drivetrain::kHighGearRatio,
+      drivetrain::kLowGearRatio,
+      drivetrain::kJ,
+      drivetrain::kMass,
+      kThreeStateDriveShifter,
+      kThreeStateDriveShifter,
+      true /* default_high_gear */,
+      0 /* down_offset if using constants use
+     constants::GetValues().down_error */
+      ,
+      0.7 /* wheel_non_linearity */,
+      1.2 /* quickturn_wheel_multiplier */,
+      1.2 /* wheel_multiplier */,
+      false /*pistol_grip_shift_enables_line_follow*/,
+      (Eigen::Matrix<double, 3, 3>() << std::cos(kImuYaw), -std::sin(kImuYaw),
+       0.0, std::sin(kImuYaw), std::cos(kImuYaw), 0.0, 0.0, 0.0, 1.0)
+          .finished(),
+      false /*is_simulated*/,
+      DownEstimatorConfig{.gravity_threshold = 0.015,
+                          .do_accel_corrections = 1000},
+      LineFollowConfig{
+          .Q = Eigen::Matrix3d((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+                                    << 1.0 / ::std::pow(0.1, 2),
+                                1.0 / ::std::pow(1.0, 2),
+                                1.0 / ::std::pow(1.0, 2))
+                                   .finished()
+                                   .asDiagonal()),
+          .R = Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+                                    << 10.0 / ::std::pow(12.0, 2),
+                                10.0 / ::std::pow(12.0, 2))
+                                   .finished()
+                                   .asDiagonal()),
+          .max_controllable_offset = 0.5},
+      frc971::control_loops::drivetrain::PistolTopButtonUse::kNone,
+      frc971::control_loops::drivetrain::PistolSecondButtonUse::kTurn1,
+      frc971::control_loops::drivetrain::PistolBottomButtonUse::
+          kControlLoopDriving,
+  };
+
+  return kDrivetrainConfig;
+};
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace y2024_defense
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_base.h b/y2024_defense/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..622de4f
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef Y2023_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2023_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2024_defense {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace y2024_defense
+
+#endif  // Y2023_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_main.cc b/y2024_defense/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..968e312
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,30 @@
+#include <memory>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("aos_config.json");
+
+  aos::ShmEventLoop event_loop(&config.message());
+  std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+      localizer = std::make_unique<
+          ::frc971::control_loops::drivetrain::PuppetLocalizer>(
+          &event_loop,
+          ::y2024_defense::control_loops::drivetrain::GetDrivetrainConfig());
+  std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
+      y2024_defense::control_loops::drivetrain::GetDrivetrainConfig(),
+      &event_loop, localizer.get());
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_simulation_config.json b/y2024_defense/control_loops/drivetrain/drivetrain_simulation_config.json
new file mode 100644
index 0000000..8251739
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -0,0 +1,6 @@
+{
+  "imports": [
+    "../../y2024_defense.json",
+    "../../../frc971/control_loops/drivetrain/drivetrain_simulation_channels.json"
+  ]
+}
diff --git a/y2024_defense/control_loops/python/BUILD b/y2024_defense/control_loops/python/BUILD
new file mode 100644
index 0000000..eec5e9f
--- /dev/null
+++ b/y2024_defense/control_loops/python/BUILD
@@ -0,0 +1,57 @@
+package(default_visibility = ["//y2024_defense:__subpackages__"])
+
+py_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain.py",
+    ],
+    legacy_create_init = False,
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:drivetrain",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
+
+py_binary(
+    name = "polydrivetrain",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    legacy_create_init = False,
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:polydrivetrain",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
+
+py_library(
+    name = "polydrivetrain_lib",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops/python:controls",
+        "//frc971/control_loops/python:drivetrain",
+        "//frc971/control_loops/python:polydrivetrain",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
+
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = ["//y2024_defense/control_loops:python_init"],
+)
diff --git a/y2024_defense/control_loops/python/__init__.py b/y2024_defense/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_defense/control_loops/python/__init__.py
diff --git a/y2024_defense/control_loops/python/drivetrain.py b/y2024_defense/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..438eb24
--- /dev/null
+++ b/y2024_defense/control_loops/python/drivetrain.py
@@ -0,0 +1,48 @@
+#!/usr/bin/python3
+
+from __future__ import print_function
+from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+kDrivetrain = drivetrain.DrivetrainParams(
+    J=6.5,
+    mass=68.0,
+    # TODO(austin): Measure radius a bit better.
+    robot_radius=0.39,
+    wheel_radius=2.5 * 0.0254,
+    motor_type=control_loop.Falcon(),
+    num_motors=3,
+    G=(14.0 / 52.0) * (26.0 / 58.0),
+    q_pos=0.24,
+    q_vel=2.5,
+    efficiency=0.92,
+    has_imu=False,
+    force=True,
+    kf_q_voltage=1.0,
+    controller_poles=[0.82, 0.82])
+
+
+def main(argv):
+    argv = FLAGS(argv)
+    glog.init()
+
+    if FLAGS.plot:
+        drivetrain.PlotDrivetrainMotions(kDrivetrain)
+    elif len(argv) != 5:
+        print("Expected .h file name and .cc file name")
+    else:
+        # Write the generated constants out to a file.
+        drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2024_defense',
+                                   kDrivetrain)
+
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))
diff --git a/y2024_defense/control_loops/python/polydrivetrain.py b/y2024_defense/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..301c7b5
--- /dev/null
+++ b/y2024_defense/control_loops/python/polydrivetrain.py
@@ -0,0 +1,34 @@
+#!/usr/bin/python3
+
+import sys
+from y2024_defense.control_loops.python import drivetrain
+from frc971.control_loops.python import polydrivetrain
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+    pass
+
+
+def main(argv):
+    if FLAGS.plot:
+        polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+    elif len(argv) != 7:
+        glog.fatal('Expected .h file name and .cc file name')
+    else:
+        polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+                                           'y2024_defense',
+                                           drivetrain.kDrivetrain)
+
+
+if __name__ == '__main__':
+    argv = FLAGS(sys.argv)
+    glog.init()
+    sys.exit(main(argv))
diff --git a/y2024_defense/joystick_reader.cc b/y2024_defense/joystick_reader.cc
new file mode 100644
index 0000000..8c3115b
--- /dev/null
+++ b/y2024_defense/joystick_reader.cc
@@ -0,0 +1,65 @@
+#include <unistd.h>
+
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/input/action_joystick_input.h"
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/drivetrain_input.h"
+#include "frc971/input/joystick_input.h"
+#include "frc971/input/redundant_joystick_data.h"
+#include "frc971/zeroing/wrap.h"
+#include "y2024_defense/constants.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_base.h"
+
+using Side = frc971::control_loops::drivetrain::RobotSide;
+
+namespace y2024_defense {
+namespace input {
+namespace joysticks {
+
+class Reader : public ::frc971::input::ActionJoystickInput {
+ public:
+  Reader(::aos::EventLoop *event_loop)
+      : ::frc971::input::ActionJoystickInput(
+            event_loop,
+            ::y2024_defense::control_loops::drivetrain::GetDrivetrainConfig(),
+            ::frc971::input::DrivetrainInputReader::InputType::kPistol,
+            {.use_redundant_joysticks = true}) {}
+
+  void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
+
+  bool has_scored_ = false;
+
+  void HandleTeleop(
+      const ::frc971::input::driver_station::Data &data) override {
+    (void)data;
+  }
+};
+
+}  // namespace joysticks
+}  // namespace input
+}  // namespace y2024_defense
+
+int main(int argc, char **argv) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("aos_config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::y2024_defense::input::joysticks::Reader reader(&event_loop);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024_defense/log_web_proxy.sh b/y2024_defense/log_web_proxy.sh
new file mode 100755
index 0000000..714d491
--- /dev/null
+++ b/y2024_defense/log_web_proxy.sh
@@ -0,0 +1 @@
+./aos/network/log_web_proxy_main --data_dir=y2024_defense/www $@
diff --git a/y2024_defense/rockpi/BUILD b/y2024_defense/rockpi/BUILD
new file mode 100644
index 0000000..756a694
--- /dev/null
+++ b/y2024_defense/rockpi/BUILD
@@ -0,0 +1,12 @@
+cc_binary(
+    name = "imu_main",
+    srcs = ["imu_main.cc"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/imu_reader:imu",
+        "//y2024_defense:constants",
+    ],
+)
diff --git a/y2024_defense/rockpi/imu_main.cc b/y2024_defense/rockpi/imu_main.cc
new file mode 100644
index 0000000..8ab51fe
--- /dev/null
+++ b/y2024_defense/rockpi/imu_main.cc
@@ -0,0 +1,26 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/realtime.h"
+#include "frc971/imu_reader/imu.h"
+#include "y2024_defense/constants.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+  aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+  frc971::imu::Imu imu(
+      &event_loop,
+      y2024_defense::constants::Values::DrivetrainEncoderToMeters(1));
+
+  event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+  event_loop.SetRuntimeRealtimePriority(55);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2024_defense/wpilib_interface.cc b/y2024_defense/wpilib_interface.cc
new file mode 100644
index 0000000..d599813
--- /dev/null
+++ b/y2024_defense/wpilib_interface.cc
@@ -0,0 +1,367 @@
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cinttypes>
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include "ctre/phoenix/CANifier.h"
+
+#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/Servo.h"
+#include "frc971/wpilib/ahal/TalonFX.h"
+#include "frc971/wpilib/ahal/VictorSP.h"
+#undef ERROR
+
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
+#include "ctre/phoenix6/TalonFX.hpp"
+
+#include "aos/commonmath.h"
+#include "aos/containers/sized_array.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/realtime.h"
+#include "aos/time/time.h"
+#include "aos/util/log_interval.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/wrapping_counter.h"
+#include "frc971/autonomous/auto_mode_generated.h"
+#include "frc971/can_configuration_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_static.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/input/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/can_drivetrain_writer.h"
+#include "frc971/wpilib/can_sensor_reader.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/talonfx.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2024_defense/constants.h"
+
+DEFINE_bool(ctre_diag_server, false,
+            "If true, enable the diagnostics server for interacting with "
+            "devices on the CAN bus using Phoenix Tuner");
+
+using ::aos::monotonic_clock;
+using ::frc971::CANConfiguration;
+using ::y2024_defense::constants::Values;
+
+using frc971::control_loops::drivetrain::CANPositionStatic;
+using frc971::wpilib::TalonFX;
+
+using std::make_unique;
+
+namespace y2024_defense {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+double drivetrain_velocity_translate(double in) {
+  return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
+          (2.0 * M_PI)) *
+         Values::kDrivetrainEncoderRatio() *
+         control_loops::drivetrain::kWheelRadius;
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
+    Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+              "fast encoders are too fast");
+
+}  // namespace
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+  SensorReader(::aos::ShmEventLoop *event_loop,
+               std::shared_ptr<const Values> values)
+      : ::frc971::wpilib::SensorReader(event_loop),
+        values_(std::move(values)),
+        auto_mode_sender_(
+            event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+                "/autonomous")),
+        drivetrain_position_sender_(
+            event_loop
+                ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+                    "/drivetrain")),
+        gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
+            "/drivetrain")) {
+    // Set to filter out anything shorter than 1/4 of the minimum pulse width
+    // we should ever see.
+    UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+    event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+  }
+
+  void Start() override { AddToDMA(&imu_yaw_rate_reader_); }
+
+  // Auto mode switches.
+  void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+    autonomous_modes_.at(i) = ::std::move(sensor);
+  }
+
+  void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+    imu_yaw_rate_input_ = ::std::move(sensor);
+    imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
+  }
+
+  void RunIteration() override {
+    superstructure_reading_->Set(true);
+    {
+      {
+        auto builder = drivetrain_position_sender_.MakeBuilder();
+        frc971::control_loops::drivetrain::Position::Builder
+            drivetrain_builder =
+                builder
+                    .MakeBuilder<frc971::control_loops::drivetrain::Position>();
+        drivetrain_builder.add_left_encoder(
+            constants::Values::DrivetrainEncoderToMeters(
+                drivetrain_left_encoder_->GetRaw()));
+        drivetrain_builder.add_left_speed(drivetrain_velocity_translate(
+            drivetrain_left_encoder_->GetPeriod()));
+
+        drivetrain_builder.add_right_encoder(
+            -constants::Values::DrivetrainEncoderToMeters(
+                drivetrain_right_encoder_->GetRaw()));
+        drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+            drivetrain_right_encoder_->GetPeriod()));
+
+        builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
+      }
+
+      {
+        auto builder = gyro_sender_.MakeBuilder();
+        ::frc971::sensors::GyroReading::Builder gyro_reading_builder =
+            builder.MakeBuilder<::frc971::sensors::GyroReading>();
+        // +/- 2000 deg / sec
+        constexpr double kMaxVelocity = 4000;  // degrees / second
+        constexpr double kVelocityRadiansPerSecond =
+            kMaxVelocity / 360 * (2.0 * M_PI);
+
+        // Only part of the full range is used to prevent being 100% on or off.
+        constexpr double kScaledRangeLow = 0.1;
+        constexpr double kScaledRangeHigh = 0.9;
+
+        constexpr double kPWMFrequencyHz = 200;
+        double velocity_duty_cycle =
+            imu_yaw_rate_reader_.last_width() * kPWMFrequencyHz;
+
+        constexpr double kDutyCycleScale =
+            1 / (kScaledRangeHigh - kScaledRangeLow);
+        // scale from 0.1 - 0.9 to 0 - 1
+        double rescaled_velocity_duty_cycle =
+            (velocity_duty_cycle - kScaledRangeLow) * kDutyCycleScale;
+
+        if (!std::isnan(rescaled_velocity_duty_cycle)) {
+          gyro_reading_builder.add_velocity(
+              (rescaled_velocity_duty_cycle - 0.5) * kVelocityRadiansPerSecond);
+        }
+        builder.CheckOk(builder.Send(gyro_reading_builder.Finish()));
+      }
+
+      {
+        auto builder = auto_mode_sender_.MakeBuilder();
+
+        uint32_t mode = 0;
+        for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+          if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+            mode |= 1 << i;
+          }
+        }
+
+        auto auto_mode_builder =
+            builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+        auto_mode_builder.add_mode(mode);
+
+        builder.CheckOk(builder.Send(auto_mode_builder.Finish()));
+      }
+    }
+  }
+
+  std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
+
+  void set_superstructure_reading(
+      std::shared_ptr<frc::DigitalOutput> superstructure_reading) {
+    superstructure_reading_ = superstructure_reading;
+  }
+
+ private:
+  std::shared_ptr<const Values> values_;
+
+  aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
+  aos::Sender<frc971::control_loops::drivetrain::Position>
+      drivetrain_position_sender_;
+  ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
+
+  std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
+  std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+
+  frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+  ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+    return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+                                     frc::Encoder::k4X);
+  }
+
+  void Run() override {
+    std::shared_ptr<const Values> values =
+        std::make_shared<const Values>(constants::MakeValues());
+
+    aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+        aos::configuration::ReadConfig("aos_config.json");
+
+    // Thread 1.
+    ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
+    ::frc971::wpilib::JoystickSender joystick_sender(
+        &joystick_sender_event_loop);
+    AddLoop(&joystick_sender_event_loop);
+
+    // Thread 2.
+    ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
+    ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
+    AddLoop(&pdp_fetcher_event_loop);
+
+    // Thread 3.
+    ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+    SensorReader sensor_reader(&sensor_reader_event_loop, values);
+    std::shared_ptr<frc::DigitalOutput> superstructure_reading =
+        make_unique<frc::DigitalOutput>(25);
+
+    sensor_reader.set_pwm_trigger(true);
+    sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
+    sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
+    sensor_reader.set_superstructure_reading(superstructure_reading);
+    sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
+
+    AddLoop(&sensor_reader_event_loop);
+
+    // Thread 4.
+    std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
+
+    std::shared_ptr<TalonFX> right_front = std::make_shared<TalonFX>(
+        1, true, "Drivetrain Bus", &signals_registry,
+        constants::Values::kDrivetrainStatorCurrentLimit(),
+        constants::Values::kDrivetrainSupplyCurrentLimit());
+    std::shared_ptr<TalonFX> right_back = std::make_shared<TalonFX>(
+        2, true, "Drivetrain Bus", &signals_registry,
+        constants::Values::kDrivetrainStatorCurrentLimit(),
+        constants::Values::kDrivetrainSupplyCurrentLimit());
+    std::shared_ptr<TalonFX> right_under = std::make_shared<TalonFX>(
+        3, true, "Drivetrain Bus", &signals_registry,
+        constants::Values::kDrivetrainStatorCurrentLimit(),
+        constants::Values::kDrivetrainSupplyCurrentLimit());
+    std::shared_ptr<TalonFX> left_front = std::make_shared<TalonFX>(
+        4, false, "Drivetrain Bus", &signals_registry,
+        constants::Values::kDrivetrainStatorCurrentLimit(),
+        constants::Values::kDrivetrainSupplyCurrentLimit());
+    std::shared_ptr<TalonFX> left_back = std::make_shared<TalonFX>(
+        5, false, "Drivetrain Bus", &signals_registry,
+        constants::Values::kDrivetrainStatorCurrentLimit(),
+        constants::Values::kDrivetrainSupplyCurrentLimit());
+    std::shared_ptr<TalonFX> left_under = std::make_shared<TalonFX>(
+        6, false, "Drivetrain Bus", &signals_registry,
+        constants::Values::kDrivetrainStatorCurrentLimit(),
+        constants::Values::kDrivetrainSupplyCurrentLimit());
+
+    // Setting up CAN.
+    if (!FLAGS_ctre_diag_server) {
+      c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+      c_Phoenix_Diagnostics_Dispose();
+    }
+
+    // Creating list of falcons for CANSensorReader
+    std::vector<std::shared_ptr<TalonFX>> falcons;
+    for (auto falcon : {right_front, right_back, right_under, left_front,
+                        left_back, left_under}) {
+      falcons.push_back(falcon);
+    }
+
+    ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
+        constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
+    ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
+        constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+
+    ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+    can_sensor_reader_event_loop.set_name("CANSensorReader");
+
+    aos::Sender<CANPositionStatic> can_position_sender =
+        can_sensor_reader_event_loop.MakeSender<CANPositionStatic>(
+            "/drivetrain");
+
+    frc971::wpilib::CANSensorReader can_sensor_reader(
+        &can_sensor_reader_event_loop, std::move(signals_registry), falcons,
+        [falcons, &can_position_sender](ctre::phoenix::StatusCode status) {
+          aos::Sender<CANPositionStatic>::StaticBuilder builder =
+              can_position_sender.MakeStaticBuilder();
+
+          auto falcon_vector = builder->add_talonfxs();
+
+          for (auto falcon : falcons) {
+            falcon->SerializePosition(
+                falcon_vector->emplace_back(),
+                control_loops::drivetrain::kHighOutputRatio);
+          }
+
+          builder->set_timestamp(falcons.front()->GetTimestamp());
+          builder->set_status(static_cast<int>(status));
+
+          builder.CheckOk(builder.Send());
+        });
+
+    AddLoop(&can_sensor_reader_event_loop);
+
+    // Thread 5.
+    ::aos::ShmEventLoop can_drivetrain_writer_event_loop(&config.message());
+    can_drivetrain_writer_event_loop.set_name("CANDrivetrainWriter");
+
+    frc971::wpilib::CANDrivetrainWriter can_drivetrain_writer(
+        &can_drivetrain_writer_event_loop);
+
+    can_drivetrain_writer.set_talonfxs({right_front, right_back, right_under},
+                                       {left_front, left_back, left_under});
+
+    can_drivetrain_writer_event_loop.MakeWatcher(
+        "/roborio", [&can_drivetrain_writer](
+                        const frc971::CANConfiguration &configuration) {
+          can_drivetrain_writer.HandleCANConfiguration(configuration);
+        });
+
+    AddLoop(&can_drivetrain_writer_event_loop);
+
+    RunLoops();
+  }
+};
+
+}  // namespace wpilib
+}  // namespace y2024_defense
+
+AOS_ROBOT_CLASS(::y2024_defense::wpilib::WPILibRobot);
diff --git a/y2024_defense/www/BUILD b/y2024_defense/www/BUILD
new file mode 100644
index 0000000..7af68fe
--- /dev/null
+++ b/y2024_defense/www/BUILD
@@ -0,0 +1,63 @@
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+filegroup(
+    name = "files",
+    srcs = glob([
+        "**/*.html",
+        "**/*.css",
+        "**/*.png",
+    ]) + ["2023.png"],
+    visibility = ["//visibility:public"],
+)
+
+# TODO(max): This needs to be changed once the season starts
+genrule(
+    name = "2023_field_png",
+    srcs = ["//third_party/y2023/field:pictures"],
+    outs = ["2023.png"],
+    cmd = "cp third_party/y2023/field/2023.png $@",
+)
+
+ts_project(
+    name = "field_main",
+    srcs = [
+        "constants.ts",
+        "field_handler.ts",
+        "field_main.ts",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos/network:connect_ts_fbs",
+        "//aos/network:message_bridge_client_ts_fbs",
+        "//aos/network:message_bridge_server_ts_fbs",
+        "//aos/network:web_proxy_ts_fbs",
+        "//aos/network/www:proxy",
+        "//frc971/control_loops:control_loops_ts_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+        "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
+        "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+    ],
+)
+
+rollup_bundle(
+    name = "field_main_bundle",
+    entry_point = "field_main.ts",
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2024_defense:__subpackages__"],
+    deps = [
+        ":field_main",
+    ],
+)
+
+aos_downloader_dir(
+    name = "www_files",
+    srcs = [
+        ":field_main_bundle.min.js",
+        ":files",
+        "//frc971/analysis:plot_index_bundle.min.js",
+    ],
+    dir = "www",
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
diff --git a/y2024_defense/www/constants.ts b/y2024_defense/www/constants.ts
new file mode 100644
index 0000000..d6ecfaf
--- /dev/null
+++ b/y2024_defense/www/constants.ts
@@ -0,0 +1,8 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+// Numbers are slightly hand-tuned to match the PNG that we are using.
+export const FIELD_WIDTH = 26 * FT_TO_M + 7.25 * IN_TO_M;
+export const FIELD_LENGTH = 54 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2024_defense/www/field.html b/y2024_defense/www/field.html
new file mode 100644
index 0000000..d086af7
--- /dev/null
+++ b/y2024_defense/www/field.html
@@ -0,0 +1,41 @@
+<html>
+  <head>
+    <script src="field_main_bundle.min.js" defer></script>
+    <link rel="stylesheet" href="styles.css">
+  </head>
+  <body>
+    <div id="field"> </div>
+    <div id="legend"> </div>
+    <div id="readouts">
+      <table>
+        <tr>
+          <th colspan="2">Robot State</th>
+        </tr>
+        <tr>
+          <td>X</td>
+          <td id="x"> NA </td>
+        </tr>
+        <tr>
+          <td>Y</td>
+          <td id="y"> NA </td>
+        </tr>
+        <tr>
+          <td>Theta</td>
+          <td id="theta"> NA </td>
+        </tr>
+      </table>
+  <h3>Zeroing Faults:</h3>
+  <p id="zeroing_faults"> NA </p>
+  </div>
+  <div id="middle_readouts">
+    <div id="message_bridge_status">
+      <div>
+        <div>Node</div>
+        <div>Client</div>
+        <div>Server</div>
+      </div>
+    </div>
+  </div>
+  </body>
+</html>
+
diff --git a/y2024_defense/www/field_handler.ts b/y2024_defense/www/field_handler.ts
new file mode 100644
index 0000000..8010b10
--- /dev/null
+++ b/y2024_defense/www/field_handler.ts
@@ -0,0 +1,261 @@
+import {ByteBuffer} from 'flatbuffers'
+import {ClientStatistics} from '../../aos/network/message_bridge_client_generated'
+import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
+import {Connection} from '../../aos/network/www/proxy'
+import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
+
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+// (0,0) is field center, +X is toward red DS
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
+
+const ROBOT_WIDTH = 25 * IN_TO_M;
+const ROBOT_LENGTH = 32 * IN_TO_M;
+
+const PI_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+const PIS = ['pi1', 'pi2', 'pi3', 'pi4'];
+
+export class FieldHandler {
+  private canvas = document.createElement('canvas');
+  private localizerOutput: LocalizerOutput|null = null;
+  private drivetrainStatus: DrivetrainStatus|null = null;
+
+  private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+  private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+  private theta: HTMLElement =
+      (document.getElementById('theta') as HTMLElement);
+  // HTML elements for rejection reasons for individual pis. Indices
+  // corresponding to RejectionReason enum values will be for those reasons. The
+  // final row will account for images rejected by the aprilrobotics detector
+  // instead of the localizer.
+  private messageBridgeDiv: HTMLElement =
+      (document.getElementById('message_bridge_status') as HTMLElement);
+  private clientStatuses = new Map<string, HTMLElement>();
+  private serverStatuses = new Map<string, HTMLElement>();
+  private fieldImage: HTMLImageElement = new Image();
+  
+  constructor(private readonly connection: Connection) {
+    (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
+
+    this.fieldImage.src = '2023.png';
+
+    this.connection.addConfigHandler(() => {
+      // Visualization message is reliable so that we can see *all* the vision
+      // matches.
+      this.connection.addHandler(
+          '/drivetrain', 'frc971.control_loops.drivetrain.Status', (data) => {
+            this.handleDrivetrainStatus(data);
+          });
+      this.connection.addHandler(
+          '/localizer', 'frc971.controls.LocalizerOutput', (data) => {
+            this.handleLocalizerOutput(data);
+          });
+      this.connection.addHandler(
+          '/aos', 'aos.message_bridge.ServerStatistics',
+          (data) => {this.handleServerStatistics(data)});
+      this.connection.addHandler(
+          '/aos', 'aos.message_bridge.ClientStatistics',
+          (data) => {this.handleClientStatistics(data)});
+    });
+  }
+
+  private handleLocalizerOutput(data: Uint8Array): void {
+    const fbBuffer = new ByteBuffer(data);
+    this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(fbBuffer);
+  }
+
+  private handleDrivetrainStatus(data: Uint8Array): void {
+    const fbBuffer = new ByteBuffer(data);
+    this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
+  }
+
+  private populateNodeConnections(nodeName: string): void {
+    const row = document.createElement('div');
+    this.messageBridgeDiv.appendChild(row);
+    const nodeDiv = document.createElement('div');
+    nodeDiv.innerHTML = nodeName;
+    row.appendChild(nodeDiv);
+    const clientDiv = document.createElement('div');
+    clientDiv.innerHTML = 'N/A';
+    row.appendChild(clientDiv);
+    const serverDiv = document.createElement('div');
+    serverDiv.innerHTML = 'N/A';
+    row.appendChild(serverDiv);
+    this.serverStatuses.set(nodeName, serverDiv);
+    this.clientStatuses.set(nodeName, clientDiv);
+  }
+
+  private setCurrentNodeState(element: HTMLElement, state: ConnectionState):
+      void {
+    if (state === ConnectionState.CONNECTED) {
+      element.innerHTML = ConnectionState[state];
+      element.classList.remove('faulted');
+      element.classList.add('connected');
+    } else {
+      element.innerHTML = ConnectionState[state];
+      element.classList.remove('connected');
+      element.classList.add('faulted');
+    }
+  }
+
+  private handleServerStatistics(data: Uint8Array): void {
+    const fbBuffer = new ByteBuffer(data);
+    const serverStatistics =
+        ServerStatistics.getRootAsServerStatistics(fbBuffer);
+
+    for (let ii = 0; ii < serverStatistics.connectionsLength(); ++ii) {
+      const connection = serverStatistics.connections(ii);
+      const nodeName = connection.node().name();
+      if (!this.serverStatuses.has(nodeName)) {
+        this.populateNodeConnections(nodeName);
+      }
+      this.setCurrentNodeState(
+          this.serverStatuses.get(nodeName), connection.state());
+    }
+  }
+
+  private handleClientStatistics(data: Uint8Array): void {
+    const fbBuffer = new ByteBuffer(data);
+    const clientStatistics =
+        ClientStatistics.getRootAsClientStatistics(fbBuffer);
+
+    for (let ii = 0; ii < clientStatistics.connectionsLength(); ++ii) {
+      const connection = clientStatistics.connections(ii);
+      const nodeName = connection.node().name();
+      if (!this.clientStatuses.has(nodeName)) {
+        this.populateNodeConnections(nodeName);
+      }
+      this.setCurrentNodeState(
+          this.clientStatuses.get(nodeName), connection.state());
+    }
+  }
+
+  setZeroing(div: HTMLElement): void {
+    div.innerHTML = 'zeroing';
+    div.classList.remove('faulted');
+    div.classList.add('zeroing');
+    div.classList.remove('near');
+  }
+
+  setValue(div: HTMLElement, val: number): void {
+    div.innerHTML = val.toFixed(4);
+    div.classList.remove('faulted');
+    div.classList.remove('zeroing');
+    div.classList.remove('near');
+  }
+
+  drawField(): void {
+    const ctx = this.canvas.getContext('2d');
+    ctx.save();
+    ctx.scale(1.0, -1.0);
+    ctx.drawImage(
+        this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height,
+        -FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH);
+    ctx.restore();
+  }
+
+  drawCamera(x: number, y: number, theta: number, color: string = 'blue'):
+      void {
+    const ctx = this.canvas.getContext('2d');
+    ctx.save();
+    ctx.translate(x, y);
+    ctx.rotate(theta);
+    ctx.strokeStyle = color;
+    ctx.beginPath();
+    ctx.moveTo(0.5, 0.5);
+    ctx.lineTo(0, 0);
+    ctx.lineTo(0.5, -0.5);
+    ctx.stroke();
+    ctx.beginPath();
+    ctx.arc(0, 0, 0.25, -Math.PI / 4, Math.PI / 4);
+    ctx.stroke();
+    ctx.restore();
+  }
+
+  drawRobot(
+      x: number, y: number, theta: number, color: string = 'blue',
+      dashed: boolean = false): void {
+    const ctx = this.canvas.getContext('2d');
+    ctx.save();
+    ctx.translate(x, y);
+    ctx.rotate(theta);
+    ctx.strokeStyle = color;
+    ctx.lineWidth = ROBOT_WIDTH / 10.0;
+    if (dashed) {
+      ctx.setLineDash([0.05, 0.05]);
+    } else {
+      // Empty array = solid line.
+      ctx.setLineDash([]);
+    }
+    ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+    ctx.stroke();
+
+    // Draw line indicating which direction is forwards on the robot.
+    ctx.beginPath();
+    ctx.moveTo(0, 0);
+    ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+    ctx.stroke();
+
+    ctx.restore();
+  }
+
+  draw(): void {
+    this.reset();
+    this.drawField();
+
+    // Draw the matches with debugging information from the localizer.
+    const now = Date.now() / 1000.0;
+
+    if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
+      this.drawRobot(
+          this.drivetrainStatus.trajectoryLogging().x(),
+          this.drivetrainStatus.trajectoryLogging().y(),
+          this.drivetrainStatus.trajectoryLogging().theta(), '#000000FF',
+          false);
+    }
+
+    if (this.localizerOutput) {
+      if (!this.localizerOutput.zeroed()) {
+        this.setZeroing(this.x);
+        this.setZeroing(this.y);
+        this.setZeroing(this.theta);
+      } else {
+        this.setValue(this.x, this.localizerOutput.x());
+        this.setValue(this.y, this.localizerOutput.y());
+        this.setValue(this.theta, this.localizerOutput.theta());
+      }
+
+      this.drawRobot(
+          this.localizerOutput.x(), this.localizerOutput.y(),
+          this.localizerOutput.theta());
+    }
+
+
+    window.requestAnimationFrame(() => this.draw());
+  }
+
+  reset(): void {
+    const ctx = this.canvas.getContext('2d');
+    ctx.setTransform(1, 0, 0, 1, 0, 0);
+    const size = window.innerHeight * 0.9;
+    ctx.canvas.height = size;
+    const width = size / 2 + 20;
+    ctx.canvas.width = width;
+    ctx.clearRect(0, 0, size, width);
+
+    // Translate to center of display.
+    ctx.translate(width / 2, size / 2);
+    // Coordinate system is:
+    // x -> forward.
+    // y -> to the left.
+    ctx.rotate(-Math.PI / 2);
+    ctx.scale(1, -1);
+
+    const M_TO_PX = (size - 10) / FIELD_LENGTH;
+    ctx.scale(M_TO_PX, M_TO_PX);
+    ctx.lineWidth = 1 / M_TO_PX;
+  }
+}
diff --git a/y2024_defense/www/field_main.ts b/y2024_defense/www/field_main.ts
new file mode 100644
index 0000000..d71a45e
--- /dev/null
+++ b/y2024_defense/www/field_main.ts
@@ -0,0 +1,12 @@
+import {Connection} from '../../aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+fieldHandler.draw();
+
diff --git a/y2024_defense/www/index.html b/y2024_defense/www/index.html
new file mode 100644
index 0000000..e4e185e
--- /dev/null
+++ b/y2024_defense/www/index.html
@@ -0,0 +1,6 @@
+<html>
+  <body>
+    <a href="field.html">Field Visualization</a><br>
+    <a href="plotter.html">Plots</a>
+  </body>
+</html>
diff --git a/y2024_defense/www/plotter.html b/y2024_defense/www/plotter.html
new file mode 100644
index 0000000..629ceaa
--- /dev/null
+++ b/y2024_defense/www/plotter.html
@@ -0,0 +1,7 @@
+<html>
+  <head>
+    <script src="plot_index_bundle.min.js" defer></script>
+  </head>
+  <body>
+  </body>
+</html>
diff --git a/y2024_defense/www/styles.css b/y2024_defense/www/styles.css
new file mode 100644
index 0000000..c2c44d2
--- /dev/null
+++ b/y2024_defense/www/styles.css
@@ -0,0 +1,74 @@
+.channel {
+  display: flex;
+  border-bottom: 1px solid;
+  font-size: 24px;
+}
+#field {
+  display: inline-block
+}
+
+#readouts,
+#middle_readouts
+{
+  display: inline-block;
+  vertical-align: top;
+  float: right;
+}
+
+
+#legend {
+  display: inline-block;
+}
+
+table, th, td {
+  border: 1px solid black;
+  border-collapse: collapse;
+  padding: 5px;
+  margin: 10px;
+}
+
+th, td {
+  text-align: right;
+  width: 70px;
+}
+
+td:first-child {
+  width: 150px;
+}
+
+.connected, .near {
+  background-color: LightGreen;
+  border-radius: 10px;
+}
+
+.zeroing {
+  background-color: yellow;
+  border-radius: 10px;
+}
+
+.faulted {
+  background-color: red;
+  border-radius: 10px;
+}
+
+#vision_readouts > div {
+  display: table-row;
+  padding: 5px;
+}
+
+#vision_readouts > div > div {
+  display: table-cell;
+  padding: 5px;
+  text-align: right;
+}
+
+#message_bridge_status > div {
+  display: table-row;
+  padding: 5px;
+}
+
+#message_bridge_status > div > div {
+  display: table-cell;
+  padding: 5px;
+  text-align: right;
+}
diff --git a/y2024_defense/y2024_defense.json b/y2024_defense/y2024_defense.json
new file mode 100644
index 0000000..03e7373
--- /dev/null
+++ b/y2024_defense/y2024_defense.json
@@ -0,0 +1,18 @@
+{
+  "channel_storage_duration": 10000000000,
+  "maps": [
+    {
+      "match": {
+        "name": "/aos",
+        "type": "aos.RobotState"
+      },
+      "rename": {
+        "name": "/roborio/aos"
+      }
+    }
+  ],
+  "imports": [
+    "y2024_defense_roborio.json",
+    "y2024_defense_imu.json",
+  ]
+}
diff --git a/y2024_defense/y2024_defense_imu.json b/y2024_defense/y2024_defense_imu.json
new file mode 100644
index 0000000..1d64398
--- /dev/null
+++ b/y2024_defense/y2024_defense_imu.json
@@ -0,0 +1,280 @@
+{
+  "channels": [
+    {
+      "name": "/imu/aos",
+      "type": "aos.JoystickState",
+      "source_node": "imu",
+      "frequency": 100,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+          "roborio"
+      ]
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.timing.Report",
+      "source_node": "imu",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 4096
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.logging.LogMessageFbs",
+      "source_node": "imu",
+      "frequency": 200,
+      "num_senders": 20
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.starter.Status",
+      "source_node": "imu",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 2048
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.starter.StarterRpc",
+      "source_node": "imu",
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.message_bridge.ServerStatistics",
+      "source_node": "imu",
+      "max_size": 2048,
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.message_bridge.ClientStatistics",
+      "source_node": "imu",
+      "frequency": 20,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.logging.DynamicLogCommand",
+      "source_node": "imu",
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/imu/aos",
+      "type": "aos.message_bridge.Timestamp",
+      "source_node": "imu",
+      "frequency": 15,
+      "num_senders": 2,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "roborio",
+      ],
+      "max_size": 400,
+      "destination_nodes": [
+        {
+          "name": "roborio",
+          "priority": 1,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "imu"
+          ],
+          "time_to_live": 5000000
+        },
+      ]
+    },
+    {
+      "name": "/imu/aos/remote_timestamps/roborio/imu/aos/aos-message_bridge-Timestamp",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 20,
+      "source_node": "imu",
+      "max_size": 208
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.starter.StarterRpc",
+      "source_node": "roborio",
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "roborio"
+          ],
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+      "type": "aos.message_bridge.RemoteMessage",
+      "source_node": "roborio",
+      "logger": "NOT_LOGGED",
+      "frequency": 20,
+      "num_senders": 2,
+      "max_size": 200
+    },
+    {
+      "name": "/localizer",
+      "type": "frc971.IMUValuesBatch",
+      "source_node": "imu",
+      "frequency": 2200,
+      "max_size": 1600,
+      "num_senders": 2
+    },
+    {
+      "name": "/localizer",
+      "type": "frc971.controls.LocalizerOutput",
+      "source_node": "imu",
+      "frequency": 52,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "roborio"
+      ],
+      "destination_nodes": [
+        {
+          "name": "roborio",
+          "priority": 5,
+          "time_to_live": 5000000,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "imu"
+          ]
+        }
+      ]
+    },
+    {
+      "name": "/imu/aos/remote_timestamps/roborio/localizer/frc971-controls-LocalizerOutput",
+      "type": "aos.message_bridge.RemoteMessage",
+      "source_node": "imu",
+      "logger": "NOT_LOGGED",
+      "frequency": 52,
+      "num_senders": 2,
+      "max_size": 200
+    },
+  ],
+  "applications": [
+    {
+      "name": "message_bridge_client",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "localizer",
+      "executable_name": "localizer_main",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "imu",
+      "executable_name": "imu_main",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "joystick_republish",
+      "executable_name": "joystick_republish",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "message_bridge_server",
+      "executable_name": "message_bridge_server",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "localizer_logger",
+      "executable_name": "logger_main",
+      "args": [
+        "--logging_folder",
+        "",
+        "--snappy_compress",
+        "--rotate_every", "30.0"
+      ],
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "web_proxy",
+      "executable_name": "web_proxy_main",
+      "args": [
+        "--min_ice_port=5800",
+        "--max_ice_port=5810"
+      ],
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "foxglove_websocket",
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    },
+    {
+      "name": "constants_sender",
+      "autorestart": false,
+      "user": "pi",
+      "nodes": [
+        "imu"
+      ]
+    }
+  ],
+  "maps": [
+    {
+      "match": {
+        "name": "/constants*",
+        "source_node": "imu"
+      },
+      "rename": {
+        "name": "/imu/constants"
+      }
+    },
+    {
+      "match": {
+        "name": "/aos*",
+        "source_node": "imu"
+      },
+      "rename": {
+        "name": "/imu/aos"
+      }
+    }
+  ],
+  "nodes": [
+    {
+      "name": "imu",
+      "hostname": "pi6",
+      "hostnames": [
+        "pi-971-6",
+        "pi-7971-6",
+        "pi-8971-6",
+        "pi-9971-6"
+      ],
+      "port": 9971
+    },
+    {
+      "name": "roborio"
+    },
+  ]
+}
diff --git a/y2024_defense/y2024_defense_roborio.json b/y2024_defense/y2024_defense_roborio.json
new file mode 100644
index 0000000..f397de3
--- /dev/null
+++ b/y2024_defense/y2024_defense_roborio.json
@@ -0,0 +1,385 @@
+{
+  "channels": [
+    {
+      "name": "/roborio/aos",
+      "type": "aos.JoystickState",
+      "source_node": "roborio",
+      "frequency": 100,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "time_to_live": 50000000
+        }
+      ]
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.RobotState",
+      "source_node": "roborio",
+      "frequency": 250
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.timing.Report",
+      "source_node": "roborio",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 8192
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.logging.LogMessageFbs",
+      "source_node": "roborio",
+      "frequency": 500,
+      "max_size": 1000,
+      "num_senders": 20
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.starter.Status",
+      "source_node": "roborio",
+      "frequency": 50,
+      "num_senders": 20,
+      "max_size": 2000
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.starter.StarterRpc",
+      "source_node": "roborio",
+      "frequency": 10,
+      "max_size": 400,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.message_bridge.ServerStatistics",
+      "source_node": "roborio",
+      "max_size": 2048,
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.message_bridge.ClientStatistics",
+      "source_node": "roborio",
+      "frequency": 20,
+      "max_size": 2000,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.logging.DynamicLogCommand",
+      "source_node": "roborio",
+      "frequency": 10,
+      "num_senders": 2
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 20,
+      "source_node": "roborio",
+      "max_size": 208
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "aos.message_bridge.Timestamp",
+      "source_node": "roborio",
+      "frequency": 15,
+      "num_senders": 2,
+      "max_size": 512,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 1,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "roborio"
+          ],
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/can",
+      "type": "frc971.can_logger.CanFrame",
+      "source_node": "roborio",
+      "frequency": 6000,
+      "num_senders": 2,
+      "max_size": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.CANPosition",
+      "source_node": "roborio",
+      "frequency": 220,
+      "num_senders": 2,
+      "max_size": 400
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.sensors.GyroReading",
+      "source_node": "roborio",
+      "frequency": 250,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.sensors.Uid",
+      "source_node": "roborio",
+      "frequency": 250,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.fb.Trajectory",
+      "source_node": "roborio",
+      "max_size": 600000,
+      "frequency": 10,
+      "logger": "NOT_LOGGED",
+      "num_senders": 2,
+      "read_method": "PIN",
+      "num_readers": 10
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.SplineGoal",
+      "source_node": "roborio",
+      "frequency": 10
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Goal",
+      "source_node": "roborio",
+      "max_size": 224,
+      "frequency": 250
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Position",
+      "source_node": "roborio",
+      "frequency": 400,
+      "max_size": 112,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Output",
+      "source_node": "roborio",
+      "frequency": 400,
+      "max_size": 80,
+      "num_senders": 2,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Status",
+      "source_node": "roborio",
+      "frequency": 400,
+      "max_size": 1616,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.LocalizerControl",
+      "source_node": "roborio",
+      "frequency": 250,
+      "max_size": 96,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 5,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "roborio"
+          ],
+          "time_to_live": 0
+        }
+      ]
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-LocalizerControl",
+      "type": "aos.message_bridge.RemoteMessage",
+      "source_node": "roborio",
+      "logger": "NOT_LOGGED",
+      "frequency": 400,
+      "num_senders": 2,
+      "max_size": 200
+    },
+    {
+      "name": "/autonomous",
+      "type": "aos.common.actions.Status",
+      "source_node": "roborio"
+    },
+    {
+      "name": "/autonomous",
+      "type": "frc971.autonomous.Goal",
+      "source_node": "roborio"
+    },
+    {
+      "name": "/autonomous",
+      "type": "frc971.autonomous.AutonomousMode",
+      "source_node": "roborio",
+      "frequency": 250
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "frc971.PDPValues",
+      "source_node": "roborio",
+      "frequency": 55,
+      "max_size": 368
+    },
+    {
+      "name": "/roborio/aos",
+      "type": "frc971.wpilib.PneumaticsToLog",
+      "source_node": "roborio",
+      "frequency": 50
+    },
+    {
+      "name": "/roborio",
+      "type": "frc971.CANConfiguration",
+      "source_node": "roborio",
+      "frequency": 2
+    },
+  ],
+  "applications": [
+    {
+      "name": "drivetrain",
+      "executable_name": "drivetrain",
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_irq_affinity",
+      "executable_name": "irq_affinity",
+      "args": [
+        "--irq_config=/home/admin/bin/roborio_irq_config.json"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "joystick_reader",
+      "executable_name": "joystick_reader",
+      "args": [
+        "--nodie_on_malloc"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "wpilib_interface",
+      "executable_name": "wpilib_interface",
+      "args": [
+        "--nodie_on_malloc"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_web_proxy",
+      "executable_name": "web_proxy_main",
+      "args": [
+        "--min_ice_port=5800",
+        "--max_ice_port=5810"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_message_bridge_client",
+      "executable_name": "message_bridge_client",
+      "args": [
+        "--rt_priority=16",
+        "--sinit_max_init_timeout=5000"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "roborio_message_bridge_server",
+      "executable_name": "message_bridge_server",
+      "args": [
+        "--rt_priority=16"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "logger",
+      "executable_name": "logger_main",
+      "args": [
+        "--snappy_compress",
+        "--logging_folder=/home/admin/logs/",
+        "--rotate_every", "30.0"
+      ],
+      "nodes": [
+        "roborio"
+      ]
+    },
+    {
+      "name": "can_logger",
+      "executable_name": "can_logger",
+      "nodes": [
+        "roborio"
+      ]
+    }
+  ],
+  "maps": [
+    {
+      "match": {
+        "name": "/aos*",
+        "source_node": "roborio"
+      },
+      "rename": {
+        "name": "/roborio/aos"
+      }
+    }
+  ],
+  "nodes": [
+    {
+      "name": "roborio",
+      "hostname": "roborio",
+      "hostnames": [
+        "roboRIO-971-FRC",
+        "roboRIO-6971-FRC",
+        "roboRIO-7971-FRC",
+        "roboRIO-8971-FRC",
+        "roboRIO-9972-FRC"
+      ],
+      "port": 9972
+    },
+    {
+      "name": "imu"
+    }
+  ]
+}